From 90801f8815bb0512836e3191af6e614f18db2522 Mon Sep 17 00:00:00 2001
From: bilsef <bilsef1@gmail.com>
Date: Fri, 2 Oct 2020 15:08:50 -0700
Subject: [PATCH] Fix onboard SD card support for Teensy 3.6 & 4.1 (#19593)

---
 Marlin/src/sd/Sd2Card.cpp | 15 +++++++++++++++
 Marlin/src/sd/Sd2Card.h   |  5 +++++
 2 files changed, 20 insertions(+)

diff --git a/Marlin/src/sd/Sd2Card.cpp b/Marlin/src/sd/Sd2Card.cpp
index 6ff2793e9..491c0692c 100644
--- a/Marlin/src/sd/Sd2Card.cpp
+++ b/Marlin/src/sd/Sd2Card.cpp
@@ -231,6 +231,13 @@ bool Sd2Card::eraseSingleBlockEnable() {
  * The reason for failure can be determined by calling errorCode() and errorData().
  */
 bool Sd2Card::init(const uint8_t sckRateID, const pin_t chipSelectPin) {
+  #if IS_TEENSY_35_36 || IS_TEENSY_40_41
+    chipSelectPin_ = BUILTIN_SDCARD;
+    const uint8_t ret = SDHC_CardInit();
+    type_ = SDHC_CardGetType();
+    return (ret == 0);
+  #endif
+
   errorCode_ = type_ = 0;
   chipSelectPin_ = chipSelectPin;
   // 16-bit init start time allows over a minute
@@ -332,6 +339,10 @@ bool Sd2Card::init(const uint8_t sckRateID, const pin_t chipSelectPin) {
  * \return true for success, false for failure.
  */
 bool Sd2Card::readBlock(uint32_t blockNumber, uint8_t* dst) {
+  #if IS_TEENSY_35_36 || IS_TEENSY_40_41
+    return 0 == SDHC_CardReadBlock(dst, blockNumber);
+  #endif
+
   if (type() != SD_CARD_TYPE_SDHC) blockNumber <<= 9;   // Use address if not SDHC card
 
   #if ENABLED(SD_CHECK_AND_RETRY)
@@ -547,6 +558,10 @@ bool Sd2Card::waitNotBusy(const millis_t timeout_ms) {
 bool Sd2Card::writeBlock(uint32_t blockNumber, const uint8_t* src) {
   if (ENABLED(SDCARD_READONLY)) return false;
 
+  #if IS_TEENSY_35_36 || IS_TEENSY_40_41
+    return 0 == SDHC_CardWriteBlock(src, blockNumber);
+  #endif
+
   bool success = false;
   if (type() != SD_CARD_TYPE_SDHC) blockNumber <<= 9;   // Use address if not SDHC card
   if (!cardCommand(CMD24, blockNumber)) {
diff --git a/Marlin/src/sd/Sd2Card.h b/Marlin/src/sd/Sd2Card.h
index 5a7ed375e..6900502e0 100644
--- a/Marlin/src/sd/Sd2Card.h
+++ b/Marlin/src/sd/Sd2Card.h
@@ -84,6 +84,11 @@ uint8_t const SD_CARD_TYPE_SD1  = 1,                    // Standard capacity V1
   #define SOFTWARE_SPI
 #endif
 
+#if IS_TEENSY_35_36 || IS_TEENSY_40_41
+  #include "NXP_SDHC.h"
+  #define BUILTIN_SDCARD 254
+#endif
+
 /**
  * \class Sd2Card
  * \brief Raw access to SD and SDHC flash memory cards.