Initial commit

18 files changed, 1380 insertions, 0 deletions

diff --git a/Marlin/src/HAL/TEENSY40_41/HAL.cpp b/Marlin/src/HAL/TEENSY40_41/HAL.cpp
new file mode 100644
index 0000000..26449d7
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/HAL.cpp
@@ -0,0 +1,170 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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 4.0 / 4.1 (IMXRT1062)
+ */
+
+#ifdef __IMXRT1062__
+
+#include "HAL.h"
+#include "../shared/Delay.h"
+#include "timers.h"
+
+#include <Wire.h>
+
+DefaultSerial MSerial(false);
+USBSerialType USBSerial(false, SerialUSB);
+
+uint16_t HAL_adc_result, HAL_adc_select;
+
+static const uint8_t pin2sc1a[] = {
+  0x07,  // 0/A0  AD_B1_02
+  0x08,  // 1/A1  AD_B1_03
+  0x0C,  // 2/A2  AD_B1_07
+  0x0B,  // 3/A3  AD_B1_06
+  0x06,  // 4/A4  AD_B1_01
+  0x05,  // 5/A5  AD_B1_00
+  0x0F,  // 6/A6  AD_B1_10
+  0x00,  // 7/A7  AD_B1_11
+  0x0D,  // 8/A8  AD_B1_08
+  0x0E,  // 9/A9  AD_B1_09
+  0x01,  // 24/A10 AD_B0_12
+  0x02,  // 25/A11 AD_B0_13
+  0x83,  // 26/A12 AD_B1_14 - only on ADC2, 3
+  0x84,  // 27/A13 AD_B1_15 - only on ADC2, 4
+  0x07,  // 14/A0  AD_B1_02
+  0x08,  // 15/A1  AD_B1_03
+  0x0C,  // 16/A2  AD_B1_07
+  0x0B,  // 17/A3  AD_B1_06
+  0x06,  // 18/A4  AD_B1_01
+  0x05,  // 19/A5  AD_B1_00
+  0x0F,  // 20/A6  AD_B1_10
+  0x00,  // 21/A7  AD_B1_11
+  0x0D,  // 22/A8  AD_B1_08
+  0x0E,  // 23/A9  AD_B1_09
+  0x01,  // 24/A10 AD_B0_12
+  0x02,  // 25/A11 AD_B0_13
+  0x83,  // 26/A12 AD_B1_14 - only on ADC2, 3
+  0x84,  // 27/A13 AD_B1_15 - only on ADC2, 4
+  #ifdef ARDUINO_TEENSY41
+    0xFF,  // 28
+    0xFF,  // 29
+    0xFF,  // 30
+    0xFF,  // 31
+    0xFF,  // 32
+    0xFF,  // 33
+    0xFF,  // 34
+    0xFF,  // 35
+    0xFF,  // 36
+    0xFF,  // 37
+    0x81,  // 38/A14 AD_B1_12 - only on ADC2, 1
+    0x82,  // 39/A15 AD_B1_13 - only on ADC2, 2
+    0x09,  // 40/A16 AD_B1_04
+    0x0A,  // 41/A17 AD_B1_05
+  #endif
+};
+
+/*
+// disable interrupts
+void cli() { noInterrupts(); }
+
+// enable interrupts
+void sei() { interrupts(); }
+*/
+
+void HAL_adc_init() {
+  analog_init();
+  while (ADC1_GC & ADC_GC_CAL) ;
+  while (ADC2_GC & ADC_GC_CAL) ;
+}
+
+void HAL_clear_reset_source() {
+  uint32_t reset_source = SRC_SRSR;
+  SRC_SRSR = reset_source;
+ }
+
+uint8_t HAL_get_reset_source() {
+  switch (SRC_SRSR & 0xFF) {
+    case 1: return RST_POWER_ON; break;
+    case 2: return RST_SOFTWARE; break;
+    case 4: return RST_EXTERNAL; break;
+    // case 8: return RST_BROWN_OUT; break;
+    case 16: return RST_WATCHDOG; break;
+     case 64: return RST_JTAG; break;
+    // case 128: return RST_OVERTEMP; break;
+  }
+  return 0;
+}
+
+#define __bss_end _ebss
+
+extern "C" {
+  extern char __bss_end;
+  extern char __heap_start;
+  extern void* __brkval;
+
+  // Doesn't work on Teensy 4.x
+  uint32_t freeMemory() {
+    uint32_t free_memory;
+    if ((uint32_t)__brkval == 0)
+      free_memory = ((uint32_t)&free_memory) - ((uint32_t)&__bss_end);
+    else
+      free_memory = ((uint32_t)&free_memory) - ((uint32_t)__brkval);
+    return free_memory;
+  }
+}
+
+void HAL_adc_start_conversion(const uint8_t adc_pin) {
+  const uint16_t pin = pin2sc1a[adc_pin];
+  if (pin == 0xFF) {
+    HAL_adc_select = -1; // Digital only
+  }
+  else if (pin & 0x80) {
+    HAL_adc_select = 1;
+    ADC2_HC0 = pin & 0x7F;
+  }
+  else {
+    HAL_adc_select = 0;
+    ADC1_HC0 = pin;
+  }
+}
+
+uint16_t HAL_adc_get_result() {
+  switch (HAL_adc_select) {
+    case 0:
+      while (!(ADC1_HS & ADC_HS_COCO0)) ; // wait
+      return ADC1_R0;
+    case 1:
+      while (!(ADC2_HS & ADC_HS_COCO0)) ; // wait
+      return ADC2_R0;
+  }
+  return 0;
+}
+
+bool is_output(uint8_t pin) {
+  const struct digital_pin_bitband_and_config_table_struct *p;
+  p = digital_pin_to_info_PGM + pin;
+  return (*(p->reg + 1) & p->mask);
+}
+
+#endif // __IMXRT1062__
diff --git a/Marlin/src/HAL/TEENSY40_41/HAL.h b/Marlin/src/HAL/TEENSY40_41/HAL.h
new file mode 100644
index 0000000..6aa1e52
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/HAL.h
@@ -0,0 +1,153 @@
+/**
+ * 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 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A)
+ */
+
+#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>
+#include <util/atomic.h>
+
+#if HAS_ETHERNET
+  #include "../../feature/ethernet.h"
+#endif
+
+//#define ST7920_DELAY_1 DELAY_NS(600)
+//#define ST7920_DELAY_2 DELAY_NS(750)
+//#define ST7920_DELAY_3 DELAY_NS(750)
+
+// ------------------------
+// Defines
+// ------------------------
+
+#define IS_32BIT_TEENSY 1
+#define IS_TEENSY_40_41 1
+#ifndef IS_TEENSY40
+  #define IS_TEENSY41 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 SerialUSB
+#elif WITHIN(SERIAL_PORT, 0, 8)
+  #define MYSERIAL0 MSERIAL(SERIAL_PORT)
+#else
+  #error "The required SERIAL_PORT must be from -1 to 8. Please update your configuration."
+#endif
+
+#ifdef SERIAL_PORT_2
+  #if SERIAL_PORT_2 == -1
+    #define MYSERIAL1 usbSerial
+  #elif SERIAL_PORT_2 == -2
+    #define MYSERIAL1 ethernet.telnetClient
+  #elif WITHIN(SERIAL_PORT_2, 0, 8)
+    #define MYSERIAL1 MSERIAL(SERIAL_PORT_2)
+  #else
+    #error "SERIAL_PORT_2 must be from -2 to 8. Please update your configuration."
+  #endif
+#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()
+
+#undef sq
+#define sq(x) ((x)*(x))
+
+// Don't place string constants in PROGMEM
+#undef PSTR
+#define PSTR(str) ({static const char *data = (str); &data[0];})
+
+// Enable hooks into idle and setup for HAL
+#define HAL_IDLETASK 1
+FORCE_INLINE void HAL_idletask() {}
+FORCE_INLINE void HAL_init() {}
+
+// Clear reset reason
+void HAL_clear_reset_source();
+
+// Reset reason
+uint8_t HAL_get_reset_source();
+
+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" uint32_t 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_ADC_FILTERED      // turn off ADC oversampling
+#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)
+
+bool is_output(uint8_t pin);
diff --git a/Marlin/src/HAL/TEENSY40_41/HAL_SPI.cpp b/Marlin/src/HAL/TEENSY40_41/HAL_SPI.cpp
new file mode 100644
index 0000000..8c93049
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/HAL_SPI.cpp
@@ -0,0 +1,143 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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 SPI for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A)
+ */
+
+#ifdef __IMXRT1062__
+
+#include "HAL.h"
+#include <SPI.h>
+#include <pins_arduino.h>
+#include "spi_pins.h"
+#include "../../core/macros.h"
+
+static SPISettings spiConfig;
+
+// ------------------------
+// Public functions
+// ------------------------
+
+#if ENABLED(SOFTWARE_SPI)
+  // ------------------------
+  // Software SPI
+  // ------------------------
+  #error "Software SPI not supported for Teensy 4. Use Hardware SPI."
+#else
+
+// ------------------------
+// Hardware SPI
+// ------------------------
+
+void spiBegin() {
+  #ifndef SD_SS_PIN
+    #error "SD_SS_PIN is 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
+}
+
+void spiInit(uint8_t spiRate) {
+  // Use Marlin data-rates
+  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();
+}
+
+uint8_t spiRec() {
+  SPI.beginTransaction(spiConfig);
+  uint8_t returnByte = SPI.transfer(0xFF);
+  SPI.endTransaction();
+  return returnByte;
+  //SPDR = 0xFF;
+  //while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
+  //return SPDR;
+}
+
+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;
+}
+
+void spiSend(uint8_t b) {
+  SPI.beginTransaction(spiConfig);
+  SPI.transfer(b);
+  SPI.endTransaction();
+  //SPDR = b;
+  //while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
+}
+
+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 // SOFTWARE_SPI
+#endif // __IMXRT1062__
diff --git a/Marlin/src/HAL/TEENSY40_41/Servo.cpp b/Marlin/src/HAL/TEENSY40_41/Servo.cpp
new file mode 100644
index 0000000..ffb1102
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/Servo.cpp
@@ -0,0 +1,61 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * HAL Servo for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A)
+ */
+
+#ifdef __IMXRT1062__
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_SERVOS
+
+#include "Servo.h"
+
+int8_t libServo::attach(const int inPin) {
+  if (inPin > 0) servoPin = inPin;
+  return super::attach(servoPin);
+}
+
+int8_t libServo::attach(const int inPin, const int inMin, const int inMax) {
+  if (inPin > 0) servoPin = inPin;
+  return super::attach(servoPin, 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());
+  }
+}
+
+void libServo::detach() {
+  // PWMServo library does not have detach() function
+  //super::detach();
+}
+
+#endif // HAS_SERVOS
+#endif // __IMXRT1062__
diff --git a/Marlin/src/HAL/TEENSY40_41/Servo.h b/Marlin/src/HAL/TEENSY40_41/Servo.h
new file mode 100644
index 0000000..699fd70
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/Servo.h
@@ -0,0 +1,43 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * HAL Servo for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A)
+ */
+
+#include <PWMServo.h>
+
+// Inherit and expand on core Servo library
+class libServo : public PWMServo {
+  public:
+    int8_t attach(const int pin);
+    int8_t attach(const int pin, const int min, const int max);
+    void move(const int value);
+    void detach(void);
+  private:
+    typedef PWMServo super;
+    uint8_t servoPin;
+    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/TEENSY40_41/eeprom.cpp b/Marlin/src/HAL/TEENSY40_41/eeprom.cpp
new file mode 100644
index 0000000..fe2de38
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/eeprom.cpp
@@ -0,0 +1,76 @@
+/**
+ * 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
+ * Copyright (c) 2016 Victor Perez victor_pv@hotmail.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/>.
+ *
+ */
+#ifdef __IMXRT1062__
+
+#include "../../inc/MarlinConfig.h"
+
+#if USE_WIRED_EEPROM
+
+/**
+ * HAL PersistentStore for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A)
+ */
+
+#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 // __IMXRT1062__
diff --git a/Marlin/src/HAL/TEENSY40_41/endstop_interrupts.h b/Marlin/src/HAL/TEENSY40_41/endstop_interrupts.h
new file mode 100644
index 0000000..a05e911
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/endstop_interrupts.h
@@ -0,0 +1,66 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * HAL Endstop Interrupts for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A)
+ *
+ * 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/TEENSY40_41/fastio.h b/Marlin/src/HAL/TEENSY40_41/fastio.h
new file mode 100644
index 0000000..52f991d
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/fastio.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
+ * Copyright (c) 2017 Victor Perez
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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 interfaces for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A)
+ * These use GPIO functions instead of Direct Port Manipulation, as on AVR.
+ */
+
+#ifndef PWM
+  #define PWM OUTPUT
+#endif
+
+#define READ(IO)                digitalRead(IO)
+#define WRITE(IO,V)             digitalWrite(IO,V)
+
+#define _GET_MODE(IO)           !is_output(IO)
+#define _SET_MODE(IO,M)         pinMode(IO, M)
+#define _SET_OUTPUT(IO)         pinMode(IO, OUTPUT)                               /*!< Output Push Pull Mode & GPIO_NOPULL   */
+
+#define OUT_WRITE(IO,V)         do{ _SET_OUTPUT(IO); WRITE(IO,V); }while(0)
+
+#define SET_INPUT(IO)           _SET_MODE(IO, INPUT)                              /*!< Input Floating Mode                   */
+#define SET_INPUT_PULLUP(IO)    _SET_MODE(IO, INPUT_PULLUP)                       /*!< Input with Pull-up activation         */
+#define SET_INPUT_PULLDOWN(IO)  _SET_MODE(IO, INPUT_PULLDOWN)                     /*!< Input with Pull-down activation       */
+#define SET_OUTPUT(IO)          OUT_WRITE(IO, LOW)
+#define SET_PWM(IO)             _SET_MODE(IO, PWM)
+
+#define TOGGLE(IO)              OUT_WRITE(IO, !READ(IO))
+
+#define IS_INPUT(IO)            !is_output(IO)
+#define IS_OUTPUT(IO)           is_output(IO)
+
+#define PWM_PIN(P)            digitalPinHasPWM(P)
+
+// digitalRead/Write wrappers
+#define extDigitalRead(IO)    digitalRead(IO)
+#define extDigitalWrite(IO,V) digitalWrite(IO,V)
diff --git a/Marlin/src/HAL/TEENSY40_41/inc/Conditionals_LCD.h b/Marlin/src/HAL/TEENSY40_41/inc/Conditionals_LCD.h
new file mode 100644
index 0000000..6a85409
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/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/TEENSY40_41."
+#endif
diff --git a/Marlin/src/HAL/TEENSY40_41/inc/Conditionals_adv.h b/Marlin/src/HAL/TEENSY40_41/inc/Conditionals_adv.h
new file mode 100644
index 0000000..5f1c4b1
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/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/TEENSY40_41/inc/Conditionals_post.h b/Marlin/src/HAL/TEENSY40_41/inc/Conditionals_post.h
new file mode 100644
index 0000000..998f1dc
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/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/TEENSY40_41/inc/SanityCheck.h b/Marlin/src/HAL/TEENSY40_41/inc/SanityCheck.h
new file mode 100644
index 0000000..fbfe7b0
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/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 TEENSY41 specific configuration values for errors at compile-time.
+ */
+
+#if ENABLED(EMERGENCY_PARSER)
+  #error "EMERGENCY_PARSER is not yet implemented for Teensy 4.0/4.1. 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 4.0/4.1."
+#endif
+
+#if HAS_TMC_SW_SERIAL
+  #error "TMC220x Software Serial is not supported on this platform."
+#endif
diff --git a/Marlin/src/HAL/TEENSY40_41/pinsDebug.h b/Marlin/src/HAL/TEENSY40_41/pinsDebug.h
new file mode 100644
index 0000000..4ad62d0
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/pinsDebug.h
@@ -0,0 +1,150 @@
+/**
+ * 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
+
+/**
+ * HAL Pins Debugging for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A)
+ */
+
+#warning "PINS_DEBUGGING is not fully supported for Teensy 4.0 / 4.1 so 'M43' may cause hangs."
+
+#define NUMBER_PINS_TOTAL NUM_DIGITAL_PINS
+
+#define digitalRead_mod(p) extDigitalRead(p)  // AVR digitalRead disabled PWM before it read the pin
+#define PRINT_PORT(p)
+#define PRINT_ARRAY_NAME(x) do{ sprintf_P(buffer, PSTR("%-" STRINGIFY(MAX_NAME_LENGTH) "s"), pin_array[x].name); SERIAL_ECHO(buffer); }while(0)
+#define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("%02d"), p); SERIAL_ECHO(buffer); }while(0)
+#define GET_ARRAY_PIN(p) pin_array[p].pin
+#define GET_ARRAY_IS_DIGITAL(p) pin_array[p].is_digital
+#define VALID_PIN(pin) (pin >= 0 && pin < (int8_t)NUMBER_PINS_TOTAL ? 1 : 0)
+#define DIGITAL_PIN_TO_ANALOG_PIN(p) int(p - analogInputToDigitalPin(0))
+#define IS_ANALOG(P) ((P) >= analogInputToDigitalPin(0) && (P) <= analogInputToDigitalPin(13)) || ((P) >= analogInputToDigitalPin(14) && (P) <= analogInputToDigitalPin(17))
+#define pwm_status(pin) HAL_pwm_status(pin)
+#define GET_PINMODE(PIN) (VALID_PIN(pin) && IS_OUTPUT(pin))
+#define MULTI_NAME_PAD 16 // space needed to be pretty if not first name assigned to a pin
+
+struct pwm_pin_info_struct {
+  uint8_t type;    // 0=no pwm, 1=flexpwm, 2=quad
+  uint8_t module;  // 0-3, 0-3
+  uint8_t channel; // 0=X, 1=A, 2=B
+  uint8_t muxval;  //
+};
+
+#define M(a, b) ((((a) - 1) << 4) | (b))
+
+const struct pwm_pin_info_struct pwm_pin_info[] = {
+  {1, M(1, 1), 0, 4},  // FlexPWM1_1_X   0  // AD_B0_03
+  {1, M(1, 0), 0, 4},  // FlexPWM1_0_X   1  // AD_B0_02
+  {1, M(4, 2), 1, 1},  // FlexPWM4_2_A   2  // EMC_04
+  {1, M(4, 2), 2, 1},  // FlexPWM4_2_B   3  // EMC_05
+  {1, M(2, 0), 1, 1},  // FlexPWM2_0_A   4  // EMC_06
+  {1, M(2, 1), 1, 1},  // FlexPWM2_1_A   5  // EMC_08
+  {1, M(2, 2), 1, 2},  // FlexPWM2_2_A   6  // B0_10
+  {1, M(1, 3), 2, 6},  // FlexPWM1_3_B   7  // B1_01
+  {1, M(1, 3), 1, 6},  // FlexPWM1_3_A   8  // B1_00
+  {1, M(2, 2), 2, 2},  // FlexPWM2_2_B   9  // B0_11
+  {2, M(1, 0), 0, 1},  // QuadTimer1_0  10  // B0_00
+  {2, M(1, 2), 0, 1},  // QuadTimer1_2  11  // B0_02
+  {2, M(1, 1), 0, 1},  // QuadTimer1_1  12  // B0_01
+  {2, M(2, 0), 0, 1},  // QuadTimer2_0  13  // B0_03
+  {2, M(3, 2), 0, 1},  // QuadTimer3_2  14  // AD_B1_02
+  {2, M(3, 3), 0, 1},  // QuadTimer3_3  15  // AD_B1_03
+  {0, M(1, 0), 0, 0},
+  {0, M(1, 0), 0, 0},
+  {2, M(3, 1), 0, 1},  // QuadTimer3_1  18  // AD_B1_01
+  {2, M(3, 0), 0, 1},  // QuadTimer3_0  19  // AD_B1_00
+  {0, M(1, 0), 0, 0},
+  {0, M(1, 0), 0, 0},
+  {1, M(4, 0), 1, 1},  // FlexPWM4_0_A  22  // AD_B1_08
+  {1, M(4, 1), 1, 1},  // FlexPWM4_1_A  23  // AD_B1_09
+  {1, M(1, 2), 0, 4},  // FlexPWM1_2_X  24  // AD_B0_12
+  {1, M(1, 3), 0, 4},  // FlexPWM1_3_X  25  // AD_B0_13
+  {0, M(1, 0), 0, 0},
+  {0, M(1, 0), 0, 0},
+  {1, M(3, 1), 2, 1},  // FlexPWM3_1_B  28  // EMC_32
+  {1, M(3, 1), 1, 1},  // FlexPWM3_1_A  29  // EMC_31
+  {0, M(1, 0), 0, 0},
+  {0, M(1, 0), 0, 0},
+  {0, M(1, 0), 0, 0},
+  {1, M(2, 0), 2, 1},  // FlexPWM2_0_B  33  // EMC_07
+  #ifdef ARDUINO_TEENSY40
+    {1, M(1, 1), 2, 1},  // FlexPWM1_1_B  34  // SD_B0_03
+    {1, M(1, 1), 1, 1},  // FlexPWM1_1_A  35  // SD_B0_02
+    {1, M(1, 0), 2, 1},  // FlexPWM1_0_B  36  // SD_B0_01
+    {1, M(1, 0), 1, 1},  // FlexPWM1_0_A  37  // SD_B0_00
+    {1, M(1, 2), 2, 1},  // FlexPWM1_2_B  38  // SD_B0_05
+    {1, M(1, 2), 1, 1},  // FlexPWM1_2_A  39  // SD_B0_04
+  #endif
+  #ifdef ARDUINO_TEENSY41
+    {0, M(1, 0), 0, 0},
+    {0, M(1, 0), 0, 0},
+    {1, M(2, 3), 1, 6},  // FlexPWM2_3_A  36  // B1_00
+    {1, M(2, 3), 2, 6},  // FlexPWM2_3_B  37  // B1_01
+    {0, M(1, 0), 0, 0},
+    {0, M(1, 0), 0, 0},
+    {0, M(1, 0), 0, 0},
+    {0, M(1, 0), 0, 0},
+    {1, M(1, 1), 2, 1},  // FlexPWM1_1_B  42  // SD_B0_03
+    {1, M(1, 1), 1, 1},  // FlexPWM1_1_A  43  // SD_B0_02
+    {1, M(1, 0), 2, 1},  // FlexPWM1_0_B  44  // SD_B0_01
+    {1, M(1, 0), 1, 1},  // FlexPWM1_0_A  45  // SD_B0_00
+    {1, M(1, 2), 2, 1},  // FlexPWM1_2_B  46  // SD_B0_05
+    {1, M(1, 2), 1, 1},  // FlexPWM1_2_A  47  // SD_B0_04
+    {0, M(1, 0), 0, 0},  // duplicate FlexPWM1_0_B
+    {0, M(1, 0), 0, 0},  // duplicate FlexPWM1_2_A
+    {0, M(1, 0), 0, 0},  // duplicate FlexPWM1_2_B
+    {1, M(3, 3), 2, 1},  // FlexPWM3_3_B  51  // EMC_22
+    {0, M(1, 0), 0, 0},  // duplicate FlexPWM1_1_B
+    {0, M(1, 0), 0, 0},  // duplicate FlexPWM1_1_A
+    {1, M(3, 0), 1, 1},  // FlexPWM3_0_A  53  // EMC_29
+  #endif
+};
+
+void HAL_print_analog_pin(char buffer[], int8_t pin) {
+  if (pin <= 23)      sprintf_P(buffer, PSTR("(A%2d)  "), int(pin - 14));
+  else if (pin <= 41) sprintf_P(buffer, PSTR("(A%2d)  "), int(pin - 24));
+}
+
+void HAL_analog_pin_state(char buffer[], int8_t pin) {
+  if (pin <= 23)      sprintf_P(buffer, PSTR("Analog in =% 5d"), analogRead(pin - 14));
+  else if (pin <= 41) sprintf_P(buffer, PSTR("Analog in =% 5d"), analogRead(pin - 24));
+}
+
+#define PWM_PRINT(V) do{ sprintf_P(buffer, PSTR("PWM:  %4d"), V); SERIAL_ECHO(buffer); }while(0)
+
+/**
+ * Print a pin's PWM status.
+ * Return true if it's currently a PWM pin.
+ */
+bool HAL_pwm_status(int8_t pin) {
+  char buffer[20];   // for the sprintf statements
+  const struct pwm_pin_info_struct *info;
+
+  if (pin >= CORE_NUM_DIGITAL) return 0;
+  info = pwm_pin_info + pin;
+
+  if (info->type == 0) return 0;
+
+  /* TODO decode pwm value from timers */
+  // for now just indicate if output is set as pwm
+  PWM_PRINT(*(portConfigRegister(pin)) == info->muxval);
+  return (*(portConfigRegister(pin)) == info->muxval);
+}
+
+static void pwm_details(uint8_t pin) { /* TODO */ }
diff --git a/Marlin/src/HAL/TEENSY40_41/spi_pins.h b/Marlin/src/HAL/TEENSY40_41/spi_pins.h
new file mode 100644
index 0000000..ba4a2c7
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/spi_pins.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
+
+/**
+ * HAL SPI Pins for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A)
+ */
+
+#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/TEENSY40_41/timers.cpp b/Marlin/src/HAL/TEENSY40_41/timers.cpp
new file mode 100644
index 0000000..81c9b08
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/timers.cpp
@@ -0,0 +1,114 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * HAL Timers for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A)
+ */
+
+#ifdef __IMXRT1062__
+
+#include "../../inc/MarlinConfig.h"
+
+void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
+  switch (timer_num) {
+    case 0:
+      CCM_CSCMR1 &= ~CCM_CSCMR1_PERCLK_CLK_SEL; // turn off 24mhz mode
+      CCM_CCGR1 |= CCM_CCGR1_GPT1_BUS(CCM_CCGR_ON);
+
+      GPT1_CR = 0;                   // disable timer
+      GPT1_SR = 0x3F;                // clear all prior status
+      GPT1_PR = GPT1_TIMER_PRESCALE - 1;
+      GPT1_CR |= GPT_CR_CLKSRC(1);   //clock selection #1 (peripheral clock = 150 MHz)
+      GPT1_CR |= GPT_CR_ENMOD;       //reset count to zero before enabling
+      GPT1_CR |= GPT_CR_OM1(1);      // toggle mode
+      GPT1_OCR1 = (GPT1_TIMER_RATE / frequency) -1; // Initial compare value
+      GPT1_IR = GPT_IR_OF1IE;        // Compare3 value
+      GPT1_CR |= GPT_CR_EN;          //enable GPT2 counting at 150 MHz
+
+      OUT_WRITE(15, HIGH);
+      attachInterruptVector(IRQ_GPT1, &stepTC_Handler);
+      NVIC_SET_PRIORITY(IRQ_GPT1, 16);
+      break;
+    case 1:
+      CCM_CSCMR1 &= ~CCM_CSCMR1_PERCLK_CLK_SEL; // turn off 24mhz mode
+      CCM_CCGR0 |= CCM_CCGR0_GPT2_BUS(CCM_CCGR_ON);
+
+      GPT2_CR = 0;                   // disable timer
+      GPT2_SR = 0x3F;                // clear all prior status
+      GPT2_PR = GPT2_TIMER_PRESCALE - 1;
+      GPT2_CR |= GPT_CR_CLKSRC(1);   //clock selection #1 (peripheral clock = 150 MHz)
+      GPT2_CR |= GPT_CR_ENMOD;       //reset count to zero before enabling
+      GPT2_CR |= GPT_CR_OM1(1);      // toggle mode
+      GPT2_OCR1 = (GPT2_TIMER_RATE / frequency) -1; // Initial compare value
+      GPT2_IR = GPT_IR_OF1IE;        // Compare3 value
+      GPT2_CR |= GPT_CR_EN;          //enable GPT2 counting at 150 MHz
+
+      OUT_WRITE(14, HIGH);
+      attachInterruptVector(IRQ_GPT2, &tempTC_Handler);
+      NVIC_SET_PRIORITY(IRQ_GPT2, 32);
+      break;
+  }
+}
+
+void HAL_timer_enable_interrupt(const uint8_t timer_num) {
+  switch (timer_num) {
+    case 0:
+      NVIC_ENABLE_IRQ(IRQ_GPT1);
+      break;
+    case 1:
+      NVIC_ENABLE_IRQ(IRQ_GPT2);
+      break;
+  }
+}
+
+void HAL_timer_disable_interrupt(const uint8_t timer_num) {
+  switch (timer_num) {
+    case 0: NVIC_DISABLE_IRQ(IRQ_GPT1); break;
+    case 1: NVIC_DISABLE_IRQ(IRQ_GPT2); break;
+  }
+
+  // We NEED memory barriers to ensure Interrupts are actually disabled!
+  // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+  asm volatile("dsb");
+}
+
+bool HAL_timer_interrupt_enabled(const uint8_t timer_num) {
+  switch (timer_num) {
+    case 0: return (NVIC_IS_ENABLED(IRQ_GPT1));
+    case 1: return (NVIC_IS_ENABLED(IRQ_GPT2));
+  }
+  return false;
+}
+
+void HAL_timer_isr_prologue(const uint8_t timer_num) {
+  switch (timer_num) {
+    case 0:
+      GPT1_SR = GPT_IR_OF1IE;  // clear OF3 bit
+      break;
+    case 1:
+      GPT2_SR = GPT_IR_OF1IE;  // clear OF3 bit
+      break;
+  }
+  asm volatile("dsb");
+}
+
+#endif // __IMXRT1062__
diff --git a/Marlin/src/HAL/TEENSY40_41/timers.h b/Marlin/src/HAL/TEENSY40_41/timers.h
new file mode 100644
index 0000000..556333d
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/timers.h
@@ -0,0 +1,121 @@
+/**
+ * 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 Timers for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A)
+ */
+
+#include <stdint.h>
+
+// ------------------------
+// Defines
+// ------------------------
+
+#define FORCE_INLINE __attribute__((always_inline)) inline
+
+typedef uint32_t hal_timer_t;
+#define HAL_TIMER_TYPE_MAX 0xFFFFFFFE
+
+#define GPT_TIMER_RATE F_BUS_ACTUAL   // 150MHz
+
+#define GPT1_TIMER_PRESCALE 2
+#define GPT2_TIMER_PRESCALE 10
+
+#define GPT1_TIMER_RATE (GPT_TIMER_RATE / GPT1_TIMER_PRESCALE) // 75MHz
+#define GPT2_TIMER_RATE (GPT_TIMER_RATE / GPT2_TIMER_PRESCALE) // 15MHz
+
+#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_RATE        1000000
+#define TEMP_TIMER_FREQUENCY   1000
+
+#define STEPPER_TIMER_RATE     GPT1_TIMER_RATE
+#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000)
+#define STEPPER_TIMER_PRESCALE ((GPT_TIMER_RATE / 1000000) / 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 stepTC_Handler() // GPT1_Handler()
+#endif
+#ifndef HAL_TEMP_TIMER_ISR
+  #define HAL_TEMP_TIMER_ISR() extern "C" void tempTC_Handler() // GPT2_Handler()
+#endif
+
+extern "C" {
+  void stepTC_Handler();
+  void tempTC_Handler();
+}
+
+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:
+      GPT1_OCR1 = compare - 1;
+      break;
+    case 1:
+      GPT2_OCR1 = compare - 1;
+      break;
+  }
+}
+
+FORCE_INLINE static hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) {
+  switch (timer_num) {
+    case 0: return GPT1_OCR1;
+    case 1: return GPT2_OCR1;
+  }
+  return 0;
+}
+
+FORCE_INLINE static hal_timer_t HAL_timer_get_count(const uint8_t timer_num) {
+  switch (timer_num) {
+    case 0: return GPT1_CNT;
+    case 1: return GPT2_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);
+//void HAL_timer_isr_epilogue(const uint8_t timer_num) {}
+#define HAL_timer_isr_epilogue(TIMER_NUM)
diff --git a/Marlin/src/HAL/TEENSY40_41/watchdog.cpp b/Marlin/src/HAL/TEENSY40_41/watchdog.cpp
new file mode 100644
index 0000000..dd7c0aa
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/watchdog.cpp
@@ -0,0 +1,52 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#ifdef __IMXRT1062__
+
+/**
+ * HAL Watchdog for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A)
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(USE_WATCHDOG)
+
+#include "watchdog.h"
+
+#define WDT_TIMEOUT TERN(WATCHDOG_DURATION_8S, 8, 4) // 4 or 8 second timeout
+
+constexpr uint8_t timeoutval = (WDT_TIMEOUT - 0.5f) / 0.5f;
+
+void watchdog_init() {
+  CCM_CCGR3 |= CCM_CCGR3_WDOG1(3);  // enable WDOG1 clocks
+  WDOG1_WMCR = 0;                   // disable power down PDE
+  WDOG1_WCR |= WDOG_WCR_SRS | WDOG_WCR_WT(timeoutval);
+  WDOG1_WCR |= WDOG_WCR_WDE | WDOG_WCR_WDT | WDOG_WCR_SRE;
+}
+
+void HAL_watchdog_refresh() {
+  // Watchdog refresh sequence
+  WDOG1_WSR = 0x5555;
+  WDOG1_WSR = 0xAAAA;
+}
+
+#endif // USE_WATCHDOG
+#endif // __IMXRT1062__
diff --git a/Marlin/src/HAL/TEENSY40_41/watchdog.h b/Marlin/src/HAL/TEENSY40_41/watchdog.h
new file mode 100644
index 0000000..03ab151
--- /dev/null
+++ b/Marlin/src/HAL/TEENSY40_41/watchdog.h
@@ -0,0 +1,30 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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 Watchdog for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A)
+ */
+
+void watchdog_init();
+
+void HAL_watchdog_refresh();