Support for Teensy 4 (#19311)

4 years ago · ea39d3c45e
31 changed files with 1814 additions and 7 deletions
--- a/Marlin/src/HAL/TEENSY40_41/HAL.cpp
+++ b/Marlin/src/HAL/TEENSY40_41/HAL.cpp
@ -0,0 +1,167 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * Description: HAL for Teensy40 (IMXRT1062)
+ */
+
+#ifdef __IMXRT1062__
+
+#include "HAL.h"
+#include "../shared/Delay.h"
+#include "timers.h"
+
+#include <Wire.h>
+
+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__
--- a/Marlin/src/HAL/TEENSY40_41/HAL.h
+++ b/Marlin/src/HAL/TEENSY40_41/HAL.h
@ -0,0 +1,180 @@
+/**
+ * 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
+
+/**
+ * Description: HAL for Teensy 4.0 and Teensy 4.1
+ */
+
+#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>
+
+//#define ST7920_DELAY_1 DELAY_NS(600)
+//#define ST7920_DELAY_2 DELAY_NS(750)
+//#define ST7920_DELAY_3 DELAY_NS(750)
+
+// ------------------------
+// Defines
+// ------------------------
+
+#ifdef __IMXRT1062__
+  #define IS_32BIT_TEENSY 1
+  #define IS_TEENSY41 1
+#endif
+
+#if SERIAL_PORT == -1
+  #define MYSERIAL0 SerialUSB
+#elif SERIAL_PORT == 0
+  #define MYSERIAL0 Serial
+#elif SERIAL_PORT == 1
+  #define MYSERIAL0 Serial1
+#elif SERIAL_PORT == 2
+  #define MYSERIAL0 Serial2
+#elif SERIAL_PORT == 3
+  #define MYSERIAL0 Serial3
+#elif SERIAL_PORT == 4
+  #define MYSERIAL0 Serial4
+#elif SERIAL_PORT == 5
+  #define MYSERIAL0 Serial5
+#elif SERIAL_PORT == 6
+  #define MYSERIAL0 Serial6
+#elif SERIAL_PORT == 7
+  #define MYSERIAL0 Serial7
+#elif SERIAL_PORT == 8
+  #define MYSERIAL0 Serial8
+#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 == SERIAL_PORT
+    #error "SERIAL_PORT_2 must be different from SERIAL_PORT. Please update your configuration."
+  #elif SERIAL_PORT_2 == -1
+    #define MYSERIAL1 usbSerial
+  #elif SERIAL_PORT_2 == 0
+    #define MYSERIAL1 Serial
+  #elif SERIAL_PORT_2 == 1
+    #define MYSERIAL1 Serial1
+  #elif SERIAL_PORT_2 == 2
+    #define MYSERIAL1 Serial2
+  #elif SERIAL_PORT_2 == 3
+    #define MYSERIAL1 Serial3
+  #elif SERIAL_PORT_2 == 4
+    #define MYSERIAL1 Serial4
+  #elif SERIAL_PORT_2 == 5
+    #define MYSERIAL1 Serial5
+  #elif SERIAL_PORT_2 == 6
+    #define MYSERIAL1 Serial6
+  #elif SERIAL_PORT_2 == 7
+    #define MYSERIAL1 Serial7
+  #elif SERIAL_PORT_2 == 8
+    #define MYSERIAL1 Serial8
+  #else
+      #error "SERIAL_PORT_2 must be from -1 to 8. Please update your configuration."
+  #endif
+  #define NUM_SERIAL 2
+#else
+  #define NUM_SERIAL 1
+#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))
+
+#ifndef strncpy_P
+  #define strncpy_P(dest, src, num) strncpy((dest), (src), (num))
+#endif
+
+// Don't place string constants in PROGMEM
+#undef PSTR
+#define PSTR(str) ({static const char *data = (str); &data[0];})
+
+// Fix bug in pgm_read_ptr
+#undef pgm_read_ptr
+#define pgm_read_ptr(addr) (*((void**)(addr)))
+// Add type-checking to pgm_read_word
+#undef pgm_read_word
+#define pgm_read_word(addr) (*((uint16_t*)(addr)))
+
+// 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); }
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-function"
+extern "C" {
+  uint32_t freeMemory();
+}
+#pragma GCC diagnostic pop
+
+// 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);
--- a/Marlin/src/HAL/TEENSY40_41/HAL_SPI.cpp
+++ b/Marlin/src/HAL/TEENSY40_41/HAL_SPI.cpp
@ -0,0 +1,138 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#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 SS_PIN
+    #error "SS_PIN is not defined!"
+  #endif
+
+  OUT_WRITE(SS_PIN, HIGH);
+
+  //SET_OUTPUT(SCK_PIN);
+  //SET_INPUT(MISO_PIN);
+  //SET_OUTPUT(MOSI_PIN);
+
+  #if 0 && DISABLED(SOFTWARE_SPI)
+    // set SS high - may be chip select for another SPI device
+    #if SET_SPI_SS_HIGH
+      WRITE(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__
--- a/Marlin/src/HAL/TEENSY40_41/Servo.cpp
+++ b/Marlin/src/HAL/TEENSY40_41/Servo.cpp
@ -0,0 +1,57 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#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__
--- a/Marlin/src/HAL/TEENSY40_41/Servo.h
+++ b/Marlin/src/HAL/TEENSY40_41/Servo.h
@ -0,0 +1,39 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#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
+};
--- a/Marlin/src/HAL/TEENSY40_41/eeprom.cpp
+++ b/Marlin/src/HAL/TEENSY40_41/eeprom.cpp
@ -0,0 +1,77 @@
+/**
+ * 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
+
+/**
+ * PersistentStore for Arduino-style EEPROM interface
+ * with implementations supplied by the framework.
+ */
+
+#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__
--- a/Marlin/src/HAL/TEENSY40_41/endstop_interrupts.h
+++ 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
+
+/**
+ * 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));
+}
--- a/Marlin/src/HAL/TEENSY40_41/fastio.h
+++ 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
+ * 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)
--- a/Marlin/src/HAL/TEENSY40_41/inc/Conditionals_LCD.h
+++ 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
--- a/Marlin/src/HAL/TEENSY40_41/inc/Conditionals_adv.h
+++ 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
--- a/Marlin/src/HAL/TEENSY40_41/inc/Conditionals_post.h
+++ 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
--- a/Marlin/src/HAL/TEENSY40_41/inc/SanityCheck.h
+++ 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
--- a/Marlin/src/HAL/TEENSY40_41/pinsDebug.h
+++ b/Marlin/src/HAL/TEENSY40_41/pinsDebug.h
@ -0,0 +1,146 @@
+/**
+ * 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
+
+#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 */ }
--- a/Marlin/src/HAL/TEENSY40_41/spi_pins.h
+++ b/Marlin/src/HAL/TEENSY40_41/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 SCK_PIN   13
+#define MISO_PIN  12
+#define MOSI_PIN  11
+#define SS_PIN    20 // SDSS // A.28, A.29, B.21, C.26, C.29
--- a/Marlin/src/HAL/TEENSY40_41/timers.cpp
+++ 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/>.
+ *
+ */
+
+/**
+ * Teensy4.0/4.1 (__IMXRT1062__)
+ */
+
+#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__
--- a/Marlin/src/HAL/TEENSY40_41/timers.h
+++ b/Marlin/src/HAL/TEENSY40_41/timers.h
@ -0,0 +1,119 @@
+/**
+ * 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
+
+/**
+ * Description: HAL for
+ * Teensy4.0/4.1 (__IMXRT1062__)
+ */
+
+#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();
+extern "C" 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)
--- a/Marlin/src/HAL/TEENSY40_41/watchdog.cpp
+++ 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__
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(USE_WATCHDOG)
+
+#include "watchdog.h"
+
+// 4 seconds timeout
+#define WDTO 4 //seconds
+
+uint8_t timeoutval = (WDTO - 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__
--- a/Marlin/src/HAL/TEENSY40_41/watchdog.h
+++ 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
+
+/**
+ * Watchdog for Teensy4.0/4.1 (__IMXRT1062__)
+ */
+
+void watchdog_init();
+
+void HAL_watchdog_refresh();
--- a/Marlin/src/HAL/platforms.h
+++ b/Marlin/src/HAL/platforms.h
@ -31,6 +31,8 @@
  #define HAL_PATH(PATH, NAME) XSTR(PATH/TEENSY31_32/NAME)
 #elif defined(__MK64FX512__) || defined(__MK66FX1M0__)
  #define HAL_PATH(PATH, NAME) XSTR(PATH/TEENSY35_36/NAME)
+#elif defined(__IMXRT1062__)
+  #define HAL_PATH(PATH, NAME) XSTR(PATH/TEENSY40_41/NAME)
 #elif defined(TARGET_LPC1768)
  #define HAL_PATH(PATH, NAME) XSTR(PATH/LPC1768/NAME)
 #elif defined(__STM32F1__) || defined(TARGET_STM32F1)
--- a/Marlin/src/HAL/shared/backtrace/unwmemaccess.cpp
+++ b/Marlin/src/HAL/shared/backtrace/unwmemaccess.cpp
@ -129,6 +129,19 @@
  #define START_FLASH_ADDR  0x00000000
  #define END_FLASH_ADDR    0x00140000

+#elif defined(__IMXRT1062__)
+
+  // For IMXRT1062 in TEENSY 4.0/4/1
+  //  ITCM (rwx):  ORIGIN = 0x00000000, LENGTH = 512K
+  //  DTCM (rwx):  ORIGIN = 0x20000000, LENGTH = 512K
+  //  RAM (rwx):   ORIGIN = 0x20200000, LENGTH = 512K
+  //  FLASH (rwx): ORIGIN = 0x60000000, LENGTH = 1984K
+  //
+  #define START_SRAM_ADDR   0x00000000
+  #define END_SRAM_ADDR     0x20280000
+  #define START_FLASH_ADDR  0x60000000
+  #define END_FLASH_ADDR    0x601F0000
+
 #elif defined(__SAMD51P20A__)

  // For SAMD51x20, valid address ranges are
--- a/Marlin/src/HAL/shared/servo.h
+++ b/Marlin/src/HAL/shared/servo.h
@ -71,6 +71,8 @@
  #include "../TEENSY31_32/Servo.h"
 #elif IS_TEENSY35 || IS_TEENSY36
  #include "../TEENSY35_36/Servo.h"
+#elif IS_TEENSY40 || IS_TEENSY41
+  #include "../TEENSY40_41/Servo.h"
 #elif defined(TARGET_LPC1768)
  #include "../LPC1768/Servo.h"
 #elif defined(__STM32F1__) || defined(TARGET_STM32F1)
--- a/Marlin/src/core/boards.h
+++ b/Marlin/src/core/boards.h
@ -356,10 +356,13 @@

 #define BOARD_THE_BORG                5000  // THE-BORG (Power outputs: Hotend0, Hotend1, Bed, Fan)
 #define BOARD_REMRAM_V1               5001  // RemRam v1
+#define BOARD_TEENSY41                5002  // Teensy4.0 and Teensy4.1
+#define BOARD_T41U5XBB                5003  // T41U5XBB Teensy4.1 breakout board

 //
 // Espressif ESP32 WiFi
 //
+
 #define BOARD_ESPRESSIF_ESP32         6000  // Generic ESP32
 #define BOARD_MRR_ESPA                6001  // MRR ESPA board based on ESP32 (native pins only)
 #define BOARD_MRR_ESPE                6002  // MRR ESPE board based on ESP32 (with I2S stepper stream)
@ -368,11 +371,13 @@
 //
 // SAMD51 ARM Cortex M4
 //
+
 #define BOARD_AGCM4_RAMPS_144         6100  // RAMPS 1.4.4

 //
 // Custom board
 //
+
 #define BOARD_CUSTOM                  9998  // Custom pins definition for development and/or rare boards

 //
--- a/Marlin/src/libs/private_spi.h
+++ b/Marlin/src/libs/private_spi.h
@ -43,7 +43,7 @@ class SPIclass<MISO_PIN, MOSI_PIN, SCK_PIN> {
      SET_INPUT_PULLUP(MISO_PIN);
    }
    FORCE_INLINE static uint8_t receive() {
-      #if defined(__AVR__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__)
+      #if defined(__AVR__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) || defined(__IMXRT1062__)
        SPDR = 0;
        for (;!TEST(SPSR, SPIF););
        return SPDR;
--- a/Marlin/src/module/motion.h
+++ b/Marlin/src/module/motion.h
@ -108,22 +108,28 @@ extern int16_t feedrate_percentage;
  extern float e_move_accumulator;
 #endif

-inline float pgm_read_any(const float *p) { return pgm_read_float(p); }
-inline signed char pgm_read_any(const signed char *p) { return pgm_read_byte(p); }
+#ifdef __IMXRT1062__
+  #define DEFS_PROGMEM
+#else
+  #define DEFS_PROGMEM PROGMEM
+#endif
+
+inline float pgm_read_any(const float *p)   { return TERN(__IMXRT1062__, *p, pgm_read_float(p)); }
+inline int8_t pgm_read_any(const int8_t *p) { return TERN(__IMXRT1062__, *p, pgm_read_byte(p)); }

 #define XYZ_DEFS(T, NAME, OPT) \
  inline T NAME(const AxisEnum axis) { \
-    static const XYZval<T> NAME##_P PROGMEM = { X_##OPT, Y_##OPT, Z_##OPT }; \
+    static const XYZval<T> NAME##_P DEFS_PROGMEM = { X_##OPT, Y_##OPT, Z_##OPT }; \
    return pgm_read_any(&NAME##_P[axis]); \
  }
 XYZ_DEFS(float, base_min_pos,   MIN_POS);
 XYZ_DEFS(float, base_max_pos,   MAX_POS);
 XYZ_DEFS(float, base_home_pos,  HOME_POS);
 XYZ_DEFS(float, max_length,     MAX_LENGTH);
-XYZ_DEFS(signed char, home_dir, HOME_DIR);
+XYZ_DEFS(int8_t, home_dir, HOME_DIR);

 inline float home_bump_mm(const AxisEnum axis) {
-  static const xyz_pos_t home_bump_mm_P PROGMEM = HOMING_BUMP_MM;
+  static const xyz_pos_t home_bump_mm_P DEFS_PROGMEM = HOMING_BUMP_MM;
  return pgm_read_any(&home_bump_mm_P[axis]);
 }

--- a/Marlin/src/pins/pins.h
+++ b/Marlin/src/pins/pins.h
@ -619,6 +619,10 @@
  #include "stm32f7/pins_THE_BORG.h"            // STM32F7                                env:STM32F7
 #elif MB(REMRAM_V1)
  #include "stm32f7/pins_REMRAM_V1.h"           // STM32F7                                env:STM32F7
+#elif MB(TEENSY40_41)
+  #include "teensy4/pins_TEENSY41.h"            // Teensy-4.x                             env:teensy41
+#elif MB(T41U5XBB)
+  #include "teensy4/pins_T41U5XBB.h"            // Teensy-4.x                             env:teensy41

 //
 // Espressif ESP32
--- a/Marlin/src/pins/teensy4/pins_T41U5XBB.h
+++ b/Marlin/src/pins/teensy4/pins_T41U5XBB.h
@ -0,0 +1,119 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/****************************************************************************************
+* Teensy 4.1 (IMXRT1062) Breadboard pin assignments
+* Requires the Teensyduino software with Teensy 4.1 selected in Arduino IDE!
+* https://www.pjrc.com/teensy/teensyduino.html
+****************************************************************************************/
+
+#if !IS_32BIT_TEENSY || !IS_TEENSY41
+  #error "Oops! Select 'Teensy 4.1' in 'Tools > Board.'"
+#else
+  #define BOARD_INFO_NAME "Teensy4.1"
+#endif
+
+#define AT90USB          1286  // Disable MarlinSerial etc.
+#define USBCON         //1286  // Disable MarlinSerial etc.
+/*
+
+   plan for Teensy4.0 and Teensy4.1:
+                                                     USB
+                                          GND |-----#####-----| VIN (3.65 TO 5.5V)
+      RX1                 CS1     RX1  PWM  0 |     #####     | GND
+      TX1                 MISO1   TX1  PWM  1 |               | 3.3V
+      STPX                             PWM  2 |               | 23  A9 PWM              
+      DIRX                             PWM  3 |               | 22  A8 PWM              LIMZ
+      STPY                             PWM  4 |               | 21  A7      RX5         LIMY
+      DIRY                             PWM  5 |               | 20  A6      TX5         LIMX
+      STPZ                             PWM  6 |               | 19  A5 PWM        SCL0  COOL
+      DIRZ                        RX2  PWM  7 |               | 18  A4 PWM        SDA0  MIST
+      STPA                        TX2  PWM  8 |               | 17  A3      RX4   SDA1  CYST
+      DIRA                             PWM  9 |               | 16  A2      TX4   SCL1  EHOLD
+      STEN                             PWM 10 |               | 15  A1 PWM  RX3         PRB
+      SPDI                MOSI0        PWM 11 |               | 14  A0 PWM  TX3         PANIC
+      SPEN                MISO0        PWM 12 |               | 13 LED PWM  SCK0        SPWM
+                                         3.3V |               | GND
+      SCL                              PWM 24 |               | 41 A17                  KPSTR
+      SDA                              PWM 25 |               | 40 A16                  STENY
+      STPB                MOSI1            26 |               | 39 A15      MISO1       STENZ
+      DIRB                SCK1             27 | *  *  *  *  * | 38 A14                  STENA
+      LIMB                        RX7  PWM 28 |               | 37     PWM              STENB  
+      DOOR                        TX7  PWM 29 |               | 36     PWM              ST0
+      ST1                                  30 |               | 35          TX8         ST3
+      AUX0                                 31 |    SDCARD     | 34          RX8         ST2
+      AUX1                                 32 |_______________| 33     PWM              AUX2
+
+ 
+*/
+
+//
+// Limit Switches
+//
+#define X_STOP_PIN                            20
+#define Y_STOP_PIN                            21
+#define Z_STOP_PIN                            22
+
+//
+// Steppers
+//
+#define X_STEP_PIN                             2
+#define X_DIR_PIN                              3
+#define X_ENABLE_PIN                          10
+//#define X_CS_PIN                              30
+
+#define Y_STEP_PIN                             4
+#define Y_DIR_PIN                              5
+#define Y_ENABLE_PIN                          40
+//#define Y_CS_PIN                              31
+
+#define Z_STEP_PIN                             6
+#define Z_DIR_PIN                              7
+#define Z_ENABLE_PIN                          39
+//#define Z_CS_PIN                              32
+
+#define E0_STEP_PIN                            8
+#define E0_DIR_PIN                             9
+#define E0_ENABLE_PIN                         38
+
+#define E1_STEP_PIN                           26
+#define E1_DIR_PIN                            27
+#define E1_ENABLE_PIN                         37
+
+#define HEATER_0_PIN                          31
+#define HEATER_1_PIN                          32
+#define HEATER_BED_PIN                        33
+
+#define TEMP_0_PIN                             5  // Extruder / Analog pin numbering: 2 => A2
+#define TEMP_1_PIN                             4
+#define TEMP_BED_PIN                          15  // Bed / Analog pin numbering
+
+#define LED_PIN                               13
+
+#define SOL0_PIN                              17
+#define SERVO0_PIN                            24
+#define SERVO1_PIN                            25
+
+#ifndef SDCARD_CONNECTION
+  #define SDCARD_CONNECTION              ONBOARD
+#endif
--- a/Marlin/src/pins/teensy4/pins_TEENSY41.h
+++ b/Marlin/src/pins/teensy4/pins_TEENSY41.h
@ -0,0 +1,129 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/****************************************************************************************
+* Teensy 4.1 (IMXRT1062) Breadboard pin assignments
+* Requires the Teensyduino software with Teensy 4.1 selected in Arduino IDE!
+* https://www.pjrc.com/teensy/teensyduino.html
+****************************************************************************************/
+
+#if !IS_32BIT_TEENSY || !IS_TEENSY41
+  #error "Oops! Select 'Teensy 4.1' in 'Tools > Board.'"
+#else
+  #define BOARD_INFO_NAME "Teensy4.1"
+#endif
+
+#define AT90USB          1286  // Disable MarlinSerial etc.
+#define USBCON         //1286  // Disable MarlinSerial etc.
+/*
+
+   plan for Teensy4.0 and Teensy4.1:
+                                                     USB
+                                          GND |-----#####-----| VIN (3.65 TO 5.5V)
+      X_STEP_PIN          CS1     RX1  PWM  0 |     #####     | GND
+      X_DIR_PIN           MISO1   TX1  PWM  1 |               | 3.3V
+      Y_STEP_PIN                       PWM  2 |               | 23  A9 PWM              SERVO1_PIN
+      Y_DIR_PIN                        PWM  3 |               | 22  A8 PWM              SERVO0_PIN
+      Z_STEP_PIN                       PWM  4 |               | 21  A7      RX5        
+      Z_DIR_PIN                        PWM  5 |               | 20  A6      TX5         FILWIDTH_PIN
+      X_ENABLE_PIN                     PWM  6 |               | 19  A5 PWM        SCL0  
+      Y_ENABLE_PIN                RX2  PWM  7 |               | 18  A4 PWM        SDA0  HEATER_1_PIN
+      Z_ENABLE_PIN                TX2  PWM  8 |               | 17  A3      RX4   SDA1  
+      E0_STEP_PIN                      PWM  9 |               | 16  A2      TX4   SCL1  TEMP_0_PIN
+      E0_DIR_PIN                       PWM 10 |               | 15  A1 PWM  RX3         TEMP_BED_PIN
+      MOSI_PIN            MOSI0        PWM 11 |               | 14  A0 PWM  TX3         TEMP_1_PIN
+      MISO_PIN            MISO0        PWM 12 |               | 13 LED PWM  SCK0        SCK_PIN
+                                         3.3V |               | GND
+      Z_STOP_PIN                       PWM 24 |               | 41 A17                  
+      E0_ENABLE_PIN                    PWM 25 |               | 40 A16                  
+      FAN_PIN             MOSI1            26 |               | 39 A15      MISO1       X_STOP_PIN
+      Z-PROBE PWR         SCK1             27 | *  *  *  *  * | 38 A14                  Y_STOP_PIN
+      SOL1_PIN                    RX7  PWM 28 |               | 37     PWM              HEATER_0_PIN  
+      FAN_PIN                     TX7  PWM 29 |               | 36     PWM              HEATER_BED_PIN
+      X_CS_PIN                             30 |               | 35          TX8         E1_ENABLE_PIN
+      y_CS_PIN                             31 |    SDCARD     | 34          RX8         E1_DIR_PIN
+      Z_CS_PIN                             32 |_______________| 33     PWM              E1_STEP_PIN
+
+ 
+*/
+
+//
+// Limit Switches
+//
+#define X_STOP_PIN                            39
+#define Y_STOP_PIN                            38
+#define Z_STOP_PIN                            24
+
+//
+// Steppers
+//
+#define X_STEP_PIN                             0
+#define X_DIR_PIN                              1
+#define X_ENABLE_PIN                           6
+//#define X_CS_PIN                              30
+
+#define Y_STEP_PIN                             2
+#define Y_DIR_PIN                              3
+#define Y_ENABLE_PIN                           7
+//#define Y_CS_PIN                              31
+
+#define Z_STEP_PIN                             4
+#define Z_DIR_PIN                              5
+#define Z_ENABLE_PIN                           8
+//#define Z_CS_PIN                              32
+
+#define E0_STEP_PIN                            9
+#define E0_DIR_PIN                            10
+#define E0_ENABLE_PIN                         25
+
+#define E1_STEP_PIN                           33
+#define E1_DIR_PIN                            34
+#define E1_ENABLE_PIN                         35
+
+#define HEATER_0_PIN                          37
+#define HEATER_1_PIN                          18
+#define HEATER_BED_PIN                        36
+#ifndef FAN_PIN
+  #define FAN_PIN                             29
+#endif
+
+#define TEMP_0_PIN                             2  // Extruder / Analog pin numbering: 2 => A2
+#define TEMP_1_PIN                             0
+#define TEMP_BED_PIN                           1  // Bed / Analog pin numbering
+
+#define LED_PIN                               13
+//#define PS_ON_PIN                              1
+//#define ALARM_PIN                             -1
+
+//#define FILWIDTH_PIN                           6  // A6
+#define SOL0_PIN                              28
+#define SERVO0_PIN                            22
+#define SERVO1_PIN                            23
+
+//#define SCK_PIN                               13
+//#define MISO_PIN                              12
+//#define MOSI_PIN                              11
+
+#ifndef SDCARD_CONNECTION
+  #define SDCARD_CONNECTION              ONBOARD
+#endif
--- a/README.md
+++ b/README.md
@ -96,6 +96,13 @@ Marlin 2.0 introduces a layer of abstraction so that all the existing high-level
  [Teensy 3.5](https://www.pjrc.com/store/teensy35.html)|[MK64FX512VMD12](https://www.mouser.com/ProductDetail/NXP-Freescale/MK64FX512VMD12) ARM-Cortex M4|120MHz|512k|192k|3.3-5V|yes
  [Teensy 3.6](https://www.pjrc.com/store/teensy36.html)|[MK66FX1M0VMD18](https://www.mouser.com/ProductDetail/NXP-Freescale/MK66FX1M0VMD18) ARM-Cortex M4|180MHz|1M|256k|3.3V|yes

+  #### Teensy 4.0 / 4.1
+
+  boards|processor|speed|flash|sram|logic|fpu
+  ----|---------|-----|-----|----|-----|---
+  [Teensy 4.0](https://www.pjrc.com/store/teensy40.html)|[IMXRT1062](https://www.mouser.com/new/nxp-semiconductors/nxp-imx-rt1060-crossover-processor/) ARM-Cortex M7|600MHz|1M|2M|3.3V|yes
+  [Teensy 4.1](https://www.pjrc.com/store/teensy41.html)|[IMXRT1062](https://www.mouser.com/new/nxp-semiconductors/nxp-imx-rt1060-crossover-processor/) ARM-Cortex M7|600MHz|1M|2M|3.3V|yes
+
 ## Submitting Patches

 Proposed patches should be submitted as a Pull Request against the ([bugfix-2.0.x](https://github.com/MarlinFirmware/Marlin/tree/bugfix-2.0.x)) branch.
--- a/buildroot/share/git/mftest
+++ b/buildroot/share/git/mftest
@ -48,6 +48,8 @@ case $TESTENV in
     t32) TESTENV='teensy31' ;;
     t35) TESTENV='teensy35' ;;
     t36) TESTENV='teensy35' ;;
+     t40) TESTENV='teensy41' ;;
+     t41) TESTENV='teensy41' ;;

 -h|--help)  echo -e "$(basename $0) : Marlin Firmware test, build, and upload\n"
            echo "Usage: $(basename $0) ................. Select env and test to apply / run"
@ -56,7 +58,7 @@ case $TESTENV in
            echo "       $(basename $0) -b [variant] .... Auto-build the specified variant"
            echo "       $(basename $0) -u [variant] .... Auto-build and upload the specified variant"
            echo
-            echo "env shortcuts: tree due esp lin lpc|lpc8 lpc9 m128 m256|mega stm|f1 f4 f7 s6 teensy|t31|t32 t35|t36"
+            echo "env shortcuts: tree due esp lin lpc|lpc8 lpc9 m128 m256|mega stm|f1 f4 f7 s6 teensy|t31|t32 t35|t36 t40|t41"
            exit
            ;;

--- a/buildroot/tests/teensy41-tests
+++ b/buildroot/tests/teensy41-tests
@ -0,0 +1,123 @@
+#!/usr/bin/env bash
+#
+# Build tests for Teensy 4.0/4.1 (ARM Cortex-M7)
+#
+
+# exit on first failure
+set -e
+
+restore_configs
+opt_set MOTHERBOARD BOARD_TEENSY41
+exec_test $1 $2 "Teensy4.1 with default config"
+
+#
+# Test as many features together as possible
+#
+restore_configs
+opt_set MOTHERBOARD BOARD_TEENSY41
+opt_set EXTRUDERS 2
+opt_set TEMP_SENSOR_0 1
+opt_set TEMP_SENSOR_1 5
+opt_set TEMP_SENSOR_BED 1
+opt_enable REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER LCD_INFO_MENU SDSUPPORT SDCARD_SORT_ALPHA \
+           FILAMENT_WIDTH_SENSOR FILAMENT_LCD_DISPLAY CALIBRATION_GCODE BAUD_RATE_GCODE \
+           FIX_MOUNTED_PROBE Z_SAFE_HOMING AUTO_BED_LEVELING_BILINEAR Z_MIN_PROBE_REPEATABILITY_TEST DEBUG_LEVELING_FEATURE \
+           BABYSTEPPING BABYSTEP_XY BABYSTEP_ZPROBE_OFFSET BABYSTEP_ZPROBE_GFX_OVERLAY \
+           PRINTCOUNTER NOZZLE_PARK_FEATURE NOZZLE_CLEAN_FEATURE SLOW_PWM_HEATERS PIDTEMPBED EEPROM_SETTINGS INCH_MODE_SUPPORT TEMPERATURE_UNITS_SUPPORT M100_FREE_MEMORY_WATCHER \
+           ADVANCED_PAUSE_FEATURE ARC_SUPPORT BEZIER_CURVE_SUPPORT EXPERIMENTAL_I2CBUS EXTENDED_CAPABILITIES_REPORT AUTO_REPORT_TEMPERATURES PARK_HEAD_ON_PAUSE \
+           PHOTO_GCODE PHOTO_POSITION PHOTO_SWITCH_POSITION PHOTO_SWITCH_MS PHOTO_DELAY_MS PHOTO_RETRACT_MM \
+           HOST_ACTION_COMMANDS HOST_PROMPT_SUPPORT
+opt_set I2C_SLAVE_ADDRESS 63
+opt_set GRID_MAX_POINTS_X 16
+exec_test $1 $2 "Teensy4.1 with many features"
+
+#
+# Test a Sled Z Probe with Linear leveling
+#
+restore_configs
+opt_set MOTHERBOARD BOARD_TEENSY41
+opt_enable EEPROM_SETTINGS Z_PROBE_SLED Z_SAFE_HOMING AUTO_BED_LEVELING_LINEAR DEBUG_LEVELING_FEATURE GCODE_MACROS
+exec_test $1 $2 "Sled Z Probe with Linear leveling"
+
+#
+# Test a Servo Probe
+#
+# restore_configs
+# opt_set MOTHERBOARD BOARD_TEENSY41
+# opt_enable Z_PROBE_SERVO_NR Z_SERVO_ANGLES DEACTIVATE_SERVOS_AFTER_MOVE \
+#            AUTO_BED_LEVELING_3POINT DEBUG_LEVELING_FEATURE EEPROM_SETTINGS
+# opt_set NUM_SERVOS 1
+# exec_test $1 $2 "Servo Probe"
+#
+# ...with AUTO_BED_LEVELING_3POINT, DEBUG_LEVELING_FEATURE, EEPROM_SETTINGS, EEPROM_CHITCHAT, EXTENDED_CAPABILITIES_REPORT, and AUTO_REPORT_TEMPERATURES
+#
+# opt_enable AUTO_BED_LEVELING_3POINT DEBUG_LEVELING_FEATURE EEPROM_SETTINGS \
+#            EXTENDED_CAPABILITIES_REPORT AUTO_REPORT_TEMPERATURES
+# exec_test $1 $2 "...with AUTO_BED_LEVELING_3POINT, DEBUG_LEVELING_FEATURE, EEPROM_SETTINGS, EEPROM_CHITCHAT, EXTENDED_CAPABILITIES_REPORT, and AUTO_REPORT_TEMPERATURES"
+
+#
+# Test MAGNETIC_PARKING_EXTRUDER with LCD
+#
+restore_configs
+opt_set MOTHERBOARD BOARD_TEENSY41
+opt_set EXTRUDERS 2
+opt_set TEMP_SENSOR_1 1
+opt_enable MAGNETIC_PARKING_EXTRUDER ULTIMAKERCONTROLLER
+exec_test $1 $2 "MAGNETIC_PARKING_EXTRUDER with LCD"
+
+#
+# Mixing Extruder
+#
+restore_configs
+opt_set MOTHERBOARD BOARD_TEENSY41
+opt_enable MIXING_EXTRUDER DIRECT_MIXING_IN_G1 GRADIENT_MIX GRADIENT_VTOOL REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER
+opt_set MIXING_STEPPERS 2
+exec_test $1 $2 "Mixing Extruder"
+
+#
+# Test SWITCHING_EXTRUDER
+#
+# restore_configs
+# opt_set MOTHERBOARD BOARD_TEENSY41
+# opt_set EXTRUDERS 2
+# opt_set NUM_SERVOS 1
+# opt_enable SWITCHING_EXTRUDER ULTIMAKERCONTROLLER
+# exec_test $1 $2 "SWITCHING_EXTRUDER"
+
+#
+# Enable COREXY
+#
+restore_configs
+opt_set MOTHERBOARD BOARD_TEENSY41
+opt_enable COREXY
+opt_set X_DRIVER_TYPE TMC5160
+opt_set Y_DRIVER_TYPE TMC5160
+opt_set X_MIN_ENDSTOP_INVERTING true
+opt_set Y_MIN_ENDSTOP_INVERTING true
+opt_add X_CS_PIN 46
+opt_add Y_CS_PIN 47
+opt_enable USE_ZMIN_PLUG MONITOR_DRIVER_STATUS SENSORLESS_HOMING
+exec_test $1 $2 "Teensy 4.0/4.1 COREXY"
+
+#
+# Enable COREXZ
+#
+restore_configs
+opt_set MOTHERBOARD BOARD_TEENSY41
+opt_enable COREXZ
+exec_test $1 $2 "Teensy 4.0/4.1 COREXZ"
+
+#
+# Enable Dual Z with Dual Z endstops
+#
+restore_configs
+opt_set MOTHERBOARD BOARD_TEENSY41
+opt_enable Z_MULTI_ENDSTOPS
+opt_set NUM_Z_STEPPER_DRIVERS 2
+pins_set ramps/RAMPS X_MAX_PIN -1
+opt_add Z2_MAX_PIN 2
+opt_enable USE_XMAX_PLUG
+exec_test $1 $2 "Dual Z with Dual Z endstops"
+
+# Clean up
+restore_configs
--- a/platformio.ini
+++ b/platformio.ini
@ -1264,6 +1264,19 @@ platform      = teensy
 board         = teensy35
 src_filter    = ${common.default_src_filter} +<src/HAL/TEENSY35_36>

+[env:teensy36]
+platform      = teensy
+board         = teensy36
+src_filter    = ${common.default_src_filter} +<src/HAL/TEENSY35_36>
+
+#
+# Teensy 4.0 / 4.1 (ARM Cortex-M7)
+#
+[env:teensy41]
+platform      = teensy
+board         = teensy41
+src_filter    = ${common.default_src_filter} +<src/HAL/TEENSY40_41>
+
 #
 # Native
 # No supported Arduino libraries, base Marlin only