2 files changed, 37 insertions, 2 deletions

diff --git a/arch/arm/plat-omap/dma.c b/arch/arm/plat-omap/dma.c
index 420cef370b33..f5c5b8da9a87 100644
--- a/arch/arm/plat-omap/dma.c
+++ b/arch/arm/plat-omap/dma.c
@@ -30,6 +30,7 @@
 #include <linux/irq.h>
 #include <linux/io.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 
 #include <asm/system.h>
 #include <mach/hardware.h>
@@ -1024,8 +1025,39 @@ void omap_stop_dma(int lch)
                 dma_write(0, CICR(lch));
 
         l = dma_read(CCR(lch));
-        l &= ~OMAP_DMA_CCR_EN;
-        dma_write(l, CCR(lch));
+        /* OMAP3 Errata i541: sDMA FIFO draining does not finish */
+        if (cpu_is_omap34xx() && (l & OMAP_DMA_CCR_SEL_SRC_DST_SYNC)) {
+                int i = 0;
+                u32 sys_cf;
+
+                /* Configure No-Standby */
+                l = dma_read(OCP_SYSCONFIG);
+                sys_cf = l;
+                l &= ~DMA_SYSCONFIG_MIDLEMODE_MASK;
+                l |= DMA_SYSCONFIG_MIDLEMODE(DMA_IDLEMODE_NO_IDLE);
+                dma_write(l , OCP_SYSCONFIG);
+
+                l = dma_read(CCR(lch));
+                l &= ~OMAP_DMA_CCR_EN;
+                dma_write(l, CCR(lch));
+
+                /* Wait for sDMA FIFO drain */
+                l = dma_read(CCR(lch));
+                while (i < 100 && (l & (OMAP_DMA_CCR_RD_ACTIVE |
+                                        OMAP_DMA_CCR_WR_ACTIVE))) {
+                        udelay(5);
+                        i++;
+                        l = dma_read(CCR(lch));
+                }
+                if (i >= 100)
+                        printk(KERN_ERR "DMA drain did not complete on "
+                                        "lch %d\n", lch);
+                /* Restore OCP_SYSCONFIG */
+                dma_write(sys_cf, OCP_SYSCONFIG);
+        } else {
+                l &= ~OMAP_DMA_CCR_EN;
+                dma_write(l, CCR(lch));
+        }
 
         if (!omap_dma_in_1510_mode() && dma_chan[lch].next_lch != -1) {
                 int next_lch, cur_lch = lch;
diff --git a/arch/arm/plat-omap/include/plat/dma.h b/arch/arm/plat-omap/include/plat/dma.h
index 6f70f7cfe91d..0cce4ca83aa0 100644
--- a/arch/arm/plat-omap/include/plat/dma.h
+++ b/arch/arm/plat-omap/include/plat/dma.h
@@ -337,6 +337,9 @@
 #define OMAP2_DMA_MISALIGNED_ERR_IRQ    (1 << 11)
 
 #define OMAP_DMA_CCR_EN                 (1 << 7)
+#define OMAP_DMA_CCR_RD_ACTIVE          (1 << 9)
+#define OMAP_DMA_CCR_WR_ACTIVE          (1 << 10)
+#define OMAP_DMA_CCR_SEL_SRC_DST_SYNC   (1 << 24)
 #define OMAP_DMA_CCR_BUFFERING_DISABLE  (1 << 25)
 
 #define OMAP_DMA_DATA_TYPE_S8           0x00