sd read debugging

Index: sd-as3525.c
===================================================================
--- sd-as3525.c (revision 27164)
+++ sd-as3525.c (working copy)
@@ -257,6 +257,28 @@
     return false;
 }

+static void sd_set_widebus_mode(IF_MD2(int drive,) int on) {
+#ifndef HAVE_MULTIDRIVE
+    const int drive = 0;
+#endif
+    long response;
+    if(sd_wait_for_tran_state(drive) < 0)
+        panicf("sd_set_widebus: tran");
+    /* CMD55 */              /*  Response is requested due to timing issue  */
+    if(!send_cmd(drive, SD_APP_CMD, card_info[drive].rca, MCI_RESP, &response))
+        panicf("sd_set_widebus: app_cmd");
+    /* ACMD6  */
+    if(!send_cmd(drive, SD_SET_BUS_WIDTH, on ? 2 : 0, MCI_RESP, &response))
+        panicf("sd_set_widebus: set_bus_width");
+    /* Now that card is widebus make controller aware */
+    if (on) {
+        MCI_CLOCK(drive) |= MCI_CLOCK_WIDEBUS;
+    } else {
+        MCI_CLOCK(drive) &= ~MCI_CLOCK_WIDEBUS;
+    }
+}
+
+
 #define MCI_FULLSPEED     (MCI_CLOCK_ENABLE | MCI_CLOCK_BYPASS)     /* MCLK   */
 #define MCI_HALFSPEED     (MCI_CLOCK_ENABLE)                        /* MCLK/2 */
 #define MCI_QUARTERSPEED  (MCI_CLOCK_ENABLE | 1)                    /* MCLK/4 */
@@ -362,24 +384,17 @@
     if(!send_cmd(drive, SD_SELECT_CARD, card_info[drive].rca, MCI_RESP, &response))
         return -10;

-    /*  Switch to to 4 bit widebus mode  */
-    if(sd_wait_for_tran_state(drive) < 0)
-        return -11;
+    /* Disable card detect pullup, we don't need it */
     /* CMD55 */             /*  Response is requested due to timing issue  */
     if(!send_cmd(drive, SD_APP_CMD, card_info[drive].rca, MCI_RESP, &response))
         return -14;
     /* ACMD42  */
     if(!send_cmd(drive, SD_SET_CLR_CARD_DETECT, 0, MCI_RESP, &response))
         return -15;
-    /* CMD55 */              /*  Response is requested due to timing issue  */
-    if(!send_cmd(drive, SD_APP_CMD, card_info[drive].rca, MCI_RESP, &response))
-        return -12;
-    /* ACMD6  */
-    if(!send_cmd(drive, SD_SET_BUS_WIDTH, 2, MCI_RESP, &response))
-        return -13;
-    /* Now that card is widebus make controller aware */
-    MCI_CLOCK(drive) |= MCI_CLOCK_WIDEBUS;

+    /*  Switch to to 4 bit widebus mode  */
+    sd_set_widebus_mode(IF_MD2(drive,) 1);
+
     /*
      * enable bank switching
      * without issuing this command, we only have access to 1/4 of the blocks
@@ -874,10 +889,38 @@
     return ret;
 }

+#define VERIFY_READ 1
+
 int sd_read_sectors(IF_MD2(int drive,) unsigned long start, int count,
                      void* buf)
 {
-    return sd_transfer_sectors(IF_MD2(drive,) start, count, buf, false);
+    int ret;
+
+    ret = sd_transfer_sectors(IF_MD2(drive,) start, count, buf, false);
+
+#ifdef VERIFY_READ
+    if (ret) /* read failed, no point in verifying? */
+        return ret;
+
+    sd_set_widebus_mode(IF_MD2(drive,) 0); /* verify in 1bit mode */
+
+    while (count) {
+        int transfer = count;
+        if(transfer > UNALIGNED_NUM_SECTORS)
+            transfer = UNALIGNED_NUM_SECTORS;
+
+        sd_transfer_sectors(IF_MD2(drive,) start, transfer, aligned_buffer, false);
+        if (memcmp(buf, aligned_buffer, transfer * 512) != 0)
+            panicf("sd: read verify failed: sec=%ld n=%d!", start, transfer);
+
+        buf   += transfer * 512;
+        count -= transfer;
+        start += transfer;
+    }
+
+    sd_set_widebus_mode(IF_MD2(drive,) 1); /* switch back to 4bit mode */
+#endif
+    return ret;
 }

 int sd_write_sectors(IF_MD2(int drive,) unsigned long start, int count,
@@ -908,7 +951,7 @@
         if (memcmp(buf, aligned_buffer, transfer * 512) != 0) {
             /* try the write again in the hope to repair the damage */
             sd_transfer_sectors(IF_MD2(drive,) saved_start, saved_count, saved_buf, true);
-            panicf("sd: verify failed: sec=%ld n=%d!", start, transfer);
+            panicf("sd: write verify failed: sec=%ld n=%d!", start, transfer);
         }

         buf   += transfer * 512;