9 files changed, 476 insertions, 316 deletions

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 d82fb173cc..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,30 +101,19 @@ 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
 static long power_off_tick = 0;
 #endif
 
-static unsigned long total_sectors;
+static sector_t total_sectors;
 static int multisectors; /* number of supported multisectors */
-static unsigned short identify_info[SECTOR_SIZE/2];
 
-#ifdef MAX_PHYS_SECTOR_SIZE
-
-struct sector_cache_entry {
-    bool inuse;
-    unsigned long 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())
@@ -381,7 +380,7 @@ static ICODE_ATTR void copy_write_sectors(const unsigned char* buf,
 }
 #endif /* !ATA_OPTIMIZED_WRITING */
 
-static int ata_transfer_sectors(unsigned long start,
+static int ata_transfer_sectors(uint64_t start,
                                 int incount,
                                 void* inbuf,
                                 int write)
@@ -437,15 +436,15 @@ static int ata_transfer_sectors(unsigned long start,
 #endif
 
 #ifdef HAVE_LBA48
-        if (lba48)
+        if (ata_lba48)
         {
             ATA_OUT8(ATA_NSECTOR, count >> 8);
             ATA_OUT8(ATA_NSECTOR, count & 0xff);
             ATA_OUT8(ATA_SECTOR, (start >> 24) & 0xff); /* 31:24 */
             ATA_OUT8(ATA_SECTOR, start & 0xff); /* 7:0 */
-            ATA_OUT8(ATA_LCYL, 0); /* 39:32 */
+            ATA_OUT8(ATA_LCYL, (start >> 32) & 0xff); /* 39:32 */
             ATA_OUT8(ATA_LCYL, (start >> 8) & 0xff); /* 15:8 */
-            ATA_OUT8(ATA_HCYL, 0); /* 47:40 */
+            ATA_OUT8(ATA_HCYL, (start >> 40) & 0xff); /* 47:40 */
             ATA_OUT8(ATA_HCYL, (start >> 16) & 0xff); /* 23:16 */
             ATA_OUT8(ATA_SELECT, SELECT_LBA | ata_device);
 #ifdef HAVE_ATA_DMA
@@ -464,7 +463,7 @@ static int ata_transfer_sectors(unsigned long 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,9 +589,11 @@ static int ata_transfer_sectors(unsigned long start,
     return ret;
 }
 
+#include "ata-common.c"
+
 #ifndef MAX_PHYS_SECTOR_SIZE
 int ata_read_sectors(IF_MD(int drive,)
-                     unsigned long start,
+                     sector_t start,
                      int incount,
                      void* inbuf)
 {
@@ -600,14 +601,14 @@ 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;
 }
 
 int ata_write_sectors(IF_MD(int drive,)
-                      unsigned long start,
+                      sector_t start,
                       int count,
                       const void* buf)
 {
@@ -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(unsigned long 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,)
-                     unsigned long 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,)
-                      unsigned long 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;
 }
@@ -916,7 +745,7 @@ static int identify(void)
         return -2;
     }
 
-    for (i=0; i<SECTOR_SIZE/2; i++) {
+    for (i=0; i<ATA_IDENTIFY_WORDS; i++) {
         /* the IDENTIFY words are already swapped, so we need to treat
            this info differently that normal sector data */
         identify_info[i] = ATA_SWAP_IDENTIFY(ATA_IN16(ATA_DATA));
@@ -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,59 +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) */
-            if (identify_info[102] || identify_info[103])
-                panicf("Unsupported disk size: >= 2^32 sectors");
-
-            total_sectors = identify_info[100] | (identify_info[101] << 16);
-            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();
     }
@@ -1323,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;
 }
 
@@ -1360,7 +1180,13 @@ void ata_get_info(IF_MD(int drive,)struct storage_info *info)
     (void)drive; /* unused for now */
 #endif
     int i;
-    info->sector_size = SECTOR_SIZE;
+
+    /* 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;
+
     info->num_sectors = total_sectors;
 
     src = (unsigned short*)&identify_info[27];
@@ -1418,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);
@@ -1432,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) {
@@ -1442,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))) {
@@ -1450,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/fat.c b/firmware/drivers/fat.c
index ebf0f92798..f3c273cc05 100644
--- a/firmware/drivers/fat.c
+++ b/firmware/drivers/fat.c
@@ -173,9 +173,9 @@ union raw_dirent
 
 struct fsinfo
 {
-    unsigned long freecount; /* last known free cluster count */
-    unsigned long nextfree;  /* first cluster to start looking for free
-                                clusters, or 0xffffffff for no hint */
+    sector_t freecount; /* last known free cluster count */
+    sector_t nextfree;  /* first cluster to start looking for free
+                           clusters, or 0xffffffff for no hint */
 };
 /* fsinfo offsets */
 #define FSINFO_SIGNATURE 0
@@ -233,7 +233,7 @@ static struct bpb
     unsigned long totalsectors;
     unsigned long rootdirsector;
     unsigned long firstdatasector;
-    unsigned long startsector;
+    sector_t startsector;
     unsigned long dataclusters;
     unsigned long fatrgnstart;
     unsigned long fatrgnend;
@@ -241,8 +241,8 @@ static struct bpb
 #ifdef HAVE_FAT16SUPPORT
     unsigned int bpb_rootentcnt;    /* Number of dir entries in the root */
     /* internals for FAT16 support */
-    unsigned long rootdirsectornum; /* sector offset of root dir relative to start
-                                     * of first pseudo cluster */
+    sector_t rootdirsectornum; /* sector offset of root dir relative to start
+                                * of first pseudo cluster */
 #endif /* HAVE_FAT16SUPPORT */
 
     /** Additional information kept for each volume **/
@@ -329,7 +329,7 @@ static void cache_discard(IF_MV_NONVOID(struct bpb *fat_bpb))
 }
 
 /* caches a FAT or data area sector */
-static void * cache_sector(struct bpb *fat_bpb, unsigned long secnum)
+static void * cache_sector(struct bpb *fat_bpb, sector_t secnum)
 {
     unsigned int flags;
     void *buf = dc_cache_probe(IF_MV(fat_bpb->volume,) secnum, &flags);
@@ -340,8 +340,8 @@ static void * cache_sector(struct bpb *fat_bpb, unsigned long secnum)
                                       secnum + fat_bpb->startsector, 1, buf);
         if (UNLIKELY(rc < 0))
         {
-            DEBUGF("%s() - Could not read sector %ld"
-                   " (error %d)\n", __func__, secnum, rc);
+            DEBUGF("%s() - Could not read sector %llu"
+                   " (error %d)\n", __func__, (uint64_t)secnum, rc);
             dc_discard_buf(buf);
             return NULL;
         }
@@ -354,14 +354,14 @@ static void * cache_sector(struct bpb *fat_bpb, unsigned long secnum)
  * contents are NOT loaded before returning - use when completely overwriting
  * a sector's contents in order to avoid a fill */
 static void * cache_sector_buffer(IF_MV(struct bpb *fat_bpb,)
-                                  unsigned long secnum)
+                                  sector_t secnum)
 {
     unsigned int flags;
     return dc_cache_probe(IF_MV(fat_bpb->volume,) secnum, &flags);
 }
 
 /* flush a cache buffer to storage */
-void dc_writeback_callback(IF_MV(int volume,) unsigned long sector, void *buf)
+void dc_writeback_callback(IF_MV(int volume,) sector_t sector, void *buf)
 {
     struct bpb * const fat_bpb = &fat_bpbs[IF_MV_VOL(volume)];
     unsigned int copies = !IS_FAT_SECTOR(fat_bpb, sector) ?
@@ -374,8 +374,8 @@ void dc_writeback_callback(IF_MV(int volume,) unsigned long sector, void *buf)
         int rc = storage_write_sectors(IF_MD(fat_bpb->drive,) sector, 1, buf);
         if (rc < 0)
         {
-            panicf("%s() - Could not write sector %ld"
-                   " (error %d)\n", __func__, sector, rc);
+            panicf("%s() - Could not write sector %llu"
+                   " (error %d)\n", __func__, (uint64_t)sector, rc);
         }
 
         if (--copies == 0)
@@ -2397,12 +2397,12 @@ unsigned long fat_query_sectornum(const struct fat_filestr *filestr)
 }
 
 /* helper for fat_readwrite */
-static long transfer(struct bpb *fat_bpb, unsigned long start, long count,
+static long transfer(struct bpb *fat_bpb, sector_t start, long count,
                      char *buf, bool write)
 {
     long rc = 0;
 
-    DEBUGF("%s(s=%lx, c=%lx, wr=%u)\n", __func__,
+    DEBUGF("%s(s=%llx, c=%lx, wr=%u)\n", __func__,
            start + fat_bpb->startsector, count, write ? 1 : 0);
 
     if (write)
@@ -2416,12 +2416,12 @@ static long transfer(struct bpb *fat_bpb, unsigned long start, long count,
             firstallowed = fat_bpb->firstdatasector;
 
         if (start < firstallowed)
-            panicf("Write %ld before data\n", firstallowed - start);
+            panicf("Write %llu before data\n", (uint64_t)(firstallowed - start));
 
         if (start + count > fat_bpb->totalsectors)
         {
-            panicf("Write %ld after data\n",
-                   start + count - fat_bpb->totalsectors);
+            panicf("Write %llu after data\n",
+                   (uint64_t)(start + count - fat_bpb->totalsectors));
         }
     }
 
@@ -2487,14 +2487,14 @@ long fat_readwrite(struct fat_filestr *filestr, unsigned long sectorcount,
     long rc;
 
     long          cluster    = filestr->lastcluster;
-    unsigned long sector     = filestr->lastsector;
+    sector_t sector     = filestr->lastsector;
     long          clusternum = filestr->clusternum;
     unsigned long sectornum  = filestr->sectornum;
 
     DEBUGF("%s(file:%lx,count:0x%lx,buf:%lx,%s)\n", __func__,
            file->firstcluster, sectorcount, (long)buf,
            write ? "write":"read");
-    DEBUGF("%s: sec:%lx numsec:%ld eof:%d\n", __func__,
+    DEBUGF("%s: sec:%llx numsec:%ld eof:%d\n", __func__,
            sector, (long)sectornum, eof ? 1 : 0);
 
     eof = false;
@@ -2534,7 +2534,7 @@ long fat_readwrite(struct fat_filestr *filestr, unsigned long sectorcount,
 
     unsigned long transferred = 0;
     unsigned long count = 0;
-    unsigned long last = sector;
+    sector_t last = sector;
 
     while (transferred + count < sectorcount)
     {
@@ -2961,7 +2961,7 @@ void fat_recalc_free(IF_MV_NONVOID(int volume))
     dc_unlock_cache();
 }
 
-bool fat_size(IF_MV(int volume,) unsigned long *size, unsigned long *free)
+bool fat_size(IF_MV(int volume,) sector_t *size, sector_t *free)
 {
     struct bpb * const fat_bpb = FAT_BPB(volume);
     if (!fat_bpb)
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 9f73b6b5c3..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];
@@ -32,7 +33,7 @@ static unsigned char ramdisk[SECTOR_SIZE * NUM_SECTORS];
 static long last_disk_activity = -1;
 
 int ramdisk_read_sectors(IF_MD(int drive,)
-                     unsigned long start,
+                     sector_t start,
                      int count,
                      void* buf)
 {
@@ -48,7 +49,7 @@ int ramdisk_read_sectors(IF_MD(int drive,)
 }
 
 int ramdisk_write_sectors(IF_MD(int drive,)
-                      unsigned long start,
+                      sector_t start,
                       int count,
                       const void* buf)
 {
@@ -134,7 +135,7 @@ int ramdisk_num_drives(int first_drive)
 {
     /* We don't care which logical drive number(s) we have been assigned */
     (void)first_drive;
-    
+
     return 1;
 }
 #endif
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/sd.c b/firmware/drivers/sd.c
index 6185d5382d..ca83498087 100644
--- a/firmware/drivers/sd.c
+++ b/firmware/drivers/sd.c
@@ -49,6 +49,13 @@ void sd_parse_csd(tCardInfo *card)
         c_size = card_extract_bits(card->csd, 69, 22) + 1;
         card->numblocks = c_size << 10;
     }
+    else if(csd_version == 2)
+    {
+        /* CSD version 3.0 */
+        c_size = card_extract_bits(card->csd, 75, 28) + 1;
+        card->numblocks = c_size << 10;
+    }
+    card->sd2plus = csd_version >= 1;
 
     card->blocksize = 512;  /* Always use 512 byte blocks */
 
@@ -62,7 +69,9 @@ void sd_parse_csd(tCardInfo *card)
 
     card->r2w_factor = card_extract_bits(card->csd, 28, 3);
 
-    logf("CSD%d.0 numblocks:%ld speed:%ld", csd_version+1, card->numblocks, card->speed);
+
+
+    logf("CSD%d.0 numblocks:%lld speed:%ld", csd_version+1, card->numblocks, card->speed);
     logf("nsac: %d taac: %ld r2w: %d", card->nsac, card->taac, card->r2w_factor);
 }
 
@@ -99,4 +108,3 @@ void sd_get_info(IF_MD(int drive,) struct storage_info *info)
     info->revision="0.00";
 }
 #endif
-
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)