1 files changed, 20 insertions, 22 deletions

diff --git a/drivers/mmc/gen_atmel_mci.c b/drivers/mmc/gen_atmel_mci.c
index 22154d13d7..1c108b5470 100644
--- a/drivers/mmc/gen_atmel_mci.c
+++ b/drivers/mmc/gen_atmel_mci.c
@@ -74,6 +74,20 @@ static void dump_cmd(u32 cmdr, u32 arg, u32 status, const char* msg)
 	      cmdr, cmdr & 0x3F, arg, status, msg);
 }
 
+static inline void mci_set_blklen(atmel_mci_t *mci, int blklen)
+{
+	unsigned int version = atmel_mci_get_version(mci);
+
+	blklen &= 0xfffc;
+
+	/* MCI IP version >= 0x200 has blkr */
+	if (version >= 0x200)
+		writel(MMCI_BFINS(BLKLEN, blklen, readl(&mci->blkr)),
+		       &mci->blkr);
+	else
+		writel(MMCI_BFINS(BLKLEN, blklen, readl(&mci->mr)), &mci->mr);
+}
+
 /* Setup for MCI Clock and Block Size */
 #ifdef CONFIG_DM_MMC
 static void mci_set_mode(struct udevice *dev, u32 hz, u32 blklen)
@@ -124,7 +138,6 @@ static void mci_set_mode(struct mmc *mmc, u32 hz, u32 blklen)
 		priv->curr_clk = bus_hz / (clkdiv * 2 + clkodd + 2);
 	else
 		priv->curr_clk = (bus_hz / (clkdiv + 1)) / 2;
-	blklen &= 0xfffc;
 
 	mr = MMCI_BF(CLKDIV, clkdiv);
 
@@ -138,14 +151,10 @@ static void mci_set_mode(struct mmc *mmc, u32 hz, u32 blklen)
 	 */
 	if (version >= 0x500)
 		mr |= MMCI_BF(CLKODD, clkodd);
-	else
-		mr |= MMCI_BF(BLKLEN, blklen);
 
 	writel(mr, &mci->mr);
 
-	/* MCI IP version >= 0x200 has blkr */
-	if (version >= 0x200)
-		writel(MMCI_BF(BLKLEN, blklen), &mci->blkr);
+	mci_set_blklen(mci, blklen);
 
 	if (mmc->card_caps & mmc->cfg->host_caps & MMC_MODE_HS)
 		writel(MMCI_BIT(HSMODE), &mci->cfg);
@@ -236,7 +245,6 @@ static int atmel_mci_send_cmd(struct udevice *dev, struct mmc_cmd *cmd,
 {
 	struct atmel_mci_plat *plat = dev_get_platdata(dev);
 	struct atmel_mci_priv *priv = dev_get_priv(dev);
-	struct mmc *mmc = mmc_get_mmc_dev(dev);
 	atmel_mci_t *mci = plat->mci;
 #else
 static int
@@ -257,11 +265,13 @@ mci_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 	/* Figure out the transfer arguments */
 	cmdr = mci_encode_cmd(cmd, data, &error_flags);
 
+	mci_set_blklen(mci, data->blocksize);
+
 	/* For multi blocks read/write, set the block register */
 	if ((cmd->cmdidx == MMC_CMD_READ_MULTIPLE_BLOCK)
 			|| (cmd->cmdidx == MMC_CMD_WRITE_MULTIPLE_BLOCK))
-		writel(data->blocks | MMCI_BF(BLKLEN, mmc->read_bl_len),
-			&mci->blkr);
+		writel(data->blocks | MMCI_BF(BLKLEN, data->blocksize),
+		       &mci->blkr);
 
 	/* Send the command */
 	writel(cmd->cmdarg, &mci->argr);
@@ -295,17 +305,15 @@ mci_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 	if (data) {
 		u32 word_count, block_count;
 		u32* ioptr;
-		u32 sys_blocksize, dummy, i;
+		u32 i;
 		u32 (*mci_data_op)
 			(atmel_mci_t *mci, u32* data, u32 error_flags);
 
 		if (data->flags & MMC_DATA_READ) {
 			mci_data_op = mci_data_read;
-			sys_blocksize = mmc->read_bl_len;
 			ioptr = (u32*)data->dest;
 		} else {
 			mci_data_op = mci_data_write;
-			sys_blocksize = mmc->write_bl_len;
 			ioptr = (u32*)data->src;
 		}
 
@@ -328,16 +336,6 @@ mci_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 					     1, cnt, 0);
 			}
 #endif
-#ifdef DEBUG
-			if (!status && word_count < (sys_blocksize / 4))
-				printf("filling rest of block...\n");
-#endif
-			/* fill the rest of a full block */
-			while (!status && word_count < (sys_blocksize / 4)) {
-				status = mci_data_op(mci, &dummy,
-					error_flags);
-				word_count++;
-			}
 			if (status) {
 				dump_cmd(cmdr, cmd->cmdarg, status,
 					"Data Transfer Failed");