diff --git a/components/mch2022-bsp/hardware.c b/components/mch2022-bsp/hardware.c
index 718cddf..3409421 100644
--- a/components/mch2022-bsp/hardware.c
+++ b/components/mch2022-bsp/hardware.c
@@ -117,22 +117,30 @@ esp_err_t board_init(bool* aLcdReady) {
         ESP_LOGE(TAG, "Initializing RP2040 failed");
         return res;
     }
+    
+    uint8_t rp2040_fw_version;
+    if (rp2040_get_firmware_version(&dev_rp2040, &rp2040_fw_version) != ESP_OK) {
+        ESP_LOGE(TAG, "Initializing RP2040 failed to read firmware version");
+        return ESP_FAIL;
+    }
+    
+    if (rp2040_fw_version != 0xFF) { // Only init FPGA when RP2040 is not in bootloader mode
+        // FPGA
+        dev_ice40.spi_bus = SPI_BUS;
+        dev_ice40.pin_cs = GPIO_SPI_CS_FPGA;
+        dev_ice40.pin_done = -1;
+        dev_ice40.pin_reset = -1;
+        dev_ice40.pin_int = GPIO_INT_FPGA;
+        dev_ice40.spi_speed = 23000000; // 23MHz
+        dev_ice40.spi_max_transfer_size = SPI_MAX_TRANSFER_SIZE;
+        dev_ice40.get_done = ice40_get_done_wrapper;
+        dev_ice40.set_reset = ice40_set_reset_wrapper;
 
-    // FPGA
-    dev_ice40.spi_bus = SPI_BUS;
-    dev_ice40.pin_cs = GPIO_SPI_CS_FPGA;
-    dev_ice40.pin_done = -1;
-    dev_ice40.pin_reset = -1;
-    dev_ice40.pin_int = GPIO_INT_FPGA;
-    dev_ice40.spi_speed = 10000000; // 10 MHz //23000000; // 23MHz
-    dev_ice40.spi_max_transfer_size = SPI_MAX_TRANSFER_SIZE;
-    dev_ice40.get_done = ice40_get_done_wrapper;
-    dev_ice40.set_reset = ice40_set_reset_wrapper;
-
-    res = ice40_init(&dev_ice40);
-    if (res != ESP_OK) {
-        ESP_LOGE(TAG, "Initializing FPGA failed");
-        return res;
+        res = ice40_init(&dev_ice40);
+        if (res != ESP_OK) {
+            ESP_LOGE(TAG, "Initializing FPGA failed");
+            return res;
+        }
     }
 
     // BNO055 sensor on system I2C bus
diff --git a/components/mch2022-bsp/rp2040.c b/components/mch2022-bsp/rp2040.c
index d250d53..0d6c09f 100644
--- a/components/mch2022-bsp/rp2040.c
+++ b/components/mch2022-bsp/rp2040.c
@@ -51,15 +51,14 @@ void rp2040_intr_handler(void *arg) {
 esp_err_t rp2040_init(RP2040* device) {
     esp_err_t res;
 
-    uint8_t firmware_version;
-    res = rp2040_get_firmware_version(device, &firmware_version);
+    res = rp2040_get_firmware_version(device, &device->_fw_version);
     if (res != ESP_OK) {
         ESP_LOGE(TAG, "Failed to read firmware version");
         return res;
     }
 
-    if (firmware_version != 1) {
-        ESP_LOGE(TAG, "Unsupported RP2040 firmware version (%u) found", firmware_version);
+    if (device->_fw_version < 1) {
+        ESP_LOGE(TAG, "Unsupported RP2040 firmware version (%u) found", device->_fw_version);
         return ESP_ERR_INVALID_VERSION;
     }
 
@@ -106,13 +105,36 @@ esp_err_t rp2040_get_firmware_version(RP2040* device, uint8_t* version) {
     return i2c_read_reg(device->i2c_bus, device->i2c_address, RP2040_REG_FW_VER, version, 1);
 }
 
+esp_err_t rp2040_get_bootloader_version(RP2040* device, uint8_t* version) {
+    if (device->_fw_version != 0xFF) return ESP_FAIL;
+    return i2c_read_reg(device->i2c_bus, device->i2c_address, RP2040_BL_REG_BL_VER, version, 1);
+}
+
+esp_err_t rp2040_get_bootloader_state(RP2040* device, uint8_t* state) {
+    if (device->_fw_version != 0xFF) return ESP_FAIL;
+    return i2c_read_reg(device->i2c_bus, device->i2c_address, RP2040_BL_REG_BL_STATE, state, 1);
+}
+
+esp_err_t rp2040_set_bootloader_ctrl(RP2040* device, uint8_t action) {
+    if (device->_fw_version != 0xFF) return ESP_FAIL;
+    return i2c_write_reg_n(device->i2c_bus, device->i2c_address, RP2040_BL_REG_BL_CTRL, &action, 1);
+}
+
+esp_err_t rp2040_reboot_to_bootloader(RP2040* device) {
+    if ((device->_fw_version < 0x01) && (device->_fw_version >= 0xFF)) return ESP_FAIL;
+    uint8_t value = 0xBE;
+    return i2c_write_reg_n(device->i2c_bus, device->i2c_address, RP2040_REG_BL_TRIGGER, &value, 1);
+}
+
 esp_err_t rp2040_get_gpio_dir(RP2040* device, uint8_t gpio, bool* direction) {
+    if ((device->_fw_version < 0x01) && (device->_fw_version >= 0xFF)) return ESP_FAIL;
     esp_err_t res = i2c_read_reg(device->i2c_bus, device->i2c_address, RP2040_REG_GPIO_DIR, &device->_gpio_direction, 1);
     if (res != ESP_OK) return res;
     *direction = (device->_gpio_direction >> gpio) & 0x01;
     return ESP_OK;
 }
 esp_err_t rp2040_set_gpio_dir(RP2040* device, uint8_t gpio, bool direction) {
+    if ((device->_fw_version < 0x01) && (device->_fw_version >= 0xFF)) return ESP_FAIL;
     if (direction) {
         device->_gpio_direction |= 1UL << gpio;
     } else {
@@ -122,6 +144,7 @@ esp_err_t rp2040_set_gpio_dir(RP2040* device, uint8_t gpio, bool direction) {
 }
 
 esp_err_t rp2040_get_gpio_value(RP2040* device, uint8_t gpio, bool* value) {
+    if ((device->_fw_version < 0x01) && (device->_fw_version >= 0xFF)) return ESP_FAIL;
     uint8_t reg_value;
     esp_err_t res = i2c_read_reg(device->i2c_bus, device->i2c_address, RP2040_REG_GPIO_IN, &reg_value, 1);
     if (res != ESP_OK) return res;
@@ -130,6 +153,7 @@ esp_err_t rp2040_get_gpio_value(RP2040* device, uint8_t gpio, bool* value) {
 }
 
 esp_err_t rp2040_set_gpio_value(RP2040* device, uint8_t gpio, bool value) {
+    if ((device->_fw_version < 0x01) && (device->_fw_version >= 0xFF)) return ESP_FAIL;
     if (value) {
         device->_gpio_value |= 1UL << gpio;
     } else {
@@ -139,18 +163,22 @@ esp_err_t rp2040_set_gpio_value(RP2040* device, uint8_t gpio, bool value) {
 }
 
 esp_err_t rp2040_get_lcd_backlight(RP2040* device, uint8_t* brightness) {
+    if ((device->_fw_version < 0x01) && (device->_fw_version >= 0xFF)) return ESP_FAIL;
     return i2c_read_reg(device->i2c_bus, device->i2c_address, RP2040_REG_LCD_BACKLIGHT, brightness, 1);
 }
 
 esp_err_t rp2040_set_lcd_backlight(RP2040* device, uint8_t brightness) {
+    if ((device->_fw_version < 0x01) && (device->_fw_version >= 0xFF)) return ESP_OK; // Ignore if unsupported
     return i2c_write_reg_n(device->i2c_bus, device->i2c_address, RP2040_REG_LCD_BACKLIGHT, &brightness, 1);
 }
 
 esp_err_t rp2040_set_fpga(RP2040* device, bool enabled) {
+    if ((device->_fw_version < 0x01) && (device->_fw_version >= 0xFF)) return ESP_FAIL;
     uint8_t value = enabled ? 0x01 : 0x00;
     return i2c_write_reg_n(device->i2c_bus, device->i2c_address, RP2040_REG_FPGA, &value, 1);
 }
 
 esp_err_t rp2040_read_buttons(RP2040* device, uint16_t* value) {
+    if ((device->_fw_version < 0x01) && (device->_fw_version >= 0xFF)) return ESP_FAIL;
     return i2c_read_reg(device->i2c_bus, device->i2c_address, RP2040_REG_INPUT1, (uint8_t*) value, 2);
 }
diff --git a/components/mch2022-bsp/rp2040.h b/components/mch2022-bsp/rp2040.h
index 1f5ccfc..ca7ddfb 100644
--- a/components/mch2022-bsp/rp2040.h
+++ b/components/mch2022-bsp/rp2040.h
@@ -25,6 +25,7 @@ enum {
     RP2040_REG_ADC_VALUE_VBAT1,
     RP2040_REG_ADC_VALUE_VBAT2,
     RP2040_REG_USB,
+    RP2040_REG_BL_TRIGGER,
     RP2040_REG_SCRATCH0, // Used by the ESP32 to store boot parameters, can also be read and written to from WebUSB
     RP2040_REG_SCRATCH1,
     RP2040_REG_SCRATCH2,
@@ -91,6 +92,13 @@ enum {
     RP2040_REG_SCRATCH63
 };
 
+enum {
+    RP2040_BL_REG_FW_VER,
+    RP2040_BL_REG_BL_VER,
+    RP2040_BL_REG_BL_STATE,
+    RP2040_BL_REG_BL_CTRL
+};
+
 enum {
     RP2040_INPUT_BUTTON_HOME = 0,
     RP2040_INPUT_BUTTON_MENU,
@@ -119,6 +127,7 @@ typedef struct {
     xSemaphoreHandle _intr_trigger;
     uint8_t          _gpio_direction;
     uint8_t          _gpio_value;
+    uint8_t          _fw_version;
 } RP2040;
 
 typedef struct _rp2040_input_message {
@@ -130,6 +139,11 @@ esp_err_t rp2040_init(RP2040* device);
 
 esp_err_t rp2040_get_firmware_version(RP2040* device, uint8_t* version);
 
+esp_err_t rp2040_get_bootloader_version(RP2040* device, uint8_t* version);
+esp_err_t rp2040_get_bootloader_state(RP2040* device, uint8_t* state);
+esp_err_t rp2040_set_bootloader_ctrl(RP2040* device, uint8_t action);
+esp_err_t rp2040_reboot_to_bootloader(RP2040* device);
+
 esp_err_t rp2040_get_gpio_dir(RP2040* device, uint8_t gpio, bool* direction);
 esp_err_t rp2040_set_gpio_dir(RP2040* device, uint8_t gpio, bool direction);
 
diff --git a/main/main.c b/main/main.c
index ea6b263..13df72b 100644
--- a/main/main.c
+++ b/main/main.c
@@ -268,10 +268,44 @@ void app_main(void) {
     }
     
     ws2812_init(GPIO_LED_DATA);
-    uint8_t ledBuffer[15] = {50, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+    uint8_t ledBuffer[15] = {50, 0, 0, 50, 0, 0, 50, 0, 0, 50, 0, 0, 50, 0, 0};
     ws2812_send_data(ledBuffer, sizeof(ledBuffer));
     
-    //fpga_test(ili9341, ice40, rp2040->queue);
+    uint8_t fw_version;
+    if (rp2040_get_firmware_version(rp2040, &fw_version) != ESP_OK) {
+        graphics_task(pax_buffer, ili9341, framebuffer, NULL, "RP2040 FW VERSION READ FAILED");
+        restart();
+    }
+    
+    if (fw_version == 0xFF) {
+        // RP2040 is in bootloader mode
+        char buffer[255] = {0};
+        graphics_task(pax_buffer, ili9341, framebuffer, NULL, "RP2040 BL");
+        uint8_t bl_version;
+        if (rp2040_get_bootloader_version(rp2040, &bl_version) != ESP_OK) {
+            graphics_task(pax_buffer, ili9341, framebuffer, NULL, "RP2040 BL VERSION READ FAILED");
+            restart();
+        }
+        while (true) {
+            uint8_t bl_state;
+            if (rp2040_get_bootloader_state(rp2040, &bl_state) != ESP_OK) {
+                graphics_task(pax_buffer, ili9341, framebuffer, NULL, "RP2040 BL STATE READ FAILED");
+                restart();
+            }
+            snprintf(buffer, sizeof(buffer), "RP2040 BL (%02X): %02X", bl_version, bl_state);
+            graphics_task(pax_buffer, ili9341, framebuffer, NULL, buffer);
+            vTaskDelay(200 / portTICK_PERIOD_MS);
+            if (bl_state == 0xB0) {
+                printf("SYNC");
+            }
+        }
+        return;
+    }
+    
+    /*while (true) {
+        fpga_test(ili9341, ice40, rp2040->queue);
+        vTaskDelay(200 / portTICK_PERIOD_MS);
+    }*/
     
     /*while (true) {
         uint16_t state;
@@ -297,7 +331,9 @@ void app_main(void) {
             appfs_boot_app(appfs_fd);
         } else if (menu_action == ACTION_FPGA) {
             graphics_task(pax_buffer, ili9341, framebuffer, NULL, "FPGA TEST");
-            fpga_test(ili9341, ice40, rp2040->queue);
+            //fpga_test(ili9341, ice40, rp2040->queue);
+            rp2040_reboot_to_bootloader(rp2040);
+            restart();
         } else if (menu_action == ACTION_INSTALLER) {
             graphics_task(pax_buffer, ili9341, framebuffer, NULL, "INSTALLER");
             //appfs_store_app();