diff --git a/drivers/mmc/host/sdhci-of-esdhc.c b/drivers/mmc/host/sdhci-of-esdhc.c
index 15039e2..adfaadd 100644
--- a/drivers/mmc/host/sdhci-of-esdhc.c
+++ b/drivers/mmc/host/sdhci-of-esdhc.c
@@ -21,9 +21,13 @@
 #include <linux/mmc/host.h>
 #include "sdhci-pltfm.h"
 #include "sdhci-esdhc.h"
+#include <asm/mpc85xx.h>
 
 #define VENDOR_V_22	0x12
 #define VENDOR_V_23	0x13
+
+static u32 svr;
+
 static u32 esdhc_readl(struct sdhci_host *host, int reg)
 {
 	u32 ret;
@@ -142,6 +146,26 @@ static void esdhc_writeb(struct sdhci_host *host, u8 val, int reg)
 	sdhci_be32bs_writeb(host, val, reg);
 }
 
+static void esdhc_reset(struct sdhci_host *host, u8 mask)
+{
+	u32 ier;
+	u32 uninitialized_var(isav);
+
+	if (host->quirks & SDHCI_QUIRK_RESTORE_IRQS_AFTER_RESET)
+		isav = esdhc_readl(host, SDHCI_INT_ENABLE);
+
+	esdhc_writeb(host, mask, SDHCI_SOFTWARE_RESET);
+	mdelay(100);
+
+	if (host->quirks & SDHCI_QUIRK_RESTORE_IRQS_AFTER_RESET) {
+		ier = esdhc_readl(host, SDHCI_INT_ENABLE);
+		ier &= ~SDHCI_INT_ALL_MASK;
+		ier |= isav;
+		esdhc_writel(host, ier, SDHCI_INT_ENABLE);
+		esdhc_writel(host, ier, SDHCI_SIGNAL_ENABLE);
+	}
+}
+
 /*
  * For Abort or Suspend after Stop at Block Gap, ignore the ADMA
  * error(IRQSTAT[ADMAE]) if both Transfer Complete(IRQSTAT[TC])
@@ -156,25 +180,92 @@ static void esdhci_of_adma_workaround(struct sdhci_host *host, u32 intmask)
 	dma_addr_t dmastart;
 	dma_addr_t dmanow;
 
-	tmp = in_be32(host->ioaddr + SDHCI_SLOT_INT_STATUS);
+	tmp = esdhc_readl(host, SDHCI_SLOT_INT_STATUS);
 	tmp = (tmp & SDHCI_VENDOR_VER_MASK) >> SDHCI_VENDOR_VER_SHIFT;
 
 	applicable = (intmask & SDHCI_INT_DATA_END) &&
 		(intmask & SDHCI_INT_BLK_GAP) &&
 		(tmp == VENDOR_V_23);
-	if (!applicable)
+	if (applicable) {
+
+		esdhc_reset(host, SDHCI_RESET_DATA);
+		host->data->error = 0;
+		dmastart = sg_dma_address(host->data->sg);
+		dmanow = dmastart + host->data->bytes_xfered;
+
+		/* Force update to the next DMA block boundary. */
+		dmanow = (dmanow & ~(SDHCI_DEFAULT_BOUNDARY_SIZE - 1)) +
+			SDHCI_DEFAULT_BOUNDARY_SIZE;
+		host->data->bytes_xfered = dmanow - dmastart;
+		esdhc_writel(host, dmanow, SDHCI_DMA_ADDRESS);
+
 		return;
+	}
 
-	host->data->error = 0;
-	dmastart = sg_dma_address(host->data->sg);
-	dmanow = dmastart + host->data->bytes_xfered;
 	/*
-	 * Force update to the next DMA block boundary.
+	 * Check for A-004388: eSDHC DMA might not stop if error
+	 * occurs on system transaction
+	 * Impact list:
+	 * T4240-R1.0 B4860-R1.0 P1010-R1.0
+	 * P3041-R1.0-R2.0-R1.1 P2041-R1.0-R1.1-R2.0
+	 * P5040-R2.0
 	 */
-	dmanow = (dmanow & ~(SDHCI_DEFAULT_BOUNDARY_SIZE - 1)) +
-		SDHCI_DEFAULT_BOUNDARY_SIZE;
-	host->data->bytes_xfered = dmanow - dmastart;
-	sdhci_writel(host, dmanow, SDHCI_DMA_ADDRESS);
+	if (!(((SVR_SOC_VER(svr) == SVR_T4240) && (SVR_REV(svr) == 0x10)) ||
+		((SVR_SOC_VER(svr) == SVR_B4860) && (SVR_REV(svr) == 0x10)) ||
+		((SVR_SOC_VER(svr) == SVR_P1010) && (SVR_REV(svr) == 0x10)) ||
+		((SVR_SOC_VER(svr) == SVR_P3041) && (SVR_REV(svr) <= 0x20)) ||
+		((SVR_SOC_VER(svr) == SVR_P2041) && (SVR_REV(svr) <= 0x20)) ||
+		((SVR_SOC_VER(svr) == SVR_P5040) && SVR_REV(svr) == 0x20)))
+		return;
+
+	esdhc_reset(host, SDHCI_RESET_DATA);
+
+	if (host->flags & SDHCI_USE_ADMA) {
+		u32 mod, i, offset;
+		u8 *desc;
+		dma_addr_t addr;
+		struct scatterlist *sg;
+		__le32 *dataddr;
+		__le32 *cmdlen;
+
+		/*
+		 * If block count was enabled, in case read transfer there
+		 * is no data was corrupted
+		 */
+		mod = esdhc_readl(host, SDHCI_TRANSFER_MODE);
+		if ((mod & SDHCI_TRNS_BLK_CNT_EN) &&
+			(host->data->flags & MMC_DATA_READ))
+			host->data->error = 0;
+
+		BUG_ON(!host->data);
+		desc = host->adma_desc;
+		for_each_sg(host->data->sg, sg, host->sg_count, i) {
+			addr = sg_dma_address(sg);
+			offset = (4 - (addr & 0x3)) & 0x3;
+			if (offset)
+				desc += 8;
+			desc += 8;
+		}
+
+		/*
+		 * Add an extra zero descriptor next to the
+		 * terminating descriptor.
+		 */
+		desc += 8;
+		WARN_ON((desc - host->adma_desc) > (128 * 2 + 1) * 4);
+
+		dataddr = (__le32 __force *)(desc + 4);
+		cmdlen = (__le32 __force *)desc;
+
+		cmdlen[0] = cpu_to_le32(0);
+		dataddr[0] = cpu_to_le32(0);
+	}
+
+	if ((host->flags & SDHCI_USE_SDMA) &&
+		(host->data->flags & MMC_DATA_READ))
+		host->data->error = 0;
+
+	return;
 }
 
 static int esdhc_of_enable_dma(struct sdhci_host *host)
@@ -299,6 +390,7 @@ static int sdhci_esdhc_probe(struct platform_device *pdev)
 	struct device_node *np;
 	int ret;
 
+	svr = mfspr(SPRN_SVR);
 	host = sdhci_pltfm_init(pdev, &sdhci_esdhc_pdata, 0);
 	if (IS_ERR(host))
 		return PTR_ERR(host);
