diff --git a/drivers/mtd/spi/sf_internal.h b/drivers/mtd/spi/sf_internal.h
index 19d4914..49e5a2c 100644
--- a/drivers/mtd/spi/sf_internal.h
+++ b/drivers/mtd/spi/sf_internal.h
@@ -36,6 +36,11 @@
 #define CMD_WRITE_ENABLE		0x06
 #define CMD_READ_CONFIG			0x35
 #define CMD_FLAG_STATUS			0x70
+#define CMD_CLEAR_FLAG_STATUS		0x50
+
+/* Used for Macronix and Winbond flashes */
+#define	CMD_ENTER_4B_ADDR		0xB7
+#define	CMD_EXIT_4B_ADDR		0xE9
 
 /* Read commands */
 #define CMD_READ_ARRAY_SLOW		0x03
@@ -59,6 +64,8 @@
 #define STATUS_QEB_WINSPAN		(1 << 1)
 #define STATUS_QEB_MXIC			(1 << 6)
 #define STATUS_PEC			(1 << 7)
+#define STATUS_PROT			(1 << 1)
+#define STATUS_ERASE			(1 << 5)
 
 #ifdef CONFIG_SYS_SPI_ST_ENABLE_WP_PIN
 #define STATUS_SRWD			(1 << 7) /* SR write protect */
@@ -124,6 +131,12 @@ static inline int spi_flash_cmd_write_disable(struct spi_flash *flash)
 	return spi_flash_cmd(flash->spi, CMD_WRITE_DISABLE, NULL, 0);
 }
 
+/* Clear flag status register */
+static inline int spi_flash_cmd_clear_flag_status(struct spi_flash *flash)
+{
+	return spi_flash_cmd(flash->spi, CMD_CLEAR_FLAG_STATUS, NULL, 0);
+}
+
 /*
  * Send the read status command to the device and wait for the wip
  * (write-in-progress) bit to clear itself.
@@ -160,4 +173,8 @@ int spi_flash_read_common(struct spi_flash *flash, const u8 *cmd,
 int spi_flash_cmd_read_ops(struct spi_flash *flash, u32 offset,
 		size_t len, void *data);
 
+#if defined(CONFIG_SPI_FLASH_STMICRO)
+int spi_flash_cmd_4B_addr_switch(struct spi_flash *flash, int enable);
+#endif
+
 #endif /* _SF_INTERNAL_H_ */
diff --git a/drivers/mtd/spi/sf_ops.c b/drivers/mtd/spi/sf_ops.c
index 85cf22d..8a532b8 100644
--- a/drivers/mtd/spi/sf_ops.c
+++ b/drivers/mtd/spi/sf_ops.c
@@ -93,6 +93,30 @@ int spi_flash_cmd_write_config(struct spi_flash *flash, u8 wc)
 }
 #endif
 
+#if defined(CONFIG_SPI_FLASH_STMICRO)
+int spi_flash_cmd_4B_addr_switch(struct spi_flash *flash, int enable)
+{
+	int ret;
+	u8 cmd;
+
+	cmd = enable ? CMD_ENTER_4B_ADDR : CMD_EXIT_4B_ADDR;
+
+	ret = spi_claim_bus(flash->spi);
+	if (ret) {
+		debug("SF: unable to claim SPI bus\n");
+		return ret;
+	}
+
+	ret = spi_flash_cmd_write_enable(flash);
+	if (ret < 0) {
+		debug("SF: enabling write failed\n");
+		return ret;
+	}
+
+	return spi_flash_cmd(flash->spi, cmd, NULL, 0);
+}
+#endif
+
 #ifdef CONFIG_SPI_FLASH_BAR
 static int spi_flash_cmd_bankaddr_write(struct spi_flash *flash, u8 bank_sel)
 {
@@ -160,6 +184,7 @@ int spi_flash_cmd_wait_ready(struct spi_flash *flash, unsigned long timeout)
 	unsigned long timebase;
 	unsigned long flags = SPI_XFER_BEGIN;
 	int ret;
+	int out_of_time = 1;
 	u8 status;
 	u8 check_status = 0x0;
 	u8 poll_bit = STATUS_WIP;
@@ -186,22 +211,45 @@ int spi_flash_cmd_wait_ready(struct spi_flash *flash, unsigned long timeout)
 		WATCHDOG_RESET();
 
 		ret = spi_xfer(spi, 8, NULL, &status, 0);
-		if (ret)
+		if (ret) {
+			spi_xfer(spi, 0, NULL, NULL, SPI_XFER_END);
 			return -1;
+		}
 
-		if ((status & poll_bit) == check_status)
+		if ((status & poll_bit) == check_status) {
+			out_of_time = 0;
 			break;
+		}
 
 	} while (get_timer(timebase) < timeout);
 
 	spi_xfer(spi, 0, NULL, NULL, SPI_XFER_END);
 
-	if ((status & poll_bit) == check_status)
-		return 0;
+	if (out_of_time) {
+		/* Timed out */
+		debug("SF: time out!\n");
+		if (cmd == CMD_FLAG_STATUS) {
+			if (spi_flash_cmd_clear_flag_status(flash) < 0)
+				debug("SF: clear flag status failed\n");
+		}
+		ret = -1;
+	}
+#ifdef CONFIG_SPI_FLASH_STMICRO
+	else if (cmd == CMD_FLAG_STATUS) {
+		if (!(status & (STATUS_PROT | STATUS_ERASE))) {
+			ret = 0;
+		} else {
+			debug("SF: flag status error");
+			ret = -1;
+		}
 
-	/* Timed out */
-	debug("SF: time out!\n");
-	return -1;
+		if (spi_flash_cmd_clear_flag_status(flash) < 0) {
+			debug("SF: clear flag status failed\n");
+			ret = -1;
+		}
+	}
+#endif
+	return ret;
 }
 
 int spi_flash_write_common(struct spi_flash *flash, const u8 *cmd,
@@ -234,7 +282,7 @@ int spi_flash_write_common(struct spi_flash *flash, const u8 *cmd,
 
 	ret = spi_flash_cmd_wait_ready(flash, timeout);
 	if (ret < 0) {
-		debug("SF: write %s timed out\n",
+		debug("SF: write %s failed\n",
 		      timeout == SPI_FLASH_PROG_TIMEOUT ?
 			"program" : "page erase");
 		return ret;
diff --git a/drivers/mtd/spi/sf_probe.c b/drivers/mtd/spi/sf_probe.c
index 4d148d1..79b876d 100644
--- a/drivers/mtd/spi/sf_probe.c
+++ b/drivers/mtd/spi/sf_probe.c
@@ -221,6 +221,11 @@ static struct spi_flash *spi_flash_validate_params(struct spi_slave *spi,
 #ifdef CONFIG_SPI_FLASH_STMICRO
 	if (params->flags & E_FSR)
 		flash->poll_cmd = CMD_FLAG_STATUS;
+
+	if (flash->size > SPI_FLASH_16MB_BOUN) {
+		if (spi_flash_cmd_4B_addr_switch(flash, 0) < 0)
+			debug("SF: enter 3B address mode failed\n");
+	}
 #endif
 
 	/* Configure the BAR - discover bank cmds and read current bank */
