diff --git a/sw/bootloader/N64.ld b/sw/bootloader/N64.ld
index f495947..a44e160 100644
--- a/sw/bootloader/N64.ld
+++ b/sw/bootloader/N64.ld
@@ -56,9 +56,9 @@ SECTIONS {
     _sp = .;
 
     .fill : {
-        . = ALIGN(1024);
+        . = ALIGN(1024) - 4;
+        LONG(0xDEADBEEF);
         FILL(0xFFFFFFFF);
-        BYTE(0xFFFFFFFF);
         . = ORIGIN(rom) + LENGTH(rom);
     } > rom
 
diff --git a/sw/controller/src/hw.h b/sw/controller/src/hw.h
index afc830f..6daaaea 100644
--- a/sw/controller/src/hw.h
+++ b/sw/controller/src/hw.h
@@ -29,14 +29,6 @@ typedef enum {
     GPIO_IRQ_RISING     = 0b10,
 } gpio_irq_t;
 
-typedef enum {
-    I2C_START   = (1 << 0),
-    I2C_AUTOEND = (1 << 1),
-    I2C_STOP    = (1 << 2),
-    I2C_READ    = (1 << 3),
-    I2C_WRITE   = (1 << 4),
-} i2c_type_t;
-
 typedef enum {
     TIM_ID_CIC          = 0,
     TIM_ID_RTC          = 1,
diff --git a/sw/controller/src/lcmxo2.c b/sw/controller/src/lcmxo2.c
index a485299..ba66e81 100644
--- a/sw/controller/src/lcmxo2.c
+++ b/sw/controller/src/lcmxo2.c
@@ -217,12 +217,14 @@ static void lcmxo2_read_flash_page (uint8_t *buffer) {
     lcmxo2_execute_cmd(LSC_READ_INCR_NV, 1, CMD_DELAYED, buffer, FLASH_PAGE_SIZE, false);
 }
 
-static void lcmxo2_program_done (void) {
+static bool lcmxo2_program_done (void) {
     lcmxo2_execute_cmd(ISC_PROGRAM_DONE, 0, CMD_NORMAL, NULL, 0, false);
+    return lcmxo2_wait_busy();
 }
 
-static void lcmxo2_write_featbits (uint8_t *buffer) {
+static bool lcmxo2_write_featbits (uint8_t *buffer) {
     lcmxo2_execute_cmd(LSC_PROG_FEABITS, 0, CMD_NORMAL, buffer, FEATBITS_SIZE, true);
+    return lcmxo2_wait_busy();
 }
 
 static void lcmxo2_read_featbits (uint8_t *buffer) {
@@ -291,7 +293,9 @@ vendor_error_t vendor_update (uint32_t address, uint32_t length) {
             return lcmxo2_fail(VENDOR_ERROR_PROGRAM);
         }
     }
-    lcmxo2_program_done();
+    if (lcmxo2_program_done()) {
+        return lcmxo2_fail(VENDOR_ERROR_PROGRAM);
+    }
     lcmxo2_reset_flash_address();
     for (int i = 0; i < length; i += FLASH_PAGE_SIZE) {
         lcmxo2_read_flash_page(buffer);
@@ -346,6 +350,26 @@ static bool primer_check_rx_length (primer_cmd_e cmd, size_t rx_length) {
     return true;
 }
 
+static bool lcmxo2_init_featbits (void) {
+    uint8_t programmed[2] = { 0x00, 0x00 };
+    uint8_t target[2] = { FEATBITS_0_SPI_OFF, FEATBITS_1_PROGRAMN_OFF };
+    lcmxo2_read_featbits(programmed);
+    if ((programmed[0] == target[0]) && (programmed[1] == target[1])) {
+        return false;
+    }
+    if (lcmxo2_erase_featbits()) {
+        return true;
+    }
+    if (lcmxo2_write_featbits(target)) {
+        return true;
+    }
+    lcmxo2_read_featbits(programmed);
+    if ((programmed[0] != target[0]) || (programmed[1] != target[1])) {
+        return true;
+    }
+    return false;
+}
+
 
 void vendor_initial_configuration (vendor_get_cmd_t get_cmd, vendor_send_response_t send_response) {
     bool runninng = true;
@@ -419,17 +443,11 @@ void vendor_initial_configuration (vendor_get_cmd_t get_cmd, vendor_send_respons
                 break;
 
             case CMD_PROGRAM_DONE:
-                lcmxo2_program_done();
+                error = lcmxo2_program_done();
                 break;
 
             case CMD_INIT_FEATBITS:
-                lcmxo2_read_featbits(buffer);
-                if ((buffer[0] != FEATBITS_0_SPI_OFF) && (buffer[1] != FEATBITS_1_PROGRAMN_OFF)) {
-                    buffer[0] = FEATBITS_0_SPI_OFF;
-                    buffer[1] = FEATBITS_1_PROGRAMN_OFF;
-                    lcmxo2_erase_featbits();
-                    lcmxo2_write_featbits(buffer);
-                }
+                error = lcmxo2_init_featbits();
                 break;
 
             case CMD_REFRESH: