3 files changed, 60 insertions, 9 deletions

diff --git a/drivers/mtd/spi/sf_internal.h b/drivers/mtd/spi/sf_internal.h
index 53998fca45..8a3e5ec3d7 100644
--- a/drivers/mtd/spi/sf_internal.h
+++ b/drivers/mtd/spi/sf_internal.h
@@ -53,6 +53,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 c820d48b6c..f2a9244a14 100644
--- a/drivers/mtd/spi/sf_ops.c
+++ b/drivers/mtd/spi/sf_ops.c
@@ -41,6 +41,20 @@ 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;
+	const u8 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;
@@ -139,22 +153,60 @@ static void spi_flash_dual_flash(struct spi_flash *flash, u32 *addr)
 }
 #endif
 
-int spi_flash_cmd_wait_ready(struct spi_flash *flash, unsigned long timeout)
+static int spi_flash_sr_ready(struct spi_flash *flash)
 {
 	u8 sr;
+	int ret;
+
+	ret = spi_flash_cmd_read_status(flash, &sr);
+	if (ret < 0)
+		return ret;
+
+	return !(sr & STATUS_WIP);
+}
+
+static int spi_flash_fsr_ready(struct spi_flash *flash)
+{
+	u8 fsr;
+	int ret;
+
+	ret = read_fsr(flash, &fsr);
+	if (ret < 0)
+		return ret;
+
+	return fsr & STATUS_PEC;
+}
+
+static int spi_flash_ready(struct spi_flash *flash)
+{
+	int sr, fsr;
+
+	sr = spi_flash_sr_ready(flash);
+	if (sr < 0)
+		return sr;
+
+	fsr = 1;
+	if (flash->flags & SNOR_F_USE_FSR) {
+		fsr = spi_flash_fsr_ready(flash);
+		if (fsr < 0)
+			return fsr;
+	}
+
+	return sr && fsr;
+}
+
+int spi_flash_cmd_wait_ready(struct spi_flash *flash, unsigned long timeout)
+{
 	int timebase, ret;
 
 	timebase = get_timer(0);
 
 	while (get_timer(timebase) < timeout) {
-		ret = spi_flash_cmd_read_status(flash, &sr);
+		ret = spi_flash_ready(flash);
 		if (ret < 0)
 			return ret;
-
-		if (!(sr & STATUS_WIP))
+		if (ret)
 			return 0;
-		else
-			break;
 	}
 
 	printf("SF: Timeout!\n");
diff --git a/drivers/mtd/spi/sf_probe.c b/drivers/mtd/spi/sf_probe.c
index 2634e90666..f591ab140b 100644
--- a/drivers/mtd/spi/sf_probe.c
+++ b/drivers/mtd/spi/sf_probe.c
@@ -257,11 +257,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 */