1 files changed, 36 insertions, 27 deletions

diff --git a/drivers/mmc/fsl_esdhc.c b/drivers/mmc/fsl_esdhc.c
index 5863774..00a454f 100644
--- a/drivers/mmc/fsl_esdhc.c
+++ b/drivers/mmc/fsl_esdhc.c
@@ -265,6 +265,7 @@ static void check_and_invalidate_dcache_range
 static int
 esdhc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 {
+	int	err = 0;
 	uint	xfertyp;
 	uint	irqstat;
 	struct fsl_esdhc_cfg *cfg = mmc->priv;
@@ -296,8 +297,6 @@ esdhc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 
 	/* Set up for a data transfer if we have one */
 	if (data) {
-		int err;
-
 		err = esdhc_setup_data(mmc, data);
 		if(err)
 			return err;
@@ -325,27 +324,15 @@ esdhc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 
 	irqstat = esdhc_read32(&regs->irqstat);
 
-	/* Reset CMD and DATA portions on error */
-	if (irqstat & (CMD_ERR | IRQSTAT_CTOE)) {
-		esdhc_write32(&regs->sysctl, esdhc_read32(&regs->sysctl) |
-			      SYSCTL_RSTC);
-		while (esdhc_read32(&regs->sysctl) & SYSCTL_RSTC)
-			;
-
-		if (data) {
-			esdhc_write32(&regs->sysctl,
-				      esdhc_read32(&regs->sysctl) |
-				      SYSCTL_RSTD);
-			while ((esdhc_read32(&regs->sysctl) & SYSCTL_RSTD))
-				;
-		}
+	if (irqstat & CMD_ERR) {
+		err = COMM_ERR;
+		goto out;
 	}
 
-	if (irqstat & CMD_ERR)
-		return COMM_ERR;
-
-	if (irqstat & IRQSTAT_CTOE)
-		return TIMEOUT;
+	if (irqstat & IRQSTAT_CTOE) {
+		err = TIMEOUT;
+		goto out;
+	}
 
 	/* Workaround for ESDHC errata ENGcm03648 */
 	if (!data && (cmd->resp_type & MMC_RSP_BUSY)) {
@@ -360,7 +347,8 @@ esdhc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 
 		if (timeout <= 0) {
 			printf("Timeout waiting for DAT0 to go high!\n");
-			return TIMEOUT;
+			err = TIMEOUT;
+			goto out;
 		}
 	}
 
@@ -387,20 +375,41 @@ esdhc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 		do {
 			irqstat = esdhc_read32(&regs->irqstat);
 
-			if (irqstat & IRQSTAT_DTOE)
-				return TIMEOUT;
+			if (irqstat & IRQSTAT_DTOE) {
+				err = TIMEOUT;
+				goto out;
+			}
 
-			if (irqstat & DATA_ERR)
-				return COMM_ERR;
+			if (irqstat & DATA_ERR) {
+				err = COMM_ERR;
+				goto out;
+			}
 		} while ((irqstat & DATA_COMPLETE) != DATA_COMPLETE);
 #endif
 		if (data->flags & MMC_DATA_READ)
 			check_and_invalidate_dcache_range(cmd, data);
 	}
 
+out:
+	/* Reset CMD and DATA portions on error */
+	if (err) {
+		esdhc_write32(&regs->sysctl, esdhc_read32(&regs->sysctl) |
+			      SYSCTL_RSTC);
+		while (esdhc_read32(&regs->sysctl) & SYSCTL_RSTC)
+			;
+
+		if (data) {
+			esdhc_write32(&regs->sysctl,
+				      esdhc_read32(&regs->sysctl) |
+				      SYSCTL_RSTD);
+			while ((esdhc_read32(&regs->sysctl) & SYSCTL_RSTD))
+				;
+		}
+	}
+
 	esdhc_write32(&regs->irqstat, -1);
 
-	return 0;
+	return err;
 }
 
 static void set_sysctl(struct mmc *mmc, uint clock)