From add830cdae46931346cc1ab5465f76a97cad408e Mon Sep 17 00:00:00 2001
From: Gerard Swiderski <gerard@swiderski.eu>
Date: Mon, 30 Oct 2023 11:01:54 +0100
Subject: [PATCH] WIP3 (to squash)

---
 hal/armv7m/imxrt/117x/imxrt.c | 48 ++++++++++++++++++++++++++---------
 1 file changed, 36 insertions(+), 12 deletions(-)

diff --git a/hal/armv7m/imxrt/117x/imxrt.c b/hal/armv7m/imxrt/117x/imxrt.c
index 11117a03..956ed4fa 100644
--- a/hal/armv7m/imxrt/117x/imxrt.c
+++ b/hal/armv7m/imxrt/117x/imxrt.c
@@ -543,15 +543,9 @@ static void _imxrt_pmuEnablePllLdo(void)
 }
 
 
-static int _imxrt_initArmPll(u8 loopDivider, u8 postDivider)
+static u32 _imxrt_deinitArmPll(void)
 {
-	u32 reg;
-
-	if ((loopDivider < 104u) || (208u < loopDivider)) {
-		return -1;
-	}
-
-	reg = *(imxrt_common.anadig_pll + arm_pll_ctrl) & ~(1uL << 29u);
+	u32 reg = *(imxrt_common.anadig_pll + arm_pll_ctrl) & ~(1uL << 29u);
 
 	/* Disable and gate clock if not already */
 	if ((reg & ((1uL << 13u) | (1uL << 14u))) != 0u) {
@@ -560,14 +554,36 @@ static int _imxrt_initArmPll(u8 loopDivider, u8 postDivider)
 		/* Gate the clock */
 		reg |= 1uL << 30u;
 		*(imxrt_common.anadig_pll + arm_pll_ctrl) = reg;
+
+		hal_cpuDataSyncBarrier();
+		hal_cpuInstrBarrier();
+	}
+
+	return reg;
+}
+
+
+static int _imxrt_initArmPll(u8 loopDivider, u8 postDivider)
+{
+	u32 reg;
+
+	/*
+	 * Fin = XTALOSC = 24MHz
+	 * Fout = Fin * (loopDivider / (2 * postDivider))
+	 */
+
+	if ((loopDivider < 104u) || (208u < loopDivider)) {
+		return -1;
 	}
 
+	reg = _imxrt_deinitArmPll();
+
 	/* Set the configuration. */
 	reg &= ~((3uL << 15u) | 0xffu);
-	reg |= ((loopDivider & 0xffu) | ((postDivider & 3uL) << 15u)) | (1uL << 30u) | (1uL << 13u);
+	reg |= ((u32)(loopDivider & 0xffu) | (((u32)postDivider & 3uL) << 15u)) | (1uL << 30u) | (1uL << 13u);
 	*(imxrt_common.anadig_pll + arm_pll_ctrl) = reg;
 
-	hal_cpuDataMemoryBarrier();
+	hal_cpuDataSyncBarrier();
 	hal_cpuInstrBarrier();
 
 	_imxrt_waitus(30);
@@ -821,7 +837,7 @@ static void _imxrt_initClocks(void)
 	}
 
 	/* Make sure main clocks are not using ARM PLL, PLL1, PLL2, PLL3 */
-	_imxrt_setRootClock(clkroot_m7, mux_clkroot_m7_oscrc400m, 1, 0);
+	_imxrt_setRootClock(clkroot_m7, mux_clkroot_m7_oscrc48mdiv2, 1, 0);
 	_imxrt_setRootClock(clkroot_m7_systick, mux_clkroot_m7_oscrc48mdiv2, 240, 0);
 	_imxrt_setRootClock(clkroot_bus_lpsr, mux_clkroot_m7_oscrc48mdiv2, 1, 0);
 	/* _imxrt_setRootClock(clkroot_m4, mux_clkroot_m7_oscrc48mdiv2, 1, 0); */
@@ -829,7 +845,9 @@ static void _imxrt_initClocks(void)
 	/* Power up ARM PLL, PLL3 slices */
 	_imxrt_pmuEnablePllLdo();
 
-#ifndef PLATFORM_INDUSTRIAL
+	/* FIXME: Improve platform CPU speed selector */
+
+#ifdef PLATFORM_CONSUMER_MARKET
 	/* commercial-qualified devices up to 1GHz */
 
 	/* Initialize ARM PLL to 996 MHz */
@@ -838,9 +856,15 @@ static void _imxrt_initClocks(void)
 #else
 	/* industrial-qualified devices up to 800MHz */
 
+#if 1
 	/* Initialize ARM PLL to 798 MHz */
 	_imxrt_initArmPll(133, 0);
 	imxrt_common.cpuclk = 798000000u;
+#else
+	/* Initialize ARM PLL to 696 MHz */
+	_imxrt_initArmPll(116, 0);
+	imxrt_common.cpuclk = 696000000u;
+#endif
 #endif
 
 	_imxrt_setPllBypass(clk_pllsys1, 1);