From dc6e924a95d89868a628ae13bddfe7683b33ddb3 Mon Sep 17 00:00:00 2001
From: Ricardo Maurizio Paul <ricardopaul@geotab.com>
Date: Mon, 5 Oct 2026 14:54:52 +0200
Subject: [PATCH] arch/arm/stm32h5: disable the ICACHE while program memory is
 modified.

An enabled ICACHE does not manage write transactions: it flags cacheable
writes as errors (ICACHE_SR.ERRF), and RM0481 8.4.5 recommends modifying
the memory with the ICACHE disabled.  On an STM32H563, erasing and
programming a flash block with the ICACHE enabled leaves ICACHE_SR at
0x6 (BSYENDF and ERRF set).

The ICACHE also keeps serving lines cached before the change.  When the
block had been read through the ICACHE just before it was programmed,
up_progmem_write() failed its read-back check with -EIO: the flash held
the new data, with no flash or ECC error flagged, but the read-back hit
the cached erased data.  up_progmem_eraseblock() fails its erased-range
check the same way when programmed data of the block is cached.

Disable the ICACHE for the duration of up_progmem_eraseblock() and
up_progmem_write(), and enable it again afterwards if it was enabled on
entry.  Disabling it invalidates it, so the refill after re-enabling it
sees the new flash content.

If the ICACHE cannot be re-enabled because its invalidate times out, it
is left disabled, which is safe but slower, and an error is logged.

Assisted-by: Claude:claude-sonnet-5-5
Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Ricardo Maurizio Paul <ricardopaul@geotab.com>
---
 arch/arm/src/stm32h5/stm32h563xx_flash.c | 61 ++++++++++++++++++++++++
 1 file changed, 61 insertions(+)

diff --git a/arch/arm/src/stm32h5/stm32h563xx_flash.c b/arch/arm/src/stm32h5/stm32h563xx_flash.c
index 4233a13a6d3..3ec1784e1cb 100644
--- a/arch/arm/src/stm32h5/stm32h563xx_flash.c
+++ b/arch/arm/src/stm32h5/stm32h563xx_flash.c
@@ -48,6 +48,7 @@
 #include <errno.h>
 #include <inttypes.h>
 #include <string.h>
+#include <syslog.h>
 #include <sys/param.h>
 
 #include "hardware/stm32_flash.h"
@@ -205,6 +206,58 @@ struct stm32h5_flash_priv_s * flash_bank(size_t address)
   return priv;
 }
 
+/****************************************************************************
+ * Name: flash_icache_suspend
+ *
+ * Description:
+ *   Disable the ICACHE before flash is modified (RM0481 8.4.5).  This also
+ *   invalidates it.  Must be called with g_lock held.
+ *
+ * Returned Value:
+ *   true if the ICACHE was enabled and must be restored with
+ *   flash_icache_resume(); false otherwise.
+ *
+ ****************************************************************************/
+
+static bool flash_icache_suspend(void)
+{
+#ifdef CONFIG_STM32_ICACHE
+  if (stm32_icache_enabled())
+    {
+      stm32_disable_icache();
+      return true;
+    }
+#endif
+
+  return false;
+}
+
+/****************************************************************************
+ * Name: flash_icache_resume
+ *
+ * Description:
+ *   Enable the ICACHE again if flash_icache_suspend() disabled it.  Must be
+ *   called with g_lock held.
+ *
+ * Input Parameters:
+ *   enabled - The value returned by flash_icache_suspend()
+ *
+ ****************************************************************************/
+
+static void flash_icache_resume(bool enabled)
+{
+#ifdef CONFIG_STM32_ICACHE
+  if (enabled && stm32_enable_icache() != OK)
+    {
+      /* The ICACHE stays off, which is safe but slower */
+
+      syslog(LOG_ERR, "ICACHE invalidate timed out, ICACHE left off\n");
+    }
+#else
+  UNUSED(enabled);
+#endif
+}
+
 /****************************************************************************
  * Name: flash_unlock_nscr
  *
@@ -1924,6 +1977,7 @@ ssize_t up_progmem_eraseblock(size_t block)
 {
   bool bank_swap;
   bool phy_bank1;
+  bool icache;
   int ret;
   size_t block_address = STM32_FLASH_BASE + (block * FLASH_BLOCK_SIZE);
 
@@ -1961,6 +2015,8 @@ ssize_t up_progmem_eraseblock(size_t block)
       return (ssize_t)ret;
     }
 
+  icache = flash_icache_suspend();
+
   if (flash_wait_for_operation())
     {
       ret = -EIO;
@@ -2002,6 +2058,7 @@ exit_with_unlock:
   flash_lock_nscr();
 
 exit_with_lock:
+  flash_icache_resume(icache);
   nxmutex_unlock(&g_lock);
 
   /* Verify */
@@ -2027,6 +2084,7 @@ ssize_t up_progmem_write(size_t addr, const void *buf, size_t count)
   uint32_t     *ll        = (uint32_t *)buf;
   size_t       faddr;
   size_t       written    = count;
+  bool         icache;
   int          ret;
   const size_t pagesize   = up_progmem_pagesize(0); /* 128bit, 16 bytes per page */
   const size_t llperpage  = pagesize / sizeof(uint32_t);
@@ -2058,6 +2116,8 @@ ssize_t up_progmem_write(size_t addr, const void *buf, size_t count)
   DEBUGASSERT(!(addr % pagesize));
   DEBUGASSERT(!(count % pagesize));
 
+  icache = flash_icache_suspend();
+
   if (flash_wait_for_operation())
     {
       written = -EIO;
@@ -2149,6 +2209,7 @@ exit_with_unlock:
     }
 
 exit_with_lock:
+  flash_icache_resume(icache);
   nxmutex_unlock(&g_lock);
   return written;
 }