Initial commit

18 files changed, 1115 insertions, 0 deletions

diff --git a/Marlin/src/HAL/TEENSY31_32/HAL.cpp b/Marlin/src/HAL/TEENSY31_32/HAL.cpp
new file mode 100644
index 0000000..51636d2
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/HAL.cpp
@@ -0,0 +1,94 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * HAL for Teensy 3.2 (MK20DX256)
+ */
+
+#ifdef __MK20DX256__
+
+#include "HAL.h"
+#include "../shared/Delay.h"
+
+#include <Wire.h>
+
+DefaultSerial MSerial(false);
+USBSerialType USBSerial(false, SerialUSB);
+
+uint16_t HAL_adc_result;
+
+static const uint8_t pin2sc1a[] = {
+    5, 14, 8, 9, 13, 12, 6, 7, 15, 4, 0, 19, 3, 31, // 0-13, we treat them as A0-A13
+    5, 14, 8, 9, 13, 12, 6, 7, 15, 4, // 14-23 (A0-A9)
+    31, 31, 31, 31, 31, 31, 31, 31, 31, 31, // 24-33
+    0+64, 19+64, 3+64, 31+64, // 34-37 (A10-A13)
+    26, 22, 23, 27, 29, 30 // 38-43: temp. sensor, VREF_OUT, A14, bandgap, VREFH, VREFL. A14 isn't connected to anything in Teensy 3.0.
+};
+
+/*
+  // disable interrupts
+  void cli() { noInterrupts(); }
+
+  // enable interrupts
+  void sei() { interrupts(); }
+*/
+
+void HAL_adc_init() {
+  analog_init();
+  while (ADC0_SC3 & ADC_SC3_CAL) {}; // Wait for calibration to finish
+  NVIC_ENABLE_IRQ(IRQ_FTM1);
+}
+
+void HAL_clear_reset_source() { }
+
+uint8_t HAL_get_reset_source() {
+  switch (RCM_SRS0) {
+    case 128: return RST_POWER_ON; break;
+    case 64: return RST_EXTERNAL; break;
+    case 32: return RST_WATCHDOG; break;
+    // case 8: return RST_LOSS_OF_LOCK; break;
+    // case 4: return RST_LOSS_OF_CLOCK; break;
+    // case 2: return RST_LOW_VOLTAGE; break;
+  }
+  return 0;
+}
+
+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;
+  }
+}
+
+void HAL_adc_start_conversion(const uint8_t adc_pin) { ADC0_SC1A = pin2sc1a[adc_pin]; }
+
+uint16_t HAL_adc_get_result() { return ADC0_RA; }
+
+#endif // __MK20DX256__
diff --git a/Marlin/src/HAL/TEENSY31_32/HAL.h b/Marlin/src/HAL/TEENSY31_32/HAL.h
new file mode 100644
index 0000000..5273b38
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/HAL.h
@@ -0,0 +1,123 @@
+/**
+ * Marlin 3D Printer Firmware
+ *
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com
+ * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.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
+
+/**
+ * HAL for Teensy 3.2 (MK20DX256)
+ */
+
+#define CPU_32_BIT
+
+#include "../shared/Marduino.h"
+#include "../shared/math_32bit.h"
+#include "../shared/HAL_SPI.h"
+
+#include "fastio.h"
+#include "watchdog.h"
+
+
+#include <stdint.h>
+
+#define ST7920_DELAY_1 DELAY_NS(600)
+#define ST7920_DELAY_2 DELAY_NS(750)
+#define ST7920_DELAY_3 DELAY_NS(750)
+
+//#undef MOTHERBOARD
+//#define MOTHERBOARD BOARD_TEENSY31_32
+
+#define IS_32BIT_TEENSY 1
+#define IS_TEENSY_31_32 1
+#ifndef IS_TEENSY31
+  #define IS_TEENSY32 1
+#endif
+
+#include "../../core/serial_hook.h"
+typedef Serial0Type<decltype(Serial)> DefaultSerial;
+extern DefaultSerial MSerial;
+typedef ForwardSerial0Type<decltype(SerialUSB)> USBSerialType;
+extern USBSerialType USBSerial;
+
+#define _MSERIAL(X) MSerial##X
+#define MSERIAL(X) _MSERIAL(X)
+#define MSerial0 MSerial
+
+#if SERIAL_PORT == -1
+  #define MYSERIAL0 USBSerial
+#elif WITHIN(SERIAL_PORT, 0, 3)
+  #define MYSERIAL0 MSERIAL(SERIAL_PORT)
+#endif
+
+#define HAL_SERVO_LIB libServo
+
+typedef int8_t pin_t;
+
+#ifndef analogInputToDigitalPin
+  #define analogInputToDigitalPin(p) ((p < 12U) ? (p) + 54U : -1)
+#endif
+
+#define CRITICAL_SECTION_START()  uint32_t primask = __get_PRIMASK(); __disable_irq()
+#define CRITICAL_SECTION_END()    if (!primask) __enable_irq()
+#define ISRS_ENABLED() (!__get_PRIMASK())
+#define ENABLE_ISRS()  __enable_irq()
+#define DISABLE_ISRS() __disable_irq()
+
+inline void HAL_init() {}
+
+// Clear the reset reason
+void HAL_clear_reset_source();
+
+// Get the reason for the reset
+uint8_t HAL_get_reset_source();
+
+inline void HAL_reboot() {}  // reboot the board or restart the bootloader
+
+FORCE_INLINE void _delay_ms(const int delay_ms) { delay(delay_ms); }
+
+#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
+
+void HAL_adc_init();
+
+#define HAL_ADC_VREF         3.3
+#define HAL_ADC_RESOLUTION  10
+#define HAL_START_ADC(pin)  HAL_adc_start_conversion(pin)
+#define HAL_READ_ADC()      HAL_adc_get_result()
+#define HAL_ADC_READY()     true
+
+#define HAL_ANALOG_SELECT(pin)
+
+void HAL_adc_start_conversion(const uint8_t adc_pin);
+uint16_t HAL_adc_get_result();
+
+#define GET_PIN_MAP_PIN(index) index
+#define GET_PIN_MAP_INDEX(pin) pin
+#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval)
diff --git a/Marlin/src/HAL/TEENSY31_32/HAL_SPI.cpp b/Marlin/src/HAL/TEENSY31_32/HAL_SPI.cpp
new file mode 100644
index 0000000..dce236e
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/HAL_SPI.cpp
@@ -0,0 +1,130 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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 __MK20DX256__
+
+#include "HAL.h"
+#include <SPI.h>
+#include <pins_arduino.h>
+#include "spi_pins.h"
+#include "../../core/macros.h"
+
+static SPISettings spiConfig;
+
+/**
+ * Standard SPI functions
+ */
+
+// Initialize SPI bus
+void spiBegin() {
+  #if !PIN_EXISTS(SD_SS)
+    #error "SD_SS_PIN not defined!"
+  #endif
+  OUT_WRITE(SD_SS_PIN, HIGH);
+  SET_OUTPUT(SD_SCK_PIN);
+  SET_INPUT(SD_MISO_PIN);
+  SET_OUTPUT(SD_MOSI_PIN);
+
+  #if 0 && DISABLED(SOFTWARE_SPI)
+    // 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(SPI_HALF_SPEED); // 1
+  #endif
+}
+
+// Configure SPI for specified SPI speed
+void spiInit(uint8_t spiRate) {
+  // Use data rates Marlin uses
+  uint32_t clock;
+  switch (spiRate) {
+    case SPI_FULL_SPEED:    clock = 10000000; break;
+    case SPI_HALF_SPEED:    clock =  5000000; break;
+    case SPI_QUARTER_SPEED: clock =  2500000; break;
+    case SPI_EIGHTH_SPEED:  clock =  1250000; break;
+    case SPI_SPEED_5:       clock =   625000; break;
+    case SPI_SPEED_6:       clock =   312500; break;
+    default:                clock = 4000000; // Default from the SPI libarary
+  }
+  spiConfig = SPISettings(clock, MSBFIRST, SPI_MODE0);
+  SPI.begin();
+}
+
+// SPI receive a byte
+uint8_t spiRec() {
+  SPI.beginTransaction(spiConfig);
+  const uint8_t returnByte = SPI.transfer(0xFF);
+  SPI.endTransaction();
+  return returnByte;
+  //SPDR = 0xFF;
+  //while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
+  //return SPDR;
+}
+
+// SPI read data
+void spiRead(uint8_t* buf, uint16_t nbyte) {
+  SPI.beginTransaction(spiConfig);
+  SPI.transfer(buf, nbyte);
+  SPI.endTransaction();
+  //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) {
+  SPI.beginTransaction(spiConfig);
+  SPI.transfer(b);
+  SPI.endTransaction();
+  //SPDR = b;
+  //while (!TEST(SPSR, SPIF)) { /* nada */ }
+}
+
+// SPI send block
+void spiSendBlock(uint8_t token, const uint8_t* buf) {
+  SPI.beginTransaction(spiConfig);
+  SPDR = token;
+  for (uint16_t i = 0; i < 512; i += 2) {
+    while (!TEST(SPSR, SPIF)) { /* nada */ };
+    SPDR = buf[i];
+    while (!TEST(SPSR, SPIF)) { /* nada */ };
+    SPDR = buf[i + 1];
+  }
+  while (!TEST(SPSR, SPIF)) { /* nada */ };
+  SPI.endTransaction();
+}
+
+
+// Begin SPI transaction, set clock, bit order, data mode
+void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode) {
+  spiConfig = SPISettings(spiClock, bitOrder, dataMode);
+  SPI.beginTransaction(spiConfig);
+}
+
+#endif // __MK20DX256__
diff --git a/Marlin/src/HAL/TEENSY31_32/Servo.cpp b/Marlin/src/HAL/TEENSY31_32/Servo.cpp
new file mode 100644
index 0000000..19d57cf
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/Servo.cpp
@@ -0,0 +1,54 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#ifdef __MK20DX256__
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_SERVOS
+
+#include "Servo.h"
+
+uint8_t servoPin[MAX_SERVOS] = { 0 };
+
+int8_t libServo::attach(const int inPin) {
+  if (servoIndex >= MAX_SERVOS) return -1;
+  if (inPin > 0) servoPin[servoIndex] = inPin;
+  return super::attach(servoPin[servoIndex]);
+}
+
+int8_t libServo::attach(const int inPin, const int inMin, const int inMax) {
+  if (inPin > 0) servoPin[servoIndex] = inPin;
+  return super::attach(servoPin[servoIndex], inMin, inMax);
+}
+
+void libServo::move(const int value) {
+  constexpr uint16_t servo_delay[] = SERVO_DELAY;
+  static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long.");
+  if (attach(0) >= 0) {
+    write(value);
+    safe_delay(servo_delay[servoIndex]);
+    TERN_(DEACTIVATE_SERVOS_AFTER_MOVE, detach());
+  }
+}
+
+#endif // HAS_SERVOS
+#endif // __MK20DX256__
diff --git a/Marlin/src/HAL/TEENSY31_32/Servo.h b/Marlin/src/HAL/TEENSY31_32/Servo.h
new file mode 100644
index 0000000..82b601d
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/Servo.h
@@ -0,0 +1,37 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include <Servo.h>
+
+// Inherit and expand on the official library
+class libServo : public Servo {
+  public:
+    int8_t attach(const int pin);
+    int8_t attach(const int pin, const int min, const int max);
+    void move(const int value);
+  private:
+    typedef Servo super;
+    uint16_t min_ticks;
+    uint16_t max_ticks;
+    uint8_t servoIndex;               // index into the channel data for this servo
+};
diff --git a/Marlin/src/HAL/TEENSY31_32/eeprom.cpp b/Marlin/src/HAL/TEENSY31_32/eeprom.cpp
new file mode 100644
index 0000000..f2ae5dd
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/eeprom.cpp
@@ -0,0 +1,72 @@
+/**
+ * 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/>.
+ *
+ */
+#ifdef __MK20DX256__
+
+#include "../../inc/MarlinConfig.h"
+
+#if USE_WIRED_EEPROM
+
+/**
+ * HAL PersistentStore for Teensy 3.2 (MK20DX256)
+ */
+
+#include "../shared/eeprom_api.h"
+#include <avr/eeprom.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;
+}
+
+#endif // USE_WIRED_EEPROM
+#endif // __MK20DX256__
diff --git a/Marlin/src/HAL/TEENSY31_32/endstop_interrupts.h b/Marlin/src/HAL/TEENSY31_32/endstop_interrupts.h
new file mode 100644
index 0000000..999ada5
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/endstop_interrupts.h
@@ -0,0 +1,67 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#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 an 'external 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"
+
+// One ISR for all EXT-Interrupts
+void endstop_ISR() { endstops.update(); }
+
+/**
+ *  Endstop interrupts for Due based targets.
+ *  On Due, all pins support external interrupt capability.
+ */
+
+void setup_endstop_interrupts() {
+  #define _ATTACH(P) attachInterrupt(digitalPinToInterrupt(P), endstop_ISR, CHANGE)
+  TERN_(HAS_X_MAX, _ATTACH(X_MAX_PIN));
+  TERN_(HAS_X_MIN, _ATTACH(X_MIN_PIN));
+  TERN_(HAS_Y_MAX, _ATTACH(Y_MAX_PIN));
+  TERN_(HAS_Y_MIN, _ATTACH(Y_MIN_PIN));
+  TERN_(HAS_Z_MAX, _ATTACH(Z_MAX_PIN));
+  TERN_(HAS_Z_MIN, _ATTACH(Z_MIN_PIN));
+  TERN_(HAS_X2_MAX, _ATTACH(X2_MAX_PIN));
+  TERN_(HAS_X2_MIN, _ATTACH(X2_MIN_PIN));
+  TERN_(HAS_Y2_MAX, _ATTACH(Y2_MAX_PIN));
+  TERN_(HAS_Y2_MIN, _ATTACH(Y2_MIN_PIN));
+  TERN_(HAS_Z2_MAX, _ATTACH(Z2_MAX_PIN));
+  TERN_(HAS_Z2_MIN, _ATTACH(Z2_MIN_PIN));
+  TERN_(HAS_Z3_MAX, _ATTACH(Z3_MAX_PIN));
+  TERN_(HAS_Z3_MIN, _ATTACH(Z3_MIN_PIN));
+  TERN_(HAS_Z4_MAX, _ATTACH(Z4_MAX_PIN));
+  TERN_(HAS_Z4_MIN, _ATTACH(Z4_MIN_PIN));
+  TERN_(HAS_Z_MIN_PROBE_PIN, _ATTACH(Z_MIN_PROBE_PIN));
+}
diff --git a/Marlin/src/HAL/TEENSY31_32/fastio.h b/Marlin/src/HAL/TEENSY31_32/fastio.h
new file mode 100644
index 0000000..622799e
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/fastio.h
@@ -0,0 +1,98 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * Fast I/O Routines for Teensy 3.5 and Teensy 3.6
+ * Use direct port manipulation to save scads of processor time.
+ * Contributed by Triffid_Hunter and modified by Kliment, thinkyhead, Bob-the-Kuhn, et.al.
+ */
+
+#ifndef MASK
+  #define MASK(PIN) _BV(PIN)
+#endif
+
+#define GPIO_BITBAND_ADDR(reg, bit) (((uint32_t)&(reg) - 0x40000000) * 32 + (bit) * 4 + 0x42000000)
+#define GPIO_BITBAND(reg, bit) (*(uint32_t *)GPIO_BITBAND_ADDR((reg), (bit)))
+
+/**
+ * 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/gcc-4.8.5/cpp/Stringification.html
+ */
+
+#define _READ(P) bool(CORE_PIN ## P ## _PINREG & CORE_PIN ## P ## _BITMASK)
+
+#define _WRITE(P,V) do{ \
+  if (V) CORE_PIN ## P ## _PORTSET = CORE_PIN ## P ## _BITMASK; \
+  else CORE_PIN ## P ## _PORTCLEAR = CORE_PIN ## P ## _BITMASK; \
+}while(0)
+
+#define _TOGGLE(P) (*(&(CORE_PIN ## P ## _PORTCLEAR)+1) = CORE_PIN ## P ## _BITMASK)
+
+#define _SET_INPUT(P) do{ \
+  CORE_PIN ## P ## _CONFIG = PORT_PCR_MUX(1); \
+  GPIO_BITBAND(CORE_PIN ## P ## _DDRREG , CORE_PIN ## P ## _BIT) = 0; \
+}while(0)
+
+#define _SET_OUTPUT(P) do{ \
+  CORE_PIN ## P ## _CONFIG = PORT_PCR_MUX(1)|PORT_PCR_SRE|PORT_PCR_DSE; \
+  GPIO_BITBAND(CORE_PIN ## P ## _DDRREG , CORE_PIN ## P ## _BIT) = 1; \
+}while(0)
+
+#define _SET_INPUT_PULLUP(P) do{ \
+  CORE_PIN ## P ## _CONFIG = PORT_PCR_MUX(1) | PORT_PCR_PE | PORT_PCR_PS; \
+  GPIO_BITBAND(CORE_PIN ## P ## _DDRREG , CORE_PIN ## P ## _BIT) = 0; \
+}while(0)
+
+#define _IS_INPUT(P)    ((CORE_PIN ## P ## _DDRREG & CORE_PIN ## P ## _BITMASK) == 0)
+#define _IS_OUTPUT(P)   ((CORE_PIN ## P ## _DDRREG & CORE_PIN ## P ## _BITMASK) == 0)
+
+#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)  _SET_INPUT_PULLUP(IO)
+#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)
+
+// digitalRead/Write wrappers
+#define extDigitalRead(IO)    digitalRead(IO)
+#define extDigitalWrite(IO,V) digitalWrite(IO,V)
+
+#define PWM_PIN(P)            digitalPinHasPWM(P)
+
+/**
+ * Ports, functions, and pins
+ */
+
+#define DIO0_PIN 8
diff --git a/Marlin/src/HAL/TEENSY31_32/inc/Conditionals_LCD.h b/Marlin/src/HAL/TEENSY31_32/inc/Conditionals_LCD.h
new file mode 100644
index 0000000..54ec166
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/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/TEENSY31_32."
+#endif
diff --git a/Marlin/src/HAL/TEENSY31_32/inc/Conditionals_adv.h b/Marlin/src/HAL/TEENSY31_32/inc/Conditionals_adv.h
new file mode 100644
index 0000000..5f1c4b1
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/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/TEENSY31_32/inc/Conditionals_post.h b/Marlin/src/HAL/TEENSY31_32/inc/Conditionals_post.h
new file mode 100644
index 0000000..998f1dc
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/inc/Conditionals_post.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 USE_FALLBACK_EEPROM
+  #define USE_WIRED_EEPROM 1
+#endif
diff --git a/Marlin/src/HAL/TEENSY31_32/inc/SanityCheck.h b/Marlin/src/HAL/TEENSY31_32/inc/SanityCheck.h
new file mode 100644
index 0000000..3932ee6
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/inc/SanityCheck.h
@@ -0,0 +1,38 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * Test TEENSY35_36 specific configuration values for errors at compile-time.
+ */
+
+#if ENABLED(EMERGENCY_PARSER)
+  #error "EMERGENCY_PARSER is not yet implemented for Teensy 3.1/3.2. Disable EMERGENCY_PARSER to continue."
+#endif
+
+#if ENABLED(FAST_PWM_FAN) || SPINDLE_LASER_FREQUENCY
+  #error "Features requiring Hardware PWM (FAST_PWM_FAN, SPINDLE_LASER_FREQUENCY) are not yet supported on Teensy 3.1/3.2."
+#endif
+
+#if HAS_TMC_SW_SERIAL
+  #error "TMC220x Software Serial is not supported on this platform."
+#endif
diff --git a/Marlin/src/HAL/TEENSY31_32/pinsDebug.h b/Marlin/src/HAL/TEENSY31_32/pinsDebug.h
new file mode 100644
index 0000000..d4a91ce
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/pinsDebug.h
@@ -0,0 +1 @@
+#error "PINS_DEBUGGING is not yet supported for Teensy 3.1 / 3.2!"
diff --git a/Marlin/src/HAL/TEENSY31_32/spi_pins.h b/Marlin/src/HAL/TEENSY31_32/spi_pins.h
new file mode 100644
index 0000000..6d0d05f
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/spi_pins.h
@@ -0,0 +1,27 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#define SD_SCK_PIN  13
+#define SD_MISO_PIN 12
+#define SD_MOSI_PIN 11
+#define SD_SS_PIN   20 // SDSS // A.28, A.29, B.21, C.26, C.29
diff --git a/Marlin/src/HAL/TEENSY31_32/timers.cpp b/Marlin/src/HAL/TEENSY31_32/timers.cpp
new file mode 100644
index 0000000..7e01a38
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/timers.cpp
@@ -0,0 +1,113 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * HAL Timers for Teensy 3.2 (MK20DX256)
+ */
+
+#ifdef __MK20DX256__
+
+#include "../../inc/MarlinConfig.h"
+
+/** \brief Instruction Synchronization Barrier
+  Instruction Synchronization Barrier flushes the pipeline in the processor,
+  so that all instructions following the ISB are fetched from cache or
+  memory, after the instruction has been completed.
+*/
+FORCE_INLINE static void __ISB() {
+  __asm__ __volatile__("isb 0xF":::"memory");
+}
+
+/** \brief Data Synchronization Barrier
+  This function acts as a special kind of Data Memory Barrier.
+  It completes when all explicit memory accesses before this instruction complete.
+*/
+FORCE_INLINE static void __DSB() {
+  __asm__ __volatile__("dsb 0xF":::"memory");
+}
+
+void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
+  switch (timer_num) {
+    case 0:
+      FTM0_MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN;
+      FTM0_SC = 0x00; // Set this to zero before changing the modulus
+      FTM0_CNT = 0x0000; // Reset the count to zero
+      FTM0_MOD = 0xFFFF; // max modulus = 65535
+      FTM0_C0V = (FTM0_TIMER_RATE) / frequency; // Initial FTM Channel 0 compare value
+      FTM0_SC = (FTM_SC_CLKS(0b1) & FTM_SC_CLKS_MASK) | (FTM_SC_PS(FTM0_TIMER_PRESCALE_BITS) & FTM_SC_PS_MASK); // Bus clock 60MHz divided by prescaler 8
+      FTM0_C0SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSA;
+      break;
+    case 1:
+      FTM1_MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN; // Disable write protection, Enable FTM1
+      FTM1_SC = 0x00; // Set this to zero before changing the modulus
+      FTM1_CNT = 0x0000; // Reset the count to zero
+      FTM1_MOD = 0xFFFF; // max modulus = 65535
+      FTM1_C0V = (FTM1_TIMER_RATE) / frequency; // Initial FTM Channel 0 compare value 65535
+      FTM1_SC = (FTM_SC_CLKS(0b1) & FTM_SC_CLKS_MASK) | (FTM_SC_PS(FTM1_TIMER_PRESCALE_BITS) & FTM_SC_PS_MASK); // Bus clock 60MHz divided by prescaler 4
+      FTM1_C0SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSA;
+      break;
+  }
+}
+
+void HAL_timer_enable_interrupt(const uint8_t timer_num) {
+  switch (timer_num) {
+    case 0: NVIC_ENABLE_IRQ(IRQ_FTM0); break;
+    case 1: NVIC_ENABLE_IRQ(IRQ_FTM1); break;
+  }
+}
+
+void HAL_timer_disable_interrupt(const uint8_t timer_num) {
+  switch (timer_num) {
+    case 0: NVIC_DISABLE_IRQ(IRQ_FTM0); break;
+    case 1: NVIC_DISABLE_IRQ(IRQ_FTM1); break;
+  }
+
+  // We NEED memory barriers to ensure Interrupts are actually disabled!
+  // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+  __DSB();
+  __ISB();
+}
+
+bool HAL_timer_interrupt_enabled(const uint8_t timer_num) {
+  switch (timer_num) {
+    case 0: return NVIC_IS_ENABLED(IRQ_FTM0);
+    case 1: return NVIC_IS_ENABLED(IRQ_FTM1);
+  }
+  return false;
+}
+
+void HAL_timer_isr_prologue(const uint8_t timer_num) {
+  switch (timer_num) {
+    case 0:
+      FTM0_CNT = 0x0000;
+      FTM0_SC &= ~FTM_SC_TOF; // Clear FTM Overflow flag
+      FTM0_C0SC &= ~FTM_CSC_CHF; // Clear FTM Channel Compare flag
+      break;
+    case 1:
+      FTM1_CNT = 0x0000;
+      FTM1_SC &= ~FTM_SC_TOF; // Clear FTM Overflow flag
+      FTM1_C0SC &= ~FTM_CSC_CHF; // Clear FTM Channel Compare flag
+      break;
+  }
+}
+
+#endif // __MK20DX256__
diff --git a/Marlin/src/HAL/TEENSY31_32/timers.h b/Marlin/src/HAL/TEENSY31_32/timers.h
new file mode 100644
index 0000000..61b8673
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/timers.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
+
+/**
+ * HAL Timers for Teensy 3.2 (MK20DX256)
+ */
+
+#include <stdint.h>
+
+// ------------------------
+// Defines
+// ------------------------
+
+#define FORCE_INLINE __attribute__((always_inline)) inline
+
+typedef uint32_t hal_timer_t;
+#define HAL_TIMER_TYPE_MAX 0xFFFFFFFF
+
+#define FTM0_TIMER_PRESCALE 8
+#define FTM1_TIMER_PRESCALE 4
+#define FTM0_TIMER_PRESCALE_BITS 0b011
+#define FTM1_TIMER_PRESCALE_BITS 0b010
+
+#define FTM0_TIMER_RATE (F_BUS / (FTM0_TIMER_PRESCALE)) // 60MHz / 8 = 7500kHz
+#define FTM1_TIMER_RATE (F_BUS / (FTM1_TIMER_PRESCALE)) // 60MHz / 4 = 15MHz
+
+#define HAL_TIMER_RATE         (FTM0_TIMER_RATE)
+
+#ifndef STEP_TIMER_NUM
+  #define STEP_TIMER_NUM        0  // Timer Index for Stepper
+#endif
+#ifndef PULSE_TIMER_NUM
+  #define PULSE_TIMER_NUM       STEP_TIMER_NUM
+#endif
+#ifndef TEMP_TIMER_NUM
+  #define TEMP_TIMER_NUM        1  // Timer Index for Temperature
+#endif
+
+#define TEMP_TIMER_FREQUENCY    1000
+
+#define STEPPER_TIMER_RATE     HAL_TIMER_RATE
+#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000)
+#define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US)
+
+#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() HAL_timer_enable_interrupt(STEP_TIMER_NUM)
+#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(STEP_TIMER_NUM)
+#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(STEP_TIMER_NUM)
+
+#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(TEMP_TIMER_NUM)
+#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(TEMP_TIMER_NUM)
+
+#ifndef HAL_STEP_TIMER_ISR
+  #define HAL_STEP_TIMER_ISR() extern "C" void ftm0_isr() //void TC3_Handler()
+#endif
+#ifndef HAL_TEMP_TIMER_ISR
+  #define HAL_TEMP_TIMER_ISR() extern "C" void ftm1_isr() //void TC4_Handler()
+#endif
+
+void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency);
+
+FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) {
+  switch (timer_num) {
+    case 0: FTM0_C0V = compare; break;
+    case 1: FTM1_C0V = compare; break;
+  }
+}
+
+FORCE_INLINE static hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) {
+  switch (timer_num) {
+    case 0: return FTM0_C0V;
+    case 1: return FTM1_C0V;
+  }
+  return 0;
+}
+
+FORCE_INLINE static hal_timer_t HAL_timer_get_count(const uint8_t timer_num) {
+  switch (timer_num) {
+    case 0: return FTM0_CNT;
+    case 1: return FTM1_CNT;
+  }
+  return 0;
+}
+
+void HAL_timer_enable_interrupt(const uint8_t timer_num);
+void HAL_timer_disable_interrupt(const uint8_t timer_num);
+bool HAL_timer_interrupt_enabled(const uint8_t timer_num);
+
+void HAL_timer_isr_prologue(const uint8_t timer_num);
+#define HAL_timer_isr_epilogue(TIMER_NUM)
diff --git a/Marlin/src/HAL/TEENSY31_32/watchdog.cpp b/Marlin/src/HAL/TEENSY31_32/watchdog.cpp
new file mode 100644
index 0000000..5e21236
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/watchdog.cpp
@@ -0,0 +1,40 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#ifdef __MK20DX256__
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(USE_WATCHDOG)
+
+#include "watchdog.h"
+
+#define WDT_TIMEOUT_MS TERN(WATCHDOG_DURATION_8S, 8000, 4000) // 4 or 8 second timeout
+
+void watchdog_init() {
+  WDOG_TOVALH = 0;
+  WDOG_TOVALL = WDT_TIMEOUT_MS;
+  WDOG_STCTRLH = WDOG_STCTRLH_WDOGEN;
+}
+
+#endif // USE_WATCHDOG
+
+#endif // __MK20DX256__
diff --git a/Marlin/src/HAL/TEENSY31_32/watchdog.h b/Marlin/src/HAL/TEENSY31_32/watchdog.h
new file mode 100644
index 0000000..b8b46a4
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY31_32/watchdog.h
@@ -0,0 +1,34 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "HAL.h"
+
+// Arduino Due core now has watchdog support
+
+void watchdog_init();
+
+inline void HAL_watchdog_refresh() {
+  // Watchdog refresh sequence
+  WDOG_REFRESH = 0xA602;
+  WDOG_REFRESH = 0xB480;
+}