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
author: Tom Rini <trini@konsulko.com> 2019-04-12 15:43:19 -0400
committer: Tom Rini <trini@konsulko.com> 2019-04-12 15:43:19 -0400
commit: 015289580f81733f7358227743e3e5881653a797 (patch)
tree: beca1eea1bfa7b4e4740b9e7a08493711b7b8bee
parent: dd758c6720266c9b7bcd8d0da91a64a2fa1e181a (diff)
parent: 7bb75023a720432a32840c6df543aae92653b23d (diff)
59 files changed, 5258 insertions, 1520 deletions
diff --git a/MAINTAINERS b/MAINTAINERS
index f9ee4281d9..083bf47c8b 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -295,7 +295,9 @@ F:	drivers/misc/stm32mp_fuse.c
 F:	drivers/mmc/stm32_sdmmc2.c
 F:	drivers/phy/phy-stm32-usbphyc.c
 F:	drivers/pinctrl/pinctrl_stm32.c
+F:	drivers/power/pmic/stpmic1.c
 F:	drivers/power/regulator/stm32-vrefbuf.c
+F:	drivers/power/regulator/stpmic1.c
 F:	drivers/ram/stm32mp1/
 F:	drivers/misc/stm32_rcc.c
 F:	drivers/reset/stm32-reset.c
diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 398dbef1cb..4640f3b3bd 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -1403,11 +1403,15 @@ config ARCH_STM32MP
 	select SYSRESET
 	select SYS_THUMB_BUILD
 	imply CMD_DM
+	imply CMD_POWEROFF
+	imply ENV_VARS_UBOOT_RUNTIME_CONFIG
 	help
 	  Support for STM32MP SoC family developed by STMicroelectronics,
 	  MPUs based on ARM cortex A core
-	  U-BOOT is running in DDR and SPL support is the unsecure First Stage
-	  BootLoader (FSBL)
+	  U-BOOT is running in DDR, loaded by the First Stage BootLoader (FSBL).
+	  FSBL can be TF-A: Trusted Firmware for Cortex A, for trusted boot
+	  chain.
+	  SPL is the unsecure FSBL for the basic boot chain.
 
 config ARCH_ROCKCHIP
 	bool "Support Rockchip SoCs"
diff --git a/arch/arm/dts/Makefile b/arch/arm/dts/Makefile
index e3633f5954..8167cdb4e8 100644
--- a/arch/arm/dts/Makefile
+++ b/arch/arm/dts/Makefile
@@ -688,6 +688,8 @@ dtb-$(CONFIG_ARCH_ASPEED) += ast2500-evb.dtb
 dtb-$(CONFIG_ARCH_STI) += stih410-b2260.dtb
 
 dtb-$(CONFIG_TARGET_STM32MP1) += \
+	stm32mp157a-dk1.dtb \
+	stm32mp157c-dk2.dtb \
 	stm32mp157c-ed1.dtb \
 	stm32mp157c-ev1.dtb
 
diff --git a/arch/arm/dts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi b/arch/arm/dts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi
new file mode 100644
index 0000000000..7d9b95ccf1
--- /dev/null
+++ b/arch/arm/dts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi
@@ -0,0 +1,120 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ *
+ * STM32MP157C DK1/DK2 BOARD configuration
+ * 1x DDR3L 4Gb, 16-bit, 533MHz.
+ * Reference used NT5CC256M16DP-DI from NANYA
+ *
+ * DDR type / Platform	DDR3/3L
+ * freq		533MHz
+ * width	16
+ * datasheet	0  = MT41J256M16-187 / DDR3-1066 bin G
+ * DDR density	4
+ * timing mode	optimized
+ * Scheduling/QoS options : type = 2
+ * address mapping : RBC
+ * Tc > + 85C : N
+ */
+#define DDR_MEM_NAME "DDR3-1066/888 bin G 1x4Gb 533MHz v1.43"
+#define DDR_MEM_SPEED 533
+#define DDR_MEM_SIZE 0x20000000
+
+#define DDR_MSTR 0x00041401
+#define DDR_MRCTRL0 0x00000010
+#define DDR_MRCTRL1 0x00000000
+#define DDR_DERATEEN 0x00000000
+#define DDR_DERATEINT 0x00800000
+#define DDR_PWRCTL 0x00000000
+#define DDR_PWRTMG 0x00400010
+#define DDR_HWLPCTL 0x00000000
+#define DDR_RFSHCTL0 0x00210000
+#define DDR_RFSHCTL3 0x00000000
+#define DDR_RFSHTMG 0x0081008B
+#define DDR_CRCPARCTL0 0x00000000
+#define DDR_DRAMTMG0 0x121B2414
+#define DDR_DRAMTMG1 0x000A041C
+#define DDR_DRAMTMG2 0x0608090F
+#define DDR_DRAMTMG3 0x0050400C
+#define DDR_DRAMTMG4 0x08040608
+#define DDR_DRAMTMG5 0x06060403
+#define DDR_DRAMTMG6 0x02020002
+#define DDR_DRAMTMG7 0x00000202
+#define DDR_DRAMTMG8 0x00001005
+#define DDR_DRAMTMG14 0x000000A0
+#define DDR_ZQCTL0 0xC2000040
+#define DDR_DFITMG0 0x02060105
+#define DDR_DFITMG1 0x00000202
+#define DDR_DFILPCFG0 0x07000000
+#define DDR_DFIUPD0 0xC0400003
+#define DDR_DFIUPD1 0x00000000
+#define DDR_DFIUPD2 0x00000000
+#define DDR_DFIPHYMSTR 0x00000000
+#define DDR_ADDRMAP1 0x00070707
+#define DDR_ADDRMAP2 0x00000000
+#define DDR_ADDRMAP3 0x1F000000
+#define DDR_ADDRMAP4 0x00001F1F
+#define DDR_ADDRMAP5 0x06060606
+#define DDR_ADDRMAP6 0x0F060606
+#define DDR_ADDRMAP9 0x00000000
+#define DDR_ADDRMAP10 0x00000000
+#define DDR_ADDRMAP11 0x00000000
+#define DDR_ODTCFG 0x06000600
+#define DDR_ODTMAP 0x00000001
+#define DDR_SCHED 0x00000C01
+#define DDR_SCHED1 0x00000000
+#define DDR_PERFHPR1 0x01000001
+#define DDR_PERFLPR1 0x08000200
+#define DDR_PERFWR1 0x08000400
+#define DDR_DBG0 0x00000000
+#define DDR_DBG1 0x00000000
+#define DDR_DBGCMD 0x00000000
+#define DDR_POISONCFG 0x00000000
+#define DDR_PCCFG 0x00000010
+#define DDR_PCFGR_0 0x00010000
+#define DDR_PCFGW_0 0x00000000
+#define DDR_PCFGQOS0_0 0x02100C03
+#define DDR_PCFGQOS1_0 0x00800100
+#define DDR_PCFGWQOS0_0 0x01100C03
+#define DDR_PCFGWQOS1_0 0x01000200
+#define DDR_PCFGR_1 0x00010000
+#define DDR_PCFGW_1 0x00000000
+#define DDR_PCFGQOS0_1 0x02100C03
+#define DDR_PCFGQOS1_1 0x00800040
+#define DDR_PCFGWQOS0_1 0x01100C03
+#define DDR_PCFGWQOS1_1 0x01000200
+#define DDR_PGCR 0x01442E02
+#define DDR_PTR0 0x0022AA5B
+#define DDR_PTR1 0x04841104
+#define DDR_PTR2 0x042DA068
+#define DDR_ACIOCR 0x10400812
+#define DDR_DXCCR 0x00000C40
+#define DDR_DSGCR 0xF200001F
+#define DDR_DCR 0x0000000B
+#define DDR_DTPR0 0x38D488D0
+#define DDR_DTPR1 0x098B00D8
+#define DDR_DTPR2 0x10023600
+#define DDR_MR0 0x00000840
+#define DDR_MR1 0x00000000
+#define DDR_MR2 0x00000208
+#define DDR_MR3 0x00000000
+#define DDR_ODTCR 0x00010000
+#define DDR_ZQ0CR1 0x00000038
+#define DDR_DX0GCR 0x0000CE81
+#define DDR_DX0DLLCR 0x40000000
+#define DDR_DX0DQTR 0xFFFFFFFF
+#define DDR_DX0DQSTR 0x3DB02000
+#define DDR_DX1GCR 0x0000CE81
+#define DDR_DX1DLLCR 0x40000000
+#define DDR_DX1DQTR 0xFFFFFFFF
+#define DDR_DX1DQSTR 0x3DB02000
+#define DDR_DX2GCR 0x0000CE81
+#define DDR_DX2DLLCR 0x40000000
+#define DDR_DX2DQTR 0xFFFFFFFF
+#define DDR_DX2DQSTR 0x3DB02000
+#define DDR_DX3GCR 0x0000CE81
+#define DDR_DX3DLLCR 0x40000000
+#define DDR_DX3DQTR 0xFFFFFFFF
+#define DDR_DX3DQSTR 0x3DB02000
+
+#include "stm32mp15-ddr.dtsi"
diff --git a/arch/arm/dts/stm32mp157-pinctrl.dtsi b/arch/arm/dts/stm32mp157-pinctrl.dtsi
index 85da592655..c069875486 100644
--- a/arch/arm/dts/stm32mp157-pinctrl.dtsi
+++ b/arch/arm/dts/stm32mp157-pinctrl.dtsi
@@ -148,6 +148,13 @@
 				gpio-ranges = <&pinctrl 0 160 8>;
 			};
 
+			adc12_usb_pwr_pins_a: adc12-usb-pwr-pins-0 {
+				pins {
+					pinmux = <STM32_PINMUX('A', 4, ANALOG)>, /* ADC12 in18 */
+						 <STM32_PINMUX('A', 5, ANALOG)>; /* ADC12 in19 */
+				};
+			};
+
 			cec_pins_a: cec-0 {
 				pins {
 					pinmux = <STM32_PINMUX('A', 15, AF4)>;
@@ -157,6 +164,52 @@
 				};
 			};
 
+			ethernet0_rgmii_pins_a: rgmii-0 {
+				pins1 {
+					pinmux = <STM32_PINMUX('G', 5, AF11)>, /* ETH_RGMII_CLK125 */
+						 <STM32_PINMUX('G', 4, AF11)>, /* ETH_RGMII_GTX_CLK */
+						 <STM32_PINMUX('G', 13, AF11)>, /* ETH_RGMII_TXD0 */
+						 <STM32_PINMUX('G', 14, AF11)>, /* ETH_RGMII_TXD1 */
+						 <STM32_PINMUX('C', 2, AF11)>, /* ETH_RGMII_TXD2 */
+						 <STM32_PINMUX('E', 2, AF11)>, /* ETH_RGMII_TXD3 */
+						 <STM32_PINMUX('B', 11, AF11)>, /* ETH_RGMII_TX_CTL */
+						 <STM32_PINMUX('A', 2, AF11)>, /* ETH_MDIO */
+						 <STM32_PINMUX('C', 1, AF11)>; /* ETH_MDC */
+					bias-disable;
+					drive-push-pull;
+					slew-rate = <3>;
+				};
+				pins2 {
+					pinmux = <STM32_PINMUX('C', 4, AF11)>, /* ETH_RGMII_RXD0 */
+						 <STM32_PINMUX('C', 5, AF11)>, /* ETH_RGMII_RXD1 */
+						 <STM32_PINMUX('B', 0, AF11)>, /* ETH_RGMII_RXD2 */
+						 <STM32_PINMUX('B', 1, AF11)>, /* ETH_RGMII_RXD3 */
+						 <STM32_PINMUX('A', 1, AF11)>, /* ETH_RGMII_RX_CLK */
+						 <STM32_PINMUX('A', 7, AF11)>; /* ETH_RGMII_RX_CTL */
+					bias-disable;
+				};
+			};
+
+			ethernet0_rgmii_pins_sleep_a: rgmii-sleep-0 {
+				pins1 {
+					pinmux = <STM32_PINMUX('G', 5, ANALOG)>, /* ETH_RGMII_CLK125 */
+						 <STM32_PINMUX('G', 4, ANALOG)>, /* ETH_RGMII_GTX_CLK */
+						 <STM32_PINMUX('G', 13, ANALOG)>, /* ETH_RGMII_TXD0 */
+						 <STM32_PINMUX('G', 14, ANALOG)>, /* ETH_RGMII_TXD1 */
+						 <STM32_PINMUX('C', 2, ANALOG)>, /* ETH_RGMII_TXD2 */
+						 <STM32_PINMUX('E', 2, ANALOG)>, /* ETH_RGMII_TXD3 */
+						 <STM32_PINMUX('B', 11, ANALOG)>, /* ETH_RGMII_TX_CTL */
+						 <STM32_PINMUX('A', 2, ANALOG)>, /* ETH_MDIO */
+						 <STM32_PINMUX('C', 1, ANALOG)>, /* ETH_MDC */
+						 <STM32_PINMUX('C', 4, ANALOG)>, /* ETH_RGMII_RXD0 */
+						 <STM32_PINMUX('C', 5, ANALOG)>, /* ETH_RGMII_RXD1 */
+						 <STM32_PINMUX('B', 0, ANALOG)>, /* ETH_RGMII_RXD2 */
+						 <STM32_PINMUX('B', 1, ANALOG)>, /* ETH_RGMII_RXD3 */
+						 <STM32_PINMUX('A', 1, ANALOG)>, /* ETH_RGMII_RX_CLK */
+						 <STM32_PINMUX('A', 7, ANALOG)>; /* ETH_RGMII_RX_CTL */
+				};
+			};
+
 			i2c1_pins_a: i2c1-0 {
 				pins {
 					pinmux = <STM32_PINMUX('D', 12, AF5)>, /* I2C1_SCL */
@@ -187,6 +240,19 @@
 				};
 			};
 
+			m_can1_pins_a: m-can1-0 {
+				pins1 {
+					pinmux = <STM32_PINMUX('H', 13, AF9)>; /* CAN1_TX */
+					slew-rate = <1>;
+					drive-push-pull;
+					bias-disable;
+				};
+				pins2 {
+					pinmux = <STM32_PINMUX('I', 9, AF9)>; /* CAN1_RX */
+					bias-disable;
+				};
+			};
+
 			pwm2_pins_a: pwm2-0 {
 				pins {
 					pinmux = <STM32_PINMUX('A', 3, AF1)>; /* TIM2_CH4 */
@@ -360,6 +426,21 @@
 					slew-rate = <0>;
 				};
 			};
+
+			spi1_pins_a: spi1-0 {
+				pins1 {
+					pinmux = <STM32_PINMUX('Z', 0, AF5)>, /* SPI1_SCK */
+						 <STM32_PINMUX('Z', 2, AF5)>; /* SPI1_MOSI */
+					bias-disable;
+					drive-push-pull;
+					slew-rate = <1>;
+				};
+
+				pins2 {
+					pinmux = <STM32_PINMUX('Z', 1, AF5)>; /* SPI1_MISO */
+					bias-disable;
+				};
+			};
 		};
 	};
 };
diff --git a/arch/arm/dts/stm32mp157-u-boot.dtsi b/arch/arm/dts/stm32mp157-u-boot.dtsi
index 90d13f35c4..ab6f673ea2 100644
--- a/arch/arm/dts/stm32mp157-u-boot.dtsi
+++ b/arch/arm/dts/stm32mp157-u-boot.dtsi
@@ -17,6 +17,8 @@
 		gpio9 = &gpioj;
 		gpio10 = &gpiok;
 		gpio25 = &gpioz;
+		pinctrl0 = &pinctrl;
+		pinctrl1 = &pinctrl_z;
 	};
 
 	config {
@@ -39,6 +41,10 @@
 	};
 };
 
+&bsec {
+	u-boot,dm-pre-reloc;
+};
+
 &clk_hsi {
 	u-boot,dm-pre-reloc;
 };
diff --git a/arch/arm/dts/stm32mp157a-dk1-u-boot.dtsi b/arch/arm/dts/stm32mp157a-dk1-u-boot.dtsi
new file mode 100644
index 0000000000..af7acfa037
--- /dev/null
+++ b/arch/arm/dts/stm32mp157a-dk1-u-boot.dtsi
@@ -0,0 +1,199 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright : STMicroelectronics 2018
+ */
+
+#include <dt-bindings/clock/stm32mp1-clksrc.h>
+#include "stm32mp157-u-boot.dtsi"
+#include "stm32mp15-ddr3-1x4Gb-1066-binG.dtsi"
+
+/ {
+	aliases {
+		i2c3 = &i2c4;
+		mmc0 = &sdmmc1;
+	};
+	config {
+		u-boot,boot-led = "heartbeat";
+		u-boot,error-led = "error";
+		st,adc_usb_pd = <&adc1 18>, <&adc1 19>;
+	};
+	led {
+		red {
+			label = "error";
+			gpios = <&gpioa 13 GPIO_ACTIVE_LOW>;
+			default-state = "off";
+			status = "okay";
+		};
+
+		blue {
+			default-state = "on";
+		};
+	};
+};
+
+&adc {
+	pinctrl-names = "default";
+	pinctrl-0 = <&adc12_usb_pwr_pins_a>;
+	vdd-supply = <&vdd>;
+	vdda-supply = <&vdd>;
+	vref-supply = <&vrefbuf>;
+	status = "okay";
+	adc1: adc@0 {
+		/*
+		 * Type-C USB_PWR_CC1 & USB_PWR_CC2 on in18 & in19.
+		 * Use at least 5 * RC time, e.g. 5 * (Rp + Rd) * C:
+		 * 5 * (56 + 47kOhms) * 5pF => 2.5us.
+		 * Use arbitrary margin here (e.g. 5µs).
+		 */
+		st,min-sample-time-nsecs = <5000>;
+		/* ANA0, ANA1, USB Type-C CC1 & CC2 */
+		st,adc-channels = <0 1 18 19>;
+		status = "okay";
+	};
+};
+
+&clk_hse {
+	st,digbypass;
+};
+
+&i2c4 {
+	u-boot,dm-pre-reloc;
+};
+
+&i2c4_pins_a {
+	u-boot,dm-pre-reloc;
+	pins {
+		u-boot,dm-pre-reloc;
+	};
+};
+
+&pmic {
+	u-boot,dm-pre-reloc;
+};
+
+&rcc {
+	st,clksrc = <
+		CLK_MPU_PLL1P
+		CLK_AXI_PLL2P
+		CLK_MCU_PLL3P
+		CLK_PLL12_HSE
+		CLK_PLL3_HSE
+		CLK_PLL4_HSE
+		CLK_RTC_LSE
+		CLK_MCO1_DISABLED
+		CLK_MCO2_DISABLED
+	>;
+
+	st,clkdiv = <
+		1 /*MPU*/
+		0 /*AXI*/
+		0 /*MCU*/
+		1 /*APB1*/
+		1 /*APB2*/
+		1 /*APB3*/
+		1 /*APB4*/
+		2 /*APB5*/
+		23 /*RTC*/
+		0 /*MCO1*/
+		0 /*MCO2*/
+	>;
+
+	st,pkcs = <
+		CLK_CKPER_HSE
+		CLK_FMC_ACLK
+		CLK_QSPI_ACLK
+		CLK_ETH_DISABLED
+		CLK_SDMMC12_PLL4P
+		CLK_DSI_DSIPLL
+		CLK_STGEN_HSE
+		CLK_USBPHY_HSE
+		CLK_SPI2S1_PLL3Q
+		CLK_SPI2S23_PLL3Q
+		CLK_SPI45_HSI
+		CLK_SPI6_HSI
+		CLK_I2C46_HSI
+		CLK_SDMMC3_PLL4P
+		CLK_USBO_USBPHY
+		CLK_ADC_CKPER
+		CLK_CEC_LSE
+		CLK_I2C12_HSI
+		CLK_I2C35_HSI
+		CLK_UART1_HSI
+		CLK_UART24_HSI
+		CLK_UART35_HSI
+		CLK_UART6_HSI
+		CLK_UART78_HSI
+		CLK_SPDIF_PLL4P
+		CLK_FDCAN_PLL4Q
+		CLK_SAI1_PLL3Q
+		CLK_SAI2_PLL3Q
+		CLK_SAI3_PLL3Q
+		CLK_SAI4_PLL3Q
+		CLK_RNG1_LSI
+		CLK_RNG2_LSI
+		CLK_LPTIM1_PCLK1
+		CLK_LPTIM23_PCLK3
+		CLK_LPTIM45_LSE
+	>;
+
+	/* VCO = 1300.0 MHz => P = 650 (CPU) */
+	pll1: st,pll@0 {
+		cfg = < 2 80 0 0 0 PQR(1,0,0) >;
+		frac = < 0x800 >;
+		u-boot,dm-pre-reloc;
+	};
+
+	/* VCO = 1066.0 MHz => P = 266 (AXI), Q = 533 (GPU), R = 533 (DDR) */
+	pll2: st,pll@1 {
+		cfg = < 2 65 1 0 0 PQR(1,1,1) >;
+		frac = < 0x1400 >;
+		u-boot,dm-pre-reloc;
+	};
+
+	/* VCO = 417.8 MHz => P = 209, Q = 24, R = 11 */
+	pll3: st,pll@2 {
+		cfg = < 1 33 1 16 36 PQR(1,1,1) >;
+		frac = < 0x1a04 >;
+		u-boot,dm-pre-reloc;
+	};
+
+	/* VCO = 594.0 MHz => P = 99, Q = 74, R = 74 */
+	pll4: st,pll@3 {
+		cfg = < 3 98 5 7 7 PQR(1,1,1) >;
+		u-boot,dm-pre-reloc;
+	};
+};
+
+&sdmmc1 {
+	u-boot,dm-spl;
+};
+
+&sdmmc1_b4_pins_a {
+	u-boot,dm-spl;
+	pins {
+		u-boot,dm-spl;
+	};
+};
+
+&uart4 {
+	u-boot,dm-pre-reloc;
+};
+
+&uart4_pins_a {
+	u-boot,dm-pre-reloc;
+	pins1 {
+		u-boot,dm-pre-reloc;
+	};
+	pins2 {
+		u-boot,dm-pre-reloc;
+	};
+};
+
+&usbotg_hs {
+	usb1600;
+	hnp-srp-disable;
+};
+
+&v3v3 {
+	regulator-always-on;
+};
diff --git a/arch/arm/dts/stm32mp157a-dk1.dts b/arch/arm/dts/stm32mp157a-dk1.dts
new file mode 100644
index 0000000000..0882765d0c
--- /dev/null
+++ b/arch/arm/dts/stm32mp157a-dk1.dts
@@ -0,0 +1,262 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
+ */
+
+/dts-v1/;
+
+#include "stm32mp157c.dtsi"
+#include "stm32mp157-pinctrl.dtsi"
+#include <dt-bindings/gpio/gpio.h>
+#include <dt-bindings/mfd/st,stpmic1.h>
+
+/ {
+	model = "STMicroelectronics STM32MP157A-DK1 Discovery Board";
+	compatible = "st,stm32mp157a-dk1", "st,stm32mp157";
+
+	aliases {
+		ethernet0 = &ethernet0;
+		serial0 = &uart4;
+	};
+
+	chosen {
+		stdout-path = "serial0:115200n8";
+	};
+
+	memory@c0000000 {
+		reg = <0xc0000000 0x20000000>;
+	};
+
+	led {
+		compatible = "gpio-leds";
+		blue {
+			label = "heartbeat";
+			gpios = <&gpiod 11 GPIO_ACTIVE_HIGH>;
+			linux,default-trigger = "heartbeat";
+			default-state = "off";
+		};
+	};
+};
+
+&ethernet0 {
+	status = "okay";
+	pinctrl-0 = <&ethernet0_rgmii_pins_a>;
+	pinctrl-1 = <&ethernet0_rgmii_pins_sleep_a>;
+	pinctrl-names = "default", "sleep";
+	phy-mode = "rgmii";
+	max-speed = <1000>;
+	phy-handle = <&phy0>;
+
+	mdio0 {
+		#address-cells = <1>;
+		#size-cells = <0>;
+		compatible = "snps,dwmac-mdio";
+		phy0: ethernet-phy@0 {
+			reg = <0>;
+		};
+	};
+};
+
+&i2c4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&i2c4_pins_a>;
+	i2c-scl-rising-time-ns = <185>;
+	i2c-scl-falling-time-ns = <20>;
+	status = "okay";
+	/delete-property/dmas;
+	/delete-property/dma-names;
+
+	pmic: stpmic@33 {
+		compatible = "st,stpmic1";
+		reg = <0x33>;
+		interrupt-controller;
+		#interrupt-cells = <2>;
+		status = "okay";
+
+		st,main-control-register = <0x04>;
+		st,vin-control-register = <0xc0>;
+		st,usb-control-register = <0x20>;
+
+		regulators {
+			compatible = "st,stpmic1-regulators";
+
+			ldo1-supply = <&v3v3>;
+			ldo3-supply = <&vdd_ddr>;
+			ldo6-supply = <&v3v3>;
+			pwr_sw1-supply = <&bst_out>;
+			pwr_sw2-supply = <&bst_out>;
+
+			vddcore: buck1 {
+				regulator-name = "vddcore";
+				regulator-min-microvolt = <1200000>;
+				regulator-max-microvolt = <1350000>;
+				regulator-always-on;
+				regulator-initial-mode = <0>;
+				regulator-over-current-protection;
+			};
+
+			vdd_ddr: buck2 {
+				regulator-name = "vdd_ddr";
+				regulator-min-microvolt = <1350000>;
+				regulator-max-microvolt = <1350000>;
+				regulator-always-on;
+				regulator-initial-mode = <0>;
+				regulator-over-current-protection;
+			};
+
+			vdd: buck3 {
+				regulator-name = "vdd";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+				st,mask-reset;
+				regulator-initial-mode = <0>;
+				regulator-over-current-protection;
+			};
+
+			v3v3: buck4 {
+				regulator-name = "v3v3";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+				regulator-over-current-protection;
+				regulator-initial-mode = <0>;
+			};
+
+			v1v8_audio: ldo1 {
+				regulator-name = "v1v8_audio";
+				regulator-min-microvolt = <1800000>;
+				regulator-max-microvolt = <1800000>;
+				regulator-always-on;
+				interrupts = <IT_CURLIM_LDO1 0>;
+			};
+
+			v3v3_hdmi: ldo2 {
+				regulator-name = "v3v3_hdmi";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+				interrupts = <IT_CURLIM_LDO2 0>;
+			};
+
+			vtt_ddr: ldo3 {
+				regulator-name = "vtt_ddr";
+				regulator-min-microvolt = <500000>;
+				regulator-max-microvolt = <750000>;
+				regulator-always-on;
+				regulator-over-current-protection;
+			};
+
+			vdd_usb: ldo4 {
+				regulator-name = "vdd_usb";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+				interrupts = <IT_CURLIM_LDO4 0>;
+			};
+
+			vdda: ldo5 {
+				regulator-name = "vdda";
+				regulator-min-microvolt = <2900000>;
+				regulator-max-microvolt = <2900000>;
+				interrupts = <IT_CURLIM_LDO5 0>;
+				regulator-boot-on;
+			};
+
+			v1v2_hdmi: ldo6 {
+				regulator-name = "v1v2_hdmi";
+				regulator-min-microvolt = <1200000>;
+				regulator-max-microvolt = <1200000>;
+				regulator-always-on;
+				interrupts = <IT_CURLIM_LDO6 0>;
+
+			};
+
+			vref_ddr: vref_ddr {
+				regulator-name = "vref_ddr";
+				regulator-always-on;
+				regulator-over-current-protection;
+			};
+
+			 bst_out: boost {
+				regulator-name = "bst_out";
+				interrupts = <IT_OCP_BOOST 0>;
+			 };
+
+			vbus_otg: pwr_sw1 {
+				regulator-name = "vbus_otg";
+				interrupts = <IT_OCP_OTG 0>;
+				regulator-active-discharge;
+			 };
+
+			 vbus_sw: pwr_sw2 {
+				regulator-name = "vbus_sw";
+				interrupts = <IT_OCP_SWOUT 0>;
+				regulator-active-discharge;
+			 };
+		};
+
+		onkey {
+			compatible = "st,stpmic1-onkey";
+			interrupts = <IT_PONKEY_F 0>, <IT_PONKEY_R 1>;
+			interrupt-names = "onkey-falling", "onkey-rising";
+			status = "okay";
+		};
+
+		watchdog {
+			compatible = "st,stpmic1-wdt";
+			status = "disabled";
+		};
+	};
+};
+
+&iwdg2 {
+	timeout-sec = <32>;
+	status = "okay";
+};
+
+&pwr {
+	pwr-supply = <&vdd>;
+};
+
+&rng1 {
+	status = "okay";
+};
+
+&rtc {
+	status = "okay";
+};
+
+&sdmmc1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&sdmmc1_b4_pins_a>;
+	broken-cd;
+	st,neg-edge;
+	bus-width = <4>;
+	vmmc-supply = <&v3v3>;
+	status = "okay";
+};
+
+&uart4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&uart4_pins_a>;
+	status = "okay";
+};
+
+&usbh_ehci {
+	phys = <&usbphyc_port0>;
+	phy-names = "usb";
+	status = "okay";
+};
+
+&usbphyc {
+	vdd3v3-supply = <&vdd_usb>;
+	status = "okay";
+};
+
+&vrefbuf {
+	regulator-min-microvolt = <2500000>;
+	regulator-max-microvolt = <2500000>;
+	vdda-supply = <&vdd>;
+	status = "okay";
+};
diff --git a/arch/arm/dts/stm32mp157c-dk2-u-boot.dtsi b/arch/arm/dts/stm32mp157c-dk2-u-boot.dtsi
new file mode 100644
index 0000000000..06ef3a4095
--- /dev/null
+++ b/arch/arm/dts/stm32mp157c-dk2-u-boot.dtsi
@@ -0,0 +1,6 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright : STMicroelectronics 2018
+ */
+
+#include "stm32mp157a-dk1-u-boot.dtsi"
diff --git a/arch/arm/dts/stm32mp157c-dk2.dts b/arch/arm/dts/stm32mp157c-dk2.dts
new file mode 100644
index 0000000000..9a81d2d472
--- /dev/null
+++ b/arch/arm/dts/stm32mp157c-dk2.dts
@@ -0,0 +1,67 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
+ */
+
+/dts-v1/;
+
+#include "stm32mp157a-dk1.dts"
+
+/ {
+	model = "STMicroelectronics STM32MP157C-DK2 Discovery Board";
+	compatible = "st,stm32mp157c-dk2", "st,stm32mp157";
+};
+
+&dsi {
+	#address-cells = <1>;
+	#size-cells = <0>;
+	status = "okay";
+	phy-dsi-supply = <&reg18>;
+
+	ports {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		port@0 {
+			reg = <0>;
+			dsi_in: endpoint {
+				remote-endpoint = <&ltdc_ep1_out>;
+			};
+		};
+
+		port@1 {
+			reg = <1>;
+			dsi_out: endpoint {
+				remote-endpoint = <&panel_in>;
+			};
+		};
+	};
+
+	panel@0 {
+		compatible = "orisetech,otm8009a";
+		reg = <0>;
+		reset-gpios = <&gpioe 4 GPIO_ACTIVE_LOW>;
+		status = "okay";
+
+		port {
+			panel_in: endpoint {
+				remote-endpoint = <&dsi_out>;
+			};
+		};
+	};
+};
+
+&ltdc {
+	status = "okay";
+
+	port {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		ltdc_ep1_out: endpoint@1 {
+			reg = <1>;
+			remote-endpoint = <&dsi_in>;
+		};
+	};
+};
diff --git a/arch/arm/dts/stm32mp157c-ed1-u-boot.dtsi b/arch/arm/dts/stm32mp157c-ed1-u-boot.dtsi
index 70bbf66704..55f99037b2 100644
--- a/arch/arm/dts/stm32mp157c-ed1-u-boot.dtsi
+++ b/arch/arm/dts/stm32mp157c-ed1-u-boot.dtsi
@@ -9,9 +9,14 @@
 
 / {
 	aliases {
+		i2c3 = &i2c4;
 		mmc0 = &sdmmc1;
 		mmc1 = &sdmmc2;
-		i2c3 = &i2c4;
+	};
+
+	config {
+		st,fastboot-gpios = <&gpioa 13 GPIO_ACTIVE_LOW>;
+		st,stm32prog-gpios = <&gpioa 14 GPIO_ACTIVE_LOW>;
 	};
 
 	led {
@@ -43,14 +48,8 @@
 	st,digbypass;
 };
 
-&uart4_pins_a {
+&i2c4 {
 	u-boot,dm-pre-reloc;
-	pins1 {
-		u-boot,dm-pre-reloc;
-	};
-	pins2 {
-		u-boot,dm-pre-reloc;
-	};
 };
 
 &i2c4_pins_a {
@@ -60,14 +59,6 @@
 	};
 };
 
-&uart4 {
-	u-boot,dm-pre-reloc;
-};
-
-&i2c4 {
-	u-boot,dm-pre-reloc;
-};
-
 &pmic {
 	u-boot,dm-pre-reloc;
 };
@@ -165,8 +156,6 @@
 	};
 };
 
-/* SPL part **************************************/
-/* MMC1 boot */
 &sdmmc1_b4_pins_a {
 	u-boot,dm-spl;
 	pins {
@@ -185,7 +174,6 @@
 	u-boot,dm-spl;
 };
 
-/* MMC2 boot */
 &sdmmc2_b4_pins_a {
 	u-boot,dm-spl;
 	pins {
@@ -203,3 +191,17 @@
 &sdmmc2 {
 	u-boot,dm-spl;
 };
+
+&uart4 {
+	u-boot,dm-pre-reloc;
+};
+
+&uart4_pins_a {
+	u-boot,dm-pre-reloc;
+	pins1 {
+		u-boot,dm-pre-reloc;
+	};
+	pins2 {
+		u-boot,dm-pre-reloc;
+	};
+};
diff --git a/arch/arm/dts/stm32mp157c-ed1.dts b/arch/arm/dts/stm32mp157c-ed1.dts
index 7a9b742d36..2664c9ce90 100644
--- a/arch/arm/dts/stm32mp157c-ed1.dts
+++ b/arch/arm/dts/stm32mp157c-ed1.dts
@@ -8,20 +8,24 @@
 #include "stm32mp157c.dtsi"
 #include "stm32mp157-pinctrl.dtsi"
 #include <dt-bindings/gpio/gpio.h>
-#include <dt-bindings/mfd/st,stpmu1.h>
+#include <dt-bindings/mfd/st,stpmic1.h>
 
 / {
 	model = "STMicroelectronics STM32MP157C eval daughter";
 	compatible = "st,stm32mp157c-ed1", "st,stm32mp157";
 
 	chosen {
-		stdout-path = "serial3:115200n8";
+		stdout-path = "serial0:115200n8";
 	};
 
 	memory@c0000000 {
 		reg = <0xC0000000 0x40000000>;
 	};
 
+	aliases {
+		serial0 = &uart4;
+	};
+
 	sd_switch: regulator-sd_switch {
 		compatible = "regulator-gpio";
 		regulator-name = "sd_switch";
@@ -36,15 +40,8 @@
 	};
 };
 
-&rng1 {
-	status = "okay";
-};
-
-&timers6 {
+&hwspinlock {
 	status = "okay";
-	timer@5 {
-		status = "okay";
-	};
 };
 
 &i2c4 {
@@ -54,8 +51,8 @@
 	i2c-scl-falling-time-ns = <20>;
 	status = "okay";
 
-	pmic: stpmu1@33 {
-		compatible = "st,stpmu1";
+	pmic: stpmic1@33 {
+		compatible = "st,stpmic1";
 		reg = <0x33>;
 		interrupts = <0 2>;
 		interrupt-parent = <&gpioa>;
@@ -68,7 +65,7 @@
 		st,usb_control_register = <0x30>;
 
 		regulators {
-			compatible = "st,stpmu1-regulators";
+			compatible = "st,stpmic1-regulators";
 
 			ldo1-supply = <&v3v3>;
 			ldo2-supply = <&v3v3>;
@@ -321,10 +318,27 @@
 	};
 };
 
+&iwdg2 {
+	timeout-sec = <32>;
+	status = "okay";
+};
+
+&pinctrl {
+	hwlocks = <&hwspinlock 0>;
+};
+
 &pwr {
 	pwr-supply = <&vdd>;
 };
 
+&rng1 {
+	status = "okay";
+};
+
+&rtc {
+	status = "okay";
+};
+
 &sdmmc1 {
 	pinctrl-0 = <&sdmmc1_b4_pins_a &sdmmc1_dir_pins_a>;
 	broken-cd;
@@ -355,6 +369,13 @@
 	status = "okay";
 };
 
+&timers6 {
+	status = "okay";
+	timer@5 {
+		status = "okay";
+	};
+};
+
 &uart4 {
 	pinctrl-names = "default";
 	pinctrl-0 = <&uart4_pins_a>;
@@ -365,14 +386,6 @@
 	usb33d-supply = <&usb33>;
 };
 
-&hwspinlock {
-	status = "okay";
-};
-
-&pinctrl {
-	hwlocks = <&hwspinlock 0>;
-};
-
 &usbphyc_port0 {
 	phy-supply = <&vdd_usb>;
 	vdda1v1-supply = <&reg11>;
diff --git a/arch/arm/dts/stm32mp157c-ev1-u-boot.dtsi b/arch/arm/dts/stm32mp157c-ev1-u-boot.dtsi
index 30b173478c..6a18d032ae 100644
--- a/arch/arm/dts/stm32mp157c-ev1-u-boot.dtsi
+++ b/arch/arm/dts/stm32mp157c-ev1-u-boot.dtsi
@@ -7,29 +7,23 @@
 
 / {
 	aliases {
-		spi0 = &qspi;
+		gpio26 = &stmfx_pinctrl;
 		i2c1 = &i2c2;
 		i2c4 = &i2c5;
+		pinctrl2 = &stmfx_pinctrl;
+		spi0 = &qspi;
 	};
 };
 
 &flash0 {
 	compatible = "spi-flash";
+	u-boot,dm-spl;
 };
 
 &flash1 {
 	compatible = "spi-flash";
 };
 
-&v3v3 {
-	regulator-always-on;
-};
-
-&usbotg_hs {
-	g-tx-fifo-size = <576>;
-};
-
-/* SPL part **************************************/
 &qspi {
 	u-boot,dm-spl;
 };
@@ -61,7 +55,10 @@
 	};
 };
 
-&flash0 {
-	u-boot,dm-spl;
+&usbotg_hs {
+	g-tx-fifo-size = <576>;
 };
 
+&v3v3 {
+	regulator-always-on;
+};
diff --git a/arch/arm/dts/stm32mp157c-ev1.dts b/arch/arm/dts/stm32mp157c-ev1.dts
index 902a42bee2..a6ee37924f 100644
--- a/arch/arm/dts/stm32mp157c-ev1.dts
+++ b/arch/arm/dts/stm32mp157c-ev1.dts
@@ -11,6 +11,21 @@
 	model = "STMicroelectronics STM32MP157C eval daughter on eval mother";
 	compatible = "st,stm32mp157c-ev1", "st,stm32mp157c-ed1", "st,stm32mp157";
 
+	chosen {
+		stdout-path = "serial0:115200n8";
+	};
+
+	aliases {
+		serial0 = &uart4;
+		ethernet0 = &ethernet0;
+	};
+
+	panel_backlight: panel-backlight {
+		compatible = "gpio-backlight";
+		gpios = <&gpiod 13 GPIO_ACTIVE_LOW>;
+		default-on;
+		status = "okay";
+	};
 };
 
 &cec {
@@ -19,12 +34,88 @@
 	status = "okay";
 };
 
+&dsi {
+	#address-cells = <1>;
+	#size-cells = <0>;
+	status = "okay";
+
+	ports {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		port@0 {
+			reg = <0>;
+			dsi_in: endpoint {
+				remote-endpoint = <&ltdc_ep0_out>;
+			};
+		};
+
+		port@1 {
+			reg = <1>;
+			dsi_out: endpoint {
+				remote-endpoint = <&dsi_panel_in>;
+			};
+		};
+	};
+
+	panel-dsi@0 {
+		compatible = "raydium,rm68200";
+		reg = <0>;
+		reset-gpios = <&gpiof 15 GPIO_ACTIVE_LOW>;
+		backlight = <&panel_backlight>;
+		status = "okay";
+
+		port {
+			dsi_panel_in: endpoint {
+				remote-endpoint = <&dsi_out>;
+			};
+		};
+	};
+};
+
+&ethernet0 {
+	status = "okay";
+	pinctrl-0 = <&ethernet0_rgmii_pins_a>;
+	pinctrl-1 = <&ethernet0_rgmii_pins_sleep_a>;
+	pinctrl-names = "default", "sleep";
+	phy-mode = "rgmii";
+	max-speed = <1000>;
+	phy-handle = <&phy0>;
+
+	mdio0 {
+		#address-cells = <1>;
+		#size-cells = <0>;
+		compatible = "snps,dwmac-mdio";
+		phy0: ethernet-phy@0 {
+			reg = <0>;
+		};
+	};
+};
+
 &i2c2 {
 	pinctrl-names = "default";
 	pinctrl-0 = <&i2c2_pins_a>;
 	i2c-scl-rising-time-ns = <185>;
 	i2c-scl-falling-time-ns = <20>;
 	status = "okay";
+
+	stmfx: stmfx@42 {
+		compatible = "st,stmfx-0300";
+		reg = <0x42>;
+		interrupts = <8 IRQ_TYPE_EDGE_RISING>;
+		interrupt-parent = <&gpioi>;
+		vdd-supply = <&v3v3>;
+
+		stmfx_pinctrl: stmfx-pin-controller {
+			compatible = "st,stmfx-0300-pinctrl";
+			gpio-controller;
+			#gpio-cells = <2>;
+			interrupt-controller;
+			#interrupt-cells = <2>;
+			gpio-ranges = <&stmfx_pinctrl 0 0 24>;
+			status = "disabled";
+		};
+	};
 };
 
 &i2c5 {
@@ -35,6 +126,26 @@
 	status = "okay";
 };
 
+&ltdc {
+	status = "okay";
+
+	port {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		ltdc_ep0_out: endpoint@0 {
+			reg = <0>;
+			remote-endpoint = <&dsi_in>;
+		};
+	};
+};
+
+&m_can1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&m_can1_pins_a>;
+	status = "okay";
+};
+
 &qspi {
 	pinctrl-names = "default";
 	pinctrl-0 = <&qspi_clk_pins_a &qspi_bk1_pins_a &qspi_bk2_pins_a>;
@@ -60,6 +171,12 @@
 	};
 };
 
+&spi1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&spi1_pins_a>;
+	status = "disabled";
+};
+
 &timers2 {
 	status = "disabled";
 	pwm {
@@ -106,6 +223,7 @@
 &usbotg_hs {
 	pinctrl-names = "default";
 	pinctrl-0 = <&usbotg_hs_pins_a>;
+	dr_mode = "peripheral";
 	phys = <&usbphyc_port1 0>;
 	phy-names = "usb2-phy";
 	status = "okay";
diff --git a/arch/arm/dts/stm32mp157c.dtsi b/arch/arm/dts/stm32mp157c.dtsi
index 37cadfa30c..7eb4bee31c 100644
--- a/arch/arm/dts/stm32mp157c.dtsi
+++ b/arch/arm/dts/stm32mp157c.dtsi
@@ -29,7 +29,7 @@
 	};
 
 	psci {
-		compatible = "arm,psci";
+		compatible = "arm,psci-1.0";
 		method = "smc";
 		cpu_off = <0x84000002>;
 		cpu_on = <0x84000003>;
@@ -106,26 +106,6 @@
 		};
 	};
 
-	pm_domain {
-		#address-cells = <1>;
-		#size-cells = <0>;
-		compatible = "st,stm32mp157c-pd";
-
-		pd_core_ret: core-ret-power-domain@1 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			reg = <1>;
-			#power-domain-cells = <0>;
-			label = "CORE-RETENTION";
-
-			pd_core: core-power-domain@2 {
-				reg = <2>;
-				#power-domain-cells = <0>;
-				label = "CORE";
-			};
-		};
-	};
-
 	soc {
 		compatible = "simple-bus";
 		#address-cells = <1>;
@@ -339,6 +319,34 @@
 			};
 		};
 
+		spi2: spi@4000b000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "st,stm32h7-spi";
+			reg = <0x4000b000 0x400>;
+			interrupts = <GIC_SPI 36 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&rcc SPI2_K>;
+			resets = <&rcc SPI2_R>;
+			dmas = <&dmamux1 39 0x400 0x05>,
+			       <&dmamux1 40 0x400 0x05>;
+			dma-names = "rx", "tx";
+			status = "disabled";
+		};
+
+		spi3: spi@4000c000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "st,stm32h7-spi";
+			reg = <0x4000c000 0x400>;
+			interrupts = <GIC_SPI 51 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&rcc SPI3_K>;
+			resets = <&rcc SPI3_R>;
+			dmas = <&dmamux1 61 0x400 0x05>,
+			       <&dmamux1 62 0x400 0x05>;
+			dma-names = "rx", "tx";
+			status = "disabled";
+		};
+
 		usart2: serial@4000e000 {
 			compatible = "st,stm32h7-uart";
 			reg = <0x4000e000 0x400>;
@@ -522,6 +530,34 @@
 			status = "disabled";
 		};
 
+		spi1: spi@44004000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "st,stm32h7-spi";
+			reg = <0x44004000 0x400>;
+			interrupts = <GIC_SPI 35 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&rcc SPI1_K>;
+			resets = <&rcc SPI1_R>;
+			dmas = <&dmamux1 37 0x400 0x05>,
+			       <&dmamux1 38 0x400 0x05>;
+			dma-names = "rx", "tx";
+			status = "disabled";
+		};
+
+		spi4: spi@44005000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "st,stm32h7-spi";
+			reg = <0x44005000 0x400>;
+			interrupts = <GIC_SPI 84 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&rcc SPI4_K>;
+			resets = <&rcc SPI4_R>;
+			dmas = <&dmamux1 83 0x400 0x05>,
+			       <&dmamux1 84 0x400 0x05>;
+			dma-names = "rx", "tx";
+			status = "disabled";
+		};
+
 		timers15: timer@44006000 {
 			#address-cells = <1>;
 			#size-cells = <0>;
@@ -584,6 +620,116 @@
 			};
 		};
 
+		spi5: spi@44009000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "st,stm32h7-spi";
+			reg = <0x44009000 0x400>;
+			interrupts = <GIC_SPI 85 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&rcc SPI5_K>;
+			resets = <&rcc SPI5_R>;
+			dmas = <&dmamux1 85 0x400 0x05>,
+			       <&dmamux1 86 0x400 0x05>;
+			dma-names = "rx", "tx";
+			status = "disabled";
+		};
+
+		dfsdm: dfsdm@4400d000 {
+			compatible = "st,stm32mp1-dfsdm";
+			reg = <0x4400d000 0x800>;
+			clocks = <&rcc DFSDM_K>;
+			clock-names = "dfsdm";
+			#address-cells = <1>;
+			#size-cells = <0>;
+			status = "disabled";
+
+			dfsdm0: filter@0 {
+				compatible = "st,stm32-dfsdm-adc";
+				#io-channel-cells = <1>;
+				reg = <0>;
+				interrupts = <GIC_SPI 110 IRQ_TYPE_LEVEL_HIGH>;
+				dmas = <&dmamux1 101 0x400 0x01>;
+				dma-names = "rx";
+				status = "disabled";
+			};
+
+			dfsdm1: filter@1 {
+				compatible = "st,stm32-dfsdm-adc";
+				#io-channel-cells = <1>;
+				reg = <1>;
+				interrupts = <GIC_SPI 111 IRQ_TYPE_LEVEL_HIGH>;
+				dmas = <&dmamux1 102 0x400 0x01>;
+				dma-names = "rx";
+				status = "disabled";
+			};
+
+			dfsdm2: filter@2 {
+				compatible = "st,stm32-dfsdm-adc";
+				#io-channel-cells = <1>;
+				reg = <2>;
+				interrupts = <GIC_SPI 112 IRQ_TYPE_LEVEL_HIGH>;
+				dmas = <&dmamux1 103 0x400 0x01>;
+				dma-names = "rx";
+				status = "disabled";
+			};
+
+			dfsdm3: filter@3 {
+				compatible = "st,stm32-dfsdm-adc";
+				#io-channel-cells = <1>;
+				reg = <3>;
+				interrupts = <GIC_SPI 113 IRQ_TYPE_LEVEL_HIGH>;
+				dmas = <&dmamux1 104 0x400 0x01>;
+				dma-names = "rx";
+				status = "disabled";
+			};
+
+			dfsdm4: filter@4 {
+				compatible = "st,stm32-dfsdm-adc";
+				#io-channel-cells = <1>;
+				reg = <4>;
+				interrupts = <GIC_SPI 115 IRQ_TYPE_LEVEL_HIGH>;
+				dmas = <&dmamux1 91 0x400 0x01>;
+				dma-names = "rx";
+				status = "disabled";
+			};
+
+			dfsdm5: filter@5 {
+				compatible = "st,stm32-dfsdm-adc";
+				#io-channel-cells = <1>;
+				reg = <5>;
+				interrupts = <GIC_SPI 126 IRQ_TYPE_LEVEL_HIGH>;
+				dmas = <&dmamux1 92 0x400 0x01>;
+				dma-names = "rx";
+				status = "disabled";
+			};
+		};
+
+		m_can1: can@4400e000 {
+			compatible = "bosch,m_can";
+			reg = <0x4400e000 0x400>, <0x44011000 0x2800>;
+			reg-names = "m_can", "message_ram";
+			interrupts = <GIC_SPI 19 IRQ_TYPE_LEVEL_HIGH>,
+				     <GIC_SPI 21 IRQ_TYPE_LEVEL_HIGH>;
+			interrupt-names = "int0", "int1";
+			clocks = <&rcc CK_HSE>, <&rcc FDCAN_K>;
+			clock-names = "hclk", "cclk";
+			bosch,mram-cfg = <0x0 0 0 32 0 0 2 2>;
+			status = "disabled";
+		};
+
+		m_can2: can@4400f000 {
+			compatible = "bosch,m_can";
+			reg = <0x4400f000 0x400>, <0x44011000 0x2800>;
+			reg-names = "m_can", "message_ram";
+			interrupts = <GIC_SPI 20 IRQ_TYPE_LEVEL_HIGH>,
+				     <GIC_SPI 22 IRQ_TYPE_LEVEL_HIGH>;
+			interrupt-names = "int0", "int1";
+			clocks = <&rcc CK_HSE>, <&rcc FDCAN_K>;
+			clock-names = "hclk", "cclk";
+			bosch,mram-cfg = <0x0 0 0 32 0 0 2 2>;
+			status = "disabled";
+		};
+
 		dma1: dma@48000000 {
 			compatible = "st,stm32-dma";
 			reg = <0x48000000 0x400>;
@@ -647,6 +793,8 @@
 				reg = <0x0>;
 				interrupt-parent = <&adc>;
 				interrupts = <0>;
+				dmas = <&dmamux1 9 0x400 0x01>;
+				dma-names = "rx";
 				status = "disabled";
 			};
 
@@ -656,6 +804,8 @@
 				reg = <0x100>;
 				interrupt-parent = <&adc>;
 				interrupts = <1>;
+				dmas = <&dmamux1 10 0x400 0x01>;
+				dma-names = "rx";
 				status = "disabled";
 			};
 		};
@@ -675,7 +825,7 @@
 		};
 
 		usbotg_hs: usb-otg@49000000 {
-			compatible = "st,stm32mp1-hsotg", "snps,dwc2";
+			compatible = "snps,dwc2";
 			reg = <0x49000000 0x10000>;
 			clocks = <&rcc USBO_K>;
 			clock-names = "otg";
@@ -686,7 +836,6 @@
 			g-np-tx-fifo-size = <32>;
 			g-tx-fifo-size = <128 128 64 64 64 64 32 32>;
 			dr_mode = "otg";
-			power-domains = <&pd_core>;
 			status = "disabled";
 		};
 
@@ -753,8 +902,8 @@
 			reg = <0x5000d000 0x400>;
 		};
 
-		syscfg: system-config@50020000 {
-			compatible = "st,stm32-syscfg", "syscon";
+		syscfg: syscon@50020000 {
+			compatible = "st,stm32mp157-syscfg", "syscon";
 			reg = <0x50020000 0x400>;
 		};
 
@@ -853,6 +1002,18 @@
 			status = "disabled";
 		};
 
+		hash1: hash@54002000 {
+			compatible = "st,stm32f756-hash";
+			reg = <0x54002000 0x400>;
+			interrupts = <GIC_SPI 80 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&rcc HASH1>;
+			resets = <&rcc HASH1_R>;
+			dmas = <&mdma1 31 0x10 0x1000A02 0x0 0x0>;
+			dma-names = "in";
+			dma-maxburst = <2>;
+			status = "disabled";
+		};
+
 		rng1: rng@54003000 {
 			compatible = "st,stm32-rng";
 			reg = <0x54003000 0x400>;
@@ -871,7 +1032,7 @@
 			dma-requests = <48>;
 		};
 
-		qspi: qspi@58003000 {
+		qspi: spi@58003000 {
 			compatible = "st,stm32f469-qspi";
 			reg = <0x58003000 0x1000>, <0x70000000 0x10000000>;
 			reg-names = "qspi", "qspi_mm";
@@ -915,6 +1076,36 @@
 			status = "disabled";
 		};
 
+		stmmac_axi_config_0: stmmac-axi-config {
+			snps,wr_osr_lmt = <0x7>;
+			snps,rd_osr_lmt = <0x7>;
+			snps,blen = <0 0 0 0 16 8 4>;
+		};
+
+		ethernet0: ethernet@5800a000 {
+			compatible = "st,stm32mp1-dwmac", "snps,dwmac-4.20a";
+			reg = <0x5800a000 0x2000>;
+			reg-names = "stmmaceth";
+			interrupts-extended = <&intc GIC_SPI 61 IRQ_TYPE_LEVEL_HIGH>;
+			interrupt-names = "macirq";
+			clock-names = "stmmaceth",
+				      "mac-clk-tx",
+				      "mac-clk-rx",
+				      "ethstp",
+				      "syscfg-clk";
+			clocks = <&rcc ETHMAC>,
+				 <&rcc ETHTX>,
+				 <&rcc ETHRX>,
+				 <&rcc ETHSTP>,
+				 <&rcc SYSCFG>;
+			st,syscon = <&syscfg 0x4>;
+			snps,mixed-burst;
+			snps,pbl = <2>;
+			snps,axi-config = <&stmmac_axi_config_0>;
+			snps,tso;
+			status = "disabled";
+		};
+
 		usbh_ohci: usbh-ohci@5800c000 {
 			compatible = "generic-ohci";
 			reg = <0x5800c000 0x1000>;
@@ -955,6 +1146,14 @@
 			status = "disabled";
 		};
 
+		iwdg2: watchdog@5a002000 {
+			compatible = "st,stm32mp1-iwdg";
+			reg = <0x5a002000 0x400>;
+			clocks = <&rcc IWDG2>, <&rcc CK_LSI>;
+			clock-names = "pclk", "lsi";
+			status = "disabled";
+		};
+
 		usbphyc: usbphyc@5a006000 {
 			#address-cells = <1>;
 			#size-cells = <0>;
@@ -983,6 +1182,20 @@
 			status = "disabled";
 		};
 
+		spi6: spi@5c001000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "st,stm32h7-spi";
+			reg = <0x5c001000 0x400>;
+			interrupts = <GIC_SPI 86 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&rcc SPI6_K>;
+			resets = <&rcc SPI6_R>;
+			dmas = <&mdma1 34 0x0 0x40008 0x0 0x0>,
+			       <&mdma1 35 0x0 0x40002 0x0 0x0>;
+			dma-names = "rx", "tx";
+			status = "disabled";
+		};
+
 		i2c4: i2c@5c002000 {
 			compatible = "st,stm32f7-i2c";
 			reg = <0x5c002000 0x400>;
@@ -996,6 +1209,22 @@
 			status = "disabled";
 		};
 
+		rtc: rtc@5c004000 {
+			compatible = "st,stm32mp1-rtc";
+			reg = <0x5c004000 0x400>;
+			clocks = <&rcc RTCAPB>, <&rcc RTC>;
+			clock-names = "pclk", "rtc_ck";
+			interrupts = <GIC_SPI 3 IRQ_TYPE_LEVEL_HIGH>;
+			status = "disabled";
+		};
+
+		bsec: nvmem@5c005000 {
+			compatible = "st,stm32mp15-bsec";
+			reg = <0x5c005000 0x400>;
+			#address-cells = <1>;
+			#size-cells = <1>;
+		};
+
 		i2c6: i2c@5c009000 {
 			compatible = "st,stm32f7-i2c";
 			reg = <0x5c009000 0x400>;
diff --git a/arch/arm/mach-stm32mp/Kconfig b/arch/arm/mach-stm32mp/Kconfig
index 8a929fa913..73aa382712 100644
--- a/arch/arm/mach-stm32mp/Kconfig
+++ b/arch/arm/mach-stm32mp/Kconfig
@@ -17,7 +17,7 @@ config SPL
 	select SPL_DM_RESET
 	select SPL_SERIAL_SUPPORT
 	select SPL_SYSCON
-	select SPL_DRIVERS_MISC_SUPPORT
+	imply SPL_DISPLAY_PRINT
 	imply SPL_LIBDISK_SUPPORT
 
 config SYS_SOC
@@ -25,18 +25,31 @@ config SYS_SOC
 
 config TARGET_STM32MP1
 	bool "Support stm32mp1xx"
-	select ARCH_SUPPORT_PSCI
+	select ARCH_SUPPORT_PSCI if !STM32MP1_TRUSTED
 	select CPU_V7A
-	select CPU_V7_HAS_NONSEC
+	select CPU_V7_HAS_NONSEC if !STM32MP1_TRUSTED
 	select CPU_V7_HAS_VIRT
 	select PINCTRL_STM32
 	select STM32_RCC
 	select STM32_RESET
 	select SYS_ARCH_TIMER
-	select SYSRESET_SYSCON
+	imply SYSRESET_PSCI if STM32MP1_TRUSTED
+	imply SYSRESET_SYSCON if !STM32MP1_TRUSTED
 	help
 		target STMicroelectronics SOC STM32MP1 family
+		STM32MP157, STM32MP153 or STM32MP151
 		STMicroelectronics MPU with core ARMv7
+		dual core A7 for STM32MP157/3, monocore for STM32MP151
+
+config STM32MP1_TRUSTED
+	bool "Support trusted boot with TF-A"
+	default y if !SPL
+	select ARM_SMCCC
+	help
+		Say Y here to enable boot with TF-A
+		Trusted boot chain is :
+		BootRom => TF-A.stm32 (clock & DDR) => U-Boot.stm32
+		TF-A monitor provides proprietary smc to manage secure devices
 
 config SYS_TEXT_BASE
 	prompt "U-Boot base address"
@@ -46,6 +59,9 @@ config SYS_TEXT_BASE
 		when DDR driver is used:
 		  DDR + 1MB (0xC0100000)
 
+config NR_DRAM_BANKS
+	default 1
+
 config SYS_MMCSD_RAW_MODE_U_BOOT_PARTITION_MMC2
 	hex "Partition on MMC2 to use to load U-Boot from"
 	depends on SYS_MMCSD_RAW_MODE_U_BOOT_USE_PARTITION
@@ -54,9 +70,6 @@ config SYS_MMCSD_RAW_MODE_U_BOOT_PARTITION_MMC2
 	  Partition on the second MMC to load U-Boot from when the MMC is being
 	  used in raw mode
 
-source "board/st/stm32mp1/Kconfig"
-
-# currently activated for debug / should be deactivated for real product
 if DEBUG_UART
 
 config DEBUG_UART_BOARD_INIT
@@ -71,4 +84,6 @@ config DEBUG_UART_CLOCK
 	default 64000000
 endif
 
+source "board/st/stm32mp1/Kconfig"
+
 endif
diff --git a/arch/arm/mach-stm32mp/Makefile b/arch/arm/mach-stm32mp/Makefile
index f59ced5ee1..1493914a11 100644
--- a/arch/arm/mach-stm32mp/Makefile
+++ b/arch/arm/mach-stm32mp/Makefile
@@ -11,6 +11,9 @@ ifdef CONFIG_SPL_BUILD
 obj-y += spl.o
 else
 obj-y += bsec.o
+ifndef CONFIG_STM32MP1_TRUSTED
+obj-$(CONFIG_SYSRESET) += cmd_poweroff.o
+endif
 endif
 obj-$(CONFIG_ARMV7_PSCI) += psci.o
 obj-$(CONFIG_$(SPL_)DM_REGULATOR) += pwr_regulator.o
diff --git a/arch/arm/mach-stm32mp/bsec.c b/arch/arm/mach-stm32mp/bsec.c
index d087a31389..9ed8d8c56c 100644
--- a/arch/arm/mach-stm32mp/bsec.c
+++ b/arch/arm/mach-stm32mp/bsec.c
@@ -8,9 +8,12 @@
 #include <misc.h>
 #include <asm/io.h>
 #include <linux/iopoll.h>
+#include <asm/arch/stm32mp1_smc.h>
+#include <linux/arm-smccc.h>
 
 #define BSEC_OTP_MAX_VALUE		95
 
+#ifndef CONFIG_STM32MP1_TRUSTED
 #define BSEC_TIMEOUT_US			10000
 
 /* BSEC REGISTER OFFSET (base relative) */
@@ -168,7 +171,7 @@ static int bsec_shadow_register(u32 base, u32 otp)
 		ret = bsec_power_safmem(base, true);
 		if (ret)
 			return ret;
-		power_up = 1;
+		power_up = true;
 	}
 	/* set BSEC_OTP_CTRL_OFF with the otp value*/
 	writel(otp | BSEC_READ, base + BSEC_OTP_CTRL_OFF);
@@ -270,6 +273,7 @@ static int bsec_program_otp(long base, u32 val, u32 otp)
 
 	return ret;
 }
+#endif /* CONFIG_STM32MP1_TRUSTED */
 
 /* BSEC MISC driver *******************************************************/
 struct stm32mp_bsec_platdata {
@@ -278,6 +282,11 @@ struct stm32mp_bsec_platdata {
 
 static int stm32mp_bsec_read_otp(struct udevice *dev, u32 *val, u32 otp)
 {
+#ifdef CONFIG_STM32MP1_TRUSTED
+	return stm32_smc(STM32_SMC_BSEC,
+			 STM32_SMC_READ_OTP,
+			 otp, 0, val);
+#else
 	struct stm32mp_bsec_platdata *plat = dev_get_platdata(dev);
 	u32 tmp_data = 0;
 	int ret;
@@ -299,27 +308,46 @@ static int stm32mp_bsec_read_otp(struct udevice *dev, u32 *val, u32 otp)
 	/* restore shadow value */
 	ret = bsec_write_shadow(plat->base, tmp_data, otp);
 	return ret;
+#endif
 }
 
 static int stm32mp_bsec_read_shadow(struct udevice *dev, u32 *val, u32 otp)
 {
+#ifdef CONFIG_STM32MP1_TRUSTED
+	return stm32_smc(STM32_SMC_BSEC,
+			 STM32_SMC_READ_SHADOW,
+			 otp, 0, val);
+#else
 	struct stm32mp_bsec_platdata *plat = dev_get_platdata(dev);
 
 	return bsec_read_shadow(plat->base, val, otp);
+#endif
 }
 
 static int stm32mp_bsec_write_otp(struct udevice *dev, u32 val, u32 otp)
 {
+#ifdef CONFIG_STM32MP1_TRUSTED
+	return stm32_smc_exec(STM32_SMC_BSEC,
+			      STM32_SMC_PROG_OTP,
+			      otp, val);
+#else
 	struct stm32mp_bsec_platdata *plat = dev_get_platdata(dev);
 
 	return bsec_program_otp(plat->base, val, otp);
+#endif
 }
 
 static int stm32mp_bsec_write_shadow(struct udevice *dev, u32 val, u32 otp)
 {
+#ifdef CONFIG_STM32MP1_TRUSTED
+	return stm32_smc_exec(STM32_SMC_BSEC,
+			      STM32_SMC_WRITE_SHADOW,
+			      otp, val);
+#else
 	struct stm32mp_bsec_platdata *plat = dev_get_platdata(dev);
 
 	return bsec_write_shadow(plat->base, val, otp);
+#endif
 }
 
 static int stm32mp_bsec_read(struct udevice *dev, int offset,
@@ -405,8 +433,23 @@ static int stm32mp_bsec_ofdata_to_platdata(struct udevice *dev)
 	return 0;
 }
 
+#ifndef CONFIG_STM32MP1_TRUSTED
+static int stm32mp_bsec_probe(struct udevice *dev)
+{
+	int otp;
+	struct stm32mp_bsec_platdata *plat = dev_get_platdata(dev);
+
+	/* update unlocked shadow for OTP cleared by the rom code */
+	for (otp = 57; otp <= BSEC_OTP_MAX_VALUE; otp++)
+		if (!bsec_read_SR_lock(plat->base, otp))
+			bsec_shadow_register(plat->base, otp);
+
+	return 0;
+}
+#endif
+
 static const struct udevice_id stm32mp_bsec_ids[] = {
-	{ .compatible = "st,stm32mp-bsec" },
+	{ .compatible = "st,stm32mp15-bsec" },
 	{}
 };
 
@@ -417,14 +460,7 @@ U_BOOT_DRIVER(stm32mp_bsec) = {
 	.ofdata_to_platdata = stm32mp_bsec_ofdata_to_platdata,
 	.platdata_auto_alloc_size = sizeof(struct stm32mp_bsec_platdata),
 	.ops = &stm32mp_bsec_ops,
-};
-
-/* bsec IP is not present in device tee, manage IP address by platdata */
-static struct stm32mp_bsec_platdata stm32_bsec_platdata = {
-	.base = STM32_BSEC_BASE,
-};
-
-U_BOOT_DEVICE(stm32mp_bsec) = {
-	.name = "stm32mp_bsec",
-	.platdata = &stm32_bsec_platdata,
+#ifndef CONFIG_STM32MP1_TRUSTED
+	.probe = stm32mp_bsec_probe,
+#endif
 };
diff --git a/arch/arm/mach-stm32mp/cmd_poweroff.c b/arch/arm/mach-stm32mp/cmd_poweroff.c
new file mode 100644
index 0000000000..f54dd1daf2
--- /dev/null
+++ b/arch/arm/mach-stm32mp/cmd_poweroff.c
@@ -0,0 +1,24 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2019, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <command.h>
+#include <sysreset.h>
+
+int do_poweroff(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	int ret;
+
+	puts("poweroff ...\n");
+	mdelay(100);
+
+	ret = sysreset_walk(SYSRESET_POWER);
+
+	if (ret == -EINPROGRESS)
+		mdelay(1000);
+
+	/*NOTREACHED when power off*/
+	return CMD_RET_FAILURE;
+}
diff --git a/arch/arm/mach-stm32mp/config.mk b/arch/arm/mach-stm32mp/config.mk
index f371aac75b..403af2a225 100644
--- a/arch/arm/mach-stm32mp/config.mk
+++ b/arch/arm/mach-stm32mp/config.mk
@@ -3,7 +3,20 @@
 # Copyright (C) 2018, STMicroelectronics - All Rights Reserved
 #
 
-ALL-$(CONFIG_SPL_BUILD) += u-boot-spl.stm32
+ifndef CONFIG_SPL
+ALL-y += u-boot.stm32
+else
+ifdef CONFIG_SPL_BUILD
+ALL-y += u-boot-spl.stm32
+endif
+endif
+
+MKIMAGEFLAGS_u-boot.stm32 = -T stm32image -a $(CONFIG_SYS_TEXT_BASE) -e $(CONFIG_SYS_TEXT_BASE)
+
+u-boot.stm32: MKIMAGEOUTPUT = u-boot.stm32.log
+
+u-boot.stm32: u-boot.bin FORCE
+	$(call if_changed,mkimage)
 
 MKIMAGEFLAGS_u-boot-spl.stm32 = -T stm32image -a $(CONFIG_SPL_TEXT_BASE) -e $(CONFIG_SPL_TEXT_BASE)
 
diff --git a/arch/arm/mach-stm32mp/cpu.c b/arch/arm/mach-stm32mp/cpu.c
index b8933587ad..7b4431c9c7 100644
--- a/arch/arm/mach-stm32mp/cpu.c
+++ b/arch/arm/mach-stm32mp/cpu.c
@@ -18,6 +18,7 @@
 #define RCC_DBGCFGR		(STM32_RCC_BASE + 0x080C)
 #define RCC_BDCR		(STM32_RCC_BASE + 0x0140)
 #define RCC_MP_APB5ENSETR	(STM32_RCC_BASE + 0x0208)
+#define RCC_MP_AHB5ENSETR	(STM32_RCC_BASE + 0x0210)
 #define RCC_BDCR_VSWRST		BIT(31)
 #define RCC_BDCR_RTCSRC		GENMASK(17, 16)
 #define RCC_DBGCFGR_DBGCKEN	BIT(8)
@@ -44,6 +45,9 @@
 #define DBGMCU_IDC_REV_ID_MASK	GENMASK(31, 16)
 #define DBGMCU_IDC_REV_ID_SHIFT	16
 
+/* GPIOZ registers */
+#define GPIOZ_SECCFGR		0x54004030
+
 /* boot interface from Bootrom
  * - boot instance = bit 31:16
  * - boot device = bit 15:0
@@ -55,10 +59,32 @@
 #define BOOTROM_INSTANCE_SHIFT	16
 
 /* BSEC OTP index */
+#define BSEC_OTP_RPN	1
 #define BSEC_OTP_SERIAL	13
+#define BSEC_OTP_PKG	16
 #define BSEC_OTP_MAC	57
 
+/* Device Part Number (RPN) = OTP_DATA1 lower 8 bits */
+#define RPN_SHIFT	0
+#define RPN_MASK	GENMASK(7, 0)
+
+/* Package = bit 27:29 of OTP16
+ * - 100: LBGA448 (FFI) => AA = LFBGA 18x18mm 448 balls p. 0.8mm
+ * - 011: LBGA354 (LCI) => AB = LFBGA 16x16mm 359 balls p. 0.8mm
+ * - 010: TFBGA361 (FFC) => AC = TFBGA 12x12mm 361 balls p. 0.5mm
+ * - 001: TFBGA257 (LCC) => AD = TFBGA 10x10mm 257 balls p. 0.5mm
+ * - others: Reserved
+ */
+#define PKG_SHIFT	27
+#define PKG_MASK	GENMASK(2, 0)
+
+#define PKG_AA_LBGA448	4
+#define PKG_AB_LBGA354	3
+#define PKG_AC_TFBGA361	2
+#define PKG_AD_TFBGA257	1
+
 #if !defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD)
+#ifndef CONFIG_STM32MP1_TRUSTED
 static void security_init(void)
 {
 	/* Disable the backup domain write protection */
@@ -113,7 +139,12 @@ static void security_init(void)
 	 * Bit 16 ITAMP1E: RTC power domain supply monitoring
 	 */
 	writel(0x0, TAMP_CR1);
+
+	/* GPIOZ: deactivate the security */
+	writel(BIT(0), RCC_MP_AHB5ENSETR);
+	writel(0x0, GPIOZ_SECCFGR);
 }
+#endif /* CONFIG_STM32MP1_TRUSTED */
 
 /*
  * Debug init
@@ -127,13 +158,19 @@ static void dbgmcu_init(void)
 }
 #endif /* !defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD) */
 
-static u32 get_bootmode(void)
+#if !defined(CONFIG_STM32MP1_TRUSTED) && \
+	(!defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD))
+/* get bootmode from ROM code boot context: saved in TAMP register */
+static void update_bootmode(void)
 {
 	u32 boot_mode;
-#if !defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD)
 	u32 bootrom_itf = readl(BOOTROM_PARAM_ADDR);
 	u32 bootrom_device, bootrom_instance;
 
+	/* enable TAMP clock = RTCAPBEN */
+	writel(BIT(8), RCC_MP_APB5ENSETR);
+
+	/* read bootrom context */
 	bootrom_device =
 		(bootrom_itf & BOOTROM_MODE_MASK) >> BOOTROM_MODE_SHIFT;
 	bootrom_instance =
@@ -147,12 +184,14 @@ static u32 get_bootmode(void)
 	clrsetbits_le32(TAMP_BOOT_CONTEXT,
 			TAMP_BOOT_MODE_MASK,
 			boot_mode << TAMP_BOOT_MODE_SHIFT);
-#else
-	/* read TAMP backup register */
-	boot_mode = (readl(TAMP_BOOT_CONTEXT) & TAMP_BOOT_MODE_MASK) >>
-		    TAMP_BOOT_MODE_SHIFT;
+}
 #endif
-	return boot_mode;
+
+u32 get_bootmode(void)
+{
+	/* read bootmode from TAMP backup register */
+	return (readl(TAMP_BOOT_CONTEXT) & TAMP_BOOT_MODE_MASK) >>
+		    TAMP_BOOT_MODE_SHIFT;
 }
 
 /*
@@ -167,16 +206,18 @@ int arch_cpu_init(void)
 
 #if !defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD)
 	dbgmcu_init();
-
+#ifndef CONFIG_STM32MP1_TRUSTED
 	security_init();
+	update_bootmode();
+#endif
 #endif
 
-	/* get bootmode from BootRom context: saved in TAMP register */
 	boot_mode = get_bootmode();
 
 	if ((boot_mode & TAMP_BOOT_DEVICE_MASK) == BOOT_SERIAL_UART)
 		gd->flags |= GD_FLG_SILENT | GD_FLG_DISABLE_CONSOLE;
 #if defined(CONFIG_DEBUG_UART) && \
+	!defined(CONFIG_STM32MP1_TRUSTED) && \
 	(!defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD))
 	else
 		debug_uart_init();
@@ -203,25 +244,94 @@ u32 get_cpu_rev(void)
 	return (read_idc() & DBGMCU_IDC_REV_ID_MASK) >> DBGMCU_IDC_REV_ID_SHIFT;
 }
 
+static u32 get_otp(int index, int shift, int mask)
+{
+	int ret;
+	struct udevice *dev;
+	u32 otp = 0;
+
+	ret = uclass_get_device_by_driver(UCLASS_MISC,
+					  DM_GET_DRIVER(stm32mp_bsec),
+					  &dev);
+
+	if (!ret)
+		ret = misc_read(dev, STM32_BSEC_SHADOW(index),
+				&otp, sizeof(otp));
+
+	return (otp >> shift) & mask;
+}
+
+/* Get Device Part Number (RPN) from OTP */
+static u32 get_cpu_rpn(void)
+{
+	return get_otp(BSEC_OTP_RPN, RPN_SHIFT, RPN_MASK);
+}
+
 u32 get_cpu_type(void)
 {
-	return (read_idc() & DBGMCU_IDC_DEV_ID_MASK) >> DBGMCU_IDC_DEV_ID_SHIFT;
+	u32 id;
+
+	id = (read_idc() & DBGMCU_IDC_DEV_ID_MASK) >> DBGMCU_IDC_DEV_ID_SHIFT;
+
+	return (id << 16) | get_cpu_rpn();
+}
+
+/* Get Package options from OTP */
+static u32 get_cpu_package(void)
+{
+	return get_otp(BSEC_OTP_PKG, PKG_SHIFT, PKG_MASK);
 }
 
 #if defined(CONFIG_DISPLAY_CPUINFO)
 int print_cpuinfo(void)
 {
-	char *cpu_s, *cpu_r;
+	char *cpu_s, *cpu_r, *pkg;
 
+	/* MPUs Part Numbers */
 	switch (get_cpu_type()) {
-	case CPU_STMP32MP15x:
-		cpu_s = "15x";
+	case CPU_STM32MP157Cxx:
+		cpu_s = "157C";
+		break;
+	case CPU_STM32MP157Axx:
+		cpu_s = "157A";
+		break;
+	case CPU_STM32MP153Cxx:
+		cpu_s = "153C";
+		break;
+	case CPU_STM32MP153Axx:
+		cpu_s = "153A";
+		break;
+	case CPU_STM32MP151Cxx:
+		cpu_s = "151C";
+		break;
+	case CPU_STM32MP151Axx:
+		cpu_s = "151A";
+		break;
+	default:
+		cpu_s = "????";
+		break;
+	}
+
+	/* Package */
+	switch (get_cpu_package()) {
+	case PKG_AA_LBGA448:
+		pkg = "AA";
+		break;
+	case PKG_AB_LBGA354:
+		pkg = "AB";
+		break;
+	case PKG_AC_TFBGA361:
+		pkg = "AC";
+		break;
+	case PKG_AD_TFBGA257:
+		pkg = "AD";
 		break;
 	default:
-		cpu_s = "?";
+		pkg = "??";
 		break;
 	}
 
+	/* REVISION */
 	switch (get_cpu_rev()) {
 	case CPU_REVA:
 		cpu_r = "A";
@@ -234,7 +344,7 @@ int print_cpuinfo(void)
 		break;
 	}
 
-	printf("CPU: STM32MP%s.%s\n", cpu_s, cpu_r);
+	printf("CPU: STM32MP%s%s Rev.%s\n", cpu_s, pkg, cpu_r);
 
 	return 0;
 }
@@ -242,20 +352,48 @@ int print_cpuinfo(void)
 
 static void setup_boot_mode(void)
 {
+	const u32 serial_addr[] = {
+		STM32_USART1_BASE,
+		STM32_USART2_BASE,
+		STM32_USART3_BASE,
+		STM32_UART4_BASE,
+		STM32_UART5_BASE,
+		STM32_USART6_BASE,
+		STM32_UART7_BASE,
+		STM32_UART8_BASE
+	};
 	char cmd[60];
 	u32 boot_ctx = readl(TAMP_BOOT_CONTEXT);
 	u32 boot_mode =
 		(boot_ctx & TAMP_BOOT_MODE_MASK) >> TAMP_BOOT_MODE_SHIFT;
 	int instance = (boot_mode & TAMP_BOOT_INSTANCE_MASK) - 1;
+	u32 forced_mode = (boot_ctx & TAMP_BOOT_FORCED_MASK);
+	struct udevice *dev;
+	int alias;
 
-	pr_debug("%s: boot_ctx=0x%x => boot_mode=%x, instance=%d\n",
-		 __func__, boot_ctx, boot_mode, instance);
-
+	pr_debug("%s: boot_ctx=0x%x => boot_mode=%x, instance=%d forced=%x\n",
+		 __func__, boot_ctx, boot_mode, instance, forced_mode);
 	switch (boot_mode & TAMP_BOOT_DEVICE_MASK) {
 	case BOOT_SERIAL_UART:
-		sprintf(cmd, "%d", instance);
-		env_set("boot_device", "uart");
+		if (instance > ARRAY_SIZE(serial_addr))
+			break;
+		/* serial : search associated alias in devicetree */
+		sprintf(cmd, "serial@%x", serial_addr[instance]);
+		if (uclass_get_device_by_name(UCLASS_SERIAL, cmd, &dev))
+			break;
+		if (fdtdec_get_alias_seq(gd->fdt_blob, "serial",
+					 dev_of_offset(dev), &alias))
+			break;
+		sprintf(cmd, "%d", alias);
+		env_set("boot_device", "serial");
 		env_set("boot_instance", cmd);
+
+		/* restore console on uart when not used */
+		if (gd->cur_serial_dev != dev) {
+			gd->flags &= ~(GD_FLG_SILENT |
+				       GD_FLG_DISABLE_CONSOLE);
+			printf("serial boot with console enabled!\n");
+		}
 		break;
 	case BOOT_SERIAL_USB:
 		env_set("boot_device", "usb");
@@ -279,6 +417,36 @@ static void setup_boot_mode(void)
 		pr_debug("unexpected boot mode = %x\n", boot_mode);
 		break;
 	}
+
+	switch (forced_mode) {
+	case BOOT_FASTBOOT:
+		printf("Enter fastboot!\n");
+		env_set("preboot", "env set preboot; fastboot 0");
+		break;
+	case BOOT_STM32PROG:
+		env_set("boot_device", "usb");
+		env_set("boot_instance", "0");
+		break;
+	case BOOT_UMS_MMC0:
+	case BOOT_UMS_MMC1:
+	case BOOT_UMS_MMC2:
+		printf("Enter UMS!\n");
+		instance = forced_mode - BOOT_UMS_MMC0;
+		sprintf(cmd, "env set preboot; ums 0 mmc %d", instance);
+		env_set("preboot", cmd);
+		break;
+	case BOOT_RECOVERY:
+		env_set("preboot", "env set preboot; run altbootcmd");
+		break;
+	case BOOT_NORMAL:
+		break;
+	default:
+		pr_debug("unexpected forced boot mode = %x\n", forced_mode);
+		break;
+	}
+
+	/* clear TAMP for next reboot */
+	clrsetbits_le32(TAMP_BOOT_CONTEXT, TAMP_BOOT_FORCED_MASK, BOOT_NORMAL);
 }
 
 /*
@@ -304,7 +472,7 @@ static int setup_mac_address(void)
 	if (ret)
 		return ret;
 
-	ret = misc_read(dev, BSEC_OTP_MAC * 4 + STM32_BSEC_OTP_OFFSET,
+	ret = misc_read(dev, STM32_BSEC_SHADOW(BSEC_OTP_MAC),
 			otp, sizeof(otp));
 	if (ret < 0)
 		return ret;
@@ -342,12 +510,12 @@ static int setup_serial_number(void)
 	if (ret)
 		return ret;
 
-	ret = misc_read(dev, BSEC_OTP_SERIAL * 4 + STM32_BSEC_OTP_OFFSET,
+	ret = misc_read(dev, STM32_BSEC_SHADOW(BSEC_OTP_SERIAL),
 			otp, sizeof(otp));
 	if (ret < 0)
 		return ret;
 
-	sprintf(serial_string, "%08x%08x%08x", otp[0], otp[1], otp[2]);
+	sprintf(serial_string, "%08X%08X%08X", otp[0], otp[1], otp[2]);
 	env_set("serial#", serial_string);
 
 	return 0;
diff --git a/arch/arm/mach-stm32mp/include/mach/stm32.h b/arch/arm/mach-stm32mp/include/mach/stm32.h
index 5d0bdca178..c526c88e3e 100644
--- a/arch/arm/mach-stm32mp/include/mach/stm32.h
+++ b/arch/arm/mach-stm32mp/include/mach/stm32.h
@@ -13,13 +13,10 @@
 #define STM32_RCC_BASE			0x50000000
 #define STM32_PWR_BASE			0x50001000
 #define STM32_DBGMCU_BASE		0x50081000
-#define STM32_BSEC_BASE			0x5C005000
 #define STM32_TZC_BASE			0x5C006000
 #define STM32_ETZPC_BASE		0x5C007000
 #define STM32_TAMP_BASE			0x5C00A000
 
-#ifdef CONFIG_DEBUG_UART_BASE
-/* hardcoded value can be only used for DEBUG UART */
 #define STM32_USART1_BASE		0x5C000000
 #define STM32_USART2_BASE		0x4000E000
 #define STM32_USART3_BASE		0x4000F000
@@ -28,7 +25,6 @@
 #define STM32_USART6_BASE		0x44003000
 #define STM32_UART7_BASE		0x40018000
 #define STM32_UART8_BASE		0x40019000
-#endif
 
 #define STM32_SYSRAM_BASE		0x2FFC0000
 #define STM32_SYSRAM_SIZE		SZ_256K
@@ -40,8 +36,10 @@
 /* enumerated used to identify the SYSCON driver instance */
 enum {
 	STM32MP_SYSCON_UNKNOWN,
-	STM32MP_SYSCON_STGEN,
+	STM32MP_SYSCON_ETZPC,
 	STM32MP_SYSCON_PWR,
+	STM32MP_SYSCON_STGEN,
+	STM32MP_SYSCON_SYSCFG,
 };
 
 /*
@@ -95,10 +93,25 @@ enum boot_device {
 #define TAMP_BOOT_MODE_SHIFT		8
 #define TAMP_BOOT_DEVICE_MASK		GENMASK(7, 4)
 #define TAMP_BOOT_INSTANCE_MASK		GENMASK(3, 0)
+#define TAMP_BOOT_FORCED_MASK		GENMASK(7, 0)
+
+enum forced_boot_mode {
+	BOOT_NORMAL = 0x00,
+	BOOT_FASTBOOT = 0x01,
+	BOOT_RECOVERY = 0x02,
+	BOOT_STM32PROG = 0x03,
+	BOOT_UMS_MMC0 = 0x10,
+	BOOT_UMS_MMC1 = 0x11,
+	BOOT_UMS_MMC2 = 0x12,
+};
 
 /* offset used for BSEC driver: misc_read and misc_write */
 #define STM32_BSEC_SHADOW_OFFSET	0x0
+#define STM32_BSEC_SHADOW(id)		(STM32_BSEC_SHADOW_OFFSET + (id) * 4)
 #define STM32_BSEC_OTP_OFFSET		0x80000000
+#define STM32_BSEC_OTP(id)		(STM32_BSEC_OTP_OFFSET + (id) * 4)
+
+#define BSEC_OTP_BOARD	59
 
 #endif /* __ASSEMBLY__*/
 #endif /* _MACH_STM32_H_ */
diff --git a/arch/arm/mach-stm32mp/include/mach/stm32mp1_smc.h b/arch/arm/mach-stm32mp/include/mach/stm32mp1_smc.h
new file mode 100644
index 0000000000..8130546b27
--- /dev/null
+++ b/arch/arm/mach-stm32mp/include/mach/stm32mp1_smc.h
@@ -0,0 +1,64 @@
+/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
+/*
+ * Copyright (C) 2019, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef __STM32MP1_SMC_H__
+#define __STM32MP1_SMC_H__
+
+#include <linux/arm-smccc.h>
+
+/*
+ * SMC function IDs for STM32 Service queries
+ * STM32 SMC services use the space between 0x82000000 and 0x8200FFFF
+ * like this is defined in SMC calling Convention by ARM
+ * for SiP (silicon Partner)
+ * http://infocenter.arm.com/help/topic/com.arm.doc.den0028a/index.html
+ */
+#define STM32_SMC_VERSION		0x82000000
+
+/* Secure Service access from Non-secure */
+#define STM32_SMC_BSEC			0x82001003
+
+/* Service for BSEC */
+#define STM32_SMC_READ_SHADOW		0x01
+#define STM32_SMC_PROG_OTP		0x02
+#define STM32_SMC_WRITE_SHADOW		0x03
+#define STM32_SMC_READ_OTP		0x04
+#define STM32_SMC_READ_ALL		0x05
+#define STM32_SMC_WRITE_ALL		0x06
+
+/* SMC error codes */
+#define STM32_SMC_OK			0x0
+#define STM32_SMC_NOT_SUPPORTED		-1
+#define STM32_SMC_FAILED		-2
+#define STM32_SMC_INVALID_PARAMS	-3
+
+#define stm32_smc_exec(svc, op, data1, data2) \
+	stm32_smc(svc, op, data1, data2, NULL)
+
+#ifdef CONFIG_ARM_SMCCC
+static inline u32 stm32_smc(u32 svc, u8 op, u32 data1, u32 data2, u32 *result)
+{
+	struct arm_smccc_res res;
+
+	arm_smccc_smc(svc, op, data1, data2, 0, 0, 0, 0, &res);
+
+	if (res.a0) {
+		pr_err("%s: Failed to exec in secure mode (err = %ld)\n",
+		       __func__, res.a0);
+		return -EINVAL;
+	}
+	if (result)
+		*result = (u32)res.a1;
+
+	return 0;
+}
+#else
+static inline u32 stm32_smc(u32 svc, u8 op, u32 data1, u32 data2, u32 *result)
+{
+	return 0;
+}
+#endif
+
+#endif /* __STM32MP1_SMC_H__ */
diff --git a/arch/arm/mach-stm32mp/include/mach/sys_proto.h b/arch/arm/mach-stm32mp/include/mach/sys_proto.h
index 41d4b40bcb..71a3ba794d 100644
--- a/arch/arm/mach-stm32mp/include/mach/sys_proto.h
+++ b/arch/arm/mach-stm32mp/include/mach/sys_proto.h
@@ -3,9 +3,15 @@
  * Copyright (C) 2015-2017, STMicroelectronics - All Rights Reserved
  */
 
-#define CPU_STMP32MP15x	0x500
+/* ID = Device Version (bit31:16) + Device Part Number (RPN) (bit15:0)*/
+#define CPU_STM32MP157Cxx	0x05000000
+#define CPU_STM32MP157Axx	0x05000001
+#define CPU_STM32MP153Cxx	0x05000024
+#define CPU_STM32MP153Axx	0x05000025
+#define CPU_STM32MP151Cxx	0x0500002E
+#define CPU_STM32MP151Axx	0x0500002F
 
-/* return CPU_STMP32MPxx constants */
+/* return CPU_STMP32MP...Xxx constants */
 u32 get_cpu_type(void);
 
 #define CPU_REVA	0x1000
@@ -13,3 +19,5 @@ u32 get_cpu_type(void);
 
 /* return CPU_REV constants */
 u32 get_cpu_rev(void);
+/* return boot mode */
+u32 get_bootmode(void);
diff --git a/arch/arm/mach-stm32mp/psci.c b/arch/arm/mach-stm32mp/psci.c
index 6ed2482080..c2dff38c36 100644
--- a/arch/arm/mach-stm32mp/psci.c
+++ b/arch/arm/mach-stm32mp/psci.c
@@ -103,7 +103,13 @@ int __secure psci_affinity_info(u32 function_id, u32 target_affinity,
 
 int __secure psci_migrate_info_type(u32 function_id)
 {
-	/* Trusted OS is either not present or does not require migration */
+	/*
+	 * in Power_State_Coordination_Interface_PDD_v1_1_DEN0022D.pdf
+	 * return 2 = Trusted OS is either not present or does not require
+	 * migration, system of this type does not require the caller
+	 * to use the MIGRATE function.
+	 * MIGRATE function calls return NOT_SUPPORTED.
+	 */
 	return 2;
 }
 
diff --git a/arch/arm/mach-stm32mp/spl.c b/arch/arm/mach-stm32mp/spl.c
index 790973e8b6..a3b0d6f382 100644
--- a/arch/arm/mach-stm32mp/spl.c
+++ b/arch/arm/mach-stm32mp/spl.c
@@ -7,13 +7,14 @@
 #include <dm.h>
 #include <spl.h>
 #include <asm/io.h>
+#include <asm/arch/sys_proto.h>
+#include <linux/libfdt.h>
 
 u32 spl_boot_device(void)
 {
 	u32 boot_mode;
 
-	boot_mode = (readl(TAMP_BOOT_CONTEXT) & TAMP_BOOT_MODE_MASK) >>
-		    TAMP_BOOT_MODE_SHIFT;
+	boot_mode = get_bootmode();
 
 	switch (boot_mode) {
 	case BOOT_FLASH_SD_1:
@@ -22,6 +23,21 @@ u32 spl_boot_device(void)
 	case BOOT_FLASH_SD_2:
 	case BOOT_FLASH_EMMC_2:
 		return BOOT_DEVICE_MMC2;
+	case BOOT_SERIAL_UART_1:
+	case BOOT_SERIAL_UART_2:
+	case BOOT_SERIAL_UART_3:
+	case BOOT_SERIAL_UART_4:
+	case BOOT_SERIAL_UART_5:
+	case BOOT_SERIAL_UART_6:
+	case BOOT_SERIAL_UART_7:
+	case BOOT_SERIAL_UART_8:
+		return BOOT_DEVICE_UART;
+	case BOOT_SERIAL_USB_OTG:
+		return BOOT_DEVICE_USB;
+	case BOOT_FLASH_NAND_FMC:
+		return BOOT_DEVICE_NAND;
+	case BOOT_FLASH_NOR_QSPI:
+		return BOOT_DEVICE_SPI;
 	}
 
 	return BOOT_DEVICE_MMC1;
@@ -44,6 +60,21 @@ int spl_boot_partition(const u32 boot_device)
 	}
 }
 
+#ifdef CONFIG_SPL_DISPLAY_PRINT
+void spl_display_print(void)
+{
+	DECLARE_GLOBAL_DATA_PTR;
+	const char *model;
+
+	/* same code than show_board_info() but not compiled for SPL
+	 * see CONFIG_DISPLAY_BOARDINFO & common/board_info.c
+	 */
+	model = fdt_getprop(gd->fdt_blob, 0, "model", NULL);
+	if (model)
+		printf("Model: %s\n", model);
+}
+#endif
+
 void board_init_f(ulong dummy)
 {
 	struct udevice *dev;
@@ -80,7 +111,7 @@ void board_init_f(ulong dummy)
 
 	ret = uclass_get_device(UCLASS_RAM, 0, &dev);
 	if (ret) {
-		debug("DRAM init failed: %d\n", ret);
-		return;
+		printf("DRAM init failed: %d\n", ret);
+		hang();
 	}
 }
diff --git a/arch/arm/mach-stm32mp/syscon.c b/arch/arm/mach-stm32mp/syscon.c
index eb7f435f10..242f8340ab 100644
--- a/arch/arm/mach-stm32mp/syscon.c
+++ b/arch/arm/mach-stm32mp/syscon.c
@@ -9,10 +9,11 @@
 #include <asm/arch/stm32.h>
 
 static const struct udevice_id stm32mp_syscon_ids[] = {
-	{ .compatible = "st,stm32-stgen",
-	  .data = STM32MP_SYSCON_STGEN },
-	{ .compatible = "st,stm32mp1-pwr",
-	  .data = STM32MP_SYSCON_PWR },
+	{ .compatible = "st,stm32mp1-etzpc", .data = STM32MP_SYSCON_ETZPC },
+	{ .compatible = "st,stm32mp1-pwr", .data = STM32MP_SYSCON_PWR },
+	{ .compatible = "st,stm32-stgen", .data = STM32MP_SYSCON_STGEN },
+	{ .compatible = "st,stm32mp157-syscfg",
+	  .data = STM32MP_SYSCON_SYSCFG },
 	{ }
 };
 
diff --git a/board/st/stm32mp1/MAINTAINERS b/board/st/stm32mp1/MAINTAINERS
index 48d8fd2c3f..0a2eddbe03 100644
--- a/board/st/stm32mp1/MAINTAINERS
+++ b/board/st/stm32mp1/MAINTAINERS
@@ -2,7 +2,8 @@ STM32MP1 BOARD
 M:	Patrick Delaunay <patrick.delaunay@st.com>
 L:	uboot-stm32@st-md-mailman.stormreply.com (moderated for non-subscribers)
 S:	Maintained
+F:	arch/arm/dts/stm32mp157*
 F:	board/st/stm32mp1
-F:	include/configs/stm32mp1.h
 F:	configs/stm32mp15_basic_defconfig
-F:	arch/arm/dts/stm32mp157*
+F:	configs/stm32mp15_trusted_defconfig
+F:	include/configs/stm32mp1.h
diff --git a/board/st/stm32mp1/README b/board/st/stm32mp1/README
index 174e6db148..1cd3534ae4 100644
--- a/board/st/stm32mp1/README
+++ b/board/st/stm32mp1/README
@@ -28,14 +28,15 @@ Everything is supported in Linux but U-Boot is limited to:
 
 And the necessary drivers
 1. I2C
-2. STPMU1
-2. STPMU1 (PMIC and regulator)
+2. STPMIC1 (PMIC and regulator)
 3. Clock, Reset, Sysreset
 4. Fuse
 
 Currently the following boards are supported:
 + stm32mp157c-ev1
 + stm32mp157c-ed1
++ stm32mp157a-dk1
++ stm32mp157c-dk2
 
 3. Boot Sequences
 =================
@@ -45,15 +46,22 @@ BootRom => FSBL in SYSRAM => SSBL in DDR => OS (Linux Kernel)
 with FSBL = First Stage Bootloader
      SSBL = Second Stage Bootloader
 
-One boot configuration is supported:
+2 boot configurations are supported:
 
-   The "Basic" boot chain (defconfig_file : stm32mp15_basic_defconfig)
+1) The "Trusted" boot chain (defconfig_file : stm32mp15_trusted_defconfig)
+   BootRom => FSBL = Trusted Firmware-A (TF-A) => SSBL = U-Boot
+   TF-A performs a full initialization of Secure peripherals and installs a
+   secure monitor.
+   U-Boot is running in normal world and uses TF-A monitor
+   to access to secure resources
+
+2) The "Basic" boot chain (defconfig_file : stm32mp15_basic_defconfig)
    BootRom => FSBL = U-Boot SPL => SSBL = U-Boot
    SPL has limited security initialisation
    U-Boot is running in secure mode and provide a secure monitor to the kernel
    with only PSCI support (Power State Coordination Interface defined by ARM)
 
-All the STM32MP1 board supported by U-Boot use the same generic board
+All the STM32MP1 boards supported by U-Boot use the same generic board
 stm32mp1 which support all the bootable devices.
 
 Each board is configurated only with the associated device tree.
@@ -64,12 +72,18 @@ Each board is configurated only with the associated device tree.
 You need to select the appropriate device tree for your board,
 the supported device trees for stm32mp157 are:
 
-+ ev1: eval board with pmic stpmu1 (ev1 = mother board + daughter ed1)
++ ev1: eval board with pmic stpmic1 (ev1 = mother board + daughter ed1)
   dts: stm32mp157c-ev1
 
-+ ed1: daughter board with pmic stpmu1
++ ed1: daughter board with pmic stpmic1
   dts: stm32mp157c-ed1
 
++ dk1: Discovery board
+  dts: stm32mp157a-dk1
+
++ dk2: Discovery board = dk1 with a BT/WiFI combo and a DSI panel
+  dts: stm32mp157c-dk2
+
 5. Build Procedure
 ==================
 
@@ -90,12 +104,14 @@ the supported device trees for stm32mp157 are:
 	# export KBUILD_OUTPUT=/path/to/output
 
 	for example: use one output directory for each configuration
+	# export KBUILD_OUTPUT=stm32mp15_trusted
 	# export KBUILD_OUTPUT=stm32mp15_basic
 
-4. Configure the U-Boot:
+4. Configure U-Boot:
 
 	# make <defconfig_file>
 
+	- For trusted boot mode : "stm32mp15_trusted_defconfig"
 	- For basic boot mode: "stm32mp15_basic_defconfig"
 
 5. Configure the device-tree and build the U-Boot image:
@@ -104,16 +120,26 @@ the supported device trees for stm32mp157 are:
 
 
   example:
-     basic boot on ev1
+  a) trusted boot on ev1
+	# export KBUILD_OUTPUT=stm32mp15_trusted
+	# make stm32mp15_trusted_defconfig
+	# make DEVICE_TREE=stm32mp157c-ev1 all
+
+  b) basic boot on ev1
 	# export KBUILD_OUTPUT=stm32mp15_basic
 	# make stm32mp15_basic_defconfig
 	# make DEVICE_TREE=stm32mp157c-ev1 all
 
-     basic boot on ed1
+  c) basic boot on ed1
 	# export KBUILD_OUTPUT=stm32mp15_basic
 	# make stm32mp15_basic_defconfig
 	# make DEVICE_TREE=stm32mp157c-ed1 all
 
+  d) basic boot on dk2
+	# export KBUILD_OUTPUT=stm32mp15_basic
+	# make stm32mp15_basic_defconfig
+	# make DEVICE_TREE=stm32mp157c-dk2 all
+
 6. Output files
 
   BootRom and TF-A expect binaries with STM32 image header
@@ -122,6 +148,11 @@ the supported device trees for stm32mp157 are:
   So in the output directory (selected by KBUILD_OUTPUT),
   you can found the needed files:
 
+  a) For Trusted boot
+   + FSBL = tf-a.stm32 (provided by TF-A compilation)
+   + SSBL = u-boot.stm32
+
+  b) For Basic boot
    + FSBL = spl/u-boot-spl.stm32
    + SSBL = u-boot.img
 
@@ -135,13 +166,22 @@ You can select the boot mode, on the board ed1 with the switch SW1
  -----------------------------------
   Reserved	0	0	0
   NOR		0	0	1
-  SD-Card	1	1	1
   SD-Card	1	0	1
   eMMC		0	1	0
   NAND		0	1	1
   Recovery	1	1	0
   Recovery	0	0	0
 
+- on board DK1/DK2 with the switch SW1 : BOOT0, BOOT2
+  (BOOT1 forced to 0, NOR not supported)
+
+ --------------------------
+  Boot Mode   BOOT2  BOOT0
+ --------------------------
+  Reserved	1      0
+  SD-Card	1      1
+  Recovery	0      0
+
 Recovery is a boot from serial link (UART/USB) and it is used with
 STM32CubeProgrammer tool to load executable in RAM and to update the flash
 devices available on the board (NOR/NAND/eMMC/SDCARD).
@@ -158,14 +198,14 @@ The minimal requirements for STMP32MP1 boot up to U-Boot are:
 - one ssbl partition for U-Boot
 
 Then the minimal GPT partition is:
-   ----- ------- --------- -------------
-  | Num | Name  | Size    |  Content    |
-   ----- ------- -------- --------------
+   ----- ------- --------- --------------
+  | Num | Name  | Size    |  Content     |
+   ----- ------- -------- ---------------
   |  1  | fsbl1 | 256 KiB |  TF-A or SPL |
   |  2  | fsbl2 | 256 KiB |  TF-A or SPL |
-  |  3  | ssbl  | enought |  U-Boot     |
-  |  *  |  -    |  -      |  Boot/Rootfs|
-   ----- ------- --------- -------------
+  |  3  | ssbl  | enought |  U-Boot      |
+  |  *  |  -    |  -      |  Boot/Rootfs |
+   ----- ------- --------- --------------
 
 (*) add bootable partition for extlinux.conf
     following Generic Distribution
@@ -189,7 +229,7 @@ for example: with gpt table with 128 entries
 
 	you can add other partitions for kernel
 	one partition rootfs for example:
-		-n 3:5154:		-c 4:rootfs
+		-n 4:5154:		-c 4:rootfs \
 
   c) copy the FSBL (2 times) and SSBL file on the correct partition.
      in this example in partition 1 to 3
@@ -199,6 +239,11 @@ for example: with gpt table with 128 entries
 	# dd if=u-boot-spl.stm32 of=/dev/mmcblk0p2
 	# dd if=u-boot.img of=/dev/mmcblk0p3
 
+     for trusted boot mode :
+	# dd if=tf-a.stm32 of=/dev/mmcblk0p1
+	# dd if=tf-a.stm32 of=/dev/mmcblk0p2
+	# dd if=u-boot.stm32 of=/dev/mmcblk0p3
+
 To boot from SDCard, select BootPinMode = 1 1 1 and reset.
 
 8. Prepare eMMC
@@ -208,7 +253,7 @@ You can use U-Boot to copy binary in eMMC.
 In the next example, you need to boot from SDCARD and the images (u-boot-spl.stm32, u-boot.img)
 are presents on SDCARD (mmc 0) in ext4 partition 4 (bootfs).
 
-To boot from SDCard, select BootPinMode = 1 1 1 and reset.
+To boot from SDCard, select BootPinMode = 1 0 1 and reset.
 
 Then you update the eMMC with the next U-Boot command :
 
@@ -227,7 +272,7 @@ b) copy SPL on eMMC on firts boot partition
 	# mmc write ${fileaddr} 0 200
 	# mmc partconf 1 1 1 0
 
-b) copy U-Boot in first GPT partition of eMMC
+c) copy U-Boot in first GPT partition of eMMC
 
 	# ext4load mmc 0:4 0xC0000000 u-boot.img
 	# mmc dev 1
diff --git a/board/st/stm32mp1/board.c b/board/st/stm32mp1/board.c
index 5f31ea99f5..5c1acca20d 100644
--- a/board/st/stm32mp1/board.c
+++ b/board/st/stm32mp1/board.c
@@ -8,7 +8,7 @@
 #include <asm/io.h>
 #include <asm/arch/ddr.h>
 #include <power/pmic.h>
-#include <power/stpmu1.h>
+#include <power/stpmic1.h>
 
 #ifdef CONFIG_DEBUG_UART_BOARD_INIT
 void board_debug_uart_init(void)
@@ -37,64 +37,65 @@ void board_debug_uart_init(void)
 }
 #endif
 
-#ifdef CONFIG_PMIC_STPMU1
+#ifdef CONFIG_PMIC_STPMIC1
 int board_ddr_power_init(void)
 {
 	struct udevice *dev;
 	int ret;
 
 	ret = uclass_get_device_by_driver(UCLASS_PMIC,
-					  DM_GET_DRIVER(pmic_stpmu1), &dev);
+					  DM_GET_DRIVER(pmic_stpmic1), &dev);
 	if (ret)
 		/* No PMIC on board */
 		return 0;
 
-	/* Set LDO3 to sync mode */
-	ret = pmic_reg_read(dev, STPMU1_LDOX_CTRL_REG(STPMU1_LDO3));
+	/* VTT = Set LDO3 to sync mode */
+	ret = pmic_reg_read(dev, STPMIC1_LDOX_MAIN_CR(STPMIC1_LDO3));
 	if (ret < 0)
 		return ret;
 
-	ret &= ~STPMU1_LDO3_MODE;
-	ret &= ~STPMU1_LDO12356_OUTPUT_MASK;
-	ret |= STPMU1_LDO3_DDR_SEL << STPMU1_LDO12356_OUTPUT_SHIFT;
+	ret &= ~STPMIC1_LDO3_MODE;
+	ret &= ~STPMIC1_LDO12356_VOUT_MASK;
+	ret |= STPMIC1_LDO_VOUT(STPMIC1_LDO3_DDR_SEL);
 
-	ret = pmic_reg_write(dev, STPMU1_LDOX_CTRL_REG(STPMU1_LDO3),
+	ret = pmic_reg_write(dev, STPMIC1_LDOX_MAIN_CR(STPMIC1_LDO3),
 			     ret);
 	if (ret < 0)
 		return ret;
 
-	/* Set BUCK2 to 1.35V */
+	/* VDD_DDR = Set BUCK2 to 1.35V */
 	ret = pmic_clrsetbits(dev,
-			      STPMU1_BUCKX_CTRL_REG(STPMU1_BUCK2),
-			      STPMU1_BUCK_OUTPUT_MASK,
-			      STPMU1_BUCK2_1350000V);
+			      STPMIC1_BUCKX_MAIN_CR(STPMIC1_BUCK2),
+			      STPMIC1_BUCK_VOUT_MASK,
+			      STPMIC1_BUCK2_1350000V);
 	if (ret < 0)
 		return ret;
 
-	/* Enable BUCK2 and VREF */
+	/* Enable VDD_DDR = BUCK2 */
 	ret = pmic_clrsetbits(dev,
-			      STPMU1_BUCKX_CTRL_REG(STPMU1_BUCK2),
-			      STPMU1_BUCK_EN, STPMU1_BUCK_EN);
+			      STPMIC1_BUCKX_MAIN_CR(STPMIC1_BUCK2),
+			      STPMIC1_BUCK_ENA, STPMIC1_BUCK_ENA);
 	if (ret < 0)
 		return ret;
 
-	mdelay(STPMU1_DEFAULT_START_UP_DELAY_MS);
+	mdelay(STPMIC1_DEFAULT_START_UP_DELAY_MS);
 
-	ret = pmic_clrsetbits(dev, STPMU1_VREF_CTRL_REG,
-			      STPMU1_VREF_EN, STPMU1_VREF_EN);
+	/* Enable VREF */
+	ret = pmic_clrsetbits(dev, STPMIC1_REFDDR_MAIN_CR,
+			      STPMIC1_VREF_ENA, STPMIC1_VREF_ENA);
 	if (ret < 0)
 		return ret;
 
-	mdelay(STPMU1_DEFAULT_START_UP_DELAY_MS);
+	mdelay(STPMIC1_DEFAULT_START_UP_DELAY_MS);
 
 	/* Enable LDO3 */
 	ret = pmic_clrsetbits(dev,
-			      STPMU1_LDOX_CTRL_REG(STPMU1_LDO3),
-			      STPMU1_LDO_EN, STPMU1_LDO_EN);
+			      STPMIC1_LDOX_MAIN_CR(STPMIC1_LDO3),
+			      STPMIC1_LDO_ENA, STPMIC1_LDO_ENA);
 	if (ret < 0)
 		return ret;
 
-	mdelay(STPMU1_DEFAULT_START_UP_DELAY_MS);
+	mdelay(STPMIC1_DEFAULT_START_UP_DELAY_MS);
 
 	return 0;
 }
diff --git a/board/st/stm32mp1/spl.c b/board/st/stm32mp1/spl.c
index f3db0d6385..a7844f244b 100644
--- a/board/st/stm32mp1/spl.c
+++ b/board/st/stm32mp1/spl.c
@@ -11,22 +11,22 @@
 #include <asm/io.h>
 #include <post.h>
 #include <power/pmic.h>
-#include <power/stpmu1.h>
+#include <power/stpmic1.h>
 #include <asm/arch/ddr.h>
 
 void spl_board_init(void)
 {
 	/* Keep vdd on during the reset cycle */
-#if defined(CONFIG_PMIC_STPMU1) && defined(CONFIG_SPL_POWER_SUPPORT)
+#if defined(CONFIG_PMIC_STPMIC1) && defined(CONFIG_SPL_POWER_SUPPORT)
 	struct udevice *dev;
 	int ret;
 
 	ret = uclass_get_device_by_driver(UCLASS_PMIC,
-					  DM_GET_DRIVER(pmic_stpmu1), &dev);
+					  DM_GET_DRIVER(pmic_stpmic1), &dev);
 	if (!ret)
 		pmic_clrsetbits(dev,
-				STPMU1_MASK_RESET_BUCK,
-				STPMU1_MASK_RESET_BUCK3,
-				STPMU1_MASK_RESET_BUCK3);
+				STPMIC1_BUCKS_MRST_CR,
+				STPMIC1_MRST_BUCK(STPMIC1_BUCK3),
+				STPMIC1_MRST_BUCK(STPMIC1_BUCK3));
 #endif
 }
diff --git a/board/st/stm32mp1/stm32mp1.c b/board/st/stm32mp1/stm32mp1.c
index 54feca0ecf..24d299ac33 100644
--- a/board/st/stm32mp1/stm32mp1.c
+++ b/board/st/stm32mp1/stm32mp1.c
@@ -2,20 +2,57 @@
 /*
  * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
  */
-#include <config.h>
 #include <common.h>
-#include <led.h>
+#include <adc.h>
+#include <config.h>
 #include <clk.h>
 #include <dm.h>
 #include <generic-phy.h>
+#include <led.h>
+#include <misc.h>
 #include <phy.h>
 #include <reset.h>
+#include <syscon.h>
 #include <usb.h>
-#include <asm/arch/stm32.h>
 #include <asm/io.h>
+#include <asm/gpio.h>
+#include <asm/arch/stm32.h>
 #include <power/regulator.h>
 #include <usb/dwc2_udc.h>
 
+/* SYSCFG registers */
+#define SYSCFG_BOOTR		0x00
+#define SYSCFG_PMCSETR		0x04
+#define SYSCFG_IOCTRLSETR	0x18
+#define SYSCFG_ICNR		0x1C
+#define SYSCFG_CMPCR		0x20
+#define SYSCFG_CMPENSETR	0x24
+#define SYSCFG_PMCCLRR		0x44
+
+#define SYSCFG_BOOTR_BOOT_MASK		GENMASK(2, 0)
+#define SYSCFG_BOOTR_BOOTPD_SHIFT	4
+
+#define SYSCFG_IOCTRLSETR_HSLVEN_TRACE		BIT(0)
+#define SYSCFG_IOCTRLSETR_HSLVEN_QUADSPI	BIT(1)
+#define SYSCFG_IOCTRLSETR_HSLVEN_ETH		BIT(2)
+#define SYSCFG_IOCTRLSETR_HSLVEN_SDMMC		BIT(3)
+#define SYSCFG_IOCTRLSETR_HSLVEN_SPI		BIT(4)
+
+#define SYSCFG_CMPCR_SW_CTRL		BIT(1)
+#define SYSCFG_CMPCR_READY		BIT(8)
+
+#define SYSCFG_CMPENSETR_MPU_EN		BIT(0)
+
+#define SYSCFG_PMCSETR_ETH_CLK_SEL	BIT(16)
+#define SYSCFG_PMCSETR_ETH_REF_CLK_SEL	BIT(17)
+
+#define SYSCFG_PMCSETR_ETH_SELMII	BIT(20)
+
+#define SYSCFG_PMCSETR_ETH_SEL_MASK	GENMASK(23, 21)
+#define SYSCFG_PMCSETR_ETH_SEL_GMII_MII	(0 << 21)
+#define SYSCFG_PMCSETR_ETH_SEL_RGMII	(1 << 21)
+#define SYSCFG_PMCSETR_ETH_SEL_RMII	(4 << 21)
+
 /*
  * Get a global data pointer
  */
@@ -26,6 +63,98 @@ DECLARE_GLOBAL_DATA_PTR;
 #define STM32MP_GGPIO 0x38
 #define STM32MP_GGPIO_VBUS_SENSING BIT(21)
 
+#define USB_WARNING_LOW_THRESHOLD_UV	660000
+#define USB_START_LOW_THRESHOLD_UV	1230000
+#define USB_START_HIGH_THRESHOLD_UV	2100000
+
+int checkboard(void)
+{
+	int ret;
+	char *mode;
+	u32 otp;
+	struct udevice *dev;
+	const char *fdt_compat;
+	int fdt_compat_len;
+
+	if (IS_ENABLED(CONFIG_STM32MP1_TRUSTED))
+		mode = "trusted";
+	else
+		mode = "basic";
+
+	printf("Board: stm32mp1 in %s mode", mode);
+	fdt_compat = fdt_getprop(gd->fdt_blob, 0, "compatible",
+				 &fdt_compat_len);
+	if (fdt_compat && fdt_compat_len)
+		printf(" (%s)", fdt_compat);
+	puts("\n");
+
+	ret = uclass_get_device_by_driver(UCLASS_MISC,
+					  DM_GET_DRIVER(stm32mp_bsec),
+					  &dev);
+
+	if (!ret)
+		ret = misc_read(dev, STM32_BSEC_SHADOW(BSEC_OTP_BOARD),
+				&otp, sizeof(otp));
+	if (!ret && otp) {
+		printf("Board: MB%04x Var%d Rev.%c-%02d\n",
+		       otp >> 16,
+		       (otp >> 12) & 0xF,
+		       ((otp >> 8) & 0xF) - 1 + 'A',
+		       otp & 0xF);
+	}
+
+	return 0;
+}
+
+static void board_key_check(void)
+{
+#if defined(CONFIG_FASTBOOT) || defined(CONFIG_CMD_STM32PROG)
+	ofnode node;
+	struct gpio_desc gpio;
+	enum forced_boot_mode boot_mode = BOOT_NORMAL;
+
+	node = ofnode_path("/config");
+	if (!ofnode_valid(node)) {
+		debug("%s: no /config node?\n", __func__);
+		return;
+	}
+#ifdef CONFIG_FASTBOOT
+	if (gpio_request_by_name_nodev(node, "st,fastboot-gpios", 0,
+				       &gpio, GPIOD_IS_IN)) {
+		debug("%s: could not find a /config/st,fastboot-gpios\n",
+		      __func__);
+	} else {
+		if (dm_gpio_get_value(&gpio)) {
+			puts("Fastboot key pressed, ");
+			boot_mode = BOOT_FASTBOOT;
+		}
+
+		dm_gpio_free(NULL, &gpio);
+	}
+#endif
+#ifdef CONFIG_CMD_STM32PROG
+	if (gpio_request_by_name_nodev(node, "st,stm32prog-gpios", 0,
+				       &gpio, GPIOD_IS_IN)) {
+		debug("%s: could not find a /config/st,stm32prog-gpios\n",
+		      __func__);
+	} else {
+		if (dm_gpio_get_value(&gpio)) {
+			puts("STM32Programmer key pressed, ");
+			boot_mode = BOOT_STM32PROG;
+		}
+		dm_gpio_free(NULL, &gpio);
+	}
+#endif
+
+	if (boot_mode != BOOT_NORMAL) {
+		puts("entering download mode...\n");
+		clrsetbits_le32(TAMP_BOOT_CONTEXT,
+				TAMP_BOOT_FORCED_MASK,
+				boot_mode);
+	}
+#endif
+}
+
 static struct dwc2_plat_otg_data stm32mp_otg_data = {
 	.usb_gusbcfg = STM32MP_GUSBCFG,
 };
@@ -170,6 +299,145 @@ clk_err:
 	return ret;
 }
 
+static int get_led(struct udevice **dev, char *led_string)
+{
+	char *led_name;
+	int ret;
+
+	led_name = fdtdec_get_config_string(gd->fdt_blob, led_string);
+	if (!led_name) {
+		pr_debug("%s: could not find %s config string\n",
+			 __func__, led_string);
+		return -ENOENT;
+	}
+	ret = led_get_by_label(led_name, dev);
+	if (ret) {
+		debug("%s: get=%d\n", __func__, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int setup_led(enum led_state_t cmd)
+{
+	struct udevice *dev;
+	int ret;
+
+	ret = get_led(&dev, "u-boot,boot-led");
+	if (ret)
+		return ret;
+
+	ret = led_set_state(dev, cmd);
+	return ret;
+}
+
+static int board_check_usb_power(void)
+{
+	struct ofnode_phandle_args adc_args;
+	struct udevice *adc;
+	struct udevice *led;
+	ofnode node;
+	unsigned int raw;
+	int max_uV = 0;
+	int ret, uV, adc_count;
+	u8 i, nb_blink;
+
+	node = ofnode_path("/config");
+	if (!ofnode_valid(node)) {
+		debug("%s: no /config node?\n", __func__);
+		return -ENOENT;
+	}
+
+	/*
+	 * Retrieve the ADC channels devices and get measurement
+	 * for each of them
+	 */
+	adc_count = ofnode_count_phandle_with_args(node, "st,adc_usb_pd",
+						   "#io-channel-cells");
+	if (adc_count < 0) {
+		if (adc_count == -ENOENT)
+			return 0;
+
+		pr_err("%s: can't find adc channel (%d)\n", __func__,
+		       adc_count);
+
+		return adc_count;
+	}
+
+	for (i = 0; i < adc_count; i++) {
+		if (ofnode_parse_phandle_with_args(node, "st,adc_usb_pd",
+						   "#io-channel-cells", 0, i,
+						   &adc_args)) {
+			pr_debug("%s: can't find /config/st,adc_usb_pd\n",
+				 __func__);
+			return 0;
+		}
+
+		ret = uclass_get_device_by_ofnode(UCLASS_ADC, adc_args.node,
+						  &adc);
+
+		if (ret) {
+			pr_err("%s: Can't get adc device(%d)\n", __func__,
+			       ret);
+			return ret;
+		}
+
+		ret = adc_channel_single_shot(adc->name, adc_args.args[0],
+					      &raw);
+		if (ret) {
+			pr_err("%s: single shot failed for %s[%d]!\n",
+			       __func__, adc->name, adc_args.args[0]);
+			return ret;
+		}
+		/* Convert to uV */
+		if (!adc_raw_to_uV(adc, raw, &uV)) {
+			if (uV > max_uV)
+				max_uV = uV;
+			pr_debug("%s: %s[%02d] = %u, %d uV\n", __func__,
+				 adc->name, adc_args.args[0], raw, uV);
+		} else {
+			pr_err("%s: Can't get uV value for %s[%d]\n",
+			       __func__, adc->name, adc_args.args[0]);
+		}
+	}
+
+	/*
+	 * If highest value is inside 1.23 Volts and 2.10 Volts, that means
+	 * board is plugged on an USB-C 3A power supply and boot process can
+	 * continue.
+	 */
+	if (max_uV > USB_START_LOW_THRESHOLD_UV &&
+	    max_uV < USB_START_HIGH_THRESHOLD_UV)
+		return 0;
+
+	/* Display warning message and make u-boot,error-led blinking */
+	pr_err("\n*******************************************\n");
+
+	if (max_uV < USB_WARNING_LOW_THRESHOLD_UV) {
+		pr_err("*   WARNING 500mA power supply detected   *\n");
+		nb_blink = 2;
+	} else {
+		pr_err("* WARNING 1.5A power supply detected      *\n");
+		nb_blink = 3;
+	}
+
+	pr_err("* Current too low, use a 3A power supply! *\n");
+	pr_err("*******************************************\n\n");
+
+	ret = get_led(&led, "u-boot,error-led");
+	if (ret)
+		return ret;
+
+	for (i = 0; i < nb_blink * 2; i++) {
+		led_set_state(led, LEDST_TOGGLE);
+		mdelay(125);
+	}
+	led_set_state(led, LEDST_ON);
+
+	return 0;
+}
+
 int board_usb_cleanup(int index, enum usb_init_type init)
 {
 	/* Reset usbotg */
@@ -180,19 +448,146 @@ int board_usb_cleanup(int index, enum usb_init_type init)
 	return 0;
 }
 
-int board_late_init(void)
+static void sysconf_init(void)
 {
-	return 0;
+#ifndef CONFIG_STM32MP1_TRUSTED
+	u8 *syscfg;
+#ifdef CONFIG_DM_REGULATOR
+	struct udevice *pwr_dev;
+	struct udevice *pwr_reg;
+	struct udevice *dev;
+	int ret;
+	u32 otp = 0;
+#endif
+	u32 bootr;
+
+	syscfg = (u8 *)syscon_get_first_range(STM32MP_SYSCON_SYSCFG);
+
+	/* interconnect update : select master using the port 1 */
+	/* LTDC = AXI_M9 */
+	/* GPU  = AXI_M8 */
+	/* today information is hardcoded in U-Boot */
+	writel(BIT(9), syscfg + SYSCFG_ICNR);
+
+	/* disable Pull-Down for boot pin connected to VDD */
+	bootr = readl(syscfg + SYSCFG_BOOTR);
+	bootr &= ~(SYSCFG_BOOTR_BOOT_MASK << SYSCFG_BOOTR_BOOTPD_SHIFT);
+	bootr |= (bootr & SYSCFG_BOOTR_BOOT_MASK) << SYSCFG_BOOTR_BOOTPD_SHIFT;
+	writel(bootr, syscfg + SYSCFG_BOOTR);
+
+#ifdef CONFIG_DM_REGULATOR
+	/* High Speed Low Voltage Pad mode Enable for SPI, SDMMC, ETH, QSPI
+	 * and TRACE. Needed above ~50MHz and conditioned by AFMUX selection.
+	 * The customer will have to disable this for low frequencies
+	 * or if AFMUX is selected but the function not used, typically for
+	 * TRACE. Otherwise, impact on power consumption.
+	 *
+	 * WARNING:
+	 *   enabling High Speed mode while VDD>2.7V
+	 *   with the OTP product_below_2v5 (OTP 18, BIT 13)
+	 *   erroneously set to 1 can damage the IC!
+	 *   => U-Boot set the register only if VDD < 2.7V (in DT)
+	 *      but this value need to be consistent with board design
+	 */
+	ret = syscon_get_by_driver_data(STM32MP_SYSCON_PWR, &pwr_dev);
+	if (!ret) {
+		ret = uclass_get_device_by_driver(UCLASS_MISC,
+						  DM_GET_DRIVER(stm32mp_bsec),
+						  &dev);
+		if (ret) {
+			pr_err("Can't find stm32mp_bsec driver\n");
+			return;
+		}
+
+		ret = misc_read(dev, STM32_BSEC_SHADOW(18), &otp, 4);
+		if (!ret)
+			otp = otp & BIT(13);
+
+		/* get VDD = pwr-supply */
+		ret = device_get_supply_regulator(pwr_dev, "pwr-supply",
+						  &pwr_reg);
+
+		/* check if VDD is Low Voltage */
+		if (!ret) {
+			if (regulator_get_value(pwr_reg) < 2700000) {
+				writel(SYSCFG_IOCTRLSETR_HSLVEN_TRACE |
+				       SYSCFG_IOCTRLSETR_HSLVEN_QUADSPI |
+				       SYSCFG_IOCTRLSETR_HSLVEN_ETH |
+				       SYSCFG_IOCTRLSETR_HSLVEN_SDMMC |
+				       SYSCFG_IOCTRLSETR_HSLVEN_SPI,
+				       syscfg + SYSCFG_IOCTRLSETR);
+
+				if (!otp)
+					pr_err("product_below_2v5=0: HSLVEN protected by HW\n");
+			} else {
+				if (otp)
+					pr_err("product_below_2v5=1: HSLVEN update is destructive, no update as VDD>2.7V\n");
+			}
+		} else {
+			debug("VDD unknown");
+		}
+	}
+#endif
+
+	/* activate automatic I/O compensation
+	 * warning: need to ensure CSI enabled and ready in clock driver
+	 */
+	writel(SYSCFG_CMPENSETR_MPU_EN, syscfg + SYSCFG_CMPENSETR);
+
+	while (!(readl(syscfg + SYSCFG_CMPCR) & SYSCFG_CMPCR_READY))
+		;
+	clrbits_le32(syscfg + SYSCFG_CMPCR, SYSCFG_CMPCR_SW_CTRL);
+#endif
 }
 
 /* board dependent setup after realloc */
 int board_init(void)
 {
+	struct udevice *dev;
+
 	/* address of boot parameters */
 	gd->bd->bi_boot_params = STM32_DDR_BASE + 0x100;
 
+	/* probe all PINCTRL for hog */
+	for (uclass_first_device(UCLASS_PINCTRL, &dev);
+	     dev;
+	     uclass_next_device(&dev)) {
+		pr_debug("probe pincontrol = %s\n", dev->name);
+	}
+
+	board_key_check();
+
+	sysconf_init();
+
 	if (IS_ENABLED(CONFIG_LED))
 		led_default_state();
 
 	return 0;
 }
+
+int board_late_init(void)
+{
+#ifdef CONFIG_ENV_VARS_UBOOT_RUNTIME_CONFIG
+	const void *fdt_compat;
+	int fdt_compat_len;
+
+	fdt_compat = fdt_getprop(gd->fdt_blob, 0, "compatible",
+				 &fdt_compat_len);
+	if (fdt_compat && fdt_compat_len) {
+		if (strncmp(fdt_compat, "st,", 3) != 0)
+			env_set("board_name", fdt_compat);
+		else
+			env_set("board_name", fdt_compat + 3);
+	}
+#endif
+
+	/* for DK1/DK2 boards */
+	board_check_usb_power();
+
+	return 0;
+}
+
+void board_quiesce_devices(void)
+{
+	setup_led(LEDST_OFF);
+}
diff --git a/configs/stm32mp15_basic_defconfig b/configs/stm32mp15_basic_defconfig
index d20b2ab350..6781adb81e 100644
--- a/configs/stm32mp15_basic_defconfig
+++ b/configs/stm32mp15_basic_defconfig
@@ -1,11 +1,12 @@
 CONFIG_ARM=y
 CONFIG_ARCH_STM32MP=y
-CONFIG_SYS_MALLOC_F_LEN=0x2000
+CONFIG_SYS_MALLOC_F_LEN=0x3000
 CONFIG_SPL_MMC_SUPPORT=y
 CONFIG_SPL=y
 CONFIG_TARGET_STM32MP1=y
 CONFIG_DISTRO_DEFAULTS=y
-CONFIG_NR_DRAM_BANKS=1
+CONFIG_FIT=y
+CONFIG_BOOTCOMMAND="run bootcmd_stm32mp"
 CONFIG_SYS_MMCSD_RAW_MODE_U_BOOT_USE_PARTITION=y
 CONFIG_SYS_MMCSD_RAW_MODE_U_BOOT_PARTITION=3
 CONFIG_SPL_I2C_SUPPORT=y
@@ -18,8 +19,10 @@ CONFIG_SYS_PROMPT="STM32MP> "
 # CONFIG_CMD_EXPORTENV is not set
 # CONFIG_CMD_IMPORTENV is not set
 CONFIG_CMD_MEMINFO=y
+CONFIG_CMD_MEMTEST=y
 CONFIG_CMD_ADC=y
 CONFIG_CMD_CLK=y
+CONFIG_CMD_DFU=y
 CONFIG_CMD_FUSE=y
 CONFIG_CMD_GPIO=y
 CONFIG_CMD_GPT=y
@@ -27,12 +30,21 @@ CONFIG_CMD_I2C=y
 CONFIG_CMD_MMC=y
 CONFIG_CMD_USB=y
 CONFIG_CMD_USB_MASS_STORAGE=y
+CONFIG_CMD_CACHE=y
+CONFIG_CMD_TIME=y
+CONFIG_CMD_TIMER=y
 CONFIG_CMD_PMIC=y
 CONFIG_CMD_REGULATOR=y
 CONFIG_CMD_EXT4_WRITE=y
 # CONFIG_SPL_DOS_PARTITION is not set
 CONFIG_DEFAULT_DEVICE_TREE="stm32mp157c-ev1"
 CONFIG_STM32_ADC=y
+CONFIG_USB_FUNCTION_FASTBOOT=y
+CONFIG_FASTBOOT_BUF_ADDR=0xC0000000
+CONFIG_FASTBOOT_BUF_SIZE=0x02000000
+CONFIG_FASTBOOT_USB_DEV=1
+CONFIG_FASTBOOT_FLASH=y
+CONFIG_FASTBOOT_FLASH_MMC_DEV=1
 CONFIG_DM_HWSPINLOCK=y
 CONFIG_HWSPINLOCK_STM32=y
 CONFIG_DM_I2C=y
@@ -43,15 +55,16 @@ CONFIG_DM_MMC=y
 CONFIG_STM32_SDMMC2=y
 CONFIG_PHY=y
 CONFIG_PHY_STM32_USBPHYC=y
-# CONFIG_PINCTRL_FULL is not set
+CONFIG_PINCONF=y
 # CONFIG_SPL_PINCTRL_FULL is not set
+CONFIG_PINCTRL_STMFX=y
 CONFIG_DM_PMIC=y
 # CONFIG_SPL_PMIC_CHILDREN is not set
-CONFIG_PMIC_STPMU1=y
+CONFIG_PMIC_STPMIC1=y
 CONFIG_DM_REGULATOR_FIXED=y
 CONFIG_DM_REGULATOR_GPIO=y
 CONFIG_DM_REGULATOR_STM32_VREFBUF=y
-CONFIG_DM_REGULATOR_STPMU1=y
+CONFIG_DM_REGULATOR_STPMIC1=y
 CONFIG_SERIAL_RX_BUFFER=y
 CONFIG_STM32_SERIAL=y
 CONFIG_USB=y
@@ -64,4 +77,3 @@ CONFIG_USB_GADGET_MANUFACTURER="STMicroelectronics"
 CONFIG_USB_GADGET_VENDOR_NUM=0x0483
 CONFIG_USB_GADGET_PRODUCT_NUM=0x5720
 CONFIG_USB_GADGET_DWC2_OTG=y
-CONFIG_USB_GADGET_DOWNLOAD=y
diff --git a/configs/stm32mp15_trusted_defconfig b/configs/stm32mp15_trusted_defconfig
new file mode 100644
index 0000000000..a050cee691
--- /dev/null
+++ b/configs/stm32mp15_trusted_defconfig
@@ -0,0 +1,70 @@
+CONFIG_ARM=y
+CONFIG_ARCH_STM32MP=y
+CONFIG_SYS_MALLOC_F_LEN=0x3000
+CONFIG_TARGET_STM32MP1=y
+CONFIG_DISTRO_DEFAULTS=y
+CONFIG_FIT=y
+CONFIG_BOOTCOMMAND="run bootcmd_stm32mp"
+CONFIG_SYS_PROMPT="STM32MP> "
+# CONFIG_CMD_BOOTD is not set
+# CONFIG_CMD_ELF is not set
+# CONFIG_CMD_IMI is not set
+# CONFIG_CMD_XIMG is not set
+# CONFIG_CMD_EXPORTENV is not set
+# CONFIG_CMD_IMPORTENV is not set
+CONFIG_CMD_MEMINFO=y
+CONFIG_CMD_MEMTEST=y
+CONFIG_CMD_ADC=y
+CONFIG_CMD_CLK=y
+CONFIG_CMD_DFU=y
+CONFIG_CMD_FUSE=y
+CONFIG_CMD_GPIO=y
+CONFIG_CMD_GPT=y
+CONFIG_CMD_I2C=y
+CONFIG_CMD_MMC=y
+CONFIG_CMD_USB=y
+CONFIG_CMD_USB_MASS_STORAGE=y
+CONFIG_CMD_CACHE=y
+CONFIG_CMD_TIME=y
+CONFIG_CMD_TIMER=y
+CONFIG_CMD_PMIC=y
+CONFIG_CMD_REGULATOR=y
+CONFIG_CMD_EXT4_WRITE=y
+CONFIG_DEFAULT_DEVICE_TREE="stm32mp157c-ev1"
+CONFIG_STM32_ADC=y
+CONFIG_USB_FUNCTION_FASTBOOT=y
+CONFIG_FASTBOOT_BUF_ADDR=0xC0000000
+CONFIG_FASTBOOT_BUF_SIZE=0x02000000
+CONFIG_FASTBOOT_USB_DEV=1
+CONFIG_FASTBOOT_FLASH=y
+CONFIG_FASTBOOT_FLASH_MMC_DEV=1
+CONFIG_DM_HWSPINLOCK=y
+CONFIG_HWSPINLOCK_STM32=y
+CONFIG_DM_I2C=y
+CONFIG_SYS_I2C_STM32F7=y
+CONFIG_LED=y
+CONFIG_LED_GPIO=y
+CONFIG_DM_MMC=y
+CONFIG_STM32_SDMMC2=y
+CONFIG_PHY=y
+CONFIG_PHY_STM32_USBPHYC=y
+CONFIG_PINCONF=y
+CONFIG_PINCTRL_STMFX=y
+CONFIG_DM_PMIC=y
+CONFIG_PMIC_STPMIC1=y
+CONFIG_DM_REGULATOR_FIXED=y
+CONFIG_DM_REGULATOR_GPIO=y
+CONFIG_DM_REGULATOR_STM32_VREFBUF=y
+CONFIG_DM_REGULATOR_STPMIC1=y
+CONFIG_SERIAL_RX_BUFFER=y
+CONFIG_STM32_SERIAL=y
+CONFIG_USB=y
+CONFIG_DM_USB=y
+CONFIG_USB_EHCI_HCD=y
+CONFIG_USB_EHCI_GENERIC=y
+CONFIG_USB_DWC2=y
+CONFIG_USB_GADGET=y
+CONFIG_USB_GADGET_MANUFACTURER="STMicroelectronics"
+CONFIG_USB_GADGET_VENDOR_NUM=0x0483
+CONFIG_USB_GADGET_PRODUCT_NUM=0x5720
+CONFIG_USB_GADGET_DWC2_OTG=y
diff --git a/doc/device-tree-bindings/mtd/stm32-fmc2-nand.txt b/doc/device-tree-bindings/mtd/stm32-fmc2-nand.txt
new file mode 100644
index 0000000000..70e76be2a3
--- /dev/null
+++ b/doc/device-tree-bindings/mtd/stm32-fmc2-nand.txt
@@ -0,0 +1,59 @@
+STMicroelectronics Flexible Memory Controller 2 (FMC2)
+NAND Interface
+
+Required properties:
+- compatible: Should be one of:
+              * st,stm32mp15-fmc2
+- reg: NAND flash controller memory areas.
+       First region contains the register location.
+       Regions 2 to 4 respectively contain the data, command,
+       and address space for CS0.
+       Regions 5 to 7 contain the same areas for CS1.
+- interrupts: The interrupt number
+- pinctrl-0: Standard Pinctrl phandle (see: pinctrl/pinctrl-bindings.txt)
+- clocks: The clock needed by the NAND flash controller
+
+Optional properties:
+- resets: Reference to a reset controller asserting the FMC controller
+- dmas: DMA specifiers (see: dma/stm32-mdma.txt)
+- dma-names: Must be "tx", "rx" and "ecc"
+
+Optional children nodes:
+Children nodes represent the available NAND chips.
+
+Optional properties:
+- nand-on-flash-bbt: see nand.txt
+- nand-ecc-strength: see nand.txt
+- nand-ecc-step-size: see nand.txt
+
+The following ECC strength and step size are currently supported:
+ - nand-ecc-strength = <1>, nand-ecc-step-size = <512> (Hamming)
+ - nand-ecc-strength = <4>, nand-ecc-step-size = <512> (BCH4)
+ - nand-ecc-strength = <8>, nand-ecc-step-size = <512> (BCH8) (default)
+
+Example:
+
+	fmc: nand-controller@58002000 {
+		compatible = "st,stm32mp15-fmc2";
+		reg = <0x58002000 0x1000>,
+		      <0x80000000 0x1000>,
+		      <0x88010000 0x1000>,
+		      <0x88020000 0x1000>,
+		      <0x81000000 0x1000>,
+		      <0x89010000 0x1000>,
+		      <0x89020000 0x1000>;
+		interrupts = <GIC_SPI 48 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&rcc FMC_K>;
+		resets = <&rcc FMC_R>;
+		pinctrl-names = "default";
+		pinctrl-0 = <&fmc_pins_a>;
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		nand@0 {
+			reg = <0>;
+			nand-on-flash-bbt;
+			#address-cells = <1>;
+			#size-cells = <1>;
+		};
+	};
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,
 };
diff --git a/include/configs/stm32mp1.h b/include/configs/stm32mp1.h
index 701298cab8..737dfd6a5c 100644
--- a/include/configs/stm32mp1.h
+++ b/include/configs/stm32mp1.h
@@ -10,17 +10,17 @@
 #include <linux/sizes.h>
 #include <asm/arch/stm32.h>
 
-#define CONFIG_PREBOOT
-
 /*
  * Number of clock ticks in 1 sec
  */
 #define CONFIG_SYS_HZ				1000
 
+#ifndef CONFIG_STM32MP1_TRUSTED
 /* PSCI support */
 #define CONFIG_ARMV7_PSCI_1_0
 #define CONFIG_ARMV7_SECURE_BASE		STM32_SYSRAM_BASE
 #define CONFIG_ARMV7_SECURE_MAX_SIZE		STM32_SYSRAM_SIZE
+#endif
 
 /*
  * malloc() pool size
@@ -53,6 +53,9 @@
 #define CONFIG_SETUP_MEMORY_TAGS
 #define CONFIG_INITRD_TAG
 
+/* Extend size of kernel image for uncompression */
+#define CONFIG_SYS_BOOTM_LEN			SZ_32M
+
 /* SPL support */
 #ifdef CONFIG_SPL
 /* BOOTROM load address */
@@ -69,36 +72,65 @@
 					 STM32_SYSRAM_SIZE)
 #endif /* #ifdef CONFIG_SPL */
 
+#define CONFIG_SYS_MEMTEST_START	STM32_DDR_BASE
+#define CONFIG_SYS_MEMTEST_END		(CONFIG_SYS_MEMTEST_START + SZ_64M)
+#define CONFIG_SYS_MEMTEST_SCRATCH	(CONFIG_SYS_MEMTEST_END + 4)
+
 /*MMC SD*/
 #define CONFIG_SYS_MMC_MAX_DEVICE	3
 #define CONFIG_SUPPORT_EMMC_BOOT
 
-#if !defined(CONFIG_SPL) || !defined(CONFIG_SPL_BUILD)
+/*****************************************************************************/
+#ifdef CONFIG_DISTRO_DEFAULTS
+/*****************************************************************************/
+
+#if !defined(CONFIG_SPL_BUILD)
 
 #define BOOT_TARGET_DEVICES(func) \
 	func(MMC, mmc, 1) \
 	func(MMC, mmc, 0) \
 	func(MMC, mmc, 2)
+/*
+ * bootcmd for stm32mp1:
+ * for serial/usb: execute the stm32prog command
+ * for mmc boot (eMMC, SD card), boot only on the same device
+ * for nand boot, boot with on ubifs partition on nand
+ * for nor boot, use the default order
+ */
+#define CONFIG_PREBOOT
 
-#include <config_distro_bootcmd.h>
+#define STM32MP_BOOTCMD "bootcmd_stm32mp=" \
+	"echo \"Boot over ${boot_device}${boot_instance}!\";" \
+	"if test ${boot_device} = serial || test ${boot_device} = usb;" \
+	"then stm32prog ${boot_device} ${boot_instance}; " \
+	"else " \
+		"if test ${boot_device} = mmc;" \
+		"then env set boot_targets \"mmc${boot_instance}\"; fi;" \
+		"if test ${boot_device} = nand;" \
+		"then env set boot_targets ubifs0; fi;" \
+		"run distro_bootcmd;" \
+	"fi;\0"
 
-#define STM32MP_PREBOOT	\
-	"echo \"Boot over ${boot_device}${boot_instance}!\"; " \
-	"if test \"${boot_device}\" = \"mmc\"; then " \
-		"env set boot_targets \"mmc${boot_instance}\"; "\
-	"fi;"
+#include <config_distro_bootcmd.h>
 
+/*
+ * memory layout for 32M uncompressed/compressed kernel,
+ * 1M fdt, 1M script, 1M pxe and 1M for splashimage
+ * and the ramdisk at the end.
+ */
 #define CONFIG_EXTRA_ENV_SETTINGS \
-	"scriptaddr=0xC0000000\0" \
-	"pxefile_addr_r=0xC0000000\0" \
-	"kernel_addr_r=0xC1000000\0" \
-	"fdt_addr_r=0xC4000000\0" \
-	"ramdisk_addr_r=0xC4100000\0" \
+	"kernel_addr_r=0xc2000000\0" \
+	"fdt_addr_r=0xc4000000\0" \
+	"scriptaddr=0xc4100000\0" \
+	"pxefile_addr_r=0xc4200000\0" \
+	"splashimage=0xc4300000\0"  \
+	"ramdisk_addr_r=0xc4400000\0" \
 	"fdt_high=0xffffffff\0" \
 	"initrd_high=0xffffffff\0" \
-	"preboot=" STM32MP_PREBOOT "\0" \
+	STM32MP_BOOTCMD \
 	BOOTENV
 
 #endif /* ifndef CONFIG_SPL_BUILD */
+#endif /* ifdef CONFIG_DISTRO_DEFAULTS*/
 
 #endif /* __CONFIG_H */
diff --git a/include/dt-bindings/mfd/st,stpmic1.h b/include/dt-bindings/mfd/st,stpmic1.h
new file mode 100644
index 0000000000..b2d6c83462
--- /dev/null
+++ b/include/dt-bindings/mfd/st,stpmic1.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) STMicroelectronics 2018 - All Rights Reserved
+ * Author: Philippe Peurichard <philippe.peurichard@st.com>,
+ * Pascal Paillet <p.paillet@st.com> for STMicroelectronics.
+ */
+
+#ifndef __DT_BINDINGS_STPMIC1_H__
+#define __DT_BINDINGS_STPMIC1_H__
+
+/* IRQ definitions */
+#define IT_PONKEY_F	0
+#define IT_PONKEY_R	1
+#define IT_WAKEUP_F	2
+#define IT_WAKEUP_R	3
+#define IT_VBUS_OTG_F	4
+#define IT_VBUS_OTG_R	5
+#define IT_SWOUT_F	6
+#define IT_SWOUT_R	7
+
+#define IT_CURLIM_BUCK1	8
+#define IT_CURLIM_BUCK2	9
+#define IT_CURLIM_BUCK3	10
+#define IT_CURLIM_BUCK4	11
+#define IT_OCP_OTG	12
+#define IT_OCP_SWOUT	13
+#define IT_OCP_BOOST	14
+#define IT_OVP_BOOST	15
+
+#define IT_CURLIM_LDO1	16
+#define IT_CURLIM_LDO2	17
+#define IT_CURLIM_LDO3	18
+#define IT_CURLIM_LDO4	19
+#define IT_CURLIM_LDO5	20
+#define IT_CURLIM_LDO6	21
+#define IT_SHORT_SWOTG	22
+#define IT_SHORT_SWOUT	23
+
+#define IT_TWARN_F	24
+#define IT_TWARN_R	25
+#define IT_VINLOW_F	26
+#define IT_VINLOW_R	27
+#define IT_SWIN_F	30
+#define IT_SWIN_R	31
+
+#endif /* __DT_BINDINGS_STPMIC1_H__ */
diff --git a/include/dt-bindings/mfd/st,stpmu1.h b/include/dt-bindings/mfd/st,stpmu1.h
deleted file mode 100644
index 81982ebe2c..0000000000
--- a/include/dt-bindings/mfd/st,stpmu1.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/*
- * This file is part of stpmu1 pmic driver
- *
- * Copyright (C) 2017, STMicroelectronics - All Rights Reserved
- * Author: Pascal Paillet <p.paillet@st.com> for STMicroelectronics.
- *
- * License type: GPLv2
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License version 2 as published by
- * the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
- * or FITNESS FOR A PARTICULAR PURPOSE.
- * See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __DT_BINDINGS_STPMU1_H__
-#define __DT_BINDINGS_STPMU1_H__
-
-/* IRQ definitions */
-#define IT_PONKEY_F 0
-#define IT_PONKEY_R 1
-#define IT_WAKEUP_F 2
-#define IT_WAKEUP_R 3
-#define IT_VBUS_OTG_F 4
-#define IT_VBUS_OTG_R 5
-#define IT_SWOUT_F 6
-#define IT_SWOUT_R 7
-
-#define IT_CURLIM_BUCK1 8
-#define IT_CURLIM_BUCK2 9
-#define IT_CURLIM_BUCK3 10
-#define IT_CURLIM_BUCK4 11
-#define IT_OCP_OTG 12
-#define IT_OCP_SWOUT 13
-#define IT_OCP_BOOST 14
-#define IT_OVP_BOOST 15
-
-#define IT_CURLIM_LDO1 16
-#define IT_CURLIM_LDO2 17
-#define IT_CURLIM_LDO3 18
-#define IT_CURLIM_LDO4 19
-#define IT_CURLIM_LDO5 20
-#define IT_CURLIM_LDO6 21
-#define IT_SHORT_SWOTG 22
-#define IT_SHORT_SWOUT 23
-
-#define IT_TWARN_F 24
-#define IT_TWARN_R 25
-#define IT_VINLOW_F 26
-#define IT_VINLOW_R 27
-#define IT_SWIN_F 30
-#define IT_SWIN_R 31
-
-#endif /* __DT_BINDINGS_STPMU1_H__ */
diff --git a/include/power/stpmic1.h b/include/power/stpmic1.h
new file mode 100644
index 0000000000..0e6721d852
--- /dev/null
+++ b/include/power/stpmic1.h
@@ -0,0 +1,117 @@
+/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef __PMIC_STPMIC1_H_
+#define __PMIC_STPMIC1_H_
+
+#define STPMIC1_MAIN_CR			0x10
+#define STPMIC1_BUCKS_MRST_CR		0x18
+#define STPMIC1_LDOS_MRST_CR		0x1a
+#define STPMIC1_BUCKX_MAIN_CR(buck)	(0x20 + (buck))
+#define STPMIC1_REFDDR_MAIN_CR		0x24
+#define STPMIC1_LDOX_MAIN_CR(ldo)	(0x25 + (ldo))
+#define STPMIC1_BST_SW_CR		0x40
+#define STPMIC1_NVM_SR			0xb8
+#define STPMIC1_NVM_CR			0xb9
+
+/* Main PMIC Control Register (MAIN_CR) */
+#define STPMIC1_SWOFF			BIT(0)
+#define STPMIC1_RREQ_EN			BIT(1)
+
+/* BUCKS_MRST_CR */
+#define STPMIC1_MRST_BUCK(buck)		BIT(buck)
+#define STPMIC1_MRST_BUCK_ALL		GENMASK(3, 0)
+
+/* LDOS_MRST_CR */
+#define STPMIC1_MRST_LDO(ldo)		BIT(ldo)
+#define STPMIC1_MRST_LDO_ALL		GENMASK(6, 0)
+
+/* BUCKx_MAIN_CR (x=1...4) */
+#define STPMIC1_BUCK_ENA		BIT(0)
+#define STPMIC1_BUCK_PREG_MODE		BIT(1)
+#define STPMIC1_BUCK_VOUT_MASK		GENMASK(7, 2)
+#define STPMIC1_BUCK_VOUT_SHIFT		2
+#define STPMIC1_BUCK_VOUT(sel)		(sel << STPMIC1_BUCK_VOUT_SHIFT)
+
+#define STPMIC1_BUCK2_1200000V		STPMIC1_BUCK_VOUT(24)
+#define STPMIC1_BUCK2_1350000V		STPMIC1_BUCK_VOUT(30)
+
+#define STPMIC1_BUCK3_1800000V		STPMIC1_BUCK_VOUT(39)
+
+/* REFDDR_MAIN_CR */
+#define STPMIC1_VREF_ENA		BIT(0)
+
+/* LDOX_MAIN_CR */
+#define STPMIC1_LDO_ENA			BIT(0)
+#define STPMIC1_LDO12356_VOUT_MASK	GENMASK(6, 2)
+#define STPMIC1_LDO12356_VOUT_SHIFT	2
+#define STPMIC1_LDO_VOUT(sel)		(sel << STPMIC1_LDO12356_VOUT_SHIFT)
+
+#define STPMIC1_LDO3_MODE		BIT(7)
+#define STPMIC1_LDO3_DDR_SEL		31
+#define STPMIC1_LDO3_1800000		STPMIC1_LDO_VOUT(9)
+
+#define STPMIC1_LDO4_UV			3300000
+
+/* BST_SW_CR */
+#define STPMIC1_BST_ON			BIT(0)
+#define STPMIC1_VBUSOTG_ON		BIT(1)
+#define STPMIC1_SWOUT_ON		BIT(2)
+#define STPMIC1_PWR_SW_ON		(STPMIC1_VBUSOTG_ON | STPMIC1_SWOUT_ON)
+
+/* NVM_SR */
+#define STPMIC1_NVM_BUSY		BIT(0)
+
+/* NVM_CR */
+#define STPMIC1_NVM_CMD_PROGRAM		1
+#define STPMIC1_NVM_CMD_READ		2
+
+/* Timeout */
+#define STPMIC1_DEFAULT_START_UP_DELAY_MS	1
+#define STPMIC1_DEFAULT_STOP_DELAY_MS		5
+#define STPMIC1_USB_BOOST_START_UP_DELAY_MS	10
+
+enum {
+	STPMIC1_BUCK1,
+	STPMIC1_BUCK2,
+	STPMIC1_BUCK3,
+	STPMIC1_BUCK4,
+	STPMIC1_MAX_BUCK,
+};
+
+enum {
+	STPMIC1_PREG_MODE_HP,
+	STPMIC1_PREG_MODE_LP,
+};
+
+enum {
+	STPMIC1_LDO1,
+	STPMIC1_LDO2,
+	STPMIC1_LDO3,
+	STPMIC1_LDO4,
+	STPMIC1_LDO5,
+	STPMIC1_LDO6,
+	STPMIC1_MAX_LDO,
+};
+
+enum {
+	STPMIC1_LDO_MODE_NORMAL,
+	STPMIC1_LDO_MODE_BYPASS,
+	STPMIC1_LDO_MODE_SINK_SOURCE,
+};
+
+enum {
+	STPMIC1_PWR_SW1,
+	STPMIC1_PWR_SW2,
+	STPMIC1_MAX_PWR_SW,
+};
+
+int stpmic1_shadow_read_byte(u8 addr, u8 *buf);
+int stpmic1_shadow_write_byte(u8 addr, u8 *buf);
+int stpmic1_nvm_read_byte(u8 addr, u8 *buf);
+int stpmic1_nvm_write_byte(u8 addr, u8 *buf);
+int stpmic1_nvm_read_all(u8 *buf, int buf_len);
+int stpmic1_nvm_write_all(u8 *buf, int buf_len);
+#endif
diff --git a/include/power/stpmu1.h b/include/power/stpmu1.h
deleted file mode 100644
index 5906fbf832..0000000000
--- a/include/power/stpmu1.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
-/*
- * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
- */
-
-#ifndef __PMIC_STPMU1_H_
-#define __PMIC_STPMU1_H_
-
-#define STPMU1_MASK_RESET_BUCK		0x18
-#define STPMU1_BUCKX_CTRL_REG(buck)	(0x20 + (buck))
-#define STPMU1_VREF_CTRL_REG		0x24
-#define STPMU1_LDOX_CTRL_REG(ldo)	(0x25 + (ldo))
-#define STPMU1_USB_CTRL_REG		0x40
-#define STPMU1_NVM_USER_STATUS_REG	0xb8
-#define STPMU1_NVM_USER_CONTROL_REG	0xb9
-
-#define STPMU1_MASK_RESET_BUCK3		BIT(2)
-
-#define STPMU1_BUCK_EN			BIT(0)
-#define STPMU1_BUCK_MODE		BIT(1)
-#define STPMU1_BUCK_OUTPUT_MASK		GENMASK(7, 2)
-#define STPMU1_BUCK_OUTPUT_SHIFT	2
-#define STPMU1_BUCK2_1200000V		(24 << STPMU1_BUCK_OUTPUT_SHIFT)
-#define STPMU1_BUCK2_1350000V		(30 << STPMU1_BUCK_OUTPUT_SHIFT)
-#define STPMU1_BUCK3_1800000V		(39 << STPMU1_BUCK_OUTPUT_SHIFT)
-
-#define STPMU1_VREF_EN			BIT(0)
-
-#define STPMU1_LDO_EN			BIT(0)
-#define STPMU1_LDO12356_OUTPUT_MASK	GENMASK(6, 2)
-#define STPMU1_LDO12356_OUTPUT_SHIFT	2
-#define STPMU1_LDO3_MODE		BIT(7)
-#define STPMU1_LDO3_DDR_SEL		31
-#define STPMU1_LDO3_1800000		(9 << STPMU1_LDO12356_OUTPUT_SHIFT)
-#define STPMU1_LDO4_UV			3300000
-
-#define STPMU1_USB_BOOST_EN		BIT(0)
-#define STPMU1_USB_PWR_SW_EN		GENMASK(2, 1)
-
-#define STPMU1_NVM_USER_CONTROL_PROGRAM	BIT(0)
-#define STPMU1_NVM_USER_CONTROL_READ	BIT(1)
-
-#define STPMU1_NVM_USER_STATUS_BUSY	BIT(0)
-#define STPMU1_NVM_USER_STATUS_ERROR	BIT(1)
-
-#define STPMU1_DEFAULT_START_UP_DELAY_MS	1
-#define STPMU1_DEFAULT_STOP_DELAY_MS		5
-#define STPMU1_USB_BOOST_START_UP_DELAY_MS	10
-
-enum {
-	STPMU1_BUCK1,
-	STPMU1_BUCK2,
-	STPMU1_BUCK3,
-	STPMU1_BUCK4,
-	STPMU1_MAX_BUCK,
-};
-
-enum {
-	STPMU1_BUCK_MODE_HP,
-	STPMU1_BUCK_MODE_LP,
-};
-
-enum {
-	STPMU1_LDO1,
-	STPMU1_LDO2,
-	STPMU1_LDO3,
-	STPMU1_LDO4,
-	STPMU1_LDO5,
-	STPMU1_LDO6,
-	STPMU1_MAX_LDO,
-};
-
-enum {
-	STPMU1_LDO_MODE_NORMAL,
-	STPMU1_LDO_MODE_BYPASS,
-	STPMU1_LDO_MODE_SINK_SOURCE,
-};
-
-enum {
-	STPMU1_PWR_SW1,
-	STPMU1_PWR_SW2,
-	STPMU1_MAX_PWR_SW,
-};
-
-#endif
author	Tom Rini <trini@konsulko.com>	2019-04-12 15:43:19 -0400
committer	Tom Rini <trini@konsulko.com>	2019-04-12 15:43:19 -0400
commit	015289580f81733f7358227743e3e5881653a797 (patch)
tree	beca1eea1bfa7b4e4740b9e7a08493711b7b8bee
parent	dd758c6720266c9b7bcd8d0da91a64a2fa1e181a (diff)
parent	7bb75023a720432a32840c6df543aae92653b23d (diff)