diff --git a/drivers/mtd/spi/sf_internal.h b/drivers/mtd/spi/sf_internal.h
index e97c716..4ecfd0c 100644
--- a/drivers/mtd/spi/sf_internal.h
+++ b/drivers/mtd/spi/sf_internal.h
@@ -49,6 +49,7 @@ enum {
 
 enum spi_nor_option_flags {
 	SNOR_F_SST_WR		= (1 << 0),
+	SNOR_F_USE_FSR		= (1 << 1),
 };
 
 #define SPI_FLASH_3B_ADDR_LEN		3
diff --git a/drivers/mtd/spi/sf_ops.c b/drivers/mtd/spi/sf_ops.c
index f1be815..388fdd0 100644
--- a/drivers/mtd/spi/sf_ops.c
+++ b/drivers/mtd/spi/sf_ops.c
@@ -40,6 +40,21 @@ int spi_flash_cmd_read_status(struct spi_flash *flash, u8 *rs)
 	return 0;
 }
 
+static int read_fsr(struct spi_flash *flash, u8 *fsr)
+{
+	int ret;
+	u8 cmd;
+
+	cmd = CMD_FLAG_STATUS;
+	ret = spi_flash_read_common(flash, &cmd, 1, fsr, 1);
+	if (ret < 0) {
+		debug("SF: fail to read flag status register\n");
+		return ret;
+	}
+
+	return 0;
+}
+
 int spi_flash_cmd_write_status(struct spi_flash *flash, u8 ws)
 {
 	u8 cmd;
@@ -138,72 +153,71 @@ static void spi_flash_dual_flash(struct spi_flash *flash, u32 *addr)
 }
 #endif
 
-static int spi_flash_poll_status(struct spi_slave *spi, unsigned long timeout,
-				 u8 cmd, u8 poll_bit)
+static inline int spi_flash_sr_ready(struct spi_flash *flash)
 {
-	unsigned long timebase;
-	unsigned long flags = SPI_XFER_BEGIN;
+	u8 sr;
 	int ret;
-	u8 status;
-	u8 check_status = 0x0;
-
-	if (cmd == CMD_FLAG_STATUS)
-		check_status = poll_bit;
 
-#ifdef CONFIG_SF_DUAL_FLASH
-	if (spi->flags & SPI_XFER_U_PAGE)
-		flags |= SPI_XFER_U_PAGE;
-#endif
-	ret = spi_xfer(spi, 8, &cmd, NULL, flags);
-	if (ret) {
-		debug("SF: fail to read %s status register\n",
-		      cmd == CMD_READ_STATUS ? "read" : "flag");
+	ret = spi_flash_cmd_read_status(flash, &sr);
+	if (ret < 0)
 		return ret;
-	}
-
-	timebase = get_timer(0);
-	do {
-		WATCHDOG_RESET();
-
-		ret = spi_xfer(spi, 8, NULL, &status, 0);
-		if (ret)
-			return -1;
 
-		if ((status & poll_bit) == check_status)
-			break;
+	if (sr < 0)
+		return sr;
+	else
+		return !(sr & STATUS_WIP);
+}
 
-	} while (get_timer(timebase) < timeout);
+static inline int spi_flash_fsr_ready(struct spi_flash *flash)
+{
+	u8 fsr;
+	int ret;
 
-	spi_xfer(spi, 0, NULL, NULL, SPI_XFER_END);
+	ret = read_fsr(flash, &fsr);
+	if (ret < 0)
+		return ret;
 
-	if ((status & poll_bit) == check_status)
-		return 0;
+	if (fsr < 0)
+		return fsr;
+	else
+		return fsr & STATUS_PEC;
+}
 
-	/* Timed out */
-	debug("SF: time out!\n");
-	return -1;
+static int spi_flash_ready(struct spi_flash *flash)
+{
+	int sr, fsr;
+	sr = spi_flash_sr_ready(flash);
+	if (sr < 0)
+		return sr;
+	fsr = flash->flags & SNOR_F_USE_FSR ? spi_flash_fsr_ready(flash) : 1;
+	if (fsr < 0)
+		return fsr;
+	return sr && fsr;
 }
 
-int spi_flash_cmd_wait_ready(struct spi_flash *flash, unsigned long timeout)
+/*
+ * Service routine to read status register until ready, or timeout occurs.
+ * Returns non-zero if error.
+ */
+int spi_flash_cmd_wait_ready(struct spi_flash *flash, unsigned long deadline)
 {
-	struct spi_slave *spi = flash->spi;
-	int ret;
-	u8 poll_bit = STATUS_WIP;
-	u8 cmd = CMD_READ_STATUS;
+	int timeout, ret;
 
-	ret = spi_flash_poll_status(spi, timeout, cmd, poll_bit);
-	if (ret < 0)
-		return ret;
+	timeout = get_timer(0);
 
-	if (flash->poll_cmd == CMD_FLAG_STATUS) {
-		poll_bit = STATUS_PEC;
-		cmd = CMD_FLAG_STATUS;
-		ret = spi_flash_poll_status(spi, timeout, cmd, poll_bit);
+	while (get_timer(timeout) < deadline) {
+		ret = spi_flash_ready(flash);
 		if (ret < 0)
 			return ret;
+		if (ret)
+			return 0;
+
+		cond_resched();
 	}
 
-	return 0;
+	printf("SF: Timeout!\n");
+
+	return -ETIMEDOUT;
 }
 
 int spi_flash_write_common(struct spi_flash *flash, const u8 *cmd,
diff --git a/drivers/mtd/spi/sf_probe.c b/drivers/mtd/spi/sf_probe.c
index 1de2bbb..fb79b02 100644
--- a/drivers/mtd/spi/sf_probe.c
+++ b/drivers/mtd/spi/sf_probe.c
@@ -256,11 +256,9 @@ static int spi_flash_validate_params(struct spi_slave *spi, u8 *idcode,
 		flash->dummy_byte = 1;
 	}
 
-	/* Poll cmd selection */
-	flash->poll_cmd = CMD_READ_STATUS;
 #ifdef CONFIG_SPI_FLASH_STMICRO
 	if (params->flags & E_FSR)
-		flash->poll_cmd = CMD_FLAG_STATUS;
+		flash->flags |= SNOR_F_USE_FSR;
 #endif
 
 	/* Configure the BAR - discover bank cmds and read current bank */
diff --git a/include/spi_flash.h b/include/spi_flash.h
index 8d85468..4312d3d 100644
--- a/include/spi_flash.h
+++ b/include/spi_flash.h
@@ -49,7 +49,6 @@ struct spi_slave;
  * @bank_read_cmd:	Bank read cmd
  * @bank_write_cmd:	Bank write cmd
  * @bank_curr:		Current flash bank
- * @poll_cmd:		Poll cmd - for flash erase/program
  * @erase_cmd:		Erase cmd 4K, 32K, 64K
  * @read_cmd:		Read cmd - Array Fast, Extn read and quad read.
  * @write_cmd:		Write cmd - page and quad program.
@@ -82,7 +81,6 @@ struct spi_flash {
 	u8 bank_write_cmd;
 	u8 bank_curr;
 #endif
-	u8 poll_cmd;
 	u8 erase_cmd;
 	u8 read_cmd;
 	u8 write_cmd;
