Bootloader: Verification

diff --git a/lib/dfu-bootloader.c b/lib/dfu-bootloader.c
index f40676c925c52d23f98549aa6cb5a41c19bf43a8..1ef4806b84c6e911601065dd52b8388a5e5e98e6 100644 (file)
--- a/lib/dfu-bootloader.c
+++ b/lib/dfu-bootloader.c
@@ -36,7 +36,9 @@
 #define HDR_LENGTH 0x1c
 #define HDR_FLASH_IN_PROGRESS 0x20
 
-byte usbd_control_buffer[1024];
+// DFU blocks should be equal to erase blocks of the flash
+#define BLOCK_SIZE 1024
+byte usbd_control_buffer[BLOCK_SIZE];
 
 static enum dfu_state dfu_state = STATE_DFU_IDLE;
 
@@ -82,7 +84,7 @@ const struct usb_dfu_descriptor dfu_function = {
        .bDescriptorType = DFU_FUNCTIONAL,
        .bmAttributes = USB_DFU_CAN_DOWNLOAD | USB_DFU_WILL_DETACH,
        .wDetachTimeout = 255,
-       .wTransferSize = 1024,
+       .wTransferSize = BLOCK_SIZE,
        .bcdDFUVersion = 0x0100,
 };
 
@@ -127,6 +129,8 @@ static byte dfu_getstatus(usbd_device *usbd_dev UNUSED, u32 *bwPollTimeout)
                /* Device will reset when read is complete. */
                dfu_state = STATE_DFU_MANIFEST;
                return DFU_STATUS_OK;
+       case STATE_DFU_ERROR:
+               return DFU_STATUS_ERR_VERIFY;
        default:
                return DFU_STATUS_OK;
        }
@@ -140,15 +144,21 @@ static void dfu_getstatus_complete(usbd_device *usbd_dev UNUSED, struct usb_setu
                        // The "flash in progress" word is programmed as 0xffff first and reset later
                        *(u16*)(prog.buf + HDR_FLASH_IN_PROGRESS) = 0xffff;
                }
-               flash_unlock();
-               u32 baseaddr = BOOTLOADER_APP_START + prog.blocknum * dfu_function.wTransferSize;
+               u32 baseaddr = BOOTLOADER_APP_START + prog.blocknum * BLOCK_SIZE;
                DEBUG("DFU: Block %u -> %08x + %u\n", prog.blocknum, (uint) baseaddr, prog.len);
+               flash_unlock();
                flash_erase_page(baseaddr);
                for (uint i = 0; i < prog.len; i += 2) {
                        u16 data = *(u16 *)(prog.buf + i);
                        flash_program_half_word(baseaddr + i, data);
                }
                flash_lock();
+               for (uint i = 0; i < prog.len; i++) {
+                       if (*(byte *)(baseaddr + i) != prog.buf[i]) {
+                               DEBUG("DFU: Verification failed\n");
+                               dfu_state = STATE_DFU_ERROR;
+                       }
+               }
                dfu_state = STATE_DFU_DNLOAD_IDLE;
                return;
        case STATE_DFU_MANIFEST: