From 77e28ce0f0eec6b6c7d2a97e7513e799e2052e2a Mon Sep 17 00:00:00 2001
From: Dimitris Mantzouranis <d3xter93@gmail.com>
Date: Tue, 12 Oct 2021 12:35:12 +0300
Subject: [PATCH] [sn32] 240b: decouple Core clock and Flash clock update
 introduce a controller for Slow mode

---
 os/common/ext/SN/SN32F24xB/system_SN32F240B.c | 37 +++++++++++++++----
 os/common/ext/SN/SN32F24xB/system_SN32F240B.h | 19 ++++++++++
 os/hal/ports/SN32/SN32F240B/hal_lld.c         |  1 +
 3 files changed, 49 insertions(+), 8 deletions(-)

diff --git a/os/common/ext/SN/SN32F24xB/system_SN32F240B.c b/os/common/ext/SN/SN32F24xB/system_SN32F240B.c
index 049a10f3..7f4949dd 100644
--- a/os/common/ext/SN/SN32F24xB/system_SN32F240B.c
+++ b/os/common/ext/SN/SN32F24xB/system_SN32F240B.c
@@ -144,22 +144,44 @@ void SystemCoreClockUpdate (void)            /* Get Core Clock Frequency      */
 		case 7:	AHB_prescaler = 128;break;
 		default: break;
 	}
-	SystemCoreClock /= AHB_prescaler;
 
+	SystemCoreClock /= AHB_prescaler;
+}
+/**
+ * Initialize the Flash controller
+ *
+ * @param  none
+ * @return none
+ *
+ * @brief  Update the Flash power control.
+ */
+void FlashClockUpdate (void)
+{
 	//;;;;;;;;; Need for SN32F240B Begin
 	if (SystemCoreClock > 24000000)
-	{
 		SN_FLASH->LPCTRL = 0x5AFA0005;
-	}
 	else if (SystemCoreClock > 12000000)
 		SN_FLASH->LPCTRL = 0x5AFA0003;
-	else
+	else if (SystemCoreClock > 8000)
 		SN_FLASH->LPCTRL = 0x5AFA0000;
+	else  //Slow mode required for SystemCoreClock <= 8000
+		SlowModeSwitch();
 	//;;;;;;;;; Need for SN32F240B End
-
-	return;
 }
-
+/**
+ * Switch System to Slow Mode
+ * @param  none
+ * @return none
+ *
+ * @brief  Special init required for SystemCoreClock <= 8000
+ */
+void SlowModeSwitch (void)
+{
+	SN_SYS0->CLKCFG_b.SYSCLKSEL = 1; //Switch to ILRC
+	SN_SYS0->AHBCP_b.AHBPRE =0x2; //8kHz only for now
+	SystemCoreClockUpdate();
+	SN_FLASH->LPCTRL = 0x5AFA0002;
+}
 /**
  * Initialize the system
  *
@@ -175,7 +197,6 @@ void SystemInit (void)
 
 	#if SYS0_CLKCFG_VAL == IHRC48			//IHRC=48MHz
 
-        SN_FLASH->LPCTRL = 0x5AFA0004;
         SN_FLASH->LPCTRL = 0x5AFA0005;
 
         SN_SYS0->ANBCTRL = 0x1;
diff --git a/os/common/ext/SN/SN32F24xB/system_SN32F240B.h b/os/common/ext/SN/SN32F24xB/system_SN32F240B.h
index f95daf76..e8ec5c30 100644
--- a/os/common/ext/SN/SN32F24xB/system_SN32F240B.h
+++ b/os/common/ext/SN/SN32F24xB/system_SN32F240B.h
@@ -57,6 +57,25 @@ extern void SystemInit (void);
  */
 extern void SystemCoreClockUpdate (void);
 
+/**
+ * Initialize the Flash controller
+ *
+ * @param  none
+ * @return none
+ *
+ * @brief  Update the Flash power control.
+ */
+extern void FlashClockUpdate (void);
+
+/**
+ * Switch System to Slow Mode
+ * @param  none
+ * @return none
+ *
+ * @brief  Special init required for SystemCoreClock <= 8000
+ */
+extern void SlowModeSwitch (void);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/os/hal/ports/SN32/SN32F240B/hal_lld.c b/os/hal/ports/SN32/SN32F240B/hal_lld.c
index fd7be654..a6794a05 100644
--- a/os/hal/ports/SN32/SN32F240B/hal_lld.c
+++ b/os/hal/ports/SN32/SN32F240B/hal_lld.c
@@ -58,6 +58,7 @@
  */
 void sn32_clock_init(void) {
   SystemCoreClockUpdate();
+  FlashClockUpdate();
 }
 
 /**