Add IO priority handling. Currently all IO has equal priority, except the dircache scanning thread which is lower. This fixes the slow boot problem for me, with the added benefit that actual audio playback also starts faster.

Lots of the changes are due to changing storage_(read|write)sectors() from macros to wrapper functions. This means that they have to be called with IF_MD2(drive,) again.

Flyspray: FS#11167
Author: Frank Gevaerts


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

10 files changed, 216 insertions, 98 deletions

diff --git a/firmware/SOURCES b/firmware/SOURCES
index e4bdca258e..fe97fe2253 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -152,9 +152,7 @@ drivers/sd.c
 #if (CONFIG_STORAGE & STORAGE_RAMDISK)
 drivers/ramdisk.c
 #endif
-#ifdef CONFIG_STORAGE_MULTI
 storage.c
-#endif
 drivers/fat.c
 #if (CONFIG_STORAGE & STORAGE_MMC) || (CONFIG_STORAGE & STORAGE_SD)
 hotswap.c
diff --git a/firmware/common/dircache.c b/firmware/common/dircache.c
index 9f19ac3d24..8b9be78b35 100644
--- a/firmware/common/dircache.c
+++ b/firmware/common/dircache.c
@@ -40,6 +40,7 @@
 #include "file.h"
 #include "buffer.h"
 #include "dir.h"
+#include "storage.h"
 #if CONFIG_RTC
 #include "time.h"
 #include "timefuncs.h"
@@ -760,6 +761,7 @@ void* dircache_steal_buffer(long *size)
 void dircache_init(void)
 {
     int i;
+    int thread_id;
     
     dircache_initialized = false;
     dircache_initializing = false;
@@ -771,10 +773,14 @@ void dircache_init(void)
     }
     
     queue_init(&dircache_queue, true);
-    create_thread(dircache_thread, dircache_stack,
+    thread_id = create_thread(dircache_thread, dircache_stack,
                 sizeof(dircache_stack), 0, dircache_thread_name
                 IF_PRIO(, PRIORITY_BACKGROUND)
                 IF_COP(, CPU));
+#ifdef HAVE_IO_PRIORITY
+    thread_set_io_priority(thread_id,IO_PRIORITY_BACKGROUND);
+#endif
+
 }
 
 /**
diff --git a/firmware/common/disk.c b/firmware/common/disk.c
index c4553099ec..f8efe1c88b 100644
--- a/firmware/common/disk.c
+++ b/firmware/common/disk.c
@@ -82,7 +82,7 @@ struct partinfo* disk_init(IF_MD_NONVOID(int drive))
     (void)drive;
 #endif
 
-    storage_read_sectors(drive, 0,1, &sector);
+    storage_read_sectors(IF_MD2(drive,) 0,1, &sector);
     /* check that the boot sector is initialized */
     if ( (sector[510] != 0x55) ||
          (sector[511] != 0xaa)) {
diff --git a/firmware/drivers/fat.c b/firmware/drivers/fat.c
index ff2bebce92..f93b32f832 100644
--- a/firmware/drivers/fat.c
+++ b/firmware/drivers/fat.c
@@ -311,7 +311,7 @@ int fat_mount(IF_MV2(int volume,) IF_MD2(int drive,) long startsector)
 #endif
 
     /* Read the sector */
-    rc = storage_read_sectors(drive, startsector,1,buf);
+    rc = storage_read_sectors(IF_MD2(drive,) startsector,1,buf);
     if(rc)
     {
         DEBUGF( "fat_mount() - Couldn't read BPB (error code %d)\n", rc);
@@ -433,7 +433,7 @@ int fat_mount(IF_MV2(int volume,) IF_MD2(int drive,) long startsector)
 #endif /* #ifdef HAVE_FAT16SUPPORT */
     {
         /* Read the fsinfo sector */
-        rc = storage_read_sectors(drive, 
+        rc = storage_read_sectors(IF_MD2(drive,)
             startsector + fat_bpb->bpb_fsinfo, 1, buf);
         if (rc < 0)
         {
@@ -618,7 +618,7 @@ static void flush_fat_sector(struct fat_cache_entry *fce,
 #endif
 
     /* Write to the first FAT */
-    rc = storage_write_sectors(fce->fat_vol->drive,
+    rc = storage_write_sectors(IF_MD2(fce->fat_vol->drive,)
                            secnum, 1,
                            sectorbuf);
     if(rc < 0)
@@ -639,7 +639,7 @@ static void flush_fat_sector(struct fat_cache_entry *fce,
 #else
         secnum += fat_bpbs[0].fatsize;
 #endif
-        rc = storage_write_sectors(fce->fat_vol->drive,
+        rc = storage_write_sectors(IF_MD2(fce->fat_vol->drive,)
                                secnum, 1, sectorbuf);
         if(rc < 0)
         {
@@ -685,7 +685,7 @@ static void *cache_fat_sector(IF_MV2(struct bpb* fat_bpb,)
     /* Load the sector if it is not cached */
     if(!fce->inuse)
     {
-        rc = storage_read_sectors(fat_bpb->drive,
+        rc = storage_read_sectors(IF_MD2(fat_bpb->drive,)
                               secnum + fat_bpb->startsector,1,
                               sectorbuf);
         if(rc < 0)
@@ -944,7 +944,7 @@ static int update_fsinfo(IF_MV_NONVOID(struct bpb* fat_bpb))
 #endif /* #ifdef HAVE_FAT16SUPPORT */
 
     /* update fsinfo */
-    rc = storage_read_sectors(fat_bpb->drive, 
+    rc = storage_read_sectors(IF_MD2(fat_bpb->drive,)
                           fat_bpb->startsector + fat_bpb->bpb_fsinfo, 1,fsinfo);
     if (rc < 0)
     {
@@ -957,7 +957,7 @@ static int update_fsinfo(IF_MV_NONVOID(struct bpb* fat_bpb))
     intptr = (long*)&(fsinfo[FSINFO_NEXTFREE]);
     *intptr = htole32(fat_bpb->fsinfo.nextfree);
 
-    rc = storage_write_sectors(fat_bpb->drive,
+    rc = storage_write_sectors(IF_MD2(fat_bpb->drive,)
                            fat_bpb->startsector + fat_bpb->bpb_fsinfo,1,fsinfo);
     if (rc < 0)
     {
@@ -2110,11 +2110,11 @@ static int transfer(IF_MV2(struct bpb* fat_bpb,)
         if (start + count > fat_bpb->totalsectors)
             panicf("Write %ld after data\n",
                 start + count - fat_bpb->totalsectors);
-        rc = storage_write_sectors(fat_bpb->drive,
+        rc = storage_write_sectors(IF_MD2(fat_bpb->drive,)
                                start + fat_bpb->startsector, count, buf);
     }
     else
-        rc = storage_read_sectors(fat_bpb->drive,
+        rc = storage_read_sectors(IF_MD2(fat_bpb->drive,)
                               start + fat_bpb->startsector, count, buf);
     if (rc < 0) {
         DEBUGF( "transfer() - Couldn't %s sector %lx"
diff --git a/firmware/export/config.h b/firmware/export/config.h
index 9d8dc41111..858d9bf250 100644
--- a/firmware/export/config.h
+++ b/firmware/export/config.h
@@ -921,4 +921,8 @@ Lyre prototype 1 */
 #define HAVE_PLUGIN_CHECK_OPEN_CLOSE
 #endif
 
+#ifdef HAVE_DIRCACHE
+#define HAVE_IO_PRIORITY
+#endif
+
 #endif /* __CONFIG_H__ */
diff --git a/firmware/export/storage.h b/firmware/export/storage.h
index 0096e87a8d..e62cf0d902 100644
--- a/firmware/export/storage.h
+++ b/firmware/export/storage.h
@@ -57,6 +57,7 @@ struct storage_info
  */
     #define storage_num_drives() NUM_DRIVES
     #if (CONFIG_STORAGE & STORAGE_ATA)
+        #define STORAGE_FUNCTION(NAME) (ata_## NAME)
         #define storage_spindown ata_spindown
         #define storage_sleep ata_sleep
         #define storage_spin ata_spin
@@ -67,8 +68,6 @@ struct storage_info
         #define storage_soft_reset() ata_soft_reset()
         #define storage_init() ata_init()
         #define storage_close() ata_close()
-        #define storage_read_sectors(drive, start, count, buf) ata_read_sectors(IF_MD2(drive,) start, count, buf)
-        #define storage_write_sectors(drive, start, count, buf) ata_write_sectors(IF_MD2(drive,) start, count, buf)
         #ifdef HAVE_STORAGE_FLUSH
             #define storage_flush() (void)0
         #endif
@@ -84,6 +83,7 @@ struct storage_info
             #define storage_present(drive) ata_present(IF_MD(drive))
         #endif
     #elif (CONFIG_STORAGE & STORAGE_SD)
+        #define STORAGE_FUNCTION(NAME) (sd_## NAME)
         #define storage_spindown sd_spindown
         #define storage_sleep sd_sleep
         #define storage_spin sd_spin
@@ -93,8 +93,6 @@ struct storage_info
         #define storage_disk_is_active() 0
         #define storage_soft_reset() (void)0
         #define storage_init() sd_init()
-        #define storage_read_sectors(drive, start, count, buf) sd_read_sectors(IF_MD2(drive,) start, count, buf)
-        #define storage_write_sectors(drive, start, count, buf) sd_write_sectors(IF_MD2(drive,) start, count, buf)
         #ifdef HAVE_STORAGE_FLUSH
             #define storage_flush() (void)0
         #endif
@@ -110,6 +108,7 @@ struct storage_info
             #define storage_present(drive) sd_present(IF_MD(drive))
         #endif
      #elif (CONFIG_STORAGE & STORAGE_MMC)
+        #define STORAGE_FUNCTION(NAME) (mmc_## NAME)
         #define storage_spindown mmc_spindown
         #define storage_sleep mmc_sleep
         #define storage_spin mmc_spin
@@ -119,8 +118,6 @@ struct storage_info
         #define storage_disk_is_active() mmc_disk_is_active()
         #define storage_soft_reset() (void)0
         #define storage_init() mmc_init()
-        #define storage_read_sectors(drive, start, count, buf) mmc_read_sectors(IF_MD2(drive,) start, count, buf)
-        #define storage_write_sectors(drive, start, count, buf) mmc_write_sectors(IF_MD2(drive,) start, count, buf)
         #ifdef HAVE_STORAGE_FLUSH
             #define storage_flush() (void)0
         #endif
@@ -136,6 +133,7 @@ struct storage_info
             #define storage_present(drive) mmc_present(IF_MD(drive))
         #endif
     #elif (CONFIG_STORAGE & STORAGE_NAND)
+        #define STORAGE_FUNCTION(NAME) (nand_## NAME)
         #define storage_spindown nand_spindown
         #define storage_sleep nand_sleep
         #define storage_spin nand_spin
@@ -145,8 +143,6 @@ struct storage_info
         #define storage_disk_is_active() 0
         #define storage_soft_reset() (void)0
         #define storage_init() nand_init()
-        #define storage_read_sectors(drive, start, count, buf) nand_read_sectors(IF_MD2(drive,) start, count, buf)
-        #define storage_write_sectors(drive, start, count, buf) nand_write_sectors(IF_MD2(drive,) start, count, buf)
         #ifdef HAVE_STORAGE_FLUSH
             #define storage_flush() nand_flush()
         #endif
@@ -162,6 +158,7 @@ struct storage_info
             #define storage_present(drive) nand_present(IF_MD(drive))
         #endif
     #elif (CONFIG_STORAGE & STORAGE_RAMDISK)
+        #define STORAGE_FUNCTION(NAME) (ramdisk_## NAME)
         #define storage_spindown ramdisk_spindown
         #define storage_sleep ramdisk_sleep
         #define storage_spin ramdisk_spin
@@ -171,8 +168,6 @@ struct storage_info
         #define storage_disk_is_active() 0
         #define storage_soft_reset() (void)0
         #define storage_init() ramdisk_init()
-        #define storage_read_sectors(drive, start, count, buf) ramdisk_read_sectors(IF_MD2(drive,) start, count, buf)
-        #define storage_write_sectors(drive, start, count, buf) ramdisk_write_sectors(IF_MD2(drive,) start, count, buf)
         #ifdef HAVE_STORAGE_FLUSH
             #define storage_flush() (void)0
         #endif
@@ -200,8 +195,6 @@ void storage_sleepnow(void);
 bool storage_disk_is_active(void);
 int storage_soft_reset(void);
 int storage_init(void);
-int storage_read_sectors(int drive, unsigned long start, int count, void* buf);
-int storage_write_sectors(int drive, unsigned long start, int count, const void* buf);
 int storage_flush(void);
 void storage_spin(void);
 void storage_spindown(int seconds);
@@ -217,4 +210,7 @@ bool storage_present(int drive);
 #endif
 
 #endif /* NOT CONFIG_STORAGE_MULTI and NOT SIMULATOR*/
+
+int storage_read_sectors(IF_MD2(int drive,) unsigned long start, int count, void* buf);
+int storage_write_sectors(IF_MD2(int drive,) unsigned long start, int count, const void* buf);
 #endif
diff --git a/firmware/export/thread.h b/firmware/export/thread.h
index 2a2a7a8619..a75981dcba 100644
--- a/firmware/export/thread.h
+++ b/firmware/export/thread.h
@@ -58,6 +58,9 @@
 #define NUM_PRIORITIES           32
 #define PRIORITY_IDLE            32  /* Priority representative of no tasks */
 
+#define IO_PRIORITY_IMMEDIATE    0
+#define IO_PRIORITY_BACKGROUND   32
+
 #if CONFIG_CODEC == SWCODEC
 
 #ifdef HAVE_RECORDING
@@ -294,6 +297,9 @@ struct thread_entry
     struct corelock waiter_cl; /* Corelock for thread_wait */
     struct corelock slot_cl;   /* Corelock to lock thread slot */
 #endif
+#ifdef HAVE_IO_PRIORITY
+    unsigned char io_priority;
+#endif
 };
 
 /*** Macros for internal use ***/
@@ -539,6 +545,10 @@ unsigned int wakeup_thread(struct thread_entry **list);
 int thread_set_priority(unsigned int thread_id, int priority);
 int thread_get_priority(unsigned int thread_id);
 #endif /* HAVE_PRIORITY_SCHEDULING */
+#ifdef HAVE_IO_PRIORITY
+void thread_set_io_priority(unsigned int thread_id, int io_priority);
+int thread_get_io_priority(unsigned int thread_id);
+#endif /* HAVE_IO_PRIORITY */
 #if NUM_CORES > 1
 unsigned int switch_core(unsigned int new_core);
 #endif
diff --git a/firmware/storage.c b/firmware/storage.c
index d6700d1148..ceae98fa40 100644
--- a/firmware/storage.c
+++ b/firmware/storage.c
@@ -19,6 +19,9 @@
  *
  ****************************************************************************/
 #include "storage.h"
+#include "kernel.h"
+
+#ifdef CONFIG_STORAGE_MULTI
 
 #define DRIVER_MASK     0xff000000
 #define DRIVER_OFFSET   24
@@ -28,116 +31,112 @@
 
 static unsigned int storage_drivers[NUM_DRIVES];
 static unsigned int num_drives;
+#endif
 
-int storage_num_drives(void)
-{
-    return num_drives;
-}
 
-int storage_init(void)
-{
-    int rc=0;
-    int i;
-    num_drives=0;
-    
-#if (CONFIG_STORAGE & STORAGE_ATA)
-    if ((rc=ata_init())) return rc;
-    
-    int ata_drives = ata_num_drives(num_drives);
-    for (i=0; i<ata_drives; i++)
-    {
-        storage_drivers[num_drives++] = 
-            (STORAGE_ATA<<DRIVER_OFFSET) | (i << DRIVE_OFFSET);
-    }
-#endif
+#ifdef HAVE_IO_PRIORITY
 
-#if (CONFIG_STORAGE & STORAGE_MMC)
-    if ((rc=mmc_init())) return rc;
-    
-    int mmc_drives = mmc_num_drives(num_drives);
-    for (i=0; i<mmc_drives ;i++)
-    {
-        storage_drivers[num_drives++] =
-            (STORAGE_MMC<<DRIVER_OFFSET) | (i << DRIVE_OFFSET);
-    }
-#endif
+/* Same for flash? */
+#define STORAGE_MINIMUM_IDLE_TIME (HZ/10)
+#define STORAGE_DELAY_UNIT  (HZ/20)
 
-#if (CONFIG_STORAGE & STORAGE_SD)
-    if ((rc=sd_init())) return rc;
-    
-    int sd_drives = sd_num_drives(num_drives);
-    for (i=0; i<sd_drives; i++)
+unsigned int storage_last_thread[NUM_DRIVES];
+unsigned int storage_last_activity[NUM_DRIVES];
+
+static bool storage_should_wait(int drive, int prio)
+{
+    int other_prio = thread_get_io_priority(storage_last_thread[drive]);
+    if(TIME_BEFORE(current_tick,storage_last_activity[drive]+STORAGE_MINIMUM_IDLE_TIME))
     {
-        storage_drivers[num_drives++] =
-            (STORAGE_SD<<DRIVER_OFFSET) | (i << DRIVE_OFFSET);
+        if(prio<=other_prio)
+        {
+            /* There is another active thread, but we have lower priority */
+            return false;
+        }
+        else
+        {
+            /* There is another active thread, but it has lower priority */
+            return true;
+        }
     }
-#endif
-
-#if (CONFIG_STORAGE & STORAGE_NAND)
-    if ((rc=nand_init())) return rc;
-    
-    int nand_drives = nand_num_drives(num_drives);
-    for (i=0; i<nand_drives; i++)
+    else
     {
-        storage_drivers[num_drives++] =
-            (STORAGE_NAND<<DRIVER_OFFSET) | (i << DRIVE_OFFSET);
+        /* There's nothing going on anyway */
+        return false;
     }
-#endif
+}
 
-#if (CONFIG_STORAGE & STORAGE_RAMDISK)
-    if ((rc=ramdisk_init())) return rc;
-    
-    int ramdisk_drives = ramdisk_num_drives(num_drives);
-    for (i=0; i<ramdisk_drives; i++)
+static void storage_wait_turn(IF_MD_NONVOID(int drive))
+{
+#ifndef HAVE_MULTIDRIVE
+    int drive=0;
+#endif
+    int my_prio = thread_get_io_priority(thread_get_current());
+    int loops=my_prio;
+    while(storage_should_wait(drive, my_prio) && (loops--)>=0)
     {
-        storage_drivers[num_drives++] =
-            (STORAGE_RAMDISK<<DRIVER_OFFSET) | (i << DRIVE_OFFSET);
+        sleep(STORAGE_DELAY_UNIT);
     }
-#endif
 
-    return 0;
+    storage_last_thread[drive] = thread_get_current();
+    storage_last_activity[drive] = current_tick;
 }
+#endif
 
-int storage_read_sectors(int drive, unsigned long start, int count,
+int storage_read_sectors(IF_MD2(int drive,) unsigned long start, int count,
                          void* buf)
 {
+#ifdef HAVE_IO_PRIORITY
+    storage_wait_turn(IF_MD(drive));
+#endif
+
+#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)
     case STORAGE_ATA:
-        return ata_read_sectors(ldrive,start,count,buf);
+        return ata_read_sectors(IF_MD2(ldrive,) start,count,buf);
 #endif
 
 #if (CONFIG_STORAGE & STORAGE_MMC)
     case STORAGE_MMC:
-        return mmc_read_sectors(ldrive,start,count,buf);
+        return mmc_read_sectors(IF_MD2(ldrive,) start,count,buf);
 #endif
 
 #if (CONFIG_STORAGE & STORAGE_SD)
     case STORAGE_SD:
-        return sd_read_sectors(ldrive,start,count,buf);
+        return sd_read_sectors(IF_MD2(ldrive,) start,count,buf);
 #endif
 
 #if (CONFIG_STORAGE & STORAGE_NAND)
     case STORAGE_NAND:
-        return nand_read_sectors(ldrive,start,count,buf);
+        return nand_read_sectors(IF_MD2(ldrive,) start,count,buf);
 #endif
 
 #if (CONFIG_STORAGE & STORAGE_RAMDISK)
     case STORAGE_RAMDISK:
-        return ramdisk_read_sectors(ldrive,start,count,buf);
+        return ramdisk_read_sectors(IF_MD2(ldrive,) start,count,buf);
 #endif
     }
     
     return -1;
+#else /* CONFIG_STORAGE_MULTI */
+    return STORAGE_FUNCTION(read_sectors)(IF_MD2(drive,)start,count,buf);
+#endif /* CONFIG_STORAGE_MULTI */
+
 }
 
-int storage_write_sectors(int drive, unsigned long start, int count,
+int storage_write_sectors(IF_MD2(int drive,) unsigned long start, int count,
                           const void* buf)
 {
+#ifdef HAVE_IO_PRIORITY
+    storage_wait_turn(IF_MD(drive));
+#endif
+
+#ifdef CONFIG_STORAGE_MULTI
     int driver=(storage_drivers[drive] & DRIVER_MASK)>>DRIVER_OFFSET;
     int ldrive=(storage_drivers[drive] & DRIVE_MASK)>>DRIVE_OFFSET;
     
@@ -145,33 +144,117 @@ int storage_write_sectors(int drive, unsigned long start, int count,
     {
 #if (CONFIG_STORAGE & STORAGE_ATA)
     case STORAGE_ATA:
-        return ata_write_sectors(ldrive,start,count,buf);
+        return ata_write_sectors(IF_MD2(ldrive,)start,count,buf);
 #endif
 
 #if (CONFIG_STORAGE & STORAGE_MMC)
     case STORAGE_MMC:
-        return mmc_write_sectors(ldrive,start,count,buf);
+        return mmc_write_sectors(IF_MD2(ldrive,)start,count,buf);
 #endif
 
 #if (CONFIG_STORAGE & STORAGE_SD)
     case STORAGE_SD:
-        return sd_write_sectors(ldrive,start,count,buf);
+        return sd_write_sectors(IF_MD2(ldrive,)start,count,buf);
 #endif
 
 #if (CONFIG_STORAGE & STORAGE_NAND)
     case STORAGE_NAND:
-        return nand_write_sectors(ldrive,start,count,buf);
+        return nand_write_sectors(IF_MD2(ldrive,)start,count,buf);
 #endif
 
 #if (CONFIG_STORAGE & STORAGE_RAMDISK)
     case STORAGE_RAMDISK:
-        return ramdisk_write_sectors(ldrive,start,count,buf);
+        return ramdisk_write_sectors(IF_MD2(ldrive,)start,count,buf);
 #endif
     }
     
     return -1;
+#else /* CONFIG_STORAGE_MULTI */
+    return STORAGE_FUNCTION(write_sectors)(IF_MD2(drive,)start,count,buf);
+#endif /* CONFIG_STORAGE_MULTI */
+}
+
+#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_init(void)
+{
+    int rc=0;
+    int i;
+    num_drives=0;
+    
+#if (CONFIG_STORAGE & STORAGE_ATA)
+    if ((rc=ata_init())) return rc;
+    
+    int ata_drives = ata_num_drives(num_drives);
+    for (i=0; i<ata_drives; i++)
+    {
+        storage_drivers[num_drives++] = 
+            (STORAGE_ATA<<DRIVER_OFFSET) | (i << DRIVE_OFFSET);
+    }
+#endif
+
+#if (CONFIG_STORAGE & STORAGE_MMC)
+    if ((rc=mmc_init())) return rc;
+    
+    int mmc_drives = mmc_num_drives(num_drives);
+    for (i=0; i<mmc_drives ;i++)
+    {
+        storage_drivers[num_drives++] =
+            (STORAGE_MMC<<DRIVER_OFFSET) | (i << DRIVE_OFFSET);
+    }
+#endif
+
+#if (CONFIG_STORAGE & STORAGE_SD)
+    if ((rc=sd_init())) return rc;
+    
+    int sd_drives = sd_num_drives(num_drives);
+    for (i=0; i<sd_drives; i++)
+    {
+        storage_drivers[num_drives++] =
+            (STORAGE_SD<<DRIVER_OFFSET) | (i << DRIVE_OFFSET);
+    }
+#endif
+
+#if (CONFIG_STORAGE & STORAGE_NAND)
+    if ((rc=nand_init())) return rc;
+    
+    int nand_drives = nand_num_drives(num_drives);
+    for (i=0; i<nand_drives; i++)
+    {
+        storage_drivers[num_drives++] =
+            (STORAGE_NAND<<DRIVER_OFFSET) | (i << DRIVE_OFFSET);
+    }
+#endif
+
+#if (CONFIG_STORAGE & STORAGE_RAMDISK)
+    if ((rc=ramdisk_init())) return rc;
+    
+    int ramdisk_drives = ramdisk_num_drives(num_drives);
+    for (i=0; i<ramdisk_drives; i++)
+    {
+        storage_drivers[num_drives++] =
+            (STORAGE_RAMDISK<<DRIVER_OFFSET) | (i << DRIVE_OFFSET);
+    }
+#endif
+
+    return 0;
 }
 
+
 void storage_enable(bool on)
 {
 #if (CONFIG_STORAGE & STORAGE_ATA)
@@ -572,3 +655,5 @@ bool storage_present(int drive)
     return ret;
 }
 #endif
+
+#endif /*CONFIG_STORAGE_MULTI*/
diff --git a/firmware/thread.c b/firmware/thread.c
index 38933f623e..81ef42a6b0 100644
--- a/firmware/thread.c
+++ b/firmware/thread.c
@@ -2413,6 +2413,11 @@ unsigned int create_thread(void (*function)(void),
     prio_add_entry(&thread->pdist, priority);
 #endif
 
+#ifdef HAVE_IO_PRIORITY
+    /* Default to high (foreground) priority */
+    thread->io_priority = IO_PRIORITY_IMMEDIATE;
+#endif
+
 #if NUM_CORES > 1
     thread->core = core;
 
@@ -2918,6 +2923,20 @@ int thread_get_priority(unsigned int thread_id)
 }
 #endif /* HAVE_PRIORITY_SCHEDULING */
 
+#ifdef HAVE_IO_PRIORITY
+int thread_get_io_priority(unsigned int thread_id)
+{
+    struct thread_entry *thread = thread_id_entry(thread_id);
+    return thread->io_priority;
+}
+
+void thread_set_io_priority(unsigned int thread_id,int io_priority)
+{
+    struct thread_entry *thread = thread_id_entry(thread_id);
+    thread->io_priority = io_priority;
+}
+#endif
+
 /*---------------------------------------------------------------------------
  * Starts a frozen thread - similar semantics to wakeup_thread except that
  * the thread is on no scheduler or wakeup queue at all. It exists simply by
diff --git a/firmware/usbstack/usb_storage.c b/firmware/usbstack/usb_storage.c
index 4a8f2dc397..647fbd8e5b 100644
--- a/firmware/usbstack/usb_storage.c
+++ b/firmware/usbstack/usb_storage.c
@@ -353,7 +353,7 @@ static bool check_disk_present(IF_MD_NONVOID(int volume))
     return true;
 #else
     unsigned char sector[SECTOR_SIZE];
-    return storage_read_sectors(volume,0,1,sector) == 0;
+    return storage_read_sectors(IF_MD2(volume,)0,1,sector) == 0;
 #endif
 }
 
@@ -537,7 +537,7 @@ void usb_storage_transfer_complete(int ep,int dir,int status,int length)
                         cur_cmd.data[cur_cmd.data_select],
                         MIN(WRITE_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count)*SECTOR_SIZE);
 #else
-                int result = storage_write_sectors(cur_cmd.lun,
+                int result = storage_write_sectors(IF_MD2(cur_cmd.lun,)
                         cur_cmd.sector,
                         MIN(WRITE_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count),
                         cur_cmd.data[cur_cmd.data_select]);
@@ -726,7 +726,7 @@ static void send_and_read_next(void)
                 ramdisk_buffer + cur_cmd.sector*SECTOR_SIZE,
                 MIN(READ_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count)*SECTOR_SIZE);
 #else
-        cur_cmd.last_result = storage_read_sectors(cur_cmd.lun,
+        cur_cmd.last_result = storage_read_sectors(IF_MD2(cur_cmd.lun,)
                 cur_cmd.sector,
                 MIN(READ_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count),
                 cur_cmd.data[cur_cmd.data_select]);
@@ -1070,7 +1070,7 @@ static void handle_scsi(struct command_block_wrapper* cbw)
                         ramdisk_buffer + cur_cmd.sector*SECTOR_SIZE,
                         MIN(READ_BUFFER_SIZE/SECTOR_SIZE,cur_cmd.count)*SECTOR_SIZE);
 #else
-                cur_cmd.last_result = storage_read_sectors(cur_cmd.lun,
+                cur_cmd.last_result = storage_read_sectors(IF_MD2(cur_cmd.lun,)
                         cur_cmd.sector,
                         MIN(READ_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count),
                         cur_cmd.data[cur_cmd.data_select]);