Initial commit

166 files changed, 23211 insertions, 0 deletions

diff --git a/Marlin/src/gcode/bedlevel/G26.cpp b/Marlin/src/gcode/bedlevel/G26.cpp
new file mode 100644
index 0000000..5a79aaa
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/G26.cpp
@@ -0,0 +1,872 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ *   G26 Mesh Validation Tool
+ *
+ *   G26 is a Mesh Validation Tool intended to provide support for the Marlin Unified Bed Leveling System.
+ *   In order to fully utilize and benefit from the Marlin Unified Bed Leveling System an accurate Mesh must
+ *   be defined. G29 is designed to allow the user to quickly validate the correctness of her Mesh. It will
+ *   first heat the bed and nozzle. It will then print lines and circles along the Mesh Cell boundaries and
+ *   the intersections of those lines (respectively).
+ *
+ *   This action allows the user to immediately see where the Mesh is properly defined and where it needs to
+ *   be edited. The command will generate the Mesh lines closest to the nozzle's starting position. Alternatively
+ *   the user can specify the X and Y position of interest with command parameters. This allows the user to
+ *   focus on a particular area of the Mesh where attention is needed.
+ *
+ *   B #  Bed         Set the Bed Temperature. If not specified, a default of 60 C. will be assumed.
+ *
+ *   C    Current     When searching for Mesh Intersection points to draw, use the current nozzle location
+ *                    as the base for any distance comparison.
+ *
+ *   D    Disable     Disable the Unified Bed Leveling System. In the normal case the user is invoking this
+ *                    command to see how well a Mesh as been adjusted to match a print surface. In order to do
+ *                    this the Unified Bed Leveling System is turned on by the G26 command. The D parameter
+ *                    alters the command's normal behavior and disables the Unified Bed Leveling System even if
+ *                    it is on.
+ *
+ *   H #  Hotend      Set the Nozzle Temperature. If not specified, a default of 205 C. will be assumed.
+ *
+ *   I #  Preset      Heat the Nozzle and Bed based on a Material Preset (if material presets are defined).
+ *
+ *   F #  Filament    Used to specify the diameter of the filament being used. If not specified
+ *                    1.75mm filament is assumed. If you are not getting acceptable results by using the
+ *                    'correct' numbers, you can scale this number up or down a little bit to change the amount
+ *                    of filament that is being extruded during the printing of the various lines on the bed.
+ *
+ *   K    Keep-On     Keep the heaters turned on at the end of the command.
+ *
+ *   L #  Layer       Layer height. (Height of nozzle above bed)  If not specified .20mm will be used.
+ *
+ *   O #  Ooooze      How much your nozzle will Ooooze filament while getting in position to print. This
+ *                    is over kill, but using this parameter will let you get the very first 'circle' perfect
+ *                    so you have a trophy to peel off of the bed and hang up to show how perfectly you have your
+ *                    Mesh calibrated. If not specified, a filament length of .3mm is assumed.
+ *
+ *   P #  Prime       Prime the nozzle with specified length of filament. If this parameter is not
+ *                    given, no prime action will take place. If the parameter specifies an amount, that much
+ *                    will be purged before continuing. If no amount is specified the command will start
+ *                    purging filament until the user provides an LCD Click and then it will continue with
+ *                    printing the Mesh. You can carefully remove the spent filament with a needle nose
+ *                    pliers while holding the LCD Click wheel in a depressed state. If you do not have
+ *                    an LCD, you must specify a value if you use P.
+ *
+ *   Q #  Multiplier  Retraction Multiplier. Normally not needed. Retraction defaults to 1.0mm and
+ *                    un-retraction is at 1.2mm   These numbers will be scaled by the specified amount
+ *
+ *   R #  Repeat      Prints the number of patterns given as a parameter, starting at the current location.
+ *                    If a parameter isn't given, every point will be printed unless G26 is interrupted.
+ *                    This works the same way that the UBL G29 P4 R parameter works.
+ *
+ *                    NOTE:  If you do not have an LCD, you -must- specify R. This is to ensure that you are
+ *                    aware that there's some risk associated with printing without the ability to abort in
+ *                    cases where mesh point Z value may be inaccurate. As above, if you do not include a
+ *                    parameter, every point will be printed.
+ *
+ *   S #  Nozzle      Used to control the size of nozzle diameter. If not specified, a .4mm nozzle is assumed.
+ *
+ *   U #  Random      Randomize the order that the circles are drawn on the bed. The search for the closest
+ *                    un-drawn circle is still done. But the distance to the location for each circle has a
+ *                    random number of the specified size added to it. Specifying S50 will give an interesting
+ *                    deviation from the normal behavior on a 10 x 10 Mesh.
+ *
+ *   X #  X Coord.    Specify the starting location of the drawing activity.
+ *
+ *   Y #  Y Coord.    Specify the starting location of the drawing activity.
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(G26_MESH_VALIDATION)
+
+#define G26_OK false
+#define G26_ERR true
+
+#include "../../gcode/gcode.h"
+#include "../../feature/bedlevel/bedlevel.h"
+
+#include "../../MarlinCore.h"
+#include "../../module/planner.h"
+#include "../../module/stepper.h"
+#include "../../module/motion.h"
+#include "../../module/tool_change.h"
+#include "../../module/temperature.h"
+#include "../../lcd/marlinui.h"
+
+#define EXTRUSION_MULTIPLIER 1.0
+#define PRIME_LENGTH 10.0
+#define OOZE_AMOUNT 0.3
+
+#define INTERSECTION_CIRCLE_RADIUS 5
+#define CROSSHAIRS_SIZE 3
+
+#ifndef G26_RETRACT_MULTIPLIER
+  #define G26_RETRACT_MULTIPLIER 1.0 // x 1mm
+#endif
+
+#ifndef G26_XY_FEEDRATE
+  #define G26_XY_FEEDRATE (PLANNER_XY_FEEDRATE() / 3.0)
+#endif
+
+#ifndef G26_XY_FEEDRATE_TRAVEL
+  #define G26_XY_FEEDRATE_TRAVEL (PLANNER_XY_FEEDRATE() / 1.5)
+#endif
+
+#if CROSSHAIRS_SIZE >= INTERSECTION_CIRCLE_RADIUS
+  #error "CROSSHAIRS_SIZE must be less than INTERSECTION_CIRCLE_RADIUS."
+#endif
+
+#define G26_OK false
+#define G26_ERR true
+
+#if ENABLED(ARC_SUPPORT)
+  void plan_arc(const xyze_pos_t&, const ab_float_t&, const bool, const uint8_t);
+#endif
+
+constexpr float g26_e_axis_feedrate = 0.025;
+
+static MeshFlags circle_flags, horizontal_mesh_line_flags, vertical_mesh_line_flags;
+float g26_random_deviation = 0.0;
+
+static bool g26_retracted = false; // Track the retracted state of the nozzle so mismatched
+                                   // retracts/recovers won't result in a bad state.
+
+float g26_extrusion_multiplier,
+      g26_retraction_multiplier,
+      g26_layer_height,
+      g26_prime_length;
+
+xy_pos_t g26_xy_pos; // = { 0, 0 }
+
+int16_t g26_bed_temp,
+        g26_hotend_temp;
+
+int8_t g26_prime_flag;
+
+#if HAS_LCD_MENU
+
+  /**
+   * If the LCD is clicked, cancel, wait for release, return true
+   */
+  bool user_canceled() {
+    if (!ui.button_pressed()) return false; // Return if the button isn't pressed
+    ui.set_status_P(GET_TEXT(MSG_G26_CANCELED), 99);
+    TERN_(HAS_LCD_MENU, ui.quick_feedback());
+    ui.wait_for_release();
+    return true;
+  }
+
+#endif
+
+mesh_index_pair find_closest_circle_to_print(const xy_pos_t &pos) {
+  float closest = 99999.99;
+  mesh_index_pair out_point;
+
+  out_point.pos = -1;
+
+  GRID_LOOP(i, j) {
+    if (!circle_flags.marked(i, j)) {
+      // We found a circle that needs to be printed
+      const xy_pos_t m = { _GET_MESH_X(i), _GET_MESH_Y(j) };
+
+      // Get the distance to this intersection
+      float f = (pos - m).magnitude();
+
+      // It is possible that we are being called with the values
+      // to let us find the closest circle to the start position.
+      // But if this is not the case, add a small weighting to the
+      // distance calculation to help it choose a better place to continue.
+      f += (g26_xy_pos - m).magnitude() / 15.0f;
+
+      // Add the specified amount of Random Noise to our search
+      if (g26_random_deviation > 1.0) f += random(0.0, g26_random_deviation);
+
+      if (f < closest) {
+        closest = f;          // Found a closer un-printed location
+        out_point.pos.set(i, j);  // Save its data
+        out_point.distance = closest;
+      }
+    }
+  }
+  circle_flags.mark(out_point); // Mark this location as done.
+  return out_point;
+}
+
+void move_to(const float &rx, const float &ry, const float &z, const float &e_delta) {
+  static float last_z = -999.99;
+
+  const xy_pos_t dest = { rx, ry };
+
+  const bool has_xy_component = dest != current_position; // Check if X or Y is involved in the movement.
+  const bool has_e_component = e_delta != 0.0;
+
+  destination = current_position;
+
+  if (z != last_z) {
+    last_z = destination.z = z;
+    const feedRate_t fr_mm_s = planner.settings.max_feedrate_mm_s[Z_AXIS] * 0.5f; // Use half of the Z_AXIS max feed rate
+    prepare_internal_move_to_destination(fr_mm_s);
+  }
+
+  // If X or Y in combination with E is involved do a 'normal' move.
+  // If X or Y with no E is involved do a 'fast' move
+  // Otherwise retract/recover/hop.
+  destination = dest;
+  destination.e += e_delta;
+  const feedRate_t fr_mm_s = has_xy_component
+    ? (has_e_component ? feedRate_t(G26_XY_FEEDRATE) : feedRate_t(G26_XY_FEEDRATE_TRAVEL))
+    : planner.settings.max_feedrate_mm_s[E_AXIS] * 0.666f;
+  prepare_internal_move_to_destination(fr_mm_s);
+}
+
+FORCE_INLINE void move_to(const xyz_pos_t &where, const float &de) { move_to(where.x, where.y, where.z, de); }
+
+void retract_filament(const xyz_pos_t &where) {
+  if (!g26_retracted) { // Only retract if we are not already retracted!
+    g26_retracted = true;
+    move_to(where, -1.0f * g26_retraction_multiplier);
+  }
+}
+
+// TODO: Parameterize the Z lift with a define
+void retract_lift_move(const xyz_pos_t &s) {
+  retract_filament(destination);
+  move_to(current_position.x, current_position.y, current_position.z + 0.5f, 0.0);  // Z lift to minimize scraping
+  move_to(s.x, s.y, s.z + 0.5f, 0.0);  // Get to the starting point with no extrusion while lifted
+}
+
+void recover_filament(const xyz_pos_t &where) {
+  if (g26_retracted) { // Only un-retract if we are retracted.
+    move_to(where, 1.2f * g26_retraction_multiplier);
+    g26_retracted = false;
+  }
+}
+
+/**
+ * print_line_from_here_to_there() takes two cartesian coordinates and draws a line from one
+ * to the other. But there are really three sets of coordinates involved. The first coordinate
+ * is the present location of the nozzle. We don't necessarily want to print from this location.
+ * We first need to move the nozzle to the start of line segment where we want to print. Once
+ * there, we can use the two coordinates supplied to draw the line.
+ *
+ * Note:  Although we assume the first set of coordinates is the start of the line and the second
+ * set of coordinates is the end of the line, it does not always work out that way. This function
+ * optimizes the movement to minimize the travel distance before it can start printing. This saves
+ * a lot of time and eliminates a lot of nonsensical movement of the nozzle. However, it does
+ * cause a lot of very little short retracement of th nozzle when it draws the very first line
+ * segment of a 'circle'. The time this requires is very short and is easily saved by the other
+ * cases where the optimization comes into play.
+ */
+void print_line_from_here_to_there(const xyz_pos_t &s, const xyz_pos_t &e) {
+
+  // Distances to the start / end of the line
+  xy_float_t svec = current_position - s, evec = current_position - e;
+
+  const float dist_start = HYPOT2(svec.x, svec.y),
+              dist_end = HYPOT2(evec.x, evec.y),
+              line_length = HYPOT(e.x - s.x, e.y - s.y);
+
+  // If the end point of the line is closer to the nozzle, flip the direction,
+  // moving from the end to the start. On very small lines the optimization isn't worth it.
+  if (dist_end < dist_start && (INTERSECTION_CIRCLE_RADIUS) < ABS(line_length))
+    return print_line_from_here_to_there(e, s);
+
+  // Decide whether to retract & lift
+  if (dist_start > 2.0) retract_lift_move(s);
+
+  move_to(s, 0.0); // Get to the starting point with no extrusion / un-Z lift
+
+  const float e_pos_delta = line_length * g26_e_axis_feedrate * g26_extrusion_multiplier;
+
+  recover_filament(destination);
+  move_to(e, e_pos_delta);  // Get to the ending point with an appropriate amount of extrusion
+}
+
+inline bool look_for_lines_to_connect() {
+  xyz_pos_t s, e;
+  s.z = e.z = g26_layer_height;
+
+  GRID_LOOP(i, j) {
+
+    if (TERN0(HAS_LCD_MENU, user_canceled())) return true;
+
+    if (i < (GRID_MAX_POINTS_X)) {  // Can't connect to anything farther to the right than GRID_MAX_POINTS_X.
+                                    // Already a half circle at the edge of the bed.
+
+      if (circle_flags.marked(i, j) && circle_flags.marked(i + 1, j)) {   // Test whether a leftward line can be done
+        if (!horizontal_mesh_line_flags.marked(i, j)) {
+          // Two circles need a horizontal line to connect them
+          s.x = _GET_MESH_X(  i  ) + (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // right edge
+          e.x = _GET_MESH_X(i + 1) - (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // left edge
+
+          LIMIT(s.x, X_MIN_POS + 1, X_MAX_POS - 1);
+          s.y = e.y = constrain(_GET_MESH_Y(j), Y_MIN_POS + 1, Y_MAX_POS - 1);
+          LIMIT(e.x, X_MIN_POS + 1, X_MAX_POS - 1);
+
+          if (position_is_reachable(s.x, s.y) && position_is_reachable(e.x, e.y))
+            print_line_from_here_to_there(s, e);
+
+          horizontal_mesh_line_flags.mark(i, j); // Mark done, even if skipped
+        }
+      }
+
+      if (j < (GRID_MAX_POINTS_Y)) {  // Can't connect to anything further back than GRID_MAX_POINTS_Y.
+                                      // Already a half circle at the edge of the bed.
+
+        if (circle_flags.marked(i, j) && circle_flags.marked(i, j + 1)) {   // Test whether a downward line can be done
+          if (!vertical_mesh_line_flags.marked(i, j)) {
+            // Two circles that need a vertical line to connect them
+            s.y = _GET_MESH_Y(  j  ) + (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // top edge
+            e.y = _GET_MESH_Y(j + 1) - (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // bottom edge
+
+            s.x = e.x = constrain(_GET_MESH_X(i), X_MIN_POS + 1, X_MAX_POS - 1);
+            LIMIT(s.y, Y_MIN_POS + 1, Y_MAX_POS - 1);
+            LIMIT(e.y, Y_MIN_POS + 1, Y_MAX_POS - 1);
+
+            if (position_is_reachable(s.x, s.y) && position_is_reachable(e.x, e.y))
+              print_line_from_here_to_there(s, e);
+
+            vertical_mesh_line_flags.mark(i, j); // Mark done, even if skipped
+          }
+        }
+      }
+    }
+  }
+  return false;
+}
+
+/**
+ * Turn on the bed and nozzle heat and
+ * wait for them to get up to temperature.
+ */
+inline bool turn_on_heaters() {
+
+  SERIAL_ECHOLNPGM("Waiting for heatup.");
+
+  #if HAS_HEATED_BED
+
+    if (g26_bed_temp > 25) {
+      #if HAS_WIRED_LCD
+        ui.set_status_P(GET_TEXT(MSG_G26_HEATING_BED), 99);
+        ui.quick_feedback();
+        TERN_(HAS_LCD_MENU, ui.capture());
+      #endif
+      thermalManager.setTargetBed(g26_bed_temp);
+
+      // Wait for the temperature to stabilize
+      if (!thermalManager.wait_for_bed(true
+          #if G26_CLICK_CAN_CANCEL
+            , true
+          #endif
+        )
+      ) return G26_ERR;
+    }
+
+  #endif // HAS_HEATED_BED
+
+  // Start heating the active nozzle
+  #if HAS_WIRED_LCD
+    ui.set_status_P(GET_TEXT(MSG_G26_HEATING_NOZZLE), 99);
+    ui.quick_feedback();
+  #endif
+  thermalManager.setTargetHotend(g26_hotend_temp, active_extruder);
+
+  // Wait for the temperature to stabilize
+  if (!thermalManager.wait_for_hotend(active_extruder, true
+    #if G26_CLICK_CAN_CANCEL
+      , true
+    #endif
+  )) return G26_ERR;
+
+  #if HAS_WIRED_LCD
+    ui.reset_status();
+    ui.quick_feedback();
+  #endif
+
+  return G26_OK;
+}
+
+/**
+ * Prime the nozzle if needed. Return true on error.
+ */
+inline bool prime_nozzle() {
+
+  const feedRate_t fr_slow_e = planner.settings.max_feedrate_mm_s[E_AXIS] / 15.0f;
+  #if HAS_LCD_MENU && !HAS_TOUCH_BUTTONS // ui.button_pressed issue with touchscreen
+    #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
+      float Total_Prime = 0.0;
+    #endif
+
+    if (g26_prime_flag == -1) {  // The user wants to control how much filament gets purged
+      ui.capture();
+      ui.set_status_P(GET_TEXT(MSG_G26_MANUAL_PRIME), 99);
+      ui.chirp();
+
+      destination = current_position;
+
+      recover_filament(destination); // Make sure G26 doesn't think the filament is retracted().
+
+      while (!ui.button_pressed()) {
+        ui.chirp();
+        destination.e += 0.25;
+        #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
+          Total_Prime += 0.25;
+          if (Total_Prime >= EXTRUDE_MAXLENGTH) {
+            ui.release();
+            return G26_ERR;
+          }
+        #endif
+        prepare_internal_move_to_destination(fr_slow_e);
+        destination = current_position;
+        planner.synchronize();    // Without this synchronize, the purge is more consistent,
+                                  // but because the planner has a buffer, we won't be able
+                                  // to stop as quickly. So we put up with the less smooth
+                                  // action to give the user a more responsive 'Stop'.
+      }
+
+      ui.wait_for_release();
+
+      ui.set_status_P(GET_TEXT(MSG_G26_PRIME_DONE), 99);
+      ui.quick_feedback();
+      ui.release();
+    }
+    else
+  #endif
+  {
+    #if HAS_WIRED_LCD
+      ui.set_status_P(GET_TEXT(MSG_G26_FIXED_LENGTH), 99);
+      ui.quick_feedback();
+    #endif
+    destination = current_position;
+    destination.e += g26_prime_length;
+    prepare_internal_move_to_destination(fr_slow_e);
+    destination.e -= g26_prime_length;
+    retract_filament(destination);
+  }
+
+  return G26_OK;
+}
+
+/**
+ * G26: Mesh Validation Pattern generation.
+ *
+ * Used to interactively edit the mesh by placing the
+ * nozzle in a problem area and doing a G29 P4 R command.
+ *
+ * Parameters:
+ *
+ *  B  Bed Temperature
+ *  C  Continue from the Closest mesh point
+ *  D  Disable leveling before starting
+ *  F  Filament diameter
+ *  H  Hotend Temperature
+ *  K  Keep heaters on when completed
+ *  L  Layer Height
+ *  O  Ooze extrusion length
+ *  P  Prime length
+ *  Q  Retraction multiplier
+ *  R  Repetitions (number of grid points)
+ *  S  Nozzle Size (diameter) in mm
+ *  T  Tool index to change to, if included
+ *  U  Random deviation (50 if no value given)
+ *  X  X position
+ *  Y  Y position
+ */
+void GcodeSuite::G26() {
+  SERIAL_ECHOLNPGM("G26 starting...");
+
+  // Don't allow Mesh Validation without homing first,
+  // or if the parameter parsing did not go OK, abort
+  if (homing_needed_error()) return;
+
+  // Change the tool first, if specified
+  if (parser.seenval('T')) tool_change(parser.value_int());
+
+  g26_extrusion_multiplier    = EXTRUSION_MULTIPLIER;
+  g26_retraction_multiplier   = G26_RETRACT_MULTIPLIER;
+  g26_layer_height            = MESH_TEST_LAYER_HEIGHT;
+  g26_prime_length            = PRIME_LENGTH;
+  g26_bed_temp                = MESH_TEST_BED_TEMP;
+  g26_hotend_temp             = MESH_TEST_HOTEND_TEMP;
+  g26_prime_flag              = 0;
+
+  float g26_nozzle            = MESH_TEST_NOZZLE_SIZE,
+        g26_filament_diameter = DEFAULT_NOMINAL_FILAMENT_DIA,
+        g26_ooze_amount       = parser.linearval('O', OOZE_AMOUNT);
+
+  bool g26_continue_with_closest = parser.boolval('C'),
+       g26_keep_heaters_on       = parser.boolval('K');
+
+  // Accept 'I' if temperature presets are defined
+  #if PREHEAT_COUNT
+    const uint8_t preset_index = parser.seenval('I') ? _MIN(parser.value_byte(), PREHEAT_COUNT - 1) + 1 : 0;
+  #endif
+
+  #if HAS_HEATED_BED
+
+    // Get a temperature from 'I' or 'B'
+    int16_t bedtemp = 0;
+
+    // Use the 'I' index if temperature presets are defined
+    #if PREHEAT_COUNT
+      if (preset_index) bedtemp = ui.material_preset[preset_index - 1].bed_temp;
+    #endif
+
+    // Look for 'B' Bed Temperature
+    if (parser.seenval('B')) bedtemp = parser.value_celsius();
+
+    if (bedtemp) {
+      if (!WITHIN(bedtemp, 40, BED_MAX_TARGET)) {
+        SERIAL_ECHOLNPAIR("?Specified bed temperature not plausible (40-", int(BED_MAX_TARGET), "C).");
+        return;
+      }
+      g26_bed_temp = bedtemp;
+    }
+
+  #endif // HAS_HEATED_BED
+
+  if (parser.seenval('L')) {
+    g26_layer_height = parser.value_linear_units();
+    if (!WITHIN(g26_layer_height, 0.0, 2.0)) {
+      SERIAL_ECHOLNPGM("?Specified layer height not plausible.");
+      return;
+    }
+  }
+
+  if (parser.seen('Q')) {
+    if (parser.has_value()) {
+      g26_retraction_multiplier = parser.value_float();
+      if (!WITHIN(g26_retraction_multiplier, 0.05, 15.0)) {
+        SERIAL_ECHOLNPGM("?Specified Retraction Multiplier not plausible.");
+        return;
+      }
+    }
+    else {
+      SERIAL_ECHOLNPGM("?Retraction Multiplier must be specified.");
+      return;
+    }
+  }
+
+  if (parser.seenval('S')) {
+    g26_nozzle = parser.value_float();
+    if (!WITHIN(g26_nozzle, 0.1, 2.0)) {
+      SERIAL_ECHOLNPGM("?Specified nozzle size not plausible.");
+      return;
+    }
+  }
+
+  if (parser.seen('P')) {
+    if (!parser.has_value()) {
+      #if HAS_LCD_MENU
+        g26_prime_flag = -1;
+      #else
+        SERIAL_ECHOLNPGM("?Prime length must be specified when not using an LCD.");
+        return;
+      #endif
+    }
+    else {
+      g26_prime_flag++;
+      g26_prime_length = parser.value_linear_units();
+      if (!WITHIN(g26_prime_length, 0.0, 25.0)) {
+        SERIAL_ECHOLNPGM("?Specified prime length not plausible.");
+        return;
+      }
+    }
+  }
+
+  if (parser.seenval('F')) {
+    g26_filament_diameter = parser.value_linear_units();
+    if (!WITHIN(g26_filament_diameter, 1.0, 4.0)) {
+      SERIAL_ECHOLNPGM("?Specified filament size not plausible.");
+      return;
+    }
+  }
+  g26_extrusion_multiplier *= sq(1.75) / sq(g26_filament_diameter); // If we aren't using 1.75mm filament, we need to
+                                                                    // scale up or down the length needed to get the
+                                                                    // same volume of filament
+
+  g26_extrusion_multiplier *= g26_filament_diameter * sq(g26_nozzle) / sq(0.3); // Scale up by nozzle size
+
+  // Get a temperature from 'I' or 'H'
+  int16_t noztemp = 0;
+
+  // Accept 'I' if temperature presets are defined
+  #if PREHEAT_COUNT
+    if (preset_index) noztemp = ui.material_preset[preset_index - 1].hotend_temp;
+  #endif
+
+  // Look for 'H' Hotend Temperature
+  if (parser.seenval('H')) noztemp = parser.value_celsius();
+
+  // If any preset or temperature was specified
+  if (noztemp) {
+    if (!WITHIN(noztemp, 165, (HEATER_0_MAXTEMP) - (HOTEND_OVERSHOOT))) {
+      SERIAL_ECHOLNPGM("?Specified nozzle temperature not plausible.");
+      return;
+    }
+    g26_hotend_temp = noztemp;
+  }
+
+  // 'U' to Randomize and optionally set circle deviation
+  if (parser.seen('U')) {
+    randomSeed(millis());
+    // This setting will persist for the next G26
+    g26_random_deviation = parser.has_value() ? parser.value_float() : 50.0;
+  }
+
+  // Get repeat from 'R', otherwise do one full circuit
+  int16_t g26_repeats;
+  #if HAS_LCD_MENU
+    g26_repeats = parser.intval('R', GRID_MAX_POINTS + 1);
+  #else
+    if (!parser.seen('R')) {
+      SERIAL_ECHOLNPGM("?(R)epeat must be specified when not using an LCD.");
+      return;
+    }
+    else
+      g26_repeats = parser.has_value() ? parser.value_int() : GRID_MAX_POINTS + 1;
+  #endif
+  if (g26_repeats < 1) {
+    SERIAL_ECHOLNPGM("?(R)epeat value not plausible; must be at least 1.");
+    return;
+  }
+
+  // Set a position with 'X' and/or 'Y'. Default: current_position
+  g26_xy_pos.set(parser.seenval('X') ? RAW_X_POSITION(parser.value_linear_units()) : current_position.x,
+                 parser.seenval('Y') ? RAW_Y_POSITION(parser.value_linear_units()) : current_position.y);
+  if (!position_is_reachable(g26_xy_pos)) {
+    SERIAL_ECHOLNPGM("?Specified X,Y coordinate out of bounds.");
+    return;
+  }
+
+  /**
+   * Wait until all parameters are verified before altering the state!
+   */
+  set_bed_leveling_enabled(!parser.seen('D'));
+
+  do_z_clearance(Z_CLEARANCE_BETWEEN_PROBES);
+
+  #if DISABLED(NO_VOLUMETRICS)
+    bool volumetric_was_enabled = parser.volumetric_enabled;
+    parser.volumetric_enabled = false;
+    planner.calculate_volumetric_multipliers();
+  #endif
+
+  if (turn_on_heaters() != G26_OK) goto LEAVE;
+
+  current_position.e = 0.0;
+  sync_plan_position_e();
+
+  if (g26_prime_flag && prime_nozzle() != G26_OK) goto LEAVE;
+
+  /**
+   *  Bed is preheated
+   *
+   *  Nozzle is at temperature
+   *
+   *  Filament is primed!
+   *
+   *  It's  "Show Time" !!!
+   */
+
+  circle_flags.reset();
+  horizontal_mesh_line_flags.reset();
+  vertical_mesh_line_flags.reset();
+
+  // Move nozzle to the specified height for the first layer
+  destination = current_position;
+  destination.z = g26_layer_height;
+  move_to(destination, 0.0);
+  move_to(destination, g26_ooze_amount);
+
+  TERN_(HAS_LCD_MENU, ui.capture());
+
+  #if DISABLED(ARC_SUPPORT)
+
+    /**
+     * Pre-generate radius offset values at 30 degree intervals to reduce CPU load.
+     */
+    #define A_INT 30
+    #define _ANGS (360 / A_INT)
+    #define A_CNT (_ANGS / 2)
+    #define _IND(A) ((A + _ANGS * 8) % _ANGS)
+    #define _COS(A) (trig_table[_IND(A) % A_CNT] * (_IND(A) >= A_CNT ? -1 : 1))
+    #define _SIN(A) (-_COS((A + A_CNT / 2) % _ANGS))
+    #if A_CNT & 1
+      #error "A_CNT must be a positive value. Please change A_INT."
+    #endif
+    float trig_table[A_CNT];
+    LOOP_L_N(i, A_CNT)
+      trig_table[i] = INTERSECTION_CIRCLE_RADIUS * cos(RADIANS(i * A_INT));
+
+  #endif // !ARC_SUPPORT
+
+  mesh_index_pair location;
+  do {
+    // Find the nearest confluence
+    location = find_closest_circle_to_print(g26_continue_with_closest ? xy_pos_t(current_position) : g26_xy_pos);
+
+    if (location.valid()) {
+      const xy_pos_t circle = _GET_MESH_POS(location.pos);
+
+      // If this mesh location is outside the printable radius, skip it.
+      if (!position_is_reachable(circle)) continue;
+
+      // Determine where to start and end the circle,
+      // which is always drawn counter-clockwise.
+      const xy_int8_t st = location;
+      const bool f = st.y == 0,
+                 r = st.x >= GRID_MAX_POINTS_X - 1,
+                 b = st.y >= GRID_MAX_POINTS_Y - 1;
+
+      #if ENABLED(ARC_SUPPORT)
+
+        #define ARC_LENGTH(quarters)  (INTERSECTION_CIRCLE_RADIUS * M_PI * (quarters) / 2)
+        #define INTERSECTION_CIRCLE_DIAM  ((INTERSECTION_CIRCLE_RADIUS) * 2)
+
+        xy_float_t e = { circle.x + INTERSECTION_CIRCLE_RADIUS, circle.y };
+        xyz_float_t s = e;
+
+        // Figure out where to start and end the arc - we always print counterclockwise
+        float arc_length = ARC_LENGTH(4);
+        if (st.x == 0) {                             // left edge
+          if (!f) { s.x = circle.x; s.y -= INTERSECTION_CIRCLE_RADIUS; }
+          if (!b) { e.x = circle.x; e.y += INTERSECTION_CIRCLE_RADIUS; }
+          arc_length = (f || b) ? ARC_LENGTH(1) : ARC_LENGTH(2);
+        }
+        else if (r) {                             // right edge
+          if (b) s.set(circle.x - (INTERSECTION_CIRCLE_RADIUS), circle.y);
+          else   s.set(circle.x, circle.y + INTERSECTION_CIRCLE_RADIUS);
+          if (f) e.set(circle.x - (INTERSECTION_CIRCLE_RADIUS), circle.y);
+          else   e.set(circle.x, circle.y - (INTERSECTION_CIRCLE_RADIUS));
+          arc_length = (f || b) ? ARC_LENGTH(1) : ARC_LENGTH(2);
+        }
+        else if (f) {
+          e.x -= INTERSECTION_CIRCLE_DIAM;
+          arc_length = ARC_LENGTH(2);
+        }
+        else if (b) {
+          s.x -= INTERSECTION_CIRCLE_DIAM;
+          arc_length = ARC_LENGTH(2);
+        }
+
+        const ab_float_t arc_offset = circle - s;
+        const xy_float_t dist = current_position - s;   // Distance from the start of the actual circle
+        const float dist_start = HYPOT2(dist.x, dist.y);
+        const xyze_pos_t endpoint = {
+          e.x, e.y, g26_layer_height,
+          current_position.e + (arc_length * g26_e_axis_feedrate * g26_extrusion_multiplier)
+        };
+
+        if (dist_start > 2.0) {
+          s.z = g26_layer_height + 0.5f;
+          retract_lift_move(s);
+        }
+
+        s.z = g26_layer_height;
+        move_to(s, 0.0);  // Get to the starting point with no extrusion / un-Z lift
+
+        recover_filament(destination);
+
+        const feedRate_t old_feedrate = feedrate_mm_s;
+        feedrate_mm_s = PLANNER_XY_FEEDRATE() * 0.1f;
+        plan_arc(endpoint, arc_offset, false, 0);  // Draw a counter-clockwise arc
+        feedrate_mm_s = old_feedrate;
+        destination = current_position;
+
+        if (TERN0(HAS_LCD_MENU, user_canceled())) goto LEAVE; // Check if the user wants to stop the Mesh Validation
+
+      #else // !ARC_SUPPORT
+
+        int8_t start_ind = -2, end_ind = 9; // Assume a full circle (from 5:00 to 5:00)
+        if (st.x == 0) {                    // Left edge? Just right half.
+          start_ind = f ? 0 : -3;           //  03:00 to 12:00 for front-left
+          end_ind = b ? 0 : 2;              //  06:00 to 03:00 for back-left
+        }
+        else if (r) {                       // Right edge? Just left half.
+          start_ind = b ? 6 : 3;            //  12:00 to 09:00 for front-right
+          end_ind = f ? 5 : 8;              //  09:00 to 06:00 for back-right
+        }
+        else if (f) {                       // Front edge? Just back half.
+          start_ind = 0;                    //  03:00
+          end_ind = 5;                      //  09:00
+        }
+        else if (b) {                       // Back edge? Just front half.
+          start_ind = 6;                    //  09:00
+          end_ind = 11;                     //  03:00
+        }
+
+        for (int8_t ind = start_ind; ind <= end_ind; ind++) {
+
+          if (TERN0(HAS_LCD_MENU, user_canceled())) goto LEAVE; // Check if the user wants to stop the Mesh Validation
+
+          xyz_float_t p = { circle.x + _COS(ind    ), circle.y + _SIN(ind    ), g26_layer_height },
+                      q = { circle.x + _COS(ind + 1), circle.y + _SIN(ind + 1), g26_layer_height };
+
+          #if IS_KINEMATIC
+            // Check to make sure this segment is entirely on the bed, skip if not.
+            if (!position_is_reachable(p) || !position_is_reachable(q)) continue;
+          #else
+            LIMIT(p.x, X_MIN_POS + 1, X_MAX_POS - 1); // Prevent hitting the endstops
+            LIMIT(p.y, Y_MIN_POS + 1, Y_MAX_POS - 1);
+            LIMIT(q.x, X_MIN_POS + 1, X_MAX_POS - 1);
+            LIMIT(q.y, Y_MIN_POS + 1, Y_MAX_POS - 1);
+          #endif
+
+          print_line_from_here_to_there(p, q);
+          SERIAL_FLUSH();   // Prevent host M105 buffer overrun.
+        }
+
+      #endif // !ARC_SUPPORT
+
+      if (look_for_lines_to_connect()) goto LEAVE;
+    }
+
+    SERIAL_FLUSH(); // Prevent host M105 buffer overrun.
+
+  } while (--g26_repeats && location.valid());
+
+  LEAVE:
+  ui.set_status_P(GET_TEXT(MSG_G26_LEAVING), -1);
+
+  retract_filament(destination);
+  destination.z = Z_CLEARANCE_BETWEEN_PROBES;
+  move_to(destination, 0);                                    // Raise the nozzle
+
+  destination = g26_xy_pos;                                   // Move back to the starting XY position
+  move_to(destination, 0);                                    // Move back to the starting position
+
+  #if DISABLED(NO_VOLUMETRICS)
+    parser.volumetric_enabled = volumetric_was_enabled;
+    planner.calculate_volumetric_multipliers();
+  #endif
+
+  TERN_(HAS_LCD_MENU, ui.release()); // Give back control of the LCD
+
+  if (!g26_keep_heaters_on) {
+    TERN_(HAS_HEATED_BED, thermalManager.setTargetBed(0));
+    thermalManager.setTargetHotend(active_extruder, 0);
+  }
+}
+
+#endif // G26_MESH_VALIDATION
diff --git a/Marlin/src/gcode/bedlevel/G35.cpp b/Marlin/src/gcode/bedlevel/G35.cpp
new file mode 100644
index 0000000..46f75f2
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/G35.cpp
@@ -0,0 +1,167 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(ASSISTED_TRAMMING)
+
+#include "../gcode.h"
+#include "../../module/planner.h"
+#include "../../module/probe.h"
+#include "../../feature/bedlevel/bedlevel.h"
+
+#if HAS_MULTI_HOTEND
+  #include "../../module/tool_change.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../core/debug_out.h"
+
+//
+// Define tramming point names.
+//
+
+#include "../../feature/tramming.h"
+
+/**
+ * G35: Read bed corners to help adjust bed screws
+ *
+ *   S<screw_thread>
+ *
+ * Screw thread: 30 - Clockwise M3
+ *               31 - Counter-Clockwise M3
+ *               40 - Clockwise M4
+ *               41 - Counter-Clockwise M4
+ *               50 - Clockwise M5
+ *               51 - Counter-Clockwise M5
+ **/
+void GcodeSuite::G35() {
+  DEBUG_SECTION(log_G35, "G35", DEBUGGING(LEVELING));
+
+  if (DEBUGGING(LEVELING)) log_machine_info();
+
+  float z_measured[G35_PROBE_COUNT] = { 0 };
+
+  const uint8_t screw_thread = parser.byteval('S', TRAMMING_SCREW_THREAD);
+  if (!WITHIN(screw_thread, 30, 51) || screw_thread % 10 > 1) {
+    SERIAL_ECHOLNPGM("?(S)crew thread must be 30, 31, 40, 41, 50, or 51.");
+    return;
+  }
+
+  // Wait for planner moves to finish!
+  planner.synchronize();
+
+  // Disable the leveling matrix before auto-aligning
+  #if HAS_LEVELING
+    TERN_(RESTORE_LEVELING_AFTER_G35, const bool leveling_was_active = planner.leveling_active);
+    set_bed_leveling_enabled(false);
+  #endif
+
+  #if ENABLED(CNC_WORKSPACE_PLANES)
+    workspace_plane = PLANE_XY;
+  #endif
+
+  // Always home with tool 0 active
+  #if HAS_MULTI_HOTEND
+    const uint8_t old_tool_index = active_extruder;
+    tool_change(0, true);
+  #endif
+
+  // Disable duplication mode on homing
+  TERN_(HAS_DUPLICATION_MODE, set_duplication_enabled(false));
+
+  // Home all before this procedure
+  home_all_axes();
+
+  bool err_break = false;
+
+  // Probe all positions
+  LOOP_L_N(i, G35_PROBE_COUNT) {
+
+    // In BLTOUCH HS mode, the probe travels in a deployed state.
+    // Users of G35 might have a badly misaligned bed, so raise Z by the
+    // length of the deployed pin (BLTOUCH stroke < 7mm)
+    do_blocking_move_to_z((Z_CLEARANCE_BETWEEN_PROBES) + TERN0(BLTOUCH_HS_MODE, 7));
+    const float z_probed_height = probe.probe_at_point(screws_tilt_adjust_pos[i], PROBE_PT_RAISE, 0, true);
+
+    if (isnan(z_probed_height)) {
+      SERIAL_ECHOPAIR("G35 failed at point ", int(i), " (");
+      SERIAL_ECHOPGM_P((char *)pgm_read_ptr(&tramming_point_name[i]));
+      SERIAL_CHAR(')');
+      SERIAL_ECHOLNPAIR_P(SP_X_STR, screws_tilt_adjust_pos[i].x, SP_Y_STR, screws_tilt_adjust_pos[i].y);
+      err_break = true;
+      break;
+    }
+
+    if (DEBUGGING(LEVELING)) {
+      DEBUG_ECHOPAIR("Probing point ", int(i), " (");
+      DEBUG_PRINT_P((char *)pgm_read_ptr(&tramming_point_name[i]));
+      DEBUG_CHAR(')');
+      DEBUG_ECHOLNPAIR_P(SP_X_STR, screws_tilt_adjust_pos[i].x, SP_Y_STR, screws_tilt_adjust_pos[i].y, SP_Z_STR, z_probed_height);
+    }
+
+    z_measured[i] = z_probed_height;
+  }
+
+  if (!err_break) {
+    const float threads_factor[] = { 0.5, 0.7, 0.8 };
+
+    // Calculate adjusts
+    LOOP_S_L_N(i, 1, G35_PROBE_COUNT) {
+      const float diff = z_measured[0] - z_measured[i],
+                  adjust = abs(diff) < 0.001f ? 0 : diff / threads_factor[(screw_thread - 30) / 10];
+
+      const int full_turns = trunc(adjust);
+      const float decimal_part = adjust - float(full_turns);
+      const int minutes = trunc(decimal_part * 60.0f);
+
+      SERIAL_ECHOPGM("Turn ");
+      SERIAL_ECHOPGM_P((char *)pgm_read_ptr(&tramming_point_name[i]));
+      SERIAL_ECHOPAIR(" ", (screw_thread & 1) == (adjust > 0) ? "CCW" : "CW", " by ", abs(full_turns), " turns");
+      if (minutes) SERIAL_ECHOPAIR(" and ", abs(minutes), " minutes");
+      if (ENABLED(REPORT_TRAMMING_MM)) SERIAL_ECHOPAIR(" (", -diff, "mm)");
+      SERIAL_EOL();
+    }
+  }
+  else
+    SERIAL_ECHOLNPGM("G35 aborted.");
+
+  // Restore the active tool after homing
+  #if HAS_MULTI_HOTEND
+    tool_change(old_tool_index, DISABLED(PARKING_EXTRUDER)); // Fetch previous toolhead if not PARKING_EXTRUDER
+  #endif
+
+  #if BOTH(HAS_LEVELING, RESTORE_LEVELING_AFTER_G35)
+    set_bed_leveling_enabled(leveling_was_active);
+  #endif
+
+  // Stow the probe, as the last call to probe.probe_at_point(...) left
+  // the probe deployed if it was successful.
+  probe.stow();
+
+  move_to_tramming_wait_pos();
+
+  // After this operation the Z position needs correction
+  set_axis_never_homed(Z_AXIS);
+}
+
+#endif // ASSISTED_TRAMMING
diff --git a/Marlin/src/gcode/bedlevel/G42.cpp b/Marlin/src/gcode/bedlevel/G42.cpp
new file mode 100644
index 0000000..a2896ed
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/G42.cpp
@@ -0,0 +1,73 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_MESH
+
+#include "../gcode.h"
+#include "../../MarlinCore.h" // for IsRunning()
+#include "../../module/motion.h"
+#include "../../module/probe.h" // for probe.offset
+#include "../../feature/bedlevel/bedlevel.h"
+
+/**
+ * G42: Move X & Y axes to mesh coordinates (I & J)
+ */
+void GcodeSuite::G42() {
+  if (MOTION_CONDITIONS) {
+    const bool hasI = parser.seenval('I');
+    const int8_t ix = hasI ? parser.value_int() : 0;
+    const bool hasJ = parser.seenval('J');
+    const int8_t iy = hasJ ? parser.value_int() : 0;
+
+    if ((hasI && !WITHIN(ix, 0, GRID_MAX_POINTS_X - 1)) || (hasJ && !WITHIN(iy, 0, GRID_MAX_POINTS_Y - 1))) {
+      SERIAL_ECHOLNPGM(STR_ERR_MESH_XY);
+      return;
+    }
+
+    // Move to current_position, as modified by I, J, P parameters
+    destination = current_position;
+
+    if (hasI) destination.x = _GET_MESH_X(ix);
+    if (hasJ) destination.y = _GET_MESH_Y(iy);
+
+    #if HAS_PROBE_XY_OFFSET
+      if (parser.boolval('P')) {
+        if (hasI) destination.x -= probe.offset_xy.x;
+        if (hasJ) destination.y -= probe.offset_xy.y;
+      }
+    #endif
+
+    const feedRate_t fval = parser.linearval('F'),
+                     fr_mm_s = MMM_TO_MMS(fval > 0 ? fval : 0.0f);
+
+    // SCARA kinematic has "safe" XY raw moves
+    #if IS_SCARA
+      prepare_internal_fast_move_to_destination(fr_mm_s);
+    #else
+      prepare_internal_move_to_destination(fr_mm_s);
+    #endif
+  }
+}
+
+#endif // HAS_MESH
diff --git a/Marlin/src/gcode/bedlevel/M420.cpp b/Marlin/src/gcode/bedlevel/M420.cpp
new file mode 100644
index 0000000..96122c1
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/M420.cpp
@@ -0,0 +1,245 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_LEVELING
+
+#include "../gcode.h"
+#include "../../feature/bedlevel/bedlevel.h"
+#include "../../module/planner.h"
+#include "../../module/probe.h"
+
+#if ENABLED(EEPROM_SETTINGS)
+  #include "../../module/settings.h"
+#endif
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../lcd/extui/ui_api.h"
+#endif
+
+//#define M420_C_USE_MEAN
+
+/**
+ * M420: Enable/Disable Bed Leveling and/or set the Z fade height.
+ *
+ *   S[bool]   Turns leveling on or off
+ *   Z[height] Sets the Z fade height (0 or none to disable)
+ *   V[bool]   Verbose - Print the leveling grid
+ *
+ * With AUTO_BED_LEVELING_UBL only:
+ *
+ *   L[index]  Load UBL mesh from index (0 is default)
+ *   T[map]    0:Human-readable 1:CSV 2:"LCD" 4:Compact
+ *
+ * With mesh-based leveling only:
+ *
+ *   C         Center mesh on the mean of the lowest and highest
+ *
+ * With MARLIN_DEV_MODE:
+ *   S2        Create a simple random mesh and enable
+ */
+void GcodeSuite::M420() {
+  const bool seen_S = parser.seen('S'),
+             to_enable = seen_S ? parser.value_bool() : planner.leveling_active;
+
+  #if ENABLED(MARLIN_DEV_MODE)
+    if (parser.intval('S') == 2) {
+      const float x_min = probe.min_x(), x_max = probe.max_x(),
+                  y_min = probe.min_y(), y_max = probe.max_y();
+      #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+        bilinear_start.set(x_min, y_min);
+        bilinear_grid_spacing.set((x_max - x_min) / (GRID_MAX_POINTS_X - 1),
+                                  (y_max - y_min) / (GRID_MAX_POINTS_Y - 1));
+      #endif
+      GRID_LOOP(x, y) {
+        Z_VALUES(x, y) = 0.001 * random(-200, 200);
+        TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, Z_VALUES(x, y)));
+      }
+      SERIAL_ECHOPGM("Simulated " STRINGIFY(GRID_MAX_POINTS_X) "x" STRINGIFY(GRID_MAX_POINTS_Y) " mesh ");
+      SERIAL_ECHOPAIR(" (", x_min);
+      SERIAL_CHAR(','); SERIAL_ECHO(y_min);
+      SERIAL_ECHOPAIR(")-(", x_max);
+      SERIAL_CHAR(','); SERIAL_ECHO(y_max);
+      SERIAL_ECHOLNPGM(")");
+    }
+  #endif
+
+  xyz_pos_t oldpos = current_position;
+
+  // If disabling leveling do it right away
+  // (Don't disable for just M420 or M420 V)
+  if (seen_S && !to_enable) set_bed_leveling_enabled(false);
+
+  #if ENABLED(AUTO_BED_LEVELING_UBL)
+
+    // L to load a mesh from the EEPROM
+    if (parser.seen('L')) {
+
+      set_bed_leveling_enabled(false);
+
+      #if ENABLED(EEPROM_SETTINGS)
+        const int8_t storage_slot = parser.has_value() ? parser.value_int() : ubl.storage_slot;
+        const int16_t a = settings.calc_num_meshes();
+
+        if (!a) {
+          SERIAL_ECHOLNPGM("?EEPROM storage not available.");
+          return;
+        }
+
+        if (!WITHIN(storage_slot, 0, a - 1)) {
+          SERIAL_ECHOLNPGM("?Invalid storage slot.");
+          SERIAL_ECHOLNPAIR("?Use 0 to ", a - 1);
+          return;
+        }
+
+        settings.load_mesh(storage_slot);
+        ubl.storage_slot = storage_slot;
+
+      #else
+
+        SERIAL_ECHOLNPGM("?EEPROM storage not available.");
+        return;
+
+      #endif
+    }
+
+    // L or V display the map info
+    if (parser.seen("LV")) {
+      ubl.display_map(parser.byteval('T'));
+      SERIAL_ECHOPGM("Mesh is ");
+      if (!ubl.mesh_is_valid()) SERIAL_ECHOPGM("in");
+      SERIAL_ECHOLNPAIR("valid\nStorage slot: ", ubl.storage_slot);
+    }
+
+  #endif // AUTO_BED_LEVELING_UBL
+
+  const bool seenV = parser.seen('V');
+
+  #if HAS_MESH
+
+    if (leveling_is_valid()) {
+
+      // Subtract the given value or the mean from all mesh values
+      if (parser.seen('C')) {
+        const float cval = parser.value_float();
+        #if ENABLED(AUTO_BED_LEVELING_UBL)
+
+          set_bed_leveling_enabled(false);
+          ubl.adjust_mesh_to_mean(true, cval);
+
+        #else
+
+          #if ENABLED(M420_C_USE_MEAN)
+
+            // Get the sum and average of all mesh values
+            float mesh_sum = 0;
+            GRID_LOOP(x, y) mesh_sum += Z_VALUES(x, y);
+            const float zmean = mesh_sum / float(GRID_MAX_POINTS);
+
+          #else
+
+            // Find the low and high mesh values
+            float lo_val = 100, hi_val = -100;
+            GRID_LOOP(x, y) {
+              const float z = Z_VALUES(x, y);
+              NOMORE(lo_val, z);
+              NOLESS(hi_val, z);
+            }
+            // Take the mean of the lowest and highest
+            const float zmean = (lo_val + hi_val) / 2.0 + cval;
+
+          #endif
+
+          // If not very close to 0, adjust the mesh
+          if (!NEAR_ZERO(zmean)) {
+            set_bed_leveling_enabled(false);
+            // Subtract the mean from all values
+            GRID_LOOP(x, y) {
+              Z_VALUES(x, y) -= zmean;
+              TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, Z_VALUES(x, y)));
+            }
+            TERN_(ABL_BILINEAR_SUBDIVISION, bed_level_virt_interpolate());
+          }
+
+        #endif
+      }
+
+    }
+    else if (to_enable || seenV) {
+      SERIAL_ECHO_MSG("Invalid mesh.");
+      goto EXIT_M420;
+    }
+
+  #endif // HAS_MESH
+
+  // V to print the matrix or mesh
+  if (seenV) {
+    #if ABL_PLANAR
+      planner.bed_level_matrix.debug(PSTR("Bed Level Correction Matrix:"));
+    #else
+      if (leveling_is_valid()) {
+        #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+          print_bilinear_leveling_grid();
+          TERN_(ABL_BILINEAR_SUBDIVISION, print_bilinear_leveling_grid_virt());
+        #elif ENABLED(MESH_BED_LEVELING)
+          SERIAL_ECHOLNPGM("Mesh Bed Level data:");
+          mbl.report_mesh();
+        #endif
+      }
+    #endif
+  }
+
+  #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+    if (parser.seen('Z')) set_z_fade_height(parser.value_linear_units(), false);
+  #endif
+
+  // Enable leveling if specified, or if previously active
+  set_bed_leveling_enabled(to_enable);
+
+  #if HAS_MESH
+    EXIT_M420:
+  #endif
+
+  // Error if leveling failed to enable or reenable
+  if (to_enable && !planner.leveling_active)
+    SERIAL_ERROR_MSG(STR_ERR_M420_FAILED);
+
+  SERIAL_ECHO_START();
+  SERIAL_ECHOPGM("Bed Leveling ");
+  serialprintln_onoff(planner.leveling_active);
+
+  #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM("Fade Height ");
+    if (planner.z_fade_height > 0.0)
+      SERIAL_ECHOLN(planner.z_fade_height);
+    else
+      SERIAL_ECHOLNPGM(STR_OFF);
+  #endif
+
+  // Report change in position
+  if (oldpos != current_position)
+    report_current_position();
+}
+
+#endif // HAS_LEVELING
diff --git a/Marlin/src/gcode/bedlevel/abl/G29.cpp b/Marlin/src/gcode/bedlevel/abl/G29.cpp
new file mode 100644
index 0000000..2e80f09
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/abl/G29.cpp
@@ -0,0 +1,901 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * G29.cpp - Auto Bed Leveling
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if HAS_ABL_NOT_UBL
+
+#include "../../gcode.h"
+#include "../../../feature/bedlevel/bedlevel.h"
+#include "../../../module/motion.h"
+#include "../../../module/planner.h"
+#include "../../../module/stepper.h"
+#include "../../../module/probe.h"
+#include "../../queue.h"
+
+#if ENABLED(PROBE_TEMP_COMPENSATION)
+  #include "../../../feature/probe_temp_comp.h"
+  #include "../../../module/temperature.h"
+#endif
+
+#if HAS_DISPLAY
+  #include "../../../lcd/marlinui.h"
+#endif
+
+#if ENABLED(AUTO_BED_LEVELING_LINEAR)
+  #include "../../../libs/least_squares_fit.h"
+#endif
+
+#if ABL_PLANAR
+  #include "../../../libs/vector_3.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
+
+#if ENABLED(DWIN_CREALITY_LCD)
+  #include "../../../lcd/dwin/e3v2/dwin.h"
+#endif
+
+#if HAS_MULTI_HOTEND
+  #include "../../../module/tool_change.h"
+#endif
+
+#if ABL_GRID
+  #if ENABLED(PROBE_Y_FIRST)
+    #define PR_OUTER_VAR meshCount.x
+    #define PR_OUTER_END abl_grid_points.x
+    #define PR_INNER_VAR meshCount.y
+    #define PR_INNER_END abl_grid_points.y
+  #else
+    #define PR_OUTER_VAR meshCount.y
+    #define PR_OUTER_END abl_grid_points.y
+    #define PR_INNER_VAR meshCount.x
+    #define PR_INNER_END abl_grid_points.x
+  #endif
+#endif
+
+#define G29_RETURN(b) return TERN_(G29_RETRY_AND_RECOVER, b)
+
+/**
+ * G29: Detailed Z probe, probes the bed at 3 or more points.
+ *      Will fail if the printer has not been homed with G28.
+ *
+ * Enhanced G29 Auto Bed Leveling Probe Routine
+ *
+ *  O  Auto-level only if needed
+ *
+ *  D  Dry-Run mode. Just evaluate the bed Topology - Don't apply
+ *     or alter the bed level data. Useful to check the topology
+ *     after a first run of G29.
+ *
+ *  J  Jettison current bed leveling data
+ *
+ *  V  Set the verbose level (0-4). Example: "G29 V3"
+ *
+ * Parameters With LINEAR leveling only:
+ *
+ *  P  Set the size of the grid that will be probed (P x P points).
+ *     Example: "G29 P4"
+ *
+ *  X  Set the X size of the grid that will be probed (X x Y points).
+ *     Example: "G29 X7 Y5"
+ *
+ *  Y  Set the Y size of the grid that will be probed (X x Y points).
+ *
+ *  T  Generate a Bed Topology Report. Example: "G29 P5 T" for a detailed report.
+ *     This is useful for manual bed leveling and finding flaws in the bed (to
+ *     assist with part placement).
+ *     Not supported by non-linear delta printer bed leveling.
+ *
+ * Parameters With LINEAR and BILINEAR leveling only:
+ *
+ *  S  Set the XY travel speed between probe points (in units/min)
+ *
+ *  H  Set bounds to a centered square H x H units in size
+ *
+ *     -or-
+ *
+ *  F  Set the Front limit of the probing grid
+ *  B  Set the Back limit of the probing grid
+ *  L  Set the Left limit of the probing grid
+ *  R  Set the Right limit of the probing grid
+ *
+ * Parameters with DEBUG_LEVELING_FEATURE only:
+ *
+ *  C  Make a totally fake grid with no actual probing.
+ *     For use in testing when no probing is possible.
+ *
+ * Parameters with BILINEAR leveling only:
+ *
+ *  Z  Supply an additional Z probe offset
+ *
+ * Extra parameters with PROBE_MANUALLY:
+ *
+ *  To do manual probing simply repeat G29 until the procedure is complete.
+ *  The first G29 accepts parameters. 'G29 Q' for status, 'G29 A' to abort.
+ *
+ *  Q  Query leveling and G29 state
+ *
+ *  A  Abort current leveling procedure
+ *
+ * Extra parameters with BILINEAR only:
+ *
+ *  W  Write a mesh point. (If G29 is idle.)
+ *  I  X index for mesh point
+ *  J  Y index for mesh point
+ *  X  X for mesh point, overrides I
+ *  Y  Y for mesh point, overrides J
+ *  Z  Z for mesh point. Otherwise, raw current Z.
+ *
+ * Without PROBE_MANUALLY:
+ *
+ *  E  By default G29 will engage the Z probe, test the bed, then disengage.
+ *     Include "E" to engage/disengage the Z probe for each sample.
+ *     There's no extra effect if you have a fixed Z probe.
+ */
+G29_TYPE GcodeSuite::G29() {
+
+  reset_stepper_timeout();
+
+  const bool seenQ = EITHER(DEBUG_LEVELING_FEATURE, PROBE_MANUALLY) && parser.seen('Q');
+
+  // G29 Q is also available if debugging
+  #if ENABLED(DEBUG_LEVELING_FEATURE)
+    const uint8_t old_debug_flags = marlin_debug_flags;
+    if (seenQ) marlin_debug_flags |= MARLIN_DEBUG_LEVELING;
+    DEBUG_SECTION(log_G29, "G29", DEBUGGING(LEVELING));
+    if (DEBUGGING(LEVELING)) log_machine_info();
+    marlin_debug_flags = old_debug_flags;
+    if (DISABLED(PROBE_MANUALLY) && seenQ) G29_RETURN(false);
+  #endif
+
+  const bool seenA = TERN0(PROBE_MANUALLY, parser.seen('A')),
+         no_action = seenA || seenQ,
+              faux = ENABLED(DEBUG_LEVELING_FEATURE) && DISABLED(PROBE_MANUALLY) ? parser.boolval('C') : no_action;
+
+  if (!no_action && planner.leveling_active && parser.boolval('O')) { // Auto-level only if needed
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> Auto-level not needed, skip");
+    G29_RETURN(false);
+  }
+
+  // Send 'N' to force homing before G29 (internal only)
+  if (parser.seen('N'))
+    process_subcommands_now_P(TERN(G28_L0_ENSURES_LEVELING_OFF, PSTR("G28L0"), G28_STR));
+
+  // Don't allow auto-leveling without homing first
+  if (homing_needed_error()) G29_RETURN(false);
+
+  // Define local vars 'static' for manual probing, 'auto' otherwise
+  #define ABL_VAR TERN_(PROBE_MANUALLY, static)
+
+  ABL_VAR int verbose_level;
+  ABL_VAR xy_pos_t probePos;
+  ABL_VAR float measured_z;
+  ABL_VAR bool dryrun, abl_should_enable;
+
+  #if EITHER(PROBE_MANUALLY, AUTO_BED_LEVELING_LINEAR)
+    ABL_VAR int abl_probe_index;
+  #endif
+
+  #if ABL_GRID
+
+    #if ENABLED(PROBE_MANUALLY)
+      ABL_VAR xy_int8_t meshCount;
+    #endif
+
+    ABL_VAR xy_pos_t probe_position_lf, probe_position_rb;
+    ABL_VAR xy_float_t gridSpacing = { 0, 0 };
+
+    #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+      ABL_VAR bool do_topography_map;
+      ABL_VAR xy_uint8_t abl_grid_points;
+    #else // Bilinear
+      constexpr xy_uint8_t abl_grid_points = { GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y };
+    #endif
+
+    #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+      ABL_VAR int abl_points;
+    #else
+      int constexpr abl_points = GRID_MAX_POINTS;
+    #endif
+
+    #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+      ABL_VAR float zoffset;
+
+    #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
+
+      ABL_VAR int indexIntoAB[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
+
+      ABL_VAR float eqnAMatrix[(GRID_MAX_POINTS) * 3], // "A" matrix of the linear system of equations
+                    eqnBVector[GRID_MAX_POINTS],       // "B" vector of Z points
+                    mean;
+    #endif
+
+  #elif ENABLED(AUTO_BED_LEVELING_3POINT)
+
+    #if ENABLED(PROBE_MANUALLY)
+      int constexpr abl_points = 3; // used to show total points
+    #endif
+
+    vector_3 points[3];
+    probe.get_three_points(points);
+
+  #endif // AUTO_BED_LEVELING_3POINT
+
+  #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+    struct linear_fit_data lsf_results;
+  #endif
+
+  /**
+   * On the initial G29 fetch command parameters.
+   */
+  if (!g29_in_progress) {
+
+    TERN_(HAS_MULTI_HOTEND, if (active_extruder) tool_change(0));
+
+    #if EITHER(PROBE_MANUALLY, AUTO_BED_LEVELING_LINEAR)
+      abl_probe_index = -1;
+    #endif
+
+    abl_should_enable = planner.leveling_active;
+
+    #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+      const bool seen_w = parser.seen('W');
+      if (seen_w) {
+        if (!leveling_is_valid()) {
+          SERIAL_ERROR_MSG("No bilinear grid");
+          G29_RETURN(false);
+        }
+
+        const float rz = parser.seenval('Z') ? RAW_Z_POSITION(parser.value_linear_units()) : current_position.z;
+        if (!WITHIN(rz, -10, 10)) {
+          SERIAL_ERROR_MSG("Bad Z value");
+          G29_RETURN(false);
+        }
+
+        const float rx = RAW_X_POSITION(parser.linearval('X', NAN)),
+                    ry = RAW_Y_POSITION(parser.linearval('Y', NAN));
+        int8_t i = parser.byteval('I', -1), j = parser.byteval('J', -1);
+
+        if (!isnan(rx) && !isnan(ry)) {
+          // Get nearest i / j from rx / ry
+          i = (rx - bilinear_start.x + 0.5 * gridSpacing.x) / gridSpacing.x;
+          j = (ry - bilinear_start.y + 0.5 * gridSpacing.y) / gridSpacing.y;
+          LIMIT(i, 0, GRID_MAX_POINTS_X - 1);
+          LIMIT(j, 0, GRID_MAX_POINTS_Y - 1);
+        }
+        if (WITHIN(i, 0, GRID_MAX_POINTS_X - 1) && WITHIN(j, 0, GRID_MAX_POINTS_Y)) {
+          set_bed_leveling_enabled(false);
+          z_values[i][j] = rz;
+          TERN_(ABL_BILINEAR_SUBDIVISION, bed_level_virt_interpolate());
+          TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(i, j, rz));
+          set_bed_leveling_enabled(abl_should_enable);
+          if (abl_should_enable) report_current_position();
+        }
+        G29_RETURN(false);
+      } // parser.seen('W')
+
+    #else
+
+      constexpr bool seen_w = false;
+
+    #endif
+
+    // Jettison bed leveling data
+    if (!seen_w && parser.seen('J')) {
+      reset_bed_level();
+      G29_RETURN(false);
+    }
+
+    verbose_level = parser.intval('V');
+    if (!WITHIN(verbose_level, 0, 4)) {
+      SERIAL_ECHOLNPGM("?(V)erbose level implausible (0-4).");
+      G29_RETURN(false);
+    }
+
+    dryrun = parser.boolval('D') || TERN0(PROBE_MANUALLY, no_action);
+
+    #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+
+      incremental_LSF_reset(&lsf_results);
+
+      do_topography_map = verbose_level > 2 || parser.boolval('T');
+
+      // X and Y specify points in each direction, overriding the default
+      // These values may be saved with the completed mesh
+      abl_grid_points.set(
+        parser.byteval('X', GRID_MAX_POINTS_X),
+        parser.byteval('Y', GRID_MAX_POINTS_Y)
+      );
+      if (parser.seenval('P')) abl_grid_points.x = abl_grid_points.y = parser.value_int();
+
+      if (!WITHIN(abl_grid_points.x, 2, GRID_MAX_POINTS_X)) {
+        SERIAL_ECHOLNPGM("?Probe points (X) implausible (2-" STRINGIFY(GRID_MAX_POINTS_X) ").");
+        G29_RETURN(false);
+      }
+      if (!WITHIN(abl_grid_points.y, 2, GRID_MAX_POINTS_Y)) {
+        SERIAL_ECHOLNPGM("?Probe points (Y) implausible (2-" STRINGIFY(GRID_MAX_POINTS_Y) ").");
+        G29_RETURN(false);
+      }
+
+      abl_points = abl_grid_points.x * abl_grid_points.y;
+      mean = 0;
+
+    #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+      zoffset = parser.linearval('Z');
+
+    #endif
+
+    #if ABL_GRID
+
+      xy_probe_feedrate_mm_s = MMM_TO_MMS(parser.linearval('S', XY_PROBE_SPEED));
+
+      const float x_min = probe.min_x(), x_max = probe.max_x(),
+                  y_min = probe.min_y(), y_max = probe.max_y();
+
+      if (parser.seen('H')) {
+        const int16_t size = (int16_t)parser.value_linear_units();
+        probe_position_lf.set(_MAX((X_CENTER) - size / 2, x_min), _MAX((Y_CENTER) - size / 2, y_min));
+        probe_position_rb.set(_MIN(probe_position_lf.x + size, x_max), _MIN(probe_position_lf.y + size, y_max));
+      }
+      else {
+        probe_position_lf.set(parser.linearval('L', x_min), parser.linearval('F', y_min));
+        probe_position_rb.set(parser.linearval('R', x_max), parser.linearval('B', y_max));
+      }
+
+      if (!probe.good_bounds(probe_position_lf, probe_position_rb)) {
+        if (DEBUGGING(LEVELING)) {
+          DEBUG_ECHOLNPAIR("G29 L", probe_position_lf.x, " R", probe_position_rb.x,
+                              " F", probe_position_lf.y, " B", probe_position_rb.y);
+        }
+        SERIAL_ECHOLNPGM("? (L,R,F,B) out of bounds.");
+        G29_RETURN(false);
+      }
+
+      // Probe at the points of a lattice grid
+      gridSpacing.set((probe_position_rb.x - probe_position_lf.x) / (abl_grid_points.x - 1),
+                      (probe_position_rb.y - probe_position_lf.y) / (abl_grid_points.y - 1));
+
+    #endif // ABL_GRID
+
+    if (verbose_level > 0) {
+      SERIAL_ECHOPGM("G29 Auto Bed Leveling");
+      if (dryrun) SERIAL_ECHOPGM(" (DRYRUN)");
+      SERIAL_EOL();
+    }
+
+    planner.synchronize();
+
+    #if ENABLED(AUTO_BED_LEVELING_3POINT)
+      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> 3-point Leveling");
+      points[0].z = points[1].z = points[2].z = 0;  // Probe at 3 arbitrary points
+    #endif
+
+    #if BOTH(AUTO_BED_LEVELING_BILINEAR, EXTENSIBLE_UI)
+      ExtUI::onMeshLevelingStart();
+    #endif
+
+    if (!faux) {
+      remember_feedrate_scaling_off();
+
+      #if ENABLED(PREHEAT_BEFORE_LEVELING)
+        if (!dryrun) probe.preheat_for_probing(LEVELING_NOZZLE_TEMP, LEVELING_BED_TEMP);
+      #endif
+    }
+
+    // Disable auto bed leveling during G29.
+    // Be formal so G29 can be done successively without G28.
+    if (!no_action) set_bed_leveling_enabled(false);
+
+    // Deploy certain probes before starting probing
+    #if HAS_BED_PROBE
+      if (ENABLED(BLTOUCH))
+        do_z_clearance(Z_CLEARANCE_DEPLOY_PROBE);
+      else if (probe.deploy()) {
+        set_bed_leveling_enabled(abl_should_enable);
+        G29_RETURN(false);
+      }
+    #endif
+
+    #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+      if (TERN1(PROBE_MANUALLY, !no_action)
+        && (gridSpacing != bilinear_grid_spacing || probe_position_lf != bilinear_start)
+      ) {
+        // Reset grid to 0.0 or "not probed". (Also disables ABL)
+        reset_bed_level();
+
+        // Initialize a grid with the given dimensions
+        bilinear_grid_spacing = gridSpacing;
+        bilinear_start = probe_position_lf;
+
+        // Can't re-enable (on error) until the new grid is written
+        abl_should_enable = false;
+      }
+    #endif // AUTO_BED_LEVELING_BILINEAR
+
+  } // !g29_in_progress
+
+  #if ENABLED(PROBE_MANUALLY)
+
+    // For manual probing, get the next index to probe now.
+    // On the first probe this will be incremented to 0.
+    if (!no_action) {
+      ++abl_probe_index;
+      g29_in_progress = true;
+    }
+
+    // Abort current G29 procedure, go back to idle state
+    if (seenA && g29_in_progress) {
+      SERIAL_ECHOLNPGM("Manual G29 aborted");
+      SET_SOFT_ENDSTOP_LOOSE(false);
+      set_bed_leveling_enabled(abl_should_enable);
+      g29_in_progress = false;
+      TERN_(LCD_BED_LEVELING, ui.wait_for_move = false);
+    }
+
+    // Query G29 status
+    if (verbose_level || seenQ) {
+      SERIAL_ECHOPGM("Manual G29 ");
+      if (g29_in_progress) {
+        SERIAL_ECHOPAIR("point ", _MIN(abl_probe_index + 1, abl_points));
+        SERIAL_ECHOLNPAIR(" of ", abl_points);
+      }
+      else
+        SERIAL_ECHOLNPGM("idle");
+    }
+
+    if (no_action) G29_RETURN(false);
+
+    if (abl_probe_index == 0) {
+      // For the initial G29 S2 save software endstop state
+      SET_SOFT_ENDSTOP_LOOSE(true);
+      // Move close to the bed before the first point
+      do_blocking_move_to_z(0);
+    }
+    else {
+
+      #if EITHER(AUTO_BED_LEVELING_LINEAR, AUTO_BED_LEVELING_3POINT)
+        const uint16_t index = abl_probe_index - 1;
+      #endif
+
+      // For G29 after adjusting Z.
+      // Save the previous Z before going to the next point
+      measured_z = current_position.z;
+
+      #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+
+        mean += measured_z;
+        eqnBVector[index] = measured_z;
+        eqnAMatrix[index + 0 * abl_points] = probePos.x;
+        eqnAMatrix[index + 1 * abl_points] = probePos.y;
+        eqnAMatrix[index + 2 * abl_points] = 1;
+
+        incremental_LSF(&lsf_results, probePos, measured_z);
+
+      #elif ENABLED(AUTO_BED_LEVELING_3POINT)
+
+        points[index].z = measured_z;
+
+      #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+        const float newz = measured_z + zoffset;
+        z_values[meshCount.x][meshCount.y] = newz;
+        TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(meshCount, newz));
+
+        if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR_P(PSTR("Save X"), meshCount.x, SP_Y_STR, meshCount.y, SP_Z_STR, measured_z + zoffset);
+
+      #endif
+    }
+
+    //
+    // If there's another point to sample, move there with optional lift.
+    //
+
+    #if ABL_GRID
+
+      // Skip any unreachable points
+      while (abl_probe_index < abl_points) {
+
+        // Set meshCount.x, meshCount.y based on abl_probe_index, with zig-zag
+        PR_OUTER_VAR = abl_probe_index / PR_INNER_END;
+        PR_INNER_VAR = abl_probe_index - (PR_OUTER_VAR * PR_INNER_END);
+
+        // Probe in reverse order for every other row/column
+        const bool zig = (PR_OUTER_VAR & 1); // != ((PR_OUTER_END) & 1);
+        if (zig) PR_INNER_VAR = (PR_INNER_END - 1) - PR_INNER_VAR;
+
+        probePos = probe_position_lf + gridSpacing * meshCount.asFloat();
+
+        TERN_(AUTO_BED_LEVELING_LINEAR, indexIntoAB[meshCount.x][meshCount.y] = abl_probe_index);
+
+        // Keep looping till a reachable point is found
+        if (position_is_reachable(probePos)) break;
+        ++abl_probe_index;
+      }
+
+      // Is there a next point to move to?
+      if (abl_probe_index < abl_points) {
+        _manual_goto_xy(probePos); // Can be used here too!
+        // Disable software endstops to allow manual adjustment
+        // If G29 is not completed, they will not be re-enabled
+        SET_SOFT_ENDSTOP_LOOSE(true);
+        G29_RETURN(false);
+      }
+      else {
+        // Leveling done! Fall through to G29 finishing code below
+        SERIAL_ECHOLNPGM("Grid probing done.");
+        // Re-enable software endstops, if needed
+        SET_SOFT_ENDSTOP_LOOSE(false);
+      }
+
+    #elif ENABLED(AUTO_BED_LEVELING_3POINT)
+
+      // Probe at 3 arbitrary points
+      if (abl_probe_index < abl_points) {
+        probePos = points[abl_probe_index];
+        _manual_goto_xy(probePos);
+        // Disable software endstops to allow manual adjustment
+        // If G29 is not completed, they will not be re-enabled
+        SET_SOFT_ENDSTOP_LOOSE(true);
+        G29_RETURN(false);
+      }
+      else {
+
+        SERIAL_ECHOLNPGM("3-point probing done.");
+
+        // Re-enable software endstops, if needed
+        SET_SOFT_ENDSTOP_LOOSE(false);
+
+        if (!dryrun) {
+          vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
+          if (planeNormal.z < 0) planeNormal *= -1;
+          planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
+
+          // Can't re-enable (on error) until the new grid is written
+          abl_should_enable = false;
+        }
+
+      }
+
+    #endif // AUTO_BED_LEVELING_3POINT
+
+  #else // !PROBE_MANUALLY
+  {
+    const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;
+
+    measured_z = 0;
+
+    #if ABL_GRID
+
+      bool zig = PR_OUTER_END & 1;  // Always end at RIGHT and BACK_PROBE_BED_POSITION
+
+      measured_z = 0;
+
+      xy_int8_t meshCount;
+
+      // Outer loop is X with PROBE_Y_FIRST enabled
+      // Outer loop is Y with PROBE_Y_FIRST disabled
+      for (PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_END && !isnan(measured_z); PR_OUTER_VAR++) {
+
+        int8_t inStart, inStop, inInc;
+
+        if (zig) {                    // Zig away from origin
+          inStart = 0;                // Left or front
+          inStop = PR_INNER_END;      // Right or back
+          inInc = 1;                  // Zig right
+        }
+        else {                        // Zag towards origin
+          inStart = PR_INNER_END - 1; // Right or back
+          inStop = -1;                // Left or front
+          inInc = -1;                 // Zag left
+        }
+
+        zig ^= true; // zag
+
+        // An index to print current state
+        uint8_t pt_index = (PR_OUTER_VAR) * (PR_INNER_END) + 1;
+
+        // Inner loop is Y with PROBE_Y_FIRST enabled
+        // Inner loop is X with PROBE_Y_FIRST disabled
+        for (PR_INNER_VAR = inStart; PR_INNER_VAR != inStop; pt_index++, PR_INNER_VAR += inInc) {
+
+          probePos = probe_position_lf + gridSpacing * meshCount.asFloat();
+
+          TERN_(AUTO_BED_LEVELING_LINEAR, indexIntoAB[meshCount.x][meshCount.y] = ++abl_probe_index); // 0...
+
+          // Avoid probing outside the round or hexagonal area
+          if (TERN0(IS_KINEMATIC, !probe.can_reach(probePos))) continue;
+
+          if (verbose_level) SERIAL_ECHOLNPAIR("Probing mesh point ", int(pt_index), "/", abl_points, ".");
+          TERN_(HAS_DISPLAY, ui.status_printf_P(0, PSTR(S_FMT " %i/%i"), GET_TEXT(MSG_PROBING_MESH), int(pt_index), int(abl_points)));
+
+          measured_z = faux ? 0.001f * random(-100, 101) : probe.probe_at_point(probePos, raise_after, verbose_level);
+
+          if (isnan(measured_z)) {
+            set_bed_leveling_enabled(abl_should_enable);
+            break; // Breaks out of both loops
+          }
+
+          #if ENABLED(PROBE_TEMP_COMPENSATION)
+            temp_comp.compensate_measurement(TSI_BED, thermalManager.degBed(), measured_z);
+            temp_comp.compensate_measurement(TSI_PROBE, thermalManager.degProbe(), measured_z);
+            TERN_(USE_TEMP_EXT_COMPENSATION, temp_comp.compensate_measurement(TSI_EXT, thermalManager.degHotend(), measured_z));
+          #endif
+
+          #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+
+            mean += measured_z;
+            eqnBVector[abl_probe_index] = measured_z;
+            eqnAMatrix[abl_probe_index + 0 * abl_points] = probePos.x;
+            eqnAMatrix[abl_probe_index + 1 * abl_points] = probePos.y;
+            eqnAMatrix[abl_probe_index + 2 * abl_points] = 1;
+
+            incremental_LSF(&lsf_results, probePos, measured_z);
+
+          #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+            const float z = measured_z + zoffset;
+            z_values[meshCount.x][meshCount.y] = z;
+            TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(meshCount, z));
+
+          #endif
+
+          abl_should_enable = false;
+          idle_no_sleep();
+
+        } // inner
+      } // outer
+
+    #elif ENABLED(AUTO_BED_LEVELING_3POINT)
+
+      // Probe at 3 arbitrary points
+
+      LOOP_L_N(i, 3) {
+        if (verbose_level) SERIAL_ECHOLNPAIR("Probing point ", int(i + 1), "/3.");
+        TERN_(HAS_DISPLAY, ui.status_printf_P(0, PSTR(S_FMT " %i/3"), GET_TEXT(MSG_PROBING_MESH), int(i + 1)));
+
+        // Retain the last probe position
+        probePos = points[i];
+        measured_z = faux ? 0.001 * random(-100, 101) : probe.probe_at_point(probePos, raise_after, verbose_level);
+        if (isnan(measured_z)) {
+          set_bed_leveling_enabled(abl_should_enable);
+          break;
+        }
+        points[i].z = measured_z;
+      }
+
+      if (!dryrun && !isnan(measured_z)) {
+        vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
+        if (planeNormal.z < 0) planeNormal *= -1;
+        planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
+
+        // Can't re-enable (on error) until the new grid is written
+        abl_should_enable = false;
+      }
+
+    #endif // AUTO_BED_LEVELING_3POINT
+
+    TERN_(HAS_DISPLAY, ui.reset_status());
+
+    // Stow the probe. No raise for FIX_MOUNTED_PROBE.
+    if (probe.stow()) {
+      set_bed_leveling_enabled(abl_should_enable);
+      measured_z = NAN;
+    }
+  }
+  #endif // !PROBE_MANUALLY
+
+  //
+  // G29 Finishing Code
+  //
+  // Unless this is a dry run, auto bed leveling will
+  // definitely be enabled after this point.
+  //
+  // If code above wants to continue leveling, it should
+  // return or loop before this point.
+  //
+
+  if (DEBUGGING(LEVELING)) DEBUG_POS("> probing complete", current_position);
+
+  #if ENABLED(PROBE_MANUALLY)
+    g29_in_progress = false;
+    TERN_(LCD_BED_LEVELING, ui.wait_for_move = false);
+  #endif
+
+  // Calculate leveling, print reports, correct the position
+  if (!isnan(measured_z)) {
+    #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+      if (!dryrun) extrapolate_unprobed_bed_level();
+      print_bilinear_leveling_grid();
+
+      refresh_bed_level();
+
+      TERN_(ABL_BILINEAR_SUBDIVISION, print_bilinear_leveling_grid_virt());
+
+    #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
+
+      // For LINEAR leveling calculate matrix, print reports, correct the position
+
+      /**
+       * solve the plane equation ax + by + d = z
+       * A is the matrix with rows [x y 1] for all the probed points
+       * B is the vector of the Z positions
+       * the normal vector to the plane is formed by the coefficients of the
+       * plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
+       * so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
+       */
+      struct { float a, b, d; } plane_equation_coefficients;
+
+      finish_incremental_LSF(&lsf_results);
+      plane_equation_coefficients.a = -lsf_results.A;  // We should be able to eliminate the '-' on these three lines and down below
+      plane_equation_coefficients.b = -lsf_results.B;  // but that is not yet tested.
+      plane_equation_coefficients.d = -lsf_results.D;
+
+      mean /= abl_points;
+
+      if (verbose_level) {
+        SERIAL_ECHOPAIR_F("Eqn coefficients: a: ", plane_equation_coefficients.a, 8);
+        SERIAL_ECHOPAIR_F(" b: ", plane_equation_coefficients.b, 8);
+        SERIAL_ECHOPAIR_F(" d: ", plane_equation_coefficients.d, 8);
+        if (verbose_level > 2)
+          SERIAL_ECHOPAIR_F("\nMean of sampled points: ", mean, 8);
+        SERIAL_EOL();
+      }
+
+      // Create the matrix but don't correct the position yet
+      if (!dryrun)
+        planner.bed_level_matrix = matrix_3x3::create_look_at(
+          vector_3(-plane_equation_coefficients.a, -plane_equation_coefficients.b, 1)    // We can eliminate the '-' here and up above
+        );
+
+      // Show the Topography map if enabled
+      if (do_topography_map) {
+
+        float min_diff = 999;
+
+        auto print_topo_map = [&](PGM_P const title, const bool get_min) {
+          serialprintPGM(title);
+          for (int8_t yy = abl_grid_points.y - 1; yy >= 0; yy--) {
+            LOOP_L_N(xx, abl_grid_points.x) {
+              const int ind = indexIntoAB[xx][yy];
+              xyz_float_t tmp = { eqnAMatrix[ind + 0 * abl_points],
+                                  eqnAMatrix[ind + 1 * abl_points], 0 };
+              apply_rotation_xyz(planner.bed_level_matrix, tmp);
+              if (get_min) NOMORE(min_diff, eqnBVector[ind] - tmp.z);
+              const float subval = get_min ? mean : tmp.z + min_diff,
+                            diff = eqnBVector[ind] - subval;
+              SERIAL_CHAR(' '); if (diff >= 0.0) SERIAL_CHAR('+');   // Include + for column alignment
+              SERIAL_ECHO_F(diff, 5);
+            } // xx
+            SERIAL_EOL();
+          } // yy
+          SERIAL_EOL();
+        };
+
+        print_topo_map(PSTR("\nBed Height Topography:\n"
+                               "   +--- BACK --+\n"
+                               "   |           |\n"
+                               " L |    (+)    | R\n"
+                               " E |           | I\n"
+                               " F | (-) N (+) | G\n"
+                               " T |           | H\n"
+                               "   |    (-)    | T\n"
+                               "   |           |\n"
+                               "   O-- FRONT --+\n"
+                               " (0,0)\n"), true);
+        if (verbose_level > 3)
+          print_topo_map(PSTR("\nCorrected Bed Height vs. Bed Topology:\n"), false);
+
+      } //do_topography_map
+
+    #endif // AUTO_BED_LEVELING_LINEAR
+
+    #if ABL_PLANAR
+
+      // For LINEAR and 3POINT leveling correct the current position
+
+      if (verbose_level > 0)
+        planner.bed_level_matrix.debug(PSTR("\n\nBed Level Correction Matrix:"));
+
+      if (!dryrun) {
+        //
+        // Correct the current XYZ position based on the tilted plane.
+        //
+
+        if (DEBUGGING(LEVELING)) DEBUG_POS("G29 uncorrected XYZ", current_position);
+
+        xyze_pos_t converted = current_position;
+        planner.force_unapply_leveling(converted); // use conversion machinery
+
+        // Use the last measured distance to the bed, if possible
+        if ( NEAR(current_position.x, probePos.x - probe.offset_xy.x)
+          && NEAR(current_position.y, probePos.y - probe.offset_xy.y)
+        ) {
+          const float simple_z = current_position.z - measured_z;
+          if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Probed Z", simple_z, "  Matrix Z", converted.z, "  Discrepancy ", simple_z - converted.z);
+          converted.z = simple_z;
+        }
+
+        // The rotated XY and corrected Z are now current_position
+        current_position = converted;
+
+        if (DEBUGGING(LEVELING)) DEBUG_POS("G29 corrected XYZ", current_position);
+      }
+
+    #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+      if (!dryrun) {
+        if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("G29 uncorrected Z:", current_position.z);
+
+        // Unapply the offset because it is going to be immediately applied
+        // and cause compensation movement in Z
+        const float fade_scaling_factor = TERN(ENABLE_LEVELING_FADE_HEIGHT, planner.fade_scaling_factor_for_z(current_position.z), 1);
+        current_position.z -= fade_scaling_factor * bilinear_z_offset(current_position);
+
+        if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR(" corrected Z:", current_position.z);
+      }
+
+    #endif // ABL_PLANAR
+
+    // Auto Bed Leveling is complete! Enable if possible.
+    planner.leveling_active = dryrun ? abl_should_enable : true;
+  } // !isnan(measured_z)
+
+  // Restore state after probing
+  if (!faux) restore_feedrate_and_scaling();
+
+  // Sync the planner from the current_position
+  if (planner.leveling_active) sync_plan_position();
+
+  #if HAS_BED_PROBE
+    probe.move_z_after_probing();
+  #endif
+
+  #ifdef Z_PROBE_END_SCRIPT
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Z Probe End Script: ", Z_PROBE_END_SCRIPT);
+    planner.synchronize();
+    process_subcommands_now_P(PSTR(Z_PROBE_END_SCRIPT));
+  #endif
+
+  #if ENABLED(DWIN_CREALITY_LCD)
+    DWIN_CompletedLeveling();
+  #endif
+
+  report_current_position();
+
+  G29_RETURN(isnan(measured_z));
+}
+
+#endif // HAS_ABL_NOT_UBL
diff --git a/Marlin/src/gcode/bedlevel/abl/M421.cpp b/Marlin/src/gcode/bedlevel/abl/M421.cpp
new file mode 100644
index 0000000..182dc32
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/abl/M421.cpp
@@ -0,0 +1,74 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * M421.cpp - Auto Bed Leveling
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+#include "../../gcode.h"
+#include "../../../feature/bedlevel/bedlevel.h"
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../../lcd/extui/ui_api.h"
+#endif
+
+/**
+ * M421: Set one or more Mesh Bed Leveling Z coordinates
+ *
+ * Usage:
+ *   M421 I<xindex> J<yindex> Z<linear>
+ *   M421 I<xindex> J<yindex> Q<offset>
+ *
+ *  - If I is omitted, set the entire row
+ *  - If J is omitted, set the entire column
+ *  - If both I and J are omitted, set all
+ */
+void GcodeSuite::M421() {
+  int8_t ix = parser.intval('I', -1), iy = parser.intval('J', -1);
+  const bool hasZ = parser.seenval('Z'),
+             hasQ = !hasZ && parser.seenval('Q');
+
+  if (hasZ || hasQ) {
+    if (WITHIN(ix, -1, GRID_MAX_POINTS_X - 1) && WITHIN(iy, -1, GRID_MAX_POINTS_Y - 1)) {
+      const float zval = parser.value_linear_units();
+      uint8_t sx = ix >= 0 ? ix : 0, ex = ix >= 0 ? ix : GRID_MAX_POINTS_X - 1,
+              sy = iy >= 0 ? iy : 0, ey = iy >= 0 ? iy : GRID_MAX_POINTS_Y - 1;
+      LOOP_S_LE_N(x, sx, ex) {
+        LOOP_S_LE_N(y, sy, ey) {
+          z_values[x][y] = zval + (hasQ ? z_values[x][y] : 0);
+          TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, z_values[x][y]));
+        }
+      }
+      TERN_(ABL_BILINEAR_SUBDIVISION, bed_level_virt_interpolate());
+    }
+    else
+      SERIAL_ERROR_MSG(STR_ERR_MESH_XY);
+  }
+  else
+    SERIAL_ERROR_MSG(STR_ERR_M421_PARAMETERS);
+}
+
+#endif // AUTO_BED_LEVELING_BILINEAR
diff --git a/Marlin/src/gcode/bedlevel/mbl/G29.cpp b/Marlin/src/gcode/bedlevel/mbl/G29.cpp
new file mode 100644
index 0000000..cf27c14
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/mbl/G29.cpp
@@ -0,0 +1,193 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * G29.cpp - Mesh Bed Leveling
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(MESH_BED_LEVELING)
+
+#include "../../../feature/bedlevel/bedlevel.h"
+
+#include "../../gcode.h"
+#include "../../queue.h"
+
+#include "../../../libs/buzzer.h"
+#include "../../../lcd/marlinui.h"
+#include "../../../module/motion.h"
+#include "../../../module/stepper.h"
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../../lcd/extui/ui_api.h"
+#endif
+
+// Save 130 bytes with non-duplication of PSTR
+inline void echo_not_entered(const char c) { SERIAL_CHAR(c); SERIAL_ECHOLNPGM(" not entered."); }
+
+/**
+ * G29: Mesh-based Z probe, probes a grid and produces a
+ *      mesh to compensate for variable bed height
+ *
+ * Parameters With MESH_BED_LEVELING:
+ *
+ *  S0              Report the current mesh values
+ *  S1              Start probing mesh points
+ *  S2              Probe the next mesh point
+ *  S3 In Jn Zn.nn  Manually modify a single point
+ *  S4 Zn.nn        Set z offset. Positive away from bed, negative closer to bed.
+ *  S5              Reset and disable mesh
+ */
+void GcodeSuite::G29() {
+
+  static int mbl_probe_index = -1;
+
+  MeshLevelingState state = (MeshLevelingState)parser.byteval('S', (int8_t)MeshReport);
+  if (!WITHIN(state, 0, 5)) {
+    SERIAL_ECHOLNPGM("S out of range (0-5).");
+    return;
+  }
+
+  int8_t ix, iy;
+
+  switch (state) {
+    case MeshReport:
+      SERIAL_ECHOPGM("Mesh Bed Leveling ");
+      if (leveling_is_valid()) {
+        serialprintln_onoff(planner.leveling_active);
+        mbl.report_mesh();
+      }
+      else
+        SERIAL_ECHOLNPGM("has no data.");
+      break;
+
+    case MeshStart:
+      mbl.reset();
+      mbl_probe_index = 0;
+      if (!ui.wait_for_move) {
+        queue.inject_P(parser.seen('N') ? PSTR("G28" TERN(G28_L0_ENSURES_LEVELING_OFF, "L0", "") "\nG29S2") : PSTR("G29S2"));
+        return;
+      }
+      state = MeshNext;
+
+    case MeshNext:
+      if (mbl_probe_index < 0) {
+        SERIAL_ECHOLNPGM("Start mesh probing with \"G29 S1\" first.");
+        return;
+      }
+      // For each G29 S2...
+      if (mbl_probe_index == 0) {
+        // Move close to the bed before the first point
+        do_blocking_move_to_z(0);
+      }
+      else {
+        // Save Z for the previous mesh position
+        mbl.set_zigzag_z(mbl_probe_index - 1, current_position.z);
+        SET_SOFT_ENDSTOP_LOOSE(false);
+      }
+      // If there's another point to sample, move there with optional lift.
+      if (mbl_probe_index < GRID_MAX_POINTS) {
+        // Disable software endstops to allow manual adjustment
+        // If G29 is left hanging without completion they won't be re-enabled!
+        SET_SOFT_ENDSTOP_LOOSE(true);
+        mbl.zigzag(mbl_probe_index++, ix, iy);
+        _manual_goto_xy({ mbl.index_to_xpos[ix], mbl.index_to_ypos[iy] });
+      }
+      else {
+        // One last "return to the bed" (as originally coded) at completion
+        current_position.z = MANUAL_PROBE_HEIGHT;
+        line_to_current_position();
+        planner.synchronize();
+
+        // After recording the last point, activate home and activate
+        mbl_probe_index = -1;
+        SERIAL_ECHOLNPGM("Mesh probing done.");
+        BUZZ(100, 659);
+        BUZZ(100, 698);
+
+        home_all_axes();
+        set_bed_leveling_enabled(true);
+
+        #if ENABLED(MESH_G28_REST_ORIGIN)
+          current_position.z = 0;
+          line_to_current_position(homing_feedrate(Z_AXIS));
+          planner.synchronize();
+        #endif
+
+        TERN_(LCD_BED_LEVELING, ui.wait_for_move = false);
+      }
+      break;
+
+    case MeshSet:
+      if (parser.seenval('I')) {
+        ix = parser.value_int();
+        if (!WITHIN(ix, 0, GRID_MAX_POINTS_X - 1)) {
+          SERIAL_ECHOPAIR("I out of range (0-", int(GRID_MAX_POINTS_X - 1));
+          SERIAL_ECHOLNPGM(")");
+          return;
+        }
+      }
+      else
+        return echo_not_entered('J');
+
+      if (parser.seenval('J')) {
+        iy = parser.value_int();
+        if (!WITHIN(iy, 0, GRID_MAX_POINTS_Y - 1)) {
+          SERIAL_ECHOPAIR("J out of range (0-", int(GRID_MAX_POINTS_Y - 1));
+          SERIAL_ECHOLNPGM(")");
+          return;
+        }
+      }
+      else
+        return echo_not_entered('J');
+
+      if (parser.seenval('Z')) {
+        mbl.z_values[ix][iy] = parser.value_linear_units();
+        TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(ix, iy, mbl.z_values[ix][iy]));
+      }
+      else
+        return echo_not_entered('Z');
+      break;
+
+    case MeshSetZOffset:
+      if (parser.seenval('Z'))
+        mbl.z_offset = parser.value_linear_units();
+      else
+        return echo_not_entered('Z');
+      break;
+
+    case MeshReset:
+      reset_bed_level();
+      break;
+
+  } // switch(state)
+
+  if (state == MeshNext) {
+    SERIAL_ECHOPAIR("MBL G29 point ", _MIN(mbl_probe_index, GRID_MAX_POINTS));
+    SERIAL_ECHOLNPAIR(" of ", int(GRID_MAX_POINTS));
+  }
+
+  report_current_position();
+}
+
+#endif // MESH_BED_LEVELING
diff --git a/Marlin/src/gcode/bedlevel/mbl/M421.cpp b/Marlin/src/gcode/bedlevel/mbl/M421.cpp
new file mode 100644
index 0000000..1368ab0
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/mbl/M421.cpp
@@ -0,0 +1,59 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * M421.cpp - Mesh Bed Leveling
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(MESH_BED_LEVELING)
+
+#include "../../gcode.h"
+#include "../../../module/motion.h"
+#include "../../../feature/bedlevel/mbl/mesh_bed_leveling.h"
+
+/**
+ * M421: Set a single Mesh Bed Leveling Z coordinate
+ *
+ * Usage:
+ *   M421 X<linear> Y<linear> Z<linear>
+ *   M421 X<linear> Y<linear> Q<offset>
+ *   M421 I<xindex> J<yindex> Z<linear>
+ *   M421 I<xindex> J<yindex> Q<offset>
+ */
+void GcodeSuite::M421() {
+  const bool hasX = parser.seen('X'), hasI = parser.seen('I');
+  const int8_t ix = hasI ? parser.value_int() : hasX ? mbl.probe_index_x(RAW_X_POSITION(parser.value_linear_units())) : -1;
+  const bool hasY = parser.seen('Y'), hasJ = parser.seen('J');
+  const int8_t iy = hasJ ? parser.value_int() : hasY ? mbl.probe_index_y(RAW_Y_POSITION(parser.value_linear_units())) : -1;
+  const bool hasZ = parser.seen('Z'), hasQ = !hasZ && parser.seen('Q');
+
+  if (int(hasI && hasJ) + int(hasX && hasY) != 1 || !(hasZ || hasQ))
+    SERIAL_ERROR_MSG(STR_ERR_M421_PARAMETERS);
+  else if (ix < 0 || iy < 0)
+    SERIAL_ERROR_MSG(STR_ERR_MESH_XY);
+  else
+    mbl.set_z(ix, iy, parser.value_linear_units() + (hasQ ? mbl.z_values[ix][iy] : 0));
+}
+
+#endif // MESH_BED_LEVELING
diff --git a/Marlin/src/gcode/bedlevel/ubl/G29.cpp b/Marlin/src/gcode/bedlevel/ubl/G29.cpp
new file mode 100644
index 0000000..2ef3ab4
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/ubl/G29.cpp
@@ -0,0 +1,36 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * G29.cpp - Unified Bed Leveling
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(AUTO_BED_LEVELING_UBL)
+
+#include "../../gcode.h"
+#include "../../../feature/bedlevel/bedlevel.h"
+
+void GcodeSuite::G29() { ubl.G29(); }
+
+#endif // AUTO_BED_LEVELING_UBL
diff --git a/Marlin/src/gcode/bedlevel/ubl/M421.cpp b/Marlin/src/gcode/bedlevel/ubl/M421.cpp
new file mode 100644
index 0000000..600c1fc
--- /dev/null
+++ b/Marlin/src/gcode/bedlevel/ubl/M421.cpp
@@ -0,0 +1,70 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * unified.cpp - Unified Bed Leveling
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(AUTO_BED_LEVELING_UBL)
+
+#include "../../gcode.h"
+#include "../../../feature/bedlevel/bedlevel.h"
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../../lcd/extui/ui_api.h"
+#endif
+
+/**
+ * M421: Set a single Mesh Bed Leveling Z coordinate
+ *
+ * Usage:
+ *   M421 I<xindex> J<yindex> Z<linear>
+ *   M421 I<xindex> J<yindex> Q<offset>
+ *   M421 I<xindex> J<yindex> N
+ *   M421 C Z<linear>
+ *   M421 C Q<offset>
+ */
+void GcodeSuite::M421() {
+  xy_int8_t ij = { int8_t(parser.intval('I', -1)), int8_t(parser.intval('J', -1)) };
+  const bool hasI = ij.x >= 0,
+             hasJ = ij.y >= 0,
+             hasC = parser.seen('C'),
+             hasN = parser.seen('N'),
+             hasZ = parser.seen('Z'),
+             hasQ = !hasZ && parser.seen('Q');
+
+  if (hasC) ij = ubl.find_closest_mesh_point_of_type(REAL, current_position);
+
+  if (int(hasC) + int(hasI && hasJ) != 1 || !(hasZ || hasQ || hasN))
+    SERIAL_ERROR_MSG(STR_ERR_M421_PARAMETERS);
+  else if (!WITHIN(ij.x, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(ij.y, 0, GRID_MAX_POINTS_Y - 1))
+    SERIAL_ERROR_MSG(STR_ERR_MESH_XY);
+  else {
+    float &zval = ubl.z_values[ij.x][ij.y];
+    zval = hasN ? NAN : parser.value_linear_units() + (hasQ ? zval : 0);
+    TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(ij.x, ij.y, zval));
+  }
+}
+
+#endif // AUTO_BED_LEVELING_UBL
diff --git a/Marlin/src/gcode/calibrate/G28.cpp b/Marlin/src/gcode/calibrate/G28.cpp
new file mode 100644
index 0000000..49dee87
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/G28.cpp
@@ -0,0 +1,493 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#include "../gcode.h"
+
+#include "../../module/stepper.h"
+#include "../../module/endstops.h"
+
+#if HAS_MULTI_HOTEND
+  #include "../../module/tool_change.h"
+#endif
+
+#if HAS_LEVELING
+  #include "../../feature/bedlevel/bedlevel.h"
+#endif
+
+#if ENABLED(SENSORLESS_HOMING)
+  #include "../../feature/tmc_util.h"
+#endif
+
+#include "../../module/probe.h"
+
+#if ENABLED(BLTOUCH)
+  #include "../../feature/bltouch.h"
+#endif
+
+#include "../../lcd/marlinui.h"
+#if ENABLED(DWIN_CREALITY_LCD)
+  #include "../../lcd/dwin/e3v2/dwin.h"
+#endif
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../lcd/extui/ui_api.h"
+#endif
+
+#if HAS_L64XX                         // set L6470 absolute position registers to counts
+  #include "../../libs/L64XX/L64XX_Marlin.h"
+#endif
+
+#if ENABLED(LASER_MOVE_G28_OFF)
+  #include "../../feature/spindle_laser.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../core/debug_out.h"
+
+#if ENABLED(QUICK_HOME)
+
+  static void quick_home_xy() {
+
+    // Pretend the current position is 0,0
+    current_position.set(0.0, 0.0);
+    sync_plan_position();
+
+    const int x_axis_home_dir = x_home_dir(active_extruder);
+
+    const float mlx = max_length(X_AXIS),
+                mly = max_length(Y_AXIS),
+                mlratio = mlx > mly ? mly / mlx : mlx / mly,
+                fr_mm_s = _MIN(homing_feedrate(X_AXIS), homing_feedrate(Y_AXIS)) * SQRT(sq(mlratio) + 1.0);
+
+    #if ENABLED(SENSORLESS_HOMING)
+      sensorless_t stealth_states {
+          tmc_enable_stallguard(stepperX)
+        , tmc_enable_stallguard(stepperY)
+        , false
+        , false
+          #if AXIS_HAS_STALLGUARD(X2)
+            || tmc_enable_stallguard(stepperX2)
+          #endif
+        , false
+          #if AXIS_HAS_STALLGUARD(Y2)
+            || tmc_enable_stallguard(stepperY2)
+          #endif
+      };
+    #endif
+
+    do_blocking_move_to_xy(1.5 * mlx * x_axis_home_dir, 1.5 * mly * home_dir(Y_AXIS), fr_mm_s);
+
+    endstops.validate_homing_move();
+
+    current_position.set(0.0, 0.0);
+
+    #if ENABLED(SENSORLESS_HOMING)
+      tmc_disable_stallguard(stepperX, stealth_states.x);
+      tmc_disable_stallguard(stepperY, stealth_states.y);
+      #if AXIS_HAS_STALLGUARD(X2)
+        tmc_disable_stallguard(stepperX2, stealth_states.x2);
+      #endif
+      #if AXIS_HAS_STALLGUARD(Y2)
+        tmc_disable_stallguard(stepperY2, stealth_states.y2);
+      #endif
+    #endif
+  }
+
+#endif // QUICK_HOME
+
+#if ENABLED(Z_SAFE_HOMING)
+
+  inline void home_z_safely() {
+    DEBUG_SECTION(log_G28, "home_z_safely", DEBUGGING(LEVELING));
+
+    // Disallow Z homing if X or Y homing is needed
+    if (homing_needed_error(_BV(X_AXIS) | _BV(Y_AXIS))) return;
+
+    sync_plan_position();
+
+    /**
+     * Move the Z probe (or just the nozzle) to the safe homing point
+     * (Z is already at the right height)
+     */
+    constexpr xy_float_t safe_homing_xy = { Z_SAFE_HOMING_X_POINT, Z_SAFE_HOMING_Y_POINT };
+    #if HAS_HOME_OFFSET
+      xy_float_t okay_homing_xy = safe_homing_xy;
+      okay_homing_xy -= home_offset;
+    #else
+      constexpr xy_float_t okay_homing_xy = safe_homing_xy;
+    #endif
+
+    destination.set(okay_homing_xy, current_position.z);
+
+    TERN_(HOMING_Z_WITH_PROBE, destination -= probe.offset_xy);
+
+    if (position_is_reachable(destination)) {
+
+      if (DEBUGGING(LEVELING)) DEBUG_POS("home_z_safely", destination);
+
+      // Free the active extruder for movement
+      TERN_(DUAL_X_CARRIAGE, idex_set_parked(false));
+
+      TERN_(SENSORLESS_HOMING, safe_delay(500)); // Short delay needed to settle
+
+      do_blocking_move_to_xy(destination);
+      homeaxis(Z_AXIS);
+    }
+    else {
+      LCD_MESSAGEPGM(MSG_ZPROBE_OUT);
+      SERIAL_ECHO_MSG(STR_ZPROBE_OUT_SER);
+    }
+  }
+
+#endif // Z_SAFE_HOMING
+
+#if ENABLED(IMPROVE_HOMING_RELIABILITY)
+
+  slow_homing_t begin_slow_homing() {
+    slow_homing_t slow_homing{0};
+    slow_homing.acceleration.set(planner.settings.max_acceleration_mm_per_s2[X_AXIS],
+                                 planner.settings.max_acceleration_mm_per_s2[Y_AXIS]);
+    planner.settings.max_acceleration_mm_per_s2[X_AXIS] = 100;
+    planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = 100;
+    #if HAS_CLASSIC_JERK
+      slow_homing.jerk_xy = planner.max_jerk;
+      planner.max_jerk.set(0, 0);
+    #endif
+    planner.reset_acceleration_rates();
+    return slow_homing;
+  }
+
+  void end_slow_homing(const slow_homing_t &slow_homing) {
+    planner.settings.max_acceleration_mm_per_s2[X_AXIS] = slow_homing.acceleration.x;
+    planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = slow_homing.acceleration.y;
+    TERN_(HAS_CLASSIC_JERK, planner.max_jerk = slow_homing.jerk_xy);
+    planner.reset_acceleration_rates();
+  }
+
+#endif // IMPROVE_HOMING_RELIABILITY
+
+/**
+ * G28: Home all axes according to settings
+ *
+ * Parameters
+ *
+ *  None  Home to all axes with no parameters.
+ *        With QUICK_HOME enabled XY will home together, then Z.
+ *
+ *  O   Home only if position is unknown
+ *
+ *  Rn  Raise by n mm/inches before homing
+ *
+ * Cartesian/SCARA parameters
+ *
+ *  X   Home to the X endstop
+ *  Y   Home to the Y endstop
+ *  Z   Home to the Z endstop
+ */
+void GcodeSuite::G28() {
+  DEBUG_SECTION(log_G28, "G28", DEBUGGING(LEVELING));
+  if (DEBUGGING(LEVELING)) log_machine_info();
+
+  TERN_(LASER_MOVE_G28_OFF, cutter.set_inline_enabled(false));  // turn off laser
+
+  #if ENABLED(DUAL_X_CARRIAGE)
+    bool IDEX_saved_duplication_state = extruder_duplication_enabled;
+    DualXMode IDEX_saved_mode = dual_x_carriage_mode;
+  #endif
+
+  #if ENABLED(MARLIN_DEV_MODE)
+    if (parser.seen('S')) {
+      LOOP_XYZ(a) set_axis_is_at_home((AxisEnum)a);
+      sync_plan_position();
+      SERIAL_ECHOLNPGM("Simulated Homing");
+      report_current_position();
+      return;
+    }
+  #endif
+
+  // Home (O)nly if position is unknown
+  if (!axes_should_home() && parser.boolval('O')) {
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> homing not needed, skip");
+    return;
+  }
+
+  TERN_(DWIN_CREALITY_LCD, DWIN_StartHoming());
+  TERN_(EXTENSIBLE_UI, ExtUI::onHomingStart());
+
+  planner.synchronize();          // Wait for planner moves to finish!
+
+  SET_SOFT_ENDSTOP_LOOSE(false);  // Reset a leftover 'loose' motion state
+
+  // Disable the leveling matrix before homing
+  #if HAS_LEVELING
+    const bool leveling_restore_state = parser.boolval('L', TERN(RESTORE_LEVELING_AFTER_G28, planner.leveling_active, ENABLED(ENABLE_LEVELING_AFTER_G28)));
+    IF_ENABLED(PROBE_MANUALLY, g29_in_progress = false); // Cancel the active G29 session
+    set_bed_leveling_enabled(false);
+  #endif
+
+  // Reset to the XY plane
+  TERN_(CNC_WORKSPACE_PLANES, workspace_plane = PLANE_XY);
+
+  // Count this command as movement / activity
+  reset_stepper_timeout();
+
+  #define HAS_CURRENT_HOME(N) (defined(N##_CURRENT_HOME) && N##_CURRENT_HOME != N##_CURRENT)
+  #if HAS_CURRENT_HOME(X) || HAS_CURRENT_HOME(X2) || HAS_CURRENT_HOME(Y) || HAS_CURRENT_HOME(Y2)
+    #define HAS_HOMING_CURRENT 1
+  #endif
+
+  #if HAS_HOMING_CURRENT
+    auto debug_current = [](PGM_P const s, const int16_t a, const int16_t b){
+      serialprintPGM(s); DEBUG_ECHOLNPAIR(" current: ", a, " -> ", b);
+    };
+    #if HAS_CURRENT_HOME(X)
+      const int16_t tmc_save_current_X = stepperX.getMilliamps();
+      stepperX.rms_current(X_CURRENT_HOME);
+      if (DEBUGGING(LEVELING)) debug_current(PSTR("X"), tmc_save_current_X, X_CURRENT_HOME);
+    #endif
+    #if HAS_CURRENT_HOME(X2)
+      const int16_t tmc_save_current_X2 = stepperX2.getMilliamps();
+      stepperX2.rms_current(X2_CURRENT_HOME);
+      if (DEBUGGING(LEVELING)) debug_current(PSTR("X2"), tmc_save_current_X2, X2_CURRENT_HOME);
+    #endif
+    #if HAS_CURRENT_HOME(Y)
+      const int16_t tmc_save_current_Y = stepperY.getMilliamps();
+      stepperY.rms_current(Y_CURRENT_HOME);
+      if (DEBUGGING(LEVELING)) debug_current(PSTR("Y"), tmc_save_current_Y, Y_CURRENT_HOME);
+    #endif
+    #if HAS_CURRENT_HOME(Y2)
+      const int16_t tmc_save_current_Y2 = stepperY2.getMilliamps();
+      stepperY2.rms_current(Y2_CURRENT_HOME);
+      if (DEBUGGING(LEVELING)) debug_current(PSTR("Y2"), tmc_save_current_Y2, Y2_CURRENT_HOME);
+    #endif
+  #endif
+
+  TERN_(IMPROVE_HOMING_RELIABILITY, slow_homing_t slow_homing = begin_slow_homing());
+
+  // Always home with tool 0 active
+  #if HAS_MULTI_HOTEND
+    #if DISABLED(DELTA) || ENABLED(DELTA_HOME_TO_SAFE_ZONE)
+      const uint8_t old_tool_index = active_extruder;
+    #endif
+    // PARKING_EXTRUDER homing requires different handling of movement / solenoid activation, depending on the side of homing
+    #if ENABLED(PARKING_EXTRUDER)
+      const bool pe_final_change_must_unpark = parking_extruder_unpark_after_homing(old_tool_index, X_HOME_DIR + 1 == old_tool_index * 2);
+    #endif
+    tool_change(0, true);
+  #endif
+
+  TERN_(HAS_DUPLICATION_MODE, set_duplication_enabled(false));
+
+  remember_feedrate_scaling_off();
+
+  endstops.enable(true); // Enable endstops for next homing move
+
+  #if ENABLED(DELTA)
+
+    constexpr bool doZ = true; // for NANODLP_Z_SYNC if your DLP is on a DELTA
+
+    home_delta();
+
+    TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(slow_homing));
+
+  #else // NOT DELTA
+
+    const bool homeZ = parser.seen('Z'),
+               needX = homeZ && TERN0(Z_SAFE_HOMING, axes_should_home(_BV(X_AXIS))),
+               needY = homeZ && TERN0(Z_SAFE_HOMING, axes_should_home(_BV(Y_AXIS))),
+               homeX = needX || parser.seen('X'), homeY = needY || parser.seen('Y'),
+               home_all = homeX == homeY && homeX == homeZ, // All or None
+               doX = home_all || homeX, doY = home_all || homeY, doZ = home_all || homeZ;
+
+    #if ENABLED(HOME_Z_FIRST)
+
+      if (doZ) homeaxis(Z_AXIS);
+
+    #endif
+
+    const float z_homing_height = TERN1(UNKNOWN_Z_NO_RAISE, axis_is_trusted(Z_AXIS))
+                                  ? (parser.seenval('R') ? parser.value_linear_units() : Z_HOMING_HEIGHT)
+                                  : 0;
+
+    if (z_homing_height && (doX || doY || TERN0(Z_SAFE_HOMING, doZ))) {
+      // Raise Z before homing any other axes and z is not already high enough (never lower z)
+      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Raise Z (before homing) by ", z_homing_height);
+      do_z_clearance(z_homing_height, axis_is_trusted(Z_AXIS), DISABLED(UNKNOWN_Z_NO_RAISE));
+    }
+
+    #if ENABLED(QUICK_HOME)
+
+      if (doX && doY) quick_home_xy();
+
+    #endif
+
+    // Home Y (before X)
+    if (ENABLED(HOME_Y_BEFORE_X) && (doY || TERN0(CODEPENDENT_XY_HOMING, doX)))
+      homeaxis(Y_AXIS);
+
+    // Home X
+    if (doX || (doY && ENABLED(CODEPENDENT_XY_HOMING) && DISABLED(HOME_Y_BEFORE_X))) {
+
+      #if ENABLED(DUAL_X_CARRIAGE)
+
+        // Always home the 2nd (right) extruder first
+        active_extruder = 1;
+        homeaxis(X_AXIS);
+
+        // Remember this extruder's position for later tool change
+        inactive_extruder_x = current_position.x;
+
+        // Home the 1st (left) extruder
+        active_extruder = 0;
+        homeaxis(X_AXIS);
+
+        // Consider the active extruder to be in its "parked" position
+        idex_set_parked();
+
+      #else
+
+        homeaxis(X_AXIS);
+
+      #endif
+    }
+
+    // Home Y (after X)
+    if (DISABLED(HOME_Y_BEFORE_X) && doY)
+      homeaxis(Y_AXIS);
+
+    TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(slow_homing));
+
+    // Home Z last if homing towards the bed
+    #if DISABLED(HOME_Z_FIRST)
+      if (doZ) {
+        #if EITHER(Z_MULTI_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
+          stepper.set_all_z_lock(false);
+          stepper.set_separate_multi_axis(false);
+        #endif
+
+        TERN_(BLTOUCH, bltouch.init());
+        TERN(Z_SAFE_HOMING, home_z_safely(), homeaxis(Z_AXIS));
+        probe.move_z_after_homing();
+      }
+    #endif
+
+    sync_plan_position();
+
+  #endif // !DELTA (G28)
+
+  /**
+   * Preserve DXC mode across a G28 for IDEX printers in DXC_DUPLICATION_MODE.
+   * This is important because it lets a user use the LCD Panel to set an IDEX Duplication mode, and
+   * then print a standard GCode file that contains a single print that does a G28 and has no other
+   * IDEX specific commands in it.
+   */
+  #if ENABLED(DUAL_X_CARRIAGE)
+
+    if (idex_is_duplicating()) {
+
+      TERN_(IMPROVE_HOMING_RELIABILITY, slow_homing = begin_slow_homing());
+
+      // Always home the 2nd (right) extruder first
+      active_extruder = 1;
+      homeaxis(X_AXIS);
+
+      // Remember this extruder's position for later tool change
+      inactive_extruder_x = current_position.x;
+
+      // Home the 1st (left) extruder
+      active_extruder = 0;
+      homeaxis(X_AXIS);
+
+      // Consider the active extruder to be parked
+      idex_set_parked();
+
+      dual_x_carriage_mode = IDEX_saved_mode;
+      set_duplication_enabled(IDEX_saved_duplication_state);
+
+      TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(slow_homing));
+    }
+
+  #endif // DUAL_X_CARRIAGE
+
+  endstops.not_homing();
+
+  // Clear endstop state for polled stallGuard endstops
+  TERN_(SPI_ENDSTOPS, endstops.clear_endstop_state());
+
+  #if BOTH(DELTA, DELTA_HOME_TO_SAFE_ZONE)
+    // move to a height where we can use the full xy-area
+    do_blocking_move_to_z(delta_clip_start_height);
+  #endif
+
+  TERN_(HAS_LEVELING, set_bed_leveling_enabled(leveling_restore_state));
+
+  restore_feedrate_and_scaling();
+
+  // Restore the active tool after homing
+  #if HAS_MULTI_HOTEND && (DISABLED(DELTA) || ENABLED(DELTA_HOME_TO_SAFE_ZONE))
+    tool_change(old_tool_index, TERN(PARKING_EXTRUDER, !pe_final_change_must_unpark, DISABLED(DUAL_X_CARRIAGE)));   // Do move if one of these
+  #endif
+
+  #if HAS_HOMING_CURRENT
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Restore driver current...");
+    #if HAS_CURRENT_HOME(X)
+      stepperX.rms_current(tmc_save_current_X);
+    #endif
+    #if HAS_CURRENT_HOME(X2)
+      stepperX2.rms_current(tmc_save_current_X2);
+    #endif
+    #if HAS_CURRENT_HOME(Y)
+      stepperY.rms_current(tmc_save_current_Y);
+    #endif
+    #if HAS_CURRENT_HOME(Y2)
+      stepperY2.rms_current(tmc_save_current_Y2);
+    #endif
+  #endif
+
+  ui.refresh();
+
+  TERN_(DWIN_CREALITY_LCD, DWIN_CompletedHoming());
+  TERN_(EXTENSIBLE_UI, ExtUI::onHomingComplete());
+
+  report_current_position();
+
+  if (ENABLED(NANODLP_Z_SYNC) && (doZ || ENABLED(NANODLP_ALL_AXIS)))
+    SERIAL_ECHOLNPGM(STR_Z_MOVE_COMP);
+
+  #if HAS_L64XX
+    // Set L6470 absolute position registers to counts
+    // constexpr *might* move this to PROGMEM.
+    // If not, this will need a PROGMEM directive and an accessor.
+    static constexpr AxisEnum L64XX_axis_xref[MAX_L64XX] = {
+      X_AXIS, Y_AXIS, Z_AXIS,
+      X_AXIS, Y_AXIS, Z_AXIS, Z_AXIS,
+      E_AXIS, E_AXIS, E_AXIS, E_AXIS, E_AXIS, E_AXIS
+    };
+    for (uint8_t j = 1; j <= L64XX::chain[0]; j++) {
+      const uint8_t cv = L64XX::chain[j];
+      L64xxManager.set_param((L64XX_axis_t)cv, L6470_ABS_POS, stepper.position(L64XX_axis_xref[cv]));
+    }
+  #endif
+  TERN_(HAS_LEVELING, set_bed_leveling_enabled(true));
+}
diff --git a/Marlin/src/gcode/calibrate/G33.cpp b/Marlin/src/gcode/calibrate/G33.cpp
new file mode 100644
index 0000000..77cc457
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/G33.cpp
@@ -0,0 +1,648 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(DELTA_AUTO_CALIBRATION)
+
+#include "../gcode.h"
+#include "../../module/delta.h"
+#include "../../module/motion.h"
+#include "../../module/stepper.h"
+#include "../../module/endstops.h"
+#include "../../lcd/marlinui.h"
+
+#if HAS_BED_PROBE
+  #include "../../module/probe.h"
+#endif
+
+#if HAS_MULTI_HOTEND
+  #include "../../module/tool_change.h"
+#endif
+
+#if HAS_LEVELING
+  #include "../../feature/bedlevel/bedlevel.h"
+#endif
+
+constexpr uint8_t _7P_STEP = 1,              // 7-point step - to change number of calibration points
+                  _4P_STEP = _7P_STEP * 2,   // 4-point step
+                  NPP      = _7P_STEP * 6;   // number of calibration points on the radius
+enum CalEnum : char {                        // the 7 main calibration points - add definitions if needed
+  CEN      = 0,
+  __A      = 1,
+  _AB      = __A + _7P_STEP,
+  __B      = _AB + _7P_STEP,
+  _BC      = __B + _7P_STEP,
+  __C      = _BC + _7P_STEP,
+  _CA      = __C + _7P_STEP,
+};
+
+#define LOOP_CAL_PT(VAR, S, N) for (uint8_t VAR=S; VAR<=NPP; VAR+=N)
+#define F_LOOP_CAL_PT(VAR, S, N) for (float VAR=S; VAR<NPP+0.9999; VAR+=N)
+#define I_LOOP_CAL_PT(VAR, S, N) for (float VAR=S; VAR>CEN+0.9999; VAR-=N)
+#define LOOP_CAL_ALL(VAR) LOOP_CAL_PT(VAR, CEN, 1)
+#define LOOP_CAL_RAD(VAR) LOOP_CAL_PT(VAR, __A, _7P_STEP)
+#define LOOP_CAL_ACT(VAR, _4P, _OP) LOOP_CAL_PT(VAR, _OP ? _AB : __A, _4P ? _4P_STEP : _7P_STEP)
+
+TERN_(HAS_MULTI_HOTEND, const uint8_t old_tool_index = active_extruder);
+
+float lcd_probe_pt(const xy_pos_t &xy);
+
+void ac_home() {
+  endstops.enable(true);
+  home_delta();
+  endstops.not_homing();
+}
+
+void ac_setup(const bool reset_bed) {
+  TERN_(HAS_MULTI_HOTEND, tool_change(0, true));
+
+  planner.synchronize();
+  remember_feedrate_scaling_off();
+
+  #if HAS_LEVELING
+    if (reset_bed) reset_bed_level(); // After full calibration bed-level data is no longer valid
+  #endif
+}
+
+void ac_cleanup(TERN_(HAS_MULTI_HOTEND, const uint8_t old_tool_index)) {
+  TERN_(DELTA_HOME_TO_SAFE_ZONE, do_blocking_move_to_z(delta_clip_start_height));
+  TERN_(HAS_BED_PROBE, probe.stow());
+  restore_feedrate_and_scaling();
+  TERN_(HAS_MULTI_HOTEND, tool_change(old_tool_index, true));
+}
+
+void print_signed_float(PGM_P const prefix, const float &f) {
+  SERIAL_ECHOPGM("  ");
+  serialprintPGM(prefix);
+  SERIAL_CHAR(':');
+  if (f >= 0) SERIAL_CHAR('+');
+  SERIAL_ECHO_F(f, 2);
+}
+
+/**
+ *  - Print the delta settings
+ */
+static void print_calibration_settings(const bool end_stops, const bool tower_angles) {
+  SERIAL_ECHOPAIR(".Height:", delta_height);
+  if (end_stops) {
+    print_signed_float(PSTR("Ex"), delta_endstop_adj.a);
+    print_signed_float(PSTR("Ey"), delta_endstop_adj.b);
+    print_signed_float(PSTR("Ez"), delta_endstop_adj.c);
+  }
+  if (end_stops && tower_angles) {
+    SERIAL_ECHOPAIR("  Radius:", delta_radius);
+    SERIAL_EOL();
+    SERIAL_CHAR('.');
+    SERIAL_ECHO_SP(13);
+  }
+  if (tower_angles) {
+    print_signed_float(PSTR("Tx"), delta_tower_angle_trim.a);
+    print_signed_float(PSTR("Ty"), delta_tower_angle_trim.b);
+    print_signed_float(PSTR("Tz"), delta_tower_angle_trim.c);
+  }
+  if ((!end_stops && tower_angles) || (end_stops && !tower_angles)) { // XOR
+    SERIAL_ECHOPAIR("  Radius:", delta_radius);
+  }
+  SERIAL_EOL();
+}
+
+/**
+ *  - Print the probe results
+ */
+static void print_calibration_results(const float z_pt[NPP + 1], const bool tower_points, const bool opposite_points) {
+  SERIAL_ECHOPGM(".    ");
+  print_signed_float(PSTR("c"), z_pt[CEN]);
+  if (tower_points) {
+    print_signed_float(PSTR(" x"), z_pt[__A]);
+    print_signed_float(PSTR(" y"), z_pt[__B]);
+    print_signed_float(PSTR(" z"), z_pt[__C]);
+  }
+  if (tower_points && opposite_points) {
+    SERIAL_EOL();
+    SERIAL_CHAR('.');
+    SERIAL_ECHO_SP(13);
+  }
+  if (opposite_points) {
+    print_signed_float(PSTR("yz"), z_pt[_BC]);
+    print_signed_float(PSTR("zx"), z_pt[_CA]);
+    print_signed_float(PSTR("xy"), z_pt[_AB]);
+  }
+  SERIAL_EOL();
+}
+
+/**
+ *  - Calculate the standard deviation from the zero plane
+ */
+static float std_dev_points(float z_pt[NPP + 1], const bool _0p_cal, const bool _1p_cal, const bool _4p_cal, const bool _4p_opp) {
+  if (!_0p_cal) {
+    float S2 = sq(z_pt[CEN]);
+    int16_t N = 1;
+    if (!_1p_cal) { // std dev from zero plane
+      LOOP_CAL_ACT(rad, _4p_cal, _4p_opp) {
+        S2 += sq(z_pt[rad]);
+        N++;
+      }
+      return LROUND(SQRT(S2 / N) * 1000.0f) / 1000.0f + 0.00001f;
+    }
+  }
+  return 0.00001f;
+}
+
+/**
+ *  - Probe a point
+ */
+static float calibration_probe(const xy_pos_t &xy, const bool stow) {
+  #if HAS_BED_PROBE
+    return probe.probe_at_point(xy, stow ? PROBE_PT_STOW : PROBE_PT_RAISE, 0, true, false);
+  #else
+    UNUSED(stow);
+    return lcd_probe_pt(xy);
+  #endif
+}
+
+/**
+ *  - Probe a grid
+ */
+static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_points, const bool towers_set, const bool stow_after_each) {
+  const bool _0p_calibration      = probe_points == 0,
+             _1p_calibration      = probe_points == 1 || probe_points == -1,
+             _4p_calibration      = probe_points == 2,
+             _4p_opposite_points  = _4p_calibration && !towers_set,
+             _7p_calibration      = probe_points >= 3,
+             _7p_no_intermediates = probe_points == 3,
+             _7p_1_intermediates  = probe_points == 4,
+             _7p_2_intermediates  = probe_points == 5,
+             _7p_4_intermediates  = probe_points == 6,
+             _7p_6_intermediates  = probe_points == 7,
+             _7p_8_intermediates  = probe_points == 8,
+             _7p_11_intermediates = probe_points == 9,
+             _7p_14_intermediates = probe_points == 10,
+             _7p_intermed_points  = probe_points >= 4,
+             _7p_6_center         = probe_points >= 5 && probe_points <= 7,
+             _7p_9_center         = probe_points >= 8;
+
+  LOOP_CAL_ALL(rad) z_pt[rad] = 0.0f;
+
+  if (!_0p_calibration) {
+
+    const float dcr = delta_calibration_radius();
+
+    if (!_7p_no_intermediates && !_7p_4_intermediates && !_7p_11_intermediates) { // probe the center
+      const xy_pos_t center{0};
+      z_pt[CEN] += calibration_probe(center, stow_after_each);
+      if (isnan(z_pt[CEN])) return false;
+    }
+
+    if (_7p_calibration) { // probe extra center points
+      const float start  = _7p_9_center ? float(_CA) + _7P_STEP / 3.0f : _7p_6_center ? float(_CA) : float(__C),
+                  steps  = _7p_9_center ? _4P_STEP / 3.0f : _7p_6_center ? _7P_STEP : _4P_STEP;
+      I_LOOP_CAL_PT(rad, start, steps) {
+        const float a = RADIANS(210 + (360 / NPP) *  (rad - 1)),
+                    r = dcr * 0.1;
+        const xy_pos_t vec = { cos(a), sin(a) };
+        z_pt[CEN] += calibration_probe(vec * r, stow_after_each);
+        if (isnan(z_pt[CEN])) return false;
+     }
+      z_pt[CEN] /= float(_7p_2_intermediates ? 7 : probe_points);
+    }
+
+    if (!_1p_calibration) {  // probe the radius
+      const CalEnum start  = _4p_opposite_points ? _AB : __A;
+      const float   steps  = _7p_14_intermediates ? _7P_STEP / 15.0f : // 15r * 6 + 10c = 100
+                             _7p_11_intermediates ? _7P_STEP / 12.0f : // 12r * 6 +  9c = 81
+                             _7p_8_intermediates  ? _7P_STEP /  9.0f : //  9r * 6 + 10c = 64
+                             _7p_6_intermediates  ? _7P_STEP /  7.0f : //  7r * 6 +  7c = 49
+                             _7p_4_intermediates  ? _7P_STEP /  5.0f : //  5r * 6 +  6c = 36
+                             _7p_2_intermediates  ? _7P_STEP /  3.0f : //  3r * 6 +  7c = 25
+                             _7p_1_intermediates  ? _7P_STEP /  2.0f : //  2r * 6 +  4c = 16
+                             _7p_no_intermediates ? _7P_STEP :        //  1r * 6 +  3c = 9
+                             _4P_STEP;                                // .5r * 6 +  1c = 4
+      bool zig_zag = true;
+      F_LOOP_CAL_PT(rad, start, _7p_9_center ? steps * 3 : steps) {
+        const int8_t offset = _7p_9_center ? 2 : 0;
+        for (int8_t circle = 0; circle <= offset; circle++) {
+          const float a = RADIANS(210 + (360 / NPP) *  (rad - 1)),
+                      r = dcr * (1 - 0.1 * (zig_zag ? offset - circle : circle)),
+                      interpol = FMOD(rad, 1);
+          const xy_pos_t vec = { cos(a), sin(a) };
+          const float z_temp = calibration_probe(vec * r, stow_after_each);
+          if (isnan(z_temp)) return false;
+          // split probe point to neighbouring calibration points
+          z_pt[uint8_t(LROUND(rad - interpol + NPP - 1)) % NPP + 1] += z_temp * sq(cos(RADIANS(interpol * 90)));
+          z_pt[uint8_t(LROUND(rad - interpol))           % NPP + 1] += z_temp * sq(sin(RADIANS(interpol * 90)));
+        }
+        zig_zag = !zig_zag;
+      }
+      if (_7p_intermed_points)
+        LOOP_CAL_RAD(rad)
+          z_pt[rad] /= _7P_STEP / steps;
+
+      do_blocking_move_to_xy(0.0f, 0.0f);
+    }
+  }
+  return true;
+}
+
+/**
+ * kinematics routines and auto tune matrix scaling parameters:
+ * see https://github.com/LVD-AC/Marlin-AC/tree/1.1.x-AC/documentation for
+ *  - formulae for approximative forward kinematics in the end-stop displacement matrix
+ *  - definition of the matrix scaling parameters
+ */
+static void reverse_kinematics_probe_points(float z_pt[NPP + 1], abc_float_t mm_at_pt_axis[NPP + 1]) {
+  xyz_pos_t pos{0};
+
+  const float dcr = delta_calibration_radius();
+  LOOP_CAL_ALL(rad) {
+    const float a = RADIANS(210 + (360 / NPP) *  (rad - 1)),
+                r = (rad == CEN ? 0.0f : dcr);
+    pos.set(cos(a) * r, sin(a) * r, z_pt[rad]);
+    inverse_kinematics(pos);
+    mm_at_pt_axis[rad] = delta;
+  }
+}
+
+static void forward_kinematics_probe_points(abc_float_t mm_at_pt_axis[NPP + 1], float z_pt[NPP + 1]) {
+  const float r_quot = delta_calibration_radius() / delta_radius;
+
+  #define ZPP(N,I,A) (((1.0f + r_quot * (N)) / 3.0f) * mm_at_pt_axis[I].A)
+  #define Z00(I, A) ZPP( 0, I, A)
+  #define Zp1(I, A) ZPP(+1, I, A)
+  #define Zm1(I, A) ZPP(-1, I, A)
+  #define Zp2(I, A) ZPP(+2, I, A)
+  #define Zm2(I, A) ZPP(-2, I, A)
+
+  z_pt[CEN] = Z00(CEN, a) + Z00(CEN, b) + Z00(CEN, c);
+  z_pt[__A] = Zp2(__A, a) + Zm1(__A, b) + Zm1(__A, c);
+  z_pt[__B] = Zm1(__B, a) + Zp2(__B, b) + Zm1(__B, c);
+  z_pt[__C] = Zm1(__C, a) + Zm1(__C, b) + Zp2(__C, c);
+  z_pt[_BC] = Zm2(_BC, a) + Zp1(_BC, b) + Zp1(_BC, c);
+  z_pt[_CA] = Zp1(_CA, a) + Zm2(_CA, b) + Zp1(_CA, c);
+  z_pt[_AB] = Zp1(_AB, a) + Zp1(_AB, b) + Zm2(_AB, c);
+}
+
+static void calc_kinematics_diff_probe_points(float z_pt[NPP + 1], abc_float_t delta_e, const float delta_r, abc_float_t delta_t) {
+  const float z_center = z_pt[CEN];
+  abc_float_t diff_mm_at_pt_axis[NPP + 1], new_mm_at_pt_axis[NPP + 1];
+
+  reverse_kinematics_probe_points(z_pt, diff_mm_at_pt_axis);
+
+  delta_radius += delta_r;
+  delta_tower_angle_trim += delta_t;
+  recalc_delta_settings();
+  reverse_kinematics_probe_points(z_pt, new_mm_at_pt_axis);
+
+  LOOP_CAL_ALL(rad) diff_mm_at_pt_axis[rad] -= new_mm_at_pt_axis[rad] + delta_e;
+  forward_kinematics_probe_points(diff_mm_at_pt_axis, z_pt);
+
+  LOOP_CAL_RAD(rad) z_pt[rad] -= z_pt[CEN] - z_center;
+  z_pt[CEN] = z_center;
+
+  delta_radius -= delta_r;
+  delta_tower_angle_trim -= delta_t;
+  recalc_delta_settings();
+}
+
+static float auto_tune_h() {
+  const float r_quot = delta_calibration_radius() / delta_radius;
+  return RECIPROCAL(r_quot / (2.0f / 3.0f));  // (2/3)/CR
+}
+
+static float auto_tune_r() {
+  constexpr float diff = 0.01f, delta_r = diff;
+  float r_fac = 0.0f, z_pt[NPP + 1] = { 0.0f };
+  abc_float_t delta_e = { 0.0f }, delta_t = { 0.0f };
+
+  calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t);
+  r_fac = -(z_pt[__A] + z_pt[__B] + z_pt[__C] + z_pt[_BC] + z_pt[_CA] + z_pt[_AB]) / 6.0f;
+  r_fac = diff / r_fac / 3.0f; // 1/(3*delta_Z)
+  return r_fac;
+}
+
+static float auto_tune_a() {
+  constexpr float diff = 0.01f, delta_r = 0.0f;
+  float a_fac = 0.0f, z_pt[NPP + 1] = { 0.0f };
+  abc_float_t delta_e = { 0.0f }, delta_t = { 0.0f };
+
+  delta_t.reset();
+  LOOP_XYZ(axis) {
+    delta_t[axis] = diff;
+    calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t);
+    delta_t[axis] = 0;
+    a_fac += z_pt[uint8_t((axis * _4P_STEP) - _7P_STEP + NPP) % NPP + 1] / 6.0f;
+    a_fac -= z_pt[uint8_t((axis * _4P_STEP) + 1 + _7P_STEP)] / 6.0f;
+  }
+  a_fac = diff / a_fac / 3.0f; // 1/(3*delta_Z)
+  return a_fac;
+}
+
+/**
+ * G33 - Delta '1-4-7-point' Auto-Calibration
+ *       Calibrate height, z_offset, endstops, delta radius, and tower angles.
+ *
+ * Parameters:
+ *
+ *   Pn  Number of probe points:
+ *      P0       Normalizes calibration.
+ *      P1       Calibrates height only with center probe.
+ *      P2       Probe center and towers. Calibrate height, endstops and delta radius.
+ *      P3       Probe all positions: center, towers and opposite towers. Calibrate all.
+ *      P4-P10   Probe all positions at different intermediate locations and average them.
+ *
+ *   T   Don't calibrate tower angle corrections
+ *
+ *   Cn.nn  Calibration precision; when omitted calibrates to maximum precision
+ *
+ *   Fn  Force to run at least n iterations and take the best result
+ *
+ *   Vn  Verbose level:
+ *      V0  Dry-run mode. Report settings and probe results. No calibration.
+ *      V1  Report start and end settings only
+ *      V2  Report settings at each iteration
+ *      V3  Report settings and probe results
+ *
+ *   E   Engage the probe for each point
+ */
+void GcodeSuite::G33() {
+
+  const int8_t probe_points = parser.intval('P', DELTA_CALIBRATION_DEFAULT_POINTS);
+  if (!WITHIN(probe_points, 0, 10)) {
+    SERIAL_ECHOLNPGM("?(P)oints implausible (0-10).");
+    return;
+  }
+
+  const bool towers_set = !parser.seen('T');
+
+  const float calibration_precision = parser.floatval('C', 0.0f);
+  if (calibration_precision < 0) {
+    SERIAL_ECHOLNPGM("?(C)alibration precision implausible (>=0).");
+    return;
+  }
+
+  const int8_t force_iterations = parser.intval('F', 0);
+  if (!WITHIN(force_iterations, 0, 30)) {
+    SERIAL_ECHOLNPGM("?(F)orce iteration implausible (0-30).");
+    return;
+  }
+
+  const int8_t verbose_level = parser.byteval('V', 1);
+  if (!WITHIN(verbose_level, 0, 3)) {
+    SERIAL_ECHOLNPGM("?(V)erbose level implausible (0-3).");
+    return;
+  }
+
+  const bool stow_after_each = parser.seen('E');
+
+  const bool _0p_calibration      = probe_points == 0,
+             _1p_calibration      = probe_points == 1 || probe_points == -1,
+             _4p_calibration      = probe_points == 2,
+             _4p_opposite_points  = _4p_calibration && !towers_set,
+             _7p_9_center         = probe_points >= 8,
+             _tower_results       = (_4p_calibration && towers_set) || probe_points >= 3,
+             _opposite_results    = (_4p_calibration && !towers_set) || probe_points >= 3,
+             _endstop_results     = probe_points != 1 && probe_points != -1 && probe_points != 0,
+             _angle_results       = probe_points >= 3 && towers_set;
+  int8_t iterations = 0;
+  float test_precision,
+        zero_std_dev = (verbose_level ? 999.0f : 0.0f), // 0.0 in dry-run mode : forced end
+        zero_std_dev_min = zero_std_dev,
+        zero_std_dev_old = zero_std_dev,
+        h_factor, r_factor, a_factor,
+        r_old = delta_radius,
+        h_old = delta_height;
+
+  abc_pos_t e_old = delta_endstop_adj, a_old = delta_tower_angle_trim;
+
+  SERIAL_ECHOLNPGM("G33 Auto Calibrate");
+
+  const float dcr = delta_calibration_radius();
+
+  if (!_1p_calibration && !_0p_calibration) { // test if the outer radius is reachable
+    LOOP_CAL_RAD(axis) {
+      const float a = RADIANS(210 + (360 / NPP) *  (axis - 1));
+      if (!position_is_reachable(cos(a) * dcr, sin(a) * dcr)) {
+        SERIAL_ECHOLNPGM("?Bed calibration radius implausible.");
+        return;
+      }
+    }
+  }
+
+  // Report settings
+  PGM_P const checkingac = PSTR("Checking... AC");
+  serialprintPGM(checkingac);
+  if (verbose_level == 0) SERIAL_ECHOPGM(" (DRY-RUN)");
+  SERIAL_EOL();
+  ui.set_status_P(checkingac);
+
+  print_calibration_settings(_endstop_results, _angle_results);
+
+  ac_setup(!_0p_calibration && !_1p_calibration);
+
+  if (!_0p_calibration) ac_home();
+
+  do { // start iterations
+
+    float z_at_pt[NPP + 1] = { 0.0f };
+
+    test_precision = zero_std_dev_old != 999.0f ? (zero_std_dev + zero_std_dev_old) / 2.0f : zero_std_dev;
+    iterations++;
+
+    // Probe the points
+    zero_std_dev_old = zero_std_dev;
+    if (!probe_calibration_points(z_at_pt, probe_points, towers_set, stow_after_each)) {
+      SERIAL_ECHOLNPGM("Correct delta settings with M665 and M666");
+      return ac_cleanup(TERN_(HAS_MULTI_HOTEND, old_tool_index));
+    }
+    zero_std_dev = std_dev_points(z_at_pt, _0p_calibration, _1p_calibration, _4p_calibration, _4p_opposite_points);
+
+    // Solve matrices
+
+    if ((zero_std_dev < test_precision || iterations <= force_iterations) && zero_std_dev > calibration_precision) {
+
+      #if !HAS_BED_PROBE
+        test_precision = 0.0f; // forced end
+      #endif
+
+      if (zero_std_dev < zero_std_dev_min) {
+        // set roll-back point
+        e_old = delta_endstop_adj;
+        r_old = delta_radius;
+        h_old = delta_height;
+        a_old = delta_tower_angle_trim;
+      }
+
+      abc_float_t e_delta = { 0.0f }, t_delta = { 0.0f };
+      float r_delta = 0.0f;
+
+      /**
+       * convergence matrices:
+       * see https://github.com/LVD-AC/Marlin-AC/tree/1.1.x-AC/documentation for
+       *  - definition of the matrix scaling parameters
+       *  - matrices for 4 and 7 point calibration
+       */
+      #define ZP(N,I) ((N) * z_at_pt[I] / 4.0f) // 4.0 = divider to normalize to integers
+      #define Z12(I) ZP(12, I)
+      #define Z4(I) ZP(4, I)
+      #define Z2(I) ZP(2, I)
+      #define Z1(I) ZP(1, I)
+      #define Z0(I) ZP(0, I)
+
+      // calculate factors
+      if (_7p_9_center) calibration_radius_factor = 0.9f;
+      h_factor = auto_tune_h();
+      r_factor = auto_tune_r();
+      a_factor = auto_tune_a();
+      calibration_radius_factor = 1.0f;
+
+      switch (probe_points) {
+        case 0:
+          test_precision = 0.0f; // forced end
+          break;
+
+        case 1:
+          test_precision = 0.0f; // forced end
+          LOOP_XYZ(axis) e_delta[axis] = +Z4(CEN);
+          break;
+
+        case 2:
+          if (towers_set) { // see 4 point calibration (towers) matrix
+            e_delta.set((+Z4(__A) -Z2(__B) -Z2(__C)) * h_factor  +Z4(CEN),
+                        (-Z2(__A) +Z4(__B) -Z2(__C)) * h_factor  +Z4(CEN),
+                        (-Z2(__A) -Z2(__B) +Z4(__C)) * h_factor  +Z4(CEN));
+            r_delta   = (+Z4(__A) +Z4(__B) +Z4(__C) -Z12(CEN)) * r_factor;
+          }
+          else { // see 4 point calibration (opposites) matrix
+            e_delta.set((-Z4(_BC) +Z2(_CA) +Z2(_AB)) * h_factor  +Z4(CEN),
+                        (+Z2(_BC) -Z4(_CA) +Z2(_AB)) * h_factor  +Z4(CEN),
+                        (+Z2(_BC) +Z2(_CA) -Z4(_AB)) * h_factor  +Z4(CEN));
+            r_delta   = (+Z4(_BC) +Z4(_CA) +Z4(_AB) -Z12(CEN)) * r_factor;
+          }
+          break;
+
+        default: // see 7 point calibration (towers & opposites) matrix
+          e_delta.set((+Z2(__A) -Z1(__B) -Z1(__C) -Z2(_BC) +Z1(_CA) +Z1(_AB)) * h_factor  +Z4(CEN),
+                      (-Z1(__A) +Z2(__B) -Z1(__C) +Z1(_BC) -Z2(_CA) +Z1(_AB)) * h_factor  +Z4(CEN),
+                      (-Z1(__A) -Z1(__B) +Z2(__C) +Z1(_BC) +Z1(_CA) -Z2(_AB)) * h_factor  +Z4(CEN));
+          r_delta   = (+Z2(__A) +Z2(__B) +Z2(__C) +Z2(_BC) +Z2(_CA) +Z2(_AB) -Z12(CEN)) * r_factor;
+
+          if (towers_set) { // see 7 point tower angle calibration (towers & opposites) matrix
+            t_delta.set((+Z0(__A) -Z4(__B) +Z4(__C) +Z0(_BC) -Z4(_CA) +Z4(_AB) +Z0(CEN)) * a_factor,
+                        (+Z4(__A) +Z0(__B) -Z4(__C) +Z4(_BC) +Z0(_CA) -Z4(_AB) +Z0(CEN)) * a_factor,
+                        (-Z4(__A) +Z4(__B) +Z0(__C) -Z4(_BC) +Z4(_CA) +Z0(_AB) +Z0(CEN)) * a_factor);
+          }
+          break;
+      }
+      delta_endstop_adj += e_delta;
+      delta_radius += r_delta;
+      delta_tower_angle_trim += t_delta;
+    }
+    else if (zero_std_dev >= test_precision) {
+      // roll back
+      delta_endstop_adj = e_old;
+      delta_radius = r_old;
+      delta_height = h_old;
+      delta_tower_angle_trim = a_old;
+    }
+
+    if (verbose_level != 0) {                                    // !dry run
+
+      // Normalize angles to least-squares
+      if (_angle_results) {
+        float a_sum = 0.0f;
+        LOOP_XYZ(axis) a_sum += delta_tower_angle_trim[axis];
+        LOOP_XYZ(axis) delta_tower_angle_trim[axis] -= a_sum / 3.0f;
+      }
+
+      // adjust delta_height and endstops by the max amount
+      const float z_temp = _MAX(delta_endstop_adj.a, delta_endstop_adj.b, delta_endstop_adj.c);
+      delta_height -= z_temp;
+      LOOP_XYZ(axis) delta_endstop_adj[axis] -= z_temp;
+    }
+    recalc_delta_settings();
+    NOMORE(zero_std_dev_min, zero_std_dev);
+
+    // print report
+
+    if (verbose_level == 3)
+      print_calibration_results(z_at_pt, _tower_results, _opposite_results);
+
+    if (verbose_level != 0) { // !dry run
+      if ((zero_std_dev >= test_precision && iterations > force_iterations) || zero_std_dev <= calibration_precision) { // end iterations
+        SERIAL_ECHOPGM("Calibration OK");
+        SERIAL_ECHO_SP(32);
+        #if HAS_BED_PROBE
+          if (zero_std_dev >= test_precision && !_1p_calibration && !_0p_calibration)
+            SERIAL_ECHOPGM("rolling back.");
+          else
+        #endif
+          {
+            SERIAL_ECHOPAIR_F("std dev:", zero_std_dev_min, 3);
+          }
+        SERIAL_EOL();
+        char mess[21];
+        strcpy_P(mess, PSTR("Calibration sd:"));
+        if (zero_std_dev_min < 1)
+          sprintf_P(&mess[15], PSTR("0.%03i"), (int)LROUND(zero_std_dev_min * 1000.0f));
+        else
+          sprintf_P(&mess[15], PSTR("%03i.x"), (int)LROUND(zero_std_dev_min));
+        ui.set_status(mess);
+        print_calibration_settings(_endstop_results, _angle_results);
+        SERIAL_ECHOLNPGM("Save with M500 and/or copy to Configuration.h");
+      }
+      else { // !end iterations
+        char mess[15];
+        if (iterations < 31)
+          sprintf_P(mess, PSTR("Iteration : %02i"), (unsigned int)iterations);
+        else
+          strcpy_P(mess, PSTR("No convergence"));
+        SERIAL_ECHO(mess);
+        SERIAL_ECHO_SP(32);
+        SERIAL_ECHOLNPAIR_F("std dev:", zero_std_dev, 3);
+        ui.set_status(mess);
+        if (verbose_level > 1)
+          print_calibration_settings(_endstop_results, _angle_results);
+      }
+    }
+    else { // dry run
+      PGM_P const enddryrun = PSTR("End DRY-RUN");
+      serialprintPGM(enddryrun);
+      SERIAL_ECHO_SP(35);
+      SERIAL_ECHOLNPAIR_F("std dev:", zero_std_dev, 3);
+
+      char mess[21];
+      strcpy_P(mess, enddryrun);
+      strcpy_P(&mess[11], PSTR(" sd:"));
+      if (zero_std_dev < 1)
+        sprintf_P(&mess[15], PSTR("0.%03i"), (int)LROUND(zero_std_dev * 1000.0f));
+      else
+        sprintf_P(&mess[15], PSTR("%03i.x"), (int)LROUND(zero_std_dev));
+      ui.set_status(mess);
+    }
+    ac_home();
+  }
+  while (((zero_std_dev < test_precision && iterations < 31) || iterations <= force_iterations) && zero_std_dev > calibration_precision);
+
+  ac_cleanup(TERN_(HAS_MULTI_HOTEND, old_tool_index));
+}
+
+#endif // DELTA_AUTO_CALIBRATION
diff --git a/Marlin/src/gcode/calibrate/G34.cpp b/Marlin/src/gcode/calibrate/G34.cpp
new file mode 100644
index 0000000..bcca00d
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/G34.cpp
@@ -0,0 +1,157 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if ENABLED(MECHANICAL_GANTRY_CALIBRATION)
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/stepper.h"
+#include "../../module/endstops.h"
+
+#if HAS_LEVELING
+  #include "../../feature/bedlevel/bedlevel.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../core/debug_out.h"
+
+void GcodeSuite::G34() {
+
+  // Home before the alignment procedure
+  if (!all_axes_trusted()) home_all_axes();
+
+  TERN_(HAS_LEVELING, TEMPORARY_BED_LEVELING_STATE(false));
+
+  SET_SOFT_ENDSTOP_LOOSE(true);
+  TemporaryGlobalEndstopsState unlock_z(false);
+
+  #ifdef GANTRY_CALIBRATION_COMMANDS_PRE
+    gcode.process_subcommands_now_P(PSTR(GANTRY_CALIBRATION_COMMANDS_PRE));
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Sub Commands Processed");
+  #endif
+
+  #ifdef GANTRY_CALIBRATION_SAFE_POSITION
+    // Move XY to safe position
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Parking XY");
+    const xy_pos_t safe_pos = GANTRY_CALIBRATION_SAFE_POSITION;
+    do_blocking_move_to(safe_pos, MMM_TO_MMS(GANTRY_CALIBRATION_XY_PARK_FEEDRATE));
+  #endif
+
+  const float move_distance = parser.intval('Z', GANTRY_CALIBRATION_EXTRA_HEIGHT),
+              zbase = ENABLED(GANTRY_CALIBRATION_TO_MIN) ? Z_MIN_POS : Z_MAX_POS,
+              zpounce = zbase - move_distance, zgrind = zbase + move_distance;
+
+  // Move Z to pounce position
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Setting Z Pounce");
+  do_blocking_move_to_z(zpounce, homing_feedrate(Z_AXIS));
+
+  // Store current motor settings, then apply reduced value
+
+  #define _REDUCE_CURRENT ANY(HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_PWM, HAS_MOTOR_CURRENT_DAC, HAS_MOTOR_CURRENT_I2C, HAS_TRINAMIC_CONFIG)
+  #if _REDUCE_CURRENT
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Reducing Current");
+  #endif
+
+  #if HAS_MOTOR_CURRENT_SPI
+    const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT);
+    const uint32_t previous_current = stepper.motor_current_setting[Z_AXIS];
+    stepper.set_digipot_current(Z_AXIS, target_current);
+  #elif HAS_MOTOR_CURRENT_PWM
+    const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT);
+    const uint32_t previous_current = stepper.motor_current_setting[Z_AXIS];
+    stepper.set_digipot_current(1, target_current);
+  #elif ENABLED(HAS_MOTOR_CURRENT_DAC)
+    const float target_current = parser.floatval('S', GANTRY_CALIBRATION_CURRENT);
+    const float previous_current = dac_amps(Z_AXIS, target_current);
+    stepper_dac.set_current_value(Z_AXIS, target_current);
+  #elif ENABLED(HAS_MOTOR_CURRENT_I2C)
+    const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT);
+    previous_current = dac_amps(Z_AXIS);
+    digipot_i2c.set_current(Z_AXIS, target_current)
+  #elif HAS_TRINAMIC_CONFIG
+    const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT);
+    static uint16_t previous_current_arr[NUM_Z_STEPPER_DRIVERS];
+    #if AXIS_IS_TMC(Z)
+      previous_current_arr[0] = stepperZ.getMilliamps();
+      stepperZ.rms_current(target_current);
+    #endif
+    #if AXIS_IS_TMC(Z2)
+      previous_current_arr[1] = stepperZ2.getMilliamps();
+      stepperZ2.rms_current(target_current);
+    #endif
+    #if AXIS_IS_TMC(Z3)
+      previous_current_arr[2] = stepperZ3.getMilliamps();
+      stepperZ3.rms_current(target_current);
+    #endif
+    #if AXIS_IS_TMC(Z4)
+      previous_current_arr[3] = stepperZ4.getMilliamps();
+      stepperZ4.rms_current(target_current);
+    #endif
+  #endif
+
+  // Do Final Z move to adjust
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Final Z Move");
+  do_blocking_move_to_z(zgrind, MMM_TO_MMS(GANTRY_CALIBRATION_FEEDRATE));
+
+  // Back off end plate, back to normal motion range
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Z Backoff");
+  do_blocking_move_to_z(zpounce, MMM_TO_MMS(GANTRY_CALIBRATION_FEEDRATE));
+
+  #if _REDUCE_CURRENT
+    // Reset current to original values
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Restore Current");
+  #endif
+
+  #if HAS_MOTOR_CURRENT_SPI
+    stepper.set_digipot_current(Z_AXIS, previous_current);
+  #elif HAS_MOTOR_CURRENT_PWM
+    stepper.set_digipot_current(1, previous_current);
+  #elif ENABLED(HAS_MOTOR_CURRENT_DAC)
+    stepper_dac.set_current_value(Z_AXIS, previous_current);
+  #elif ENABLED(HAS_MOTOR_CURRENT_I2C)
+    digipot_i2c.set_current(Z_AXIS, previous_current)
+  #elif HAS_TRINAMIC_CONFIG
+    #if AXIS_IS_TMC(Z)
+      stepperZ.rms_current(previous_current_arr[0]);
+    #endif
+    #if AXIS_IS_TMC(Z2)
+      stepperZ2.rms_current(previous_current_arr[1]);
+    #endif
+    #if AXIS_IS_TMC(Z3)
+      stepperZ3.rms_current(previous_current_arr[2]);
+    #endif
+    #if AXIS_IS_TMC(Z4)
+      stepperZ4.rms_current(previous_current_arr[3]);
+    #endif
+  #endif
+
+  #ifdef GANTRY_CALIBRATION_COMMANDS_POST
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Running Post Commands");
+    gcode.process_subcommands_now_P(PSTR(GANTRY_CALIBRATION_COMMANDS_POST));
+  #endif
+
+  SET_SOFT_ENDSTOP_LOOSE(false);
+}
+
+#endif // MECHANICAL_GANTRY_CALIBRATION
diff --git a/Marlin/src/gcode/calibrate/G34_M422.cpp b/Marlin/src/gcode/calibrate/G34_M422.cpp
new file mode 100644
index 0000000..0bcf954
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/G34_M422.cpp
@@ -0,0 +1,533 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if EITHER(Z_MULTI_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
+
+#include "../../feature/z_stepper_align.h"
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/stepper.h"
+#include "../../module/planner.h"
+#include "../../module/probe.h"
+#include "../../lcd/marlinui.h" // for LCD_MESSAGEPGM
+
+#if HAS_LEVELING
+  #include "../../feature/bedlevel/bedlevel.h"
+#endif
+
+#if HAS_MULTI_HOTEND
+  #include "../../module/tool_change.h"
+#endif
+
+#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+  #include "../../libs/least_squares_fit.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../core/debug_out.h"
+
+/**
+ * G34: Z-Stepper automatic alignment
+ *
+ * Manual stepper lock controls (reset by G28):
+ *   L                 Unlock all steppers
+ *   Z<1-4>            Z stepper to lock / unlock
+ *   S<state>          0=UNLOCKED 1=LOCKED. If omitted, assume LOCKED.
+ *
+ *   Examples:
+ *     G34 Z1     ; Lock Z1
+ *     G34 L Z2   ; Unlock all, then lock Z2
+ *     G34 Z2 S0  ; Unlock Z2
+ *
+ * With Z_STEPPER_AUTO_ALIGN:
+ *   I<iterations>     Number of tests. If omitted, Z_STEPPER_ALIGN_ITERATIONS.
+ *   T<accuracy>       Target Accuracy factor. If omitted, Z_STEPPER_ALIGN_ACC.
+ *   A<amplification>  Provide an Amplification value. If omitted, Z_STEPPER_ALIGN_AMP.
+ *   R                 Flag to recalculate points based on current probe offsets
+ */
+void GcodeSuite::G34() {
+  DEBUG_SECTION(log_G34, "G34", DEBUGGING(LEVELING));
+  if (DEBUGGING(LEVELING)) log_machine_info();
+
+  planner.synchronize();  // Prevent damage
+
+  const bool seenL = parser.seen('L');
+  if (seenL) stepper.set_all_z_lock(false);
+
+  const bool seenZ = parser.seenval('Z');
+  if (seenZ) {
+    const bool state = parser.boolval('S', true);
+    switch (parser.intval('Z')) {
+      case 1: stepper.set_z1_lock(state); break;
+      case 2: stepper.set_z2_lock(state); break;
+      #if NUM_Z_STEPPER_DRIVERS >= 3
+        case 3: stepper.set_z3_lock(state); break;
+        #if NUM_Z_STEPPER_DRIVERS >= 4
+          case 4: stepper.set_z4_lock(state); break;
+        #endif
+      #endif
+    }
+  }
+
+  if (seenL || seenZ) {
+    stepper.set_separate_multi_axis(seenZ);
+    return;
+  }
+
+  #if ENABLED(Z_STEPPER_AUTO_ALIGN)
+    do { // break out on error
+
+      #if NUM_Z_STEPPER_DRIVERS == 4
+        SERIAL_ECHOLNPGM("Alignment for 4 steppers is Experimental!");
+      #elif NUM_Z_STEPPER_DRIVERS > 4
+        SERIAL_ECHOLNPGM("Alignment not supported for over 4 steppers");
+        break;
+      #endif
+
+      const int8_t z_auto_align_iterations = parser.intval('I', Z_STEPPER_ALIGN_ITERATIONS);
+      if (!WITHIN(z_auto_align_iterations, 1, 30)) {
+        SERIAL_ECHOLNPGM("?(I)teration out of bounds (1-30).");
+        break;
+      }
+
+      const float z_auto_align_accuracy = parser.floatval('T', Z_STEPPER_ALIGN_ACC);
+      if (!WITHIN(z_auto_align_accuracy, 0.01f, 1.0f)) {
+        SERIAL_ECHOLNPGM("?(T)arget accuracy out of bounds (0.01-1.0).");
+        break;
+      }
+
+      const float z_auto_align_amplification = TERN(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS, Z_STEPPER_ALIGN_AMP, parser.floatval('A', Z_STEPPER_ALIGN_AMP));
+      if (!WITHIN(ABS(z_auto_align_amplification), 0.5f, 2.0f)) {
+        SERIAL_ECHOLNPGM("?(A)mplification out of bounds (0.5-2.0).");
+        break;
+      }
+
+      if (parser.seen('R')) z_stepper_align.reset_to_default();
+
+      const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;
+
+      // Disable the leveling matrix before auto-aligning
+      #if HAS_LEVELING
+        TERN_(RESTORE_LEVELING_AFTER_G34, const bool leveling_was_active = planner.leveling_active);
+        set_bed_leveling_enabled(false);
+      #endif
+
+      TERN_(CNC_WORKSPACE_PLANES, workspace_plane = PLANE_XY);
+
+      // Always home with tool 0 active
+      #if HAS_MULTI_HOTEND
+        const uint8_t old_tool_index = active_extruder;
+        tool_change(0, true);
+      #endif
+
+      TERN_(HAS_DUPLICATION_MODE, set_duplication_enabled(false));
+
+      // In BLTOUCH HS mode, the probe travels in a deployed state.
+      // Users of G34 might have a badly misaligned bed, so raise Z by the
+      // length of the deployed pin (BLTOUCH stroke < 7mm)
+      #define Z_BASIC_CLEARANCE (Z_CLEARANCE_BETWEEN_PROBES + 7.0f * BOTH(BLTOUCH, BLTOUCH_HS_MODE))
+
+      // Compute a worst-case clearance height to probe from. After the first
+      // iteration this will be re-calculated based on the actual bed position
+      auto magnitude2 = [&](const uint8_t i, const uint8_t j) {
+        const xy_pos_t diff = z_stepper_align.xy[i] - z_stepper_align.xy[j];
+        return HYPOT2(diff.x, diff.y);
+      };
+      float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * SQRT(
+        #if NUM_Z_STEPPER_DRIVERS == 3
+          _MAX(magnitude2(0, 1), magnitude2(1, 2), magnitude2(2, 0))
+        #elif NUM_Z_STEPPER_DRIVERS == 4
+          _MAX(magnitude2(0, 1), magnitude2(1, 2), magnitude2(2, 3),
+                magnitude2(3, 0), magnitude2(0, 2), magnitude2(1, 3))
+        #else
+          magnitude2(0, 1)
+        #endif
+      );
+
+      // Home before the alignment procedure
+      if (!all_axes_trusted()) home_all_axes();
+
+      // Move the Z coordinate realm towards the positive - dirty trick
+      current_position.z += z_probe * 0.5f;
+      sync_plan_position();
+      // Now, the Z origin lies below the build plate. That allows to probe deeper, before run_z_probe throws an error.
+      // This hack is un-done at the end of G34 - either by re-homing, or by using the probed heights of the last iteration.
+
+      #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+        float last_z_align_move[NUM_Z_STEPPER_DRIVERS] = ARRAY_N(NUM_Z_STEPPER_DRIVERS, 10000.0f, 10000.0f, 10000.0f, 10000.0f);
+      #else
+        float last_z_align_level_indicator = 10000.0f;
+      #endif
+      float z_measured[NUM_Z_STEPPER_DRIVERS] = { 0 },
+            z_maxdiff = 0.0f,
+            amplification = z_auto_align_amplification;
+
+      #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+        bool adjustment_reverse = false;
+      #endif
+
+      #if HAS_DISPLAY
+        PGM_P const msg_iteration = GET_TEXT(MSG_ITERATION);
+        const uint8_t iter_str_len = strlen_P(msg_iteration);
+      #endif
+
+      // Final z and iteration values will be used after breaking the loop
+      float z_measured_min;
+      uint8_t iteration = 0;
+      bool err_break = false; // To break out of nested loops
+      while (iteration < z_auto_align_iterations) {
+        if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> probing all positions.");
+
+        const int iter = iteration + 1;
+        SERIAL_ECHOLNPAIR("\nG34 Iteration: ", iter);
+        #if HAS_DISPLAY
+          char str[iter_str_len + 2 + 1];
+          sprintf_P(str, msg_iteration, iter);
+          ui.set_status(str);
+        #endif
+
+        // Initialize minimum value
+        z_measured_min =  100000.0f;
+        float z_measured_max = -100000.0f;
+
+        // Probe all positions (one per Z-Stepper)
+        LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) {
+          // iteration odd/even --> downward / upward stepper sequence
+          const uint8_t iprobe = (iteration & 1) ? NUM_Z_STEPPER_DRIVERS - 1 - i : i;
+
+          // Safe clearance even on an incline
+          if ((iteration == 0 || i > 0) && z_probe > current_position.z) do_blocking_move_to_z(z_probe);
+
+          if (DEBUGGING(LEVELING))
+            DEBUG_ECHOLNPAIR_P(PSTR("Probing X"), z_stepper_align.xy[iprobe].x, SP_Y_STR, z_stepper_align.xy[iprobe].y);
+
+          // Probe a Z height for each stepper.
+          // Probing sanity check is disabled, as it would trigger even in normal cases because
+          // current_position.z has been manually altered in the "dirty trick" above.
+          const float z_probed_height = probe.probe_at_point(z_stepper_align.xy[iprobe], raise_after, 0, true, false);
+          if (isnan(z_probed_height)) {
+            SERIAL_ECHOLNPGM("Probing failed");
+            LCD_MESSAGEPGM(MSG_LCD_PROBING_FAILED);
+            err_break = true;
+            break;
+          }
+
+          // Add height to each value, to provide a more useful target height for
+          // the next iteration of probing. This allows adjustments to be made away from the bed.
+          z_measured[iprobe] = z_probed_height + Z_CLEARANCE_BETWEEN_PROBES;
+
+          if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", int(iprobe + 1), " measured position is ", z_measured[iprobe]);
+
+          // Remember the minimum measurement to calculate the correction later on
+          z_measured_min = _MIN(z_measured_min, z_measured[iprobe]);
+          z_measured_max = _MAX(z_measured_max, z_measured[iprobe]);
+        } // for (i)
+
+        if (err_break) break;
+
+        // Adapt the next probe clearance height based on the new measurements.
+        // Safe_height = lowest distance to bed (= highest measurement) plus highest measured misalignment.
+        z_maxdiff = z_measured_max - z_measured_min;
+        z_probe = Z_BASIC_CLEARANCE + z_measured_max + z_maxdiff;
+
+        #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+          // Replace the initial values in z_measured with calculated heights at
+          // each stepper position. This allows the adjustment algorithm to be
+          // shared between both possible probing mechanisms.
+
+          // This must be done after the next z_probe height is calculated, so that
+          // the height is calculated from actual print area positions, and not
+          // extrapolated motor movements.
+
+          // Compute the least-squares fit for all probed points.
+          // Calculate the Z position of each stepper and store it in z_measured.
+          // This allows the actual adjustment logic to be shared by both algorithms.
+          linear_fit_data lfd;
+          incremental_LSF_reset(&lfd);
+          LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) {
+            SERIAL_ECHOLNPAIR("PROBEPT_", int(i), ": ", z_measured[i]);
+            incremental_LSF(&lfd, z_stepper_align.xy[i], z_measured[i]);
+          }
+          finish_incremental_LSF(&lfd);
+
+          z_measured_min = 100000.0f;
+          LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) {
+            z_measured[i] = -(lfd.A * z_stepper_align.stepper_xy[i].x + lfd.B * z_stepper_align.stepper_xy[i].y + lfd.D);
+            z_measured_min = _MIN(z_measured_min, z_measured[i]);
+          }
+
+          SERIAL_ECHOLNPAIR("CALCULATED STEPPER POSITIONS: Z1=", z_measured[0], " Z2=", z_measured[1], " Z3=", z_measured[2]);
+        #endif
+
+        SERIAL_ECHOLNPAIR("\n"
+          "DIFFERENCE Z1-Z2=", ABS(z_measured[0] - z_measured[1])
+          #if NUM_Z_STEPPER_DRIVERS == 3
+            , " Z2-Z3=", ABS(z_measured[1] - z_measured[2])
+            , " Z3-Z1=", ABS(z_measured[2] - z_measured[0])
+          #endif
+        );
+        #if HAS_DISPLAY
+          char fstr1[10];
+          #if NUM_Z_STEPPER_DRIVERS == 2
+            char msg[6 + (6 + 5) * 1 + 1];
+          #else
+            char msg[6 + (6 + 5) * 3 + 1], fstr2[10], fstr3[10];
+          #endif
+          sprintf_P(msg,
+            PSTR("Diffs Z1-Z2=%s"
+              #if NUM_Z_STEPPER_DRIVERS == 3
+                " Z2-Z3=%s"
+                " Z3-Z1=%s"
+              #endif
+            ), dtostrf(ABS(z_measured[0] - z_measured[1]), 1, 3, fstr1)
+            #if NUM_Z_STEPPER_DRIVERS == 3
+              , dtostrf(ABS(z_measured[1] - z_measured[2]), 1, 3, fstr2)
+              , dtostrf(ABS(z_measured[2] - z_measured[0]), 1, 3, fstr3)
+            #endif
+          );
+          ui.set_status(msg);
+        #endif
+
+        auto decreasing_accuracy = [](const float &v1, const float &v2){
+          if (v1 < v2 * 0.7f) {
+            SERIAL_ECHOLNPGM("Decreasing Accuracy Detected.");
+            LCD_MESSAGEPGM(MSG_DECREASING_ACCURACY);
+            return true;
+          }
+          return false;
+        };
+
+        #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+          // Check if the applied corrections go in the correct direction.
+          // Calculate the sum of the absolute deviations from the mean of the probe measurements.
+          // Compare to the last iteration to ensure it's getting better.
+
+          // Calculate mean value as a reference
+          float z_measured_mean = 0.0f;
+          LOOP_L_N(zstepper, NUM_Z_STEPPER_DRIVERS) z_measured_mean += z_measured[zstepper];
+          z_measured_mean /= NUM_Z_STEPPER_DRIVERS;
+
+          // Calculate the sum of the absolute deviations from the mean value
+          float z_align_level_indicator = 0.0f;
+          LOOP_L_N(zstepper, NUM_Z_STEPPER_DRIVERS)
+            z_align_level_indicator += ABS(z_measured[zstepper] - z_measured_mean);
+
+          // If it's getting worse, stop and throw an error
+          err_break = decreasing_accuracy(last_z_align_level_indicator, z_align_level_indicator);
+          if (err_break) break;
+
+          last_z_align_level_indicator = z_align_level_indicator;
+        #endif
+
+        // The following correction actions are to be enabled for select Z-steppers only
+        stepper.set_separate_multi_axis(true);
+
+        bool success_break = true;
+        // Correct the individual stepper offsets
+        LOOP_L_N(zstepper, NUM_Z_STEPPER_DRIVERS) {
+          // Calculate current stepper move
+          float z_align_move = z_measured[zstepper] - z_measured_min;
+          const float z_align_abs = ABS(z_align_move);
+
+          #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+            // Optimize one iteration's correction based on the first measurements
+            if (z_align_abs) amplification = (iteration == 1) ? _MIN(last_z_align_move[zstepper] / z_align_abs, 2.0f) : z_auto_align_amplification;
+
+            // Check for less accuracy compared to last move
+            if (decreasing_accuracy(last_z_align_move[zstepper], z_align_abs)) {
+              if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", int(zstepper + 1), " last_z_align_move = ", last_z_align_move[zstepper]);
+              if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", int(zstepper + 1), " z_align_abs = ", z_align_abs);
+              adjustment_reverse = !adjustment_reverse;
+            }
+
+            // Remember the alignment for the next iteration, but only if steppers move,
+            // otherwise it would be just zero (in case this stepper was at z_measured_min already)
+            if (z_align_abs > 0) last_z_align_move[zstepper] = z_align_abs;
+          #endif
+
+          // Stop early if all measured points achieve accuracy target
+          if (z_align_abs > z_auto_align_accuracy) success_break = false;
+
+          if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", int(zstepper + 1), " corrected by ", z_align_move);
+
+          // Lock all steppers except one
+          stepper.set_all_z_lock(true, zstepper);
+
+          #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+            // Decreasing accuracy was detected so move was inverted.
+            // Will match reversed Z steppers on dual steppers. Triple will need more work to map.
+            if (adjustment_reverse) {
+              z_align_move = -z_align_move;
+              if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", int(zstepper + 1), " correction reversed to ", z_align_move);
+            }
+          #endif
+
+          // Do a move to correct part of the misalignment for the current stepper
+          do_blocking_move_to_z(amplification * z_align_move + current_position.z);
+        } // for (zstepper)
+
+        // Back to normal stepper operations
+        stepper.set_all_z_lock(false);
+        stepper.set_separate_multi_axis(false);
+
+        if (err_break) break;
+
+        if (success_break) {
+          SERIAL_ECHOLNPGM("Target accuracy achieved.");
+          LCD_MESSAGEPGM(MSG_ACCURACY_ACHIEVED);
+          break;
+        }
+
+        iteration++;
+      } // while (iteration < z_auto_align_iterations)
+
+      if (err_break)
+        SERIAL_ECHOLNPGM("G34 aborted.");
+      else {
+        SERIAL_ECHOLNPAIR("Did ", int(iteration + (iteration != z_auto_align_iterations)), " of ", int(z_auto_align_iterations));
+        SERIAL_ECHOLNPAIR_F("Accuracy: ", z_maxdiff);
+      }
+
+      // Stow the probe, as the last call to probe.probe_at_point(...) left
+      // the probe deployed if it was successful.
+      probe.stow();
+
+      #if ENABLED(HOME_AFTER_G34)
+        // After this operation the z position needs correction
+        set_axis_never_homed(Z_AXIS);
+        // Home Z after the alignment procedure
+        process_subcommands_now_P(PSTR("G28Z"));
+      #else
+        // Use the probed height from the last iteration to determine the Z height.
+        // z_measured_min is used, because all steppers are aligned to z_measured_min.
+        // Ideally, this would be equal to the 'z_probe * 0.5f' which was added earlier.
+        current_position.z -= z_measured_min - (float)Z_CLEARANCE_BETWEEN_PROBES;
+        sync_plan_position();
+      #endif
+
+      // Restore the active tool after homing
+      TERN_(HAS_MULTI_HOTEND, tool_change(old_tool_index, DISABLED(PARKING_EXTRUDER))); // Fetch previous tool for parking extruder
+
+      #if BOTH(HAS_LEVELING, RESTORE_LEVELING_AFTER_G34)
+        set_bed_leveling_enabled(leveling_was_active);
+      #endif
+
+    }while(0);
+  #endif
+}
+
+#endif // Z_MULTI_ENDSTOPS || Z_STEPPER_AUTO_ALIGN
+
+#if ENABLED(Z_STEPPER_AUTO_ALIGN)
+
+/**
+ * M422: Set a Z-Stepper automatic alignment XY point.
+ *       Use repeatedly to set multiple points.
+ *
+ *   S<index> : Index of the probe point to set
+ *
+ * With Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS:
+ *   W<index> : Index of the Z stepper position to set
+ *              The W and S parameters may not be combined.
+ *
+ * S and W require an X and/or Y parameter
+ *   X<pos>   : X position to set (Unchanged if omitted)
+ *   Y<pos>   : Y position to set (Unchanged if omitted)
+ *
+ * R : Recalculate points based on current probe offsets
+ */
+void GcodeSuite::M422() {
+
+  if (parser.seen('R')) {
+    z_stepper_align.reset_to_default();
+    return;
+  }
+
+  if (!parser.seen_any()) {
+    LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS)
+      SERIAL_ECHOLNPAIR_P(PSTR("M422 S"), int(i + 1), SP_X_STR, z_stepper_align.xy[i].x, SP_Y_STR, z_stepper_align.xy[i].y);
+    #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+      LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS)
+        SERIAL_ECHOLNPAIR_P(PSTR("M422 W"), int(i + 1), SP_X_STR, z_stepper_align.stepper_xy[i].x, SP_Y_STR, z_stepper_align.stepper_xy[i].y);
+    #endif
+    return;
+  }
+
+  const bool is_probe_point = parser.seen('S');
+
+  if (TERN0(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS, is_probe_point && parser.seen('W'))) {
+    SERIAL_ECHOLNPGM("?(S) and (W) may not be combined.");
+    return;
+  }
+
+  xy_pos_t *pos_dest = (
+    TERN_(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS, !is_probe_point ? z_stepper_align.stepper_xy :)
+    z_stepper_align.xy
+  );
+
+  if (!is_probe_point && TERN1(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS, !parser.seen('W'))) {
+    SERIAL_ECHOLNPGM("?(S)" TERN_(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS, " or (W)") " is required.");
+    return;
+  }
+
+  // Get the Probe Position Index or Z Stepper Index
+  int8_t position_index;
+  if (is_probe_point) {
+    position_index = parser.intval('S') - 1;
+    if (!WITHIN(position_index, 0, int8_t(NUM_Z_STEPPER_DRIVERS) - 1)) {
+      SERIAL_ECHOLNPGM("?(S) Probe-position index invalid.");
+      return;
+    }
+  }
+  else {
+    #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+      position_index = parser.intval('W') - 1;
+      if (!WITHIN(position_index, 0, NUM_Z_STEPPER_DRIVERS - 1)) {
+        SERIAL_ECHOLNPGM("?(W) Z-stepper index invalid.");
+        return;
+      }
+    #endif
+  }
+
+  const xy_pos_t pos = {
+    parser.floatval('X', pos_dest[position_index].x),
+    parser.floatval('Y', pos_dest[position_index].y)
+  };
+
+  if (is_probe_point) {
+    if (!probe.can_reach(pos.x, Y_CENTER)) {
+      SERIAL_ECHOLNPGM("?(X) out of bounds.");
+      return;
+    }
+    if (!probe.can_reach(pos)) {
+      SERIAL_ECHOLNPGM("?(Y) out of bounds.");
+      return;
+    }
+  }
+
+  pos_dest[position_index] = pos;
+}
+
+#endif // Z_STEPPER_AUTO_ALIGN
diff --git a/Marlin/src/gcode/calibrate/G425.cpp b/Marlin/src/gcode/calibrate/G425.cpp
new file mode 100644
index 0000000..9510da7
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/G425.cpp
@@ -0,0 +1,623 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../MarlinCore.h"
+
+#if ENABLED(CALIBRATION_GCODE)
+
+#include "../gcode.h"
+
+#if ENABLED(BACKLASH_GCODE)
+  #include "../../feature/backlash.h"
+#endif
+
+#include "../../lcd/marlinui.h"
+#include "../../module/motion.h"
+#include "../../module/planner.h"
+#include "../../module/tool_change.h"
+#include "../../module/endstops.h"
+#include "../../feature/bedlevel/bedlevel.h"
+
+#if !AXIS_CAN_CALIBRATE(X)
+  #undef CALIBRATION_MEASURE_LEFT
+  #undef CALIBRATION_MEASURE_RIGHT
+#endif
+
+#if !AXIS_CAN_CALIBRATE(Y)
+  #undef CALIBRATION_MEASURE_FRONT
+  #undef CALIBRATION_MEASURE_BACK
+#endif
+
+#if !AXIS_CAN_CALIBRATE(Z)
+  #undef CALIBRATION_MEASURE_AT_TOP_EDGES
+#endif
+
+/**
+ * G425 backs away from the calibration object by various distances
+ * depending on the confidence level:
+ *
+ *   UNKNOWN   - No real notion on where the calibration object is on the bed
+ *   UNCERTAIN - Measurement may be uncertain due to backlash
+ *   CERTAIN   - Measurement obtained with backlash compensation
+ */
+
+#ifndef CALIBRATION_MEASUREMENT_UNKNOWN
+  #define CALIBRATION_MEASUREMENT_UNKNOWN   5.0 // mm
+#endif
+#ifndef CALIBRATION_MEASUREMENT_UNCERTAIN
+  #define CALIBRATION_MEASUREMENT_UNCERTAIN 1.0 // mm
+#endif
+#ifndef CALIBRATION_MEASUREMENT_CERTAIN
+  #define CALIBRATION_MEASUREMENT_CERTAIN   0.5 // mm
+#endif
+
+#if BOTH(CALIBRATION_MEASURE_LEFT, CALIBRATION_MEASURE_RIGHT)
+  #define HAS_X_CENTER 1
+#endif
+#if BOTH(CALIBRATION_MEASURE_FRONT, CALIBRATION_MEASURE_BACK)
+  #define HAS_Y_CENTER 1
+#endif
+
+enum side_t : uint8_t { TOP, RIGHT, FRONT, LEFT, BACK, NUM_SIDES };
+
+static constexpr xyz_pos_t true_center CALIBRATION_OBJECT_CENTER;
+static constexpr xyz_float_t dimensions CALIBRATION_OBJECT_DIMENSIONS;
+static constexpr xy_float_t nod = { CALIBRATION_NOZZLE_OUTER_DIAMETER, CALIBRATION_NOZZLE_OUTER_DIAMETER };
+
+struct measurements_t {
+  xyz_pos_t obj_center = true_center; // Non-static must be assigned from xyz_pos_t
+
+  float obj_side[NUM_SIDES], backlash[NUM_SIDES];
+  xyz_float_t pos_error;
+
+  xy_float_t nozzle_outer_dimension = nod;
+};
+
+#if ENABLED(BACKLASH_GCODE)
+  #define TEMPORARY_BACKLASH_CORRECTION(value) REMEMBER(tbst, backlash.correction, value)
+#else
+  #define TEMPORARY_BACKLASH_CORRECTION(value)
+#endif
+
+#if ENABLED(BACKLASH_GCODE) && defined(BACKLASH_SMOOTHING_MM)
+  #define TEMPORARY_BACKLASH_SMOOTHING(value) REMEMBER(tbsm, backlash.smoothing_mm, value)
+#else
+  #define TEMPORARY_BACKLASH_SMOOTHING(value)
+#endif
+
+inline void calibration_move() {
+  do_blocking_move_to(current_position, MMM_TO_MMS(CALIBRATION_FEEDRATE_TRAVEL));
+}
+
+/**
+ * Move to the exact center above the calibration object
+ *
+ *   m                  in     - Measurement record
+ *   uncertainty        in     - How far away from the object top to park
+ */
+inline void park_above_object(measurements_t &m, const float uncertainty) {
+  // Move to safe distance above calibration object
+  current_position.z = m.obj_center.z + dimensions.z / 2 + uncertainty;
+  calibration_move();
+
+  // Move to center of calibration object in XY
+  current_position = xy_pos_t(m.obj_center);
+  calibration_move();
+}
+
+#if HAS_MULTI_HOTEND
+  inline void set_nozzle(measurements_t &m, const uint8_t extruder) {
+    if (extruder != active_extruder) {
+      park_above_object(m, CALIBRATION_MEASUREMENT_UNKNOWN);
+      tool_change(extruder);
+    }
+  }
+#endif
+
+#if HAS_HOTEND_OFFSET
+
+  inline void normalize_hotend_offsets() {
+    LOOP_S_L_N(e, 1, HOTENDS)
+      hotend_offset[e] -= hotend_offset[0];
+    hotend_offset[0].reset();
+  }
+
+#endif
+
+#if !PIN_EXISTS(CALIBRATION)
+  #include "../../module/probe.h"
+#endif
+
+inline bool read_calibration_pin() {
+  return (
+    #if PIN_EXISTS(CALIBRATION)
+      READ(CALIBRATION_PIN) != CALIBRATION_PIN_INVERTING
+    #else
+      PROBE_TRIGGERED()
+    #endif
+  );
+}
+
+/**
+ * Move along axis in the specified dir until the probe value becomes stop_state,
+ * then return the axis value.
+ *
+ *   axis         in - Axis along which the measurement will take place
+ *   dir          in - Direction along that axis (-1 or 1)
+ *   stop_state   in - Move until probe pin becomes this value
+ *   fast         in - Fast vs. precise measurement
+ */
+float measuring_movement(const AxisEnum axis, const int dir, const bool stop_state, const bool fast) {
+  const float step     = fast ? 0.25 : CALIBRATION_MEASUREMENT_RESOLUTION;
+  const feedRate_t mms = fast ? MMM_TO_MMS(CALIBRATION_FEEDRATE_FAST) : MMM_TO_MMS(CALIBRATION_FEEDRATE_SLOW);
+  const float limit    = fast ? 50 : 5;
+
+  destination = current_position;
+  for (float travel = 0; travel < limit; travel += step) {
+    destination[axis] += dir * step;
+    do_blocking_move_to(destination, mms);
+    planner.synchronize();
+    if (read_calibration_pin() == stop_state) break;
+  }
+  return destination[axis];
+}
+
+/**
+ * Move along axis until the probe is triggered. Move toolhead to its starting
+ * point and return the measured value.
+ *
+ *   axis               in     - Axis along which the measurement will take place
+ *   dir                in     - Direction along that axis (-1 or 1)
+ *   stop_state         in     - Move until probe pin becomes this value
+ *   backlash_ptr       in/out - When not nullptr, measure and record axis backlash
+ *   uncertainty        in     - If uncertainty is CALIBRATION_MEASUREMENT_UNKNOWN, do a fast probe.
+ */
+inline float measure(const AxisEnum axis, const int dir, const bool stop_state, float * const backlash_ptr, const float uncertainty) {
+  const bool fast = uncertainty == CALIBRATION_MEASUREMENT_UNKNOWN;
+
+  // Save position
+  destination = current_position;
+  const float start_pos = destination[axis];
+  const float measured_pos = measuring_movement(axis, dir, stop_state, fast);
+  // Measure backlash
+  if (backlash_ptr && !fast) {
+    const float release_pos = measuring_movement(axis, -dir, !stop_state, fast);
+    *backlash_ptr = ABS(release_pos - measured_pos);
+  }
+  // Return to starting position
+  destination[axis] = start_pos;
+  do_blocking_move_to(destination, MMM_TO_MMS(CALIBRATION_FEEDRATE_TRAVEL));
+  return measured_pos;
+}
+
+/**
+ * Probe one side of the calibration object
+ *
+ *   m                  in/out - Measurement record, m.obj_center and m.obj_side will be updated.
+ *   uncertainty        in     - How far away from the calibration object to begin probing
+ *   side               in     - Side of probe where probe will occur
+ *   probe_top_at_edge  in     - When probing sides, probe top of calibration object nearest edge
+ *                               to find out height of edge
+ */
+inline void probe_side(measurements_t &m, const float uncertainty, const side_t side, const bool probe_top_at_edge=false) {
+  const xyz_float_t dimensions = CALIBRATION_OBJECT_DIMENSIONS;
+  AxisEnum axis;
+  float dir = 1;
+
+  park_above_object(m, uncertainty);
+
+  switch (side) {
+    #if AXIS_CAN_CALIBRATE(Z)
+      case TOP: {
+        const float measurement = measure(Z_AXIS, -1, true, &m.backlash[TOP], uncertainty);
+        m.obj_center.z = measurement - dimensions.z / 2;
+        m.obj_side[TOP] = measurement;
+        return;
+      }
+    #endif
+    #if AXIS_CAN_CALIBRATE(X)
+      case LEFT:  axis = X_AXIS; break;
+      case RIGHT: axis = X_AXIS; dir = -1; break;
+    #endif
+    #if AXIS_CAN_CALIBRATE(Y)
+      case FRONT: axis = Y_AXIS; break;
+      case BACK:  axis = Y_AXIS; dir = -1; break;
+    #endif
+    default: return;
+  }
+
+  if (probe_top_at_edge) {
+    #if AXIS_CAN_CALIBRATE(Z)
+      // Probe top nearest the side we are probing
+      current_position[axis] = m.obj_center[axis] + (-dir) * (dimensions[axis] / 2 - m.nozzle_outer_dimension[axis]);
+      calibration_move();
+      m.obj_side[TOP] = measure(Z_AXIS, -1, true, &m.backlash[TOP], uncertainty);
+      m.obj_center.z = m.obj_side[TOP] - dimensions.z / 2;
+    #endif
+  }
+
+  if ((AXIS_CAN_CALIBRATE(X) && axis == X_AXIS) || (AXIS_CAN_CALIBRATE(Y) && axis == Y_AXIS)) {
+    // Move to safe distance to the side of the calibration object
+    current_position[axis] = m.obj_center[axis] + (-dir) * (dimensions[axis] / 2 + m.nozzle_outer_dimension[axis] / 2 + uncertainty);
+    calibration_move();
+
+    // Plunge below the side of the calibration object and measure
+    current_position.z = m.obj_side[TOP] - (CALIBRATION_NOZZLE_TIP_HEIGHT) * 0.7f;
+    calibration_move();
+    const float measurement = measure(axis, dir, true, &m.backlash[side], uncertainty);
+    m.obj_center[axis] = measurement + dir * (dimensions[axis] / 2 + m.nozzle_outer_dimension[axis] / 2);
+    m.obj_side[side] = measurement;
+  }
+}
+
+/**
+ * Probe all sides of the calibration calibration object
+ *
+ *   m                  in/out - Measurement record: center, backlash and error values be updated.
+ *   uncertainty        in     - How far away from the calibration object to begin probing
+ */
+inline void probe_sides(measurements_t &m, const float uncertainty) {
+  #if ENABLED(CALIBRATION_MEASURE_AT_TOP_EDGES)
+    constexpr bool probe_top_at_edge = true;
+  #else
+    // Probing at the exact center only works if the center is flat. Probing on a washer
+    // or bolt will require probing the top near the side edges, away from the center.
+    constexpr bool probe_top_at_edge = false;
+    probe_side(m, uncertainty, TOP);
+  #endif
+
+  TERN_(CALIBRATION_MEASURE_RIGHT, probe_side(m, uncertainty, RIGHT, probe_top_at_edge));
+  TERN_(CALIBRATION_MEASURE_FRONT, probe_side(m, uncertainty, FRONT, probe_top_at_edge));
+  TERN_(CALIBRATION_MEASURE_LEFT,  probe_side(m, uncertainty, LEFT,  probe_top_at_edge));
+  TERN_(CALIBRATION_MEASURE_BACK,  probe_side(m, uncertainty, BACK,  probe_top_at_edge));
+
+  // Compute the measured center of the calibration object.
+  TERN_(HAS_X_CENTER, m.obj_center.x = (m.obj_side[LEFT] + m.obj_side[RIGHT]) / 2);
+  TERN_(HAS_Y_CENTER, m.obj_center.y = (m.obj_side[FRONT] + m.obj_side[BACK]) / 2);
+
+  // Compute the outside diameter of the nozzle at the height
+  // at which it makes contact with the calibration object
+  TERN_(HAS_X_CENTER, m.nozzle_outer_dimension.x = m.obj_side[RIGHT] - m.obj_side[LEFT] - dimensions.x);
+  TERN_(HAS_Y_CENTER, m.nozzle_outer_dimension.y = m.obj_side[BACK]  - m.obj_side[FRONT] - dimensions.y);
+
+  park_above_object(m, uncertainty);
+
+  // The difference between the known and the measured location
+  // of the calibration object is the positional error
+  m.pos_error.x = (0
+    #if HAS_X_CENTER
+      + true_center.x - m.obj_center.x
+    #endif
+  );
+  m.pos_error.y = (0
+    #if HAS_Y_CENTER
+      + true_center.y - m.obj_center.y
+    #endif
+  );
+  m.pos_error.z = true_center.z - m.obj_center.z;
+}
+
+#if ENABLED(CALIBRATION_REPORTING)
+  inline void report_measured_faces(const measurements_t &m) {
+    SERIAL_ECHOLNPGM("Sides:");
+    #if AXIS_CAN_CALIBRATE(Z)
+      SERIAL_ECHOLNPAIR("  Top: ", m.obj_side[TOP]);
+    #endif
+    #if ENABLED(CALIBRATION_MEASURE_LEFT)
+      SERIAL_ECHOLNPAIR("  Left: ", m.obj_side[LEFT]);
+    #endif
+    #if ENABLED(CALIBRATION_MEASURE_RIGHT)
+      SERIAL_ECHOLNPAIR("  Right: ", m.obj_side[RIGHT]);
+    #endif
+    #if ENABLED(CALIBRATION_MEASURE_FRONT)
+      SERIAL_ECHOLNPAIR("  Front: ", m.obj_side[FRONT]);
+    #endif
+    #if ENABLED(CALIBRATION_MEASURE_BACK)
+      SERIAL_ECHOLNPAIR("  Back: ", m.obj_side[BACK]);
+    #endif
+    SERIAL_EOL();
+  }
+
+  inline void report_measured_center(const measurements_t &m) {
+    SERIAL_ECHOLNPGM("Center:");
+    #if HAS_X_CENTER
+      SERIAL_ECHOLNPAIR_P(SP_X_STR, m.obj_center.x);
+    #endif
+    #if HAS_Y_CENTER
+      SERIAL_ECHOLNPAIR_P(SP_Y_STR, m.obj_center.y);
+    #endif
+    SERIAL_ECHOLNPAIR_P(SP_Z_STR, m.obj_center.z);
+    SERIAL_EOL();
+  }
+
+  inline void report_measured_backlash(const measurements_t &m) {
+    SERIAL_ECHOLNPGM("Backlash:");
+    #if AXIS_CAN_CALIBRATE(X)
+      #if ENABLED(CALIBRATION_MEASURE_LEFT)
+        SERIAL_ECHOLNPAIR("  Left: ", m.backlash[LEFT]);
+      #endif
+      #if ENABLED(CALIBRATION_MEASURE_RIGHT)
+        SERIAL_ECHOLNPAIR("  Right: ", m.backlash[RIGHT]);
+      #endif
+    #endif
+    #if AXIS_CAN_CALIBRATE(Y)
+      #if ENABLED(CALIBRATION_MEASURE_FRONT)
+        SERIAL_ECHOLNPAIR("  Front: ", m.backlash[FRONT]);
+      #endif
+      #if ENABLED(CALIBRATION_MEASURE_BACK)
+        SERIAL_ECHOLNPAIR("  Back: ", m.backlash[BACK]);
+      #endif
+    #endif
+    #if AXIS_CAN_CALIBRATE(Z)
+      SERIAL_ECHOLNPAIR("  Top: ", m.backlash[TOP]);
+    #endif
+    SERIAL_EOL();
+  }
+
+  inline void report_measured_positional_error(const measurements_t &m) {
+    SERIAL_CHAR('T');
+    SERIAL_ECHO(int(active_extruder));
+    SERIAL_ECHOLNPGM(" Positional Error:");
+    #if HAS_X_CENTER
+      SERIAL_ECHOLNPAIR_P(SP_X_STR, m.pos_error.x);
+    #endif
+    #if HAS_Y_CENTER
+      SERIAL_ECHOLNPAIR_P(SP_Y_STR, m.pos_error.y);
+    #endif
+    if (AXIS_CAN_CALIBRATE(Z)) SERIAL_ECHOLNPAIR_P(SP_Z_STR, m.pos_error.z);
+    SERIAL_EOL();
+  }
+
+  inline void report_measured_nozzle_dimensions(const measurements_t &m) {
+    SERIAL_ECHOLNPGM("Nozzle Tip Outer Dimensions:");
+    #if HAS_X_CENTER || HAS_Y_CENTER
+      #if HAS_X_CENTER
+        SERIAL_ECHOLNPAIR_P(SP_X_STR, m.nozzle_outer_dimension.x);
+      #endif
+      #if HAS_Y_CENTER
+        SERIAL_ECHOLNPAIR_P(SP_Y_STR, m.nozzle_outer_dimension.y);
+      #endif
+    #else
+      UNUSED(m);
+    #endif
+    SERIAL_EOL();
+  }
+
+  #if HAS_HOTEND_OFFSET
+    //
+    // This function requires normalize_hotend_offsets() to be called
+    //
+    inline void report_hotend_offsets() {
+      LOOP_S_L_N(e, 1, HOTENDS)
+        SERIAL_ECHOLNPAIR_P(PSTR("T"), int(e), PSTR(" Hotend Offset X"), hotend_offset[e].x, SP_Y_STR, hotend_offset[e].y, SP_Z_STR, hotend_offset[e].z);
+    }
+  #endif
+
+#endif // CALIBRATION_REPORTING
+
+/**
+ * Probe around the calibration object to measure backlash
+ *
+ *   m              in/out - Measurement record, updated with new readings
+ *   uncertainty    in     - How far away from the object to begin probing
+ */
+inline void calibrate_backlash(measurements_t &m, const float uncertainty) {
+  // Backlash compensation should be off while measuring backlash
+
+  {
+    // New scope for TEMPORARY_BACKLASH_CORRECTION
+    TEMPORARY_BACKLASH_CORRECTION(all_off);
+    TEMPORARY_BACKLASH_SMOOTHING(0.0f);
+
+    probe_sides(m, uncertainty);
+
+    #if ENABLED(BACKLASH_GCODE)
+
+      #if HAS_X_CENTER
+        backlash.distance_mm.x = (m.backlash[LEFT] + m.backlash[RIGHT]) / 2;
+      #elif ENABLED(CALIBRATION_MEASURE_LEFT)
+        backlash.distance_mm.x = m.backlash[LEFT];
+      #elif ENABLED(CALIBRATION_MEASURE_RIGHT)
+        backlash.distance_mm.x = m.backlash[RIGHT];
+      #endif
+
+      #if HAS_Y_CENTER
+        backlash.distance_mm.y = (m.backlash[FRONT] + m.backlash[BACK]) / 2;
+      #elif ENABLED(CALIBRATION_MEASURE_FRONT)
+        backlash.distance_mm.y = m.backlash[FRONT];
+      #elif ENABLED(CALIBRATION_MEASURE_BACK)
+        backlash.distance_mm.y = m.backlash[BACK];
+      #endif
+
+      if (AXIS_CAN_CALIBRATE(Z)) backlash.distance_mm.z = m.backlash[TOP];
+    #endif
+  }
+
+  #if ENABLED(BACKLASH_GCODE)
+    // Turn on backlash compensation and move in all
+    // allowed directions to take up any backlash
+    {
+      // New scope for TEMPORARY_BACKLASH_CORRECTION
+      TEMPORARY_BACKLASH_CORRECTION(all_on);
+      TEMPORARY_BACKLASH_SMOOTHING(0.0f);
+      const xyz_float_t move = { AXIS_CAN_CALIBRATE(X) * 3, AXIS_CAN_CALIBRATE(Y) * 3, AXIS_CAN_CALIBRATE(Z) * 3 };
+      current_position += move; calibration_move();
+      current_position -= move; calibration_move();
+    }
+  #endif
+}
+
+inline void update_measurements(measurements_t &m, const AxisEnum axis) {
+  current_position[axis] += m.pos_error[axis];
+  m.obj_center[axis] = true_center[axis];
+  m.pos_error[axis] = 0;
+}
+
+/**
+ * Probe around the calibration object. Adjust the position and toolhead offset
+ * using the deviation from the known position of the calibration object.
+ *
+ *   m              in/out - Measurement record, updated with new readings
+ *   uncertainty    in     - How far away from the object to begin probing
+ *   extruder       in     - What extruder to probe
+ *
+ * Prerequisites:
+ *    - Call calibrate_backlash() beforehand for best accuracy
+ */
+inline void calibrate_toolhead(measurements_t &m, const float uncertainty, const uint8_t extruder) {
+  TEMPORARY_BACKLASH_CORRECTION(all_on);
+  TEMPORARY_BACKLASH_SMOOTHING(0.0f);
+
+  #if HAS_MULTI_HOTEND
+    set_nozzle(m, extruder);
+  #else
+    UNUSED(extruder);
+  #endif
+
+  probe_sides(m, uncertainty);
+
+  // Adjust the hotend offset
+  #if HAS_HOTEND_OFFSET
+    if (ENABLED(HAS_X_CENTER) && AXIS_CAN_CALIBRATE(X)) hotend_offset[extruder].x += m.pos_error.x;
+    if (ENABLED(HAS_Y_CENTER) && AXIS_CAN_CALIBRATE(Y)) hotend_offset[extruder].y += m.pos_error.y;
+                             if (AXIS_CAN_CALIBRATE(Z)) hotend_offset[extruder].z += m.pos_error.z;
+    normalize_hotend_offsets();
+  #endif
+
+  // Correct for positional error, so the object
+  // is at the known actual spot
+  planner.synchronize();
+  if (ENABLED(HAS_X_CENTER) && AXIS_CAN_CALIBRATE(X)) update_measurements(m, X_AXIS);
+  if (ENABLED(HAS_Y_CENTER) && AXIS_CAN_CALIBRATE(Y)) update_measurements(m, Y_AXIS);
+                           if (AXIS_CAN_CALIBRATE(Z)) update_measurements(m, Z_AXIS);
+
+  sync_plan_position();
+}
+
+/**
+ * Probe around the calibration object for all toolheads, adjusting the coordinate
+ * system for the first nozzle and the nozzle offset for subsequent nozzles.
+ *
+ *   m              in/out - Measurement record, updated with new readings
+ *   uncertainty    in     - How far away from the object to begin probing
+ */
+inline void calibrate_all_toolheads(measurements_t &m, const float uncertainty) {
+  TEMPORARY_BACKLASH_CORRECTION(all_on);
+  TEMPORARY_BACKLASH_SMOOTHING(0.0f);
+
+  HOTEND_LOOP() calibrate_toolhead(m, uncertainty, e);
+
+  TERN_(HAS_HOTEND_OFFSET, normalize_hotend_offsets());
+
+  TERN_(HAS_MULTI_HOTEND, set_nozzle(m, 0));
+}
+
+/**
+ * Perform a full auto-calibration routine:
+ *
+ *   1) For each nozzle, touch top and sides of object to determine object position and
+ *      nozzle offsets. Do a fast but rough search over a wider area.
+ *   2) With the first nozzle, touch top and sides of object to determine backlash values
+ *      for all axis (if BACKLASH_GCODE is enabled)
+ *   3) For each nozzle, touch top and sides of object slowly to determine precise
+ *      position of object. Adjust coordinate system and nozzle offsets so probed object
+ *      location corresponds to known object location with a high degree of precision.
+ */
+inline void calibrate_all() {
+  measurements_t m;
+
+  TERN_(HAS_HOTEND_OFFSET, reset_hotend_offsets());
+
+  TEMPORARY_BACKLASH_CORRECTION(all_on);
+  TEMPORARY_BACKLASH_SMOOTHING(0.0f);
+
+  // Do a fast and rough calibration of the toolheads
+  calibrate_all_toolheads(m, CALIBRATION_MEASUREMENT_UNKNOWN);
+
+  TERN_(BACKLASH_GCODE, calibrate_backlash(m, CALIBRATION_MEASUREMENT_UNCERTAIN));
+
+  // Cycle the toolheads so the servos settle into their "natural" positions
+  #if HAS_MULTI_HOTEND
+    HOTEND_LOOP() set_nozzle(m, e);
+  #endif
+
+  // Do a slow and precise calibration of the toolheads
+  calibrate_all_toolheads(m, CALIBRATION_MEASUREMENT_UNCERTAIN);
+
+  current_position.x = X_CENTER;
+  calibration_move();         // Park nozzle away from calibration object
+}
+
+/**
+ * G425: Perform calibration with calibration object.
+ *
+ *   B           - Perform calibration of backlash only.
+ *   T<extruder> - Perform calibration of toolhead only.
+ *   V           - Probe object and print position, error, backlash and hotend offset.
+ *   U           - Uncertainty, how far to start probe away from the object (mm)
+ *
+ *   no args     - Perform entire calibration sequence (backlash + position on all toolheads)
+ */
+void GcodeSuite::G425() {
+
+  #ifdef CALIBRATION_SCRIPT_PRE
+    GcodeSuite::process_subcommands_now_P(PSTR(CALIBRATION_SCRIPT_PRE));
+  #endif
+
+  if (homing_needed_error()) return;
+
+  TEMPORARY_BED_LEVELING_STATE(false);
+  SET_SOFT_ENDSTOP_LOOSE(true);
+
+  measurements_t m;
+  float uncertainty = parser.seenval('U') ? parser.value_float() : CALIBRATION_MEASUREMENT_UNCERTAIN;
+
+  if (parser.seen('B'))
+    calibrate_backlash(m, uncertainty);
+  else if (parser.seen('T'))
+    calibrate_toolhead(m, uncertainty, parser.has_value() ? parser.value_int() : active_extruder);
+  #if ENABLED(CALIBRATION_REPORTING)
+    else if (parser.seen('V')) {
+      probe_sides(m, uncertainty);
+      SERIAL_EOL();
+      report_measured_faces(m);
+      report_measured_center(m);
+      report_measured_backlash(m);
+      report_measured_nozzle_dimensions(m);
+      report_measured_positional_error(m);
+      #if HAS_HOTEND_OFFSET
+        normalize_hotend_offsets();
+        report_hotend_offsets();
+      #endif
+    }
+  #endif
+  else
+    calibrate_all();
+
+  SET_SOFT_ENDSTOP_LOOSE(false);
+
+  #ifdef CALIBRATION_SCRIPT_POST
+    GcodeSuite::process_subcommands_now_P(PSTR(CALIBRATION_SCRIPT_POST));
+  #endif
+}
+
+#endif // CALIBRATION_GCODE
diff --git a/Marlin/src/gcode/calibrate/G76_M192_M871.cpp b/Marlin/src/gcode/calibrate/G76_M192_M871.cpp
new file mode 100644
index 0000000..5d0bb0d
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/G76_M192_M871.cpp
@@ -0,0 +1,369 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * G76_M871.cpp - Temperature calibration/compensation for z-probing
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(PROBE_TEMP_COMPENSATION)
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/planner.h"
+#include "../../module/probe.h"
+#include "../../feature/bedlevel/bedlevel.h"
+#include "../../module/temperature.h"
+#include "../../module/probe.h"
+#include "../../feature/probe_temp_comp.h"
+
+#include "../../lcd/marlinui.h"
+#include "../../MarlinCore.h" // for wait_for_heatup, idle()
+
+#if ENABLED(PRINTJOB_TIMER_AUTOSTART)
+  #include "../../module/printcounter.h"
+#endif
+
+#if ENABLED(PRINTER_EVENTS_LEDS)
+  #include "../../feature/leds/leds.h"
+#endif
+
+/**
+ * G76: calibrate probe and/or bed temperature offsets
+ *  Notes:
+ *  - When calibrating probe, bed temperature is held constant.
+ *    Compensation values are deltas to first probe measurement at probe temp. = 30°C.
+ *  - When calibrating bed, probe temperature is held constant.
+ *    Compensation values are deltas to first probe measurement at bed temp. = 60°C.
+ *  - The hotend will not be heated at any time.
+ *  - On my Průša MK3S clone I put a piece of paper between the probe and the hotend
+ *    so the hotend fan would not cool my probe constantly. Alternativly you could just
+ *    make sure the fan is not running while running the calibration process.
+ *
+ *  Probe calibration:
+ *  - Moves probe to cooldown point.
+ *  - Heats up bed to 100°C.
+ *  - Moves probe to probing point (1mm above heatbed).
+ *  - Waits until probe reaches target temperature (30°C).
+ *  - Does a z-probing (=base value) and increases target temperature by 5°C.
+ *  - Waits until probe reaches increased target temperature.
+ *  - Does a z-probing (delta to base value will be a compensation value) and increases target temperature by 5°C.
+ *  - Repeats last two steps until max. temperature reached or timeout (i.e. probe does not heat up any further).
+ *  - Compensation values of higher temperatures will be extrapolated (using linear regression first).
+ *    While this is not exact by any means it is still better than simply using the last compensation value.
+ *
+ *  Bed calibration:
+ *  - Moves probe to cooldown point.
+ *  - Heats up bed to 60°C.
+ *  - Moves probe to probing point (1mm above heatbed).
+ *  - Waits until probe reaches target temperature (30°C).
+ *  - Does a z-probing (=base value) and increases bed temperature by 5°C.
+ *  - Moves probe to cooldown point.
+ *  - Waits until probe is below 30°C and bed has reached target temperature.
+ *  - Moves probe to probing point and waits until it reaches target temperature (30°C).
+ *  - Does a z-probing (delta to base value will be a compensation value) and increases bed temperature by 5°C.
+ *  - Repeats last four points until max. bed temperature reached (110°C) or timeout.
+ *  - Compensation values of higher temperatures will be extrapolated (using linear regression first).
+ *    While this is not exact by any means it is still better than simply using the last compensation value.
+ *
+ *  G76 [B | P]
+ *  - no flag - Both calibration procedures will be run.
+ *  - `B` - Run bed temperature calibration.
+ *  - `P` - Run probe temperature calibration.
+ */
+
+static void say_waiting_for()               { SERIAL_ECHOPGM("Waiting for "); }
+static void say_waiting_for_probe_heating() { say_waiting_for(); SERIAL_ECHOLNPGM("probe heating."); }
+static void say_successfully_calibrated()   { SERIAL_ECHOPGM("Successfully calibrated"); }
+static void say_failed_to_calibrate()       { SERIAL_ECHOPGM("!Failed to calibrate"); }
+
+void GcodeSuite::G76() {
+  // Check if heated bed is available and z-homing is done with probe
+  #if TEMP_SENSOR_BED == 0 || !(HOMING_Z_WITH_PROBE)
+    return;
+  #endif
+
+  auto report_temps = [](millis_t &ntr, millis_t timeout=0) {
+    idle_no_sleep();
+    const millis_t ms = millis();
+    if (ELAPSED(ms, ntr)) {
+      ntr = ms + 1000;
+      thermalManager.print_heater_states(active_extruder);
+    }
+    return (timeout && ELAPSED(ms, timeout));
+  };
+
+  auto wait_for_temps = [&](const float tb, const float tp, millis_t &ntr, const millis_t timeout=0) {
+    say_waiting_for(); SERIAL_ECHOLNPGM("bed and probe temperature.");
+    while (fabs(thermalManager.degBed() - tb) > 0.1f || thermalManager.degProbe() > tp)
+      if (report_temps(ntr, timeout)) return true;
+    return false;
+  };
+
+  auto g76_probe = [](const TempSensorID sid, uint16_t &targ, const xy_pos_t &nozpos) {
+    do_z_clearance(5.0); // Raise nozzle before probing
+    const float measured_z = probe.probe_at_point(nozpos, PROBE_PT_STOW, 0, false);  // verbose=0, probe_relative=false
+    if (isnan(measured_z))
+      SERIAL_ECHOLNPGM("!Received NAN. Aborting.");
+    else {
+      SERIAL_ECHOLNPAIR_F("Measured: ", measured_z);
+      if (targ == cali_info_init[sid].start_temp)
+        temp_comp.prepare_new_calibration(measured_z);
+      else
+        temp_comp.push_back_new_measurement(sid, measured_z);
+      targ += cali_info_init[sid].temp_res;
+    }
+    return measured_z;
+  };
+
+  #if ENABLED(BLTOUCH)
+    // Make sure any BLTouch error condition is cleared
+    bltouch_command(BLTOUCH_RESET, BLTOUCH_RESET_DELAY);
+    set_bltouch_deployed(false);
+  #endif
+
+  bool do_bed_cal = parser.boolval('B'), do_probe_cal = parser.boolval('P');
+  if (!do_bed_cal && !do_probe_cal) do_bed_cal = do_probe_cal = true;
+
+  // Synchronize with planner
+  planner.synchronize();
+
+  const xyz_pos_t parkpos = temp_comp.park_point,
+            probe_pos_xyz = xyz_pos_t(temp_comp.measure_point) + xyz_pos_t({ 0.0f, 0.0f, PTC_PROBE_HEATING_OFFSET }),
+              noz_pos_xyz = probe_pos_xyz - probe.offset_xy;  // Nozzle position based on probe position
+
+  if (do_bed_cal || do_probe_cal) {
+    // Ensure park position is reachable
+    bool reachable = position_is_reachable(parkpos) || WITHIN(parkpos.z, Z_MIN_POS - fslop, Z_MAX_POS + fslop);
+    if (!reachable)
+      SERIAL_ECHOLNPGM("!Park");
+    else {
+      // Ensure probe position is reachable
+      reachable = probe.can_reach(probe_pos_xyz);
+      if (!reachable) SERIAL_ECHOLNPGM("!Probe");
+    }
+
+    if (!reachable) {
+      SERIAL_ECHOLNPGM(" position unreachable - aborting.");
+      return;
+    }
+
+    process_subcommands_now_P(G28_STR);
+  }
+
+  remember_feedrate_scaling_off();
+
+
+  /******************************************
+   * Calibrate bed temperature offsets
+   ******************************************/
+
+  // Report temperatures every second and handle heating timeouts
+  millis_t next_temp_report = millis() + 1000;
+
+  auto report_targets = [&](const uint16_t tb, const uint16_t tp) {
+    SERIAL_ECHOLNPAIR("Target Bed:", tb, " Probe:", tp);
+  };
+
+  if (do_bed_cal) {
+
+    uint16_t target_bed = cali_info_init[TSI_BED].start_temp,
+             target_probe = temp_comp.bed_calib_probe_temp;
+
+    say_waiting_for(); SERIAL_ECHOLNPGM(" cooling.");
+    while (thermalManager.degBed() > target_bed || thermalManager.degProbe() > target_probe)
+      report_temps(next_temp_report);
+
+    // Disable leveling so it won't mess with us
+    TERN_(HAS_LEVELING, set_bed_leveling_enabled(false));
+
+    for (;;) {
+      thermalManager.setTargetBed(target_bed);
+
+      report_targets(target_bed, target_probe);
+
+      // Park nozzle
+      do_blocking_move_to(parkpos);
+
+      // Wait for heatbed to reach target temp and probe to cool below target temp
+      if (wait_for_temps(target_bed, target_probe, next_temp_report, millis() + MIN_TO_MS(15))) {
+        SERIAL_ECHOLNPGM("!Bed heating timeout.");
+        break;
+      }
+
+      // Move the nozzle to the probing point and wait for the probe to reach target temp
+      do_blocking_move_to(noz_pos_xyz);
+      say_waiting_for_probe_heating();
+      SERIAL_EOL();
+      while (thermalManager.degProbe() < target_probe)
+        report_temps(next_temp_report);
+
+      const float measured_z = g76_probe(TSI_BED, target_bed, noz_pos_xyz);
+      if (isnan(measured_z) || target_bed > BED_MAX_TARGET) break;
+    }
+
+    SERIAL_ECHOLNPAIR("Retrieved measurements: ", temp_comp.get_index());
+    if (temp_comp.finish_calibration(TSI_BED)) {
+      say_successfully_calibrated();
+      SERIAL_ECHOLNPGM(" bed.");
+    }
+    else {
+      say_failed_to_calibrate();
+      SERIAL_ECHOLNPGM(" bed. Values reset.");
+    }
+
+    // Cleanup
+    thermalManager.setTargetBed(0);
+    TERN_(HAS_LEVELING, set_bed_leveling_enabled(true));
+  } // do_bed_cal
+
+  /********************************************
+   * Calibrate probe temperature offsets
+   ********************************************/
+
+  if (do_probe_cal) {
+
+    // Park nozzle
+    do_blocking_move_to(parkpos);
+
+    // Initialize temperatures
+    const uint16_t target_bed = temp_comp.probe_calib_bed_temp;
+    thermalManager.setTargetBed(target_bed);
+
+    uint16_t target_probe = cali_info_init[TSI_PROBE].start_temp;
+
+    report_targets(target_bed, target_probe);
+
+    // Wait for heatbed to reach target temp and probe to cool below target temp
+    wait_for_temps(target_bed, target_probe, next_temp_report);
+
+    // Disable leveling so it won't mess with us
+    TERN_(HAS_LEVELING, set_bed_leveling_enabled(false));
+
+    bool timeout = false;
+    for (;;) {
+      // Move probe to probing point and wait for it to reach target temperature
+      do_blocking_move_to(noz_pos_xyz);
+
+      say_waiting_for_probe_heating();
+      SERIAL_ECHOLNPAIR(" Bed:", target_bed, " Probe:", target_probe);
+      const millis_t probe_timeout_ms = millis() + SEC_TO_MS(900UL);
+      while (thermalManager.degProbe() < target_probe) {
+        if (report_temps(next_temp_report, probe_timeout_ms)) {
+          SERIAL_ECHOLNPGM("!Probe heating timed out.");
+          timeout = true;
+          break;
+        }
+      }
+      if (timeout) break;
+
+      const float measured_z = g76_probe(TSI_PROBE, target_probe, noz_pos_xyz);
+      if (isnan(measured_z) || target_probe > cali_info_init[TSI_PROBE].end_temp) break;
+    }
+
+    SERIAL_ECHOLNPAIR("Retrieved measurements: ", temp_comp.get_index());
+    if (temp_comp.finish_calibration(TSI_PROBE))
+      say_successfully_calibrated();
+    else
+      say_failed_to_calibrate();
+    SERIAL_ECHOLNPGM(" probe.");
+
+    // Cleanup
+    thermalManager.setTargetBed(0);
+    TERN_(HAS_LEVELING, set_bed_leveling_enabled(true));
+
+    SERIAL_ECHOLNPGM("Final compensation values:");
+    temp_comp.print_offsets();
+  } // do_probe_cal
+
+  restore_feedrate_and_scaling();
+}
+
+/**
+ * M871: Report / reset temperature compensation offsets.
+ *       Note: This does not affect values in EEPROM until M500.
+ *
+ *   M871 [ R | B | P | E ]
+ *
+ *    No Parameters - Print current offset values.
+ *
+ * Select only one of these flags:
+ *    R - Reset all offsets to zero (i.e., disable compensation).
+ *    B - Manually set offset for bed
+ *    P - Manually set offset for probe
+ *    E - Manually set offset for extruder
+ *
+ * With B, P, or E:
+ *    I[index] - Index in the array
+ *    V[value] - Adjustment in µm
+ */
+void GcodeSuite::M871() {
+
+  if (parser.seen('R')) {
+    // Reset z-probe offsets to factory defaults
+    temp_comp.clear_all_offsets();
+    SERIAL_ECHOLNPGM("Offsets reset to default.");
+  }
+  else if (parser.seen("BPE")) {
+    if (!parser.seenval('V')) return;
+    const int16_t offset_val = parser.value_int();
+    if (!parser.seenval('I')) return;
+    const int16_t idx = parser.value_int();
+    const TempSensorID mod = (parser.seen('B') ? TSI_BED :
+                                #if ENABLED(USE_TEMP_EXT_COMPENSATION)
+                                  parser.seen('E') ? TSI_EXT :
+                                #endif
+                                TSI_PROBE
+                              );
+    if (idx > 0 && temp_comp.set_offset(mod, idx - 1, offset_val))
+      SERIAL_ECHOLNPAIR("Set value: ", offset_val);
+    else
+      SERIAL_ECHOLNPGM("!Invalid index. Failed to set value (note: value at index 0 is constant).");
+
+  }
+  else // Print current Z-probe adjustments. Note: Values in EEPROM might differ.
+    temp_comp.print_offsets();
+}
+
+/**
+ * M192: Wait for probe temperature sensor to reach a target
+ *
+ * Select only one of these flags:
+ *    R - Wait for heating or cooling
+ *    S - Wait only for heating
+ */
+void GcodeSuite::M192() {
+  if (DEBUGGING(DRYRUN)) return;
+
+  const bool no_wait_for_cooling = parser.seenval('S');
+  if (!no_wait_for_cooling && ! parser.seenval('R')) {
+    SERIAL_ERROR_MSG("No target temperature set.");
+    return;
+  }
+
+  const float target_temp = parser.value_celsius();
+  ui.set_status_P(thermalManager.isProbeBelowTemp(target_temp) ? GET_TEXT(MSG_PROBE_HEATING) : GET_TEXT(MSG_PROBE_COOLING));
+  thermalManager.wait_for_probe(target_temp, no_wait_for_cooling);
+}
+
+#endif // PROBE_TEMP_COMPENSATION
diff --git a/Marlin/src/gcode/calibrate/M100.cpp b/Marlin/src/gcode/calibrate/M100.cpp
new file mode 100644
index 0000000..9ac2380
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/M100.cpp
@@ -0,0 +1,379 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(M100_FREE_MEMORY_WATCHER)
+
+#include "../gcode.h"
+#include "../queue.h"
+#include "../../libs/hex_print.h"
+
+#include "../../MarlinCore.h" // for idle()
+
+/**
+ * M100 Free Memory Watcher
+ *
+ * This code watches the free memory block between the bottom of the heap and the top of the stack.
+ * This memory block is initialized and watched via the M100 command.
+ *
+ * M100 I   Initializes the free memory block and prints vitals statistics about the area
+ *
+ * M100 F   Identifies how much of the free memory block remains free and unused. It also
+ *          detects and reports any corruption within the free memory block that may have
+ *          happened due to errant firmware.
+ *
+ * M100 D   Does a hex display of the free memory block along with a flag for any errant
+ *          data that does not match the expected value.
+ *
+ * M100 C x Corrupts x locations within the free memory block. This is useful to check the
+ *          correctness of the M100 F and M100 D commands.
+ *
+ * Also, there are two support functions that can be called from a developer's C code.
+ *
+ *    uint16_t check_for_free_memory_corruption(PGM_P const free_memory_start);
+ *    void M100_dump_routine(PGM_P const title, const char * const start, const char * const end);
+ *
+ * Initial version by Roxy-3D
+ */
+#define M100_FREE_MEMORY_DUMPER     // Enable for the `M100 D` Dump sub-command
+#define M100_FREE_MEMORY_CORRUPTOR  // Enable for the `M100 C` Corrupt sub-command
+
+#define TEST_BYTE ((char) 0xE5)
+
+#if EITHER(__AVR__, IS_32BIT_TEENSY)
+
+  extern char __bss_end;
+  char *end_bss = &__bss_end,
+       *free_memory_start = end_bss, *free_memory_end = 0,
+       *stacklimit = 0, *heaplimit = 0;
+
+  #define MEMORY_END_CORRECTION 0
+
+#elif defined(TARGET_LPC1768)
+
+  extern char __bss_end__, __StackLimit, __HeapLimit;
+
+  char *end_bss = &__bss_end__,
+       *stacklimit = &__StackLimit,
+       *heaplimit = &__HeapLimit;
+
+  #define MEMORY_END_CORRECTION 0x200
+
+  char *free_memory_start = heaplimit,
+       *free_memory_end = stacklimit - MEMORY_END_CORRECTION;
+
+#elif defined(__SAM3X8E__)
+
+  extern char _ebss;
+
+  char *end_bss = &_ebss,
+       *free_memory_start = end_bss,
+       *free_memory_end = 0,
+       *stacklimit = 0,
+       *heaplimit = 0;
+
+  #define MEMORY_END_CORRECTION 0x10000  // need to stay well below 0x20080000 or M100 F crashes
+
+#elif defined(__SAMD51__)
+
+  extern unsigned int __bss_end__, __StackLimit, __HeapLimit;
+  extern "C" void * _sbrk(int incr);
+
+  void *end_bss = &__bss_end__,
+       *stacklimit = &__StackLimit,
+       *heaplimit = &__HeapLimit;
+
+  #define MEMORY_END_CORRECTION 0x400
+
+  char *free_memory_start = (char *)_sbrk(0) + 0x200,     //  Leave some heap space
+       *free_memory_end = (char *)stacklimit - MEMORY_END_CORRECTION;
+
+#else
+  #error "M100 - unsupported CPU"
+#endif
+
+//
+// Utility functions
+//
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wreturn-local-addr"
+
+// Location of a variable in its stack frame.
+// The returned address will be above the stack (after it returns).
+char *top_of_stack() {
+  char x;
+  return &x + 1; // x is pulled on return;
+}
+
+#pragma GCC diagnostic pop
+
+// Count the number of test bytes at the specified location.
+inline int32_t count_test_bytes(const char * const start_free_memory) {
+  for (uint32_t i = 0; i < 32000; i++)
+    if (char(start_free_memory[i]) != TEST_BYTE)
+      return i - 1;
+
+  return -1;
+}
+
+//
+// M100 sub-commands
+//
+
+#if ENABLED(M100_FREE_MEMORY_DUMPER)
+  /**
+   * M100 D
+   *  Dump the free memory block from brkval to the stack pointer.
+   *  malloc() eats memory from the start of the block and the stack grows
+   *  up from the bottom of the block. Solid test bytes indicate nothing has
+   *  used that memory yet. There should not be anything but test bytes within
+   *  the block. If so, it may indicate memory corruption due to a bad pointer.
+   *  Unexpected bytes are flagged in the right column.
+   */
+  inline void dump_free_memory(char *start_free_memory, char *end_free_memory) {
+    //
+    // Start and end the dump on a nice 16 byte boundary
+    // (even though the values are not 16-byte aligned).
+    //
+    start_free_memory = (char*)(uintptr_t(uint32_t(start_free_memory) & ~0xFUL)); // Align to 16-byte boundary
+    end_free_memory   = (char*)(uintptr_t(uint32_t(end_free_memory)   |  0xFUL)); // Align end_free_memory to the 15th byte (at or above end_free_memory)
+
+    // Dump command main loop
+    while (start_free_memory < end_free_memory) {
+      print_hex_address(start_free_memory);             // Print the address
+      SERIAL_CHAR(':');
+      LOOP_L_N(i, 16) {  // and 16 data bytes
+        if (i == 8) SERIAL_CHAR('-');
+        print_hex_byte(start_free_memory[i]);
+        SERIAL_CHAR(' ');
+      }
+      serial_delay(25);
+      SERIAL_CHAR('|');                   // Point out non test bytes
+      LOOP_L_N(i, 16) {
+        char ccc = (char)start_free_memory[i]; // cast to char before automatically casting to char on assignment, in case the compiler is broken
+        ccc = (ccc == TEST_BYTE) ? ' ' : '?';
+        SERIAL_CHAR(ccc);
+      }
+      SERIAL_EOL();
+      start_free_memory += 16;
+      serial_delay(25);
+      idle();
+    }
+  }
+
+  void M100_dump_routine(PGM_P const title, const char * const start, const char * const end) {
+    serialprintPGM(title);
+    SERIAL_EOL();
+    //
+    // Round the start and end locations to produce full lines of output
+    //
+    dump_free_memory(
+      (char*)(uintptr_t(uint32_t(start) & ~0xFUL)), // Align to 16-byte boundary
+      (char*)(uintptr_t(uint32_t(end)   |  0xFUL))  // Align end_free_memory to the 15th byte (at or above end_free_memory)
+    );
+  }
+
+#endif // M100_FREE_MEMORY_DUMPER
+
+inline int check_for_free_memory_corruption(PGM_P const title) {
+  serialprintPGM(title);
+
+  char *start_free_memory = free_memory_start, *end_free_memory = free_memory_end;
+  int n = end_free_memory - start_free_memory;
+
+  SERIAL_ECHOPAIR("\nfmc() n=", n);
+  SERIAL_ECHOPAIR("\nfree_memory_start=", hex_address(free_memory_start));
+  SERIAL_ECHOLNPAIR("  end_free_memory=", hex_address(end_free_memory));
+
+  if (end_free_memory < start_free_memory)  {
+    SERIAL_ECHOPGM(" end_free_memory < Heap ");
+    // SET_INPUT_PULLUP(63);           // if the developer has a switch wired up to their controller board
+    // safe_delay(5);                  // this code can be enabled to pause the display as soon as the
+    // while ( READ(63))               // malfunction is detected.   It is currently defaulting to a switch
+    //   idle();                       // being on pin-63 which is unassigend and available on most controller
+    // safe_delay(20);                 // boards.
+    // while ( !READ(63))
+    //   idle();
+    serial_delay(20);
+    #if ENABLED(M100_FREE_MEMORY_DUMPER)
+      M100_dump_routine(PSTR("   Memory corruption detected with end_free_memory<Heap\n"), (const char*)0x1B80, (const char*)0x21FF);
+    #endif
+  }
+
+  // Scan through the range looking for the biggest block of 0xE5's we can find
+  int block_cnt = 0;
+  for (int i = 0; i < n; i++) {
+    if (start_free_memory[i] == TEST_BYTE) {
+      int32_t j = count_test_bytes(start_free_memory + i);
+      if (j > 8) {
+        // SERIAL_ECHOPAIR("Found ", j);
+        // SERIAL_ECHOLNPAIR(" bytes free at ", hex_address(start_free_memory + i));
+        i += j;
+        block_cnt++;
+        SERIAL_ECHOPAIR(" (", block_cnt);
+        SERIAL_ECHOPAIR(") found=", j);
+        SERIAL_ECHOLNPGM("   ");
+      }
+    }
+  }
+  SERIAL_ECHOPAIR("  block_found=", block_cnt);
+
+  if (block_cnt != 1)
+    SERIAL_ECHOLNPGM("\nMemory Corruption detected in free memory area.");
+
+  if (block_cnt == 0)       // Make sure the special case of no free blocks shows up as an
+    block_cnt = -1;         //  error to the calling code!
+
+  SERIAL_ECHOPGM(" return=");
+  if (block_cnt == 1) {
+    SERIAL_CHAR('0');       // If the block_cnt is 1, nothing has broken up the free memory
+    SERIAL_EOL();           //  area and it is appropriate to say 'no corruption'.
+    return 0;
+  }
+  SERIAL_ECHOLNPGM("true");
+  return block_cnt;
+}
+
+/**
+ * M100 F
+ *  Return the number of free bytes in the memory pool,
+ *  with other vital statistics defining the pool.
+ */
+inline void free_memory_pool_report(char * const start_free_memory, const int32_t size) {
+  int32_t max_cnt = -1, block_cnt = 0;
+  char *max_addr = nullptr;
+  // Find the longest block of test bytes in the buffer
+  for (int32_t i = 0; i < size; i++) {
+    char *addr = start_free_memory + i;
+    if (*addr == TEST_BYTE) {
+      const int32_t j = count_test_bytes(addr);
+      if (j > 8) {
+        SERIAL_ECHOPAIR("Found ", j);
+        SERIAL_ECHOLNPAIR(" bytes free at ", hex_address(addr));
+        if (j > max_cnt) {
+          max_cnt  = j;
+          max_addr = addr;
+        }
+        i += j;
+        block_cnt++;
+      }
+    }
+  }
+  if (block_cnt > 1) {
+    SERIAL_ECHOLNPGM("\nMemory Corruption detected in free memory area.");
+    SERIAL_ECHOPAIR("\nLargest free block is ", max_cnt);
+    SERIAL_ECHOLNPAIR(" bytes at ", hex_address(max_addr));
+  }
+  SERIAL_ECHOLNPAIR("check_for_free_memory_corruption() = ", check_for_free_memory_corruption(PSTR("M100 F ")));
+}
+
+#if ENABLED(M100_FREE_MEMORY_CORRUPTOR)
+  /**
+   * M100 C<num>
+   *  Corrupt <num> locations in the free memory pool and report the corrupt addresses.
+   *  This is useful to check the correctness of the M100 D and the M100 F commands.
+   */
+  inline void corrupt_free_memory(char *start_free_memory, const uint32_t size) {
+    start_free_memory += 8;
+    const uint32_t near_top = top_of_stack() - start_free_memory - 250, // -250 to avoid interrupt activity that's altered the stack.
+                   j = near_top / (size + 1);
+
+    SERIAL_ECHOLNPGM("Corrupting free memory block.\n");
+    for (uint32_t i = 1; i <= size; i++) {
+      char * const addr = start_free_memory + i * j;
+      *addr = i;
+      SERIAL_ECHOPAIR("\nCorrupting address: ", hex_address(addr));
+    }
+    SERIAL_EOL();
+  }
+#endif // M100_FREE_MEMORY_CORRUPTOR
+
+/**
+ * M100 I
+ *  Init memory for the M100 tests. (Automatically applied on the first M100.)
+ */
+inline void init_free_memory(char *start_free_memory, int32_t size) {
+  SERIAL_ECHOLNPGM("Initializing free memory block.\n\n");
+
+  size -= 250;    // -250 to avoid interrupt activity that's altered the stack.
+  if (size < 0) {
+    SERIAL_ECHOLNPGM("Unable to initialize.\n");
+    return;
+  }
+
+  start_free_memory += 8;       // move a few bytes away from the heap just because we don't want
+                  // to be altering memory that close to it.
+  memset(start_free_memory, TEST_BYTE, size);
+
+  SERIAL_ECHO(size);
+  SERIAL_ECHOLNPGM(" bytes of memory initialized.\n");
+
+  for (int32_t i = 0; i < size; i++) {
+    if (start_free_memory[i] != TEST_BYTE) {
+      SERIAL_ECHOPAIR("? address : ", hex_address(start_free_memory + i));
+      SERIAL_ECHOLNPAIR("=", hex_byte(start_free_memory[i]));
+      SERIAL_EOL();
+    }
+  }
+}
+
+/**
+ * M100: Free Memory Check
+ */
+void GcodeSuite::M100() {
+
+  char *sp = top_of_stack();
+  if (!free_memory_end) free_memory_end = sp - MEMORY_END_CORRECTION;
+  SERIAL_ECHOPAIR("\nbss_end               : ", hex_address(end_bss));
+  if (heaplimit) SERIAL_ECHOPAIR("\n__heaplimit           : ", hex_address(heaplimit));
+  SERIAL_ECHOPAIR("\nfree_memory_start     : ", hex_address(free_memory_start));
+  if (stacklimit) SERIAL_ECHOPAIR("\n__stacklimit          : ", hex_address(stacklimit));
+  SERIAL_ECHOPAIR("\nfree_memory_end       : ", hex_address(free_memory_end));
+  if (MEMORY_END_CORRECTION)  SERIAL_ECHOPAIR("\nMEMORY_END_CORRECTION: ", MEMORY_END_CORRECTION);
+  SERIAL_ECHOLNPAIR("\nStack Pointer         : ", hex_address(sp));
+
+  // Always init on the first invocation of M100
+  static bool m100_not_initialized = true;
+  if (m100_not_initialized || parser.seen('I')) {
+    m100_not_initialized = false;
+    init_free_memory(free_memory_start, free_memory_end - free_memory_start);
+  }
+
+  #if ENABLED(M100_FREE_MEMORY_DUMPER)
+    if (parser.seen('D'))
+      return dump_free_memory(free_memory_start, free_memory_end);
+  #endif
+
+  if (parser.seen('F'))
+    return free_memory_pool_report(free_memory_start, free_memory_end - free_memory_start);
+
+  #if ENABLED(M100_FREE_MEMORY_CORRUPTOR)
+
+    if (parser.seen('C'))
+      return corrupt_free_memory(free_memory_start, parser.value_int());
+
+  #endif
+}
+
+#endif // M100_FREE_MEMORY_WATCHER
diff --git a/Marlin/src/gcode/calibrate/M12.cpp b/Marlin/src/gcode/calibrate/M12.cpp
new file mode 100644
index 0000000..da24454
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/M12.cpp
@@ -0,0 +1,39 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#include "../../inc/MarlinConfigPre.h"
+
+#if ENABLED(EXTERNAL_CLOSED_LOOP_CONTROLLER)
+
+#include "../gcode.h"
+#include "../../module/planner.h"
+#include "../../feature/closedloop.h"
+
+void GcodeSuite::M12() {
+
+  planner.synchronize();
+
+  if (parser.seenval('S'))
+    closedloop.set(parser.value_int()); // Force a CLC set
+
+}
+
+#endif
diff --git a/Marlin/src/gcode/calibrate/M425.cpp b/Marlin/src/gcode/calibrate/M425.cpp
new file mode 100644
index 0000000..40441ac
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/M425.cpp
@@ -0,0 +1,111 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(BACKLASH_GCODE)
+
+#include "../../feature/backlash.h"
+#include "../../module/planner.h"
+
+#include "../gcode.h"
+
+/**
+ * M425: Enable and tune backlash correction.
+ *
+ *   F<fraction>     Enable/disable/fade-out backlash correction (0.0 to 1.0)
+ *   S<smoothing_mm> Distance over which backlash correction is spread
+ *   X<distance_mm>  Set the backlash distance on X (0 to disable)
+ *   Y<distance_mm>                        ... on Y
+ *   Z<distance_mm>                        ... on Z
+ *   X               If a backlash measurement was done on X, copy that value
+ *   Y                                              ... on Y
+ *   Z                                              ... on Z
+ *
+ * Type M425 without any arguments to show active values.
+ */
+void GcodeSuite::M425() {
+  bool noArgs = true;
+
+  auto axis_can_calibrate = [](const uint8_t a) {
+    switch (a) {
+      default:
+      case X_AXIS: return AXIS_CAN_CALIBRATE(X);
+      case Y_AXIS: return AXIS_CAN_CALIBRATE(Y);
+      case Z_AXIS: return AXIS_CAN_CALIBRATE(Z);
+    }
+  };
+
+  LOOP_XYZ(a) {
+    if (axis_can_calibrate(a) && parser.seen(XYZ_CHAR(a))) {
+      planner.synchronize();
+      backlash.distance_mm[a] = parser.has_value() ? parser.value_linear_units() : backlash.get_measurement(AxisEnum(a));
+      noArgs = false;
+    }
+  }
+
+  if (parser.seen('F')) {
+    planner.synchronize();
+    backlash.set_correction(parser.value_float());
+    noArgs = false;
+  }
+
+  #ifdef BACKLASH_SMOOTHING_MM
+    if (parser.seen('S')) {
+      planner.synchronize();
+      backlash.smoothing_mm = parser.value_linear_units();
+      noArgs = false;
+    }
+  #endif
+
+  if (noArgs) {
+    SERIAL_ECHOPGM("Backlash Correction ");
+    if (!backlash.correction) SERIAL_ECHOPGM("in");
+    SERIAL_ECHOLNPGM("active:");
+    SERIAL_ECHOLNPAIR("  Correction Amount/Fade-out:     F", backlash.get_correction(), " (F1.0 = full, F0.0 = none)");
+    SERIAL_ECHOPGM("  Backlash Distance (mm):        ");
+    LOOP_XYZ(a) if (axis_can_calibrate(a)) {
+      SERIAL_CHAR(' ', XYZ_CHAR(a));
+      SERIAL_ECHO(backlash.distance_mm[a]);
+      SERIAL_EOL();
+    }
+
+    #ifdef BACKLASH_SMOOTHING_MM
+      SERIAL_ECHOLNPAIR("  Smoothing (mm):                 S", backlash.smoothing_mm);
+    #endif
+
+    #if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
+      SERIAL_ECHOPGM("  Average measured backlash (mm):");
+      if (backlash.has_any_measurement()) {
+        LOOP_XYZ(a) if (axis_can_calibrate(a) && backlash.has_measurement(AxisEnum(a))) {
+          SERIAL_CHAR(' ', XYZ_CHAR(a));
+          SERIAL_ECHO(backlash.get_measurement(AxisEnum(a)));
+        }
+      }
+      else
+        SERIAL_ECHOPGM(" (Not yet measured)");
+      SERIAL_EOL();
+    #endif
+  }
+}
+
+#endif // BACKLASH_GCODE
diff --git a/Marlin/src/gcode/calibrate/M48.cpp b/Marlin/src/gcode/calibrate/M48.cpp
new file mode 100644
index 0000000..97aea59
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/M48.cpp
@@ -0,0 +1,275 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/probe.h"
+#include "../../lcd/marlinui.h"
+
+#include "../../feature/bedlevel/bedlevel.h"
+
+#if HAS_LEVELING
+  #include "../../module/planner.h"
+#endif
+
+/**
+ * M48: Z probe repeatability measurement function.
+ *
+ * Usage:
+ *   M48 <P#> <X#> <Y#> <V#> <E> <L#> <S>
+ *     P = Number of sampled points (4-50, default 10)
+ *     X = Sample X position
+ *     Y = Sample Y position
+ *     V = Verbose level (0-4, default=1)
+ *     E = Engage Z probe for each reading
+ *     L = Number of legs of movement before probe
+ *     S = Schizoid (Or Star if you prefer)
+ *
+ * This function requires the machine to be homed before invocation.
+ */
+
+void GcodeSuite::M48() {
+
+  if (homing_needed_error()) return;
+
+  const int8_t verbose_level = parser.byteval('V', 1);
+  if (!WITHIN(verbose_level, 0, 4)) {
+    SERIAL_ECHOLNPGM("?(V)erbose level implausible (0-4).");
+    return;
+  }
+
+  if (verbose_level > 0)
+    SERIAL_ECHOLNPGM("M48 Z-Probe Repeatability Test");
+
+  const int8_t n_samples = parser.byteval('P', 10);
+  if (!WITHIN(n_samples, 4, 50)) {
+    SERIAL_ECHOLNPGM("?Sample size not plausible (4-50).");
+    return;
+  }
+
+  const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;
+
+  // Test at the current position by default, overridden by X and Y
+  const xy_pos_t test_position = {
+    parser.linearval('X', current_position.x + probe.offset_xy.x),  // If no X use the probe's current X position
+    parser.linearval('Y', current_position.y + probe.offset_xy.y)   // If no Y, ditto
+  };
+
+  if (!probe.can_reach(test_position)) {
+    ui.set_status_P(GET_TEXT(MSG_M48_OUT_OF_BOUNDS), 99);
+    SERIAL_ECHOLNPGM("? (X,Y) out of bounds.");
+    return;
+  }
+
+  // Get the number of leg moves per test-point
+  bool seen_L = parser.seen('L');
+  uint8_t n_legs = seen_L ? parser.value_byte() : 0;
+  if (n_legs > 15) {
+    SERIAL_ECHOLNPGM("?Legs of movement implausible (0-15).");
+    return;
+  }
+  if (n_legs == 1) n_legs = 2;
+
+  // Schizoid motion as an optional stress-test
+  const bool schizoid_flag = parser.boolval('S');
+  if (schizoid_flag && !seen_L) n_legs = 7;
+
+  if (verbose_level > 2)
+    SERIAL_ECHOLNPGM("Positioning the probe...");
+
+  // Always disable Bed Level correction before probing...
+
+  #if HAS_LEVELING
+    const bool was_enabled = planner.leveling_active;
+    set_bed_leveling_enabled(false);
+  #endif
+
+  // Work with reasonable feedrates
+  remember_feedrate_scaling_off();
+
+  // Working variables
+  float mean = 0.0,     // The average of all points so far, used to calculate deviation
+        sigma = 0.0,    // Standard deviation of all points so far
+        min = 99999.9,  // Smallest value sampled so far
+        max = -99999.9, // Largest value sampled so far
+        sample_set[n_samples];  // Storage for sampled values
+
+  auto dev_report = [](const bool verbose, const float &mean, const float &sigma, const float &min, const float &max, const bool final=false) {
+    if (verbose) {
+      SERIAL_ECHOPAIR_F("Mean: ", mean, 6);
+      if (!final) SERIAL_ECHOPAIR_F(" Sigma: ", sigma, 6);
+      SERIAL_ECHOPAIR_F(" Min: ", min, 3);
+      SERIAL_ECHOPAIR_F(" Max: ", max, 3);
+      SERIAL_ECHOPAIR_F(" Range: ", max-min, 3);
+      if (final) SERIAL_EOL();
+    }
+    if (final) {
+      SERIAL_ECHOLNPAIR_F("Standard Deviation: ", sigma, 6);
+      SERIAL_EOL();
+    }
+  };
+
+  // Move to the first point, deploy, and probe
+  const float t = probe.probe_at_point(test_position, raise_after, verbose_level);
+  bool probing_good = !isnan(t);
+
+  if (probing_good) {
+    randomSeed(millis());
+
+    float sample_sum = 0.0;
+
+    LOOP_L_N(n, n_samples) {
+      #if HAS_WIRED_LCD
+        // Display M48 progress in the status bar
+        ui.status_printf_P(0, PSTR(S_FMT ": %d/%d"), GET_TEXT(MSG_M48_POINT), int(n + 1), int(n_samples));
+      #endif
+
+      // When there are "legs" of movement move around the point before probing
+      if (n_legs) {
+
+        // Pick a random direction, starting angle, and radius
+        const int dir = (random(0, 10) > 5.0) ? -1 : 1;  // clockwise or counter clockwise
+        float angle = random(0, 360);
+        const float radius = random(
+          #if ENABLED(DELTA)
+            int(0.1250000000 * (DELTA_PRINTABLE_RADIUS)),
+            int(0.3333333333 * (DELTA_PRINTABLE_RADIUS))
+          #else
+            int(5), int(0.125 * _MIN(X_BED_SIZE, Y_BED_SIZE))
+          #endif
+        );
+        if (verbose_level > 3) {
+          SERIAL_ECHOPAIR("Start radius:", radius, " angle:", angle, " dir:");
+          if (dir > 0) SERIAL_CHAR('C');
+          SERIAL_ECHOLNPGM("CW");
+        }
+
+        // Move from leg to leg in rapid succession
+        LOOP_L_N(l, n_legs - 1) {
+
+          // Move some distance around the perimeter
+          float delta_angle;
+          if (schizoid_flag) {
+            // The points of a 5 point star are 72 degrees apart.
+            // Skip a point and go to the next one on the star.
+            delta_angle = dir * 2.0 * 72.0;
+          }
+          else {
+            // Just move further along the perimeter.
+            delta_angle = dir * (float)random(25, 45);
+          }
+          angle += delta_angle;
+
+          // Trig functions work without clamping, but just to be safe...
+          while (angle > 360.0) angle -= 360.0;
+          while (angle < 0.0) angle += 360.0;
+
+          // Choose the next position as an offset to chosen test position
+          const xy_pos_t noz_pos = test_position - probe.offset_xy;
+          xy_pos_t next_pos = {
+            noz_pos.x + float(cos(RADIANS(angle))) * radius,
+            noz_pos.y + float(sin(RADIANS(angle))) * radius
+          };
+
+          #if ENABLED(DELTA)
+            // If the probe can't reach the point on a round bed...
+            // Simply scale the numbers to bring them closer to origin.
+            while (!probe.can_reach(next_pos)) {
+              next_pos *= 0.8f;
+              if (verbose_level > 3)
+                SERIAL_ECHOLNPAIR_P(PSTR("Moving inward: X"), next_pos.x, SP_Y_STR, next_pos.y);
+            }
+          #else
+            // For a rectangular bed just keep the probe in bounds
+            LIMIT(next_pos.x, X_MIN_POS, X_MAX_POS);
+            LIMIT(next_pos.y, Y_MIN_POS, Y_MAX_POS);
+          #endif
+
+          if (verbose_level > 3)
+            SERIAL_ECHOLNPAIR_P(PSTR("Going to: X"), next_pos.x, SP_Y_STR, next_pos.y);
+
+          do_blocking_move_to_xy(next_pos);
+        } // n_legs loop
+      } // n_legs
+
+      // Probe a single point
+      const float pz = probe.probe_at_point(test_position, raise_after, 0);
+
+      // Break the loop if the probe fails
+      probing_good = !isnan(pz);
+      if (!probing_good) break;
+
+      // Store the new sample
+      sample_set[n] = pz;
+
+      // Keep track of the largest and smallest samples
+      NOMORE(min, pz);
+      NOLESS(max, pz);
+
+      // Get the mean value of all samples thus far
+      sample_sum += pz;
+      mean = sample_sum / (n + 1);
+
+      // Calculate the standard deviation so far.
+      // The value after the last sample will be the final output.
+      float dev_sum = 0.0;
+      LOOP_LE_N(j, n) dev_sum += sq(sample_set[j] - mean);
+      sigma = SQRT(dev_sum / (n + 1));
+
+      if (verbose_level > 1) {
+        SERIAL_ECHO((int)(n + 1));
+        SERIAL_ECHOPAIR(" of ", (int)n_samples);
+        SERIAL_ECHOPAIR_F(": z: ", pz, 3);
+        SERIAL_CHAR(' ');
+        dev_report(verbose_level > 2, mean, sigma, min, max);
+        SERIAL_EOL();
+      }
+
+    } // n_samples loop
+  }
+
+  probe.stow();
+
+  if (probing_good) {
+    SERIAL_ECHOLNPGM("Finished!");
+    dev_report(verbose_level > 0, mean, sigma, min, max, true);
+
+    #if HAS_WIRED_LCD
+      // Display M48 results in the status bar
+      char sigma_str[8];
+      ui.status_printf_P(0, PSTR(S_FMT ": %s"), GET_TEXT(MSG_M48_DEVIATION), dtostrf(sigma, 2, 6, sigma_str));
+    #endif
+  }
+
+  restore_feedrate_and_scaling();
+
+  // Re-enable bed level correction if it had been on
+  TERN_(HAS_LEVELING, set_bed_leveling_enabled(was_enabled));
+
+  report_current_position();
+}
+
+#endif // Z_MIN_PROBE_REPEATABILITY_TEST
diff --git a/Marlin/src/gcode/calibrate/M665.cpp b/Marlin/src/gcode/calibrate/M665.cpp
new file mode 100644
index 0000000..557204c
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/M665.cpp
@@ -0,0 +1,112 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if IS_KINEMATIC
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+
+#if ENABLED(DELTA)
+
+  #include "../../module/delta.h"
+  /**
+   * M665: Set delta configurations
+   *
+   *    H = delta height
+   *    L = diagonal rod
+   *    R = delta radius
+   *    S = segments per second
+   *    X = Alpha (Tower 1) angle trim
+   *    Y = Beta  (Tower 2) angle trim
+   *    Z = Gamma (Tower 3) angle trim
+   *    A = Alpha (Tower 1) digonal rod trim
+   *    B = Beta  (Tower 2) digonal rod trim
+   *    C = Gamma (Tower 3) digonal rod trim
+   */
+  void GcodeSuite::M665() {
+    if (parser.seen('H')) delta_height              = parser.value_linear_units();
+    if (parser.seen('L')) delta_diagonal_rod        = parser.value_linear_units();
+    if (parser.seen('R')) delta_radius              = parser.value_linear_units();
+    if (parser.seen('S')) delta_segments_per_second = parser.value_float();
+    if (parser.seen('X')) delta_tower_angle_trim.a  = parser.value_float();
+    if (parser.seen('Y')) delta_tower_angle_trim.b  = parser.value_float();
+    if (parser.seen('Z')) delta_tower_angle_trim.c  = parser.value_float();
+    if (parser.seen('A')) delta_diagonal_rod_trim.a = parser.value_float();
+    if (parser.seen('B')) delta_diagonal_rod_trim.b = parser.value_float();
+    if (parser.seen('C')) delta_diagonal_rod_trim.c = parser.value_float();
+    recalc_delta_settings();
+  }
+
+#elif IS_SCARA
+
+  #include "../../module/scara.h"
+
+  /**
+   * M665: Set SCARA settings
+   *
+   * Parameters:
+   *
+   *   S[segments-per-second] - Segments-per-second
+   *   P[theta-psi-offset]    - Theta-Psi offset, added to the shoulder (A/X) angle
+   *   T[theta-offset]        - Theta     offset, added to the elbow    (B/Y) angle
+   *   Z[z-offset]            - Z offset, added to Z
+   *
+   *   A, P, and X are all aliases for the shoulder angle
+   *   B, T, and Y are all aliases for the elbow angle
+   */
+  void GcodeSuite::M665() {
+    if (parser.seenval('S')) delta_segments_per_second = parser.value_float();
+
+    #if HAS_SCARA_OFFSET
+
+      if (parser.seenval('Z')) scara_home_offset.z = parser.value_linear_units();
+
+      const bool hasA = parser.seenval('A'), hasP = parser.seenval('P'), hasX = parser.seenval('X');
+      const uint8_t sumAPX = hasA + hasP + hasX;
+      if (sumAPX) {
+        if (sumAPX == 1)
+          scara_home_offset.a = parser.value_float();
+        else {
+          SERIAL_ERROR_MSG("Only one of A, P, or X is allowed.");
+          return;
+        }
+      }
+
+      const bool hasB = parser.seenval('B'), hasT = parser.seenval('T'), hasY = parser.seenval('Y');
+      const uint8_t sumBTY = hasB + hasT + hasY;
+      if (sumBTY) {
+        if (sumBTY == 1)
+          scara_home_offset.b = parser.value_float();
+        else {
+          SERIAL_ERROR_MSG("Only one of B, T, or Y is allowed.");
+          return;
+        }
+      }
+
+    #endif // HAS_SCARA_OFFSET
+  }
+
+#endif
+
+#endif // IS_KINEMATIC
diff --git a/Marlin/src/gcode/calibrate/M666.cpp b/Marlin/src/gcode/calibrate/M666.cpp
new file mode 100644
index 0000000..e915aa8
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/M666.cpp
@@ -0,0 +1,105 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(DELTA) || HAS_EXTRA_ENDSTOPS
+
+#include "../gcode.h"
+
+#if ENABLED(DELTA)
+
+  #include "../../module/delta.h"
+  #include "../../module/motion.h"
+
+  #define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+  #include "../../core/debug_out.h"
+
+  /**
+   * M666: Set delta endstop adjustment
+   */
+  void GcodeSuite::M666() {
+    DEBUG_SECTION(log_M666, "M666", DEBUGGING(LEVELING));
+    LOOP_XYZ(i) {
+      if (parser.seen(XYZ_CHAR(i))) {
+        const float v = parser.value_linear_units();
+        if (v * Z_HOME_DIR <= 0) delta_endstop_adj[i] = v;
+        if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("delta_endstop_adj[", XYZ_CHAR(i), "] = ", delta_endstop_adj[i]);
+      }
+    }
+  }
+
+#elif HAS_EXTRA_ENDSTOPS
+
+  #include "../../module/endstops.h"
+
+  /**
+   * M666: Set Dual Endstops offsets for X, Y, and/or Z.
+   *       With no parameters report current offsets.
+   *
+   * For Triple / Quad Z Endstops:
+   *   Set Z2 Only: M666 S2 Z<offset>
+   *   Set Z3 Only: M666 S3 Z<offset>
+   *   Set Z4 Only: M666 S4 Z<offset>
+   *       Set All: M666 Z<offset>
+   */
+  void GcodeSuite::M666() {
+    #if ENABLED(X_DUAL_ENDSTOPS)
+      if (parser.seenval('X')) endstops.x2_endstop_adj = parser.value_linear_units();
+    #endif
+    #if ENABLED(Y_DUAL_ENDSTOPS)
+      if (parser.seenval('Y')) endstops.y2_endstop_adj = parser.value_linear_units();
+    #endif
+    #if ENABLED(Z_MULTI_ENDSTOPS)
+      if (parser.seenval('Z')) {
+        #if NUM_Z_STEPPER_DRIVERS >= 3
+          const float z_adj = parser.value_linear_units();
+          const int ind = parser.intval('S');
+          if (!ind || ind == 2) endstops.z2_endstop_adj = z_adj;
+          if (!ind || ind == 3) endstops.z3_endstop_adj = z_adj;
+          #if NUM_Z_STEPPER_DRIVERS >= 4
+            if (!ind || ind == 4) endstops.z4_endstop_adj = z_adj;
+          #endif
+        #else
+          endstops.z2_endstop_adj = parser.value_linear_units();
+        #endif
+      }
+    #endif
+    if (!parser.seen("XYZ")) {
+      SERIAL_ECHOPGM("Dual Endstop Adjustment (mm): ");
+      #if ENABLED(X_DUAL_ENDSTOPS)
+        SERIAL_ECHOPAIR(" X2:", endstops.x2_endstop_adj);
+      #endif
+      #if ENABLED(Y_DUAL_ENDSTOPS)
+        SERIAL_ECHOPAIR(" Y2:", endstops.y2_endstop_adj);
+      #endif
+      #if ENABLED(Z_MULTI_ENDSTOPS)
+        #define _ECHO_ZADJ(N) SERIAL_ECHOPAIR(" Z" STRINGIFY(N) ":", endstops.z##N##_endstop_adj);
+        REPEAT_S(2, INCREMENT(NUM_Z_STEPPER_DRIVERS), _ECHO_ZADJ)
+      #endif
+      SERIAL_EOL();
+    }
+  }
+
+#endif // HAS_EXTRA_ENDSTOPS
+
+#endif // DELTA || HAS_EXTRA_ENDSTOPS
diff --git a/Marlin/src/gcode/calibrate/M852.cpp b/Marlin/src/gcode/calibrate/M852.cpp
new file mode 100644
index 0000000..b60f417
--- /dev/null
+++ b/Marlin/src/gcode/calibrate/M852.cpp
@@ -0,0 +1,106 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(SKEW_CORRECTION_GCODE)
+
+#include "../gcode.h"
+#include "../../module/planner.h"
+
+/**
+ * M852: Get or set the machine skew factors. Reports current values with no arguments.
+ *
+ *  S[xy_factor] - Alias for 'I'
+ *  I[xy_factor] - New XY skew factor
+ *  J[xz_factor] - New XZ skew factor
+ *  K[yz_factor] - New YZ skew factor
+ */
+void GcodeSuite::M852() {
+  uint8_t ijk = 0, badval = 0, setval = 0;
+
+  if (parser.seen('I') || parser.seen('S')) {
+    ++ijk;
+    const float value = parser.value_linear_units();
+    if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX)) {
+      if (planner.skew_factor.xy != value) {
+        planner.skew_factor.xy = value;
+        ++setval;
+      }
+    }
+    else
+      ++badval;
+  }
+
+  #if ENABLED(SKEW_CORRECTION_FOR_Z)
+
+    if (parser.seen('J')) {
+      ++ijk;
+      const float value = parser.value_linear_units();
+      if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX)) {
+        if (planner.skew_factor.xz != value) {
+          planner.skew_factor.xz = value;
+          ++setval;
+        }
+      }
+      else
+        ++badval;
+    }
+
+    if (parser.seen('K')) {
+      ++ijk;
+      const float value = parser.value_linear_units();
+      if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX)) {
+        if (planner.skew_factor.yz != value) {
+          planner.skew_factor.yz = value;
+          ++setval;
+        }
+      }
+      else
+        ++badval;
+    }
+
+  #endif
+
+  if (badval)
+    SERIAL_ECHOLNPGM(STR_SKEW_MIN " " STRINGIFY(SKEW_FACTOR_MIN) " " STR_SKEW_MAX " " STRINGIFY(SKEW_FACTOR_MAX));
+
+  // When skew is changed the current position changes
+  if (setval) {
+    set_current_from_steppers_for_axis(ALL_AXES);
+    sync_plan_position();
+    report_current_position();
+  }
+
+  if (!ijk) {
+    SERIAL_ECHO_START();
+    serialprintPGM(GET_TEXT(MSG_SKEW_FACTOR));
+    SERIAL_ECHOPAIR_F(" XY: ", planner.skew_factor.xy, 6);
+    #if ENABLED(SKEW_CORRECTION_FOR_Z)
+      SERIAL_ECHOPAIR_F(" XZ: ", planner.skew_factor.xz, 6);
+      SERIAL_ECHOPAIR_F(" YZ: ", planner.skew_factor.yz, 6);
+    #endif
+    SERIAL_EOL();
+  }
+}
+
+#endif // SKEW_CORRECTION_GCODE
diff --git a/Marlin/src/gcode/config/M200-M205.cpp b/Marlin/src/gcode/config/M200-M205.cpp
new file mode 100644
index 0000000..cb17fc4
--- /dev/null
+++ b/Marlin/src/gcode/config/M200-M205.cpp
@@ -0,0 +1,191 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../MarlinCore.h"
+#include "../../module/planner.h"
+
+#if DISABLED(NO_VOLUMETRICS)
+
+  /**
+   * M200: Set filament diameter and set E axis units to cubic units
+   *
+   *    T<extruder> - Optional extruder number. Current extruder if omitted.
+   *    D<linear>   - Set filament diameter and enable. D0 disables volumetric.
+   *    S<bool>     - Turn volumetric ON or OFF.
+   *    L<float>    - Volumetric extruder limit (in mm^3/sec). L0 disables the limit.
+   */
+  void GcodeSuite::M200() {
+
+    const int8_t target_extruder = get_target_extruder_from_command();
+    if (target_extruder < 0) return;
+
+    bool vol_enable = parser.volumetric_enabled,
+         can_enable = true;
+
+    if (parser.seenval('D')) {
+      const float dval = parser.value_linear_units();
+      if (dval) { // Set filament size for volumetric calculation
+        planner.set_filament_size(target_extruder, dval);
+        vol_enable = true;    // Dn = enable for compatibility
+      }
+      else
+        can_enable = false;   // D0 = disable for compatibility
+    }
+
+    // Enable or disable with S1 / S0
+    parser.volumetric_enabled = can_enable && parser.boolval('S', vol_enable);
+
+    #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
+      if (parser.seenval('L')) {
+        // Set volumetric limit (in mm^3/sec)
+        const float lval = parser.value_float();
+        if (WITHIN(lval, 0, 20))
+          planner.set_volumetric_extruder_limit(target_extruder, lval);
+        else
+          SERIAL_ECHOLNPGM("?L value out of range (0-20).");
+      }
+    #endif
+
+    planner.calculate_volumetric_multipliers();
+  }
+
+#endif // !NO_VOLUMETRICS
+
+/**
+ * M201: Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
+ *
+ *       With multiple extruders use T to specify which one.
+ */
+void GcodeSuite::M201() {
+
+  const int8_t target_extruder = get_target_extruder_from_command();
+  if (target_extruder < 0) return;
+
+  #ifdef XY_FREQUENCY_LIMIT
+    if (parser.seenval('F')) planner.set_frequency_limit(parser.value_byte());
+    if (parser.seenval('G')) planner.xy_freq_min_speed_factor = constrain(parser.value_float(), 1, 100) / 100;
+  #endif
+
+  LOOP_XYZE(i) {
+    if (parser.seen(axis_codes[i])) {
+      const uint8_t a = (i == E_AXIS ? uint8_t(E_AXIS_N(target_extruder)) : i);
+      planner.set_max_acceleration(a, parser.value_axis_units((AxisEnum)a));
+    }
+  }
+}
+
+/**
+ * M203: Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in units/sec
+ *
+ *       With multiple extruders use T to specify which one.
+ */
+void GcodeSuite::M203() {
+
+  const int8_t target_extruder = get_target_extruder_from_command();
+  if (target_extruder < 0) return;
+
+  LOOP_XYZE(i)
+    if (parser.seen(axis_codes[i])) {
+      const uint8_t a = (i == E_AXIS ? uint8_t(E_AXIS_N(target_extruder)) : i);
+      planner.set_max_feedrate(a, parser.value_axis_units((AxisEnum)a));
+    }
+}
+
+/**
+ * M204: Set Accelerations in units/sec^2 (M204 P1200 R3000 T3000)
+ *
+ *    P = Printing moves
+ *    R = Retract only (no X, Y, Z) moves
+ *    T = Travel (non printing) moves
+ */
+void GcodeSuite::M204() {
+  if (!parser.seen("PRST")) {
+    SERIAL_ECHOPAIR("Acceleration: P", planner.settings.acceleration);
+    SERIAL_ECHOPAIR(" R", planner.settings.retract_acceleration);
+    SERIAL_ECHOLNPAIR_P(SP_T_STR, planner.settings.travel_acceleration);
+  }
+  else {
+    //planner.synchronize();
+    // 'S' for legacy compatibility. Should NOT BE USED for new development
+    if (parser.seenval('S')) planner.settings.travel_acceleration = planner.settings.acceleration = parser.value_linear_units();
+    if (parser.seenval('P')) planner.settings.acceleration = parser.value_linear_units();
+    if (parser.seenval('R')) planner.settings.retract_acceleration = parser.value_linear_units();
+    if (parser.seenval('T')) planner.settings.travel_acceleration = parser.value_linear_units();
+  }
+}
+
+/**
+ * M205: Set Advanced Settings
+ *
+ *    B = Min Segment Time (µs)
+ *    S = Min Feed Rate (units/s)
+ *    T = Min Travel Feed Rate (units/s)
+ *    X = Max X Jerk (units/sec^2)
+ *    Y = Max Y Jerk (units/sec^2)
+ *    Z = Max Z Jerk (units/sec^2)
+ *    E = Max E Jerk (units/sec^2)
+ *    J = Junction Deviation (mm) (If not using CLASSIC_JERK)
+ */
+void GcodeSuite::M205() {
+  #if HAS_JUNCTION_DEVIATION
+    #define J_PARAM  "J"
+  #else
+    #define J_PARAM
+  #endif
+  #if HAS_CLASSIC_JERK
+    #define XYZE_PARAM "XYZE"
+  #else
+    #define XYZE_PARAM
+  #endif
+  if (!parser.seen("BST" J_PARAM XYZE_PARAM)) return;
+
+  //planner.synchronize();
+  if (parser.seen('B')) planner.settings.min_segment_time_us = parser.value_ulong();
+  if (parser.seen('S')) planner.settings.min_feedrate_mm_s = parser.value_linear_units();
+  if (parser.seen('T')) planner.settings.min_travel_feedrate_mm_s = parser.value_linear_units();
+  #if HAS_JUNCTION_DEVIATION
+    if (parser.seen('J')) {
+      const float junc_dev = parser.value_linear_units();
+      if (WITHIN(junc_dev, 0.01f, 0.3f)) {
+        planner.junction_deviation_mm = junc_dev;
+        TERN_(LIN_ADVANCE, planner.recalculate_max_e_jerk());
+      }
+      else
+        SERIAL_ERROR_MSG("?J out of range (0.01 to 0.3)");
+    }
+  #endif
+  #if HAS_CLASSIC_JERK
+    if (parser.seen('X')) planner.set_max_jerk(X_AXIS, parser.value_linear_units());
+    if (parser.seen('Y')) planner.set_max_jerk(Y_AXIS, parser.value_linear_units());
+    if (parser.seen('Z')) {
+      planner.set_max_jerk(Z_AXIS, parser.value_linear_units());
+      #if HAS_MESH && DISABLED(LIMITED_JERK_EDITING)
+        if (planner.max_jerk.z <= 0.1f)
+          SERIAL_ECHOLNPGM("WARNING! Low Z Jerk may lead to unwanted pauses.");
+      #endif
+    }
+    #if HAS_CLASSIC_E_JERK
+      if (parser.seen('E')) planner.set_max_jerk(E_AXIS, parser.value_linear_units());
+    #endif
+  #endif
+}
diff --git a/Marlin/src/gcode/config/M217.cpp b/Marlin/src/gcode/config/M217.cpp
new file mode 100644
index 0000000..f2fefb5
--- /dev/null
+++ b/Marlin/src/gcode/config/M217.cpp
@@ -0,0 +1,171 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if HAS_MULTI_EXTRUDER
+
+#include "../gcode.h"
+#include "../../module/tool_change.h"
+
+#if ENABLED(TOOLCHANGE_MIGRATION_FEATURE)
+  #include "../../module/motion.h"
+#endif
+
+#include "../../MarlinCore.h" // for SP_X_STR, etc.
+
+void M217_report(const bool eeprom=false) {
+
+  #if ENABLED(TOOLCHANGE_FILAMENT_SWAP)
+    serialprintPGM(eeprom ? PSTR("  M217") : PSTR("Toolchange:"));
+    SERIAL_ECHOPAIR(" S", LINEAR_UNIT(toolchange_settings.swap_length));
+    SERIAL_ECHOPAIR_P(SP_B_STR, LINEAR_UNIT(toolchange_settings.extra_resume),
+                      SP_E_STR, LINEAR_UNIT(toolchange_settings.extra_prime),
+                      SP_P_STR, LINEAR_UNIT(toolchange_settings.prime_speed));
+    SERIAL_ECHOPAIR(" R", LINEAR_UNIT(toolchange_settings.retract_speed),
+                    " U", LINEAR_UNIT(toolchange_settings.unretract_speed),
+                    " F", toolchange_settings.fan_speed,
+                    " G", toolchange_settings.fan_time);
+
+    #if ENABLED(TOOLCHANGE_MIGRATION_FEATURE)
+      SERIAL_ECHOPAIR(" A", int(migration.automode));
+      SERIAL_ECHOPAIR(" L", LINEAR_UNIT(migration.last));
+    #endif
+
+    #if ENABLED(TOOLCHANGE_PARK)
+      SERIAL_ECHOPAIR(" W", LINEAR_UNIT(toolchange_settings.enable_park));
+      SERIAL_ECHOPAIR_P(SP_X_STR, LINEAR_UNIT(toolchange_settings.change_point.x));
+      SERIAL_ECHOPAIR_P(SP_Y_STR, LINEAR_UNIT(toolchange_settings.change_point.y));
+    #endif
+
+    #if ENABLED(TOOLCHANGE_FS_PRIME_FIRST_USED)
+      SERIAL_ECHOPAIR(" V", LINEAR_UNIT(enable_first_prime));
+    #endif
+
+  #else
+
+    UNUSED(eeprom);
+
+  #endif
+
+  SERIAL_ECHOPAIR_P(SP_Z_STR, LINEAR_UNIT(toolchange_settings.z_raise));
+  SERIAL_EOL();
+}
+
+/**
+ * M217 - Set SINGLENOZZLE toolchange parameters
+ *
+ *  // Tool change command
+ *  Q           Prime active tool and exit
+ *
+ *  // Tool change settings
+ *  S[linear]   Swap length
+ *  B[linear]   Extra Swap length
+ *  E[linear]   Prime length
+ *  P[linear/m] Prime speed
+ *  R[linear/m] Retract speed
+ *  U[linear/m] UnRetract speed
+ *  V[linear]   0/1 Enable auto prime first extruder used
+ *  W[linear]   0/1 Enable park & Z Raise
+ *  X[linear]   Park X (Requires TOOLCHANGE_PARK)
+ *  Y[linear]   Park Y (Requires TOOLCHANGE_PARK)
+ *  Z[linear]   Z Raise
+ *  F[linear]   Fan Speed 0-255
+ *  G[linear/s] Fan time
+ *
+ * Tool migration settings
+ *  A[0|1]      Enable auto-migration on runout
+ *  L[index]    Last extruder to use for auto-migration
+ *
+ * Tool migration command
+ *  T[index]    Migrate to next extruder or the given extruder
+ */
+void GcodeSuite::M217() {
+
+  #if ENABLED(TOOLCHANGE_FILAMENT_SWAP)
+
+    static constexpr float max_extrude = TERN(PREVENT_LENGTHY_EXTRUDE, EXTRUDE_MAXLENGTH, 500);
+
+    if (parser.seen('Q')) { tool_change_prime(); return; }
+
+    if (parser.seenval('S')) { const float v = parser.value_linear_units(); toolchange_settings.swap_length = constrain(v, 0, max_extrude); }
+    if (parser.seenval('B')) { const float v = parser.value_linear_units(); toolchange_settings.extra_resume = constrain(v, -10, 10); }
+    if (parser.seenval('E')) { const float v = parser.value_linear_units(); toolchange_settings.extra_prime = constrain(v, 0, max_extrude); }
+    if (parser.seenval('P')) { const int16_t v = parser.value_linear_units(); toolchange_settings.prime_speed = constrain(v, 10, 5400); }
+    if (parser.seenval('R')) { const int16_t v = parser.value_linear_units(); toolchange_settings.retract_speed = constrain(v, 10, 5400); }
+    if (parser.seenval('U')) { const int16_t v = parser.value_linear_units(); toolchange_settings.unretract_speed = constrain(v, 10, 5400); }
+    #if TOOLCHANGE_FS_FAN >= 0 && HAS_FAN
+      if (parser.seenval('F')) { const int16_t v = parser.value_linear_units(); toolchange_settings.fan_speed = constrain(v, 0, 255); }
+      if (parser.seenval('G')) { const int16_t v = parser.value_linear_units(); toolchange_settings.fan_time = constrain(v, 1, 30); }
+    #endif
+  #endif
+
+  #if ENABLED(TOOLCHANGE_FS_PRIME_FIRST_USED)
+    if (parser.seenval('V')) { enable_first_prime = parser.value_linear_units(); }
+  #endif
+
+  #if ENABLED(TOOLCHANGE_PARK)
+    if (parser.seenval('W')) { toolchange_settings.enable_park = parser.value_linear_units(); }
+    if (parser.seenval('X')) { const int16_t v = parser.value_linear_units(); toolchange_settings.change_point.x = constrain(v, X_MIN_POS, X_MAX_POS); }
+    if (parser.seenval('Y')) { const int16_t v = parser.value_linear_units(); toolchange_settings.change_point.y = constrain(v, Y_MIN_POS, Y_MAX_POS); }
+  #endif
+
+  if (parser.seenval('Z')) { toolchange_settings.z_raise = parser.value_linear_units(); }
+
+  #if ENABLED(TOOLCHANGE_MIGRATION_FEATURE)
+    migration.target = 0;       // 0 = disabled
+
+    if (parser.seenval('L')) {  // Last
+      const int16_t lval = parser.value_int();
+      if (WITHIN(lval, 0, EXTRUDERS - 1)) {
+        migration.last = lval;
+        migration.automode = (active_extruder < migration.last);
+      }
+    }
+
+    if (parser.seen('A'))       // Auto on/off
+      migration.automode = parser.value_bool();
+
+    if (parser.seen('T')) {     // Migrate now
+      if (parser.has_value()) {
+        const int16_t tval = parser.value_int();
+        if (WITHIN(tval, 0, EXTRUDERS - 1) && tval != active_extruder) {
+          migration.target = tval + 1;
+          extruder_migration();
+          migration.target = 0; // disable
+          return;
+        }
+        else
+          migration.target = 0; // disable
+      }
+      else {
+        extruder_migration();
+        return;
+      }
+    }
+
+  #endif
+
+  M217_report();
+}
+
+#endif // HAS_MULTI_EXTRUDER
diff --git a/Marlin/src/gcode/config/M218.cpp b/Marlin/src/gcode/config/M218.cpp
new file mode 100644
index 0000000..7701320
--- /dev/null
+++ b/Marlin/src/gcode/config/M218.cpp
@@ -0,0 +1,71 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_HOTEND_OFFSET
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+
+#if ENABLED(DELTA)
+  #include "../../module/planner.h"
+#endif
+
+/**
+ * M218 - set hotend offset (in linear units)
+ *
+ *   T<tool>
+ *   X<xoffset>
+ *   Y<yoffset>
+ *   Z<zoffset>
+ */
+void GcodeSuite::M218() {
+
+  const int8_t target_extruder = get_target_extruder_from_command();
+  if (target_extruder < 0) return;
+
+  if (parser.seenval('X')) hotend_offset[target_extruder].x = parser.value_linear_units();
+  if (parser.seenval('Y')) hotend_offset[target_extruder].y = parser.value_linear_units();
+  if (parser.seenval('Z')) hotend_offset[target_extruder].z = parser.value_linear_units();
+
+  if (!parser.seen("XYZ")) {
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM(STR_HOTEND_OFFSET);
+    HOTEND_LOOP() {
+      SERIAL_CHAR(' ');
+      SERIAL_ECHO(hotend_offset[e].x);
+      SERIAL_CHAR(',');
+      SERIAL_ECHO(hotend_offset[e].y);
+      SERIAL_CHAR(',');
+      SERIAL_ECHO_F(hotend_offset[e].z, 3);
+    }
+    SERIAL_EOL();
+  }
+
+  #if ENABLED(DELTA)
+    if (target_extruder == active_extruder)
+      do_blocking_move_to_xy(current_position, planner.settings.max_feedrate_mm_s[X_AXIS]);
+  #endif
+}
+
+#endif // HAS_HOTEND_OFFSET
diff --git a/Marlin/src/gcode/config/M220.cpp b/Marlin/src/gcode/config/M220.cpp
new file mode 100644
index 0000000..75339f1
--- /dev/null
+++ b/Marlin/src/gcode/config/M220.cpp
@@ -0,0 +1,51 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+
+/**
+ * M220: Set speed percentage factor, aka "Feed Rate"
+ *
+ * Parameters
+ *   S<percent> : Set the feed rate percentage factor
+ *
+ * Report the current speed percentage factor if no parameter is specified
+ *
+ * For MMU2 and MMU2S devices...
+ *   B : Flag to back up the current factor
+ *   R : Flag to restore the last-saved factor
+ */
+void GcodeSuite::M220() {
+
+  static int16_t backup_feedrate_percentage = 100;
+  if (parser.seen('B')) backup_feedrate_percentage = feedrate_percentage;
+  if (parser.seen('R')) feedrate_percentage = backup_feedrate_percentage;
+
+  if (parser.seenval('S')) feedrate_percentage = parser.value_int();
+
+  if (!parser.seen_any()) {
+    SERIAL_ECHOPAIR("FR:", feedrate_percentage);
+    SERIAL_CHAR('%');
+    SERIAL_EOL();
+  }
+}
diff --git a/Marlin/src/gcode/config/M221.cpp b/Marlin/src/gcode/config/M221.cpp
new file mode 100644
index 0000000..7ba22d1
--- /dev/null
+++ b/Marlin/src/gcode/config/M221.cpp
@@ -0,0 +1,47 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../module/planner.h"
+
+#if EXTRUDERS
+
+/**
+ * M221: Set extrusion percentage (M221 T0 S95)
+ */
+void GcodeSuite::M221() {
+
+  const int8_t target_extruder = get_target_extruder_from_command();
+  if (target_extruder < 0) return;
+
+  if (parser.seenval('S'))
+    planner.set_flow(target_extruder, parser.value_int());
+  else {
+    SERIAL_ECHO_START();
+    SERIAL_CHAR('E', '0' + target_extruder);
+    SERIAL_ECHOPAIR(" Flow: ", planner.flow_percentage[target_extruder]);
+    SERIAL_CHAR('%');
+    SERIAL_EOL();
+  }
+}
+
+#endif // EXTRUDERS
diff --git a/Marlin/src/gcode/config/M281.cpp b/Marlin/src/gcode/config/M281.cpp
new file mode 100644
index 0000000..018ca1c
--- /dev/null
+++ b/Marlin/src/gcode/config/M281.cpp
@@ -0,0 +1,68 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(EDITABLE_SERVO_ANGLES)
+
+#include "../gcode.h"
+#include "../../module/servo.h"
+
+/**
+ * M281 - Edit / Report Servo Angles
+ *
+ *  P<index> - Servo to update
+ *  L<angle> - Deploy Angle
+ *  U<angle> - Stowed Angle
+ */
+void GcodeSuite::M281() {
+  if (!parser.seenval('P')) return;
+  const int servo_index = parser.value_int();
+  if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) {
+    #if ENABLED(BLTOUCH)
+      if (servo_index == Z_PROBE_SERVO_NR) {
+        SERIAL_ERROR_MSG("BLTouch angles can't be changed.");
+        return;
+      }
+    #endif
+    bool angle_change = false;
+    if (parser.seen('L')) {
+      servo_angles[servo_index][0] = parser.value_int();
+      angle_change = true;
+    }
+    if (parser.seen('U')) {
+      servo_angles[servo_index][1] = parser.value_int();
+      angle_change = true;
+    }
+    if (!angle_change) {
+      SERIAL_ECHO_START();
+      SERIAL_ECHOLNPAIR(" Servo ", servo_index,
+                        " L", servo_angles[servo_index][0],
+                        " U", servo_angles[servo_index][1]);
+    }
+  }
+  else {
+    SERIAL_ERROR_START();
+    SERIAL_ECHOLNPAIR("Servo ", servo_index, " out of range");
+  }
+}
+
+#endif // EDITABLE_SERVO_ANGLES
diff --git a/Marlin/src/gcode/config/M301.cpp b/Marlin/src/gcode/config/M301.cpp
new file mode 100644
index 0000000..7b3f576
--- /dev/null
+++ b/Marlin/src/gcode/config/M301.cpp
@@ -0,0 +1,91 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(PIDTEMP)
+
+#include "../gcode.h"
+#include "../../module/temperature.h"
+
+/**
+ * M301: Set PID parameters P I D (and optionally C, L)
+ *
+ *   E[extruder] Default: 0
+ *
+ *   P[float] Kp term
+ *   I[float] Ki term (unscaled)
+ *   D[float] Kd term (unscaled)
+ *
+ * With PID_EXTRUSION_SCALING:
+ *
+ *   C[float] Kc term
+ *   L[int] LPQ length
+ *
+ * With PID_FAN_SCALING:
+ *
+ *   F[float] Kf term
+ */
+void GcodeSuite::M301() {
+
+  // multi-extruder PID patch: M301 updates or prints a single extruder's PID values
+  // default behavior (omitting E parameter) is to update for extruder 0 only
+  const uint8_t e = parser.byteval('E'); // extruder being updated
+
+  if (e < HOTENDS) { // catch bad input value
+    if (parser.seen('P')) PID_PARAM(Kp, e) = parser.value_float();
+    if (parser.seen('I')) PID_PARAM(Ki, e) = scalePID_i(parser.value_float());
+    if (parser.seen('D')) PID_PARAM(Kd, e) = scalePID_d(parser.value_float());
+    #if ENABLED(PID_EXTRUSION_SCALING)
+      if (parser.seen('C')) PID_PARAM(Kc, e) = parser.value_float();
+      if (parser.seenval('L')) thermalManager.lpq_len = parser.value_int();
+      NOMORE(thermalManager.lpq_len, LPQ_MAX_LEN);
+      NOLESS(thermalManager.lpq_len, 0);
+    #endif
+
+    #if ENABLED(PID_FAN_SCALING)
+      if (parser.seen('F')) PID_PARAM(Kf, e) = parser.value_float();
+    #endif
+
+    thermalManager.updatePID();
+
+    SERIAL_ECHO_START();
+    #if ENABLED(PID_PARAMS_PER_HOTEND)
+      SERIAL_ECHOPAIR(" e:", e); // specify extruder in serial output
+    #endif
+    SERIAL_ECHOPAIR(" p:", PID_PARAM(Kp, e),
+                    " i:", unscalePID_i(PID_PARAM(Ki, e)),
+                    " d:", unscalePID_d(PID_PARAM(Kd, e)));
+    #if ENABLED(PID_EXTRUSION_SCALING)
+      SERIAL_ECHOPAIR(" c:", PID_PARAM(Kc, e));
+    #endif
+    #if ENABLED(PID_FAN_SCALING)
+      SERIAL_ECHOPAIR(" f:", PID_PARAM(Kf, e));
+    #endif
+
+    SERIAL_EOL();
+  }
+  else
+    SERIAL_ERROR_MSG(STR_INVALID_EXTRUDER);
+}
+
+#endif // PIDTEMP
diff --git a/Marlin/src/gcode/config/M302.cpp b/Marlin/src/gcode/config/M302.cpp
new file mode 100644
index 0000000..afdc6c9
--- /dev/null
+++ b/Marlin/src/gcode/config/M302.cpp
@@ -0,0 +1,63 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(PREVENT_COLD_EXTRUSION)
+
+#include "../gcode.h"
+#include "../../module/temperature.h"
+
+/**
+ * M302: Allow cold extrudes, or set the minimum extrude temperature
+ *
+ *       S<temperature> sets the minimum extrude temperature
+ *       P<bool> enables (1) or disables (0) cold extrusion
+ *
+ *  Examples:
+ *
+ *       M302         ; report current cold extrusion state
+ *       M302 P0      ; enable cold extrusion checking
+ *       M302 P1      ; disable cold extrusion checking
+ *       M302 S0      ; always allow extrusion (disables checking)
+ *       M302 S170    ; only allow extrusion above 170
+ *       M302 S170 P1 ; set min extrude temp to 170 but leave disabled
+ */
+void GcodeSuite::M302() {
+  const bool seen_S = parser.seen('S');
+  if (seen_S) {
+    thermalManager.extrude_min_temp = parser.value_celsius();
+    thermalManager.allow_cold_extrude = (thermalManager.extrude_min_temp == 0);
+  }
+
+  if (parser.seen('P'))
+    thermalManager.allow_cold_extrude = (thermalManager.extrude_min_temp == 0) || parser.value_bool();
+  else if (!seen_S) {
+    // Report current state
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM("Cold extrudes are ");
+    serialprintPGM(thermalManager.allow_cold_extrude ? PSTR("en") : PSTR("dis"));
+    SERIAL_ECHOLNPAIR("abled (min temp ", thermalManager.extrude_min_temp, "C)");
+  }
+}
+
+#endif // PREVENT_COLD_EXTRUSION
diff --git a/Marlin/src/gcode/config/M304.cpp b/Marlin/src/gcode/config/M304.cpp
new file mode 100644
index 0000000..10739be
--- /dev/null
+++ b/Marlin/src/gcode/config/M304.cpp
@@ -0,0 +1,48 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(PIDTEMPBED)
+
+#include "../gcode.h"
+#include "../../module/temperature.h"
+
+/**
+ * M304 - Set and/or Report the current Bed PID values
+ *
+ *  P<pval> - Set the P value
+ *  I<ival> - Set the I value
+ *  D<dval> - Set the D value
+ */
+void GcodeSuite::M304() {
+  if (parser.seen('P')) thermalManager.temp_bed.pid.Kp = parser.value_float();
+  if (parser.seen('I')) thermalManager.temp_bed.pid.Ki = scalePID_i(parser.value_float());
+  if (parser.seen('D')) thermalManager.temp_bed.pid.Kd = scalePID_d(parser.value_float());
+
+  SERIAL_ECHO_START();
+  SERIAL_ECHOLNPAIR(" p:", thermalManager.temp_bed.pid.Kp,
+                    " i:", unscalePID_i(thermalManager.temp_bed.pid.Ki),
+                    " d:", unscalePID_d(thermalManager.temp_bed.pid.Kd));
+}
+
+#endif // PIDTEMPBED
diff --git a/Marlin/src/gcode/config/M305.cpp b/Marlin/src/gcode/config/M305.cpp
new file mode 100644
index 0000000..3e7206a
--- /dev/null
+++ b/Marlin/src/gcode/config/M305.cpp
@@ -0,0 +1,81 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_USER_THERMISTORS
+
+#include "../gcode.h"
+#include "../../module/temperature.h"
+
+/**
+ * M305: Set (or report) custom thermistor parameters
+ *
+ *   P[index]  Thermistor table index
+ *   R[ohms]   Pullup resistor value
+ *   T[ohms]   Resistance at 25C
+ *   B[beta]   Thermistor "beta" value
+ *   C[coeff]  Steinhart-Hart Coefficient 'C'
+ *
+ *   Format: M305 P[tbl_index] R[pullup_resistor_val] T[therm_25C_resistance] B[therm_beta] C[Steinhart_Hart_C_coeff]
+ *
+ * Examples: M305 P0 R4700 T100000 B3950 C0.0
+ *           M305 P0 R4700
+ *           M305 P0 T100000
+ *           M305 P0 B3950
+ *           M305 P0 C0.0
+ */
+void GcodeSuite::M305() {
+  const int8_t t_index = parser.intval('P', -1);
+  const bool do_set = parser.seen("BCRT");
+
+  // A valid P index is required
+  if (t_index >= (USER_THERMISTORS) || (do_set && t_index < 0)) {
+    SERIAL_ECHO_START();
+    SERIAL_ECHOLNPAIR("!Invalid index. (0 <= P <= ", int(USER_THERMISTORS - 1), ")");
+  }
+  else if (do_set) {
+    if (parser.seen('R')) // Pullup resistor value
+      if (!thermalManager.set_pull_up_res(t_index, parser.value_float()))
+        SERIAL_ECHO_MSG("!Invalid series resistance. (0 < R < 1000000)");
+
+    if (parser.seen('T')) // Resistance at 25C
+      if (!thermalManager.set_res25(t_index, parser.value_float()))
+        SERIAL_ECHO_MSG("!Invalid 25C resistance. (0 < T < 10000000)");
+
+    if (parser.seen('B')) // Beta value
+      if (!thermalManager.set_beta(t_index, parser.value_float()))
+        SERIAL_ECHO_MSG("!Invalid beta. (0 < B < 1000000)");
+
+    if (parser.seen('C')) // Steinhart-Hart C coefficient
+      if (!thermalManager.set_sh_coeff(t_index, parser.value_float()))
+        SERIAL_ECHO_MSG("!Invalid Steinhart-Hart C coeff. (-0.01 < C < +0.01)");
+  }                       // If not setting then report parameters
+  else if (t_index < 0) { // ...all user thermistors
+    LOOP_L_N(i, USER_THERMISTORS)
+      thermalManager.log_user_thermistor(i);
+  }
+  else                    // ...one user thermistor
+    thermalManager.log_user_thermistor(t_index);
+}
+
+#endif // HAS_USER_THERMISTORS
diff --git a/Marlin/src/gcode/config/M43.cpp b/Marlin/src/gcode/config/M43.cpp
new file mode 100644
index 0000000..005fdf0
--- /dev/null
+++ b/Marlin/src/gcode/config/M43.cpp
@@ -0,0 +1,386 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(PINS_DEBUGGING)
+
+#include "../gcode.h"
+#include "../../MarlinCore.h" // for pin_is_protected
+#include "../../pins/pinsDebug.h"
+#include "../../module/endstops.h"
+
+#if HAS_Z_SERVO_PROBE
+  #include "../../module/probe.h"
+  #include "../../module/servo.h"
+#endif
+
+#if ENABLED(BLTOUCH)
+  #include "../../feature/bltouch.h"
+#endif
+
+#if ENABLED(HOST_PROMPT_SUPPORT)
+  #include "../../feature/host_actions.h"
+#endif
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../lcd/extui/ui_api.h"
+#endif
+
+#if HAS_RESUME_CONTINUE
+  #include "../../lcd/marlinui.h"
+#endif
+
+#ifndef GET_PIN_MAP_PIN_M43
+  #define GET_PIN_MAP_PIN_M43(Q) GET_PIN_MAP_PIN(Q)
+#endif
+
+inline void toggle_pins() {
+  const bool ignore_protection = parser.boolval('I');
+  const int repeat = parser.intval('R', 1),
+            start = PARSED_PIN_INDEX('S', 0),
+            end = PARSED_PIN_INDEX('L', NUM_DIGITAL_PINS - 1),
+            wait = parser.intval('W', 500);
+
+  LOOP_S_LE_N(i, start, end) {
+    pin_t pin = GET_PIN_MAP_PIN_M43(i);
+    if (!VALID_PIN(pin)) continue;
+    if (M43_NEVER_TOUCH(i) || (!ignore_protection && pin_is_protected(pin))) {
+      report_pin_state_extended(pin, ignore_protection, true, PSTR("Untouched "));
+      SERIAL_EOL();
+    }
+    else {
+      watchdog_refresh();
+      report_pin_state_extended(pin, ignore_protection, true, PSTR("Pulsing   "));
+      #ifdef __STM32F1__
+        const auto prior_mode = _GET_MODE(i);
+      #else
+        const bool prior_mode = GET_PINMODE(pin);
+      #endif
+      #if AVR_AT90USB1286_FAMILY // Teensy IDEs don't know about these pins so must use FASTIO
+        if (pin == TEENSY_E2) {
+          SET_OUTPUT(TEENSY_E2);
+          for (int16_t j = 0; j < repeat; j++) {
+            WRITE(TEENSY_E2, LOW);  safe_delay(wait);
+            WRITE(TEENSY_E2, HIGH); safe_delay(wait);
+            WRITE(TEENSY_E2, LOW);  safe_delay(wait);
+          }
+        }
+        else if (pin == TEENSY_E3) {
+          SET_OUTPUT(TEENSY_E3);
+          for (int16_t j = 0; j < repeat; j++) {
+            WRITE(TEENSY_E3, LOW);  safe_delay(wait);
+            WRITE(TEENSY_E3, HIGH); safe_delay(wait);
+            WRITE(TEENSY_E3, LOW);  safe_delay(wait);
+          }
+        }
+        else
+      #endif
+      {
+        pinMode(pin, OUTPUT);
+        for (int16_t j = 0; j < repeat; j++) {
+          watchdog_refresh(); extDigitalWrite(pin, 0); safe_delay(wait);
+          watchdog_refresh(); extDigitalWrite(pin, 1); safe_delay(wait);
+          watchdog_refresh(); extDigitalWrite(pin, 0); safe_delay(wait);
+          watchdog_refresh();
+        }
+      }
+      #ifdef __STM32F1__
+        _SET_MODE(i, prior_mode);
+      #else
+        pinMode(pin, prior_mode);
+      #endif
+    }
+    SERIAL_EOL();
+  }
+  SERIAL_ECHOLNPGM("Done.");
+
+} // toggle_pins
+
+inline void servo_probe_test() {
+
+  #if !(NUM_SERVOS > 0 && HAS_SERVO_0)
+
+    SERIAL_ERROR_MSG("SERVO not set up.");
+
+  #elif !HAS_Z_SERVO_PROBE
+
+    SERIAL_ERROR_MSG("Z_PROBE_SERVO_NR not set up.");
+
+  #else // HAS_Z_SERVO_PROBE
+
+    const uint8_t probe_index = parser.byteval('P', Z_PROBE_SERVO_NR);
+
+    SERIAL_ECHOLNPAIR("Servo probe test\n"
+                      ". using index:  ", int(probe_index),
+                      ", deploy angle: ", servo_angles[probe_index][0],
+                      ", stow angle:   ", servo_angles[probe_index][1]
+    );
+
+    bool deploy_state = false, stow_state;
+
+    #if ENABLED(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN)
+
+      #define PROBE_TEST_PIN Z_MIN_PIN
+      constexpr bool probe_inverting = Z_MIN_ENDSTOP_INVERTING;
+
+      SERIAL_ECHOLNPAIR(". Probe Z_MIN_PIN: ", int(PROBE_TEST_PIN));
+      SERIAL_ECHOPGM(". Z_MIN_ENDSTOP_INVERTING: ");
+
+    #else
+
+      #define PROBE_TEST_PIN Z_MIN_PROBE_PIN
+      constexpr bool probe_inverting = Z_MIN_PROBE_ENDSTOP_INVERTING;
+
+      SERIAL_ECHOLNPAIR(". Probe Z_MIN_PROBE_PIN: ", int(PROBE_TEST_PIN));
+      SERIAL_ECHOPGM(   ". Z_MIN_PROBE_ENDSTOP_INVERTING: ");
+
+    #endif
+
+    serialprint_truefalse(probe_inverting);
+    SERIAL_EOL();
+
+    SET_INPUT_PULLUP(PROBE_TEST_PIN);
+
+    // First, check for a probe that recognizes an advanced BLTouch sequence.
+    // In addition to STOW and DEPLOY, it uses SW MODE (and RESET in the beginning)
+    // to see if this is one of the following: BLTOUCH Classic 1.2, 1.3,  or
+    // BLTouch Smart 1.0, 2.0, 2.2, 3.0, 3.1. But only if the user has actually
+    // configured a BLTouch as being present. If the user has not configured this,
+    // the BLTouch will be detected in the last phase of these tests (see further on).
+    bool blt = false;
+    // This code will try to detect a BLTouch probe or clone
+    #if ENABLED(BLTOUCH)
+      SERIAL_ECHOLNPGM(". Check for BLTOUCH");
+      bltouch._reset();
+      bltouch._stow();
+      if (probe_inverting == READ(PROBE_TEST_PIN)) {
+        bltouch._set_SW_mode();
+        if (probe_inverting != READ(PROBE_TEST_PIN)) {
+          bltouch._deploy();
+          if (probe_inverting == READ(PROBE_TEST_PIN)) {
+            bltouch._stow();
+            SERIAL_ECHOLNPGM("= BLTouch Classic 1.2, 1.3, Smart 1.0, 2.0, 2.2, 3.0, 3.1 detected.");
+            // Check for a 3.1 by letting the user trigger it, later
+            blt = true;
+        }
+      }
+    }
+    #endif
+
+    // The following code is common to all kinds of servo probes.
+    // Since it could be a real servo or a BLTouch (any kind) or a clone,
+    // use only "common" functions - i.e. SERVO_MOVE. No bltouch.xxxx stuff.
+
+    // If it is already recognised as a being a BLTouch, no need for this test
+    if (!blt) {
+      // DEPLOY and STOW 4 times and see if the signal follows
+      // Then it is a mechanical switch
+      uint8_t i = 0;
+      SERIAL_ECHOLNPGM(". Deploy & stow 4 times");
+      do {
+        MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][0]); // Deploy
+        safe_delay(500);
+        deploy_state = READ(PROBE_TEST_PIN);
+        MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][1]); // Stow
+        safe_delay(500);
+        stow_state = READ(PROBE_TEST_PIN);
+      } while (++i < 4);
+
+      if (probe_inverting != deploy_state) SERIAL_ECHOLNPGM("WARNING: INVERTING setting probably backwards.");
+
+      if (deploy_state != stow_state) {
+        SERIAL_ECHOLNPGM("= Mechanical Switch detected");
+        if (deploy_state) {
+          SERIAL_ECHOLNPAIR("  DEPLOYED state: HIGH (logic 1)",
+                            "  STOWED (triggered) state: LOW (logic 0)");
+        }
+        else {
+          SERIAL_ECHOLNPAIR("  DEPLOYED state: LOW (logic 0)",
+                            "  STOWED (triggered) state: HIGH (logic 1)");
+        }
+        #if ENABLED(BLTOUCH)
+          SERIAL_ECHOLNPGM("FAIL: BLTOUCH enabled - Set up this device as a Servo Probe with INVERTING set to 'true'.");
+        #endif
+        return;
+      }
+    }
+
+    // Ask the user for a trigger event and measure the pulse width.
+    MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][0]); // Deploy
+    safe_delay(500);
+    SERIAL_ECHOLNPGM("** Please trigger probe within 30 sec **");
+    uint16_t probe_counter = 0;
+
+    // Wait 30 seconds for user to trigger probe
+    for (uint16_t j = 0; j < 500 * 30 && probe_counter == 0 ; j++) {
+      safe_delay(2);
+
+      if (0 == j % (500 * 1)) gcode.reset_stepper_timeout();    // Keep steppers powered
+
+      if (deploy_state != READ(PROBE_TEST_PIN)) {               // probe triggered
+        for (probe_counter = 0; probe_counter < 15 && deploy_state != READ(PROBE_TEST_PIN); ++probe_counter) safe_delay(2);
+
+        SERIAL_ECHOPGM(". Pulse width");
+        if (probe_counter == 15)
+          SERIAL_ECHOLNPGM(": 30ms or more");
+        else
+          SERIAL_ECHOLNPAIR(" (+/- 4ms): ", probe_counter * 2);
+
+        if (probe_counter >= 4) {
+          if (probe_counter == 15) {
+            if (blt) SERIAL_ECHOPGM("= BLTouch V3.1");
+            else     SERIAL_ECHOPGM("= Z Servo Probe");
+          }
+          else SERIAL_ECHOPGM("= BLTouch pre V3.1 (or compatible)");
+          SERIAL_ECHOLNPGM(" detected.");
+        }
+        else SERIAL_ECHOLNPGM("FAIL: Noise detected - please re-run test");
+
+        MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][1]); // Stow
+        return;
+      }
+    }
+
+    if (!probe_counter) SERIAL_ECHOLNPGM("FAIL: No trigger detected");
+
+  #endif // HAS_Z_SERVO_PROBE
+
+} // servo_probe_test
+
+/**
+ * M43: Pin debug - report pin state, watch pins, toggle pins and servo probe test/report
+ *
+ *  M43         - report name and state of pin(s)
+ *                  P<pin>  Pin to read or watch. If omitted, reads all pins.
+ *                  I       Flag to ignore Marlin's pin protection.
+ *
+ *  M43 W       - Watch pins -reporting changes- until reset, click, or M108.
+ *                  P<pin>  Pin to read or watch. If omitted, read/watch all pins.
+ *                  I       Flag to ignore Marlin's pin protection.
+ *
+ *  M43 E<bool> - Enable / disable background endstop monitoring
+ *                  - Machine continues to operate
+ *                  - Reports changes to endstops
+ *                  - Toggles LED_PIN when an endstop changes
+ *                  - Cannot reliably catch the 5mS pulse from BLTouch type probes
+ *
+ *  M43 T       - Toggle pin(s) and report which pin is being toggled
+ *                  S<pin>  - Start Pin number.   If not given, will default to 0
+ *                  L<pin>  - End Pin number.   If not given, will default to last pin defined for this board
+ *                  I<bool> - Flag to ignore Marlin's pin protection.   Use with caution!!!!
+ *                  R       - Repeat pulses on each pin this number of times before continueing to next pin
+ *                  W       - Wait time (in miliseconds) between pulses.  If not given will default to 500
+ *
+ *  M43 S       - Servo probe test
+ *                  P<index> - Probe index (optional - defaults to 0
+ */
+void GcodeSuite::M43() {
+
+  // 'T' must be first. It uses 'S' and 'E' differently.
+  if (parser.seen('T')) return toggle_pins();
+
+  // 'E' Enable or disable endstop monitoring and return
+  if (parser.seen('E')) {
+    endstops.monitor_flag = parser.value_bool();
+    SERIAL_ECHOPGM("endstop monitor ");
+    serialprintPGM(endstops.monitor_flag ? PSTR("en") : PSTR("dis"));
+    SERIAL_ECHOLNPGM("abled");
+    return;
+  }
+
+  // 'S' Run servo probe test and return
+  if (parser.seen('S')) return servo_probe_test();
+
+  // 'P' Get the range of pins to test or watch
+  uint8_t first_pin = PARSED_PIN_INDEX('P', 0),
+          last_pin = parser.seenval('P') ? first_pin : NUMBER_PINS_TOTAL - 1;
+
+  if (first_pin > last_pin) return;
+
+  // 'I' to ignore protected pins
+  const bool ignore_protection = parser.boolval('I');
+
+  // 'W' Watch until click, M108, or reset
+  if (parser.boolval('W')) {
+    SERIAL_ECHOLNPGM("Watching pins");
+    #ifdef ARDUINO_ARCH_SAM
+      NOLESS(first_pin, 2); // Don't hijack the UART pins
+    #endif
+    uint8_t pin_state[last_pin - first_pin + 1];
+    LOOP_S_LE_N(i, first_pin, last_pin) {
+      pin_t pin = GET_PIN_MAP_PIN_M43(i);
+      if (!VALID_PIN(pin)) continue;
+      if (M43_NEVER_TOUCH(i) || (!ignore_protection && pin_is_protected(pin))) continue;
+      pinMode(pin, INPUT_PULLUP);
+      delay(1);
+        /*
+        if (IS_ANALOG(pin))
+          pin_state[pin - first_pin] = analogRead(DIGITAL_PIN_TO_ANALOG_PIN(pin)); // int16_t pin_state[...]
+        else
+        //*/
+          pin_state[i - first_pin] = extDigitalRead(pin);
+    }
+
+    #if HAS_RESUME_CONTINUE
+      KEEPALIVE_STATE(PAUSED_FOR_USER);
+      wait_for_user = true;
+      TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, PSTR("M43 Wait Called"), CONTINUE_STR));
+      TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(PSTR("M43 Wait Called")));
+    #endif
+
+    for (;;) {
+      LOOP_S_LE_N(i, first_pin, last_pin) {
+        pin_t pin = GET_PIN_MAP_PIN_M43(i);
+        if (!VALID_PIN(pin)) continue;
+        if (M43_NEVER_TOUCH(i) || (!ignore_protection && pin_is_protected(pin))) continue;
+        const byte val =
+          /*
+          IS_ANALOG(pin)
+            ? analogRead(DIGITAL_PIN_TO_ANALOG_PIN(pin)) : // int16_t val
+            :
+          //*/
+            extDigitalRead(pin);
+        if (val != pin_state[i - first_pin]) {
+          report_pin_state_extended(pin, ignore_protection, false);
+          pin_state[i - first_pin] = val;
+        }
+      }
+
+      #if HAS_RESUME_CONTINUE
+        ui.update();
+        if (!wait_for_user) break;
+      #endif
+
+      safe_delay(200);
+    }
+  }
+  else {
+    // Report current state of selected pin(s)
+    LOOP_S_LE_N(i, first_pin, last_pin) {
+      pin_t pin = GET_PIN_MAP_PIN_M43(i);
+      if (VALID_PIN(pin)) report_pin_state_extended(pin, ignore_protection, true);
+    }
+  }
+}
+
+#endif // PINS_DEBUGGING
diff --git a/Marlin/src/gcode/config/M540.cpp b/Marlin/src/gcode/config/M540.cpp
new file mode 100644
index 0000000..54d52f3
--- /dev/null
+++ b/Marlin/src/gcode/config/M540.cpp
@@ -0,0 +1,40 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(SD_ABORT_ON_ENDSTOP_HIT)
+
+#include "../gcode.h"
+#include "../../module/stepper.h"
+
+/**
+ * M540: Set whether SD card print should abort on endstop hit (M540 S<0|1>)
+ */
+void GcodeSuite::M540() {
+
+  if (parser.seen('S'))
+    planner.abort_on_endstop_hit = parser.value_bool();
+
+}
+
+#endif // SD_ABORT_ON_ENDSTOP_HIT
diff --git a/Marlin/src/gcode/config/M575.cpp b/Marlin/src/gcode/config/M575.cpp
new file mode 100644
index 0000000..44723b7
--- /dev/null
+++ b/Marlin/src/gcode/config/M575.cpp
@@ -0,0 +1,75 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(BAUD_RATE_GCODE)
+
+#include "../gcode.h"
+
+/**
+ * M575 - Change serial baud rate
+ *
+ *   P<index>    - Serial port index. Omit for all.
+ *   B<baudrate> - Baud rate (bits per second)
+ */
+void GcodeSuite::M575() {
+  int32_t baud = parser.ulongval('B');
+  switch (baud) {
+    case   24:
+    case   96:
+    case  192:
+    case  384:
+    case  576:
+    case 1152: baud *= 100; break;
+    case  250:
+    case  500: baud *= 1000; break;
+    case   19: baud = 19200; break;
+    case   38: baud = 38400; break;
+    case   57: baud = 57600; break;
+    case  115: baud = 115200; break;
+  }
+  switch (baud) {
+    case 2400: case 9600: case 19200: case 38400: case 57600:
+    case 115200: case 250000: case 500000: case 1000000: {
+      const int8_t port = parser.intval('P', -99);
+      const bool set0 = (port == -99 || port == 0);
+      if (set0) SERIAL_ECHO_MSG(" Serial ", '0', " baud rate set to ", baud);
+      #if HAS_MULTI_SERIAL
+        const bool set1 = (port == -99 || port == 1);
+        if (set1) SERIAL_ECHO_MSG(" Serial ", '1', " baud rate set to ", baud);
+      #endif
+
+      SERIAL_FLUSH();
+
+      if (set0) { MYSERIAL0.end(); MYSERIAL0.begin(baud); }
+
+      #if HAS_MULTI_SERIAL
+        if (set1) { MYSERIAL1.end(); MYSERIAL1.begin(baud); }
+      #endif
+
+    } break;
+    default: SERIAL_ECHO_MSG("?(B)aud rate implausible.");
+  }
+}
+
+#endif // BAUD_RATE_GCODE
diff --git a/Marlin/src/gcode/config/M672.cpp b/Marlin/src/gcode/config/M672.cpp
new file mode 100644
index 0000000..af74230
--- /dev/null
+++ b/Marlin/src/gcode/config/M672.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 <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(DUET_SMART_EFFECTOR) && PIN_EXISTS(SMART_EFFECTOR_MOD)
+
+#include "../gcode.h"
+#include "../../HAL/shared/Delay.h"
+#include "../parser.h"
+
+/**
+ * The Marlin format for the M672 command is different than shown in the Duet Smart Effector
+ * documentation https://duet3d.dozuki.com/Wiki/Smart_effector_and_carriage_adapters_for_delta_printer
+ *
+ * To set custom sensitivity:
+ *   Duet:   M672 S105:aaa:bbb
+ *   Marlin: M672 Saaa
+ *
+ *   (where aaa is the desired sensitivity and bbb is 255 - aaa).
+ *
+ * Revert sensitivity to factory settings:
+ *   Duet:   M672 S105:131:131
+ *   Marlin: M672 R
+ */
+
+#define M672_PROGBYTE    105                // magic byte to start programming custom sensitivity
+#define M672_ERASEBYTE   131                // magic byte to clear custom sensitivity
+
+//
+// Smart Effector byte send protocol:
+//
+//  0  0  1  0       ... always 0010
+//  b7 b6 b5 b4 ~b4  ... hi bits, NOT last bit
+//  b3 b2 b1 b0 ~b0  ... lo bits, NOT last bit
+//
+void M672_send(uint8_t b) {    // bit rate requirement: 1KHz +/- 30%
+  LOOP_L_N(bits, 14) {
+    switch (bits) {
+      default: { OUT_WRITE(SMART_EFFECTOR_MOD_PIN, !!(b & 0x80)); b <<= 1; break; } // send bit, shift next into place
+      case  7:
+      case 12: { OUT_WRITE(SMART_EFFECTOR_MOD_PIN, !!(b & 0x80));          break; } // send bit. no shift
+      case  8:
+      case 13: { OUT_WRITE(SMART_EFFECTOR_MOD_PIN,  !(b & 0x80)); b <<= 1; break; } // send inverted previous bit
+      case  0: case  1:                                   // 00
+      case  3: { OUT_WRITE(SMART_EFFECTOR_MOD_PIN, LOW); break; }   // 0010
+      case  2: { OUT_WRITE(SMART_EFFECTOR_MOD_PIN, HIGH); break; }  // 001
+    }
+    DELAY_US(1000);
+  }
+}
+
+/**
+ * M672 - Set/reset Duet Smart Effector sensitivity
+ *
+ *  One of these is required:
+ *    S<sensitivity> - 0-255
+ *    R              - Flag to reset sensitivity to default
+ */
+void GcodeSuite::M672() {
+  if (parser.seen('R')) {
+    M672_send(M672_ERASEBYTE);
+    M672_send(M672_ERASEBYTE);
+  }
+  else if (parser.seenval('S')) {
+    const int8_t M672_sensitivity = parser.value_byte();
+    M672_send(M672_PROGBYTE);
+    M672_send(M672_sensitivity);
+    M672_send(255 - M672_sensitivity);
+  }
+  else {
+    SERIAL_ECHO_MSG("!'S' or 'R' parameter required.");
+    return;
+  }
+
+  OUT_WRITE(SMART_EFFECTOR_MOD_PIN, LOW);  // Keep Smart Effector in NORMAL mode
+}
+
+#endif // DUET_SMART_EFFECTOR && SMART_EFFECTOR_MOD_PIN
diff --git a/Marlin/src/gcode/config/M92.cpp b/Marlin/src/gcode/config/M92.cpp
new file mode 100644
index 0000000..0a7d52b
--- /dev/null
+++ b/Marlin/src/gcode/config/M92.cpp
@@ -0,0 +1,114 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../module/planner.h"
+
+void report_M92(const bool echo=true, const int8_t e=-1) {
+  if (echo) SERIAL_ECHO_START(); else SERIAL_CHAR(' ');
+  SERIAL_ECHOPAIR_P(PSTR(" M92 X"), LINEAR_UNIT(planner.settings.axis_steps_per_mm[X_AXIS]),
+                          SP_Y_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[Y_AXIS]),
+                          SP_Z_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[Z_AXIS]));
+  #if DISABLED(DISTINCT_E_FACTORS)
+    SERIAL_ECHOPAIR_P(SP_E_STR, VOLUMETRIC_UNIT(planner.settings.axis_steps_per_mm[E_AXIS]));
+  #endif
+  SERIAL_EOL();
+
+  #if ENABLED(DISTINCT_E_FACTORS)
+    LOOP_L_N(i, E_STEPPERS) {
+      if (e >= 0 && i != e) continue;
+      if (echo) SERIAL_ECHO_START(); else SERIAL_CHAR(' ');
+      SERIAL_ECHOLNPAIR_P(PSTR(" M92 T"), (int)i,
+                        SP_E_STR, VOLUMETRIC_UNIT(planner.settings.axis_steps_per_mm[E_AXIS_N(i)]));
+    }
+  #endif
+
+  UNUSED_E(e);
+}
+
+/**
+ * M92: Set axis steps-per-unit for one or more axes, X, Y, Z, and E.
+ *      (Follows the same syntax as G92)
+ *
+ *      With multiple extruders use T to specify which one.
+ *
+ *      If no argument is given print the current values.
+ *
+ *    With MAGIC_NUMBERS_GCODE:
+ *      Use 'H' and/or 'L' to get ideal layer-height information.
+ *      'H' specifies micro-steps to use. We guess if it's not supplied.
+ *      'L' specifies a desired layer height. Nearest good heights are shown.
+ */
+void GcodeSuite::M92() {
+
+  const int8_t target_extruder = get_target_extruder_from_command();
+  if (target_extruder < 0) return;
+
+  // No arguments? Show M92 report.
+  if (!parser.seen("XYZE"
+    #if ENABLED(MAGIC_NUMBERS_GCODE)
+      "HL"
+    #endif
+  )) return report_M92(true, target_extruder);
+
+  LOOP_XYZE(i) {
+    if (parser.seenval(axis_codes[i])) {
+      if (i == E_AXIS) {
+        const float value = parser.value_per_axis_units((AxisEnum)(E_AXIS_N(target_extruder)));
+        if (value < 20) {
+          float factor = planner.settings.axis_steps_per_mm[E_AXIS_N(target_extruder)] / value; // increase e constants if M92 E14 is given for netfab.
+          #if HAS_CLASSIC_JERK && HAS_CLASSIC_E_JERK
+            planner.max_jerk.e *= factor;
+          #endif
+          planner.settings.max_feedrate_mm_s[E_AXIS_N(target_extruder)] *= factor;
+          planner.max_acceleration_steps_per_s2[E_AXIS_N(target_extruder)] *= factor;
+        }
+        planner.settings.axis_steps_per_mm[E_AXIS_N(target_extruder)] = value;
+      }
+      else {
+        planner.settings.axis_steps_per_mm[i] = parser.value_per_axis_units((AxisEnum)i);
+      }
+    }
+  }
+  planner.refresh_positioning();
+
+  #if ENABLED(MAGIC_NUMBERS_GCODE)
+    #ifndef Z_MICROSTEPS
+      #define Z_MICROSTEPS 16
+    #endif
+    const float wanted = parser.floatval('L');
+    if (parser.seen('H') || wanted) {
+      const uint16_t argH = parser.ushortval('H'),
+                     micro_steps = argH ?: Z_MICROSTEPS;
+      const float z_full_step_mm = micro_steps * planner.steps_to_mm[Z_AXIS];
+      SERIAL_ECHO_START();
+      SERIAL_ECHOPAIR("{ micro_steps:", micro_steps, ", z_full_step_mm:", z_full_step_mm);
+      if (wanted) {
+        const float best = uint16_t(wanted / z_full_step_mm) * z_full_step_mm;
+        SERIAL_ECHOPAIR(", best:[", best);
+        if (best != wanted) { SERIAL_CHAR(','); SERIAL_DECIMAL(best + z_full_step_mm); }
+        SERIAL_CHAR(']');
+      }
+      SERIAL_ECHOLNPGM(" }");
+    }
+  #endif
+}
diff --git a/Marlin/src/gcode/control/M108_M112_M410.cpp b/Marlin/src/gcode/control/M108_M112_M410.cpp
new file mode 100644
index 0000000..309c806
--- /dev/null
+++ b/Marlin/src/gcode/control/M108_M112_M410.cpp
@@ -0,0 +1,56 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if DISABLED(EMERGENCY_PARSER)
+
+#include "../gcode.h"
+#include "../../MarlinCore.h" // for wait_for_heatup, kill, M112_KILL_STR
+#include "../../module/motion.h" // for quickstop_stepper
+
+/**
+ * M108: Stop the waiting for heaters in M109, M190, M303. Does not affect the target temperature.
+ */
+void GcodeSuite::M108() {
+  TERN_(HAS_RESUME_CONTINUE, wait_for_user = false);
+  wait_for_heatup = false;
+}
+
+/**
+ * M112: Full Shutdown
+ */
+void GcodeSuite::M112() {
+  kill(M112_KILL_STR, nullptr, true);
+}
+
+/**
+ * M410: Quickstop - Abort all planned moves
+ *
+ * This will stop the carriages mid-move, so most likely they
+ * will be out of sync with the stepper position after this.
+ */
+void GcodeSuite::M410() {
+  quickstop_stepper();
+}
+
+#endif // !EMERGENCY_PARSER
diff --git a/Marlin/src/gcode/control/M111.cpp b/Marlin/src/gcode/control/M111.cpp
new file mode 100644
index 0000000..38cb065
--- /dev/null
+++ b/Marlin/src/gcode/control/M111.cpp
@@ -0,0 +1,77 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+
+/**
+ * M111: Set the debug level
+ */
+void GcodeSuite::M111() {
+  if (parser.seen('S')) marlin_debug_flags = parser.byteval('S');
+
+  static PGMSTR(str_debug_1, STR_DEBUG_ECHO);
+  static PGMSTR(str_debug_2, STR_DEBUG_INFO);
+  static PGMSTR(str_debug_4, STR_DEBUG_ERRORS);
+  static PGMSTR(str_debug_8, STR_DEBUG_DRYRUN);
+  static PGMSTR(str_debug_16, STR_DEBUG_COMMUNICATION);
+  #if ENABLED(DEBUG_LEVELING_FEATURE)
+    static PGMSTR(str_debug_lvl, STR_DEBUG_LEVELING);
+  #endif
+
+  static PGM_P const debug_strings[] PROGMEM = {
+    str_debug_1, str_debug_2, str_debug_4, str_debug_8, str_debug_16,
+    TERN_(DEBUG_LEVELING_FEATURE, str_debug_lvl)
+  };
+
+  SERIAL_ECHO_START();
+  SERIAL_ECHOPGM(STR_DEBUG_PREFIX);
+  if (marlin_debug_flags) {
+    uint8_t comma = 0;
+    LOOP_L_N(i, COUNT(debug_strings)) {
+      if (TEST(marlin_debug_flags, i)) {
+        if (comma++) SERIAL_CHAR(',');
+        serialprintPGM((char*)pgm_read_ptr(&debug_strings[i]));
+      }
+    }
+  }
+  else {
+    SERIAL_ECHOPGM(STR_DEBUG_OFF);
+    #if !defined(__AVR__) || !defined(USBCON)
+      #if ENABLED(SERIAL_STATS_RX_BUFFER_OVERRUNS)
+        SERIAL_ECHOPAIR("\nBuffer Overruns: ", MYSERIAL0.buffer_overruns());
+      #endif
+
+      #if ENABLED(SERIAL_STATS_RX_FRAMING_ERRORS)
+        SERIAL_ECHOPAIR("\nFraming Errors: ", MYSERIAL0.framing_errors());
+      #endif
+
+      #if ENABLED(SERIAL_STATS_DROPPED_RX)
+        SERIAL_ECHOPAIR("\nDropped bytes: ", MYSERIAL0.dropped());
+      #endif
+
+      #if ENABLED(SERIAL_STATS_MAX_RX_QUEUED)
+        SERIAL_ECHOPAIR("\nMax RX Queue Size: ", MYSERIAL0.rxMaxEnqueued());
+      #endif
+    #endif // !__AVR__ || !USBCON
+  }
+  SERIAL_EOL();
+}
diff --git a/Marlin/src/gcode/control/M120_M121.cpp b/Marlin/src/gcode/control/M120_M121.cpp
new file mode 100644
index 0000000..570f2e9
--- /dev/null
+++ b/Marlin/src/gcode/control/M120_M121.cpp
@@ -0,0 +1,34 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../module/endstops.h"
+
+/**
+ * M120: Enable endstops and set non-homing endstop state to "enabled"
+ */
+void GcodeSuite::M120() { endstops.enable_globally(true); }
+
+/**
+ * M121: Disable endstops and set non-homing endstop state to "disabled"
+ */
+void GcodeSuite::M121() { endstops.enable_globally(false); }
diff --git a/Marlin/src/gcode/control/M17_M18_M84.cpp b/Marlin/src/gcode/control/M17_M18_M84.cpp
new file mode 100644
index 0000000..b35b465
--- /dev/null
+++ b/Marlin/src/gcode/control/M17_M18_M84.cpp
@@ -0,0 +1,69 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../MarlinCore.h" // for stepper_inactive_time, disable_e_steppers
+#include "../../lcd/marlinui.h"
+#include "../../module/stepper.h"
+
+#if ENABLED(AUTO_BED_LEVELING_UBL)
+  #include "../../feature/bedlevel/bedlevel.h"
+#endif
+
+/**
+ * M17: Enable stepper motors
+ */
+void GcodeSuite::M17() {
+  if (parser.seen("XYZE")) {
+    if (parser.seen('X')) ENABLE_AXIS_X();
+    if (parser.seen('Y')) ENABLE_AXIS_Y();
+    if (parser.seen('Z')) ENABLE_AXIS_Z();
+    if (TERN0(HAS_E_STEPPER_ENABLE, parser.seen('E'))) enable_e_steppers();
+  }
+  else {
+    LCD_MESSAGEPGM(MSG_NO_MOVE);
+    enable_all_steppers();
+  }
+}
+
+/**
+ * M18, M84: Disable stepper motors
+ */
+void GcodeSuite::M18_M84() {
+  if (parser.seenval('S')) {
+    reset_stepper_timeout();
+    stepper_inactive_time = parser.value_millis_from_seconds();
+  }
+  else {
+    if (parser.seen("XYZE")) {
+      planner.synchronize();
+      if (parser.seen('X')) DISABLE_AXIS_X();
+      if (parser.seen('Y')) DISABLE_AXIS_Y();
+      if (parser.seen('Z')) DISABLE_AXIS_Z();
+      if (TERN0(HAS_E_STEPPER_ENABLE, parser.seen('E'))) disable_e_steppers();
+    }
+    else
+      planner.finish_and_disable();
+
+    TERN_(AUTO_BED_LEVELING_UBL, ubl.steppers_were_disabled());
+  }
+}
diff --git a/Marlin/src/gcode/control/M211.cpp b/Marlin/src/gcode/control/M211.cpp
new file mode 100644
index 0000000..2049f1e
--- /dev/null
+++ b/Marlin/src/gcode/control/M211.cpp
@@ -0,0 +1,46 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if HAS_SOFTWARE_ENDSTOPS
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+
+/**
+ * M211: Enable, Disable, and/or Report software endstops
+ *
+ * Usage: M211 S1 to enable, M211 S0 to disable, M211 alone for report
+ */
+void GcodeSuite::M211() {
+  const xyz_pos_t l_soft_min = soft_endstop.min.asLogical(),
+                  l_soft_max = soft_endstop.max.asLogical();
+  SERIAL_ECHO_START();
+  SERIAL_ECHOPGM(STR_SOFT_ENDSTOPS);
+  if (parser.seen('S')) soft_endstop._enabled = parser.value_bool();
+  serialprint_onoff(soft_endstop._enabled);
+  print_xyz(l_soft_min, PSTR(STR_SOFT_MIN), PSTR(" "));
+  print_xyz(l_soft_max, PSTR(STR_SOFT_MAX));
+}
+
+#endif
diff --git a/Marlin/src/gcode/control/M226.cpp b/Marlin/src/gcode/control/M226.cpp
new file mode 100644
index 0000000..63f022e
--- /dev/null
+++ b/Marlin/src/gcode/control/M226.cpp
@@ -0,0 +1,60 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(DIRECT_PIN_CONTROL)
+
+#include "../gcode.h"
+#include "../../MarlinCore.h" // for pin_is_protected and idle()
+#include "../../module/stepper.h"
+
+void protected_pin_err();
+
+/**
+ * M226: Wait until the specified pin reaches the state required (M226 P<pin> S<state>)
+ */
+void GcodeSuite::M226() {
+  if (parser.seen('P')) {
+    const int pin_number = PARSED_PIN_INDEX('P', 0),
+              pin_state = parser.intval('S', -1); // required pin state - default is inverted
+    const pin_t pin = GET_PIN_MAP_PIN(pin_number);
+
+    if (WITHIN(pin_state, -1, 1) && pin > -1) {
+      if (pin_is_protected(pin))
+        protected_pin_err();
+      else {
+        int target = LOW;
+        planner.synchronize();
+        pinMode(pin, INPUT);
+        switch (pin_state) {
+          case 1: target = HIGH; break;
+          case 0: target = LOW; break;
+          case -1: target = !extDigitalRead(pin); break;
+        }
+        while (int(extDigitalRead(pin)) != target) idle();
+      }
+    } // pin_state -1 0 1 && pin > -1
+  } // parser.seen('P')
+}
+
+#endif // DIRECT_PIN_CONTROL
diff --git a/Marlin/src/gcode/control/M280.cpp b/Marlin/src/gcode/control/M280.cpp
new file mode 100644
index 0000000..6ccbb7b
--- /dev/null
+++ b/Marlin/src/gcode/control/M280.cpp
@@ -0,0 +1,55 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_SERVOS
+
+#include "../gcode.h"
+#include "../../module/servo.h"
+
+/**
+ * M280: Get or set servo position. P<index> [S<angle>]
+ */
+void GcodeSuite::M280() {
+  if (!parser.seen('P')) return;
+  const int servo_index = parser.value_int();
+  if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) {
+    if (parser.seen('S')) {
+      const int a = parser.value_int();
+      if (a == -1)
+        servo[servo_index].detach();
+      else
+        MOVE_SERVO(servo_index, a);
+    }
+    else {
+      SERIAL_ECHO_START();
+      SERIAL_ECHOLNPAIR(" Servo ", servo_index, ": ", servo[servo_index].read());
+    }
+  }
+  else {
+    SERIAL_ERROR_START();
+    SERIAL_ECHOLNPAIR("Servo ", servo_index, " out of range");
+  }
+}
+
+#endif // HAS_SERVOS
diff --git a/Marlin/src/gcode/control/M3-M5.cpp b/Marlin/src/gcode/control/M3-M5.cpp
new file mode 100644
index 0000000..711bb7e
--- /dev/null
+++ b/Marlin/src/gcode/control/M3-M5.cpp
@@ -0,0 +1,140 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_CUTTER
+
+#include "../gcode.h"
+#include "../../feature/spindle_laser.h"
+#include "../../module/stepper.h"
+
+/**
+ * Laser:
+ *  M3 - Laser ON/Power (Ramped power)
+ *  M4 - Laser ON/Power (Continuous power)
+ *
+ * Spindle:
+ *  M3 - Spindle ON (Clockwise)
+ *  M4 - Spindle ON (Counter-clockwise)
+ *
+ * Parameters:
+ *  S<power> - Set power. S0 will turn the spindle/laser off, except in relative mode.
+ *  O<ocr>   - Set power and OCR (oscillator count register)
+ *
+ *  If no PWM pin is defined then M3/M4 just turns it on.
+ *
+ *  At least 12.8KHz (50Hz * 256) is needed for Spindle PWM.
+ *  Hardware PWM is required on AVR. ISRs are too slow.
+ *
+ * NOTE: WGM for timers 3, 4, and 5 must be either Mode 1 or Mode 5.
+ *       No other settings give a PWM signal that goes from 0 to 5 volts.
+ *
+ *       The system automatically sets WGM to Mode 1, so no special
+ *       initialization is needed.
+ *
+ *       WGM bits for timer 2 are automatically set by the system to
+ *       Mode 1. This produces an acceptable 0 to 5 volt signal.
+ *       No special initialization is needed.
+ *
+ * NOTE: A minimum PWM frequency of 50 Hz is needed. All prescaler
+ *       factors for timers 2, 3, 4, and 5 are acceptable.
+ *
+ *  SPINDLE_LASER_ENA_PIN needs an external pullup or it may power on
+ *  the spindle/laser during power-up or when connecting to the host
+ *  (usually goes through a reset which sets all I/O pins to tri-state)
+ *
+ *  PWM duty cycle goes from 0 (off) to 255 (always on).
+ */
+void GcodeSuite::M3_M4(const bool is_M4) {
+  auto get_s_power = [] {
+    if (parser.seenval('S')) {
+      const float spwr = parser.value_float();
+      #if ENABLED(SPINDLE_SERVO)
+        cutter.unitPower = spwr;
+      #else
+        cutter.unitPower = TERN(SPINDLE_LASER_PWM,
+                              cutter.power_to_range(cutter_power_t(round(spwr))),
+                              spwr > 0 ? 255 : 0);
+      #endif
+    }
+    else
+      cutter.unitPower = cutter.cpwr_to_upwr(SPEED_POWER_STARTUP);
+    return cutter.unitPower;
+  };
+
+  #if ENABLED(LASER_POWER_INLINE)
+    if (parser.seen('I') == DISABLED(LASER_POWER_INLINE_INVERT)) {
+      // Laser power in inline mode
+      cutter.inline_direction(is_M4); // Should always be unused
+      #if ENABLED(SPINDLE_LASER_PWM)
+        if (parser.seen('O')) {
+          cutter.unitPower = cutter.power_to_range(parser.value_byte(), 0);
+          cutter.inline_ocr_power(cutter.unitPower); // The OCR is a value from 0 to 255 (uint8_t)
+        }
+        else
+          cutter.inline_power(cutter.upower_to_ocr(get_s_power()));
+      #else
+        cutter.set_inline_enabled(true);
+      #endif
+      return;
+    }
+    // Non-inline, standard case
+    cutter.inline_disable(); // Prevent future blocks re-setting the power
+  #endif
+
+  planner.synchronize();   // Wait for previous movement commands (G0/G0/G2/G3) to complete before changing power
+  cutter.set_reverse(is_M4);
+
+  #if ENABLED(SPINDLE_LASER_PWM)
+    if (parser.seenval('O')) {
+      cutter.unitPower = cutter.power_to_range(parser.value_byte(), 0);
+      cutter.set_ocr_power(cutter.unitPower); // The OCR is a value from 0 to 255 (uint8_t)
+    }
+    else
+      cutter.set_power(cutter.upower_to_ocr(get_s_power()));
+  #elif ENABLED(SPINDLE_SERVO)
+    cutter.set_power(get_s_power());
+  #else
+    cutter.set_enabled(true);
+  #endif
+  cutter.menuPower = cutter.unitPower;
+}
+
+/**
+ * M5 - Cutter OFF (when moves are complete)
+ */
+void GcodeSuite::M5() {
+  #if ENABLED(LASER_POWER_INLINE)
+    if (parser.seen('I') == DISABLED(LASER_POWER_INLINE_INVERT)) {
+      cutter.set_inline_enabled(false); // Laser power in inline mode
+      return;
+    }
+    // Non-inline, standard case
+    cutter.inline_disable(); // Prevent future blocks re-setting the power
+  #endif
+  planner.synchronize();
+  cutter.set_enabled(false);
+  cutter.menuPower = cutter.unitPower;
+}
+
+#endif // HAS_CUTTER
diff --git a/Marlin/src/gcode/control/M350_M351.cpp b/Marlin/src/gcode/control/M350_M351.cpp
new file mode 100644
index 0000000..463bd2a
--- /dev/null
+++ b/Marlin/src/gcode/control/M350_M351.cpp
@@ -0,0 +1,64 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_MICROSTEPS
+
+#include "../gcode.h"
+#include "../../module/stepper.h"
+
+/**
+ * M350: Set axis microstepping modes. S sets mode for all drivers.
+ *
+ * Warning: Steps-per-unit remains unchanged.
+ */
+void GcodeSuite::M350() {
+  if (parser.seen('S')) LOOP_LE_N(i, 4) stepper.microstep_mode(i, parser.value_byte());
+  LOOP_XYZE(i) if (parser.seen(axis_codes[i])) stepper.microstep_mode(i, parser.value_byte());
+  if (parser.seen('B')) stepper.microstep_mode(4, parser.value_byte());
+  stepper.microstep_readings();
+}
+
+/**
+ * M351: Toggle MS1 MS2 pins directly with axis codes X Y Z E B
+ *       S# determines MS1, MS2 or MS3, X# sets the pin high/low.
+ */
+void GcodeSuite::M351() {
+  if (parser.seenval('S')) switch (parser.value_byte()) {
+    case 1:
+      LOOP_XYZE(i) if (parser.seenval(axis_codes[i])) stepper.microstep_ms(i, parser.value_byte(), -1, -1);
+      if (parser.seenval('B')) stepper.microstep_ms(4, parser.value_byte(), -1, -1);
+      break;
+    case 2:
+      LOOP_XYZE(i) if (parser.seenval(axis_codes[i])) stepper.microstep_ms(i, -1, parser.value_byte(), -1);
+      if (parser.seenval('B')) stepper.microstep_ms(4, -1, parser.value_byte(), -1);
+      break;
+    case 3:
+      LOOP_XYZE(i) if (parser.seenval(axis_codes[i])) stepper.microstep_ms(i, -1, -1, parser.value_byte());
+      if (parser.seenval('B')) stepper.microstep_ms(4, -1, -1, parser.value_byte());
+      break;
+  }
+  stepper.microstep_readings();
+}
+
+#endif // HAS_MICROSTEPS
diff --git a/Marlin/src/gcode/control/M380_M381.cpp b/Marlin/src/gcode/control/M380_M381.cpp
new file mode 100644
index 0000000..3f5b252
--- /dev/null
+++ b/Marlin/src/gcode/control/M380_M381.cpp
@@ -0,0 +1,56 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if EITHER(EXT_SOLENOID, MANUAL_SOLENOID_CONTROL)
+
+#include "../gcode.h"
+#include "../../feature/solenoid.h"
+#include "../../module/motion.h"
+
+/**
+ * M380: Enable solenoid on the active extruder
+ *
+ *   S<index> to specify a solenoid (Requires MANUAL_SOLENOID_CONTROL)
+ */
+void GcodeSuite::M380() {
+  #if ENABLED(MANUAL_SOLENOID_CONTROL)
+    enable_solenoid(parser.intval('S', active_extruder));
+  #else
+    enable_solenoid_on_active_extruder();
+  #endif
+}
+
+/**
+ * M381: Disable all solenoids if EXT_SOLENOID
+ *       Disable selected/active solenoid if MANUAL_SOLENOID_CONTROL
+ */
+void GcodeSuite::M381() {
+  #if ENABLED(MANUAL_SOLENOID_CONTROL)
+    disable_solenoid(parser.intval('S', active_extruder));
+  #else
+    disable_all_solenoids();
+  #endif
+}
+
+#endif // EXT_SOLENOID || MANUAL_SOLENOID_CONTROL
diff --git a/Marlin/src/gcode/control/M400.cpp b/Marlin/src/gcode/control/M400.cpp
new file mode 100644
index 0000000..9a5ad4e
--- /dev/null
+++ b/Marlin/src/gcode/control/M400.cpp
@@ -0,0 +1,33 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../module/stepper.h"
+
+/**
+ * M400: Finish all moves
+ */
+void GcodeSuite::M400() {
+
+  planner.synchronize();
+
+}
diff --git a/Marlin/src/gcode/control/M42.cpp b/Marlin/src/gcode/control/M42.cpp
new file mode 100644
index 0000000..6ef8455
--- /dev/null
+++ b/Marlin/src/gcode/control/M42.cpp
@@ -0,0 +1,107 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(DIRECT_PIN_CONTROL)
+
+#include "../gcode.h"
+#include "../../MarlinCore.h" // for pin_is_protected
+
+#if HAS_FAN
+  #include "../../module/temperature.h"
+#endif
+
+void protected_pin_err() {
+  SERIAL_ERROR_MSG(STR_ERR_PROTECTED_PIN);
+}
+
+/**
+ * M42: Change pin status via GCode
+ *
+ *  P<pin>  Pin number (LED if omitted)
+ *          For LPC1768 specify pin P1_02 as M42 P102,
+ *                                  P1_20 as M42 P120, etc.
+ *
+ *  S<byte> Pin status from 0 - 255
+ *  I       Flag to ignore Marlin's pin protection
+ *
+ *  M<mode> Pin mode: 0=INPUT  1=OUTPUT  2=INPUT_PULLUP  3=INPUT_PULLDOWN
+ */
+void GcodeSuite::M42() {
+  const int pin_index = PARSED_PIN_INDEX('P', GET_PIN_MAP_INDEX(LED_PIN));
+  if (pin_index < 0) return;
+
+  const pin_t pin = GET_PIN_MAP_PIN(pin_index);
+
+  if (!parser.boolval('I') && pin_is_protected(pin)) return protected_pin_err();
+
+  if (parser.seenval('M')) {
+    switch (parser.value_byte()) {
+      case 0: pinMode(pin, INPUT); break;
+      case 1: pinMode(pin, OUTPUT); break;
+      case 2: pinMode(pin, INPUT_PULLUP); break;
+      #ifdef INPUT_PULLDOWN
+        case 3: pinMode(pin, INPUT_PULLDOWN); break;
+      #endif
+      default: SERIAL_ECHOLNPGM("Invalid Pin Mode"); return;
+    }
+  }
+
+  if (!parser.seenval('S')) return;
+  const byte pin_status = parser.value_byte();
+
+  #if HAS_FAN
+    switch (pin) {
+      #if HAS_FAN0
+        case FAN0_PIN: thermalManager.fan_speed[0] = pin_status; return;
+      #endif
+      #if HAS_FAN1
+        case FAN1_PIN: thermalManager.fan_speed[1] = pin_status; return;
+      #endif
+      #if HAS_FAN2
+        case FAN2_PIN: thermalManager.fan_speed[2] = pin_status; return;
+      #endif
+      #if HAS_FAN3
+        case FAN3_PIN: thermalManager.fan_speed[3] = pin_status; return;
+      #endif
+      #if HAS_FAN4
+        case FAN4_PIN: thermalManager.fan_speed[4] = pin_status; return;
+      #endif
+      #if HAS_FAN5
+        case FAN5_PIN: thermalManager.fan_speed[5] = pin_status; return;
+      #endif
+      #if HAS_FAN6
+        case FAN6_PIN: thermalManager.fan_speed[6] = pin_status; return;
+      #endif
+      #if HAS_FAN7
+        case FAN7_PIN: thermalManager.fan_speed[7] = pin_status; return;
+      #endif
+    }
+  #endif
+
+  pinMode(pin, OUTPUT);
+  extDigitalWrite(pin, pin_status);
+  analogWrite(pin, pin_status);
+}
+
+#endif // DIRECT_PIN_CONTROL
diff --git a/Marlin/src/gcode/control/M605.cpp b/Marlin/src/gcode/control/M605.cpp
new file mode 100644
index 0000000..0d7a9f4
--- /dev/null
+++ b/Marlin/src/gcode/control/M605.cpp
@@ -0,0 +1,185 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_DUPLICATION_MODE
+
+//#define DEBUG_DXC_MODE
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/stepper.h"
+#include "../../module/tool_change.h"
+#include "../../module/planner.h"
+
+#define DEBUG_OUT ENABLED(DEBUG_DXC_MODE)
+#include "../../core/debug_out.h"
+
+#if ENABLED(DUAL_X_CARRIAGE)
+
+  /**
+   * M605: Set dual x-carriage movement mode
+   *
+   *   M605 S0 : (FULL_CONTROL) The slicer has full control over both X-carriages and can achieve optimal travel
+   *             results as long as it supports dual X-carriages.
+   *
+   *   M605 S1 : (AUTO_PARK) The firmware automatically parks and unparks the X-carriages on tool-change so that
+   *             additional slicer support is not required.
+   *
+   *   M605 S2 X R : (DUPLICATION) The firmware moves the second X-carriage and extruder in synchronization with
+   *             the first X-carriage and extruder, to print 2 copies of the same object at the same time.
+   *             Set the constant X-offset and temperature differential with M605 S2 X[offs] R[deg] and
+   *             follow with "M605 S2" to initiate duplicated movement. For example, use "M605 S2 X100 R2" to
+   *             make a copy 100mm to the right with E1 2° hotter than E0.
+   *
+   *   M605 S3 : (MIRRORED) Formbot/Vivedino-inspired mirrored mode in which the second extruder duplicates
+   *             the movement of the first except the second extruder is reversed in the X axis.
+   *             The temperature differential and initial X offset must be set with "M605 S2 X[offs] R[deg]",
+   *             then followed by "M605 S3" to initiate mirrored movement.
+   *
+   *    M605 W  : IDEX What? command.
+   *
+   *    Note: the X axis should be homed after changing Dual X-carriage mode.
+   */
+  void GcodeSuite::M605() {
+    planner.synchronize();
+
+    if (parser.seen('S')) {
+      const DualXMode previous_mode = dual_x_carriage_mode;
+
+      dual_x_carriage_mode = (DualXMode)parser.value_byte();
+      idex_set_mirrored_mode(false);
+
+      if (dual_x_carriage_mode == DXC_MIRRORED_MODE) {
+        if (previous_mode != DXC_DUPLICATION_MODE) {
+          SERIAL_ECHOLNPGM("Printer must be in DXC_DUPLICATION_MODE prior to ");
+          SERIAL_ECHOLNPGM("specifying DXC_MIRRORED_MODE.");
+          dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE;
+          return;
+        }
+        idex_set_mirrored_mode(true);
+        float x_jog = current_position.x - .1;
+        for (uint8_t i = 2; --i;) {
+          planner.buffer_line(x_jog, current_position.y, current_position.z, current_position.e, feedrate_mm_s, 0);
+          x_jog += .1;
+        }
+        return;
+      }
+
+      switch (dual_x_carriage_mode) {
+        case DXC_FULL_CONTROL_MODE:
+        case DXC_AUTO_PARK_MODE:
+          break;
+        case DXC_DUPLICATION_MODE:
+          // Set the X offset, but no less than the safety gap
+          if (parser.seen('X')) duplicate_extruder_x_offset = _MAX(parser.value_linear_units(), (X2_MIN_POS) - (X1_MIN_POS));
+          if (parser.seen('R')) duplicate_extruder_temp_offset = parser.value_celsius_diff();
+          // Always switch back to tool 0
+          if (active_extruder != 0) tool_change(0);
+          break;
+        default:
+          dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE;
+          break;
+      }
+      idex_set_parked(false);
+      set_duplication_enabled(false);
+
+      #ifdef EVENT_GCODE_IDEX_AFTER_MODECHANGE
+        gcode.process_subcommands_now_P(PSTR(EVENT_GCODE_IDEX_AFTER_MODECHANGE));
+      #endif
+    }
+    else if (!parser.seen('W'))  // if no S or W parameter, the DXC mode gets reset to the user's default
+      dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE;
+
+    #ifdef DEBUG_DXC_MODE
+
+      if (parser.seen('W')) {
+        DEBUG_ECHO_START();
+        DEBUG_ECHOPGM("Dual X Carriage Mode ");
+        switch (dual_x_carriage_mode) {
+          case DXC_FULL_CONTROL_MODE: DEBUG_ECHOPGM("FULL_CONTROL"); break;
+          case DXC_AUTO_PARK_MODE:    DEBUG_ECHOPGM("AUTO_PARK");    break;
+          case DXC_DUPLICATION_MODE:  DEBUG_ECHOPGM("DUPLICATION");  break;
+          case DXC_MIRRORED_MODE:     DEBUG_ECHOPGM("MIRRORED");     break;
+        }
+        DEBUG_ECHOPAIR("\nActive Ext: ", int(active_extruder));
+        if (!active_extruder_parked) DEBUG_ECHOPGM(" NOT ");
+        DEBUG_ECHOPGM(" parked.");
+        DEBUG_ECHOPAIR("\nactive_extruder_x_pos: ", current_position.x);
+        DEBUG_ECHOPAIR("\ninactive_extruder_x: ", inactive_extruder_x);
+        DEBUG_ECHOPAIR("\nextruder_duplication_enabled: ", int(extruder_duplication_enabled));
+        DEBUG_ECHOPAIR("\nduplicate_extruder_x_offset: ", duplicate_extruder_x_offset);
+        DEBUG_ECHOPAIR("\nduplicate_extruder_temp_offset: ", duplicate_extruder_temp_offset);
+        DEBUG_ECHOPAIR("\ndelayed_move_time: ", delayed_move_time);
+        DEBUG_ECHOPAIR("\nX1 Home X: ", x_home_pos(0), "\nX1_MIN_POS=", int(X1_MIN_POS), "\nX1_MAX_POS=", int(X1_MAX_POS));
+        DEBUG_ECHOPAIR("\nX2 Home X: ", x_home_pos(1), "\nX2_MIN_POS=", int(X2_MIN_POS), "\nX2_MAX_POS=", int(X2_MAX_POS));
+        DEBUG_ECHOPAIR("\nX2_HOME_DIR=", int(X2_HOME_DIR), "\nX2_HOME_POS=", int(X2_HOME_POS));
+        DEBUG_ECHOPAIR("\nDEFAULT_DUAL_X_CARRIAGE_MODE=", STRINGIFY(DEFAULT_DUAL_X_CARRIAGE_MODE));
+        DEBUG_ECHOPAIR("\toolchange_settings.z_raise=", toolchange_settings.z_raise);
+        DEBUG_ECHOPAIR("\nDEFAULT_DUPLICATION_X_OFFSET=", int(DEFAULT_DUPLICATION_X_OFFSET));
+        DEBUG_EOL();
+
+        HOTEND_LOOP() {
+          DEBUG_ECHOPAIR_P(SP_T_STR, int(e));
+          LOOP_XYZ(a) DEBUG_ECHOPAIR("  hotend_offset[", int(e), "].", XYZ_CHAR(a) | 0x20, "=", hotend_offset[e][a]);
+          DEBUG_EOL();
+        }
+        DEBUG_EOL();
+      }
+    #endif // DEBUG_DXC_MODE
+  }
+
+#elif ENABLED(MULTI_NOZZLE_DUPLICATION)
+
+  /**
+   * M605: Set multi-nozzle duplication mode
+   *
+   *  S2       - Enable duplication mode
+   *  P[mask]  - Bit-mask of nozzles to include in the duplication set.
+   *             A value of 0 disables duplication.
+   *  E[index] - Last nozzle index to include in the duplication set.
+   *             A value of 0 disables duplication.
+   */
+  void GcodeSuite::M605() {
+    bool ena = false;
+    if (parser.seen("EPS")) {
+      planner.synchronize();
+      if (parser.seenval('P')) duplication_e_mask = parser.value_int();   // Set the mask directly
+      else if (parser.seenval('E')) duplication_e_mask = pow(2, parser.value_int() + 1) - 1; // Set the mask by E index
+      ena = (2 == parser.intval('S', extruder_duplication_enabled ? 2 : 0));
+      set_duplication_enabled(ena && (duplication_e_mask >= 3));
+    }
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM(STR_DUPLICATION_MODE);
+    serialprint_onoff(extruder_duplication_enabled);
+    if (ena) {
+      SERIAL_ECHOPGM(" ( ");
+      HOTEND_LOOP() if (TEST(duplication_e_mask, e)) { SERIAL_ECHO(e); SERIAL_CHAR(' '); }
+      SERIAL_CHAR(')');
+    }
+    SERIAL_EOL();
+  }
+
+#endif // MULTI_NOZZLE_DUPLICATION
+
+#endif // HAS_DUPICATION_MODE
diff --git a/Marlin/src/gcode/control/M7-M9.cpp b/Marlin/src/gcode/control/M7-M9.cpp
new file mode 100644
index 0000000..a33e432
--- /dev/null
+++ b/Marlin/src/gcode/control/M7-M9.cpp
@@ -0,0 +1,63 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(COOLANT_CONTROL)
+
+#include "../gcode.h"
+#include "../../module/planner.h"
+
+#if ENABLED(COOLANT_MIST)
+  /**
+   * M7: Mist Coolant On
+   */
+  void GcodeSuite::M7() {
+    planner.synchronize();                            // Wait for move to arrive
+    WRITE(COOLANT_MIST_PIN, !(COOLANT_MIST_INVERT));  // Turn on Mist coolant
+  }
+#endif
+
+#if ENABLED(COOLANT_FLOOD)
+  /**
+   * M8: Flood Coolant On
+   */
+  void GcodeSuite::M8() {
+    planner.synchronize();                              // Wait for move to arrive
+    WRITE(COOLANT_FLOOD_PIN, !(COOLANT_FLOOD_INVERT));  // Turn on Flood coolant
+  }
+#endif
+
+/**
+ * M9: Coolant OFF
+ */
+void GcodeSuite::M9() {
+  planner.synchronize();                            // Wait for move to arrive
+  #if ENABLED(COOLANT_MIST)
+    WRITE(COOLANT_MIST_PIN, COOLANT_MIST_INVERT);   // Turn off Mist coolant
+  #endif
+  #if ENABLED(COOLANT_FLOOD)
+    WRITE(COOLANT_FLOOD_PIN, COOLANT_FLOOD_INVERT); // Turn off Flood coolant
+  #endif
+}
+
+#endif // COOLANT_CONTROL
diff --git a/Marlin/src/gcode/control/M80_M81.cpp b/Marlin/src/gcode/control/M80_M81.cpp
new file mode 100644
index 0000000..394b06d
--- /dev/null
+++ b/Marlin/src/gcode/control/M80_M81.cpp
@@ -0,0 +1,113 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+
+#include "../../module/temperature.h"
+#include "../../module/planner.h"       // for planner.finish_and_disable
+#include "../../module/printcounter.h"  // for print_job_timer.stop
+#include "../../lcd/marlinui.h"         // for LCD_MESSAGEPGM_P
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_SUICIDE
+  #include "../../MarlinCore.h"
+#endif
+
+#if ENABLED(PSU_CONTROL)
+
+  #if ENABLED(AUTO_POWER_CONTROL)
+    #include "../../feature/power.h"
+  #else
+    void restore_stepper_drivers();
+  #endif
+
+  // Could be moved to a feature, but this is all the data
+  bool powersupply_on;
+
+  #if HAS_TRINAMIC_CONFIG
+    #include "../../feature/tmc_util.h"
+  #endif
+
+  /**
+   * M80   : Turn on the Power Supply
+   * M80 S : Report the current state and exit
+   */
+  void GcodeSuite::M80() {
+
+    // S: Report the current power supply state and exit
+    if (parser.seen('S')) {
+      serialprintPGM(powersupply_on ? PSTR("PS:1\n") : PSTR("PS:0\n"));
+      return;
+    }
+
+    PSU_ON();
+
+    /**
+     * If you have a switch on suicide pin, this is useful
+     * if you want to start another print with suicide feature after
+     * a print without suicide...
+     */
+    #if HAS_SUICIDE
+      OUT_WRITE(SUICIDE_PIN, !SUICIDE_PIN_INVERTING);
+    #endif
+
+    #if DISABLED(AUTO_POWER_CONTROL)
+      safe_delay(PSU_POWERUP_DELAY);
+      restore_stepper_drivers();
+      TERN_(HAS_TRINAMIC_CONFIG, safe_delay(PSU_POWERUP_DELAY));
+    #endif
+
+    TERN_(HAS_LCD_MENU, ui.reset_status());
+  }
+
+#endif // PSU_CONTROL
+
+/**
+ * M81: Turn off Power, including Power Supply, if there is one.
+ *
+ *      This code should ALWAYS be available for FULL SHUTDOWN!
+ */
+void GcodeSuite::M81() {
+  thermalManager.disable_all_heaters();
+  planner.finish_and_disable();
+
+  print_job_timer.stop();
+
+  #if HAS_FAN
+    thermalManager.zero_fan_speeds();
+    #if ENABLED(PROBING_FANS_OFF)
+      thermalManager.fans_paused = false;
+      ZERO(thermalManager.saved_fan_speed);
+    #endif
+  #endif
+
+  safe_delay(1000); // Wait 1 second before switching off
+
+  #if HAS_SUICIDE
+    suicide();
+  #elif ENABLED(PSU_CONTROL)
+    PSU_OFF_SOON();
+  #endif
+
+  LCD_MESSAGEPGM_P(PSTR(MACHINE_NAME " " STR_OFF "."));
+}
diff --git a/Marlin/src/gcode/control/M85.cpp b/Marlin/src/gcode/control/M85.cpp
new file mode 100644
index 0000000..9c8c02c
--- /dev/null
+++ b/Marlin/src/gcode/control/M85.cpp
@@ -0,0 +1,35 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+
+/**
+ * M85: Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
+ */
+void GcodeSuite::M85() {
+
+  if (parser.seen('S')) {
+    reset_stepper_timeout();
+    max_inactive_time = parser.value_millis_from_seconds();
+  }
+
+}
diff --git a/Marlin/src/gcode/control/M993_M994.cpp b/Marlin/src/gcode/control/M993_M994.cpp
new file mode 100644
index 0000000..ff9ff85
--- /dev/null
+++ b/Marlin/src/gcode/control/M993_M994.cpp
@@ -0,0 +1,88 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ALL(HAS_SPI_FLASH, SDSUPPORT, MARLIN_DEV_MODE)
+
+#include "../gcode.h"
+#include "../../sd/cardreader.h"
+#include "../../libs/W25Qxx.h"
+
+/**
+ * M993: Backup SPI Flash to SD
+ */
+void GcodeSuite::M993() {
+  if (!card.isMounted()) card.mount();
+
+  char fname[] = "spiflash.bin";
+  card.openFileWrite(fname);
+  if (!card.isFileOpen()) {
+    SERIAL_ECHOLNPAIR("Failed to open ", fname, " to write.");
+    return;
+  }
+
+  uint8_t buf[1024];
+  uint32_t addr = 0;
+  W25QXX.init(SPI_QUARTER_SPEED);
+  SERIAL_ECHOPGM("Save SPI Flash");
+  while (addr < SPI_FLASH_SIZE) {
+    W25QXX.SPI_FLASH_BufferRead(buf, addr, COUNT(buf));
+    addr += COUNT(buf);
+    card.write(buf, COUNT(buf));
+    if (addr % (COUNT(buf) * 10) == 0) SERIAL_CHAR('.');
+  }
+  SERIAL_ECHOLNPGM(" done");
+
+  card.closefile();
+}
+
+/**
+ * M994: Load a backup from SD to SPI Flash
+ */
+void GcodeSuite::M994() {
+  if (!card.isMounted()) card.mount();
+
+  char fname[] = "spiflash.bin";
+  card.openFileRead(fname);
+  if (!card.isFileOpen()) {
+    SERIAL_ECHOLNPAIR("Failed to open ", fname, " to read.");
+    return;
+  }
+
+  uint8_t buf[1024];
+  uint32_t addr = 0;
+  W25QXX.init(SPI_QUARTER_SPEED);
+  W25QXX.SPI_FLASH_BulkErase();
+  SERIAL_ECHOPGM("Load SPI Flash");
+  while (addr < SPI_FLASH_SIZE) {
+    card.read(buf, COUNT(buf));
+    W25QXX.SPI_FLASH_BufferWrite(buf, addr, COUNT(buf));
+    addr += COUNT(buf);
+    if (addr % (COUNT(buf) * 10) == 0) SERIAL_CHAR('.');
+  }
+  SERIAL_ECHOLNPGM(" done");
+
+  card.closefile();
+}
+
+#endif // HAS_SPI_FLASH && SDSUPPORT && MARLIN_DEV_MODE
diff --git a/Marlin/src/gcode/control/M997.cpp b/Marlin/src/gcode/control/M997.cpp
new file mode 100644
index 0000000..cdff96f
--- /dev/null
+++ b/Marlin/src/gcode/control/M997.cpp
@@ -0,0 +1,36 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+
+#if ENABLED(PLATFORM_M997_SUPPORT)
+
+/**
+ * M997: Perform in-application firmware update
+ */
+void GcodeSuite::M997() {
+
+  flashFirmware(parser.intval('S'));
+
+}
+
+#endif
diff --git a/Marlin/src/gcode/control/M999.cpp b/Marlin/src/gcode/control/M999.cpp
new file mode 100644
index 0000000..7487b4c
--- /dev/null
+++ b/Marlin/src/gcode/control/M999.cpp
@@ -0,0 +1,45 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+
+#include "../../lcd/marlinui.h" // for lcd_reset_alert_level
+#include "../../MarlinCore.h"   // for marlin_state
+#include "../queue.h"           // for flush_and_request_resend
+
+/**
+ * M999: Restart after being stopped
+ *
+ * Default behavior is to flush the serial buffer and request
+ * a resend to the host starting on the last N line received.
+ *
+ * Sending "M999 S1" will resume printing without flushing the
+ * existing command buffer.
+ */
+void GcodeSuite::M999() {
+  marlin_state = MF_RUNNING;
+  ui.reset_alert_level();
+
+  if (parser.boolval('S')) return;
+
+  queue.flush_and_request_resend();
+}
diff --git a/Marlin/src/gcode/control/T.cpp b/Marlin/src/gcode/control/T.cpp
new file mode 100644
index 0000000..3ce284f
--- /dev/null
+++ b/Marlin/src/gcode/control/T.cpp
@@ -0,0 +1,70 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../module/tool_change.h"
+
+#if EITHER(HAS_MULTI_EXTRUDER, DEBUG_LEVELING_FEATURE)
+  #include "../../module/motion.h"
+#endif
+
+#if HAS_PRUSA_MMU2
+  #include "../../feature/mmu/mmu2.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../core/debug_out.h"
+
+/**
+ * T0-T<n>: Switch tool, usually switching extruders
+ *
+ *   F[units/min] Set the movement feedrate
+ *   S1           Don't move the tool in XY after change
+ *
+ * For PRUSA_MMU2(S) and SMUFF_EMU_MMU2(S)
+ *   T[n] Gcode to extrude at least 38.10 mm at feedrate 19.02 mm/s must follow immediately to load to extruder wheels.
+ *   T?   Gcode to extrude shouldn't have to follow. Load to extruder wheels is done automatically.
+ *   Tx   Same as T?, but nozzle doesn't have to be preheated. Tc requires a preheated nozzle to finish filament load.
+ *   Tc   Load to nozzle after filament was prepared by Tc and nozzle is already heated.
+ */
+void GcodeSuite::T(const int8_t tool_index) {
+
+  DEBUG_SECTION(log_T, "T", DEBUGGING(LEVELING));
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("...(", tool_index, ")");
+
+  // Count this command as movement / activity
+  reset_stepper_timeout();
+
+  #if HAS_PRUSA_MMU2
+    if (parser.string_arg) {
+      mmu2.tool_change(parser.string_arg);   // Special commands T?/Tx/Tc
+      return;
+    }
+  #endif
+
+  tool_change(tool_index
+    #if HAS_MULTI_EXTRUDER
+      ,  TERN(PARKING_EXTRUDER, false, tool_index == active_extruder) // For PARKING_EXTRUDER motion is decided in tool_change()
+      || parser.boolval('S')
+    #endif
+  );
+}
diff --git a/Marlin/src/gcode/eeprom/M500-M504.cpp b/Marlin/src/gcode/eeprom/M500-M504.cpp
new file mode 100644
index 0000000..26c50a6
--- /dev/null
+++ b/Marlin/src/gcode/eeprom/M500-M504.cpp
@@ -0,0 +1,104 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../module/settings.h"
+#include "../../core/serial.h"
+#include "../../inc/MarlinConfig.h"
+
+/**
+ * M500: Store settings in EEPROM
+ */
+void GcodeSuite::M500() {
+  (void)settings.save();
+}
+
+/**
+ * M501: Read settings from EEPROM
+ */
+void GcodeSuite::M501() {
+  (void)settings.load();
+}
+
+/**
+ * M502: Revert to default settings
+ */
+void GcodeSuite::M502() {
+  (void)settings.reset();
+}
+
+#if DISABLED(DISABLE_M503)
+
+  /**
+   * M503: print settings currently in memory
+   */
+  void GcodeSuite::M503() {
+    (void)settings.report(!parser.boolval('S', true));
+  }
+
+#endif // !DISABLE_M503
+
+#if ENABLED(EEPROM_SETTINGS)
+
+  #if ENABLED(MARLIN_DEV_MODE)
+    #include "../../libs/hex_print.h"
+  #endif
+
+  /**
+   * M504: Validate EEPROM Contents
+   */
+  void GcodeSuite::M504() {
+    #if ENABLED(MARLIN_DEV_MODE)
+      const bool dowrite = parser.seenval('W');
+      if (dowrite || parser.seenval('R')) {
+        uint8_t val = 0;
+        int addr = parser.value_ushort();
+        if (dowrite) {
+          val = parser.byteval('V');
+          persistentStore.write_data(addr, &val);
+          SERIAL_ECHOLNPAIR("Wrote address ", addr, " with ", int(val));
+        }
+        else {
+          if (parser.seenval('T')) {
+            const int endaddr = parser.value_ushort();
+            while (addr <= endaddr) {
+              persistentStore.read_data(addr, &val);
+              SERIAL_ECHOLNPAIR("0x", hex_word(addr), ":", hex_byte(val));
+              addr++;
+              safe_delay(10);
+            }
+            SERIAL_EOL();
+          }
+          else {
+            persistentStore.read_data(addr, &val);
+            SERIAL_ECHOLNPAIR("Read address ", addr, " and got ", int(val));
+          }
+        }
+        return;
+      }
+    #endif
+
+    if (settings.validate())
+      SERIAL_ECHO_MSG("EEPROM OK");
+  }
+
+#endif
diff --git a/Marlin/src/gcode/feature/L6470/M122.cpp b/Marlin/src/gcode/feature/L6470/M122.cpp
new file mode 100644
index 0000000..d2b7f73
--- /dev/null
+++ b/Marlin/src/gcode/feature/L6470/M122.cpp
@@ -0,0 +1,151 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if HAS_L64XX
+
+#include "../../gcode.h"
+#include "../../../libs/L64XX/L64XX_Marlin.h"
+#include "../../../module/stepper/indirection.h"
+
+void echo_yes_no(const bool yes);
+
+inline void L6470_say_status(const L64XX_axis_t axis) {
+  if (L64xxManager.spi_abort) return;
+  const L64XX_Marlin::L64XX_shadow_t &sh = L64xxManager.shadow;
+  L64xxManager.get_status(axis);
+  L64xxManager.say_axis(axis);
+  #if ENABLED(L6470_CHITCHAT)
+    char temp_buf[20];
+    sprintf_P(temp_buf, PSTR("   status: %4x   "), sh.STATUS_AXIS_RAW);
+    SERIAL_ECHO(temp_buf);
+    print_bin(sh.STATUS_AXIS_RAW);
+    switch (sh.STATUS_AXIS_LAYOUT) {
+      case L6470_STATUS_LAYOUT: serialprintPGM(PSTR("   L6470")); break;
+      case L6474_STATUS_LAYOUT: serialprintPGM(PSTR("   L6474")); break;
+      case L6480_STATUS_LAYOUT: serialprintPGM(PSTR("   L6480/powerSTEP01")); break;
+    }
+  #endif
+  SERIAL_ECHOPGM("\n...OUTPUT: ");
+  serialprintPGM(sh.STATUS_AXIS & STATUS_HIZ ? PSTR("OFF") : PSTR("ON "));
+  SERIAL_ECHOPGM("   BUSY: "); echo_yes_no((sh.STATUS_AXIS & STATUS_BUSY) == 0);
+  SERIAL_ECHOPGM("   DIR: ");
+  serialprintPGM((((sh.STATUS_AXIS & STATUS_DIR) >> 4) ^ L64xxManager.index_to_dir[axis]) ? PSTR("FORWARD") : PSTR("REVERSE"));
+  if (sh.STATUS_AXIS_LAYOUT == L6480_STATUS_LAYOUT) {
+    SERIAL_ECHOPGM("   Last Command: ");
+    if (sh.STATUS_AXIS & sh.STATUS_AXIS_WRONG_CMD) SERIAL_ECHOPGM("VALID");
+    else                                           SERIAL_ECHOPGM("ERROR");
+    SERIAL_ECHOPGM("\n...THERMAL: ");
+    switch ((sh.STATUS_AXIS & (sh.STATUS_AXIS_TH_SD | sh.STATUS_AXIS_TH_WRN)) >> 11) {
+      case 0: SERIAL_ECHOPGM("DEVICE SHUTDOWN"); break;
+      case 1: SERIAL_ECHOPGM("BRIDGE SHUTDOWN"); break;
+      case 2: SERIAL_ECHOPGM("WARNING        "); break;
+      case 3: SERIAL_ECHOPGM("OK             "); break;
+    }
+  }
+  else {
+    SERIAL_ECHOPGM("   Last Command: ");
+    if (!(sh.STATUS_AXIS & sh.STATUS_AXIS_WRONG_CMD)) SERIAL_ECHOPGM("IN");
+    SERIAL_ECHOPGM("VALID    ");
+    serialprintPGM(sh.STATUS_AXIS & sh.STATUS_AXIS_NOTPERF_CMD ?  PSTR("COMPLETED    ") : PSTR("Not PERFORMED"));
+    SERIAL_ECHOPAIR("\n...THERMAL: ", !(sh.STATUS_AXIS & sh.STATUS_AXIS_TH_SD) ? "SHUTDOWN       " : !(sh.STATUS_AXIS & sh.STATUS_AXIS_TH_WRN) ? "WARNING        " : "OK             ");
+  }
+  SERIAL_ECHOPGM("   OVERCURRENT:"); echo_yes_no((sh.STATUS_AXIS & sh.STATUS_AXIS_OCD) == 0);
+  if (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT) {
+    SERIAL_ECHOPGM("   STALL:"); echo_yes_no((sh.STATUS_AXIS & sh.STATUS_AXIS_STEP_LOSS_A) == 0 || (sh.STATUS_AXIS & sh.STATUS_AXIS_STEP_LOSS_B) == 0);
+    SERIAL_ECHOPGM("   STEP-CLOCK MODE:"); echo_yes_no((sh.STATUS_AXIS & sh.STATUS_AXIS_SCK_MOD) != 0);
+  }
+  else {
+    SERIAL_ECHOPGM("   STALL: NA "
+                   "   STEP-CLOCK MODE: NA"
+                   "   UNDER VOLTAGE LOCKOUT: "); echo_yes_no((sh.STATUS_AXIS & sh.STATUS_AXIS_UVLO) == 0);
+  }
+  SERIAL_EOL();
+}
+
+/**
+ * M122: Debug L6470 drivers
+ */
+void GcodeSuite::M122() {
+  L64xxManager.pause_monitor(true); // Keep monitor_driver() from stealing status
+  L64xxManager.spi_active = true;   // Tell set_directions() a series of SPI transfers is underway
+
+  //if (parser.seen('S'))
+  //  tmc_set_report_interval(parser.value_bool());
+  //else
+
+  #if AXIS_IS_L64XX(X)
+    L6470_say_status(X);
+  #endif
+  #if AXIS_IS_L64XX(X2)
+    L6470_say_status(X2);
+  #endif
+  #if AXIS_IS_L64XX(Y)
+    L6470_say_status(Y);
+  #endif
+  #if AXIS_IS_L64XX(Y2)
+    L6470_say_status(Y2);
+  #endif
+  #if AXIS_IS_L64XX(Z)
+    L6470_say_status(Z);
+  #endif
+  #if AXIS_IS_L64XX(Z2)
+    L6470_say_status(Z2);
+  #endif
+  #if AXIS_IS_L64XX(Z3)
+    L6470_say_status(Z3);
+  #endif
+  #if AXIS_IS_L64XX(Z4)
+    L6470_say_status(Z4);
+  #endif
+  #if AXIS_IS_L64XX(E0)
+    L6470_say_status(E0);
+  #endif
+  #if AXIS_IS_L64XX(E1)
+    L6470_say_status(E1);
+  #endif
+  #if AXIS_IS_L64XX(E2)
+    L6470_say_status(E2);
+  #endif
+  #if AXIS_IS_L64XX(E3)
+    L6470_say_status(E3);
+  #endif
+  #if AXIS_IS_L64XX(E4)
+    L6470_say_status(E4);
+  #endif
+  #if AXIS_IS_L64XX(E5)
+    L6470_say_status(E5);
+  #endif
+  #if AXIS_IS_L64XX(E6)
+    L6470_say_status(E6);
+  #endif
+  #if AXIS_IS_L64XX(E7)
+    L6470_say_status(E7);
+  #endif
+
+  L64xxManager.spi_active = false;   // done with all SPI transfers - clear handshake flags
+  L64xxManager.spi_abort = false;
+  L64xxManager.pause_monitor(false);
+}
+
+#endif // HAS_L64XX
diff --git a/Marlin/src/gcode/feature/L6470/M906.cpp b/Marlin/src/gcode/feature/L6470/M906.cpp
new file mode 100644
index 0000000..7bd446a
--- /dev/null
+++ b/Marlin/src/gcode/feature/L6470/M906.cpp
@@ -0,0 +1,370 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if HAS_L64XX
+
+#include "../../gcode.h"
+#include "../../../libs/L64XX/L64XX_Marlin.h"
+#include "../../../module/stepper/indirection.h"
+#include "../../../module/planner.h"
+
+#define DEBUG_OUT ENABLED(L6470_CHITCHAT)
+#include "../../../core/debug_out.h"
+
+/**
+ * M906: report or set KVAL_HOLD which sets the maximum effective voltage provided by the
+ *       PWMs to the steppers
+ *
+ * On L6474 this sets the TVAL register (same address).
+ *
+ * I - select which driver(s) to change on multi-driver axis
+ *     0 - (default) all drivers on the axis or E0
+ *     1 - monitor only X, Y, Z or E1
+ *     2 - monitor only X2, Y2, Z2 or E2
+ *     3 - monitor only Z3 or E3
+ *     4 - monitor only Z4 or E4
+ *     5 - monitor only E5
+ * Xxxx, Yxxx, Zxxx, Exxx - axis to change (optional)
+ *     L6474 - current in mA (4A max)
+ *     All others - 0-255
+ */
+
+/**
+ * Sets KVAL_HOLD wich affects the current being driven through the stepper.
+ *
+ * L6470 is used in the STEP-CLOCK mode.  KVAL_HOLD is the only KVAL_xxx
+ * that affects the effective voltage seen by the stepper.
+ */
+
+/**
+ * MACRO to fetch information on the items associated with current limiting
+ * and maximum voltage output.
+ *
+ * L6470 can be setup to shutdown if either current threshold is exceeded.
+ *
+ * L6470 output current can not be set directly.  It is set indirectly by
+ * setting the maximum effective output voltage.
+ *
+ *  Effective output voltage is set by PWM duty cycle.
+ *
+ *  Maximum effective output voltage is affected by MANY variables.  The main ones are:
+ *    KVAL_HOLD
+ *    KVAL_RUN
+ *    KVAL_ACC
+ *    KVAL_DEC
+ *    Vs compensation (if enabled)
+ */
+void L64XX_report_current(L64XX &motor, const L64XX_axis_t axis) {
+
+  if (L64xxManager.spi_abort) return;  // don't do anything if set_directions() has occurred
+
+  const L64XX_Marlin::L64XX_shadow_t &sh = L64xxManager.shadow;
+  const uint16_t status = L64xxManager.get_status(axis);    //also populates shadow structure
+  const uint8_t OverCurrent_Threshold = uint8_t(motor.GetParam(L6470_OCD_TH));
+
+  auto say_axis_status = [](const L64XX_axis_t axis, const uint16_t status) {
+    L64xxManager.say_axis(axis);
+    #if ENABLED(L6470_CHITCHAT)
+      char tmp[10];
+      sprintf_P(tmp, PSTR("%4x   "), status);
+      DEBUG_ECHOPAIR("   status: ", tmp);
+      print_bin(status);
+    #else
+      UNUSED(status);
+    #endif
+    SERIAL_EOL();
+  };
+
+  char temp_buf[10];
+
+  switch (sh.STATUS_AXIS_LAYOUT) {
+    case L6470_STATUS_LAYOUT:       // L6470
+    case L6480_STATUS_LAYOUT: {     // L6480 & powerstep01
+      const uint16_t Stall_Threshold = (uint8_t)motor.GetParam(L6470_STALL_TH),
+                     motor_status = (status & (STATUS_MOT_STATUS)) >> 5,
+                     L6470_ADC_out = motor.GetParam(L6470_ADC_OUT),
+                     L6470_ADC_out_limited = constrain(L6470_ADC_out, 8, 24);
+      const float comp_coef = 1600.0f / L6470_ADC_out_limited;
+      const uint16_t MicroSteps = _BV(motor.GetParam(L6470_STEP_MODE) & 0x07);
+
+      say_axis_status(axis, sh.STATUS_AXIS_RAW);
+
+      SERIAL_ECHOPGM("...OverCurrent Threshold: ");
+      sprintf_P(temp_buf, PSTR("%2d ("), OverCurrent_Threshold);
+      SERIAL_ECHO(temp_buf);
+      SERIAL_ECHO((OverCurrent_Threshold + 1) * motor.OCD_CURRENT_CONSTANT_INV);
+      SERIAL_ECHOPGM(" mA)");
+      SERIAL_ECHOPGM("   Stall Threshold: ");
+      sprintf_P(temp_buf, PSTR("%2d ("), Stall_Threshold);
+      SERIAL_ECHO(temp_buf);
+      SERIAL_ECHO((Stall_Threshold + 1) * motor.STALL_CURRENT_CONSTANT_INV);
+      SERIAL_ECHOPGM(" mA)");
+      SERIAL_ECHOPGM("   Motor Status: ");
+      switch (motor_status) {
+        case 0: SERIAL_ECHOPGM("stopped"); break;
+        case 1: SERIAL_ECHOPGM("accelerating"); break;
+        case 2: SERIAL_ECHOPGM("decelerating"); break;
+        case 3: SERIAL_ECHOPGM("at constant speed"); break;
+      }
+      SERIAL_EOL();
+
+      SERIAL_ECHOPAIR("...MicroSteps: ", MicroSteps,
+                      "   ADC_OUT: ", L6470_ADC_out);
+      SERIAL_ECHOPGM("   Vs_compensation: ");
+      serialprintPGM((motor.GetParam(sh.L6470_AXIS_CONFIG) & CONFIG_EN_VSCOMP) ? PSTR("ENABLED ") : PSTR("DISABLED"));
+      SERIAL_ECHOLNPAIR("   Compensation coefficient: ~", comp_coef * 0.01f);
+
+      SERIAL_ECHOPAIR("...KVAL_HOLD: ", motor.GetParam(L6470_KVAL_HOLD),
+                      "   KVAL_RUN : ", motor.GetParam(L6470_KVAL_RUN),
+                      "   KVAL_ACC: ", motor.GetParam(L6470_KVAL_ACC),
+                      "   KVAL_DEC: ", motor.GetParam(L6470_KVAL_DEC),
+                      "   V motor max = ");
+      switch (motor_status) {
+        case 0: SERIAL_ECHO(motor.GetParam(L6470_KVAL_HOLD) * 100 / 256); SERIAL_ECHOPGM("% (KVAL_HOLD)"); break;
+        case 1: SERIAL_ECHO(motor.GetParam(L6470_KVAL_RUN)  * 100 / 256); SERIAL_ECHOPGM("% (KVAL_RUN)"); break;
+        case 2: SERIAL_ECHO(motor.GetParam(L6470_KVAL_ACC)  * 100 / 256); SERIAL_ECHOPGM("% (KVAL_ACC)"); break;
+        case 3: SERIAL_ECHO(motor.GetParam(L6470_KVAL_DEC)  * 100 / 256); SERIAL_ECHOPGM("% (KVAL_HOLD)"); break;
+      }
+      SERIAL_EOL();
+
+      #if ENABLED(L6470_CHITCHAT)
+        DEBUG_ECHOPGM("...SLEW RATE: ");
+        switch (sh.STATUS_AXIS_LAYOUT) {
+          case L6470_STATUS_LAYOUT: {
+            switch ((motor.GetParam(sh.L6470_AXIS_CONFIG) & CONFIG_POW_SR) >> CONFIG_POW_SR_BIT) {
+              case 0: { DEBUG_ECHOLNPGM("320V/uS") ; break; }
+              case 1: { DEBUG_ECHOLNPGM("75V/uS")  ; break; }
+              case 2: { DEBUG_ECHOLNPGM("110V/uS") ; break; }
+              case 3: { DEBUG_ECHOLNPGM("260V/uS") ; break; }
+            }
+            break;
+          }
+        case L6480_STATUS_LAYOUT: {
+            switch (motor.GetParam(L6470_GATECFG1) & CONFIG1_SR ) {
+              case CONFIG1_SR_220V_us: { DEBUG_ECHOLNPGM("220V/uS") ; break; }
+              case CONFIG1_SR_400V_us: { DEBUG_ECHOLNPGM("400V/uS") ; break; }
+              case CONFIG1_SR_520V_us: { DEBUG_ECHOLNPGM("520V/uS") ; break; }
+              case CONFIG1_SR_980V_us: { DEBUG_ECHOLNPGM("980V/uS") ; break; }
+              default: { DEBUG_ECHOLNPGM("unknown") ; break; }
+            }
+          }
+        }
+      #endif
+      SERIAL_EOL();
+      break;
+    }
+
+    case L6474_STATUS_LAYOUT: {  // L6474
+      const uint16_t L6470_ADC_out = motor.GetParam(L6470_ADC_OUT) & 0x1F,
+                     L6474_TVAL_val = motor.GetParam(L6474_TVAL) & 0x7F;
+
+      say_axis_status(axis, sh.STATUS_AXIS_RAW);
+
+      SERIAL_ECHOPGM("...OverCurrent Threshold: ");
+      sprintf_P(temp_buf, PSTR("%2d ("), OverCurrent_Threshold);
+      SERIAL_ECHO(temp_buf);
+      SERIAL_ECHO((OverCurrent_Threshold + 1) * motor.OCD_CURRENT_CONSTANT_INV);
+      SERIAL_ECHOPGM(" mA)");
+      SERIAL_ECHOPGM("   TVAL: ");
+      sprintf_P(temp_buf, PSTR("%2d ("), L6474_TVAL_val);
+      SERIAL_ECHO(temp_buf);
+      SERIAL_ECHO((L6474_TVAL_val + 1) * motor.STALL_CURRENT_CONSTANT_INV);
+      SERIAL_ECHOLNPGM(" mA)   Motor Status: NA");
+
+      const uint16_t MicroSteps = _BV(motor.GetParam(L6470_STEP_MODE) & 0x07); //NOMORE(MicroSteps, 16);
+      SERIAL_ECHOPAIR("...MicroSteps: ", MicroSteps,
+                      "   ADC_OUT: ", L6470_ADC_out);
+
+      SERIAL_ECHOLNPGM("   Vs_compensation: NA\n");
+      SERIAL_ECHOLNPGM("...KVAL_HOLD: NA"
+                       "   KVAL_RUN : NA"
+                       "   KVAL_ACC: NA"
+                       "   KVAL_DEC: NA"
+                       "   V motor max =  NA");
+
+      #if ENABLED(L6470_CHITCHAT)
+        DEBUG_ECHOPGM("...SLEW RATE: ");
+        switch ((motor.GetParam(sh.L6470_AXIS_CONFIG) & CONFIG_POW_SR) >> CONFIG_POW_SR_BIT) {
+          case 0:  DEBUG_ECHOLNPGM("320V/uS") ; break;
+          case 1:  DEBUG_ECHOLNPGM("75V/uS")  ; break;
+          case 2:  DEBUG_ECHOLNPGM("110V/uS") ; break;
+          case 3:  DEBUG_ECHOLNPGM("260V/uS") ; break;
+          default: DEBUG_ECHOLNPAIR("slew rate: ", (motor.GetParam(sh.L6470_AXIS_CONFIG) & CONFIG_POW_SR) >> CONFIG_POW_SR_BIT); break;
+        }
+      #endif
+      SERIAL_EOL();
+      SERIAL_EOL();
+      break;
+    }
+  }
+}
+
+void GcodeSuite::M906() {
+
+  L64xxManager.pause_monitor(true); // Keep monitor_driver() from stealing status
+
+  #define L6470_SET_KVAL_HOLD(Q) (AXIS_IS_L64XX(Q) ? stepper##Q.setTVALCurrent(value) : stepper##Q.SetParam(L6470_KVAL_HOLD, uint8_t(value)))
+
+  DEBUG_ECHOLNPGM("M906");
+
+  uint8_t report_current = true;
+
+  #if HAS_L64XX
+    const uint8_t index = parser.byteval('I');
+  #endif
+
+  LOOP_XYZE(i) if (uint16_t value = parser.intval(axis_codes[i])) {
+
+    report_current = false;
+
+    if (planner.has_blocks_queued() || planner.cleaning_buffer_counter) {
+      SERIAL_ECHOLNPGM("Test aborted. Can't set KVAL_HOLD while steppers are moving.");
+      return;
+    }
+
+    switch (i) {
+      case X_AXIS:
+        #if AXIS_IS_L64XX(X)
+          if (index == 0) L6470_SET_KVAL_HOLD(X);
+        #endif
+        #if AXIS_IS_L64XX(X2)
+          if (index == 1) L6470_SET_KVAL_HOLD(X2);
+        #endif
+        break;
+      case Y_AXIS:
+        #if AXIS_IS_L64XX(Y)
+          if (index == 0) L6470_SET_KVAL_HOLD(Y);
+        #endif
+        #if AXIS_IS_L64XX(Y2)
+          if (index == 1) L6470_SET_KVAL_HOLD(Y2);
+        #endif
+        break;
+      case Z_AXIS:
+        #if AXIS_IS_L64XX(Z)
+          if (index == 0) L6470_SET_KVAL_HOLD(Z);
+        #endif
+        #if AXIS_IS_L64XX(Z2)
+          if (index == 1) L6470_SET_KVAL_HOLD(Z2);
+        #endif
+        #if AXIS_IS_L64XX(Z3)
+          if (index == 2) L6470_SET_KVAL_HOLD(Z3);
+        #endif
+        #if AXIS_DRIVER_TYPE_Z4(L6470)
+          if (index == 3) L6470_SET_KVAL_HOLD(Z4);
+        #endif
+        break;
+      case E_AXIS: {
+        const int8_t target_extruder = get_target_extruder_from_command();
+        if (target_extruder < 0) return;
+        switch (target_extruder) {
+          #if AXIS_IS_L64XX(E0)
+            case 0: L6470_SET_KVAL_HOLD(E0); break;
+          #endif
+          #if AXIS_IS_L64XX(E1)
+            case 1: L6470_SET_KVAL_HOLD(E1); break;
+          #endif
+          #if AXIS_IS_L64XX(E2)
+            case 2: L6470_SET_KVAL_HOLD(E2); break;
+          #endif
+          #if AXIS_IS_L64XX(E3)
+            case 3: L6470_SET_KVAL_HOLD(E3); break;
+          #endif
+          #if AXIS_IS_L64XX(E4)
+            case 4: L6470_SET_KVAL_HOLD(E4); break;
+          #endif
+          #if AXIS_IS_L64XX(E5)
+            case 5: L6470_SET_KVAL_HOLD(E5); break;
+          #endif
+          #if AXIS_IS_L64XX(E6)
+            case 6: L6470_SET_KVAL_HOLD(E6); break;
+          #endif
+          #if AXIS_IS_L64XX(E7)
+            case 7: L6470_SET_KVAL_HOLD(E7); break;
+          #endif
+        }
+      } break;
+    }
+  }
+
+  if (report_current) {
+    #define L64XX_REPORT_CURRENT(Q) L64XX_report_current(stepper##Q, Q)
+
+    L64xxManager.spi_active = true; // Tell set_directions() a series of SPI transfers is underway
+
+    #if AXIS_IS_L64XX(X)
+      L64XX_REPORT_CURRENT(X);
+    #endif
+    #if AXIS_IS_L64XX(X2)
+      L64XX_REPORT_CURRENT(X2);
+    #endif
+    #if AXIS_IS_L64XX(Y)
+      L64XX_REPORT_CURRENT(Y);
+    #endif
+    #if AXIS_IS_L64XX(Y2)
+      L64XX_REPORT_CURRENT(Y2);
+    #endif
+    #if AXIS_IS_L64XX(Z)
+      L64XX_REPORT_CURRENT(Z);
+    #endif
+    #if AXIS_IS_L64XX(Z2)
+      L64XX_REPORT_CURRENT(Z2);
+    #endif
+    #if AXIS_IS_L64XX(Z3)
+      L64XX_REPORT_CURRENT(Z3);
+    #endif
+    #if AXIS_IS_L64XX(Z4)
+      L64XX_REPORT_CURRENT(Z4);
+    #endif
+    #if AXIS_IS_L64XX(E0)
+      L64XX_REPORT_CURRENT(E0);
+    #endif
+    #if AXIS_IS_L64XX(E1)
+      L64XX_REPORT_CURRENT(E1);
+    #endif
+    #if AXIS_IS_L64XX(E2)
+      L64XX_REPORT_CURRENT(E2);
+    #endif
+    #if AXIS_IS_L64XX(E3)
+      L64XX_REPORT_CURRENT(E3);
+    #endif
+    #if AXIS_IS_L64XX(E4)
+      L64XX_REPORT_CURRENT(E4);
+    #endif
+    #if AXIS_IS_L64XX(E5)
+      L64XX_REPORT_CURRENT(E5);
+    #endif
+    #if AXIS_IS_L64XX(E6)
+      L64XX_REPORT_CURRENT(E6);
+    #endif
+    #if AXIS_IS_L64XX(E7)
+      L64XX_REPORT_CURRENT(E7);
+    #endif
+
+    L64xxManager.spi_active = false;   // done with all SPI transfers - clear handshake flags
+    L64xxManager.spi_abort = false;
+    L64xxManager.pause_monitor(false);
+  }
+}
+
+#endif // HAS_L64XX
diff --git a/Marlin/src/gcode/feature/L6470/M916-918.cpp b/Marlin/src/gcode/feature/L6470/M916-918.cpp
new file mode 100644
index 0000000..8a1ea48
--- /dev/null
+++ b/Marlin/src/gcode/feature/L6470/M916-918.cpp
@@ -0,0 +1,651 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+//
+// NOTE: All tests assume each axis uses matching driver chips.
+//
+
+#include "../../../inc/MarlinConfig.h"
+
+#if HAS_L64XX
+
+#include "../../gcode.h"
+#include "../../../module/stepper/indirection.h"
+#include "../../../module/planner.h"
+#include "../../../libs/L64XX/L64XX_Marlin.h"
+
+#define DEBUG_OUT ENABLED(L6470_CHITCHAT)
+#include "../../../core/debug_out.h"
+
+/**
+ * M916: increase KVAL_HOLD until get thermal warning
+ *       NOTE - on L6474 it is TVAL that is used
+ *
+ * J - select which driver(s) to monitor on multi-driver axis
+ *     0 - (default) monitor all drivers on the axis or E0
+ *     1 - monitor only X, Y, Z, E1
+ *     2 - monitor only X2, Y2, Z2, E2
+ *     3 - monitor only Z3, E3
+ *     4 - monitor only Z4, E4
+ *
+ * Xxxx, Yxxx, Zxxx, Exxx - axis to be monitored with displacement
+ *     xxx (1-255) is distance moved on either side of current position
+ *
+ * F - feedrate
+ *     optional - will use default max feedrate from configuration.h if not specified
+ *
+ * T - current (mA) setting for TVAL (0 - 4A in 31.25mA increments, rounds down) - L6474 only
+ *     optional - will report current value from driver if not specified
+ *
+ * K - value for KVAL_HOLD (0 - 255) (ignored for L6474)
+ *     optional - will report current value from driver if not specified
+ *
+ * D - time (in seconds) to run each setting of KVAL_HOLD/TVAL
+ *     optional - defaults to zero (runs each setting once)
+ */
+
+/**
+ * This routine is also useful for determining the approximate KVAL_HOLD
+ * where the stepper stops losing steps. The sound will get noticeably quieter
+ * as it stops losing steps.
+ */
+
+void GcodeSuite::M916() {
+
+  DEBUG_ECHOLNPGM("M916");
+
+  L64xxManager.pause_monitor(true); // Keep monitor_driver() from stealing status
+
+  // Variables used by L64xxManager.get_user_input function - some may not be used
+  char axis_mon[3][3] = { {"  "}, {"  "}, {"  "} };   // list of Axes to be monitored
+  L64XX_axis_t axis_index[3];
+  uint16_t axis_status[3];
+  uint8_t driver_count = 1;
+  float position_max;
+  float position_min;
+  float final_feedrate;
+  uint8_t kval_hold;
+  uint8_t OCD_TH_val = 0;
+  uint8_t STALL_TH_val = 0;
+  uint16_t over_current_threshold;
+  constexpr uint8_t over_current_flag = false;  // M916 doesn't play with the overcurrent thresholds
+
+  #define DRIVER_TYPE_L6474(Q) AXIS_DRIVER_TYPE_##Q(L6474)
+
+  uint8_t j;   // general purpose counter
+
+  if (L64xxManager.get_user_input(driver_count, axis_index, axis_mon, position_max, position_min, final_feedrate, kval_hold, over_current_flag, OCD_TH_val, STALL_TH_val, over_current_threshold))
+    return;  // quit if invalid user input
+
+  DEBUG_ECHOLNPAIR("feedrate = ", final_feedrate);
+
+  planner.synchronize();                             // wait for all current movement commands to complete
+
+  const L64XX_Marlin::L64XX_shadow_t &sh = L64xxManager.shadow;
+  for (j = 0; j < driver_count; j++)
+    L64xxManager.get_status(axis_index[j]);  // clear out any pre-existing error flags
+
+  char temp_axis_string[] = " ";
+  temp_axis_string[0] = axis_mon[0][0];  // need to have a string for use within sprintf format section
+  char gcode_string[80];
+  uint16_t status_composite = 0;
+
+  uint16_t M91x_counter = kval_hold;
+  uint16_t M91x_counter_max;
+  if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) {
+    M91x_counter_max = 128;  // TVAL is 7 bits
+    LIMIT(M91x_counter, 0U, 127U);
+  }
+  else
+    M91x_counter_max = 256;  // KVAL_HOLD is 8 bits
+
+  uint8_t M91x_delay_s = parser.byteval('D');   // get delay in seconds
+  millis_t M91x_delay_ms = SEC_TO_MS(M91x_delay_s * 60);
+  millis_t M91x_delay_end;
+
+  DEBUG_ECHOLNPGM(".\n.");
+
+  do {
+
+    if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT)
+      DEBUG_ECHOLNPAIR("TVAL current (mA) = ", (M91x_counter + 1) * sh.AXIS_STALL_CURRENT_CONSTANT_INV);        // report TVAL current for this run
+    else
+      DEBUG_ECHOLNPAIR("kval_hold = ", M91x_counter);                                   // report KVAL_HOLD for this run
+
+    for (j = 0; j < driver_count; j++)
+      L64xxManager.set_param(axis_index[j], L6470_KVAL_HOLD, M91x_counter);  //set KVAL_HOLD or TVAL (same register address)
+
+    M91x_delay_end = millis() + M91x_delay_ms;
+    do {
+      // turn the motor(s) both directions
+      sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_min), uint16_t(final_feedrate));
+      gcode.process_subcommands_now_P(gcode_string);
+
+      sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_max), uint16_t(final_feedrate));
+      gcode.process_subcommands_now_P(gcode_string);
+
+      // get the status after the motors have stopped
+      planner.synchronize();
+
+      status_composite = 0;    // clear out the old bits
+
+      for (j = 0; j < driver_count; j++) {
+        axis_status[j] = (~L64xxManager.get_status(axis_index[j])) & sh.L6470_ERROR_MASK;    // bits of interest are all active low
+        status_composite |= axis_status[j] ;
+      }
+
+      if (status_composite) break;
+    } while (millis() < M91x_delay_end);
+
+    if (status_composite) break;
+
+    M91x_counter++;
+
+  } while (!(status_composite & (sh.STATUS_AXIS_TH_WRN | sh.STATUS_AXIS_TH_SD)) && (M91x_counter < M91x_counter_max));
+
+  DEBUG_ECHOLNPGM(".");
+
+  #if ENABLED(L6470_CHITCHAT)
+    if (status_composite) {
+      L64xxManager.error_status_decode(status_composite, axis_index[0],
+        sh.STATUS_AXIS_TH_SD, sh.STATUS_AXIS_TH_WRN,
+        sh.STATUS_AXIS_STEP_LOSS_A, sh.STATUS_AXIS_STEP_LOSS_B,
+        sh.STATUS_AXIS_OCD, sh.STATUS_AXIS_LAYOUT);
+      DEBUG_ECHOLNPGM(".");
+    }
+  #endif
+
+  if ((status_composite & (sh.STATUS_AXIS_TH_WRN | sh.STATUS_AXIS_TH_SD)))
+    DEBUG_ECHOLNPGM(".\n.\nTest completed normally - Thermal warning/shutdown has occurred");
+  else if (status_composite)
+    DEBUG_ECHOLNPGM(".\n.\nTest completed abnormally - non-thermal error has occured");
+  else
+    DEBUG_ECHOLNPGM(".\n.\nTest completed normally - Unable to get to thermal warning/shutdown");
+
+  L64xxManager.pause_monitor(false);
+}
+
+/**
+ * M917: Find minimum current thresholds
+ *
+ *   Decrease OCD current until overcurrent error
+ *   Increase OCD until overcurrent error goes away
+ *   Decrease stall threshold until stall (not done on L6474)
+ *   Increase stall until stall error goes away (not done on L6474)
+ *
+ * J - select which driver(s) to monitor on multi-driver axis
+ *     0 - (default) monitor all drivers on the axis or E0
+ *     1 - monitor only X, Y, Z, E1
+ *     2 - monitor only X2, Y2, Z2, E2
+ * Xxxx, Yxxx, Zxxx, Exxx - axis to be monitored with displacement
+ *     xxx (1-255) is distance moved on either side of current position
+ *
+ * F - feedrate
+ *     optional - will use default max feedrate from Configuration.h if not specified
+ *
+ * I - starting over-current threshold
+ *     optional - will report current value from driver if not specified
+ *     if there are multiple drivers on the axis then all will be set the same
+ *
+ * T - current (mA) setting for TVAL (0 - 4A in 31.25mA increments, rounds down) - L6474 only
+ *     optional - will report current value from driver if not specified
+ *
+ * K - value for KVAL_HOLD (0 - 255) (ignored for L6474)
+ *     optional - will report current value from driver if not specified
+ */
+void GcodeSuite::M917() {
+
+  DEBUG_ECHOLNPGM("M917");
+
+  L64xxManager.pause_monitor(true); // Keep monitor_driver() from stealing status
+
+  char axis_mon[3][3] = { {"  "}, {"  "}, {"  "} };   // list of Axes to be monitored
+  L64XX_axis_t axis_index[3];
+  uint16_t axis_status[3];
+  uint8_t driver_count = 1;
+  float position_max;
+  float position_min;
+  float final_feedrate;
+  uint8_t kval_hold;
+  uint8_t OCD_TH_val = 0;
+  uint8_t STALL_TH_val = 0;
+  uint16_t over_current_threshold;
+  constexpr uint8_t over_current_flag = true;
+
+  uint8_t j;   // general purpose counter
+
+  if (L64xxManager.get_user_input(driver_count, axis_index, axis_mon, position_max, position_min, final_feedrate, kval_hold, over_current_flag, OCD_TH_val, STALL_TH_val, over_current_threshold))
+    return;  // quit if invalid user input
+
+  DEBUG_ECHOLNPAIR("feedrate = ", final_feedrate);
+
+  planner.synchronize();                             // wait for all current movement commands to complete
+
+  const L64XX_Marlin::L64XX_shadow_t &sh = L64xxManager.shadow;
+  for (j = 0; j < driver_count; j++)
+    L64xxManager.get_status(axis_index[j]);  // clear error flags
+  char temp_axis_string[] = " ";
+  temp_axis_string[0] = axis_mon[0][0];   // need a sprintf format string
+  char gcode_string[80];
+  uint16_t status_composite = 0;
+  uint8_t test_phase = 0;                 // 0 - decreasing OCD - exit when OCD warning occurs (ignore STALL)
+                                          // 1 - increasing OCD - exit when OCD warning stops (ignore STALL)
+                                          // 2 - OCD finalized - decreasing STALL - exit when STALL warning happens
+                                          // 3 - OCD finalized - increasing STALL - exit when STALL warning stop
+                                          // 4 - all testing completed
+  DEBUG_ECHOPAIR(".\n.\n.\nover_current threshold : ", (OCD_TH_val + 1) * 375);   // first status display
+  DEBUG_ECHOPAIR("  (OCD_TH:  : ", OCD_TH_val);
+  if (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT) {
+    DEBUG_ECHOPAIR(")   Stall threshold: ", (STALL_TH_val + 1) * 31.25);
+    DEBUG_ECHOPAIR("  (STALL_TH: ", STALL_TH_val);
+  }
+  DEBUG_ECHOLNPGM(")");
+
+  do {
+
+    if (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT) DEBUG_ECHOPAIR("STALL threshold : ", (STALL_TH_val + 1) * 31.25);
+    DEBUG_ECHOLNPAIR("   OCD threshold : ", (OCD_TH_val + 1) * 375);
+
+    sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_min), uint16_t(final_feedrate));
+    gcode.process_subcommands_now_P(gcode_string);
+
+    sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_max), uint16_t(final_feedrate));
+    gcode.process_subcommands_now_P(gcode_string);
+
+    planner.synchronize();
+
+    status_composite = 0;    // clear out the old bits
+
+    for (j = 0; j < driver_count; j++) {
+      axis_status[j] = (~L64xxManager.get_status(axis_index[j])) & sh.L6470_ERROR_MASK;    // bits of interest are all active low
+      status_composite |= axis_status[j];
+    }
+
+    if (status_composite && (status_composite & sh.STATUS_AXIS_UVLO)) {
+      DEBUG_ECHOLNPGM("Test aborted (Undervoltage lockout active)");
+      #if ENABLED(L6470_CHITCHAT)
+        for (j = 0; j < driver_count; j++) {
+          if (j) DEBUG_ECHOPGM("...");
+          L64xxManager.error_status_decode(axis_status[j], axis_index[j],
+            sh.STATUS_AXIS_TH_SD, sh.STATUS_AXIS_TH_WRN,
+            sh.STATUS_AXIS_STEP_LOSS_A, sh.STATUS_AXIS_STEP_LOSS_B,
+            sh.STATUS_AXIS_OCD, sh.STATUS_AXIS_LAYOUT);
+        }
+      #endif
+      return;
+    }
+
+    if (status_composite & (sh.STATUS_AXIS_TH_WRN | sh.STATUS_AXIS_TH_SD)) {
+      DEBUG_ECHOLNPGM("thermal problem - waiting for chip(s) to cool down ");
+      uint16_t status_composite_temp = 0;
+      uint8_t k = 0;
+      do {
+        k++;
+        if (!(k % 4)) {
+          kval_hold *= 0.95;
+          DEBUG_EOL();
+          DEBUG_ECHOLNPAIR("Lowering KVAL_HOLD by about 5% to ", kval_hold);
+          for (j = 0; j < driver_count; j++)
+            L64xxManager.set_param(axis_index[j], L6470_KVAL_HOLD, kval_hold);
+        }
+        DEBUG_ECHOLNPGM(".");
+        gcode.reset_stepper_timeout(); // keep steppers powered
+        watchdog_refresh();
+        safe_delay(5000);
+        status_composite_temp = 0;
+        for (j = 0; j < driver_count; j++) {
+          axis_status[j] = (~L64xxManager.get_status(axis_index[j])) & sh.L6470_ERROR_MASK;    // bits of interest are all active low
+          status_composite_temp |= axis_status[j];
+        }
+      }
+      while (status_composite_temp & (sh.STATUS_AXIS_TH_WRN | sh.STATUS_AXIS_TH_SD));
+      DEBUG_EOL();
+    }
+    if (status_composite & (sh.STATUS_AXIS_STEP_LOSS_A | sh.STATUS_AXIS_STEP_LOSS_B | sh.STATUS_AXIS_OCD)) {
+      switch (test_phase) {
+
+        case 0: {
+          if (status_composite & sh.STATUS_AXIS_OCD) {
+            // phase 0 with OCD warning - time to go to next phase
+            if (OCD_TH_val >= sh.AXIS_OCD_TH_MAX) {
+              OCD_TH_val = sh.AXIS_OCD_TH_MAX;           // limit to max
+              test_phase = 2;            // at highest value so skip phase 1
+              //DEBUG_ECHOLNPGM("LOGIC E0A OCD at highest - skip to 2");
+              DEBUG_ECHOLNPGM("OCD at highest - OCD finalized");
+            }
+            else {
+              OCD_TH_val++;              // normal exit to next phase
+              test_phase = 1;            // setup for first pass of phase 1
+              //DEBUG_ECHOLNPGM("LOGIC E0B - inc OCD  & go to 1");
+              DEBUG_ECHOLNPGM("inc OCD");
+            }
+          }
+          else {  // phase 0 without OCD warning - keep on decrementing if can
+            if (OCD_TH_val) {
+              OCD_TH_val--;              // try lower value
+              //DEBUG_ECHOLNPGM("LOGIC E0C - dec OCD");
+              DEBUG_ECHOLNPGM("dec OCD");
+            }
+            else {
+              test_phase = 2;            // at lowest value without warning so skip phase 1
+              //DEBUG_ECHOLNPGM("LOGIC E0D - OCD at latest - go to 2");
+              DEBUG_ECHOLNPGM("OCD finalized");
+            }
+          }
+        } break;
+
+        case 1: {
+          if (status_composite & sh.STATUS_AXIS_OCD) {
+            // phase 1 with OCD warning - increment if can
+            if (OCD_TH_val >= sh.AXIS_OCD_TH_MAX) {
+              OCD_TH_val = sh.AXIS_OCD_TH_MAX;           // limit to max
+              test_phase = 2;            // at highest value so go to next phase
+              //DEBUG_ECHOLNPGM("LOGIC E1A - OCD at max - go to 2");
+              DEBUG_ECHOLNPGM("OCD finalized");
+            }
+            else {
+              OCD_TH_val++;              // try a higher value
+              //DEBUG_ECHOLNPGM("LOGIC E1B - inc OCD");
+              DEBUG_ECHOLNPGM("inc OCD");
+            }
+          }
+          else { // phase 1 without OCD warning - normal exit to phase 2
+            test_phase = 2;
+            //DEBUG_ECHOLNPGM("LOGIC E1C - no OCD warning - go to 1");
+            DEBUG_ECHOLNPGM("OCD finalized");
+          }
+        } break;
+
+        case 2: {
+          if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) {  // skip all STALL_TH steps if L6474
+            test_phase = 4;
+            break;
+          }
+          if (status_composite & (sh.STATUS_AXIS_STEP_LOSS_A | sh.STATUS_AXIS_STEP_LOSS_B)) {
+            // phase 2 with stall warning - time to go to next phase
+            if (STALL_TH_val >= 127) {
+              STALL_TH_val = 127;  // limit to max
+              //DEBUG_ECHOLNPGM("LOGIC E2A - STALL warning, STALL at max, quit");
+              DEBUG_ECHOLNPGM("finished - STALL at maximum value but still have stall warning");
+              test_phase = 4;
+            }
+            else {
+              test_phase = 3;              // normal exit to next phase (found failing value of STALL)
+              STALL_TH_val++;              // setup for first pass of phase 3
+              //DEBUG_ECHOLNPGM("LOGIC E2B - INC - STALL warning, inc Stall, go to 3");
+              DEBUG_ECHOLNPGM("inc Stall");
+            }
+          }
+          else {  // phase 2 without stall warning - decrement if can
+            if (STALL_TH_val) {
+              STALL_TH_val--;              // try a lower value
+              //DEBUG_ECHOLNPGM("LOGIC E2C - no STALL, dec STALL");
+              DEBUG_ECHOLNPGM("dec STALL");
+            }
+            else {
+              DEBUG_ECHOLNPGM("finished - STALL at lowest value but still do NOT have stall warning");
+              test_phase = 4;
+              //DEBUG_ECHOLNPGM("LOGIC E2D - no STALL, at lowest so quit");
+            }
+          }
+        } break;
+
+        case 3: {
+          if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) {  // skip all STALL_TH steps if L6474
+            test_phase = 4;
+            break;
+          }
+          if (status_composite & (sh.STATUS_AXIS_STEP_LOSS_A | sh.STATUS_AXIS_STEP_LOSS_B)) {
+            // phase 3 with stall warning - increment if can
+            if (STALL_TH_val >= 127) {
+              STALL_TH_val = 127; // limit to max
+              DEBUG_ECHOLNPGM("finished - STALL at maximum value but still have stall warning");
+              test_phase = 4;
+              //DEBUG_ECHOLNPGM("LOGIC E3A - STALL, at max so quit");
+            }
+            else {
+              STALL_TH_val++;              // still looking for passing value
+              //DEBUG_ECHOLNPGM("LOGIC E3B - STALL, inc stall");
+              DEBUG_ECHOLNPGM("inc stall");
+            }
+          }
+          else {  //phase 3 without stall warning  but have OCD warning
+            DEBUG_ECHOLNPGM("Hardware problem - OCD warning without STALL warning");
+            test_phase = 4;
+            //DEBUG_ECHOLNPGM("LOGIC E3C - not STALLED, hardware problem (quit)");
+          }
+        } break;
+
+      }
+
+    }
+    else {
+      switch (test_phase) {
+        case 0: { // phase 0 without OCD warning - keep on decrementing if can
+          if (OCD_TH_val) {
+            OCD_TH_val--;             // try lower value
+            //DEBUG_ECHOLNPGM("LOGIC N0A - DEC OCD");
+            DEBUG_ECHOLNPGM("DEC OCD");
+          }
+          else {
+            test_phase = 2;           // at lowest value without warning so skip phase 1
+            //DEBUG_ECHOLNPGM("LOGIC N0B - OCD at lowest (go to phase 2)");
+            DEBUG_ECHOLNPGM("OCD finalized");
+          }
+        } break;
+
+        case 1: //DEBUG_ECHOLNPGM("LOGIC N1 (go directly to 2)"); // phase 1 without OCD warning - drop directly to phase 2
+                DEBUG_ECHOLNPGM("OCD finalized");
+
+        case 2: { // phase 2 without stall warning - keep on decrementing if can
+          if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) {  // skip all STALL_TH steps if L6474
+            test_phase = 4;
+            break;
+          }
+          if (STALL_TH_val) {
+            STALL_TH_val--;              // try a lower value (stay in phase 2)
+            //DEBUG_ECHOLNPGM("LOGIC N2B - dec STALL");
+            DEBUG_ECHOLNPGM("dec STALL");
+          }
+          else {
+            DEBUG_ECHOLNPGM("finished - STALL at lowest value but still no stall warning");
+            test_phase = 4;
+            //DEBUG_ECHOLNPGM("LOGIC N2C - STALL at lowest (quit)");
+          }
+        } break;
+
+        case 3: {
+          if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) {  // skip all STALL_TH steps if L6474
+            test_phase = 4;
+            break;
+          }
+          test_phase = 4;
+           //DEBUG_ECHOLNPGM("LOGIC N3 - finished!");
+           DEBUG_ECHOLNPGM("finished!");
+        } break;  // phase 3 without any warnings - desired exit
+      }  //
+    }  // end of status checks
+
+    if (test_phase != 4) {
+      for (j = 0; j < driver_count; j++) {                       // update threshold(s)
+        L64xxManager.set_param(axis_index[j], L6470_OCD_TH, OCD_TH_val);
+        if (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT) L64xxManager.set_param(axis_index[j], L6470_STALL_TH, STALL_TH_val);
+        if (L64xxManager.get_param(axis_index[j], L6470_OCD_TH) != OCD_TH_val) DEBUG_ECHOLNPGM("OCD mismatch");
+        if ((L64xxManager.get_param(axis_index[j], L6470_STALL_TH) != STALL_TH_val) && (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT)) DEBUG_ECHOLNPGM("STALL mismatch");
+      }
+    }
+
+  } while (test_phase != 4);
+
+  DEBUG_ECHOLNPGM(".");
+  if (status_composite) {
+    #if ENABLED(L6470_CHITCHAT)
+      for (j = 0; j < driver_count; j++) {
+        if (j) DEBUG_ECHOPGM("...");
+        L64xxManager.error_status_decode(axis_status[j], axis_index[j],
+          sh.STATUS_AXIS_TH_SD, sh.STATUS_AXIS_TH_WRN,
+          sh.STATUS_AXIS_STEP_LOSS_A, sh.STATUS_AXIS_STEP_LOSS_B,
+          sh.STATUS_AXIS_OCD, sh.STATUS_AXIS_LAYOUT);
+      }
+      DEBUG_ECHOLNPGM(".");
+    #endif
+    DEBUG_ECHOLNPGM("Completed with errors");
+  }
+  else
+    DEBUG_ECHOLNPGM("Completed with no errors");
+  DEBUG_ECHOLNPGM(".");
+
+  L64xxManager.pause_monitor(false);
+}
+
+/**
+ * M918: increase speed until error or max feedrate achieved (as shown in configuration.h))
+ *
+ * J - select which driver(s) to monitor on multi-driver axis
+ *     0 - (default) monitor all drivers on the axis or E0
+ *     1 - monitor only X, Y, Z, E1
+ *     2 - monitor only X2, Y2, Z2, E2
+ * Xxxx, Yxxx, Zxxx, Exxx - axis to be monitored with displacement
+ *     xxx (1-255) is distance moved on either side of current position
+ *
+ * I - over current threshold
+ *     optional - will report current value from driver if not specified
+ *
+ * T - current (mA) setting for TVAL (0 - 4A in 31.25mA increments, rounds down) - L6474 only
+ *     optional - will report current value from driver if not specified
+ *
+ * K - value for KVAL_HOLD (0 - 255) (ignored for L6474)
+ *     optional - will report current value from driver if not specified
+ *
+ * M - value for microsteps (1 - 128) (optional)
+ *     optional - will report current value from driver if not specified
+ */
+void GcodeSuite::M918() {
+
+  DEBUG_ECHOLNPGM("M918");
+
+  L64xxManager.pause_monitor(true); // Keep monitor_driver() from stealing status
+
+  char axis_mon[3][3] = { {"  "}, {"  "}, {"  "} };   // list of Axes to be monitored
+  L64XX_axis_t axis_index[3];
+  uint16_t axis_status[3];
+  uint8_t driver_count = 1;
+  float position_max, position_min;
+  float final_feedrate;
+  uint8_t kval_hold;
+  uint8_t OCD_TH_val = 0;
+  uint8_t STALL_TH_val = 0;
+  uint16_t over_current_threshold;
+  constexpr uint8_t over_current_flag = true;
+
+  const L64XX_Marlin::L64XX_shadow_t &sh = L64xxManager.shadow;
+
+  uint8_t j;   // general purpose counter
+
+  if (L64xxManager.get_user_input(driver_count, axis_index, axis_mon, position_max, position_min, final_feedrate, kval_hold, over_current_flag, OCD_TH_val, STALL_TH_val, over_current_threshold))
+    return;  // quit if invalid user input
+
+  L64xxManager.get_status(axis_index[0]); // populate shadow array
+
+  uint8_t m_steps = parser.byteval('M');
+
+  if (m_steps != 0) {
+    LIMIT(m_steps, 1, sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT ? 16 : 128);  // L6474
+
+    uint8_t stepVal;
+    for (stepVal = 0; stepVal < 8; stepVal++) {  // convert to L64xx register value
+      if (m_steps == 1) break;
+      m_steps >>= 1;
+    }
+
+    if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT)
+      stepVal |= 0x98;  // NO SYNC
+    else
+      stepVal |= (!SYNC_EN) | SYNC_SEL_1 | stepVal;
+
+    for (j = 0; j < driver_count; j++) {
+      L64xxManager.set_param(axis_index[j], dSPIN_HARD_HIZ, 0);          // can't write STEP register if stepper being powered
+                                                                         //   results in an extra NOOP being sent (data 00)
+      L64xxManager.set_param(axis_index[j], L6470_STEP_MODE, stepVal);   // set microsteps
+    }
+  }
+  m_steps = L64xxManager.get_param(axis_index[0], L6470_STEP_MODE) & 0x07;   // get microsteps
+
+  DEBUG_ECHOLNPAIR("Microsteps = ", _BV(m_steps));
+  DEBUG_ECHOLNPAIR("target (maximum) feedrate = ", final_feedrate);
+
+  const float feedrate_inc = final_feedrate / 10,   // Start at 1/10 of max & go up by 1/10 per step
+              fr_limit = final_feedrate * 0.99f;    // Rounding-safe comparison value
+  float current_feedrate = 0;
+
+  planner.synchronize();                            // Wait for moves to complete
+
+  for (j = 0; j < driver_count; j++)
+    L64xxManager.get_status(axis_index[j]);         // Clear error flags
+
+  char temp_axis_string[2] = " ";
+  temp_axis_string[0] = axis_mon[0][0];             // Need a sprintf format string
+  //temp_axis_string[1] = '\n';
+
+  char gcode_string[80];
+  uint16_t status_composite = 0;
+  DEBUG_ECHOLNPGM(".\n.\n.");                       // Make feedrate outputs easier to read
+
+  do {
+    current_feedrate += feedrate_inc;
+    DEBUG_ECHOLNPAIR("...feedrate = ", current_feedrate);
+
+    sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_min), uint16_t(current_feedrate));
+    gcode.process_subcommands_now_P(gcode_string);
+
+    sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_max), uint16_t(current_feedrate));
+    gcode.process_subcommands_now_P(gcode_string);
+
+    planner.synchronize();
+
+    for (j = 0; j < driver_count; j++) {
+      axis_status[j] = (~L64xxManager.get_status(axis_index[j])) & 0x0800;  // Bits of interest are all active LOW
+      status_composite |= axis_status[j];
+    }
+    if (status_composite) break;              // Break on any error
+  } while (current_feedrate < fr_limit);
+
+  DEBUG_ECHOPGM("Completed with ");
+  if (status_composite) {
+    DEBUG_ECHOLNPGM("errors");
+    #if ENABLED(L6470_CHITCHAT)
+      for (j = 0; j < driver_count; j++) {
+        if (j) DEBUG_ECHOPGM("...");
+        L64xxManager.error_status_decode(axis_status[j], axis_index[j],
+          sh.STATUS_AXIS_TH_SD, sh.STATUS_AXIS_TH_WRN,
+          sh.STATUS_AXIS_STEP_LOSS_A, sh.STATUS_AXIS_STEP_LOSS_B,
+          sh.STATUS_AXIS_OCD, sh.STATUS_AXIS_LAYOUT);
+      }
+    #endif
+  }
+  else
+    DEBUG_ECHOLNPGM("no errors");
+
+  L64xxManager.pause_monitor(false);
+}
+
+#endif // HAS_L64XX
diff --git a/Marlin/src/gcode/feature/advance/M900.cpp b/Marlin/src/gcode/feature/advance/M900.cpp
new file mode 100644
index 0000000..5c7155d
--- /dev/null
+++ b/Marlin/src/gcode/feature/advance/M900.cpp
@@ -0,0 +1,147 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(LIN_ADVANCE)
+
+#include "../../gcode.h"
+#include "../../../module/planner.h"
+#include "../../../module/stepper.h"
+
+#if ENABLED(EXTRA_LIN_ADVANCE_K)
+  float other_extruder_advance_K[EXTRUDERS];
+  uint8_t lin_adv_slot = 0;
+#endif
+
+/**
+ * M900: Get or Set Linear Advance K-factor
+ *  T<tool>     Which tool to address
+ *  K<factor>   Set current advance K factor (Slot 0).
+ *  L<factor>   Set secondary advance K factor (Slot 1). Requires EXTRA_LIN_ADVANCE_K.
+ *  S<0/1>      Activate slot 0 or 1. Requires EXTRA_LIN_ADVANCE_K.
+ */
+void GcodeSuite::M900() {
+
+  auto echo_value_oor = [](const char ltr, const bool ten=true) {
+    SERIAL_CHAR('?'); SERIAL_CHAR(ltr);
+    SERIAL_ECHOPGM(" value out of range");
+    if (ten) SERIAL_ECHOPGM(" (0-10)");
+    SERIAL_ECHOLNPGM(".");
+  };
+
+  #if EXTRUDERS < 2
+    constexpr uint8_t tool_index = 0;
+  #else
+    const uint8_t tool_index = parser.intval('T', active_extruder);
+    if (tool_index >= EXTRUDERS) {
+      echo_value_oor('T', false);
+      return;
+    }
+  #endif
+
+  float &kref = planner.extruder_advance_K[tool_index], newK = kref;
+  const float oldK = newK;
+
+  #if ENABLED(EXTRA_LIN_ADVANCE_K)
+
+    float &lref = other_extruder_advance_K[tool_index];
+
+    const bool old_slot = TEST(lin_adv_slot, tool_index), // The tool's current slot (0 or 1)
+               new_slot = parser.boolval('S', old_slot);  // The passed slot (default = current)
+
+    // If a new slot is being selected swap the current and
+    // saved K values. Do here so K/L will apply correctly.
+    if (new_slot != old_slot) {                       // Not the same slot?
+      SET_BIT_TO(lin_adv_slot, tool_index, new_slot); // Update the slot for the tool
+      newK = lref;                                    // Get new K value from backup
+      lref = oldK;                                    // Save K to backup
+    }
+
+    // Set the main K value. Apply if the main slot is active.
+    if (parser.seenval('K')) {
+      const float K = parser.value_float();
+      if (!WITHIN(K, 0, 10)) echo_value_oor('K');
+      else if (new_slot)        lref = K;             // S1 Knn
+      else                      newK = K;             // S0 Knn
+    }
+
+    // Set the extra K value. Apply if the extra slot is active.
+    if (parser.seenval('L')) {
+      const float L = parser.value_float();
+      if (!WITHIN(L, 0, 10)) echo_value_oor('L');
+      else if (!new_slot)       lref = L;             // S0 Lnn
+      else                      newK = L;             // S1 Lnn
+    }
+
+  #else
+
+    if (parser.seenval('K')) {
+      const float K = parser.value_float();
+      if (WITHIN(K, 0, 10))
+        newK = K;
+      else
+        echo_value_oor('K');
+    }
+
+  #endif
+
+  if (newK != oldK) {
+    planner.synchronize();
+    kref = newK;
+  }
+
+  if (!parser.seen_any()) {
+
+    #if ENABLED(EXTRA_LIN_ADVANCE_K)
+
+      #if EXTRUDERS < 2
+        SERIAL_ECHOLNPAIR("Advance S", int(new_slot), " K", kref, "(S", int(!new_slot), " K", lref, ")");
+      #else
+        LOOP_L_N(i, EXTRUDERS) {
+          const bool slot = TEST(lin_adv_slot, i);
+          SERIAL_ECHOLNPAIR("Advance T", int(i), " S", int(slot), " K", planner.extruder_advance_K[i],
+                            "(S", int(!slot), " K", other_extruder_advance_K[i], ")");
+          SERIAL_EOL();
+        }
+      #endif
+
+    #else
+
+      SERIAL_ECHO_START();
+      #if EXTRUDERS < 2
+        SERIAL_ECHOLNPAIR("Advance K=", planner.extruder_advance_K[0]);
+      #else
+        SERIAL_ECHOPGM("Advance K");
+        LOOP_L_N(i, EXTRUDERS) {
+          SERIAL_CHAR(' ', '0' + i, ':');
+          SERIAL_DECIMAL(planner.extruder_advance_K[i]);
+        }
+        SERIAL_EOL();
+      #endif
+
+    #endif
+  }
+
+}
+
+#endif // LIN_ADVANCE
diff --git a/Marlin/src/gcode/feature/baricuda/M126-M129.cpp b/Marlin/src/gcode/feature/baricuda/M126-M129.cpp
new file mode 100644
index 0000000..edeba0d
--- /dev/null
+++ b/Marlin/src/gcode/feature/baricuda/M126-M129.cpp
@@ -0,0 +1,58 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(BARICUDA)
+
+#include "../../gcode.h"
+#include "../../../feature/baricuda.h"
+
+#if HAS_HEATER_1
+
+  /**
+   * M126: Heater 1 valve open
+   */
+  void GcodeSuite::M126() { baricuda_valve_pressure = parser.byteval('S', 255); }
+
+  /**
+   * M127: Heater 1 valve close
+   */
+  void GcodeSuite::M127() { baricuda_valve_pressure = 0; }
+
+#endif // HAS_HEATER_1
+
+#if HAS_HEATER_2
+
+  /**
+   * M128: Heater 2 valve open
+   */
+  void GcodeSuite::M128() { baricuda_e_to_p_pressure = parser.byteval('S', 255); }
+
+  /**
+   * M129: Heater 2 valve close
+   */
+  void GcodeSuite::M129() { baricuda_e_to_p_pressure = 0; }
+
+#endif // HAS_HEATER_2
+
+#endif // BARICUDA
diff --git a/Marlin/src/gcode/feature/camera/M240.cpp b/Marlin/src/gcode/feature/camera/M240.cpp
new file mode 100644
index 0000000..fc350d8
--- /dev/null
+++ b/Marlin/src/gcode/feature/camera/M240.cpp
@@ -0,0 +1,204 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(PHOTO_GCODE)
+
+#include "../../gcode.h"
+#include "../../../module/motion.h" // for active_extruder and current_position
+
+#if PIN_EXISTS(CHDK)
+  millis_t chdk_timeout; // = 0
+#endif
+
+#if defined(PHOTO_POSITION) && PHOTO_DELAY_MS > 0
+  #include "../../../MarlinCore.h" // for idle()
+#endif
+
+#ifdef PHOTO_RETRACT_MM
+
+  #define _PHOTO_RETRACT_MM (PHOTO_RETRACT_MM + 0)
+
+  #include "../../../module/planner.h"
+  #include "../../../module/temperature.h"
+
+  #if ENABLED(ADVANCED_PAUSE_FEATURE)
+    #include "../../../feature/pause.h"
+  #endif
+
+  #ifdef PHOTO_RETRACT_MM
+    inline void e_move_m240(const float length, const feedRate_t &fr_mm_s) {
+      if (length && thermalManager.hotEnoughToExtrude(active_extruder))
+        unscaled_e_move(length, fr_mm_s);
+    }
+  #endif
+
+#endif
+
+#if PIN_EXISTS(PHOTOGRAPH)
+
+  FORCE_INLINE void set_photo_pin(const uint8_t state) {
+    constexpr uint32_t pulse_length = (
+      #ifdef PHOTO_PULSES_US
+        PHOTO_PULSE_DELAY_US
+      #else
+        15                    // 15.24 from _delay_ms(0.01524)
+      #endif
+    );
+    WRITE(PHOTOGRAPH_PIN, state);
+    delayMicroseconds(pulse_length);
+  }
+
+  FORCE_INLINE void tweak_photo_pin() { set_photo_pin(HIGH); set_photo_pin(LOW); }
+
+  #ifdef PHOTO_PULSES_US
+
+    inline void pulse_photo_pin(const uint32_t duration, const uint8_t state) {
+      if (state) {
+        for (const uint32_t stop = micros() + duration; micros() < stop;)
+          tweak_photo_pin();
+      }
+      else
+        delayMicroseconds(duration);
+    }
+
+    inline void spin_photo_pin() {
+      static constexpr uint32_t sequence[] = PHOTO_PULSES_US;
+      LOOP_L_N(i, COUNT(sequence))
+        pulse_photo_pin(sequence[i], !(i & 1));
+    }
+
+  #else
+
+    constexpr uint8_t NUM_PULSES = 16;
+    inline void spin_photo_pin() { for (uint8_t i = NUM_PULSES; i--;) tweak_photo_pin(); }
+
+  #endif
+#endif
+
+/**
+ * M240: Trigger a camera by...
+ *
+ *  - CHDK                  : Emulate a Canon RC-1 with a configurable ON duration.
+ *                            https://captain-slow.dk/2014/03/09/3d-printing-timelapses/
+ *  - PHOTOGRAPH_PIN        : Pulse a digital pin 16 times.
+ *                            See https://www.doc-diy.net/photo/rc-1_hacked/
+ *  - PHOTO_SWITCH_POSITION : Bump a physical switch with the X-carriage using a
+ *                            configured position, delay, and retract length.
+ *
+ * PHOTO_POSITION parameters:
+ *    A - X offset to the return position
+ *    B - Y offset to the return position
+ *    F - Override the XY movement feedrate
+ *    R - Retract/recover length (current units)
+ *    S - Retract/recover feedrate (mm/m)
+ *    X - Move to X before triggering the shutter
+ *    Y - Move to Y before triggering the shutter
+ *    Z - Raise Z by a distance before triggering the shutter
+ *
+ * PHOTO_SWITCH_POSITION parameters:
+ *    D - Duration (ms) to hold down switch (Requires PHOTO_SWITCH_MS)
+ *    P - Delay (ms) after triggering the shutter (Requires PHOTO_SWITCH_MS)
+ *    I - Switch trigger position override X
+ *    J - Switch trigger position override Y
+ */
+void GcodeSuite::M240() {
+
+  #ifdef PHOTO_POSITION
+
+    if (homing_needed_error()) return;
+
+    const xyz_pos_t old_pos = {
+      current_position.x + parser.linearval('A'),
+      current_position.y + parser.linearval('B'),
+      current_position.z
+    };
+
+    #ifdef PHOTO_RETRACT_MM
+      const float rval = parser.seenval('R') ? parser.value_linear_units() : _PHOTO_RETRACT_MM;
+      feedRate_t sval = (
+        #if ENABLED(ADVANCED_PAUSE_FEATURE)
+          PAUSE_PARK_RETRACT_FEEDRATE
+        #elif ENABLED(FWRETRACT)
+          RETRACT_FEEDRATE
+        #else
+          45
+        #endif
+      );
+      if (parser.seenval('S')) sval = parser.value_feedrate();
+      e_move_m240(-rval, sval);
+    #endif
+
+    feedRate_t fr_mm_s = MMM_TO_MMS(parser.linearval('F'));
+    if (fr_mm_s) NOLESS(fr_mm_s, 10.0f);
+
+    constexpr xyz_pos_t photo_position = PHOTO_POSITION;
+    xyz_pos_t raw = {
+       parser.seenval('X') ? RAW_X_POSITION(parser.value_linear_units()) : photo_position.x,
+       parser.seenval('Y') ? RAW_Y_POSITION(parser.value_linear_units()) : photo_position.y,
+      (parser.seenval('Z') ? parser.value_linear_units() : photo_position.z) + current_position.z
+    };
+    apply_motion_limits(raw);
+    do_blocking_move_to(raw, fr_mm_s);
+
+    #ifdef PHOTO_SWITCH_POSITION
+      constexpr xy_pos_t photo_switch_position = PHOTO_SWITCH_POSITION;
+      const xy_pos_t sraw = {
+         parser.seenval('I') ? RAW_X_POSITION(parser.value_linear_units()) : photo_switch_position.x,
+         parser.seenval('J') ? RAW_Y_POSITION(parser.value_linear_units()) : photo_switch_position.y
+      };
+      do_blocking_move_to_xy(sraw, get_homing_bump_feedrate(X_AXIS));
+      #if PHOTO_SWITCH_MS > 0
+        safe_delay(parser.intval('D', PHOTO_SWITCH_MS));
+      #endif
+      do_blocking_move_to(raw);
+    #endif
+
+  #endif
+
+  #if PIN_EXISTS(CHDK)
+
+    OUT_WRITE(CHDK_PIN, HIGH);
+    chdk_timeout = millis() + parser.intval('D', PHOTO_SWITCH_MS);
+
+  #elif HAS_PHOTOGRAPH
+
+    spin_photo_pin();
+    delay(7.33);
+    spin_photo_pin();
+
+  #endif
+
+  #ifdef PHOTO_POSITION
+    #if PHOTO_DELAY_MS > 0
+      const millis_t timeout = millis() + parser.intval('P', PHOTO_DELAY_MS);
+      while (PENDING(millis(), timeout)) idle();
+    #endif
+    do_blocking_move_to(old_pos, fr_mm_s);
+    #ifdef PHOTO_RETRACT_MM
+      e_move_m240(rval, sval);
+    #endif
+  #endif
+}
+
+#endif // PHOTO_GCODE
diff --git a/Marlin/src/gcode/feature/cancel/M486.cpp b/Marlin/src/gcode/feature/cancel/M486.cpp
new file mode 100644
index 0000000..1f14ae0
--- /dev/null
+++ b/Marlin/src/gcode/feature/cancel/M486.cpp
@@ -0,0 +1,57 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(CANCEL_OBJECTS)
+
+#include "../../gcode.h"
+#include "../../../feature/cancel_object.h"
+
+/**
+ * M486: A simple interface to cancel objects
+ *
+ *   T[count] : Reset objects and/or set the count
+ *   S<index> : Start an object with the given index
+ *   P<index> : Cancel the object with the given index
+ *   U<index> : Un-cancel object with the given index
+ *   C        : Cancel the current object (the last index given by S<index>)
+ *   S-1      : Start a non-object like a brim or purge tower that should always print
+ */
+void GcodeSuite::M486() {
+
+  if (parser.seen('T')) {
+    cancelable.reset();
+    cancelable.object_count = parser.intval('T', 1);
+  }
+
+  if (parser.seen('S'))
+    cancelable.set_active_object(parser.value_int());
+
+  if (parser.seen('C')) cancelable.cancel_active_object();
+
+  if (parser.seen('P')) cancelable.cancel_object(parser.value_int());
+
+  if (parser.seen('U')) cancelable.uncancel_object(parser.value_int());
+}
+
+#endif // CANCEL_OBJECTS
diff --git a/Marlin/src/gcode/feature/caselight/M355.cpp b/Marlin/src/gcode/feature/caselight/M355.cpp
new file mode 100644
index 0000000..12ae5cf
--- /dev/null
+++ b/Marlin/src/gcode/feature/caselight/M355.cpp
@@ -0,0 +1,73 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(CASE_LIGHT_ENABLE)
+
+#include "../../../feature/caselight.h"
+#include "../../gcode.h"
+
+/**
+ * M355: Turn case light on/off and set brightness
+ *
+ *   P<byte>  Set case light brightness (PWM pin required - ignored otherwise)
+ *
+ *   S<bool>  Set case light on/off
+ *
+ *   When S turns on the light on a PWM pin then the current brightness level is used/restored
+ *
+ *   M355 P200 S0 turns off the light & sets the brightness level
+ *   M355 S1 turns on the light with a brightness of 200 (assuming a PWM pin)
+ */
+void GcodeSuite::M355() {
+  bool didset = false;
+  #if CASELIGHT_USES_BRIGHTNESS
+    if (parser.seenval('P')) {
+      didset = true;
+      caselight.brightness = parser.value_byte();
+    }
+  #endif
+  const bool sflag = parser.seenval('S');
+  if (sflag) {
+    didset = true;
+    caselight.on = parser.value_bool();
+  }
+  if (didset) caselight.update(sflag);
+
+  // Always report case light status
+  SERIAL_ECHO_START();
+  SERIAL_ECHOPGM("Case light: ");
+  if (!caselight.on)
+    SERIAL_ECHOLNPGM(STR_OFF);
+  else {
+    #if CASELIGHT_USES_BRIGHTNESS
+      if (PWM_PIN(CASE_LIGHT_PIN)) {
+        SERIAL_ECHOLN(int(caselight.brightness));
+        return;
+      }
+    #endif
+    SERIAL_ECHOLNPGM(STR_ON);
+  }
+}
+
+#endif // CASE_LIGHT_ENABLE
diff --git a/Marlin/src/gcode/feature/clean/G12.cpp b/Marlin/src/gcode/feature/clean/G12.cpp
new file mode 100644
index 0000000..216db5b
--- /dev/null
+++ b/Marlin/src/gcode/feature/clean/G12.cpp
@@ -0,0 +1,80 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(NOZZLE_CLEAN_FEATURE)
+
+#include "../../../libs/nozzle.h"
+
+#include "../../gcode.h"
+#include "../../parser.h"
+#include "../../../module/motion.h"
+
+#if HAS_LEVELING
+  #include "../../../module/planner.h"
+  #include "../../../feature/bedlevel/bedlevel.h"
+#endif
+
+/**
+ * G12: Clean the nozzle
+ *
+ *  E<bool>          : 0=Never or 1=Always apply the "software endstop" limits
+ *  P0 S<strokes>    : Stroke cleaning with S strokes
+ *  P1 Sn T<objects> : Zigzag cleaning with S repeats and T zigzags
+ *  P2 Sn R<radius>  : Circle cleaning with S repeats and R radius
+ */
+void GcodeSuite::G12() {
+  // Don't allow nozzle cleaning without homing first
+  if (homing_needed_error()) return;
+
+  #ifdef WIPE_SEQUENCE_COMMANDS
+    if (!parser.seen_any()) {
+      gcode.process_subcommands_now_P(PSTR(WIPE_SEQUENCE_COMMANDS));
+      return;
+    }
+  #endif
+
+  const uint8_t pattern = parser.ushortval('P', 0),
+                strokes = parser.ushortval('S', NOZZLE_CLEAN_STROKES),
+                objects = parser.ushortval('T', NOZZLE_CLEAN_TRIANGLES);
+  const float radius = parser.linearval('R', NOZZLE_CLEAN_CIRCLE_RADIUS);
+
+  const bool seenxyz = parser.seen("XYZ");
+  const uint8_t cleans =  (!seenxyz || parser.boolval('X') ? _BV(X_AXIS) : 0)
+                        | (!seenxyz || parser.boolval('Y') ? _BV(Y_AXIS) : 0)
+                        | TERN(NOZZLE_CLEAN_NO_Z, 0, (!seenxyz || parser.boolval('Z') ? _BV(Z_AXIS) : 0))
+                      ;
+
+  #if HAS_LEVELING
+    // Disable bed leveling if cleaning Z
+    TEMPORARY_BED_LEVELING_STATE(!TEST(cleans, Z_AXIS) && planner.leveling_active);
+  #endif
+
+  SET_SOFT_ENDSTOP_LOOSE(!parser.boolval('E'));
+
+  nozzle.clean(pattern, strokes, radius, objects, cleans);
+
+  SET_SOFT_ENDSTOP_LOOSE(false);
+}
+
+#endif // NOZZLE_CLEAN_FEATURE
diff --git a/Marlin/src/gcode/feature/controllerfan/M710.cpp b/Marlin/src/gcode/feature/controllerfan/M710.cpp
new file mode 100644
index 0000000..cc45073
--- /dev/null
+++ b/Marlin/src/gcode/feature/controllerfan/M710.cpp
@@ -0,0 +1,81 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfigPre.h"
+
+#if ENABLED(CONTROLLER_FAN_EDITABLE)
+
+#include "../../gcode.h"
+#include "../../../feature/controllerfan.h"
+
+void M710_report(const bool forReplay) {
+  if (!forReplay) { SERIAL_ECHOLNPGM("; Controller Fan"); SERIAL_ECHO_START(); }
+  SERIAL_ECHOLNPAIR("  M710"
+    " S", int(controllerFan.settings.active_speed),
+    " I", int(controllerFan.settings.idle_speed),
+    " A", int(controllerFan.settings.auto_mode),
+    " D", controllerFan.settings.duration,
+    " ; (", (int(controllerFan.settings.active_speed) * 100) / 255, "%"
+    " ", (int(controllerFan.settings.idle_speed) * 100) / 255, "%)"
+  );
+}
+
+/**
+ * M710: Set controller fan settings
+ *
+ *  R         : Reset to defaults
+ *  S[0-255]  : Fan speed when motors are active
+ *  I[0-255]  : Fan speed when motors are idle
+ *  A[0|1]    : Turn auto mode on or off
+ *  D         : Set auto mode idle duration
+ *
+ * Examples:
+ *   M710                   ; Report current Settings
+ *   M710 R                 ; Reset SIAD to defaults
+ *   M710 I64               ; Set controller fan Idle Speed to 25%
+ *   M710 S255              ; Set controller fan Active Speed to 100%
+ *   M710 S0                ; Set controller fan Active Speed to OFF
+ *   M710 I255 A0           ; Set controller fan Idle Speed to 100% with Auto Mode OFF
+ *   M710 I127 A1 S255 D160 ; Set controller fan idle speed 50%, AutoMode On, Fan speed 100%, duration to 160 Secs
+ */
+void GcodeSuite::M710() {
+
+  const bool seenR = parser.seen('R');
+  if (seenR) controllerFan.reset();
+
+  const bool seenS = parser.seenval('S');
+  if (seenS) controllerFan.settings.active_speed = parser.value_byte();
+
+  const bool seenI = parser.seenval('I');
+  if (seenI) controllerFan.settings.idle_speed = parser.value_byte();
+
+  const bool seenA = parser.seenval('A');
+  if (seenA) controllerFan.settings.auto_mode = parser.value_bool();
+
+  const bool seenD = parser.seenval('D');
+  if (seenD) controllerFan.settings.duration = parser.value_ushort();
+
+  if (!(seenR || seenS || seenI || seenA || seenD))
+    M710_report(false);
+}
+
+#endif // CONTROLLER_FAN_EDITABLE
diff --git a/Marlin/src/gcode/feature/digipot/M907-M910.cpp b/Marlin/src/gcode/feature/digipot/M907-M910.cpp
new file mode 100644
index 0000000..e463666
--- /dev/null
+++ b/Marlin/src/gcode/feature/digipot/M907-M910.cpp
@@ -0,0 +1,102 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ANY(HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_PWM, HAS_MOTOR_CURRENT_I2C, HAS_MOTOR_CURRENT_DAC)
+
+#include "../../gcode.h"
+
+#if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM
+  #include "../../../module/stepper.h"
+#endif
+
+#if HAS_MOTOR_CURRENT_I2C
+  #include "../../../feature/digipot/digipot.h"
+#endif
+
+#if ENABLED(HAS_MOTOR_CURRENT_DAC)
+  #include "../../../feature/dac/stepper_dac.h"
+#endif
+
+/**
+ * M907: Set digital trimpot motor current using axis codes X, Y, Z, E, B, S
+ */
+void GcodeSuite::M907() {
+  #if HAS_MOTOR_CURRENT_SPI
+
+    LOOP_XYZE(i) if (parser.seenval(axis_codes[i])) stepper.set_digipot_current(i, parser.value_int());
+    if (parser.seenval('B')) stepper.set_digipot_current(4, parser.value_int());
+    if (parser.seenval('S')) LOOP_LE_N(i, 4) stepper.set_digipot_current(i, parser.value_int());
+
+  #elif HAS_MOTOR_CURRENT_PWM
+
+    #if ANY_PIN(MOTOR_CURRENT_PWM_X, MOTOR_CURRENT_PWM_Y, MOTOR_CURRENT_PWM_XY)
+      if (parser.seenval('X') || parser.seenval('Y')) stepper.set_digipot_current(0, parser.value_int());
+    #endif
+    #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
+      if (parser.seenval('Z')) stepper.set_digipot_current(1, parser.value_int());
+    #endif
+    #if PIN_EXISTS(MOTOR_CURRENT_PWM_E)
+      if (parser.seenval('E')) stepper.set_digipot_current(2, parser.value_int());
+    #endif
+
+  #endif
+
+  #if HAS_MOTOR_CURRENT_I2C
+    // this one uses actual amps in floating point
+    LOOP_XYZE(i) if (parser.seenval(axis_codes[i])) digipot_i2c.set_current(i, parser.value_float());
+    // Additional extruders use B,C,D for channels 4,5,6.
+    // TODO: Change these parameters because 'E' is used. B<index>?
+    for (uint8_t i = E_AXIS + 1; i < DIGIPOT_I2C_NUM_CHANNELS; i++)
+      if (parser.seenval('B' + i - (E_AXIS + 1))) digipot_i2c.set_current(i, parser.value_float());
+  #endif
+
+  #if ENABLED(HAS_MOTOR_CURRENT_DAC)
+    if (parser.seenval('S')) {
+      const float dac_percent = parser.value_float();
+      LOOP_LE_N(i, 4) stepper_dac.set_current_percent(i, dac_percent);
+    }
+    LOOP_XYZE(i) if (parser.seenval(axis_codes[i])) stepper_dac.set_current_percent(i, parser.value_float());
+  #endif
+}
+
+#if EITHER(HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_DAC)
+
+  /**
+   * M908: Control digital trimpot directly (M908 P<pin> S<current>)
+   */
+  void GcodeSuite::M908() {
+    TERN_(HAS_MOTOR_CURRENT_SPI, stepper.set_digipot_value_spi(parser.intval('P'), parser.intval('S')));
+    TERN_(HAS_MOTOR_CURRENT_DAC, stepper_dac.set_current_value(parser.byteval('P', -1), parser.ushortval('S', 0)));
+  }
+
+  #if ENABLED(HAS_MOTOR_CURRENT_DAC)
+
+    void GcodeSuite::M909() { stepper_dac.print_values(); }
+    void GcodeSuite::M910() { stepper_dac.commit_eeprom(); }
+
+  #endif // HAS_MOTOR_CURRENT_DAC
+
+#endif // HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_DAC
+
+#endif // HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM || HAS_MOTOR_CURRENT_I2C || HAS_MOTOR_CURRENT_DAC
diff --git a/Marlin/src/gcode/feature/filwidth/M404-M407.cpp b/Marlin/src/gcode/feature/filwidth/M404-M407.cpp
new file mode 100644
index 0000000..a70f7a6
--- /dev/null
+++ b/Marlin/src/gcode/feature/filwidth/M404-M407.cpp
@@ -0,0 +1,71 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(FILAMENT_WIDTH_SENSOR)
+
+#include "../../../feature/filwidth.h"
+#include "../../../module/planner.h"
+#include "../../../MarlinCore.h"
+#include "../../gcode.h"
+
+/**
+ * M404: Display or set (in current units) the nominal filament width (3mm, 1.75mm ) W<3.0>
+ */
+void GcodeSuite::M404() {
+  if (parser.seenval('W')) {
+    filwidth.nominal_mm = parser.value_linear_units();
+    planner.volumetric_area_nominal = CIRCLE_AREA(filwidth.nominal_mm * 0.5);
+  }
+  else
+    SERIAL_ECHOLNPAIR("Filament dia (nominal mm):", filwidth.nominal_mm);
+}
+
+/**
+ * M405: Turn on filament sensor for control
+ */
+void GcodeSuite::M405() {
+  // This is technically a linear measurement, but since it's quantized to centimeters and is a different
+  // unit than everything else, it uses parser.value_byte() instead of parser.value_linear_units().
+  if (parser.seenval('D'))
+    filwidth.set_delay_cm(parser.value_byte());
+
+  filwidth.enable(true);
+}
+
+/**
+ * M406: Turn off filament sensor for control
+ */
+void GcodeSuite::M406() {
+  filwidth.enable(false);
+  planner.calculate_volumetric_multipliers();   // Restore correct 'volumetric_multiplier' value
+}
+
+/**
+ * M407: Get measured filament diameter on serial output
+ */
+void GcodeSuite::M407() {
+  SERIAL_ECHOLNPAIR("Filament dia (measured mm):", filwidth.measured_mm);
+}
+
+#endif // FILAMENT_WIDTH_SENSOR
diff --git a/Marlin/src/gcode/feature/fwretract/G10_G11.cpp b/Marlin/src/gcode/feature/fwretract/G10_G11.cpp
new file mode 100644
index 0000000..219502f
--- /dev/null
+++ b/Marlin/src/gcode/feature/fwretract/G10_G11.cpp
@@ -0,0 +1,51 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(FWRETRACT)
+
+#include "../../../feature/fwretract.h"
+#include "../../gcode.h"
+#include "../../../module/motion.h"
+
+/**
+ * G10 - Retract filament according to settings of M207
+ *       TODO: Handle 'G10 P' for tool settings and 'G10 L' for workspace settings
+ */
+void GcodeSuite::G10() {
+  #if HAS_MULTI_EXTRUDER
+    const bool rs = parser.boolval('S');
+  #endif
+  fwretract.retract(true
+    #if HAS_MULTI_EXTRUDER
+      , rs
+    #endif
+  );
+}
+
+/**
+ * G11 - Recover filament according to settings of M208
+ */
+void GcodeSuite::G11() { fwretract.retract(false); }
+
+#endif // FWRETRACT
diff --git a/Marlin/src/gcode/feature/fwretract/M207-M209.cpp b/Marlin/src/gcode/feature/fwretract/M207-M209.cpp
new file mode 100644
index 0000000..538f16c
--- /dev/null
+++ b/Marlin/src/gcode/feature/fwretract/M207-M209.cpp
@@ -0,0 +1,74 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(FWRETRACT)
+
+#include "../../../feature/fwretract.h"
+#include "../../gcode.h"
+
+/**
+ * M207: Set firmware retraction values
+ *
+ *   S[+units]    retract_length
+ *   W[+units]    swap_retract_length (multi-extruder)
+ *   F[units/min] retract_feedrate_mm_s
+ *   Z[units]     retract_zraise
+ */
+void GcodeSuite::M207() {
+  if (parser.seen('S')) fwretract.settings.retract_length = parser.value_axis_units(E_AXIS);
+  if (parser.seen('F')) fwretract.settings.retract_feedrate_mm_s = MMM_TO_MMS(parser.value_axis_units(E_AXIS));
+  if (parser.seen('Z')) fwretract.settings.retract_zraise = parser.value_linear_units();
+  if (parser.seen('W')) fwretract.settings.swap_retract_length = parser.value_axis_units(E_AXIS);
+}
+
+/**
+ * M208: Set firmware un-retraction values
+ *
+ *   S[+units]    retract_recover_extra (in addition to M207 S*)
+ *   W[+units]    swap_retract_recover_extra (multi-extruder)
+ *   F[units/min] retract_recover_feedrate_mm_s
+ *   R[units/min] swap_retract_recover_feedrate_mm_s
+ */
+void GcodeSuite::M208() {
+  if (parser.seen('S')) fwretract.settings.retract_recover_extra = parser.value_axis_units(E_AXIS);
+  if (parser.seen('F')) fwretract.settings.retract_recover_feedrate_mm_s = MMM_TO_MMS(parser.value_axis_units(E_AXIS));
+  if (parser.seen('R')) fwretract.settings.swap_retract_recover_feedrate_mm_s = MMM_TO_MMS(parser.value_axis_units(E_AXIS));
+  if (parser.seen('W')) fwretract.settings.swap_retract_recover_extra = parser.value_axis_units(E_AXIS);
+}
+
+#if ENABLED(FWRETRACT_AUTORETRACT)
+
+  /**
+   * M209: Enable automatic retract (M209 S1)
+   *   For slicers that don't support G10/11, reversed extrude-only
+   *   moves will be classified as retraction.
+   */
+  void GcodeSuite::M209() {
+    if (MIN_AUTORETRACT <= MAX_AUTORETRACT && parser.seen('S'))
+      fwretract.enable_autoretract(parser.value_bool());
+  }
+
+#endif // FWRETRACT_AUTORETRACT
+
+#endif // FWRETRACT
diff --git a/Marlin/src/gcode/feature/i2c/M260_M261.cpp b/Marlin/src/gcode/feature/i2c/M260_M261.cpp
new file mode 100644
index 0000000..438d152
--- /dev/null
+++ b/Marlin/src/gcode/feature/i2c/M260_M261.cpp
@@ -0,0 +1,76 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(EXPERIMENTAL_I2CBUS)
+
+#include "../../gcode.h"
+
+#include "../../../feature/twibus.h"
+
+/**
+ * M260: Send data to a I2C slave device
+ *
+ * This is a PoC, the formatting and arguments for the GCODE will
+ * change to be more compatible, the current proposal is:
+ *
+ *  M260 A<slave device address base 10> ; Sets the I2C slave address the data will be sent to
+ *
+ *  M260 B<byte-1 value in base 10>
+ *  M260 B<byte-2 value in base 10>
+ *  M260 B<byte-3 value in base 10>
+ *
+ *  M260 S1 ; Send the buffered data and reset the buffer
+ *  M260 R1 ; Reset the buffer without sending data
+ */
+void GcodeSuite::M260() {
+  // Set the target address
+  if (parser.seen('A')) i2c.address(parser.value_byte());
+
+  // Add a new byte to the buffer
+  if (parser.seen('B')) i2c.addbyte(parser.value_byte());
+
+  // Flush the buffer to the bus
+  if (parser.seen('S')) i2c.send();
+
+  // Reset and rewind the buffer
+  else if (parser.seen('R')) i2c.reset();
+}
+
+/**
+ * M261: Request X bytes from I2C slave device
+ *
+ * Usage: M261 A<slave device address base 10> B<number of bytes>
+ */
+void GcodeSuite::M261() {
+  if (parser.seen('A')) i2c.address(parser.value_byte());
+
+  uint8_t bytes = parser.byteval('B', 1);
+
+  if (i2c.addr && bytes && bytes <= TWIBUS_BUFFER_SIZE)
+    i2c.relay(bytes);
+  else
+    SERIAL_ERROR_MSG("Bad i2c request");
+}
+
+#endif
diff --git a/Marlin/src/gcode/feature/leds/M150.cpp b/Marlin/src/gcode/feature/leds/M150.cpp
new file mode 100644
index 0000000..cf09bf1
--- /dev/null
+++ b/Marlin/src/gcode/feature/leds/M150.cpp
@@ -0,0 +1,84 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if HAS_COLOR_LEDS
+
+#include "../../gcode.h"
+#include "../../../feature/leds/leds.h"
+
+/**
+ * M150: Set Status LED Color - Use R-U-B-W for R-G-B-W
+ *       and Brightness       - Use P (for NEOPIXEL only)
+ *
+ * Always sets all 3 or 4 components. If a component is left out, set to 0.
+ *                                    If brightness is left out, no value changed
+ *
+ * With NEOPIXEL_LED:
+ *  I<index>  Set the NeoPixel index to affect. Default: All
+ *
+ * With NEOPIXEL2_SEPARATE:
+ *  S<index>  The NeoPixel strip to set. Default is index 0.
+ *
+ * Examples:
+ *
+ *   M150 R255       ; Turn LED red
+ *   M150 R255 U127  ; Turn LED orange (PWM only)
+ *   M150            ; Turn LED off
+ *   M150 R U B      ; Turn LED white
+ *   M150 W          ; Turn LED white using a white LED
+ *   M150 P127       ; Set LED 50% brightness
+ *   M150 P          ; Set LED full brightness
+ *   M150 I1 R       ; Set NEOPIXEL index 1 to red
+ *   M150 S1 I1 R    ; Set SEPARATE index 1 to red
+ */
+
+void GcodeSuite::M150() {
+  #if ENABLED(NEOPIXEL_LED)
+    const uint8_t index = parser.intval('I', -1);
+    #if ENABLED(NEOPIXEL2_SEPARATE)
+      const uint8_t unit = parser.intval('S'),
+                    brightness = unit ? neo2.brightness() : neo.brightness();
+      *(unit ? &neo2.neoindex : &neo.neoindex) = index;
+    #else
+      const uint8_t brightness = neo.brightness();
+      neo.neoindex = index;
+    #endif
+  #endif
+
+  const LEDColor color = MakeLEDColor(
+    parser.seen('R') ? (parser.has_value() ? parser.value_byte() : 255) : 0,
+    parser.seen('U') ? (parser.has_value() ? parser.value_byte() : 255) : 0,
+    parser.seen('B') ? (parser.has_value() ? parser.value_byte() : 255) : 0,
+    parser.seen('W') ? (parser.has_value() ? parser.value_byte() : 255) : 0,
+    parser.seen('P') ? (parser.has_value() ? parser.value_byte() : 255) : brightness
+  );
+
+  #if ENABLED(NEOPIXEL2_SEPARATE)
+    if (unit == 1) { leds2.set_color(color); return; }
+  #endif
+
+  leds.set_color(color);
+}
+
+#endif // HAS_COLOR_LEDS
diff --git a/Marlin/src/gcode/feature/leds/M7219.cpp b/Marlin/src/gcode/feature/leds/M7219.cpp
new file mode 100644
index 0000000..a6ee711
--- /dev/null
+++ b/Marlin/src/gcode/feature/leds/M7219.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 <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfigPre.h"
+
+#if ENABLED(MAX7219_GCODE)
+
+#include "../../gcode.h"
+#include "../../../feature/max7219.h"
+
+/**
+ * M7219: Control the Max7219 LED matrix
+ *
+ *  I         - Initialize (clear) the matrix
+ *  F         - Fill the matrix (set all bits)
+ *  P         - Dump the led_line[] array values
+ *  C<column> - Set a column to the bitmask given by 'V' (Units 0-3 in portrait layout)
+ *  R<row>    - Set a row to the bitmask given by 'V' (Units 0-3 in landscape layout)
+ *  X<pos>    - X index of an LED to set or toggle
+ *  Y<pos>    - Y index of an LED to set or toggle
+ *  V<value>  - LED on/off state or row/column bitmask (8, 16, 24, or 32-bits)
+ *              ('C' / 'R' can be used to update up to 4 units at once)
+ *
+ * Directly set a native matrix row to the 8-bit value 'V':
+ *  D<line>   - Display line (0..7)
+ *  U<unit>   - Unit index (0..MAX7219_NUMBER_UNITS-1)
+ */
+void GcodeSuite::M7219() {
+  if (parser.seen('I')) {
+    max7219.register_setup();
+    max7219.clear();
+  }
+
+  if (parser.seen('F')) max7219.fill();
+
+  const uint32_t v = parser.ulongval('V');
+
+  if (parser.seenval('R')) {
+    const uint8_t r = parser.value_byte();
+    max7219.set_row(r, v);
+  }
+  else if (parser.seenval('C')) {
+    const uint8_t c = parser.value_byte();
+    max7219.set_column(c, v);
+  }
+  else if (parser.seenval('X') || parser.seenval('Y')) {
+    const uint8_t x = parser.byteval('X'), y = parser.byteval('Y');
+    if (parser.seenval('V'))
+      max7219.led_set(x, y, v > 0);
+    else
+      max7219.led_toggle(x, y);
+  }
+  else if (parser.seen('D')) {
+    const uint8_t uline = parser.value_byte() & 0x7,
+                  line = uline + (parser.byteval('U') << 3);
+    if (line < MAX7219_LINES) {
+      max7219.led_line[line] = v;
+      return max7219.refresh_line(line);
+    }
+  }
+
+  if (parser.seen('P')) {
+    LOOP_L_N(r, MAX7219_LINES) {
+      SERIAL_ECHOPGM("led_line[");
+      if (r < 10) SERIAL_CHAR(' ');
+      SERIAL_ECHO(int(r));
+      SERIAL_ECHOPGM("]=");
+      for (uint8_t b = 8; b--;) SERIAL_CHAR('0' + TEST(max7219.led_line[r], b));
+      SERIAL_EOL();
+    }
+  }
+}
+
+#endif // MAX7219_GCODE
diff --git a/Marlin/src/gcode/feature/macro/M810-M819.cpp b/Marlin/src/gcode/feature/macro/M810-M819.cpp
new file mode 100644
index 0000000..7b9e1a1
--- /dev/null
+++ b/Marlin/src/gcode/feature/macro/M810-M819.cpp
@@ -0,0 +1,65 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(GCODE_MACROS)
+
+#include "../../gcode.h"
+#include "../../queue.h"
+#include "../../parser.h"
+
+char gcode_macros[GCODE_MACROS_SLOTS][GCODE_MACROS_SLOT_SIZE + 1] = {{ 0 }};
+
+/**
+ * M810_819: Set/execute a G-code macro.
+ *
+ * Usage:
+ *   M810 <command>|...   Set Macro 0 to the given commands, separated by the pipe character
+ *   M810                 Execute Macro 0
+ */
+void GcodeSuite::M810_819() {
+  const uint8_t index = parser.codenum - 810;
+  if (index >= GCODE_MACROS_SLOTS) return;
+
+  const size_t len = strlen(parser.string_arg);
+
+  if (len) {
+    // Set a macro
+    if (len > GCODE_MACROS_SLOT_SIZE)
+      SERIAL_ERROR_MSG("Macro too long.");
+    else {
+      char c, *s = parser.string_arg, *d = gcode_macros[index];
+      do {
+        c = *s++;
+        *d++ = c == '|' ? '\n' : c;
+      } while (c);
+    }
+  }
+  else {
+    // Execute a macro
+    char * const cmd = gcode_macros[index];
+    if (strlen(cmd)) process_subcommands_now(cmd);
+  }
+}
+
+#endif // GCODE_MACROS
diff --git a/Marlin/src/gcode/feature/mixing/M163-M165.cpp b/Marlin/src/gcode/feature/mixing/M163-M165.cpp
new file mode 100644
index 0000000..a4cb64e
--- /dev/null
+++ b/Marlin/src/gcode/feature/mixing/M163-M165.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 <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(MIXING_EXTRUDER)
+
+#include "../../gcode.h"
+#include "../../../feature/mixing.h"
+
+/**
+ * M163: Set a single mix factor for a mixing extruder
+ *       This is called "weight" by some systems.
+ *       Must be followed by M164 to normalize and commit them.
+ *
+ *   S[index]   The channel index to set
+ *   P[float]   The mix value
+ */
+void GcodeSuite::M163() {
+  const int mix_index = parser.intval('S');
+  if (mix_index < MIXING_STEPPERS)
+    mixer.set_collector(mix_index, parser.floatval('P'));
+}
+
+/**
+ * M164: Normalize and commit the mix.
+ *
+ *   S[index]   The virtual tool to store
+ *              If 'S' is omitted update the active virtual tool.
+ */
+void GcodeSuite::M164() {
+  #if MIXING_VIRTUAL_TOOLS > 1
+    const int tool_index = parser.intval('S', -1);
+  #else
+    constexpr int tool_index = 0;
+  #endif
+  if (tool_index >= 0) {
+    if (tool_index < MIXING_VIRTUAL_TOOLS)
+      mixer.normalize(tool_index);
+  }
+  else
+    mixer.normalize();
+}
+
+#if ENABLED(DIRECT_MIXING_IN_G1)
+
+  /**
+   * M165: Set multiple mix factors for a mixing extruder.
+   *       Omitted factors will be set to 0.
+   *       The mix is normalized and stored in the current virtual tool.
+   *
+   *   A[factor] Mix factor for extruder stepper 1
+   *   B[factor] Mix factor for extruder stepper 2
+   *   C[factor] Mix factor for extruder stepper 3
+   *   D[factor] Mix factor for extruder stepper 4
+   *   H[factor] Mix factor for extruder stepper 5
+   *   I[factor] Mix factor for extruder stepper 6
+   */
+  void GcodeSuite::M165() {
+    // Get mixing parameters from the GCode
+    // The total "must" be 1.0 (but it will be normalized)
+    // If no mix factors are given, the old mix is preserved
+    const char mixing_codes[] = { LIST_N(MIXING_STEPPERS, 'A', 'B', 'C', 'D', 'H', 'I') };
+    uint8_t mix_bits = 0;
+    MIXER_STEPPER_LOOP(i) {
+      if (parser.seenval(mixing_codes[i])) {
+        SBI(mix_bits, i);
+        mixer.set_collector(i, parser.value_float());
+      }
+    }
+    // If any mixing factors were included, clear the rest
+    // If none were included, preserve the last mix
+    if (mix_bits) {
+      MIXER_STEPPER_LOOP(i)
+        if (!TEST(mix_bits, i)) mixer.set_collector(i, 0.0f);
+      mixer.normalize();
+    }
+  }
+
+#endif // DIRECT_MIXING_IN_G1
+
+#endif // MIXING_EXTRUDER
diff --git a/Marlin/src/gcode/feature/mixing/M166.cpp b/Marlin/src/gcode/feature/mixing/M166.cpp
new file mode 100644
index 0000000..9e071a4
--- /dev/null
+++ b/Marlin/src/gcode/feature/mixing/M166.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 <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(GRADIENT_MIX)
+
+#include "../../gcode.h"
+#include "../../../module/motion.h"
+#include "../../../module/planner.h"
+#include "../../../feature/mixing.h"
+
+inline void echo_mix() {
+  SERIAL_ECHOPAIR(" (", int(mixer.mix[0]), "%|", int(mixer.mix[1]), "%)");
+}
+
+inline void echo_zt(const int t, const float &z) {
+  mixer.update_mix_from_vtool(t);
+  SERIAL_ECHOPAIR_P(SP_Z_STR, z, SP_T_STR, t);
+  echo_mix();
+}
+
+/**
+ * M166: Set a simple gradient mix for a two-component mixer
+ *       based on the Geeetech A10M implementation by Jone Liu.
+ *
+ *   S[bool]  - Enable / disable gradients
+ *   A[float] - Starting Z for the gradient
+ *   Z[float] - Ending Z for the gradient. (Must be greater than the starting Z.)
+ *   I[index] - V-Tool to use as the starting mix.
+ *   J[index] - V-Tool to use as the ending mix.
+ *
+ *   T[index] - A V-Tool index to use as an alias for the Gradient (Requires GRADIENT_VTOOL)
+ *              T with no index clears the setting. Note: This can match the I or J value.
+ *
+ * Example: M166 S1 A0 Z20 I0 J1
+ */
+void GcodeSuite::M166() {
+  if (parser.seenval('A')) mixer.gradient.start_z = parser.value_float();
+  if (parser.seenval('Z')) mixer.gradient.end_z = parser.value_float();
+  if (parser.seenval('I')) mixer.gradient.start_vtool = (uint8_t)constrain(parser.value_int(), 0, MIXING_VIRTUAL_TOOLS);
+  if (parser.seenval('J')) mixer.gradient.end_vtool = (uint8_t)constrain(parser.value_int(), 0, MIXING_VIRTUAL_TOOLS);
+
+  #if ENABLED(GRADIENT_VTOOL)
+    if (parser.seen('T')) mixer.gradient.vtool_index = parser.byteval('T', -1);
+  #endif
+
+  if (parser.seen('S')) mixer.gradient.enabled = parser.value_bool();
+
+  mixer.refresh_gradient();
+
+  SERIAL_ECHOPGM("Gradient Mix ");
+  serialprint_onoff(mixer.gradient.enabled);
+  if (mixer.gradient.enabled) {
+
+    #if ENABLED(GRADIENT_VTOOL)
+      if (mixer.gradient.vtool_index >= 0) {
+        SERIAL_ECHOPAIR(" (T", int(mixer.gradient.vtool_index));
+        SERIAL_CHAR(')');
+      }
+    #endif
+
+    SERIAL_ECHOPGM(" ; Start");
+    echo_zt(mixer.gradient.start_vtool, mixer.gradient.start_z);
+
+    SERIAL_ECHOPGM(" ; End");
+    echo_zt(mixer.gradient.end_vtool, mixer.gradient.end_z);
+
+    mixer.update_mix_from_gradient();
+
+    SERIAL_ECHOPGM(" ; Current Z");
+    #if ENABLED(DELTA)
+      get_cartesian_from_steppers();
+      SERIAL_ECHO(cartes.z);
+    #else
+      SERIAL_ECHO(planner.get_axis_position_mm(Z_AXIS));
+    #endif
+    echo_mix();
+  }
+
+  SERIAL_EOL();
+}
+
+#endif // GRADIENT_MIX
diff --git a/Marlin/src/gcode/feature/network/M552-M554.cpp b/Marlin/src/gcode/feature/network/M552-M554.cpp
new file mode 100644
index 0000000..6ea15fe
--- /dev/null
+++ b/Marlin/src/gcode/feature/network/M552-M554.cpp
@@ -0,0 +1,126 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfigPre.h"
+
+#if HAS_ETHERNET
+
+#include "../../../feature/ethernet.h"
+#include "../../../core/serial.h"
+#include "../../gcode.h"
+
+void say_ethernet() { SERIAL_ECHOPGM("  Ethernet "); }
+
+void ETH0_report() {
+  say_ethernet();
+  SERIAL_ECHO_TERNARY(ethernet.hardware_enabled, "port ", "en", "dis", "abled.\n");
+  if (ethernet.hardware_enabled) {
+    say_ethernet();
+    SERIAL_ECHO_TERNARY(ethernet.have_telnet_client, "client ", "en", "dis", "abled.\n");
+  }
+  else
+    SERIAL_ECHOLNPGM("Send 'M552 S1' to enable.");
+}
+
+void MAC_report() {
+  uint8_t mac[6];
+  if (ethernet.hardware_enabled) {
+    Ethernet.MACAddress(mac);
+    SERIAL_ECHOPGM("  MAC: ");
+    LOOP_L_N(i, 6) {
+      if (mac[i] < 16) SERIAL_CHAR('0');
+      SERIAL_PRINT(mac[i], HEX);
+      if (i < 5) SERIAL_CHAR(':');
+    }
+  }
+  SERIAL_EOL();
+}
+
+// Display current values when the link is active,
+// otherwise show the stored values
+void ip_report(const uint16_t cmd, PGM_P const post, const IPAddress &ipo) {
+  SERIAL_CHAR('M'); SERIAL_ECHO(cmd); SERIAL_CHAR(' ');
+  LOOP_L_N(i, 4) {
+    SERIAL_ECHO(ipo[i]);
+    if (i < 3) SERIAL_CHAR('.');
+  }
+  SERIAL_ECHOPGM(" ; ");
+  SERIAL_ECHOPGM_P(post);
+  SERIAL_EOL();
+}
+void M552_report() {
+  ip_report(552, PSTR("ip address"), Ethernet.linkStatus() == LinkON ? Ethernet.localIP() : ethernet.ip);
+}
+void M553_report() {
+  ip_report(553, PSTR("subnet mask"), Ethernet.linkStatus() == LinkON ? Ethernet.subnetMask() : ethernet.subnet);
+}
+void M554_report() {
+  ip_report(554, PSTR("gateway"), Ethernet.linkStatus() == LinkON ? Ethernet.gatewayIP() : ethernet.gateway);
+}
+
+/**
+ * M552: Set IP address, enable/disable network interface
+ *
+ *  S0   : disable networking
+ *  S1   : enable networking
+ *  S-1  : reset network interface
+ *
+ *  Pnnn : Set IP address, 0.0.0.0 means acquire an IP address using DHCP
+ */
+void GcodeSuite::M552() {
+  const bool seenP = parser.seenval('P');
+  if (seenP) ethernet.ip.fromString(parser.value_string());
+
+  const bool seenS = parser.seenval('S');
+  if (seenS) {
+    switch (parser.value_int()) {
+      case -1:
+        if (ethernet.telnetClient) ethernet.telnetClient.stop();
+        ethernet.init();
+        break;
+      case 0: ethernet.hardware_enabled = false; break;
+      case 1: ethernet.hardware_enabled = true; break;
+      default: break;
+    }
+  }
+  const bool nopar = !seenS && !seenP;
+  if (nopar || seenS) ETH0_report();
+  if (nopar || seenP) M552_report();
+}
+
+/**
+ * M553 Pnnn - Set netmask
+ */
+void GcodeSuite::M553() {
+  if (parser.seenval('P')) ethernet.subnet.fromString(parser.value_string());
+  M553_report();
+}
+
+/**
+ * M554 Pnnn - Set Gateway
+ */
+void GcodeSuite::M554() {
+  if (parser.seenval('P')) ethernet.gateway.fromString(parser.value_string());
+  M554_report();
+}
+
+#endif // HAS_ETHERNET
diff --git a/Marlin/src/gcode/feature/password/M510-M512.cpp b/Marlin/src/gcode/feature/password/M510-M512.cpp
new file mode 100644
index 0000000..eeb9b1d
--- /dev/null
+++ b/Marlin/src/gcode/feature/password/M510-M512.cpp
@@ -0,0 +1,83 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfigPre.h"
+
+#if ENABLED(PASSWORD_FEATURE)
+
+#include "../../../feature/password/password.h"
+#include "../../../core/serial.h"
+#include "../../gcode.h"
+
+//
+// M510: Lock Printer
+//
+void GcodeSuite::M510() {
+  password.lock_machine();
+}
+
+//
+// M511: Unlock Printer
+//
+#if ENABLED(PASSWORD_UNLOCK_GCODE)
+
+  void GcodeSuite::M511() {
+    if (password.is_locked) {
+      password.value_entry = parser.ulongval('P');
+      password.authentication_check();
+    }
+  }
+
+#endif // PASSWORD_UNLOCK_GCODE
+
+//
+// M512: Set/Change/Remove Password
+//
+#if ENABLED(PASSWORD_CHANGE_GCODE)
+
+  void GcodeSuite::M512() {
+    if (password.is_set && parser.ulongval('P') != password.value) {
+      SERIAL_ECHOLNPGM(STR_WRONG_PASSWORD);
+      return;
+    }
+
+    if (parser.seenval('S')) {
+      password.value_entry = parser.ulongval('S');
+
+      if (password.value_entry < CAT(1e, PASSWORD_LENGTH)) {
+        password.is_set = true;
+        password.value = password.value_entry;
+        SERIAL_ECHOLNPAIR(STR_PASSWORD_SET, password.value); // TODO: Update password.string
+      }
+      else
+        SERIAL_ECHOLNPGM(STR_PASSWORD_TOO_LONG);
+    }
+    else {
+      password.is_set = false;
+      SERIAL_ECHOLNPGM(STR_PASSWORD_REMOVED);
+    }
+    SERIAL_ECHOLNPGM(STR_REMINDER_SAVE_SETTINGS);
+  }
+
+#endif // PASSWORD_CHANGE_GCODE
+
+#endif // PASSWORD_FEATURE
diff --git a/Marlin/src/gcode/feature/pause/G27.cpp b/Marlin/src/gcode/feature/pause/G27.cpp
new file mode 100644
index 0000000..3ce618d
--- /dev/null
+++ b/Marlin/src/gcode/feature/pause/G27.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 <https://www.gnu.org/licenses/>.
+ *
+ */
+
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(NOZZLE_PARK_FEATURE)
+
+#include "../../gcode.h"
+#include "../../../libs/nozzle.h"
+#include "../../../module/motion.h"
+
+/**
+ * G27: Park the nozzle
+ */
+void GcodeSuite::G27() {
+  // Don't allow nozzle parking without homing first
+  if (homing_needed_error()) return;
+  nozzle.park(parser.ushortval('P'));
+}
+
+#endif // NOZZLE_PARK_FEATURE
diff --git a/Marlin/src/gcode/feature/pause/G60.cpp b/Marlin/src/gcode/feature/pause/G60.cpp
new file mode 100644
index 0000000..6f695b9
--- /dev/null
+++ b/Marlin/src/gcode/feature/pause/G60.cpp
@@ -0,0 +1,58 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if SAVED_POSITIONS
+
+#include "../../gcode.h"
+#include "../../../module/motion.h"
+
+#define DEBUG_OUT ENABLED(SAVED_POSITIONS_DEBUG)
+#include "../../../core/debug_out.h"
+
+/**
+ * G60: Save current position
+ *
+ *   S<slot> - Memory slot # (0-based) to save into (default 0)
+ */
+void GcodeSuite::G60() {
+  const uint8_t slot = parser.byteval('S');
+
+  if (slot >= SAVED_POSITIONS) {
+    SERIAL_ERROR_MSG(STR_INVALID_POS_SLOT STRINGIFY(SAVED_POSITIONS));
+    return;
+  }
+
+  stored_position[slot] = current_position;
+  SBI(saved_slots[slot >> 3], slot & 0x07);
+
+  #if ENABLED(SAVED_POSITIONS_DEBUG)
+    const xyze_pos_t &pos = stored_position[slot];
+    DEBUG_ECHOPAIR_F(STR_SAVED_POS " S", slot);
+    DEBUG_ECHOPAIR_F(" : X", pos.x);
+    DEBUG_ECHOPAIR_F_P(SP_Y_STR, pos.y);
+    DEBUG_ECHOLNPAIR_F_P(SP_Z_STR, pos.z);
+  #endif
+}
+
+#endif // SAVED_POSITIONS
diff --git a/Marlin/src/gcode/feature/pause/G61.cpp b/Marlin/src/gcode/feature/pause/G61.cpp
new file mode 100644
index 0000000..5d89af0
--- /dev/null
+++ b/Marlin/src/gcode/feature/pause/G61.cpp
@@ -0,0 +1,73 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if SAVED_POSITIONS
+
+#include "../../../module/planner.h"
+#include "../../gcode.h"
+#include "../../../module/motion.h"
+
+/**
+ * G61: Return to saved position
+ *
+ *   F<rate>  - Feedrate (optional) for the move back.
+ *   S<slot>  - Slot # (0-based) to restore from (default 0).
+ *   X Y Z    - Axes to restore. At least one is required.
+ */
+void GcodeSuite::G61(void) {
+
+  const uint8_t slot = parser.byteval('S');
+
+  #if SAVED_POSITIONS < 256
+    if (slot >= SAVED_POSITIONS) {
+      SERIAL_ERROR_MSG(STR_INVALID_POS_SLOT STRINGIFY(SAVED_POSITIONS));
+      return;
+    }
+  #endif
+
+  // No saved position? No axes being restored?
+  if (!TEST(saved_slots[slot >> 3], slot & 0x07) || !parser.seen("XYZ")) return;
+
+  SERIAL_ECHOPAIR(STR_RESTORING_POS " S", int(slot));
+  LOOP_XYZ(i) {
+    destination[i] = parser.seen(XYZ_CHAR(i))
+      ? stored_position[slot][i] + parser.value_axis_units((AxisEnum)i)
+      : current_position[i];
+    SERIAL_CHAR(' ', XYZ_CHAR(i));
+    SERIAL_ECHO_F(destination[i]);
+  }
+  SERIAL_EOL();
+
+  // Apply any given feedrate over 0.0
+  feedRate_t saved_feedrate = feedrate_mm_s;
+  const float fr = parser.linearval('F');
+  if (fr > 0.0) feedrate_mm_s = MMM_TO_MMS(fr);
+
+  // Move to the saved position
+  prepare_line_to_destination();
+
+  feedrate_mm_s = saved_feedrate;
+}
+
+#endif // SAVED_POSITIONS
diff --git a/Marlin/src/gcode/feature/pause/M125.cpp b/Marlin/src/gcode/feature/pause/M125.cpp
new file mode 100644
index 0000000..9391b86
--- /dev/null
+++ b/Marlin/src/gcode/feature/pause/M125.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 <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(PARK_HEAD_ON_PAUSE)
+
+#include "../../gcode.h"
+#include "../../parser.h"
+#include "../../../feature/pause.h"
+#include "../../../lcd/marlinui.h"
+#include "../../../module/motion.h"
+#include "../../../module/printcounter.h"
+#include "../../../sd/cardreader.h"
+
+#if ENABLED(POWER_LOSS_RECOVERY)
+  #include "../../../feature/powerloss.h"
+#endif
+
+/**
+ * M125: Store current position and move to parking position.
+ *       Called on pause (by M25) to prevent material leaking onto the
+ *       object. On resume (M24) the head will be moved back and the
+ *       print will resume.
+ *
+ *       When not actively SD printing, M125 simply moves to the park
+ *       position and waits, resuming with a button click or M108.
+ *       Without PARK_HEAD_ON_PAUSE the M125 command does nothing.
+ *
+ *    L<linear> = Override retract Length
+ *    X<pos>    = Override park position X
+ *    Y<pos>    = Override park position Y
+ *    Z<linear> = Override Z raise
+ *
+ *  With an LCD menu:
+ *    P<bool>   = Always show a prompt and await a response
+ */
+void GcodeSuite::M125() {
+  // Initial retract before move to filament change position
+  const float retract = -ABS(parser.seen('L') ? parser.value_axis_units(E_AXIS) : (PAUSE_PARK_RETRACT_LENGTH));
+
+  xyz_pos_t park_point = NOZZLE_PARK_POINT;
+
+  // Move XY axes to filament change position or given position
+  if (parser.seenval('X')) park_point.x = RAW_X_POSITION(parser.linearval('X'));
+  if (parser.seenval('Y')) park_point.y = RAW_X_POSITION(parser.linearval('Y'));
+
+  // Lift Z axis
+  if (parser.seenval('Z')) park_point.z = parser.linearval('Z');
+
+  #if HAS_HOTEND_OFFSET && NONE(DUAL_X_CARRIAGE, DELTA)
+    park_point += hotend_offset[active_extruder];
+  #endif
+
+  const bool sd_printing = TERN0(SDSUPPORT, IS_SD_PRINTING());
+
+  ui.pause_show_message(PAUSE_MESSAGE_PARKING, PAUSE_MODE_PAUSE_PRINT);
+
+  // If possible, show an LCD prompt with the 'P' flag
+  const bool show_lcd = TERN0(HAS_LCD_MENU, parser.boolval('P'));
+
+  if (pause_print(retract, park_point, 0, show_lcd)) {
+    TERN_(POWER_LOSS_RECOVERY, if (recovery.enabled) recovery.save(true));
+    if (ENABLED(EXTENSIBLE_UI) || !sd_printing || show_lcd) {
+      wait_for_confirmation(false, 0);
+      resume_print(0, 0, -retract, 0);
+    }
+  }
+}
+
+#endif // PARK_HEAD_ON_PAUSE
diff --git a/Marlin/src/gcode/feature/pause/M600.cpp b/Marlin/src/gcode/feature/pause/M600.cpp
new file mode 100644
index 0000000..1c282f2
--- /dev/null
+++ b/Marlin/src/gcode/feature/pause/M600.cpp
@@ -0,0 +1,172 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(ADVANCED_PAUSE_FEATURE)
+
+#include "../../gcode.h"
+#include "../../../feature/pause.h"
+#include "../../../module/motion.h"
+#include "../../../module/printcounter.h"
+#include "../../../lcd/marlinui.h"
+
+#if HAS_MULTI_EXTRUDER
+  #include "../../../module/tool_change.h"
+#endif
+
+#if ENABLED(MMU2_MENUS)
+  #include "../../../lcd/menu/menu_mmu2.h"
+#endif
+
+#if ENABLED(MIXING_EXTRUDER)
+  #include "../../../feature/mixing.h"
+#endif
+
+#if HAS_FILAMENT_SENSOR
+  #include "../../../feature/runout.h"
+#endif
+
+/**
+ * M600: Pause for filament change
+ *
+ *  E[distance] - Retract the filament this far
+ *  Z[distance] - Move the Z axis by this distance
+ *  X[position] - Move to this X position, with Y
+ *  Y[position] - Move to this Y position, with X
+ *  U[distance] - Retract distance for removal (manual reload)
+ *  L[distance] - Extrude distance for insertion (manual reload)
+ *  B[count]    - Number of times to beep, -1 for indefinite (if equipped with a buzzer)
+ *  T[toolhead] - Select extruder for filament change
+ *  R[temp]     - Resume temperature (in current units)
+ *
+ *  Default values are used for omitted arguments.
+ */
+void GcodeSuite::M600() {
+
+  #if ENABLED(MIXING_EXTRUDER)
+    const int8_t target_e_stepper = get_target_e_stepper_from_command();
+    if (target_e_stepper < 0) return;
+
+    const uint8_t old_mixing_tool = mixer.get_current_vtool();
+    mixer.T(MIXER_DIRECT_SET_TOOL);
+
+    MIXER_STEPPER_LOOP(i) mixer.set_collector(i, i == uint8_t(target_e_stepper) ? 1.0 : 0.0);
+    mixer.normalize();
+
+    const int8_t target_extruder = active_extruder;
+  #else
+    const int8_t target_extruder = get_target_extruder_from_command();
+    if (target_extruder < 0) return;
+  #endif
+
+  #if ENABLED(DUAL_X_CARRIAGE)
+    int8_t DXC_ext = target_extruder;
+    if (!parser.seen('T')) {  // If no tool index is specified, M600 was (probably) sent in response to filament runout.
+                              // In this case, for duplicating modes set DXC_ext to the extruder that ran out.
+      #if HAS_FILAMENT_SENSOR && NUM_RUNOUT_SENSORS > 1
+        if (idex_is_duplicating())
+          DXC_ext = (READ(FIL_RUNOUT2_PIN) == FIL_RUNOUT2_STATE) ? 1 : 0;
+      #else
+        DXC_ext = active_extruder;
+      #endif
+    }
+  #endif
+
+  // Show initial "wait for start" message
+  #if DISABLED(MMU2_MENUS)
+    ui.pause_show_message(PAUSE_MESSAGE_CHANGING, PAUSE_MODE_PAUSE_PRINT, target_extruder);
+  #endif
+
+  #if ENABLED(HOME_BEFORE_FILAMENT_CHANGE)
+    // If needed, home before parking for filament change
+    if (!all_axes_trusted()) home_all_axes(true);
+  #endif
+
+  #if HAS_MULTI_EXTRUDER
+    // Change toolhead if specified
+    const uint8_t active_extruder_before_filament_change = active_extruder;
+    if (active_extruder != target_extruder && TERN1(DUAL_X_CARRIAGE, !idex_is_duplicating()))
+      tool_change(target_extruder, false);
+  #endif
+
+  // Initial retract before move to filament change position
+  const float retract = -ABS(parser.seen('E') ? parser.value_axis_units(E_AXIS) : (PAUSE_PARK_RETRACT_LENGTH));
+
+  xyz_pos_t park_point NOZZLE_PARK_POINT;
+
+  // Lift Z axis
+  if (parser.seenval('Z')) park_point.z = parser.linearval('Z');
+
+  // Move XY axes to filament change position or given position
+  if (parser.seenval('X')) park_point.x = parser.linearval('X');
+  if (parser.seenval('Y')) park_point.y = parser.linearval('Y');
+
+  #if HAS_HOTEND_OFFSET && NONE(DUAL_X_CARRIAGE, DELTA)
+    park_point += hotend_offset[active_extruder];
+  #endif
+
+  #if ENABLED(MMU2_MENUS)
+    // For MMU2 reset retract and load/unload values so they don't mess with MMU filament handling
+    constexpr float unload_length = 0.5f,
+                    slow_load_length = 0.0f,
+                    fast_load_length = 0.0f;
+  #else
+    // Unload filament
+    const float unload_length = -ABS(parser.seen('U') ? parser.value_axis_units(E_AXIS)
+                                                      : fc_settings[active_extruder].unload_length);
+
+    // Slow load filament
+    constexpr float slow_load_length = FILAMENT_CHANGE_SLOW_LOAD_LENGTH;
+
+    // Fast load filament
+    const float fast_load_length = ABS(parser.seen('L') ? parser.value_axis_units(E_AXIS)
+                                                        : fc_settings[active_extruder].load_length);
+  #endif
+
+  const int beep_count = parser.intval('B', -1
+    #ifdef FILAMENT_CHANGE_ALERT_BEEPS
+      + 1 + FILAMENT_CHANGE_ALERT_BEEPS
+    #endif
+  );
+
+  if (pause_print(retract, park_point, unload_length, true DXC_PASS)) {
+    #if ENABLED(MMU2_MENUS)
+      mmu2_M600();
+      resume_print(slow_load_length, fast_load_length, 0, beep_count DXC_PASS);
+    #else
+      wait_for_confirmation(true, beep_count DXC_PASS);
+      resume_print(slow_load_length, fast_load_length, ADVANCED_PAUSE_PURGE_LENGTH,
+                   beep_count, (parser.seenval('R') ? parser.value_celsius() : 0) DXC_PASS);
+    #endif
+  }
+
+  #if HAS_MULTI_EXTRUDER
+    // Restore toolhead if it was changed
+    if (active_extruder_before_filament_change != active_extruder)
+      tool_change(active_extruder_before_filament_change, false);
+  #endif
+
+  TERN_(MIXING_EXTRUDER, mixer.T(old_mixing_tool)); // Restore original mixing tool
+}
+
+#endif // ADVANCED_PAUSE_FEATURE
diff --git a/Marlin/src/gcode/feature/pause/M603.cpp b/Marlin/src/gcode/feature/pause/M603.cpp
new file mode 100644
index 0000000..9c3b774
--- /dev/null
+++ b/Marlin/src/gcode/feature/pause/M603.cpp
@@ -0,0 +1,65 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(ADVANCED_PAUSE_FEATURE)
+
+#include "../../gcode.h"
+#include "../../../feature/pause.h"
+#include "../../../module/motion.h"
+#include "../../../module/printcounter.h"
+
+#if HAS_MULTI_EXTRUDER
+  #include "../../../module/tool_change.h"
+#endif
+
+/**
+ * M603: Configure filament change
+ *
+ *  T[toolhead] - Select extruder to configure, active extruder if not specified
+ *  U[distance] - Retract distance for removal, for the specified extruder
+ *  L[distance] - Extrude distance for insertion, for the specified extruder
+ */
+void GcodeSuite::M603() {
+
+  const int8_t target_extruder = get_target_extruder_from_command();
+  if (target_extruder < 0) return;
+
+  // Unload length
+  if (parser.seen('U')) {
+    fc_settings[target_extruder].unload_length = ABS(parser.value_axis_units(E_AXIS));
+    #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
+      NOMORE(fc_settings[target_extruder].unload_length, EXTRUDE_MAXLENGTH);
+    #endif
+  }
+
+  // Load length
+  if (parser.seen('L')) {
+    fc_settings[target_extruder].load_length = ABS(parser.value_axis_units(E_AXIS));
+    #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
+      NOMORE(fc_settings[target_extruder].load_length, EXTRUDE_MAXLENGTH);
+    #endif
+  }
+}
+
+#endif // ADVANCED_PAUSE_FEATURE
diff --git a/Marlin/src/gcode/feature/pause/M701_M702.cpp b/Marlin/src/gcode/feature/pause/M701_M702.cpp
new file mode 100644
index 0000000..9a2b774
--- /dev/null
+++ b/Marlin/src/gcode/feature/pause/M701_M702.cpp
@@ -0,0 +1,235 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfigPre.h"
+
+#if ENABLED(FILAMENT_LOAD_UNLOAD_GCODES)
+
+#include "../../gcode.h"
+#include "../../../MarlinCore.h"
+#include "../../../module/motion.h"
+#include "../../../module/temperature.h"
+#include "../../../feature/pause.h"
+#include "../../../lcd/marlinui.h"
+
+#if HAS_MULTI_EXTRUDER
+  #include "../../../module/tool_change.h"
+#endif
+
+#if HAS_PRUSA_MMU2
+  #include "../../../feature/mmu/mmu2.h"
+#endif
+
+#if ENABLED(MIXING_EXTRUDER)
+  #include "../../../feature/mixing.h"
+#endif
+
+/**
+ * M701: Load filament
+ *
+ *  T<extruder> - Extruder number. Required for mixing extruder.
+ *                For non-mixing, current extruder if omitted.
+ *  Z<distance> - Move the Z axis by this distance
+ *  L<distance> - Extrude distance for insertion (positive value) (manual reload)
+ *
+ *  Default values are used for omitted arguments.
+ */
+void GcodeSuite::M701() {
+  xyz_pos_t park_point = NOZZLE_PARK_POINT;
+
+  // Don't raise Z if the machine isn't homed
+  if (TERN0(NO_MOTION_BEFORE_HOMING, axes_should_home())) park_point.z = 0;
+
+  #if ENABLED(MIXING_EXTRUDER)
+    const int8_t target_e_stepper = get_target_e_stepper_from_command();
+    if (target_e_stepper < 0) return;
+
+    const uint8_t old_mixing_tool = mixer.get_current_vtool();
+    mixer.T(MIXER_DIRECT_SET_TOOL);
+
+    MIXER_STEPPER_LOOP(i) mixer.set_collector(i, (i == (uint8_t)target_e_stepper) ? 1.0 : 0.0);
+    mixer.normalize();
+
+    const int8_t target_extruder = active_extruder;
+  #else
+    const int8_t target_extruder = get_target_extruder_from_command();
+    if (target_extruder < 0) return;
+  #endif
+
+  // Z axis lift
+  if (parser.seenval('Z')) park_point.z = parser.linearval('Z');
+
+  // Show initial "wait for load" message
+  ui.pause_show_message(PAUSE_MESSAGE_LOAD, PAUSE_MODE_LOAD_FILAMENT, target_extruder);
+
+  #if HAS_MULTI_EXTRUDER && (HAS_PRUSA_MMU1 || !HAS_MMU)
+    // Change toolhead if specified
+    uint8_t active_extruder_before_filament_change = active_extruder;
+    if (active_extruder != target_extruder)
+      tool_change(target_extruder, false);
+  #endif
+
+  // Lift Z axis
+  if (park_point.z > 0)
+    do_blocking_move_to_z(_MIN(current_position.z + park_point.z, Z_MAX_POS), feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
+
+  // Load filament
+  #if HAS_PRUSA_MMU2
+    mmu2.load_filament_to_nozzle(target_extruder);
+  #else
+    constexpr float     purge_length = ADVANCED_PAUSE_PURGE_LENGTH,
+                    slow_load_length = FILAMENT_CHANGE_SLOW_LOAD_LENGTH;
+        const float fast_load_length = ABS(parser.seen('L') ? parser.value_axis_units(E_AXIS)
+                                                            : fc_settings[active_extruder].load_length);
+    load_filament(
+      slow_load_length, fast_load_length, purge_length,
+      FILAMENT_CHANGE_ALERT_BEEPS,
+      true,                                           // show_lcd
+      thermalManager.still_heating(target_extruder),  // pause_for_user
+      PAUSE_MODE_LOAD_FILAMENT                        // pause_mode
+      #if ENABLED(DUAL_X_CARRIAGE)
+        , target_extruder                             // Dual X target
+      #endif
+    );
+  #endif
+
+  // Restore Z axis
+  if (park_point.z > 0)
+    do_blocking_move_to_z(_MAX(current_position.z - park_point.z, 0), feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
+
+  #if HAS_MULTI_EXTRUDER && (HAS_PRUSA_MMU1 || !HAS_MMU)
+    // Restore toolhead if it was changed
+    if (active_extruder_before_filament_change != active_extruder)
+      tool_change(active_extruder_before_filament_change, false);
+  #endif
+
+  TERN_(MIXING_EXTRUDER, mixer.T(old_mixing_tool)); // Restore original mixing tool
+
+  // Show status screen
+  ui.pause_show_message(PAUSE_MESSAGE_STATUS);
+}
+
+/**
+ * M702: Unload filament
+ *
+ *  T<extruder> - Extruder number. Required for mixing extruder.
+ *                For non-mixing, if omitted, current extruder
+ *                (or ALL extruders with FILAMENT_UNLOAD_ALL_EXTRUDERS).
+ *  Z<distance> - Move the Z axis by this distance
+ *  U<distance> - Retract distance for removal (manual reload)
+ *
+ *  Default values are used for omitted arguments.
+ */
+void GcodeSuite::M702() {
+  xyz_pos_t park_point = NOZZLE_PARK_POINT;
+
+  // Don't raise Z if the machine isn't homed
+  if (TERN0(NO_MOTION_BEFORE_HOMING, axes_should_home())) park_point.z = 0;
+
+  #if ENABLED(MIXING_EXTRUDER)
+    const uint8_t old_mixing_tool = mixer.get_current_vtool();
+
+    #if ENABLED(FILAMENT_UNLOAD_ALL_EXTRUDERS)
+      float mix_multiplier = 1.0;
+      const bool seenT = parser.seenval('T');
+      if (!seenT) {
+        mixer.T(MIXER_AUTORETRACT_TOOL);
+        mix_multiplier = MIXING_STEPPERS;
+      }
+    #else
+      constexpr bool seenT = true;
+    #endif
+
+    if (seenT) {
+      const int8_t target_e_stepper = get_target_e_stepper_from_command();
+      if (target_e_stepper < 0) return;
+      mixer.T(MIXER_DIRECT_SET_TOOL);
+      MIXER_STEPPER_LOOP(i) mixer.set_collector(i, (i == (uint8_t)target_e_stepper) ? 1.0 : 0.0);
+      mixer.normalize();
+    }
+
+    const int8_t target_extruder = active_extruder;
+  #else
+    const int8_t target_extruder = get_target_extruder_from_command();
+    if (target_extruder < 0) return;
+  #endif
+
+  // Z axis lift
+  if (parser.seenval('Z')) park_point.z = parser.linearval('Z');
+
+  // Show initial "wait for unload" message
+  ui.pause_show_message(PAUSE_MESSAGE_UNLOAD, PAUSE_MODE_UNLOAD_FILAMENT, target_extruder);
+
+  #if HAS_MULTI_EXTRUDER && (HAS_PRUSA_MMU1 || !HAS_MMU)
+    // Change toolhead if specified
+    uint8_t active_extruder_before_filament_change = active_extruder;
+    if (active_extruder != target_extruder)
+      tool_change(target_extruder, false);
+  #endif
+
+  // Lift Z axis
+  if (park_point.z > 0)
+    do_blocking_move_to_z(_MIN(current_position.z + park_point.z, Z_MAX_POS), feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
+
+  // Unload filament
+  #if HAS_PRUSA_MMU2
+    mmu2.unload();
+  #else
+    #if BOTH(HAS_MULTI_EXTRUDER, FILAMENT_UNLOAD_ALL_EXTRUDERS)
+      if (!parser.seenval('T')) {
+        HOTEND_LOOP() {
+          if (e != active_extruder) tool_change(e, false);
+          unload_filament(-fc_settings[e].unload_length, true, PAUSE_MODE_UNLOAD_FILAMENT);
+        }
+      }
+      else
+    #endif
+    {
+      // Unload length
+      const float unload_length = -ABS(parser.seen('U') ? parser.value_axis_units(E_AXIS)
+                                                        : fc_settings[target_extruder].unload_length);
+
+      unload_filament(unload_length, true, PAUSE_MODE_UNLOAD_FILAMENT
+        #if ALL(FILAMENT_UNLOAD_ALL_EXTRUDERS, MIXING_EXTRUDER)
+          , mix_multiplier
+        #endif
+      );
+    }
+  #endif
+
+  // Restore Z axis
+  if (park_point.z > 0)
+    do_blocking_move_to_z(_MAX(current_position.z - park_point.z, 0), feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
+
+  #if HAS_MULTI_EXTRUDER && (HAS_PRUSA_MMU1 || !HAS_MMU)
+    // Restore toolhead if it was changed
+    if (active_extruder_before_filament_change != active_extruder)
+      tool_change(active_extruder_before_filament_change, false);
+  #endif
+
+  TERN_(MIXING_EXTRUDER, mixer.T(old_mixing_tool)); // Restore original mixing tool
+
+  // Show status screen
+  ui.pause_show_message(PAUSE_MESSAGE_STATUS);
+}
+
+#endif // ADVANCED_PAUSE_FEATURE
diff --git a/Marlin/src/gcode/feature/power_monitor/M430.cpp b/Marlin/src/gcode/feature/power_monitor/M430.cpp
new file mode 100644
index 0000000..9559404
--- /dev/null
+++ b/Marlin/src/gcode/feature/power_monitor/M430.cpp
@@ -0,0 +1,70 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if HAS_POWER_MONITOR
+
+#include "../../../feature/power_monitor.h"
+#include "../../../MarlinCore.h"
+#include "../../gcode.h"
+
+/**
+ * M430: Enable/disable current LCD display
+ *       With no parameters report the system current draw (in Amps)
+ *
+ *  I[bool] - Set Display of current on the LCD
+ *  V[bool] - Set Display of voltage on the LCD
+ *  W[bool] - Set Display of power on the LCD
+ */
+void GcodeSuite::M430() {
+  bool do_report = true;
+  #if HAS_WIRED_LCD
+    #if ENABLED(POWER_MONITOR_CURRENT)
+      if (parser.seen('I')) { power_monitor.set_current_display(parser.value_bool()); do_report = false; }
+    #endif
+    #if HAS_POWER_MONITOR_VREF
+      if (parser.seen('V')) { power_monitor.set_voltage_display(parser.value_bool()); do_report = false; }
+    #endif
+    #if HAS_POWER_MONITOR_WATTS
+      if (parser.seen('W')) { power_monitor.set_power_display(parser.value_bool()); do_report = false; }
+    #endif
+  #endif
+  if (do_report) {
+    SERIAL_ECHOLNPAIR(
+      #if ENABLED(POWER_MONITOR_CURRENT)
+        "Current: ", power_monitor.getAmps(), "A"
+        #if HAS_POWER_MONITOR_VREF
+          "  "
+        #endif
+      #endif
+      #if HAS_POWER_MONITOR_VREF
+        "Voltage: ", power_monitor.getVolts(), "V"
+      #endif
+      #if HAS_POWER_MONITOR_WATTS
+        "  Power: ", power_monitor.getPower(), "W"
+      #endif
+    );
+  }
+}
+
+#endif // HAS_POWER_MONITOR
diff --git a/Marlin/src/gcode/feature/powerloss/M1000.cpp b/Marlin/src/gcode/feature/powerloss/M1000.cpp
new file mode 100644
index 0000000..14c9253
--- /dev/null
+++ b/Marlin/src/gcode/feature/powerloss/M1000.cpp
@@ -0,0 +1,89 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(POWER_LOSS_RECOVERY)
+
+#include "../../gcode.h"
+#include "../../../feature/powerloss.h"
+#include "../../../module/motion.h"
+#include "../../../lcd/marlinui.h"
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../../lcd/extui/ui_api.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_POWER_LOSS_RECOVERY)
+#include "../../../core/debug_out.h"
+
+void menu_job_recovery();
+
+inline void plr_error(PGM_P const prefix) {
+  #if ENABLED(DEBUG_POWER_LOSS_RECOVERY)
+    DEBUG_ECHO_START();
+    serialprintPGM(prefix);
+    DEBUG_ECHOLNPGM(" Job Recovery Data");
+  #else
+    UNUSED(prefix);
+  #endif
+}
+
+#if HAS_LCD_MENU
+  void lcd_power_loss_recovery_cancel();
+#endif
+
+/**
+ * M1000: Resume from power-loss (undocumented)
+ *   - With 'S' go to the Resume/Cancel menu
+ *   - With no parameters, run recovery commands
+ */
+void GcodeSuite::M1000() {
+
+  if (recovery.valid()) {
+    if (parser.seen('S')) {
+      #if HAS_LCD_MENU
+        ui.goto_screen(menu_job_recovery);
+      #elif ENABLED(DWIN_CREALITY_LCD)
+        recovery.dwin_flag = true;
+      #elif ENABLED(EXTENSIBLE_UI)
+        ExtUI::onPowerLossResume();
+      #else
+        SERIAL_ECHO_MSG("Resume requires LCD.");
+      #endif
+    }
+    else if (parser.seen('C')) {
+      #if HAS_LCD_MENU
+        lcd_power_loss_recovery_cancel();
+      #else
+        recovery.cancel();
+      #endif
+      TERN_(EXTENSIBLE_UI, ExtUI::onPrintTimerStopped());
+    }
+    else
+      recovery.resume();
+  }
+  else
+    plr_error(recovery.info.valid_head ? PSTR("No") : PSTR("Invalid"));
+
+}
+
+#endif // POWER_LOSS_RECOVERY
diff --git a/Marlin/src/gcode/feature/powerloss/M413.cpp b/Marlin/src/gcode/feature/powerloss/M413.cpp
new file mode 100644
index 0000000..64573e5
--- /dev/null
+++ b/Marlin/src/gcode/feature/powerloss/M413.cpp
@@ -0,0 +1,62 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(POWER_LOSS_RECOVERY)
+
+#include "../../gcode.h"
+#include "../../../feature/powerloss.h"
+#include "../../../module/motion.h"
+#include "../../../lcd/marlinui.h"
+
+/**
+ * M413: Enable / Disable power-loss recovery
+ *
+ * Parameters
+ *   S[bool] - Flag to enable / disable.
+ *             If omitted, report current state.
+ */
+void GcodeSuite::M413() {
+
+  if (parser.seen('S'))
+    recovery.enable(parser.value_bool());
+  else {
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM("Power-loss recovery ");
+    serialprintln_onoff(recovery.enabled);
+  }
+
+  #if ENABLED(DEBUG_POWER_LOSS_RECOVERY)
+    if (parser.seen("RL")) recovery.load();
+    if (parser.seen('W')) recovery.save(true);
+    if (parser.seen('P')) recovery.purge();
+    if (parser.seen('D')) recovery.debug(PSTR("M413"));
+    #if PIN_EXISTS(POWER_LOSS)
+      if (parser.seen('O')) recovery._outage();
+    #endif
+    if (parser.seen('E')) serialprintPGM(recovery.exists() ? PSTR("PLR Exists\n") : PSTR("No PLR\n"));
+    if (parser.seen('V')) serialprintPGM(recovery.valid() ? PSTR("Valid\n") : PSTR("Invalid\n"));
+  #endif
+}
+
+#endif // POWER_LOSS_RECOVERY
diff --git a/Marlin/src/gcode/feature/prusa_MMU2/M403.cpp b/Marlin/src/gcode/feature/prusa_MMU2/M403.cpp
new file mode 100644
index 0000000..31d0763
--- /dev/null
+++ b/Marlin/src/gcode/feature/prusa_MMU2/M403.cpp
@@ -0,0 +1,49 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfigPre.h"
+
+#if HAS_PRUSA_MMU2
+
+#include "../../gcode.h"
+#include "../../../feature/mmu/mmu2.h"
+
+/**
+ * M403: Set filament type for MMU2
+ *
+ * Valid filament type values:
+ *
+ *  0   Default
+ *  1   Flexible
+ *  2   PVA
+ */
+void GcodeSuite::M403() {
+  int8_t index = parser.intval('E', -1),
+         type = parser.intval('F', -1);
+
+  if (WITHIN(index, 0, 4) && WITHIN(type, 0, 2))
+    mmu2.set_filament_type(index, type);
+  else
+    SERIAL_ECHO_MSG("M403 - bad arguments.");
+}
+
+#endif // HAS_PRUSA_MMU2
diff --git a/Marlin/src/gcode/feature/runout/M412.cpp b/Marlin/src/gcode/feature/runout/M412.cpp
new file mode 100644
index 0000000..130f9c8
--- /dev/null
+++ b/Marlin/src/gcode/feature/runout/M412.cpp
@@ -0,0 +1,64 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if HAS_FILAMENT_SENSOR
+
+#include "../../gcode.h"
+#include "../../../feature/runout.h"
+
+/**
+ * M412: Enable / Disable filament runout detection
+ *
+ * Parameters
+ *  R         : Reset the runout sensor
+ *  S<bool>   : Reset and enable/disable the runout sensor
+ *  H<bool>   : Enable/disable host handling of filament runout
+ *  D<linear> : Extra distance to continue after runout is triggered
+ */
+void GcodeSuite::M412() {
+  if (parser.seen("RS"
+    TERN_(HAS_FILAMENT_RUNOUT_DISTANCE, "D")
+    TERN_(HOST_ACTION_COMMANDS, "H")
+  )) {
+    #if ENABLED(HOST_ACTION_COMMANDS)
+      if (parser.seen('H')) runout.host_handling = parser.value_bool();
+    #endif
+    const bool seenR = parser.seen('R'), seenS = parser.seen('S');
+    if (seenR || seenS) runout.reset();
+    if (seenS) runout.enabled = parser.value_bool();
+    #if HAS_FILAMENT_RUNOUT_DISTANCE
+      if (parser.seen('D')) runout.set_runout_distance(parser.value_linear_units());
+    #endif
+  }
+  else {
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM("Filament runout ");
+    serialprintln_onoff(runout.enabled);
+    #if HAS_FILAMENT_RUNOUT_DISTANCE
+      SERIAL_ECHOLNPAIR("Filament runout distance (mm): ", runout.runout_distance());
+    #endif
+  }
+}
+
+#endif // HAS_FILAMENT_SENSOR
diff --git a/Marlin/src/gcode/feature/trinamic/M122.cpp b/Marlin/src/gcode/feature/trinamic/M122.cpp
new file mode 100644
index 0000000..46e4365
--- /dev/null
+++ b/Marlin/src/gcode/feature/trinamic/M122.cpp
@@ -0,0 +1,60 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if HAS_TRINAMIC_CONFIG
+
+#include "../../gcode.h"
+#include "../../../feature/tmc_util.h"
+#include "../../../module/stepper/indirection.h"
+
+/**
+ * M122: Debug TMC drivers
+ */
+void GcodeSuite::M122() {
+  xyze_bool_t print_axis = { false, false, false, false };
+  bool print_all = true;
+  LOOP_XYZE(i) if (parser.seen(axis_codes[i])) { print_axis[i] = true; print_all = false; }
+
+  if (print_all) LOOP_XYZE(i) print_axis[i] = true;
+
+  if (parser.boolval('I')) restore_stepper_drivers();
+
+  #if ENABLED(TMC_DEBUG)
+    #if ENABLED(MONITOR_DRIVER_STATUS)
+      uint16_t interval = MONITOR_DRIVER_STATUS_INTERVAL_MS;
+      if (parser.seen('S') && !parser.value_bool()) interval = 0;
+      if (parser.seenval('P')) NOMORE(interval, parser.value_ushort());
+      tmc_set_report_interval(interval);
+    #endif
+
+    if (parser.seen('V'))
+      tmc_get_registers(print_axis.x, print_axis.y, print_axis.z, print_axis.e);
+    else
+      tmc_report_all(print_axis.x, print_axis.y, print_axis.z, print_axis.e);
+  #endif
+
+  test_tmc_connection(print_axis.x, print_axis.y, print_axis.z, print_axis.e);
+}
+
+#endif // HAS_TRINAMIC_CONFIG
diff --git a/Marlin/src/gcode/feature/trinamic/M569.cpp b/Marlin/src/gcode/feature/trinamic/M569.cpp
new file mode 100644
index 0000000..6b379f1
--- /dev/null
+++ b/Marlin/src/gcode/feature/trinamic/M569.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 <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if HAS_STEALTHCHOP
+
+#include "../../gcode.h"
+#include "../../../feature/tmc_util.h"
+#include "../../../module/stepper/indirection.h"
+
+template<typename TMC>
+void tmc_say_stealth_status(TMC &st) {
+  st.printLabel();
+  SERIAL_ECHOPGM(" driver mode:\t");
+  serialprintPGM(st.get_stealthChop() ? PSTR("stealthChop") : PSTR("spreadCycle"));
+  SERIAL_EOL();
+}
+template<typename TMC>
+void tmc_set_stealthChop(TMC &st, const bool enable) {
+  st.stored.stealthChop_enabled = enable;
+  st.refresh_stepping_mode();
+}
+
+static void set_stealth_status(const bool enable, const int8_t target_extruder) {
+  #define TMC_SET_STEALTH(Q) tmc_set_stealthChop(stepper##Q, enable)
+
+  #if    AXIS_HAS_STEALTHCHOP(X)  || AXIS_HAS_STEALTHCHOP(X2) \
+      || AXIS_HAS_STEALTHCHOP(Y)  || AXIS_HAS_STEALTHCHOP(Y2) \
+      || AXIS_HAS_STEALTHCHOP(Z)  || AXIS_HAS_STEALTHCHOP(Z2) \
+      || AXIS_HAS_STEALTHCHOP(Z3) || AXIS_HAS_STEALTHCHOP(Z4)
+    const uint8_t index = parser.byteval('I');
+  #endif
+
+  LOOP_XYZE(i) if (parser.seen(axis_codes[i])) {
+    switch (i) {
+      case X_AXIS:
+        #if AXIS_HAS_STEALTHCHOP(X)
+          if (index == 0) TMC_SET_STEALTH(X);
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(X2)
+          if (index == 1) TMC_SET_STEALTH(X2);
+        #endif
+        break;
+      case Y_AXIS:
+        #if AXIS_HAS_STEALTHCHOP(Y)
+          if (index == 0) TMC_SET_STEALTH(Y);
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(Y2)
+          if (index == 1) TMC_SET_STEALTH(Y2);
+        #endif
+        break;
+      case Z_AXIS:
+        #if AXIS_HAS_STEALTHCHOP(Z)
+          if (index == 0) TMC_SET_STEALTH(Z);
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(Z2)
+          if (index == 1) TMC_SET_STEALTH(Z2);
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(Z3)
+          if (index == 2) TMC_SET_STEALTH(Z3);
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(Z4)
+          if (index == 3) TMC_SET_STEALTH(Z4);
+        #endif
+        break;
+      case E_AXIS: {
+        if (target_extruder < 0) return;
+        switch (target_extruder) {
+          #if AXIS_HAS_STEALTHCHOP(E0)
+            case 0: TMC_SET_STEALTH(E0); break;
+          #endif
+          #if AXIS_HAS_STEALTHCHOP(E1)
+            case 1: TMC_SET_STEALTH(E1); break;
+          #endif
+          #if AXIS_HAS_STEALTHCHOP(E2)
+            case 2: TMC_SET_STEALTH(E2); break;
+          #endif
+          #if AXIS_HAS_STEALTHCHOP(E3)
+            case 3: TMC_SET_STEALTH(E3); break;
+          #endif
+          #if AXIS_HAS_STEALTHCHOP(E4)
+            case 4: TMC_SET_STEALTH(E4); break;
+          #endif
+          #if AXIS_HAS_STEALTHCHOP(E5)
+            case 5: TMC_SET_STEALTH(E5); break;
+          #endif
+          #if AXIS_HAS_STEALTHCHOP(E6)
+            case 6: TMC_SET_STEALTH(E6); break;
+          #endif
+          #if AXIS_HAS_STEALTHCHOP(E7)
+            case 7: TMC_SET_STEALTH(E7); break;
+          #endif
+        }
+      } break;
+    }
+  }
+}
+
+static void say_stealth_status() {
+  #define TMC_SAY_STEALTH_STATUS(Q) tmc_say_stealth_status(stepper##Q)
+
+  #if AXIS_HAS_STEALTHCHOP(X)
+    TMC_SAY_STEALTH_STATUS(X);
+  #endif
+  #if AXIS_HAS_STEALTHCHOP(X2)
+    TMC_SAY_STEALTH_STATUS(X2);
+  #endif
+  #if AXIS_HAS_STEALTHCHOP(Y)
+    TMC_SAY_STEALTH_STATUS(Y);
+  #endif
+  #if AXIS_HAS_STEALTHCHOP(Y2)
+    TMC_SAY_STEALTH_STATUS(Y2);
+  #endif
+  #if AXIS_HAS_STEALTHCHOP(Z)
+    TMC_SAY_STEALTH_STATUS(Z);
+  #endif
+  #if AXIS_HAS_STEALTHCHOP(Z2)
+    TMC_SAY_STEALTH_STATUS(Z2);
+  #endif
+  #if AXIS_HAS_STEALTHCHOP(Z3)
+    TMC_SAY_STEALTH_STATUS(Z3);
+  #endif
+  #if AXIS_HAS_STEALTHCHOP(Z4)
+    TMC_SAY_STEALTH_STATUS(Z4);
+  #endif
+  #if AXIS_HAS_STEALTHCHOP(E0)
+    TMC_SAY_STEALTH_STATUS(E0);
+  #endif
+  #if AXIS_HAS_STEALTHCHOP(E1)
+    TMC_SAY_STEALTH_STATUS(E1);
+  #endif
+  #if AXIS_HAS_STEALTHCHOP(E2)
+    TMC_SAY_STEALTH_STATUS(E2);
+  #endif
+  #if AXIS_HAS_STEALTHCHOP(E3)
+    TMC_SAY_STEALTH_STATUS(E3);
+  #endif
+  #if AXIS_HAS_STEALTHCHOP(E4)
+    TMC_SAY_STEALTH_STATUS(E4);
+  #endif
+  #if AXIS_HAS_STEALTHCHOP(E5)
+    TMC_SAY_STEALTH_STATUS(E5);
+  #endif
+  #if AXIS_HAS_STEALTHCHOP(E6)
+    TMC_SAY_STEALTH_STATUS(E6);
+  #endif
+  #if AXIS_HAS_STEALTHCHOP(E7)
+    TMC_SAY_STEALTH_STATUS(E7);
+  #endif
+}
+
+/**
+ * M569: Enable stealthChop on an axis
+ *
+ *   S[1|0] to enable or disable
+ *   XYZE to target an axis
+ *   No arguments reports the stealthChop status of all capable drivers.
+ */
+void GcodeSuite::M569() {
+  if (parser.seen('S'))
+    set_stealth_status(parser.value_bool(), get_target_extruder_from_command());
+  else
+    say_stealth_status();
+}
+
+#endif // HAS_STEALTHCHOP
diff --git a/Marlin/src/gcode/feature/trinamic/M906.cpp b/Marlin/src/gcode/feature/trinamic/M906.cpp
new file mode 100644
index 0000000..e834ebd
--- /dev/null
+++ b/Marlin/src/gcode/feature/trinamic/M906.cpp
@@ -0,0 +1,173 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if HAS_TRINAMIC_CONFIG
+
+#include "../../gcode.h"
+#include "../../../feature/tmc_util.h"
+#include "../../../module/stepper/indirection.h"
+
+/**
+ * M906: Set motor current in milliamps.
+ *
+ * Parameters:
+ *   X[current]  - Set mA current for X driver(s)
+ *   Y[current]  - Set mA current for Y driver(s)
+ *   Z[current]  - Set mA current for Z driver(s)
+ *   E[current]  - Set mA current for E driver(s)
+ *
+ *   I[index]    - Axis sub-index (Omit or 0 for X, Y, Z; 1 for X2, Y2, Z2; 2 for Z3; 3 for Z4.)
+ *   T[index]    - Extruder index (Zero-based. Omit for E0 only.)
+ *
+ * With no parameters report driver currents.
+ */
+void GcodeSuite::M906() {
+  #define TMC_SAY_CURRENT(Q) tmc_print_current(stepper##Q)
+  #define TMC_SET_CURRENT(Q) stepper##Q.rms_current(value)
+
+  bool report = true;
+
+  #if AXIS_IS_TMC(X) || AXIS_IS_TMC(X2) || AXIS_IS_TMC(Y) || AXIS_IS_TMC(Y2) || AXIS_IS_TMC(Z) || AXIS_IS_TMC(Z2) || AXIS_IS_TMC(Z3) || AXIS_IS_TMC(Z4)
+    const uint8_t index = parser.byteval('I');
+  #endif
+
+  LOOP_XYZE(i) if (uint16_t value = parser.intval(axis_codes[i])) {
+    report = false;
+    switch (i) {
+      case X_AXIS:
+        #if AXIS_IS_TMC(X)
+          if (index == 0) TMC_SET_CURRENT(X);
+        #endif
+        #if AXIS_IS_TMC(X2)
+          if (index == 1) TMC_SET_CURRENT(X2);
+        #endif
+        break;
+      case Y_AXIS:
+        #if AXIS_IS_TMC(Y)
+          if (index == 0) TMC_SET_CURRENT(Y);
+        #endif
+        #if AXIS_IS_TMC(Y2)
+          if (index == 1) TMC_SET_CURRENT(Y2);
+        #endif
+        break;
+      case Z_AXIS:
+        #if AXIS_IS_TMC(Z)
+          if (index == 0) TMC_SET_CURRENT(Z);
+        #endif
+        #if AXIS_IS_TMC(Z2)
+          if (index == 1) TMC_SET_CURRENT(Z2);
+        #endif
+        #if AXIS_IS_TMC(Z3)
+          if (index == 2) TMC_SET_CURRENT(Z3);
+        #endif
+        #if AXIS_IS_TMC(Z4)
+          if (index == 3) TMC_SET_CURRENT(Z4);
+        #endif
+        break;
+      case E_AXIS: {
+        const int8_t target_extruder = get_target_extruder_from_command();
+        if (target_extruder < 0) return;
+        switch (target_extruder) {
+          #if AXIS_IS_TMC(E0)
+            case 0: TMC_SET_CURRENT(E0); break;
+          #endif
+          #if AXIS_IS_TMC(E1)
+            case 1: TMC_SET_CURRENT(E1); break;
+          #endif
+          #if AXIS_IS_TMC(E2)
+            case 2: TMC_SET_CURRENT(E2); break;
+          #endif
+          #if AXIS_IS_TMC(E3)
+            case 3: TMC_SET_CURRENT(E3); break;
+          #endif
+          #if AXIS_IS_TMC(E4)
+            case 4: TMC_SET_CURRENT(E4); break;
+          #endif
+          #if AXIS_IS_TMC(E5)
+            case 5: TMC_SET_CURRENT(E5); break;
+          #endif
+          #if AXIS_IS_TMC(E6)
+            case 6: TMC_SET_CURRENT(E6); break;
+          #endif
+          #if AXIS_IS_TMC(E7)
+            case 7: TMC_SET_CURRENT(E7); break;
+          #endif
+        }
+      } break;
+    }
+  }
+
+  if (report) {
+    #if AXIS_IS_TMC(X)
+      TMC_SAY_CURRENT(X);
+    #endif
+    #if AXIS_IS_TMC(X2)
+      TMC_SAY_CURRENT(X2);
+    #endif
+    #if AXIS_IS_TMC(Y)
+      TMC_SAY_CURRENT(Y);
+    #endif
+    #if AXIS_IS_TMC(Y2)
+      TMC_SAY_CURRENT(Y2);
+    #endif
+    #if AXIS_IS_TMC(Z)
+      TMC_SAY_CURRENT(Z);
+    #endif
+    #if AXIS_IS_TMC(Z2)
+      TMC_SAY_CURRENT(Z2);
+    #endif
+    #if AXIS_IS_TMC(Z3)
+      TMC_SAY_CURRENT(Z3);
+    #endif
+    #if AXIS_IS_TMC(Z4)
+      TMC_SAY_CURRENT(Z4);
+    #endif
+    #if AXIS_IS_TMC(E0)
+      TMC_SAY_CURRENT(E0);
+    #endif
+    #if AXIS_IS_TMC(E1)
+      TMC_SAY_CURRENT(E1);
+    #endif
+    #if AXIS_IS_TMC(E2)
+      TMC_SAY_CURRENT(E2);
+    #endif
+    #if AXIS_IS_TMC(E3)
+      TMC_SAY_CURRENT(E3);
+    #endif
+    #if AXIS_IS_TMC(E4)
+      TMC_SAY_CURRENT(E4);
+    #endif
+    #if AXIS_IS_TMC(E5)
+      TMC_SAY_CURRENT(E5);
+    #endif
+    #if AXIS_IS_TMC(E6)
+      TMC_SAY_CURRENT(E6);
+    #endif
+    #if AXIS_IS_TMC(E7)
+      TMC_SAY_CURRENT(E7);
+    #endif
+  }
+}
+
+#endif // HAS_TRINAMIC_CONFIG
diff --git a/Marlin/src/gcode/feature/trinamic/M911-M914.cpp b/Marlin/src/gcode/feature/trinamic/M911-M914.cpp
new file mode 100644
index 0000000..8c840db
--- /dev/null
+++ b/Marlin/src/gcode/feature/trinamic/M911-M914.cpp
@@ -0,0 +1,429 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if HAS_TRINAMIC_CONFIG
+
+#include "../../gcode.h"
+#include "../../../feature/tmc_util.h"
+#include "../../../module/stepper/indirection.h"
+#include "../../../module/planner.h"
+#include "../../queue.h"
+
+#if ENABLED(MONITOR_DRIVER_STATUS)
+
+  #define M91x_USE(ST) (AXIS_DRIVER_TYPE(ST, TMC2130) || AXIS_DRIVER_TYPE(ST, TMC2160) || AXIS_DRIVER_TYPE(ST, TMC2208) || AXIS_DRIVER_TYPE(ST, TMC2209) || AXIS_DRIVER_TYPE(ST, TMC2660) || AXIS_DRIVER_TYPE(ST, TMC5130) || AXIS_DRIVER_TYPE(ST, TMC5160))
+  #define M91x_USE_E(N) (E_STEPPERS > N && M91x_USE(E##N))
+
+  #define M91x_SOME_X (M91x_USE(X) || M91x_USE(X2))
+  #define M91x_SOME_Y (M91x_USE(Y) || M91x_USE(Y2))
+  #define M91x_SOME_Z (M91x_USE(Z) || M91x_USE(Z2) || M91x_USE(Z3) || M91x_USE(Z4))
+  #define M91x_SOME_E (M91x_USE_E(0) || M91x_USE_E(1) || M91x_USE_E(2) || M91x_USE_E(3) || M91x_USE_E(4) || M91x_USE_E(5) || M91x_USE_E(6) || M91x_USE_E(7))
+
+  #if !M91x_SOME_X && !M91x_SOME_Y && !M91x_SOME_Z && !M91x_SOME_E
+    #error "MONITOR_DRIVER_STATUS requires at least one TMC2130, 2160, 2208, 2209, 2660, 5130, or 5160."
+  #endif
+
+  /**
+   * M911: Report TMC stepper driver overtemperature pre-warn flag
+   *       This flag is held by the library, persisting until cleared by M912
+   */
+  void GcodeSuite::M911() {
+    #if M91x_USE(X)
+      tmc_report_otpw(stepperX);
+    #endif
+    #if M91x_USE(X2)
+      tmc_report_otpw(stepperX2);
+    #endif
+    #if M91x_USE(Y)
+      tmc_report_otpw(stepperY);
+    #endif
+    #if M91x_USE(Y2)
+      tmc_report_otpw(stepperY2);
+    #endif
+    #if M91x_USE(Z)
+      tmc_report_otpw(stepperZ);
+    #endif
+    #if M91x_USE(Z2)
+      tmc_report_otpw(stepperZ2);
+    #endif
+    #if M91x_USE(Z3)
+      tmc_report_otpw(stepperZ3);
+    #endif
+    #if M91x_USE(Z4)
+      tmc_report_otpw(stepperZ4);
+    #endif
+    #if M91x_USE_E(0)
+      tmc_report_otpw(stepperE0);
+    #endif
+    #if M91x_USE_E(1)
+      tmc_report_otpw(stepperE1);
+    #endif
+    #if M91x_USE_E(2)
+      tmc_report_otpw(stepperE2);
+    #endif
+    #if M91x_USE_E(3)
+      tmc_report_otpw(stepperE3);
+    #endif
+    #if M91x_USE_E(4)
+      tmc_report_otpw(stepperE4);
+    #endif
+    #if M91x_USE_E(5)
+      tmc_report_otpw(stepperE5);
+    #endif
+    #if M91x_USE_E(6)
+      tmc_report_otpw(stepperE6);
+    #endif
+    #if M91x_USE_E(7)
+      tmc_report_otpw(stepperE7);
+    #endif
+  }
+
+  /**
+   * M912: Clear TMC stepper driver overtemperature pre-warn flag held by the library
+   *       Specify one or more axes with X, Y, Z, X1, Y1, Z1, X2, Y2, Z2, Z3, Z4 and E[index].
+   *       If no axes are given, clear all.
+   *
+   * Examples:
+   *       M912 X   ; clear X and X2
+   *       M912 X1  ; clear X1 only
+   *       M912 X2  ; clear X2 only
+   *       M912 X E ; clear X, X2, and all E
+   *       M912 E1  ; clear E1 only
+   */
+  void GcodeSuite::M912() {
+    #if M91x_SOME_X
+      const bool hasX = parser.seen(axis_codes.x);
+    #else
+      constexpr bool hasX = false;
+    #endif
+
+    #if M91x_SOME_Y
+      const bool hasY = parser.seen(axis_codes.y);
+    #else
+      constexpr bool hasY = false;
+    #endif
+
+    #if M91x_SOME_Z
+      const bool hasZ = parser.seen(axis_codes.z);
+    #else
+      constexpr bool hasZ = false;
+    #endif
+
+    #if M91x_SOME_E
+      const bool hasE = parser.seen(axis_codes.e);
+    #else
+      constexpr bool hasE = false;
+    #endif
+
+    const bool hasNone = !hasX && !hasY && !hasZ && !hasE;
+
+    #if M91x_SOME_X
+      const int8_t xval = int8_t(parser.byteval(axis_codes.x, 0xFF));
+      #if M91x_USE(X)
+        if (hasNone || xval == 1 || (hasX && xval < 0)) tmc_clear_otpw(stepperX);
+      #endif
+      #if M91x_USE(X2)
+        if (hasNone || xval == 2 || (hasX && xval < 0)) tmc_clear_otpw(stepperX2);
+      #endif
+    #endif
+
+    #if M91x_SOME_Y
+      const int8_t yval = int8_t(parser.byteval(axis_codes.y, 0xFF));
+      #if M91x_USE(Y)
+        if (hasNone || yval == 1 || (hasY && yval < 0)) tmc_clear_otpw(stepperY);
+      #endif
+      #if M91x_USE(Y2)
+        if (hasNone || yval == 2 || (hasY && yval < 0)) tmc_clear_otpw(stepperY2);
+      #endif
+    #endif
+
+    #if M91x_SOME_Z
+      const int8_t zval = int8_t(parser.byteval(axis_codes.z, 0xFF));
+      #if M91x_USE(Z)
+        if (hasNone || zval == 1 || (hasZ && zval < 0)) tmc_clear_otpw(stepperZ);
+      #endif
+      #if M91x_USE(Z2)
+        if (hasNone || zval == 2 || (hasZ && zval < 0)) tmc_clear_otpw(stepperZ2);
+      #endif
+      #if M91x_USE(Z3)
+        if (hasNone || zval == 3 || (hasZ && zval < 0)) tmc_clear_otpw(stepperZ3);
+      #endif
+      #if M91x_USE(Z4)
+        if (hasNone || zval == 4 || (hasZ && zval < 0)) tmc_clear_otpw(stepperZ4);
+      #endif
+    #endif
+
+    #if M91x_SOME_E
+      const int8_t eval = int8_t(parser.byteval(axis_codes.e, 0xFF));
+      #if M91x_USE_E(0)
+        if (hasNone || eval == 0 || (hasE && eval < 0)) tmc_clear_otpw(stepperE0);
+      #endif
+      #if M91x_USE_E(1)
+        if (hasNone || eval == 1 || (hasE && eval < 0)) tmc_clear_otpw(stepperE1);
+      #endif
+      #if M91x_USE_E(2)
+        if (hasNone || eval == 2 || (hasE && eval < 0)) tmc_clear_otpw(stepperE2);
+      #endif
+      #if M91x_USE_E(3)
+        if (hasNone || eval == 3 || (hasE && eval < 0)) tmc_clear_otpw(stepperE3);
+      #endif
+      #if M91x_USE_E(4)
+        if (hasNone || eval == 4 || (hasE && eval < 0)) tmc_clear_otpw(stepperE4);
+      #endif
+      #if M91x_USE_E(5)
+        if (hasNone || eval == 5 || (hasE && eval < 0)) tmc_clear_otpw(stepperE5);
+      #endif
+      #if M91x_USE_E(6)
+        if (hasNone || eval == 6 || (hasE && eval < 0)) tmc_clear_otpw(stepperE6);
+      #endif
+      #if M91x_USE_E(7)
+        if (hasNone || eval == 7 || (hasE && eval < 0)) tmc_clear_otpw(stepperE7);
+      #endif
+    #endif
+  }
+
+#endif // MONITOR_DRIVER_STATUS
+
+/**
+ * M913: Set HYBRID_THRESHOLD speed.
+ */
+#if ENABLED(HYBRID_THRESHOLD)
+  void GcodeSuite::M913() {
+    #define TMC_SAY_PWMTHRS(A,Q) tmc_print_pwmthrs(stepper##Q)
+    #define TMC_SET_PWMTHRS(A,Q) stepper##Q.set_pwm_thrs(value)
+    #define TMC_SAY_PWMTHRS_E(E) tmc_print_pwmthrs(stepperE##E)
+    #define TMC_SET_PWMTHRS_E(E) stepperE##E.set_pwm_thrs(value)
+
+    bool report = true;
+    #if AXIS_IS_TMC(X) || AXIS_IS_TMC(X2) || AXIS_IS_TMC(Y) || AXIS_IS_TMC(Y2) || AXIS_IS_TMC(Z) || AXIS_IS_TMC(Z2) || AXIS_IS_TMC(Z3) || AXIS_IS_TMC(Z4)
+      const uint8_t index = parser.byteval('I');
+    #endif
+    LOOP_XYZE(i) if (int32_t value = parser.longval(axis_codes[i])) {
+      report = false;
+      switch (i) {
+        case X_AXIS:
+          #if AXIS_HAS_STEALTHCHOP(X)
+            if (index < 2) TMC_SET_PWMTHRS(X,X);
+          #endif
+          #if AXIS_HAS_STEALTHCHOP(X2)
+            if (!(index & 1)) TMC_SET_PWMTHRS(X,X2);
+          #endif
+          break;
+        case Y_AXIS:
+          #if AXIS_HAS_STEALTHCHOP(Y)
+            if (index < 2) TMC_SET_PWMTHRS(Y,Y);
+          #endif
+          #if AXIS_HAS_STEALTHCHOP(Y2)
+            if (!(index & 1)) TMC_SET_PWMTHRS(Y,Y2);
+          #endif
+          break;
+        case Z_AXIS:
+          #if AXIS_HAS_STEALTHCHOP(Z)
+            if (index < 2) TMC_SET_PWMTHRS(Z,Z);
+          #endif
+          #if AXIS_HAS_STEALTHCHOP(Z2)
+            if (index == 0 || index == 2) TMC_SET_PWMTHRS(Z,Z2);
+          #endif
+          #if AXIS_HAS_STEALTHCHOP(Z3)
+            if (index == 0 || index == 3) TMC_SET_PWMTHRS(Z,Z3);
+          #endif
+          #if AXIS_HAS_STEALTHCHOP(Z4)
+            if (index == 0 || index == 4) TMC_SET_PWMTHRS(Z,Z4);
+          #endif
+          break;
+        case E_AXIS: {
+          #if E_STEPPERS
+            const int8_t target_extruder = get_target_extruder_from_command();
+            if (target_extruder < 0) return;
+            switch (target_extruder) {
+              #if AXIS_HAS_STEALTHCHOP(E0)
+                case 0: TMC_SET_PWMTHRS_E(0); break;
+              #endif
+              #if E_STEPPERS > 1 && AXIS_HAS_STEALTHCHOP(E1)
+                case 1: TMC_SET_PWMTHRS_E(1); break;
+              #endif
+              #if E_STEPPERS > 2 && AXIS_HAS_STEALTHCHOP(E2)
+                case 2: TMC_SET_PWMTHRS_E(2); break;
+              #endif
+              #if E_STEPPERS > 3 && AXIS_HAS_STEALTHCHOP(E3)
+                case 3: TMC_SET_PWMTHRS_E(3); break;
+              #endif
+              #if E_STEPPERS > 4 && AXIS_HAS_STEALTHCHOP(E4)
+                case 4: TMC_SET_PWMTHRS_E(4); break;
+              #endif
+              #if E_STEPPERS > 5 && AXIS_HAS_STEALTHCHOP(E5)
+                case 5: TMC_SET_PWMTHRS_E(5); break;
+              #endif
+              #if E_STEPPERS > 6 && AXIS_HAS_STEALTHCHOP(E6)
+                case 6: TMC_SET_PWMTHRS_E(6); break;
+              #endif
+              #if E_STEPPERS > 7 && AXIS_HAS_STEALTHCHOP(E7)
+                case 7: TMC_SET_PWMTHRS_E(7); break;
+              #endif
+            }
+          #endif // E_STEPPERS
+        } break;
+      }
+    }
+
+    if (report) {
+      #if AXIS_HAS_STEALTHCHOP(X)
+        TMC_SAY_PWMTHRS(X,X);
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(X2)
+        TMC_SAY_PWMTHRS(X,X2);
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(Y)
+        TMC_SAY_PWMTHRS(Y,Y);
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(Y2)
+        TMC_SAY_PWMTHRS(Y,Y2);
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(Z)
+        TMC_SAY_PWMTHRS(Z,Z);
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(Z2)
+        TMC_SAY_PWMTHRS(Z,Z2);
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(Z3)
+        TMC_SAY_PWMTHRS(Z,Z3);
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(Z4)
+        TMC_SAY_PWMTHRS(Z,Z4);
+      #endif
+      #if E_STEPPERS && AXIS_HAS_STEALTHCHOP(E0)
+        TMC_SAY_PWMTHRS_E(0);
+      #endif
+      #if E_STEPPERS > 1 && AXIS_HAS_STEALTHCHOP(E1)
+        TMC_SAY_PWMTHRS_E(1);
+      #endif
+      #if E_STEPPERS > 2 && AXIS_HAS_STEALTHCHOP(E2)
+        TMC_SAY_PWMTHRS_E(2);
+      #endif
+      #if E_STEPPERS > 3 && AXIS_HAS_STEALTHCHOP(E3)
+        TMC_SAY_PWMTHRS_E(3);
+      #endif
+      #if E_STEPPERS > 4 && AXIS_HAS_STEALTHCHOP(E4)
+        TMC_SAY_PWMTHRS_E(4);
+      #endif
+      #if E_STEPPERS > 5 && AXIS_HAS_STEALTHCHOP(E5)
+        TMC_SAY_PWMTHRS_E(5);
+      #endif
+      #if E_STEPPERS > 6 && AXIS_HAS_STEALTHCHOP(E6)
+        TMC_SAY_PWMTHRS_E(6);
+      #endif
+      #if E_STEPPERS > 7 && AXIS_HAS_STEALTHCHOP(E7)
+        TMC_SAY_PWMTHRS_E(7);
+      #endif
+    }
+  }
+#endif // HYBRID_THRESHOLD
+
+/**
+ * M914: Set StallGuard sensitivity.
+ */
+#if USE_SENSORLESS
+  void GcodeSuite::M914() {
+
+    bool report = true;
+    const uint8_t index = parser.byteval('I');
+    LOOP_XYZ(i) if (parser.seen(XYZ_CHAR(i))) {
+      const int16_t value = parser.value_int();
+      report = false;
+      switch (i) {
+        #if X_SENSORLESS
+          case X_AXIS:
+            #if AXIS_HAS_STALLGUARD(X)
+              if (index < 2) stepperX.homing_threshold(value);
+            #endif
+            #if AXIS_HAS_STALLGUARD(X2)
+              if (!(index & 1)) stepperX2.homing_threshold(value);
+            #endif
+            break;
+        #endif
+        #if Y_SENSORLESS
+          case Y_AXIS:
+            #if AXIS_HAS_STALLGUARD(Y)
+              if (index < 2) stepperY.homing_threshold(value);
+            #endif
+            #if AXIS_HAS_STALLGUARD(Y2)
+              if (!(index & 1)) stepperY2.homing_threshold(value);
+            #endif
+            break;
+        #endif
+        #if Z_SENSORLESS
+          case Z_AXIS:
+            #if AXIS_HAS_STALLGUARD(Z)
+              if (index < 2) stepperZ.homing_threshold(value);
+            #endif
+            #if AXIS_HAS_STALLGUARD(Z2)
+              if (index == 0 || index == 2) stepperZ2.homing_threshold(value);
+            #endif
+            #if AXIS_HAS_STALLGUARD(Z3)
+              if (index == 0 || index == 3) stepperZ3.homing_threshold(value);
+            #endif
+            #if AXIS_HAS_STALLGUARD(Z4)
+              if (index == 0 || index == 4) stepperZ4.homing_threshold(value);
+            #endif
+            break;
+        #endif
+      }
+    }
+
+    if (report) {
+      #if X_SENSORLESS
+        #if AXIS_HAS_STALLGUARD(X)
+          tmc_print_sgt(stepperX);
+        #endif
+        #if AXIS_HAS_STALLGUARD(X2)
+          tmc_print_sgt(stepperX2);
+        #endif
+      #endif
+      #if Y_SENSORLESS
+        #if AXIS_HAS_STALLGUARD(Y)
+          tmc_print_sgt(stepperY);
+        #endif
+        #if AXIS_HAS_STALLGUARD(Y2)
+          tmc_print_sgt(stepperY2);
+        #endif
+      #endif
+      #if Z_SENSORLESS
+        #if AXIS_HAS_STALLGUARD(Z)
+          tmc_print_sgt(stepperZ);
+        #endif
+        #if AXIS_HAS_STALLGUARD(Z2)
+          tmc_print_sgt(stepperZ2);
+        #endif
+        #if AXIS_HAS_STALLGUARD(Z3)
+          tmc_print_sgt(stepperZ3);
+        #endif
+        #if AXIS_HAS_STALLGUARD(Z4)
+          tmc_print_sgt(stepperZ4);
+        #endif
+      #endif
+    }
+  }
+#endif // USE_SENSORLESS
+
+#endif // HAS_TRINAMIC_CONFIG
diff --git a/Marlin/src/gcode/gcode.cpp b/Marlin/src/gcode/gcode.cpp
new file mode 100644
index 0000000..f917318
--- /dev/null
+++ b/Marlin/src/gcode/gcode.cpp
@@ -0,0 +1,1084 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * gcode.cpp - Temporary container for all gcode handlers
+ *             Most will migrate to classes, by feature.
+ */
+
+#include "gcode.h"
+GcodeSuite gcode;
+
+#if ENABLED(WIFI_CUSTOM_COMMAND)
+  extern bool wifi_custom_command(char * const command_ptr);
+#endif
+
+#include "parser.h"
+#include "queue.h"
+#include "../module/motion.h"
+
+#if ENABLED(PRINTCOUNTER)
+  #include "../module/printcounter.h"
+#endif
+
+#if ENABLED(HOST_ACTION_COMMANDS)
+  #include "../feature/host_actions.h"
+#endif
+
+#if ENABLED(POWER_LOSS_RECOVERY)
+  #include "../sd/cardreader.h"
+  #include "../feature/powerloss.h"
+#endif
+
+#if ENABLED(CANCEL_OBJECTS)
+  #include "../feature/cancel_object.h"
+#endif
+
+#if ENABLED(LASER_MOVE_POWER)
+  #include "../feature/spindle_laser.h"
+#endif
+
+#if ENABLED(PASSWORD_FEATURE)
+  #include "../feature/password/password.h"
+#endif
+
+#include "../MarlinCore.h" // for idle, kill
+
+// Inactivity shutdown
+millis_t GcodeSuite::previous_move_ms = 0,
+         GcodeSuite::max_inactive_time = 0,
+         GcodeSuite::stepper_inactive_time = SEC_TO_MS(DEFAULT_STEPPER_DEACTIVE_TIME);
+
+// Relative motion mode for each logical axis
+static constexpr xyze_bool_t ar_init = AXIS_RELATIVE_MODES;
+uint8_t GcodeSuite::axis_relative = (
+    (ar_init.x ? _BV(REL_X) : 0)
+  | (ar_init.y ? _BV(REL_Y) : 0)
+  | (ar_init.z ? _BV(REL_Z) : 0)
+  | (ar_init.e ? _BV(REL_E) : 0)
+);
+
+#if EITHER(HAS_AUTO_REPORTING, HOST_KEEPALIVE_FEATURE)
+  bool GcodeSuite::autoreport_paused; // = false
+#endif
+
+#if ENABLED(HOST_KEEPALIVE_FEATURE)
+  GcodeSuite::MarlinBusyState GcodeSuite::busy_state = NOT_BUSY;
+  uint8_t GcodeSuite::host_keepalive_interval = DEFAULT_KEEPALIVE_INTERVAL;
+#endif
+
+#if ENABLED(CNC_WORKSPACE_PLANES)
+  GcodeSuite::WorkspacePlane GcodeSuite::workspace_plane = PLANE_XY;
+#endif
+
+#if ENABLED(CNC_COORDINATE_SYSTEMS)
+  int8_t GcodeSuite::active_coordinate_system = -1; // machine space
+  xyz_pos_t GcodeSuite::coordinate_system[MAX_COORDINATE_SYSTEMS];
+#endif
+
+/**
+ * Get the target extruder from the T parameter or the active_extruder
+ * Return -1 if the T parameter is out of range
+ */
+int8_t GcodeSuite::get_target_extruder_from_command() {
+  if (parser.seenval('T')) {
+    const int8_t e = parser.value_byte();
+    if (e < EXTRUDERS) return e;
+    SERIAL_ECHO_START();
+    SERIAL_CHAR('M'); SERIAL_ECHO(parser.codenum);
+    SERIAL_ECHOLNPAIR(" " STR_INVALID_EXTRUDER " ", int(e));
+    return -1;
+  }
+  return active_extruder;
+}
+
+/**
+ * Get the target e stepper from the T parameter
+ * Return -1 if the T parameter is out of range or unspecified
+ */
+int8_t GcodeSuite::get_target_e_stepper_from_command() {
+  const int8_t e = parser.intval('T', -1);
+  if (WITHIN(e, 0, E_STEPPERS - 1)) return e;
+
+  SERIAL_ECHO_START();
+  SERIAL_CHAR('M'); SERIAL_ECHO(parser.codenum);
+  if (e == -1)
+    SERIAL_ECHOLNPGM(" " STR_E_STEPPER_NOT_SPECIFIED);
+  else
+    SERIAL_ECHOLNPAIR(" " STR_INVALID_E_STEPPER " ", int(e));
+  return -1;
+}
+
+/**
+ * Set XYZE destination and feedrate from the current GCode command
+ *
+ *  - Set destination from included axis codes
+ *  - Set to current for missing axis codes
+ *  - Set the feedrate, if included
+ */
+void GcodeSuite::get_destination_from_command() {
+  xyze_bool_t seen = { false, false, false, false };
+
+  #if ENABLED(CANCEL_OBJECTS)
+    const bool &skip_move = cancelable.skipping;
+  #else
+    constexpr bool skip_move = false;
+  #endif
+
+  // Get new XYZ position, whether absolute or relative
+  LOOP_XYZ(i) {
+    if ( (seen[i] = parser.seenval(XYZ_CHAR(i))) ) {
+      const float v = parser.value_axis_units((AxisEnum)i);
+      if (skip_move)
+        destination[i] = current_position[i];
+      else
+        destination[i] = axis_is_relative(AxisEnum(i)) ? current_position[i] + v : LOGICAL_TO_NATIVE(v, i);
+    }
+    else
+      destination[i] = current_position[i];
+  }
+
+  // Get new E position, whether absolute or relative
+  if ( (seen.e = parser.seenval('E')) ) {
+    const float v = parser.value_axis_units(E_AXIS);
+    destination.e = axis_is_relative(E_AXIS) ? current_position.e + v : v;
+  }
+  else
+    destination.e = current_position.e;
+
+  #if ENABLED(POWER_LOSS_RECOVERY) && !PIN_EXISTS(POWER_LOSS)
+    // Only update power loss recovery on moves with E
+    if (recovery.enabled && IS_SD_PRINTING() && seen.e && (seen.x || seen.y))
+      recovery.save();
+  #endif
+
+  if (parser.linearval('F') > 0)
+    feedrate_mm_s = parser.value_feedrate();
+
+  #if ENABLED(PRINTCOUNTER)
+    if (!DEBUGGING(DRYRUN) && !skip_move)
+      print_job_timer.incFilamentUsed(destination.e - current_position.e);
+  #endif
+
+  // Get ABCDHI mixing factors
+  #if BOTH(MIXING_EXTRUDER, DIRECT_MIXING_IN_G1)
+    M165();
+  #endif
+
+  #if ENABLED(LASER_MOVE_POWER)
+    // Set the laser power in the planner to configure this move
+    if (parser.seen('S')) {
+      const float spwr = parser.value_float();
+      cutter.inline_power(TERN(SPINDLE_LASER_PWM, cutter.power_to_range(cutter_power_t(round(spwr))), spwr > 0 ? 255 : 0));
+    }
+    else if (ENABLED(LASER_MOVE_G0_OFF) && parser.codenum == 0) // G0
+      cutter.set_inline_enabled(false);
+  #endif
+}
+
+/**
+ * Dwell waits immediately. It does not synchronize. Use M400 instead of G4
+ */
+void GcodeSuite::dwell(millis_t time) {
+  time += millis();
+  while (PENDING(millis(), time)) idle();
+}
+
+/**
+ * When G29_RETRY_AND_RECOVER is enabled, call G29() in
+ * a loop with recovery and retry handling.
+ */
+#if BOTH(HAS_LEVELING, G29_RETRY_AND_RECOVER)
+
+  void GcodeSuite::event_probe_recover() {
+    TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_INFO, PSTR("G29 Retrying"), DISMISS_STR));
+    #ifdef ACTION_ON_G29_RECOVER
+      host_action(PSTR(ACTION_ON_G29_RECOVER));
+    #endif
+    #ifdef G29_RECOVER_COMMANDS
+      process_subcommands_now_P(PSTR(G29_RECOVER_COMMANDS));
+    #endif
+  }
+
+  void GcodeSuite::event_probe_failure() {
+    #ifdef ACTION_ON_G29_FAILURE
+      host_action(PSTR(ACTION_ON_G29_FAILURE));
+    #endif
+    #ifdef G29_FAILURE_COMMANDS
+      process_subcommands_now_P(PSTR(G29_FAILURE_COMMANDS));
+    #endif
+    #if ENABLED(G29_HALT_ON_FAILURE)
+      #ifdef ACTION_ON_CANCEL
+        host_action_cancel();
+      #endif
+      kill(GET_TEXT(MSG_LCD_PROBING_FAILED));
+    #endif
+  }
+
+  #ifndef G29_MAX_RETRIES
+    #define G29_MAX_RETRIES 0
+  #endif
+
+  void GcodeSuite::G29_with_retry() {
+    uint8_t retries = G29_MAX_RETRIES;
+    while (G29()) { // G29 should return true for failed probes ONLY
+      if (retries) {
+        event_probe_recover();
+        --retries;
+      }
+      else {
+        event_probe_failure();
+        return;
+      }
+    }
+
+    TERN_(HOST_PROMPT_SUPPORT, host_action_prompt_end());
+
+    #ifdef G29_SUCCESS_COMMANDS
+      process_subcommands_now_P(PSTR(G29_SUCCESS_COMMANDS));
+    #endif
+  }
+
+#endif // HAS_LEVELING && G29_RETRY_AND_RECOVER
+
+//
+// Placeholders for non-migrated codes
+//
+#if ENABLED(M100_FREE_MEMORY_WATCHER)
+  extern void M100_dump_routine(PGM_P const title, const char * const start, const char * const end);
+#endif
+
+/**
+ * Process the parsed command and dispatch it to its handler
+ */
+void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) {
+  KEEPALIVE_STATE(IN_HANDLER);
+
+ /**
+  * Block all Gcodes except M511 Unlock Printer, if printer is locked
+  * Will still block Gcodes if M511 is disabled, in which case the printer should be unlocked via LCD Menu
+  */
+  #if ENABLED(PASSWORD_FEATURE)
+    if (password.is_locked && !parser.is_command('M', 511)) {
+      SERIAL_ECHO_MSG(STR_PRINTER_LOCKED);
+      if (!no_ok) queue.ok_to_send();
+      return;
+    }
+  #endif
+
+  // Handle a known G, M, or T
+  switch (parser.command_letter) {
+    case 'G': switch (parser.codenum) {
+
+      case 0: case 1:                                             // G0: Fast Move, G1: Linear Move
+        G0_G1(TERN_(HAS_FAST_MOVES, parser.codenum == 0)); break;
+
+      #if ENABLED(ARC_SUPPORT) && DISABLED(SCARA)
+        case 2: case 3: G2_G3(parser.codenum == 2); break;        // G2: CW ARC, G3: CCW ARC
+      #endif
+
+      case 4: G4(); break;                                        // G4: Dwell
+
+      #if ENABLED(BEZIER_CURVE_SUPPORT)
+        case 5: G5(); break;                                      // G5: Cubic B_spline
+      #endif
+
+      #if ENABLED(DIRECT_STEPPING)
+        case 6: G6(); break;                                      // G6: Direct Stepper Move
+      #endif
+
+      #if ENABLED(FWRETRACT)
+        case 10: G10(); break;                                    // G10: Retract / Swap Retract
+        case 11: G11(); break;                                    // G11: Recover / Swap Recover
+      #endif
+
+      #if ENABLED(NOZZLE_CLEAN_FEATURE)
+        case 12: G12(); break;                                    // G12: Nozzle Clean
+      #endif
+
+      #if ENABLED(CNC_WORKSPACE_PLANES)
+        case 17: G17(); break;                                    // G17: Select Plane XY
+        case 18: G18(); break;                                    // G18: Select Plane ZX
+        case 19: G19(); break;                                    // G19: Select Plane YZ
+      #endif
+
+      #if ENABLED(INCH_MODE_SUPPORT)
+        case 20: G20(); break;                                    // G20: Inch Mode
+        case 21: G21(); break;                                    // G21: MM Mode
+      #else
+        case 21: NOOP; break;                                     // No error on unknown G21
+      #endif
+
+      #if ENABLED(G26_MESH_VALIDATION)
+        case 26: G26(); break;                                    // G26: Mesh Validation Pattern generation
+      #endif
+
+      #if ENABLED(NOZZLE_PARK_FEATURE)
+        case 27: G27(); break;                                    // G27: Nozzle Park
+      #endif
+
+      case 28: G28(); break;                                      // G28: Home one or more axes
+
+      #if HAS_LEVELING
+        case 29:                                                  // G29: Bed leveling calibration
+          TERN(G29_RETRY_AND_RECOVER, G29_with_retry, G29)();
+          break;
+      #endif
+
+      #if HAS_BED_PROBE
+        case 30: G30(); break;                                    // G30: Single Z probe
+        #if ENABLED(Z_PROBE_SLED)
+          case 31: G31(); break;                                  // G31: dock the sled
+          case 32: G32(); break;                                  // G32: undock the sled
+        #endif
+      #endif
+
+      #if ENABLED(DELTA_AUTO_CALIBRATION)
+        case 33: G33(); break;                                    // G33: Delta Auto-Calibration
+      #endif
+
+      #if ANY(Z_MULTI_ENDSTOPS, Z_STEPPER_AUTO_ALIGN, MECHANICAL_GANTRY_CALIBRATION)
+        case 34: G34(); break;                                    // G34: Z Stepper automatic alignment using probe
+      #endif
+
+      #if ENABLED(ASSISTED_TRAMMING)
+        case 35: G35(); break;                                    // G35: Read four bed corners to help adjust bed screws
+      #endif
+
+      #if ENABLED(G38_PROBE_TARGET)
+        case 38:                                                  // G38.2, G38.3: Probe towards target
+          if (WITHIN(parser.subcode, 2, TERN(G38_PROBE_AWAY, 5, 3)))
+            G38(parser.subcode);                                  // G38.4, G38.5: Probe away from target
+          break;
+      #endif
+
+      #if ENABLED(CNC_COORDINATE_SYSTEMS)
+        case 53: G53(); break;                                    // G53: (prefix) Apply native workspace
+        case 54: G54(); break;                                    // G54: Switch to Workspace 1
+        case 55: G55(); break;                                    // G55: Switch to Workspace 2
+        case 56: G56(); break;                                    // G56: Switch to Workspace 3
+        case 57: G57(); break;                                    // G57: Switch to Workspace 4
+        case 58: G58(); break;                                    // G58: Switch to Workspace 5
+        case 59: G59(); break;                                    // G59.0 - G59.3: Switch to Workspace 6-9
+      #endif
+
+      #if SAVED_POSITIONS
+        case 60: G60(); break;                                    // G60:  save current position
+        case 61: G61(); break;                                    // G61:  Apply/restore saved coordinates.
+      #endif
+
+      #if ENABLED(PROBE_TEMP_COMPENSATION)
+        case 76: G76(); break;                                    // G76: Calibrate first layer compensation values
+      #endif
+
+      #if ENABLED(GCODE_MOTION_MODES)
+        case 80: G80(); break;                                    // G80: Reset the current motion mode
+      #endif
+
+      case 90: set_relative_mode(false); break;                   // G90: Absolute Mode
+      case 91: set_relative_mode(true);  break;                   // G91: Relative Mode
+
+      case 92: G92(); break;                                      // G92: Set current axis position(s)
+
+      #if HAS_MESH
+        case 42: G42(); break;                                    // G42: Coordinated move to a mesh point
+      #endif
+
+      #if ENABLED(CALIBRATION_GCODE)
+        case 425: G425(); break;                                  // G425: Perform calibration with calibration cube
+      #endif
+
+      #if ENABLED(DEBUG_GCODE_PARSER)
+        case 800: parser.debug(); break;                          // G800: GCode Parser Test for G
+      #endif
+
+      default: parser.unknown_command_warning(); break;
+    }
+    break;
+
+    case 'M': switch (parser.codenum) {
+
+      #if HAS_RESUME_CONTINUE
+        case 0:                                                   // M0: Unconditional stop - Wait for user button press on LCD
+        case 1: M0_M1(); break;                                   // M1: Conditional stop - Wait for user button press on LCD
+      #endif
+
+      #if HAS_CUTTER
+        case 3: M3_M4(false); break;                              // M3: Turn ON Laser | Spindle (clockwise), set Power | Speed
+        case 4: M3_M4(true ); break;                              // M4: Turn ON Laser | Spindle (counter-clockwise), set Power | Speed
+        case 5: M5(); break;                                      // M5: Turn OFF Laser | Spindle
+      #endif
+
+      #if ENABLED(COOLANT_CONTROL)
+        #if ENABLED(COOLANT_MIST)
+          case 7: M7(); break;                                    // M7: Mist coolant ON
+        #endif
+        #if ENABLED(COOLANT_FLOOD)
+          case 8: M8(); break;                                    // M8: Flood coolant ON
+        #endif
+        case 9: M9(); break;                                      // M9: Coolant OFF
+      #endif
+
+      #if ENABLED(EXTERNAL_CLOSED_LOOP_CONTROLLER)
+        case 12: M12(); break;                                    // M12: Synchronize and optionally force a CLC set
+      #endif
+
+      #if ENABLED(EXPECTED_PRINTER_CHECK)
+        case 16: M16(); break;                                    // M16: Expected printer check
+      #endif
+
+      case 17: M17(); break;                                      // M17: Enable all stepper motors
+
+      #if ENABLED(SDSUPPORT)
+        case 20: M20(); break;                                    // M20: List SD card
+        case 21: M21(); break;                                    // M21: Init SD card
+        case 22: M22(); break;                                    // M22: Release SD card
+        case 23: M23(); break;                                    // M23: Select file
+        case 24: M24(); break;                                    // M24: Start SD print
+        case 25: M25(); break;                                    // M25: Pause SD print
+        case 26: M26(); break;                                    // M26: Set SD index
+        case 27: M27(); break;                                    // M27: Get SD status
+        case 28: M28(); break;                                    // M28: Start SD write
+        case 29: M29(); break;                                    // M29: Stop SD write
+        case 30: M30(); break;                                    // M30 <filename> Delete File
+
+        #if HAS_MEDIA_SUBCALLS
+          case 32: M32(); break;                                  // M32: Select file and start SD print
+        #endif
+
+        #if ENABLED(LONG_FILENAME_HOST_SUPPORT)
+          case 33: M33(); break;                                  // M33: Get the long full path to a file or folder
+        #endif
+
+        #if BOTH(SDCARD_SORT_ALPHA, SDSORT_GCODE)
+          case 34: M34(); break;                                  // M34: Set SD card sorting options
+        #endif
+
+        case 928: M928(); break;                                  // M928: Start SD write
+      #endif // SDSUPPORT
+
+      case 31: M31(); break;                                      // M31: Report time since the start of SD print or last M109
+
+      #if ENABLED(DIRECT_PIN_CONTROL)
+        case 42: M42(); break;                                    // M42: Change pin state
+      #endif
+
+      #if ENABLED(PINS_DEBUGGING)
+        case 43: M43(); break;                                    // M43: Read pin state
+      #endif
+
+      #if ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)
+        case 48: M48(); break;                                    // M48: Z probe repeatability test
+      #endif
+
+      #if ENABLED(LCD_SET_PROGRESS_MANUALLY)
+        case 73: M73(); break;                                    // M73: Set progress percentage (for display on LCD)
+      #endif
+
+      case 75: M75(); break;                                      // M75: Start print timer
+      case 76: M76(); break;                                      // M76: Pause print timer
+      case 77: M77(); break;                                      // M77: Stop print timer
+
+      #if ENABLED(PRINTCOUNTER)
+        case 78: M78(); break;                                    // M78: Show print statistics
+      #endif
+
+      #if ENABLED(M100_FREE_MEMORY_WATCHER)
+        case 100: M100(); break;                                  // M100: Free Memory Report
+      #endif
+
+      #if EXTRUDERS
+        case 104: M104(); break;                                  // M104: Set hot end temperature
+        case 109: M109(); break;                                  // M109: Wait for hotend temperature to reach target
+      #endif
+
+      case 105: M105(); return;                                   // M105: Report Temperatures (and say "ok")
+
+      #if HAS_FAN
+        case 106: M106(); break;                                  // M106: Fan On
+        case 107: M107(); break;                                  // M107: Fan Off
+      #endif
+
+      case 110: M110(); break;                                    // M110: Set Current Line Number
+      case 111: M111(); break;                                    // M111: Set debug level
+
+      #if DISABLED(EMERGENCY_PARSER)
+        case 108: M108(); break;                                  // M108: Cancel Waiting
+        case 112: M112(); break;                                  // M112: Full Shutdown
+        case 410: M410(); break;                                  // M410: Quickstop - Abort all the planned moves.
+        TERN_(HOST_PROMPT_SUPPORT, case 876:)                     // M876: Handle Host prompt responses
+      #else
+        case 108: case 112: case 410:
+        TERN_(HOST_PROMPT_SUPPORT, case 876:)
+        break;
+      #endif
+
+      #if ENABLED(HOST_KEEPALIVE_FEATURE)
+        case 113: M113(); break;                                  // M113: Set Host Keepalive interval
+      #endif
+
+      #if HAS_HEATED_BED
+        case 140: M140(); break;                                  // M140: Set bed temperature
+        case 190: M190(); break;                                  // M190: Wait for bed temperature to reach target
+      #endif
+
+      #if HAS_HEATED_CHAMBER
+        case 141: M141(); break;                                  // M141: Set chamber temperature
+        case 191: M191(); break;                                  // M191: Wait for chamber temperature to reach target
+      #endif
+
+      #if BOTH(AUTO_REPORT_TEMPERATURES, HAS_TEMP_SENSOR)
+        case 155: M155(); break;                                  // M155: Set temperature auto-report interval
+      #endif
+
+      #if ENABLED(PARK_HEAD_ON_PAUSE)
+        case 125: M125(); break;                                  // M125: Store current position and move to filament change position
+      #endif
+
+      #if ENABLED(BARICUDA)
+        // PWM for HEATER_1_PIN
+        #if HAS_HEATER_1
+          case 126: M126(); break;                                // M126: valve open
+          case 127: M127(); break;                                // M127: valve closed
+        #endif
+
+        // PWM for HEATER_2_PIN
+        #if HAS_HEATER_2
+          case 128: M128(); break;                                // M128: valve open
+          case 129: M129(); break;                                // M129: valve closed
+        #endif
+      #endif // BARICUDA
+
+      #if ENABLED(PSU_CONTROL)
+        case 80: M80(); break;                                    // M80: Turn on Power Supply
+      #endif
+      case 81: M81(); break;                                      // M81: Turn off Power, including Power Supply, if possible
+
+      case 82: M82(); break;                                      // M82: Set E axis normal mode (same as other axes)
+      case 83: M83(); break;                                      // M83: Set E axis relative mode
+      case 18: case 84: M18_M84(); break;                         // M18/M84: Disable Steppers / Set Timeout
+      case 85: M85(); break;                                      // M85: Set inactivity stepper shutdown timeout
+      case 92: M92(); break;                                      // M92: Set the steps-per-unit for one or more axes
+      case 114: M114(); break;                                    // M114: Report current position
+      case 115: M115(); break;                                    // M115: Report capabilities
+      case 117: M117(); break;                                    // M117: Set LCD message text, if possible
+      case 118: M118(); break;                                    // M118: Display a message in the host console
+      case 119: M119(); break;                                    // M119: Report endstop states
+      case 120: M120(); break;                                    // M120: Enable endstops
+      case 121: M121(); break;                                    // M121: Disable endstops
+
+      #if PREHEAT_COUNT
+        case 145: M145(); break;                                  // M145: Set material heatup parameters
+      #endif
+
+      #if ENABLED(TEMPERATURE_UNITS_SUPPORT)
+        case 149: M149(); break;                                  // M149: Set temperature units
+      #endif
+
+      #if HAS_COLOR_LEDS
+        case 150: M150(); break;                                  // M150: Set Status LED Color
+      #endif
+
+      #if ENABLED(MIXING_EXTRUDER)
+        case 163: M163(); break;                                  // M163: Set a component weight for mixing extruder
+        case 164: M164(); break;                                  // M164: Save current mix as a virtual extruder
+        #if ENABLED(DIRECT_MIXING_IN_G1)
+          case 165: M165(); break;                                // M165: Set multiple mix weights
+        #endif
+        #if ENABLED(GRADIENT_MIX)
+          case 166: M166(); break;                                // M166: Set Gradient Mix
+        #endif
+      #endif
+
+      #if DISABLED(NO_VOLUMETRICS)
+        case 200: M200(); break;                                  // M200: Set filament diameter, E to cubic units
+      #endif
+
+      case 201: M201(); break;                                    // M201: Set max acceleration for print moves (units/s^2)
+
+      #if 0
+        case 202: M202(); break;                                  // M202: Not used for Sprinter/grbl gen6
+      #endif
+
+      case 203: M203(); break;                                    // M203: Set max feedrate (units/sec)
+      case 204: M204(); break;                                    // M204: Set acceleration
+      case 205: M205(); break;                                    // M205: Set advanced settings
+
+      #if HAS_M206_COMMAND
+        case 206: M206(); break;                                  // M206: Set home offsets
+      #endif
+
+      #if ENABLED(FWRETRACT)
+        case 207: M207(); break;                                  // M207: Set Retract Length, Feedrate, and Z lift
+        case 208: M208(); break;                                  // M208: Set Recover (unretract) Additional Length and Feedrate
+        #if ENABLED(FWRETRACT_AUTORETRACT)
+          case 209:
+            if (MIN_AUTORETRACT <= MAX_AUTORETRACT) M209();       // M209: Turn Automatic Retract Detection on/off
+            break;
+        #endif
+      #endif
+
+      #if HAS_SOFTWARE_ENDSTOPS
+        case 211: M211(); break;                                  // M211: Enable, Disable, and/or Report software endstops
+      #endif
+
+      #if HAS_MULTI_EXTRUDER
+        case 217: M217(); break;                                  // M217: Set filament swap parameters
+      #endif
+
+      #if HAS_HOTEND_OFFSET
+        case 218: M218(); break;                                  // M218: Set a tool offset
+      #endif
+
+      case 220: M220(); break;                                    // M220: Set Feedrate Percentage: S<percent> ("FR" on your LCD)
+
+      #if EXTRUDERS
+        case 221: M221(); break;                                  // M221: Set Flow Percentage
+      #endif
+
+      #if ENABLED(DIRECT_PIN_CONTROL)
+        case 226: M226(); break;                                  // M226: Wait until a pin reaches a state
+      #endif
+
+      #if HAS_SERVOS
+        case 280: M280(); break;                                  // M280: Set servo position absolute
+        #if ENABLED(EDITABLE_SERVO_ANGLES)
+          case 281: M281(); break;                                // M281: Set servo angles
+        #endif
+      #endif
+
+      #if ENABLED(BABYSTEPPING)
+        case 290: M290(); break;                                  // M290: Babystepping
+      #endif
+
+      #if HAS_BUZZER
+        case 300: M300(); break;                                  // M300: Play beep tone
+      #endif
+
+      #if ENABLED(PIDTEMP)
+        case 301: M301(); break;                                  // M301: Set hotend PID parameters
+      #endif
+
+      #if ENABLED(PIDTEMPBED)
+        case 304: M304(); break;                                  // M304: Set bed PID parameters
+      #endif
+
+      #if ENABLED(PHOTO_GCODE)
+        case 240: M240(); break;                                  // M240: Trigger a camera
+      #endif
+
+      #if HAS_LCD_CONTRAST
+        case 250: M250(); break;                                  // M250: Set LCD contrast
+      #endif
+
+      #if ENABLED(EXPERIMENTAL_I2CBUS)
+        case 260: M260(); break;                                  // M260: Send data to an i2c slave
+        case 261: M261(); break;                                  // M261: Request data from an i2c slave
+      #endif
+
+      #if ENABLED(PREVENT_COLD_EXTRUSION)
+        case 302: M302(); break;                                  // M302: Allow cold extrudes (set the minimum extrude temperature)
+      #endif
+
+      #if HAS_PID_HEATING
+        case 303: M303(); break;                                  // M303: PID autotune
+      #endif
+
+      #if HAS_USER_THERMISTORS
+        case 305: M305(); break;                                  // M305: Set user thermistor parameters
+      #endif
+
+      #if ENABLED(REPETIER_GCODE_M360)
+        case 360: M360(); break;                                  // M360: Firmware settings
+      #endif
+
+      #if ENABLED(MORGAN_SCARA)
+        case 360: if (M360()) return; break;                      // M360: SCARA Theta pos1
+        case 361: if (M361()) return; break;                      // M361: SCARA Theta pos2
+        case 362: if (M362()) return; break;                      // M362: SCARA Psi pos1
+        case 363: if (M363()) return; break;                      // M363: SCARA Psi pos2
+        case 364: if (M364()) return; break;                      // M364: SCARA Psi pos3 (90 deg to Theta)
+      #endif
+
+      #if EITHER(EXT_SOLENOID, MANUAL_SOLENOID_CONTROL)
+        case 380: M380(); break;                                  // M380: Activate solenoid on active (or specified) extruder
+        case 381: M381(); break;                                  // M381: Disable all solenoids or, if MANUAL_SOLENOID_CONTROL, active (or specified) solenoid
+      #endif
+
+      case 400: M400(); break;                                    // M400: Finish all moves
+
+      #if HAS_BED_PROBE
+        case 401: M401(); break;                                  // M401: Deploy probe
+        case 402: M402(); break;                                  // M402: Stow probe
+      #endif
+
+      #if HAS_PRUSA_MMU2
+        case 403: M403(); break;
+      #endif
+
+      #if ENABLED(FILAMENT_WIDTH_SENSOR)
+        case 404: M404(); break;                                  // M404: Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or display nominal filament width
+        case 405: M405(); break;                                  // M405: Turn on filament sensor for control
+        case 406: M406(); break;                                  // M406: Turn off filament sensor for control
+        case 407: M407(); break;                                  // M407: Display measured filament diameter
+      #endif
+
+      #if HAS_FILAMENT_SENSOR
+        case 412: M412(); break;                                  // M412: Enable/Disable filament runout detection
+      #endif
+
+      #if HAS_MULTI_LANGUAGE
+        case 414: M414(); break;                                  // M414: Select multi language menu
+      #endif
+
+      #if HAS_LEVELING
+        case 420: M420(); break;                                  // M420: Enable/Disable Bed Leveling
+      #endif
+
+      #if HAS_MESH
+        case 421: M421(); break;                                  // M421: Set a Mesh Bed Leveling Z coordinate
+      #endif
+
+      #if ENABLED(BACKLASH_GCODE)
+        case 425: M425(); break;                                  // M425: Tune backlash compensation
+      #endif
+
+      #if HAS_M206_COMMAND
+        case 428: M428(); break;                                  // M428: Apply current_position to home_offset
+      #endif
+
+      #if HAS_POWER_MONITOR
+        case 430: M430(); break;                                  // M430: Read the system current (A), voltage (V), and power (W)
+      #endif
+
+      #if ENABLED(CANCEL_OBJECTS)
+        case 486: M486(); break;                                  // M486: Identify and cancel objects
+      #endif
+
+      case 500: M500(); break;                                    // M500: Store settings in EEPROM
+      case 501: M501(); break;                                    // M501: Read settings from EEPROM
+      case 502: M502(); break;                                    // M502: Revert to default settings
+      #if DISABLED(DISABLE_M503)
+        case 503: M503(); break;                                  // M503: print settings currently in memory
+      #endif
+      #if ENABLED(EEPROM_SETTINGS)
+        case 504: M504(); break;                                  // M504: Validate EEPROM contents
+      #endif
+
+      #if ENABLED(PASSWORD_FEATURE)
+        case 510: M510(); break;                                  // M510: Lock Printer
+        #if ENABLED(PASSWORD_UNLOCK_GCODE)
+          case 511: M511(); break;                                // M511: Unlock Printer
+        #endif
+        #if ENABLED(PASSWORD_CHANGE_GCODE)
+          case 512: M512(); break;                                // M512: Set/Change/Remove Password
+        #endif
+      #endif
+
+      #if ENABLED(SDSUPPORT)
+        case 524: M524(); break;                                  // M524: Abort the current SD print job
+      #endif
+
+      #if ENABLED(SD_ABORT_ON_ENDSTOP_HIT)
+        case 540: M540(); break;                                  // M540: Set abort on endstop hit for SD printing
+      #endif
+
+      #if HAS_ETHERNET
+        case 552: M552(); break;                                  // M552: Set IP address
+        case 553: M553(); break;                                  // M553: Set gateway
+        case 554: M554(); break;                                  // M554: Set netmask
+      #endif
+
+      #if ENABLED(BAUD_RATE_GCODE)
+        case 575: M575(); break;                                  // M575: Set serial baudrate
+      #endif
+
+      #if ENABLED(ADVANCED_PAUSE_FEATURE)
+        case 600: M600(); break;                                  // M600: Pause for Filament Change
+        case 603: M603(); break;                                  // M603: Configure Filament Change
+      #endif
+
+      #if HAS_DUPLICATION_MODE
+        case 605: M605(); break;                                  // M605: Set Dual X Carriage movement mode
+      #endif
+
+      #if ENABLED(DELTA)
+        case 665: M665(); break;                                  // M665: Set delta configurations
+      #endif
+
+      #if ENABLED(DELTA) || HAS_EXTRA_ENDSTOPS
+        case 666: M666(); break;                                  // M666: Set delta or multiple endstop adjustment
+      #endif
+
+      #if ENABLED(DUET_SMART_EFFECTOR) && PIN_EXISTS(SMART_EFFECTOR_MOD)
+        case 672: M672(); break;                                  // M672: Set/clear Duet Smart Effector sensitivity
+      #endif
+
+      #if ENABLED(FILAMENT_LOAD_UNLOAD_GCODES)
+        case 701: M701(); break;                                  // M701: Load Filament
+        case 702: M702(); break;                                  // M702: Unload Filament
+      #endif
+
+      #if ENABLED(CONTROLLER_FAN_EDITABLE)
+        case 710: M710(); break;                                  // M710: Set Controller Fan settings
+      #endif
+
+      #if ENABLED(GCODE_MACROS)
+        case 810: case 811: case 812: case 813: case 814:
+        case 815: case 816: case 817: case 818: case 819:
+        M810_819(); break;                                        // M810-M819: Define/execute G-code macro
+      #endif
+
+      #if HAS_BED_PROBE
+        case 851: M851(); break;                                  // M851: Set Z Probe Z Offset
+      #endif
+
+      #if ENABLED(SKEW_CORRECTION_GCODE)
+        case 852: M852(); break;                                  // M852: Set Skew factors
+      #endif
+
+      #if ENABLED(PROBE_TEMP_COMPENSATION)
+        case 192: M192(); break;                                  // M192: Wait for probe temp
+        case 871: M871(); break;                                  // M871: Print/reset/clear first layer temperature offset values
+      #endif
+
+      #if ENABLED(LIN_ADVANCE)
+        case 900: M900(); break;                                  // M900: Set advance K factor.
+      #endif
+
+      #if ANY(HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_PWM, HAS_MOTOR_CURRENT_I2C, HAS_MOTOR_CURRENT_DAC)
+        case 907: M907(); break;                                  // M907: Set digital trimpot motor current using axis codes.
+        #if EITHER(HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_DAC)
+          case 908: M908(); break;                                // M908: Control digital trimpot directly.
+          #if ENABLED(HAS_MOTOR_CURRENT_DAC)
+            case 909: M909(); break;                              // M909: Print digipot/DAC current value
+            case 910: M910(); break;                              // M910: Commit digipot/DAC value to external EEPROM
+          #endif
+        #endif
+      #endif
+
+      #if HAS_TRINAMIC_CONFIG
+        case 122: M122(); break;                                  // M122: Report driver configuration and status
+        case 906: M906(); break;                                  // M906: Set motor current in milliamps using axis codes X, Y, Z, E
+        #if HAS_STEALTHCHOP
+          case 569: M569(); break;                                // M569: Enable stealthChop on an axis.
+        #endif
+        #if ENABLED(MONITOR_DRIVER_STATUS)
+          case 911: M911(); break;                                // M911: Report TMC2130 prewarn triggered flags
+          case 912: M912(); break;                                // M912: Clear TMC2130 prewarn triggered flags
+        #endif
+        #if ENABLED(HYBRID_THRESHOLD)
+          case 913: M913(); break;                                // M913: Set HYBRID_THRESHOLD speed.
+        #endif
+        #if USE_SENSORLESS
+          case 914: M914(); break;                                // M914: Set StallGuard sensitivity.
+        #endif
+      #endif
+
+      #if HAS_L64XX
+        case 122: M122(); break;                                   // M122: Report status
+        case 906: M906(); break;                                   // M906: Set or get motor drive level
+        case 916: M916(); break;                                   // M916: L6470 tuning: Increase drive level until thermal warning
+        case 917: M917(); break;                                   // M917: L6470 tuning: Find minimum current thresholds
+        case 918: M918(); break;                                   // M918: L6470 tuning: Increase speed until max or error
+      #endif
+
+      #if HAS_MICROSTEPS
+        case 350: M350(); break;                                  // M350: Set microstepping mode. Warning: Steps per unit remains unchanged. S code sets stepping mode for all drivers.
+        case 351: M351(); break;                                  // M351: Toggle MS1 MS2 pins directly, S# determines MS1 or MS2, X# sets the pin high/low.
+      #endif
+
+      #if ENABLED(CASE_LIGHT_ENABLE)
+        case 355: M355(); break;                                  // M355: Set case light brightness
+      #endif
+
+      #if ENABLED(DEBUG_GCODE_PARSER)
+        case 800: parser.debug(); break;                          // M800: GCode Parser Test for M
+      #endif
+
+      #if ENABLED(GCODE_REPEAT_MARKERS)
+        case 808: M808(); break;                                  // M808: Set / Goto repeat markers
+      #endif
+
+      #if ENABLED(I2C_POSITION_ENCODERS)
+        case 860: M860(); break;                                  // M860: Report encoder module position
+        case 861: M861(); break;                                  // M861: Report encoder module status
+        case 862: M862(); break;                                  // M862: Perform axis test
+        case 863: M863(); break;                                  // M863: Calibrate steps/mm
+        case 864: M864(); break;                                  // M864: Change module address
+        case 865: M865(); break;                                  // M865: Check module firmware version
+        case 866: M866(); break;                                  // M866: Report axis error count
+        case 867: M867(); break;                                  // M867: Toggle error correction
+        case 868: M868(); break;                                  // M868: Set error correction threshold
+        case 869: M869(); break;                                  // M869: Report axis error
+      #endif
+
+      #if ENABLED(MAGNETIC_PARKING_EXTRUDER)
+        case 951: M951(); break;                                  // M951: Set Magnetic Parking Extruder parameters
+      #endif
+
+      #if ENABLED(Z_STEPPER_AUTO_ALIGN)
+        case 422: M422(); break;                                  // M422: Set Z Stepper automatic alignment position using probe
+      #endif
+
+      #if ALL(HAS_SPI_FLASH, SDSUPPORT, MARLIN_DEV_MODE)
+        case 993: M993(); break;                                  // M993: Backup SPI Flash to SD
+        case 994: M994(); break;                                  // M994: Load a Backup from SD to SPI Flash
+      #endif
+
+      #if ENABLED(TOUCH_SCREEN_CALIBRATION)
+        case 995: M995(); break;                                  // M995: Touch screen calibration for TFT display
+      #endif
+
+      #if ENABLED(PLATFORM_M997_SUPPORT)
+        case 997: M997(); break;                                  // M997: Perform in-application firmware update
+      #endif
+
+      case 999: M999(); break;                                    // M999: Restart after being Stopped
+
+      #if ENABLED(POWER_LOSS_RECOVERY)
+        case 413: M413(); break;                                  // M413: Enable/disable/query Power-Loss Recovery
+        case 1000: M1000(); break;                                // M1000: [INTERNAL] Resume from power-loss
+      #endif
+
+      #if ENABLED(SDSUPPORT)
+        case 1001: M1001(); break;                                // M1001: [INTERNAL] Handle SD completion
+      #endif
+
+      #if ENABLED(MAX7219_GCODE)
+        case 7219: M7219(); break;                                // M7219: Set LEDs, columns, and rows
+      #endif
+
+      default: parser.unknown_command_warning(); break;
+    }
+    break;
+
+    case 'T': T(parser.codenum); break;                           // Tn: Tool Change
+
+    #if ENABLED(MARLIN_DEV_MODE)
+      case 'D': D(parser.codenum); break;                         // Dn: Debug codes
+    #endif
+
+    default:
+      #if ENABLED(WIFI_CUSTOM_COMMAND)
+        if (wifi_custom_command(parser.command_ptr)) break;
+      #endif
+      parser.unknown_command_warning();
+  }
+
+  if (!no_ok) queue.ok_to_send();
+
+  SERIAL_OUT(msgDone); // Call the msgDone serial hook to signal command processing done
+}
+
+/**
+ * Process a single command and dispatch it to its handler
+ * This is called from the main loop()
+ */
+void GcodeSuite::process_next_command() {
+  char * const current_command = queue.command_buffer[queue.index_r];
+
+  PORT_REDIRECT(SERIAL_PORTMASK(queue.port[queue.index_r]));
+
+  #if ENABLED(POWER_LOSS_RECOVERY)
+    recovery.queue_index_r = queue.index_r;
+  #endif
+
+  if (DEBUGGING(ECHO)) {
+    SERIAL_ECHO_START();
+    SERIAL_ECHOLN(current_command);
+    #if ENABLED(M100_FREE_MEMORY_DUMPER)
+      SERIAL_ECHOPAIR("slot:", queue.index_r);
+      M100_dump_routine(PSTR("   Command Queue:"), &queue.command_buffer[0][0], &queue.command_buffer[BUFSIZE - 1][MAX_CMD_SIZE - 1]);
+    #endif
+  }
+
+  // Parse the next command in the queue
+  parser.parse(current_command);
+  process_parsed_command();
+}
+
+/**
+ * Run a series of commands, bypassing the command queue to allow
+ * G-code "macros" to be called from within other G-code handlers.
+ */
+
+void GcodeSuite::process_subcommands_now_P(PGM_P pgcode) {
+  char * const saved_cmd = parser.command_ptr;        // Save the parser state
+  for (;;) {
+    PGM_P const delim = strchr_P(pgcode, '\n');       // Get address of next newline
+    const size_t len = delim ? delim - pgcode : strlen_P(pgcode); // Get the command length
+    char cmd[len + 1];                                // Allocate a stack buffer
+    strncpy_P(cmd, pgcode, len);                      // Copy the command to the stack
+    cmd[len] = '\0';                                  // End with a nul
+    parser.parse(cmd);                                // Parse the command
+    process_parsed_command(true);                     // Process it
+    if (!delim) break;                                // Last command?
+    pgcode = delim + 1;                               // Get the next command
+  }
+  parser.parse(saved_cmd);                            // Restore the parser state
+}
+
+void GcodeSuite::process_subcommands_now(char * gcode) {
+  char * const saved_cmd = parser.command_ptr;        // Save the parser state
+  for (;;) {
+    char * const delim = strchr(gcode, '\n');         // Get address of next newline
+    if (delim) *delim = '\0';                         // Replace with nul
+    parser.parse(gcode);                              // Parse the current command
+    if (delim) *delim = '\n';                         // Put back the newline
+    process_parsed_command(true);                     // Process it
+    if (!delim) break;                                // Last command?
+    gcode = delim + 1;                                // Get the next command
+  }
+  parser.parse(saved_cmd);                            // Restore the parser state
+}
+
+#if ENABLED(HOST_KEEPALIVE_FEATURE)
+
+  /**
+   * Output a "busy" message at regular intervals
+   * while the machine is not accepting commands.
+   */
+  void GcodeSuite::host_keepalive() {
+    const millis_t ms = millis();
+    static millis_t next_busy_signal_ms = 0;
+    if (!autoreport_paused && host_keepalive_interval && busy_state != NOT_BUSY) {
+      if (PENDING(ms, next_busy_signal_ms)) return;
+      switch (busy_state) {
+        case IN_HANDLER:
+        case IN_PROCESS:
+          SERIAL_ECHO_MSG(STR_BUSY_PROCESSING);
+          break;
+        case PAUSED_FOR_USER:
+          SERIAL_ECHO_MSG(STR_BUSY_PAUSED_FOR_USER);
+          break;
+        case PAUSED_FOR_INPUT:
+          SERIAL_ECHO_MSG(STR_BUSY_PAUSED_FOR_INPUT);
+          break;
+        default:
+          break;
+      }
+    }
+    next_busy_signal_ms = ms + SEC_TO_MS(host_keepalive_interval);
+  }
+
+#endif // HOST_KEEPALIVE_FEATURE
diff --git a/Marlin/src/gcode/gcode.h b/Marlin/src/gcode/gcode.h
new file mode 100644
index 0000000..7fd8d69
--- /dev/null
+++ b/Marlin/src/gcode/gcode.h
@@ -0,0 +1,906 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * gcode.h - Temporary container for all gcode handlers
+ */
+
+/**
+ * -----------------
+ * G-Codes in Marlin
+ * -----------------
+ *
+ * Helpful G-code references:
+ *  - https://marlinfw.org/meta/gcode
+ *  - https://reprap.org/wiki/G-code
+ *  - https://linuxcnc.org/docs/html/gcode.html
+ *
+ * Help to document Marlin's G-codes online:
+ *  - https://github.com/MarlinFirmware/MarlinDocumentation
+ *
+ * -----------------
+ *
+ * "G" Codes
+ *
+ * G0   -> G1
+ * G1   - Coordinated Movement X Y Z E
+ * G2   - CW ARC
+ * G3   - CCW ARC
+ * G4   - Dwell S<seconds> or P<milliseconds>
+ * G5   - Cubic B-spline with XYZE destination and IJPQ offsets
+ * G10  - Retract filament according to settings of M207 (Requires FWRETRACT)
+ * G11  - Retract recover filament according to settings of M208 (Requires FWRETRACT)
+ * G12  - Clean tool (Requires NOZZLE_CLEAN_FEATURE)
+ * G17  - Select Plane XY (Requires CNC_WORKSPACE_PLANES)
+ * G18  - Select Plane ZX (Requires CNC_WORKSPACE_PLANES)
+ * G19  - Select Plane YZ (Requires CNC_WORKSPACE_PLANES)
+ * G20  - Set input units to inches (Requires INCH_MODE_SUPPORT)
+ * G21  - Set input units to millimeters (Requires INCH_MODE_SUPPORT)
+ * G26  - Mesh Validation Pattern (Requires G26_MESH_VALIDATION)
+ * G27  - Park Nozzle (Requires NOZZLE_PARK_FEATURE)
+ * G28  - Home one or more axes
+ * G29  - Start or continue the bed leveling probe procedure (Requires bed leveling)
+ * G30  - Single Z probe, probes bed at X Y location (defaults to current XY location)
+ * G31  - Dock sled (Z_PROBE_SLED only)
+ * G32  - Undock sled (Z_PROBE_SLED only)
+ * G33  - Delta Auto-Calibration (Requires DELTA_AUTO_CALIBRATION)
+ * G34  - Z Stepper automatic alignment using probe: I<iterations> T<accuracy> A<amplification> (Requires Z_STEPPER_AUTO_ALIGN)
+ * G35  - Read bed corners to help adjust bed screws: T<screw_thread> (Requires ASSISTED_TRAMMING)
+ * G38  - Probe in any direction using the Z_MIN_PROBE (Requires G38_PROBE_TARGET)
+ * G42  - Coordinated move to a mesh point (Requires MESH_BED_LEVELING, AUTO_BED_LEVELING_BLINEAR, or AUTO_BED_LEVELING_UBL)
+ * G60  - Save current position. (Requires SAVED_POSITIONS)
+ * G61  - Apply/restore saved coordinates. (Requires SAVED_POSITIONS)
+ * G76  - Calibrate first layer temperature offsets. (Requires PROBE_TEMP_COMPENSATION)
+ * G80  - Cancel current motion mode (Requires GCODE_MOTION_MODES)
+ * G90  - Use Absolute Coordinates
+ * G91  - Use Relative Coordinates
+ * G92  - Set current position to coordinates given
+ *
+ * "M" Codes
+ *
+ * M0   - Unconditional stop - Wait for user to press a button on the LCD (Only if ULTRA_LCD is enabled)
+ * M1   -> M0
+ * M3   - Turn ON Laser | Spindle (clockwise), set Power | Speed. (Requires SPINDLE_FEATURE or LASER_FEATURE)
+ * M4   - Turn ON Laser | Spindle (counter-clockwise), set Power | Speed. (Requires SPINDLE_FEATURE or LASER_FEATURE)
+ * M5   - Turn OFF Laser | Spindle. (Requires SPINDLE_FEATURE or LASER_FEATURE)
+ * M7   - Turn mist coolant ON. (Requires COOLANT_CONTROL)
+ * M8   - Turn flood coolant ON. (Requires COOLANT_CONTROL)
+ * M9   - Turn coolant OFF. (Requires COOLANT_CONTROL)
+ * M12  - Set up closed loop control system. (Requires EXTERNAL_CLOSED_LOOP_CONTROLLER)
+ * M16  - Expected printer check. (Requires EXPECTED_PRINTER_CHECK)
+ * M17  - Enable/Power all stepper motors
+ * M18  - Disable all stepper motors; same as M84
+ * M20  - List SD card. (Requires SDSUPPORT)
+ * M21  - Init SD card. (Requires SDSUPPORT)
+ * M22  - Release SD card. (Requires SDSUPPORT)
+ * M23  - Select SD file: "M23 /path/file.gco". (Requires SDSUPPORT)
+ * M24  - Start/resume SD print. (Requires SDSUPPORT)
+ * M25  - Pause SD print. (Requires SDSUPPORT)
+ * M26  - Set SD position in bytes: "M26 S12345". (Requires SDSUPPORT)
+ * M27  - Report SD print status. (Requires SDSUPPORT)
+ *        OR, with 'S<seconds>' set the SD status auto-report interval. (Requires AUTO_REPORT_SD_STATUS)
+ *        OR, with 'C' get the current filename.
+ * M28  - Start SD write: "M28 /path/file.gco". (Requires SDSUPPORT)
+ * M29  - Stop SD write. (Requires SDSUPPORT)
+ * M30  - Delete file from SD: "M30 /path/file.gco"
+ * M31  - Report time since last M109 or SD card start to serial.
+ * M32  - Select file and start SD print: "M32 [S<bytepos>] !/path/file.gco#". (Requires SDSUPPORT)
+ *        Use P to run other files as sub-programs: "M32 P !filename#"
+ *        The '#' is necessary when calling from within sd files, as it stops buffer prereading
+ * M33  - Get the longname version of a path. (Requires LONG_FILENAME_HOST_SUPPORT)
+ * M34  - Set SD Card sorting options. (Requires SDCARD_SORT_ALPHA)
+ * M42  - Change pin status via gcode: M42 P<pin> S<value>. LED pin assumed if P is omitted. (Requires DIRECT_PIN_CONTROL)
+ * M43  - Display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
+ * M48  - Measure Z Probe repeatability: M48 P<points> X<pos> Y<pos> V<level> E<engage> L<legs> S<chizoid>. (Requires Z_MIN_PROBE_REPEATABILITY_TEST)
+ * M73  - Set the progress percentage. (Requires LCD_SET_PROGRESS_MANUALLY)
+ * M75  - Start the print job timer.
+ * M76  - Pause the print job timer.
+ * M77  - Stop the print job timer.
+ * M78  - Show statistical information about the print jobs. (Requires PRINTCOUNTER)
+ * M80  - Turn on Power Supply. (Requires PSU_CONTROL)
+ * M81  - Turn off Power Supply. (Requires PSU_CONTROL)
+ * M82  - Set E codes absolute (default).
+ * M83  - Set E codes relative while in Absolute (G90) mode.
+ * M84  - Disable steppers until next move, or use S<seconds> to specify an idle
+ *        duration after which steppers should turn off. S0 disables the timeout.
+ * M85  - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
+ * M92  - Set planner.settings.axis_steps_per_mm for one or more axes.
+ * M100 - Watch Free Memory (for debugging) (Requires M100_FREE_MEMORY_WATCHER)
+ * M104 - Set extruder target temp.
+ * M105 - Report current temperatures.
+ * M106 - Set print fan speed.
+ * M107 - Print fan off.
+ * M108 - Break out of heating loops (M109, M190, M303). With no controller, breaks out of M0/M1. (Requires EMERGENCY_PARSER)
+ * M109 - S<temp> Wait for extruder current temp to reach target temp. ** Wait only when heating! **
+ *        R<temp> Wait for extruder current temp to reach target temp. ** Wait for heating or cooling. **
+ *        If AUTOTEMP is enabled, S<mintemp> B<maxtemp> F<factor>. Exit autotemp by any M109 without F
+ * M110 - Set the current line number. (Used by host printing)
+ * M111 - Set debug flags: "M111 S<flagbits>". See flag bits defined in enum.h.
+ * M112 - Full Shutdown.
+ * M113 - Get or set the timeout interval for Host Keepalive "busy" messages. (Requires HOST_KEEPALIVE_FEATURE)
+ * M114 - Report current position.
+ * M115 - Report capabilities. (Extended capabilities requires EXTENDED_CAPABILITIES_REPORT)
+ * M117 - Display a message on the controller screen. (Requires an LCD)
+ * M118 - Display a message in the host console.
+ * M119 - Report endstops status.
+ * M120 - Enable endstops detection.
+ * M121 - Disable endstops detection.
+ * M122 - Debug stepper (Requires at least one _DRIVER_TYPE defined as TMC2130/2160/5130/5160/2208/2209/2660 or L6470)
+ * M125 - Save current position and move to filament change position. (Requires PARK_HEAD_ON_PAUSE)
+ * M126 - Solenoid Air Valve Open. (Requires BARICUDA)
+ * M127 - Solenoid Air Valve Closed. (Requires BARICUDA)
+ * M128 - EtoP Open. (Requires BARICUDA)
+ * M129 - EtoP Closed. (Requires BARICUDA)
+ * M140 - Set bed target temp. S<temp>
+ * M141 - Set heated chamber target temp. S<temp> (Requires a chamber heater)
+ * M145 - Set heatup values for materials on the LCD. H<hotend> B<bed> F<fan speed> for S<material> (0=PLA, 1=ABS)
+ * M149 - Set temperature units. (Requires TEMPERATURE_UNITS_SUPPORT)
+ * M150 - Set Status LED Color as R<red> U<green> B<blue> W<white> P<bright>. Values 0-255. (Requires BLINKM, RGB_LED, RGBW_LED, NEOPIXEL_LED, PCA9533, or PCA9632).
+ * M155 - Auto-report temperatures with interval of S<seconds>. (Requires AUTO_REPORT_TEMPERATURES)
+ * M163 - Set a single proportion for a mixing extruder. (Requires MIXING_EXTRUDER)
+ * M164 - Commit the mix and save to a virtual tool (current, or as specified by 'S'). (Requires MIXING_EXTRUDER)
+ * M165 - Set the mix for the mixing extruder (and current virtual tool) with parameters ABCDHI. (Requires MIXING_EXTRUDER and DIRECT_MIXING_IN_G1)
+ * M166 - Set the Gradient Mix for the mixing extruder. (Requires GRADIENT_MIX)
+ * M190 - S<temp> Wait for bed current temp to reach target temp. ** Wait only when heating! **
+ *        R<temp> Wait for bed current temp to reach target temp. ** Wait for heating or cooling. **
+ * M200 - Set filament diameter, D<diameter>, setting E axis units to cubic. (Use S0 to revert to linear units.)
+ * M201 - Set max acceleration in units/s^2 for print moves: "M201 X<accel> Y<accel> Z<accel> E<accel>"
+ * M202 - Set max acceleration in units/s^2 for travel moves: "M202 X<accel> Y<accel> Z<accel> E<accel>" ** UNUSED IN MARLIN! **
+ * M203 - Set maximum feedrate: "M203 X<fr> Y<fr> Z<fr> E<fr>" in units/sec.
+ * M204 - Set default acceleration in units/sec^2: P<printing> R<extruder_only> T<travel>
+ * M205 - Set advanced settings. Current units apply:
+            S<print> T<travel> minimum speeds
+            B<minimum segment time>
+            X<max X jerk>, Y<max Y jerk>, Z<max Z jerk>, E<max E jerk>
+ * M206 - Set additional homing offset. (Disabled by NO_WORKSPACE_OFFSETS or DELTA)
+ * M207 - Set Retract Length: S<length>, Feedrate: F<units/min>, and Z lift: Z<distance>. (Requires FWRETRACT)
+ * M208 - Set Recover (unretract) Additional (!) Length: S<length> and Feedrate: F<units/min>. (Requires FWRETRACT)
+ * M209 - Turn Automatic Retract Detection on/off: S<0|1> (For slicers that don't support G10/11). (Requires FWRETRACT_AUTORETRACT)
+          Every normal extrude-only move will be classified as retract depending on the direction.
+ * M211 - Enable, Disable, and/or Report software endstops: S<0|1> (Requires MIN_SOFTWARE_ENDSTOPS or MAX_SOFTWARE_ENDSTOPS)
+ * M217 - Set filament swap parameters: "M217 S<length> P<feedrate> R<feedrate>". (Requires SINGLENOZZLE)
+ * M218 - Set/get a tool offset: "M218 T<index> X<offset> Y<offset>". (Requires 2 or more extruders)
+ * M220 - Set Feedrate Percentage: "M220 S<percent>" (i.e., "FR" on the LCD)
+ *        Use "M220 B" to back up the Feedrate Percentage and "M220 R" to restore it. (Requires an MMU_MODEL version 2 or 2S)
+ * M221 - Set Flow Percentage: "M221 S<percent>"
+ * M226 - Wait until a pin is in a given state: "M226 P<pin> S<state>" (Requires DIRECT_PIN_CONTROL)
+ * M240 - Trigger a camera to take a photograph. (Requires PHOTO_GCODE)
+ * M250 - Set LCD contrast: "M250 C<contrast>" (0-63). (Requires LCD support)
+ * M260 - i2c Send Data (Requires EXPERIMENTAL_I2CBUS)
+ * M261 - i2c Request Data (Requires EXPERIMENTAL_I2CBUS)
+ * M280 - Set servo position absolute: "M280 P<index> S<angle|µs>". (Requires servos)
+ * M281 - Set servo min|max position: "M281 P<index> L<min> U<max>". (Requires EDITABLE_SERVO_ANGLES)
+ * M290 - Babystepping (Requires BABYSTEPPING)
+ * M300 - Play beep sound S<frequency Hz> P<duration ms>
+ * M301 - Set PID parameters P I and D. (Requires PIDTEMP)
+ * M302 - Allow cold extrudes, or set the minimum extrude S<temperature>. (Requires PREVENT_COLD_EXTRUSION)
+ * M303 - PID relay autotune S<temperature> sets the target temperature. Default 150C. (Requires PIDTEMP)
+ * M304 - Set bed PID parameters P I and D. (Requires PIDTEMPBED)
+ * M305 - Set user thermistor parameters R T and P. (Requires TEMP_SENSOR_x 1000)
+ * M350 - Set microstepping mode. (Requires digital microstepping pins.)
+ * M351 - Toggle MS1 MS2 pins directly. (Requires digital microstepping pins.)
+ * M355 - Set Case Light on/off and set brightness. (Requires CASE_LIGHT_PIN)
+ * M380 - Activate solenoid on active extruder. (Requires EXT_SOLENOID)
+ * M381 - Disable all solenoids. (Requires EXT_SOLENOID)
+ * M400 - Finish all moves.
+ * M401 - Deploy and activate Z probe. (Requires a probe)
+ * M402 - Deactivate and stow Z probe. (Requires a probe)
+ * M403 - Set filament type for PRUSA MMU2
+ * M404 - Display or set the Nominal Filament Width: "W<diameter>". (Requires FILAMENT_WIDTH_SENSOR)
+ * M405 - Enable Filament Sensor flow control. "M405 D<delay_cm>". (Requires FILAMENT_WIDTH_SENSOR)
+ * M406 - Disable Filament Sensor flow control. (Requires FILAMENT_WIDTH_SENSOR)
+ * M407 - Display measured filament diameter in millimeters. (Requires FILAMENT_WIDTH_SENSOR)
+ * M410 - Quickstop. Abort all planned moves.
+ * M412 - Enable / Disable Filament Runout Detection. (Requires FILAMENT_RUNOUT_SENSOR)
+ * M413 - Enable / Disable Power-Loss Recovery. (Requires POWER_LOSS_RECOVERY)
+ * M414 - Set language by index. (Requires LCD_LANGUAGE_2...)
+ * M420 - Enable/Disable Leveling (with current values) S1=enable S0=disable (Requires MESH_BED_LEVELING or ABL)
+ * M421 - Set a single Z coordinate in the Mesh Leveling grid. X<units> Y<units> Z<units> (Requires MESH_BED_LEVELING, AUTO_BED_LEVELING_BILINEAR, or AUTO_BED_LEVELING_UBL)
+ * M422 - Set Z Stepper automatic alignment position using probe. X<units> Y<units> A<axis> (Requires Z_STEPPER_AUTO_ALIGN)
+ * M425 - Enable/Disable and tune backlash correction. (Requires BACKLASH_COMPENSATION and BACKLASH_GCODE)
+ * M428 - Set the home_offset based on the current_position. Nearest edge applies. (Disabled by NO_WORKSPACE_OFFSETS or DELTA)
+ * M430 - Read the system current, voltage, and power (Requires POWER_MONITOR_CURRENT, POWER_MONITOR_VOLTAGE, or POWER_MONITOR_FIXED_VOLTAGE)
+ * M486 - Identify and cancel objects. (Requires CANCEL_OBJECTS)
+ * M500 - Store parameters in EEPROM. (Requires EEPROM_SETTINGS)
+ * M501 - Restore parameters from EEPROM. (Requires EEPROM_SETTINGS)
+ * M502 - Revert to the default "factory settings". ** Does not write them to EEPROM! **
+ * M503 - Print the current settings (in memory): "M503 S<verbose>". S0 specifies compact output.
+ * M504 - Validate EEPROM contents. (Requires EEPROM_SETTINGS)
+ * M510 - Lock Printer
+ * M511 - Unlock Printer
+ * M512 - Set/Change/Remove Password
+ * M524 - Abort the current SD print job started with M24. (Requires SDSUPPORT)
+ * M540 - Enable/disable SD card abort on endstop hit: "M540 S<state>". (Requires SD_ABORT_ON_ENDSTOP_HIT)
+ * M552 - Get or set IP address. Enable/disable network interface. (Requires enabled Ethernet port)
+ * M553 - Get or set IP netmask. (Requires enabled Ethernet port)
+ * M554 - Get or set IP gateway. (Requires enabled Ethernet port)
+ * M569 - Enable stealthChop on an axis. (Requires at least one _DRIVER_TYPE to be TMC2130/2160/2208/2209/5130/5160)
+ * M600 - Pause for filament change: "M600 X<pos> Y<pos> Z<raise> E<first_retract> L<later_retract>". (Requires ADVANCED_PAUSE_FEATURE)
+ * M603 - Configure filament change: "M603 T<tool> U<unload_length> L<load_length>". (Requires ADVANCED_PAUSE_FEATURE)
+ * M605 - Set Dual X-Carriage movement mode: "M605 S<mode> [X<x_offset>] [R<temp_offset>]". (Requires DUAL_X_CARRIAGE)
+ * M665 - Set delta configurations: "M665 H<delta height> L<diagonal rod> R<delta radius> S<segments/s> B<calibration radius> X<Alpha angle trim> Y<Beta angle trim> Z<Gamma angle trim> (Requires DELTA)
+ * M666 - Set/get offsets for delta (Requires DELTA) or dual endstops. (Requires [XYZ]_DUAL_ENDSTOPS)
+ * M672 - Set/Reset Duet Smart Effector's sensitivity. (Requires DUET_SMART_EFFECTOR and SMART_EFFECTOR_MOD_PIN)
+ * M701 - Load filament (Requires FILAMENT_LOAD_UNLOAD_GCODES)
+ * M702 - Unload filament (Requires FILAMENT_LOAD_UNLOAD_GCODES)
+ * M808 - Set or Goto a Repeat Marker (Requires GCODE_REPEAT_MARKERS)
+ * M810-M819 - Define/execute a G-code macro (Requires GCODE_MACROS)
+ * M851 - Set Z probe's XYZ offsets in current units. (Negative values: X=left, Y=front, Z=below)
+ * M852 - Set skew factors: "M852 [I<xy>] [J<xz>] [K<yz>]". (Requires SKEW_CORRECTION_GCODE, and SKEW_CORRECTION_FOR_Z for IJ)
+ * M860 - Report the position of position encoder modules.
+ * M861 - Report the status of position encoder modules.
+ * M862 - Perform an axis continuity test for position encoder modules.
+ * M863 - Perform steps-per-mm calibration for position encoder modules.
+ * M864 - Change position encoder module I2C address.
+ * M865 - Check position encoder module firmware version.
+ * M866 - Report or reset position encoder module error count.
+ * M867 - Enable/disable or toggle error correction for position encoder modules.
+ * M868 - Report or set position encoder module error correction threshold.
+ * M869 - Report position encoder module error.
+ * M871 - Print/reset/clear first layer temperature offset values. (Requires PROBE_TEMP_COMPENSATION)
+ * M192 - Wait for probe temp (Requires PROBE_TEMP_COMPENSATION)
+ * M876 - Handle Prompt Response. (Requires HOST_PROMPT_SUPPORT and not EMERGENCY_PARSER)
+ * M900 - Get or Set Linear Advance K-factor. (Requires LIN_ADVANCE)
+ * M906 - Set or get motor current in milliamps using axis codes X, Y, Z, E. Report values if no axis codes given. (Requires at least one _DRIVER_TYPE defined as TMC2130/2160/5130/5160/2208/2209/2660 or L6470)
+ * M907 - Set digital trimpot motor current using axis codes. (Requires a board with digital trimpots)
+ * M908 - Control digital trimpot directly. (Requires HAS_MOTOR_CURRENT_DAC or DIGIPOTSS_PIN)
+ * M909 - Print digipot/DAC current value. (Requires HAS_MOTOR_CURRENT_DAC)
+ * M910 - Commit digipot/DAC value to external EEPROM via I2C. (Requires HAS_MOTOR_CURRENT_DAC)
+ * M911 - Report stepper driver overtemperature pre-warn condition. (Requires at least one _DRIVER_TYPE defined as TMC2130/2160/5130/5160/2208/2209/2660)
+ * M912 - Clear stepper driver overtemperature pre-warn condition flag. (Requires at least one _DRIVER_TYPE defined as TMC2130/2160/5130/5160/2208/2209/2660)
+ * M913 - Set HYBRID_THRESHOLD speed. (Requires HYBRID_THRESHOLD)
+ * M914 - Set StallGuard sensitivity. (Requires SENSORLESS_HOMING or SENSORLESS_PROBING)
+ * M916 - L6470 tuning: Increase KVAL_HOLD until thermal warning. (Requires at least one _DRIVER_TYPE L6470)
+ * M917 - L6470 tuning: Find minimum current thresholds. (Requires at least one _DRIVER_TYPE L6470)
+ * M918 - L6470 tuning: Increase speed until max or error. (Requires at least one _DRIVER_TYPE L6470)
+ * M951 - Set Magnetic Parking Extruder parameters. (Requires MAGNETIC_PARKING_EXTRUDER)
+ * M7219 - Control Max7219 Matrix LEDs. (Requires MAX7219_GCODE)
+ *
+ * M360 - SCARA calibration: Move to cal-position ThetaA (0 deg calibration)
+ * M361 - SCARA calibration: Move to cal-position ThetaB (90 deg calibration - steps per degree)
+ * M362 - SCARA calibration: Move to cal-position PsiA (0 deg calibration)
+ * M363 - SCARA calibration: Move to cal-position PsiB (90 deg calibration - steps per degree)
+ * M364 - SCARA calibration: Move to cal-position PSIC (90 deg to Theta calibration position)
+ *
+ * ************ Custom codes - This can change to suit future G-code regulations
+ * G425 - Calibrate using a conductive object. (Requires CALIBRATION_GCODE)
+ * M928 - Start SD logging: "M928 filename.gco". Stop with M29. (Requires SDSUPPORT)
+ * M993 - Backup SPI Flash to SD
+ * M994 - Load a Backup from SD to SPI Flash
+ * M995 - Touch screen calibration for TFT display
+ * M997 - Perform in-application firmware update
+ * M999 - Restart after being stopped by error
+ * D... - Custom Development G-code. Add hooks to 'gcode_D.cpp' for developers to test features. (Requires MARLIN_DEV_MODE)
+ *
+ * "T" Codes
+ *
+ * T0-T3 - Select an extruder (tool) by index: "T<n> F<units/min>"
+ */
+
+#include "../inc/MarlinConfig.h"
+#include "parser.h"
+
+#if ENABLED(I2C_POSITION_ENCODERS)
+  #include "../feature/encoder_i2c.h"
+#endif
+
+#if IS_SCARA || defined(G0_FEEDRATE)
+  #define HAS_FAST_MOVES 1
+#endif
+
+enum AxisRelative : uint8_t { REL_X, REL_Y, REL_Z, REL_E, E_MODE_ABS, E_MODE_REL };
+
+extern const char G28_STR[];
+
+class GcodeSuite {
+public:
+
+  static uint8_t axis_relative;
+
+  static inline bool axis_is_relative(const AxisEnum a) {
+    if (a == E_AXIS) {
+      if (TEST(axis_relative, E_MODE_REL)) return true;
+      if (TEST(axis_relative, E_MODE_ABS)) return false;
+    }
+    return TEST(axis_relative, a);
+  }
+  static inline void set_relative_mode(const bool rel) {
+    axis_relative = rel ? _BV(REL_X) | _BV(REL_Y) | _BV(REL_Z) | _BV(REL_E) : 0;
+  }
+  static inline void set_e_relative() {
+    CBI(axis_relative, E_MODE_ABS);
+    SBI(axis_relative, E_MODE_REL);
+  }
+  static inline void set_e_absolute() {
+    CBI(axis_relative, E_MODE_REL);
+    SBI(axis_relative, E_MODE_ABS);
+  }
+
+  #if ENABLED(CNC_WORKSPACE_PLANES)
+    /**
+     * Workspace planes only apply to G2/G3 moves
+     * (and "canned cycles" - not a current feature)
+     */
+    enum WorkspacePlane : char { PLANE_XY, PLANE_ZX, PLANE_YZ };
+    static WorkspacePlane workspace_plane;
+  #endif
+
+  #define MAX_COORDINATE_SYSTEMS 9
+  #if ENABLED(CNC_COORDINATE_SYSTEMS)
+    static int8_t active_coordinate_system;
+    static xyz_pos_t coordinate_system[MAX_COORDINATE_SYSTEMS];
+    static bool select_coordinate_system(const int8_t _new);
+  #endif
+
+  static millis_t previous_move_ms, max_inactive_time, stepper_inactive_time;
+  FORCE_INLINE static void reset_stepper_timeout(const millis_t ms=millis()) { previous_move_ms = ms; }
+  FORCE_INLINE static bool stepper_max_timed_out(const millis_t ms=millis()) {
+    return max_inactive_time && ELAPSED(ms, previous_move_ms + max_inactive_time);
+  }
+  FORCE_INLINE static bool stepper_inactive_timeout(const millis_t ms=millis()) {
+    return ELAPSED(ms, previous_move_ms + stepper_inactive_time);
+  }
+
+  static int8_t get_target_extruder_from_command();
+  static int8_t get_target_e_stepper_from_command();
+  static void get_destination_from_command();
+
+  static void process_parsed_command(const bool no_ok=false);
+  static void process_next_command();
+
+  // Execute G-code in-place, preserving current G-code parameters
+  static void process_subcommands_now_P(PGM_P pgcode);
+  static void process_subcommands_now(char * gcode);
+
+  static inline void home_all_axes(const bool keep_leveling=false) {
+    process_subcommands_now_P(keep_leveling ? G28_STR : TERN(G28_L0_ENSURES_LEVELING_OFF, PSTR("G28L0"), G28_STR));
+  }
+
+  #if EITHER(HAS_AUTO_REPORTING, HOST_KEEPALIVE_FEATURE)
+    static bool autoreport_paused;
+    static inline bool set_autoreport_paused(const bool p) {
+      const bool was = autoreport_paused;
+      autoreport_paused = p;
+      return was;
+    }
+  #else
+    static constexpr bool autoreport_paused = false;
+    static inline bool set_autoreport_paused(const bool) { return false; }
+  #endif
+
+  #if ENABLED(HOST_KEEPALIVE_FEATURE)
+    /**
+     * States for managing Marlin and host communication
+     * Marlin sends messages if blocked or busy
+     */
+    enum MarlinBusyState : char {
+      NOT_BUSY,           // Not in a handler
+      IN_HANDLER,         // Processing a GCode
+      IN_PROCESS,         // Known to be blocking command input (as in G29)
+      PAUSED_FOR_USER,    // Blocking pending any input
+      PAUSED_FOR_INPUT    // Blocking pending text input (concept)
+    };
+
+    static MarlinBusyState busy_state;
+    static uint8_t host_keepalive_interval;
+
+    static void host_keepalive();
+
+    #define KEEPALIVE_STATE(N) REMEMBER(_KA_, gcode.busy_state, gcode.N)
+  #else
+    #define KEEPALIVE_STATE(N) NOOP
+  #endif
+
+  static void dwell(millis_t time);
+
+private:
+
+  TERN_(MARLIN_DEV_MODE, static void D(const int16_t dcode));
+
+  static void G0_G1(TERN_(HAS_FAST_MOVES, const bool fast_move=false));
+
+  TERN_(ARC_SUPPORT, static void G2_G3(const bool clockwise));
+
+  static void G4();
+
+  TERN_(BEZIER_CURVE_SUPPORT, static void G5());
+
+  TERN_(DIRECT_STEPPING, static void G6());
+
+  #if ENABLED(FWRETRACT)
+    static void G10();
+    static void G11();
+  #endif
+
+  TERN_(NOZZLE_CLEAN_FEATURE, static void G12());
+
+  #if ENABLED(CNC_WORKSPACE_PLANES)
+    static void G17();
+    static void G18();
+    static void G19();
+  #endif
+
+  #if ENABLED(INCH_MODE_SUPPORT)
+    static void G20();
+    static void G21();
+  #endif
+
+  TERN_(G26_MESH_VALIDATION, static void G26());
+
+  TERN_(NOZZLE_PARK_FEATURE, static void G27());
+
+  static void G28();
+
+  #if HAS_LEVELING
+    #if ENABLED(G29_RETRY_AND_RECOVER)
+      static void event_probe_failure();
+      static void event_probe_recover();
+      static void G29_with_retry();
+      #define G29_TYPE bool
+    #else
+      #define G29_TYPE void
+    #endif
+    static G29_TYPE G29();
+  #endif
+
+  #if HAS_BED_PROBE
+    static void G30();
+    #if ENABLED(Z_PROBE_SLED)
+      static void G31();
+      static void G32();
+    #endif
+  #endif
+
+  TERN_(DELTA_AUTO_CALIBRATION, static void G33());
+
+  #if ANY(Z_MULTI_ENDSTOPS, Z_STEPPER_AUTO_ALIGN, MECHANICAL_GANTRY_CALIBRATION)
+    static void G34();
+  #endif
+
+  TERN_(Z_STEPPER_AUTO_ALIGN, static void M422());
+
+  TERN_(ASSISTED_TRAMMING, static void G35());
+
+  TERN_(G38_PROBE_TARGET, static void G38(const int8_t subcode));
+
+  TERN_(HAS_MESH, static void G42());
+
+  #if ENABLED(CNC_COORDINATE_SYSTEMS)
+    static void G53();
+    static void G54();
+    static void G55();
+    static void G56();
+    static void G57();
+    static void G58();
+    static void G59();
+  #endif
+
+  TERN_(PROBE_TEMP_COMPENSATION, static void G76());
+
+  #if SAVED_POSITIONS
+    static void G60();
+    static void G61();
+  #endif
+
+  TERN_(GCODE_MOTION_MODES, static void G80());
+
+  static void G92();
+
+  TERN_(CALIBRATION_GCODE, static void G425());
+
+  TERN_(HAS_RESUME_CONTINUE, static void M0_M1());
+
+  #if HAS_CUTTER
+    static void M3_M4(const bool is_M4);
+    static void M5();
+  #endif
+
+  #if ENABLED(COOLANT_CONTROL)
+    TERN_(COOLANT_MIST, static void M7());
+    TERN_(COOLANT_FLOOD, static void M8());
+    static void M9();
+  #endif
+
+  TERN_(EXTERNAL_CLOSED_LOOP_CONTROLLER, static void M12());
+
+  TERN_(EXPECTED_PRINTER_CHECK, static void M16());
+
+  static void M17();
+
+  static void M18_M84();
+
+  #if ENABLED(SDSUPPORT)
+    static void M20();
+    static void M21();
+    static void M22();
+    static void M23();
+    static void M24();
+    static void M25();
+    static void M26();
+    static void M27();
+    static void M28();
+    static void M29();
+    static void M30();
+  #endif
+
+  static void M31();
+
+  #if ENABLED(SDSUPPORT)
+    TERN_(HAS_MEDIA_SUBCALLS, static void M32());
+    TERN_(LONG_FILENAME_HOST_SUPPORT, static void M33());
+    #if BOTH(SDCARD_SORT_ALPHA, SDSORT_GCODE)
+      static void M34();
+    #endif
+  #endif
+
+  TERN_(DIRECT_PIN_CONTROL, static void M42());
+  TERN_(PINS_DEBUGGING, static void M43());
+
+  TERN_(Z_MIN_PROBE_REPEATABILITY_TEST, static void M48());
+
+  TERN_(LCD_SET_PROGRESS_MANUALLY, static void M73());
+
+  static void M75();
+  static void M76();
+  static void M77();
+
+  TERN_(PRINTCOUNTER, static void M78());
+
+  TERN_(PSU_CONTROL, static void M80());
+
+  static void M81();
+  static void M82();
+  static void M83();
+  static void M85();
+  static void M92();
+
+  TERN_(M100_FREE_MEMORY_WATCHER, static void M100());
+
+  #if EXTRUDERS
+    static void M104();
+    static void M109();
+  #endif
+
+  static void M105();
+
+  #if HAS_FAN
+    static void M106();
+    static void M107();
+  #endif
+
+  #if DISABLED(EMERGENCY_PARSER)
+    static void M108();
+    static void M112();
+    static void M410();
+    TERN_(HOST_PROMPT_SUPPORT, static void M876());
+  #endif
+
+  static void M110();
+  static void M111();
+
+  TERN_(HOST_KEEPALIVE_FEATURE, static void M113());
+
+  static void M114();
+  static void M115();
+  static void M117();
+  static void M118();
+  static void M119();
+  static void M120();
+  static void M121();
+
+  TERN_(PARK_HEAD_ON_PAUSE, static void M125());
+
+  #if ENABLED(BARICUDA)
+    #if HAS_HEATER_1
+      static void M126();
+      static void M127();
+    #endif
+    #if HAS_HEATER_2
+      static void M128();
+      static void M129();
+    #endif
+  #endif
+
+  #if HAS_HEATED_BED
+    static void M140();
+    static void M190();
+  #endif
+
+  #if HAS_HEATED_CHAMBER
+    static void M141();
+    static void M191();
+  #endif
+
+  #if PREHEAT_COUNT
+    static void M145();
+  #endif
+
+  TERN_(TEMPERATURE_UNITS_SUPPORT, static void M149());
+
+  TERN_(HAS_COLOR_LEDS, static void M150());
+
+  #if BOTH(AUTO_REPORT_TEMPERATURES, HAS_TEMP_SENSOR)
+    static void M155();
+  #endif
+
+  #if ENABLED(MIXING_EXTRUDER)
+    static void M163();
+    static void M164();
+    TERN_(DIRECT_MIXING_IN_G1, static void M165());
+    TERN_(GRADIENT_MIX, static void M166());
+  #endif
+
+  static void M200();
+  static void M201();
+
+  #if 0
+    static void M202(); // Not used for Sprinter/grbl gen6
+  #endif
+
+  static void M203();
+  static void M204();
+  static void M205();
+
+  TERN_(HAS_M206_COMMAND, static void M206());
+
+  #if ENABLED(FWRETRACT)
+    static void M207();
+    static void M208();
+    TERN_(FWRETRACT_AUTORETRACT, static void M209());
+  #endif
+
+  static void M211();
+
+  TERN_(HAS_MULTI_EXTRUDER, static void M217());
+
+  TERN_(HAS_HOTEND_OFFSET, static void M218());
+
+  static void M220();
+
+  #if EXTRUDERS
+    static void M221();
+  #endif
+
+  TERN_(DIRECT_PIN_CONTROL, static void M226());
+
+  TERN_(PHOTO_GCODE, static void M240());
+
+  TERN_(HAS_LCD_CONTRAST, static void M250());
+
+  #if ENABLED(EXPERIMENTAL_I2CBUS)
+    static void M260();
+    static void M261();
+  #endif
+
+  #if HAS_SERVOS
+    static void M280();
+    TERN_(EDITABLE_SERVO_ANGLES, static void M281());
+  #endif
+
+  TERN_(BABYSTEPPING, static void M290());
+
+  TERN_(HAS_BUZZER, static void M300());
+
+  TERN_(PIDTEMP, static void M301());
+
+  TERN_(PREVENT_COLD_EXTRUSION, static void M302());
+
+  TERN_(HAS_PID_HEATING, static void M303());
+
+  TERN_(PIDTEMPBED, static void M304());
+
+  TERN_(HAS_USER_THERMISTORS, static void M305());
+
+  #if HAS_MICROSTEPS
+    static void M350();
+    static void M351();
+  #endif
+
+  TERN_(CASE_LIGHT_ENABLE, static void M355());
+
+  TERN_(REPETIER_GCODE_M360, static void M360());
+
+  #if ENABLED(MORGAN_SCARA)
+    static bool M360();
+    static bool M361();
+    static bool M362();
+    static bool M363();
+    static bool M364();
+  #endif
+
+  #if EITHER(EXT_SOLENOID, MANUAL_SOLENOID_CONTROL)
+    static void M380();
+    static void M381();
+  #endif
+
+  static void M400();
+
+  #if HAS_BED_PROBE
+    static void M401();
+    static void M402();
+  #endif
+
+  TERN_(HAS_PRUSA_MMU2, static void M403());
+
+  #if ENABLED(FILAMENT_WIDTH_SENSOR)
+    static void M404();
+    static void M405();
+    static void M406();
+    static void M407();
+  #endif
+
+  TERN_(HAS_FILAMENT_SENSOR, static void M412());
+
+  TERN_(HAS_MULTI_LANGUAGE, static void M414());
+
+  #if HAS_LEVELING
+    static void M420();
+    static void M421();
+  #endif
+
+  TERN_(BACKLASH_GCODE, static void M425());
+
+  TERN_(HAS_M206_COMMAND, static void M428());
+
+  TERN_(HAS_POWER_MONITOR, static void M430());
+
+  TERN_(CANCEL_OBJECTS, static void M486());
+
+  static void M500();
+  static void M501();
+  static void M502();
+  #if DISABLED(DISABLE_M503)
+    static void M503();
+  #endif
+  TERN_(EEPROM_SETTINGS, static void M504());
+
+  #if ENABLED(PASSWORD_FEATURE)
+    static void M510();
+    TERN_(PASSWORD_UNLOCK_GCODE, static void M511());
+    TERN_(PASSWORD_CHANGE_GCODE, static void M512());
+  #endif
+
+  TERN_(SDSUPPORT, static void M524());
+
+  TERN_(SD_ABORT_ON_ENDSTOP_HIT, static void M540());
+
+  #if HAS_ETHERNET
+    static void M552();
+    static void M553();
+    static void M554();
+  #endif
+
+  TERN_(BAUD_RATE_GCODE, static void M575());
+
+  #if ENABLED(ADVANCED_PAUSE_FEATURE)
+    static void M600();
+    static void M603();
+  #endif
+
+  TERN_(HAS_DUPLICATION_MODE, static void M605());
+
+  TERN_(IS_KINEMATIC, static void M665());
+
+  #if ENABLED(DELTA) || HAS_EXTRA_ENDSTOPS
+    static void M666();
+  #endif
+
+  #if ENABLED(DUET_SMART_EFFECTOR) && PIN_EXISTS(SMART_EFFECTOR_MOD)
+    static void M672();
+  #endif
+
+  #if ENABLED(FILAMENT_LOAD_UNLOAD_GCODES)
+    static void M701();
+    static void M702();
+  #endif
+
+  TERN_(GCODE_REPEAT_MARKERS, static void M808());
+
+  TERN_(GCODE_MACROS, static void M810_819());
+
+  TERN_(HAS_BED_PROBE, static void M851());
+
+  TERN_(SKEW_CORRECTION_GCODE, static void M852());
+
+  #if ENABLED(I2C_POSITION_ENCODERS)
+    FORCE_INLINE static void M860() { I2CPEM.M860(); }
+    FORCE_INLINE static void M861() { I2CPEM.M861(); }
+    FORCE_INLINE static void M862() { I2CPEM.M862(); }
+    FORCE_INLINE static void M863() { I2CPEM.M863(); }
+    FORCE_INLINE static void M864() { I2CPEM.M864(); }
+    FORCE_INLINE static void M865() { I2CPEM.M865(); }
+    FORCE_INLINE static void M866() { I2CPEM.M866(); }
+    FORCE_INLINE static void M867() { I2CPEM.M867(); }
+    FORCE_INLINE static void M868() { I2CPEM.M868(); }
+    FORCE_INLINE static void M869() { I2CPEM.M869(); }
+  #endif
+
+  #if ENABLED(PROBE_TEMP_COMPENSATION)
+    static void M192();
+    static void M871();
+  #endif
+
+  TERN_(LIN_ADVANCE, static void M900());
+
+  #if HAS_TRINAMIC_CONFIG
+    static void M122();
+    static void M906();
+    TERN_(HAS_STEALTHCHOP, static void M569());
+    #if ENABLED(MONITOR_DRIVER_STATUS)
+      static void M911();
+      static void M912();
+    #endif
+    TERN_(HYBRID_THRESHOLD, static void M913());
+    TERN_(USE_SENSORLESS, static void M914());
+  #endif
+
+  #if HAS_L64XX
+    static void M122();
+    static void M906();
+    static void M916();
+    static void M917();
+    static void M918();
+  #endif
+
+  #if ANY(HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_PWM, HAS_MOTOR_CURRENT_I2C, HAS_MOTOR_CURRENT_DAC)
+    static void M907();
+    #if EITHER(HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_DAC)
+      static void M908();
+      #if ENABLED(HAS_MOTOR_CURRENT_DAC)
+        static void M909();
+        static void M910();
+      #endif
+    #endif
+  #endif
+
+  TERN_(SDSUPPORT, static void M928());
+
+  TERN_(MAGNETIC_PARKING_EXTRUDER, static void M951());
+
+  TERN_(TOUCH_SCREEN_CALIBRATION, static void M995());
+
+  #if BOTH(HAS_SPI_FLASH, SDSUPPORT)
+    static void M993();
+    static void M994();
+  #endif
+
+  TERN_(PLATFORM_M997_SUPPORT, static void M997());
+
+  static void M999();
+
+  #if ENABLED(POWER_LOSS_RECOVERY)
+    static void M413();
+    static void M1000();
+  #endif
+
+  TERN_(SDSUPPORT, static void M1001());
+
+  TERN_(MAX7219_GCODE, static void M7219());
+
+  TERN_(CONTROLLER_FAN_EDITABLE, static void M710());
+
+  static void T(const int8_t tool_index);
+
+};
+
+extern GcodeSuite gcode;
diff --git a/Marlin/src/gcode/gcode_d.cpp b/Marlin/src/gcode/gcode_d.cpp
new file mode 100644
index 0000000..0bd2955
--- /dev/null
+++ b/Marlin/src/gcode/gcode_d.cpp
@@ -0,0 +1,181 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#include "../inc/MarlinConfigPre.h"
+
+#if ENABLED(MARLIN_DEV_MODE)
+
+  #include "gcode.h"
+  #include "../module/settings.h"
+  #include "../module/temperature.h"
+  #include "../libs/hex_print.h"
+  #include "../HAL/shared/eeprom_if.h"
+  #include "../HAL/shared/Delay.h"
+
+  /**
+   * Dn: G-code for development and testing
+   *
+   * See https://reprap.org/wiki/G-code#D:_Debug_codes
+   *
+   * Put whatever else you need here to test ongoing development.
+   */
+  void GcodeSuite::D(const int16_t dcode) {
+    switch (dcode) {
+
+      case -1:
+        for (;;); // forever
+
+      case 0:
+        HAL_reboot();
+        break;
+
+      case 1: {
+        // Zero or pattern-fill the EEPROM data
+        #if ENABLED(EEPROM_SETTINGS)
+          persistentStore.access_start();
+          size_t total = persistentStore.capacity();
+          int pos = 0;
+          const uint8_t value = 0x0;
+          while (total--) persistentStore.write_data(pos, &value, 1);
+          persistentStore.access_finish();
+        #else
+          settings.reset();
+          settings.save();
+        #endif
+        HAL_reboot();
+      } break;
+
+      case 2: { // D2 Read / Write SRAM
+        #define SRAM_SIZE 8192
+        uint8_t *pointer = parser.hex_adr_val('A');
+        uint16_t len = parser.ushortval('C', 1);
+        uintptr_t addr = (uintptr_t)pointer;
+        NOMORE(addr, size_t(SRAM_SIZE - 1));
+        NOMORE(len, SRAM_SIZE - addr);
+        if (parser.seenval('X')) {
+          // Write the hex bytes after the X
+          uint16_t val = parser.hex_val('X');
+          while (len--) {
+            *pointer = val;
+            pointer++;
+          }
+        }
+        else {
+          while (len--) print_hex_byte(*(pointer++));
+          SERIAL_EOL();
+        }
+      } break;
+
+      #if ENABLED(EEPROM_SETTINGS)
+        case 3: { // D3 Read / Write EEPROM
+          uint8_t *pointer = parser.hex_adr_val('A');
+          uint16_t len = parser.ushortval('C', 1);
+          uintptr_t addr = (uintptr_t)pointer;
+          NOMORE(addr, size_t(persistentStore.capacity() - 1));
+          NOMORE(len, persistentStore.capacity() - addr);
+          if (parser.seenval('X')) {
+            uint16_t val = parser.hex_val('X');
+            #if ENABLED(EEPROM_SETTINGS)
+              persistentStore.access_start();
+              while (len--) {
+                int pos = 0;
+                persistentStore.write_data(pos, (uint8_t *)&val, sizeof(val));
+              }
+              SERIAL_EOL();
+              persistentStore.access_finish();
+            #else
+              SERIAL_ECHOLNPGM("NO EEPROM");
+            #endif
+          }
+          else {
+            // Read bytes from EEPROM
+            #if ENABLED(EEPROM_SETTINGS)
+              persistentStore.access_start();
+              int pos = 0;
+              uint8_t val;
+              while (len--) if (!persistentStore.read_data(pos, &val, 1)) print_hex_byte(val);
+              SERIAL_EOL();
+              persistentStore.access_finish();
+            #else
+              SERIAL_ECHOLNPGM("NO EEPROM");
+              len = 0;
+            #endif
+            SERIAL_EOL();
+          }
+        } break;
+      #endif
+
+      case 4: { // D4 Read / Write PIN
+        // const uint8_t pin = parser.byteval('P');
+        // const bool is_out = parser.boolval('F'),
+        //            val = parser.byteval('V', LOW);
+        if (parser.seenval('X')) {
+          // TODO: Write the hex bytes after the X
+          //while (len--) {
+          //}
+        }
+        else {
+          // while (len--) {
+            // TODO: Read bytes from EEPROM
+            // print_hex_byte(eeprom_read_byte(*(adr++));
+          // }
+          SERIAL_EOL();
+        }
+      } break;
+
+      case 5: { // D4 Read / Write onboard Flash
+        #define FLASH_SIZE 1024
+        uint8_t *pointer = parser.hex_adr_val('A');
+        uint16_t len = parser.ushortval('C', 1);
+        uintptr_t addr = (uintptr_t)pointer;
+        NOMORE(addr, size_t(FLASH_SIZE - 1));
+        NOMORE(len, FLASH_SIZE - addr);
+        if (parser.seenval('X')) {
+          // TODO: Write the hex bytes after the X
+          //while (len--) {
+          //}
+        }
+        else {
+          // while (len--) {
+            // TODO: Read bytes from EEPROM
+            // print_hex_byte(eeprom_read_byte(adr++));
+          // }
+          SERIAL_EOL();
+        }
+      } break;
+
+      case 100: { // D100 Disable heaters and attempt a hard hang (Watchdog Test)
+        SERIAL_ECHOLNPGM("Disabling heaters and attempting to trigger Watchdog");
+        SERIAL_ECHOLNPGM("(USE_WATCHDOG " TERN(USE_WATCHDOG, "ENABLED", "DISABLED") ")");
+        thermalManager.disable_all_heaters();
+        delay(1000); // Allow time to print
+        DISABLE_ISRS();
+        // Use a low-level delay that does not rely on interrupts to function
+        // Do not spin forever, to avoid thermal risks if heaters are enabled and
+        // watchdog does not work.
+        for (int i = 10000; i--;) DELAY_US(1000UL);
+        ENABLE_ISRS();
+        SERIAL_ECHOLNPGM("FAILURE: Watchdog did not trigger board reset.");
+      }
+    }
+  }
+
+#endif
diff --git a/Marlin/src/gcode/geometry/G17-G19.cpp b/Marlin/src/gcode/geometry/G17-G19.cpp
new file mode 100644
index 0000000..7510eab
--- /dev/null
+++ b/Marlin/src/gcode/geometry/G17-G19.cpp
@@ -0,0 +1,53 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(CNC_WORKSPACE_PLANES)
+
+#include "../gcode.h"
+
+inline void report_workspace_plane() {
+  SERIAL_ECHO_START();
+  SERIAL_ECHOPGM("Workspace Plane ");
+  serialprintPGM(
+      gcode.workspace_plane == GcodeSuite::PLANE_YZ ? PSTR("YZ\n")
+    : gcode.workspace_plane == GcodeSuite::PLANE_ZX ? PSTR("ZX\n")
+                                                    : PSTR("XY\n")
+  );
+}
+
+inline void set_workspace_plane(const GcodeSuite::WorkspacePlane plane) {
+  gcode.workspace_plane = plane;
+  if (DEBUGGING(INFO)) report_workspace_plane();
+}
+
+/**
+ * G17: Select Plane XY
+ * G18: Select Plane ZX
+ * G19: Select Plane YZ
+ */
+void GcodeSuite::G17() { set_workspace_plane(PLANE_XY); }
+void GcodeSuite::G18() { set_workspace_plane(PLANE_ZX); }
+void GcodeSuite::G19() { set_workspace_plane(PLANE_YZ); }
+
+#endif // CNC_WORKSPACE_PLANES
diff --git a/Marlin/src/gcode/geometry/G53-G59.cpp b/Marlin/src/gcode/geometry/G53-G59.cpp
new file mode 100644
index 0000000..05bc522
--- /dev/null
+++ b/Marlin/src/gcode/geometry/G53-G59.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 <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+
+#if ENABLED(CNC_COORDINATE_SYSTEMS)
+
+#include "../../module/stepper.h"
+
+//#define DEBUG_M53
+
+/**
+ * Select a coordinate system and update the workspace offset.
+ * System index -1 is used to specify machine-native.
+ */
+bool GcodeSuite::select_coordinate_system(const int8_t _new) {
+  if (active_coordinate_system == _new) return false;
+  active_coordinate_system = _new;
+  xyz_float_t new_offset{0};
+  if (WITHIN(_new, 0, MAX_COORDINATE_SYSTEMS - 1))
+    new_offset = coordinate_system[_new];
+  LOOP_XYZ(i) {
+    if (position_shift[i] != new_offset[i]) {
+      position_shift[i] = new_offset[i];
+      update_workspace_offset((AxisEnum)i);
+    }
+  }
+  return true;
+}
+
+/**
+ * G53: Apply native workspace to the current move
+ *
+ * In CNC G-code G53 is a modifier.
+ * It precedes a movement command (or other modifiers) on the same line.
+ * This is the first command to use parser.chain() to make this possible.
+ *
+ * Marlin also uses G53 on a line by itself to go back to native space.
+ */
+void GcodeSuite::G53() {
+  const int8_t old_system = active_coordinate_system;
+  select_coordinate_system(-1);   // Always remove workspace offsets
+  #ifdef DEBUG_M53
+    SERIAL_ECHOLNPGM("Go to native space");
+    report_current_position();
+  #endif
+
+  if (parser.chain()) {       // Command to chain?
+    process_parsed_command(); // ...process the chained command
+    select_coordinate_system(old_system);
+    #ifdef DEBUG_M53
+      SERIAL_ECHOLNPAIR("Go back to workspace ", old_system);
+      report_current_position();
+    #endif
+  }
+}
+
+/**
+ * G54-G59.3: Select a new workspace
+ *
+ * A workspace is an XYZ offset to the machine native space.
+ * All workspaces default to 0,0,0 at start, or with EEPROM
+ * support they may be restored from a previous session.
+ *
+ * G92 is used to set the current workspace's offset.
+ */
+void G54_59(uint8_t subcode=0) {
+  const int8_t _space = parser.codenum - 54 + subcode;
+  if (gcode.select_coordinate_system(_space)) {
+    SERIAL_ECHOLNPAIR("Select workspace ", _space);
+    report_current_position();
+  }
+}
+void GcodeSuite::G54() { G54_59(); }
+void GcodeSuite::G55() { G54_59(); }
+void GcodeSuite::G56() { G54_59(); }
+void GcodeSuite::G57() { G54_59(); }
+void GcodeSuite::G58() { G54_59(); }
+void GcodeSuite::G59() { G54_59(parser.subcode); }
+
+#endif // CNC_COORDINATE_SYSTEMS
diff --git a/Marlin/src/gcode/geometry/G92.cpp b/Marlin/src/gcode/geometry/G92.cpp
new file mode 100644
index 0000000..1a0382e
--- /dev/null
+++ b/Marlin/src/gcode/geometry/G92.cpp
@@ -0,0 +1,105 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/stepper.h"
+
+#if ENABLED(I2C_POSITION_ENCODERS)
+  #include "../../feature/encoder_i2c.h"
+#endif
+
+/**
+ * G92: Set current position to given X Y Z E
+ */
+void GcodeSuite::G92() {
+
+  bool sync_E = false, sync_XYZ = false;
+
+  #if ENABLED(USE_GCODE_SUBCODES)
+    const uint8_t subcode_G92 = parser.subcode;
+  #else
+    constexpr uint8_t subcode_G92 = 0;
+  #endif
+
+  switch (subcode_G92) {
+    default: break;
+    #if ENABLED(CNC_COORDINATE_SYSTEMS)
+      case 1: {
+        // Zero the G92 values and restore current position
+        #if !IS_SCARA
+          LOOP_XYZ(i) if (position_shift[i]) {
+            position_shift[i] = 0;
+            update_workspace_offset((AxisEnum)i);
+          }
+        #endif // Not SCARA
+      } return;
+    #endif
+    #if ENABLED(POWER_LOSS_RECOVERY)
+      case 9: {
+        LOOP_XYZE(i) {
+          if (parser.seenval(axis_codes[i])) {
+            current_position[i] = parser.value_axis_units((AxisEnum)i);
+            if (i == E_AXIS) sync_E = true; else sync_XYZ = true;
+          }
+        }
+      } break;
+    #endif
+    case 0: {
+      LOOP_XYZE(i) {
+        if (parser.seenval(axis_codes[i])) {
+          const float l = parser.value_axis_units((AxisEnum)i),
+                      v = i == E_AXIS ? l : LOGICAL_TO_NATIVE(l, i),
+                      d = v - current_position[i];
+          if (!NEAR_ZERO(d)) {
+            #if IS_SCARA || !HAS_POSITION_SHIFT
+              if (i == E_AXIS) sync_E = true; else sync_XYZ = true;
+              current_position[i] = v;        // Without workspaces revert to Marlin 1.0 behavior
+            #elif HAS_POSITION_SHIFT
+              if (i == E_AXIS) {
+                sync_E = true;
+                current_position.e = v;       // When using coordinate spaces, only E is set directly
+              }
+              else {
+                position_shift[i] += d;       // Other axes simply offset the coordinate space
+                update_workspace_offset((AxisEnum)i);
+              }
+            #endif
+          }
+        }
+      }
+    } break;
+  }
+
+  #if ENABLED(CNC_COORDINATE_SYSTEMS)
+    // Apply workspace offset to the active coordinate system
+    if (WITHIN(active_coordinate_system, 0, MAX_COORDINATE_SYSTEMS - 1))
+      coordinate_system[active_coordinate_system] = position_shift;
+  #endif
+
+  if    (sync_XYZ) sync_plan_position();
+  else if (sync_E) sync_plan_position_e();
+
+  #if DISABLED(DIRECT_STEPPING)
+    report_current_position();
+  #endif
+}
diff --git a/Marlin/src/gcode/geometry/M206_M428.cpp b/Marlin/src/gcode/geometry/M206_M428.cpp
new file mode 100644
index 0000000..2a2cdb1
--- /dev/null
+++ b/Marlin/src/gcode/geometry/M206_M428.cpp
@@ -0,0 +1,94 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_M206_COMMAND
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../lcd/marlinui.h"
+#include "../../libs/buzzer.h"
+#include "../../MarlinCore.h"
+
+void m206_report() {
+  SERIAL_ECHOLNPAIR_P(PSTR("M206 X"), home_offset.x, SP_Y_STR, home_offset.y, SP_Z_STR, home_offset.z);
+}
+
+/**
+ * M206: Set Additional Homing Offset (X Y Z). SCARA aliases T=X, P=Y
+ *
+ * *** @thinkyhead: I recommend deprecating M206 for SCARA in favor of M665.
+ * ***              M206 for SCARA will remain enabled in 1.1.x for compatibility.
+ * ***              In the 2.0 release, it will simply be disabled by default.
+ */
+void GcodeSuite::M206() {
+  LOOP_XYZ(i)
+    if (parser.seen(XYZ_CHAR(i)))
+      set_home_offset((AxisEnum)i, parser.value_linear_units());
+
+  #if ENABLED(MORGAN_SCARA)
+    if (parser.seen('T')) set_home_offset(A_AXIS, parser.value_float()); // Theta
+    if (parser.seen('P')) set_home_offset(B_AXIS, parser.value_float()); // Psi
+  #endif
+
+  if (!parser.seen("XYZ"))
+    m206_report();
+  else
+    report_current_position();
+}
+
+/**
+ * M428: Set home_offset based on the distance between the
+ *       current_position and the nearest "reference point."
+ *       If an axis is past center its endstop position
+ *       is the reference-point. Otherwise it uses 0. This allows
+ *       the Z offset to be set near the bed when using a max endstop.
+ *
+ *       M428 can't be used more than 2cm away from 0 or an endstop.
+ *
+ *       Use M206 to set these values directly.
+ */
+void GcodeSuite::M428() {
+  if (homing_needed_error()) return;
+
+  xyz_float_t diff;
+  LOOP_XYZ(i) {
+    diff[i] = base_home_pos((AxisEnum)i) - current_position[i];
+    if (!WITHIN(diff[i], -20, 20) && home_dir((AxisEnum)i) > 0)
+      diff[i] = -current_position[i];
+    if (!WITHIN(diff[i], -20, 20)) {
+      SERIAL_ERROR_MSG(STR_ERR_M428_TOO_FAR);
+      LCD_ALERTMESSAGEPGM_P(PSTR("Err: Too far!"));
+      BUZZ(200, 40);
+      return;
+    }
+  }
+
+  LOOP_XYZ(i) set_home_offset((AxisEnum)i, diff[i]);
+  report_current_position();
+  LCD_MESSAGEPGM(MSG_HOME_OFFSETS_APPLIED);
+  BUZZ(100, 659);
+  BUZZ(100, 698);
+}
+
+#endif // HAS_M206_COMMAND
diff --git a/Marlin/src/gcode/host/M110.cpp b/Marlin/src/gcode/host/M110.cpp
new file mode 100644
index 0000000..b12b38e
--- /dev/null
+++ b/Marlin/src/gcode/host/M110.cpp
@@ -0,0 +1,34 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../queue.h" // for last_N
+
+/**
+ * M110: Set Current Line Number
+ */
+void GcodeSuite::M110() {
+
+  if (parser.seenval('N'))
+    queue.last_N[queue.command_port()] = parser.value_long();
+
+}
diff --git a/Marlin/src/gcode/host/M113.cpp b/Marlin/src/gcode/host/M113.cpp
new file mode 100644
index 0000000..ce826d6
--- /dev/null
+++ b/Marlin/src/gcode/host/M113.cpp
@@ -0,0 +1,45 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(HOST_KEEPALIVE_FEATURE)
+
+#include "../gcode.h"
+
+/**
+ * M113: Get or set Host Keepalive interval (0 to disable)
+ *
+ *   S<seconds> Optional. Set the keepalive interval.
+ */
+void GcodeSuite::M113() {
+  if (parser.seenval('S')) {
+    host_keepalive_interval = parser.value_byte();
+    NOMORE(host_keepalive_interval, 60);
+  }
+  else {
+    SERIAL_ECHO_START();
+    SERIAL_ECHOLNPAIR("M113 S", (unsigned long)host_keepalive_interval);
+  }
+}
+
+#endif // HOST_KEEPALIVE_FEATURE
diff --git a/Marlin/src/gcode/host/M114.cpp b/Marlin/src/gcode/host/M114.cpp
new file mode 100644
index 0000000..75356ff
--- /dev/null
+++ b/Marlin/src/gcode/host/M114.cpp
@@ -0,0 +1,214 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/stepper.h"
+
+#if ENABLED(M114_DETAIL)
+
+  #if HAS_L64XX
+    #include "../../libs/L64XX/L64XX_Marlin.h"
+    #define DEBUG_OUT ENABLED(L6470_CHITCHAT)
+    #include "../../core/debug_out.h"
+  #endif
+
+  void report_xyze(const xyze_pos_t &pos, const uint8_t n=XYZE, const uint8_t precision=3) {
+    char str[12];
+    LOOP_L_N(a, n) {
+      SERIAL_CHAR(' ', axis_codes[a], ':');
+      if (pos[a] >= 0) SERIAL_CHAR(' ');
+      SERIAL_ECHO(dtostrf(pos[a], 1, precision, str));
+    }
+    SERIAL_EOL();
+  }
+  inline void report_xyz(const xyze_pos_t &pos) { report_xyze(pos, XYZ); }
+
+  void report_xyz(const xyz_pos_t &pos, const uint8_t precision=3) {
+    char str[12];
+    LOOP_XYZ(a) {
+      SERIAL_CHAR(' ', XYZ_CHAR(a), ':');
+      SERIAL_ECHO(dtostrf(pos[a], 1, precision, str));
+    }
+    SERIAL_EOL();
+  }
+
+  void report_current_position_detail() {
+    // Position as sent by G-code
+    SERIAL_ECHOPGM("\nLogical:");
+    report_xyz(current_position.asLogical());
+
+    // Cartesian position in native machine space
+    SERIAL_ECHOPGM("Raw:    ");
+    report_xyz(current_position);
+
+    xyze_pos_t leveled = current_position;
+
+    #if HAS_LEVELING
+      // Current position with leveling applied
+      SERIAL_ECHOPGM("Leveled:");
+      planner.apply_leveling(leveled);
+      report_xyz(leveled);
+
+      // Test planner un-leveling. This should match the Raw result.
+      SERIAL_ECHOPGM("UnLevel:");
+      xyze_pos_t unleveled = leveled;
+      planner.unapply_leveling(unleveled);
+      report_xyz(unleveled);
+    #endif
+
+    #if IS_KINEMATIC
+      // Kinematics applied to the leveled position
+      SERIAL_ECHOPGM(TERN(IS_SCARA, "ScaraK: ", "DeltaK: "));
+      inverse_kinematics(leveled);  // writes delta[]
+      report_xyz(delta);
+    #endif
+
+    planner.synchronize();
+
+    #if HAS_L64XX
+      char temp_buf[80];
+      int32_t temp;
+      //#define ABS_POS_SIGN_MASK 0b1111 1111 1110 0000 0000 0000 0000 0000
+      #define ABS_POS_SIGN_MASK 0b11111111111000000000000000000000
+      #define REPORT_ABSOLUTE_POS(Q) do{                            \
+          L64xxManager.say_axis(Q, false);                          \
+          temp = L6470_GETPARAM(L6470_ABS_POS,Q);                   \
+          if (temp & ABS_POS_SIGN_MASK) temp |= ABS_POS_SIGN_MASK;  \
+          sprintf_P(temp_buf, PSTR(":%8ld   "), temp);              \
+          DEBUG_ECHO(temp_buf);                                     \
+        }while(0)
+
+      DEBUG_ECHOPGM("\nL6470:");
+      #if AXIS_IS_L64XX(X)
+        REPORT_ABSOLUTE_POS(X);
+      #endif
+      #if AXIS_IS_L64XX(X2)
+        REPORT_ABSOLUTE_POS(X2);
+      #endif
+      #if AXIS_IS_L64XX(Y)
+        REPORT_ABSOLUTE_POS(Y);
+      #endif
+      #if AXIS_IS_L64XX(Y2)
+        REPORT_ABSOLUTE_POS(Y2);
+      #endif
+      #if AXIS_IS_L64XX(Z)
+        REPORT_ABSOLUTE_POS(Z);
+      #endif
+      #if AXIS_IS_L64XX(Z2)
+        REPORT_ABSOLUTE_POS(Z2);
+      #endif
+      #if AXIS_IS_L64XX(Z3)
+        REPORT_ABSOLUTE_POS(Z3);
+      #endif
+      #if AXIS_IS_L64XX(Z4)
+        REPORT_ABSOLUTE_POS(Z4);
+      #endif
+      #if AXIS_IS_L64XX(E0)
+        REPORT_ABSOLUTE_POS(E0);
+      #endif
+      #if AXIS_IS_L64XX(E1)
+        REPORT_ABSOLUTE_POS(E1);
+      #endif
+      #if AXIS_IS_L64XX(E2)
+        REPORT_ABSOLUTE_POS(E2);
+      #endif
+      #if AXIS_IS_L64XX(E3)
+        REPORT_ABSOLUTE_POS(E3);
+      #endif
+      #if AXIS_IS_L64XX(E4)
+        REPORT_ABSOLUTE_POS(E4);
+      #endif
+      #if AXIS_IS_L64XX(E5)
+        REPORT_ABSOLUTE_POS(E5);
+      #endif
+      #if AXIS_IS_L64XX(E6)
+        REPORT_ABSOLUTE_POS(E6);
+      #endif
+      #if AXIS_IS_L64XX(E7)
+        REPORT_ABSOLUTE_POS(E7);
+      #endif
+      SERIAL_EOL();
+    #endif // HAS_L64XX
+
+    SERIAL_ECHOPGM("Stepper:");
+    LOOP_XYZE(i) {
+      SERIAL_CHAR(' ', axis_codes[i], ':');
+      SERIAL_ECHO(stepper.position((AxisEnum)i));
+    }
+    SERIAL_EOL();
+
+    #if IS_SCARA
+      const xy_float_t deg = {
+        planner.get_axis_position_degrees(A_AXIS),
+        planner.get_axis_position_degrees(B_AXIS)
+      };
+      SERIAL_ECHOPGM("Degrees:");
+      report_xyze(deg, 2);
+    #endif
+
+    SERIAL_ECHOPGM("FromStp:");
+    get_cartesian_from_steppers();  // writes 'cartes' (with forward kinematics)
+    xyze_pos_t from_steppers = { cartes.x, cartes.y, cartes.z, planner.get_axis_position_mm(E_AXIS) };
+    report_xyze(from_steppers);
+
+    const xyze_float_t diff = from_steppers - leveled;
+    SERIAL_ECHOPGM("Diff:   ");
+    report_xyze(diff);
+  }
+
+#endif // M114_DETAIL
+
+/**
+ * M114: Report the current position to host.
+ *       Since steppers are moving, the count positions are
+ *       projected by using planner calculations.
+ *   D - Report more detail. This syncs the planner. (Requires M114_DETAIL)
+ *   E - Report E stepper position (Requires M114_DETAIL)
+ *   R - Report the realtime position instead of projected.
+ */
+void GcodeSuite::M114() {
+
+  #if ENABLED(M114_DETAIL)
+    if (parser.seen('D')) {
+      #if DISABLED(M114_LEGACY)
+        planner.synchronize();
+      #endif
+      report_current_position();
+      report_current_position_detail();
+      return;
+    }
+    if (parser.seen('E')) {
+      SERIAL_ECHOLNPAIR("Count E:", stepper.position(E_AXIS));
+      return;
+    }
+  #endif
+
+  #if ENABLED(M114_REALTIME)
+    if (parser.seen('R')) { report_real_position(); return; }
+  #endif
+
+  TERN_(M114_LEGACY, planner.synchronize());
+  report_current_position_projected();
+}
diff --git a/Marlin/src/gcode/host/M115.cpp b/Marlin/src/gcode/host/M115.cpp
new file mode 100644
index 0000000..0501f3f
--- /dev/null
+++ b/Marlin/src/gcode/host/M115.cpp
@@ -0,0 +1,171 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(M115_GEOMETRY_REPORT)
+  #include "../../module/motion.h"
+#endif
+
+#if ENABLED(CASE_LIGHT_ENABLE)
+  #include "../../feature/caselight.h"
+#endif
+
+#if ENABLED(EXTENDED_CAPABILITIES_REPORT)
+  static void cap_line(PGM_P const name, bool ena=false) {
+    SERIAL_ECHOPGM("Cap:");
+    serialprintPGM(name);
+    SERIAL_CHAR(':', ena ? '1' : '0');
+    SERIAL_EOL();
+  }
+#endif
+
+/**
+ * M115: Capabilities string and extended capabilities report
+ *       If a capability is not reported, hosts should assume
+ *       the capability is not present.
+ */
+void GcodeSuite::M115() {
+  SERIAL_ECHOLNPGM(
+    "FIRMWARE_NAME:Marlin " DETAILED_BUILD_VERSION " (" __DATE__ " " __TIME__ ") "
+    "SOURCE_CODE_URL:" SOURCE_CODE_URL " "
+    "PROTOCOL_VERSION:" PROTOCOL_VERSION " "
+    "MACHINE_TYPE:" MACHINE_NAME " "
+    "EXTRUDER_COUNT:" STRINGIFY(EXTRUDERS) " "
+    #ifdef MACHINE_UUID
+      "UUID:" MACHINE_UUID
+    #endif
+  );
+
+  #if ENABLED(EXTENDED_CAPABILITIES_REPORT)
+
+    // PAREN_COMMENTS
+    TERN_(PAREN_COMMENTS, cap_line(PSTR("PAREN_COMMENTS"), true));
+
+    // QUOTED_STRINGS
+    TERN_(GCODE_QUOTED_STRINGS, cap_line(PSTR("QUOTED_STRINGS"), true));
+
+    // SERIAL_XON_XOFF
+    cap_line(PSTR("SERIAL_XON_XOFF"), ENABLED(SERIAL_XON_XOFF));
+
+    // BINARY_FILE_TRANSFER (M28 B1)
+    cap_line(PSTR("BINARY_FILE_TRANSFER"), ENABLED(BINARY_FILE_TRANSFER));
+
+    // EEPROM (M500, M501)
+    cap_line(PSTR("EEPROM"), ENABLED(EEPROM_SETTINGS));
+
+    // Volumetric Extrusion (M200)
+    cap_line(PSTR("VOLUMETRIC"), DISABLED(NO_VOLUMETRICS));
+
+    // AUTOREPORT_TEMP (M155)
+    cap_line(PSTR("AUTOREPORT_TEMP"), ENABLED(AUTO_REPORT_TEMPERATURES));
+
+    // PROGRESS (M530 S L, M531 <file>, M532 X L)
+    cap_line(PSTR("PROGRESS"));
+
+    // Print Job timer M75, M76, M77
+    cap_line(PSTR("PRINT_JOB"), true);
+
+    // AUTOLEVEL (G29)
+    cap_line(PSTR("AUTOLEVEL"), ENABLED(HAS_AUTOLEVEL));
+
+    // RUNOUT (M412, M600)
+    cap_line(PSTR("RUNOUT"), ENABLED(FILAMENT_RUNOUT_SENSOR));
+
+    // Z_PROBE (G30)
+    cap_line(PSTR("Z_PROBE"), ENABLED(HAS_BED_PROBE));
+
+    // MESH_REPORT (M420 V)
+    cap_line(PSTR("LEVELING_DATA"), ENABLED(HAS_LEVELING));
+
+    // BUILD_PERCENT (M73)
+    cap_line(PSTR("BUILD_PERCENT"), ENABLED(LCD_SET_PROGRESS_MANUALLY));
+
+    // SOFTWARE_POWER (M80, M81)
+    cap_line(PSTR("SOFTWARE_POWER"), ENABLED(PSU_CONTROL));
+
+    // TOGGLE_LIGHTS (M355)
+    cap_line(PSTR("TOGGLE_LIGHTS"), ENABLED(CASE_LIGHT_ENABLE));
+    cap_line(PSTR("CASE_LIGHT_BRIGHTNESS"), TERN0(CASE_LIGHT_ENABLE, TERN0(CASELIGHT_USES_BRIGHTNESS, TERN(CASE_LIGHT_USE_NEOPIXEL, true, PWM_PIN(CASE_LIGHT_PIN)))));
+
+    // EMERGENCY_PARSER (M108, M112, M410, M876)
+    cap_line(PSTR("EMERGENCY_PARSER"), ENABLED(EMERGENCY_PARSER));
+
+    // PROMPT SUPPORT (M876)
+    cap_line(PSTR("PROMPT_SUPPORT"), ENABLED(HOST_PROMPT_SUPPORT));
+
+    // SDCARD (M20, M23, M24, etc.)
+    cap_line(PSTR("SDCARD"), ENABLED(SDSUPPORT));
+
+    // REPEAT (M808)
+    cap_line(PSTR("REPEAT"), ENABLED(GCODE_REPEAT_MARKERS));
+
+    // AUTOREPORT_SD_STATUS (M27 extension)
+    cap_line(PSTR("AUTOREPORT_SD_STATUS"), ENABLED(AUTO_REPORT_SD_STATUS));
+
+    // LONG_FILENAME_HOST_SUPPORT (M33)
+    cap_line(PSTR("LONG_FILENAME"), ENABLED(LONG_FILENAME_HOST_SUPPORT));
+
+    // THERMAL_PROTECTION
+    cap_line(PSTR("THERMAL_PROTECTION"), ENABLED(THERMALLY_SAFE));
+
+    // MOTION_MODES (M80-M89)
+    cap_line(PSTR("MOTION_MODES"), ENABLED(GCODE_MOTION_MODES));
+
+    // ARC_SUPPORT (G2-G3)
+    cap_line(PSTR("ARCS"), ENABLED(ARC_SUPPORT));
+
+    // BABYSTEPPING (M290)
+    cap_line(PSTR("BABYSTEPPING"), ENABLED(BABYSTEPPING));
+
+    // CHAMBER_TEMPERATURE (M141, M191)
+    cap_line(PSTR("CHAMBER_TEMPERATURE"), ENABLED(HAS_HEATED_CHAMBER));
+
+    // MEATPACK Compresson
+    cap_line(PSTR("MEATPACK"), ENABLED(MEATPACK));
+
+    // Machine Geometry
+    #if ENABLED(M115_GEOMETRY_REPORT)
+      const xyz_pos_t dmin = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS },
+                      dmax = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS };
+      xyz_pos_t cmin = dmin, cmax = dmax;
+      apply_motion_limits(cmin);
+      apply_motion_limits(cmax);
+      const xyz_pos_t lmin = dmin.asLogical(), lmax = dmax.asLogical(),
+                      wmin = cmin.asLogical(), wmax = cmax.asLogical();
+      SERIAL_ECHOLNPAIR(
+        "area:{"
+          "full:{"
+            "min:{x:", lmin.x, ",y:", lmin.y, ",z:", lmin.z, "},"
+            "max:{x:", lmax.x, ",y:", lmax.y, ",z:", lmax.z, "}"
+          "},"
+          "work:{"
+            "min:{x:", wmin.x, ",y:", wmin.y, ",z:", wmin.z, "},"
+            "max:{x:", wmax.x, ",y:", wmax.y, ",z:", wmax.z, "}",
+          "}"
+        "}"
+      );
+    #endif
+
+  #endif // EXTENDED_CAPABILITIES_REPORT
+}
diff --git a/Marlin/src/gcode/host/M118.cpp b/Marlin/src/gcode/host/M118.cpp
new file mode 100644
index 0000000..73115d5
--- /dev/null
+++ b/Marlin/src/gcode/host/M118.cpp
@@ -0,0 +1,66 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../core/serial.h"
+
+/**
+ * M118: Display a message in the host console.
+ *
+ *  A1  Prepend '// ' for an action command, as in OctoPrint
+ *  E1  Have the host 'echo:' the text
+ *  Pn  Redirect to another serial port
+ *        0 : Announce to all ports
+ *      1-9 : Serial ports 1 to 9
+ */
+void GcodeSuite::M118() {
+  bool hasE = false, hasA = false;
+  #if HAS_MULTI_SERIAL
+    int8_t port = -1; // Assume no redirect
+  #endif
+  char *p = parser.string_arg;
+  for (uint8_t i = 3; i--;) {
+    // A1, E1, and Pn are always parsed out
+    if (!( ((p[0] == 'A' || p[0] == 'E') && p[1] == '1') || (p[0] == 'P' && NUMERIC(p[1])) )) break;
+    switch (p[0]) {
+      case 'A': hasA = true; break;
+      case 'E': hasE = true; break;
+      #if HAS_MULTI_SERIAL
+        case 'P': port = p[1] - '0'; break;
+      #endif
+    }
+    p += 2;
+    while (*p == ' ') ++p;
+  }
+
+  #if HAS_MULTI_SERIAL
+    const int8_t old_serial = multiSerial.portMask;
+    if (WITHIN(port, 0, NUM_SERIAL))
+      multiSerial.portMask = port ? _BV(port - 1) : SERIAL_ALL;
+  #endif
+
+  if (hasE) SERIAL_ECHO_START();
+  if (hasA) SERIAL_ECHOPGM("//");
+  SERIAL_ECHOLN(p);
+
+  TERN_(HAS_MULTI_SERIAL, multiSerial.portMask = old_serial);
+}
diff --git a/Marlin/src/gcode/host/M119.cpp b/Marlin/src/gcode/host/M119.cpp
new file mode 100644
index 0000000..f0066bd
--- /dev/null
+++ b/Marlin/src/gcode/host/M119.cpp
@@ -0,0 +1,33 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../module/endstops.h"
+
+/**
+ * M119: Output endstop states to serial output
+ */
+void GcodeSuite::M119() {
+
+  endstops.report_states();
+
+}
diff --git a/Marlin/src/gcode/host/M16.cpp b/Marlin/src/gcode/host/M16.cpp
new file mode 100644
index 0000000..1ac8580
--- /dev/null
+++ b/Marlin/src/gcode/host/M16.cpp
@@ -0,0 +1,40 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if ENABLED(EXPECTED_PRINTER_CHECK)
+
+#include "../gcode.h"
+#include "../../MarlinCore.h"
+
+/**
+ * M16: Expected Printer Check
+ */
+void GcodeSuite::M16() {
+
+  if (strcmp_P(parser.string_arg, PSTR(MACHINE_NAME)))
+    kill(GET_TEXT(MSG_KILL_EXPECTED_PRINTER));
+
+}
+
+#endif
diff --git a/Marlin/src/gcode/host/M360.cpp b/Marlin/src/gcode/host/M360.cpp
new file mode 100644
index 0000000..f49a32c
--- /dev/null
+++ b/Marlin/src/gcode/host/M360.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 <https://www.gnu.org/licenses/>.
+ *
+ */
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(REPETIER_GCODE_M360)
+
+#include "../gcode.h"
+
+#include "../../module/motion.h"
+#include "../../module/planner.h"
+
+#if EXTRUDERS
+  #include "../../module/temperature.h"
+#endif
+
+static void config_prefix(PGM_P const name, PGM_P const pref=nullptr, const int8_t ind=-1) {
+  SERIAL_ECHOPGM("Config:");
+  if (pref) serialprintPGM(pref);
+  if (ind >= 0) { SERIAL_ECHO(int(ind)); SERIAL_CHAR(':'); }
+  serialprintPGM(name);
+  SERIAL_CHAR(':');
+}
+static void config_line(PGM_P const name, const float val, PGM_P const pref=nullptr, const int8_t ind=-1) {
+  config_prefix(name, pref, ind);
+  SERIAL_ECHOLN(val);
+}
+static void config_line_e(const int8_t e, PGM_P const name, const float val) {
+  config_line(name, val, PSTR("Extr."), e + 1);
+}
+
+/**
+ * M360: Report Firmware configuration
+ *       in RepRapFirmware-compatible format
+ */
+void GcodeSuite::M360() {
+  PGMSTR(X_STR,    "X");
+  PGMSTR(Y_STR,    "Y");
+  PGMSTR(Z_STR,    "Z");
+  PGMSTR(JERK_STR, "Jerk");
+
+  //
+  // Basics and Enabled items
+  //
+  config_line(PSTR("Baudrate"),                   BAUDRATE);
+  config_line(PSTR("InputBuffer"),                MAX_CMD_SIZE);
+  config_line(PSTR("PrintlineCache"),             BUFSIZE);
+  config_line(PSTR("MixingExtruder"),             ENABLED(MIXING_EXTRUDER));
+  config_line(PSTR("SDCard"),                     ENABLED(SDSUPPORT));
+  config_line(PSTR("Fan"),                        ENABLED(HAS_FAN));
+  config_line(PSTR("LCD"),                        ENABLED(HAS_DISPLAY));
+  config_line(PSTR("SoftwarePowerSwitch"), 1);
+  config_line(PSTR("SupportLocalFilamentchange"), ENABLED(ADVANCED_PAUSE_FEATURE));
+  config_line(PSTR("CaseLights"),                 ENABLED(CASE_LIGHT_ENABLE));
+  config_line(PSTR("ZProbe"),                     ENABLED(HAS_BED_PROBE));
+  config_line(PSTR("Autolevel"),                  ENABLED(HAS_LEVELING));
+  config_line(PSTR("EEPROM"),                     ENABLED(EEPROM_SETTINGS));
+
+  //
+  // Homing Directions
+  //
+  PGMSTR(H_DIR_STR, "HomeDir");
+  config_line(H_DIR_STR, X_HOME_DIR, X_STR);
+  config_line(H_DIR_STR, Y_HOME_DIR, Y_STR);
+  config_line(H_DIR_STR, Z_HOME_DIR, Z_STR);
+
+  //
+  // XYZ Axis Jerk
+  //
+  #if HAS_CLASSIC_JERK
+    if (planner.max_jerk.x == planner.max_jerk.y)
+      config_line(PSTR("XY"), planner.max_jerk.x, JERK_STR);
+    else {
+      config_line(X_STR, planner.max_jerk.x, JERK_STR);
+      config_line(Y_STR, planner.max_jerk.y, JERK_STR);
+    }
+    config_line(Z_STR, planner.max_jerk.z, JERK_STR);
+  #endif
+
+  //
+  // Firmware Retraction
+  //
+  config_line(PSTR("SupportG10G11"), ENABLED(FWRETRACT));
+  #if ENABLED(FWRETRACT)
+    PGMSTR(RET_STR, "Retraction");
+    PGMSTR(UNRET_STR, "RetractionUndo");
+    PGMSTR(SPEED_STR, "Speed");
+    // M10 Retract with swap (long) moves
+    config_line(PSTR("Length"),     fwretract.settings.retract_length, RET_STR);
+    config_line(SPEED_STR,          fwretract.settings.retract_feedrate_mm_s, RET_STR);
+    config_line(PSTR("ZLift"),      fwretract.settings.retract_zraise, RET_STR);
+    config_line(PSTR("LongLength"), fwretract.settings.swap_retract_length, RET_STR);
+    // M11 Recover (undo) with swap (long) moves
+    config_line(SPEED_STR,               fwretract.settings.retract_recover_feedrate_mm_s, UNRET_STR);
+    config_line(PSTR("ExtraLength"),     fwretract.settings.retract_recover_extra, UNRET_STR);
+    config_line(PSTR("ExtraLongLength"), fwretract.settings.swap_retract_recover_extra, UNRET_STR);
+    config_line(PSTR("LongSpeed"),       fwretract.settings.swap_retract_recover_feedrate_mm_s, UNRET_STR);
+  #endif
+
+  //
+  // Workspace boundaries
+  //
+  const xyz_pos_t dmin = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS },
+                  dmax = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS };
+  xyz_pos_t cmin = dmin, cmax = dmax;
+  apply_motion_limits(cmin);
+  apply_motion_limits(cmax);
+  const xyz_pos_t wmin = cmin.asLogical(), wmax = cmax.asLogical();
+
+  PGMSTR(MIN_STR, "Min");
+  PGMSTR(MAX_STR, "Max");
+  PGMSTR(SIZE_STR, "Size");
+  config_line(MIN_STR, wmin.x, X_STR);
+  config_line(MIN_STR, wmin.y, Y_STR);
+  config_line(MIN_STR, wmin.z, Z_STR);
+  config_line(MAX_STR, wmax.x, X_STR);
+  config_line(MAX_STR, wmax.y, Y_STR);
+  config_line(MAX_STR, wmax.z, Z_STR);
+  config_line(SIZE_STR, wmax.x - wmin.x, X_STR);
+  config_line(SIZE_STR, wmax.y - wmin.y, Y_STR);
+  config_line(SIZE_STR, wmax.z - wmin.z, Z_STR);
+
+  //
+  // Print and Travel Acceleration
+  //
+  #define _ACCEL(A,B) _MIN(planner.settings.max_acceleration_mm_per_s2[A##_AXIS], planner.settings.B)
+  PGMSTR(P_ACC_STR, "PrintAccel");
+  PGMSTR(T_ACC_STR, "TravelAccel");
+  config_line(P_ACC_STR, _ACCEL(X, acceleration), X_STR);
+  config_line(P_ACC_STR, _ACCEL(Y, acceleration), Y_STR);
+  config_line(P_ACC_STR, _ACCEL(Z, acceleration), Z_STR);
+  config_line(T_ACC_STR, _ACCEL(X, travel_acceleration), X_STR);
+  config_line(T_ACC_STR, _ACCEL(Y, travel_acceleration), Y_STR);
+  config_line(T_ACC_STR, _ACCEL(Z, travel_acceleration), Z_STR);
+
+  config_prefix(PSTR("PrinterType"));
+  SERIAL_ECHOLNPGM(
+    TERN_(DELTA,         "Delta")
+    TERN_(IS_SCARA,      "SCARA")
+    TERN_(IS_CORE,       "Core")
+    TERN_(MARKFORGED_XY, "MarkForged")
+    TERN_(IS_CARTESIAN,  "Cartesian")
+  );
+
+  //
+  // Heated Bed
+  //
+  config_line(PSTR("HeatedBed"), ENABLED(HAS_HEATED_BED));
+  #if HAS_HEATED_BED
+    config_line(PSTR("MaxBedTemp"), BED_MAX_TARGET);
+  #endif
+
+  //
+  // Per-Extruder settings
+  //
+  config_line(PSTR("NumExtruder"), EXTRUDERS);
+  #if EXTRUDERS
+    LOOP_L_N(e, EXTRUDERS) {
+      config_line_e(e, JERK_STR, TERN(HAS_LINEAR_E_JERK, planner.max_e_jerk[E_INDEX_N(e)], TERN(HAS_CLASSIC_JERK, planner.max_jerk.e, DEFAULT_EJERK)));
+      config_line_e(e, PSTR("MaxSpeed"), planner.settings.max_feedrate_mm_s[E_AXIS_N(e)]);
+      config_line_e(e, PSTR("Acceleration"), planner.settings.max_acceleration_mm_per_s2[E_AXIS_N(e)]);
+      config_line_e(e, PSTR("Diameter"), TERN(NO_VOLUMETRICS, DEFAULT_NOMINAL_FILAMENT_DIA, planner.filament_size[e]));
+      config_line_e(e, PSTR("MaxTemp"), thermalManager.heater_maxtemp[e]);
+    }
+  #endif
+}
+
+#endif
diff --git a/Marlin/src/gcode/host/M876.cpp b/Marlin/src/gcode/host/M876.cpp
new file mode 100644
index 0000000..0d8256c
--- /dev/null
+++ b/Marlin/src/gcode/host/M876.cpp
@@ -0,0 +1,39 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(HOST_PROMPT_SUPPORT) && DISABLED(EMERGENCY_PARSER)
+
+#include "../../feature/host_actions.h"
+#include "../gcode.h"
+#include "../../MarlinCore.h"
+
+/**
+ * M876: Handle Prompt Response
+ */
+void GcodeSuite::M876() {
+
+  if (parser.seenval('S')) host_response_handler((uint8_t)parser.value_int());
+
+}
+
+#endif // HOST_PROMPT_SUPPORT && !EMERGENCY_PARSER
diff --git a/Marlin/src/gcode/lcd/M0_M1.cpp b/Marlin/src/gcode/lcd/M0_M1.cpp
new file mode 100644
index 0000000..414c4ce
--- /dev/null
+++ b/Marlin/src/gcode/lcd/M0_M1.cpp
@@ -0,0 +1,87 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if HAS_RESUME_CONTINUE
+
+#include "../../inc/MarlinConfig.h"
+
+#include "../gcode.h"
+
+#include "../../module/planner.h" // for synchronize()
+#include "../../MarlinCore.h"     // for wait_for_user_response()
+
+#if HAS_LCD_MENU
+  #include "../../lcd/marlinui.h"
+#elif ENABLED(EXTENSIBLE_UI)
+  #include "../../lcd/extui/ui_api.h"
+#endif
+
+#if ENABLED(HOST_PROMPT_SUPPORT)
+  #include "../../feature/host_actions.h"
+#endif
+
+/**
+ * M0: Unconditional stop - Wait for user button press on LCD
+ * M1: Conditional stop   - Wait for user button press on LCD
+ */
+void GcodeSuite::M0_M1() {
+  millis_t ms = 0;
+  if (parser.seenval('P')) ms = parser.value_millis();              // Milliseconds to wait
+  if (parser.seenval('S')) ms = parser.value_millis_from_seconds(); // Seconds to wait
+
+  planner.synchronize();
+
+  #if HAS_LCD_MENU
+
+    if (parser.string_arg)
+      ui.set_status(parser.string_arg, true);
+    else {
+      LCD_MESSAGEPGM(MSG_USERWAIT);
+      #if ENABLED(LCD_PROGRESS_BAR) && PROGRESS_MSG_EXPIRE > 0
+        ui.reset_progress_bar_timeout();
+      #endif
+    }
+
+  #elif ENABLED(EXTENSIBLE_UI)
+    if (parser.string_arg)
+      ExtUI::onUserConfirmRequired(parser.string_arg); // Can this take an SRAM string??
+    else
+      ExtUI::onUserConfirmRequired_P(GET_TEXT(MSG_USERWAIT));
+  #else
+
+    if (parser.string_arg) {
+      SERIAL_ECHO_START();
+      SERIAL_ECHOLN(parser.string_arg);
+    }
+
+  #endif
+
+  TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, parser.codenum ? PSTR("M1 Stop") : PSTR("M0 Stop"), CONTINUE_STR));
+
+  wait_for_user_response(ms);
+
+  TERN_(HAS_LCD_MENU, ui.reset_status());
+}
+
+#endif // HAS_RESUME_CONTINUE
diff --git a/Marlin/src/gcode/lcd/M117.cpp b/Marlin/src/gcode/lcd/M117.cpp
new file mode 100644
index 0000000..59305d9
--- /dev/null
+++ b/Marlin/src/gcode/lcd/M117.cpp
@@ -0,0 +1,36 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../lcd/marlinui.h"
+
+/**
+ * M117: Set LCD Status Message
+ */
+void GcodeSuite::M117() {
+
+  if (parser.string_arg && parser.string_arg[0])
+    ui.set_status(parser.string_arg);
+  else
+    ui.reset_status();
+
+}
diff --git a/Marlin/src/gcode/lcd/M145.cpp b/Marlin/src/gcode/lcd/M145.cpp
new file mode 100644
index 0000000..84a7e75
--- /dev/null
+++ b/Marlin/src/gcode/lcd/M145.cpp
@@ -0,0 +1,59 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if PREHEAT_COUNT
+
+#include "../gcode.h"
+#include "../../lcd/marlinui.h"
+
+/**
+ * M145: Set the heatup state for a material in the LCD menu
+ *
+ *   S<material>
+ *   H<hotend temp>
+ *   B<bed temp>
+ *   F<fan speed>
+ */
+void GcodeSuite::M145() {
+  const uint8_t material = (uint8_t)parser.intval('S');
+  if (material >= PREHEAT_COUNT)
+    SERIAL_ERROR_MSG(STR_ERR_MATERIAL_INDEX);
+  else {
+    preheat_t &mat = ui.material_preset[material];
+    #if HAS_HOTEND
+      if (parser.seenval('H'))
+        mat.hotend_temp = constrain(parser.value_int(), EXTRUDE_MINTEMP, (HEATER_0_MAXTEMP) - (HOTEND_OVERSHOOT));
+    #endif
+    #if HAS_HEATED_BED
+      if (parser.seenval('B'))
+        mat.bed_temp = constrain(parser.value_int(), BED_MINTEMP, BED_MAX_TARGET);
+    #endif
+    #if HAS_FAN
+      if (parser.seenval('F'))
+        mat.fan_speed = constrain(parser.value_int(), 0, 255);
+    #endif
+  }
+}
+
+#endif // PREHEAT_COUNT
diff --git a/Marlin/src/gcode/lcd/M250.cpp b/Marlin/src/gcode/lcd/M250.cpp
new file mode 100644
index 0000000..f553044
--- /dev/null
+++ b/Marlin/src/gcode/lcd/M250.cpp
@@ -0,0 +1,38 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_LCD_CONTRAST
+
+#include "../gcode.h"
+#include "../../lcd/marlinui.h"
+
+/**
+ * M250: Read and optionally set the LCD contrast
+ */
+void GcodeSuite::M250() {
+  if (parser.seen('C')) ui.set_contrast(parser.value_int());
+  SERIAL_ECHOLNPAIR("LCD Contrast: ", ui.contrast);
+}
+
+#endif // HAS_LCD_CONTRAST
diff --git a/Marlin/src/gcode/lcd/M300.cpp b/Marlin/src/gcode/lcd/M300.cpp
new file mode 100644
index 0000000..5250774
--- /dev/null
+++ b/Marlin/src/gcode/lcd/M300.cpp
@@ -0,0 +1,45 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_BUZZER
+
+#include "../gcode.h"
+
+#include "../../lcd/marlinui.h" // i2c-based BUZZ
+#include "../../libs/buzzer.h"  // Buzzer, if possible
+
+/**
+ * M300: Play beep sound S<frequency Hz> P<duration ms>
+ */
+void GcodeSuite::M300() {
+  uint16_t const frequency = parser.ushortval('S', 260);
+  uint16_t duration = parser.ushortval('P', 1000);
+
+  // Limits the tone duration to 0-5 seconds.
+  NOMORE(duration, 5000U);
+
+  BUZZ(duration, frequency);
+}
+
+#endif // HAS_BUZZER
diff --git a/Marlin/src/gcode/lcd/M414.cpp b/Marlin/src/gcode/lcd/M414.cpp
new file mode 100644
index 0000000..760028a
--- /dev/null
+++ b/Marlin/src/gcode/lcd/M414.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 <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_MULTI_LANGUAGE
+
+#include "../gcode.h"
+#include "../../MarlinCore.h"
+#include "../../lcd/marlinui.h"
+
+/**
+ * M414: Set the language for the UI
+ *
+ * Parameters
+ *  S<index> : The language to select
+ */
+void GcodeSuite::M414() {
+
+  if (parser.seenval('S'))
+    ui.set_language(parser.value_byte());
+
+}
+
+#endif // HAS_MULTI_LANGUAGE
diff --git a/Marlin/src/gcode/lcd/M73.cpp b/Marlin/src/gcode/lcd/M73.cpp
new file mode 100644
index 0000000..5b135bd
--- /dev/null
+++ b/Marlin/src/gcode/lcd/M73.cpp
@@ -0,0 +1,48 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(LCD_SET_PROGRESS_MANUALLY)
+
+#include "../gcode.h"
+#include "../../lcd/marlinui.h"
+#include "../../sd/cardreader.h"
+
+/**
+ * M73: Set percentage complete (for display on LCD)
+ *
+ * Example:
+ *   M73 P25 ; Set progress to 25%
+ */
+void GcodeSuite::M73() {
+  if (parser.seen('P'))
+    ui.set_progress((PROGRESS_SCALE) > 1
+      ? parser.value_float() * (PROGRESS_SCALE)
+      : parser.value_byte()
+    );
+  #if BOTH(LCD_SET_PROGRESS_MANUALLY, USE_M73_REMAINING_TIME)
+    if (parser.seen('R')) ui.set_remaining_time(60 * parser.value_ulong());
+  #endif
+}
+
+#endif // LCD_SET_PROGRESS_MANUALLY
diff --git a/Marlin/src/gcode/lcd/M995.cpp b/Marlin/src/gcode/lcd/M995.cpp
new file mode 100644
index 0000000..bc8dc35
--- /dev/null
+++ b/Marlin/src/gcode/lcd/M995.cpp
@@ -0,0 +1,48 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(TOUCH_SCREEN_CALIBRATION)
+
+#include "../gcode.h"
+
+#if ENABLED(TFT_LVGL_UI)
+  #include "../../lcd/extui/lib/mks_ui/draw_touch_calibration.h"
+#else
+  #include "../../lcd/menu/menu.h"
+#endif
+
+/**
+ * M995: Touch screen calibration for TFT display
+ */
+void GcodeSuite::M995() {
+
+  #if ENABLED(TFT_LVGL_UI)
+    lv_draw_touch_calibration_screen();
+  #else
+    ui.goto_screen(touch_screen_calibration);
+  #endif
+
+}
+
+#endif // TOUCH_SCREEN_CALIBRATION
diff --git a/Marlin/src/gcode/motion/G0_G1.cpp b/Marlin/src/gcode/motion/G0_G1.cpp
new file mode 100644
index 0000000..9ac49bd
--- /dev/null
+++ b/Marlin/src/gcode/motion/G0_G1.cpp
@@ -0,0 +1,121 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+
+#include "../../MarlinCore.h"
+
+#if BOTH(FWRETRACT, FWRETRACT_AUTORETRACT)
+  #include "../../feature/fwretract.h"
+#endif
+
+#include "../../sd/cardreader.h"
+
+#if ENABLED(NANODLP_Z_SYNC)
+  #include "../../module/stepper.h"
+#endif
+
+extern xyze_pos_t destination;
+
+#if ENABLED(VARIABLE_G0_FEEDRATE)
+  feedRate_t fast_move_feedrate = MMM_TO_MMS(G0_FEEDRATE);
+#endif
+
+/**
+ * G0, G1: Coordinated movement of X Y Z E axes
+ */
+void GcodeSuite::G0_G1(TERN_(HAS_FAST_MOVES, const bool fast_move/*=false*/)) {
+
+  if (IsRunning()
+    #if ENABLED(NO_MOTION_BEFORE_HOMING)
+      && !homing_needed_error(
+          (parser.seen('X') ? _BV(X_AXIS) : 0)
+        | (parser.seen('Y') ? _BV(Y_AXIS) : 0)
+        | (parser.seen('Z') ? _BV(Z_AXIS) : 0) )
+    #endif
+  ) {
+
+    #ifdef G0_FEEDRATE
+      feedRate_t old_feedrate;
+      #if ENABLED(VARIABLE_G0_FEEDRATE)
+        if (fast_move) {
+          old_feedrate = feedrate_mm_s;             // Back up the (old) motion mode feedrate
+          feedrate_mm_s = fast_move_feedrate;       // Get G0 feedrate from last usage
+        }
+      #endif
+    #endif
+
+    get_destination_from_command();                 // Get X Y Z E F (and set cutter power)
+
+    #ifdef G0_FEEDRATE
+      if (fast_move) {
+        #if ENABLED(VARIABLE_G0_FEEDRATE)
+          fast_move_feedrate = feedrate_mm_s;       // Save feedrate for the next G0
+        #else
+          old_feedrate = feedrate_mm_s;             // Back up the (new) motion mode feedrate
+          feedrate_mm_s = MMM_TO_MMS(G0_FEEDRATE);  // Get the fixed G0 feedrate
+        #endif
+      }
+    #endif
+
+    #if BOTH(FWRETRACT, FWRETRACT_AUTORETRACT)
+
+      if (MIN_AUTORETRACT <= MAX_AUTORETRACT) {
+        // When M209 Autoretract is enabled, convert E-only moves to firmware retract/recover moves
+        if (fwretract.autoretract_enabled && parser.seen('E') && !(parser.seen('X') || parser.seen('Y') || parser.seen('Z'))) {
+          const float echange = destination.e - current_position.e;
+          // Is this a retract or recover move?
+          if (WITHIN(ABS(echange), MIN_AUTORETRACT, MAX_AUTORETRACT) && fwretract.retracted[active_extruder] == (echange > 0.0)) {
+            current_position.e = destination.e;       // Hide a G1-based retract/recover from calculations
+            sync_plan_position_e();                   // AND from the planner
+            return fwretract.retract(echange < 0.0);  // Firmware-based retract/recover (double-retract ignored)
+          }
+        }
+      }
+
+    #endif // FWRETRACT
+
+    #if IS_SCARA
+      fast_move ? prepare_fast_move_to_destination() : prepare_line_to_destination();
+    #else
+      prepare_line_to_destination();
+    #endif
+
+    #ifdef G0_FEEDRATE
+      // Restore the motion mode feedrate
+      if (fast_move) feedrate_mm_s = old_feedrate;
+    #endif
+
+    #if ENABLED(NANODLP_Z_SYNC)
+      #if ENABLED(NANODLP_ALL_AXIS)
+        #define _MOVE_SYNC parser.seenval('X') || parser.seenval('Y') || parser.seenval('Z')  // For any move wait and output sync message
+      #else
+        #define _MOVE_SYNC parser.seenval('Z')  // Only for Z move
+      #endif
+      if (_MOVE_SYNC) {
+        planner.synchronize();
+        SERIAL_ECHOLNPGM(STR_Z_MOVE_COMP);
+      }
+    #endif
+  }
+}
diff --git a/Marlin/src/gcode/motion/G2_G3.cpp b/Marlin/src/gcode/motion/G2_G3.cpp
new file mode 100644
index 0000000..61e5024
--- /dev/null
+++ b/Marlin/src/gcode/motion/G2_G3.cpp
@@ -0,0 +1,370 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(ARC_SUPPORT)
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/planner.h"
+#include "../../module/temperature.h"
+
+#if ENABLED(DELTA)
+  #include "../../module/delta.h"
+#elif ENABLED(SCARA)
+  #include "../../module/scara.h"
+#endif
+
+#if N_ARC_CORRECTION < 1
+  #undef N_ARC_CORRECTION
+  #define N_ARC_CORRECTION 1
+#endif
+
+/**
+ * Plan an arc in 2 dimensions, with optional linear motion in a 3rd dimension
+ *
+ * The arc is traced by generating many small linear segments, as configured by
+ * MM_PER_ARC_SEGMENT (Default 1mm). In the future we hope more slicers will include
+ * an option to generate G2/G3 arcs for curved surfaces, as this will allow faster
+ * boards to produce much smoother curved surfaces.
+ */
+void plan_arc(
+  const xyze_pos_t &cart,   // Destination position
+  const ab_float_t &offset, // Center of rotation relative to current_position
+  const bool clockwise,     // Clockwise?
+  const uint8_t circles     // Take the scenic route
+) {
+  #if ENABLED(CNC_WORKSPACE_PLANES)
+    AxisEnum p_axis, q_axis, l_axis;
+    switch (gcode.workspace_plane) {
+      default:
+      case GcodeSuite::PLANE_XY: p_axis = X_AXIS; q_axis = Y_AXIS; l_axis = Z_AXIS; break;
+      case GcodeSuite::PLANE_YZ: p_axis = Y_AXIS; q_axis = Z_AXIS; l_axis = X_AXIS; break;
+      case GcodeSuite::PLANE_ZX: p_axis = Z_AXIS; q_axis = X_AXIS; l_axis = Y_AXIS; break;
+    }
+  #else
+    constexpr AxisEnum p_axis = X_AXIS, q_axis = Y_AXIS, l_axis = Z_AXIS;
+  #endif
+
+  // Radius vector from center to current location
+  ab_float_t rvec = -offset;
+
+  const float radius = HYPOT(rvec.a, rvec.b),
+              center_P = current_position[p_axis] - rvec.a,
+              center_Q = current_position[q_axis] - rvec.b,
+              rt_X = cart[p_axis] - center_P,
+              rt_Y = cart[q_axis] - center_Q,
+              start_L = current_position[l_axis];
+
+  #ifdef MIN_ARC_SEGMENTS
+    uint16_t min_segments = MIN_ARC_SEGMENTS;
+  #else
+    constexpr uint16_t min_segments = 1;
+  #endif
+
+  // Angle of rotation between position and target from the circle center.
+  float angular_travel;
+
+  // Do a full circle if starting and ending positions are "identical"
+  if (NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis])) {
+    // Preserve direction for circles
+    angular_travel = clockwise ? -RADIANS(360) : RADIANS(360);
+  }
+  else {
+    // Calculate the angle
+    angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y);
+
+    // Angular travel too small to detect? Just return.
+    if (!angular_travel) return;
+
+    // Make sure angular travel over 180 degrees goes the other way around.
+    switch (((angular_travel < 0) << 1) | clockwise) {
+      case 1: angular_travel -= RADIANS(360); break; // Positive but CW? Reverse direction.
+      case 2: angular_travel += RADIANS(360); break; // Negative but CCW? Reverse direction.
+    }
+
+    #ifdef MIN_ARC_SEGMENTS
+      min_segments = CEIL(min_segments * ABS(angular_travel) / RADIANS(360));
+      NOLESS(min_segments, 1U);
+    #endif
+  }
+
+  float linear_travel = cart[l_axis] - start_L,
+        extruder_travel = cart.e - current_position.e;
+
+  // If circling around...
+  if (ENABLED(ARC_P_CIRCLES) && circles) {
+    const float total_angular = angular_travel + circles * RADIANS(360),  // Total rotation with all circles and remainder
+              part_per_circle = RADIANS(360) / total_angular,             // Each circle's part of the total
+                 l_per_circle = linear_travel * part_per_circle,          // L movement per circle
+                 e_per_circle = extruder_travel * part_per_circle;        // E movement per circle
+    xyze_pos_t temp_position = current_position;                          // for plan_arc to compare to current_position
+    for (uint16_t n = circles; n--;) {
+      temp_position.e += e_per_circle;                                    // Destination E axis
+      temp_position[l_axis] += l_per_circle;                              // Destination L axis
+      plan_arc(temp_position, offset, clockwise, 0);                      // Plan a single whole circle
+    }
+    linear_travel = cart[l_axis] - current_position[l_axis];
+    extruder_travel = cart.e - current_position.e;
+  }
+
+  const float flat_mm = radius * angular_travel,
+              mm_of_travel = linear_travel ? HYPOT(flat_mm, linear_travel) : ABS(flat_mm);
+  if (mm_of_travel < 0.001f) return;
+
+  const feedRate_t scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
+
+  // Start with a nominal segment length
+  float seg_length = (
+    #ifdef ARC_SEGMENTS_PER_R
+      constrain(MM_PER_ARC_SEGMENT * radius, MM_PER_ARC_SEGMENT, ARC_SEGMENTS_PER_R)
+    #elif ARC_SEGMENTS_PER_SEC
+      _MAX(scaled_fr_mm_s * RECIPROCAL(ARC_SEGMENTS_PER_SEC), MM_PER_ARC_SEGMENT)
+    #else
+      MM_PER_ARC_SEGMENT
+    #endif
+  );
+  // Divide total travel by nominal segment length
+  uint16_t segments = FLOOR(mm_of_travel / seg_length);
+  NOLESS(segments, min_segments);         // At least some segments
+  seg_length = mm_of_travel / segments;
+
+  /**
+   * Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
+   * and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
+   *     r_T = [cos(phi) -sin(phi);
+   *            sin(phi)  cos(phi)] * r ;
+   *
+   * For arc generation, the center of the circle is the axis of rotation and the radius vector is
+   * defined from the circle center to the initial position. Each line segment is formed by successive
+   * vector rotations. This requires only two cos() and sin() computations to form the rotation
+   * matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
+   * all double numbers are single precision on the Arduino. (True double precision will not have
+   * round off issues for CNC applications.) Single precision error can accumulate to be greater than
+   * tool precision in some cases. Therefore, arc path correction is implemented.
+   *
+   * Small angle approximation may be used to reduce computation overhead further. This approximation
+   * holds for everything, but very small circles and large MM_PER_ARC_SEGMENT values. In other words,
+   * theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
+   * to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
+   * numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
+   * issue for CNC machines with the single precision Arduino calculations.
+   *
+   * This approximation also allows plan_arc to immediately insert a line segment into the planner
+   * without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
+   * a correction, the planner should have caught up to the lag caused by the initial plan_arc overhead.
+   * This is important when there are successive arc motions.
+   */
+  // Vector rotation matrix values
+  xyze_pos_t raw;
+  const float theta_per_segment = angular_travel / segments,
+              linear_per_segment = linear_travel / segments,
+              extruder_per_segment = extruder_travel / segments,
+              sq_theta_per_segment = sq(theta_per_segment),
+              sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
+              cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation
+
+  // Initialize the linear axis
+  raw[l_axis] = current_position[l_axis];
+
+  // Initialize the extruder axis
+  raw.e = current_position.e;
+
+  #if ENABLED(SCARA_FEEDRATE_SCALING)
+    const float inv_duration = scaled_fr_mm_s / seg_length;
+  #endif
+
+  millis_t next_idle_ms = millis() + 200UL;
+
+  #if N_ARC_CORRECTION > 1
+    int8_t arc_recalc_count = N_ARC_CORRECTION;
+  #endif
+
+  for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
+
+    thermalManager.manage_heater();
+    if (ELAPSED(millis(), next_idle_ms)) {
+      next_idle_ms = millis() + 200UL;
+      idle();
+    }
+
+    #if N_ARC_CORRECTION > 1
+      if (--arc_recalc_count) {
+        // Apply vector rotation matrix to previous rvec.a / 1
+        const float r_new_Y = rvec.a * sin_T + rvec.b * cos_T;
+        rvec.a = rvec.a * cos_T - rvec.b * sin_T;
+        rvec.b = r_new_Y;
+      }
+      else
+    #endif
+    {
+      #if N_ARC_CORRECTION > 1
+        arc_recalc_count = N_ARC_CORRECTION;
+      #endif
+
+      // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
+      // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
+      // To reduce stuttering, the sin and cos could be computed at different times.
+      // For now, compute both at the same time.
+      const float cos_Ti = cos(i * theta_per_segment), sin_Ti = sin(i * theta_per_segment);
+      rvec.a = -offset[0] * cos_Ti + offset[1] * sin_Ti;
+      rvec.b = -offset[0] * sin_Ti - offset[1] * cos_Ti;
+    }
+
+    // Update raw location
+    raw[p_axis] = center_P + rvec.a;
+    raw[q_axis] = center_Q + rvec.b;
+    #if ENABLED(AUTO_BED_LEVELING_UBL)
+      raw[l_axis] = start_L;
+      UNUSED(linear_per_segment);
+    #else
+      raw[l_axis] += linear_per_segment;
+    #endif
+    raw.e += extruder_per_segment;
+
+    apply_motion_limits(raw);
+
+    #if HAS_LEVELING && !PLANNER_LEVELING
+      planner.apply_leveling(raw);
+    #endif
+
+    if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0
+      #if ENABLED(SCARA_FEEDRATE_SCALING)
+        , inv_duration
+      #endif
+    )) break;
+  }
+
+  // Ensure last segment arrives at target location.
+  raw = cart;
+  TERN_(AUTO_BED_LEVELING_UBL, raw[l_axis] = start_L);
+
+  apply_motion_limits(raw);
+
+  #if HAS_LEVELING && !PLANNER_LEVELING
+    planner.apply_leveling(raw);
+  #endif
+
+  planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0
+    #if ENABLED(SCARA_FEEDRATE_SCALING)
+      , inv_duration
+    #endif
+  );
+
+  TERN_(AUTO_BED_LEVELING_UBL, raw[l_axis] = start_L);
+  current_position = raw;
+
+} // plan_arc
+
+/**
+ * G2: Clockwise Arc
+ * G3: Counterclockwise Arc
+ *
+ * This command has two forms: IJ-form (JK, KI) and R-form.
+ *
+ *  - Depending on the current Workspace Plane orientation,
+ *    use parameters IJ/JK/KI to specify the XY/YZ/ZX offsets.
+ *    At least one of the IJ/JK/KI parameters is required.
+ *    XY/YZ/ZX can be omitted to do a complete circle.
+ *    The given XY/YZ/ZX is not error-checked. The arc ends
+ *    based on the angle of the destination.
+ *    Mixing IJ/JK/KI with R will throw an error.
+ *
+ *  - R specifies the radius. X or Y (Y or Z / Z or X) is required.
+ *    Omitting both XY/YZ/ZX will throw an error.
+ *    XY/YZ/ZX must differ from the current XY/YZ/ZX.
+ *    Mixing R with IJ/JK/KI will throw an error.
+ *
+ *  - P specifies the number of full circles to do
+ *    before the specified arc move.
+ *
+ *  Examples:
+ *
+ *    G2 I10           ; CW circle centered at X+10
+ *    G3 X20 Y12 R14   ; CCW circle with r=14 ending at X20 Y12
+ */
+void GcodeSuite::G2_G3(const bool clockwise) {
+  if (MOTION_CONDITIONS) {
+
+    #if ENABLED(SF_ARC_FIX)
+      const bool relative_mode_backup = relative_mode;
+      relative_mode = true;
+    #endif
+
+    get_destination_from_command();   // Get X Y Z E F (and set cutter power)
+
+    TERN_(SF_ARC_FIX, relative_mode = relative_mode_backup);
+
+    ab_float_t arc_offset = { 0, 0 };
+    if (parser.seenval('R')) {
+      const float r = parser.value_linear_units();
+      if (r) {
+        const xy_pos_t p1 = current_position, p2 = destination;
+        if (p1 != p2) {
+          const xy_pos_t d2 = (p2 - p1) * 0.5f;          // XY vector to midpoint of move from current
+          const float e = clockwise ^ (r < 0) ? -1 : 1,  // clockwise -1/1, counterclockwise 1/-1
+                      len = d2.magnitude(),              // Distance to mid-point of move from current
+                      h2 = (r - len) * (r + len),        // factored to reduce rounding error
+                      h = (h2 >= 0) ? SQRT(h2) : 0.0f;   // Distance to the arc pivot-point from midpoint
+          const xy_pos_t s = { -d2.y, d2.x };            // Perpendicular bisector. (Divide by len for unit vector.)
+          arc_offset = d2 + s / len * e * h;             // The calculated offset (mid-point if |r| <= len)
+        }
+      }
+    }
+    else {
+      #if ENABLED(CNC_WORKSPACE_PLANES)
+        char achar, bchar;
+        switch (gcode.workspace_plane) {
+          default:
+          case GcodeSuite::PLANE_XY: achar = 'I'; bchar = 'J'; break;
+          case GcodeSuite::PLANE_YZ: achar = 'J'; bchar = 'K'; break;
+          case GcodeSuite::PLANE_ZX: achar = 'K'; bchar = 'I'; break;
+        }
+      #else
+        constexpr char achar = 'I', bchar = 'J';
+      #endif
+      if (parser.seenval(achar)) arc_offset.a = parser.value_linear_units();
+      if (parser.seenval(bchar)) arc_offset.b = parser.value_linear_units();
+    }
+
+    if (arc_offset) {
+
+      #if ENABLED(ARC_P_CIRCLES)
+        // P indicates number of circles to do
+        const int8_t circles_to_do = parser.byteval('P');
+        if (!WITHIN(circles_to_do, 0, 100))
+          SERIAL_ERROR_MSG(STR_ERR_ARC_ARGS);
+      #else
+        constexpr uint8_t circles_to_do = 0;
+      #endif
+
+      // Send the arc to the planner
+      plan_arc(destination, arc_offset, clockwise, circles_to_do);
+      reset_stepper_timeout();
+    }
+    else
+      SERIAL_ERROR_MSG(STR_ERR_ARC_ARGS);
+  }
+}
+
+#endif // ARC_SUPPORT
diff --git a/Marlin/src/gcode/motion/G4.cpp b/Marlin/src/gcode/motion/G4.cpp
new file mode 100644
index 0000000..724ed7f
--- /dev/null
+++ b/Marlin/src/gcode/motion/G4.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 <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../module/stepper.h"
+#include "../../lcd/marlinui.h"
+
+/**
+ * G4: Dwell S<seconds> or P<milliseconds>
+ */
+void GcodeSuite::G4() {
+  millis_t dwell_ms = 0;
+
+  if (parser.seenval('P')) dwell_ms = parser.value_millis(); // milliseconds to wait
+  if (parser.seenval('S')) dwell_ms = parser.value_millis_from_seconds(); // seconds to wait
+
+  planner.synchronize();
+  #if ENABLED(NANODLP_Z_SYNC)
+    SERIAL_ECHOLNPGM(STR_Z_MOVE_COMP);
+  #endif
+
+  if (!ui.has_status()) LCD_MESSAGEPGM(MSG_DWELL);
+
+  dwell(dwell_ms);
+}
diff --git a/Marlin/src/gcode/motion/G5.cpp b/Marlin/src/gcode/motion/G5.cpp
new file mode 100644
index 0000000..2c98fae
--- /dev/null
+++ b/Marlin/src/gcode/motion/G5.cpp
@@ -0,0 +1,65 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(BEZIER_CURVE_SUPPORT)
+
+#include "../../module/motion.h"
+#include "../../module/planner_bezier.h"
+
+/**
+ * Parameters interpreted according to:
+ * https://linuxcnc.org/docs/2.7/html/gcode/g-code.html#gcode:g5
+ * However I, J omission is not supported at this point; all
+ * parameters can be omitted and default to zero.
+ */
+
+#include "../gcode.h"
+#include "../../MarlinCore.h" // for IsRunning()
+
+/**
+ * G5: Cubic B-spline
+ */
+void GcodeSuite::G5() {
+  if (MOTION_CONDITIONS) {
+
+    #if ENABLED(CNC_WORKSPACE_PLANES)
+      if (workspace_plane != PLANE_XY) {
+        SERIAL_ERROR_MSG(STR_ERR_BAD_PLANE_MODE);
+        return;
+      }
+    #endif
+
+    get_destination_from_command();
+
+    const xy_pos_t offsets[2] = {
+      { parser.linearval('I'), parser.linearval('J') },
+      { parser.linearval('P'), parser.linearval('Q') }
+    };
+
+    cubic_b_spline(current_position, destination, offsets, MMS_SCALED(feedrate_mm_s), active_extruder);
+    current_position = destination;
+  }
+}
+
+#endif // BEZIER_CURVE_SUPPORT
diff --git a/Marlin/src/gcode/motion/G6.cpp b/Marlin/src/gcode/motion/G6.cpp
new file mode 100644
index 0000000..168dc28
--- /dev/null
+++ b/Marlin/src/gcode/motion/G6.cpp
@@ -0,0 +1,61 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(DIRECT_STEPPING)
+
+#include "../../feature/direct_stepping.h"
+
+#include "../gcode.h"
+#include "../../module/planner.h"
+
+/**
+ * G6: Direct Stepper Move
+ */
+void GcodeSuite::G6() {
+  // TODO: feedrate support?
+  if (parser.seen('R'))
+    planner.last_page_step_rate = parser.value_ulong();
+
+  if (!DirectStepping::Config::DIRECTIONAL) {
+    if (parser.seen('X')) planner.last_page_dir.x = !!parser.value_byte();
+    if (parser.seen('Y')) planner.last_page_dir.y = !!parser.value_byte();
+    if (parser.seen('Z')) planner.last_page_dir.z = !!parser.value_byte();
+    if (parser.seen('E')) planner.last_page_dir.e = !!parser.value_byte();
+  }
+
+  // No index means we just set the state
+  if (!parser.seen('I')) return;
+
+  // No speed is set, can't schedule the move
+  if (!planner.last_page_step_rate) return;
+
+  const page_idx_t page_idx = (page_idx_t) parser.value_ulong();
+
+  uint16_t num_steps = DirectStepping::Config::TOTAL_STEPS;
+  if (parser.seen('S')) num_steps = parser.value_ushort();
+
+  planner.buffer_page(page_idx, 0, num_steps);
+  reset_stepper_timeout();
+}
+
+#endif // DIRECT_STEPPING
diff --git a/Marlin/src/gcode/motion/G80.cpp b/Marlin/src/gcode/motion/G80.cpp
new file mode 100644
index 0000000..f674596
--- /dev/null
+++ b/Marlin/src/gcode/motion/G80.cpp
@@ -0,0 +1,38 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if ENABLED(GCODE_MOTION_MODES)
+
+#include "../gcode.h"
+
+/**
+ * G80: Cancel current motion mode
+ */
+void GcodeSuite::G80() {
+
+  parser.cancel_motion_mode();
+
+}
+
+#endif // GCODE_MOTION_MODES
diff --git a/Marlin/src/gcode/motion/M290.cpp b/Marlin/src/gcode/motion/M290.cpp
new file mode 100644
index 0000000..df8dad7
--- /dev/null
+++ b/Marlin/src/gcode/motion/M290.cpp
@@ -0,0 +1,136 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(BABYSTEPPING)
+
+#include "../gcode.h"
+#include "../../feature/babystep.h"
+#include "../../module/probe.h"
+#include "../../module/planner.h"
+
+#if ENABLED(BABYSTEP_ZPROBE_OFFSET)
+  #include "../../core/serial.h"
+#endif
+
+#if ENABLED(MESH_BED_LEVELING)
+  #include "../../feature/bedlevel/bedlevel.h"
+#endif
+
+#if ENABLED(BABYSTEP_ZPROBE_OFFSET)
+
+  FORCE_INLINE void mod_probe_offset(const float &offs) {
+    if (TERN1(BABYSTEP_HOTEND_Z_OFFSET, active_extruder == 0)) {
+      probe.offset.z += offs;
+      SERIAL_ECHO_START();
+      SERIAL_ECHOLNPAIR(STR_PROBE_OFFSET " " STR_Z, probe.offset.z);
+    }
+    else {
+      #if ENABLED(BABYSTEP_HOTEND_Z_OFFSET)
+        hotend_offset[active_extruder].z -= offs;
+        SERIAL_ECHO_START();
+        SERIAL_ECHOLNPAIR(STR_PROBE_OFFSET STR_Z ": ", hotend_offset[active_extruder].z);
+      #endif
+    }
+  }
+
+#endif
+
+/**
+ * M290: Babystepping
+ *
+ * Send 'R' or no parameters for a report.
+ *
+ *  X<linear> - Distance to step X
+ *  Y<linear> - Distance to step Y
+ *  Z<linear> - Distance to step Z
+ *  S<linear> - Distance to step Z (alias for Z)
+ *
+ * With BABYSTEP_ZPROBE_OFFSET:
+ *  P0 - Don't adjust the Z probe offset
+ */
+void GcodeSuite::M290() {
+  #if ENABLED(BABYSTEP_XY)
+    LOOP_XYZ(a)
+      if (parser.seenval(XYZ_CHAR(a)) || (a == Z_AXIS && parser.seenval('S'))) {
+        const float offs = constrain(parser.value_axis_units((AxisEnum)a), -2, 2);
+        babystep.add_mm((AxisEnum)a, offs);
+        #if ENABLED(BABYSTEP_ZPROBE_OFFSET)
+          if (a == Z_AXIS && (!parser.seen('P') || parser.value_bool())) mod_probe_offset(offs);
+        #endif
+      }
+  #else
+    if (parser.seenval('Z') || parser.seenval('S')) {
+      const float offs = constrain(parser.value_axis_units(Z_AXIS), -2, 2);
+      babystep.add_mm(Z_AXIS, offs);
+      #if ENABLED(BABYSTEP_ZPROBE_OFFSET)
+        if (!parser.seen('P') || parser.value_bool()) mod_probe_offset(offs);
+      #endif
+    }
+  #endif
+
+  if (!parser.seen("XYZ") || parser.seen('R')) {
+    SERIAL_ECHO_START();
+
+    #if ENABLED(BABYSTEP_ZPROBE_OFFSET)
+      SERIAL_ECHOLNPAIR(STR_PROBE_OFFSET " " STR_Z, probe.offset.z);
+    #endif
+
+    #if ENABLED(BABYSTEP_HOTEND_Z_OFFSET)
+    {
+      SERIAL_ECHOLNPAIR_P(
+        PSTR("Hotend "), int(active_extruder)
+        #if ENABLED(BABYSTEP_XY)
+          , PSTR("Offset X"), hotend_offset[active_extruder].x
+          , SP_Y_STR, hotend_offset[active_extruder].y
+          , SP_Z_STR
+        #else
+          , PSTR("Offset Z")
+        #endif
+        , hotend_offset[active_extruder].z
+      );
+    }
+    #endif
+
+    #if ENABLED(MESH_BED_LEVELING)
+      SERIAL_ECHOLNPAIR("MBL Adjust Z", mbl.z_offset);
+    #endif
+
+    #if ENABLED(BABYSTEP_DISPLAY_TOTAL)
+    {
+      SERIAL_ECHOLNPAIR_P(
+        #if ENABLED(BABYSTEP_XY)
+            PSTR("Babystep X"), babystep.axis_total[X_AXIS]
+          , SP_Y_STR, babystep.axis_total[Y_AXIS]
+          , SP_Z_STR
+        #else
+          PSTR("Babystep Z")
+        #endif
+        , babystep.axis_total[BS_TOTAL_IND(Z_AXIS)]
+      );
+    }
+    #endif
+  }
+}
+
+#endif // BABYSTEPPING
diff --git a/Marlin/src/gcode/parser.cpp b/Marlin/src/gcode/parser.cpp
new file mode 100644
index 0000000..a513c4b
--- /dev/null
+++ b/Marlin/src/gcode/parser.cpp
@@ -0,0 +1,406 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * parser.cpp - Parser for a GCode line, providing a parameter interface.
+ */
+
+#include "parser.h"
+
+#include "../MarlinCore.h"
+
+// Must be declared for allocation and to satisfy the linker
+// Zero values need no initialization.
+
+bool GCodeParser::volumetric_enabled;
+
+#if ENABLED(INCH_MODE_SUPPORT)
+  float GCodeParser::linear_unit_factor, GCodeParser::volumetric_unit_factor;
+#endif
+
+#if ENABLED(TEMPERATURE_UNITS_SUPPORT)
+  TempUnit GCodeParser::input_temp_units = TEMPUNIT_C;
+#endif
+
+char *GCodeParser::command_ptr,
+     *GCodeParser::string_arg,
+     *GCodeParser::value_ptr;
+char GCodeParser::command_letter;
+uint16_t GCodeParser::codenum;
+
+#if ENABLED(USE_GCODE_SUBCODES)
+  uint8_t GCodeParser::subcode;
+#endif
+
+#if ENABLED(GCODE_MOTION_MODES)
+  int16_t GCodeParser::motion_mode_codenum = -1;
+  #if ENABLED(USE_GCODE_SUBCODES)
+    uint8_t GCodeParser::motion_mode_subcode;
+  #endif
+#endif
+
+#if ENABLED(FASTER_GCODE_PARSER)
+  // Optimized Parameters
+  uint32_t GCodeParser::codebits;  // found bits
+  uint8_t GCodeParser::param[26];  // parameter offsets from command_ptr
+#else
+  char *GCodeParser::command_args; // start of parameters
+#endif
+
+// Create a global instance of the GCode parser singleton
+GCodeParser parser;
+
+/**
+ * Clear all code-seen (and value pointers)
+ *
+ * Since each param is set/cleared on seen codes,
+ * this may be optimized by commenting out ZERO(param)
+ */
+void GCodeParser::reset() {
+  string_arg = nullptr;                 // No whole line argument
+  command_letter = '?';                 // No command letter
+  codenum = 0;                          // No command code
+  TERN_(USE_GCODE_SUBCODES, subcode = 0); // No command sub-code
+  #if ENABLED(FASTER_GCODE_PARSER)
+    codebits = 0;                       // No codes yet
+    //ZERO(param);                      // No parameters (should be safe to comment out this line)
+  #endif
+}
+
+#if ENABLED(GCODE_QUOTED_STRINGS)
+
+  // Pass the address after the first quote (if any)
+  char* GCodeParser::unescape_string(char* &src) {
+    if (*src == '"') ++src;     // Skip the leading quote
+    char * const out = src;     // Start of the string
+    char *dst = src;            // Prepare to unescape and terminate
+    for (;;) {
+      char c = *src++;          // Get the next char
+      switch (c) {
+        case '\\': c = *src++; break; // Get the escaped char
+        case '"' : c = '\0'; break;   // Convert bare quote to nul
+      }
+      if (!(*dst++ = c)) break; // Copy and break on nul
+    }
+    return out;
+  }
+
+#endif
+
+// Populate all fields by parsing a single line of GCode
+// 58 bytes of SRAM are used to speed up seen/value
+void GCodeParser::parse(char *p) {
+
+  reset(); // No codes to report
+
+  auto uppercase = [](char c) {
+    if (TERN0(GCODE_CASE_INSENSITIVE, WITHIN(c, 'a', 'z')))
+      c += 'A' - 'a';
+    return c;
+  };
+
+  // Skip spaces
+  while (*p == ' ') ++p;
+
+  // Skip N[-0-9] if included in the command line
+  if (uppercase(*p) == 'N' && NUMERIC_SIGNED(p[1])) {
+    //TERN_(FASTER_GCODE_PARSER, set('N', p + 1)); // (optional) Set the 'N' parameter value
+    p += 2;                  // skip N[-0-9]
+    while (NUMERIC(*p)) ++p; // skip [0-9]*
+    while (*p == ' ')   ++p; // skip [ ]*
+  }
+
+  // *p now points to the current command, which should be G, M, or T
+  command_ptr = p;
+
+  // Get the command letter, which must be G, M, or T
+  const char letter = uppercase(*p++);
+
+  // Nullify asterisk and trailing whitespace
+  char *starpos = strchr(p, '*');
+  if (starpos) {
+    --starpos;                          // *
+    while (*starpos == ' ') --starpos;  // spaces...
+    starpos[1] = '\0';
+  }
+
+  #if ANY(MARLIN_DEV_MODE, SWITCHING_TOOLHEAD, MAGNETIC_SWITCHING_TOOLHEAD, ELECTROMAGNETIC_SWITCHING_TOOLHEAD)
+    #define SIGNED_CODENUM 1
+  #endif
+
+  // Bail if the letter is not G, M, or T
+  // (or a valid parameter for the current motion mode)
+  switch (letter) {
+
+    case 'G': case 'M': case 'T': TERN_(MARLIN_DEV_MODE, case 'D':)
+      // Skip spaces to get the numeric part
+      while (*p == ' ') p++;
+
+      #if HAS_PRUSA_MMU2
+        if (letter == 'T') {
+          // check for special MMU2 T?/Tx/Tc commands
+          if (*p == '?' || *p == 'x' || *p == 'c') {
+            command_letter = letter;
+            string_arg = p;
+            return;
+          }
+        }
+      #endif
+
+      // Bail if there's no command code number
+      if (!TERN(SIGNED_CODENUM, NUMERIC_SIGNED(*p), NUMERIC(*p))) return;
+
+      // Save the command letter at this point
+      // A '?' signifies an unknown command
+      command_letter = letter;
+
+      {
+        #if ENABLED(SIGNED_CODENUM)
+          int sign = 1; // Allow for a negative code like D-1 or T-1
+          if (*p == '-') { sign = -1; ++p; }
+        #endif
+
+        // Get the code number - integer digits only
+        codenum = 0;
+
+        do { codenum = codenum * 10 + *p++ - '0'; } while (NUMERIC(*p));
+
+        // Apply the sign, if any
+        TERN_(SIGNED_CODENUM, codenum *= sign);
+      }
+
+      // Allow for decimal point in command
+      #if ENABLED(USE_GCODE_SUBCODES)
+        if (*p == '.') {
+          p++;
+          while (NUMERIC(*p))
+            subcode = subcode * 10 + *p++ - '0';
+        }
+      #endif
+
+      // Skip all spaces to get to the first argument, or nul
+      while (*p == ' ') p++;
+
+      #if ENABLED(GCODE_MOTION_MODES)
+        if (letter == 'G'
+          && (codenum <= TERN(ARC_SUPPORT, 3, 1) || codenum == 5 || TERN0(G38_PROBE_TARGET, codenum == 38))
+        ) {
+          motion_mode_codenum = codenum;
+          TERN_(USE_GCODE_SUBCODES, motion_mode_subcode = subcode);
+        }
+      #endif
+
+      break;
+
+    #if ENABLED(GCODE_MOTION_MODES)
+      #if ENABLED(ARC_SUPPORT)
+        case 'I' ... 'J': case 'R':
+          if (motion_mode_codenum != 2 && motion_mode_codenum != 3) return;
+      #endif
+      case 'P' ... 'Q':
+        if (motion_mode_codenum != 5) return;
+      case 'X' ... 'Z': case 'E' ... 'F':
+        if (motion_mode_codenum < 0) return;
+        command_letter = 'G';
+        codenum = motion_mode_codenum;
+        TERN_(USE_GCODE_SUBCODES, subcode = motion_mode_subcode);
+        p--; // Back up one character to use the current parameter
+      break;
+    #endif // GCODE_MOTION_MODES
+
+    default: return;
+  }
+
+  // The command parameters (if any) start here, for sure!
+
+  #if DISABLED(FASTER_GCODE_PARSER)
+    command_args = p; // Scan for parameters in seen()
+  #endif
+
+  // Only use string_arg for these M codes
+  if (letter == 'M') switch (codenum) {
+    #if ENABLED(GCODE_MACROS)
+      case 810 ... 819:
+    #endif
+    #if ENABLED(EXPECTED_PRINTER_CHECK)
+      case 16:
+    #endif
+    case 23: case 28: case 30: case 117 ... 118: case 928:
+      string_arg = unescape_string(p);
+      return;
+    default: break;
+  }
+
+  #if ENABLED(DEBUG_GCODE_PARSER)
+    const bool debug = codenum == 800;
+  #endif
+
+  /**
+   * Find all parameters, set flags and pointers for fast parsing
+   *
+   * Most codes ignore 'string_arg', but those that want a string will get the right pointer.
+   * The following loop assigns the first "parameter" having no numeric value to 'string_arg'.
+   * This allows M0/M1 with expire time to work: "M0 S5 You Win!"
+   * For 'M118' you must use 'E1' and 'A1' rather than just 'E' or 'A'
+   */
+  #if ENABLED(GCODE_QUOTED_STRINGS)
+    bool quoted_string_arg = false;
+  #endif
+  string_arg = nullptr;
+  while (const char param = uppercase(*p++)) {  // Get the next parameter. A NUL ends the loop
+
+    // Special handling for M32 [P] !/path/to/file.g#
+    // The path must be the last parameter
+    if (param == '!' && is_command('M', 32)) {
+      string_arg = p;                           // Name starts after '!'
+      char * const lb = strchr(p, '#');         // Already seen '#' as SD char (to pause buffering)
+      if (lb) *lb = '\0';                       // Safe to mark the end of the filename
+      return;
+    }
+
+    #if ENABLED(GCODE_QUOTED_STRINGS)
+      if (!quoted_string_arg && param == '"') {
+        quoted_string_arg = true;
+        string_arg = unescape_string(p);
+      }
+    #endif
+
+    #if ENABLED(FASTER_GCODE_PARSER)
+      // Arguments MUST be uppercase for fast GCode parsing
+      #define PARAM_OK(P) WITHIN((P), 'A', 'Z')
+    #else
+      #define PARAM_OK(P) true
+    #endif
+
+    if (PARAM_OK(param)) {
+
+      while (*p == ' ') p++;                    // Skip spaces between parameters & values
+
+      #if ENABLED(GCODE_QUOTED_STRINGS)
+        const bool is_str = (*p == '"'), has_val = is_str || valid_float(p);
+        char * const valptr = has_val ? is_str ? unescape_string(p) : p : nullptr;
+      #else
+        const bool has_val = valid_float(p);
+        #if ENABLED(FASTER_GCODE_PARSER)
+          char * const valptr = has_val ? p : nullptr;
+        #endif
+      #endif
+
+      #if ENABLED(DEBUG_GCODE_PARSER)
+        if (debug) {
+          SERIAL_ECHOPAIR("Got param ", param, " at index ", (int)(p - command_ptr - 1));
+          if (has_val) SERIAL_ECHOPGM(" (has_val)");
+        }
+      #endif
+
+      if (!has_val && !string_arg) {            // No value? First time, keep as string_arg
+        string_arg = p - 1;
+        #if ENABLED(DEBUG_GCODE_PARSER)
+          if (debug) SERIAL_ECHOPAIR(" string_arg: ", hex_address((void*)string_arg)); // DEBUG
+        #endif
+      }
+
+      if (TERN0(DEBUG_GCODE_PARSER, debug)) SERIAL_EOL();
+
+      TERN_(FASTER_GCODE_PARSER, set(param, valptr)); // Set parameter exists and pointer (nullptr for no value)
+    }
+    else if (!string_arg) {                     // Not A-Z? First time, keep as the string_arg
+      string_arg = p - 1;
+      #if ENABLED(DEBUG_GCODE_PARSER)
+        if (debug) SERIAL_ECHOPAIR(" string_arg: ", hex_address((void*)string_arg)); // DEBUG
+      #endif
+    }
+
+    if (!WITHIN(*p, 'A', 'Z')) {                // Another parameter right away?
+      while (*p && DECIMAL_SIGNED(*p)) p++;     // Skip over the value section of a parameter
+      while (*p == ' ') p++;                    // Skip over all spaces
+    }
+  }
+}
+
+#if ENABLED(CNC_COORDINATE_SYSTEMS)
+
+  // Parse the next parameter as a new command
+  bool GCodeParser::chain() {
+    #if ENABLED(FASTER_GCODE_PARSER)
+      char *next_command = command_ptr;
+      if (next_command) {
+        while (*next_command && *next_command != ' ') ++next_command;
+        while (*next_command == ' ') ++next_command;
+        if (!*next_command) next_command = nullptr;
+      }
+    #else
+      const char *next_command = command_args;
+    #endif
+    if (next_command) parse(next_command);
+    return !!next_command;
+  }
+
+#endif // CNC_COORDINATE_SYSTEMS
+
+void GCodeParser::unknown_command_warning() {
+  SERIAL_ECHO_MSG(STR_UNKNOWN_COMMAND, command_ptr, "\"");
+}
+
+#if ENABLED(DEBUG_GCODE_PARSER)
+
+  void GCodeParser::debug() {
+    SERIAL_ECHOPAIR("Command: ", command_ptr, " (", command_letter);
+    SERIAL_ECHO(codenum);
+    SERIAL_ECHOLNPGM(")");
+    #if ENABLED(FASTER_GCODE_PARSER)
+      SERIAL_ECHOPGM(" args: { ");
+      for (char c = 'A'; c <= 'Z'; ++c) if (seen(c)) SERIAL_CHAR(c, ' ');
+      SERIAL_CHAR('}');
+    #else
+      SERIAL_ECHOPAIR(" args: { ", command_args, " }");
+    #endif
+    if (string_arg) {
+      SERIAL_ECHOPAIR(" string: \"", string_arg);
+      SERIAL_CHAR('"');
+    }
+    SERIAL_ECHOLNPGM("\n");
+    for (char c = 'A'; c <= 'Z'; ++c) {
+      if (seen(c)) {
+        SERIAL_ECHOPAIR("Code '", c); SERIAL_ECHOPGM("':");
+        if (has_value()) {
+          SERIAL_ECHOLNPAIR(
+            "\n    float: ", value_float(),
+            "\n     long: ", value_long(),
+            "\n    ulong: ", value_ulong(),
+            "\n   millis: ", value_millis(),
+            "\n   sec-ms: ", value_millis_from_seconds(),
+            "\n      int: ", value_int(),
+            "\n   ushort: ", value_ushort(),
+            "\n     byte: ", (int)value_byte(),
+            "\n     bool: ", (int)value_bool(),
+            "\n   linear: ", value_linear_units(),
+            "\n  celsius: ", value_celsius()
+          );
+        }
+        else
+          SERIAL_ECHOLNPGM(" (no value)");
+      }
+    }
+  }
+
+#endif // DEBUG_GCODE_PARSER
diff --git a/Marlin/src/gcode/parser.h b/Marlin/src/gcode/parser.h
new file mode 100644
index 0000000..cf531c4
--- /dev/null
+++ b/Marlin/src/gcode/parser.h
@@ -0,0 +1,441 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * parser.h - Parser for a GCode line, providing a parameter interface.
+ *           Codes like M149 control the way the GCode parser behaves,
+ *           so settings for these codes are located in this class.
+ */
+
+#include "../inc/MarlinConfig.h"
+
+//#define DEBUG_GCODE_PARSER
+#if ENABLED(DEBUG_GCODE_PARSER)
+  #include "../libs/hex_print.h"
+#endif
+
+#if ENABLED(TEMPERATURE_UNITS_SUPPORT)
+  typedef enum : uint8_t { TEMPUNIT_C, TEMPUNIT_K, TEMPUNIT_F } TempUnit;
+#endif
+
+#if ENABLED(INCH_MODE_SUPPORT)
+  typedef enum : uint8_t { LINEARUNIT_MM, LINEARUNIT_INCH } LinearUnit;
+#endif
+
+/**
+ * GCode parser
+ *
+ *  - Parse a single gcode line for its letter, code, subcode, and parameters
+ *  - FASTER_GCODE_PARSER:
+ *    - Flags existing params (1 bit each)
+ *    - Stores value offsets (1 byte each)
+ *  - Provide accessors for parameters:
+ *    - Parameter exists
+ *    - Parameter has value
+ *    - Parameter value in different units and types
+ */
+class GCodeParser {
+
+private:
+  static char *value_ptr;           // Set by seen, used to fetch the value
+
+  #if ENABLED(FASTER_GCODE_PARSER)
+    static uint32_t codebits;       // Parameters pre-scanned
+    static uint8_t param[26];       // For A-Z, offsets into command args
+  #else
+    static char *command_args;      // Args start here, for slow scan
+  #endif
+
+public:
+
+  // Global states for GCode-level units features
+
+  static bool volumetric_enabled;
+
+  #if ENABLED(INCH_MODE_SUPPORT)
+    static float linear_unit_factor, volumetric_unit_factor;
+  #endif
+
+  #if ENABLED(TEMPERATURE_UNITS_SUPPORT)
+    static TempUnit input_temp_units;
+  #endif
+
+  // Command line state
+  static char *command_ptr,               // The command, so it can be echoed
+              *string_arg,                // string of command line
+              command_letter;             // G, M, or T
+  static uint16_t codenum;                // 123
+  #if ENABLED(USE_GCODE_SUBCODES)
+    static uint8_t subcode;               // .1
+  #endif
+
+  #if ENABLED(GCODE_MOTION_MODES)
+    static int16_t motion_mode_codenum;
+    #if ENABLED(USE_GCODE_SUBCODES)
+      static uint8_t motion_mode_subcode;
+    #endif
+    FORCE_INLINE static void cancel_motion_mode() { motion_mode_codenum = -1; }
+  #endif
+
+  #if ENABLED(DEBUG_GCODE_PARSER)
+    static void debug();
+  #endif
+
+  // Reset is done before parsing
+  static void reset();
+
+  #define LETTER_BIT(N) ((N) - 'A')
+
+  FORCE_INLINE static bool valid_signless(const char * const p) {
+    return NUMERIC(p[0]) || (p[0] == '.' && NUMERIC(p[1])); // .?[0-9]
+  }
+
+  FORCE_INLINE static bool valid_float(const char * const p) {
+    return valid_signless(p) || ((p[0] == '-' || p[0] == '+') && valid_signless(&p[1])); // [-+]?.?[0-9]
+  }
+
+  FORCE_INLINE static bool valid_number(const char * const p) {
+    // TODO: With MARLIN_DEV_MODE allow HEX values starting with "x"
+    return valid_float(p);
+  }
+
+  #if ENABLED(FASTER_GCODE_PARSER)
+
+    FORCE_INLINE static bool valid_int(const char * const p) {
+      return NUMERIC(p[0]) || ((p[0] == '-' || p[0] == '+') && NUMERIC(p[1])); // [-+]?[0-9]
+    }
+
+    // Set the flag and pointer for a parameter
+    static inline void set(const char c, char * const ptr) {
+      const uint8_t ind = LETTER_BIT(c);
+      if (ind >= COUNT(param)) return;           // Only A-Z
+      SBI32(codebits, ind);                      // parameter exists
+      param[ind] = ptr ? ptr - command_ptr : 0;  // parameter offset or 0
+      #if ENABLED(DEBUG_GCODE_PARSER)
+        if (codenum == 800) {
+          SERIAL_ECHOPAIR("Set bit ", (int)ind, " of codebits (", hex_address((void*)(codebits >> 16)));
+          print_hex_word((uint16_t)(codebits & 0xFFFF));
+          SERIAL_ECHOLNPAIR(") | param = ", (int)param[ind]);
+        }
+      #endif
+    }
+
+    // Code seen bit was set. If not found, value_ptr is unchanged.
+    // This allows "if (seen('A')||seen('B'))" to use the last-found value.
+    static inline bool seen(const char c) {
+      const uint8_t ind = LETTER_BIT(c);
+      if (ind >= COUNT(param)) return false; // Only A-Z
+      const bool b = TEST32(codebits, ind);
+      if (b) {
+        if (param[ind]) {
+          char * const ptr = command_ptr + param[ind];
+          value_ptr = valid_number(ptr) ? ptr : nullptr;
+        }
+        else
+          value_ptr = nullptr;
+      }
+      return b;
+    }
+
+    FORCE_INLINE static constexpr uint32_t letter_bits(const char * const str) {
+      return  (str[0] ? _BV32(LETTER_BIT(str[0])) |
+              (str[1] ? _BV32(LETTER_BIT(str[1])) |
+              (str[2] ? _BV32(LETTER_BIT(str[2])) |
+              (str[3] ? _BV32(LETTER_BIT(str[3])) |
+              (str[4] ? _BV32(LETTER_BIT(str[4])) |
+              (str[5] ? _BV32(LETTER_BIT(str[5])) |
+              (str[6] ? _BV32(LETTER_BIT(str[6])) |
+              (str[7] ? _BV32(LETTER_BIT(str[7])) |
+              (str[8] ? _BV32(LETTER_BIT(str[8])) |
+              (str[9] ? _BV32(LETTER_BIT(str[9]))
+            : 0) : 0) : 0) : 0) : 0) : 0) : 0) : 0) : 0) : 0);
+    }
+
+    // At least one of a list of code letters was seen
+    #ifdef CPU_32_BIT
+      FORCE_INLINE static bool seen(const char * const str) { return !!(codebits & letter_bits(str)); }
+    #else
+      FORCE_INLINE static bool seen(const char * const str) {
+        const uint32_t letrbits = letter_bits(str);
+        const uint8_t * const cb = (uint8_t*)&codebits;
+        const uint8_t * const lb = (uint8_t*)&letrbits;
+        return (cb[0] & lb[0]) || (cb[1] & lb[1]) || (cb[2] & lb[2]) || (cb[3] & lb[3]);
+      }
+    #endif
+
+    static inline bool seen_any() { return !!codebits; }
+
+    FORCE_INLINE static bool seen_test(const char c) { return TEST32(codebits, LETTER_BIT(c)); }
+
+  #else // !FASTER_GCODE_PARSER
+
+    #if ENABLED(GCODE_CASE_INSENSITIVE)
+      FORCE_INLINE static char* strgchr(char *p, char g) {
+        auto uppercase = [](char c) {
+          return c + (WITHIN(c, 'a', 'z') ? 'A' - 'a' : 0);
+        };
+        const char d = uppercase(g);
+        for (char cc; (cc = uppercase(*p)); p++) if (cc == d) return p;
+        return nullptr;
+      }
+    #else
+      #define strgchr strchr
+    #endif
+
+    // Code is found in the string. If not found, value_ptr is unchanged.
+    // This allows "if (seen('A')||seen('B'))" to use the last-found value.
+    static inline bool seen(const char c) {
+      char *p = strgchr(command_args, c);
+      const bool b = !!p;
+      if (b) value_ptr = valid_number(&p[1]) ? &p[1] : nullptr;
+      return b;
+    }
+
+    static inline bool seen_any() { return *command_args == '\0'; }
+
+    FORCE_INLINE static bool seen_test(const char c) { return (bool)strgchr(command_args, c); }
+
+    // At least one of a list of code letters was seen
+    static inline bool seen(const char * const str) {
+      for (uint8_t i = 0; const char c = str[i]; i++)
+        if (seen_test(c)) return true;
+      return false;
+    }
+
+  #endif // !FASTER_GCODE_PARSER
+
+  // Seen any axis parameter
+  static inline bool seen_axis() {
+    return seen_test('X') || seen_test('Y') || seen_test('Z') || seen_test('E');
+  }
+
+  #if ENABLED(GCODE_QUOTED_STRINGS)
+    static char* unescape_string(char* &src);
+  #else
+    FORCE_INLINE static char* unescape_string(char* &src) { return src; }
+  #endif
+
+  // Populate all fields by parsing a single line of GCode
+  // This uses 54 bytes of SRAM to speed up seen/value
+  static void parse(char * p);
+
+  #if ENABLED(CNC_COORDINATE_SYSTEMS)
+    // Parse the next parameter as a new command
+    static bool chain();
+  #endif
+
+  // Test whether the parsed command matches the input
+  static inline bool is_command(const char ltr, const uint16_t num) { return command_letter == ltr && codenum == num; }
+
+  // The code value pointer was set
+  FORCE_INLINE static bool has_value() { return !!value_ptr; }
+
+  // Seen a parameter with a value
+  static inline bool seenval(const char c) { return seen(c) && has_value(); }
+
+  // The value as a string
+  static inline char* value_string() { return value_ptr; }
+
+  // Float removes 'E' to prevent scientific notation interpretation
+  static inline float value_float() {
+    if (value_ptr) {
+      char *e = value_ptr;
+      for (;;) {
+        const char c = *e;
+        if (c == '\0' || c == ' ') break;
+        if (c == 'E' || c == 'e') {
+          *e = '\0';
+          const float ret = strtof(value_ptr, nullptr);
+          *e = c;
+          return ret;
+        }
+        ++e;
+      }
+      return strtof(value_ptr, nullptr);
+    }
+    return 0;
+  }
+
+  // Code value as a long or ulong
+  static inline int32_t value_long() { return value_ptr ? strtol(value_ptr, nullptr, 10) : 0L; }
+  static inline uint32_t value_ulong() { return value_ptr ? strtoul(value_ptr, nullptr, 10) : 0UL; }
+
+  // Code value for use as time
+  static inline millis_t value_millis() { return value_ulong(); }
+  static inline millis_t value_millis_from_seconds() { return (millis_t)SEC_TO_MS(value_float()); }
+
+  // Reduce to fewer bits
+  static inline int16_t value_int() { return (int16_t)value_long(); }
+  static inline uint16_t value_ushort() { return (uint16_t)value_long(); }
+  static inline uint8_t value_byte() { return (uint8_t)constrain(value_long(), 0, 255); }
+
+  // Bool is true with no value or non-zero
+  static inline bool value_bool() { return !has_value() || !!value_byte(); }
+
+  // Units modes: Inches, Fahrenheit, Kelvin
+
+  #if ENABLED(INCH_MODE_SUPPORT)
+    static inline float mm_to_linear_unit(const float mm)     { return mm / linear_unit_factor; }
+    static inline float mm_to_volumetric_unit(const float mm) { return mm / (volumetric_enabled ? volumetric_unit_factor : linear_unit_factor); }
+
+    // Init linear units by constructor
+    GCodeParser() { set_input_linear_units(LINEARUNIT_MM); }
+
+    static inline void set_input_linear_units(const LinearUnit units) {
+      switch (units) {
+        default:
+        case LINEARUNIT_MM:   linear_unit_factor =  1.0f; break;
+        case LINEARUNIT_INCH: linear_unit_factor = 25.4f; break;
+      }
+      volumetric_unit_factor = POW(linear_unit_factor, 3);
+    }
+
+    static inline float axis_unit_factor(const AxisEnum axis) {
+      return (axis >= E_AXIS && volumetric_enabled ? volumetric_unit_factor : linear_unit_factor);
+    }
+
+    static inline float linear_value_to_mm(const float v)                    { return v * linear_unit_factor; }
+    static inline float axis_value_to_mm(const AxisEnum axis, const float v) { return v * axis_unit_factor(axis); }
+    static inline float per_axis_value(const AxisEnum axis, const float v)   { return v / axis_unit_factor(axis); }
+
+  #else
+
+    static inline float mm_to_linear_unit(const float mm)     { return mm; }
+    static inline float mm_to_volumetric_unit(const float mm) { return mm; }
+
+    static inline float linear_value_to_mm(const float v)               { return v; }
+    static inline float axis_value_to_mm(const AxisEnum, const float v) { return v; }
+    static inline float per_axis_value(const AxisEnum, const float v)   { return v; }
+
+  #endif
+
+  static inline bool using_inch_units() { return mm_to_linear_unit(1.0f) != 1.0f; }
+
+  #define IN_TO_MM(I)        ((I) * 25.4f)
+  #define MM_TO_IN(M)        ((M) / 25.4f)
+  #define LINEAR_UNIT(V)     parser.mm_to_linear_unit(V)
+  #define VOLUMETRIC_UNIT(V) parser.mm_to_volumetric_unit(V)
+
+  static inline float value_linear_units()                      { return linear_value_to_mm(value_float()); }
+  static inline float value_axis_units(const AxisEnum axis)     { return axis_value_to_mm(axis, value_float()); }
+  static inline float value_per_axis_units(const AxisEnum axis) { return per_axis_value(axis, value_float()); }
+
+  #if ENABLED(TEMPERATURE_UNITS_SUPPORT)
+
+    static inline void set_input_temp_units(const TempUnit units) { input_temp_units = units; }
+
+    #if HAS_LCD_MENU && DISABLED(DISABLE_M503)
+
+      static inline char temp_units_code() {
+        return input_temp_units == TEMPUNIT_K ? 'K' : input_temp_units == TEMPUNIT_F ? 'F' : 'C';
+      }
+      static inline PGM_P temp_units_name() {
+        return input_temp_units == TEMPUNIT_K ? PSTR("Kelvin") : input_temp_units == TEMPUNIT_F ? PSTR("Fahrenheit") : PSTR("Celsius");
+      }
+      static inline float to_temp_units(const float &f) {
+        switch (input_temp_units) {
+          case TEMPUNIT_F:
+            return f * 0.5555555556f + 32;
+          case TEMPUNIT_K:
+            return f + 273.15f;
+          case TEMPUNIT_C:
+          default:
+            return f;
+        }
+      }
+
+    #endif // HAS_LCD_MENU && !DISABLE_M503
+
+    static inline float value_celsius() {
+      const float f = value_float();
+      switch (input_temp_units) {
+        case TEMPUNIT_F:
+          return (f - 32) * 0.5555555556f;
+        case TEMPUNIT_K:
+          return f - 273.15f;
+        case TEMPUNIT_C:
+        default:
+          return f;
+      }
+    }
+
+    static inline float value_celsius_diff() {
+      switch (input_temp_units) {
+        case TEMPUNIT_F:
+          return value_float() * 0.5555555556f;
+        case TEMPUNIT_C:
+        case TEMPUNIT_K:
+        default:
+          return value_float();
+      }
+    }
+
+    #define TEMP_UNIT(N) parser.to_temp_units(N)
+
+  #else // !TEMPERATURE_UNITS_SUPPORT
+
+    static inline float value_celsius()      { return value_float(); }
+    static inline float value_celsius_diff() { return value_float(); }
+
+    #define TEMP_UNIT(N) (N)
+
+  #endif // !TEMPERATURE_UNITS_SUPPORT
+
+  static inline feedRate_t value_feedrate() { return MMM_TO_MMS(value_linear_units()); }
+
+  void unknown_command_warning();
+
+  // Provide simple value accessors with default option
+  static inline char*    stringval(const char c, char * const dval=nullptr) { return seenval(c) ? value_string()   : dval; }
+  static inline float    floatval(const char c, const float dval=0.0)   { return seenval(c) ? value_float()        : dval; }
+  static inline bool     boolval(const char c, const bool dval=false)   { return seenval(c) ? value_bool()         : (seen(c) ? true : dval); }
+  static inline uint8_t  byteval(const char c, const uint8_t dval=0)    { return seenval(c) ? value_byte()         : dval; }
+  static inline int16_t  intval(const char c, const int16_t dval=0)     { return seenval(c) ? value_int()          : dval; }
+  static inline uint16_t ushortval(const char c, const uint16_t dval=0) { return seenval(c) ? value_ushort()       : dval; }
+  static inline int32_t  longval(const char c, const int32_t dval=0)    { return seenval(c) ? value_long()         : dval; }
+  static inline uint32_t ulongval(const char c, const uint32_t dval=0)  { return seenval(c) ? value_ulong()        : dval; }
+  static inline float    linearval(const char c, const float dval=0)    { return seenval(c) ? value_linear_units() : dval; }
+  static inline float    celsiusval(const char c, const float dval=0)   { return seenval(c) ? value_celsius()      : dval; }
+
+  #if ENABLED(MARLIN_DEV_MODE)
+
+    static inline uint8_t* hex_adr_val(const char c, uint8_t * const dval=nullptr) {
+      if (!seen(c) || *value_ptr != 'x') return dval;
+      uint8_t *out = nullptr;
+      for (char *vp = value_ptr + 1; HEXCHR(*vp) >= 0; vp++)
+        out = (uint8_t*)((uintptr_t(out) << 8) | HEXCHR(*vp));
+      return out;
+    }
+
+    static inline uint16_t hex_val(const char c, uint16_t const dval=0) {
+      if (!seen(c) || *value_ptr != 'x') return dval;
+      uint16_t out = 0;
+      for (char *vp = value_ptr + 1; HEXCHR(*vp) >= 0; vp++)
+        out = ((out) << 8) | HEXCHR(*vp);
+      return out;
+    }
+
+  #endif
+};
+
+extern GCodeParser parser;
diff --git a/Marlin/src/gcode/probe/G30.cpp b/Marlin/src/gcode/probe/G30.cpp
new file mode 100644
index 0000000..4347f55
--- /dev/null
+++ b/Marlin/src/gcode/probe/G30.cpp
@@ -0,0 +1,66 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_BED_PROBE
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/probe.h"
+#include "../../feature/bedlevel/bedlevel.h"
+
+/**
+ * G30: Do a single Z probe at the current XY
+ *
+ * Parameters:
+ *
+ *   X   Probe X position (default current X)
+ *   Y   Probe Y position (default current Y)
+ *   E   Engage the probe for each probe (default 1)
+ */
+void GcodeSuite::G30() {
+
+  const xy_pos_t pos = { parser.linearval('X', current_position.x + probe.offset_xy.x),
+                         parser.linearval('Y', current_position.y + probe.offset_xy.y) };
+
+  if (!probe.can_reach(pos)) return;
+
+  // Disable leveling so the planner won't mess with us
+  TERN_(HAS_LEVELING, set_bed_leveling_enabled(false));
+
+  remember_feedrate_scaling_off();
+
+  const ProbePtRaise raise_after = parser.boolval('E', true) ? PROBE_PT_STOW : PROBE_PT_NONE;
+  const float measured_z = probe.probe_at_point(pos, raise_after, 1);
+  if (!isnan(measured_z))
+    SERIAL_ECHOLNPAIR("Bed X: ", pos.x, " Y: ", pos.y, " Z: ", measured_z);
+
+  restore_feedrate_and_scaling();
+
+  if (raise_after == PROBE_PT_STOW)
+    probe.move_z_after_probing();
+
+  report_current_position();
+}
+
+#endif // HAS_BED_PROBE
diff --git a/Marlin/src/gcode/probe/G31_G32.cpp b/Marlin/src/gcode/probe/G31_G32.cpp
new file mode 100644
index 0000000..af44257
--- /dev/null
+++ b/Marlin/src/gcode/probe/G31_G32.cpp
@@ -0,0 +1,40 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(Z_PROBE_SLED)
+
+#include "../gcode.h"
+#include "../../module/probe.h"
+
+/**
+ * G31: Deploy the Z probe
+ */
+void GcodeSuite::G31() { probe.deploy(); }
+
+/**
+ * G32: Stow the Z probe
+ */
+void GcodeSuite::G32() { probe.stow(); }
+
+#endif // Z_PROBE_SLED
diff --git a/Marlin/src/gcode/probe/G38.cpp b/Marlin/src/gcode/probe/G38.cpp
new file mode 100644
index 0000000..b06cd47
--- /dev/null
+++ b/Marlin/src/gcode/probe/G38.cpp
@@ -0,0 +1,133 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(G38_PROBE_TARGET)
+
+#include "../gcode.h"
+
+#include "../../module/endstops.h"
+#include "../../module/motion.h"
+#include "../../module/stepper.h"
+#include "../../module/probe.h"
+
+inline void G38_single_probe(const uint8_t move_value) {
+  endstops.enable(true);
+  G38_move = move_value;
+  prepare_line_to_destination();
+  planner.synchronize();
+  G38_move = 0;
+  endstops.hit_on_purpose();
+  set_current_from_steppers_for_axis(ALL_AXES);
+  sync_plan_position();
+}
+
+inline bool G38_run_probe() {
+
+  bool G38_pass_fail = false;
+
+  #if MULTIPLE_PROBING > 1
+    // Get direction of move and retract
+    xyz_float_t retract_mm;
+    LOOP_XYZ(i) {
+      const float dist = destination[i] - current_position[i];
+      retract_mm[i] = ABS(dist) < G38_MINIMUM_MOVE ? 0 : home_bump_mm((AxisEnum)i) * (dist > 0 ? -1 : 1);
+    }
+  #endif
+
+  planner.synchronize();  // wait until the machine is idle
+
+  // Move flag value
+  #if ENABLED(G38_PROBE_AWAY)
+    const uint8_t move_value = parser.subcode;
+  #else
+    constexpr uint8_t move_value = 1;
+  #endif
+
+  G38_did_trigger = false;
+
+  // Move until destination reached or target hit
+  G38_single_probe(move_value);
+
+  if (G38_did_trigger) {
+
+    G38_pass_fail = true;
+
+    #if MULTIPLE_PROBING > 1
+      // Move away by the retract distance
+      destination = current_position + retract_mm;
+      endstops.enable(false);
+      prepare_line_to_destination();
+      planner.synchronize();
+
+      REMEMBER(fr, feedrate_mm_s, feedrate_mm_s * 0.25);
+
+      // Bump the target more slowly
+      destination -= retract_mm * 2;
+
+      G38_single_probe(move_value);
+    #endif
+  }
+
+  endstops.not_homing();
+  return G38_pass_fail;
+}
+
+/**
+ * G38 Probe Target
+ *
+ *  G38.2 - Probe toward workpiece, stop on contact, signal error if failure
+ *  G38.3 - Probe toward workpiece, stop on contact
+ *
+ * With G38_PROBE_AWAY:
+ *
+ *  G38.4 - Probe away from workpiece, stop on contact break, signal error if failure
+ *  G38.5 - Probe away from workpiece, stop on contact break
+ */
+void GcodeSuite::G38(const int8_t subcode) {
+  // Get X Y Z E F
+  get_destination_from_command();
+
+  remember_feedrate_scaling_off();
+
+  const bool error_on_fail =
+    #if ENABLED(G38_PROBE_AWAY)
+      !TEST(subcode, 0)
+    #else
+      (subcode == 2)
+    #endif
+  ;
+
+  // If any axis has enough movement, do the move
+  LOOP_XYZ(i)
+    if (ABS(destination[i] - current_position[i]) >= G38_MINIMUM_MOVE) {
+      if (!parser.seenval('F')) feedrate_mm_s = homing_feedrate((AxisEnum)i);
+      // If G38.2 fails throw an error
+      if (!G38_run_probe() && error_on_fail) SERIAL_ERROR_MSG("Failed to reach target");
+      break;
+    }
+
+  restore_feedrate_and_scaling();
+}
+
+#endif // G38_PROBE_TARGET
diff --git a/Marlin/src/gcode/probe/M401_M402.cpp b/Marlin/src/gcode/probe/M401_M402.cpp
new file mode 100644
index 0000000..bd9bb44
--- /dev/null
+++ b/Marlin/src/gcode/probe/M401_M402.cpp
@@ -0,0 +1,49 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_BED_PROBE
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/probe.h"
+
+/**
+ * M401: Deploy and activate the Z probe
+ */
+void GcodeSuite::M401() {
+  probe.deploy();
+  TERN_(PROBE_TARE, probe.tare());
+  report_current_position();
+}
+
+/**
+ * M402: Deactivate and stow the Z probe
+ */
+void GcodeSuite::M402() {
+  probe.stow();
+  probe.move_z_after_probing();
+  report_current_position();
+}
+
+#endif // HAS_BED_PROBE
diff --git a/Marlin/src/gcode/probe/M851.cpp b/Marlin/src/gcode/probe/M851.cpp
new file mode 100644
index 0000000..04b293d
--- /dev/null
+++ b/Marlin/src/gcode/probe/M851.cpp
@@ -0,0 +1,97 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_BED_PROBE
+
+#include "../gcode.h"
+#include "../../feature/bedlevel/bedlevel.h"
+#include "../../module/probe.h"
+
+/**
+ * M851: Set the nozzle-to-probe offsets in current units
+ */
+void GcodeSuite::M851() {
+
+  // Show usage with no parameters
+  if (!parser.seen("XYZ")) {
+    SERIAL_ECHOLNPAIR_P(
+      #if HAS_PROBE_XY_OFFSET
+        PSTR(STR_PROBE_OFFSET " X"), probe.offset_xy.x, SP_Y_STR, probe.offset_xy.y, SP_Z_STR
+      #else
+        PSTR(STR_PROBE_OFFSET " X0 Y0 Z")
+      #endif
+      , probe.offset.z
+    );
+    return;
+  }
+
+  // Start with current offsets and modify
+  xyz_pos_t offs = probe.offset;
+
+  // Assume no errors
+  bool ok = true;
+
+  if (parser.seenval('X')) {
+    const float x = parser.value_float();
+    #if HAS_PROBE_XY_OFFSET
+      if (WITHIN(x, -(X_BED_SIZE), X_BED_SIZE))
+        offs.x = x;
+      else {
+        SERIAL_ECHOLNPAIR("?X out of range (-", int(X_BED_SIZE), " to ", int(X_BED_SIZE), ")");
+        ok = false;
+      }
+    #else
+      if (x) SERIAL_ECHOLNPAIR("?X must be 0 (NOZZLE_AS_PROBE)."); // ...but let 'ok' stay true
+    #endif
+  }
+
+  if (parser.seenval('Y')) {
+    const float y = parser.value_float();
+    #if HAS_PROBE_XY_OFFSET
+      if (WITHIN(y, -(Y_BED_SIZE), Y_BED_SIZE))
+        offs.y = y;
+      else {
+        SERIAL_ECHOLNPAIR("?Y out of range (-", int(Y_BED_SIZE), " to ", int(Y_BED_SIZE), ")");
+        ok = false;
+      }
+    #else
+      if (y) SERIAL_ECHOLNPAIR("?Y must be 0 (NOZZLE_AS_PROBE)."); // ...but let 'ok' stay true
+    #endif
+  }
+
+  if (parser.seenval('Z')) {
+    const float z = parser.value_float();
+    if (WITHIN(z, Z_PROBE_OFFSET_RANGE_MIN, Z_PROBE_OFFSET_RANGE_MAX))
+      offs.z = z;
+    else {
+      SERIAL_ECHOLNPAIR("?Z out of range (", int(Z_PROBE_OFFSET_RANGE_MIN), " to ", int(Z_PROBE_OFFSET_RANGE_MAX), ")");
+      ok = false;
+    }
+  }
+
+  // Save the new offsets
+  if (ok) probe.offset = offs;
+}
+
+#endif // HAS_BED_PROBE
diff --git a/Marlin/src/gcode/probe/M951.cpp b/Marlin/src/gcode/probe/M951.cpp
new file mode 100644
index 0000000..f461fc2
--- /dev/null
+++ b/Marlin/src/gcode/probe/M951.cpp
@@ -0,0 +1,71 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if ENABLED(MAGNETIC_PARKING_EXTRUDER)
+
+#include "../gcode.h"
+#include "../../module/tool_change.h"
+#include "../../module/motion.h"
+
+mpe_settings_t mpe_settings;
+
+inline void mpe_settings_report() {
+  SERIAL_ECHO_MSG("Magnetic Parking Extruder");
+  SERIAL_ECHO_START(); SERIAL_ECHOLNPAIR("L: Left parking  :", mpe_settings.parking_xpos[0]);
+  SERIAL_ECHO_START(); SERIAL_ECHOLNPAIR("R: Right parking :", mpe_settings.parking_xpos[1]);
+  SERIAL_ECHO_START(); SERIAL_ECHOLNPAIR("I: Grab Offset   :", mpe_settings.grab_distance);
+  SERIAL_ECHO_START(); SERIAL_ECHOLNPAIR("J: Normal speed  :", long(MMS_TO_MMM(mpe_settings.slow_feedrate)));
+  SERIAL_ECHO_START(); SERIAL_ECHOLNPAIR("H: High speed    :", long(MMS_TO_MMM(mpe_settings.fast_feedrate)));
+  SERIAL_ECHO_START(); SERIAL_ECHOLNPAIR("D: Distance trav.:", mpe_settings.travel_distance);
+  SERIAL_ECHO_START(); SERIAL_ECHOLNPAIR("C: Compenstion   :", mpe_settings.compensation_factor);
+}
+
+void mpe_settings_init() {
+  constexpr float pex[2] = PARKING_EXTRUDER_PARKING_X;
+  mpe_settings.parking_xpos[0]      = pex[0];                         // M951 L
+  mpe_settings.parking_xpos[1]      = pex[1];                         // M951 R
+  mpe_settings.grab_distance        = PARKING_EXTRUDER_GRAB_DISTANCE; // M951 I
+  TERN_(HAS_HOME_OFFSET, set_home_offset(X_AXIS, mpe_settings.grab_distance * -1));
+  mpe_settings.slow_feedrate        = MMM_TO_MMS(MPE_SLOW_SPEED);     // M951 J
+  mpe_settings.fast_feedrate        = MMM_TO_MMS(MPE_FAST_SPEED);     // M951 H
+  mpe_settings.travel_distance      = MPE_TRAVEL_DISTANCE;            // M951 D
+  mpe_settings.compensation_factor  = MPE_COMPENSATION;               // M951 C
+  mpe_settings_report();
+}
+
+void GcodeSuite::M951() {
+  if (parser.seenval('L')) mpe_settings.parking_xpos[0] = parser.value_linear_units();
+  if (parser.seenval('R')) mpe_settings.parking_xpos[1] = parser.value_linear_units();
+  if (parser.seenval('I')) {
+    mpe_settings.grab_distance = parser.value_linear_units();
+    TERN_(HAS_HOME_OFFSET, set_home_offset(X_AXIS, mpe_settings.grab_distance * -1));
+  }
+  if (parser.seenval('J')) mpe_settings.slow_feedrate       = MMM_TO_MMS(parser.value_linear_units());
+  if (parser.seenval('H')) mpe_settings.fast_feedrate       = MMM_TO_MMS(parser.value_linear_units());
+  if (parser.seenval('D')) mpe_settings.travel_distance     = parser.value_linear_units();
+  if (parser.seenval('C')) mpe_settings.compensation_factor = parser.value_float();
+  if (!parser.seen("CDHIJLR")) mpe_settings_report();
+}
+
+#endif // MAGNETIC_PARKING_EXTRUDER
diff --git a/Marlin/src/gcode/queue.cpp b/Marlin/src/gcode/queue.cpp
new file mode 100644
index 0000000..4c42f7e
--- /dev/null
+++ b/Marlin/src/gcode/queue.cpp
@@ -0,0 +1,699 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * queue.cpp - The G-code command queue
+ */
+
+#include "queue.h"
+GCodeQueue queue;
+
+#include "gcode.h"
+
+#include "../lcd/marlinui.h"
+#include "../sd/cardreader.h"
+#include "../module/motion.h"
+#include "../module/planner.h"
+#include "../module/temperature.h"
+#include "../MarlinCore.h"
+
+#if ENABLED(PRINTER_EVENT_LEDS)
+  #include "../feature/leds/printer_event_leds.h"
+#endif
+
+#if HAS_ETHERNET
+  #include "../feature/ethernet.h"
+#endif
+
+#if ENABLED(BINARY_FILE_TRANSFER)
+  #include "../feature/binary_stream.h"
+#endif
+
+#if ENABLED(MEATPACK)
+  #include "../feature/meatpack.h"
+#endif
+
+#if ENABLED(POWER_LOSS_RECOVERY)
+  #include "../feature/powerloss.h"
+#endif
+
+#if ENABLED(GCODE_REPEAT_MARKERS)
+  #include "../feature/repeat.h"
+#endif
+
+// Frequently used G-code strings
+PGMSTR(G28_STR, "G28");
+
+/**
+ * GCode line number handling. Hosts may opt to include line numbers when
+ * sending commands to Marlin, and lines will be checked for sequentiality.
+ * M110 N<int> sets the current line number.
+ */
+long GCodeQueue::last_N[NUM_SERIAL];
+
+/**
+ * GCode Command Queue
+ * A simple ring buffer of BUFSIZE command strings.
+ *
+ * Commands are copied into this buffer by the command injectors
+ * (immediate, serial, sd card) and they are processed sequentially by
+ * the main loop. The gcode.process_next_command method parses the next
+ * command and hands off execution to individual handler functions.
+ */
+uint8_t GCodeQueue::length = 0,  // Count of commands in the queue
+        GCodeQueue::index_r = 0, // Ring buffer read position
+        GCodeQueue::index_w = 0; // Ring buffer write position
+
+char GCodeQueue::command_buffer[BUFSIZE][MAX_CMD_SIZE];
+
+/*
+ * The port that the command was received on
+ */
+#if HAS_MULTI_SERIAL
+  serial_index_t GCodeQueue::port[BUFSIZE];
+#endif
+
+/**
+ * Serial command injection
+ */
+
+// Number of characters read in the current line of serial input
+static int serial_count[NUM_SERIAL] = { 0 };
+
+bool send_ok[BUFSIZE];
+
+/**
+ * Next Injected PROGMEM Command pointer. (nullptr == empty)
+ * Internal commands are enqueued ahead of serial / SD commands.
+ */
+PGM_P GCodeQueue::injected_commands_P; // = nullptr
+
+/**
+ * Injected SRAM Commands
+ */
+char GCodeQueue::injected_commands[64]; // = { 0 }
+
+GCodeQueue::GCodeQueue() {
+  // Send "ok" after commands by default
+  LOOP_L_N(i, COUNT(send_ok)) send_ok[i] = true;
+}
+
+/**
+ * Check whether there are any commands yet to be executed
+ */
+bool GCodeQueue::has_commands_queued() {
+  return queue.length || injected_commands_P || injected_commands[0];
+}
+
+/**
+ * Clear the Marlin command queue
+ */
+void GCodeQueue::clear() {
+  index_r = index_w = length = 0;
+}
+
+/**
+ * Once a new command is in the ring buffer, call this to commit it
+ */
+void GCodeQueue::_commit_command(bool say_ok
+  #if HAS_MULTI_SERIAL
+    , serial_index_t serial_ind/*=-1*/
+  #endif
+) {
+  send_ok[index_w] = say_ok;
+  TERN_(HAS_MULTI_SERIAL, port[index_w] = serial_ind);
+  TERN_(POWER_LOSS_RECOVERY, recovery.commit_sdpos(index_w));
+  if (++index_w >= BUFSIZE) index_w = 0;
+  length++;
+}
+
+/**
+ * Copy a command from RAM into the main command buffer.
+ * Return true if the command was successfully added.
+ * Return false for a full buffer, or if the 'command' is a comment.
+ */
+bool GCodeQueue::_enqueue(const char* cmd, bool say_ok/*=false*/
+  #if HAS_MULTI_SERIAL
+    , serial_index_t serial_ind/*=-1*/
+  #endif
+) {
+  if (*cmd == ';' || length >= BUFSIZE) return false;
+  strcpy(command_buffer[index_w], cmd);
+  _commit_command(say_ok
+    #if HAS_MULTI_SERIAL
+      , serial_ind
+    #endif
+  );
+  return true;
+}
+
+/**
+ * Enqueue with Serial Echo
+ * Return true if the command was consumed
+ */
+bool GCodeQueue::enqueue_one(const char* cmd) {
+
+  //SERIAL_ECHOPGM("enqueue_one(\"");
+  //SERIAL_ECHO(cmd);
+  //SERIAL_ECHOPGM("\") \n");
+
+  if (*cmd == 0 || ISEOL(*cmd)) return true;
+
+  if (_enqueue(cmd)) {
+    SERIAL_ECHO_MSG(STR_ENQUEUEING, cmd, "\"");
+    return true;
+  }
+  return false;
+}
+
+/**
+ * Process the next "immediate" command from PROGMEM.
+ * Return 'true' if any commands were processed.
+ */
+bool GCodeQueue::process_injected_command_P() {
+  if (!injected_commands_P) return false;
+
+  char c;
+  size_t i = 0;
+  while ((c = pgm_read_byte(&injected_commands_P[i])) && c != '\n') i++;
+
+  // Extract current command and move pointer to next command
+  char cmd[i + 1];
+  memcpy_P(cmd, injected_commands_P, i);
+  cmd[i] = '\0';
+  injected_commands_P = c ? injected_commands_P + i + 1 : nullptr;
+
+  // Execute command if non-blank
+  if (i) {
+    parser.parse(cmd);
+    gcode.process_parsed_command();
+  }
+  return true;
+}
+
+/**
+ * Process the next "immediate" command from SRAM.
+ * Return 'true' if any commands were processed.
+ */
+bool GCodeQueue::process_injected_command() {
+  if (injected_commands[0] == '\0') return false;
+
+  char c;
+  size_t i = 0;
+  while ((c = injected_commands[i]) && c != '\n') i++;
+
+  // Execute a non-blank command
+  if (i) {
+    injected_commands[i] = '\0';
+    parser.parse(injected_commands);
+    gcode.process_parsed_command();
+  }
+
+  // Copy the next command into place
+  for (
+    uint8_t d = 0, s = i + !!c;                     // dst, src
+    (injected_commands[d] = injected_commands[s]);  // copy, exit if 0
+    d++, s++                                        // next dst, src
+  );
+
+  return true;
+}
+
+/**
+ * Enqueue and return only when commands are actually enqueued.
+ * Never call this from a G-code handler!
+ */
+void GCodeQueue::enqueue_one_now(const char* cmd) { while (!enqueue_one(cmd)) idle(); }
+
+/**
+ * Attempt to enqueue a single G-code command
+ * and return 'true' if successful.
+ */
+bool GCodeQueue::enqueue_one_P(PGM_P const pgcode) {
+  size_t i = 0;
+  PGM_P p = pgcode;
+  char c;
+  while ((c = pgm_read_byte(&p[i])) && c != '\n') i++;
+  char cmd[i + 1];
+  memcpy_P(cmd, p, i);
+  cmd[i] = '\0';
+  return _enqueue(cmd);
+}
+
+/**
+ * Enqueue from program memory and return only when commands are actually enqueued
+ * Never call this from a G-code handler!
+ */
+void GCodeQueue::enqueue_now_P(PGM_P const pgcode) {
+  size_t i = 0;
+  PGM_P p = pgcode;
+  for (;;) {
+    char c;
+    while ((c = pgm_read_byte(&p[i])) && c != '\n') i++;
+    char cmd[i + 1];
+    memcpy_P(cmd, p, i);
+    cmd[i] = '\0';
+    enqueue_one_now(cmd);
+    if (!c) break;
+    p += i + 1;
+  }
+}
+
+/**
+ * Send an "ok" message to the host, indicating
+ * that a command was successfully processed.
+ *
+ * If ADVANCED_OK is enabled also include:
+ *   N<int>  Line number of the command, if any
+ *   P<int>  Planner space remaining
+ *   B<int>  Block queue space remaining
+ */
+void GCodeQueue::ok_to_send() {
+  #if HAS_MULTI_SERIAL
+    const serial_index_t serial_ind = command_port();
+    if (serial_ind < 0) return;
+    PORT_REDIRECT(SERIAL_PORTMASK(serial_ind));   // Reply to the serial port that sent the command
+  #endif
+  if (!send_ok[index_r]) return;
+  SERIAL_ECHOPGM(STR_OK);
+  #if ENABLED(ADVANCED_OK)
+    char* p = command_buffer[index_r];
+    if (*p == 'N') {
+      SERIAL_ECHO(' ');
+      SERIAL_ECHO(*p++);
+      while (NUMERIC_SIGNED(*p))
+        SERIAL_ECHO(*p++);
+    }
+    SERIAL_ECHOPAIR_P(SP_P_STR, int(planner.moves_free()),
+                      SP_B_STR, int(BUFSIZE - length));
+  #endif
+  SERIAL_EOL();
+}
+
+/**
+ * Send a "Resend: nnn" message to the host to
+ * indicate that a command needs to be re-sent.
+ */
+void GCodeQueue::flush_and_request_resend() {
+  const serial_index_t serial_ind = command_port();
+  #if HAS_MULTI_SERIAL
+    if (serial_ind < 0) return;                   // Never mind. Command came from SD or Flash Drive
+    PORT_REDIRECT(SERIAL_PORTMASK(serial_ind));   // Reply to the serial port that sent the command
+  #endif
+  SERIAL_FLUSH();
+  SERIAL_ECHOPGM(STR_RESEND);
+  SERIAL_ECHOLN(last_N[serial_ind] + 1);
+  ok_to_send();
+}
+
+inline bool serial_data_available() {
+  byte data_available = 0;
+  if (MYSERIAL0.available()) data_available++;
+  #ifdef SERIAL_PORT_2
+    const bool port2_open = TERN1(HAS_ETHERNET, ethernet.have_telnet_client);
+    if (port2_open && MYSERIAL1.available()) data_available++;
+  #endif
+  return data_available > 0;
+}
+
+inline int read_serial(const uint8_t index) {
+  switch (index) {
+    case 0: return MYSERIAL0.read();
+    case 1: {
+      #if HAS_MULTI_SERIAL
+        const bool port2_open = TERN1(HAS_ETHERNET, ethernet.have_telnet_client);
+        if (port2_open) return MYSERIAL1.read();
+      #endif
+    }
+    default: return -1;
+  }
+}
+
+void GCodeQueue::gcode_line_error(PGM_P const err, const serial_index_t serial_ind) {
+  PORT_REDIRECT(SERIAL_PORTMASK(serial_ind)); // Reply to the serial port that sent the command
+  SERIAL_ERROR_START();
+  serialprintPGM(err);
+  SERIAL_ECHOLN(last_N[serial_ind]);
+  while (read_serial(serial_ind) != -1);      // Clear out the RX buffer
+  flush_and_request_resend();
+  serial_count[serial_ind] = 0;
+}
+
+FORCE_INLINE bool is_M29(const char * const cmd) {  // matches "M29" & "M29 ", but not "M290", etc
+  const char * const m29 = strstr_P(cmd, PSTR("M29"));
+  return m29 && !NUMERIC(m29[3]);
+}
+
+#define PS_NORMAL 0
+#define PS_EOL    1
+#define PS_QUOTED 2
+#define PS_PAREN  3
+#define PS_ESC    4
+
+inline void process_stream_char(const char c, uint8_t &sis, char (&buff)[MAX_CMD_SIZE], int &ind) {
+
+  if (sis == PS_EOL) return;    // EOL comment or overflow
+
+  #if ENABLED(PAREN_COMMENTS)
+    else if (sis == PS_PAREN) { // Inline comment
+      if (c == ')') sis = PS_NORMAL;
+      return;
+    }
+  #endif
+
+  else if (sis >= PS_ESC)       // End escaped char
+    sis -= PS_ESC;
+
+  else if (c == '\\') {         // Start escaped char
+    sis += PS_ESC;
+    if (sis == PS_ESC) return;  // Keep if quoting
+  }
+
+  #if ENABLED(GCODE_QUOTED_STRINGS)
+
+    else if (sis == PS_QUOTED) {
+      if (c == '"') sis = PS_NORMAL; // End quoted string
+    }
+    else if (c == '"')          // Start quoted string
+      sis = PS_QUOTED;
+
+  #endif
+
+  else if (c == ';') {          // Start end-of-line comment
+    sis = PS_EOL;
+    return;
+  }
+
+  #if ENABLED(PAREN_COMMENTS)
+    else if (c == '(') {        // Start inline comment
+      sis = PS_PAREN;
+      return;
+    }
+  #endif
+
+  // Backspace erases previous characters
+  if (c == 0x08) {
+    if (ind) buff[--ind] = '\0';
+  }
+  else {
+    buff[ind++] = c;
+    if (ind >= MAX_CMD_SIZE - 1)
+      sis = PS_EOL;             // Skip the rest on overflow
+  }
+}
+
+/**
+ * Handle a line being completed. For an empty line
+ * keep sensor readings going and watchdog alive.
+ */
+inline bool process_line_done(uint8_t &sis, char (&buff)[MAX_CMD_SIZE], int &ind) {
+  sis = PS_NORMAL;                    // "Normal" Serial Input State
+  buff[ind] = '\0';                   // Of course, I'm a Terminator.
+  const bool is_empty = (ind == 0);   // An empty line?
+  if (is_empty)
+    thermalManager.manage_heater();   // Keep sensors satisfied
+  else
+    ind = 0;                          // Start a new line
+  return is_empty;                    // Inform the caller
+}
+
+/**
+ * Get all commands waiting on the serial port and queue them.
+ * Exit when the buffer is full or when no more characters are
+ * left on the serial port.
+ */
+void GCodeQueue::get_serial_commands() {
+  static char serial_line_buffer[NUM_SERIAL][MAX_CMD_SIZE];
+
+  static uint8_t serial_input_state[NUM_SERIAL] = { PS_NORMAL };
+
+  #if ENABLED(BINARY_FILE_TRANSFER)
+    if (card.flag.binary_mode) {
+      /**
+       * For binary stream file transfer, use serial_line_buffer as the working
+       * receive buffer (which limits the packet size to MAX_CMD_SIZE).
+       * The receive buffer also limits the packet size for reliable transmission.
+       */
+      binaryStream[card.transfer_port_index].receive(serial_line_buffer[card.transfer_port_index]);
+      return;
+    }
+  #endif
+
+  // If the command buffer is empty for too long,
+  // send "wait" to indicate Marlin is still waiting.
+  #if NO_TIMEOUTS > 0
+    static millis_t last_command_time = 0;
+    const millis_t ms = millis();
+    if (length == 0 && !serial_data_available() && ELAPSED(ms, last_command_time + NO_TIMEOUTS)) {
+      SERIAL_ECHOLNPGM(STR_WAIT);
+      last_command_time = ms;
+    }
+  #endif
+
+  /**
+   * Loop while serial characters are incoming and the queue is not full
+   */
+  while (length < BUFSIZE && serial_data_available()) {
+    LOOP_L_N(p, NUM_SERIAL) {
+
+      const int c = read_serial(p);
+      if (c < 0) continue;
+
+      #if ENABLED(MEATPACK)
+        meatpack.handle_rx_char(uint8_t(c), p);
+        char c_res[2] = { 0, 0 };
+        const uint8_t char_count = meatpack.get_result_char(c_res);
+      #else
+        constexpr uint8_t char_count = 1;
+      #endif
+
+      LOOP_L_N(char_index, char_count) {
+        const char serial_char = TERN(MEATPACK, c_res[char_index], c);
+
+        if (ISEOL(serial_char)) {
+
+          // Reset our state, continue if the line was empty
+          if (process_line_done(serial_input_state[p], serial_line_buffer[p], serial_count[p]))
+            continue;
+
+          char* command = serial_line_buffer[p];
+
+          while (*command == ' ') command++;                   // Skip leading spaces
+          char *npos = (*command == 'N') ? command : nullptr;  // Require the N parameter to start the line
+
+          if (npos) {
+
+            const bool M110 = !!strstr_P(command, PSTR("M110"));
+
+            if (M110) {
+              char* n2pos = strchr(command + 4, 'N');
+              if (n2pos) npos = n2pos;
+            }
+
+            const long gcode_N = strtol(npos + 1, nullptr, 10);
+
+            if (gcode_N != last_N[p] + 1 && !M110)
+              return gcode_line_error(PSTR(STR_ERR_LINE_NO), p);
+
+            char *apos = strrchr(command, '*');
+            if (apos) {
+              uint8_t checksum = 0, count = uint8_t(apos - command);
+              while (count) checksum ^= command[--count];
+              if (strtol(apos + 1, nullptr, 10) != checksum)
+                return gcode_line_error(PSTR(STR_ERR_CHECKSUM_MISMATCH), p);
+            }
+            else
+              return gcode_line_error(PSTR(STR_ERR_NO_CHECKSUM), p);
+
+            last_N[p] = gcode_N;
+          }
+          #if ENABLED(SDSUPPORT)
+            // Pronterface "M29" and "M29 " has no line number
+            else if (card.flag.saving && !is_M29(command))
+              return gcode_line_error(PSTR(STR_ERR_NO_CHECKSUM), p);
+          #endif
+
+          //
+          // Movement commands give an alert when the machine is stopped
+          //
+
+          if (IsStopped()) {
+            char* gpos = strchr(command, 'G');
+            if (gpos) {
+              switch (strtol(gpos + 1, nullptr, 10)) {
+                case 0: case 1:
+                #if ENABLED(ARC_SUPPORT)
+                  case 2: case 3:
+                #endif
+                #if ENABLED(BEZIER_CURVE_SUPPORT)
+                  case 5:
+                #endif
+                  PORT_REDIRECT(SERIAL_PORTMASK(p));     // Reply to the serial port that sent the command
+                  SERIAL_ECHOLNPGM(STR_ERR_STOPPED);
+                  LCD_MESSAGEPGM(MSG_STOPPED);
+                  break;
+              }
+            }
+          }
+
+          #if DISABLED(EMERGENCY_PARSER)
+            // Process critical commands early
+            if (command[0] == 'M') switch (command[3]) {
+              case '8': if (command[2] == '0' && command[1] == '1') { wait_for_heatup = false; TERN_(HAS_LCD_MENU, wait_for_user = false); } break;
+              case '2': if (command[2] == '1' && command[1] == '1') kill(M112_KILL_STR, nullptr, true); break;
+              case '0': if (command[1] == '4' && command[2] == '1') quickstop_stepper(); break;
+            }
+          #endif
+
+          #if defined(NO_TIMEOUTS) && NO_TIMEOUTS > 0
+            last_command_time = ms;
+          #endif
+
+          // Add the command to the queue
+          _enqueue(serial_line_buffer[p], true
+            #if HAS_MULTI_SERIAL
+              , p
+            #endif
+          );
+        }
+        else
+          process_stream_char(serial_char, serial_input_state[p], serial_line_buffer[p], serial_count[p]);
+
+      } // char_count loop
+
+    } // NUM_SERIAL loop
+  } // queue has space, serial has data
+}
+
+#if ENABLED(SDSUPPORT)
+
+  /**
+   * Get lines from the SD Card until the command buffer is full
+   * or until the end of the file is reached. Because this method
+   * always receives complete command-lines, they can go directly
+   * into the main command queue.
+   */
+  inline void GCodeQueue::get_sdcard_commands() {
+    static uint8_t sd_input_state = PS_NORMAL;
+
+    if (!IS_SD_PRINTING()) return;
+
+    int sd_count = 0;
+    while (length < BUFSIZE && !card.eof()) {
+      const int16_t n = card.get();
+      const bool card_eof = card.eof();
+      if (n < 0 && !card_eof) { SERIAL_ERROR_MSG(STR_SD_ERR_READ); continue; }
+
+      const char sd_char = (char)n;
+      const bool is_eol = ISEOL(sd_char);
+      if (is_eol || card_eof) {
+
+        // Reset stream state, terminate the buffer, and commit a non-empty command
+        if (!is_eol && sd_count) ++sd_count;          // End of file with no newline
+        if (!process_line_done(sd_input_state, command_buffer[index_w], sd_count)) {
+
+          // M808 S saves the sdpos of the next line. M808 loops to a new sdpos.
+          TERN_(GCODE_REPEAT_MARKERS, repeat.early_parse_M808(command_buffer[index_w]));
+
+          // Put the new command into the buffer (no "ok" sent)
+          _commit_command(false);
+
+          // Prime Power-Loss Recovery for the NEXT _commit_command
+          TERN_(POWER_LOSS_RECOVERY, recovery.cmd_sdpos = card.getIndex());
+        }
+
+        if (card.eof()) card.fileHasFinished();         // Handle end of file reached
+      }
+      else
+        process_stream_char(sd_char, sd_input_state, command_buffer[index_w], sd_count);
+    }
+  }
+
+#endif // SDSUPPORT
+
+/**
+ * Add to the circular command queue the next command from:
+ *  - The command-injection queues (injected_commands_P, injected_commands)
+ *  - The active serial input (usually USB)
+ *  - The SD card file being actively printed
+ */
+void GCodeQueue::get_available_commands() {
+
+  get_serial_commands();
+
+  TERN_(SDSUPPORT, get_sdcard_commands());
+}
+
+/**
+ * Get the next command in the queue, optionally log it to SD, then dispatch it
+ */
+void GCodeQueue::advance() {
+
+  // Process immediate commands
+  if (process_injected_command_P() || process_injected_command()) return;
+
+  // Return if the G-code buffer is empty
+  if (!length) return;
+
+  #if ENABLED(SDSUPPORT)
+
+    if (card.flag.saving) {
+      char* command = command_buffer[index_r];
+      if (is_M29(command)) {
+        // M29 closes the file
+        card.closefile();
+        SERIAL_ECHOLNPGM(STR_FILE_SAVED);
+
+        #if !defined(__AVR__) || !defined(USBCON)
+          #if ENABLED(SERIAL_STATS_DROPPED_RX)
+            SERIAL_ECHOLNPAIR("Dropped bytes: ", MYSERIAL0.dropped());
+          #endif
+          #if ENABLED(SERIAL_STATS_MAX_RX_QUEUED)
+            SERIAL_ECHOLNPAIR("Max RX Queue Size: ", MYSERIAL0.rxMaxEnqueued());
+          #endif
+        #endif
+
+        ok_to_send();
+      }
+      else {
+        // Write the string from the read buffer to SD
+        card.write_command(command);
+        if (card.flag.logging)
+          gcode.process_next_command(); // The card is saving because it's logging
+        else
+          ok_to_send();
+      }
+    }
+    else
+      gcode.process_next_command();
+
+  #else
+
+    gcode.process_next_command();
+
+  #endif // SDSUPPORT
+
+  // The queue may be reset by a command handler or by code invoked by idle() within a handler
+  --length;
+  if (++index_r >= BUFSIZE) index_r = 0;
+
+}
diff --git a/Marlin/src/gcode/queue.h b/Marlin/src/gcode/queue.h
new file mode 100644
index 0000000..d677146
--- /dev/null
+++ b/Marlin/src/gcode/queue.h
@@ -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 <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * queue.h - The G-code command queue, which holds commands before they
+ *           go to the parser and dispatcher.
+ */
+
+#include "../inc/MarlinConfig.h"
+
+class GCodeQueue {
+public:
+  /**
+   * GCode line number handling. Hosts may include line numbers when sending
+   * commands to Marlin, and lines will be checked for sequentiality.
+   * M110 N<int> sets the current line number.
+   */
+
+  static long last_N[NUM_SERIAL];
+
+  /**
+   * GCode Command Queue
+   * A simple ring buffer of BUFSIZE command strings.
+   *
+   * Commands are copied into this buffer by the command injectors
+   * (immediate, serial, sd card) and they are processed sequentially by
+   * the main loop. The gcode.process_next_command method parses the next
+   * command and hands off execution to individual handler functions.
+   */
+  static uint8_t length,  // Count of commands in the queue
+                 index_r; // Ring buffer read position
+
+  static char command_buffer[BUFSIZE][MAX_CMD_SIZE];
+
+  /**
+   * The port that the command was received on
+   */
+  #if HAS_MULTI_SERIAL
+    static serial_index_t port[BUFSIZE];
+  #endif
+  static inline serial_index_t command_port() { return TERN0(HAS_MULTI_SERIAL, port[index_r]); }
+
+  GCodeQueue();
+
+  /**
+   * Clear the Marlin command queue
+   */
+  static void clear();
+
+  /**
+   * Next Injected Command (PROGMEM) pointer. (nullptr == empty)
+   * Internal commands are enqueued ahead of serial / SD commands.
+   */
+  static PGM_P injected_commands_P;
+
+  /**
+   * Injected Commands (SRAM)
+   */
+  static char injected_commands[64];
+
+  /**
+   * Enqueue command(s) to run from PROGMEM. Drained by process_injected_command_P().
+   * Don't inject comments or use leading spaces!
+   * Aborts the current PROGMEM queue so only use for one or two commands.
+   */
+  static inline void inject_P(PGM_P const pgcode) { injected_commands_P = pgcode; }
+
+  /**
+   * Enqueue command(s) to run from SRAM. Drained by process_injected_command().
+   * Aborts the current SRAM queue so only use for one or two commands.
+   */
+  static inline void inject(char * const gcode) {
+    strncpy(injected_commands, gcode, sizeof(injected_commands) - 1);
+  }
+
+  /**
+   * Enqueue and return only when commands are actually enqueued
+   */
+  static void enqueue_one_now(const char* cmd);
+
+  /**
+   * Attempt to enqueue a single G-code command
+   * and return 'true' if successful.
+   */
+  static bool enqueue_one_P(PGM_P const pgcode);
+
+  /**
+   * Enqueue from program memory and return only when commands are actually enqueued
+   */
+  static void enqueue_now_P(PGM_P const cmd);
+
+  /**
+   * Check whether there are any commands yet to be executed
+   */
+  static bool has_commands_queued();
+
+  /**
+   * Get the next command in the queue, optionally log it to SD, then dispatch it
+   */
+  static void advance();
+
+  /**
+   * Add to the circular command queue the next command from:
+   *  - The command-injection queue (injected_commands_P)
+   *  - The active serial input (usually USB)
+   *  - The SD card file being actively printed
+   */
+  static void get_available_commands();
+
+  /**
+   * Send an "ok" message to the host, indicating
+   * that a command was successfully processed.
+   *
+   * If ADVANCED_OK is enabled also include:
+   *   N<int>  Line number of the command, if any
+   *   P<int>  Planner space remaining
+   *   B<int>  Block queue space remaining
+   */
+  static void ok_to_send();
+
+  /**
+   * Clear the serial line and request a resend of
+   * the next expected line number.
+   */
+  static void flush_and_request_resend();
+
+private:
+
+  static uint8_t index_w;  // Ring buffer write position
+
+  static void get_serial_commands();
+
+  #if ENABLED(SDSUPPORT)
+    static void get_sdcard_commands();
+  #endif
+
+  static void _commit_command(bool say_ok
+    #if HAS_MULTI_SERIAL
+      , serial_index_t serial_ind=-1
+    #endif
+  );
+
+  static bool _enqueue(const char* cmd, bool say_ok=false
+    #if HAS_MULTI_SERIAL
+      , serial_index_t serial_ind=-1
+    #endif
+  );
+
+  // Process the next "immediate" command (PROGMEM)
+  static bool process_injected_command_P();
+
+  // Process the next "immediate" command (SRAM)
+  static bool process_injected_command();
+
+  /**
+   * Enqueue with Serial Echo
+   * Return true on success
+   */
+  static bool enqueue_one(const char* cmd);
+
+  static void gcode_line_error(PGM_P const err, const serial_index_t serial_ind);
+
+};
+
+extern GCodeQueue queue;
+
+extern const char G28_STR[];
diff --git a/Marlin/src/gcode/scara/M360-M364.cpp b/Marlin/src/gcode/scara/M360-M364.cpp
new file mode 100644
index 0000000..562beee
--- /dev/null
+++ b/Marlin/src/gcode/scara/M360-M364.cpp
@@ -0,0 +1,81 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(MORGAN_SCARA)
+
+#include "../gcode.h"
+#include "../../module/scara.h"
+#include "../../module/motion.h"
+#include "../../MarlinCore.h" // for IsRunning()
+
+inline bool SCARA_move_to_cal(const uint8_t delta_a, const uint8_t delta_b) {
+  if (IsRunning()) {
+    forward_kinematics_SCARA(delta_a, delta_b);
+    do_blocking_move_to_xy(cartes);
+    return true;
+  }
+  return false;
+}
+
+/**
+ * M360: SCARA calibration: Move to cal-position ThetaA (0 deg calibration)
+ */
+bool GcodeSuite::M360() {
+  SERIAL_ECHOLNPGM(" Cal: Theta 0");
+  return SCARA_move_to_cal(0, 120);
+}
+
+/**
+ * M361: SCARA calibration: Move to cal-position ThetaB (90 deg calibration - steps per degree)
+ */
+bool GcodeSuite::M361() {
+  SERIAL_ECHOLNPGM(" Cal: Theta 90");
+  return SCARA_move_to_cal(90, 130);
+}
+
+/**
+ * M362: SCARA calibration: Move to cal-position PsiA (0 deg calibration)
+ */
+bool GcodeSuite::M362() {
+  SERIAL_ECHOLNPGM(" Cal: Psi 0");
+  return SCARA_move_to_cal(60, 180);
+}
+
+/**
+ * M363: SCARA calibration: Move to cal-position PsiB (90 deg calibration - steps per degree)
+ */
+bool GcodeSuite::M363() {
+  SERIAL_ECHOLNPGM(" Cal: Psi 90");
+  return SCARA_move_to_cal(50, 90);
+}
+
+/**
+ * M364: SCARA calibration: Move to cal-position PsiC (90 deg to Theta calibration position)
+ */
+bool GcodeSuite::M364() {
+  SERIAL_ECHOLNPGM(" Cal: Theta-Psi 90");
+  return SCARA_move_to_cal(45, 135);
+}
+
+#endif // MORGAN_SCARA
diff --git a/Marlin/src/gcode/sd/M1001.cpp b/Marlin/src/gcode/sd/M1001.cpp
new file mode 100644
index 0000000..1cf700a
--- /dev/null
+++ b/Marlin/src/gcode/sd/M1001.cpp
@@ -0,0 +1,111 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(SDSUPPORT)
+
+#include "../gcode.h"
+#include "../../module/printcounter.h"
+
+#if DISABLED(NO_SD_AUTOSTART)
+  #include "../../sd/cardreader.h"
+#endif
+
+#ifdef SD_FINISHED_RELEASECOMMAND
+  #include "../queue.h"
+#endif
+
+#if EITHER(LCD_SET_PROGRESS_MANUALLY, SD_REPRINT_LAST_SELECTED_FILE)
+  #include "../../lcd/marlinui.h"
+#endif
+
+#if ENABLED(POWER_LOSS_RECOVERY)
+  #include "../../feature/powerloss.h"
+#endif
+
+#if HAS_LEDS_OFF_FLAG
+  #include "../../MarlinCore.h" // for wait_for_user_response()
+  #include "../../feature/leds/printer_event_leds.h"
+#endif
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../lcd/extui/ui_api.h"
+#endif
+
+#if ENABLED(HOST_ACTION_COMMANDS)
+  #include "../../feature/host_actions.h"
+#endif
+
+#ifndef PE_LEDS_COMPLETED_TIME
+  #define PE_LEDS_COMPLETED_TIME (30*60)
+#endif
+
+/**
+ * M1001: Execute actions for SD print completion
+ */
+void GcodeSuite::M1001() {
+  // If there's another auto#.g file to run...
+  if (TERN(NO_SD_AUTOSTART, false, card.autofile_check())) return;
+
+  // Purge the recovery file...
+  TERN_(POWER_LOSS_RECOVERY, recovery.purge());
+
+  // Report total print time
+  const bool long_print = print_job_timer.duration() > 60;
+  if (long_print) gcode.process_subcommands_now_P(PSTR("M31"));
+
+  // Stop the print job timer
+  gcode.process_subcommands_now_P(PSTR("M77"));
+
+  // Set the progress bar "done" state
+  TERN_(LCD_SET_PROGRESS_MANUALLY, ui.set_progress_done());
+
+  // Announce SD file completion
+  {
+    PORT_REDIRECT(SERIAL_ALL);
+    SERIAL_ECHOLNPGM(STR_FILE_PRINTED);
+  }
+
+  // Update the status LED color
+  #if HAS_LEDS_OFF_FLAG
+    if (long_print) {
+      printerEventLEDs.onPrintCompleted();
+      TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(GET_TEXT(MSG_PRINT_DONE)));
+      TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, GET_TEXT(MSG_PRINT_DONE), CONTINUE_STR));
+      wait_for_user_response(SEC_TO_MS(TERN(HAS_LCD_MENU, PE_LEDS_COMPLETED_TIME, 30)));
+      printerEventLEDs.onResumeAfterWait();
+    }
+  #endif
+
+  // Inject SD_FINISHED_RELEASECOMMAND, if any
+  #ifdef SD_FINISHED_RELEASECOMMAND
+    gcode.process_subcommands_now_P(PSTR(SD_FINISHED_RELEASECOMMAND));
+  #endif
+
+  TERN_(EXTENSIBLE_UI, ExtUI::onPrintFinished());
+
+  // Re-select the last printed file in the UI
+  TERN_(SD_REPRINT_LAST_SELECTED_FILE, ui.reselect_last_file());
+}
+
+#endif // SDSUPPORT
diff --git a/Marlin/src/gcode/sd/M20.cpp b/Marlin/src/gcode/sd/M20.cpp
new file mode 100644
index 0000000..7ac4aff
--- /dev/null
+++ b/Marlin/src/gcode/sd/M20.cpp
@@ -0,0 +1,43 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(SDSUPPORT)
+
+#include "../gcode.h"
+#include "../../sd/cardreader.h"
+
+/**
+ * M20: List SD card to serial output
+ */
+void GcodeSuite::M20() {
+  if (card.flag.mounted) {
+    SERIAL_ECHOLNPGM(STR_BEGIN_FILE_LIST);
+    card.ls();
+    SERIAL_ECHOLNPGM(STR_END_FILE_LIST);
+  }
+  else
+    SERIAL_ECHO_MSG(STR_NO_MEDIA);
+}
+
+#endif // SDSUPPORT
diff --git a/Marlin/src/gcode/sd/M21_M22.cpp b/Marlin/src/gcode/sd/M21_M22.cpp
new file mode 100644
index 0000000..f42784d
--- /dev/null
+++ b/Marlin/src/gcode/sd/M21_M22.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 <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(SDSUPPORT)
+
+#include "../gcode.h"
+#include "../../sd/cardreader.h"
+#include "../../lcd/marlinui.h"
+
+/**
+ * M21: Init SD Card
+ */
+void GcodeSuite::M21() { card.mount(); }
+
+/**
+ * M22: Release SD Card
+ */
+void GcodeSuite::M22() {
+  if (!IS_SD_PRINTING()) card.release();
+  IF_ENABLED(TFT_COLOR_UI, ui.refresh(LCDVIEW_CALL_REDRAW_NEXT));
+}
+
+#endif // SDSUPPORT
diff --git a/Marlin/src/gcode/sd/M23.cpp b/Marlin/src/gcode/sd/M23.cpp
new file mode 100644
index 0000000..51bc824
--- /dev/null
+++ b/Marlin/src/gcode/sd/M23.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 <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(SDSUPPORT)
+
+#include "../gcode.h"
+#include "../../sd/cardreader.h"
+#include "../../lcd/marlinui.h"
+
+/**
+ * M23: Open a file
+ *
+ * The path is relative to the root directory
+ */
+void GcodeSuite::M23() {
+  // Simplify3D includes the size, so zero out all spaces (#7227)
+  for (char *fn = parser.string_arg; *fn; ++fn) if (*fn == ' ') *fn = '\0';
+  card.openFileRead(parser.string_arg);
+
+  TERN_(LCD_SET_PROGRESS_MANUALLY, ui.set_progress(0));
+}
+
+#endif // SDSUPPORT
diff --git a/Marlin/src/gcode/sd/M24_M25.cpp b/Marlin/src/gcode/sd/M24_M25.cpp
new file mode 100644
index 0000000..611ba17
--- /dev/null
+++ b/Marlin/src/gcode/sd/M24_M25.cpp
@@ -0,0 +1,115 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(SDSUPPORT)
+
+#include "../gcode.h"
+#include "../../sd/cardreader.h"
+#include "../../module/printcounter.h"
+#include "../../lcd/marlinui.h"
+
+#if ENABLED(PARK_HEAD_ON_PAUSE)
+  #include "../../feature/pause.h"
+#endif
+
+#if ENABLED(HOST_ACTION_COMMANDS)
+  #include "../../feature/host_actions.h"
+#endif
+
+#if ENABLED(POWER_LOSS_RECOVERY)
+  #include "../../feature/powerloss.h"
+#endif
+
+#include "../../MarlinCore.h" // for startOrResumeJob
+
+/**
+ * M24: Start or Resume SD Print
+ */
+void GcodeSuite::M24() {
+
+  #if ENABLED(POWER_LOSS_RECOVERY)
+    if (parser.seenval('S')) card.setIndex(parser.value_long());
+    if (parser.seenval('T')) print_job_timer.resume(parser.value_long());
+  #endif
+
+  #if ENABLED(PARK_HEAD_ON_PAUSE)
+    if (did_pause_print) {
+      resume_print(); // will call print_job_timer.start()
+      return;
+    }
+  #endif
+
+  if (card.isFileOpen()) {
+    card.startFileprint();            // SD card will now be read for commands
+    startOrResumeJob();               // Start (or resume) the print job timer
+    TERN_(POWER_LOSS_RECOVERY, recovery.prepare());
+  }
+
+  #if ENABLED(HOST_ACTION_COMMANDS)
+    #ifdef ACTION_ON_RESUME
+      host_action_resume();
+    #endif
+    TERN_(HOST_PROMPT_SUPPORT, host_prompt_open(PROMPT_INFO, PSTR("Resuming SD"), DISMISS_STR));
+  #endif
+
+  ui.reset_status();
+}
+
+/**
+ * M25: Pause SD Print
+ */
+void GcodeSuite::M25() {
+
+  #if ENABLED(PARK_HEAD_ON_PAUSE)
+
+    M125();
+
+  #else
+
+    // Set initial pause flag to prevent more commands from landing in the queue while we try to pause
+    #if ENABLED(SDSUPPORT)
+      if (IS_SD_PRINTING()) card.pauseSDPrint();
+    #endif
+
+    #if ENABLED(POWER_LOSS_RECOVERY)
+      if (recovery.enabled) recovery.save(true);
+    #endif
+
+    print_job_timer.pause();
+
+    #if DISABLED(DWIN_CREALITY_LCD)
+      ui.reset_status();
+    #endif
+
+    #if ENABLED(HOST_ACTION_COMMANDS)
+      TERN_(HOST_PROMPT_SUPPORT, host_prompt_open(PROMPT_PAUSE_RESUME, PSTR("Pause SD"), PSTR("Resume")));
+      #ifdef ACTION_ON_PAUSE
+        host_action_pause();
+      #endif
+    #endif
+
+  #endif
+}
+
+#endif // SDSUPPORT
diff --git a/Marlin/src/gcode/sd/M26.cpp b/Marlin/src/gcode/sd/M26.cpp
new file mode 100644
index 0000000..e0557bf
--- /dev/null
+++ b/Marlin/src/gcode/sd/M26.cpp
@@ -0,0 +1,38 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(SDSUPPORT)
+
+#include "../gcode.h"
+#include "../../sd/cardreader.h"
+
+/**
+ * M26: Set SD Card file index
+ */
+void GcodeSuite::M26() {
+  if (card.isMounted() && parser.seenval('S'))
+    card.setIndex(parser.value_long());
+}
+
+#endif // SDSUPPORT
diff --git a/Marlin/src/gcode/sd/M27.cpp b/Marlin/src/gcode/sd/M27.cpp
new file mode 100644
index 0000000..a76070f
--- /dev/null
+++ b/Marlin/src/gcode/sd/M27.cpp
@@ -0,0 +1,52 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(SDSUPPORT)
+
+#include "../gcode.h"
+#include "../../sd/cardreader.h"
+
+/**
+ * M27: Get SD Card status
+ *      OR, with 'S<seconds>' set the SD status auto-report interval. (Requires AUTO_REPORT_SD_STATUS)
+ *      OR, with 'C' get the current filename.
+ */
+void GcodeSuite::M27() {
+  if (parser.seen('C')) {
+    SERIAL_ECHOPGM("Current file: ");
+    card.printFilename();
+    return;
+  }
+
+  #if ENABLED(AUTO_REPORT_SD_STATUS)
+    if (parser.seenval('S')) {
+      card.auto_reporter.set_interval(parser.value_byte());
+      return;
+    }
+  #endif
+
+  card.report_status();
+}
+
+#endif // SDSUPPORT
diff --git a/Marlin/src/gcode/sd/M28_M29.cpp b/Marlin/src/gcode/sd/M28_M29.cpp
new file mode 100644
index 0000000..6f3f245
--- /dev/null
+++ b/Marlin/src/gcode/sd/M28_M29.cpp
@@ -0,0 +1,72 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(SDSUPPORT)
+
+#include "../gcode.h"
+#include "../../sd/cardreader.h"
+
+#if HAS_MULTI_SERIAL
+  #include "../queue.h"
+#endif
+
+/**
+ * M28: Start SD Write
+ */
+void GcodeSuite::M28() {
+
+  #if ENABLED(BINARY_FILE_TRANSFER)
+
+    bool binary_mode = false;
+    char *p = parser.string_arg;
+    if (p[0] == 'B' && NUMERIC(p[1])) {
+      binary_mode = p[1] > '0';
+      p += 2;
+      while (*p == ' ') ++p;
+    }
+
+    // Binary transfer mode
+    if ((card.flag.binary_mode = binary_mode)) {
+      SERIAL_ECHO_MSG("Switching to Binary Protocol");
+      TERN_(HAS_MULTI_SERIAL, card.transfer_port_index = queue.port[queue.index_r]);
+    }
+    else
+      card.openFileWrite(p);
+
+  #else
+
+    card.openFileWrite(parser.string_arg);
+
+  #endif
+}
+
+/**
+ * M29: Stop SD Write
+ * (Processed in write-to-file routine)
+ */
+void GcodeSuite::M29() {
+  card.flag.saving = false;
+}
+
+#endif // SDSUPPORT
diff --git a/Marlin/src/gcode/sd/M30.cpp b/Marlin/src/gcode/sd/M30.cpp
new file mode 100644
index 0000000..b95a895
--- /dev/null
+++ b/Marlin/src/gcode/sd/M30.cpp
@@ -0,0 +1,40 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(SDSUPPORT)
+
+#include "../gcode.h"
+#include "../../sd/cardreader.h"
+
+/**
+ * M30 <filename>: Delete SD Card file
+ */
+void GcodeSuite::M30() {
+  if (card.isMounted()) {
+    card.closefile();
+    card.removeFile(parser.string_arg);
+  }
+}
+
+#endif // SDSUPPORT
diff --git a/Marlin/src/gcode/sd/M32.cpp b/Marlin/src/gcode/sd/M32.cpp
new file mode 100644
index 0000000..ea893c9
--- /dev/null
+++ b/Marlin/src/gcode/sd/M32.cpp
@@ -0,0 +1,59 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_MEDIA_SUBCALLS
+
+#include "../gcode.h"
+#include "../../sd/cardreader.h"
+#include "../../module/planner.h" // for synchronize()
+
+#include "../../MarlinCore.h" // for startOrResumeJob
+
+/**
+ * M32: Select file and start SD Print
+ *
+ * Examples:
+ *
+ *    M32 !PATH/TO/FILE.GCO#      ; Start FILE.GCO
+ *    M32 P !PATH/TO/FILE.GCO#    ; Start FILE.GCO as a procedure
+ *    M32 S60 !PATH/TO/FILE.GCO#  ; Start FILE.GCO at byte 60
+ */
+void GcodeSuite::M32() {
+  if (IS_SD_PRINTING()) planner.synchronize();
+
+  if (card.isMounted()) {
+    const uint8_t call_procedure = parser.boolval('P');
+
+    card.openFileRead(parser.string_arg, call_procedure);
+
+    if (parser.seenval('S')) card.setIndex(parser.value_long());
+
+    card.startFileprint();
+
+    // Procedure calls count as normal print time.
+    if (!call_procedure) startOrResumeJob();
+  }
+}
+
+#endif // HAS_MEDIA_SUBCALLS
diff --git a/Marlin/src/gcode/sd/M33.cpp b/Marlin/src/gcode/sd/M33.cpp
new file mode 100644
index 0000000..b611c8b
--- /dev/null
+++ b/Marlin/src/gcode/sd/M33.cpp
@@ -0,0 +1,48 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(LONG_FILENAME_HOST_SUPPORT)
+
+#include "../gcode.h"
+#include "../../sd/cardreader.h"
+
+/**
+ * M33: Get the long full path of a file or folder
+ *
+ * Parameters:
+ *   <dospath> Case-insensitive DOS-style path to a file or folder
+ *
+ * Example:
+ *   M33 miscel~1/armchair/armcha~1.gco
+ *
+ * Output:
+ *   /Miscellaneous/Armchair/Armchair.gcode
+ */
+void GcodeSuite::M33() {
+
+  card.printLongPath(parser.string_arg);
+
+}
+
+#endif // LONG_FILENAME_HOST_SUPPORT
diff --git a/Marlin/src/gcode/sd/M34.cpp b/Marlin/src/gcode/sd/M34.cpp
new file mode 100644
index 0000000..2dd7dc5
--- /dev/null
+++ b/Marlin/src/gcode/sd/M34.cpp
@@ -0,0 +1,42 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if BOTH(SDCARD_SORT_ALPHA, SDSORT_GCODE)
+
+#include "../gcode.h"
+#include "../../sd/cardreader.h"
+
+/**
+ * M34: Set SD Card Sorting Options
+ */
+void GcodeSuite::M34() {
+  if (parser.seen('S')) card.setSortOn(parser.value_bool());
+  if (parser.seenval('F')) {
+    const int v = parser.value_long();
+    card.setSortFolders(v < 0 ? -1 : v > 0 ? 1 : 0);
+  }
+  //if (parser.seen('R')) card.setSortReverse(parser.value_bool());
+}
+
+#endif // SDCARD_SORT_ALPHA && SDSORT_GCODE
diff --git a/Marlin/src/gcode/sd/M524.cpp b/Marlin/src/gcode/sd/M524.cpp
new file mode 100644
index 0000000..089d2e2
--- /dev/null
+++ b/Marlin/src/gcode/sd/M524.cpp
@@ -0,0 +1,42 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(SDSUPPORT)
+
+#include "../gcode.h"
+#include "../../sd/cardreader.h"
+
+/**
+ * M524: Abort the current SD print job (started with M24)
+ */
+void GcodeSuite::M524() {
+
+  if (IS_SD_PRINTING())
+    card.flag.abort_sd_printing = true;
+  else if (card.isMounted())
+    card.closefile();
+
+}
+
+#endif // SDSUPPORT
diff --git a/Marlin/src/gcode/sd/M808.cpp b/Marlin/src/gcode/sd/M808.cpp
new file mode 100644
index 0000000..0d11b16
--- /dev/null
+++ b/Marlin/src/gcode/sd/M808.cpp
@@ -0,0 +1,51 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(GCODE_REPEAT_MARKERS)
+
+#include "../gcode.h"
+#include "../../feature/repeat.h"
+
+/**
+ * M808: Set / Goto a repeat marker
+ *
+ *  L<count> - Set a repeat marker with 'count' repetitions. If omitted, infinity.
+ *
+ * Examples:
+ *
+ *    M808 L   ; Set a loop marker with a count of infinity
+ *    M808 L2  ; Set a loop marker with a count of 2
+ *    M808     ; Decrement and loop if not zero.
+ */
+void GcodeSuite::M808() {
+
+  // Handled early and ignored here in the queue.
+  // Allowed to go into the queue for logging purposes.
+
+  // M808 K sent from the host to cancel all loops
+  if (parser.seen('K')) repeat.cancel();
+
+}
+
+#endif // GCODE_REPEAT_MARKERS
diff --git a/Marlin/src/gcode/sd/M928.cpp b/Marlin/src/gcode/sd/M928.cpp
new file mode 100644
index 0000000..03a7877
--- /dev/null
+++ b/Marlin/src/gcode/sd/M928.cpp
@@ -0,0 +1,39 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(SDSUPPORT)
+
+#include "../gcode.h"
+#include "../../sd/cardreader.h"
+
+/**
+ * M928: Start SD Logging
+ */
+void GcodeSuite::M928() {
+
+  card.openLogFile(parser.string_arg);
+
+}
+
+#endif // SDSUPPORT
diff --git a/Marlin/src/gcode/stats/M31.cpp b/Marlin/src/gcode/stats/M31.cpp
new file mode 100644
index 0000000..207f9e1
--- /dev/null
+++ b/Marlin/src/gcode/stats/M31.cpp
@@ -0,0 +1,40 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../core/serial.h"
+#include "../../module/printcounter.h"
+#include "../../libs/duration_t.h"
+#include "../../lcd/marlinui.h"
+
+/**
+ * M31: Get the time since the start of SD Print (or last M109)
+ */
+void GcodeSuite::M31() {
+  char buffer[22];
+  duration_t(print_job_timer.duration()).toString(buffer);
+
+  ui.set_status(buffer);
+
+  SERIAL_ECHO_START();
+  SERIAL_ECHOLNPAIR("Print time: ", buffer);
+}
diff --git a/Marlin/src/gcode/stats/M75-M78.cpp b/Marlin/src/gcode/stats/M75-M78.cpp
new file mode 100644
index 0000000..568d9b0
--- /dev/null
+++ b/Marlin/src/gcode/stats/M75-M78.cpp
@@ -0,0 +1,73 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../module/printcounter.h"
+#include "../../lcd/marlinui.h"
+
+#include "../../MarlinCore.h" // for startOrResumeJob
+
+/**
+ * M75: Start print timer
+ */
+void GcodeSuite::M75() {
+  startOrResumeJob();
+}
+
+/**
+ * M76: Pause print timer
+ */
+void GcodeSuite::M76() {
+  print_job_timer.pause();
+}
+
+/**
+ * M77: Stop print timer
+ */
+void GcodeSuite::M77() {
+  print_job_timer.stop();
+}
+
+#if ENABLED(PRINTCOUNTER)
+
+/**
+ * M78: Show print statistics
+ */
+void GcodeSuite::M78() {
+  if (parser.intval('S') == 78) {  // "M78 S78" will reset the statistics
+    print_job_timer.initStats();
+    ui.reset_status();
+    return;
+  }
+
+  #if HAS_SERVICE_INTERVALS
+    if (parser.seenval('R')) {
+      print_job_timer.resetServiceInterval(parser.value_int());
+      ui.reset_status();
+      return;
+    }
+  #endif
+
+  print_job_timer.showStats();
+}
+
+#endif // PRINTCOUNTER
diff --git a/Marlin/src/gcode/temp/M104_M109.cpp b/Marlin/src/gcode/temp/M104_M109.cpp
new file mode 100644
index 0000000..07e46e1
--- /dev/null
+++ b/Marlin/src/gcode/temp/M104_M109.cpp
@@ -0,0 +1,200 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * gcode/temp/M104_M109.cpp
+ *
+ * Hotend target temperature control
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if EXTRUDERS
+
+#include "../gcode.h"
+#include "../../module/temperature.h"
+#include "../../module/motion.h"
+#include "../../module/planner.h"
+#include "../../lcd/marlinui.h"
+
+#include "../../MarlinCore.h" // for startOrResumeJob, etc.
+
+#if ENABLED(PRINTJOB_TIMER_AUTOSTART)
+  #include "../../module/printcounter.h"
+  #if ENABLED(CANCEL_OBJECTS)
+    #include "../../feature/cancel_object.h"
+  #endif
+#endif
+
+#if ENABLED(SINGLENOZZLE_STANDBY_TEMP)
+  #include "../../module/tool_change.h"
+#endif
+
+/**
+ * M104: Set Hotend Temperature target and return immediately
+ *
+ * Parameters:
+ *  I<preset> : Material Preset index (if material presets are defined)
+ *  T<index>  : Tool index. If omitted, applies to the active tool
+ *  S<target> : The target temperature in current units
+ */
+void GcodeSuite::M104() {
+
+  if (DEBUGGING(DRYRUN)) return;
+
+  #if ENABLED(MIXING_EXTRUDER) && MIXING_VIRTUAL_TOOLS > 1
+    constexpr int8_t target_extruder = 0;
+  #else
+    const int8_t target_extruder = get_target_extruder_from_command();
+    if (target_extruder < 0) return;
+  #endif
+
+  bool got_temp = false;
+  int16_t temp = 0;
+
+  // Accept 'I' if temperature presets are defined
+  #if PREHEAT_COUNT
+    got_temp = parser.seenval('I');
+    if (got_temp) {
+      const uint8_t index = parser.value_byte();
+      temp = ui.material_preset[_MIN(index, PREHEAT_COUNT - 1)].hotend_temp;
+    }
+  #endif
+
+  // If no 'I' get the temperature from 'S'
+  if (!got_temp) {
+    got_temp = parser.seenval('S');
+    if (got_temp) temp = parser.value_celsius();
+  }
+
+  if (got_temp) {
+    #if ENABLED(SINGLENOZZLE_STANDBY_TEMP)
+      thermalManager.singlenozzle_temp[target_extruder] = temp;
+      if (target_extruder != active_extruder) return;
+    #endif
+    thermalManager.setTargetHotend(temp, target_extruder);
+
+    #if ENABLED(DUAL_X_CARRIAGE)
+      if (idex_is_duplicating() && target_extruder == 0)
+        thermalManager.setTargetHotend(temp ? temp + duplicate_extruder_temp_offset : 0, 1);
+    #endif
+
+    #if ENABLED(PRINTJOB_TIMER_AUTOSTART)
+      /**
+       * Stop the timer at the end of print. Start is managed by 'heat and wait' M109.
+       * Hotends use EXTRUDE_MINTEMP / 2 to allow nozzles to be put into hot standby
+       * mode, for instance in a dual extruder setup, without affecting the running
+       * print timer.
+       */
+      thermalManager.auto_job_check_timer(false, true);
+    #endif
+  }
+
+  TERN_(AUTOTEMP, planner.autotemp_M104_M109());
+}
+
+/**
+ * M109: Set Hotend Temperature target and wait
+ *
+ * Parameters
+ *  I<preset> : Material Preset index (if material presets are defined)
+ *  T<index>  : Tool index. If omitted, applies to the active tool
+ *  S<target> : The target temperature in current units. Wait for heating only.
+ *  R<target> : The target temperature in current units. Wait for heating and cooling.
+ *
+ * With AUTOTEMP...
+ *  F<factor> : Autotemp Scaling Factor. Set non-zero to enable Auto-temp.
+ *  S<min>    : Minimum temperature, in current units.
+ *  B<max>    : Maximum temperature, in current units.
+ *
+ * Examples
+ *  M109 S100 : Set target to 100°. Wait until the hotend is at or above 100°.
+ *  M109 R150 : Set target to 150°. Wait until the hotend gets close to 150°.
+ *
+ * With PRINTJOB_TIMER_AUTOSTART turning on heaters will start the print job timer
+ *  (used by printingIsActive, etc.) and turning off heaters will stop the timer.
+ */
+void GcodeSuite::M109() {
+
+  if (DEBUGGING(DRYRUN)) return;
+
+  #if ENABLED(MIXING_EXTRUDER) && MIXING_VIRTUAL_TOOLS > 1
+    constexpr int8_t target_extruder = 0;
+  #else
+    const int8_t target_extruder = get_target_extruder_from_command();
+    if (target_extruder < 0) return;
+  #endif
+
+  bool got_temp = false;
+  int16_t temp = 0;
+
+  // Accept 'I' if temperature presets are defined
+  #if PREHEAT_COUNT
+    got_temp = parser.seenval('I');
+    if (got_temp) {
+      const uint8_t index = parser.value_byte();
+      temp = ui.material_preset[_MIN(index, PREHEAT_COUNT - 1)].hotend_temp;
+    }
+  #endif
+
+  // Get the temperature from 'S' or 'R'
+  bool no_wait_for_cooling = false;
+  if (!got_temp) {
+    no_wait_for_cooling = parser.seenval('S');
+    got_temp = no_wait_for_cooling || parser.seenval('R');
+    if (got_temp) temp = int16_t(parser.value_celsius());
+  }
+
+  if (got_temp) {
+    #if ENABLED(SINGLENOZZLE_STANDBY_TEMP)
+      thermalManager.singlenozzle_temp[target_extruder] = temp;
+      if (target_extruder != active_extruder) return;
+    #endif
+    thermalManager.setTargetHotend(temp, target_extruder);
+
+    #if ENABLED(DUAL_X_CARRIAGE)
+      if (idex_is_duplicating() && target_extruder == 0)
+        thermalManager.setTargetHotend(temp ? temp + duplicate_extruder_temp_offset : 0, 1);
+    #endif
+
+    #if ENABLED(PRINTJOB_TIMER_AUTOSTART)
+      /**
+       * Use half EXTRUDE_MINTEMP to allow nozzles to be put into hot
+       * standby mode, (e.g., in a dual extruder setup) without affecting
+       * the running print timer.
+       */
+      thermalManager.auto_job_check_timer(true, true);
+    #endif
+
+    #if HAS_DISPLAY
+      if (thermalManager.isHeatingHotend(target_extruder) || !no_wait_for_cooling)
+        thermalManager.set_heating_message(target_extruder);
+    #endif
+  }
+
+  TERN_(AUTOTEMP, planner.autotemp_M104_M109());
+
+  if (got_temp)
+    (void)thermalManager.wait_for_hotend(target_extruder, no_wait_for_cooling);
+}
+
+#endif // EXTRUDERS
diff --git a/Marlin/src/gcode/temp/M105.cpp b/Marlin/src/gcode/temp/M105.cpp
new file mode 100644
index 0000000..eefc3ae
--- /dev/null
+++ b/Marlin/src/gcode/temp/M105.cpp
@@ -0,0 +1,51 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../module/temperature.h"
+
+/**
+ * M105: Read hot end and bed temperature
+ */
+void GcodeSuite::M105() {
+
+  const int8_t target_extruder = get_target_extruder_from_command();
+  if (target_extruder < 0) return;
+
+  SERIAL_ECHOPGM(STR_OK);
+
+  #if HAS_TEMP_SENSOR
+
+    thermalManager.print_heater_states(target_extruder
+      #if ENABLED(TEMP_SENSOR_1_AS_REDUNDANT)
+        , parser.boolval('R')
+      #endif
+    );
+
+    SERIAL_EOL();
+
+  #else
+
+    SERIAL_ECHOLNPGM(" T:0"); // Some hosts send M105 to test the serial connection
+
+  #endif
+}
diff --git a/Marlin/src/gcode/temp/M106_M107.cpp b/Marlin/src/gcode/temp/M106_M107.cpp
new file mode 100644
index 0000000..9c70f1e
--- /dev/null
+++ b/Marlin/src/gcode/temp/M106_M107.cpp
@@ -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 <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_FAN
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/temperature.h"
+
+#if PREHEAT_COUNT
+  #include "../../lcd/marlinui.h"
+#endif
+
+#if ENABLED(SINGLENOZZLE)
+  #define _ALT_P active_extruder
+  #define _CNT_P EXTRUDERS
+#else
+  #define _ALT_P _MIN(active_extruder, FAN_COUNT - 1)
+  #define _CNT_P FAN_COUNT
+#endif
+
+/**
+ * M106: Set Fan Speed
+ *
+ *  I<index> Material Preset index (if material presets are defined)
+ *  S<int>   Speed between 0-255
+ *  P<index> Fan index, if more than one fan
+ *
+ * With EXTRA_FAN_SPEED enabled:
+ *
+ *  T<int>   Restore/Use/Set Temporary Speed:
+ *           1     = Restore previous speed after T2
+ *           2     = Use temporary speed set with T3-255
+ *           3-255 = Set the speed for use with T2
+ */
+void GcodeSuite::M106() {
+  const uint8_t pfan = parser.byteval('P', _ALT_P);
+
+  if (pfan < _CNT_P) {
+
+    #if ENABLED(EXTRA_FAN_SPEED)
+      const uint16_t t = parser.intval('T');
+      if (t > 0) return thermalManager.set_temp_fan_speed(pfan, t);
+    #endif
+
+    const uint16_t dspeed = parser.seen('A') ? thermalManager.fan_speed[active_extruder] : 255;
+
+    uint16_t speed = dspeed;
+
+    // Accept 'I' if temperature presets are defined
+    #if PREHEAT_COUNT
+      const bool got_preset = parser.seenval('I');
+      if (got_preset) speed = ui.material_preset[_MIN(parser.value_byte(), PREHEAT_COUNT - 1)].fan_speed;
+    #else
+      constexpr bool got_preset = false;
+    #endif
+
+    if (!got_preset && parser.seenval('S'))
+      speed = parser.value_ushort();
+
+    // Set speed, with constraint
+    thermalManager.set_fan_speed(pfan, speed);
+  }
+}
+
+/**
+ * M107: Fan Off
+ */
+void GcodeSuite::M107() {
+  const uint8_t p = parser.byteval('P', _ALT_P);
+  thermalManager.set_fan_speed(p, 0);
+}
+
+#endif // HAS_FAN
diff --git a/Marlin/src/gcode/temp/M140_M190.cpp b/Marlin/src/gcode/temp/M140_M190.cpp
new file mode 100644
index 0000000..d684127
--- /dev/null
+++ b/Marlin/src/gcode/temp/M140_M190.cpp
@@ -0,0 +1,138 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * gcode/temp/M140_M190.cpp
+ *
+ * Bed target temperature control
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_HEATED_BED
+
+#include "../gcode.h"
+#include "../../module/temperature.h"
+#include "../../module/motion.h"
+#include "../../lcd/marlinui.h"
+
+#if ENABLED(PRINTJOB_TIMER_AUTOSTART)
+  #include "../../module/printcounter.h"
+#endif
+
+#if ENABLED(PRINTER_EVENT_LEDS)
+  #include "../../feature/leds/leds.h"
+#endif
+
+#include "../../MarlinCore.h" // for wait_for_heatup, idle, startOrResumeJob
+
+/**
+ * M140: Set bed temperature
+ *
+ *  I<index>  : Preset index (if material presets are defined)
+ *  S<target> : The target temperature in current units
+ */
+void GcodeSuite::M140() {
+  if (DEBUGGING(DRYRUN)) return;
+
+  bool got_temp = false;
+  int16_t temp = 0;
+
+  // Accept 'I' if temperature presets are defined
+  #if PREHEAT_COUNT
+    got_temp = parser.seenval('I');
+    if (got_temp) {
+      const uint8_t index = parser.value_byte();
+      temp = ui.material_preset[_MIN(index, PREHEAT_COUNT - 1)].bed_temp;
+    }
+  #endif
+
+  // If no 'I' get the temperature from 'S'
+  if (!got_temp) {
+    got_temp = parser.seenval('S');
+    if (got_temp) temp = parser.value_celsius();
+  }
+
+  if (got_temp) {
+    thermalManager.setTargetBed(temp);
+
+    #if ENABLED(PRINTJOB_TIMER_AUTOSTART)
+      /**
+       * Stop the timer at the end of print. Hotend, bed target, and chamber
+       * temperatures need to be set below mintemp. Order of M140, M104, and M141
+       * at the end of the print does not matter.
+       */
+      thermalManager.auto_job_check_timer(false, true);
+    #endif
+  }
+}
+
+/**
+ * M190 - Set Bed Temperature target and wait
+ *
+ * Parameters:
+ *  I<index>  : Preset index (if material presets are defined)
+ *  S<target> : The target temperature in current units. Wait for heating only.
+ *  R<target> : The target temperature in current units. Wait for heating and cooling.
+ *
+ * Examples:
+ *  M190 S60 : Set target to 60°. Wait until the bed is at or above 60°.
+ *  M190 R40 : Set target to 40°. Wait until the bed gets close to 40°.
+ *
+ * With PRINTJOB_TIMER_AUTOSTART turning on heaters will start the print job timer
+ *  (used by printingIsActive, etc.) and turning off heaters will stop the timer.
+ */
+void GcodeSuite::M190() {
+  if (DEBUGGING(DRYRUN)) return;
+
+  bool got_temp = false;
+  int16_t temp = 0;
+
+  // Accept 'I' if temperature presets are defined
+  #if PREHEAT_COUNT
+    got_temp = parser.seenval('I');
+    if (got_temp) {
+      const uint8_t index = parser.value_byte();
+      temp = ui.material_preset[_MIN(index, PREHEAT_COUNT - 1)].bed_temp;
+    }
+  #endif
+
+  // Get the temperature from 'S' or 'R'
+  bool no_wait_for_cooling = false;
+  if (!got_temp) {
+    no_wait_for_cooling = parser.seenval('S');
+    got_temp = no_wait_for_cooling || parser.seenval('R');
+    if (got_temp) temp = int16_t(parser.value_celsius());
+  }
+
+  if (!got_temp) return;
+
+  thermalManager.setTargetBed(temp);
+
+  TERN_(PRINTJOB_TIMER_AUTOSTART, thermalManager.auto_job_check_timer(true, false));
+
+  ui.set_status_P(thermalManager.isHeatingBed() ? GET_TEXT(MSG_BED_HEATING) : GET_TEXT(MSG_BED_COOLING));
+
+  thermalManager.wait_for_bed(no_wait_for_cooling);
+}
+
+#endif // HAS_HEATED_BED
diff --git a/Marlin/src/gcode/temp/M141_M191.cpp b/Marlin/src/gcode/temp/M141_M191.cpp
new file mode 100644
index 0000000..17eb71e
--- /dev/null
+++ b/Marlin/src/gcode/temp/M141_M191.cpp
@@ -0,0 +1,89 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * gcode/temp/M141_M191.cpp
+ *
+ * Chamber target temperature control
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_HEATED_CHAMBER
+
+#include "../gcode.h"
+#include "../../module/temperature.h"
+
+#include "../../module/motion.h"
+#include "../../lcd/marlinui.h"
+
+#if ENABLED(PRINTJOB_TIMER_AUTOSTART)
+  #include "../../module/printcounter.h"
+#endif
+
+#if ENABLED(PRINTER_EVENT_LEDS)
+  #include "../../feature/leds/leds.h"
+#endif
+
+#include "../../MarlinCore.h" // for wait_for_heatup, idle, startOrResumeJob
+
+/**
+ * M141: Set chamber temperature
+ */
+void GcodeSuite::M141() {
+  if (DEBUGGING(DRYRUN)) return;
+  if (parser.seenval('S')) {
+    thermalManager.setTargetChamber(parser.value_celsius());
+
+    #if ENABLED(PRINTJOB_TIMER_AUTOSTART)
+      /**
+       * Stop the timer at the end of print. Hotend, bed target, and chamber
+       * temperatures need to be set below mintemp. Order of M140, M104, and M141
+       * at the end of the print does not matter.
+       */
+      thermalManager.auto_job_check_timer(false, true);
+    #endif
+  }
+}
+
+/**
+ * M191: Sxxx Wait for chamber current temp to reach target temp. Waits only when heating
+ *       Rxxx Wait for chamber current temp to reach target temp. Waits when heating and cooling
+ */
+void GcodeSuite::M191() {
+  if (DEBUGGING(DRYRUN)) return;
+
+  const bool no_wait_for_cooling = parser.seenval('S');
+  if (no_wait_for_cooling || parser.seenval('R')) {
+    thermalManager.setTargetChamber(parser.value_celsius());
+    TERN_(PRINTJOB_TIMER_AUTOSTART, thermalManager.auto_job_check_timer(true, false));
+  }
+  else return;
+
+  const bool is_heating = thermalManager.isHeatingChamber();
+  if (is_heating || !no_wait_for_cooling) {
+    ui.set_status_P(is_heating ? GET_TEXT(MSG_CHAMBER_HEATING) : GET_TEXT(MSG_CHAMBER_COOLING));
+    thermalManager.wait_for_chamber(false);
+  }
+}
+
+#endif // HAS_HEATED_CHAMBER
diff --git a/Marlin/src/gcode/temp/M155.cpp b/Marlin/src/gcode/temp/M155.cpp
new file mode 100644
index 0000000..48c2398
--- /dev/null
+++ b/Marlin/src/gcode/temp/M155.cpp
@@ -0,0 +1,40 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if BOTH(AUTO_REPORT_TEMPERATURES, HAS_TEMP_SENSOR)
+
+#include "../gcode.h"
+#include "../../module/temperature.h"
+
+/**
+ * M155: Set temperature auto-report interval. M155 S<seconds>
+ */
+void GcodeSuite::M155() {
+
+  if (parser.seenval('S'))
+    thermalManager.auto_reporter.set_interval(parser.value_byte());
+
+}
+
+#endif // AUTO_REPORT_TEMPERATURES && HAS_TEMP_SENSOR
diff --git a/Marlin/src/gcode/temp/M303.cpp b/Marlin/src/gcode/temp/M303.cpp
new file mode 100644
index 0000000..a066ddc
--- /dev/null
+++ b/Marlin/src/gcode/temp/M303.cpp
@@ -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 <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_PID_HEATING
+
+#include "../gcode.h"
+#include "../../lcd/marlinui.h"
+#include "../../module/temperature.h"
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../lcd/extui/ui_api.h"
+#endif
+
+/**
+ * M303: PID relay autotune
+ *
+ *  S<temperature>  Set the target temperature. (Default: 150C / 70C)
+ *  E<extruder>     Extruder number to tune, or -1 for the bed. (Default: E0)
+ *  C<cycles>       Number of times to repeat the procedure. (Minimum: 3, Default: 5)
+ *  U<bool>         Flag to apply the result to the current PID values
+ *
+ * With PID_DEBUG:
+ *  D               Toggle PID debugging and EXIT without further action.
+ */
+
+#if ENABLED(PID_DEBUG)
+  bool pid_debug_flag = 0;
+#endif
+
+void GcodeSuite::M303() {
+
+  #if ENABLED(PID_DEBUG)
+    if (parser.seen('D')) {
+      pid_debug_flag = !pid_debug_flag;
+      SERIAL_ECHO_START();
+      SERIAL_ECHOPGM("PID Debug ");
+      serialprintln_onoff(pid_debug_flag);
+      return;
+    }
+  #endif
+
+  #define SI TERN(PIDTEMPBED, H_BED, H_E0)
+  #define EI TERN(PIDTEMP, HOTENDS - 1, H_BED)
+  const heater_id_t e = (heater_id_t)parser.intval('E');
+  if (!WITHIN(e, SI, EI)) {
+    SERIAL_ECHOLNPGM(STR_PID_BAD_EXTRUDER_NUM);
+    TERN_(EXTENSIBLE_UI, ExtUI::onPidTuning(ExtUI::result_t::PID_BAD_EXTRUDER_NUM));
+    return;
+  }
+
+  const int c = parser.intval('C', 5);
+  const bool u = parser.boolval('U');
+  const int16_t temp = parser.celsiusval('S', e < 0 ? PREHEAT_1_TEMP_BED : PREHEAT_1_TEMP_HOTEND);
+
+  #if DISABLED(BUSY_WHILE_HEATING)
+    KEEPALIVE_STATE(NOT_BUSY);
+  #endif
+
+  LCD_MESSAGEPGM(MSG_PID_AUTOTUNE);
+  thermalManager.PID_autotune(temp, e, c, u);
+  ui.reset_status();
+}
+
+#endif // HAS_PID_HEATING
diff --git a/Marlin/src/gcode/units/G20_G21.cpp b/Marlin/src/gcode/units/G20_G21.cpp
new file mode 100644
index 0000000..6f1d5ad
--- /dev/null
+++ b/Marlin/src/gcode/units/G20_G21.cpp
@@ -0,0 +1,39 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(INCH_MODE_SUPPORT)
+
+#include "../gcode.h"
+
+/**
+ * G20: Set input mode to inches
+ */
+void GcodeSuite::G20() { parser.set_input_linear_units(LINEARUNIT_INCH); }
+
+/**
+ * G21: Set input mode to millimeters
+ */
+void GcodeSuite::G21() { parser.set_input_linear_units(LINEARUNIT_MM); }
+
+#endif // INCH_MODE_SUPPORT
diff --git a/Marlin/src/gcode/units/M149.cpp b/Marlin/src/gcode/units/M149.cpp
new file mode 100644
index 0000000..5d9f832
--- /dev/null
+++ b/Marlin/src/gcode/units/M149.cpp
@@ -0,0 +1,38 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(TEMPERATURE_UNITS_SUPPORT)
+
+#include "../gcode.h"
+
+/**
+ * M149: Set temperature units
+ */
+void GcodeSuite::M149() {
+       if (parser.seenval('C')) parser.set_input_temp_units(TEMPUNIT_C);
+  else if (parser.seenval('K')) parser.set_input_temp_units(TEMPUNIT_K);
+  else if (parser.seenval('F')) parser.set_input_temp_units(TEMPUNIT_F);
+}
+
+#endif // TEMPERATURE_UNITS_SUPPORT
diff --git a/Marlin/src/gcode/units/M82_M83.cpp b/Marlin/src/gcode/units/M82_M83.cpp
new file mode 100644
index 0000000..d93f0ea
--- /dev/null
+++ b/Marlin/src/gcode/units/M82_M83.cpp
@@ -0,0 +1,33 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../gcode.h"
+
+/**
+ * M82: Set E codes absolute (default)
+ */
+void GcodeSuite::M82() { set_e_absolute(); }
+
+/**
+ * M83: Set E codes relative while in Absolute Coordinates (G90) mode
+ */
+void GcodeSuite::M83() { set_e_relative(); }