5 files changed, 103 insertions, 62 deletions

diff --git a/firmware/drivers/ata.c b/firmware/drivers/ata.c
index c995f6de85..8df9acb9da 100644
--- a/firmware/drivers/ata.c
+++ b/firmware/drivers/ata.c
@@ -103,6 +103,7 @@ static long sleep_timeout = 5*HZ;
 #ifdef HAVE_LBA48
 static bool lba48 = false; /* set for 48 bit addressing */
 #endif
+static bool canflush = true;
 
 static long last_disk_activity = -1;
 #ifdef HAVE_ATA_POWER_OFF
@@ -141,11 +142,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 &&
@@ -223,7 +219,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);
@@ -257,13 +253,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 (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;
     }
 
@@ -286,6 +287,16 @@ static int ata_perform_flush_cache(void)
     return 0;
 }
 
+int ata_flush(void)
+{
+    if (ata_state >= ATA_SPINUP) {
+        mutex_lock(&ata_mtx);
+        ata_perform_flush_cache();
+        mutex_unlock(&ata_mtx);
+    }
+    return 0;
+}
+
 static ICODE_ATTR int wait_for_start_of_transfer(void)
 {
     if (!wait_for_bsy())
@@ -844,7 +855,7 @@ void ata_spindown(int seconds)
 
 bool ata_disk_is_active(void)
 {
-    return ata_disk_can_poweroff() ? (ata_state >= ATA_SPINUP) : 0;
+    return (ata_state >= ATA_SPINUP);
 }
 
 void ata_sleepnow(void)
@@ -856,8 +867,9 @@ void ata_sleepnow(void)
             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
             }
         }
@@ -967,6 +979,9 @@ static int perform_soft_reset(void)
     if (set_multiple_mode(multisectors))
         return -3;
 
+    if (identify())
+        return -2;
+
     if (freeze_lock())
         return -4;
 
@@ -1017,6 +1032,9 @@ static int ata_power_on(void)
     if (set_multiple_mode(multisectors))
         return -3;
 
+    if (identify())
+        return -2;
+
     if (freeze_lock())
         return -4;
 
@@ -1255,7 +1273,6 @@ int STORAGE_INIT_ATTR ata_init(void)
         }
 
         rc = identify();
-
         if (rc) {
             rc = -40 + rc;
             goto error;
@@ -1280,13 +1297,12 @@ int STORAGE_INIT_ATTR ata_init(void)
 #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;
@@ -1325,6 +1341,12 @@ 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);
     return rc;
@@ -1440,7 +1462,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) {
diff --git a/firmware/export/ata.h b/firmware/export/ata.h
index 0d84a91524..dfa73eebf5 100644
--- a/firmware/export/ata.h
+++ b/firmware/export/ata.h
@@ -208,8 +208,8 @@ static inline int ata_disk_isssd(void)
            );
 }
 
-/* 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.
@@ -217,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/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/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/s5l8702/ipod6g/storage_ata-6g.c b/firmware/target/arm/s5l8702/ipod6g/storage_ata-6g.c
index 87450aca73..21c6f3f7c0 100644
--- a/firmware/target/arm/s5l8702/ipod6g/storage_ata-6g.c
+++ b/firmware/target/arm/s5l8702/ipod6g/storage_ata-6g.c
@@ -83,6 +83,7 @@ static uint32_t ata_dma_flags;
 static long ata_last_activity_value = -1;
 static long ata_sleep_timeout = 7 * HZ;
 static bool ata_powered;
+static bool canflush = true;
 static struct semaphore mmc_wakeup;
 static struct semaphore mmc_comp_wakeup;
 static int spinup_time = 0;
@@ -578,7 +579,7 @@ static void ata_set_active(void)
 
 bool ata_disk_is_active(void)
 {
-    return ata_disk_can_poweroff() ? ata_powered : 0;
+    return ata_powered;
 }
 
 static int ata_set_feature(uint32_t feature, uint32_t param)
@@ -745,6 +746,8 @@ static int ata_power_up(void)
         ata_dma = param ? true : false;
         dma_mode = param;
         PASS_RC(ata_set_feature(0x03, param), 3, 4); /* Transfer mode */
+
+        /* SET_FEATURE only supported on PATA, not CE-ATA */
         if (ata_identify_data[82] & BIT(5))
             PASS_RC(ata_set_feature(0x02, 0), 3, 5); /* Enable volatile write cache */
         if (ata_identify_data[82] & BIT(6))
@@ -753,7 +756,10 @@ static int ata_power_up(void)
             PASS_RC(ata_set_feature(0x05, 0x80), 3, 7); /* Enable lowest power mode w/o standby */
         if (ata_identify_data[83] & BIT(9))
             PASS_RC(ata_set_feature(0x42, 0x80), 3, 8); /* Enable lowest noise mode */
+
+        PASS_RC(ata_identify(ata_identify_data), 3, 9); /* Finally, re-read identify info */
     }
+
     spinup_time = current_tick - spinup_start;
 
     ata_total_sectors = (ata_identify_data[61] << 16) | ata_identify_data[60];
@@ -778,28 +784,6 @@ 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);
-    }
     PCON(7) = 0;
     PCON(8) = 0;
     PCON(9) = 0;
@@ -1080,26 +1064,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 && ata_identify_data[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 (ata_identify_data[83] & BIT(12)) {
+            cmd = CMD_FLUSH_CACHE; /* Flag, mandatory, ATA-6 and up */
+        } else if (ata_identify_data[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);
@@ -1107,14 +1092,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);
 }