1 files changed, 47 insertions, 34 deletions

diff --git a/bootloader/sansa_as3525.c b/bootloader/sansa_as3525.c
index 3eb6159800..4219038d9a 100644
--- a/bootloader/sansa_as3525.c
+++ b/bootloader/sansa_as3525.c
@@ -27,10 +27,8 @@
 #include <inttypes.h>
 #include "config.h"
 #include "lcd.h"
-#ifdef USE_ROCKBOX_USB
 #include "usb.h"
 #include "sysfont.h"
-#endif /* USE_ROCKBOX_USB */
 #include "backlight.h"
 #include "button-target.h"
 #include "common.h"
@@ -41,6 +39,33 @@
 
 int show_logo(void);
 
+static void usb_mode(void)
+{
+    if(usb_detect() != USB_INSERTED)
+    {
+        const char msg[] = "Plug USB cable";
+        reset_screen();
+        lcd_putsxy( (LCD_WIDTH - (SYSFONT_WIDTH * sizeof(msg))) / 2,
+                    (LCD_HEIGHT - SYSFONT_HEIGHT) / 2, msg);
+        lcd_update();
+
+        /* wait until USB is plugged */
+        while(usb_detect() != USB_INSERTED) ;
+    }
+
+    const char msg[] = "Bootloader USB mode";
+    reset_screen();
+    lcd_putsxy( (LCD_WIDTH - (SYSFONT_WIDTH * sizeof(msg))) / 2,
+                (LCD_HEIGHT - SYSFONT_HEIGHT) / 2, msg);
+    lcd_update();
+
+    while(usb_detect() == USB_INSERTED)
+        sleep(HZ);
+
+    reset_screen();
+    lcd_update();
+}
+
 void main(void) __attribute__((noreturn));
 void main(void)
 {
@@ -84,53 +109,41 @@ void main(void)
 
     ret = storage_init();
     if(ret < 0)
-        error(EATA,ret);
+        error(EATA, ret, true);
 
-#ifdef USE_ROCKBOX_USB
     usb_init();
     usb_start_monitoring();
-    if(usb_detect() == USB_INSERTED)
-    {
-        const char msg[] = "Bootloader USB mode";
-        reset_screen();
-        lcd_putsxy( (LCD_WIDTH - (SYSFONT_WIDTH * sizeof(msg))) / 2,
-                    (LCD_HEIGHT - SYSFONT_HEIGHT) / 2, msg);
-        lcd_update();
-
-        while(usb_detect() == USB_INSERTED)
-            sleep(HZ);
-
-        reset_screen();
-        lcd_update();
-    }
-#endif /* USE_ROCKBOX_USB */
 
-    if(!disk_init(IF_MV(0)))
-        panicf("disk_init failed!");
+    /* Enter USB mode if USB is plugged and SELECT button is pressed */
+    if(btn & BUTTON_SELECT && usb_detect() == USB_INSERTED)
+        usb_mode();
 
-    ret = disk_mount_all();
+    while(!disk_init(IF_MV(0)))
+        usb_mode();
 
-    if(ret <= 0)
-        error(EDISK, ret);
+    while((ret = disk_mount_all()) <= 0)
+    {
+        error(EDISK, ret, false);
+        usb_mode();
+    }
 
     printf("Loading firmware");
 
     loadbuffer = (unsigned char*)DRAM_ORIG; /* DRAM */
     buffer_size = (int)(loadbuffer + (DRAM_SIZE) - TTB_SIZE);
 
-    ret = load_firmware(loadbuffer, BOOTFILE, buffer_size);
-    if(ret < 0)
-        error(EBOOTFILE, ret);
-
-    if (ret == EOK)
+    while((ret = load_firmware(loadbuffer, BOOTFILE, buffer_size)) < 0)
     {
-        kernel_entry = (void*) loadbuffer;
-        cpucache_invalidate();
-        printf("Executing");
-        kernel_entry();
-        printf("ERR: Failed to boot");
+        error(EBOOTFILE, ret, false);
+        usb_mode();
     }
 
+    kernel_entry = (void*) loadbuffer;
+    cpucache_invalidate();
+    printf("Executing");
+    kernel_entry();
+    printf("ERR: Failed to boot");
+
     /* never returns */
     while(1) ;
 }