MX28: Fix up the MMC driver DMA mode

index 4187a94..a637db3 100644
@@ -79,6 +79,7 @@ mxsmmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 	uint32_t *data_ptr;
 #else
 	uint32_t cache_data_count;
+	int dmach;
 #endif
 
 	debug("MMC%d: CMD%d\n", mmc->block_dev.dev, cmd->cmdidx);

@@ -201,6 +202,8 @@ mxsmmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 	timeout = MXSMMC_MAX_TIMEOUT;
 
 #ifdef CONFIG_MXS_MMC_DMA
+	writel(SSP_CTRL1_DMA_ENABLE, &ssp_regs->hw_ssp_ctrl1_set);
+
 	if (data_count % ARCH_DMA_MINALIGN)
 		cache_data_count = roundup(data_count, ARCH_DMA_MINALIGN);
 	else

@@ -222,8 +225,9 @@ mxsmmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 				(data_count << MXS_DMA_DESC_BYTES_OFFSET);
 
 
-	mxs_dma_desc_append(MXS_DMA_CHANNEL_AHB_APBH_SSP0, priv->desc);
-	if (mxs_dma_go(MXS_DMA_CHANNEL_AHB_APBH_SSP0)) {
+	dmach = MXS_DMA_CHANNEL_AHB_APBH_SSP0 + priv->id;
+	mxs_dma_desc_append(dmach, priv->desc);
+	if (mxs_dma_go(dmach)) {
 		printf("MMC%d: DMA transfer failed\n", mmc->block_dev.dev);
 		return COMM_ERR;
 	}

@@ -234,6 +238,8 @@ mxsmmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 			(uint32_t)(priv->desc->cmd.address + cache_data_count));
 	}
 #else
+	writel(SSP_CTRL1_DMA_ENABLE, &ssp_regs->hw_ssp_ctrl1_clr);
+
 	if (data->flags & MMC_DATA_READ) {
 		data_ptr = (uint32_t *)data->dest;
 		while (data_count && --timeout) {

index a637db3..1d1b2ac 100644
@@ -62,6 +62,85 @@ struct mxsmmc_priv {
 
 #define	MXSMMC_MAX_TIMEOUT	10000
 
+#ifndef CONFIG_MXS_MMC_DMA
+static int mxsmmc_send_cmd_pio(struct mxsmmc_priv *priv, struct mmc_data *data)
+{
+	struct mx28_ssp_regs *ssp_regs = priv->regs;
+	uint32_t *data_ptr;
+	int timeout = MXSMMC_MAX_TIMEOUT;
+	uint32_t reg;
+	uint32_t data_count = data->blocksize * data->blocks;
+
+	if (data->flags & MMC_DATA_READ) {
+		data_ptr = (uint32_t *)data->dest;
+		while (data_count && --timeout) {
+			reg = readl(&ssp_regs->hw_ssp_status);
+			if (!(reg & SSP_STATUS_FIFO_EMPTY)) {
+				*data_ptr++ = readl(&ssp_regs->hw_ssp_data);
+				data_count -= 4;
+				timeout = MXSMMC_MAX_TIMEOUT;
+			} else
+				udelay(1000);
+		}
+	} else {
+		data_ptr = (uint32_t *)data->src;
+		timeout *= 100;
+		while (data_count && --timeout) {
+			reg = readl(&ssp_regs->hw_ssp_status);
+			if (!(reg & SSP_STATUS_FIFO_FULL)) {
+				writel(*data_ptr++, &ssp_regs->hw_ssp_data);
+				data_count -= 4;
+				timeout = MXSMMC_MAX_TIMEOUT;
+			} else
+				udelay(1000);
+		}
+	}
+
+	return timeout ? 0 : COMM_ERR;
+}
+#else
+static int mxsmmc_send_cmd_dma(struct mxsmmc_priv *priv, struct mmc_data *data)
+{
+	uint32_t data_count = data->blocksize * data->blocks;
+	uint32_t cache_data_count;
+	int dmach;
+
+	if (data_count % ARCH_DMA_MINALIGN)
+		cache_data_count = roundup(data_count, ARCH_DMA_MINALIGN);
+	else
+		cache_data_count = data_count;
+
+	if (data->flags & MMC_DATA_READ) {
+		priv->desc->cmd.data = MXS_DMA_DESC_COMMAND_DMA_WRITE;
+		priv->desc->cmd.address = (dma_addr_t)data->dest;
+	} else {
+		priv->desc->cmd.data = MXS_DMA_DESC_COMMAND_DMA_READ;
+		priv->desc->cmd.address = (dma_addr_t)data->src;
+
+		/* Flush data to DRAM so DMA can pick them up */
+		flush_dcache_range((uint32_t)priv->desc->cmd.address,
+			(uint32_t)(priv->desc->cmd.address + cache_data_count));
+	}
+
+	priv->desc->cmd.data |= MXS_DMA_DESC_IRQ | MXS_DMA_DESC_DEC_SEM |
+				(data_count << MXS_DMA_DESC_BYTES_OFFSET);
+
+
+	dmach = MXS_DMA_CHANNEL_AHB_APBH_SSP0 + priv->id;
+	mxs_dma_desc_append(dmach, priv->desc);
+	if (mxs_dma_go(dmach))
+		return COMM_ERR;
+
+	/* The data arrived into DRAM, invalidate cache over them */
+	if (data->flags & MMC_DATA_READ) {
+		invalidate_dcache_range((uint32_t)priv->desc->cmd.address,
+			(uint32_t)(priv->desc->cmd.address + cache_data_count));
+	}
+
+	return 0;
+}
+#endif
+
 /*
  * Sends a command out on the bus.  Takes the mmc pointer,
  * a command pointer, and an optional data pointer.

@@ -73,14 +152,8 @@ mxsmmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 	struct mx28_ssp_regs *ssp_regs = priv->regs;
 	uint32_t reg;
 	int timeout;
-	uint32_t data_count;
 	uint32_t ctrl0;
-#ifndef CONFIG_MXS_MMC_DMA
-	uint32_t *data_ptr;
-#else
-	uint32_t cache_data_count;
-	int dmach;
-#endif
+	int ret;
 
 	debug("MMC%d: CMD%d\n", mmc->block_dev.dev, cmd->cmdidx);
 

@@ -198,77 +271,22 @@ mxsmmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 	if (!data)
 		return 0;
 
-	data_count = data->blocksize * data->blocks;
-	timeout = MXSMMC_MAX_TIMEOUT;
-
 #ifdef CONFIG_MXS_MMC_DMA
 	writel(SSP_CTRL1_DMA_ENABLE, &ssp_regs->hw_ssp_ctrl1_set);
 
-	if (data_count % ARCH_DMA_MINALIGN)
-		cache_data_count = roundup(data_count, ARCH_DMA_MINALIGN);
-	else
-		cache_data_count = data_count;
-
-	if (data->flags & MMC_DATA_READ) {
-		priv->desc->cmd.data = MXS_DMA_DESC_COMMAND_DMA_WRITE;
-		priv->desc->cmd.address = (dma_addr_t)data->dest;
-	} else {
-		priv->desc->cmd.data = MXS_DMA_DESC_COMMAND_DMA_READ;
-		priv->desc->cmd.address = (dma_addr_t)data->src;
-
-		/* Flush data to DRAM so DMA can pick them up */
-		flush_dcache_range((uint32_t)priv->desc->cmd.address,
-			(uint32_t)(priv->desc->cmd.address + cache_data_count));
-	}
-
-	priv->desc->cmd.data |= MXS_DMA_DESC_IRQ | MXS_DMA_DESC_DEC_SEM |
-				(data_count << MXS_DMA_DESC_BYTES_OFFSET);
-
-
-	dmach = MXS_DMA_CHANNEL_AHB_APBH_SSP0 + priv->id;
-	mxs_dma_desc_append(dmach, priv->desc);
-	if (mxs_dma_go(dmach)) {
+	ret = mxsmmc_send_cmd_dma(priv, data);
+	if (ret) {
 		printf("MMC%d: DMA transfer failed\n", mmc->block_dev.dev);
-		return COMM_ERR;
-	}
-
-	/* The data arrived into DRAM, invalidate cache over them */
-	if (data->flags & MMC_DATA_READ) {
-		invalidate_dcache_range((uint32_t)priv->desc->cmd.address,
-			(uint32_t)(priv->desc->cmd.address + cache_data_count));
+		return ret;
 	}
 #else
 	writel(SSP_CTRL1_DMA_ENABLE, &ssp_regs->hw_ssp_ctrl1_clr);
 
-	if (data->flags & MMC_DATA_READ) {
-		data_ptr = (uint32_t *)data->dest;
-		while (data_count && --timeout) {
-			reg = readl(&ssp_regs->hw_ssp_status);
-			if (!(reg & SSP_STATUS_FIFO_EMPTY)) {
-				*data_ptr++ = readl(&ssp_regs->hw_ssp_data);
-				data_count -= 4;
-				timeout = MXSMMC_MAX_TIMEOUT;
-			} else
-				udelay(1000);
-		}
-	} else {
-		data_ptr = (uint32_t *)data->src;
-		timeout *= 100;
-		while (data_count && --timeout) {
-			reg = readl(&ssp_regs->hw_ssp_status);
-			if (!(reg & SSP_STATUS_FIFO_FULL)) {
-				writel(*data_ptr++, &ssp_regs->hw_ssp_data);
-				data_count -= 4;
-				timeout = MXSMMC_MAX_TIMEOUT;
-			} else
-				udelay(1000);
-		}
-	}
-
-	if (!timeout) {
+	ret = mxsmmc_send_cmd_pio(priv, data);
+	if (ret) {
 		printf("MMC%d: Data timeout with command %d (status 0x%08x)!\n",
 			mmc->block_dev.dev, cmd->cmdidx, reg);
-		return COMM_ERR;
+		return ret;
 	}
 #endif
 

index 1d1b2ac..52f16f5 100644
@@ -43,13 +43,6 @@
 #include <asm/arch/sys_proto.h>
 #include <asm/arch/dma.h>
 
-/*
- * CONFIG_MXS_MMC_DMA: This feature is highly experimental and has no
- *                     performance benefit unless you operate the platform with
- *                     data cache enabled. This is disabled by default, enable
- *                     only if you know what you're doing.
- */
-
 struct mxsmmc_priv {
 	int			id;
 	struct mx28_ssp_regs	*regs;

@@ -61,8 +54,8 @@ struct mxsmmc_priv {
 };
 
 #define	MXSMMC_MAX_TIMEOUT	10000
+#define MXSMMC_SMALL_TRANSFER	512
 
-#ifndef CONFIG_MXS_MMC_DMA
 static int mxsmmc_send_cmd_pio(struct mxsmmc_priv *priv, struct mmc_data *data)
 {
 	struct mx28_ssp_regs *ssp_regs = priv->regs;

@@ -98,7 +91,7 @@ static int mxsmmc_send_cmd_pio(struct mxsmmc_priv *priv, struct mmc_data *data)
 
 	return timeout ? 0 : COMM_ERR;
 }
-#else
+
 static int mxsmmc_send_cmd_dma(struct mxsmmc_priv *priv, struct mmc_data *data)
 {
 	uint32_t data_count = data->blocksize * data->blocks;

@@ -139,7 +132,6 @@ static int mxsmmc_send_cmd_dma(struct mxsmmc_priv *priv, struct mmc_data *data)
 
 	return 0;
 }
-#endif
 
 /*
  * Sends a command out on the bus.  Takes the mmc pointer,

@@ -271,24 +263,26 @@ mxsmmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 	if (!data)
 		return 0;
 
-#ifdef CONFIG_MXS_MMC_DMA
-	writel(SSP_CTRL1_DMA_ENABLE, &ssp_regs->hw_ssp_ctrl1_set);
-
-	ret = mxsmmc_send_cmd_dma(priv, data);
-	if (ret) {
-		printf("MMC%d: DMA transfer failed\n", mmc->block_dev.dev);
-		return ret;
-	}
-#else
-	writel(SSP_CTRL1_DMA_ENABLE, &ssp_regs->hw_ssp_ctrl1_clr);
+	if (data->blocksize * data->blocks < MXSMMC_SMALL_TRANSFER) {
+		writel(SSP_CTRL1_DMA_ENABLE, &ssp_regs->hw_ssp_ctrl1_set);
 
-	ret = mxsmmc_send_cmd_pio(priv, data);
-	if (ret) {
-		printf("MMC%d: Data timeout with command %d (status 0x%08x)!\n",
-			mmc->block_dev.dev, cmd->cmdidx, reg);
-		return ret;
+		ret = mxsmmc_send_cmd_dma(priv, data);
+		if (ret) {
+			printf("MMC%d: DMA transfer failed\n",
+				mmc->block_dev.dev);
+			return ret;
+		}
+	} else {
+		writel(SSP_CTRL1_DMA_ENABLE, &ssp_regs->hw_ssp_ctrl1_clr);
+
+		ret = mxsmmc_send_cmd_pio(priv, data);
+		if (ret) {
+			printf("MMC%d: Data timeout with command %d "
+				"(status 0x%08x)!\n",
+				mmc->block_dev.dev, cmd->cmdidx, reg);
+			return ret;
+		}
 	}
-#endif
 
 	/* Check data errors */
 	reg = readl(&ssp_regs->hw_ssp_status);

index 52f16f5..d196a50 100644
@@ -97,6 +97,10 @@ static int mxsmmc_send_cmd_dma(struct mxsmmc_priv *priv, struct mmc_data *data)
 	uint32_t data_count = data->blocksize * data->blocks;
 	uint32_t cache_data_count;
 	int dmach;
+	struct mxs_dma_desc *desc = priv->desc;
+
+	memset(desc, 0, sizeof(struct mxs_dma_desc));
+	desc->address = (dma_addr_t)desc;
 
 	if (data_count % ARCH_DMA_MINALIGN)
 		cache_data_count = roundup(data_count, ARCH_DMA_MINALIGN);

@@ -118,7 +122,6 @@ static int mxsmmc_send_cmd_dma(struct mxsmmc_priv *priv, struct mmc_data *data)
 	priv->desc->cmd.data |= MXS_DMA_DESC_IRQ | MXS_DMA_DESC_DEC_SEM |
 				(data_count << MXS_DMA_DESC_BYTES_OFFSET);
 
-
 	dmach = MXS_DMA_CHANNEL_AHB_APBH_SSP0 + priv->id;
 	mxs_dma_desc_append(dmach, priv->desc);
 	if (mxs_dma_go(dmach))

@@ -183,6 +186,11 @@ mxsmmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 	if (cmd->resp_type & MMC_RSP_136)	/* It's a 136 bits response */
 		ctrl0 |= SSP_CTRL0_LONG_RESP;
 
+	if (data && (data->blocksize * data->blocks < MXSMMC_SMALL_TRANSFER))
+		writel(SSP_CTRL1_DMA_ENABLE, &ssp_regs->hw_ssp_ctrl1_clr);
+	else
+		writel(SSP_CTRL1_DMA_ENABLE, &ssp_regs->hw_ssp_ctrl1_set);
+
 	/* Command index */
 	reg = readl(&ssp_regs->hw_ssp_cmd0);
 	reg &= ~(SSP_CMD0_CMD_MASK | SSP_CMD0_APPEND_8CYC);

@@ -264,17 +272,6 @@ mxsmmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 		return 0;
 
 	if (data->blocksize * data->blocks < MXSMMC_SMALL_TRANSFER) {
-		writel(SSP_CTRL1_DMA_ENABLE, &ssp_regs->hw_ssp_ctrl1_set);
-
-		ret = mxsmmc_send_cmd_dma(priv, data);
-		if (ret) {
-			printf("MMC%d: DMA transfer failed\n",
-				mmc->block_dev.dev);
-			return ret;
-		}
-	} else {
-		writel(SSP_CTRL1_DMA_ENABLE, &ssp_regs->hw_ssp_ctrl1_clr);
-
 		ret = mxsmmc_send_cmd_pio(priv, data);
 		if (ret) {
 			printf("MMC%d: Data timeout with command %d "

@@ -282,6 +279,13 @@ mxsmmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 				mmc->block_dev.dev, cmd->cmdidx, reg);
 			return ret;
 		}
+	} else {
+		ret = mxsmmc_send_cmd_dma(priv, data);
+		if (ret) {
+			printf("MMC%d: DMA transfer failed\n",
+				mmc->block_dev.dev);
+			return ret;
+		}
 	}
 
 	/* Check data errors */

@@ -336,9 +340,9 @@ static int mxsmmc_init(struct mmc *mmc)
 
 	/* 8 bits word length in MMC mode */
 	clrsetbits_le32(&ssp_regs->hw_ssp_ctrl1,
-		SSP_CTRL1_SSP_MODE_MASK | SSP_CTRL1_WORD_LENGTH_MASK,
-		SSP_CTRL1_SSP_MODE_SD_MMC | SSP_CTRL1_WORD_LENGTH_EIGHT_BITS |
-		SSP_CTRL1_DMA_ENABLE);
+		SSP_CTRL1_SSP_MODE_MASK | SSP_CTRL1_WORD_LENGTH_MASK |
+		SSP_CTRL1_DMA_ENABLE,
+		SSP_CTRL1_SSP_MODE_SD_MMC | SSP_CTRL1_WORD_LENGTH_EIGHT_BITS);
 
 	/* Set initial bit clock 400 KHz */
 	mx28_set_ssp_busclock(priv->id, 400);