1 files changed, 299 insertions, 84 deletions

diff --git a/firmware/storage.c b/firmware/storage.c
index e9a3396f30..790cddcd1a 100644
--- a/firmware/storage.c
+++ b/firmware/storage.c
@@ -20,6 +20,9 @@
  ****************************************************************************/
 #include "storage.h"
 #include "kernel.h"
+#include "ata_idle_notify.h"
+#include "usb.h"
+#include "disk.h"
 
 #ifdef CONFIG_STORAGE_MULTI
 
@@ -31,118 +34,243 @@
 
 static unsigned int storage_drivers[NUM_DRIVES];
 static unsigned int num_drives;
+#endif /* CONFIG_STORAGE_MULTI */
+
+/* defaults: override elsewhere target-wise if they must be different */
+#if (CONFIG_STORAGE & STORAGE_ATA)
+ #ifndef ATA_THREAD_STACK_SIZE
+  #define ATA_THREAD_STACK_SIZE     (DEFAULT_STACK_SIZE*2)
+ #endif
+#endif
+#if (CONFIG_STORAGE & STORAGE_MMC)
+ #ifndef MMC_THREAD_STACK_SIZE
+  #define MMC_THREAD_STACK_SIZE     (DEFAULT_STACK_SIZE*2)
+ #endif
+#endif
+#if (CONFIG_STORAGE & STORAGE_SD)
+ #ifndef SD_THREAD_STACK_SIZE
+  #define SD_THREAD_STACK_SIZE      (DEFAULT_STACK_SIZE*2)
+ #endif
+#endif
+#if (CONFIG_STORAGE & STORAGE_NAND)
+ #ifndef NAND_THREAD_STACK_SIZE
+  #define NAND_THREAD_STACK_SIZE    (DEFAULT_STACK_SIZE*2)
+ #endif
+#endif
+#if (CONFIG_STORAGE & STORAGE_RAMDISK)
+ #ifndef RAMDISK_THREAD_STACK_SIZE
+  #define RAMDISK_THREAD_STACK_SIZE (0) /* not used on its own */
+ #endif
 #endif
 
-int storage_read_sectors(IF_MD(int drive,) unsigned long start, int count,
-                         void* buf)
-{
-#ifdef CONFIG_STORAGE_MULTI
-    int driver=(storage_drivers[drive] & DRIVER_MASK)>>DRIVER_OFFSET;
-    int ldrive=(storage_drivers[drive] & DRIVE_MASK)>>DRIVE_OFFSET;
+static struct event_queue storage_queue SHAREDBSS_ATTR;
+static unsigned int storage_thread_id = 0;
 
-    switch (driver)
-    {
+static union {
 #if (CONFIG_STORAGE & STORAGE_ATA)
-    case STORAGE_ATA:
-        return ata_read_sectors(IF_MD(ldrive,) start,count,buf);
+    long stk_ata[ATA_THREAD_STACK_SIZE / sizeof (long)];
 #endif
-
 #if (CONFIG_STORAGE & STORAGE_MMC)
-    case STORAGE_MMC:
-        return mmc_read_sectors(IF_MD(ldrive,) start,count,buf);
+    long stk_mmc[MMC_THREAD_STACK_SIZE / sizeof (long)];
 #endif
-
 #if (CONFIG_STORAGE & STORAGE_SD)
-    case STORAGE_SD:
-        return sd_read_sectors(IF_MD(ldrive,) start,count,buf);
+    long stk_sd[SD_THREAD_STACK_SIZE / sizeof (long)];
 #endif
-
 #if (CONFIG_STORAGE & STORAGE_NAND)
-    case STORAGE_NAND:
-        return nand_read_sectors(IF_MD(ldrive,) start,count,buf);
+    long stk_nand[NAND_THREAD_STACK_SIZE / sizeof (long)];
+#endif
+#if (CONFIG_STORAGE & STORAGE_RAMDISK)
+    long stk_ramdisk[RAMDISK_THREAD_STACK_SIZE / sizeof (long)];
 #endif
+} storage_thread_stack;
 
+static const char storage_thread_name[] =
+#if (CONFIG_STORAGE & STORAGE_ATA)
+    "/ata"
+#endif
+#if (CONFIG_STORAGE & STORAGE_MMC)
+    "/mmc"
+#endif
+#if (CONFIG_STORAGE & STORAGE_SD)
+    "/sd"
+#endif
+#if (CONFIG_STORAGE & STORAGE_NAND)
+    "/nand"
+#endif
 #if (CONFIG_STORAGE & STORAGE_RAMDISK)
-    case STORAGE_RAMDISK:
-        return ramdisk_read_sectors(IF_MD(ldrive,) start,count,buf);
+    "/ramdisk"
 #endif
-    }
-    
-    return -1;
-#else /* CONFIG_STORAGE_MULTI */
-    return STORAGE_FUNCTION(read_sectors)(IF_MD(drive,)start,count,buf);
-#endif /* CONFIG_STORAGE_MULTI */
+    ;
 
-}
+/* event is targeted to a specific drive */
+#define DRIVE_EVT  (1 << STORAGE_NUM_TYPES)
 
-int storage_write_sectors(IF_MD(int drive,) unsigned long start, int count,
-                          const void* buf)
-{
 #ifdef CONFIG_STORAGE_MULTI
-    int driver=(storage_drivers[drive] & DRIVER_MASK)>>DRIVER_OFFSET;
-    int ldrive=(storage_drivers[drive] & DRIVE_MASK)>>DRIVE_OFFSET;
-    
-    switch (driver)
-    {
+static int storage_event_send(unsigned int route, long id, intptr_t data)
+{
+    /* most events go to everyone */
+    if (UNLIKELY(route == DRIVE_EVT)) {
+        route = (storage_drivers[data] & DRIVER_MASK) >> DRIVER_OFFSET;
+        data  = (storage_drivers[data] & DRIVE_MASK) >> DRIVE_OFFSET;
+    }
+
+    int rc = 0;
+
 #if (CONFIG_STORAGE & STORAGE_ATA)
-    case STORAGE_ATA:
-        return ata_write_sectors(IF_MD(ldrive,)start,count,buf);
+    if (route & STORAGE_ATA) {
+        rc = ata_event(id, data);
+    }
 #endif
-
 #if (CONFIG_STORAGE & STORAGE_MMC)
-    case STORAGE_MMC:
-        return mmc_write_sectors(IF_MD(ldrive,)start,count,buf);
+    if (route & STORAGE_MMC) {
+        rc = mmc_event(id, data);
+    }
 #endif
-
 #if (CONFIG_STORAGE & STORAGE_SD)
-    case STORAGE_SD:
-        return sd_write_sectors(IF_MD(ldrive,)start,count,buf);
+    if (route & STORAGE_SD) {
+        rc = sd_event(id, data);
+    }
 #endif
-
 #if (CONFIG_STORAGE & STORAGE_NAND)
-    case STORAGE_NAND:
-        return nand_write_sectors(IF_MD(ldrive,)start,count,buf);
+    if (route & STORAGE_NAND) {
+        rc = nand_event(id, data);
+    }
 #endif
-
 #if (CONFIG_STORAGE & STORAGE_RAMDISK)
-    case STORAGE_RAMDISK:
-        return ramdisk_write_sectors(IF_MD(ldrive,)start,count,buf);
-#endif
+    if (route & STORAGE_RAMDISK) {
+        rc = ramdisk_event(id, data);
     }
-    
-    return -1;
-#else /* CONFIG_STORAGE_MULTI */
-    return STORAGE_FUNCTION(write_sectors)(IF_MD(drive,)start,count,buf);
+#endif
+
+    return rc;
+}
 #endif /* CONFIG_STORAGE_MULTI */
+
+#ifndef CONFIG_STORAGE_MULTI
+static FORCE_INLINE int storage_event_send(unsigned int route, long id,
+                                           intptr_t data)
+{
+    return route ? STORAGE_FUNCTION(event)(id, data) : 0;
 }
+#endif /* ndef CONFIG_STORAGE_MULTI */
 
-#ifdef CONFIG_STORAGE_MULTI
+static void NORETURN_ATTR storage_thread(void)
+{
+    unsigned int bdcast = CONFIG_STORAGE;
+    bool usb_mode = false;
+    struct queue_event ev;
 
-#define DRIVER_MASK     0xff000000
-#define DRIVER_OFFSET   24
-#define DRIVE_MASK      0x00ff0000
-#define DRIVE_OFFSET    16
-#define PARTITION_MASK  0x0000ff00
+    while (1)
+    {
+        queue_wait_w_tmo(&storage_queue, &ev, HZ/2);
+
+        switch (ev.id)
+        {
+        case SYS_TIMEOUT:;
+            /* drivers hold their bit low when they want to
+               sleep and keep it high otherwise */
+            unsigned int trig = 0;
+            storage_event_send(bdcast, Q_STORAGE_TICK, (intptr_t)&trig);
+            trig = bdcast & ~trig;
+            if (trig) {
+                if (!usb_mode) {
+                    call_storage_idle_notifys(false);
+                }
+                storage_event_send(trig, Q_STORAGE_SLEEPNOW, 0);
+            }
+            break;
 
-static unsigned int storage_drivers[NUM_DRIVES];
-static unsigned int num_drives;
+#if (CONFIG_STORAGE & STORAGE_ATA)
+        case Q_STORAGE_SLEEP:
+            storage_event_send(bdcast, ev.id, 0);
+            break;
+#endif
 
-int storage_num_drives(void)
+#ifdef STORAGE_CLOSE
+        case Q_STORAGE_CLOSE:
+            storage_event_send(CONFIG_STORAGE, ev.id, 0);
+            thread_exit();
+#endif /* STORAGE_CLOSE */
+
+#ifdef HAVE_HOTSWAP
+        case SYS_HOTSWAP_INSERTED:
+        case SYS_HOTSWAP_EXTRACTED:
+            if (!usb_mode) {
+                int drive = IF_MD_DRV(ev.data);
+                if (!CHECK_DRV(drive)) {
+                    break;
+                }
+
+                int umnt = disk_unmount(drive);
+                int mnt = 0;
+                int rci = storage_event_send(DRIVE_EVT, ev.id, drive);
+
+                if (ev.id == SYS_HOTSWAP_INSERTED && !rci) {
+                    mnt = disk_mount(drive);
+                }
+
+                if (umnt > 0 || mnt > 0) {
+                    /* something was unmounted and/or mounted */
+                    queue_broadcast(SYS_FS_CHANGED, drive);
+                }
+            }
+            break;
+#endif /* HAVE_HOTSWAP */
+
+#ifndef USB_NONE
+        case SYS_USB_CONNECTED:
+        case SYS_USB_DISCONNECTED:
+            bdcast = 0;
+            storage_event_send(CONFIG_STORAGE, ev.id, (intptr_t)&bdcast);
+            usb_mode = ev.id == SYS_USB_CONNECTED;
+            if (usb_mode) {
+                usb_acknowledge(SYS_USB_CONNECTED_ACK);
+            }
+            else {
+                bdcast = CONFIG_STORAGE;
+            }
+            break;
+#endif /* ndef USB_NONE */
+        }
+    }
+}
+
+#if (CONFIG_STORAGE & STORAGE_ATA)
+void storage_sleep(void)
 {
-    return num_drives;
+    if (storage_thread_id) {
+        queue_post(&storage_queue, Q_STORAGE_SLEEP, 0);
+    }
 }
+#endif /* (CONFIG_STORAGE & STORAGE_ATA) */
 
-int storage_driver_type(int drive)
+#ifdef STORAGE_CLOSE
+void storage_close(void)
 {
-    if ((unsigned int)drive >= num_drives)
-        return -1;
+    if (storage_thread_id) {
+        queue_post(&storage_queue, Q_STORAGE_CLOSE, 0);
+        thread_wait(storage_thread_id);
+    }
+}
+#endif /* STORAGE_CLOSE */
 
-    unsigned int bit = (storage_drivers[drive] & DRIVER_MASK)>>DRIVER_OFFSET;
-    return bit ? find_first_set_bit(bit) : -1;
+static inline void storage_thread_init(void)
+{
+    if (storage_thread_id) {
+        return;
+    }
+
+    queue_init(&storage_queue, true);
+    storage_thread_id = create_thread(storage_thread, &storage_thread_stack,
+                                      sizeof (storage_thread_stack),
+                                      0, &storage_thread_name[1]
+                                      IF_PRIO(, PRIORITY_USER_INTERFACE)
+                                      IF_COP(, CPU));
 }
 
 int storage_init(void)
 {
+#ifdef CONFIG_STORAGE_MULTI
     int rc=0;
     int i;
     num_drives=0;
@@ -201,54 +329,142 @@ int storage_init(void)
             (STORAGE_RAMDISK<<DRIVER_OFFSET) | (i << DRIVE_OFFSET);
     }
 #endif
+#else /* ndef CONFIG_STORAGE_MULTI */
+    STORAGE_FUNCTION(init)();
+#endif /* CONFIG_STORAGE_MULTI */
 
+    storage_thread_init();
     return 0;
 }
 
+int storage_read_sectors(IF_MD(int drive,) unsigned long start, int count,
+                         void* buf)
+{
+#ifdef CONFIG_STORAGE_MULTI
+    int driver=(storage_drivers[drive] & DRIVER_MASK)>>DRIVER_OFFSET;
+    int ldrive=(storage_drivers[drive] & DRIVE_MASK)>>DRIVE_OFFSET;
 
-void storage_enable(bool on)
+    switch (driver)
+    {
+#if (CONFIG_STORAGE & STORAGE_ATA)
+    case STORAGE_ATA:
+        return ata_read_sectors(IF_MD(ldrive,) start,count,buf);
+#endif
+
+#if (CONFIG_STORAGE & STORAGE_MMC)
+    case STORAGE_MMC:
+        return mmc_read_sectors(IF_MD(ldrive,) start,count,buf);
+#endif
+
+#if (CONFIG_STORAGE & STORAGE_SD)
+    case STORAGE_SD:
+        return sd_read_sectors(IF_MD(ldrive,) start,count,buf);
+#endif
+
+#if (CONFIG_STORAGE & STORAGE_NAND)
+    case STORAGE_NAND:
+        return nand_read_sectors(IF_MD(ldrive,) start,count,buf);
+#endif
+
+#if (CONFIG_STORAGE & STORAGE_RAMDISK)
+    case STORAGE_RAMDISK:
+        return ramdisk_read_sectors(IF_MD(ldrive,) start,count,buf);
+#endif
+    }
+
+    return -1;
+#else /* CONFIG_STORAGE_MULTI */
+    return STORAGE_FUNCTION(read_sectors)(IF_MD(drive,)start,count,buf);
+#endif /* CONFIG_STORAGE_MULTI */
+
+}
+
+int storage_write_sectors(IF_MD(int drive,) unsigned long start, int count,
+                          const void* buf)
 {
+#ifdef CONFIG_STORAGE_MULTI
+    int driver=(storage_drivers[drive] & DRIVER_MASK)>>DRIVER_OFFSET;
+    int ldrive=(storage_drivers[drive] & DRIVE_MASK)>>DRIVE_OFFSET;
+
+    switch (driver)
+    {
 #if (CONFIG_STORAGE & STORAGE_ATA)
-    ata_enable(on);
+    case STORAGE_ATA:
+        return ata_write_sectors(IF_MD(ldrive,)start,count,buf);
 #endif
 
 #if (CONFIG_STORAGE & STORAGE_MMC)
-    mmc_enable(on);
+    case STORAGE_MMC:
+        return mmc_write_sectors(IF_MD(ldrive,)start,count,buf);
 #endif
 
 #if (CONFIG_STORAGE & STORAGE_SD)
-    sd_enable(on);
+    case STORAGE_SD:
+        return sd_write_sectors(IF_MD(ldrive,)start,count,buf);
 #endif
 
 #if (CONFIG_STORAGE & STORAGE_NAND)
-    nand_enable(on);
+    case STORAGE_NAND:
+        return nand_write_sectors(IF_MD(ldrive,)start,count,buf);
 #endif
 
 #if (CONFIG_STORAGE & STORAGE_RAMDISK)
-    ramdisk_enable(on);
+    case STORAGE_RAMDISK:
+        return ramdisk_write_sectors(IF_MD(ldrive,)start,count,buf);
 #endif
+    }
+
+    return -1;
+#else /* CONFIG_STORAGE_MULTI */
+    return STORAGE_FUNCTION(write_sectors)(IF_MD(drive,)start,count,buf);
+#endif /* CONFIG_STORAGE_MULTI */
 }
 
-void storage_sleep(void)
+#ifdef CONFIG_STORAGE_MULTI
+
+#define DRIVER_MASK     0xff000000
+#define DRIVER_OFFSET   24
+#define DRIVE_MASK      0x00ff0000
+#define DRIVE_OFFSET    16
+#define PARTITION_MASK  0x0000ff00
+
+static unsigned int storage_drivers[NUM_DRIVES];
+static unsigned int num_drives;
+
+int storage_num_drives(void)
+{
+    return num_drives;
+}
+
+int storage_driver_type(int drive)
+{
+    if ((unsigned int)drive >= num_drives)
+        return -1;
+
+    unsigned int bit = (storage_drivers[drive] & DRIVER_MASK)>>DRIVER_OFFSET;
+    return bit ? find_first_set_bit(bit) : -1;
+}
+
+void storage_enable(bool on)
 {
 #if (CONFIG_STORAGE & STORAGE_ATA)
-    ata_sleep();
+    ata_enable(on);
 #endif
 
 #if (CONFIG_STORAGE & STORAGE_MMC)
-    mmc_sleep();
+    mmc_enable(on);
 #endif
 
 #if (CONFIG_STORAGE & STORAGE_SD)
-    sd_sleep();
+    sd_enable(on);
 #endif
 
 #if (CONFIG_STORAGE & STORAGE_NAND)
-    nand_sleep();
+    nand_enable(on);
 #endif
 
 #if (CONFIG_STORAGE & STORAGE_RAMDISK)
-    ramdisk_sleep();
+    ramdisk_enable(on);
 #endif
 }
 
@@ -603,6 +819,5 @@ bool storage_present(int drive)
         return false;
     }
 }
-#endif
-
+#endif /* HAVE_HOTSWAP */
 #endif /*CONFIG_STORAGE_MULTI*/