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]);