ata: Support for ATA logical sector sizes > 512B

Basically this requires un-hardcoding SECTOR_SIZE everywhere, in
favor of using a variable containing what was reported in IDENTIFY INFO.

Note that the rest of the storage subsystem still needs to be fixed up!

Change-Id: I7f2dbd54ff2bc16b15010721e011949cbf308e12

3 files changed, 55 insertions, 45 deletions

diff --git a/firmware/drivers/ata-common.c b/firmware/drivers/ata-common.c
index 53a7780262..fa908127b4 100644
--- a/firmware/drivers/ata-common.c
+++ b/firmware/drivers/ata-common.c
@@ -22,6 +22,12 @@
 
 #ifdef MAX_PHYS_SECTOR_SIZE
 
+#ifdef MAX_LOG_SECTOR_SIZE
+#define __MAX_LOG_SECTOR_SIZE MAX_LOG_SECTOR_SIZE
+#else
+#define __MAX_LOG_SECTOR_SIZE SECTOR_SIZE
+#endif
+
 struct sector_cache_entry {
     unsigned char data[MAX_PHYS_SECTOR_SIZE];
     sector_t sectornum;  /* logical sector */
@@ -29,7 +35,7 @@ struct sector_cache_entry {
 };
 /* 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 uint16_t phys_sector_mult = 1;
 
 static int cache_sector(sector_t sector)
 {
@@ -84,11 +90,11 @@ int ata_read_sectors(IF_MD(int drive,)
             rc = rc * 10 - 1;
             goto error;
         }
-        memcpy(inbuf, sector_cache.data + offset * SECTOR_SIZE,
-               partcount * SECTOR_SIZE);
+        memcpy(inbuf, sector_cache.data + offset * log_sector_size,
+               partcount * log_sector_size);
 
         start += partcount;
-        inbuf += partcount * SECTOR_SIZE;
+        inbuf += partcount * log_sector_size;
         incount -= partcount;
     }
     if (incount)
@@ -105,7 +111,7 @@ int ata_read_sectors(IF_MD(int drive,)
                 goto error;
             }
             start += incount;
-            inbuf += incount * SECTOR_SIZE;
+            inbuf += incount * log_sector_size;
         }
         if (offset)
         {
@@ -115,7 +121,7 @@ int ata_read_sectors(IF_MD(int drive,)
                 rc = rc * 10 - 3;
                 goto error;
             }
-            memcpy(inbuf, sector_cache.data, offset * SECTOR_SIZE);
+            memcpy(inbuf, sector_cache.data, offset * log_sector_size);
         }
     }
 
@@ -150,8 +156,8 @@ int ata_write_sectors(IF_MD(int drive,)
             rc = rc * 10 - 1;
             goto error;
         }
-        memcpy(sector_cache.data + offset * SECTOR_SIZE, buf,
-               partcount * SECTOR_SIZE);
+        memcpy(sector_cache.data + offset * log_sector_size, buf,
+               partcount * log_sector_size);
         rc = flush_current_sector();
         if (rc)
         {
@@ -159,7 +165,7 @@ int ata_write_sectors(IF_MD(int drive,)
             goto error;
         }
         start += partcount;
-        buf += partcount * SECTOR_SIZE;
+        buf += partcount * log_sector_size;
         count -= partcount;
     }
     if (count)
@@ -176,7 +182,7 @@ int ata_write_sectors(IF_MD(int drive,)
                 goto error;
             }
             start += count;
-            buf += count * SECTOR_SIZE;
+            buf += count * log_sector_size;
         }
         if (offset)
         {
@@ -186,7 +192,7 @@ int ata_write_sectors(IF_MD(int drive,)
                 rc = rc * 10 - 4;
                 goto error;
             }
-            memcpy(sector_cache.data, buf, offset * SECTOR_SIZE);
+            memcpy(sector_cache.data, buf, offset * log_sector_size);
             rc = flush_current_sector();
             if (rc)
             {
@@ -220,15 +226,15 @@ static int ata_get_phys_sector_mult(void)
            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];
+        char throwaway[__MAX_LOG_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);
+    if (phys_sector_mult > (MAX_PHYS_SECTOR_SIZE/log_sector_size))
+        panicf("Unsupported physical sector size: %ld",
+               phys_sector_mult * log_sector_size);
 
     memset(&sector_cache, 0, sizeof(sector_cache));
 
diff --git a/firmware/drivers/ata.c b/firmware/drivers/ata.c
index 7b43d3c536..b688da494e 100644
--- a/firmware/drivers/ata.c
+++ b/firmware/drivers/ata.c
@@ -111,6 +111,7 @@ static long power_off_tick = 0;
 #endif
 
 static sector_t total_sectors;
+static uint32_t log_sector_size;
 static int multisectors; /* number of supported multisectors */
 
 static unsigned short identify_info[ATA_IDENTIFY_WORDS] STORAGE_ALIGN_ATTR;
@@ -431,7 +432,7 @@ static int ata_transfer_sectors(uint64_t start,
 
 #ifdef HAVE_ATA_DMA
         /* If DMA is supported and parameters are ok for DMA, use it */
-        if (dma_mode && ata_dma_setup(inbuf, incount * SECTOR_SIZE, write))
+        if (dma_mode && ata_dma_setup(inbuf, incount * log_sector_size, write))
             usedma = true;
 #endif
 
@@ -533,7 +534,7 @@ static int ata_transfer_sectors(uint64_t start,
                 else
                     sectors = count;
 
-                wordcount = sectors * SECTOR_SIZE / 2;
+                wordcount = sectors * log_sector_size / 2;
 
                 if (write)
                     copy_write_sectors(buf, wordcount);
@@ -557,7 +558,7 @@ static int ata_transfer_sectors(uint64_t start,
                     goto retry;
                 }
 
-                buf += sectors * SECTOR_SIZE; /* Advance one chunk of sectors */
+                buf += sectors * log_sector_size; /* Advance one chunk of sectors */
                 count -= sectors;
 
                 keep_ata_active();
@@ -1097,7 +1098,7 @@ int STORAGE_INIT_ATTR ata_init(void)
         if (multisectors == 0) /* Invalid multisector info, try with 16 */
             multisectors = 16;
 
-        DEBUGF("ata: %d sectors per ata request\n",multisectors);
+        DEBUGF("ata: %d sectors per ata request\n", multisectors);
 
         total_sectors = (identify_info[61] << 16) | identify_info[60];
 
@@ -1111,6 +1112,12 @@ int STORAGE_INIT_ATTR ata_init(void)
         }
 #endif /* HAVE_LBA48 */
 
+        /* Logical sector size > 512B ? */
+        if ((identify_info[106] & 0xd000) == 0x5000) /* B14, B12 */
+            log_sector_size = (identify_info[117] | (identify_info[118] << 16)) * 2;
+        else
+            log_sector_size = SECTOR_SIZE;
+
         rc = freeze_lock();
         if (rc) {
             rc = -50 + rc;
@@ -1181,12 +1188,7 @@ void ata_get_info(IF_MD(int drive,)struct storage_info *info)
 #endif
     int i;
 
-    /* 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->sector_size = log_sector_size;
     info->num_sectors = total_sectors;
 
     src = (unsigned short*)&identify_info[27];
diff --git a/firmware/target/arm/s5l8702/ipod6g/storage_ata-6g.c b/firmware/target/arm/s5l8702/ipod6g/storage_ata-6g.c
index 8cc5b44aca..3bde5647ba 100644
--- a/firmware/target/arm/s5l8702/ipod6g/storage_ata-6g.c
+++ b/firmware/target/arm/s5l8702/ipod6g/storage_ata-6g.c
@@ -68,6 +68,7 @@ static bool ceata;
 static bool ata_lba48;
 static bool ata_dma;
 static uint64_t ata_total_sectors;
+static uint32_t log_sector_size;
 static struct mutex ata_mutex;
 static struct semaphore ata_wakeup;
 static uint32_t ata_dma_flags;
@@ -836,16 +837,16 @@ static int ata_rw_chunk_internal(uint64_t sector, uint32_t cnt, void* buffer, bo
             if (write)
             {
                 ATA_SBUF_START = buffer;
-                ATA_SBUF_SIZE = SECTOR_SIZE * cnt;
+                ATA_SBUF_SIZE = log_sector_size * cnt;
                 ATA_CFG |= BIT(4);
             }
             else
             {
                 ATA_TBUF_START = buffer;
-                ATA_TBUF_SIZE = SECTOR_SIZE * cnt;
+                ATA_TBUF_SIZE = log_sector_size * cnt;
                 ATA_CFG &= ~BIT(4);
             }
-            ATA_XFR_NUM = SECTOR_SIZE * cnt - 1;
+            ATA_XFR_NUM = log_sector_size * cnt - 1;
             ATA_CFG |= ata_dma_flags;
             ATA_CFG &= ~(BIT(7) | BIT(8));
             semaphore_wait(&ata_wakeup, 0);
@@ -866,15 +867,15 @@ static int ata_rw_chunk_internal(uint64_t sector, uint32_t cnt, void* buffer, bo
         {
             while (cnt--)
             {
-                int i;
+                uint16_t i;
                 PASS_RC(ata_wait_for_start_of_transfer(500000), 2, 1);
                 if (write)
-                    for (i = 0; i < SECTOR_SIZE/2; i++)
+                    for (i = 0; i < log_sector_size/2; i++)
                         ata_write_cbr(&ATA_PIO_DTR, ((uint16_t*)buffer)[i]);
                 else
-                    for (i = 0; i < SECTOR_SIZE/2; i++)
+                    for (i = 0; i < log_sector_size/2; i++)
                         ((uint16_t*)buffer)[i] = ata_read_cbr(&ATA_PIO_DTR);
-                buffer += SECTOR_SIZE;
+                buffer += log_sector_size;
             }
         }
         PASS_RC(ata_wait_for_end_of_transfer(100000), 2, 3);
@@ -927,11 +928,11 @@ static int ata_transfer_sectors(uint64_t sector, uint32_t count, void* buffer, b
                 }
                 if (rc)
                     break;
-                buf += SECTOR_SIZE;
+                buf += log_sector_size;
             }
         }
         PASS_RC(rc, 1, 1);
-        buffer += SECTOR_SIZE * cnt;
+        buffer += log_sector_size * cnt;
         sector += cnt;
         count -= cnt;
     }
@@ -1119,16 +1120,11 @@ void ata_spin(void)
 #ifdef STORAGE_GET_INFO
 void ata_get_info(IF_MD(int drive,) struct storage_info *info)
 {
-    /* 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";
+    info->sector_size = log_sector_size;
+    info->num_sectors = ata_total_sectors;
+    info->vendor = "Apple";
+    info->product = "iPod Classic";
+    info->revision = "1.0";
 }
 #endif
 
@@ -1149,11 +1145,17 @@ int ata_init(void)
 
     /* get identify_info */
     mutex_lock(&ata_mutex);
-    int rc = ata_power_up();
+    int rc = ata_power_up(); /* Include identify() call */
     mutex_unlock(&ata_mutex);
     if (IS_ERR(rc))
         return rc;
 
+    /* Logical sector size */
+    if ((identify_info[106] & 0xd000) == 0x5000) /* B14, B12 */
+        log_sector_size = (identify_info[117] | (identify_info[118] << 16)) * 2;
+    else
+        log_sector_size = SECTOR_SIZE;
+
 #ifdef MAX_PHYS_SECTOR_SIZE
     rc = ata_get_phys_sector_mult();
     if (IS_ERR(rc))