firmware/target/arm/as3525/sd-as3525v2.c |   40 ++++++++++++++++++-----------
 1 files changed, 25 insertions(+), 15 deletions(-)


diff --git a/firmware/target/arm/as3525/sd-as3525v2.c b/firmware/target/arm/as3525/sd-as3525v2.c
index d1224b7..e5b68b1 100644
--- a/firmware/target/arm/as3525/sd-as3525v2.c
+++ b/firmware/target/arm/as3525/sd-as3525v2.c
@@ -336,7 +336,9 @@ bool sd_enabled = false;
 #endif
 
 static struct wakeup transfer_completion_signal;
+static struct wakeup command_completion_signal;
 static volatile bool retry;
+static volatile int cmd_error;
 
 #if defined(HAVE_MULTIDRIVE)
 int active_card = 0;
@@ -347,7 +349,8 @@ static inline void mci_delay(void) { int i = 0xffff; while(i--) ; }
 void INT_NAND(void)
 {
     MCI_CTRL &= ~INT_ENABLE;
-    const int status = MCI_MASK_STATUS;
+    /* use raw status here as we need to check some Ints that are masked */
+    const int status = MCI_RAW_STATUS;
 
     MCI_RAW_STATUS = status;    /* clear status */
 
@@ -357,6 +360,11 @@ void INT_NAND(void)
     if( status & (MCI_INT_DTO|MCI_DATA_ERROR))
         wakeup_signal(&transfer_completion_signal);
 
+    cmd_error = status & MCI_CMD_ERROR;
+
+    if(status & MCI_INT_CD)
+        wakeup_signal(&command_completion_signal);
+
     MCI_CTRL |= INT_ENABLE;
 }
 
@@ -401,22 +409,28 @@ static bool send_cmd(const int drive, const int cmd, const int arg, const int fl
             MCI_COMMAND |= CMD_RW_BIT | CMD_CHECK_CRC_BIT;
     }
 
+/*  RCRC & RTO interrupts should be set together with the CD interrupt but
+ *  in practice sometimes incorrectly precede the CD interrupt.  If we leave
+ *  them masked for now we can check them in the isr by reading raw status when
+ *  the CD int is triggered.
+ */
+    MCI_MASK |= MCI_INT_CD;
+
     MCI_ARGUMENT = arg;
     MCI_COMMAND |= CMD_DONE_BIT;
 
-    int max = 0x40000;
-    while(MCI_COMMAND & CMD_DONE_BIT)
-    {
-        if(--max == 0)  /* timeout */
-            return false;
-    }
+    wakeup_wait(&command_completion_signal, TIMEOUT_BLOCK);
 
+    MCI_MASK &= ~MCI_INT_CD;
 
+    /*  Handle command responses */
     if(flags & MCI_RESP)
     {
-        int i = 0xff; while(i--) ;
-        /* if we read the response too fast we might read the response
-         * of the previous command instead */
+        response[0] = MCI_RESP0;    /*  Always prepare short response  */
+
+        if(cmd_error)
+//            panicf("cmderr: %8x cmd: %d", cmd_error, cmd);
+            return false;
 
         if(flags & MCI_LONG_RESP)
         {
@@ -425,8 +439,6 @@ static bool send_cmd(const int drive, const int cmd, const int arg, const int fl
             response[2] = MCI_RESP1;
             response[3] = MCI_RESP0;
         }
-        else
-            response[0] = MCI_RESP0;
     }
     return true;
 }
@@ -700,9 +712,7 @@ static int sd_wait_for_state(const int drive, unsigned int state)
     {
         long tick;
 
-        if(!send_cmd(drive, SD_SEND_STATUS, card_info[drive].rca,
-                    MCI_RESP, &response))
-            return -1;
+        while(!(send_cmd(drive, SD_SEND_STATUS, card_info[drive].rca, MCI_RESP, &response)));
 
         if (((response >> 9) & 0xf) == state)
             return 0;