diff --git a/bootloader/main.c b/bootloader/main.c
index 2c7a9e36ace0c94917d3b12acb43b6cdb9cecb60..1e06e0eebeba8636d040767c0fd55186e41b8838 100644
--- a/bootloader/main.c
+++ b/bootloader/main.c
@@ -15,6 +15,9 @@
 #include "ff.h"
 #include "crc16-ccitt.h"
 #include "pb.h"
+#include "display.h"
+#include "GUI_Paint.h"
+#include "card10.h"
 
 #include "pmic.h"
 
@@ -194,19 +197,20 @@ static void pmic_button(bool falling)
 /******************************************************************************/
 int main(void)
 {
-    /* Copied from card10_init() */
-    I2C_Shutdown(MXC_I2C0_BUS0);
-    I2C_Init(MXC_I2C0_BUS0, I2C_FAST_MODE, NULL);
-
-    I2C_Shutdown(MXC_I2C1_BUS0);
-    I2C_Init(MXC_I2C1_BUS0, I2C_FAST_MODE, NULL);
+    printf("\n\nBootloader\n");
+    card10_init();
 
-    pmic_init();
     pmic_set_button_callback(pmic_button);
 
-    printf("\n\nBootloader\n");
+    Paint_DrawString_EN(0, 16*0, "Bootloader", &Font16, 0x0000, 0xffff);
+    Paint_DrawString_EN(0, 16*1, __DATE__, &Font16, 0x0000, 0xffff);
+
+    LCD_Update();
+
     // If the button is pressed, we go into MSC mode.
     if (PB_Get(0) || PB_Get(1)) {
+        Paint_DrawString_EN(0, 16*2, "USB activated. Waiting.", &Font16, 0x0000, 0xffff);
+        LCD_Update();
         run_usbmsc();
 
         // If we return, don't try to boot. Maybe rather trigger a software reset.
@@ -215,28 +219,34 @@ int main(void)
         while(1);
     }
 
-    //MXC_FLC0->clkdiv = 96;
-    //MXC_FLC0->clkdiv = 96;
-
     if(mount()) {
         if(check_integrity()) {
             printf("Found valid application image\n");
             if(is_update_needed()) {
                 printf("Trying to update application from external flash\n");
+                Paint_DrawString_EN(0, 16*4, "Updating...", &Font16, 0x0000, 0xffff);
+                LCD_Update();
                 erase_partition();
                 flash_partition();
             } else {
                 printf("No update needed\n");
             }
         } else {
+            Paint_DrawString_EN(0, 16*2, "Integrity check failed", &Font16, 0x0000, 0xffff);
+            LCD_Update();
             printf("Integrity check failed\n");
         }
     } else {
+        Paint_DrawString_EN(0, 16*2, "Failed to mount file system", &Font16, 0x0000, 0xffff);
+        LCD_Update();
         printf("Failed to mount the external flash\n");
     }
 
 
     printf("Trying to boot\n");
+    Paint_DrawString_EN(0, 16*4, "Trying to boot", &Font16, 0x0000, 0xffff);
+    LCD_Update();
+    //while(1);
     // boot partition
     boot((uintptr_t *) PARTITION_START);
 
diff --git a/lib/card10/card10.c b/lib/card10/card10.c
index 0b6b38fd5c656f63a9f78e8838a5edb1a6e2133e..5bcf409cd7017eeda189c97d385567ab63581dfc 100644
--- a/lib/card10/card10.c
+++ b/lib/card10/card10.c
@@ -65,35 +65,12 @@ void card10_init(void)
         while (1);
     }
 
-
     display_init();
 
     leds_init();
 
     GPIO_Config(&bhi_interrupt_pin);
 
-    if(bhy_driver_init(bhy1_fw)) {
-        printf("Failed to init bhy\n");
-    } else {
-        /* wait for the bhy trigger the interrupt pin go down and up again */
-        while (GPIO_InGet(&bhi_interrupt_pin));
-        while (!GPIO_InGet(&bhi_interrupt_pin));
-
-        /* the remapping matrix for BHI and Magmetometer should be configured here to make sure rotation vector is */
-        /* calculated in a correct coordinates system. */
-        int8_t                     bhy_mapping_matrix_config[3*3] = {0,-1,0,1,0,0,0,0,1};
-        int8_t                     mag_mapping_matrix_config[3*3] = {-1,0,0,0,1,0,0,0,-1};
-        bhy_mapping_matrix_set(PHYSICAL_SENSOR_INDEX_ACC, bhy_mapping_matrix_config);
-        bhy_mapping_matrix_set(PHYSICAL_SENSOR_INDEX_MAG, mag_mapping_matrix_config);
-        bhy_mapping_matrix_set(PHYSICAL_SENSOR_INDEX_GYRO, bhy_mapping_matrix_config);
-
-        /* the sic matrix should be calculated for customer platform by logging uncalibrated magnetometer data. */
-        /* the sic matrix here is only an example array (identity matrix). Customer should generate their own matrix. */
-        /* This affects magnetometer fusion performance. */
-        float sic_array[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
-        bhy_set_sic_matrix(sic_array);
-    }
-
     portexpander_init();
 }
 
@@ -135,6 +112,29 @@ void card10_diag(void)
         }
     }
 
+    if(bhy_driver_init(bhy1_fw)) {
+        printf("Failed to init bhy\n");
+    } else {
+        /* wait for the bhy trigger the interrupt pin go down and up again */
+        while (GPIO_InGet(&bhi_interrupt_pin));
+        while (!GPIO_InGet(&bhi_interrupt_pin));
+
+        /* the remapping matrix for BHI and Magmetometer should be configured here to make sure rotation vector is */
+        /* calculated in a correct coordinates system. */
+        int8_t                     bhy_mapping_matrix_config[3*3] = {0,-1,0,1,0,0,0,0,1};
+        int8_t                     mag_mapping_matrix_config[3*3] = {-1,0,0,0,1,0,0,0,-1};
+        bhy_mapping_matrix_set(PHYSICAL_SENSOR_INDEX_ACC, bhy_mapping_matrix_config);
+        bhy_mapping_matrix_set(PHYSICAL_SENSOR_INDEX_MAG, mag_mapping_matrix_config);
+        bhy_mapping_matrix_set(PHYSICAL_SENSOR_INDEX_GYRO, bhy_mapping_matrix_config);
+
+        /* the sic matrix should be calculated for customer platform by logging uncalibrated magnetometer data. */
+        /* the sic matrix here is only an example array (identity matrix). Customer should generate their own matrix. */
+        /* This affects magnetometer fusion performance. */
+        float sic_array[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
+        bhy_set_sic_matrix(sic_array);
+    }
+
+
 
     struct bme680_dev gas_sensor;
     gas_sensor.dev_id = BME680_I2C_ADDR_PRIMARY;