diff --git a/src/flashcmd.c b/src/flashcmd.c
index 3af9a52..11e85e0 100644
--- a/src/flashcmd.c
+++ b/src/flashcmd.c
@@ -72,7 +72,7 @@ flashcmd_is_in_transfer(void)
 static void
 write_page(uint32_t page_address)
 {
-    flash_write_page(page_address, (uint16_t*)page_buffer);
+    flash_write_page(page_address, page_buffer);
     memset(page_buffer, 0xFF, sizeof(page_buffer));
     last_page_address = page_address;
     page_pending = 0;
diff --git a/src/stm32/flash.c b/src/stm32/flash.c
index 4896786..12bd269 100644
--- a/src/stm32/flash.c
+++ b/src/stm32/flash.c
@@ -9,6 +9,13 @@
 #include "flash.h" // flash_write_page
 #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)
+#endif
+
 uint32_t
 flash_get_page_size(void)
 {
@@ -38,11 +45,64 @@ flash_complete(void)
 }
 
 void
-flash_write_page(uint32_t page_address, uint16_t *data)
+flash_write_stm32f4xx(uint32_t page_address, uint32_t *data)
 {
-    // A page_index of 0 is the first page of the application area
+#if CONFIG_MACH_STM32F4
     uint32_t flash_page_size = flash_get_page_size();
-    uint16_t* page_addr = (uint16_t*)(page_address);
+    uint32_t* page = (uint32_t*)(page_address);
+
+    if (FLASH->CR & FLASH_CR_LOCK)
+        unlock_flash();
+
+    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;
+    }
+    while (FLASH->SR & FLASH_SR_BSY);
+    FLASH->CR &= ~FLASH_CR_PSIZE;
+    FLASH->CR &= ~FLASH_CR_SNB;
+    if (need_erase) {
+        FLASH->CR |= FLASH_CR_PSIZE_1;
+        FLASH->CR |= FLASH_CR_SER | ((sector_index & 0xF) << 3);
+        FLASH->CR |= FLASH_CR_STRT;
+        while (FLASH->SR & FLASH_SR_BSY);
+        FLASH->CR &= ~FLASH_CR_SNB;
+        do {
+            FLASH->CR &= ~FLASH_CR_SER;
+        } while(FLASH->CR & FLASH_CR_SER);
+    }
+
+    FLASH->CR |= FLASH_CR_PSIZE_1;
+    FLASH->CR |= FLASH_CR_PG;
+    for (uint16_t i = 0; i < flash_page_size / 4; i++)
+    {
+        page[i] = data[i];
+        while (FLASH->SR & FLASH_SR_BSY);
+    }
+    do {
+        FLASH->CR &= ~FLASH_CR_PG;
+    } while(FLASH->CR & FLASH_CR_PG);
+
+    FLASH->CR |= FLASH_CR_LOCK;
+#endif
+}
+
+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
     if (FLASH->CR & FLASH_CR_LOCK)
@@ -50,7 +110,7 @@ flash_write_page(uint32_t page_address, uint16_t *data)
 
     // Erase page
     FLASH->CR |= FLASH_CR_PER;
-    FLASH->AR = (uint32_t)page_addr;
+    FLASH->AR = (uint32_t)page;
     FLASH->CR |= FLASH_CR_STRT;
     while (FLASH->SR & FLASH_SR_BSY);
     do {
@@ -61,13 +121,25 @@ flash_write_page(uint32_t page_address, uint16_t *data)
     FLASH->CR |= FLASH_CR_PG;
     for (uint16_t i = 0; i < flash_page_size / 2; i++)
     {
-        page_addr[i] = data[i];
+        page[i] = data[i];
         while (FLASH->SR & FLASH_SR_BSY);
     }
     do {
         FLASH->CR &= ~FLASH_CR_PG;
     } while (FLASH->CR & FLASH_CR_PG);
+
     FLASH->CR |= FLASH_CR_LOCK;
+#endif
+}
+
+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);
+    }
 }
 
 void
diff --git a/src/stm32/flash.h b/src/stm32/flash.h
index 1636709..d2b3882 100644
--- a/src/stm32/flash.h
+++ b/src/stm32/flash.h
@@ -5,7 +5,7 @@
 
 uint32_t flash_get_page_size(void);
 void flash_complete(void);
-void flash_write_page(uint32_t page_address, uint16_t *data);
+void flash_write_page(uint32_t page_address, void *data);
 void flash_read_block(uint32_t block_address, uint32_t *buffer);
 
 #endif