diff --git a/radio/src/bootloader/boot.cpp b/radio/src/bootloader/boot.cpp
index c724dc674..ed6c92978 100644
--- a/radio/src/bootloader/boot.cpp
+++ b/radio/src/bootloader/boot.cpp
@@ -356,7 +356,7 @@ int menuFlashFile(uint32_t index, uint8_t event)
     fr = openFirmwareFile(index);
     fr = f_close(&FlashFile);
     Valid = 1;
-    if (isFirmwareStart(Block_buffer) == 0) {
+    if (!isFirmwareStart(Block_buffer)) {
       Valid = 2;
     }
   }
@@ -648,6 +648,13 @@ int main()
         // Commit to flashing
         uint32_t blockOffset = 0;
         lcd_putsLeft(4*FH, "\032Loading...");
+
+        if (firmwareAddress == FIRMWARE_ADDRESS + BOOTLOADER_SIZE) {
+          if (!isFirmwareStart(Block_buffer)) {
+            state = ST_FLASH_DONE;
+          }
+        }
+
         while (BlockCount) {
           writeFlash((uint32_t *)firmwareAddress, &Block_buffer[blockOffset]);
           blockOffset += FLASH_PAGESIZE/4; // 32-bit words
@@ -659,6 +666,7 @@ int main()
             BlockCount = 0;
           }
         }
+
         firmwareWritten += 4; // 4K blocks
 
         lcd_rect( 3, 6*FH+4, 204, 7);
diff --git a/radio/src/targets/sky9x/flash_driver.cpp b/radio/src/targets/sky9x/flash_driver.cpp
index 41cd808b3..5d3606395 100644
--- a/radio/src/targets/sky9x/flash_driver.cpp
+++ b/radio/src/targets/sky9x/flash_driver.cpp
@@ -7,18 +7,6 @@ void writeFlash(uint32_t *address, uint32_t *buffer)    // size is 256 bytes
   uint32_t FlashSectorNum;
   uint32_t flash_cmd = 0;
 
-  /* Initialize the function pointer (retrieve function address from NMI vector) */
-  if ((uint32_t) address == FIRMWARE_START+BOOTLOADER_SIZE) {
-    if (isFirmwareStart(buffer))
-      FlashBlocked = 0;
-    else
-      FlashBlocked = 1;
-  }
-
-  if (FlashBlocked) {
-    return;
-  }
-
   // Always initialise this here, setting a default doesn't seem to work
   IAP_Function = (uint32_t (*)(uint32_t, uint32_t)) *(( uint32_t *)0x00800008);
   FlashSectorNum = (uint32_t) address;
diff --git a/radio/src/targets/taranis/flash_driver.cpp b/radio/src/targets/taranis/flash_driver.cpp
index 2f74af46e..88e8c5687 100644
--- a/radio/src/targets/taranis/flash_driver.cpp
+++ b/radio/src/targets/taranis/flash_driver.cpp
@@ -43,47 +43,34 @@ void eraseSector(uint32_t sector)
   FLASH->CR &= SECTOR_MASK;
 }
 
-void writeFlash(uint32_t *address, uint32_t *buffer)    // size is 256 bytes
+void writeFlash(uint32_t *address, uint32_t *buffer) // page size is 256 bytes
 {
-  static uint8_t FlashBlocked = 1;
-
-  uint32_t i;
-
-  if ((uint32_t) address == 0x08008000) {
-    if (isFirmwareStart(buffer)) {
-      FlashBlocked = 0;
-    }
-    else {
-      FlashBlocked = 1;
-    }
+  if ((uint32_t) address == 0x08000000) {
+    eraseSector(0);
   }
-
-  if (FlashBlocked) {
-    return;
+  else if ((uint32_t) address == 0x08004000) {
+    eraseSector(1);
   }
-
-  if ((uint32_t) address == 0x08008000) {
+  else if ((uint32_t) address == 0x08008000) {
     eraseSector(2);
   }
-  if ((uint32_t) address == 0x0800C000) {
+  else if ((uint32_t) address == 0x0800C000) {
     eraseSector(3);
   }
-  if ((uint32_t) address == 0x08010000) {
+  else if ((uint32_t) address == 0x08010000) {
     eraseSector(4);
   }
-  if ((uint32_t) address == 0x08020000) {
+  else if ((uint32_t) address == 0x08020000) {
     eraseSector(5);
   }
-  if ((uint32_t) address == 0x08040000) {
+  else if ((uint32_t) address == 0x08040000) {
     eraseSector(6);
   }
-  if ((uint32_t) address == 0x08060000) {
+  else if ((uint32_t) address == 0x08060000) {
     eraseSector(7);
   }
 
-  // Now program the 256 bytes
-
-  for (i = 0; i < 64; i += 1) {
+  for (uint32_t i=0; i<FLASH_PAGESIZE/4; i++) {
     /* Device voltage range supposed to be [2.7V to 3.6V], the operation will
      be done by word */