Merge tag 'u-boot-stm32-20190412' of https://github.com/patrickdelaunay/u-boot

stm32 patches for v2019.07-rc1
- Add trusted boot with TF-A for stm32mp1
- stm32mp1 dts files sync'ed with Linux version
- add STM32MP1 Discovery boards (DK1 and DK2)
- add STMFX gpio expander driver
- misc improvement for stm3mp1 supports
- rename stpmu1 to stpmic1 (official name)
- stm32_qspi: move to exec_op (spi nor driver for stm32 mpu and mcu)
- add STM32 FMC2 NAND flash controller driver

19 files changed, 2774 insertions, 1159 deletions

diff --git a/drivers/clk/clk_stm32mp1.c b/drivers/clk/clk_stm32mp1.c
index aebc6f0a34..24859fd054 100644
--- a/drivers/clk/clk_stm32mp1.c
+++ b/drivers/clk/clk_stm32mp1.c
@@ -15,10 +15,12 @@
 #include <dt-bindings/clock/stm32mp1-clks.h>
 #include <dt-bindings/clock/stm32mp1-clksrc.h>
 
+#ifndef CONFIG_STM32MP1_TRUSTED
 #if !defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD)
 /* activate clock tree initialization in the driver */
 #define STM32MP1_CLOCK_TREE_INIT
 #endif
+#endif
 
 #define MAX_HSI_HZ		64000000
 
diff --git a/drivers/misc/stm32mp_fuse.c b/drivers/misc/stm32mp_fuse.c
index 33943a231b..8dc246b0db 100644
--- a/drivers/misc/stm32mp_fuse.c
+++ b/drivers/misc/stm32mp_fuse.c
@@ -9,8 +9,10 @@
 #include <errno.h>
 #include <dm/device.h>
 #include <dm/uclass.h>
+#include <power/stpmic1.h>
 
 #define STM32MP_OTP_BANK	0
+#define STM32MP_NVM_BANK	1
 
 /*
  * The 'fuse' command API
@@ -34,6 +36,13 @@ int fuse_read(u32 bank, u32 word, u32 *val)
 		ret = 0;
 		break;
 
+#ifdef CONFIG_PMIC_STPMIC1
+	case STM32MP_NVM_BANK:
+		*val = 0;
+		ret = stpmic1_shadow_read_byte(word, (u8 *)val);
+		break;
+#endif /* CONFIG_PMIC_STPMIC1 */
+
 	default:
 		printf("stm32mp %s: wrong value for bank %i\n", __func__, bank);
 		ret = -EINVAL;
@@ -62,6 +71,12 @@ int fuse_prog(u32 bank, u32 word, u32 val)
 		ret = 0;
 		break;
 
+#ifdef CONFIG_PMIC_STPMIC1
+	case STM32MP_NVM_BANK:
+		ret = stpmic1_nvm_write_byte(word, (u8 *)&val);
+		break;
+#endif /* CONFIG_PMIC_STPMIC1 */
+
 	default:
 		printf("stm32mp %s: wrong value for bank %i\n", __func__, bank);
 		ret = -EINVAL;
@@ -89,6 +104,13 @@ int fuse_sense(u32 bank, u32 word, u32 *val)
 		ret = 0;
 		break;
 
+#ifdef CONFIG_PMIC_STPMIC1
+	case STM32MP_NVM_BANK:
+		*val = 0;
+		ret = stpmic1_nvm_read_byte(word, (u8 *)val);
+		break;
+#endif /* CONFIG_PMIC_STPMIC1 */
+
 	default:
 		printf("stm32mp %s: wrong value for bank %i\n", __func__, bank);
 		ret = -EINVAL;
@@ -117,6 +139,12 @@ int fuse_override(u32 bank, u32 word, u32 val)
 		ret = 0;
 		break;
 
+#ifdef CONFIG_PMIC_STPMIC1
+	case STM32MP_NVM_BANK:
+		ret = stpmic1_shadow_write_byte(word, (u8 *)&val);
+		break;
+#endif /* CONFIG_PMIC_STPMIC1 */
+
 	default:
 		printf("stm32mp %s: wrong value for bank %i\n",
 		       __func__, bank);
diff --git a/drivers/mtd/nand/raw/Kconfig b/drivers/mtd/nand/raw/Kconfig
index 7f76e5ecef..dc087ab641 100644
--- a/drivers/mtd/nand/raw/Kconfig
+++ b/drivers/mtd/nand/raw/Kconfig
@@ -256,6 +256,17 @@ config NAND_ZYNQ_USE_BOOTLOADER1_TIMINGS
 	  This flag prevent U-boot reconfigure NAND flash controller and reuse
 	  the NAND timing from 1st stage bootloader.
 
+config NAND_STM32_FMC2
+	bool "Support for NAND controller on STM32MP SoCs"
+	depends on ARCH_STM32MP
+	select SYS_NAND_SELF_INIT
+	imply CMD_NAND
+	help
+	  Enables support for NAND Flash chips on SoCs containing the FMC2
+	  NAND controller. This controller is found on STM32MP SoCs.
+	  The controller supports a maximum 8k page size and supports
+	  a maximum 8-bit correction error per sector of 512 bytes.
+
 comment "Generic NAND options"
 
 config SYS_NAND_BLOCK_SIZE
diff --git a/drivers/mtd/nand/raw/Makefile b/drivers/mtd/nand/raw/Makefile
index c61e3f3839..b10e718d15 100644
--- a/drivers/mtd/nand/raw/Makefile
+++ b/drivers/mtd/nand/raw/Makefile
@@ -65,6 +65,7 @@ obj-$(CONFIG_NAND_OMAP_ELM) += omap_elm.o
 obj-$(CONFIG_NAND_PLAT) += nand_plat.o
 obj-$(CONFIG_NAND_SUNXI) += sunxi_nand.o
 obj-$(CONFIG_NAND_ZYNQ) += zynq_nand.o
+obj-$(CONFIG_NAND_STM32_FMC2) += stm32_fmc2_nand.o
 
 else  # minimal SPL drivers
 
diff --git a/drivers/mtd/nand/raw/stm32_fmc2_nand.c b/drivers/mtd/nand/raw/stm32_fmc2_nand.c
new file mode 100644
index 0000000000..2bb749d7f7
--- /dev/null
+++ b/drivers/mtd/nand/raw/stm32_fmc2_nand.c
@@ -0,0 +1,1092 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) STMicroelectronics 2019
+ * Author: Christophe Kerello <christophe.kerello@st.com>
+ */
+
+#include <common.h>
+#include <clk.h>
+#include <dm.h>
+#include <nand.h>
+#include <reset.h>
+#include <linux/iopoll.h>
+#include <linux/ioport.h>
+
+/* Bad block marker length */
+#define FMC2_BBM_LEN			2
+
+/* ECC step size */
+#define FMC2_ECC_STEP_SIZE		512
+
+/* Command delay */
+#define FMC2_RB_DELAY_US		30
+
+/* Max chip enable */
+#define FMC2_MAX_CE			2
+
+/* Timings */
+#define FMC2_THIZ			1
+#define FMC2_TIO			8000
+#define FMC2_TSYNC			3000
+#define FMC2_PCR_TIMING_MASK		0xf
+#define FMC2_PMEM_PATT_TIMING_MASK	0xff
+
+/* FMC2 Controller Registers */
+#define FMC2_BCR1			0x0
+#define FMC2_PCR			0x80
+#define FMC2_SR				0x84
+#define FMC2_PMEM			0x88
+#define FMC2_PATT			0x8c
+#define FMC2_HECCR			0x94
+#define FMC2_BCHISR			0x254
+#define FMC2_BCHICR			0x258
+#define FMC2_BCHPBR1			0x260
+#define FMC2_BCHPBR2			0x264
+#define FMC2_BCHPBR3			0x268
+#define FMC2_BCHPBR4			0x26c
+#define FMC2_BCHDSR0			0x27c
+#define FMC2_BCHDSR1			0x280
+#define FMC2_BCHDSR2			0x284
+#define FMC2_BCHDSR3			0x288
+#define FMC2_BCHDSR4			0x28c
+
+/* Register: FMC2_BCR1 */
+#define FMC2_BCR1_FMC2EN		BIT(31)
+
+/* Register: FMC2_PCR */
+#define FMC2_PCR_PWAITEN		BIT(1)
+#define FMC2_PCR_PBKEN			BIT(2)
+#define FMC2_PCR_PWID_MASK		GENMASK(5, 4)
+#define FMC2_PCR_PWID(x)		(((x) & 0x3) << 4)
+#define FMC2_PCR_PWID_BUSWIDTH_8	0
+#define FMC2_PCR_PWID_BUSWIDTH_16	1
+#define FMC2_PCR_ECCEN			BIT(6)
+#define FMC2_PCR_ECCALG			BIT(8)
+#define FMC2_PCR_TCLR_MASK		GENMASK(12, 9)
+#define FMC2_PCR_TCLR(x)		(((x) & 0xf) << 9)
+#define FMC2_PCR_TCLR_DEFAULT		0xf
+#define FMC2_PCR_TAR_MASK		GENMASK(16, 13)
+#define FMC2_PCR_TAR(x)			(((x) & 0xf) << 13)
+#define FMC2_PCR_TAR_DEFAULT		0xf
+#define FMC2_PCR_ECCSS_MASK		GENMASK(19, 17)
+#define FMC2_PCR_ECCSS(x)		(((x) & 0x7) << 17)
+#define FMC2_PCR_ECCSS_512		1
+#define FMC2_PCR_ECCSS_2048		3
+#define FMC2_PCR_BCHECC			BIT(24)
+#define FMC2_PCR_WEN			BIT(25)
+
+/* Register: FMC2_SR */
+#define FMC2_SR_NWRF			BIT(6)
+
+/* Register: FMC2_PMEM */
+#define FMC2_PMEM_MEMSET(x)		(((x) & 0xff) << 0)
+#define FMC2_PMEM_MEMWAIT(x)		(((x) & 0xff) << 8)
+#define FMC2_PMEM_MEMHOLD(x)		(((x) & 0xff) << 16)
+#define FMC2_PMEM_MEMHIZ(x)		(((x) & 0xff) << 24)
+#define FMC2_PMEM_DEFAULT		0x0a0a0a0a
+
+/* Register: FMC2_PATT */
+#define FMC2_PATT_ATTSET(x)		(((x) & 0xff) << 0)
+#define FMC2_PATT_ATTWAIT(x)		(((x) & 0xff) << 8)
+#define FMC2_PATT_ATTHOLD(x)		(((x) & 0xff) << 16)
+#define FMC2_PATT_ATTHIZ(x)		(((x) & 0xff) << 24)
+#define FMC2_PATT_DEFAULT		0x0a0a0a0a
+
+/* Register: FMC2_BCHISR */
+#define FMC2_BCHISR_DERF		BIT(1)
+#define FMC2_BCHISR_EPBRF		BIT(4)
+
+/* Register: FMC2_BCHICR */
+#define FMC2_BCHICR_CLEAR_IRQ		GENMASK(4, 0)
+
+/* Register: FMC2_BCHDSR0 */
+#define FMC2_BCHDSR0_DUE		BIT(0)
+#define FMC2_BCHDSR0_DEF		BIT(1)
+#define FMC2_BCHDSR0_DEN_MASK		GENMASK(7, 4)
+#define FMC2_BCHDSR0_DEN_SHIFT		4
+
+/* Register: FMC2_BCHDSR1 */
+#define FMC2_BCHDSR1_EBP1_MASK		GENMASK(12, 0)
+#define FMC2_BCHDSR1_EBP2_MASK		GENMASK(28, 16)
+#define FMC2_BCHDSR1_EBP2_SHIFT		16
+
+/* Register: FMC2_BCHDSR2 */
+#define FMC2_BCHDSR2_EBP3_MASK		GENMASK(12, 0)
+#define FMC2_BCHDSR2_EBP4_MASK		GENMASK(28, 16)
+#define FMC2_BCHDSR2_EBP4_SHIFT		16
+
+/* Register: FMC2_BCHDSR3 */
+#define FMC2_BCHDSR3_EBP5_MASK		GENMASK(12, 0)
+#define FMC2_BCHDSR3_EBP6_MASK		GENMASK(28, 16)
+#define FMC2_BCHDSR3_EBP6_SHIFT		16
+
+/* Register: FMC2_BCHDSR4 */
+#define FMC2_BCHDSR4_EBP7_MASK		GENMASK(12, 0)
+#define FMC2_BCHDSR4_EBP8_MASK		GENMASK(28, 16)
+#define FMC2_BCHDSR4_EBP8_SHIFT		16
+
+#define FMC2_NSEC_PER_SEC		1000000000L
+
+enum stm32_fmc2_ecc {
+	FMC2_ECC_HAM = 1,
+	FMC2_ECC_BCH4 = 4,
+	FMC2_ECC_BCH8 = 8
+};
+
+struct stm32_fmc2_timings {
+	u8 tclr;
+	u8 tar;
+	u8 thiz;
+	u8 twait;
+	u8 thold_mem;
+	u8 tset_mem;
+	u8 thold_att;
+	u8 tset_att;
+};
+
+struct stm32_fmc2_nand {
+	struct nand_chip chip;
+	struct stm32_fmc2_timings timings;
+	int ncs;
+	int cs_used[FMC2_MAX_CE];
+};
+
+static inline struct stm32_fmc2_nand *to_fmc2_nand(struct nand_chip *chip)
+{
+	return container_of(chip, struct stm32_fmc2_nand, chip);
+}
+
+struct stm32_fmc2_nfc {
+	struct nand_hw_control base;
+	struct stm32_fmc2_nand nand;
+	struct nand_ecclayout ecclayout;
+	void __iomem *io_base;
+	void __iomem *data_base[FMC2_MAX_CE];
+	void __iomem *cmd_base[FMC2_MAX_CE];
+	void __iomem *addr_base[FMC2_MAX_CE];
+	struct clk clk;
+
+	u8 cs_assigned;
+	int cs_sel;
+};
+
+static inline struct stm32_fmc2_nfc *to_stm32_nfc(struct nand_hw_control *base)
+{
+	return container_of(base, struct stm32_fmc2_nfc, base);
+}
+
+/* Timings configuration */
+static void stm32_fmc2_timings_init(struct nand_chip *chip)
+{
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+	struct stm32_fmc2_nand *nand = to_fmc2_nand(chip);
+	struct stm32_fmc2_timings *timings = &nand->timings;
+	u32 pcr = readl(fmc2->io_base + FMC2_PCR);
+	u32 pmem, patt;
+
+	/* Set tclr/tar timings */
+	pcr &= ~FMC2_PCR_TCLR_MASK;
+	pcr |= FMC2_PCR_TCLR(timings->tclr);
+	pcr &= ~FMC2_PCR_TAR_MASK;
+	pcr |= FMC2_PCR_TAR(timings->tar);
+
+	/* Set tset/twait/thold/thiz timings in common bank */
+	pmem = FMC2_PMEM_MEMSET(timings->tset_mem);
+	pmem |= FMC2_PMEM_MEMWAIT(timings->twait);
+	pmem |= FMC2_PMEM_MEMHOLD(timings->thold_mem);
+	pmem |= FMC2_PMEM_MEMHIZ(timings->thiz);
+
+	/* Set tset/twait/thold/thiz timings in attribut bank */
+	patt = FMC2_PATT_ATTSET(timings->tset_att);
+	patt |= FMC2_PATT_ATTWAIT(timings->twait);
+	patt |= FMC2_PATT_ATTHOLD(timings->thold_att);
+	patt |= FMC2_PATT_ATTHIZ(timings->thiz);
+
+	writel(pcr, fmc2->io_base + FMC2_PCR);
+	writel(pmem, fmc2->io_base + FMC2_PMEM);
+	writel(patt, fmc2->io_base + FMC2_PATT);
+}
+
+/* Controller configuration */
+static void stm32_fmc2_setup(struct nand_chip *chip)
+{
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+	u32 pcr = readl(fmc2->io_base + FMC2_PCR);
+
+	/* Configure ECC algorithm (default configuration is Hamming) */
+	pcr &= ~FMC2_PCR_ECCALG;
+	pcr &= ~FMC2_PCR_BCHECC;
+	if (chip->ecc.strength == FMC2_ECC_BCH8) {
+		pcr |= FMC2_PCR_ECCALG;
+		pcr |= FMC2_PCR_BCHECC;
+	} else if (chip->ecc.strength == FMC2_ECC_BCH4) {
+		pcr |= FMC2_PCR_ECCALG;
+	}
+
+	/* Set buswidth */
+	pcr &= ~FMC2_PCR_PWID_MASK;
+	if (chip->options & NAND_BUSWIDTH_16)
+		pcr |= FMC2_PCR_PWID(FMC2_PCR_PWID_BUSWIDTH_16);
+
+	/* Set ECC sector size */
+	pcr &= ~FMC2_PCR_ECCSS_MASK;
+	pcr |= FMC2_PCR_ECCSS(FMC2_PCR_ECCSS_512);
+
+	writel(pcr, fmc2->io_base + FMC2_PCR);
+}
+
+/* Select target */
+static void stm32_fmc2_select_chip(struct mtd_info *mtd, int chipnr)
+{
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+	struct stm32_fmc2_nand *nand = to_fmc2_nand(chip);
+
+	if (chipnr < 0 || chipnr >= nand->ncs)
+		return;
+
+	if (nand->cs_used[chipnr] == fmc2->cs_sel)
+		return;
+
+	fmc2->cs_sel = nand->cs_used[chipnr];
+	chip->IO_ADDR_R = fmc2->data_base[fmc2->cs_sel];
+	chip->IO_ADDR_W = fmc2->data_base[fmc2->cs_sel];
+
+	/* FMC2 setup routine */
+	stm32_fmc2_setup(chip);
+
+	/* Apply timings */
+	stm32_fmc2_timings_init(chip);
+}
+
+/* Set bus width to 16-bit or 8-bit */
+static void stm32_fmc2_set_buswidth_16(struct stm32_fmc2_nfc *fmc2, bool set)
+{
+	u32 pcr = readl(fmc2->io_base + FMC2_PCR);
+
+	pcr &= ~FMC2_PCR_PWID_MASK;
+	if (set)
+		pcr |= FMC2_PCR_PWID(FMC2_PCR_PWID_BUSWIDTH_16);
+	writel(pcr, fmc2->io_base + FMC2_PCR);
+}
+
+/* Enable/disable ECC */
+static void stm32_fmc2_set_ecc(struct stm32_fmc2_nfc *fmc2, bool enable)
+{
+	u32 pcr = readl(fmc2->io_base + FMC2_PCR);
+
+	pcr &= ~FMC2_PCR_ECCEN;
+	if (enable)
+		pcr |= FMC2_PCR_ECCEN;
+	writel(pcr, fmc2->io_base + FMC2_PCR);
+}
+
+/* Clear irq sources in case of bch is used */
+static inline void stm32_fmc2_clear_bch_irq(struct stm32_fmc2_nfc *fmc2)
+{
+	writel(FMC2_BCHICR_CLEAR_IRQ, fmc2->io_base + FMC2_BCHICR);
+}
+
+/* Send command and address cycles */
+static void stm32_fmc2_cmd_ctrl(struct mtd_info *mtd, int cmd,
+				unsigned int ctrl)
+{
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+
+	if (cmd == NAND_CMD_NONE)
+		return;
+
+	if (ctrl & NAND_CLE) {
+		writeb(cmd, fmc2->cmd_base[fmc2->cs_sel]);
+		return;
+	}
+
+	writeb(cmd, fmc2->addr_base[fmc2->cs_sel]);
+}
+
+/*
+ * Enable ECC logic and reset syndrome/parity bits previously calculated
+ * Syndrome/parity bits is cleared by setting the ECCEN bit to 0
+ */
+static void stm32_fmc2_hwctl(struct mtd_info *mtd, int mode)
+{
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+
+	stm32_fmc2_set_ecc(fmc2, false);
+
+	if (chip->ecc.strength != FMC2_ECC_HAM) {
+		u32 pcr = readl(fmc2->io_base + FMC2_PCR);
+
+		if (mode == NAND_ECC_WRITE)
+			pcr |= FMC2_PCR_WEN;
+		else
+			pcr &= ~FMC2_PCR_WEN;
+		writel(pcr, fmc2->io_base + FMC2_PCR);
+
+		stm32_fmc2_clear_bch_irq(fmc2);
+	}
+
+	stm32_fmc2_set_ecc(fmc2, true);
+}
+
+/*
+ * ECC Hamming calculation
+ * ECC is 3 bytes for 512 bytes of data (supports error correction up to
+ * max of 1-bit)
+ */
+static int stm32_fmc2_ham_calculate(struct mtd_info *mtd, const u8 *data,
+				    u8 *ecc)
+{
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+	u32 heccr, sr;
+	int ret;
+
+	ret = readl_poll_timeout(fmc2->io_base + FMC2_SR, sr,
+				 sr & FMC2_SR_NWRF, 10000);
+	if (ret < 0) {
+		pr_err("Ham timeout\n");
+		return ret;
+	}
+
+	heccr = readl(fmc2->io_base + FMC2_HECCR);
+
+	ecc[0] = heccr;
+	ecc[1] = heccr >> 8;
+	ecc[2] = heccr >> 16;
+
+	/* Disable ecc */
+	stm32_fmc2_set_ecc(fmc2, false);
+
+	return 0;
+}
+
+static int stm32_fmc2_ham_correct(struct mtd_info *mtd, u8 *dat,
+				  u8 *read_ecc, u8 *calc_ecc)
+{
+	u8 bit_position = 0, b0, b1, b2;
+	u32 byte_addr = 0, b;
+	u32 i, shifting = 1;
+
+	/* Indicate which bit and byte is faulty (if any) */
+	b0 = read_ecc[0] ^ calc_ecc[0];
+	b1 = read_ecc[1] ^ calc_ecc[1];
+	b2 = read_ecc[2] ^ calc_ecc[2];
+	b = b0 | (b1 << 8) | (b2 << 16);
+
+	/* No errors */
+	if (likely(!b))
+		return 0;
+
+	/* Calculate bit position */
+	for (i = 0; i < 3; i++) {
+		switch (b % 4) {
+		case 2:
+			bit_position += shifting;
+		case 1:
+			break;
+		default:
+			return -EBADMSG;
+		}
+		shifting <<= 1;
+		b >>= 2;
+	}
+
+	/* Calculate byte position */
+	shifting = 1;
+	for (i = 0; i < 9; i++) {
+		switch (b % 4) {
+		case 2:
+			byte_addr += shifting;
+		case 1:
+			break;
+		default:
+			return -EBADMSG;
+		}
+		shifting <<= 1;
+		b >>= 2;
+	}
+
+	/* Flip the bit */
+	dat[byte_addr] ^= (1 << bit_position);
+
+	return 1;
+}
+
+/*
+ * ECC BCH calculation and correction
+ * ECC is 7/13 bytes for 512 bytes of data (supports error correction up to
+ * max of 4-bit/8-bit)
+ */
+
+static int stm32_fmc2_bch_calculate(struct mtd_info *mtd, const u8 *data,
+				    u8 *ecc)
+{
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+	u32 bchpbr, bchisr;
+	int ret;
+
+	/* Wait until the BCH code is ready */
+	ret = readl_poll_timeout(fmc2->io_base + FMC2_BCHISR, bchisr,
+				 bchisr & FMC2_BCHISR_EPBRF, 10000);
+	if (ret < 0) {
+		pr_err("Bch timeout\n");
+		return ret;
+	}
+
+	/* Read parity bits */
+	bchpbr = readl(fmc2->io_base + FMC2_BCHPBR1);
+	ecc[0] = bchpbr;
+	ecc[1] = bchpbr >> 8;
+	ecc[2] = bchpbr >> 16;
+	ecc[3] = bchpbr >> 24;
+
+	bchpbr = readl(fmc2->io_base + FMC2_BCHPBR2);
+	ecc[4] = bchpbr;
+	ecc[5] = bchpbr >> 8;
+	ecc[6] = bchpbr >> 16;
+
+	if (chip->ecc.strength == FMC2_ECC_BCH8) {
+		ecc[7] = bchpbr >> 24;
+
+		bchpbr = readl(fmc2->io_base + FMC2_BCHPBR3);
+		ecc[8] = bchpbr;
+		ecc[9] = bchpbr >> 8;
+		ecc[10] = bchpbr >> 16;
+		ecc[11] = bchpbr >> 24;
+
+		bchpbr = readl(fmc2->io_base + FMC2_BCHPBR4);
+		ecc[12] = bchpbr;
+	}
+
+	/* Disable ecc */
+	stm32_fmc2_set_ecc(fmc2, false);
+
+	return 0;
+}
+
+/* BCH algorithm correction */
+static int stm32_fmc2_bch_correct(struct mtd_info *mtd, u8 *dat,
+				  u8 *read_ecc, u8 *calc_ecc)
+{
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+	u32 bchdsr0, bchdsr1, bchdsr2, bchdsr3, bchdsr4, bchisr;
+	u16 pos[8];
+	int i, ret, den, eccsize = chip->ecc.size;
+	unsigned int nb_errs = 0;
+
+	/* Wait until the decoding error is ready */
+	ret = readl_poll_timeout(fmc2->io_base + FMC2_BCHISR, bchisr,
+				 bchisr & FMC2_BCHISR_DERF, 10000);
+	if (ret < 0) {
+		pr_err("Bch timeout\n");
+		return ret;
+	}
+
+	bchdsr0 = readl(fmc2->io_base + FMC2_BCHDSR0);
+	bchdsr1 = readl(fmc2->io_base + FMC2_BCHDSR1);
+	bchdsr2 = readl(fmc2->io_base + FMC2_BCHDSR2);
+	bchdsr3 = readl(fmc2->io_base + FMC2_BCHDSR3);
+	bchdsr4 = readl(fmc2->io_base + FMC2_BCHDSR4);
+
+	/* Disable ECC */
+	stm32_fmc2_set_ecc(fmc2, false);
+
+	/* No errors found */
+	if (likely(!(bchdsr0 & FMC2_BCHDSR0_DEF)))
+		return 0;
+
+	/* Too many errors detected */
+	if (unlikely(bchdsr0 & FMC2_BCHDSR0_DUE))
+		return -EBADMSG;
+
+	pos[0] = bchdsr1 & FMC2_BCHDSR1_EBP1_MASK;
+	pos[1] = (bchdsr1 & FMC2_BCHDSR1_EBP2_MASK) >> FMC2_BCHDSR1_EBP2_SHIFT;
+	pos[2] = bchdsr2 & FMC2_BCHDSR2_EBP3_MASK;
+	pos[3] = (bchdsr2 & FMC2_BCHDSR2_EBP4_MASK) >> FMC2_BCHDSR2_EBP4_SHIFT;
+	pos[4] = bchdsr3 & FMC2_BCHDSR3_EBP5_MASK;
+	pos[5] = (bchdsr3 & FMC2_BCHDSR3_EBP6_MASK) >> FMC2_BCHDSR3_EBP6_SHIFT;
+	pos[6] = bchdsr4 & FMC2_BCHDSR4_EBP7_MASK;
+	pos[7] = (bchdsr4 & FMC2_BCHDSR4_EBP8_MASK) >> FMC2_BCHDSR4_EBP8_SHIFT;
+
+	den = (bchdsr0 & FMC2_BCHDSR0_DEN_MASK) >> FMC2_BCHDSR0_DEN_SHIFT;
+	for (i = 0; i < den; i++) {
+		if (pos[i] < eccsize * 8) {
+			__change_bit(pos[i], (unsigned long *)dat);
+			nb_errs++;
+		}
+	}
+
+	return nb_errs;
+}
+
+static int stm32_fmc2_read_page(struct mtd_info *mtd,
+				struct nand_chip *chip, u8 *buf,
+				int oob_required, int page)
+{
+	int i, s, stat, eccsize = chip->ecc.size;
+	int eccbytes = chip->ecc.bytes;
+	int eccsteps = chip->ecc.steps;
+	int eccstrength = chip->ecc.strength;
+	u8 *p = buf;
+	u8 *ecc_calc = chip->buffers->ecccalc;
+	u8 *ecc_code = chip->buffers->ecccode;
+	unsigned int max_bitflips = 0;
+
+	for (i = mtd->writesize + FMC2_BBM_LEN, s = 0; s < eccsteps;
+	     s++, i += eccbytes, p += eccsize) {
+		chip->ecc.hwctl(mtd, NAND_ECC_READ);
+
+		/* Read the nand page sector (512 bytes) */
+		chip->cmdfunc(mtd, NAND_CMD_RNDOUT, s * eccsize, -1);
+		chip->read_buf(mtd, p, eccsize);
+
+		/* Read the corresponding ECC bytes */
+		chip->cmdfunc(mtd, NAND_CMD_RNDOUT, i, -1);
+		chip->read_buf(mtd, ecc_code, eccbytes);
+
+		/* Correct the data */
+		stat = chip->ecc.correct(mtd, p, ecc_code, ecc_calc);
+		if (stat == -EBADMSG)
+			/* Check for empty pages with bitflips */
+			stat = nand_check_erased_ecc_chunk(p, eccsize,
+							   ecc_code, eccbytes,
+							   NULL, 0,
+							   eccstrength);
+
+		if (stat < 0) {
+			mtd->ecc_stats.failed++;
+		} else {
+			mtd->ecc_stats.corrected += stat;
+			max_bitflips = max_t(unsigned int, max_bitflips, stat);
+		}
+	}
+
+	/* Read oob */
+	if (oob_required) {
+		chip->cmdfunc(mtd, NAND_CMD_RNDOUT, mtd->writesize, -1);
+		chip->read_buf(mtd, chip->oob_poi, mtd->oobsize);
+	}
+
+	return max_bitflips;
+}
+
+/* Controller initialization */
+static void stm32_fmc2_init(struct stm32_fmc2_nfc *fmc2)
+{
+	u32 pcr = readl(fmc2->io_base + FMC2_PCR);
+	u32 bcr1 = readl(fmc2->io_base + FMC2_BCR1);
+
+	/* Set CS used to undefined */
+	fmc2->cs_sel = -1;
+
+	/* Enable wait feature and nand flash memory bank */
+	pcr |= FMC2_PCR_PWAITEN;
+	pcr |= FMC2_PCR_PBKEN;
+
+	/* Set buswidth to 8 bits mode for identification */
+	pcr &= ~FMC2_PCR_PWID_MASK;
+
+	/* ECC logic is disabled */
+	pcr &= ~FMC2_PCR_ECCEN;
+
+	/* Default mode */
+	pcr &= ~FMC2_PCR_ECCALG;
+	pcr &= ~FMC2_PCR_BCHECC;
+	pcr &= ~FMC2_PCR_WEN;
+
+	/* Set default ECC sector size */
+	pcr &= ~FMC2_PCR_ECCSS_MASK;
+	pcr |= FMC2_PCR_ECCSS(FMC2_PCR_ECCSS_2048);
+
+	/* Set default tclr/tar timings */
+	pcr &= ~FMC2_PCR_TCLR_MASK;
+	pcr |= FMC2_PCR_TCLR(FMC2_PCR_TCLR_DEFAULT);
+	pcr &= ~FMC2_PCR_TAR_MASK;
+	pcr |= FMC2_PCR_TAR(FMC2_PCR_TAR_DEFAULT);
+
+	/* Enable FMC2 controller */
+	bcr1 |= FMC2_BCR1_FMC2EN;
+
+	writel(bcr1, fmc2->io_base + FMC2_BCR1);
+	writel(pcr, fmc2->io_base + FMC2_PCR);
+	writel(FMC2_PMEM_DEFAULT, fmc2->io_base + FMC2_PMEM);
+	writel(FMC2_PATT_DEFAULT, fmc2->io_base + FMC2_PATT);
+}
+
+/* Controller timings */
+static void stm32_fmc2_calc_timings(struct nand_chip *chip,
+				    const struct nand_sdr_timings *sdrt)
+{
+	struct stm32_fmc2_nfc *fmc2 = to_stm32_nfc(chip->controller);
+	struct stm32_fmc2_nand *nand = to_fmc2_nand(chip);
+	struct stm32_fmc2_timings *tims = &nand->timings;
+	unsigned long hclk = clk_get_rate(&fmc2->clk);
+	unsigned long hclkp = FMC2_NSEC_PER_SEC / (hclk / 1000);
+	int tar, tclr, thiz, twait, tset_mem, tset_att, thold_mem, thold_att;
+
+	tar = hclkp;
+	if (tar < sdrt->tAR_min)
+		tar = sdrt->tAR_min;
+	tims->tar = DIV_ROUND_UP(tar, hclkp) - 1;
+	if (tims->tar > FMC2_PCR_TIMING_MASK)
+		tims->tar = FMC2_PCR_TIMING_MASK;
+
+	tclr = hclkp;
+	if (tclr < sdrt->tCLR_min)
+		tclr = sdrt->tCLR_min;
+	tims->tclr = DIV_ROUND_UP(tclr, hclkp) - 1;
+	if (tims->tclr > FMC2_PCR_TIMING_MASK)
+		tims->tclr = FMC2_PCR_TIMING_MASK;
+
+	tims->thiz = FMC2_THIZ;
+	thiz = (tims->thiz + 1) * hclkp;
+
+	/*
+	 * tWAIT > tRP
+	 * tWAIT > tWP
+	 * tWAIT > tREA + tIO
+	 */
+	twait = hclkp;
+	if (twait < sdrt->tRP_min)
+		twait = sdrt->tRP_min;
+	if (twait < sdrt->tWP_min)
+		twait = sdrt->tWP_min;
+	if (twait < sdrt->tREA_max + FMC2_TIO)
+		twait = sdrt->tREA_max + FMC2_TIO;
+	tims->twait = DIV_ROUND_UP(twait, hclkp);
+	if (tims->twait == 0)
+		tims->twait = 1;
+	else if (tims->twait > FMC2_PMEM_PATT_TIMING_MASK)
+		tims->twait = FMC2_PMEM_PATT_TIMING_MASK;
+
+	/*
+	 * tSETUP_MEM > tCS - tWAIT
+	 * tSETUP_MEM > tALS - tWAIT
+	 * tSETUP_MEM > tDS - (tWAIT - tHIZ)
+	 */
+	tset_mem = hclkp;
+	if (sdrt->tCS_min > twait && (tset_mem < sdrt->tCS_min - twait))
+		tset_mem = sdrt->tCS_min - twait;
+	if (sdrt->tALS_min > twait && (tset_mem < sdrt->tALS_min - twait))
+		tset_mem = sdrt->tALS_min - twait;
+	if (twait > thiz && (sdrt->tDS_min > twait - thiz) &&
+	    (tset_mem < sdrt->tDS_min - (twait - thiz)))
+		tset_mem = sdrt->tDS_min - (twait - thiz);
+	tims->tset_mem = DIV_ROUND_UP(tset_mem, hclkp);
+	if (tims->tset_mem == 0)
+		tims->tset_mem = 1;
+	else if (tims->tset_mem > FMC2_PMEM_PATT_TIMING_MASK)
+		tims->tset_mem = FMC2_PMEM_PATT_TIMING_MASK;
+
+	/*
+	 * tHOLD_MEM > tCH
+	 * tHOLD_MEM > tREH - tSETUP_MEM
+	 * tHOLD_MEM > max(tRC, tWC) - (tSETUP_MEM + tWAIT)
+	 */
+	thold_mem = hclkp;
+	if (thold_mem < sdrt->tCH_min)
+		thold_mem = sdrt->tCH_min;
+	if (sdrt->tREH_min > tset_mem &&
+	    (thold_mem < sdrt->tREH_min - tset_mem))
+		thold_mem = sdrt->tREH_min - tset_mem;
+	if ((sdrt->tRC_min > tset_mem + twait) &&
+	    (thold_mem < sdrt->tRC_min - (tset_mem + twait)))
+		thold_mem = sdrt->tRC_min - (tset_mem + twait);
+	if ((sdrt->tWC_min > tset_mem + twait) &&
+	    (thold_mem < sdrt->tWC_min - (tset_mem + twait)))
+		thold_mem = sdrt->tWC_min - (tset_mem + twait);
+	tims->thold_mem = DIV_ROUND_UP(thold_mem, hclkp);
+	if (tims->thold_mem == 0)
+		tims->thold_mem = 1;
+	else if (tims->thold_mem > FMC2_PMEM_PATT_TIMING_MASK)
+		tims->thold_mem = FMC2_PMEM_PATT_TIMING_MASK;
+
+	/*
+	 * tSETUP_ATT > tCS - tWAIT
+	 * tSETUP_ATT > tCLS - tWAIT
+	 * tSETUP_ATT > tALS - tWAIT
+	 * tSETUP_ATT > tRHW - tHOLD_MEM
+	 * tSETUP_ATT > tDS - (tWAIT - tHIZ)
+	 */
+	tset_att = hclkp;
+	if (sdrt->tCS_min > twait && (tset_att < sdrt->tCS_min - twait))
+		tset_att = sdrt->tCS_min - twait;
+	if (sdrt->tCLS_min > twait && (tset_att < sdrt->tCLS_min - twait))
+		tset_att = sdrt->tCLS_min - twait;
+	if (sdrt->tALS_min > twait && (tset_att < sdrt->tALS_min - twait))
+		tset_att = sdrt->tALS_min - twait;
+	if (sdrt->tRHW_min > thold_mem &&
+	    (tset_att < sdrt->tRHW_min - thold_mem))
+		tset_att = sdrt->tRHW_min - thold_mem;
+	if (twait > thiz && (sdrt->tDS_min > twait - thiz) &&
+	    (tset_att < sdrt->tDS_min - (twait - thiz)))
+		tset_att = sdrt->tDS_min - (twait - thiz);
+	tims->tset_att = DIV_ROUND_UP(tset_att, hclkp);
+	if (tims->tset_att == 0)
+		tims->tset_att = 1;
+	else if (tims->tset_att > FMC2_PMEM_PATT_TIMING_MASK)
+		tims->tset_att = FMC2_PMEM_PATT_TIMING_MASK;
+
+	/*
+	 * tHOLD_ATT > tALH
+	 * tHOLD_ATT > tCH
+	 * tHOLD_ATT > tCLH
+	 * tHOLD_ATT > tCOH
+	 * tHOLD_ATT > tDH
+	 * tHOLD_ATT > tWB + tIO + tSYNC - tSETUP_MEM
+	 * tHOLD_ATT > tADL - tSETUP_MEM
+	 * tHOLD_ATT > tWH - tSETUP_MEM
+	 * tHOLD_ATT > tWHR - tSETUP_MEM
+	 * tHOLD_ATT > tRC - (tSETUP_ATT + tWAIT)
+	 * tHOLD_ATT > tWC - (tSETUP_ATT + tWAIT)
+	 */
+	thold_att = hclkp;
+	if (thold_att < sdrt->tALH_min)
+		thold_att = sdrt->tALH_min;
+	if (thold_att < sdrt->tCH_min)
+		thold_att = sdrt->tCH_min;
+	if (thold_att < sdrt->tCLH_min)
+		thold_att = sdrt->tCLH_min;
+	if (thold_att < sdrt->tCOH_min)
+		thold_att = sdrt->tCOH_min;
+	if (thold_att < sdrt->tDH_min)
+		thold_att = sdrt->tDH_min;
+	if ((sdrt->tWB_max + FMC2_TIO + FMC2_TSYNC > tset_mem) &&
+	    (thold_att < sdrt->tWB_max + FMC2_TIO + FMC2_TSYNC - tset_mem))
+		thold_att = sdrt->tWB_max + FMC2_TIO + FMC2_TSYNC - tset_mem;
+	if (sdrt->tADL_min > tset_mem &&
+	    (thold_att < sdrt->tADL_min - tset_mem))
+		thold_att = sdrt->tADL_min - tset_mem;
+	if (sdrt->tWH_min > tset_mem &&
+	    (thold_att < sdrt->tWH_min - tset_mem))
+		thold_att = sdrt->tWH_min - tset_mem;
+	if (sdrt->tWHR_min > tset_mem &&
+	    (thold_att < sdrt->tWHR_min - tset_mem))
+		thold_att = sdrt->tWHR_min - tset_mem;
+	if ((sdrt->tRC_min > tset_att + twait) &&
+	    (thold_att < sdrt->tRC_min - (tset_att + twait)))
+		thold_att = sdrt->tRC_min - (tset_att + twait);
+	if ((sdrt->tWC_min > tset_att + twait) &&
+	    (thold_att < sdrt->tWC_min - (tset_att + twait)))
+		thold_att = sdrt->tWC_min - (tset_att + twait);
+	tims->thold_att = DIV_ROUND_UP(thold_att, hclkp);
+	if (tims->thold_att == 0)
+		tims->thold_att = 1;
+	else if (tims->thold_att > FMC2_PMEM_PATT_TIMING_MASK)
+		tims->thold_att = FMC2_PMEM_PATT_TIMING_MASK;
+}
+
+static int stm32_fmc2_setup_interface(struct mtd_info *mtd, int chipnr,
+				      const struct nand_data_interface *conf)
+{
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	const struct nand_sdr_timings *sdrt;
+
+	sdrt = nand_get_sdr_timings(conf);
+	if (IS_ERR(sdrt))
+		return PTR_ERR(sdrt);
+
+	if (chipnr == NAND_DATA_IFACE_CHECK_ONLY)
+		return 0;
+
+	stm32_fmc2_calc_timings(chip, sdrt);
+
+	/* Apply timings */
+	stm32_fmc2_timings_init(chip);
+
+	return 0;
+}
+
+/* NAND callbacks setup */
+static void stm32_fmc2_nand_callbacks_setup(struct nand_chip *chip)
+{
+	chip->ecc.hwctl = stm32_fmc2_hwctl;
+
+	/*
+	 * Specific callbacks to read/write a page depending on
+	 * the algo used (Hamming, BCH).
+	 */
+	if (chip->ecc.strength == FMC2_ECC_HAM) {
+		/* Hamming is used */
+		chip->ecc.calculate = stm32_fmc2_ham_calculate;
+		chip->ecc.correct = stm32_fmc2_ham_correct;
+		chip->ecc.bytes = chip->options & NAND_BUSWIDTH_16 ? 4 : 3;
+		chip->ecc.options |= NAND_ECC_GENERIC_ERASED_CHECK;
+		return;
+	}
+
+	/* BCH is used */
+	chip->ecc.read_page = stm32_fmc2_read_page;
+	chip->ecc.calculate = stm32_fmc2_bch_calculate;
+	chip->ecc.correct = stm32_fmc2_bch_correct;
+
+	if (chip->ecc.strength == FMC2_ECC_BCH8)
+		chip->ecc.bytes = chip->options & NAND_BUSWIDTH_16 ? 14 : 13;
+	else
+		chip->ecc.bytes = chip->options & NAND_BUSWIDTH_16 ? 8 : 7;
+}
+
+/* FMC2 caps */
+static int stm32_fmc2_calc_ecc_bytes(int step_size, int strength)
+{
+	/* Hamming */
+	if (strength == FMC2_ECC_HAM)
+		return 4;
+
+	/* BCH8 */
+	if (strength == FMC2_ECC_BCH8)
+		return 14;
+
+	/* BCH4 */
+	return 8;
+}
+
+NAND_ECC_CAPS_SINGLE(stm32_fmc2_ecc_caps, stm32_fmc2_calc_ecc_bytes,
+		     FMC2_ECC_STEP_SIZE,
+		     FMC2_ECC_HAM, FMC2_ECC_BCH4, FMC2_ECC_BCH8);
+
+/* FMC2 probe */
+static int stm32_fmc2_parse_child(struct stm32_fmc2_nfc *fmc2,
+				  ofnode node)
+{
+	struct stm32_fmc2_nand *nand = &fmc2->nand;
+	u32 cs[FMC2_MAX_CE];
+	int ret, i;
+
+	if (!ofnode_get_property(node, "reg", &nand->ncs))
+		return -EINVAL;
+
+	nand->ncs /= sizeof(u32);
+	if (!nand->ncs) {
+		pr_err("Invalid reg property size\n");
+		return -EINVAL;
+	}
+
+	ret = ofnode_read_u32_array(node, "reg", cs, nand->ncs);
+	if (ret < 0) {
+		pr_err("Could not retrieve reg property\n");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < nand->ncs; i++) {
+		if (cs[i] > FMC2_MAX_CE) {
+			pr_err("Invalid reg value: %d\n",
+			       nand->cs_used[i]);
+			return -EINVAL;
+		}
+
+		if (fmc2->cs_assigned & BIT(cs[i])) {
+			pr_err("Cs already assigned: %d\n",
+			       nand->cs_used[i]);
+			return -EINVAL;
+		}
+
+		fmc2->cs_assigned |= BIT(cs[i]);
+		nand->cs_used[i] = cs[i];
+	}
+
+	nand->chip.flash_node = ofnode_to_offset(node);
+
+	return 0;
+}
+
+static int stm32_fmc2_parse_dt(struct udevice *dev,
+			       struct stm32_fmc2_nfc *fmc2)
+{
+	ofnode child;
+	int ret, nchips = 0;
+
+	dev_for_each_subnode(child, dev)
+		nchips++;
+
+	if (!nchips) {
+		pr_err("NAND chip not defined\n");
+		return -EINVAL;
+	}
+
+	if (nchips > 1) {
+		pr_err("Too many NAND chips defined\n");
+		return -EINVAL;
+	}
+
+	dev_for_each_subnode(child, dev) {
+		ret = stm32_fmc2_parse_child(fmc2, child);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int stm32_fmc2_probe(struct udevice *dev)
+{
+	struct stm32_fmc2_nfc *fmc2 = dev_get_priv(dev);
+	struct stm32_fmc2_nand *nand = &fmc2->nand;
+	struct nand_chip *chip = &nand->chip;
+	struct mtd_info *mtd = &chip->mtd;
+	struct nand_ecclayout *ecclayout;
+	struct resource resource;
+	struct reset_ctl reset;
+	int oob_index, chip_cs, mem_region, ret, i;
+
+	spin_lock_init(&fmc2->controller.lock);
+	init_waitqueue_head(&fmc2->controller.wq);
+
+	ret = stm32_fmc2_parse_dt(dev, fmc2);
+	if (ret)
+		return ret;
+
+	/* Get resources */
+	ret = dev_read_resource(dev, 0, &resource);
+	if (ret) {
+		pr_err("Resource io_base not found");
+		return ret;
+	}
+	fmc2->io_base = (void __iomem *)resource.start;
+
+	for (chip_cs = 0, mem_region = 1; chip_cs < FMC2_MAX_CE;
+	     chip_cs++, mem_region += 3) {
+		if (!(fmc2->cs_assigned & BIT(chip_cs)))
+			continue;
+
+		ret = dev_read_resource(dev, mem_region, &resource);
+		if (ret) {
+			pr_err("Resource data_base not found for cs%d",
+			       chip_cs);
+			return ret;
+		}
+		fmc2->data_base[chip_cs] = (void __iomem *)resource.start;
+
+		ret = dev_read_resource(dev, mem_region + 1, &resource);
+		if (ret) {
+			pr_err("Resource cmd_base not found for cs%d",
+			       chip_cs);
+			return ret;
+		}
+		fmc2->cmd_base[chip_cs] = (void __iomem *)resource.start;
+
+		ret = dev_read_resource(dev, mem_region + 2, &resource);
+		if (ret) {
+			pr_err("Resource addr_base not found for cs%d",
+			       chip_cs);
+			return ret;
+		}
+		fmc2->addr_base[chip_cs] = (void __iomem *)resource.start;
+	}
+
+	/* Enable the clock */
+	ret = clk_get_by_index(dev, 0, &fmc2->clk);
+	if (ret)
+		return ret;
+
+	ret = clk_enable(&fmc2->clk);
+	if (ret)
+		return ret;
+
+	/* Reset */
+	ret = reset_get_by_index(dev, 0, &reset);
+	if (!ret) {
+		reset_assert(&reset);
+		udelay(2);
+		reset_deassert(&reset);
+	}
+
+	/* FMC2 init routine */
+	stm32_fmc2_init(fmc2);
+
+	chip->controller = &fmc2->base;
+	chip->select_chip = stm32_fmc2_select_chip;
+	chip->setup_data_interface = stm32_fmc2_setup_interface;
+	chip->cmd_ctrl = stm32_fmc2_cmd_ctrl;
+	chip->chip_delay = FMC2_RB_DELAY_US;
+	chip->options |= NAND_BUSWIDTH_AUTO | NAND_NO_SUBPAGE_WRITE |
+			 NAND_USE_BOUNCE_BUFFER;
+
+	/* Default ECC settings */
+	chip->ecc.mode = NAND_ECC_HW;
+	chip->ecc.size = FMC2_ECC_STEP_SIZE;
+	chip->ecc.strength = FMC2_ECC_BCH8;
+
+	/* Scan to find existence of the device */
+	ret = nand_scan_ident(mtd, nand->ncs, NULL);
+	if (ret)
+		return ret;
+
+	/*
+	 * Only NAND_ECC_HW mode is actually supported
+	 * Hamming => ecc.strength = 1
+	 * BCH4 => ecc.strength = 4
+	 * BCH8 => ecc.strength = 8
+	 * ECC sector size = 512
+	 */
+	if (chip->ecc.mode != NAND_ECC_HW) {
+		pr_err("Nand_ecc_mode is not well defined in the DT\n");
+		return -EINVAL;
+	}
+
+	ret = nand_check_ecc_caps(chip, &stm32_fmc2_ecc_caps,
+				  mtd->oobsize - FMC2_BBM_LEN);
+	if (ret) {
+		pr_err("No valid ECC settings set\n");
+		return ret;
+	}
+
+	if (chip->bbt_options & NAND_BBT_USE_FLASH)
+		chip->bbt_options |= NAND_BBT_NO_OOB;
+
+	/* NAND callbacks setup */
+	stm32_fmc2_nand_callbacks_setup(chip);
+
+	/* Define ECC layout */
+	ecclayout = &fmc2->ecclayout;
+	ecclayout->eccbytes = chip->ecc.bytes *
+			      (mtd->writesize / chip->ecc.size);
+	oob_index = FMC2_BBM_LEN;
+	for (i = 0; i < ecclayout->eccbytes; i++, oob_index++)
+		ecclayout->eccpos[i] = oob_index;
+	ecclayout->oobfree->offset = oob_index;
+	ecclayout->oobfree->length = mtd->oobsize - ecclayout->oobfree->offset;
+	chip->ecc.layout = ecclayout;
+
+	/* Configure bus width to 16-bit */
+	if (chip->options & NAND_BUSWIDTH_16)
+		stm32_fmc2_set_buswidth_16(fmc2, true);
+
+	/* Scan the device to fill MTD data-structures */
+	ret = nand_scan_tail(mtd);
+	if (ret)
+		return ret;
+
+	return nand_register(0, mtd);
+}
+
+static const struct udevice_id stm32_fmc2_match[] = {
+	{ .compatible = "st,stm32mp15-fmc2" },
+	{ /* Sentinel */ }
+};
+
+U_BOOT_DRIVER(stm32_fmc2_nand) = {
+	.name = "stm32_fmc2_nand",
+	.id = UCLASS_MTD,
+	.of_match = stm32_fmc2_match,
+	.probe = stm32_fmc2_probe,
+	.priv_auto_alloc_size = sizeof(struct stm32_fmc2_nfc),
+};
+
+void board_nand_init(void)
+{
+	struct udevice *dev;
+	int ret;
+
+	ret = uclass_get_device_by_driver(UCLASS_MTD,
+					  DM_GET_DRIVER(stm32_fmc2_nand),
+					  &dev);
+	if (ret && ret != -ENODEV)
+		pr_err("Failed to initialize STM32 FMC2 NAND controller. (error %d)\n",
+		       ret);
+}
diff --git a/drivers/pinctrl/Kconfig b/drivers/pinctrl/Kconfig
index be709f73d7..a0ac167d14 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -209,6 +209,25 @@ config PINCTRL_STM32
 	  the GPIO definitions and pin control functions for each available
 	  multiplex function.
 
+config PINCTRL_STMFX
+	bool "STMicroelectronics STMFX I2C GPIO expander pinctrl driver"
+	depends on DM && PINCTRL_FULL
+	help
+	  I2C driver for STMicroelectronics Multi-Function eXpander (STMFX)
+	  GPIO expander.
+	  Supports pin multiplexing control on stm32 SoCs.
+
+	  The driver is controlled by a device tree node which contains both
+	  the GPIO definitions and pin control functions for each available
+	  multiplex function.
+
+config SPL_PINCTRL_STMFX
+	bool "STMicroelectronics STMFX I2C GPIO expander pinctrl driver in SPL"
+	depends on SPL_PINCTRL_FULL
+	help
+	  This option is an SPL-variant of the SPL_PINCTRL_STMFX option.
+	  See the help of PINCTRL_STMFX for details.
+
 config ASPEED_AST2500_PINCTRL
   bool "Aspeed AST2500 pin control driver"
   depends on DM && PINCTRL_GENERIC && ASPEED_AST2500
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index e2c2b159d8..4b080b74dc 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -22,4 +22,5 @@ obj-$(CONFIG_ARCH_MVEBU)	+= mvebu/
 obj-$(CONFIG_PINCTRL_SINGLE)	+= pinctrl-single.o
 obj-$(CONFIG_PINCTRL_STI)	+= pinctrl-sti.o
 obj-$(CONFIG_PINCTRL_STM32)	+= pinctrl_stm32.o
+obj-$(CONFIG_$(SPL_)PINCTRL_STMFX)	+= pinctrl-stmfx.o
 obj-y				+= broadcom/
diff --git a/drivers/pinctrl/pinctrl-stmfx.c b/drivers/pinctrl/pinctrl-stmfx.c
new file mode 100644
index 0000000000..5431df9813
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-stmfx.c
@@ -0,0 +1,431 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ *
+ * Driver for STMicroelectronics Multi-Function eXpander (STMFX) GPIO expander
+ * based on Linux driver : pinctrl/pinctrl-stmfx.c
+ */
+#include <common.h>
+#include <dm.h>
+#include <i2c.h>
+#include <asm/gpio.h>
+#include <dm/device.h>
+#include <dm/device-internal.h>
+#include <dm/lists.h>
+#include <dm/pinctrl.h>
+#include <linux/bitfield.h>
+#include <power/regulator.h>
+
+/* STMFX pins = GPIO[15:0] + aGPIO[7:0] */
+#define STMFX_MAX_GPIO			16
+#define STMFX_MAX_AGPIO			8
+
+/* General */
+#define STMFX_REG_CHIP_ID		0x00 /* R */
+#define STMFX_REG_FW_VERSION_MSB	0x01 /* R */
+#define STMFX_REG_FW_VERSION_LSB	0x02 /* R */
+#define STMFX_REG_SYS_CTRL		0x40 /* RW */
+
+/* MFX boot time is around 10ms, so after reset, we have to wait this delay */
+#define STMFX_BOOT_TIME_MS 10
+
+/* GPIOs expander */
+/* GPIO_STATE1 0x10, GPIO_STATE2 0x11, GPIO_STATE3 0x12 */
+#define STMFX_REG_GPIO_STATE		0x10 /* R */
+/* GPIO_DIR1 0x60, GPIO_DIR2 0x61, GPIO_DIR3 0x63 */
+#define STMFX_REG_GPIO_DIR		0x60 /* RW */
+/* GPIO_TYPE1 0x64, GPIO_TYPE2 0x65, GPIO_TYPE3 0x66 */
+#define STMFX_REG_GPIO_TYPE		0x64 /* RW */
+/* GPIO_PUPD1 0x68, GPIO_PUPD2 0x69, GPIO_PUPD3 0x6A */
+#define STMFX_REG_GPIO_PUPD		0x68 /* RW */
+/* GPO_SET1 0x6C, GPO_SET2 0x6D, GPO_SET3 0x6E */
+#define STMFX_REG_GPO_SET		0x6C /* RW */
+/* GPO_CLR1 0x70, GPO_CLR2 0x71, GPO_CLR3 0x72 */
+#define STMFX_REG_GPO_CLR		0x70 /* RW */
+
+/* STMFX_REG_CHIP_ID bitfields */
+#define STMFX_REG_CHIP_ID_MASK		GENMASK(7, 0)
+
+/* STMFX_REG_SYS_CTRL bitfields */
+#define STMFX_REG_SYS_CTRL_GPIO_EN	BIT(0)
+#define STMFX_REG_SYS_CTRL_ALTGPIO_EN	BIT(3)
+#define STMFX_REG_SYS_CTRL_SWRST	BIT(7)
+
+#define NR_GPIO_REGS			3
+#define NR_GPIOS_PER_REG		8
+#define get_reg(offset)			((offset) / NR_GPIOS_PER_REG)
+#define get_shift(offset)		((offset) % NR_GPIOS_PER_REG)
+#define get_mask(offset)		(BIT(get_shift(offset)))
+
+struct stmfx_pinctrl {
+	struct udevice *gpio;
+};
+
+static int stmfx_read(struct udevice *dev, uint offset)
+{
+	return  dm_i2c_reg_read(dev_get_parent(dev), offset);
+}
+
+static int stmfx_write(struct udevice *dev, uint offset, unsigned int val)
+{
+	return dm_i2c_reg_write(dev_get_parent(dev), offset, val);
+}
+
+static int stmfx_gpio_get(struct udevice *dev, unsigned int offset)
+{
+	u32 reg = STMFX_REG_GPIO_STATE + get_reg(offset);
+	u32 mask = get_mask(offset);
+	int ret;
+
+	ret = stmfx_read(dev, reg);
+
+	return ret < 0 ? ret : !!(ret & mask);
+}
+
+static int stmfx_gpio_set(struct udevice *dev, unsigned int offset, int value)
+{
+	u32 reg = value ? STMFX_REG_GPO_SET : STMFX_REG_GPO_CLR;
+	u32 mask = get_mask(offset);
+
+	return stmfx_write(dev, reg + get_reg(offset), mask);
+}
+
+static int stmfx_gpio_get_function(struct udevice *dev, unsigned int offset)
+{
+	u32 reg = STMFX_REG_GPIO_DIR + get_reg(offset);
+	u32 mask = get_mask(offset);
+	int ret;
+
+	ret = stmfx_read(dev, reg);
+
+	if (ret < 0)
+		return ret;
+	/* On stmfx, gpio pins direction is (0)input, (1)output. */
+
+	return ret & mask ? GPIOF_OUTPUT : GPIOF_INPUT;
+}
+
+static int stmfx_gpio_direction_input(struct udevice *dev, unsigned int offset)
+{
+	u32 reg = STMFX_REG_GPIO_DIR + get_reg(offset);
+	u32 mask = get_mask(offset);
+	int ret;
+
+	ret = stmfx_read(dev, reg);
+	if (ret < 0)
+		return ret;
+
+	ret &= ~mask;
+
+	return stmfx_write(dev, reg, ret & ~mask);
+}
+
+static int stmfx_gpio_direction_output(struct udevice *dev,
+				       unsigned int offset, int value)
+{
+	u32 reg = STMFX_REG_GPIO_DIR + get_reg(offset);
+	u32 mask = get_mask(offset);
+	int ret;
+
+	ret = stmfx_gpio_set(dev, offset, value);
+	if (ret < 0)
+		return ret;
+
+	ret = stmfx_read(dev, reg);
+	if (ret < 0)
+		return ret;
+
+	return stmfx_write(dev, reg, ret | mask);
+}
+
+static int stmfx_gpio_probe(struct udevice *dev)
+{
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
+	struct ofnode_phandle_args args;
+	u8 sys_ctrl;
+
+	uc_priv->bank_name = "stmfx";
+	uc_priv->gpio_count = STMFX_MAX_GPIO + STMFX_MAX_AGPIO;
+	if (!dev_read_phandle_with_args(dev, "gpio-ranges",
+					NULL, 3, 0, &args)) {
+		uc_priv->gpio_count = args.args[2];
+	}
+
+	/* enable GPIO function */
+	sys_ctrl = STMFX_REG_SYS_CTRL_GPIO_EN;
+	if (uc_priv->gpio_count > STMFX_MAX_GPIO)
+		sys_ctrl |= STMFX_REG_SYS_CTRL_ALTGPIO_EN;
+	stmfx_write(dev, STMFX_REG_SYS_CTRL, sys_ctrl);
+
+	return 0;
+}
+
+static const struct dm_gpio_ops stmfx_gpio_ops = {
+	.set_value = stmfx_gpio_set,
+	.get_value = stmfx_gpio_get,
+	.get_function = stmfx_gpio_get_function,
+	.direction_input = stmfx_gpio_direction_input,
+	.direction_output = stmfx_gpio_direction_output,
+};
+
+U_BOOT_DRIVER(stmfx_gpio) = {
+	.name	= "stmfx-gpio",
+	.id	= UCLASS_GPIO,
+	.probe	= stmfx_gpio_probe,
+	.ops	= &stmfx_gpio_ops,
+};
+
+#if CONFIG_IS_ENABLED(PINCONF)
+static const struct pinconf_param stmfx_pinctrl_conf_params[] = {
+	{ "bias-disable", PIN_CONFIG_BIAS_DISABLE, 0 },
+	{ "bias-pull-up", PIN_CONFIG_BIAS_PULL_UP, 0 },
+	{ "bias-pull-pin-default", PIN_CONFIG_BIAS_PULL_PIN_DEFAULT, 0 },
+	{ "bias-pull-down", PIN_CONFIG_BIAS_PULL_DOWN, 0 },
+	{ "drive-open-drain", PIN_CONFIG_DRIVE_OPEN_DRAIN, 0 },
+	{ "drive-push-pull", PIN_CONFIG_DRIVE_PUSH_PULL, 0 },
+	{ "output-high", PIN_CONFIG_OUTPUT, 1 },
+	{ "output-low", PIN_CONFIG_OUTPUT, 0 },
+};
+
+static int stmfx_pinctrl_set_pupd(struct udevice *dev,
+				  unsigned int pin, u32 pupd)
+{
+	u8 reg = STMFX_REG_GPIO_PUPD + get_reg(pin);
+	u32 mask = get_mask(pin);
+	int ret;
+
+	ret = stmfx_read(dev, reg);
+	if (ret < 0)
+		return ret;
+	ret = (ret & ~mask) | (pupd ? mask : 0);
+
+	return stmfx_write(dev, reg, ret);
+}
+
+static int stmfx_pinctrl_set_type(struct udevice *dev,
+				  unsigned int pin, u32 type)
+{
+	u8 reg = STMFX_REG_GPIO_TYPE + get_reg(pin);
+	u32 mask = get_mask(pin);
+	int ret;
+
+	ret = stmfx_read(dev, reg);
+	if (ret < 0)
+		return ret;
+	ret = (ret & ~mask) | (type ? mask : 0);
+
+	return stmfx_write(dev, reg, ret);
+}
+
+static int stmfx_pinctrl_conf_set(struct udevice *dev, unsigned int pin,
+				  unsigned int param, unsigned int arg)
+{
+	int ret, dir;
+	struct stmfx_pinctrl *plat = dev_get_platdata(dev);
+
+	dir = stmfx_gpio_get_function(plat->gpio, pin);
+
+	if (dir < 0)
+		return dir;
+
+	switch (param) {
+	case PIN_CONFIG_BIAS_PULL_PIN_DEFAULT:
+	case PIN_CONFIG_BIAS_DISABLE:
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		ret = stmfx_pinctrl_set_pupd(dev, pin, 0);
+		break;
+	case PIN_CONFIG_BIAS_PULL_UP:
+		ret = stmfx_pinctrl_set_pupd(dev, pin, 1);
+		break;
+	case PIN_CONFIG_DRIVE_OPEN_DRAIN:
+		if (dir == GPIOF_OUTPUT)
+			ret = stmfx_pinctrl_set_type(dev, pin, 1);
+		else
+			ret = stmfx_pinctrl_set_type(dev, pin, 0);
+		break;
+	case PIN_CONFIG_DRIVE_PUSH_PULL:
+		if (dir == GPIOF_OUTPUT)
+			ret = stmfx_pinctrl_set_type(dev, pin, 0);
+		else
+			ret = stmfx_pinctrl_set_type(dev, pin, 1);
+		break;
+	case PIN_CONFIG_OUTPUT:
+		ret = stmfx_gpio_direction_output(plat->gpio, pin, arg);
+		break;
+	default:
+		return -ENOTSUPP;
+	}
+
+	return ret;
+}
+#endif
+
+static int stmfx_pinctrl_get_pins_count(struct udevice *dev)
+{
+	struct stmfx_pinctrl *plat = dev_get_platdata(dev);
+	struct gpio_dev_priv *uc_priv;
+
+	uc_priv = dev_get_uclass_priv(plat->gpio);
+
+	return uc_priv->gpio_count;
+}
+
+/*
+ * STMFX pins[15:0] are called "gpio[15:0]"
+ * and STMFX pins[23:16] are called "agpio[7:0]"
+ */
+#define MAX_PIN_NAME_LEN 7
+static char pin_name[MAX_PIN_NAME_LEN];
+static const char *stmfx_pinctrl_get_pin_name(struct udevice *dev,
+					      unsigned int selector)
+{
+	if (selector < STMFX_MAX_GPIO)
+		snprintf(pin_name, MAX_PIN_NAME_LEN, "gpio%u", selector);
+	else
+		snprintf(pin_name, MAX_PIN_NAME_LEN, "agpio%u", selector - 16);
+	return pin_name;
+}
+
+static int stmfx_pinctrl_get_pin_muxing(struct udevice *dev,
+					unsigned int selector,
+					char *buf, int size)
+{
+	struct stmfx_pinctrl *plat = dev_get_platdata(dev);
+	int func;
+
+	func = stmfx_gpio_get_function(plat->gpio, selector);
+	if (func < 0)
+		return func;
+
+	snprintf(buf, size, "%s", func == GPIOF_INPUT ? "input" : "output");
+
+	return 0;
+}
+
+static int stmfx_pinctrl_bind(struct udevice *dev)
+{
+	struct stmfx_pinctrl *plat = dev_get_platdata(dev);
+
+	return device_bind_driver_to_node(dev->parent,
+					  "stmfx-gpio", "stmfx-gpio",
+					  dev_ofnode(dev), &plat->gpio);
+};
+
+static int stmfx_pinctrl_probe(struct udevice *dev)
+{
+	struct stmfx_pinctrl *plat = dev_get_platdata(dev);
+
+	return device_probe(plat->gpio);
+};
+
+const struct pinctrl_ops stmfx_pinctrl_ops = {
+	.get_pins_count = stmfx_pinctrl_get_pins_count,
+	.get_pin_name = stmfx_pinctrl_get_pin_name,
+	.set_state = pinctrl_generic_set_state,
+	.get_pin_muxing	= stmfx_pinctrl_get_pin_muxing,
+#if CONFIG_IS_ENABLED(PINCONF)
+	.pinconf_set = stmfx_pinctrl_conf_set,
+	.pinconf_num_params = ARRAY_SIZE(stmfx_pinctrl_conf_params),
+	.pinconf_params = stmfx_pinctrl_conf_params,
+#endif
+};
+
+static const struct udevice_id stmfx_pinctrl_match[] = {
+	{ .compatible = "st,stmfx-0300-pinctrl", },
+};
+
+U_BOOT_DRIVER(stmfx_pinctrl) = {
+	.name = "stmfx-pinctrl",
+	.id = UCLASS_PINCTRL,
+	.of_match = of_match_ptr(stmfx_pinctrl_match),
+	.bind = stmfx_pinctrl_bind,
+	.probe = stmfx_pinctrl_probe,
+	.ops = &stmfx_pinctrl_ops,
+	.platdata_auto_alloc_size = sizeof(struct stmfx_pinctrl),
+};
+
+static int stmfx_chip_init(struct udevice *dev)
+{
+	u8 id;
+	u8 version[2];
+	int ret;
+	struct dm_i2c_chip *chip = dev_get_parent_platdata(dev);
+
+	id = dm_i2c_reg_read(dev, STMFX_REG_CHIP_ID);
+	if (id < 0) {
+		dev_err(dev, "error reading chip id: %d\n", id);
+		return ret;
+	}
+	/*
+	 * Check that ID is the complement of the I2C address:
+	 * STMFX I2C address follows the 7-bit format (MSB), that's why
+	 * client->addr is shifted.
+	 *
+	 * STMFX_I2C_ADDR|       STMFX         |        Linux
+	 *   input pin   | I2C device address  | I2C device address
+	 *---------------------------------------------------------
+	 *       0       | b: 1000 010x h:0x84 |       0x42
+	 *       1       | b: 1000 011x h:0x86 |       0x43
+	 */
+	if (FIELD_GET(STMFX_REG_CHIP_ID_MASK, ~id) != (chip->chip_addr << 1)) {
+		dev_err(dev, "unknown chip id: %#x\n", id);
+		return -EINVAL;
+	}
+
+	ret = dm_i2c_read(dev, STMFX_REG_FW_VERSION_MSB,
+			  version, sizeof(version));
+	if (ret) {
+		dev_err(dev, "error reading fw version: %d\n", ret);
+		return ret;
+	}
+
+	dev_info(dev, "STMFX id: %#x, fw version: %x.%02x\n",
+		 id, version[0], version[1]);
+
+	ret = dm_i2c_reg_read(dev, STMFX_REG_SYS_CTRL);
+
+	if (ret < 0)
+		return ret;
+
+	ret = dm_i2c_reg_write(dev, STMFX_REG_SYS_CTRL,
+			       ret | STMFX_REG_SYS_CTRL_SWRST);
+	if (ret)
+		return ret;
+
+	mdelay(STMFX_BOOT_TIME_MS);
+
+	return ret;
+}
+
+static int stmfx_probe(struct udevice *dev)
+{
+	struct udevice *vdd;
+	int ret;
+
+	ret = device_get_supply_regulator(dev, "vdd-supply", &vdd);
+	if (ret && ret != -ENOENT) {
+		dev_err(dev, "vdd regulator error:%d\n", ret);
+		return ret;
+	}
+	if (!ret) {
+		ret = regulator_set_enable(vdd, true);
+		if (ret) {
+			dev_err(dev, "vdd enable failed: %d\n", ret);
+			return ret;
+		}
+	}
+
+	return stmfx_chip_init(dev);
+}
+
+static const struct udevice_id stmfx_match[] = {
+	{ .compatible = "st,stmfx-0300", },
+};
+
+U_BOOT_DRIVER(stmfx) = {
+	.name = "stmfx",
+	.id = UCLASS_I2C_GENERIC,
+	.of_match = of_match_ptr(stmfx_match),
+	.probe = stmfx_probe,
+	.bind = dm_scan_fdt_dev,
+};
diff --git a/drivers/power/pmic/Kconfig b/drivers/power/pmic/Kconfig
index 8cf60ebcf3..b0cd260354 100644
--- a/drivers/power/pmic/Kconfig
+++ b/drivers/power/pmic/Kconfig
@@ -231,10 +231,10 @@ config DM_PMIC_TPS65910
 	DC-DC converter, 8 LDOs and a RTC. This driver binds the SMPS and LDO
 	pmic children.
 
-config PMIC_STPMU1
-	bool "Enable support for STMicroelectronics STPMU1 PMIC"
+config PMIC_STPMIC1
+	bool "Enable support for STMicroelectronics STPMIC1 PMIC"
 	depends on DM_PMIC && DM_I2C
 	---help---
-	The STPMU1 PMIC provides 4 BUCKs, 6 LDOs, 1 VREF and 2 power switches.
+	The STPMIC1 PMIC provides 4 BUCKs, 6 LDOs, 1 VREF and 2 power switches.
 	It is accessed via an I2C interface. The device is used with STM32MP1
 	SoCs. This driver implements register read/write operations.
diff --git a/drivers/power/pmic/Makefile b/drivers/power/pmic/Makefile
index 637352ab2b..ce250cb155 100644
--- a/drivers/power/pmic/Makefile
+++ b/drivers/power/pmic/Makefile
@@ -23,7 +23,7 @@ obj-$(CONFIG_DM_PMIC_TPS65910) += pmic_tps65910_dm.o
 obj-$(CONFIG_$(SPL_)PMIC_PALMAS) += palmas.o
 obj-$(CONFIG_$(SPL_)PMIC_LP873X) += lp873x.o
 obj-$(CONFIG_$(SPL_)PMIC_LP87565) += lp87565.o
-obj-$(CONFIG_PMIC_STPMU1) += stpmu1.o
+obj-$(CONFIG_PMIC_STPMIC1) += stpmic1.o
 
 obj-$(CONFIG_POWER_LTC3676) += pmic_ltc3676.o
 obj-$(CONFIG_POWER_MAX77696) += pmic_max77696.o
diff --git a/drivers/power/pmic/stpmic1.c b/drivers/power/pmic/stpmic1.c
new file mode 100644
index 0000000000..65296c5fc3
--- /dev/null
+++ b/drivers/power/pmic/stpmic1.c
@@ -0,0 +1,255 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <i2c.h>
+#include <sysreset.h>
+#include <dm/device.h>
+#include <dm/lists.h>
+#include <power/pmic.h>
+#include <power/stpmic1.h>
+
+#define STPMIC1_NUM_OF_REGS 0x100
+
+#define STPMIC1_NVM_SIZE 8
+#define STPMIC1_NVM_POLL_TIMEOUT 100000
+#define STPMIC1_NVM_START_ADDRESS 0xf8
+
+enum pmic_nvm_op {
+	SHADOW_READ,
+	SHADOW_WRITE,
+	NVM_READ,
+	NVM_WRITE,
+};
+
+#if CONFIG_IS_ENABLED(DM_REGULATOR)
+static const struct pmic_child_info stpmic1_children_info[] = {
+	{ .prefix = "ldo", .driver = "stpmic1_ldo" },
+	{ .prefix = "buck", .driver = "stpmic1_buck" },
+	{ .prefix = "vref_ddr", .driver = "stpmic1_vref_ddr" },
+	{ .prefix = "pwr_sw", .driver = "stpmic1_pwr_sw" },
+	{ .prefix = "boost", .driver = "stpmic1_boost" },
+	{ },
+};
+#endif /* DM_REGULATOR */
+
+static int stpmic1_reg_count(struct udevice *dev)
+{
+	return STPMIC1_NUM_OF_REGS;
+}
+
+static int stpmic1_write(struct udevice *dev, uint reg, const uint8_t *buff,
+			 int len)
+{
+	int ret;
+
+	ret = dm_i2c_write(dev, reg, buff, len);
+	if (ret)
+		dev_err(dev, "%s: failed to write register %#x :%d",
+			__func__, reg, ret);
+
+	return ret;
+}
+
+static int stpmic1_read(struct udevice *dev, uint reg, uint8_t *buff, int len)
+{
+	int ret;
+
+	ret = dm_i2c_read(dev, reg, buff, len);
+	if (ret)
+		dev_err(dev, "%s: failed to read register %#x : %d",
+			__func__, reg, ret);
+
+	return ret;
+}
+
+static int stpmic1_bind(struct udevice *dev)
+{
+#if CONFIG_IS_ENABLED(DM_REGULATOR)
+	ofnode regulators_node;
+	int children;
+
+	regulators_node = dev_read_subnode(dev, "regulators");
+	if (!ofnode_valid(regulators_node)) {
+		dev_dbg(dev, "regulators subnode not found!");
+		return -ENXIO;
+	}
+	dev_dbg(dev, "found regulators subnode\n");
+
+	children = pmic_bind_children(dev, regulators_node,
+				      stpmic1_children_info);
+	if (!children)
+		dev_dbg(dev, "no child found\n");
+#endif /* DM_REGULATOR */
+
+	if (CONFIG_IS_ENABLED(SYSRESET))
+		return device_bind_driver(dev, "stpmic1-sysreset",
+					  "stpmic1-sysreset", NULL);
+
+	return 0;
+}
+
+static struct dm_pmic_ops stpmic1_ops = {
+	.reg_count = stpmic1_reg_count,
+	.read = stpmic1_read,
+	.write = stpmic1_write,
+};
+
+static const struct udevice_id stpmic1_ids[] = {
+	{ .compatible = "st,stpmic1" },
+	{ }
+};
+
+U_BOOT_DRIVER(pmic_stpmic1) = {
+	.name = "stpmic1_pmic",
+	.id = UCLASS_PMIC,
+	.of_match = stpmic1_ids,
+	.bind = stpmic1_bind,
+	.ops = &stpmic1_ops,
+};
+
+#ifndef CONFIG_SPL_BUILD
+static int stpmic1_nvm_rw(u8 addr, u8 *buf, int buf_len, enum pmic_nvm_op op)
+{
+	struct udevice *dev;
+	unsigned long timeout;
+	u8 cmd = STPMIC1_NVM_CMD_READ;
+	int ret;
+
+	ret = uclass_get_device_by_driver(UCLASS_PMIC,
+					  DM_GET_DRIVER(pmic_stpmic1), &dev);
+	if (ret)
+		/* No PMIC on power discrete board */
+		return -EOPNOTSUPP;
+
+	if (addr < STPMIC1_NVM_START_ADDRESS)
+		return -EACCES;
+
+	if (op == SHADOW_READ)
+		return pmic_read(dev, addr, buf, buf_len);
+
+	if (op == SHADOW_WRITE)
+		return pmic_write(dev, addr, buf, buf_len);
+
+	if (op == NVM_WRITE) {
+		cmd = STPMIC1_NVM_CMD_PROGRAM;
+
+		ret = pmic_write(dev, addr, buf, buf_len);
+		if (ret < 0)
+			return ret;
+	}
+
+	ret = pmic_reg_read(dev, STPMIC1_NVM_CR);
+	if (ret < 0)
+		return ret;
+
+	ret = pmic_reg_write(dev, STPMIC1_NVM_CR, ret | cmd);
+	if (ret < 0)
+		return ret;
+
+	timeout = timer_get_us() + STPMIC1_NVM_POLL_TIMEOUT;
+	for (;;) {
+		ret = pmic_reg_read(dev, STPMIC1_NVM_SR);
+		if (ret < 0)
+			return ret;
+
+		if (!(ret & STPMIC1_NVM_BUSY))
+			break;
+
+		if (time_after(timer_get_us(), timeout))
+			break;
+	}
+
+	if (ret & STPMIC1_NVM_BUSY)
+		return -ETIMEDOUT;
+
+	if (op == NVM_READ) {
+		ret = pmic_read(dev, addr, buf, buf_len);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+int stpmic1_shadow_read_byte(u8 addr, u8 *buf)
+{
+	return stpmic1_nvm_rw(addr, buf, 1, SHADOW_READ);
+}
+
+int stpmic1_shadow_write_byte(u8 addr, u8 *buf)
+{
+	return stpmic1_nvm_rw(addr, buf, 1, SHADOW_WRITE);
+}
+
+int stpmic1_nvm_read_byte(u8 addr, u8 *buf)
+{
+	return stpmic1_nvm_rw(addr, buf, 1, NVM_READ);
+}
+
+int stpmic1_nvm_write_byte(u8 addr, u8 *buf)
+{
+	return stpmic1_nvm_rw(addr, buf, 1, NVM_WRITE);
+}
+
+int stpmic1_nvm_read_all(u8 *buf, int buf_len)
+{
+	if (buf_len != STPMIC1_NVM_SIZE)
+		return -EINVAL;
+
+	return stpmic1_nvm_rw(STPMIC1_NVM_START_ADDRESS,
+			     buf, buf_len, NVM_READ);
+}
+
+int stpmic1_nvm_write_all(u8 *buf, int buf_len)
+{
+	if (buf_len != STPMIC1_NVM_SIZE)
+		return -EINVAL;
+
+	return stpmic1_nvm_rw(STPMIC1_NVM_START_ADDRESS,
+			     buf, buf_len, NVM_WRITE);
+}
+#endif /* CONFIG_SPL_BUILD */
+
+#ifdef CONFIG_SYSRESET
+static int stpmic1_sysreset_request(struct udevice *dev, enum sysreset_t type)
+{
+	struct udevice *pmic_dev;
+	int ret;
+
+	if (type != SYSRESET_POWER)
+		return -EPROTONOSUPPORT;
+
+	ret = uclass_get_device_by_driver(UCLASS_PMIC,
+					  DM_GET_DRIVER(pmic_stpmic1),
+					  &pmic_dev);
+
+	if (ret)
+		return -EOPNOTSUPP;
+
+	ret = pmic_reg_read(pmic_dev, STPMIC1_MAIN_CR);
+	if (ret < 0)
+		return ret;
+
+	ret = pmic_reg_write(pmic_dev, STPMIC1_MAIN_CR,
+			     ret | STPMIC1_SWOFF | STPMIC1_RREQ_EN);
+	if (ret < 0)
+		return ret;
+
+	return -EINPROGRESS;
+}
+
+static struct sysreset_ops stpmic1_sysreset_ops = {
+	.request = stpmic1_sysreset_request,
+};
+
+U_BOOT_DRIVER(stpmic1_sysreset) = {
+	.name = "stpmic1-sysreset",
+	.id = UCLASS_SYSRESET,
+	.ops = &stpmic1_sysreset_ops,
+};
+#endif
diff --git a/drivers/power/pmic/stpmu1.c b/drivers/power/pmic/stpmu1.c
deleted file mode 100644
index 47af012332..0000000000
--- a/drivers/power/pmic/stpmu1.c
+++ /dev/null
@@ -1,95 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
-/*
- * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
- */
-
-#include <common.h>
-#include <dm.h>
-#include <errno.h>
-#include <i2c.h>
-#include <power/pmic.h>
-#include <power/stpmu1.h>
-
-#define STMPU1_NUM_OF_REGS 0x100
-
-#ifndef CONFIG_SPL_BUILD
-static const struct pmic_child_info stpmu1_children_info[] = {
-	{ .prefix = "ldo", .driver = "stpmu1_ldo" },
-	{ .prefix = "buck", .driver = "stpmu1_buck" },
-	{ .prefix = "vref_ddr", .driver = "stpmu1_vref_ddr" },
-	{ .prefix = "pwr_sw", .driver = "stpmu1_pwr_sw" },
-	{ .prefix = "boost", .driver = "stpmu1_boost" },
-	{ },
-};
-#endif /* CONFIG_SPL_BUILD */
-
-static int stpmu1_reg_count(struct udevice *dev)
-{
-	return STMPU1_NUM_OF_REGS;
-}
-
-static int stpmu1_write(struct udevice *dev, uint reg, const uint8_t *buff,
-			int len)
-{
-	int ret;
-
-	ret = dm_i2c_write(dev, reg, buff, len);
-	if (ret)
-		dev_err(dev, "%s: failed to write register %#x :%d",
-			__func__, reg, ret);
-
-	return ret;
-}
-
-static int stpmu1_read(struct udevice *dev, uint reg, uint8_t *buff, int len)
-{
-	int ret;
-
-	ret = dm_i2c_read(dev, reg, buff, len);
-	if (ret)
-		dev_err(dev, "%s: failed to read register %#x : %d",
-			__func__, reg, ret);
-
-	return ret;
-}
-
-static int stpmu1_bind(struct udevice *dev)
-{
-#ifndef CONFIG_SPL_BUILD
-	ofnode regulators_node;
-	int children;
-
-	regulators_node = dev_read_subnode(dev, "regulators");
-	if (!ofnode_valid(regulators_node)) {
-		dev_dbg(dev, "regulators subnode not found!\n");
-		return -ENXIO;
-	}
-	dev_dbg(dev, "found regulators subnode\n");
-
-	children = pmic_bind_children(dev, regulators_node,
-				      stpmu1_children_info);
-	if (!children)
-		dev_dbg(dev, "no child found\n");
-#endif /* CONFIG_SPL_BUILD */
-
-	return 0;
-}
-
-static struct dm_pmic_ops stpmu1_ops = {
-	.reg_count = stpmu1_reg_count,
-	.read = stpmu1_read,
-	.write = stpmu1_write,
-};
-
-static const struct udevice_id stpmu1_ids[] = {
-	{ .compatible = "st,stpmu1" },
-	{ }
-};
-
-U_BOOT_DRIVER(pmic_stpmu1) = {
-	.name = "stpmu1_pmic",
-	.id = UCLASS_PMIC,
-	.of_match = stpmu1_ids,
-	.bind = stpmu1_bind,
-	.ops = &stpmu1_ops,
-};
diff --git a/drivers/power/regulator/Kconfig b/drivers/power/regulator/Kconfig
index 3ed0dd2264..72dfc48981 100644
--- a/drivers/power/regulator/Kconfig
+++ b/drivers/power/regulator/Kconfig
@@ -244,11 +244,17 @@ config DM_REGULATOR_TPS65910
 	regulator types of the TPS65910 (BUCK, BOOST and LDO). It implements
 	the get/set api for value and enable.
 
-config DM_REGULATOR_STPMU1
-	bool "Enable driver for STPMU1 regulators"
-	depends on DM_REGULATOR && PMIC_STPMU1
+config DM_REGULATOR_STPMIC1
+	bool "Enable driver for STPMIC1 regulators"
+	depends on DM_REGULATOR && PMIC_STPMIC1
 	---help---
-	Enable support for the regulator functions of the STPMU1 PMIC. The
+	Enable support for the regulator functions of the STPMIC1 PMIC. The
 	driver implements get/set api for the various BUCKS and LDOs supported
 	by the PMIC device. This driver is controlled by a device tree node
 	which includes voltage limits.
+
+config SPL_DM_REGULATOR_STPMIC1
+	bool "Enable driver for STPMIC1 regulators in SPL"
+	depends on SPL_DM_REGULATOR && PMIC_STPMIC1
+	help
+	  Enable support for the regulator functions of the STPMIC1 PMIC in SPL.
diff --git a/drivers/power/regulator/Makefile b/drivers/power/regulator/Makefile
index f617ce723a..8c1506c88e 100644
--- a/drivers/power/regulator/Makefile
+++ b/drivers/power/regulator/Makefile
@@ -24,4 +24,4 @@ obj-$(CONFIG_$(SPL_)DM_REGULATOR_LP873X) += lp873x_regulator.o
 obj-$(CONFIG_$(SPL_)DM_REGULATOR_LP87565) += lp87565_regulator.o
 obj-$(CONFIG_$(SPL_)DM_REGULATOR_STM32_VREFBUF) += stm32-vrefbuf.o
 obj-$(CONFIG_DM_REGULATOR_TPS65910) += tps65910_regulator.o
-obj-$(CONFIG_$(SPL_)DM_REGULATOR_STPMU1) += stpmu1.o
+obj-$(CONFIG_$(SPL_)DM_REGULATOR_STPMIC1) += stpmic1.o
diff --git a/drivers/power/regulator/stpmic1.c b/drivers/power/regulator/stpmic1.c
new file mode 100644
index 0000000000..50ef2a21d1
--- /dev/null
+++ b/drivers/power/regulator/stpmic1.c
@@ -0,0 +1,672 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ * Author: Christophe Kerello <christophe.kerello@st.com>
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <power/pmic.h>
+#include <power/regulator.h>
+#include <power/stpmic1.h>
+
+struct stpmic1_range {
+	int min_uv;
+	int min_sel;
+	int max_sel;
+	int step;
+};
+
+struct stpmic1_output {
+	const struct stpmic1_range *ranges;
+	int nbranges;
+};
+
+#define STPMIC1_MODE(_id, _val, _name) { \
+	.id = _id,			\
+	.register_value = _val,		\
+	.name = _name,			\
+}
+
+#define STPMIC1_RANGE(_min_uv, _min_sel, _max_sel, _step) { \
+	.min_uv = _min_uv,		\
+	.min_sel = _min_sel,		\
+	.max_sel = _max_sel,		\
+	.step = _step,			\
+}
+
+#define STPMIC1_OUTPUT(_ranges, _nbranges) { \
+	.ranges = _ranges,		\
+	.nbranges = _nbranges,		\
+}
+
+static int stpmic1_output_find_uv(int sel,
+				  const struct stpmic1_output *output)
+{
+	const struct stpmic1_range *range;
+	int i;
+
+	for (i = 0, range = output->ranges;
+	     i < output->nbranges; i++, range++) {
+		if (sel >= range->min_sel && sel <= range->max_sel)
+			return range->min_uv +
+			       (sel - range->min_sel) * range->step;
+	}
+
+	return -EINVAL;
+}
+
+static int stpmic1_output_find_sel(int uv,
+				   const struct stpmic1_output *output)
+{
+	const struct stpmic1_range *range;
+	int i;
+
+	for (i = 0, range = output->ranges;
+	     i < output->nbranges; i++, range++) {
+		if (uv == range->min_uv && !range->step)
+			return range->min_sel;
+
+		if (uv >= range->min_uv &&
+		    uv <= range->min_uv +
+			  (range->max_sel - range->min_sel) * range->step)
+			return range->min_sel +
+			       (uv - range->min_uv) / range->step;
+	}
+
+	return -EINVAL;
+}
+
+/*
+ * BUCK regulators
+ */
+
+static const struct stpmic1_range buck1_ranges[] = {
+	STPMIC1_RANGE(725000, 0, 4, 0),
+	STPMIC1_RANGE(725000, 5, 36, 25000),
+	STPMIC1_RANGE(1500000, 37, 63, 0),
+};
+
+static const struct stpmic1_range buck2_ranges[] = {
+	STPMIC1_RANGE(1000000, 0, 17, 0),
+	STPMIC1_RANGE(1050000, 18, 19, 0),
+	STPMIC1_RANGE(1100000, 20, 21, 0),
+	STPMIC1_RANGE(1150000, 22, 23, 0),
+	STPMIC1_RANGE(1200000, 24, 25, 0),
+	STPMIC1_RANGE(1250000, 26, 27, 0),
+	STPMIC1_RANGE(1300000, 28, 29, 0),
+	STPMIC1_RANGE(1350000, 30, 31, 0),
+	STPMIC1_RANGE(1400000, 32, 33, 0),
+	STPMIC1_RANGE(1450000, 34, 35, 0),
+	STPMIC1_RANGE(1500000, 36, 63, 0),
+};
+
+static const struct stpmic1_range buck3_ranges[] = {
+	STPMIC1_RANGE(1000000, 0, 19, 0),
+	STPMIC1_RANGE(1100000, 20, 23, 0),
+	STPMIC1_RANGE(1200000, 24, 27, 0),
+	STPMIC1_RANGE(1300000, 28, 31, 0),
+	STPMIC1_RANGE(1400000, 32, 35, 0),
+	STPMIC1_RANGE(1500000, 36, 55, 100000),
+	STPMIC1_RANGE(3400000, 56, 63, 0),
+};
+
+static const struct stpmic1_range buck4_ranges[] = {
+	STPMIC1_RANGE(600000, 0, 27, 25000),
+	STPMIC1_RANGE(1300000, 28, 29, 0),
+	STPMIC1_RANGE(1350000, 30, 31, 0),
+	STPMIC1_RANGE(1400000, 32, 33, 0),
+	STPMIC1_RANGE(1450000, 34, 35, 0),
+	STPMIC1_RANGE(1500000, 36, 60, 100000),
+	STPMIC1_RANGE(3900000, 61, 63, 0),
+};
+
+/* BUCK: 1,2,3,4 - voltage ranges */
+static const struct stpmic1_output buck_voltage_range[] = {
+	STPMIC1_OUTPUT(buck1_ranges, ARRAY_SIZE(buck1_ranges)),
+	STPMIC1_OUTPUT(buck2_ranges, ARRAY_SIZE(buck2_ranges)),
+	STPMIC1_OUTPUT(buck3_ranges, ARRAY_SIZE(buck3_ranges)),
+	STPMIC1_OUTPUT(buck4_ranges, ARRAY_SIZE(buck4_ranges)),
+};
+
+/* BUCK modes */
+static const struct dm_regulator_mode buck_modes[] = {
+	STPMIC1_MODE(STPMIC1_PREG_MODE_HP, STPMIC1_PREG_MODE_HP, "HP"),
+	STPMIC1_MODE(STPMIC1_PREG_MODE_LP, STPMIC1_PREG_MODE_LP, "LP"),
+};
+
+static int stpmic1_buck_get_uv(struct udevice *dev, int buck)
+{
+	int sel;
+
+	sel = pmic_reg_read(dev, STPMIC1_BUCKX_MAIN_CR(buck));
+	if (sel < 0)
+		return sel;
+
+	sel &= STPMIC1_BUCK_VOUT_MASK;
+	sel >>= STPMIC1_BUCK_VOUT_SHIFT;
+
+	return stpmic1_output_find_uv(sel, &buck_voltage_range[buck]);
+}
+
+static int stpmic1_buck_get_value(struct udevice *dev)
+{
+	return stpmic1_buck_get_uv(dev->parent, dev->driver_data - 1);
+}
+
+static int stpmic1_buck_set_value(struct udevice *dev, int uv)
+{
+	int sel, buck = dev->driver_data - 1;
+
+	sel = stpmic1_output_find_sel(uv, &buck_voltage_range[buck]);
+	if (sel < 0)
+		return sel;
+
+	return pmic_clrsetbits(dev->parent,
+			       STPMIC1_BUCKX_MAIN_CR(buck),
+			       STPMIC1_BUCK_VOUT_MASK,
+			       sel << STPMIC1_BUCK_VOUT_SHIFT);
+}
+
+static int stpmic1_buck_get_enable(struct udevice *dev)
+{
+	int ret;
+
+	ret = pmic_reg_read(dev->parent,
+			    STPMIC1_BUCKX_MAIN_CR(dev->driver_data - 1));
+	if (ret < 0)
+		return false;
+
+	return ret & STPMIC1_BUCK_ENA ? true : false;
+}
+
+static int stpmic1_buck_set_enable(struct udevice *dev, bool enable)
+{
+	struct dm_regulator_uclass_platdata *uc_pdata;
+	int delay = enable ? STPMIC1_DEFAULT_START_UP_DELAY_MS :
+			     STPMIC1_DEFAULT_STOP_DELAY_MS;
+	int ret, uv;
+
+	/* if regulator is already in the wanted state, nothing to do */
+	if (stpmic1_buck_get_enable(dev) == enable)
+		return 0;
+
+	if (enable) {
+		uc_pdata = dev_get_uclass_platdata(dev);
+		uv = stpmic1_buck_get_value(dev);
+		if (uv < uc_pdata->min_uV || uv > uc_pdata->max_uV)
+			stpmic1_buck_set_value(dev, uc_pdata->min_uV);
+	}
+
+	ret = pmic_clrsetbits(dev->parent,
+			      STPMIC1_BUCKX_MAIN_CR(dev->driver_data - 1),
+			      STPMIC1_BUCK_ENA, enable ? STPMIC1_BUCK_ENA : 0);
+	mdelay(delay);
+
+	return ret;
+}
+
+static int stpmic1_buck_get_mode(struct udevice *dev)
+{
+	int ret;
+
+	ret = pmic_reg_read(dev->parent,
+			    STPMIC1_BUCKX_MAIN_CR(dev->driver_data - 1));
+	if (ret < 0)
+		return ret;
+
+	return ret & STPMIC1_BUCK_PREG_MODE ? STPMIC1_PREG_MODE_LP :
+					      STPMIC1_PREG_MODE_HP;
+}
+
+static int stpmic1_buck_set_mode(struct udevice *dev, int mode)
+{
+	return pmic_clrsetbits(dev->parent,
+			       STPMIC1_BUCKX_MAIN_CR(dev->driver_data - 1),
+			       STPMIC1_BUCK_PREG_MODE,
+			       mode ? STPMIC1_BUCK_PREG_MODE : 0);
+}
+
+static int stpmic1_buck_probe(struct udevice *dev)
+{
+	struct dm_regulator_uclass_platdata *uc_pdata;
+
+	if (!dev->driver_data || dev->driver_data > STPMIC1_MAX_BUCK)
+		return -EINVAL;
+
+	uc_pdata = dev_get_uclass_platdata(dev);
+
+	uc_pdata->type = REGULATOR_TYPE_BUCK;
+	uc_pdata->mode = (struct dm_regulator_mode *)buck_modes;
+	uc_pdata->mode_count = ARRAY_SIZE(buck_modes);
+
+	return 0;
+}
+
+static const struct dm_regulator_ops stpmic1_buck_ops = {
+	.get_value  = stpmic1_buck_get_value,
+	.set_value  = stpmic1_buck_set_value,
+	.get_enable = stpmic1_buck_get_enable,
+	.set_enable = stpmic1_buck_set_enable,
+	.get_mode   = stpmic1_buck_get_mode,
+	.set_mode   = stpmic1_buck_set_mode,
+};
+
+U_BOOT_DRIVER(stpmic1_buck) = {
+	.name = "stpmic1_buck",
+	.id = UCLASS_REGULATOR,
+	.ops = &stpmic1_buck_ops,
+	.probe = stpmic1_buck_probe,
+};
+
+/*
+ * LDO regulators
+ */
+
+static const struct stpmic1_range ldo12_ranges[] = {
+	STPMIC1_RANGE(1700000, 0, 7, 0),
+	STPMIC1_RANGE(1700000, 8, 24, 100000),
+	STPMIC1_RANGE(3300000, 25, 31, 0),
+};
+
+static const struct stpmic1_range ldo3_ranges[] = {
+	STPMIC1_RANGE(1700000, 0, 7, 0),
+	STPMIC1_RANGE(1700000, 8, 24, 100000),
+	STPMIC1_RANGE(3300000, 25, 30, 0),
+	/* Sel 31 is special case when LDO3 is in mode sync_source (BUCK2/2) */
+};
+
+static const struct stpmic1_range ldo5_ranges[] = {
+	STPMIC1_RANGE(1700000, 0, 7, 0),
+	STPMIC1_RANGE(1700000, 8, 30, 100000),
+	STPMIC1_RANGE(3900000, 31, 31, 0),
+};
+
+static const struct stpmic1_range ldo6_ranges[] = {
+	STPMIC1_RANGE(900000, 0, 24, 100000),
+	STPMIC1_RANGE(3300000, 25, 31, 0),
+};
+
+/* LDO: 1,2,3,4,5,6 - voltage ranges */
+static const struct stpmic1_output ldo_voltage_range[] = {
+	STPMIC1_OUTPUT(ldo12_ranges, ARRAY_SIZE(ldo12_ranges)),
+	STPMIC1_OUTPUT(ldo12_ranges, ARRAY_SIZE(ldo12_ranges)),
+	STPMIC1_OUTPUT(ldo3_ranges, ARRAY_SIZE(ldo3_ranges)),
+	STPMIC1_OUTPUT(NULL, 0),
+	STPMIC1_OUTPUT(ldo5_ranges, ARRAY_SIZE(ldo5_ranges)),
+	STPMIC1_OUTPUT(ldo6_ranges, ARRAY_SIZE(ldo6_ranges)),
+};
+
+/* LDO modes */
+static const struct dm_regulator_mode ldo_modes[] = {
+	STPMIC1_MODE(STPMIC1_LDO_MODE_NORMAL,
+		     STPMIC1_LDO_MODE_NORMAL, "NORMAL"),
+	STPMIC1_MODE(STPMIC1_LDO_MODE_BYPASS,
+		     STPMIC1_LDO_MODE_BYPASS, "BYPASS"),
+	STPMIC1_MODE(STPMIC1_LDO_MODE_SINK_SOURCE,
+		     STPMIC1_LDO_MODE_SINK_SOURCE, "SINK SOURCE"),
+};
+
+static int stpmic1_ldo_get_value(struct udevice *dev)
+{
+	int sel, ldo = dev->driver_data - 1;
+
+	sel = pmic_reg_read(dev->parent, STPMIC1_LDOX_MAIN_CR(ldo));
+	if (sel < 0)
+		return sel;
+
+	/* ldo4 => 3,3V */
+	if (ldo == STPMIC1_LDO4)
+		return STPMIC1_LDO4_UV;
+
+	sel &= STPMIC1_LDO12356_VOUT_MASK;
+	sel >>= STPMIC1_LDO12356_VOUT_SHIFT;
+
+	/* ldo3, sel = 31 => BUCK2/2 */
+	if (ldo == STPMIC1_LDO3 && sel == STPMIC1_LDO3_DDR_SEL)
+		return stpmic1_buck_get_uv(dev->parent, STPMIC1_BUCK2) / 2;
+
+	return stpmic1_output_find_uv(sel, &ldo_voltage_range[ldo]);
+}
+
+static int stpmic1_ldo_set_value(struct udevice *dev, int uv)
+{
+	int sel, ldo = dev->driver_data - 1;
+
+	/* ldo4 => not possible */
+	if (ldo == STPMIC1_LDO4)
+		return -EINVAL;
+
+	sel = stpmic1_output_find_sel(uv, &ldo_voltage_range[ldo]);
+	if (sel < 0)
+		return sel;
+
+	return pmic_clrsetbits(dev->parent,
+			       STPMIC1_LDOX_MAIN_CR(ldo),
+			       STPMIC1_LDO12356_VOUT_MASK,
+			       sel << STPMIC1_LDO12356_VOUT_SHIFT);
+}
+
+static int stpmic1_ldo_get_enable(struct udevice *dev)
+{
+	int ret;
+
+	ret = pmic_reg_read(dev->parent,
+			    STPMIC1_LDOX_MAIN_CR(dev->driver_data - 1));
+	if (ret < 0)
+		return false;
+
+	return ret & STPMIC1_LDO_ENA ? true : false;
+}
+
+static int stpmic1_ldo_set_enable(struct udevice *dev, bool enable)
+{
+	struct dm_regulator_uclass_platdata *uc_pdata;
+	int delay = enable ? STPMIC1_DEFAULT_START_UP_DELAY_MS :
+			     STPMIC1_DEFAULT_STOP_DELAY_MS;
+	int ret, uv;
+
+	/* if regulator is already in the wanted state, nothing to do */
+	if (stpmic1_ldo_get_enable(dev) == enable)
+		return 0;
+
+	if (enable) {
+		uc_pdata = dev_get_uclass_platdata(dev);
+		uv = stpmic1_ldo_get_value(dev);
+		if (uv < uc_pdata->min_uV || uv > uc_pdata->max_uV)
+			stpmic1_ldo_set_value(dev, uc_pdata->min_uV);
+	}
+
+	ret = pmic_clrsetbits(dev->parent,
+			      STPMIC1_LDOX_MAIN_CR(dev->driver_data - 1),
+			      STPMIC1_LDO_ENA, enable ? STPMIC1_LDO_ENA : 0);
+	mdelay(delay);
+
+	return ret;
+}
+
+static int stpmic1_ldo_get_mode(struct udevice *dev)
+{
+	int ret, ldo = dev->driver_data - 1;
+
+	if (ldo != STPMIC1_LDO3)
+		return -EINVAL;
+
+	ret = pmic_reg_read(dev->parent, STPMIC1_LDOX_MAIN_CR(ldo));
+	if (ret < 0)
+		return ret;
+
+	if (ret & STPMIC1_LDO3_MODE)
+		return STPMIC1_LDO_MODE_BYPASS;
+
+	ret &= STPMIC1_LDO12356_VOUT_MASK;
+	ret >>= STPMIC1_LDO12356_VOUT_SHIFT;
+
+	return ret == STPMIC1_LDO3_DDR_SEL ? STPMIC1_LDO_MODE_SINK_SOURCE :
+					     STPMIC1_LDO_MODE_NORMAL;
+}
+
+static int stpmic1_ldo_set_mode(struct udevice *dev, int mode)
+{
+	int ret, ldo = dev->driver_data - 1;
+
+	if (ldo != STPMIC1_LDO3)
+		return -EINVAL;
+
+	ret = pmic_reg_read(dev->parent, STPMIC1_LDOX_MAIN_CR(ldo));
+	if (ret < 0)
+		return ret;
+
+	switch (mode) {
+	case STPMIC1_LDO_MODE_SINK_SOURCE:
+		ret &= ~STPMIC1_LDO12356_VOUT_MASK;
+		ret |= STPMIC1_LDO3_DDR_SEL << STPMIC1_LDO12356_VOUT_SHIFT;
+	case STPMIC1_LDO_MODE_NORMAL:
+		ret &= ~STPMIC1_LDO3_MODE;
+		break;
+	case STPMIC1_LDO_MODE_BYPASS:
+		ret |= STPMIC1_LDO3_MODE;
+		break;
+	}
+
+	return pmic_reg_write(dev->parent, STPMIC1_LDOX_MAIN_CR(ldo), ret);
+}
+
+static int stpmic1_ldo_probe(struct udevice *dev)
+{
+	struct dm_regulator_uclass_platdata *uc_pdata;
+
+	if (!dev->driver_data || dev->driver_data > STPMIC1_MAX_LDO)
+		return -EINVAL;
+
+	uc_pdata = dev_get_uclass_platdata(dev);
+
+	uc_pdata->type = REGULATOR_TYPE_LDO;
+	if (dev->driver_data - 1 == STPMIC1_LDO3) {
+		uc_pdata->mode = (struct dm_regulator_mode *)ldo_modes;
+		uc_pdata->mode_count = ARRAY_SIZE(ldo_modes);
+	} else {
+		uc_pdata->mode_count = 0;
+	}
+
+	return 0;
+}
+
+static const struct dm_regulator_ops stpmic1_ldo_ops = {
+	.get_value  = stpmic1_ldo_get_value,
+	.set_value  = stpmic1_ldo_set_value,
+	.get_enable = stpmic1_ldo_get_enable,
+	.set_enable = stpmic1_ldo_set_enable,
+	.get_mode   = stpmic1_ldo_get_mode,
+	.set_mode   = stpmic1_ldo_set_mode,
+};
+
+U_BOOT_DRIVER(stpmic1_ldo) = {
+	.name = "stpmic1_ldo",
+	.id = UCLASS_REGULATOR,
+	.ops = &stpmic1_ldo_ops,
+	.probe = stpmic1_ldo_probe,
+};
+
+/*
+ * VREF DDR regulator
+ */
+
+static int stpmic1_vref_ddr_get_value(struct udevice *dev)
+{
+	/* BUCK2/2 */
+	return stpmic1_buck_get_uv(dev->parent, STPMIC1_BUCK2) / 2;
+}
+
+static int stpmic1_vref_ddr_get_enable(struct udevice *dev)
+{
+	int ret;
+
+	ret = pmic_reg_read(dev->parent, STPMIC1_REFDDR_MAIN_CR);
+	if (ret < 0)
+		return false;
+
+	return ret & STPMIC1_VREF_ENA ? true : false;
+}
+
+static int stpmic1_vref_ddr_set_enable(struct udevice *dev, bool enable)
+{
+	int delay = enable ? STPMIC1_DEFAULT_START_UP_DELAY_MS :
+			     STPMIC1_DEFAULT_STOP_DELAY_MS;
+	int ret;
+
+	/* if regulator is already in the wanted state, nothing to do */
+	if (stpmic1_vref_ddr_get_enable(dev) == enable)
+		return 0;
+
+	ret = pmic_clrsetbits(dev->parent, STPMIC1_REFDDR_MAIN_CR,
+			      STPMIC1_VREF_ENA, enable ? STPMIC1_VREF_ENA : 0);
+	mdelay(delay);
+
+	return ret;
+}
+
+static int stpmic1_vref_ddr_probe(struct udevice *dev)
+{
+	struct dm_regulator_uclass_platdata *uc_pdata;
+
+	uc_pdata = dev_get_uclass_platdata(dev);
+
+	uc_pdata->type = REGULATOR_TYPE_FIXED;
+	uc_pdata->mode_count = 0;
+
+	return 0;
+}
+
+static const struct dm_regulator_ops stpmic1_vref_ddr_ops = {
+	.get_value  = stpmic1_vref_ddr_get_value,
+	.get_enable = stpmic1_vref_ddr_get_enable,
+	.set_enable = stpmic1_vref_ddr_set_enable,
+};
+
+U_BOOT_DRIVER(stpmic1_vref_ddr) = {
+	.name = "stpmic1_vref_ddr",
+	.id = UCLASS_REGULATOR,
+	.ops = &stpmic1_vref_ddr_ops,
+	.probe = stpmic1_vref_ddr_probe,
+};
+
+/*
+ * BOOST regulator
+ */
+
+static int stpmic1_boost_get_enable(struct udevice *dev)
+{
+	int ret;
+
+	ret = pmic_reg_read(dev->parent, STPMIC1_BST_SW_CR);
+	if (ret < 0)
+		return false;
+
+	return ret & STPMIC1_BST_ON ? true : false;
+}
+
+static int stpmic1_boost_set_enable(struct udevice *dev, bool enable)
+{
+	int ret;
+
+	ret = pmic_reg_read(dev->parent, STPMIC1_BST_SW_CR);
+	if (ret < 0)
+		return ret;
+
+	if (!enable && ret & STPMIC1_PWR_SW_ON)
+		return -EINVAL;
+
+	/* if regulator is already in the wanted state, nothing to do */
+	if (!!(ret & STPMIC1_BST_ON) == enable)
+		return 0;
+
+	ret = pmic_clrsetbits(dev->parent, STPMIC1_BST_SW_CR,
+			      STPMIC1_BST_ON,
+			      enable ? STPMIC1_BST_ON : 0);
+	if (enable)
+		mdelay(STPMIC1_USB_BOOST_START_UP_DELAY_MS);
+
+	return ret;
+}
+
+static int stpmic1_boost_probe(struct udevice *dev)
+{
+	struct dm_regulator_uclass_platdata *uc_pdata;
+
+	uc_pdata = dev_get_uclass_platdata(dev);
+
+	uc_pdata->type = REGULATOR_TYPE_FIXED;
+	uc_pdata->mode_count = 0;
+
+	return 0;
+}
+
+static const struct dm_regulator_ops stpmic1_boost_ops = {
+	.get_enable = stpmic1_boost_get_enable,
+	.set_enable = stpmic1_boost_set_enable,
+};
+
+U_BOOT_DRIVER(stpmic1_boost) = {
+	.name = "stpmic1_boost",
+	.id = UCLASS_REGULATOR,
+	.ops = &stpmic1_boost_ops,
+	.probe = stpmic1_boost_probe,
+};
+
+/*
+ * USB power switch
+ */
+
+static int stpmic1_pwr_sw_get_enable(struct udevice *dev)
+{
+	uint mask = 1 << dev->driver_data;
+	int ret;
+
+	ret = pmic_reg_read(dev->parent, STPMIC1_BST_SW_CR);
+	if (ret < 0)
+		return false;
+
+	return ret & mask ? true : false;
+}
+
+static int stpmic1_pwr_sw_set_enable(struct udevice *dev, bool enable)
+{
+	uint mask = 1 << dev->driver_data;
+	int delay = enable ? STPMIC1_DEFAULT_START_UP_DELAY_MS :
+			     STPMIC1_DEFAULT_STOP_DELAY_MS;
+	int ret;
+
+	ret = pmic_reg_read(dev->parent, STPMIC1_BST_SW_CR);
+	if (ret < 0)
+		return ret;
+
+	/* if regulator is already in the wanted state, nothing to do */
+	if (!!(ret & mask) == enable)
+		return 0;
+
+	/* Boost management */
+	if (enable && !(ret & STPMIC1_BST_ON)) {
+		pmic_clrsetbits(dev->parent, STPMIC1_BST_SW_CR,
+				STPMIC1_BST_ON, STPMIC1_BST_ON);
+		mdelay(STPMIC1_USB_BOOST_START_UP_DELAY_MS);
+	} else if (!enable && ret & STPMIC1_BST_ON &&
+		   (ret & STPMIC1_PWR_SW_ON) != STPMIC1_PWR_SW_ON) {
+		pmic_clrsetbits(dev->parent, STPMIC1_BST_SW_CR,
+				STPMIC1_BST_ON, 0);
+	}
+
+	ret = pmic_clrsetbits(dev->parent, STPMIC1_BST_SW_CR,
+			      mask, enable ? mask : 0);
+	mdelay(delay);
+
+	return ret;
+}
+
+static int stpmic1_pwr_sw_probe(struct udevice *dev)
+{
+	struct dm_regulator_uclass_platdata *uc_pdata;
+
+	if (!dev->driver_data || dev->driver_data > STPMIC1_MAX_PWR_SW)
+		return -EINVAL;
+
+	uc_pdata = dev_get_uclass_platdata(dev);
+
+	uc_pdata->type = REGULATOR_TYPE_FIXED;
+	uc_pdata->mode_count = 0;
+
+	return 0;
+}
+
+static const struct dm_regulator_ops stpmic1_pwr_sw_ops = {
+	.get_enable = stpmic1_pwr_sw_get_enable,
+	.set_enable = stpmic1_pwr_sw_set_enable,
+};
+
+U_BOOT_DRIVER(stpmic1_pwr_sw) = {
+	.name = "stpmic1_pwr_sw",
+	.id = UCLASS_REGULATOR,
+	.ops = &stpmic1_pwr_sw_ops,
+	.probe = stpmic1_pwr_sw_probe,
+};
diff --git a/drivers/power/regulator/stpmu1.c b/drivers/power/regulator/stpmu1.c
deleted file mode 100644
index 6eb2420b6b..0000000000
--- a/drivers/power/regulator/stpmu1.c
+++ /dev/null
@@ -1,671 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
-/*
- * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
- * Author: Christophe Kerello <christophe.kerello@st.com>
- */
-
-#include <common.h>
-#include <dm.h>
-#include <errno.h>
-#include <power/pmic.h>
-#include <power/regulator.h>
-#include <power/stpmu1.h>
-
-struct stpmu1_range {
-	int min_uv;
-	int min_sel;
-	int max_sel;
-	int step;
-};
-
-struct stpmu1_output_range {
-	const struct stpmu1_range *ranges;
-	int nbranges;
-};
-
-#define STPMU1_MODE(_id, _val, _name) { \
-	.id = _id,			\
-	.register_value = _val,		\
-	.name = _name,			\
-}
-
-#define STPMU1_RANGE(_min_uv, _min_sel, _max_sel, _step) { \
-	.min_uv = _min_uv,		\
-	.min_sel = _min_sel,		\
-	.max_sel = _max_sel,		\
-	.step = _step,			\
-}
-
-#define STPMU1_OUTPUT_RANGE(_ranges, _nbranges) { \
-	.ranges = _ranges,		\
-	.nbranges = _nbranges,		\
-}
-
-static int stpmu1_output_find_uv(int sel,
-				 const struct stpmu1_output_range *output_range)
-{
-	const struct stpmu1_range *range;
-	int i;
-
-	for (i = 0, range = output_range->ranges;
-	     i < output_range->nbranges; i++, range++) {
-		if (sel >= range->min_sel && sel <= range->max_sel)
-			return range->min_uv +
-			       (sel - range->min_sel) * range->step;
-	}
-
-	return -EINVAL;
-}
-
-static int stpmu1_output_find_sel(int uv,
-				  const struct stpmu1_output_range *output_range)
-{
-	const struct stpmu1_range *range;
-	int i;
-
-	for (i = 0, range = output_range->ranges;
-	     i < output_range->nbranges; i++, range++) {
-		if (uv == range->min_uv && !range->step)
-			return range->min_sel;
-
-		if (uv >= range->min_uv &&
-		    uv <= range->min_uv +
-			  (range->max_sel - range->min_sel) * range->step)
-			return range->min_sel +
-			       (uv - range->min_uv) / range->step;
-	}
-
-	return -EINVAL;
-}
-
-/*
- * BUCK regulators
- */
-
-static const struct stpmu1_range buck1_ranges[] = {
-	STPMU1_RANGE(600000, 0, 30, 25000),
-	STPMU1_RANGE(1350000, 31, 63, 0),
-};
-
-static const struct stpmu1_range buck2_ranges[] = {
-	STPMU1_RANGE(1000000, 0, 17, 0),
-	STPMU1_RANGE(1050000, 18, 19, 0),
-	STPMU1_RANGE(1100000, 20, 21, 0),
-	STPMU1_RANGE(1150000, 22, 23, 0),
-	STPMU1_RANGE(1200000, 24, 25, 0),
-	STPMU1_RANGE(1250000, 26, 27, 0),
-	STPMU1_RANGE(1300000, 28, 29, 0),
-	STPMU1_RANGE(1350000, 30, 31, 0),
-	STPMU1_RANGE(1400000, 32, 33, 0),
-	STPMU1_RANGE(1450000, 34, 35, 0),
-	STPMU1_RANGE(1500000, 36, 63, 0),
-};
-
-static const struct stpmu1_range buck3_ranges[] = {
-	STPMU1_RANGE(1000000, 0, 19, 0),
-	STPMU1_RANGE(1100000, 20, 23, 0),
-	STPMU1_RANGE(1200000, 24, 27, 0),
-	STPMU1_RANGE(1300000, 28, 31, 0),
-	STPMU1_RANGE(1400000, 32, 35, 0),
-	STPMU1_RANGE(1500000, 36, 55, 100000),
-	STPMU1_RANGE(3400000, 56, 63, 0),
-};
-
-static const struct stpmu1_range buck4_ranges[] = {
-	STPMU1_RANGE(600000, 0, 27, 25000),
-	STPMU1_RANGE(1300000, 28, 29, 0),
-	STPMU1_RANGE(1350000, 30, 31, 0),
-	STPMU1_RANGE(1400000, 32, 33, 0),
-	STPMU1_RANGE(1450000, 34, 35, 0),
-	STPMU1_RANGE(1500000, 36, 60, 100000),
-	STPMU1_RANGE(3900000, 61, 63, 0),
-};
-
-/* BUCK: 1,2,3,4 - voltage ranges */
-static const struct stpmu1_output_range buck_voltage_range[] = {
-	STPMU1_OUTPUT_RANGE(buck1_ranges, ARRAY_SIZE(buck1_ranges)),
-	STPMU1_OUTPUT_RANGE(buck2_ranges, ARRAY_SIZE(buck2_ranges)),
-	STPMU1_OUTPUT_RANGE(buck3_ranges, ARRAY_SIZE(buck3_ranges)),
-	STPMU1_OUTPUT_RANGE(buck4_ranges, ARRAY_SIZE(buck4_ranges)),
-};
-
-/* BUCK modes */
-static const struct dm_regulator_mode buck_modes[] = {
-	STPMU1_MODE(STPMU1_BUCK_MODE_HP, STPMU1_BUCK_MODE_HP, "HP"),
-	STPMU1_MODE(STPMU1_BUCK_MODE_LP, STPMU1_BUCK_MODE_LP, "LP"),
-};
-
-static int stpmu1_buck_get_uv(struct udevice *dev, int buck)
-{
-	int sel;
-
-	sel = pmic_reg_read(dev, STPMU1_BUCKX_CTRL_REG(buck));
-	if (sel < 0)
-		return sel;
-
-	sel &= STPMU1_BUCK_OUTPUT_MASK;
-	sel >>= STPMU1_BUCK_OUTPUT_SHIFT;
-
-	return stpmu1_output_find_uv(sel, &buck_voltage_range[buck]);
-}
-
-static int stpmu1_buck_get_value(struct udevice *dev)
-{
-	return stpmu1_buck_get_uv(dev->parent, dev->driver_data - 1);
-}
-
-static int stpmu1_buck_set_value(struct udevice *dev, int uv)
-{
-	int sel, buck = dev->driver_data - 1;
-
-	sel = stpmu1_output_find_sel(uv, &buck_voltage_range[buck]);
-	if (sel < 0)
-		return sel;
-
-	return pmic_clrsetbits(dev->parent,
-			       STPMU1_BUCKX_CTRL_REG(buck),
-			       STPMU1_BUCK_OUTPUT_MASK,
-			       sel << STPMU1_BUCK_OUTPUT_SHIFT);
-}
-
-static int stpmu1_buck_get_enable(struct udevice *dev)
-{
-	int ret;
-
-	ret = pmic_reg_read(dev->parent,
-			    STPMU1_BUCKX_CTRL_REG(dev->driver_data - 1));
-	if (ret < 0)
-		return false;
-
-	return ret & STPMU1_BUCK_EN ? true : false;
-}
-
-static int stpmu1_buck_set_enable(struct udevice *dev, bool enable)
-{
-	struct dm_regulator_uclass_platdata *uc_pdata;
-	int delay = enable ? STPMU1_DEFAULT_START_UP_DELAY_MS :
-			     STPMU1_DEFAULT_STOP_DELAY_MS;
-	int ret, uv;
-
-	/* if regulator is already in the wanted state, nothing to do */
-	if (stpmu1_buck_get_enable(dev) == enable)
-		return 0;
-
-	if (enable) {
-		uc_pdata = dev_get_uclass_platdata(dev);
-		uv = stpmu1_buck_get_value(dev);
-		if ((uv < uc_pdata->min_uV) || (uv > uc_pdata->max_uV))
-			stpmu1_buck_set_value(dev, uc_pdata->min_uV);
-	}
-
-	ret = pmic_clrsetbits(dev->parent,
-			      STPMU1_BUCKX_CTRL_REG(dev->driver_data - 1),
-			      STPMU1_BUCK_EN, enable ? STPMU1_BUCK_EN : 0);
-	mdelay(delay);
-
-	return ret;
-}
-
-static int stpmu1_buck_get_mode(struct udevice *dev)
-{
-	int ret;
-
-	ret = pmic_reg_read(dev->parent,
-			    STPMU1_BUCKX_CTRL_REG(dev->driver_data - 1));
-	if (ret < 0)
-		return ret;
-
-	return ret & STPMU1_BUCK_MODE ? STPMU1_BUCK_MODE_LP :
-					 STPMU1_BUCK_MODE_HP;
-}
-
-static int stpmu1_buck_set_mode(struct udevice *dev, int mode)
-{
-	return pmic_clrsetbits(dev->parent,
-			       STPMU1_BUCKX_CTRL_REG(dev->driver_data - 1),
-			       STPMU1_BUCK_MODE,
-			       mode ? STPMU1_BUCK_MODE : 0);
-}
-
-static int stpmu1_buck_probe(struct udevice *dev)
-{
-	struct dm_regulator_uclass_platdata *uc_pdata;
-
-	if (!dev->driver_data || dev->driver_data > STPMU1_MAX_BUCK)
-		return -EINVAL;
-
-	uc_pdata = dev_get_uclass_platdata(dev);
-
-	uc_pdata->type = REGULATOR_TYPE_BUCK;
-	uc_pdata->mode = (struct dm_regulator_mode *)buck_modes;
-	uc_pdata->mode_count = ARRAY_SIZE(buck_modes);
-
-	return 0;
-}
-
-static const struct dm_regulator_ops stpmu1_buck_ops = {
-	.get_value  = stpmu1_buck_get_value,
-	.set_value  = stpmu1_buck_set_value,
-	.get_enable = stpmu1_buck_get_enable,
-	.set_enable = stpmu1_buck_set_enable,
-	.get_mode   = stpmu1_buck_get_mode,
-	.set_mode   = stpmu1_buck_set_mode,
-};
-
-U_BOOT_DRIVER(stpmu1_buck) = {
-	.name = "stpmu1_buck",
-	.id = UCLASS_REGULATOR,
-	.ops = &stpmu1_buck_ops,
-	.probe = stpmu1_buck_probe,
-};
-
-/*
- * LDO regulators
- */
-
-static const struct stpmu1_range ldo12_ranges[] = {
-	STPMU1_RANGE(1700000, 0, 7, 0),
-	STPMU1_RANGE(1700000, 8, 24, 100000),
-	STPMU1_RANGE(3300000, 25, 31, 0),
-};
-
-static const struct stpmu1_range ldo3_ranges[] = {
-	STPMU1_RANGE(1700000, 0, 7, 0),
-	STPMU1_RANGE(1700000, 8, 24, 100000),
-	STPMU1_RANGE(3300000, 25, 30, 0),
-	/* Sel 31 is special case when LDO3 is in mode sync_source (BUCK2/2) */
-};
-
-static const struct stpmu1_range ldo5_ranges[] = {
-	STPMU1_RANGE(1700000, 0, 7, 0),
-	STPMU1_RANGE(1700000, 8, 30, 100000),
-	STPMU1_RANGE(3900000, 31, 31, 0),
-};
-
-static const struct stpmu1_range ldo6_ranges[] = {
-	STPMU1_RANGE(900000, 0, 24, 100000),
-	STPMU1_RANGE(3300000, 25, 31, 0),
-};
-
-/* LDO: 1,2,3,4,5,6 - voltage ranges */
-static const struct stpmu1_output_range ldo_voltage_range[] = {
-	STPMU1_OUTPUT_RANGE(ldo12_ranges, ARRAY_SIZE(ldo12_ranges)),
-	STPMU1_OUTPUT_RANGE(ldo12_ranges, ARRAY_SIZE(ldo12_ranges)),
-	STPMU1_OUTPUT_RANGE(ldo3_ranges, ARRAY_SIZE(ldo3_ranges)),
-	STPMU1_OUTPUT_RANGE(NULL, 0),
-	STPMU1_OUTPUT_RANGE(ldo5_ranges, ARRAY_SIZE(ldo5_ranges)),
-	STPMU1_OUTPUT_RANGE(ldo6_ranges, ARRAY_SIZE(ldo6_ranges)),
-};
-
-/* LDO modes */
-static const struct dm_regulator_mode ldo_modes[] = {
-	STPMU1_MODE(STPMU1_LDO_MODE_NORMAL,
-		    STPMU1_LDO_MODE_NORMAL, "NORMAL"),
-	STPMU1_MODE(STPMU1_LDO_MODE_BYPASS,
-		    STPMU1_LDO_MODE_BYPASS, "BYPASS"),
-	STPMU1_MODE(STPMU1_LDO_MODE_SINK_SOURCE,
-		    STPMU1_LDO_MODE_SINK_SOURCE, "SINK SOURCE"),
-};
-
-static int stpmu1_ldo_get_value(struct udevice *dev)
-{
-	int sel, ldo = dev->driver_data - 1;
-
-	sel = pmic_reg_read(dev->parent, STPMU1_LDOX_CTRL_REG(ldo));
-	if (sel < 0)
-		return sel;
-
-	/* ldo4 => 3,3V */
-	if (ldo == STPMU1_LDO4)
-		return STPMU1_LDO4_UV;
-
-	sel &= STPMU1_LDO12356_OUTPUT_MASK;
-	sel >>= STPMU1_LDO12356_OUTPUT_SHIFT;
-
-	/* ldo3, sel = 31 => BUCK2/2 */
-	if (ldo == STPMU1_LDO3 && sel == STPMU1_LDO3_DDR_SEL)
-		return stpmu1_buck_get_uv(dev->parent, STPMU1_BUCK2) / 2;
-
-	return stpmu1_output_find_uv(sel, &ldo_voltage_range[ldo]);
-}
-
-static int stpmu1_ldo_set_value(struct udevice *dev, int uv)
-{
-	int sel, ldo = dev->driver_data - 1;
-
-	/* ldo4 => not possible */
-	if (ldo == STPMU1_LDO4)
-		return -EINVAL;
-
-	sel = stpmu1_output_find_sel(uv, &ldo_voltage_range[ldo]);
-	if (sel < 0)
-		return sel;
-
-	return pmic_clrsetbits(dev->parent,
-			       STPMU1_LDOX_CTRL_REG(ldo),
-			       STPMU1_LDO12356_OUTPUT_MASK,
-			       sel << STPMU1_LDO12356_OUTPUT_SHIFT);
-}
-
-static int stpmu1_ldo_get_enable(struct udevice *dev)
-{
-	int ret;
-
-	ret = pmic_reg_read(dev->parent,
-			    STPMU1_LDOX_CTRL_REG(dev->driver_data - 1));
-	if (ret < 0)
-		return false;
-
-	return ret & STPMU1_LDO_EN ? true : false;
-}
-
-static int stpmu1_ldo_set_enable(struct udevice *dev, bool enable)
-{
-	struct dm_regulator_uclass_platdata *uc_pdata;
-	int delay = enable ? STPMU1_DEFAULT_START_UP_DELAY_MS :
-			     STPMU1_DEFAULT_STOP_DELAY_MS;
-	int ret, uv;
-
-	/* if regulator is already in the wanted state, nothing to do */
-	if (stpmu1_ldo_get_enable(dev) == enable)
-		return 0;
-
-	if (enable) {
-		uc_pdata = dev_get_uclass_platdata(dev);
-		uv = stpmu1_ldo_get_value(dev);
-		if ((uv < uc_pdata->min_uV) || (uv > uc_pdata->max_uV))
-			stpmu1_ldo_set_value(dev, uc_pdata->min_uV);
-	}
-
-	ret = pmic_clrsetbits(dev->parent,
-			      STPMU1_LDOX_CTRL_REG(dev->driver_data - 1),
-			      STPMU1_LDO_EN, enable ? STPMU1_LDO_EN : 0);
-	mdelay(delay);
-
-	return ret;
-}
-
-static int stpmu1_ldo_get_mode(struct udevice *dev)
-{
-	int ret, ldo = dev->driver_data - 1;
-
-	if (ldo != STPMU1_LDO3)
-		return -EINVAL;
-
-	ret = pmic_reg_read(dev->parent, STPMU1_LDOX_CTRL_REG(ldo));
-	if (ret < 0)
-		return ret;
-
-	if (ret & STPMU1_LDO3_MODE)
-		return STPMU1_LDO_MODE_BYPASS;
-
-	ret &= STPMU1_LDO12356_OUTPUT_MASK;
-	ret >>= STPMU1_LDO12356_OUTPUT_SHIFT;
-
-	return ret == STPMU1_LDO3_DDR_SEL ? STPMU1_LDO_MODE_SINK_SOURCE :
-					     STPMU1_LDO_MODE_NORMAL;
-}
-
-static int stpmu1_ldo_set_mode(struct udevice *dev, int mode)
-{
-	int ret, ldo = dev->driver_data - 1;
-
-	if (ldo != STPMU1_LDO3)
-		return -EINVAL;
-
-	ret = pmic_reg_read(dev->parent, STPMU1_LDOX_CTRL_REG(ldo));
-	if (ret < 0)
-		return ret;
-
-	switch (mode) {
-	case STPMU1_LDO_MODE_SINK_SOURCE:
-		ret &= ~STPMU1_LDO12356_OUTPUT_MASK;
-		ret |= STPMU1_LDO3_DDR_SEL << STPMU1_LDO12356_OUTPUT_SHIFT;
-	case STPMU1_LDO_MODE_NORMAL:
-		ret &= ~STPMU1_LDO3_MODE;
-		break;
-	case STPMU1_LDO_MODE_BYPASS:
-		ret |= STPMU1_LDO3_MODE;
-		break;
-	}
-
-	return pmic_reg_write(dev->parent, STPMU1_LDOX_CTRL_REG(ldo), ret);
-}
-
-static int stpmu1_ldo_probe(struct udevice *dev)
-{
-	struct dm_regulator_uclass_platdata *uc_pdata;
-
-	if (!dev->driver_data || dev->driver_data > STPMU1_MAX_LDO)
-		return -EINVAL;
-
-	uc_pdata = dev_get_uclass_platdata(dev);
-
-	uc_pdata->type = REGULATOR_TYPE_LDO;
-	if (dev->driver_data - 1 == STPMU1_LDO3) {
-		uc_pdata->mode = (struct dm_regulator_mode *)ldo_modes;
-		uc_pdata->mode_count = ARRAY_SIZE(ldo_modes);
-	} else {
-		uc_pdata->mode_count = 0;
-	}
-
-	return 0;
-}
-
-static const struct dm_regulator_ops stpmu1_ldo_ops = {
-	.get_value  = stpmu1_ldo_get_value,
-	.set_value  = stpmu1_ldo_set_value,
-	.get_enable = stpmu1_ldo_get_enable,
-	.set_enable = stpmu1_ldo_set_enable,
-	.get_mode   = stpmu1_ldo_get_mode,
-	.set_mode   = stpmu1_ldo_set_mode,
-};
-
-U_BOOT_DRIVER(stpmu1_ldo) = {
-	.name = "stpmu1_ldo",
-	.id = UCLASS_REGULATOR,
-	.ops = &stpmu1_ldo_ops,
-	.probe = stpmu1_ldo_probe,
-};
-
-/*
- * VREF DDR regulator
- */
-
-static int stpmu1_vref_ddr_get_value(struct udevice *dev)
-{
-	/* BUCK2/2 */
-	return stpmu1_buck_get_uv(dev->parent, STPMU1_BUCK2) / 2;
-}
-
-static int stpmu1_vref_ddr_get_enable(struct udevice *dev)
-{
-	int ret;
-
-	ret = pmic_reg_read(dev->parent, STPMU1_VREF_CTRL_REG);
-	if (ret < 0)
-		return false;
-
-	return ret & STPMU1_VREF_EN ? true : false;
-}
-
-static int stpmu1_vref_ddr_set_enable(struct udevice *dev, bool enable)
-{
-	int delay = enable ? STPMU1_DEFAULT_START_UP_DELAY_MS :
-			     STPMU1_DEFAULT_STOP_DELAY_MS;
-	int ret;
-
-	/* if regulator is already in the wanted state, nothing to do */
-	if (stpmu1_vref_ddr_get_enable(dev) == enable)
-		return 0;
-
-	ret = pmic_clrsetbits(dev->parent, STPMU1_VREF_CTRL_REG,
-			      STPMU1_VREF_EN, enable ? STPMU1_VREF_EN : 0);
-	mdelay(delay);
-
-	return ret;
-}
-
-static int stpmu1_vref_ddr_probe(struct udevice *dev)
-{
-	struct dm_regulator_uclass_platdata *uc_pdata;
-
-	uc_pdata = dev_get_uclass_platdata(dev);
-
-	uc_pdata->type = REGULATOR_TYPE_FIXED;
-	uc_pdata->mode_count = 0;
-
-	return 0;
-}
-
-static const struct dm_regulator_ops stpmu1_vref_ddr_ops = {
-	.get_value  = stpmu1_vref_ddr_get_value,
-	.get_enable = stpmu1_vref_ddr_get_enable,
-	.set_enable = stpmu1_vref_ddr_set_enable,
-};
-
-U_BOOT_DRIVER(stpmu1_vref_ddr) = {
-	.name = "stpmu1_vref_ddr",
-	.id = UCLASS_REGULATOR,
-	.ops = &stpmu1_vref_ddr_ops,
-	.probe = stpmu1_vref_ddr_probe,
-};
-
-/*
- * BOOST regulator
- */
-
-static int stpmu1_boost_get_enable(struct udevice *dev)
-{
-	int ret;
-
-	ret = pmic_reg_read(dev->parent, STPMU1_USB_CTRL_REG);
-	if (ret < 0)
-		return false;
-
-	return ret & STPMU1_USB_BOOST_EN ? true : false;
-}
-
-static int stpmu1_boost_set_enable(struct udevice *dev, bool enable)
-{
-	int ret;
-
-	ret = pmic_reg_read(dev->parent, STPMU1_USB_CTRL_REG);
-	if (ret < 0)
-		return ret;
-
-	if (!enable && ret & STPMU1_USB_PWR_SW_EN)
-		return -EINVAL;
-
-	/* if regulator is already in the wanted state, nothing to do */
-	if (!!(ret & STPMU1_USB_BOOST_EN) == enable)
-		return 0;
-
-	ret = pmic_clrsetbits(dev->parent, STPMU1_USB_CTRL_REG,
-			      STPMU1_USB_BOOST_EN,
-			      enable ? STPMU1_USB_BOOST_EN : 0);
-	if (enable)
-		mdelay(STPMU1_USB_BOOST_START_UP_DELAY_MS);
-
-	return ret;
-}
-
-static int stpmu1_boost_probe(struct udevice *dev)
-{
-	struct dm_regulator_uclass_platdata *uc_pdata;
-
-	uc_pdata = dev_get_uclass_platdata(dev);
-
-	uc_pdata->type = REGULATOR_TYPE_FIXED;
-	uc_pdata->mode_count = 0;
-
-	return 0;
-}
-
-static const struct dm_regulator_ops stpmu1_boost_ops = {
-	.get_enable = stpmu1_boost_get_enable,
-	.set_enable = stpmu1_boost_set_enable,
-};
-
-U_BOOT_DRIVER(stpmu1_boost) = {
-	.name = "stpmu1_boost",
-	.id = UCLASS_REGULATOR,
-	.ops = &stpmu1_boost_ops,
-	.probe = stpmu1_boost_probe,
-};
-
-/*
- * USB power switch
- */
-
-static int stpmu1_pwr_sw_get_enable(struct udevice *dev)
-{
-	uint mask = 1 << dev->driver_data;
-	int ret;
-
-	ret = pmic_reg_read(dev->parent, STPMU1_USB_CTRL_REG);
-	if (ret < 0)
-		return false;
-
-	return ret & mask ? true : false;
-}
-
-static int stpmu1_pwr_sw_set_enable(struct udevice *dev, bool enable)
-{
-	uint mask = 1 << dev->driver_data;
-	int delay = enable ? STPMU1_DEFAULT_START_UP_DELAY_MS :
-			     STPMU1_DEFAULT_STOP_DELAY_MS;
-	int ret;
-
-	ret = pmic_reg_read(dev->parent, STPMU1_USB_CTRL_REG);
-	if (ret < 0)
-		return ret;
-
-	/* if regulator is already in the wanted state, nothing to do */
-	if (!!(ret & mask) == enable)
-		return 0;
-
-	/* Boost management */
-	if (enable && !(ret & STPMU1_USB_BOOST_EN)) {
-		pmic_clrsetbits(dev->parent, STPMU1_USB_CTRL_REG,
-				STPMU1_USB_BOOST_EN, STPMU1_USB_BOOST_EN);
-		mdelay(STPMU1_USB_BOOST_START_UP_DELAY_MS);
-	} else if (!enable && ret & STPMU1_USB_BOOST_EN &&
-		   (ret & STPMU1_USB_PWR_SW_EN) != STPMU1_USB_PWR_SW_EN) {
-		pmic_clrsetbits(dev->parent, STPMU1_USB_CTRL_REG,
-				STPMU1_USB_BOOST_EN, 0);
-	}
-
-	ret = pmic_clrsetbits(dev->parent, STPMU1_USB_CTRL_REG,
-			      mask, enable ? mask : 0);
-	mdelay(delay);
-
-	return ret;
-}
-
-static int stpmu1_pwr_sw_probe(struct udevice *dev)
-{
-	struct dm_regulator_uclass_platdata *uc_pdata;
-
-	if (!dev->driver_data || dev->driver_data > STPMU1_MAX_PWR_SW)
-		return -EINVAL;
-
-	uc_pdata = dev_get_uclass_platdata(dev);
-
-	uc_pdata->type = REGULATOR_TYPE_FIXED;
-	uc_pdata->mode_count = 0;
-
-	return 0;
-}
-
-static const struct dm_regulator_ops stpmu1_pwr_sw_ops = {
-	.get_enable = stpmu1_pwr_sw_get_enable,
-	.set_enable = stpmu1_pwr_sw_set_enable,
-};
-
-U_BOOT_DRIVER(stpmu1_pwr_sw) = {
-	.name = "stpmu1_pwr_sw",
-	.id = UCLASS_REGULATOR,
-	.ops = &stpmu1_pwr_sw_ops,
-	.probe = stpmu1_pwr_sw_probe,
-};
diff --git a/drivers/ram/stm32mp1/stm32mp1_ram.c b/drivers/ram/stm32mp1/stm32mp1_ram.c
index bd497a3021..e45a3b2658 100644
--- a/drivers/ram/stm32mp1/stm32mp1_ram.c
+++ b/drivers/ram/stm32mp1/stm32mp1_ram.c
@@ -157,7 +157,8 @@ static int stm32mp1_ddr_probe(struct udevice *dev)
 
 	priv->info.base = STM32_DDR_BASE;
 
-#if !defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD)
+#if !defined(CONFIG_STM32MP1_TRUSTED) && \
+	(!defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD))
 	priv->info.size = 0;
 	return stm32mp1_ddr_setup(dev);
 #else
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index 098372e093..a700f240ad 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -222,8 +222,7 @@ config SPI_SUNXI
 
 config STM32_QSPI
 	bool "STM32F7 QSPI driver"
-	depends on STM32F7
-	imply SPI_FLASH_BAR
+	depends on STM32F7 || ARCH_STM32MP
 	help
 	  Enable the STM32F7 Quad-SPI (QSPI) driver. This driver can be
 	  used to access the SPI NOR flash chips on platforms embedding
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,
 };