3 files changed, 54 insertions, 13 deletions

diff --git a/apps/screens.c b/apps/screens.c
index c327e65037..e1e1c37bf5 100644
--- a/apps/screens.c
+++ b/apps/screens.c
@@ -91,6 +91,42 @@ static int clamp_value_wrap(int value, int max, int min)
 }
 #endif
 
+int handle_usb_events(struct event_queue *q)
+{
+    struct queue_event ev;
+    int next_update=0;
+
+    /* Don't return until we get SYS_USB_DISCONNECTED or SYS_TIMEOUT */
+    while(1)
+    {
+        queue_wait_w_tmo(q, &ev, HZ/4);
+        switch(ev.id)
+        {
+            case SYS_USB_DISCONNECTED:
+                usb_acknowledge(SYS_USB_DISCONNECTED_ACK);
+                return 0;
+            case SYS_TIMEOUT:
+                break;
+        }
+#if defined(HAVE_USBSTACK) && defined(USE_ROCKBOX_USB)
+        if((button_status() & ~USBPOWER_BTN_IGNORE) == USBPOWER_BUTTON)
+        {
+            usb_storage_try_release_storage();
+        }
+#endif
+        if(TIME_AFTER(current_tick,next_update))
+        {
+            if(usb_inserted()) {
+#if (CONFIG_STORAGE & STORAGE_MMC) /* USB-MMC bridge can report activity */
+                led(mmc_usb_active(HZ));
+#endif /* STORAGE_MMC */
+                gui_syncstatusbar_draw(&statusbars, false);
+            }
+            next_update=current_tick+HZ/2;
+        }
+    }
+}
+
 void usb_screen(void)
 {
 #ifdef USB_NONE
@@ -142,14 +178,7 @@ void usb_screen(void)
     while (button_get(true) & BUTTON_REL);
 #else
     usb_acknowledge(SYS_USB_CONNECTED_ACK);
-    while(usb_wait_for_disconnect_w_tmo(&button_queue, HZ)) {
-        if(usb_inserted()) {
-#if (CONFIG_STORAGE & STORAGE_MMC) /* USB-MMC bridge can report activity */
-            led(mmc_usb_active(HZ));
-#endif /* STORAGE_MMC */
-            gui_syncstatusbar_draw(&statusbars, false);
-        }
-    }
+    while(handle_usb_events(&button_queue));
 #endif /* SIMULATOR */
 #ifdef HAVE_LCD_CHARCELLS
     status_set_usb(false);
diff --git a/firmware/export/usb.h b/firmware/export/usb.h
index ecbec3a914..be36ee0b31 100644
--- a/firmware/export/usb.h
+++ b/firmware/export/usb.h
@@ -131,6 +131,7 @@ bool usb_charging_enabled(void);
 void usb_signal_transfer_completion(struct usb_transfer_completion_event_data* event_data);
 bool usb_driver_enabled(int driver);
 bool usb_exclusive_storage(void); /* storage is available for usb */
+void usb_storage_try_release_storage(void);
 #endif
 int usb_release_exclusive_storage(void);
 
diff --git a/firmware/usbstack/usb_storage.c b/firmware/usbstack/usb_storage.c
index 9b0625e4e2..aa8cb29da3 100644
--- a/firmware/usbstack/usb_storage.c
+++ b/firmware/usbstack/usb_storage.c
@@ -260,6 +260,7 @@ static void receive_block_data(void *data,int size);
 static void fill_inquiry(IF_MV_NONVOID(int lun));
 static void send_and_read_next(void);
 static bool ejected[NUM_VOLUMES];
+static bool locked[NUM_VOLUMES];
 
 static int usb_interface;
 static int ep_in, ep_out;
@@ -304,14 +305,14 @@ static bool check_disk_present(IF_MV_NONVOID(int volume))
 #endif
 }
 
-static void try_release_ata(void)
+void usb_storage_try_release_storage(void)
 {
     /* Check if there is a connected drive left. If not, 
        release excusive access */
     bool canrelease=true;
     int i;
     for(i=0;i<NUM_VOLUMES;i++) {
-        if(ejected[i]==false){
+        if(ejected[i]==false && locked[i]==true){
             canrelease=false;
             break;
         }
@@ -331,7 +332,9 @@ void usb_storage_notify_hotswap(int volume,bool inserted)
     }
     else {
         ejected[volume] = true;
-        try_release_ata();
+        /* If this happens while the device is locked, weird things may happen.
+           At least try to keep our state consistent */
+        locked[volume]=false;
     }
 }
 #endif
@@ -419,6 +422,14 @@ void usb_storage_init_connection(void)
 
     int i;
     for(i=0;i<NUM_VOLUMES;i++) {
+#ifdef TOSHIBA_GIGABEAT_S
+        /* As long as the Gigabeat S is a non-removable device, we need
+           to mark the device as locked to avoid usb_storage_try_release_ata()
+           to leave MSC mode while the device is in use */
+        locked[i] = true;
+#else
+        locked[i] = false;
+#endif
         ejected[i] = !check_disk_present(IF_MV(i));
         queue_broadcast(SYS_USB_LUN_LOCKED, (i<<16)+0);
     }
@@ -685,7 +696,6 @@ static void handle_scsi(struct command_block_wrapper* cbw)
 #ifdef HAVE_HOTSWAP
     if(storage_removable(lun) && !storage_present(lun)) {
         ejected[lun] = true;
-        try_release_ata();
     }
 #endif
 
@@ -889,7 +899,6 @@ static void handle_scsi(struct command_block_wrapper* cbw)
                     {
                         logf("scsi eject");
                         ejected[lun]=true;
-                        try_release_ata();
                     }
                 }
             }
@@ -900,10 +909,12 @@ static void handle_scsi(struct command_block_wrapper* cbw)
             logf("scsi allow_medium_removal %d",lun);
             if((cbw->command_block[4] & 0x03) == 0) 
             {
+                locked[lun]=false;
                 queue_broadcast(SYS_USB_LUN_LOCKED, (lun<<16)+0);
             }
             else
             {
+                locked[lun]=true;
                 queue_broadcast(SYS_USB_LUN_LOCKED, (lun<<16)+1);
             }
             send_csw(UMS_STATUS_GOOD);