1 files changed, 47 insertions, 1 deletions

diff --git a/firmware/target/arm/imx233/dma-imx233.c b/firmware/target/arm/imx233/dma-imx233.c
index d75c334aeb..8a42bd12da 100644
--- a/firmware/target/arm/imx233/dma-imx233.c
+++ b/firmware/target/arm/imx233/dma-imx233.c
@@ -106,8 +106,54 @@ bool imx233_dma_is_channel_error_irq(unsigned chan)
             HW_APBH_CTRL2__CHx_ERROR_IRQ(APB_GET_DMA_CHANNEL(chan)));
 }
 
+/* Commit and/or discard all DMA descriptors and buffers pointed by them,
+ * handle circular lists */
+static void imx233_dma_commit_and_discard(struct apb_dma_command_t *cmd)
+{
+    /* We handle circular descriptors by using unused bits:
+     * bits 8-11 are not used by the hardware so we first go through the whole
+     * list and mark them all a special value at the same time we commit buffers
+     * and then we go through the list another time to clear the mark and
+     * commit the descriptors */
+    struct apb_dma_command_t *cur = cmd;
+    
+    while((cur->cmd & HW_APB_CHx_CMD__UNUSED_BM) != HW_APB_CHx_CMD__UNUSED_MAGIC)
+    {
+        cur->cmd = (cur->cmd & ~HW_APB_CHx_CMD__UNUSED_BM) | HW_APB_CHx_CMD__UNUSED_MAGIC;
+        int op = cur->cmd & HW_APB_CHx_CMD__COMMAND_BM;
+        int sz = (cur->cmd & HW_APB_CHx_CMD__XFER_COUNT_BM) >> HW_APB_CHx_CMD__XFER_COUNT_BP;
+        /* device > host: discard */
+        if(op == HW_APB_CHx_CMD__COMMAND__WRITE)
+            discard_dcache_range(cur->buffer, sz);
+        /* host > device: commit and discard */
+        else if(op == HW_APB_CHx_CMD__COMMAND__READ)
+            commit_discard_dcache_range(cur->buffer, sz);
+        /* chain ? */
+        if(cur->cmd & HW_APB_CHx_CMD__CHAIN)
+            cur = cur->next;
+        else
+            break;
+    }
+
+    cur = cmd;
+    while((cur->cmd & HW_APB_CHx_CMD__UNUSED_BM) != 0)
+    {
+        cur->cmd = cur->cmd & ~HW_APB_CHx_CMD__UNUSED_BM;
+        int sz = (cur->cmd & HW_APB_CHx_CMD__CMDWORDS_BM) >> HW_APB_CHx_CMD__CMDWORDS_BP;
+        /* commit descriptor (don't discard since we access it after) */
+        commit_dcache_range(cur,
+            sizeof(struct apb_dma_command_t) + sizeof(uint32_t) * sz);
+        /* chain ? */
+        if(cur->cmd & HW_APB_CHx_CMD__CHAIN)
+            cur = cur->next;
+        else
+            break;
+    }
+}
+
 void imx233_dma_start_command(unsigned chan, struct apb_dma_command_t *cmd)
 {
+    imx233_dma_commit_and_discard(cmd);
     if(APB_IS_APBX_CHANNEL(chan))
     {
         HW_APBX_CHx_NXTCMDAR(APB_GET_DMA_CHANNEL(chan)) = (uint32_t)cmd;
@@ -129,5 +175,5 @@ void imx233_dma_wait_completion(unsigned chan)
         sema = &HW_APBH_CHx_SEMA(APB_GET_DMA_CHANNEL(chan));
 
     while(*sema & HW_APB_CHx_SEMA__PHORE_BM)
-        ;
+        yield();
 }