From 54e2e33ee1f5317d5b8b937ca2691796b829cf4b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 25 Jun 2023 19:50:09 +1000
Subject: [PATCH] HAL_ChibiOS: allow hwdef to specify a NRST_MODE override

this allows the boot behaviour of a board to be changes to avoid a
reset issue
---
 libraries/AP_HAL_ChibiOS/hwdef/common/board.c |  6 +++
 libraries/AP_HAL_ChibiOS/hwdef/common/flash.c | 44 ++++++++++++++++++-
 libraries/AP_HAL_ChibiOS/hwdef/common/flash.h |  1 +
 3 files changed, 50 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/board.c b/libraries/AP_HAL_ChibiOS/hwdef/common/board.c
index 61318595eb..99c48dfd6d 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/board.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/board.c
@@ -18,6 +18,7 @@
 #include "hal.h"
 #include "usbcfg.h"
 #include "stm32_util.h"
+#include "flash.h"
 #include "watchdog.h"
 
 
@@ -294,6 +295,11 @@ void __late_init(void) {
 #ifdef HAL_USB_PRODUCT_ID
   setup_usb_strings();
 #endif
+
+#ifdef HAL_FLASH_SET_NRST_MODE
+  // ensure NRST_MODE is set correctly
+  stm32_flash_set_NRST_MODE(HAL_FLASH_SET_NRST_MODE);
+#endif
 }
 
 #if HAL_USE_SDC || defined(__DOXYGEN__)
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c
index f655b3d6e1..21355bd069 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c
@@ -306,24 +306,35 @@ void stm32_flash_lock(void)
 #endif
 }
 
-#if defined(STM32H7) && HAL_FLASH_PROTECTION
+#if (defined(STM32H7) && HAL_FLASH_PROTECTION) || defined(HAL_FLASH_SET_NRST_MODE)
 static void stm32_flash_wait_opt_idle(void)
 {
     __DSB();
+#if defined(STM32H7)
     while (FLASH->OPTSR_CUR & FLASH_OPTSR_OPT_BUSY) {
         // nop
     }
+#else
+    while (FLASH->SR & FLASH_SR_BSY) {
+        // nop
+    }
+#endif
 }
 
 static void stm32_flash_opt_clear_errors(void)
 {
+#if defined(STM32H7)
     FLASH->OPTCCR = FLASH_OPTCCR_CLR_OPTCHANGEERR;
+#else
+    FLASH->SR |= FLASH_SR_OPERR;
+#endif
 }
 
 static bool stm32_flash_unlock_options(void)
 {
     stm32_flash_wait_opt_idle();
 
+#if defined(STM32H7)
     if (FLASH->OPTCR & FLASH_OPTCR_OPTLOCK) {
         /* Unlock sequence */
         FLASH->OPTKEYR = FLASH_OPTKEY1;
@@ -333,6 +344,11 @@ static bool stm32_flash_unlock_options(void)
     if (FLASH->OPTSR_CUR & FLASH_OPTSR_OPTCHANGEERR) {
         return false;
     }
+#else
+    FLASH->OPTKEYR = FLASH_OPTKEY1;
+    FLASH->OPTKEYR = FLASH_OPTKEY2;
+    stm32_flash_wait_opt_idle();
+#endif
     return true;
 }
 
@@ -340,11 +356,15 @@ static bool stm32_flash_lock_options(void)
 {
     stm32_flash_wait_opt_idle();
 
+#if defined(STM32H7)
     FLASH->OPTCR |= FLASH_OPTCR_OPTLOCK;
 
     if (FLASH->OPTSR_CUR & FLASH_OPTSR_OPTCHANGEERR) {
         return false;
     }
+#else
+    FLASH->CR |= FLASH_CR_OPTLOCK;
+#endif
     return true;
 }
 #endif
@@ -1038,6 +1058,28 @@ void stm32_flash_unprotect_flash()
 #endif
 }
 
+#if defined(HAL_FLASH_SET_NRST_MODE)
+/*
+  set NRST_MODE bits if not already set
+ */
+void stm32_flash_set_NRST_MODE(uint8_t nrst_mode)
+{
+    if ((FLASH->OPTR & FLASH_OPTR_NRST_MODE_Msk) == (((uint32_t)nrst_mode)<<FLASH_OPTR_NRST_MODE_Pos)) {
+        // already set correctly
+        return;
+    }
+    stm32_flash_unlock();
+    stm32_flash_opt_clear_errors();
+    if (stm32_flash_unlock_options()) {
+        FLASH->OPTR = (FLASH->OPTR & ~FLASH_OPTR_NRST_MODE_Msk) | (((uint32_t)nrst_mode)<<FLASH_OPTR_NRST_MODE_Pos);
+        FLASH->CR |= FLASH_CR_OPTSTRT;
+        stm32_flash_wait_opt_idle();
+        stm32_flash_lock_options();
+    }
+    stm32_flash_lock();
+}
+#endif // HAL_FLASH_SET_NRST_MODE
+
 #ifndef HAL_BOOTLOADER_BUILD
 /*
   return true if we had a recent erase
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.h b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.h
index a80fa7e6e1..7ec7c17031 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.h
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.h
@@ -29,6 +29,7 @@ void stm32_flash_keep_unlocked(bool set);
 bool stm32_flash_ispageerased(uint32_t page);
 void stm32_flash_protect_flash(bool bootloader, bool protect);
 void stm32_flash_unprotect_flash(void);
+void stm32_flash_set_NRST_MODE(uint8_t nrst_mode);
 #ifndef HAL_BOOTLOADER_BUILD
 bool stm32_flash_recent_erase(void);
 #endif