1 files changed, 342 insertions, 243 deletions

diff --git a/firmware/target/arm/s5l8702/ipod6g/storage_ata-6g.c b/firmware/target/arm/s5l8702/ipod6g/storage_ata-6g.c
index 7a3be20577..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
@@ -58,10 +61,9 @@
 #define CEATA_DAT_NONBUSY_TIMEOUT 5000000
 #define CEATA_MMC_RCA 1
 
-
 /** static, private data **/
 static uint8_t ceata_taskfile[16] STORAGE_ALIGN_ATTR;
-static uint16_t ata_identify_data[0x100] 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;
@@ -70,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)
 {
@@ -103,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();
     }
 }
@@ -116,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();
     }
 }
@@ -129,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();
     }
 }
@@ -140,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);
 }
 
@@ -149,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;
 }
 
@@ -164,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
@@ -179,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;
 }
 
@@ -227,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
@@ -252,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;
 }
 
@@ -286,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);
@@ -299,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;
 }
 
@@ -321,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;
@@ -335,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;
 }
@@ -344,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;
@@ -358,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);
@@ -381,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,
@@ -395,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)
@@ -403,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;
 }
@@ -427,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);
     }
 }
@@ -477,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);
@@ -510,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 < 0x100; 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;
 }
@@ -549,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.
@@ -573,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;
@@ -596,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;
@@ -620,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;
+
+    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;
@@ -709,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;
@@ -745,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
     {
@@ -764,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);
@@ -780,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);
@@ -812,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));
@@ -823,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);
@@ -851,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;
@@ -896,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;
             }
         }
@@ -916,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));
@@ -945,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))
@@ -957,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;
     }
@@ -966,23 +1000,35 @@ static int ata_reset(void)
     return rc;
 }
 
-int ata_read_sectors(IF_MD(int drive,) unsigned long start, int incount,
+#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;
 }
 
-int ata_write_sectors(IF_MD(int drive,) unsigned long start, int count,
+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)
 {
@@ -993,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);
@@ -1020,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);
 }
@@ -1037,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)
 {
@@ -1061,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;
 }
@@ -1082,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);
@@ -1102,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;
@@ -1129,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)
@@ -1137,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;
 }