diff --git a/lib/usb_dfu.c b/lib/usb_dfu.c
index 1df306a..f65f5d2 100644
--- a/lib/usb_dfu.c
+++ b/lib/usb_dfu.c
@@ -15,7 +15,7 @@
 /** library for USB DFU to write on internal flash (code)
  *  @file
  *  @author King Kévin <kingkevin@cuvoodoo.info>
- *  @date 2017-2019
+ *  @date 2017-2020
  */
 
 /* standard libraries */
@@ -43,6 +43,7 @@ static enum dfu_status usb_dfu_status = DFU_STATUS_OK; /**< current DFU status *
 static uint8_t download_data[sizeof(usbd_control_buffer)] = {0}; /**< downloaded data to be programmed in flash */
 static uint16_t download_length = 0; /**< length of downloaded data */
 static uint32_t download_offset = 0; /**< destination offset of where the downloaded data should be flashed */
+static uint8_t firmware_head[8] = {0xff}; /**< to store the first bytes of the firmware, to be flashed at the end so we will stay in bootloader mode when the download is interrupted */
 
 /** USB DFU device descriptor
  *  @note as defined in USB Device Firmware Upgrade specification section 4.2.1
@@ -155,25 +156,6 @@ static void usb_disconnect(void)
 	}
 }
 
-/** flash downloaded data block
- *  @param[in] usbd_dev USB device (unused)
- *  @param[in] req USB request (unused)
- *  @note this function is called after the corresponding GETSTATUS request
- */
-static void usb_dfu_flash(usbd_device *usbd_dev, struct usb_setup_data *req)
-{
-	(void)usbd_dev; // variable not used
-	(void)req; // variable not used
-	led_off(); // indicate we are processing
-	if (flash_internal_write((uint32_t)&__application_beginning + download_offset, download_data, download_length, true) >= download_length) { // write downloaded data
-		usb_dfu_state = STATE_DFU_DNLOAD_IDLE; // go back to idle stat to wait for next segment
-	} else { // warn about writing error
-		usb_dfu_status = DFU_STATUS_ERR_WRITE;
-		usb_dfu_state = STATE_DFU_ERROR;
-	}
-	led_on(); // indicate we finished processing
-}
-
 /** disconnect USB and perform system reset
  *  @param[in] usbd_dev USB device (unused)
  *  @param[in] req USB request (unused)
@@ -188,6 +170,39 @@ static void usb_dfu_reset(usbd_device *usbd_dev, struct usb_setup_data *req)
 	while (true); // wait for the reset to happen
 }
 
+/** flash downloaded data block
+ *  @param[in] usbd_dev USB device (unused)
+ *  @param[in] req USB request (unused)
+ *  @note this function is called after the corresponding GETSTATUS request
+ */
+static void usb_dfu_flash(usbd_device *usbd_dev, struct usb_setup_data *req)
+{
+	(void)usbd_dev; // variable not used
+	(void)req; // variable not used
+	led_off(); // indicate we are processing
+	int32_t rc = flash_internal_write((uint32_t)&__application_beginning + download_offset, download_data, download_length, true); // write downloaded data
+	if (rc >= download_length) { // check if flashing worked
+		switch (usb_dfu_state) {
+		case STATE_DFU_DNLOAD_SYNC: // download ongoing
+		case STATE_DFU_DNBUSY: // flashing ongoing
+			usb_dfu_state = STATE_DFU_DNLOAD_IDLE; // go back to idle stat to wait for next segment
+			break;
+		case STATE_DFU_MANIFEST: // this was the last part
+			led_off(); // indicate the end
+			usb_dfu_reset(NULL, NULL); // start reset without waiting for request since we advertised we would detach
+			break;
+		default: // unknown state
+			usb_dfu_status = DFU_STATUS_ERR_PROG;
+			usb_dfu_state = STATE_DFU_ERROR;
+			break;
+		}
+	} else { // warn about writing error
+		usb_dfu_status = DFU_STATUS_ERR_WRITE;
+		usb_dfu_state = STATE_DFU_ERROR;
+	}
+	led_on(); // indicate we finished processing
+}
+
 /** handle incoming USB DFU control request
  *  @param[in] usbd_dev USB device descriptor
  *  @param[in] req control request information
@@ -244,6 +259,18 @@ static enum usbd_request_return_codes usb_dfu_control_request(usbd_device *usbd_
 					if (*len % 2) { // we can only write half words
 						download_data[*len++] = 0xff; // leave flash untouched
 					}
+					if (0 == download_offset) { // this is the beginning of the firmware
+						// we will keep the beginning of the firmware and flash is at the head
+						// this is because the head is used to check if the bootloader should boot into the application
+						// but if the download gets interrupted, the head is valid and the application will be started
+						// but the application is corrupted, and will not allow to go back to the bootloader
+						// by flashing the firmware head at the end, we ensure the application to boot is valid
+						// note: the force DFU input/button could be used to start the bootloader when the application is corrupted
+						for (uint16_t i = 0 ; i < LENGTH(firmware_head) && i < sizeof(download_data); i++) {
+							firmware_head[i] = download_data[i]; // backup head
+							download_data[i] = 0xff; // invalided head, but will value which is quick to flash
+						}
+					}
 					usb_dfu_state = STATE_DFU_DNLOAD_SYNC; // go to sync state
 					*complete = usb_dfu_flash; // start flashing the downloaded data
 				}
@@ -263,9 +290,14 @@ static enum usbd_request_return_codes usb_dfu_control_request(usbd_device *usbd_
 			if (STATE_DFU_DNLOAD_SYNC == usb_dfu_state) {
 				usb_dfu_state = STATE_DFU_DNBUSY; // switch to busy state
 			} else if (STATE_DFU_MANIFEST_SYNC == usb_dfu_state) {
+				// we still need to flash the saved head of the firmware
+				for (uint16_t i = 0 ; i < LENGTH(firmware_head) && i < sizeof(download_data); i++) {
+					download_data[i] = firmware_head[i]; // prepare data to be flashed
+				}
+				download_offset = 0; // flash the head of the firmware
+				download_length = sizeof(firmware_head); // only flash the head
 				usb_dfu_state = STATE_DFU_MANIFEST; // go to manifest mode
-				led_off(); // indicate the end
-				*complete = usb_dfu_reset; // start reset without waiting for request since we advertised we would detach
+				*complete = usb_dfu_flash; // flash the head
 			}
 			break;
 		case DFU_CLRSTATUS: // clear status