1 files changed, 26 insertions, 0 deletions

diff --git a/drivers/mtd/devices/st_spi_fsm.c b/drivers/mtd/devices/st_spi_fsm.c
index 5e22c8688197..73e0f2731bcb 100644
--- a/drivers/mtd/devices/st_spi_fsm.c
+++ b/drivers/mtd/devices/st_spi_fsm.c
@@ -281,6 +281,32 @@ static void stfsm_wait_seq(struct stfsm *fsm)
         dev_err(fsm->dev, "timeout on sequence completion\n");
 }
 
+static void stfsm_read_fifo(struct stfsm *fsm, uint32_t *buf,
+                            const uint32_t size)
+{
+        uint32_t remaining = size >> 2;
+        uint32_t avail;
+        uint32_t words;
+
+        dev_dbg(fsm->dev, "Reading %d bytes from FIFO\n", size);
+
+        BUG_ON((((uint32_t)buf) & 0x3) || (size & 0x3));
+
+        while (remaining) {
+                for (;;) {
+                        avail = stfsm_fifo_available(fsm);
+                        if (avail)
+                                break;
+                        udelay(1);
+                }
+                words = min(avail, remaining);
+                remaining -= words;
+
+                readsl(fsm->base + SPI_FAST_SEQ_DATA_REG, buf, words);
+                buf += words;
+        }
+}
+
 static int stfsm_set_mode(struct stfsm *fsm, uint32_t mode)
 {
         int ret, timeout = 10;