diff --git a/drivers/mmc/dw_mmc.c b/drivers/mmc/dw_mmc.c
index afc674d..074f86c 100644
--- a/drivers/mmc/dw_mmc.c
+++ b/drivers/mmc/dw_mmc.c
@@ -120,9 +120,9 @@
 
 		if (host->fifo_mode && size) {
 			len = 0;
-			if (data->flags == MMC_DATA_READ) {
-				if ((dwmci_readl(host, DWMCI_RINTSTS) &
-				     DWMCI_INTMSK_RXDR)) {
+			if (data->flags == MMC_DATA_READ &&
+			    (mask & DWMCI_INTMSK_RXDR)) {
+				while (size) {
 					len = dwmci_readl(host, DWMCI_STATUS);
 					len = (len >> DWMCI_FIFO_SHIFT) &
 						    DWMCI_FIFO_MASK;
@@ -130,12 +130,13 @@
 					for (i = 0; i < len; i++)
 						*buf++ =
 						dwmci_readl(host, DWMCI_DATA);
-					dwmci_writel(host, DWMCI_RINTSTS,
-						     DWMCI_INTMSK_RXDR);
+					size = size > len ? (size - len) : 0;
 				}
-			} else {
-				if ((dwmci_readl(host, DWMCI_RINTSTS) &
-				     DWMCI_INTMSK_TXDR)) {
+				dwmci_writel(host, DWMCI_RINTSTS,
+					     DWMCI_INTMSK_RXDR);
+			} else if (data->flags == MMC_DATA_WRITE &&
+				   (mask & DWMCI_INTMSK_TXDR)) {
+				while (size) {
 					len = dwmci_readl(host, DWMCI_STATUS);
 					len = fifo_depth - ((len >>
 						   DWMCI_FIFO_SHIFT) &
@@ -144,11 +145,11 @@
 					for (i = 0; i < len; i++)
 						dwmci_writel(host, DWMCI_DATA,
 							     *buf++);
-					dwmci_writel(host, DWMCI_RINTSTS,
-						     DWMCI_INTMSK_TXDR);
+					size = size > len ? (size - len) : 0;
 				}
+				dwmci_writel(host, DWMCI_RINTSTS,
+					     DWMCI_INTMSK_TXDR);
 			}
-			size = size > len ? (size - len) : 0;
 		}
 
 		/* Data arrived correctly. */
diff --git a/drivers/mmc/mmc.c b/drivers/mmc/mmc.c
index 43ea0bb..0312da9 100644
--- a/drivers/mmc/mmc.c
+++ b/drivers/mmc/mmc.c
@@ -21,6 +21,14 @@
 #include <div64.h>
 #include "mmc_private.h"
 
+static const unsigned int sd_au_size[] = {
+	0,		SZ_16K / 512,		SZ_32K / 512,
+	SZ_64K / 512,	SZ_128K / 512,		SZ_256K / 512,
+	SZ_512K / 512,	SZ_1M / 512,		SZ_2M / 512,
+	SZ_4M / 512,	SZ_8M / 512,		(SZ_8M + SZ_4M) / 512,
+	SZ_16M / 512,	(SZ_16M + SZ_8M) / 512,	SZ_32M / 512,	SZ_64M / 512,
+};
+
 #ifndef CONFIG_DM_MMC_OPS
 __weak int board_mmc_getwp(struct mmc *mmc)
 {
@@ -945,6 +953,62 @@
 	return 0;
 }
 
+static int sd_read_ssr(struct mmc *mmc)
+{
+	int err, i;
+	struct mmc_cmd cmd;
+	ALLOC_CACHE_ALIGN_BUFFER(uint, ssr, 16);
+	struct mmc_data data;
+	int timeout = 3;
+	unsigned int au, eo, et, es;
+
+	cmd.cmdidx = MMC_CMD_APP_CMD;
+	cmd.resp_type = MMC_RSP_R1;
+	cmd.cmdarg = mmc->rca << 16;
+
+	err = mmc_send_cmd(mmc, &cmd, NULL);
+	if (err)
+		return err;
+
+	cmd.cmdidx = SD_CMD_APP_SD_STATUS;
+	cmd.resp_type = MMC_RSP_R1;
+	cmd.cmdarg = 0;
+
+retry_ssr:
+	data.dest = (char *)ssr;
+	data.blocksize = 64;
+	data.blocks = 1;
+	data.flags = MMC_DATA_READ;
+
+	err = mmc_send_cmd(mmc, &cmd, &data);
+	if (err) {
+		if (timeout--)
+			goto retry_ssr;
+
+		return err;
+	}
+
+	for (i = 0; i < 16; i++)
+		ssr[i] = be32_to_cpu(ssr[i]);
+
+	au = (ssr[2] >> 12) & 0xF;
+	if ((au <= 9) || (mmc->version == SD_VERSION_3)) {
+		mmc->ssr.au = sd_au_size[au];
+		es = (ssr[3] >> 24) & 0xFF;
+		es |= (ssr[2] & 0xFF) << 8;
+		et = (ssr[3] >> 18) & 0x3F;
+		if (es && et) {
+			eo = (ssr[3] >> 16) & 0x3;
+			mmc->ssr.erase_timeout = (et * 1000) / es;
+			mmc->ssr.erase_offset = eo * 1000;
+		}
+	} else {
+		debug("Invalid Allocation Unit Size.\n");
+	}
+
+	return 0;
+}
+
 /* frequency bases */
 /* divided by 10 to be nice to platforms without floating point */
 static const int fbase[] = {
@@ -1350,6 +1414,10 @@
 			mmc_set_bus_width(mmc, 4);
 		}
 
+		err = sd_read_ssr(mmc);
+		if (err)
+			return err;
+
 		if (mmc->card_caps & MMC_MODE_HS)
 			mmc->tran_speed = 50000000;
 		else
diff --git a/drivers/mmc/mmc_write.c b/drivers/mmc/mmc_write.c
index 0f8b5c7..2289640 100644
--- a/drivers/mmc/mmc_write.c
+++ b/drivers/mmc/mmc_write.c
@@ -100,8 +100,13 @@
 		       & ~(mmc->erase_grp_size - 1)) - 1);
 
 	while (blk < blkcnt) {
-		blk_r = ((blkcnt - blk) > mmc->erase_grp_size) ?
-			mmc->erase_grp_size : (blkcnt - blk);
+		if (IS_SD(mmc) && mmc->ssr.au) {
+			blk_r = ((blkcnt - blk) > mmc->ssr.au) ?
+				mmc->ssr.au : (blkcnt - blk);
+		} else {
+			blk_r = ((blkcnt - blk) > mmc->erase_grp_size) ?
+				mmc->erase_grp_size : (blkcnt - blk);
+		}
 		err = mmc_erase_t(mmc, start + blk, blk_r);
 		if (err)
 			break;
diff --git a/drivers/mmc/sdhci.c b/drivers/mmc/sdhci.c
index 7ddb549..b2bf5a0 100644
--- a/drivers/mmc/sdhci.c
+++ b/drivers/mmc/sdhci.c
@@ -121,13 +121,10 @@
  * for card ready state.
  * Every time when card is busy after timeout then (last) timeout value will be
  * increased twice but only if it doesn't exceed global defined maximum.
- * Each function call will use last timeout value. Max timeout can be redefined
- * in board config file.
+ * Each function call will use last timeout value.
  */
-#ifndef CONFIG_SDHCI_CMD_MAX_TIMEOUT
-#define CONFIG_SDHCI_CMD_MAX_TIMEOUT		3200
-#endif
-#define CONFIG_SDHCI_CMD_DEFAULT_TIMEOUT	100
+#define SDHCI_CMD_MAX_TIMEOUT			3200
+#define SDHCI_CMD_DEFAULT_TIMEOUT		100
 #define SDHCI_READ_STATUS_TIMEOUT		1000
 
 #ifdef CONFIG_DM_MMC_OPS
@@ -151,7 +148,7 @@
 	unsigned start = get_timer(0);
 
 	/* Timeout unit - ms */
-	static unsigned int cmd_timeout = CONFIG_SDHCI_CMD_DEFAULT_TIMEOUT;
+	static unsigned int cmd_timeout = SDHCI_CMD_DEFAULT_TIMEOUT;
 
 	sdhci_writel(host, SDHCI_INT_ALL_MASK, SDHCI_INT_STATUS);
 	mask = SDHCI_CMD_INHIBIT | SDHCI_DATA_INHIBIT;
@@ -164,7 +161,7 @@
 	while (sdhci_readl(host, SDHCI_PRESENT_STATE) & mask) {
 		if (time >= cmd_timeout) {
 			printf("%s: MMC: %d busy ", __func__, mmc_dev);
-			if (2 * cmd_timeout <= CONFIG_SDHCI_CMD_MAX_TIMEOUT) {
+			if (2 * cmd_timeout <= SDHCI_CMD_MAX_TIMEOUT) {
 				cmd_timeout += cmd_timeout;
 				printf("timeout increasing to: %u ms.\n",
 				       cmd_timeout);
@@ -297,7 +294,7 @@
 static int sdhci_set_clock(struct mmc *mmc, unsigned int clock)
 {
 	struct sdhci_host *host = mmc->priv;
-	unsigned int div, clk, timeout, reg;
+	unsigned int div, clk = 0, timeout, reg;
 
 	/* Wait max 20 ms */
 	timeout = 200;
@@ -321,14 +318,36 @@
 		return 0;
 
 	if (SDHCI_GET_VERSION(host) >= SDHCI_SPEC_300) {
-		/* Version 3.00 divisors must be a multiple of 2. */
-		if (mmc->cfg->f_max <= clock)
-			div = 1;
-		else {
-			for (div = 2; div < SDHCI_MAX_DIV_SPEC_300; div += 2) {
-				if ((mmc->cfg->f_max / div) <= clock)
+		/*
+		 * Check if the Host Controller supports Programmable Clock
+		 * Mode.
+		 */
+		if (host->clk_mul) {
+			for (div = 1; div <= 1024; div++) {
+				if ((mmc->cfg->f_max * host->clk_mul / div)
+					<= clock)
 					break;
 			}
+
+			/*
+			 * Set Programmable Clock Mode in the Clock
+			 * Control register.
+			 */
+			clk = SDHCI_PROG_CLOCK_MODE;
+			div--;
+		} else {
+			/* Version 3.00 divisors must be a multiple of 2. */
+			if (mmc->cfg->f_max <= clock) {
+				div = 1;
+			} else {
+				for (div = 2;
+				     div < SDHCI_MAX_DIV_SPEC_300;
+				     div += 2) {
+					if ((mmc->cfg->f_max / div) <= clock)
+						break;
+				}
+			}
+			div >>= 1;
 		}
 	} else {
 		/* Version 2.00 divisors must be a power of 2. */
@@ -336,13 +355,13 @@
 			if ((mmc->cfg->f_max / div) <= clock)
 				break;
 		}
+		div >>= 1;
 	}
-	div >>= 1;
 
 	if (host->set_clock)
 		host->set_clock(host->index, div);
 
-	clk = (div & SDHCI_DIV_MASK) << SDHCI_DIVIDER_SHIFT;
+	clk |= (div & SDHCI_DIV_MASK) << SDHCI_DIVIDER_SHIFT;
 	clk |= ((div & SDHCI_DIV_HI_MASK) >> SDHCI_DIV_MASK_LEN)
 		<< SDHCI_DIVIDER_HI_SHIFT;
 	clk |= SDHCI_CLOCK_INT_EN;
@@ -451,6 +470,8 @@
 {
 	struct sdhci_host *host = mmc->priv;
 
+	sdhci_reset(host, SDHCI_RESET_ALL);
+
 	if ((host->quirks & SDHCI_QUIRK_32BIT_DMA_ADDR) && !aligned_buffer) {
 		aligned_buffer = memalign(8, 512*1024);
 		if (!aligned_buffer) {
@@ -514,9 +535,17 @@
 int sdhci_setup_cfg(struct mmc_config *cfg, struct sdhci_host *host,
 		u32 max_clk, u32 min_clk)
 {
-	u32 caps;
+	u32 caps, caps_1;
 
 	caps = sdhci_readl(host, SDHCI_CAPABILITIES);
+
+#ifdef CONFIG_MMC_SDMA
+	if (!(caps & SDHCI_CAN_DO_SDMA)) {
+		printf("%s: Your controller doesn't support SDMA!!\n",
+		       __func__);
+		return -EINVAL;
+	}
+#endif
 	host->version = sdhci_readw(host, SDHCI_HOST_VERSION);
 
 	cfg->name = host->name;
@@ -534,8 +563,11 @@
 				SDHCI_CLOCK_BASE_SHIFT;
 		cfg->f_max *= 1000000;
 	}
-	if (cfg->f_max == 0)
+	if (cfg->f_max == 0) {
+		printf("%s: Hardware doesn't specify base clock frequency\n",
+		       __func__);
 		return -EINVAL;
+	}
 	if (min_clk)
 		cfg->f_min = min_clk;
 	else {
@@ -552,6 +584,9 @@
 	if (caps & SDHCI_CAN_VDD_180)
 		cfg->voltages |= MMC_VDD_165_195;
 
+	if (host->quirks & SDHCI_QUIRK_BROKEN_VOLTAGE)
+		cfg->voltages |= host->voltages;
+
 	cfg->host_caps = MMC_MODE_HS | MMC_MODE_HS_52MHz | MMC_MODE_4BIT;
 	if (SDHCI_GET_VERSION(host) >= SDHCI_SPEC_300) {
 		if (caps & SDHCI_CAN_DO_8BIT)
@@ -564,6 +599,14 @@
 
 	cfg->b_max = CONFIG_SYS_MMC_MAX_BLK_COUNT;
 
+	/*
+	 * In case of Host Controller v3.00, find out whether clock
+	 * multiplier is supported.
+	 */
+	caps_1 = sdhci_readl(host, SDHCI_CAPABILITIES_1);
+	host->clk_mul = (caps_1 & SDHCI_CLOCK_MUL_MASK) >>
+			SDHCI_CLOCK_MUL_SHIFT;
+
 	return 0;
 }
 
@@ -575,27 +618,11 @@
 #else
 int add_sdhci(struct sdhci_host *host, u32 max_clk, u32 min_clk)
 {
-#ifdef CONFIG_MMC_SDMA
-	unsigned int caps;
+	int ret;
 
-	caps = sdhci_readl(host, SDHCI_CAPABILITIES);
-	if (!(caps & SDHCI_CAN_DO_SDMA)) {
-		printf("%s: Your controller doesn't support SDMA!!\n",
-		       __func__);
-		return -1;
-	}
-#endif
-
-	if (sdhci_setup_cfg(&host->cfg, host, max_clk, min_clk)) {
-		printf("%s: Hardware doesn't specify base clock frequency\n",
-		       __func__);
-		return -EINVAL;
-	}
-
-	if (host->quirks & SDHCI_QUIRK_BROKEN_VOLTAGE)
-		host->cfg.voltages |= host->voltages;
-
-	sdhci_reset(host, SDHCI_RESET_ALL);
+	ret = sdhci_setup_cfg(&host->cfg, host, max_clk, min_clk);
+	if (ret)
+		return ret;
 
 	host->mmc = mmc_create(&host->cfg, host);
 	if (host->mmc == NULL) {
diff --git a/include/mmc.h b/include/mmc.h
index aa6d5d1..e815eb3 100644
--- a/include/mmc.h
+++ b/include/mmc.h
@@ -11,6 +11,7 @@
 #define _MMC_H_
 
 #include <linux/list.h>
+#include <linux/sizes.h>
 #include <linux/compiler.h>
 #include <part.h>
 
@@ -102,6 +103,7 @@
 #define SD_CMD_SWITCH_UHS18V		11
 
 #define SD_CMD_APP_SET_BUS_WIDTH	6
+#define SD_CMD_APP_SD_STATUS		13
 #define SD_CMD_ERASE_WR_BLK_START	32
 #define SD_CMD_ERASE_WR_BLK_END		33
 #define SD_CMD_APP_SEND_OP_COND		41
@@ -392,6 +394,12 @@
 	unsigned char part_type;
 };
 
+struct sd_ssr {
+	unsigned int au;		/* In sectors */
+	unsigned int erase_timeout;	/* In milliseconds */
+	unsigned int erase_offset;	/* In milliseconds */
+};
+
 /*
  * With CONFIG_DM_MMC enabled, struct mmc can be accessed from the MMC device
  * with mmc_get_mmc_dev().
@@ -426,6 +434,7 @@
 	uint write_bl_len;
 	uint erase_grp_size;	/* in 512-byte sectors */
 	uint hc_wp_grp_size;	/* in 512-byte sectors */
+	struct sd_ssr	ssr;	/* SD status register */
 	u64 capacity;
 	u64 capacity_user;
 	u64 capacity_boot;
diff --git a/include/sdhci.h b/include/sdhci.h
index 6844c73..144570f 100644
--- a/include/sdhci.h
+++ b/include/sdhci.h
@@ -97,6 +97,7 @@
 #define  SDHCI_DIV_MASK	0xFF
 #define  SDHCI_DIV_MASK_LEN	8
 #define  SDHCI_DIV_HI_MASK	0x300
+#define  SDHCI_PROG_CLOCK_MODE  0x0020
 #define  SDHCI_CLOCK_CARD_EN	0x0004
 #define  SDHCI_CLOCK_INT_STABLE	0x0002
 #define  SDHCI_CLOCK_INT_EN	0x0001
@@ -242,6 +243,7 @@
 	unsigned int quirks;
 	unsigned int host_caps;
 	unsigned int version;
+	unsigned int clk_mul;   /* Clock Multiplier value */
 	unsigned int clock;
 	struct mmc *mmc;
 	const struct sdhci_ops *ops;