diff --git a/firmware/export/dm320.h b/firmware/export/dm320.h
index def8508b0b..01f206bfc9 100644
--- a/firmware/export/dm320.h
+++ b/firmware/export/dm320.h
@@ -930,11 +930,27 @@ extern unsigned long _ttbstart;
 #define MMC_ST1_DXFULL                 (1 << 3)
 #define MMC_ST1_DAT3ST                 (1 << 4)
 
+#define MMC_IE_EDATDNE                 (1 << 0)
+#define MMC_IE_EBSYDNE                 (1 << 1)
+#define MMC_IE_ERSPDNE                 (1 << 2)
+#define MMC_IE_ETOUTRD                 (1 << 3)
+#define MMC_IE_ETOUTRS                 (1 << 4)
+#define MMC_IE_ECRCWR                  (1 << 5)
+#define MMC_IE_ECRCRD                  (1 << 6)
+#define MMC_IE_ECRCRS                  (1 << 7)
+#define MMC_IE_EDXRDY                  (1 << 9)
+#define MMC_IE_EDRRDY                  (1 << 10)
+#define MMC_IE_EDATED                  (1 << 11)
+
 #define MMC_DMAMODE_RD_WORDSWAP        (1 << 10)
 #define MMC_DMAMODE_WR_WORDSWAP        (1 << 11)
 #define MMC_DMAMODE_WRITE              (1 << 12)
 #define MMC_DMAMODE_ENABLE             (1 << 13)
 #define MMC_DMAMODE_TIMEOUTIRQ_EN      (1 << 14)
+
+#define MMC_DMASTAT1_RUNST             (1 << 12)
+#define MMC_DMASTAT1_TOUTDT            (1 << 13)
+
 /*
  *  IO_EINTx bits
  */
@@ -1001,6 +1017,7 @@ extern unsigned long _ttbstart;
 #define INTR_IRQ1_EXT0        INTR_EINT1_EXT0
 #define INTR_IRQ1_EXT2        INTR_EINT1_EXT2
 #define INTR_IRQ1_EXT7        INTR_EINT1_EXT7
+#define INTR_IRQ1_MMCSDMS0    INTR_EINT1_MMCSDMS0
 #define INTR_IRQ1_MTC0        INTR_EINT1_MTC0
 
 /*
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
+    semaphore_wait(&data_done, TIMEOUT_BLOCK);
+
+    dma_status = IO_MMC_SD_DMA_STATUS1;
+    if (dma_status & MMC_DMASTAT1_TOUTDT)
     {
-        long time = current_tick;
+        return -EC_DATA_TIMEOUT;
+    }
 
-        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 & (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);
+    }
+}