From b5586f413e10ce8389ffd115a1ee395c80f9d6c6 Mon Sep 17 00:00:00 2001
From: jcj83429 <jcj83429@gmail.com>
Date: Tue, 22 Dec 2020 18:17:19 -0800
Subject: [PATCH] setI2SFreq support for T4.x

---
 output_i2s_f32.cpp | 27 +++++++++++++++++++++++++++
 1 file changed, 27 insertions(+)

diff --git a/output_i2s_f32.cpp b/output_i2s_f32.cpp
index 9ed2e40..2a55de3 100644
--- a/output_i2s_f32.cpp
+++ b/output_i2s_f32.cpp
@@ -75,6 +75,33 @@ float setI2SFreq(const float freq_Hz) {
     }
   }
   return 0.0f;
+  
+#elif defined(__IMXRT1062__)
+
+  // PLL between 27*24 = 648MHz und 54*24=1296MHz
+  int n1 = 4; //SAI prescaler 4 => (n1*n2) = multiple of 4
+  int n2 = 1 + (24000000 * 27) / (freq_Hz * 256 * n1);
+
+  double C = ((double)freq_Hz * 256 * n1 * n2) / 24000000;
+  int c0 = C;
+  int c2 = 10000;
+  int c1 = C * c2 - (c0 * c2);
+  /*
+  Serial.print("n2=");
+  Serial.print(n2);
+  Serial.print(" C=");
+  Serial.println(C, 4);
+  */
+
+  // PLL
+  set_audioClock(c0, c1, c2, true);
+  // divider after PLL
+  CCM_CS1CDR = (CCM_CS1CDR & ~(CCM_CS1CDR_SAI1_CLK_PRED_MASK | CCM_CS1CDR_SAI1_CLK_PODF_MASK))
+       | CCM_CS1CDR_SAI1_CLK_PRED(n1-1) // &0x07
+       | CCM_CS1CDR_SAI1_CLK_PODF(n2-1); // &0x3f
+
+  return freq_Hz;
+
 #endif
 }