1 files changed, 32 insertions, 17 deletions

diff --git a/firmware/target/arm/tms320dm320/sdmmc-dm320.c b/firmware/target/arm/tms320dm320/sdmmc-dm320.c
index cc5c4a822e..33354a3146 100644
--- a/firmware/target/arm/tms320dm320/sdmmc-dm320.c
+++ b/firmware/target/arm/tms320dm320/sdmmc-dm320.c
@@ -120,6 +120,7 @@ static struct sd_card_status sd_status[NUM_CARDS] =
 
 /* Shoot for around 75% usage */
 static struct mutex             sd_mtx SHAREDBSS_ATTR;
+static struct semaphore         data_done SHAREDBSS_ATTR;
 static volatile unsigned int    transfer_error[NUM_DRIVES];
 /* align on cache line size */
 static unsigned char    aligned_buffer[UNALIGNED_NUM_SECTORS * SD_BLOCK_SIZE] 
@@ -234,25 +235,20 @@ static int sd_poll_status(int st_reg_num, volatile unsigned int flag)
 
 static int dma_wait_for_completion(void)
 {
-    unsigned short dma_status;
+    uint16_t dma_status;
 
-    do
-    {
-        long time = current_tick;
+    semaphore_wait(&data_done, TIMEOUT_BLOCK);
 
-        if (TIME_AFTER(time, next_yield))
-        {
-            long ty = current_tick;
-            yield();
-            next_yield = ty + MIN_YIELD_PERIOD;
-        }
+    dma_status = IO_MMC_SD_DMA_STATUS1;
+    if (dma_status & MMC_DMASTAT1_TOUTDT)
+    {
+        return -EC_DATA_TIMEOUT;
+    }
 
-        dma_status = IO_MMC_SD_DMA_STATUS1;
-        if (dma_status & (1 << 13))
-        {
-            return -EC_DATA_TIMEOUT;
-        }
-    } while (dma_status & (1 << 12));
+    if (dma_status & MMC_DMASTAT1_RUNST)
+    {
+        panicf("Semaphore released while DMA still running");
+    }
 
     return EC_OK;
 }
@@ -794,6 +790,7 @@ int sd_init(void)
     if (!initialized)
     {
         mutex_init(&sd_mtx);
+        semaphore_init(&data_done, 1, 0);
         initialized = true;
     }
 
@@ -810,7 +807,7 @@ int sd_init(void)
 
     /* mmc module clock: 75 Mhz (AHB) / 2 = ~37.5 Mhz
      * (Frequencies above are taken from Sansa Connect's OF source code) */
-    IO_CLK_DIV3 = (IO_CLK_DIV3 & 0xFF00) | 0x02; /* OF uses 1 */
+    IO_CLK_DIV3 = (IO_CLK_DIV3 & 0xFF00) | 0x01;
 
     bitset16(&IO_CLK_MOD2, CLK_MOD2_MMC);
 
@@ -847,6 +844,10 @@ int sd_init(void)
 #endif
 #endif
 
+    /* Enable data done interrupt */
+    IO_MMC_INT_ENABLE |= MMC_IE_EDATDNE;
+    IO_INTC_EINT1 |= INTR_EINT1_MMCSDMS0;
+
     /* Disable Memory Card CLK - it is enabled on demand by TMS320DM320 */
     bitclr16(&IO_MMC_MEM_CLK_CONTROL, (1 << 8));
     mutex_unlock(&sd_mtx);
@@ -888,3 +889,17 @@ int sd_event(long id, intptr_t data)
 
     return rc;
 }
+
+void SD_MMC(void) __attribute__ ((section(".icode")));
+void SD_MMC(void)
+{
+    uint16_t status = IO_MMC_STATUS0;
+
+    /* Clear interrupt */
+    IO_INTC_IRQ1 = INTR_IRQ1_MMCSDMS0;
+
+    if (status & MMC_ST0_DATDNE)
+    {
+        semaphore_release(&data_done);
+    }
+}