diff --git a/arch/arm/plat-omap/dma.c b/arch/arm/plat-omap/dma.c
index 420cef370b33cdeed1fb02bf353c2be9b8d45073..f5c5b8da9a87f194683afa8b51ebecb39e53f302 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 6f70f7cfe91df97c77430f64aa4cbf0a5108ddfc..0cce4ca83aa0c70217fadd03ea07856665436ef0 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