stm32u5: Add flash bank selection when erasing a sector

1 files changed, 13 insertions, 3 deletions

diff --git a/embassy-stm32/src/flash/u5.rs b/embassy-stm32/src/flash/u5.rs
index 0601017ce..e5af4f1f7 100644
--- a/embassy-stm32/src/flash/u5.rs
+++ b/embassy-stm32/src/flash/u5.rs
@@ -1,7 +1,7 @@
 use core::ptr::write_volatile;
 use core::sync::atomic::{fence, Ordering};
 
-use super::{FlashRegion, FlashSector, FLASH_REGIONS, WRITE_SIZE};
+use super::{FlashBank, FlashRegion, FlashSector, FLASH_REGIONS, WRITE_SIZE};
 use crate::flash::Error;
 use crate::pac;
 
@@ -70,12 +70,22 @@ pub(crate) unsafe fn blocking_erase_sector(sector: &FlashSector) -> Result<(), E
     #[cfg(feature = "trustzone-secure")]
     pac::FLASH.seccr().modify(|w| {
         w.set_per(pac::flash::vals::SeccrPer::B_0X1);
-        w.set_pnb(sector.index_in_bank)
+        w.set_pnb(sector.index_in_bank);
+        // TODO: add check for bank swap
+        w.set_bker(match sector.bank {
+            FlashBank::Bank1 => pac::flash::vals::SeccrBker::B_0X0,
+            FlashBank::Bank2 => pac::flash::vals::SeccrBker::B_0X1,
+        });
     });
     #[cfg(not(feature = "trustzone-secure"))]
     pac::FLASH.nscr().modify(|w| {
         w.set_per(pac::flash::vals::NscrPer::B_0X1);
-        w.set_pnb(sector.index_in_bank)
+        w.set_pnb(sector.index_in_bank);
+        // TODO: add check for bank swap
+        w.set_bker(match sector.bank {
+            FlashBank::Bank1 => pac::flash::vals::NscrBker::B_0X0,
+            FlashBank::Bank2 => pac::flash::vals::NscrBker::B_0X1,
+        });
     });
 
     #[cfg(feature = "trustzone-secure")]