58 files changed, 1762 insertions, 881 deletions

diff --git a/firmware/SOURCES b/firmware/SOURCES
index 4e6fcbf70c..192cdb711d 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -62,6 +62,10 @@ common/bootdata.c
 #endif
 #endif
 
+#if defined(HAVE_DEVICEDATA)
+common/devicedata.c
+#endif
+
 #ifdef HAVE_SDL
 target/hosted/sdl/button-sdl.c
 target/hosted/sdl/kernel-sdl.c
diff --git a/firmware/asm/thread.h b/firmware/asm/thread.h
index 5372be73ab..831de9ae8d 100644
--- a/firmware/asm/thread.h
+++ b/firmware/asm/thread.h
@@ -46,10 +46,16 @@ struct regs
      * give a decent amount of space and hope for the best...
      * FIXME: this isn't a great solution. */
     #undef MINSIGSTKSZ
-    #define MINSIGSTKSZ 16384
     #endif
-    /* MINSIGSTKSZ for the OS to deliver the signal + 0x3000 for us */
+    #ifndef MINSIGSTKSZ
+      #define MINSIGSTKSZ 16384
+    #endif
+    /* MINSIGSTKSZ for the OS to deliver the signal, plus more for us */
+#if defined(SIMULATOR) || defined(__aarch64__)
+    #define DEFAULT_STACK_SIZE (MINSIGSTKSZ+0x6000) /* Bytes */
+#else
     #define DEFAULT_STACK_SIZE (MINSIGSTKSZ+0x3000) /* Bytes */
+#endif
   #elif defined(HAVE_WIN32_FIBER_THREADS)
     #define DEFAULT_STACK_SIZE 0x1000 /* Bytes */
   #endif
diff --git a/firmware/common/devicedata.c b/firmware/common/devicedata.c
new file mode 100644
index 0000000000..75fe79d7fa
--- /dev/null
+++ b/firmware/common/devicedata.c
@@ -0,0 +1,88 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ *
+ * Copyright (C) 2024 by William Wilgus
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+
+#include "devicedata.h"
+#include "crc32.h"
+#include <stddef.h>
+#include <string.h>
+#include "debug.h"
+
+#ifndef BOOTLOADER
+void verify_device_data(void) INIT_ATTR;
+void verify_device_data(void)
+{
+    DEBUGF("%s", __func__);
+    /* verify payload with checksum */
+    uint32_t crc = crc_32(device_data.payload, device_data.length, 0xffffffff);
+    if (crc == device_data.crc)
+        return; /* return if data is valid */
+
+    /* Write the default if data is invalid */
+    memset(device_data.payload, 0xff, DEVICE_DATA_PAYLOAD_SIZE); /* Invalid data */
+    device_data.length = DEVICE_DATA_PAYLOAD_SIZE;
+    device_data.crc = crc_32(device_data.payload, device_data.length, 0xffffffff);
+
+}
+
+/******************************************************************************/
+#endif /* ndef BOOTLOADER ******************************************************/
+/******************************************************************************/
+
+#if defined(HAVE_DEVICEDATA)
+void  __attribute__((weak)) fill_devicedata(struct device_data_t *data)
+{
+    memset(data->payload, 0xff, data->length);
+}
+#endif
+
+/* Write bootdata into location in FIRMWARE marked by magic header
+ * Assumes buffer is already loaded with the firmware image
+ * We just need to find the location and write data into the
+ * payload region along with the crc for later verification and use.
+ * Returns payload len on success,
+ * On error returns false
+ */
+bool write_devicedata(unsigned char* buf, int len)
+{
+    int search_len = MIN(len, DEVICE_DATA_SEARCH_SIZE) - sizeof(struct device_data_t);
+
+    /* search for decvice data header prior to search_len */
+    for(int i = 0; i < search_len; i++)
+    {
+        struct device_data_t *data = (struct device_data_t *)&buf[i];
+        if (data->magic[0] != DEVICE_DATA_MAGIC0 ||
+            data->magic[1] != DEVICE_DATA_MAGIC1)
+            continue;
+
+        /* Ignore it if the length extends past the end of the buffer. */
+        int data_len = offsetof(struct device_data_t, payload) + data->length;
+        if (i + data_len > len)
+            continue;
+
+        fill_devicedata(data);
+
+        /* Calculate payload CRC */
+        data->crc = crc_32(data->payload, data->length, 0xffffffff);
+        return true;
+    }
+
+    return false;
+}
+
diff --git a/firmware/common/disk.c b/firmware/common/disk.c
index 576eede143..958f09755a 100644
--- a/firmware/common/disk.c
+++ b/firmware/common/disk.c
@@ -29,6 +29,7 @@
 #include "dir.h"
 #include "rb_namespace.h"
 #include "disk.h"
+#include "panic.h"
 
 #if defined(HAVE_BOOTDATA) && !defined(SIMULATOR) && !defined(BOOTLOADER)
 #include "bootdata.h"
@@ -110,7 +111,7 @@ static void init_volume(struct volumeinfo *vi, int drive, int part)
 }
 
 #ifdef MAX_LOG_SECTOR_SIZE
-static int disk_sector_multiplier[NUM_DRIVES] =
+static uint16_t disk_sector_multiplier[NUM_DRIVES] =
     { [0 ... NUM_DRIVES-1] = 1 };
 
 int disk_get_sector_multiplier(IF_MD_NONVOID(int drive))
@@ -125,6 +126,33 @@ int disk_get_sector_multiplier(IF_MD_NONVOID(int drive))
 }
 #endif /* MAX_LOG_SECTOR_SIZE */
 
+#if (CONFIG_STORAGE & STORAGE_ATA)  // XXX make this more generic?
+static uint16_t disk_log_sector_size[NUM_DRIVES] =
+    { [0 ... NUM_DRIVES-1] = SECTOR_SIZE }; /* Updated from STORAGE_INFO */
+int disk_get_log_sector_size(IF_MD_NONVOID(int drive))
+{
+    if (!CHECK_DRV(drive))
+        return 0;
+
+    disk_reader_lock();
+    int size = disk_log_sector_size[IF_MD_DRV(drive)];
+    disk_reader_unlock();
+    return size;
+}
+#ifdef HAVE_MULTIDRIVE
+#define LOG_SECTOR_SIZE(__drive) disk_log_sector_size[__drive]
+#else
+#define LOG_SECTOR_SIZE(__drive) disk_log_sector_size[0]
+#endif /* HAVE_MULTIDRIVE */
+#else /* !STORAGE_ATA */
+#define LOG_SECTOR_SIZE(__drive) SECTOR_SIZE
+int disk_get_log_sector_size(IF_MD_NONVOID(int drive))
+{
+    IF_MD((void)drive);
+    return SECTOR_SIZE;
+}
+#endif /* !CONFIG_STORAGE & STORAGE_ATA */
+
 bool disk_init(IF_MD_NONVOID(int drive))
 {
     if (!CHECK_DRV(drive))
@@ -134,7 +162,30 @@ bool disk_init(IF_MD_NONVOID(int drive))
     if (!sector)
         return false;
 
-    memset(sector, 0, SECTOR_SIZE);
+#if (CONFIG_STORAGE & STORAGE_ATA)
+    /* Query logical sector size */
+    struct storage_info *info = (struct storage_info*) sector;
+    storage_get_info(IF_MD_DRV(drive), info);
+    disk_writer_lock();
+#ifdef MAX_LOG_SECTOR_SIZE
+    disk_log_sector_size[IF_MD_DRV(drive)] = info->sector_size;
+#endif
+    disk_writer_unlock();
+
+#ifdef MAX_LOG_SECTOR_SIZE
+    if (info->sector_size > MAX_LOG_SECTOR_SIZE) {
+        panicf("Unsupported logical sector size: %d",
+               info->sector_size);
+    }
+#else
+    if (info->sector_size != SECTOR_SIZE) {
+        panicf("Unsupported logical sector size: %d",
+               info->sector_size);
+    }
+#endif
+#endif /* CONFIG_STORAGE & STORAGE_ATA */
+
+    memset(sector, 0, LOG_SECTOR_SIZE(drive));
     storage_read_sectors(IF_MD(drive,) 0, 1, sector);
 
     bool init = false;
@@ -213,11 +264,11 @@ bool disk_init(IF_MD_NONVOID(int drive))
 reload:
             storage_read_sectors(IF_MD(drive,) part_lba, 1, sector);
             uint8_t *pptr = ptr;
-            while (part < MAX_PARTITIONS_PER_DRIVE && part_entries) {
-                if (pptr - ptr >= SECTOR_SIZE) {
-                    part_lba++;
-                    goto reload;
-                }
+            while (part < MAX_PARTITIONS_PER_DRIVE && part_entries) {
+                if (pptr - ptr >= LOG_SECTOR_SIZE(drive)) {
+                    part_lba++;
+                    goto reload;
+                }
 
                 /* Parse GPT entry.  We only care about the "General Data" type, ie:
                      EBD0A0A2-B9E5-4433-87C0-68B6B72699C7
@@ -331,19 +382,18 @@ int disk_mount(int drive)
     disk_sector_multiplier[IF_MD_DRV(drive)] = 1;
 #endif
 
-        /* try "superfloppy" mode */
-        DEBUGF("Trying to mount sector 0.\n");
+    /* try "superfloppy" mode */
+    DEBUGF("Trying to mount sector 0.\n");
 
-        if (!fat_mount(IF_MV(volume,) IF_MD(drive,) 0))
-        {
-        #ifdef MAX_LOG_SECTOR_SIZE
-            disk_sector_multiplier[drive] =
-                fat_get_bytes_per_sector(IF_MV(volume)) / SECTOR_SIZE;
-        #endif
-            mounted = 1;
-            init_volume(&volumes[volume], drive, 0);
-            volume_onmount_internal(IF_MV(volume));
-        }
+    if (!fat_mount(IF_MV(volume,) IF_MD(drive,) 0))
+    {
+#ifdef MAX_LOG_SECTOR_SIZE
+        disk_sector_multiplier[drive] = fat_get_bytes_per_sector(IF_MV(volume)) / LOG_SECTOR_SIZE(drive);
+#endif
+        mounted = 1;
+        init_volume(&volumes[volume], drive, 0);
+        volume_onmount_internal(IF_MV(volume));
+    }
 
     if (mounted == 0 && volume != -1) /* not a "superfloppy"? */
     {
@@ -354,10 +404,10 @@ int disk_mount(int drive)
             if (pinfo[i].type == 0 || pinfo[i].type == 5)
                 continue;  /* skip free/extended partitions */
 
-        DEBUGF("Trying to mount partition %d.\n", i);
+            DEBUGF("Trying to mount partition %d.\n", i);
 
-        #ifdef MAX_LOG_SECTOR_SIZE
-            for (int j = 1; j <= (MAX_LOG_SECTOR_SIZE/SECTOR_SIZE); j <<= 1)
+#ifdef MAX_LOG_SECTOR_SIZE
+            for (int j = 1; j <= (MAX_LOG_SECTOR_SIZE/LOG_SECTOR_SIZE(drive)); j <<= 1)
             {
                 if (!fat_mount(IF_MV(volume,) IF_MD(drive,) pinfo[i].start * j))
                 {
@@ -371,7 +421,7 @@ int disk_mount(int drive)
                     break;
                 }
             }
-        #else /* ndef MAX_LOG_SECTOR_SIZE */
+#else /* ndef MAX_LOG_SECTOR_SIZE */
             if (!fat_mount(IF_MV(volume,) IF_MD(drive,) pinfo[i].start))
             {
                 mounted++;
@@ -379,7 +429,7 @@ int disk_mount(int drive)
                 volume_onmount_internal(IF_MV(volume));
                 volume = get_free_volume(); /* prepare next entry */
             }
-        #endif /* MAX_LOG_SECTOR_SIZE */
+#endif /* MAX_LOG_SECTOR_SIZE */
         }
     }
 
diff --git a/firmware/common/rb-loader.c b/firmware/common/rb-loader.c
index 61d8b1ddd2..2f5e06e165 100644
--- a/firmware/common/rb-loader.c
+++ b/firmware/common/rb-loader.c
@@ -30,6 +30,9 @@
 #include "multiboot.h"
 #endif
 
+#ifdef HAVE_DEVICEDATA
+#include "devicedata.h"
+#endif
 /* loads a firmware file from supplied filename
  * file opened, checks firmware size and checksum
  * if no error, firmware loaded to supplied buffer
@@ -118,7 +121,6 @@ int load_firmware(unsigned char* buf, const char* firmware, int buffer_size)
             /* if ret is valid breaks from loop to continue loading */
         }
 #endif
-
         if (ret < 0) /* Check default volume, no valid firmware file loaded yet */
         {
             /* First check in BOOTDIR */
@@ -141,5 +143,9 @@ int load_firmware(unsigned char* buf, const char* firmware, int buffer_size)
     else /* full path passed ROLO etc.*/
         ret = load_firmware_filename(buf, firmware, buffer_size);
 
+#ifdef HAVE_DEVICEDATA
+        write_devicedata(buf, ret);
+#endif
+
     return ret;
 }
diff --git a/firmware/drivers/ata-common.c b/firmware/drivers/ata-common.c
new file mode 100644
index 0000000000..53a7780262
--- /dev/null
+++ b/firmware/drivers/ata-common.c
@@ -0,0 +1,238 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ *
+ * Copyright (C) 2024 Solomon Peachy
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+
+/* This is intended to be #included into the ATA driver */
+
+#ifdef MAX_PHYS_SECTOR_SIZE
+
+struct sector_cache_entry {
+    unsigned char data[MAX_PHYS_SECTOR_SIZE];
+    sector_t sectornum;  /* logical sector */
+    bool inuse;
+};
+/* buffer for reading and writing large physical sectors */
+static struct sector_cache_entry sector_cache STORAGE_ALIGN_ATTR;
+static int phys_sector_mult = 1;
+
+static int cache_sector(sector_t sector)
+{
+    int rc;
+
+    /* round down to physical sector boundary */
+    sector &= ~(phys_sector_mult - 1);
+
+    /* check whether the sector is already cached */
+    if (sector_cache.inuse && (sector_cache.sectornum == sector))
+        return 0;
+
+    /* not found: read the sector */
+    sector_cache.inuse = false;
+    rc = ata_transfer_sectors(sector, phys_sector_mult, sector_cache.data, false);
+    if (!rc)
+    {
+        sector_cache.sectornum = sector;
+        sector_cache.inuse = true;
+    }
+    return rc;
+}
+
+static inline int flush_current_sector(void)
+{
+    return ata_transfer_sectors(sector_cache.sectornum, phys_sector_mult,
+                                sector_cache.data, true);
+}
+
+int ata_read_sectors(IF_MD(int drive,)
+                     sector_t start,
+                     int incount,
+                     void* inbuf)
+{
+    int rc = 0;
+    int offset;
+
+#ifdef HAVE_MULTIDRIVE
+    (void)drive; /* unused for now */
+#endif
+    mutex_lock(&ata_mutex);
+
+    offset = start & (phys_sector_mult - 1);
+
+    if (offset) /* first partial sector */
+    {
+        int partcount = MIN(incount, phys_sector_mult - offset);
+
+        rc = cache_sector(start);
+        if (rc)
+        {
+            rc = rc * 10 - 1;
+            goto error;
+        }
+        memcpy(inbuf, sector_cache.data + offset * SECTOR_SIZE,
+               partcount * SECTOR_SIZE);
+
+        start += partcount;
+        inbuf += partcount * SECTOR_SIZE;
+        incount -= partcount;
+    }
+    if (incount)
+    {
+        offset = incount & (phys_sector_mult - 1);
+        incount -= offset;
+
+        if (incount)
+        {
+            rc = ata_transfer_sectors(start, incount, inbuf, false);
+            if (rc)
+            {
+                rc = rc * 10 - 2;
+                goto error;
+            }
+            start += incount;
+            inbuf += incount * SECTOR_SIZE;
+        }
+        if (offset)
+        {
+            rc = cache_sector(start);
+            if (rc)
+            {
+                rc = rc * 10 - 3;
+                goto error;
+            }
+            memcpy(inbuf, sector_cache.data, offset * SECTOR_SIZE);
+        }
+    }
+
+  error:
+    mutex_unlock(&ata_mutex);
+
+    return rc;
+}
+
+int ata_write_sectors(IF_MD(int drive,)
+                      sector_t start,
+                      int count,
+                      const void* buf)
+{
+    int rc = 0;
+    int offset;
+
+#ifdef HAVE_MULTIDRIVE
+    (void)drive; /* unused for now */
+#endif
+    mutex_lock(&ata_mutex);
+
+    offset = start & (phys_sector_mult - 1);
+
+    if (offset) /* first partial sector */
+    {
+        int partcount = MIN(count, phys_sector_mult - offset);
+
+        rc = cache_sector(start);
+        if (rc)
+        {
+            rc = rc * 10 - 1;
+            goto error;
+        }
+        memcpy(sector_cache.data + offset * SECTOR_SIZE, buf,
+               partcount * SECTOR_SIZE);
+        rc = flush_current_sector();
+        if (rc)
+        {
+            rc = rc * 10 - 2;
+            goto error;
+        }
+        start += partcount;
+        buf += partcount * SECTOR_SIZE;
+        count -= partcount;
+    }
+    if (count)
+    {
+        offset = count & (phys_sector_mult - 1);
+        count -= offset;
+
+        if (count)
+        {
+            rc = ata_transfer_sectors(start, count, (void*)buf, true);
+            if (rc)
+            {
+                rc = rc * 10 - 3;
+                goto error;
+            }
+            start += count;
+            buf += count * SECTOR_SIZE;
+        }
+        if (offset)
+        {
+            rc = cache_sector(start);
+            if (rc)
+            {
+                rc = rc * 10 - 4;
+                goto error;
+            }
+            memcpy(sector_cache.data, buf, offset * SECTOR_SIZE);
+            rc = flush_current_sector();
+            if (rc)
+            {
+                rc = rc * 10 - 5;
+                goto error;
+            }
+        }
+    }
+
+  error:
+    mutex_unlock(&ata_mutex);
+
+    return rc;
+}
+
+static int ata_get_phys_sector_mult(void)
+{
+    int rc = 0;
+
+    /* Find out the physical sector size */
+    if((identify_info[106] & 0xe000) == 0x6000) /* B14, B13 */
+        phys_sector_mult = BIT_N(identify_info[106] & 0x000f);
+    else
+        phys_sector_mult = 1;
+
+    DEBUGF("ata: %d logical sectors per phys sector", phys_sector_mult);
+
+    if (phys_sector_mult > 1)
+    {
+        /* Check if drive really needs emulation  - if we can access
+           sector 1 then assume the drive supports "512e" and will handle
+           it better than us, so ignore the large physical sectors.
+        */
+        char throwaway[SECTOR_SIZE];
+        rc = ata_transfer_sectors(1, 1, &throwaway, false);
+        if (rc == 0)
+            phys_sector_mult = 1;
+    }
+
+    if (phys_sector_mult > (MAX_PHYS_SECTOR_SIZE/SECTOR_SIZE))
+        panicf("Unsupported physical sector size: %d",
+               phys_sector_mult * SECTOR_SIZE);
+
+    memset(&sector_cache, 0, sizeof(sector_cache));
+
+    return 0;
+}
+
+#endif  /* MAX_PHYS_SECTOR_SIZE */
diff --git a/firmware/drivers/ata.c b/firmware/drivers/ata.c
index 7b9c4910cf..7b43d3c536 100644
--- a/firmware/drivers/ata.c
+++ b/firmware/drivers/ata.c
@@ -90,7 +90,7 @@ enum {
 
 static int ata_state = ATA_BOOT;
 
-static struct mutex ata_mtx SHAREDBSS_ATTR;
+static struct mutex ata_mutex SHAREDBSS_ATTR;
 static int ata_device; /* device 0 (master) or 1 (slave) */
 
 static int spinup_time = 0;
@@ -101,8 +101,9 @@ static bool ata_led_on = false;
 
 static long sleep_timeout = 5*HZ;
 #ifdef HAVE_LBA48
-static bool lba48 = false; /* set for 48 bit addressing */
+static bool ata_lba48 = false; /* set for 48 bit addressing */
 #endif
+static bool canflush = true;
 
 static long last_disk_activity = -1;
 #ifdef HAVE_ATA_POWER_OFF
@@ -111,20 +112,8 @@ static long power_off_tick = 0;
 
 static sector_t total_sectors;
 static int multisectors; /* number of supported multisectors */
-static unsigned short identify_info[ATA_IDENTIFY_WORDS];
 
-#ifdef MAX_PHYS_SECTOR_SIZE
-
-struct sector_cache_entry {
-    bool inuse;
-    sector_t sectornum;  /* logical sector */
-    unsigned char data[MAX_PHYS_SECTOR_SIZE];
-};
-/* buffer for reading and writing large physical sectors */
-#define NUMCACHES 2
-static struct sector_cache_entry sector_cache;
-static int phys_sector_mult = 1;
-#endif
+static unsigned short identify_info[ATA_IDENTIFY_WORDS] STORAGE_ALIGN_ATTR;
 
 #ifdef HAVE_ATA_DMA
 static int dma_mode = 0;
@@ -142,11 +131,6 @@ static inline void keep_ata_active(void)
     last_disk_activity = current_tick;
 }
 
-static inline void schedule_ata_sleep(long from_now)
-{
-    last_disk_activity = current_tick - sleep_timeout + from_now;
-}
-
 static inline bool ata_sleep_timed_out(void)
 {
     return sleep_timeout &&
@@ -224,7 +208,7 @@ static int ata_perform_wakeup(int state)
 static int ata_perform_sleep(void)
 {
     /* If device doesn't support PM features, don't try to sleep. */
-    if (!ata_disk_can_poweroff())
+    if (!ata_disk_can_sleep())
         return 0; // XXX or return a failure?
 
     logf("ata SLEEP %ld", current_tick);
@@ -258,13 +242,18 @@ static int ata_perform_flush_cache(void)
 {
     uint8_t cmd;
 
-    if (identify_info[83] & (1 << 13)) {
-        cmd = CMD_FLUSH_CACHE_EXT;
+    if (!canflush) {
+        return 0;
+    } else if (ata_lba48 && identify_info[83] & (1 << 13)) {
+        cmd = CMD_FLUSH_CACHE_EXT;  /* Flag, optional, ATA-6 and up, for use with LBA48 devices */
     } else if (identify_info[83] & (1 << 12)) {
-        cmd = CMD_FLUSH_CACHE;
+        cmd = CMD_FLUSH_CACHE; /* Flag, mandatory, ATA-6 and up */
+    } else if (identify_info[80] >= (1 << 5)) { /* Use >= instead of '&' because bits lower than the latest standard we support don't have to be set */
+        cmd = CMD_FLUSH_CACHE; /* No flag, mandatory, ATA-5  (Optional for ATA-4) */
     } else {
         /* If neither (mandatory!) command is supported
            then don't issue it. */
+       canflush = 0;
        return 0;
     }
 
@@ -287,6 +276,16 @@ static int ata_perform_flush_cache(void)
     return 0;
 }
 
+int ata_flush(void)
+{
+    if (ata_state >= ATA_SPINUP) {
+        mutex_lock(&ata_mutex);
+        ata_perform_flush_cache();
+        mutex_unlock(&ata_mutex);
+    }
+    return 0;
+}
+
 static ICODE_ATTR int wait_for_start_of_transfer(void)
 {
     if (!wait_for_bsy())
@@ -437,7 +436,7 @@ static int ata_transfer_sectors(uint64_t start,
 #endif
 
 #ifdef HAVE_LBA48
-        if (lba48)
+        if (ata_lba48)
         {
             ATA_OUT8(ATA_NSECTOR, count >> 8);
             ATA_OUT8(ATA_NSECTOR, count & 0xff);
@@ -464,7 +463,7 @@ static int ata_transfer_sectors(uint64_t start,
             ATA_OUT8(ATA_SECTOR, start & 0xff);
             ATA_OUT8(ATA_LCYL, (start >> 8) & 0xff);
             ATA_OUT8(ATA_HCYL, (start >> 16) & 0xff);
-            ATA_OUT8(ATA_SELECT, ((start >> 24) & 0xf) | SELECT_LBA | ata_device);
+            ATA_OUT8(ATA_SELECT, ((start >> 24) & 0xf) | SELECT_LBA | ata_device);  /* LBA28, mask off upper 4 bits of 32-bit sector address */
 #ifdef HAVE_ATA_DMA
             if (write)
                 ATA_OUT8(ATA_COMMAND, usedma ? CMD_WRITE_DMA : CMD_WRITE_MULTIPLE);
@@ -590,6 +589,8 @@ static int ata_transfer_sectors(uint64_t start,
     return ret;
 }
 
+#include "ata-common.c"
+
 #ifndef MAX_PHYS_SECTOR_SIZE
 int ata_read_sectors(IF_MD(int drive,)
                      sector_t start,
@@ -600,9 +601,9 @@ int ata_read_sectors(IF_MD(int drive,)
     (void)drive; /* unused for now */
 #endif
 
-    mutex_lock(&ata_mtx);
+    mutex_lock(&ata_mutex);
     int rc = ata_transfer_sectors(start, incount, inbuf, false);
-    mutex_unlock(&ata_mtx);
+    mutex_unlock(&ata_mutex);
     return rc;
 }
 
@@ -615,186 +616,13 @@ int ata_write_sectors(IF_MD(int drive,)
     (void)drive; /* unused for now */
 #endif
 
-    mutex_lock(&ata_mtx);
+    mutex_lock(&ata_mutex);
     int rc = ata_transfer_sectors(start, count, (void*)buf, true);
-    mutex_unlock(&ata_mtx);
+    mutex_unlock(&ata_mutex);
     return rc;
 }
 #endif /* ndef MAX_PHYS_SECTOR_SIZE */
 
-#ifdef MAX_PHYS_SECTOR_SIZE
-static int cache_sector(sector_t sector)
-{
-    int rc;
-
-    sector &= ~(phys_sector_mult - 1);
-              /* round down to physical sector boundary */
-
-    /* check whether the sector is already cached */
-    if (sector_cache.inuse && (sector_cache.sectornum == sector))
-        return 0;
-
-    /* not found: read the sector */
-    sector_cache.inuse = false;
-    rc = ata_transfer_sectors(sector, phys_sector_mult, sector_cache.data, false);
-    if (!rc)
-    {
-        sector_cache.sectornum = sector;
-        sector_cache.inuse = true;
-    }
-    return rc;
-}
-
-static inline int flush_current_sector(void)
-{
-    return ata_transfer_sectors(sector_cache.sectornum, phys_sector_mult,
-                                sector_cache.data, true);
-}
-
-int ata_read_sectors(IF_MD(int drive,)
-                     sector_t start,
-                     int incount,
-                     void* inbuf)
-{
-    int rc = 0;
-    int offset;
-
-#ifdef HAVE_MULTIDRIVE
-    (void)drive; /* unused for now */
-#endif
-    mutex_lock(&ata_mtx);
-
-    offset = start & (phys_sector_mult - 1);
-
-    if (offset) /* first partial sector */
-    {
-        int partcount = MIN(incount, phys_sector_mult - offset);
-
-        rc = cache_sector(start);
-        if (rc)
-        {
-            rc = rc * 10 - 1;
-            goto error;
-        }
-        memcpy(inbuf, sector_cache.data + offset * SECTOR_SIZE,
-               partcount * SECTOR_SIZE);
-
-        start += partcount;
-        inbuf += partcount * SECTOR_SIZE;
-        incount -= partcount;
-    }
-    if (incount)
-    {
-        offset = incount & (phys_sector_mult - 1);
-        incount -= offset;
-
-        if (incount)
-        {
-            rc = ata_transfer_sectors(start, incount, inbuf, false);
-            if (rc)
-            {
-                rc = rc * 10 - 2;
-                goto error;
-            }
-            start += incount;
-            inbuf += incount * SECTOR_SIZE;
-        }
-        if (offset)
-        {
-            rc = cache_sector(start);
-            if (rc)
-            {
-                rc = rc * 10 - 3;
-                goto error;
-            }
-            memcpy(inbuf, sector_cache.data, offset * SECTOR_SIZE);
-        }
-    }
-
-  error:
-    mutex_unlock(&ata_mtx);
-
-    return rc;
-}
-
-int ata_write_sectors(IF_MD(int drive,)
-                      sector_t start,
-                      int count,
-                      const void* buf)
-{
-    int rc = 0;
-    int offset;
-
-#ifdef HAVE_MULTIDRIVE
-    (void)drive; /* unused for now */
-#endif
-    mutex_lock(&ata_mtx);
-
-    offset = start & (phys_sector_mult - 1);
-
-    if (offset) /* first partial sector */
-    {
-        int partcount = MIN(count, phys_sector_mult - offset);
-
-        rc = cache_sector(start);
-        if (rc)
-        {
-            rc = rc * 10 - 1;
-            goto error;
-        }
-        memcpy(sector_cache.data + offset * SECTOR_SIZE, buf,
-               partcount * SECTOR_SIZE);
-        rc = flush_current_sector();
-        if (rc)
-        {
-            rc = rc * 10 - 2;
-            goto error;
-        }
-        start += partcount;
-        buf += partcount * SECTOR_SIZE;
-        count -= partcount;
-    }
-    if (count)
-    {
-        offset = count & (phys_sector_mult - 1);
-        count -= offset;
-
-        if (count)
-        {
-            rc = ata_transfer_sectors(start, count, (void*)buf, true);
-            if (rc)
-            {
-                rc = rc * 10 - 3;
-                goto error;
-            }
-            start += count;
-            buf += count * SECTOR_SIZE;
-        }
-        if (offset)
-        {
-            rc = cache_sector(start);
-            if (rc)
-            {
-                rc = rc * 10 - 4;
-                goto error;
-            }
-            memcpy(sector_cache.data, buf, offset * SECTOR_SIZE);
-            rc = flush_current_sector();
-            if (rc)
-            {
-                rc = rc * 10 - 5;
-                goto error;
-            }
-        }
-    }
-
-  error:
-    mutex_unlock(&ata_mtx);
-
-    return rc;
-}
-#endif /* MAX_PHYS_SECTOR_SIZE */
-
 static int STORAGE_INIT_ATTR check_registers(void)
 {
     int i;
@@ -845,24 +673,25 @@ void ata_spindown(int seconds)
 
 bool ata_disk_is_active(void)
 {
-    return ata_state >= ATA_SPINUP;
+    return (ata_state >= ATA_SPINUP);
 }
 
 void ata_sleepnow(void)
 {
     if (ata_state >= ATA_SPINUP) {
         logf("ata SLEEPNOW %ld", current_tick);
-        mutex_lock(&ata_mtx);
+        mutex_lock(&ata_mutex);
         if (ata_state == ATA_ON) {
             if (!ata_perform_flush_cache() && !ata_perform_sleep()) {
                 ata_state = ATA_SLEEPING;
 #ifdef HAVE_ATA_POWER_OFF
-                if (ata_disk_can_poweroff())
+                if (ata_disk_can_sleep() || canflush) {
                     power_off_tick = current_tick + ATA_POWER_OFF_TIMEOUT;
+                }
 #endif
             }
         }
-        mutex_unlock(&ata_mtx);
+        mutex_unlock(&ata_mutex);
     }
 }
 
@@ -880,7 +709,7 @@ static int STORAGE_INIT_ATTR ata_hard_reset(void)
 {
     int ret;
 
-    mutex_lock(&ata_mtx);
+    mutex_lock(&ata_mutex);
 
     ata_reset();
 
@@ -891,7 +720,7 @@ static int STORAGE_INIT_ATTR ata_hard_reset(void)
     /* Massage the return code so it is 0 on success and -1 on failure */
     ret = ret?0:-1;
 
-    mutex_unlock(&ata_mtx);
+    mutex_unlock(&ata_mutex);
 
     return ret;
 }
@@ -968,6 +797,9 @@ static int perform_soft_reset(void)
     if (set_multiple_mode(multisectors))
         return -3;
 
+    if (identify())
+        return -2;
+
     if (freeze_lock())
         return -4;
 
@@ -978,13 +810,13 @@ int ata_soft_reset(void)
 {
     int ret = -6;
 
-    mutex_lock(&ata_mtx);
+    mutex_lock(&ata_mutex);
 
     if (ata_state > ATA_OFF) {
         ret = perform_soft_reset();
     }
 
-    mutex_unlock(&ata_mtx);
+    mutex_unlock(&ata_mutex);
     return ret;
 }
 
@@ -1018,6 +850,9 @@ static int ata_power_on(void)
     if (set_multiple_mode(multisectors))
         return -3;
 
+    if (identify())
+        return -2;
+
     if (freeze_lock())
         return -4;
 
@@ -1068,7 +903,7 @@ static int set_multiple_mode(int sectors)
 }
 
 #ifdef HAVE_ATA_DMA
-static int get_best_mode(unsigned short identword, int max, int modetype)
+static int ata_get_best_mode(unsigned short identword, int max, int modetype)
 {
     unsigned short testbit = BIT_N(max);
 
@@ -1117,13 +952,18 @@ static int set_features(void)
 
 #ifdef HAVE_ATA_DMA
     if (identify_info[53] & (1<<2)) {
+        int max_udma = ATA_MAX_UDMA;
+#if ATA_MAX_UDMA > 2
+        if (!(identify_info[93] & (1<<13)))
+            max_udma = 2;
+#endif
         /* Ultra DMA mode info present, find a mode */
-        dma_mode = get_best_mode(identify_info[88], ATA_MAX_UDMA, 0x40);
+        dma_mode = ata_get_best_mode(identify_info[88], max_udma, 0x40);
     }
 
     if (!dma_mode) {
         /* No UDMA mode found, try to find a multi-word DMA mode */
-        dma_mode = get_best_mode(identify_info[63], ATA_MAX_MWDMA, 0x20);
+        dma_mode = ata_get_best_mode(identify_info[63], ATA_MAX_MWDMA, 0x20);
         features[1].id_word = 63;
     } else {
         features[1].id_word = 88;
@@ -1210,19 +1050,16 @@ int STORAGE_INIT_ATTR ata_init(void)
     bool coldstart;
 
     if (ata_state == ATA_BOOT) {
-        mutex_init(&ata_mtx);
+        mutex_init(&ata_mutex);
     }
 
-    mutex_lock(&ata_mtx);
+    mutex_lock(&ata_mutex);
 
     /* must be called before ata_device_init() */
     coldstart = ata_is_coldstart();
     ata_led(false);
     ata_device_init();
     ata_enable(true);
-#ifdef MAX_PHYS_SECTOR_SIZE
-    memset(&sector_cache, 0, sizeof(sector_cache));
-#endif
 
     if (ata_state == ATA_BOOT) {
         ata_state = ATA_OFF;
@@ -1251,7 +1088,6 @@ int STORAGE_INIT_ATTR ata_init(void)
         }
 
         rc = identify();
-
         if (rc) {
             rc = -40 + rc;
             goto error;
@@ -1263,57 +1099,37 @@ int STORAGE_INIT_ATTR ata_init(void)
 
         DEBUGF("ata: %d sectors per ata request\n",multisectors);
 
-        total_sectors = identify_info[60] | (identify_info[61] << 16);
+        total_sectors = (identify_info[61] << 16) | identify_info[60];
 
 #ifdef HAVE_LBA48
-        if (identify_info[83] & 0x0400       /* 48 bit address support */
-            && total_sectors == 0x0FFFFFFF)  /* and disk size >= 128 GiB */
-        {                                    /* (needs BigLBA addressing) */
-            total_sectors = identify_info[100] | (identify_info[101] << 16) | ((uint64_t)identify_info[102] << 32) | ((uint64_t)identify_info[103] << 48);
-
-            lba48 = true; /* use BigLBA */
+        if (identify_info[83] & 0x0400 && total_sectors == 0x0FFFFFFF) {
+            total_sectors = ((uint64_t)identify_info[103] << 48) |
+                    ((uint64_t)identify_info[102] << 32) |
+                    ((uint64_t)identify_info[101] << 16) |
+                    identify_info[100];
+            ata_lba48 = true; /* use BigLBA */
         }
 #endif /* HAVE_LBA48 */
 
         rc = freeze_lock();
-
         if (rc) {
             rc = -50 + rc;
             goto error;
         }
 
-        rc = set_features(); // rror codes are between -1 and -49
+        rc = set_features(); // error codes are between -1 and -49
         if (rc) {
             rc = -60 + rc;
             goto error;
         }
 
 #ifdef MAX_PHYS_SECTOR_SIZE
-        /* Find out the physical sector size */
-        if((identify_info[106] & 0xe000) == 0x6000)
-            phys_sector_mult = BIT_N(identify_info[106] & 0x000f);
-        else
-            phys_sector_mult = 1;
-
-        DEBUGF("ata: %d logical sectors per phys sector", phys_sector_mult);
-
-        if (phys_sector_mult > 1)
-        {
-            /* Check if drive really needs emulation  - if we can access
-             * sector 1 then assume the drive will handle it better than
-             * us, and ignore the large physical sectors.
-             */
-            char throwaway[SECTOR_SIZE];
-            rc = ata_transfer_sectors(1, 1, &throwaway, false);
-            if (rc == 0)
-                phys_sector_mult = 1;
+        rc = ata_get_phys_sector_mult();
+        if (rc) {
+            rc = -70 + rc;
+            goto error;
         }
-
-        if (phys_sector_mult > (MAX_PHYS_SECTOR_SIZE/SECTOR_SIZE))
-            panicf("Unsupported physical sector size: %d",
-                   phys_sector_mult * SECTOR_SIZE);
-#endif /* MAX_PHYS_SECTOR_SIZE */
-
+#endif
         ata_state = ATA_ON;
         keep_ata_active();
     }
@@ -1321,8 +1137,14 @@ int STORAGE_INIT_ATTR ata_init(void)
     if (rc)
         rc = -100 + rc;
 
+    rc = identify();
+    if (rc) {
+        rc = -40 + rc;
+        goto error;
+    }
+
 error:
-    mutex_unlock(&ata_mtx);
+    mutex_unlock(&ata_mutex);
     return rc;
 }
 
@@ -1359,9 +1181,9 @@ void ata_get_info(IF_MD(int drive,)struct storage_info *info)
 #endif
     int i;
 
-    /* Logical sector size */
-    if ((identify_info[106] & 0xd000) == 0x5000)
-        info->sector_size = identify_info[117] | (identify_info[118] << 16);
+    /* Logical sector size > 512B ? */
+    if ((identify_info[106] & 0xd000) == 0x5000) /* B14, B12 */
+        info->sector_size = (identify_info[117] | (identify_info[118] << 16)) * 2;
     else
         info->sector_size = SECTOR_SIZE;
 
@@ -1422,11 +1244,11 @@ int ata_event(long id, intptr_t data)
 #ifdef HAVE_ATA_POWER_OFF
             if (ata_state == ATA_SLEEPING && ata_power_off_timed_out()) {
                 power_off_tick = 0;
-                mutex_lock(&ata_mtx);
+                mutex_lock(&ata_mutex);
                 logf("ata OFF %ld", current_tick);
                 ide_power_enable(false);
                 ata_state = ATA_OFF;
-                mutex_unlock(&ata_mtx);
+                mutex_unlock(&ata_mutex);
             }
 #endif
             STG_EVENT_ASSERT_ACTIVE(STORAGE_ATA);
@@ -1436,7 +1258,7 @@ int ata_event(long id, intptr_t data)
         ata_sleepnow();
     }
     else if (id == Q_STORAGE_SLEEP) {
-        schedule_ata_sleep(HZ/5);
+        last_disk_activity = current_tick - sleep_timeout + HZ / 5;
     }
 #ifndef USB_NONE
     else if (id == SYS_USB_CONNECTED) {
@@ -1446,7 +1268,7 @@ int ata_event(long id, intptr_t data)
             STG_EVENT_ASSERT_ACTIVE(STORAGE_ATA);
         }
         else {
-            mutex_lock(&ata_mtx);
+            mutex_lock(&ata_mutex);
             if (ata_state < ATA_ON) {
                 ata_led(true);
                 if (!(rc = ata_perform_wakeup(ata_state))) {
@@ -1454,7 +1276,7 @@ int ata_event(long id, intptr_t data)
                 }
                 ata_led(false);
             }
-            mutex_unlock(&ata_mtx);
+            mutex_unlock(&ata_mutex);
         }
     }
 #endif /* ndef USB_NONE */
diff --git a/firmware/drivers/audio/erosqlinux_codec.c b/firmware/drivers/audio/erosqlinux_codec.c
index 02c35e3c00..d1b3e884d3 100644
--- a/firmware/drivers/audio/erosqlinux_codec.c
+++ b/firmware/drivers/audio/erosqlinux_codec.c
@@ -53,11 +53,29 @@
    BUFFER_BYTES: [4096 524288]
    TICK_TIME: ALL
 
- Mixer controls:
+ Mixer controls (v1):
 
      numid=1,iface=MIXER,name='Output Port Switch'
       ; type=INTEGER,access=rw------,values=1,min=0,max=5,step=0
       : values=4
+
+  Mixer controls (v2+):
+
+     numid=3,iface=MIXER,name='ES9018_K2M Digital Filter'
+       ; type=INTEGER,access=rw------,values=1,min=0,max=4,step=0
+       : values=0
+     numid=1,iface=MIXER,name='Left Playback Volume'
+       ; type=INTEGER,access=rw------,values=1,min=0,max=255,step=0
+       : values=0
+     numid=4,iface=MIXER,name='Output Port Switch'
+       ; type=INTEGER,access=rw------,values=1,min=0,max=5,step=0
+       : values=0
+     numid=2,iface=MIXER,name='Right Playback Volume'
+       ; type=INTEGER,access=rw------,values=1,min=0,max=255,step=0
+       : values=0
+     numid=5,iface=MIXER,name='isDSD'
+       ; type=BOOLEAN,access=rw------,values=1
+       : values=off
 */
 
 static int hw_init = 0;
@@ -68,6 +86,8 @@ static long int last_ps = -1;
 
 static int muted = -1;
 
+extern int hwver;
+
 void audiohw_mute(int mute)
 {
     logf("mute %d", mute);
@@ -129,6 +149,13 @@ void audiohw_preinit(void)
     logf("hw preinit");
     alsa_controls_init("default");
     hw_init = 1;
+
+    /* See if we have hw2 or later */
+    if (alsa_controls_find("Left Playback Volume") == -1)
+        hwver = 1;
+    else if (hwver == 1)
+        hwver = 23;
+
     audiohw_mute(false);  /* No need to stay muted */
 }
 
@@ -154,10 +181,8 @@ void audiohw_set_frequency(int fsel)
 const int min_pcm = -740;
 const int max_pcm = 0;
 
-void audiohw_set_volume(int vol_l, int vol_r)
+static void audiohw_set_volume_v1(int vol_l, int vol_r)
 {
-    logf("hw vol %d %d", vol_l, vol_r);
-
     long l,r;
 
     vol_l_hw = vol_l;
@@ -180,6 +205,39 @@ void audiohw_set_volume(int vol_l, int vol_r)
     pcm_set_mixer_volume(sw_volume_l / 20, sw_volume_r / 20);
 }
 
+static void audiohw_set_volume_v2(int vol_l, int vol_r)
+{
+    long l,r;
+
+    if (lineout_inserted()) {
+        vol_l_hw = vol_r_hw = global_settings.volume_limit * 10;
+    } else {
+        vol_l_hw = -vol_l;
+        vol_r_hw = -vol_r;
+    }
+
+    if (!hw_init)
+       return;
+
+    l = vol_l_hw / 5;
+    r = vol_l_hw / 5;
+
+    alsa_controls_set_ints("Left Playback Volume", 1, &l);
+    alsa_controls_set_ints("Right Playback Volume", 1, &r);
+
+    /* Dial back PCM mixer to avoid compression */
+    pcm_set_mixer_volume(global_settings.volume_limit / 2, global_settings.volume_limit / 2);
+}
+
+void audiohw_set_volume(int vol_l, int vol_r)
+{
+    if (hwver >= 2) {
+        audiohw_set_volume_v2(vol_l, vol_r);
+    } else {
+        audiohw_set_volume_v1(vol_l, vol_r);
+    }
+}
+
 void audiohw_set_lineout_volume(int vol_l, int vol_r)
 {
     long l,r;
@@ -196,7 +254,16 @@ void audiohw_set_lineout_volume(int vol_l, int vol_r)
         r = vol_r_hw;
     }
 
-    int sw_volume_l = l <= min_pcm ? min_pcm : MIN(l, max_pcm);
-    int sw_volume_r = r <= min_pcm ? min_pcm : MIN(r, max_pcm);
-    pcm_set_mixer_volume(sw_volume_l / 20, sw_volume_r / 20);
+    if (hwver >= 2) {
+         if (hw_init) {
+             l /= 5;
+             r /= 5;
+             alsa_controls_set_ints("Left Playback Volume", 1, &l);
+             alsa_controls_set_ints("Right Playback Volume", 1, &r);
+         }
+    } else {
+        int sw_volume_l = l <= min_pcm ? min_pcm : MIN(l, max_pcm);
+        int sw_volume_r = r <= min_pcm ? min_pcm : MIN(r, max_pcm);
+        pcm_set_mixer_volume(sw_volume_l / 20, sw_volume_r / 20);
+    }
 }
diff --git a/firmware/drivers/lcd-scroll.c b/firmware/drivers/lcd-scroll.c
index 895cf98cba..2a58d6ff21 100644
--- a/firmware/drivers/lcd-scroll.c
+++ b/firmware/drivers/lcd-scroll.c
@@ -195,8 +195,14 @@ static void LCDFN(scroll_worker)(void)
         s = &si->scroll[index];
 
         /* check pause */
-        if (TIME_BEFORE(current_tick, s->start_tick))
+        if (TIME_BEFORE(current_tick, s->start_tick)) {
             continue;
+        }
+
+        if (global_settings.disable_mainmenu_scrolling && get_current_activity() == ACTIVITY_MAINMENU) {
+            // No scrolling on the main menu if disabled (to not break themes with lockscreens)
+            continue;
+        }
 
         s->start_tick = current_tick;
 
diff --git a/firmware/drivers/ramdisk.c b/firmware/drivers/ramdisk.c
index cecc943352..5798de252e 100644
--- a/firmware/drivers/ramdisk.c
+++ b/firmware/drivers/ramdisk.c
@@ -22,9 +22,10 @@
 #include <stdbool.h>
 #include <string.h>
 
+#include "config.h"
+#include "fs_defines.h"
 #include "storage.h"
 
-#define SECTOR_SIZE 512
 #define NUM_SECTORS 16384
 
 static unsigned char ramdisk[SECTOR_SIZE * NUM_SECTORS];
diff --git a/firmware/drivers/rtc/rtc_pcf50606.c b/firmware/drivers/rtc/rtc_pcf50606.c
index 16ae5a3e9c..8f5371635c 100644
--- a/firmware/drivers/rtc/rtc_pcf50606.c
+++ b/firmware/drivers/rtc/rtc_pcf50606.c
@@ -57,7 +57,7 @@ int rtc_read_datetime(struct tm *tm)
     tm->tm_mday = buf[4];
     tm->tm_mon = buf[5] - 1;
 
-#ifdef IRIVER_H300_SERIES 
+#ifdef IRIVER_H300_SERIES
     /* Special kludge to coexist with the iriver firmware. The iriver firmware
        stores the date as 1965+nn, and allows a range of 1980..2064. We use
        1964+nn here to make leap years work correctly, so the date will be one
@@ -158,7 +158,7 @@ bool rtc_check_alarm_started(bool release_alarm)
 
     if (run_before) {
         rc = alarm_state;
-        alarm_state &= ~release_alarm;
+        alarm_state &= !release_alarm;
     } else {
         char rt[3], at[3];
         /* The Ipod bootloader seems to read (and thus clear) the PCF interrupt
diff --git a/firmware/drivers/tuner/rda5802.c b/firmware/drivers/tuner/rda5802.c
index 377bdd1e6f..55e775e985 100644
--- a/firmware/drivers/tuner/rda5802.c
+++ b/firmware/drivers/tuner/rda5802.c
@@ -42,17 +42,17 @@
 #define RSSI_MAX 70
 
 /** Registers and bits **/
+#define CHIPID      0x0
 #define POWERCFG    0x2
 #define CHANNEL     0x3
 #define SYSCONFIG1  0x4
 #define SYSCONFIG2  0x5
 #define SYSCONFIG3  0x6
-#define SYSCONFIG4  0x7 /* undocumented */
-#define SYSCONFIG5  0x8 /* undocumented */
-#define SYSCONFIG6  0x9 /* undocumented */
+#define SYSCONFIG4  0x7
+#define SYSCONFIG5  0x8
+#define SYSCONFIG6  0x9 /* suspected not to exists */
 #define READCHAN    0xA
 #define STATUSRSSI  0xB
-#define IDENT       0xC
 
 
 /* POWERCFG (0x2) */
@@ -81,7 +81,7 @@
 
 /* SYSCONFIG1 (0x4) */
 #define SYSCONFIG1_DE       (0x1 << 11)
-    #define SYSCONFIG1_SOFTMUTE_EN  (0x1 << 9)
+#define SYSCONFIG1_SOFTMUTE_EN  (0x1 << 9)
 
 /* SYSCONFIG2 (0x5) */
 #define SYSCONFIG2_VOLUME   (0xF <<  0)
@@ -105,9 +105,9 @@ static uint16_t cache[16] = {
     [SYSCONFIG1] =  0x0200, /* SYSCONFIG1_SOFTMUTE_EN */
     [SYSCONFIG2] =  0x867F, /* INT_MODE (def), SEEKTH=1100b, LNA_PORT_SEL=LNAN,
                                LNA_ICSEL=3.0mA, VOLUME=max */
-    [SYSCONFIG3] =  0x8000, /* I2S slave mode */
-    [SYSCONFIG4] =  0x4712, /* undocumented, affects stereo blend */
-    [SYSCONFIG5] =  0x5EC6, /* undocumented */
+    [SYSCONFIG3] =  0x8000, /* RSVD */
+    [SYSCONFIG4] =  0x4712, /* TH_SOFRBLEND=34dB, 65M_50M MODE=65~76MHz, RSVD, SEEK_TH_OLD=4, SOFTBLEND_EN */
+    [SYSCONFIG5] =  0x5EC6, /* FREQ_DIRECT=24262kHz (unused) */
     [SYSCONFIG6] =  0x0000  /* undocumented */
 };
 
@@ -190,7 +190,21 @@ static void rda5802_sleep(int snooze)
 
 bool rda5802_detect(void)
 {
-    return ((rda5802_read_reg(IDENT) & 0xFF00) == 0x5800);
+    /* The RDA5802 has a weird wrap-around at 0x40. Upon initialisation, it will copy the Chip ID
+     * to register 0xC but since this register is also used for RDS, we cannot rely on its content
+     * until we softreset. But we cannot soft-reset until we confirm the tuner type... So we really
+     * need to register 0, which means reading 0x40 registers.
+     * NOTE: the datasheet says that it wraps around at 0x3A but this is wrong. */
+
+    /* we want to read registers 0x00/0x01, aka 0x40/0x41 because of wrapping, tuner starts reading
+     * at 0xA so we need to read 0x40 - 0xA + 2 registers, each register is two bytes. Thats
+     * (0x40 - 0xA + 2) * 2 = 0x6e bytes */
+    unsigned char buf[0x70];
+    fmradio_i2c_read(I2C_ADR, buf, sizeof(buf));
+    cache[CHIPID] = buf[0x6c] << 8 | buf[0x6d];
+    cache[1] = buf[0x6e] << 8 | buf[0x6f]; /* unknown register, maybe firmware ID or related */
+
+    return ((cache[CHIPID] & 0xFF00) == 0x5800);
  }
 
 void rda5802_init(void)
diff --git a/firmware/export/ata.h b/firmware/export/ata.h
index 6165eaf633..dfa73eebf5 100644
--- a/firmware/export/ata.h
+++ b/firmware/export/ata.h
@@ -101,7 +101,7 @@ struct ata_smart_values
     unsigned char reserved_375_385[11];
     unsigned char vendor_specific_386_510[125];
     unsigned char chksum;
-} __attribute__((packed));
+} __attribute__((packed)) __attribute__((aligned(2)));
 
 /* Raw attribute value print formats */
 enum ata_attr_raw_format
@@ -176,7 +176,6 @@ static inline int ata_disk_isssd(void)
         0x0401 -> 0xffe == RPM
         All others reserved
 
-        Some CF cards return 0x0100 (ie byteswapped 0x0001) so accept either.
         However, this is a relatively recent change, and we can't rely on it,
         especially for the FC1307A CF->SD adapters!
 
@@ -199,19 +198,18 @@ static inline int ata_disk_isssd(void)
         then device is standard flash CF.  However this is not foolproof
         as newer CF cards (and those CF->SD adapters) may report this.
 
-
      */
-    return ( (identify_info[217] == 0x0001 || identify_info[217] == 0x0100) /* "Solid state" rotational rate */
-             || ((identify_info[168] & 0x0f) >= 0x06)                       /* Explicit SSD form factors */
-             || (identify_info[169] & (1<<0))                               /* TRIM supported */
-             || (identify_info[163] > 0)                                    /* CF Advanced timing modes */
-             || ((identify_info[83] & (1<<2)) &&                            /* CFA compliant */
-                 ((identify_info[160] & (1<<15)) == 0))                       /* CF power level 0 */
+    return ( (identify_info[217] == 0x0001)               /* "Solid state" rotational rate */
+             || ((identify_info[168] & 0x0f) >= 0x06)     /* Explicit SSD form factors */
+             || (identify_info[169] & (1<<0))             /* TRIM supported */
+             || (identify_info[163] > 0)                  /* CF Advanced timing modes */
+             || ((identify_info[83] & (1<<2)) &&          /* CFA compliant */
+                 ((identify_info[160] & (1<<15)) == 0))   /* CF power level 0 */
            );
 }
 
-/* Returns 1 if the drive can be powered off safely */
-static inline int ata_disk_can_poweroff(void)
+/* Returns 1 if the drive supports power management commands */
+static inline int ata_disk_can_sleep(void)
 {
     unsigned short *identify_info = ata_get_identify();
     /* Only devices that claim to support PM can be safely powered off.
@@ -219,6 +217,8 @@ static inline int ata_disk_can_poweroff(void)
     return (identify_info[82] & (1<<3) && identify_info[85] & (1<<3));
 }
 
+int ata_flush(void);
+
 #ifdef HAVE_ATA_DMA
 /* Returns current DMA mode */
 int ata_get_dma_mode(void);
diff --git a/firmware/export/config.h b/firmware/export/config.h
index 37ea9a1dd7..65f05b3e8e 100644
--- a/firmware/export/config.h
+++ b/firmware/export/config.h
@@ -635,6 +635,12 @@ Lyre prototype 1 */
 #endif
 #endif
 
+#if defined(__PCTOOL__) || defined(SIMULATOR)
+#ifndef CONFIG_PLATFORM
+#define CONFIG_PLATFORM PLATFORM_HOSTED
+#endif
+#endif
+
 #ifndef CONFIG_PLATFORM
 #define CONFIG_PLATFORM PLATFORM_NATIVE
 #endif
@@ -893,7 +899,8 @@ Lyre prototype 1 */
 #endif
 
 /* Bootloaders don't need multivolume awareness */
-#if defined(BOOTLOADER) && defined(HAVE_MULTIVOLUME) && !(CONFIG_PLATFORM & PLATFORM_HOSTED)
+#if defined(BOOTLOADER) && defined(HAVE_MULTIVOLUME) \
+    && !(CONFIG_PLATFORM & PLATFORM_HOSTED) && !defined(BOOT_REDIR)
 #undef HAVE_MULTIVOLUME
 #endif
 
@@ -1252,10 +1259,14 @@ Lyre prototype 1 */
 # define INCLUDE_TIMEOUT_API
 #endif
 
-#ifndef SIMULATOR
-#if defined(HAVE_USBSTACK) || (CONFIG_STORAGE & STORAGE_NAND) || (CONFIG_STORAGE & STORAGE_RAMDISK)
+#if (!defined(SIMULATOR) && !defined(HAVE_HOSTFS) && !(CONFIG_STORAGE & STORAGE_HOSTFS))
 #define STORAGE_GET_INFO
 #endif
+
+#if defined(HAVE_SIGALTSTACK_THREADS)
+#ifndef _XOPEN_SOURCE
+#define _XOPEN_SOURCE 600   /* For sigaltstack */
+#endif
 #endif
 
 #ifdef CPU_MIPS
diff --git a/firmware/export/config/erosqnative.h b/firmware/export/config/erosqnative.h
index 26073a5f34..adb1b29e01 100644
--- a/firmware/export/config/erosqnative.h
+++ b/firmware/export/config/erosqnative.h
@@ -9,6 +9,19 @@
 #define BOOTFILE     "rockbox." BOOTFILE_EXT
 #define BOOTDIR      "/.rockbox"
 
+/* Define EROSQN_VER as an "extradefine" in the configure script -
+ * v1, v2 players: "1"
+ * v3 players: "3"
+ * Only bootloader will be affected.
+ *
+ * This allows us to fix the LCD init issues with v3 players.
+ */
+#ifdef BOOTLOADER
+#ifndef EROSQN_VER
+#error "Must define EROSQN_VER"
+#endif
+#endif
+
 /* CPU defines */
 #define CONFIG_CPU      X1000
 #define X1000_EXCLK_FREQ   24000000
@@ -106,6 +119,9 @@
 #define HAVE_BOOTDATA
 #define BOOT_REDIR "rockbox_main.aigo_erosqn"
 
+/* DeviceData */
+#define HAVE_DEVICEDATA
+
 /* USB support */
 #ifndef SIMULATOR
 #define CONFIG_USBOTG USBOTG_DESIGNWARE
diff --git a/firmware/export/config/hibylinux.h b/firmware/export/config/hibylinux.h
index ad168c9c01..2da7633389 100644
--- a/firmware/export/config/hibylinux.h
+++ b/firmware/export/config/hibylinux.h
@@ -86,5 +86,7 @@
 #define ROOTDRIVE_DEV "/sys/block/mmcblk0"
 
 /* More common stuff */
+#ifndef EROS_Q
 #define BATTERY_DEV_NAME "battery"
+#endif
 #define POWER_DEV_NAME "usb"
diff --git a/firmware/export/config/ipod6g.h b/firmware/export/config/ipod6g.h
index 50aa5d8e8c..7eb92f86db 100644
--- a/firmware/export/config/ipod6g.h
+++ b/firmware/export/config/ipod6g.h
@@ -12,7 +12,7 @@
 /* define this if you use an ATA controller */
 #define CONFIG_STORAGE STORAGE_ATA
 
-#define HAVE_ATA_DMA 
+#define HAVE_ATA_DMA
 #define ATA_MAX_UDMA 4
 #define ATA_MAX_MWDMA 2
 
@@ -139,7 +139,7 @@
 
 /* 6g has a standard battery of 550mAh, except for the thick 6g (2007 160gb)
  * which has a standard battery of 850mAh.
- * 
+ *
  * It's possible to buy 3rd party batteries up to 3000mAh.
  */
 #define BATTERY_CAPACITY_DEFAULT 550 /* default battery capacity */
@@ -189,17 +189,12 @@
 /* Define this if you can read an absolute wheel position */
 #define HAVE_WHEEL_POSITION
 
-#define SECTOR_SIZE 4096
-
 #define HAVE_ATA_SMART
 
 /* define this if the device has larger sectors when accessed via USB */
-/* (only relevant in disk.c, fat.c now always supports large virtual sectors) */
-//#define MAX_LOG_SECTOR_SIZE 4096
-
-/* define this if the hard drive uses large physical sectors (ATA-7 feature) */
-/* and doesn't handle them in the drive firmware */
-//#define MAX_PHYS_SECTOR_SIZE 4096
+#define MAX_LOG_SECTOR_SIZE 4096
+/* This is the minimum access size for the device, even if it's larger than the logical sector size */
+#define MAX_PHYS_SECTOR_SIZE 4096
 
 #define HAVE_HARDWARE_CLICK
 
diff --git a/firmware/export/config/vibe500.h b/firmware/export/config/vibe500.h
index b6d2e5082a..48140e7a4a 100644
--- a/firmware/export/config/vibe500.h
+++ b/firmware/export/config/vibe500.h
@@ -170,14 +170,6 @@
 /* Define this if you have adjustable CPU frequency */
 #define HAVE_ADJUSTABLE_CPU_FREQ
 
-/* define this if the device has larger sectors when accessed via USB */
-/* (only relevant in disk.c, fat.c now always supports large virtual sectors) */
-/* #define MAX_LOG_SECTOR_SIZE 2048 */
-
-/* define this if the hard drive uses large physical sectors (ATA-7 feature) */
-/* and doesn't handle them in the drive firmware */
-/* #define MAX_PHYS_SECTOR_SIZE 1024 */
-
 #define MI4_FORMAT
 #define BOOTFILE_EXT "mi4"
 #define BOOTFILE "rockbox." BOOTFILE_EXT
@@ -185,9 +177,6 @@
 
 #define ICODE_ATTR_TREMOR_NOT_MDCT
 
-/* DMA is used only for reading on PP502x because although reads are ~8x faster
- * writes appear to be ~25% slower.
- */
 #ifndef BOOTLOADER
 #define HAVE_ATA_DMA
 #endif
diff --git a/firmware/export/devicedata.h b/firmware/export/devicedata.h
new file mode 100644
index 0000000000..c19b0ca25d
--- /dev/null
+++ b/firmware/export/devicedata.h
@@ -0,0 +1,94 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ *
+ * Copyright (C) 2017 by Amaury Pouly
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#ifndef __RB_DEVICEDATA__
+#define __RB_DEVICEDATA__
+
+#ifndef __ASSEMBLER__
+#include <stdint.h>
+#include "system.h"
+#endif
+
+/* /!\ This file can be included in assembly files /!\ */
+
+/** The device data will be filled by the bootloader with information that might
+ * be relevant for Rockbox. The bootloader will search for the structure using
+ * the magic header within the first DEVICE_DATA_SEARCH_SIZE bytes of the binary.
+ * Typically, this structure should be as close as possible to the entry point */
+
+/* Search size for the data structure after entry point */
+#define DEVICE_DATA_SEARCH_SIZE   1024
+
+#define DEVICE_DATA_MAGIC0    ('r' | 'b' << 8 | 'd' << 16 | 'e' << 24)
+#define DEVICE_DATA_MAGIC1    ('v' | 'i' << 8 | 'c' << 16 | 'e' << 24)
+
+/* maximum size of payload */
+#define DEVICE_DATA_PAYLOAD_SIZE  4
+
+#ifndef __ASSEMBLER__
+/* This is the C structure */
+struct device_data_t
+{
+    union
+    {
+        uint32_t crc; /* crc of payload data (CRC32 with 0xffffffff for initial value) */
+        uint32_t magic[2]; /* DEVICE_DATA_MAGIC0/1 */
+    };
+
+    uint32_t length; /* length of the payload */
+
+    /* add fields here */
+    union
+    {
+        struct
+        {
+#if defined(EROS_QN)
+            uint8_t lcd_version;
+#endif
+        };
+        uint8_t payload[DEVICE_DATA_PAYLOAD_SIZE];
+    };
+} __attribute__((packed));
+
+
+void fill_devicedata(struct device_data_t *data);
+bool write_devicedata(unsigned char* buf, int len);
+#ifndef BOOTLOADER
+extern struct device_data_t device_data;
+
+void verify_device_data(void) INIT_ATTR;
+
+#endif
+
+#else /* __ASSEMBLER__ */
+
+/* This assembler macro implements an empty device data structure with just the magic
+ * string and payload size */
+.macro put_device_data_here
+.global device_data
+device_data:
+    .word   DEVICE_DATA_MAGIC0
+    .word   DEVICE_DATA_MAGIC1
+    .word   DEVICE_DATA_PAYLOAD_SIZE
+    .space  BOOT_DATA_PAYLOAD_SIZE, 0xff /* payload, initialised with value 0xff */
+.endm
+
+#endif
+
+#endif /* __RB_DEVICEDATA__ */
diff --git a/firmware/export/disk.h b/firmware/export/disk.h
index a19e011170..4f11438b1b 100644
--- a/firmware/export/disk.h
+++ b/firmware/export/disk.h
@@ -47,10 +47,12 @@ int disk_mount(int drive);
 int disk_unmount_all(void);
 int disk_unmount(int drive);
 
-/* The number of 512-byte sectors in a "logical" sector. Needed for ipod 5.5G */
+/* Used when the drive's logical sector size is smaller than the sector size used by the partition table and filesystem.  Notably needed for ipod 5.5G/6G. */
 #ifdef MAX_LOG_SECTOR_SIZE
 int disk_get_sector_multiplier(IF_MD_NONVOID(int drive));
 #endif
+/* The size of the drive's smallest addressible unit */
+int disk_get_log_sector_size(IF_MD_NONVOID(int drive));
 
 bool disk_present(IF_MD_NONVOID(int drive));
 
diff --git a/firmware/export/storage.h b/firmware/export/storage.h
index e2ae4056be..f97cbf34d5 100644
--- a/firmware/export/storage.h
+++ b/firmware/export/storage.h
@@ -174,7 +174,7 @@ static inline void storage_sleep(void) {};
         #define storage_disk_is_active() ata_disk_is_active()
         #define storage_soft_reset() ata_soft_reset()
         #ifdef HAVE_STORAGE_FLUSH
-            #define storage_flush() (void)0
+            #define storage_flush() ata_flush()
         #endif
         #define storage_last_disk_activity() ata_last_disk_activity()
         #define storage_spinup_time() ata_spinup_time()
diff --git a/firmware/export/usb_ch9.h b/firmware/export/usb_ch9.h
index 659bcca101..db311c592e 100644
--- a/firmware/export/usb_ch9.h
+++ b/firmware/export/usb_ch9.h
@@ -292,7 +292,7 @@ struct usb_string_descriptor {
         uint8_t  bDescriptorType;
 
         uint16_t wString[];             /* UTF-16LE encoded */
-} __attribute__ ((packed));
+} __attribute__ ((packed)) __attribute__((aligned(2)));
 
 /* note that "string" zero is special, it holds language codes that
  * the device supports, not Unicode characters.
diff --git a/firmware/include/string-extra.h b/firmware/include/string-extra.h
index a9b34661a7..9572638e23 100644
--- a/firmware/include/string-extra.h
+++ b/firmware/include/string-extra.h
@@ -30,7 +30,7 @@
 #include "memset16.h"
 
 #if defined(WIN32) || defined(APPLICATION) \
-        || defined(__PCTOOL__)
+        || defined(__PCTOOL__) || defined(SIMULATOR)
 #ifndef mempcpy
 #define mempcpy __builtin_mempcpy
 #endif
diff --git a/firmware/kernel/include/queue.h b/firmware/kernel/include/queue.h
index a9c3b5a93a..da03f5312e 100644
--- a/firmware/kernel/include/queue.h
+++ b/firmware/kernel/include/queue.h
@@ -141,6 +141,7 @@ extern void queue_reply(struct event_queue *q, intptr_t retval);
 extern bool queue_in_queue_send(struct event_queue *q);
 #endif /* HAVE_EXTENDED_MESSAGING_AND_NAME */
 extern bool queue_empty(const struct event_queue* q);
+extern bool queue_full(const struct event_queue* q);
 extern bool queue_peek(struct event_queue *q, struct queue_event *ev);
 
 #define QPEEK_FILTER_COUNT_MASK (0xffu) /* 0x00=1 filter, 0xff=256 filters */
diff --git a/firmware/kernel/queue.c b/firmware/kernel/queue.c
index 5566604c5d..29611e34c7 100644
--- a/firmware/kernel/queue.c
+++ b/firmware/kernel/queue.c
@@ -701,6 +701,12 @@ bool queue_empty(const struct event_queue* q)
     return ( q->read == q->write );
 }
 
+/* Poll queue to see if it is full */
+bool queue_full(const struct event_queue* q)
+{
+    return ((q->write - q->read) >= QUEUE_LENGTH);
+}
+
 void queue_clear(struct event_queue* q)
 {
     int oldlevel;
diff --git a/firmware/libc/include/stdlib.h b/firmware/libc/include/stdlib.h
index e24d6a579f..855663efac 100644
--- a/firmware/libc/include/stdlib.h
+++ b/firmware/libc/include/stdlib.h
@@ -50,7 +50,8 @@ int rand(void);
 #endif
 
 #define abs(x) ((int)ABS(x))
-#define labs(x) ((long)abs(x))
+#define labs(x) ((long)ABS(x))
+#define llabs(x) ((long long)ABS(x))
 
 #ifdef SIMULATOR
 void exit(int status);
diff --git a/firmware/rolo.c b/firmware/rolo.c
index f9b0cc9e61..38d350d432 100644
--- a/firmware/rolo.c
+++ b/firmware/rolo.c
@@ -56,6 +56,10 @@
 #include "crc32.h"
 #endif
 
+#if defined(HAVE_DEVICEDATA) && !defined(SIMULATOR)
+#include "devicedata.h"
+#endif
+
 #if CONFIG_CPU == AS3525v2
 #include "ascodec.h"
 #endif
@@ -250,9 +254,8 @@ int rolo_load(const char* filename)
 
     err = LOAD_FIRMWARE(filebuf, filename, filebuf_size);
 #if defined(HAVE_BOOTDATA) && !defined(SIMULATOR)
-    /* write the bootdata as if rolo were the bootloader
-     * FIXME: this won't work for root redirect... */
-    if (!strcmp(filename, BOOTDIR "/" BOOTFILE) && boot_data_valid)
+    // Write bootdata as long as the existing bootdata is valid
+    if (boot_data_valid)
     {
         int volume = 0;
 
@@ -275,6 +278,10 @@ int rolo_load(const char* filename)
     }
 #endif
 
+#if defined(HAVE_DEVICEDATA)
+    write_devicedata(filebuf, filebuf_size);
+#endif
+
     if (err <= 0)
     {
         rolo_error(loader_strerror(err));
diff --git a/firmware/scroll_engine.c b/firmware/scroll_engine.c
index c32f4632e2..74c08c1da5 100644
--- a/firmware/scroll_engine.c
+++ b/firmware/scroll_engine.c
@@ -32,6 +32,10 @@
 #include "usb.h"
 #include "lcd.h"
 #include "font.h"
+#if !defined(BOOTLOADER)
+#include "misc.h"
+#include "settings.h"
+#endif
 #ifdef HAVE_REMOTE_LCD
 #include "lcd-remote.h"
 #endif
diff --git a/firmware/storage.c b/firmware/storage.c
index da3e06146d..2d4490369e 100644
--- a/firmware/storage.c
+++ b/firmware/storage.c
@@ -555,7 +555,7 @@ int storage_flush(void)
     int rc=0;
 
 #if (CONFIG_STORAGE & STORAGE_ATA)
-    //if ((rc=ata_flush())) return rc;
+    if ((rc=ata_flush())) return rc;
 #endif
 
 #if (CONFIG_STORAGE & STORAGE_MMC)
diff --git a/firmware/target/arm/as3525/system-as3525.c b/firmware/target/arm/as3525/system-as3525.c
index 0450c8b410..26a228cddf 100644
--- a/firmware/target/arm/as3525/system-as3525.c
+++ b/firmware/target/arm/as3525/system-as3525.c
@@ -92,7 +92,7 @@ void INT_GPIOA(void);
 default_interrupt(INT_GPIOB);
 default_interrupt(INT_GPIOC);
 
-static const char const irqname[][9] =
+static const char irqname[][9] =
 {
     "WATCHDOG", "TIMER1", "TIMER2", "USB", "DMAC", "NAND",
     "IDE", "MCI0", "MCI1", "AUDIO", "SSP", "I2C_MS",
diff --git a/firmware/target/arm/imx233/clkctrl-imx233.c b/firmware/target/arm/imx233/clkctrl-imx233.c
index 59c23c1a76..1a8f523d5a 100644
--- a/firmware/target/arm/imx233/clkctrl-imx233.c
+++ b/firmware/target/arm/imx233/clkctrl-imx233.c
@@ -87,7 +87,9 @@ void imx233_clkctrl_set_div(enum imx233_clock_t clk, int div)
         case CLK_EMI: BF_WR(CLKCTRL_EMI, DIV(div)); break;
 #endif
         case CLK_SSP: BF_WR(CLKCTRL_SSP, DIV(div)); break;
-        case CLK_HBUS: BF_WR(CLKCTRL_HBUS, DIV(div)); break;
+        case CLK_HBUS:
+            /* make sure to switch to integer divide mode simulteanously */
+            BF_WR(CLKCTRL_HBUS, DIV_FRAC_EN(0), DIV(div)); break;
         case CLK_XBUS: BF_WR(CLKCTRL_XBUS, DIV(div)); break;
         default: return;
     }
@@ -107,7 +109,12 @@ int imx233_clkctrl_get_div(enum imx233_clock_t clk)
         case CLK_EMI: return BF_RD(CLKCTRL_EMI, DIV);
 #endif
         case CLK_SSP: return BF_RD(CLKCTRL_SSP, DIV);
-        case CLK_HBUS: return BF_RD(CLKCTRL_HBUS, DIV);
+        case CLK_HBUS:
+            /* since fractional and integer divider share the same field, clain it is disabled in frac mode */
+            if(BF_RD(CLKCTRL_HBUS, DIV_FRAC_EN))
+                return 0;
+            else
+                return BF_RD(CLKCTRL_HBUS, DIV);
         case CLK_XBUS: return BF_RD(CLKCTRL_XBUS, DIV);
         default: return 0;
     }
@@ -130,6 +137,14 @@ void imx233_clkctrl_set_frac_div(enum imx233_clock_t clk, int fracdiv)
         handle_frac(IO)
         handle_frac(CPU)
         handle_frac(EMI)
+        case CLK_HBUS:
+            if(fracdiv == 0)
+                panicf("Don't set hbus fracdiv to 0!");
+            /* value 0 is forbidden because we can't simply disabble the divider, it's always
+             * active but either in integer or fractional mode
+             * make sure we write both the value and frac_en bit at the same time */
+            BF_WR(CLKCTRL_HBUS, DIV_FRAC_EN(1), DIV(fracdiv));
+            break;
         default: break;
     }
 #undef handle_frac
@@ -151,6 +166,11 @@ int imx233_clkctrl_get_frac_div(enum imx233_clock_t clk)
         handle_frac(IO)
         handle_frac(CPU)
         handle_frac(EMI)
+        case CLK_HBUS:
+            if(BF_RD(CLKCTRL_HBUS, DIV_FRAC_EN))
+                return BF_RD(CLKCTRL_HBUS, DIV);
+            else
+                return 0;
         default: return 0;
     }
 #undef handle_frac
@@ -303,8 +323,12 @@ unsigned imx233_clkctrl_get_freq(enum imx233_clock_t clk)
             /* Derived from clk_p via integer/fractional div */
             unsigned ref = imx233_clkctrl_get_freq(CLK_CPU);
 #if IMX233_SUBTARGET >= 3700
+            /* if divider is in fractional mode, integer divider does not take effect (in fact it's
+             * the same divider but in a different mode ). Also the fractiona value is encoded as
+             * a fraction, not a divider */
             if(imx233_clkctrl_get_frac_div(CLK_HBUS) != 0)
                 ref = (ref * imx233_clkctrl_get_frac_div(CLK_HBUS)) / 32;
+            else
 #endif
             if(imx233_clkctrl_get_div(CLK_HBUS) != 0)
                 ref /= imx233_clkctrl_get_div(CLK_HBUS);
diff --git a/firmware/target/arm/imx233/clkctrl-imx233.h b/firmware/target/arm/imx233/clkctrl-imx233.h
index f12d181c50..d150912cfe 100644
--- a/firmware/target/arm/imx233/clkctrl-imx233.h
+++ b/firmware/target/arm/imx233/clkctrl-imx233.h
@@ -68,7 +68,7 @@ int imx233_clkctrl_get_div(enum imx233_clock_t clk);
 #if IMX233_SUBTARGET >= 3700
 /* call with fracdiv=0 to disable it */
 void imx233_clkctrl_set_frac_div(enum imx233_clock_t clk, int fracdiv);
-/* 0 means fractional dividor disable */
+/* 0 means fractional dividor disabled */
 int imx233_clkctrl_get_frac_div(enum imx233_clock_t clk);
 void imx233_clkctrl_set_bypass(enum imx233_clock_t clk, bool bypass);
 bool imx233_clkctrl_get_bypass(enum imx233_clock_t clk);
diff --git a/firmware/target/arm/ipod/button-clickwheel.c b/firmware/target/arm/ipod/button-clickwheel.c
index 8f238936e7..9308b7664e 100644
--- a/firmware/target/arm/ipod/button-clickwheel.c
+++ b/firmware/target/arm/ipod/button-clickwheel.c
@@ -327,7 +327,13 @@ void ipod_4g_button_int(void)
 void button_init_device(void)
 {
     opto_i2c_init();
-    
+
+    /* fixes first button press being ignored */
+#if defined(IPOD_4G) || defined(IPOD_COLOR)
+    outl(inl(0x7000c100) & ~0x60000000, 0x7000c100);
+    outl(inl(0x7000c104) | 0x04000000, 0x7000c104);
+    outl(inl(0x7000c100) | 0x60000000, 0x7000c100);
+#endif
     /* hold button - enable as input */
     GPIOA_ENABLE |= 0x20;
     GPIOA_OUTPUT_EN &= ~0x20; 
diff --git a/firmware/target/arm/s5l8700/ipodnano2g/ftl-nano2g.c b/firmware/target/arm/s5l8700/ipodnano2g/ftl-nano2g.c
index 0d831a7167..7071d411b5 100644
--- a/firmware/target/arm/s5l8700/ipodnano2g/ftl-nano2g.c
+++ b/firmware/target/arm/s5l8700/ipodnano2g/ftl-nano2g.c
@@ -186,7 +186,6 @@ struct ftl_cxt_type
    There is one of these per bank. */
 struct ftl_vfl_cxt_type
 {
-
     /* Cross-bank update sequence number, incremented on every VFL
        context commit on any bank. */
     uint32_t usn;
@@ -525,18 +524,24 @@ static uint32_t ftl_vfl_verify_checksum(uint32_t bank)
     return 1;
 }
 
-
 #ifndef FTL_READONLY
+
+#if __GNUC__ >= 9
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
+#endif
+
 /* Updates the checksums of the VFL context of the specified bank */
 static void ftl_vfl_update_checksum(uint32_t bank)
 {
     ftl_vfl_calculate_checksum(bank, &ftl_vfl_cxt[bank].checksum1,
                                &ftl_vfl_cxt[bank].checksum2);
 }
-#endif
 
+#if __GNUC__ >= 9
+#pragma GCC diagnostic pop
+#endif
 
-#ifndef FTL_READONLY
 /* Writes 8 copies of the VFL context of the specified bank to flash,
    and succeeds if at least 4 can be read back properly. */
 static uint32_t ftl_vfl_store_cxt(uint32_t bank)
@@ -2107,7 +2112,7 @@ uint32_t ftl_sync(void)
     if (ftl_cxt.clean_flag == 1) return 0;
 
     mutex_lock(&ftl_mtx);
-    
+
 #ifdef FTL_TRACE
     DEBUGF("FTL: Syncing\n");
 #endif
diff --git a/firmware/target/arm/s5l8702/ipod6g/storage_ata-6g.c b/firmware/target/arm/s5l8702/ipod6g/storage_ata-6g.c
index 9629b3e30f..8cc5b44aca 100644
--- a/firmware/target/arm/s5l8702/ipod6g/storage_ata-6g.c
+++ b/firmware/target/arm/s5l8702/ipod6g/storage_ata-6g.c
@@ -29,6 +29,9 @@
 #include "mmcdefs-target.h"
 #include "s5l8702.h"
 #include "led.h"
+#include "debug.h"
+#include "panic.h"
+#include "fs_defines.h"
 
 #ifndef ATA_RETRIES
 #define ATA_RETRIES 3
@@ -60,7 +63,7 @@
 
 /** static, private data **/
 static uint8_t ceata_taskfile[16] STORAGE_ALIGN_ATTR;
-static uint16_t ata_identify_data[ATA_IDENTIFY_WORDS] STORAGE_ALIGN_ATTR;
+static uint16_t identify_info[ATA_IDENTIFY_WORDS] STORAGE_ALIGN_ATTR;
 static bool ceata;
 static bool ata_lba48;
 static bool ata_dma;
@@ -69,18 +72,18 @@ static struct mutex ata_mutex;
 static struct semaphore ata_wakeup;
 static uint32_t ata_dma_flags;
 static long ata_last_activity_value = -1;
-static long ata_sleep_timeout = 20 * HZ;
+static long ata_sleep_timeout = 7 * HZ;
 static bool ata_powered;
-static const int ata_retries = ATA_RETRIES;
-static const bool ata_error_srst = true;
+static bool canflush = true;
 static struct semaphore mmc_wakeup;
 static struct semaphore mmc_comp_wakeup;
 static int spinup_time = 0;
 static int dma_mode = 0;
-static char aligned_buffer[SECTOR_SIZE] STORAGE_ALIGN_ATTR;
 
-static int ata_reset(void);
+static const int ata_retries = ATA_RETRIES;
+static const bool ata_error_srst = true;
 
+static int ata_reset(void);
 
 static uint16_t ata_read_cbr(uint32_t volatile* reg)
 {
@@ -102,8 +105,10 @@ static int ata_wait_for_not_bsy(long timeout)
     while (true)
     {
         uint8_t csd = ata_read_cbr(&ATA_PIO_CSD);
-        if (!(csd & BIT(7))) return 0;
-        if (TIMEOUT_EXPIRED(startusec, timeout)) RET_ERR(0);
+        if (!(csd & BIT(7)))
+            return 0;
+        if (TIMEOUT_EXPIRED(startusec, timeout))
+            RET_ERR(0);
         yield();
     }
 }
@@ -115,8 +120,10 @@ static int ata_wait_for_rdy(long timeout)
     while (true)
     {
         uint8_t dad = ata_read_cbr(&ATA_PIO_DAD);
-        if (dad & BIT(6)) return 0;
-        if (TIMEOUT_EXPIRED(startusec, timeout)) RET_ERR(1);
+        if (dad & BIT(6))
+            return 0;
+        if (TIMEOUT_EXPIRED(startusec, timeout))
+            RET_ERR(1);
         yield();
     }
 }
@@ -128,9 +135,12 @@ static int ata_wait_for_start_of_transfer(long timeout)
     while (true)
     {
         uint8_t dad = ata_read_cbr(&ATA_PIO_DAD);
-        if (dad & BIT(0)) RET_ERR(1);
-        if ((dad & (BIT(7) | BIT(3))) == BIT(3)) return 0;
-        if (TIMEOUT_EXPIRED(startusec, timeout)) RET_ERR(2);
+        if (dad & BIT(0))
+            RET_ERR(1);
+        if ((dad & (BIT(7) | BIT(3))) == BIT(3))
+            return 0;
+        if (TIMEOUT_EXPIRED(startusec, timeout))
+            RET_ERR(2);
         yield();
     }
 }
@@ -139,8 +149,10 @@ static int ata_wait_for_end_of_transfer(long timeout)
 {
     PASS_RC(ata_wait_for_not_bsy(timeout), 2, 0);
     uint8_t dad = ata_read_cbr(&ATA_PIO_DAD);
-    if (dad & BIT(0)) RET_ERR(1);
-    if ((dad & (BIT(3) | BITRANGE(5, 7))) == BIT(6)) return 0;
+    if (dad & BIT(0))
+        RET_ERR(1);
+    if ((dad & (BIT(3) | BITRANGE(5, 7))) == BIT(6))
+        return 0;
     RET_ERR(2);
 }
 
@@ -148,13 +160,17 @@ static int mmc_dsta_check_command_success(bool disable_crc)
 {
     int rc = 0;
     uint32_t dsta = SDCI_DSTA;
-    if (dsta & SDCI_DSTA_RESTOUTE) rc |= 1;
-    if (dsta & SDCI_DSTA_RESENDE) rc |= 2;
-    if (dsta & SDCI_DSTA_RESINDE) rc |= 4;
+    if (dsta & SDCI_DSTA_RESTOUTE)
+        rc |= 1;
+    if (dsta & SDCI_DSTA_RESENDE)
+        rc |= 2;
+    if (dsta & SDCI_DSTA_RESINDE)
+        rc |= 4;
     if (!disable_crc)
         if (dsta & SDCI_DSTA_RESCRCE)
             rc |= 8;
-    if (rc) RET_ERR(rc);
+    if (rc)
+        RET_ERR(rc);
     return 0;
 }
 
@@ -163,7 +179,8 @@ static bool mmc_send_command(uint32_t cmd, uint32_t arg, uint32_t* result, int t
     long starttime = USEC_TIMER;
     while ((SDCI_STATE & SDCI_STATE_CMD_STATE_MASK) != SDCI_STATE_CMD_STATE_CMD_IDLE)
     {
-        if (TIMEOUT_EXPIRED(starttime, timeout)) RET_ERR(0);
+        if (TIMEOUT_EXPIRED(starttime, timeout))
+            RET_ERR(0);
         yield();
     }
     SDCI_STAC = SDCI_STAC_CLR_CMDEND | SDCI_STAC_CLR_BIT_3
@@ -178,32 +195,38 @@ static bool mmc_send_command(uint32_t cmd, uint32_t arg, uint32_t* result, int t
               | SDCI_STAC_CLR_RD_DATENDE6 | SDCI_STAC_CLR_RD_DATENDE7;
     SDCI_ARGU = arg;
     SDCI_CMD = cmd;
-    if (!(SDCI_DSTA & SDCI_DSTA_CMDRDY)) RET_ERR(1);
+    if (!(SDCI_DSTA & SDCI_DSTA_CMDRDY))
+        RET_ERR(1);
     SDCI_CMD = cmd | SDCI_CMD_CMDSTR;
     long sleepbase = USEC_TIMER;
-    while (TIMEOUT_EXPIRED(sleepbase, 1000)) yield();
+    while (TIMEOUT_EXPIRED(sleepbase, 1000))
+        yield();
     while (!(SDCI_DSTA & SDCI_DSTA_CMDEND))
     {
-        if (TIMEOUT_EXPIRED(starttime, timeout)) RET_ERR(2);
+        if (TIMEOUT_EXPIRED(starttime, timeout))
+            RET_ERR(2);
         yield();
     }
     if ((cmd & SDCI_CMD_RES_TYPE_MASK) != SDCI_CMD_RES_TYPE_NONE)
     {
         while (!(SDCI_DSTA & SDCI_DSTA_RESEND))
         {
-            if (TIMEOUT_EXPIRED(starttime, timeout)) RET_ERR(3);
+            if (TIMEOUT_EXPIRED(starttime, timeout))
+                RET_ERR(3);
             yield();
         }
         if (cmd & SDCI_CMD_RES_BUSY)
             while (SDCI_DSTA & SDCI_DSTA_DAT_BUSY)
             {
-                if (TIMEOUT_EXPIRED(starttime, CEATA_DAT_NONBUSY_TIMEOUT)) RET_ERR(4);
+                if (TIMEOUT_EXPIRED(starttime, CEATA_DAT_NONBUSY_TIMEOUT))
+                    RET_ERR(4);
                 yield();
             }
     }
     bool nocrc = (cmd & SDCI_CMD_RES_SIZE_MASK) == SDCI_CMD_RES_SIZE_136;
     PASS_RC(mmc_dsta_check_command_success(nocrc), 3, 5);
-    if (result) *result = SDCI_RESP0;
+    if (result)
+        *result = SDCI_RESP0;
     return 0;
 }
 
@@ -226,7 +249,8 @@ static int mmc_init(void)
     uint32_t result;
     do
     {
-        if (TIMEOUT_EXPIRED(startusec, CEATA_POWERUP_TIMEOUT)) RET_ERR(1);
+        if (TIMEOUT_EXPIRED(startusec, CEATA_POWERUP_TIMEOUT))
+            RET_ERR(1);
         sleep(HZ / 100);
         PASS_RC(mmc_send_command(SDCI_CMD_CMD_NUM(MMC_CMD_SEND_OP_COND)
                                | SDCI_CMD_CMD_TYPE_BCR | SDCI_CMD_RES_TYPE_R3
@@ -251,7 +275,8 @@ static int mmc_init(void)
                              MMC_CMD_SELECT_CARD_RCA(CEATA_MMC_RCA),
                              NULL, CEATA_COMMAND_TIMEOUT), 3, 5);
     PASS_RC(mmc_get_card_status(&result), 3, 6);
-    if ((result & MMC_STATUS_CURRENT_STATE_MASK) != MMC_STATUS_CURRENT_STATE_TRAN) RET_ERR(7);
+    if ((result & MMC_STATUS_CURRENT_STATE_MASK) != MMC_STATUS_CURRENT_STATE_TRAN)
+        RET_ERR(7);
     return 0;
 }
 
@@ -285,7 +310,8 @@ static int ceata_soft_reset(void)
     do
     {
         PASS_RC(mmc_fastio_read(0xf, &status), 2, 2);
-        if (TIMEOUT_EXPIRED(startusec, CEATA_POWERUP_TIMEOUT)) RET_ERR(3);
+        if (TIMEOUT_EXPIRED(startusec, CEATA_POWERUP_TIMEOUT))
+            RET_ERR(3);
         sleep(HZ / 100);
     }
     while (status & 0x80);
@@ -298,16 +324,21 @@ static int mmc_dsta_check_data_success(void)
     uint32_t dsta = SDCI_DSTA;
     if (dsta & (SDCI_DSTA_WR_DATCRCE | SDCI_DSTA_RD_DATCRCE))
     {
-        if (dsta & SDCI_DSTA_WR_DATCRCE) rc |= 1;
-        if (dsta & SDCI_DSTA_RD_DATCRCE) rc |= 2;
-        if ((dsta & SDCI_DSTA_WR_CRC_STATUS_MASK) == SDCI_DSTA_WR_CRC_STATUS_TXERR) rc |= 4;
-        else if ((dsta & SDCI_DSTA_WR_CRC_STATUS_MASK) == SDCI_DSTA_WR_CRC_STATUS_CARDERR) rc |= 8;
+        if (dsta & SDCI_DSTA_WR_DATCRCE)
+            rc |= 1;
+        if (dsta & SDCI_DSTA_RD_DATCRCE)
+            rc |= 2;
+        if ((dsta & SDCI_DSTA_WR_CRC_STATUS_MASK) == SDCI_DSTA_WR_CRC_STATUS_TXERR)
+            rc |= 4;
+        else if ((dsta & SDCI_DSTA_WR_CRC_STATUS_MASK) == SDCI_DSTA_WR_CRC_STATUS_CARDERR)
+            rc |= 8;
     }
     if (dsta & (SDCI_DSTA_RD_DATENDE0 | SDCI_DSTA_RD_DATENDE1 | SDCI_DSTA_RD_DATENDE2
               | SDCI_DSTA_RD_DATENDE3 | SDCI_DSTA_RD_DATENDE4 | SDCI_DSTA_RD_DATENDE5
               | SDCI_DSTA_RD_DATENDE6 | SDCI_DSTA_RD_DATENDE7))
         rc |= 16;
-    if (rc) RET_ERR(rc);
+    if (rc)
+        RET_ERR(rc);
     return 0;
 }
 
@@ -320,7 +351,8 @@ static void mmc_discard_irq(void)
 
 static int ceata_read_multiple_register(uint32_t addr, void* dest, uint32_t size)
 {
-    if (size > 0x10) RET_ERR(0);
+    if (size > 0x10)
+        RET_ERR(0);
     mmc_discard_irq();
     SDCI_DMASIZE = size;
     SDCI_DMACOUNT = 1;
@@ -334,8 +366,8 @@ static int ceata_read_multiple_register(uint32_t addr, void* dest, uint32_t size
                            | MMC_CMD_CEATA_RW_MULTIPLE_REG_ADDRESS(addr & 0xfc)
                            | MMC_CMD_CEATA_RW_MULTIPLE_REG_COUNT(size & 0xfc),
                              NULL, CEATA_COMMAND_TIMEOUT), 2, 1);
-    if (semaphore_wait(&mmc_wakeup, CEATA_COMMAND_TIMEOUT * HZ / 1000000)
-        == OBJ_WAIT_TIMEDOUT) RET_ERR(2);
+    if (semaphore_wait(&mmc_wakeup, CEATA_COMMAND_TIMEOUT * HZ / 1000000) == OBJ_WAIT_TIMEDOUT)
+        RET_ERR(2);
     PASS_RC(mmc_dsta_check_data_success(), 2, 3);
     return 0;
 }
@@ -343,7 +375,8 @@ static int ceata_read_multiple_register(uint32_t addr, void* dest, uint32_t size
 static int ceata_write_multiple_register(uint32_t addr, void* dest, uint32_t size)
 {
     uint32_t i;
-    if (size > 0x10) RET_ERR(0);
+    if (size > 0x10)
+        RET_ERR(0);
     mmc_discard_irq();
     SDCI_DMASIZE = size;
     SDCI_DMACOUNT = 0;
@@ -357,13 +390,15 @@ static int ceata_write_multiple_register(uint32_t addr, void* dest, uint32_t siz
                            | MMC_CMD_CEATA_RW_MULTIPLE_REG_COUNT(size & 0xfc),
                              NULL, CEATA_COMMAND_TIMEOUT), 3, 1);
     SDCI_DCTRL = SDCI_DCTRL_TRCONT_TX;
-    for (i = 0; i < size / 4; i++) SDCI_DATA = ((uint32_t*)dest)[i];
+    for (i = 0; i < size / 4; i++)
+        SDCI_DATA = ((uint32_t*)dest)[i];
     long startusec = USEC_TIMER;
-    if (semaphore_wait(&mmc_wakeup, CEATA_COMMAND_TIMEOUT * HZ / 1000000)
-        == OBJ_WAIT_TIMEDOUT) RET_ERR(2);
+    if (semaphore_wait(&mmc_wakeup, CEATA_COMMAND_TIMEOUT * HZ / 1000000) == OBJ_WAIT_TIMEDOUT)
+        RET_ERR(2);
     while ((SDCI_STATE & SDCI_STATE_DAT_STATE_MASK) != SDCI_STATE_DAT_STATE_IDLE)
     {
-        if (TIMEOUT_EXPIRED(startusec, CEATA_COMMAND_TIMEOUT)) RET_ERR(3);
+        if (TIMEOUT_EXPIRED(startusec, CEATA_COMMAND_TIMEOUT))
+            RET_ERR(3);
         yield();
     }
     PASS_RC(mmc_dsta_check_data_success(), 3, 4);
@@ -380,13 +415,17 @@ static int ceata_init(int buswidth)
                            | MMC_CMD_SWITCH_INDEX(MMC_CMD_SWITCH_FIELD_HS_TIMING)
                            | MMC_CMD_SWITCH_VALUE(MMC_CMD_SWITCH_FIELD_HS_TIMING_HIGH_SPEED),
                              &result, CEATA_COMMAND_TIMEOUT), 3, 0);
-    if (result & MMC_STATUS_SWITCH_ERROR) RET_ERR(1);
+    if (result & MMC_STATUS_SWITCH_ERROR)
+        RET_ERR(1);
     if (buswidth > 1)
     {
         int setting;
-        if (buswidth == 4) setting = MMC_CMD_SWITCH_FIELD_BUS_WIDTH_4BIT;
-        else if (buswidth == 8) setting = MMC_CMD_SWITCH_FIELD_BUS_WIDTH_8BIT;
-        else setting = MMC_CMD_SWITCH_FIELD_BUS_WIDTH_1BIT;
+        if (buswidth == 4)
+            setting = MMC_CMD_SWITCH_FIELD_BUS_WIDTH_4BIT;
+        else if (buswidth == 8)
+            setting = MMC_CMD_SWITCH_FIELD_BUS_WIDTH_8BIT;
+        else
+            setting = MMC_CMD_SWITCH_FIELD_BUS_WIDTH_1BIT;
         PASS_RC(mmc_send_command(SDCI_CMD_CMD_NUM(MMC_CMD_SWITCH) | SDCI_CMD_RES_BUSY
                                | SDCI_CMD_CMD_TYPE_AC | SDCI_CMD_RES_TYPE_R1
                                | SDCI_CMD_RES_SIZE_48 | SDCI_CMD_NCR_NID_NCR,
@@ -394,7 +433,8 @@ static int ceata_init(int buswidth)
                                | MMC_CMD_SWITCH_INDEX(MMC_CMD_SWITCH_FIELD_BUS_WIDTH)
                                | MMC_CMD_SWITCH_VALUE(setting),
                                  &result, CEATA_COMMAND_TIMEOUT), 3, 2);
-        if (result & MMC_STATUS_SWITCH_ERROR) RET_ERR(3);
+        if (result & MMC_STATUS_SWITCH_ERROR)
+            RET_ERR(3);
         if (buswidth == 4)
             SDCI_CTRL = (SDCI_CTRL & ~SDCI_CTRL_BUS_WIDTH_MASK) | SDCI_CTRL_BUS_WIDTH_4BIT;
         else if (buswidth == 8)
@@ -402,7 +442,8 @@ static int ceata_init(int buswidth)
     }
     PASS_RC(ceata_soft_reset(), 3, 4);
     PASS_RC(ceata_read_multiple_register(0, ceata_taskfile, 0x10), 3, 5);
-    if (ceata_taskfile[0xc] != 0xce || ceata_taskfile[0xd] != 0xaa) RET_ERR(6);
+    if (ceata_taskfile[0xc] != 0xce || ceata_taskfile[0xd] != 0xaa)
+        RET_ERR(6);
     PASS_RC(mmc_fastio_write(6, 0), 3, 7);
     return 0;
 }
@@ -426,8 +467,10 @@ static int ceata_wait_idle(void)
     {
         uint32_t status;
         PASS_RC(mmc_fastio_read(0xf, &status), 1, 0);
-        if (!(status & 0x88)) return 0;
-        if (TIMEOUT_EXPIRED(startusec, CEATA_DAT_NONBUSY_TIMEOUT)) RET_ERR(1);
+        if (!(status & 0x88))
+            return 0;
+        if (TIMEOUT_EXPIRED(startusec, CEATA_DAT_NONBUSY_TIMEOUT))
+            RET_ERR(1);
         sleep(HZ / 20);
     }
 }
@@ -476,7 +519,8 @@ static int ceata_rw_multiple_block(bool write, void* buf, uint32_t count, long t
                            | SDCI_CMD_RES_SIZE_48 | SDCI_CMD_NCR_NID_NCR,
                              direction | MMC_CMD_CEATA_RW_MULTIPLE_BLOCK_COUNT(count),
                              NULL, CEATA_COMMAND_TIMEOUT), 3, 0);
-    if (write) SDCI_DCTRL = SDCI_DCTRL_TRCONT_TX;
+    if (write)
+        SDCI_DCTRL = SDCI_DCTRL_TRCONT_TX;
     if (semaphore_wait(&mmc_wakeup, timeout) == OBJ_WAIT_TIMEDOUT)
     {
         PASS_RC(ceata_cancel_command(), 3, 1);
@@ -509,7 +553,8 @@ static int ata_identify(uint16_t* buf)
         ata_write_cbr(&ATA_PIO_DVR, 0);
         ata_write_cbr(&ATA_PIO_CSD, CMD_IDENTIFY);
         PASS_RC(ata_wait_for_start_of_transfer(10000000), 1, 1);
-        for (i = 0; i < ATA_IDENTIFY_WORDS; i++) buf[i] = ata_read_cbr(&ATA_PIO_DTR);
+        for (i = 0; i < ATA_IDENTIFY_WORDS; i++)
+            buf[i] = ata_read_cbr(&ATA_PIO_DTR);
     }
     return 0;
 }
@@ -548,6 +593,41 @@ static int ata_set_feature(uint32_t feature, uint32_t param)
     return 0;
 }
 
+#ifdef HAVE_ATA_DMA
+static int udmatimes[ATA_MAX_UDMA + 1] = {
+        0x4071152,
+        0x2050d52,
+#if ATA_MAX_UDMA >= 2
+        0x2030a52,
+#endif
+#if ATA_MAX_UDMA >= 3
+        0x1020a52,
+#endif
+#if ATA_MAX_UDMA >= 4
+        0x1010a52,
+#endif
+};
+static int mwdmatimes[ATA_MAX_MWDMA + 1] = {
+        0x1c175,
+        0x7083,
+        0x5072,
+};
+
+static int ata_get_best_mode(unsigned short identword, int max, int modetype)
+{
+    unsigned short testbit = BIT_N(max);
+
+    while (1) {
+        if (identword & testbit)
+            return max | modetype;
+        testbit >>= 1;
+        if (!testbit)
+            return 0;
+        max--;
+    }
+}
+#endif
+
 /*
  * ATA_UDMA_TIME register is documented on s3c6400 datasheet, information
  * included in s5l8700 datasheet is wrong or not valid for s5l8702.
@@ -572,10 +652,10 @@ static int ata_power_up(void)
     ata_set_active();
     ide_power_enable(true);
     long spinup_start = current_tick;
-    if (ceata)
-    {
+    if (ceata) {
         ata_lba48 = true;
         ata_dma = true;
+        dma_mode = 0xff; /* Canary */
         PCON(8) = 0x33333333;
         PCON(9) = 0x00000033;
         PCON(11) |= 0xf;
@@ -595,11 +675,8 @@ static int ata_power_up(void)
         SDCI_CDIV = SDCI_CDIV_CLKDIV(4);
         sleep(HZ / 100);
         PASS_RC(ceata_init(8), 3, 1);
-        PASS_RC(ata_identify(ata_identify_data), 3, 2);
-        dma_mode = 0x44;
-    }
-    else
-    {
+        PASS_RC(ata_identify(identify_info), 3, 2);
+    } else {
         PCON(7) = 0x44444444;
         PCON(8) = 0x44444444;
         PCON(9) = 0x44444444;
@@ -619,88 +696,71 @@ static int ata_power_up(void)
         ATA_PIO_LHR = 0;
         ATA_CFG = BIT(6);
         while (!(ATA_PIO_READY & BIT(1))) yield();
-        PASS_RC(ata_identify(ata_identify_data), 3, 3);
-        uint32_t piotime = 0x11f3;
-        uint32_t mdmatime = 0x1c175;
-        uint32_t udmatime = 0x4071152;
-        uint32_t param = 0;
-        ata_dma_flags = 0;
-        ata_lba48 = ata_identify_data[83] & BIT(10) ? true : false;
-        if (ata_identify_data[53] & BIT(1))
-        {
-            if (ata_identify_data[64] & BIT(1)) piotime = 0x2072;
-            else if (ata_identify_data[64] & BIT(0)) piotime = 0x7083;
-        }
-        if (ata_identify_data[63] & BIT(2))
+
+        PASS_RC(ata_identify(identify_info), 3, 3);
+
+        uint32_t piotime = 0x11f3; /* PIO0-2? */
+        if (identify_info[53] & BIT(1)) /* Word 64..70 valid */
         {
-            mdmatime = 0x5072;
-            param = 0x22;
+            if (identify_info[64] & BIT(1))
+                piotime = 0x2072; /* PIO mode 4 */
+            else if (identify_info[64] & BIT(0))
+                piotime = 0x7083; /* PIO mode 3 */
         }
-        else if (ata_identify_data[63] & BIT(1))
+        ATA_PIO_TIME = piotime;
+
+        uint32_t param = 0;
+        ata_dma_flags = 0;
+#ifdef HAVE_ATA_DMA
+        if ((identify_info[53] & BIT(2)) && (identify_info[88] & BITRANGE(0, 4))) /* Any UDMA */
         {
-            mdmatime = 0x7083;
-            param = 0x21;
+            int max_udma = ATA_MAX_UDMA;
+#if ATA_MAX_UDMA > 2
+            if (!(identify_info[93] & BIT(13)))
+                max_udma = 2;
+#endif
+            param = ata_get_best_mode(identify_info[88], max_udma, 0x40);
+            ATA_UDMA_TIME = udmatimes[param & 0xf];
+            ata_dma_flags = BIT(2) | BIT(3) | BIT(9) | BIT(10);
         }
-        if (ata_identify_data[63] & BITRANGE(0, 2))
+        if (!param && identify_info[63] & BITRANGE(0, 2)) /* Fall back to any MWDMA */
         {
+            param = ata_get_best_mode(identify_info[63], ATA_MAX_MWDMA, 0x20);
+            ATA_MDMA_TIME = mwdmatimes[param & 0xf];
             ata_dma_flags = BIT(3) | BIT(10);
-            param |= 0x20;
-        }
-        if (ata_identify_data[53] & BIT(2))
-        {
-            if (ata_identify_data[88] & BIT(4))
-            {
-                udmatime = 0x1010a52;
-                param = 0x44;
-            }
-            else if (ata_identify_data[88] & BIT(3))
-            {
-                udmatime = 0x1020a52;
-                param = 0x43;
-            }
-            else if (ata_identify_data[88] & BIT(2))
-            {
-                udmatime = 0x2030a52;
-                param = 0x42;
-            }
-            else if (ata_identify_data[88] & BIT(1))
-            {
-                udmatime = 0x2050d52;
-                param = 0x41;
-            }
-            else if (ata_identify_data[88] & BIT(0))
-            {
-                param = 0x40;
-            }
-            if (ata_identify_data[88] & BITRANGE(0, 4))
-            {
-                ata_dma_flags = BIT(2) | BIT(3) | BIT(9) | BIT(10);
-            }
         }
+#endif /* HAVE_ATA_DMA */
         ata_dma = param ? true : false;
         dma_mode = param;
         PASS_RC(ata_set_feature(0x03, param), 3, 4); /* Transfer mode */
-        if (ata_identify_data[82] & BIT(5))
+
+        /* SET_FEATURE only supported on PATA, not CE-ATA */
+        if (identify_info[82] & BIT(5))
             PASS_RC(ata_set_feature(0x02, 0), 3, 5); /* Enable volatile write cache */
-        if (ata_identify_data[82] & BIT(6))
+        if (identify_info[82] & BIT(6))
             PASS_RC(ata_set_feature(0xaa, 0), 3, 6); /* Enable read lookahead */
-        if (ata_identify_data[83] & BIT(3))
+        if (identify_info[83] & BIT(3))
             PASS_RC(ata_set_feature(0x05, 0x80), 3, 7); /* Enable lowest power mode w/o standby */
-        if (ata_identify_data[83] & BIT(9))
+        if (identify_info[83] & BIT(9))
             PASS_RC(ata_set_feature(0x42, 0x80), 3, 8); /* Enable lowest noise mode */
-        ATA_PIO_TIME = piotime;
-        ATA_MDMA_TIME = mdmatime;
-        ATA_UDMA_TIME = udmatime;
+
+        PASS_RC(ata_identify(identify_info), 3, 9); /* Finally, re-read identify info */
     }
+
     spinup_time = current_tick - spinup_start;
-    if (ata_lba48)
-        ata_total_sectors = ata_identify_data[100]
-                            | (((uint64_t)ata_identify_data[101]) << 16)
-                            | (((uint64_t)ata_identify_data[102]) << 32)
-                            | (((uint64_t)ata_identify_data[103]) << 48);
-    else
-        ata_total_sectors = ata_identify_data[60] | (((uint32_t)ata_identify_data[61]) << 16);
-    ata_total_sectors >>= 3; /* ie SECTOR_SIZE/512. */
+
+    ata_total_sectors = (identify_info[61] << 16) | identify_info[60];
+    if ( identify_info[83] & BIT(10) && ata_total_sectors == 0x0FFFFFFF)
+    {
+        ata_total_sectors = ((uint64_t)identify_info[103] << 48) |
+                ((uint64_t)identify_info[102] << 32) |
+                ((uint64_t)identify_info[101] << 16) |
+                identify_info[100];
+        ata_lba48 = true;
+    } else {
+        ata_lba48 = false;
+    }
+
     ata_powered = true;
     ata_set_active();
     return 0;
@@ -708,28 +768,8 @@ static int ata_power_up(void)
 
 static void ata_power_down(void)
 {
-    if (!ata_powered) return;
-    if (ceata)
-    {
-        memset(ceata_taskfile, 0, 16);
-        ceata_taskfile[0xf] = CMD_STANDBY_IMMEDIATE;
-        ceata_wait_idle();
-        ceata_write_multiple_register(0, ceata_taskfile, 16);
-        ceata_wait_idle();
-        sleep(HZ);
-        PWRCON(0) |= (1 << 9);
-    }
-    else
-    {
-        ata_wait_for_rdy(1000000);
-        ata_write_cbr(&ATA_PIO_DVR, 0);
-        ata_write_cbr(&ATA_PIO_CSD, CMD_STANDBY_IMMEDIATE);
-        ata_wait_for_rdy(1000000);
-        sleep(HZ / 30);
-        ATA_CONTROL = 0;
-        while (!(ATA_CONTROL & BIT(1))) yield();
-        PWRCON(0) |= (1 << 5);
-    }
+    if (!ata_powered)
+        return;
     PCON(7) = 0;
     PCON(8) = 0;
     PCON(9) = 0;
@@ -744,18 +784,18 @@ static int ata_rw_chunk_internal(uint64_t sector, uint32_t cnt, void* buffer, bo
     if (ceata)
     {
         memset(ceata_taskfile, 0, 16);
-        ceata_taskfile[0x2] = cnt >> 5;
-        ceata_taskfile[0x3] = sector >> 21;
-        ceata_taskfile[0x4] = sector >> 29;
-        ceata_taskfile[0x5] = sector >> 37;
-        ceata_taskfile[0xa] = cnt << 3;
-        ceata_taskfile[0xb] = sector << 3;
-        ceata_taskfile[0xc] = sector >> 5;
-        ceata_taskfile[0xd] = sector >> 13;
+        ceata_taskfile[0x2] = cnt >> 8;
+        ceata_taskfile[0x3] = sector >> 24;
+        ceata_taskfile[0x4] = sector >> 32;
+        ceata_taskfile[0x5] = sector >> 40;
+        ceata_taskfile[0xa] = cnt;
+        ceata_taskfile[0xb] = sector;
+        ceata_taskfile[0xc] = sector >> 8;
+        ceata_taskfile[0xd] = sector >> 16;
         ceata_taskfile[0xf] = write ? CMD_WRITE_DMA_EXT : CMD_READ_DMA_EXT;
         PASS_RC(ceata_wait_idle(), 2, 0);
         PASS_RC(ceata_write_multiple_register(0, ceata_taskfile, 16), 2, 1);
-        PASS_RC(ceata_rw_multiple_block(write, buffer, cnt << 3, CEATA_COMMAND_TIMEOUT * HZ / 1000000), 2, 2);
+        PASS_RC(ceata_rw_multiple_block(write, buffer, cnt, CEATA_COMMAND_TIMEOUT * HZ / 1000000), 2, 2);
     }
     else
     {
@@ -763,14 +803,14 @@ static int ata_rw_chunk_internal(uint64_t sector, uint32_t cnt, void* buffer, bo
         ata_write_cbr(&ATA_PIO_DVR, 0);
         if (ata_lba48)
         {
-            ata_write_cbr(&ATA_PIO_SCR, cnt >> 5);
-            ata_write_cbr(&ATA_PIO_SCR, (cnt << 3) & 0xff);
-            ata_write_cbr(&ATA_PIO_LHR, (sector >> 37) & 0xff);
-            ata_write_cbr(&ATA_PIO_LMR, (sector >> 29) & 0xff);
-            ata_write_cbr(&ATA_PIO_LLR, (sector >> 21) & 0xff);
-            ata_write_cbr(&ATA_PIO_LHR, (sector >> 13) & 0xff);
-            ata_write_cbr(&ATA_PIO_LMR, (sector >> 5) & 0xff);
-            ata_write_cbr(&ATA_PIO_LLR, (sector << 3) & 0xff);
+            ata_write_cbr(&ATA_PIO_SCR, (cnt >> 8) & 0xff);
+            ata_write_cbr(&ATA_PIO_SCR, (cnt) & 0xff);
+            ata_write_cbr(&ATA_PIO_LHR, (sector >> 40) & 0xff);
+            ata_write_cbr(&ATA_PIO_LMR, (sector >> 32) & 0xff);
+            ata_write_cbr(&ATA_PIO_LLR, (sector >> 24) & 0xff);
+            ata_write_cbr(&ATA_PIO_LHR, (sector >> 16) & 0xff);
+            ata_write_cbr(&ATA_PIO_LMR, (sector >> 8) & 0xff);
+            ata_write_cbr(&ATA_PIO_LLR, (sector) & 0xff);
             ata_write_cbr(&ATA_PIO_DVR, BIT(6));
             if (write)
                 ata_write_cbr(&ATA_PIO_CSD, ata_dma ? CMD_WRITE_DMA_EXT : CMD_WRITE_MULTIPLE_EXT);
@@ -779,16 +819,17 @@ static int ata_rw_chunk_internal(uint64_t sector, uint32_t cnt, void* buffer, bo
         }
         else
         {
-            ata_write_cbr(&ATA_PIO_SCR, (cnt << 3) & 0xff);
-            ata_write_cbr(&ATA_PIO_LHR, (sector >> 13) & 0xff);
-            ata_write_cbr(&ATA_PIO_LMR, (sector >> 5) & 0xff);
-            ata_write_cbr(&ATA_PIO_LLR, (sector << 3) & 0xff);
-            ata_write_cbr(&ATA_PIO_DVR, BIT(6) | ((sector >> 21) & 0xf));
+            ata_write_cbr(&ATA_PIO_SCR, (cnt) & 0xff);
+            ata_write_cbr(&ATA_PIO_LHR, (sector >> 16) & 0xff);
+            ata_write_cbr(&ATA_PIO_LMR, (sector >> 8) & 0xff);
+            ata_write_cbr(&ATA_PIO_LLR, (sector) & 0xff);
+            ata_write_cbr(&ATA_PIO_DVR, BIT(6) | ((sector >> 24) & 0xf)); /* LBA28, mask off upper 4 bits of 32-bit sector address */
             if (write)
                 ata_write_cbr(&ATA_PIO_CSD, ata_dma ? CMD_WRITE_DMA : CMD_WRITE_SECTORS);
             else
                 ata_write_cbr(&ATA_PIO_CSD, ata_dma ? CMD_READ_DMA : CMD_READ_MULTIPLE);
         }
+#ifdef HAVE_ATA_DMA
         if (ata_dma)
         {
             PASS_RC(ata_wait_for_start_of_transfer(500000), 2, 1);
@@ -811,8 +852,7 @@ static int ata_rw_chunk_internal(uint64_t sector, uint32_t cnt, void* buffer, bo
             ATA_IRQ = BITRANGE(0, 4);
             ATA_IRQ_MASK = BIT(0);
             ATA_COMMAND = BIT(0);
-            if (semaphore_wait(&ata_wakeup, 500000 * HZ / 1000000)
-                == OBJ_WAIT_TIMEDOUT)
+            if (semaphore_wait(&ata_wakeup, 500000 * HZ / 1000000) == OBJ_WAIT_TIMEDOUT)
             {
                 ATA_COMMAND = BIT(1);
                 ATA_CFG &= ~(BITRANGE(2, 3) | BIT(12));
@@ -822,19 +862,19 @@ static int ata_rw_chunk_internal(uint64_t sector, uint32_t cnt, void* buffer, bo
             ATA_CFG &= ~(BITRANGE(2, 3) | BIT(12));
         }
         else
+#endif // HAVE_ATA_DMA
         {
-            cnt *= SECTOR_SIZE / 512;
             while (cnt--)
             {
                 int i;
                 PASS_RC(ata_wait_for_start_of_transfer(500000), 2, 1);
                 if (write)
-                    for (i = 0; i < 256; i++)
+                    for (i = 0; i < SECTOR_SIZE/2; i++)
                         ata_write_cbr(&ATA_PIO_DTR, ((uint16_t*)buffer)[i]);
                 else
-                    for (i = 0; i < 256; i++)
+                    for (i = 0; i < SECTOR_SIZE/2; i++)
                         ((uint16_t*)buffer)[i] = ata_read_cbr(&ATA_PIO_DTR);
-                buffer += 512;
+                buffer += SECTOR_SIZE;
             }
         }
         PASS_RC(ata_wait_for_end_of_transfer(100000), 2, 3);
@@ -850,40 +890,27 @@ static int ata_rw_chunk(uint64_t sector, uint32_t cnt, void* buffer, bool write)
     return rc;
 }
 
-static int ata_rw_sectors(uint64_t sector, uint32_t count, void* buffer, bool write)
+static int ata_transfer_sectors(uint64_t sector, uint32_t count, void* buffer, bool write)
 {
-    if (STORAGE_OVERLAP((uint32_t)buffer))
-    {
-        while (count)
-        {
-            if (write)
-                memcpy(aligned_buffer, buffer, SECTOR_SIZE);
-
-            PASS_RC(ata_rw_sectors(sector, 1, aligned_buffer, write), 0, 0);
-
-            if (!write)
-                memcpy(buffer, aligned_buffer, SECTOR_SIZE);
-
-            buffer += SECTOR_SIZE;
-            sector++;
-            count--;
-        }
-
-        return 0;
-    }
-
-    if (!ata_powered) ata_power_up();
-    if (sector + count > ata_total_sectors) RET_ERR(0);
+    if (!ata_powered)
+        ata_power_up();
+    if (sector + count > ata_total_sectors)
+        RET_ERR(0);
     ata_set_active();
-    if (ata_dma && write) commit_dcache();
-    else if (ata_dma) commit_discard_dcache();
-    if (!ceata) ATA_COMMAND = BIT(1);
+    if (ata_dma && write)
+        commit_dcache();
+    else if (ata_dma)
+        commit_discard_dcache();
+    if (!ceata)
+        ATA_COMMAND = BIT(1);
+
     while (count)
     {
-        uint32_t cnt = MIN(ata_lba48 ? 8192 : 32, count);
+        uint32_t cnt = MIN(ata_lba48 ? 65536 : 256, count);
         int rc = -1;
         rc = ata_rw_chunk(sector, cnt, buffer, write);
-        if (rc && ata_error_srst) ata_reset();
+        if (rc && ata_error_srst)
+            ata_reset();
         if (rc && ata_retries)
         {
             void* buf = buffer;
@@ -895,9 +922,11 @@ static int ata_rw_sectors(uint64_t sector, uint32_t count, void* buffer, bool wr
                 while (tries-- && rc)
                 {
                     rc = ata_rw_chunk(sect, 1, buf, write);
-                    if (rc && ata_error_srst) ata_reset();
+                    if (rc && ata_error_srst)
+                        ata_reset();
                 }
-                if (rc) break;
+                if (rc)
+                    break;
                 buf += SECTOR_SIZE;
             }
         }
@@ -915,9 +944,13 @@ int ata_soft_reset(void)
 {
     int rc;
     mutex_lock(&ata_mutex);
-    if (!ata_powered) PASS_RC(ata_power_up(), 1, 0);
+    if (!ata_powered)
+        PASS_RC(ata_power_up(), 1, 0);
     ata_set_active();
-    if (ceata) rc = ceata_soft_reset();
+    if (ceata)
+    {
+        rc = ceata_soft_reset();
+    }
     else
     {
         ata_write_cbr(&ATA_PIO_DAD, BIT(1) | BIT(2));
@@ -944,7 +977,8 @@ static int ata_reset(void)
 {
     int rc;
     mutex_lock(&ata_mutex);
-    if (!ata_powered) PASS_RC(ata_power_up(), 2, 0);
+    if (!ata_powered)
+        PASS_RC(ata_power_up(), 2, 0);
     ata_set_active();
     rc = ata_soft_reset();
     if (IS_ERR(rc))
@@ -956,7 +990,8 @@ static int ata_reset(void)
             ata_power_down();
             sleep(HZ * 3);
             int rc2 = ata_power_up();
-            if (IS_ERR(rc2)) rc = ERR_RC((rc << 2) | 2);
+            if (IS_ERR(rc2))
+                rc = ERR_RC((rc << 2) | 2);
         }
         else rc = 1;
     }
@@ -965,11 +1000,18 @@ static int ata_reset(void)
     return rc;
 }
 
+#include "ata-common.c"
+
+#ifndef MAX_PHYS_SECTOR_SIZE
 int ata_read_sectors(IF_MD(int drive,) sector_t start, int incount,
                      void* inbuf)
 {
+#ifdef HAVE_MULTIDRIVE
+    (void)drive; /* unused for now */
+#endif
+
     mutex_lock(&ata_mutex);
-    int rc = ata_rw_sectors(start, incount, inbuf, false);
+    int rc = ata_transfer_sectors(start, incount, inbuf, false);
     mutex_unlock(&ata_mutex);
     return rc;
 }
@@ -977,11 +1019,16 @@ int ata_read_sectors(IF_MD(int drive,) sector_t start, int incount,
 int ata_write_sectors(IF_MD(int drive,) sector_t start, int count,
                       const void* outbuf)
 {
+#ifdef HAVE_MULTIDRIVE
+    (void)drive; /* unused for now */
+#endif
+
     mutex_lock(&ata_mutex);
-    int rc = ata_rw_sectors(start, count, (void*)((uint32_t)outbuf), true);
+    int rc = ata_transfer_sectors(start, count, (void*)((uint32_t)outbuf), true);
     mutex_unlock(&ata_mutex);
     return rc;
 }
+#endif /* ndef MAX_PHYS_SECTOR_SIZE */
 
 void ata_spindown(int seconds)
 {
@@ -992,26 +1039,27 @@ static void ata_flush_cache(void)
 {
     uint8_t cmd;
 
-    if (ata_identify_data[83] & BIT(13)) {
-        cmd = CMD_FLUSH_CACHE_EXT;
-    } else if (ata_identify_data[83] & BIT(12)) {
-        cmd = CMD_FLUSH_CACHE;
-    } else {
-        /* If neither (mandatory!) command is supported
-           then don't issue it. */
-       return;
-    }
-
-    if (ceata)
-    {
+    if (ceata) {
         memset(ceata_taskfile, 0, 16);
-        ceata_taskfile[0xf] = cmd;
+        ceata_taskfile[0xf] = CMD_FLUSH_CACHE_EXT;  /* CE-ATA only supports EXT */
         ceata_wait_idle();
         ceata_write_multiple_register(0, ceata_taskfile, 16);
         ceata_wait_idle();
-    }
-    else
-    {
+    } else {
+        if (!canflush) {
+            return;
+        } else if (ata_lba48 && identify_info[83] & BIT(13)) {
+            cmd = CMD_FLUSH_CACHE_EXT; /* Flag, optional, ATA-6 and up, for use with LBA48 devices. Mandatory for CE-ATA */
+        } else if (identify_info[83] & BIT(12)) {
+            cmd = CMD_FLUSH_CACHE; /* Flag, mandatory, ATA-6 and up */
+        } else if (identify_info[80] >= BIT(5)) {  /* Use >= instead of '&' because bits lower than the latest standard we support don't have to be set */
+            cmd = CMD_FLUSH_CACHE; /* No flag, mandatory, ATA-5  (Optional for ATA-4) */
+        } else {
+            /* If neither command is supported then don't issue it. */
+            canflush = 0;
+            return;
+        }
+
         ata_wait_for_rdy(1000000);
         ata_write_cbr(&ATA_PIO_DVR, 0);
         ata_write_cbr(&ATA_PIO_CSD, cmd);
@@ -1019,14 +1067,46 @@ static void ata_flush_cache(void)
     }
 }
 
+int ata_flush(void)
+{
+    if (ata_powered) {
+        mutex_lock(&ata_mutex);
+        ata_flush_cache();
+        mutex_unlock(&ata_mutex);
+    }
+    return 0;
+}
+
 void ata_sleepnow(void)
 {
     mutex_lock(&ata_mutex);
 
-    if (ata_disk_can_poweroff())
-        ata_power_down();
-    else
-        ata_flush_cache();
+    ata_flush_cache();
+
+    if (ata_disk_can_sleep()) {
+        if (ceata) {
+            memset(ceata_taskfile, 0, 16);
+            ceata_taskfile[0xf] = CMD_STANDBY_IMMEDIATE;
+            ceata_wait_idle();
+            ceata_write_multiple_register(0, ceata_taskfile, 16);
+            ceata_wait_idle();
+            sleep(HZ);
+            PWRCON(0) |= (1 << 9);
+        } else {
+            ata_wait_for_rdy(1000000);
+            ata_write_cbr(&ATA_PIO_DVR, 0);
+            ata_write_cbr(&ATA_PIO_CSD, CMD_STANDBY_IMMEDIATE);
+            ata_wait_for_rdy(1000000);
+            sleep(HZ / 30);
+            ATA_CONTROL = 0;
+            while (!(ATA_CONTROL & BIT(1)))
+                yield();
+            PWRCON(0) |= (1 << 5);
+        }
+    }
+
+    if (ata_disk_can_sleep() || canflush)
+        ata_power_down(); // XXX add a powerdown delay similar to main ATA driver?
 
     mutex_unlock(&ata_mutex);
 }
@@ -1036,14 +1116,21 @@ void ata_spin(void)
     ata_set_active();
 }
 
+#ifdef STORAGE_GET_INFO
 void ata_get_info(IF_MD(int drive,) struct storage_info *info)
 {
-    (*info).sector_size = SECTOR_SIZE;
+    /* Logical sector size */
+    if ((identify_info[106] & 0xd000) == 0x5000) /* B14, B12 */
+        info->sector_size = (identify_info[117] | (identify_info[118] << 16)) * 2;
+    else
+        info->sector_size = SECTOR_SIZE;
+
     (*info).num_sectors = ata_total_sectors;
     (*info).vendor = "Apple";
     (*info).product = "iPod Classic";
     (*info).revision = "1.0";
 }
+#endif
 
 long ata_last_disk_activity(void)
 {
@@ -1060,11 +1147,18 @@ int ata_init(void)
     ata_powered = false;
     ata_total_sectors = 0;
 
-    /* get ata_identify_data */
+    /* get identify_info */
     mutex_lock(&ata_mutex);
     int rc = ata_power_up();
     mutex_unlock(&ata_mutex);
-    if (IS_ERR(rc)) return rc;
+    if (IS_ERR(rc))
+        return rc;
+
+#ifdef MAX_PHYS_SECTOR_SIZE
+    rc = ata_get_phys_sector_mult();
+    if (IS_ERR(rc))
+        return rc;
+#endif
 
     return 0;
 }
@@ -1081,7 +1175,7 @@ static int ata_smart(uint16_t* buf)
         ceata_taskfile[0xe] = BIT(6);
         ceata_taskfile[0xf] = CMD_SMART;
         PASS_RC(ceata_wait_idle(), 3, 1);
-        if (((uint8_t*)ata_identify_data)[54] != 'A')  /* Model != aAmsung */
+        if (((uint8_t*)identify_info)[54] != 'A')  /* Model != aAmsung */
         {
             ceata_taskfile[0x9] = 0xd8; /* SMART enable operations */
             PASS_RC(ceata_write_multiple_register(0, ceata_taskfile, 16), 3, 2);
@@ -1101,7 +1195,8 @@ static int ata_smart(uint16_t* buf)
         ata_write_cbr(&ATA_PIO_DVR, BIT(6));
         ata_write_cbr(&ATA_PIO_CSD, CMD_SMART);
         PASS_RC(ata_wait_for_start_of_transfer(10000000), 3, 7);
-        for (i = 0; i < 0x100; i++) buf[i] = ata_read_cbr(&ATA_PIO_DTR);
+        for (i = 0; i < 0x100; i++)
+            buf[i] = ata_read_cbr(&ATA_PIO_DTR);
     }
     ata_set_active();
     return 0;
@@ -1128,7 +1223,7 @@ static int ata_num_drives(int first_drive)
 
 unsigned short* ata_get_identify(void)
 {
-    return ata_identify_data;
+    return identify_info;
 }
 
 int ata_spinup_time(void)
@@ -1136,24 +1231,29 @@ int ata_spinup_time(void)
     return spinup_time;
 }
 
+#ifdef HAVE_ATA_DMA
 int ata_get_dma_mode(void)
 {
     return dma_mode;
 }
+#endif
 
 void INT_ATA(void)
 {
     uint32_t ata_irq = ATA_IRQ;
     ATA_IRQ = ata_irq;
-    if (ata_irq & ATA_IRQ_MASK) semaphore_release(&ata_wakeup);
+    if (ata_irq & ATA_IRQ_MASK)
+        semaphore_release(&ata_wakeup);
     ATA_IRQ_MASK = 0;
 }
 
 void INT_MMC(void)
 {
     uint32_t irq = SDCI_IRQ;
-    if (irq & SDCI_IRQ_DAT_DONE_INT) semaphore_release(&mmc_wakeup);
-    if (irq & SDCI_IRQ_IOCARD_IRQ_INT) semaphore_release(&mmc_comp_wakeup);
+    if (irq & SDCI_IRQ_DAT_DONE_INT)
+        semaphore_release(&mmc_wakeup);
+    if (irq & SDCI_IRQ_IOCARD_IRQ_INT)
+        semaphore_release(&mmc_comp_wakeup);
     SDCI_IRQ = irq;
 }
 
diff --git a/firmware/target/arm/tms320dm320/system-dm320.c b/firmware/target/arm/tms320dm320/system-dm320.c
index 6cf616184d..62eaf1f8ce 100644
--- a/firmware/target/arm/tms320dm320/system-dm320.c
+++ b/firmware/target/arm/tms320dm320/system-dm320.c
@@ -100,7 +100,7 @@ default_interrupt(RESERVED);
  * change the offset for the interrupt in the entry table.
  */
 
-static const unsigned short const irqpriority[] =
+static const unsigned short irqpriority[] =
 {
     IRQ_TIMER0,IRQ_TIMER1,IRQ_TIMER2,IRQ_TIMER3,IRQ_CCD_VD0,IRQ_CCD_VD1,
     IRQ_CCD_WEN,IRQ_VENC,IRQ_SERIAL0,IRQ_SERIAL1,IRQ_EXT_HOST,IRQ_DSPHINT,
diff --git a/firmware/target/coldfire/system-coldfire.c b/firmware/target/coldfire/system-coldfire.c
index ed130fca1f..2f2f2cb45b 100644
--- a/firmware/target/coldfire/system-coldfire.c
+++ b/firmware/target/coldfire/system-coldfire.c
@@ -26,6 +26,11 @@
 #include "lcd.h"
 #include "font.h"
 
+#if __GNUC__ >= 9
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmissing-attributes"
+#endif
+
 #define default_interrupt(name) \
   extern __attribute__((weak,alias("UIE"))) void name (void)
 
diff --git a/firmware/target/hosted/agptek/debug-agptek.c b/firmware/target/hosted/agptek/debug-agptek.c
index a9b829f7ec..de4bc946bc 100644
--- a/firmware/target/hosted/agptek/debug-agptek.c
+++ b/firmware/target/hosted/agptek/debug-agptek.c
@@ -37,6 +37,8 @@
 
 static int line = 0;
 
+extern int hwver;
+
 bool dbg_hw_info(void)
 {
     int btn = 0;
@@ -61,6 +63,10 @@ bool dbg_hw_info(void)
             lcd_putsf(0, line++, "Boot ver: %s", verstr);
         }
 
+#ifdef EROS_Q
+        lcd_putsf(0, line++, "hwver: %d", hwver);
+#endif
+
         lcd_putsf(0, line++, "pcm srate: %d", pcm_alsa_get_rate());
         lcd_putsf(0, line++, "pcm xruns: %d", pcm_alsa_get_xruns());
 #ifdef HAVE_HEADPHONE_DETECTION
diff --git a/firmware/target/hosted/aigo/power-erosq.c b/firmware/target/hosted/aigo/power-erosq.c
index 73fa5fe972..7c4515f616 100644
--- a/firmware/target/hosted/aigo/power-erosq.c
+++ b/firmware/target/hosted/aigo/power-erosq.c
@@ -28,17 +28,34 @@
 #include "power.h"
 #include "panic.h"
 #include "sysfs.h"
+#include "tick.h"
 
-const char * const sysfs_bat_voltage =
-    "/sys/class/power_supply/battery/voltage_now";
+const char * const sysfs_bat_voltage[2] = {
+    "/sys/class/power_supply/battery/voltage_now",
+    "/sys/class/power_supply/axp_battery/voltage_now",
+};
 
-const char * const sysfs_bat_capacity =
-    "/sys/class/power_supply/battery/capacity";
+const char * const sysfs_bat_capacity[2] = {
+    "/sys/class/power_supply/battery/capacity",
+    "/sys/class/power_supply/axp_battery/capacity",
+};
+
+const char * const sysfs_bat_status[2] = {
+    "/sys/class/power_supply/battery/status",
+    "/sys/class/power_supply/axp_battery/status",
+};
+
+int hwver = 1;  /* Exported */
 
 unsigned int erosq_power_get_battery_voltage(void)
 {
     int battery_voltage;
-    sysfs_get_int(sysfs_bat_voltage, &battery_voltage);
+    int x = sysfs_get_int(sysfs_bat_voltage[hwver == 4], &battery_voltage);
+
+    if (!x && hwver != 4) {
+        hwver = 4;
+        sysfs_get_int(sysfs_bat_voltage[hwver == 4], &battery_voltage);
+    }
 
     return battery_voltage/1000;
 }
@@ -46,7 +63,33 @@ unsigned int erosq_power_get_battery_voltage(void)
 unsigned int erosq_power_get_battery_capacity(void)
 {
     int battery_capacity;
-    sysfs_get_int(sysfs_bat_capacity, &battery_capacity);
+    int x = sysfs_get_int(sysfs_bat_capacity[hwver == 4], &battery_capacity);
+
+    if (!x && hwver != 4) {
+        hwver = 4;
+        sysfs_get_int(sysfs_bat_capacity[hwver == 4], &battery_capacity);
+    }
 
     return battery_capacity;
 }
+
+/* We get called multiple times per tick, let's cut that back! */
+static long last_tick = 0;
+static bool last_power = false;
+
+bool charging_state(void)
+{
+    if ((current_tick - last_tick) > HZ/2 ) {
+        char buf[12] = {0};
+        int x = sysfs_get_string(sysfs_bat_status[hwver == 4], buf, sizeof(buf));
+
+        if (!x && hwver != 4) {
+            hwver = 4;
+            sysfs_get_string(sysfs_bat_status[hwver == 4], buf, sizeof(buf));
+        }
+
+        last_tick = current_tick;
+        last_power = (strncmp(buf, "Charging", 8) == 0);
+    }
+    return last_power;
+}
diff --git a/firmware/target/hosted/alsa-controls.h b/firmware/target/hosted/alsa-controls.h
index af3e584cd9..b868ef52fb 100644
--- a/firmware/target/hosted/alsa-controls.h
+++ b/firmware/target/hosted/alsa-controls.h
@@ -36,6 +36,8 @@ void alsa_controls_close(void);
 
 /* check wether a control exists */
 bool alsa_has_control(const char *name);
+/* find a control element ID by name, return -1 of not found or index into array */
+int alsa_controls_find(const char *name);
 /* find a control element enum index by name, return -1 if not found */
 int alsa_controls_find_enum(const char *name, const char *enum_name);
 /* set a control, potentially supports several values */
diff --git a/firmware/target/hosted/power-linux.c b/firmware/target/hosted/power-linux.c
index ddd7ad63e3..8d263eebec 100644
--- a/firmware/target/hosted/power-linux.c
+++ b/firmware/target/hosted/power-linux.c
@@ -33,9 +33,13 @@
 #include "usb.h"
 #endif
 
+#ifdef BATTERY_DEV_NAME
 #define BATTERY_STATUS_PATH "/sys/class/power_supply/" BATTERY_DEV_NAME "/status"
+#endif
+
 #define POWER_STATUS_PATH "/sys/class/power_supply/" POWER_DEV_NAME "/online"
 
+#ifdef BATTERY_DEV_NAME
 /* We get called multiple times per tick, let's cut that back! */
 static long last_tick = 0;
 static bool last_power = false;
@@ -51,6 +55,7 @@ bool charging_state(void)
     }
     return last_power;
 }
+#endif
 
 unsigned int power_input_status(void)
 {
diff --git a/firmware/target/hosted/sdl/app/button-application.c b/firmware/target/hosted/sdl/app/button-application.c
index 9c313b76b1..8071df201a 100644
--- a/firmware/target/hosted/sdl/app/button-application.c
+++ b/firmware/target/hosted/sdl/app/button-application.c
@@ -40,23 +40,23 @@ int key_to_button(int keyboard_key)
 #if (CONFIG_PLATFORM & PLATFORM_MAEMO4)
         case SDLK_ESCAPE:
 #endif
-        case SDLK_KP7:
+        case SDLK_KP_7:
             new_btn = BUTTON_TOPLEFT;
             break;
-        case SDLK_KP8:
+        case SDLK_KP_8:
         case SDLK_UP:
             new_btn = BUTTON_TOPMIDDLE;
             break;
 #if (CONFIG_PLATFORM & PLATFORM_MAEMO4)
         case SDLK_F7:
 #endif
-        case SDLK_KP9:
+        case SDLK_KP_9:
             new_btn = BUTTON_TOPRIGHT;
             break;
 #if (CONFIG_PLATFORM & PLATFORM_PANDORA)
         case SDLK_RSHIFT:
 #endif
-        case SDLK_KP4:
+        case SDLK_KP_4:
         case SDLK_LEFT:
             new_btn = BUTTON_MIDLEFT;
             break;
@@ -64,40 +64,32 @@ int key_to_button(int keyboard_key)
         case SDLK_RETURN:
         case SDLK_KP_ENTER:
 #endif
-        case SDLK_KP5:
+        case SDLK_KP_5:
             new_btn = BUTTON_CENTER;
             break;
 #if (CONFIG_PLATFORM & PLATFORM_PANDORA)
         case SDLK_RCTRL:
 #endif
-        case SDLK_KP6:
+        case SDLK_KP_6:
         case SDLK_RIGHT:
             new_btn = BUTTON_MIDRIGHT;
             break;
 #if (CONFIG_PLATFORM & PLATFORM_MAEMO4)
         case SDLK_F6:
 #endif
-        case SDLK_KP1:
+        case SDLK_KP_1:
             new_btn = BUTTON_BOTTOMLEFT;
             break;
-        case SDLK_KP2:
+        case SDLK_KP_2:
         case SDLK_DOWN:
             new_btn = BUTTON_BOTTOMMIDDLE;
             break;
 #if (CONFIG_PLATFORM & PLATFORM_MAEMO4)
         case SDLK_F8:
 #endif
-        case SDLK_KP3:
+        case SDLK_KP_3:
             new_btn = BUTTON_BOTTOMRIGHT;
             break;
-#ifdef HAVE_SCROLLWHEEL
-        case SDL_BUTTON_WHEELUP:
-            new_btn = BUTTON_SCROLL_BACK;
-            break;
-        case SDL_BUTTON_WHEELDOWN:
-            new_btn = BUTTON_SCROLL_FWD;
-            break;
-#endif
         case SDL_BUTTON_RIGHT:
             new_btn = BUTTON_MIDLEFT;
             break;
diff --git a/firmware/target/hosted/sdl/button-sdl.c b/firmware/target/hosted/sdl/button-sdl.c
index b5f3720a3b..8af6f3ffaf 100644
--- a/firmware/target/hosted/sdl/button-sdl.c
+++ b/firmware/target/hosted/sdl/button-sdl.c
@@ -107,6 +107,30 @@ static void touchscreen_event(int x, int y)
 }
 #endif
 
+#if defined(HAVE_SCROLLWHEEL) || ((defined(BUTTON_SCROLL_FWD) && defined(BUTTON_SCROLL_BACK)))
+static void scrollwheel_event(int x, int y)
+{
+    int new_btn = 0;
+    if (y > 0)
+        new_btn = BUTTON_SCROLL_BACK;
+    else if (y < 0)
+        new_btn = BUTTON_SCROLL_FWD;
+    else
+        return;
+
+#ifdef HAVE_BACKLIGHT
+    backlight_on();
+#endif
+#ifdef HAVE_BUTTON_LIGHT
+    buttonlight_on();
+#endif
+    reset_poweroff_timer();
+    if (new_btn && !queue_full(&button_queue))
+        queue_post(&button_queue, new_btn, 1<<24);
+
+    (void)x;
+}
+#endif
 static void mouse_event(SDL_MouseButtonEvent *event, bool button_up)
 {
 #define SQUARE(x) ((x)*(x))
@@ -118,10 +142,6 @@ static void mouse_event(SDL_MouseButtonEvent *event, bool button_up)
     if(button_up) {
         switch ( event->button )
         {
-#ifdef HAVE_SCROLLWHEEL
-        case SDL_BUTTON_WHEELUP:
-        case SDL_BUTTON_WHEELDOWN:
-#endif
         case SDL_BUTTON_MIDDLE:
         case SDL_BUTTON_RIGHT:
             button_event( event->button, false );
@@ -145,13 +165,9 @@ static void mouse_event(SDL_MouseButtonEvent *event, bool button_up)
 #endif
             break;
         }
-    } else {    /* button down */
+    } else {   /* button down */
         switch ( event->button )
         {
-#ifdef HAVE_SCROLLWHEEL
-        case SDL_BUTTON_WHEELUP:
-        case SDL_BUTTON_WHEELDOWN:
-#endif
         case SDL_BUTTON_MIDDLE:
         case SDL_BUTTON_RIGHT:
             button_event( event->button, true );
@@ -215,18 +231,15 @@ static void mouse_event(SDL_MouseButtonEvent *event, bool button_up)
 
 static bool event_handler(SDL_Event *event)
 {
-    SDLKey ev_key;
+    SDL_Keycode ev_key;
 
     switch(event->type)
     {
-    case SDL_ACTIVEEVENT:
-        if (event->active.state & SDL_APPINPUTFOCUS)
-        {
-            if (event->active.gain == 1)
-                sdl_app_has_input_focus = 1;
-            else
-                sdl_app_has_input_focus = 0;
-        }
+    case SDL_WINDOWEVENT:
+        if (event->window.event == SDL_WINDOWEVENT_FOCUS_GAINED)
+            sdl_app_has_input_focus = 1;
+        else if (event->window.event == SDL_WINDOWEVENT_FOCUS_LOST)
+            sdl_app_has_input_focus = 0;
         break;
     case SDL_KEYDOWN:
     case SDL_KEYUP:
@@ -275,6 +288,13 @@ static bool event_handler(SDL_Event *event)
         mouse_event(mev, event->type == SDL_MOUSEBUTTONUP);
         break;
     }
+#ifdef HAVE_SCROLLWHEEL
+    case SDL_MOUSEWHEEL:
+    {
+        scrollwheel_event(event->wheel.x, event->wheel.y);
+        break;
+    }
+#endif
     case SDL_QUIT:
         /* Will post SDL_USEREVENT in shutdown_hw() if successful. */
         sdl_sys_quit();
@@ -362,7 +382,7 @@ static void button_event(int key, bool pressed)
         }
         return;
 #endif
-        
+
 #ifdef HAS_REMOTE_BUTTON_HOLD
     case SDLK_j:
         if(pressed)
@@ -379,7 +399,7 @@ static void button_event(int key, bool pressed)
         if(pressed)
             switch(_remote_type)
             {
-                case REMOTETYPE_UNPLUGGED: 
+                case REMOTETYPE_UNPLUGGED:
                     _remote_type=REMOTETYPE_H100_LCD;
                     DEBUGF("Changed remote type to H100\n");
                     break;
@@ -399,7 +419,7 @@ static void button_event(int key, bool pressed)
         break;
 #endif
 #ifndef APPLICATION
-    case SDLK_KP0:
+    case SDLK_KP_0:
     case SDLK_F5:
         if(pressed)
         {
@@ -430,32 +450,17 @@ static void button_event(int key, bool pressed)
 #endif
         break;
     }
-    /* Call to make up for scrollwheel target implementation.  This is
-     * not handled in the main button.c driver, but on the target
-     * implementation (look at button-e200.c for example if you are trying to 
-     * figure out why using button_get_data needed a hack before).
-     */
+
 #if defined(BUTTON_SCROLL_FWD) && defined(BUTTON_SCROLL_BACK)
-    if((new_btn == BUTTON_SCROLL_FWD || new_btn == BUTTON_SCROLL_BACK) && 
+    if((new_btn == BUTTON_SCROLL_FWD || new_btn == BUTTON_SCROLL_BACK) &&
         pressed)
     {
-        /* Clear these buttons from the data - adding them to the queue is
-         *  handled in the scrollwheel drivers for the targets.  They do not
-         *  store the scroll forward/back buttons in their button data for
-         *  the button_read call.
-         */
-#ifdef HAVE_BACKLIGHT
-        backlight_on();
-#endif
-#ifdef HAVE_BUTTON_LIGHT
-        buttonlight_on();
-#endif
-        reset_poweroff_timer();
-        queue_post(&button_queue, new_btn, 1<<24);
+        scrollwheel_event(0, new_btn == BUTTON_SCROLL_FWD ? -1 : 1);
         new_btn &= ~(BUTTON_SCROLL_FWD | BUTTON_SCROLL_BACK);
     }
 #endif
 
+    /* Update global button press state */
     if (pressed)
         btn |= new_btn;
     else
@@ -496,5 +501,4 @@ int button_read_device(void)
 
 void button_init_device(void)
 {
-    SDL_EnableKeyRepeat(SDL_DEFAULT_REPEAT_DELAY, SDL_DEFAULT_REPEAT_INTERVAL);
 }
diff --git a/firmware/target/hosted/sdl/kernel-sdl.c b/firmware/target/hosted/sdl/kernel-sdl.c
index 5c16f86749..bcad39b4d8 100644
--- a/firmware/target/hosted/sdl/kernel-sdl.c
+++ b/firmware/target/hosted/sdl/kernel-sdl.c
@@ -153,7 +153,7 @@ void sim_kernel_shutdown(void)
         SDL_Delay(10);
 
     SDL_DestroyMutex(sim_irq_mtx);
-    SDL_DestroyCond(sim_thread_cond); 
+    SDL_DestroyCond(sim_thread_cond);
 }
 
 Uint32 tick_timer(Uint32 interval, void *param)
@@ -162,9 +162,9 @@ Uint32 tick_timer(Uint32 interval, void *param)
 
     (void) interval;
     (void) param;
-    
+
     new_tick = (SDL_GetTicks() - start_tick) / (1000/HZ);
-        
+
     while(new_tick != current_tick)
     {
         sim_enter_irq_handler();
@@ -175,7 +175,7 @@ Uint32 tick_timer(Uint32 interval, void *param)
 
         sim_exit_irq_handler();
     }
-    
+
     return interval;
 }
 
@@ -187,10 +187,10 @@ void tick_start(unsigned int interval_in_ms)
         exit(-1);
     }
 
-    if (tick_timer_id != NULL)
+    if (tick_timer_id != 0)
     {
         SDL_RemoveTimer(tick_timer_id);
-        tick_timer_id = NULL;
+        tick_timer_id = 0;
     }
     else
     {
diff --git a/firmware/target/hosted/sdl/key_to_touch-sdl.c b/firmware/target/hosted/sdl/key_to_touch-sdl.c
index d56d06ba74..b7052e043f 100644
--- a/firmware/target/hosted/sdl/key_to_touch-sdl.c
+++ b/firmware/target/hosted/sdl/key_to_touch-sdl.c
@@ -52,12 +52,12 @@ int key_to_touch(int keyboard_button, unsigned int mouse_coords)
             }
             break;
 #ifndef APPLICATION
-        case SDLK_KP7:
+        case SDLK_KP_7:
         case SDLK_7:
         case SDLK_HOME:
             new_btn = BUTTON_TOPLEFT;
             break;
-        case SDLK_KP8:
+        case SDLK_KP_8:
         case SDLK_8:
         case SDLK_UP:
 #ifdef HAVE_SCROLLWHEEL
@@ -65,32 +65,32 @@ int key_to_touch(int keyboard_button, unsigned int mouse_coords)
 #endif
             new_btn = BUTTON_TOPMIDDLE;
             break;
-        case SDLK_KP9:
+        case SDLK_KP_9:
         case SDLK_9:
         case SDLK_PAGEUP:
             new_btn = BUTTON_TOPRIGHT;
             break;
-        case SDLK_KP4:
+        case SDLK_KP_4:
         case SDLK_u:
         case SDLK_LEFT:
             new_btn = BUTTON_MIDLEFT;
             break;
-        case SDLK_KP5:
+        case SDLK_KP_5:
         case SDLK_i:
         case SDL_BUTTON_MIDDLE:
             new_btn = BUTTON_CENTER;
             break;
-        case SDLK_KP6:
+        case SDLK_KP_6:
         case SDLK_o:
         case SDLK_RIGHT:
             new_btn = BUTTON_MIDRIGHT;
             break;
-        case SDLK_KP1:
+        case SDLK_KP_1:
         case SDLK_j:
         case SDLK_END:
             new_btn = BUTTON_BOTTOMLEFT;
             break;
-        case SDLK_KP2:
+        case SDLK_KP_2:
         case SDLK_k:
 #ifdef HAVE_SCROLLWHEEL
         case SDL_BUTTON_WHEELDOWN:
@@ -98,7 +98,7 @@ int key_to_touch(int keyboard_button, unsigned int mouse_coords)
         case SDLK_DOWN:
             new_btn = BUTTON_BOTTOMMIDDLE;
             break;
-        case SDLK_KP3:
+        case SDLK_KP_3:
         case SDLK_l:
         case SDLK_PAGEDOWN:
             new_btn = BUTTON_BOTTOMRIGHT;
@@ -106,4 +106,5 @@ int key_to_touch(int keyboard_button, unsigned int mouse_coords)
 #endif
     }
     return new_btn;
+
 }
diff --git a/firmware/target/hosted/sdl/lcd-bitmap.c b/firmware/target/hosted/sdl/lcd-bitmap.c
index ea30afea3b..ef284eeb48 100644
--- a/firmware/target/hosted/sdl/lcd-bitmap.c
+++ b/firmware/target/hosted/sdl/lcd-bitmap.c
@@ -164,12 +164,12 @@ void sim_backlight(int value)
 #else /* LCD_DEPTH > 8 */
 #if defined(HAVE_TRANSFLECTIVE_LCD) && defined(HAVE_LCD_SLEEP)
     if (!lcd_active())
-        SDL_SetAlpha(lcd_surface, SDL_SRCALPHA, 0);
+        SDL_SetSurfaceAlphaMod(lcd_surface, 0);
     else
-        SDL_SetAlpha(lcd_surface, SDL_SRCALPHA,
+        SDL_SetSurfaceAlphaMod(lcd_surface,
                         MAX(BACKLIGHT_OFF_ALPHA, (value * 255) / 100));
 #else
-    SDL_SetAlpha(lcd_surface, SDL_SRCALPHA, (value * 255) / 100);
+    SDL_SetSurfaceAlphaMod(lcd_surface, (value * 255) / 100);
 #endif
 #endif /* LCD_DEPTH */
 
@@ -187,6 +187,7 @@ void lcd_init_device(void)
                                        SIM_LCD_WIDTH * display_zoom,
                                        SIM_LCD_HEIGHT * display_zoom,
                                        LCD_DEPTH, 0, 0, 0, 0);
+    SDL_SetSurfaceBlendMode(lcd_surface, SDL_BLENDMODE_BLEND);
 #elif LCD_DEPTH <= 8
     lcd_surface = SDL_CreateRGBSurface(SDL_SWSURFACE,
                                        SIM_LCD_WIDTH * display_zoom,
@@ -222,7 +223,7 @@ void sim_lcd_ex_update_rect(int x_start, int y_start, int width, int height)
     if (lcd_ex_getpixel) {
         sdl_update_rect(lcd_surface, x_start, y_start, width, height,
                         LCD_WIDTH, LCD_HEIGHT, lcd_ex_getpixel);
-        sdl_gui_update(lcd_surface, x_start, y_start, width, 
+        sdl_gui_update(lcd_surface, x_start, y_start, width,
                        height + LCD_SPLIT_LINES, SIM_LCD_WIDTH, SIM_LCD_HEIGHT,
                        background ? UI_LCD_POSX : 0,
                        background ? UI_LCD_POSY : 0);
diff --git a/firmware/target/hosted/sdl/lcd-sdl.c b/firmware/target/hosted/sdl/lcd-sdl.c
index 8cbb6c5651..1e9daeffd3 100644
--- a/firmware/target/hosted/sdl/lcd-sdl.c
+++ b/firmware/target/hosted/sdl/lcd-sdl.c
@@ -115,12 +115,17 @@ void sdl_gui_update(SDL_Surface *surface, int x_start, int y_start, int width,
                     (ui_y + y_start) * display_zoom,
                     width * display_zoom, height * display_zoom};
 
-    if (surface->flags & SDL_SRCALPHA) /* alpha needs a black background */
+    uint8_t alpha;
+    if (SDL_GetSurfaceAlphaMod(surface,&alpha) == 0 && alpha < 255)
         SDL_FillRect(gui_surface, &dest, 0);
 
-    SDL_BlitSurface(surface, &src, gui_surface, &dest);
+    SDL_BlitSurface(surface, &src, gui_surface, &dest); /* alpha needs a black background */
 
-    SDL_Flip(gui_surface);
+    SDL_Texture *sdlTexture = SDL_CreateTextureFromSurface(sdlRenderer, gui_surface);
+    SDL_RenderClear(sdlRenderer);
+    SDL_RenderCopy(sdlRenderer, sdlTexture, NULL, NULL);
+    SDL_RenderPresent(sdlRenderer);
+    SDL_DestroyTexture(sdlTexture);
 }
 
 /* set a range of bitmap indices to a gradient from startcolour to endcolour */
@@ -136,7 +141,7 @@ void sdl_set_gradient(SDL_Surface *surface, SDL_Color *start, SDL_Color *end,
         palette[i].b = start->b + (end->b - start->b) * i / (steps - 1);
     }
 
-    SDL_SetPalette(surface, SDL_LOGPAL|SDL_PHYSPAL, palette, first, steps);
+    SDL_SetPaletteColors(surface->format->palette, palette, first , steps);
 }
 
 int lcd_get_dpi(void)
diff --git a/firmware/target/hosted/sdl/lcd-sdl.h b/firmware/target/hosted/sdl/lcd-sdl.h
index a964c9bc40..3f1334e4a2 100644
--- a/firmware/target/hosted/sdl/lcd-sdl.h
+++ b/firmware/target/hosted/sdl/lcd-sdl.h
@@ -3,7 +3,7 @@
  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
- *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \ 
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
  *                     \/            \/     \/    \/            \/
  * $Id$
  *
@@ -27,6 +27,7 @@
 
 /* Default display zoom level */
 extern SDL_Surface *gui_surface;
+extern SDL_Renderer *sdlRenderer;
 
 void sdl_update_rect(SDL_Surface *surface, int x_start, int y_start, int width,
                      int height, int max_x, int max_y,
@@ -39,4 +40,3 @@ void sdl_set_gradient(SDL_Surface *surface, SDL_Color *start, SDL_Color *end,
                       int first, int steps);
 
 #endif /* #ifndef __LCDSDL_H__ */
-
diff --git a/firmware/target/hosted/sdl/pcm-sdl.c b/firmware/target/hosted/sdl/pcm-sdl.c
index 940403002f..dec12af2f1 100644
--- a/firmware/target/hosted/sdl/pcm-sdl.c
+++ b/firmware/target/hosted/sdl/pcm-sdl.c
@@ -1,10 +1,10 @@
 /***************************************************************************
- *             __________               __   ___.                  
- *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___  
- *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /  
- *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <   
- *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \  
- *                     \/            \/     \/    \/            \/ 
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
  * $Id$
  *
  * Copyright (C) 2005 by Nick Lanham
@@ -85,8 +85,43 @@ void pcm_play_unlock(void)
         SDL_UnlockMutex(audio_lock);
 }
 
+static void sdl_audio_callback(struct pcm_udata *udata, Uint8 *stream, int len);
 static void pcm_dma_apply_settings_nolock(void)
 {
+    SDL_AudioSpec wanted_spec;
+    wanted_spec.freq = pcm_sampr;
+    wanted_spec.format = AUDIO_S16SYS;
+    wanted_spec.channels = 2;
+    wanted_spec.samples = 2048;
+    wanted_spec.callback =
+        (void (SDLCALL *)(void *userdata,
+            Uint8 *stream, int len))sdl_audio_callback;
+    wanted_spec.userdata = &udata;
+    SDL_CloseAudio();
+    /* Open the audio device and start playing sound! */
+    if(SDL_OpenAudio(&wanted_spec, &obtained) < 0) {
+        DEBUGF("Unable to open audio: %s\n", SDL_GetError());
+        return;
+    }
+    switch (obtained.format)
+    {
+    case AUDIO_U8:
+    case AUDIO_S8:
+        pcm_channel_bytes = 1;
+        break;
+    case AUDIO_U16LSB:
+    case AUDIO_S16LSB:
+    case AUDIO_U16MSB:
+    case AUDIO_S16MSB:
+        pcm_channel_bytes = 2;
+        break;
+    default:
+        DEBUGF("Unknown sample format obtained: %u\n",
+                (unsigned)obtained.format);
+        return;
+    }
+    pcm_sample_bytes = obtained.channels * pcm_channel_bytes;
+
     cvt_status = SDL_BuildAudioCVT(&cvt, AUDIO_S16SYS, 2, pcm_sampr,
                     obtained.format, obtained.channels, obtained.freq);
 
@@ -104,7 +139,6 @@ void pcm_dma_apply_settings(void)
 
 void pcm_play_dma_start(const void *addr, size_t size)
 {
-    pcm_dma_apply_settings_nolock();
 
     pcm_data = addr;
     pcm_data_size = size;
@@ -257,9 +291,7 @@ static void sdl_audio_callback(struct pcm_udata *udata, Uint8 *stream, int len)
 
                 if (delay > 0)
                 {
-                    SDL_UnlockMutex(audio_lock);
                     SDL_Delay(delay);
-                    SDL_LockMutex(audio_lock);
 
                     if (!pcm_is_playing())
                         break;
@@ -340,7 +372,6 @@ void pcm_play_dma_init(void)
         return;
     }
 
-    SDL_AudioSpec wanted_spec;
 #ifdef DEBUG
     udata.debug = NULL;
     if (debug_audio) {
@@ -348,43 +379,6 @@ void pcm_play_dma_init(void)
         DEBUGF("Audio debug file open\n");
     }
 #endif
-    /* Set 16-bit stereo audio at 44Khz */
-    wanted_spec.freq = 44100;
-    wanted_spec.format = AUDIO_S16SYS;
-    wanted_spec.channels = 2;
-    wanted_spec.samples = 2048;
-    wanted_spec.callback =
-        (void (SDLCALL *)(void *userdata,
-            Uint8 *stream, int len))sdl_audio_callback;
-    wanted_spec.userdata = &udata;
-
-    /* Open the audio device and start playing sound! */
-    if(SDL_OpenAudio(&wanted_spec, &obtained) < 0) {
-        DEBUGF("Unable to open audio: %s\n", SDL_GetError());
-        return;
-    }
-
-    switch (obtained.format)
-    {
-    case AUDIO_U8:
-    case AUDIO_S8:
-        pcm_channel_bytes = 1;
-        break;
-    case AUDIO_U16LSB:
-    case AUDIO_S16LSB:
-    case AUDIO_U16MSB:
-    case AUDIO_S16MSB:
-        pcm_channel_bytes = 2;
-        break;
-    default:
-        DEBUGF("Unknown sample format obtained: %u\n",
-                (unsigned)obtained.format);
-        return;
-    }
-
-    pcm_sample_bytes = obtained.channels * pcm_channel_bytes;
-
-    pcm_dma_apply_settings_nolock();
 }
 
 void pcm_play_dma_postinit(void)
diff --git a/firmware/target/hosted/sdl/system-sdl.c b/firmware/target/hosted/sdl/system-sdl.c
index e53643f449..56cff3f1be 100644
--- a/firmware/target/hosted/sdl/system-sdl.c
+++ b/firmware/target/hosted/sdl/system-sdl.c
@@ -1,10 +1,10 @@
 /***************************************************************************
- *             __________               __   ___.                  
- *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___  
- *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /  
- *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <   
- *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \  
- *                     \/            \/     \/    \/            \/ 
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
  * $Id$
  *
  * Copyright (C) 2006 by Daniel Everton <dan@iocaine.org>
@@ -50,6 +50,8 @@
 #define SIMULATOR_DEFAULT_ROOT "simdisk"
 
 SDL_Surface    *gui_surface;
+SDL_Window     *window;
+SDL_Renderer   *sdlRenderer;
 
 bool            background = true;          /* use backgrounds by default */
 #ifdef HAVE_REMOTE_LCD
@@ -72,26 +74,12 @@ bool debug_audio = false;
 bool debug_wps = false;
 int wps_verbose_level = 3;
 
-/*
- * This thread will read the buttons in an interrupt like fashion, and
- * also initializes SDL_INIT_VIDEO and the surfaces
- *
- * it must be done in the same thread (at least on windows) because events only
- * work in the thread which called SDL_Init(SubSystem) with SDL_INIT_VIDEO
- *
- * This is an SDL thread and relies on preemptive behavoir of the host
- **/
-static int sdl_event_thread(void * param)
+static void sdl_window_setup(void)
 {
-    SDL_InitSubSystem(SDL_INIT_VIDEO);
-
-#if (CONFIG_PLATFORM & PLATFORM_MAEMO)
-    SDL_sem *wait_for_maemo_startup;
-#endif
     SDL_Surface *picture_surface = NULL;
     int width, height;
     int depth;
-    Uint32 flags;
+    Uint32 flags = 0;
 
     /* Try and load the background image. If it fails go without */
     if (background) {
@@ -101,14 +89,14 @@ static int sdl_event_thread(void * param)
             DEBUGF("warn: %s\n", SDL_GetError());
         }
     }
-    
+
     /* Set things up */
     if (background)
     {
         width = UI_WIDTH;
         height = UI_HEIGHT;
-    } 
-    else 
+    }
+    else
     {
 #ifdef HAVE_REMOTE_LCD
         if (showremote)
@@ -128,17 +116,47 @@ static int sdl_event_thread(void * param)
     if (depth < 8)
         depth = 16;
 
-    flags = SDL_HWSURFACE|SDL_DOUBLEBUF;
 #if (CONFIG_PLATFORM & (PLATFORM_MAEMO|PLATFORM_PANDORA))
     /* Fullscreen mode for maemo app */
-    flags |= SDL_FULLSCREEN;
+    flags |= SDL_WINDOW_FULLSCREEN;
 #endif
 
-    SDL_WM_SetCaption(UI_TITLE, NULL);
-
-    if ((gui_surface = SDL_SetVideoMode(width * display_zoom, height * display_zoom, depth, flags)) == NULL) {
+    if ((window = SDL_CreateWindow(UI_TITLE, SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED,
+                      width * display_zoom, height * display_zoom , flags)) == NULL)
+        panicf("%s", SDL_GetError());
+    if ((sdlRenderer = SDL_CreateRenderer(window, -1, SDL_RENDERER_PRESENTVSYNC)) == NULL)
         panicf("%s", SDL_GetError());
+    if ((gui_surface = SDL_CreateRGBSurface(0, width * display_zoom, height * display_zoom, depth,
+                                       0, 0, 0, 0)) == NULL)
+        panicf("%s", SDL_GetError());
+
+    /* If we have a background, blit it over to the display surface */
+    if (background && picture_surface)
+    {
+        SDL_BlitSurface(picture_surface, NULL, gui_surface, NULL);
+        SDL_FreeSurface(picture_surface);
     }
+}
+
+/*
+ * This thread will read the buttons in an interrupt like fashion, and
+ * also initializes SDL_INIT_VIDEO and the surfaces
+ *
+ * it must be done in the same thread (at least on windows) because events only
+ * work in the thread that called SDL_InitSubSystem(SDL_INIT_VIDEO)
+ *
+ * This is an SDL thread and relies on preemptive behavoir of the host
+ **/
+static int sdl_event_thread(void * param)
+{
+#ifdef __WIN32 /* Fails on Linux and MacOS */
+    SDL_InitSubSystem(SDL_INIT_VIDEO);
+    sdl_window_setup();
+#endif
+
+#if (CONFIG_PLATFORM & PLATFORM_MAEMO)
+    SDL_sem *wait_for_maemo_startup;
+#endif
 
 #if (CONFIG_PLATFORM & (PLATFORM_MAEMO|PLATFORM_PANDORA))
     /* SDL touch screen fix: Work around a SDL assumption that returns
@@ -152,14 +170,10 @@ static int sdl_event_thread(void * param)
     SDL_SetCursor(hiddenCursor);
 #endif
 
-    if (background && picture_surface != NULL)
-        SDL_BlitSurface(picture_surface, NULL, gui_surface, NULL);
-
 #if (CONFIG_PLATFORM & PLATFORM_MAEMO)
     /* start maemo thread: Listen to display on/off events and battery monitoring */
     wait_for_maemo_startup = SDL_CreateSemaphore(0); /* 0-count so it blocks */
-    SDL_Thread *maemo_thread = SDL_CreateThread(maemo_thread_func, wait_for_maemo_startup);
-
+    SDL_Thread *maemo_thread = SDL_CreateThread(maemo_thread_func, NULL, wait_for_maemo_startup);
     SDL_SemWait(wait_for_maemo_startup);
     SDL_DestroySemaphore(wait_for_maemo_startup);
 #endif
@@ -184,9 +198,6 @@ static int sdl_event_thread(void * param)
     SDL_FreeCursor(hiddenCursor);
 #endif
 
-    if(picture_surface)
-        SDL_FreeSurface(picture_surface);
-
     /* Order here is relevent to prevent deadlocks and use of destroyed
        sync primitives by kernel threads */
 #ifdef HAVE_SDL_THREADS
@@ -251,15 +262,16 @@ void system_init(void)
     g_type_init();
 #endif
 
-    if (SDL_Init(SDL_INIT_TIMER))
+    if (SDL_InitSubSystem(SDL_INIT_TIMER))
         panicf("%s", SDL_GetError());
 
-    s = SDL_CreateSemaphore(0); /* 0-count so it blocks */
-
-    evt_thread = SDL_CreateThread(sdl_event_thread, s);
+#ifndef __WIN32  /* Fails on Windows */
+    SDL_InitSubSystem(SDL_INIT_VIDEO);
+    sdl_window_setup();
+#endif
 
-    /* wait for sdl_event_thread to run so that it can initialize the surfaces
-     * and video subsystem needed for SDL events */
+    s = SDL_CreateSemaphore(0); /* 0-count so it blocks */
+    evt_thread = SDL_CreateThread(sdl_event_thread, NULL, s);
     SDL_SemWait(s);
     /* cleanup */
     SDL_DestroySemaphore(s);
@@ -303,29 +315,29 @@ int hostfs_flush(void)
 
 void sys_handle_argv(int argc, char *argv[])
 {
-    if (argc >= 1) 
+    if (argc >= 1)
     {
         int x;
-        for (x = 1; x < argc; x++) 
+        for (x = 1; x < argc; x++)
         {
 #ifdef DEBUG
-            if (!strcmp("--debugaudio", argv[x])) 
+            if (!strcmp("--debugaudio", argv[x]))
             {
                 debug_audio = true;
                 printf("Writing debug audio file.\n");
             }
-            else 
+            else
 #endif
                 if (!strcmp("--debugwps", argv[x]))
             {
                 debug_wps = true;
                 printf("WPS debug mode enabled.\n");
-            } 
+            }
             else if (!strcmp("--nobackground", argv[x]))
             {
                 background = false;
                 printf("Disabling background image.\n");
-            } 
+            }
 #ifdef HAVE_REMOTE_LCD
             else if (!strcmp("--noremote", argv[x]))
             {
@@ -346,7 +358,7 @@ void sys_handle_argv(int argc, char *argv[])
                     display_zoom=atof(argv[x]);
                 else
                     display_zoom = 2;
-                printf("Window zoom is %d\n", display_zoom);
+                printf("Window zoom is %f\n", display_zoom);
             }
             else if (!strcmp("--alarm", argv[x]))
             {
@@ -372,7 +384,7 @@ void sys_handle_argv(int argc, char *argv[])
                     debug_buttons = true;
                     printf("Printing background button clicks.\n");
             }
-            else 
+            else
             {
                 printf("rockboxui\n");
                 printf("Arguments:\n");
diff --git a/firmware/target/hosted/sdl/thread-sdl.c b/firmware/target/hosted/sdl/thread-sdl.c
index 17a0f847b5..abd6339cee 100644
--- a/firmware/target/hosted/sdl/thread-sdl.c
+++ b/firmware/target/hosted/sdl/thread-sdl.c
@@ -340,7 +340,7 @@ unsigned int create_thread(void (*function)(void),
         return 0;
     }
 
-    SDL_Thread *t = SDL_CreateThread(runthread, thread);
+    SDL_Thread *t = SDL_CreateThread(runthread, NULL, thread);
     if (t == NULL)
     {
         DEBUGF("Failed to create SDL thread\n");
@@ -447,7 +447,7 @@ void init_threads(void)
     thread->context.s = SDL_CreateSemaphore(0);
     thread->context.t = NULL; /* NULL for the implicit main thread */
     __running_self_entry() = thread;
- 
+
     if (thread->context.s == NULL)
     {
         fprintf(stderr, "Failed to create main semaphore\n");
diff --git a/firmware/target/mips/ingenic_jz47xx/xduoo_x3/power-xduoo_x3.c b/firmware/target/mips/ingenic_jz47xx/xduoo_x3/power-xduoo_x3.c
index 9ae602ba56..c9bcb47fcd 100644
--- a/firmware/target/mips/ingenic_jz47xx/xduoo_x3/power-xduoo_x3.c
+++ b/firmware/target/mips/ingenic_jz47xx/xduoo_x3/power-xduoo_x3.c
@@ -23,15 +23,19 @@
 #include "power.h"
 #include "cpu.h"
 
-#define CHARGE_STAT_GPIO (32*1+6) /* STAT port */
+#define CHARGE_STAT_GPIO (32*1+6)  /* STAT port */
+#define PIN_USB_DET      (32*4+19) /* USB connected */
 
 /* Detect which power sources are present. */
 unsigned int power_input_status(void)
 {
+    int rval = POWER_INPUT_NONE;
+    if(!__gpio_get_pin(PIN_USB_DET))
+        rval |= POWER_INPUT_USB;
     if(!__gpio_get_pin(CHARGE_STAT_GPIO))
-        return POWER_INPUT_USB_CHARGER;
+        rval |= POWER_INPUT_USB_CHARGER;
 
-    return POWER_INPUT_NONE;
+    return rval;
 }
 
 void power_init(void)
@@ -42,5 +46,5 @@ void power_init(void)
 
 bool charging_state(void)
 {
-    return (power_input_status() == POWER_INPUT_USB_CHARGER);
+    return (power_input_status() & POWER_INPUT_USB_CHARGER);
 }
diff --git a/firmware/target/mips/ingenic_x1000/crt0.S b/firmware/target/mips/ingenic_x1000/crt0.S
index 6c0942b0db..23daaefb5e 100644
--- a/firmware/target/mips/ingenic_x1000/crt0.S
+++ b/firmware/target/mips/ingenic_x1000/crt0.S
@@ -23,6 +23,10 @@
 #include "mips.h"
 #include "bootdata.h"
 
+#if defined(HAVE_DEVICEDATA) && !defined(BOOTLOADER)
+#include "devicedata.h"
+#endif
+
     .text
     .extern main
     .extern system_early_init
@@ -52,6 +56,9 @@ _header:
 #ifndef BOOTLOADER
     /* Multiboot support header; this is not part of the above header. */
     put_boot_data_here
+#ifdef HAVE_DEVICEDATA
+    put_device_data_here
+#endif
 #endif
 
 _realstart:
diff --git a/firmware/target/mips/ingenic_x1000/erosqnative/lcd-erosqnative.c b/firmware/target/mips/ingenic_x1000/erosqnative/lcd-erosqnative.c
index 0d43a3f010..bcc30a71bd 100644
--- a/firmware/target/mips/ingenic_x1000/erosqnative/lcd-erosqnative.c
+++ b/firmware/target/mips/ingenic_x1000/erosqnative/lcd-erosqnative.c
@@ -25,11 +25,138 @@
 #include "lcd-x1000.h"
 #include "gpio-x1000.h"
 #include "system.h"
+#include "devicedata.h"
 
 /* for reference on these command/data hex values, see the mipi dcs lcd spec.  *
  * Not everything here is there, but all the standard stuff is.                */
 
-static const uint32_t erosqnative_lcd_cmd_enable[] = {
+/* New Display Eroq 2.1 / Hifiwalker 1.7+ / Surfans v3.2, unknown Controller  *
+ * (partially GC9A01 register compatible)                                     *
+ *   https://espruino.microcosm.app/api/v1/files/ \                           *
+ *   9dc1b976d621a2ab3854312cce862c4a9a50dc1b.html#GC9A01 ,                   *
+ *   https://www.buydisplay.com/download/ic/GC9A01A.pdf ,                     *
+ *   https://lcddisplay.co/wp-content/uploads/2023/02/GC9A01.pdf              *
+ * Init sequence From 'EROS Q （c口）_V2.1_20231209固件.zip'                  *
+ * update.upt/.iso -> In 'uboot.bin' at 0x52da0-0x5305f                       *
+ *  http://www.eroshifi.com/download/firmware/122.html                        */
+static const uint32_t erosqnative_lcd_cmd_enable_v3[] = {
+
+    /* Unlock EXTC? */
+    LCD_INSTR_CMD,      0xfe, // Inter Register Enable1
+    LCD_INSTR_CMD,      0xef, // Inter Register Enable2
+
+    LCD_INSTR_CMD,      0x36, // Memory Access Control
+/* Bit7 1:vertical flip   0:no vertical flip
+   Bit6 1:horizontal flip 0:no horizontal flip
+   Bit3 1:BGR 0:RGB */
+    LCD_INSTR_DAT,      0x90,
+    /* Pixel Format Set */
+    LCD_INSTR_CMD,      0x3a,
+    LCD_INSTR_DAT,      0x55, /* Rockbox uses 16pp, OF specified 18 bpp */
+
+    LCD_INSTR_CMD,      0x84, // ?? (undocumented)
+    LCD_INSTR_DAT,      0x04,
+    LCD_INSTR_CMD,      0x86, // ??
+    LCD_INSTR_DAT,      0xfb,
+    LCD_INSTR_CMD,      0x87, // ??
+    LCD_INSTR_DAT,      0x79,
+    LCD_INSTR_CMD,      0x89, // ??
+    LCD_INSTR_DAT,      0x0b,
+    LCD_INSTR_CMD,      0x8a, // ??
+    LCD_INSTR_DAT,      0x20,
+    LCD_INSTR_CMD,      0x8b, // ??
+    LCD_INSTR_DAT,      0x80,
+    LCD_INSTR_CMD,      0x8d, // ??
+    LCD_INSTR_DAT,      0x3b,
+    LCD_INSTR_CMD,      0x8e, // ??
+    LCD_INSTR_DAT,      0xcf,
+
+    LCD_INSTR_CMD,      0xec, // Charge Pump Frequent Control
+    LCD_INSTR_DAT,      0x33,
+    LCD_INSTR_DAT,      0x02,
+    LCD_INSTR_DAT,      0x4c,
+
+    LCD_INSTR_CMD,      0x98, // ?? (undocumented)
+    LCD_INSTR_DAT,      0x3e,
+    LCD_INSTR_CMD,      0x9c, // ??
+    LCD_INSTR_DAT,      0x4b,
+    LCD_INSTR_CMD,      0x99, // ??
+    LCD_INSTR_DAT,      0x3e,
+    LCD_INSTR_CMD,      0x9d, // ??
+    LCD_INSTR_DAT,      0x4b,
+    LCD_INSTR_CMD,      0x9b, // ??
+    LCD_INSTR_DAT,      0x55,
+
+    LCD_INSTR_CMD,      0xe8, // Frame Rate
+    LCD_INSTR_DAT,      0x11,
+    LCD_INSTR_DAT,      0x00,
+
+    LCD_INSTR_CMD,      0xff, // ?? (Adafruit & Co lib.  C:0xFF, D:0x60, D:0x01, D:0x04)
+    LCD_INSTR_DAT,      0x62, // LCD_INSTR_DAT, 0x01, LCD_INSTR_DAT, 0x04,
+    LCD_INSTR_CMD,      0xc3, // Vreg1a voltage Control
+    LCD_INSTR_DAT,      0x20,
+    LCD_INSTR_CMD,      0xc4, // Vreg1b voltage Control
+    LCD_INSTR_DAT,      0x03,
+    LCD_INSTR_CMD,      0xc9, // Vreg2a voltage Control
+    LCD_INSTR_DAT,      0x2a,
+
+    LCD_INSTR_CMD,      0xf0, // SET_GAMMA1
+    LCD_INSTR_DAT,      0x4a,
+    LCD_INSTR_DAT,      0x10,
+    LCD_INSTR_DAT,      0x0a,
+    LCD_INSTR_DAT,      0x0a,
+    LCD_INSTR_DAT,      0x26,
+    LCD_INSTR_DAT,      0x39,
+
+    LCD_INSTR_CMD,      0xf2, // SET_GAMMA3
+    LCD_INSTR_DAT,      0x4a,
+    LCD_INSTR_DAT,      0x10,
+    LCD_INSTR_DAT,      0x0a,
+    LCD_INSTR_DAT,      0x0a,
+    LCD_INSTR_DAT,      0x26,
+    LCD_INSTR_DAT,      0x39,
+
+    LCD_INSTR_CMD,      0xf1, // SET_GAMMA2
+    LCD_INSTR_DAT,      0x50,
+    LCD_INSTR_DAT,      0x8f,
+    LCD_INSTR_DAT,      0xaf,
+    LCD_INSTR_DAT,      0x3b,
+    LCD_INSTR_DAT,      0x3f,
+    LCD_INSTR_DAT,      0x7f,
+
+    LCD_INSTR_CMD,      0xf3, // SET_GAMMA4
+    LCD_INSTR_DAT,      0x50,
+    LCD_INSTR_DAT,      0x8f,
+    LCD_INSTR_DAT,      0xaf,
+    LCD_INSTR_DAT,      0x3b,
+    LCD_INSTR_DAT,      0x3f,
+    LCD_INSTR_DAT,      0x7f,
+
+    LCD_INSTR_CMD,      0xba, // TE Control
+    LCD_INSTR_DAT,      0x0a,
+
+#ifdef BOOTLOADER
+    LCD_INSTR_CMD,      0x35, // Tearing Effect Line ON
+    LCD_INSTR_DAT,      0x00,
+#endif
+
+    LCD_INSTR_CMD,      0x21, /* Invert */
+
+    /* Lock EXTC? */
+    LCD_INSTR_CMD,      0xfe, // Inter Register Enable1
+    LCD_INSTR_CMD,      0xee,
+
+    /* Exit Sleep */
+    LCD_INSTR_CMD,      0x11,
+    LCD_INSTR_UDELAY,   120000,
+    /* Display On */
+    LCD_INSTR_CMD,      0x29,
+    LCD_INSTR_UDELAY,   20000,
+    LCD_INSTR_END,
+};
+
+/* Original Display / Hifiwalker -1.5 / Surfans -2.7 */
+static const uint32_t erosqnative_lcd_cmd_enable_v1[] = {
     /* Set EXTC? */
     LCD_INSTR_CMD,      0xc8,
     LCD_INSTR_DAT,      0xff,
@@ -179,7 +306,22 @@ void lcd_tgt_enable(bool enable)
         mdelay(5);
         gpio_set_level(GPIO_LCD_CE, 0);
 
-        lcd_exec_commands(&erosqnative_lcd_cmd_enable[0]);
+#ifdef BOOTLOADER
+# if EROSQN_VER == 3
+        lcd_exec_commands(&erosqnative_lcd_cmd_enable_v3[0]);
+# else
+        lcd_exec_commands(&erosqnative_lcd_cmd_enable_v1[0]);
+# endif
+#else
+        if (device_data.lcd_version == 3)
+        {
+            lcd_exec_commands(&erosqnative_lcd_cmd_enable_v3[0]);
+        }
+        else
+        {
+            lcd_exec_commands(&erosqnative_lcd_cmd_enable_v1[0]);
+        }
+#endif
     } else {
         /* doesn't flash white if we don't do anything... */
 #if 0
diff --git a/firmware/target/mips/ingenic_x1000/system-x1000.c b/firmware/target/mips/ingenic_x1000/system-x1000.c
index 64890a6c3a..1c850736b6 100644
--- a/firmware/target/mips/ingenic_x1000/system-x1000.c
+++ b/firmware/target/mips/ingenic_x1000/system-x1000.c
@@ -20,6 +20,7 @@
  ****************************************************************************/
 
 #include "system.h"
+#include <string.h>
 #include "mips.h"
 #include "panic.h"
 #include "button.h"
@@ -36,6 +37,10 @@
 #include "x1000/msc.h"
 #include "x1000/aic.h"
 
+#if defined(HAVE_DEVICEDATA)
+#include "devicedata.h"
+#endif
+
 #ifdef X1000_CPUIDLE_STATS
 int __cpu_idle_avg = 0;
 int __cpu_idle_cur = 0;
@@ -81,6 +86,20 @@ void system_early_init(void)
     clk_init();
 }
 
+#if defined (HAVE_DEVICEDATA) && defined(EROS_QN)
+void fill_devicedata(struct device_data_t *data)
+{
+#ifdef BOOTLOADER
+    memset(data->payload, 0xff, data->length);
+    data->lcd_version = EROSQN_VER;
+#else
+    uint8_t lcd_version = device_data.lcd_version;
+    memset(data->payload, 0xff, data->length);
+    data->lcd_version = lcd_version;
+#endif
+}
+#endif
+
 /* First thing called from Rockbox main() */
 void system_init(void)
 {
diff --git a/firmware/target/mips/ingenic_x1000/x1000boot.make b/firmware/target/mips/ingenic_x1000/x1000boot.make
index 0bdf5cf7b4..7a861b0a3d 100644
--- a/firmware/target/mips/ingenic_x1000/x1000boot.make
+++ b/firmware/target/mips/ingenic_x1000/x1000boot.make
@@ -12,7 +12,7 @@ include $(ROOTDIR)/lib/microtar/microtar.make
 INCLUDES += -I$(APPSDIR)
 SRC += $(call preprocess, $(APPSDIR)/SOURCES)
 
-LDSDEP := $(FIRMDIR)/export/cpu.h $(FIRMDIR)/export/config/$(MODELNAME).h
+LDSDEP := $(FIRMDIR)/export/cpu.h $(FIRMDIR)/export/config.h
 
 BOOTLDS := $(FIRMDIR)/target/$(CPU)/$(MANUFACTURER)/boot.lds
 BOOTLINK := $(BUILDDIR)/boot.link
diff --git a/firmware/usbstack/usb_core.c b/firmware/usbstack/usb_core.c
index b4a117d959..92cdeb9a59 100644
--- a/firmware/usbstack/usb_core.c
+++ b/firmware/usbstack/usb_core.c
@@ -119,16 +119,13 @@ static const struct usb_qualifier_descriptor __attribute__((aligned(2)))
     .bNumConfigurations = 1
 };
 
-static const struct usb_string_descriptor __attribute__((aligned(2)))
-                                    usb_string_iManufacturer =
+static const struct usb_string_descriptor usb_string_iManufacturer =
 USB_STRING_INITIALIZER(u"Rockbox.org");
 
-static const struct usb_string_descriptor __attribute__((aligned(2)))
-                                    usb_string_iProduct =
+static const struct usb_string_descriptor usb_string_iProduct =
 USB_STRING_INITIALIZER(u"Rockbox media player");
 
-static struct usb_string_descriptor __attribute__((aligned(2)))
-                                    usb_string_iSerial =
+static struct usb_string_descriptor usb_string_iSerial =
 USB_STRING_INITIALIZER(u"00000000000000000000000000000000000000000");
 
 /* Generic for all targets */
@@ -523,11 +520,11 @@ void usb_core_hotswap_event(int volume, bool inserted)
 static void usb_core_set_serial_function_id(void)
 {
     int i, id = 0;
-    
+
     for(i = 0; i < USB_NUM_DRIVERS; i++)
         if(drivers[i].enabled)
             id |= 1 << i;
-    
+
     usb_string_iSerial.wString[0] = hex[id];
 }
 
@@ -856,10 +853,10 @@ static void request_handler_endpoint_drivers(struct usb_ctrlrequest* req, void*
     if(EP_NUM(req->wIndex) < USB_NUM_ENDPOINTS)
         control_handler =
             ep_data[EP_NUM(req->wIndex)].control_handler[EP_DIR(req->wIndex)];
-    
+
     if(control_handler)
         handled = control_handler(req, reqdata, response_data);
-    
+
     if(!handled) {
         /* nope. flag error */
         logf("usb bad req %d", req->bRequest);
diff --git a/firmware/usbstack/usb_storage.c b/firmware/usbstack/usb_storage.c
index e226c15f99..617988b19f 100644
--- a/firmware/usbstack/usb_storage.c
+++ b/firmware/usbstack/usb_storage.c
@@ -106,8 +106,9 @@
 #define SCSI_MODE_SENSE_10        0x5a
 #define SCSI_REQUEST_SENSE        0x03
 #define SCSI_ALLOW_MEDIUM_REMOVAL 0x1e
-#define SCSI_READ_CAPACITY        0x25
-#define SCSI_READ_FORMAT_CAPACITY 0x23
+#define SCSI_READ_CAPACITY_10     0x25
+#define SCSI_READ_CAPACITY_16     0x9e
+#define USB_UFI_READ_FORMAT_CAPACITY 0x23
 #define SCSI_READ_10              0x28
 #define SCSI_WRITE_10             0x2a
 #define SCSI_START_STOP_UNIT      0x1b
@@ -248,9 +249,16 @@ struct command_status_wrapper {
     unsigned char status;
 } __attribute__ ((packed));
 
-struct capacity {
-    unsigned int block_count;
-    unsigned int block_size;
+struct capacity_10 {
+    unsigned char block_count[4];
+    unsigned char block_size[4];
+} __attribute__ ((packed));
+
+struct capacity_16 {
+    unsigned char block_count[8];
+    unsigned char block_size[4];
+    unsigned char flags[2];
+    unsigned char lowest_aligned_lba[2];
 } __attribute__ ((packed));
 
 struct format_capacity {
@@ -263,7 +271,8 @@ struct format_capacity {
 static union {
     unsigned char* transfer_buffer;
     struct inquiry_data* inquiry;
-    struct capacity* capacity_data;
+    struct capacity_10* capacity_data_10;
+    struct capacity_16* capacity_data_16;
     struct format_capacity* format_capacity_data;
     struct sense_data *sense_data;
     struct mode_sense_data_6 *ms_data_6;
@@ -278,8 +287,8 @@ static char *cbw_buffer;
 static struct {
     sector_t sector;
     unsigned int count;
-    unsigned int orig_count;
     unsigned int cur_cmd;
+    unsigned int block_size;
     unsigned int tag;
     unsigned int lun;
     unsigned char *data[2];
@@ -501,6 +510,7 @@ void usb_storage_transfer_complete(int ep,int dir,int status,int length)
 #endif
 
     logf("transfer result for ep %d/%d %X %d", ep,dir,status, length);
+
     switch(state) {
         case RECEIVING_BLOCKS:
             if(dir==USB_DIR_IN) {
@@ -508,37 +518,37 @@ void usb_storage_transfer_complete(int ep,int dir,int status,int length)
             }
             logf("scsi write %llu %d", cur_cmd.sector, cur_cmd.count);
             if(status==0) {
-                if((unsigned int)length!=(SECTOR_SIZE* cur_cmd.count)
+                if((unsigned int)length!=(cur_cmd.block_size* cur_cmd.count)
                   && (unsigned int)length!=WRITE_BUFFER_SIZE) {
                     logf("unexpected length :%d",length);
                     break;
                 }
 
                 sector_t next_sector = cur_cmd.sector +
-                             (WRITE_BUFFER_SIZE/SECTOR_SIZE);
+                             (WRITE_BUFFER_SIZE/cur_cmd.block_size);
                 unsigned int next_count = cur_cmd.count -
-                             MIN(cur_cmd.count,WRITE_BUFFER_SIZE/SECTOR_SIZE);
+                             MIN(cur_cmd.count,WRITE_BUFFER_SIZE/cur_cmd.block_size);
                 int next_select = !cur_cmd.data_select;
 
                 if(next_count!=0) {
                     /* Ask the host to send more, to the other buffer */
                     receive_block_data(cur_cmd.data[next_select],
-                                       MIN(WRITE_BUFFER_SIZE,next_count*SECTOR_SIZE));
+                                       MIN(WRITE_BUFFER_SIZE,next_count*cur_cmd.block_size));
                 }
 
                 /* Now write the data that just came in, while the host is
                    sending the next bit */
 #ifdef USB_USE_RAMDISK
-                memcpy(ramdisk_buffer + cur_cmd.sector*SECTOR_SIZE,
+                memcpy(ramdisk_buffer + cur_cmd.sector*cur_cmd.block_size,
                         cur_cmd.data[cur_cmd.data_select],
-                        MIN(WRITE_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count)*SECTOR_SIZE);
+                        MIN(WRITE_BUFFER_SIZE/cur_cmd.block_size, cur_cmd.count)*cur_cmd.block_size);
 #else
                 int result = USBSTOR_WRITE_SECTORS_FILTER();
 
                 if (result == 0) {
                     result = storage_write_sectors(IF_MD(cur_cmd.lun,)
                         cur_cmd.sector,
-                        MIN(WRITE_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count),
+                        MIN(WRITE_BUFFER_SIZE/cur_cmd.block_size, cur_cmd.count),
                         cur_cmd.data[cur_cmd.data_select]);
                 }
 
@@ -712,25 +722,25 @@ static void send_and_read_next(void)
         cur_cmd.last_result = result;
 
     send_block_data(cur_cmd.data[cur_cmd.data_select],
-                    MIN(READ_BUFFER_SIZE,cur_cmd.count*SECTOR_SIZE));
+                    MIN(READ_BUFFER_SIZE,cur_cmd.count*cur_cmd.block_size));
 
     /* Switch buffers for the next one */
     cur_cmd.data_select=!cur_cmd.data_select;
 
-    cur_cmd.sector+=(READ_BUFFER_SIZE/SECTOR_SIZE);
-    cur_cmd.count-=MIN(cur_cmd.count,READ_BUFFER_SIZE/SECTOR_SIZE);
+    cur_cmd.sector+=(READ_BUFFER_SIZE/cur_cmd.block_size);
+    cur_cmd.count-=MIN(cur_cmd.count,READ_BUFFER_SIZE/cur_cmd.block_size);
 
     if(cur_cmd.count!=0) {
         /* already read the next bit, so we can send it out immediately when the
          * current transfer completes.  */
 #ifdef USB_USE_RAMDISK
         memcpy(cur_cmd.data[cur_cmd.data_select],
-                ramdisk_buffer + cur_cmd.sector*SECTOR_SIZE,
-                MIN(READ_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count)*SECTOR_SIZE);
+                ramdisk_buffer + cur_cmd.sector*cur_cmd.block_size,
+                MIN(READ_BUFFER_SIZE/cur_cmd.block_size, cur_cmd.count)*cur_cmd.block_size);
 #else
         result = storage_read_sectors(IF_MD(cur_cmd.lun,)
                 cur_cmd.sector,
-                MIN(READ_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count),
+                MIN(READ_BUFFER_SIZE/cur_cmd.block_size, cur_cmd.count),
                 cur_cmd.data[cur_cmd.data_select]);
         if(cur_cmd.last_result == 0)
             cur_cmd.last_result = result;
@@ -748,7 +758,6 @@ static void handle_scsi(struct command_block_wrapper* cbw)
     unsigned int block_size;
     bool lun_present=true;
     unsigned char lun = cbw->lun;
-    unsigned int block_size_mult = 1;
 
     if(letoh32(cbw->signature) != CBW_SIGNATURE) {
         logf("ums: bad cbw signature (%x)", cbw->signature);
@@ -783,10 +792,14 @@ static void handle_scsi(struct command_block_wrapper* cbw)
     if(ejected[lun])
         lun_present = false;
 
+    unsigned int block_size_mult = 1; /* Number of LOGICAL storage device blocks in each USB block */
 #ifdef MAX_LOG_SECTOR_SIZE
     block_size_mult = disk_get_sector_multiplier(IF_MD(lun));
 #endif
 
+    uint32_t bsize = block_size*block_size_mult;
+    sector_t bcount = block_count/block_size_mult;
+
     cur_cmd.tag = cbw->tag;
     cur_cmd.lun = lun;
     cur_cmd.cur_cmd = cbw->command_block[0];
@@ -887,31 +900,33 @@ static void handle_scsi(struct command_block_wrapper* cbw)
                         htobe16(sizeof(struct mode_sense_bdesc_longlba));
 
                     memset(tb.ms_data_10->block_descriptor.reserved,0,4);
-                    memset(tb.ms_data_10->block_descriptor.num_blocks,0,8);
 
 #ifdef STORAGE_64BIT_SECTOR
                     tb.ms_data_10->block_descriptor.num_blocks[2] =
-                        ((block_count/block_size_mult) & 0xff00000000ULL)>>40;
+                        (bcount & 0xff00000000ULL)>>40;
                     tb.ms_data_10->block_descriptor.num_blocks[3] =
-                        ((block_count/block_size_mult) & 0x00ff000000ULL)>>32;
+                        (bcount & 0x00ff000000ULL)>>32;
+#else
+                    tb.ms_data_10->block_descriptor.num_blocks[2] = 0;
+                    tb.ms_data_10->block_descriptor.num_blocks[3] = 0;
 #endif
                     tb.ms_data_10->block_descriptor.num_blocks[4] =
-                        ((block_count/block_size_mult) & 0xff000000)>>24;
+                        (bcount & 0xff000000)>>24;
                     tb.ms_data_10->block_descriptor.num_blocks[5] =
-                        ((block_count/block_size_mult) & 0x00ff0000)>>16;
+                        (bcount & 0x00ff0000)>>16;
                     tb.ms_data_10->block_descriptor.num_blocks[6] =
-                        ((block_count/block_size_mult) & 0x0000ff00)>>8;
+                        (bcount & 0x0000ff00)>>8;
                     tb.ms_data_10->block_descriptor.num_blocks[7] =
-                        ((block_count/block_size_mult) & 0x000000ff);
+                        (bcount & 0x000000ff);
 
                     tb.ms_data_10->block_descriptor.block_size[0] =
-                        ((block_size*block_size_mult) & 0xff000000)>>24;
+                        (bsize & 0xff000000)>>24;
                     tb.ms_data_10->block_descriptor.block_size[1] =
-                        ((block_size*block_size_mult) & 0x00ff0000)>>16;
+                        (bsize & 0x00ff0000)>>16;
                     tb.ms_data_10->block_descriptor.block_size[2] =
-                        ((block_size*block_size_mult) & 0x0000ff00)>>8;
+                        (bsize & 0x0000ff00)>>8;
                     tb.ms_data_10->block_descriptor.block_size[3] =
-                        ((block_size*block_size_mult) & 0x000000ff);
+                        (bsize & 0x000000ff);
                     send_command_result(tb.ms_data_10,
                             MIN(sizeof(struct mode_sense_data_10), length));
                     break;
@@ -947,25 +962,24 @@ static void handle_scsi(struct command_block_wrapper* cbw)
                         sizeof(struct mode_sense_bdesc_shortlba);
                     tb.ms_data_6->block_descriptor.density_code = 0;
                     tb.ms_data_6->block_descriptor.reserved = 0;
-                    if(block_count/block_size_mult > 0xffffff) {
+                    if(bcount > 0xffffff) {
                         tb.ms_data_6->block_descriptor.num_blocks[0] = 0xff;
                         tb.ms_data_6->block_descriptor.num_blocks[1] = 0xff;
                         tb.ms_data_6->block_descriptor.num_blocks[2] = 0xff;
-                    }
-                    else {
+                    } else {
                         tb.ms_data_6->block_descriptor.num_blocks[0] =
-                            ((block_count/block_size_mult) & 0xff0000)>>16;
+                            (bcount & 0xff0000)>>16;
                         tb.ms_data_6->block_descriptor.num_blocks[1] =
-                            ((block_count/block_size_mult) & 0x00ff00)>>8;
+                            (bcount & 0x00ff00)>>8;
                         tb.ms_data_6->block_descriptor.num_blocks[2] =
-                            ((block_count/block_size_mult) & 0x0000ff);
+                            (bcount & 0x0000ff);
                     }
                     tb.ms_data_6->block_descriptor.block_size[0] =
-                        ((block_size*block_size_mult) & 0xff0000)>>16;
+                        (bsize & 0xff0000)>>16;
                     tb.ms_data_6->block_descriptor.block_size[1] =
-                        ((block_size*block_size_mult) & 0x00ff00)>>8;
+                        (bsize & 0x00ff00)>>8;
                     tb.ms_data_6->block_descriptor.block_size[2] =
-                        ((block_size*block_size_mult) & 0x0000ff);
+                        (bsize & 0x0000ff);
                     send_command_result(tb.ms_data_6,
                         MIN(sizeof(struct mode_sense_data_6), length));
                     break;
@@ -1009,15 +1023,16 @@ static void handle_scsi(struct command_block_wrapper* cbw)
             send_csw(UMS_STATUS_GOOD);
             break;
 
-        case SCSI_READ_FORMAT_CAPACITY: {
-            logf("scsi read_format_capacity %d",lun);
+        case USB_UFI_READ_FORMAT_CAPACITY: {
+            logf("usb ufi read_format_capacity %d",lun);
             if(lun_present) {
                 tb.format_capacity_data->following_length=htobe32(8);
                 /* "block count" actually means "number of last block" */
-                tb.format_capacity_data->block_count =
-                    htobe32(block_count/block_size_mult - 1);
-                tb.format_capacity_data->block_size =
-                    htobe32(block_size*block_size_mult);
+                if (bcount > 0xffffffff)
+                    tb.format_capacity_data->block_count = 0xffffffff;
+                else
+                    tb.format_capacity_data->block_count = htobe32(bcount - 1);
+                tb.format_capacity_data->block_size = htobe32(bsize);
                 tb.format_capacity_data->block_size |=
                     htobe32(SCSI_FORMAT_CAPACITY_FORMATTED_MEDIA);
 
@@ -1033,20 +1048,30 @@ static void handle_scsi(struct command_block_wrapper* cbw)
            break;
         }
 
-        case SCSI_READ_CAPACITY: {
-            logf("scsi read_capacity %d",lun);
+        case SCSI_READ_CAPACITY_10: {
+            logf("scsi read_capacity10 %d",lun);
 
             if(lun_present) {
                 /* "block count" actually means "number of last block" */
-                tb.capacity_data->block_count =
-                    htobe32(block_count/block_size_mult - 1);
-                tb.capacity_data->block_size =
-                    htobe32(block_size*block_size_mult);
-
-                send_command_result(tb.capacity_data,
-                    MIN(sizeof(struct capacity), length));
-            }
-            else {
+                if (bcount >= 0xffffffff) {
+                    tb.capacity_data_10->block_count[0] = 0xff;
+                    tb.capacity_data_10->block_count[1] = 0xff;
+                    tb.capacity_data_10->block_count[2] = 0xff;
+                    tb.capacity_data_10->block_count[3] = 0xff;
+                } else {
+                    tb.capacity_data_10->block_count[0] = ((bcount-1) & 0xff000000)>>24;
+                    tb.capacity_data_10->block_count[1] = ((bcount-1) & 0x00ff0000)>>16;
+                    tb.capacity_data_10->block_count[2] = ((bcount-1) & 0x0000ff00)>>8;
+                    tb.capacity_data_10->block_count[3] = ((bcount-1) & 0x000000ff);
+                }
+                tb.capacity_data_10->block_size[0] = (bsize & 0xff000000)>>24;
+                tb.capacity_data_10->block_size[1] = (bsize & 0x00ff0000)>>16;
+                tb.capacity_data_10->block_size[2] = (bsize & 0x0000ff00)>>8;
+                tb.capacity_data_10->block_size[3] = (bsize & 0x000000ff);
+
+                send_command_result(tb.capacity_data_10,
+                    MIN(sizeof(struct capacity_10), length));
+            } else {
                 send_command_failed_result();
                 cur_sense_data.sense_key=SENSE_NOT_READY;
                 cur_sense_data.asc=ASC_MEDIUM_NOT_PRESENT;
@@ -1054,7 +1079,59 @@ static void handle_scsi(struct command_block_wrapper* cbw)
             }
             break;
         }
+        case SCSI_READ_CAPACITY_16: {
+            logf("scsi read_capacity16 %d",lun);
 
+            if(lun_present) {
+                /* "block count" actually means "number of last block" */
+#ifdef STORAGE_64BIT_SECTOR
+                    tb.capacity_data_16->block_count[2] =
+                        ((bcount-1) & 0xff00000000ULL)>>40;
+                    tb.capacity_data_16->block_count[3] =
+                        ((bcount-1) & 0x00ff000000ULL)>>32;
+#else
+                    tb.capacity_data_16->block_count[2] = 0;
+                    tb.capacity_data_16->block_count[3] = 0;
+#endif
+                    tb.capacity_data_16->block_count[4] =
+                        ((bcount-1) & 0xff000000)>>24;
+                    tb.capacity_data_16->block_count[5] =
+                        ((bcount-1) & 0x00ff0000)>>16;
+                    tb.capacity_data_16->block_count[6] =
+                        ((bcount-1) & 0x0000ff00)>>8;
+                    tb.capacity_data_16->block_count[7] =
+                        ((bcount-1) & 0x000000ff);
+
+                    tb.ms_data_10->block_descriptor.block_size[0] =
+                        (bsize & 0xff000000)>>24;
+                    tb.ms_data_10->block_descriptor.block_size[1] =
+                        (bsize & 0x00ff0000)>>16;
+                    tb.ms_data_10->block_descriptor.block_size[2] =
+                        (bsize & 0x0000ff00)>>8;
+                    tb.ms_data_10->block_descriptor.block_size[3] =
+                        (bsize & 0x000000ff);
+
+                tb.capacity_data_16->block_size[0] = (bsize & 0xff000000)>>24;
+                tb.capacity_data_16->block_size[1] = (bsize & 0x00ff0000)>>16;
+                tb.capacity_data_16->block_size[2] = (bsize & 0x0000ff00)>>8;
+                tb.capacity_data_16->block_size[3] = (bsize & 0x000000ff);
+
+                uint16_t flags = htobe16((1<<12) | find_first_set_bit(block_size_mult));
+                memcpy(tb.capacity_data_16->flags, &flags, sizeof(flags));
+
+                tb.capacity_data_16->lowest_aligned_lba[0] = 0;
+                tb.capacity_data_16->lowest_aligned_lba[1] = 1;
+
+                send_command_result(tb.capacity_data_16,
+                    MIN(sizeof(struct capacity_16), length));
+            } else {
+                send_command_failed_result();
+                cur_sense_data.sense_key=SENSE_NOT_READY;
+                cur_sense_data.asc=ASC_MEDIUM_NOT_PRESENT;
+                cur_sense_data.ascq=0;
+            }
+            break;
+        }
         case SCSI_READ_10:
             logf("scsi read10 %d",lun);
             if(!lun_present) {
@@ -1075,7 +1152,7 @@ static void handle_scsi(struct command_block_wrapper* cbw)
             cur_cmd.count = block_size_mult *
                 (cbw->command_block[7] << 8 |
                  cbw->command_block[8]);
-            cur_cmd.orig_count = cur_cmd.count;
+            cur_cmd.block_size = block_size;
 
             logf("scsi read %llu %d", cur_cmd.sector, cur_cmd.count);
 
@@ -1088,12 +1165,12 @@ static void handle_scsi(struct command_block_wrapper* cbw)
             else {
 #ifdef USB_USE_RAMDISK
                 memcpy(cur_cmd.data[cur_cmd.data_select],
-                        ramdisk_buffer + cur_cmd.sector*SECTOR_SIZE,
-                        MIN(READ_BUFFER_SIZE/SECTOR_SIZE,cur_cmd.count)*SECTOR_SIZE);
+                        ramdisk_buffer + cur_cmd.sector*cur_cmd.block_size,
+                        MIN(READ_BUFFER_SIZE/cur_cmd.block_size,cur_cmd.count)*cur_cmd.block_size);
 #else
                 cur_cmd.last_result = storage_read_sectors(IF_MD(cur_cmd.lun,)
                         cur_cmd.sector,
-                        MIN(READ_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count),
+                        MIN(READ_BUFFER_SIZE/cur_cmd.block_size, cur_cmd.count),
                         cur_cmd.data[cur_cmd.data_select]);
 #endif
                 send_and_read_next();
@@ -1126,7 +1203,7 @@ static void handle_scsi(struct command_block_wrapper* cbw)
                  cbw->command_block[11] << 16 |
                  cbw->command_block[12] << 8 |
                  cbw->command_block[13]);
-            cur_cmd.orig_count = cur_cmd.count;
+            cur_cmd.block_size = block_size;
 
             logf("scsi read %llu %d", cur_cmd.sector, cur_cmd.count);
 
@@ -1139,12 +1216,12 @@ static void handle_scsi(struct command_block_wrapper* cbw)
             else {
 #ifdef USB_USE_RAMDISK
                 memcpy(cur_cmd.data[cur_cmd.data_select],
-                        ramdisk_buffer + cur_cmd.sector*SECTOR_SIZE,
-                        MIN(READ_BUFFER_SIZE/SECTOR_SIZE,cur_cmd.count)*SECTOR_SIZE);
+                        ramdisk_buffer + cur_cmd.sector*cur_cmd.block_size,
+                        MIN(READ_BUFFER_SIZE/cur_cmd.block_size,cur_cmd.count)*cur_cmd.block_size);
 #else
                 cur_cmd.last_result = storage_read_sectors(IF_MD(cur_cmd.lun,)
                         cur_cmd.sector,
-                        MIN(READ_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count),
+                        MIN(READ_BUFFER_SIZE/cur_cmd.block_size, cur_cmd.count),
                         cur_cmd.data[cur_cmd.data_select]);
 #endif
                 send_and_read_next();
@@ -1171,7 +1248,7 @@ static void handle_scsi(struct command_block_wrapper* cbw)
             cur_cmd.count = block_size_mult *
                 (cbw->command_block[7] << 8 |
                  cbw->command_block[8]);
-            cur_cmd.orig_count = cur_cmd.count;
+            cur_cmd.block_size = block_size;
 
             /* expect data */
             if((cur_cmd.sector + cur_cmd.count) > block_count) {
@@ -1182,7 +1259,7 @@ static void handle_scsi(struct command_block_wrapper* cbw)
             }
             else {
                 receive_block_data(cur_cmd.data[0],
-                        MIN(WRITE_BUFFER_SIZE, cur_cmd.count*SECTOR_SIZE));
+                        MIN(WRITE_BUFFER_SIZE, cur_cmd.count*cur_cmd.block_size));
             }
             break;
 #ifdef STORAGE_64BIT_SECTOR
@@ -1212,7 +1289,7 @@ static void handle_scsi(struct command_block_wrapper* cbw)
                  cbw->command_block[11] << 16 |
                  cbw->command_block[12] << 8 |
                  cbw->command_block[13]);
-            cur_cmd.orig_count = cur_cmd.count;
+            cur_cmd.block_size = block_size;
 
             /* expect data */
             if((cur_cmd.sector + cur_cmd.count) > block_count) {
@@ -1223,7 +1300,7 @@ static void handle_scsi(struct command_block_wrapper* cbw)
             }
             else {
                 receive_block_data(cur_cmd.data[0],
-                        MIN(WRITE_BUFFER_SIZE, cur_cmd.count*SECTOR_SIZE));
+                        MIN(WRITE_BUFFER_SIZE, cur_cmd.count*cur_cmd.block_size));
             }
             break;
 #endif