stm32: Fix H7 unaligned erase

1 files changed, 6 insertions, 11 deletions

diff --git a/embassy-stm32/src/flash/h7.rs b/embassy-stm32/src/flash/h7.rs
index 3f2129de8..3178b3be9 100644
--- a/embassy-stm32/src/flash/h7.rs
+++ b/embassy-stm32/src/flash/h7.rs
@@ -79,24 +79,19 @@ pub(crate) unsafe fn blocking_erase(from: u32, to: u32) -> Result<(), Error> {
     let from = from - super::FLASH_BASE as u32;
     let to = to - super::FLASH_BASE as u32;
 
-    let bank_size = (super::FLASH_SIZE / 2) as u32;
-
-    let (bank, start, end) = if to <= bank_size {
+    let (start, end) = if to <= super::FLASH_SIZE as u32 {
         let start_sector = from / super::ERASE_SIZE as u32;
         let end_sector = to / super::ERASE_SIZE as u32;
-        (0, start_sector, end_sector)
-    } else if from >= SECOND_BANK_OFFSET as u32 && to <= (SECOND_BANK_OFFSET as u32 + bank_size) {
-        let start_sector = (from - SECOND_BANK_OFFSET as u32) / super::ERASE_SIZE as u32;
-        let end_sector = (to - SECOND_BANK_OFFSET as u32) / super::ERASE_SIZE as u32;
-        (1, start_sector, end_sector)
+        (start_sector, end_sector)
     } else {
-        error!("Attempting to write outside of defined sectors");
+        error!("Attempting to write outside of defined sectors {:x} {:x}", from, to);
         return Err(Error::Unaligned);
     };
 
-    trace!("Erasing bank {}, sectors from {} to {}", bank, start, end);
+    trace!("Erasing sectors from {} to {}", start, end);
     for sector in start..end {
-        let ret = erase_sector(pac::FLASH.bank(bank), sector as u8);
+        let bank = if sector >= 8 { 1 } else { 0 };
+        let ret = erase_sector(pac::FLASH.bank(bank), (sector % 8) as u8);
         if ret.is_err() {
             return ret;
         }