From dd1bd43a2e7c16227e243899239459eaad77ed56 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 25 Jun 2018 08:29:40 +1000
Subject: [PATCH] HAL_ChibiOS: added stm32_flash_keep_unlocked()

---
 libraries/AP_HAL_ChibiOS/hwdef/common/flash.c | 19 ++++++++++++++++++-
 libraries/AP_HAL_ChibiOS/hwdef/common/flash.h |  1 +
 2 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c
index acbbd1336b..777505b679 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c
@@ -114,7 +114,7 @@ static const uint32_t flash_memmap[STM32_FLASH_NPAGES] = { KB(32), KB(32), KB(32
 // keep a cache of the page addresses
 static uint32_t flash_pageaddr[STM32_FLASH_NPAGES];
 static bool flash_pageaddr_initialised;
-
+static bool flash_keep_unlocked;
 
 #define FLASH_KEY1      0x45670123
 #define FLASH_KEY2      0xCDEF89AB
@@ -144,6 +144,9 @@ static void stm32_flash_wait_idle(void)
 
 static void stm32_flash_unlock(void)
 {
+    if (flash_keep_unlocked) {
+        return;
+    }
     stm32_flash_wait_idle();
 
     if (FLASH->CR & FLASH_CR_LOCK) {
@@ -160,6 +163,9 @@ static void stm32_flash_unlock(void)
 
 void stm32_flash_lock(void)
 {
+    if (flash_keep_unlocked) {
+        return;
+    }
     stm32_flash_wait_idle();
     FLASH->CR |= FLASH_CR_LOCK;
 
@@ -352,5 +358,16 @@ failed:
     return -1;
 }
 
+void stm32_flash_keep_unlocked(bool set)
+{
+    if (set && !flash_keep_unlocked) {
+        stm32_flash_unlock();
+        flash_keep_unlocked = true;
+    } else if (!set && flash_keep_unlocked) {
+        flash_keep_unlocked = false;
+        stm32_flash_lock();        
+    }
+}
+
 #endif // HAL_NO_FLASH_SUPPORT
 
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.h b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.h
index 84dde1c935..d1085fa6fe 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.h
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.h
@@ -25,6 +25,7 @@ uint32_t stm32_flash_getpagesize(uint32_t page);
 uint32_t stm32_flash_getnumpages(void);
 bool stm32_flash_erasepage(uint32_t page);
 int32_t stm32_flash_write(uint32_t addr, const void *buf, uint32_t count);
+void stm32_flash_keep_unlocked(bool set);
 #ifdef __cplusplus
 }
 #endif