src folder

src/libs/BL24CXX.cpp

@@ -0,0 +1,285 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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(IIC_BL24CXX_EEPROM)
+
+/**
+ * PersistentStore for Arduino-style EEPROM interface
+ * with simple implementations supplied by Marlin.
+ */
+
+#include "BL24CXX.h"
+#ifdef __STM32F1__
+  #include <libmaple/gpio.h>
+#else
+  #include "../HAL/shared/Delay.h"
+  #define delay_us(n) DELAY_US(n)
+#endif
+
+#ifndef EEPROM_WRITE_DELAY
+  #define EEPROM_WRITE_DELAY    10
+#endif
+#ifndef EEPROM_DEVICE_ADDRESS
+  #define EEPROM_DEVICE_ADDRESS (0x50 << 1)
+#endif
+
+// IO direction setting
+#ifdef __STM32F1__
+  #define SDA_IN()  do{ PIN_MAP[IIC_EEPROM_SDA].gpio_device->regs->CRH &= 0XFFFF0FFF; PIN_MAP[IIC_EEPROM_SDA].gpio_device->regs->CRH |= 8 << 12; }while(0)
+  #define SDA_OUT() do{ PIN_MAP[IIC_EEPROM_SDA].gpio_device->regs->CRH &= 0XFFFF0FFF; PIN_MAP[IIC_EEPROM_SDA].gpio_device->regs->CRH |= 3 << 12; }while(0)
+#elif defined(STM32F1) || defined(STM32F4)
+  #define SDA_IN()  SET_INPUT(IIC_EEPROM_SDA)
+  #define SDA_OUT() SET_OUTPUT(IIC_EEPROM_SDA)
+#endif
+
+// IO ops
+#define IIC_SCL_0() WRITE(IIC_EEPROM_SCL, LOW)
+#define IIC_SCL_1() WRITE(IIC_EEPROM_SCL, HIGH)
+#define IIC_SDA_0() WRITE(IIC_EEPROM_SDA, LOW)
+#define IIC_SDA_1() WRITE(IIC_EEPROM_SDA, HIGH)
+#define READ_SDA()  READ(IIC_EEPROM_SDA)
+
+//
+// Simple IIC interface via libmaple
+//
+
+// Initialize IIC
+void IIC::init() {
+  SET_OUTPUT(IIC_EEPROM_SDA);
+  SET_OUTPUT(IIC_EEPROM_SCL);
+  IIC_SCL_1();
+  IIC_SDA_1();
+}
+
+// Generate IIC start signal
+void IIC::start() {
+  SDA_OUT();    // SDA line output
+  IIC_SDA_1();
+  IIC_SCL_1();
+  delay_us(4);
+  IIC_SDA_0();  // START:when CLK is high, DATA change from high to low
+  delay_us(4);
+  IIC_SCL_0();  // Clamp the I2C bus, ready to send or receive data
+}
+
+// Generate IIC stop signal
+void IIC::stop() {
+  SDA_OUT();    // SDA line output
+  IIC_SCL_0();
+  IIC_SDA_0();  // STOP:when CLK is high DATA change from low to high
+  delay_us(4);
+  IIC_SCL_1();
+  IIC_SDA_1();  // Send I2C bus end signal
+  delay_us(4);
+}
+
+// Wait for the response signal to arrive
+// 1 = failed to receive response
+// 0 = response received
+uint8_t IIC::wait_ack() {
+  uint8_t ucErrTime = 0;
+  SDA_IN();      // SDA is set as input
+  IIC_SDA_1(); delay_us(1);
+  IIC_SCL_1(); delay_us(1);
+  while (READ_SDA()) {
+    if (++ucErrTime > 250) {
+      stop();
+      return 1;
+    }
+  }
+  IIC_SCL_0(); // Clock output 0
+  return 0;
+}
+
+// Generate ACK response
+void IIC::ack() {
+  IIC_SCL_0();
+  SDA_OUT();
+  IIC_SDA_0();
+  delay_us(2);
+  IIC_SCL_1();
+  delay_us(2);
+  IIC_SCL_0();
+}
+
+// No ACK response
+void IIC::nAck() {
+  IIC_SCL_0();
+  SDA_OUT();
+  IIC_SDA_1();
+  delay_us(2);
+  IIC_SCL_1();
+  delay_us(2);
+  IIC_SCL_0();
+}
+
+// Send one IIC byte
+// Return whether the slave responds
+// 1 = there is a response
+// 0 = no response
+void IIC::send_byte(uint8_t txd) {
+  SDA_OUT();
+  IIC_SCL_0(); // Pull down the clock to start data transmission
+  LOOP_L_N(t, 8) {
+    // IIC_SDA = (txd & 0x80) >> 7;
+    if (txd & 0x80) IIC_SDA_1(); else IIC_SDA_0();
+    txd <<= 1;
+    delay_us(2);   // All three delays are necessary for TEA5767
+    IIC_SCL_1();
+    delay_us(2);
+    IIC_SCL_0();
+    delay_us(2);
+  }
+}
+
+// Read 1 byte, when ack=1, send ACK, ack=0, send nACK
+uint8_t IIC::read_byte(unsigned char ack_chr) {
+  unsigned char receive = 0;
+  SDA_IN(); // SDA is set as input
+  LOOP_L_N(i, 8) {
+    IIC_SCL_0();
+    delay_us(2);
+    IIC_SCL_1();
+    receive <<= 1;
+    if (READ_SDA()) receive++;
+    delay_us(1);
+  }
+  ack_chr ? ack() : nAck(); // Send ACK / send nACK
+  return receive;
+}
+
+/******************** EEPROM ********************/
+
+// Initialize the IIC interface
+void BL24CXX::init() { IIC::init(); }
+
+// Read a byte at the specified address
+// ReadAddr: the address to start reading
+// Return: the byte read
+uint8_t BL24CXX::readOneByte(uint16_t ReadAddr) {
+  uint8_t temp = 0;
+  IIC::start();
+  if (EE_TYPE > BL24C16) {
+    IIC::send_byte(EEPROM_DEVICE_ADDRESS);        // Send write command
+    IIC::wait_ack();
+    IIC::send_byte(ReadAddr >> 8);                // Send high address
+    IIC::wait_ack();
+  }
+  else
+    IIC::send_byte(EEPROM_DEVICE_ADDRESS + ((ReadAddr >> 8) << 1)); // Send device address 0xA0, write data
+
+  IIC::wait_ack();
+  IIC::send_byte(ReadAddr & 0xFF);                // Send low address
+  IIC::wait_ack();
+  IIC::start();
+  IIC::send_byte(EEPROM_DEVICE_ADDRESS | 0x01);   // Send byte
+  IIC::wait_ack();
+  temp = IIC::read_byte(0);
+  IIC::stop();                                    // Generate a stop condition
+  return temp;
+}
+
+// Write a data at the address specified by BL24CXX
+// WriteAddr: The destination address for writing data
+// DataToWrite: the data to be written
+void BL24CXX::writeOneByte(uint16_t WriteAddr, uint8_t DataToWrite) {
+  IIC::start();
+  if (EE_TYPE > BL24C16) {
+    IIC::send_byte(EEPROM_DEVICE_ADDRESS);        // Send write command
+    IIC::wait_ack();
+    IIC::send_byte(WriteAddr >> 8);               // Send high address
+  }
+  else
+    IIC::send_byte(EEPROM_DEVICE_ADDRESS + ((WriteAddr >> 8) << 1)); // Send device address 0xA0, write data
+
+  IIC::wait_ack();
+  IIC::send_byte(WriteAddr & 0xFF);               // Send low address
+  IIC::wait_ack();
+  IIC::send_byte(DataToWrite);                    // Receiving mode
+  IIC::wait_ack();
+  IIC::stop();                                    // Generate a stop condition
+  delay(10);
+}
+
+// Start writing data of length Len at the specified address in BL24CXX
+// This function is used to write 16bit or 32bit data.
+// WriteAddr: the address to start writing
+// DataToWrite: the first address of the data array
+// Len: The length of the data to be written 2, 4
+void BL24CXX::writeLenByte(uint16_t WriteAddr, uint32_t DataToWrite, uint8_t Len) {
+  LOOP_L_N(t, Len)
+    writeOneByte(WriteAddr + t, (DataToWrite >> (8 * t)) & 0xFF);
+}
+
+// Start reading data of length Len from the specified address in BL24CXX
+// This function is used to read 16bit or 32bit data.
+// ReadAddr: the address to start reading
+// Return value: data
+// Len: The length of the data to be read 2,4
+uint32_t BL24CXX::readLenByte(uint16_t ReadAddr, uint8_t Len) {
+  uint32_t temp = 0;
+  LOOP_L_N(t, Len) {
+    temp <<= 8;
+    temp += readOneByte(ReadAddr + Len - t - 1);
+  }
+  return temp;
+}
+
+// Check if BL24CXX is normal
+// Return 1: Detection failed
+// return 0: detection is successful
+#define BL24CXX_TEST_ADDRESS 0x00
+#define BL24CXX_TEST_VALUE   0x55
+
+bool BL24CXX::_check() {
+  return (readOneByte(BL24CXX_TEST_ADDRESS) != BL24CXX_TEST_VALUE); // false = success!
+}
+
+bool BL24CXX::check() {
+  if (_check()) {                                           // Value was written? Good EEPROM!
+    writeOneByte(BL24CXX_TEST_ADDRESS, BL24CXX_TEST_VALUE); // Write now and check.
+    return _check();
+  }
+  return false; // success!
+}
+
+// Start reading the specified number of data at the specified address in BL24CXX
+// ReadAddr: The address to start reading is 0~255 for 24c02
+// pBuffer: the first address of the data array
+// NumToRead: the number of data to be read
+void BL24CXX::read(uint16_t ReadAddr, uint8_t *pBuffer, uint16_t NumToRead) {
+  for (; NumToRead; NumToRead--)
+    *pBuffer++ = readOneByte(ReadAddr++);
+}
+
+// Start writing the specified number of data at the specified address in BL24CXX
+// WriteAddr: the address to start writing, 0~255 for 24c02
+// pBuffer: the first address of the data array
+// NumToWrite: the number of data to be written
+void BL24CXX::write(uint16_t WriteAddr, uint8_t *pBuffer, uint16_t NumToWrite) {
+  for (; NumToWrite; NumToWrite--, WriteAddr++)
+    writeOneByte(WriteAddr, *pBuffer++);
+}
+
+#endif // IIC_BL24CXX_EEPROM

src/libs/BL24CXX.h

@@ -0,0 +1,72 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/********************************************************************************
+ * @file     BL24CXX.h
+ * @brief    i2c EEPROM for Ender 3 v2 board (4.2.2)
+ ********************************************************************************/
+
+/******************** IIC ********************/
+
+class BL24CXX;
+
+// All operation functions of IIC
+class IIC {
+friend class BL24CXX;
+protected:
+  static void init();                // Initialize the IO port of IIC
+  static void start();               // Send IIC start signal
+  static void stop();                // Send IIC stop signal
+  static void send_byte(uint8_t txd); // IIC sends a byte
+  static uint8_t read_byte(unsigned char ack); // IIC reads a byte
+  static uint8_t wait_ack();         // IIC waits for ACK signal
+  static void ack();                 // IIC sends ACK signal
+  static void nAck();                // IIC does not send ACK signal
+};
+
+/******************** EEPROM ********************/
+
+#define BL24C01     127
+#define BL24C02     255
+#define BL24C04     511
+#define BL24C08    1023
+#define BL24C16    2047
+#define BL24C32    4095
+#define BL24C64    8191
+#define BL24C128  16383
+#define BL24C256  32767
+#define EE_TYPE BL24C16
+
+class BL24CXX {
+private:
+  static bool _check();                                                             // Check the device
+public:
+  static void init();                                                               // Initialize IIC
+  static bool check();                                                              // Check / recheck the device
+  static uint8_t readOneByte(uint16_t ReadAddr);                                    // Read a byte at the specified address
+  static void writeOneByte(uint16_t WriteAddr, uint8_t DataToWrite);                // Write a byte at the specified address
+  static void writeLenByte(uint16_t WriteAddr, uint32_t DataToWrite, uint8_t Len);  // The specified address begins to write the data of the specified length
+  static uint32_t readLenByte(uint16_t ReadAddr, uint8_t Len);                      // The specified address starts to read the data of the specified length
+  static void write(uint16_t WriteAddr, uint8_t *pBuffer, uint16_t NumToWrite);     // Write the specified length of data from the specified address
+  static void read(uint16_t ReadAddr, uint8_t *pBuffer, uint16_t NumToRead);        // Read the data of the specified length from the specified address
+};

src/libs/L64XX/L64XX_Marlin.cpp

@@ -0,0 +1,998 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * The monitor_driver routines are a close copy of the TMC code
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_L64XX
+
+#include "L64XX_Marlin.h"
+
+L64XX_Marlin L64xxManager;
+
+#include "../../module/stepper/indirection.h"
+#include "../../gcode/gcode.h"
+#include "../../module/planner.h"
+#include "../../HAL/shared/Delay.h"
+
+static const char NUM_AXIS_LIST(
+                   str_X[] PROGMEM = "X ",      str_Y[] PROGMEM = "Y ",      str_Z[] PROGMEM = "Z ",
+                   str_I[] PROGMEM = STR_I " ", str_J[] PROGMEM = STR_J " ", str_K[] PROGMEM = STR_K " "
+                 ),
+                 str_X2[] PROGMEM = "X2", str_Y2[] PROGMEM = "Y2",
+                 str_Z2[] PROGMEM = "Z2", str_Z3[] PROGMEM = "Z3", str_Z4[] PROGMEM = "Z4",
+                 LIST_N(EXTRUDERS,
+                   str_E0[] PROGMEM = "E0", str_E1[] PROGMEM = "E1",
+                   str_E2[] PROGMEM = "E2", str_E3[] PROGMEM = "E3",
+                   str_E4[] PROGMEM = "E4", str_E5[] PROGMEM = "E5",
+                   str_E6[] PROGMEM = "E6", str_E7[] PROGMEM = "E7"
+                 )
+                 ;
+
+#define _EN_ITEM(N) , str_E##N
+PGM_P const L64XX_Marlin::index_to_axis[] PROGMEM = {
+  NUM_AXIS_LIST(str_X, str_Y, str_Z, str_I, str_J, str_K),
+  str_X2, str_Y2, str_Z2, str_Z3, str_Z4
+  REPEAT(E_STEPPERS, _EN_ITEM)
+};
+#undef _EN_ITEM
+
+#define DEBUG_OUT ENABLED(L6470_CHITCHAT)
+#include "../../core/debug_out.h"
+
+void echo_yes_no(const bool yes) { DEBUG_ECHOPGM_P(yes ? PSTR(" YES") : PSTR(" NO ")); UNUSED(yes); }
+
+uint8_t L64XX_Marlin::dir_commands[MAX_L64XX];  // array to hold direction command for each driver
+
+#define _EN_ITEM(N) , ENABLED(INVERT_E##N##_DIR)
+const uint8_t L64XX_Marlin::index_to_dir[MAX_L64XX] = {
+    NUM_AXIS_LIST(ENABLED(INVERT_X_DIR), ENABLED(INVERT_Y_DIR), ENABLED(INVERT_Z_DIR), ENABLED(INVERT_I_DIR), ENABLED(INVERT_J_DIR), ENABLED(INVERT_K_DIR))
+  , ENABLED(INVERT_X_DIR) ^ BOTH(HAS_DUAL_X_STEPPERS, INVERT_X2_VS_X_DIR) // X2
+  , ENABLED(INVERT_Y_DIR) ^ BOTH(HAS_DUAL_Y_STEPPERS, INVERT_Y2_VS_Y_DIR) // Y2
+  , ENABLED(INVERT_Z_DIR) ^ ENABLED(INVERT_Z2_VS_Z_DIR) // Z2
+  , ENABLED(INVERT_Z_DIR) ^ ENABLED(INVERT_Z3_VS_Z_DIR) // Z3
+  , ENABLED(INVERT_Z_DIR) ^ ENABLED(INVERT_Z4_VS_Z_DIR) // Z4
+    REPEAT(E_STEPPERS, _EN_ITEM)
+};
+#undef _EN_ITEM
+
+volatile uint8_t L64XX_Marlin::spi_abort = false;
+uint8_t L64XX_Marlin::spi_active = false;
+
+L64XX_Marlin::L64XX_shadow_t L64XX_Marlin::shadow;
+
+//uint32_t UVLO_ADC = 0x0400; // ADC undervoltage event
+
+void L6470_populate_chain_array() {
+
+  #define _L6470_INIT_SPI(Q)  do{ stepper##Q.set_chain_info(Q, Q##_CHAIN_POS); }while(0)
+
+  #if AXIS_IS_L64XX(X)
+    _L6470_INIT_SPI(X);
+  #endif
+  #if AXIS_IS_L64XX(X2)
+    _L6470_INIT_SPI(X2);
+  #endif
+  #if AXIS_IS_L64XX(Y)
+    _L6470_INIT_SPI(Y);
+  #endif
+  #if AXIS_IS_L64XX(Y2)
+    _L6470_INIT_SPI(Y2);
+  #endif
+  #if AXIS_IS_L64XX(Z)
+    _L6470_INIT_SPI(Z);
+  #endif
+  #if AXIS_IS_L64XX(Z2)
+    _L6470_INIT_SPI(Z2);
+  #endif
+  #if AXIS_IS_L64XX(Z3)
+    _L6470_INIT_SPI(Z3);
+  #endif
+  #if AXIS_IS_L64XX(Z4)
+    _L6470_INIT_SPI(Z4);
+  #endif
+  #if AXIS_IS_L64XX(E0)
+    _L6470_INIT_SPI(E0);
+  #endif
+  #if AXIS_IS_L64XX(E1)
+    _L6470_INIT_SPI(E1);
+  #endif
+  #if AXIS_IS_L64XX(E2)
+    _L6470_INIT_SPI(E2);
+  #endif
+  #if AXIS_IS_L64XX(E3)
+    _L6470_INIT_SPI(E3);
+  #endif
+  #if AXIS_IS_L64XX(E4)
+    _L6470_INIT_SPI(E4);
+  #endif
+  #if AXIS_IS_L64XX(E5)
+    _L6470_INIT_SPI(E5);
+  #endif
+  #if AXIS_IS_L64XX(E6)
+    _L6470_INIT_SPI(E6);
+  #endif
+  #if AXIS_IS_L64XX(E7)
+    _L6470_INIT_SPI(E7);
+  #endif
+}
+
+
+/**
+ * Some status bit positions & definitions differ per driver.
+ * Copy info to known locations to simplfy check/display logic.
+ *   1. Copy stepper status
+ *   2. Copy status bit definitions
+ *   3. Copy status layout
+ *   4. Make all error bits active low (as needed)
+ */
+uint16_t L64XX_Marlin::get_stepper_status(L64XX &st) {
+  shadow.STATUS_AXIS_RAW           = st.getStatus();
+  shadow.STATUS_AXIS               = shadow.STATUS_AXIS_RAW;
+  shadow.STATUS_AXIS_LAYOUT        = st.L6470_status_layout;
+  shadow.AXIS_OCD_TH_MAX           = st.OCD_TH_MAX;
+  shadow.AXIS_STALL_TH_MAX         = st.STALL_TH_MAX;
+  shadow.AXIS_OCD_CURRENT_CONSTANT_INV   = st.OCD_CURRENT_CONSTANT_INV;
+  shadow.AXIS_STALL_CURRENT_CONSTANT_INV = st.STALL_CURRENT_CONSTANT_INV;
+  shadow.L6470_AXIS_CONFIG         = st.L64XX_CONFIG;
+  shadow.L6470_AXIS_STATUS         = st.L64XX_STATUS;
+  shadow.STATUS_AXIS_OCD           = st.STATUS_OCD;
+  shadow.STATUS_AXIS_SCK_MOD       = st.STATUS_SCK_MOD;
+  shadow.STATUS_AXIS_STEP_LOSS_A   = st.STATUS_STEP_LOSS_A;
+  shadow.STATUS_AXIS_STEP_LOSS_B   = st.STATUS_STEP_LOSS_B;
+  shadow.STATUS_AXIS_TH_SD         = st.STATUS_TH_SD;
+  shadow.STATUS_AXIS_TH_WRN        = st.STATUS_TH_WRN;
+  shadow.STATUS_AXIS_UVLO          = st.STATUS_UVLO;
+  shadow.STATUS_AXIS_WRONG_CMD     = st.STATUS_WRONG_CMD;
+  shadow.STATUS_AXIS_CMD_ERR       = st.STATUS_CMD_ERR;
+  shadow.STATUS_AXIS_NOTPERF_CMD   = st.STATUS_NOTPERF_CMD;
+
+  switch (shadow.STATUS_AXIS_LAYOUT) {
+    case L6470_STATUS_LAYOUT: {   // L6470
+      shadow.L6470_ERROR_MASK      = shadow.STATUS_AXIS_UVLO | shadow.STATUS_AXIS_TH_WRN | shadow.STATUS_AXIS_TH_SD | shadow.STATUS_AXIS_OCD | shadow.STATUS_AXIS_STEP_LOSS_A | shadow.STATUS_AXIS_STEP_LOSS_B;
+      shadow.STATUS_AXIS           ^= (shadow.STATUS_AXIS_WRONG_CMD | shadow.STATUS_AXIS_NOTPERF_CMD);  // invert just error bits that are active high
+      break;
+    }
+    case L6474_STATUS_LAYOUT: {   // L6474
+      shadow.L6470_ERROR_MASK      = shadow.STATUS_AXIS_UVLO | shadow.STATUS_AXIS_TH_WRN | shadow.STATUS_AXIS_TH_SD | shadow.STATUS_AXIS_OCD ;
+      shadow.STATUS_AXIS           ^= (shadow.STATUS_AXIS_WRONG_CMD | shadow.STATUS_AXIS_NOTPERF_CMD);  // invert just error bits that are active high
+      break;
+    }
+    case L6480_STATUS_LAYOUT: {   // L6480 & powerSTEP01
+      shadow.L6470_ERROR_MASK      = shadow.STATUS_AXIS_UVLO | shadow.STATUS_AXIS_TH_WRN | shadow.STATUS_AXIS_TH_SD | shadow.STATUS_AXIS_OCD | shadow.STATUS_AXIS_STEP_LOSS_A | shadow.STATUS_AXIS_STEP_LOSS_B;
+      shadow.STATUS_AXIS           ^= (shadow.STATUS_AXIS_CMD_ERR | shadow.STATUS_AXIS_TH_WRN | shadow.STATUS_AXIS_TH_SD);  // invert just error bits that are active high
+      break;
+    }
+  }
+  return shadow.STATUS_AXIS;
+}
+
+
+void L64XX_Marlin::init() {               // Set up SPI and then init chips
+  ENABLE_RESET_L64XX_CHIPS(LOW);          // hardware reset of drivers
+  DELAY_US(100);
+  ENABLE_RESET_L64XX_CHIPS(HIGH);
+  DELAY_US(1000);                         // need about 650µs for the chip(s) to fully start up
+  L6470_populate_chain_array();           // Set up array to control where in the SPI transfer sequence a particular stepper's data goes
+
+  spi_init();                             // Since L64XX SPI pins are unset we must init SPI here
+
+  init_to_defaults();                     // init the chips
+}
+
+uint16_t L64XX_Marlin::get_status(const L64XX_axis_t axis) {
+
+  #define STATUS_L6470(Q) get_stepper_status(stepper##Q)
+
+  switch (axis) {
+    default: break;
+    #if AXIS_IS_L64XX(X)
+      case X : return STATUS_L6470(X);
+    #endif
+    #if AXIS_IS_L64XX(Y)
+      case Y : return STATUS_L6470(Y);
+    #endif
+    #if AXIS_IS_L64XX(Z)
+      case Z : return STATUS_L6470(Z);
+    #endif
+    #if AXIS_IS_L64XX(X2)
+      case X2: return STATUS_L6470(X2);
+    #endif
+    #if AXIS_IS_L64XX(Y2)
+      case Y2: return STATUS_L6470(Y2);
+    #endif
+    #if AXIS_IS_L64XX(Z2)
+      case Z2: return STATUS_L6470(Z2);
+    #endif
+    #if AXIS_IS_L64XX(Z3)
+      case Z3: return STATUS_L6470(Z3);
+    #endif
+    #if AXIS_IS_L64XX(Z4)
+      case Z4: return STATUS_L6470(Z4);
+    #endif
+    #if AXIS_IS_L64XX(E0)
+      case E0: return STATUS_L6470(E0);
+    #endif
+    #if AXIS_IS_L64XX(E1)
+      case E1: return STATUS_L6470(E1);
+    #endif
+    #if AXIS_IS_L64XX(E2)
+      case E2: return STATUS_L6470(E2);
+    #endif
+    #if AXIS_IS_L64XX(E3)
+      case E3: return STATUS_L6470(E3);
+    #endif
+    #if AXIS_IS_L64XX(E4)
+      case E4: return STATUS_L6470(E4);
+    #endif
+    #if AXIS_IS_L64XX(E5)
+      case E5: return STATUS_L6470(E5);
+    #endif
+    #if AXIS_IS_L64XX(E6)
+      case E6: return STATUS_L6470(E6);
+    #endif
+    #if AXIS_IS_L64XX(E7)
+      case E7: return STATUS_L6470(E7);
+    #endif
+  }
+
+  return 0; // Not needed but kills a compiler warning
+}
+
+uint32_t L64XX_Marlin::get_param(const L64XX_axis_t axis, const uint8_t param) {
+
+  #define GET_L6470_PARAM(Q) L6470_GETPARAM(param, Q)
+
+  switch (axis) {
+    default: break;
+    #if AXIS_IS_L64XX(X)
+      case X : return GET_L6470_PARAM(X);
+    #endif
+    #if AXIS_IS_L64XX(Y)
+      case Y : return GET_L6470_PARAM(Y);
+    #endif
+    #if AXIS_IS_L64XX(Z)
+      case Z : return GET_L6470_PARAM(Z);
+    #endif
+    #if AXIS_IS_L64XX(X2)
+      case X2: return GET_L6470_PARAM(X2);
+    #endif
+    #if AXIS_IS_L64XX(Y2)
+      case Y2: return GET_L6470_PARAM(Y2);
+    #endif
+    #if AXIS_IS_L64XX(Z2)
+      case Z2: return GET_L6470_PARAM(Z2);
+    #endif
+    #if AXIS_IS_L64XX(Z3)
+      case Z3: return GET_L6470_PARAM(Z3);
+    #endif
+    #if AXIS_IS_L64XX(Z4)
+      case Z4: return GET_L6470_PARAM(Z4);
+    #endif
+    #if AXIS_IS_L64XX(E0)
+      case E0: return GET_L6470_PARAM(E0);
+    #endif
+    #if AXIS_IS_L64XX(E1)
+      case E1: return GET_L6470_PARAM(E1);
+    #endif
+    #if AXIS_IS_L64XX(E2)
+      case E2: return GET_L6470_PARAM(E2);
+    #endif
+    #if AXIS_IS_L64XX(E3)
+      case E3: return GET_L6470_PARAM(E3);
+    #endif
+    #if AXIS_IS_L64XX(E4)
+      case E4: return GET_L6470_PARAM(E4);
+    #endif
+    #if AXIS_IS_L64XX(E5)
+      case E5: return GET_L6470_PARAM(E5);
+    #endif
+    #if AXIS_IS_L64XX(E6)
+      case E6: return GET_L6470_PARAM(E6);
+    #endif
+    #if AXIS_IS_L64XX(E7)
+      case E7: return GET_L6470_PARAM(E7);
+    #endif
+  }
+
+  return 0; // not needed but kills a compiler warning
+}
+
+void L64XX_Marlin::set_param(const L64XX_axis_t axis, const uint8_t param, const uint32_t value) {
+
+  #define SET_L6470_PARAM(Q) stepper##Q.SetParam(param, value)
+
+  switch (axis) {
+    default: break;
+    #if AXIS_IS_L64XX(X)
+      case X : SET_L6470_PARAM(X); break;
+    #endif
+    #if AXIS_IS_L64XX(Y)
+      case Y : SET_L6470_PARAM(Y); break;
+    #endif
+    #if AXIS_IS_L64XX(Z)
+      case Z : SET_L6470_PARAM(Z); break;
+    #endif
+    #if AXIS_IS_L64XX(I)
+      case I : SET_L6470_PARAM(I); break;
+    #endif
+    #if AXIS_IS_L64XX(J)
+      case J : SET_L6470_PARAM(J); break;
+    #endif
+    #if AXIS_IS_L64XX(K)
+      case K : SET_L6470_PARAM(K); break;
+    #endif
+    #if AXIS_IS_L64XX(X2)
+      case X2: SET_L6470_PARAM(X2); break;
+    #endif
+    #if AXIS_IS_L64XX(Y2)
+      case Y2: SET_L6470_PARAM(Y2); break;
+    #endif
+    #if AXIS_IS_L64XX(Z2)
+      case Z2: SET_L6470_PARAM(Z2); break;
+    #endif
+    #if AXIS_IS_L64XX(Z3)
+      case Z3: SET_L6470_PARAM(Z3); break;
+    #endif
+    #if AXIS_IS_L64XX(Z4)
+      case Z4: SET_L6470_PARAM(Z4); break;
+    #endif
+    #if AXIS_IS_L64XX(E0)
+      case E0: SET_L6470_PARAM(E0); break;
+    #endif
+    #if AXIS_IS_L64XX(E1)
+      case E1: SET_L6470_PARAM(E1); break;
+    #endif
+    #if AXIS_IS_L64XX(E2)
+      case E2: SET_L6470_PARAM(E2); break;
+    #endif
+    #if AXIS_IS_L64XX(E3)
+      case E3: SET_L6470_PARAM(E3); break;
+    #endif
+    #if AXIS_IS_L64XX(E4)
+      case E4: SET_L6470_PARAM(E4); break;
+    #endif
+    #if AXIS_IS_L64XX(E5)
+      case E5: SET_L6470_PARAM(E5); break;
+    #endif
+    #if AXIS_IS_L64XX(E6)
+      case E6: SET_L6470_PARAM(E6); break;
+    #endif
+    #if AXIS_IS_L64XX(E7)
+      case E7: SET_L6470_PARAM(E7); break;
+    #endif
+  }
+}
+
+inline void echo_min_max(const char a, const_float_t min, const_float_t max) {
+  DEBUG_CHAR(' '); DEBUG_CHAR(a);
+  DEBUG_ECHOLNPGM(" min = ", min, "  max = ", max);
+}
+inline void echo_oct_used(const_float_t oct, const uint8_t stall) {
+  DEBUG_ECHOPGM("over_current_threshold used     : ", oct);
+  DEBUG_ECHOPGM_P(stall ? PSTR("  (Stall") : PSTR("  (OCD"));
+  DEBUG_ECHOLNPGM(" threshold)");
+}
+inline void err_out_of_bounds() { DEBUG_ECHOLNPGM("Test aborted - motion out of bounds"); }
+
+uint8_t L64XX_Marlin::get_user_input(uint8_t &driver_count, L64XX_axis_t axis_index[3],  char axis_mon[3][3],
+                                 float &position_max, float &position_min, float &final_feedrate, uint8_t &kval_hold,
+                                 uint8_t over_current_flag, uint8_t &OCD_TH_val, uint8_t &STALL_TH_val, uint16_t &over_current_threshold
+) {
+  // Return TRUE if the calling routine needs to abort/kill
+
+  uint16_t displacement = 0;  // " = 0" to eliminate compiler warning
+  uint8_t j;   // general purpose counter
+
+  if (!all_axes_homed()) {
+    DEBUG_ECHOLNPGM("Test aborted - home all before running this command");
+    return true;
+  }
+
+  uint8_t found_displacement = false;
+  LOOP_LOGICAL_AXES(i) if (uint16_t _displacement = parser.intval(AXIS_CHAR(i))) {
+    found_displacement = true;
+    displacement = _displacement;
+    const uint8_t axis_offset = parser.byteval('J');
+    axis_mon[0][0] = AXIS_CHAR(i);          // Axis first character, one of XYZ...E
+    const bool single_or_e = axis_offset >= 2 || axis_mon[0][0] == 'E',
+               one_or_more = !single_or_e && axis_offset == 0;
+    uint8_t driver_count_local = 0;         // Can't use "driver_count" directly as a subscript because it's passed by reference
+    if (single_or_e)                        // Single axis, E0, or E1
+      axis_mon[0][1] = axis_offset + '0';   // Index given by 'J' parameter
+
+    if (single_or_e || one_or_more) {
+      for (j = 0; j < MAX_L64XX; j++) {     // Count up the drivers on this axis
+        PGM_P str = (PGM_P)pgm_read_ptr(&index_to_axis[j]); // Get a PGM_P from progmem
+        const char c = pgm_read_byte(str);                  // Get a char from progmem
+        if (axis_mon[0][0] == c) {          // For each stepper on this axis...
+          char *mon = axis_mon[driver_count_local];
+          *mon++ = c;                        // Copy the 3 letter axis name
+          *mon++ = pgm_read_byte(&str[1]);   //  to the axis_mon array
+          *mon   = pgm_read_byte(&str[2]);
+          axis_index[driver_count_local] = (L64XX_axis_t)j; // And store the L64XX axis index
+          driver_count_local++;
+        }
+      }
+      if (one_or_more) driver_count = driver_count_local;
+    }
+    break; // only take first axis found
+  }
+
+  if (!found_displacement) {
+    DEBUG_ECHOLNPGM("Test aborted - AXIS with displacement is required");
+    return true;
+  }
+
+  //
+  // Position calcs & checks
+  //
+
+  const float LOGICAL_AXIS_LIST(
+                E_center = current_position.e,
+                X_center = LOGICAL_X_POSITION(current_position.x),
+                Y_center = LOGICAL_Y_POSITION(current_position.y),
+                Z_center = LOGICAL_Z_POSITION(current_position.z),
+                I_center = LOGICAL_I_POSITION(current_position.i),
+                J_center = LOGICAL_J_POSITION(current_position.j),
+                K_center = LOGICAL_K_POSITION(current_position.k)
+              );
+
+  switch (axis_mon[0][0]) {
+    default: position_max = position_min = 0; break;
+
+    case 'X': {
+      position_min = X_center - displacement;
+      position_max = X_center + displacement;
+      echo_min_max('X', position_min, position_max);
+      if (TERN0(HAS_ENDSTOPS, position_min < (X_MIN_POS) || position_max > (X_MAX_POS))) {
+        err_out_of_bounds();
+        return true;
+      }
+    } break;
+
+    #if HAS_Y_AXIS
+      case 'Y': {
+        position_min = Y_center - displacement;
+        position_max = Y_center + displacement;
+        echo_min_max('Y', position_min, position_max);
+        if (TERN0(HAS_ENDSTOPS, position_min < (Y_MIN_POS) || position_max > (Y_MAX_POS))) {
+          err_out_of_bounds();
+          return true;
+        }
+      } break;
+    #endif
+
+    #if HAS_Z_AXIS
+      case 'Z': {
+        position_min = Z_center - displacement;
+        position_max = Z_center + displacement;
+        echo_min_max('Z', position_min, position_max);
+        if (TERN0(HAS_ENDSTOPS, position_min < (Z_MIN_POS) || position_max > (Z_MAX_POS))) {
+          err_out_of_bounds();
+          return true;
+        }
+      } break;
+    #endif
+
+    #if HAS_I_AXIS
+      case AXIS4_NAME: {
+        position_min = I_center - displacement;
+        position_max = I_center + displacement;
+        echo_min_max(AXIS4_NAME, position_min, position_max);
+        if (TERN0(HAS_ENDSTOPS, position_min < (I_MIN_POS) || position_max > (I_MAX_POS))) {
+          err_out_of_bounds();
+          return true;
+        }
+      } break;
+    #endif
+
+    #if HAS_J_AXIS
+      case AXIS5_NAME: {
+        position_min = J_center - displacement;
+        position_max = J_center + displacement;
+        echo_min_max(AXIS5_NAME, position_min, position_max);
+        if (TERN1(HAS_ENDSTOPS, position_min < (J_MIN_POS) || position_max > (J_MAX_POS))) {
+          err_out_of_bounds();
+          return true;
+        }
+      } break;
+    #endif
+
+    #if HAS_K_AXIS
+      case AXIS6_NAME: {
+        position_min = K_center - displacement;
+        position_max = K_center + displacement;
+        echo_min_max(AXIS6_NAME, position_min, position_max);
+        if (TERN2(HAS_ENDSTOPS, position_min < (K_MIN_POS) || position_max > (K_MAX_POS))) {
+          err_out_of_bounds();
+          return true;
+        }
+      } break;
+    #endif
+
+    #if HAS_EXTRUDERS
+      case 'E': {
+        position_min = E_center - displacement;
+        position_max = E_center + displacement;
+        echo_min_max('E', position_min, position_max);
+      } break;
+    #endif
+  }
+
+  //
+  // Work on the drivers
+  //
+
+  LOOP_L_N(k, driver_count) {
+    uint8_t not_found = true;
+    for (j = 1; j <= L64XX::chain[0]; j++) {
+      PGM_P const str = (PGM_P)pgm_read_ptr(&index_to_axis[L64XX::chain[j]]);
+      if (pgm_read_byte(&str[0]) == axis_mon[k][0] && pgm_read_byte(&str[1]) == axis_mon[k][1]) { // See if a L6470 driver
+        not_found = false;
+        break;
+      }
+    }
+    if (not_found) {
+      driver_count = k;
+      axis_mon[k][0] = ' ';  // mark this entry invalid
+      break;
+    }
+  }
+
+  if (driver_count == 0) {
+    DEBUG_ECHOLNPGM("Test aborted - not a L6470 axis");
+    return true;
+  }
+
+  DEBUG_ECHOPGM("Monitoring:");
+  for (j = 0; j < driver_count; j++) DEBUG_ECHOPGM("  ", axis_mon[j]);
+  DEBUG_EOL();
+
+  // now have a list of driver(s) to monitor
+
+  //
+  // TVAL & kVAL_HOLD checks & settings
+  //
+  const L64XX_shadow_t &sh = shadow;
+  get_status(axis_index[0]); // populate shadow array
+
+  if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) {  // L6474 - use TVAL
+    uint16_t TVAL_current = parser.ushortval('T');
+    if (TVAL_current) {
+      uint8_t TVAL_count = (TVAL_current /  sh.AXIS_STALL_CURRENT_CONSTANT_INV) - 1;
+      LIMIT(TVAL_count, 0, sh.AXIS_STALL_TH_MAX);
+      for (j = 0; j < driver_count; j++)
+        set_param(axis_index[j], L6474_TVAL, TVAL_count);
+    }
+    // only print the tval from one of the drivers
+    kval_hold = get_param(axis_index[0], L6474_TVAL);
+    DEBUG_ECHOLNPGM("TVAL current (mA) = ", (kval_hold + 1) * sh.AXIS_STALL_CURRENT_CONSTANT_INV);
+  }
+  else {
+    kval_hold = parser.byteval('K');
+    if (kval_hold) {
+      DEBUG_ECHOLNPGM("kval_hold = ", kval_hold);
+      for (j = 0; j < driver_count; j++)
+        set_param(axis_index[j], L6470_KVAL_HOLD, kval_hold);
+    }
+    else {
+      // only print the KVAL_HOLD from one of the drivers
+      kval_hold = get_param(axis_index[0], L6470_KVAL_HOLD);
+      DEBUG_ECHOLNPGM("KVAL_HOLD = ", kval_hold);
+    }
+  }
+
+  //
+  // Overcurrent checks & settings
+  //
+
+  if (over_current_flag) {
+
+    uint8_t OCD_TH_val_local = 0,       // compiler thinks OCD_TH_val is unused if use it directly
+            STALL_TH_val_local = 0;     // just in case ...
+
+    over_current_threshold = parser.intval('I');
+
+    if (over_current_threshold) {
+
+      OCD_TH_val_local = over_current_threshold/375;
+      LIMIT(OCD_TH_val_local, 0, 15);
+      STALL_TH_val_local = over_current_threshold/31.25;
+      LIMIT(STALL_TH_val_local, 0, 127);
+      uint16_t OCD_TH_actual = (OCD_TH_val_local + 1) * 375,
+               STALL_TH_actual = (STALL_TH_val_local + 1) * 31.25;
+      if (OCD_TH_actual < STALL_TH_actual) {
+        OCD_TH_val_local++;
+        OCD_TH_actual = (OCD_TH_val_local + 1) * 375;
+      }
+
+      DEBUG_ECHOLNPGM("over_current_threshold specified: ", over_current_threshold);
+      if (!(sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT)) echo_oct_used((STALL_TH_val_local + 1) * 31.25, true);
+      echo_oct_used((OCD_TH_val_local + 1) * 375, false);
+
+      #define SET_OVER_CURRENT(Q) do { stepper##Q.SetParam(L6470_STALL_TH, STALL_TH_val_local); stepper##Q.SetParam(L6470_OCD_TH, OCD_TH_val_local);} while (0)
+
+      for (j = 0; j < driver_count; j++) {
+        set_param(axis_index[j], L6470_STALL_TH, STALL_TH_val_local);
+        set_param(axis_index[j], L6470_OCD_TH, OCD_TH_val_local);
+      }
+    }
+    else {
+      // only get & print the OVER_CURRENT values from one of the drivers
+      STALL_TH_val_local = get_param(axis_index[0], L6470_STALL_TH);
+      OCD_TH_val_local = get_param(axis_index[0], L6470_OCD_TH);
+
+      if (!(sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT)) echo_oct_used((STALL_TH_val_local + 1) * 31.25, true);
+      echo_oct_used((OCD_TH_val_local + 1) * 375, false);
+    } // over_current_threshold
+
+    for (j = 0; j < driver_count; j++) {                 // set all drivers on axis the same
+      set_param(axis_index[j], L6470_STALL_TH, STALL_TH_val_local);
+      set_param(axis_index[j], L6470_OCD_TH, OCD_TH_val_local);
+    }
+
+    OCD_TH_val = OCD_TH_val_local;        // force compiler to update the main routine's copy
+    STALL_TH_val = STALL_TH_val_local;    // force compiler to update the main routine's copy
+  } // end of overcurrent
+
+  //
+  // Feedrate
+  //
+
+  final_feedrate = parser.floatval('F');
+  if (final_feedrate == 0) {
+    static constexpr float default_max_feedrate[] = DEFAULT_MAX_FEEDRATE;
+    const uint8_t num_feedrates = COUNT(default_max_feedrate);
+    for (j = 0; j < num_feedrates; j++) {
+      if (AXIS_CHAR(j) == axis_mon[0][0]) {
+        final_feedrate = default_max_feedrate[j];
+        break;
+      }
+    }
+    if (j == 3 && num_feedrates > 4) {   // have more than one extruder feedrate
+      uint8_t extruder_num = axis_mon[0][1] - '0';
+      if (j <= num_feedrates - extruder_num)     // have a feedrate specifically for this extruder
+        final_feedrate = default_max_feedrate[j + extruder_num];
+      else
+        final_feedrate = default_max_feedrate[3];  // use E0 feedrate for this extruder
+    }
+    final_feedrate *= 60;  // convert to mm/minute
+  } // end of feedrate
+
+  return false;   // FALSE indicates no user input problems
+}
+
+void L64XX_Marlin::say_axis(const L64XX_axis_t axis, const uint8_t label/*=true*/) {
+  if (label) SERIAL_ECHOPGM("AXIS:");
+  const char * const str = L64xxManager.index_to_axis[axis];
+  SERIAL_CHAR(' ', str[0], str[1], ' ');
+}
+
+#if ENABLED(L6470_CHITCHAT)
+
+  // Assumes status bits have been inverted
+  void L64XX_Marlin::error_status_decode(const uint16_t status, const L64XX_axis_t axis,
+        const uint16_t _status_axis_th_sd, const uint16_t _status_axis_th_wrn,
+        const uint16_t _status_axis_step_loss_a, const uint16_t _status_axis_step_loss_b,
+        const uint16_t _status_axis_ocd, const uint8_t _status_axis_layout
+  ) {
+    say_axis(axis);
+    DEBUG_ECHOPGM("  THERMAL: ");
+    DEBUG_ECHOPGM_P((status & _status_axis_th_sd) ? PSTR("SHUTDOWN") : (status & _status_axis_th_wrn) ? PSTR("WARNING ") : PSTR("OK      "));
+    DEBUG_ECHOPGM("   OVERCURRENT: ");
+    echo_yes_no((status & _status_axis_ocd) != 0);
+    if (!(_status_axis_layout == L6474_STATUS_LAYOUT)) {  // L6474 doesn't have these bits
+      DEBUG_ECHOPGM("   STALL: ");
+      echo_yes_no((status & (_status_axis_step_loss_a | _status_axis_step_loss_b)) != 0);
+    }
+    DEBUG_EOL();
+  }
+
+#endif
+
+//////////////////////////////////////////////////////////////////////////////////////////////////
+////
+////  MONITOR_L6470_DRIVER_STATUS routines
+////
+//////////////////////////////////////////////////////////////////////////////////////////////////
+
+#if ENABLED(MONITOR_L6470_DRIVER_STATUS)
+
+  bool L64XX_Marlin::monitor_paused = false; // Flag to skip monitor during M122, M906, M916, M917, M918, etc.
+
+  struct L6470_driver_data {
+    L64XX_axis_t driver_index;
+    uint32_t driver_status;
+    uint8_t is_otw;
+    uint8_t otw_counter;
+    uint8_t is_ot;
+    uint8_t is_hi_Z;
+    uint8_t com_counter;
+  };
+
+  L6470_driver_data driver_L6470_data[] = {
+    #if AXIS_IS_L64XX(X)
+      {  X, 0, 0, 0, 0, 0, 0 },
+    #endif
+    #if AXIS_IS_L64XX(Y)
+      {  Y, 0, 0, 0, 0, 0, 0 },
+    #endif
+    #if AXIS_IS_L64XX(Z)
+      {  Z, 0, 0, 0, 0, 0, 0 },
+    #endif
+    #if AXIS_IS_L64XX(I)
+      {  I, 0, 0, 0, 0, 0, 0 },
+    #endif
+    #if AXIS_IS_L64XX(J)
+      {  J, 0, 0, 0, 0, 0, 0 },
+    #endif
+    #if AXIS_IS_L64XX(K)
+      {  K, 0, 0, 0, 0, 0, 0 },
+    #endif
+    #if AXIS_IS_L64XX(X2)
+      { X2, 0, 0, 0, 0, 0, 0 },
+    #endif
+    #if AXIS_IS_L64XX(Y2)
+      { Y2, 0, 0, 0, 0, 0, 0 },
+    #endif
+    #if AXIS_IS_L64XX(Z2)
+      { Z2, 0, 0, 0, 0, 0, 0 },
+    #endif
+    #if AXIS_IS_L64XX(Z3)
+      { Z3, 0, 0, 0, 0, 0, 0 },
+    #endif
+    #if AXIS_IS_L64XX(Z4)
+      { Z4, 0, 0, 0, 0, 0, 0 },
+    #endif
+    #if AXIS_IS_L64XX(E0)
+      { E0, 0, 0, 0, 0, 0, 0 },
+    #endif
+    #if AXIS_IS_L64XX(E1)
+      { E1, 0, 0, 0, 0, 0, 0 },
+    #endif
+    #if AXIS_IS_L64XX(E2)
+      { E2, 0, 0, 0, 0, 0, 0 },
+    #endif
+    #if AXIS_IS_L64XX(E3)
+      { E3, 0, 0, 0, 0, 0, 0 },
+    #endif
+    #if AXIS_IS_L64XX(E4)
+      { E4, 0, 0, 0, 0, 0, 0 },
+    #endif
+    #if AXIS_IS_L64XX(E5)
+      { E5, 0, 0, 0, 0, 0, 0 }
+    #endif
+    #if AXIS_IS_L64XX(E6)
+      { E6, 0, 0, 0, 0, 0, 0 }
+    #endif
+    #if AXIS_IS_L64XX(E7)
+      { E7, 0, 0, 0, 0, 0, 0 }
+    #endif
+  };
+
+  void L64XX_Marlin::append_stepper_err(char* &p, const uint8_t stepper_index, const char * const err/*=nullptr*/) {
+    PGM_P const str = (PGM_P)pgm_read_ptr(&index_to_axis[stepper_index]);
+    p += sprintf_P(p, PSTR("Stepper %c%c "), pgm_read_byte(&str[0]), pgm_read_byte(&str[1]));
+    if (err) p += sprintf_P(p, err);
+  }
+
+  void L64XX_Marlin::monitor_update(L64XX_axis_t stepper_index) {
+    if (spi_abort) return;  // don't do anything if set_directions() has occurred
+    const L64XX_shadow_t &sh = shadow;
+    get_status(stepper_index); // get stepper status and details
+    uint16_t status = sh.STATUS_AXIS;
+    uint8_t kval_hold, tval;
+    char temp_buf[120], *p = temp_buf;
+    uint8_t j;
+    for (j = 0; j < L64XX::chain[0]; j++) // find the table for this stepper
+      if (driver_L6470_data[j].driver_index == stepper_index) break;
+
+    driver_L6470_data[j].driver_status = status;
+    uint16_t _status = ~status;     // all error bits are active low
+
+    if (status == 0 || status == 0xFFFF) {              // com problem
+      if (driver_L6470_data[j].com_counter == 0) {      // warn user when it first happens
+        driver_L6470_data[j].com_counter++;
+        append_stepper_err(p, stepper_index, PSTR(" - communications lost\n"));
+        DEBUG_ECHO(temp_buf);
+      }
+      else {
+        driver_L6470_data[j].com_counter++;
+        if (driver_L6470_data[j].com_counter > 240) {  // remind of com problem about every 2 minutes
+          driver_L6470_data[j].com_counter = 1;
+          append_stepper_err(p, stepper_index, PSTR(" - still no communications\n"));
+          DEBUG_ECHO(temp_buf);
+        }
+      }
+    }
+    else {
+      if (driver_L6470_data[j].com_counter) {   // comms re-established
+        driver_L6470_data[j].com_counter = 0;
+        append_stepper_err(p, stepper_index, PSTR(" - communications re-established\n.. setting all drivers to default values\n"));
+        DEBUG_ECHO(temp_buf);
+        init_to_defaults();
+      }
+      else {
+        // no com problems - do the usual checks
+        if (_status & sh.L6470_ERROR_MASK) {
+          append_stepper_err(p, stepper_index);
+
+          if (status & STATUS_HIZ) {                         // The driver has shut down. HiZ is active high
+            driver_L6470_data[j].is_hi_Z = true;
+            p += sprintf_P(p, PSTR("%cIS SHUT DOWN"), ' ');
+            //if (_status & sh.STATUS_AXIS_TH_SD) { // strange - TH_SD never seems to go active, must be implied by the HiZ and TH_WRN
+            if (_status & sh.STATUS_AXIS_TH_WRN) {                    // over current shutdown
+              p += sprintf_P(p, PSTR("%cdue to over temperature"), ' ');
+              driver_L6470_data[j].is_ot = true;
+              if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) {  // L6474
+                tval = get_param(stepper_index, L6474_TVAL) - 2 * KVAL_HOLD_STEP_DOWN;
+              set_param(stepper_index, L6474_TVAL, tval);     // reduce TVAL
+                p += sprintf_P(p, PSTR(" - TVAL reduced by %d to %d mA"), uint16_t (2 * KVAL_HOLD_STEP_DOWN * sh.AXIS_STALL_CURRENT_CONSTANT_INV), uint16_t ((tval + 1) * sh.AXIS_STALL_CURRENT_CONSTANT_INV));   // let user know
+              }
+              else {
+                kval_hold = get_param(stepper_index, L6470_KVAL_HOLD) - 2 * KVAL_HOLD_STEP_DOWN;
+                set_param(stepper_index, L6470_KVAL_HOLD, kval_hold);     // reduce KVAL_HOLD
+                p += sprintf_P(p, PSTR(" - KVAL_HOLD reduced by %d to %d"), 2 * KVAL_HOLD_STEP_DOWN, kval_hold);   // let user know
+              }
+            }
+            else
+              driver_L6470_data[j].is_ot = false;
+          }
+          else {
+            driver_L6470_data[j].is_hi_Z = false;
+
+            if (_status & sh.STATUS_AXIS_TH_WRN) {     // have an over temperature warning
+              driver_L6470_data[j].is_otw = true;
+              driver_L6470_data[j].otw_counter++;
+              kval_hold = get_param(stepper_index, L6470_KVAL_HOLD);
+              if (driver_L6470_data[j].otw_counter > 4) {  // otw present for 2 - 2.5 seconds, reduce KVAL_HOLD
+                driver_L6470_data[j].otw_counter = 0;
+                driver_L6470_data[j].is_otw = true;
+                if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) {  // L6474
+                  tval = get_param(stepper_index, L6474_TVAL) - KVAL_HOLD_STEP_DOWN;
+                  set_param(stepper_index, L6474_TVAL, tval);     // reduce TVAL
+                  p += sprintf_P(p, PSTR(" - TVAL reduced by %d to %d mA"), uint16_t (KVAL_HOLD_STEP_DOWN * sh.AXIS_STALL_CURRENT_CONSTANT_INV), uint16_t ((tval + 1) * sh.AXIS_STALL_CURRENT_CONSTANT_INV));   // let user know
+                }
+                else {
+                  kval_hold = get_param(stepper_index, L6470_KVAL_HOLD) - KVAL_HOLD_STEP_DOWN;
+                  set_param(stepper_index, L6470_KVAL_HOLD, kval_hold);     // reduce KVAL_HOLD
+                  p += sprintf_P(p, PSTR(" - KVAL_HOLD reduced by %d to %d"), KVAL_HOLD_STEP_DOWN, kval_hold);   // let user know
+                }
+              }
+              else if (driver_L6470_data[j].otw_counter)
+                p += sprintf_P(p, PSTR("%c- thermal warning"), ' ');   // warn user
+            }
+          }
+
+          #if ENABLED(L6470_STOP_ON_ERROR)
+            if (_status & (sh.STATUS_AXIS_UVLO | sh.STATUS_AXIS_TH_WRN | sh.STATUS_AXIS_TH_SD))
+              kill(temp_buf);
+          #endif
+
+          #if ENABLED(L6470_CHITCHAT)
+            if (_status & sh.STATUS_AXIS_OCD)
+              p += sprintf_P(p, PSTR("%c  over current"), ' ');
+
+            if (_status & (sh.STATUS_AXIS_STEP_LOSS_A | sh.STATUS_AXIS_STEP_LOSS_B))
+              p += sprintf_P(p, PSTR("%c  stall"), ' ');
+
+            if (_status & sh.STATUS_AXIS_UVLO)
+              p += sprintf_P(p, PSTR("%c  under voltage lock out"), ' ');
+
+            p += sprintf_P(p, PSTR("%c\n"), ' ');
+          #endif
+
+          DEBUG_ECHOLN(temp_buf);  // print the error message
+        }
+        else {
+          driver_L6470_data[j].is_ot = false;
+          driver_L6470_data[j].otw_counter = 0;   //clear out warning indicators
+          driver_L6470_data[j].is_otw = false;
+        } // end usual checks
+
+      } // comms established but have errors
+    } // comms re-established
+  } // end monitor_update()
+
+
+  void L64XX_Marlin::monitor_driver() {
+    static millis_t next_cOT = 0;
+    if (ELAPSED(millis(), next_cOT)) {
+      next_cOT = millis() + 500;
+
+      if (!monitor_paused) {  // Skip during M122, M906, M916, M917 or M918 (could steal status result from test)
+
+        spi_active = true;    // Tell set_directions() a series of SPI transfers is underway
+
+        #if AXIS_IS_L64XX(X)
+          monitor_update(X);
+        #endif
+        #if AXIS_IS_L64XX(Y)
+          monitor_update(Y);
+        #endif
+        #if AXIS_IS_L64XX(Z)
+          monitor_update(Z);
+        #endif
+        #if AXIS_IS_L64XX(I)
+          monitor_update(I);
+        #endif
+        #if AXIS_IS_L64XX(J)
+          monitor_update(J);
+        #endif
+        #if AXIS_IS_L64XX(K)
+          monitor_update(K);
+        #endif
+        #if AXIS_IS_L64XX(X2)
+          monitor_update(X2);
+        #endif
+        #if AXIS_IS_L64XX(Y2)
+          monitor_update(Y2);
+        #endif
+        #if AXIS_IS_L64XX(Z2)
+          monitor_update(Z2);
+        #endif
+        #if AXIS_IS_L64XX(Z3)
+          monitor_update(Z3);
+        #endif
+        #if AXIS_IS_L64XX(Z4)
+          monitor_update(Z4);
+        #endif
+        #if AXIS_IS_L64XX(E0)
+          monitor_update(E0);
+        #endif
+        #if AXIS_IS_L64XX(E1)
+          monitor_update(E1);
+        #endif
+        #if AXIS_IS_L64XX(E2)
+          monitor_update(E2);
+        #endif
+        #if AXIS_IS_L64XX(E3)
+          monitor_update(E3);
+        #endif
+        #if AXIS_IS_L64XX(E4)
+          monitor_update(E4);
+        #endif
+        #if AXIS_IS_L64XX(E5)
+          monitor_update(E5);
+        #endif
+        #if AXIS_IS_L64XX(E6)
+          monitor_update(E6);
+        #endif
+        #if AXIS_IS_L64XX(E7)
+          monitor_update(E7);
+        #endif
+
+        if (TERN0(L6470_DEBUG, report_L6470_status)) DEBUG_EOL();
+
+        spi_active = false;   // done with all SPI transfers - clear handshake flags
+        spi_abort = false;
+      }
+    }
+  }
+
+#endif // MONITOR_L6470_DRIVER_STATUS
+
+#endif // HAS_L64XX

src/libs/L64XX/L64XX_Marlin.h

@@ -0,0 +1,141 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "../../inc/MarlinConfig.h"
+
+#include <L6470.h>
+#if !(L6470_LIBRARY_VERSION >= 0x000800)
+  #error 'L6470_LIBRARY_VERSION 0x000800 or later required'
+#endif
+
+#define L6470_GETPARAM(P,Q) stepper##Q.GetParam(P)
+
+#define dSPIN_STEP_CLOCK      0x58
+#define dSPIN_STEP_CLOCK_FWD dSPIN_STEP_CLOCK
+#define dSPIN_STEP_CLOCK_REV dSPIN_STEP_CLOCK+1
+#define HAS_L64XX_EXTRUDER (AXIS_IS_L64XX(E0) || AXIS_IS_L64XX(E1) || AXIS_IS_L64XX(E2) || AXIS_IS_L64XX(E3) || AXIS_IS_L64XX(E4) || AXIS_IS_L64XX(E5) || AXIS_IS_L64XX(E6) || AXIS_IS_L64XX(E7))
+
+#define _EN_ITEM(N) , E##N
+enum L64XX_axis_t : uint8_t { MAIN_AXIS_NAMES, X2, Y2, Z2, Z3, Z4 REPEAT(E_STEPPERS, _EN_ITEM), MAX_L64XX };
+#undef _EN_ITEM
+
+class L64XX_Marlin : public L64XXHelper {
+public:
+  static PGM_P const index_to_axis[MAX_L64XX];
+
+  static const uint8_t index_to_dir[MAX_L64XX];
+
+  static uint8_t dir_commands[MAX_L64XX];
+
+  // Flags to guarantee graceful switch if stepper interrupts L6470 SPI transfer
+  static volatile uint8_t spi_abort;
+  static uint8_t spi_active;
+
+  L64XX_Marlin() {}
+
+  static void init();
+  static void init_to_defaults();
+
+  static uint16_t get_stepper_status(L64XX &st);
+
+  static uint16_t get_status(const L64XX_axis_t axis);
+
+  static uint32_t get_param(const L64XX_axis_t axis, const uint8_t param);
+
+  static void set_param(const L64XX_axis_t axis, const uint8_t param, const uint32_t value);
+
+  //static void send_command(const L64XX_axis_t axis, uint8_t command);
+
+  static uint8_t get_user_input(uint8_t &driver_count, L64XX_axis_t axis_index[3], char axis_mon[3][3],
+                            float &position_max, float &position_min, float &final_feedrate, uint8_t &kval_hold,
+                            uint8_t over_current_flag, uint8_t &OCD_TH_val, uint8_t &STALL_TH_val, uint16_t &over_current_threshold);
+
+  static void transfer(uint8_t L6470_buf[], const uint8_t length);
+
+  static void say_axis(const L64XX_axis_t axis, const uint8_t label=true);
+  #if ENABLED(L6470_CHITCHAT)
+    static void error_status_decode(
+      const uint16_t status, const L64XX_axis_t axis,
+      const uint16_t _status_axis_th_sd, const uint16_t _status_axis_th_wrn,
+      const uint16_t _status_axis_step_loss_a, const uint16_t _status_axis_step_loss_b,
+      const uint16_t _status_axis_ocd, const uint8_t _status_axis_layout
+    );
+  #else
+    FORCE_INLINE static void error_status_decode(
+      const uint16_t, const L64XX_axis_t,
+      const uint16_t, const uint16_t,
+      const uint16_t, const uint16_t,
+      const uint16_t, const uint8_t
+    ){}
+  #endif
+
+  // ~40 bytes SRAM to simplify status decode routines
+  typedef struct {
+    uint8_t STATUS_AXIS_LAYOUT;              // Copy of L6470_status_layout
+    uint8_t AXIS_OCD_TH_MAX;              // Size of OCD_TH field
+    uint8_t AXIS_STALL_TH_MAX;            // Size of STALL_TH field
+    float AXIS_OCD_CURRENT_CONSTANT_INV;   // mA per count
+    float AXIS_STALL_CURRENT_CONSTANT_INV; // mA per count
+    uint8_t L6470_AXIS_CONFIG,            // Address of the CONFIG register
+            L6470_AXIS_STATUS;            // Address of the STATUS register
+    uint16_t L6470_ERROR_MASK,            // STATUS_UVLO | STATUS_TH_WRN | STATUS_TH_SD  | STATUS_OCD | STATUS_STEP_LOSS_A | STATUS_STEP_LOSS_B
+             L6474_ERROR_MASK,            // STATUS_UVLO | STATUS_TH_WRN | STATUS_TH_SD  | STATUS_OCD
+             STATUS_AXIS_RAW,             // Copy of status register contents
+             STATUS_AXIS,                 // Copy of status register contents but with all error bits active low
+             STATUS_AXIS_OCD,             // Overcurrent detected bit position
+             STATUS_AXIS_SCK_MOD,         // Step clock mode is active bit position
+             STATUS_AXIS_STEP_LOSS_A,     // Stall detected on A bridge bit position
+             STATUS_AXIS_STEP_LOSS_B,     // Stall detected on B bridge bit position
+             STATUS_AXIS_TH_SD,           // Thermal shutdown bit position
+             STATUS_AXIS_TH_WRN,          // Thermal warning bit position
+             STATUS_AXIS_UVLO,            // Undervoltage lockout is active bit position
+             STATUS_AXIS_WRONG_CMD,       // Last command not valid bit position
+             STATUS_AXIS_CMD_ERR,         // Command error bit position
+             STATUS_AXIS_NOTPERF_CMD;     // Last command not performed bit position
+  } L64XX_shadow_t;
+
+  static L64XX_shadow_t shadow;
+
+  #if ENABLED(MONITOR_L6470_DRIVER_STATUS)
+    static bool monitor_paused;
+    static void pause_monitor(const bool p) { monitor_paused = p; }
+    static void monitor_update(L64XX_axis_t stepper_index);
+    static void monitor_driver();
+  #else
+    static void pause_monitor(const bool) {}
+  #endif
+
+//protected:
+  // L64XXHelper methods
+  static void spi_init();
+  static uint8_t transfer_single(uint8_t data, int16_t ss_pin);
+  static uint8_t transfer_chain(uint8_t data, int16_t ss_pin, uint8_t chain_position);
+
+private:
+  static void append_stepper_err(char* &p, const uint8_t stepper_index, const char * const err=nullptr);
+
+};
+
+void echo_yes_no(const bool yes);
+
+extern L64XX_Marlin L64xxManager;

src/libs/L64XX/README.md

@@ -0,0 +1,98 @@
+### L64XX Stepper Driver
+
+*Arduino-L6470* library revision 0.8.0 or above is required.
+
+This software can be used with the L6470, L6474, L6480 and the powerSTEP01 (collectively referred to as "L64xx" from now on). Different drivers can be mixed within a system.
+
+These devices use voltage PWMs to drive the stepper phases. On the L6474 the phase current is controlled by the `TVAL` register. On all the other drivers the phase current is indirectly controlled via the `KVAL_HOLD` register which scales the PWM duty cycle.
+
+This software assumes that all drivers are in one SPI daisy chain.
+
+### Hardware Setup
+
+- MOSI from controller tied to SDI on the first device
+
+- SDO of the first device is tied to SDI of the next device
+
+- SDO of the last device is tied to MISO of the controller
+
+- All devices share the same `SCK_PIN` and `SS_PIN` pins. The user must supply a macro to control the `RESET_PIN`(s).
+
+- Each L6470 passes the data it saw on its SDI to its neighbor on the **NEXT** SPI cycle (8 bit delay).
+
+- Each L6470 acts on the **last** SPI data it saw when the `SS_PIN` **goes high**.
+
+The L6474 uses the standard STEP DIR interface. Phase currents are changed in response to step pulses. The direction is set by the DIR pin. Instead of an ENA pin, stepper power is controlled with SPI commands.
+
+The other drivers operate in `STEP_CLOCK` mode. In this mode the Direction / Enable functions are done with SPI commands and the phase currents are changed in response to STEP pulses.
+
+### Hardware / Software Interaction
+
+Except for the L6474, powering up a stepper and setting the direction are done by the same command. You can't do one without the other.
+
+**All** directions are set **every time** a new block is popped off the queue by the stepper ISR.
+
+When setting direction, SPI transfers are minimized by using arrays and a specialized SPI method. *Arduino-L6470* library calls are not used. For N L64xx drivers, this results in N bytes transferred. If library calls were used then N<sup>2</sup> bytes would be sent.
+
+### Power-up (Reset) Sequence
+
+- Stepper objects are instantiated before the `setup()` entry point is reached.
+
+- In `setup()` (before stepper drivers are initialized) the `L6470_init()` method is called to do the following:
+
+  - If present, pulse the hardware reset pin.
+
+  - Populate the `L6470_chain` array, which maps positions in the SPI stream to commands/data for L64XX stepper drivers.
+
+  - Initialize the L64XX Software SPI pin states.
+
+  - Initialize L64XX drivers. They may be reset later by a call to `L6470_init_to_defaults()`.
+
+The steppers are **NOT** powered up (enabled) during this sequence.
+
+### `L6470_chain` array
+
+This array is used by all routines that transmit SPI data. For a chain with N devices, the array contains:
+
+Index|Value
+-----|-----
+0|Number of drivers in chain
+1|Axis index of the first device in the chain (closest to MOSI)
+...|
+N|Axis index of the last device chain (closest to MISO)
+
+### Set Direction and Enable
+
+The `DIR_WRITE` macros for the L64xx drivers are written so that the standard X, Y, Z and extruder logic used by the `set_directions()` routine is not altered. These macros write the correct forward/reverse command to the corresponding location in the array `L6470_dir_commands`. On the L6474 the array the command used just enables the stepper because direction is set by the DIR pin.
+
+At the end of the `set_directions()` routine, the array `L6470_chain` is used to grab the corresponding direction/enable commands out of the array `L6470_dir_commands` and put them in the correct sequence in the array `L6470_buf`. Array `L6470_buf` is then passed to the **`void`** `L6470_Transfer` function which actually sends the data to the devices.
+
+### Utilities, etc.
+
+The **absolute position** registers should accurately reflect Marlin’s stepper position counts. They are set to zero during initialization. `G28` sets them to the Marlin counts for the corresponding axis after homing. NOTE: These registers are often the negative of the Marlin counts. This is because the Marlin counts reflect the logical direction while the registers reflect the stepper direction. The register contents are displayed via the `M114 D` command.
+
+The `L6470_monitor` feature reads the status of each device every half second. It will report if there are any error conditions present or if communications has been lost/restored. The `KVAL_HOLD` value is reduced every 2 – 2.5 seconds if the thermal warning or thermal shutdown conditions are present.
+
+**M122** displays the settings of most of the bits in the status register plus a couple of other items.
+
+**M906** can be used to set the `KVAL_HOLD` register (`TVAL` on L6474) one driver at a time. If a setting is not included with the command then the contents of the registers that affect the phase current/voltage are displayed.
+
+**M916, M917 & M918**
+
+These utilities are used to tune the system. They can get you in the ballpark for acceptable jerk, acceleration, top speed and `KVAL_HOLD` settings (`TVAL` on L6474). In general they seem to provide an overly optimistic `KVAL_HOLD` (`TVAL`) setting because of the lag between setting `KVAL_HOLD` (`TVAL`) and the driver reaching final temperature. Enabling the `L6470_monitor` feature during prints will provide the **final useful setting**.
+
+The amount of power needed to move the stepper without skipping steps increases as jerk, acceleration, top speed, and micro-steps increase. The power dissipated by the driver increases as the power to the stepper increases. The net result is a balancing act between jerk, acceleration, top speed, micro-steps, and power dissipated by the driver.
+
+**M916** - Increases `KVAL_HOLD` (`TVAL`) while moving one axis until a thermal warning is generated. This routine is also useful for determining the approximate `KVAL_HOLD` (`TVAL`) where the stepper stops losing steps. The sound will get noticeably quieter as it stops losing steps.
+
+**M917** - Find minimum current thresholds. This is accomplished by doing the following steps while moving an axis:
+
+1. Decrease OCD current until overcurrent error.
+
+2. Increase OCD until overcurrent error goes away.
+
+3. Decrease stall threshold until stall error (not available on the L6474).
+
+4. Increase stall until stall error goes away (not available on the L6474).
+
+**M918** - Increase speed until error or max feedrate achieved.

src/libs/MAX31865.cpp

@@ -0,0 +1,638 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * Based on Based on Adafruit MAX31865 library:
+ *
+ * This is a library for the Adafruit PT100/P1000 RTD Sensor w/MAX31865
+ * Designed specifically to work with the Adafruit RTD Sensor
+ * https://www.adafruit.com/products/3328
+ *
+ * This sensor uses SPI to communicate, 4 pins are required to interface.
+ *
+ * Adafruit invests time and resources providing this open source code,
+ * please support Adafruit and open-source hardware by purchasing
+ * products from Adafruit!
+ *
+ * Written by Limor Fried/Ladyada for Adafruit Industries.
+ *
+ * Modifications by JoAnn Manges (@GadgetAngel)
+ * Copyright (c) 2020, JoAnn Manges
+ * All rights reserved.
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if HAS_MAX31865 && !USE_ADAFRUIT_MAX31865
+
+#include "MAX31865.h"
+
+#ifndef MAX31865_MIN_SAMPLING_TIME_MSEC
+  #define MAX31865_MIN_SAMPLING_TIME_MSEC 0
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_MAX31865)
+#include "../core/debug_out.h"
+
+// The maximum speed the MAX31865 can do is 5 MHz
+SPISettings MAX31865::spiConfig = SPISettings(
+  TERN(TARGET_LPC1768, SPI_QUARTER_SPEED, TERN(ARDUINO_ARCH_STM32, SPI_CLOCK_DIV4, 500000)),
+  MSBFIRST,
+  SPI_MODE1 // CPOL0 CPHA1
+);
+
+#if DISABLED(LARGE_PINMAP)
+
+  /**
+   * Create the interface object using software (bitbang) SPI for PIN values
+   * less than or equal to 127.
+   *
+   * @param spi_cs    the SPI CS pin to use
+   * @param spi_mosi  the SPI MOSI pin to use
+   * @param spi_miso  the SPI MISO pin to use
+   * @param spi_clk   the SPI clock pin to use
+  */
+  MAX31865::MAX31865(int8_t spi_cs, int8_t spi_mosi, int8_t spi_miso, int8_t spi_clk) {
+    cselPin = spi_cs;
+    mosiPin = spi_mosi;
+    misoPin = spi_miso;
+    sclkPin = spi_clk;
+  }
+
+  /**
+   * Create the interface object using hardware SPI for PIN for PIN values less
+   * than or equal to 127.
+   *
+   * @param spi_cs  the SPI CS pin to use along with the default SPI device
+   */
+  MAX31865::MAX31865(int8_t spi_cs) {
+    cselPin = spi_cs;
+    sclkPin = misoPin = mosiPin = -1;
+  }
+
+#else // LARGE_PINMAP
+
+  /**
+   * Create the interface object using software (bitbang) SPI for PIN values
+   * which are larger than 127. If you have PIN values less than or equal to
+   * 127 use the other call for SW SPI.
+   *
+   * @param spi_cs       the SPI CS pin to use
+   * @param spi_mosi     the SPI MOSI pin to use
+   * @param spi_miso     the SPI MISO pin to use
+   * @param spi_clk      the SPI clock pin to use
+   * @param pin_mapping  set to 1 for positive pin values
+   */
+  MAX31865::MAX31865(uint32_t spi_cs, uint32_t spi_mosi, uint32_t spi_miso, uint32_t spi_clk, uint8_t pin_mapping) {
+    cselPin = spi_cs;
+    mosiPin = spi_mosi;
+    misoPin = spi_miso;
+    sclkPin = spi_clk;
+  }
+
+  /**
+   * Create the interface object using hardware SPI for PIN values which are
+   * larger than 127. If you have PIN values less than or equal to 127 use
+   * the other call for HW SPI.
+   *
+   * @param spi_cs       the SPI CS pin to use along with the default SPI device
+   * @param pin_mapping  set to 1 for positive pin values
+   */
+  MAX31865::MAX31865(uint32_t spi_cs, uint8_t pin_mapping) {
+    cselPin = spi_cs;
+    sclkPin = misoPin = mosiPin = -1UL;  //-1UL or 0xFFFFFFFF or 4294967295
+  }
+
+#endif // LARGE_PINMAP
+
+/**
+ *
+ * Instance & Class methods
+ *
+ */
+
+/**
+ * Initialize the SPI interface and set the number of RTD wires used
+ *
+ * @param wires     The number of wires as an enum: MAX31865_2WIRE, MAX31865_3WIRE, or MAX31865_4WIRE.
+ * @param zero_res  The resistance of the RTD at 0°C, in ohms.
+ * @param ref_res   The resistance of the reference resistor, in ohms.
+ * @param wire_res  The resistance of the wire connecting the sensor to the RTD, in ohms.
+ */
+void MAX31865::begin(max31865_numwires_t wires, const_float_t zero_res, const_float_t ref_res, const_float_t wire_res) {
+  resNormalizer = 100.0f / zero_res;  // reciprocal of resistance, scaled by 100
+  refRes = ref_res;
+  wireRes = wire_res;
+
+  pinMode(cselPin, OUTPUT);
+  digitalWrite(cselPin, HIGH);
+
+  if (sclkPin != TERN(LARGE_PINMAP, -1UL, 255))
+    softSpiInit(); // Define pin modes for Software SPI
+  else {
+    DEBUG_ECHOLNPGM("Init MAX31865 Hardware SPI");
+    SPI.begin();    // Start and configure hardware SPI
+  }
+
+  initFixedFlags(wires);
+
+  DEBUG_ECHOLNPGM("MAX31865 Regs: CFG ", readRegister8(MAX31865_CONFIG_REG),
+    "|RTD ", readRegister16(MAX31865_RTDMSB_REG),
+    "|HTHRS ", readRegister16(MAX31865_HFAULTMSB_REG),
+    "|LTHRS ", readRegister16(MAX31865_LFAULTMSB_REG),
+    "|FLT  ", readRegister8(MAX31865_FAULTSTAT_REG));
+
+  // fault detection cycle seems to initialize the sensor better
+  runAutoFaultDetectionCycle(); // also initializes flags
+
+  if (lastFault)
+    SERIAL_ECHOLNPGM("MAX31865 init fault ", lastFault);
+
+  writeRegister16(MAX31865_HFAULTMSB_REG, 0xFFFF);
+  writeRegister16(MAX31865_LFAULTMSB_REG, 0);
+
+  #if ENABLED(MAX31865_USE_AUTO_MODE) // make a proper first read to initialize _lastRead
+
+   uint16_t rtd = readRegister16(MAX31865_RTDMSB_REG);
+
+    #if MAX31865_IGNORE_INITIAL_FAULTY_READS > 0
+      rtd = fixFault(rtd);
+    #endif
+
+    if (rtd & 1) {
+      lastRead = 0xFFFF; // some invalid value
+      lastFault = readRegister8(MAX31865_FAULTSTAT_REG);
+      clearFault(); // also clears the bias voltage flag, so no further action is required
+
+      DEBUG_ECHOLNPGM("MAX31865 read fault: ", rtd);
+    }
+    else {
+      DEBUG_ECHOLNPGM("RTD MSB:", (rtd >> 8), "  RTD LSB:", (rtd & 0x00FF));
+      lastRead = rtd;
+      TERN_(MAX31865_USE_READ_ERROR_DETECTION, lastReadStamp = millis());
+    }
+
+  #else
+
+    enableBias();
+    DELAY_US(2000); // according to the datasheet, 10.5τ+1msec (see below)
+    oneShot();
+    DELAY_US(63000);
+    uint16_t rtd = readRegister16(MAX31865_RTDMSB_REG);
+
+    #if MAX31865_IGNORE_INITIAL_FAULTY_READS > 0
+      rtd = fixFault(rtd);
+    #endif
+
+    if (rtd & 1) {
+      lastRead = 0xFFFF; // some invalid value
+      lastFault = readRegister8(MAX31865_FAULTSTAT_REG);
+      clearFault(); // also clears the bias voltage flag, so no further action is required
+
+      DEBUG_ECHOLNPGM("MAX31865 read fault: ", rtd);
+    }
+    else {
+      DEBUG_ECHOLNPGM("RTD MSB:", (rtd >> 8), "  RTD LSB:", (rtd & 0x00FF));
+
+      resetFlags();
+
+      lastRead = rtd;
+      nextEvent = SETUP_BIAS_VOLTAGE;
+      millis_t now = millis();
+      nextEventStamp = now + MAX31865_MIN_SAMPLING_TIME_MSEC;
+
+      TERN_(MAX31865_USE_READ_ERROR_DETECTION, lastReadStamp = now);
+    }
+
+  #endif // MAX31865_USE_AUTO_MODE
+
+  DEBUG_ECHOLNPGM(
+    TERN(LARGE_PINMAP, "LARGE_PINMAP", "Regular")
+    " begin call with cselPin: ", cselPin,
+    " misoPin: ", misoPin,
+    " sclkPin: ", sclkPin,
+    " mosiPin: ", mosiPin,
+    " config: ", readRegister8(MAX31865_CONFIG_REG)
+    );
+}
+
+/**
+ * Return and clear the last fault value
+ *
+ * @return The raw unsigned 8-bit FAULT status register or spike fault
+ */
+uint8_t MAX31865::readFault() {
+  uint8_t r = lastFault;
+  lastFault = 0;
+  return r;
+}
+
+/**
+ * Clear last fault
+ */
+void MAX31865::clearFault() {
+  setConfig(MAX31865_CONFIG_FAULTSTAT, 1);
+}
+
+/**
+ * Reset flags
+ */
+void MAX31865::resetFlags() {
+  writeRegister8(MAX31865_CONFIG_REG, stdFlags);
+}
+
+/**
+ * Enable the bias voltage on the RTD sensor
+ */
+void MAX31865::enableBias() {
+  setConfig(MAX31865_CONFIG_BIAS, 1);
+}
+
+/**
+ * Start a one-shot temperature reading.
+ */
+void MAX31865::oneShot() {
+  setConfig(MAX31865_CONFIG_1SHOT | MAX31865_CONFIG_BIAS, 1);
+}
+
+void MAX31865::runAutoFaultDetectionCycle() {
+  writeRegister8(MAX31865_CONFIG_REG, (stdFlags & 0x11) | 0x84 ); // cfg reg = 100X010Xb
+  DELAY_US(600);
+  for (int i = 0; i < 10 && (readRegister8(MAX31865_CONFIG_REG) & 0xC) > 0; i++) DELAY_US(100); // Fault det completes when bits 2 and 3 are zero (or after 10 tries)
+  readFault();
+  clearFault();
+}
+
+/**
+ * Set a value in the configuration register.
+ *
+ * @param config  8-bit value for the config item
+ * @param enable  whether to enable or disable the value
+ */
+void MAX31865::setConfig(uint8_t config, bool enable) {
+  uint8_t t = stdFlags;
+  if (enable)
+    t |= config;
+  else
+    t &= ~config;
+  writeRegister8(MAX31865_CONFIG_REG, t);
+}
+
+/**
+ * Initialize standard flags with flags that will not change during operation (Hz, polling mode and no. of wires)
+ *
+ * @param wires The number of wires in enum format
+ */
+void MAX31865::initFixedFlags(max31865_numwires_t wires) {
+
+  // set config-defined flags (same for all sensors)
+  stdFlags = TERN(MAX31865_50HZ_FILTER, MAX31865_CONFIG_FILT50HZ, MAX31865_CONFIG_FILT60HZ) |
+             TERN(MAX31865_USE_AUTO_MODE, MAX31865_CONFIG_MODEAUTO | MAX31865_CONFIG_BIAS, MAX31865_CONFIG_MODEOFF);
+
+  if (wires == MAX31865_3WIRE)
+    stdFlags |= MAX31865_CONFIG_3WIRE;   // 3 wire
+  else
+    stdFlags &= ~MAX31865_CONFIG_3WIRE;  // 2 or 4 wire
+}
+
+#if MAX31865_IGNORE_INITIAL_FAULTY_READS > 0
+
+  inline uint16_t MAX31865::fixFault(uint16_t rtd) {
+    if (!ignore_faults || !(rtd & 1))
+      return rtd;
+
+    ignore_faults--;
+    clearFault();
+
+    DEBUG_ECHOLNPGM("MAX31865 ignoring fault ", (MAX31865_IGNORE_INITIAL_FAULTY_READS) - ignore_faults);
+
+    return rtd & ~1;  // 0xFFFE
+  }
+
+#endif
+
+inline uint16_t MAX31865::readRawImmediate() {
+  uint16_t rtd = readRegister16(MAX31865_RTDMSB_REG);
+  DEBUG_ECHOLNPGM("MAX31865 RTD MSB:", (rtd >> 8), " LSB:", (rtd & 0x00FF));
+
+  #if MAX31865_IGNORE_INITIAL_FAULTY_READS > 0
+    rtd = fixFault(rtd);
+  #endif
+
+  if (rtd & 1) {
+    lastFault = readRegister8(MAX31865_FAULTSTAT_REG);
+    lastRead |= 1;
+    clearFault(); // also clears the bias voltage flag, so no further action is required
+    DEBUG_ECHOLNPGM("MAX31865 read fault: ", lastFault);
+  }
+  else {
+    TERN_(MAX31865_USE_READ_ERROR_DETECTION, const millis_t ms = millis());
+    if (TERN0(MAX31865_USE_READ_ERROR_DETECTION, ABS((int)(lastRead - rtd)) > 500 && PENDING(ms, lastReadStamp + 1000))) {
+      // If 2 readings within 1s differ too much (~20°C) it's a read error.
+      lastFault = 0x01;
+      lastRead |= 1;
+      DEBUG_ECHOLNPGM("MAX31865 read error: ", rtd);
+    }
+    else {
+      lastRead = rtd;
+      TERN_(MAX31865_USE_READ_ERROR_DETECTION, lastReadStamp = ms);
+    }
+  }
+
+  return rtd;
+}
+
+/**
+ * Read the raw 16-bit value from the RTD_REG in one shot mode. This will include
+ * the fault bit, D0.
+ *
+ * @return The raw unsigned 16-bit register value with ERROR bit attached, NOT temperature!
+ */
+uint16_t MAX31865::readRaw() {
+
+  #if ENABLED(MAX31865_USE_AUTO_MODE)
+
+    readRawImmediate();
+
+  #else
+
+    const millis_t ms = millis();
+
+    if (PENDING(ms, nextEventStamp)) {
+      DEBUG_ECHOLNPGM("MAX31865 waiting for event ", nextEvent);
+      return lastRead;
+    }
+
+    switch (nextEvent) {
+      case SETUP_BIAS_VOLTAGE:
+        enableBias();
+        nextEventStamp = ms + 2; // wait at least 10.5*τ (τ = 100nF*430Ω max for PT100 / 10nF*4.3ΚΩ for PT1000 = 43μsec) + 1msec
+        nextEvent = SETUP_1_SHOT_MODE;
+        DEBUG_ECHOLNPGM("MAX31865 bias voltage enabled");
+        break;
+
+      case SETUP_1_SHOT_MODE:
+        oneShot();
+        nextEventStamp = ms + TERN(MAX31865_50HZ_FILTER, 63, 52); // wait at least 52msec for 60Hz (63msec for 50Hz) before reading RTD register
+        nextEvent = READ_RTD_REG;
+        DEBUG_ECHOLNPGM("MAX31865 1 shot mode enabled");
+        break;
+
+      case READ_RTD_REG:
+
+        if (!(readRawImmediate() & 1))   // if clearFault() was not invoked, need to clear the bias voltage and 1-shot flags
+          resetFlags();
+
+        nextEvent = SETUP_BIAS_VOLTAGE;
+        nextEventStamp = ms + (MAX31865_MIN_SAMPLING_TIME_MSEC); // next step should not occur within less than MAX31865_MIN_SAMPLING_TIME_MSEC from the last one
+        break;
+    }
+
+  #endif
+
+  return lastRead;
+}
+
+/**
+ * Calculate and return the resistance value of the connected RTD.
+ *
+ * @return             The raw RTD resistance value, NOT temperature!
+ */
+float MAX31865::readResistance() {
+  // Strip the error bit (D0) and convert to a float ratio.
+  // less precise method: (readRaw() * refRes) >> 16
+  return ((readRaw() * RECIPROCAL(65536.0f)) * refRes - wireRes);
+}
+
+/**
+ * Read the RTD and pass it to temperature(float) for calculation.
+ *
+ * @return  Temperature in C
+ */
+float MAX31865::temperature() {
+  return temperature(readResistance());
+}
+
+/**
+ * Given the 15-bit ADC value, calculate the resistance and pass it to temperature(float) for calculation.
+ *
+ * @return  Temperature in C
+ */
+float MAX31865::temperature(const uint16_t adc_val) {
+  return temperature(((adc_val) * RECIPROCAL(32768.0f)) * refRes - wireRes);
+}
+
+/**
+ * Calculate the temperature in C from the RTD resistance.
+ *
+ * @param    rtd_res  the resistance value in ohms
+ * @return            the temperature in °C
+ */
+float MAX31865::temperature(float rtd_res) {
+
+  rtd_res *= resNormalizer; // normalize to 100 ohm
+
+  // Constants for calculating temperature from the measured RTD resistance.
+  // http://www.analog.com/media/en/technical-documentation/application-notes/AN709_0.pdf
+  constexpr float RTD_Z1 = -0.0039083,
+                  RTD_Z2 = +1.758480889e-5,
+                  RTD_Z3 = -2.31e-8,
+                  RTD_Z4 = -1.155e-6;
+
+  // Callender-Van Dusen equation
+  float temp = (RTD_Z1 + sqrt(RTD_Z2 + (RTD_Z3 * rtd_res))) * RECIPROCAL(RTD_Z4);
+
+  //
+  // The previous equation is valid only for temperatures of 0°C and above.
+  // The equation for RRTD(t) that defines negative temperature behavior is a
+  // fourth-order polynomial (after expanding the third term) and is quite
+  // impractical to solve for a single expression of temperature as a function
+  // of resistance. So here we use a Linear Approximation instead.
+  //
+  if (temp < 0) {
+    #ifndef MAX31865_APPROX
+      #define MAX31865_APPROX 5
+    #endif
+
+    constexpr float RTD_C[] = {
+      #if MAX31865_APPROX == 5
+        -242.02, +2.2228, +2.5859e-3, -4.8260e-6, -2.8183e-8, +1.5243e-10
+      #elif MAX31865_APPROX == 4
+        -241.96, +2.2163, +2.8541e-3, -9.9121e-6, -1.7152e-8
+      #elif MAX31865_APPROX == 3
+        -242.09, +2.2276, +2.5178e-3, -5.8620e-6
+      #else
+        -242.97, +2.2838, +1.4727e-3
+      #endif
+    };
+
+    float rpoly = rtd_res;
+    temp = RTD_C[0];
+    temp += rpoly * RTD_C[1];
+    rpoly *= rtd_res; temp += rpoly * RTD_C[2];
+    if (MAX31865_APPROX >= 3) { rpoly *= rtd_res; temp += rpoly * RTD_C[3]; }
+    if (MAX31865_APPROX >= 4) { rpoly *= rtd_res; temp += rpoly * RTD_C[4]; }
+    if (MAX31865_APPROX >= 5) { rpoly *= rtd_res; temp += rpoly * RTD_C[5]; }
+  }
+
+  return temp;
+}
+
+/**
+ * MAX31865 SPI Timing constants
+ * See MAX31865 datasheet (https://datasheets.maximintegrated.com/en/ds/MAX31865.pdf)
+ * All timings in nsec, minimum values.
+ */
+
+#define MAX31865_SPI_TIMING_TCC    400    // CS to SCLK setup
+#define MAX31865_SPI_TIMING_TDC     35    // Data to SCLK setup
+#define MAX31865_SPI_TIMING_TCL    100    // SCK half period
+#define MAX31865_SPI_TIMING_TCCH   100    // SCK to CS hold
+#define MAX31865_SPI_TIMING_TCWH   400    // CS inactive time (min)
+
+/**
+ * Read a single byte from the specified register address.
+ *
+ * @param  addr  the register address
+ * @return       the register contents
+ */
+uint8_t MAX31865::readRegister8(uint8_t addr) {
+  uint8_t ret = 0;
+  readRegisterN(addr, &ret, 1);
+  return ret;
+}
+
+/**
+ * Read two bytes: 1 from the specified register address, and 1 from the next address.
+ *
+ * @param  addr  the first register address
+ * @return       both register contents as a single 16-bit int
+ */
+uint16_t MAX31865::readRegister16(uint8_t addr) {
+  uint8_t buffer[2] = { 0 };
+  readRegisterN(addr, buffer, 2);
+  return uint16_t(buffer[0]) << 8 | buffer[1];
+}
+
+/**
+ * Read +n+ bytes from a specified address into +buffer+. Set D7 to 0 to specify a read.
+ *
+ * @param addr    the first register address
+ * @param buffer  storage for the read bytes
+ * @param n       the number of bytes to read
+ */
+void MAX31865::readRegisterN(uint8_t addr, uint8_t buffer[], uint8_t n) {
+
+  addr &= 0x7F; // make sure top bit is not set
+
+  spiBeginTransaction();
+  spiTransfer(addr);
+
+  while (n--) {
+    buffer[0] = spiTransfer(0xFF);
+    buffer++;
+  }
+
+  spiEndTransaction();
+}
+
+void MAX31865::writeRegister16(uint8_t addr, uint16_t data) {
+  spiBeginTransaction();
+  spiTransfer(addr | 0x80); // make sure top bit is set
+  spiTransfer(data >> 8);
+  spiTransfer(data & 0xFF);
+  spiEndTransaction();
+}
+
+/**
+ * Write an 8-bit value to a register. Set D7 to 1 to specify a write.
+ *
+ * @param addr  the address to write to
+ * @param data  the data to write
+ */
+void MAX31865::writeRegister8(uint8_t addr, uint8_t data) {
+  spiBeginTransaction();
+  spiTransfer(addr | 0x80); // make sure top bit is set
+  spiTransfer(data);
+  spiEndTransaction();
+}
+
+void MAX31865::spiBeginTransaction() {
+  digitalWrite(sclkPin, LOW); // ensure CPOL0
+  DELAY_NS_VAR(MAX31865_SPI_TIMING_TCWH); // ensure minimum time of CS inactivity after previous operation
+  digitalWrite(cselPin, LOW);
+  DELAY_NS_VAR(MAX31865_SPI_TIMING_TCC);
+
+  if (sclkPin == TERN(LARGE_PINMAP, -1UL, 255))
+    SPI.beginTransaction(spiConfig);
+  else
+    digitalWrite(sclkPin, HIGH);
+}
+
+void MAX31865::spiEndTransaction() {
+  if (sclkPin == TERN(LARGE_PINMAP, -1UL, 255))
+    SPI.endTransaction();
+  else
+    digitalWrite(sclkPin, LOW);
+
+  DELAY_NS_VAR(MAX31865_SPI_TIMING_TCCH);
+
+  digitalWrite(cselPin, HIGH);
+}
+
+/**
+ * Transfer SPI data +x+ and read the response. From the datasheet...
+ * Input data (SDI) is latched on the internal strobe edge and output data (SDO) is
+ * shifted out on the shift edge. There is one clock for each bit transferred.
+ * Address and data bits are transferred in groups of eight, MSB first.
+ *
+ * @param  x  an 8-bit chunk of data to write
+ * @return    the 8-bit response
+ */
+uint8_t MAX31865::spiTransfer(uint8_t x) {
+  if (sclkPin == TERN(LARGE_PINMAP, -1UL, 255))
+    return SPI.transfer(x);
+
+  uint8_t reply = 0;
+  for (int i = 7; i >= 0; i--) {
+    digitalWrite(mosiPin, x & _BV(i));
+    DELAY_NS_VAR(MAX31865_SPI_TIMING_TDC);
+    digitalWrite(sclkPin, LOW);
+    DELAY_NS_VAR(MAX31865_SPI_TIMING_TCL - MAX31865_SPI_TIMING_TDC);
+    reply <<= 1;
+    if (digitalRead(misoPin)) reply |= 1;
+    DELAY_NS_VAR(MAX31865_SPI_TIMING_TDC);
+    digitalWrite(sclkPin, HIGH);
+    DELAY_NS_VAR(MAX31865_SPI_TIMING_TCL - MAX31865_SPI_TIMING_TDC);
+  }
+  return reply;
+}
+
+void MAX31865::softSpiInit() {
+  DEBUG_ECHOLNPGM("Initializing MAX31865 Software SPI");
+  pinMode(sclkPin, OUTPUT);
+  digitalWrite(sclkPin, LOW);
+  pinMode(mosiPin, OUTPUT);
+  pinMode(misoPin, INPUT);
+}
+
+#endif // HAS_MAX31865 && !USE_ADAFRUIT_MAX31865

src/libs/MAX31865.h

@@ -0,0 +1,166 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * Based on Adafruit MAX31865 library:
+ *
+ * This is a library for the Adafruit PT100/P1000 RTD Sensor w/MAX31865
+ * Designed specifically to work with the Adafruit RTD Sensor
+ * https://www.adafruit.com/products/3328
+ *
+ * This sensor uses SPI to communicate, 4 pins are required to interface.
+ *
+ * Adafruit invests time and resources providing this open source code,
+ * please support Adafruit and open-source hardware by purchasing
+ * products from Adafruit!
+ *
+ * Written by Limor Fried/Ladyada for Adafruit Industries.
+ *
+ * Modifications by JoAnn Manges (@GadgetAngel)
+ * Copyright (c) 2020, JoAnn Manges
+ * All rights reserved.
+ */
+#pragma once
+
+//#define DEBUG_MAX31865
+
+#include "../inc/MarlinConfig.h"
+#include "../HAL/shared/Delay.h"
+#include HAL_PATH(../HAL, MarlinSPI.h)
+
+#define MAX31865_CONFIG_REG 0x00
+#define MAX31865_CONFIG_BIAS 0x80
+#define MAX31865_CONFIG_MODEAUTO 0x40
+#define MAX31865_CONFIG_MODEOFF 0x00
+#define MAX31865_CONFIG_1SHOT 0x20
+#define MAX31865_CONFIG_3WIRE 0x10
+#define MAX31865_CONFIG_24WIRE 0x00
+#define MAX31865_CONFIG_FAULTSTAT 0x02
+#define MAX31865_CONFIG_FILT50HZ 0x01
+#define MAX31865_CONFIG_FILT60HZ 0x00
+
+#define MAX31865_RTDMSB_REG 0x01
+#define MAX31865_RTDLSB_REG 0x02
+#define MAX31865_HFAULTMSB_REG 0x03
+#define MAX31865_HFAULTLSB_REG 0x04
+#define MAX31865_LFAULTMSB_REG 0x05
+#define MAX31865_LFAULTLSB_REG 0x06
+#define MAX31865_FAULTSTAT_REG 0x07
+
+#define MAX31865_FAULT_HIGHTHRESH 0x80  // D7
+#define MAX31865_FAULT_LOWTHRESH 0x40   // D6
+#define MAX31865_FAULT_REFINLOW 0x20    // D5
+#define MAX31865_FAULT_REFINHIGH 0x10   // D4
+#define MAX31865_FAULT_RTDINLOW 0x08    // D3
+#define MAX31865_FAULT_OVUV 0x04        // D2
+
+typedef enum max31865_numwires {
+  MAX31865_2WIRE = 0,
+  MAX31865_3WIRE = 1,
+  MAX31865_4WIRE = 0
+} max31865_numwires_t;
+
+#if DISABLED(MAX31865_USE_AUTO_MODE)
+  typedef enum one_shot_event : uint8_t {
+    SETUP_BIAS_VOLTAGE,
+    SETUP_1_SHOT_MODE,
+    READ_RTD_REG
+  } one_shot_event_t;
+#endif
+
+/* Interface class for the MAX31865 RTD Sensor reader */
+class MAX31865 {
+private:
+  static SPISettings spiConfig;
+
+  TERN(LARGE_PINMAP, uint32_t, uint8_t) sclkPin, misoPin, mosiPin, cselPin;
+
+  uint16_t spiDelay;
+
+  float resNormalizer, refRes, wireRes;
+
+  #if ENABLED(MAX31865_USE_READ_ERROR_DETECTION)
+    millis_t lastReadStamp = 0;
+  #endif
+
+  uint16_t lastRead = 0;
+  uint8_t lastFault = 0;
+
+  #if DISABLED(MAX31865_USE_AUTO_MODE)
+    millis_t nextEventStamp;
+    one_shot_event_t nextEvent;
+  #endif
+
+  #ifdef MAX31865_IGNORE_INITIAL_FAULTY_READS
+    uint8_t ignore_faults = MAX31865_IGNORE_INITIAL_FAULTY_READS;
+    uint16_t fixFault(uint16_t rtd);
+  #endif
+
+  uint8_t stdFlags = 0;
+
+  void setConfig(uint8_t config, bool enable);
+
+  void readRegisterN(uint8_t addr, uint8_t buffer[], uint8_t n);
+  uint8_t readRegister8(uint8_t addr);
+  uint16_t readRegister16(uint8_t addr);
+
+  void writeRegister8(uint8_t addr, uint8_t reg);
+  void writeRegister16(uint8_t addr, uint16_t reg);
+
+  void softSpiInit();
+  void spiBeginTransaction();
+  uint8_t spiTransfer(uint8_t addr);
+  void spiEndTransaction();
+
+  void initFixedFlags(max31865_numwires_t wires);
+
+  void enable50HzFilter(bool b);
+  void enableBias();
+  void oneShot();
+  void resetFlags();
+
+  uint16_t readRawImmediate();
+
+  void runAutoFaultDetectionCycle();
+
+public:
+  #if ENABLED(LARGE_PINMAP)
+    MAX31865(uint32_t spi_cs, uint8_t pin_mapping);
+    MAX31865(uint32_t spi_cs, uint32_t spi_mosi, uint32_t spi_miso,
+             uint32_t spi_clk, uint8_t pin_mapping);
+  #else
+    MAX31865(int8_t spi_cs);
+    MAX31865(int8_t spi_cs, int8_t spi_mosi, int8_t spi_miso,
+             int8_t spi_clk);
+  #endif
+
+  void begin(max31865_numwires_t wires, const_float_t zero_res, const_float_t ref_res, const_float_t wire_res);
+
+  uint8_t readFault();
+  void clearFault();
+
+  uint16_t readRaw();
+  float readResistance();
+  float temperature();
+  float temperature(const uint16_t adc_val);
+  float temperature(float rtd_res);
+};

src/libs/W25Qxx.cpp

@@ -0,0 +1,383 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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_SPI_FLASH
+
+#include "W25Qxx.h"
+
+W25QXXFlash W25QXX;
+
+#ifndef NC
+  #define NC -1
+#endif
+
+MarlinSPI W25QXXFlash::mySPI(SPI_FLASH_MOSI_PIN, SPI_FLASH_MISO_PIN, SPI_FLASH_SCK_PIN, NC);
+
+#define SPI_FLASH_CS_H() OUT_WRITE(SPI_FLASH_CS_PIN, HIGH)
+#define SPI_FLASH_CS_L() OUT_WRITE(SPI_FLASH_CS_PIN, LOW)
+
+bool flash_dma_mode = true;
+
+void W25QXXFlash::init(uint8_t spiRate) {
+
+  OUT_WRITE(SPI_FLASH_CS_PIN, HIGH);
+
+  /**
+   * STM32F1 APB2 = 72MHz, APB1 = 36MHz, max SPI speed of this MCU if 18Mhz
+   * STM32F1 has 3 SPI ports, SPI1 in APB2, SPI2/SPI3 in APB1
+   * so the minimum prescale of SPI1 is DIV4, SPI2/SPI3 is DIV2
+   */
+  #if SPI_DEVICE == 1
+    #define SPI_CLOCK_MAX SPI_CLOCK_DIV4
+  #else
+    #define SPI_CLOCK_MAX SPI_CLOCK_DIV2
+  #endif
+  uint8_t clock;
+  switch (spiRate) {
+    case SPI_FULL_SPEED:    clock = SPI_CLOCK_MAX;  break;
+    case SPI_HALF_SPEED:    clock = SPI_CLOCK_DIV4; break;
+    case SPI_QUARTER_SPEED: clock = SPI_CLOCK_DIV8; break;
+    case SPI_EIGHTH_SPEED:  clock = SPI_CLOCK_DIV16; break;
+    case SPI_SPEED_5:       clock = SPI_CLOCK_DIV32; break;
+    case SPI_SPEED_6:       clock = SPI_CLOCK_DIV64; break;
+    default:                clock = SPI_CLOCK_DIV2;// Default from the SPI library
+  }
+
+  mySPI.setClockDivider(clock);
+  mySPI.setBitOrder(MSBFIRST);
+  mySPI.setDataMode(SPI_MODE0);
+  mySPI.begin();
+}
+
+/**
+ * @brief  Receive a single byte from the SPI port.
+ *
+ * @return Byte received
+ */
+uint8_t W25QXXFlash::spi_flash_Rec() {
+  const uint8_t returnByte = mySPI.transfer(0xFF);
+  return returnByte;
+}
+
+uint8_t W25QXXFlash::spi_flash_read_write_byte(uint8_t data) {
+  const uint8_t returnByte = mySPI.transfer(data);
+  return returnByte;
+}
+
+/**
+ * @brief  Receive a number of bytes from the SPI port to a buffer
+ *
+ * @param  buf   Pointer to starting address of buffer to write to.
+ * @param  nbyte Number of bytes to receive.
+ * @return Nothing
+ *
+ * @details Uses DMA
+ */
+void W25QXXFlash::spi_flash_Read(uint8_t *buf, uint16_t nbyte) {
+  mySPI.dmaTransfer(0, const_cast<uint8_t*>(buf), nbyte);
+}
+
+/**
+ * @brief  Send a single byte on SPI port
+ *
+ * @param  b Byte to send
+ *
+ * @details
+ */
+void W25QXXFlash::spi_flash_Send(uint8_t b) { mySPI.transfer(b); }
+
+/**
+ * @brief  Write token and then write from 512 byte buffer to SPI (for SD card)
+ *
+ * @param  buf   Pointer with buffer start address
+ * @return Nothing
+ *
+ * @details Use DMA
+ */
+void W25QXXFlash::spi_flash_SendBlock(uint8_t token, const uint8_t *buf) {
+  mySPI.transfer(token);
+  mySPI.dmaSend(const_cast<uint8_t*>(buf), 512);
+}
+
+uint16_t W25QXXFlash::W25QXX_ReadID(void) {
+  uint16_t Temp = 0;
+  SPI_FLASH_CS_L();
+  spi_flash_Send(0x90);
+  spi_flash_Send(0x00);
+  spi_flash_Send(0x00);
+  spi_flash_Send(0x00);
+  Temp |= spi_flash_Rec() << 8;
+  Temp |= spi_flash_Rec();
+  SPI_FLASH_CS_H();
+  return Temp;
+}
+
+void W25QXXFlash::SPI_FLASH_WriteEnable() {
+  // Select the FLASH: Chip Select low
+  SPI_FLASH_CS_L();
+  // Send "Write Enable" instruction
+  spi_flash_Send(W25X_WriteEnable);
+  // Deselect the FLASH: Chip Select high
+  SPI_FLASH_CS_H();
+}
+
+/*******************************************************************************
+* Function Name  : SPI_FLASH_WaitForWriteEnd
+* Description    : Polls the status of the Write In Progress (WIP) flag in the
+*                  FLASH's status register and loop until write operation has
+*                  completed.
+* Input          : None
+* Output         : None
+* Return         : None
+*******************************************************************************/
+void W25QXXFlash::SPI_FLASH_WaitForWriteEnd() {
+  uint8_t FLASH_Status = 0;
+
+  // Select the FLASH: Chip Select low
+  SPI_FLASH_CS_L();
+  // Send "Read Status Register" instruction
+  spi_flash_Send(W25X_ReadStatusReg);
+
+  // Loop as long as the memory is busy with a write cycle
+  do
+    /* Send a dummy byte to generate the clock needed by the FLASH
+    and put the value of the status register in FLASH_Status variable */
+    FLASH_Status = spi_flash_Rec();
+  while ((FLASH_Status & WIP_Flag) == 0x01); // Write in progress
+
+  // Deselect the FLASH: Chip Select high
+  SPI_FLASH_CS_H();
+}
+
+void W25QXXFlash::SPI_FLASH_SectorErase(uint32_t SectorAddr) {
+  // Send write enable instruction
+  SPI_FLASH_WriteEnable();
+
+  // Sector Erase
+  // Select the FLASH: Chip Select low
+  SPI_FLASH_CS_L();
+  // Send Sector Erase instruction
+  spi_flash_Send(W25X_SectorErase);
+  // Send SectorAddr high nybble address byte
+  spi_flash_Send((SectorAddr & 0xFF0000) >> 16);
+  // Send SectorAddr medium nybble address byte
+  spi_flash_Send((SectorAddr & 0xFF00) >> 8);
+  // Send SectorAddr low nybble address byte
+  spi_flash_Send(SectorAddr & 0xFF);
+  // Deselect the FLASH: Chip Select high
+
+  SPI_FLASH_CS_H();
+  // Wait the end of Flash writing
+  SPI_FLASH_WaitForWriteEnd();
+}
+
+void W25QXXFlash::SPI_FLASH_BlockErase(uint32_t BlockAddr) {
+  SPI_FLASH_WriteEnable();
+  SPI_FLASH_CS_L();
+  // Send Sector Erase instruction
+  spi_flash_Send(W25X_BlockErase);
+  // Send SectorAddr high nybble address byte
+  spi_flash_Send((BlockAddr & 0xFF0000) >> 16);
+  // Send SectorAddr medium nybble address byte
+  spi_flash_Send((BlockAddr & 0xFF00) >> 8);
+  // Send SectorAddr low nybble address byte
+  spi_flash_Send(BlockAddr & 0xFF);
+
+  SPI_FLASH_CS_H();
+
+  SPI_FLASH_WaitForWriteEnd();
+}
+
+/*******************************************************************************
+* Function Name  : SPI_FLASH_BulkErase
+* Description    : Erases the entire FLASH.
+* Input          : None
+* Output         : None
+* Return         : None
+*******************************************************************************/
+void W25QXXFlash::SPI_FLASH_BulkErase() {
+  // Send write enable instruction
+  SPI_FLASH_WriteEnable();
+
+  // Bulk Erase
+  // Select the FLASH: Chip Select low
+  SPI_FLASH_CS_L();
+
+  // Send Bulk Erase instruction
+  spi_flash_Send(W25X_ChipErase);
+  // Deselect the FLASH: Chip Select high
+  SPI_FLASH_CS_H();
+  // Wait the end of Flash writing
+  SPI_FLASH_WaitForWriteEnd();
+}
+
+/*******************************************************************************
+* Function Name  : SPI_FLASH_PageWrite
+* Description    : Writes more than one byte to the FLASH with a single WRITE
+*                  cycle(Page WRITE sequence). The number of byte can't exceed
+*                  the FLASH page size.
+* Input          : - pBuffer : pointer to the buffer  containing the data to be
+*                    written to the FLASH.
+*                  - WriteAddr : FLASH's internal address to write to.
+*                  - NumByteToWrite : number of bytes to write to the FLASH,
+*                    must be equal or less than "SPI_FLASH_PageSize" value.
+* Output         : None
+* Return         : None
+*******************************************************************************/
+void W25QXXFlash::SPI_FLASH_PageWrite(uint8_t *pBuffer, uint32_t WriteAddr, uint16_t NumByteToWrite) {
+  // Enable the write access to the FLASH
+  SPI_FLASH_WriteEnable();
+
+  // Select the FLASH: Chip Select low
+  SPI_FLASH_CS_L();
+  // Send "Write to Memory " instruction
+  spi_flash_Send(W25X_PageProgram);
+  // Send WriteAddr high nybble address byte to write to
+  spi_flash_Send((WriteAddr & 0xFF0000) >> 16);
+  // Send WriteAddr medium nybble address byte to write to
+  spi_flash_Send((WriteAddr & 0xFF00) >> 8);
+  // Send WriteAddr low nybble address byte to write to
+  spi_flash_Send(WriteAddr & 0xFF);
+
+  NOMORE(NumByteToWrite, SPI_FLASH_PerWritePageSize);
+
+  // While there is data to be written on the FLASH
+  while (NumByteToWrite--) {
+    // Send the current byte
+    spi_flash_Send(*pBuffer);
+    // Point on the next byte to be written
+    pBuffer++;
+  }
+
+  // Deselect the FLASH: Chip Select high
+  SPI_FLASH_CS_H();
+
+  // Wait the end of Flash writing
+  SPI_FLASH_WaitForWriteEnd();
+}
+
+/*******************************************************************************
+* Function Name  : SPI_FLASH_BufferWrite
+* Description    : Writes block of data to the FLASH. In this function, the
+*                  number of WRITE cycles are reduced, using Page WRITE sequence.
+* Input          : - pBuffer : pointer to the buffer  containing the data to be
+*                    written to the FLASH.
+*                  - WriteAddr : FLASH's internal address to write to.
+*                  - NumByteToWrite : number of bytes to write to the FLASH.
+* Output         : None
+* Return         : None
+*******************************************************************************/
+void W25QXXFlash::SPI_FLASH_BufferWrite(uint8_t *pBuffer, uint32_t WriteAddr, uint16_t NumByteToWrite) {
+  uint8_t NumOfPage = 0, NumOfSingle = 0, Addr = 0, count = 0, temp = 0;
+
+  Addr = WriteAddr % SPI_FLASH_PageSize;
+  count = SPI_FLASH_PageSize - Addr;
+  NumOfPage = NumByteToWrite / SPI_FLASH_PageSize;
+  NumOfSingle = NumByteToWrite % SPI_FLASH_PageSize;
+
+  if (Addr == 0) { // WriteAddr is SPI_FLASH_PageSize aligned
+    if (NumOfPage == 0) { // NumByteToWrite < SPI_FLASH_PageSize
+      SPI_FLASH_PageWrite(pBuffer, WriteAddr, NumByteToWrite);
+    }
+    else { // NumByteToWrite > SPI_FLASH_PageSize
+      while (NumOfPage--) {
+        SPI_FLASH_PageWrite(pBuffer, WriteAddr, SPI_FLASH_PageSize);
+        WriteAddr += SPI_FLASH_PageSize;
+        pBuffer += SPI_FLASH_PageSize;
+      }
+      SPI_FLASH_PageWrite(pBuffer, WriteAddr, NumOfSingle);
+    }
+  }
+  else { // WriteAddr is not SPI_FLASH_PageSize aligned
+    if (NumOfPage == 0) { // NumByteToWrite < SPI_FLASH_PageSize
+      if (NumOfSingle > count) { // (NumByteToWrite + WriteAddr) > SPI_FLASH_PageSize
+        temp = NumOfSingle - count;
+        SPI_FLASH_PageWrite(pBuffer, WriteAddr, count);
+        WriteAddr += count;
+        pBuffer += count;
+        SPI_FLASH_PageWrite(pBuffer, WriteAddr, temp);
+      }
+      else
+        SPI_FLASH_PageWrite(pBuffer, WriteAddr, NumByteToWrite);
+    }
+    else { // NumByteToWrite > SPI_FLASH_PageSize
+      NumByteToWrite -= count;
+      NumOfPage = NumByteToWrite / SPI_FLASH_PageSize;
+      NumOfSingle = NumByteToWrite % SPI_FLASH_PageSize;
+
+      SPI_FLASH_PageWrite(pBuffer, WriteAddr, count);
+      WriteAddr += count;
+      pBuffer += count;
+
+      while (NumOfPage--) {
+        SPI_FLASH_PageWrite(pBuffer, WriteAddr, SPI_FLASH_PageSize);
+        WriteAddr += SPI_FLASH_PageSize;
+        pBuffer += SPI_FLASH_PageSize;
+      }
+
+      if (NumOfSingle != 0)
+        SPI_FLASH_PageWrite(pBuffer, WriteAddr, NumOfSingle);
+    }
+  }
+}
+
+/*******************************************************************************
+* Function Name  : SPI_FLASH_BufferRead
+* Description    : Reads a block of data from the FLASH.
+* Input          : - pBuffer : pointer to the buffer that receives the data read
+*                    from the FLASH.
+*                  - ReadAddr : FLASH's internal address to read from.
+*                  - NumByteToRead : number of bytes to read from the FLASH.
+* Output         : None
+* Return         : None
+*******************************************************************************/
+void W25QXXFlash::SPI_FLASH_BufferRead(uint8_t *pBuffer, uint32_t ReadAddr, uint16_t NumByteToRead) {
+  // Select the FLASH: Chip Select low
+  SPI_FLASH_CS_L();
+
+  // Send "Read from Memory " instruction
+  spi_flash_Send(W25X_ReadData);
+
+  // Send ReadAddr high nybble address byte to read from
+  spi_flash_Send((ReadAddr & 0xFF0000) >> 16);
+  // Send ReadAddr medium nybble address byte to read from
+  spi_flash_Send((ReadAddr & 0xFF00) >> 8);
+  // Send ReadAddr low nybble address byte to read from
+  spi_flash_Send(ReadAddr & 0xFF);
+
+  if (NumByteToRead <= 32 || !flash_dma_mode) {
+    while (NumByteToRead--) { // While there is data to be read
+      // Read a byte from the FLASH
+      *pBuffer = spi_flash_Rec();
+      // Point to the next location where the byte read will be saved
+      pBuffer++;
+    }
+  }
+  else
+    spi_flash_Read(pBuffer, NumByteToRead);
+
+  SPI_FLASH_CS_H();
+}
+
+#endif // HAS_SPI_FLASH

src/libs/W25Qxx.h

@@ -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/>.
+ *
+ */
+#pragma once
+
+#include <stdint.h>
+
+#include HAL_PATH(../HAL, MarlinSPI.h)
+
+#define W25X_WriteEnable        0x06
+#define W25X_WriteDisable       0x04
+#define W25X_ReadStatusReg      0x05
+#define W25X_WriteStatusReg     0x01
+#define W25X_ReadData           0x03
+#define W25X_FastReadData       0x0B
+#define W25X_FastReadDual       0x3B
+#define W25X_PageProgram        0x02
+#define W25X_BlockErase         0xD8
+#define W25X_SectorErase        0x20
+#define W25X_ChipErase          0xC7
+#define W25X_PowerDown          0xB9
+#define W25X_ReleasePowerDown   0xAB
+#define W25X_DeviceID           0xAB
+#define W25X_ManufactDeviceID   0x90
+#define W25X_JedecDeviceID      0x9F
+
+#define WIP_Flag                0x01  /* Write In Progress (WIP) flag */
+
+#define Dummy_Byte              0xA5
+
+#define SPI_FLASH_SectorSize         4096
+#define SPI_FLASH_PageSize           256
+#define SPI_FLASH_PerWritePageSize   256
+
+class W25QXXFlash {
+private:
+  static MarlinSPI mySPI;
+public:
+  void init(uint8_t spiRate);
+  static uint8_t spi_flash_Rec();
+  static uint8_t spi_flash_read_write_byte(uint8_t data);
+  static void spi_flash_Read(uint8_t *buf, uint16_t nbyte);
+  static void spi_flash_Send(uint8_t b);
+  static void spi_flash_SendBlock(uint8_t token, const uint8_t *buf);
+  static uint16_t W25QXX_ReadID(void);
+  static void SPI_FLASH_WriteEnable();
+  static void SPI_FLASH_WaitForWriteEnd();
+  static void SPI_FLASH_SectorErase(uint32_t SectorAddr);
+  static void SPI_FLASH_BlockErase(uint32_t BlockAddr);
+  static void SPI_FLASH_BulkErase();
+  static void SPI_FLASH_PageWrite(uint8_t *pBuffer, uint32_t WriteAddr, uint16_t NumByteToWrite);
+  static void SPI_FLASH_BufferWrite(uint8_t *pBuffer, uint32_t WriteAddr, uint16_t NumByteToWrite);
+  static void SPI_FLASH_BufferRead(uint8_t *pBuffer, uint32_t ReadAddr, uint16_t NumByteToRead);
+};
+
+extern W25QXXFlash W25QXX;

src/libs/autoreport.h

@@ -0,0 +1,50 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "../inc/MarlinConfig.h"
+
+template <typename Helper>
+struct AutoReporter {
+  millis_t next_report_ms;
+  uint8_t report_interval;
+  #if HAS_MULTI_SERIAL
+    SerialMask report_port_mask;
+    AutoReporter() : report_port_mask(SerialMask::All) {}
+  #endif
+
+  inline void set_interval(uint8_t seconds, const uint8_t limit=60) {
+    report_interval = _MIN(seconds, limit);
+    next_report_ms = millis() + SEC_TO_MS(seconds);
+  }
+
+  inline void tick() {
+    if (!report_interval) return;
+    const millis_t ms = millis();
+    if (ELAPSED(ms, next_report_ms)) {
+      next_report_ms = ms + SEC_TO_MS(report_interval);
+      PORT_REDIRECT(report_port_mask);
+      Helper::report();
+      PORT_RESTORE();
+    }
+  }
+};

src/libs/bresenham.h

@@ -0,0 +1,132 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "../core/serial.h"
+
+/**
+ * bresenham_t.h - Bresenham algorithm template
+ *
+ * An array of values / counters that tick together
+ */
+
+#define FORCE_INLINE  __attribute__((always_inline)) inline
+#define __O3          __attribute__((optimize("O3")))
+
+template <uint8_t uid, uint8_t size>
+struct BresenhamCfg { static constexpr uint8_t UID = uid, SIZE = size; };
+
+template<typename T, typename Cfg>
+class Bresenham {
+private:
+
+  static constexpr T signtest = -1;
+  static_assert(signtest < 0, "Bresenham type must be signed!");
+
+public:
+
+  static T divisor, value[Cfg::SIZE], dir[Cfg::SIZE], dividend[Cfg::SIZE], counter[Cfg::SIZE];
+
+  // Default: Instantiate all items with the identical parameters
+  Bresenham(const T &inDivisor=1, const int8_t &inDir=1, const T &inDividend=1, const T &inValue=0) {
+    for (uint8_t i = 0; i < Cfg::SIZE; i++) init(i, inDivisor, inDir, inDividend, inValue);
+  }
+
+  // Instantiate all items with the same divisor
+  Bresenham(const T &inDivisor, const int8_t (&inDir)[Cfg::SIZE], const T (&inDividend)[Cfg::SIZE], const T (&inValue)[Cfg::SIZE]={0}) {
+    init(inDivisor, inDir, inDividend, inValue);
+  }
+
+  // Instantiate all items with the same divisor and direction
+  Bresenham(const T &inDivisor, const int8_t &inDir, const T (&inDividend)[Cfg::SIZE], const T (&inValue)[Cfg::SIZE]={0}) {
+    init(inDivisor, inDir, inDividend, inValue);
+  }
+
+  // Init all items with the same parameters
+  FORCE_INLINE static void init(const uint8_t index, const T &inDivisor=1, const int8_t &inDir=1, const T &inDividend=1, const T &inValue=0) {
+    divisor         = inDivisor;
+    dir[index]      = inDir;
+    dividend[index] = inDividend;
+    value[index]    = inValue;
+    prime(index);
+  }
+
+  // Init all items with the same divisor
+  FORCE_INLINE static void init(const T &inDivisor, const int8_t (&inDir)[Cfg::SIZE], const T (&inDividend)[Cfg::SIZE], const T (&inValue)[Cfg::SIZE]={0}) {
+    divisor = inDivisor;
+    for (uint8_t i = 0; i < Cfg::SIZE; i++) {
+      dir[i]      = inDir[i];
+      dividend[i] = inDividend[i];
+      value[i]    = inValue[i];
+    }
+    prime();
+  }
+
+  // Init all items with the same divisor and direction
+  FORCE_INLINE static void init(const T &inDivisor, const int8_t &inDir, const T (&inDividend)[Cfg::SIZE], const T (&inValue)[Cfg::SIZE]={0}) {
+    divisor = inDivisor;
+    for (uint8_t i = 0; i < Cfg::SIZE; i++) {
+      dir[i]      = inDir;
+      dividend[i] = inDividend[i];
+      value[i]    = inValue[i];
+    }
+    prime();
+  }
+
+  // Reinit item with new dir, dividend, value keeping the same divisor
+  FORCE_INLINE static void reinit(const uint8_t index, const int8_t &inDir=1, const T &inDividend=1, const T &inValue=0) {
+    dir[index]      = inDir;
+    dividend[index] = inDividend;
+    value[index]    = inValue;
+    prime();
+  }
+
+  FORCE_INLINE static void prime(const uint8_t index) { counter[index] = -(divisor / 2); }
+  FORCE_INLINE static void prime() { for (uint8_t i = 0; i < Cfg::SIZE; i++) prime(i); }
+
+  FORCE_INLINE static void back(const uint8_t index) { counter[index] -= divisor; }
+
+  FORCE_INLINE static bool tick1(const uint8_t index) {
+    counter[index] += dividend[index];
+    return counter[index] > 0;
+  }
+
+  FORCE_INLINE static void tick(const uint8_t index) {
+    if (tick1(index)) { value[index] += dir[index]; back(index); }
+  }
+
+  FORCE_INLINE static void tick1() __O3 { for (uint8_t i = 0; i < Cfg::SIZE; i++) (void)tick1(i); }
+
+  FORCE_INLINE static void tick() __O3 { for (uint8_t i = 0; i < Cfg::SIZE; i++) (void)tick(i); }
+
+  static void report(const uint8_t index) {
+    if (index < Cfg::SIZE) {
+      SERIAL_ECHOPGM("bresenham ", index, " : (", dividend[index], "/", divisor, ") ");
+      if (counter[index] >= 0) SERIAL_CHAR(' ');
+      if (labs(counter[index]) < 100) { SERIAL_CHAR(' '); if (labs(counter[index]) < 10) SERIAL_CHAR(' '); }
+      SERIAL_ECHO(counter[index]);
+      SERIAL_ECHOLNPGM(" ... ", value[index]);
+    }
+  }
+
+  static void report() { for (uint8_t i = 0; i < Cfg::SIZE; i++) report(i); }
+};

src/libs/buzzer.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_BEEPER
+
+#include "buzzer.h"
+#include "../module/temperature.h"
+#include "../lcd/marlinui.h"
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../lcd/extui/ui_api.h"
+#endif
+
+Buzzer::state_t Buzzer::state;
+CircularQueue<tone_t, TONE_QUEUE_LENGTH> Buzzer::buffer;
+Buzzer buzzer;
+
+/**
+ * @brief Add a tone to the queue
+ * @details Adds a tone_t structure to the ring buffer, will block IO if the
+ *          queue is full waiting for one slot to get available.
+ *
+ * @param duration Duration of the tone in milliseconds
+ * @param frequency Frequency of the tone in hertz
+ */
+void Buzzer::tone(const uint16_t duration, const uint16_t frequency/*=0*/) {
+  if (!ui.sound_on) return;
+  while (buffer.isFull()) {
+    tick();
+    thermalManager.task();
+  }
+  tone_t tone = { duration, frequency };
+  buffer.enqueue(tone);
+}
+
+void Buzzer::tick() {
+  if (!ui.sound_on) return;
+  const millis_t now = millis();
+
+  if (!state.endtime) {
+    if (buffer.isEmpty()) return;
+
+    state.tone = buffer.dequeue();
+    state.endtime = now + state.tone.duration;
+
+    if (state.tone.frequency > 0) {
+      #if ENABLED(EXTENSIBLE_UI) && DISABLED(EXTUI_LOCAL_BEEPER)
+        CRITICAL_SECTION_START();
+        ExtUI::onPlayTone(state.tone.frequency, state.tone.duration);
+        CRITICAL_SECTION_END();
+      #elif ENABLED(SPEAKER)
+        CRITICAL_SECTION_START();
+        ::tone(BEEPER_PIN, state.tone.frequency, state.tone.duration);
+        CRITICAL_SECTION_END();
+      #else
+        on();
+      #endif
+    }
+  }
+  else if (ELAPSED(now, state.endtime)) reset();
+}
+
+#endif // HAS_BEEPER

src/libs/buzzer.h

@@ -0,0 +1,135 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "../inc/MarlinConfig.h"
+
+#if HAS_BEEPER
+
+  #include "circularqueue.h"
+
+  #define TONE_QUEUE_LENGTH 4
+
+  /**
+   * @brief Tone structure
+   * @details Simple abstraction of a tone based on a duration and a frequency.
+   */
+  struct tone_t {
+    uint16_t duration;
+    uint16_t frequency;
+  };
+
+  /**
+   * @brief Buzzer class
+   */
+  class Buzzer {
+    public:
+
+      typedef struct {
+        tone_t   tone;
+        uint32_t endtime;
+      } state_t;
+
+    private:
+      static state_t state;
+
+    protected:
+      static CircularQueue<tone_t, TONE_QUEUE_LENGTH> buffer;
+
+      /**
+       * @brief Inverts the state of a digital PIN
+       * @details This will invert the current state of an digital IO pin.
+       */
+      FORCE_INLINE static void invert() { TOGGLE(BEEPER_PIN); }
+
+      /**
+       * @brief Resets the state of the class
+       * @details Brings the class state to a known one.
+       */
+      static void reset() {
+        off();
+        state.endtime = 0;
+      }
+
+    public:
+      /**
+       * @brief Init Buzzer
+       */
+      static void init() {
+        SET_OUTPUT(BEEPER_PIN);
+        reset();
+      }
+
+      /**
+       * @brief Turn on a digital PIN
+       * @details Alias of digitalWrite(PIN, HIGH) using FastIO
+       */
+      FORCE_INLINE static void on() { WRITE(BEEPER_PIN, HIGH); }
+
+      /**
+       * @brief Turn off a digital PIN
+       * @details Alias of digitalWrite(PIN, LOW) using FastIO
+       */
+      FORCE_INLINE static void off() { WRITE(BEEPER_PIN, LOW); }
+
+      static void click(const uint16_t duration) { on(); delay(duration); off(); }
+
+      /**
+       * @brief Add a tone to the queue
+       * @details Adds a tone_t structure to the ring buffer, will block IO if the
+       *          queue is full waiting for one slot to get available.
+       *
+       * @param duration Duration of the tone in milliseconds
+       * @param frequency Frequency of the tone in hertz
+       */
+      static void tone(const uint16_t duration, const uint16_t frequency=0);
+
+      /**
+       * @brief Tick function
+       * @details This function should be called at loop, it will take care of
+       *          playing the tones in the queue.
+       */
+      static void tick();
+  };
+
+  // Provide a buzzer instance
+  extern Buzzer buzzer;
+
+  // Buzz directly via the BEEPER pin tone queue
+  #define BUZZ(d,f) buzzer.tone(d, f)
+
+#elif USE_MARLINUI_BUZZER
+
+  // Use MarlinUI for a buzzer on the LCD
+  #include "../lcd/marlinui.h"
+  #define BUZZ(d,f) ui.buzz(d,f)
+
+#else
+
+  // No buzz capability
+  #define BUZZ(d,f) NOOP
+
+#endif
+
+#define ERR_BUZZ() BUZZ(400, 40);
+#define OKAY_BUZZ() do{ BUZZ(100, 659); BUZZ(10, 0); BUZZ(100, 698); }while(0)
+#define DONE_BUZZ(OK) do{ if (OK) OKAY_BUZZ(); else ERR_BUZZ(); }while(0)

src/libs/circularqueue.h

@@ -0,0 +1,131 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include <stdint.h>
+
+/**
+ * @brief   Circular Queue class
+ * @details Implementation of the classic ring buffer data structure
+ */
+template<typename T, uint8_t N>
+class CircularQueue {
+  private:
+
+    /**
+     * @brief   Buffer structure
+     * @details This structure consolidates all the overhead required to handle
+     *          a circular queue such as the pointers and the buffer vector.
+     */
+    struct buffer_t {
+      uint8_t head;
+      uint8_t tail;
+      uint8_t count;
+      uint8_t size;
+      T queue[N];
+    } buffer;
+
+  public:
+    /**
+     * @brief   Class constructor
+     * @details This class requires two template parameters, T defines the type
+     *          of item this queue will handle and N defines the maximum number of
+     *          items that can be stored on the queue.
+     */
+    CircularQueue<T, N>() {
+      buffer.size = N;
+      buffer.count = buffer.head = buffer.tail = 0;
+    }
+
+    /**
+     * @brief   Removes and returns a item from the queue
+     * @details Removes the oldest item on the queue, pointed to by the
+     *          buffer_t head field. The item is returned to the caller.
+     * @return  type T item
+     */
+    T dequeue() {
+      if (isEmpty()) return T();
+
+      uint8_t index = buffer.head;
+
+      --buffer.count;
+      if (++buffer.head == buffer.size)
+        buffer.head = 0;
+
+      return buffer.queue[index];
+    }
+
+    /**
+     * @brief   Adds an item to the queue
+     * @details Adds an item to the queue on the location pointed by the buffer_t
+     *          tail variable. Returns false if no queue space is available.
+     * @param   item Item to be added to the queue
+     * @return  true if the operation was successful
+     */
+    bool enqueue(T const &item) {
+      if (isFull()) return false;
+
+      buffer.queue[buffer.tail] = item;
+
+      ++buffer.count;
+      if (++buffer.tail == buffer.size)
+        buffer.tail = 0;
+
+      return true;
+    }
+
+    /**
+     * @brief   Checks if the queue has no items
+     * @details Returns true if there are no items on the queue, false otherwise.
+     * @return  true if queue is empty
+     */
+    bool isEmpty() { return buffer.count == 0; }
+
+    /**
+     * @brief   Checks if the queue is full
+     * @details Returns true if the queue is full, false otherwise.
+     * @return  true if queue is full
+     */
+    bool isFull() { return buffer.count == buffer.size; }
+
+    /**
+     * @brief   Gets the queue size
+     * @details Returns the maximum number of items a queue can have.
+     * @return  the queue size
+     */
+    uint8_t size() { return buffer.size; }
+
+    /**
+     * @brief   Gets the next item from the queue without removing it
+     * @details Returns the next item in the queue without removing it
+     *          or updating the pointers.
+     * @return  first item in the queue
+     */
+    T peek() { return buffer.queue[buffer.head]; }
+
+    /**
+     * @brief Gets the number of items on the queue
+     * @details Returns the current number of items stored on the queue.
+     * @return number of items in the queue
+     */
+    uint8_t count() { return buffer.count; }
+};

src/libs/crc16.cpp

@@ -0,0 +1,32 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "crc16.h"
+
+void crc16(uint16_t *crc, const void * const data, uint16_t cnt) {
+  uint8_t *ptr = (uint8_t *)data;
+  while (cnt--) {
+    *crc = (uint16_t)(*crc ^ (uint16_t)(((uint16_t)*ptr++) << 8));
+    for (uint8_t i = 0; i < 8; i++)
+      *crc = (uint16_t)((*crc & 0x8000) ? ((uint16_t)(*crc << 1) ^ 0x1021) : (*crc << 1));
+  }
+}

src/libs/crc16.h

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

src/libs/duration_t.h

@@ -0,0 +1,180 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "../HAL/shared/Marduino.h"
+
+struct duration_t {
+  /**
+   * @brief Duration is stored in seconds
+   */
+  uint32_t value;
+
+  /**
+   * @brief Constructor
+   */
+  duration_t()
+    : duration_t(0) {};
+
+  /**
+   * @brief Constructor
+   *
+   * @param seconds The number of seconds
+   */
+  duration_t(uint32_t const &seconds) {
+    this->value = seconds;
+  }
+
+  /**
+   * @brief Equality comparison
+   * @details Overloads the equality comparison operator
+   *
+   * @param value The number of seconds to compare to
+   * @return True if both durations are equal
+   */
+  bool operator==(const uint32_t &value) const {
+    return (this->value == value);
+  }
+
+  /**
+   * @brief Inequality comparison
+   * @details Overloads the inequality comparison operator
+   *
+   * @param value The number of seconds to compare to
+   * @return False if both durations are equal
+   */
+  bool operator!=(const uint32_t &value) const {
+    return ! this->operator==(value);
+  }
+
+  /**
+   * @brief Formats the duration as years
+   * @return The number of years
+   */
+  inline uint8_t year() const {
+    return this->day() / 365;
+  }
+
+  /**
+   * @brief Formats the duration as days
+   * @return The number of days
+   */
+  inline uint16_t day() const {
+    return this->hour() / 24;
+  }
+
+  /**
+   * @brief Formats the duration as hours
+   * @return The number of hours
+   */
+  inline uint32_t hour() const {
+    return this->minute() / 60;
+  }
+
+  /**
+   * @brief Formats the duration as minutes
+   * @return The number of minutes
+   */
+  inline uint32_t minute() const {
+    return this->second() / 60;
+  }
+
+  /**
+   * @brief Formats the duration as seconds
+   * @return The number of seconds
+   */
+  inline uint32_t second() const {
+    return this->value;
+  }
+
+  #pragma GCC diagnostic push
+  #if GCC_VERSION <= 50000
+    #pragma GCC diagnostic ignored "-Wformat-overflow"
+  #endif
+
+  /**
+   * @brief Formats the duration as a string
+   * @details String will be formatted using a "full" representation of duration
+   *
+   * @param buffer The array pointed to must be able to accommodate 22 bytes
+   *               (21 for the string, 1 more for the terminating nul)
+   *
+   * Output examples:
+   *  123456789012345678901 (strlen)
+   *  135y 364d 23h 59m 59s
+   *  364d 23h 59m 59s
+   *  23h 59m 59s
+   *  59m 59s
+   *  59s
+   */
+  char* toString(char * const buffer) const {
+    const uint16_t y = this->year(),
+                   d = this->day() % 365,
+                   h = this->hour() % 24,
+                   m = this->minute() % 60,
+                   s = this->second() % 60;
+
+         if (y) sprintf_P(buffer, PSTR("%iy %id %ih %im %is"), y, d, h, m, s);
+    else if (d) sprintf_P(buffer, PSTR("%id %ih %im %is"), d, h, m, s);
+    else if (h) sprintf_P(buffer, PSTR("%ih %im %is"), h, m, s);
+    else if (m) sprintf_P(buffer, PSTR("%im %is"), m, s);
+    else sprintf_P(buffer, PSTR("%is"), s);
+    return buffer;
+  }
+
+  /**
+   * @brief Formats the duration as a string
+   * @details String will be formatted using a "digital" representation of duration
+   *
+   * @param buffer The array pointed to must be able to accommodate 10 bytes
+   *
+   * Output examples:
+   *  123456789 (strlen)
+   *  12'34
+   *  99:59
+   *  11d 12:33
+   */
+  uint8_t toDigital(char *buffer, bool with_days=false) const {
+    const uint16_t h = uint16_t(this->hour()),
+                   m = uint16_t(this->minute() % 60UL);
+    if (with_days) {
+      const uint16_t d = this->day();
+      sprintf_P(buffer, PSTR("%hud %02hu:%02hu"), d, h % 24, m);  // 1d 23:45
+      return d >= 10 ? 9 : 8;
+    }
+    else if (!h) {
+      const uint16_t s = uint16_t(this->second() % 60UL);
+      sprintf_P(buffer, PSTR("%02hu'%02hu"), m, s);     // 12'34
+      return 5;
+    }
+    else if (h < 100) {
+      sprintf_P(buffer, PSTR("%02hu:%02hu"), h, m);     // 12:34
+      return 5;
+    }
+    else {
+      sprintf_P(buffer, PSTR("%hu:%02hu"), h, m);       // 123:45
+      return 6;
+    }
+  }
+
+  #pragma GCC diagnostic pop
+};

src/libs/heatshrink/LICENSE

@@ -0,0 +1,14 @@
+Copyright (c) 2013-2015, Scott Vokes <vokes.s@gmail.com>
+All rights reserved.
+
+Permission to use, copy, modify, and/or distribute this software for any
+purpose with or without fee is hereby granted, provided that the above
+copyright notice and this permission notice appear in all copies.
+
+THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

src/libs/heatshrink/heatshrink_common.h

@@ -0,0 +1,20 @@
+/**
+ * libs/heatshrink/heatshrink_common.h
+ */
+#pragma once
+
+#define HEATSHRINK_AUTHOR "Scott Vokes <vokes.s@gmail.com>"
+#define HEATSHRINK_URL "github.com/atomicobject/heatshrink"
+
+/* Version 0.4.1 */
+#define HEATSHRINK_VERSION_MAJOR 0
+#define HEATSHRINK_VERSION_MINOR 4
+#define HEATSHRINK_VERSION_PATCH 1
+
+#define HEATSHRINK_MIN_WINDOW_BITS 4
+#define HEATSHRINK_MAX_WINDOW_BITS 15
+
+#define HEATSHRINK_MIN_LOOKAHEAD_BITS 3
+
+#define HEATSHRINK_LITERAL_MARKER 0x01
+#define HEATSHRINK_BACKREF_MARKER 0x00

src/libs/heatshrink/heatshrink_config.h

@@ -0,0 +1,26 @@
+/**
+ * libs/heatshrink/heatshrink_config.h
+ */
+#pragma once
+
+// Should functionality assuming dynamic allocation be used?
+#ifndef HEATSHRINK_DYNAMIC_ALLOC
+  //#define HEATSHRINK_DYNAMIC_ALLOC 1
+#endif
+
+#if HEATSHRINK_DYNAMIC_ALLOC
+  // Optional replacement of malloc/free
+  #define HEATSHRINK_MALLOC(SZ) malloc(SZ)
+  #define HEATSHRINK_FREE(P, SZ) free(P)
+#else
+  // Required parameters for static configuration
+  #define HEATSHRINK_STATIC_INPUT_BUFFER_SIZE 32
+  #define HEATSHRINK_STATIC_WINDOW_BITS 8
+  #define HEATSHRINK_STATIC_LOOKAHEAD_BITS 4
+#endif
+
+// Turn on logging for debugging
+#define HEATSHRINK_DEBUGGING_LOGS 0
+
+// Use indexing for faster compression. (This requires additional space.)
+#define HEATSHRINK_USE_INDEX 1

src/libs/heatshrink/heatshrink_decoder.cpp

@@ -0,0 +1,384 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if ENABLED(BINARY_FILE_TRANSFER)
+
+/**
+ * libs/heatshrink/heatshrink_decoder.cpp
+ */
+#include "heatshrink_decoder.h"
+
+#include <stdlib.h>
+#include <string.h>
+
+#pragma GCC optimize ("O3")
+
+/* States for the polling state machine. */
+typedef enum {
+  HSDS_TAG_BIT,               /* tag bit */
+  HSDS_YIELD_LITERAL,         /* ready to yield literal byte */
+  HSDS_BACKREF_INDEX_MSB,     /* most significant byte of index */
+  HSDS_BACKREF_INDEX_LSB,     /* least significant byte of index */
+  HSDS_BACKREF_COUNT_MSB,     /* most significant byte of count */
+  HSDS_BACKREF_COUNT_LSB,     /* least significant byte of count */
+  HSDS_YIELD_BACKREF          /* ready to yield back-reference */
+} HSD_state;
+
+#if HEATSHRINK_DEBUGGING_LOGS
+  #include <stdio.h>
+  #include <ctype.h>
+  #include <assert.h>
+  #define LOG(...) fprintf(stderr, __VA_ARGS__)
+  #define ASSERT(X) assert(X)
+  static const char *state_names[] = {
+    "tag_bit",
+    "yield_literal",
+    "backref_index_msb",
+    "backref_index_lsb",
+    "backref_count_msb",
+    "backref_count_lsb",
+    "yield_backref"
+  };
+#else
+  #define LOG(...) /* no-op */
+  #define ASSERT(X) /* no-op */
+#endif
+
+typedef struct {
+  uint8_t *buf;               /* output buffer */
+  size_t buf_size;            /* buffer size */
+  size_t *output_size;        /* bytes pushed to buffer, so far */
+} output_info;
+
+#define NO_BITS ((uint16_t)-1)
+
+/* Forward references. */
+static uint16_t get_bits(heatshrink_decoder *hsd, uint8_t count);
+static void push_byte(heatshrink_decoder *hsd, output_info *oi, uint8_t byte);
+
+#if HEATSHRINK_DYNAMIC_ALLOC
+heatshrink_decoder *heatshrink_decoder_alloc(uint16_t input_buffer_size, uint8_t window_sz2, uint8_t lookahead_sz2) {
+  if ((window_sz2 < HEATSHRINK_MIN_WINDOW_BITS) ||
+      (window_sz2 > HEATSHRINK_MAX_WINDOW_BITS) ||
+      (input_buffer_size == 0) ||
+      (lookahead_sz2 < HEATSHRINK_MIN_LOOKAHEAD_BITS) ||
+      (lookahead_sz2 >= window_sz2)) {
+      return nullptr;
+  }
+  size_t buffers_sz = (1 << window_sz2) + input_buffer_size;
+  size_t sz = sizeof(heatshrink_decoder) + buffers_sz;
+  heatshrink_decoder *hsd = HEATSHRINK_MALLOC(sz);
+  if (!hsd) return nullptr;
+  hsd->input_buffer_size = input_buffer_size;
+  hsd->window_sz2 = window_sz2;
+  hsd->lookahead_sz2 = lookahead_sz2;
+  heatshrink_decoder_reset(hsd);
+  LOG("-- allocated decoder with buffer size of %zu (%zu + %u + %u)\n",
+      sz, sizeof(heatshrink_decoder), (1 << window_sz2), input_buffer_size);
+  return hsd;
+}
+
+void heatshrink_decoder_free(heatshrink_decoder *hsd) {
+  size_t buffers_sz = (1 << hsd->window_sz2) + hsd->input_buffer_size;
+  size_t sz = sizeof(heatshrink_decoder) + buffers_sz;
+  HEATSHRINK_FREE(hsd, sz);
+  (void)sz;   /* may not be used by free */
+}
+#endif
+
+void heatshrink_decoder_reset(heatshrink_decoder *hsd) {
+  size_t buf_sz = 1 << HEATSHRINK_DECODER_WINDOW_BITS(hsd);
+  size_t input_sz = HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(hsd);
+  memset(hsd->buffers, 0, buf_sz + input_sz);
+  hsd->state = HSDS_TAG_BIT;
+  hsd->input_size = 0;
+  hsd->input_index = 0;
+  hsd->bit_index = 0x00;
+  hsd->current_byte = 0x00;
+  hsd->output_count = 0;
+  hsd->output_index = 0;
+  hsd->head_index = 0;
+}
+
+/* Copy SIZE bytes into the decoder's input buffer, if it will fit. */
+HSD_sink_res heatshrink_decoder_sink(heatshrink_decoder *hsd,
+  uint8_t *in_buf, size_t size, size_t *input_size) {
+  if (!hsd || !in_buf || !input_size)
+    return HSDR_SINK_ERROR_NULL;
+
+  size_t rem = HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(hsd) - hsd->input_size;
+  if (rem == 0) {
+    *input_size = 0;
+    return HSDR_SINK_FULL;
+  }
+
+  size = rem < size ? rem : size;
+  LOG("-- sinking %zd bytes\n", size);
+  /* copy into input buffer (at head of buffers) */
+  memcpy(&hsd->buffers[hsd->input_size], in_buf, size);
+  hsd->input_size += size;
+  *input_size = size;
+  return HSDR_SINK_OK;
+}
+
+
+/*****************
+ * Decompression *
+ *****************/
+
+#define BACKREF_COUNT_BITS(HSD) (HEATSHRINK_DECODER_LOOKAHEAD_BITS(HSD))
+#define BACKREF_INDEX_BITS(HSD) (HEATSHRINK_DECODER_WINDOW_BITS(HSD))
+
+// States
+static HSD_state st_tag_bit(heatshrink_decoder *hsd);
+static HSD_state st_yield_literal(heatshrink_decoder *hsd, output_info *oi);
+static HSD_state st_backref_index_msb(heatshrink_decoder *hsd);
+static HSD_state st_backref_index_lsb(heatshrink_decoder *hsd);
+static HSD_state st_backref_count_msb(heatshrink_decoder *hsd);
+static HSD_state st_backref_count_lsb(heatshrink_decoder *hsd);
+static HSD_state st_yield_backref(heatshrink_decoder *hsd, output_info *oi);
+
+HSD_poll_res heatshrink_decoder_poll(heatshrink_decoder *hsd, uint8_t *out_buf, size_t out_buf_size, size_t *output_size) {
+  if (!hsd || !out_buf || !output_size)
+    return HSDR_POLL_ERROR_NULL;
+
+  *output_size = 0;
+
+  output_info oi;
+  oi.buf = out_buf;
+  oi.buf_size = out_buf_size;
+  oi.output_size = output_size;
+
+  while (1) {
+    LOG("-- poll, state is %d (%s), input_size %d\n", hsd->state, state_names[hsd->state], hsd->input_size);
+    uint8_t in_state = hsd->state;
+    switch (in_state) {
+      case HSDS_TAG_BIT:
+        hsd->state = st_tag_bit(hsd);
+        break;
+      case HSDS_YIELD_LITERAL:
+        hsd->state = st_yield_literal(hsd, &oi);
+        break;
+      case HSDS_BACKREF_INDEX_MSB:
+        hsd->state = st_backref_index_msb(hsd);
+        break;
+      case HSDS_BACKREF_INDEX_LSB:
+        hsd->state = st_backref_index_lsb(hsd);
+        break;
+      case HSDS_BACKREF_COUNT_MSB:
+        hsd->state = st_backref_count_msb(hsd);
+        break;
+      case HSDS_BACKREF_COUNT_LSB:
+        hsd->state = st_backref_count_lsb(hsd);
+        break;
+      case HSDS_YIELD_BACKREF:
+        hsd->state = st_yield_backref(hsd, &oi);
+        break;
+      default:
+        return HSDR_POLL_ERROR_UNKNOWN;
+    }
+
+    // If the current state cannot advance, check if input or output
+    // buffer are exhausted.
+    if (hsd->state == in_state)
+      return (*output_size == out_buf_size) ? HSDR_POLL_MORE : HSDR_POLL_EMPTY;
+  }
+}
+
+static HSD_state st_tag_bit(heatshrink_decoder *hsd) {
+  uint32_t bits = get_bits(hsd, 1);  // get tag bit
+  if (bits == NO_BITS)
+    return HSDS_TAG_BIT;
+  else if (bits)
+    return HSDS_YIELD_LITERAL;
+  else if (HEATSHRINK_DECODER_WINDOW_BITS(hsd) > 8)
+    return HSDS_BACKREF_INDEX_MSB;
+  else {
+    hsd->output_index = 0;
+    return HSDS_BACKREF_INDEX_LSB;
+  }
+}
+
+static HSD_state st_yield_literal(heatshrink_decoder *hsd, output_info *oi) {
+  /* Emit a repeated section from the window buffer, and add it (again)
+   * to the window buffer. (Note that the repetition can include
+   * itself.)*/
+  if (*oi->output_size < oi->buf_size) {
+    uint16_t byte = get_bits(hsd, 8);
+    if (byte == NO_BITS) { return HSDS_YIELD_LITERAL; } /* out of input */
+    uint8_t *buf = &hsd->buffers[HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(hsd)];
+    uint16_t mask = (1 << HEATSHRINK_DECODER_WINDOW_BITS(hsd))  - 1;
+    uint8_t c = byte & 0xFF;
+    LOG("-- emitting literal byte 0x%02x ('%c')\n", c, isprint(c) ? c : '.');
+    buf[hsd->head_index++ & mask] = c;
+    push_byte(hsd, oi, c);
+    return HSDS_TAG_BIT;
+  }
+  return HSDS_YIELD_LITERAL;
+}
+
+static HSD_state st_backref_index_msb(heatshrink_decoder *hsd) {
+  uint8_t bit_ct = BACKREF_INDEX_BITS(hsd);
+  ASSERT(bit_ct > 8);
+  uint16_t bits = get_bits(hsd, bit_ct - 8);
+  LOG("-- backref index (msb), got 0x%04x (+1)\n", bits);
+  if (bits == NO_BITS) { return HSDS_BACKREF_INDEX_MSB; }
+  hsd->output_index = bits << 8;
+  return HSDS_BACKREF_INDEX_LSB;
+}
+
+static HSD_state st_backref_index_lsb(heatshrink_decoder *hsd) {
+  uint8_t bit_ct = BACKREF_INDEX_BITS(hsd);
+  uint16_t bits = get_bits(hsd, bit_ct < 8 ? bit_ct : 8);
+  LOG("-- backref index (lsb), got 0x%04x (+1)\n", bits);
+  if (bits == NO_BITS) { return HSDS_BACKREF_INDEX_LSB; }
+  hsd->output_index |= bits;
+  hsd->output_index++;
+  uint8_t br_bit_ct = BACKREF_COUNT_BITS(hsd);
+  hsd->output_count = 0;
+  return (br_bit_ct > 8) ? HSDS_BACKREF_COUNT_MSB : HSDS_BACKREF_COUNT_LSB;
+}
+
+static HSD_state st_backref_count_msb(heatshrink_decoder *hsd) {
+  uint8_t br_bit_ct = BACKREF_COUNT_BITS(hsd);
+  ASSERT(br_bit_ct > 8);
+  uint16_t bits = get_bits(hsd, br_bit_ct - 8);
+  LOG("-- backref count (msb), got 0x%04x (+1)\n", bits);
+  if (bits == NO_BITS) { return HSDS_BACKREF_COUNT_MSB; }
+  hsd->output_count = bits << 8;
+  return HSDS_BACKREF_COUNT_LSB;
+}
+
+static HSD_state st_backref_count_lsb(heatshrink_decoder *hsd) {
+  uint8_t br_bit_ct = BACKREF_COUNT_BITS(hsd);
+  uint16_t bits = get_bits(hsd, br_bit_ct < 8 ? br_bit_ct : 8);
+  LOG("-- backref count (lsb), got 0x%04x (+1)\n", bits);
+  if (bits == NO_BITS) { return HSDS_BACKREF_COUNT_LSB; }
+  hsd->output_count |= bits;
+  hsd->output_count++;
+  return HSDS_YIELD_BACKREF;
+}
+
+static HSD_state st_yield_backref(heatshrink_decoder *hsd, output_info *oi) {
+  size_t count = oi->buf_size - *oi->output_size;
+  if (count > 0) {
+    size_t i = 0;
+    if (hsd->output_count < count) count = hsd->output_count;
+    uint8_t *buf = &hsd->buffers[HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(hsd)];
+    uint16_t mask = (1 << HEATSHRINK_DECODER_WINDOW_BITS(hsd)) - 1;
+    uint16_t neg_offset = hsd->output_index;
+    LOG("-- emitting %zu bytes from -%u bytes back\n", count, neg_offset);
+    ASSERT(neg_offset <= mask + 1);
+    ASSERT(count <= (size_t)(1 << BACKREF_COUNT_BITS(hsd)));
+
+    for (i = 0; i < count; i++) {
+      uint8_t c = buf[(hsd->head_index - neg_offset) & mask];
+      push_byte(hsd, oi, c);
+      buf[hsd->head_index & mask] = c;
+      hsd->head_index++;
+      LOG("  -- ++ 0x%02x\n", c);
+    }
+    hsd->output_count -= count;
+    if (hsd->output_count == 0) { return HSDS_TAG_BIT; }
+  }
+  return HSDS_YIELD_BACKREF;
+}
+
+/* Get the next COUNT bits from the input buffer, saving incremental progress.
+ * Returns NO_BITS on end of input, or if more than 15 bits are requested. */
+static uint16_t get_bits(heatshrink_decoder *hsd, uint8_t count) {
+  uint16_t accumulator = 0;
+  int i = 0;
+  if (count > 15) return NO_BITS;
+  LOG("-- popping %u bit(s)\n", count);
+
+  /* If we aren't able to get COUNT bits, suspend immediately, because we
+   * don't track how many bits of COUNT we've accumulated before suspend. */
+  if (hsd->input_size == 0 && hsd->bit_index < (1 << (count - 1))) return NO_BITS;
+
+  for (i = 0; i < count; i++) {
+    if (hsd->bit_index == 0x00) {
+      if (hsd->input_size == 0) {
+        LOG("  -- out of bits, suspending w/ accumulator of %u (0x%02x)\n", accumulator, accumulator);
+        return NO_BITS;
+      }
+      hsd->current_byte = hsd->buffers[hsd->input_index++];
+      LOG("  -- pulled byte 0x%02x\n", hsd->current_byte);
+      if (hsd->input_index == hsd->input_size) {
+        hsd->input_index = 0; /* input is exhausted */
+        hsd->input_size = 0;
+      }
+      hsd->bit_index = 0x80;
+    }
+    accumulator <<= 1;
+    if (hsd->current_byte & hsd->bit_index) {
+      accumulator |= 0x01;
+      if (0) {
+        LOG("  -- got 1, accumulator 0x%04x, bit_index 0x%02x\n",
+        accumulator, hsd->bit_index);
+      }
+    }
+    else if (0) {
+      LOG("  -- got 0, accumulator 0x%04x, bit_index 0x%02x\n",
+      accumulator, hsd->bit_index);
+    }
+    hsd->bit_index >>= 1;
+  }
+
+  if (count > 1) LOG("  -- accumulated %08x\n", accumulator);
+  return accumulator;
+}
+
+HSD_finish_res heatshrink_decoder_finish(heatshrink_decoder *hsd) {
+  if (!hsd) return HSDR_FINISH_ERROR_NULL;
+  switch (hsd->state) {
+    case HSDS_TAG_BIT:
+      return hsd->input_size == 0 ? HSDR_FINISH_DONE : HSDR_FINISH_MORE;
+
+    /* If we want to finish with no input, but are in these states, it's
+     * because the 0-bit padding to the last byte looks like a backref
+     * marker bit followed by all 0s for index and count bits. */
+    case HSDS_BACKREF_INDEX_LSB:
+    case HSDS_BACKREF_INDEX_MSB:
+    case HSDS_BACKREF_COUNT_LSB:
+    case HSDS_BACKREF_COUNT_MSB:
+      return hsd->input_size == 0 ? HSDR_FINISH_DONE : HSDR_FINISH_MORE;
+
+    /* If the output stream is padded with 0xFFs (possibly due to being in
+     * flash memory), also explicitly check the input size rather than
+     * uselessly returning MORE but yielding 0 bytes when polling. */
+    case HSDS_YIELD_LITERAL:
+      return hsd->input_size == 0 ? HSDR_FINISH_DONE : HSDR_FINISH_MORE;
+
+    default: return HSDR_FINISH_MORE;
+  }
+}
+
+static void push_byte(heatshrink_decoder *hsd, output_info *oi, uint8_t byte) {
+  LOG(" -- pushing byte: 0x%02x ('%c')\n", byte, isprint(byte) ? byte : '.');
+  oi->buf[(*oi->output_size)++] = byte;
+  (void)hsd;
+}
+
+#endif // BINARY_FILE_TRANSFER

src/libs/heatshrink/heatshrink_decoder.h

@@ -0,0 +1,119 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * libs/heatshrink/heatshrink_decoder.h
+ */
+#pragma once
+
+#include "heatshrink_common.h"
+#include "heatshrink_config.h"
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef enum {
+  HSDR_SINK_OK,               /* data sunk, ready to poll */
+  HSDR_SINK_FULL,             /* out of space in internal buffer */
+  HSDR_SINK_ERROR_NULL=-1,    /* NULL argument */
+} HSD_sink_res;
+
+typedef enum {
+  HSDR_POLL_EMPTY,            /* input exhausted */
+  HSDR_POLL_MORE,             /* more data remaining, call again w/ fresh output buffer */
+  HSDR_POLL_ERROR_NULL=-1,    /* NULL arguments */
+  HSDR_POLL_ERROR_UNKNOWN=-2,
+} HSD_poll_res;
+
+typedef enum {
+  HSDR_FINISH_DONE,           /* output is done */
+  HSDR_FINISH_MORE,           /* more output remains */
+  HSDR_FINISH_ERROR_NULL=-1,  /* NULL arguments */
+} HSD_finish_res;
+
+#if HEATSHRINK_DYNAMIC_ALLOC
+#define HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(BUF) \
+  ((BUF)->input_buffer_size)
+#define HEATSHRINK_DECODER_WINDOW_BITS(BUF) \
+  ((BUF)->window_sz2)
+#define HEATSHRINK_DECODER_LOOKAHEAD_BITS(BUF) \
+  ((BUF)->lookahead_sz2)
+#else
+#define HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(_) \
+  HEATSHRINK_STATIC_INPUT_BUFFER_SIZE
+#define HEATSHRINK_DECODER_WINDOW_BITS(_) \
+  (HEATSHRINK_STATIC_WINDOW_BITS)
+#define HEATSHRINK_DECODER_LOOKAHEAD_BITS(BUF) \
+  (HEATSHRINK_STATIC_LOOKAHEAD_BITS)
+#endif
+
+typedef struct {
+  uint16_t input_size;        /* bytes in input buffer */
+  uint16_t input_index;       /* offset to next unprocessed input byte */
+  uint16_t output_count;      /* how many bytes to output */
+  uint16_t output_index;      /* index for bytes to output */
+  uint16_t head_index;        /* head of window buffer */
+  uint8_t state;              /* current state machine node */
+  uint8_t current_byte;       /* current byte of input */
+  uint8_t bit_index;          /* current bit index */
+
+#if HEATSHRINK_DYNAMIC_ALLOC
+  /* Fields that are only used if dynamically allocated. */
+  uint8_t window_sz2;         /* window buffer bits */
+  uint8_t lookahead_sz2;      /* lookahead bits */
+  uint16_t input_buffer_size; /* input buffer size */
+
+  /* Input buffer, then expansion window buffer */
+  uint8_t buffers[];
+#else
+  /* Input buffer, then expansion window buffer */
+  uint8_t buffers[(1 << HEATSHRINK_DECODER_WINDOW_BITS(_)) + HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(_)];
+#endif
+} heatshrink_decoder;
+
+#if HEATSHRINK_DYNAMIC_ALLOC
+/* Allocate a decoder with an input buffer of INPUT_BUFFER_SIZE bytes,
+ * an expansion buffer size of 2^WINDOW_SZ2, and a lookahead
+ * size of 2^lookahead_sz2. (The window buffer and lookahead sizes
+ * must match the settings used when the data was compressed.)
+ * Returns NULL on error. */
+heatshrink_decoder *heatshrink_decoder_alloc(uint16_t input_buffer_size, uint8_t expansion_buffer_sz2, uint8_t lookahead_sz2);
+
+/* Free a decoder. */
+void heatshrink_decoder_free(heatshrink_decoder *hsd);
+#endif
+
+/* Reset a decoder. */
+void heatshrink_decoder_reset(heatshrink_decoder *hsd);
+
+/* Sink at most SIZE bytes from IN_BUF into the decoder. *INPUT_SIZE is set to
+ * indicate how many bytes were actually sunk (in case a buffer was filled). */
+HSD_sink_res heatshrink_decoder_sink(heatshrink_decoder *hsd, uint8_t *in_buf, size_t size, size_t *input_size);
+
+/* Poll for output from the decoder, copying at most OUT_BUF_SIZE bytes into
+ * OUT_BUF (setting *OUTPUT_SIZE to the actual amount copied). */
+HSD_poll_res heatshrink_decoder_poll(heatshrink_decoder *hsd, uint8_t *out_buf, size_t out_buf_size, size_t *output_size);
+
+/* Notify the dencoder that the input stream is finished.
+ * If the return value is HSDR_FINISH_MORE, there is still more output, so
+ * call heatshrink_decoder_poll and repeat. */
+HSD_finish_res heatshrink_decoder_finish(heatshrink_decoder *hsd);

src/libs/hex_print.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/MarlinConfigPre.h"
+
+#if NEED_HEX_PRINT
+
+#include "hex_print.h"
+#include "../core/serial.h"
+
+#ifdef CPU_32_BIT
+  constexpr int byte_start = 4;
+  static char _hex[] = "0x00000000";
+#else
+  constexpr int byte_start = 0;
+  static char _hex[] = "0x0000";
+#endif
+
+char* hex_byte(const uint8_t b) {
+  _hex[byte_start + 4] = hex_nybble(b >> 4);
+  _hex[byte_start + 5] = hex_nybble(b);
+  return &_hex[byte_start + 4];
+}
+
+inline void _hex_word(const uint16_t w) {
+  _hex[byte_start + 2] = hex_nybble(w >> 12);
+  _hex[byte_start + 3] = hex_nybble(w >> 8);
+  _hex[byte_start + 4] = hex_nybble(w >> 4);
+  _hex[byte_start + 5] = hex_nybble(w);
+}
+
+char* hex_word(const uint16_t w) {
+  _hex_word(w);
+  return &_hex[byte_start + 2];
+}
+
+#ifdef CPU_32_BIT
+  char* hex_long(const uintptr_t l) {
+    _hex[2] = hex_nybble(l >> 28);
+    _hex[3] = hex_nybble(l >> 24);
+    _hex[4] = hex_nybble(l >> 20);
+    _hex[5] = hex_nybble(l >> 16);
+    _hex_word((uint16_t)(l & 0xFFFF));
+    return &_hex[2];
+  }
+#endif
+
+char* hex_address(const void * const w) {
+  #ifdef CPU_32_BIT
+    (void)hex_long((uintptr_t)w);
+  #else
+    (void)hex_word((uintptr_t)w);
+  #endif
+  return _hex;
+}
+
+void print_hex_nybble(const uint8_t n)       { SERIAL_CHAR(hex_nybble(n));  }
+void print_hex_byte(const uint8_t b)         { SERIAL_ECHO(hex_byte(b));    }
+void print_hex_word(const uint16_t w)        { SERIAL_ECHO(hex_word(w));    }
+void print_hex_address(const void * const w) { SERIAL_ECHO(hex_address(w)); }
+
+void print_hex_long(const uint32_t w, const char delimiter) {
+  SERIAL_ECHOPGM("0x");
+  for (int B = 24; B >= 8; B -= 8) {
+    print_hex_byte(w >> B);
+    SERIAL_CHAR(delimiter);
+  }
+  print_hex_byte(w);
+}
+
+#endif // NEED_HEX_PRINT

src/libs/hex_print.h

@@ -0,0 +1,41 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include <stdint.h>
+
+//
+// Utility functions to create and print hex strings as nybble, byte, and word.
+//
+
+constexpr char hex_nybble(const uint8_t n) {
+  return (n & 0xF) + ((n & 0xF) < 10 ? '0' : 'A' - 10);
+}
+char* hex_byte(const uint8_t b);
+char* hex_word(const uint16_t w);
+char* hex_address(const void * const w);
+
+void print_hex_nybble(const uint8_t n);
+void print_hex_byte(const uint8_t b);
+void print_hex_word(const uint16_t w);
+void print_hex_address(const void * const w);
+void print_hex_long(const uint32_t w, const char delimiter);

src/libs/least_squares_fit.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/>.
+ *
+ */
+
+/**
+ * Least Squares Best Fit by Roxy and Ed Williams
+ *
+ * This algorithm is high speed and has a very small code footprint.
+ * Its results are identical to both the Iterative Least-Squares published
+ * earlier by Roxy and the QR_SOLVE solution. If used in place of QR_SOLVE
+ * it saves roughly 10K of program memory. It also does not require all of
+ * coordinates to be present during the calculations. Each point can be
+ * probed and then discarded.
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if NEED_LSF
+
+#include "least_squares_fit.h"
+
+#include <math.h>
+
+int finish_incremental_LSF(struct linear_fit_data *lsf) {
+
+  const float N = lsf->N;
+
+  if (N == 0.0)
+    return 1;
+
+  const float RN = 1.0f / N,
+              xbar = lsf->xbar * RN,
+              ybar = lsf->ybar * RN,
+              zbar = lsf->zbar * RN,
+              x2bar = lsf->x2bar * RN - sq(xbar),
+              y2bar = lsf->y2bar * RN - sq(ybar),
+              xybar = lsf->xybar * RN - xbar * ybar,
+              yzbar = lsf->yzbar * RN - ybar * zbar,
+              xzbar = lsf->xzbar * RN - xbar * zbar,
+              DD = x2bar * y2bar - sq(xybar);
+
+  if (ABS(DD) <= 1e-10 * (lsf->max_absx + lsf->max_absy))
+    return 1;
+
+  lsf->A = (yzbar * xybar - xzbar * y2bar) / DD;
+  lsf->B = (xzbar * xybar - yzbar * x2bar) / DD;
+  lsf->D = -(zbar + lsf->A * xbar + lsf->B * ybar);
+  return 0;
+}
+
+#endif // NEED_LSF

src/libs/least_squares_fit.h

@@ -0,0 +1,87 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * Incremental Least Squares Best Fit  By Roxy and Ed Williams
+ *
+ * This algorithm is high speed and has a very small code footprint.
+ * Its results are identical to both the Iterative Least-Squares published
+ * earlier by Roxy and the QR_SOLVE solution. If used in place of QR_SOLVE
+ * it saves roughly 10K of program memory.   And even better...  the data
+ * fed into the algorithm does not need to all be present at the same time.
+ * A point can be probed and its values fed into the algorithm and then discarded.
+ */
+
+#include "../inc/MarlinConfig.h"
+#include <math.h>
+
+struct linear_fit_data {
+  float xbar, ybar, zbar,
+        x2bar, y2bar,
+        xybar, xzbar, yzbar,
+        max_absx, max_absy,
+        A, B, D, N;
+};
+
+inline void incremental_LSF_reset(struct linear_fit_data *lsf) {
+  memset(lsf, 0, sizeof(linear_fit_data));
+}
+
+inline void incremental_WLSF(struct linear_fit_data *lsf, const_float_t x, const_float_t y, const_float_t z, const_float_t w) {
+  // weight each accumulator by factor w, including the "number" of samples
+  // (analogous to calling inc_LSF twice with same values to weight it by 2X)
+  const float wx = w * x, wy = w * y, wz = w * z;
+  lsf->xbar  += wx;
+  lsf->ybar  += wy;
+  lsf->zbar  += wz;
+  lsf->x2bar += wx * x;
+  lsf->y2bar += wy * y;
+  lsf->xybar += wx * y;
+  lsf->xzbar += wx * z;
+  lsf->yzbar += wy * z;
+  lsf->N     += w;
+  lsf->max_absx = _MAX(ABS(wx), lsf->max_absx);
+  lsf->max_absy = _MAX(ABS(wy), lsf->max_absy);
+}
+inline void incremental_WLSF(struct linear_fit_data *lsf, const xy_pos_t &pos, const_float_t z, const_float_t w) {
+  incremental_WLSF(lsf, pos.x, pos.y, z, w);
+}
+
+inline void incremental_LSF(struct linear_fit_data *lsf, const_float_t x, const_float_t y, const_float_t z) {
+  lsf->xbar += x;
+  lsf->ybar += y;
+  lsf->zbar += z;
+  lsf->x2bar += sq(x);
+  lsf->y2bar += sq(y);
+  lsf->xybar += x * y;
+  lsf->xzbar += x * z;
+  lsf->yzbar += y * z;
+  lsf->max_absx = _MAX(ABS(x), lsf->max_absx);
+  lsf->max_absy = _MAX(ABS(y), lsf->max_absy);
+  lsf->N += 1.0;
+}
+inline void incremental_LSF(struct linear_fit_data *lsf, const xy_pos_t &pos, const_float_t z) {
+  incremental_LSF(lsf, pos.x, pos.y, z);
+}
+
+int finish_incremental_LSF(struct linear_fit_data *);

src/libs/nozzle.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 EITHER(NOZZLE_CLEAN_FEATURE, NOZZLE_PARK_FEATURE)
+
+#include "nozzle.h"
+
+Nozzle nozzle;
+
+#include "../MarlinCore.h"
+#include "../module/motion.h"
+
+#if NOZZLE_CLEAN_MIN_TEMP > 20
+  #include "../module/temperature.h"
+#endif
+
+#if ENABLED(NOZZLE_CLEAN_FEATURE)
+
+  /**
+   * @brief Stroke clean pattern
+   * @details Wipes the nozzle back and forth in a linear movement
+   *
+   * @param start xyz_pos_t defining the starting point
+   * @param end xyz_pos_t defining the ending point
+   * @param strokes number of strokes to execute
+   */
+  void Nozzle::stroke(const xyz_pos_t &start, const xyz_pos_t &end, const uint8_t &strokes) {
+    #if ENABLED(NOZZLE_CLEAN_GOBACK)
+      const xyz_pos_t oldpos = current_position;
+    #endif
+
+    // Move to the starting point
+    #if ENABLED(NOZZLE_CLEAN_NO_Z)
+      #if ENABLED(NOZZLE_CLEAN_NO_Y)
+        do_blocking_move_to_x(start.x);
+      #else
+        do_blocking_move_to_xy(start);
+      #endif
+    #else
+      do_blocking_move_to(start);
+    #endif
+
+    // Start the stroke pattern
+    LOOP_L_N(i, strokes >> 1) {
+      #if ENABLED(NOZZLE_CLEAN_NO_Y)
+        do_blocking_move_to_x(end.x);
+        do_blocking_move_to_x(start.x);
+      #else
+        do_blocking_move_to_xy(end);
+        do_blocking_move_to_xy(start);
+      #endif
+    }
+
+    TERN_(NOZZLE_CLEAN_GOBACK, do_blocking_move_to(oldpos));
+  }
+
+  /**
+   * @brief Zig-zag clean pattern
+   * @details Apply a zig-zag cleaning pattern
+   *
+   * @param start xyz_pos_t defining the starting point
+   * @param end xyz_pos_t defining the ending point
+   * @param strokes number of strokes to execute
+   * @param objects number of triangles to do
+   */
+  void Nozzle::zigzag(const xyz_pos_t &start, const xyz_pos_t &end, const uint8_t &strokes, const uint8_t &objects) {
+    const xy_pos_t diff = end - start;
+    if (!diff.x || !diff.y) return;
+
+    #if ENABLED(NOZZLE_CLEAN_GOBACK)
+      const xyz_pos_t back = current_position;
+    #endif
+
+    #if ENABLED(NOZZLE_CLEAN_NO_Z)
+      do_blocking_move_to_xy(start);
+    #else
+      do_blocking_move_to(start);
+    #endif
+
+    const uint8_t zigs = objects << 1;
+    const bool horiz = ABS(diff.x) >= ABS(diff.y);    // Do a horizontal wipe?
+    const float P = (horiz ? diff.x : diff.y) / zigs; // Period of each zig / zag
+    const xyz_pos_t *side;
+    LOOP_L_N(j, strokes) {
+      for (int8_t i = 0; i < zigs; i++) {
+        side = (i & 1) ? &end : &start;
+        if (horiz)
+          do_blocking_move_to_xy(start.x + i * P, side->y);
+        else
+          do_blocking_move_to_xy(side->x, start.y + i * P);
+      }
+      for (int8_t i = zigs; i >= 0; i--) {
+        side = (i & 1) ? &end : &start;
+        if (horiz)
+          do_blocking_move_to_xy(start.x + i * P, side->y);
+        else
+          do_blocking_move_to_xy(side->x, start.y + i * P);
+      }
+    }
+
+    TERN_(NOZZLE_CLEAN_GOBACK, do_blocking_move_to(back));
+  }
+
+  /**
+   * @brief Circular clean pattern
+   * @details Apply a circular cleaning pattern
+   *
+   * @param start xyz_pos_t defining the middle of circle
+   * @param strokes number of strokes to execute
+   * @param radius radius of circle
+   */
+  void Nozzle::circle(const xyz_pos_t &start, const xyz_pos_t &middle, const uint8_t &strokes, const_float_t radius) {
+    if (strokes == 0) return;
+
+    #if ENABLED(NOZZLE_CLEAN_GOBACK)
+      const xyz_pos_t back = current_position;
+    #endif
+    TERN(NOZZLE_CLEAN_NO_Z, do_blocking_move_to_xy, do_blocking_move_to)(start);
+
+    LOOP_L_N(s, strokes)
+      LOOP_L_N(i, NOZZLE_CLEAN_CIRCLE_FN)
+        do_blocking_move_to_xy(
+          middle.x + sin((RADIANS(360) / NOZZLE_CLEAN_CIRCLE_FN) * i) * radius,
+          middle.y + cos((RADIANS(360) / NOZZLE_CLEAN_CIRCLE_FN) * i) * radius
+        );
+
+    // Let's be safe
+    do_blocking_move_to_xy(start);
+
+    TERN_(NOZZLE_CLEAN_GOBACK, do_blocking_move_to(back));
+  }
+
+  /**
+   * @brief Clean the nozzle
+   * @details Starts the selected clean procedure pattern
+   *
+   * @param pattern one of the available patterns
+   * @param argument depends on the cleaning pattern
+   */
+  void Nozzle::clean(const uint8_t &pattern, const uint8_t &strokes, const_float_t radius, const uint8_t &objects, const uint8_t cleans) {
+    xyz_pos_t start[HOTENDS] = NOZZLE_CLEAN_START_POINT, end[HOTENDS] = NOZZLE_CLEAN_END_POINT, middle[HOTENDS] = NOZZLE_CLEAN_CIRCLE_MIDDLE;
+
+    const uint8_t arrPos = EITHER(SINGLENOZZLE, MIXING_EXTRUDER) ? 0 : active_extruder;
+
+    #if NOZZLE_CLEAN_MIN_TEMP > 20
+      if (thermalManager.degTargetHotend(arrPos) < NOZZLE_CLEAN_MIN_TEMP) {
+        #if ENABLED(NOZZLE_CLEAN_HEATUP)
+          SERIAL_ECHOLNPGM("Nozzle too Cold - Heating");
+          thermalManager.setTargetHotend(NOZZLE_CLEAN_MIN_TEMP, arrPos);
+          thermalManager.wait_for_hotend(arrPos);
+        #else
+          SERIAL_ECHOLNPGM("Nozzle too cold - Skipping wipe");
+          return;
+        #endif
+      }
+    #endif
+
+    #if HAS_SOFTWARE_ENDSTOPS
+
+      #define LIMIT_AXIS(A) do{ \
+        LIMIT( start[arrPos].A, soft_endstop.min.A, soft_endstop.max.A); \
+        LIMIT(middle[arrPos].A, soft_endstop.min.A, soft_endstop.max.A); \
+        LIMIT(   end[arrPos].A, soft_endstop.min.A, soft_endstop.max.A); \
+      }while(0)
+
+      if (soft_endstop.enabled()) {
+
+        LIMIT_AXIS(x);
+        LIMIT_AXIS(y);
+        LIMIT_AXIS(z);
+        const bool radiusOutOfRange = (middle[arrPos].x + radius > soft_endstop.max.x)
+                                   || (middle[arrPos].x - radius < soft_endstop.min.x)
+                                   || (middle[arrPos].y + radius > soft_endstop.max.y)
+                                   || (middle[arrPos].y - radius < soft_endstop.min.y);
+        if (radiusOutOfRange && pattern == 2) {
+          SERIAL_ECHOLNPGM("Warning: Radius Out of Range");
+          return;
+        }
+
+      }
+
+    #endif
+
+    if (pattern == 2) {
+      if (!(cleans & (_BV(X_AXIS) | _BV(Y_AXIS)))) {
+        SERIAL_ECHOLNPGM("Warning: Clean Circle requires XY");
+        return;
+      }
+    }
+    else {
+      if (!TEST(cleans, X_AXIS)) start[arrPos].x = end[arrPos].x = current_position.x;
+      if (!TEST(cleans, Y_AXIS)) start[arrPos].y = end[arrPos].y = current_position.y;
+    }
+    if (!TEST(cleans, Z_AXIS)) start[arrPos].z = end[arrPos].z = current_position.z;
+
+    switch (pattern) {
+       case 1: zigzag(start[arrPos], end[arrPos], strokes, objects); break;
+       case 2: circle(start[arrPos], middle[arrPos], strokes, radius);  break;
+      default: stroke(start[arrPos], end[arrPos], strokes);
+    }
+  }
+
+#endif // NOZZLE_CLEAN_FEATURE
+
+#if ENABLED(NOZZLE_PARK_FEATURE)
+
+  float Nozzle::park_mode_0_height(const_float_t park_z) {
+    // Apply a minimum raise, if specified. Use park.z as a minimum height instead.
+    return _MAX(park_z,                       // Minimum height over 0 based on input
+      _MIN(Z_MAX_POS,                         // Maximum height is fixed
+        #ifdef NOZZLE_PARK_Z_RAISE_MIN
+          NOZZLE_PARK_Z_RAISE_MIN +           // Minimum raise...
+        #endif
+        current_position.z                    // ...over current position
+      )
+    );
+  }
+
+  void Nozzle::park(const uint8_t z_action, const xyz_pos_t &park/*=NOZZLE_PARK_POINT*/) {
+    constexpr feedRate_t fr_xy = NOZZLE_PARK_XY_FEEDRATE, fr_z = NOZZLE_PARK_Z_FEEDRATE;
+
+    switch (z_action) {
+      case 1: // Go to Z-park height
+        do_blocking_move_to_z(park.z, fr_z);
+        break;
+
+      case 2: // Raise by Z-park height
+        do_blocking_move_to_z(_MIN(current_position.z + park.z, Z_MAX_POS), fr_z);
+        break;
+
+      default: // Raise by NOZZLE_PARK_Z_RAISE_MIN, use park.z as a minimum height
+        do_blocking_move_to_z(park_mode_0_height(park.z), fr_z);
+        break;
+    }
+
+    #ifndef NOZZLE_PARK_MOVE
+      #define NOZZLE_PARK_MOVE 0
+    #endif
+    switch (NOZZLE_PARK_MOVE) {
+      case 0: do_blocking_move_to_xy(park, fr_xy); break;
+      case 1: do_blocking_move_to_x(park.x, fr_xy); break;
+      case 2: do_blocking_move_to_y(park.y, fr_xy); break;
+      case 3: do_blocking_move_to_x(park.x, fr_xy);
+              do_blocking_move_to_y(park.y, fr_xy); break;
+      case 4: do_blocking_move_to_y(park.y, fr_xy);
+              do_blocking_move_to_x(park.x, fr_xy); break;
+    }
+
+    report_current_position();
+  }
+
+#endif // NOZZLE_PARK_FEATURE
+
+#endif // NOZZLE_CLEAN_FEATURE || NOZZLE_PARK_FEATURE

src/libs/nozzle.h

@@ -0,0 +1,92 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "../inc/MarlinConfig.h"
+
+/**
+ * @brief Nozzle class
+ *
+ * @todo: Do not ignore the end.z value and allow XYZ movements
+ */
+class Nozzle {
+  private:
+
+  #if ENABLED(NOZZLE_CLEAN_FEATURE)
+
+    /**
+     * @brief Stroke clean pattern
+     * @details Wipes the nozzle back and forth in a linear movement
+     *
+     * @param start xyz_pos_t defining the starting point
+     * @param end xyz_pos_t defining the ending point
+     * @param strokes number of strokes to execute
+     */
+    static void stroke(const xyz_pos_t &start, const xyz_pos_t &end, const uint8_t &strokes) __Os;
+
+    /**
+     * @brief Zig-zag clean pattern
+     * @details Apply a zig-zag cleaning pattern
+     *
+     * @param start xyz_pos_t defining the starting point
+     * @param end xyz_pos_t defining the ending point
+     * @param strokes number of strokes to execute
+     * @param objects number of objects to create
+     */
+    static void zigzag(const xyz_pos_t &start, const xyz_pos_t &end, const uint8_t &strokes, const uint8_t &objects) __Os;
+
+    /**
+     * @brief Circular clean pattern
+     * @details Apply a circular cleaning pattern
+     *
+     * @param start xyz_pos_t defining the middle of circle
+     * @param strokes number of strokes to execute
+     * @param radius radius of circle
+     */
+    static void circle(const xyz_pos_t &start, const xyz_pos_t &middle, const uint8_t &strokes, const_float_t radius) __Os;
+
+  #endif // NOZZLE_CLEAN_FEATURE
+
+  public:
+
+  #if ENABLED(NOZZLE_CLEAN_FEATURE)
+
+    /**
+     * @brief Clean the nozzle
+     * @details Starts the selected clean procedure pattern
+     *
+     * @param pattern one of the available patterns
+     * @param argument depends on the cleaning pattern
+     */
+    static void clean(const uint8_t &pattern, const uint8_t &strokes, const_float_t radius, const uint8_t &objects, const uint8_t cleans) __Os;
+
+  #endif // NOZZLE_CLEAN_FEATURE
+
+  #if ENABLED(NOZZLE_PARK_FEATURE)
+
+    static float park_mode_0_height(const_float_t park_z) __Os;
+    static void park(const uint8_t z_action, const xyz_pos_t &park=NOZZLE_PARK_POINT) __Os;
+
+  #endif
+};
+
+extern Nozzle nozzle;

src/libs/numtostr.cpp

@@ -0,0 +1,417 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "numtostr.h"
+
+#include "../inc/MarlinConfigPre.h"
+#include "../core/utility.h"
+
+char conv[8] = { 0 };
+
+#define DIGIT(n) ('0' + (n))
+#define DIGIMOD(n, f) DIGIT((n)/(f) % 10)
+#define RJDIGIT(n, f) ((n) >= (f) ? DIGIMOD(n, f) : ' ')
+#define MINUSOR(n, alt) (n >= 0 ? (alt) : (n = -n, '-'))
+#define INTFLOAT(V,N) (((V) * 10 * pow(10, N) + ((V) < 0 ? -5: 5)) / 10)      // pow10?
+#define UINTFLOAT(V,N) INTFLOAT((V) < 0 ? -(V) : (V), N)
+
+// Format uint8_t (0-100) as rj string with 123% / _12% / __1% format
+const char* pcttostrpctrj(const uint8_t i) {
+  conv[3] = RJDIGIT(i, 100);
+  conv[4] = RJDIGIT(i, 10);
+  conv[5] = DIGIMOD(i, 1);
+  conv[6] = '%';
+  return &conv[3];
+}
+
+// Convert uint8_t (0-255) to a percentage, format as above
+const char* ui8tostr4pctrj(const uint8_t i) {
+  return pcttostrpctrj(ui8_to_percent(i));
+}
+
+// Convert unsigned 8bit int to string 123 format
+const char* ui8tostr3rj(const uint8_t i) {
+  conv[4] = RJDIGIT(i, 100);
+  conv[5] = RJDIGIT(i, 10);
+  conv[6] = DIGIMOD(i, 1);
+  return &conv[4];
+}
+
+// Convert uint8_t to string with 12 format
+const char* ui8tostr2(const uint8_t i) {
+  conv[5] = DIGIMOD(i, 10);
+  conv[6] = DIGIMOD(i, 1);
+  return &conv[5];
+}
+
+// Convert signed 8bit int to rj string with 123 or -12 format
+const char* i8tostr3rj(const int8_t x) {
+  int xx = x;
+  conv[4] = MINUSOR(xx, RJDIGIT(xx, 100));
+  conv[5] = RJDIGIT(xx, 10);
+  conv[6] = DIGIMOD(xx, 1);
+  return &conv[4];
+}
+
+#if HAS_PRINT_PROGRESS_PERMYRIAD
+  // Convert unsigned 16-bit permyriad to percent with 100 / 23 / 23.4 / 3.45 format
+  const char* permyriadtostr4(const uint16_t xx) {
+    if (xx >= 10000)
+      return "100";
+    else if (xx >= 1000) {
+      conv[3] = DIGIMOD(xx, 1000);
+      conv[4] = DIGIMOD(xx, 100);
+      conv[5] = '.';
+      conv[6] = DIGIMOD(xx, 10);
+      return &conv[3];
+    }
+    else if (xx % 100 == 0) {
+      conv[4] = ' ';
+      conv[5] = RJDIGIT(xx, 1000);
+      conv[6] = DIGIMOD(xx, 100);
+      return &conv[4];
+    }
+    else {
+      conv[3] = DIGIMOD(xx, 100);
+      conv[4] = '.';
+      conv[5] = DIGIMOD(xx, 10);
+      conv[6] = RJDIGIT(xx, 1);
+      return &conv[3];
+    }
+  }
+#endif
+
+// Convert unsigned 16bit int to string 12345 format
+const char* ui16tostr5rj(const uint16_t xx) {
+  conv[2] = RJDIGIT(xx, 10000);
+  conv[3] = RJDIGIT(xx, 1000);
+  conv[4] = RJDIGIT(xx, 100);
+  conv[5] = RJDIGIT(xx, 10);
+  conv[6] = DIGIMOD(xx, 1);
+  return &conv[2];
+}
+
+// Convert unsigned 16bit int to string 1234 format
+const char* ui16tostr4rj(const uint16_t xx) {
+  conv[3] = RJDIGIT(xx, 1000);
+  conv[4] = RJDIGIT(xx, 100);
+  conv[5] = RJDIGIT(xx, 10);
+  conv[6] = DIGIMOD(xx, 1);
+  return &conv[3];
+}
+
+// Convert unsigned 16bit int to string 123 format
+const char* ui16tostr3rj(const uint16_t xx) {
+  conv[4] = RJDIGIT(xx, 100);
+  conv[5] = RJDIGIT(xx, 10);
+  conv[6] = DIGIMOD(xx, 1);
+  return &conv[4];
+}
+
+// Convert signed 16bit int to rj string with 123 or -12 format
+const char* i16tostr3rj(const int16_t x) {
+  int xx = x;
+  conv[4] = MINUSOR(xx, RJDIGIT(xx, 100));
+  conv[5] = RJDIGIT(xx, 10);
+  conv[6] = DIGIMOD(xx, 1);
+  return &conv[4];
+}
+
+// Convert unsigned 16bit int to lj string with 123 format
+const char* i16tostr3left(const int16_t i) {
+  char *str = &conv[6];
+  *str = DIGIMOD(i, 1);
+  if (i >= 10) {
+    *(--str) = DIGIMOD(i, 10);
+    if (i >= 100)
+      *(--str) = DIGIMOD(i, 100);
+  }
+  return str;
+}
+
+// Convert signed 16bit int to rj string with 1234, _123, -123, _-12, or __-1 format
+const char* i16tostr4signrj(const int16_t i) {
+  const bool neg = i < 0;
+  const int ii = neg ? -i : i;
+  if (i >= 1000) {
+    conv[3] = DIGIMOD(ii, 1000);
+    conv[4] = DIGIMOD(ii, 100);
+    conv[5] = DIGIMOD(ii, 10);
+  }
+  else if (ii >= 100) {
+    conv[3] = neg ? '-' : ' ';
+    conv[4] = DIGIMOD(ii, 100);
+    conv[5] = DIGIMOD(ii, 10);
+  }
+  else {
+    conv[3] = ' ';
+    conv[4] = ' ';
+    if (ii >= 10) {
+      conv[4] = neg ? '-' : ' ';
+      conv[5] = DIGIMOD(ii, 10);
+    }
+    else {
+      conv[5] = neg ? '-' : ' ';
+    }
+  }
+  conv[6] = DIGIMOD(ii, 1);
+  return &conv[3];
+}
+
+// Convert unsigned float to string with 1.1 format
+const char* ftostr11ns(const_float_t f) {
+  const long i = UINTFLOAT(f, 1);
+  conv[4] = DIGIMOD(i, 10);
+  conv[5] = '.';
+  conv[6] = DIGIMOD(i, 1);
+  return &conv[4];
+}
+
+// Convert unsigned float to string with 1.23 format
+const char* ftostr12ns(const_float_t f) {
+  const long i = UINTFLOAT(f, 2);
+  conv[3] = DIGIMOD(i, 100);
+  conv[4] = '.';
+  conv[5] = DIGIMOD(i, 10);
+  conv[6] = DIGIMOD(i, 1);
+  return &conv[3];
+}
+
+// Convert unsigned float to string with 12.3 format
+const char* ftostr31ns(const_float_t f) {
+  const long i = UINTFLOAT(f, 1);
+  conv[3] = DIGIMOD(i, 100);
+  conv[4] = DIGIMOD(i, 10);
+  conv[5] = '.';
+  conv[6] = DIGIMOD(i, 1);
+  return &conv[3];
+}
+
+// Convert unsigned float to string with 123.4 format
+const char* ftostr41ns(const_float_t f) {
+  const long i = UINTFLOAT(f, 1);
+  conv[2] = DIGIMOD(i, 1000);
+  conv[3] = DIGIMOD(i, 100);
+  conv[4] = DIGIMOD(i, 10);
+  conv[5] = '.';
+  conv[6] = DIGIMOD(i, 1);
+  return &conv[2];
+}
+
+// Convert signed float to fixed-length string with 12.34 / _2.34 / -2.34 or -23.45 / 123.45 format
+const char* ftostr42_52(const_float_t f) {
+  if (f <= -10 || f >= 100) return ftostr52(f); // -23.45 / 123.45
+  long i = INTFLOAT(f, 2);
+  conv[2] = (f >= 0 && f < 10) ? ' ' : MINUSOR(i, DIGIMOD(i, 1000));
+  conv[3] = DIGIMOD(i, 100);
+  conv[4] = '.';
+  conv[5] = DIGIMOD(i, 10);
+  conv[6] = DIGIMOD(i, 1);
+  return &conv[2];
+}
+
+// Convert signed float to fixed-length string with 023.45 / -23.45 format
+const char* ftostr52(const_float_t f) {
+  long i = INTFLOAT(f, 2);
+  conv[1] = MINUSOR(i, DIGIMOD(i, 10000));
+  conv[2] = DIGIMOD(i, 1000);
+  conv[3] = DIGIMOD(i, 100);
+  conv[4] = '.';
+  conv[5] = DIGIMOD(i, 10);
+  conv[6] = DIGIMOD(i, 1);
+  return &conv[1];
+}
+
+// Convert signed float to fixed-length string with 12.345 / _2.345 / -2.345 or -23.45 / 123.45 format
+const char* ftostr53_63(const_float_t f) {
+  if (f <= -10 || f >= 100) return ftostr63(f); // -23.456 / 123.456
+  long i = INTFLOAT(f, 3);
+  conv[1] = (f >= 0 && f < 10) ? ' ' : MINUSOR(i, DIGIMOD(i, 10000));
+  conv[2] = DIGIMOD(i, 1000);
+  conv[3] = '.';
+  conv[4] = DIGIMOD(i, 100);
+  conv[5] = DIGIMOD(i, 10);
+  conv[6] = DIGIMOD(i, 1);
+  return &conv[1];
+}
+
+// Convert signed float to fixed-length string with 023.456 / -23.456 format
+const char* ftostr63(const_float_t f) {
+  long i = INTFLOAT(f, 3);
+  conv[0] = MINUSOR(i, DIGIMOD(i, 100000));
+  conv[1] = DIGIMOD(i, 10000);
+  conv[2] = DIGIMOD(i, 1000);
+  conv[3] = '.';
+  conv[4] = DIGIMOD(i, 100);
+  conv[5] = DIGIMOD(i, 10);
+  conv[6] = DIGIMOD(i, 1);
+  return &conv[0];
+}
+
+#if ENABLED(LCD_DECIMAL_SMALL_XY)
+
+  // Convert float to rj string with 1234, _123, -123, _-12, 12.3, _1.2, or -1.2 format
+  const char* ftostr4sign(const_float_t f) {
+    const int i = INTFLOAT(f, 1);
+    if (!WITHIN(i, -99, 999)) return i16tostr4signrj((int)f);
+    const bool neg = i < 0;
+    const int ii = neg ? -i : i;
+    conv[3] = neg ? '-' : (ii >= 100 ? DIGIMOD(ii, 100) : ' ');
+    conv[4] = DIGIMOD(ii, 10);
+    conv[5] = '.';
+    conv[6] = DIGIMOD(ii, 1);
+    return &conv[3];
+  }
+
+#endif
+
+// Convert float to fixed-length string with +12.3 / -12.3 format
+const char* ftostr31sign(const_float_t f) {
+  int i = INTFLOAT(f, 1);
+  conv[2] = MINUSOR(i, '+');
+  conv[3] = DIGIMOD(i, 100);
+  conv[4] = DIGIMOD(i, 10);
+  conv[5] = '.';
+  conv[6] = DIGIMOD(i, 1);
+  return &conv[2];
+}
+
+// Convert float to fixed-length string with +123.4 / -123.4 format
+const char* ftostr41sign(const_float_t f) {
+  int i = INTFLOAT(f, 1);
+  conv[1] = MINUSOR(i, '+');
+  conv[2] = DIGIMOD(i, 1000);
+  conv[3] = DIGIMOD(i, 100);
+  conv[4] = DIGIMOD(i, 10);
+  conv[5] = '.';
+  conv[6] = DIGIMOD(i, 1);
+  return &conv[1];
+}
+
+// Convert signed float to string (6 digit) with -1.234 / _0.000 / +1.234 format
+const char* ftostr43sign(const_float_t f, char plus/*=' '*/) {
+  long i = INTFLOAT(f, 3);
+  conv[1] = i ? MINUSOR(i, plus) : ' ';
+  conv[2] = DIGIMOD(i, 1000);
+  conv[3] = '.';
+  conv[4] = DIGIMOD(i, 100);
+  conv[5] = DIGIMOD(i, 10);
+  conv[6] = DIGIMOD(i, 1);
+  return &conv[1];
+}
+
+// Convert signed float to string (5 digit) with -1.2345 / _0.0000 / +1.2345 format
+const char* ftostr54sign(const_float_t f, char plus/*=' '*/) {
+  long i = INTFLOAT(f, 4);
+  conv[0] = i ? MINUSOR(i, plus) : ' ';
+  conv[1] = DIGIMOD(i, 10000);
+  conv[2] = '.';
+  conv[3] = DIGIMOD(i, 1000);
+  conv[4] = DIGIMOD(i, 100);
+  conv[5] = DIGIMOD(i, 10);
+  conv[6] = DIGIMOD(i, 1);
+  return &conv[0];
+}
+
+// Convert unsigned float to rj string with 12345 format
+const char* ftostr5rj(const_float_t f) {
+  const long i = UINTFLOAT(f, 0);
+  return ui16tostr5rj(i);
+}
+
+// Convert signed float to string with +1234.5 format
+const char* ftostr51sign(const_float_t f) {
+  long i = INTFLOAT(f, 1);
+  conv[0] = MINUSOR(i, '+');
+  conv[1] = DIGIMOD(i, 10000);
+  conv[2] = DIGIMOD(i, 1000);
+  conv[3] = DIGIMOD(i, 100);
+  conv[4] = DIGIMOD(i, 10);
+  conv[5] = '.';
+  conv[6] = DIGIMOD(i, 1);
+  return conv;
+}
+
+// Convert signed float to string with +123.45 format
+const char* ftostr52sign(const_float_t f) {
+  long i = INTFLOAT(f, 2);
+  conv[0] = MINUSOR(i, '+');
+  conv[1] = DIGIMOD(i, 10000);
+  conv[2] = DIGIMOD(i, 1000);
+  conv[3] = DIGIMOD(i, 100);
+  conv[4] = '.';
+  conv[5] = DIGIMOD(i, 10);
+  conv[6] = DIGIMOD(i, 1);
+  return conv;
+}
+
+// Convert signed float to string with +12.345 format
+const char* ftostr53sign(const_float_t f) {
+  long i = INTFLOAT(f, 3);
+  conv[0] = MINUSOR(i, '+');
+  conv[1] = DIGIMOD(i, 10000);
+  conv[2] = DIGIMOD(i, 1000);
+  conv[3] = '.';
+  conv[4] = DIGIMOD(i, 100);
+  conv[5] = DIGIMOD(i, 10);
+  conv[6] = DIGIMOD(i, 1);
+  return conv;
+}
+
+// Convert unsigned float to string with ____5.6, ___45.6, __345.6, _2345.6, 12345.6 format
+const char* ftostr61rj(const_float_t f) {
+  const long i = UINTFLOAT(f, 1);
+  conv[0] = RJDIGIT(i, 100000);
+  conv[1] = RJDIGIT(i, 10000);
+  conv[2] = RJDIGIT(i, 1000);
+  conv[3] = RJDIGIT(i, 100);
+  conv[4] = DIGIMOD(i, 10);
+  conv[5] = '.';
+  conv[6] = DIGIMOD(i, 1);
+  return conv;
+}
+
+// Convert signed float to space-padded string with -_23.4_ format
+const char* ftostr52sp(const_float_t f) {
+  long i = INTFLOAT(f, 2);
+  uint8_t dig;
+  conv[0] = MINUSOR(i, ' ');
+  conv[1] = RJDIGIT(i, 10000);
+  conv[2] = RJDIGIT(i, 1000);
+  conv[3] = DIGIMOD(i, 100);
+
+  if ((dig = i % 10)) {          // second digit after decimal point?
+    conv[4] = '.';
+    conv[5] = DIGIMOD(i, 10);
+    conv[6] = DIGIT(dig);
+  }
+  else {
+    if ((dig = (i / 10) % 10)) { // first digit after decimal point?
+      conv[4] = '.';
+      conv[5] = DIGIT(dig);
+    }
+    else                          // nothing after decimal point
+      conv[4] = conv[5] = ' ';
+    conv[6] = ' ';
+  }
+  return conv;
+}

src/libs/numtostr.h

@@ -0,0 +1,128 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "../inc/MarlinConfigPre.h"
+#include "../core/types.h"
+
+// Format uint8_t (0-100) as rj string with 123% / _12% / __1% format
+const char* pcttostrpctrj(const uint8_t i);
+
+// Convert uint8_t (0-255) to a percentage, format as above
+const char* ui8tostr4pctrj(const uint8_t i);
+
+// Convert uint8_t to string with 12 format
+const char* ui8tostr2(const uint8_t x);
+
+// Convert uint8_t to string with 123 format
+const char* ui8tostr3rj(const uint8_t i);
+
+// Convert int8_t to string with 123 format
+const char* i8tostr3rj(const int8_t x);
+
+#if HAS_PRINT_PROGRESS_PERMYRIAD
+  // Convert 16-bit unsigned permyriad value to percent: 100 / 23 / 23.4 / 3.45
+  const char* permyriadtostr4(const uint16_t xx);
+#endif
+
+// Convert uint16_t to string with 12345 format
+const char* ui16tostr5rj(const uint16_t x);
+
+// Convert uint16_t to string with 1234 format
+const char* ui16tostr4rj(const uint16_t x);
+
+// Convert uint16_t to string with 123 format
+const char* ui16tostr3rj(const uint16_t x);
+
+// Convert int16_t to string with 123 format
+const char* i16tostr3rj(const int16_t x);
+
+// Convert unsigned int to lj string with 123 format
+const char* i16tostr3left(const int16_t xx);
+
+// Convert signed int to rj string with _123, -123, _-12, or __-1 format
+const char* i16tostr4signrj(const int16_t x);
+
+// Convert unsigned float to string with 1.2 format
+const char* ftostr11ns(const_float_t x);
+
+// Convert unsigned float to string with 1.23 format
+const char* ftostr12ns(const_float_t x);
+
+// Convert unsigned float to string with 12.3 format
+const char* ftostr31ns(const_float_t x);
+
+// Convert unsigned float to string with 123.4 format
+const char* ftostr41ns(const_float_t x);
+
+// Convert signed float to fixed-length string with 12.34 / _2.34 / -2.34 or -23.45 / 123.45 format
+const char* ftostr42_52(const_float_t x);
+
+// Convert signed float to fixed-length string with 023.45 / -23.45 format
+const char* ftostr52(const_float_t x);
+
+// Convert signed float to fixed-length string with 12.345 / -2.345 or 023.456 / -23.456 format
+const char* ftostr53_63(const_float_t x);
+
+// Convert signed float to fixed-length string with 023.456 / -23.456 format
+const char* ftostr63(const_float_t x);
+
+// Convert float to fixed-length string with +12.3 / -12.3 format
+const char* ftostr31sign(const_float_t x);
+
+// Convert float to fixed-length string with +123.4 / -123.4 format
+const char* ftostr41sign(const_float_t x);
+
+// Convert signed float to string (6 digit) with -1.234 / _0.000 / +1.234 format
+const char* ftostr43sign(const_float_t x, char plus=' ');
+
+// Convert signed float to string (5 digit) with -1.2345 / _0.0000 / +1.2345 format
+const char* ftostr54sign(const_float_t x, char plus=' ');
+
+// Convert unsigned float to rj string with 12345 format
+const char* ftostr5rj(const_float_t x);
+
+// Convert signed float to string with +1234.5 format
+const char* ftostr51sign(const_float_t x);
+
+// Convert signed float to space-padded string with -_23.4_ format
+const char* ftostr52sp(const_float_t x);
+
+// Convert signed float to string with +123.45 format
+const char* ftostr52sign(const_float_t x);
+
+// Convert signed float to string with +12.345 format
+const char* ftostr53sign(const_float_t f);
+
+// Convert unsigned float to string with 12345.6 format omitting trailing zeros
+const char* ftostr61rj(const_float_t x);
+
+// Convert float to rj string with 123 or -12 format
+FORCE_INLINE const char* ftostr3(const_float_t x) { return i16tostr3rj(int16_t(x + (x < 0 ? -0.5f : 0.5f))); }
+
+#if ENABLED(LCD_DECIMAL_SMALL_XY)
+  // Convert float to rj string with 1234, _123, 12.3, _1.2, -123, _-12, or -1.2 format
+  const char* ftostr4sign(const_float_t fx);
+#else
+  // Convert float to rj string with 1234, _123, -123, __12, _-12, ___1, or __-1 format
+  FORCE_INLINE const char* ftostr4sign(const_float_t x) { return i16tostr4signrj(int16_t(x + (x < 0 ? -0.5f : 0.5f))); }
+#endif

src/libs/private_spi.h

@@ -0,0 +1,54 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "softspi.h"
+#include <stdint.h>
+
+template<uint8_t MisoPin, uint8_t MosiPin, uint8_t SckPin>
+class SPIclass {
+  static SoftSPI<MisoPin, MosiPin, SckPin> softSPI;
+  public:
+    FORCE_INLINE static void init() { softSPI.begin(); }
+    FORCE_INLINE static void send(uint8_t data) { softSPI.send(data); }
+    FORCE_INLINE static uint8_t receive() { return softSPI.receive(); }
+};
+
+// Hardware SPI
+template<>
+class SPIclass<SD_MISO_PIN, SD_MOSI_PIN, SD_SCK_PIN> {
+  public:
+    FORCE_INLINE static void init() {
+      OUT_WRITE(SD_SCK_PIN, LOW);
+      OUT_WRITE(SD_MOSI_PIN, HIGH);
+      SET_INPUT_PULLUP(SD_MISO_PIN);
+    }
+    FORCE_INLINE static uint8_t receive() {
+      #if defined(__AVR__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) || defined(__IMXRT1062__)
+        SPDR = 0;
+        for (;!TEST(SPSR, SPIF););
+        return SPDR;
+      #else
+        return spiRec();
+      #endif
+    }
+};

src/libs/softspi.h

@@ -0,0 +1,742 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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
+
+//
+// Based on https://github.com/niteris/ArduinoSoftSpi
+//
+
+#include "../HAL/shared/Marduino.h" // CORE_TEENSY
+
+#define nop __asm__ volatile ("nop") // NOP for timing
+
+#ifdef __arm__
+
+  #ifdef CORE_TEENSY
+
+    /**
+     * Read pin value
+     * @param[in] pin Arduino pin number
+     * @return value read
+     */
+    FORCE_INLINE static bool fastDigitalRead(uint8_t pin) {
+      return *portInputRegister(pin);
+    }
+
+    /**
+     * Set pin value
+     * @param[in] pin Arduino pin number
+     * @param[in] level value to write
+     */
+    FORCE_INLINE static void fastDigitalWrite(uint8_t pin, bool value) {
+      if (value)
+        *portSetRegister(pin) = 1;
+      else
+        *portClearRegister(pin) = 1;
+    }
+
+  #else // !CORE_TEENSY
+
+    /**
+     * Read pin value
+     * @param[in] pin Arduino pin number
+     * @return value read
+     */
+    FORCE_INLINE static bool fastDigitalRead(uint8_t pin) {
+      return digitalRead(pin);
+    }
+
+    /**
+     * Set pin value
+     * @param[in] pin Arduino pin number
+     * @param[in] level value to write
+     */
+    FORCE_INLINE static void fastDigitalWrite(uint8_t pin, bool value) {
+      digitalWrite(pin, value);
+    }
+
+  #endif // !CORE_TEENSY
+
+  inline void fastDigitalToggle(uint8_t pin) { fastDigitalWrite(pin, !fastDigitalRead(pin)); }
+
+  inline void fastPinMode(uint8_t pin, bool mode) { pinMode(pin, mode); }
+
+#else  // !__arm__
+
+  #include <avr/io.h>
+  #include <util/atomic.h>
+
+  /**
+   * @class pin_map_t
+   * @brief struct for mapping digital pins
+   */
+  struct pin_map_t {
+    volatile uint8_t* ddr;   /**< address of DDR for this pin */
+    volatile uint8_t* pin;   /**< address of PIN for this pin */
+    volatile uint8_t* port;  /**< address of PORT for this pin */
+    uint8_t bit;             /**< bit number for this pin */
+  };
+
+  #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega168P__) || defined(__AVR_ATmega328P__)
+
+    // 168 and 328 Arduinos
+    const static pin_map_t pinMap[] = {
+      {&DDRD, &PIND, &PORTD, 0},  // D0  0
+      {&DDRD, &PIND, &PORTD, 1},  // D1  1
+      {&DDRD, &PIND, &PORTD, 2},  // D2  2
+      {&DDRD, &PIND, &PORTD, 3},  // D3  3
+      {&DDRD, &PIND, &PORTD, 4},  // D4  4
+      {&DDRD, &PIND, &PORTD, 5},  // D5  5
+      {&DDRD, &PIND, &PORTD, 6},  // D6  6
+      {&DDRD, &PIND, &PORTD, 7},  // D7  7
+      {&DDRB, &PINB, &PORTB, 0},  // B0  8
+      {&DDRB, &PINB, &PORTB, 1},  // B1  9
+      {&DDRB, &PINB, &PORTB, 2},  // B2 10
+      {&DDRB, &PINB, &PORTB, 3},  // B3 11
+      {&DDRB, &PINB, &PORTB, 4},  // B4 12
+      {&DDRB, &PINB, &PORTB, 5},  // B5 13
+      {&DDRC, &PINC, &PORTC, 0},  // C0 14
+      {&DDRC, &PINC, &PORTC, 1},  // C1 15
+      {&DDRC, &PINC, &PORTC, 2},  // C2 16
+      {&DDRC, &PINC, &PORTC, 3},  // C3 17
+      {&DDRC, &PINC, &PORTC, 4},  // C4 18
+      {&DDRC, &PINC, &PORTC, 5}   // C5 19
+    };
+
+  #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+
+    // Mega
+    static const pin_map_t pinMap[] = {
+      {&DDRE, &PINE, &PORTE, 0},  // E0  0
+      {&DDRE, &PINE, &PORTE, 1},  // E1  1
+      {&DDRE, &PINE, &PORTE, 4},  // E4  2
+      {&DDRE, &PINE, &PORTE, 5},  // E5  3
+      {&DDRG, &PING, &PORTG, 5},  // G5  4
+      {&DDRE, &PINE, &PORTE, 3},  // E3  5
+      {&DDRH, &PINH, &PORTH, 3},  // H3  6
+      {&DDRH, &PINH, &PORTH, 4},  // H4  7
+      {&DDRH, &PINH, &PORTH, 5},  // H5  8
+      {&DDRH, &PINH, &PORTH, 6},  // H6  9
+      {&DDRB, &PINB, &PORTB, 4},  // B4 10
+      {&DDRB, &PINB, &PORTB, 5},  // B5 11
+      {&DDRB, &PINB, &PORTB, 6},  // B6 12
+      {&DDRB, &PINB, &PORTB, 7},  // B7 13
+      {&DDRJ, &PINJ, &PORTJ, 1},  // J1 14
+      {&DDRJ, &PINJ, &PORTJ, 0},  // J0 15
+      {&DDRH, &PINH, &PORTH, 1},  // H1 16
+      {&DDRH, &PINH, &PORTH, 0},  // H0 17
+      {&DDRD, &PIND, &PORTD, 3},  // D3 18
+      {&DDRD, &PIND, &PORTD, 2},  // D2 19
+      {&DDRD, &PIND, &PORTD, 1},  // D1 20
+      {&DDRD, &PIND, &PORTD, 0},  // D0 21
+      {&DDRA, &PINA, &PORTA, 0},  // A0 22
+      {&DDRA, &PINA, &PORTA, 1},  // A1 23
+      {&DDRA, &PINA, &PORTA, 2},  // A2 24
+      {&DDRA, &PINA, &PORTA, 3},  // A3 25
+      {&DDRA, &PINA, &PORTA, 4},  // A4 26
+      {&DDRA, &PINA, &PORTA, 5},  // A5 27
+      {&DDRA, &PINA, &PORTA, 6},  // A6 28
+      {&DDRA, &PINA, &PORTA, 7},  // A7 29
+      {&DDRC, &PINC, &PORTC, 7},  // C7 30
+      {&DDRC, &PINC, &PORTC, 6},  // C6 31
+      {&DDRC, &PINC, &PORTC, 5},  // C5 32
+      {&DDRC, &PINC, &PORTC, 4},  // C4 33
+      {&DDRC, &PINC, &PORTC, 3},  // C3 34
+      {&DDRC, &PINC, &PORTC, 2},  // C2 35
+      {&DDRC, &PINC, &PORTC, 1},  // C1 36
+      {&DDRC, &PINC, &PORTC, 0},  // C0 37
+      {&DDRD, &PIND, &PORTD, 7},  // D7 38
+      {&DDRG, &PING, &PORTG, 2},  // G2 39
+      {&DDRG, &PING, &PORTG, 1},  // G1 40
+      {&DDRG, &PING, &PORTG, 0},  // G0 41
+      {&DDRL, &PINL, &PORTL, 7},  // L7 42
+      {&DDRL, &PINL, &PORTL, 6},  // L6 43
+      {&DDRL, &PINL, &PORTL, 5},  // L5 44
+      {&DDRL, &PINL, &PORTL, 4},  // L4 45
+      {&DDRL, &PINL, &PORTL, 3},  // L3 46
+      {&DDRL, &PINL, &PORTL, 2},  // L2 47
+      {&DDRL, &PINL, &PORTL, 1},  // L1 48
+      {&DDRL, &PINL, &PORTL, 0},  // L0 49
+      {&DDRB, &PINB, &PORTB, 3},  // B3 50
+      {&DDRB, &PINB, &PORTB, 2},  // B2 51
+      {&DDRB, &PINB, &PORTB, 1},  // B1 52
+      {&DDRB, &PINB, &PORTB, 0},  // B0 53
+      {&DDRF, &PINF, &PORTF, 0},  // F0 54
+      {&DDRF, &PINF, &PORTF, 1},  // F1 55
+      {&DDRF, &PINF, &PORTF, 2},  // F2 56
+      {&DDRF, &PINF, &PORTF, 3},  // F3 57
+      {&DDRF, &PINF, &PORTF, 4},  // F4 58
+      {&DDRF, &PINF, &PORTF, 5},  // F5 59
+      {&DDRF, &PINF, &PORTF, 6},  // F6 60
+      {&DDRF, &PINF, &PORTF, 7},  // F7 61
+      {&DDRK, &PINK, &PORTK, 0},  // K0 62
+      {&DDRK, &PINK, &PORTK, 1},  // K1 63
+      {&DDRK, &PINK, &PORTK, 2},  // K2 64
+      {&DDRK, &PINK, &PORTK, 3},  // K3 65
+      {&DDRK, &PINK, &PORTK, 4},  // K4 66
+      {&DDRK, &PINK, &PORTK, 5},  // K5 67
+      {&DDRK, &PINK, &PORTK, 6},  // K6 68
+      {&DDRK, &PINK, &PORTK, 7},  // K7 69
+
+      // pins_MIGHTYBOARD_REVE.h
+      {&DDRG, &PING, &PORTG, 4},  // G4 70
+      {&DDRG, &PING, &PORTG, 3},  // G3 71
+      {&DDRJ, &PINJ, &PORTJ, 2},  // J2 72
+      {&DDRJ, &PINJ, &PORTJ, 3},  // J3 73
+      {&DDRJ, &PINJ, &PORTJ, 7},  // J7 74
+      {&DDRJ, &PINJ, &PORTJ, 4},  // J4 75
+      {&DDRJ, &PINJ, &PORTJ, 5},  // J5 76
+      {&DDRJ, &PINJ, &PORTJ, 6},  // J6 77
+      {&DDRE, &PINE, &PORTE, 2},  // E2 78
+      {&DDRE, &PINE, &PORTE, 6}   // E6 79
+    };
+
+  #elif defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega1284__) \
+  || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) \
+  || defined(__AVR_ATmega64__) || defined(__AVR_ATmega32__) \
+  || defined(__AVR_ATmega324__) || defined(__AVR_ATmega16__)
+
+    #ifdef VARIANT_MIGHTY
+
+      // Mighty Layout
+      static const pin_map_t pinMap[] = {
+        {&DDRB, &PINB, &PORTB, 0},  // B0  0
+        {&DDRB, &PINB, &PORTB, 1},  // B1  1
+        {&DDRB, &PINB, &PORTB, 2},  // B2  2
+        {&DDRB, &PINB, &PORTB, 3},  // B3  3
+        {&DDRB, &PINB, &PORTB, 4},  // B4  4
+        {&DDRB, &PINB, &PORTB, 5},  // B5  5
+        {&DDRB, &PINB, &PORTB, 6},  // B6  6
+        {&DDRB, &PINB, &PORTB, 7},  // B7  7
+        {&DDRD, &PIND, &PORTD, 0},  // D0  8
+        {&DDRD, &PIND, &PORTD, 1},  // D1  9
+        {&DDRD, &PIND, &PORTD, 2},  // D2 10
+        {&DDRD, &PIND, &PORTD, 3},  // D3 11
+        {&DDRD, &PIND, &PORTD, 4},  // D4 12
+        {&DDRD, &PIND, &PORTD, 5},  // D5 13
+        {&DDRD, &PIND, &PORTD, 6},  // D6 14
+        {&DDRD, &PIND, &PORTD, 7},  // D7 15
+        {&DDRC, &PINC, &PORTC, 0},  // C0 16
+        {&DDRC, &PINC, &PORTC, 1},  // C1 17
+        {&DDRC, &PINC, &PORTC, 2},  // C2 18
+        {&DDRC, &PINC, &PORTC, 3},  // C3 19
+        {&DDRC, &PINC, &PORTC, 4},  // C4 20
+        {&DDRC, &PINC, &PORTC, 5},  // C5 21
+        {&DDRC, &PINC, &PORTC, 6},  // C6 22
+        {&DDRC, &PINC, &PORTC, 7},  // C7 23
+        {&DDRA, &PINA, &PORTA, 0},  // A0 24
+        {&DDRA, &PINA, &PORTA, 1},  // A1 25
+        {&DDRA, &PINA, &PORTA, 2},  // A2 26
+        {&DDRA, &PINA, &PORTA, 3},  // A3 27
+        {&DDRA, &PINA, &PORTA, 4},  // A4 28
+        {&DDRA, &PINA, &PORTA, 5},  // A5 29
+        {&DDRA, &PINA, &PORTA, 6},  // A6 30
+        {&DDRA, &PINA, &PORTA, 7}   // A7 31
+      };
+
+    #elif defined(VARIANT_BOBUINO)
+
+      // Bobuino Layout
+      static const pin_map_t pinMap[] = {
+        {&DDRD, &PIND, &PORTD, 0},  // D0  0
+        {&DDRD, &PIND, &PORTD, 1},  // D1  1
+        {&DDRD, &PIND, &PORTD, 2},  // D2  2
+        {&DDRD, &PIND, &PORTD, 3},  // D3  3
+        {&DDRB, &PINB, &PORTB, 0},  // B0  4
+        {&DDRB, &PINB, &PORTB, 1},  // B1  5
+        {&DDRB, &PINB, &PORTB, 2},  // B2  6
+        {&DDRB, &PINB, &PORTB, 3},  // B3  7
+        {&DDRD, &PIND, &PORTD, 5},  // D5  8
+        {&DDRD, &PIND, &PORTD, 6},  // D6  9
+        {&DDRB, &PINB, &PORTB, 4},  // B4 10
+        {&DDRB, &PINB, &PORTB, 5},  // B5 11
+        {&DDRB, &PINB, &PORTB, 6},  // B6 12
+        {&DDRB, &PINB, &PORTB, 7},  // B7 13
+        {&DDRA, &PINA, &PORTA, 7},  // A7 14
+        {&DDRA, &PINA, &PORTA, 6},  // A6 15
+        {&DDRA, &PINA, &PORTA, 5},  // A5 16
+        {&DDRA, &PINA, &PORTA, 4},  // A4 17
+        {&DDRA, &PINA, &PORTA, 3},  // A3 18
+        {&DDRA, &PINA, &PORTA, 2},  // A2 19
+        {&DDRA, &PINA, &PORTA, 1},  // A1 20
+        {&DDRA, &PINA, &PORTA, 0},  // A0 21
+        {&DDRC, &PINC, &PORTC, 0},  // C0 22
+        {&DDRC, &PINC, &PORTC, 1},  // C1 23
+        {&DDRC, &PINC, &PORTC, 2},  // C2 24
+        {&DDRC, &PINC, &PORTC, 3},  // C3 25
+        {&DDRC, &PINC, &PORTC, 4},  // C4 26
+        {&DDRC, &PINC, &PORTC, 5},  // C5 27
+        {&DDRC, &PINC, &PORTC, 6},  // C6 28
+        {&DDRC, &PINC, &PORTC, 7},  // C7 29
+        {&DDRD, &PIND, &PORTD, 4},  // D4 30
+        {&DDRD, &PIND, &PORTD, 7}   // D7 31
+      };
+
+    #elif defined(VARIANT_STANDARD)
+
+      // Standard Layout
+      static const pin_map_t pinMap[] = {
+        {&DDRB, &PINB, &PORTB, 0},  // B0  0
+        {&DDRB, &PINB, &PORTB, 1},  // B1  1
+        {&DDRB, &PINB, &PORTB, 2},  // B2  2
+        {&DDRB, &PINB, &PORTB, 3},  // B3  3
+        {&DDRB, &PINB, &PORTB, 4},  // B4  4
+        {&DDRB, &PINB, &PORTB, 5},  // B5  5
+        {&DDRB, &PINB, &PORTB, 6},  // B6  6
+        {&DDRB, &PINB, &PORTB, 7},  // B7  7
+        {&DDRD, &PIND, &PORTD, 0},  // D0  8
+        {&DDRD, &PIND, &PORTD, 1},  // D1  9
+        {&DDRD, &PIND, &PORTD, 2},  // D2 10
+        {&DDRD, &PIND, &PORTD, 3},  // D3 11
+        {&DDRD, &PIND, &PORTD, 4},  // D4 12
+        {&DDRD, &PIND, &PORTD, 5},  // D5 13
+        {&DDRD, &PIND, &PORTD, 6},  // D6 14
+        {&DDRD, &PIND, &PORTD, 7},  // D7 15
+        {&DDRC, &PINC, &PORTC, 0},  // C0 16
+        {&DDRC, &PINC, &PORTC, 1},  // C1 17
+        {&DDRC, &PINC, &PORTC, 2},  // C2 18
+        {&DDRC, &PINC, &PORTC, 3},  // C3 19
+        {&DDRC, &PINC, &PORTC, 4},  // C4 20
+        {&DDRC, &PINC, &PORTC, 5},  // C5 21
+        {&DDRC, &PINC, &PORTC, 6},  // C6 22
+        {&DDRC, &PINC, &PORTC, 7},  // C7 23
+        {&DDRA, &PINA, &PORTA, 7},  // A7 24
+        {&DDRA, &PINA, &PORTA, 6},  // A6 25
+        {&DDRA, &PINA, &PORTA, 5},  // A5 26
+        {&DDRA, &PINA, &PORTA, 4},  // A4 27
+        {&DDRA, &PINA, &PORTA, 3},  // A3 28
+        {&DDRA, &PINA, &PORTA, 2},  // A2 29
+        {&DDRA, &PINA, &PORTA, 1},  // A1 30
+        {&DDRA, &PINA, &PORTA, 0}   // A0 31
+      };
+
+    #else // !(VARIANT_MIGHTY || VARIANT_BOBUINO || VARIANT_STANDARD)
+
+      #error Undefined variant 1284, 644, 324, 64, 32
+
+    #endif
+
+  #elif defined(__AVR_ATmega32U4__)
+
+    #ifdef CORE_TEENSY
+
+      // Teensy 2.0
+      static const pin_map_t pinMap[] = {
+        {&DDRB, &PINB, &PORTB, 0},  // B0  0
+        {&DDRB, &PINB, &PORTB, 1},  // B1  1
+        {&DDRB, &PINB, &PORTB, 2},  // B2  2
+        {&DDRB, &PINB, &PORTB, 3},  // B3  3
+        {&DDRB, &PINB, &PORTB, 7},  // B7  4
+        {&DDRD, &PIND, &PORTD, 0},  // D0  5
+        {&DDRD, &PIND, &PORTD, 1},  // D1  6
+        {&DDRD, &PIND, &PORTD, 2},  // D2  7
+        {&DDRD, &PIND, &PORTD, 3},  // D3  8
+        {&DDRC, &PINC, &PORTC, 6},  // C6  9
+        {&DDRC, &PINC, &PORTC, 7},  // C7 10
+        {&DDRD, &PIND, &PORTD, 6},  // D6 11
+        {&DDRD, &PIND, &PORTD, 7},  // D7 12
+        {&DDRB, &PINB, &PORTB, 4},  // B4 13
+        {&DDRB, &PINB, &PORTB, 5},  // B5 14
+        {&DDRB, &PINB, &PORTB, 6},  // B6 15
+        {&DDRF, &PINF, &PORTF, 7},  // F7 16
+        {&DDRF, &PINF, &PORTF, 6},  // F6 17
+        {&DDRF, &PINF, &PORTF, 5},  // F5 18
+        {&DDRF, &PINF, &PORTF, 4},  // F4 19
+        {&DDRF, &PINF, &PORTF, 1},  // F1 20
+        {&DDRF, &PINF, &PORTF, 0},  // F0 21
+        {&DDRD, &PIND, &PORTD, 4},  // D4 22
+        {&DDRD, &PIND, &PORTD, 5},  // D5 23
+        {&DDRE, &PINE, &PORTE, 6}   // E6 24
+      };
+
+    #else // !CORE_TEENSY
+
+      // Leonardo
+      static const pin_map_t pinMap[] = {
+        {&DDRD, &PIND, &PORTD, 2},  // D2  0
+        {&DDRD, &PIND, &PORTD, 3},  // D3  1
+        {&DDRD, &PIND, &PORTD, 1},  // D1  2
+        {&DDRD, &PIND, &PORTD, 0},  // D0  3
+        {&DDRD, &PIND, &PORTD, 4},  // D4  4
+        {&DDRC, &PINC, &PORTC, 6},  // C6  5
+        {&DDRD, &PIND, &PORTD, 7},  // D7  6
+        {&DDRE, &PINE, &PORTE, 6},  // E6  7
+        {&DDRB, &PINB, &PORTB, 4},  // B4  8
+        {&DDRB, &PINB, &PORTB, 5},  // B5  9
+        {&DDRB, &PINB, &PORTB, 6},  // B6 10
+        {&DDRB, &PINB, &PORTB, 7},  // B7 11
+        {&DDRD, &PIND, &PORTD, 6},  // D6 12
+        {&DDRC, &PINC, &PORTC, 7},  // C7 13
+        {&DDRB, &PINB, &PORTB, 3},  // B3 14
+        {&DDRB, &PINB, &PORTB, 1},  // B1 15
+        {&DDRB, &PINB, &PORTB, 2},  // B2 16
+        {&DDRB, &PINB, &PORTB, 0},  // B0 17
+        {&DDRF, &PINF, &PORTF, 7},  // F7 18
+        {&DDRF, &PINF, &PORTF, 6},  // F6 19
+        {&DDRF, &PINF, &PORTF, 5},  // F5 20
+        {&DDRF, &PINF, &PORTF, 4},  // F4 21
+        {&DDRF, &PINF, &PORTF, 1},  // F1 22
+        {&DDRF, &PINF, &PORTF, 0},  // F0 23
+        {&DDRD, &PIND, &PORTD, 4},  // D4 24
+        {&DDRD, &PIND, &PORTD, 7},  // D7 25
+        {&DDRB, &PINB, &PORTB, 4},  // B4 26
+        {&DDRB, &PINB, &PORTB, 5},  // B5 27
+        {&DDRB, &PINB, &PORTB, 6},  // B6 28
+        {&DDRD, &PIND, &PORTD, 6}   // D6 29
+      };
+
+    #endif // !CORE_TEENSY
+
+  #elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
+
+    // Teensy++ 1.0 & 2.0
+    static const pin_map_t pinMap[] = {
+      {&DDRD, &PIND, &PORTD, 0},  // D0  0
+      {&DDRD, &PIND, &PORTD, 1},  // D1  1
+      {&DDRD, &PIND, &PORTD, 2},  // D2  2
+      {&DDRD, &PIND, &PORTD, 3},  // D3  3
+      {&DDRD, &PIND, &PORTD, 4},  // D4  4
+      {&DDRD, &PIND, &PORTD, 5},  // D5  5
+      {&DDRD, &PIND, &PORTD, 6},  // D6  6
+      {&DDRD, &PIND, &PORTD, 7},  // D7  7
+      {&DDRE, &PINE, &PORTE, 0},  // E0  8
+      {&DDRE, &PINE, &PORTE, 1},  // E1  9
+      {&DDRC, &PINC, &PORTC, 0},  // C0 10
+      {&DDRC, &PINC, &PORTC, 1},  // C1 11
+      {&DDRC, &PINC, &PORTC, 2},  // C2 12
+      {&DDRC, &PINC, &PORTC, 3},  // C3 13
+      {&DDRC, &PINC, &PORTC, 4},  // C4 14
+      {&DDRC, &PINC, &PORTC, 5},  // C5 15
+      {&DDRC, &PINC, &PORTC, 6},  // C6 16
+      {&DDRC, &PINC, &PORTC, 7},  // C7 17
+      {&DDRE, &PINE, &PORTE, 6},  // E6 18
+      {&DDRE, &PINE, &PORTE, 7},  // E7 19
+      {&DDRB, &PINB, &PORTB, 0},  // B0 20
+      {&DDRB, &PINB, &PORTB, 1},  // B1 21
+      {&DDRB, &PINB, &PORTB, 2},  // B2 22
+      {&DDRB, &PINB, &PORTB, 3},  // B3 23
+      {&DDRB, &PINB, &PORTB, 4},  // B4 24
+      {&DDRB, &PINB, &PORTB, 5},  // B5 25
+      {&DDRB, &PINB, &PORTB, 6},  // B6 26
+      {&DDRB, &PINB, &PORTB, 7},  // B7 27
+      {&DDRA, &PINA, &PORTA, 0},  // A0 28
+      {&DDRA, &PINA, &PORTA, 1},  // A1 29
+      {&DDRA, &PINA, &PORTA, 2},  // A2 30
+      {&DDRA, &PINA, &PORTA, 3},  // A3 31
+      {&DDRA, &PINA, &PORTA, 4},  // A4 32
+      {&DDRA, &PINA, &PORTA, 5},  // A5 33
+      {&DDRA, &PINA, &PORTA, 6},  // A6 34
+      {&DDRA, &PINA, &PORTA, 7},  // A7 35
+      {&DDRE, &PINE, &PORTE, 4},  // E4 36
+      {&DDRE, &PINE, &PORTE, 5},  // E5 37
+      {&DDRF, &PINF, &PORTF, 0},  // F0 38
+      {&DDRF, &PINF, &PORTF, 1},  // F1 39
+      {&DDRF, &PINF, &PORTF, 2},  // F2 40
+      {&DDRF, &PINF, &PORTF, 3},  // F3 41
+      {&DDRF, &PINF, &PORTF, 4},  // F4 42
+      {&DDRF, &PINF, &PORTF, 5},  // F5 43
+      {&DDRF, &PINF, &PORTF, 6},  // F6 44
+      {&DDRF, &PINF, &PORTF, 7}   // F7 45
+    };
+
+  #else // CPU type
+
+    #error "Unknown CPU type for Software SPI"
+
+  #endif // CPU type
+
+  /** count of pins */
+  static constexpr uint8_t digitalPinCount = sizeof(pinMap) / sizeof(pin_map_t);
+
+  /** generate bad pin number error */
+  void badPinNumber()
+    __attribute__((error("Pin number is too large or not a constant")));
+
+  /**
+   * Check for valid pin number
+   * @param[in] pin Number of pin to be checked.
+   */
+  FORCE_INLINE static void badPinCheck(const uint8_t pin) {
+    if (!__builtin_constant_p(pin) || pin >= digitalPinCount) badPinNumber();
+  }
+
+  /**
+   * Fast write helper
+   * @param[in] address I/O register address
+   * @param[in] bit bit number to write
+   * @param[in] level value for bit
+   */
+  FORCE_INLINE static void fastBitWriteSafe(volatile uint8_t* address, uint8_t bit, bool level) {
+    uint8_t oldSREG;
+    if (address > (uint8_t*)0x5F) { oldSREG = SREG; cli(); }
+    if (level) SBI(*address, bit); else CBI(*address, bit);
+    if (address > (uint8_t*)0x5F) SREG = oldSREG;
+  }
+
+  /**
+   * Read pin value
+   * @param[in] pin Arduino pin number
+   * @return value read
+   */
+  FORCE_INLINE static bool fastDigitalRead(uint8_t pin) {
+    badPinCheck(pin);
+    return (*pinMap[pin].pin >> pinMap[pin].bit) & 1;
+  }
+
+  /**
+   * Toggle a pin
+   * @param[in] pin Arduino pin number
+   *
+   * If the pin is in output mode toggle the pin level.
+   * If the pin is in input mode toggle the state of the 20K pullup.
+   */
+  FORCE_INLINE static void fastDigitalToggle(uint8_t pin) {
+    badPinCheck(pin);
+    if (pinMap[pin].pin > (uint8_t*)0x5F)
+      *pinMap[pin].pin = _BV(pinMap[pin].bit);  // Must write bit to high address port
+    else
+      SBI(*pinMap[pin].pin, pinMap[pin].bit);   // Compiles to sbi and PIN register will not be read
+  }
+
+  /**
+   * Set pin value
+   * @param[in] pin Arduino pin number
+   * @param[in] level value to write
+   */
+  FORCE_INLINE static void fastDigitalWrite(uint8_t pin, bool level) {
+    badPinCheck(pin);
+    fastBitWriteSafe(pinMap[pin].port, pinMap[pin].bit, level);
+  }
+
+  /**
+   * Set pin mode
+   * @param[in] pin Arduino pin number
+   * @param[in] mode if true set output mode else input mode
+   *
+   * fastPinMode does not enable or disable the 20K pullup for input mode.
+   */
+  FORCE_INLINE static void fastPinMode(uint8_t pin, bool mode) {
+    badPinCheck(pin);
+    fastBitWriteSafe(pinMap[pin].ddr, pinMap[pin].bit, mode);
+  }
+
+#endif // !__arm__
+
+/**
+ * Set pin configuration
+ * @param[in] pin Arduino pin number
+ * @param[in] mode If true set output mode else input mode
+ * @param[in] level If mode is output, set level high/low.
+ *                  If mode is input, enable or disable the pin's 20K pullup.
+ */
+FORCE_INLINE static void fastPinConfig(uint8_t pin, bool mode, bool level) {
+  fastPinMode(pin, mode);
+  fastDigitalWrite(pin, level);
+}
+
+/**
+ * @class DigitalPin
+ * @brief Fast digital port I/O
+ */
+template<uint8_t PinNumber>
+class DigitalPin {
+public:
+
+  /** Constructor */
+  DigitalPin() {}
+
+  /**
+   * Constructor
+   * @param[in] pinMode if true set output mode else input mode.
+   */
+  explicit DigitalPin(bool pinMode) { mode(pinMode); }
+
+  /**
+   * Constructor
+   * @param[in] mode If true set output mode else input mode
+   * @param[in] level If mode is output, set level high/low.
+   *                  If mode is input, enable or disable the pin's 20K pullup.
+   */
+  DigitalPin(bool mode, bool level) { config(mode, level); }
+
+  /**
+   * Assignment operator
+   * @param[in] value If true set the pin's level high else set the
+   *  pin's level low.
+   *
+   * @return This DigitalPin instance.
+   */
+  FORCE_INLINE DigitalPin & operator = (bool value) { write(value); return *this; }
+
+  /**
+   * Parentheses operator
+   * @return Pin's level
+   */
+  FORCE_INLINE operator bool () const { return read(); }
+
+  /**
+   * Set pin configuration
+   * @param[in] mode If true set output mode else input mode
+   * @param[in] level If mode is output, set level high/low.
+   *                  If mode is input, enable or disable the pin's 20K pullup.
+   */
+  FORCE_INLINE void config(bool mode, bool level) { fastPinConfig(PinNumber, mode, level); }
+
+  /**
+   * Set pin level high if output mode or enable 20K pullup if input mode.
+   */
+  FORCE_INLINE void high() { write(true); }
+
+  /**
+   * Set pin level low if output mode or disable 20K pullup if input mode.
+   */
+  FORCE_INLINE void low() { write(false); }
+
+  /**
+   * Set pin mode
+   * @param[in] pinMode if true set output mode else input mode.
+   *
+   * mode() does not enable or disable the 20K pullup for input mode.
+   */
+  FORCE_INLINE void mode(bool pinMode) { fastPinMode(PinNumber, pinMode); }
+
+  /** @return Pin's level */
+  FORCE_INLINE bool read() const { return fastDigitalRead(PinNumber); }
+
+  /**
+   * Toggle a pin
+   * If the pin is in output mode toggle the pin's level.
+   * If the pin is in input mode toggle the state of the 20K pullup.
+   */
+  FORCE_INLINE void toggle() { fastDigitalToggle(PinNumber); }
+
+  /**
+   * Write the pin's level.
+   * @param[in] value If true set the pin's level high else set the
+   *  pin's level low.
+   */
+  FORCE_INLINE void write(bool value) { fastDigitalWrite(PinNumber, value); }
+};
+
+const bool MISO_MODE  = false,  // Pin Mode for MISO is input.
+           MISO_LEVEL = false,  // Pullups disabled for MISO are disabled.
+           MOSI_MODE  = true,   // Pin Mode for MOSI is output.
+           SCK_MODE   = true;   // Pin Mode for SCK is output.
+
+/**
+ * @class SoftSPI
+ * @brief Fast software SPI.
+ */
+template<uint8_t MisoPin, uint8_t MosiPin, uint8_t SckPin, uint8_t Mode = 0>
+class SoftSPI {
+ public:
+
+  /** Initialize SoftSPI pins. */
+  void begin() {
+    fastPinConfig(MisoPin, MISO_MODE, MISO_LEVEL);
+    fastPinConfig(MosiPin, MOSI_MODE, !MODE_CPHA(Mode));
+    fastPinConfig(SckPin, SCK_MODE, MODE_CPOL(Mode));
+  }
+
+  /**
+   * Soft SPI receive byte.
+   * @return Data byte received.
+   */
+  FORCE_INLINE uint8_t receive() {
+    uint8_t data = 0;
+    receiveBit(7, &data);
+    receiveBit(6, &data);
+    receiveBit(5, &data);
+    receiveBit(4, &data);
+    receiveBit(3, &data);
+    receiveBit(2, &data);
+    receiveBit(1, &data);
+    receiveBit(0, &data);
+    return data;
+  }
+
+  /**
+   * Soft SPI send byte.
+   * @param[in] data Data byte to send.
+   */
+  FORCE_INLINE void send(uint8_t data) {
+    sendBit(7, data);
+    sendBit(6, data);
+    sendBit(5, data);
+    sendBit(4, data);
+    sendBit(3, data);
+    sendBit(2, data);
+    sendBit(1, data);
+    sendBit(0, data);
+  }
+
+  /**
+   * Soft SPI transfer byte.
+   * @param[in] txData Data byte to send.
+   * @return Data byte received.
+   */
+  FORCE_INLINE uint8_t transfer(uint8_t txData) {
+    uint8_t rxData = 0;
+    transferBit(7, &rxData, txData);
+    transferBit(6, &rxData, txData);
+    transferBit(5, &rxData, txData);
+    transferBit(4, &rxData, txData);
+    transferBit(3, &rxData, txData);
+    transferBit(2, &rxData, txData);
+    transferBit(1, &rxData, txData);
+    transferBit(0, &rxData, txData);
+    return rxData;
+  }
+
+ private:
+
+  FORCE_INLINE bool MODE_CPHA(uint8_t mode) { return bool(mode & 1); }
+  FORCE_INLINE bool MODE_CPOL(uint8_t mode) { return bool(mode & 2); }
+  FORCE_INLINE void receiveBit(uint8_t bit, uint8_t *data) {
+    if (MODE_CPHA(Mode)) fastDigitalWrite(SckPin, !MODE_CPOL(Mode));
+    nop;
+    nop;
+    fastDigitalWrite(SckPin,
+      MODE_CPHA(Mode) ? MODE_CPOL(Mode) : !MODE_CPOL(Mode));
+    if (fastDigitalRead(MisoPin)) SBI(*data, bit);
+    if (!MODE_CPHA(Mode)) fastDigitalWrite(SckPin, MODE_CPOL(Mode));
+  }
+
+  FORCE_INLINE void sendBit(uint8_t bit, uint8_t data) {
+    if (MODE_CPHA(Mode)) fastDigitalWrite(SckPin, !MODE_CPOL(Mode));
+    fastDigitalWrite(MosiPin, data & _BV(bit));
+    fastDigitalWrite(SckPin, MODE_CPHA(Mode) ? MODE_CPOL(Mode) : !MODE_CPOL(Mode));
+    nop;
+    nop;
+    if (!MODE_CPHA(Mode)) fastDigitalWrite(SckPin, MODE_CPOL(Mode));
+  }
+
+  FORCE_INLINE void transferBit(uint8_t bit, uint8_t *rxData, uint8_t txData) {
+    if (MODE_CPHA(Mode)) fastDigitalWrite(SckPin, !MODE_CPOL(Mode));
+    fastDigitalWrite(MosiPin, txData & _BV(bit));
+    fastDigitalWrite(SckPin,
+      MODE_CPHA(Mode) ? MODE_CPOL(Mode) : !MODE_CPOL(Mode));
+    if (fastDigitalRead(MisoPin)) SBI(*rxData, bit);
+    if (!MODE_CPHA(Mode)) fastDigitalWrite(SckPin, MODE_CPOL(Mode));
+  }
+
+};

src/libs/stopwatch.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 "stopwatch.h"
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../lcd/extui/ui_api.h"
+#endif
+
+Stopwatch::State Stopwatch::state;
+millis_t Stopwatch::accumulator;
+millis_t Stopwatch::startTimestamp;
+millis_t Stopwatch::stopTimestamp;
+
+bool Stopwatch::stop() {
+  debug(F("stop"));
+
+  if (isRunning() || isPaused()) {
+    TERN_(EXTENSIBLE_UI, ExtUI::onPrintTimerStopped());
+    state = STOPPED;
+    stopTimestamp = millis();
+    return true;
+  }
+  else return false;
+}
+
+bool Stopwatch::pause() {
+  debug(F("pause"));
+
+  if (isRunning()) {
+    TERN_(EXTENSIBLE_UI, ExtUI::onPrintTimerPaused());
+    state = PAUSED;
+    stopTimestamp = millis();
+    return true;
+  }
+  else return false;
+}
+
+bool Stopwatch::start() {
+  debug(F("start"));
+
+  TERN_(EXTENSIBLE_UI, ExtUI::onPrintTimerStarted());
+
+  if (!isRunning()) {
+    if (isPaused()) accumulator = duration();
+    else reset();
+
+    state = RUNNING;
+    startTimestamp = millis();
+    return true;
+  }
+  else return false;
+}
+
+void Stopwatch::resume(const millis_t with_time) {
+  debug(F("resume"));
+
+  reset();
+  if ((accumulator = with_time)) state = RUNNING;
+}
+
+void Stopwatch::reset() {
+  debug(F("reset"));
+
+  state = STOPPED;
+  startTimestamp = 0;
+  stopTimestamp = 0;
+  accumulator = 0;
+}
+
+millis_t Stopwatch::duration() {
+  return accumulator + MS_TO_SEC((isRunning() ? millis() : stopTimestamp) - startTimestamp);
+}
+
+#if ENABLED(DEBUG_STOPWATCH)
+
+  void Stopwatch::debug(FSTR_P const func) {
+    if (DEBUGGING(INFO)) SERIAL_ECHOLNPGM("Stopwatch::", func, "()");
+  }
+
+#endif

src/libs/stopwatch.h

@@ -0,0 +1,120 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "../inc/MarlinConfig.h"
+
+// Print debug messages with M111 S2 (Uses 156 bytes of PROGMEM)
+//#define DEBUG_STOPWATCH
+
+/**
+ * @brief Stopwatch class
+ * @details This class acts as a timer proving stopwatch functionality including
+ * the ability to pause the running time counter.
+ */
+class Stopwatch {
+  private:
+    enum State : char { STOPPED, RUNNING, PAUSED };
+
+    static Stopwatch::State state;
+    static millis_t accumulator;
+    static millis_t startTimestamp;
+    static millis_t stopTimestamp;
+
+  public:
+    /**
+     * @brief Initialize the stopwatch
+     */
+    FORCE_INLINE static void init() { reset(); }
+
+    /**
+     * @brief Stop the stopwatch
+     * @details Stop the running timer, it will silently ignore the request if
+     *          no timer is currently running.
+     * @return true on success
+     */
+    static bool stop();
+    static bool abort() { return stop(); } // Alias by default
+
+    /**
+     * @brief Pause the stopwatch
+     * @details Pause the running timer, it will silently ignore the request if
+     *          no timer is currently running.
+     * @return true on success
+     */
+    static bool pause();
+
+    /**
+     * @brief Start the stopwatch
+     * @details Start the timer, it will silently ignore the request if the
+     *          timer is already running.
+     * @return true on success
+     */
+    static bool start();
+
+    /**
+     * @brief Resume the stopwatch
+     * @details Resume a timer from a given duration
+     */
+    static void resume(const millis_t with_time);
+
+    /**
+     * @brief Reset the stopwatch
+     * @details Reset all settings to their default values.
+     */
+    static void reset();
+
+    /**
+     * @brief Check if the timer is running
+     * @details Return true if the timer is currently running, false otherwise.
+     * @return true if stopwatch is running
+     */
+    FORCE_INLINE static bool isRunning() { return state == RUNNING; }
+
+    /**
+     * @brief Check if the timer is paused
+     * @details Return true if the timer is currently paused, false otherwise.
+     * @return true if stopwatch is paused
+     */
+    FORCE_INLINE static bool isPaused() { return state == PAUSED; }
+
+    /**
+     * @brief Get the running time
+     * @details Return the total number of seconds the timer has been running.
+     * @return the delta since starting the stopwatch
+     */
+    static millis_t duration();
+
+    #ifdef DEBUG_STOPWATCH
+
+      /**
+       * @brief Print a debug message
+       * @details Print a simple debug message "Stopwatch::function"
+       */
+      static void debug(FSTR_P const);
+
+    #else
+
+      static void debug(FSTR_P const) {}
+
+    #endif
+};

src/libs/vector_3.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/>.
+ *
+ */
+
+/**
+ * vector_3.cpp - Vector library for bed leveling
+ * Copyright (c) 2012 Lars Brubaker.  All right reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ABL_PLANAR || ENABLED(AUTO_BED_LEVELING_UBL)
+
+#include "vector_3.h"
+
+#include <math.h>
+
+/**
+ *  vector_3
+ */
+
+vector_3 vector_3::cross(const vector_3 &left, const vector_3 &right) {
+  return vector_3(left.y * right.z - left.z * right.y,  // YZ cross
+                  left.z * right.x - left.x * right.z,  // ZX cross
+                  left.x * right.y - left.y * right.x); // XY cross
+}
+
+vector_3 vector_3::get_normal() const {
+  vector_3 normalized = *this;
+  normalized.normalize();
+  return normalized;
+}
+
+float vector_3::magnitude() const { return SQRT(sq(x) + sq(y) + sq(z)); }
+
+void vector_3::normalize() { *this *= RSQRT(sq(x) + sq(y) + sq(z)); }
+
+// Apply a rotation to the matrix
+void vector_3::apply_rotation(const matrix_3x3 &matrix) {
+  const float _x = x, _y = y, _z = z;
+  *this = { matrix.vectors[0].x * _x + matrix.vectors[1].x * _y + matrix.vectors[2].x * _z,
+            matrix.vectors[0].y * _x + matrix.vectors[1].y * _y + matrix.vectors[2].y * _z,
+            matrix.vectors[0].z * _x + matrix.vectors[1].z * _y + matrix.vectors[2].z * _z };
+}
+
+void vector_3::debug(FSTR_P const title) {
+  SERIAL_ECHOF(title);
+  SERIAL_ECHOPAIR_F_P(SP_X_STR, x, 6);
+  SERIAL_ECHOPAIR_F_P(SP_Y_STR, y, 6);
+  SERIAL_ECHOLNPAIR_F_P(SP_Z_STR, z, 6);
+}
+
+/**
+ *  matrix_3x3
+ */
+
+void matrix_3x3::apply_rotation_xyz(float &_x, float &_y, float &_z) {
+  vector_3 vec = vector_3(_x, _y, _z); vec.apply_rotation(*this);
+  _x = vec.x; _y = vec.y; _z = vec.z;
+}
+
+// Reset to identity. No rotate or translate.
+void matrix_3x3::set_to_identity() {
+  LOOP_L_N(i, 3)
+    LOOP_L_N(j, 3)
+      vectors[i][j] = float(i == j);
+}
+
+// Create a matrix from 3 vector_3 inputs
+matrix_3x3 matrix_3x3::create_from_rows(const vector_3 &row_0, const vector_3 &row_1, const vector_3 &row_2) {
+  //row_0.debug(F("row_0"));
+  //row_1.debug(F("row_1"));
+  //row_2.debug(F("row_2"));
+  matrix_3x3 new_matrix;
+  new_matrix.vectors[0] = row_0;
+  new_matrix.vectors[1] = row_1;
+  new_matrix.vectors[2] = row_2;
+  //new_matrix.debug(F("new_matrix"));
+  return new_matrix;
+}
+
+// Create a matrix rotated to point towards a target
+matrix_3x3 matrix_3x3::create_look_at(const vector_3 &target) {
+  const vector_3 z_row = target.get_normal(),
+                 x_row = vector_3(1, 0, -target.x / target.z).get_normal(),
+                 y_row = vector_3::cross(z_row, x_row).get_normal();
+
+  // x_row.debug(F("x_row"));
+  // y_row.debug(F("y_row"));
+  // z_row.debug(F("z_row"));
+
+  // create the matrix already correctly transposed
+  matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row);
+
+  // rot.debug(F("rot"));
+  return rot;
+}
+
+// Get a transposed copy of the matrix
+matrix_3x3 matrix_3x3::transpose(const matrix_3x3 &original) {
+  matrix_3x3 new_matrix;
+  LOOP_L_N(i, 3)
+    LOOP_L_N(j, 3)
+      new_matrix.vectors[i][j] = original.vectors[j][i];
+  return new_matrix;
+}
+
+void matrix_3x3::debug(FSTR_P const title) {
+  if (title) SERIAL_ECHOLNF(title);
+  LOOP_L_N(i, 3) {
+    LOOP_L_N(j, 3) {
+      serial_offset(vectors[i][j], 2);
+      SERIAL_CHAR(' ');
+    }
+    SERIAL_EOL();
+  }
+}
+
+#endif // HAS_ABL_OR_UBL

src/libs/vector_3.h

@@ -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/>.
+ *
+ */
+#pragma once
+
+/**
+ * vector_3.cpp - Vector library for bed leveling
+ * Copyright (c) 2012 Lars Brubaker.  All right reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#include "../core/types.h"
+
+class matrix_3x3;
+
+struct vector_3 {
+  union {
+    struct { float x, y, z; };
+    float pos[3];
+  };
+  vector_3(const_float_t _x, const_float_t _y, const_float_t _z) : x(_x), y(_y), z(_z) {}
+  vector_3(const xy_float_t   &in) { x = in.x; TERN_(HAS_Y_AXIS, y = in.y); }
+  vector_3(const xyz_float_t  &in) { x = in.x; TERN_(HAS_Y_AXIS, y = in.y); TERN_(HAS_Z_AXIS, z = in.z); }
+  vector_3(const xyze_float_t &in) { x = in.x; TERN_(HAS_Y_AXIS, y = in.y); TERN_(HAS_Z_AXIS, z = in.z); }
+  vector_3() { x = y = z = 0; }
+
+  // Factory method
+  static vector_3 cross(const vector_3 &a, const vector_3 &b);
+
+  // Modifiers
+  void normalize();
+  void apply_rotation(const matrix_3x3 &matrix);
+
+  // Accessors
+  float magnitude() const;
+  vector_3 get_normal() const;
+
+  // Operators
+        float& operator[](const int n)       { return pos[n]; }
+  const float& operator[](const int n) const { return pos[n]; }
+
+  vector_3& operator*=(const float &v)  { x *= v; y *= v; z *= v; return *this; }
+  vector_3 operator+(const vector_3 &v) { return vector_3(x + v.x, y + v.y, z + v.z); }
+  vector_3 operator-(const vector_3 &v) { return vector_3(x - v.x, y - v.y, z - v.z); }
+  vector_3 operator*(const float &v)    { return vector_3(x * v, y * v, z * v); }
+
+  operator xy_float_t() { return xy_float_t({ x, y }); }
+  operator xyz_float_t() { return xyz_float_t({ x, y, z }); }
+
+  void debug(FSTR_P const title);
+};
+
+struct matrix_3x3 {
+  vector_3 vectors[3];
+
+  // Factory methods
+  static matrix_3x3 create_from_rows(const vector_3 &row_0, const vector_3 &row_1, const vector_3 &row_2);
+  static matrix_3x3 create_look_at(const vector_3 &target);
+  static matrix_3x3 transpose(const matrix_3x3 &original);
+
+  void set_to_identity();
+
+  void debug(FSTR_P const title);
+
+  void apply_rotation_xyz(float &x, float &y, float &z);
+};

src/module/delta.cpp

@@ -0,0 +1,289 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * delta.cpp
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(DELTA)
+
+#include "delta.h"
+#include "motion.h"
+
+// For homing:
+#include "planner.h"
+#include "endstops.h"
+#include "../lcd/marlinui.h"
+#include "../MarlinCore.h"
+
+#if HAS_BED_PROBE
+  #include "probe.h"
+#endif
+
+#if ENABLED(SENSORLESS_HOMING)
+  #include "../feature/tmc_util.h"
+  #include "stepper/indirection.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../core/debug_out.h"
+
+// Initialized by settings.load()
+float delta_height;
+abc_float_t delta_endstop_adj{0};
+float delta_radius,
+      delta_diagonal_rod,
+      segments_per_second;
+abc_float_t delta_tower_angle_trim;
+xy_float_t delta_tower[ABC];
+abc_float_t delta_diagonal_rod_2_tower;
+float delta_clip_start_height = Z_MAX_POS;
+abc_float_t delta_diagonal_rod_trim;
+#if HAS_DELTA_SENSORLESS_PROBING
+  abc_float_t offset_sensorless_adj{0};
+  float largest_sensorless_adj = 0;
+#endif
+
+float delta_safe_distance_from_top();
+
+void refresh_delta_clip_start_height() {
+  delta_clip_start_height = TERN(HAS_SOFTWARE_ENDSTOPS,
+    soft_endstop.max.z,
+    DIFF_TERN(HAS_BED_PROBE, delta_height, probe.offset.z)
+  ) - delta_safe_distance_from_top();
+}
+
+/**
+ * Recalculate factors used for delta kinematics whenever
+ * settings have been changed (e.g., by M665).
+ */
+void recalc_delta_settings() {
+  constexpr abc_float_t trt = DELTA_RADIUS_TRIM_TOWER;
+  delta_tower[A_AXIS].set(cos(RADIANS(210 + delta_tower_angle_trim.a)) * (delta_radius + trt.a), // front left tower
+                          sin(RADIANS(210 + delta_tower_angle_trim.a)) * (delta_radius + trt.a));
+  delta_tower[B_AXIS].set(cos(RADIANS(330 + delta_tower_angle_trim.b)) * (delta_radius + trt.b), // front right tower
+                          sin(RADIANS(330 + delta_tower_angle_trim.b)) * (delta_radius + trt.b));
+  delta_tower[C_AXIS].set(cos(RADIANS( 90 + delta_tower_angle_trim.c)) * (delta_radius + trt.c), // back middle tower
+                          sin(RADIANS( 90 + delta_tower_angle_trim.c)) * (delta_radius + trt.c));
+  delta_diagonal_rod_2_tower.set(sq(delta_diagonal_rod + delta_diagonal_rod_trim.a),
+                                 sq(delta_diagonal_rod + delta_diagonal_rod_trim.b),
+                                 sq(delta_diagonal_rod + delta_diagonal_rod_trim.c));
+  update_software_endstops(Z_AXIS);
+  set_all_unhomed();
+}
+
+/**
+ * Delta Inverse Kinematics
+ *
+ * Calculate the tower positions for a given machine
+ * position, storing the result in the delta[] array.
+ *
+ * This is an expensive calculation, requiring 3 square
+ * roots per segmented linear move, and strains the limits
+ * of a Mega2560 with a Graphical Display.
+ *
+ * Suggested optimizations include:
+ *
+ * - Disable the home_offset (M206) and/or position_shift (G92)
+ *   features to remove up to 12 float additions.
+ */
+
+#define DELTA_DEBUG(VAR) do { \
+    SERIAL_ECHOLNPGM_P(PSTR("Cartesian X"), VAR.x, SP_Y_STR, VAR.y, SP_Z_STR, VAR.z); \
+    SERIAL_ECHOLNPGM_P(PSTR("Delta A"), delta.a, SP_B_STR, delta.b, SP_C_STR, delta.c); \
+  }while(0)
+
+void inverse_kinematics(const xyz_pos_t &raw) {
+  #if HAS_HOTEND_OFFSET
+    // Delta hotend offsets must be applied in Cartesian space with no "spoofing"
+    xyz_pos_t pos = { raw.x - hotend_offset[active_extruder].x,
+                      raw.y - hotend_offset[active_extruder].y,
+                      raw.z };
+    DELTA_IK(pos);
+    //DELTA_DEBUG(pos);
+  #else
+    DELTA_IK(raw);
+    //DELTA_DEBUG(raw);
+  #endif
+}
+
+/**
+ * Calculate the highest Z position where the
+ * effector has the full range of XY motion.
+ */
+float delta_safe_distance_from_top() {
+  xyz_pos_t cartesian{0};
+  inverse_kinematics(cartesian);
+  const float centered_extent = delta.a;
+  cartesian.y = DELTA_PRINTABLE_RADIUS;
+  inverse_kinematics(cartesian);
+  return ABS(centered_extent - delta.a);
+}
+
+/**
+ * Delta Forward Kinematics
+ *
+ * See the Wikipedia article "Trilateration"
+ * https://en.wikipedia.org/wiki/Trilateration
+ *
+ * Establish a new coordinate system in the plane of the
+ * three carriage points. This system has its origin at
+ * tower1, with tower2 on the X axis. Tower3 is in the X-Y
+ * plane with a Z component of zero.
+ * We will define unit vectors in this coordinate system
+ * in our original coordinate system. Then when we calculate
+ * the Xnew, Ynew and Znew values, we can translate back into
+ * the original system by moving along those unit vectors
+ * by the corresponding values.
+ *
+ * Variable names matched to Marlin, c-version, and avoid the
+ * use of any vector library.
+ *
+ * by Andreas Hardtung 2016-06-07
+ * based on a Java function from "Delta Robot Kinematics V3"
+ * by Steve Graves
+ *
+ * The result is stored in the cartes[] array.
+ */
+void forward_kinematics(const_float_t z1, const_float_t z2, const_float_t z3) {
+  // Create a vector in old coordinates along x axis of new coordinate
+  const float p12[3] = { delta_tower[B_AXIS].x - delta_tower[A_AXIS].x, delta_tower[B_AXIS].y - delta_tower[A_AXIS].y, z2 - z1 },
+
+  // Get the reciprocal of Magnitude of vector.
+  d2 = sq(p12[0]) + sq(p12[1]) + sq(p12[2]), inv_d = RSQRT(d2),
+
+  // Create unit vector by multiplying by the inverse of the magnitude.
+  ex[3] = { p12[0] * inv_d, p12[1] * inv_d, p12[2] * inv_d },
+
+  // Get the vector from the origin of the new system to the third point.
+  p13[3] = { delta_tower[C_AXIS].x - delta_tower[A_AXIS].x, delta_tower[C_AXIS].y - delta_tower[A_AXIS].y, z3 - z1 },
+
+  // Use the dot product to find the component of this vector on the X axis.
+  i = ex[0] * p13[0] + ex[1] * p13[1] + ex[2] * p13[2],
+
+  // Create a vector along the x axis that represents the x component of p13.
+  iex[3] = { ex[0] * i, ex[1] * i, ex[2] * i };
+
+  // Subtract the X component from the original vector leaving only Y. We use the
+  // variable that will be the unit vector after we scale it.
+  float ey[3] = { p13[0] - iex[0], p13[1] - iex[1], p13[2] - iex[2] };
+
+  // The magnitude and the inverse of the magnitude of Y component
+  const float j2 = sq(ey[0]) + sq(ey[1]) + sq(ey[2]), inv_j = RSQRT(j2);
+
+  // Convert to a unit vector
+  ey[0] *= inv_j; ey[1] *= inv_j; ey[2] *= inv_j;
+
+  // The cross product of the unit x and y is the unit z
+  // float[] ez = vectorCrossProd(ex, ey);
+  const float ez[3] = {
+    ex[1] * ey[2] - ex[2] * ey[1],
+    ex[2] * ey[0] - ex[0] * ey[2],
+    ex[0] * ey[1] - ex[1] * ey[0]
+  },
+
+  // We now have the d, i and j values defined in Wikipedia.
+  // Plug them into the equations defined in Wikipedia for Xnew, Ynew and Znew
+  Xnew = (delta_diagonal_rod_2_tower.a - delta_diagonal_rod_2_tower.b + d2) * inv_d * 0.5,
+  Ynew = ((delta_diagonal_rod_2_tower.a - delta_diagonal_rod_2_tower.c + sq(i) + j2) * 0.5 - i * Xnew) * inv_j,
+  Znew = SQRT(delta_diagonal_rod_2_tower.a - HYPOT2(Xnew, Ynew));
+
+  // Start from the origin of the old coordinates and add vectors in the
+  // old coords that represent the Xnew, Ynew and Znew to find the point
+  // in the old system.
+  cartes.set(delta_tower[A_AXIS].x + ex[0] * Xnew + ey[0] * Ynew - ez[0] * Znew,
+             delta_tower[A_AXIS].y + ex[1] * Xnew + ey[1] * Ynew - ez[1] * Znew,
+                                z1 + ex[2] * Xnew + ey[2] * Ynew - ez[2] * Znew);
+}
+
+/**
+ * A delta can only safely home all axes at the same time
+ * This is like quick_home_xy() but for 3 towers.
+ */
+void home_delta() {
+  DEBUG_SECTION(log_home_delta, "home_delta", DEBUGGING(LEVELING));
+
+  // Init the current position of all carriages to 0,0,0
+  current_position.reset();
+  destination.reset();
+  sync_plan_position();
+
+  // Disable stealthChop if used. Enable diag1 pin on driver.
+  #if ENABLED(SENSORLESS_HOMING)
+    TERN_(X_SENSORLESS, sensorless_t stealth_states_x = start_sensorless_homing_per_axis(X_AXIS));
+    TERN_(Y_SENSORLESS, sensorless_t stealth_states_y = start_sensorless_homing_per_axis(Y_AXIS));
+    TERN_(Z_SENSORLESS, sensorless_t stealth_states_z = start_sensorless_homing_per_axis(Z_AXIS));
+    TERN_(I_SENSORLESS, sensorless_t stealth_states_i = start_sensorless_homing_per_axis(I_AXIS));
+    TERN_(J_SENSORLESS, sensorless_t stealth_states_j = start_sensorless_homing_per_axis(J_AXIS));
+    TERN_(K_SENSORLESS, sensorless_t stealth_states_k = start_sensorless_homing_per_axis(K_AXIS));
+    #if SENSORLESS_STALLGUARD_DELAY
+      safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle
+    #endif
+  #endif
+
+  // Move all carriages together linearly until an endstop is hit.
+  current_position.z = DIFF_TERN(HAS_BED_PROBE, delta_height + 10, probe.offset.z);
+  line_to_current_position(homing_feedrate(Z_AXIS));
+  planner.synchronize();
+  TERN_(HAS_DELTA_SENSORLESS_PROBING, endstops.report_states());
+
+  // Re-enable stealthChop if used. Disable diag1 pin on driver.
+  #if ENABLED(SENSORLESS_HOMING) && DISABLED(ENDSTOPS_ALWAYS_ON_DEFAULT)
+    TERN_(X_SENSORLESS, end_sensorless_homing_per_axis(X_AXIS, stealth_states_x));
+    TERN_(Y_SENSORLESS, end_sensorless_homing_per_axis(Y_AXIS, stealth_states_y));
+    TERN_(Z_SENSORLESS, end_sensorless_homing_per_axis(Z_AXIS, stealth_states_z));
+    TERN_(I_SENSORLESS, end_sensorless_homing_per_axis(I_AXIS, stealth_states_i));
+    TERN_(J_SENSORLESS, end_sensorless_homing_per_axis(J_AXIS, stealth_states_j));
+    TERN_(K_SENSORLESS, end_sensorless_homing_per_axis(K_AXIS, stealth_states_k));
+    #if SENSORLESS_STALLGUARD_DELAY
+      safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle
+    #endif
+  #endif
+
+  endstops.validate_homing_move();
+
+  // At least one carriage has reached the top.
+  // Now re-home each carriage separately.
+  homeaxis(A_AXIS);
+  homeaxis(B_AXIS);
+  homeaxis(C_AXIS);
+
+  // Set all carriages to their home positions
+  // Do this here all at once for Delta, because
+  // XYZ isn't ABC. Applying this per-tower would
+  // give the impression that they are the same.
+  LOOP_ABC(i) set_axis_is_at_home((AxisEnum)i);
+
+  sync_plan_position();
+
+  #if DISABLED(DELTA_HOME_TO_SAFE_ZONE) && defined(HOMING_BACKOFF_POST_MM)
+    constexpr xyz_float_t endstop_backoff = HOMING_BACKOFF_POST_MM;
+    if (endstop_backoff.z) {
+      current_position.z -= ABS(endstop_backoff.z) * Z_HOME_DIR;
+      line_to_current_position(homing_feedrate(Z_AXIS));
+    }
+  #endif
+}
+
+#endif // DELTA

src/module/delta.h

@@ -0,0 +1,129 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * delta.h - Delta-specific functions
+ */
+
+#include "../core/types.h"
+#include "../core/macros.h"
+
+extern float delta_height;
+extern abc_float_t delta_endstop_adj;
+extern float delta_radius,
+             delta_diagonal_rod,
+             segments_per_second;
+extern abc_float_t delta_tower_angle_trim;
+extern xy_float_t delta_tower[ABC];
+extern abc_float_t delta_diagonal_rod_2_tower;
+extern float delta_clip_start_height;
+extern abc_float_t delta_diagonal_rod_trim;
+#if HAS_DELTA_SENSORLESS_PROBING
+  extern abc_float_t offset_sensorless_adj;
+  extern float largest_sensorless_adj;
+#endif
+
+/**
+ * Recalculate factors used for delta kinematics whenever
+ * settings have been changed (e.g., by M665).
+ */
+void recalc_delta_settings();
+
+/**
+ * Get a safe radius for calibration
+ */
+#if HAS_DELTA_SENSORLESS_PROBING
+  static constexpr float sensorless_radius_factor = 0.7f;
+#endif
+
+/**
+ * Delta Inverse Kinematics
+ *
+ * Calculate the tower positions for a given machine
+ * position, storing the result in the delta[] array.
+ *
+ * This is an expensive calculation, requiring 3 square
+ * roots per segmented linear move, and strains the limits
+ * of a Mega2560 with a Graphical Display.
+ *
+ * Suggested optimizations include:
+ *
+ * - Disable the home_offset (M206) and/or position_shift (G92)
+ *   features to remove up to 12 float additions.
+ *
+ * - Use a fast-inverse-sqrt function and add the reciprocal.
+ *   (see above)
+ */
+
+// Macro to obtain the Z position of an individual tower
+#define DELTA_Z(V,T) V.z + SQRT(          \
+  delta_diagonal_rod_2_tower[T] - HYPOT2( \
+      delta_tower[T].x - V.x,             \
+      delta_tower[T].y - V.y              \
+    )                                     \
+  )
+
+#define DELTA_IK(V) delta.set(DELTA_Z(V, A_AXIS), DELTA_Z(V, B_AXIS), DELTA_Z(V, C_AXIS))
+
+void inverse_kinematics(const xyz_pos_t &raw);
+
+/**
+ * Calculate the highest Z position where the
+ * effector has the full range of XY motion.
+ */
+float delta_safe_distance_from_top();
+
+void refresh_delta_clip_start_height();
+
+/**
+ * Delta Forward Kinematics
+ *
+ * See the Wikipedia article "Trilateration"
+ * https://en.wikipedia.org/wiki/Trilateration
+ *
+ * Establish a new coordinate system in the plane of the
+ * three carriage points. This system has its origin at
+ * tower1, with tower2 on the X axis. Tower3 is in the X-Y
+ * plane with a Z component of zero.
+ * We will define unit vectors in this coordinate system
+ * in our original coordinate system. Then when we calculate
+ * the Xnew, Ynew and Znew values, we can translate back into
+ * the original system by moving along those unit vectors
+ * by the corresponding values.
+ *
+ * Variable names matched to Marlin, c-version, and avoid the
+ * use of any vector library.
+ *
+ * by Andreas Hardtung 2016-06-07
+ * based on a Java function from "Delta Robot Kinematics V3"
+ * by Steve Graves
+ *
+ * The result is stored in the cartes[] array.
+ */
+void forward_kinematics(const_float_t z1, const_float_t z2, const_float_t z3);
+
+FORCE_INLINE void forward_kinematics(const abc_float_t &point) {
+  forward_kinematics(point.a, point.b, point.c);
+}
+
+void home_delta();

src/module/endstops.h

@@ -0,0 +1,265 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * endstops.h - manages endstops
+ */
+
+#include "../inc/MarlinConfig.h"
+#include <stdint.h>
+
+#define __ES_ITEM(N) N,
+#define _ES_ITEM(K,N) TERN_(K,DEFER4(__ES_ITEM)(N))
+
+enum EndstopEnum : char {
+  // Common XYZ (ABC) endstops. Defined according to USE_[XYZ](MIN|MAX)_PLUG settings.
+  _ES_ITEM(HAS_X_MIN, X_MIN)
+  _ES_ITEM(HAS_X_MAX, X_MAX)
+  _ES_ITEM(HAS_Y_MIN, Y_MIN)
+  _ES_ITEM(HAS_Y_MAX, Y_MAX)
+  _ES_ITEM(HAS_Z_MIN, Z_MIN)
+  _ES_ITEM(HAS_Z_MAX, Z_MAX)
+  _ES_ITEM(HAS_I_MIN, I_MIN)
+  _ES_ITEM(HAS_I_MAX, I_MAX)
+  _ES_ITEM(HAS_J_MIN, J_MIN)
+  _ES_ITEM(HAS_J_MAX, J_MAX)
+  _ES_ITEM(HAS_K_MIN, K_MIN)
+  _ES_ITEM(HAS_K_MAX, K_MAX)
+
+  // Extra Endstops for XYZ
+  #if ENABLED(X_DUAL_ENDSTOPS)
+    _ES_ITEM(HAS_X_MIN, X2_MIN)
+    _ES_ITEM(HAS_X_MAX, X2_MAX)
+  #endif
+  #if ENABLED(Y_DUAL_ENDSTOPS)
+    _ES_ITEM(HAS_Y_MIN, Y2_MIN)
+    _ES_ITEM(HAS_Y_MAX, Y2_MAX)
+  #endif
+  #if ENABLED(Z_MULTI_ENDSTOPS)
+    _ES_ITEM(HAS_Z_MIN, Z2_MIN)
+    _ES_ITEM(HAS_Z_MAX, Z2_MAX)
+    #if NUM_Z_STEPPERS >= 3
+      _ES_ITEM(HAS_Z_MIN, Z3_MIN)
+      _ES_ITEM(HAS_Z_MAX, Z3_MAX)
+    #endif
+    #if NUM_Z_STEPPERS >= 4
+      _ES_ITEM(HAS_Z_MIN, Z4_MIN)
+      _ES_ITEM(HAS_Z_MAX, Z4_MAX)
+    #endif
+  #endif
+
+  // Bed Probe state is distinct or shared with Z_MIN (i.e., when the probe is the only Z endstop)
+  #if !HAS_DELTA_SENSORLESS_PROBING
+    _ES_ITEM(HAS_BED_PROBE, Z_MIN_PROBE IF_DISABLED(USES_Z_MIN_PROBE_PIN, = Z_MIN))
+  #endif
+
+  // The total number of states
+  NUM_ENDSTOP_STATES
+
+  // Endstops can be either MIN or MAX but not both
+  #if HAS_X_MIN || HAS_X_MAX
+    , X_ENDSTOP = TERN(X_HOME_TO_MAX, X_MAX, X_MIN)
+  #endif
+  #if HAS_Y_MIN || HAS_Y_MAX
+    , Y_ENDSTOP = TERN(Y_HOME_TO_MAX, Y_MAX, Y_MIN)
+  #endif
+  #if HAS_Z_MIN || HAS_Z_MAX || HOMING_Z_WITH_PROBE
+    , Z_ENDSTOP = TERN(Z_HOME_TO_MAX, Z_MAX, TERN(HOMING_Z_WITH_PROBE, Z_MIN_PROBE, Z_MIN))
+  #endif
+  #if HAS_I_MIN || HAS_I_MAX
+    , I_ENDSTOP = TERN(I_HOME_TO_MAX, I_MAX, I_MIN)
+  #endif
+  #if HAS_J_MIN || HAS_J_MAX
+    , J_ENDSTOP = TERN(J_HOME_TO_MAX, J_MAX, J_MIN)
+  #endif
+  #if HAS_K_MIN || HAS_K_MAX
+    , K_ENDSTOP = TERN(K_HOME_TO_MAX, K_MAX, K_MIN)
+  #endif
+};
+
+#undef __ES_ITEM
+#undef _ES_ITEM
+
+class Endstops {
+  public:
+
+    typedef IF<(NUM_ENDSTOP_STATES > 8), uint16_t, uint8_t>::type endstop_mask_t;
+
+    #if ENABLED(X_DUAL_ENDSTOPS)
+      static float x2_endstop_adj;
+    #endif
+    #if ENABLED(Y_DUAL_ENDSTOPS)
+      static float y2_endstop_adj;
+    #endif
+    #if ENABLED(Z_MULTI_ENDSTOPS)
+      static float z2_endstop_adj;
+    #endif
+    #if ENABLED(Z_MULTI_ENDSTOPS) && NUM_Z_STEPPERS >= 3
+      static float z3_endstop_adj;
+    #endif
+    #if ENABLED(Z_MULTI_ENDSTOPS) && NUM_Z_STEPPERS >= 4
+      static float z4_endstop_adj;
+    #endif
+
+  private:
+    static bool enabled, enabled_globally;
+    static endstop_mask_t live_state;
+    static volatile endstop_mask_t hit_state; // Use X_MIN, Y_MIN, Z_MIN and Z_MIN_PROBE as BIT index
+
+    #if ENDSTOP_NOISE_THRESHOLD
+      static endstop_mask_t validated_live_state;
+      static uint8_t endstop_poll_count;    // Countdown from threshold for polling
+    #endif
+
+  public:
+    Endstops() {};
+
+    /**
+     * Initialize the endstop pins
+     */
+    static void init();
+
+    /**
+     * Are endstops or the probe set to abort the move?
+     */
+    FORCE_INLINE static bool abort_enabled() {
+      return enabled || TERN0(HAS_BED_PROBE, z_probe_enabled);
+    }
+
+    static bool global_enabled() { return enabled_globally; }
+
+    /**
+     * Periodic call to poll endstops if required. Called from temperature ISR
+     */
+    static void poll();
+
+    /**
+     * Update endstops bits from the pins. Apply filtering to get a verified state.
+     * If abort_enabled() and moving towards a triggered switch, abort the current move.
+     * Called from ISR contexts.
+     */
+    static void update();
+
+    /**
+     * Get Endstop hit state.
+     */
+    FORCE_INLINE static endstop_mask_t trigger_state() { return hit_state; }
+
+    /**
+     * Get current endstops state
+     */
+    FORCE_INLINE static endstop_mask_t state() {
+      return
+        #if ENDSTOP_NOISE_THRESHOLD
+          validated_live_state
+        #else
+          live_state
+        #endif
+      ;
+    }
+
+    static bool probe_switch_activated() {
+      return (true
+        #if ENABLED(PROBE_ACTIVATION_SWITCH)
+          && READ(PROBE_ACTIVATION_SWITCH_PIN) == PROBE_ACTIVATION_SWITCH_STATE
+        #endif
+      );
+    }
+
+    /**
+     * Report endstop hits to serial. Called from loop().
+     */
+    static void event_handler();
+
+    /**
+     * Report endstop states in response to M119
+     */
+    static void report_states();
+
+    // Enable / disable endstop checking globally
+    static void enable_globally(const bool onoff=true);
+
+    // Enable / disable endstop checking
+    static void enable(const bool onoff=true);
+
+    // Disable / Enable endstops based on ENSTOPS_ONLY_FOR_HOMING and global enable
+    static void not_homing();
+
+    #if ENABLED(VALIDATE_HOMING_ENDSTOPS)
+      // If the last move failed to trigger an endstop, call kill
+      static void validate_homing_move();
+    #else
+      FORCE_INLINE static void validate_homing_move() { hit_on_purpose(); }
+    #endif
+
+    // Clear endstops (i.e., they were hit intentionally) to suppress the report
+    FORCE_INLINE static void hit_on_purpose() { hit_state = 0; }
+
+    // Enable / disable endstop z-probe checking
+    #if HAS_BED_PROBE
+      static volatile bool z_probe_enabled;
+      static void enable_z_probe(const bool onoff=true);
+    #endif
+
+    static void resync();
+
+    // Debugging of endstops
+    #if ENABLED(PINS_DEBUGGING)
+      static bool monitor_flag;
+      static void monitor();
+      static void run_monitor();
+    #endif
+
+    #if ENABLED(SPI_ENDSTOPS)
+      typedef struct {
+        union {
+          bool any;
+          struct { bool NUM_AXIS_LIST(x:1, y:1, z:1, i:1, j:1, k:1); };
+        };
+      } tmc_spi_homing_t;
+      static tmc_spi_homing_t tmc_spi_homing;
+      static void clear_endstop_state();
+      static bool tmc_spi_homing_check();
+    #endif
+  public:
+    // Basic functions for Sensorless Homing
+    #if USE_SENSORLESS
+      static void set_homing_current(const bool onoff);
+    #endif
+};
+
+extern Endstops endstops;
+
+/**
+ * A class to save and change the endstop state,
+ * then restore it when it goes out of scope.
+ */
+class TemporaryGlobalEndstopsState {
+  bool saved;
+
+  public:
+    TemporaryGlobalEndstopsState(const bool enable) : saved(endstops.global_enabled()) {
+      endstops.enable_globally(enable);
+    }
+    ~TemporaryGlobalEndstopsState() { endstops.enable_globally(saved); }
+};

src/module/motion.h

@@ -0,0 +1,581 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * motion.h
+ *
+ * High-level motion commands to feed the planner
+ * Some of these methods may migrate to the planner class.
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if IS_SCARA
+  #include "scara.h"
+#endif
+
+// Error margin to work around float imprecision
+constexpr float fslop = 0.0001;
+
+extern bool relative_mode;
+
+extern xyze_pos_t current_position,  // High-level current tool position
+                  destination;       // Destination for a move
+
+// G60/G61 Position Save and Return
+#if SAVED_POSITIONS
+  extern uint8_t saved_slots[(SAVED_POSITIONS + 7) >> 3]; // TODO: Add support for HAS_I_AXIS
+  extern xyze_pos_t stored_position[SAVED_POSITIONS];
+#endif
+
+// Scratch space for a cartesian result
+extern xyz_pos_t cartes;
+
+// Until kinematics.cpp is created, declare this here
+#if IS_KINEMATIC
+  extern abce_pos_t delta;
+#endif
+
+#if HAS_ABL_NOT_UBL
+  extern feedRate_t xy_probe_feedrate_mm_s;
+  #define XY_PROBE_FEEDRATE_MM_S xy_probe_feedrate_mm_s
+#elif defined(XY_PROBE_FEEDRATE)
+  #define XY_PROBE_FEEDRATE_MM_S MMM_TO_MMS(XY_PROBE_FEEDRATE)
+#else
+  #define XY_PROBE_FEEDRATE_MM_S PLANNER_XY_FEEDRATE()
+#endif
+
+#if HAS_BED_PROBE
+  constexpr feedRate_t z_probe_fast_mm_s = MMM_TO_MMS(Z_PROBE_FEEDRATE_FAST);
+#endif
+
+/**
+ * Feed rates are often configured with mm/m
+ * but the planner and stepper like mm/s units.
+ */
+constexpr xyz_feedrate_t homing_feedrate_mm_m = HOMING_FEEDRATE_MM_M;
+FORCE_INLINE feedRate_t homing_feedrate(const AxisEnum a) {
+  float v = TERN0(HAS_Z_AXIS, homing_feedrate_mm_m.z);
+  #if DISABLED(DELTA)
+    NUM_AXIS_CODE(
+           if (a == X_AXIS) v = homing_feedrate_mm_m.x,
+      else if (a == Y_AXIS) v = homing_feedrate_mm_m.y,
+      else if (a == Z_AXIS) v = homing_feedrate_mm_m.z,
+      else if (a == I_AXIS) v = homing_feedrate_mm_m.i,
+      else if (a == J_AXIS) v = homing_feedrate_mm_m.j,
+      else if (a == K_AXIS) v = homing_feedrate_mm_m.k
+    );
+  #endif
+  return MMM_TO_MMS(v);
+}
+
+feedRate_t get_homing_bump_feedrate(const AxisEnum axis);
+
+/**
+ * The default feedrate for many moves, set by the most recent move
+ */
+extern feedRate_t feedrate_mm_s;
+
+/**
+ * Feedrate scaling is applied to all G0/G1, G2/G3, and G5 moves
+ */
+extern int16_t feedrate_percentage;
+#define MMS_SCALED(V) ((V) * 0.01f * feedrate_percentage)
+
+// The active extruder (tool). Set with T<extruder> command.
+#if HAS_MULTI_EXTRUDER
+  extern uint8_t active_extruder;
+#else
+  constexpr uint8_t active_extruder = 0;
+#endif
+
+#if ENABLED(LCD_SHOW_E_TOTAL)
+  extern float e_move_accumulator;
+#endif
+
+#ifdef __IMXRT1062__
+  #define DEFS_PROGMEM
+#else
+  #define DEFS_PROGMEM PROGMEM
+#endif
+
+inline float pgm_read_any(const float *p)   { return TERN(__IMXRT1062__, *p, pgm_read_float(p)); }
+inline int8_t pgm_read_any(const int8_t *p) { return TERN(__IMXRT1062__, *p, pgm_read_byte(p)); }
+
+#define XYZ_DEFS(T, NAME, OPT) \
+  inline T NAME(const AxisEnum axis) { \
+    static const XYZval<T> NAME##_P DEFS_PROGMEM = NUM_AXIS_ARRAY(X_##OPT, Y_##OPT, Z_##OPT, I_##OPT, J_##OPT, K_##OPT); \
+    return pgm_read_any(&NAME##_P[axis]); \
+  }
+XYZ_DEFS(float, base_min_pos,   MIN_POS);
+XYZ_DEFS(float, base_max_pos,   MAX_POS);
+XYZ_DEFS(float, base_home_pos,  HOME_POS);
+XYZ_DEFS(float, max_length,     MAX_LENGTH);
+XYZ_DEFS(int8_t, home_dir, HOME_DIR);
+
+inline float home_bump_mm(const AxisEnum axis) {
+  static const xyz_pos_t home_bump_mm_P DEFS_PROGMEM = HOMING_BUMP_MM;
+  return pgm_read_any(&home_bump_mm_P[axis]);
+}
+
+#if HAS_WORKSPACE_OFFSET
+  void update_workspace_offset(const AxisEnum axis);
+#else
+  inline void update_workspace_offset(const AxisEnum) {}
+#endif
+
+#if HAS_HOTEND_OFFSET
+  extern xyz_pos_t hotend_offset[HOTENDS];
+  void reset_hotend_offsets();
+#elif HOTENDS
+  constexpr xyz_pos_t hotend_offset[HOTENDS] = { { 0 } };
+#else
+  constexpr xyz_pos_t hotend_offset[1] = { { 0 } };
+#endif
+
+#if HAS_SOFTWARE_ENDSTOPS
+
+  typedef struct {
+    bool _enabled, _loose;
+    bool enabled() { return _enabled && !_loose; }
+
+    xyz_pos_t min, max;
+    void get_manual_axis_limits(const AxisEnum axis, float &amin, float &amax) {
+      amin = -100000; amax = 100000; // "No limits"
+      #if HAS_SOFTWARE_ENDSTOPS
+        if (enabled()) switch (axis) {
+          case X_AXIS:
+            TERN_(MIN_SOFTWARE_ENDSTOP_X, amin = min.x);
+            TERN_(MAX_SOFTWARE_ENDSTOP_X, amax = max.x);
+            break;
+          #if HAS_Y_AXIS
+            case Y_AXIS:
+              TERN_(MIN_SOFTWARE_ENDSTOP_Y, amin = min.y);
+              TERN_(MAX_SOFTWARE_ENDSTOP_Y, amax = max.y);
+              break;
+          #endif
+          #if HAS_Z_AXIS
+            case Z_AXIS:
+              TERN_(MIN_SOFTWARE_ENDSTOP_Z, amin = min.z);
+              TERN_(MAX_SOFTWARE_ENDSTOP_Z, amax = max.z);
+              break;
+          #endif
+          #if HAS_I_AXIS
+            case I_AXIS:
+              TERN_(MIN_SOFTWARE_ENDSTOP_I, amin = min.i);
+              TERN_(MIN_SOFTWARE_ENDSTOP_I, amax = max.i);
+              break;
+          #endif
+          #if HAS_J_AXIS
+            case J_AXIS:
+              TERN_(MIN_SOFTWARE_ENDSTOP_J, amin = min.j);
+              TERN_(MIN_SOFTWARE_ENDSTOP_J, amax = max.j);
+              break;
+          #endif
+          #if HAS_K_AXIS
+            case K_AXIS:
+              TERN_(MIN_SOFTWARE_ENDSTOP_K, amin = min.k);
+              TERN_(MIN_SOFTWARE_ENDSTOP_K, amax = max.k);
+              break;
+          #endif
+          default: break;
+        }
+      #endif
+    }
+  } soft_endstops_t;
+
+  extern soft_endstops_t soft_endstop;
+  void apply_motion_limits(xyz_pos_t &target);
+  void update_software_endstops(const AxisEnum axis
+    #if HAS_HOTEND_OFFSET
+      , const uint8_t old_tool_index=0, const uint8_t new_tool_index=0
+    #endif
+  );
+  #define SET_SOFT_ENDSTOP_LOOSE(loose) (soft_endstop._loose = loose)
+
+#else // !HAS_SOFTWARE_ENDSTOPS
+
+  typedef struct {
+    bool enabled() { return false; }
+    void get_manual_axis_limits(const AxisEnum axis, float &amin, float &amax) {
+      // No limits
+      amin = current_position[axis] - 1000;
+      amax = current_position[axis] + 1000;
+    }
+  } soft_endstops_t;
+  extern soft_endstops_t soft_endstop;
+  #define apply_motion_limits(V)        NOOP
+  #define update_software_endstops(...) NOOP
+  #define SET_SOFT_ENDSTOP_LOOSE(V)     NOOP
+
+#endif // !HAS_SOFTWARE_ENDSTOPS
+
+void report_real_position();
+void report_current_position();
+void report_current_position_projected();
+
+#if ENABLED(AUTO_REPORT_POSITION)
+  #include "../libs/autoreport.h"
+  struct PositionReport { static void report() { report_current_position_projected(); } };
+  extern AutoReporter<PositionReport> position_auto_reporter;
+#endif
+
+#if EITHER(FULL_REPORT_TO_HOST_FEATURE, REALTIME_REPORTING_COMMANDS)
+  #define HAS_GRBL_STATE 1
+  /**
+   * Machine states for GRBL or TinyG
+   */
+  enum M_StateEnum : uint8_t {
+    M_INIT = 0, //  0 machine is initializing
+    M_RESET,    //  1 machine is ready for use
+    M_ALARM,    //  2 machine is in alarm state (soft shut down)
+    M_IDLE,     //  3 program stop or no more blocks (M0, M1, M60)
+    M_END,      //  4 program end via M2, M30
+    M_RUNNING,  //  5 motion is running
+    M_HOLD,     //  6 motion is holding
+    M_PROBE,    //  7 probe cycle active
+    M_CYCLING,  //  8 machine is running (cycling)
+    M_HOMING,   //  9 machine is homing
+    M_JOGGING,  // 10 machine is jogging
+    M_ERROR     // 11 machine is in hard alarm state (shut down)
+  };
+  extern M_StateEnum M_State_grbl;
+  M_StateEnum grbl_state_for_marlin_state();
+  void report_current_grblstate_moving();
+  void report_current_position_moving();
+
+  #if ENABLED(FULL_REPORT_TO_HOST_FEATURE)
+    inline void set_and_report_grblstate(const M_StateEnum state, const bool force=true) {
+      if (force || M_State_grbl != state) {
+        M_State_grbl = state;
+        report_current_grblstate_moving();
+      }
+    }
+  #endif
+
+  #if ENABLED(REALTIME_REPORTING_COMMANDS)
+    void quickpause_stepper();
+    void quickresume_stepper();
+  #endif
+#endif
+
+void get_cartesian_from_steppers();
+void set_current_from_steppers_for_axis(const AxisEnum axis);
+
+void quickstop_stepper();
+
+/**
+ * Set the planner/stepper positions directly from current_position with
+ * no kinematic translation. Used for homing axes and cartesian/core syncing.
+ */
+void sync_plan_position();
+
+#if HAS_EXTRUDERS
+  void sync_plan_position_e();
+#endif
+
+/**
+ * Move the planner to the current position from wherever it last moved
+ * (or from wherever it has been told it is located).
+ */
+void line_to_current_position(const_feedRate_t fr_mm_s=feedrate_mm_s);
+
+#if HAS_EXTRUDERS
+  void unscaled_e_move(const_float_t length, const_feedRate_t fr_mm_s);
+#endif
+
+void prepare_line_to_destination();
+
+void _internal_move_to_destination(const_feedRate_t fr_mm_s=0.0f OPTARG(IS_KINEMATIC, const bool is_fast=false));
+
+inline void prepare_internal_move_to_destination(const_feedRate_t fr_mm_s=0.0f) {
+  _internal_move_to_destination(fr_mm_s);
+}
+
+#if IS_KINEMATIC
+  void prepare_fast_move_to_destination(const_feedRate_t scaled_fr_mm_s=MMS_SCALED(feedrate_mm_s));
+
+  inline void prepare_internal_fast_move_to_destination(const_feedRate_t fr_mm_s=0.0f) {
+    _internal_move_to_destination(fr_mm_s, true);
+  }
+#endif
+
+/**
+ * Blocking movement and shorthand functions
+ */
+void do_blocking_move_to(NUM_AXIS_ARGS(const float), const_feedRate_t fr_mm_s=0.0f);
+void do_blocking_move_to(const xy_pos_t &raw, const_feedRate_t fr_mm_s=0.0f);
+void do_blocking_move_to(const xyz_pos_t &raw, const_feedRate_t fr_mm_s=0.0f);
+void do_blocking_move_to(const xyze_pos_t &raw, const_feedRate_t fr_mm_s=0.0f);
+
+void do_blocking_move_to_x(const_float_t rx, const_feedRate_t fr_mm_s=0.0f);
+#if HAS_Y_AXIS
+  void do_blocking_move_to_y(const_float_t ry, const_feedRate_t fr_mm_s=0.0f);
+#endif
+#if HAS_Z_AXIS
+  void do_blocking_move_to_z(const_float_t rz, const_feedRate_t fr_mm_s=0.0f);
+#endif
+#if HAS_I_AXIS
+  void do_blocking_move_to_i(const_float_t ri, const_feedRate_t fr_mm_s=0.0f);
+  void do_blocking_move_to_xyz_i(const xyze_pos_t &raw, const_float_t i, const_feedRate_t fr_mm_s=0.0f);
+#endif
+#if HAS_J_AXIS
+  void do_blocking_move_to_j(const_float_t rj, const_feedRate_t fr_mm_s=0.0f);
+  void do_blocking_move_to_xyzi_j(const xyze_pos_t &raw, const_float_t j, const_feedRate_t fr_mm_s=0.0f);
+#endif
+#if HAS_K_AXIS
+  void do_blocking_move_to_k(const_float_t rk, const_feedRate_t fr_mm_s=0.0f);
+  void do_blocking_move_to_xyzij_k(const xyze_pos_t &raw, const_float_t k, const_feedRate_t fr_mm_s=0.0f);
+#endif
+
+#if HAS_Y_AXIS
+  void do_blocking_move_to_xy(const_float_t rx, const_float_t ry, const_feedRate_t fr_mm_s=0.0f);
+  void do_blocking_move_to_xy(const xy_pos_t &raw, const_feedRate_t fr_mm_s=0.0f);
+  FORCE_INLINE void do_blocking_move_to_xy(const xyz_pos_t &raw, const_feedRate_t fr_mm_s=0.0f)  { do_blocking_move_to_xy(xy_pos_t(raw), fr_mm_s); }
+  FORCE_INLINE void do_blocking_move_to_xy(const xyze_pos_t &raw, const_feedRate_t fr_mm_s=0.0f) { do_blocking_move_to_xy(xy_pos_t(raw), fr_mm_s); }
+#endif
+
+#if HAS_Z_AXIS
+  void do_blocking_move_to_xy_z(const xy_pos_t &raw, const_float_t z, const_feedRate_t fr_mm_s=0.0f);
+  FORCE_INLINE void do_blocking_move_to_xy_z(const xyz_pos_t &raw, const_float_t z, const_feedRate_t fr_mm_s=0.0f)  { do_blocking_move_to_xy_z(xy_pos_t(raw), z, fr_mm_s); }
+  FORCE_INLINE void do_blocking_move_to_xy_z(const xyze_pos_t &raw, const_float_t z, const_feedRate_t fr_mm_s=0.0f) { do_blocking_move_to_xy_z(xy_pos_t(raw), z, fr_mm_s); }
+#endif
+
+void remember_feedrate_and_scaling();
+void remember_feedrate_scaling_off();
+void restore_feedrate_and_scaling();
+
+#if HAS_Z_AXIS
+  void do_z_clearance(const_float_t zclear, const bool lower_allowed=false);
+#else
+  inline void do_z_clearance(float, bool=false) {}
+#endif
+
+/**
+ * Homing and Trusted Axes
+ */
+typedef IF<(NUM_AXES > 8), uint16_t, uint8_t>::type main_axes_bits_t;
+constexpr main_axes_bits_t main_axes_mask = _BV(NUM_AXES) - 1;
+
+typedef IF<(NUM_AXES + EXTRUDERS > 8), uint16_t, uint8_t>::type e_axis_bits_t;
+constexpr e_axis_bits_t e_axis_mask = (_BV(EXTRUDERS) - 1) << NUM_AXES;
+
+void set_axis_is_at_home(const AxisEnum axis);
+
+#if HAS_ENDSTOPS
+  /**
+   * axes_homed
+   *   Flags that each linear axis was homed.
+   *   XYZ on cartesian, ABC on delta, ABZ on SCARA.
+   *
+   * axes_trusted
+   *   Flags that the position is trusted in each linear axis. Set when homed.
+   *   Cleared whenever a stepper powers off, potentially losing its position.
+   */
+  extern main_axes_bits_t axes_homed, axes_trusted;
+  void homeaxis(const AxisEnum axis);
+  void set_axis_never_homed(const AxisEnum axis);
+  main_axes_bits_t axes_should_home(main_axes_bits_t axes_mask=main_axes_mask);
+  bool homing_needed_error(main_axes_bits_t axes_mask=main_axes_mask);
+  inline void set_axis_unhomed(const AxisEnum axis)   { CBI(axes_homed, axis); }
+  inline void set_axis_untrusted(const AxisEnum axis) { CBI(axes_trusted, axis); }
+  inline void set_all_unhomed()                       { axes_homed = axes_trusted = 0; }
+  inline void set_axis_homed(const AxisEnum axis)     { SBI(axes_homed, axis); }
+  inline void set_axis_trusted(const AxisEnum axis)   { SBI(axes_trusted, axis); }
+  inline void set_all_homed()                         { axes_homed = axes_trusted = main_axes_mask; }
+#else
+  constexpr main_axes_bits_t axes_homed = main_axes_mask, axes_trusted = main_axes_mask; // Zero-endstop machines are always homed and trusted
+  inline void homeaxis(const AxisEnum axis)           {}
+  inline void set_axis_never_homed(const AxisEnum)    {}
+  inline main_axes_bits_t axes_should_home(main_axes_bits_t=main_axes_mask) { return 0; }
+  inline bool homing_needed_error(main_axes_bits_t=main_axes_mask) { return false; }
+  inline void set_axis_unhomed(const AxisEnum axis)   {}
+  inline void set_axis_untrusted(const AxisEnum axis) {}
+  inline void set_all_unhomed()                       {}
+  inline void set_axis_homed(const AxisEnum axis)     {}
+  inline void set_axis_trusted(const AxisEnum axis)   {}
+  inline void set_all_homed()                         {}
+#endif
+
+inline bool axis_was_homed(const AxisEnum axis)       { return TEST(axes_homed, axis); }
+inline bool axis_is_trusted(const AxisEnum axis)      { return TEST(axes_trusted, axis); }
+inline bool axis_should_home(const AxisEnum axis)     { return (axes_should_home() & _BV(axis)) != 0; }
+inline bool no_axes_homed()                           { return !axes_homed; }
+inline bool all_axes_homed()                          { return main_axes_mask == (axes_homed & main_axes_mask); }
+inline bool homing_needed()                           { return !all_axes_homed(); }
+inline bool all_axes_trusted()                        { return main_axes_mask == (axes_trusted & main_axes_mask); }
+
+void home_if_needed(const bool keeplev=false);
+
+#if ENABLED(NO_MOTION_BEFORE_HOMING)
+  #define MOTION_CONDITIONS (IsRunning() && !homing_needed_error())
+#else
+  #define MOTION_CONDITIONS IsRunning()
+#endif
+
+#define BABYSTEP_ALLOWED() ((ENABLED(BABYSTEP_WITHOUT_HOMING) || all_axes_trusted()) && (ENABLED(BABYSTEP_ALWAYS_AVAILABLE) || printer_busy()))
+
+/**
+ * Workspace offsets
+ */
+#if HAS_HOME_OFFSET || HAS_POSITION_SHIFT
+  #if HAS_HOME_OFFSET
+    extern xyz_pos_t home_offset;
+  #endif
+  #if HAS_POSITION_SHIFT
+    extern xyz_pos_t position_shift;
+  #endif
+  #if HAS_HOME_OFFSET && HAS_POSITION_SHIFT
+    extern xyz_pos_t workspace_offset;
+    #define _WS workspace_offset
+  #elif HAS_HOME_OFFSET
+    #define _WS home_offset
+  #else
+    #define _WS position_shift
+  #endif
+  #define NATIVE_TO_LOGICAL(POS, AXIS) ((POS) + _WS[AXIS])
+  #define LOGICAL_TO_NATIVE(POS, AXIS) ((POS) - _WS[AXIS])
+  FORCE_INLINE void toLogical(xy_pos_t &raw)   { raw += _WS; }
+  FORCE_INLINE void toLogical(xyz_pos_t &raw)  { raw += _WS; }
+  FORCE_INLINE void toLogical(xyze_pos_t &raw) { raw += _WS; }
+  FORCE_INLINE void toNative(xy_pos_t &raw)    { raw -= _WS; }
+  FORCE_INLINE void toNative(xyz_pos_t &raw)   { raw -= _WS; }
+  FORCE_INLINE void toNative(xyze_pos_t &raw)  { raw -= _WS; }
+#else
+  #define NATIVE_TO_LOGICAL(POS, AXIS) (POS)
+  #define LOGICAL_TO_NATIVE(POS, AXIS) (POS)
+  FORCE_INLINE void toLogical(xy_pos_t&)   {}
+  FORCE_INLINE void toLogical(xyz_pos_t&)  {}
+  FORCE_INLINE void toLogical(xyze_pos_t&) {}
+  FORCE_INLINE void toNative(xy_pos_t&)    {}
+  FORCE_INLINE void toNative(xyz_pos_t&)   {}
+  FORCE_INLINE void toNative(xyze_pos_t&)  {}
+#endif
+#define LOGICAL_X_POSITION(POS) NATIVE_TO_LOGICAL(POS, X_AXIS)
+#define RAW_X_POSITION(POS)     LOGICAL_TO_NATIVE(POS, X_AXIS)
+#if HAS_Y_AXIS
+  #define LOGICAL_Y_POSITION(POS) NATIVE_TO_LOGICAL(POS, Y_AXIS)
+  #define RAW_Y_POSITION(POS)     LOGICAL_TO_NATIVE(POS, Y_AXIS)
+#endif
+#if HAS_Z_AXIS
+  #define LOGICAL_Z_POSITION(POS) NATIVE_TO_LOGICAL(POS, Z_AXIS)
+  #define RAW_Z_POSITION(POS)     LOGICAL_TO_NATIVE(POS, Z_AXIS)
+#endif
+#if HAS_I_AXIS
+  #define LOGICAL_I_POSITION(POS) NATIVE_TO_LOGICAL(POS, I_AXIS)
+  #define RAW_I_POSITION(POS)     LOGICAL_TO_NATIVE(POS, I_AXIS)
+#endif
+#if HAS_J_AXIS
+  #define LOGICAL_J_POSITION(POS) NATIVE_TO_LOGICAL(POS, J_AXIS)
+  #define RAW_J_POSITION(POS)     LOGICAL_TO_NATIVE(POS, J_AXIS)
+#endif
+#if HAS_K_AXIS
+  #define LOGICAL_K_POSITION(POS) NATIVE_TO_LOGICAL(POS, K_AXIS)
+  #define RAW_K_POSITION(POS)     LOGICAL_TO_NATIVE(POS, K_AXIS)
+#endif
+
+/**
+ * position_is_reachable family of functions
+ */
+#if IS_KINEMATIC // (DELTA or SCARA)
+
+  #if HAS_SCARA_OFFSET
+    extern abc_pos_t scara_home_offset; // A and B angular offsets, Z mm offset
+  #endif
+
+  // Return true if the given point is within the printable area
+  bool position_is_reachable(const_float_t rx, const_float_t ry, const float inset=0);
+
+  inline bool position_is_reachable(const xy_pos_t &pos, const float inset=0) {
+    return position_is_reachable(pos.x, pos.y, inset);
+  }
+
+#else
+
+  // Return true if the given position is within the machine bounds.
+  bool position_is_reachable(const_float_t rx, const_float_t ry);
+  inline bool position_is_reachable(const xy_pos_t &pos) {
+    return position_is_reachable(pos.x, pos.y);
+  }
+
+#endif
+
+/**
+ * Duplication mode
+ */
+#if HAS_DUPLICATION_MODE
+  extern bool extruder_duplication_enabled;       // Used in Dual X mode 2
+#endif
+
+/**
+ * Dual X Carriage
+ */
+#if ENABLED(DUAL_X_CARRIAGE)
+
+  enum DualXMode : char {
+    DXC_FULL_CONTROL_MODE,
+    DXC_AUTO_PARK_MODE,
+    DXC_DUPLICATION_MODE,
+    DXC_MIRRORED_MODE
+  };
+
+  extern DualXMode dual_x_carriage_mode;
+  extern float inactive_extruder_x,                 // Used in mode 0 & 1
+               duplicate_extruder_x_offset;         // Used in mode 2 & 3
+  extern xyz_pos_t raised_parked_position;          // Used in mode 1
+  extern bool active_extruder_parked;               // Used in mode 1, 2 & 3
+  extern millis_t delayed_move_time;                // Used in mode 1
+  extern celsius_t duplicate_extruder_temp_offset;  // Used in mode 2 & 3
+  extern bool idex_mirrored_mode;                   // Used in mode 3
+
+  FORCE_INLINE bool idex_is_duplicating() { return dual_x_carriage_mode >= DXC_DUPLICATION_MODE; }
+
+  float x_home_pos(const uint8_t extruder);
+
+  #define TOOL_X_HOME_DIR(T) ((T) ? X2_HOME_DIR : X_HOME_DIR)
+
+  void set_duplication_enabled(const bool dupe, const int8_t tool_index=-1);
+  void idex_set_mirrored_mode(const bool mirr);
+  void idex_set_parked(const bool park=true);
+
+#else
+
+  #if ENABLED(MULTI_NOZZLE_DUPLICATION)
+    extern uint8_t duplication_e_mask;
+    enum DualXMode : char { DXC_DUPLICATION_MODE = 2 };
+    FORCE_INLINE void set_duplication_enabled(const bool dupe) { extruder_duplication_enabled = dupe; }
+  #endif
+
+  #define TOOL_X_HOME_DIR(T) X_HOME_DIR
+
+#endif
+
+#if HAS_M206_COMMAND
+  void set_home_offset(const AxisEnum axis, const float v);
+#endif
+
+#if USE_SENSORLESS
+  struct sensorless_t;
+  sensorless_t start_sensorless_homing_per_axis(const AxisEnum axis);
+  void end_sensorless_homing_per_axis(const AxisEnum axis, sensorless_t enable_stealth);
+#endif

src/module/planner_bezier.cpp

@@ -0,0 +1,211 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * planner_bezier.cpp
+ *
+ * Compute and buffer movement commands for bezier curves
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(BEZIER_CURVE_SUPPORT)
+
+#include "planner.h"
+#include "motion.h"
+#include "temperature.h"
+
+#include "../MarlinCore.h"
+#include "../gcode/queue.h"
+
+// See the meaning in the documentation of cubic_b_spline().
+#define MIN_STEP 0.002f
+#define MAX_STEP 0.1f
+#define SIGMA 0.1f
+
+// Compute the linear interpolation between two real numbers.
+static inline float interp(const_float_t a, const_float_t b, const_float_t t) { return (1 - t) * a + t * b; }
+
+/**
+ * Compute a Bézier curve using the De Casteljau's algorithm (see
+ * https://en.wikipedia.org/wiki/De_Casteljau%27s_algorithm), which is
+ * easy to code and has good numerical stability (very important,
+ * since Arudino works with limited precision real numbers).
+ */
+static inline float eval_bezier(const_float_t a, const_float_t b, const_float_t c, const_float_t d, const_float_t t) {
+  const float iab = interp(a, b, t),
+              ibc = interp(b, c, t),
+              icd = interp(c, d, t),
+              iabc = interp(iab, ibc, t),
+              ibcd = interp(ibc, icd, t);
+  return interp(iabc, ibcd, t);
+}
+
+/**
+ * We approximate Euclidean distance with the sum of the coordinates
+ * offset (so-called "norm 1"), which is quicker to compute.
+ */
+static inline float dist1(const_float_t x1, const_float_t y1, const_float_t x2, const_float_t y2) { return ABS(x1 - x2) + ABS(y1 - y2); }
+
+/**
+ * The algorithm for computing the step is loosely based on the one in Kig
+ * (See https://sources.debian.net/src/kig/4:15.08.3-1/misc/kigpainter.cpp/#L759)
+ * However, we do not use the stack.
+ *
+ * The algorithm goes as it follows: the parameters t runs from 0.0 to
+ * 1.0 describing the curve, which is evaluated by eval_bezier(). At
+ * each iteration we have to choose a step, i.e., the increment of the
+ * t variable. By default the step of the previous iteration is taken,
+ * and then it is enlarged or reduced depending on how straight the
+ * curve locally is. The step is always clamped between MIN_STEP/2 and
+ * 2*MAX_STEP. MAX_STEP is taken at the first iteration.
+ *
+ * For some t, the step value is considered acceptable if the curve in
+ * the interval [t, t+step] is sufficiently straight, i.e.,
+ * sufficiently close to linear interpolation. In practice the
+ * following test is performed: the distance between eval_bezier(...,
+ * t+step/2) is evaluated and compared with 0.5*(eval_bezier(...,
+ * t)+eval_bezier(..., t+step)). If it is smaller than SIGMA, then the
+ * step value is considered acceptable, otherwise it is not. The code
+ * seeks to find the larger step value which is considered acceptable.
+ *
+ * At every iteration the recorded step value is considered and then
+ * iteratively halved until it becomes acceptable. If it was already
+ * acceptable in the beginning (i.e., no halving were done), then
+ * maybe it was necessary to enlarge it; then it is iteratively
+ * doubled while it remains acceptable. The last acceptable value
+ * found is taken, provided that it is between MIN_STEP and MAX_STEP
+ * and does not bring t over 1.0.
+ *
+ * Caveat: this algorithm is not perfect, since it can happen that a
+ * step is considered acceptable even when the curve is not linear at
+ * all in the interval [t, t+step] (but its mid point coincides "by
+ * chance" with the midpoint according to the parametrization). This
+ * kind of glitches can be eliminated with proper first derivative
+ * estimates; however, given the improbability of such configurations,
+ * the mitigation offered by MIN_STEP and the small computational
+ * power available on Arduino, I think it is not wise to implement it.
+ */
+void cubic_b_spline(
+  const xyze_pos_t &position,       // current position
+  const xyze_pos_t &target,         // target position
+  const xy_pos_t (&offsets)[2],     // a pair of offsets
+  const_feedRate_t scaled_fr_mm_s,  // mm/s scaled by feedrate %
+  const uint8_t extruder
+) {
+  // Absolute first and second control points are recovered.
+  const xy_pos_t first = position + offsets[0], second = target + offsets[1];
+
+  xyze_pos_t bez_target;
+  bez_target.set(position.x, position.y);
+  float step = MAX_STEP;
+
+  millis_t next_idle_ms = millis() + 200UL;
+
+  // Hints to help optimize the move
+  PlannerHints hints;
+
+  for (float t = 0; t < 1;) {
+
+    thermalManager.task();
+    millis_t now = millis();
+    if (ELAPSED(now, next_idle_ms)) {
+      next_idle_ms = now + 200UL;
+      idle();
+    }
+
+    // First try to reduce the step in order to make it sufficiently
+    // close to a linear interpolation.
+    bool did_reduce = false;
+    float new_t = t + step;
+    NOMORE(new_t, 1);
+    float new_pos0 = eval_bezier(position.x, first.x, second.x, target.x, new_t),
+          new_pos1 = eval_bezier(position.y, first.y, second.y, target.y, new_t);
+    for (;;) {
+      if (new_t - t < (MIN_STEP)) break;
+      const float candidate_t = 0.5f * (t + new_t),
+                  candidate_pos0 = eval_bezier(position.x, first.x, second.x, target.x, candidate_t),
+                  candidate_pos1 = eval_bezier(position.y, first.y, second.y, target.y, candidate_t),
+                  interp_pos0 = 0.5f * (bez_target.x + new_pos0),
+                  interp_pos1 = 0.5f * (bez_target.y + new_pos1);
+      if (dist1(candidate_pos0, candidate_pos1, interp_pos0, interp_pos1) <= (SIGMA)) break;
+      new_t = candidate_t;
+      new_pos0 = candidate_pos0;
+      new_pos1 = candidate_pos1;
+      did_reduce = true;
+    }
+
+    // If we did not reduce the step, maybe we should enlarge it.
+    if (!did_reduce) for (;;) {
+      if (new_t - t > MAX_STEP) break;
+      const float candidate_t = t + 2 * (new_t - t);
+      if (candidate_t >= 1) break;
+      const float candidate_pos0 = eval_bezier(position.x, first.x, second.x, target.x, candidate_t),
+                  candidate_pos1 = eval_bezier(position.y, first.y, second.y, target.y, candidate_t),
+                  interp_pos0 = 0.5f * (bez_target.x + candidate_pos0),
+                  interp_pos1 = 0.5f * (bez_target.y + candidate_pos1);
+      if (dist1(new_pos0, new_pos1, interp_pos0, interp_pos1) > (SIGMA)) break;
+      new_t = candidate_t;
+      new_pos0 = candidate_pos0;
+      new_pos1 = candidate_pos1;
+    }
+
+    // Check some postcondition; they are disabled in the actual
+    // Marlin build, but if you test the same code on a computer you
+    // may want to check they are respect.
+    /*
+      assert(new_t <= 1.0);
+      if (new_t < 1.0) {
+        assert(new_t - t >= (MIN_STEP) / 2.0);
+        assert(new_t - t <= (MAX_STEP) * 2.0);
+      }
+    */
+
+    hints.millimeters = new_t - t;
+    t = new_t;
+
+    // Compute and send new position
+    xyze_pos_t new_bez = LOGICAL_AXIS_ARRAY(
+      interp(position.e, target.e, t),  // FIXME. Wrong, since t is not linear in the distance.
+      new_pos0,
+      new_pos1,
+      interp(position.z, target.z, t),  // FIXME. Wrong, since t is not linear in the distance.
+      interp(position.i, target.i, t),  // FIXME. Wrong, since t is not linear in the distance.
+      interp(position.j, target.j, t),  // FIXME. Wrong, since t is not linear in the distance.
+      interp(position.k, target.k, t)   // FIXME. Wrong, since t is not linear in the distance.
+    );
+    apply_motion_limits(new_bez);
+    bez_target = new_bez;
+
+    #if HAS_LEVELING && !PLANNER_LEVELING
+      xyze_pos_t pos = bez_target;
+      planner.apply_leveling(pos);
+    #else
+      const xyze_pos_t &pos = bez_target;
+    #endif
+
+    if (!planner.buffer_line(pos, scaled_fr_mm_s, active_extruder, hints))
+      break;
+  }
+}
+
+#endif // BEZIER_CURVE_SUPPORT

src/module/planner_bezier.h

@@ -0,0 +1,38 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * planner_bezier.h
+ *
+ * Compute and buffer movement commands for Bézier curves
+ */
+
+#include "../core/types.h"
+
+void cubic_b_spline(
+  const xyze_pos_t &position,       // current position
+  const xyze_pos_t &target,         // target position
+  const xy_pos_t (&offsets)[2],     // a pair of offsets
+  const_feedRate_t scaled_fr_mm_s,  // mm/s scaled by feedrate %
+  const uint8_t extruder
+);

src/module/polargraph.cpp

@@ -0,0 +1,54 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * polargraph.cpp
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(POLARGRAPH)
+
+#include "polargraph.h"
+#include "motion.h"
+
+// For homing:
+#include "planner.h"
+#include "endstops.h"
+#include "../lcd/marlinui.h"
+#include "../MarlinCore.h"
+
+float segments_per_second; // Initialized by settings.load()
+
+xy_pos_t draw_area_min = { X_MIN_POS, Y_MIN_POS },
+         draw_area_max = { X_MAX_POS, Y_MAX_POS };
+
+xy_float_t draw_area_size = { X_MAX_POS - X_MIN_POS, Y_MAX_POS - Y_MIN_POS };
+
+float polargraph_max_belt_len = HYPOT(draw_area_size.x, draw_area_size.y);
+
+void inverse_kinematics(const xyz_pos_t &raw) {
+  const float x1 = raw.x - (draw_area_min.x), x2 = (draw_area_max.x) - raw.x, y = raw.y - (draw_area_max.y);
+  delta.set(HYPOT(x1, y), HYPOT(x2, y), raw.z);
+}
+
+#endif // POLARGRAPH

src/module/polargraph.h

@@ -0,0 +1,36 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * polargraph.h - Polargraph-specific functions
+ */
+
+#include "../core/types.h"
+#include "../core/macros.h"
+
+extern float segments_per_second;
+extern xy_pos_t draw_area_min, draw_area_max;
+extern xy_float_t draw_area_size;
+extern float polargraph_max_belt_len;
+
+void inverse_kinematics(const xyz_pos_t &raw);

src/module/printcounter.cpp

@@ -0,0 +1,359 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if DISABLED(PRINTCOUNTER)
+
+#include "../libs/stopwatch.h"
+Stopwatch print_job_timer;      // Global Print Job Timer instance
+
+#else // PRINTCOUNTER
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../lcd/extui/ui_api.h"
+#endif
+
+#include "printcounter.h"
+#include "../MarlinCore.h"
+#include "../HAL/shared/eeprom_api.h"
+
+#if HAS_SOUND && SERVICE_WARNING_BUZZES > 0
+  #include "../libs/buzzer.h"
+#endif
+
+#if PRINTCOUNTER_SYNC
+  #include "../module/planner.h"
+#endif
+
+// Service intervals
+#if HAS_SERVICE_INTERVALS
+  #if SERVICE_INTERVAL_1 > 0
+    #define SERVICE_INTERVAL_SEC_1   (3600UL * SERVICE_INTERVAL_1)
+  #else
+    #define SERVICE_INTERVAL_SEC_1   (3600UL * 100)
+  #endif
+  #if SERVICE_INTERVAL_2 > 0
+    #define SERVICE_INTERVAL_SEC_2   (3600UL * SERVICE_INTERVAL_2)
+  #else
+    #define SERVICE_INTERVAL_SEC_2   (3600UL * 100)
+  #endif
+  #if SERVICE_INTERVAL_3 > 0
+    #define SERVICE_INTERVAL_SEC_3   (3600UL * SERVICE_INTERVAL_3)
+  #else
+    #define SERVICE_INTERVAL_SEC_3   (3600UL * 100)
+  #endif
+#endif
+
+PrintCounter print_job_timer;   // Global Print Job Timer instance
+
+printStatistics PrintCounter::data;
+
+const PrintCounter::eeprom_address_t PrintCounter::address = STATS_EEPROM_ADDRESS;
+
+millis_t PrintCounter::lastDuration;
+bool PrintCounter::loaded = false;
+
+millis_t PrintCounter::deltaDuration() {
+  TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("deltaDuration")));
+  millis_t tmp = lastDuration;
+  lastDuration = duration();
+  return lastDuration - tmp;
+}
+
+#if HAS_EXTRUDERS
+  void PrintCounter::incFilamentUsed(float const &amount) {
+    TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("incFilamentUsed")));
+
+    // Refuses to update data if object is not loaded
+    if (!isLoaded()) return;
+
+    data.filamentUsed += amount; // mm
+  }
+#endif
+
+void PrintCounter::initStats() {
+  TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("initStats")));
+
+  loaded = true;
+
+  data = {
+      .totalPrints = 0
+    , .finishedPrints = 0
+    , .printTime = 0
+    , .longestPrint = 0
+    OPTARG(HAS_EXTRUDERS, .filamentUsed = 0.0)
+    #if SERVICE_INTERVAL_1 > 0
+      , .nextService1 = SERVICE_INTERVAL_SEC_1
+    #endif
+    #if SERVICE_INTERVAL_2 > 0
+      , .nextService2 = SERVICE_INTERVAL_SEC_2
+    #endif
+    #if SERVICE_INTERVAL_3 > 0
+      , .nextService3 = SERVICE_INTERVAL_SEC_3
+    #endif
+  };
+
+  saveStats();
+  persistentStore.access_start();
+  persistentStore.write_data(address, (uint8_t)0x16);
+  persistentStore.access_finish();
+}
+
+#if HAS_SERVICE_INTERVALS
+  inline void _print_divider() { SERIAL_ECHO_MSG("============================================="); }
+  inline bool _service_warn(const char * const msg) {
+    _print_divider();
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM_P(msg);
+    SERIAL_ECHOLNPGM("!");
+    _print_divider();
+    return true;
+  }
+#endif
+
+void PrintCounter::loadStats() {
+  TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("loadStats")));
+
+  // Check if the EEPROM block is initialized
+  uint8_t value = 0;
+  persistentStore.access_start();
+  persistentStore.read_data(address, &value, sizeof(uint8_t));
+  if (value != 0x16)
+    initStats();
+  else
+    persistentStore.read_data(address + sizeof(uint8_t), (uint8_t*)&data, sizeof(printStatistics));
+  persistentStore.access_finish();
+  loaded = true;
+
+  #if HAS_SERVICE_INTERVALS
+    bool doBuzz = false;
+    #if SERVICE_INTERVAL_1 > 0
+      if (data.nextService1 == 0) doBuzz = _service_warn(PSTR(" " SERVICE_NAME_1));
+    #endif
+    #if SERVICE_INTERVAL_2 > 0
+      if (data.nextService2 == 0) doBuzz = _service_warn(PSTR(" " SERVICE_NAME_2));
+    #endif
+    #if SERVICE_INTERVAL_3 > 0
+      if (data.nextService3 == 0) doBuzz = _service_warn(PSTR(" " SERVICE_NAME_3));
+    #endif
+    #if HAS_SOUND && SERVICE_WARNING_BUZZES > 0
+      if (doBuzz) for (int i = 0; i < SERVICE_WARNING_BUZZES; i++) { BUZZ(200, 404); BUZZ(10, 0); }
+    #else
+      UNUSED(doBuzz);
+    #endif
+  #endif // HAS_SERVICE_INTERVALS
+}
+
+void PrintCounter::saveStats() {
+  TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("saveStats")));
+
+  // Refuses to save data if object is not loaded
+  if (!isLoaded()) return;
+
+  TERN_(PRINTCOUNTER_SYNC, planner.synchronize());
+
+  // Saves the struct to EEPROM
+  persistentStore.access_start();
+  persistentStore.write_data(address + sizeof(uint8_t), (uint8_t*)&data, sizeof(printStatistics));
+  persistentStore.access_finish();
+
+  TERN_(EXTENSIBLE_UI, ExtUI::onSettingsStored(true));
+}
+
+#if HAS_SERVICE_INTERVALS
+  inline void _service_when(char buffer[], const char * const msg, const uint32_t when) {
+    SERIAL_ECHOPGM(STR_STATS);
+    SERIAL_ECHOPGM_P(msg);
+    SERIAL_ECHOLNPGM(" in ", duration_t(when).toString(buffer));
+  }
+#endif
+
+void PrintCounter::showStats() {
+  char buffer[22];
+
+  SERIAL_ECHOPGM(STR_STATS);
+  SERIAL_ECHOLNPGM(
+    "Prints: ", data.totalPrints,
+    ", Finished: ", data.finishedPrints,
+    ", Failed: ", data.totalPrints - data.finishedPrints
+                    - ((isRunning() || isPaused()) ? 1 : 0) // Remove 1 from failures with an active counter
+  );
+
+  SERIAL_ECHOPGM(STR_STATS);
+  duration_t elapsed = data.printTime;
+  elapsed.toString(buffer);
+  SERIAL_ECHOPGM("Total time: ", buffer);
+  #if ENABLED(DEBUG_PRINTCOUNTER)
+    SERIAL_ECHOPGM(" (", data.printTime);
+    SERIAL_CHAR(')');
+  #endif
+
+  elapsed = data.longestPrint;
+  elapsed.toString(buffer);
+  SERIAL_ECHOPGM(", Longest job: ", buffer);
+  #if ENABLED(DEBUG_PRINTCOUNTER)
+    SERIAL_ECHOPGM(" (", data.longestPrint);
+    SERIAL_CHAR(')');
+  #endif
+
+  #if HAS_EXTRUDERS
+    SERIAL_ECHOPGM("\n" STR_STATS "Filament used: ", data.filamentUsed / 1000);
+    SERIAL_CHAR('m');
+  #endif
+
+  SERIAL_EOL();
+
+  #if SERVICE_INTERVAL_1 > 0
+    _service_when(buffer, PSTR(SERVICE_NAME_1), data.nextService1);
+  #endif
+  #if SERVICE_INTERVAL_2 > 0
+    _service_when(buffer, PSTR(SERVICE_NAME_2), data.nextService2);
+  #endif
+  #if SERVICE_INTERVAL_3 > 0
+    _service_when(buffer, PSTR(SERVICE_NAME_3), data.nextService3);
+  #endif
+}
+
+void PrintCounter::tick() {
+  if (!isRunning()) return;
+
+  millis_t now = millis();
+
+  static millis_t update_next; // = 0
+  if (ELAPSED(now, update_next)) {
+    update_next = now + updateInterval;
+
+    TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("tick")));
+
+    millis_t delta = deltaDuration();
+    data.printTime += delta;
+
+    #if SERVICE_INTERVAL_1 > 0
+      data.nextService1 -= _MIN(delta, data.nextService1);
+    #endif
+    #if SERVICE_INTERVAL_2 > 0
+      data.nextService2 -= _MIN(delta, data.nextService2);
+    #endif
+    #if SERVICE_INTERVAL_3 > 0
+      data.nextService3 -= _MIN(delta, data.nextService3);
+    #endif
+  }
+
+  #if PRINTCOUNTER_SAVE_INTERVAL > 0
+    static millis_t eeprom_next; // = 0
+    if (ELAPSED(now, eeprom_next)) {
+      eeprom_next = now + saveInterval;
+      saveStats();
+    }
+  #endif
+}
+
+// @Override
+bool PrintCounter::start() {
+  TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("start")));
+
+  bool paused = isPaused();
+
+  if (super::start()) {
+    if (!paused) {
+      data.totalPrints++;
+      lastDuration = 0;
+    }
+    return true;
+  }
+
+  return false;
+}
+
+bool PrintCounter::_stop(const bool completed) {
+  TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("stop")));
+
+  const bool did_stop = super::stop();
+  if (did_stop) {
+    data.printTime += deltaDuration();
+    if (completed) {
+      data.finishedPrints++;
+      if (duration() > data.longestPrint)
+        data.longestPrint = duration();
+    }
+  }
+  saveStats();
+  return did_stop;
+}
+
+// @Override
+void PrintCounter::reset() {
+  TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("stop")));
+
+  super::reset();
+  lastDuration = 0;
+}
+
+#if HAS_SERVICE_INTERVALS
+
+  void PrintCounter::resetServiceInterval(const int index) {
+    switch (index) {
+      #if SERVICE_INTERVAL_1 > 0
+        case 1: data.nextService1 = SERVICE_INTERVAL_SEC_1;
+      #endif
+      #if SERVICE_INTERVAL_2 > 0
+        case 2: data.nextService2 = SERVICE_INTERVAL_SEC_2;
+      #endif
+      #if SERVICE_INTERVAL_3 > 0
+        case 3: data.nextService3 = SERVICE_INTERVAL_SEC_3;
+      #endif
+    }
+    saveStats();
+  }
+
+  bool PrintCounter::needsService(const int index) {
+    if (!loaded) loadStats();
+    switch (index) {
+      #if SERVICE_INTERVAL_1 > 0
+        case 1: return data.nextService1 == 0;
+      #endif
+      #if SERVICE_INTERVAL_2 > 0
+        case 2: return data.nextService2 == 0;
+      #endif
+      #if SERVICE_INTERVAL_3 > 0
+        case 3: return data.nextService3 == 0;
+      #endif
+      default: return false;
+    }
+  }
+
+#endif // HAS_SERVICE_INTERVALS
+
+#if ENABLED(DEBUG_PRINTCOUNTER)
+
+  void PrintCounter::debug(const char func[]) {
+    if (DEBUGGING(INFO)) {
+      SERIAL_ECHOPGM("PrintCounter::");
+      SERIAL_ECHOPGM_P(func);
+      SERIAL_ECHOLNPGM("()");
+    }
+  }
+
+#endif
+
+#endif // PRINTCOUNTER

src/module/printcounter.h

@@ -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/>.
+ *
+ */
+#pragma once
+
+#include "../libs/stopwatch.h"
+#include "../libs/duration_t.h"
+#include "../inc/MarlinConfig.h"
+
+// Print debug messages with M111 S2
+//#define DEBUG_PRINTCOUNTER
+
+// Round up I2C / SPI address to next page boundary (assuming 32 byte pages)
+#define STATS_EEPROM_ADDRESS TERN(USE_WIRED_EEPROM, 0x40, 0x32)
+
+struct printStatistics {    // 16 bytes
+  //const uint8_t magic;    // Magic header, it will always be 0x16
+  uint16_t totalPrints;     // Number of prints
+  uint16_t finishedPrints;  // Number of complete prints
+  uint32_t printTime;       // Accumulated printing time
+  uint32_t longestPrint;    // Longest successful print job
+  #if HAS_EXTRUDERS
+    float  filamentUsed;    // Accumulated filament consumed in mm
+  #endif
+  #if SERVICE_INTERVAL_1 > 0
+    uint32_t nextService1;  // Service intervals (or placeholders)
+  #endif
+  #if SERVICE_INTERVAL_2 > 0
+    uint32_t nextService2;
+  #endif
+  #if SERVICE_INTERVAL_3 > 0
+    uint32_t nextService3;
+  #endif
+};
+
+class PrintCounter: public Stopwatch {
+  private:
+    typedef Stopwatch super;
+    typedef IF<EITHER(USE_WIRED_EEPROM, CPU_32_BIT), uint32_t, uint16_t>::type eeprom_address_t;
+
+    static printStatistics data;
+
+    /**
+     * @brief EEPROM address
+     * @details Defines the start offset address where the data is stored.
+     */
+    static const eeprom_address_t address;
+
+    /**
+     * @brief Interval in seconds between counter updates
+     * @details This const value defines what will be the time between each
+     * accumulator update. This is different from the EEPROM save interval.
+     */
+    static constexpr millis_t updateInterval = SEC_TO_MS(10);
+
+    #if PRINTCOUNTER_SAVE_INTERVAL > 0
+      /**
+       * @brief Interval in seconds between EEPROM saves
+       * @details This const value defines what will be the time between each
+       * EEPROM save cycle, the development team recommends to set this value
+       * no lower than 3600 secs (1 hour).
+       */
+      static constexpr millis_t saveInterval = MIN_TO_MS(PRINTCOUNTER_SAVE_INTERVAL);
+    #endif
+
+    /**
+     * @brief Timestamp of the last call to deltaDuration()
+     * @details Store the timestamp of the last deltaDuration(), this is
+     * required due to the updateInterval cycle.
+     */
+    static millis_t lastDuration;
+
+    /**
+     * @brief Stats were loaded from EEPROM
+     * @details If set to true it indicates if the statistical data was already
+     * loaded from the EEPROM.
+     */
+    static bool loaded;
+
+  protected:
+    /**
+     * @brief dT since the last call
+     * @details Return the elapsed time in seconds since the last call, this is
+     * used internally for print statistics accounting is not intended to be a
+     * user callable function.
+     */
+    static millis_t deltaDuration();
+
+  public:
+
+    /**
+     * @brief Initialize the print counter
+     */
+    static void init() {
+      super::init();
+      loadStats();
+    }
+
+    /**
+     * @brief Check if Print Statistics has been loaded
+     * @details Return true if the statistical data has been loaded.
+     * @return bool
+     */
+    FORCE_INLINE static bool isLoaded() { return loaded; }
+
+    #if HAS_EXTRUDERS
+      /**
+       * @brief Increment the total filament used
+       * @details The total filament used counter will be incremented by "amount".
+       *
+       * @param amount The amount of filament used in mm
+       */
+      static void incFilamentUsed(float const &amount);
+    #endif
+
+    /**
+     * @brief Reset the Print Statistics
+     * @details Reset the statistics to zero and saves them to EEPROM creating
+     * also the magic header.
+     */
+    static void initStats();
+
+    /**
+     * @brief Load the Print Statistics
+     * @details Load the statistics from EEPROM
+     */
+    static void loadStats();
+
+    /**
+     * @brief Save the Print Statistics
+     * @details Save the statistics to EEPROM
+     */
+    static void saveStats();
+
+    /**
+     * @brief Serial output the Print Statistics
+     * @details This function may change in the future, for now it directly
+     * prints the statistical data to serial.
+     */
+    static void showStats();
+
+    /**
+     * @brief Return the currently loaded statistics
+     * @details Return the raw data, in the same structure used internally
+     */
+    static printStatistics getStats() { return data; }
+
+    /**
+     * @brief Loop function
+     * @details This function should be called at loop, it will take care of
+     * periodically save the statistical data to EEPROM and do time keeping.
+     */
+    static void tick();
+
+    /**
+     * The following functions are being overridden
+     */
+    static bool start();
+    static bool _stop(const bool completed);
+    static bool stop()  { return _stop(true);  }
+    static bool abort() { return _stop(false); }
+
+    static void reset();
+
+    #if HAS_SERVICE_INTERVALS
+      static void resetServiceInterval(const int index);
+      static bool needsService(const int index);
+    #endif
+
+    #if ENABLED(DEBUG_PRINTCOUNTER)
+
+      /**
+       * @brief Print a debug message
+       * @details Print a simple debug message
+       */
+      static void debug(const char func[]);
+
+    #endif
+};
+
+// Global Print Job Timer instance
+#if ENABLED(PRINTCOUNTER)
+  extern PrintCounter print_job_timer;
+#else
+  extern Stopwatch print_job_timer;
+#endif

src/module/probe.cpp

@@ -0,0 +1,941 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * module/probe.cpp
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if HAS_BED_PROBE
+
+#include "probe.h"
+
+#include "../libs/buzzer.h"
+#include "motion.h"
+#include "temperature.h"
+#include "endstops.h"
+
+#include "../gcode/gcode.h"
+#include "../lcd/marlinui.h"
+
+#include "../MarlinCore.h" // for stop(), disable_e_steppers(), wait_for_user_response()
+
+#if HAS_LEVELING
+  #include "../feature/bedlevel/bedlevel.h"
+#endif
+
+#if ENABLED(DELTA)
+  #include "delta.h"
+#endif
+
+#if EITHER(HAS_QUIET_PROBING, USE_SENSORLESS)
+  #include "stepper/indirection.h"
+  #if BOTH(HAS_QUIET_PROBING, PROBING_ESTEPPERS_OFF)
+    #include "stepper.h"
+  #endif
+  #if USE_SENSORLESS
+    #include "../feature/tmc_util.h"
+    #if ENABLED(IMPROVE_HOMING_RELIABILITY)
+      #include "planner.h"
+    #endif
+  #endif
+#endif
+
+#if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
+  #include "../feature/backlash.h"
+#endif
+
+#if ENABLED(BLTOUCH)
+  #include "../feature/bltouch.h"
+#endif
+
+#if ENABLED(HOST_PROMPT_SUPPORT)
+  #include "../feature/host_actions.h" // for PROMPT_USER_CONTINUE
+#endif
+
+#if HAS_Z_SERVO_PROBE
+  #include "servo.h"
+#endif
+
+#if HAS_PTC
+  #include "../feature/probe_temp_comp.h"
+#endif
+
+#if ENABLED(X_AXIS_TWIST_COMPENSATION)
+  #include "../feature/x_twist.h"
+#endif
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../lcd/extui/ui_api.h"
+#elif ENABLED(DWIN_LCD_PROUI)
+  #include "../lcd/e3v2/proui/dwin.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../core/debug_out.h"
+
+Probe probe;
+
+xyz_pos_t Probe::offset; // Initialized by settings.load()
+
+#if HAS_PROBE_XY_OFFSET
+  const xy_pos_t &Probe::offset_xy = Probe::offset;
+#endif
+
+#if ENABLED(SENSORLESS_PROBING)
+  Probe::sense_bool_t Probe::test_sensitivity = { true, true, true };
+#endif
+
+#if ENABLED(Z_PROBE_SLED)
+
+  #ifndef SLED_DOCKING_OFFSET
+    #define SLED_DOCKING_OFFSET 0
+  #endif
+
+  /**
+   * Method to dock/undock a sled designed by Charles Bell.
+   *
+   * stow[in]     If false, move to MAX_X and engage the solenoid
+   *              If true, move to MAX_X and release the solenoid
+   */
+  static void dock_sled(const bool stow) {
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("dock_sled(", stow, ")");
+
+    // Dock sled a bit closer to ensure proper capturing
+    do_blocking_move_to_x(X_MAX_POS + SLED_DOCKING_OFFSET - ((stow) ? 1 : 0));
+
+    #if HAS_SOLENOID_1 && DISABLED(EXT_SOLENOID)
+      WRITE(SOL1_PIN, !stow); // switch solenoid
+    #endif
+  }
+
+#elif ENABLED(MAGLEV4)
+
+  // Write trigger pin to release the probe
+  inline void maglev_deploy() {
+    WRITE(MAGLEV_TRIGGER_PIN, HIGH);
+    delay(MAGLEV_TRIGGER_DELAY);
+    WRITE(MAGLEV_TRIGGER_PIN, LOW);
+  }
+
+  inline void maglev_idle() { do_blocking_move_to_z(10); }
+
+#elif ENABLED(TOUCH_MI_PROBE)
+
+  // Move to the magnet to unlock the probe
+  inline void run_deploy_moves_script() {
+    #ifndef TOUCH_MI_DEPLOY_XPOS
+      #define TOUCH_MI_DEPLOY_XPOS X_MIN_POS
+    #elif TOUCH_MI_DEPLOY_XPOS > X_MAX_BED
+      TemporaryGlobalEndstopsState unlock_x(false);
+    #endif
+    #if TOUCH_MI_DEPLOY_YPOS > Y_MAX_BED
+      TemporaryGlobalEndstopsState unlock_y(false);
+    #endif
+
+    #if ENABLED(TOUCH_MI_MANUAL_DEPLOY)
+
+      const screenFunc_t prev_screen = ui.currentScreen;
+      LCD_MESSAGE(MSG_MANUAL_DEPLOY_TOUCHMI);
+      ui.return_to_status();
+
+      TERN_(HOST_PROMPT_SUPPORT, hostui.prompt_do(PROMPT_USER_CONTINUE, F("Deploy TouchMI"), FPSTR(CONTINUE_STR)));
+      TERN_(HAS_RESUME_CONTINUE, wait_for_user_response());
+      ui.reset_status();
+      ui.goto_screen(prev_screen);
+
+    #elif defined(TOUCH_MI_DEPLOY_XPOS) && defined(TOUCH_MI_DEPLOY_YPOS)
+      do_blocking_move_to_xy(TOUCH_MI_DEPLOY_XPOS, TOUCH_MI_DEPLOY_YPOS);
+    #elif defined(TOUCH_MI_DEPLOY_XPOS)
+      do_blocking_move_to_x(TOUCH_MI_DEPLOY_XPOS);
+    #elif defined(TOUCH_MI_DEPLOY_YPOS)
+      do_blocking_move_to_y(TOUCH_MI_DEPLOY_YPOS);
+    #endif
+  }
+
+  // Move down to the bed to stow the probe
+  inline void run_stow_moves_script() {
+    const xyz_pos_t oldpos = current_position;
+    endstops.enable_z_probe(false);
+    do_blocking_move_to_z(TOUCH_MI_RETRACT_Z, homing_feedrate(Z_AXIS));
+    do_blocking_move_to(oldpos, homing_feedrate(Z_AXIS));
+  }
+
+#elif ENABLED(Z_PROBE_ALLEN_KEY)
+
+  inline void run_deploy_moves_script() {
+    #ifdef Z_PROBE_ALLEN_KEY_DEPLOY_1
+      #ifndef Z_PROBE_ALLEN_KEY_DEPLOY_1_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_DEPLOY_1_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t deploy_1 = Z_PROBE_ALLEN_KEY_DEPLOY_1;
+      do_blocking_move_to(deploy_1, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_DEPLOY_1_FEEDRATE));
+    #endif
+    #ifdef Z_PROBE_ALLEN_KEY_DEPLOY_2
+      #ifndef Z_PROBE_ALLEN_KEY_DEPLOY_2_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_DEPLOY_2_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t deploy_2 = Z_PROBE_ALLEN_KEY_DEPLOY_2;
+      do_blocking_move_to(deploy_2, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_DEPLOY_2_FEEDRATE));
+    #endif
+    #ifdef Z_PROBE_ALLEN_KEY_DEPLOY_3
+      #ifndef Z_PROBE_ALLEN_KEY_DEPLOY_3_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_DEPLOY_3_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t deploy_3 = Z_PROBE_ALLEN_KEY_DEPLOY_3;
+      do_blocking_move_to(deploy_3, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_DEPLOY_3_FEEDRATE));
+    #endif
+    #ifdef Z_PROBE_ALLEN_KEY_DEPLOY_4
+      #ifndef Z_PROBE_ALLEN_KEY_DEPLOY_4_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_DEPLOY_4_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t deploy_4 = Z_PROBE_ALLEN_KEY_DEPLOY_4;
+      do_blocking_move_to(deploy_4, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_DEPLOY_4_FEEDRATE));
+    #endif
+    #ifdef Z_PROBE_ALLEN_KEY_DEPLOY_5
+      #ifndef Z_PROBE_ALLEN_KEY_DEPLOY_5_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_DEPLOY_5_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t deploy_5 = Z_PROBE_ALLEN_KEY_DEPLOY_5;
+      do_blocking_move_to(deploy_5, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_DEPLOY_5_FEEDRATE));
+    #endif
+  }
+
+  inline void run_stow_moves_script() {
+    #ifdef Z_PROBE_ALLEN_KEY_STOW_1
+      #ifndef Z_PROBE_ALLEN_KEY_STOW_1_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_STOW_1_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t stow_1 = Z_PROBE_ALLEN_KEY_STOW_1;
+      do_blocking_move_to(stow_1, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_STOW_1_FEEDRATE));
+    #endif
+    #ifdef Z_PROBE_ALLEN_KEY_STOW_2
+      #ifndef Z_PROBE_ALLEN_KEY_STOW_2_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_STOW_2_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t stow_2 = Z_PROBE_ALLEN_KEY_STOW_2;
+      do_blocking_move_to(stow_2, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_STOW_2_FEEDRATE));
+    #endif
+    #ifdef Z_PROBE_ALLEN_KEY_STOW_3
+      #ifndef Z_PROBE_ALLEN_KEY_STOW_3_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_STOW_3_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t stow_3 = Z_PROBE_ALLEN_KEY_STOW_3;
+      do_blocking_move_to(stow_3, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_STOW_3_FEEDRATE));
+    #endif
+    #ifdef Z_PROBE_ALLEN_KEY_STOW_4
+      #ifndef Z_PROBE_ALLEN_KEY_STOW_4_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_STOW_4_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t stow_4 = Z_PROBE_ALLEN_KEY_STOW_4;
+      do_blocking_move_to(stow_4, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_STOW_4_FEEDRATE));
+    #endif
+    #ifdef Z_PROBE_ALLEN_KEY_STOW_5
+      #ifndef Z_PROBE_ALLEN_KEY_STOW_5_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_STOW_5_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t stow_5 = Z_PROBE_ALLEN_KEY_STOW_5;
+      do_blocking_move_to(stow_5, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_STOW_5_FEEDRATE));
+    #endif
+  }
+
+#endif // Z_PROBE_ALLEN_KEY
+
+#if HAS_QUIET_PROBING
+
+  #ifndef DELAY_BEFORE_PROBING
+    #define DELAY_BEFORE_PROBING 25
+  #endif
+
+  void Probe::set_probing_paused(const bool dopause) {
+    TERN_(PROBING_HEATERS_OFF, thermalManager.pause_heaters(dopause));
+    TERN_(PROBING_FANS_OFF, thermalManager.set_fans_paused(dopause));
+    TERN_(PROBING_ESTEPPERS_OFF, if (dopause) stepper.disable_e_steppers());
+    #if ENABLED(PROBING_STEPPERS_OFF) && DISABLED(DELTA)
+      static uint8_t old_trusted;
+      if (dopause) {
+        old_trusted = axes_trusted;
+        stepper.disable_axis(X_AXIS);
+        stepper.disable_axis(Y_AXIS);
+      }
+      else {
+        if (TEST(old_trusted, X_AXIS)) stepper.enable_axis(X_AXIS);
+        if (TEST(old_trusted, Y_AXIS)) stepper.enable_axis(Y_AXIS);
+        axes_trusted = old_trusted;
+      }
+    #endif
+    if (dopause) safe_delay(_MAX(DELAY_BEFORE_PROBING, 25));
+  }
+
+#endif // HAS_QUIET_PROBING
+
+/**
+ * Raise Z to a minimum height to make room for a probe to move
+ */
+void Probe::do_z_raise(const float z_raise) {
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Probe::do_z_raise(", z_raise, ")");
+  float z_dest = z_raise;
+  if (offset.z < 0) z_dest -= offset.z;
+  do_z_clearance(z_dest);
+}
+
+FORCE_INLINE void probe_specific_action(const bool deploy) {
+  #if ENABLED(PAUSE_BEFORE_DEPLOY_STOW)
+    do {
+      #if ENABLED(PAUSE_PROBE_DEPLOY_WHEN_TRIGGERED)
+        if (deploy != PROBE_TRIGGERED()) break;
+      #endif
+
+      OKAY_BUZZ();
+
+      FSTR_P const ds_str = deploy ? GET_TEXT_F(MSG_MANUAL_DEPLOY) : GET_TEXT_F(MSG_MANUAL_STOW);
+      ui.return_to_status();       // To display the new status message
+      ui.set_status(ds_str, 99);
+      SERIAL_ECHOLNF(deploy ? GET_EN_TEXT_F(MSG_MANUAL_DEPLOY) : GET_EN_TEXT_F(MSG_MANUAL_STOW));
+
+      TERN_(HOST_PROMPT_SUPPORT, hostui.prompt_do(PROMPT_USER_CONTINUE, ds_str, FPSTR(CONTINUE_STR)));
+      TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired(ds_str));
+      TERN_(DWIN_LCD_PROUI, DWIN_Popup_Confirm(ICON_BLTouch, ds_str, FPSTR(CONTINUE_STR)));
+      TERN_(HAS_RESUME_CONTINUE, wait_for_user_response());
+      ui.reset_status();
+
+    } while (ENABLED(PAUSE_PROBE_DEPLOY_WHEN_TRIGGERED));
+
+  #endif // PAUSE_BEFORE_DEPLOY_STOW
+
+  #if ENABLED(SOLENOID_PROBE)
+
+    #if HAS_SOLENOID_1
+      WRITE(SOL1_PIN, deploy);
+    #endif
+
+  #elif ENABLED(MAGLEV4)
+
+    deploy ? maglev_deploy() : maglev_idle();
+
+  #elif ENABLED(Z_PROBE_SLED)
+
+    dock_sled(!deploy);
+
+  #elif ENABLED(BLTOUCH)
+
+    deploy ? bltouch.deploy() : bltouch.stow();
+
+  #elif HAS_Z_SERVO_PROBE
+
+    servo[Z_PROBE_SERVO_NR].move(servo_angles[Z_PROBE_SERVO_NR][deploy ? 0 : 1]);
+
+  #elif EITHER(TOUCH_MI_PROBE, Z_PROBE_ALLEN_KEY)
+
+    deploy ? run_deploy_moves_script() : run_stow_moves_script();
+
+  #elif ENABLED(RACK_AND_PINION_PROBE)
+
+    do_blocking_move_to_x(deploy ? Z_PROBE_DEPLOY_X : Z_PROBE_RETRACT_X);
+
+  #elif DISABLED(PAUSE_BEFORE_DEPLOY_STOW)
+
+    UNUSED(deploy);
+
+  #endif
+}
+
+#if EITHER(PREHEAT_BEFORE_PROBING, PREHEAT_BEFORE_LEVELING)
+
+  #if ENABLED(PREHEAT_BEFORE_PROBING)
+    #ifndef PROBING_NOZZLE_TEMP
+      #define PROBING_NOZZLE_TEMP 0
+    #endif
+    #ifndef PROBING_BED_TEMP
+      #define PROBING_BED_TEMP 0
+    #endif
+  #endif
+
+  /**
+   * Do preheating as required before leveling or probing.
+   *  - If a preheat input is higher than the current target, raise the target temperature.
+   *  - If a preheat input is higher than the current temperature, wait for stabilization.
+   */
+  void Probe::preheat_for_probing(const celsius_t hotend_temp, const celsius_t bed_temp) {
+    #if HAS_HOTEND && (PROBING_NOZZLE_TEMP || LEVELING_NOZZLE_TEMP)
+      #define WAIT_FOR_NOZZLE_HEAT
+    #endif
+    #if HAS_HEATED_BED && (PROBING_BED_TEMP || LEVELING_BED_TEMP)
+      #define WAIT_FOR_BED_HEAT
+    #endif
+
+    LCD_MESSAGE(MSG_PREHEATING);
+
+    DEBUG_ECHOPGM("Preheating ");
+
+    #if ENABLED(WAIT_FOR_NOZZLE_HEAT)
+      const celsius_t hotendPreheat = hotend_temp > thermalManager.degTargetHotend(0) ? hotend_temp : 0;
+      if (hotendPreheat) {
+        DEBUG_ECHOPGM("hotend (", hotendPreheat, ")");
+        thermalManager.setTargetHotend(hotendPreheat, 0);
+      }
+    #elif ENABLED(WAIT_FOR_BED_HEAT)
+      constexpr celsius_t hotendPreheat = 0;
+    #endif
+
+    #if ENABLED(WAIT_FOR_BED_HEAT)
+      const celsius_t bedPreheat = bed_temp > thermalManager.degTargetBed() ? bed_temp : 0;
+      if (bedPreheat) {
+        if (hotendPreheat) DEBUG_ECHOPGM(" and ");
+        DEBUG_ECHOPGM("bed (", bedPreheat, ")");
+        thermalManager.setTargetBed(bedPreheat);
+      }
+    #endif
+
+    DEBUG_EOL();
+
+    TERN_(WAIT_FOR_NOZZLE_HEAT, if (hotend_temp > thermalManager.wholeDegHotend(0) + (TEMP_WINDOW)) thermalManager.wait_for_hotend(0));
+    TERN_(WAIT_FOR_BED_HEAT,    if (bed_temp    > thermalManager.wholeDegBed() + (TEMP_BED_WINDOW)) thermalManager.wait_for_bed_heating());
+  }
+
+#endif
+
+/**
+ * Print an error and stop()
+ */
+void Probe::probe_error_stop() {
+  SERIAL_ERROR_START();
+  SERIAL_ECHOPGM(STR_STOP_PRE);
+  #if EITHER(Z_PROBE_SLED, Z_PROBE_ALLEN_KEY)
+    SERIAL_ECHOPGM(STR_STOP_UNHOMED);
+  #elif ENABLED(BLTOUCH)
+    SERIAL_ECHOPGM(STR_STOP_BLTOUCH);
+  #endif
+  SERIAL_ECHOLNPGM(STR_STOP_POST);
+  stop();
+}
+
+/**
+ * Attempt to deploy or stow the probe
+ *
+ * Return TRUE if the probe could not be deployed/stowed
+ */
+bool Probe::set_deployed(const bool deploy) {
+
+  if (DEBUGGING(LEVELING)) {
+    DEBUG_POS("Probe::set_deployed", current_position);
+    DEBUG_ECHOLNPGM("deploy: ", deploy);
+  }
+
+  if (endstops.z_probe_enabled == deploy) return false;
+
+  // Make room for probe to deploy (or stow)
+  // Fix-mounted probe should only raise for deploy
+  // unless PAUSE_BEFORE_DEPLOY_STOW is enabled
+  #if EITHER(FIX_MOUNTED_PROBE, NOZZLE_AS_PROBE) && DISABLED(PAUSE_BEFORE_DEPLOY_STOW)
+    const bool z_raise_wanted = deploy;
+  #else
+    constexpr bool z_raise_wanted = true;
+  #endif
+
+  if (z_raise_wanted)
+    do_z_raise(_MAX(Z_CLEARANCE_BETWEEN_PROBES, Z_CLEARANCE_DEPLOY_PROBE));
+
+  #if EITHER(Z_PROBE_SLED, Z_PROBE_ALLEN_KEY)
+    if (homing_needed_error(TERN_(Z_PROBE_SLED, _BV(X_AXIS)))) {
+      probe_error_stop();
+      return true;
+    }
+  #endif
+
+  const xy_pos_t old_xy = current_position;
+
+  #if ENABLED(PROBE_TRIGGERED_WHEN_STOWED_TEST)
+
+    // Only deploy/stow if needed
+    if (PROBE_TRIGGERED() == deploy) {
+      if (!deploy) endstops.enable_z_probe(false); // Switch off triggered when stowed probes early
+                                                   // otherwise an Allen-Key probe can't be stowed.
+      probe_specific_action(deploy);
+    }
+
+    if (PROBE_TRIGGERED() == deploy) {             // Unchanged after deploy/stow action?
+      if (IsRunning()) {
+        SERIAL_ERROR_MSG("Z-Probe failed");
+        LCD_ALERTMESSAGE_F("Err: ZPROBE");
+      }
+      stop();
+      return true;
+    }
+
+  #else
+
+    probe_specific_action(deploy);
+
+  #endif
+
+  // If preheating is required before any probing...
+  TERN_(PREHEAT_BEFORE_PROBING, if (deploy) preheat_for_probing(PROBING_NOZZLE_TEMP, PROBING_BED_TEMP));
+
+  do_blocking_move_to(old_xy);
+  endstops.enable_z_probe(deploy);
+  return false;
+}
+
+/**
+ * @brief Move down until the probe triggers or the low limit is reached
+ *        Used by run_z_probe to do a single Z probe move.
+ *
+ * @param  z        Z destination
+ * @param  fr_mm_s  Feedrate in mm/s
+ * @return true to indicate an error
+ *
+ * @details Used by run_z_probe to get each bed Z height measurement.
+ *          Sets current_position.z to the height where the probe triggered
+ *          (according to the Z stepper count). The float Z is propagated
+ *          back to the planner.position to preempt any rounding error.
+ *
+ * @return TRUE if the probe failed to trigger.
+ */
+bool Probe::probe_down_to_z(const_float_t z, const_feedRate_t fr_mm_s) {
+  DEBUG_SECTION(log_probe, "Probe::probe_down_to_z", DEBUGGING(LEVELING));
+
+  #if BOTH(HAS_HEATED_BED, WAIT_FOR_BED_HEATER)
+    thermalManager.wait_for_bed_heating();
+  #endif
+
+  #if BOTH(HAS_TEMP_HOTEND, WAIT_FOR_HOTEND)
+    thermalManager.wait_for_hotend_heating(active_extruder);
+  #endif
+  #if ENABLED(BLTOUCH)
+    if (!bltouch.high_speed_mode && bltouch.deploy())
+      return true; // Deploy in LOW SPEED MODE on every probe action
+  #endif
+
+  // Disable stealthChop if used. Enable diag1 pin on driver.
+  #if ENABLED(SENSORLESS_PROBING)
+    sensorless_t stealth_states { false };
+    #if HAS_DELTA_SENSORLESS_PROBING
+      if (test_sensitivity.x) stealth_states.x = tmc_enable_stallguard(stepperX); // Delta watches all DIAG pins for a stall
+      if (test_sensitivity.y) stealth_states.y = tmc_enable_stallguard(stepperY);
+    #endif
+    if (test_sensitivity.z) stealth_states.z = tmc_enable_stallguard(stepperZ);   // All machines will check Z-DIAG for stall
+    endstops.set_homing_current(true);                                            // The "homing" current also applies to probing
+    endstops.enable(true);
+  #endif
+
+  TERN_(HAS_QUIET_PROBING, set_probing_paused(true));
+
+  // Move down until the probe is triggered
+  do_blocking_move_to_z(z, fr_mm_s);
+
+  // Check to see if the probe was triggered
+  const bool probe_triggered =
+    #if HAS_DELTA_SENSORLESS_PROBING
+      endstops.trigger_state() & (_BV(X_MAX) | _BV(Y_MAX) | _BV(Z_MAX))
+    #else
+      TEST(endstops.trigger_state(), Z_MIN_PROBE)
+    #endif
+  ;
+
+  // Offset sensorless probing
+  #if HAS_DELTA_SENSORLESS_PROBING
+    if (probe_triggered) probe.refresh_largest_sensorless_adj();
+  #endif
+
+  TERN_(HAS_QUIET_PROBING, set_probing_paused(false));
+
+  // Re-enable stealthChop if used. Disable diag1 pin on driver.
+  #if ENABLED(SENSORLESS_PROBING)
+    endstops.not_homing();
+    #if HAS_DELTA_SENSORLESS_PROBING
+      if (test_sensitivity.x) tmc_disable_stallguard(stepperX, stealth_states.x);
+      if (test_sensitivity.y) tmc_disable_stallguard(stepperY, stealth_states.y);
+    #endif
+    if (test_sensitivity.z) tmc_disable_stallguard(stepperZ, stealth_states.z);
+    endstops.set_homing_current(false);
+  #endif
+
+  #if ENABLED(BLTOUCH)
+    if (probe_triggered && !bltouch.high_speed_mode && bltouch.stow())
+      return true; // Stow in LOW SPEED MODE on every trigger
+  #endif
+
+  // Clear endstop flags
+  endstops.hit_on_purpose();
+
+  // Get Z where the steppers were interrupted
+  set_current_from_steppers_for_axis(Z_AXIS);
+
+  // Tell the planner where we actually are
+  sync_plan_position();
+
+  return !probe_triggered;
+}
+
+#if ENABLED(PROBE_TARE)
+
+  /**
+   * @brief Init the tare pin
+   *
+   * @details Init tare pin to ON state for a strain gauge, otherwise OFF
+   */
+  void Probe::tare_init() {
+    OUT_WRITE(PROBE_TARE_PIN, !PROBE_TARE_STATE);
+  }
+
+  /**
+   * @brief Tare the Z probe
+   *
+   * @details Signal to the probe to tare itself
+   *
+   * @return TRUE if the tare cold not be completed
+   */
+  bool Probe::tare() {
+    #if BOTH(PROBE_ACTIVATION_SWITCH, PROBE_TARE_ONLY_WHILE_INACTIVE)
+      if (endstops.probe_switch_activated()) {
+        SERIAL_ECHOLNPGM("Cannot tare an active probe");
+        return true;
+      }
+    #endif
+
+    SERIAL_ECHOLNPGM("Taring probe");
+    WRITE(PROBE_TARE_PIN, PROBE_TARE_STATE);
+    delay(PROBE_TARE_TIME);
+    WRITE(PROBE_TARE_PIN, !PROBE_TARE_STATE);
+    delay(PROBE_TARE_DELAY);
+
+    endstops.hit_on_purpose();
+    return false;
+  }
+#endif
+
+/**
+ * @brief Probe at the current XY (possibly more than once) to find the bed Z.
+ *
+ * @details Used by probe_at_point to get the bed Z height at the current XY.
+ *          Leaves current_position.z at the height where the probe triggered.
+ *
+ * @return The Z position of the bed at the current XY or NAN on error.
+ */
+float Probe::run_z_probe(const bool sanity_check/*=true*/) {
+  DEBUG_SECTION(log_probe, "Probe::run_z_probe", DEBUGGING(LEVELING));
+
+  auto try_to_probe = [&](PGM_P const plbl, const_float_t z_probe_low_point, const feedRate_t fr_mm_s, const bool scheck, const float clearance) -> bool {
+    // Tare the probe, if supported
+    if (TERN0(PROBE_TARE, tare())) return true;
+
+    // Do a first probe at the fast speed
+    const bool probe_fail = probe_down_to_z(z_probe_low_point, fr_mm_s),            // No probe trigger?
+               early_fail = (scheck && current_position.z > -offset.z + clearance); // Probe triggered too high?
+    #if ENABLED(DEBUG_LEVELING_FEATURE)
+      if (DEBUGGING(LEVELING) && (probe_fail || early_fail)) {
+        DEBUG_ECHOPGM_P(plbl);
+        DEBUG_ECHOPGM(" Probe fail! -");
+        if (probe_fail) DEBUG_ECHOPGM(" No trigger.");
+        if (early_fail) DEBUG_ECHOPGM(" Triggered early.");
+        DEBUG_EOL();
+      }
+    #else
+      UNUSED(plbl);
+    #endif
+    return probe_fail || early_fail;
+  };
+
+  // Stop the probe before it goes too low to prevent damage.
+  // If Z isn't known then probe to -10mm.
+  const float z_probe_low_point = axis_is_trusted(Z_AXIS) ? -offset.z + Z_PROBE_LOW_POINT : -10.0;
+
+  // Double-probing does a fast probe followed by a slow probe
+  #if TOTAL_PROBING == 2
+
+    // Attempt to tare the probe
+    if (TERN0(PROBE_TARE, tare())) return NAN;
+
+    // Do a first probe at the fast speed
+    if (try_to_probe(PSTR("FAST"), z_probe_low_point, z_probe_fast_mm_s,
+                     sanity_check, Z_CLEARANCE_BETWEEN_PROBES) ) return NAN;
+
+    const float first_probe_z = DIFF_TERN(HAS_DELTA_SENSORLESS_PROBING, current_position.z, largest_sensorless_adj);
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("1st Probe Z:", first_probe_z);
+
+    // Raise to give the probe clearance
+    do_blocking_move_to_z(current_position.z + Z_CLEARANCE_MULTI_PROBE, z_probe_fast_mm_s);
+
+  #elif Z_PROBE_FEEDRATE_FAST != Z_PROBE_FEEDRATE_SLOW
+
+    // If the nozzle is well over the travel height then
+    // move down quickly before doing the slow probe
+    const float z = Z_CLEARANCE_DEPLOY_PROBE + 5.0 + (offset.z < 0 ? -offset.z : 0);
+    if (current_position.z > z) {
+      // Probe down fast. If the probe never triggered, raise for probe clearance
+      if (!probe_down_to_z(z, z_probe_fast_mm_s))
+        do_blocking_move_to_z(current_position.z + Z_CLEARANCE_BETWEEN_PROBES, z_probe_fast_mm_s);
+    }
+  #endif
+
+  #if EXTRA_PROBING > 0
+    float probes[TOTAL_PROBING];
+  #endif
+
+  #if TOTAL_PROBING > 2
+    float probes_z_sum = 0;
+    for (
+      #if EXTRA_PROBING > 0
+        uint8_t p = 0; p < TOTAL_PROBING; p++
+      #else
+        uint8_t p = TOTAL_PROBING; p--;
+      #endif
+    )
+  #endif
+    {
+      // If the probe won't tare, return
+      if (TERN0(PROBE_TARE, tare())) return true;
+
+      // Probe downward slowly to find the bed
+      if (try_to_probe(PSTR("SLOW"), z_probe_low_point, MMM_TO_MMS(Z_PROBE_FEEDRATE_SLOW),
+                       sanity_check, Z_CLEARANCE_MULTI_PROBE) ) return NAN;
+
+      TERN_(MEASURE_BACKLASH_WHEN_PROBING, backlash.measure_with_probe());
+
+      const float z = DIFF_TERN(HAS_DELTA_SENSORLESS_PROBING, current_position.z, largest_sensorless_adj);
+
+      #if EXTRA_PROBING > 0
+        // Insert Z measurement into probes[]. Keep it sorted ascending.
+        LOOP_LE_N(i, p) {                            // Iterate the saved Zs to insert the new Z
+          if (i == p || probes[i] > z) {                              // Last index or new Z is smaller than this Z
+            for (int8_t m = p; --m >= i;) probes[m + 1] = probes[m];  // Shift items down after the insertion point
+            probes[i] = z;                                            // Insert the new Z measurement
+            break;                                                    // Only one to insert. Done!
+          }
+        }
+      #elif TOTAL_PROBING > 2
+        probes_z_sum += z;
+      #else
+        UNUSED(z);
+      #endif
+
+      #if TOTAL_PROBING > 2
+        // Small Z raise after all but the last probe
+        if (p
+          #if EXTRA_PROBING > 0
+            < TOTAL_PROBING - 1
+          #endif
+        ) do_blocking_move_to_z(z + Z_CLEARANCE_MULTI_PROBE, z_probe_fast_mm_s);
+      #endif
+    }
+
+  #if TOTAL_PROBING > 2
+
+    #if EXTRA_PROBING > 0
+      // Take the center value (or average the two middle values) as the median
+      static constexpr int PHALF = (TOTAL_PROBING - 1) / 2;
+      const float middle = probes[PHALF],
+                  median = ((TOTAL_PROBING) & 1) ? middle : (middle + probes[PHALF + 1]) * 0.5f;
+
+      // Remove values farthest from the median
+      uint8_t min_avg_idx = 0, max_avg_idx = TOTAL_PROBING - 1;
+      for (uint8_t i = EXTRA_PROBING; i--;)
+        if (ABS(probes[max_avg_idx] - median) > ABS(probes[min_avg_idx] - median))
+          max_avg_idx--; else min_avg_idx++;
+
+      // Return the average value of all remaining probes.
+      LOOP_S_LE_N(i, min_avg_idx, max_avg_idx)
+        probes_z_sum += probes[i];
+
+    #endif
+
+    const float measured_z = probes_z_sum * RECIPROCAL(MULTIPLE_PROBING);
+
+  #elif TOTAL_PROBING == 2
+
+    const float z2 = DIFF_TERN(HAS_DELTA_SENSORLESS_PROBING, current_position.z, largest_sensorless_adj);
+
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("2nd Probe Z:", z2, " Discrepancy:", first_probe_z - z2);
+
+    // Return a weighted average of the fast and slow probes
+    const float measured_z = (z2 * 3.0 + first_probe_z * 2.0) * 0.2;
+
+  #else
+
+    // Return the single probe result
+    const float measured_z = current_position.z;
+
+  #endif
+
+  return measured_z;
+}
+
+/**
+ * - Move to the given XY
+ * - Deploy the probe, if not already deployed
+ * - Probe the bed, get the Z position
+ * - Depending on the 'stow' flag
+ *   - Stow the probe, or
+ *   - Raise to the BETWEEN height
+ * - Return the probed Z position
+ */
+float Probe::probe_at_point(const_float_t rx, const_float_t ry, const ProbePtRaise raise_after/*=PROBE_PT_NONE*/, const uint8_t verbose_level/*=0*/, const bool probe_relative/*=true*/, const bool sanity_check/*=true*/) {
+  DEBUG_SECTION(log_probe, "Probe::probe_at_point", DEBUGGING(LEVELING));
+
+  if (DEBUGGING(LEVELING)) {
+    DEBUG_ECHOLNPGM(
+      "...(", LOGICAL_X_POSITION(rx), ", ", LOGICAL_Y_POSITION(ry),
+      ", ", raise_after == PROBE_PT_RAISE ? "raise" : raise_after == PROBE_PT_LAST_STOW ? "stow (last)" : raise_after == PROBE_PT_STOW ? "stow" : "none",
+      ", ", verbose_level,
+      ", ", probe_relative ? "probe" : "nozzle", "_relative)"
+    );
+    DEBUG_POS("", current_position);
+  }
+
+  #if ENABLED(BLTOUCH)
+    if (bltouch.high_speed_mode && bltouch.triggered())
+      bltouch._reset();
+  #endif
+
+  // On delta keep Z below clip height or do_blocking_move_to will abort
+  xyz_pos_t npos = NUM_AXIS_ARRAY(
+    rx, ry, TERN(DELTA, _MIN(delta_clip_start_height, current_position.z), current_position.z),
+    current_position.i, current_position.j, current_position.k
+  );
+  if (!can_reach(npos, probe_relative)) {
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Position Not Reachable");
+    return NAN;
+  }
+  if (probe_relative) npos -= offset_xy;  // Get the nozzle position
+
+  // Move the probe to the starting XYZ
+  do_blocking_move_to(npos, feedRate_t(XY_PROBE_FEEDRATE_MM_S));
+
+  float measured_z = NAN;
+  if (!deploy()) {
+    measured_z = run_z_probe(sanity_check) + offset.z;
+    TERN_(HAS_PTC, ptc.apply_compensation(measured_z));
+    TERN_(X_AXIS_TWIST_COMPENSATION, measured_z += xatc.compensation(npos + offset_xy));
+  }
+  if (!isnan(measured_z)) {
+    const bool big_raise = raise_after == PROBE_PT_BIG_RAISE;
+    if (big_raise || raise_after == PROBE_PT_RAISE)
+      do_blocking_move_to_z(current_position.z + (big_raise ? 25 : Z_CLEARANCE_BETWEEN_PROBES), z_probe_fast_mm_s);
+    else if (raise_after == PROBE_PT_STOW || raise_after == PROBE_PT_LAST_STOW)
+      if (stow()) measured_z = NAN;   // Error on stow?
+
+    if (verbose_level > 2)
+      SERIAL_ECHOLNPGM("Bed X: ", LOGICAL_X_POSITION(rx), " Y: ", LOGICAL_Y_POSITION(ry), " Z: ", measured_z);
+  }
+
+  if (isnan(measured_z)) {
+    stow();
+    LCD_MESSAGE(MSG_LCD_PROBING_FAILED);
+    #if DISABLED(G29_RETRY_AND_RECOVER)
+      SERIAL_ERROR_MSG(STR_ERR_PROBING_FAILED);
+    #endif
+  }
+  DEBUG_ECHOLNPGM("measured_z: ", measured_z);
+  return measured_z;
+}
+
+#if HAS_Z_SERVO_PROBE
+
+  void Probe::servo_probe_init() {
+    /**
+     * Set position of Z Servo Endstop
+     *
+     * The servo might be deployed and positioned too low to stow
+     * when starting up the machine or rebooting the board.
+     * There's no way to know where the nozzle is positioned until
+     * homing has been done - no homing with z-probe without init!
+     */
+    STOW_Z_SERVO();
+  }
+
+#endif // HAS_Z_SERVO_PROBE
+
+#if USE_SENSORLESS
+
+  sensorless_t stealth_states { false };
+
+  /**
+   * Disable stealthChop if used. Enable diag1 pin on driver.
+   */
+  void Probe::enable_stallguard_diag1() {
+    #if ENABLED(SENSORLESS_PROBING)
+      #if HAS_DELTA_SENSORLESS_PROBING
+        stealth_states.x = tmc_enable_stallguard(stepperX);
+        stealth_states.y = tmc_enable_stallguard(stepperY);
+      #endif
+      stealth_states.z = tmc_enable_stallguard(stepperZ);
+      endstops.enable(true);
+    #endif
+  }
+
+  /**
+   * Re-enable stealthChop if used. Disable diag1 pin on driver.
+   */
+  void Probe::disable_stallguard_diag1() {
+    #if ENABLED(SENSORLESS_PROBING)
+      endstops.not_homing();
+      #if HAS_DELTA_SENSORLESS_PROBING
+        tmc_disable_stallguard(stepperX, stealth_states.x);
+        tmc_disable_stallguard(stepperY, stealth_states.y);
+      #endif
+      tmc_disable_stallguard(stepperZ, stealth_states.z);
+    #endif
+  }
+
+  /**
+   * Set the sensorless Z offset
+   */
+  void Probe::set_offset_sensorless_adj(const_float_t sz) {
+    #if ENABLED(SENSORLESS_PROBING)
+      DEBUG_SECTION(pso, "Probe::set_offset_sensorless_adj", true);
+      #if HAS_DELTA_SENSORLESS_PROBING
+        if (test_sensitivity.x) offset_sensorless_adj.a = sz;
+        if (test_sensitivity.y) offset_sensorless_adj.b = sz;
+      #endif
+      if (test_sensitivity.z) offset_sensorless_adj.c = sz;
+    #endif
+  }
+
+  /**
+   * Refresh largest_sensorless_adj based on triggered endstops
+   */
+  void Probe::refresh_largest_sensorless_adj() {
+    #if ENABLED(SENSORLESS_PROBING)
+      DEBUG_SECTION(rso, "Probe::refresh_largest_sensorless_adj", true);
+      largest_sensorless_adj = -3;                                             // A reference away from any real probe height
+      #if HAS_DELTA_SENSORLESS_PROBING
+        if (TEST(endstops.state(), X_MAX)) {
+          NOLESS(largest_sensorless_adj, offset_sensorless_adj.a);
+          DEBUG_ECHOLNPGM("Endstop_X: ", largest_sensorless_adj, " TowerX");
+        }
+        if (TEST(endstops.state(), Y_MAX)) {
+          NOLESS(largest_sensorless_adj, offset_sensorless_adj.b);
+          DEBUG_ECHOLNPGM("Endstop_Y: ", largest_sensorless_adj, " TowerY");
+        }
+      #endif
+      if (TEST(endstops.state(), Z_MAX)) {
+        NOLESS(largest_sensorless_adj, offset_sensorless_adj.c);
+        DEBUG_ECHOLNPGM("Endstop_Z: ", largest_sensorless_adj, " TowerZ");
+      }
+    #endif
+  }
+
+#endif // SENSORLESS_PROBING || SENSORLESS_HOMING
+
+#endif // HAS_BED_PROBE

src/module/probe.h

@@ -0,0 +1,318 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * module/probe.h - Move, deploy, enable, etc.
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#include "motion.h"
+
+#if HAS_BED_PROBE
+  enum ProbePtRaise : uint8_t {
+    PROBE_PT_NONE,      // No raise or stow after run_z_probe
+    PROBE_PT_STOW,      // Do a complete stow after run_z_probe
+    PROBE_PT_LAST_STOW, // Stow for sure, even in BLTouch HS mode
+    PROBE_PT_RAISE,     // Raise to "between" clearance after run_z_probe
+    PROBE_PT_BIG_RAISE  // Raise to big clearance after run_z_probe
+  };
+#endif
+
+#if USES_Z_MIN_PROBE_PIN
+  #define PROBE_TRIGGERED() (READ(Z_MIN_PROBE_PIN) != Z_MIN_PROBE_ENDSTOP_INVERTING)
+#else
+  #define PROBE_TRIGGERED() (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING)
+#endif
+
+#ifdef Z_AFTER_HOMING
+   #define Z_POST_CLEARANCE Z_AFTER_HOMING
+#elif defined(Z_HOMING_HEIGHT)
+   #define Z_POST_CLEARANCE Z_HOMING_HEIGHT
+#else
+   #define Z_POST_CLEARANCE 10
+#endif
+
+#if ENABLED(PREHEAT_BEFORE_LEVELING)
+  #ifndef LEVELING_NOZZLE_TEMP
+    #define LEVELING_NOZZLE_TEMP 0
+  #endif
+  #ifndef LEVELING_BED_TEMP
+    #define LEVELING_BED_TEMP 0
+  #endif
+#endif
+
+class Probe {
+public:
+
+  #if ENABLED(SENSORLESS_PROBING)
+    typedef struct {
+      #if HAS_DELTA_SENSORLESS_PROBING
+        bool x:1, y:1, z:1;
+      #else
+        bool z;
+      #endif
+    } sense_bool_t;
+    static sense_bool_t test_sensitivity;
+  #endif
+
+  #if HAS_BED_PROBE
+
+    static xyz_pos_t offset;
+
+    #if EITHER(PREHEAT_BEFORE_PROBING, PREHEAT_BEFORE_LEVELING)
+      static void preheat_for_probing(const celsius_t hotend_temp, const celsius_t bed_temp);
+    #endif
+
+    static void probe_error_stop();
+
+    static bool set_deployed(const bool deploy);
+
+    #if IS_KINEMATIC
+
+      #if HAS_PROBE_XY_OFFSET
+        // Return true if the both nozzle and the probe can reach the given point.
+        // Note: This won't work on SCARA since the probe offset rotates with the arm.
+        static bool can_reach(const_float_t rx, const_float_t ry, const bool probe_relative=true) {
+          if (probe_relative) {
+            return position_is_reachable(rx - offset_xy.x, ry - offset_xy.y) // The nozzle can go where it needs to go?
+                && position_is_reachable(rx, ry, PROBING_MARGIN);            // Can the probe also go near there?
+          }
+          else {
+            return position_is_reachable(rx, ry)
+                && position_is_reachable(rx + offset_xy.x, ry + offset_xy.y, PROBING_MARGIN);
+          }
+        }
+      #else
+        static bool can_reach(const_float_t rx, const_float_t ry, const bool=true) {
+          return position_is_reachable(rx, ry)
+              && position_is_reachable(rx, ry, PROBING_MARGIN);
+        }
+      #endif
+
+    #else
+
+      /**
+       * Return whether the given position is within the bed, and whether the nozzle
+       * can reach the position required to put the probe at the given position.
+       *
+       * Example: For a probe offset of -10,+10, then for the probe to reach 0,0 the
+       *          nozzle must be be able to reach +10,-10.
+       */
+      static bool can_reach(const_float_t rx, const_float_t ry, const bool probe_relative=true) {
+        if (probe_relative) {
+          return position_is_reachable(rx - offset_xy.x, ry - offset_xy.y)
+              && COORDINATE_OKAY(rx, min_x() - fslop, max_x() + fslop)
+              && COORDINATE_OKAY(ry, min_y() - fslop, max_y() + fslop);
+        }
+        else {
+          return position_is_reachable(rx, ry)
+              && COORDINATE_OKAY(rx + offset_xy.x, min_x() - fslop, max_x() + fslop)
+              && COORDINATE_OKAY(ry + offset_xy.y, min_y() - fslop, max_y() + fslop);
+        }
+      }
+
+    #endif
+
+    static void move_z_after_probing() {
+      #ifdef Z_AFTER_PROBING
+        do_z_clearance(Z_AFTER_PROBING, true); // Move down still permitted
+      #endif
+    }
+    static float probe_at_point(const_float_t rx, const_float_t ry, const ProbePtRaise raise_after=PROBE_PT_NONE, const uint8_t verbose_level=0, const bool probe_relative=true, const bool sanity_check=true);
+    static float probe_at_point(const xy_pos_t &pos, const ProbePtRaise raise_after=PROBE_PT_NONE, const uint8_t verbose_level=0, const bool probe_relative=true, const bool sanity_check=true) {
+      return probe_at_point(pos.x, pos.y, raise_after, verbose_level, probe_relative, sanity_check);
+    }
+
+  #else
+
+    static constexpr xyz_pos_t offset = xyz_pos_t(NUM_AXIS_ARRAY(0, 0, 0, 0, 0, 0)); // See #16767
+
+    static bool set_deployed(const bool) { return false; }
+
+    static bool can_reach(const_float_t rx, const_float_t ry, const bool=true) { return position_is_reachable(rx, ry); }
+
+  #endif
+
+  static void move_z_after_homing() {
+    #ifdef Z_AFTER_HOMING
+      do_z_clearance(Z_AFTER_HOMING, true);
+    #elif BOTH(Z_AFTER_PROBING, HAS_BED_PROBE)
+      move_z_after_probing();
+    #endif
+  }
+
+  static bool can_reach(const xy_pos_t &pos, const bool probe_relative=true) { return can_reach(pos.x, pos.y, probe_relative); }
+
+  static bool good_bounds(const xy_pos_t &lf, const xy_pos_t &rb) {
+    return (
+      #if IS_KINEMATIC
+        can_reach(lf.x, 0) && can_reach(rb.x, 0) && can_reach(0, lf.y) && can_reach(0, rb.y)
+      #else
+        can_reach(lf) && can_reach(rb)
+      #endif
+    );
+  }
+
+  // Use offset_xy for read only access
+  // More optimal the XY offset is known to always be zero.
+  #if HAS_PROBE_XY_OFFSET
+    static const xy_pos_t &offset_xy;
+  #else
+    static constexpr xy_pos_t offset_xy = xy_pos_t({ 0, 0 });   // See #16767
+  #endif
+
+  static bool deploy() { return set_deployed(true); }
+  static bool stow()   { return set_deployed(false); }
+
+  #if HAS_BED_PROBE || HAS_LEVELING
+    #if IS_KINEMATIC
+      static constexpr float printable_radius = (
+        TERN_(DELTA, DELTA_PRINTABLE_RADIUS)
+        TERN_(IS_SCARA, SCARA_PRINTABLE_RADIUS)
+      );
+      static constexpr float probe_radius(const xy_pos_t &probe_offset_xy=offset_xy) {
+        return printable_radius - _MAX(PROBING_MARGIN, HYPOT(probe_offset_xy.x, probe_offset_xy.y));
+      }
+    #endif
+
+    /**
+     * The nozzle is only able to move within the physical bounds of the machine.
+     * If the PROBE has an OFFSET Marlin may need to apply additional limits so
+     * the probe can be prevented from going to unreachable points.
+     *
+     * e.g., If the PROBE is to the LEFT of the NOZZLE, it will be limited in how
+     * close it can get the RIGHT edge of the bed (unless the nozzle is able move
+     * far enough past the right edge).
+     */
+    static constexpr float _min_x(const xy_pos_t &probe_offset_xy=offset_xy) {
+      return TERN(IS_KINEMATIC,
+        (X_CENTER) - probe_radius(probe_offset_xy),
+        _MAX((X_MIN_BED) + (PROBING_MARGIN_LEFT), (X_MIN_POS) + probe_offset_xy.x)
+      );
+    }
+    static constexpr float _max_x(const xy_pos_t &probe_offset_xy=offset_xy) {
+      return TERN(IS_KINEMATIC,
+        (X_CENTER) + probe_radius(probe_offset_xy),
+        _MIN((X_MAX_BED) - (PROBING_MARGIN_RIGHT), (X_MAX_POS) + probe_offset_xy.x)
+      );
+    }
+    static constexpr float _min_y(const xy_pos_t &probe_offset_xy=offset_xy) {
+      return TERN(IS_KINEMATIC,
+        (Y_CENTER) - probe_radius(probe_offset_xy),
+        _MAX((Y_MIN_BED) + (PROBING_MARGIN_FRONT), (Y_MIN_POS) + probe_offset_xy.y)
+      );
+    }
+    static constexpr float _max_y(const xy_pos_t &probe_offset_xy=offset_xy) {
+      return TERN(IS_KINEMATIC,
+        (Y_CENTER) + probe_radius(probe_offset_xy),
+        _MIN((Y_MAX_BED) - (PROBING_MARGIN_BACK), (Y_MAX_POS) + probe_offset_xy.y)
+      );
+    }
+
+    static float min_x() { return _min_x() TERN_(NOZZLE_AS_PROBE, TERN_(HAS_HOME_OFFSET, - home_offset.x)); }
+    static float max_x() { return _max_x() TERN_(NOZZLE_AS_PROBE, TERN_(HAS_HOME_OFFSET, - home_offset.x)); }
+    static float min_y() { return _min_y() TERN_(NOZZLE_AS_PROBE, TERN_(HAS_HOME_OFFSET, - home_offset.y)); }
+    static float max_y() { return _max_y() TERN_(NOZZLE_AS_PROBE, TERN_(HAS_HOME_OFFSET, - home_offset.y)); }
+
+    // constexpr helpers used in build-time static_asserts, relying on default probe offsets.
+    class build_time {
+      static constexpr xyz_pos_t default_probe_xyz_offset = xyz_pos_t(
+        #if HAS_BED_PROBE
+          NOZZLE_TO_PROBE_OFFSET
+        #else
+          { 0 }
+        #endif
+      );
+      static constexpr xy_pos_t default_probe_xy_offset = xy_pos_t({ default_probe_xyz_offset.x,  default_probe_xyz_offset.y });
+
+    public:
+      static constexpr bool can_reach(float x, float y) {
+        #if IS_KINEMATIC
+          return HYPOT2(x, y) <= sq(probe_radius(default_probe_xy_offset));
+        #else
+          return COORDINATE_OKAY(x, _min_x(default_probe_xy_offset) - fslop, _max_x(default_probe_xy_offset) + fslop)
+              && COORDINATE_OKAY(y, _min_y(default_probe_xy_offset) - fslop, _max_y(default_probe_xy_offset) + fslop);
+        #endif
+      }
+
+      static constexpr bool can_reach(const xy_pos_t &point) { return can_reach(point.x, point.y); }
+    };
+
+    #if NEEDS_THREE_PROBE_POINTS
+      // Retrieve three points to probe the bed. Any type exposing set(X,Y) may be used.
+      template <typename T>
+      static void get_three_points(T points[3]) {
+        #if HAS_FIXED_3POINT
+          #define VALIDATE_PROBE_PT(N) static_assert(Probe::build_time::can_reach(xy_pos_t{PROBE_PT_##N##_X, PROBE_PT_##N##_Y}), \
+            "PROBE_PT_" STRINGIFY(N) "_(X|Y) is unreachable using default NOZZLE_TO_PROBE_OFFSET and PROBING_MARGIN");
+          VALIDATE_PROBE_PT(1); VALIDATE_PROBE_PT(2); VALIDATE_PROBE_PT(3);
+          points[0] = xy_float_t({ PROBE_PT_1_X, PROBE_PT_1_Y });
+          points[1] = xy_float_t({ PROBE_PT_2_X, PROBE_PT_2_Y });
+          points[2] = xy_float_t({ PROBE_PT_3_X, PROBE_PT_3_Y });
+        #else
+          #if IS_KINEMATIC
+            constexpr float SIN0 = 0.0, SIN120 = 0.866025, SIN240 = -0.866025,
+                            COS0 = 1.0, COS120 = -0.5    , COS240 = -0.5;
+            points[0] = xy_float_t({ (X_CENTER) + probe_radius() * COS0,   (Y_CENTER) + probe_radius() * SIN0 });
+            points[1] = xy_float_t({ (X_CENTER) + probe_radius() * COS120, (Y_CENTER) + probe_radius() * SIN120 });
+            points[2] = xy_float_t({ (X_CENTER) + probe_radius() * COS240, (Y_CENTER) + probe_radius() * SIN240 });
+          #else
+            points[0] = xy_float_t({ min_x(), min_y() });
+            points[1] = xy_float_t({ max_x(), min_y() });
+            points[2] = xy_float_t({ (min_x() + max_x()) / 2, max_y() });
+          #endif
+        #endif
+      }
+    #endif
+
+  #endif // HAS_BED_PROBE
+
+  #if HAS_Z_SERVO_PROBE
+    static void servo_probe_init();
+  #endif
+
+  #if HAS_QUIET_PROBING
+    static void set_probing_paused(const bool p);
+  #endif
+
+  #if ENABLED(PROBE_TARE)
+    static void tare_init();
+    static bool tare();
+  #endif
+
+  // Basic functions for Sensorless Homing and Probing
+  #if USE_SENSORLESS
+    static void enable_stallguard_diag1();
+    static void disable_stallguard_diag1();
+    static void set_offset_sensorless_adj(const_float_t sz);
+    static void refresh_largest_sensorless_adj();
+  #endif
+
+private:
+  static bool probe_down_to_z(const_float_t z, const_feedRate_t fr_mm_s);
+  static void do_z_raise(const float z_raise);
+  static float run_z_probe(const bool sanity_check=true);
+};
+
+extern Probe probe;

src/module/scara.cpp

@@ -0,0 +1,309 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * scara.cpp
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if IS_SCARA
+
+#include "scara.h"
+#include "motion.h"
+#include "planner.h"
+
+#if ENABLED(AXEL_TPARA)
+  #include "endstops.h"
+  #include "../MarlinCore.h"
+#endif
+
+float segments_per_second = TERN(AXEL_TPARA, TPARA_SEGMENTS_PER_SECOND, SCARA_SEGMENTS_PER_SECOND);
+
+#if EITHER(MORGAN_SCARA, MP_SCARA)
+
+  static constexpr xy_pos_t scara_offset = { SCARA_OFFSET_X, SCARA_OFFSET_Y };
+
+  /**
+   * Morgan SCARA Forward Kinematics. Results in 'cartes'.
+   * Maths and first version by QHARLEY.
+   * Integrated into Marlin and slightly restructured by Joachim Cerny.
+   */
+  void forward_kinematics(const_float_t a, const_float_t b) {
+    const float a_sin = sin(RADIANS(a)) * L1,
+                a_cos = cos(RADIANS(a)) * L1,
+                b_sin = sin(RADIANS(SUM_TERN(MP_SCARA, b, a))) * L2,
+                b_cos = cos(RADIANS(SUM_TERN(MP_SCARA, b, a))) * L2;
+
+    cartes.x = a_cos + b_cos + scara_offset.x;  // theta
+    cartes.y = a_sin + b_sin + scara_offset.y;  // phi
+
+    /*
+      DEBUG_ECHOLNPGM(
+        "SCARA FK Angle a=", a,
+        " b=", b,
+        " a_sin=", a_sin,
+        " a_cos=", a_cos,
+        " b_sin=", b_sin,
+        " b_cos=", b_cos
+      );
+      DEBUG_ECHOLNPGM(" cartes (X,Y) = "(cartes.x, ", ", cartes.y, ")");
+    //*/
+  }
+
+#endif
+
+#if ENABLED(MORGAN_SCARA)
+
+  void scara_set_axis_is_at_home(const AxisEnum axis) {
+    if (axis == Z_AXIS)
+      current_position.z = Z_HOME_POS;
+    else {
+      // MORGAN_SCARA uses a Cartesian XY home position
+      xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
+      //DEBUG_ECHOLNPGM_P(PSTR("homeposition X"), homeposition.x, SP_Y_LBL, homeposition.y);
+
+      delta = homeposition;
+      forward_kinematics(delta.a, delta.b);
+      current_position[axis] = cartes[axis];
+
+      //DEBUG_ECHOLNPGM_P(PSTR("Cartesian X"), current_position.x, SP_Y_LBL, current_position.y);
+      update_software_endstops(axis);
+    }
+  }
+
+  /**
+   * Morgan SCARA Inverse Kinematics. Results are stored in 'delta'.
+   *
+   * See https://reprap.org/forum/read.php?185,283327
+   *
+   * Maths and first version by QHARLEY.
+   * Integrated into Marlin and slightly restructured by Joachim Cerny.
+   */
+  void inverse_kinematics(const xyz_pos_t &raw) {
+    float C2, S2, SK1, SK2, THETA, PSI;
+
+    // Translate SCARA to standard XY with scaling factor
+    const xy_pos_t spos = raw - scara_offset;
+
+    const float H2 = HYPOT2(spos.x, spos.y);
+    if (L1 == L2)
+      C2 = H2 / L1_2_2 - 1;
+    else
+      C2 = (H2 - (L1_2 + L2_2)) / (2.0f * L1 * L2);
+
+    LIMIT(C2, -1, 1);
+
+    S2 = SQRT(1.0f - sq(C2));
+
+    // Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End
+    SK1 = L1 + L2 * C2;
+
+    // Rotated Arm2 gives the distance from Arm1 to Arm2
+    SK2 = L2 * S2;
+
+    // Angle of Arm1 is the difference between Center-to-End angle and the Center-to-Elbow
+    THETA = ATAN2(SK1, SK2) - ATAN2(spos.x, spos.y);
+
+    // Angle of Arm2
+    PSI = ATAN2(S2, C2);
+
+    delta.set(DEGREES(THETA), DEGREES(SUM_TERN(MORGAN_SCARA, PSI, THETA)), raw.z);
+
+    /*
+      DEBUG_POS("SCARA IK", raw);
+      DEBUG_POS("SCARA IK", delta);
+      DEBUG_ECHOLNPGM("  SCARA (x,y) ", sx, ",", sy, " C2=", C2, " S2=", S2, " Theta=", THETA, " Psi=", PSI);
+    //*/
+  }
+
+#elif ENABLED(MP_SCARA)
+
+  void scara_set_axis_is_at_home(const AxisEnum axis) {
+    if (axis == Z_AXIS)
+      current_position.z = Z_HOME_POS;
+    else {
+      // MP_SCARA uses arm angles for AB home position
+      #ifndef SCARA_OFFSET_THETA1
+        #define SCARA_OFFSET_THETA1  12 // degrees
+      #endif
+      #ifndef SCARA_OFFSET_THETA2
+        #define SCARA_OFFSET_THETA2 131 // degrees
+      #endif
+      ab_float_t homeposition = { SCARA_OFFSET_THETA1, SCARA_OFFSET_THETA2 };
+      //DEBUG_ECHOLNPGM("homeposition A:", homeposition.a, " B:", homeposition.b);
+
+      inverse_kinematics(homeposition);
+      forward_kinematics(delta.a, delta.b);
+      current_position[axis] = cartes[axis];
+
+      //DEBUG_ECHOLNPGM_P(PSTR("Cartesian X"), current_position.x, SP_Y_LBL, current_position.y);
+      update_software_endstops(axis);
+    }
+  }
+
+  void inverse_kinematics(const xyz_pos_t &raw) {
+    const float x = raw.x, y = raw.y, c = HYPOT(x, y),
+                THETA3 = ATAN2(y, x),
+                THETA1 = THETA3 + ACOS((sq(c) + sq(L1) - sq(L2)) / (2.0f * c * L1)),
+                THETA2 = THETA3 - ACOS((sq(c) + sq(L2) - sq(L1)) / (2.0f * c * L2));
+
+    delta.set(DEGREES(THETA1), DEGREES(THETA2), raw.z);
+
+    /*
+      DEBUG_POS("SCARA IK", raw);
+      DEBUG_POS("SCARA IK", delta);
+      SERIAL_ECHOLNPGM("  SCARA (x,y) ", x, ",", y," Theta1=", THETA1, " Theta2=", THETA2);
+    //*/
+  }
+
+#elif ENABLED(AXEL_TPARA)
+
+  static constexpr xyz_pos_t robot_offset = { TPARA_OFFSET_X, TPARA_OFFSET_Y, TPARA_OFFSET_Z };
+
+  void scara_set_axis_is_at_home(const AxisEnum axis) {
+    if (axis == Z_AXIS)
+      current_position.z = Z_HOME_POS;
+    else {
+      xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
+      //DEBUG_ECHOLNPGM_P(PSTR("homeposition X"), homeposition.x, SP_Y_LBL, homeposition.y, SP_Z_LBL, homeposition.z);
+
+      inverse_kinematics(homeposition);
+      forward_kinematics(delta.a, delta.b, delta.c);
+      current_position[axis] = cartes[axis];
+
+      //DEBUG_ECHOLNPGM_P(PSTR("Cartesian X"), current_position.x, SP_Y_LBL, current_position.y);
+      update_software_endstops(axis);
+    }
+  }
+
+  // Convert ABC inputs in degrees to XYZ outputs in mm
+  void forward_kinematics(const_float_t a, const_float_t b, const_float_t c) {
+    const float w = c - b,
+                r = L1 * cos(RADIANS(b)) + L2 * sin(RADIANS(w - (90 - b))),
+                x = r  * cos(RADIANS(a)),
+                y = r  * sin(RADIANS(a)),
+                rho2 = L1_2 + L2_2 - 2.0f * L1 * L2 * cos(RADIANS(w));
+
+    cartes = robot_offset + xyz_pos_t({ x, y, SQRT(rho2 - sq(x) - sq(y)) });
+  }
+
+  // Home YZ together, then X (or all at once). Based on quick_home_xy & home_delta
+  void home_TPARA() {
+    // Init the current position of all carriages to 0,0,0
+    current_position.reset();
+    destination.reset();
+    sync_plan_position();
+
+    // Disable stealthChop if used. Enable diag1 pin on driver.
+    #if ENABLED(SENSORLESS_HOMING)
+      TERN_(X_SENSORLESS, sensorless_t stealth_states_x = start_sensorless_homing_per_axis(X_AXIS));
+      TERN_(Y_SENSORLESS, sensorless_t stealth_states_y = start_sensorless_homing_per_axis(Y_AXIS));
+      TERN_(Z_SENSORLESS, sensorless_t stealth_states_z = start_sensorless_homing_per_axis(Z_AXIS));
+    #endif
+
+    //const int x_axis_home_dir = TOOL_X_HOME_DIR(active_extruder);
+
+    //const xy_pos_t pos { max_length(X_AXIS) , max_length(Y_AXIS) };
+    //const float mlz = max_length(X_AXIS),
+
+    // Move all carriages together linearly until an endstop is hit.
+    //do_blocking_move_to_xy_z(pos, mlz, homing_feedrate(Z_AXIS));
+
+    current_position.x = 0 ;
+    current_position.y = 0 ;
+    current_position.z = max_length(Z_AXIS) ;
+    line_to_current_position(homing_feedrate(Z_AXIS));
+    planner.synchronize();
+
+    // Re-enable stealthChop if used. Disable diag1 pin on driver.
+    #if ENABLED(SENSORLESS_HOMING)
+      TERN_(X_SENSORLESS, end_sensorless_homing_per_axis(X_AXIS, stealth_states_x));
+      TERN_(Y_SENSORLESS, end_sensorless_homing_per_axis(Y_AXIS, stealth_states_y));
+      TERN_(Z_SENSORLESS, end_sensorless_homing_per_axis(Z_AXIS, stealth_states_z));
+    #endif
+
+    endstops.validate_homing_move();
+
+    // At least one motor has reached its endstop.
+    // Now re-home each motor separately.
+    homeaxis(A_AXIS);
+    homeaxis(C_AXIS);
+    homeaxis(B_AXIS);
+
+    // Set all carriages to their home positions
+    // Do this here all at once for Delta, because
+    // XYZ isn't ABC. Applying this per-tower would
+    // give the impression that they are the same.
+    LOOP_NUM_AXES(i) set_axis_is_at_home((AxisEnum)i);
+
+    sync_plan_position();
+  }
+
+  void inverse_kinematics(const xyz_pos_t &raw) {
+    const xyz_pos_t spos = raw - robot_offset;
+
+    const float RXY = SQRT(HYPOT2(spos.x, spos.y)),
+                RHO2 = NORMSQ(spos.x, spos.y, spos.z),
+                //RHO = SQRT(RHO2),
+                LSS = L1_2 + L2_2,
+                LM = 2.0f * L1 * L2,
+
+                CG = (LSS - RHO2) / LM,
+                SG = SQRT(1 - POW(CG, 2)), // Method 2
+                K1 = L1 - L2 * CG,
+                K2 = L2 * SG,
+
+                // Angle of Body Joint
+                THETA = ATAN2(spos.y, spos.x),
+
+                // Angle of Elbow Joint
+                //GAMMA = ACOS(CG),
+                GAMMA = ATAN2(SG, CG), // Method 2
+
+                // Angle of Shoulder Joint, elevation angle measured from horizontal (r+)
+                //PHI = asin(spos.z/RHO) + asin(L2 * sin(GAMMA) / RHO),
+                PHI = ATAN2(spos.z, RXY) + ATAN2(K2, K1),   // Method 2
+
+                // Elbow motor angle measured from horizontal, same frame as shoulder  (r+)
+                PSI = PHI + GAMMA;
+
+    delta.set(DEGREES(THETA), DEGREES(PHI), DEGREES(PSI));
+
+    //SERIAL_ECHOLNPGM(" SCARA (x,y,z) ", spos.x , ",", spos.y, ",", spos.z, " Rho=", RHO, " Rho2=", RHO2, " Theta=", THETA, " Phi=", PHI, " Psi=", PSI, " Gamma=", GAMMA);
+  }
+
+#endif
+
+void scara_report_positions() {
+  SERIAL_ECHOLNPGM("SCARA Theta:", planner.get_axis_position_degrees(A_AXIS)
+    #if ENABLED(AXEL_TPARA)
+      , "  Phi:", planner.get_axis_position_degrees(B_AXIS)
+      , "  Psi:", planner.get_axis_position_degrees(C_AXIS)
+    #else
+      , "  Psi" TERN_(MORGAN_SCARA, "+Theta") ":", planner.get_axis_position_degrees(B_AXIS)
+    #endif
+  );
+  SERIAL_EOL();
+}
+
+#endif // IS_SCARA

src/module/scara.h

@@ -0,0 +1,53 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * scara.h - SCARA-specific functions
+ */
+
+#include "../core/macros.h"
+
+extern float segments_per_second;
+
+#if ENABLED(AXEL_TPARA)
+
+  float constexpr L1 = TPARA_LINKAGE_1, L2 = TPARA_LINKAGE_2,   // Float constants for Robot arm calculations
+                  L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2,
+                  L2_2 = sq(float(L2));
+
+  void forward_kinematics(const_float_t a, const_float_t b, const_float_t c);
+  void home_TPARA();
+
+#else
+
+  float constexpr L1 = SCARA_LINKAGE_1, L2 = SCARA_LINKAGE_2,   // Float constants for SCARA calculations
+                  L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2,
+                  L2_2 = sq(float(L2));
+
+  void forward_kinematics(const_float_t a, const_float_t b);
+
+#endif
+
+void inverse_kinematics(const xyz_pos_t &raw);
+void scara_set_axis_is_at_home(const AxisEnum axis);
+void scara_report_positions();

src/module/servo.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/>.
+ *
+ */
+
+/**
+ * module/servo.cpp
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if HAS_SERVOS
+
+#include "servo.h"
+
+hal_servo_t servo[NUM_SERVOS];
+
+#if ENABLED(EDITABLE_SERVO_ANGLES)
+  uint16_t servo_angles[NUM_SERVOS][2];
+#endif
+
+void servo_init() {
+  #if NUM_SERVOS >= 1 && HAS_SERVO_0
+    servo[0].attach(SERVO0_PIN);
+    servo[0].detach(); // Just set up the pin. We don't have a position yet. Don't move to a random position.
+  #endif
+  #if NUM_SERVOS >= 2 && HAS_SERVO_1
+    servo[1].attach(SERVO1_PIN);
+    servo[1].detach();
+  #endif
+  #if NUM_SERVOS >= 3 && HAS_SERVO_2
+    servo[2].attach(SERVO2_PIN);
+    servo[2].detach();
+  #endif
+  #if NUM_SERVOS >= 4 && HAS_SERVO_3
+    servo[3].attach(SERVO3_PIN);
+    servo[3].detach();
+  #endif
+}
+
+#endif // HAS_SERVOS

src/module/servo.h

@@ -0,0 +1,113 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * module/servo.h
+ */
+
+#include "../inc/MarlinConfig.h"
+#include "../HAL/shared/servo.h"
+
+#if HAS_SERVO_ANGLES
+
+  #if ENABLED(SWITCHING_EXTRUDER)
+    // Switching extruder can have 2 or 4 angles
+    #if EXTRUDERS > 3
+      #define REQ_ANGLES 4
+    #else
+      #define REQ_ANGLES 2
+    #endif
+    constexpr uint16_t sase[] = SWITCHING_EXTRUDER_SERVO_ANGLES;
+    static_assert(COUNT(sase) == REQ_ANGLES, "SWITCHING_EXTRUDER_SERVO_ANGLES needs " STRINGIFY(REQ_ANGLES) " angles.");
+  #else
+    constexpr uint16_t sase[4] = { 0 };
+  #endif
+
+  #if ENABLED(SWITCHING_NOZZLE)
+    constexpr uint16_t sasn[] = SWITCHING_NOZZLE_SERVO_ANGLES;
+    static_assert(COUNT(sasn) == 2, "SWITCHING_NOZZLE_SERVO_ANGLES needs 2 angles.");
+  #else
+    constexpr uint16_t sasn[2] = { 0 };
+  #endif
+
+  #ifdef Z_PROBE_SERVO_NR
+    #if ENABLED(BLTOUCH)
+      #include "../feature/bltouch.h"
+      #undef Z_SERVO_ANGLES
+      #define Z_SERVO_ANGLES { BLTOUCH_DEPLOY, BLTOUCH_STOW }
+    #endif
+    constexpr uint16_t sazp[] = Z_SERVO_ANGLES;
+    static_assert(COUNT(sazp) == 2, "Z_SERVO_ANGLES needs 2 angles.");
+  #else
+    constexpr uint16_t sazp[2] = { 0 };
+  #endif
+
+  #ifndef SWITCHING_EXTRUDER_SERVO_NR
+    #define SWITCHING_EXTRUDER_SERVO_NR -1
+  #endif
+  #ifndef SWITCHING_EXTRUDER_E23_SERVO_NR
+    #define SWITCHING_EXTRUDER_E23_SERVO_NR -1
+  #endif
+  #ifndef SWITCHING_NOZZLE_SERVO_NR
+    #define SWITCHING_NOZZLE_SERVO_NR -1
+  #endif
+  #ifndef Z_PROBE_SERVO_NR
+    #define Z_PROBE_SERVO_NR -1
+  #endif
+
+  #define ASRC(N,I) (                                  \
+      N == SWITCHING_EXTRUDER_SERVO_NR     ? sase[I]   \
+    : N == SWITCHING_EXTRUDER_E23_SERVO_NR ? sase[I+2] \
+    : N == SWITCHING_NOZZLE_SERVO_NR       ? sasn[I]   \
+    : N == Z_PROBE_SERVO_NR                ? sazp[I]   \
+    : 0                                                )
+
+  #if ENABLED(EDITABLE_SERVO_ANGLES)
+    extern uint16_t servo_angles[NUM_SERVOS][2];
+    #define CONST_SERVO_ANGLES base_servo_angles
+  #else
+    #define CONST_SERVO_ANGLES servo_angles
+  #endif
+
+  constexpr uint16_t CONST_SERVO_ANGLES [NUM_SERVOS][2] = {
+      { ASRC(0,0), ASRC(0,1) }
+    #if NUM_SERVOS > 1
+      , { ASRC(1,0), ASRC(1,1) }
+      #if NUM_SERVOS > 2
+        , { ASRC(2,0), ASRC(2,1) }
+        #if NUM_SERVOS > 3
+          , { ASRC(3,0), ASRC(3,1) }
+        #endif
+      #endif
+    #endif
+  };
+
+  #if HAS_Z_SERVO_PROBE
+    #define DEPLOY_Z_SERVO() servo[Z_PROBE_SERVO_NR].move(servo_angles[Z_PROBE_SERVO_NR][0])
+    #define STOW_Z_SERVO() servo[Z_PROBE_SERVO_NR].move(servo_angles[Z_PROBE_SERVO_NR][1])
+  #endif
+
+#endif // HAS_SERVO_ANGLES
+
+extern hal_servo_t servo[NUM_SERVOS];
+void servo_init();

src/module/settings.h

@@ -0,0 +1,149 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+//
+// settings.cpp - Settings and EEPROM storage
+//
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(EEPROM_SETTINGS)
+  #include "../HAL/shared/eeprom_api.h"
+#endif
+
+class MarlinSettings {
+  public:
+    static uint16_t datasize();
+
+    static void reset();
+    static bool save();    // Return 'true' if data was saved
+
+    FORCE_INLINE static bool init_eeprom() {
+      reset();
+      #if ENABLED(EEPROM_SETTINGS)
+        const bool success = save();
+        if (TERN0(EEPROM_CHITCHAT, success)) report();
+        return success;
+      #else
+        return true;
+      #endif
+    }
+
+    #if ENABLED(SD_FIRMWARE_UPDATE)
+      static bool sd_update_status();                       // True if the SD-Firmware-Update EEPROM flag is set
+      static bool set_sd_update_status(const bool enable);  // Return 'true' after EEPROM is set (-> always true)
+    #endif
+
+    #if ENABLED(EEPROM_SETTINGS)
+
+      static bool load();      // Return 'true' if data was loaded ok
+      static bool validate();  // Return 'true' if EEPROM data is ok
+
+      static void first_load() {
+        static bool loaded = false;
+        if (!loaded && load()) loaded = true;
+      }
+
+      #if ENABLED(AUTO_BED_LEVELING_UBL) // Eventually make these available if any leveling system
+                                         // That can store is enabled
+        static uint16_t meshes_start_index();
+        FORCE_INLINE static uint16_t meshes_end_index() { return meshes_end; }
+        static uint16_t calc_num_meshes();
+        static int mesh_slot_offset(const int8_t slot);
+        static void store_mesh(const int8_t slot);
+        static void load_mesh(const int8_t slot, void * const into=nullptr);
+
+        //static void delete_mesh();    // necessary if we have a MAT
+        //static void defrag_meshes();  // "
+      #endif
+
+    #else // !EEPROM_SETTINGS
+
+      FORCE_INLINE
+      static bool load() { reset(); report(); return true; }
+      FORCE_INLINE
+      static void first_load() { (void)load(); }
+
+    #endif // !EEPROM_SETTINGS
+
+    #if DISABLED(DISABLE_M503)
+      static void report(const bool forReplay=false);
+    #else
+      FORCE_INLINE
+      static void report(const bool=false) {}
+    #endif
+
+  private:
+    static void postprocess();
+
+    #if ENABLED(EEPROM_SETTINGS)
+
+      static bool eeprom_error, validating;
+
+      #if ENABLED(AUTO_BED_LEVELING_UBL)  // Eventually make these available if any leveling system
+                                          // That can store is enabled
+        static const uint16_t meshes_end; // 128 is a placeholder for the size of the MAT; the MAT will always
+                                          // live at the very end of the eeprom
+      #endif
+
+      static bool _load();
+      static bool size_error(const uint16_t size);
+
+      static int eeprom_index;
+      static uint16_t working_crc;
+
+      static bool EEPROM_START(int eeprom_offset) {
+        if (!persistentStore.access_start()) { SERIAL_ECHO_MSG("No EEPROM."); return false; }
+        eeprom_index = eeprom_offset;
+        working_crc = 0;
+        return true;
+      }
+
+      static void EEPROM_FINISH(void) { persistentStore.access_finish(); }
+
+      template<typename T>
+      static void EEPROM_SKIP(const T &VAR) { eeprom_index += sizeof(VAR); }
+
+      template<typename T>
+      static void EEPROM_WRITE(const T &VAR) {
+        persistentStore.write_data(eeprom_index, (const uint8_t *) &VAR, sizeof(VAR), &working_crc);
+      }
+
+      template<typename T>
+      static void EEPROM_READ(T &VAR) {
+        persistentStore.read_data(eeprom_index, (uint8_t *) &VAR, sizeof(VAR), &working_crc, !validating);
+      }
+
+      static void EEPROM_READ(uint8_t *VAR, size_t sizeof_VAR) {
+        persistentStore.read_data(eeprom_index, VAR, sizeof_VAR, &working_crc, !validating);
+      }
+
+      template<typename T>
+      static void EEPROM_READ_ALWAYS(T &VAR) {
+        persistentStore.read_data(eeprom_index, (uint8_t *) &VAR, sizeof(VAR), &working_crc);
+      }
+
+    #endif // EEPROM_SETTINGS
+};
+
+extern MarlinSettings settings;

src/module/stepper.h

@@ -0,0 +1,694 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * stepper.h - stepper motor driver: executes motion plans of planner.c using the stepper motors
+ * Derived from Grbl
+ *
+ * Copyright (c) 2009-2011 Simen Svale Skogsrud
+ *
+ * Grbl is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Grbl is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Grbl.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#include "planner.h"
+#include "stepper/indirection.h"
+#ifdef __AVR__
+  #include "stepper/speed_lookuptable.h"
+#endif
+
+// Disable multiple steps per ISR
+//#define DISABLE_MULTI_STEPPING
+
+//
+// Estimate the amount of time the Stepper ISR will take to execute
+//
+
+/**
+ * The method of calculating these cycle-constants is unclear.
+ * Most of them are no longer used directly for pulse timing, and exist
+ * only to estimate a maximum step rate based on the user's configuration.
+ * As 32-bit processors continue to diverge, maintaining cycle counts
+ * will become increasingly difficult and error-prone.
+ */
+
+#ifdef CPU_32_BIT
+  /**
+   * Duration of START_TIMED_PULSE
+   *
+   * ...as measured on an LPC1768 with a scope and converted to cycles.
+   * Not applicable to other 32-bit processors, but as long as others
+   * take longer, pulses will be longer. For example the SKR Pro
+   * (stm32f407zgt6) requires ~60 cyles.
+   */
+  #define TIMER_READ_ADD_AND_STORE_CYCLES 34UL
+
+  // The base ISR takes 792 cycles
+  #define ISR_BASE_CYCLES  792UL
+
+  // Linear advance base time is 64 cycles
+  #if ENABLED(LIN_ADVANCE)
+    #define ISR_LA_BASE_CYCLES 64UL
+  #else
+    #define ISR_LA_BASE_CYCLES 0UL
+  #endif
+
+  // S curve interpolation adds 40 cycles
+  #if ENABLED(S_CURVE_ACCELERATION)
+    #define ISR_S_CURVE_CYCLES 40UL
+  #else
+    #define ISR_S_CURVE_CYCLES 0UL
+  #endif
+
+  // Stepper Loop base cycles
+  #define ISR_LOOP_BASE_CYCLES 4UL
+
+  // To start the step pulse, in the worst case takes
+  #define ISR_START_STEPPER_CYCLES 13UL
+
+  // And each stepper (start + stop pulse) takes in worst case
+  #define ISR_STEPPER_CYCLES 16UL
+
+#else
+  // Cycles to perform actions in START_TIMED_PULSE
+  #define TIMER_READ_ADD_AND_STORE_CYCLES 13UL
+
+  // The base ISR takes 752 cycles
+  #define ISR_BASE_CYCLES  752UL
+
+  // Linear advance base time is 32 cycles
+  #if ENABLED(LIN_ADVANCE)
+    #define ISR_LA_BASE_CYCLES 32UL
+  #else
+    #define ISR_LA_BASE_CYCLES 0UL
+  #endif
+
+  // S curve interpolation adds 160 cycles
+  #if ENABLED(S_CURVE_ACCELERATION)
+    #define ISR_S_CURVE_CYCLES 160UL
+  #else
+    #define ISR_S_CURVE_CYCLES 0UL
+  #endif
+
+  // Stepper Loop base cycles
+  #define ISR_LOOP_BASE_CYCLES 32UL
+
+  // To start the step pulse, in the worst case takes
+  #define ISR_START_STEPPER_CYCLES 57UL
+
+  // And each stepper (start + stop pulse) takes in worst case
+  #define ISR_STEPPER_CYCLES 88UL
+
+#endif
+
+// If linear advance is disabled, the loop also handles them
+#if DISABLED(LIN_ADVANCE) && ENABLED(MIXING_EXTRUDER)
+  #define ISR_MIXING_STEPPER_CYCLES ((MIXING_STEPPERS) * (ISR_STEPPER_CYCLES))
+#else
+  #define ISR_MIXING_STEPPER_CYCLES  0UL
+#endif
+
+// Add time for each stepper
+#if HAS_X_STEP
+  #define ISR_X_STEPPER_CYCLES  ISR_STEPPER_CYCLES
+#endif
+#if HAS_Y_STEP
+  #define ISR_Y_STEPPER_CYCLES  ISR_STEPPER_CYCLES
+#endif
+#if HAS_Z_STEP
+  #define ISR_Z_STEPPER_CYCLES  ISR_STEPPER_CYCLES
+#endif
+#if HAS_I_STEP
+  #define ISR_I_STEPPER_CYCLES  ISR_STEPPER_CYCLES
+#endif
+#if HAS_J_STEP
+  #define ISR_J_STEPPER_CYCLES  ISR_STEPPER_CYCLES
+#endif
+#if HAS_K_STEP
+  #define ISR_K_STEPPER_CYCLES  ISR_STEPPER_CYCLES
+#endif
+#if HAS_EXTRUDERS
+  #define ISR_E_STEPPER_CYCLES  ISR_STEPPER_CYCLES    // E is always interpolated, even for mixing extruders
+#endif
+
+// And the total minimum loop time, not including the base
+#define MIN_ISR_LOOP_CYCLES (ISR_MIXING_STEPPER_CYCLES LOGICAL_AXIS_GANG(+ ISR_E_STEPPER_CYCLES, + ISR_X_STEPPER_CYCLES, + ISR_Y_STEPPER_CYCLES, + ISR_Z_STEPPER_CYCLES, + ISR_I_STEPPER_CYCLES, + ISR_J_STEPPER_CYCLES, + ISR_K_STEPPER_CYCLES))
+
+// Calculate the minimum MPU cycles needed per pulse to enforce, limited to the max stepper rate
+#define _MIN_STEPPER_PULSE_CYCLES(N) _MAX(uint32_t((F_CPU) / (MAXIMUM_STEPPER_RATE)), ((F_CPU) / 500000UL) * (N))
+#if MINIMUM_STEPPER_PULSE
+  #define MIN_STEPPER_PULSE_CYCLES _MIN_STEPPER_PULSE_CYCLES(uint32_t(MINIMUM_STEPPER_PULSE))
+#elif HAS_DRIVER(LV8729)
+  #define MIN_STEPPER_PULSE_CYCLES uint32_t((((F_CPU) - 1) / 2000000) + 1) // 0.5µs, aka 500ns
+#else
+  #define MIN_STEPPER_PULSE_CYCLES _MIN_STEPPER_PULSE_CYCLES(1UL)
+#endif
+
+// Calculate the minimum pulse times (high and low)
+#if MINIMUM_STEPPER_PULSE && MAXIMUM_STEPPER_RATE
+  constexpr uint32_t _MIN_STEP_PERIOD_NS = 1000000000UL / MAXIMUM_STEPPER_RATE;
+  constexpr uint32_t _MIN_PULSE_HIGH_NS = 1000UL * MINIMUM_STEPPER_PULSE;
+  constexpr uint32_t _MIN_PULSE_LOW_NS = _MAX((_MIN_STEP_PERIOD_NS - _MIN(_MIN_STEP_PERIOD_NS, _MIN_PULSE_HIGH_NS)), _MIN_PULSE_HIGH_NS);
+#elif MINIMUM_STEPPER_PULSE
+  // Assume 50% duty cycle
+  constexpr uint32_t _MIN_PULSE_HIGH_NS = 1000UL * MINIMUM_STEPPER_PULSE;
+  constexpr uint32_t _MIN_PULSE_LOW_NS = _MIN_PULSE_HIGH_NS;
+#elif MAXIMUM_STEPPER_RATE
+  // Assume 50% duty cycle
+  constexpr uint32_t _MIN_PULSE_HIGH_NS = 500000000UL / MAXIMUM_STEPPER_RATE;
+  constexpr uint32_t _MIN_PULSE_LOW_NS = _MIN_PULSE_HIGH_NS;
+#else
+  #error "Expected at least one of MINIMUM_STEPPER_PULSE or MAXIMUM_STEPPER_RATE to be defined"
+#endif
+
+// But the user could be enforcing a minimum time, so the loop time is
+#define ISR_LOOP_CYCLES (ISR_LOOP_BASE_CYCLES + _MAX(MIN_STEPPER_PULSE_CYCLES, MIN_ISR_LOOP_CYCLES))
+
+// If linear advance is enabled, then it is handled separately
+#if ENABLED(LIN_ADVANCE)
+
+  // Estimate the minimum LA loop time
+  #if ENABLED(MIXING_EXTRUDER) // ToDo: ???
+    // HELP ME: What is what?
+    // Directions are set up for MIXING_STEPPERS - like before.
+    // Finding the right stepper may last up to MIXING_STEPPERS loops in get_next_stepper().
+    //   These loops are a bit faster than advancing a bresenham counter.
+    // Always only one E stepper is stepped.
+    #define MIN_ISR_LA_LOOP_CYCLES ((MIXING_STEPPERS) * (ISR_STEPPER_CYCLES))
+  #else
+    #define MIN_ISR_LA_LOOP_CYCLES ISR_STEPPER_CYCLES
+  #endif
+
+  // And the real loop time
+  #define ISR_LA_LOOP_CYCLES _MAX(MIN_STEPPER_PULSE_CYCLES, MIN_ISR_LA_LOOP_CYCLES)
+
+#else
+  #define ISR_LA_LOOP_CYCLES 0UL
+#endif
+
+// Now estimate the total ISR execution time in cycles given a step per ISR multiplier
+#define ISR_EXECUTION_CYCLES(R) (((ISR_BASE_CYCLES + ISR_S_CURVE_CYCLES + (ISR_LOOP_CYCLES) * (R) + ISR_LA_BASE_CYCLES + ISR_LA_LOOP_CYCLES)) / (R))
+
+// The maximum allowable stepping frequency when doing x128-x1 stepping (in Hz)
+#define MAX_STEP_ISR_FREQUENCY_128X ((F_CPU) / ISR_EXECUTION_CYCLES(128))
+#define MAX_STEP_ISR_FREQUENCY_64X  ((F_CPU) / ISR_EXECUTION_CYCLES(64))
+#define MAX_STEP_ISR_FREQUENCY_32X  ((F_CPU) / ISR_EXECUTION_CYCLES(32))
+#define MAX_STEP_ISR_FREQUENCY_16X  ((F_CPU) / ISR_EXECUTION_CYCLES(16))
+#define MAX_STEP_ISR_FREQUENCY_8X   ((F_CPU) / ISR_EXECUTION_CYCLES(8))
+#define MAX_STEP_ISR_FREQUENCY_4X   ((F_CPU) / ISR_EXECUTION_CYCLES(4))
+#define MAX_STEP_ISR_FREQUENCY_2X   ((F_CPU) / ISR_EXECUTION_CYCLES(2))
+#define MAX_STEP_ISR_FREQUENCY_1X   ((F_CPU) / ISR_EXECUTION_CYCLES(1))
+
+// The minimum step ISR rate used by ADAPTIVE_STEP_SMOOTHING to target 50% CPU usage
+// This does not account for the possibility of multi-stepping.
+// Perhaps DISABLE_MULTI_STEPPING should be required with ADAPTIVE_STEP_SMOOTHING.
+#define MIN_STEP_ISR_FREQUENCY (MAX_STEP_ISR_FREQUENCY_1X / 2)
+
+#define ENABLE_COUNT (NUM_AXES + E_STEPPERS)
+typedef IF<(ENABLE_COUNT > 8), uint16_t, uint8_t>::type ena_mask_t;
+
+// Axis flags type, for enabled state or other simple state
+typedef struct {
+  union {
+    ena_mask_t bits;
+    struct {
+      bool NUM_AXIS_LIST(X:1, Y:1, Z:1, I:1, J:1, K:1);
+      #if HAS_EXTRUDERS
+        bool LIST_N(EXTRUDERS, E0:1, E1:1, E2:1, E3:1, E4:1, E5:1, E6:1, E7:1);
+      #endif
+    };
+  };
+} stepper_flags_t;
+
+// All the stepper enable pins
+constexpr pin_t ena_pins[] = {
+  NUM_AXIS_LIST(X_ENABLE_PIN, Y_ENABLE_PIN, Z_ENABLE_PIN, I_ENABLE_PIN, J_ENABLE_PIN, K_ENABLE_PIN),
+  LIST_N(E_STEPPERS, E0_ENABLE_PIN, E1_ENABLE_PIN, E2_ENABLE_PIN, E3_ENABLE_PIN, E4_ENABLE_PIN, E5_ENABLE_PIN, E6_ENABLE_PIN, E7_ENABLE_PIN)
+};
+
+// Index of the axis or extruder element in a combined array
+constexpr uint8_t index_of_axis(const AxisEnum axis E_OPTARG(const uint8_t eindex=0)) {
+  return uint8_t(axis) + (E_TERN0(axis < NUM_AXES ? 0 : eindex));
+}
+//#define __IAX_N(N,V...)           _IAX_##N(V)
+//#define _IAX_N(N,V...)            __IAX_N(N,V)
+//#define _IAX_1(A)                 index_of_axis(A)
+//#define _IAX_2(A,B)               index_of_axis(A E_OPTARG(B))
+//#define INDEX_OF_AXIS(V...)       _IAX_N(TWO_ARGS(V),V)
+
+#define INDEX_OF_AXIS(A,V...)     index_of_axis(A E_OPTARG(V+0))
+
+// Bit mask for a matching enable pin, or 0
+constexpr ena_mask_t ena_same(const uint8_t a, const uint8_t b) {
+  return ena_pins[a] == ena_pins[b] ? _BV(b) : 0;
+}
+
+// Recursively get the enable overlaps mask for a given linear axis or extruder
+constexpr ena_mask_t ena_overlap(const uint8_t a=0, const uint8_t b=0) {
+  return b >= ENABLE_COUNT ? 0 : (a == b ? 0 : ena_same(a, b)) | ena_overlap(a, b + 1);
+}
+
+// Recursively get whether there's any overlap at all
+constexpr bool any_enable_overlap(const uint8_t a=0) {
+  return a >= ENABLE_COUNT ? false : ena_overlap(a) || any_enable_overlap(a + 1);
+}
+
+// Array of axes that overlap with each
+// TODO: Consider cases where >=2 steppers are used by a linear axis or extruder
+//       (e.g., CoreXY, Dual XYZ, or E with multiple steppers, etc.).
+constexpr ena_mask_t enable_overlap[] = {
+  #define _OVERLAP(N) ena_overlap(INDEX_OF_AXIS(AxisEnum(N))),
+  REPEAT(NUM_AXES, _OVERLAP)
+  #if HAS_EXTRUDERS
+    #define _E_OVERLAP(N) ena_overlap(INDEX_OF_AXIS(E_AXIS, N)),
+    REPEAT(E_STEPPERS, _E_OVERLAP)
+  #endif
+};
+
+//static_assert(!any_enable_overlap(), "There is some overlap.");
+
+//
+// Stepper class definition
+//
+class Stepper {
+
+  public:
+
+    #if EITHER(HAS_EXTRA_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
+      static bool separate_multi_axis;
+    #endif
+
+    #if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM
+      #if HAS_MOTOR_CURRENT_PWM
+        #ifndef PWM_MOTOR_CURRENT
+          #define PWM_MOTOR_CURRENT DEFAULT_PWM_MOTOR_CURRENT
+        #endif
+        #ifndef MOTOR_CURRENT_PWM_FREQUENCY
+          #define MOTOR_CURRENT_PWM_FREQUENCY 31400
+        #endif
+        #define MOTOR_CURRENT_COUNT 3
+      #elif HAS_MOTOR_CURRENT_SPI
+        static constexpr uint32_t digipot_count[] = DIGIPOT_MOTOR_CURRENT;
+        #define MOTOR_CURRENT_COUNT COUNT(Stepper::digipot_count)
+      #endif
+      static bool initialized;
+      static uint32_t motor_current_setting[MOTOR_CURRENT_COUNT]; // Initialized by settings.load()
+    #endif
+
+    // Last-moved extruder, as set when the last movement was fetched from planner
+    #if HAS_MULTI_EXTRUDER
+      static uint8_t last_moved_extruder;
+    #else
+      static constexpr uint8_t last_moved_extruder = 0;
+    #endif
+
+    #if ENABLED(FREEZE_FEATURE)
+      static bool frozen;                   // Set this flag to instantly freeze motion
+    #endif
+
+  private:
+
+    static block_t* current_block;          // A pointer to the block currently being traced
+
+    static axis_bits_t last_direction_bits, // The next stepping-bits to be output
+                       axis_did_move;       // Last Movement in the given direction is not null, as computed when the last movement was fetched from planner
+
+    static bool abort_current_block;        // Signals to the stepper that current block should be aborted
+
+    #if ENABLED(X_DUAL_ENDSTOPS)
+      static bool locked_X_motor, locked_X2_motor;
+    #endif
+    #if ENABLED(Y_DUAL_ENDSTOPS)
+      static bool locked_Y_motor, locked_Y2_motor;
+    #endif
+    #if EITHER(Z_MULTI_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
+      static bool locked_Z_motor, locked_Z2_motor
+                  #if NUM_Z_STEPPERS >= 3
+                    , locked_Z3_motor
+                    #if NUM_Z_STEPPERS >= 4
+                      , locked_Z4_motor
+                    #endif
+                  #endif
+                  ;
+    #endif
+
+    static uint32_t acceleration_time, deceleration_time; // time measured in Stepper Timer ticks
+    static uint8_t steps_per_isr;         // Count of steps to perform per Stepper ISR call
+
+    #if ENABLED(ADAPTIVE_STEP_SMOOTHING)
+      static uint8_t oversampling_factor; // Oversampling factor (log2(multiplier)) to increase temporal resolution of axis
+    #else
+      static constexpr uint8_t oversampling_factor = 0;
+    #endif
+
+    // Delta error variables for the Bresenham line tracer
+    static xyze_long_t delta_error;
+    static xyze_ulong_t advance_dividend;
+    static uint32_t advance_divisor,
+                    step_events_completed,  // The number of step events executed in the current block
+                    accelerate_until,       // The point from where we need to stop acceleration
+                    decelerate_after,       // The point from where we need to start decelerating
+                    step_event_count;       // The total event count for the current block
+
+    #if EITHER(HAS_MULTI_EXTRUDER, MIXING_EXTRUDER)
+      static uint8_t stepper_extruder;
+    #else
+      static constexpr uint8_t stepper_extruder = 0;
+    #endif
+
+    #if ENABLED(S_CURVE_ACCELERATION)
+      static int32_t bezier_A,     // A coefficient in Bézier speed curve
+                     bezier_B,     // B coefficient in Bézier speed curve
+                     bezier_C;     // C coefficient in Bézier speed curve
+      static uint32_t bezier_F,    // F coefficient in Bézier speed curve
+                      bezier_AV;   // AV coefficient in Bézier speed curve
+      #ifdef __AVR__
+        static bool A_negative;    // If A coefficient was negative
+      #endif
+      static bool bezier_2nd_half; // If Bézier curve has been initialized or not
+    #endif
+
+    #if ENABLED(LIN_ADVANCE)
+      static constexpr uint32_t LA_ADV_NEVER = 0xFFFFFFFF;
+      static uint32_t nextAdvanceISR, LA_isr_rate;
+      static uint16_t LA_current_adv_steps, LA_final_adv_steps, LA_max_adv_steps; // Copy from current executed block. Needed because current_block is set to NULL "too early".
+      static int8_t LA_steps;
+      static bool LA_use_advance_lead;
+    #endif
+
+    #if ENABLED(INTEGRATED_BABYSTEPPING)
+      static constexpr uint32_t BABYSTEP_NEVER = 0xFFFFFFFF;
+      static uint32_t nextBabystepISR;
+    #endif
+
+    #if ENABLED(DIRECT_STEPPING)
+      static page_step_state_t page_step_state;
+    #endif
+
+    static int32_t ticks_nominal;
+    #if DISABLED(S_CURVE_ACCELERATION)
+      static uint32_t acc_step_rate; // needed for deceleration start point
+    #endif
+
+    // Exact steps at which an endstop was triggered
+    static xyz_long_t endstops_trigsteps;
+
+    // Positions of stepper motors, in step units
+    static xyze_long_t count_position;
+
+    // Current stepper motor directions (+1 or -1)
+    static xyze_int8_t count_direction;
+
+  public:
+    // Initialize stepper hardware
+    static void init();
+
+    // Interrupt Service Routine and phases
+
+    // The stepper subsystem goes to sleep when it runs out of things to execute.
+    // Call this to notify the subsystem that it is time to go to work.
+    static void wake_up() { ENABLE_STEPPER_DRIVER_INTERRUPT(); }
+
+    static bool is_awake() { return STEPPER_ISR_ENABLED(); }
+
+    static bool suspend() {
+      const bool awake = is_awake();
+      if (awake) DISABLE_STEPPER_DRIVER_INTERRUPT();
+      return awake;
+    }
+
+    // The ISR scheduler
+    static void isr();
+
+    // The stepper pulse ISR phase
+    static void pulse_phase_isr();
+
+    // The stepper block processing ISR phase
+    static uint32_t block_phase_isr();
+
+    #if ENABLED(LIN_ADVANCE)
+      // The Linear advance ISR phase
+      static uint32_t advance_isr();
+      FORCE_INLINE static void initiateLA() { nextAdvanceISR = 0; }
+    #endif
+
+    #if ENABLED(INTEGRATED_BABYSTEPPING)
+      // The Babystepping ISR phase
+      static uint32_t babystepping_isr();
+      FORCE_INLINE static void initiateBabystepping() {
+        if (nextBabystepISR == BABYSTEP_NEVER) {
+          nextBabystepISR = 0;
+          wake_up();
+        }
+      }
+    #endif
+
+    // Check if the given block is busy or not - Must not be called from ISR contexts
+    static bool is_block_busy(const block_t * const block);
+
+    // Get the position of a stepper, in steps
+    static int32_t position(const AxisEnum axis);
+
+    // Set the current position in steps
+    static void set_position(const xyze_long_t &spos);
+    static void set_axis_position(const AxisEnum a, const int32_t &v);
+
+    // Report the positions of the steppers, in steps
+    static void report_a_position(const xyz_long_t &pos);
+    static void report_positions();
+
+    // Discard current block and free any resources
+    FORCE_INLINE static void discard_current_block() {
+      #if ENABLED(DIRECT_STEPPING)
+        if (current_block->is_page()) page_manager.free_page(current_block->page_idx);
+      #endif
+      current_block = nullptr;
+      axis_did_move = 0;
+      planner.release_current_block();
+    }
+
+    // Quickly stop all steppers
+    FORCE_INLINE static void quick_stop() { abort_current_block = true; }
+
+    // The direction of a single motor
+    FORCE_INLINE static bool motor_direction(const AxisEnum axis) { return TEST(last_direction_bits, axis); }
+
+    // The last movement direction was not null on the specified axis. Note that motor direction is not necessarily the same.
+    FORCE_INLINE static bool axis_is_moving(const AxisEnum axis) { return TEST(axis_did_move, axis); }
+
+    // Handle a triggered endstop
+    static void endstop_triggered(const AxisEnum axis);
+
+    // Triggered position of an axis in steps
+    static int32_t triggered_position(const AxisEnum axis);
+
+    #if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM
+      static void set_digipot_value_spi(const int16_t address, const int16_t value);
+      static void set_digipot_current(const uint8_t driver, const int16_t current);
+    #endif
+
+    #if HAS_MICROSTEPS
+      static void microstep_ms(const uint8_t driver, const int8_t ms1, const int8_t ms2, const int8_t ms3);
+      static void microstep_mode(const uint8_t driver, const uint8_t stepping);
+      static void microstep_readings();
+    #endif
+
+    #if EITHER(HAS_EXTRA_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
+      FORCE_INLINE static void set_separate_multi_axis(const bool state) { separate_multi_axis = state; }
+    #endif
+    #if ENABLED(X_DUAL_ENDSTOPS)
+      FORCE_INLINE static void set_x_lock(const bool state) { locked_X_motor = state; }
+      FORCE_INLINE static void set_x2_lock(const bool state) { locked_X2_motor = state; }
+    #endif
+    #if ENABLED(Y_DUAL_ENDSTOPS)
+      FORCE_INLINE static void set_y_lock(const bool state) { locked_Y_motor = state; }
+      FORCE_INLINE static void set_y2_lock(const bool state) { locked_Y2_motor = state; }
+    #endif
+    #if EITHER(Z_MULTI_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
+      FORCE_INLINE static void set_z1_lock(const bool state) { locked_Z_motor = state; }
+      FORCE_INLINE static void set_z2_lock(const bool state) { locked_Z2_motor = state; }
+      #if NUM_Z_STEPPERS >= 3
+        FORCE_INLINE static void set_z3_lock(const bool state) { locked_Z3_motor = state; }
+        #if NUM_Z_STEPPERS >= 4
+          FORCE_INLINE static void set_z4_lock(const bool state) { locked_Z4_motor = state; }
+        #endif
+      #endif
+      static void set_all_z_lock(const bool lock, const int8_t except=-1) {
+        set_z1_lock(lock ^ (except == 0));
+        set_z2_lock(lock ^ (except == 1));
+        #if NUM_Z_STEPPERS >= 3
+          set_z3_lock(lock ^ (except == 2));
+          #if NUM_Z_STEPPERS >= 4
+            set_z4_lock(lock ^ (except == 3));
+          #endif
+        #endif
+      }
+    #endif
+
+    #if ENABLED(BABYSTEPPING)
+      static void do_babystep(const AxisEnum axis, const bool direction); // perform a short step with a single stepper motor, outside of any convention
+    #endif
+
+    #if HAS_MOTOR_CURRENT_PWM
+      static void refresh_motor_power();
+    #endif
+
+    static stepper_flags_t axis_enabled;  // Axis stepper(s) ENABLED states
+
+    static bool axis_is_enabled(const AxisEnum axis E_OPTARG(const uint8_t eindex=0)) {
+      return TEST(axis_enabled.bits, INDEX_OF_AXIS(axis, eindex));
+    }
+    static void mark_axis_enabled(const AxisEnum axis E_OPTARG(const uint8_t eindex=0)) {
+      SBI(axis_enabled.bits, INDEX_OF_AXIS(axis, eindex));
+    }
+    static void mark_axis_disabled(const AxisEnum axis E_OPTARG(const uint8_t eindex=0)) {
+      CBI(axis_enabled.bits, INDEX_OF_AXIS(axis, eindex));
+    }
+    static bool can_axis_disable(const AxisEnum axis E_OPTARG(const uint8_t eindex=0)) {
+      return !any_enable_overlap() || !(axis_enabled.bits & enable_overlap[INDEX_OF_AXIS(axis, eindex)]);
+    }
+
+    static void enable_axis(const AxisEnum axis);
+    static bool disable_axis(const AxisEnum axis);
+
+    #if HAS_EXTRUDERS
+      static void enable_extruder(E_TERN_(const uint8_t eindex=0));
+      static bool disable_extruder(E_TERN_(const uint8_t eindex=0));
+      static void enable_e_steppers();
+      static void disable_e_steppers();
+    #else
+      static void enable_extruder() {}
+      static bool disable_extruder() { return true; }
+      static void enable_e_steppers() {}
+      static void disable_e_steppers() {}
+    #endif
+
+    #define  ENABLE_EXTRUDER(N)  enable_extruder(E_TERN_(N))
+    #define DISABLE_EXTRUDER(N) disable_extruder(E_TERN_(N))
+    #define AXIS_IS_ENABLED(N,V...) axis_is_enabled(N E_OPTARG(#V))
+
+    static void enable_all_steppers();
+    static void disable_all_steppers();
+
+    // Update direction states for all steppers
+    static void set_directions();
+
+    // Set direction bits and update all stepper DIR states
+    static void set_directions(const axis_bits_t bits) {
+      last_direction_bits = bits;
+      set_directions();
+    }
+
+  private:
+
+    // Set the current position in steps
+    static void _set_position(const abce_long_t &spos);
+
+    FORCE_INLINE static uint32_t calc_timer_interval(uint32_t step_rate, uint8_t *loops) {
+      uint32_t timer;
+
+      // Scale the frequency, as requested by the caller
+      step_rate <<= oversampling_factor;
+
+      uint8_t multistep = 1;
+      #if DISABLED(DISABLE_MULTI_STEPPING)
+
+        // The stepping frequency limits for each multistepping rate
+        static const uint32_t limit[] PROGMEM = {
+          (  MAX_STEP_ISR_FREQUENCY_1X     ),
+          (  MAX_STEP_ISR_FREQUENCY_2X >> 1),
+          (  MAX_STEP_ISR_FREQUENCY_4X >> 2),
+          (  MAX_STEP_ISR_FREQUENCY_8X >> 3),
+          ( MAX_STEP_ISR_FREQUENCY_16X >> 4),
+          ( MAX_STEP_ISR_FREQUENCY_32X >> 5),
+          ( MAX_STEP_ISR_FREQUENCY_64X >> 6),
+          (MAX_STEP_ISR_FREQUENCY_128X >> 7)
+        };
+
+        // Select the proper multistepping
+        uint8_t idx = 0;
+        while (idx < 7 && step_rate > (uint32_t)pgm_read_dword(&limit[idx])) {
+          step_rate >>= 1;
+          multistep <<= 1;
+          ++idx;
+        };
+      #else
+        NOMORE(step_rate, uint32_t(MAX_STEP_ISR_FREQUENCY_1X));
+      #endif
+      *loops = multistep;
+
+      #ifdef CPU_32_BIT
+        // In case of high-performance processor, it is able to calculate in real-time
+        timer = uint32_t(STEPPER_TIMER_RATE) / step_rate;
+      #else
+        constexpr uint32_t min_step_rate = (F_CPU) / 500000U;
+        NOLESS(step_rate, min_step_rate);
+        step_rate -= min_step_rate; // Correct for minimal speed
+        if (step_rate >= (8 * 256)) { // higher step rate
+          const uint8_t tmp_step_rate = (step_rate & 0x00FF);
+          const uint16_t table_address = (uint16_t)&speed_lookuptable_fast[(uint8_t)(step_rate >> 8)][0],
+                         gain = (uint16_t)pgm_read_word(table_address + 2);
+          timer = MultiU16X8toH16(tmp_step_rate, gain);
+          timer = (uint16_t)pgm_read_word(table_address) - timer;
+        }
+        else { // lower step rates
+          uint16_t table_address = (uint16_t)&speed_lookuptable_slow[0][0];
+          table_address += ((step_rate) >> 1) & 0xFFFC;
+          timer = (uint16_t)pgm_read_word(table_address)
+                - (((uint16_t)pgm_read_word(table_address + 2) * (uint8_t)(step_rate & 0x0007)) >> 3);
+        }
+        // (there is no need to limit the timer value here. All limits have been
+        // applied above, and AVR is able to keep up at 30khz Stepping ISR rate)
+      #endif
+
+      return timer;
+    }
+
+    #if ENABLED(S_CURVE_ACCELERATION)
+      static void _calc_bezier_curve_coeffs(const int32_t v0, const int32_t v1, const uint32_t av);
+      static int32_t _eval_bezier_curve(const uint32_t curr_step);
+    #endif
+
+    #if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM
+      static void digipot_init();
+    #endif
+
+    #if HAS_MICROSTEPS
+      static void microstep_init();
+    #endif
+
+};
+
+extern Stepper stepper;

src/module/stepper/L64xx.cpp

@@ -0,0 +1,246 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * stepper/L64xx.cpp
+ * Stepper driver indirection for L64XX drivers
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_L64XX
+
+#include "L64xx.h"
+
+#if AXIS_IS_L64XX(X)
+  L64XX_CLASS(X) stepperX(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(X2)
+  L64XX_CLASS(X2) stepperX2(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(Y)
+  L64XX_CLASS(Y) stepperY(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(Y2)
+  L64XX_CLASS(Y2) stepperY2(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(Z)
+  L64XX_CLASS(Z) stepperZ(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(Z2)
+  L64XX_CLASS(Z2) stepperZ2(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(Z3)
+  L64XX_CLASS(Z3) stepperZ3(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(Z4)
+  L64XX_CLASS(Z4) stepperZ4(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(I)
+  L64XX_CLASS(I) stepperI(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(J)
+  L64XX_CLASS(J) stepperJ(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(K)
+  L64XX_CLASS(K) stepperK(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(E0)
+  L64XX_CLASS(E0) stepperE0(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(E1)
+  L64XX_CLASS(E1) stepperE1(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(E2)
+  L64XX_CLASS(E2) stepperE2(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(E3)
+  L64XX_CLASS(E3) stepperE3(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(E4)
+  L64XX_CLASS(E4) stepperE4(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(E5)
+  L64XX_CLASS(E5) stepperE5(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(E6)
+  L64XX_CLASS(E6) stepperE6(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(E7)
+  L64XX_CLASS(E7) stepperE7(L6470_CHAIN_SS_PIN);
+#endif
+
+// Not using L64XX class init method because it
+// briefly sends power to the steppers
+
+inline void L6470_init_chip(L64XX &st, const int ms, const int oc, const int sc, const int mv, const int slew_rate) {
+  st.set_handlers(L64xxManager.spi_init, L64xxManager.transfer_single, L64xxManager.transfer_chain);  // specify which external SPI routines to use
+  switch (st.L6470_status_layout) {
+    case L6470_STATUS_LAYOUT: {
+      st.resetDev();
+      st.softFree();
+      st.SetParam(st.L64XX_CONFIG, CONFIG_PWM_DIV_1 | CONFIG_PWM_MUL_2 | CONFIG_OC_SD_DISABLE | CONFIG_VS_COMP_DISABLE | CONFIG_SW_HARD_STOP | CONFIG_INT_16MHZ);
+      st.SetParam(L6470_KVAL_RUN, 0xFF);
+      st.SetParam(L6470_KVAL_ACC, 0xFF);
+      st.SetParam(L6470_KVAL_DEC, 0xFF);
+      st.setMicroSteps(ms);
+      st.setOverCurrent(oc);
+      st.setStallCurrent(sc);
+      st.SetParam(L6470_KVAL_HOLD, mv);
+      st.SetParam(L6470_ABS_POS, 0);
+      uint32_t config_temp = st.GetParam(st.L64XX_CONFIG);
+      config_temp &= ~CONFIG_POW_SR;
+      switch (slew_rate) {
+        case 0: st.SetParam(st.L64XX_CONFIG, config_temp | CONFIG_SR_75V_us); break;
+        default:
+        case 1: st.SetParam(st.L64XX_CONFIG, config_temp | CONFIG_SR_110V_us); break;
+        case 3:
+        case 2: st.SetParam(st.L64XX_CONFIG, config_temp | CONFIG_SR_260V_us); break;
+      }
+      st.getStatus();
+      st.getStatus();
+      break;
+    }
+
+    case L6474_STATUS_LAYOUT: {
+      st.free();
+      //st.SetParam(st.L64XX_CONFIG, CONFIG_PWM_DIV_1 | CONFIG_PWM_MUL_2 | CONFIG_OC_SD_DISABLE | CONFIG_VS_COMP_DISABLE | CONFIG_SW_HARD_STOP | CONFIG_INT_16MHZ);
+      //st.SetParam(L6474_TVAL, 0xFF);
+      st.setMicroSteps(ms);
+      st.setOverCurrent(oc);
+      st.setTVALCurrent(sc);
+      st.SetParam(L6470_ABS_POS, 0);
+      uint32_t config_temp = st.GetParam(st.L64XX_CONFIG);
+      config_temp &= ~CONFIG_POW_SR & ~CONFIG_EN_TQREG;  // clear out slew rate and set current to be controlled by TVAL register
+      switch (slew_rate) {
+        case 0: st.SetParam(st.L64XX_CONFIG, config_temp | CONFIG_SR_75V_us); break;
+        default:
+        case 1: st.SetParam(st.L64XX_CONFIG, config_temp | CONFIG_SR_110V_us); break;
+        case 3:
+        case 2: st.SetParam(st.L64XX_CONFIG, config_temp | CONFIG_SR_260V_us); break;
+        //case 0: st.SetParam(st.L64XX_CONFIG, 0x2E88 | CONFIG_EN_TQREG | CONFIG_SR_75V_us); break;
+        //default:
+        //case 1: st.SetParam(st.L64XX_CONFIG, 0x2E88 | CONFIG_EN_TQREG | CONFIG_SR_110V_us); break;
+        //case 3:
+        //case 2: st.SetParam(st.L64XX_CONFIG, 0x2E88 | CONFIG_EN_TQREG | CONFIG_SR_260V_us); break;
+
+        //case 0: st.SetParam(st.L64XX_CONFIG, 0x2E88 ); break;
+        //default:
+        //case 1: st.SetParam(st.L64XX_CONFIG, 0x2E88 ); break;
+        //case 3:
+        //case 2: st.SetParam(st.L64XX_CONFIG, 0x2E88 ); break;
+      }
+      st.getStatus();
+      st.getStatus();
+      break;
+    }
+
+    case L6480_STATUS_LAYOUT: {
+      st.resetDev();
+      st.softFree();
+      st.SetParam(st.L64XX_CONFIG, CONFIG_PWM_DIV_1 | CONFIG_PWM_MUL_2 | CONFIG_OC_SD_DISABLE | CONFIG_VS_COMP_DISABLE | CONFIG_SW_HARD_STOP | CONFIG_INT_16MHZ);
+      st.SetParam(L6470_KVAL_RUN, 0xFF);
+      st.SetParam(L6470_KVAL_ACC, 0xFF);
+      st.SetParam(L6470_KVAL_DEC, 0xFF);
+      st.setMicroSteps(ms);
+      st.setOverCurrent(oc);
+      st.setStallCurrent(sc);
+      st.SetParam(+-L6470_KVAL_HOLD, mv);
+      st.SetParam(L6470_ABS_POS, 0);
+      st.SetParam(st.L64XX_CONFIG,(st.GetParam(st.L64XX_CONFIG) | PWR_VCC_7_5V));
+      st.getStatus();     // must clear out status bits before can set slew rate
+      st.getStatus();
+      switch (slew_rate) {
+        case 0: st.SetParam(L6470_GATECFG1, CONFIG1_SR_220V_us); st.SetParam(L6470_GATECFG2, CONFIG2_SR_220V_us); break;
+        default:
+        case 1: st.SetParam(L6470_GATECFG1, CONFIG1_SR_400V_us); st.SetParam(L6470_GATECFG2, CONFIG2_SR_400V_us); break;
+        case 2: st.SetParam(L6470_GATECFG1, CONFIG1_SR_520V_us); st.SetParam(L6470_GATECFG2, CONFIG2_SR_520V_us); break;
+        case 3: st.SetParam(L6470_GATECFG1, CONFIG1_SR_980V_us); st.SetParam(L6470_GATECFG2, CONFIG2_SR_980V_us); break;
+      }
+      break;
+    }
+  }
+}
+
+#define L6470_INIT_CHIP(Q) L6470_init_chip(stepper##Q, Q##_MICROSTEPS, Q##_OVERCURRENT, Q##_STALLCURRENT, Q##_MAX_VOLTAGE, Q##_SLEW_RATE)
+
+void L64XX_Marlin::init_to_defaults() {
+  #if AXIS_IS_L64XX(X)
+    L6470_INIT_CHIP(X);
+  #endif
+  #if AXIS_IS_L64XX(X2)
+    L6470_INIT_CHIP(X2);
+  #endif
+  #if AXIS_IS_L64XX(Y)
+    L6470_INIT_CHIP(Y);
+  #endif
+  #if AXIS_IS_L64XX(Y2)
+    L6470_INIT_CHIP(Y2);
+  #endif
+  #if AXIS_IS_L64XX(Z)
+    L6470_INIT_CHIP(Z);
+  #endif
+  #if AXIS_IS_L64XX(Z2)
+    L6470_INIT_CHIP(Z2);
+  #endif
+  #if AXIS_IS_L64XX(Z3)
+    L6470_INIT_CHIP(Z3);
+  #endif
+  #if AXIS_IS_L64XX(Z4)
+    L6470_INIT_CHIP(Z4);
+  #endif
+  #if AXIS_IS_L64XX(I)
+    L6470_INIT_CHIP(I);
+  #endif
+  #if AXIS_IS_L64XX(J)
+    L6470_INIT_CHIP(J);
+  #endif
+  #if AXIS_IS_L64XX(K)
+    L6470_INIT_CHIP(K);
+  #endif
+  #if AXIS_IS_L64XX(E0)
+    L6470_INIT_CHIP(E0);
+  #endif
+  #if AXIS_IS_L64XX(E1)
+    L6470_INIT_CHIP(E1);
+  #endif
+  #if AXIS_IS_L64XX(E2)
+    L6470_INIT_CHIP(E2);
+  #endif
+  #if AXIS_IS_L64XX(E3)
+    L6470_INIT_CHIP(E3);
+  #endif
+  #if AXIS_IS_L64XX(E4)
+    L6470_INIT_CHIP(E4);
+  #endif
+  #if AXIS_IS_L64XX(E5)
+    L6470_INIT_CHIP(E5);
+  #endif
+  #if AXIS_IS_L64XX(E6)
+    L6470_INIT_CHIP(E6);
+  #endif
+  #if AXIS_IS_L64XX(E7)
+    L6470_INIT_CHIP(E7);
+  #endif
+}
+
+#endif // HAS_L64XX

src/module/stepper/L64xx.h

@@ -0,0 +1,424 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * stepper/L64xx.h
+ * Stepper driver indirection for L64XX drivers
+ */
+
+#include "../../inc/MarlinConfig.h"
+#include "../../libs/L64XX/L64XX_Marlin.h"
+
+// Convert option names to L64XX classes
+#define CLASS_L6470       L6470
+#define CLASS_L6474       L6474
+#define CLASS_POWERSTEP01 powerSTEP01
+
+#define __L64XX_CLASS(TYPE) CLASS_##TYPE
+#define _L64XX_CLASS(TYPE)  __L64XX_CLASS(TYPE)
+#define L64XX_CLASS(ST)     _L64XX_CLASS(ST##_DRIVER_TYPE)
+
+#define L6474_DIR_WRITE(A,STATE) do{ L64xxManager.dir_commands[A] = dSPIN_L6474_ENABLE; WRITE(A##_DIR_PIN, STATE); }while(0)
+#define L64XX_DIR_WRITE(A,STATE) do{ L64xxManager.dir_commands[A] = (STATE) ? dSPIN_STEP_CLOCK_REV : dSPIN_STEP_CLOCK_FWD; }while(0)
+
+// X Stepper
+#if AXIS_IS_L64XX(X)
+  extern L64XX_CLASS(X)         stepperX;
+  #define X_ENABLE_INIT()       NOOP
+  #define X_ENABLE_WRITE(STATE) (STATE ? stepperX.hardStop() : stepperX.free())
+  #define X_ENABLE_READ()       (stepperX.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_X(L6474)
+    #define X_DIR_INIT()        SET_OUTPUT(X_DIR_PIN)
+    #define X_DIR_WRITE(STATE)  L6474_DIR_WRITE(X, STATE)
+    #define X_DIR_READ()        READ(X_DIR_PIN)
+  #else
+    #define X_DIR_INIT()        NOOP
+    #define X_DIR_WRITE(STATE)  L64XX_DIR_WRITE(X, STATE)
+    #define X_DIR_READ()        (stepper##X.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_X(L6470)
+      #define DISABLE_STEPPER_X() stepperX.free()
+    #endif
+  #endif
+#endif
+
+// Y Stepper
+#if AXIS_IS_L64XX(Y)
+  extern L64XX_CLASS(Y)         stepperY;
+  #define Y_ENABLE_INIT()       NOOP
+  #define Y_ENABLE_WRITE(STATE) (STATE ? stepperY.hardStop() : stepperY.free())
+  #define Y_ENABLE_READ()       (stepperY.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_Y(L6474)
+    #define Y_DIR_INIT()        SET_OUTPUT(Y_DIR_PIN)
+    #define Y_DIR_WRITE(STATE)  L6474_DIR_WRITE(Y, STATE)
+    #define Y_DIR_READ()        READ(Y_DIR_PIN)
+  #else
+    #define Y_DIR_INIT()        NOOP
+    #define Y_DIR_WRITE(STATE)  L64XX_DIR_WRITE(Y, STATE)
+    #define Y_DIR_READ()        (stepper##Y.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_Y(L6470)
+      #define DISABLE_STEPPER_Y() stepperY.free()
+    #endif
+  #endif
+#endif
+
+// Z Stepper
+#if AXIS_IS_L64XX(Z)
+  extern L64XX_CLASS(Z)         stepperZ;
+  #define Z_ENABLE_INIT()       NOOP
+  #define Z_ENABLE_WRITE(STATE) (STATE ? stepperZ.hardStop() : stepperZ.free())
+  #define Z_ENABLE_READ()       (stepperZ.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_Z(L6474)
+    #define Z_DIR_INIT()        SET_OUTPUT(Z_DIR_PIN)
+    #define Z_DIR_WRITE(STATE)  L6474_DIR_WRITE(Z, STATE)
+    #define Z_DIR_READ()        READ(Z_DIR_PIN)
+  #else
+    #define Z_DIR_INIT()        NOOP
+    #define Z_DIR_WRITE(STATE)  L64XX_DIR_WRITE(Z, STATE)
+    #define Z_DIR_READ()        (stepper##Z.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_Z(L6470)
+      #define DISABLE_STEPPER_Z() stepperZ.free()
+    #endif
+  #endif
+#endif
+
+// X2 Stepper
+#if HAS_X2_ENABLE && AXIS_IS_L64XX(X2)
+  extern L64XX_CLASS(X2)         stepperX2;
+  #define X2_ENABLE_INIT()       NOOP
+  #define X2_ENABLE_WRITE(STATE) (STATE ? stepperX2.hardStop() : stepperX2.free())
+  #define X2_ENABLE_READ()       (stepperX2.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_X2(L6474)
+    #define X2_DIR_INIT()        SET_OUTPUT(X2_DIR_PIN)
+    #define X2_DIR_WRITE(STATE)  L6474_DIR_WRITE(X2, STATE)
+    #define X2_DIR_READ()        READ(X2_DIR_PIN)
+  #else
+    #define X2_DIR_INIT()        NOOP
+    #define X2_DIR_WRITE(STATE)  L64XX_DIR_WRITE(X2, STATE)
+    #define X2_DIR_READ()        (stepper##X2.getStatus() & STATUS_DIR);
+  #endif
+#endif
+
+#if AXIS_DRIVER_TYPE_X2(L6470)
+  #define DISABLE_STEPPER_X2() stepperX2.free()
+#endif
+
+// Y2 Stepper
+#if HAS_Y2_ENABLE && AXIS_IS_L64XX(Y2)
+  extern L64XX_CLASS(Y2)         stepperY2;
+  #define Y2_ENABLE_INIT()       NOOP
+  #define Y2_ENABLE_WRITE(STATE) (STATE ? stepperY2.hardStop() : stepperY2.free())
+  #define Y2_ENABLE_READ()       (stepperY2.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_Y2(L6474)
+    #define Y2_DIR_INIT()        SET_OUTPUT(Y2_DIR_PIN)
+    #define Y2_DIR_WRITE(STATE)  L6474_DIR_WRITE(Y2, STATE)
+    #define Y2_DIR_READ()        READ(Y2_DIR_PIN)
+  #else
+    #define Y2_DIR_INIT()        NOOP
+    #define Y2_DIR_WRITE(STATE)  L64XX_DIR_WRITE(Y2, STATE)
+    #define Y2_DIR_READ()        (stepper##Y2.getStatus() & STATUS_DIR);
+  #endif
+#endif
+
+#if AXIS_DRIVER_TYPE_Y2(L6470)
+  #define DISABLE_STEPPER_Y2() stepperY2.free()
+#endif
+
+// Z2 Stepper
+#if HAS_Z2_ENABLE && AXIS_IS_L64XX(Z2)
+  extern L64XX_CLASS(Z2)         stepperZ2;
+  #define Z2_ENABLE_INIT()       NOOP
+  #define Z2_ENABLE_WRITE(STATE) (STATE ? stepperZ2.hardStop() : stepperZ2.free())
+  #define Z2_ENABLE_READ()       (stepperZ2.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_Z2(L6474)
+    #define Z2_DIR_INIT()        SET_OUTPUT(Z2_DIR_PIN)
+    #define Z2_DIR_WRITE(STATE)  L6474_DIR_WRITE(Z2, STATE)
+    #define Z2_DIR_READ()        READ(Z2_DIR_PIN)
+  #else
+    #define Z2_DIR_INIT()        NOOP
+    #define Z2_DIR_WRITE(STATE)  L64XX_DIR_WRITE(Z2, STATE)
+    #define Z2_DIR_READ()        (stepper##Z2.getStatus() & STATUS_DIR);
+  #endif
+#endif
+
+#if AXIS_DRIVER_TYPE_Z2(L6470)
+  #define DISABLE_STEPPER_Z2() stepperZ2.free()
+#endif
+
+// Z3 Stepper
+#if HAS_Z3_ENABLE && AXIS_IS_L64XX(Z3)
+  extern L64XX_CLASS(Z3)         stepperZ3;
+  #define Z3_ENABLE_INIT()       NOOP
+  #define Z3_ENABLE_WRITE(STATE) (STATE ? stepperZ3.hardStop() : stepperZ3.free())
+  #define Z3_ENABLE_READ()       (stepperZ3.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_Z3(L6474)
+    #define Z3_DIR_INIT()        SET_OUTPUT(Z3_DIR_PIN)
+    #define Z3_DIR_WRITE(STATE)  L6474_DIR_WRITE(Z3, STATE)
+    #define Z3_DIR_READ()        READ(Z3_DIR_PIN)
+  #else
+    #define Z3_DIR_INIT()        NOOP
+    #define Z3_DIR_WRITE(STATE)  L64XX_DIR_WRITE(Z3, STATE)
+    #define Z3_DIR_READ()        (stepper##Z3.getStatus() & STATUS_DIR);
+  #endif
+#endif
+
+#if AXIS_DRIVER_TYPE_Z3(L6470)
+  #define DISABLE_STEPPER_Z3() stepperZ3.free()
+#endif
+
+// Z4 Stepper
+#if HAS_Z4_ENABLE && AXIS_IS_L64XX(Z4)
+  extern L64XX_CLASS(Z4)         stepperZ4;
+  #define Z4_ENABLE_INIT()       NOOP
+  #define Z4_ENABLE_WRITE(STATE) (STATE ? stepperZ4.hardStop() : stepperZ4.free())
+  #define Z4_ENABLE_READ()       (stepperZ4.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_Z4(L6474)
+    #define Z4_DIR_INIT()        SET_OUTPUT(Z4_DIR_PIN)
+    #define Z4_DIR_WRITE(STATE)  L6474_DIR_WRITE(Z4, STATE)
+    #define Z4_DIR_READ()        READ(Z4_DIR_PIN)
+  #else
+    #define Z4_DIR_INIT()        NOOP
+    #define Z4_DIR_WRITE(STATE)  L64XX_DIR_WRITE(Z4, STATE)
+    #define Z4_DIR_READ()        (stepper##Z4.getStatus() & STATUS_DIR);
+  #endif
+#endif
+
+#if AXIS_DRIVER_TYPE_Z4(L6470)
+  #define DISABLE_STEPPER_Z4() stepperZ4.free()
+#endif
+
+// I Stepper
+#if AXIS_IS_L64XX(I)
+  extern L64XX_CLASS(I)         stepperI;
+  #define I_ENABLE_INIT()       NOOP
+  #define I_ENABLE_WRITE(STATE) (STATE ? stepperI.hardStop() : stepperI.free())
+  #define I_ENABLE_READ()       (stepperI.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_I(L6474)
+    #define I_DIR_INIT()        SET_OUTPUT(I_DIR_PIN)
+    #define I_DIR_WRITE(STATE)  L6474_DIR_WRITE(I, STATE)
+    #define I_DIR_READ()        READ(I_DIR_PIN)
+  #else
+    #define I_DIR_INIT()        NOOP
+    #define I_DIR_WRITE(STATE)  L64XX_DIR_WRITE(I, STATE)
+    #define I_DIR_READ()        (stepper##I.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_I(L6470)
+      #define DISABLE_STEPPER_I() stepperI.free()
+    #endif
+  #endif
+#endif
+
+// J Stepper
+#if AXIS_IS_L64XX(J)
+  extern L64XX_CLASS(J)         stepperJ;
+  #define J_ENABLE_INIT()       NOOP
+  #define J_ENABLE_WRITE(STATE) (STATE ? stepperJ.hardStop() : stepperJ.free())
+  #define J_ENABLE_READ()       (stepperJ.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_J(L6474)
+    #define J_DIR_INIT()        SET_OUTPUT(J_DIR_PIN)
+    #define J_DIR_WRITE(STATE)  L6474_DIR_WRITE(J, STATE)
+    #define J_DIR_READ()        READ(J_DIR_PIN)
+  #else
+    #define J_DIR_INIT()        NOOP
+    #define J_DIR_WRITE(STATE)  L64XX_DIR_WRITE(J, STATE)
+    #define J_DIR_READ()        (stepper##J.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_J(L6470)
+      #define DISABLE_STEPPER_J() stepperJ.free()
+    #endif
+  #endif
+#endif
+
+// K Stepper
+#if AXIS_IS_L64XX(K)
+  extern L64XX_CLASS(K)         stepperK;
+  #define K_ENABLE_INIT()       NOOP
+  #define K_ENABLE_WRITE(STATE) (STATE ? stepperK.hardStop() : stepperK.free())
+  #define K_ENABLE_READ()       (stepperK.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_K(L6474)
+    #define K_DIR_INIT()        SET_OUTPUT(K_DIR_PIN)
+    #define K_DIR_WRITE(STATE)  L6474_DIR_WRITE(K, STATE)
+    #define K_DIR_READ()        READ(K_DIR_PIN)
+  #else
+    #define K_DIR_INIT()        NOOP
+    #define K_DIR_WRITE(STATE)  L64XX_DIR_WRITE(K, STATE)
+    #define K_DIR_READ()        (stepper##K.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_K(L6470)
+      #define DISABLE_STEPPER_K() stepperK.free()
+    #endif
+  #endif
+#endif
+
+// E0 Stepper
+#if AXIS_IS_L64XX(E0)
+  extern L64XX_CLASS(E0)         stepperE0;
+  #define E0_ENABLE_INIT()       NOOP
+  #define E0_ENABLE_WRITE(STATE)  (STATE ? stepperE0.hardStop() : stepperE0.free())
+  #define E0_ENABLE_READ()       (stepperE0.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_E0(L6474)
+    #define E0_DIR_INIT()        SET_OUTPUT(E0_DIR_PIN)
+    #define E0_DIR_WRITE(STATE)  L6474_DIR_WRITE(E0, STATE)
+    #define E0_DIR_READ()        READ(E0_DIR_PIN)
+  #else
+    #define E0_DIR_INIT()        NOOP
+    #define E0_DIR_WRITE(STATE)  L64XX_DIR_WRITE(E0, STATE)
+    #define E0_DIR_READ()        (stepper##E0.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_E0(L6470)
+      #define DISABLE_STEPPER_E0() do{ stepperE0.free(); }while(0)
+    #endif
+  #endif
+#endif
+
+// E1 Stepper
+#if AXIS_IS_L64XX(E1)
+  extern L64XX_CLASS(E1)         stepperE1;
+  #define E1_ENABLE_INIT()       NOOP
+  #define E1_ENABLE_WRITE(STATE) (STATE ? stepperE1.hardStop() : stepperE1.free())
+  #define E1_ENABLE_READ()       (stepperE1.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_E1(L6474)
+    #define E1_DIR_INIT()        SET_OUTPUT(E1_DIR_PIN)
+    #define E1_DIR_WRITE(STATE)  L6474_DIR_WRITE(E1, STATE)
+    #define E1_DIR_READ()        READ(E1_DIR_PIN)
+  #else
+    #define E1_DIR_INIT()        NOOP
+    #define E1_DIR_WRITE(STATE)  L64XX_DIR_WRITE(E1, STATE)
+    #define E1_DIR_READ()        (stepper##E1.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_E1(L6470)
+      #define DISABLE_STEPPER_E1() do{ stepperE1.free(); }while(0)
+    #endif
+  #endif
+#endif
+
+// E2 Stepper
+#if AXIS_IS_L64XX(E2)
+  extern L64XX_CLASS(E2)         stepperE2;
+  #define E2_ENABLE_INIT()       NOOP
+  #define E2_ENABLE_WRITE(STATE) (STATE ? stepperE2.hardStop() : stepperE2.free())
+  #define E2_ENABLE_READ()       (stepperE2.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_E2(L6474)
+    #define E2_DIR_INIT()        SET_OUTPUT(E2_DIR_PIN)
+    #define E2_DIR_WRITE(STATE)  L6474_DIR_WRITE(E2, STATE)
+    #define E2_DIR_READ()        READ(E2_DIR_PIN)
+  #else
+    #define E2_DIR_INIT()        NOOP
+    #define E2_DIR_WRITE(STATE)  L64XX_DIR_WRITE(E2, STATE)
+    #define E2_DIR_READ()        (stepper##E2.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_E2(L6470)
+      #define DISABLE_STEPPER_E2() do{ stepperE2.free(); }while(0)
+    #endif
+  #endif
+#endif
+
+// E3 Stepper
+#if AXIS_IS_L64XX(E3)
+  extern L64XX_CLASS(E3)         stepperE3;
+  #define E3_ENABLE_INIT()       NOOP
+  #define E3_ENABLE_WRITE(STATE) (STATE ? stepperE3.hardStop() : stepperE3.free())
+  #define E3_ENABLE_READ()       (stepperE3.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_E3(L6474)
+    #define E3_DIR_INIT()        SET_OUTPUT(E3_DIR_PIN)
+    #define E3_DIR_WRITE(STATE)  L6474_DIR_WRITE(E3, STATE)
+    #define E3_DIR_READ()        READ(E3_DIR_PIN)
+  #else
+    #define E3_DIR_INIT()        NOOP
+    #define E3_DIR_WRITE(STATE)  L64XX_DIR_WRITE(E3, STATE)
+    #define E3_DIR_READ()        (stepper##E3.getStatus() & STATUS_DIR);
+  #endif
+#endif
+
+// E4 Stepper
+#if AXIS_IS_L64XX(E4)
+  extern L64XX_CLASS(E4)         stepperE4;
+  #define E4_ENABLE_INIT()       NOOP
+  #define E4_ENABLE_WRITE(STATE) (STATE ? stepperE4.hardStop() : stepperE4.free())
+  #define E4_ENABLE_READ()       (stepperE4.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_E4(L6474)
+    #define E4_DIR_INIT()        SET_OUTPUT(E4_DIR_PIN)
+    #define E4_DIR_WRITE(STATE)  L6474_DIR_WRITE(E4, STATE)
+    #define E4_DIR_READ()        READ(E4_DIR_PIN)
+  #else
+    #define E4_DIR_INIT()        NOOP
+    #define E4_DIR_WRITE(STATE)  L64XX_DIR_WRITE(E4, STATE)
+    #define E4_DIR_READ()        (stepper##E4.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_E4(L6470)
+      #define DISABLE_STEPPER_E4() do{ stepperE4.free(); }while(0)
+    #endif
+  #endif
+#endif
+
+// E5 Stepper
+#if AXIS_IS_L64XX(E5)
+  extern L64XX_CLASS(E5)         stepperE5;
+  #define E5_ENABLE_INIT()       NOOP
+  #define E5_ENABLE_WRITE(STATE) (STATE ? stepperE5.hardStop() : stepperE5.free())
+  #define E5_ENABLE_READ()       (stepperE5.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_E5(L6474)
+    #define E5_DIR_INIT()        SET_OUTPUT(E5_DIR_PIN)
+    #define E5_DIR_WRITE(STATE)  L6474_DIR_WRITE(E5, STATE)
+    #define E5_DIR_READ()        READ(E5_DIR_PIN)
+  #else
+    #define E5_DIR_INIT()        NOOP
+    #define E5_DIR_WRITE(STATE)  L64XX_DIR_WRITE(E5, STATE)
+    #define E5_DIR_READ()        (stepper##E5.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_E5(L6470)
+      #define DISABLE_STEPPER_E5() do{ stepperE5.free(); }while(0)
+    #endif
+  #endif
+#endif
+
+// E6 Stepper
+#if AXIS_IS_L64XX(E6)
+  extern L64XX_CLASS(E6)         stepperE6;
+  #define E6_ENABLE_INIT()       NOOP
+  #define E6_ENABLE_WRITE(STATE) (STATE ? stepperE6.hardStop() : stepperE6.free())
+  #define E6_ENABLE_READ()       (stepperE6.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_E6(L6474)
+    #define E6_DIR_INIT()        SET_OUTPUT(E6_DIR_PIN)
+    #define E6_DIR_WRITE(STATE)  L6474_DIR_WRITE(E6, STATE)
+    #define E6_DIR_READ()        READ(E6_DIR_PIN)
+  #else
+    #define E6_DIR_INIT()        NOOP
+    #define E6_DIR_WRITE(STATE)  L64XX_DIR_WRITE(E6, STATE)
+    #define E6_DIR_READ()        (stepper##E6.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_E6(L6470)
+      #define DISABLE_STEPPER_E6() do{ stepperE6.free(); }while(0)
+    #endif
+  #endif
+#endif
+
+// E7 Stepper
+#if AXIS_IS_L64XX(E7)
+  extern L64XX_CLASS(E7)         stepperE7;
+  #define E7_ENABLE_INIT()       NOOP
+  #define E7_ENABLE_WRITE(STATE) (STATE ? stepperE7.hardStop() : stepperE7.free())
+  #define E7_ENABLE_READ()       (stepperE7.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_E7(L6474)
+    #define E7_DIR_INIT()        SET_OUTPUT(E7_DIR_PIN)
+    #define E7_DIR_WRITE(STATE)  L6474_DIR_WRITE(E7, STATE)
+    #define E7_DIR_READ()        READ(E7_DIR_PIN)
+  #else
+    #define E7_DIR_INIT()        NOOP
+    #define E7_DIR_WRITE(STATE)  L64XX_DIR_WRITE(E7, STATE)
+    #define E7_DIR_READ()        (stepper##E7.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_E7(L6470)
+      #define DISABLE_STEPPER_E7() do{ stepperE7.free(); }while(0)
+    #endif
+  #endif
+#endif

src/module/stepper/TMC26X.cpp

@@ -0,0 +1,162 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * stepper/TMC26X.cpp
+ * Stepper driver indirection for TMC26X drivers
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+//
+// TMC26X Driver objects and inits
+//
+#if HAS_TMC26X
+
+#include "TMC26X.h"
+
+#define _TMC26X_DEFINE(ST) TMC26XStepper stepper##ST(200, ST##_CS_PIN, ST##_STEP_PIN, ST##_DIR_PIN, ST##_MAX_CURRENT, ST##_SENSE_RESISTOR)
+
+#if AXIS_DRIVER_TYPE_X(TMC26X)
+  _TMC26X_DEFINE(X);
+#endif
+#if AXIS_DRIVER_TYPE_X2(TMC26X)
+  _TMC26X_DEFINE(X2);
+#endif
+#if AXIS_DRIVER_TYPE_Y(TMC26X)
+  _TMC26X_DEFINE(Y);
+#endif
+#if AXIS_DRIVER_TYPE_Y2(TMC26X)
+  _TMC26X_DEFINE(Y2);
+#endif
+#if AXIS_DRIVER_TYPE_Z(TMC26X)
+  _TMC26X_DEFINE(Z);
+#endif
+#if AXIS_DRIVER_TYPE_Z2(TMC26X)
+  _TMC26X_DEFINE(Z2);
+#endif
+#if AXIS_DRIVER_TYPE_Z3(TMC26X)
+  _TMC26X_DEFINE(Z3);
+#endif
+#if AXIS_DRIVER_TYPE_Z4(TMC26X)
+  _TMC26X_DEFINE(Z4);
+#endif
+#if AXIS_DRIVER_TYPE_I(TMC26X)
+  _TMC26X_DEFINE(I);
+#endif
+#if AXIS_DRIVER_TYPE_J(TMC26X)
+  _TMC26X_DEFINE(J);
+#endif
+#if AXIS_DRIVER_TYPE_K(TMC26X)
+  _TMC26X_DEFINE(K);
+#endif
+#if AXIS_DRIVER_TYPE_E0(TMC26X)
+  _TMC26X_DEFINE(E0);
+#endif
+#if AXIS_DRIVER_TYPE_E1(TMC26X)
+  _TMC26X_DEFINE(E1);
+#endif
+#if AXIS_DRIVER_TYPE_E2(TMC26X)
+  _TMC26X_DEFINE(E2);
+#endif
+#if AXIS_DRIVER_TYPE_E3(TMC26X)
+  _TMC26X_DEFINE(E3);
+#endif
+#if AXIS_DRIVER_TYPE_E4(TMC26X)
+  _TMC26X_DEFINE(E4);
+#endif
+#if AXIS_DRIVER_TYPE_E5(TMC26X)
+  _TMC26X_DEFINE(E5);
+#endif
+#if AXIS_DRIVER_TYPE_E6(TMC26X)
+  _TMC26X_DEFINE(E6);
+#endif
+#if AXIS_DRIVER_TYPE_E7(TMC26X)
+  _TMC26X_DEFINE(E7);
+#endif
+
+#define _TMC26X_INIT(A) do{ \
+  stepper##A.setMicrosteps(A##_MICROSTEPS); \
+  stepper##A.start(); \
+}while(0)
+
+void tmc26x_init_to_defaults() {
+  #if AXIS_DRIVER_TYPE_X(TMC26X)
+    _TMC26X_INIT(X);
+  #endif
+  #if AXIS_DRIVER_TYPE_X2(TMC26X)
+    _TMC26X_INIT(X2);
+  #endif
+  #if AXIS_DRIVER_TYPE_Y(TMC26X)
+    _TMC26X_INIT(Y);
+  #endif
+  #if AXIS_DRIVER_TYPE_Y2(TMC26X)
+    _TMC26X_INIT(Y2);
+  #endif
+  #if AXIS_DRIVER_TYPE_Z(TMC26X)
+    _TMC26X_INIT(Z);
+  #endif
+  #if AXIS_DRIVER_TYPE_Z2(TMC26X)
+    _TMC26X_INIT(Z2);
+  #endif
+  #if AXIS_DRIVER_TYPE_Z3(TMC26X)
+    _TMC26X_INIT(Z3);
+  #endif
+  #if AXIS_DRIVER_TYPE_Z4(TMC26X)
+    _TMC26X_INIT(Z4);
+  #endif
+  #if AXIS_DRIVER_TYPE_I(TMC26X)
+     _TMC26X_INIT(I);
+  #endif
+  #if AXIS_DRIVER_TYPE_J(TMC26X)
+    _TMC26X_INIT(J);
+  #endif
+  #if AXIS_DRIVER_TYPE_K(TMC26X)
+    _TMC26X_INIT(K);
+  #endif
+  #if AXIS_DRIVER_TYPE_E0(TMC26X)
+    _TMC26X_INIT(E0);
+  #endif
+  #if AXIS_DRIVER_TYPE_E1(TMC26X)
+    _TMC26X_INIT(E1);
+  #endif
+  #if AXIS_DRIVER_TYPE_E2(TMC26X)
+    _TMC26X_INIT(E2);
+  #endif
+  #if AXIS_DRIVER_TYPE_E3(TMC26X)
+    _TMC26X_INIT(E3);
+  #endif
+  #if AXIS_DRIVER_TYPE_E4(TMC26X)
+    _TMC26X_INIT(E4);
+  #endif
+  #if AXIS_DRIVER_TYPE_E5(TMC26X)
+    _TMC26X_INIT(E5);
+  #endif
+  #if AXIS_DRIVER_TYPE_E6(TMC26X)
+    _TMC26X_INIT(E6);
+  #endif
+  #if AXIS_DRIVER_TYPE_E7(TMC26X)
+    _TMC26X_INIT(E7);
+  #endif
+}
+
+#endif // HAS_TMC26X

src/module/stepper/TMC26X.h

@@ -0,0 +1,188 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * stepper/TMC26X.h
+ * Stepper driver indirection for TMC26X drivers
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+// TMC26X drivers have STEP/DIR on normal pins, but ENABLE via SPI
+
+#include <SPI.h>
+#include <TMC26XStepper.h>
+
+void tmc26x_init_to_defaults();
+
+// X Stepper
+#if AXIS_DRIVER_TYPE_X(TMC26X)
+  extern TMC26XStepper stepperX;
+  #define X_ENABLE_INIT() NOOP
+  #define X_ENABLE_WRITE(STATE) stepperX.setEnabled(STATE)
+  #define X_ENABLE_READ() stepperX.isEnabled()
+#endif
+
+// Y Stepper
+#if AXIS_DRIVER_TYPE_Y(TMC26X)
+  extern TMC26XStepper stepperY;
+  #define Y_ENABLE_INIT() NOOP
+  #define Y_ENABLE_WRITE(STATE) stepperY.setEnabled(STATE)
+  #define Y_ENABLE_READ() stepperY.isEnabled()
+#endif
+
+// Z Stepper
+#if AXIS_DRIVER_TYPE_Z(TMC26X)
+  extern TMC26XStepper stepperZ;
+  #define Z_ENABLE_INIT() NOOP
+  #define Z_ENABLE_WRITE(STATE) stepperZ.setEnabled(STATE)
+  #define Z_ENABLE_READ() stepperZ.isEnabled()
+#endif
+
+// X2 Stepper
+#if HAS_X2_ENABLE && AXIS_DRIVER_TYPE_X2(TMC26X)
+  extern TMC26XStepper stepperX2;
+  #define X2_ENABLE_INIT() NOOP
+  #define X2_ENABLE_WRITE(STATE) stepperX2.setEnabled(STATE)
+  #define X2_ENABLE_READ() stepperX2.isEnabled()
+#endif
+
+// Y2 Stepper
+#if HAS_Y2_ENABLE && AXIS_DRIVER_TYPE_Y2(TMC26X)
+  extern TMC26XStepper stepperY2;
+  #define Y2_ENABLE_INIT() NOOP
+  #define Y2_ENABLE_WRITE(STATE) stepperY2.setEnabled(STATE)
+  #define Y2_ENABLE_READ() stepperY2.isEnabled()
+#endif
+
+// Z2 Stepper
+#if HAS_Z2_ENABLE && AXIS_DRIVER_TYPE_Z2(TMC26X)
+  extern TMC26XStepper stepperZ2;
+  #define Z2_ENABLE_INIT() NOOP
+  #define Z2_ENABLE_WRITE(STATE) stepperZ2.setEnabled(STATE)
+  #define Z2_ENABLE_READ() stepperZ2.isEnabled()
+#endif
+
+// Z3 Stepper
+#if HAS_Z3_ENABLE && AXIS_DRIVER_TYPE_Z3(TMC26X)
+  extern TMC26XStepper stepperZ3;
+  #define Z3_ENABLE_INIT() NOOP
+  #define Z3_ENABLE_WRITE(STATE) stepperZ3.setEnabled(STATE)
+  #define Z3_ENABLE_READ() stepperZ3.isEnabled()
+#endif
+
+// Z4 Stepper
+#if HAS_Z4_ENABLE && AXIS_DRIVER_TYPE_Z4(TMC26X)
+  extern TMC26XStepper stepperZ4;
+  #define Z4_ENABLE_INIT() NOOP
+  #define Z4_ENABLE_WRITE(STATE) stepperZ4.setEnabled(STATE)
+  #define Z4_ENABLE_READ() stepperZ4.isEnabled()
+#endif
+
+// I Stepper
+#if HAS_I_ENABLE && AXIS_DRIVER_TYPE_I(TMC26X)
+  extern TMC26XStepper stepperI;
+  #define I_ENABLE_INIT() NOOP
+  #define I_ENABLE_WRITE(STATE) stepperI.setEnabled(STATE)
+  #define I_ENABLE_READ() stepperI.isEnabled()
+#endif
+
+// J Stepper
+#if HAS_J_ENABLE && AXIS_DRIVER_TYPE_J(TMC26X)
+  extern TMC26XStepper stepperJ;
+  #define J_ENABLE_INIT() NOOP
+  #define J_ENABLE_WRITE(STATE) stepperJ.setEnabled(STATE)
+  #define J_ENABLE_READ() stepperJ.isEnabled()
+#endif
+
+// K Stepper
+#if HAS_K_ENABLE && AXIS_DRIVER_TYPE_K(TMC26X)
+  extern TMC26XStepper stepperK;
+  #define K_ENABLE_INIT() NOOP
+  #define K_ENABLE_WRITE(STATE) stepperK.setEnabled(STATE)
+  #define K_ENABLE_READ() stepperK.isEnabled()
+#endif
+
+// E0 Stepper
+#if AXIS_DRIVER_TYPE_E0(TMC26X)
+  extern TMC26XStepper stepperE0;
+  #define E0_ENABLE_INIT() NOOP
+  #define E0_ENABLE_WRITE(STATE) stepperE0.setEnabled(STATE)
+  #define E0_ENABLE_READ() stepperE0.isEnabled()
+#endif
+
+// E1 Stepper
+#if AXIS_DRIVER_TYPE_E1(TMC26X)
+  extern TMC26XStepper stepperE1;
+  #define E1_ENABLE_INIT() NOOP
+  #define E1_ENABLE_WRITE(STATE) stepperE1.setEnabled(STATE)
+  #define E1_ENABLE_READ() stepperE1.isEnabled()
+#endif
+
+// E2 Stepper
+#if AXIS_DRIVER_TYPE_E2(TMC26X)
+  extern TMC26XStepper stepperE2;
+  #define E2_ENABLE_INIT() NOOP
+  #define E2_ENABLE_WRITE(STATE) stepperE2.setEnabled(STATE)
+  #define E2_ENABLE_READ() stepperE2.isEnabled()
+#endif
+
+// E3 Stepper
+#if AXIS_DRIVER_TYPE_E3(TMC26X)
+  extern TMC26XStepper stepperE3;
+  #define E3_ENABLE_INIT() NOOP
+  #define E3_ENABLE_WRITE(STATE) stepperE3.setEnabled(STATE)
+  #define E3_ENABLE_READ() stepperE3.isEnabled()
+#endif
+
+// E4 Stepper
+#if AXIS_DRIVER_TYPE_E4(TMC26X)
+  extern TMC26XStepper stepperE4;
+  #define E4_ENABLE_INIT() NOOP
+  #define E4_ENABLE_WRITE(STATE) stepperE4.setEnabled(STATE)
+  #define E4_ENABLE_READ() stepperE4.isEnabled()
+#endif
+
+// E5 Stepper
+#if AXIS_DRIVER_TYPE_E5(TMC26X)
+  extern TMC26XStepper stepperE5;
+  #define E5_ENABLE_INIT() NOOP
+  #define E5_ENABLE_WRITE(STATE) stepperE5.setEnabled(STATE)
+  #define E5_ENABLE_READ() stepperE5.isEnabled()
+#endif
+
+// E6 Stepper
+#if AXIS_DRIVER_TYPE_E6(TMC26X)
+  extern TMC26XStepper stepperE6;
+  #define E6_ENABLE_INIT() NOOP
+  #define E6_ENABLE_WRITE(STATE) stepperE6.setEnabled(STATE)
+  #define E6_ENABLE_READ() stepperE6.isEnabled()
+#endif
+
+// E7 Stepper
+#if AXIS_DRIVER_TYPE_E7(TMC26X)
+  extern TMC26XStepper stepperE7;
+  #define E7_ENABLE_INIT() NOOP
+  #define E7_ENABLE_WRITE(STATE) stepperE7.setEnabled(STATE)
+  #define E7_ENABLE_READ() stepperE7.isEnabled()
+#endif

src/module/stepper/indirection.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/>.
+ *
+ */
+
+/**
+ * stepper/indirection.cpp
+ *
+ * Stepper motor driver indirection to allow some stepper functions to
+ * be done via SPI/I2c instead of direct pin manipulation.
+ *
+ * Copyright (c) 2015 Dominik Wenger
+ */
+
+#include "../../inc/MarlinConfig.h"
+#include "indirection.h"
+
+void restore_stepper_drivers() {
+  TERN_(HAS_TRINAMIC_CONFIG, restore_trinamic_drivers());
+}
+
+void reset_stepper_drivers() {
+  TERN_(HAS_TMC26X, tmc26x_init_to_defaults());
+  TERN_(HAS_L64XX, L64xxManager.init_to_defaults());
+  TERN_(HAS_TRINAMIC_CONFIG, reset_trinamic_drivers());
+}
+
+#if ENABLED(SOFTWARE_DRIVER_ENABLE)
+  // Flags to optimize XYZ Enabled state
+  xyz_bool_t axis_sw_enabled; // = { false, false, false }
+#endif

src/module/stepper/speed_lookuptable.h

@@ -0,0 +1,168 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#if F_CPU == 16000000
+
+  const uint16_t speed_lookuptable_fast[256][2] PROGMEM = {
+    { 62500, 55556}, { 6944, 3268}, { 3676, 1176}, { 2500, 607}, { 1893, 369}, { 1524, 249}, { 1275, 179}, { 1096, 135},
+    { 961, 105}, { 856, 85}, { 771, 69}, { 702, 58}, { 644, 49}, { 595, 42}, { 553, 37}, { 516, 32},
+    { 484, 28}, { 456, 25}, { 431, 23}, { 408, 20}, { 388, 19}, { 369, 16}, { 353, 16}, { 337, 14},
+    { 323, 13}, { 310, 11}, { 299, 11}, { 288, 11}, { 277, 9}, { 268, 9}, { 259, 8}, { 251, 8},
+    { 243, 8}, { 235, 7}, { 228, 6}, { 222, 6}, { 216, 6}, { 210, 6}, { 204, 5}, { 199, 5},
+    { 194, 5}, { 189, 4}, { 185, 4}, { 181, 4}, { 177, 4}, { 173, 4}, { 169, 4}, { 165, 3},
+    { 162, 3}, { 159, 4}, { 155, 3}, { 152, 3}, { 149, 2}, { 147, 3}, { 144, 3}, { 141, 2},
+    { 139, 3}, { 136, 2}, { 134, 2}, { 132, 3}, { 129, 2}, { 127, 2}, { 125, 2}, { 123, 2},
+    { 121, 2}, { 119, 1}, { 118, 2}, { 116, 2}, { 114, 1}, { 113, 2}, { 111, 2}, { 109, 1},
+    { 108, 2}, { 106, 1}, { 105, 2}, { 103, 1}, { 102, 1}, { 101, 1}, { 100, 2}, { 98, 1},
+    { 97, 1}, { 96, 1}, { 95, 2}, { 93, 1}, { 92, 1}, { 91, 1}, { 90, 1}, { 89, 1},
+    { 88, 1}, { 87, 1}, { 86, 1}, { 85, 1}, { 84, 1}, { 83, 0}, { 83, 1}, { 82, 1},
+    { 81, 1}, { 80, 1}, { 79, 1}, { 78, 0}, { 78, 1}, { 77, 1}, { 76, 1}, { 75, 0},
+    { 75, 1}, { 74, 1}, { 73, 1}, { 72, 0}, { 72, 1}, { 71, 1}, { 70, 0}, { 70, 1},
+    { 69, 0}, { 69, 1}, { 68, 1}, { 67, 0}, { 67, 1}, { 66, 0}, { 66, 1}, { 65, 0},
+    { 65, 1}, { 64, 1}, { 63, 0}, { 63, 1}, { 62, 0}, { 62, 1}, { 61, 0}, { 61, 1},
+    { 60, 0}, { 60, 0}, { 60, 1}, { 59, 0}, { 59, 1}, { 58, 0}, { 58, 1}, { 57, 0},
+    { 57, 1}, { 56, 0}, { 56, 0}, { 56, 1}, { 55, 0}, { 55, 1}, { 54, 0}, { 54, 0},
+    { 54, 1}, { 53, 0}, { 53, 0}, { 53, 1}, { 52, 0}, { 52, 0}, { 52, 1}, { 51, 0},
+    { 51, 0}, { 51, 1}, { 50, 0}, { 50, 0}, { 50, 1}, { 49, 0}, { 49, 0}, { 49, 1},
+    { 48, 0}, { 48, 0}, { 48, 1}, { 47, 0}, { 47, 0}, { 47, 0}, { 47, 1}, { 46, 0},
+    { 46, 0}, { 46, 1}, { 45, 0}, { 45, 0}, { 45, 0}, { 45, 1}, { 44, 0}, { 44, 0},
+    { 44, 0}, { 44, 1}, { 43, 0}, { 43, 0}, { 43, 0}, { 43, 1}, { 42, 0}, { 42, 0},
+    { 42, 0}, { 42, 1}, { 41, 0}, { 41, 0}, { 41, 0}, { 41, 0}, { 41, 1}, { 40, 0},
+    { 40, 0}, { 40, 0}, { 40, 0}, { 40, 1}, { 39, 0}, { 39, 0}, { 39, 0}, { 39, 0},
+    { 39, 1}, { 38, 0}, { 38, 0}, { 38, 0}, { 38, 0}, { 38, 1}, { 37, 0}, { 37, 0},
+    { 37, 0}, { 37, 0}, { 37, 0}, { 37, 1}, { 36, 0}, { 36, 0}, { 36, 0}, { 36, 0},
+    { 36, 1}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 1},
+    { 34, 0}, { 34, 0}, { 34, 0}, { 34, 0}, { 34, 0}, { 34, 1}, { 33, 0}, { 33, 0},
+    { 33, 0}, { 33, 0}, { 33, 0}, { 33, 0}, { 33, 1}, { 32, 0}, { 32, 0}, { 32, 0},
+    { 32, 0}, { 32, 0}, { 32, 0}, { 32, 0}, { 32, 1}, { 31, 0}, { 31, 0}, { 31, 0},
+    { 31, 0}, { 31, 0}, { 31, 0}, { 31, 1}, { 30, 0}, { 30, 0}, { 30, 0}, { 30, 0}
+  };
+
+  const uint16_t speed_lookuptable_slow[256][2] PROGMEM = {
+    { 62500, 12500}, { 50000, 8334}, { 41666, 5952}, { 35714, 4464}, { 31250, 3473}, { 27777, 2777}, { 25000, 2273}, { 22727, 1894},
+    { 20833, 1603}, { 19230, 1373}, { 17857, 1191}, { 16666, 1041}, { 15625, 920}, { 14705, 817}, { 13888, 731}, { 13157, 657},
+    { 12500, 596}, { 11904, 541}, { 11363, 494}, { 10869, 453}, { 10416, 416}, { 10000, 385}, { 9615, 356}, { 9259, 331},
+    { 8928, 308}, { 8620, 287}, { 8333, 269}, { 8064, 252}, { 7812, 237}, { 7575, 223}, { 7352, 210}, { 7142, 198},
+    { 6944, 188}, { 6756, 178}, { 6578, 168}, { 6410, 160}, { 6250, 153}, { 6097, 145}, { 5952, 139}, { 5813, 132},
+    { 5681, 126}, { 5555, 121}, { 5434, 115}, { 5319, 111}, { 5208, 106}, { 5102, 102}, { 5000, 99}, { 4901, 94},
+    { 4807, 91}, { 4716, 87}, { 4629, 84}, { 4545, 81}, { 4464, 79}, { 4385, 75}, { 4310, 73}, { 4237, 71},
+    { 4166, 68}, { 4098, 66}, { 4032, 64}, { 3968, 62}, { 3906, 60}, { 3846, 59}, { 3787, 56}, { 3731, 55},
+    { 3676, 53}, { 3623, 52}, { 3571, 50}, { 3521, 49}, { 3472, 48}, { 3424, 46}, { 3378, 45}, { 3333, 44},
+    { 3289, 43}, { 3246, 41}, { 3205, 41}, { 3164, 39}, { 3125, 39}, { 3086, 38}, { 3048, 36}, { 3012, 36},
+    { 2976, 35}, { 2941, 35}, { 2906, 33}, { 2873, 33}, { 2840, 32}, { 2808, 31}, { 2777, 30}, { 2747, 30},
+    { 2717, 29}, { 2688, 29}, { 2659, 28}, { 2631, 27}, { 2604, 27}, { 2577, 26}, { 2551, 26}, { 2525, 25},
+    { 2500, 25}, { 2475, 25}, { 2450, 23}, { 2427, 24}, { 2403, 23}, { 2380, 22}, { 2358, 22}, { 2336, 22},
+    { 2314, 21}, { 2293, 21}, { 2272, 20}, { 2252, 20}, { 2232, 20}, { 2212, 20}, { 2192, 19}, { 2173, 18},
+    { 2155, 19}, { 2136, 18}, { 2118, 18}, { 2100, 17}, { 2083, 17}, { 2066, 17}, { 2049, 17}, { 2032, 16},
+    { 2016, 16}, { 2000, 16}, { 1984, 16}, { 1968, 15}, { 1953, 16}, { 1937, 14}, { 1923, 15}, { 1908, 15},
+    { 1893, 14}, { 1879, 14}, { 1865, 14}, { 1851, 13}, { 1838, 14}, { 1824, 13}, { 1811, 13}, { 1798, 13},
+    { 1785, 12}, { 1773, 13}, { 1760, 12}, { 1748, 12}, { 1736, 12}, { 1724, 12}, { 1712, 12}, { 1700, 11},
+    { 1689, 12}, { 1677, 11}, { 1666, 11}, { 1655, 11}, { 1644, 11}, { 1633, 10}, { 1623, 11}, { 1612, 10},
+    { 1602, 10}, { 1592, 10}, { 1582, 10}, { 1572, 10}, { 1562, 10}, { 1552, 9}, { 1543, 10}, { 1533, 9},
+    { 1524, 9}, { 1515, 9}, { 1506, 9}, { 1497, 9}, { 1488, 9}, { 1479, 9}, { 1470, 9}, { 1461, 8},
+    { 1453, 8}, { 1445, 9}, { 1436, 8}, { 1428, 8}, { 1420, 8}, { 1412, 8}, { 1404, 8}, { 1396, 8},
+    { 1388, 7}, { 1381, 8}, { 1373, 7}, { 1366, 8}, { 1358, 7}, { 1351, 7}, { 1344, 8}, { 1336, 7},
+    { 1329, 7}, { 1322, 7}, { 1315, 7}, { 1308, 6}, { 1302, 7}, { 1295, 7}, { 1288, 6}, { 1282, 7},
+    { 1275, 6}, { 1269, 7}, { 1262, 6}, { 1256, 6}, { 1250, 7}, { 1243, 6}, { 1237, 6}, { 1231, 6},
+    { 1225, 6}, { 1219, 6}, { 1213, 6}, { 1207, 6}, { 1201, 5}, { 1196, 6}, { 1190, 6}, { 1184, 5},
+    { 1179, 6}, { 1173, 5}, { 1168, 6}, { 1162, 5}, { 1157, 5}, { 1152, 6}, { 1146, 5}, { 1141, 5},
+    { 1136, 5}, { 1131, 5}, { 1126, 5}, { 1121, 5}, { 1116, 5}, { 1111, 5}, { 1106, 5}, { 1101, 5},
+    { 1096, 5}, { 1091, 5}, { 1086, 4}, { 1082, 5}, { 1077, 5}, { 1072, 4}, { 1068, 5}, { 1063, 4},
+    { 1059, 5}, { 1054, 4}, { 1050, 4}, { 1046, 5}, { 1041, 4}, { 1037, 4}, { 1033, 5}, { 1028, 4},
+    { 1024, 4}, { 1020, 4}, { 1016, 4}, { 1012, 4}, { 1008, 4}, { 1004, 4}, { 1000, 4}, { 996, 4},
+    { 992, 4}, { 988, 4}, { 984, 4}, { 980, 4}, { 976, 4}, { 972, 4}, { 968, 3}, { 965, 3}
+  };
+
+#elif F_CPU == 20000000
+
+  const uint16_t speed_lookuptable_fast[256][2] PROGMEM = {
+    {62500, 54055}, {8445, 3917}, {4528, 1434}, {3094, 745}, {2349, 456}, {1893, 307}, {1586, 222}, {1364, 167},
+    {1197, 131}, {1066, 105}, {961, 86}, {875, 72}, {803, 61}, {742, 53}, {689, 45}, {644, 40},
+    {604, 35}, {569, 32}, {537, 28}, {509, 25}, {484, 23}, {461, 21}, {440, 19}, {421, 17},
+    {404, 16}, {388, 15}, {373, 14}, {359, 13}, {346, 12}, {334, 11}, {323, 10}, {313, 10},
+    {303, 9}, {294, 9}, {285, 8}, {277, 7}, {270, 8}, {262, 7}, {255, 6}, {249, 6},
+    {243, 6}, {237, 6}, {231, 5}, {226, 5}, {221, 5}, {216, 5}, {211, 4}, {207, 5},
+    {202, 4}, {198, 4}, {194, 4}, {190, 3}, {187, 4}, {183, 3}, {180, 3}, {177, 4},
+    {173, 3}, {170, 3}, {167, 2}, {165, 3}, {162, 3}, {159, 2}, {157, 3}, {154, 2},
+    {152, 3}, {149, 2}, {147, 2}, {145, 2}, {143, 2}, {141, 2}, {139, 2}, {137, 2},
+    {135, 2}, {133, 2}, {131, 2}, {129, 1}, {128, 2}, {126, 2}, {124, 1}, {123, 2},
+    {121, 1}, {120, 2}, {118, 1}, {117, 1}, {116, 2}, {114, 1}, {113, 1}, {112, 2},
+    {110, 1}, {109, 1}, {108, 1}, {107, 2}, {105, 1}, {104, 1}, {103, 1}, {102, 1},
+    {101, 1}, {100, 1}, {99, 1}, {98, 1}, {97, 1}, {96, 1}, {95, 1}, {94, 1},
+    {93, 1}, {92, 1}, {91, 0}, {91, 1}, {90, 1}, {89, 1}, {88, 1}, {87, 0},
+    {87, 1}, {86, 1}, {85, 1}, {84, 0}, {84, 1}, {83, 1}, {82, 1}, {81, 0},
+    {81, 1}, {80, 1}, {79, 0}, {79, 1}, {78, 0}, {78, 1}, {77, 1}, {76, 0},
+    {76, 1}, {75, 0}, {75, 1}, {74, 1}, {73, 0}, {73, 1}, {72, 0}, {72, 1},
+    {71, 0}, {71, 1}, {70, 0}, {70, 1}, {69, 0}, {69, 1}, {68, 0}, {68, 1},
+    {67, 0}, {67, 1}, {66, 0}, {66, 1}, {65, 0}, {65, 0}, {65, 1}, {64, 0},
+    {64, 1}, {63, 0}, {63, 1}, {62, 0}, {62, 0}, {62, 1}, {61, 0}, {61, 1},
+    {60, 0}, {60, 0}, {60, 1}, {59, 0}, {59, 0}, {59, 1}, {58, 0}, {58, 0},
+    {58, 1}, {57, 0}, {57, 0}, {57, 1}, {56, 0}, {56, 0}, {56, 1}, {55, 0},
+    {55, 0}, {55, 1}, {54, 0}, {54, 0}, {54, 1}, {53, 0}, {53, 0}, {53, 0},
+    {53, 1}, {52, 0}, {52, 0}, {52, 1}, {51, 0}, {51, 0}, {51, 0}, {51, 1},
+    {50, 0}, {50, 0}, {50, 0}, {50, 1}, {49, 0}, {49, 0}, {49, 0}, {49, 1},
+    {48, 0}, {48, 0}, {48, 0}, {48, 1}, {47, 0}, {47, 0}, {47, 0}, {47, 1},
+    {46, 0}, {46, 0}, {46, 0}, {46, 0}, {46, 1}, {45, 0}, {45, 0}, {45, 0},
+    {45, 1}, {44, 0}, {44, 0}, {44, 0}, {44, 0}, {44, 1}, {43, 0}, {43, 0},
+    {43, 0}, {43, 0}, {43, 1}, {42, 0}, {42, 0}, {42, 0}, {42, 0}, {42, 0},
+    {42, 1}, {41, 0}, {41, 0}, {41, 0}, {41, 0}, {41, 0}, {41, 1}, {40, 0},
+    {40, 0}, {40, 0}, {40, 0}, {40, 1}, {39, 0}, {39, 0}, {39, 0}, {39, 0},
+    {39, 0}, {39, 0}, {39, 1}, {38, 0}, {38, 0}, {38, 0}, {38, 0}, {38, 0},
+  };
+
+  const uint16_t speed_lookuptable_slow[256][2] PROGMEM = {
+    {62500, 10417}, {52083, 7441}, {44642, 5580}, {39062, 4340}, {34722, 3472}, {31250, 2841}, {28409, 2368}, {26041, 2003},
+    {24038, 1717}, {22321, 1488}, {20833, 1302}, {19531, 1149}, {18382, 1021}, {17361, 914}, {16447, 822}, {15625, 745},
+    {14880, 676}, {14204, 618}, {13586, 566}, {13020, 520}, {12500, 481}, {12019, 445}, {11574, 414}, {11160, 385},
+    {10775, 359}, {10416, 336}, {10080, 315}, {9765, 296}, {9469, 278}, {9191, 263}, {8928, 248}, {8680, 235},
+    {8445, 222}, {8223, 211}, {8012, 200}, {7812, 191}, {7621, 181}, {7440, 173}, {7267, 165}, {7102, 158},
+    {6944, 151}, {6793, 145}, {6648, 138}, {6510, 133}, {6377, 127}, {6250, 123}, {6127, 118}, {6009, 113},
+    {5896, 109}, {5787, 106}, {5681, 101}, {5580, 98}, {5482, 95}, {5387, 91}, {5296, 88}, {5208, 86},
+    {5122, 82}, {5040, 80}, {4960, 78}, {4882, 75}, {4807, 73}, {4734, 70}, {4664, 69}, {4595, 67},
+    {4528, 64}, {4464, 63}, {4401, 61}, {4340, 60}, {4280, 58}, {4222, 56}, {4166, 55}, {4111, 53},
+    {4058, 52}, {4006, 51}, {3955, 49}, {3906, 48}, {3858, 48}, {3810, 45}, {3765, 45}, {3720, 44},
+    {3676, 43}, {3633, 42}, {3591, 40}, {3551, 40}, {3511, 39}, {3472, 38}, {3434, 38}, {3396, 36},
+    {3360, 36}, {3324, 35}, {3289, 34}, {3255, 34}, {3221, 33}, {3188, 32}, {3156, 31}, {3125, 31},
+    {3094, 31}, {3063, 30}, {3033, 29}, {3004, 28}, {2976, 28}, {2948, 28}, {2920, 27}, {2893, 27},
+    {2866, 26}, {2840, 25}, {2815, 25}, {2790, 25}, {2765, 24}, {2741, 24}, {2717, 24}, {2693, 23},
+    {2670, 22}, {2648, 22}, {2626, 22}, {2604, 22}, {2582, 21}, {2561, 21}, {2540, 20}, {2520, 20},
+    {2500, 20}, {2480, 20}, {2460, 19}, {2441, 19}, {2422, 19}, {2403, 18}, {2385, 18}, {2367, 18},
+    {2349, 17}, {2332, 18}, {2314, 17}, {2297, 16}, {2281, 17}, {2264, 16}, {2248, 16}, {2232, 16},
+    {2216, 16}, {2200, 15}, {2185, 15}, {2170, 15}, {2155, 15}, {2140, 15}, {2125, 14}, {2111, 14},
+    {2097, 14}, {2083, 14}, {2069, 14}, {2055, 13}, {2042, 13}, {2029, 13}, {2016, 13}, {2003, 13},
+    {1990, 13}, {1977, 12}, {1965, 12}, {1953, 13}, {1940, 11}, {1929, 12}, {1917, 12}, {1905, 12},
+    {1893, 11}, {1882, 11}, {1871, 11}, {1860, 11}, {1849, 11}, {1838, 11}, {1827, 11}, {1816, 10},
+    {1806, 11}, {1795, 10}, {1785, 10}, {1775, 10}, {1765, 10}, {1755, 10}, {1745, 9}, {1736, 10},
+    {1726, 9}, {1717, 10}, {1707, 9}, {1698, 9}, {1689, 9}, {1680, 9}, {1671, 9}, {1662, 9},
+    {1653, 9}, {1644, 8}, {1636, 9}, {1627, 8}, {1619, 9}, {1610, 8}, {1602, 8}, {1594, 8},
+    {1586, 8}, {1578, 8}, {1570, 8}, {1562, 8}, {1554, 7}, {1547, 8}, {1539, 8}, {1531, 7},
+    {1524, 8}, {1516, 7}, {1509, 7}, {1502, 7}, {1495, 7}, {1488, 7}, {1481, 7}, {1474, 7},
+    {1467, 7}, {1460, 7}, {1453, 7}, {1446, 6}, {1440, 7}, {1433, 7}, {1426, 6}, {1420, 6},
+    {1414, 7}, {1407, 6}, {1401, 6}, {1395, 7}, {1388, 6}, {1382, 6}, {1376, 6}, {1370, 6},
+    {1364, 6}, {1358, 6}, {1352, 6}, {1346, 5}, {1341, 6}, {1335, 6}, {1329, 5}, {1324, 6},
+    {1318, 5}, {1313, 6}, {1307, 5}, {1302, 6}, {1296, 5}, {1291, 5}, {1286, 6}, {1280, 5},
+    {1275, 5}, {1270, 5}, {1265, 5}, {1260, 5}, {1255, 5}, {1250, 5}, {1245, 5}, {1240, 5},
+    {1235, 5}, {1230, 5}, {1225, 5}, {1220, 5}, {1215, 4}, {1211, 5}, {1206, 5}, {1201, 5},
+  };
+
+#endif

src/module/stepper/trinamic.cpp

@@ -0,0 +1,988 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * stepper/trinamic.cpp
+ * Stepper driver indirection for Trinamic
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_TRINAMIC_CONFIG
+
+#include "trinamic.h"
+#include "../stepper.h"
+
+#include <HardwareSerial.h>
+#include <SPI.h>
+
+enum StealthIndex : uint8_t {
+  LOGICAL_AXIS_LIST(STEALTH_AXIS_E, STEALTH_AXIS_X, STEALTH_AXIS_Y, STEALTH_AXIS_Z, STEALTH_AXIS_I, STEALTH_AXIS_J, STEALTH_AXIS_K)
+};
+#define TMC_INIT(ST, STEALTH_INDEX) tmc_init(stepper##ST, ST##_CURRENT, ST##_MICROSTEPS, ST##_HYBRID_THRESHOLD, stealthchop_by_axis[STEALTH_INDEX], chopper_timing_##ST, ST##_INTERPOLATE, ST##_HOLD_MULTIPLIER)
+
+//   IC = TMC model number
+//   ST = Stepper object letter
+//   L  = Label characters
+//   AI = Axis Enum Index
+// SWHW = SW/SH UART selection
+#if ENABLED(TMC_USE_SW_SPI)
+  #define __TMC_SPI_DEFINE(IC, ST, L, AI) TMCMarlin<IC##Stepper, L, AI> stepper##ST(ST##_CS_PIN, float(ST##_RSENSE), TMC_SW_MOSI, TMC_SW_MISO, TMC_SW_SCK, ST##_CHAIN_POS)
+#else
+  #define __TMC_SPI_DEFINE(IC, ST, L, AI) TMCMarlin<IC##Stepper, L, AI> stepper##ST(ST##_CS_PIN, float(ST##_RSENSE), ST##_CHAIN_POS)
+#endif
+
+#if ENABLED(TMC_SERIAL_MULTIPLEXER)
+  #define TMC_UART_HW_DEFINE(IC, ST, L, AI) TMCMarlin<IC##Stepper, L, AI> stepper##ST(&ST##_HARDWARE_SERIAL, float(ST##_RSENSE), ST##_SLAVE_ADDRESS, SERIAL_MUL_PIN1, SERIAL_MUL_PIN2)
+#else
+  #define TMC_UART_HW_DEFINE(IC, ST, L, AI) TMCMarlin<IC##Stepper, L, AI> stepper##ST(&ST##_HARDWARE_SERIAL, float(ST##_RSENSE), ST##_SLAVE_ADDRESS)
+#endif
+#define TMC_UART_SW_DEFINE(IC, ST, L, AI) TMCMarlin<IC##Stepper, L, AI> stepper##ST(ST##_SERIAL_RX_PIN, ST##_SERIAL_TX_PIN, float(ST##_RSENSE), ST##_SLAVE_ADDRESS)
+
+#define _TMC_SPI_DEFINE(IC, ST, AI) __TMC_SPI_DEFINE(IC, ST, TMC_##ST##_LABEL, AI)
+#define TMC_SPI_DEFINE(ST, AI) _TMC_SPI_DEFINE(ST##_DRIVER_TYPE, ST, AI##_AXIS)
+
+#define _TMC_UART_DEFINE(SWHW, IC, ST, AI) TMC_UART_##SWHW##_DEFINE(IC, ST, TMC_##ST##_LABEL, AI)
+#define TMC_UART_DEFINE(SWHW, ST, AI) _TMC_UART_DEFINE(SWHW, ST##_DRIVER_TYPE, ST, AI##_AXIS)
+
+#if ENABLED(DISTINCT_E_FACTORS)
+  #define TMC_SPI_DEFINE_E(AI) TMC_SPI_DEFINE(E##AI, E##AI)
+  #define TMC_UART_DEFINE_E(SWHW, AI) TMC_UART_DEFINE(SWHW, E##AI, E##AI)
+#else
+  #define TMC_SPI_DEFINE_E(AI) TMC_SPI_DEFINE(E##AI, E)
+  #define TMC_UART_DEFINE_E(SWHW, AI) TMC_UART_DEFINE(SWHW, E##AI, E)
+#endif
+
+// Stepper objects of TMC2130/TMC2160/TMC2660/TMC5130/TMC5160 steppers used
+#if AXIS_HAS_SPI(X)
+  TMC_SPI_DEFINE(X, X);
+#endif
+#if AXIS_HAS_SPI(X2)
+  TMC_SPI_DEFINE(X2, X);
+#endif
+#if AXIS_HAS_SPI(Y)
+  TMC_SPI_DEFINE(Y, Y);
+#endif
+#if AXIS_HAS_SPI(Y2)
+  TMC_SPI_DEFINE(Y2, Y);
+#endif
+#if AXIS_HAS_SPI(Z)
+  TMC_SPI_DEFINE(Z, Z);
+#endif
+#if AXIS_HAS_SPI(Z2)
+  TMC_SPI_DEFINE(Z2, Z);
+#endif
+#if AXIS_HAS_SPI(Z3)
+  TMC_SPI_DEFINE(Z3, Z);
+#endif
+#if AXIS_HAS_SPI(Z4)
+  TMC_SPI_DEFINE(Z4, Z);
+#endif
+#if AXIS_HAS_SPI(I)
+  TMC_SPI_DEFINE(I, I);
+#endif
+#if AXIS_HAS_SPI(J)
+  TMC_SPI_DEFINE(J, J);
+#endif
+#if AXIS_HAS_SPI(K)
+  TMC_SPI_DEFINE(K, K);
+#endif
+#if AXIS_HAS_SPI(E0)
+  TMC_SPI_DEFINE_E(0);
+#endif
+#if AXIS_HAS_SPI(E1)
+  TMC_SPI_DEFINE_E(1);
+#endif
+#if AXIS_HAS_SPI(E2)
+  TMC_SPI_DEFINE_E(2);
+#endif
+#if AXIS_HAS_SPI(E3)
+  TMC_SPI_DEFINE_E(3);
+#endif
+#if AXIS_HAS_SPI(E4)
+  TMC_SPI_DEFINE_E(4);
+#endif
+#if AXIS_HAS_SPI(E5)
+  TMC_SPI_DEFINE_E(5);
+#endif
+#if AXIS_HAS_SPI(E6)
+  TMC_SPI_DEFINE_E(6);
+#endif
+#if AXIS_HAS_SPI(E7)
+  TMC_SPI_DEFINE_E(7);
+#endif
+
+#ifndef TMC_BAUD_RATE
+  // Reduce baud rate for boards not already overriding TMC_BAUD_RATE for software serial.
+  // Testing has shown that 115200 is not 100% reliable on AVR platforms, occasionally
+  // failing to read status properly. 32-bit platforms typically define an even lower
+  // TMC_BAUD_RATE, due to differences in how SoftwareSerial libraries work on different
+  // platforms.
+  #define TMC_BAUD_RATE TERN(HAS_TMC_SW_SERIAL, 57600, 115200)
+#endif
+
+#ifndef TMC_X_BAUD_RATE
+  #define TMC_X_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_X2_BAUD_RATE
+  #define TMC_X2_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_Y_BAUD_RATE
+  #define TMC_Y_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_Y2_BAUD_RATE
+  #define TMC_Y2_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_Z_BAUD_RATE
+  #define TMC_Z_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_Z2_BAUD_RATE
+  #define TMC_Z2_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_Z3_BAUD_RATE
+  #define TMC_Z3_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_Z4_BAUD_RATE
+  #define TMC_Z4_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_I_BAUD_RATE
+  #define TMC_I_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_J_BAUD_RATE
+  #define TMC_J_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_K_BAUD_RATE
+  #define TMC_K_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_E0_BAUD_RATE
+  #define TMC_E0_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_E1_BAUD_RATE
+  #define TMC_E1_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_E2_BAUD_RATE
+  #define TMC_E2_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_E3_BAUD_RATE
+  #define TMC_E3_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_E4_BAUD_RATE
+  #define TMC_E4_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_E5_BAUD_RATE
+  #define TMC_E5_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_E6_BAUD_RATE
+  #define TMC_E6_BAUD_RATE TMC_BAUD_RATE
+#endif
+#ifndef TMC_E7_BAUD_RATE
+  #define TMC_E7_BAUD_RATE TMC_BAUD_RATE
+#endif
+
+#if HAS_DRIVER(TMC2130)
+  template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+  void tmc_init(TMCMarlin<TMC2130Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &st, const uint16_t mA, const uint16_t microsteps, const uint32_t hyb_thrs, const bool stealth, const chopper_timing_t &chop_init, const bool interpolate, float hold_multiplier) {
+    st.begin();
+
+    CHOPCONF_t chopconf{0};
+    chopconf.tbl = 0b01;
+    chopconf.toff = chop_init.toff;
+    chopconf.intpol = interpolate;
+    chopconf.hend = chop_init.hend + 3;
+    chopconf.hstrt = chop_init.hstrt - 1;
+    TERN_(SQUARE_WAVE_STEPPING, chopconf.dedge = true);
+    st.CHOPCONF(chopconf.sr);
+
+    st.rms_current(mA, hold_multiplier);
+    st.microsteps(microsteps);
+    st.iholddelay(10);
+    st.TPOWERDOWN(128); // ~2s until driver lowers to hold current
+
+    st.en_pwm_mode(stealth);
+    st.stored.stealthChop_enabled = stealth;
+
+    PWMCONF_t pwmconf{0};
+    pwmconf.pwm_freq = 0b01; // f_pwm = 2/683 f_clk
+    pwmconf.pwm_autoscale = true;
+    pwmconf.pwm_grad = 5;
+    pwmconf.pwm_ampl = 180;
+    st.PWMCONF(pwmconf.sr);
+
+    TERN(HYBRID_THRESHOLD, st.set_pwm_thrs(hyb_thrs), UNUSED(hyb_thrs));
+
+    st.GSTAT(); // Clear GSTAT
+  }
+#endif // TMC2130
+
+#if HAS_DRIVER(TMC2160)
+  template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+  void tmc_init(TMCMarlin<TMC2160Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &st, const uint16_t mA, const uint16_t microsteps, const uint32_t hyb_thrs, const bool stealth, const chopper_timing_t &chop_init, const bool interpolate, float hold_multiplier) {
+    st.begin();
+
+    CHOPCONF_t chopconf{0};
+    chopconf.tbl = 0b01;
+    chopconf.toff = chop_init.toff;
+    chopconf.intpol = interpolate;
+    chopconf.hend = chop_init.hend + 3;
+    chopconf.hstrt = chop_init.hstrt - 1;
+    TERN_(SQUARE_WAVE_STEPPING, chopconf.dedge = true);
+    st.CHOPCONF(chopconf.sr);
+
+    st.rms_current(mA, hold_multiplier);
+    st.microsteps(microsteps);
+    st.iholddelay(10);
+    st.TPOWERDOWN(128); // ~2s until driver lowers to hold current
+
+    st.en_pwm_mode(stealth);
+    st.stored.stealthChop_enabled = stealth;
+
+    TMC2160_n::PWMCONF_t pwmconf{0};
+    pwmconf.pwm_lim = 12;
+    pwmconf.pwm_reg = 8;
+    pwmconf.pwm_autograd = true;
+    pwmconf.pwm_autoscale = true;
+    pwmconf.pwm_freq = 0b01;
+    pwmconf.pwm_grad = 14;
+    pwmconf.pwm_ofs = 36;
+    st.PWMCONF(pwmconf.sr);
+
+    TERN(HYBRID_THRESHOLD, st.set_pwm_thrs(hyb_thrs), UNUSED(hyb_thrs));
+
+    st.GSTAT(); // Clear GSTAT
+  }
+#endif // TMC2160
+
+//
+// TMC2208/2209 Driver objects and inits
+//
+#if HAS_TMC220x
+  #if AXIS_HAS_UART(X)
+    #ifdef X_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, X, X);
+      #define X_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, X, X);
+      #define X_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(X2)
+    #ifdef X2_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, X2, X);
+      #define X2_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, X2, X);
+      #define X2_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(Y)
+    #ifdef Y_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, Y, Y);
+      #define Y_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, Y, Y);
+      #define Y_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(Y2)
+    #ifdef Y2_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, Y2, Y);
+      #define Y2_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, Y2, Y);
+      #define Y2_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(Z)
+    #ifdef Z_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, Z, Z);
+      #define Z_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, Z, Z);
+      #define Z_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(Z2)
+    #ifdef Z2_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, Z2, Z);
+      #define Z2_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, Z2, Z);
+      #define Z2_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(Z3)
+    #ifdef Z3_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, Z3, Z);
+      #define Z3_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, Z3, Z);
+      #define Z3_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(Z4)
+    #ifdef Z4_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, Z4, Z);
+      #define Z4_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, Z4, Z);
+      #define Z4_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(I)
+    #ifdef I_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, I, I);
+      #define I_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, I, I);
+      #define I_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(J)
+    #ifdef J_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, J, J);
+      #define J_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, J, J);
+      #define J_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(K)
+    #ifdef K_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, K, K);
+      #define K_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, K, K);
+      #define K_HAS_SW_SERIAL 1
+    #endif
+  #endif
+
+  #if AXIS_HAS_UART(E0)
+    #ifdef E0_HARDWARE_SERIAL
+      TMC_UART_DEFINE_E(HW, 0);
+      #define E0_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE_E(SW, 0);
+      #define E0_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(E1)
+    #ifdef E1_HARDWARE_SERIAL
+      TMC_UART_DEFINE_E(HW, 1);
+      #define E1_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE_E(SW, 1);
+      #define E1_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(E2)
+    #ifdef E2_HARDWARE_SERIAL
+      TMC_UART_DEFINE_E(HW, 2);
+      #define E2_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE_E(SW, 2);
+      #define E2_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(E3)
+    #ifdef E3_HARDWARE_SERIAL
+      TMC_UART_DEFINE_E(HW, 3);
+      #define E3_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE_E(SW, 3);
+      #define E3_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(E4)
+    #ifdef E4_HARDWARE_SERIAL
+      TMC_UART_DEFINE_E(HW, 4);
+      #define E4_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE_E(SW, 4);
+      #define E4_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(E5)
+    #ifdef E5_HARDWARE_SERIAL
+      TMC_UART_DEFINE_E(HW, 5);
+      #define E5_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE_E(SW, 5);
+      #define E5_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(E6)
+    #ifdef E6_HARDWARE_SERIAL
+      TMC_UART_DEFINE_E(HW, 6);
+      #define E6_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE_E(SW, 6);
+      #define E6_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(E7)
+    #ifdef E7_HARDWARE_SERIAL
+      TMC_UART_DEFINE_E(HW, 7);
+      #define E7_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE_E(SW, 7);
+      #define E7_HAS_SW_SERIAL 1
+    #endif
+  #endif
+
+  #define _EN_ITEM(N) , E##N
+  enum TMCAxis : uint8_t { MAIN_AXIS_NAMES, X2, Y2, Z2, Z3, Z4 REPEAT(EXTRUDERS, _EN_ITEM), TOTAL };
+  #undef _EN_ITEM
+
+  void tmc_serial_begin() {
+    #if HAS_TMC_HW_SERIAL
+      struct {
+        const void *ptr[TMCAxis::TOTAL];
+        bool began(const TMCAxis a, const void * const p) {
+          LOOP_L_N(i, a) if (p == ptr[i]) return true;
+          ptr[a] = p; return false;
+        };
+      } sp_helper;
+
+      #define HW_SERIAL_BEGIN(A) do{ if (!sp_helper.began(TMCAxis::A, &A##_HARDWARE_SERIAL)) \
+                                          A##_HARDWARE_SERIAL.begin(TMC_##A##_BAUD_RATE); }while(0)
+    #endif
+
+    #if AXIS_HAS_UART(X)
+      #ifdef X_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(X);
+      #else
+        stepperX.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(X2)
+      #ifdef X2_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(X2);
+      #else
+        stepperX2.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(Y)
+      #ifdef Y_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(Y);
+      #else
+        stepperY.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(Y2)
+      #ifdef Y2_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(Y2);
+      #else
+        stepperY2.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(Z)
+      #ifdef Z_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(Z);
+      #else
+        stepperZ.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(Z2)
+      #ifdef Z2_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(Z2);
+      #else
+        stepperZ2.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(Z3)
+      #ifdef Z3_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(Z3);
+      #else
+        stepperZ3.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(Z4)
+      #ifdef Z4_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(Z4);
+      #else
+        stepperZ4.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(I)
+      #ifdef I_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(I);
+      #else
+        stepperI.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(J)
+      #ifdef J_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(J);
+      #else
+        stepperJ.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(K)
+      #ifdef K_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(K);
+      #else
+        stepperK.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(E0)
+      #ifdef E0_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(E0);
+      #else
+        stepperE0.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(E1)
+      #ifdef E1_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(E1);
+      #else
+        stepperE1.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(E2)
+      #ifdef E2_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(E2);
+      #else
+        stepperE2.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(E3)
+      #ifdef E3_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(E3);
+      #else
+        stepperE3.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(E4)
+      #ifdef E4_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(E4);
+      #else
+        stepperE4.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(E5)
+      #ifdef E5_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(E5);
+      #else
+        stepperE5.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(E6)
+      #ifdef E6_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(E6);
+      #else
+        stepperE6.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(E7)
+      #ifdef E7_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(E7);
+      #else
+        stepperE7.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+  }
+#endif
+
+#if HAS_DRIVER(TMC2208)
+  template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+  void tmc_init(TMCMarlin<TMC2208Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &st, const uint16_t mA, const uint16_t microsteps, const uint32_t hyb_thrs, const bool stealth, const chopper_timing_t &chop_init, const bool interpolate, float hold_multiplier) {
+    TMC2208_n::GCONF_t gconf{0};
+    gconf.pdn_disable = true; // Use UART
+    gconf.mstep_reg_select = true; // Select microsteps with UART
+    gconf.i_scale_analog = false;
+    gconf.en_spreadcycle = !stealth;
+    st.GCONF(gconf.sr);
+    st.stored.stealthChop_enabled = stealth;
+
+    TMC2208_n::CHOPCONF_t chopconf{0};
+    chopconf.tbl = 0b01; // blank_time = 24
+    chopconf.toff = chop_init.toff;
+    chopconf.intpol = interpolate;
+    chopconf.hend = chop_init.hend + 3;
+    chopconf.hstrt = chop_init.hstrt - 1;
+    TERN_(SQUARE_WAVE_STEPPING, chopconf.dedge = true);
+    st.CHOPCONF(chopconf.sr);
+
+    st.rms_current(mA, hold_multiplier);
+    st.microsteps(microsteps);
+    st.iholddelay(10);
+    st.TPOWERDOWN(128); // ~2s until driver lowers to hold current
+
+    TMC2208_n::PWMCONF_t pwmconf{0};
+    pwmconf.pwm_lim = 12;
+    pwmconf.pwm_reg = 8;
+    pwmconf.pwm_autograd = true;
+    pwmconf.pwm_autoscale = true;
+    pwmconf.pwm_freq = 0b01;
+    pwmconf.pwm_grad = 14;
+    pwmconf.pwm_ofs = 36;
+    st.PWMCONF(pwmconf.sr);
+
+    TERN(HYBRID_THRESHOLD, st.set_pwm_thrs(hyb_thrs), UNUSED(hyb_thrs));
+
+    st.GSTAT(0b111); // Clear
+    delay(200);
+  }
+#endif // TMC2208
+
+#if HAS_DRIVER(TMC2209)
+  template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+  void tmc_init(TMCMarlin<TMC2209Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &st, const uint16_t mA, const uint16_t microsteps, const uint32_t hyb_thrs, const bool stealth, const chopper_timing_t &chop_init, const bool interpolate, float hold_multiplier) {
+    TMC2208_n::GCONF_t gconf{0};
+    gconf.pdn_disable = true; // Use UART
+    gconf.mstep_reg_select = true; // Select microsteps with UART
+    gconf.i_scale_analog = false;
+    gconf.en_spreadcycle = !stealth;
+    st.GCONF(gconf.sr);
+    st.stored.stealthChop_enabled = stealth;
+
+    TMC2208_n::CHOPCONF_t chopconf{0};
+    chopconf.tbl = 0b01; // blank_time = 24
+    chopconf.toff = chop_init.toff;
+    chopconf.intpol = interpolate;
+    chopconf.hend = chop_init.hend + 3;
+    chopconf.hstrt = chop_init.hstrt - 1;
+    TERN_(SQUARE_WAVE_STEPPING, chopconf.dedge = true);
+    st.CHOPCONF(chopconf.sr);
+
+    st.rms_current(mA, hold_multiplier);
+    st.microsteps(microsteps);
+    st.iholddelay(10);
+    st.TPOWERDOWN(128); // ~2s until driver lowers to hold current
+
+    TMC2208_n::PWMCONF_t pwmconf{0};
+    pwmconf.pwm_lim = 12;
+    pwmconf.pwm_reg = 8;
+    pwmconf.pwm_autograd = true;
+    pwmconf.pwm_autoscale = true;
+    pwmconf.pwm_freq = 0b01;
+    pwmconf.pwm_grad = 14;
+    pwmconf.pwm_ofs = 36;
+    st.PWMCONF(pwmconf.sr);
+
+    TERN(HYBRID_THRESHOLD, st.set_pwm_thrs(hyb_thrs), UNUSED(hyb_thrs));
+
+    st.GSTAT(0b111); // Clear
+    delay(200);
+  }
+#endif // TMC2209
+
+#if HAS_DRIVER(TMC2660)
+  template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+  void tmc_init(TMCMarlin<TMC2660Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &st, const uint16_t mA, const uint16_t microsteps, const uint32_t, const bool, const chopper_timing_t &chop_init, const bool interpolate, float hold_multiplier) {
+    st.begin();
+
+    TMC2660_n::CHOPCONF_t chopconf{0};
+    chopconf.tbl = 0b01;
+    chopconf.toff = chop_init.toff;
+    chopconf.hend = chop_init.hend + 3;
+    chopconf.hstrt = chop_init.hstrt - 1;
+    st.CHOPCONF(chopconf.sr);
+
+    st.sdoff(0);
+    st.rms_current(mA);
+    st.microsteps(microsteps);
+    TERN_(SQUARE_WAVE_STEPPING, st.dedge(true));
+    st.intpol(interpolate);
+    st.diss2g(true); // Disable short to ground protection. Too many false readings?
+    TERN_(TMC_DEBUG, st.rdsel(0b01));
+  }
+#endif // TMC2660
+
+#if HAS_DRIVER(TMC5130)
+  template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+  void tmc_init(TMCMarlin<TMC5130Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &st, const uint16_t mA, const uint16_t microsteps, const uint32_t hyb_thrs, const bool stealth, const chopper_timing_t &chop_init, const bool interpolate, float hold_multiplier) {
+    st.begin();
+
+    CHOPCONF_t chopconf{0};
+    chopconf.tbl = 0b01;
+    chopconf.toff = chop_init.toff;
+    chopconf.intpol = interpolate;
+    chopconf.hend = chop_init.hend + 3;
+    chopconf.hstrt = chop_init.hstrt - 1;
+    TERN_(SQUARE_WAVE_STEPPING, chopconf.dedge = true);
+    st.CHOPCONF(chopconf.sr);
+
+    st.rms_current(mA, hold_multiplier);
+    st.microsteps(microsteps);
+    st.iholddelay(10);
+    st.TPOWERDOWN(128); // ~2s until driver lowers to hold current
+
+    st.en_pwm_mode(stealth);
+    st.stored.stealthChop_enabled = stealth;
+
+    PWMCONF_t pwmconf{0};
+    pwmconf.pwm_freq = 0b01; // f_pwm = 2/683 f_clk
+    pwmconf.pwm_autoscale = true;
+    pwmconf.pwm_grad = 5;
+    pwmconf.pwm_ampl = 180;
+    st.PWMCONF(pwmconf.sr);
+
+    TERN(HYBRID_THRESHOLD, st.set_pwm_thrs(hyb_thrs), UNUSED(hyb_thrs));
+
+    st.GSTAT(); // Clear GSTAT
+  }
+#endif // TMC5130
+
+#if HAS_DRIVER(TMC5160)
+  template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+  void tmc_init(TMCMarlin<TMC5160Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &st, const uint16_t mA, const uint16_t microsteps, const uint32_t hyb_thrs, const bool stealth, const chopper_timing_t &chop_init, const bool interpolate, float hold_multiplier) {
+    st.begin();
+
+    CHOPCONF_t chopconf{0};
+    chopconf.tbl = 0b01;
+    chopconf.toff = chop_init.toff;
+    chopconf.intpol = interpolate;
+    chopconf.hend = chop_init.hend + 3;
+    chopconf.hstrt = chop_init.hstrt - 1;
+    TERN_(SQUARE_WAVE_STEPPING, chopconf.dedge = true);
+    st.CHOPCONF(chopconf.sr);
+
+    st.rms_current(mA, hold_multiplier);
+    st.microsteps(microsteps);
+    st.iholddelay(10);
+    st.TPOWERDOWN(128); // ~2s until driver lowers to hold current
+
+    st.en_pwm_mode(stealth);
+    st.stored.stealthChop_enabled = stealth;
+
+    TMC2160_n::PWMCONF_t pwmconf{0};
+    pwmconf.pwm_lim = 12;
+    pwmconf.pwm_reg = 8;
+    pwmconf.pwm_autograd = true;
+    pwmconf.pwm_autoscale = true;
+    pwmconf.pwm_freq = 0b01;
+    pwmconf.pwm_grad = 14;
+    pwmconf.pwm_ofs = 36;
+    st.PWMCONF(pwmconf.sr);
+
+    TERN(HYBRID_THRESHOLD, st.set_pwm_thrs(hyb_thrs), UNUSED(hyb_thrs));
+    st.GSTAT(); // Clear GSTAT
+  }
+#endif // TMC5160
+
+void restore_trinamic_drivers() {
+  #if AXIS_IS_TMC(X)
+    stepperX.push();
+  #endif
+  #if AXIS_IS_TMC(X2)
+    stepperX2.push();
+  #endif
+  #if AXIS_IS_TMC(Y)
+    stepperY.push();
+  #endif
+  #if AXIS_IS_TMC(Y2)
+    stepperY2.push();
+  #endif
+  #if AXIS_IS_TMC(Z)
+    stepperZ.push();
+  #endif
+  #if AXIS_IS_TMC(Z2)
+    stepperZ2.push();
+  #endif
+  #if AXIS_IS_TMC(Z3)
+    stepperZ3.push();
+  #endif
+  #if AXIS_IS_TMC(Z4)
+    stepperZ4.push();
+  #endif
+  #if AXIS_IS_TMC(I)
+    stepperI.push();
+  #endif
+  #if AXIS_IS_TMC(J)
+    stepperJ.push();
+  #endif
+  #if AXIS_IS_TMC(K)
+    stepperK.push();
+  #endif
+  #if AXIS_IS_TMC(E0)
+    stepperE0.push();
+  #endif
+  #if AXIS_IS_TMC(E1)
+    stepperE1.push();
+  #endif
+  #if AXIS_IS_TMC(E2)
+    stepperE2.push();
+  #endif
+  #if AXIS_IS_TMC(E3)
+    stepperE3.push();
+  #endif
+  #if AXIS_IS_TMC(E4)
+    stepperE4.push();
+  #endif
+  #if AXIS_IS_TMC(E5)
+    stepperE5.push();
+  #endif
+  #if AXIS_IS_TMC(E6)
+    stepperE6.push();
+  #endif
+  #if AXIS_IS_TMC(E7)
+    stepperE7.push();
+  #endif
+}
+
+void reset_trinamic_drivers() {
+  static constexpr bool stealthchop_by_axis[] = LOGICAL_AXIS_ARRAY(
+    ENABLED(STEALTHCHOP_E),
+    ENABLED(STEALTHCHOP_XY), ENABLED(STEALTHCHOP_XY), ENABLED(STEALTHCHOP_Z),
+    ENABLED(STEALTHCHOP_I), ENABLED(STEALTHCHOP_J), ENABLED(STEALTHCHOP_K)
+  );
+
+  #if AXIS_IS_TMC(X)
+    TMC_INIT(X, STEALTH_AXIS_X);
+  #endif
+  #if AXIS_IS_TMC(X2)
+    TMC_INIT(X2, STEALTH_AXIS_X);
+  #endif
+  #if AXIS_IS_TMC(Y)
+    TMC_INIT(Y, STEALTH_AXIS_Y);
+  #endif
+  #if AXIS_IS_TMC(Y2)
+    TMC_INIT(Y2, STEALTH_AXIS_Y);
+  #endif
+  #if AXIS_IS_TMC(Z)
+    TMC_INIT(Z, STEALTH_AXIS_Z);
+  #endif
+  #if AXIS_IS_TMC(Z2)
+    TMC_INIT(Z2, STEALTH_AXIS_Z);
+  #endif
+  #if AXIS_IS_TMC(Z3)
+    TMC_INIT(Z3, STEALTH_AXIS_Z);
+  #endif
+  #if AXIS_IS_TMC(Z4)
+    TMC_INIT(Z4, STEALTH_AXIS_Z);
+  #endif
+  #if AXIS_IS_TMC(I)
+    TMC_INIT(I, STEALTH_AXIS_I);
+  #endif
+  #if AXIS_IS_TMC(J)
+    TMC_INIT(J, STEALTH_AXIS_J);
+  #endif
+  #if AXIS_IS_TMC(K)
+    TMC_INIT(K, STEALTH_AXIS_K);
+  #endif
+  #if AXIS_IS_TMC(E0)
+    TMC_INIT(E0, STEALTH_AXIS_E);
+  #endif
+  #if AXIS_IS_TMC(E1)
+    TMC_INIT(E1, STEALTH_AXIS_E);
+  #endif
+  #if AXIS_IS_TMC(E2)
+    TMC_INIT(E2, STEALTH_AXIS_E);
+  #endif
+  #if AXIS_IS_TMC(E3)
+    TMC_INIT(E3, STEALTH_AXIS_E);
+  #endif
+  #if AXIS_IS_TMC(E4)
+    TMC_INIT(E4, STEALTH_AXIS_E);
+  #endif
+  #if AXIS_IS_TMC(E5)
+    TMC_INIT(E5, STEALTH_AXIS_E);
+  #endif
+  #if AXIS_IS_TMC(E6)
+    TMC_INIT(E6, STEALTH_AXIS_E);
+  #endif
+  #if AXIS_IS_TMC(E7)
+    TMC_INIT(E7, STEALTH_AXIS_E);
+  #endif
+
+  #if USE_SENSORLESS
+    TERN_(X_SENSORLESS, stepperX.homing_threshold(X_STALL_SENSITIVITY));
+    TERN_(X2_SENSORLESS, stepperX2.homing_threshold(CAT(TERN(X2_SENSORLESS, X2, X), _STALL_SENSITIVITY)));
+    TERN_(Y_SENSORLESS, stepperY.homing_threshold(Y_STALL_SENSITIVITY));
+    TERN_(Y2_SENSORLESS, stepperY2.homing_threshold(CAT(TERN(Y2_SENSORLESS, Y2, Y), _STALL_SENSITIVITY)));
+    TERN_(Z_SENSORLESS, stepperZ.homing_threshold(Z_STALL_SENSITIVITY));
+    TERN_(Z2_SENSORLESS, stepperZ2.homing_threshold(CAT(TERN(Z2_SENSORLESS, Z2, Z), _STALL_SENSITIVITY)));
+    TERN_(Z3_SENSORLESS, stepperZ3.homing_threshold(CAT(TERN(Z3_SENSORLESS, Z3, Z), _STALL_SENSITIVITY)));
+    TERN_(Z4_SENSORLESS, stepperZ4.homing_threshold(CAT(TERN(Z4_SENSORLESS, Z4, Z), _STALL_SENSITIVITY)));
+    TERN_(I_SENSORLESS, stepperI.homing_threshold(I_STALL_SENSITIVITY));
+    TERN_(J_SENSORLESS, stepperJ.homing_threshold(J_STALL_SENSITIVITY));
+    TERN_(K_SENSORLESS, stepperK.homing_threshold(K_STALL_SENSITIVITY));
+  #endif
+
+  #ifdef TMC_ADV
+    TMC_ADV()
+  #endif
+
+  stepper.set_directions();
+}
+
+// TMC Slave Address Conflict Detection
+//
+// Conflict detection is performed in the following way. Similar methods are used for
+// hardware and software serial, but the implementations are independent.
+//
+// 1. Populate a data structure with UART parameters and addresses for all possible axis.
+//      If an axis is not in use, populate it with recognizable placeholder data.
+// 2. For each axis in use, static_assert using a constexpr function, which counts the
+//      number of matching/conflicting axis. If the value is not exactly 1, fail.
+
+#define ALL_AXIS_NAMES X, X2, Y, Y2, Z, Z2, Z3, Z4, I, J, K, E0, E1, E2, E3, E4, E5, E6, E7
+
+#if ANY_AXIS_HAS(HW_SERIAL)
+  // Hardware serial names are compared as strings, since actually resolving them cannot occur in a constexpr.
+  // Using a fixed-length character array for the port name allows this to be constexpr compatible.
+  struct SanityHwSerialDetails { const char port[20]; uint32_t address; };
+  #define TMC_HW_DETAIL_ARGS(A) TERN(A##_HAS_HW_SERIAL, STRINGIFY(A##_HARDWARE_SERIAL), ""), TERN0(A##_HAS_HW_SERIAL, A##_SLAVE_ADDRESS)
+  #define TMC_HW_DETAIL(A) { TMC_HW_DETAIL_ARGS(A) }
+  constexpr SanityHwSerialDetails sanity_tmc_hw_details[] = { MAPLIST(TMC_HW_DETAIL, ALL_AXIS_NAMES) };
+
+  // constexpr compatible string comparison
+  constexpr bool str_eq_ce(const char * a, const char * b) {
+    return *a == *b && (*a == '\0' || str_eq_ce(a+1,b+1));
+  }
+
+  constexpr bool sc_hw_done(size_t start, size_t end) { return start == end; }
+  constexpr bool sc_hw_skip(const char *port_name) { return !(*port_name); }
+  constexpr bool sc_hw_match(const char *port_name, uint32_t address, size_t start, size_t end) {
+    return !sc_hw_done(start, end) && !sc_hw_skip(port_name) && (address == sanity_tmc_hw_details[start].address && str_eq_ce(port_name, sanity_tmc_hw_details[start].port));
+  }
+  constexpr int count_tmc_hw_serial_matches(const char *port_name, uint32_t address, size_t start, size_t end) {
+    return sc_hw_done(start, end) ? 0 : ((sc_hw_skip(port_name) ? 0 : (sc_hw_match(port_name, address, start, end) ? 1 : 0)) + count_tmc_hw_serial_matches(port_name, address, start + 1, end));
+  }
+
+  #define TMC_HWSERIAL_CONFLICT_MSG(A) STRINGIFY(A) "_SLAVE_ADDRESS conflicts with another driver using the same " STRINGIFY(A) "_HARDWARE_SERIAL"
+  #define SA_NO_TMC_HW_C(A) static_assert(1 >= count_tmc_hw_serial_matches(TMC_HW_DETAIL_ARGS(A), 0, COUNT(sanity_tmc_hw_details)), TMC_HWSERIAL_CONFLICT_MSG(A));
+  MAP(SA_NO_TMC_HW_C, ALL_AXIS_NAMES)
+#endif
+
+#if ANY_AXIS_HAS(SW_SERIAL)
+  struct SanitySwSerialDetails { int32_t txpin; int32_t rxpin; uint32_t address; };
+  #define TMC_SW_DETAIL_ARGS(A) TERN(A##_HAS_SW_SERIAL, A##_SERIAL_TX_PIN, -1), TERN(A##_HAS_SW_SERIAL, A##_SERIAL_RX_PIN, -1), TERN0(A##_HAS_SW_SERIAL, A##_SLAVE_ADDRESS)
+  #define TMC_SW_DETAIL(A) { TMC_SW_DETAIL_ARGS(A) }
+  constexpr SanitySwSerialDetails sanity_tmc_sw_details[] = { MAPLIST(TMC_SW_DETAIL, ALL_AXIS_NAMES) };
+
+  constexpr bool sc_sw_done(size_t start, size_t end) { return start == end; }
+  constexpr bool sc_sw_skip(int32_t txpin) { return txpin < 0; }
+  constexpr bool sc_sw_match(int32_t txpin, int32_t rxpin, uint32_t address, size_t start, size_t end) {
+    return !sc_sw_done(start, end) && !sc_sw_skip(txpin) && (txpin == sanity_tmc_sw_details[start].txpin || rxpin == sanity_tmc_sw_details[start].rxpin) && (address == sanity_tmc_sw_details[start].address);
+  }
+  constexpr int count_tmc_sw_serial_matches(int32_t txpin, int32_t rxpin, uint32_t address, size_t start, size_t end) {
+    return sc_sw_done(start, end) ? 0 : ((sc_sw_skip(txpin) ? 0 : (sc_sw_match(txpin, rxpin, address, start, end) ? 1 : 0)) + count_tmc_sw_serial_matches(txpin, rxpin, address, start + 1, end));
+  }
+
+  #define TMC_SWSERIAL_CONFLICT_MSG(A) STRINGIFY(A) "_SLAVE_ADDRESS conflicts with another driver using the same " STRINGIFY(A) "_SERIAL_RX_PIN or " STRINGIFY(A) "_SERIAL_TX_PIN"
+  #define SA_NO_TMC_SW_C(A) static_assert(1 >= count_tmc_sw_serial_matches(TMC_SW_DETAIL_ARGS(A), 0, COUNT(sanity_tmc_sw_details)), TMC_SWSERIAL_CONFLICT_MSG(A));
+  MAP(SA_NO_TMC_SW_C, ALL_AXIS_NAMES)
+#endif
+
+#endif // HAS_TRINAMIC_CONFIG

src/module/stepper/trinamic.h

@@ -0,0 +1,411 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * stepper/trinamic.h
+ * Stepper driver indirection for Trinamic
+ */
+
+#include <TMCStepper.h>
+#if TMCSTEPPER_VERSION < 0x000500
+  #error "Update TMCStepper library to 0.5.0 or newer."
+#endif
+
+#include "../../inc/MarlinConfig.h"
+#include "../../feature/tmc_util.h"
+
+#define CLASS_TMC2130 TMC2130Stepper
+#define CLASS_TMC2160 TMC2160Stepper
+#define CLASS_TMC2208 TMC2208Stepper
+#define CLASS_TMC2209 TMC2209Stepper
+#define CLASS_TMC2660 TMC2660Stepper
+#define CLASS_TMC5130 TMC5130Stepper
+#define CLASS_TMC5160 TMC5160Stepper
+
+#define TMC_X_LABEL 'X', '0'
+#define TMC_Y_LABEL 'Y', '0'
+#define TMC_Z_LABEL 'Z', '0'
+
+#define TMC_I_LABEL 'I', '0'
+#define TMC_J_LABEL 'J', '0'
+#define TMC_K_LABEL 'K', '0'
+
+#define TMC_X2_LABEL 'X', '2'
+#define TMC_Y2_LABEL 'Y', '2'
+#define TMC_Z2_LABEL 'Z', '2'
+#define TMC_Z3_LABEL 'Z', '3'
+#define TMC_Z4_LABEL 'Z', '4'
+
+#define TMC_E0_LABEL 'E', '0'
+#define TMC_E1_LABEL 'E', '1'
+#define TMC_E2_LABEL 'E', '2'
+#define TMC_E3_LABEL 'E', '3'
+#define TMC_E4_LABEL 'E', '4'
+#define TMC_E5_LABEL 'E', '5'
+#define TMC_E6_LABEL 'E', '6'
+#define TMC_E7_LABEL 'E', '7'
+
+#define __TMC_CLASS(TYPE, L, I, A) TMCMarlin<CLASS_##TYPE, L, I, A>
+#define _TMC_CLASS(TYPE, LandI, A) __TMC_CLASS(TYPE, LandI, A)
+#define TMC_CLASS(ST, A) _TMC_CLASS(ST##_DRIVER_TYPE, TMC_##ST##_LABEL, A##_AXIS)
+#if ENABLED(DISTINCT_E_FACTORS)
+  #define TMC_CLASS_E(N) TMC_CLASS(E##N, E##N)
+#else
+  #define TMC_CLASS_E(N) TMC_CLASS(E##N, E)
+#endif
+
+#ifndef CHOPPER_TIMING_X
+  #define CHOPPER_TIMING_X CHOPPER_TIMING
+#endif
+#if HAS_Y_AXIS && !defined(CHOPPER_TIMING_Y)
+  #define CHOPPER_TIMING_Y CHOPPER_TIMING
+#endif
+#if HAS_Z_AXIS && !defined(CHOPPER_TIMING_Z)
+  #define CHOPPER_TIMING_Z CHOPPER_TIMING
+#endif
+#if HAS_I_AXIS && !defined(CHOPPER_TIMING_I)
+  #define CHOPPER_TIMING_I CHOPPER_TIMING
+#endif
+#if HAS_J_AXIS && !defined(CHOPPER_TIMING_J)
+  #define CHOPPER_TIMING_J CHOPPER_TIMING
+#endif
+#if HAS_K_AXIS && !defined(CHOPPER_TIMING_K)
+  #define CHOPPER_TIMING_K CHOPPER_TIMING
+#endif
+#if HAS_EXTRUDERS && !defined(CHOPPER_TIMING_E)
+  #define CHOPPER_TIMING_E CHOPPER_TIMING
+#endif
+
+#if HAS_TMC220x
+  void tmc_serial_begin();
+#endif
+
+void restore_trinamic_drivers();
+void reset_trinamic_drivers();
+
+#define AXIS_HAS_SQUARE_WAVE(A) (AXIS_IS_TMC(A) && ENABLED(SQUARE_WAVE_STEPPING))
+
+// X Stepper
+#if AXIS_IS_TMC(X)
+  extern TMC_CLASS(X, X) stepperX;
+  static constexpr chopper_timing_t chopper_timing_X = CHOPPER_TIMING_X;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define X_ENABLE_INIT() NOOP
+    #define X_ENABLE_WRITE(STATE) stepperX.toff((STATE)==X_ENABLE_ON ? chopper_timing_X.toff : 0)
+    #define X_ENABLE_READ() stepperX.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(X)
+    #define X_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(X_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// Y Stepper
+#if AXIS_IS_TMC(Y)
+  extern TMC_CLASS(Y, Y) stepperY;
+  static constexpr chopper_timing_t chopper_timing_Y = CHOPPER_TIMING_Y;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define Y_ENABLE_INIT() NOOP
+    #define Y_ENABLE_WRITE(STATE) stepperY.toff((STATE)==Y_ENABLE_ON ? chopper_timing_Y.toff : 0)
+    #define Y_ENABLE_READ() stepperY.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(Y)
+    #define Y_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(Y_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// Z Stepper
+#if AXIS_IS_TMC(Z)
+  extern TMC_CLASS(Z, Z) stepperZ;
+  static constexpr chopper_timing_t chopper_timing_Z = CHOPPER_TIMING_Z;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define Z_ENABLE_INIT() NOOP
+    #define Z_ENABLE_WRITE(STATE) stepperZ.toff((STATE)==Z_ENABLE_ON ? chopper_timing_Z.toff : 0)
+    #define Z_ENABLE_READ() stepperZ.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(Z)
+    #define Z_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(Z_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// X2 Stepper
+#if HAS_X2_ENABLE && AXIS_IS_TMC(X2)
+  extern TMC_CLASS(X2, X) stepperX2;
+  #ifndef CHOPPER_TIMING_X2
+    #define CHOPPER_TIMING_X2 CHOPPER_TIMING_X
+  #endif
+  static constexpr chopper_timing_t chopper_timing_X2 = CHOPPER_TIMING_X2;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define X2_ENABLE_INIT() NOOP
+    #define X2_ENABLE_WRITE(STATE) stepperX2.toff((STATE)==X_ENABLE_ON ? chopper_timing_X2.toff : 0)
+    #define X2_ENABLE_READ() stepperX2.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(X2)
+    #define X2_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(X2_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// Y2 Stepper
+#if HAS_Y2_ENABLE && AXIS_IS_TMC(Y2)
+  extern TMC_CLASS(Y2, Y) stepperY2;
+  #ifndef CHOPPER_TIMING_Y2
+    #define CHOPPER_TIMING_Y2 CHOPPER_TIMING_Y
+  #endif
+  static constexpr chopper_timing_t chopper_timing_Y2 = CHOPPER_TIMING_Y2;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define Y2_ENABLE_INIT() NOOP
+    #define Y2_ENABLE_WRITE(STATE) stepperY2.toff((STATE)==Y_ENABLE_ON ? chopper_timing_Y2.toff : 0)
+    #define Y2_ENABLE_READ() stepperY2.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(Y2)
+    #define Y2_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(Y2_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// Z2 Stepper
+#if HAS_Z2_ENABLE && AXIS_IS_TMC(Z2)
+  extern TMC_CLASS(Z2, Z) stepperZ2;
+  #ifndef CHOPPER_TIMING_Z2
+    #define CHOPPER_TIMING_Z2 CHOPPER_TIMING_Z
+  #endif
+  static constexpr chopper_timing_t chopper_timing_Z2 = CHOPPER_TIMING_Z2;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define Z2_ENABLE_INIT() NOOP
+    #define Z2_ENABLE_WRITE(STATE) stepperZ2.toff((STATE)==Z_ENABLE_ON ? chopper_timing_Z2.toff : 0)
+    #define Z2_ENABLE_READ() stepperZ2.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(Z2)
+    #define Z2_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(Z2_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// Z3 Stepper
+#if HAS_Z3_ENABLE && AXIS_IS_TMC(Z3)
+  extern TMC_CLASS(Z3, Z) stepperZ3;
+  #ifndef CHOPPER_TIMING_Z3
+    #define CHOPPER_TIMING_Z3 CHOPPER_TIMING_Z
+  #endif
+  static constexpr chopper_timing_t chopper_timing_Z3 = CHOPPER_TIMING_Z3;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define Z3_ENABLE_INIT() NOOP
+    #define Z3_ENABLE_WRITE(STATE) stepperZ3.toff((STATE)==Z_ENABLE_ON ? chopper_timing_Z3.toff : 0)
+    #define Z3_ENABLE_READ() stepperZ3.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(Z3)
+    #define Z3_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(Z3_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// Z4 Stepper
+#if HAS_Z4_ENABLE && AXIS_IS_TMC(Z4)
+  extern TMC_CLASS(Z4, Z) stepperZ4;
+  #ifndef CHOPPER_TIMING_Z4
+    #define CHOPPER_TIMING_Z4 CHOPPER_TIMING_Z
+  #endif
+  static constexpr chopper_timing_t chopper_timing_Z4 = CHOPPER_TIMING_Z4;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define Z4_ENABLE_INIT() NOOP
+    #define Z4_ENABLE_WRITE(STATE) stepperZ4.toff((STATE)==Z_ENABLE_ON ? chopper_timing_Z4.toff : 0)
+    #define Z4_ENABLE_READ() stepperZ4.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(Z4)
+    #define Z4_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(Z4_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// I Stepper
+#if AXIS_IS_TMC(I)
+  extern TMC_CLASS(I, I) stepperI;
+  static constexpr chopper_timing_t chopper_timing_I = CHOPPER_TIMING_I;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define I_ENABLE_INIT() NOOP
+    #define I_ENABLE_WRITE(STATE) stepperI.toff((STATE)==I_ENABLE_ON ? chopper_timing.toff : 0)
+    #define I_ENABLE_READ() stepperI.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(I)
+    #define I_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(I_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// J Stepper
+#if AXIS_IS_TMC(J)
+  extern TMC_CLASS(J, J) stepperJ;
+  static constexpr chopper_timing_t chopper_timing_J = CHOPPER_TIMING_J;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define J_ENABLE_INIT() NOOP
+    #define J_ENABLE_WRITE(STATE) stepperJ.toff((STATE)==J_ENABLE_ON ? chopper_timing.toff : 0)
+    #define J_ENABLE_READ() stepperJ.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(J)
+    #define J_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(J_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// K Stepper
+#if AXIS_IS_TMC(K)
+  extern TMC_CLASS(K, K) stepperK;
+  static constexpr chopper_timing_t chopper_timing_K = CHOPPER_TIMING_K;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define K_ENABLE_INIT() NOOP
+    #define K_ENABLE_WRITE(STATE) stepperK.toff((STATE)==K_ENABLE_ON ? chopper_timing.toff : 0)
+    #define K_ENABLE_READ() stepperK.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(K)
+    #define K_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(K_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// E0 Stepper
+#if AXIS_IS_TMC(E0)
+  extern TMC_CLASS_E(0) stepperE0;
+  #ifndef CHOPPER_TIMING_E0
+    #define CHOPPER_TIMING_E0 CHOPPER_TIMING_E
+  #endif
+  static constexpr chopper_timing_t chopper_timing_E0 = CHOPPER_TIMING_E0;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define E0_ENABLE_INIT() NOOP
+    #define E0_ENABLE_WRITE(STATE) stepperE0.toff((STATE)==E_ENABLE_ON ? chopper_timing_E0.toff : 0)
+    #define E0_ENABLE_READ() stepperE0.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(E0)
+    #define E0_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(E0_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// E1 Stepper
+#if AXIS_IS_TMC(E1)
+  extern TMC_CLASS_E(1) stepperE1;
+  #ifndef CHOPPER_TIMING_E1
+    #define CHOPPER_TIMING_E1 CHOPPER_TIMING_E
+  #endif
+  static constexpr chopper_timing_t chopper_timing_E1 = CHOPPER_TIMING_E1;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define E1_ENABLE_INIT() NOOP
+    #define E1_ENABLE_WRITE(STATE) stepperE1.toff((STATE)==E_ENABLE_ON ? chopper_timing_E1.toff : 0)
+    #define E1_ENABLE_READ() stepperE1.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(E1)
+    #define E1_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(E1_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// E2 Stepper
+#if AXIS_IS_TMC(E2)
+  extern TMC_CLASS_E(2) stepperE2;
+  #ifndef CHOPPER_TIMING_E2
+    #define CHOPPER_TIMING_E2 CHOPPER_TIMING_E
+  #endif
+  static constexpr chopper_timing_t chopper_timing_E2 = CHOPPER_TIMING_E2;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define E2_ENABLE_INIT() NOOP
+    #define E2_ENABLE_WRITE(STATE) stepperE2.toff((STATE)==E_ENABLE_ON ? chopper_timing_E2.toff : 0)
+    #define E2_ENABLE_READ() stepperE2.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(E2)
+    #define E2_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(E2_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// E3 Stepper
+#if AXIS_IS_TMC(E3)
+  extern TMC_CLASS_E(3) stepperE3;
+  #ifndef CHOPPER_TIMING_E3
+    #define CHOPPER_TIMING_E3 CHOPPER_TIMING_E
+  #endif
+  static constexpr chopper_timing_t chopper_timing_E3 = CHOPPER_TIMING_E3;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define E3_ENABLE_INIT() NOOP
+    #define E3_ENABLE_WRITE(STATE) stepperE3.toff((STATE)==E_ENABLE_ON ? chopper_timing_E3.toff : 0)
+    #define E3_ENABLE_READ() stepperE3.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(E3)
+    #define E3_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(E3_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// E4 Stepper
+#if AXIS_IS_TMC(E4)
+  extern TMC_CLASS_E(4) stepperE4;
+  #ifndef CHOPPER_TIMING_E4
+    #define CHOPPER_TIMING_E4 CHOPPER_TIMING_E
+  #endif
+  static constexpr chopper_timing_t chopper_timing_E4 = CHOPPER_TIMING_E4;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define E4_ENABLE_INIT() NOOP
+    #define E4_ENABLE_WRITE(STATE) stepperE4.toff((STATE)==E_ENABLE_ON ? chopper_timing_E4.toff : 0)
+    #define E4_ENABLE_READ() stepperE4.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(E4)
+    #define E4_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(E4_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// E5 Stepper
+#if AXIS_IS_TMC(E5)
+  extern TMC_CLASS_E(5) stepperE5;
+  #ifndef CHOPPER_TIMING_E5
+    #define CHOPPER_TIMING_E5 CHOPPER_TIMING_E
+  #endif
+  static constexpr chopper_timing_t chopper_timing_E5 = CHOPPER_TIMING_E5;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define E5_ENABLE_INIT() NOOP
+    #define E5_ENABLE_WRITE(STATE) stepperE5.toff((STATE)==E_ENABLE_ON ? chopper_timing_E5.toff : 0)
+    #define E5_ENABLE_READ() stepperE5.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(E5)
+    #define E5_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(E5_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// E6 Stepper
+#if AXIS_IS_TMC(E6)
+  extern TMC_CLASS_E(6) stepperE6;
+  #ifndef CHOPPER_TIMING_E6
+    #define CHOPPER_TIMING_E6 CHOPPER_TIMING_E
+  #endif
+  static constexpr chopper_timing_t chopper_timing_E6 = CHOPPER_TIMING_E6;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define E6_ENABLE_INIT() NOOP
+    #define E6_ENABLE_WRITE(STATE) stepperE6.toff((STATE)==E_ENABLE_ON ? chopper_timing_E6.toff : 0)
+    #define E6_ENABLE_READ() stepperE6.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(E6)
+    #define E6_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(E6_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// E7 Stepper
+#if AXIS_IS_TMC(E7)
+  extern TMC_CLASS_E(7) stepperE7;
+  #ifndef CHOPPER_TIMING_E7
+    #define CHOPPER_TIMING_E7 CHOPPER_TIMING_E
+  #endif
+  static constexpr chopper_timing_t chopper_timing_E7 = CHOPPER_TIMING_E7;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define E7_ENABLE_INIT() NOOP
+    #define E7_ENABLE_WRITE(STATE) stepperE7.toff((STATE)==E_ENABLE_ON ? chopper_timing_E7.toff : 0)
+    #define E7_ENABLE_READ() stepperE7.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(E7)
+    #define E7_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(E7_STEP_PIN); }while(0)
+  #endif
+#endif

src/module/thermistor/thermistor_1.h

@@ -0,0 +1,90 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+// R25 = 100 kOhm, beta25 = 4092 K, 4.7 kOhm pull-up, bed thermistor
+constexpr temp_entry_t temptable_1[] PROGMEM = {
+  { OV(  23), 300 },
+  { OV(  25), 295 },
+  { OV(  27), 290 },
+  { OV(  28), 285 },
+  { OV(  31), 280 },
+  { OV(  33), 275 },
+  { OV(  35), 270 },
+  { OV(  38), 265 },
+  { OV(  41), 260 },
+  { OV(  44), 255 },
+  { OV(  48), 250 },
+  { OV(  52), 245 },
+  { OV(  56), 240 },
+  { OV(  61), 235 },
+  { OV(  66), 230 },
+  { OV(  71), 225 },
+  { OV(  78), 220 },
+  { OV(  84), 215 },
+  { OV(  92), 210 },
+  { OV( 100), 205 },
+  { OV( 109), 200 },
+  { OV( 120), 195 },
+  { OV( 131), 190 },
+  { OV( 143), 185 },
+  { OV( 156), 180 },
+  { OV( 171), 175 },
+  { OV( 187), 170 },
+  { OV( 205), 165 },
+  { OV( 224), 160 },
+  { OV( 245), 155 },
+  { OV( 268), 150 },
+  { OV( 293), 145 },
+  { OV( 320), 140 },
+  { OV( 348), 135 },
+  { OV( 379), 130 },
+  { OV( 411), 125 },
+  { OV( 445), 120 },
+  { OV( 480), 115 },
+  { OV( 516), 110 },
+  { OV( 553), 105 },
+  { OV( 591), 100 },
+  { OV( 628),  95 },
+  { OV( 665),  90 },
+  { OV( 702),  85 },
+  { OV( 737),  80 },
+  { OV( 770),  75 },
+  { OV( 801),  70 },
+  { OV( 830),  65 },
+  { OV( 857),  60 },
+  { OV( 881),  55 },
+  { OV( 903),  50 },
+  { OV( 922),  45 },
+  { OV( 939),  40 },
+  { OV( 954),  35 },
+  { OV( 966),  30 },
+  { OV( 977),  25 },
+  { OV( 985),  20 },
+  { OV( 993),  15 },
+  { OV( 999),  10 },
+  { OV(1004),   5 },
+  { OV(1008),   0 },
+  { OV(1012),  -5 },
+  { OV(1016), -10 },
+  { OV(1020), -15 }
+};

src/module/thermistor/thermistor_10.h

@@ -0,0 +1,57 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+// R25 = 100 kOhm, beta25 = 3960 K, 4.7 kOhm pull-up, RS thermistor 198-961
+constexpr temp_entry_t temptable_10[] PROGMEM = {
+  { OV(   1), 929 },
+  { OV(  36), 299 },
+  { OV(  71), 246 },
+  { OV( 106), 217 },
+  { OV( 141), 198 },
+  { OV( 176), 184 },
+  { OV( 211), 173 },
+  { OV( 246), 163 },
+  { OV( 281), 154 },
+  { OV( 316), 147 },
+  { OV( 351), 140 },
+  { OV( 386), 134 },
+  { OV( 421), 128 },
+  { OV( 456), 122 },
+  { OV( 491), 117 },
+  { OV( 526), 112 },
+  { OV( 561), 107 },
+  { OV( 596), 102 },
+  { OV( 631),  97 },
+  { OV( 666),  91 },
+  { OV( 701),  86 },
+  { OV( 736),  81 },
+  { OV( 771),  76 },
+  { OV( 806),  70 },
+  { OV( 841),  63 },
+  { OV( 876),  56 },
+  { OV( 911),  48 },
+  { OV( 946),  38 },
+  { OV( 981),  23 },
+  { OV(1005),   5 },
+  { OV(1016),   0 }
+};

src/module/thermistor/thermistor_1010.h

@@ -0,0 +1,41 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#define REVERSE_TEMP_SENSOR_RANGE_1010 1
+
+// Pt1000 with 1k0 pullup
+constexpr temp_entry_t temptable_1010[] PROGMEM = {
+  PtLine(  0, 1000, 1000),
+  PtLine( 25, 1000, 1000),
+  PtLine( 50, 1000, 1000),
+  PtLine( 75, 1000, 1000),
+  PtLine(100, 1000, 1000),
+  PtLine(125, 1000, 1000),
+  PtLine(150, 1000, 1000),
+  PtLine(175, 1000, 1000),
+  PtLine(200, 1000, 1000),
+  PtLine(225, 1000, 1000),
+  PtLine(250, 1000, 1000),
+  PtLine(275, 1000, 1000),
+  PtLine(300, 1000, 1000)
+};

src/module/thermistor/thermistor_1047.h

@@ -0,0 +1,40 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#define REVERSE_TEMP_SENSOR_RANGE_1047 1
+
+// Pt1000 with 4k7 pullup
+constexpr temp_entry_t temptable_1047[] PROGMEM = {
+  // only a few values are needed as the curve is very flat
+  PtLine(  0, 1000, 4700),
+  PtLine( 50, 1000, 4700),
+  PtLine(100, 1000, 4700),
+  PtLine(150, 1000, 4700),
+  PtLine(200, 1000, 4700),
+  PtLine(250, 1000, 4700),
+  PtLine(300, 1000, 4700),
+  PtLine(350, 1000, 4700),
+  PtLine(400, 1000, 4700),
+  PtLine(450, 1000, 4700),
+  PtLine(500, 1000, 4700)
+};

src/module/thermistor/thermistor_11.h

@@ -0,0 +1,76 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+// R25 = 100 kOhm, beta25 = 3950 K, 4.7 kOhm pull-up, QU-BD silicone bed QWG-104F-3950 thermistor
+constexpr temp_entry_t temptable_11[] PROGMEM = {
+  { OV(   1), 938 },
+  { OV(  31), 314 },
+  { OV(  41), 290 },
+  { OV(  51), 272 },
+  { OV(  61), 258 },
+  { OV(  71), 247 },
+  { OV(  81), 237 },
+  { OV(  91), 229 },
+  { OV( 101), 221 },
+  { OV( 111), 215 },
+  { OV( 121), 209 },
+  { OV( 131), 204 },
+  { OV( 141), 199 },
+  { OV( 151), 195 },
+  { OV( 161), 190 },
+  { OV( 171), 187 },
+  { OV( 181), 183 },
+  { OV( 191), 179 },
+  { OV( 201), 176 },
+  { OV( 221), 170 },
+  { OV( 241), 165 },
+  { OV( 261), 160 },
+  { OV( 281), 155 },
+  { OV( 301), 150 },
+  { OV( 331), 144 },
+  { OV( 361), 139 },
+  { OV( 391), 133 },
+  { OV( 421), 128 },
+  { OV( 451), 123 },
+  { OV( 491), 117 },
+  { OV( 531), 111 },
+  { OV( 571), 105 },
+  { OV( 611), 100 },
+  { OV( 641),  95 },
+  { OV( 681),  90 },
+  { OV( 711),  85 },
+  { OV( 751),  79 },
+  { OV( 791),  72 },
+  { OV( 811),  69 },
+  { OV( 831),  65 },
+  { OV( 871),  57 },
+  { OV( 881),  55 },
+  { OV( 901),  51 },
+  { OV( 921),  45 },
+  { OV( 941),  39 },
+  { OV( 971),  28 },
+  { OV( 981),  23 },
+  { OV( 991),  17 },
+  { OV(1001),   9 },
+  { OV(1021), -27 }
+};

src/module/thermistor/thermistor_110.h

@@ -0,0 +1,36 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#define REVERSE_TEMP_SENSOR_RANGE_110 1
+
+// Pt100 with 1k0 pullup
+constexpr temp_entry_t temptable_110[] PROGMEM = {
+  // only a few values are needed as the curve is very flat
+  PtLine(  0, 100, 1000),
+  PtLine( 50, 100, 1000),
+  PtLine(100, 100, 1000),
+  PtLine(150, 100, 1000),
+  PtLine(200, 100, 1000),
+  PtLine(250, 100, 1000),
+  PtLine(300, 100, 1000)
+};

src/module/thermistor/thermistor_12.h

@@ -0,0 +1,56 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+// R25 = 100 kOhm, beta25 = 4700 K, 4.7 kOhm pull-up, (personal calibration for Makibox hot bed)
+constexpr temp_entry_t temptable_12[] PROGMEM = {
+  { OV(  35), 180 }, // top rating 180C
+  { OV( 211), 140 },
+  { OV( 233), 135 },
+  { OV( 261), 130 },
+  { OV( 290), 125 },
+  { OV( 328), 120 },
+  { OV( 362), 115 },
+  { OV( 406), 110 },
+  { OV( 446), 105 },
+  { OV( 496), 100 },
+  { OV( 539),  95 },
+  { OV( 585),  90 },
+  { OV( 629),  85 },
+  { OV( 675),  80 },
+  { OV( 718),  75 },
+  { OV( 758),  70 },
+  { OV( 793),  65 },
+  { OV( 822),  60 },
+  { OV( 841),  55 },
+  { OV( 875),  50 },
+  { OV( 899),  45 },
+  { OV( 926),  40 },
+  { OV( 946),  35 },
+  { OV( 962),  30 },
+  { OV( 977),  25 },
+  { OV( 987),  20 },
+  { OV( 995),  15 },
+  { OV(1001),  10 },
+  { OV(1010),   0 },
+  { OV(1023), -40 }
+};

src/module/thermistor/thermistor_13.h

@@ -0,0 +1,49 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+// R25 = 100 kOhm, beta25 = 4100 K, 4.7 kOhm pull-up, Hisens thermistor
+constexpr temp_entry_t temptable_13[] PROGMEM = {
+  { OV( 20.04), 300 },
+  { OV( 23.19), 290 },
+  { OV( 26.71), 280 },
+  { OV( 31.23), 270 },
+  { OV( 36.52), 260 },
+  { OV( 42.75), 250 },
+  { OV( 50.68), 240 },
+  { OV( 60.22), 230 },
+  { OV( 72.03), 220 },
+  { OV( 86.84), 210 },
+  { OV(102.79), 200 },
+  { OV(124.46), 190 },
+  { OV(151.02), 180 },
+  { OV(182.86), 170 },
+  { OV(220.72), 160 },
+  { OV(316.96), 140 },
+  { OV(447.17), 120 },
+  { OV(590.61), 100 },
+  { OV(737.31),  80 },
+  { OV(857.77),  60 },
+  { OV(939.52),  40 },
+  { OV(986.03),  20 },
+  { OV(1008.7),   0 }
+};

src/module/thermistor/thermistor_147.h

@@ -0,0 +1,36 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#define REVERSE_TEMP_SENSOR_RANGE_147 1
+
+// Pt100 with 4k7 pullup
+constexpr temp_entry_t temptable_147[] PROGMEM = {
+  // only a few values are needed as the curve is very flat
+  PtLine(  0, 100, 4700),
+  PtLine( 50, 100, 4700),
+  PtLine(100, 100, 4700),
+  PtLine(150, 100, 4700),
+  PtLine(200, 100, 4700),
+  PtLine(250, 100, 4700),
+  PtLine(300, 100, 4700)
+};

src/module/thermistor/thermistor_15.h

@@ -0,0 +1,65 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+ // 100k bed thermistor in JGAurora A5. Calibrated by Sam Pinches 21st Jan 2018 using cheap k-type thermocouple inserted into heater block, using TM-902C meter.
+constexpr temp_entry_t temptable_15[] PROGMEM = {
+  { OV(  31), 275 },
+  { OV(  33), 270 },
+  { OV(  35), 260 },
+  { OV(  38), 253 },
+  { OV(  41), 248 },
+  { OV(  48), 239 },
+  { OV(  56), 232 },
+  { OV(  66), 222 },
+  { OV(  78), 212 },
+  { OV(  93), 206 },
+  { OV( 106), 199 },
+  { OV( 118), 191 },
+  { OV( 130), 186 },
+  { OV( 158), 176 },
+  { OV( 187), 167 },
+  { OV( 224), 158 },
+  { OV( 270), 148 },
+  { OV( 321), 137 },
+  { OV( 379), 127 },
+  { OV( 446), 117 },
+  { OV( 518), 106 },
+  { OV( 593),  96 },
+  { OV( 668),  86 },
+  { OV( 739),  76 },
+  { OV( 767),  72 },
+  { OV( 830),  62 },
+  { OV( 902),  48 },
+  { OV( 926),  45 },
+  { OV( 955),  35 },
+  { OV( 966),  30 },
+  { OV( 977),  25 },
+  { OV( 985),  20 },
+  { OV( 993),  15 },
+  { OV( 999),  10 },
+  { OV(1004),   5 },
+  { OV(1008),   0 },
+  { OV(1012),  -5 },
+  { OV(1016), -10 },
+  { OV(1020), -15 }
+};

src/module/thermistor/thermistor_17.h

@@ -0,0 +1,78 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+// Dagoma NTC 100k white thermistor
+constexpr temp_entry_t temptable_17[] PROGMEM = {
+  { OV(  16),  309 },
+  { OV(  18),  307 },
+  { OV(  20),  300 },
+  { OV(  22),  293 },
+  { OV(  26),  284 },
+  { OV(  29),  272 },
+  { OV(  33),  266 },
+  { OV(  36),  260 },
+  { OV(  42),  252 },
+  { OV(  46),  247 },
+  { OV(  48),  244 },
+  { OV(  51),  241 },
+  { OV(  62),  231 },
+  { OV(  73),  222 },
+  { OV(  78),  219 },
+  { OV(  87),  212 },
+  { OV(  98),  207 },
+  { OV( 109),  201 },
+  { OV( 118),  197 },
+  { OV( 131),  191 },
+  { OV( 145),  186 },
+  { OV( 160),  181 },
+  { OV( 177),  175 },
+  { OV( 203),  169 },
+  { OV( 222),  164 },
+  { OV( 256),  156 },
+  { OV( 283),  151 },
+  { OV( 312),  145 },
+  { OV( 343),  140 },
+  { OV( 377),  131 },
+  { OV( 413),  125 },
+  { OV( 454),  119 },
+  { OV( 496),  113 },
+  { OV( 537),  108 },
+  { OV( 578),  102 },
+  { OV( 619),   97 },
+  { OV( 658),   92 },
+  { OV( 695),   87 },
+  { OV( 735),   81 },
+  { OV( 773),   75 },
+  { OV( 808),   70 },
+  { OV( 844),   64 },
+  { OV( 868),   59 },
+  { OV( 892),   54 },
+  { OV( 914),   49 },
+  { OV( 935),   42 },
+  { OV( 951),   38 },
+  { OV( 967),   32 },
+  { OV( 975),   28 },
+  { OV(1000),   20 },
+  { OV(1010),   10 },
+  { OV(1024), -273 } // for safety
+};

src/module/thermistor/thermistor_18.h

@@ -0,0 +1,59 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+// ATC Semitec 204GT-2 (4.7k pullup) Dagoma.Fr - MKS_Base_DKU001327 - version (measured/tested/approved)
+constexpr temp_entry_t temptable_18[] PROGMEM = {
+  { OV(   1), 713 },
+  { OV(  17), 284 },
+  { OV(  20), 275 },
+  { OV(  23), 267 },
+  { OV(  27), 257 },
+  { OV(  31), 250 },
+  { OV(  37), 240 },
+  { OV(  43), 232 },
+  { OV(  51), 222 },
+  { OV(  61), 213 },
+  { OV(  73), 204 },
+  { OV(  87), 195 },
+  { OV( 106), 185 },
+  { OV( 128), 175 },
+  { OV( 155), 166 },
+  { OV( 189), 156 },
+  { OV( 230), 146 },
+  { OV( 278), 137 },
+  { OV( 336), 127 },
+  { OV( 402), 117 },
+  { OV( 476), 107 },
+  { OV( 554),  97 },
+  { OV( 635),  87 },
+  { OV( 713),  78 },
+  { OV( 784),  68 },
+  { OV( 846),  58 },
+  { OV( 897),  49 },
+  { OV( 937),  39 },
+  { OV( 966),  30 },
+  { OV( 986),  20 },
+  { OV(1000),  10 },
+  { OV(1010),   0 },
+  { OV(1024),-273 } // for safety
+};

src/module/thermistor/thermistor_2.h

@@ -0,0 +1,62 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+//
+// R25 = 200 kOhm, beta25 = 4338 K, 4.7 kOhm pull-up, ATC Semitec 204GT-2
+// Verified by linagee. Source: https://www.mouser.com/datasheet/2/362/semitec%20usa%20corporation_gtthermistor-1202937.pdf
+// Calculated using 4.7kohm pullup, voltage divider math, and manufacturer provided temp/resistance
+//
+constexpr temp_entry_t temptable_2[] PROGMEM = {
+  { OV(   1), 848 },
+  { OV(  30), 300 }, // top rating 300C
+  { OV(  34), 290 },
+  { OV(  39), 280 },
+  { OV(  46), 270 },
+  { OV(  53), 260 },
+  { OV(  63), 250 },
+  { OV(  74), 240 },
+  { OV(  87), 230 },
+  { OV( 104), 220 },
+  { OV( 124), 210 },
+  { OV( 148), 200 },
+  { OV( 176), 190 },
+  { OV( 211), 180 },
+  { OV( 252), 170 },
+  { OV( 301), 160 },
+  { OV( 357), 150 },
+  { OV( 420), 140 },
+  { OV( 489), 130 },
+  { OV( 562), 120 },
+  { OV( 636), 110 },
+  { OV( 708), 100 },
+  { OV( 775),  90 },
+  { OV( 835),  80 },
+  { OV( 884),  70 },
+  { OV( 924),  60 },
+  { OV( 955),  50 },
+  { OV( 977),  40 },
+  { OV( 993),  30 },
+  { OV(1004),  20 },
+  { OV(1012),  10 },
+  { OV(1016),   0 }
+};

src/module/thermistor/thermistor_20.h

@@ -0,0 +1,77 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#define REVERSE_TEMP_SENSOR_RANGE_20 1
+
+// Pt100 with INA826 amp on Ultimaker v2.0 electronics
+constexpr temp_entry_t temptable_20[] PROGMEM = {
+  { OV(  0),    0 },
+  { OV(227),    1 },
+  { OV(236),   10 },
+  { OV(245),   20 },
+  { OV(253),   30 },
+  { OV(262),   40 },
+  { OV(270),   50 },
+  { OV(279),   60 },
+  { OV(287),   70 },
+  { OV(295),   80 },
+  { OV(304),   90 },
+  { OV(312),  100 },
+  { OV(320),  110 },
+  { OV(329),  120 },
+  { OV(337),  130 },
+  { OV(345),  140 },
+  { OV(353),  150 },
+  { OV(361),  160 },
+  { OV(369),  170 },
+  { OV(377),  180 },
+  { OV(385),  190 },
+  { OV(393),  200 },
+  { OV(401),  210 },
+  { OV(409),  220 },
+  { OV(417),  230 },
+  { OV(424),  240 },
+  { OV(432),  250 },
+  { OV(440),  260 },
+  { OV(447),  270 },
+  { OV(455),  280 },
+  { OV(463),  290 },
+  { OV(470),  300 },
+  { OV(478),  310 },
+  { OV(485),  320 },
+  { OV(493),  330 },
+  { OV(500),  340 },
+  { OV(507),  350 },
+  { OV(515),  360 },
+  { OV(522),  370 },
+  { OV(529),  380 },
+  { OV(537),  390 },
+  { OV(544),  400 },
+  { OV(614),  500 },
+  { OV(681),  600 },
+  { OV(744),  700 },
+  { OV(805),  800 },
+  { OV(862),  900 },
+  { OV(917), 1000 },
+  { OV(968), 1100 }
+};

src/module/thermistor/thermistor_2000.h

@@ -0,0 +1,60 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+// R25 = 100 KOhm, beta25 = 4550 K, 4.7 kOhm pull-up, TDK NTCG104LH104KT1 https://product.tdk.com/en/search/sensor/ntc/chip-ntc-thermistor/info?part_no=NTCG104LH104KT1
+constexpr temp_entry_t temptable_2000[] PROGMEM = {
+{ OV(313),   125 },
+{ OV(347),   120 },
+{ OV(383),   115 },
+{ OV(422),   110 },
+{ OV(463),   105 },
+{ OV(506),   100 },
+{ OV(549),   95 },
+{ OV(594),   90 },
+{ OV(638),   85 },
+{ OV(681),   80 },
+{ OV(722),   75 },
+{ OV(762),   70 },
+{ OV(799),   65 },
+{ OV(833),   60 },
+{ OV(863),   55 },
+{ OV(890),   50 },
+{ OV(914),   45 },
+{ OV(934),   40 },
+{ OV(951),   35 },
+{ OV(966),   30 },
+{ OV(978),   25 },
+{ OV(988),   20 },
+{ OV(996),   15 },
+{ OV(1002),  10 },
+{ OV(1007),   5 },
+{ OV(1012),   0 },
+{ OV(1015),  -5 },
+{ OV(1017), -10 },
+{ OV(1019), -15 },
+{ OV(1020), -20 },
+{ OV(1021), -25 },
+{ OV(1022), -30 },
+{ OV(1023), -35 },
+{ OV(1023), -40 }
+};

src/module/thermistor/thermistor_201.h

@@ -0,0 +1,57 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#define REVERSE_TEMP_SENSOR_RANGE_201 1
+
+// Pt100 with LMV324 amp on Overlord v1.1 electronics
+constexpr temp_entry_t temptable_201[] PROGMEM = {
+  { OV(   0),   0 },
+  { OV(   8),   1 },
+  { OV(  23),   6 },
+  { OV(  41),  15 },
+  { OV(  51),  20 },
+  { OV(  68),  28 },
+  { OV(  74),  30 },
+  { OV(  88),  35 },
+  { OV(  99),  40 },
+  { OV( 123),  50 },
+  { OV( 148),  60 },
+  { OV( 173),  70 },
+  { OV( 198),  80 },
+  { OV( 221),  90 },
+  { OV( 245), 100 },
+  { OV( 269), 110 },
+  { OV( 294), 120 },
+  { OV( 316), 130 },
+  { OV( 342), 140 },
+  { OV( 364), 150 },
+  { OV( 387), 160 },
+  { OV( 412), 170 },
+  { OV( 433), 180 },
+  { OV( 456), 190 },
+  { OV( 480), 200 },
+  { OV( 500), 210 },
+  { OV( 548), 224 },
+  { OV( 572), 233 },
+  { OV(1155), 490 }
+};

src/module/thermistor/thermistor_202.h

@@ -0,0 +1,69 @@
+//
+// Unknown 200K thermistor on a Copymaster 3D hotend
+// Temptable sent from dealer technologyoutlet.co.uk
+//
+
+constexpr temp_entry_t temptable_202[] PROGMEM = {
+  { OV(   1), 864 },
+  { OV(  35), 300 },
+  { OV(  38), 295 },
+  { OV(  41), 290 },
+  { OV(  44), 285 },
+  { OV(  47), 280 },
+  { OV(  51), 275 },
+  { OV(  55), 270 },
+  { OV(  60), 265 },
+  { OV(  65), 260 },
+  { OV(  70), 255 },
+  { OV(  76), 250 },
+  { OV(  83), 245 },
+  { OV(  90), 240 },
+  { OV(  98), 235 },
+  { OV( 107), 230 },
+  { OV( 116), 225 },
+  { OV( 127), 220 },
+  { OV( 138), 215 },
+  { OV( 151), 210 },
+  { OV( 164), 205 },
+  { OV( 179), 200 },
+  { OV( 195), 195 },
+  { OV( 213), 190 },
+  { OV( 232), 185 },
+  { OV( 253), 180 },
+  { OV( 275), 175 },
+  { OV( 299), 170 },
+  { OV( 325), 165 },
+  { OV( 352), 160 },
+  { OV( 381), 155 },
+  { OV( 411), 150 },
+  { OV( 443), 145 },
+  { OV( 476), 140 },
+  { OV( 511), 135 },
+  { OV( 546), 130 },
+  { OV( 581), 125 },
+  { OV( 617), 120 },
+  { OV( 652), 115 },
+  { OV( 687), 110 },
+  { OV( 720), 105 },
+  { OV( 753), 100 },
+  { OV( 783),  95 },
+  { OV( 812),  90 },
+  { OV( 839),  85 },
+  { OV( 864),  80 },
+  { OV( 886),  75 },
+  { OV( 906),  70 },
+  { OV( 924),  65 },
+  { OV( 940),  60 },
+  { OV( 954),  55 },
+  { OV( 966),  50 },
+  { OV( 976),  45 },
+  { OV( 985),  40 },
+  { OV( 992),  35 },
+  { OV( 998),  30 },
+  { OV(1003),  25 },
+  { OV(1007),  20 },
+  { OV(1011),  15 },
+  { OV(1014),  10 },
+  { OV(1016),   5 },
+  { OV(1018),   0 }
+};

src/module/thermistor/thermistor_21.h

@@ -0,0 +1,78 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#define REVERSE_TEMP_SENSOR_RANGE_21 1
+
+#undef OV_SCALE
+#define OV_SCALE(N) (float((N) * 5) / 3.3f)
+
+// Pt100 with INA826 amplifier board with 5v supply based on Thermistor 20, with 3v3 ADC reference on the mainboard.
+// If the ADC reference and INA826 board supply voltage are identical, Thermistor 20 instead.
+constexpr temp_entry_t temptable_21[] PROGMEM = {
+  { OV(  0),    0 },
+  { OV(227),    1 },
+  { OV(236),   10 },
+  { OV(245),   20 },
+  { OV(253),   30 },
+  { OV(262),   40 },
+  { OV(270),   50 },
+  { OV(279),   60 },
+  { OV(287),   70 },
+  { OV(295),   80 },
+  { OV(304),   90 },
+  { OV(312),  100 },
+  { OV(320),  110 },
+  { OV(329),  120 },
+  { OV(337),  130 },
+  { OV(345),  140 },
+  { OV(353),  150 },
+  { OV(361),  160 },
+  { OV(369),  170 },
+  { OV(377),  180 },
+  { OV(385),  190 },
+  { OV(393),  200 },
+  { OV(401),  210 },
+  { OV(409),  220 },
+  { OV(417),  230 },
+  { OV(424),  240 },
+  { OV(432),  250 },
+  { OV(440),  260 },
+  { OV(447),  270 },
+  { OV(455),  280 },
+  { OV(463),  290 },
+  { OV(470),  300 },
+  { OV(478),  310 },
+  { OV(485),  320 },
+  { OV(493),  330 },
+  { OV(500),  340 },
+  { OV(507),  350 },
+  { OV(515),  360 },
+  { OV(522),  370 },
+  { OV(529),  380 },
+  { OV(537),  390 },
+  { OV(544),  400 },
+  { OV(614),  500 }
+};
+
+#undef OV_SCALE
+#define OV_SCALE(N) (N)

src/module/thermistor/thermistor_22.h

@@ -0,0 +1,72 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+// 100k hotend thermistor with 4.7k pull up to 3.3v and 220R to analog input as in GTM32 Pro vB
+constexpr temp_entry_t temptable_22[] PROGMEM = {
+  { OV(   1), 352 },
+  { OV(   6), 341 },
+  { OV(  11), 330 },
+  { OV(  16), 319 },
+  { OV(  20), 307 },
+  { OV(  26), 296 },
+  { OV(  31), 285 },
+  { OV(  40), 274 },
+  { OV(  51), 263 },
+  { OV(  61), 251 },
+  { OV(  72), 245 },
+  { OV(  77), 240 },
+  { OV(  82), 237 },
+  { OV(  87), 232 },
+  { OV(  91), 229 },
+  { OV(  94), 227 },
+  { OV(  97), 225 },
+  { OV( 100), 223 },
+  { OV( 104), 221 },
+  { OV( 108), 219 },
+  { OV( 115), 214 },
+  { OV( 126), 209 },
+  { OV( 137), 204 },
+  { OV( 147), 200 },
+  { OV( 158), 193 },
+  { OV( 167), 192 },
+  { OV( 177), 189 },
+  { OV( 197), 163 },
+  { OV( 230), 174 },
+  { OV( 267), 165 },
+  { OV( 310), 158 },
+  { OV( 336), 151 },
+  { OV( 379), 143 },
+  { OV( 413), 138 },
+  { OV( 480), 127 },
+  { OV( 580), 110 },
+  { OV( 646), 100 },
+  { OV( 731),  88 },
+  { OV( 768),  84 },
+  { OV( 861),  69 },
+  { OV( 935),  50 },
+  { OV( 975),  38 },
+  { OV(1001),  27 },
+  { OV(1011),  22 },
+  { OV(1015),  13 },
+  { OV(1020),   6 },
+  { OV(1023),   0 }
+};

src/module/thermistor/thermistor_23.h

@@ -0,0 +1,128 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+// 100k hotbed thermistor with 4.7k pull up to 3.3v and 220R to analog input as in GTM32 Pro vB
+constexpr temp_entry_t temptable_23[] PROGMEM = {
+  { OV(   1), 938 },
+  { OV(  11), 423 },
+  { OV(  21), 351 },
+  { OV(  31), 314 },
+  { OV(  41), 290 },
+  { OV(  51), 272 },
+  { OV(  61), 258 },
+  { OV(  71), 247 },
+  { OV(  81), 237 },
+  { OV(  91), 229 },
+  { OV( 101), 221 },
+  { OV( 111), 215 },
+  { OV( 121), 209 },
+  { OV( 131), 204 },
+  { OV( 141), 199 },
+  { OV( 151), 195 },
+  { OV( 161), 190 },
+  { OV( 171), 187 },
+  { OV( 181), 183 },
+  { OV( 191), 179 },
+  { OV( 201), 176 },
+  { OV( 211), 173 },
+  { OV( 221), 170 },
+  { OV( 231), 167 },
+  { OV( 241), 165 },
+  { OV( 251), 162 },
+  { OV( 261), 160 },
+  { OV( 271), 157 },
+  { OV( 281), 155 },
+  { OV( 291), 153 },
+  { OV( 301), 150 },
+  { OV( 311), 148 },
+  { OV( 321), 146 },
+  { OV( 331), 144 },
+  { OV( 341), 142 },
+  { OV( 351), 140 },
+  { OV( 361), 139 },
+  { OV( 371), 137 },
+  { OV( 381), 135 },
+  { OV( 391), 133 },
+  { OV( 401), 131 },
+  { OV( 411), 130 },
+  { OV( 421), 128 },
+  { OV( 431), 126 },
+  { OV( 441), 125 },
+  { OV( 451), 123 },
+  { OV( 461), 122 },
+  { OV( 471), 120 },
+  { OV( 481), 119 },
+  { OV( 491), 117 },
+  { OV( 501), 116 },
+  { OV( 511), 114 },
+  { OV( 521), 113 },
+  { OV( 531), 111 },
+  { OV( 541), 110 },
+  { OV( 551), 108 },
+  { OV( 561), 107 },
+  { OV( 571), 105 },
+  { OV( 581), 104 },
+  { OV( 591), 102 },
+  { OV( 601), 101 },
+  { OV( 611), 100 },
+  { OV( 621),  98 },
+  { OV( 631),  97 },
+  { OV( 641),  95 },
+  { OV( 651),  94 },
+  { OV( 661),  92 },
+  { OV( 671),  91 },
+  { OV( 681),  90 },
+  { OV( 691),  88 },
+  { OV( 701),  87 },
+  { OV( 711),  85 },
+  { OV( 721),  84 },
+  { OV( 731),  82 },
+  { OV( 741),  81 },
+  { OV( 751),  79 },
+  { OV( 761),  77 },
+  { OV( 771),  76 },
+  { OV( 781),  74 },
+  { OV( 791),  72 },
+  { OV( 801),  71 },
+  { OV( 811),  69 },
+  { OV( 821),  67 },
+  { OV( 831),  65 },
+  { OV( 841),  63 },
+  { OV( 851),  62 },
+  { OV( 861),  60 },
+  { OV( 871),  57 },
+  { OV( 881),  55 },
+  { OV( 891),  53 },
+  { OV( 901),  51 },
+  { OV( 911),  48 },
+  { OV( 921),  45 },
+  { OV( 931),  42 },
+  { OV( 941),  39 },
+  { OV( 951),  36 },
+  { OV( 961),  32 },
+  { OV( 971),  28 },
+  { OV( 981),  25 },
+  { OV( 991),  23 },
+  { OV(1001),  21 },
+  { OV(1011),  19 },
+  { OV(1021),   5 }
+};

src/module/thermistor/thermistor_3.h

@@ -0,0 +1,54 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+// R25 = 100 kOhm, beta25 = 4120 K, 4.7 kOhm pull-up, mendel-parts
+constexpr temp_entry_t temptable_3[] PROGMEM = {
+  { OV(   1), 864 },
+  { OV(  21), 300 },
+  { OV(  25), 290 },
+  { OV(  29), 280 },
+  { OV(  33), 270 },
+  { OV(  39), 260 },
+  { OV(  46), 250 },
+  { OV(  54), 240 },
+  { OV(  64), 230 },
+  { OV(  75), 220 },
+  { OV(  90), 210 },
+  { OV( 107), 200 },
+  { OV( 128), 190 },
+  { OV( 154), 180 },
+  { OV( 184), 170 },
+  { OV( 221), 160 },
+  { OV( 265), 150 },
+  { OV( 316), 140 },
+  { OV( 375), 130 },
+  { OV( 441), 120 },
+  { OV( 513), 110 },
+  { OV( 588), 100 },
+  { OV( 734),  80 },
+  { OV( 856),  60 },
+  { OV( 938),  40 },
+  { OV( 986),  20 },
+  { OV(1008),   0 },
+  { OV(1018), -20 }
+};

src/module/thermistor/thermistor_30.h

@@ -0,0 +1,66 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+// R25 = 100 kOhm, beta25 = 3950 K, 4.7 kOhm pull-up
+// Resistance         100k Ohms at 25deg. C
+// Resistance Tolerance     + / -1%
+// B Value             3950K at 25/50 deg. C
+// B Value Tolerance         + / - 1%
+// Kis3d Silicone Heater 24V 200W/300W with 6mm Precision cast plate (EN AW 5083)
+// Temperature setting time 10 min to determine the 12Bit ADC value on the surface. (le3tspeak)
+constexpr temp_entry_t temptable_30[] PROGMEM = {
+  { OV(   1), 938 },
+  { OV( 298), 125 }, // 1193 - 125°
+  { OV( 321), 121 }, // 1285 - 121°
+  { OV( 348), 117 }, // 1392 - 117°
+  { OV( 387), 113 }, // 1550 - 113°
+  { OV( 411), 110 }, // 1644 - 110°
+  { OV( 445), 106 }, // 1780 - 106°
+  { OV( 480), 101 }, // 1920 - 101°
+  { OV( 516),  97 }, // 2064 - 97°
+  { OV( 553),  92 }, // 2212 - 92°
+  { OV( 591),  88 }, // 2364 - 88°
+  { OV( 628),  84 }, // 2512 - 84°
+  { OV( 665),  79 }, // 2660 - 79°
+  { OV( 702),  75 }, // 2808 - 75°
+  { OV( 736),  71 }, // 2945 - 71°
+  { OV( 770),  67 }, // 3080 - 67°
+  { OV( 801),  63 }, // 3204 - 63°
+  { OV( 830),  59 }, // 3320 - 59°
+  { OV( 857),  55 }, // 3428 - 55°
+  { OV( 881),  51 }, // 3524 - 51°
+  { OV( 902),  47 }, // 3611 - 47°
+  { OV( 922),  42 }, // 3688 - 42°
+  { OV( 938),  38 }, // 3754 - 38°
+  { OV( 952),  34 }, // 3811 - 34°
+  { OV( 964),  29 }, // 3857 - 29°
+  { OV( 975),  25 }, // 3900 - 25°
+  { OV( 980),  23 }, // 3920 - 23°
+  { OV( 991),  17 }, // 3964 - 17°
+  { OV(1001),   9 }, // Calculated
+  { OV(1004),   5 }, // Calculated
+  { OV(1008),   0 }, // Calculated
+  { OV(1012),  -5 }, // Calculated
+  { OV(1016), -10 }, // Calculated
+  { OV(1020), -15 }  // Calculated
+};

src/module/thermistor/thermistor_331.h

@@ -0,0 +1,92 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#define OVM(V) OV((V)*(0.327/0.5))
+
+// R25 = 100 kOhm, beta25 = 4092 K, 4.7 kOhm pull-up, bed thermistor
+constexpr temp_entry_t temptable_331[] PROGMEM = {
+  { OVM(  23), 300 },
+  { OVM(  25), 295 },
+  { OVM(  27), 290 },
+  { OVM(  28), 285 },
+  { OVM(  31), 280 },
+  { OVM(  33), 275 },
+  { OVM(  35), 270 },
+  { OVM(  38), 265 },
+  { OVM(  41), 260 },
+  { OVM(  44), 255 },
+  { OVM(  48), 250 },
+  { OVM(  52), 245 },
+  { OVM(  56), 240 },
+  { OVM(  61), 235 },
+  { OVM(  66), 230 },
+  { OVM(  71), 225 },
+  { OVM(  78), 220 },
+  { OVM(  84), 215 },
+  { OVM(  92), 210 },
+  { OVM( 100), 205 },
+  { OVM( 109), 200 },
+  { OVM( 120), 195 },
+  { OVM( 131), 190 },
+  { OVM( 143), 185 },
+  { OVM( 156), 180 },
+  { OVM( 171), 175 },
+  { OVM( 187), 170 },
+  { OVM( 205), 165 },
+  { OVM( 224), 160 },
+  { OVM( 245), 155 },
+  { OVM( 268), 150 },
+  { OVM( 293), 145 },
+  { OVM( 320), 140 },
+  { OVM( 348), 135 },
+  { OVM( 379), 130 },
+  { OVM( 411), 125 },
+  { OVM( 445), 120 },
+  { OVM( 480), 115 },
+  { OVM( 516), 110 },
+  { OVM( 553), 105 },
+  { OVM( 591), 100 },
+  { OVM( 628),  95 },
+  { OVM( 665),  90 },
+  { OVM( 702),  85 },
+  { OVM( 737),  80 },
+  { OVM( 770),  75 },
+  { OVM( 801),  70 },
+  { OVM( 830),  65 },
+  { OVM( 857),  60 },
+  { OVM( 881),  55 },
+  { OVM( 903),  50 },
+  { OVM( 922),  45 },
+  { OVM( 939),  40 },
+  { OVM( 954),  35 },
+  { OVM( 966),  30 },
+  { OVM( 977),  25 },
+  { OVM( 985),  20 },
+  { OVM( 993),  15 },
+  { OVM( 999),  10 },
+  { OVM(1004),   5 },
+  { OVM(1008),   0 },
+  { OVM(1012),  -5 },
+  { OVM(1016), -10 },
+  { OVM(1020), -15 }
+};

src/module/thermistor/thermistor_332.h

@@ -0,0 +1,50 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#define OVM(V) OV((V)*(0.327/0.327))
+
+// R25 = 100 kOhm, beta25 = 4092 K, 4.7 kOhm pull-up, bed thermistor
+constexpr temp_entry_t temptable_332[] PROGMEM = {
+  { OVM( 268), 150 },
+  { OVM( 293), 145 },
+  { OVM( 320), 141 },
+  { OVM( 379), 133 },
+  { OVM( 445), 122 },
+  { OVM( 516), 108 },
+  { OVM( 591),  98 },
+  { OVM( 665),  88 },
+  { OVM( 737),  79 },
+  { OVM( 801),  70 },
+  { OVM( 857),  55 },
+  { OVM( 903),  46 },
+  { OVM( 939),  39 },
+  { OVM( 954),  33 },
+  { OVM( 966),  27 },
+  { OVM( 977),  22 },
+  { OVM( 999),  15 },
+  { OVM(1004),   5 },
+  { OVM(1008),   0 },
+  { OVM(1012),  -5 },
+  { OVM(1016), -10 },
+  { OVM(1020), -15 }
+};

src/module/thermistor/thermistor_4.h

@@ -0,0 +1,46 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+// R25 = 10 kOhm, beta25 = 3950 K, 4.7 kOhm pull-up, Generic 10k thermistor
+constexpr temp_entry_t temptable_4[] PROGMEM = {
+  { OV(   1), 430 },
+  { OV(  54), 137 },
+  { OV( 107), 107 },
+  { OV( 160),  91 },
+  { OV( 213),  80 },
+  { OV( 266),  71 },
+  { OV( 319),  64 },
+  { OV( 372),  57 },
+  { OV( 425),  51 },
+  { OV( 478),  46 },
+  { OV( 531),  41 },
+  { OV( 584),  35 },
+  { OV( 637),  30 },
+  { OV( 690),  25 },
+  { OV( 743),  20 },
+  { OV( 796),  14 },
+  { OV( 849),   7 },
+  { OV( 902),   0 },
+  { OV( 955), -11 },
+  { OV(1008), -35 }
+};

src/module/thermistor/thermistor_5.h

@@ -0,0 +1,62 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+// R25 = 100 kOhm, beta25 = 4267 K, 4.7 kOhm pull-up
+// 100k ParCan thermistor (104GT-2)
+// ATC Semitec 104GT-2/104NT-4-R025H42G (Used in ParCan)
+// Verified by linagee. Source: https://www.mouser.com/datasheet/2/362/semitec%20usa%20corporation_gtthermistor-1202937.pdf
+// Calculated using 4.7kohm pullup, voltage divider math, and manufacturer provided temp/resistance
+constexpr temp_entry_t temptable_5[] PROGMEM = {
+  { OV(   1), 713 },
+  { OV(  17), 300 }, // top rating 300C
+  { OV(  20), 290 },
+  { OV(  23), 280 },
+  { OV(  27), 270 },
+  { OV(  31), 260 },
+  { OV(  37), 250 },
+  { OV(  43), 240 },
+  { OV(  51), 230 },
+  { OV(  61), 220 },
+  { OV(  73), 210 },
+  { OV(  87), 200 },
+  { OV( 106), 190 },
+  { OV( 128), 180 },
+  { OV( 155), 170 },
+  { OV( 189), 160 },
+  { OV( 230), 150 },
+  { OV( 278), 140 },
+  { OV( 336), 130 },
+  { OV( 402), 120 },
+  { OV( 476), 110 },
+  { OV( 554), 100 },
+  { OV( 635),  90 },
+  { OV( 713),  80 },
+  { OV( 784),  70 },
+  { OV( 846),  60 },
+  { OV( 897),  50 },
+  { OV( 937),  40 },
+  { OV( 966),  30 },
+  { OV( 986),  20 },
+  { OV(1000),  10 },
+  { OV(1010),   0 }
+};

src/module/thermistor/thermistor_501.h

@@ -0,0 +1,58 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+// 100k Zonestar thermistor. Adjusted By Hally
+constexpr temp_entry_t temptable_501[] PROGMEM = {
+   { OV(   1), 713 },
+   { OV(  14), 300 }, // Top rating 300C
+   { OV(  16), 290 },
+   { OV(  19), 280 },
+   { OV(  23), 270 },
+   { OV(  27), 260 },
+   { OV(  31), 250 },
+   { OV(  37), 240 },
+   { OV(  47), 230 },
+   { OV(  57), 220 },
+   { OV(  68), 210 },
+   { OV(  84), 200 },
+   { OV( 100), 190 },
+   { OV( 128), 180 },
+   { OV( 155), 170 },
+   { OV( 189), 160 },
+   { OV( 230), 150 },
+   { OV( 278), 140 },
+   { OV( 336), 130 },
+   { OV( 402), 120 },
+   { OV( 476), 110 },
+   { OV( 554), 100 },
+   { OV( 635),  90 },
+   { OV( 713),  80 },
+   { OV( 784),  70 },
+   { OV( 846),  60 },
+   { OV( 897),  50 },
+   { OV( 937),  40 },
+   { OV( 966),  30 },
+   { OV( 986),  20 },
+   { OV(1000),  10 },
+   { OV(1010),   0 }
+};

src/module/thermistor/thermistor_502.h

@@ -0,0 +1,60 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+// Unknown thermistor for the Zonestar P802M hot bed. Adjusted By Nerseth
+// These were the shipped settings from Zonestar in original firmware: P802M_8_Repetier_V1.6_Zonestar.zip
+constexpr temp_entry_t temptable_502[] PROGMEM = {
+   { OV(  56.0 / 4), 300 },
+   { OV( 187.0 / 4), 250 },
+   { OV( 615.0 / 4), 190 },
+   { OV( 690.0 / 4), 185 },
+   { OV( 750.0 / 4), 180 },
+   { OV( 830.0 / 4), 175 },
+   { OV( 920.0 / 4), 170 },
+   { OV(1010.0 / 4), 165 },
+   { OV(1118.0 / 4), 160 },
+   { OV(1215.0 / 4), 155 },
+   { OV(1330.0 / 4), 145 },
+   { OV(1460.0 / 4), 140 },
+   { OV(1594.0 / 4), 135 },
+   { OV(1752.0 / 4), 130 },
+   { OV(1900.0 / 4), 125 },
+   { OV(2040.0 / 4), 120 },
+   { OV(2200.0 / 4), 115 },
+   { OV(2350.0 / 4), 110 },
+   { OV(2516.0 / 4), 105 },
+   { OV(2671.0 / 4),  98 },
+   { OV(2831.0 / 4),  92 },
+   { OV(2975.0 / 4),  85 },
+   { OV(3115.0 / 4),  76 },
+   { OV(3251.0 / 4),  72 },
+   { OV(3480.0 / 4),  62 },
+   { OV(3580.0 / 4),  52 },
+   { OV(3660.0 / 4),  46 },
+   { OV(3740.0 / 4),  40 },
+   { OV(3869.0 / 4),  30 },
+   { OV(3912.0 / 4),  25 },
+   { OV(3948.0 / 4),  20 },
+   { OV(4077.0 / 4), -20 },
+   { OV(4094.0 / 4), -55 }
+};

src/module/thermistor/thermistor_503.h

@@ -0,0 +1,57 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+// Zonestar (Z8XM2) Heated Bed thermistor. Added By AvanOsch
+// These are taken from the Zonestar settings in original Repetier firmware: Z8XM2_ZRIB_LCD12864_V51.zip
+constexpr temp_entry_t temptable_503[] PROGMEM = {
+   { OV(  12), 300 },
+   { OV(  27), 270 },
+   { OV(  47), 250 },
+   { OV(  68), 230 },
+   { OV(  99), 210 },
+   { OV( 120), 200 },
+   { OV( 141), 190 },
+   { OV( 171), 180 },
+   { OV( 201), 170 },
+   { OV( 261), 160 },
+   { OV( 321), 150 },
+   { OV( 401), 140 },
+   { OV( 451), 130 },
+   { OV( 551), 120 },
+   { OV( 596), 110 },
+   { OV( 626), 105 },
+   { OV( 666), 100 },
+   { OV( 697), 90 },
+   { OV( 717), 85 },
+   { OV( 798), 69 },
+   { OV( 819), 65 },
+   { OV( 870), 55 },
+   { OV( 891), 51 },
+   { OV( 922), 39 },
+   { OV( 968), 28 },
+   { OV( 980), 23 },
+   { OV( 991), 17 },
+   { OV( 1001), 9 },
+   { OV(1021), -27 },
+   { OV(1023), -200}
+};