diff --git a/common/spl/spl_mmc.c b/common/spl/spl_mmc.c
index 7bae16beba..c2e596be69 100644
--- a/common/spl/spl_mmc.c
+++ b/common/spl/spl_mmc.c
@@ -172,11 +172,24 @@ void spl_mmc_load_image(void)
 		err = mmc_load_image_raw_sector(mmc,
 			CONFIG_SYS_MMCSD_RAW_MODE_U_BOOT_SECTOR);
 #endif
-	} else {
-#ifdef CONFIG_SPL_LIBCOMMON_SUPPORT
-		puts("spl: wrong MMC boot mode\n");
+	}
+
+	switch(boot_mode){
+		case MMCSD_MODE_RAW:
+#if defined(CONFIG_SPL_FAT_SUPPORT) || defined(CONFIG_SPL_EXT_SUPPORT)
+		case MMCSD_MODE_FS:
 #endif
-		hang();
+#ifdef CONFIG_SUPPORT_EMMC_BOOT
+		case MMCSD_MODE_EMMCBOOT:
+#endif
+			/* Boot mode is ok. Nothing to do. */
+			break;
+		case MMCSD_MODE_UNDEFINED:
+		default:
+#ifdef CONFIG_SPL_LIBCOMMON_SUPPORT
+			puts("spl: wrong MMC boot mode\n");
+#endif
+			hang();
 	}
 
 	if (err)
diff --git a/drivers/mmc/mvebu_mmc.c b/drivers/mmc/mvebu_mmc.c
index 9f98c3f37c..8ca09042d8 100644
--- a/drivers/mmc/mvebu_mmc.c
+++ b/drivers/mmc/mvebu_mmc.c
@@ -1,7 +1,7 @@
 /*
  * Marvell MMC/SD/SDIO driver
  *
- * (C) Copyright 2012
+ * (C) Copyright 2012-2014
  * Marvell Semiconductor <www.marvell.com>
  * Written-by: Maen Suleiman, Gerald Kerma
  *
@@ -23,6 +23,8 @@ DECLARE_GLOBAL_DATA_PTR;
 
 #define MVEBU_TARGET_DRAM 0
 
+#define TIMEOUT_DELAY	5*CONFIG_SYS_HZ		/* wait 5 seconds */
+
 static void mvebu_mmc_write(u32 offs, u32 val)
 {
 	writel(val, CONFIG_SYS_MMC_BASE + (offs));
@@ -63,37 +65,47 @@ static int mvebu_mmc_setup_data(struct mmc_data *data)
 static int mvebu_mmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd,
 			      struct mmc_data *data)
 {
-	int timeout = 10;
+	ulong start;
 	ushort waittype = 0;
 	ushort resptype = 0;
 	ushort xfertype = 0;
 	ushort resp_indx = 0;
 
-	debug("cmdidx [0x%x] resp_type[0x%x] cmdarg[0x%x]\n",
-	      cmd->cmdidx, cmd->resp_type, cmd->cmdarg);
-
-	udelay(10*1000);
+	debug("%s: cmdidx [0x%x] resp_type[0x%x] cmdarg[0x%x]\n",
+	      DRIVER_NAME, cmd->cmdidx, cmd->resp_type, cmd->cmdarg);
 
 	debug("%s: cmd %d (hw state 0x%04x)\n", DRIVER_NAME,
 	      cmd->cmdidx, mvebu_mmc_read(SDIO_HW_STATE));
 
-	/* Checking if card is busy */
-	while ((mvebu_mmc_read(SDIO_HW_STATE) & CARD_BUSY)) {
-		if (timeout == 0) {
-			printf("%s: card busy!\n", DRIVER_NAME);
-			return -1;
-		}
-		timeout--;
-		udelay(1000);
+	/*
+	 * Hardware weirdness.  The FIFO_EMPTY bit of the HW_STATE
+	 * register is sometimes not set before a while when some
+	 * "unusual" data block sizes are used (such as with the SWITCH
+	 * command), even despite the fact that the XFER_DONE interrupt
+	 * was raised.  And if another data transfer starts before
+	 * this bit comes to good sense (which eventually happens by
+	 * itself) then the new transfer simply fails with a timeout.
+	 */
+	if (!(mvebu_mmc_read(SDIO_HW_STATE) & CMD_FIFO_EMPTY)) {
+		ushort hw_state, count = 0;
+
+		start = get_timer(0);
+		do {
+			hw_state = mvebu_mmc_read(SDIO_HW_STATE);
+			if ((get_timer(0) - start) > TIMEOUT_DELAY) {
+				printf("%s : FIFO_EMPTY bit missing\n",
+				       DRIVER_NAME);
+				break;
+			}
+			count++;
+		} while (!(hw_state & CMD_FIFO_EMPTY));
+		debug("%s *** wait for FIFO_EMPTY bit (hw=0x%04x, count=%d, jiffies=%ld)\n",
+		      DRIVER_NAME, hw_state, count, (get_timer(0) - (start)));
 	}
 
-	/* Set up for a data transfer if we have one */
-	if (data) {
-		int err = mvebu_mmc_setup_data(data);
-
-		if (err)
-			return err;
-	}
+	/* Clear status */
+	mvebu_mmc_write(SDIO_NOR_INTR_STATUS, SDIO_POLL_MASK);
+	mvebu_mmc_write(SDIO_ERR_INTR_STATUS, SDIO_POLL_MASK);
 
 	resptype = SDIO_CMD_INDEX(cmd->cmdidx);
 
@@ -119,6 +131,14 @@ static int mvebu_mmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd,
 	}
 
 	if (data) {
+		int err = mvebu_mmc_setup_data(data);
+
+		if (err) {
+			debug("%s: command DATA error :%x\n",
+			      DRIVER_NAME, err);
+			return err;
+		}
+
 		resptype |= SDIO_CMD_DATA_PRESENT | SDIO_CMD_CHECK_DATACRC16;
 		xfertype |= SDIO_XFER_MODE_HW_WR_DATA_EN;
 		if (data->flags & MMC_DATA_READ) {
@@ -138,17 +158,10 @@ static int mvebu_mmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd,
 	/* Setting Xfer mode */
 	mvebu_mmc_write(SDIO_XFER_MODE, xfertype);
 
-	mvebu_mmc_write(SDIO_NOR_INTR_STATUS, ~SDIO_NOR_CARD_INT);
-	mvebu_mmc_write(SDIO_ERR_INTR_STATUS, SDIO_POLL_MASK);
-
 	/* Sending command */
 	mvebu_mmc_write(SDIO_CMD, resptype);
 
-	mvebu_mmc_write(SDIO_NOR_INTR_EN, SDIO_POLL_MASK);
-	mvebu_mmc_write(SDIO_ERR_INTR_EN, SDIO_POLL_MASK);
-
-	/* Waiting for completion */
-	timeout = 1000000;
+	start = get_timer(0);
 
 	while (!((mvebu_mmc_read(SDIO_NOR_INTR_STATUS)) & waittype)) {
 		if (mvebu_mmc_read(SDIO_NOR_INTR_STATUS) & SDIO_NOR_ERROR) {
@@ -156,21 +169,20 @@ static int mvebu_mmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd,
 			      DRIVER_NAME, cmd->cmdidx,
 			      mvebu_mmc_read(SDIO_ERR_INTR_STATUS));
 			if (mvebu_mmc_read(SDIO_ERR_INTR_STATUS) &
-				(SDIO_ERR_CMD_TIMEOUT | SDIO_ERR_DATA_TIMEOUT))
+			    (SDIO_ERR_CMD_TIMEOUT | SDIO_ERR_DATA_TIMEOUT)) {
+				debug("%s: command READ timed out\n",
+				      DRIVER_NAME);
 				return TIMEOUT;
+			}
+			debug("%s: command READ error\n", DRIVER_NAME);
 			return COMM_ERR;
 		}
 
-		timeout--;
-		udelay(1);
-		if (timeout <= 0) {
-			printf("%s: command timed out\n", DRIVER_NAME);
+		if ((get_timer(0) - start) > TIMEOUT_DELAY) {
+			debug("%s: command timed out\n", DRIVER_NAME);
 			return TIMEOUT;
 		}
 	}
-	if (mvebu_mmc_read(SDIO_ERR_INTR_STATUS) &
-		(SDIO_ERR_CMD_TIMEOUT | SDIO_ERR_DATA_TIMEOUT))
-		return TIMEOUT;
 
 	/* Handling response */
 	if (cmd->resp_type & MMC_RSP_136) {
@@ -204,6 +216,11 @@ static int mvebu_mmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd,
 		cmd->response[1] =	((response[0] & 0xfc00) >> 10);
 		cmd->response[2] =	0;
 		cmd->response[3] =	0;
+	} else {
+		cmd->response[0] =	0;
+		cmd->response[1] =	0;
+		cmd->response[2] =	0;
+		cmd->response[3] =	0;
 	}
 
 	debug("%s: resp[0x%x] ", DRIVER_NAME, cmd->resp_type);
@@ -213,6 +230,10 @@ static int mvebu_mmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd,
 	debug("[0x%x] ", cmd->response[3]);
 	debug("\n");
 
+	if (mvebu_mmc_read(SDIO_ERR_INTR_STATUS) &
+		(SDIO_ERR_CMD_TIMEOUT | SDIO_ERR_DATA_TIMEOUT))
+		return TIMEOUT;
+
 	return 0;
 }
 
@@ -251,9 +272,8 @@ static void mvebu_mmc_set_clk(unsigned int clock)
 		if (m > MVEBU_MMC_BASE_DIV_MAX)
 			m = MVEBU_MMC_BASE_DIV_MAX;
 		mvebu_mmc_write(SDIO_CLK_DIV, m & MVEBU_MMC_BASE_DIV_MAX);
+		debug("%s: clock (%d) div : %d\n", DRIVER_NAME, clock, m);
 	}
-
-	udelay(10*1000);
 }
 
 static void mvebu_mmc_set_bus(unsigned int bus)
@@ -293,7 +313,6 @@ static void mvebu_mmc_set_bus(unsigned int bus)
 	      "high-speed" : "");
 
 	mvebu_mmc_write(SDIO_HOST_CTRL, ctrl_reg);
-	udelay(10*1000);
 }
 
 static void mvebu_mmc_set_ios(struct mmc *mmc)
@@ -355,7 +374,7 @@ static void mvebu_window_setup(void)
 
 static int mvebu_mmc_initialize(struct mmc *mmc)
 {
-	debug("%s: mvebu_mmc_initialize", DRIVER_NAME);
+	debug("%s: mvebu_mmc_initialize\n", DRIVER_NAME);
 
 	/*
 	 * Setting host parameters
@@ -384,8 +403,6 @@ static int mvebu_mmc_initialize(struct mmc *mmc)
 	/* SW reset */
 	mvebu_mmc_write(SDIO_SW_RESET, SDIO_SW_RESET_NOW);
 
-	udelay(10*1000);
-
 	return 0;
 }
 
diff --git a/include/mvebu_mmc.h b/include/mvebu_mmc.h
index 28d98fe148..7fb71f7219 100644
--- a/include/mvebu_mmc.h
+++ b/include/mvebu_mmc.h
@@ -79,6 +79,7 @@
 #define CMD_INHIBIT				(1 << 0)
 #define CMD_TXACTIVE				(1 << 8)
 #define CMD_RXACTIVE				(1 << 9)
+#define CMD_FIFO_EMPTY				(1 << 13)
 #define CMD_AUTOCMD12ACTIVE			(1 << 14)
 #define CMD_BUS_BUSY				(CMD_AUTOCMD12ACTIVE |	\
 						CMD_RXACTIVE |	\