From 2d1fd0e2dde83a5cab1f4108195e50b445e7b165 Mon Sep 17 00:00:00 2001
From: Mike S <CyberMerln@gmail.com>
Date: Sat, 24 Aug 2024 09:35:37 -0400
Subject: [PATCH] added set_psram_clock

---
 teensy4/clockspeed.c | 34 ++++++++++++++++++++++++++++++++++
 1 file changed, 34 insertions(+)

diff --git a/teensy4/clockspeed.c b/teensy4/clockspeed.c
index be2e1acb3..c3baa7797 100644
--- a/teensy4/clockspeed.c
+++ b/teensy4/clockspeed.c
@@ -176,3 +176,37 @@ uint32_t set_arm_clock(uint32_t frequency)
 	return frequency;
 }
 
+#ifdef ARDUINO_TEENSY41
+void set_psram_clock(int speed_mhz) {
+    // What clocks exist:
+    static const int flexspio2_clock_speeds[] = { 396, 720, 665, 528 };
+
+    // See what the closest setting might be:
+    uint8_t clk_save = 0;
+    uint8_t divider_save =0;
+    int min_delta = speed_mhz;
+    for (uint8_t clk = 0; clk < 4; clk++) {
+        uint8_t divider = (flexspio2_clock_speeds[clk] + (speed_mhz / 2)) / speed_mhz;
+        int delta = abs(speed_mhz - flexspio2_clock_speeds[clk] / divider);
+        if ((delta < min_delta) && (divider < 8)) {
+            min_delta = delta;
+            clk_save = clk;
+            divider_save = divider;
+        }
+    }
+
+    // first turn off FLEXSPI2
+    CCM_CCGR7 &= ~CCM_CCGR7_FLEXSPI2(CCM_CCGR_ON);
+
+    divider_save--; // 0 biased.
+    //Serial.printf("Update FLEXSPI2 speed: %u clk:%u div:%u Actual:%u\n", speed_mhz, clk_save, divider_save,
+    //    flexspio2_clock_speeds[clk_save]/ (divider_save + 1));
+
+    // Set the clock settings.
+    CCM_CBCMR = (CCM_CBCMR & ~(CCM_CBCMR_FLEXSPI2_PODF_MASK | CCM_CBCMR_FLEXSPI2_CLK_SEL_MASK))
+                | CCM_CBCMR_FLEXSPI2_PODF(divider_save) | CCM_CBCMR_FLEXSPI2_CLK_SEL(clk_save);
+
+    // Turn FlexSPI2 clock back on
+    CCM_CCGR7 |= CCM_CCGR7_FLEXSPI2(CCM_CCGR_ON);
+}
+#endif
\ No newline at end of file