diff --git a/main/main.c b/main/main.c
index 6f8bfbb..a75879c 100644
--- a/main/main.c
+++ b/main/main.c
@@ -18,6 +18,8 @@
 #include "driver_framebuffer.h"
 
 #include "rp2040.h"
+#include "rp2040bl.h"
+#include "rp2040firmware.h"
 
 #include "fpga_test.h"
 
@@ -30,8 +32,6 @@
 
 #include "ws2812.h"
 
-#include "rp2040firmware.h"
-
 #include "esp32/rom/crc.h"
 
 static const char *TAG = "main";
@@ -261,150 +261,7 @@ void menu_wifi_settings(xQueueHandle buttonQueue, pax_buf_t* pax_buffer, ILI9341
     menu_free(menu);
 }
 
-void flush_stdin() {
-    uint8_t* data = malloc(2048);
-    uart_read_bytes(0, data, 2048 - 1, 20 / portTICK_PERIOD_MS);
-    free(data);
-    /*while (true) {
-        char in = getc(stdin);
-        if (in == 0xFF) {
-            break;
-        }
-    }*/
-}
-
-bool read_stdin(uint8_t* buffer, uint32_t len, uint32_t timeout) {
-    int read = uart_read_bytes(0, buffer, len, timeout / portTICK_PERIOD_MS);
-    return (read == len);
-    
-    /*uint8_t index = 0;
-    uint32_t timeout_counter = 0;
-    while (index < len) {
-        char in = getc(stdin);
-        if (in != 0xFF) {
-            buffer[index] = in;
-            index++;
-        } else {
-            vTaskDelay(10 / portTICK_PERIOD_MS);
-            timeout_counter+=10;
-            if (timeout_counter > timeout) {
-                return false;
-            }
-        }
-    }
-    return true;*/
-}
-
-bool rp2040_bl_sync() {
-    flush_stdin();
-    char command[] = "SYNC";
-    uart_write_bytes(0, command, 4);
-    uint8_t rx_buffer[4 * 6];
-    read_stdin(rx_buffer, sizeof(rx_buffer), 1000);
-    if (memcmp(rx_buffer, "PICO", 4) != 0) return false;
-    return true;
-}
-
-bool rp2040_bl_get_info(uint32_t* flash_start, uint32_t* flash_size, uint32_t* erase_size, uint32_t* write_size, uint32_t* max_data_len) {
-    flush_stdin();
-    char command[] = "INFO";
-    uart_write_bytes(0, command, 4);
-    /*printf("INFO");*/
-    uint8_t rx_buffer[4 * 6];
-    read_stdin(rx_buffer, sizeof(rx_buffer), 1000);
-    if (memcmp(rx_buffer, "OKOK", 4) != 0) return false;
-    memcpy((uint8_t*) flash_start,  &rx_buffer[4 * 1], 4);
-    memcpy((uint8_t*) flash_size,   &rx_buffer[4 * 2], 4);
-    memcpy((uint8_t*) erase_size,   &rx_buffer[4 * 3], 4);
-    memcpy((uint8_t*) write_size,   &rx_buffer[4 * 4], 4);
-    memcpy((uint8_t*) max_data_len, &rx_buffer[4 * 5], 4);
-    return true;
-}
-
-bool rp2040_bl_erase(uint32_t address, uint32_t length) {
-    flush_stdin();
-    char command[12];
-    snprintf(command, 5, "ERAS");
-    memcpy(command + 4, (char*) &address, 4);
-    memcpy(command + 8, (char*) &length, 4);
-    uart_write_bytes(0, command, sizeof(command));
-    /*printf("ERAS");
-    uint8_t* addres_u8 = (uint8_t*) &address;
-    putc(addres_u8[0], stdout);
-    putc(addres_u8[1], stdout);
-    putc(addres_u8[2], stdout);
-    putc(addres_u8[3], stdout);
-    uint8_t* length_u8 = (uint8_t*) &length;
-    putc(length_u8[0], stdout);
-    putc(length_u8[1], stdout);
-    putc(length_u8[2], stdout);
-    putc(length_u8[3], stdout);*/
-    uint8_t rx_buffer[4];
-    read_stdin(rx_buffer, sizeof(rx_buffer), 10000);
-    if (memcmp(rx_buffer, "OKOK", 4) != 0) return false;
-    return true;
-}
-
-bool rp2040_bl_write(uint32_t address, uint32_t length, uint8_t* data, uint32_t* crc) {
-    flush_stdin();
-    char command[12];
-    snprintf(command, 5, "WRIT");
-    memcpy(command + 4, (char*) &address, 4);
-    memcpy(command + 8, (char*) &length, 4);
-    uart_write_bytes(0, command, sizeof(command));
-    /*printf("WRIT");
-    uint8_t* addres_u8 = (uint8_t*) &address;
-    putc(addres_u8[0], stdout);
-    putc(addres_u8[1], stdout);
-    putc(addres_u8[2], stdout);
-    putc(addres_u8[3], stdout);
-    uint8_t* length_u8 = (uint8_t*) &length;
-    putc(length_u8[0], stdout);
-    putc(length_u8[1], stdout);
-    putc(length_u8[2], stdout);
-    putc(length_u8[3], stdout);
-    
-    for (uint32_t index = 0; index < length; index++) {
-        putc(data[index], stdout);
-    }*/
-    
-    uart_write_bytes(0, data, length);
-    
-    
-    uint8_t rx_buffer[8];
-    read_stdin(rx_buffer, sizeof(rx_buffer), 10000);
-    if (memcmp(rx_buffer, "OKOK", 4) != 0) return false;
-    
-    memcpy((uint8_t*) crc, &rx_buffer[4 * 1], 4);
-    return true;
-}
-
-bool rp2040_bl_seal(uint32_t addr, uint32_t vtor, uint32_t length, uint32_t crc) {
-    flush_stdin();
-    char command[20];
-    snprintf(command, 5, "SEAL");
-    memcpy(command + 4, (char*) &addr, 4);
-    memcpy(command + 8, (char*) &vtor, 4);
-    memcpy(command + 12, (char*) &length, 4);
-    memcpy(command + 16, (char*) &crc, 4);
-    uart_write_bytes(0, command, sizeof(command));
-    uint8_t rx_buffer[4];
-    read_stdin(rx_buffer, sizeof(rx_buffer), 10000);
-    if (memcmp(rx_buffer, "OKOK", 4) != 0) return false;
-    return true;
-}
-
-bool rp2040_bl_go(uint32_t vtor) {
-    flush_stdin();
-    char command[8];
-    snprintf(command, 5, "GOGO");
-    memcpy(command + 4, (char*) &vtor, 4);
-    uart_write_bytes(0, command, sizeof(command));
-    return true;
-}
-
 void app_main(void) {
-    flush_stdin();
     esp_err_t res;
     
     /* Initialize memory */
@@ -480,42 +337,54 @@ void app_main(void) {
         restart();
     }
     
-    if (fw_version == 0xFF) {
-        // RP2040 is in bootloader mode
-        char buffer[255] = {0};
-        graphics_task(pax_buffer, ili9341, framebuffer, NULL, "Updating RP2040...");
-        vTaskDelay(1 / portTICK_PERIOD_MS);
+    if (fw_version == 0xFF) { // RP2040 is in bootloader mode
+        char message[64];
+        pax_noclip(pax_buffer);
+        pax_background(pax_buffer, 0x325aa8);
+        snprintf(message, sizeof(message) - 1, "Updating RP2040...");
+        pax_draw_text(pax_buffer, 0xFFFFFFFF, NULL, 18, 0, 20*0, message);
+        ili9341_write(ili9341, framebuffer);
+
         uint8_t bl_version;
         if (rp2040_get_bootloader_version(rp2040, &bl_version) != ESP_OK) {
-            graphics_task(pax_buffer, ili9341, framebuffer, NULL, "Communication error (1)");
+            pax_noclip(pax_buffer);
+            pax_background(pax_buffer, 0xa85a32);
+            snprintf(message, sizeof(message) - 1, "RP2040 update failed");
+            pax_draw_text(pax_buffer, 0xFFFFFFFF, NULL, 18, 0, 20*0, message);
+            snprintf(message, sizeof(message) - 1, "Communication error (1)");
+            pax_draw_text(pax_buffer, 0xFFFFFFFF, NULL, 12, 0, 20*1, message);
+            ili9341_write(ili9341, framebuffer);
             restart();
         }
-        if (bl_version != 0x01) {
+        /*if (bl_version != 0x01) {
             graphics_task(pax_buffer, ili9341, framebuffer, NULL, "Unknown BL version");
             restart();
-        }
+        }*/
         
-        ESP_ERROR_CHECK(uart_driver_install(0, 2048, 0, 0, NULL, 0));
-        uart_config_t uart_config = {
-            .baud_rate = 115200,
-            .data_bits = UART_DATA_8_BITS,
-            .parity    = UART_PARITY_DISABLE,
-            .stop_bits = UART_STOP_BITS_1,
-            .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
-            .source_clk = UART_SCLK_APB,
-        };
-        ESP_ERROR_CHECK(uart_param_config(0, &uart_config));
+        rp2040_bl_install_uart();
         
-        graphics_task(pax_buffer, ili9341, framebuffer, NULL, "Waiting for bootloader...");
+        pax_noclip(pax_buffer);
+        pax_background(pax_buffer, 0x325aa8);
+        snprintf(message, sizeof(message) - 1, "Updating RP2040...");
+        pax_draw_text(pax_buffer, 0xFFFFFFFF, NULL, 18, 0, 20*0, message);
+        snprintf(message, sizeof(message) - 1, "Waiting for bootloader");
+        pax_draw_text(pax_buffer, 0xFFFFFFFF, NULL, 12, 0, 20*1, message);
+        ili9341_write(ili9341, framebuffer);
+
         while (true) {
             vTaskDelay(1 / portTICK_PERIOD_MS);
             uint8_t bl_state;
             if (rp2040_get_bootloader_state(rp2040, &bl_state) != ESP_OK) {
-                graphics_task(pax_buffer, ili9341, framebuffer, NULL, "Communication error (2)");
+                pax_noclip(pax_buffer);
+                pax_background(pax_buffer, 0xa85a32);
+                snprintf(message, sizeof(message) - 1, "RP2040 update failed");
+                pax_draw_text(pax_buffer, 0xFFFFFFFF, NULL, 18, 0, 20*0, message);
+                snprintf(message, sizeof(message) - 1, "Communication error (2)");
+                pax_draw_text(pax_buffer, 0xFFFFFFFF, NULL, 12, 0, 20*1, message);
+                ili9341_write(ili9341, framebuffer);
                 restart();
             }
             if (bl_state == 0xB0) {
-                graphics_task(pax_buffer, ili9341, framebuffer, NULL, "Bootloader ready");
                 break;
             }
             if (bl_state > 0xB0) {
@@ -523,63 +392,39 @@ void app_main(void) {
                 restart();
             }
         }
+
+        pax_noclip(pax_buffer);
+        pax_background(pax_buffer, 0x325aa8);
+        snprintf(message, sizeof(message) - 1, "Updating RP2040...");
+        pax_draw_text(pax_buffer, 0xFFFFFFFF, NULL, 18, 0, 20*0, message);
+        snprintf(message, sizeof(message) - 1, "Waiting for bootloader sync");
+        pax_draw_text(pax_buffer, 0xFFFFFFFF, NULL, 12, 0, 20*1, message);
+        ili9341_write(ili9341, framebuffer);
         
-        graphics_task(pax_buffer, ili9341, framebuffer, NULL, "Sync to BL...");
         char rx_buffer[16];
         uint8_t rx_buffer_pos = 0;
         memset(rx_buffer, 0, sizeof(rx_buffer));
         while (true) {
-            //printf("SYNC");
-            
-            //uint8_t rx_buffer[4];
-            //read_stdin(rx_buffer, sizeof(rx_buffer), 1000);
-            
-            /*char in = 0xFF;
-            do {
-                in = getc(stdin);
-                if (in != 0xFF) {
-                    rx_buffer[rx_buffer_pos] = in;
-                    rx_buffer_pos++;
-                }
-            } while ((in != 0xFF) && (rx_buffer_pos < sizeof(rx_buffer)));*/
-            
-            //if (memcmp(rx_buffer, "PICO", 4) == 0) {
-            //    break;
-            //}
-            
-            /*if (rx_buffer_pos >= 4) {
-                for (uint8_t i = 0; i < sizeof(rx_buffer) - 1; i++) {
-                    rx_buffer[i] = rx_buffer[i+1];
-                    if (memcmp(rx_buffer, "PICO", 4) == 0) {
-                        break;
-                    }
-                }
-                rx_buffer_pos--;
-            }*/
-            
-            flush_stdin();
             if (rp2040_bl_sync()) break;
-            
             vTaskDelay(500 / portTICK_PERIOD_MS);
         }
-        graphics_task(pax_buffer, ili9341, framebuffer, NULL, "Synced to BL!");
-        
-        /*char in = 0xFF;
-        do {
-            in = getc(stdin);
-        } while (in != 0xFF);*/
-        flush_stdin();
         
         uint32_t flash_start = 0, flash_size = 0, erase_size = 0, write_size = 0, max_data_len = 0;
         
         bool success = rp2040_bl_get_info(&flash_start, &flash_size, &erase_size, &write_size, &max_data_len);
         
         if (!success) {
-            graphics_task(pax_buffer, ili9341, framebuffer, NULL, "BL INFO FAIL");
+            pax_noclip(pax_buffer);
+            pax_background(pax_buffer, 0xa85a32);
+            snprintf(message, sizeof(message) - 1, "RP2040 update failed");
+            pax_draw_text(pax_buffer, 0xFFFFFFFF, NULL, 18, 0, 20*0, message);
+            snprintf(message, sizeof(message) - 1, "Failed to read information");
+            pax_draw_text(pax_buffer, 0xFFFFFFFF, NULL, 12, 0, 20*1, message);
+            ili9341_write(ili9341, framebuffer);
             restart();
         }
         
-        pax_noclip(pax_buffer);
+        /*pax_noclip(pax_buffer);
         pax_background(pax_buffer, 0xCCCCCC);
         char message[64];
         memset(message, 0, sizeof(message));
@@ -593,17 +438,33 @@ void app_main(void) {
         pax_draw_text(pax_buffer, 0xFF000000, NULL, 18, 0, 20*3, message);
         snprintf(message, sizeof(message) - 1, "Max data ln: 0x%08X", max_data_len);
         pax_draw_text(pax_buffer, 0xFF000000, NULL, 18, 0, 20*4, message);
+        ili9341_write(ili9341, framebuffer);*/
+        
+        pax_noclip(pax_buffer);
+        pax_background(pax_buffer, 0x325aa8);
+        snprintf(message, sizeof(message) - 1, "Updating RP2040...");
+        pax_draw_text(pax_buffer, 0xFFFFFFFF, NULL, 18, 0, 20*0, message);
+        snprintf(message, sizeof(message) - 1, "Erasing flash");
+        pax_draw_text(pax_buffer, 0xFFFFFFFF, NULL, 12, 0, 20*1, message);
         ili9341_write(ili9341, framebuffer);
         
-        bool eraseSuccess = rp2040_bl_erase(flash_start, flash_size - erase_size);
-        snprintf(message, sizeof(message) - 1, "Erase      : %s", eraseSuccess ? "YES" : "NO");
-        pax_draw_text(pax_buffer, eraseSuccess ? 0xFF000000 : 0xFFFF0000, NULL, 18, 0, 20*5, message);
-        ili9341_write(ili9341, framebuffer);
-        vTaskDelay(1000 / portTICK_PERIOD_MS);
+        uint32_t erase_length = sizeof(mch2022_firmware_bin);
+        erase_length = erase_length + erase_size - (erase_length % erase_size); // Round up to erase size
+        
+        if (erase_length > flash_size - erase_size) {
+            erase_length = flash_size - erase_size;
+        }
+        
+        bool eraseSuccess = rp2040_bl_erase(flash_start, erase_length);
         
         if (!eraseSuccess) {
-            vTaskDelay(1000 / portTICK_PERIOD_MS);
-            vTaskDelay(1000 / portTICK_PERIOD_MS);
+            pax_noclip(pax_buffer);
+            pax_background(pax_buffer, 0xa85a32);
+            snprintf(message, sizeof(message) - 1, "RP2040 update failed");
+            pax_draw_text(pax_buffer, 0xFFFFFFFF, NULL, 18, 0, 20*0, message);
+            snprintf(message, sizeof(message) - 1, "Failed to erase flash");
+            pax_draw_text(pax_buffer, 0xFFFFFFFF, NULL, 12, 0, 20*1, message);
+            ili9341_write(ili9341, framebuffer);
             vTaskDelay(1000 / portTICK_PERIOD_MS);
             restart();
         }
@@ -622,15 +483,17 @@ void app_main(void) {
             }
             
             if (txSize == 0) break;
+
+            uint8_t percentage = position * 100 / sizeof(mch2022_firmware_bin);
             
             pax_noclip(pax_buffer);
-            pax_background(pax_buffer, 0xCCCCCC);
-            memset(message, 0, sizeof(message));
-            snprintf(message, sizeof(message) - 1, "Write to : 0x%08X", 0x10010000 + position);
-            pax_draw_text(pax_buffer, 0xFF000000, NULL, 18, 0, 20*0, message);
-            snprintf(message, sizeof(message) - 1, "Write len: 0x%08X", txSize);
-            pax_draw_text(pax_buffer, 0xFF000000, NULL, 18, 0, 20*1, message);
+            pax_background(pax_buffer, 0x325aa8);
+            snprintf(message, sizeof(message) - 1, "Updating RP2040... %u%%", percentage);
+            pax_draw_text(pax_buffer, 0xFFFFFFFF, NULL, 18, 0, 20*0, message);
+            snprintf(message, sizeof(message) - 1, "Writing @ 0x%08X", 0x10010000 + position);
+            pax_draw_text(pax_buffer, 0xFFFFFFFF, NULL, 12, 0, 20*1, message);
             ili9341_write(ili9341, framebuffer);
+
             uint32_t checkCrc = 0;
             memset(txBuffer, 0, write_size);
             memcpy(txBuffer, &mch2022_firmware_bin[position], txSize);
@@ -638,23 +501,10 @@ void app_main(void) {
             totalCrc = crc32_le(totalCrc, txBuffer, write_size);
             totalLength += write_size;
             bool writeSuccess = rp2040_bl_write(0x10010000 + position, write_size, txBuffer, &checkCrc);
-            snprintf(message, sizeof(message) - 1, "Write res: %s", writeSuccess ? "YES" : "NO");
-            pax_draw_text(pax_buffer, 0xFF000000, NULL, 18, 0, 20*2, message);
-            snprintf(message, sizeof(message) - 1, "CRC      : %08X", blockCrc);
-            pax_draw_text(pax_buffer, 0xFF000000, NULL, 18, 0, 20*3, message);
-            snprintf(message, sizeof(message) - 1, "CRC CHECK: %08X", checkCrc);
-            pax_draw_text(pax_buffer, 0xFF000000, NULL, 18, 0, 20*4, message);
-            snprintf(message, sizeof(message) - 1, "CRC OK?  : %s", (blockCrc == checkCrc) ? "YES" : "NO");
-            pax_draw_text(pax_buffer, 0xFF000000, NULL, 18, 0, 20*5, message);
-            snprintf(message, sizeof(message) - 1, "CRC TOTAL: %08X", totalCrc);
-            pax_draw_text(pax_buffer, 0xFF000000, NULL, 18, 0, 20*7, message);
-            ili9341_write(ili9341, framebuffer);
             if (writeSuccess && (blockCrc == checkCrc)) {
-                vTaskDelay(10 / portTICK_PERIOD_MS);
                 position += txSize;
             } else {
                 while (!rp2040_bl_sync()) {
-                    flush_stdin();
                     vTaskDelay(20 / portTICK_PERIOD_MS);
                 }
             }
@@ -699,27 +549,6 @@ void app_main(void) {
     pax_draw_text(pax_buffer, 0xFF000000, NULL, 18, 0, 20*0, message);
     ili9341_write(ili9341, framebuffer);
     vTaskDelay(1000 / portTICK_PERIOD_MS);
-    
-    /*while (true) {
-        fpga_test(ili9341, ice40, rp2040->queue);
-        vTaskDelay(200 / portTICK_PERIOD_MS);
-    }*/
-    
-    /*while (true) {
-        uint16_t state;
-        rp2040_read_buttons(rp2040, &state);
-        printf("Button state: %04X\n", state);
-        vTaskDelay(100 / portTICK_PERIOD_MS);
-        ledBuffer[1] = 255;
-        ws2812_send_data(ledBuffer, sizeof(ledBuffer));
-        vTaskDelay(200 / portTICK_PERIOD_MS);
-        ledBuffer[1] = 0;
-        ledBuffer[0] = 255;
-        ws2812_send_data(ledBuffer, sizeof(ledBuffer));
-        fpga_test(ili9341, ice40, rp2040->queue);
-        ledBuffer[0] = 0;
-        ws2812_send_data(ledBuffer, sizeof(ledBuffer));
-    }*/
 
     while (true) {
         menu_action_t menu_action;