2 files changed, 24 insertions, 2 deletions

diff --git a/drivers/mmc/host/mmci.c b/drivers/mmc/host/mmci.c
index 7dd3ccf5baf0..387ff14587b8 100644
--- a/drivers/mmc/host/mmci.c
+++ b/drivers/mmc/host/mmci.c
@@ -1127,6 +1127,12 @@ mmci_start_command(struct mmci_host *host, struct mmc_command *cmd, u32 c)
         writel(c, base + MMCICOMMAND);
 }
 
+static void mmci_stop_command(struct mmci_host *host)
+{
+        host->stop_abort.error = 0;
+        mmci_start_command(host, &host->stop_abort, 0);
+}
+
 static void
 mmci_data_irq(struct mmci_host *host, struct mmc_data *data,
               unsigned int status)
@@ -1196,10 +1202,16 @@ mmci_data_irq(struct mmci_host *host, struct mmc_data *data,
                         /* The error clause is handled above, success! */
                         data->bytes_xfered = data->blksz * data->blocks;
 
-                if (!data->stop || (host->mrq->sbc && !data->error))
+                if (!data->stop) {
+                        if (host->variant->cmdreg_stop && data->error)
+                                mmci_stop_command(host);
+                        else
+                                mmci_request_end(host, data->mrq);
+                } else if (host->mrq->sbc && !data->error) {
                         mmci_request_end(host, data->mrq);
-                else
+                } else {
                         mmci_start_command(host, data->stop, 0);
+                }
         }
 }
 
@@ -1298,6 +1310,10 @@ mmci_cmd_irq(struct mmci_host *host, struct mmc_command *cmd,
                         mmci_dma_error(host);
 
                         mmci_stop_data(host);
+                        if (host->variant->cmdreg_stop && cmd->error) {
+                                mmci_stop_command(host);
+                                return;
+                        }
                 }
                 mmci_request_end(host, host->mrq);
         } else if (sbc) {
@@ -1956,6 +1972,11 @@ static int mmci_probe(struct amba_device *dev,
                 mmc->max_busy_timeout = 0;
         }
 
+        /* Prepare a CMD12 - needed to clear the DPSM on some variants. */
+        host->stop_abort.opcode = MMC_STOP_TRANSMISSION;
+        host->stop_abort.arg = 0;
+        host->stop_abort.flags = MMC_RSP_R1B | MMC_CMD_AC;
+
         mmc->ops = &mmci_ops;
 
         /* We support these PM capabilities. */
diff --git a/drivers/mmc/host/mmci.h b/drivers/mmc/host/mmci.h
index 24229097d05c..14df81054438 100644
--- a/drivers/mmc/host/mmci.h
+++ b/drivers/mmc/host/mmci.h
@@ -377,6 +377,7 @@ struct mmci_host {
         void __iomem            *base;
         struct mmc_request      *mrq;
         struct mmc_command      *cmd;
+        struct mmc_command      stop_abort;
         struct mmc_data         *data;
         struct mmc_host         *mmc;
         struct clk              *clk;