From 5851da1728ac705c8fa9079c68c883a73a32fae8 Mon Sep 17 00:00:00 2001
From: christophe montaud <christophe.montaud@st.com>
Date: Fri, 4 Jan 2019 15:02:23 +0100
Subject: [PATCH 1/5] ARM v2018.11 stm32mp r1 MACHINE

---
 arch/arm/Kconfig                                   |   10 +-
 arch/arm/cpu/armv7/arch_timer.c                    |    3 +
 arch/arm/include/asm/arch-stm32/gpio.h             |    5 +
 arch/arm/mach-stm32mp/Kconfig                      |  108 +-
 arch/arm/mach-stm32mp/Makefile                     |    8 +-
 arch/arm/mach-stm32mp/bsec.c                       |   64 +-
 arch/arm/mach-stm32mp/cmd_poweroff.c               |   24 +
 arch/arm/mach-stm32mp/cmd_stm32key.c               |  100 ++
 arch/arm/mach-stm32mp/cmd_stm32prog/Makefile       |    9 +
 .../arm/mach-stm32mp/cmd_stm32prog/cmd_stm32prog.c |   80 +
 arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.c    | 1647 ++++++++++++++++++++
 arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.h    |  202 +++
 .../mach-stm32mp/cmd_stm32prog/stm32prog_serial.c  |  972 ++++++++++++
 .../arm/mach-stm32mp/cmd_stm32prog/stm32prog_usb.c |  278 ++++
 arch/arm/mach-stm32mp/config.mk                    |   16 +-
 arch/arm/mach-stm32mp/cpu.c                        |  236 ++-
 arch/arm/mach-stm32mp/include/mach/ddr.h           |    9 +-
 arch/arm/mach-stm32mp/include/mach/gpio.h          |    6 +
 arch/arm/mach-stm32mp/include/mach/stm32.h         |   37 +-
 arch/arm/mach-stm32mp/include/mach/stm32mp1_smc.h  |   75 +
 arch/arm/mach-stm32mp/include/mach/sys_proto.h     |   10 +-
 arch/arm/mach-stm32mp/psci.c                       |    8 +-
 arch/arm/mach-stm32mp/pwr_regulator.c              |    8 +
 arch/arm/mach-stm32mp/spl.c                        |   47 +-
 arch/arm/mach-stm32mp/stm32-etzpc.c                |  199 +++
 arch/arm/mach-stm32mp/syscon.c                     |    9 +-
 26 files changed, 4103 insertions(+), 67 deletions(-)
 create mode 100644 arch/arm/mach-stm32mp/cmd_poweroff.c
 create mode 100644 arch/arm/mach-stm32mp/cmd_stm32key.c
 create mode 100644 arch/arm/mach-stm32mp/cmd_stm32prog/Makefile
 create mode 100644 arch/arm/mach-stm32mp/cmd_stm32prog/cmd_stm32prog.c
 create mode 100644 arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.c
 create mode 100644 arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.h
 create mode 100644 arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog_serial.c
 create mode 100644 arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog_usb.c
 create mode 100644 arch/arm/mach-stm32mp/include/mach/stm32mp1_smc.h
 create mode 100644 arch/arm/mach-stm32mp/stm32-etzpc.c

diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 1f3fa15..e9eac9d 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -1313,21 +1313,27 @@ config ARCH_STM32MP
 	select DM_GPIO
 	select DM_RESET
 	select DM_SERIAL
+	select ENV_VARS_UBOOT_RUNTIME_CONFIG
 	select MISC
 	select OF_CONTROL
 	select OF_LIBFDT
+	imply OF_LIBFDT_OVERLAY
+	select OF_SYSTEM_SETUP
 	select PINCTRL
 	select REGMAP
 	select SUPPORT_SPL
 	select SYSCON
 	select SYSRESET
+	select SYS_ARCH_TIMER
 	select SYS_THUMB_BUILD
 	imply CMD_DM
+	imply CMD_POWEROFF
 	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).
+	  FBSL 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/cpu/armv7/arch_timer.c b/arch/arm/cpu/armv7/arch_timer.c
index 3db31c0..5de6305 100644
--- a/arch/arm/cpu/armv7/arch_timer.c
+++ b/arch/arm/cpu/armv7/arch_timer.c
@@ -49,6 +49,9 @@ unsigned long long get_ticks(void)
 
 ulong timer_get_boot_us(void)
 {
+	if (!gd->arch.timer_rate_hz)
+		timer_init();
+
 	return lldiv(get_ticks(), gd->arch.timer_rate_hz / 1000000);
 }
 
diff --git a/arch/arm/include/asm/arch-stm32/gpio.h b/arch/arm/include/asm/arch-stm32/gpio.h
index 84859b1..570e80a 100644
--- a/arch/arm/include/asm/arch-stm32/gpio.h
+++ b/arch/arm/include/asm/arch-stm32/gpio.h
@@ -7,6 +7,8 @@
 #ifndef _GPIO_H_
 #define _GPIO_H_
 
+#define STM32_GPIOS_PER_BANK		16
+
 enum stm32_gpio_port {
 	STM32_GPIO_PORT_A = 0,
 	STM32_GPIO_PORT_B,
@@ -109,6 +111,9 @@ struct stm32_gpio_regs {
 
 struct stm32_gpio_priv {
 	struct stm32_gpio_regs *regs;
+	unsigned int gpio_range;
 };
 
+int stm32_offset_to_index(struct udevice *dev, unsigned int offset);
+
 #endif /* _GPIO_H_ */
diff --git a/arch/arm/mach-stm32mp/Kconfig b/arch/arm/mach-stm32mp/Kconfig
index 8a929fa..0c68c24 100644
--- a/arch/arm/mach-stm32mp/Kconfig
+++ b/arch/arm/mach-stm32mp/Kconfig
@@ -16,8 +16,12 @@ config SPL
 	select SPL_REGMAP
 	select SPL_DM_RESET
 	select SPL_SERIAL_SUPPORT
+	select SPL_SPI_LOAD
 	select SPL_SYSCON
-	select SPL_DRIVERS_MISC_SUPPORT
+	select SPL_WATCHDOG_SUPPORT
+	imply BOOTSTAGE_STASH if SPL_BOOTSTAGE
+	imply SPL_BOOTSTAGE
+	imply SPL_DISPLAY_PRINT
 	imply SPL_LIBDISK_SUPPORT
 
 config SYS_SOC
@@ -25,18 +29,87 @@ 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 OF_BOARD_SETUP
 	select PINCTRL_STM32
 	select STM32_RCC
 	select STM32_RESET
-	select SYS_ARCH_TIMER
-	select SYSRESET_SYSCON
+	select STM32_SERIAL
+	imply BOOTCOUNT_LIMIT
+	imply SYSRESET_PSCI if STM32MP1_TRUSTED
+	imply SYSRESET_SYSCON if !STM32MP1_TRUSTED
 	help
 		target STMicroelectronics SOC STM32MP1 family
+		STM32MP153 or STM32MP151
 		STMicroelectronics MPU with core ARMv7
+		dual core A7 for STM32MP153, 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 provide ST smc to manage secure devices
+
+config STM32MP1_OPTEE
+	bool "Support trusted boot with TF-A and OPTEE"
+	depends on STM32MP1_TRUSTED
+	default n
+	help
+		Say Y here to enable boot with TF-A and OPTEE
+		Trusted boot chain is :
+		BootRom => TF-A.stm32 (clock & DDR) => OPTEE => U-Boot.stm32
+		OPTEE monitor provide ST smc to manage secure devices
+
+config SYS_MMCSD_RAW_MODE_U_BOOT_PARTITION_MMC2
+	hex "Partition to use for MMC2 to load U-Boot from"
+	depends on SYS_MMCSD_RAW_MODE_U_BOOT_USE_PARTITION && TARGET_STM32MP1
+	default 1
+	help
+	  Partition on the MMC2 to load U-Boot from when the MMC2 is being
+	  used in raw mode
+
+config STM32_ETZPC
+	bool "STM32 Extended TrustZone Protection"
+	depends on TARGET_STM32MP1
+	default y
+	help
+	  Say y to enable STM32 Extended TrustZone Protection
+	  Controller (ETZPC)
+
+source "board/st/stm32mp1/Kconfig"
+
+config CMD_STM32PROG
+	bool "command stm32prog for STM32CudeProgrammer"
+	default y
+	depends on CMD_DFU
+	imply CMD_GPT if MMC
+	imply DFU_MMC if MMC
+	imply DFU_NAND if NAND
+	select DFU_RAM
+	imply DFU_SF if DM_SPI_FLASH
+	select DFU_VIRT
+	select PARTITION_TYPE_GUID
+	help
+		activate a specific command stm32prog for STM32MP soc family
+		witch update the device with the tools STM32CubeProgrammer,
+		using UART with STM32 protocol or USB with DFU protocol
+		NB: access to not volatile memory (NOR/NAND/SD/eMMC) is based
+		    on U-Boot DFU framework
+
+config CMD_STM32KEY
+	bool "command stm32key to fuse public key hash"
+	default y
+	depends on CMD_FUSE
+	help
+		fuse public key hash in corresponding fuse used to authenticate
+		binary.
 
 config SYS_TEXT_BASE
 	prompt "U-Boot base address"
@@ -46,22 +119,23 @@ config SYS_TEXT_BASE
 		when DDR driver is used:
 		  DDR + 1MB (0xC0100000)
 
-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
+config NR_DRAM_BANKS
 	default 1
-	help
-	  Partition on the second MMC to load U-Boot from when the MMC is being
-	  used in raw mode
 
-source "board/st/stm32mp1/Kconfig"
+config BOOTSTAGE_STASH_ADDR
+	default 0xC3000000
 
-# currently activated for debug / should be deactivated for real product
-if DEBUG_UART
 
-config DEBUG_UART_BOARD_INIT
+if BOOTCOUNT_LIMIT
+config SYS_BOOTCOUNT_SINGLEWORD
 	default y
 
+# TAMP_BOOTCOUNT = TAMP_BACKUP_REGISTER(21)
+config SYS_BOOTCOUNT_ADDR
+	default 0x5C00A154
+endif
+
+if DEBUG_UART
 # debug on UART4 by default
 config DEBUG_UART_BASE
 	default 0x40010000
@@ -69,6 +143,10 @@ config DEBUG_UART_BASE
 # clock source is HSI on reset
 config DEBUG_UART_CLOCK
 	default 64000000
+
+# currently activated for debug / should be deactivated for real product
+config DEBUG_UART_BOARD_INIT
+	default y
 endif
 
 endif
diff --git a/arch/arm/mach-stm32mp/Makefile b/arch/arm/mach-stm32mp/Makefile
index f59ced5..09636db 100644
--- a/arch/arm/mach-stm32mp/Makefile
+++ b/arch/arm/mach-stm32mp/Makefile
@@ -10,7 +10,13 @@ obj-y += syscon.o
 ifdef CONFIG_SPL_BUILD
 obj-y += spl.o
 else
+obj-$(CONFIG_CMD_STM32PROG) += cmd_stm32prog/
+obj-$(CONFIG_CMD_STM32KEY) += cmd_stm32key.o
+
+obj-$(CONFIG_ARMV7_PSCI) += psci.o
 obj-y += bsec.o
+obj-y += cmd_poweroff.o
 endif
-obj-$(CONFIG_ARMV7_PSCI) += psci.o
+
 obj-$(CONFIG_$(SPL_)DM_REGULATOR) += pwr_regulator.o
+obj-$(CONFIG_STM32_ETZPC) += stm32-etzpc.o
diff --git a/arch/arm/mach-stm32mp/bsec.c b/arch/arm/mach-stm32mp/bsec.c
index 0e152ef..913dbef 100644
--- a/arch/arm/mach-stm32mp/bsec.c
+++ b/arch/arm/mach-stm32mp/bsec.c
@@ -1,4 +1,4 @@
-// SPDX-License-Identifier: GPL-2.0+
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
 /*
  * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
  */
@@ -7,10 +7,14 @@
 #include <dm.h>
 #include <misc.h>
 #include <asm/io.h>
+#include <asm/arch/stm32mp1_smc.h>
+#include <linux/arm-smccc.h>
 #include <linux/iopoll.h>
 
 #define BSEC_OTP_MAX_VALUE		95
 
+#ifndef CONFIG_STM32MP1_TRUSTED
+
 #define BSEC_TIMEOUT_US			10000
 
 /* BSEC REGISTER OFFSET (base relative) */
@@ -168,7 +172,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 +274,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 +283,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 +309,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 +434,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,15 +461,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,
-	.flags  = DM_FLAG_PRE_RELOC,
-};
-
-/* 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 0000000..a6fdc79
--- /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) 2018, 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/cmd_stm32key.c b/arch/arm/mach-stm32mp/cmd_stm32key.c
new file mode 100644
index 0000000..4245e6f
--- /dev/null
+++ b/arch/arm/mach-stm32mp/cmd_stm32key.c
@@ -0,0 +1,100 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <command.h>
+#include <console.h>
+#include <misc.h>
+#include <dm/device.h>
+#include <dm/uclass.h>
+
+#define STM32_OTP_HASH_KEY_START 24
+#define STM32_OTP_HASH_KEY_SIZE 8
+
+static void read_hash_value(u32 addr)
+{
+	int i;
+
+	for (i = 0; i < STM32_OTP_HASH_KEY_SIZE; i++) {
+		printf("OTP value %i: %x\n", STM32_OTP_HASH_KEY_START + i,
+		       __be32_to_cpu(*(u32 *)addr));
+		addr += 4;
+	}
+}
+
+static void fuse_hash_value(u32 addr, bool print)
+{
+	struct udevice *dev;
+	u32 word, val;
+	int i, 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;
+	}
+
+	for (i = 0; i < STM32_OTP_HASH_KEY_SIZE; i++) {
+		if (print)
+			printf("Fuse OTP %i : %x\n",
+			       STM32_OTP_HASH_KEY_START + i,
+			       __be32_to_cpu(*(u32 *)addr));
+
+		word = STM32_OTP_HASH_KEY_START + i;
+		val = __be32_to_cpu(*(u32 *)addr);
+		misc_write(dev, STM32_BSEC_OTP(word), &val, 4);
+
+		addr += 4;
+	}
+}
+
+static int confirm_prog(void)
+{
+	puts("Warning: Programming fuses is an irreversible operation!\n"
+			"         This may brick your system.\n"
+			"         Use this command only if you are sure of what you are doing!\n"
+			"\nReally perform this fuse programming? <y/N>\n");
+
+	if (confirm_yesno())
+		return 1;
+
+	puts("Fuse programming aborted\n");
+	return 0;
+}
+
+int do_stm32key(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	u32 addr;
+	const char *op = argc >= 2 ? argv[1] : NULL;
+	int confirmed = argc > 3 && !strcmp(argv[2], "-y");
+
+	argc -= 2 + confirmed;
+	argv += 2 + confirmed;
+
+	if (argc < 1)
+		return CMD_RET_USAGE;
+
+	addr = simple_strtoul(argv[0], NULL, 16);
+	if (!addr)
+		return CMD_RET_USAGE;
+
+	if (!strcmp(op, "read"))
+		read_hash_value(addr);
+
+	if (!strcmp(op, "fuse")) {
+		if (!confirmed && !confirm_prog())
+			return CMD_RET_FAILURE;
+		fuse_hash_value(addr, !confirmed);
+	}
+
+	return CMD_RET_SUCCESS;
+}
+
+U_BOOT_CMD(stm32key, 4, 1, do_stm32key,
+	   "Fuse ST Hash key",
+	   "read <addr>: Read the hash store at addr in memory\n"
+	   "stm32key fuse [-y] <addr> : Fuse hash store at addr in otp\n");
diff --git a/arch/arm/mach-stm32mp/cmd_stm32prog/Makefile b/arch/arm/mach-stm32mp/cmd_stm32prog/Makefile
new file mode 100644
index 0000000..bb7b7e7
--- /dev/null
+++ b/arch/arm/mach-stm32mp/cmd_stm32prog/Makefile
@@ -0,0 +1,9 @@
+# SPDX-License-Identifier: GPL-2.0+
+#
+# Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+#
+
+obj-y += cmd_stm32prog.o
+obj-y += stm32prog.o
+obj-y += stm32prog_serial.o
+obj-y += stm32prog_usb.o
diff --git a/arch/arm/mach-stm32mp/cmd_stm32prog/cmd_stm32prog.c b/arch/arm/mach-stm32mp/cmd_stm32prog/cmd_stm32prog.c
new file mode 100644
index 0000000..d1c07dc
--- /dev/null
+++ b/arch/arm/mach-stm32mp/cmd_stm32prog/cmd_stm32prog.c
@@ -0,0 +1,80 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <command.h>
+#include <dfu.h>
+#include "stm32prog.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static int do_stm32prog(cmd_tbl_t *cmdtp, int flag, int argc,
+			char * const argv[])
+{
+	struct stm32prog_data *data;
+	ulong	addr, size;
+	int dev;
+	enum stm32prog_link_t link = LINK_UNDEFINED;
+	bool reset = false;
+
+	if (argc < 3 ||  argc > 5)
+		return CMD_RET_USAGE;
+
+	if (!strcmp(argv[1], "serial")) {
+		link = LINK_SERIAL;
+	} else {
+		if (!strcmp(argv[1], "usb")) {
+			link = LINK_USB;
+		} else {
+			pr_err("not supported link=%s\n", argv[1]);
+			return CMD_RET_USAGE;
+		}
+	}
+	dev = (int)simple_strtoul(argv[2], NULL, 10);
+
+	addr = STM32_DDR_BASE;
+	size = 0;
+	if (argc > 3) {
+		addr = simple_strtoul(argv[3], NULL, 16);
+		if (!addr)
+			return CMD_RET_FAILURE;
+	}
+	if (argc > 4)
+		size = simple_strtoul(argv[4], NULL, 16);
+
+	data = stm32prog_init(link, dev, addr, size);
+	if (!data)
+		return CMD_RET_FAILURE;
+
+	switch (link) {
+	case LINK_SERIAL:
+		reset = stm32prog_serial_loop(data);
+		break;
+	case LINK_USB:
+		reset = stm32prog_usb_loop(data, dev);
+		break;
+	default:
+		break;
+	}
+
+	stm32prog_clean(data);
+
+	puts("Download done\n");
+	if (reset) {
+		puts("Reset...\n");
+		run_command("reset", 0);
+	}
+
+	return CMD_RET_SUCCESS;
+}
+
+U_BOOT_CMD(stm32prog, 5, 0, do_stm32prog,
+	   "<link> <dev> [<addr>] [<size>]\n"
+	   "start communication with tools STM32Cubeprogrammer on <link> with Flashlayout at <addr>",
+	   "<link> = serial|usb\n"
+	   "<dev>  = device instance\n"
+	   "<addr> = address of flashlayout\n"
+	   "<size> = size of flashlayout\n"
+);
diff --git a/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.c b/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.c
new file mode 100644
index 0000000..d263b38
--- /dev/null
+++ b/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.c
@@ -0,0 +1,1647 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <console.h>
+#include <dfu.h>
+#include <malloc.h>
+#include <mmc.h>
+#include <nand.h>
+#include <part.h>
+#include <spi.h>
+#include <asm/arch/stm32mp1_smc.h>
+#include <dm/uclass.h>
+#include <jffs2/load_kernel.h>
+#include <linux/libfdt.h>
+#include <linux/list.h>
+#include <linux/list_sort.h>
+#include <linux/mtd/mtd.h>
+#include <linux/sizes.h>
+#include <power/stpmic1.h>
+
+#include "stm32prog.h"
+
+/* Primary GPT header size for 128 entries : 17kB = 34 LBA of 512B */
+#define GPT_HEADER_SZ	34
+
+#define OPT_SELECT	BIT(0)
+#define OPT_EMPTY	BIT(1)
+#define OPT_DELETE	BIT(2)
+
+#define IS_SELECT(part)	(part->option & OPT_SELECT)
+#define IS_EMPTY(part)	(part->option & OPT_EMPTY)
+#define IS_DELETE(part)	(part->option & OPT_DELETE)
+
+#define ROOTFS_MMC0_UUID \
+	EFI_GUID(0xE91C4E10, 0x16E6, 0x4C0E, \
+		 0xBD, 0x0E, 0x77, 0xBE, 0xCF, 0x4A, 0x35, 0x82)
+
+#define ROOTFS_MMC1_UUID \
+	EFI_GUID(0x491F6117, 0x415D, 0x4F53, \
+		 0x88, 0xC9, 0x6E, 0x0D, 0xE5, 0x4D, 0xEA, 0xC6)
+
+#define ROOTFS_MMC2_UUID \
+	EFI_GUID(0xFD58F1C7, 0xBE0D, 0x4338, \
+		 0x88, 0xE9, 0xAD, 0x8F, 0x05, 0x0A, 0xEB, 0x18)
+
+/*
+ * unique partition guid (uuid) for partition named "rootfs"
+ * on each MMC instance = SD Card or eMMC
+ * allow fixed kernel bootcmd: "rootf=PARTUID=e91c4e10-..."
+ */
+const static efi_guid_t uuid_mmc[3] = {
+	ROOTFS_MMC0_UUID,
+	ROOTFS_MMC1_UUID,
+	ROOTFS_MMC2_UUID
+};
+
+DECLARE_GLOBAL_DATA_PTR;
+#define ENV_BUF_LEN			SZ_1K
+
+/* order of column in flash layout file */
+enum stm32prog_col_t {
+	COL_OPTION,
+	COL_ID,
+	COL_NAME,
+	COL_TYPE,
+	COL_IP,
+	COL_OFFSET,
+	COL_NB_STM32
+};
+
+/* partition handling routines : CONFIG_CMD_MTDPARTS */
+int mtdparts_init(void);
+int find_dev_and_part(const char *id, struct mtd_device **dev,
+		      u8 *part_num, struct part_info **part);
+
+char *stm32prog_get_error(struct stm32prog_data *data)
+{
+	const char error_msg[] = "Unspecified";
+
+	if (strlen(data->error) == 0)
+		strcpy(data->error, error_msg);
+
+	return data->error;
+}
+
+u8 stm32prog_header_check(struct raw_header_s *raw_header,
+			  struct image_header_s *header)
+{
+	int i;
+
+	header->present = 0;
+	header->image_checksum = 0x0;
+	header->image_length = 0x0;
+
+	/*pr_debug("%s entry\n", __func__);*/
+	if (!raw_header || !header) {
+		pr_debug("%s:no header data\n", __func__);
+		return -1;
+	}
+	if (raw_header->magic_number !=
+		(('S' << 0) | ('T' << 8) | ('M' << 16) | (0x32 << 24))) {
+		pr_debug("%s:invalid magic number : 0x%x\n",
+			 __func__, raw_header->magic_number);
+		return -2;
+	}
+	/* only header v1.0 supported */
+	if (raw_header->header_version != 0x00010000) {
+		pr_debug("%s:invalid header version : 0x%x\n",
+			 __func__, raw_header->header_version);
+		return -3;
+	}
+	if (raw_header->reserved1 != 0x0 || raw_header->reserved2) {
+		pr_debug("%s:invalid reserved field\n", __func__);
+		return -4;
+	}
+	for (i = 0; i < (sizeof(raw_header->padding) / 4); i++) {
+		if (raw_header->padding[i] != 0) {
+			pr_debug("%s:invalid padding field\n", __func__);
+			return -5;
+		}
+	}
+	header->present = 1;
+	header->image_checksum = le32_to_cpu(raw_header->image_checksum);
+	header->image_length = le32_to_cpu(raw_header->image_length);
+
+	/*pr_debug("%s exit\n", __func__);*/
+
+	return 0;
+}
+
+static u32 stm32prog_header_checksum(u32 addr, struct image_header_s *header)
+{
+	u32 i, checksum;
+	u8 *payload;
+
+	/* compute checksum on payload */
+	payload = (u8 *)addr;
+	checksum = 0;
+	for (i = header->image_length; i > 0; i--)
+		checksum += *(payload++);
+
+	return checksum;
+}
+
+/* FLASHLAYOUT PARSING *****************************************/
+static int parse_option(struct stm32prog_data *data,
+			char *p, struct stm32prog_part_t *part)
+{
+	int result = 0;
+	char *c = p;
+
+	part->option = 0;
+	if (!strcmp(p, "-"))
+		return 0;
+
+	while (*c) {
+		switch (*c) {
+		case 'P':
+			part->option |= OPT_SELECT;
+			break;
+		case 'E':
+			part->option |= OPT_EMPTY;
+			break;
+		case 'D':
+			part->option |= OPT_DELETE;
+			break;
+		default:
+			result = -EINVAL;
+			stm32prog_err("Layout: invalid option '%c' in %s)",
+				      *c, p);
+			return -EINVAL;
+		}
+		c++;
+	}
+	if (!(part->option & OPT_SELECT)) {
+		stm32prog_err("Layout: missing 'P' in option %s", p);
+		return -EINVAL;
+	}
+
+	/* pr_debug("option : %x\n", part->option); */
+
+	return result;
+}
+
+static int parse_id(struct stm32prog_data *data,
+		    char *p, struct stm32prog_part_t *part)
+{
+	int result = 0;
+	unsigned long value;
+
+	result = strict_strtoul(p, 0, &value);
+	part->id = value;
+	if (result || value > PHASE_LAST_USER) {
+		stm32prog_err("Layout: invalid phase value = %s", p);
+		result = -EINVAL;
+	}
+	/* pr_debug("phase : %x\n", part->id); */
+
+	return result;
+}
+
+static int parse_name(struct stm32prog_data *data,
+		      char *p, struct stm32prog_part_t *part)
+{
+	int result = 0;
+
+	if (strlen(p) < sizeof(part->name)) {
+		strcpy(part->name, p);
+	} else {
+		stm32prog_err("Layout: partition name too long [%d]  : %s",
+			      strlen(p), p);
+		result = -EINVAL;
+	}
+	/* pr_debug("name : %s\n", part->name); */
+
+	return result;
+}
+
+static int parse_type(struct stm32prog_data *data,
+		      char *p, struct stm32prog_part_t *part)
+{
+	int result = 0;
+	int len = 0;
+
+	part->bin_nb = 0;
+	if (!strncmp(p, "Binary", 6)) {
+		part->part_type = PART_BINARY;
+
+		/* search for Binary(X) case */
+		len = strlen(p);
+		part->bin_nb = 1;
+		if (len > 6) {
+			if (len < 8 ||
+			    (p[6] != '(') ||
+			    (p[len - 1] != ')'))
+				result = -EINVAL;
+			else
+				part->bin_nb =
+					simple_strtoul(&p[7], NULL, 10);
+		}
+	} else if (!strcmp(p, "System")) {
+		part->part_type = PART_SYSTEM;
+	} else if (!strcmp(p, "FileSystem")) {
+		part->part_type = PART_FILESYSTEM;
+	} else if (!strcmp(p, "RawImage")) {
+		part->part_type = RAW_IMAGE;
+	} else {
+		result = -EINVAL;
+	}
+	if (result)
+		stm32prog_err("Layout: type parsing error : '%s'", p);
+	/* pr_debug("type : %d\n", part->part_type); */
+
+	return result;
+}
+
+static int parse_ip(struct stm32prog_data *data,
+		    char *p, struct stm32prog_part_t *part)
+{
+	int result = 0;
+	int len = 0;
+
+	part->dev_id = 0;
+	if (!strcmp(p, "none")) {
+		part->dev_type = DFU_DEV_VIRT;
+	} else if (!strncmp(p, "mmc", 3)) {
+		part->dev_type = DFU_DEV_MMC;
+		len = 3;
+	} else if (!strncmp(p, "nor", 3)) {
+		part->dev_type = DFU_DEV_SF;
+		len = 3;
+	} else if (!strncmp(p, "nand", 4)) {
+		part->dev_type = DFU_DEV_NAND;
+		len = 4;
+	} else {
+		result = -EINVAL;
+	}
+	if (len) {
+		/* only one digit allowed for device id */
+		if (strlen(p) != len + 1) {
+			result = -EINVAL;
+		} else {
+			part->dev_id = p[len] - '0';
+			if (part->dev_id > 9)
+				result = -EINVAL;
+		}
+	}
+	if (result)
+		stm32prog_err("Layout: ip parsing error : '%s'", p);
+	/* pr_debug("dev : %d\n", part->dev_id); */
+
+	return result;
+}
+
+static int parse_offset(struct stm32prog_data *data,
+			char *p, struct stm32prog_part_t *part)
+{
+	int result = 0;
+	char *tail;
+
+	part->part_id = 0;
+	part->addr = 0;
+	part->size = 0;
+	/* eMMC boot parttion */
+	if (!strncmp(p, "boot", 4)) {
+		if (p[4] == '1') {
+			part->part_id = -1;
+		} else if (p[4] == '2') {
+			part->part_id = -2;
+		} else {
+			stm32prog_err("Layout: invalid part '%s'", p);
+			result = -EINVAL;
+		}
+	} else {
+		part->addr = simple_strtoull(p, &tail, 0);
+		if (tail == p || *tail != '\0') {
+			stm32prog_err("Layout: invalid offset '%s'", p);
+			result = -EINVAL;
+		}
+	}
+	/* pr_debug("addr : 0x%llx, part_id : %d\n", part->addr,
+	 *       part->part_id);
+	 */
+
+	return result;
+}
+
+static
+int (* const parse[COL_NB_STM32])(struct stm32prog_data *data, char *p,
+				  struct stm32prog_part_t *part) = {
+	[COL_OPTION] = parse_option,
+	[COL_ID] = parse_id,
+	[COL_NAME] =  parse_name,
+	[COL_TYPE] = parse_type,
+	[COL_IP] = parse_ip,
+	[COL_OFFSET] = parse_offset,
+};
+
+static int parse_flash_layout(struct stm32prog_data *data,
+			      ulong addr,
+			      ulong size)
+{
+	int column = 0, part_nb = 0, ret;
+	bool end_of_line, eof;
+	char *p, *start, *last, *col;
+	struct stm32prog_part_t *part;
+	int part_list_size;
+	bool stm32image = false;
+
+	data->part_nb = 0;
+
+	/* check if STM32image is detected */
+	if (!stm32prog_header_check((struct raw_header_s *)addr,
+				    &data->header)) {
+		u32 checksum;
+
+		addr = addr + BL_HEADER_SIZE;
+		size = data->header.image_length;
+		stm32image = true;
+
+		checksum = stm32prog_header_checksum(addr, &data->header);
+		if (checksum != data->header.image_checksum) {
+			stm32prog_err("Layout: invalid checksum : 0x%x expected 0x%x",
+				      checksum, data->header.image_checksum);
+			return -EIO;
+		}
+	}
+	if (!size)
+		return -EINVAL;
+
+	start = (char *)addr;
+	last = start + size;
+
+	*last = 0x0; /* force null terminated string */
+	pr_debug("flash layout =\n%s\n", start);
+
+	/* calculate expected number of partitions */
+	part_list_size = 1;
+	p = start;
+	while (*p && (p < last)) {
+		if (*p++ == '\n') {
+			part_list_size++;
+			if (p < last && *p == '#')
+				part_list_size--;
+		}
+	}
+	if (part_list_size > PHASE_LAST_USER) {
+		stm32prog_err("Layout: too many line");
+		return -1;
+	}
+	part = calloc(sizeof(struct stm32prog_part_t), part_list_size);
+	if (!part) {
+		stm32prog_err("Layout: alloc failed");
+		return -ENOMEM;
+	}
+	data->part_array = part;
+
+	/* main parsing loop */
+	eof = false;
+	p = start;
+	col = start; /* 1st column */
+	while (!eof) {
+		end_of_line = false;
+		switch (*p) {
+		/* CR is ignored and replaced by NULL chararc*/
+		case '\r':
+			*p = '\0';
+			p++;
+			continue;
+		/* end of column detected */
+		case '\0':
+			end_of_line = true;
+			eof = true;
+			break;
+		case '\n':
+			end_of_line = true;
+			break;
+		case '\t':
+			break;
+		case '#':
+			/* comment line is skipped */
+			if (column == 0 && p == col) {
+				while ((p < last) && *p)
+					if (*p++ == '\n')
+						break;
+				col = p;
+				if (p >= last || !*p)
+					eof = true;
+				continue;
+			}
+			/* no break */
+		/* by default continue with the next character */
+		default:
+			p++;
+			continue;
+		}
+
+		/* replace by \0 to allow string parsing */
+		*p = '\0';
+		p++;
+		if (p >= last) {
+			eof = true;
+			end_of_line = true;
+		}
+		/*pr_debug("%d:%d = '%s' => ", part_nb, column, col);*/
+		if (strlen(col) == 0) {
+			col = p;
+			/* skip empty line */
+			if (column == 0 && end_of_line)
+				continue;
+			/* multiple TAB allowed in tsv file */
+			if (!stm32image)
+				continue;
+			stm32prog_err("empty field for line %d", part_nb);
+			return -1;
+		}
+		if (column < COL_NB_STM32) {
+			ret = parse[column](data, col, part);
+			if (ret)
+				return ret;
+		}
+
+		/* save the beginning of the next column */
+		column++;
+		col = p;
+
+		if (!end_of_line)
+			continue;
+
+		/* end of the line detected */
+		if (column < COL_NB_STM32) {
+			stm32prog_err("Layout: no enought column for line %d",
+				      part_nb);
+			return -EINVAL;
+		}
+		column = 0;
+		part_nb++;
+		part++;
+		if (part_nb >= part_list_size) {
+			part = NULL;
+			if (!eof) {
+				stm32prog_err("Layout: no enought memory for %d part",
+					      part_nb);
+				return -EINVAL;
+			}
+		}
+	}
+	data->part_nb = part_nb;
+	if (data->part_nb == 0) {
+		stm32prog_err("Layout: no partition found");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int __init part_cmp(void *priv, struct list_head *a, struct list_head *b)
+{
+	struct stm32prog_part_t *parta, *partb;
+
+	parta = container_of(a, struct stm32prog_part_t, list);
+	partb = container_of(b, struct stm32prog_part_t, list);
+
+	if (parta->part_id != partb->part_id)
+		return parta->part_id - partb->part_id;
+	else
+		return parta->addr > partb->addr ? 1 : -1;
+}
+
+static int init_device(struct stm32prog_data *data,
+		       struct stm32prog_dev_t *dev)
+{
+	struct mmc *mmc = NULL;
+#ifdef CONFIG_MTD_PARTITIONS
+	struct blk_desc *block_dev = NULL;
+	struct mtd_info *mtd = NULL;
+	char mtd_id[16];
+	char cmdbuf[40];
+#endif
+	int part_id;
+	int ret;
+	u64 first_addr = 0, last_addr = 0;
+	struct stm32prog_part_t *part, *next_part;
+	u64 part_addr, part_size;
+
+	dev->lba_blk_size = MMC_MAX_BLOCK_LEN;
+	switch (dev->dev_type) {
+	case DFU_DEV_MMC:
+		mmc = find_mmc_device(dev->dev_id);
+		if (mmc_init(mmc)) {
+			stm32prog_err("mmc device %d not found", dev->dev_id);
+			return -ENODEV;
+		}
+		block_dev = mmc_get_blk_desc(mmc);
+		if (!block_dev) {
+			stm32prog_err("mmc device %d not probed", dev->dev_id);
+			return -ENODEV;
+		}
+		dev->lba_blk_size = mmc->read_bl_len;
+		dev->erase_size = mmc->erase_grp_size * block_dev->blksz;
+
+		/* reserve a full erase group for each GTP headers */
+		if (mmc->erase_grp_size > GPT_HEADER_SZ) {
+			first_addr = dev->erase_size;
+			last_addr = (u64)(block_dev->lba -
+					  mmc->erase_grp_size) *
+				    block_dev->blksz;
+		} else {
+			first_addr = (u64)GPT_HEADER_SZ * block_dev->blksz;
+			last_addr = (u64)(block_dev->lba - GPT_HEADER_SZ - 1) *
+				    block_dev->blksz;
+		}
+		pr_debug("MMC %d: lba=%ld blksz=%ld\n", dev->dev_id,
+			 block_dev->lba, block_dev->blksz);
+		pr_debug(" available address = 0x%llx..0x%llx\n",
+			 first_addr, last_addr);
+		break;
+#ifdef CONFIG_MTD_PARTITIONS
+	case DFU_DEV_SF:
+#ifdef CONFIG_SPI_FLASH
+		sprintf(cmdbuf, "sf probe %d", dev->dev_id);
+		if (run_command(cmdbuf, 0)) {
+			stm32prog_err("invalid device : %s", cmdbuf);
+			return -ENODEV;
+		}
+		sprintf(mtd_id, "nor%d", dev->dev_id);
+		pr_debug("%s\n", mtd_id);
+		break;
+#else
+		stm32prog_err("device SF nor supported");
+		return -ENODEV;
+#endif
+	case DFU_DEV_NAND:
+		sprintf(cmdbuf, "nand device %d", dev->dev_id);
+		if (run_command(cmdbuf, 0)) {
+			stm32prog_err("invalid device : %s", cmdbuf);
+			return -ENODEV;
+		}
+		sprintf(mtd_id, "nand%d", dev->dev_id);
+		pr_debug("%s\n", mtd_id);
+		break;
+#endif
+	default:
+		stm32prog_err("unknown device type = %d", dev->dev_type);
+		return -ENODEV;
+	}
+#ifdef CONFIG_MTD_PARTITIONS
+	if (dev->dev_type == DFU_DEV_SF ||
+	    dev->dev_type == DFU_DEV_NAND) {
+#ifdef CONFIG_SPI_FLASH
+		if (dev->dev_type == DFU_DEV_NAND) {
+			/* sf probe is needed for mtdparts
+			 * because mtdids can use nor0 and nor driver
+			 * is not probed by default as nand
+			 */
+			sprintf(cmdbuf, "sf probe %d", dev->dev_id);
+			run_command(cmdbuf, 0);
+		}
+#endif
+		mtdparts_init();
+		mtd = get_mtd_device_nm(mtd_id);
+		if (IS_ERR(mtd)) {
+			stm32prog_err("MTD device %s not found", mtd_id);
+			return -ENODEV;
+		}
+		first_addr = 0;
+		last_addr = mtd->size;
+		dev->erase_size = mtd->erasesize;
+		pr_debug("MTD device %s : size=%lld erasesize=%d\n",
+			 mtd_id, mtd->size, mtd->erasesize);
+		pr_debug(" available address = 0x%llx..0x%llx\n",
+			 first_addr, last_addr);
+	}
+	dev->mtd = mtd;
+#endif
+	pr_debug(" erase size = 0x%x\n", dev->erase_size);
+	dev->block_dev = block_dev;
+
+	/* order partition list in offset order */
+	list_sort(NULL, &dev->part_list, &part_cmp);
+	part_id = 1;
+	pr_debug("id : Opt Phase     Name  type.n dev.n addr     size     part_off part_size\n");
+	list_for_each_entry(part, &dev->part_list, list) {
+		if (part->bin_nb > 1) {
+			if (dev->dev_type != DFU_DEV_NAND ||
+			    part->id >= PHASE_FIRST_USER ||
+			    strncmp(part->name, "fsbl", 4)) {
+				stm32prog_err("%s: multiple binary %d not supported for phase %d",
+					      part->name, part->bin_nb,
+					      part->id);
+				return -EINVAL;
+			}
+		}
+		if (part->part_type == RAW_IMAGE) {
+			part->part_id = 0x0;
+			part->addr = 0x0;
+			if (block_dev)
+				part->size = block_dev->lba * block_dev->blksz;
+			else
+				part->size = last_addr;
+			pr_debug("-- : %1d %02x %14s %02d.%d %02d.%02d %08llx %08llx\n",
+				 part->option, part->id, part->name,
+				 part->part_type, part->bin_nb, part->dev_type,
+				 part->dev_id, part->addr, part->size);
+			continue;
+		}
+		if (part->part_id < 0) { /* boot hw partition for eMMC */
+			if (mmc) {
+				part->size = mmc->capacity_boot;
+			} else {
+				stm32prog_err("%s: hw partition not expected : %d",
+					      part->name, part->part_id);
+				return -ENODEV;
+			}
+		} else {
+			part->part_id = part_id++;
+
+			/* last partition : size to the end of the device */
+			if (part->list.next != &dev->part_list) {
+				next_part =
+					container_of(part->list.next,
+						     struct stm32prog_part_t,
+						     list);
+				if (part->addr < next_part->addr) {
+					part->size = next_part->addr -
+						     part->addr;
+				} else {
+					stm32prog_err("%s: invalid address : 0x%llx >= 0x%llx",
+						      part->name, part->addr,
+						      next_part->addr);
+					return -EINVAL;
+				}
+			} else {
+				if (part->addr <= last_addr) {
+					part->size = last_addr - part->addr;
+				} else {
+					stm32prog_err("%s: invalid address 0x%llx (max=0x%llx)",
+						      part->name, part->addr,
+						      last_addr);
+					return -EINVAL;
+				}
+			}
+			if (part->addr < first_addr) {
+				stm32prog_err("%s: invalid address 0x%llx (min=0x%llx)",
+					      part->name, part->addr,
+					      first_addr);
+				return -EINVAL;
+			}
+		}
+		if ((part->addr & ((u64)part->dev->erase_size - 1)) != 0) {
+			stm32prog_err("%s: not aligned address : 0x%llx on erase size 0x%x",
+				      part->name, part->addr,
+				      part->dev->erase_size);
+			return -EINVAL;
+		}
+		pr_debug("%02d : %1d %02x %14s %02d.%d %02d.%02d %08llx %08llx",
+			 part->part_id, part->option, part->id, part->name,
+			 part->part_type, part->bin_nb, part->dev_type,
+			 part->dev_id, part->addr, part->size);
+
+		part_addr = 0;
+		part_size = 0;
+		/* check coherency with existing partition */
+		if (block_dev) {
+			disk_partition_t partinfo;
+
+			/* only check partition size for partial update */
+			if (data->full_update || part->part_id < 0) {
+				pr_debug("\n");
+				continue;
+			}
+
+			ret = part_get_info(block_dev, part->part_id,
+					    &partinfo);
+
+			if (ret) {
+				stm32prog_err("Couldn't find part %d on device mmc %d",
+					      part_id, part->dev_id);
+				return -ENODEV;
+			}
+			part_addr = (u64)partinfo.start * partinfo.blksz;
+			part_size = (u64)partinfo.size * partinfo.blksz;
+		}
+
+#ifdef CONFIG_MTD_PARTITIONS
+		if (mtd) {
+			char mtd_part_id[32];
+			struct part_info *mtd_part;
+			struct mtd_device *mtd_dev;
+			u8 part_num;
+
+			sprintf(mtd_part_id, "%s,%d", mtd_id,
+				part->part_id - 1);
+			ret = find_dev_and_part(mtd_part_id, &mtd_dev,
+						&part_num, &mtd_part);
+			if (ret != 0) {
+				stm32prog_err("Invalid partition %s",
+					      mtd_part_id);
+				return -ENODEV;
+			}
+			part_addr = mtd_part->offset;
+			part_size = mtd_part->size;
+		}
+#endif
+		pr_debug(" %08llx %08llx\n", part_addr, part_size);
+
+		if (part->addr != part_addr) {
+			stm32prog_err("%s: Bad address requested for partition %d = 0x%llx <> 0x%llx",
+				      part->name, part->part_id, part->addr,
+				      part_addr);
+			return -ENODEV;
+		}
+		if (part->size != part_size) {
+			stm32prog_err("%s: Bad size requested for partition %d = 0x%llx <> 0x%llx",
+				      part->name, part->part_id, part->size,
+				      part_size);
+			return -ENODEV;
+		}
+	}
+	return 0;
+}
+
+static int treat_partition_list(struct stm32prog_data *data)
+{
+	int i, j;
+	struct stm32prog_part_t *part;
+
+	for (j = 0; j < STM32PROG_MAX_DEV; j++) {
+		data->dev[j].dev_type = -1;
+		INIT_LIST_HEAD(&data->dev[j].part_list);
+	}
+
+	data->full_update = 1;
+	/*pr_debug("id : S Phase  Name       type  dev.n  addr  id\n");*/
+	for (i = 0; i < data->part_nb; i++) {
+		part = &data->part_array[i];
+		part->alt_id = -1;
+
+		/* skip partition with IP="none" */
+		if (part->dev_type == DFU_DEV_VIRT) {
+			if (IS_SELECT(part)) {
+				stm32prog_err("Layout: selected none phase = 0x%x",
+					      part->id);
+				return -EINVAL;
+			}
+			continue;
+		}
+
+		/*
+		 * pr_debug("%02d : %1d %02x %14s %02d %02d.%02d 0x%08llx %d\n",
+		 *	 i, part->option, part->id, part->name,
+		 *	 part->part_type, part->dev_id_type, part->dev_id,
+		 *	 part->addr, part->part_id);
+		 */
+		if (!IS_SELECT(part) && part->part_type != RAW_IMAGE)
+			data->full_update = 0;
+
+		if (part->id == PHASE_FLASHLAYOUT ||
+		    part->id > PHASE_LAST_USER) {
+			stm32prog_err("Layout: invalid phase = 0x%x",
+				      part->id);
+			return -EINVAL;
+		}
+		for (j = i + 1; j < data->part_nb; j++) {
+			if (part->id == data->part_array[j].id) {
+				stm32prog_err("Layout: duplicated phase %d at line %d and %d",
+					      part->id, i, j);
+				return -EINVAL;
+			}
+		}
+		for (j = 0; j < STM32PROG_MAX_DEV; j++) {
+			if (data->dev[j].dev_type == -1) {
+				/* new device found */
+				data->dev[j].dev_type = part->dev_type;
+				data->dev[j].dev_id = part->dev_id;
+				data->dev_nb++;
+				break;
+			} else if ((part->dev_type == data->dev[j].dev_type) &&
+				   (part->dev_id == data->dev[j].dev_id)) {
+				break;
+			}
+		}
+		if (j == STM32PROG_MAX_DEV) {
+			stm32prog_err("Layout: too many device");
+			return -EINVAL;
+		}
+		part->dev = &data->dev[j];
+		list_add_tail(&part->list, &data->dev[j].part_list);
+	}
+
+	return 0;
+}
+
+static int create_partitions(struct stm32prog_data *data)
+{
+	int offset = 0;
+	char cmdbuf[32];
+	char buf[ENV_BUF_LEN];
+	char uuid[UUID_STR_LEN + 1];
+	unsigned char *uuid_bin;
+	int i, mmc_id;
+	bool rootfs_found;
+	struct stm32prog_part_t *part;
+
+	puts("partitions : ");
+	/* initialize the selected device */
+	for (i = 0; i < data->dev_nb; i++) {
+		/* gpt support only for MMC */
+		if (data->dev[i].dev_type != DFU_DEV_MMC)
+			continue;
+
+		offset = 0;
+		rootfs_found = false;
+		memset(buf, 0, sizeof(buf));
+
+		list_for_each_entry(part, &data->dev[i].part_list, list) {
+			/* skip eMMC boot partitions */
+			if (part->part_id < 0)
+				continue;
+
+			/* skip Raw Image */
+			if (part->part_type == RAW_IMAGE)
+				continue;
+
+			offset += snprintf(buf + offset, ENV_BUF_LEN - offset,
+					   "name=%s,start=0x%llx,size=0x%llx",
+					   part->name,
+					   part->addr,
+					   part->size);
+
+			if (part->part_type == PART_BINARY)
+				offset += snprintf(buf + offset,
+						   ENV_BUF_LEN - offset,
+						   ",type=data");
+			else
+				offset += snprintf(buf + offset,
+						   ENV_BUF_LEN - offset,
+						   ",type=linux");
+
+			if (part->part_type == PART_SYSTEM)
+				offset += snprintf(buf + offset,
+						   ENV_BUF_LEN - offset,
+						   ",bootable");
+
+			if (!rootfs_found && !strcmp(part->name, "rootfs")) {
+				mmc_id = part->dev_id;
+				rootfs_found = true;
+				if (mmc_id < ARRAY_SIZE(uuid_mmc)) {
+					uuid_bin =
+					  (unsigned char *)uuid_mmc[mmc_id].b;
+					uuid_bin_to_str(uuid_bin, uuid,
+							UUID_STR_FORMAT_GUID);
+					offset += snprintf(buf + offset,
+							   ENV_BUF_LEN - offset,
+							   ",uuid=%s", uuid);
+				}
+			}
+
+			offset += snprintf(buf + offset,
+					   ENV_BUF_LEN - offset,
+					   ";");
+		}
+
+		if (offset) {
+			sprintf(cmdbuf, "gpt write mmc %d \"%s\"",
+				data->dev[i].dev_id, buf);
+			pr_debug("cmd: %s\n", cmdbuf);
+			if (run_command(cmdbuf, 0)) {
+				stm32prog_err("partitionning fail : %s",
+					      cmdbuf);
+				return -1;
+			}
+		}
+
+		if (data->dev[i].block_dev)
+			part_init(data->dev[i].block_dev);
+
+#ifdef DEBUG
+		sprintf(cmdbuf, "gpt verify mmc %d",
+			data->dev[i].dev_id);
+		pr_debug("cmd: %s ", cmdbuf);
+		if (run_command(cmdbuf, 0))
+			printf("fail !\n");
+		else
+			printf("OK\n");
+
+		/* TEMP : for debug, display partition */
+		sprintf(cmdbuf, "part list mmc %d",
+			data->dev[i].dev_id);
+		run_command(cmdbuf, 0);
+#endif
+	}
+	puts("done\n");
+	return 0;
+}
+
+static int stm32prog_alt_add(struct stm32prog_data *data,
+			     struct dfu_entity *dfu,
+			     struct stm32prog_part_t *part)
+{
+	int ret = 0;
+	int offset = 0;
+	char devstr[4];
+	char dfustr[10];
+	char buf[ENV_BUF_LEN];
+	u32 size;
+	char multiplier,  type;
+
+	/* max 3 digit for sector size */
+	if (part->size > SZ_1M) {
+		size = (u32)(part->size / SZ_1M);
+		multiplier = 'M';
+	} else if (part->size > SZ_1K) {
+		size = (u32)(part->size / SZ_1K);
+		multiplier = 'K';
+	} else {
+		size = (u32)part->size;
+		multiplier = 'B';
+	}
+	if (IS_SELECT(part) && !IS_EMPTY(part))
+		type = 'e'; /*Readable and Writeable*/
+	else
+		type = 'a';/*Readable*/
+
+	memset(buf, 0, sizeof(buf));
+	offset = snprintf(buf, ENV_BUF_LEN - offset,
+			  "@%s/0x%02x/1*%d%c%c ",
+			  part->name, part->id,
+			  size, multiplier, type);
+
+	if (part->part_type == RAW_IMAGE) {
+		u64 dfu_size;
+
+		if (part->dev->dev_type == DFU_DEV_MMC)
+			dfu_size = part->size / part->dev->lba_blk_size;
+		else
+			dfu_size = part->size;
+		offset += snprintf(buf + offset, ENV_BUF_LEN - offset,
+				   "raw 0x0 0x%llx", dfu_size);
+	} else if (part->part_id < 0) {
+		u64 nb_blk = part->size / part->dev->lba_blk_size;
+
+		/* lba_blk_size, mmc->read_bl_len */
+		offset += snprintf(buf + offset, ENV_BUF_LEN - offset,
+				   "raw 0x%llx 0x%llx",
+				   part->addr, nb_blk);
+		offset += snprintf(buf + offset, ENV_BUF_LEN - offset,
+				   " mmcpart %d;", -(part->part_id));
+	} else {
+		if (part->part_type == PART_SYSTEM &&
+		    (part->dev_type == DFU_DEV_NAND ||
+		     part->dev_type == DFU_DEV_SF))
+			offset += snprintf(buf + offset,
+					   ENV_BUF_LEN - offset,
+					   "partubi");
+		else
+			offset += snprintf(buf + offset,
+					   ENV_BUF_LEN - offset,
+					   "part");
+		offset += snprintf(buf + offset, ENV_BUF_LEN - offset,
+				   " %d %d;",
+				   part->dev_id,
+				   part->part_id);
+	}
+	switch (part->dev_type) {
+	case DFU_DEV_MMC:
+		sprintf(dfustr, "mmc");
+		sprintf(devstr, "%d", part->dev_id);
+		break;
+	case DFU_DEV_SF:
+		sprintf(dfustr, "sf");
+		sprintf(devstr, "0:%d", part->dev_id);
+		break;
+	case DFU_DEV_NAND:
+		sprintf(dfustr, "nand");
+		sprintf(devstr, "%d", part->dev_id);
+		break;
+	default:
+		stm32prog_err("invalid dev_type: %d", part->dev_type);
+		return -ENODEV;
+	}
+	ret = dfu_alt_add(dfu, dfustr, devstr, buf);
+	pr_debug("dfu_alt_add(%s,%s,%s) result %d\n",
+		 dfustr, devstr, buf, ret);
+
+	return ret;
+}
+
+static int stm32prog_alt_add_virt(struct dfu_entity *dfu,
+				  char *name, int phase, int size)
+{
+	int ret = 0;
+	char devstr[4];
+	char buf[ENV_BUF_LEN];
+
+	sprintf(devstr, "%d", phase);
+	sprintf(buf, "@%s/0x%02x/1*%dBe", name, phase, size);
+	ret = dfu_alt_add(dfu, "virt", devstr, buf);
+	pr_debug("dfu_alt_add(virt,%s,%s) result %d\n", devstr, buf, ret);
+
+	return ret;
+}
+
+static int dfu_init_entities(struct stm32prog_data *data)
+{
+	int ret = 0;
+	int phase, i, alt_id;
+	struct stm32prog_part_t *part;
+	struct dfu_entity *dfu;
+	int alt_nb;
+
+	/* nb of alternate = nb part not virtual or 1 for FlashLayout
+	 * + 3 virtual for CMD and OTP and PMIC
+	 */
+	if (data->part_nb == 0) {
+		alt_nb = 4;
+	} else {
+		alt_nb = 3;
+		for (i = 0; i < data->part_nb; i++) {
+			if (data->part_array[i].dev_type != DFU_DEV_VIRT)
+				alt_nb++;
+		}
+	}
+
+	if (dfu_alt_init(alt_nb, &dfu))
+		return -ENODEV;
+
+	puts("DFU alt info setting: ");
+	if (data->part_nb) {
+		alt_id = 0;
+		for (phase = 1;
+		     (phase <= PHASE_LAST_USER) &&
+		     (alt_id < alt_nb) && !ret;
+		     phase++) {
+			/* ordering alt setting by phase id */
+			part = NULL;
+			for (i = 0; i < data->part_nb; i++) {
+				if (phase == data->part_array[i].id) {
+					part = &data->part_array[i];
+					break;
+				}
+			}
+			if (!part)
+				continue;
+			if (part->dev_type == DFU_DEV_VIRT)
+				continue;
+			part->alt_id = alt_id;
+			alt_id++;
+
+			ret = stm32prog_alt_add(data, dfu, part);
+		}
+	} else {
+		char buf[ENV_BUF_LEN];
+
+		sprintf(buf, "@FlashLayout/0x%02x/1*256Ke ram %x 40000",
+			PHASE_FLASHLAYOUT, STM32_DDR_BASE);
+		ret = dfu_alt_add(dfu, "ram", NULL, buf);
+		pr_debug("dfu_alt_add(ram, NULL,%s) result %d\n", buf, ret);
+	}
+
+	if (!ret)
+		ret = stm32prog_alt_add_virt(dfu, "virtual", PHASE_CMD, 512);
+
+	if (!ret)
+		ret = stm32prog_alt_add_virt(dfu, "OTP", PHASE_OTP, 512);
+
+#ifdef CONFIG_DM_PMIC
+	if (!ret)
+		ret = stm32prog_alt_add_virt(dfu, "PMIC", PHASE_PMIC, 8);
+#endif
+
+	if (ret)
+		stm32prog_err("dfu init failed: %d", ret);
+	puts("done\n");
+#ifdef DEBUG
+	/* TEMP : for debug */
+	dfu_show_entities();
+#endif
+	return ret;
+}
+
+int stm32prog_otp_write(struct stm32prog_data *data, u32 offset, u8 *buffer,
+			long *size)
+{
+	pr_debug("%s : %x %lx\n", __func__, offset, *size);
+
+	if (!data->otp_part) {
+		data->otp_part = memalign(CONFIG_SYS_CACHELINE_SIZE, OTP_SIZE);
+		if (!data->otp_part)
+			return -ENOMEM;
+	}
+
+	if (!offset)
+		memset(data->otp_part, 0, OTP_SIZE);
+
+	if (offset + *size > OTP_SIZE)
+		*size = OTP_SIZE - offset;
+
+	memcpy((void *)((u32)data->otp_part + offset), buffer, *size);
+	return 0;
+}
+
+int stm32prog_otp_read(struct stm32prog_data *data, u32 offset, u8 *buffer,
+		       long *size)
+{
+#ifndef CONFIG_ARM_SMCCC
+	stm32prog_err("OTP update not supported");
+	return -1;
+#else
+	int result = 0;
+
+	pr_debug("%s : %x %lx\n", __func__, offset, *size);
+	/* alway read for first packet */
+	if (!offset) {
+		if (!data->otp_part)
+			data->otp_part =
+				memalign(CONFIG_SYS_CACHELINE_SIZE, OTP_SIZE);
+
+		if (!data->otp_part) {
+			result = -ENOMEM;
+			goto end_otp_read;
+		}
+
+		/* init struct with 0 */
+		memset(data->otp_part, 0, OTP_SIZE);
+
+		/* call the service */
+		result = stm32_smc_exec(STM32_SMC_BSEC, STM32_SMC_READ_ALL,
+					(u32)data->otp_part, 0);
+		if (result)
+			goto end_otp_read;
+	}
+
+	if (!data->otp_part) {
+		result = -ENOMEM;
+		goto end_otp_read;
+	}
+
+	if (offset + *size > OTP_SIZE)
+		*size = OTP_SIZE - offset;
+	memcpy(buffer, (void *)((u32)data->otp_part + offset), *size);
+
+end_otp_read:
+	pr_debug("%s : result %i\n", __func__, result);
+	return result;
+#endif
+}
+
+int stm32prog_otp_start(struct stm32prog_data *data)
+{
+#ifndef CONFIG_ARM_SMCCC
+	stm32prog_err("OTP update not supported");
+	return -1;
+#else
+	int result = 0;
+	struct arm_smccc_res res;
+
+	if (!data->otp_part) {
+		stm32prog_err("start OTP without data");
+		return -1;
+	}
+
+	arm_smccc_smc(STM32_SMC_BSEC, STM32_SMC_WRITE_ALL,
+		      (u32)data->otp_part, 0, 0, 0, 0, 0, &res);
+
+	if (!res.a0) {
+		switch (res.a1) {
+		case 0:
+			result = 0;
+			break;
+		case 1:
+			stm32prog_err("Provisioning");
+			result = 0;
+			break;
+		default:
+			pr_err("%s: OTP incorrect value (err = %ld)\n",
+			       __func__, res.a1);
+			result = -EINVAL;
+			break;
+		}
+	} else {
+		pr_err("%s: Failed to exec in secure mode (err = %ld)\n",
+		       __func__, res.a0);
+		result = -EINVAL;
+	}
+
+	free(data->otp_part);
+	data->otp_part = NULL;
+	pr_debug("%s : result %i\n", __func__, result);
+	return result;
+#endif
+}
+
+int stm32prog_pmic_write(struct stm32prog_data *data, u32 offset, u8 *buffer,
+			 long *size)
+{
+	pr_debug("%s : %x %lx\n", __func__, offset, *size);
+
+	if (!offset)
+		memset(data->pmic_part, 0, PMIC_SIZE);
+
+	if (offset + *size > PMIC_SIZE)
+		*size = PMIC_SIZE - offset;
+
+	memcpy(&data->pmic_part[offset], buffer, *size);
+
+	return 0;
+}
+
+int stm32prog_pmic_read(struct stm32prog_data *data, u32 offset, u8 *buffer,
+			long *size)
+{
+#ifndef CONFIG_DM_PMIC
+	stm32prog_err("PMIC update not supported");
+	return -EOPNOTSUPP;
+#else /* CONFIG_DM_PMIC */
+	int result = 0;
+
+	pr_debug("%s : %x %lx\n", __func__, offset, *size);
+
+	/* alway request PMIC for first packet */
+	if (!offset) {
+		/* init struct with 0 */
+		memset(data->pmic_part, 0, PMIC_SIZE);
+
+		result = stpmic1_nvm_read_all(data->pmic_part, PMIC_SIZE);
+		if (result < 0)
+			goto end_pmic_read;
+	}
+
+	if (offset + *size > PMIC_SIZE)
+		*size = PMIC_SIZE - offset;
+
+	memcpy(buffer, &data->pmic_part[offset], *size);
+
+end_pmic_read:
+	pr_debug("%s : result %i\n", __func__, result);
+	return result;
+#endif /* CONFIG_DM_PMIC */
+}
+
+int stm32prog_pmic_start(struct stm32prog_data *data)
+{
+#ifndef CONFIG_DM_PMIC
+	stm32prog_err("PMIC update not supported");
+	return -EOPNOTSUPP;
+#else /* CONFIG_DM_PMIC */
+	return stpmic1_nvm_write_all(data->pmic_part, PMIC_SIZE);
+#endif /* CONFIG_DM_PMIC */
+}
+
+/* copy FSBL on NAND to improve reliability on NAND */
+static int stm32prog_copy_fsbl(struct stm32prog_part_t *part)
+{
+#ifndef CONFIG_CMD_NAND
+	return -1;
+#else
+	loff_t start, lim;
+	size_t count, actual = 0;
+	int ret, i;
+	void *fsbl;
+	nand_erase_options_t opts;
+	struct image_header_s header;
+	struct raw_header_s raw_header;
+
+	if (part->dev_type != DFU_DEV_NAND)
+		return -1;
+
+	start = part->addr;
+	lim = part->size;
+	count = BL_HEADER_SIZE;
+
+	ret = nand_read_skip_bad(part->dev->mtd, start, &count, &actual, lim,
+				 (void *)&raw_header);
+	if (ret)
+		return ret;
+	if (stm32prog_header_check(&raw_header, &header))
+		return -1;
+
+	count = header.image_length + BL_HEADER_SIZE;
+	fsbl = calloc(1, count);
+	if (!fsbl)
+		return -ENOMEM;
+	ret = nand_read_skip_bad(part->dev->mtd, start, &count, &actual, lim,
+				 fsbl);
+	if (ret)
+		goto error;
+
+	memset(&opts, 0, sizeof(opts));
+	opts.length = count;
+	opts.spread = 1;
+#ifndef DEBUG
+	opts.quiet = 1;
+#endif
+
+	for (i = part->bin_nb - 1; i > 0; i--) {
+		size_t block_offset;
+
+		/* copy to next block */
+		start += actual;
+		block_offset = start & (part->dev->mtd->erasesize - 1);
+		if (block_offset != 0)
+			start += part->dev->mtd->erasesize - block_offset;
+
+		lim = part->size - (start - part->addr);
+
+		/* first erase */
+		opts.offset = start;
+		opts.lim = lim;
+		ret = nand_erase_opts(part->dev->mtd, &opts);
+		if (ret)
+			goto error;
+
+		/* then write */
+		ret = nand_write_skip_bad(part->dev->mtd,
+					  start, &count, &actual,
+					  lim, fsbl, WITH_WR_VERIFY);
+		if (ret)
+			goto error;
+	}
+
+error:
+	free(fsbl);
+	/* pr_debug("%s exit ret=%d\n", __func__, ret); */
+	return ret;
+#endif
+}
+
+void stm32prog_end_phase(struct stm32prog_data *data)
+{
+	if (data->phase == PHASE_FLASHLAYOUT) {
+		if (parse_flash_layout(data, STM32_DDR_BASE, 0))
+			stm32prog_err("Layout: invalid FlashLayout");
+		return;
+	}
+
+	if (!data->cur_part)
+		return;
+
+	if (data->cur_part->part_id < 0) {
+		char cmdbuf[60];
+
+		sprintf(cmdbuf, "mmc bootbus %d 0 0 0; mmc partconf %d 1 %d 0",
+			data->cur_part->dev_id, data->cur_part->dev_id,
+			-(data->cur_part->part_id));
+		if (run_command(cmdbuf, 0)) {
+			stm32prog_err("commands %s have failed", cmdbuf);
+			return;
+		}
+	}
+
+	if (data->cur_part->bin_nb > 1) {
+		if (stm32prog_copy_fsbl(data->cur_part)) {
+			stm32prog_err("copy of fsbl failed");
+			return;
+		}
+	}
+}
+
+void stm32prog_do_reset(struct stm32prog_data *data)
+{
+	if (data->phase == PHASE_RESET) {
+		data->phase = PHASE_DO_RESET;
+		puts("Reset requested\n");
+	}
+}
+
+void stm32prog_next_phase(struct stm32prog_data *data)
+{
+	int phase, i;
+	struct stm32prog_part_t *part;
+
+	/*pr_debug("%s entry\n", __func__);*/
+
+	phase = data->phase;
+	switch (phase) {
+	case PHASE_RESET:
+	case PHASE_END:
+	case PHASE_DO_RESET:
+		return;
+	}
+
+	/* found next selected partition */
+	phase++;
+	data->cur_part = NULL;
+	data->dfu_seq = 0;
+	data->phase = PHASE_END;
+	while ((phase <= PHASE_LAST_USER) && !data->cur_part) {
+		for (i = 0; i < data->part_nb; i++) {
+			part = &data->part_array[i];
+			if (part->id == phase) {
+				if (IS_SELECT(part) && !IS_EMPTY(part)) {
+					data->cur_part = part;
+					data->phase = phase;
+				}
+				break;
+			}
+		}
+		phase++;
+	}
+	if (data->phase == PHASE_END)
+		puts("Phase=END\n");
+
+	/*pr_debug("%s exit phase=0x%x\n", __func__, data->phase);*/
+}
+
+static int part_delete(struct stm32prog_data *data,
+		       struct stm32prog_part_t *part)
+{
+	int ret = 0;
+	unsigned long blks, blks_offset, blks_size;
+#ifdef CONFIG_SPI_FLASH
+	char cmdbuf[40];
+#endif
+
+	printf("Erasing %s ", part->name);
+	switch (part->dev_type) {
+	case DFU_DEV_MMC:
+		printf("on mmc %d: ", part->dev->dev_id);
+		blks_offset = lldiv(part->addr, part->dev->lba_blk_size);
+		blks_size = lldiv(part->size, part->dev->lba_blk_size);
+		/* -1 or -2 : delete boot partition of MMC
+		 * need to switch to associated hwpart 1 or 2
+		 */
+		if (part->part_id < 0)
+			if (blk_select_hwpart_devnum(IF_TYPE_MMC,
+						     part->dev->dev_id,
+						     -part->part_id))
+				return -1;
+		blks = blk_derase(part->dev->block_dev, blks_offset, blks_size);
+		/* return to user partition */
+		if (part->part_id < 0)
+			blk_select_hwpart_devnum(IF_TYPE_MMC,
+						 part->dev->dev_id, 0);
+		if (blks != blks_size) {
+			ret = -1;
+			stm32prog_err("mmc erase failed");
+		}
+		break;
+
+#ifdef CONFIG_SPI_FLASH
+	case DFU_DEV_SF:
+		printf("on sf %d: ", part->dev->dev_id);
+		sprintf(cmdbuf, "sf erase 0x%llx 0x%llx",
+			part->addr, part->size);
+		if (run_command(cmdbuf, 0)) {
+			ret = -1;
+			stm32prog_err("sf erase commands failed (%s)", cmdbuf);
+		}
+		break;
+#endif
+
+#ifdef CONFIG_CMD_NAND
+	case DFU_DEV_NAND:
+		printf("on nand %d: ", part->dev->dev_id);
+		nand_erase_options_t opts;
+			memset(&opts, 0, sizeof(opts));
+		opts.offset = part->addr;
+		opts.length = part->size;
+		opts.quiet = 1;
+		ret = nand_erase_opts(part->dev->mtd, &opts);
+		if (ret)
+			stm32prog_err("nand erase failed");
+		break;
+#endif
+	default:
+		ret = -1;
+		stm32prog_err("erase invalid");
+		break;
+	}
+	if (!ret)
+		printf("done\n");
+
+	return ret;
+}
+
+void stm32prog_devices_init(struct stm32prog_data *data)
+{
+	int i;
+	int ret;
+	struct stm32prog_part_t *part;
+
+	ret = treat_partition_list(data);
+	if (ret)
+		goto error;
+
+	/* initialize the selected device */
+	for (i = 0; i < data->dev_nb; i++) {
+		ret = init_device(data, &data->dev[i]);
+		if (ret)
+			goto error;
+	}
+
+	/* delete RAW partition before create partition */
+	for (i = 0; i < data->part_nb; i++) {
+		part = &data->part_array[i];
+
+		if (part->part_type != RAW_IMAGE)
+			continue;
+
+		if (!IS_SELECT(part) || !IS_DELETE(part))
+			continue;
+
+		ret = part_delete(data, part);
+		if (ret)
+			goto error;
+	}
+
+	if (data->full_update) {
+		ret = create_partitions(data);
+		if (ret)
+			goto error;
+	}
+
+	/* delete partition GPT or MTD */
+	for (i = 0; i < data->part_nb; i++) {
+		part = &data->part_array[i];
+
+		if (part->part_type == RAW_IMAGE)
+			continue;
+
+		if (!IS_SELECT(part) || !IS_DELETE(part))
+			continue;
+
+		ret = part_delete(data, part);
+		if (ret)
+			goto error;
+	}
+
+	return;
+
+error:
+	data->part_nb = 0;
+}
+
+int stm32prog_dfu_init(struct stm32prog_data *data)
+{
+	/* init device if no error */
+	if (data->part_nb)
+		stm32prog_devices_init(data);
+
+	if (data->part_nb)
+		stm32prog_next_phase(data);
+
+	/* prepare DFU for device read/write */
+	dfu_free_entities();
+	return dfu_init_entities(data);
+}
+
+struct stm32prog_data *stm32prog_init(enum stm32prog_link_t link,
+				      int link_dev,
+				      ulong addr,
+				      ulong size)
+{
+	struct stm32prog_data *data;
+
+	/*pr_debug("%s entry\n", __func__);*/
+	data = (struct stm32prog_data *)malloc(sizeof(*data));
+
+	if (!data) {
+		pr_err("alloc failed");
+		goto err;
+	}
+	memset(data, 0x0, sizeof(*data));
+	data->read_phase = PHASE_RESET;
+	data->phase = PHASE_FLASHLAYOUT;
+
+	parse_flash_layout(data, addr, size);
+
+	/* prepare DFU for device read/write */
+	if (stm32prog_dfu_init(data))
+		goto err;
+
+	switch (link) {
+	case LINK_SERIAL:
+		if (stm32prog_serial_init(data, link_dev))
+			goto err;
+		data->buffer = memalign(CONFIG_SYS_CACHELINE_SIZE,
+					USART_RAM_BUFFER_SIZE);
+		break;
+	case LINK_USB:
+		break;
+	default:
+		break;
+	}
+	/*pr_debug("%s exit ok\n", __func__);*/
+	return data;
+
+err:
+	free(data);
+	pr_debug("%s exit error\n", __func__);
+	return 0;
+}
+
+void stm32prog_clean(struct stm32prog_data *data)
+{
+	/* clean */
+	dfu_free_entities();
+	free(data->part_array);
+	free(data->otp_part);
+	free(data->buffer);
+	free(data->header_data);
+	free(data);
+}
diff --git a/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.h b/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.h
new file mode 100644
index 0000000..51541bc
--- /dev/null
+++ b/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.h
@@ -0,0 +1,202 @@
+/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef _STM32PROG_H_
+#define _STM32PROG_H_
+
+/* - configuration part -----------------------------*/
+#define USART_BL_VERSION	0x40	/* USART bootloader version V4.0*/
+#define UBOOT_BL_VERSION	0x03	/* bootloader version V0.3*/
+#define DEVICE_ID_BYTE1		0x05	/* MSB byte of device ID*/
+#define DEVICE_ID_BYTE2		0x00	/* LSB byte of device ID*/
+#define USART_RAM_BUFFER_SIZE	256	/* Size of USART_RAM_Buf buffer*/
+
+/* - Commands -----------------------------*/
+#define GET_CMD_COMMAND		0x00	/* Get CMD command*/
+#define GET_VER_COMMAND		0x01	/* Get Version command*/
+#define GET_ID_COMMAND		0x02	/* Get ID command*/
+#define GET_PHASE_COMMAND	0x03	/* Get Phase command*/
+#define RM_COMMAND		0x11	/* Read Memory command*/
+#define READ_PART_COMMAND	0x12	/* Read Partition command*/
+#define START_COMMAND		0x21	/* START command (Go)*/
+#define DOWNLOAD_COMMAND	0x31	/* Download command*/
+/* existing command for other STM32 but not used */
+/* ERASE			0x43 */
+/* EXTENDED_ERASE		0x44 */
+/* WRITE_UNPROTECTED		0x73 */
+/* READOUT_PROTECT		0x82 */
+/* READOUT_UNPROTECT		0x92 */
+
+/* - miscellaneous defines ----------------------------------------*/
+#define INIT_BYTE		0x7F	/*Init Byte ID*/
+#define ACK_BYTE		0x79	/*Acknowlede Byte ID*/
+#define NACK_BYTE		0x1F	/*No Acknowlede Byte ID*/
+#define ABORT_BYTE		0x5F	/*ABORT*/
+
+/* - phase defines ------------------------------------------------*/
+#define PHASE_FLASHLAYOUT	0x00
+#define PHASE_FIRST_USER	0x10
+#define PHASE_LAST_USER		0xF0
+#define PHASE_CMD		0xF1
+#define PHASE_OTP		0xF2
+#define PHASE_SSP		0xF3
+#define PHASE_PMIC		0xF4
+#define PHASE_END		0xFE
+#define PHASE_RESET		0xFF
+#define PHASE_DO_RESET		0x1FF
+
+#define DEFAULT_ADDRESS		0xFFFFFFFF
+
+#define OTP_SIZE		1024
+#define PMIC_SIZE		8
+
+enum stm32prog_link_t {
+	LINK_SERIAL,
+	LINK_USB,
+	LINK_UNDEFINED,
+};
+
+struct image_header_s {
+	bool	present;
+	u32	image_checksum;
+	u32	image_length;
+};
+
+struct raw_header_s {
+	u32 magic_number;
+	u32 image_signature[64 / 4];
+	u32 image_checksum;
+	u32 header_version;
+	u32 image_length;
+	u32 image_entry_point;
+	u32 reserved1;
+	u32 load_address;
+	u32 reserved2;
+	u32 version_number;
+	u32 option_flags;
+	u32 ecdsa_algorithm;
+	u32 ecdsa_public_key[64 / 4];
+	u32 padding[83 / 4];
+	u32 binary_type;
+};
+
+#define BL_HEADER_SIZE	sizeof(struct raw_header_s)
+
+/* partition type in flashlayout file */
+enum stm32prog_part_type {
+	PART_BINARY,
+	PART_SYSTEM,
+	PART_FILESYSTEM,
+	RAW_IMAGE
+};
+
+/* device information */
+struct stm32prog_dev_t {
+	enum dfu_device_type	dev_type;
+	char			dev_id;
+	struct blk_desc		*block_dev;
+	u32			lba_blk_size;	/* for MMC RAW */
+	u32			erase_size;
+	struct mtd_info		*mtd;
+	/* list of partition for this device / ordered in offset */
+	struct list_head	part_list;
+};
+
+/* partition information build form FlashLayout and device */
+struct stm32prog_part_t {
+	/* FlashLayout inforamtion */
+	int			option;
+	int			id;
+	enum stm32prog_part_type part_type;
+	enum dfu_device_type	dev_type;
+	char			dev_id;
+	/* partition name
+	 * (16 char in gpt, + 1 for null terminated string
+	 */
+	char			name[16 + 1];
+	u64			addr;
+	u64			size;
+	enum stm32prog_part_type bin_nb;	/* SSBL repeatition */
+
+	/* information on associated device */
+	struct stm32prog_dev_t	*dev;		/* pointer to device */
+	/* partition id in gpt when >0, -1 and -2 for boot partition of MMC */
+	s16			part_id;
+	int			alt_id;		/* alt id in usb/dfu */
+
+	struct list_head	list;
+};
+
+#define STM32PROG_MAX_DEV 5
+struct stm32prog_data {
+	/* Layout information */
+	int			dev_nb;		/* device number*/
+	struct stm32prog_dev_t	dev[STM32PROG_MAX_DEV];	/* array of device */
+	int			part_nb;	/* nb of partition */
+	struct stm32prog_part_t	*part_array;	/* array of partition */
+	int			full_update;
+
+	/* command internal information */
+	int			phase;
+	u32			offset;
+	char			error[255];
+	struct stm32prog_part_t	*cur_part;
+	u32			*otp_part;
+	u8			pmic_part[PMIC_SIZE];
+
+	/* STM32 header information */
+	struct raw_header_s	*header_data;
+	struct image_header_s	header;
+
+	/* SERIAL information */
+	u32	cursor;
+	u32	packet_number;
+	u32	checksum;
+	u8	*buffer; /* size = USART_RAM_BUFFER_SIZE*/
+	int	dfu_seq;
+	u8	read_phase;
+};
+
+/* OTP access */
+int stm32prog_otp_write(struct stm32prog_data *data, u32 offset,
+			u8 *buffer, long *size);
+int stm32prog_otp_read(struct stm32prog_data *data, u32 offset,
+		       u8 *buffer, long *size);
+int stm32prog_otp_start(struct stm32prog_data *data);
+
+/* PMIC access */
+int stm32prog_pmic_write(struct stm32prog_data *data, u32 offset,
+			 u8 *buffer, long *size);
+int stm32prog_pmic_read(struct stm32prog_data *data, u32 offset,
+			u8 *buffer, long *size);
+int stm32prog_pmic_start(struct stm32prog_data *data);
+
+/* generic part*/
+u8 stm32prog_header_check(struct raw_header_s *raw_header,
+			  struct image_header_s *header);
+int stm32prog_dfu_init(struct stm32prog_data *data);
+void stm32prog_end_phase(struct stm32prog_data *data);
+void stm32prog_next_phase(struct stm32prog_data *data);
+void stm32prog_do_reset(struct stm32prog_data *data);
+
+int stm32prog_serial_init(struct stm32prog_data *data, int link_dev);
+
+char *stm32prog_get_error(struct stm32prog_data *data);
+
+#define stm32prog_err(args...) {\
+	if (data->phase != PHASE_RESET) { \
+		sprintf(data->error, args); \
+		data->phase = PHASE_RESET; \
+		pr_err("Error: %s\n", data->error); } \
+	}
+
+/* Main function */
+struct stm32prog_data *stm32prog_init(enum stm32prog_link_t link,
+				      int dev, ulong addr, ulong size);
+bool stm32prog_serial_loop(struct stm32prog_data *data);
+bool stm32prog_usb_loop(struct stm32prog_data *data, int dev);
+void stm32prog_clean(struct stm32prog_data *data);
+
+#endif
diff --git a/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog_serial.c b/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog_serial.c
new file mode 100644
index 0000000..36f9393
--- /dev/null
+++ b/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog_serial.c
@@ -0,0 +1,972 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <console.h>
+#include <dfu.h>
+#include <malloc.h>
+#include <serial.h>
+#include <watchdog.h>
+#include <dm/lists.h>
+#include <dm/device-internal.h>
+#include "stm32prog.h"
+
+struct udevice *down_serial_dev;
+
+const u8 cmd_id[] = {
+	GET_CMD_COMMAND,
+	GET_VER_COMMAND,
+	GET_ID_COMMAND,
+	GET_PHASE_COMMAND,
+	RM_COMMAND,
+	READ_PART_COMMAND,
+	START_COMMAND,
+	DOWNLOAD_COMMAND
+};
+
+#define NB_CMD sizeof(cmd_id)
+
+/* DFU support for serial *********************************************/
+struct dfu_entity *stm32prog_get_entity(struct stm32prog_data *data)
+{
+	int alt_id;
+
+	if (!data->cur_part)
+		if (data->phase == PHASE_FLASHLAYOUT)
+			alt_id = 0;
+		else
+			return NULL;
+	else
+		alt_id = data->cur_part->alt_id;
+
+	return dfu_get_entity(alt_id);
+}
+
+static int stm32prog_write(struct stm32prog_data *data, u8 *buffer,
+			   u32 buffer_size)
+{
+	struct dfu_entity *dfu_entity;
+	u8 ret = 0;
+
+	dfu_entity = stm32prog_get_entity(data);
+	if (!dfu_entity)
+		return -ENODEV;
+
+	ret = dfu_write(dfu_entity,
+			buffer,
+			buffer_size,
+			data->dfu_seq);
+
+	if (ret) {
+		stm32prog_err("DFU write failed [%d] cnt: %d",
+			      ret, data->dfu_seq);
+	}
+	data->dfu_seq++;
+	/* handle rollover as in driver/dfu/dfu.c */
+	data->dfu_seq &= 0xffff;
+	if (buffer_size == 0)
+		data->dfu_seq = 0; /* flush done */
+
+	return ret;
+}
+
+static int stm32prog_read(struct stm32prog_data *data, u8 phase, u32 offset,
+			  u8 *buffer, u32 buffer_size)
+{
+	struct dfu_entity *dfu_entity;
+	int ret;
+
+	/* pr_debug("%s entry\n", __func__); */
+	if (data->dfu_seq) {
+		stm32prog_err("DFU write pending for phase %d, seq %d",
+			      data->phase, data->dfu_seq);
+		return -EINVAL;
+	}
+	if (phase == PHASE_FLASHLAYOUT || phase > PHASE_LAST_USER) {
+		stm32prog_err("read failed : phase %d is invalid", phase);
+		return -EINVAL;
+	}
+	if (data->read_phase <= PHASE_LAST_USER &&
+	    phase != data->read_phase) {
+		/* clear previous read session */
+		dfu_entity = dfu_get_entity(data->read_phase - 1);
+		if (dfu_entity)
+			dfu_transaction_cleanup(dfu_entity);
+	}
+	dfu_entity = dfu_get_entity(phase - 1);
+	if (!dfu_entity) {
+		stm32prog_err("read failed : phase %d is unknown", phase);
+		return -ENODEV;
+	}
+	/* clear pending read before to force offset */
+	if (dfu_entity->inited &&
+	    (data->read_phase != phase || data->offset != offset))
+		dfu_transaction_cleanup(dfu_entity);
+
+	/* initiate before to force offset */
+	if (!dfu_entity->inited) {
+		ret = dfu_transaction_initiate(dfu_entity, true);
+			if (ret < 0) {
+				stm32prog_err("DFU read init failed [%d] phase = %d offset = 0x%08x",
+					      ret, phase, offset);
+			return ret;
+		}
+	}
+	/* force new offset */
+	if (dfu_entity->offset != offset)
+		dfu_entity->offset = offset;
+	data->offset = offset;
+	data->read_phase = phase;
+	pr_debug("\nSTM32 download read %s offset=0x%x\n",
+		 dfu_entity->name, offset);
+	ret = dfu_read(dfu_entity, buffer, buffer_size,
+		       dfu_entity->i_blk_seq_num);
+	if (ret < 0) {
+		stm32prog_err("DFU read failed [%d] phase = %d offset = 0x%08x",
+			      ret, phase, offset);
+		return ret;
+	}
+	if (ret < buffer_size) {
+		data->offset = 0;
+		data->read_phase = PHASE_END;
+		memset(buffer + ret, 0, buffer_size - ret);
+	} else {
+		data->offset += ret;
+	}
+	/*pr_debug("%s exit ret=%d\n", __func__, ret);*/
+	return ret;
+}
+
+/* UART access ***************************************************/
+int stm32prog_serial_init(struct stm32prog_data *data, int link_dev)
+{
+	struct udevice *dev = NULL;
+	int node;
+	char alias[10];
+	const char *path;
+	struct dm_serial_ops *ops;
+	/* no parity, 8 bits, 1 stop */
+	u32 serial_config = SERIAL_DEFAULT_CONFIG;
+
+	down_serial_dev = NULL;
+
+	sprintf(alias, "serial%d", link_dev);
+	path = fdt_get_alias(gd->fdt_blob, alias);
+	if (!path) {
+		pr_err("%s alias not found", alias);
+		return -ENODEV;
+	}
+	node = fdt_path_offset(gd->fdt_blob, path);
+	if (!uclass_get_device_by_of_offset(UCLASS_SERIAL, node,
+					    &dev)) {
+		down_serial_dev = dev;
+	} else if (node > 0 &&
+		   !lists_bind_fdt(gd->dm_root, offset_to_ofnode(node),
+				   &dev)) {
+		if (!device_probe(dev))
+			down_serial_dev = dev;
+	}
+	/*pr_debug("alias=%s, path=%s, node = %d, dev=%0x\n",
+	 *	 alias, path, node, (u32)down_serial_dev);
+	 */
+	if (!down_serial_dev) {
+		pr_err("%s = %s device not found", alias, path);
+		return -ENODEV;
+	}
+
+	/* force silent console on uart only when used */
+	if (gd->cur_serial_dev == down_serial_dev)
+		gd->flags |= GD_FLG_DISABLE_CONSOLE | GD_FLG_SILENT;
+	else
+		gd->flags &= ~(GD_FLG_DISABLE_CONSOLE | GD_FLG_SILENT);
+
+	ops = serial_get_ops(down_serial_dev);
+
+	if (!ops) {
+		pr_err("%s = %s missing ops", alias, path);
+		return -ENODEV;
+	}
+	if (!ops->setconfig) {
+		pr_err("%s = %s missing setconfig", alias, path);
+		return -ENODEV;
+	}
+
+	clrsetbits_le32(&serial_config, SERIAL_PAR_MASK, SERIAL_PAR_EVEN);
+
+	return ops->setconfig(down_serial_dev, serial_config);
+}
+
+static void stm32prog_serial_flush(void)
+{
+	struct dm_serial_ops *ops = serial_get_ops(down_serial_dev);
+	int err;
+
+	do {
+		err = ops->getc(down_serial_dev);
+	} while (err != -EAGAIN);
+}
+
+static int stm32prog_serial_getc_err(void)
+{
+	struct dm_serial_ops *ops = serial_get_ops(down_serial_dev);
+	int err;
+
+	do {
+		err = ops->getc(down_serial_dev);
+		if (err == -EAGAIN)
+			ctrlc();
+	} while ((err == -EAGAIN) && (!had_ctrlc()));
+
+	return err;
+}
+
+static u8 stm32prog_serial_getc(void)
+{
+	int err;
+
+	err = stm32prog_serial_getc_err();
+
+	return err >= 0 ? err : 0;
+}
+
+static bool stm32prog_serial_get_buffer(u8 *buffer, u32 *count)
+{
+	struct dm_serial_ops *ops = serial_get_ops(down_serial_dev);
+	int err;
+
+	do {
+		err = ops->getc(down_serial_dev);
+		if (err >= 0) {
+			*buffer++ = err;
+			*count -= 1;
+		} else if (err == -EAGAIN) {
+			ctrlc();
+		} else {
+			break;
+		}
+	} while (*count && !had_ctrlc());
+
+	return !!(err < 0);
+}
+
+static void stm32prog_serial_putc(u8 w_byte)
+{
+	struct dm_serial_ops *ops = serial_get_ops(down_serial_dev);
+	int err;
+
+	do {
+		err = ops->putc(down_serial_dev, w_byte);
+	} while (err == -EAGAIN);
+}
+
+/* Helper function ************************************************/
+
+static u8 stm32prog_header(struct stm32prog_data *data)
+{
+	u8 ret;
+	u8 boot = 0;
+	struct dfu_entity *dfu_entity;
+	u64 size = 0;
+
+	/*pr_debug("%s entry\n", __func__);*/
+
+	dfu_entity = stm32prog_get_entity(data);
+	if (!dfu_entity)
+		return -ENODEV;
+
+	printf("\nSTM32 download write %s\n", dfu_entity->name);
+
+	/* force cleanup to avoid issue with previous read */
+	dfu_transaction_cleanup(dfu_entity);
+
+	ret = stm32prog_header_check(data->header_data,
+				     &data->header);
+
+	/* no header : max size is partition size */
+	if (ret) {
+		dfu_entity->get_medium_size(dfu_entity, &size);
+		data->header.image_length = size;
+	}
+
+	/**** Flash the header if necessary for boot partition */
+	if (data->phase < PHASE_FIRST_USER)
+		boot = 1;
+
+	/* write header if boot partition */
+	if (boot) {
+		if (ret) {
+			stm32prog_err("invalid header (error %d)", ret);
+		} else {
+			ret = stm32prog_write(data,
+					      (u8 *)data->header_data,
+					      BL_HEADER_SIZE);
+		}
+	} else {
+		if (ret)
+			printf("  partition without checksum\n");
+		ret = 0;
+	}
+
+	free(data->header_data);
+	data->header_data = NULL;
+	/*pr_debug("%s result=%d\n", __func__, ret);*/
+	return ret;
+}
+
+static u8 stm32prog_start(struct stm32prog_data *data, u32 address)
+{
+	u8 ret = 0;
+	struct dfu_entity *dfu_entity;
+
+	/*pr_debug("%s entry\n", __func__);*/
+
+	if (address < 0x100) {
+		if (address == PHASE_OTP)
+			return stm32prog_otp_start(data);
+
+		if (address == PHASE_PMIC)
+			return stm32prog_pmic_start(data);
+
+		if (address == PHASE_RESET || address == PHASE_END) {
+			data->cur_part = NULL;
+			data->dfu_seq = 0;
+			data->phase = address;
+			return 0;
+		}
+		if (address != data->phase) {
+			stm32prog_err("invalid received phase id %d, current phase is %d",
+				      (u8)address, (u8)data->phase);
+			return -EINVAL;
+		}
+	}
+	/* check the last loaded partition */
+	if (address == DEFAULT_ADDRESS || address == data->phase) {
+		switch (data->phase) {
+		case PHASE_END:
+		case PHASE_RESET:
+		case PHASE_DO_RESET:
+			data->cur_part = NULL;
+			data->phase = PHASE_DO_RESET;
+			return 0;
+		}
+		dfu_entity = stm32prog_get_entity(data);
+		if (!dfu_entity)
+			return -ENODEV;
+
+		if (data->dfu_seq) {
+			ret = dfu_flush(dfu_entity, NULL, 0, data->dfu_seq);
+			data->dfu_seq = 0;
+			if (ret) {
+				stm32prog_err("DFU flush failed [%d]", ret);
+				return ret;
+			}
+		}
+		printf("\n  received length = 0x%x\n", data->cursor);
+		if (data->header.present) {
+			if (data->cursor !=
+			    (data->header.image_length + BL_HEADER_SIZE)) {
+				stm32prog_err("transmission interrupted (length=0x%x expected=0x%x)",
+					      data->cursor,
+					      data->header.image_length +
+					      BL_HEADER_SIZE);
+				return -EIO;
+			}
+			if (data->header.image_checksum != data->checksum) {
+				stm32prog_err("invalid checksum received (0x%x expected 0x%x)",
+					      data->checksum,
+					      data->header.image_checksum);
+				return -EIO;
+			}
+			printf("\n  checksum OK (0x%x)\n", data->checksum);
+		}
+
+		stm32prog_end_phase(data);
+		/* update DFU with received flashlayout */
+		if (data->phase == PHASE_FLASHLAYOUT)
+			stm32prog_dfu_init(data);
+		/* found next selected partition */
+		stm32prog_next_phase(data);
+	} else {
+		void (*entry)(void) = (void *)address;
+
+		printf("## Starting application at 0x%x ...\n", address);
+		(*entry)();
+		printf("## Application terminated\n");
+		ret = -ENOEXEC;
+	}
+	/*pr_debug("%s exit ret=%d, phase=0x%x, add=0x%x\n", __func__,
+	 *	 ret, data->phase, address);
+	 */
+	return ret;
+}
+
+/*
+ * Function Name  : get_address
+ * Description    : Get address if it is valid
+ * Input          : None
+ * Output         : None
+ * Return         : The address area or Error_32
+ */
+static u32 get_address(u8 *tmp_xor)
+{
+	u32 address = 0x0;
+	u8 data;
+
+	data = stm32prog_serial_getc();
+	*tmp_xor ^= data;
+	address |= ((u32)data) << 24;
+
+	data = stm32prog_serial_getc();
+	address |= ((u32)data) << 16;
+	*tmp_xor ^= data;
+
+	data = stm32prog_serial_getc();
+	address |= ((u32)data) << 8;
+	*tmp_xor ^= data;
+
+	data = stm32prog_serial_getc();
+	address |= ((u32)data);
+	*tmp_xor ^= data;
+
+	return address;
+}
+
+static void stm32prog_serial_result(u8 result)
+{
+	/* always flush fifo before to send result */
+	stm32prog_serial_flush();
+	stm32prog_serial_putc(result);
+}
+
+/* Command -----------------------------------------------*/
+/*
+ * Function Name  : get_cmd_command
+ * Description    : Respond to Get command
+ * Input          : None
+ * Output         : None
+ * Return         : None
+ */
+static void get_cmd_command(struct stm32prog_data *data)
+{
+	u32 counter = 0x0;
+
+	stm32prog_serial_putc(NB_CMD);
+	stm32prog_serial_putc(USART_BL_VERSION);
+
+	for (counter = 0; counter < NB_CMD; counter++)
+		stm32prog_serial_putc(cmd_id[counter]);
+
+	stm32prog_serial_result(ACK_BYTE);
+}
+
+/*
+ * Function Name  : get_version_command
+ * Description    : Respond to Get Version command
+ * Input          : None
+ * Output         : None
+ * Return         : None
+ */
+static void get_version_command(struct stm32prog_data *data)
+{
+	stm32prog_serial_putc(UBOOT_BL_VERSION);
+	stm32prog_serial_result(ACK_BYTE);
+}
+
+/*
+ * Function Name  : get_id_command
+ * Description    : Respond to Get ID command
+ * Input          : None
+ * Output         : None
+ * Return         : None
+ */
+static void get_id_command(struct stm32prog_data *data)
+{
+	/* Send Device IDCode */
+	stm32prog_serial_putc(0x1);
+	stm32prog_serial_putc(DEVICE_ID_BYTE1);
+	stm32prog_serial_putc(DEVICE_ID_BYTE2);
+	stm32prog_serial_result(ACK_BYTE);
+}
+
+/*
+ * Function Name  : get_phase_command
+ * Description    : Respond to Get phase
+ * Input          : None
+ * Output         : None
+ * Return         : None
+ */
+static void get_phase_command(struct stm32prog_data *data)
+{
+	char *err_msg = NULL;
+	u8 i, length = 0;
+	u32 destination = DEFAULT_ADDRESS; /* destination address */
+	int phase = data->phase;
+
+	if (phase == PHASE_RESET || phase == PHASE_DO_RESET) {
+		err_msg = stm32prog_get_error(data);
+		length = strlen(err_msg);
+	}
+	if (phase == PHASE_FLASHLAYOUT)
+		destination = STM32_DDR_BASE;
+
+	stm32prog_serial_putc(length + 5);           /* Total length */
+	stm32prog_serial_putc(phase & 0xFF);         /* partition ID */
+	stm32prog_serial_putc(destination);          /* byte 1 of address */
+	stm32prog_serial_putc(destination >> 8);     /* byte 2 of address */
+	stm32prog_serial_putc(destination >> 16);    /* byte 3 of address */
+	stm32prog_serial_putc(destination >> 24);    /* byte 4 of address */
+
+	stm32prog_serial_putc(length);               /* Information length */
+	for (i = 0; i < length; i++)
+		stm32prog_serial_putc(err_msg[i]);
+	stm32prog_serial_result(ACK_BYTE);
+
+	if (phase == PHASE_RESET)
+		stm32prog_do_reset(data);
+}
+
+/*
+ * Function Name  : read_memory_command
+ * Description    : Read data from memory
+ * Input          : None
+ * Output         : None
+ * Return         : None
+ */
+static void read_memory_command(struct stm32prog_data *data)
+{
+	u32 address = 0x0;
+	u8 rcv_data = 0x0, tmp_xor = 0x0;
+	u32 counter = 0x0;
+
+	/* Read memory address */
+	address = get_address(&tmp_xor);
+
+	/* If address memory is not received correctly */
+	rcv_data = stm32prog_serial_getc();
+	if (rcv_data != tmp_xor) {
+		stm32prog_serial_result(NACK_BYTE);
+		return;
+	}
+
+	stm32prog_serial_result(ACK_BYTE);
+
+	/* Read the number of bytes to be received:
+	 * Max NbrOfData = Data + 1 = 256
+	 */
+	rcv_data = stm32prog_serial_getc();
+	tmp_xor = ~rcv_data;
+	if (stm32prog_serial_getc() != tmp_xor) {
+		stm32prog_serial_result(NACK_BYTE);
+		return;
+	}
+
+	/* If checksum is correct send ACK */
+	stm32prog_serial_result(ACK_BYTE);
+
+	/* Send data to the host:
+	 * Number of data to read = data + 1
+	 */
+	for (counter = (rcv_data + 1); counter != 0; counter--)
+		stm32prog_serial_putc(*(u8 *)(address++));
+}
+
+/*
+ * Function Name  : start_command
+ * Description    : Jump to user application in RAM or partition check
+ * Input          : None
+ * Output         : None
+ * Return         : None
+ */
+static void start_command(struct stm32prog_data *data)
+{
+	u32 address = 0;
+	u8 tmp_xor = 0x0;
+	u8 ret, rcv_data;
+
+	/* Read memory address */
+	address = get_address(&tmp_xor);
+
+	/* If address memory is not received correctly */
+	rcv_data = stm32prog_serial_getc();
+	if (rcv_data != tmp_xor) {
+		stm32prog_serial_result(NACK_BYTE);
+		return;
+	}
+	/* validate partition */
+	ret = stm32prog_start(data,
+			      address);
+
+	if (ret)
+		stm32prog_serial_result(ABORT_BYTE);
+	else
+		stm32prog_serial_result(ACK_BYTE);
+}
+
+/*
+ * Function Name  : download_command
+ * Description    : Write data to Flash
+ * Input          : None
+ * Output         : None
+ * Return         : Result
+ */
+static void download_command(struct stm32prog_data *data)
+{
+	u32 address = 0x0;
+	u8 my_xor = 0x0;
+	u8 rcv_xor;
+	u32 counter = 0x0, codesize = 0x0;
+	u8 *ramaddress = 0;
+	u8 rcv_data = 0x0;
+	struct image_header_s *image_header = &data->header;
+	u32 cursor = data->cursor;
+	long size = 0;
+	u8 operation;
+	u32 packet_number;
+	u32 result = ACK_BYTE;
+	u8 ret;
+	int i;
+	bool error;
+	int rcv;
+
+	address = get_address(&my_xor);
+
+	/* If address memory is not received correctly */
+	rcv_xor = stm32prog_serial_getc();
+	if (rcv_xor != my_xor) {
+		result = NACK_BYTE;
+		goto end;
+	}
+
+	/* If address valid send ACK */
+	stm32prog_serial_result(ACK_BYTE);
+
+	/* get packet number and operation type */
+	operation = (u8)((u32)address >> 24);
+	packet_number = ((u32)(((u32)address << 8))) >> 8;
+
+	switch (operation) {
+	/* supported operation */
+	case PHASE_FLASHLAYOUT:
+	case PHASE_OTP:
+	case PHASE_PMIC:
+		break;
+	default:
+		result = NACK_BYTE;
+		goto end;
+	}
+	/* check the packet number */
+	if (packet_number == 0) {
+		/* erase: re-initialize the image_header struct */
+		data->packet_number = 0;
+		if (data->header_data)
+			memset(data->header_data, 0, BL_HEADER_SIZE);
+		else
+			data->header_data = calloc(1, BL_HEADER_SIZE);
+		cursor = 0;
+		data->cursor = 0;
+		data->checksum = 0;
+		/*idx = cursor;*/
+	} else {
+		data->packet_number++;
+	}
+
+	/* Check with the number of current packet if the device receive
+	 * the true packet
+	 */
+	if (packet_number != data->packet_number) {
+		data->packet_number--;
+		result = NACK_BYTE;
+		goto end;
+	}
+
+	/*-- Read number of bytes to be written and data -----------*/
+
+	/* Read the number of bytes to be written:
+	 * Max NbrOfData = data + 1 <= 256
+	 */
+	rcv_data = stm32prog_serial_getc();
+
+	/* NbrOfData to write = data + 1 */
+	codesize = rcv_data + 0x01;
+
+	if (codesize > USART_RAM_BUFFER_SIZE) {
+		result = NACK_BYTE;
+		goto end;
+	}
+
+	/* Checksum Initialization */
+	my_xor = rcv_data;
+
+	/* UART receive data and send to Buffer */
+	counter = codesize;
+	error = stm32prog_serial_get_buffer(data->buffer, &counter);
+
+	/* read checksum */
+	if (!error) {
+		rcv = stm32prog_serial_getc_err();
+		error = !!(rcv < 0);
+		rcv_xor = rcv;
+	}
+
+	if (error) {
+		printf("transmission error on packet %d, byte %d\n",
+		       packet_number, codesize - counter);
+		/* waiting end of packet before flush & NACK */
+		mdelay(30);
+		data->packet_number--;
+		result = NACK_BYTE;
+		goto end;
+	}
+
+	/* Compute Checksum */
+	ramaddress = data->buffer;
+	for (counter = codesize; counter != 0; counter--)
+		my_xor ^= *(ramaddress++);
+
+	/* If Checksum is incorrect */
+	if (rcv_xor != my_xor) {
+		printf("checksum error on packet %d\n",
+		       packet_number);
+		data->packet_number--;
+		result = NACK_BYTE;
+		goto end;
+	}
+
+	/* Update current position in buffer */
+	data->cursor += codesize;
+
+	if (operation == PHASE_OTP) {
+		size = data->cursor - cursor;
+		/* no header for OTP */
+		if (stm32prog_otp_write(data, cursor,
+					data->buffer, &size))
+			result = ABORT_BYTE;
+		goto end;
+	}
+
+	if (operation == PHASE_PMIC) {
+		size = data->cursor - cursor;
+		/* no header for PMIC */
+		if (stm32prog_pmic_write(data, cursor,
+					 data->buffer, &size))
+			result = ABORT_BYTE;
+		goto end;
+	}
+
+	if (cursor < BL_HEADER_SIZE) {
+		/* size = portion of header in this chunck */
+		if (data->cursor >= BL_HEADER_SIZE)
+			size = BL_HEADER_SIZE - cursor;
+		else
+			size = data->cursor - cursor;
+		memcpy((void *)((u32)(data->header_data) + cursor),
+		       data->buffer, size);
+		cursor += size;
+
+		if (cursor == BL_HEADER_SIZE) {
+			/* Check and Write the header */
+			if (stm32prog_header(data)) {
+				result = ABORT_BYTE;
+				goto end;
+			}
+		} else {
+			goto end;
+		}
+	}
+	/*
+	 * pr_debug("packet_number = 0x%x\n", packet_number);
+	 * pr_debug("cursor = 0x%x\n", data->cursor);
+	 * pr_debug("image_length = 0x%x\n", image_header->image_length);
+	 * pr_debug("codesize = 0x%x\n", codesize);
+	 */
+	if (image_header->present) {
+		if (data->cursor <= BL_HEADER_SIZE)
+			goto end;
+		/* compute checksum on payload */
+		for (i = size; i < codesize; i++)
+			data->checksum += data->buffer[i];
+
+		if (data->cursor >
+		    image_header->image_length + BL_HEADER_SIZE) {
+			pr_err("expected size exceeded\n");
+			result = ABORT_BYTE;
+			goto end;
+		}
+
+		/* write data (payload) */
+		ret = stm32prog_write(data,
+				      &data->buffer[size],
+				      codesize - size);
+	} else {
+		/* write all */
+		ret = stm32prog_write(data,
+				      data->buffer,
+				      codesize);
+	}
+	if (ret)
+		result = ABORT_BYTE;
+
+end:
+	/*pr_debug("%s : result = 0x%x\n", __func__, result);*/
+	stm32prog_serial_result(result);
+}
+
+/*
+ * Function Name  : read_partition
+ * Description    : read data from Flash
+ * Input          : None
+ * Output         : None
+ * Return         : Result
+ */
+static void read_partition_command(struct stm32prog_data *data)
+{
+	u32 part_id, codesize, offset = 0, rcv_data;
+	long size;
+	u8 tmp_xor;
+	int i, res;
+	u8 buffer[256];
+
+	part_id = stm32prog_serial_getc();
+	tmp_xor = part_id;
+
+	offset = get_address(&tmp_xor);
+
+	rcv_data = stm32prog_serial_getc();
+	if (rcv_data != tmp_xor) {
+		pr_debug("1st checksum received = %x, computed %x\n",
+			 rcv_data, tmp_xor);
+		goto error;
+	}
+	stm32prog_serial_putc(ACK_BYTE);
+
+	/* NbrOfData to read = data + 1 */
+	rcv_data = stm32prog_serial_getc();
+	codesize = rcv_data + 0x01;
+	tmp_xor = rcv_data;
+
+	rcv_data = stm32prog_serial_getc();
+	if ((rcv_data ^ tmp_xor) != 0xFF) {
+		pr_debug("2nd checksum received = %x, computed %x\n",
+			 rcv_data, tmp_xor);
+		goto error;
+	}
+
+	pr_debug("%s : %x\n", __func__, part_id);
+	switch (part_id) {
+	case PHASE_OTP:
+		res = 0;
+		size = codesize;
+		if (!stm32prog_otp_read(data, offset, buffer, &size))
+			res = size;
+		break;
+	case PHASE_PMIC:
+		res = 0;
+		size = codesize;
+		if (!stm32prog_pmic_read(data, offset, buffer, &size))
+			res = size;
+		break;
+	default:
+		res = stm32prog_read(data, part_id, offset,
+				     buffer, codesize);
+		break;
+	}
+	if (res > 0) {
+		stm32prog_serial_putc(ACK_BYTE);
+		/*----------- Send data to the host -----------*/
+		for (i = 0; i < res; i++)
+			stm32prog_serial_putc(buffer[i]);
+		/*----------- Send filler to the host -----------*/
+		for (; i < codesize; i++)
+			stm32prog_serial_putc(0x0);
+		return;
+	}
+	stm32prog_serial_result(ABORT_BYTE);
+	return;
+
+error:
+	stm32prog_serial_result(NACK_BYTE);
+}
+
+/** SERIAL LOOP ****************************************************/
+
+/*
+ * Function Name  : stm32prog_serial_loop
+ * Description    : USART bootloader Loop routine
+ * Input          : data
+ * Output         : None
+ * Return         : None
+ */
+bool stm32prog_serial_loop(struct stm32prog_data *data)
+{
+	u32 counter = 0x0;
+	u8 command = 0x0;
+	u8 found;
+	int phase = data->phase;
+
+	/* element of cmd_func need to aligned with cmd_id[]*/
+	void (*cmd_func[NB_CMD])(struct stm32prog_data *) = {
+		/* GET_CMD_COMMAND */	get_cmd_command,
+		/* GET_VER_COMMAND */	get_version_command,
+		/* GET_ID_COMMAND */	get_id_command,
+		/* GET_PHASE_COMMAND */	get_phase_command,
+		/* RM_COMMAND */	read_memory_command,
+		/* READ_PART_COMMAND */	read_partition_command,
+		/* START_COMMAND */	start_command,
+		/* DOWNLOAD_COMMAND */	download_command
+	};
+
+	/* flush and NACK pending command received during u-boot init
+	 * request command reemit
+	 */
+	stm32prog_serial_result(NACK_BYTE);
+
+	clear_ctrlc(); /* forget any previous Control C */
+	while (!had_ctrlc()) {
+		phase = data->phase;
+
+		if (phase == PHASE_DO_RESET)
+			return true;
+
+		/* Get the user command: read first byte */
+		command = stm32prog_serial_getc();
+
+		if (command == INIT_BYTE) {
+			puts("\nConnected\n");
+			stm32prog_serial_result(ACK_BYTE);
+			continue;
+		}
+
+		found = 0;
+		for (counter = 0; counter < NB_CMD; counter++)
+			if (cmd_id[counter] == command) {
+				found = 1;
+				break;
+			}
+		if (found)
+			if ((command ^ stm32prog_serial_getc()) != 0xFF)
+				found = 0;
+		if (!found) {
+			/* wait to be sure that all data are received
+			 * in the FIFO before flush (CMD and XOR)
+			 */
+			mdelay(2);
+			stm32prog_serial_result(NACK_BYTE);
+		} else {
+			/*pr_debug("+ cmd %x\n", counter);*/
+			stm32prog_serial_result(ACK_BYTE);
+			cmd_func[counter](data);
+		}
+		WATCHDOG_RESET();
+	}
+
+	/* clean device */
+	if (gd->cur_serial_dev == down_serial_dev) {
+		/* restore console on uart */
+		gd->flags &= ~(GD_FLG_DISABLE_CONSOLE | GD_FLG_SILENT);
+	}
+	down_serial_dev = NULL;
+
+	return false; /* no reset after ctrlc */
+}
diff --git a/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog_usb.c b/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog_usb.c
new file mode 100644
index 0000000..c7dd678
--- /dev/null
+++ b/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog_usb.c
@@ -0,0 +1,278 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <console.h>
+#include <dfu.h>
+#include <g_dnl.h>
+#include <usb.h>
+#include <watchdog.h>
+#include "stm32prog.h"
+
+struct stm32prog_data *stm32prog_data;
+
+static int stm32prog_get_alternate(struct stm32prog_data *data)
+{
+	if (data->cur_part)
+		return data->cur_part->alt_id;
+	else
+		return -EINVAL;
+}
+
+static int stm32prog_set_phase(struct stm32prog_data *data, u8 phase,
+			       u32 offset)
+{
+	struct stm32prog_part_t *part;
+	int i;
+
+	if (phase == data->phase) {
+		data->offset = offset;
+		data->dfu_seq = 0;
+		return 0;
+	}
+
+	/* found partition */
+	for (i = 0; i < data->part_nb; i++) {
+		part = &data->part_array[i];
+		if (part->id == phase) {
+			data->cur_part = part;
+			data->phase = phase;
+			data->offset = offset;
+			data->dfu_seq = 0;
+			return 0;
+		}
+	}
+
+	return  -EINVAL;
+}
+
+static int stm32prog_cmd_write(u64 offset, void *buf, long *len)
+{
+	u8 phase;
+	u32 address;
+	u8 *pt = buf;
+	void (*entry)(void);
+	int ret;
+
+	if (*len < 5) {
+		pr_err("size not allowed\n");
+		return  -EINVAL;
+	}
+	if (offset) {
+		pr_err("invalid offset\n");
+		return  -EINVAL;
+	}
+	phase = pt[0];
+	address = (pt[1] << 24) | (pt[2] << 16) | (pt[3] << 8) | pt[4];
+	if (phase == PHASE_RESET) {
+		entry = (void *)address;
+		printf("## Starting application at 0x%x ...\n", address);
+		(*entry)();
+		printf("## Application terminated\n");
+		return 0;
+	}
+	/* set phase and offset */
+	ret = stm32prog_set_phase(stm32prog_data, phase, address);
+	if (ret)
+		pr_err("failed: %d\n", ret);
+	return ret;
+}
+
+#define PHASE_MIN_SIZE	9
+static int stm32prog_cmd_read(u64 offset, void *buf, long *len)
+{
+	u32 destination = DEFAULT_ADDRESS; /* destination address */
+	u32 dfu_offset;
+	u8 *pt_buf = buf;
+	int phase;
+	char *err_msg;
+	int length;
+
+	if (*len < PHASE_MIN_SIZE) {
+		pr_err("request exceeds allowed area\n");
+		return  -EINVAL;
+	}
+	if (offset) {
+		*len = 0; /* EOF for second request */
+		return 0;
+	}
+	phase = stm32prog_data->phase;
+	if (phase == PHASE_FLASHLAYOUT)
+		destination = STM32_DDR_BASE;
+	dfu_offset = stm32prog_data->offset;
+
+	/* mandatory header, size = PHASE_MIN_SIZE */
+	*pt_buf++ = (u8)(phase & 0xFF);
+	*pt_buf++ = (u8)(destination);
+	*pt_buf++ = (u8)(destination >> 8);
+	*pt_buf++ = (u8)(destination >> 16);
+	*pt_buf++ = (u8)(destination >> 24);
+	*pt_buf++ = (u8)(dfu_offset);
+	*pt_buf++ = (u8)(dfu_offset >> 8);
+	*pt_buf++ = (u8)(dfu_offset >> 16);
+	*pt_buf++ = (u8)(dfu_offset >> 24);
+
+	if (phase == PHASE_RESET || phase == PHASE_DO_RESET) {
+		err_msg = stm32prog_get_error(stm32prog_data);
+		length = strlen(err_msg);
+		if (length + PHASE_MIN_SIZE > *len)
+			length = *len - PHASE_MIN_SIZE;
+
+		memcpy(pt_buf, err_msg, length);
+		*len = PHASE_MIN_SIZE + length;
+		stm32prog_do_reset(stm32prog_data);
+	} else if (phase == PHASE_FLASHLAYOUT) {
+		*pt_buf++ = stm32prog_data->part_nb ? 1 : 0;
+		*len = PHASE_MIN_SIZE + 1;
+	} else {
+		*len = PHASE_MIN_SIZE;
+	}
+
+	return 0;
+}
+
+/* DFU access to virtual partition */
+
+void dfu_flush_callback(struct dfu_entity *dfu)
+{
+	if (!stm32prog_data)
+		return;
+
+	if (dfu->dev_type == DFU_DEV_VIRT) {
+		if (dfu->data.virt.dev_num == PHASE_OTP)
+			stm32prog_otp_start(stm32prog_data);
+		else if (dfu->data.virt.dev_num == PHASE_PMIC)
+			stm32prog_pmic_start(stm32prog_data);
+		return;
+	}
+
+	if (dfu->dev_type == DFU_DEV_RAM) {
+		if (dfu->alt == 0 &&
+		    stm32prog_data->phase == PHASE_FLASHLAYOUT) {
+			stm32prog_end_phase(stm32prog_data);
+			/* waiting DFU DETACH for reenumeration */
+		}
+		return;
+	}
+
+	if (dfu->alt == stm32prog_get_alternate(stm32prog_data)) {
+		stm32prog_end_phase(stm32prog_data);
+		stm32prog_next_phase(stm32prog_data);
+	}
+}
+
+void dfu_initiated_callback(struct dfu_entity *dfu)
+{
+	int phase;
+
+	if (!stm32prog_data)
+		return;
+
+	phase = stm32prog_data->phase;
+	if (dfu->alt == stm32prog_get_alternate(stm32prog_data)) {
+		dfu->offset = stm32prog_data->offset;
+		stm32prog_set_phase(stm32prog_data, phase, 0);
+		pr_debug("dfu offset = 0x%llx\n", dfu->offset);
+	}
+}
+
+int dfu_write_medium_virt(struct dfu_entity *dfu, u64 offset,
+			  void *buf, long *len)
+{
+	if (dfu->dev_type != DFU_DEV_VIRT)
+		return -EINVAL;
+
+	switch (dfu->data.virt.dev_num) {
+	case PHASE_CMD:
+		return stm32prog_cmd_write(offset, buf, len);
+
+	case PHASE_OTP:
+		return stm32prog_otp_write(stm32prog_data, (u32)offset,
+					   buf, len);
+
+	case PHASE_PMIC:
+		return stm32prog_pmic_write(stm32prog_data, (u32)offset,
+					    buf, len);
+	}
+	*len = 0;
+	return 0;
+}
+
+int dfu_read_medium_virt(struct dfu_entity *dfu, u64 offset,
+			 void *buf, long *len)
+{
+	if (dfu->dev_type != DFU_DEV_VIRT)
+		return -EINVAL;
+
+	switch (dfu->data.virt.dev_num) {
+	case PHASE_CMD:
+		return stm32prog_cmd_read(offset, buf, len);
+
+	case PHASE_OTP:
+		return stm32prog_otp_read(stm32prog_data, (u32)offset,
+					  buf, len);
+
+	case PHASE_PMIC:
+		return stm32prog_pmic_read(stm32prog_data, (u32)offset,
+					   buf, len);
+	}
+	*len = 0;
+	return 0;
+}
+
+int dfu_get_medium_size_virt(struct dfu_entity *dfu, u64 *size)
+{
+	if (dfu->dev_type != DFU_DEV_VIRT) {
+		*size = 0;
+		pr_debug("%s, invalid dev_type = %d\n",
+			 __func__, dfu->dev_type);
+		return -EINVAL;
+	}
+
+	switch (dfu->data.virt.dev_num) {
+	case PHASE_CMD:
+		*size = 512;
+		break;
+	case PHASE_OTP:
+		*size = OTP_SIZE;
+		break;
+	case PHASE_PMIC:
+		*size = PMIC_SIZE;
+		break;
+	}
+
+	return 0;
+}
+
+/* USB download gadget for STM32 Programmer */
+
+static const char product[] =
+	"USB download gadget@Device ID /0x500, @Revision ID /0x0000";
+
+bool stm32prog_usb_loop(struct stm32prog_data *data, int dev)
+{
+	int ret;
+
+	stm32prog_data = data;
+	g_dnl_set_product(product);
+	if (stm32prog_data->phase == PHASE_FLASHLAYOUT) {
+		ret = run_usb_dnl_gadget(dev, "usb_dnl_dfu");
+		if (ret || stm32prog_data->phase == PHASE_DO_RESET)
+			return ret;
+		/* prepare the second enumeration with the FlashLayout */
+		if (stm32prog_data->phase == PHASE_FLASHLAYOUT)
+			stm32prog_dfu_init(data);
+		/* found next selected partition */
+		stm32prog_next_phase(data);
+	}
+	return (run_usb_dnl_gadget(dev, "usb_dnl_dfu") ||
+		(stm32prog_data->phase == PHASE_DO_RESET));
+}
+
+int g_dnl_get_board_bcd_device_number(int gcnum)
+{
+	pr_debug("%s\n", __func__);
+	return 0x200;
+}
diff --git a/arch/arm/mach-stm32mp/config.mk b/arch/arm/mach-stm32mp/config.mk
index cde5850..bd8944a 100644
--- a/arch/arm/mach-stm32mp/config.mk
+++ b/arch/arm/mach-stm32mp/config.mk
@@ -3,7 +3,21 @@
 # Copyright (C) 2018, STMicroelectronics - All Rights Reserved
 #
 
-ALL-$(CONFIG_SPL_BUILD) += spl/u-boot-spl.stm32
+ifndef CONFIG_SPL
+ALL-y += u-boot.stm32
+else
+ifdef CONFIG_SPL_BUILD
+ALL-y += spl/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 0e01f8e..5d5ce4a 100644
--- a/arch/arm/mach-stm32mp/cpu.c
+++ b/arch/arm/mach-stm32mp/cpu.c
@@ -6,18 +6,23 @@
 #include <clk.h>
 #include <debug_uart.h>
 #include <environment.h>
+#include <fdt_support.h>
 #include <misc.h>
 #include <asm/io.h>
 #include <asm/arch/stm32.h>
 #include <asm/arch/sys_proto.h>
 #include <dm/device.h>
+#include <dm/lists.h>
 #include <dm/uclass.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <dt-bindings/pinctrl/stm32-pinfunc.h>
 
 /* RCC register */
 #define RCC_TZCR		(STM32_RCC_BASE + 0x00)
 #define RCC_DBGCFGR		(STM32_RCC_BASE + 0x080C)
 #define RCC_BDCR		(STM32_RCC_BASE + 0x0140)
 #define RCC_MP_APB5ENSETR	(STM32_RCC_BASE + 0x0208)
+
 #define RCC_BDCR_VSWRST		BIT(31)
 #define RCC_BDCR_RTCSRC		GENMASK(17, 16)
 #define RCC_DBGCFGR_DBGCKEN	BIT(8)
@@ -55,10 +60,34 @@
 #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
+
+DECLARE_GLOBAL_DATA_PTR;
+
 #if !defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD)
+#ifndef CONFIG_STM32MP1_TRUSTED
 static void security_init(void)
 {
 	/* Disable the backup domain write protection */
@@ -114,6 +143,7 @@ static void security_init(void)
 	 */
 	writel(0x0, TAMP_CR1);
 }
+#endif /* CONFIG_STM32MP1_TRUSTED */
 
 /*
  * Debug init
@@ -130,10 +160,12 @@ static void dbgmcu_init(void)
 static u32 get_bootmode(void)
 {
 	u32 boot_mode;
-#if !defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD)
+#if !defined(CONFIG_STM32MP1_TRUSTED) && \
+	(!defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD))
 	u32 bootrom_itf = readl(BOOTROM_PARAM_ADDR);
 	u32 bootrom_device, bootrom_instance;
 
+	/* read bootrom context */
 	bootrom_device =
 		(bootrom_itf & BOOTROM_MODE_MASK) >> BOOTROM_MODE_SHIFT;
 	bootrom_instance =
@@ -167,16 +199,18 @@ int arch_cpu_init(void)
 
 #if !defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD)
 	dbgmcu_init();
-
+#ifndef CONFIG_STM32MP1_TRUSTED
 	security_init();
 #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) && \
+#if defined(CONFIG_DEBUG_UART) &&\
+	!defined(CONFIG_STM32MP1_TRUSTED) && \
 	(!defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD))
 	else
 		debug_uart_init();
@@ -203,25 +237,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 +337,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 +345,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);
-
+	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");
@@ -268,17 +399,44 @@ static void setup_boot_mode(void)
 		env_set("boot_instance", cmd);
 		break;
 	case BOOT_FLASH_NAND:
+		sprintf(cmd, "%d", instance);
 		env_set("boot_device", "nand");
-		env_set("boot_instance", "0");
+		env_set("boot_instance", cmd);
 		break;
 	case BOOT_FLASH_NOR:
 		env_set("boot_device", "nor");
 		env_set("boot_instance", "0");
 		break;
+	}
+
+	switch (forced_mode) {
+	case BOOT_FASTBOOT:
+		printf("Enter fastboot!\n");
+		env_set("preboot", "env set preboot; fastboot 0");
+		break;
+	case BOOT_STM32PROG:
+		printf("Enter STM32CubeProgrammer mode!\n");
+		env_set("preboot", "env set preboot; stm32prog usb 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:
 	default:
 		pr_debug("unexpected boot mode = %x\n", boot_mode);
 		break;
 	}
+
+	/* clear TAMP for next reboot */
+	clrsetbits_le32(TAMP_BOOT_CONTEXT, TAMP_BOOT_FORCED_MASK, BOOT_NORMAL);
 }
 
 /*
@@ -304,7 +462,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)
 		return ret;
@@ -342,12 +500,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)
 		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;
@@ -361,3 +519,45 @@ int arch_misc_init(void)
 
 	return 0;
 }
+
+/*
+ * This function is called right before the kernel is booted. "blob" is the
+ * device tree that will be passed to the kernel.
+ */
+int ft_system_setup(void *blob, bd_t *bd)
+{
+	int ret = 0;
+	u32 pkg;
+
+#if CONFIG_STM32_ETZPC
+	ret = stm32_fdt_fixup_etzpc(blob);
+	if (ret)
+		return ret;
+#endif
+
+	switch (get_cpu_package()) {
+	case PKG_AA_LBGA448:
+		pkg = STM32MP157CAA;
+		break;
+	case PKG_AB_LBGA354:
+		pkg = STM32MP157CAB;
+		break;
+	case PKG_AC_TFBGA361:
+		pkg = STM32MP157CAC;
+		break;
+	case PKG_AD_TFBGA257:
+		pkg = STM32MP157CAD;
+		break;
+	default:
+		pkg = 0;
+		break;
+	}
+	if (pkg) {
+		do_fixup_by_compat_u32(blob, "st,stm32mp157-pinctrl",
+				       "st,package", pkg, false);
+		do_fixup_by_compat_u32(blob, "st,stm32mp157-z-pinctrl",
+				       "st,package", pkg, false);
+	}
+
+	return ret;
+}
diff --git a/arch/arm/mach-stm32mp/include/mach/ddr.h b/arch/arm/mach-stm32mp/include/mach/ddr.h
index 1857584..b8a17cf 100644
--- a/arch/arm/mach-stm32mp/include/mach/ddr.h
+++ b/arch/arm/mach-stm32mp/include/mach/ddr.h
@@ -6,6 +6,13 @@
 #ifndef __MACH_STM32MP_DDR_H_
 #define __MACH_STM32MP_DDR_H_
 
-int board_ddr_power_init(void);
+/* DDR power initializations */
+enum ddr_type {
+	STM32MP_DDR3,
+	STM32MP_LPDDR2,
+	STM32MP_LPDDR3,
+};
+
+int board_ddr_power_init(enum ddr_type ddr_type);
 
 #endif
diff --git a/arch/arm/mach-stm32mp/include/mach/gpio.h b/arch/arm/mach-stm32mp/include/mach/gpio.h
index 5151150..5ca76d2 100644
--- a/arch/arm/mach-stm32mp/include/mach/gpio.h
+++ b/arch/arm/mach-stm32mp/include/mach/gpio.h
@@ -8,6 +8,8 @@
 #define _STM32_GPIO_H_
 #include <asm/gpio.h>
 
+#define STM32_GPIOS_PER_BANK		16
+
 enum stm32_gpio_port {
 	STM32_GPIO_PORT_A = 0,
 	STM32_GPIO_PORT_B,
@@ -110,5 +112,9 @@ struct stm32_gpio_regs {
 
 struct stm32_gpio_priv {
 	struct stm32_gpio_regs *regs;
+	unsigned int gpio_range;
 };
+
+int stm32_offset_to_index(struct udevice *dev, unsigned int offset);
+
 #endif /* _STM32_GPIO_H_ */
diff --git a/arch/arm/mach-stm32mp/include/mach/stm32.h b/arch/arm/mach-stm32mp/include/mach/stm32.h
index 5d0bdca..4147873 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,20 +25,27 @@
 #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
 
+#define STM32_MCU_SRAM_BASE		0x30000000
+#define STM32_MCU_SRAM_SIZE		(3 * SZ_128K)
+
 #define STM32_DDR_BASE			0xC0000000
 #define STM32_DDR_SIZE			SZ_1G
 
 #ifndef __ASSEMBLY__
+#include <asm/types.h>
+
 /* enumerated used to identify the SYSCON driver instance */
 enum {
 	STM32MP_SYSCON_UNKNOWN,
-	STM32MP_SYSCON_STGEN,
+	STM32MP_SYSCON_ETZPC,
+	STM32MP_SYSCON_IWDG,
 	STM32MP_SYSCON_PWR,
+	STM32MP_SYSCON_STGEN,
+	STM32MP_SYSCON_SYSCFG,
 };
 
 /*
@@ -87,18 +91,41 @@ enum boot_device {
 
 /* TAMP registers */
 #define TAMP_BACKUP_REGISTER(x)		(STM32_TAMP_BASE + 0x100 + 4 * x)
+/* secure access */
 #define TAMP_BACKUP_MAGIC_NUMBER	TAMP_BACKUP_REGISTER(4)
 #define TAMP_BACKUP_BRANCH_ADDRESS	TAMP_BACKUP_REGISTER(5)
+/* non secure access */
 #define TAMP_BOOT_CONTEXT		TAMP_BACKUP_REGISTER(20)
+#define TAMP_BOOTCOUNT			TAMP_BACKUP_REGISTER(21)
 
 #define TAMP_BOOT_MODE_MASK		GENMASK(15, 8)
 #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)
+#define TAMP_BOOT_DEBUG_ON		BIT(16)
+
+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
+
+#if CONFIG_STM32_ETZPC
+int stm32_fdt_fixup_etzpc(void *fdt);
+#endif
 
 #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 0000000..538958f
--- /dev/null
+++ b/arch/arm/mach-stm32mp/include/mach/stm32mp1_smc.h
@@ -0,0 +1,75 @@
+/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
+/*
+ * Copyright (C) 2018, 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
+/* SMC reserved for future ST services */
+#define STM32_SMC_RESERVED_ST		0x82000001
+
+/* Secure Service access from Non-secure */
+#define STM32_SMC_RCC			0x82001000
+#define STM32_SMC_PWR			0x82001001
+#define STM32_SMC_RTC			0x82001002
+#define STM32_SMC_BSEC			0x82001003
+
+/* Register access service use for RCC/RTC/PWR */
+#define STM32_SMC_REG_WRITE		0x1
+#define STM32_SMC_REG_SET		0x2
+#define STM32_SMC_REG_CLEAR		0x3
+
+/* 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 41d4b40..c565748 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
diff --git a/arch/arm/mach-stm32mp/psci.c b/arch/arm/mach-stm32mp/psci.c
index 6ed2482..c2dff38 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/pwr_regulator.c b/arch/arm/mach-stm32mp/pwr_regulator.c
index 9484645..dbfbdcc 100644
--- a/arch/arm/mach-stm32mp/pwr_regulator.c
+++ b/arch/arm/mach-stm32mp/pwr_regulator.c
@@ -8,6 +8,7 @@
 #include <errno.h>
 #include <regmap.h>
 #include <syscon.h>
+#include <asm/arch/stm32mp1_smc.h>
 #include <power/pmic.h>
 #include <power/regulator.h>
 
@@ -32,13 +33,20 @@ struct stm32mp_pwr_priv {
 static int stm32mp_pwr_write(struct udevice *dev, uint reg,
 			     const uint8_t *buff, int len)
 {
+#ifndef CONFIG_STM32MP1_TRUSTED
 	struct stm32mp_pwr_priv *priv = dev_get_priv(dev);
+#endif
 	u32 val = *(u32 *)buff;
 
 	if (len != 4)
 		return -EINVAL;
 
+#ifdef CONFIG_STM32MP1_TRUSTED
+	return stm32_smc_exec(STM32_SMC_PWR, STM32_SMC_REG_WRITE,
+			      STM32MP_PWR_CR3, val);
+#else /* CONFIG_STM32MP1_TRUSTED */
 	return regmap_write(priv->regmap, STM32MP_PWR_CR3, val);
+#endif /* CONFIG_STM32MP1_TRUSTED */
 }
 
 static int stm32mp_pwr_read(struct udevice *dev, uint reg, uint8_t *buff,
diff --git a/arch/arm/mach-stm32mp/spl.c b/arch/arm/mach-stm32mp/spl.c
index 790973e..fa393cc 100644
--- a/arch/arm/mach-stm32mp/spl.c
+++ b/arch/arm/mach-stm32mp/spl.c
@@ -7,6 +7,18 @@
 #include <dm.h>
 #include <spl.h>
 #include <asm/io.h>
+#include <asm/arch/sys_proto.h>
+#include <linux/libfdt.h>
+
+static int spl_board_load_image(struct spl_image_info *spl_image,
+				struct spl_boot_device *bootdev)
+{
+	/* TODO : add download support in SPL without TF-A */
+	return -1;
+}
+
+SPL_LOAD_IMAGE_METHOD("UART", 0, BOOT_DEVICE_UART, spl_board_load_image);
+SPL_LOAD_IMAGE_METHOD("USB", 0, BOOT_DEVICE_USB, spl_board_load_image);
 
 u32 spl_boot_device(void)
 {
@@ -22,8 +34,22 @@ 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 +70,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 +121,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/stm32-etzpc.c b/arch/arm/mach-stm32mp/stm32-etzpc.c
new file mode 100644
index 0000000..ea6dcca
--- /dev/null
+++ b/arch/arm/mach-stm32mp/stm32-etzpc.c
@@ -0,0 +1,199 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <common.h>
+#include <fdt_support.h>
+#include <syscon.h>
+#include <linux/err.h>
+#include <linux/io.h>
+
+#define ETZPC_DECPROT(n)	(0x10 + 4 * (n))
+#define ETZPC_DECPROT_NB	6
+
+#define ETZPC_IP_VER		0x3F4
+
+#define IP_VER_STM32MP1		0x00000020
+
+#define DECPROT_MASK		0x03
+#define NB_PROT_PER_REG		0x10
+#define DECPROT_NB_BITS		2
+
+#define DECPROT_SECURED		0x00
+#define DECPROT_WRITE_SECURE	0x01
+#define DECPROT_MCU_ISOLATION	0x02
+#define DECPROT_NON_SECURED	0x03
+
+#define ETZPC_RESERVED		0xffffffff
+
+static const u32 stm32mp1_ip_addr[] = {
+	0x5c008000,	/* 00 stgenc */
+	0x54000000,	/* 01 bkpsram */
+	0x5c003000,	/* 02 iwdg1 */
+	0x5c000000,	/* 03 usart1 */
+	0x5c001000,	/* 04 spi6 */
+	0x5c002000,	/* 05 i2c4 */
+	ETZPC_RESERVED,	/* 06 reserved */
+	0x54003000,	/* 07 rng1 */
+	0x54002000,	/* 08 hash1 */
+	0x54001000,	/* 09 cryp1 */
+	0x5a003000,	/* 0A ddrctrl */
+	0x5a004000,	/* 0B ddrphyc */
+	0x5c009000,	/* 0C i2c6 */
+	ETZPC_RESERVED,	/* 0D reserved */
+	ETZPC_RESERVED,	/* 0E reserved */
+	ETZPC_RESERVED,	/* 0F reserved */
+	0x40000000,	/* 10 tim2 */
+	0x40001000,	/* 11 tim3 */
+	0x40002000,	/* 12 tim4 */
+	0x40003000,	/* 13 tim5 */
+	0x40004000,	/* 14 tim6 */
+	0x40005000,	/* 15 tim7 */
+	0x40006000,	/* 16 tim12 */
+	0x40007000,	/* 17 tim13 */
+	0x40008000,	/* 18 tim14 */
+	0x40009000,	/* 19 lptim1 */
+	0x4000a000,	/* 1A wwdg1 */
+	0x4000b000,	/* 1B spi2 */
+	0x4000c000,	/* 1C spi3 */
+	0x4000d000,	/* 1D spdifrx */
+	0x4000e000,	/* 1E usart2 */
+	0x4000f000,	/* 1F usart3 */
+	0x40010000,	/* 20 uart4 */
+	0x40011000,	/* 21 uart5 */
+	0x40012000,	/* 22 i2c1 */
+	0x40013000,	/* 23 i2c2 */
+	0x40014000,	/* 24 i2c3 */
+	0x40015000,	/* 25 i2c5 */
+	0x40016000,	/* 26 cec */
+	0x40017000,	/* 27 dac */
+	0x40018000,	/* 28 uart7 */
+	0x40019000,	/* 29 uart8 */
+	ETZPC_RESERVED,	/* 2A reserved */
+	ETZPC_RESERVED,	/* 2B reserved */
+	0x4001c000,	/* 2C mdios */
+	ETZPC_RESERVED,	/* 2D reserved */
+	ETZPC_RESERVED,	/* 2E reserved */
+	ETZPC_RESERVED,	/* 2F reserved */
+	0x44000000,	/* 30 tim1 */
+	0x44001000,	/* 31 tim8 */
+	ETZPC_RESERVED,	/* 32 reserved */
+	0x44003000,	/* 33 usart6 */
+	0x44004000,	/* 34 spi1 */
+	0x44005000,	/* 35 spi4 */
+	0x44006000,	/* 36 tim15 */
+	0x44007000,	/* 37 tim16 */
+	0x44008000,	/* 38 tim17 */
+	0x44009000,	/* 39 spi5 */
+	0x4400a000,	/* 3A sai1 */
+	0x4400b000,	/* 3B sai2 */
+	0x4400c000,	/* 3C sai3 */
+	0x4400d000,	/* 3D dfsdm */
+	0x4400e000,	/* 3E tt_fdcan */
+	ETZPC_RESERVED,	/* 3F reserved */
+	0x50021000,	/* 40 lptim2 */
+	0x50022000,	/* 41 lptim3 */
+	0x50023000,	/* 42 lptim4 */
+	0x50024000,	/* 43 lptim5 */
+	0x50027000,	/* 44 sai4 */
+	0x50025000,	/* 45 vrefbuf */
+	0x4c006000,	/* 46 dcmi */
+	0x4c004000,	/* 47 crc2 */
+	0x48003000,	/* 48 adc */
+	0x4c002000,	/* 49 hash2 */
+	0x4c003000,	/* 4A rng2 */
+	0x4c005000,	/* 4B cryp2 */
+	ETZPC_RESERVED,	/* 4C reserved */
+	ETZPC_RESERVED,	/* 4D reserved */
+	ETZPC_RESERVED,	/* 4E reserved */
+	ETZPC_RESERVED,	/* 4F reserved */
+	ETZPC_RESERVED,	/* 50 sram1 */
+	ETZPC_RESERVED,	/* 51 sram2 */
+	ETZPC_RESERVED,	/* 52 sram3 */
+	ETZPC_RESERVED,	/* 53 sram4 */
+	ETZPC_RESERVED,	/* 54 retram */
+	0x49000000,	/* 55 otg */
+	0x48004000,	/* 56 sdmmc3 */
+	0x48005000,	/* 57 dlybsd3 */
+	0x48000000,	/* 58 dma1 */
+	0x48001000,	/* 59 dma2 */
+	0x48002000,	/* 5A dmamux */
+	0x58002000,	/* 5B fmc */
+	0x58003000,	/* 5C qspi */
+	0x58004000,	/* 5D dlybq */
+	0x5800a000,	/* 5E eth */
+	ETZPC_RESERVED,	/* 5F reserved */
+};
+
+/* fdt helper */
+static bool fdt_disable_subnode_by_address(void *fdt, int offset, u32 addr)
+{
+	int node;
+
+	for (node = fdt_first_subnode(fdt, offset);
+	     node >= 0;
+	     node = fdt_next_subnode(fdt, node)) {
+		if (addr == (u32)fdt_getprop(fdt, node, "reg", 0)) {
+			if (fdtdec_get_is_enabled(fdt, node)) {
+				fdt_status_disabled(fdt, node);
+
+				return true;
+			}
+			return false;
+		}
+	}
+
+	return false;
+}
+
+int stm32_fdt_fixup_etzpc(void *fdt)
+{
+	void *base;
+	u32 version;
+	const u32 *array;
+	int array_size, i;
+	int soc_node, offset, shift;
+	u32 addr, status, decprot[ETZPC_DECPROT_NB];
+
+	base = syscon_get_first_range(STM32MP_SYSCON_ETZPC);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	version = readl(base + ETZPC_IP_VER);
+
+	switch (version) {
+	case IP_VER_STM32MP1:
+		array = stm32mp1_ip_addr;
+		array_size = ARRAY_SIZE(stm32mp1_ip_addr);
+		break;
+	default:
+		return 0;
+	}
+
+	for (i = 0; i < ETZPC_DECPROT_NB; i++)
+		decprot[i] = readl(base + ETZPC_DECPROT(i));
+
+	soc_node = fdt_path_offset(fdt, "/soc");
+	if (soc_node < 0)
+		return soc_node;
+
+	for (i = 0; i < array_size; i++) {
+		offset = i / NB_PROT_PER_REG;
+		shift = (i % NB_PROT_PER_REG) * DECPROT_NB_BITS;
+		status = (decprot[offset] >> shift) & DECPROT_MASK;
+		addr = array[i];
+
+		debug("ETZPC: 0x%08x decprot %d=%d\n", addr, i, status);
+
+		if (addr == ETZPC_RESERVED ||
+		    status == DECPROT_NON_SECURED)
+			continue;
+
+		if (fdt_disable_subnode_by_address(fdt, soc_node, addr))
+			printf("ETZPC: 0x%08x node disabled, decprot %d=%d\n",
+			       addr, i, status);
+	}
+
+	return 0;
+}
diff --git a/arch/arm/mach-stm32mp/syscon.c b/arch/arm/mach-stm32mp/syscon.c
index eb7f435..242f834 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 },
 	{ }
 };
 
-- 
2.7.4

