diff --git a/src/stm32/Kconfig b/src/stm32/Kconfig
index 31b2613..95e9f43 100644
--- a/src/stm32/Kconfig
+++ b/src/stm32/Kconfig
@@ -356,12 +356,6 @@ config APPLICATION_START
     default 0x8002000 if STM32_APP_START_2000
     default 0x8008000
 
-config MAX_FLASH_PAGE_SIZE
-    hex
-    default 0x400 if MACH_STM32F042
-    default 0x800 if MACH_STM32F072 || MACH_STM32F103
-    default 0x400
-
 config BLOCK_SIZE
     int
     default 64
diff --git a/src/stm32/flash.c b/src/stm32/flash.c
index 09f82e9..42f972d 100644
--- a/src/stm32/flash.c
+++ b/src/stm32/flash.c
@@ -1,4 +1,4 @@
-// Flash (IAP) functionality for STM32F1
+// Flash (IAP) functionality for STM32
 //
 // Copyright (C) 2021 Eric Callahan <arksine.code@gmail.com
 //
@@ -10,25 +10,57 @@
 #include "flash.h" // flash_write_block
 #include "internal.h" // FLASH
 
-#define STM32F4_MIN_SECTOR_SIZE 16384
-#define STM32F4_MAX_SECTOR_SIZE 131072
 #if CONFIG_MACH_STM32F4
 #define FLASH_KEY1 (0x45670123UL)
 #define FLASH_KEY2 (0xCDEF89ABUL)
+
+// Return the flash sector index for the page at the given address
+static uint32_t
+stm32f4_sector_index(uint32_t addr)
+{
+    if (addr < 0x08010000)
+        return (addr - 0x08000000) / (16 * 1024);
+    else if (addr < 0x08020000)
+        return 4;
+    else
+        return 5 + (addr - 0x08020000) / (128 * 1024);
+}
 #endif
 
+// Return the flash page size at the given address
 static uint32_t
-flash_get_page_size(void)
+flash_get_page_size(uint32_t addr)
 {
-    if (CONFIG_MACH_STM32F103) {
+    if (CONFIG_MACH_STM32F4) {
+        if (addr < 0x08010000)
+            return 16 * 1024;
+        else if (addr < 0x08020000)
+            return 64 * 1024;
+        else
+            return 128 * 1024;
+    } else if (CONFIG_MACH_STM32F042) {
+        return 1024;
+    } else if (CONFIG_MACH_STM32F103) {
         // Check for a 1K page size on the stm32f103
         uint16_t *flash_size = (void*)FLASHSIZE_BASE;
         if (*flash_size < 256)
-            return 0x400;
+            return 1024;
     }
-    return CONFIG_MAX_FLASH_PAGE_SIZE;
+    return 2 * 1024;
 }
 
+// Check if the data at the given address has been erased (all 0xff)
+static int
+check_erased(uint32_t addr, uint32_t count)
+{
+    uint32_t *p = (void*)addr, *e = (void*)addr + count / 4;
+    while (p < e)
+        if (*p++ != 0xffffffff)
+            return 0;
+    return 1;
+}
+
+// Wait for flash hardware to report ready
 static void
 wait_flash(void)
 {
@@ -36,6 +68,7 @@ wait_flash(void)
         ;
 }
 
+// Issue low-level flash hardware unlock sequence
 static void
 unlock_flash(void)
 {
@@ -47,125 +80,107 @@ unlock_flash(void)
     wait_flash();
 }
 
+// Place low-level flash hardware into a locked state
 static void
 lock_flash(void)
 {
     FLASH->CR = FLASH_CR_LOCK;
 }
 
+// Issue a low-level flash hardware erase request for a flash page
 static void
-flash_write_stm32f4xx(uint32_t page_address, uint32_t *data)
+erase_page(uint32_t page_address)
 {
 #if CONFIG_MACH_STM32F4
-    uint32_t flash_page_size = flash_get_page_size();
-    uint32_t* page = (uint32_t*)(page_address);
-
-    uint8_t need_erase = 0;
-    uint32_t offset_addr = page_address - CONFIG_FLASH_START;
-    uint32_t sector_index = offset_addr / STM32F4_MAX_SECTOR_SIZE;
-    if (sector_index < 1) {
-        need_erase = ((page_address % STM32F4_MIN_SECTOR_SIZE) == 0);
-        sector_index = offset_addr / STM32F4_MIN_SECTOR_SIZE;
-        if (sector_index > 3) {
-            need_erase &= (sector_index == 4);
-            sector_index = 4;
-        }
-    } else {
-        need_erase = ((page_address % STM32F4_MAX_SECTOR_SIZE) == 0);
-        sector_index += 4;
-    }
-
-    // make sure flash is unlocked
-    unlock_flash();
-
-    // Erase page
-    if (need_erase) {
-        FLASH->CR = (FLASH_CR_PSIZE_1 | FLASH_CR_STRT | FLASH_CR_SER
-                     | ((sector_index & 0xF) << 3));
-        wait_flash();
-    }
-
-    // Write page
-    FLASH->CR = FLASH_CR_PSIZE_1 | FLASH_CR_PG;
-    for (int i = 0; i < flash_page_size / 4; i++) {
-        writel(&page[i], data[i]);
-        wait_flash();
-    }
-
-    lock_flash();
-#endif
-}
-
-static void
-flash_write_stm32f1xx(uint32_t page_address, uint16_t *data)
-{
-#if CONFIG_MACH_STM32F0 || CONFIG_MACH_STM32F1
-    uint32_t flash_page_size = flash_get_page_size();
-    uint16_t* page = (uint16_t*)(page_address);
-
-    // make sure flash is unlocked
-    unlock_flash();
-
-    // Erase page
+    FLASH->CR = (FLASH_CR_PSIZE_1 | FLASH_CR_STRT | FLASH_CR_SER
+                 | ((stm32f4_sector_index(page_address) & 0xF) << 3));
+#else
     FLASH->CR = FLASH_CR_PER;
     FLASH->AR = page_address;
     FLASH->CR = FLASH_CR_PER | FLASH_CR_STRT;
+#endif
     wait_flash();
+}
 
-    // Write page
-    FLASH->CR = FLASH_CR_PG;
-    for (int i = 0; i < flash_page_size / 2; i++) {
-        writew(&page[i], data[i]);
+// Write out a "block" of data to the low-level flash hardware
+static void
+write_block(uint32_t block_address, uint32_t *data)
+{
+#if CONFIG_MACH_STM32F4
+    uint32_t *page = (void*)block_address;
+    FLASH->CR = FLASH_CR_PSIZE_1 | FLASH_CR_PG;
+    for (int i = 0; i < CONFIG_BLOCK_SIZE / 4; i++) {
+        writel(&page[i], data[i]);
+        wait_flash();
+    }
+#else
+    uint16_t *page = (void*)block_address, *data16 = (void*)data;
+    FLASH->CR = FLASH_CR_PG;
+    for (int i = 0; i < CONFIG_BLOCK_SIZE / 2; i++) {
+        writew(&page[i], data16[i]);
         wait_flash();
     }
-
-    lock_flash();
 #endif
 }
 
-static void
-flash_write_page(uint32_t page_address, void *data)
-{
-    if (CONFIG_MACH_STM32F4) {
-        flash_write_stm32f4xx(page_address, (uint32_t *)data);
-    } else {
-        flash_write_stm32f1xx(page_address, (uint16_t *)data);
-    }
-}
-
-static uint8_t page_buffer[CONFIG_MAX_FLASH_PAGE_SIZE] __aligned(4);
-// Page Tracking
-static uint32_t last_page_address = 0;
-static uint8_t page_pending = 0;
-
-static void
-write_page(uint32_t page_address)
-{
-    flash_write_page(page_address, page_buffer);
-    memset(page_buffer, 0xFF, sizeof(page_buffer));
-    last_page_address = page_address;
-    page_pending = 0;
-}
+static uint32_t write_count;
 
+// Main block write interface
 int
 flash_write_block(uint32_t block_address, uint32_t *data)
 {
-    uint32_t flash_page_size = flash_get_page_size();
-    uint32_t page_pos = block_address % flash_page_size;
-    memcpy(&page_buffer[page_pos], (uint8_t *)&data[2], CONFIG_BLOCK_SIZE);
-    page_pending = 1;
-    if (page_pos + CONFIG_BLOCK_SIZE == flash_page_size)
-        write_page(block_address - page_pos);
+    if (block_address & (CONFIG_BLOCK_SIZE - 1))
+        // Not a block aligned address
+        return -1;
+    uint32_t flash_page_size = flash_get_page_size(block_address);
+    uint32_t page_address = ALIGN_DOWN(block_address, flash_page_size);
+
+    // Check if erase is needed
+    int need_erase = 0;
+    if (page_address == block_address) {
+        if (check_erased(block_address, flash_page_size)) {
+            // Page already erased
+        } else if (memcmp(data, (void*)block_address, CONFIG_BLOCK_SIZE) == 0
+                   && check_erased(block_address + CONFIG_BLOCK_SIZE
+                                   , flash_page_size - CONFIG_BLOCK_SIZE)) {
+            // Retransmitted request - just ignore
+            return 0;
+        } else {
+            need_erase = 1;
+        }
+    } else {
+        if (!check_erased(block_address, CONFIG_BLOCK_SIZE)) {
+            if (memcmp(data, (void*)block_address, CONFIG_BLOCK_SIZE) == 0)
+                // Retransmitted request - just ignore
+                return 0;
+            // Block not erased - out of order request?
+            return -2;
+        }
+    }
+
+    // make sure flash is unlocked
+    unlock_flash();
+
+    // Erase page
+    if (need_erase)
+        erase_page(page_address);
+
+    // Write block
+    write_block(block_address, data);
+
+    lock_flash();
+
+    if (memcmp(data, (void*)block_address, CONFIG_BLOCK_SIZE) != 0)
+        // Failed to write to flash?!
+        return -3;
+
+    write_count++;
     return 0;
 }
 
+// Main flash complete notification interface
 int
 flash_complete(void)
 {
-    uint32_t flash_page_size = flash_get_page_size();
-    if (page_pending) {
-        write_page(last_page_address + flash_page_size);
-    }
-    return ((last_page_address - CONFIG_APPLICATION_START)
-            / flash_page_size) + 1;
+    return write_count;
 }