From d4df6e0fa8828a0658ad7edbd3c84841f003d246 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Wed, 26 Jun 2024 15:01:05 +1000
Subject: [PATCH] AP_HAL_ChibiOS:hwdef/common: disable second core on dual core
 MCUs

---
 libraries/AP_HAL_ChibiOS/hwdef/common/board.c |  3 ++
 .../AP_HAL_ChibiOS/hwdef/common/stm32_util.c  | 28 +++++++++++++++++++
 .../AP_HAL_ChibiOS/hwdef/common/stm32_util.h  |  2 ++
 3 files changed, 33 insertions(+)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/board.c b/libraries/AP_HAL_ChibiOS/hwdef/common/board.c
index 99c48dfd6d..618fe597ac 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/board.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/board.c
@@ -269,6 +269,9 @@ void __early_init(void) {
                      STM32_NOCACHE_MPU_REGION_2_SIZE |
                      MPU_RASR_ENABLE);
 #endif
+#if defined(DUAL_CORE)
+  stm32_disable_cm4_core(); // disable second core
+#endif
 #endif
 }
 
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
index c91e68930f..755290b765 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
@@ -581,3 +581,31 @@ bool check_limit_flash_1M(void)
     return false;
 }
 #endif
+
+
+#if defined(DUAL_CORE)
+void stm32_disable_cm4_core() {
+    // Turn off second core for now
+    if ((FLASH->OPTSR_CUR & FLASH_OPTSR_BCM4)) {
+        //unlock flash
+        if (FLASH->OPTCR & FLASH_OPTCR_OPTLOCK) {
+            /* Unlock sequence */
+            FLASH->OPTKEYR = 0x08192A3B;
+            FLASH->OPTKEYR = 0x4C5D6E7F;
+        }
+        while (FLASH->OPTSR_CUR & FLASH_OPTSR_OPT_BUSY) {
+        }
+        // disable core boot
+        FLASH->OPTSR_PRG &= ~FLASH_OPTSR_BCM4;
+        // start programming
+        FLASH->OPTCR |= FLASH_OPTCR_OPTSTART;
+        // wait for completion by checking busy bit
+        while (FLASH->OPTSR_CUR & FLASH_OPTSR_OPT_BUSY) {
+        }
+        // lock flash
+        FLASH->OPTCR |= FLASH_OPTCR_OPTLOCK;
+        while (FLASH->OPTSR_CUR & FLASH_OPTSR_OPT_BUSY) {
+        }
+    }
+}
+#endif // DUAL_CORE
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
index 828ceed09c..7d78d41b82 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
@@ -191,6 +191,8 @@ extern stkalign_t __main_stack_end__;
 extern stkalign_t __main_thread_stack_base__;
 extern stkalign_t __main_thread_stack_end__;
 
+void stm32_disable_cm4_core(void);
+
 #ifdef __cplusplus
 }
 #endif