Initial commit

30 files changed, 8208 insertions, 0 deletions

diff --git a/Marlin/src/HAL/AVR/HAL.cpp b/Marlin/src/HAL/AVR/HAL.cpp
new file mode 100644
index 0000000..4c45a5d
--- /dev/null
+++ b/Marlin/src/HAL/AVR/HAL.cpp
@@ -0,0 +1,85 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#ifdef __AVR__
+
+#include "../../inc/MarlinConfig.h"
+#include "HAL.h"
+
+#ifdef USBCON
+  DefaultSerial MSerial(false, Serial);
+  #ifdef BLUETOOTH
+    BTSerial btSerial(false, bluetoothSerial);
+  #endif
+#endif
+
+// ------------------------
+// Public Variables
+// ------------------------
+
+//uint8_t MCUSR;
+
+// ------------------------
+// Public functions
+// ------------------------
+
+void HAL_init() {
+  // Init Servo Pins
+  #define INIT_SERVO(N) OUT_WRITE(SERVO##N##_PIN, LOW)
+  #if HAS_SERVO_0
+    INIT_SERVO(0);
+  #endif
+  #if HAS_SERVO_1
+    INIT_SERVO(1);
+  #endif
+  #if HAS_SERVO_2
+    INIT_SERVO(2);
+  #endif
+  #if HAS_SERVO_3
+    INIT_SERVO(3);
+  #endif
+}
+
+#if ENABLED(SDSUPPORT)
+
+  #include "../../sd/SdFatUtil.h"
+  int freeMemory() { return SdFatUtil::FreeRam(); }
+
+#else // !SDSUPPORT
+
+extern "C" {
+  extern char __bss_end;
+  extern char __heap_start;
+  extern void* __brkval;
+
+  int freeMemory() {
+    int free_memory;
+    if ((int)__brkval == 0)
+      free_memory = ((int)&free_memory) - ((int)&__bss_end);
+    else
+      free_memory = ((int)&free_memory) - ((int)__brkval);
+    return free_memory;
+  }
+}
+
+#endif // !SDSUPPORT
+
+#endif // __AVR__
diff --git a/Marlin/src/HAL/AVR/HAL.h b/Marlin/src/HAL/AVR/HAL.h
new file mode 100644
index 0000000..5e22ac0
--- /dev/null
+++ b/Marlin/src/HAL/AVR/HAL.h
@@ -0,0 +1,210 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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 "../shared/Marduino.h"
+#include "../shared/HAL_SPI.h"
+#include "fastio.h"
+#include "watchdog.h"
+#include "math.h"
+
+#ifdef USBCON
+  #include <HardwareSerial.h>
+#else
+  #define HardwareSerial_h // Hack to prevent HardwareSerial.h header inclusion
+  #include "MarlinSerial.h"
+#endif
+
+#include <stdint.h>
+#include <util/delay.h>
+#include <avr/eeprom.h>
+#include <avr/pgmspace.h>
+#include <avr/interrupt.h>
+#include <avr/io.h>
+
+#ifndef pgm_read_ptr
+  // Compatibility for avr-libc 1.8.0-4.1 included with Ubuntu for
+  // Windows Subsystem for Linux on Windows 10 as of 10/18/2019
+  #define pgm_read_ptr_far(address_long) (void*)__ELPM_word((uint32_t)(address_long))
+  #define pgm_read_ptr_near(address_short) (void*)__LPM_word((uint16_t)(address_short))
+  #define pgm_read_ptr(address_short) pgm_read_ptr_near(address_short)
+#endif
+
+// ------------------------
+// Defines
+// ------------------------
+
+// AVR PROGMEM extension for sprintf_P
+#define S_FMT "%S"
+
+// AVR PROGMEM extension for string define
+#define PGMSTR(NAM,STR) const char NAM[] PROGMEM = STR
+
+#ifndef CRITICAL_SECTION_START
+  #define CRITICAL_SECTION_START()  unsigned char _sreg = SREG; cli()
+  #define CRITICAL_SECTION_END()    SREG = _sreg
+#endif
+#define ISRS_ENABLED() TEST(SREG, SREG_I)
+#define ENABLE_ISRS()  sei()
+#define DISABLE_ISRS() cli()
+
+// ------------------------
+// Types
+// ------------------------
+
+typedef int8_t pin_t;
+
+#define SHARED_SERVOS HAS_SERVOS
+#define HAL_SERVO_LIB Servo
+
+// ------------------------
+// Public Variables
+// ------------------------
+
+//extern uint8_t MCUSR;
+
+// Serial ports
+#ifdef USBCON
+  #include "../../core/serial_hook.h"
+  typedef ForwardSerial0Type< decltype(Serial) > DefaultSerial;
+  extern DefaultSerial MSerial;
+  #ifdef BLUETOOTH
+    typedef ForwardSerial0Type< decltype(bluetoothSerial) > BTSerial;
+    extern BTSerial btSerial;
+  #endif
+
+  #define MYSERIAL0 TERN(BLUETOOTH, btSerial, MSerial)
+#else
+  #if !WITHIN(SERIAL_PORT, -1, 3)
+    #error "SERIAL_PORT must be from -1 to 3. Please update your configuration."
+  #endif
+  #define MYSERIAL0 customizedSerial1
+
+  #ifdef SERIAL_PORT_2
+    #if !WITHIN(SERIAL_PORT_2, -1, 3)
+      #error "SERIAL_PORT_2 must be from -1 to 3. Please update your configuration."
+    #endif
+    #define MYSERIAL1 customizedSerial2
+  #endif
+#endif
+
+#ifdef MMU2_SERIAL_PORT
+  #if !WITHIN(MMU2_SERIAL_PORT, -1, 3)
+    #error "MMU2_SERIAL_PORT must be from -1 to 3. Please update your configuration."
+  #endif
+  #define MMU2_SERIAL mmuSerial
+#endif
+
+#ifdef LCD_SERIAL_PORT
+  #if !WITHIN(LCD_SERIAL_PORT, -1, 3)
+    #error "LCD_SERIAL_PORT must be from -1 to 3. Please update your configuration."
+  #endif
+  #define LCD_SERIAL lcdSerial
+  #if HAS_DGUS_LCD
+    #define SERIAL_GET_TX_BUFFER_FREE() LCD_SERIAL.get_tx_buffer_free()
+  #endif
+#endif
+
+// ------------------------
+// Public functions
+// ------------------------
+
+void HAL_init();
+
+//void cli();
+
+//void _delay_ms(const int delay);
+
+inline void HAL_clear_reset_source() { MCUSR = 0; }
+inline uint8_t HAL_get_reset_source() { return MCUSR; }
+
+inline void HAL_reboot() {}  // reboot the board or restart the bootloader
+
+#if GCC_VERSION <= 50000
+  #pragma GCC diagnostic push
+  #pragma GCC diagnostic ignored "-Wunused-function"
+#endif
+
+extern "C" int freeMemory();
+
+#if GCC_VERSION <= 50000
+  #pragma GCC diagnostic pop
+#endif
+
+// ADC
+#ifdef DIDR2
+  #define HAL_ANALOG_SELECT(ind) do{ if (ind < 8) SBI(DIDR0, ind); else SBI(DIDR2, ind & 0x07); }while(0)
+#else
+  #define HAL_ANALOG_SELECT(ind) SBI(DIDR0, ind);
+#endif
+
+inline void HAL_adc_init() {
+  ADCSRA = _BV(ADEN) | _BV(ADSC) | _BV(ADIF) | 0x07;
+  DIDR0 = 0;
+  #ifdef DIDR2
+    DIDR2 = 0;
+  #endif
+}
+
+#define SET_ADMUX_ADCSRA(ch) ADMUX = _BV(REFS0) | (ch & 0x07); SBI(ADCSRA, ADSC)
+#ifdef MUX5
+  #define HAL_START_ADC(ch) if (ch > 7) ADCSRB = _BV(MUX5); else ADCSRB = 0; SET_ADMUX_ADCSRA(ch)
+#else
+  #define HAL_START_ADC(ch) ADCSRB = 0; SET_ADMUX_ADCSRA(ch)
+#endif
+
+#define HAL_ADC_VREF        5.0
+#define HAL_ADC_RESOLUTION 10
+#define HAL_READ_ADC()  ADC
+#define HAL_ADC_READY() !TEST(ADCSRA, ADSC)
+
+#define GET_PIN_MAP_PIN(index) index
+#define GET_PIN_MAP_INDEX(pin) pin
+#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval)
+
+#define HAL_SENSITIVE_PINS 0, 1
+
+#ifdef __AVR_AT90USB1286__
+  #define JTAG_DISABLE() do{ MCUCR = 0x80; MCUCR = 0x80; }while(0)
+#endif
+
+// AVR compatibility
+#define strtof strtod
+
+#define HAL_CAN_SET_PWM_FREQ   // This HAL supports PWM Frequency adjustment
+
+/**
+ *  set_pwm_frequency
+ *  Sets the frequency of the timer corresponding to the provided pin
+ *  as close as possible to the provided desired frequency. Internally
+ *  calculates the required waveform generation mode, prescaler and
+ *  resolution values required and sets the timer registers accordingly.
+ *  NOTE that the frequency is applied to all pins on the timer (Ex OC3A, OC3B and OC3B)
+ *  NOTE that there are limitations, particularly if using TIMER2. (see Configuration_adv.h -> FAST FAN PWM Settings)
+ */
+void set_pwm_frequency(const pin_t pin, int f_desired);
+
+/**
+ * set_pwm_duty
+ *  Sets the PWM duty cycle of the provided pin to the provided value
+ *  Optionally allows inverting the duty cycle [default = false]
+ *  Optionally allows changing the maximum size of the provided value to enable finer PWM duty control [default = 255]
+ */
+void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size=255, const bool invert=false);
diff --git a/Marlin/src/HAL/AVR/HAL_SPI.cpp b/Marlin/src/HAL/AVR/HAL_SPI.cpp
new file mode 100644
index 0000000..3e5572e
--- /dev/null
+++ b/Marlin/src/HAL/AVR/HAL_SPI.cpp
@@ -0,0 +1,253 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * Adapted from Arduino Sd2Card Library
+ * Copyright (c) 2009 by William Greiman
+ */
+
+/**
+ * HAL for AVR - SPI functions
+ */
+
+#ifdef __AVR__
+
+#include "../../inc/MarlinConfig.h"
+
+void spiBegin() {
+  OUT_WRITE(SD_SS_PIN, HIGH);
+  SET_OUTPUT(SD_SCK_PIN);
+  SET_INPUT(SD_MISO_PIN);
+  SET_OUTPUT(SD_MOSI_PIN);
+
+  #if DISABLED(SOFTWARE_SPI)
+    // SS must be in output mode even it is not chip select
+    //SET_OUTPUT(SD_SS_PIN);
+    // set SS high - may be chip select for another SPI device
+    //#if SET_SPI_SS_HIGH
+      //WRITE(SD_SS_PIN, HIGH);
+    //#endif
+    // set a default rate
+    spiInit(1);
+  #endif
+}
+
+#if NONE(SOFTWARE_SPI, FORCE_SOFT_SPI)
+
+  // ------------------------
+  // Hardware SPI
+  // ------------------------
+
+  // make sure SPCR rate is in expected bits
+  #if (SPR0 != 0 || SPR1 != 1)
+    #error "unexpected SPCR bits"
+  #endif
+
+  /**
+   * Initialize hardware SPI
+   * Set SCK rate to F_CPU/pow(2, 1 + spiRate) for spiRate [0,6]
+   */
+  void spiInit(uint8_t spiRate) {
+    // See avr processor documentation
+    CBI(
+      #ifdef PRR
+        PRR
+      #elif defined(PRR0)
+        PRR0
+      #endif
+        , PRSPI);
+
+    SPCR = _BV(SPE) | _BV(MSTR) | (spiRate >> 1);
+    SPSR = spiRate & 1 || spiRate == 6 ? 0 : _BV(SPI2X);
+  }
+
+  /** SPI receive a byte */
+  uint8_t spiRec() {
+    SPDR = 0xFF;
+    while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
+    return SPDR;
+  }
+
+  /** SPI read data  */
+  void spiRead(uint8_t* buf, uint16_t nbyte) {
+    if (nbyte-- == 0) return;
+    SPDR = 0xFF;
+    for (uint16_t i = 0; i < nbyte; i++) {
+      while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
+      buf[i] = SPDR;
+      SPDR = 0xFF;
+    }
+    while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
+    buf[nbyte] = SPDR;
+  }
+
+  /** SPI send a byte */
+  void spiSend(uint8_t b) {
+    SPDR = b;
+    while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
+  }
+
+  /** SPI send block  */
+  void spiSendBlock(uint8_t token, const uint8_t* buf) {
+    SPDR = token;
+    for (uint16_t i = 0; i < 512; i += 2) {
+      while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
+      SPDR = buf[i];
+      while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
+      SPDR = buf[i + 1];
+    }
+    while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
+  }
+
+
+  /** begin spi transaction */
+  void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode) {
+    // Based on Arduino SPI library
+    // Clock settings are defined as follows. Note that this shows SPI2X
+    // inverted, so the bits form increasing numbers. Also note that
+    // fosc/64 appears twice
+    // SPR1 SPR0 ~SPI2X Freq
+    //   0    0     0   fosc/2
+    //   0    0     1   fosc/4
+    //   0    1     0   fosc/8
+    //   0    1     1   fosc/16
+    //   1    0     0   fosc/32
+    //   1    0     1   fosc/64
+    //   1    1     0   fosc/64
+    //   1    1     1   fosc/128
+
+    // We find the fastest clock that is less than or equal to the
+    // given clock rate. The clock divider that results in clock_setting
+    // is 2 ^^ (clock_div + 1). If nothing is slow enough, we'll use the
+    // slowest (128 == 2 ^^ 7, so clock_div = 6).
+    uint8_t clockDiv;
+
+    // When the clock is known at compiletime, use this if-then-else
+    // cascade, which the compiler knows how to completely optimize
+    // away. When clock is not known, use a loop instead, which generates
+    // shorter code.
+    if (__builtin_constant_p(spiClock)) {
+      if (spiClock >= F_CPU / 2)       clockDiv = 0;
+      else if (spiClock >= F_CPU / 4)  clockDiv = 1;
+      else if (spiClock >= F_CPU / 8)  clockDiv = 2;
+      else if (spiClock >= F_CPU / 16) clockDiv = 3;
+      else if (spiClock >= F_CPU / 32) clockDiv = 4;
+      else if (spiClock >= F_CPU / 64) clockDiv = 5;
+      else                             clockDiv = 6;
+    }
+    else {
+      uint32_t clockSetting = F_CPU / 2;
+      clockDiv = 0;
+      while (clockDiv < 6 && spiClock < clockSetting) {
+        clockSetting /= 2;
+        clockDiv++;
+      }
+    }
+
+    // Compensate for the duplicate fosc/64
+    if (clockDiv == 6) clockDiv = 7;
+
+    // Invert the SPI2X bit
+    clockDiv ^= 0x1;
+
+    SPCR = _BV(SPE) | _BV(MSTR) | ((bitOrder == LSBFIRST) ? _BV(DORD) : 0) |
+      (dataMode << CPHA) | ((clockDiv >> 1) << SPR0);
+    SPSR = clockDiv | 0x01;
+  }
+
+
+#else // SOFTWARE_SPI || FORCE_SOFT_SPI
+
+  // ------------------------
+  // Software SPI
+  // ------------------------
+
+  // nop to tune soft SPI timing
+  #define nop asm volatile ("\tnop\n")
+
+  void spiInit(uint8_t) { /* do nothing */ }
+
+  // Begin SPI transaction, set clock, bit order, data mode
+  void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode) { /* do nothing */ }
+
+  // Soft SPI receive byte
+  uint8_t spiRec() {
+    uint8_t data = 0;
+    // no interrupts during byte receive - about 8µs
+    cli();
+    // output pin high - like sending 0xFF
+    WRITE(SD_MOSI_PIN, HIGH);
+
+    LOOP_L_N(i, 8) {
+      WRITE(SD_SCK_PIN, HIGH);
+
+      nop; // adjust so SCK is nice
+      nop;
+
+      data <<= 1;
+
+      if (READ(SD_MISO_PIN)) data |= 1;
+
+      WRITE(SD_SCK_PIN, LOW);
+    }
+
+    sei();
+    return data;
+  }
+
+  // Soft SPI read data
+  void spiRead(uint8_t* buf, uint16_t nbyte) {
+    for (uint16_t i = 0; i < nbyte; i++)
+      buf[i] = spiRec();
+  }
+
+  // Soft SPI send byte
+  void spiSend(uint8_t data) {
+    // no interrupts during byte send - about 8µs
+    cli();
+    LOOP_L_N(i, 8) {
+      WRITE(SD_SCK_PIN, LOW);
+      WRITE(SD_MOSI_PIN, data & 0x80);
+      data <<= 1;
+      WRITE(SD_SCK_PIN, HIGH);
+    }
+
+    nop; // hold SCK high for a few ns
+    nop;
+    nop;
+    nop;
+
+    WRITE(SD_SCK_PIN, LOW);
+
+    sei();
+  }
+
+  // Soft SPI send block
+  void spiSendBlock(uint8_t token, const uint8_t* buf) {
+    spiSend(token);
+    for (uint16_t i = 0; i < 512; i++)
+      spiSend(buf[i]);
+  }
+
+#endif // SOFTWARE_SPI || FORCE_SOFT_SPI
+
+#endif // __AVR__
diff --git a/Marlin/src/HAL/AVR/MarlinSerial.cpp b/Marlin/src/HAL/AVR/MarlinSerial.cpp
new file mode 100644
index 0000000..265acfa
--- /dev/null
+++ b/Marlin/src/HAL/AVR/MarlinSerial.cpp
@@ -0,0 +1,635 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * MarlinSerial.cpp - Hardware serial library for Wiring
+ * Copyright (c) 2006 Nicholas Zambetti.  All right reserved.
+ *
+ * Modified 23 November 2006 by David A. Mellis
+ * Modified 28 September 2010 by Mark Sproul
+ * Modified 14 February 2016 by Andreas Hardtung (added tx buffer)
+ * Modified 01 October 2017 by Eduardo José Tagle (added XON/XOFF)
+ * Modified 10 June 2018 by Eduardo José Tagle (See #10991)
+ * Templatized 01 October 2018 by Eduardo José Tagle to allow multiple instances
+ */
+
+#ifdef __AVR__
+
+// Disable HardwareSerial.cpp to support chips without a UART (Attiny, etc.)
+
+#include "../../inc/MarlinConfig.h"
+
+#if !defined(USBCON) && (defined(UBRRH) || defined(UBRR0H) || defined(UBRR1H) || defined(UBRR2H) || defined(UBRR3H))
+
+#include "MarlinSerial.h"
+#include "../../MarlinCore.h"
+
+#if ENABLED(DIRECT_STEPPING)
+  #include "../../feature/direct_stepping.h"
+#endif
+
+template<typename Cfg> typename MarlinSerial<Cfg>::ring_buffer_r MarlinSerial<Cfg>::rx_buffer = { 0, 0, { 0 } };
+template<typename Cfg> typename MarlinSerial<Cfg>::ring_buffer_t MarlinSerial<Cfg>::tx_buffer = { 0 };
+template<typename Cfg> bool     MarlinSerial<Cfg>::_written = false;
+template<typename Cfg> uint8_t  MarlinSerial<Cfg>::xon_xoff_state = MarlinSerial<Cfg>::XON_XOFF_CHAR_SENT | MarlinSerial<Cfg>::XON_CHAR;
+template<typename Cfg> uint8_t  MarlinSerial<Cfg>::rx_dropped_bytes = 0;
+template<typename Cfg> uint8_t  MarlinSerial<Cfg>::rx_buffer_overruns = 0;
+template<typename Cfg> uint8_t  MarlinSerial<Cfg>::rx_framing_errors = 0;
+template<typename Cfg> typename MarlinSerial<Cfg>::ring_buffer_pos_t MarlinSerial<Cfg>::rx_max_enqueued = 0;
+
+// A SW memory barrier, to ensure GCC does not overoptimize loops
+#define sw_barrier() asm volatile("": : :"memory");
+
+#include "../../feature/e_parser.h"
+
+// "Atomically" read the RX head index value without disabling interrupts:
+// This MUST be called with RX interrupts enabled, and CAN'T be called
+// from the RX ISR itself!
+template<typename Cfg>
+FORCE_INLINE typename MarlinSerial<Cfg>::ring_buffer_pos_t MarlinSerial<Cfg>::atomic_read_rx_head() {
+  if (Cfg::RX_SIZE > 256) {
+    // Keep reading until 2 consecutive reads return the same value,
+    // meaning there was no update in-between caused by an interrupt.
+    // This works because serial RX interrupts happen at a slower rate
+    // than successive reads of a variable, so 2 consecutive reads with
+    // the same value means no interrupt updated it.
+    ring_buffer_pos_t vold, vnew = rx_buffer.head;
+    sw_barrier();
+    do {
+      vold = vnew;
+      vnew = rx_buffer.head;
+      sw_barrier();
+    } while (vold != vnew);
+    return vnew;
+  }
+  else {
+    // With an 8bit index, reads are always atomic. No need for special handling
+    return rx_buffer.head;
+  }
+}
+
+template<typename Cfg>
+volatile bool MarlinSerial<Cfg>::rx_tail_value_not_stable = false;
+template<typename Cfg>
+volatile uint16_t MarlinSerial<Cfg>::rx_tail_value_backup = 0;
+
+// Set RX tail index, taking into account the RX ISR could interrupt
+//  the write to this variable in the middle - So a backup strategy
+//  is used to ensure reads of the correct values.
+//    -Must NOT be called from the RX ISR -
+template<typename Cfg>
+FORCE_INLINE void MarlinSerial<Cfg>::atomic_set_rx_tail(typename MarlinSerial<Cfg>::ring_buffer_pos_t value) {
+  if (Cfg::RX_SIZE > 256) {
+    // Store the new value in the backup
+    rx_tail_value_backup = value;
+    sw_barrier();
+    // Flag we are about to change the true value
+    rx_tail_value_not_stable = true;
+    sw_barrier();
+    // Store the new value
+    rx_buffer.tail = value;
+    sw_barrier();
+    // Signal the new value is completely stored into the value
+    rx_tail_value_not_stable = false;
+    sw_barrier();
+  }
+  else
+    rx_buffer.tail = value;
+}
+
+// Get the RX tail index, taking into account the read could be
+//  interrupting in the middle of the update of that index value
+//    -Called from the RX ISR -
+template<typename Cfg>
+FORCE_INLINE typename MarlinSerial<Cfg>::ring_buffer_pos_t MarlinSerial<Cfg>::atomic_read_rx_tail() {
+  if (Cfg::RX_SIZE > 256) {
+    // If the true index is being modified, return the backup value
+    if (rx_tail_value_not_stable) return rx_tail_value_backup;
+  }
+  // The true index is stable, return it
+  return rx_buffer.tail;
+}
+
+// (called with RX interrupts disabled)
+template<typename Cfg>
+FORCE_INLINE void MarlinSerial<Cfg>::store_rxd_char() {
+
+  static EmergencyParser::State emergency_state; // = EP_RESET
+
+  // This must read the R_UCSRA register before reading the received byte to detect error causes
+  if (Cfg::DROPPED_RX && B_DOR && !++rx_dropped_bytes) --rx_dropped_bytes;
+  if (Cfg::RX_OVERRUNS && B_DOR && !++rx_buffer_overruns) --rx_buffer_overruns;
+  if (Cfg::RX_FRAMING_ERRORS && B_FE && !++rx_framing_errors) --rx_framing_errors;
+
+  // Read the character from the USART
+  uint8_t c = R_UDR;
+
+  #if ENABLED(DIRECT_STEPPING)
+    if (page_manager.maybe_store_rxd_char(c)) return;
+  #endif
+
+  // Get the tail - Nothing can alter its value while this ISR is executing, but there's
+  // a chance that this ISR interrupted the main process while it was updating the index.
+  // The backup mechanism ensures the correct value is always returned.
+  const ring_buffer_pos_t t = atomic_read_rx_tail();
+
+  // Get the head pointer - This ISR is the only one that modifies its value, so it's safe to read here
+  ring_buffer_pos_t h = rx_buffer.head;
+
+  // Get the next element
+  ring_buffer_pos_t i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
+
+  if (Cfg::EMERGENCYPARSER) emergency_parser.update(emergency_state, c);
+
+  // If the character is to be stored at the index just before the tail
+  // (such that the head would advance to the current tail), the RX FIFO is
+  // full, so don't write the character or advance the head.
+  if (i != t) {
+    rx_buffer.buffer[h] = c;
+    h = i;
+  }
+  else if (Cfg::DROPPED_RX && !++rx_dropped_bytes)
+    --rx_dropped_bytes;
+
+  if (Cfg::MAX_RX_QUEUED) {
+    // Calculate count of bytes stored into the RX buffer
+    const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
+
+    // Keep track of the maximum count of enqueued bytes
+    NOLESS(rx_max_enqueued, rx_count);
+  }
+
+  if (Cfg::XONOFF) {
+    // If the last char that was sent was an XON
+    if ((xon_xoff_state & XON_XOFF_CHAR_MASK) == XON_CHAR) {
+
+      // Bytes stored into the RX buffer
+      const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
+
+      // If over 12.5% of RX buffer capacity, send XOFF before running out of
+      // RX buffer space .. 325 bytes @ 250kbits/s needed to let the host react
+      // and stop sending bytes. This translates to 13mS propagation time.
+      if (rx_count >= (Cfg::RX_SIZE) / 8) {
+
+        // At this point, definitely no TX interrupt was executing, since the TX ISR can't be preempted.
+        // Don't enable the TX interrupt here as a means to trigger the XOFF char, because if it happens
+        // to be in the middle of trying to disable the RX interrupt in the main program, eventually the
+        // enabling of the TX interrupt could be undone. The ONLY reliable thing this can do to ensure
+        // the sending of the XOFF char is to send it HERE AND NOW.
+
+        // About to send the XOFF char
+        xon_xoff_state = XOFF_CHAR | XON_XOFF_CHAR_SENT;
+
+        // Wait until the TX register becomes empty and send it - Here there could be a problem
+        // - While waiting for the TX register to empty, the RX register could receive a new
+        //   character. This must also handle that situation!
+        while (!B_UDRE) {
+
+          if (B_RXC) {
+            // A char arrived while waiting for the TX buffer to be empty - Receive and process it!
+
+            i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
+
+            // Read the character from the USART
+            c = R_UDR;
+
+            if (Cfg::EMERGENCYPARSER) emergency_parser.update(emergency_state, c);
+
+            // If the character is to be stored at the index just before the tail
+            // (such that the head would advance to the current tail), the FIFO is
+            // full, so don't write the character or advance the head.
+            if (i != t) {
+              rx_buffer.buffer[h] = c;
+              h = i;
+            }
+            else if (Cfg::DROPPED_RX && !++rx_dropped_bytes)
+              --rx_dropped_bytes;
+          }
+          sw_barrier();
+        }
+
+        R_UDR = XOFF_CHAR;
+
+        // Clear the TXC bit -- "can be cleared by writing a one to its bit
+        // location". This makes sure flush() won't return until the bytes
+        // actually got written
+        B_TXC = 1;
+
+        // At this point there could be a race condition between the write() function
+        // and this sending of the XOFF char. This interrupt could happen between the
+        // wait to be empty TX buffer loop and the actual write of the character. Since
+        // the TX buffer is full because it's sending the XOFF char, the only way to be
+        // sure the write() function will succeed is to wait for the XOFF char to be
+        // completely sent. Since an extra character could be received during the wait
+        // it must also be handled!
+        while (!B_UDRE) {
+
+          if (B_RXC) {
+            // A char arrived while waiting for the TX buffer to be empty - Receive and process it!
+
+            i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
+
+            // Read the character from the USART
+            c = R_UDR;
+
+            if (Cfg::EMERGENCYPARSER)
+              emergency_parser.update(emergency_state, c);
+
+            // If the character is to be stored at the index just before the tail
+            // (such that the head would advance to the current tail), the FIFO is
+            // full, so don't write the character or advance the head.
+            if (i != t) {
+              rx_buffer.buffer[h] = c;
+              h = i;
+            }
+            else if (Cfg::DROPPED_RX && !++rx_dropped_bytes)
+              --rx_dropped_bytes;
+          }
+          sw_barrier();
+        }
+
+        // At this point everything is ready. The write() function won't
+        // have any issues writing to the UART TX register if it needs to!
+      }
+    }
+  }
+
+  // Store the new head value - The main loop will retry until the value is stable
+  rx_buffer.head = h;
+}
+
+// (called with TX irqs disabled)
+template<typename Cfg>
+FORCE_INLINE void MarlinSerial<Cfg>::_tx_udr_empty_irq() {
+  if (Cfg::TX_SIZE > 0) {
+    // Read positions
+    uint8_t t = tx_buffer.tail;
+    const uint8_t h = tx_buffer.head;
+
+    if (Cfg::XONOFF) {
+      // If an XON char is pending to be sent, do it now
+      if (xon_xoff_state == XON_CHAR) {
+
+        // Send the character
+        R_UDR = XON_CHAR;
+
+        // clear the TXC bit -- "can be cleared by writing a one to its bit
+        // location". This makes sure flush() won't return until the bytes
+        // actually got written
+        B_TXC = 1;
+
+        // Remember we sent it.
+        xon_xoff_state = XON_CHAR | XON_XOFF_CHAR_SENT;
+
+        // If nothing else to transmit, just disable TX interrupts.
+        if (h == t) B_UDRIE = 0; // (Non-atomic, could be reenabled by the main program, but eventually this will succeed)
+
+        return;
+      }
+    }
+
+    // If nothing to transmit, just disable TX interrupts. This could
+    // happen as the result of the non atomicity of the disabling of RX
+    // interrupts that could end reenabling TX interrupts as a side effect.
+    if (h == t) {
+      B_UDRIE = 0; // (Non-atomic, could be reenabled by the main program, but eventually this will succeed)
+      return;
+    }
+
+    // There is something to TX, Send the next byte
+    const uint8_t c = tx_buffer.buffer[t];
+    t = (t + 1) & (Cfg::TX_SIZE - 1);
+    R_UDR = c;
+    tx_buffer.tail = t;
+
+    // Clear the TXC bit (by writing a one to its bit location).
+    // Ensures flush() won't return until the bytes are actually written/
+    B_TXC = 1;
+
+    // Disable interrupts if there is nothing to transmit following this byte
+    if (h == t) B_UDRIE = 0; // (Non-atomic, could be reenabled by the main program, but eventually this will succeed)
+  }
+}
+
+// Public Methods
+template<typename Cfg>
+void MarlinSerial<Cfg>::begin(const long baud) {
+  uint16_t baud_setting;
+  bool useU2X = true;
+
+  #if F_CPU == 16000000UL && SERIAL_PORT == 0
+    // Hard-coded exception for compatibility with the bootloader shipped
+    // with the Duemilanove and previous boards, and the firmware on the
+    // 8U2 on the Uno and Mega 2560.
+    if (baud == 57600) useU2X = false;
+  #endif
+
+  R_UCSRA = 0;
+  if (useU2X) {
+    B_U2X = 1;
+    baud_setting = (F_CPU / 4 / baud - 1) / 2;
+  }
+  else
+    baud_setting = (F_CPU / 8 / baud - 1) / 2;
+
+  // assign the baud_setting, a.k.a. ubbr (USART Baud Rate Register)
+  R_UBRRH = baud_setting >> 8;
+  R_UBRRL = baud_setting;
+
+  B_RXEN = 1;
+  B_TXEN = 1;
+  B_RXCIE = 1;
+  if (Cfg::TX_SIZE > 0) B_UDRIE = 0;
+  _written = false;
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::end() {
+  B_RXEN = 0;
+  B_TXEN = 0;
+  B_RXCIE = 0;
+  B_UDRIE = 0;
+}
+
+template<typename Cfg>
+int MarlinSerial<Cfg>::peek() {
+  const ring_buffer_pos_t h = atomic_read_rx_head(), t = rx_buffer.tail;
+  return h == t ? -1 : rx_buffer.buffer[t];
+}
+
+template<typename Cfg>
+int MarlinSerial<Cfg>::read() {
+  const ring_buffer_pos_t h = atomic_read_rx_head();
+
+  // Read the tail. Main thread owns it, so it is safe to directly read it
+  ring_buffer_pos_t t = rx_buffer.tail;
+
+  // If nothing to read, return now
+  if (h == t) return -1;
+
+  // Get the next char
+  const int v = rx_buffer.buffer[t];
+  t = (ring_buffer_pos_t)(t + 1) & (Cfg::RX_SIZE - 1);
+
+  // Advance tail - Making sure the RX ISR will always get an stable value, even
+  // if it interrupts the writing of the value of that variable in the middle.
+  atomic_set_rx_tail(t);
+
+  if (Cfg::XONOFF) {
+    // If the XOFF char was sent, or about to be sent...
+    if ((xon_xoff_state & XON_XOFF_CHAR_MASK) == XOFF_CHAR) {
+      // Get count of bytes in the RX buffer
+      const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
+      if (rx_count < (Cfg::RX_SIZE) / 10) {
+        if (Cfg::TX_SIZE > 0) {
+          // Signal we want an XON character to be sent.
+          xon_xoff_state = XON_CHAR;
+          // Enable TX ISR. Non atomic, but it will eventually enable them
+          B_UDRIE = 1;
+        }
+        else {
+          // If not using TX interrupts, we must send the XON char now
+          xon_xoff_state = XON_CHAR | XON_XOFF_CHAR_SENT;
+          while (!B_UDRE) sw_barrier();
+          R_UDR = XON_CHAR;
+        }
+      }
+    }
+  }
+
+  return v;
+}
+
+template<typename Cfg>
+typename MarlinSerial<Cfg>::ring_buffer_pos_t MarlinSerial<Cfg>::available() {
+  const ring_buffer_pos_t h = atomic_read_rx_head(), t = rx_buffer.tail;
+  return (ring_buffer_pos_t)(Cfg::RX_SIZE + h - t) & (Cfg::RX_SIZE - 1);
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::flush() {
+
+  // Set the tail to the head:
+  //  - Read the RX head index in a safe way. (See atomic_read_rx_head.)
+  //  - Set the tail, making sure the RX ISR will always get a stable value, even
+  //    if it interrupts the writing of the value of that variable in the middle.
+  atomic_set_rx_tail(atomic_read_rx_head());
+
+  if (Cfg::XONOFF) {
+    // If the XOFF char was sent, or about to be sent...
+    if ((xon_xoff_state & XON_XOFF_CHAR_MASK) == XOFF_CHAR) {
+      if (Cfg::TX_SIZE > 0) {
+        // Signal we want an XON character to be sent.
+        xon_xoff_state = XON_CHAR;
+        // Enable TX ISR. Non atomic, but it will eventually enable it.
+        B_UDRIE = 1;
+      }
+      else {
+        // If not using TX interrupts, we must send the XON char now
+        xon_xoff_state = XON_CHAR | XON_XOFF_CHAR_SENT;
+        while (!B_UDRE) sw_barrier();
+        R_UDR = XON_CHAR;
+      }
+    }
+  }
+}
+
+template<typename Cfg>
+size_t MarlinSerial<Cfg>::write(const uint8_t c) {
+  if (Cfg::TX_SIZE == 0) {
+
+    _written = true;
+    while (!B_UDRE) sw_barrier();
+    R_UDR = c;
+
+  }
+  else {
+
+    _written = true;
+
+    // If the TX interrupts are disabled and the data register
+    // is empty, just write the byte to the data register and
+    // be done. This shortcut helps significantly improve the
+    // effective datarate at high (>500kbit/s) bitrates, where
+    // interrupt overhead becomes a slowdown.
+    // Yes, there is a race condition between the sending of the
+    // XOFF char at the RX ISR, but it is properly handled there
+    if (!B_UDRIE && B_UDRE) {
+      R_UDR = c;
+
+      // clear the TXC bit -- "can be cleared by writing a one to its bit
+      // location". This makes sure flush() won't return until the bytes
+      // actually got written
+      B_TXC = 1;
+      return 1;
+    }
+
+    const uint8_t i = (tx_buffer.head + 1) & (Cfg::TX_SIZE - 1);
+
+    // If global interrupts are disabled (as the result of being called from an ISR)...
+    if (!ISRS_ENABLED()) {
+
+      // Make room by polling if it is possible to transmit, and do so!
+      while (i == tx_buffer.tail) {
+
+        // If we can transmit another byte, do it.
+        if (B_UDRE) _tx_udr_empty_irq();
+
+        // Make sure compiler rereads tx_buffer.tail
+        sw_barrier();
+      }
+    }
+    else {
+      // Interrupts are enabled, just wait until there is space
+      while (i == tx_buffer.tail) sw_barrier();
+    }
+
+    // Store new char. head is always safe to move
+    tx_buffer.buffer[tx_buffer.head] = c;
+    tx_buffer.head = i;
+
+    // Enable TX ISR - Non atomic, but it will eventually enable TX ISR
+    B_UDRIE = 1;
+  }
+  return 1;
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::flushTX() {
+
+  if (Cfg::TX_SIZE == 0) {
+    // No bytes written, no need to flush. This special case is needed since there's
+    // no way to force the TXC (transmit complete) bit to 1 during initialization.
+    if (!_written) return;
+
+    // Wait until everything was transmitted
+    while (!B_TXC) sw_barrier();
+
+    // At this point nothing is queued anymore (DRIE is disabled) and
+    // the hardware finished transmission (TXC is set).
+
+  }
+  else {
+
+    // No bytes written, no need to flush. This special case is needed since there's
+    // no way to force the TXC (transmit complete) bit to 1 during initialization.
+    if (!_written) return;
+
+    // If global interrupts are disabled (as the result of being called from an ISR)...
+    if (!ISRS_ENABLED()) {
+
+      // Wait until everything was transmitted - We must do polling, as interrupts are disabled
+      while (tx_buffer.head != tx_buffer.tail || !B_TXC) {
+
+        // If there is more space, send an extra character
+        if (B_UDRE) _tx_udr_empty_irq();
+
+        sw_barrier();
+      }
+
+    }
+    else {
+      // Wait until everything was transmitted
+      while (tx_buffer.head != tx_buffer.tail || !B_TXC) sw_barrier();
+    }
+
+    // At this point nothing is queued anymore (DRIE is disabled) and
+    // the hardware finished transmission (TXC is set).
+  }
+}
+
+// Hookup ISR handlers
+ISR(SERIAL_REGNAME(USART, SERIAL_PORT, _RX_vect)) {
+  MarlinSerial<MarlinSerialCfg<SERIAL_PORT>>::store_rxd_char();
+}
+
+ISR(SERIAL_REGNAME(USART, SERIAL_PORT, _UDRE_vect)) {
+  MarlinSerial<MarlinSerialCfg<SERIAL_PORT>>::_tx_udr_empty_irq();
+}
+
+// Because of the template definition above, it's required to instantiate the template to have all method generated
+template class MarlinSerial< MarlinSerialCfg<SERIAL_PORT> >;
+MSerialT customizedSerial1(MSerialT::HasEmergencyParser);
+
+#ifdef SERIAL_PORT_2
+
+  // Hookup ISR handlers
+  ISR(SERIAL_REGNAME(USART, SERIAL_PORT_2, _RX_vect)) {
+    MarlinSerial<MarlinSerialCfg<SERIAL_PORT_2>>::store_rxd_char();
+  }
+
+  ISR(SERIAL_REGNAME(USART, SERIAL_PORT_2, _UDRE_vect)) {
+    MarlinSerial<MarlinSerialCfg<SERIAL_PORT_2>>::_tx_udr_empty_irq();
+  }
+
+  template class MarlinSerial< MarlinSerialCfg<SERIAL_PORT_2> >;
+  MSerialT2 customizedSerial2(MSerialT2::HasEmergencyParser);
+#endif
+
+#ifdef MMU2_SERIAL_PORT
+
+  ISR(SERIAL_REGNAME(USART, MMU2_SERIAL_PORT, _RX_vect)) {
+    MarlinSerial<MMU2SerialCfg<MMU2_SERIAL_PORT>>::store_rxd_char();
+  }
+
+  ISR(SERIAL_REGNAME(USART, MMU2_SERIAL_PORT, _UDRE_vect)) {
+    MarlinSerial<MMU2SerialCfg<MMU2_SERIAL_PORT>>::_tx_udr_empty_irq();
+  }
+
+  template class MarlinSerial< MarlinSerialCfg<MMU2_SERIAL_PORT> >;
+  MSerialT3 mmuSerial(MSerialT3::HasEmergencyParser);
+#endif
+
+#ifdef LCD_SERIAL_PORT
+
+  ISR(SERIAL_REGNAME(USART, LCD_SERIAL_PORT, _RX_vect)) {
+    MarlinSerial<LCDSerialCfg<LCD_SERIAL_PORT>>::store_rxd_char();
+  }
+
+  ISR(SERIAL_REGNAME(USART, LCD_SERIAL_PORT, _UDRE_vect)) {
+    MarlinSerial<LCDSerialCfg<LCD_SERIAL_PORT>>::_tx_udr_empty_irq();
+  }
+
+  template class MarlinSerial< LCDSerialCfg<LCD_SERIAL_PORT> >;
+  MSerialT4 lcdSerial(MSerialT4::HasEmergencyParser);
+
+  #if HAS_DGUS_LCD
+    template<typename Cfg>
+    typename MarlinSerial<Cfg>::ring_buffer_pos_t MarlinSerial<Cfg>::get_tx_buffer_free() {
+      const ring_buffer_pos_t t = tx_buffer.tail,  // next byte to send.
+                              h = tx_buffer.head;  // next pos for queue.
+      int ret = t - h - 1;
+      if (ret < 0) ret += Cfg::TX_SIZE + 1;
+      return ret;
+    }
+  #endif
+
+#endif
+
+#endif // !USBCON && (UBRRH || UBRR0H || UBRR1H || UBRR2H || UBRR3H)
+
+// For AT90USB targets use the UART for BT interfacing
+#if defined(USBCON) && ENABLED(BLUETOOTH)
+  MSerialT5 bluetoothSerial(false);
+#endif
+
+#endif // __AVR__
diff --git a/Marlin/src/HAL/AVR/MarlinSerial.h b/Marlin/src/HAL/AVR/MarlinSerial.h
new file mode 100644
index 0000000..2834dbe
--- /dev/null
+++ b/Marlin/src/HAL/AVR/MarlinSerial.h
@@ -0,0 +1,303 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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
+
+/**
+ * MarlinSerial.h - Hardware serial library for Wiring
+ * Copyright (c) 2006 Nicholas Zambetti.  All right reserved.
+ *
+ * Modified 28 September 2010 by Mark Sproul
+ * Modified 14 February 2016 by Andreas Hardtung (added tx buffer)
+ * Modified 01 October 2017 by Eduardo José Tagle (added XON/XOFF)
+ * Templatized 01 October 2018 by Eduardo José Tagle to allow multiple instances
+ */
+
+#include <WString.h>
+
+#include "../../inc/MarlinConfigPre.h"
+#include "../../core/serial_hook.h"
+
+#ifndef SERIAL_PORT
+  #define SERIAL_PORT 0
+#endif
+
+#ifndef USBCON
+
+  // The presence of the UBRRH register is used to detect a UART.
+  #define UART_PRESENT(port) ((port == 0 && (defined(UBRRH) || defined(UBRR0H))) || \
+                              (port == 1 && defined(UBRR1H)) || (port == 2 && defined(UBRR2H)) || \
+                              (port == 3 && defined(UBRR3H)))
+
+  // These are macros to build serial port register names for the selected SERIAL_PORT (C preprocessor
+  // requires two levels of indirection to expand macro values properly)
+  #define SERIAL_REGNAME(registerbase,number,suffix) _SERIAL_REGNAME(registerbase,number,suffix)
+  #if SERIAL_PORT == 0 && (!defined(UBRR0H) || !defined(UDR0)) // use un-numbered registers if necessary
+    #define _SERIAL_REGNAME(registerbase,number,suffix) registerbase##suffix
+  #else
+    #define _SERIAL_REGNAME(registerbase,number,suffix) registerbase##number##suffix
+  #endif
+
+  // Registers used by MarlinSerial class (expanded depending on selected serial port)
+
+  // Templated 8bit register (generic)
+  #define UART_REGISTER_DECL_BASE(registerbase, suffix) \
+    template<int portNr> struct R_##registerbase##x##suffix {}
+
+  // Templated 8bit register (specialization for each port)
+  #define UART_REGISTER_DECL(port, registerbase, suffix) \
+    template<> struct R_##registerbase##x##suffix<port> { \
+      constexpr R_##registerbase##x##suffix(int) {} \
+      FORCE_INLINE void operator=(uint8_t newVal) const { SERIAL_REGNAME(registerbase,port,suffix) = newVal; } \
+      FORCE_INLINE operator uint8_t() const { return SERIAL_REGNAME(registerbase,port,suffix); } \
+    }
+
+  // Templated 1bit register (generic)
+  #define UART_BIT_DECL_BASE(registerbase, suffix, bit) \
+    template<int portNr>struct B_##bit##x {}
+
+  // Templated 1bit register (specialization for each port)
+  #define UART_BIT_DECL(port, registerbase, suffix, bit) \
+    template<> struct B_##bit##x<port> { \
+      constexpr B_##bit##x(int) {} \
+      FORCE_INLINE void operator=(int newVal) const { \
+        if (newVal) \
+          SBI(SERIAL_REGNAME(registerbase,port,suffix),SERIAL_REGNAME(bit,port,)); \
+        else \
+          CBI(SERIAL_REGNAME(registerbase,port,suffix),SERIAL_REGNAME(bit,port,)); \
+      } \
+      FORCE_INLINE operator bool() const { return TEST(SERIAL_REGNAME(registerbase,port,suffix),SERIAL_REGNAME(bit,port,)); } \
+    }
+
+  #define UART_DECL_BASE() \
+    UART_REGISTER_DECL_BASE(UCSR,A);\
+    UART_REGISTER_DECL_BASE(UDR,);\
+    UART_REGISTER_DECL_BASE(UBRR,H);\
+    UART_REGISTER_DECL_BASE(UBRR,L);\
+    UART_BIT_DECL_BASE(UCSR,B,RXEN);\
+    UART_BIT_DECL_BASE(UCSR,B,TXEN);\
+    UART_BIT_DECL_BASE(UCSR,A,TXC);\
+    UART_BIT_DECL_BASE(UCSR,B,RXCIE);\
+    UART_BIT_DECL_BASE(UCSR,A,UDRE);\
+    UART_BIT_DECL_BASE(UCSR,A,FE);\
+    UART_BIT_DECL_BASE(UCSR,A,DOR);\
+    UART_BIT_DECL_BASE(UCSR,B,UDRIE);\
+    UART_BIT_DECL_BASE(UCSR,A,RXC);\
+    UART_BIT_DECL_BASE(UCSR,A,U2X)
+
+  #define UART_DECL(port) \
+    UART_REGISTER_DECL(port,UCSR,A);\
+    UART_REGISTER_DECL(port,UDR,);\
+    UART_REGISTER_DECL(port,UBRR,H);\
+    UART_REGISTER_DECL(port,UBRR,L);\
+    UART_BIT_DECL(port,UCSR,B,RXEN);\
+    UART_BIT_DECL(port,UCSR,B,TXEN);\
+    UART_BIT_DECL(port,UCSR,A,TXC);\
+    UART_BIT_DECL(port,UCSR,B,RXCIE);\
+    UART_BIT_DECL(port,UCSR,A,UDRE);\
+    UART_BIT_DECL(port,UCSR,A,FE);\
+    UART_BIT_DECL(port,UCSR,A,DOR);\
+    UART_BIT_DECL(port,UCSR,B,UDRIE);\
+    UART_BIT_DECL(port,UCSR,A,RXC);\
+    UART_BIT_DECL(port,UCSR,A,U2X)
+
+  // Declare empty templates
+  UART_DECL_BASE();
+
+  // And all the specializations for each possible serial port
+  #if UART_PRESENT(0)
+    UART_DECL(0);
+  #endif
+  #if UART_PRESENT(1)
+    UART_DECL(1);
+  #endif
+  #if UART_PRESENT(2)
+    UART_DECL(2);
+  #endif
+  #if UART_PRESENT(3)
+    UART_DECL(3);
+  #endif
+
+  #define BYTE 0
+
+  // Templated type selector
+  template<bool b, typename T, typename F> struct TypeSelector { typedef T type;} ;
+  template<typename T, typename F> struct TypeSelector<false, T, F> { typedef F type; };
+
+  template<typename Cfg>
+  class MarlinSerial {
+  protected:
+    // Registers
+    static constexpr R_UCSRxA<Cfg::PORT> R_UCSRA = 0;
+    static constexpr R_UDRx<Cfg::PORT>   R_UDR   = 0;
+    static constexpr R_UBRRxH<Cfg::PORT> R_UBRRH = 0;
+    static constexpr R_UBRRxL<Cfg::PORT> R_UBRRL = 0;
+
+    // Bits
+    static constexpr B_RXENx<Cfg::PORT>  B_RXEN  = 0;
+    static constexpr B_TXENx<Cfg::PORT>  B_TXEN  = 0;
+    static constexpr B_TXCx<Cfg::PORT>   B_TXC   = 0;
+    static constexpr B_RXCIEx<Cfg::PORT> B_RXCIE = 0;
+    static constexpr B_UDREx<Cfg::PORT>  B_UDRE  = 0;
+    static constexpr B_FEx<Cfg::PORT>    B_FE    = 0;
+    static constexpr B_DORx<Cfg::PORT>   B_DOR   = 0;
+    static constexpr B_UDRIEx<Cfg::PORT> B_UDRIE = 0;
+    static constexpr B_RXCx<Cfg::PORT>   B_RXC   = 0;
+    static constexpr B_U2Xx<Cfg::PORT>   B_U2X   = 0;
+
+    // Base size of type on buffer size
+    typedef typename TypeSelector<(Cfg::RX_SIZE>256), uint16_t, uint8_t>::type ring_buffer_pos_t;
+
+    struct ring_buffer_r {
+      volatile ring_buffer_pos_t head, tail;
+      unsigned char buffer[Cfg::RX_SIZE];
+    };
+
+    struct ring_buffer_t {
+      volatile uint8_t head, tail;
+      unsigned char buffer[Cfg::TX_SIZE];
+    };
+
+    static ring_buffer_r rx_buffer;
+    static ring_buffer_t tx_buffer;
+    static bool _written;
+
+    static constexpr uint8_t XON_XOFF_CHAR_SENT = 0x80,  // XON / XOFF Character was sent
+                             XON_XOFF_CHAR_MASK = 0x1F;  // XON / XOFF character to send
+
+    // XON / XOFF character definitions
+    static constexpr uint8_t XON_CHAR  = 17, XOFF_CHAR = 19;
+    static uint8_t xon_xoff_state,
+                   rx_dropped_bytes,
+                   rx_buffer_overruns,
+                   rx_framing_errors;
+    static ring_buffer_pos_t rx_max_enqueued;
+
+    static FORCE_INLINE ring_buffer_pos_t atomic_read_rx_head();
+
+    static volatile bool rx_tail_value_not_stable;
+    static volatile uint16_t rx_tail_value_backup;
+
+    static FORCE_INLINE void atomic_set_rx_tail(ring_buffer_pos_t value);
+    static FORCE_INLINE ring_buffer_pos_t atomic_read_rx_tail();
+
+  public:
+    FORCE_INLINE static void store_rxd_char();
+    FORCE_INLINE static void _tx_udr_empty_irq();
+
+  public:
+    static void begin(const long);
+    static void end();
+    static int peek();
+    static int read();
+    static void flush();
+    static ring_buffer_pos_t available();
+    static size_t write(const uint8_t c);
+    static void flushTX();
+    #if HAS_DGUS_LCD
+      static ring_buffer_pos_t get_tx_buffer_free();
+    #endif
+
+    enum { HasEmergencyParser = Cfg::EMERGENCYPARSER };
+    static inline bool emergency_parser_enabled() { return Cfg::EMERGENCYPARSER; }
+
+    FORCE_INLINE static uint8_t dropped() { return Cfg::DROPPED_RX ? rx_dropped_bytes : 0; }
+    FORCE_INLINE static uint8_t buffer_overruns() { return Cfg::RX_OVERRUNS ? rx_buffer_overruns : 0; }
+    FORCE_INLINE static uint8_t framing_errors() { return Cfg::RX_FRAMING_ERRORS ? rx_framing_errors : 0; }
+    FORCE_INLINE static ring_buffer_pos_t rxMaxEnqueued() { return Cfg::MAX_RX_QUEUED ? rx_max_enqueued : 0; }
+  };
+
+  template <uint8_t serial>
+  struct MarlinSerialCfg {
+    static constexpr int PORT               = serial;
+    static constexpr unsigned int RX_SIZE   = RX_BUFFER_SIZE;
+    static constexpr unsigned int TX_SIZE   = TX_BUFFER_SIZE;
+    static constexpr bool XONOFF            = ENABLED(SERIAL_XON_XOFF);
+    static constexpr bool EMERGENCYPARSER   = ENABLED(EMERGENCY_PARSER);
+    static constexpr bool DROPPED_RX        = ENABLED(SERIAL_STATS_DROPPED_RX);
+    static constexpr bool RX_OVERRUNS       = ENABLED(SERIAL_STATS_RX_BUFFER_OVERRUNS);
+    static constexpr bool RX_FRAMING_ERRORS = ENABLED(SERIAL_STATS_RX_FRAMING_ERRORS);
+    static constexpr bool MAX_RX_QUEUED     = ENABLED(SERIAL_STATS_MAX_RX_QUEUED);
+  };
+
+  typedef Serial0Type< MarlinSerial< MarlinSerialCfg<SERIAL_PORT> > > MSerialT;
+  extern MSerialT customizedSerial1;
+
+  #ifdef SERIAL_PORT_2
+    typedef Serial0Type< MarlinSerial< MarlinSerialCfg<SERIAL_PORT_2> > > MSerialT2;
+    extern MSerialT2 customizedSerial2;
+  #endif
+
+#endif // !USBCON
+
+#ifdef MMU2_SERIAL_PORT
+  template <uint8_t serial>
+  struct MMU2SerialCfg {
+    static constexpr int PORT               = serial;
+    static constexpr bool XONOFF            = false;
+    static constexpr bool EMERGENCYPARSER   = false;
+    static constexpr bool DROPPED_RX        = false;
+    static constexpr bool RX_FRAMING_ERRORS = false;
+    static constexpr bool MAX_RX_QUEUED     = false;
+    static constexpr unsigned int RX_SIZE   = 32;
+    static constexpr unsigned int TX_SIZE   = 32;
+    static constexpr bool RX_OVERRUNS       = false;
+  };
+
+  typedef Serial0Type< MarlinSerial< MMU2SerialCfg<MMU2_SERIAL_PORT> > > MSerialT3;
+  extern MSerial3 mmuSerial;
+#endif
+
+#ifdef LCD_SERIAL_PORT
+
+  template <uint8_t serial>
+  struct LCDSerialCfg {
+    static constexpr int PORT                 = serial;
+    static constexpr bool XONOFF              = false;
+    static constexpr bool EMERGENCYPARSER     = ENABLED(EMERGENCY_PARSER);
+    static constexpr bool DROPPED_RX          = false;
+    static constexpr bool RX_FRAMING_ERRORS   = false;
+    static constexpr bool MAX_RX_QUEUED       = false;
+    #if HAS_DGUS_LCD
+      static constexpr unsigned int RX_SIZE   = DGUS_RX_BUFFER_SIZE;
+      static constexpr unsigned int TX_SIZE   = DGUS_TX_BUFFER_SIZE;
+      static constexpr bool RX_OVERRUNS       = ENABLED(SERIAL_STATS_RX_BUFFER_OVERRUNS);
+    #elif EITHER(ANYCUBIC_LCD_I3MEGA, ANYCUBIC_LCD_CHIRON)
+      static constexpr unsigned int RX_SIZE   = 64;
+      static constexpr unsigned int TX_SIZE   = 128;
+      static constexpr bool RX_OVERRUNS       = false;
+    #else
+      static constexpr unsigned int RX_SIZE   = 64;
+      static constexpr unsigned int TX_SIZE   = 128;
+      static constexpr bool RX_OVERRUNS       = false
+    #endif
+  };
+
+
+  typedef Serial0Type< MarlinSerial< LCDSerialCfg<LCD_SERIAL_PORT> > > MSerialT4;
+  extern MSerialT4 lcdSerial;
+#endif
+
+// Use the UART for Bluetooth in AT90USB configurations
+#if defined(USBCON) && ENABLED(BLUETOOTH)
+  typedef Serial0Type<HardwareSerial> MSerialT5;
+  extern MSerialT5 bluetoothSerial;
+#endif
diff --git a/Marlin/src/HAL/AVR/Servo.cpp b/Marlin/src/HAL/AVR/Servo.cpp
new file mode 100644
index 0000000..526352b
--- /dev/null
+++ b/Marlin/src/HAL/AVR/Servo.cpp
@@ -0,0 +1,216 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
+ * Copyright (c) 2009 Michael Margolis.  All right reserved.
+ */
+
+/**
+ * A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
+ * The servos are pulsed in the background using the value most recently written using the write() method
+ *
+ * Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached.
+ * Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four.
+ *
+ * The methods are:
+ *
+ * Servo - Class for manipulating servo motors connected to Arduino pins.
+ *
+ * attach(pin)           - Attach a servo motor to an i/o pin.
+ * attach(pin, min, max) - Attach to a pin, setting min and max values in microseconds
+ *                         Default min is 544, max is 2400
+ *
+ * write()               - Set the servo angle in degrees. (Invalid angles —over MIN_PULSE_WIDTH— are treated as µs.)
+ * writeMicroseconds()   - Set the servo pulse width in microseconds.
+ * move(pin, angle)      - Sequence of attach(pin), write(angle), safe_delay(servo_delay[servoIndex]).
+ *                         With DEACTIVATE_SERVOS_AFTER_MOVE it detaches after servo_delay[servoIndex].
+ * read()                - Get the last-written servo pulse width as an angle between 0 and 180.
+ * readMicroseconds()    - Get the last-written servo pulse width in microseconds.
+ * attached()            - Return true if a servo is attached.
+ * detach()              - Stop an attached servo from pulsing its i/o pin.
+ */
+
+#ifdef __AVR__
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_SERVOS
+
+#include <avr/interrupt.h>
+
+#include "../shared/servo.h"
+#include "../shared/servo_private.h"
+
+static volatile int8_t Channel[_Nbr_16timers];              // counter for the servo being pulsed for each timer (or -1 if refresh interval)
+
+
+/************ static functions common to all instances ***********************/
+
+static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) {
+  if (Channel[timer] < 0)
+    *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
+  else {
+    if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && SERVO(timer, Channel[timer]).Pin.isActive)
+      extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
+  }
+
+  Channel[timer]++;    // increment to the next channel
+  if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
+    *OCRnA = *TCNTn + SERVO(timer, Channel[timer]).ticks;
+    if (SERVO(timer, Channel[timer]).Pin.isActive)    // check if activated
+      extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high
+  }
+  else {
+    // finished all channels so wait for the refresh period to expire before starting over
+    if (((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL))    // allow a few ticks to ensure the next OCR1A not missed
+      *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
+    else
+      *OCRnA = *TCNTn + 4;  // at least REFRESH_INTERVAL has elapsed
+    Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
+  }
+}
+
+#ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
+
+  // Interrupt handlers for Arduino
+  #ifdef _useTimer1
+    SIGNAL(TIMER1_COMPA_vect) { handle_interrupts(_timer1, &TCNT1, &OCR1A); }
+  #endif
+
+  #ifdef _useTimer3
+    SIGNAL(TIMER3_COMPA_vect) { handle_interrupts(_timer3, &TCNT3, &OCR3A); }
+  #endif
+
+  #ifdef _useTimer4
+    SIGNAL(TIMER4_COMPA_vect) { handle_interrupts(_timer4, &TCNT4, &OCR4A); }
+  #endif
+
+  #ifdef _useTimer5
+    SIGNAL(TIMER5_COMPA_vect) { handle_interrupts(_timer5, &TCNT5, &OCR5A); }
+  #endif
+
+#else // WIRING
+
+  // Interrupt handlers for Wiring
+  #ifdef _useTimer1
+    void Timer1Service() { handle_interrupts(_timer1, &TCNT1, &OCR1A); }
+  #endif
+  #ifdef _useTimer3
+    void Timer3Service() { handle_interrupts(_timer3, &TCNT3, &OCR3A); }
+  #endif
+
+#endif // WIRING
+
+/****************** end of static functions ******************************/
+
+void initISR(timer16_Sequence_t timer) {
+  #ifdef _useTimer1
+    if (timer == _timer1) {
+      TCCR1A = 0;             // normal counting mode
+      TCCR1B = _BV(CS11);     // set prescaler of 8
+      TCNT1 = 0;              // clear the timer count
+      #if defined(__AVR_ATmega8__) || defined(__AVR_ATmega128__)
+        SBI(TIFR, OCF1A);      // clear any pending interrupts;
+        SBI(TIMSK, OCIE1A);    // enable the output compare interrupt
+      #else
+        // here if not ATmega8 or ATmega128
+        SBI(TIFR1, OCF1A);     // clear any pending interrupts;
+        SBI(TIMSK1, OCIE1A);   // enable the output compare interrupt
+      #endif
+      #ifdef WIRING
+        timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
+      #endif
+    }
+  #endif
+
+  #ifdef _useTimer3
+    if (timer == _timer3) {
+      TCCR3A = 0;             // normal counting mode
+      TCCR3B = _BV(CS31);     // set prescaler of 8
+      TCNT3 = 0;              // clear the timer count
+      #ifdef __AVR_ATmega128__
+        SBI(TIFR, OCF3A);     // clear any pending interrupts;
+        SBI(ETIMSK, OCIE3A);  // enable the output compare interrupt
+      #else
+        SBI(TIFR3, OCF3A);   // clear any pending interrupts;
+        SBI(TIMSK3, OCIE3A); // enable the output compare interrupt
+      #endif
+      #ifdef WIRING
+        timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service);  // for Wiring platform only
+      #endif
+    }
+  #endif
+
+  #ifdef _useTimer4
+    if (timer == _timer4) {
+      TCCR4A = 0;             // normal counting mode
+      TCCR4B = _BV(CS41);     // set prescaler of 8
+      TCNT4 = 0;              // clear the timer count
+      TIFR4 = _BV(OCF4A);     // clear any pending interrupts;
+      TIMSK4 = _BV(OCIE4A);   // enable the output compare interrupt
+    }
+  #endif
+
+  #ifdef _useTimer5
+    if (timer == _timer5) {
+      TCCR5A = 0;             // normal counting mode
+      TCCR5B = _BV(CS51);     // set prescaler of 8
+      TCNT5 = 0;              // clear the timer count
+      TIFR5 = _BV(OCF5A);     // clear any pending interrupts;
+      TIMSK5 = _BV(OCIE5A);   // enable the output compare interrupt
+    }
+  #endif
+}
+
+void finISR(timer16_Sequence_t timer) {
+  // Disable use of the given timer
+  #ifdef WIRING
+    if (timer == _timer1) {
+      CBI(
+        #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
+          TIMSK1
+        #else
+          TIMSK
+        #endif
+          , OCIE1A);    // disable timer 1 output compare interrupt
+      timerDetach(TIMER1OUTCOMPAREA_INT);
+    }
+    else if (timer == _timer3) {
+      CBI(
+        #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
+          TIMSK3
+        #else
+          ETIMSK
+        #endif
+          , OCIE3A);    // disable the timer3 output compare A interrupt
+      timerDetach(TIMER3OUTCOMPAREA_INT);
+    }
+  #else // !WIRING
+    // For arduino - in future: call here to a currently undefined function to reset the timer
+    UNUSED(timer);
+  #endif
+}
+
+#endif // HAS_SERVOS
+
+#endif // __AVR__
diff --git a/Marlin/src/HAL/AVR/ServoTimers.h b/Marlin/src/HAL/AVR/ServoTimers.h
new file mode 100644
index 0000000..436b281
--- /dev/null
+++ b/Marlin/src/HAL/AVR/ServoTimers.h
@@ -0,0 +1,93 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * ServoTimers.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
+ * Copyright (c) 2009 Michael Margolis.  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
+ */
+
+/**
+ * Defines for 16 bit timers used with  Servo library
+ *
+ * If _useTimerX is defined then TimerX is a 16 bit timer on the current board
+ * timer16_Sequence_t enumerates the sequence that the timers should be allocated
+ * _Nbr_16timers indicates how many 16 bit timers are available.
+ */
+
+/**
+ * AVR Only definitions
+ * --------------------
+ */
+
+#define TRIM_DURATION           2   // compensation ticks to trim adjust for digitalWrite delays
+#define SERVO_TIMER_PRESCALER   8   // timer prescaler
+
+// Say which 16 bit timers can be used and in what order
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+  //#define _useTimer1
+  #define _useTimer4
+  #if NUM_SERVOS > SERVOS_PER_TIMER
+    #define _useTimer3
+    #if !HAS_MOTOR_CURRENT_PWM && SERVOS > 2 * SERVOS_PER_TIMER
+      #define _useTimer5 // Timer 5 is used for motor current PWM and can't be used for servos.
+    #endif
+  #endif
+#elif defined(__AVR_ATmega32U4__)
+  #define _useTimer3
+#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
+  #define _useTimer3
+#elif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1281__) || defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega2561__)
+  #define _useTimer3
+#else
+  // everything else
+#endif
+
+typedef enum {
+  #ifdef _useTimer1
+    _timer1,
+  #endif
+  #ifdef _useTimer3
+    _timer3,
+  #endif
+  #ifdef _useTimer4
+    _timer4,
+  #endif
+  #ifdef _useTimer5
+    _timer5,
+  #endif
+  _Nbr_16timers
+} timer16_Sequence_t;
diff --git a/Marlin/src/HAL/AVR/eeprom.cpp b/Marlin/src/HAL/AVR/eeprom.cpp
new file mode 100644
index 0000000..ee2a73e
--- /dev/null
+++ b/Marlin/src/HAL/AVR/eeprom.cpp
@@ -0,0 +1,74 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#ifdef __AVR__
+
+#include "../../inc/MarlinConfig.h"
+
+#if EITHER(EEPROM_SETTINGS, SD_FIRMWARE_UPDATE)
+
+/**
+ * PersistentStore for Arduino-style EEPROM interface
+ * with implementations supplied by the framework.
+ */
+
+#include "../shared/eeprom_api.h"
+
+#ifndef MARLIN_EEPROM_SIZE
+  #define MARLIN_EEPROM_SIZE size_t(E2END + 1)
+#endif
+size_t PersistentStore::capacity()    { return MARLIN_EEPROM_SIZE; }
+bool PersistentStore::access_start()  { return true; }
+bool PersistentStore::access_finish() { return true; }
+
+bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) {
+  while (size--) {
+    uint8_t * const p = (uint8_t * const)pos;
+    uint8_t v = *value;
+    // EEPROM has only ~100,000 write cycles,
+    // so only write bytes that have changed!
+    if (v != eeprom_read_byte(p)) {
+      eeprom_write_byte(p, v);
+      if (eeprom_read_byte(p) != v) {
+        SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE);
+        return true;
+      }
+    }
+    crc16(crc, &v, 1);
+    pos++;
+    value++;
+  }
+  return false;
+}
+
+bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) {
+  do {
+    uint8_t c = eeprom_read_byte((uint8_t*)pos);
+    if (writing) *value = c;
+    crc16(crc, &c, 1);
+    pos++;
+    value++;
+  } while (--size);
+  return false;  // always assume success for AVR's
+}
+
+#endif // EEPROM_SETTINGS || SD_FIRMWARE_UPDATE
+#endif // __AVR__
diff --git a/Marlin/src/HAL/AVR/endstop_interrupts.h b/Marlin/src/HAL/AVR/endstop_interrupts.h
new file mode 100644
index 0000000..9fd9c38
--- /dev/null
+++ b/Marlin/src/HAL/AVR/endstop_interrupts.h
@@ -0,0 +1,261 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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
+
+/**
+ * Endstop Interrupts
+ *
+ * Without endstop interrupts the endstop pins must be polled continually in
+ * the temperature-ISR via endstops.update(), most of the time finding no change.
+ * With this feature endstops.update() is called only when we know that at
+ * least one endstop has changed state, saving valuable CPU cycles.
+ *
+ * This feature only works when all used endstop pins can generate either an
+ * 'external interrupt' or a 'pin change interrupt'.
+ *
+ * Test whether pins issue interrupts on your board by flashing 'pin_interrupt_test.ino'.
+ * (Located in Marlin/buildroot/share/pin_interrupt_test/pin_interrupt_test.ino)
+ */
+
+#include "../../module/endstops.h"
+
+#include <stdint.h>
+
+// One ISR for all EXT-Interrupts
+void endstop_ISR() { endstops.update(); }
+
+/**
+ * Patch for pins_arduino.h (...\Arduino\hardware\arduino\avr\variants\mega\pins_arduino.h)
+ *
+ * These macros for the Arduino MEGA do not include the two connected pins on Port J (D14, D15).
+ * So we extend them here because these are the normal pins for Y_MIN and Y_MAX on RAMPS.
+ * There are more PCI-enabled processor pins on Port J, but they are not connected to Arduino MEGA.
+ */
+#if defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_AVR_MEGA)
+
+  #define digitalPinHasPCICR(p)   (WITHIN(p, 10, 15) || WITHIN(p, 50, 53) || WITHIN(p, 62, 69))
+
+  #undef  digitalPinToPCICR
+  #define digitalPinToPCICR(p)    (digitalPinHasPCICR(p) ? (&PCICR) : nullptr)
+
+  #undef  digitalPinToPCICRbit
+  #define digitalPinToPCICRbit(p) (WITHIN(p, 10, 13) || WITHIN(p, 50, 53) ? 0 : \
+                                   WITHIN(p, 14, 15) ? 1 : \
+                                   WITHIN(p, 62, 69) ? 2 : \
+                                   0)
+
+  #undef  digitalPinToPCMSK
+  #define digitalPinToPCMSK(p)    (WITHIN(p, 10, 13) || WITHIN(p, 50, 53) ? (&PCMSK0) : \
+                                   WITHIN(p, 14, 15) ? (&PCMSK1) : \
+                                   WITHIN(p, 62, 69) ? (&PCMSK2) : \
+                                   nullptr)
+
+  #undef  digitalPinToPCMSKbit
+  #define digitalPinToPCMSKbit(p) (WITHIN(p, 10, 13) ? ((p) - 6) : \
+                                   (p) == 14 || (p) == 51 ? 2 : \
+                                   (p) == 15 || (p) == 52 ? 1 : \
+                                   (p) == 50 ? 3 : \
+                                   (p) == 53 ? 0 : \
+                                   WITHIN(p, 62, 69) ? ((p) - 62) : \
+                                   0)
+
+#elif defined(__AVR_ATmega164A__) || defined(__AVR_ATmega164P__) || defined(__AVR_ATmega324A__) || \
+      defined(__AVR_ATmega324P__) || defined(__AVR_ATmega324PA__) || defined(__AVR_ATmega324PB__) || \
+      defined(__AVR_ATmega644A__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega1284__) || \
+      defined(__AVR_ATmega1284P__)
+
+  #define digitalPinHasPCICR(p)   WITHIN(p, 0, NUM_DIGITAL_PINS)
+
+#else
+
+  #error "Unsupported AVR variant!"
+
+#endif
+
+
+// Install Pin change interrupt for a pin. Can be called multiple times.
+void pciSetup(const int8_t pin) {
+  if (digitalPinHasPCICR(pin)) {
+    SBI(*digitalPinToPCMSK(pin), digitalPinToPCMSKbit(pin));  // enable pin
+    SBI(PCIFR, digitalPinToPCICRbit(pin)); // clear any outstanding interrupt
+    SBI(PCICR, digitalPinToPCICRbit(pin)); // enable interrupt for the group
+  }
+}
+
+// Handlers for pin change interrupts
+#ifdef PCINT0_vect
+  ISR(PCINT0_vect) { endstop_ISR(); }
+#endif
+
+#ifdef PCINT1_vect
+  ISR(PCINT1_vect, ISR_ALIASOF(PCINT0_vect));
+#endif
+
+#ifdef PCINT2_vect
+  ISR(PCINT2_vect, ISR_ALIASOF(PCINT0_vect));
+#endif
+
+#ifdef PCINT3_vect
+  ISR(PCINT3_vect, ISR_ALIASOF(PCINT0_vect));
+#endif
+
+void setup_endstop_interrupts() {
+  #define _ATTACH(P) attachInterrupt(digitalPinToInterrupt(P), endstop_ISR, CHANGE)
+  #if HAS_X_MAX
+    #if (digitalPinToInterrupt(X_MAX_PIN) != NOT_AN_INTERRUPT)
+      _ATTACH(X_MAX_PIN);
+    #else
+      static_assert(digitalPinHasPCICR(X_MAX_PIN), "X_MAX_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue.");
+      pciSetup(X_MAX_PIN);
+    #endif
+  #endif
+  #if HAS_X_MIN
+    #if (digitalPinToInterrupt(X_MIN_PIN) != NOT_AN_INTERRUPT)
+      _ATTACH(X_MIN_PIN);
+    #else
+      static_assert(digitalPinHasPCICR(X_MIN_PIN), "X_MIN_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue.");
+      pciSetup(X_MIN_PIN);
+    #endif
+  #endif
+  #if HAS_Y_MAX
+    #if (digitalPinToInterrupt(Y_MAX_PIN) != NOT_AN_INTERRUPT)
+      _ATTACH(Y_MAX_PIN);
+    #else
+      static_assert(digitalPinHasPCICR(Y_MAX_PIN), "Y_MAX_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue.");
+      pciSetup(Y_MAX_PIN);
+    #endif
+  #endif
+  #if HAS_Y_MIN
+    #if (digitalPinToInterrupt(Y_MIN_PIN) != NOT_AN_INTERRUPT)
+      _ATTACH(Y_MIN_PIN);
+    #else
+      static_assert(digitalPinHasPCICR(Y_MIN_PIN), "Y_MIN_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue.");
+      pciSetup(Y_MIN_PIN);
+    #endif
+  #endif
+  #if HAS_Z_MAX
+    #if (digitalPinToInterrupt(Z_MAX_PIN) != NOT_AN_INTERRUPT)
+      _ATTACH(Z_MAX_PIN);
+    #else
+      static_assert(digitalPinHasPCICR(Z_MAX_PIN), "Z_MAX_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue.");
+      pciSetup(Z_MAX_PIN);
+    #endif
+  #endif
+  #if HAS_Z_MIN
+    #if (digitalPinToInterrupt(Z_MIN_PIN) != NOT_AN_INTERRUPT)
+      _ATTACH(Z_MIN_PIN);
+    #else
+      static_assert(digitalPinHasPCICR(Z_MIN_PIN), "Z_MIN_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue.");
+      pciSetup(Z_MIN_PIN);
+    #endif
+  #endif
+  #if HAS_X2_MAX
+    #if (digitalPinToInterrupt(X2_MAX_PIN) != NOT_AN_INTERRUPT)
+      _ATTACH(X2_MAX_PIN);
+    #else
+      static_assert(digitalPinHasPCICR(X2_MAX_PIN), "X2_MAX_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue.");
+      pciSetup(X2_MAX_PIN);
+    #endif
+  #endif
+  #if HAS_X2_MIN
+    #if (digitalPinToInterrupt(X2_MIN_PIN) != NOT_AN_INTERRUPT)
+      _ATTACH(X2_MIN_PIN);
+    #else
+      static_assert(digitalPinHasPCICR(X2_MIN_PIN), "X2_MIN_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue.");
+      pciSetup(X2_MIN_PIN);
+    #endif
+  #endif
+  #if HAS_Y2_MAX
+    #if (digitalPinToInterrupt(Y2_MAX_PIN) != NOT_AN_INTERRUPT)
+      _ATTACH(Y2_MAX_PIN);
+    #else
+      static_assert(digitalPinHasPCICR(Y2_MAX_PIN), "Y2_MAX_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue.");
+      pciSetup(Y2_MAX_PIN);
+    #endif
+  #endif
+  #if HAS_Y2_MIN
+    #if (digitalPinToInterrupt(Y2_MIN_PIN) != NOT_AN_INTERRUPT)
+      _ATTACH(Y2_MIN_PIN);
+    #else
+      static_assert(digitalPinHasPCICR(Y2_MIN_PIN), "Y2_MIN_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue.");
+      pciSetup(Y2_MIN_PIN);
+    #endif
+  #endif
+  #if HAS_Z2_MAX
+    #if (digitalPinToInterrupt(Z2_MAX_PIN) != NOT_AN_INTERRUPT)
+      _ATTACH(Z2_MAX_PIN);
+    #else
+      static_assert(digitalPinHasPCICR(Z2_MAX_PIN), "Z2_MAX_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue.");
+      pciSetup(Z2_MAX_PIN);
+    #endif
+  #endif
+  #if HAS_Z2_MIN
+    #if (digitalPinToInterrupt(Z2_MIN_PIN) != NOT_AN_INTERRUPT)
+      _ATTACH(Z2_MIN_PIN);
+    #else
+      static_assert(digitalPinHasPCICR(Z2_MIN_PIN), "Z2_MIN_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue.");
+      pciSetup(Z2_MIN_PIN);
+    #endif
+  #endif
+  #if HAS_Z3_MAX
+    #if (digitalPinToInterrupt(Z3_MAX_PIN) != NOT_AN_INTERRUPT)
+      _ATTACH(Z3_MAX_PIN);
+    #else
+      static_assert(digitalPinHasPCICR(Z3_MAX_PIN), "Z3_MAX_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue.");
+      pciSetup(Z3_MAX_PIN);
+    #endif
+  #endif
+  #if HAS_Z3_MIN
+    #if (digitalPinToInterrupt(Z3_MIN_PIN) != NOT_AN_INTERRUPT)
+      _ATTACH(Z3_MIN_PIN);
+    #else
+      static_assert(digitalPinHasPCICR(Z3_MIN_PIN), "Z3_MIN_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue.");
+      pciSetup(Z3_MIN_PIN);
+    #endif
+  #endif
+  #if HAS_Z4_MAX
+    #if (digitalPinToInterrupt(Z4_MAX_PIN) != NOT_AN_INTERRUPT)
+      _ATTACH(Z4_MAX_PIN);
+    #else
+      static_assert(digitalPinHasPCICR(Z4_MAX_PIN), "Z4_MAX_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue.");
+      pciSetup(Z4_MAX_PIN);
+    #endif
+  #endif
+  #if HAS_Z4_MIN
+    #if (digitalPinToInterrupt(Z4_MIN_PIN) != NOT_AN_INTERRUPT)
+      _ATTACH(Z4_MIN_PIN);
+    #else
+      static_assert(digitalPinHasPCICR(Z4_MIN_PIN), "Z4_MIN_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue.");
+      pciSetup(Z4_MIN_PIN);
+    #endif
+  #endif
+  #if HAS_Z_MIN_PROBE_PIN
+    #if (digitalPinToInterrupt(Z_MIN_PROBE_PIN) != NOT_AN_INTERRUPT)
+      _ATTACH(Z_MIN_PROBE_PIN);
+    #else
+      static_assert(digitalPinHasPCICR(Z_MIN_PROBE_PIN), "Z_MIN_PROBE_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue.");
+      pciSetup(Z_MIN_PROBE_PIN);
+    #endif
+  #endif
+
+  // If we arrive here without raising an assertion, each pin has either an EXT-interrupt or a PCI.
+}
diff --git a/Marlin/src/HAL/AVR/fast_pwm.cpp b/Marlin/src/HAL/AVR/fast_pwm.cpp
new file mode 100644
index 0000000..238c112
--- /dev/null
+++ b/Marlin/src/HAL/AVR/fast_pwm.cpp
@@ -0,0 +1,282 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#ifdef __AVR__
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if NEEDS_HARDWARE_PWM // Specific meta-flag for features that mandate PWM
+
+#include "HAL.h"
+
+struct Timer {
+  volatile uint8_t* TCCRnQ[3];  // max 3 TCCR registers per timer
+  volatile uint16_t* OCRnQ[3];  // max 3 OCR registers per timer
+  volatile uint16_t* ICRn;      // max 1 ICR register per timer
+  uint8_t n;                    // the timer number [0->5]
+  uint8_t q;                    // the timer output [0->2] (A->C)
+};
+
+/**
+ * get_pwm_timer
+ *  Get the timer information and register of the provided pin.
+ *  Return a Timer struct containing this information.
+ *  Used by set_pwm_frequency, set_pwm_duty
+ */
+Timer get_pwm_timer(const pin_t pin) {
+  uint8_t q = 0;
+  switch (digitalPinToTimer(pin)) {
+    // Protect reserved timers (TIMER0 & TIMER1)
+    #ifdef TCCR0A
+      #if !AVR_AT90USB1286_FAMILY
+        case TIMER0A:
+      #endif
+      case TIMER0B:
+    #endif
+    #ifdef TCCR1A
+      case TIMER1A: case TIMER1B:
+    #endif
+                                        break;
+    #if defined(TCCR2) || defined(TCCR2A)
+      #ifdef TCCR2
+        case TIMER2: {
+          Timer timer = {
+            /*TCCRnQ*/  { &TCCR2, nullptr, nullptr },
+            /*OCRnQ*/   { (uint16_t*)&OCR2, nullptr, nullptr },
+            /*ICRn*/      nullptr,
+            /*n, q*/      2, 0
+          };
+        }
+      #elif defined(TCCR2A)
+        #if ENABLED(USE_OCR2A_AS_TOP)
+          case TIMER2A:   break; // protect TIMER2A
+          case TIMER2B: {
+            Timer timer = {
+              /*TCCRnQ*/  { &TCCR2A,  &TCCR2B,  nullptr },
+              /*OCRnQ*/   { (uint16_t*)&OCR2A, (uint16_t*)&OCR2B, nullptr },
+              /*ICRn*/      nullptr,
+              /*n, q*/      2, 1
+            };
+            return timer;
+          }
+        #else
+          case TIMER2B:   ++q;
+          case TIMER2A: {
+            Timer timer = {
+              /*TCCRnQ*/  { &TCCR2A,  &TCCR2B,  nullptr },
+              /*OCRnQ*/   { (uint16_t*)&OCR2A, (uint16_t*)&OCR2B, nullptr },
+              /*ICRn*/      nullptr,
+                            2, q
+            };
+            return timer;
+          }
+        #endif
+      #endif
+    #endif
+    #ifdef OCR3C
+      case TIMER3C:   ++q;
+      case TIMER3B:   ++q;
+      case TIMER3A: {
+        Timer timer = {
+          /*TCCRnQ*/  { &TCCR3A,  &TCCR3B,  &TCCR3C },
+          /*OCRnQ*/   { &OCR3A,   &OCR3B,   &OCR3C },
+          /*ICRn*/      &ICR3,
+          /*n, q*/      3, q
+        };
+        return timer;
+      }
+    #elif defined(OCR3B)
+      case TIMER3B:   ++q;
+      case TIMER3A: {
+        Timer timer = {
+          /*TCCRnQ*/  { &TCCR3A,  &TCCR3B,  nullptr },
+          /*OCRnQ*/   { &OCR3A,   &OCR3B,  nullptr },
+          /*ICRn*/      &ICR3,
+          /*n, q*/      3, q
+        };
+        return timer;
+      }
+    #endif
+    #ifdef TCCR4A
+      case TIMER4C:   ++q;
+      case TIMER4B:   ++q;
+      case TIMER4A: {
+        Timer timer = {
+          /*TCCRnQ*/  { &TCCR4A,  &TCCR4B,  &TCCR4C },
+          /*OCRnQ*/   { &OCR4A,   &OCR4B,   &OCR4C },
+          /*ICRn*/      &ICR4,
+          /*n, q*/      4, q
+        };
+        return timer;
+      }
+    #endif
+    #ifdef TCCR5A
+      case TIMER5C:   ++q;
+      case TIMER5B:   ++q;
+      case TIMER5A: {
+        Timer timer = {
+          /*TCCRnQ*/  { &TCCR5A,  &TCCR5B,  &TCCR5C },
+          /*OCRnQ*/   { &OCR5A,   &OCR5B,   &OCR5C },
+          /*ICRn*/      &ICR5,
+          /*n, q*/      5, q
+        };
+        return timer;
+      }
+    #endif
+  }
+  Timer timer = {
+      /*TCCRnQ*/  { nullptr, nullptr, nullptr },
+      /*OCRnQ*/   { nullptr, nullptr, nullptr },
+      /*ICRn*/      nullptr,
+                    0, 0
+  };
+  return timer;
+}
+
+void set_pwm_frequency(const pin_t pin, int f_desired) {
+  Timer timer = get_pwm_timer(pin);
+  if (timer.n == 0) return; // Don't proceed if protected timer or not recognised
+  uint16_t size;
+  if (timer.n == 2) size = 255; else size = 65535;
+
+  uint16_t res = 255;   // resolution (TOP value)
+  uint8_t j = 0;        // prescaler index
+  uint8_t wgm = 1;      // waveform generation mode
+
+  // Calculating the prescaler and resolution to use to achieve closest frequency
+  if (f_desired != 0) {
+    int f = (F_CPU) / (2 * 1024 * size) + 1; // Initialize frequency as lowest (non-zero) achievable
+    uint16_t prescaler[] = { 0, 1, 8, /*TIMER2 ONLY*/32, 64, /*TIMER2 ONLY*/128, 256, 1024 };
+
+    // loop over prescaler values
+    LOOP_S_L_N(i, 1, 8) {
+      uint16_t res_temp_fast = 255, res_temp_phase_correct = 255;
+      if (timer.n == 2) {
+        // No resolution calculation for TIMER2 unless enabled USE_OCR2A_AS_TOP
+        #if ENABLED(USE_OCR2A_AS_TOP)
+          const uint16_t rtf = (F_CPU) / (prescaler[i] * f_desired);
+          res_temp_fast = rtf - 1;
+          res_temp_phase_correct = rtf / 2;
+        #endif
+      }
+      else {
+        // Skip TIMER2 specific prescalers when not TIMER2
+        if (i == 3 || i == 5) continue;
+        const uint16_t rtf = (F_CPU) / (prescaler[i] * f_desired);
+        res_temp_fast = rtf - 1;
+        res_temp_phase_correct = rtf / 2;
+      }
+
+      LIMIT(res_temp_fast, 1U, size);
+      LIMIT(res_temp_phase_correct, 1U, size);
+      // Calculate frequencies of test prescaler and resolution values
+      const int f_temp_fast = (F_CPU) / (prescaler[i] * (1 + res_temp_fast)),
+                f_temp_phase_correct = (F_CPU) / (2 * prescaler[i] * res_temp_phase_correct),
+                f_diff = ABS(f - f_desired),
+                f_fast_diff = ABS(f_temp_fast - f_desired),
+                f_phase_diff = ABS(f_temp_phase_correct - f_desired);
+
+      // If FAST values are closest to desired f
+      if (f_fast_diff < f_diff && f_fast_diff <= f_phase_diff) {
+        // Remember this combination
+        f = f_temp_fast;
+        res = res_temp_fast;
+        j = i;
+        // Set the Wave Generation Mode to FAST PWM
+        if (timer.n == 2) {
+          wgm = (
+            #if ENABLED(USE_OCR2A_AS_TOP)
+              WGM2_FAST_PWM_OCR2A
+            #else
+              WGM2_FAST_PWM
+            #endif
+          );
+        }
+        else wgm = WGM_FAST_PWM_ICRn;
+      }
+      // If PHASE CORRECT values are closes to desired f
+      else if (f_phase_diff < f_diff) {
+        f = f_temp_phase_correct;
+        res = res_temp_phase_correct;
+        j = i;
+        // Set the Wave Generation Mode to PWM PHASE CORRECT
+        if (timer.n == 2) {
+          wgm = (
+            #if ENABLED(USE_OCR2A_AS_TOP)
+              WGM2_PWM_PC_OCR2A
+            #else
+              WGM2_PWM_PC
+            #endif
+          );
+        }
+        else wgm = WGM_PWM_PC_ICRn;
+      }
+    }
+  }
+  _SET_WGMnQ(timer.TCCRnQ, wgm);
+  _SET_CSn(timer.TCCRnQ, j);
+
+  if (timer.n == 2) {
+    #if ENABLED(USE_OCR2A_AS_TOP)
+      _SET_OCRnQ(timer.OCRnQ, 0, res);  // Set OCR2A value (TOP) = res
+    #endif
+  }
+  else
+    _SET_ICRn(timer.ICRn, res);         // Set ICRn value (TOP) = res
+}
+
+void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=255*/, const bool invert/*=false*/) {
+  // If v is 0 or v_size (max), digitalWrite to LOW or HIGH.
+  // Note that digitalWrite also disables pwm output for us (sets COM bit to 0)
+  if (v == 0)
+    digitalWrite(pin, invert);
+  else if (v == v_size)
+    digitalWrite(pin, !invert);
+  else {
+    Timer timer = get_pwm_timer(pin);
+    if (timer.n == 0) return; // Don't proceed if protected timer or not recognised
+    // Set compare output mode to CLEAR -> SET or SET -> CLEAR (if inverted)
+    _SET_COMnQ(timer.TCCRnQ, (timer.q
+        #ifdef TCCR2
+          + (timer.q == 2) // COM20 is on bit 4 of TCCR2, thus requires q + 1 in the macro
+        #endif
+      ), COM_CLEAR_SET + invert
+    );
+
+    uint16_t top;
+    if (timer.n == 2) { // if TIMER2
+      top = (
+        #if ENABLED(USE_OCR2A_AS_TOP)
+          *timer.OCRnQ[0] // top = OCR2A
+        #else
+          255 // top = 0xFF (max)
+        #endif
+      );
+    }
+    else
+      top = *timer.ICRn; // top = ICRn
+
+    _SET_OCRnQ(timer.OCRnQ, timer.q, v * float(top) / float(v_size)); // Scale 8/16-bit v to top value
+  }
+}
+
+#endif // NEEDS_HARDWARE_PWM
+#endif // __AVR__
diff --git a/Marlin/src/HAL/AVR/fastio.cpp b/Marlin/src/HAL/AVR/fastio.cpp
new file mode 100644
index 0000000..b51d7f9
--- /dev/null
+++ b/Marlin/src/HAL/AVR/fastio.cpp
@@ -0,0 +1,288 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * Fast I/O for extended pins
+ */
+
+#ifdef __AVR__
+
+#include "fastio.h"
+
+#ifdef FASTIO_EXT_START
+
+#include "../shared/Marduino.h"
+
+#define _IS_EXT(P) WITHIN(P, FASTIO_EXT_START, FASTIO_EXT_END)
+
+void extDigitalWrite(const int8_t pin, const uint8_t state) {
+  #define _WCASE(N) case N: WRITE(N, state); break
+  switch (pin) {
+    default: digitalWrite(pin, state);
+    #if _IS_EXT(70)
+      _WCASE(70);
+    #endif
+    #if _IS_EXT(71)
+      _WCASE(71);
+    #endif
+    #if _IS_EXT(72)
+      _WCASE(72);
+    #endif
+    #if _IS_EXT(73)
+      _WCASE(73);
+    #endif
+    #if _IS_EXT(74)
+      _WCASE(74);
+    #endif
+    #if _IS_EXT(75)
+      _WCASE(75);
+    #endif
+    #if _IS_EXT(76)
+      _WCASE(76);
+    #endif
+    #if _IS_EXT(77)
+      _WCASE(77);
+    #endif
+    #if _IS_EXT(78)
+      _WCASE(78);
+    #endif
+    #if _IS_EXT(79)
+      _WCASE(79);
+    #endif
+    #if _IS_EXT(80)
+      _WCASE(80);
+    #endif
+    #if _IS_EXT(81)
+      _WCASE(81);
+    #endif
+    #if _IS_EXT(82)
+      _WCASE(82);
+    #endif
+    #if _IS_EXT(83)
+      _WCASE(83);
+    #endif
+    #if _IS_EXT(84)
+      _WCASE(84);
+    #endif
+    #if _IS_EXT(85)
+      _WCASE(85);
+    #endif
+    #if _IS_EXT(86)
+      _WCASE(86);
+    #endif
+    #if _IS_EXT(87)
+      _WCASE(87);
+    #endif
+    #if _IS_EXT(88)
+      _WCASE(88);
+    #endif
+    #if _IS_EXT(89)
+      _WCASE(89);
+    #endif
+    #if _IS_EXT(90)
+      _WCASE(90);
+    #endif
+    #if _IS_EXT(91)
+      _WCASE(91);
+    #endif
+    #if _IS_EXT(92)
+      _WCASE(92);
+    #endif
+    #if _IS_EXT(93)
+      _WCASE(93);
+    #endif
+    #if _IS_EXT(94)
+      _WCASE(94);
+    #endif
+    #if _IS_EXT(95)
+      _WCASE(95);
+    #endif
+    #if _IS_EXT(96)
+      _WCASE(96);
+    #endif
+    #if _IS_EXT(97)
+      _WCASE(97);
+    #endif
+    #if _IS_EXT(98)
+      _WCASE(98);
+    #endif
+    #if _IS_EXT(99)
+      _WCASE(99);
+    #endif
+    #if _IS_EXT(100)
+      _WCASE(100);
+    #endif
+  }
+}
+
+uint8_t extDigitalRead(const int8_t pin) {
+  #define _RCASE(N) case N: return READ(N)
+  switch (pin) {
+    default: return digitalRead(pin);
+    #if _IS_EXT(70)
+      _RCASE(70);
+    #endif
+    #if _IS_EXT(71)
+      _RCASE(71);
+    #endif
+    #if _IS_EXT(72)
+      _RCASE(72);
+    #endif
+    #if _IS_EXT(73)
+      _RCASE(73);
+    #endif
+    #if _IS_EXT(74)
+      _RCASE(74);
+    #endif
+    #if _IS_EXT(75)
+      _RCASE(75);
+    #endif
+    #if _IS_EXT(76)
+      _RCASE(76);
+    #endif
+    #if _IS_EXT(77)
+      _RCASE(77);
+    #endif
+    #if _IS_EXT(78)
+      _RCASE(78);
+    #endif
+    #if _IS_EXT(79)
+      _RCASE(79);
+    #endif
+    #if _IS_EXT(80)
+      _RCASE(80);
+    #endif
+    #if _IS_EXT(81)
+      _RCASE(81);
+    #endif
+    #if _IS_EXT(82)
+      _RCASE(82);
+    #endif
+    #if _IS_EXT(83)
+      _RCASE(83);
+    #endif
+    #if _IS_EXT(84)
+      _RCASE(84);
+    #endif
+    #if _IS_EXT(85)
+      _RCASE(85);
+    #endif
+    #if _IS_EXT(86)
+      _RCASE(86);
+    #endif
+    #if _IS_EXT(87)
+      _RCASE(87);
+    #endif
+    #if _IS_EXT(88)
+      _RCASE(88);
+    #endif
+    #if _IS_EXT(89)
+      _RCASE(89);
+    #endif
+    #if _IS_EXT(90)
+      _RCASE(90);
+    #endif
+    #if _IS_EXT(91)
+      _RCASE(91);
+    #endif
+    #if _IS_EXT(92)
+      _RCASE(92);
+    #endif
+    #if _IS_EXT(93)
+      _RCASE(93);
+    #endif
+    #if _IS_EXT(94)
+      _RCASE(94);
+    #endif
+    #if _IS_EXT(95)
+      _RCASE(95);
+    #endif
+    #if _IS_EXT(96)
+      _RCASE(96);
+    #endif
+    #if _IS_EXT(97)
+      _RCASE(97);
+    #endif
+    #if _IS_EXT(98)
+      _RCASE(98);
+    #endif
+    #if _IS_EXT(99)
+      _RCASE(99);
+    #endif
+    #if _IS_EXT(100)
+      _RCASE(100);
+    #endif
+  }
+}
+
+#if 0
+/**
+ * Set Timer 5 PWM frequency in Hz, from 3.8Hz up to ~16MHz
+ * with a minimum resolution of 100 steps.
+ *
+ * DC values -1.0 to 1.0. Negative duty cycle inverts the pulse.
+ */
+uint16_t set_pwm_frequency_hz(const float &hz, const float dca, const float dcb, const float dcc) {
+  float count = 0;
+  if (hz > 0 && (dca || dcb || dcc)) {
+    count = float(F_CPU) / hz;            // 1x prescaler, TOP for 16MHz base freq.
+    uint16_t prescaler;                   // Range of 30.5Hz (65535) 64.5KHz (>31)
+
+         if (count >= 255. * 256.) { prescaler = 1024; SET_CS(5, PRESCALER_1024); }
+    else if (count >= 255. * 64.)  { prescaler = 256;  SET_CS(5,  PRESCALER_256); }
+    else if (count >= 255. * 8.)   { prescaler = 64;   SET_CS(5,   PRESCALER_64); }
+    else if (count >= 255.)        { prescaler = 8;    SET_CS(5,    PRESCALER_8); }
+    else                           { prescaler = 1;    SET_CS(5,    PRESCALER_1); }
+
+    count /= float(prescaler);
+    const float pwm_top = round(count);   // Get the rounded count
+
+    ICR5 = (uint16_t)pwm_top - 1;         // Subtract 1 for TOP
+    OCR5A = pwm_top * ABS(dca);          // Update and scale DCs
+    OCR5B = pwm_top * ABS(dcb);
+    OCR5C = pwm_top * ABS(dcc);
+    _SET_COM(5, A, dca ? (dca < 0 ? COM_SET_CLEAR : COM_CLEAR_SET) : COM_NORMAL); // Set compare modes
+    _SET_COM(5, B, dcb ? (dcb < 0 ? COM_SET_CLEAR : COM_CLEAR_SET) : COM_NORMAL);
+    _SET_COM(5, C, dcc ? (dcc < 0 ? COM_SET_CLEAR : COM_CLEAR_SET) : COM_NORMAL);
+
+    SET_WGM(5, FAST_PWM_ICRn);            // Fast PWM with ICR5 as TOP
+
+    //SERIAL_ECHOLNPGM("Timer 5 Settings:");
+    //SERIAL_ECHOLNPAIR("  Prescaler=", prescaler);
+    //SERIAL_ECHOLNPAIR("        TOP=", ICR5);
+    //SERIAL_ECHOLNPAIR("      OCR5A=", OCR5A);
+    //SERIAL_ECHOLNPAIR("      OCR5B=", OCR5B);
+    //SERIAL_ECHOLNPAIR("      OCR5C=", OCR5C);
+  }
+  else {
+    // Restore the default for Timer 5
+    SET_WGM(5, PWM_PC_8);                 // PWM 8-bit (Phase Correct)
+    SET_COMS(5, NORMAL, NORMAL, NORMAL);  // Do nothing
+    SET_CS(5, PRESCALER_64);              // 16MHz / 64 = 250KHz
+    OCR5A = OCR5B = OCR5C = 0;
+  }
+  return round(count);
+}
+#endif
+
+#endif // FASTIO_EXT_START
+#endif // __AVR__
diff --git a/Marlin/src/HAL/AVR/fastio.h b/Marlin/src/HAL/AVR/fastio.h
new file mode 100644
index 0000000..dd01634
--- /dev/null
+++ b/Marlin/src/HAL/AVR/fastio.h
@@ -0,0 +1,373 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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
+
+/**
+ * Fast I/O Routines for AVR
+ * Use direct port manipulation to save scads of processor time.
+ * Contributed by Triffid_Hunter and modified by Kliment, thinkyhead, Bob-the-Kuhn, et.al.
+ */
+
+#include <avr/io.h>
+
+#if defined(__AVR_AT90USB1287__) || defined(__AVR_AT90USB1286__) || defined(__AVR_AT90USB1286P__) || defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB646P__) || defined(__AVR_AT90USB647__)
+  #define AVR_AT90USB1286_FAMILY 1
+#elif defined(__AVR_ATmega644__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644PA__) || defined(__AVR_ATmega1284P__)
+  #define AVR_ATmega1284_FAMILY 1
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+  #define AVR_ATmega2560_FAMILY 1
+#elif defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
+  #define AVR_ATmega2561_FAMILY 1
+#elif defined(__AVR_ATmega168__) || defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__)
+  #define AVR_ATmega328_FAMILY 1
+#endif
+
+/**
+ * Include Ports and Functions
+ */
+#if AVR_ATmega328_FAMILY
+  #include "fastio/fastio_168.h"
+#elif AVR_ATmega1284_FAMILY
+  #include "fastio/fastio_644.h"
+#elif AVR_ATmega2560_FAMILY
+  #include "fastio/fastio_1280.h"
+#elif AVR_AT90USB1286_FAMILY
+  #include "fastio/fastio_AT90USB.h"
+#elif AVR_ATmega2561_FAMILY
+  #include "fastio/fastio_1281.h"
+#else
+  #error "No FastIO definition for the selected AVR Board."
+#endif
+
+/**
+ * Magic I/O routines
+ *
+ * Now you can simply SET_OUTPUT(PIN); WRITE(PIN, HIGH); WRITE(PIN, LOW);
+ *
+ * Why double up on these macros? see https://gcc.gnu.org/onlinedocs/cpp/Stringification.html
+ */
+
+#define _READ(IO)             TEST(DIO ## IO ## _RPORT, DIO ## IO ## _PIN)
+
+#define _WRITE_NC(IO,V) do{ \
+  if (V) SBI(DIO ## IO ## _WPORT, DIO ## IO ## _PIN); \
+  else   CBI(DIO ## IO ## _WPORT, DIO ## IO ## _PIN); \
+}while(0)
+
+#define _WRITE_C(IO,V) do{ \
+  uint8_t port_bits = DIO ## IO ## _WPORT;                  /* Get a mask from the current port bits */ \
+  if (V) port_bits = ~port_bits;                            /* For setting bits, invert the mask */ \
+  DIO ## IO ## _RPORT = port_bits & _BV(DIO ## IO ## _PIN); /* Atomically toggle the output port bits */ \
+}while(0)
+
+#define _WRITE(IO,V)          do{ if (&(DIO ## IO ## _RPORT) < (uint8_t*)0x100) _WRITE_NC(IO,V); else _WRITE_C(IO,V); }while(0)
+
+#define _TOGGLE(IO)           (DIO ## IO ## _RPORT = _BV(DIO ## IO ## _PIN))
+
+#define _SET_INPUT(IO)        CBI(DIO ## IO ## _DDR, DIO ## IO ## _PIN)
+#define _SET_OUTPUT(IO)       SBI(DIO ## IO ## _DDR, DIO ## IO ## _PIN)
+
+#define _IS_INPUT(IO)         !TEST(DIO ## IO ## _DDR, DIO ## IO ## _PIN)
+#define _IS_OUTPUT(IO)        TEST(DIO ## IO ## _DDR, DIO ## IO ## _PIN)
+
+// digitalRead/Write wrappers
+#ifdef FASTIO_EXT_START
+  void extDigitalWrite(const int8_t pin, const uint8_t state);
+  uint8_t extDigitalRead(const int8_t pin);
+#else
+  #define extDigitalWrite(IO,V) digitalWrite(IO,V)
+  #define extDigitalRead(IO)    digitalRead(IO)
+#endif
+
+#define READ(IO)              _READ(IO)
+#define WRITE(IO,V)           _WRITE(IO,V)
+#define TOGGLE(IO)            _TOGGLE(IO)
+
+#define SET_INPUT(IO)         _SET_INPUT(IO)
+#define SET_INPUT_PULLUP(IO)  do{ _SET_INPUT(IO); _WRITE(IO, HIGH); }while(0)
+#define SET_INPUT_PULLDOWN    SET_INPUT
+#define SET_OUTPUT(IO)        _SET_OUTPUT(IO)
+#define SET_PWM               SET_OUTPUT
+
+#define IS_INPUT(IO)          _IS_INPUT(IO)
+#define IS_OUTPUT(IO)         _IS_OUTPUT(IO)
+
+#define OUT_WRITE(IO,V)       do{ SET_OUTPUT(IO); WRITE(IO,V); }while(0)
+
+/**
+ * Timer and Interrupt Control
+ */
+
+// Waveform Generation Modes
+enum WaveGenMode : char {
+  WGM_NORMAL,          //  0
+  WGM_PWM_PC_8,        //  1
+  WGM_PWM_PC_9,        //  2
+  WGM_PWM_PC_10,       //  3
+  WGM_CTC_OCRnA,       //  4  COM OCnx
+  WGM_FAST_PWM_8,      //  5
+  WGM_FAST_PWM_9,      //  6
+  WGM_FAST_PWM_10,     //  7
+  WGM_PWM_PC_FC_ICRn,  //  8
+  WGM_PWM_PC_FC_OCRnA, //  9  COM OCnA
+  WGM_PWM_PC_ICRn,     // 10
+  WGM_PWM_PC_OCRnA,    // 11  COM OCnA
+  WGM_CTC_ICRn,        // 12  COM OCnx
+  WGM_reserved,        // 13
+  WGM_FAST_PWM_ICRn,   // 14  COM OCnA
+  WGM_FAST_PWM_OCRnA   // 15  COM OCnA
+};
+
+// Wavefore Generation Modes (Timer 2 only)
+enum WaveGenMode2 : char {
+  WGM2_NORMAL,          // 0
+  WGM2_PWM_PC,          // 1
+  WGM2_CTC_OCR2A,       // 2
+  WGM2_FAST_PWM,        // 3
+  WGM2_reserved_1,      // 4
+  WGM2_PWM_PC_OCR2A,    // 5
+  WGM2_reserved_2,      // 6
+  WGM2_FAST_PWM_OCR2A,  // 7
+};
+
+// Compare Modes
+enum CompareMode : char {
+  COM_NORMAL,          //  0
+  COM_TOGGLE,          //  1  Non-PWM: OCnx ... Both PWM (WGM 9,11,14,15): OCnA only ... else NORMAL
+  COM_CLEAR_SET,       //  2  Non-PWM: OCnx ... Fast PWM: OCnx/Bottom ... PF-FC: OCnx Up/Down
+  COM_SET_CLEAR        //  3  Non-PWM: OCnx ... Fast PWM: OCnx/Bottom ... PF-FC: OCnx Up/Down
+};
+
+// Clock Sources
+enum ClockSource : char {
+  CS_NONE,             //  0
+  CS_PRESCALER_1,      //  1
+  CS_PRESCALER_8,      //  2
+  CS_PRESCALER_64,     //  3
+  CS_PRESCALER_256,    //  4
+  CS_PRESCALER_1024,   //  5
+  CS_EXT_FALLING,      //  6
+  CS_EXT_RISING        //  7
+};
+
+// Clock Sources (Timer 2 only)
+enum ClockSource2 : char {
+  CS2_NONE,            //  0
+  CS2_PRESCALER_1,     //  1
+  CS2_PRESCALER_8,     //  2
+  CS2_PRESCALER_32,    //  3
+  CS2_PRESCALER_64,    //  4
+  CS2_PRESCALER_128,   //  5
+  CS2_PRESCALER_256,   //  6
+  CS2_PRESCALER_1024   //  7
+};
+
+// Get interrupt bits in an orderly way
+// Ex: cs = GET_CS(0); coma1 = GET_COM(A,1);
+#define GET_WGM(T)   (((TCCR##T##A >> WGM##T##0) & 0x3) | ((TCCR##T##B >> WGM##T##2 << 2) & 0xC))
+#define GET_CS(T)    ((TCCR##T##B >> CS##T##0) & 0x7)
+#define GET_COM(T,Q) ((TCCR##T##Q >> COM##T##Q##0) & 0x3)
+#define GET_COMA(T)  GET_COM(T,A)
+#define GET_COMB(T)  GET_COM(T,B)
+#define GET_COMC(T)  GET_COM(T,C)
+#define GET_ICNC(T)  (!!(TCCR##T##B & _BV(ICNC##T)))
+#define GET_ICES(T)  (!!(TCCR##T##B & _BV(ICES##T)))
+#define GET_FOC(T,Q) (!!(TCCR##T##C & _BV(FOC##T##Q)))
+#define GET_FOCA(T)  GET_FOC(T,A)
+#define GET_FOCB(T)  GET_FOC(T,B)
+#define GET_FOCC(T)  GET_FOC(T,C)
+
+// Set Wave Generation Mode bits
+// Ex: SET_WGM(5,CTC_ICRn);
+#define _SET_WGM(T,V) do{ \
+    TCCR##T##A = (TCCR##T##A & ~(0x3 << WGM##T##0)) | (( int(V)       & 0x3) << WGM##T##0); \
+    TCCR##T##B = (TCCR##T##B & ~(0x3 << WGM##T##2)) | (((int(V) >> 2) & 0x3) << WGM##T##2); \
+  }while(0)
+#define SET_WGM(T,V) _SET_WGM(T,WGM_##V)
+// Runtime (see set_pwm_frequency):
+#define _SET_WGMnQ(TCCRnQ, V) do{ \
+    *(TCCRnQ)[0] = (*(TCCRnQ)[0] & ~(0x3 << 0)) | (( int(V)       & 0x3) << 0); \
+    *(TCCRnQ)[1] = (*(TCCRnQ)[1] & ~(0x3 << 3)) | (((int(V) >> 2) & 0x3) << 3); \
+  }while(0)
+
+// Set Clock Select bits
+// Ex: SET_CS3(PRESCALER_64);
+#define _SET_CS(T,V) (TCCR##T##B = (TCCR##T##B & ~(0x7 << CS##T##0)) | ((int(V) & 0x7) << CS##T##0))
+#define _SET_CS0(V) _SET_CS(0,V)
+#define _SET_CS1(V) _SET_CS(1,V)
+#ifdef TCCR2
+  #define _SET_CS2(V) (TCCR2 = (TCCR2 & ~(0x7 << CS20)) | (int(V) << CS20))
+#else
+  #define _SET_CS2(V) _SET_CS(2,V)
+#endif
+#define _SET_CS3(V) _SET_CS(3,V)
+#define _SET_CS4(V) _SET_CS(4,V)
+#define _SET_CS5(V) _SET_CS(5,V)
+#define SET_CS0(V) _SET_CS0(CS_##V)
+#define SET_CS1(V) _SET_CS1(CS_##V)
+#ifdef TCCR2
+  #define SET_CS2(V) _SET_CS2(CS2_##V)
+#else
+  #define SET_CS2(V) _SET_CS2(CS_##V)
+#endif
+#define SET_CS3(V) _SET_CS3(CS_##V)
+#define SET_CS4(V) _SET_CS4(CS_##V)
+#define SET_CS5(V) _SET_CS5(CS_##V)
+#define SET_CS(T,V) SET_CS##T(V)
+// Runtime (see set_pwm_frequency)
+#define _SET_CSn(TCCRnQ, V) do{ \
+    (*(TCCRnQ)[1] = (*(TCCRnQ[1]) & ~(0x7 << 0)) | ((int(V) & 0x7) << 0)); \
+  }while(0)
+
+// Set Compare Mode bits
+// Ex: SET_COMS(4,CLEAR_SET,CLEAR_SET,CLEAR_SET);
+#define _SET_COM(T,Q,V) (TCCR##T##Q = (TCCR##T##Q & ~(0x3 << COM##T##Q##0)) | (int(V) << COM##T##Q##0))
+#define SET_COM(T,Q,V) _SET_COM(T,Q,COM_##V)
+#define SET_COMA(T,V) SET_COM(T,A,V)
+#define SET_COMB(T,V) SET_COM(T,B,V)
+#define SET_COMC(T,V) SET_COM(T,C,V)
+#define SET_COMS(T,V1,V2,V3) do{ SET_COMA(T,V1); SET_COMB(T,V2); SET_COMC(T,V3); }while(0)
+// Runtime (see set_pwm_duty)
+#define _SET_COMnQ(TCCRnQ, Q, V) do{ \
+    (*(TCCRnQ)[0] = (*(TCCRnQ)[0] & ~(0x3 << (6-2*(Q)))) | (int(V) << (6-2*(Q)))); \
+  }while(0)
+
+// Set OCRnQ register
+// Runtime (see set_pwm_duty):
+#define _SET_OCRnQ(OCRnQ, Q, V) do{ \
+    (*(OCRnQ)[(Q)] = (0x0000) | (int(V) & 0xFFFF)); \
+  }while(0)
+
+// Set ICRn register (one per timer)
+// Runtime (see set_pwm_frequency)
+#define _SET_ICRn(ICRn, V) do{ \
+    (*(ICRn) = (0x0000) | (int(V) & 0xFFFF)); \
+  }while(0)
+
+// Set Noise Canceler bit
+// Ex: SET_ICNC(2,1)
+#define SET_ICNC(T,V) (TCCR##T##B = (V) ? TCCR##T##B | _BV(ICNC##T) : TCCR##T##B & ~_BV(ICNC##T))
+
+// Set Input Capture Edge Select bit
+// Ex: SET_ICES(5,0)
+#define SET_ICES(T,V) (TCCR##T##B = (V) ? TCCR##T##B | _BV(ICES##T) : TCCR##T##B & ~_BV(ICES##T))
+
+// Set Force Output Compare bit
+// Ex: SET_FOC(3,A,1)
+#define SET_FOC(T,Q,V) (TCCR##T##C = (V) ? TCCR##T##C | _BV(FOC##T##Q) : TCCR##T##C & ~_BV(FOC##T##Q))
+#define SET_FOCA(T,V) SET_FOC(T,A,V)
+#define SET_FOCB(T,V) SET_FOC(T,B,V)
+#define SET_FOCC(T,V) SET_FOC(T,C,V)
+
+#if 0
+
+/**
+ * PWM availability macros
+ */
+
+// Determine which harware PWMs are already in use
+#define _PWM_CHK_FAN_B(P) (P == E0_AUTO_FAN_PIN || P == E1_AUTO_FAN_PIN || P == E2_AUTO_FAN_PIN || P == E3_AUTO_FAN_PIN || P == E4_AUTO_FAN_PIN || P == E5_AUTO_FAN_PIN || P == E6_AUTO_FAN_PIN || P == E7_AUTO_FAN_PIN || P == CHAMBER_AUTO_FAN_PIN)
+#if PIN_EXISTS(CONTROLLER_FAN)
+  #define PWM_CHK_FAN_B(P) (_PWM_CHK_FAN_B(P) || P == CONTROLLER_FAN_PIN)
+#else
+  #define PWM_CHK_FAN_B(P) _PWM_CHK_FAN_B(P)
+#endif
+
+#if ANY_PIN(FAN, FAN1, FAN2, FAN3, FAN4, FAN5, FAN6, FAN7)
+  #if PIN_EXISTS(FAN7)
+    #define PWM_CHK_FAN_A(P) (P == FAN0_PIN || P == FAN1_PIN || P == FAN2_PIN || P == FAN3_PIN || P == FAN4_PIN || P == FAN5_PIN || P == FAN6_PIN || P == FAN7_PIN)
+  #elif PIN_EXISTS(FAN6)
+    #define PWM_CHK_FAN_A(P) (P == FAN0_PIN || P == FAN1_PIN || P == FAN2_PIN || P == FAN3_PIN || P == FAN4_PIN || P == FAN5_PIN || P == FAN6_PIN)
+  #elif PIN_EXISTS(FAN5)
+    #define PWM_CHK_FAN_A(P) (P == FAN0_PIN || P == FAN1_PIN || P == FAN2_PIN || P == FAN3_PIN || P == FAN4_PIN || P == FAN5_PIN)
+  #elif PIN_EXISTS(FAN4)
+    #define PWM_CHK_FAN_A(P) (P == FAN0_PIN || P == FAN1_PIN || P == FAN2_PIN || P == FAN3_PIN || P == FAN4_PIN)
+  #elif PIN_EXISTS(FAN3)
+    #define PWM_CHK_FAN_A(P) (P == FAN0_PIN || P == FAN1_PIN || P == FAN2_PIN || P == FAN3_PIN)
+  #elif PIN_EXISTS(FAN2)
+    #define PWM_CHK_FAN_A(P) (P == FAN0_PIN || P == FAN1_PIN || P == FAN2_PIN)
+  #elif PIN_EXISTS(FAN1)
+    #define PWM_CHK_FAN_A(P) (P == FAN0_PIN || P == FAN1_PIN)
+  #else
+    #define PWM_CHK_FAN_A(P) (P == FAN0_PIN)
+  #endif
+#else
+  #define PWM_CHK_FAN_A(P) false
+#endif
+
+#if HAS_MOTOR_CURRENT_PWM
+  #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY)
+    #define PWM_CHK_MOTOR_CURRENT(P) (P == MOTOR_CURRENT_PWM_E || P == MOTOR_CURRENT_PWM_Z || P == MOTOR_CURRENT_PWM_XY)
+  #elif PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
+    #define PWM_CHK_MOTOR_CURRENT(P) (P == MOTOR_CURRENT_PWM_E || P == MOTOR_CURRENT_PWM_Z)
+  #else
+    #define PWM_CHK_MOTOR_CURRENT(P) (P == MOTOR_CURRENT_PWM_E)
+  #endif
+#else
+  #define PWM_CHK_MOTOR_CURRENT(P) false
+#endif
+
+#ifdef NUM_SERVOS
+  #if AVR_ATmega2560_FAMILY
+    #define PWM_CHK_SERVO(P) (P == 5 || (NUM_SERVOS > 12 && P == 6) || (NUM_SERVOS > 24 && P == 46))  // PWMS 3A, 4A & 5A
+  #elif AVR_ATmega2561_FAMILY
+    #define PWM_CHK_SERVO(P)   (P == 5)  // PWM3A
+  #elif AVR_ATmega1284_FAMILY
+    #define PWM_CHK_SERVO(P)   false
+  #elif AVR_AT90USB1286_FAMILY
+    #define PWM_CHK_SERVO(P)   (P == 16) // PWM3A
+  #elif AVR_ATmega328_FAMILY
+    #define PWM_CHK_SERVO(P)   false
+  #endif
+#else
+  #define PWM_CHK_SERVO(P) false
+#endif
+
+#if ENABLED(BARICUDA)
+  #if HAS_HEATER_1 && HAS_HEATER_2
+    #define PWM_CHK_HEATER(P) (P == HEATER_1_PIN || P == HEATER_2_PIN)
+  #elif HAS_HEATER_1
+    #define PWM_CHK_HEATER(P) (P == HEATER_1_PIN)
+  #endif
+#else
+    #define PWM_CHK_HEATER(P) false
+#endif
+
+#define PWM_CHK(P) (PWM_CHK_HEATER(P) || PWM_CHK_SERVO(P) || PWM_CHK_MOTOR_CURRENT(P) || PWM_CHK_FAN_A(P) || PWM_CHK_FAN_B(P))
+
+#endif // PWM_CHK is not used in Marlin
+
+// define which hardware PWMs are available for the current CPU
+// all timer 1 PWMS deleted from this list because they are never available
+#if AVR_ATmega2560_FAMILY
+  #define PWM_PIN(P)  ((P >= 2 && P <= 10) || P == 13 || P == 44 || P == 45 || P == 46)
+#elif AVR_ATmega2561_FAMILY
+  #define PWM_PIN(P)  ((P >= 2 && P <= 6) || P == 9)
+#elif AVR_ATmega1284_FAMILY
+  #define PWM_PIN(P)  (P == 3 || P == 4 || P == 14 || P == 15)
+#elif AVR_AT90USB1286_FAMILY
+  #define PWM_PIN(P)  (P == 0 || P == 1 || P == 14 || P == 15 || P == 16 || P == 24)
+#elif AVR_ATmega328_FAMILY
+  #define PWM_PIN(P)  (P == 3 || P == 5 || P == 6 || P == 11)
+#else
+  #error "unknown CPU"
+#endif
diff --git a/Marlin/src/HAL/AVR/fastio/fastio_1280.h b/Marlin/src/HAL/AVR/fastio/fastio_1280.h
new file mode 100644
index 0000000..f482f82
--- /dev/null
+++ b/Marlin/src/HAL/AVR/fastio/fastio_1280.h
@@ -0,0 +1,1114 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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
+
+/**
+ * Pin mapping for the 1280 and 2560
+ *
+ *   Hardware Pin  : 02 03 06 07 01 05 15 16 17 18 23 24 25 26 64 63 13 12 46 45 44 43 78 77 76 75 74 73 72 71 60 59 58 57 56 55 54 53 50 70 52 51 42 41 40 39 38 37 36 35 22 21 20 19 97 96 95 94 93 92 91 90 89 88 87 86 85 84 83 82 | 04 08 09 10 11 14 27 28 29 30 31 32 33 34 47 48 49 61 62 65 66 67 68 69 79 80 81 98 99 100
+ *   Port          : E0 E1 E4 E5 G5 E3 H3 H4 H5 H6 B4 B5 B6 B7 J1 J0 H1 H0 D3 D2 D1 D0 A0 A1 A2 A3 A4 A5 A6 A7 C7 C6 C5 C4 C3 C2 C1 C0 D7 G2 G1 G0 L7 L6 L5 L4 L3 L2 L1 L0 B3 B2 B1 B0 F0 F1 F2 F3 F4 F5 F6 F7 K0 K1 K2 K3 K4 K5 K6 K7 | E2 E6 E7 xx xx H2 H7 G3 G4 xx xx xx xx xx D4 D5 D6 xx xx J2 J3 J4 J5 J6 J7 xx xx xx xx xx
+ *   Logical Pin   : 00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 | 78 79 80 xx xx 84 85 71 70 xx xx xx xx xx 81 82 83 xx xx 72 73 75 76 77 74 xx xx xx xx xx
+ */
+
+#include "../fastio.h"
+
+// change for your board
+#define DEBUG_LED   DIO21
+
+// UART
+#define RXD         DIO0
+#define TXD         DIO1
+
+// SPI
+#define SCK         DIO52
+#define MISO        DIO50
+#define MOSI        DIO51
+#define SS          DIO53
+
+// TWI (I2C)
+#define SCL         DIO21
+#define SDA         DIO20
+
+// Timers and PWM
+#define OC0A        DIO13
+#define OC0B        DIO4
+#define OC1A        DIO11
+#define OC1B        DIO12
+#define OC2A        DIO10
+#define OC2B        DIO9
+#define OC3A        DIO5
+#define OC3B        DIO2
+#define OC3C        DIO3
+#define OC4A        DIO6
+#define OC4B        DIO7
+#define OC4C        DIO8
+#define OC5A        DIO46
+#define OC5B        DIO45
+#define OC5C        DIO44
+
+// Digital I/O
+
+#define DIO0_PIN    PINE0
+#define DIO0_RPORT  PINE
+#define DIO0_WPORT  PORTE
+#define DIO0_DDR    DDRE
+#define DIO0_PWM    nullptr
+
+#define DIO1_PIN    PINE1
+#define DIO1_RPORT  PINE
+#define DIO1_WPORT  PORTE
+#define DIO1_DDR    DDRE
+#define DIO1_PWM    nullptr
+
+#define DIO2_PIN    PINE4
+#define DIO2_RPORT  PINE
+#define DIO2_WPORT  PORTE
+#define DIO2_DDR    DDRE
+#define DIO2_PWM    &OCR3BL
+
+#define DIO3_PIN    PINE5
+#define DIO3_RPORT  PINE
+#define DIO3_WPORT  PORTE
+#define DIO3_DDR    DDRE
+#define DIO3_PWM    &OCR3CL
+
+#define DIO4_PIN    PING5
+#define DIO4_RPORT  PING
+#define DIO4_WPORT  PORTG
+#define DIO4_DDR    DDRG
+#define DIO4_PWM    &OCR0B
+
+#define DIO5_PIN    PINE3
+#define DIO5_RPORT  PINE
+#define DIO5_WPORT  PORTE
+#define DIO5_DDR    DDRE
+#define DIO5_PWM    &OCR3AL
+
+#define DIO6_PIN    PINH3
+#define DIO6_RPORT  PINH
+#define DIO6_WPORT  PORTH
+#define DIO6_DDR    DDRH
+#define DIO6_PWM    &OCR4AL
+
+#define DIO7_PIN    PINH4
+#define DIO7_RPORT  PINH
+#define DIO7_WPORT  PORTH
+#define DIO7_DDR    DDRH
+#define DIO7_PWM    &OCR4BL
+
+#define DIO8_PIN    PINH5
+#define DIO8_RPORT  PINH
+#define DIO8_WPORT  PORTH
+#define DIO8_DDR    DDRH
+#define DIO8_PWM    &OCR4CL
+
+#define DIO9_PIN    PINH6
+#define DIO9_RPORT  PINH
+#define DIO9_WPORT  PORTH
+#define DIO9_DDR    DDRH
+#define DIO9_PWM    &OCR2B
+
+#define DIO10_PIN   PINB4
+#define DIO10_RPORT PINB
+#define DIO10_WPORT PORTB
+#define DIO10_DDR   DDRB
+#define DIO10_PWM   &OCR2A
+
+#define DIO11_PIN   PINB5
+#define DIO11_RPORT PINB
+#define DIO11_WPORT PORTB
+#define DIO11_DDR   DDRB
+#define DIO11_PWM   nullptr
+
+#define DIO12_PIN   PINB6
+#define DIO12_RPORT PINB
+#define DIO12_WPORT PORTB
+#define DIO12_DDR   DDRB
+#define DIO12_PWM   nullptr
+
+#define DIO13_PIN   PINB7
+#define DIO13_RPORT PINB
+#define DIO13_WPORT PORTB
+#define DIO13_DDR   DDRB
+#define DIO13_PWM   &OCR0A
+
+#define DIO14_PIN   PINJ1
+#define DIO14_RPORT PINJ
+#define DIO14_WPORT PORTJ
+#define DIO14_DDR   DDRJ
+#define DIO14_PWM   nullptr
+
+#define DIO15_PIN   PINJ0
+#define DIO15_RPORT PINJ
+#define DIO15_WPORT PORTJ
+#define DIO15_DDR   DDRJ
+#define DIO15_PWM   nullptr
+
+#define DIO16_PIN   PINH1
+#define DIO16_RPORT PINH
+#define DIO16_WPORT PORTH
+#define DIO16_DDR   DDRH
+#define DIO16_PWM   nullptr
+
+#define DIO17_PIN   PINH0
+#define DIO17_RPORT PINH
+#define DIO17_WPORT PORTH
+#define DIO17_DDR   DDRH
+#define DIO17_PWM   nullptr
+
+#define DIO18_PIN   PIND3
+#define DIO18_RPORT PIND
+#define DIO18_WPORT PORTD
+#define DIO18_DDR   DDRD
+#define DIO18_PWM   nullptr
+
+#define DIO19_PIN   PIND2
+#define DIO19_RPORT PIND
+#define DIO19_WPORT PORTD
+#define DIO19_DDR   DDRD
+#define DIO19_PWM   nullptr
+
+#define DIO20_PIN   PIND1
+#define DIO20_RPORT PIND
+#define DIO20_WPORT PORTD
+#define DIO20_DDR   DDRD
+#define DIO20_PWM   nullptr
+
+#define DIO21_PIN   PIND0
+#define DIO21_RPORT PIND
+#define DIO21_WPORT PORTD
+#define DIO21_DDR   DDRD
+#define DIO21_PWM   nullptr
+
+#define DIO22_PIN   PINA0
+#define DIO22_RPORT PINA
+#define DIO22_WPORT PORTA
+#define DIO22_DDR   DDRA
+#define DIO22_PWM   nullptr
+
+#define DIO23_PIN   PINA1
+#define DIO23_RPORT PINA
+#define DIO23_WPORT PORTA
+#define DIO23_DDR   DDRA
+#define DIO23_PWM   nullptr
+
+#define DIO24_PIN   PINA2
+#define DIO24_RPORT PINA
+#define DIO24_WPORT PORTA
+#define DIO24_DDR   DDRA
+#define DIO24_PWM   nullptr
+
+#define DIO25_PIN   PINA3
+#define DIO25_RPORT PINA
+#define DIO25_WPORT PORTA
+#define DIO25_DDR   DDRA
+#define DIO25_PWM   nullptr
+
+#define DIO26_PIN   PINA4
+#define DIO26_RPORT PINA
+#define DIO26_WPORT PORTA
+#define DIO26_DDR   DDRA
+#define DIO26_PWM   nullptr
+
+#define DIO27_PIN   PINA5
+#define DIO27_RPORT PINA
+#define DIO27_WPORT PORTA
+#define DIO27_DDR   DDRA
+#define DIO27_PWM   nullptr
+
+#define DIO28_PIN   PINA6
+#define DIO28_RPORT PINA
+#define DIO28_WPORT PORTA
+#define DIO28_DDR   DDRA
+#define DIO28_PWM   nullptr
+
+#define DIO29_PIN   PINA7
+#define DIO29_RPORT PINA
+#define DIO29_WPORT PORTA
+#define DIO29_DDR   DDRA
+#define DIO29_PWM   nullptr
+
+#define DIO30_PIN   PINC7
+#define DIO30_RPORT PINC
+#define DIO30_WPORT PORTC
+#define DIO30_DDR   DDRC
+#define DIO30_PWM   nullptr
+
+#define DIO31_PIN   PINC6
+#define DIO31_RPORT PINC
+#define DIO31_WPORT PORTC
+#define DIO31_DDR   DDRC
+#define DIO31_PWM   nullptr
+
+#define DIO32_PIN   PINC5
+#define DIO32_RPORT PINC
+#define DIO32_WPORT PORTC
+#define DIO32_DDR   DDRC
+#define DIO32_PWM   nullptr
+
+#define DIO33_PIN   PINC4
+#define DIO33_RPORT PINC
+#define DIO33_WPORT PORTC
+#define DIO33_DDR   DDRC
+#define DIO33_PWM   nullptr
+
+#define DIO34_PIN   PINC3
+#define DIO34_RPORT PINC
+#define DIO34_WPORT PORTC
+#define DIO34_DDR   DDRC
+#define DIO34_PWM   nullptr
+
+#define DIO35_PIN   PINC2
+#define DIO35_RPORT PINC
+#define DIO35_WPORT PORTC
+#define DIO35_DDR   DDRC
+#define DIO35_PWM   nullptr
+
+#define DIO36_PIN   PINC1
+#define DIO36_RPORT PINC
+#define DIO36_WPORT PORTC
+#define DIO36_DDR   DDRC
+#define DIO36_PWM   nullptr
+
+#define DIO37_PIN   PINC0
+#define DIO37_RPORT PINC
+#define DIO37_WPORT PORTC
+#define DIO37_DDR   DDRC
+#define DIO37_PWM   nullptr
+
+#define DIO38_PIN   PIND7
+#define DIO38_RPORT PIND
+#define DIO38_WPORT PORTD
+#define DIO38_DDR   DDRD
+#define DIO38_PWM   nullptr
+
+#define DIO39_PIN   PING2
+#define DIO39_RPORT PING
+#define DIO39_WPORT PORTG
+#define DIO39_DDR   DDRG
+#define DIO39_PWM   nullptr
+
+#define DIO40_PIN   PING1
+#define DIO40_RPORT PING
+#define DIO40_WPORT PORTG
+#define DIO40_DDR   DDRG
+#define DIO40_PWM   nullptr
+
+#define DIO41_PIN   PING0
+#define DIO41_RPORT PING
+#define DIO41_WPORT PORTG
+#define DIO41_DDR   DDRG
+#define DIO41_PWM   nullptr
+
+#define DIO42_PIN   PINL7
+#define DIO42_RPORT PINL
+#define DIO42_WPORT PORTL
+#define DIO42_DDR   DDRL
+#define DIO42_PWM   nullptr
+
+#define DIO43_PIN   PINL6
+#define DIO43_RPORT PINL
+#define DIO43_WPORT PORTL
+#define DIO43_DDR   DDRL
+#define DIO43_PWM   nullptr
+
+#define DIO44_PIN   PINL5
+#define DIO44_RPORT PINL
+#define DIO44_WPORT PORTL
+#define DIO44_DDR   DDRL
+#define DIO44_PWM   &OCR5CL
+
+#define DIO45_PIN   PINL4
+#define DIO45_RPORT PINL
+#define DIO45_WPORT PORTL
+#define DIO45_DDR   DDRL
+#define DIO45_PWM   &OCR5BL
+
+#define DIO46_PIN   PINL3
+#define DIO46_RPORT PINL
+#define DIO46_WPORT PORTL
+#define DIO46_DDR   DDRL
+#define DIO46_PWM   &OCR5AL
+
+#define DIO47_PIN   PINL2
+#define DIO47_RPORT PINL
+#define DIO47_WPORT PORTL
+#define DIO47_DDR   DDRL
+#define DIO47_PWM   nullptr
+
+#define DIO48_PIN   PINL1
+#define DIO48_RPORT PINL
+#define DIO48_WPORT PORTL
+#define DIO48_DDR   DDRL
+#define DIO48_PWM   nullptr
+
+#define DIO49_PIN   PINL0
+#define DIO49_RPORT PINL
+#define DIO49_WPORT PORTL
+#define DIO49_DDR   DDRL
+#define DIO49_PWM   nullptr
+
+#define DIO50_PIN   PINB3
+#define DIO50_RPORT PINB
+#define DIO50_WPORT PORTB
+#define DIO50_DDR   DDRB
+#define DIO50_PWM   nullptr
+
+#define DIO51_PIN   PINB2
+#define DIO51_RPORT PINB
+#define DIO51_WPORT PORTB
+#define DIO51_DDR   DDRB
+#define DIO51_PWM   nullptr
+
+#define DIO52_PIN   PINB1
+#define DIO52_RPORT PINB
+#define DIO52_WPORT PORTB
+#define DIO52_DDR   DDRB
+#define DIO52_PWM   nullptr
+
+#define DIO53_PIN   PINB0
+#define DIO53_RPORT PINB
+#define DIO53_WPORT PORTB
+#define DIO53_DDR   DDRB
+#define DIO53_PWM   nullptr
+
+#define DIO54_PIN   PINF0
+#define DIO54_RPORT PINF
+#define DIO54_WPORT PORTF
+#define DIO54_DDR   DDRF
+#define DIO54_PWM   nullptr
+
+#define DIO55_PIN   PINF1
+#define DIO55_RPORT PINF
+#define DIO55_WPORT PORTF
+#define DIO55_DDR   DDRF
+#define DIO55_PWM   nullptr
+
+#define DIO56_PIN   PINF2
+#define DIO56_RPORT PINF
+#define DIO56_WPORT PORTF
+#define DIO56_DDR   DDRF
+#define DIO56_PWM   nullptr
+
+#define DIO57_PIN   PINF3
+#define DIO57_RPORT PINF
+#define DIO57_WPORT PORTF
+#define DIO57_DDR   DDRF
+#define DIO57_PWM   nullptr
+
+#define DIO58_PIN   PINF4
+#define DIO58_RPORT PINF
+#define DIO58_WPORT PORTF
+#define DIO58_DDR   DDRF
+#define DIO58_PWM   nullptr
+
+#define DIO59_PIN   PINF5
+#define DIO59_RPORT PINF
+#define DIO59_WPORT PORTF
+#define DIO59_DDR   DDRF
+#define DIO59_PWM   nullptr
+
+#define DIO60_PIN   PINF6
+#define DIO60_RPORT PINF
+#define DIO60_WPORT PORTF
+#define DIO60_DDR   DDRF
+#define DIO60_PWM   nullptr
+
+#define DIO61_PIN   PINF7
+#define DIO61_RPORT PINF
+#define DIO61_WPORT PORTF
+#define DIO61_DDR   DDRF
+#define DIO61_PWM   nullptr
+
+#define DIO62_PIN   PINK0
+#define DIO62_RPORT PINK
+#define DIO62_WPORT PORTK
+#define DIO62_DDR   DDRK
+#define DIO62_PWM   nullptr
+
+#define DIO63_PIN   PINK1
+#define DIO63_RPORT PINK
+#define DIO63_WPORT PORTK
+#define DIO63_DDR   DDRK
+#define DIO63_PWM   nullptr
+
+#define DIO64_PIN   PINK2
+#define DIO64_RPORT PINK
+#define DIO64_WPORT PORTK
+#define DIO64_DDR   DDRK
+#define DIO64_PWM   nullptr
+
+#define DIO65_PIN   PINK3
+#define DIO65_RPORT PINK
+#define DIO65_WPORT PORTK
+#define DIO65_DDR   DDRK
+#define DIO65_PWM   nullptr
+
+#define DIO66_PIN   PINK4
+#define DIO66_RPORT PINK
+#define DIO66_WPORT PORTK
+#define DIO66_DDR   DDRK
+#define DIO66_PWM   nullptr
+
+#define DIO67_PIN   PINK5
+#define DIO67_RPORT PINK
+#define DIO67_WPORT PORTK
+#define DIO67_DDR   DDRK
+#define DIO67_PWM   nullptr
+
+#define DIO68_PIN   PINK6
+#define DIO68_RPORT PINK
+#define DIO68_WPORT PORTK
+#define DIO68_DDR   DDRK
+#define DIO68_PWM   nullptr
+
+#define DIO69_PIN   PINK7
+#define DIO69_RPORT PINK
+#define DIO69_WPORT PORTK
+#define DIO69_DDR   DDRK
+#define DIO69_PWM   nullptr
+
+//#define FASTIO_EXT_START 70
+//#define FASTIO_EXT_END   85
+
+#define DIO70_PIN   PING4
+#define DIO70_RPORT PING
+#define DIO70_WPORT PORTG
+#define DIO70_DDR   DDRG
+#define DIO70_PWM   nullptr
+
+#define DIO71_PIN   PING3
+#define DIO71_RPORT PING
+#define DIO71_WPORT PORTG
+#define DIO71_DDR   DDRG
+#define DIO71_PWM   nullptr
+
+#define DIO72_PIN   PINJ2
+#define DIO72_RPORT PINJ
+#define DIO72_WPORT PORTJ
+#define DIO72_DDR   DDRJ
+#define DIO72_PWM   nullptr
+
+#define DIO73_PIN   PINJ3
+#define DIO73_RPORT PINJ
+#define DIO73_WPORT PORTJ
+#define DIO73_DDR   DDRJ
+#define DIO73_PWM   nullptr
+
+#define DIO74_PIN   PINJ7
+#define DIO74_RPORT PINJ
+#define DIO74_WPORT PORTJ
+#define DIO74_DDR   DDRJ
+#define DIO74_PWM   nullptr
+
+#define DIO75_PIN   PINJ4
+#define DIO75_RPORT PINJ
+#define DIO75_WPORT PORTJ
+#define DIO75_DDR   DDRJ
+#define DIO75_PWM   nullptr
+
+#define DIO76_PIN   PINJ5
+#define DIO76_RPORT PINJ
+#define DIO76_WPORT PORTJ
+#define DIO76_DDR   DDRJ
+#define DIO76_PWM   nullptr
+
+#define DIO77_PIN   PINJ6
+#define DIO77_RPORT PINJ
+#define DIO77_WPORT PORTJ
+#define DIO77_DDR   DDRJ
+#define DIO77_PWM   nullptr
+
+#define DIO78_PIN   PINE2
+#define DIO78_RPORT PINE
+#define DIO78_WPORT PORTE
+#define DIO78_DDR   DDRE
+#define DIO78_PWM   nullptr
+
+#define DIO79_PIN   PINE6
+#define DIO79_RPORT PINE
+#define DIO79_WPORT PORTE
+#define DIO79_DDR   DDRE
+#define DIO79_PWM   nullptr
+
+#define DIO80_PIN   PINE7
+#define DIO80_RPORT PINE
+#define DIO80_WPORT PORTE
+#define DIO80_DDR   DDRE
+#define DIO80_PWM   nullptr
+
+#define DIO81_PIN   PIND4
+#define DIO81_RPORT PIND
+#define DIO81_WPORT PORTD
+#define DIO81_DDR   DDRD
+#define DIO81_PWM   nullptr
+
+#define DIO82_PIN   PIND5
+#define DIO82_RPORT PIND
+#define DIO82_WPORT PORTD
+#define DIO82_DDR   DDRD
+#define DIO82_PWM   nullptr
+
+#define DIO83_PIN   PIND6
+#define DIO83_RPORT PIND
+#define DIO83_WPORT PORTD
+#define DIO83_DDR   DDRD
+#define DIO83_PWM   nullptr
+
+#define DIO84_PIN   PINH2
+#define DIO84_RPORT PINH
+#define DIO84_WPORT PORTH
+#define DIO84_DDR   DDRH
+#define DIO84_PWM   nullptr
+
+#define DIO85_PIN   PINH7
+#define DIO85_RPORT PINH
+#define DIO85_WPORT PORTH
+#define DIO85_DDR   DDRH
+#define DIO85_PWM   nullptr
+
+#undef PA0
+#define PA0_PIN     PINA0
+#define PA0_RPORT   PINA
+#define PA0_WPORT   PORTA
+#define PA0_DDR     DDRA
+#define PA0_PWM     nullptr
+#undef PA1
+#define PA1_PIN     PINA1
+#define PA1_RPORT   PINA
+#define PA1_WPORT   PORTA
+#define PA1_DDR     DDRA
+#define PA1_PWM     nullptr
+#undef PA2
+#define PA2_PIN     PINA2
+#define PA2_RPORT   PINA
+#define PA2_WPORT   PORTA
+#define PA2_DDR     DDRA
+#define PA2_PWM     nullptr
+#undef PA3
+#define PA3_PIN     PINA3
+#define PA3_RPORT   PINA
+#define PA3_WPORT   PORTA
+#define PA3_DDR     DDRA
+#define PA3_PWM     nullptr
+#undef PA4
+#define PA4_PIN     PINA4
+#define PA4_RPORT   PINA
+#define PA4_WPORT   PORTA
+#define PA4_DDR     DDRA
+#define PA4_PWM     nullptr
+#undef PA5
+#define PA5_PIN     PINA5
+#define PA5_RPORT   PINA
+#define PA5_WPORT   PORTA
+#define PA5_DDR     DDRA
+#define PA5_PWM     nullptr
+#undef PA6
+#define PA6_PIN     PINA6
+#define PA6_RPORT   PINA
+#define PA6_WPORT   PORTA
+#define PA6_DDR     DDRA
+#define PA6_PWM     nullptr
+#undef PA7
+#define PA7_PIN     PINA7
+#define PA7_RPORT   PINA
+#define PA7_WPORT   PORTA
+#define PA7_DDR     DDRA
+#define PA7_PWM     nullptr
+
+#undef PB0
+#define PB0_PIN     PINB0
+#define PB0_RPORT   PINB
+#define PB0_WPORT   PORTB
+#define PB0_DDR     DDRB
+#define PB0_PWM     nullptr
+#undef PB1
+#define PB1_PIN     PINB1
+#define PB1_RPORT   PINB
+#define PB1_WPORT   PORTB
+#define PB1_DDR     DDRB
+#define PB1_PWM     nullptr
+#undef PB2
+#define PB2_PIN     PINB2
+#define PB2_RPORT   PINB
+#define PB2_WPORT   PORTB
+#define PB2_DDR     DDRB
+#define PB2_PWM     nullptr
+#undef PB3
+#define PB3_PIN     PINB3
+#define PB3_RPORT   PINB
+#define PB3_WPORT   PORTB
+#define PB3_DDR     DDRB
+#define PB3_PWM     nullptr
+#undef PB4
+#define PB4_PIN     PINB4
+#define PB4_RPORT   PINB
+#define PB4_WPORT   PORTB
+#define PB4_DDR     DDRB
+#define PB4_PWM     &OCR2A
+#undef PB5
+#define PB5_PIN     PINB5
+#define PB5_RPORT   PINB
+#define PB5_WPORT   PORTB
+#define PB5_DDR     DDRB
+#define PB5_PWM     nullptr
+#undef PB6
+#define PB6_PIN     PINB6
+#define PB6_RPORT   PINB
+#define PB6_WPORT   PORTB
+#define PB6_DDR     DDRB
+#define PB6_PWM     nullptr
+#undef PB7
+#define PB7_PIN     PINB7
+#define PB7_RPORT   PINB
+#define PB7_WPORT   PORTB
+#define PB7_DDR     DDRB
+#define PB7_PWM     &OCR0A
+
+#undef PC0
+#define PC0_PIN     PINC0
+#define PC0_RPORT   PINC
+#define PC0_WPORT   PORTC
+#define PC0_DDR     DDRC
+#define PC0_PWM     nullptr
+#undef PC1
+#define PC1_PIN     PINC1
+#define PC1_RPORT   PINC
+#define PC1_WPORT   PORTC
+#define PC1_DDR     DDRC
+#define PC1_PWM     nullptr
+#undef PC2
+#define PC2_PIN     PINC2
+#define PC2_RPORT   PINC
+#define PC2_WPORT   PORTC
+#define PC2_DDR     DDRC
+#define PC2_PWM     nullptr
+#undef PC3
+#define PC3_PIN     PINC3
+#define PC3_RPORT   PINC
+#define PC3_WPORT   PORTC
+#define PC3_DDR     DDRC
+#define PC3_PWM     nullptr
+#undef PC4
+#define PC4_PIN     PINC4
+#define PC4_RPORT   PINC
+#define PC4_WPORT   PORTC
+#define PC4_DDR     DDRC
+#define PC4_PWM     nullptr
+#undef PC5
+#define PC5_PIN     PINC5
+#define PC5_RPORT   PINC
+#define PC5_WPORT   PORTC
+#define PC5_DDR     DDRC
+#define PC5_PWM     nullptr
+#undef PC6
+#define PC6_PIN     PINC6
+#define PC6_RPORT   PINC
+#define PC6_WPORT   PORTC
+#define PC6_DDR     DDRC
+#define PC6_PWM     nullptr
+#undef PC7
+#define PC7_PIN     PINC7
+#define PC7_RPORT   PINC
+#define PC7_WPORT   PORTC
+#define PC7_DDR     DDRC
+#define PC7_PWM     nullptr
+
+#undef PD0
+#define PD0_PIN     PIND0
+#define PD0_RPORT   PIND
+#define PD0_WPORT   PORTD
+#define PD0_DDR     DDRD
+#define PD0_PWM     nullptr
+#undef PD1
+#define PD1_PIN     PIND1
+#define PD1_RPORT   PIND
+#define PD1_WPORT   PORTD
+#define PD1_DDR     DDRD
+#define PD1_PWM     nullptr
+#undef PD2
+#define PD2_PIN     PIND2
+#define PD2_RPORT   PIND
+#define PD2_WPORT   PORTD
+#define PD2_DDR     DDRD
+#define PD2_PWM     nullptr
+#undef PD3
+#define PD3_PIN     PIND3
+#define PD3_RPORT   PIND
+#define PD3_WPORT   PORTD
+#define PD3_DDR     DDRD
+#define PD3_PWM     nullptr
+#undef PD4
+#define PD4_PIN     PIND4
+#define PD4_RPORT   PIND
+#define PD4_WPORT   PORTD
+#define PD4_DDR     DDRD
+#define PD4_PWM     nullptr
+#undef PD5
+#define PD5_PIN     PIND5
+#define PD5_RPORT   PIND
+#define PD5_WPORT   PORTD
+#define PD5_DDR     DDRD
+#define PD5_PWM     nullptr
+#undef PD6
+#define PD6_PIN     PIND6
+#define PD6_RPORT   PIND
+#define PD6_WPORT   PORTD
+#define PD6_DDR     DDRD
+#define PD6_PWM     nullptr
+#undef PD7
+#define PD7_PIN     PIND7
+#define PD7_RPORT   PIND
+#define PD7_WPORT   PORTD
+#define PD7_DDR     DDRD
+#define PD7_PWM     nullptr
+
+#undef PE0
+#define PE0_PIN     PINE0
+#define PE0_RPORT   PINE
+#define PE0_WPORT   PORTE
+#define PE0_DDR     DDRE
+#define PE0_PWM     nullptr
+#undef PE1
+#define PE1_PIN     PINE1
+#define PE1_RPORT   PINE
+#define PE1_WPORT   PORTE
+#define PE1_DDR     DDRE
+#define PE1_PWM     nullptr
+#undef PE2
+#define PE2_PIN     PINE2
+#define PE2_RPORT   PINE
+#define PE2_WPORT   PORTE
+#define PE2_DDR     DDRE
+#define PE2_PWM     nullptr
+#undef PE3
+#define PE3_PIN     PINE3
+#define PE3_RPORT   PINE
+#define PE3_WPORT   PORTE
+#define PE3_DDR     DDRE
+#define PE3_PWM     &OCR3AL
+#undef PE4
+#define PE4_PIN     PINE4
+#define PE4_RPORT   PINE
+#define PE4_WPORT   PORTE
+#define PE4_DDR     DDRE
+#define PE4_PWM     &OCR3BL
+#undef PE5
+#define PE5_PIN     PINE5
+#define PE5_RPORT   PINE
+#define PE5_WPORT   PORTE
+#define PE5_DDR     DDRE
+#define PE5_PWM     &OCR3CL
+#undef PE6
+#define PE6_PIN     PINE6
+#define PE6_RPORT   PINE
+#define PE6_WPORT   PORTE
+#define PE6_DDR     DDRE
+#define PE6_PWM     nullptr
+#undef PE7
+#define PE7_PIN     PINE7
+#define PE7_RPORT   PINE
+#define PE7_WPORT   PORTE
+#define PE7_DDR     DDRE
+#define PE7_PWM     nullptr
+
+#undef PF0
+#define PF0_PIN     PINF0
+#define PF0_RPORT   PINF
+#define PF0_WPORT   PORTF
+#define PF0_DDR     DDRF
+#define PF0_PWM     nullptr
+#undef PF1
+#define PF1_PIN     PINF1
+#define PF1_RPORT   PINF
+#define PF1_WPORT   PORTF
+#define PF1_DDR     DDRF
+#define PF1_PWM     nullptr
+#undef PF2
+#define PF2_PIN     PINF2
+#define PF2_RPORT   PINF
+#define PF2_WPORT   PORTF
+#define PF2_DDR     DDRF
+#define PF2_PWM     nullptr
+#undef PF3
+#define PF3_PIN     PINF3
+#define PF3_RPORT   PINF
+#define PF3_WPORT   PORTF
+#define PF3_DDR     DDRF
+#define PF3_PWM     nullptr
+#undef PF4
+#define PF4_PIN     PINF4
+#define PF4_RPORT   PINF
+#define PF4_WPORT   PORTF
+#define PF4_DDR     DDRF
+#define PF4_PWM     nullptr
+#undef PF5
+#define PF5_PIN     PINF5
+#define PF5_RPORT   PINF
+#define PF5_WPORT   PORTF
+#define PF5_DDR     DDRF
+#define PF5_PWM     nullptr
+#undef PF6
+#define PF6_PIN     PINF6
+#define PF6_RPORT   PINF
+#define PF6_WPORT   PORTF
+#define PF6_DDR     DDRF
+#define PF6_PWM     nullptr
+#undef PF7
+#define PF7_PIN     PINF7
+#define PF7_RPORT   PINF
+#define PF7_WPORT   PORTF
+#define PF7_DDR     DDRF
+#define PF7_PWM     nullptr
+
+#undef PG0
+#define PG0_PIN     PING0
+#define PG0_RPORT   PING
+#define PG0_WPORT   PORTG
+#define PG0_DDR     DDRG
+#define PG0_PWM     nullptr
+#undef PG1
+#define PG1_PIN     PING1
+#define PG1_RPORT   PING
+#define PG1_WPORT   PORTG
+#define PG1_DDR     DDRG
+#define PG1_PWM     nullptr
+#undef PG2
+#define PG2_PIN     PING2
+#define PG2_RPORT   PING
+#define PG2_WPORT   PORTG
+#define PG2_DDR     DDRG
+#define PG2_PWM     nullptr
+#undef PG3
+#define PG3_PIN     PING3
+#define PG3_RPORT   PING
+#define PG3_WPORT   PORTG
+#define PG3_DDR     DDRG
+#define PG3_PWM     nullptr
+#undef PG4
+#define PG4_PIN     PING4
+#define PG4_RPORT   PING
+#define PG4_WPORT   PORTG
+#define PG4_DDR     DDRG
+#define PG4_PWM     nullptr
+#undef PG5
+#define PG5_PIN     PING5
+#define PG5_RPORT   PING
+#define PG5_WPORT   PORTG
+#define PG5_DDR     DDRG
+#define PG5_PWM     &OCR0B
+
+#undef PH0
+#define PH0_PIN     PINH0
+#define PH0_RPORT   PINH
+#define PH0_WPORT   PORTH
+#define PH0_DDR     DDRH
+#define PH0_PWM     nullptr
+#undef PH1
+#define PH1_PIN     PINH1
+#define PH1_RPORT   PINH
+#define PH1_WPORT   PORTH
+#define PH1_DDR     DDRH
+#define PH1_PWM     nullptr
+#undef PH2
+#define PH2_PIN     PINH2
+#define PH2_RPORT   PINH
+#define PH2_WPORT   PORTH
+#define PH2_DDR     DDRH
+#define PH2_PWM     nullptr
+#undef PH3
+#define PH3_PIN     PINH3
+#define PH3_RPORT   PINH
+#define PH3_WPORT   PORTH
+#define PH3_DDR     DDRH
+#define PH3_PWM     &OCR4AL
+#undef PH4
+#define PH4_PIN     PINH4
+#define PH4_RPORT   PINH
+#define PH4_WPORT   PORTH
+#define PH4_DDR     DDRH
+#define PH4_PWM     &OCR4BL
+#undef PH5
+#define PH5_PIN     PINH5
+#define PH5_RPORT   PINH
+#define PH5_WPORT   PORTH
+#define PH5_DDR     DDRH
+#define PH5_PWM     &OCR4CL
+#undef PH6
+#define PH6_PIN     PINH6
+#define PH6_RPORT   PINH
+#define PH6_WPORT   PORTH
+#define PH6_DDR     DDRH
+#define PH6_PWM     &OCR2B
+#undef PH7
+#define PH7_PIN     PINH7
+#define PH7_RPORT   PINH
+#define PH7_WPORT   PORTH
+#define PH7_DDR     DDRH
+#define PH7_PWM     nullptr
+
+#undef PJ0
+#define PJ0_PIN     PINJ0
+#define PJ0_RPORT   PINJ
+#define PJ0_WPORT   PORTJ
+#define PJ0_DDR     DDRJ
+#define PJ0_PWM     nullptr
+#undef PJ1
+#define PJ1_PIN     PINJ1
+#define PJ1_RPORT   PINJ
+#define PJ1_WPORT   PORTJ
+#define PJ1_DDR     DDRJ
+#define PJ1_PWM     nullptr
+#undef PJ2
+#define PJ2_PIN     PINJ2
+#define PJ2_RPORT   PINJ
+#define PJ2_WPORT   PORTJ
+#define PJ2_DDR     DDRJ
+#define PJ2_PWM     nullptr
+#undef PJ3
+#define PJ3_PIN     PINJ3
+#define PJ3_RPORT   PINJ
+#define PJ3_WPORT   PORTJ
+#define PJ3_DDR     DDRJ
+#define PJ3_PWM     nullptr
+#undef PJ4
+#define PJ4_PIN     PINJ4
+#define PJ4_RPORT   PINJ
+#define PJ4_WPORT   PORTJ
+#define PJ4_DDR     DDRJ
+#define PJ4_PWM     nullptr
+#undef PJ5
+#define PJ5_PIN     PINJ5
+#define PJ5_RPORT   PINJ
+#define PJ5_WPORT   PORTJ
+#define PJ5_DDR     DDRJ
+#define PJ5_PWM     nullptr
+#undef PJ6
+#define PJ6_PIN     PINJ6
+#define PJ6_RPORT   PINJ
+#define PJ6_WPORT   PORTJ
+#define PJ6_DDR     DDRJ
+#define PJ6_PWM     nullptr
+#undef PJ7
+#define PJ7_PIN     PINJ7
+#define PJ7_RPORT   PINJ
+#define PJ7_WPORT   PORTJ
+#define PJ7_DDR     DDRJ
+#define PJ7_PWM     nullptr
+
+#undef PK0
+#define PK0_PIN     PINK0
+#define PK0_RPORT   PINK
+#define PK0_WPORT   PORTK
+#define PK0_DDR     DDRK
+#define PK0_PWM     nullptr
+#undef PK1
+#define PK1_PIN     PINK1
+#define PK1_RPORT   PINK
+#define PK1_WPORT   PORTK
+#define PK1_DDR     DDRK
+#define PK1_PWM     nullptr
+#undef PK2
+#define PK2_PIN     PINK2
+#define PK2_RPORT   PINK
+#define PK2_WPORT   PORTK
+#define PK2_DDR     DDRK
+#define PK2_PWM     nullptr
+#undef PK3
+#define PK3_PIN     PINK3
+#define PK3_RPORT   PINK
+#define PK3_WPORT   PORTK
+#define PK3_DDR     DDRK
+#define PK3_PWM     nullptr
+#undef PK4
+#define PK4_PIN     PINK4
+#define PK4_RPORT   PINK
+#define PK4_WPORT   PORTK
+#define PK4_DDR     DDRK
+#define PK4_PWM     nullptr
+#undef PK5
+#define PK5_PIN     PINK5
+#define PK5_RPORT   PINK
+#define PK5_WPORT   PORTK
+#define PK5_DDR     DDRK
+#define PK5_PWM     nullptr
+#undef PK6
+#define PK6_PIN     PINK6
+#define PK6_RPORT   PINK
+#define PK6_WPORT   PORTK
+#define PK6_DDR     DDRK
+#define PK6_PWM     nullptr
+#undef PK7
+#define PK7_PIN     PINK7
+#define PK7_RPORT   PINK
+#define PK7_WPORT   PORTK
+#define PK7_DDR     DDRK
+#define PK7_PWM     nullptr
+
+#undef PL0
+#define PL0_PIN     PINL0
+#define PL0_RPORT   PINL
+#define PL0_WPORT   PORTL
+#define PL0_DDR     DDRL
+#define PL0_PWM     nullptr
+#undef PL1
+#define PL1_PIN     PINL1
+#define PL1_RPORT   PINL
+#define PL1_WPORT   PORTL
+#define PL1_DDR     DDRL
+#define PL1_PWM     nullptr
+#undef PL2
+#define PL2_PIN     PINL2
+#define PL2_RPORT   PINL
+#define PL2_WPORT   PORTL
+#define PL2_DDR     DDRL
+#define PL2_PWM     nullptr
+#undef PL3
+#define PL3_PIN     PINL3
+#define PL3_RPORT   PINL
+#define PL3_WPORT   PORTL
+#define PL3_DDR     DDRL
+#define PL3_PWM     &OCR5AL
+#undef PL4
+#define PL4_PIN     PINL4
+#define PL4_RPORT   PINL
+#define PL4_WPORT   PORTL
+#define PL4_DDR     DDRL
+#define PL4_PWM     &OCR5BL
+#undef PL5
+#define PL5_PIN     PINL5
+#define PL5_RPORT   PINL
+#define PL5_WPORT   PORTL
+#define PL5_DDR     DDRL
+#define PL5_PWM     &OCR5CL
+#undef PL6
+#define PL6_PIN     PINL6
+#define PL6_RPORT   PINL
+#define PL6_WPORT   PORTL
+#define PL6_DDR     DDRL
+#define PL6_PWM     nullptr
+#undef PL7
+#define PL7_PIN     PINL7
+#define PL7_RPORT   PINL
+#define PL7_WPORT   PORTL
+#define PL7_DDR     DDRL
+#define PL7_PWM     nullptr
diff --git a/Marlin/src/HAL/AVR/fastio/fastio_1281.h b/Marlin/src/HAL/AVR/fastio/fastio_1281.h
new file mode 100644
index 0000000..e0bc5e2
--- /dev/null
+++ b/Marlin/src/HAL/AVR/fastio/fastio_1281.h
@@ -0,0 +1,715 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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
+
+/**
+ * Pin mapping for the 1281 and 2561
+ *
+ *   Logical Pin: 38 39 40 41 42 43 44 45 16 10 11 12 06 07 08 09 30 31 32 33 34 35 36 37 17 18 19 20 21 22 23 24 00 01 13 05 02 03 14 15 46 47 48 49 50 51 52 53 25 26 27 28 29 04
+ *   Port:        A0 A1 A2 A3 A4 A5 A6 A7 B0 B1 B2 B3 B4 B5 B6 B7 C0 C1 C2 C3 C4 C5 C6 C7 D0 D1 D2 D3 D4 D5 D6 D7 E0 E1 E2 E3 E4 E5 E6 E7 F0 F1 F2 F3 F4 F5 F6 F7 G0 G1 G2 G3 G4 G5
+ */
+
+#include "../fastio.h"
+
+// change for your board
+#define DEBUG_LED   DIO46
+
+// UART
+#define RXD         DIO0
+#define TXD         DIO1
+
+// SPI
+#define SCK         DIO10
+#define MISO        DIO12
+#define MOSI        DIO11
+#define SS          DIO16
+
+// TWI (I2C)
+#define SCL         DIO17
+#define SDA         DIO18
+
+// Timers and PWM
+#define OC0A        DIO9
+#define OC0B        DIO4
+#define OC1A        DIO7
+#define OC1B        DIO8
+#define OC2A        DIO6
+#define OC3A        DIO5
+#define OC3B        DIO2
+#define OC3C        DIO3
+
+// Digital I/O
+
+#define DIO0_PIN    PINE0
+#define DIO0_RPORT  PINE
+#define DIO0_WPORT  PORTE
+#define DIO0_DDR    DDRE
+#define DIO0_PWM    nullptr
+
+#define DIO1_PIN    PINE1
+#define DIO1_RPORT  PINE
+#define DIO1_WPORT  PORTE
+#define DIO1_DDR    DDRE
+#define DIO1_PWM    nullptr
+
+#define DIO2_PIN    PINE4
+#define DIO2_RPORT  PINE
+#define DIO2_WPORT  PORTE
+#define DIO2_DDR    DDRE
+#define DIO2_PWM    &OCR3BL
+
+#define DIO3_PIN    PINE5
+#define DIO3_RPORT  PINE
+#define DIO3_WPORT  PORTE
+#define DIO3_DDR    DDRE
+#define DIO3_PWM    &OCR3CL
+
+#define DIO4_PIN    PING5
+#define DIO4_RPORT  PING
+#define DIO4_WPORT  PORTG
+#define DIO4_DDR    DDRG
+#define DIO4_PWM    &OCR0B
+
+#define DIO5_PIN    PINE3
+#define DIO5_RPORT  PINE
+#define DIO5_WPORT  PORTE
+#define DIO5_DDR    DDRE
+#define DIO5_PWM    &OCR3AL
+
+#define DIO6_PIN    PINB4
+#define DIO6_RPORT  PINB
+#define DIO6_WPORT  PORTB
+#define DIO6_DDR    DDRB
+#define DIO6_PWM    &OCR2AL
+
+#define DIO7_PIN    PINB5
+#define DIO7_RPORT  PINB
+#define DIO7_WPORT  PORTB
+#define DIO7_DDR    DDRB
+#define DIO7_PWM    &OCR1AL
+
+#define DIO8_PIN    PINB6
+#define DIO8_RPORT  PINB
+#define DIO8_WPORT  PORTB
+#define DIO8_DDR    DDRB
+#define DIO8_PWM    &OCR1BL
+
+#define DIO9_PIN    PINB7
+#define DIO9_RPORT  PINB
+#define DIO9_WPORT  PORTB
+#define DIO9_DDR    DDRB
+#define DIO9_PWM    &OCR0AL
+
+#define DIO10_PIN   PINB1
+#define DIO10_RPORT PINB
+#define DIO10_WPORT PORTB
+#define DIO10_DDR   DDRB
+#define DIO10_PWM   nullptr
+
+#define DIO11_PIN   PINB2
+#define DIO11_RPORT PINB
+#define DIO11_WPORT PORTB
+#define DIO11_DDR   DDRB
+#define DIO11_PWM   nullptr
+
+#define DIO12_PIN   PINB3
+#define DIO12_RPORT PINB
+#define DIO12_WPORT PORTB
+#define DIO12_DDR   DDRB
+#define DIO12_PWM   nullptr
+
+#define DIO13_PIN   PINE2
+#define DIO13_RPORT PINE
+#define DIO13_WPORT PORTE
+#define DIO13_DDR   DDRE
+#define DIO13_PWM   nullptr
+
+#define DIO14_PIN   PINE6
+#define DIO14_RPORT PINE
+#define DIO14_WPORT PORTE
+#define DIO14_DDR   DDRE
+#define DIO14_PWM   nullptr
+
+#define DIO15_PIN   PINE7
+#define DIO15_RPORT PINE
+#define DIO15_WPORT PORTE
+#define DIO15_DDR   DDRE
+#define DIO15_PWM   nullptr
+
+#define DIO16_PIN   PINB0
+#define DIO16_RPORT PINB
+#define DIO16_WPORT PORTB
+#define DIO16_DDR   DDRB
+#define DIO16_PWM   nullptr
+
+#define DIO17_PIN   PIND0
+#define DIO17_RPORT PIND
+#define DIO17_WPORT PORTD
+#define DIO17_DDR   DDRD
+#define DIO17_PWM   nullptr
+
+#define DIO18_PIN   PIND1
+#define DIO18_RPORT PIND
+#define DIO18_WPORT PORTD
+#define DIO18_DDR   DDRD
+#define DIO18_PWM   nullptr
+
+#define DIO19_PIN   PIND2
+#define DIO19_RPORT PIND
+#define DIO19_WPORT PORTD
+#define DIO19_DDR   DDRD
+#define DIO19_PWM   nullptr
+
+#define DIO20_PIN   PIND3
+#define DIO20_RPORT PIND
+#define DIO20_WPORT PORTD
+#define DIO20_DDR   DDRD
+#define DIO20_PWM   nullptr
+
+#define DIO21_PIN   PIND4
+#define DIO21_RPORT PIND
+#define DIO21_WPORT PORTD
+#define DIO21_DDR   DDRD
+#define DIO21_PWM   nullptr
+
+#define DIO22_PIN   PIND5
+#define DIO22_RPORT PIND
+#define DIO22_WPORT PORTD
+#define DIO22_DDR   DDRD
+#define DIO22_PWM   nullptr
+
+#define DIO23_PIN   PIND6
+#define DIO23_RPORT PIND
+#define DIO23_WPORT PORTD
+#define DIO23_DDR   DDRD
+#define DIO23_PWM   nullptr
+
+#define DIO24_PIN   PIND7
+#define DIO24_RPORT PIND
+#define DIO24_WPORT PORTD
+#define DIO24_DDR   DDRD
+#define DIO24_PWM   nullptr
+
+#define DIO25_PIN   PING0
+#define DIO25_RPORT PING
+#define DIO25_WPORT PORTG
+#define DIO25_DDR   DDRG
+#define DIO25_PWM   nullptr
+
+#define DIO26_PIN   PING1
+#define DIO26_RPORT PING
+#define DIO26_WPORT PORTG
+#define DIO26_DDR   DDRG
+#define DIO26_PWM   nullptr
+
+#define DIO27_PIN   PING2
+#define DIO27_RPORT PING
+#define DIO27_WPORT PORTG
+#define DIO27_DDR   DDRG
+#define DIO27_PWM   nullptr
+
+#define DIO28_PIN   PING3
+#define DIO28_RPORT PING
+#define DIO28_WPORT PORTG
+#define DIO28_DDR   DDRG
+#define DIO28_PWM   nullptr
+
+#define DIO29_PIN   PING4
+#define DIO29_RPORT PING
+#define DIO29_WPORT PORTG
+#define DIO29_DDR   DDRG
+#define DIO29_PWM   nullptr
+
+#define DIO30_PIN   PINC0
+#define DIO30_RPORT PINC
+#define DIO30_WPORT PORTC
+#define DIO30_DDR   DDRC
+#define DIO30_PWM   nullptr
+
+#define DIO31_PIN   PINC1
+#define DIO31_RPORT PINC
+#define DIO31_WPORT PORTC
+#define DIO31_DDR   DDRC
+#define DIO31_PWM   nullptr
+
+#define DIO32_PIN   PINC2
+#define DIO32_RPORT PINC
+#define DIO32_WPORT PORTC
+#define DIO32_DDR   DDRC
+#define DIO32_PWM   nullptr
+
+#define DIO33_PIN   PINC3
+#define DIO33_RPORT PINC
+#define DIO33_WPORT PORTC
+#define DIO33_DDR   DDRC
+#define DIO33_PWM   nullptr
+
+#define DIO34_PIN   PINC4
+#define DIO34_RPORT PINC
+#define DIO34_WPORT PORTC
+#define DIO34_DDR   DDRC
+#define DIO34_PWM   nullptr
+
+#define DIO35_PIN   PINC5
+#define DIO35_RPORT PINC
+#define DIO35_WPORT PORTC
+#define DIO35_DDR   DDRC
+#define DIO35_PWM   nullptr
+
+#define DIO36_PIN   PINC6
+#define DIO36_RPORT PINC
+#define DIO36_WPORT PORTC
+#define DIO36_DDR   DDRC
+#define DIO36_PWM   nullptr
+
+#define DIO37_PIN   PINC7
+#define DIO37_RPORT PINC
+#define DIO37_WPORT PORTC
+#define DIO37_DDR   DDRC
+#define DIO37_PWM   nullptr
+
+#define DIO38_PIN   PINA0
+#define DIO38_RPORT PINA
+#define DIO38_WPORT PORTA
+#define DIO38_DDR   DDRA
+#define DIO38_PWM   nullptr
+
+#define DIO39_PIN   PINA1
+#define DIO39_RPORT PINA
+#define DIO39_WPORT PORTA
+#define DIO39_DDR   DDRA
+#define DIO39_PWM   nullptr
+
+#define DIO40_PIN   PINA2
+#define DIO40_RPORT PINA
+#define DIO40_WPORT PORTA
+#define DIO40_DDR   DDRA
+#define DIO40_PWM   nullptr
+
+#define DIO41_PIN   PINA3
+#define DIO41_RPORT PINA
+#define DIO41_WPORT PORTA
+#define DIO41_DDR   DDRA
+#define DIO41_PWM   nullptr
+
+#define DIO42_PIN   PINA4
+#define DIO42_RPORT PINA
+#define DIO42_WPORT PORTA
+#define DIO42_DDR   DDRA
+#define DIO42_PWM   nullptr
+
+#define DIO43_PIN   PINA5
+#define DIO43_RPORT PINA
+#define DIO43_WPORT PORTA
+#define DIO43_DDR   DDRA
+#define DIO43_PWM   nullptr
+
+#define DIO44_PIN   PINA6
+#define DIO44_RPORT PINA
+#define DIO44_WPORT PORTA
+#define DIO44_DDR   DDRA
+#define DIO44_PWM   nullptr
+
+#define DIO45_PIN   PINA7
+#define DIO45_RPORT PINA
+#define DIO45_WPORT PORTA
+#define DIO45_DDR   DDRA
+#define DIO45_PWM   nullptr
+
+#define DIO46_PIN   PINF0
+#define DIO46_RPORT PINF
+#define DIO46_WPORT PORTF
+#define DIO46_DDR   DDRF
+#define DIO46_PWM   nullptr
+
+#define DIO47_PIN   PINF1
+#define DIO47_RPORT PINF
+#define DIO47_WPORT PORTF
+#define DIO47_DDR   DDRF
+#define DIO47_PWM   nullptr
+
+#define DIO48_PIN   PINF2
+#define DIO48_RPORT PINF
+#define DIO48_WPORT PORTF
+#define DIO48_DDR   DDRF
+#define DIO48_PWM   nullptr
+
+#define DIO49_PIN   PINF3
+#define DIO49_RPORT PINF
+#define DIO49_WPORT PORTF
+#define DIO49_DDR   DDRF
+#define DIO49_PWM   nullptr
+
+#define DIO50_PIN   PINF4
+#define DIO50_RPORT PINF
+#define DIO50_WPORT PORTF
+#define DIO50_DDR   DDRF
+#define DIO50_PWM   nullptr
+
+#define DIO51_PIN   PINF5
+#define DIO51_RPORT PINF
+#define DIO51_WPORT PORTF
+#define DIO51_DDR   DDRF
+#define DIO51_PWM   nullptr
+
+#define DIO52_PIN   PINF6
+#define DIO52_RPORT PINF
+#define DIO52_WPORT PORTF
+#define DIO52_DDR   DDRF
+#define DIO52_PWM   nullptr
+
+#define DIO53_PIN   PINF7
+#define DIO53_RPORT PINF
+#define DIO53_WPORT PORTF
+#define DIO53_DDR   DDRF
+#define DIO53_PWM   nullptr
+
+#undef PA0
+#define PA0_PIN     PINA0
+#define PA0_RPORT   PINA
+#define PA0_WPORT   PORTA
+#define PA0_DDR     DDRA
+#define PA0_PWM     nullptr
+#undef PA1
+#define PA1_PIN     PINA1
+#define PA1_RPORT   PINA
+#define PA1_WPORT   PORTA
+#define PA1_DDR     DDRA
+#define PA1_PWM     nullptr
+#undef PA2
+#define PA2_PIN     PINA2
+#define PA2_RPORT   PINA
+#define PA2_WPORT   PORTA
+#define PA2_DDR     DDRA
+#define PA2_PWM     nullptr
+#undef PA3
+#define PA3_PIN     PINA3
+#define PA3_RPORT   PINA
+#define PA3_WPORT   PORTA
+#define PA3_DDR     DDRA
+#define PA3_PWM     nullptr
+#undef PA4
+#define PA4_PIN     PINA4
+#define PA4_RPORT   PINA
+#define PA4_WPORT   PORTA
+#define PA4_DDR     DDRA
+#define PA4_PWM     nullptr
+#undef PA5
+#define PA5_PIN     PINA5
+#define PA5_RPORT   PINA
+#define PA5_WPORT   PORTA
+#define PA5_DDR     DDRA
+#define PA5_PWM     nullptr
+#undef PA6
+#define PA6_PIN     PINA6
+#define PA6_RPORT   PINA
+#define PA6_WPORT   PORTA
+#define PA6_DDR     DDRA
+#define PA6_PWM     nullptr
+#undef PA7
+#define PA7_PIN     PINA7
+#define PA7_RPORT   PINA
+#define PA7_WPORT   PORTA
+#define PA7_DDR     DDRA
+#define PA7_PWM     nullptr
+
+#undef PB0
+#define PB0_PIN     PINB0
+#define PB0_RPORT   PINB
+#define PB0_WPORT   PORTB
+#define PB0_DDR     DDRB
+#define PB0_PWM     nullptr
+#undef PB1
+#define PB1_PIN     PINB1
+#define PB1_RPORT   PINB
+#define PB1_WPORT   PORTB
+#define PB1_DDR     DDRB
+#define PB1_PWM     nullptr
+#undef PB2
+#define PB2_PIN     PINB2
+#define PB2_RPORT   PINB
+#define PB2_WPORT   PORTB
+#define PB2_DDR     DDRB
+#define PB2_PWM     nullptr
+#undef PB3
+#define PB3_PIN     PINB3
+#define PB3_RPORT   PINB
+#define PB3_WPORT   PORTB
+#define PB3_DDR     DDRB
+#define PB3_PWM     nullptr
+#undef PB4
+#define PB4_PIN     PINB4
+#define PB4_RPORT   PINB
+#define PB4_WPORT   PORTB
+#define PB4_DDR     DDRB
+#define PB4_PWM     &OCR2A
+#undef PB5
+#define PB5_PIN     PINB5
+#define PB5_RPORT   PINB
+#define PB5_WPORT   PORTB
+#define PB5_DDR     DDRB
+#define PB5_PWM     nullptr
+#undef PB6
+#define PB6_PIN     PINB6
+#define PB6_RPORT   PINB
+#define PB6_WPORT   PORTB
+#define PB6_DDR     DDRB
+#define PB6_PWM     nullptr
+#undef PB7
+#define PB7_PIN     PINB7
+#define PB7_RPORT   PINB
+#define PB7_WPORT   PORTB
+#define PB7_DDR     DDRB
+#define PB7_PWM     &OCR0A
+
+#undef PC0
+#define PC0_PIN     PINC0
+#define PC0_RPORT   PINC
+#define PC0_WPORT   PORTC
+#define PC0_DDR     DDRC
+#define PC0_PWM     nullptr
+#undef PC1
+#define PC1_PIN     PINC1
+#define PC1_RPORT   PINC
+#define PC1_WPORT   PORTC
+#define PC1_DDR     DDRC
+#define PC1_PWM     nullptr
+#undef PC2
+#define PC2_PIN     PINC2
+#define PC2_RPORT   PINC
+#define PC2_WPORT   PORTC
+#define PC2_DDR     DDRC
+#define PC2_PWM     nullptr
+#undef PC3
+#define PC3_PIN     PINC3
+#define PC3_RPORT   PINC
+#define PC3_WPORT   PORTC
+#define PC3_DDR     DDRC
+#define PC3_PWM     nullptr
+#undef PC4
+#define PC4_PIN     PINC4
+#define PC4_RPORT   PINC
+#define PC4_WPORT   PORTC
+#define PC4_DDR     DDRC
+#define PC4_PWM     nullptr
+#undef PC5
+#define PC5_PIN     PINC5
+#define PC5_RPORT   PINC
+#define PC5_WPORT   PORTC
+#define PC5_DDR     DDRC
+#define PC5_PWM     nullptr
+#undef PC6
+#define PC6_PIN     PINC6
+#define PC6_RPORT   PINC
+#define PC6_WPORT   PORTC
+#define PC6_DDR     DDRC
+#define PC6_PWM     nullptr
+#undef PC7
+#define PC7_PIN     PINC7
+#define PC7_RPORT   PINC
+#define PC7_WPORT   PORTC
+#define PC7_DDR     DDRC
+#define PC7_PWM     nullptr
+
+#undef PD0
+#define PD0_PIN     PIND0
+#define PD0_RPORT   PIND
+#define PD0_WPORT   PORTD
+#define PD0_DDR     DDRD
+#define PD0_PWM     nullptr
+#undef PD1
+#define PD1_PIN     PIND1
+#define PD1_RPORT   PIND
+#define PD1_WPORT   PORTD
+#define PD1_DDR     DDRD
+#define PD1_PWM     nullptr
+#undef PD2
+#define PD2_PIN     PIND2
+#define PD2_RPORT   PIND
+#define PD2_WPORT   PORTD
+#define PD2_DDR     DDRD
+#define PD2_PWM     nullptr
+#undef PD3
+#define PD3_PIN     PIND3
+#define PD3_RPORT   PIND
+#define PD3_WPORT   PORTD
+#define PD3_DDR     DDRD
+#define PD3_PWM     nullptr
+#undef PD4
+#define PD4_PIN     PIND4
+#define PD4_RPORT   PIND
+#define PD4_WPORT   PORTD
+#define PD4_DDR     DDRD
+#define PD4_PWM     nullptr
+#undef PD5
+#define PD5_PIN     PIND5
+#define PD5_RPORT   PIND
+#define PD5_WPORT   PORTD
+#define PD5_DDR     DDRD
+#define PD5_PWM     nullptr
+#undef PD6
+#define PD6_PIN     PIND6
+#define PD6_RPORT   PIND
+#define PD6_WPORT   PORTD
+#define PD6_DDR     DDRD
+#define PD6_PWM     nullptr
+#undef PD7
+#define PD7_PIN     PIND7
+#define PD7_RPORT   PIND
+#define PD7_WPORT   PORTD
+#define PD7_DDR     DDRD
+#define PD7_PWM     nullptr
+
+#undef PE0
+#define PE0_PIN     PINE0
+#define PE0_RPORT   PINE
+#define PE0_WPORT   PORTE
+#define PE0_DDR     DDRE
+#define PE0_PWM     nullptr
+#undef PE1
+#define PE1_PIN     PINE1
+#define PE1_RPORT   PINE
+#define PE1_WPORT   PORTE
+#define PE1_DDR     DDRE
+#define PE1_PWM     nullptr
+#undef PE2
+#define PE2_PIN     PINE2
+#define PE2_RPORT   PINE
+#define PE2_WPORT   PORTE
+#define PE2_DDR     DDRE
+#define PE2_PWM     nullptr
+#undef PE3
+#define PE3_PIN     PINE3
+#define PE3_RPORT   PINE
+#define PE3_WPORT   PORTE
+#define PE3_DDR     DDRE
+#define PE3_PWM     &OCR3AL
+#undef PE4
+#define PE4_PIN     PINE4
+#define PE4_RPORT   PINE
+#define PE4_WPORT   PORTE
+#define PE4_DDR     DDRE
+#define PE4_PWM     &OCR3BL
+#undef PE5
+#define PE5_PIN     PINE5
+#define PE5_RPORT   PINE
+#define PE5_WPORT   PORTE
+#define PE5_DDR     DDRE
+#define PE5_PWM     &OCR3CL
+#undef PE6
+#define PE6_PIN     PINE6
+#define PE6_RPORT   PINE
+#define PE6_WPORT   PORTE
+#define PE6_DDR     DDRE
+#define PE6_PWM     nullptr
+#undef PE7
+#define PE7_PIN     PINE7
+#define PE7_RPORT   PINE
+#define PE7_WPORT   PORTE
+#define PE7_DDR     DDRE
+#define PE7_PWM     nullptr
+
+#undef PF0
+#define PF0_PIN     PINF0
+#define PF0_RPORT   PINF
+#define PF0_WPORT   PORTF
+#define PF0_DDR     DDRF
+#define PF0_PWM     nullptr
+#undef PF1
+#define PF1_PIN     PINF1
+#define PF1_RPORT   PINF
+#define PF1_WPORT   PORTF
+#define PF1_DDR     DDRF
+#define PF1_PWM     nullptr
+#undef PF2
+#define PF2_PIN     PINF2
+#define PF2_RPORT   PINF
+#define PF2_WPORT   PORTF
+#define PF2_DDR     DDRF
+#define PF2_PWM     nullptr
+#undef PF3
+#define PF3_PIN     PINF3
+#define PF3_RPORT   PINF
+#define PF3_WPORT   PORTF
+#define PF3_DDR     DDRF
+#define PF3_PWM     nullptr
+#undef PF4
+#define PF4_PIN     PINF4
+#define PF4_RPORT   PINF
+#define PF4_WPORT   PORTF
+#define PF4_DDR     DDRF
+#define PF4_PWM     nullptr
+#undef PF5
+#define PF5_PIN     PINF5
+#define PF5_RPORT   PINF
+#define PF5_WPORT   PORTF
+#define PF5_DDR     DDRF
+#define PF5_PWM     nullptr
+#undef PF6
+#define PF6_PIN     PINF6
+#define PF6_RPORT   PINF
+#define PF6_WPORT   PORTF
+#define PF6_DDR     DDRF
+#define PF6_PWM     nullptr
+#undef PF7
+#define PF7_PIN     PINF7
+#define PF7_RPORT   PINF
+#define PF7_WPORT   PORTF
+#define PF7_DDR     DDRF
+#define PF7_PWM     nullptr
+
+#undef PG0
+#define PG0_PIN     PING0
+#define PG0_RPORT   PING
+#define PG0_WPORT   PORTG
+#define PG0_DDR     DDRG
+#define PG0_PWM     nullptr
+#undef PG1
+#define PG1_PIN     PING1
+#define PG1_RPORT   PING
+#define PG1_WPORT   PORTG
+#define PG1_DDR     DDRG
+#define PG1_PWM     nullptr
+#undef PG2
+#define PG2_PIN     PING2
+#define PG2_RPORT   PING
+#define PG2_WPORT   PORTG
+#define PG2_DDR     DDRG
+#define PG2_PWM     nullptr
+#undef PG3
+#define PG3_PIN     PING3
+#define PG3_RPORT   PING
+#define PG3_WPORT   PORTG
+#define PG3_DDR     DDRG
+#define PG3_PWM     nullptr
+#undef PG4
+#define PG4_PIN     PING4
+#define PG4_RPORT   PING
+#define PG4_WPORT   PORTG
+#define PG4_DDR     DDRG
+#define PG4_PWM     nullptr
+#undef PG5
+#define PG5_PIN     PING5
+#define PG5_RPORT   PING
+#define PG5_WPORT   PORTG
+#define PG5_DDR     DDRG
+#define PG5_PWM     &OCR0B
diff --git a/Marlin/src/HAL/AVR/fastio/fastio_168.h b/Marlin/src/HAL/AVR/fastio/fastio_168.h
new file mode 100644
index 0000000..8cfdd1e
--- /dev/null
+++ b/Marlin/src/HAL/AVR/fastio/fastio_168.h
@@ -0,0 +1,357 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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
+
+/**
+ * Pin mapping for the 168, 328, and 328P
+ *
+ *   Logical Pin: 08 09 10 11 12 13 14 15 16 17 18 19 20 21 00 01 02 03 04 05 06 07
+ *   Port:        B0 B1 B2 B3 B4 B5 C0 C1 C2 C3 C4 C5 C6 C7 D0 D1 D2 D3 D4 D5 D6 D7
+ */
+
+#include "../fastio.h"
+
+#define DEBUG_LED   AIO5
+
+// UART
+#define RXD         DIO0
+#define TXD         DIO1
+
+// SPI
+#define SCK         DIO13
+#define MISO        DIO12
+#define MOSI        DIO11
+#define SS          DIO10
+
+// TWI (I2C)
+#define SCL         AIO5
+#define SDA         AIO4
+
+// Timers and PWM
+#define OC0A        DIO6
+#define OC0B        DIO5
+#define OC1A        DIO9
+#define OC1B        DIO10
+#define OC2A        DIO11
+#define OC2B        DIO3
+
+// Digital I/O
+
+#define DIO0_PIN    PIND0
+#define DIO0_RPORT  PIND
+#define DIO0_WPORT  PORTD
+#define DIO0_DDR    DDRD
+#define DIO0_PWM    nullptr
+
+#define DIO1_PIN    PIND1
+#define DIO1_RPORT  PIND
+#define DIO1_WPORT  PORTD
+#define DIO1_DDR    DDRD
+#define DIO1_PWM    nullptr
+
+#define DIO2_PIN    PIND2
+#define DIO2_RPORT  PIND
+#define DIO2_WPORT  PORTD
+#define DIO2_DDR    DDRD
+#define DIO2_PWM    nullptr
+
+#define DIO3_PIN    PIND3
+#define DIO3_RPORT  PIND
+#define DIO3_WPORT  PORTD
+#define DIO3_DDR    DDRD
+#define DIO3_PWM    &OCR2B
+
+#define DIO4_PIN    PIND4
+#define DIO4_RPORT  PIND
+#define DIO4_WPORT  PORTD
+#define DIO4_DDR    DDRD
+#define DIO4_PWM    nullptr
+
+#define DIO5_PIN    PIND5
+#define DIO5_RPORT  PIND
+#define DIO5_WPORT  PORTD
+#define DIO5_DDR    DDRD
+#define DIO5_PWM    &OCR0B
+
+#define DIO6_PIN    PIND6
+#define DIO6_RPORT  PIND
+#define DIO6_WPORT  PORTD
+#define DIO6_DDR    DDRD
+#define DIO6_PWM    &OCR0A
+
+#define DIO7_PIN    PIND7
+#define DIO7_RPORT  PIND
+#define DIO7_WPORT  PORTD
+#define DIO7_DDR    DDRD
+#define DIO7_PWM    nullptr
+
+#define DIO8_PIN    PINB0
+#define DIO8_RPORT  PINB
+#define DIO8_WPORT  PORTB
+#define DIO8_DDR    DDRB
+#define DIO8_PWM    nullptr
+
+#define DIO9_PIN    PINB1
+#define DIO9_RPORT  PINB
+#define DIO9_WPORT  PORTB
+#define DIO9_DDR    DDRB
+#define DIO9_PWM    nullptr
+
+#define DIO10_PIN   PINB2
+#define DIO10_RPORT PINB
+#define DIO10_WPORT PORTB
+#define DIO10_DDR   DDRB
+#define DIO10_PWM   nullptr
+
+#define DIO11_PIN   PINB3
+#define DIO11_RPORT PINB
+#define DIO11_WPORT PORTB
+#define DIO11_DDR   DDRB
+#define DIO11_PWM   &OCR2A
+
+#define DIO12_PIN   PINB4
+#define DIO12_RPORT PINB
+#define DIO12_WPORT PORTB
+#define DIO12_DDR   DDRB
+#define DIO12_PWM   nullptr
+
+#define DIO13_PIN   PINB5
+#define DIO13_RPORT PINB
+#define DIO13_WPORT PORTB
+#define DIO13_DDR   DDRB
+#define DIO13_PWM   nullptr
+
+#define DIO14_PIN   PINC0
+#define DIO14_RPORT PINC
+#define DIO14_WPORT PORTC
+#define DIO14_DDR   DDRC
+#define DIO14_PWM   nullptr
+
+#define DIO15_PIN   PINC1
+#define DIO15_RPORT PINC
+#define DIO15_WPORT PORTC
+#define DIO15_DDR   DDRC
+#define DIO15_PWM   nullptr
+
+#define DIO16_PIN   PINC2
+#define DIO16_RPORT PINC
+#define DIO16_WPORT PORTC
+#define DIO16_DDR   DDRC
+#define DIO16_PWM   nullptr
+
+#define DIO17_PIN   PINC3
+#define DIO17_RPORT PINC
+#define DIO17_WPORT PORTC
+#define DIO17_DDR   DDRC
+#define DIO17_PWM   nullptr
+
+#define DIO18_PIN   PINC4
+#define DIO18_RPORT PINC
+#define DIO18_WPORT PORTC
+#define DIO18_DDR   DDRC
+#define DIO18_PWM   nullptr
+
+#define DIO19_PIN   PINC5
+#define DIO19_RPORT PINC
+#define DIO19_WPORT PORTC
+#define DIO19_DDR   DDRC
+#define DIO19_PWM   nullptr
+
+#define DIO20_PIN   PINC6
+#define DIO20_RPORT PINC
+#define DIO20_WPORT PORTC
+#define DIO20_DDR   DDRC
+#define DIO20_PWM   nullptr
+
+#define DIO21_PIN   PINC7
+#define DIO21_RPORT PINC
+#define DIO21_WPORT PORTC
+#define DIO21_DDR   DDRC
+#define DIO21_PWM   nullptr
+
+#undef PB0
+#define PB0_PIN     PINB0
+#define PB0_RPORT   PINB
+#define PB0_WPORT   PORTB
+#define PB0_DDR     DDRB
+#define PB0_PWM     nullptr
+
+#undef PB1
+#define PB1_PIN     PINB1
+#define PB1_RPORT   PINB
+#define PB1_WPORT   PORTB
+#define PB1_DDR     DDRB
+#define PB1_PWM     nullptr
+
+#undef PB2
+#define PB2_PIN     PINB2
+#define PB2_RPORT   PINB
+#define PB2_WPORT   PORTB
+#define PB2_DDR     DDRB
+#define PB2_PWM     nullptr
+
+#undef PB3
+#define PB3_PIN     PINB3
+#define PB3_RPORT   PINB
+#define PB3_WPORT   PORTB
+#define PB3_DDR     DDRB
+#define PB3_PWM     &OCR2A
+
+#undef PB4
+#define PB4_PIN     PINB4
+#define PB4_RPORT   PINB
+#define PB4_WPORT   PORTB
+#define PB4_DDR     DDRB
+#define PB4_PWM     nullptr
+
+#undef PB5
+#define PB5_PIN     PINB5
+#define PB5_RPORT   PINB
+#define PB5_WPORT   PORTB
+#define PB5_DDR     DDRB
+#define PB5_PWM     nullptr
+
+#undef PB6
+#define PB6_PIN     PINB6
+#define PB6_RPORT   PINB
+#define PB6_WPORT   PORTB
+#define PB6_DDR     DDRB
+#define PB6_PWM     nullptr
+
+#undef PB7
+#define PB7_PIN     PINB7
+#define PB7_RPORT   PINB
+#define PB7_WPORT   PORTB
+#define PB7_DDR     DDRB
+#define PB7_PWM     nullptr
+
+#undef PC0
+#define PC0_PIN     PINC0
+#define PC0_RPORT   PINC
+#define PC0_WPORT   PORTC
+#define PC0_DDR     DDRC
+#define PC0_PWM     nullptr
+
+#undef PC1
+#define PC1_PIN     PINC1
+#define PC1_RPORT   PINC
+#define PC1_WPORT   PORTC
+#define PC1_DDR     DDRC
+#define PC1_PWM     nullptr
+
+#undef PC2
+#define PC2_PIN     PINC2
+#define PC2_RPORT   PINC
+#define PC2_WPORT   PORTC
+#define PC2_DDR     DDRC
+#define PC2_PWM     nullptr
+
+#undef PC3
+#define PC3_PIN     PINC3
+#define PC3_RPORT   PINC
+#define PC3_WPORT   PORTC
+#define PC3_DDR     DDRC
+#define PC3_PWM     nullptr
+
+#undef PC4
+#define PC4_PIN     PINC4
+#define PC4_RPORT   PINC
+#define PC4_WPORT   PORTC
+#define PC4_DDR     DDRC
+#define PC4_PWM     nullptr
+
+#undef PC5
+#define PC5_PIN     PINC5
+#define PC5_RPORT   PINC
+#define PC5_WPORT   PORTC
+#define PC5_DDR     DDRC
+#define PC5_PWM     nullptr
+
+#undef PC6
+#define PC6_PIN     PINC6
+#define PC6_RPORT   PINC
+#define PC6_WPORT   PORTC
+#define PC6_DDR     DDRC
+#define PC6_PWM     nullptr
+
+#undef PC7
+#define PC7_PIN     PINC7
+#define PC7_RPORT   PINC
+#define PC7_WPORT   PORTC
+#define PC7_DDR     DDRC
+#define PC7_PWM     nullptr
+
+#undef PD0
+#define PD0_PIN     PIND0
+#define PD0_RPORT   PIND
+#define PD0_WPORT   PORTD
+#define PD0_DDR     DDRD
+#define PD0_PWM     nullptr
+
+#undef PD1
+#define PD1_PIN     PIND1
+#define PD1_RPORT   PIND
+#define PD1_WPORT   PORTD
+#define PD1_DDR     DDRD
+#define PD1_PWM     nullptr
+
+#undef PD2
+#define PD2_PIN     PIND2
+#define PD2_RPORT   PIND
+#define PD2_WPORT   PORTD
+#define PD2_DDR     DDRD
+#define PD2_PWM     nullptr
+
+#undef PD3
+#define PD3_PIN     PIND3
+#define PD3_RPORT   PIND
+#define PD3_WPORT   PORTD
+#define PD3_DDR     DDRD
+#define PD3_PWM     &OCR2B
+
+#undef PD4
+#define PD4_PIN     PIND4
+#define PD4_RPORT   PIND
+#define PD4_WPORT   PORTD
+#define PD4_DDR     DDRD
+#define PD4_PWM     nullptr
+
+#undef PD5
+#define PD5_PIN     PIND5
+#define PD5_RPORT   PIND
+#define PD5_WPORT   PORTD
+#define PD5_DDR     DDRD
+#define PD5_PWM     &OCR0B
+
+#undef PD6
+#define PD6_PIN     PIND6
+#define PD6_RPORT   PIND
+#define PD6_WPORT   PORTD
+#define PD6_DDR     DDRD
+#define PD6_PWM     &OCR0A
+
+#undef PD7
+#define PD7_PIN     PIND7
+#define PD7_RPORT   PIND
+#define PD7_WPORT   PORTD
+#define PD7_DDR     DDRD
+#define PD7_PWM     nullptr
diff --git a/Marlin/src/HAL/AVR/fastio/fastio_644.h b/Marlin/src/HAL/AVR/fastio/fastio_644.h
new file mode 100644
index 0000000..f4a9427
--- /dev/null
+++ b/Marlin/src/HAL/AVR/fastio/fastio_644.h
@@ -0,0 +1,552 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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
+
+/**
+ * Pin mapping for the 644, 644p, 644pa, and 1284p
+ *
+ *   Logical Pin: 00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31
+ *   Port:        B0 B1 B2 B3 B4 B5 B6 B7 D0 D1 D2 D3 D4 D5 D6 D7 C0 C1 C2 C3 C4 C5 C6 C7 A7 A6 A5 A4 A3 A2 A1 A0
+ */
+
+/**                        ATMega644
+ *
+ *                        +---\/---+
+ *            (D 0) PB0  1|        |40  PA0 (AI 0 / D31)
+ *            (D 1) PB1  2|        |39  PA1 (AI 1 / D30)
+ *       INT2 (D 2) PB2  3|        |38  PA2 (AI 2 / D29)
+ *        PWM (D 3) PB3  4|        |37  PA3 (AI 3 / D28)
+ *        PWM (D 4) PB4  5|        |36  PA4 (AI 4 / D27)
+ *       MOSI (D 5) PB5  6|        |35  PA5 (AI 5 / D26)
+ *       MISO (D 6) PB6  7|        |34  PA6 (AI 6 / D25)
+ *        SCK (D 7) PB7  8|        |33  PA7 (AI 7 / D24)
+ *                  RST  9|        |32  AREF
+ *                  VCC 10|        |31  GND
+ *                  GND 11|        |30  AVCC
+ *                XTAL2 12|        |29  PC7 (D 23)
+ *                XTAL1 13|        |28  PC6 (D 22)
+ *       RX0 (D 8)  PD0 14|        |27  PC5 (D 21) TDI
+ *       TX0 (D 9)  PD1 15|        |26  PC4 (D 20) TDO
+ *  INT0 RX1 (D 10) PD2 16|        |25  PC3 (D 19) TMS
+ *  INT1 TX1 (D 11) PD3 17|        |24  PC2 (D 18) TCK
+ *       PWM (D 12) PD4 18|        |23  PC1 (D 17) SDA
+ *       PWM (D 13) PD5 19|        |22  PC0 (D 16) SCL
+ *       PWM (D 14) PD6 20|        |21  PD7 (D 15) PWM
+ *                        +--------+
+ */
+
+#include "../fastio.h"
+
+#define DEBUG_LED   DIO0
+
+// UART
+#define RXD         DIO8
+#define TXD         DIO9
+#define RXD0        DIO8
+#define TXD0        DIO9
+
+#define RXD1        DIO10
+#define TXD1        DIO11
+
+// SPI
+#define SCK         DIO7
+#define MISO        DIO6
+#define MOSI        DIO5
+#define SS          DIO4
+
+// TWI (I2C)
+#define SCL         DIO16
+#define SDA         DIO17
+
+// Timers and PWM
+#define OC0A        DIO3
+#define OC0B        DIO4
+#define OC1A        DIO13
+#define OC1B        DIO12
+#define OC2A        DIO15
+#define OC2B        DIO14
+
+// Digital I/O
+
+#define DIO0_PIN    PINB0
+#define DIO0_RPORT  PINB
+#define DIO0_WPORT  PORTB
+#define DIO0_DDR    DDRB
+#define DIO0_PWM    nullptr
+
+#define DIO1_PIN    PINB1
+#define DIO1_RPORT  PINB
+#define DIO1_WPORT  PORTB
+#define DIO1_DDR    DDRB
+#define DIO1_PWM    nullptr
+
+#define DIO2_PIN    PINB2
+#define DIO2_RPORT  PINB
+#define DIO2_WPORT  PORTB
+#define DIO2_DDR    DDRB
+#define DIO2_PWM    nullptr
+
+#define DIO3_PIN    PINB3
+#define DIO3_RPORT  PINB
+#define DIO3_WPORT  PORTB
+#define DIO3_DDR    DDRB
+#define DIO3_PWM    &OCR0A
+
+#define DIO4_PIN    PINB4
+#define DIO4_RPORT  PINB
+#define DIO4_WPORT  PORTB
+#define DIO4_DDR    DDRB
+#define DIO4_PWM    &OCR0B
+
+#define DIO5_PIN    PINB5
+#define DIO5_RPORT  PINB
+#define DIO5_WPORT  PORTB
+#define DIO5_DDR    DDRB
+#define DIO5_PWM    nullptr
+
+#define DIO6_PIN    PINB6
+#define DIO6_RPORT  PINB
+#define DIO6_WPORT  PORTB
+#define DIO6_DDR    DDRB
+#define DIO6_PWM    nullptr
+
+#define DIO7_PIN    PINB7
+#define DIO7_RPORT  PINB
+#define DIO7_WPORT  PORTB
+#define DIO7_DDR    DDRB
+#define DIO7_PWM    nullptr
+
+#define DIO8_PIN    PIND0
+#define DIO8_RPORT  PIND
+#define DIO8_WPORT  PORTD
+#define DIO8_DDR    DDRD
+#define DIO8_PWM    nullptr
+
+#define DIO9_PIN    PIND1
+#define DIO9_RPORT  PIND
+#define DIO9_WPORT  PORTD
+#define DIO9_DDR    DDRD
+#define DIO9_PWM    nullptr
+
+#define DIO10_PIN   PIND2
+#define DIO10_RPORT PIND
+#define DIO10_WPORT PORTD
+#define DIO10_DDR   DDRD
+#define DIO10_PWM   nullptr
+
+#define DIO11_PIN   PIND3
+#define DIO11_RPORT PIND
+#define DIO11_WPORT PORTD
+#define DIO11_DDR   DDRD
+#define DIO11_PWM   nullptr
+
+#define DIO12_PIN   PIND4
+#define DIO12_RPORT PIND
+#define DIO12_WPORT PORTD
+#define DIO12_DDR   DDRD
+#define DIO12_PWM   &OCR1B
+
+#define DIO13_PIN   PIND5
+#define DIO13_RPORT PIND
+#define DIO13_WPORT PORTD
+#define DIO13_DDR   DDRD
+#define DIO13_PWM   &OCR1A
+
+#define DIO14_PIN   PIND6
+#define DIO14_RPORT PIND
+#define DIO14_WPORT PORTD
+#define DIO14_DDR   DDRD
+#define DIO14_PWM   &OCR2B
+
+#define DIO15_PIN   PIND7
+#define DIO15_RPORT PIND
+#define DIO15_WPORT PORTD
+#define DIO15_DDR   DDRD
+#define DIO15_PWM   &OCR2A
+
+#define DIO16_PIN   PINC0
+#define DIO16_RPORT PINC
+#define DIO16_WPORT PORTC
+#define DIO16_DDR   DDRC
+#define DIO16_PWM   nullptr
+
+#define DIO17_PIN   PINC1
+#define DIO17_RPORT PINC
+#define DIO17_WPORT PORTC
+#define DIO17_DDR   DDRC
+#define DIO17_PWM   nullptr
+
+#define DIO18_PIN   PINC2
+#define DIO18_RPORT PINC
+#define DIO18_WPORT PORTC
+#define DIO18_DDR   DDRC
+#define DIO18_PWM   nullptr
+
+#define DIO19_PIN   PINC3
+#define DIO19_RPORT PINC
+#define DIO19_WPORT PORTC
+#define DIO19_DDR   DDRC
+#define DIO19_PWM   nullptr
+
+#define DIO20_PIN   PINC4
+#define DIO20_RPORT PINC
+#define DIO20_WPORT PORTC
+#define DIO20_DDR   DDRC
+#define DIO20_PWM   nullptr
+
+#define DIO21_PIN   PINC5
+#define DIO21_RPORT PINC
+#define DIO21_WPORT PORTC
+#define DIO21_DDR   DDRC
+#define DIO21_PWM   nullptr
+
+#define DIO22_PIN   PINC6
+#define DIO22_RPORT PINC
+#define DIO22_WPORT PORTC
+#define DIO22_DDR   DDRC
+#define DIO22_PWM   nullptr
+
+#define DIO23_PIN   PINC7
+#define DIO23_RPORT PINC
+#define DIO23_WPORT PORTC
+#define DIO23_DDR   DDRC
+#define DIO23_PWM   nullptr
+
+#define DIO24_PIN   PINA7
+#define DIO24_RPORT PINA
+#define DIO24_WPORT PORTA
+#define DIO24_DDR   DDRA
+#define DIO24_PWM   nullptr
+
+#define DIO25_PIN   PINA6
+#define DIO25_RPORT PINA
+#define DIO25_WPORT PORTA
+#define DIO25_DDR   DDRA
+#define DIO25_PWM   nullptr
+
+#define DIO26_PIN   PINA5
+#define DIO26_RPORT PINA
+#define DIO26_WPORT PORTA
+#define DIO26_DDR   DDRA
+#define DIO26_PWM   nullptr
+
+#define DIO27_PIN   PINA4
+#define DIO27_RPORT PINA
+#define DIO27_WPORT PORTA
+#define DIO27_DDR   DDRA
+#define DIO27_PWM   nullptr
+
+#define DIO28_PIN   PINA3
+#define DIO28_RPORT PINA
+#define DIO28_WPORT PORTA
+#define DIO28_DDR   DDRA
+#define DIO28_PWM   nullptr
+
+#define DIO29_PIN   PINA2
+#define DIO29_RPORT PINA
+#define DIO29_WPORT PORTA
+#define DIO29_DDR   DDRA
+#define DIO29_PWM   nullptr
+
+#define DIO30_PIN   PINA1
+#define DIO30_RPORT PINA
+#define DIO30_WPORT PORTA
+#define DIO30_DDR   DDRA
+#define DIO30_PWM   nullptr
+
+#define DIO31_PIN   PINA0
+#define DIO31_RPORT PINA
+#define DIO31_WPORT PORTA
+#define DIO31_DDR   DDRA
+#define DIO31_PWM   nullptr
+
+#define AIO0_PIN    PINA0
+#define AIO0_RPORT  PINA
+#define AIO0_WPORT  PORTA
+#define AIO0_DDR    DDRA
+#define AIO0_PWM    nullptr
+
+#define AIO1_PIN    PINA1
+#define AIO1_RPORT  PINA
+#define AIO1_WPORT  PORTA
+#define AIO1_DDR    DDRA
+#define AIO1_PWM    nullptr
+
+#define AIO2_PIN    PINA2
+#define AIO2_RPORT  PINA
+#define AIO2_WPORT  PORTA
+#define AIO2_DDR    DDRA
+#define AIO2_PWM    nullptr
+
+#define AIO3_PIN    PINA3
+#define AIO3_RPORT  PINA
+#define AIO3_WPORT  PORTA
+#define AIO3_DDR    DDRA
+#define AIO3_PWM    nullptr
+
+#define AIO4_PIN    PINA4
+#define AIO4_RPORT  PINA
+#define AIO4_WPORT  PORTA
+#define AIO4_DDR    DDRA
+#define AIO4_PWM    nullptr
+
+#define AIO5_PIN    PINA5
+#define AIO5_RPORT  PINA
+#define AIO5_WPORT  PORTA
+#define AIO5_DDR    DDRA
+#define AIO5_PWM    nullptr
+
+#define AIO6_PIN    PINA6
+#define AIO6_RPORT  PINA
+#define AIO6_WPORT  PORTA
+#define AIO6_DDR    DDRA
+#define AIO6_PWM    nullptr
+
+#define AIO7_PIN    PINA7
+#define AIO7_RPORT  PINA
+#define AIO7_WPORT  PORTA
+#define AIO7_DDR    DDRA
+#define AIO7_PWM    nullptr
+
+#undef PA0
+#define PA0_PIN     PINA0
+#define PA0_RPORT   PINA
+#define PA0_WPORT   PORTA
+#define PA0_DDR     DDRA
+#define PA0_PWM     nullptr
+
+#undef PA1
+#define PA1_PIN     PINA1
+#define PA1_RPORT   PINA
+#define PA1_WPORT   PORTA
+#define PA1_DDR     DDRA
+#define PA1_PWM     nullptr
+
+#undef PA2
+#define PA2_PIN     PINA2
+#define PA2_RPORT   PINA
+#define PA2_WPORT   PORTA
+#define PA2_DDR     DDRA
+#define PA2_PWM     nullptr
+
+#undef PA3
+#define PA3_PIN     PINA3
+#define PA3_RPORT   PINA
+#define PA3_WPORT   PORTA
+#define PA3_DDR     DDRA
+#define PA3_PWM     nullptr
+
+#undef PA4
+#define PA4_PIN     PINA4
+#define PA4_RPORT   PINA
+#define PA4_WPORT   PORTA
+#define PA4_DDR     DDRA
+#define PA4_PWM     nullptr
+
+#undef PA5
+#define PA5_PIN     PINA5
+#define PA5_RPORT   PINA
+#define PA5_WPORT   PORTA
+#define PA5_DDR     DDRA
+#define PA5_PWM     nullptr
+
+#undef PA6
+#define PA6_PIN     PINA6
+#define PA6_RPORT   PINA
+#define PA6_WPORT   PORTA
+#define PA6_DDR     DDRA
+#define PA6_PWM     nullptr
+
+#undef PA7
+#define PA7_PIN     PINA7
+#define PA7_RPORT   PINA
+#define PA7_WPORT   PORTA
+#define PA7_DDR     DDRA
+#define PA7_PWM     nullptr
+
+#undef PB0
+#define PB0_PIN     PINB0
+#define PB0_RPORT   PINB
+#define PB0_WPORT   PORTB
+#define PB0_DDR     DDRB
+#define PB0_PWM     nullptr
+
+#undef PB1
+#define PB1_PIN     PINB1
+#define PB1_RPORT   PINB
+#define PB1_WPORT   PORTB
+#define PB1_DDR     DDRB
+#define PB1_PWM     nullptr
+
+#undef PB2
+#define PB2_PIN     PINB2
+#define PB2_RPORT   PINB
+#define PB2_WPORT   PORTB
+#define PB2_DDR     DDRB
+#define PB2_PWM     nullptr
+
+#undef PB3
+#define PB3_PIN     PINB3
+#define PB3_RPORT   PINB
+#define PB3_WPORT   PORTB
+#define PB3_DDR     DDRB
+#define PB3_PWM     &OCR0A
+
+#undef PB4
+#define PB4_PIN     PINB4
+#define PB4_RPORT   PINB
+#define PB4_WPORT   PORTB
+#define PB4_DDR     DDRB
+#define PB4_PWM     &OCR0B
+
+#undef PB5
+#define PB5_PIN     PINB5
+#define PB5_RPORT   PINB
+#define PB5_WPORT   PORTB
+#define PB5_DDR     DDRB
+#define PB5_PWM     nullptr
+
+#undef PB6
+#define PB6_PIN     PINB6
+#define PB6_RPORT   PINB
+#define PB6_WPORT   PORTB
+#define PB6_DDR     DDRB
+#define PB6_PWM     nullptr
+
+#undef PB7
+#define PB7_PIN     PINB7
+#define PB7_RPORT   PINB
+#define PB7_WPORT   PORTB
+#define PB7_DDR     DDRB
+#define PB7_PWM     nullptr
+
+#undef PC0
+#define PC0_PIN     PINC0
+#define PC0_RPORT   PINC
+#define PC0_WPORT   PORTC
+#define PC0_DDR     DDRC
+#define PC0_PWM     nullptr
+
+#undef PC1
+#define PC1_PIN     PINC1
+#define PC1_RPORT   PINC
+#define PC1_WPORT   PORTC
+#define PC1_DDR     DDRC
+#define PC1_PWM     nullptr
+
+#undef PC2
+#define PC2_PIN     PINC2
+#define PC2_RPORT   PINC
+#define PC2_WPORT   PORTC
+#define PC2_DDR     DDRC
+#define PC2_PWM     nullptr
+
+#undef PC3
+#define PC3_PIN     PINC3
+#define PC3_RPORT   PINC
+#define PC3_WPORT   PORTC
+#define PC3_DDR     DDRC
+#define PC3_PWM     nullptr
+
+#undef PC4
+#define PC4_PIN     PINC4
+#define PC4_RPORT   PINC
+#define PC4_WPORT   PORTC
+#define PC4_DDR     DDRC
+#define PC4_PWM     nullptr
+
+#undef PC5
+#define PC5_PIN     PINC5
+#define PC5_RPORT   PINC
+#define PC5_WPORT   PORTC
+#define PC5_DDR     DDRC
+#define PC5_PWM     nullptr
+
+#undef PC6
+#define PC6_PIN     PINC6
+#define PC6_RPORT   PINC
+#define PC6_WPORT   PORTC
+#define PC6_DDR     DDRC
+#define PC6_PWM     nullptr
+
+#undef PC7
+#define PC7_PIN     PINC7
+#define PC7_RPORT   PINC
+#define PC7_WPORT   PORTC
+#define PC7_DDR     DDRC
+#define PC7_PWM     nullptr
+
+#undef PD0
+#define PD0_PIN     PIND0
+#define PD0_RPORT   PIND
+#define PD0_WPORT   PORTD
+#define PD0_DDR     DDRD
+#define PD0_PWM     nullptr
+
+#undef PD1
+#define PD1_PIN     PIND1
+#define PD1_RPORT   PIND
+#define PD1_WPORT   PORTD
+#define PD1_DDR     DDRD
+#define PD1_PWM     nullptr
+
+#undef PD2
+#define PD2_PIN     PIND2
+#define PD2_RPORT   PIND
+#define PD2_WPORT   PORTD
+#define PD2_DDR     DDRD
+#define PD2_PWM     nullptr
+
+#undef PD3
+#define PD3_PIN     PIND3
+#define PD3_RPORT   PIND
+#define PD3_WPORT   PORTD
+#define PD3_DDR     DDRD
+#define PD3_PWM     nullptr
+
+#undef PD4
+#define PD4_PIN     PIND4
+#define PD4_RPORT   PIND
+#define PD4_WPORT   PORTD
+#define PD4_DDR     DDRD
+#define PD4_PWM     nullptr
+
+#undef PD5
+#define PD5_PIN     PIND5
+#define PD5_RPORT   PIND
+#define PD5_WPORT   PORTD
+#define PD5_DDR     DDRD
+#define PD5_PWM     nullptr
+
+#undef PD6
+#define PD6_PIN     PIND6
+#define PD6_RPORT   PIND
+#define PD6_WPORT   PORTD
+#define PD6_DDR     DDRD
+#define PD6_PWM     &OCR2B
+
+#undef PD7
+#define PD7_PIN     PIND7
+#define PD7_RPORT   PIND
+#define PD7_WPORT   PORTD
+#define PD7_DDR     DDRD
+#define PD7_PWM     &OCR2A
diff --git a/Marlin/src/HAL/AVR/fastio/fastio_AT90USB.h b/Marlin/src/HAL/AVR/fastio/fastio_AT90USB.h
new file mode 100644
index 0000000..51d400b
--- /dev/null
+++ b/Marlin/src/HAL/AVR/fastio/fastio_AT90USB.h
@@ -0,0 +1,697 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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
+
+/**
+ * Pin mapping (Teensy) for AT90USB646, 647, 1286, and 1287
+ *
+ *   Logical Pin: 28 29 30 31 32 33 34 35 20 21 22 23 24 25 26 27 10 11 12 13 14 15 16 17 00 01 02 03 04 05 06 07 08 09(46*47)36 37 18 19 38 39 40 41 42 43 44 45
+ *   Port:        A0 A1 A2 A3 A4 A5 A6 A7 B0 B1 B2 B3 B4 B5 B6 B7 C0 C1 C2 C3 C4 C5 C6 C7 D0 D1 D2 D3 D4 D5 D6 D7 E0 E1 E2 E3 E4 E5 E6 E7 F0 F1 F2 F3 F4 F5 F6 F7
+ *            The logical pins 46 and 47 are not supported by Teensyduino, but are supported below as E2 and E3
+ */
+
+#include "../fastio.h"
+
+// change for your board
+#define DEBUG_LED   DIO31 /* led D5 red */
+
+// SPI
+#define SCK         DIO21  //  9
+#define MISO        DIO23  // 11
+#define MOSI        DIO22  // 10
+#define SS          DIO20  //  8
+
+// Digital I/O
+
+#define DIO0_PIN    PIND0
+#define DIO0_RPORT  PIND
+#define DIO0_WPORT  PORTD
+#define DIO0_PWM    0
+#define DIO0_DDR    DDRD
+
+#define DIO1_PIN    PIND1
+#define DIO1_RPORT  PIND
+#define DIO1_WPORT  PORTD
+#define DIO1_PWM    0
+#define DIO1_DDR    DDRD
+
+#define DIO2_PIN    PIND2
+#define DIO2_RPORT  PIND
+#define DIO2_WPORT  PORTD
+#define DIO2_PWM    0
+#define DIO2_DDR    DDRD
+
+#define DIO3_PIN    PIND3
+#define DIO3_RPORT  PIND
+#define DIO3_WPORT  PORTD
+#define DIO3_PWM    0
+#define DIO3_DDR    DDRD
+
+#define DIO4_PIN    PIND4
+#define DIO4_RPORT  PIND
+#define DIO4_WPORT  PORTD
+#define DIO4_PWM    0
+#define DIO4_DDR    DDRD
+
+#define DIO5_PIN    PIND5
+#define DIO5_RPORT  PIND
+#define DIO5_WPORT  PORTD
+#define DIO5_PWM    0
+#define DIO5_DDR    DDRD
+
+#define DIO6_PIN    PIND6
+#define DIO6_RPORT  PIND
+#define DIO6_WPORT  PORTD
+#define DIO6_PWM    0
+#define DIO6_DDR    DDRD
+
+#define DIO7_PIN    PIND7
+#define DIO7_RPORT  PIND
+#define DIO7_WPORT  PORTD
+#define DIO7_PWM    0
+#define DIO7_DDR    DDRD
+
+#define DIO8_PIN    PINE0
+#define DIO8_RPORT  PINE
+#define DIO8_WPORT  PORTE
+#define DIO8_PWM    0
+#define DIO8_DDR    DDRE
+
+#define DIO9_PIN    PINE1
+#define DIO9_RPORT  PINE
+#define DIO9_WPORT  PORTE
+#define DIO9_PWM    0
+#define DIO9_DDR    DDRE
+
+#define DIO10_PIN   PINC0
+#define DIO10_RPORT PINC
+#define DIO10_WPORT PORTC
+#define DIO10_PWM   0
+#define DIO10_DDR   DDRC
+
+#define DIO11_PIN   PINC1
+#define DIO11_RPORT PINC
+#define DIO11_WPORT PORTC
+#define DIO11_PWM   0
+#define DIO11_DDR   DDRC
+
+#define DIO12_PIN   PINC2
+#define DIO12_RPORT PINC
+#define DIO12_WPORT PORTC
+#define DIO12_PWM   0
+#define DIO12_DDR   DDRC
+
+#define DIO13_PIN   PINC3
+#define DIO13_RPORT PINC
+#define DIO13_WPORT PORTC
+#define DIO13_PWM   0
+#define DIO13_DDR   DDRC
+
+#define DIO14_PIN   PINC4
+#define DIO14_RPORT PINC
+#define DIO14_WPORT PORTC
+#define DIO14_PWM   0 // OC3C
+#define DIO14_DDR   DDRC
+
+#define DIO15_PIN   PINC5
+#define DIO15_RPORT PINC
+#define DIO15_WPORT PORTC
+#define DIO15_PWM   0 // OC3B
+#define DIO15_DDR   DDRC
+
+#define DIO16_PIN   PINC6
+#define DIO16_RPORT PINC
+#define DIO16_WPORT PORTC
+#define DIO16_PWM   0 // OC3A
+#define DIO16_DDR   DDRC
+
+#define DIO17_PIN   PINC7
+#define DIO17_RPORT PINC
+#define DIO17_WPORT PORTC
+#define DIO17_PWM   0
+#define DIO17_DDR   DDRC
+
+#define DIO18_PIN   PINE6
+#define DIO18_RPORT PINE
+#define DIO18_WPORT PORTE
+#define DIO18_PWM   0
+#define DIO18_DDR   DDRE
+
+#define DIO19_PIN   PINE7
+#define DIO19_RPORT PINE
+#define DIO19_WPORT PORTE
+#define DIO19_PWM   0
+#define DIO19_DDR   DDRE
+
+#define DIO20_PIN   PINB0
+#define DIO20_RPORT PINB
+#define DIO20_WPORT PORTB
+#define DIO20_PWM   0
+#define DIO20_DDR   DDRB
+
+#define DIO21_PIN   PINB1
+#define DIO21_RPORT PINB
+#define DIO21_WPORT PORTB
+#define DIO21_PWM   0
+#define DIO21_DDR   DDRB
+
+#define DIO22_PIN   PINB2
+#define DIO22_RPORT PINB
+#define DIO22_WPORT PORTB
+#define DIO22_PWM   0
+#define DIO22_DDR   DDRB
+
+#define DIO23_PIN   PINB3
+#define DIO23_RPORT PINB
+#define DIO23_WPORT PORTB
+#define DIO23_PWM   0
+#define DIO23_DDR   DDRB
+
+#define DIO24_PIN   PINB4
+#define DIO24_RPORT PINB
+#define DIO24_WPORT PORTB
+#define DIO24_PWM   0 // OC2A
+#define DIO24_DDR   DDRB
+
+#define DIO25_PIN   PINB5
+#define DIO25_RPORT PINB
+#define DIO25_WPORT PORTB
+#define DIO25_PWM   0 // OC1A
+#define DIO25_DDR   DDRB
+
+#define DIO26_PIN   PINB6
+#define DIO26_RPORT PINB
+#define DIO26_WPORT PORTB
+#define DIO26_PWM   0 // OC1B
+#define DIO26_DDR   DDRB
+
+#define DIO27_PIN   PINB7
+#define DIO27_RPORT PINB
+#define DIO27_WPORT PORTB
+#define DIO27_PWM   0 // OC1C
+#define DIO27_DDR   DDRB
+
+#define DIO28_PIN   PINA0
+#define DIO28_RPORT PINA
+#define DIO28_WPORT PORTA
+#define DIO28_PWM   0
+#define DIO28_DDR   DDRA
+
+#define DIO29_PIN   PINA1
+#define DIO29_RPORT PINA
+#define DIO29_WPORT PORTA
+#define DIO29_PWM   0
+#define DIO29_DDR   DDRA
+
+#define DIO30_PIN   PINA2
+#define DIO30_RPORT PINA
+#define DIO30_WPORT PORTA
+#define DIO30_PWM   0
+#define DIO30_DDR   DDRA
+
+#define DIO31_PIN   PINA3
+#define DIO31_RPORT PINA
+#define DIO31_WPORT PORTA
+#define DIO31_PWM   0
+#define DIO31_DDR   DDRA
+
+#define DIO32_PIN   PINA4
+#define DIO32_RPORT PINA
+#define DIO32_WPORT PORTA
+#define DIO32_PWM   0
+#define DIO32_DDR   DDRA
+
+#define DIO33_PIN   PINA5
+#define DIO33_RPORT PINA
+#define DIO33_WPORT PORTA
+#define DIO33_PWM   0
+#define DIO33_DDR   DDRA
+
+#define DIO34_PIN   PINA6
+#define DIO34_RPORT PINA
+#define DIO34_WPORT PORTA
+#define DIO34_PWM   0
+#define DIO34_DDR   DDRA
+
+#define DIO35_PIN   PINA7
+#define DIO35_RPORT PINA
+#define DIO35_WPORT PORTA
+#define DIO35_PWM   0
+#define DIO35_DDR   DDRA
+
+#define DIO36_PIN   PINE4
+#define DIO36_RPORT PINE
+#define DIO36_WPORT PORTE
+#define DIO36_PWM   0
+#define DIO36_DDR   DDRE
+
+#define DIO37_PIN   PINE5
+#define DIO37_RPORT PINE
+#define DIO37_WPORT PORTE
+#define DIO37_PWM   0
+#define DIO37_DDR   DDRE
+
+#define DIO38_PIN   PINF0
+#define DIO38_RPORT PINF
+#define DIO38_WPORT PORTF
+#define DIO38_PWM   0
+#define DIO38_DDR   DDRF
+
+#define DIO39_PIN   PINF1
+#define DIO39_RPORT PINF
+#define DIO39_WPORT PORTF
+#define DIO39_PWM   0
+#define DIO39_DDR   DDRF
+
+#define DIO40_PIN   PINF2
+#define DIO40_RPORT PINF
+#define DIO40_WPORT PORTF
+#define DIO40_PWM   0
+#define DIO40_DDR   DDRF
+
+#define DIO41_PIN   PINF3
+#define DIO41_RPORT PINF
+#define DIO41_WPORT PORTF
+#define DIO41_PWM   0
+#define DIO41_DDR   DDRF
+
+#define DIO42_PIN   PINF4
+#define DIO42_RPORT PINF
+#define DIO42_WPORT PORTF
+#define DIO42_PWM   0
+#define DIO42_DDR   DDRF
+
+#define DIO43_PIN   PINF5
+#define DIO43_RPORT PINF
+#define DIO43_WPORT PORTF
+#define DIO43_PWM   0
+#define DIO43_DDR   DDRF
+
+#define DIO44_PIN   PINF6
+#define DIO44_RPORT PINF
+#define DIO44_WPORT PORTF
+#define DIO44_PWM   0
+#define DIO44_DDR   DDRF
+
+#define DIO45_PIN   PINF7
+#define DIO45_RPORT PINF
+#define DIO45_WPORT PORTF
+#define DIO45_PWM   0
+#define DIO45_DDR   DDRF
+
+#define AIO0_PIN    PINF0
+#define AIO0_RPORT  PINF
+#define AIO0_WPORT  PORTF
+#define AIO0_PWM    0
+#define AIO0_DDR    DDRF
+
+#define AIO1_PIN    PINF1
+#define AIO1_RPORT  PINF
+#define AIO1_WPORT  PORTF
+#define AIO1_PWM    0
+#define AIO1_DDR    DDRF
+
+#define AIO2_PIN    PINF2
+#define AIO2_RPORT  PINF
+#define AIO2_WPORT  PORTF
+#define AIO2_PWM    0
+#define AIO2_DDR    DDRF
+
+#define AIO3_PIN    PINF3
+#define AIO3_RPORT  PINF
+#define AIO3_WPORT  PORTF
+#define AIO3_PWM    0
+#define AIO3_DDR    DDRF
+
+#define AIO4_PIN    PINF4
+#define AIO4_RPORT  PINF
+#define AIO4_WPORT  PORTF
+#define AIO4_PWM    0
+#define AIO4_DDR    DDRF
+
+#define AIO5_PIN    PINF5
+#define AIO5_RPORT  PINF
+#define AIO5_WPORT  PORTF
+#define AIO5_PWM    0
+#define AIO5_DDR    DDRF
+
+#define AIO6_PIN    PINF6
+#define AIO6_RPORT  PINF
+#define AIO6_WPORT  PORTF
+#define AIO6_PWM    0
+#define AIO6_DDR    DDRF
+
+#define AIO7_PIN    PINF7
+#define AIO7_RPORT  PINF
+#define AIO7_WPORT  PORTF
+#define AIO7_PWM    0
+#define AIO7_DDR    DDRF
+
+//-- Begin not supported by Teensyduino
+//-- don't use Arduino functions on these pins pinMode/digitalWrite/etc
+#define DIO46_PIN   PINE2
+#define DIO46_RPORT PINE
+#define DIO46_WPORT PORTE
+#define DIO46_PWM   0
+#define DIO46_DDR   DDRE
+
+#define DIO47_PIN   PINE3
+#define DIO47_RPORT PINE
+#define DIO47_WPORT PORTE
+#define DIO47_PWM   0
+#define DIO47_DDR   DDRE
+
+#define TEENSY_E2   46
+#define TEENSY_E3   47
+
+//-- end not supported by Teensyduino
+
+#undef PA0
+#define PA0_PIN     PINA0
+#define PA0_RPORT   PINA
+#define PA0_WPORT   PORTA
+#define PA0_PWM     0
+#define PA0_DDR     DDRA
+#undef PA1
+#define PA1_PIN     PINA1
+#define PA1_RPORT   PINA
+#define PA1_WPORT   PORTA
+#define PA1_PWM     0
+#define PA1_DDR     DDRA
+#undef PA2
+#define PA2_PIN     PINA2
+#define PA2_RPORT   PINA
+#define PA2_WPORT   PORTA
+#define PA2_PWM     0
+#define PA2_DDR     DDRA
+#undef PA3
+#define PA3_PIN     PINA3
+#define PA3_RPORT   PINA
+#define PA3_WPORT   PORTA
+#define PA3_PWM     0
+#define PA3_DDR     DDRA
+#undef PA4
+#define PA4_PIN     PINA4
+#define PA4_RPORT   PINA
+#define PA4_WPORT   PORTA
+#define PA4_PWM     0
+#define PA4_DDR     DDRA
+#undef PA5
+#define PA5_PIN     PINA5
+#define PA5_RPORT   PINA
+#define PA5_WPORT   PORTA
+#define PA5_PWM     0
+#define PA5_DDR     DDRA
+#undef PA6
+#define PA6_PIN     PINA6
+#define PA6_RPORT   PINA
+#define PA6_WPORT   PORTA
+#define PA6_PWM     0
+#define PA6_DDR     DDRA
+#undef PA7
+#define PA7_PIN     PINA7
+#define PA7_RPORT   PINA
+#define PA7_WPORT   PORTA
+#define PA7_PWM     0
+#define PA7_DDR     DDRA
+
+#undef PB0
+#define PB0_PIN     PINB0
+#define PB0_RPORT   PINB
+#define PB0_WPORT   PORTB
+#define PB0_PWM     0
+#define PB0_DDR     DDRB
+#undef PB1
+#define PB1_PIN     PINB1
+#define PB1_RPORT   PINB
+#define PB1_WPORT   PORTB
+#define PB1_PWM     0
+#define PB1_DDR     DDRB
+#undef PB2
+#define PB2_PIN     PINB2
+#define PB2_RPORT   PINB
+#define PB2_WPORT   PORTB
+#define PB2_PWM     0
+#define PB2_DDR     DDRB
+#undef PB3
+#define PB3_PIN     PINB3
+#define PB3_RPORT   PINB
+#define PB3_WPORT   PORTB
+#define PB3_PWM     0
+#define PB3_DDR     DDRB
+#undef PB4
+#define PB4_PIN     PINB4
+#define PB4_RPORT   PINB
+#define PB4_WPORT   PORTB
+#define PB4_PWM     0
+#define PB4_DDR     DDRB
+#undef PB5
+#define PB5_PIN     PINB5
+#define PB5_RPORT   PINB
+#define PB5_WPORT   PORTB
+#define PB5_PWM     0
+#define PB5_DDR     DDRB
+#undef PB6
+#define PB6_PIN     PINB6
+#define PB6_RPORT   PINB
+#define PB6_WPORT   PORTB
+#define PB6_PWM     0
+#define PB6_DDR     DDRB
+#undef PB7
+#define PB7_PIN     PINB7
+#define PB7_RPORT   PINB
+#define PB7_WPORT   PORTB
+#define PB7_PWM     0
+#define PB7_DDR     DDRB
+
+#undef PC0
+#define PC0_PIN     PINC0
+#define PC0_RPORT   PINC
+#define PC0_WPORT   PORTC
+#define PC0_PWM     0
+#define PC0_DDR     DDRC
+#undef PC1
+#define PC1_PIN     PINC1
+#define PC1_RPORT   PINC
+#define PC1_WPORT   PORTC
+#define PC1_PWM     0
+#define PC1_DDR     DDRC
+#undef PC2
+#define PC2_PIN     PINC2
+#define PC2_RPORT   PINC
+#define PC2_WPORT   PORTC
+#define PC2_PWM     0
+#define PC2_DDR     DDRC
+#undef PC3
+#define PC3_PIN     PINC3
+#define PC3_RPORT   PINC
+#define PC3_WPORT   PORTC
+#define PC3_PWM     0
+#define PC3_DDR     DDRC
+#undef PC4
+#define PC4_PIN     PINC4
+#define PC4_RPORT   PINC
+#define PC4_WPORT   PORTC
+#define PC4_PWM     0
+#define PC4_DDR     DDRC
+#undef PC5
+#define PC5_PIN     PINC5
+#define PC5_RPORT   PINC
+#define PC5_WPORT   PORTC
+#define PC5_PWM     0
+#define PC5_DDR     DDRC
+#undef PC6
+#define PC6_PIN     PINC6
+#define PC6_RPORT   PINC
+#define PC6_WPORT   PORTC
+#define PC6_PWM     0
+#define PC6_DDR     DDRC
+#undef PC7
+#define PC7_PIN     PINC7
+#define PC7_RPORT   PINC
+#define PC7_WPORT   PORTC
+#define PC7_PWM     0
+#define PC7_DDR     DDRC
+
+#undef PD0
+#define PD0_PIN     PIND0
+#define PD0_RPORT   PIND
+#define PD0_WPORT   PORTD
+#define PD0_PWM     0 // OC0B
+#define PD0_DDR     DDRD
+#undef PD1
+#define PD1_PIN     PIND1
+#define PD1_RPORT   PIND
+#define PD1_WPORT   PORTD
+#define PD1_PWM     0 // OC2B
+#define PD1_DDR     DDRD
+#undef PD2
+#define PD2_PIN     PIND2
+#define PD2_RPORT   PIND
+#define PD2_WPORT   PORTD
+#define PD2_PWM     0
+#define PD2_DDR     DDRD
+#undef PD3
+#define PD3_PIN     PIND3
+#define PD3_RPORT   PIND
+#define PD3_WPORT   PORTD
+#define PD3_PWM     0
+#define PD3_DDR     DDRD
+#undef PD4
+#define PD4_PIN     PIND4
+#define PD4_RPORT   PIND
+#define PD4_WPORT   PORTD
+#define PD4_PWM     0
+#define PD4_DDR     DDRD
+#undef PD5
+#define PD5_PIN     PIND5
+#define PD5_RPORT   PIND
+#define PD5_WPORT   PORTD
+#define PD5_PWM     0
+#define PD5_DDR     DDRD
+#undef PD6
+#define PD6_PIN     PIND6
+#define PD6_RPORT   PIND
+#define PD6_WPORT   PORTD
+#define PD6_PWM     0
+#define PD6_DDR     DDRD
+#undef PD7
+#define PD7_PIN     PIND7
+#define PD7_RPORT   PIND
+#define PD7_WPORT   PORTD
+#define PD7_PWM     0
+#define PD7_DDR     DDRD
+
+#undef PE0
+#define PE0_PIN     PINE0
+#define PE0_RPORT   PINE
+#define PE0_WPORT   PORTE
+#define PE0_PWM     0
+#define PE0_DDR     DDRE
+#undef PE1
+#define PE1_PIN     PINE1
+#define PE1_RPORT   PINE
+#define PE1_WPORT   PORTE
+#define PE1_PWM     0
+#define PE1_DDR     DDRE
+#undef PE2
+#define PE2_PIN     PINE2
+#define PE2_RPORT   PINE
+#define PE2_WPORT   PORTE
+#define PE2_PWM     0
+#define PE2_DDR     DDRE
+#undef PE3
+#define PE3_PIN     PINE3
+#define PE3_RPORT   PINE
+#define PE3_WPORT   PORTE
+#define PE3_PWM     0
+#define PE3_DDR     DDRE
+#undef PE4
+#define PE4_PIN     PINE4
+#define PE4_RPORT   PINE
+#define PE4_WPORT   PORTE
+#define PE4_PWM     0
+#define PE4_DDR     DDRE
+#undef PE5
+#define PE5_PIN     PINE5
+#define PE5_RPORT   PINE
+#define PE5_WPORT   PORTE
+#define PE5_PWM     0
+#define PE5_DDR     DDRE
+#undef PE6
+#define PE6_PIN     PINE6
+#define PE6_RPORT   PINE
+#define PE6_WPORT   PORTE
+#define PE6_PWM     0
+#define PE6_DDR     DDRE
+#undef PE7
+#define PE7_PIN     PINE7
+#define PE7_RPORT   PINE
+#define PE7_WPORT   PORTE
+#define PE7_PWM     0
+#define PE7_DDR     DDRE
+
+#undef PF0
+#define PF0_PIN     PINF0
+#define PF0_RPORT   PINF
+#define PF0_WPORT   PORTF
+#define PF0_PWM     0
+#define PF0_DDR     DDRF
+#undef PF1
+#define PF1_PIN     PINF1
+#define PF1_RPORT   PINF
+#define PF1_WPORT   PORTF
+#define PF1_PWM     0
+#define PF1_DDR     DDRF
+#undef PF2
+#define PF2_PIN     PINF2
+#define PF2_RPORT   PINF
+#define PF2_WPORT   PORTF
+#define PF2_PWM     0
+#define PF2_DDR     DDRF
+#undef PF3
+#define PF3_PIN     PINF3
+#define PF3_RPORT   PINF
+#define PF3_WPORT   PORTF
+#define PF3_PWM     0
+#define PF3_DDR     DDRF
+#undef PF4
+#define PF4_PIN     PINF4
+#define PF4_RPORT   PINF
+#define PF4_WPORT   PORTF
+#define PF4_PWM     0
+#define PF4_DDR     DDRF
+#undef PF5
+#define PF5_PIN     PINF5
+#define PF5_RPORT   PINF
+#define PF5_WPORT   PORTF
+#define PF5_PWM     0
+#define PF5_DDR     DDRF
+#undef PF6
+#define PF6_PIN     PINF6
+#define PF6_RPORT   PINF
+#define PF6_WPORT   PORTF
+#define PF6_PWM     0
+#define PF6_DDR     DDRF
+#undef PF7
+#define PF7_PIN     PINF7
+#define PF7_RPORT   PINF
+#define PF7_WPORT   PORTF
+#define PF7_PWM     0
+#define PF7_DDR     DDRF
+
+
+/**
+ * Some of the pin mapping functions of the Teensduino extension to the Arduino IDE
+ * do not function the same as the other Arduino extensions.
+ */
+
+//digitalPinToTimer(pin) function works like Arduino but Timers are not defined
+#define TIMER0B 1
+#define TIMER1A 7
+#define TIMER1B 8
+#define TIMER1C 9
+#define TIMER2A 6
+#define TIMER2B 2
+#define TIMER3A 5
+#define TIMER3B 4
+#define TIMER3C 3
diff --git a/Marlin/src/HAL/AVR/inc/Conditionals_LCD.h b/Marlin/src/HAL/AVR/inc/Conditionals_LCD.h
new file mode 100644
index 0000000..a611ccd
--- /dev/null
+++ b/Marlin/src/HAL/AVR/inc/Conditionals_LCD.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
+
+#if HAS_SPI_TFT || HAS_FSMC_TFT
+  #error "Sorry! TFT displays are not available for HAL/AVR."
+#endif
diff --git a/Marlin/src/HAL/AVR/inc/Conditionals_adv.h b/Marlin/src/HAL/AVR/inc/Conditionals_adv.h
new file mode 100644
index 0000000..5f1c4b1
--- /dev/null
+++ b/Marlin/src/HAL/AVR/inc/Conditionals_adv.h
@@ -0,0 +1,22 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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
diff --git a/Marlin/src/HAL/AVR/inc/Conditionals_post.h b/Marlin/src/HAL/AVR/inc/Conditionals_post.h
new file mode 100644
index 0000000..5f1c4b1
--- /dev/null
+++ b/Marlin/src/HAL/AVR/inc/Conditionals_post.h
@@ -0,0 +1,22 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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
diff --git a/Marlin/src/HAL/AVR/inc/SanityCheck.h b/Marlin/src/HAL/AVR/inc/SanityCheck.h
new file mode 100644
index 0000000..731cf92
--- /dev/null
+++ b/Marlin/src/HAL/AVR/inc/SanityCheck.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
+
+/**
+ * Test AVR-specific configuration values for errors at compile-time.
+ */
+
+/**
+ * Checks for FAST PWM
+ */
+#if ENABLED(FAST_PWM_FAN) && (ENABLED(USE_OCR2A_AS_TOP) && defined(TCCR2))
+  #error "USE_OCR2A_AS_TOP does not apply to devices with a single output TIMER2"
+#endif
+
+/**
+ * Sanity checks for Spindle / Laser PWM
+ */
+#if ENABLED(SPINDLE_LASER_PWM)
+  #include "../ServoTimers.h"   // Needed to check timer availability (_useTimer3)
+  #if SPINDLE_LASER_PWM_PIN == 4 || WITHIN(SPINDLE_LASER_PWM_PIN, 11, 13)
+    #error "Counter/Timer for SPINDLE_LASER_PWM_PIN is used by a system interrupt."
+  #elif NUM_SERVOS > 0 && defined(_useTimer3) && (WITHIN(SPINDLE_LASER_PWM_PIN, 2, 3) || SPINDLE_LASER_PWM_PIN == 5)
+    #error "Counter/Timer for SPINDLE_LASER_PWM_PIN is used by the servo system."
+  #endif
+#elif defined(SPINDLE_LASER_FREQUENCY)
+  #error "SPINDLE_LASER_FREQUENCY requires SPINDLE_LASER_PWM."
+#endif
+
+/**
+ * The Trinamic library includes SoftwareSerial.h, leading to a compile error.
+ */
+#if BOTH(HAS_TRINAMIC_CONFIG, ENDSTOP_INTERRUPTS_FEATURE)
+  #error "TMCStepper includes SoftwareSerial.h which is incompatible with ENDSTOP_INTERRUPTS_FEATURE. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."
+#endif
+
+#if BOTH(HAS_TMC_SW_SERIAL, MONITOR_DRIVER_STATUS)
+  #error "MONITOR_DRIVER_STATUS causes performance issues when used with SoftwareSerial-connected drivers. Disable MONITOR_DRIVER_STATUS or use hardware serial to continue."
+#endif
diff --git a/Marlin/src/HAL/AVR/math.h b/Marlin/src/HAL/AVR/math.h
new file mode 100644
index 0000000..7ede4ac
--- /dev/null
+++ b/Marlin/src/HAL/AVR/math.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
+
+/**
+ * Optimized math functions for AVR
+ */
+
+// intRes = longIn1 * longIn2 >> 24
+// uses:
+// A[tmp] to store 0
+// B[tmp] to store bits 16-23 of the 48bit result. The top bit is used to round the two byte result.
+// note that the lower two bytes and the upper byte of the 48bit result are not calculated.
+// this can cause the result to be out by one as the lower bytes may cause carries into the upper ones.
+// B A are bits 24-39 and are the returned value
+// C B A is longIn1
+// D C B A is longIn2
+//
+static FORCE_INLINE uint16_t MultiU24X32toH16(uint32_t longIn1, uint32_t longIn2) {
+  uint8_t tmp1;
+  uint8_t tmp2;
+  uint16_t intRes;
+  __asm__ __volatile__(
+    A("clr %[tmp1]")
+    A("mul %A[longIn1], %B[longIn2]")
+    A("mov %[tmp2], r1")
+    A("mul %B[longIn1], %C[longIn2]")
+    A("movw %A[intRes], r0")
+    A("mul %C[longIn1], %C[longIn2]")
+    A("add %B[intRes], r0")
+    A("mul %C[longIn1], %B[longIn2]")
+    A("add %A[intRes], r0")
+    A("adc %B[intRes], r1")
+    A("mul %A[longIn1], %C[longIn2]")
+    A("add %[tmp2], r0")
+    A("adc %A[intRes], r1")
+    A("adc %B[intRes], %[tmp1]")
+    A("mul %B[longIn1], %B[longIn2]")
+    A("add %[tmp2], r0")
+    A("adc %A[intRes], r1")
+    A("adc %B[intRes], %[tmp1]")
+    A("mul %C[longIn1], %A[longIn2]")
+    A("add %[tmp2], r0")
+    A("adc %A[intRes], r1")
+    A("adc %B[intRes], %[tmp1]")
+    A("mul %B[longIn1], %A[longIn2]")
+    A("add %[tmp2], r1")
+    A("adc %A[intRes], %[tmp1]")
+    A("adc %B[intRes], %[tmp1]")
+    A("lsr %[tmp2]")
+    A("adc %A[intRes], %[tmp1]")
+    A("adc %B[intRes], %[tmp1]")
+    A("mul %D[longIn2], %A[longIn1]")
+    A("add %A[intRes], r0")
+    A("adc %B[intRes], r1")
+    A("mul %D[longIn2], %B[longIn1]")
+    A("add %B[intRes], r0")
+    A("clr r1")
+      : [intRes] "=&r" (intRes),
+        [tmp1] "=&r" (tmp1),
+        [tmp2] "=&r" (tmp2)
+      : [longIn1] "d" (longIn1),
+        [longIn2] "d" (longIn2)
+      : "cc"
+  );
+  return intRes;
+}
+
+// intRes = intIn1 * intIn2 >> 16
+// uses:
+// r26 to store 0
+// r27 to store the byte 1 of the 24 bit result
+static FORCE_INLINE uint16_t MultiU16X8toH16(uint8_t charIn1, uint16_t intIn2) {
+  uint8_t tmp;
+  uint16_t intRes;
+  __asm__ __volatile__ (
+    A("clr %[tmp]")
+    A("mul %[charIn1], %B[intIn2]")
+    A("movw %A[intRes], r0")
+    A("mul %[charIn1], %A[intIn2]")
+    A("add %A[intRes], r1")
+    A("adc %B[intRes], %[tmp]")
+    A("lsr r0")
+    A("adc %A[intRes], %[tmp]")
+    A("adc %B[intRes], %[tmp]")
+    A("clr r1")
+      : [intRes] "=&r" (intRes),
+        [tmp] "=&r" (tmp)
+      : [charIn1] "d" (charIn1),
+        [intIn2] "d" (intIn2)
+      : "cc"
+  );
+  return intRes;
+}
diff --git a/Marlin/src/HAL/AVR/pinsDebug.h b/Marlin/src/HAL/AVR/pinsDebug.h
new file mode 100644
index 0000000..dac6b1b
--- /dev/null
+++ b/Marlin/src/HAL/AVR/pinsDebug.h
@@ -0,0 +1,403 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * PWM print routines for Atmel 8 bit AVR CPUs
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#define NUMBER_PINS_TOTAL NUM_DIGITAL_PINS
+
+#if MB(BQ_ZUM_MEGA_3D, MIGHTYBOARD_REVE, MINIRAMBO, SCOOVO_X9H, TRIGORILLA_14)
+  #define AVR_ATmega2560_FAMILY_PLUS_70 1
+#endif
+
+#if AVR_AT90USB1286_FAMILY
+
+  // Working with Teensyduino extension so need to re-define some things
+  #include "pinsDebug_Teensyduino.h"
+  // Can't use the "digitalPinToPort" function from the Teensyduino type IDEs
+  // portModeRegister takes a different argument
+  #define digitalPinToTimer_DEBUG(p) digitalPinToTimer(p)
+  #define digitalPinToBitMask_DEBUG(p) digitalPinToBitMask(p)
+  #define digitalPinToPort_DEBUG(p) digitalPinToPort_Teensy(p)
+  #define GET_PINMODE(pin) (*portModeRegister(pin) & digitalPinToBitMask_DEBUG(pin))
+
+#elif AVR_ATmega2560_FAMILY_PLUS_70   // So we can access/display all the pins on boards using more than 70
+
+  #include "pinsDebug_plus_70.h"
+  #define digitalPinToTimer_DEBUG(p) digitalPinToTimer_plus_70(p)
+  #define digitalPinToBitMask_DEBUG(p) digitalPinToBitMask_plus_70(p)
+  #define digitalPinToPort_DEBUG(p) digitalPinToPort_plus_70(p)
+  bool GET_PINMODE(int8_t pin) {return *portModeRegister(digitalPinToPort_DEBUG(pin)) & digitalPinToBitMask_DEBUG(pin); }
+
+#else
+
+  #define digitalPinToTimer_DEBUG(p) digitalPinToTimer(p)
+  #define digitalPinToBitMask_DEBUG(p) digitalPinToBitMask(p)
+  #define digitalPinToPort_DEBUG(p) digitalPinToPort(p)
+  bool GET_PINMODE(int8_t pin) {return *portModeRegister(digitalPinToPort_DEBUG(pin)) & digitalPinToBitMask_DEBUG(pin); }
+  #define GET_ARRAY_PIN(p) pgm_read_byte(&pin_array[p].pin)
+
+#endif
+
+#define VALID_PIN(pin) (pin >= 0 && pin < NUM_DIGITAL_PINS ? 1 : 0)
+#if AVR_ATmega1284_FAMILY
+  #define DIGITAL_PIN_TO_ANALOG_PIN(P) int(analogInputToDigitalPin(0) - (P))
+  #define IS_ANALOG(P) ((P) >= analogInputToDigitalPin(7) && (P) <= analogInputToDigitalPin(0))
+#else
+  #define DIGITAL_PIN_TO_ANALOG_PIN(P) int((P) - analogInputToDigitalPin(0))
+  #define IS_ANALOG(P) ((P) >= analogInputToDigitalPin(0) && ((P) <= analogInputToDigitalPin(15) || (P) <= analogInputToDigitalPin(7)))
+#endif
+#define GET_ARRAY_PIN(p) pgm_read_byte(&pin_array[p].pin)
+#define MULTI_NAME_PAD 26 // space needed to be pretty if not first name assigned to a pin
+
+void PRINT_ARRAY_NAME(uint8_t x) {
+  char *name_mem_pointer = (char*)pgm_read_ptr(&pin_array[x].name);
+  LOOP_L_N(y, MAX_NAME_LENGTH) {
+    char temp_char = pgm_read_byte(name_mem_pointer + y);
+    if (temp_char != 0)
+      SERIAL_CHAR(temp_char);
+    else {
+      LOOP_L_N(i, MAX_NAME_LENGTH - y) SERIAL_CHAR(' ');
+      break;
+    }
+  }
+}
+
+#define GET_ARRAY_IS_DIGITAL(x)   pgm_read_byte(&pin_array[x].is_digital)
+
+
+#if defined(__AVR_ATmega1284P__)  // 1284 IDE extensions set this to the number of
+  #undef NUM_DIGITAL_PINS         // digital only pins while all other CPUs have it
+  #define NUM_DIGITAL_PINS 32     // set to digital only + digital/analog
+#endif
+
+#define PWM_PRINT(V) do{ sprintf_P(buffer, PSTR("PWM:  %4d"), V); SERIAL_ECHO(buffer); }while(0)
+#define PWM_CASE(N,Z)                                           \
+  case TIMER##N##Z:                                             \
+    if (TCCR##N##A & (_BV(COM##N##Z##1) | _BV(COM##N##Z##0))) { \
+      PWM_PRINT(OCR##N##Z);                                     \
+      return true;                                              \
+    } else return false
+
+
+
+/**
+ * Print a pin's PWM status.
+ * Return true if it's currently a PWM pin.
+ */
+static bool pwm_status(uint8_t pin) {
+  char buffer[20];   // for the sprintf statements
+
+  switch (digitalPinToTimer_DEBUG(pin)) {
+
+    #if defined(TCCR0A) && defined(COM0A1)
+      #ifdef TIMER0A
+        #if !AVR_AT90USB1286_FAMILY  // not available in Teensyduino type IDEs
+          PWM_CASE(0, A);
+        #endif
+      #endif
+      PWM_CASE(0, B);
+    #endif
+
+    #if defined(TCCR1A) && defined(COM1A1)
+      PWM_CASE(1, A);
+      PWM_CASE(1, B);
+     #if defined(COM1C1) && defined(TIMER1C)
+      PWM_CASE(1, C);
+     #endif
+    #endif
+
+    #if defined(TCCR2A) && defined(COM2A1)
+      PWM_CASE(2, A);
+      PWM_CASE(2, B);
+    #endif
+
+    #if defined(TCCR3A) && defined(COM3A1)
+      PWM_CASE(3, A);
+      PWM_CASE(3, B);
+      #ifdef COM3C1
+        PWM_CASE(3, C);
+      #endif
+    #endif
+
+    #ifdef TCCR4A
+      PWM_CASE(4, A);
+      PWM_CASE(4, B);
+      PWM_CASE(4, C);
+    #endif
+
+    #if defined(TCCR5A) && defined(COM5A1)
+      PWM_CASE(5, A);
+      PWM_CASE(5, B);
+      PWM_CASE(5, C);
+    #endif
+
+    case NOT_ON_TIMER:
+    default:
+      return false;
+  }
+  SERIAL_ECHO_SP(2);
+} // pwm_status
+
+
+const volatile uint8_t* const PWM_other[][3] PROGMEM = {
+    { &TCCR0A, &TCCR0B, &TIMSK0 },
+    { &TCCR1A, &TCCR1B, &TIMSK1 },
+  #if defined(TCCR2A) && defined(COM2A1)
+    { &TCCR2A, &TCCR2B, &TIMSK2 },
+  #endif
+  #if defined(TCCR3A) && defined(COM3A1)
+    { &TCCR3A, &TCCR3B, &TIMSK3 },
+  #endif
+  #ifdef TCCR4A
+    { &TCCR4A, &TCCR4B, &TIMSK4 },
+  #endif
+  #if defined(TCCR5A) && defined(COM5A1)
+    { &TCCR5A, &TCCR5B, &TIMSK5 },
+  #endif
+};
+
+
+const volatile uint8_t* const PWM_OCR[][3] PROGMEM = {
+
+  #ifdef TIMER0A
+    { &OCR0A, &OCR0B, 0 },
+  #else
+    { 0, &OCR0B, 0 },
+  #endif
+
+  #if defined(COM1C1) && defined(TIMER1C)
+   { (const uint8_t*)&OCR1A, (const uint8_t*)&OCR1B, (const uint8_t*)&OCR1C },
+  #else
+   { (const uint8_t*)&OCR1A, (const uint8_t*)&OCR1B, 0 },
+  #endif
+
+  #if defined(TCCR2A) && defined(COM2A1)
+    { &OCR2A, &OCR2B, 0 },
+  #endif
+
+  #if defined(TCCR3A) && defined(COM3A1)
+    #ifdef COM3C1
+      { (const uint8_t*)&OCR3A, (const uint8_t*)&OCR3B, (const uint8_t*)&OCR3C },
+    #else
+      { (const uint8_t*)&OCR3A, (const uint8_t*)&OCR3B, 0 },
+    #endif
+  #endif
+
+  #ifdef TCCR4A
+    { (const uint8_t*)&OCR4A, (const uint8_t*)&OCR4B, (const uint8_t*)&OCR4C },
+  #endif
+
+  #if defined(TCCR5A) && defined(COM5A1)
+    { (const uint8_t*)&OCR5A, (const uint8_t*)&OCR5B, (const uint8_t*)&OCR5C },
+  #endif
+};
+
+
+#define TCCR_A(T)   pgm_read_word(&PWM_other[T][0])
+#define TCCR_B(T)   pgm_read_word(&PWM_other[T][1])
+#define TIMSK(T)    pgm_read_word(&PWM_other[T][2])
+#define CS_0       0
+#define CS_1       1
+#define CS_2       2
+#define WGM_0      0
+#define WGM_1      1
+#define WGM_2      3
+#define WGM_3      4
+#define TOIE       0
+
+#define OCR_VAL(T, L)   pgm_read_word(&PWM_OCR[T][L])
+
+static void err_is_counter()     { SERIAL_ECHOPGM("   non-standard PWM mode"); }
+static void err_is_interrupt()   { SERIAL_ECHOPGM("   compare interrupt enabled"); }
+static void err_prob_interrupt() { SERIAL_ECHOPGM("   overflow interrupt enabled"); }
+static void print_is_also_tied() { SERIAL_ECHOPGM(" is also tied to this pin"); SERIAL_ECHO_SP(14); }
+
+inline void com_print(const uint8_t N, const uint8_t Z) {
+  const uint8_t *TCCRA = (uint8_t*)TCCR_A(N);
+  SERIAL_ECHOPGM("    COM");
+  SERIAL_CHAR('0' + N, Z);
+  SERIAL_ECHOPAIR(": ", int((*TCCRA >> (6 - Z * 2)) & 0x03));
+}
+
+void timer_prefix(uint8_t T, char L, uint8_t N) {  // T - timer    L - pwm  N - WGM bit layout
+  char buffer[20];   // for the sprintf statements
+  const uint8_t *TCCRB = (uint8_t*)TCCR_B(T),
+                *TCCRA = (uint8_t*)TCCR_A(T);
+  uint8_t WGM = (((*TCCRB & _BV(WGM_2)) >> 1) | (*TCCRA & (_BV(WGM_0) | _BV(WGM_1))));
+  if (N == 4) WGM |= ((*TCCRB & _BV(WGM_3)) >> 1);
+
+  SERIAL_ECHOPGM("    TIMER");
+  SERIAL_CHAR(T + '0', L);
+  SERIAL_ECHO_SP(3);
+
+  if (N == 3) {
+    const uint8_t *OCRVAL8 = (uint8_t*)OCR_VAL(T, L - 'A');
+    PWM_PRINT(*OCRVAL8);
+  }
+  else {
+    const uint16_t *OCRVAL16 = (uint16_t*)OCR_VAL(T, L - 'A');
+    PWM_PRINT(*OCRVAL16);
+  }
+  SERIAL_ECHOPAIR("    WGM: ", WGM);
+  com_print(T,L);
+  SERIAL_ECHOPAIR("    CS: ", (*TCCRB & (_BV(CS_0) | _BV(CS_1) | _BV(CS_2)) ));
+
+  SERIAL_ECHOPGM("    TCCR");
+  SERIAL_CHAR(T + '0');
+  SERIAL_ECHOPAIR("A: ", *TCCRA);
+
+  SERIAL_ECHOPGM("    TCCR");
+  SERIAL_CHAR(T + '0');
+  SERIAL_ECHOPAIR("B: ", *TCCRB);
+
+  const uint8_t *TMSK = (uint8_t*)TIMSK(T);
+  SERIAL_ECHOPGM("    TIMSK");
+  SERIAL_CHAR(T + '0');
+  SERIAL_ECHOPAIR(": ", *TMSK);
+
+  const uint8_t OCIE = L - 'A' + 1;
+  if (N == 3) { if (WGM == 0 || WGM == 2 || WGM ==  4 || WGM ==  6) err_is_counter(); }
+  else        { if (WGM == 0 || WGM == 4 || WGM == 12 || WGM == 13) err_is_counter(); }
+  if (TEST(*TMSK, OCIE)) err_is_interrupt();
+  if (TEST(*TMSK, TOIE)) err_prob_interrupt();
+}
+
+static void pwm_details(uint8_t pin) {
+  switch (digitalPinToTimer_DEBUG(pin)) {
+
+    #if defined(TCCR0A) && defined(COM0A1)
+      #ifdef TIMER0A
+        #if !AVR_AT90USB1286_FAMILY  // not available in Teensyduino type IDEs
+          case TIMER0A: timer_prefix(0, 'A', 3); break;
+        #endif
+      #endif
+      case TIMER0B: timer_prefix(0, 'B', 3); break;
+    #endif
+
+    #if defined(TCCR1A) && defined(COM1A1)
+      case TIMER1A: timer_prefix(1, 'A', 4); break;
+      case TIMER1B: timer_prefix(1, 'B', 4); break;
+      #if defined(COM1C1) && defined(TIMER1C)
+        case TIMER1C: timer_prefix(1, 'C', 4); break;
+      #endif
+    #endif
+
+    #if defined(TCCR2A) && defined(COM2A1)
+      case TIMER2A: timer_prefix(2, 'A', 3); break;
+      case TIMER2B: timer_prefix(2, 'B', 3); break;
+    #endif
+
+    #if defined(TCCR3A) && defined(COM3A1)
+      case TIMER3A: timer_prefix(3, 'A', 4); break;
+      case TIMER3B: timer_prefix(3, 'B', 4); break;
+      #ifdef COM3C1
+        case TIMER3C: timer_prefix(3, 'C', 4); break;
+      #endif
+    #endif
+
+    #ifdef TCCR4A
+      case TIMER4A: timer_prefix(4, 'A', 4); break;
+      case TIMER4B: timer_prefix(4, 'B', 4); break;
+      case TIMER4C: timer_prefix(4, 'C', 4); break;
+    #endif
+
+    #if defined(TCCR5A) && defined(COM5A1)
+      case TIMER5A: timer_prefix(5, 'A', 4); break;
+      case TIMER5B: timer_prefix(5, 'B', 4); break;
+      case TIMER5C: timer_prefix(5, 'C', 4); break;
+    #endif
+
+    case NOT_ON_TIMER: break;
+
+  }
+  SERIAL_ECHOPGM("  ");
+
+  // on pins that have two PWMs, print info on second PWM
+  #if AVR_ATmega2560_FAMILY || AVR_AT90USB1286_FAMILY
+    // looking for port B7 - PWMs 0A and 1C
+    if (digitalPinToPort_DEBUG(pin) == 'B' - 64 && 0x80 == digitalPinToBitMask_DEBUG(pin)) {
+      #if !AVR_AT90USB1286_FAMILY
+        SERIAL_ECHOPGM("\n .");
+        SERIAL_ECHO_SP(18);
+        SERIAL_ECHOPGM("TIMER1C");
+        print_is_also_tied();
+        timer_prefix(1, 'C', 4);
+      #else
+        SERIAL_ECHOPGM("\n .");
+        SERIAL_ECHO_SP(18);
+        SERIAL_ECHOPGM("TIMER0A");
+        print_is_also_tied();
+        timer_prefix(0, 'A', 3);
+      #endif
+    }
+  #else
+    UNUSED(print_is_also_tied);
+  #endif
+} // pwm_details
+
+
+#ifndef digitalRead_mod                   // Use Teensyduino's version of digitalRead - it doesn't disable the PWMs
+  int digitalRead_mod(const int8_t pin) { // same as digitalRead except the PWM stop section has been removed
+    const uint8_t port = digitalPinToPort_DEBUG(pin);
+    return (port != NOT_A_PIN) && (*portInputRegister(port) & digitalPinToBitMask_DEBUG(pin)) ? HIGH : LOW;
+  }
+#endif
+
+#ifndef PRINT_PORT
+
+  void print_port(int8_t pin) {   // print port number
+    #ifdef digitalPinToPort_DEBUG
+      uint8_t x;
+      SERIAL_ECHOPGM("  Port: ");
+      #if AVR_AT90USB1286_FAMILY
+        x = (pin == 46 || pin == 47) ? 'E' : digitalPinToPort_DEBUG(pin) + 64;
+      #else
+        x = digitalPinToPort_DEBUG(pin) + 64;
+      #endif
+      SERIAL_CHAR(x);
+
+      #if AVR_AT90USB1286_FAMILY
+        if (pin == 46)
+          x = '2';
+        else if (pin == 47)
+          x = '3';
+        else {
+          uint8_t temp = digitalPinToBitMask_DEBUG(pin);
+          for (x = '0'; x < '9' && temp != 1; x++) temp >>= 1;
+        }
+      #else
+        uint8_t temp = digitalPinToBitMask_DEBUG(pin);
+        for (x = '0'; x < '9' && temp != 1; x++) temp >>= 1;
+      #endif
+      SERIAL_CHAR(x);
+    #else
+      SERIAL_ECHO_SP(10);
+    #endif
+  }
+
+  #define PRINT_PORT(p) print_port(p)
+
+#endif
+
+#define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("%3d "), p); SERIAL_ECHO(buffer); }while(0)
diff --git a/Marlin/src/HAL/AVR/pinsDebug_Teensyduino.h b/Marlin/src/HAL/AVR/pinsDebug_Teensyduino.h
new file mode 100644
index 0000000..051972a
--- /dev/null
+++ b/Marlin/src/HAL/AVR/pinsDebug_Teensyduino.h
@@ -0,0 +1,108 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+//
+//  some of the pin mapping functions of the Teensduino extension to the Arduino IDE
+//  do not function the same as the other Arduino extensions
+//
+
+
+#define TEENSYDUINO_IDE
+
+//digitalPinToTimer(pin) function works like Arduino but Timers are not defined
+#define TIMER0B 1
+#define TIMER1A 7
+#define TIMER1B 8
+#define TIMER1C 9
+#define TIMER2A 6
+#define TIMER2B 2
+#define TIMER3A 5
+#define TIMER3B 4
+#define TIMER3C 3
+
+// digitalPinToPort function just returns the pin number so need to create our own
+#define PA 1
+#define PB 2
+#define PC 3
+#define PD 4
+#define PE 5
+#define PF 6
+
+#undef digitalPinToPort
+
+const uint8_t PROGMEM digital_pin_to_port_PGM[] = {
+  PD, // 0  - PD0 - INT0 - PWM
+  PD, // 1  - PD1 - INT1 - PWM
+  PD, // 2  - PD2 - INT2 - RX
+  PD, // 3  - PD3 - INT3 - TX
+  PD, // 4  - PD4
+  PD, // 5  - PD5
+  PD, // 6  - PD6
+  PD, // 7  - PD7
+  PE, // 8  - PE0
+  PE, // 9  - PE1
+  PC, // 10 - PC0
+  PC, // 11 - PC1
+  PC, // 12 - PC2
+  PC, // 13 - PC3
+  PC, // 14 - PC4 - PWM
+  PC, // 15 - PC5 - PWM
+  PC, // 16 - PC6 - PWM
+  PC, // 17 - PC7
+  PE, // 18 - PE6 - INT6
+  PE, // 19 - PE7 - INT7
+  PB, // 20 - PB0
+  PB, // 21 - PB1
+  PB, // 22 - PB2
+  PB, // 23 - PB3
+  PB, // 24 - PB4 - PWM
+  PB, // 25 - PB5 - PWM
+  PB, // 26 - PB6 - PWM
+  PB, // 27 - PB7 - PWM
+  PA, // 28 - PA0
+  PA, // 29 - PA1
+  PA, // 30 - PA2
+  PA, // 31 - PA3
+  PA, // 32 - PA4
+  PA, // 33 - PA5
+  PA, // 34 - PA6
+  PA, // 35 - PA7
+  PE, // 36 - PE4 - INT4
+  PE, // 37 - PE5 - INT5
+  PF, // 38 - PF0 - A0
+  PF, // 39 - PF1 - A1
+  PF, // 40 - PF2 - A2
+  PF, // 41 - PF3 - A3
+  PF, // 42 - PF4 - A4
+  PF, // 43 - PF5 - A5
+  PF, // 44 - PF6 - A6
+  PF, // 45 - PF7 - A7
+  PE, // 46 - PE2 (not defined in teensyduino)
+  PE, // 47 - PE3 (not defined in teensyduino)
+};
+
+#define digitalPinToPort(P) ( pgm_read_byte( digital_pin_to_port_PGM + (P) ) )
+
+// digitalPinToBitMask(pin) is OK
+
+#define digitalRead_mod(p)  extDigitalRead(p)   // Teensyduino's version of digitalRead doesn't
+                                                // disable the PWMs so we can use it as is
+
+// portModeRegister(pin) is OK
diff --git a/Marlin/src/HAL/AVR/pinsDebug_plus_70.h b/Marlin/src/HAL/AVR/pinsDebug_plus_70.h
new file mode 100644
index 0000000..db3fdf1
--- /dev/null
+++ b/Marlin/src/HAL/AVR/pinsDebug_plus_70.h
@@ -0,0 +1,329 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * Structures for 2560 family boards that use more than 70 pins
+ */
+
+#if MB(BQ_ZUM_MEGA_3D, MINIRAMBO, SCOOVO_X9H, TRIGORILLA_14)
+  #undef NUM_DIGITAL_PINS
+  #define NUM_DIGITAL_PINS            85
+#elif MB(MIGHTYBOARD_REVE)
+  #undef NUM_DIGITAL_PINS
+  #define NUM_DIGITAL_PINS            80
+#endif
+
+#define PA 1
+#define PB 2
+#define PC 3
+#define PD 4
+#define PE 5
+#define PF 6
+#define PG 7
+#define PH 8
+#define PJ 10
+#define PK 11
+#define PL 12
+
+const uint8_t PROGMEM digital_pin_to_port_PGM_plus_70[] = {
+  // PORTLIST
+  // ------------------------
+  PE  , // PE 0 ** 0 ** USART0_RX
+  PE  , // PE 1 ** 1 ** USART0_TX
+  PE  , // PE 4 ** 2 ** PWM2
+  PE  , // PE 5 ** 3 ** PWM3
+  PG  , // PG 5 ** 4 ** PWM4
+  PE  , // PE 3 ** 5 ** PWM5
+  PH  , // PH 3 ** 6 ** PWM6
+  PH  , // PH 4 ** 7 ** PWM7
+  PH  , // PH 5 ** 8 ** PWM8
+  PH  , // PH 6 ** 9 ** PWM9
+  PB  , // PB 4 ** 10 ** PWM10
+  PB  , // PB 5 ** 11 ** PWM11
+  PB  , // PB 6 ** 12 ** PWM12
+  PB  , // PB 7 ** 13 ** PWM13
+  PJ  , // PJ 1 ** 14 ** USART3_TX
+  PJ  , // PJ 0 ** 15 ** USART3_RX
+  PH  , // PH 1 ** 16 ** USART2_TX
+  PH  , // PH 0 ** 17 ** USART2_RX
+  PD  , // PD 3 ** 18 ** USART1_TX
+  PD  , // PD 2 ** 19 ** USART1_RX
+  PD  , // PD 1 ** 20 ** I2C_SDA
+  PD  , // PD 0 ** 21 ** I2C_SCL
+  PA  , // PA 0 ** 22 ** D22
+  PA  , // PA 1 ** 23 ** D23
+  PA  , // PA 2 ** 24 ** D24
+  PA  , // PA 3 ** 25 ** D25
+  PA  , // PA 4 ** 26 ** D26
+  PA  , // PA 5 ** 27 ** D27
+  PA  , // PA 6 ** 28 ** D28
+  PA  , // PA 7 ** 29 ** D29
+  PC  , // PC 7 ** 30 ** D30
+  PC  , // PC 6 ** 31 ** D31
+  PC  , // PC 5 ** 32 ** D32
+  PC  , // PC 4 ** 33 ** D33
+  PC  , // PC 3 ** 34 ** D34
+  PC  , // PC 2 ** 35 ** D35
+  PC  , // PC 1 ** 36 ** D36
+  PC  , // PC 0 ** 37 ** D37
+  PD  , // PD 7 ** 38 ** D38
+  PG  , // PG 2 ** 39 ** D39
+  PG  , // PG 1 ** 40 ** D40
+  PG  , // PG 0 ** 41 ** D41
+  PL  , // PL 7 ** 42 ** D42
+  PL  , // PL 6 ** 43 ** D43
+  PL  , // PL 5 ** 44 ** D44
+  PL  , // PL 4 ** 45 ** D45
+  PL  , // PL 3 ** 46 ** D46
+  PL  , // PL 2 ** 47 ** D47
+  PL  , // PL 1 ** 48 ** D48
+  PL  , // PL 0 ** 49 ** D49
+  PB  , // PB 3 ** 50 ** SPI_MISO
+  PB  , // PB 2 ** 51 ** SPI_MOSI
+  PB  , // PB 1 ** 52 ** SPI_SCK
+  PB  , // PB 0 ** 53 ** SPI_SS
+  PF  , // PF 0 ** 54 ** A0
+  PF  , // PF 1 ** 55 ** A1
+  PF  , // PF 2 ** 56 ** A2
+  PF  , // PF 3 ** 57 ** A3
+  PF  , // PF 4 ** 58 ** A4
+  PF  , // PF 5 ** 59 ** A5
+  PF  , // PF 6 ** 60 ** A6
+  PF  , // PF 7 ** 61 ** A7
+  PK  , // PK 0 ** 62 ** A8
+  PK  , // PK 1 ** 63 ** A9
+  PK  , // PK 2 ** 64 ** A10
+  PK  , // PK 3 ** 65 ** A11
+  PK  , // PK 4 ** 66 ** A12
+  PK  , // PK 5 ** 67 ** A13
+  PK  , // PK 6 ** 68 ** A14
+  PK  , // PK 7 ** 69 ** A15
+  PG  , // PG 4 ** 70 **
+  PG  , // PG 3 ** 71 **
+  PJ  , // PJ 2 ** 72 **
+  PJ  , // PJ 3 ** 73 **
+  PJ  , // PJ 7 ** 74 **
+  PJ  , // PJ 4 ** 75 **
+  PJ  , // PJ 5 ** 76 **
+  PJ  , // PJ 6 ** 77 **
+  PE  , // PE 2 ** 78 **
+  PE  , // PE 6 ** 79 **
+  PE  , // PE 7 ** 80 **
+  PD  , // PD 4 ** 81 **
+  PD  , // PD 5 ** 82 **
+  PD  , // PD 6 ** 83 **
+  PH  , // PH 2 ** 84 **
+  PH  , // PH 7 ** 85 **
+};
+
+#define digitalPinToPort_plus_70(P) ( pgm_read_byte( digital_pin_to_port_PGM_plus_70 + (P) ) )
+
+const uint8_t PROGMEM digital_pin_to_bit_mask_PGM_plus_70[] = {
+  // PIN IN PORT
+  // ------------------------
+  _BV( 0 )  , // PE 0 ** 0 ** USART0_RX
+  _BV( 1 )  , // PE 1 ** 1 ** USART0_TX
+  _BV( 4 )  , // PE 4 ** 2 ** PWM2
+  _BV( 5 )  , // PE 5 ** 3 ** PWM3
+  _BV( 5 )  , // PG 5 ** 4 ** PWM4
+  _BV( 3 )  , // PE 3 ** 5 ** PWM5
+  _BV( 3 )  , // PH 3 ** 6 ** PWM6
+  _BV( 4 )  , // PH 4 ** 7 ** PWM7
+  _BV( 5 )  , // PH 5 ** 8 ** PWM8
+  _BV( 6 )  , // PH 6 ** 9 ** PWM9
+  _BV( 4 )  , // PB 4 ** 10 ** PWM10
+  _BV( 5 )  , // PB 5 ** 11 ** PWM11
+  _BV( 6 )  , // PB 6 ** 12 ** PWM12
+  _BV( 7 )  , // PB 7 ** 13 ** PWM13
+  _BV( 1 )  , // PJ 1 ** 14 ** USART3_TX
+  _BV( 0 )  , // PJ 0 ** 15 ** USART3_RX
+  _BV( 1 )  , // PH 1 ** 16 ** USART2_TX
+  _BV( 0 )  , // PH 0 ** 17 ** USART2_RX
+  _BV( 3 )  , // PD 3 ** 18 ** USART1_TX
+  _BV( 2 )  , // PD 2 ** 19 ** USART1_RX
+  _BV( 1 )  , // PD 1 ** 20 ** I2C_SDA
+  _BV( 0 )  , // PD 0 ** 21 ** I2C_SCL
+  _BV( 0 )  , // PA 0 ** 22 ** D22
+  _BV( 1 )  , // PA 1 ** 23 ** D23
+  _BV( 2 )  , // PA 2 ** 24 ** D24
+  _BV( 3 )  , // PA 3 ** 25 ** D25
+  _BV( 4 )  , // PA 4 ** 26 ** D26
+  _BV( 5 )  , // PA 5 ** 27 ** D27
+  _BV( 6 )  , // PA 6 ** 28 ** D28
+  _BV( 7 )  , // PA 7 ** 29 ** D29
+  _BV( 7 )  , // PC 7 ** 30 ** D30
+  _BV( 6 )  , // PC 6 ** 31 ** D31
+  _BV( 5 )  , // PC 5 ** 32 ** D32
+  _BV( 4 )  , // PC 4 ** 33 ** D33
+  _BV( 3 )  , // PC 3 ** 34 ** D34
+  _BV( 2 )  , // PC 2 ** 35 ** D35
+  _BV( 1 )  , // PC 1 ** 36 ** D36
+  _BV( 0 )  , // PC 0 ** 37 ** D37
+  _BV( 7 )  , // PD 7 ** 38 ** D38
+  _BV( 2 )  , // PG 2 ** 39 ** D39
+  _BV( 1 )  , // PG 1 ** 40 ** D40
+  _BV( 0 )  , // PG 0 ** 41 ** D41
+  _BV( 7 )  , // PL 7 ** 42 ** D42
+  _BV( 6 )  , // PL 6 ** 43 ** D43
+  _BV( 5 )  , // PL 5 ** 44 ** D44
+  _BV( 4 )  , // PL 4 ** 45 ** D45
+  _BV( 3 )  , // PL 3 ** 46 ** D46
+  _BV( 2 )  , // PL 2 ** 47 ** D47
+  _BV( 1 )  , // PL 1 ** 48 ** D48
+  _BV( 0 )  , // PL 0 ** 49 ** D49
+  _BV( 3 )  , // PB 3 ** 50 ** SPI_MISO
+  _BV( 2 )  , // PB 2 ** 51 ** SPI_MOSI
+  _BV( 1 )  , // PB 1 ** 52 ** SPI_SCK
+  _BV( 0 )  , // PB 0 ** 53 ** SPI_SS
+  _BV( 0 )  , // PF 0 ** 54 ** A0
+  _BV( 1 )  , // PF 1 ** 55 ** A1
+  _BV( 2 )  , // PF 2 ** 56 ** A2
+  _BV( 3 )  , // PF 3 ** 57 ** A3
+  _BV( 4 )  , // PF 4 ** 58 ** A4
+  _BV( 5 )  , // PF 5 ** 59 ** A5
+  _BV( 6 )  , // PF 6 ** 60 ** A6
+  _BV( 7 )  , // PF 7 ** 61 ** A7
+  _BV( 0 )  , // PK 0 ** 62 ** A8
+  _BV( 1 )  , // PK 1 ** 63 ** A9
+  _BV( 2 )  , // PK 2 ** 64 ** A10
+  _BV( 3 )  , // PK 3 ** 65 ** A11
+  _BV( 4 )  , // PK 4 ** 66 ** A12
+  _BV( 5 )  , // PK 5 ** 67 ** A13
+  _BV( 6 )  , // PK 6 ** 68 ** A14
+  _BV( 7 )  , // PK 7 ** 69 ** A15
+  _BV( 4 )  , // PG 4 ** 70 **
+  _BV( 3 )  , // PG 3 ** 71 **
+  _BV( 2 )  , // PJ 2 ** 72 **
+  _BV( 3 )  , // PJ 3 ** 73 **
+  _BV( 7 )  , // PJ 7 ** 74 **
+  _BV( 4 )  , // PJ 4 ** 75 **
+  _BV( 5 )  , // PJ 5 ** 76 **
+  _BV( 6 )  , // PJ 6 ** 77 **
+  _BV( 2 )  , // PE 2 ** 78 **
+  _BV( 6 )  , // PE 6 ** 79 **
+  _BV( 7 )  , // PE 7 ** 80 **
+  _BV( 4 )  , // PD 4 ** 81 **
+  _BV( 5 )  , // PD 5 ** 82 **
+  _BV( 6 )  , // PD 6 ** 83 **
+  _BV( 2 )  , // PH 2 ** 84 **
+  _BV( 7 )  , // PH 7 ** 85 **
+};
+
+#define digitalPinToBitMask_plus_70(P) ( pgm_read_byte( digital_pin_to_bit_mask_PGM_plus_70 + (P) ) )
+
+
+const uint8_t PROGMEM digital_pin_to_timer_PGM_plus_70[] = {
+  // TIMERS
+  // ------------------------
+  NOT_ON_TIMER  , // PE 0 ** 0 ** USART0_RX
+  NOT_ON_TIMER  , // PE 1 ** 1 ** USART0_TX
+  TIMER3B , // PE 4 ** 2 ** PWM2
+  TIMER3C , // PE 5 ** 3 ** PWM3
+  TIMER0B , // PG 5 ** 4 ** PWM4
+  TIMER3A , // PE 3 ** 5 ** PWM5
+  TIMER4A , // PH 3 ** 6 ** PWM6
+  TIMER4B , // PH 4 ** 7 ** PWM7
+  TIMER4C , // PH 5 ** 8 ** PWM8
+  TIMER2B , // PH 6 ** 9 ** PWM9
+  TIMER2A , // PB 4 ** 10 ** PWM10
+  TIMER1A , // PB 5 ** 11 ** PWM11
+  TIMER1B , // PB 6 ** 12 ** PWM12
+  TIMER0A , // PB 7 ** 13 ** PWM13
+  NOT_ON_TIMER  , // PJ 1 ** 14 ** USART3_TX
+  NOT_ON_TIMER  , // PJ 0 ** 15 ** USART3_RX
+  NOT_ON_TIMER  , // PH 1 ** 16 ** USART2_TX
+  NOT_ON_TIMER  , // PH 0 ** 17 ** USART2_RX
+  NOT_ON_TIMER  , // PD 3 ** 18 ** USART1_TX
+  NOT_ON_TIMER  , // PD 2 ** 19 ** USART1_RX
+  NOT_ON_TIMER  , // PD 1 ** 20 ** I2C_SDA
+  NOT_ON_TIMER  , // PD 0 ** 21 ** I2C_SCL
+  NOT_ON_TIMER  , // PA 0 ** 22 ** D22
+  NOT_ON_TIMER  , // PA 1 ** 23 ** D23
+  NOT_ON_TIMER  , // PA 2 ** 24 ** D24
+  NOT_ON_TIMER  , // PA 3 ** 25 ** D25
+  NOT_ON_TIMER  , // PA 4 ** 26 ** D26
+  NOT_ON_TIMER  , // PA 5 ** 27 ** D27
+  NOT_ON_TIMER  , // PA 6 ** 28 ** D28
+  NOT_ON_TIMER  , // PA 7 ** 29 ** D29
+  NOT_ON_TIMER  , // PC 7 ** 30 ** D30
+  NOT_ON_TIMER  , // PC 6 ** 31 ** D31
+  NOT_ON_TIMER  , // PC 5 ** 32 ** D32
+  NOT_ON_TIMER  , // PC 4 ** 33 ** D33
+  NOT_ON_TIMER  , // PC 3 ** 34 ** D34
+  NOT_ON_TIMER  , // PC 2 ** 35 ** D35
+  NOT_ON_TIMER  , // PC 1 ** 36 ** D36
+  NOT_ON_TIMER  , // PC 0 ** 37 ** D37
+  NOT_ON_TIMER  , // PD 7 ** 38 ** D38
+  NOT_ON_TIMER  , // PG 2 ** 39 ** D39
+  NOT_ON_TIMER  , // PG 1 ** 40 ** D40
+  NOT_ON_TIMER  , // PG 0 ** 41 ** D41
+  NOT_ON_TIMER  , // PL 7 ** 42 ** D42
+  NOT_ON_TIMER  , // PL 6 ** 43 ** D43
+  TIMER5C , // PL 5 ** 44 ** D44
+  TIMER5B , // PL 4 ** 45 ** D45
+  TIMER5A , // PL 3 ** 46 ** D46
+  NOT_ON_TIMER  , // PL 2 ** 47 ** D47
+  NOT_ON_TIMER  , // PL 1 ** 48 ** D48
+  NOT_ON_TIMER  , // PL 0 ** 49 ** D49
+  NOT_ON_TIMER  , // PB 3 ** 50 ** SPI_MISO
+  NOT_ON_TIMER  , // PB 2 ** 51 ** SPI_MOSI
+  NOT_ON_TIMER  , // PB 1 ** 52 ** SPI_SCK
+  NOT_ON_TIMER  , // PB 0 ** 53 ** SPI_SS
+  NOT_ON_TIMER  , // PF 0 ** 54 ** A0
+  NOT_ON_TIMER  , // PF 1 ** 55 ** A1
+  NOT_ON_TIMER  , // PF 2 ** 56 ** A2
+  NOT_ON_TIMER  , // PF 3 ** 57 ** A3
+  NOT_ON_TIMER  , // PF 4 ** 58 ** A4
+  NOT_ON_TIMER  , // PF 5 ** 59 ** A5
+  NOT_ON_TIMER  , // PF 6 ** 60 ** A6
+  NOT_ON_TIMER  , // PF 7 ** 61 ** A7
+  NOT_ON_TIMER  , // PK 0 ** 62 ** A8
+  NOT_ON_TIMER  , // PK 1 ** 63 ** A9
+  NOT_ON_TIMER  , // PK 2 ** 64 ** A10
+  NOT_ON_TIMER  , // PK 3 ** 65 ** A11
+  NOT_ON_TIMER  , // PK 4 ** 66 ** A12
+  NOT_ON_TIMER  , // PK 5 ** 67 ** A13
+  NOT_ON_TIMER  , // PK 6 ** 68 ** A14
+  NOT_ON_TIMER  , // PK 7 ** 69 ** A15
+  NOT_ON_TIMER  , // PG 4 ** 70 **
+  NOT_ON_TIMER  , // PG 3 ** 71 **
+  NOT_ON_TIMER  , // PJ 2 ** 72 **
+  NOT_ON_TIMER  , // PJ 3 ** 73 **
+  NOT_ON_TIMER  , // PJ 7 ** 74 **
+  NOT_ON_TIMER  , // PJ 4 ** 75 **
+  NOT_ON_TIMER  , // PJ 5 ** 76 **
+  NOT_ON_TIMER  , // PJ 6 ** 77 **
+  NOT_ON_TIMER  , // PE 2 ** 78 **
+  NOT_ON_TIMER  , // PE 6 ** 79 **
+};
+
+#define digitalPinToTimer_plus_70(P) ( pgm_read_byte( digital_pin_to_timer_PGM_plus_70 + (P) ) )
+
+/**
+ *  Interrupts that are not implemented
+ *
+ *  INT6    E6 79
+ *  INT7    E7 80
+ *  PCINT11 J2 72
+ *  PCINT12 J3 73
+ *  PCINT13 J4 75
+ *  PCINT14 J5 76
+ *  PCINT15 J6 77
+ */
diff --git a/Marlin/src/HAL/AVR/spi_pins.h b/Marlin/src/HAL/AVR/spi_pins.h
new file mode 100644
index 0000000..8319729
--- /dev/null
+++ b/Marlin/src/HAL/AVR/spi_pins.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
+
+/**
+ * Define SPI Pins: SCK, MISO, MOSI, SS
+ */
+#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__)
+  #define AVR_SCK_PIN  13
+  #define AVR_MISO_PIN 12
+  #define AVR_MOSI_PIN 11
+  #define AVR_SS_PIN   10
+#elif defined(__AVR_ATmega644__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644PA__) || defined(__AVR_ATmega1284P__)
+  #define AVR_SCK_PIN  7
+  #define AVR_MISO_PIN 6
+  #define AVR_MOSI_PIN 5
+  #define AVR_SS_PIN   4
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+  #define AVR_SCK_PIN  52
+  #define AVR_MISO_PIN 50
+  #define AVR_MOSI_PIN 51
+  #define AVR_SS_PIN   53
+#elif defined(__AVR_AT90USB1287__) || defined(__AVR_AT90USB1286__) || defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB647__)
+  #define AVR_SCK_PIN  21
+  #define AVR_MISO_PIN 23
+  #define AVR_MOSI_PIN 22
+  #define AVR_SS_PIN   20
+#elif defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
+  #define AVR_SCK_PIN  10
+  #define AVR_MISO_PIN 12
+  #define AVR_MOSI_PIN 11
+  #define AVR_SS_PIN   16
+#endif
+
+#ifndef SD_SCK_PIN
+  #define SD_SCK_PIN  AVR_SCK_PIN
+#endif
+#ifndef SD_MISO_PIN
+  #define SD_MISO_PIN AVR_MISO_PIN
+#endif
+#ifndef SD_MOSI_PIN
+  #define SD_MOSI_PIN AVR_MOSI_PIN
+#endif
+#ifndef SD_SS_PIN
+  #define SD_SS_PIN   AVR_SS_PIN
+#endif
diff --git a/Marlin/src/HAL/AVR/timers.h b/Marlin/src/HAL/AVR/timers.h
new file mode 100644
index 0000000..82eb8b1
--- /dev/null
+++ b/Marlin/src/HAL/AVR/timers.h
@@ -0,0 +1,260 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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>
+
+// ------------------------
+// Types
+// ------------------------
+
+typedef uint16_t hal_timer_t;
+#define HAL_TIMER_TYPE_MAX 0xFFFF
+
+// ------------------------
+// Defines
+// ------------------------
+
+#define HAL_TIMER_RATE          ((F_CPU) / 8)    // i.e., 2MHz or 2.5MHz
+
+#ifndef STEP_TIMER_NUM
+  #define STEP_TIMER_NUM        1
+#endif
+#ifndef PULSE_TIMER_NUM
+  #define PULSE_TIMER_NUM       STEP_TIMER_NUM
+#endif
+#ifndef TEMP_TIMER_NUM
+  #define TEMP_TIMER_NUM        0
+#endif
+
+#define TEMP_TIMER_FREQUENCY    ((F_CPU) / 64.0 / 256.0)
+
+#define STEPPER_TIMER_RATE      HAL_TIMER_RATE
+#define STEPPER_TIMER_PRESCALE  8
+#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // Cannot be of type double
+
+#define PULSE_TIMER_RATE       STEPPER_TIMER_RATE   // frequency of pulse timer
+#define PULSE_TIMER_PRESCALE   STEPPER_TIMER_PRESCALE
+#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US
+
+#define ENABLE_STEPPER_DRIVER_INTERRUPT()  SBI(TIMSK1, OCIE1A)
+#define DISABLE_STEPPER_DRIVER_INTERRUPT() CBI(TIMSK1, OCIE1A)
+#define STEPPER_ISR_ENABLED()             TEST(TIMSK1, OCIE1A)
+
+#define ENABLE_TEMPERATURE_INTERRUPT()     SBI(TIMSK0, OCIE0B)
+#define DISABLE_TEMPERATURE_INTERRUPT()    CBI(TIMSK0, OCIE0B)
+#define TEMPERATURE_ISR_ENABLED()         TEST(TIMSK0, OCIE0B)
+
+FORCE_INLINE void HAL_timer_start(const uint8_t timer_num, const uint32_t) {
+  switch (timer_num) {
+    case STEP_TIMER_NUM:
+      // waveform generation = 0100 = CTC
+      SET_WGM(1, CTC_OCRnA);
+
+      // output mode = 00 (disconnected)
+      SET_COMA(1, NORMAL);
+
+      // Set the timer pre-scaler
+      // Generally we use a divider of 8, resulting in a 2MHz timer
+      // frequency on a 16MHz MCU. If you are going to change this, be
+      // sure to regenerate speed_lookuptable.h with
+      // create_speed_lookuptable.py
+      SET_CS(1, PRESCALER_8);  //  CS 2 = 1/8 prescaler
+
+      // Init Stepper ISR to 122 Hz for quick starting
+      // (F_CPU) / (STEPPER_TIMER_PRESCALE) / frequency
+      OCR1A = 0x4000;
+      TCNT1 = 0;
+      break;
+
+    case TEMP_TIMER_NUM:
+      // Use timer0 for temperature measurement
+      // Interleave temperature interrupt with millies interrupt
+      OCR0B = 128;
+      break;
+  }
+}
+
+#define TIMER_OCR_1             OCR1A
+#define TIMER_COUNTER_1         TCNT1
+
+#define TIMER_OCR_0             OCR0A
+#define TIMER_COUNTER_0         TCNT0
+
+#define _CAT(a,V...) a##V
+#define HAL_timer_set_compare(timer, compare) (_CAT(TIMER_OCR_, timer) = compare)
+#define HAL_timer_get_compare(timer) _CAT(TIMER_OCR_, timer)
+#define HAL_timer_get_count(timer) _CAT(TIMER_COUNTER_, timer)
+
+/**
+ * On AVR there is no hardware prioritization and preemption of
+ * interrupts, so this emulates it. The UART has first priority
+ * (otherwise, characters will be lost due to UART overflow).
+ * Then: Stepper, Endstops, Temperature, and -finally- all others.
+ */
+#define HAL_timer_isr_prologue(TIMER_NUM)
+#define HAL_timer_isr_epilogue(TIMER_NUM)
+
+/* 18 cycles maximum latency */
+#ifndef HAL_STEP_TIMER_ISR
+
+#define HAL_STEP_TIMER_ISR() \
+extern "C" void TIMER1_COMPA_vect() __attribute__ ((signal, naked, used, externally_visible)); \
+extern "C" void TIMER1_COMPA_vect_bottom() asm ("TIMER1_COMPA_vect_bottom") __attribute__ ((used, externally_visible, noinline)); \
+void TIMER1_COMPA_vect() { \
+  __asm__ __volatile__ ( \
+    A("push r16")                      /* 2 Save R16 */ \
+    A("in r16, __SREG__")              /* 1 Get SREG */ \
+    A("push r16")                      /* 2 Save SREG into stack */ \
+    A("lds r16, %[timsk0]")            /* 2 Load into R0 the Temperature timer Interrupt mask register */ \
+    A("push r16")                      /* 2 Save TIMSK0 into the stack */ \
+    A("andi r16,~%[msk0]")             /* 1 Disable the temperature ISR */ \
+    A("sts %[timsk0], r16")            /* 2 And set the new value */ \
+    A("lds r16, %[timsk1]")            /* 2 Load into R0 the stepper timer Interrupt mask register [TIMSK1] */ \
+    A("andi r16,~%[msk1]")             /* 1 Disable the stepper ISR */ \
+    A("sts %[timsk1], r16")            /* 2 And set the new value */ \
+    A("push r16")                      /* 2 Save TIMSK1 into stack */ \
+    A("in r16, 0x3B")                  /* 1 Get RAMPZ register */ \
+    A("push r16")                      /* 2 Save RAMPZ into stack */ \
+    A("in r16, 0x3C")                  /* 1 Get EIND register */ \
+    A("push r0")                       /* C runtime can modify all the following registers without restoring them */ \
+    A("push r1")                       \
+    A("push r18")                      \
+    A("push r19")                      \
+    A("push r20")                      \
+    A("push r21")                      \
+    A("push r22")                      \
+    A("push r23")                      \
+    A("push r24")                      \
+    A("push r25")                      \
+    A("push r26")                      \
+    A("push r27")                      \
+    A("push r30")                      \
+    A("push r31")                      \
+    A("clr r1")                        /* C runtime expects this register to be 0 */ \
+    A("call TIMER1_COMPA_vect_bottom") /* Call the bottom handler - No inlining allowed, otherwise registers used are not saved */   \
+    A("pop r31")                       \
+    A("pop r30")                       \
+    A("pop r27")                       \
+    A("pop r26")                       \
+    A("pop r25")                       \
+    A("pop r24")                       \
+    A("pop r23")                       \
+    A("pop r22")                       \
+    A("pop r21")                       \
+    A("pop r20")                       \
+    A("pop r19")                       \
+    A("pop r18")                       \
+    A("pop r1")                        \
+    A("pop r0")                        \
+    A("out 0x3C, r16")                 /* 1 Restore EIND register */ \
+    A("pop r16")                       /* 2 Get the original RAMPZ register value */ \
+    A("out 0x3B, r16")                 /* 1 Restore RAMPZ register to its original value */ \
+    A("pop r16")                       /* 2 Get the original TIMSK1 value but with stepper ISR disabled */ \
+    A("ori r16,%[msk1]")               /* 1 Reenable the stepper ISR */ \
+    A("cli")                           /* 1 Disable global interrupts - Reenabling Stepper ISR can reenter amd temperature can reenter, and we want that, if it happens, after this ISR has ended */ \
+    A("sts %[timsk1], r16")            /* 2 And restore the old value - This reenables the stepper ISR */ \
+    A("pop r16")                       /* 2 Get the temperature timer Interrupt mask register [TIMSK0] */ \
+    A("sts %[timsk0], r16")            /* 2 And restore the old value - This reenables the temperature ISR */ \
+    A("pop r16")                       /* 2 Get the old SREG value */ \
+    A("out __SREG__, r16")             /* 1 And restore the SREG value */ \
+    A("pop r16")                       /* 2 Restore R16 value */ \
+    A("reti")                          /* 4 Return from interrupt */ \
+    :                                   \
+    : [timsk0] "i" ((uint16_t)&TIMSK0), \
+      [timsk1] "i" ((uint16_t)&TIMSK1), \
+      [msk0] "M" ((uint8_t)(1<<OCIE0B)),\
+      [msk1] "M" ((uint8_t)(1<<OCIE1A)) \
+    : \
+  ); \
+} \
+void TIMER1_COMPA_vect_bottom()
+
+#endif // HAL_STEP_TIMER_ISR
+
+#ifndef HAL_TEMP_TIMER_ISR
+
+/* 14 cycles maximum latency */
+#define HAL_TEMP_TIMER_ISR() \
+extern "C" void TIMER0_COMPB_vect() __attribute__ ((signal, naked, used, externally_visible)); \
+extern "C" void TIMER0_COMPB_vect_bottom()  asm ("TIMER0_COMPB_vect_bottom") __attribute__ ((used, externally_visible, noinline)); \
+void TIMER0_COMPB_vect() { \
+  __asm__ __volatile__ ( \
+    A("push r16")                       /* 2 Save R16 */ \
+    A("in r16, __SREG__")               /* 1 Get SREG */ \
+    A("push r16")                       /* 2 Save SREG into stack */ \
+    A("lds r16, %[timsk0]")             /* 2 Load into R0 the Temperature timer Interrupt mask register */ \
+    A("andi r16,~%[msk0]")              /* 1 Disable the temperature ISR */ \
+    A("sts %[timsk0], r16")             /* 2 And set the new value */ \
+    A("sei")                            /* 1 Enable global interrupts - It is safe, as the temperature ISR is disabled, so we cannot reenter it */    \
+    A("push r16")                       /* 2 Save TIMSK0 into stack */ \
+    A("in r16, 0x3B")                   /* 1 Get RAMPZ register */ \
+    A("push r16")                       /* 2 Save RAMPZ into stack */ \
+    A("in r16, 0x3C")                   /* 1 Get EIND register */ \
+    A("push r0")                        /* C runtime can modify all the following registers without restoring them */ \
+    A("push r1")                        \
+    A("push r18")                       \
+    A("push r19")                       \
+    A("push r20")                       \
+    A("push r21")                       \
+    A("push r22")                       \
+    A("push r23")                       \
+    A("push r24")                       \
+    A("push r25")                       \
+    A("push r26")                       \
+    A("push r27")                       \
+    A("push r30")                       \
+    A("push r31")                       \
+    A("clr r1")                         /* C runtime expects this register to be 0 */ \
+    A("call TIMER0_COMPB_vect_bottom")  /* Call the bottom handler - No inlining allowed, otherwise registers used are not saved */   \
+    A("pop r31")                        \
+    A("pop r30")                        \
+    A("pop r27")                        \
+    A("pop r26")                        \
+    A("pop r25")                        \
+    A("pop r24")                        \
+    A("pop r23")                        \
+    A("pop r22")                        \
+    A("pop r21")                        \
+    A("pop r20")                        \
+    A("pop r19")                        \
+    A("pop r18")                        \
+    A("pop r1")                         \
+    A("pop r0")                         \
+    A("out 0x3C, r16")                  /* 1 Restore EIND register */ \
+    A("pop r16")                        /* 2 Get the original RAMPZ register value */ \
+    A("out 0x3B, r16")                  /* 1 Restore RAMPZ register to its original value */ \
+    A("pop r16")                        /* 2 Get the original TIMSK0 value but with temperature ISR disabled */ \
+    A("ori r16,%[msk0]")                /* 1 Enable temperature ISR */ \
+    A("cli")                            /* 1 Disable global interrupts - We must do this, as we will reenable the temperature ISR, and we don't want to reenter this handler until the current one is done */ \
+    A("sts %[timsk0], r16")             /* 2 And restore the old value */ \
+    A("pop r16")                        /* 2 Get the old SREG */ \
+    A("out __SREG__, r16")              /* 1 And restore the SREG value */ \
+    A("pop r16")                        /* 2 Restore R16 */ \
+    A("reti")                           /* 4 Return from interrupt */ \
+    :                                   \
+    : [timsk0] "i"((uint16_t)&TIMSK0),  \
+      [msk0] "M" ((uint8_t)(1<<OCIE0B)) \
+    : \
+  ); \
+} \
+void TIMER0_COMPB_vect_bottom()
+
+#endif // HAL_TEMP_TIMER_ISR
diff --git a/Marlin/src/HAL/AVR/u8g_com_HAL_AVR_sw_spi.cpp b/Marlin/src/HAL/AVR/u8g_com_HAL_AVR_sw_spi.cpp
new file mode 100644
index 0000000..cb95a48
--- /dev/null
+++ b/Marlin/src/HAL/AVR/u8g_com_HAL_AVR_sw_spi.cpp
@@ -0,0 +1,193 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * Based on u8g_com_st7920_hw_spi.c
+ *
+ * Universal 8bit Graphics Library
+ *
+ * Copyright (c) 2011, olikraus@gmail.com
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ *  * Redistributions of source code must retain the above copyright notice, this list
+ *    of conditions and the following disclaimer.
+ *
+ *  * Redistributions in binary form must reproduce the above copyright notice, this
+ *    list of conditions and the following disclaimer in the documentation and/or other
+ *    materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+ * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#if defined(ARDUINO) && !defined(ARDUINO_ARCH_STM32) && !defined(ARDUINO_ARCH_SAM)
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if HAS_MARLINUI_U8GLIB
+
+#include "../shared/Marduino.h"
+#include "../shared/Delay.h"
+
+#include <U8glib.h>
+
+uint8_t u8g_bitData, u8g_bitNotData, u8g_bitClock, u8g_bitNotClock;
+volatile uint8_t *u8g_outData, *u8g_outClock;
+
+static void u8g_com_arduino_init_shift_out(uint8_t dataPin, uint8_t clockPin) {
+  u8g_outData = portOutputRegister(digitalPinToPort(dataPin));
+  u8g_outClock = portOutputRegister(digitalPinToPort(clockPin));
+  u8g_bitData = digitalPinToBitMask(dataPin);
+  u8g_bitClock = digitalPinToBitMask(clockPin);
+
+  u8g_bitNotClock = u8g_bitClock;
+  u8g_bitNotClock ^= 0xFF;
+
+  u8g_bitNotData = u8g_bitData;
+  u8g_bitNotData ^= 0xFF;
+}
+
+void u8g_spiSend_sw_AVR_mode_0(uint8_t val) {
+  uint8_t bitData = u8g_bitData,
+          bitNotData = u8g_bitNotData,
+          bitClock = u8g_bitClock,
+          bitNotClock = u8g_bitNotClock;
+  volatile uint8_t *outData = u8g_outData,
+                   *outClock = u8g_outClock;
+  U8G_ATOMIC_START();
+  LOOP_L_N(i, 8) {
+    if (val & 0x80)
+      *outData |= bitData;
+    else
+      *outData &= bitNotData;
+    *outClock |= bitClock;
+    val <<= 1;
+    *outClock &= bitNotClock;
+  }
+  U8G_ATOMIC_END();
+}
+
+void u8g_spiSend_sw_AVR_mode_3(uint8_t val) {
+  uint8_t bitData = u8g_bitData,
+          bitNotData = u8g_bitNotData,
+          bitClock = u8g_bitClock,
+          bitNotClock = u8g_bitNotClock;
+  volatile uint8_t *outData = u8g_outData,
+                   *outClock = u8g_outClock;
+  U8G_ATOMIC_START();
+  LOOP_L_N(i, 8) {
+    *outClock &= bitNotClock;
+    if (val & 0x80)
+      *outData |= bitData;
+    else
+      *outData &= bitNotData;
+    *outClock |= bitClock;
+    val <<= 1;
+  }
+  U8G_ATOMIC_END();
+}
+
+
+#if ENABLED(FYSETC_MINI_12864)
+  #define SPISEND_SW_AVR u8g_spiSend_sw_AVR_mode_3
+#else
+  #define SPISEND_SW_AVR u8g_spiSend_sw_AVR_mode_0
+#endif
+
+uint8_t u8g_com_HAL_AVR_sw_sp_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) {
+  switch (msg) {
+    case U8G_COM_MSG_INIT:
+      u8g_com_arduino_init_shift_out(u8g->pin_list[U8G_PI_MOSI], u8g->pin_list[U8G_PI_SCK]);
+      u8g_com_arduino_assign_pin_output_high(u8g);
+      u8g_com_arduino_digital_write(u8g, U8G_PI_SCK, 0);
+      u8g_com_arduino_digital_write(u8g, U8G_PI_MOSI, 0);
+      break;
+
+    case U8G_COM_MSG_STOP:
+      break;
+
+    case U8G_COM_MSG_RESET:
+      if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_RESET]) u8g_com_arduino_digital_write(u8g, U8G_PI_RESET, arg_val);
+      break;
+
+    case U8G_COM_MSG_CHIP_SELECT:
+      #if ENABLED(FYSETC_MINI_12864)           // LCD SPI is running mode 3 while SD card is running mode 0
+        if (arg_val) {                         //   SCK idle state needs to be set to the proper idle state before
+                                               //   the next chip select goes active
+          u8g_com_arduino_digital_write(u8g, U8G_PI_SCK, 1);  // Set SCK to mode 3 idle state before CS goes active
+          u8g_com_arduino_digital_write(u8g, U8G_PI_CS, LOW);
+        }
+        else {
+          u8g_com_arduino_digital_write(u8g, U8G_PI_CS, HIGH);
+          u8g_com_arduino_digital_write(u8g, U8G_PI_SCK, 0);  // Set SCK to mode 0 idle state after CS goes inactive
+        }
+      #else
+        u8g_com_arduino_digital_write(u8g, U8G_PI_CS, !arg_val);
+      #endif
+      break;
+
+    case U8G_COM_MSG_WRITE_BYTE:
+      SPISEND_SW_AVR(arg_val);
+      break;
+
+    case U8G_COM_MSG_WRITE_SEQ: {
+        uint8_t *ptr = (uint8_t *)arg_ptr;
+        while (arg_val > 0) {
+          SPISEND_SW_AVR(*ptr++);
+          arg_val--;
+        }
+      }
+      break;
+
+      case U8G_COM_MSG_WRITE_SEQ_P: {
+        uint8_t *ptr = (uint8_t *)arg_ptr;
+        while (arg_val > 0) {
+          SPISEND_SW_AVR(u8g_pgm_read(ptr));
+          ptr++;
+          arg_val--;
+        }
+      }
+      break;
+
+    case U8G_COM_MSG_ADDRESS:                     /* define cmd (arg_val = 0) or data mode (arg_val = 1) */
+      u8g_com_arduino_digital_write(u8g, U8G_PI_A0, arg_val);
+      break;
+  }
+  return 1;
+}
+
+#endif // HAS_MARLINUI_U8GLIB
+#endif // ARDUINO_ARCH_SAM
diff --git a/Marlin/src/HAL/AVR/watchdog.cpp b/Marlin/src/HAL/AVR/watchdog.cpp
new file mode 100644
index 0000000..3f10c4a
--- /dev/null
+++ b/Marlin/src/HAL/AVR/watchdog.cpp
@@ -0,0 +1,70 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#ifdef __AVR__
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(USE_WATCHDOG)
+
+#include "watchdog.h"
+
+#include "../../MarlinCore.h"
+
+// Initialize watchdog with 8s timeout, if possible. Otherwise, make it 4s.
+void watchdog_init() {
+  #if ENABLED(WATCHDOG_DURATION_8S) && defined(WDTO_8S)
+    #define WDTO_NS WDTO_8S
+  #else
+    #define WDTO_NS WDTO_4S
+  #endif
+  #if ENABLED(WATCHDOG_RESET_MANUAL)
+    // Enable the watchdog timer, but only for the interrupt.
+    // Take care, as this requires the correct order of operation, with interrupts disabled.
+    // See the datasheet of any AVR chip for details.
+    wdt_reset();
+    cli();
+    _WD_CONTROL_REG = _BV(_WD_CHANGE_BIT) | _BV(WDE);
+    _WD_CONTROL_REG = _BV(WDIE) | (WDTO_NS & 0x07) | ((WDTO_NS & 0x08) << 2); // WDTO_NS directly does not work. bit 0-2 are consecutive in the register but the highest value bit is at bit 5
+                                                                              // So worked for up to WDTO_2S
+    sei();
+    wdt_reset();
+  #else
+    wdt_enable(WDTO_NS); // The function handles the upper bit correct.
+  #endif
+  //delay(10000); // test it!
+}
+
+//===========================================================================
+//=================================== ISR ===================================
+//===========================================================================
+
+// Watchdog timer interrupt, called if main program blocks >4sec and manual reset is enabled.
+#if ENABLED(WATCHDOG_RESET_MANUAL)
+  ISR(WDT_vect) {
+    sei();  // With the interrupt driven serial we need to allow interrupts.
+    SERIAL_ERROR_MSG(STR_WATCHDOG_FIRED);
+    minkill();  // interrupt-safe final kill and infinite loop
+  }
+#endif
+
+#endif // USE_WATCHDOG
+#endif // __AVR__
diff --git a/Marlin/src/HAL/AVR/watchdog.h b/Marlin/src/HAL/AVR/watchdog.h
new file mode 100644
index 0000000..a16c88b
--- /dev/null
+++ b/Marlin/src/HAL/AVR/watchdog.h
@@ -0,0 +1,31 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include <avr/wdt.h>
+
+// Initialize watchdog with a 4 second interrupt time
+void watchdog_init();
+
+// Reset watchdog. MUST be called at least every 4 seconds after the
+// first watchdog_init or AVR will go into emergency procedures.
+inline void HAL_watchdog_refresh() { wdt_reset(); }