ipod6g: Support MAX_PHYS_SECTOR_SIZE of 4K

This lets us *natively* handle varying physical sector sizes
without playing games and lying about the logical sector size.

(The original drives use 4K _physical_ sectors with 512B logical
 sectors, but you have to access everything in 4K blocks...)

Achieve this by splitting the MAX_PHYS_SECTOR_SIZE code out
of the main ATA driver and re-using it.

Change-Id: I0bc615ab4562f1e3e83171a8633c74fb60c7da1f

2 files changed, 245 insertions, 211 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 119297ff02..7b43d3c536 100644
--- a/firmware/drivers/ata.c
+++ b/firmware/drivers/ata.c
@@ -115,17 +115,6 @@ static int multisectors; /* number of supported multisectors */
 
 static unsigned short identify_info[ATA_IDENTIFY_WORDS] STORAGE_ALIGN_ATTR;
 
-#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;
-#endif
-
 #ifdef HAVE_ATA_DMA
 static int dma_mode = 0;
 #endif
@@ -600,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,
@@ -632,179 +623,6 @@ int ata_write_sectors(IF_MD(int drive,)
 }
 #endif /* ndef MAX_PHYS_SECTOR_SIZE */
 
-#ifdef MAX_PHYS_SECTOR_SIZE
-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;
-}
-#endif /* MAX_PHYS_SECTOR_SIZE */
-
 static int STORAGE_INIT_ATTR check_registers(void)
 {
     int i;
@@ -1242,9 +1060,6 @@ int STORAGE_INIT_ATTR ata_init(void)
     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;
@@ -1309,31 +1124,12 @@ int STORAGE_INIT_ATTR ata_init(void)
         }
 
 #ifdef MAX_PHYS_SECTOR_SIZE
-        /* 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;
+        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();
     }