firmware/target/arm/as3525/sd-as3525v2.c |   50 ++++++++++++++++++------------
 1 files changed, 30 insertions(+), 20 deletions(-)


diff --git a/firmware/target/arm/as3525/sd-as3525v2.c b/firmware/target/arm/as3525/sd-as3525v2.c
index 273507c..5a928c3 100644
--- a/firmware/target/arm/as3525/sd-as3525v2.c
+++ b/firmware/target/arm/as3525/sd-as3525v2.c
@@ -337,7 +337,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;
@@ -348,7 +350,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 */
 
@@ -358,6 +361,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;
 }
 
@@ -402,22 +410,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 & MCI_INT_RCRC)  /*  skipping timeout for now  */
+            panicf("cmderr: %8x cmd: %d", cmd_error, cmd);
+//            return false;
 
         if(flags & MCI_LONG_RESP)
         {
@@ -426,8 +440,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;
 }
@@ -488,6 +500,9 @@ static int sd_init_card(const int drive)
 #endif
     /*  End of Card Identification Mode   ************************************/
 
+    /*  Card back to full speed  */
+    MCI_CLKDIV &= ~(0xFF);    /* CLK_DIV_0 : bits 7:0 = 0x00 */
+
     /* Attempt to switch cards to HS timings, non HS cards just ignore this */
     /*  CMD7 w/rca: Select card to put it in TRAN state */
     if(!send_cmd(drive, SD_SELECT_CARD, card_info[drive].rca, MCI_RESP, &response))
@@ -499,7 +514,6 @@ static int sd_init_card(const int drive)
     /* CMD6 */
     if(!send_cmd(drive, SD_SWITCH_FUNC, 0x80fffff1, MCI_NO_RESP, NULL))
         return -9;
-    mci_delay();
 
     /*  We need to go back to STBY state now so we can read csd */
     /*  CMD7 w/rca=0:  Deselect card to put it in STBY state */
@@ -508,13 +522,11 @@ static int sd_init_card(const int drive)
 
     /* CMD9 send CSD */
     if(!send_cmd(drive, SD_SEND_CSD, card_info[drive].rca,
-                 MCI_RESP|MCI_LONG_RESP, card_info[drive].csd))
+                MCI_RESP|MCI_LONG_RESP, card_info[drive].csd))
         return -11;
 
     sd_parse_csd(&card_info[drive]);
 
-    /*  Card back to full speed  */
-    MCI_CLKDIV &= ~(0xFF);    /* CLK_DIV_0 : bits 7:0 = 0x00 */
 
 #ifndef HAVE_MULTIDRIVE
     /*  CMD7 w/rca: Select card to put it in TRAN state */
@@ -676,7 +688,7 @@ int sd_init(void)
                | 1;             /* clock source = PLLA */
 
     wakeup_init(&transfer_completion_signal);
-
+    wakeup_init(&command_completion_signal);
 #ifdef HAVE_MULTIDRIVE
     /* setup isr for microsd monitoring */
     VIC_INT_ENABLE = (INTERRUPT_GPIOA);
@@ -719,9 +731,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;