Bring Gigabeat S bootloader one step close to a release version.

git-svn-id: svn://svn.rockbox.org/rockbox/trunk@17442 a1c6a512-1295-4272-9138-f99709370657

17 files changed, 214 insertions, 33 deletions

diff --git a/firmware/backlight.c b/firmware/backlight.c
index 4a00f86f39..74cdee1205 100644
--- a/firmware/backlight.c
+++ b/firmware/backlight.c
@@ -39,6 +39,11 @@
 #include "backlight-target.h"
 #endif
 
+#if !defined(BOOTLOADER)
+/* The whole driver should be built */
+#define BACKLIGHT_FULL_INIT
+#endif
+
 #ifdef SIMULATOR
 /* TODO: find a better way to do it but we need a kernel thread somewhere to
    handle this */
@@ -85,7 +90,7 @@ static inline void _remote_backlight_off(void)
 
 #endif /* SIMULATOR */
 
-#if defined(HAVE_BACKLIGHT) && !defined(BOOTLOADER)
+#if defined(HAVE_BACKLIGHT) && defined(BACKLIGHT_FULL_INIT)
 
 enum {
     BACKLIGHT_ON,
@@ -104,12 +109,18 @@ enum {
     BUTTON_LIGHT_ON,
     BUTTON_LIGHT_OFF,
 #endif
+#ifdef BACKLIGHT_DRIVER_CLOSE
+    BACKLIGHT_QUIT,
+#endif
 };
 
 static void backlight_thread(void);
 static long backlight_stack[DEFAULT_STACK_SIZE/sizeof(long)];
 static const char backlight_thread_name[] = "backlight";
 static struct event_queue backlight_queue;
+#ifdef BACKLIGHT_DRIVER_CLOSE
+static struct thread_entry *backlight_thread_p = NULL;
+#endif
 
 static int backlight_timer SHAREDBSS_ATTR;
 static int backlight_timeout SHAREDBSS_ATTR;
@@ -158,7 +169,7 @@ void buttonlight_set_timeout(int value)
     buttonlight_update_state();
 }
 
-#endif
+#endif /* HAVE_BUTTON_LIGHT */
 
 #ifdef HAVE_REMOTE_LCD
 static int remote_backlight_timer;
@@ -170,7 +181,7 @@ static int remote_backlight_timeout_plugged = 5*HZ;
 #ifdef HAS_REMOTE_BUTTON_HOLD
 static int remote_backlight_on_button_hold = 0;
 #endif
-#endif
+#endif /* HAVE_REMOTE_LCD */
 
 #ifdef HAVE_LCD_SLEEP
 const signed char lcd_sleep_timeout_value[10] =
@@ -494,6 +505,12 @@ void backlight_thread(void)
             case SYS_USB_DISCONNECTED:
                 usb_acknowledge(SYS_USB_DISCONNECTED_ACK);
                 break;
+
+#ifdef BACKLIGHT_DRIVER_CLOSE
+            /* Get out of here */
+            case BACKLIGHT_QUIT:
+                return;
+#endif
         }
         if (locked)
             continue;
@@ -613,7 +630,9 @@ void backlight_init(void)
     /* Leave all lights as set by the bootloader here. The settings load will
      * call the appropriate backlight_set_*() functions, only changing light
      * status if necessary. */
-
+#ifdef BACKLIGHT_DRIVER_CLOSE
+    backlight_thread_p =
+#endif
     create_thread(backlight_thread, backlight_stack,
                   sizeof(backlight_stack), 0, backlight_thread_name
                   IF_PRIO(, PRIORITY_USER_INTERFACE)
@@ -621,6 +640,22 @@ void backlight_init(void)
     tick_add_task(backlight_tick);
 }
 
+#ifdef BACKLIGHT_DRIVER_CLOSE
+void backlight_close(void)
+{
+    struct thread_entry *thread = backlight_thread_p;
+
+    /* Wait for thread to exit */
+    if (thread == NULL)
+        return;
+
+    backlight_thread_p = NULL;
+
+    queue_post(&backlight_queue, BACKLIGHT_QUIT, 0);
+    thread_wait(thread);
+}
+#endif /* BACKLIGHT_DRIVER_CLOSE */
+
 void backlight_on(void)
 {
     queue_remove_from_head(&backlight_queue, BACKLIGHT_ON);
@@ -788,10 +823,10 @@ void buttonlight_set_brightness(int val)
 }
 #endif /* HAVE_BUTTONLIGHT_BRIGHTNESS */
 
-#else /* !defined(HAVE_BACKLIGHT) || defined(BOOTLOADER)
+#else /* !defined(HAVE_BACKLIGHT) || !defined(BACKLIGHT_FULL_INIT)
     -- no backlight, empty dummy functions */
 
-#if defined(BOOTLOADER) && defined(HAVE_BACKLIGHT)
+#if defined(HAVE_BACKLIGHT) && !defined(BACKLIGHT_FULL_INIT)
 void backlight_init(void)
 {
     (void)_backlight_init();
@@ -826,4 +861,4 @@ void backlight_set_brightness(int val) { (void)val; }
 #ifdef HAVE_BUTTONLIGHT_BRIGHTNESS
 void buttonlight_set_brightness(int val) { (void)val; }
 #endif
-#endif /* defined(HAVE_BACKLIGHT) && !defined(BOOTLOADER) */
+#endif /* defined(HAVE_BACKLIGHT) && defined(BACKLIGHT_FULL_INIT) */
diff --git a/firmware/drivers/ata.c b/firmware/drivers/ata.c
index 2dca17512e..87dacc3ed0 100644
--- a/firmware/drivers/ata.c
+++ b/firmware/drivers/ata.c
@@ -59,6 +59,7 @@
 #define CMD_SECURITY_FREEZE_LOCK   0xF5
 
 #define Q_SLEEP 0
+#define Q_CLOSE 1
 
 #define READ_TIMEOUT 5*HZ
 
@@ -66,6 +67,10 @@
 #define ATA_POWER_OFF_TIMEOUT 2*HZ
 #endif
 
+#ifdef ATA_DRIVER_CLOSE
+static struct thread_entry *ata_thread_p = NULL;
+#endif
+
 #if defined(MAX_PHYS_SECTOR_SIZE) && MEM == 64
 /* Hack - what's the deal with 5g? */
 struct ata_lock
@@ -941,6 +946,11 @@ static void ata_thread(void)
                 call_ata_idle_notifys(false);
                 last_disk_activity = current_tick - sleep_timeout + (HZ/2);
                 break;
+
+#ifdef ATA_DRIVER_CLOSE
+            case Q_CLOSE:
+                return;
+#endif
         }
     }
 }
@@ -1307,6 +1317,9 @@ int ata_init(void)
         mutex_lock(&ata_mtx); /* Balance unlock below */
 
         last_disk_activity = current_tick;
+#ifdef ATA_DRIVER_CLOSE
+        ata_thread_p =
+#endif
         create_thread(ata_thread, ata_stack,
                       sizeof(ata_stack), 0, ata_thread_name
                       IF_PRIO(, PRIORITY_USER_INTERFACE)
@@ -1322,6 +1335,21 @@ int ata_init(void)
     return rc;
 }
 
+#ifdef ATA_DRIVER_CLOSE
+void ata_close(void)
+{
+    struct thread_entry *thread = ata_thread_p;
+    
+    if (thread == NULL)
+        return;
+
+    ata_thread_p = NULL;
+
+    queue_post(&ata_queue, Q_CLOSE, 0);
+    thread_wait(thread);
+}
+#endif /* ATA_DRIVER_CLOSE */
+
 #if (CONFIG_LED == LED_REAL)
 void ata_set_led_enabled(bool enabled) 
 {
diff --git a/firmware/drivers/button.c b/firmware/drivers/button.c
index 90ff800c88..74c5a97167 100644
--- a/firmware/drivers/button.c
+++ b/firmware/drivers/button.c
@@ -410,6 +410,13 @@ void button_init(void)
     tick_add_task(button_tick);
 }
 
+#ifdef BUTTON_DRIVER_CLOSE
+void button_close(void)
+{
+    tick_remove_task(button_tick);
+}
+#endif /* BUTTON_DRIVER_CLOSE */
+
 #ifdef HAVE_LCD_BITMAP /* only bitmap displays can be flipped */
 /*
  * helper function to swap LEFT/RIGHT, UP/DOWN (if present), and F1/F3 (Recorder)
diff --git a/firmware/export/ata.h b/firmware/export/ata.h
index 35538f6354..9b5bd36a5b 100644
--- a/firmware/export/ata.h
+++ b/firmware/export/ata.h
@@ -48,6 +48,7 @@ extern bool ata_disk_is_active(void);
 extern int ata_hard_reset(void);
 extern int ata_soft_reset(void);
 extern int ata_init(void);
+extern void ata_close(void);
 extern int ata_read_sectors(IF_MV2(int drive,) unsigned long start, int count, void* buf);
 extern int ata_write_sectors(IF_MV2(int drive,) unsigned long start, int count, const void* buf);
 extern void ata_spin(void);
diff --git a/firmware/export/backlight.h b/firmware/export/backlight.h
index 35c0f86178..895328d98e 100644
--- a/firmware/export/backlight.h
+++ b/firmware/export/backlight.h
@@ -28,6 +28,7 @@ void backlight_set_timeout(int value);
 
 #ifdef HAVE_BACKLIGHT
 void backlight_init(void);
+void backlight_close(void);
 
 int  backlight_get_current_timeout(void);
 
diff --git a/firmware/export/button.h b/firmware/export/button.h
index 6decf6ec69..82ea81ffab 100644
--- a/firmware/export/button.h
+++ b/firmware/export/button.h
@@ -29,6 +29,7 @@
 extern struct event_queue button_queue;
 
 void button_init (void);
+void button_close(void);
 int button_queue_count(void);
 long button_get (bool block);
 long button_get_w_tmo(int ticks);
diff --git a/firmware/export/config-gigabeat-s.h b/firmware/export/config-gigabeat-s.h
index d74f05ce9c..a0c05dad45 100644
--- a/firmware/export/config-gigabeat-s.h
+++ b/firmware/export/config-gigabeat-s.h
@@ -54,15 +54,6 @@
 /* Define this for LCD backlight available */
 #define HAVE_BACKLIGHT
 
-#define HAVE_LCD_ENABLE
-
-#define HAVE_BACKLIGHT_BRIGHTNESS
-
-/* Main LCD backlight brightness range and defaults */
-#define MIN_BRIGHTNESS_SETTING          0
-#define MAX_BRIGHTNESS_SETTING          24
-#define DEFAULT_BRIGHTNESS_SETTING      12
-
 /* Define this if you have a software controlled poweroff */
 #define HAVE_SW_POWEROFF
 
@@ -78,7 +69,20 @@
 #define HW_SAMPR_CAPS (SAMPR_CAP_88 | SAMPR_CAP_44 | SAMPR_CAP_22 | \
                        SAMPR_CAP_11)
 
+#ifndef BOOTLOADER
+/* Not for bootloader */
+#define HAVE_LCD_ENABLE
+
+#define HAVE_BACKLIGHT_BRIGHTNESS
+
+/* Main LCD backlight brightness range and defaults */
+#define MIN_BRIGHTNESS_SETTING          0
+#define MAX_BRIGHTNESS_SETTING          24
+#define DEFAULT_BRIGHTNESS_SETTING      12
+
+
 #define HAVE_HEADPHONE_DETECTION
+#endif /* BOOTLOADER */
 
 #ifndef SIMULATOR
 
diff --git a/firmware/export/usb.h b/firmware/export/usb.h
index 4500cb2cde..ff1f55cd35 100644
--- a/firmware/export/usb.h
+++ b/firmware/export/usb.h
@@ -31,7 +31,8 @@ enum {
     USB_TRANSFER_COMPLETION,
     USB_REQUEST_DISK,
     USB_RELEASE_DISK,
-    USB_REQUEST_REBOOT
+    USB_REQUEST_REBOOT,
+    USB_QUIT,
 };
 
 
@@ -89,7 +90,7 @@ struct usb_transfer_completion_event_data
 void usb_init(void);
 void usb_enable(bool on);
 void usb_start_monitoring(void);
-void usb_stop_monitoring(void);
+void usb_close(void);
 void usb_acknowledge(long id);
 void usb_wait_for_disconnect(struct event_queue *q);
 int usb_wait_for_disconnect_w_tmo(struct event_queue *q, int ticks);
diff --git a/firmware/target/arm/imx31/gigabeat-s/ata-target.h b/firmware/target/arm/imx31/gigabeat-s/ata-target.h
index 6428e9f41f..8f8083dc8d 100644
--- a/firmware/target/arm/imx31/gigabeat-s/ata-target.h
+++ b/firmware/target/arm/imx31/gigabeat-s/ata-target.h
@@ -19,6 +19,10 @@
 #ifndef ATA_TARGET_H
 #define ATA_TARGET_H
 
+#ifdef BOOTLOADER
+#define ATA_DRIVER_CLOSE
+#endif
+
 /* Plain C read & write loops */
 #define PREFER_C_READING
 #define PREFER_C_WRITING
diff --git a/firmware/target/arm/imx31/gigabeat-s/backlight-imx31.c b/firmware/target/arm/imx31/gigabeat-s/backlight-imx31.c
index 4df9be843c..b35e3c1ad0 100644
--- a/firmware/target/arm/imx31/gigabeat-s/backlight-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/backlight-imx31.c
@@ -22,6 +22,7 @@
 #include "mc13783.h"
 #include "backlight-target.h"
 
+#ifdef HAVE_BACKLIGHT_BRIGHTNESS
 /* Table that uses combinations of current level and pwm fraction to get
  * as many uniquely-visible brightness levels as possible. The lowest current
  * level for any average current is used even though many combinations give
@@ -60,6 +61,7 @@ static const struct
     { 3, 12 }, /*    9       12      7.2  */ 
     /* Anything higher is just too much   */
 };
+#endif /* HAVE_BACKLIGHT_BRIGHTNESS */
 
 bool _backlight_init(void)
 {
@@ -85,6 +87,7 @@ void _backlight_off(void)
     mc13783_clear(MC13783_LED_CONTROL0, MC13783_LEDEN);
 }
 
+#ifdef HAVE_BACKLIGHT_BRIGHTNESS
 /* Assumes that the backlight has been initialized */
 void _backlight_set_brightness(int brightness)
 {
@@ -106,3 +109,4 @@ void _backlight_set_brightness(int brightness)
 
     mc13783_write(MC13783_LED_CONTROL2, data);
 }
+#endif /* HAVE_BACKLIGHT_BRIGHTNESS */
diff --git a/firmware/target/arm/imx31/gigabeat-s/backlight-target.h b/firmware/target/arm/imx31/gigabeat-s/backlight-target.h
index 7c4b2fa0fd..145df0d930 100644
--- a/firmware/target/arm/imx31/gigabeat-s/backlight-target.h
+++ b/firmware/target/arm/imx31/gigabeat-s/backlight-target.h
@@ -19,11 +19,15 @@
 #ifndef BACKLIGHT_TARGET_H
 #define BACKLIGHT_TARGET_H
 
+#ifdef BOOTLOADER
+#define BACKLIGHT_DRIVER_CLOSE
+/* Force the whole driver to be built */
+#define BACKLIGHT_FULL_INIT
+#endif
+
 bool _backlight_init(void);
 void _backlight_on(void);
 void _backlight_off(void);
 void _backlight_set_brightness(int brightness);
 
-/* true: backlight fades off - false: backlight fades on */
-void __backlight_dim(bool dim);
 #endif
diff --git a/firmware/target/arm/imx31/gigabeat-s/button-imx31.c b/firmware/target/arm/imx31/gigabeat-s/button-imx31.c
index e80166bca7..746883d010 100644
--- a/firmware/target/arm/imx31/gigabeat-s/button-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/button-imx31.c
@@ -29,11 +29,16 @@
 
 /* Most code in here is taken from the Linux BSP provided by Freescale
  * Copyright 2004-2006 Freescale Semiconductor, Inc. All Rights Reserved. */
-
+#ifdef HAVE_HEADPHONE_DETECTION
 static bool headphones_detect = false;
+#endif
 static uint32_t int_btn     = BUTTON_NONE;
 static bool hold_button     = false;
+#ifdef BOOTLOADER
+static bool initialized     = false;
+#else
 static bool hold_button_old = false;
+#endif
 #define _button_hold() (GPIO3_DR & 0x10)
 
 static __attribute__((interrupt("IRQ"))) void KPP_HANDLER(void)
@@ -116,6 +121,14 @@ static __attribute__((interrupt("IRQ"))) void KPP_HANDLER(void)
 
 void button_init_device(void)
 {
+#ifdef BOOTLOADER
+    /* Can be called more than once in the bootloader */
+    if (initialized)
+        return;
+
+    initialized = true;
+#endif
+
     /* Enable keypad clock */
     imx31_clkctl_module_clock_gating(CG_KPP, CGM_ON_ALL);
 
@@ -136,15 +149,26 @@ void button_init_device(void)
     KPP_KDDR = (KPP_KDDR | (0x7 << 8)) & ~0x1f;
 
     /* 5. Clear the KPKD Status Flag and Synchronizer chain.
-     * 6. Set the KDIE control bit, and set the KRIE control
-     *    bit (to force immediate scan). */
-    KPP_KPSR = KPP_KPSR_KRIE | KPP_KPSR_KDIE | KPP_KPSR_KRSS |
-               KPP_KPSR_KDSC | KPP_KPSR_KPKR | KPP_KPSR_KPKD;
+     * 6. Set the KDIE control bit bit. */
+    KPP_KPSR = KPP_KPSR_KDIE | KPP_KPSR_KRSS | KPP_KPSR_KDSC | KPP_KPSR_KPKD;
 
     /* KPP IRQ at priority 3 */
     avic_enable_int(KPP, IRQ, 3, KPP_HANDLER);
 }
 
+#ifdef BUTTON_DRIVER_CLOSE
+void button_close_device(void)
+{
+    int oldlevel = disable_irq_save();
+
+    avic_disable_int(KPP);
+    KPP_KPSR &= ~(KPP_KPSR_KRIE | KPP_KPSR_KDIE);
+    int_btn = BUTTON_NONE;
+
+    restore_irq(oldlevel);
+}
+#endif /* BUTTON_DRIVER_CLOSE */
+
 bool button_hold(void)
 {
     return _button_hold();
@@ -155,12 +179,14 @@ int button_read_device(void)
     /* Simple poll of GPIO status */
     hold_button = _button_hold();
 
+#ifndef BOOTLOADER
     /* Backlight hold handling */
     if (hold_button != hold_button_old)
     {
         hold_button_old = hold_button;
         backlight_hold_changed(hold_button);
     }
+#endif
 
     /* Enable the keypad interrupt to cause it to fire if a key is down.
      * KPP_HANDLER will clear and disable it after the scan. If no key
@@ -190,6 +216,7 @@ void button_power_set_state(bool pressed)
     restore_irq(oldlevel);
 }
 
+#ifdef HAVE_HEADPHONE_DETECTION
 /* This is called from the mc13783 interrupt thread */
 void set_headphones_inserted(bool inserted)
 {
@@ -203,3 +230,4 @@ bool headphones_inserted(void)
 {
     return headphones_detect;
 }
+#endif /* HAVE_HEADPHONE_DETECTION */
diff --git a/firmware/target/arm/imx31/gigabeat-s/button-target.h b/firmware/target/arm/imx31/gigabeat-s/button-target.h
index e2f68162f7..61d33f8e70 100644
--- a/firmware/target/arm/imx31/gigabeat-s/button-target.h
+++ b/firmware/target/arm/imx31/gigabeat-s/button-target.h
@@ -24,8 +24,13 @@
 
 #define HAS_BUTTON_HOLD
 
+#ifdef BOOTLOADER
+#define BUTTON_DRIVER_CLOSE
+#endif
+
 bool button_hold(void);
 void button_init_device(void);
+void button_close_device(void);
 int button_read_device(void);
 void button_power_set_state(bool pressed);
 void set_headphones_inserted(bool inserted);
@@ -48,6 +53,8 @@ bool headphones_inserted(void);
 #define BUTTON_NEXT         (1 << 11)
 #define BUTTON_POWER        (1 << 12) /* Read from PMIC */
 
+#define BUTTON_MAIN         (0x1fff)
+
 #define BUTTON_REMOTE       0
 
 #define POWEROFF_BUTTON     BUTTON_POWER
diff --git a/firmware/target/arm/imx31/gigabeat-s/mc13783-imx31.c b/firmware/target/arm/imx31/gigabeat-s/mc13783-imx31.c
index 3af4b48b66..ddf8d1360f 100644
--- a/firmware/target/arm/imx31/gigabeat-s/mc13783-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/mc13783-imx31.c
@@ -29,6 +29,10 @@
 #include "adc-target.h"
 #include "usb-target.h"
 
+#ifdef BOOTLOADER
+#define PMIC_DRIVER_CLOSE
+#endif
+
 /* This is all based on communicating with the MC13783 PMU which is on 
  * CSPI2 with the chip select at 0. The LCD controller resides on
  * CSPI3 cs1, but we have no idea how to communicate to it */
@@ -48,10 +52,14 @@ static struct spi_node mc13783_spi =
 static int mc13783_thread_stack[DEFAULT_STACK_SIZE/sizeof(int)];
 static const char *mc13783_thread_name = "pmic";
 static struct wakeup mc13783_wake;
+#ifdef PMIC_DRIVER_CLOSE
+static bool pmic_close = false;
+static struct thread_entry *mc13783_thread_p = NULL;
+#endif
 
 /* The next two functions are rather target-specific but they'll just be left
  * here for the moment */
-static __attribute__((noreturn)) void mc13783_interrupt_thread(void)
+static void mc13783_interrupt_thread(void)
 {
     const unsigned char status_regs[2] =
     {
@@ -76,7 +84,9 @@ static __attribute__((noreturn)) void mc13783_interrupt_thread(void)
 
     value = mc13783_read(MC13783_INTERRUPT_SENSE1);
     button_power_set_state((value & MC13783_ONOFD1) == 0);
+#ifdef HAVE_HEADPHONE_DETECTION
     set_headphones_inserted((value & MC13783_ONOFD2) == 0);
+#endif
 
     pending[0] = pending[1] = 0xffffff;
     mc13783_write_regset(status_regs, pending, 2);
@@ -90,6 +100,14 @@ static __attribute__((noreturn)) void mc13783_interrupt_thread(void)
     {
         wakeup_wait(&mc13783_wake, TIMEOUT_BLOCK);
 
+#ifdef PMIC_DRIVER_CLOSE
+        if (pmic_close)
+        {
+            gpio_disable_event(MC13783_GPIO_NUM, MC13783_EVENT_ID);
+            return;
+        }
+#endif
+
         mc13783_read_regset(status_regs, pending, 2);
         mc13783_write_regset(status_regs, pending, 2);
 
@@ -130,9 +148,10 @@ static __attribute__((noreturn)) void mc13783_interrupt_thread(void)
 
                 if (pending[1] & MC13783_ONOFD1)
                     button_power_set_state((value & MC13783_ONOFD1) == 0);
-
+#ifdef HAVE_HEADPHONE_DETECTION
                 if (pending[1] & MC13783_ONOFD2)
                     set_headphones_inserted((value & MC13783_ONOFD2) == 0);
+#endif
             }
         }
     }
@@ -161,10 +180,29 @@ void mc13783_init(void)
 
     MC13783_GPIO_ISR = (1ul << MC13783_GPIO_LINE);
 
-    create_thread(mc13783_interrupt_thread, mc13783_thread_stack,
-                  sizeof(mc13783_thread_stack), 0, mc13783_thread_name
-                  IF_PRIO(, PRIORITY_REALTIME) IF_COP(, CPU));
+#ifdef PMIC_DRIVER_CLOSE
+    mc13783_thread_p =
+#endif
+        create_thread(mc13783_interrupt_thread,
+            mc13783_thread_stack, sizeof(mc13783_thread_stack), 0,
+            mc13783_thread_name IF_PRIO(, PRIORITY_REALTIME) IF_COP(, CPU));
+}
+
+#ifdef PMIC_DRIVER_CLOSE
+void mc13783_close(void)
+{
+    struct thread_entry *thread = mc13783_thread_p;
+
+    if (thread == NULL)
+        return;
+
+    mc13783_thread_p = NULL;
+
+    pmic_close = true;
+    wakeup_signal(&mc13783_wake);
+    thread_wait(thread);
 }
+#endif
 
 uint32_t mc13783_set(unsigned address, uint32_t bits)
 {
diff --git a/firmware/target/arm/imx31/gigabeat-s/system-imx31.c b/firmware/target/arm/imx31/gigabeat-s/system-imx31.c
index 3ec46bae67..1c3abc64fc 100644
--- a/firmware/target/arm/imx31/gigabeat-s/system-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/system-imx31.c
@@ -117,6 +117,7 @@ void system_prepare_fw_start(void)
 {
     disable_interrupt(IRQ_FIQ_STATUS);
     avic_disable_int(ALL);
+    mc13783_close();
     tick_stop();
 }
 #endif
diff --git a/firmware/target/arm/imx31/gigabeat-s/usb-target.h b/firmware/target/arm/imx31/gigabeat-s/usb-target.h
index 8c9dcfc65f..7ecd9a7d49 100644
--- a/firmware/target/arm/imx31/gigabeat-s/usb-target.h
+++ b/firmware/target/arm/imx31/gigabeat-s/usb-target.h
@@ -19,6 +19,10 @@
 #ifndef USB_TARGET_H
 #define USB_TARGET_H
 
+#ifdef BOOTLOADER
+#define USB_DRIVER_CLOSE
+#endif
+
 void usb_set_status(bool plugged);
 bool usb_init_device(void);
 int usb_detect(void);
diff --git a/firmware/usb.c b/firmware/usb.c
index 3867bfb496..90991e25fd 100644
--- a/firmware/usb.c
+++ b/firmware/usb.c
@@ -172,6 +172,10 @@ static void usb_thread(void)
         queue_wait(&usb_queue, &ev);
         switch(ev.id)
         {
+#ifdef USB_DRIVER_CLOSE
+            case USB_QUIT:
+                return;
+#endif
 #ifdef HAVE_USBSTACK
             case USB_TRANSFER_COMPLETION:
                 usb_core_handle_transfer_completion((struct usb_transfer_completion_event_data*)ev.data);
@@ -531,13 +535,22 @@ void usb_start_monitoring(void)
     usb_monitor_enabled = true;
 }
 
-#ifdef TOSHIBA_GIGABEAT_S
-void usb_stop_monitoring(void)
+#ifdef USB_DRIVER_CLOSE
+void usb_close(void)
 {
+    struct thread_entry *thread = usb_thread_entry;
+    usb_thread_entry = NULL;
+
+    if (thread == NULL)
+        return;
+
     tick_remove_task(usb_tick);
     usb_monitor_enabled = false;
+
+    queue_post(&usb_queue, USB_QUIT, 0);
+    thread_wait(thread);
 }
-#endif
+#endif /* USB_DRIVER_CLOSE */
 
 bool usb_inserted(void)
 {