mtd: st_spi_fsm: Update Macronix 'QE' configuration

Update the configuration of the Macronix 'QE' bit, such that
we only set or clear the bit if required.

Signed-off-by: Angus Clark <angus.clark@st.com>
Signed-off-by: Lee Jones <lee.jones@linaro.org>
Signed-off-by: Brian Norris <computersforpeace@gmail.com>

1 files changed, 19 insertions, 4 deletions

diff --git a/drivers/mtd/devices/st_spi_fsm.c b/drivers/mtd/devices/st_spi_fsm.c
index 40eb830eea21..f86d3e01433b 100644
--- a/drivers/mtd/devices/st_spi_fsm.c
+++ b/drivers/mtd/devices/st_spi_fsm.c
@@ -1164,12 +1164,27 @@ static int stfsm_mx25_config(struct stfsm *fsm)
                                               CFG_ERASESEC_TOGGLE_32BIT_ADDR);
         }
 
-        /* For QUAD mode, set 'QE' STATUS bit */
+        /* Check status of 'QE' bit */
+        stfsm_read_status(fsm, FLASH_CMD_RDSR, &sta);
         data_pads = ((fsm->stfsm_seq_read.seq_cfg >> 16) & 0x3) + 1;
         if (data_pads == 4) {
-                stfsm_read_status(fsm, FLASH_CMD_RDSR, &sta);
-                sta |= MX25_STATUS_QE;
-                stfsm_write_status(fsm, sta, 1);
+                if (!(sta & MX25_STATUS_QE)) {
+                        /* Set 'QE' */
+                        sta |= MX25_STATUS_QE;
+
+                        stfsm_write_status(fsm, sta, 1);
+
+                        stfsm_wait_busy(fsm);
+                }
+        } else {
+                if (sta & MX25_STATUS_QE) {
+                        /* Clear 'QE' */
+                        sta &= ~MX25_STATUS_QE;
+
+                        stfsm_write_status(fsm, sta, 1);
+
+                        stfsm_wait_busy(fsm);
+                }
         }
 
         return 0;