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 #include #include +#include #include #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; }