spi: stm32_qspi: move to exec_op

We are facing issues in the driver since SPI NOR framework has moved
on SPI MEM framework, and SPI NAND framework is not running properly
with the current driver.

To be able to solve issues met on SPI NOR Flashes and to be able to
support SPI NAND Flashes, the driver has been reworked. We are now using
exec_op ops instead of using xfer ops.

Thanks to this rework, the driver has been successfully tested with:
 - mx66l51235l SPI NOR Flash on stm32f746 SOC
 - n25q128a SPI NOR Flash on stm32f769 SOC
 - mx66l51235l SPI NOR Flash on stm32mp1 SOC
 - mt29f2g01abagd SPI NAND Flash on stm32mp1 SOC

Signed-off-by: Christophe Kerello <christophe.kerello@st.com>
Tested-by: Patrick DELAUNAY <patrick.delaunay@st.com>
Reviewed-by: Patrick DELAUNAY <patrick.delaunay@st.com>

1 files changed, 244 insertions, 381 deletions

diff --git a/drivers/spi/stm32_qspi.c b/drivers/spi/stm32_qspi.c
index 8b60d7c3b2..bb1067ff4a 100644
--- a/drivers/spi/stm32_qspi.c
+++ b/drivers/spi/stm32_qspi.c
@@ -9,15 +9,11 @@
 
 #include <common.h>
 #include <clk.h>
-#include <dm.h>
-#include <errno.h>
-#include <malloc.h>
 #include <reset.h>
-#include <spi.h>
-#include <spi_flash.h>
-#include <asm/io.h>
-#include <asm/arch/stm32.h>
+#include <spi-mem.h>
+#include <linux/iopoll.h>
 #include <linux/ioport.h>
+#include <linux/sizes.h>
 
 struct stm32_qspi_regs {
 	u32 cr;		/* 0x00 */
@@ -45,8 +41,7 @@ struct stm32_qspi_regs {
 #define STM32_QSPI_CR_SSHIFT		BIT(4)
 #define STM32_QSPI_CR_DFM		BIT(6)
 #define STM32_QSPI_CR_FSEL		BIT(7)
-#define STM32_QSPI_CR_FTHRES_MASK	GENMASK(4, 0)
-#define STM32_QSPI_CR_FTHRES_SHIFT	(8)
+#define STM32_QSPI_CR_FTHRES_SHIFT	8
 #define STM32_QSPI_CR_TEIE		BIT(16)
 #define STM32_QSPI_CR_TCIE		BIT(17)
 #define STM32_QSPI_CR_FTIE		BIT(18)
@@ -55,16 +50,16 @@ struct stm32_qspi_regs {
 #define STM32_QSPI_CR_APMS		BIT(22)
 #define STM32_QSPI_CR_PMM		BIT(23)
 #define STM32_QSPI_CR_PRESCALER_MASK	GENMASK(7, 0)
-#define STM32_QSPI_CR_PRESCALER_SHIFT	(24)
+#define STM32_QSPI_CR_PRESCALER_SHIFT	24
 
 /*
  * QUADSPI device configuration register
  */
 #define STM32_QSPI_DCR_CKMODE		BIT(0)
 #define STM32_QSPI_DCR_CSHT_MASK	GENMASK(2, 0)
-#define STM32_QSPI_DCR_CSHT_SHIFT	(8)
+#define STM32_QSPI_DCR_CSHT_SHIFT	8
 #define STM32_QSPI_DCR_FSIZE_MASK	GENMASK(4, 0)
-#define STM32_QSPI_DCR_FSIZE_SHIFT	(16)
+#define STM32_QSPI_DCR_FSIZE_SHIFT	16
 
 /*
  * QUADSPI status register
@@ -75,8 +70,6 @@ struct stm32_qspi_regs {
 #define STM32_QSPI_SR_SMF		BIT(3)
 #define STM32_QSPI_SR_TOF		BIT(4)
 #define STM32_QSPI_SR_BUSY		BIT(5)
-#define STM32_QSPI_SR_FLEVEL_MASK	GENMASK(5, 0)
-#define STM32_QSPI_SR_FLEVEL_SHIFT	(8)
 
 /*
  * QUADSPI flag clear register
@@ -92,388 +85,276 @@ struct stm32_qspi_regs {
 #define STM32_QSPI_CCR_DDRM		BIT(31)
 #define STM32_QSPI_CCR_DHHC		BIT(30)
 #define STM32_QSPI_CCR_SIOO		BIT(28)
-#define STM32_QSPI_CCR_FMODE_SHIFT	(26)
-#define STM32_QSPI_CCR_DMODE_SHIFT	(24)
-#define STM32_QSPI_CCR_DCYC_SHIFT	(18)
-#define STM32_QSPI_CCR_DCYC_MASK	GENMASK(4, 0)
-#define STM32_QSPI_CCR_ABSIZE_SHIFT	(16)
-#define STM32_QSPI_CCR_ABMODE_SHIFT	(14)
-#define STM32_QSPI_CCR_ADSIZE_SHIFT	(12)
-#define STM32_QSPI_CCR_ADMODE_SHIFT	(10)
-#define STM32_QSPI_CCR_IMODE_SHIFT	(8)
-#define STM32_QSPI_CCR_INSTRUCTION_MASK	GENMASK(7, 0)
-
-enum STM32_QSPI_CCR_IMODE {
-	STM32_QSPI_CCR_IMODE_NONE = 0,
-	STM32_QSPI_CCR_IMODE_ONE_LINE = 1,
-	STM32_QSPI_CCR_IMODE_TWO_LINE = 2,
-	STM32_QSPI_CCR_IMODE_FOUR_LINE = 3,
-};
-
-enum STM32_QSPI_CCR_ADMODE {
-	STM32_QSPI_CCR_ADMODE_NONE = 0,
-	STM32_QSPI_CCR_ADMODE_ONE_LINE = 1,
-	STM32_QSPI_CCR_ADMODE_TWO_LINE = 2,
-	STM32_QSPI_CCR_ADMODE_FOUR_LINE = 3,
-};
-
-enum STM32_QSPI_CCR_ADSIZE {
-	STM32_QSPI_CCR_ADSIZE_8BIT = 0,
-	STM32_QSPI_CCR_ADSIZE_16BIT = 1,
-	STM32_QSPI_CCR_ADSIZE_24BIT = 2,
-	STM32_QSPI_CCR_ADSIZE_32BIT = 3,
-};
-
-enum STM32_QSPI_CCR_ABMODE {
-	STM32_QSPI_CCR_ABMODE_NONE = 0,
-	STM32_QSPI_CCR_ABMODE_ONE_LINE = 1,
-	STM32_QSPI_CCR_ABMODE_TWO_LINE = 2,
-	STM32_QSPI_CCR_ABMODE_FOUR_LINE = 3,
-};
-
-enum STM32_QSPI_CCR_ABSIZE {
-	STM32_QSPI_CCR_ABSIZE_8BIT = 0,
-	STM32_QSPI_CCR_ABSIZE_16BIT = 1,
-	STM32_QSPI_CCR_ABSIZE_24BIT = 2,
-	STM32_QSPI_CCR_ABSIZE_32BIT = 3,
-};
-
-enum STM32_QSPI_CCR_DMODE {
-	STM32_QSPI_CCR_DMODE_NONE = 0,
-	STM32_QSPI_CCR_DMODE_ONE_LINE = 1,
-	STM32_QSPI_CCR_DMODE_TWO_LINE = 2,
-	STM32_QSPI_CCR_DMODE_FOUR_LINE = 3,
-};
-
-enum STM32_QSPI_CCR_FMODE {
-	STM32_QSPI_CCR_IND_WRITE = 0,
-	STM32_QSPI_CCR_IND_READ = 1,
-	STM32_QSPI_CCR_AUTO_POLL = 2,
-	STM32_QSPI_CCR_MEM_MAP = 3,
-};
-
-/* default SCK frequency, unit: HZ */
-#define STM32_QSPI_DEFAULT_SCK_FREQ 108000000
-
-#define STM32_MAX_NORCHIP 2
-
-struct stm32_qspi_platdata {
-	u32 base;
-	u32 memory_map;
-	u32 max_hz;
+#define STM32_QSPI_CCR_FMODE_SHIFT	26
+#define STM32_QSPI_CCR_DMODE_SHIFT	24
+#define STM32_QSPI_CCR_DCYC_SHIFT	18
+#define STM32_QSPI_CCR_ABSIZE_SHIFT	16
+#define STM32_QSPI_CCR_ABMODE_SHIFT	14
+#define STM32_QSPI_CCR_ADSIZE_SHIFT	12
+#define STM32_QSPI_CCR_ADMODE_SHIFT	10
+#define STM32_QSPI_CCR_IMODE_SHIFT	8
+
+#define STM32_QSPI_CCR_IND_WRITE	0
+#define STM32_QSPI_CCR_IND_READ		1
+#define STM32_QSPI_CCR_MEM_MAP		3
+
+#define STM32_QSPI_MAX_MMAP_SZ		SZ_256M
+#define STM32_QSPI_MAX_CHIP		2
+
+#define STM32_QSPI_FIFO_TIMEOUT_US	30000
+#define STM32_QSPI_CMD_TIMEOUT_US	1000000
+#define STM32_BUSY_TIMEOUT_US		100000
+#define STM32_ABT_TIMEOUT_US		100000
+
+struct stm32_qspi_flash {
+	u32 cr;
+	u32 dcr;
+	bool initialized;
 };
 
 struct stm32_qspi_priv {
 	struct stm32_qspi_regs *regs;
+	struct stm32_qspi_flash flash[STM32_QSPI_MAX_CHIP];
+	void __iomem *mm_base;
+	resource_size_t mm_size;
 	ulong clock_rate;
-	u32 max_hz;
-	u32 mode;
-
-	u32 command;
-	u32 address;
-	u32 dummycycles;
-#define CMD_HAS_ADR	BIT(24)
-#define CMD_HAS_DUMMY	BIT(25)
-#define CMD_HAS_DATA	BIT(26)
+	int cs_used;
 };
 
-static void _stm32_qspi_disable(struct stm32_qspi_priv *priv)
+static int _stm32_qspi_wait_for_not_busy(struct stm32_qspi_priv *priv)
 {
-	clrbits_le32(&priv->regs->cr, STM32_QSPI_CR_EN);
-}
+	u32 sr;
+	int ret;
 
-static void _stm32_qspi_enable(struct stm32_qspi_priv *priv)
-{
-	setbits_le32(&priv->regs->cr, STM32_QSPI_CR_EN);
-}
+	ret = readl_poll_timeout(&priv->regs->sr, sr,
+				 !(sr & STM32_QSPI_SR_BUSY),
+				 STM32_BUSY_TIMEOUT_US);
+	if (ret)
+		pr_err("busy timeout (stat:%#x)\n", sr);
 
-static void _stm32_qspi_wait_for_not_busy(struct stm32_qspi_priv *priv)
-{
-	while (readl(&priv->regs->sr) & STM32_QSPI_SR_BUSY)
-		;
+	return ret;
 }
 
-static void _stm32_qspi_wait_for_complete(struct stm32_qspi_priv *priv)
+static int _stm32_qspi_wait_cmd(struct stm32_qspi_priv *priv,
+				const struct spi_mem_op *op)
 {
-	while (!(readl(&priv->regs->sr) & STM32_QSPI_SR_TCF))
-		;
-}
+	u32 sr;
+	int ret;
 
-static void _stm32_qspi_wait_for_ftf(struct stm32_qspi_priv *priv)
-{
-	while (!(readl(&priv->regs->sr) & STM32_QSPI_SR_FTF))
-		;
-}
+	if (!op->data.nbytes)
+		return _stm32_qspi_wait_for_not_busy(priv);
 
-static void _stm32_qspi_set_flash_size(struct stm32_qspi_priv *priv, u32 size)
-{
-	u32 fsize = fls(size) - 1;
+	ret = readl_poll_timeout(&priv->regs->sr, sr,
+				 sr & STM32_QSPI_SR_TCF,
+				 STM32_QSPI_CMD_TIMEOUT_US);
+	if (ret) {
+		pr_err("cmd timeout (stat:%#x)\n", sr);
+	} else if (readl(&priv->regs->sr) & STM32_QSPI_SR_TEF) {
+		pr_err("transfer error (stat:%#x)\n", sr);
+		ret = -EIO;
+	}
 
-	clrsetbits_le32(&priv->regs->dcr,
-			STM32_QSPI_DCR_FSIZE_MASK << STM32_QSPI_DCR_FSIZE_SHIFT,
-			fsize << STM32_QSPI_DCR_FSIZE_SHIFT);
+	/* clear flags */
+	writel(STM32_QSPI_FCR_CTCF | STM32_QSPI_FCR_CTEF, &priv->regs->fcr);
+
+	return ret;
 }
 
-static void _stm32_qspi_set_cs(struct stm32_qspi_priv *priv, unsigned int cs)
+static void _stm32_qspi_read_fifo(u8 *val, void __iomem *addr)
 {
-	clrsetbits_le32(&priv->regs->cr, STM32_QSPI_CR_FSEL,
-			cs ? STM32_QSPI_CR_FSEL : 0);
+	*val = readb(addr);
 }
 
-static unsigned int _stm32_qspi_gen_ccr(struct stm32_qspi_priv *priv, u8 fmode)
+static void _stm32_qspi_write_fifo(u8 *val, void __iomem *addr)
 {
-	unsigned int ccr_reg = 0;
-	u8 imode, admode, dmode;
-	u32 mode = priv->mode;
-	u32 cmd = (priv->command & STM32_QSPI_CCR_INSTRUCTION_MASK);
-
-	imode = STM32_QSPI_CCR_IMODE_ONE_LINE;
-	admode = STM32_QSPI_CCR_ADMODE_ONE_LINE;
-	dmode = STM32_QSPI_CCR_DMODE_ONE_LINE;
-
-	if ((priv->command & CMD_HAS_ADR) && (priv->command & CMD_HAS_DATA)) {
-		if (fmode == STM32_QSPI_CCR_IND_WRITE) {
-			if (mode & SPI_TX_QUAD)
-				dmode = STM32_QSPI_CCR_DMODE_FOUR_LINE;
-			else if (mode & SPI_TX_DUAL)
-				dmode = STM32_QSPI_CCR_DMODE_TWO_LINE;
-		} else if ((fmode == STM32_QSPI_CCR_MEM_MAP) ||
-			 (fmode == STM32_QSPI_CCR_IND_READ)) {
-			if (mode & SPI_RX_QUAD)
-				dmode = STM32_QSPI_CCR_DMODE_FOUR_LINE;
-			else if (mode & SPI_RX_DUAL)
-				dmode = STM32_QSPI_CCR_DMODE_TWO_LINE;
-		}
-	}
-
-	if (priv->command & CMD_HAS_DATA)
-		ccr_reg |= (dmode << STM32_QSPI_CCR_DMODE_SHIFT);
-
-	if (priv->command & CMD_HAS_DUMMY)
-		ccr_reg |= ((priv->dummycycles & STM32_QSPI_CCR_DCYC_MASK)
-				<< STM32_QSPI_CCR_DCYC_SHIFT);
-
-	if (priv->command & CMD_HAS_ADR) {
-		ccr_reg |= (STM32_QSPI_CCR_ADSIZE_24BIT
-				<< STM32_QSPI_CCR_ADSIZE_SHIFT);
-		ccr_reg |= (admode << STM32_QSPI_CCR_ADMODE_SHIFT);
-	}
-
-	ccr_reg |= (fmode << STM32_QSPI_CCR_FMODE_SHIFT);
-	ccr_reg |= (imode << STM32_QSPI_CCR_IMODE_SHIFT);
-	ccr_reg |= cmd;
-
-	return ccr_reg;
+	writeb(*val, addr);
 }
 
-static void _stm32_qspi_enable_mmap(struct stm32_qspi_priv *priv,
-				    struct spi_flash *flash)
+static int _stm32_qspi_poll(struct stm32_qspi_priv *priv,
+			    const struct spi_mem_op *op)
 {
-	unsigned int ccr_reg;
+	void (*fifo)(u8 *val, void __iomem *addr);
+	u32 len = op->data.nbytes, sr;
+	u8 *buf;
+	int ret;
 
-	priv->command = flash->read_opcode | CMD_HAS_ADR | CMD_HAS_DATA
-			| CMD_HAS_DUMMY;
-	priv->dummycycles = flash->read_dummy;
+	if (op->data.dir == SPI_MEM_DATA_IN) {
+		fifo = _stm32_qspi_read_fifo;
+		buf = op->data.buf.in;
 
-	ccr_reg = _stm32_qspi_gen_ccr(priv, STM32_QSPI_CCR_MEM_MAP);
+	} else {
+		fifo = _stm32_qspi_write_fifo;
+		buf = (u8 *)op->data.buf.out;
+	}
 
-	_stm32_qspi_wait_for_not_busy(priv);
+	while (len--) {
+		ret = readl_poll_timeout(&priv->regs->sr, sr,
+					 sr & STM32_QSPI_SR_FTF,
+					 STM32_QSPI_FIFO_TIMEOUT_US);
+		if (ret) {
+			pr_err("fifo timeout (len:%d stat:%#x)\n", len, sr);
+			return ret;
+		}
 
-	writel(ccr_reg, &priv->regs->ccr);
+		fifo(buf++, &priv->regs->dr);
+	}
 
-	priv->dummycycles = 0;
+	return 0;
 }
 
-static void _stm32_qspi_disable_mmap(struct stm32_qspi_priv *priv)
+static int stm32_qspi_mm(struct stm32_qspi_priv *priv,
+			 const struct spi_mem_op *op)
 {
-	setbits_le32(&priv->regs->cr, STM32_QSPI_CR_ABORT);
-}
+	memcpy_fromio(op->data.buf.in, priv->mm_base + op->addr.val,
+		      op->data.nbytes);
 
-static void _stm32_qspi_set_xfer_length(struct stm32_qspi_priv *priv,
-					u32 length)
-{
-	writel(length - 1, &priv->regs->dlr);
+	return 0;
 }
 
-static void _stm32_qspi_start_xfer(struct stm32_qspi_priv *priv, u32 cr_reg)
+static int _stm32_qspi_tx(struct stm32_qspi_priv *priv,
+			  const struct spi_mem_op *op,
+			  u8 mode)
 {
-	writel(cr_reg, &priv->regs->ccr);
+	if (!op->data.nbytes)
+		return 0;
+
+	if (mode == STM32_QSPI_CCR_MEM_MAP)
+		return stm32_qspi_mm(priv, op);
 
-	if (priv->command & CMD_HAS_ADR)
-		writel(priv->address, &priv->regs->ar);
+	return _stm32_qspi_poll(priv, op);
 }
 
-static int _stm32_qspi_xfer(struct stm32_qspi_priv *priv,
-			    struct spi_flash *flash, unsigned int bitlen,
-			    const u8 *dout, u8 *din, unsigned long flags)
+static int _stm32_qspi_get_mode(u8 buswidth)
 {
-	unsigned int words = bitlen / 8;
-	u32 ccr_reg;
-	int i;
+	if (buswidth == 4)
+		return 3;
 
-	if (flags & SPI_XFER_MMAP) {
-		_stm32_qspi_enable_mmap(priv, flash);
-		return 0;
-	} else if (flags & SPI_XFER_MMAP_END) {
-		_stm32_qspi_disable_mmap(priv);
-		return 0;
-	}
-
-	if (bitlen == 0)
-		return -1;
+	return buswidth;
+}
 
-	if (bitlen % 8) {
-		debug("spi_xfer: Non byte aligned SPI transfer\n");
-		return -1;
-	}
+static int stm32_qspi_exec_op(struct spi_slave *slave,
+			      const struct spi_mem_op *op)
+{
+	struct stm32_qspi_priv *priv = dev_get_priv(slave->dev->parent);
+	u32 cr, ccr, addr_max;
+	u8 mode = STM32_QSPI_CCR_IND_WRITE;
+	int timeout, ret;
+
+	debug("%s: cmd:%#x mode:%d.%d.%d.%d addr:%#llx len:%#x\n",
+	      __func__, op->cmd.opcode, op->cmd.buswidth, op->addr.buswidth,
+	      op->dummy.buswidth, op->data.buswidth,
+	      op->addr.val, op->data.nbytes);
+
+	ret = _stm32_qspi_wait_for_not_busy(priv);
+	if (ret)
+		return ret;
 
-	if (dout && din) {
-		debug("spi_xfer: QSPI cannot have data in and data out set\n");
-		return -1;
-	}
+	addr_max = op->addr.val + op->data.nbytes + 1;
 
-	if (!dout && (flags & SPI_XFER_BEGIN)) {
-		debug("spi_xfer: QSPI transfer must begin with command\n");
-		return -1;
+	if (op->data.dir == SPI_MEM_DATA_IN && op->data.nbytes) {
+		if (addr_max < priv->mm_size && op->addr.buswidth)
+			mode = STM32_QSPI_CCR_MEM_MAP;
+		else
+			mode = STM32_QSPI_CCR_IND_READ;
 	}
 
-	if (dout) {
-		if (flags & SPI_XFER_BEGIN) {
-			/* data is command */
-			priv->command = dout[0] | CMD_HAS_DATA;
-			if (words >= 4) {
-				/* address is here too */
-				priv->address = (dout[1] << 16) |
-						(dout[2] << 8) | dout[3];
-				priv->command |= CMD_HAS_ADR;
-			}
-
-			if (words > 4) {
-				/* rest is dummy bytes */
-				priv->dummycycles = (words - 4) * 8;
-				priv->command |= CMD_HAS_DUMMY;
-			}
-
-			if (flags & SPI_XFER_END) {
-				/* command without data */
-				priv->command &= ~(CMD_HAS_DATA);
-			}
-		}
-
-		if (flags & SPI_XFER_END) {
-			ccr_reg = _stm32_qspi_gen_ccr(priv,
-						      STM32_QSPI_CCR_IND_WRITE);
-
-			_stm32_qspi_wait_for_not_busy(priv);
-
-			if (priv->command & CMD_HAS_DATA)
-				_stm32_qspi_set_xfer_length(priv, words);
-
-			_stm32_qspi_start_xfer(priv, ccr_reg);
-
-			debug("%s: write: ccr:0x%08x adr:0x%08x\n",
-			      __func__, priv->regs->ccr, priv->regs->ar);
-
-			if (priv->command & CMD_HAS_DATA) {
-				_stm32_qspi_wait_for_ftf(priv);
-
-				debug("%s: words:%d data:", __func__, words);
+	if (op->data.nbytes)
+		writel(op->data.nbytes - 1, &priv->regs->dlr);
 
-				i = 0;
-				while (words > i) {
-					writeb(dout[i], &priv->regs->dr);
-					debug("%02x ", dout[i]);
-					i++;
-				}
-				debug("\n");
+	ccr = (mode << STM32_QSPI_CCR_FMODE_SHIFT);
+	ccr |= op->cmd.opcode;
+	ccr |= (_stm32_qspi_get_mode(op->cmd.buswidth)
+		<< STM32_QSPI_CCR_IMODE_SHIFT);
 
-				_stm32_qspi_wait_for_complete(priv);
-			} else {
-				_stm32_qspi_wait_for_not_busy(priv);
-			}
-		}
-	} else if (din) {
-		ccr_reg = _stm32_qspi_gen_ccr(priv, STM32_QSPI_CCR_IND_READ);
+	if (op->addr.nbytes) {
+		ccr |= ((op->addr.nbytes - 1) << STM32_QSPI_CCR_ADSIZE_SHIFT);
+		ccr |= (_stm32_qspi_get_mode(op->addr.buswidth)
+			<< STM32_QSPI_CCR_ADMODE_SHIFT);
+	}
 
-		_stm32_qspi_wait_for_not_busy(priv);
+	if (op->dummy.buswidth && op->dummy.nbytes)
+		ccr |= (op->dummy.nbytes * 8 / op->dummy.buswidth
+			<< STM32_QSPI_CCR_DCYC_SHIFT);
 
-		_stm32_qspi_set_xfer_length(priv, words);
+	if (op->data.nbytes)
+		ccr |= (_stm32_qspi_get_mode(op->data.buswidth)
+			<< STM32_QSPI_CCR_DMODE_SHIFT);
 
-		_stm32_qspi_start_xfer(priv, ccr_reg);
+	writel(ccr, &priv->regs->ccr);
 
-		debug("%s: read: ccr:0x%08x adr:0x%08x len:%d\n", __func__,
-		      priv->regs->ccr, priv->regs->ar, priv->regs->dlr);
+	if (op->addr.nbytes && mode != STM32_QSPI_CCR_MEM_MAP)
+		writel(op->addr.val, &priv->regs->ar);
 
-		debug("%s: data:", __func__);
+	ret = _stm32_qspi_tx(priv, op, mode);
+	/*
+	 * Abort in:
+	 * -error case
+	 * -read memory map: prefetching must be stopped if we read the last
+	 *  byte of device (device size - fifo size). like device size is not
+	 *  knows, the prefetching is always stop.
+	 */
+	if (ret || mode == STM32_QSPI_CCR_MEM_MAP)
+		goto abort;
 
-		i = 0;
-		while (words > i) {
-			din[i] = readb(&priv->regs->dr);
-			debug("%02x ", din[i]);
-			i++;
-		}
-		debug("\n");
-	}
+	/* Wait end of tx in indirect mode */
+	ret = _stm32_qspi_wait_cmd(priv, op);
+	if (ret)
+		goto abort;
 
 	return 0;
-}
-
-static int stm32_qspi_ofdata_to_platdata(struct udevice *bus)
-{
-	struct resource res_regs, res_mem;
-	struct stm32_qspi_platdata *plat = bus->platdata;
-	int ret;
 
-	ret = dev_read_resource_byname(bus, "qspi", &res_regs);
-	if (ret) {
-		debug("Error: can't get regs base addresses(ret = %d)!\n", ret);
-		return -ENOMEM;
-	}
-	ret = dev_read_resource_byname(bus, "qspi_mm", &res_mem);
-	if (ret) {
-		debug("Error: can't get mmap base address(ret = %d)!\n", ret);
-		return -ENOMEM;
-	}
+abort:
+	setbits_le32(&priv->regs->cr, STM32_QSPI_CR_ABORT);
 
-	plat->max_hz = dev_read_u32_default(bus, "spi-max-frequency",
-					    STM32_QSPI_DEFAULT_SCK_FREQ);
+	/* Wait clear of abort bit by hw */
+	timeout = readl_poll_timeout(&priv->regs->cr, cr,
+				     !(cr & STM32_QSPI_CR_ABORT),
+				     STM32_ABT_TIMEOUT_US);
 
-	plat->base = res_regs.start;
-	plat->memory_map = res_mem.start;
+	writel(STM32_QSPI_FCR_CTCF, &priv->regs->fcr);
 
-	debug("%s: regs=<0x%x> mapped=<0x%x>, max-frequency=%d\n",
-	      __func__,
-	      plat->base,
-	      plat->memory_map,
-	      plat->max_hz
-	      );
+	if (ret || timeout)
+		pr_err("%s ret:%d abort timeout:%d\n", __func__, ret, timeout);
 
-	return 0;
+	return ret;
 }
 
 static int stm32_qspi_probe(struct udevice *bus)
 {
-	struct stm32_qspi_platdata *plat = dev_get_platdata(bus);
 	struct stm32_qspi_priv *priv = dev_get_priv(bus);
-	struct dm_spi_bus *dm_spi_bus;
+	struct resource res;
 	struct clk clk;
 	struct reset_ctl reset_ctl;
 	int ret;
 
-	dm_spi_bus = bus->uclass_priv;
+	ret = dev_read_resource_byname(bus, "qspi", &res);
+	if (ret) {
+		dev_err(bus, "can't get regs base addresses(ret = %d)!\n", ret);
+		return ret;
+	}
 
-	dm_spi_bus->max_hz = plat->max_hz;
+	priv->regs = (struct stm32_qspi_regs *)res.start;
 
-	priv->regs = (struct stm32_qspi_regs *)(uintptr_t)plat->base;
+	ret = dev_read_resource_byname(bus, "qspi_mm", &res);
+	if (ret) {
+		dev_err(bus, "can't get mmap base address(ret = %d)!\n", ret);
+		return ret;
+	}
 
-	priv->max_hz = plat->max_hz;
+	priv->mm_base = (void __iomem *)res.start;
+
+	priv->mm_size = resource_size(&res);
+	if (priv->mm_size > STM32_QSPI_MAX_MMAP_SZ)
+		return -EINVAL;
+
+	debug("%s: regs=<0x%p> mapped=<0x%p> mapped_size=<0x%lx>\n",
+	      __func__, priv->regs, priv->mm_base, priv->mm_size);
 
 	ret = clk_get_by_index(bus, 0, &clk);
 	if (ret < 0)
 		return ret;
 
 	ret = clk_enable(&clk);
-
 	if (ret) {
 		dev_err(bus, "failed to enable clock\n");
 		return ret;
@@ -499,78 +380,68 @@ static int stm32_qspi_probe(struct udevice *bus)
 		reset_deassert(&reset_ctl);
 	}
 
+	priv->cs_used = -1;
+
 	setbits_le32(&priv->regs->cr, STM32_QSPI_CR_SSHIFT);
 
-	return 0;
-}
+	/* Set dcr fsize to max address */
+	setbits_le32(&priv->regs->dcr,
+		     STM32_QSPI_DCR_FSIZE_MASK << STM32_QSPI_DCR_FSIZE_SHIFT);
 
-static int stm32_qspi_remove(struct udevice *bus)
-{
 	return 0;
 }
 
 static int stm32_qspi_claim_bus(struct udevice *dev)
 {
-	struct stm32_qspi_priv *priv;
-	struct udevice *bus;
-	struct spi_flash *flash;
-	struct dm_spi_slave_platdata *slave_plat;
+	struct stm32_qspi_priv *priv = dev_get_priv(dev->parent);
+	struct dm_spi_slave_platdata *slave_plat = dev_get_parent_platdata(dev);
 
-	bus = dev->parent;
-	priv = dev_get_priv(bus);
-	flash = dev_get_uclass_priv(dev);
-	slave_plat = dev_get_parent_platdata(dev);
-
-	if (slave_plat->cs >= STM32_MAX_NORCHIP)
+	if (slave_plat->cs >= STM32_QSPI_MAX_CHIP)
 		return -ENODEV;
 
-	_stm32_qspi_set_cs(priv, slave_plat->cs);
-
-	_stm32_qspi_set_flash_size(priv, flash->size);
+	if (priv->cs_used != slave_plat->cs) {
+		struct stm32_qspi_flash *flash = &priv->flash[slave_plat->cs];
 
-	_stm32_qspi_enable(priv);
+		priv->cs_used = slave_plat->cs;
 
-	return 0;
-}
+		if (flash->initialized) {
+			/* Set the configuration: speed + cs */
+			writel(flash->cr, &priv->regs->cr);
+			writel(flash->dcr, &priv->regs->dcr);
+		} else {
+			/* Set chip select */
+			clrsetbits_le32(&priv->regs->cr, STM32_QSPI_CR_FSEL,
+					priv->cs_used ? STM32_QSPI_CR_FSEL : 0);
 
-static int stm32_qspi_release_bus(struct udevice *dev)
-{
-	struct stm32_qspi_priv *priv;
-	struct udevice *bus;
+			/* Save the configuration: speed + cs */
+			flash->cr = readl(&priv->regs->cr);
+			flash->dcr = readl(&priv->regs->dcr);
 
-	bus = dev->parent;
-	priv = dev_get_priv(bus);
+			flash->initialized = true;
+		}
+	}
 
-	_stm32_qspi_disable(priv);
+	setbits_le32(&priv->regs->cr, STM32_QSPI_CR_EN);
 
 	return 0;
 }
 
-static int stm32_qspi_xfer(struct udevice *dev, unsigned int bitlen,
-			   const void *dout, void *din, unsigned long flags)
+static int stm32_qspi_release_bus(struct udevice *dev)
 {
-	struct stm32_qspi_priv *priv;
-	struct udevice *bus;
-	struct spi_flash *flash;
+	struct stm32_qspi_priv *priv = dev_get_priv(dev->parent);
 
-	bus = dev->parent;
-	priv = dev_get_priv(bus);
-	flash = dev_get_uclass_priv(dev);
+	clrbits_le32(&priv->regs->cr, STM32_QSPI_CR_EN);
 
-	return _stm32_qspi_xfer(priv, flash, bitlen, (const u8 *)dout,
-				(u8 *)din, flags);
+	return 0;
 }
 
 static int stm32_qspi_set_speed(struct udevice *bus, uint speed)
 {
-	struct stm32_qspi_platdata *plat = bus->platdata;
 	struct stm32_qspi_priv *priv = dev_get_priv(bus);
 	u32 qspi_clk = priv->clock_rate;
 	u32 prescaler = 255;
 	u32 csht;
-
-	if (speed > plat->max_hz)
-		speed = plat->max_hz;
+	int ret;
 
 	if (speed > 0) {
 		prescaler = DIV_ROUND_UP(qspi_clk, speed) - 1;
@@ -583,7 +454,9 @@ static int stm32_qspi_set_speed(struct udevice *bus, uint speed)
 	csht = DIV_ROUND_UP((5 * qspi_clk) / (prescaler + 1), 100000000);
 	csht = (csht - 1) & STM32_QSPI_DCR_CSHT_MASK;
 
-	_stm32_qspi_wait_for_not_busy(priv);
+	ret = _stm32_qspi_wait_for_not_busy(priv);
+	if (ret)
+		return ret;
 
 	clrsetbits_le32(&priv->regs->cr,
 			STM32_QSPI_CR_PRESCALER_MASK <<
@@ -603,8 +476,11 @@ static int stm32_qspi_set_speed(struct udevice *bus, uint speed)
 static int stm32_qspi_set_mode(struct udevice *bus, uint mode)
 {
 	struct stm32_qspi_priv *priv = dev_get_priv(bus);
+	int ret;
 
-	_stm32_qspi_wait_for_not_busy(priv);
+	ret = _stm32_qspi_wait_for_not_busy(priv);
+	if (ret)
+		return ret;
 
 	if ((mode & SPI_CPHA) && (mode & SPI_CPOL))
 		setbits_le32(&priv->regs->dcr, STM32_QSPI_DCR_CKMODE);
@@ -616,20 +492,6 @@ static int stm32_qspi_set_mode(struct udevice *bus, uint mode)
 	if (mode & SPI_CS_HIGH)
 		return -ENODEV;
 
-	if (mode & SPI_RX_QUAD)
-		priv->mode |= SPI_RX_QUAD;
-	else if (mode & SPI_RX_DUAL)
-		priv->mode |= SPI_RX_DUAL;
-	else
-		priv->mode &= ~(SPI_RX_QUAD | SPI_RX_DUAL);
-
-	if (mode & SPI_TX_QUAD)
-		priv->mode |= SPI_TX_QUAD;
-	else if (mode & SPI_TX_DUAL)
-		priv->mode |= SPI_TX_DUAL;
-	else
-		priv->mode &= ~(SPI_TX_QUAD | SPI_TX_DUAL);
-
 	debug("%s: regs=%p, mode=%d rx: ", __func__, priv->regs, mode);
 
 	if (mode & SPI_RX_QUAD)
@@ -649,12 +511,16 @@ static int stm32_qspi_set_mode(struct udevice *bus, uint mode)
 	return 0;
 }
 
+static const struct spi_controller_mem_ops stm32_qspi_mem_ops = {
+	.exec_op = stm32_qspi_exec_op,
+};
+
 static const struct dm_spi_ops stm32_qspi_ops = {
 	.claim_bus	= stm32_qspi_claim_bus,
 	.release_bus	= stm32_qspi_release_bus,
-	.xfer		= stm32_qspi_xfer,
 	.set_speed	= stm32_qspi_set_speed,
 	.set_mode	= stm32_qspi_set_mode,
+	.mem_ops	= &stm32_qspi_mem_ops,
 };
 
 static const struct udevice_id stm32_qspi_ids[] = {
@@ -664,13 +530,10 @@ static const struct udevice_id stm32_qspi_ids[] = {
 };
 
 U_BOOT_DRIVER(stm32_qspi) = {
-	.name	= "stm32_qspi",
-	.id	= UCLASS_SPI,
+	.name = "stm32_qspi",
+	.id = UCLASS_SPI,
 	.of_match = stm32_qspi_ids,
-	.ops	= &stm32_qspi_ops,
-	.ofdata_to_platdata = stm32_qspi_ofdata_to_platdata,
-	.platdata_auto_alloc_size = sizeof(struct stm32_qspi_platdata),
+	.ops = &stm32_qspi_ops,
 	.priv_auto_alloc_size = sizeof(struct stm32_qspi_priv),
-	.probe	= stm32_qspi_probe,
-	.remove = stm32_qspi_remove,
+	.probe = stm32_qspi_probe,
 };