2 files changed, 99 insertions, 34 deletions

diff --git a/firmware/export/config/sansaclipplus.h b/firmware/export/config/sansaclipplus.h
index f54951f0ac..310df3a115 100644
--- a/firmware/export/config/sansaclipplus.h
+++ b/firmware/export/config/sansaclipplus.h
@@ -9,13 +9,13 @@
 #define FIRMWARE_OFFSET_FILE_DATA 8
 #define FIRMWARE_OFFSET_FILE_CRC 0
 
-#if 0   /* TODO */
+
 #ifndef BOOTLOADER
 #define HAVE_MULTIDRIVE
 #define NUM_DRIVES 2
 #define HAVE_HOTSWAP
 #endif
-#endif
+
 
 #if 0 /* disabled since there is no driver (yet) */
 
diff --git a/firmware/target/arm/as3525/sd-as3525v2.c b/firmware/target/arm/as3525/sd-as3525v2.c
index 37f3a70601..992db4d307 100644
--- a/firmware/target/arm/as3525/sd-as3525v2.c
+++ b/firmware/target/arm/as3525/sd-as3525v2.c
@@ -140,6 +140,7 @@
 #define CMD_WAIT_PRV_DAT_BIT    (1<<13)
 #define CMD_ABRT_CMD_BIT        (1<<14)
 #define CMD_SEND_INIT_BIT       (1<<15)
+#define CMD_CARD_NO(x)          ((x)<<16)         /* 5 bits wide  */
 #define CMD_SEND_CLK_ONLY       (1<<21)
 #define CMD_READ_CEATA          (1<<22)
 #define CMD_CCS_EXPECTED        (1<<23)
@@ -326,14 +327,9 @@ static struct wakeup transfer_completion_signal;
 static volatile bool retry;
 static volatile bool data_transfer = false;
 
-static inline bool card_detect_target(void)
-{
 #if defined(HAVE_MULTIDRIVE)
-    return !(GPIOA_PIN(2));
-#else
-    return false;
+int active_card = 0;
 #endif
-}
 
 static inline void mci_delay(void) { int i = 0xffff; while(i--) ; }
 
@@ -353,10 +349,32 @@ void INT_NAND(void)
     MCI_CTRL |= INT_ENABLE;
 }
 
-static bool send_cmd(const int cmd, const int arg, const int flags,
+static inline bool card_detect_target(void)
+{
+#if defined(HAVE_MULTIDRIVE)
+    return !(GPIOA_PIN(2));
+#else
+    return false;
+#endif
+}
+
+static bool send_cmd(const int drive, const int cmd, const int arg, const int flags,
         unsigned long *response)
 {
+#if defined(HAVE_MULTIDRIVE)
+    /*  Check to see if we need to switch cards  */
+    if(sd_present(SD_SLOT_AS3525))
+         if(active_card != drive)
+        {
+            GPIOB_PIN(5) = (1-drive) << 5;
+            active_card = drive;
+        }
+
+    MCI_COMMAND = cmd | CMD_CARD_NO(drive);
+#else
+    (void) drive;
     MCI_COMMAND = cmd;
+#endif
 
     if(flags & MCI_RESP)
     {
@@ -420,13 +438,13 @@ static int sd_init_card(const int drive)
     /*  Start of Card Identification Mode ************************************/
 
     /* CMD0 Go Idle  */
-    if(!send_cmd(SD_GO_IDLE_STATE, 0, MCI_NO_RESP, NULL))
+    if(!send_cmd(drive, SD_GO_IDLE_STATE, 0, MCI_NO_RESP, NULL))
         return -1;
     mci_delay();
 
     /* CMD8 Check for v2 sd card.  Must be sent before using ACMD41
       Non v2 cards will not respond to this command*/
-    if(send_cmd(SD_SEND_IF_COND, 0x1AA, MCI_RESP, &response))
+    if(send_cmd(drive, SD_SEND_IF_COND, 0x1AA, MCI_RESP, &response))
         if((response & 0xFFF) == 0x1AA)
             sd_v2 = true;
 
@@ -439,37 +457,45 @@ static int sd_init_card(const int drive)
             return -2;
 
         /* app_cmd */
-        send_cmd(SD_APP_CMD, 0, MCI_RESP, &response);
+        send_cmd(drive, SD_APP_CMD, 0, MCI_RESP, &response);
 
          /* ACMD41 For v2 cards set HCS bit[30] & send host voltage range to all */
-        if(!send_cmd(SD_APP_OP_COND, (sd_v2 ? 0x40FF8000 : (1<<23)),
+        if(!send_cmd(drive, SD_APP_OP_COND, (sd_v2 ? 0x40FF8000 : (1<<23)),
                         MCI_RESP, &card_info[drive].ocr))
             return -3;
     } while(!(card_info[drive].ocr & (1<<31)) );
 
     /* CMD2 send CID */
-    if(!send_cmd(SD_ALL_SEND_CID, 0, MCI_RESP|MCI_LONG_RESP, card_info[drive].cid))
-        return -5;
+    if(!send_cmd(drive, SD_ALL_SEND_CID, 0, MCI_RESP|MCI_LONG_RESP, card_info[drive].cid))
+        return -4;
 
     /* CMD3 send RCA */
-    if(!send_cmd(SD_SEND_RELATIVE_ADDR, 0, MCI_RESP, &card_info[drive].rca))
-        return -4;
+    if(!send_cmd(drive, SD_SEND_RELATIVE_ADDR, 0, MCI_RESP, &card_info[drive].rca))
+        return -5;
 
+#ifdef HAVE_MULTIDRIVE
+    /* Make sure we have 2 unique rca numbers */
+    if(card_info[INTERNAL_AS3525].rca == card_info[SD_SLOT_AS3525].rca)
+        if(!send_cmd(drive, SD_SEND_RELATIVE_ADDR, 0, MCI_RESP, &card_info[drive].rca))
+            return -6;
+#endif
     /*  End of Card Identification Mode   ************************************/
 
     /* CMD9 send CSD */
-    if(!send_cmd(SD_SEND_CSD, card_info[drive].rca,
+    if(!send_cmd(drive, SD_SEND_CSD, card_info[drive].rca,
                  MCI_RESP|MCI_LONG_RESP, card_info[drive].csd))
-        return -5;
+        return -7;
 
     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 */
-    if(!send_cmd(SD_SELECT_CARD, card_info[drive].rca, MCI_NO_RESP, NULL))
-        return -6;
+    if(!send_cmd(drive, SD_SELECT_CARD, card_info[drive].rca, MCI_NO_RESP, NULL))
+        return -8;
+#endif
 
     card_info[drive].initialized = 1;
 
@@ -566,12 +592,15 @@ static void sd_thread(void)
 
 static void init_controller(void)
 {
-    MCI_PWREN = 0x0;                       /*  power off all cards  */
+    int hcon_numcards = ((MCI_HCON>>1) & 0x1F) + 1;
+    int card_mask = (1 << hcon_numcards) - 1;
+
+    MCI_PWREN &= ~card_mask;               /*  power off all cards  */
 
     MCI_CLKSRC = 0x00;                     /* All CLK_SRC_CRD set to 0*/
     MCI_CLKDIV = 0x00;                     /* CLK_DIV_0 : bits 7:0  */
 
-    MCI_PWREN = PWR_CRD_0;                 /*  power up card 0 (internal)  */
+    MCI_PWREN |= card_mask;                /*  power up cards  */
     mci_delay();
 
     MCI_CTRL |= CTRL_RESET;
@@ -584,7 +613,7 @@ static void init_controller(void)
 
     MCI_CTYPE = 0x0;                    /*  all cards 1 bit bus for now */
 
-    MCI_CLKENA = CCLK_ENA_CRD0;
+    MCI_CLKENA = card_mask;
 
     MCI_ARGUMENT = 0;
     MCI_COMMAND = CMD_DONE_BIT|CMD_SEND_CLK_ONLY|CMD_WAIT_PRV_DAT_BIT;
@@ -598,6 +627,8 @@ static void init_controller(void)
 
     MCI_MASK = 0xffffffff & ~(MCI_INT_ACD|MCI_INT_CRDDET);
 
+    GPIOB_DIR |= (1<<5);         /* Pin B5 output  */
+
     MCI_CTRL |= INT_ENABLE;
 }
 
@@ -614,14 +645,23 @@ int sd_init(void)
     CGU_MEMSTICK = (1<<8) | (1<<7) |
         ((CLK_DIV(AS3525_PLLA_FREQ, AS3525_MS_FREQ) -1) << 2) | 1;
 
-    /* FIXME: divider should be shifted by 2, but doing prevents card
-     * initialisation */
     *(volatile int*)(CGU_BASE+0x3C) = (1<<7) |
-        (CLK_DIV(AS3525_PLLA_FREQ, 24000000) -1) | 1;
+        (CLK_DIV(AS3525_PLLA_FREQ, 24000000) -1)<<2 | 1;
 
     wakeup_init(&transfer_completion_signal);
 
-    VIC_INT_ENABLE |= INTERRUPT_NAND;
+#ifdef HAVE_MULTIDRIVE
+    /* setup isr for microsd monitoring */
+    VIC_INT_ENABLE = (INTERRUPT_GPIOA);
+    /* clear previous irq */
+    GPIOA_IC = (1<<2);
+    /* enable edge detecting */
+    GPIOA_IS &= ~(1<<2);
+    /* detect both raising and falling edges */
+    GPIOA_IBE |= (1<<2);
+#endif
+
+    VIC_INT_ENABLE = INTERRUPT_NAND;
 
     init_controller();
     ret = sd_init_card(INTERNAL_AS3525);
@@ -652,7 +692,7 @@ static int sd_wait_for_state(const int drive, unsigned int state)
     {
         long tick;
 
-        if(!send_cmd(SD_SEND_STATUS, card_info[drive].rca,
+        if(!send_cmd(drive, SD_SEND_STATUS, card_info[drive].rca,
                     MCI_RESP, &response))
             return -1;
 
@@ -698,6 +738,12 @@ static int sd_transfer_sectors(IF_MD2(int drive,) unsigned long start,
         }
     }
 
+#ifdef HAVE_MULTIDRIVE
+    /*  CMD7 w/rca: Select card to put it in TRAN state */
+    if(!send_cmd(drive, SD_SELECT_CARD, card_info[drive].rca, MCI_NO_RESP, NULL))
+        return -6;
+#endif
+
     last_disk_activity = current_tick;
     ret = sd_wait_for_state(drive, SD_TRAN);
     if (ret < 0)
@@ -706,7 +752,7 @@ static int sd_transfer_sectors(IF_MD2(int drive,) unsigned long start,
             "IDLE", "RDY", "IDENT", "STBY", "TRAN", "DATA", "RCV", "PRG", "DIS"
         };
         if(ret <= -10)
-            panicf("wait for state failed (%s)", st[(-ret / 10) % 9]);
+            panicf("wait for state failed (%s) %d", st[(-ret / 10) % 9], drive);
         else
             panicf("wait for state failed");
         goto sd_transfer_error;
@@ -745,9 +791,9 @@ static int sd_transfer_sectors(IF_MD2(int drive,) unsigned long start,
 
 
         if(card_info[drive].ocr & (1<<30) ) /* SDHC */
-            ret = send_cmd(cmd, start, MCI_NO_RESP, NULL);
+            ret = send_cmd(drive, cmd, start, MCI_NO_RESP, NULL);
         else
-            ret = send_cmd(cmd, start * SD_BLOCK_SIZE,
+            ret = send_cmd(drive, cmd, start * SD_BLOCK_SIZE,
                     MCI_NO_RESP, NULL);
 
         if (ret < 0)
@@ -766,7 +812,7 @@ static int sd_transfer_sectors(IF_MD2(int drive,) unsigned long start,
 
         last_disk_activity = current_tick;
 
-        if(!send_cmd(SD_STOP_TRANSMISSION, 0, MCI_NO_RESP, NULL))
+        if(!send_cmd(drive, SD_STOP_TRANSMISSION, 0, MCI_NO_RESP, NULL))
         {
             ret = -666;
             panicf("STOP TRANSMISSION failed");
@@ -792,6 +838,12 @@ static int sd_transfer_sectors(IF_MD2(int drive,) unsigned long start,
 
     dma_release();
 
+#ifdef HAVE_MULTIDRIVE
+    /*  CMD7 w/rca =0 : deselect card to put it in STBY state */
+    if(!send_cmd(drive, SD_SELECT_CARD, 0, MCI_NO_RESP, NULL))
+        return -6;
+#endif
+
 #ifndef BOOTLOADER
     sd_enable(false);
 #endif
@@ -836,7 +888,10 @@ long sd_last_disk_activity(void)
 void sd_enable(bool on)
 {
     /* TODO */
-    (void)on;
+    if(on)
+        CCU_IO |= (1<<2);              /* XPD is SD-MCI interface (b3:2 = 01) */
+    else
+        CCU_IO &= ~(1<<2);             /* XPD is general purpose IO (b3:2 = 00) */
     return;
 }
 
@@ -889,3 +944,13 @@ void card_enable_monitoring_target(bool on)
         GPIOA_IE &= ~(1<<2);
 }
 #endif /* HAVE_HOTSWAP */
+
+#ifdef CONFIG_STORAGE_MULTI
+int sd_num_drives(int first_drive)
+{
+    /* We don't care which logical drive number(s) we have been assigned */
+    (void)first_drive;
+
+    return NUM_DRIVES;
+}
+#endif /* CONFIG_STORAGE_MULTI */