stm32mp2: initial support

Add initial support for STM32MP2 SoCs family.

SoCs information are available here :
https://www.st.com/content/st_com/en/campaigns/microprocessor-stm32mp2.html

Migrate all MP1 related code into stm32mp1/ directory
Create stm32mp2 directory dedicated for STM32MP2 SoCs.

Common code to MP1, MP13 and MP25 is kept into
arch/arm/mach-stm32/mach-stm32mp directory :
  - boot_params.c
  - bsec
  - cmd_stm32key
  - cmd_stm32prog
  - dram_init.c
  - syscon.c
  - ecdsa_romapi.c

For STM32MP2, it also :
  - adds memory region description needed for ARMv8 MMU.
  - enables early data cache before relocation.
    During the transition before/after relocation, the MMU, initially setup
    at the beginning of DDR, must be setup again at a correct address after
    relocation. This is done in enables_caches() by disabling cache, force
    arch.tlb_fillptr to NULL which will force the MMU to be setup again but
    with a new value for gd->arch.tlb_addr. gd->arch.tlb_addr has been
    updated after relocation in arm_reserve_mmu().

Signed-off-by: Patrice Chotard <patrice.chotard@foss.st.com>
Reviewed-by: Patrick Delaunay <patrick.delaunay@foss.st.com>

9 files changed, 2922 insertions, 0 deletions

diff --git a/arch/arm/mach-stm32mp/stm32mp1/Makefile b/arch/arm/mach-stm32mp/stm32mp1/Makefile
new file mode 100644
index 0000000000..94c7724127
--- /dev/null
+++ b/arch/arm/mach-stm32mp/stm32mp1/Makefile
@@ -0,0 +1,20 @@
+# SPDX-License-Identifier: GPL-2.0+
+#
+# Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+#
+
+obj-y += cpu.o
+
+obj-$(CONFIG_STM32MP13x) += stm32mp13x.o
+obj-$(CONFIG_STM32MP15x) += stm32mp15x.o
+
+obj-$(CONFIG_STM32_ECDSA_VERIFY) += ecdsa_romapi.o
+ifdef CONFIG_SPL_BUILD
+obj-y += spl.o
+obj-y += tzc400.o
+else
+obj-$(CONFIG_ARMV7_PSCI) += psci.o
+endif
+
+obj-$(CONFIG_$(SPL_)STM32MP15_PWR) += pwr_regulator.o
+obj-$(CONFIG_OF_SYSTEM_SETUP) += fdt.o
diff --git a/arch/arm/mach-stm32mp/stm32mp1/cpu.c b/arch/arm/mach-stm32mp/stm32mp1/cpu.c
new file mode 100644
index 0000000000..e07abbe21c
--- /dev/null
+++ b/arch/arm/mach-stm32mp/stm32mp1/cpu.c
@@ -0,0 +1,431 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#define LOG_CATEGORY LOGC_ARCH
+
+#include <common.h>
+#include <clk.h>
+#include <cpu_func.h>
+#include <debug_uart.h>
+#include <env.h>
+#include <init.h>
+#include <log.h>
+#include <lmb.h>
+#include <misc.h>
+#include <net.h>
+#include <spl.h>
+#include <asm/io.h>
+#include <asm/arch/stm32.h>
+#include <asm/arch/sys_proto.h>
+#include <asm/global_data.h>
+#include <dm/device.h>
+#include <dm/uclass.h>
+#include <linux/bitops.h>
+#include <linux/printk.h>
+
+/*
+ * early TLB into the .data section so that it not get cleared
+ * with 16kB allignment (see TTBR0_BASE_ADDR_MASK)
+ */
+u8 early_tlb[PGTABLE_SIZE] __section(".data") __aligned(0x4000);
+
+struct lmb lmb;
+
+u32 get_bootmode(void)
+{
+	/* read bootmode from TAMP backup register */
+	return (readl(TAMP_BOOT_CONTEXT) & TAMP_BOOT_MODE_MASK) >>
+		    TAMP_BOOT_MODE_SHIFT;
+}
+
+/*
+ * weak function overidde: set the DDR/SYSRAM executable before to enable the
+ * MMU and configure DACR, for early early_enable_caches (SPL or pre-reloc)
+ */
+void dram_bank_mmu_setup(int bank)
+{
+	struct bd_info *bd = gd->bd;
+	int	i;
+	phys_addr_t start;
+	phys_size_t size;
+	bool use_lmb = false;
+	enum dcache_option option;
+
+	if (IS_ENABLED(CONFIG_SPL_BUILD)) {
+/* STM32_SYSRAM_BASE exist only when SPL is supported */
+#ifdef CONFIG_SPL
+		start = ALIGN_DOWN(STM32_SYSRAM_BASE, MMU_SECTION_SIZE);
+		size = ALIGN(STM32_SYSRAM_SIZE, MMU_SECTION_SIZE);
+#endif
+	} else if (gd->flags & GD_FLG_RELOC) {
+		/* bd->bi_dram is available only after relocation */
+		start = bd->bi_dram[bank].start;
+		size =  bd->bi_dram[bank].size;
+		use_lmb = true;
+	} else {
+		/* mark cacheable and executable the beggining of the DDR */
+		start = STM32_DDR_BASE;
+		size = CONFIG_DDR_CACHEABLE_SIZE;
+	}
+
+	for (i = start >> MMU_SECTION_SHIFT;
+	     i < (start >> MMU_SECTION_SHIFT) + (size >> MMU_SECTION_SHIFT);
+	     i++) {
+		option = DCACHE_DEFAULT_OPTION;
+		if (use_lmb && lmb_is_reserved_flags(&lmb, i << MMU_SECTION_SHIFT, LMB_NOMAP))
+			option = 0; /* INVALID ENTRY in TLB */
+		set_section_dcache(i, option);
+	}
+}
+/*
+ * initialize the MMU and activate cache in SPL or in U-Boot pre-reloc stage
+ * MMU/TLB is updated in enable_caches() for U-Boot after relocation
+ * or is deactivated in U-Boot entry function start.S::cpu_init_cp15
+ */
+static void early_enable_caches(void)
+{
+	/* I-cache is already enabled in start.S: cpu_init_cp15 */
+
+	if (CONFIG_IS_ENABLED(SYS_DCACHE_OFF))
+		return;
+
+	if (!(CONFIG_IS_ENABLED(SYS_ICACHE_OFF) && CONFIG_IS_ENABLED(SYS_DCACHE_OFF))) {
+		gd->arch.tlb_size = PGTABLE_SIZE;
+		gd->arch.tlb_addr = (unsigned long)&early_tlb;
+	}
+
+	/* enable MMU (default configuration) */
+	dcache_enable();
+}
+
+/*
+ * Early system init
+ */
+int arch_cpu_init(void)
+{
+	early_enable_caches();
+
+	/* early armv7 timer init: needed for polling */
+	timer_init();
+
+	return 0;
+}
+
+/* weak function for SOC specific initialization */
+__weak void stm32mp_cpu_init(void)
+{
+}
+
+int mach_cpu_init(void)
+{
+	u32 boot_mode;
+
+	stm32mp_cpu_init();
+
+	boot_mode = get_bootmode();
+
+	if (IS_ENABLED(CONFIG_CMD_STM32PROG_SERIAL) &&
+	    (boot_mode & TAMP_BOOT_DEVICE_MASK) == BOOT_SERIAL_UART)
+		gd->flags |= GD_FLG_SILENT | GD_FLG_DISABLE_CONSOLE;
+	else if (IS_ENABLED(CONFIG_DEBUG_UART) && IS_ENABLED(CONFIG_SPL_BUILD))
+		debug_uart_init();
+
+	return 0;
+}
+
+void enable_caches(void)
+{
+	/* parse device tree when data cache is still activated */
+	lmb_init_and_reserve(&lmb, gd->bd, (void *)gd->fdt_blob);
+
+	/* I-cache is already enabled in start.S: icache_enable() not needed */
+
+	/* deactivate the data cache, early enabled in arch_cpu_init() */
+	dcache_disable();
+	/*
+	 * update MMU after relocation and enable the data cache
+	 * warning: the TLB location udpated in board_f.c::reserve_mmu
+	 */
+	dcache_enable();
+}
+
+/* used when CONFIG_DISPLAY_CPUINFO is activated */
+int print_cpuinfo(void)
+{
+	char name[SOC_NAME_SIZE];
+
+	get_soc_name(name);
+	printf("CPU: %s\n", name);
+
+	return 0;
+}
+
+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
+	};
+	const u32 sdmmc_addr[] = {
+		STM32_SDMMC1_BASE,
+		STM32_SDMMC2_BASE,
+		STM32_SDMMC3_BASE
+	};
+	char cmd[60];
+	u32 boot_ctx = readl(TAMP_BOOT_CONTEXT);
+	u32 boot_mode =
+		(boot_ctx & TAMP_BOOT_MODE_MASK) >> TAMP_BOOT_MODE_SHIFT;
+	unsigned int instance = (boot_mode & TAMP_BOOT_INSTANCE_MASK) - 1;
+	u32 forced_mode = (boot_ctx & TAMP_BOOT_FORCED_MASK);
+	struct udevice *dev;
+
+	log_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:
+		if (instance >= ARRAY_SIZE(serial_addr))
+			break;
+		/* serial : search associated node in devicetree */
+		sprintf(cmd, "serial@%x", serial_addr[instance]);
+		if (uclass_get_device_by_name(UCLASS_SERIAL, cmd, &dev)) {
+			/* restore console on error */
+			if (IS_ENABLED(CONFIG_CMD_STM32PROG_SERIAL))
+				gd->flags &= ~(GD_FLG_SILENT |
+					       GD_FLG_DISABLE_CONSOLE);
+			log_err("uart%d = %s not found in device tree!\n",
+				instance + 1, cmd);
+			break;
+		}
+		sprintf(cmd, "%d", dev_seq(dev));
+		env_set("boot_device", "serial");
+		env_set("boot_instance", cmd);
+
+		/* restore console on uart when not used */
+		if (IS_ENABLED(CONFIG_CMD_STM32PROG_SERIAL) && gd->cur_serial_dev != dev) {
+			gd->flags &= ~(GD_FLG_SILENT |
+				       GD_FLG_DISABLE_CONSOLE);
+			log_info("serial boot with console enabled!\n");
+		}
+		break;
+	case BOOT_SERIAL_USB:
+		env_set("boot_device", "usb");
+		env_set("boot_instance", "0");
+		break;
+	case BOOT_FLASH_SD:
+	case BOOT_FLASH_EMMC:
+		if (instance >= ARRAY_SIZE(sdmmc_addr))
+			break;
+		/* search associated sdmmc node in devicetree */
+		sprintf(cmd, "mmc@%x", sdmmc_addr[instance]);
+		if (uclass_get_device_by_name(UCLASS_MMC, cmd, &dev)) {
+			printf("mmc%d = %s not found in device tree!\n",
+			       instance, cmd);
+			break;
+		}
+		sprintf(cmd, "%d", dev_seq(dev));
+		env_set("boot_device", "mmc");
+		env_set("boot_instance", cmd);
+		break;
+	case BOOT_FLASH_NAND:
+		env_set("boot_device", "nand");
+		env_set("boot_instance", "0");
+		break;
+	case BOOT_FLASH_SPINAND:
+		env_set("boot_device", "spi-nand");
+		env_set("boot_instance", "0");
+		break;
+	case BOOT_FLASH_NOR:
+		env_set("boot_device", "nor");
+		env_set("boot_instance", "0");
+		break;
+	default:
+		env_set("boot_device", "invalid");
+		env_set("boot_instance", "");
+		log_err("unexpected boot mode = %x\n", boot_mode);
+		break;
+	}
+
+	switch (forced_mode) {
+	case BOOT_FASTBOOT:
+		log_info("Enter fastboot!\n");
+		env_set("preboot", "env set preboot; fastboot 0");
+		break;
+	case BOOT_STM32PROG:
+		env_set("boot_device", "usb");
+		env_set("boot_instance", "0");
+		break;
+	case BOOT_UMS_MMC0:
+	case BOOT_UMS_MMC1:
+	case BOOT_UMS_MMC2:
+		log_info("Enter UMS!\n");
+		instance = forced_mode - BOOT_UMS_MMC0;
+		sprintf(cmd, "env set preboot; ums 0 mmc %d", instance);
+		env_set("preboot", cmd);
+		break;
+	case BOOT_RECOVERY:
+		env_set("preboot", "env set preboot; run altbootcmd");
+		break;
+	case BOOT_NORMAL:
+		break;
+	default:
+		log_debug("unexpected forced boot mode = %x\n", forced_mode);
+		break;
+	}
+
+	/* clear TAMP for next reboot */
+	clrsetbits_le32(TAMP_BOOT_CONTEXT, TAMP_BOOT_FORCED_MASK, BOOT_NORMAL);
+}
+
+/*
+ * If there is no MAC address in the environment, then it will be initialized
+ * (silently) from the value in the OTP.
+ */
+__weak int setup_mac_address(void)
+{
+	int ret;
+	int i;
+	u32 otp[3];
+	uchar enetaddr[6];
+	struct udevice *dev;
+	int nb_eth, nb_otp, index;
+
+	if (!IS_ENABLED(CONFIG_NET))
+		return 0;
+
+	nb_eth = get_eth_nb();
+
+	/* 6 bytes for each MAC addr and 4 bytes for each OTP */
+	nb_otp = DIV_ROUND_UP(6 * nb_eth, 4);
+
+	ret = uclass_get_device_by_driver(UCLASS_MISC,
+					  DM_DRIVER_GET(stm32mp_bsec),
+					  &dev);
+	if (ret)
+		return ret;
+
+	ret = misc_read(dev, STM32_BSEC_SHADOW(BSEC_OTP_MAC), otp, 4 * nb_otp);
+	if (ret < 0)
+		return ret;
+
+	for (index = 0; index < nb_eth; index++) {
+		/* MAC already in environment */
+		if (eth_env_get_enetaddr_by_index("eth", index, enetaddr))
+			continue;
+
+		for (i = 0; i < 6; i++)
+			enetaddr[i] = ((uint8_t *)&otp)[i + 6 * index];
+
+		if (!is_valid_ethaddr(enetaddr)) {
+			log_err("invalid MAC address %d in OTP %pM\n",
+				index, enetaddr);
+			return -EINVAL;
+		}
+		log_debug("OTP MAC address %d = %pM\n", index, enetaddr);
+		ret = eth_env_set_enetaddr_by_index("eth", index, enetaddr);
+		if (ret) {
+			log_err("Failed to set mac address %pM from OTP: %d\n",
+				enetaddr, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int setup_serial_number(void)
+{
+	char serial_string[25];
+	u32 otp[3] = {0, 0, 0 };
+	struct udevice *dev;
+	int ret;
+
+	if (env_get("serial#"))
+		return 0;
+
+	ret = uclass_get_device_by_driver(UCLASS_MISC,
+					  DM_DRIVER_GET(stm32mp_bsec),
+					  &dev);
+	if (ret)
+		return ret;
+
+	ret = misc_read(dev, STM32_BSEC_SHADOW(BSEC_OTP_SERIAL),
+			otp, sizeof(otp));
+	if (ret < 0)
+		return ret;
+
+	sprintf(serial_string, "%08X%08X%08X", otp[0], otp[1], otp[2]);
+	env_set("serial#", serial_string);
+
+	return 0;
+}
+
+__weak void stm32mp_misc_init(void)
+{
+}
+
+int arch_misc_init(void)
+{
+	setup_boot_mode();
+	setup_mac_address();
+	setup_serial_number();
+	stm32mp_misc_init();
+
+	return 0;
+}
+
+/*
+ * Without forcing the ".data" section, this would get saved in ".bss". BSS
+ * will be cleared soon after, so it's not suitable.
+ */
+static uintptr_t rom_api_table __section(".data");
+static uintptr_t nt_fw_dtb __section(".data");
+
+/*
+ * The ROM gives us the API location in r0 when starting. This is only available
+ * during SPL, as there isn't (yet) a mechanism to pass this on to u-boot. Save
+ * the FDT address provided by TF-A in r2 at boot time. This function is called
+ * from start.S
+ */
+void save_boot_params(unsigned long r0, unsigned long r1, unsigned long r2,
+		      unsigned long r3)
+{
+	if (IS_ENABLED(CONFIG_STM32_ECDSA_VERIFY))
+		rom_api_table = r0;
+
+	if (IS_ENABLED(CONFIG_TFABOOT))
+		nt_fw_dtb = r2;
+
+	save_boot_params_ret();
+}
+
+uintptr_t get_stm32mp_rom_api_table(void)
+{
+	return rom_api_table;
+}
+
+uintptr_t get_stm32mp_bl2_dtb(void)
+{
+	return nt_fw_dtb;
+}
+
+#ifdef CONFIG_SPL_BUILD
+void __noreturn jump_to_image_no_args(struct spl_image_info *spl_image)
+{
+	typedef void __noreturn (*image_entry_stm32_t)(u32 romapi);
+	uintptr_t romapi = get_stm32mp_rom_api_table();
+
+	image_entry_stm32_t image_entry =
+		(image_entry_stm32_t)spl_image->entry_point;
+
+	printf("image entry point: 0x%lx\n", spl_image->entry_point);
+	image_entry(romapi);
+}
+#endif
diff --git a/arch/arm/mach-stm32mp/stm32mp1/fdt.c b/arch/arm/mach-stm32mp/stm32mp1/fdt.c
new file mode 100644
index 0000000000..de5c5a55ea
--- /dev/null
+++ b/arch/arm/mach-stm32mp/stm32mp1/fdt.c
@@ -0,0 +1,514 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2019-2020, STMicroelectronics - All Rights Reserved
+ */
+
+#define LOG_CATEGORY LOGC_ARCH
+
+#include <common.h>
+#include <fdtdec.h>
+#include <fdt_support.h>
+#include <log.h>
+#include <tee.h>
+#include <asm/arch/sys_proto.h>
+#include <dt-bindings/pinctrl/stm32-pinfunc.h>
+#include <linux/io.h>
+
+#define ETZPC_DECPROT(n)	(STM32_ETZPC_BASE + 0x10 + 4 * (n))
+#define ETZPC_DECPROT_NB	6
+
+#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
+
+#define STM32MP13_FDCAN_BASE	0x4400F000
+#define STM32MP13_ADC1_BASE	0x48003000
+#define STM32MP13_TSC_BASE	0x5000B000
+#define STM32MP13_CRYP_BASE	0x54002000
+#define STM32MP13_ETH2_BASE	0x5800E000
+#define STM32MP13_DCMIPP_BASE	0x5A000000
+#define STM32MP13_LTDC_BASE	0x5A010000
+
+#define STM32MP15_FDCAN_BASE	0x4400e000
+#define STM32MP15_CRYP2_BASE	0x4c005000
+#define STM32MP15_CRYP1_BASE	0x54001000
+#define STM32MP15_GPU_BASE	0x59000000
+#define STM32MP15_DSI_BASE	0x5a000000
+
+static const u32 stm32mp13_ip_addr[] = {
+	0x50025000,		/* 0 VREFBUF APB3 */
+	0x50021000,		/* 1 LPTIM2 APB3 */
+	0x50022000,		/* 2 LPTIM3 APB3 */
+	STM32MP13_LTDC_BASE,	/* 3 LTDC APB4 */
+	STM32MP13_DCMIPP_BASE,	/* 4 DCMIPP APB4 */
+	0x5A006000,		/* 5 USBPHYCTRL APB4 */
+	0x5A003000,		/* 6 DDRCTRLPHY APB4 */
+	ETZPC_RESERVED,		/* 7 Reserved*/
+	ETZPC_RESERVED,		/* 8 Reserved*/
+	ETZPC_RESERVED,		/* 9 Reserved*/
+	0x5C006000,		/* 10 TZC APB5 */
+	0x58001000,		/* 11 MCE APB5 */
+	0x5C000000,		/* 12 IWDG1 APB5 */
+	0x5C008000,		/* 13 STGENC APB5 */
+	ETZPC_RESERVED,		/* 14 Reserved*/
+	ETZPC_RESERVED,		/* 15 Reserved*/
+	0x4C000000,		/* 16 USART1 APB6 */
+	0x4C001000,		/* 17 USART2 APB6 */
+	0x4C002000,		/* 18 SPI4 APB6 */
+	0x4C003000,		/* 19 SPI5 APB6 */
+	0x4C004000,		/* 20 I2C3 APB6 */
+	0x4C005000,		/* 21 I2C4 APB6 */
+	0x4C006000,		/* 22 I2C5 APB6 */
+	0x4C007000,		/* 23 TIM12 APB6 */
+	0x4C008000,		/* 24 TIM13 APB6 */
+	0x4C009000,		/* 25 TIM14 APB6 */
+	0x4C00A000,		/* 26 TIM15 APB6 */
+	0x4C00B000,		/* 27 TIM16 APB6 */
+	0x4C00C000,		/* 28 TIM17 APB6 */
+	ETZPC_RESERVED,		/* 29 Reserved*/
+	ETZPC_RESERVED,		/* 30 Reserved*/
+	ETZPC_RESERVED,		/* 31 Reserved*/
+	STM32MP13_ADC1_BASE,	/* 32 ADC1 AHB2 */
+	0x48004000,		/* 33 ADC2 AHB2 */
+	0x49000000,		/* 34 OTG AHB2 */
+	ETZPC_RESERVED,		/* 35 Reserved*/
+	ETZPC_RESERVED,		/* 36 Reserved*/
+	STM32MP13_TSC_BASE,	/* 37 TSC AHB4 */
+	ETZPC_RESERVED,		/* 38 Reserved*/
+	ETZPC_RESERVED,		/* 39 Reserved*/
+	0x54004000,		/* 40 RNG AHB5 */
+	0x54003000,		/* 41 HASH AHB5 */
+	STM32MP13_CRYP_BASE,	/* 42 CRYPT AHB5 */
+	0x54005000,		/* 43 SAES AHB5 */
+	0x54006000,		/* 44 PKA AHB5 */
+	0x54000000,		/* 45 BKPSRAM AHB5 */
+	ETZPC_RESERVED,		/* 46 Reserved*/
+	ETZPC_RESERVED,		/* 47 Reserved*/
+	0x5800A000,		/* 48 ETH1 AHB6 */
+	STM32MP13_ETH2_BASE,	/* 49 ETH2 AHB6 */
+	0x58005000,		/* 50 SDMMC1 AHB6 */
+	0x58007000,		/* 51 SDMMC2 AHB6 */
+	ETZPC_RESERVED,		/* 52 Reserved*/
+	ETZPC_RESERVED,		/* 53 Reserved*/
+	0x58002000,		/* 54 FMC AHB6 */
+	0x58003000,		/* 55 QSPI AHB6 */
+	ETZPC_RESERVED,		/* 56 Reserved*/
+	ETZPC_RESERVED,		/* 57 Reserved*/
+	ETZPC_RESERVED,		/* 58 Reserved*/
+	ETZPC_RESERVED,		/* 59 Reserved*/
+	0x30000000,		/* 60 SRAM1 MLAHB */
+	0x30004000,		/* 61 SRAM2 MLAHB */
+	0x30006000,		/* 62 SRAM3 MLAHB */
+	ETZPC_RESERVED,		/* 63 Reserved*/
+	ETZPC_RESERVED,		/* 64 Reserved*/
+	ETZPC_RESERVED,		/* 65 Reserved*/
+	ETZPC_RESERVED,		/* 66 Reserved*/
+	ETZPC_RESERVED,		/* 67 Reserved*/
+	ETZPC_RESERVED,		/* 68 Reserved*/
+	ETZPC_RESERVED,		/* 69 Reserved*/
+	ETZPC_RESERVED,		/* 70 Reserved*/
+	ETZPC_RESERVED,		/* 71 Reserved*/
+	ETZPC_RESERVED,		/* 72 Reserved*/
+	ETZPC_RESERVED,		/* 73 Reserved*/
+	ETZPC_RESERVED,		/* 74 Reserved*/
+	ETZPC_RESERVED,		/* 75 Reserved*/
+	ETZPC_RESERVED,		/* 76 Reserved*/
+	ETZPC_RESERVED,		/* 77 Reserved*/
+	ETZPC_RESERVED,		/* 78 Reserved*/
+	ETZPC_RESERVED,		/* 79 Reserved*/
+	ETZPC_RESERVED,		/* 80 Reserved*/
+	ETZPC_RESERVED,		/* 81 Reserved*/
+	ETZPC_RESERVED,		/* 82 Reserved*/
+	ETZPC_RESERVED,		/* 83 Reserved*/
+	ETZPC_RESERVED,		/* 84 Reserved*/
+	ETZPC_RESERVED,		/* 85 Reserved*/
+	ETZPC_RESERVED,		/* 86 Reserved*/
+	ETZPC_RESERVED,		/* 87 Reserved*/
+	ETZPC_RESERVED,		/* 88 Reserved*/
+	ETZPC_RESERVED,		/* 89 Reserved*/
+	ETZPC_RESERVED,		/* 90 Reserved*/
+	ETZPC_RESERVED,		/* 91 Reserved*/
+	ETZPC_RESERVED,		/* 92 Reserved*/
+	ETZPC_RESERVED,		/* 93 Reserved*/
+	ETZPC_RESERVED,		/* 94 Reserved*/
+	ETZPC_RESERVED,		/* 95 Reserved*/
+};
+
+static const u32 stm32mp15_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 */
+	STM32MP15_CRYP1_BASE,	/* 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 */
+	STM32MP15_FDCAN_BASE,	/* 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 */
+	STM32MP15_CRYP2_BASE,	/* 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;
+	fdt_addr_t regs;
+
+	for (node = fdt_first_subnode(fdt, offset);
+	     node >= 0;
+	     node = fdt_next_subnode(fdt, node)) {
+		regs = fdtdec_get_addr(fdt, node, "reg");
+		if (addr == regs) {
+			if (fdtdec_get_is_enabled(fdt, node)) {
+				fdt_status_disabled(fdt, node);
+
+				return true;
+			}
+			return false;
+		}
+	}
+
+	return false;
+}
+
+static int stm32_fdt_fixup_etzpc(void *fdt, int soc_node)
+{
+	const u32 *array;
+	int array_size, i;
+	int offset, shift;
+	u32 addr, status, decprot[ETZPC_DECPROT_NB];
+
+	if (IS_ENABLED(CONFIG_STM32MP13x)) {
+		array = stm32mp13_ip_addr;
+		array_size = ARRAY_SIZE(stm32mp13_ip_addr);
+	}
+
+	if (IS_ENABLED(CONFIG_STM32MP15x)) {
+		array = stm32mp15_ip_addr;
+		array_size = ARRAY_SIZE(stm32mp15_ip_addr);
+	}
+
+	for (i = 0; i < ETZPC_DECPROT_NB; i++)
+		decprot[i] = readl(ETZPC_DECPROT(i));
+
+	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];
+
+		log_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))
+			log_notice("ETZPC: 0x%08x node disabled, decprot %d=%d\n",
+				   addr, i, status);
+	}
+
+	return 0;
+}
+
+/* deactivate all the cpu except core 0 */
+static void stm32_fdt_fixup_cpu(void *blob, char *name)
+{
+	int off;
+	u32 reg;
+
+	off = fdt_path_offset(blob, "/cpus");
+	if (off < 0) {
+		log_warning("%s: couldn't find /cpus node\n", __func__);
+		return;
+	}
+
+	off = fdt_node_offset_by_prop_value(blob, -1, "device_type", "cpu", 4);
+	while (off != -FDT_ERR_NOTFOUND) {
+		reg = fdtdec_get_addr(blob, off, "reg");
+		if (reg != 0) {
+			fdt_del_node(blob, off);
+			log_notice("FDT: cpu %d node remove for %s\n",
+				   reg, name);
+			/* after delete we can't trust the offsets anymore */
+			off = -1;
+		}
+		off = fdt_node_offset_by_prop_value(blob, off,
+						    "device_type", "cpu", 4);
+	}
+}
+
+static void stm32_fdt_disable(void *fdt, int offset, u32 addr,
+			      const char *string, const char *name)
+{
+	if (fdt_disable_subnode_by_address(fdt, offset, addr))
+		log_notice("FDT: %s@%08x node disabled for %s\n",
+			   string, addr, name);
+}
+
+static void stm32_fdt_disable_optee(void *blob)
+{
+	int off, node;
+
+	/* Delete "optee" firmware node */
+	off = fdt_node_offset_by_compatible(blob, -1, "linaro,optee-tz");
+	if (off >= 0 && fdtdec_get_is_enabled(blob, off))
+		fdt_del_node(blob, off);
+
+	/* Delete "optee@..." reserved-memory node */
+	off = fdt_path_offset(blob, "/reserved-memory/");
+	if (off < 0)
+		return;
+	for (node = fdt_first_subnode(blob, off);
+	     node >= 0;
+	     node = fdt_next_subnode(blob, node)) {
+		if (strncmp(fdt_get_name(blob, node, NULL), "optee@", 6))
+			continue;
+
+		if (fdt_del_node(blob, node))
+			printf("Failed to remove optee reserved-memory node\n");
+	}
+}
+
+static void stm32mp13_fdt_fixup(void *blob, int soc, u32 cpu, char *name)
+{
+	switch (cpu) {
+	case CPU_STM32MP131Fxx:
+	case CPU_STM32MP131Dxx:
+	case CPU_STM32MP131Cxx:
+	case CPU_STM32MP131Axx:
+		stm32_fdt_disable(blob, soc, STM32MP13_FDCAN_BASE, "can", name);
+		stm32_fdt_disable(blob, soc, STM32MP13_ADC1_BASE, "adc", name);
+		fallthrough;
+	case CPU_STM32MP133Fxx:
+	case CPU_STM32MP133Dxx:
+	case CPU_STM32MP133Cxx:
+	case CPU_STM32MP133Axx:
+		stm32_fdt_disable(blob, soc, STM32MP13_LTDC_BASE, "ltdc", name);
+		stm32_fdt_disable(blob, soc, STM32MP13_DCMIPP_BASE, "dcmipp",
+				  name);
+		stm32_fdt_disable(blob, soc, STM32MP13_TSC_BASE, "tsc", name);
+		break;
+	default:
+		break;
+	}
+
+	switch (cpu) {
+	case CPU_STM32MP135Dxx:
+	case CPU_STM32MP135Axx:
+	case CPU_STM32MP133Dxx:
+	case CPU_STM32MP133Axx:
+	case CPU_STM32MP131Dxx:
+	case CPU_STM32MP131Axx:
+		stm32_fdt_disable(blob, soc, STM32MP13_CRYP_BASE, "cryp", name);
+		break;
+	default:
+		break;
+	}
+}
+
+static void stm32mp15_fdt_fixup(void *blob, int soc, u32 cpu, char *name)
+{
+	u32 pkg;
+
+	switch (cpu) {
+	case CPU_STM32MP151Fxx:
+	case CPU_STM32MP151Dxx:
+	case CPU_STM32MP151Cxx:
+	case CPU_STM32MP151Axx:
+		stm32_fdt_fixup_cpu(blob, name);
+		/* after cpu delete we can't trust the soc offsets anymore */
+		soc = fdt_path_offset(blob, "/soc");
+		stm32_fdt_disable(blob, soc, STM32MP15_FDCAN_BASE, "can", name);
+		fallthrough;
+	case CPU_STM32MP153Fxx:
+	case CPU_STM32MP153Dxx:
+	case CPU_STM32MP153Cxx:
+	case CPU_STM32MP153Axx:
+		stm32_fdt_disable(blob, soc, STM32MP15_GPU_BASE, "gpu", name);
+		stm32_fdt_disable(blob, soc, STM32MP15_DSI_BASE, "dsi", name);
+		break;
+	default:
+		break;
+	}
+	switch (cpu) {
+	case CPU_STM32MP157Dxx:
+	case CPU_STM32MP157Axx:
+	case CPU_STM32MP153Dxx:
+	case CPU_STM32MP153Axx:
+	case CPU_STM32MP151Dxx:
+	case CPU_STM32MP151Axx:
+		stm32_fdt_disable(blob, soc, STM32MP15_CRYP1_BASE, "cryp",
+				  name);
+		stm32_fdt_disable(blob, soc, STM32MP15_CRYP2_BASE, "cryp",
+				  name);
+		break;
+	default:
+		break;
+	}
+	switch (get_cpu_package()) {
+	case STM32MP15_PKG_AA_LBGA448:
+		pkg = STM32MP_PKG_AA;
+		break;
+	case STM32MP15_PKG_AB_LBGA354:
+		pkg = STM32MP_PKG_AB;
+		break;
+	case STM32MP15_PKG_AC_TFBGA361:
+		pkg = STM32MP_PKG_AC;
+		break;
+	case STM32MP15_PKG_AD_TFBGA257:
+		pkg = STM32MP_PKG_AD;
+		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);
+	}
+}
+
+/*
+ * 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, struct bd_info *bd)
+{
+	int ret = 0;
+	int soc;
+	u32 cpu;
+	char name[SOC_NAME_SIZE];
+
+	soc = fdt_path_offset(blob, "/soc");
+	/* when absent, nothing to do */
+	if (soc == -FDT_ERR_NOTFOUND)
+		return 0;
+	if (soc < 0)
+		return soc;
+
+	if (CONFIG_IS_ENABLED(STM32_ETZPC)) {
+		ret = stm32_fdt_fixup_etzpc(blob, soc);
+		if (ret)
+			return ret;
+	}
+
+	/* MPUs Part Numbers and name*/
+	cpu = get_cpu_type();
+	get_soc_name(name);
+
+	if (IS_ENABLED(CONFIG_STM32MP13x))
+		stm32mp13_fdt_fixup(blob, soc, cpu, name);
+
+	if (IS_ENABLED(CONFIG_STM32MP15x)) {
+		stm32mp15_fdt_fixup(blob, soc, cpu, name);
+
+		/*
+		 * TEMP: remove OP-TEE nodes in kernel device tree
+		 *       copied from U-Boot device tree by optee_copy_fdt_nodes
+		 *       when OP-TEE is not detected (probe failed)
+		 * these OP-TEE nodes are present in <board>-u-boot.dtsi
+		 * under CONFIG_STM32MP15x_STM32IMAGE only for compatibility
+		 * when FIP is not used by TF-A
+		 */
+		if (IS_ENABLED(CONFIG_STM32MP15x_STM32IMAGE) &&
+		    !tee_find_device(NULL, NULL, NULL, NULL))
+			stm32_fdt_disable_optee(blob);
+	}
+
+	return ret;
+}
diff --git a/arch/arm/mach-stm32mp/stm32mp1/psci.c b/arch/arm/mach-stm32mp/stm32mp1/psci.c
new file mode 100644
index 0000000000..8cdeb0ab3f
--- /dev/null
+++ b/arch/arm/mach-stm32mp/stm32mp1/psci.c
@@ -0,0 +1,808 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <config.h>
+#include <common.h>
+#include <asm/armv7.h>
+#include <asm/cache.h>
+#include <asm/gic.h>
+#include <asm/io.h>
+#include <asm/psci.h>
+#include <asm/secure.h>
+#include <hang.h>
+#include <linux/bitops.h>
+
+/* PWR */
+#define PWR_CR3					0x0c
+#define PWR_MPUCR				0x10
+
+#define PWR_CR3_DDRSREN				BIT(10)
+#define PWR_CR3_DDRRETEN			BIT(12)
+
+#define PWR_MPUCR_PDDS				BIT(0)
+#define PWR_MPUCR_CSTDBYDIS			BIT(3)
+#define PWR_MPUCR_CSSF				BIT(9)
+
+/* RCC */
+#define RCC_MSSCKSELR				0x48
+#define RCC_DDRITFCR				0xd8
+
+#define RCC_DDRITFCR_DDRC1EN			BIT(0)
+#define RCC_DDRITFCR_DDRC1LPEN			BIT(1)
+#define RCC_DDRITFCR_DDRC2EN			BIT(2)
+#define RCC_DDRITFCR_DDRC2LPEN			BIT(3)
+#define RCC_DDRITFCR_DDRPHYCEN			BIT(4)
+#define RCC_DDRITFCR_DDRPHYCLPEN		BIT(5)
+#define RCC_DDRITFCR_DDRCAPBEN			BIT(6)
+#define RCC_DDRITFCR_DDRCAPBLPEN		BIT(7)
+#define RCC_DDRITFCR_AXIDCGEN			BIT(8)
+#define RCC_DDRITFCR_DDRPHYCAPBEN		BIT(9)
+#define RCC_DDRITFCR_DDRPHYCAPBLPEN		BIT(10)
+#define RCC_DDRITFCR_DDRCKMOD_MASK		GENMASK(22, 20)
+#define RCC_DDRITFCR_GSKPCTRL			BIT(24)
+
+#define RCC_MP_SREQSETR				0x104
+#define RCC_MP_SREQCLRR				0x108
+
+#define RCC_MP_CIER				0x414
+#define RCC_MP_CIFR				0x418
+#define RCC_MP_CIFR_WKUPF			BIT(20)
+
+#define RCC_MCUDIVR				0x830
+#define RCC_PLL3CR				0x880
+#define RCC_PLL4CR				0x894
+
+/* SYSCFG */
+#define SYSCFG_CMPCR				0x20
+#define SYSCFG_CMPCR_SW_CTRL			BIT(2)
+#define SYSCFG_CMPENSETR			0x24
+#define SYSCFG_CMPENCLRR			0x28
+#define SYSCFG_CMPENR_MPUEN			BIT(0)
+
+/* DDR Controller registers offsets */
+#define DDRCTRL_STAT				0x004
+#define DDRCTRL_PWRCTL				0x030
+#define DDRCTRL_PWRTMG				0x034
+#define DDRCTRL_HWLPCTL				0x038
+#define DDRCTRL_DFIMISC				0x1b0
+#define DDRCTRL_SWCTL				0x320
+#define DDRCTRL_SWSTAT				0x324
+#define DDRCTRL_PSTAT				0x3fc
+#define DDRCTRL_PCTRL_0				0x490
+#define DDRCTRL_PCTRL_1				0x540
+
+/* DDR Controller Register fields */
+#define DDRCTRL_STAT_OPERATING_MODE_MASK	GENMASK(2, 0)
+#define DDRCTRL_STAT_OPERATING_MODE_NORMAL	0x1
+#define DDRCTRL_STAT_OPERATING_MODE_SR		0x3
+#define DDRCTRL_STAT_SELFREF_TYPE_MASK		GENMASK(5, 4)
+#define DDRCTRL_STAT_SELFREF_TYPE_ASR		(0x3 << 4)
+#define DDRCTRL_STAT_SELFREF_TYPE_SR		(0x2 << 4)
+
+#define DDRCTRL_PWRCTL_SELFREF_EN		BIT(0)
+#define DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE	BIT(3)
+#define DDRCTRL_PWRCTL_SELFREF_SW		BIT(5)
+
+#define DDRCTRL_PWRTMG_SELFREF_TO_X32_MASK	GENMASK(23, 16)
+#define DDRCTRL_PWRTMG_SELFREF_TO_X32_0		BIT(16)
+
+#define DDRCTRL_HWLPCTL_HW_LP_EN		BIT(0)
+
+#define DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN	BIT(0)
+
+#define DDRCTRL_SWCTL_SW_DONE			BIT(0)
+
+#define DDRCTRL_SWSTAT_SW_DONE_ACK		BIT(0)
+
+#define DDRCTRL_PSTAT_RD_PORT_BUSY_0		BIT(0)
+#define DDRCTRL_PSTAT_RD_PORT_BUSY_1		BIT(1)
+#define DDRCTRL_PSTAT_WR_PORT_BUSY_0		BIT(16)
+#define DDRCTRL_PSTAT_WR_PORT_BUSY_1		BIT(17)
+
+#define DDRCTRL_PCTRL_N_PORT_EN			BIT(0)
+
+/* DDR PHY registers offsets */
+#define DDRPHYC_PIR				0x004
+#define DDRPHYC_PGSR				0x00c
+#define DDRPHYC_ACDLLCR				0x014
+#define DDRPHYC_ACIOCR				0x024
+#define DDRPHYC_DXCCR				0x028
+#define DDRPHYC_DSGCR				0x02c
+#define DDRPHYC_ZQ0CR0				0x180
+#define DDRPHYC_DX0DLLCR			0x1cc
+#define DDRPHYC_DX1DLLCR			0x20c
+#define DDRPHYC_DX2DLLCR			0x24c
+#define DDRPHYC_DX3DLLCR			0x28c
+
+/* DDR PHY Register fields */
+#define DDRPHYC_PIR_INIT			BIT(0)
+#define DDRPHYC_PIR_DLLSRST			BIT(1)
+#define DDRPHYC_PIR_DLLLOCK			BIT(2)
+#define DDRPHYC_PIR_ITMSRST			BIT(4)
+
+#define DDRPHYC_PGSR_IDONE			BIT(0)
+
+#define DDRPHYC_ACDLLCR_DLLSRST			BIT(30)
+#define DDRPHYC_ACDLLCR_DLLDIS			BIT(31)
+
+#define DDRPHYC_ACIOCR_ACOE			BIT(1)
+#define DDRPHYC_ACIOCR_ACPDD			BIT(3)
+#define DDRPHYC_ACIOCR_ACPDR			BIT(4)
+#define DDRPHYC_ACIOCR_CKPDD_MASK		GENMASK(10, 8)
+#define DDRPHYC_ACIOCR_CKPDD_0			BIT(8)
+#define DDRPHYC_ACIOCR_CKPDR_MASK		GENMASK(13, 11)
+#define DDRPHYC_ACIOCR_CKPDR_0			BIT(11)
+#define DDRPHYC_ACIOCR_CSPDD_MASK		GENMASK(20, 18)
+#define DDRPHYC_ACIOCR_CSPDD_0			BIT(18)
+
+#define DDRPHYC_DXCCR_DXPDD			BIT(2)
+#define DDRPHYC_DXCCR_DXPDR			BIT(3)
+
+#define DDRPHYC_DSGCR_CKEPDD_MASK		GENMASK(19, 16)
+#define DDRPHYC_DSGCR_CKEPDD_0			BIT(16)
+#define DDRPHYC_DSGCR_ODTPDD_MASK		GENMASK(23, 20)
+#define DDRPHYC_DSGCR_ODTPDD_0			BIT(20)
+#define DDRPHYC_DSGCR_NL2PD			BIT(24)
+#define DDRPHYC_DSGCR_CKOE			BIT(28)
+
+#define DDRPHYC_ZQ0CRN_ZQPD			BIT(31)
+
+#define DDRPHYC_DXNDLLCR_DLLDIS			BIT(31)
+
+#define BOOT_API_A7_CORE0_MAGIC_NUMBER		0xca7face0
+#define BOOT_API_A7_CORE1_MAGIC_NUMBER		0xca7face1
+
+#define MPIDR_AFF0				GENMASK(7, 0)
+
+#define RCC_MP_GRSTCSETR			(STM32_RCC_BASE + 0x0404)
+#define RCC_MP_GRSTCSETR_MPSYSRST		BIT(0)
+#define RCC_MP_GRSTCSETR_MPUP0RST		BIT(4)
+#define RCC_MP_GRSTCSETR_MPUP1RST		BIT(5)
+
+/* IWDG */
+#define IWDG_KR					0x00
+#define IWDG_KR_RELOAD_KEY			0xaaaa
+#define IWDG_EWCR				0x14
+#define IWDG_EWCR_EWIC				BIT(14)
+
+#define STM32MP1_PSCI_NR_CPUS			2
+#if STM32MP1_PSCI_NR_CPUS > CONFIG_ARMV7_PSCI_NR_CPUS
+#error "invalid value for CONFIG_ARMV7_PSCI_NR_CPUS"
+#endif
+
+u8 psci_state[STM32MP1_PSCI_NR_CPUS] __secure_data = {
+	 PSCI_AFFINITY_LEVEL_ON,
+	 PSCI_AFFINITY_LEVEL_OFF};
+
+static u32 __secure_data cntfrq;
+
+static u32 __secure cp15_read_cntfrq(void)
+{
+	u32 frq;
+
+	asm volatile("mrc p15, 0, %0, c14, c0, 0" : "=r" (frq));
+
+	return frq;
+}
+
+static void __secure cp15_write_cntfrq(u32 frq)
+{
+	asm volatile ("mcr p15, 0, %0, c14, c0, 0" : : "r" (frq));
+}
+
+static inline void psci_set_state(int cpu, u8 state)
+{
+	psci_state[cpu] = state;
+	dsb();
+	isb();
+}
+
+static u32 __secure stm32mp_get_gicd_base_address(void)
+{
+	u32 periphbase;
+
+	/* get the GIC base address from the CBAR register */
+	asm("mrc p15, 4, %0, c15, c0, 0\n" : "=r" (periphbase));
+
+	return (periphbase & CBAR_MASK) + GIC_DIST_OFFSET;
+}
+
+static void __secure stm32mp_raise_sgi0(int cpu)
+{
+	u32 gic_dist_addr;
+
+	gic_dist_addr = stm32mp_get_gicd_base_address();
+
+	/* ask cpu with SGI0 */
+	writel((BIT(cpu) << 16), gic_dist_addr + GICD_SGIR);
+}
+
+void __secure psci_arch_cpu_entry(void)
+{
+	u32 cpu = psci_get_cpu_id();
+
+	psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON);
+
+	/* write the saved cntfrq */
+	cp15_write_cntfrq(cntfrq);
+
+	/* reset magic in TAMP register */
+	writel(0xFFFFFFFF, TAMP_BACKUP_MAGIC_NUMBER);
+}
+
+s32 __secure psci_features(u32 function_id, u32 psci_fid)
+{
+	switch (psci_fid) {
+	case ARM_PSCI_0_2_FN_PSCI_VERSION:
+	case ARM_PSCI_0_2_FN_CPU_OFF:
+	case ARM_PSCI_0_2_FN_CPU_ON:
+	case ARM_PSCI_0_2_FN_AFFINITY_INFO:
+	case ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE:
+	case ARM_PSCI_0_2_FN_SYSTEM_OFF:
+	case ARM_PSCI_0_2_FN_SYSTEM_RESET:
+	case ARM_PSCI_1_0_FN_SYSTEM_SUSPEND:
+		return 0x0;
+	}
+	return ARM_PSCI_RET_NI;
+}
+
+u32 __secure psci_version(void)
+{
+	return ARM_PSCI_VER_1_0;
+}
+
+s32 __secure psci_affinity_info(u32 function_id, u32 target_affinity,
+				u32  lowest_affinity_level)
+{
+	u32 cpu = target_affinity & MPIDR_AFF0;
+
+	if (lowest_affinity_level > 0)
+		return ARM_PSCI_RET_INVAL;
+
+	if (target_affinity & ~MPIDR_AFF0)
+		return ARM_PSCI_RET_INVAL;
+
+	if (cpu >= STM32MP1_PSCI_NR_CPUS)
+		return ARM_PSCI_RET_INVAL;
+
+	return psci_state[cpu];
+}
+
+u32 __secure psci_migrate_info_type(void)
+{
+	/*
+	 * 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;
+}
+
+s32 __secure psci_cpu_on(u32 function_id, u32 target_cpu, u32 pc,
+			 u32 context_id)
+{
+	u32 cpu = target_cpu & MPIDR_AFF0;
+
+	if (target_cpu & ~MPIDR_AFF0)
+		return ARM_PSCI_RET_INVAL;
+
+	if (cpu >= STM32MP1_PSCI_NR_CPUS)
+		return ARM_PSCI_RET_INVAL;
+
+	if (psci_state[cpu] == PSCI_AFFINITY_LEVEL_ON)
+		return ARM_PSCI_RET_ALREADY_ON;
+
+	/* read and save cntfrq of current cpu to write on target cpu  */
+	cntfrq = cp15_read_cntfrq();
+
+	/* reset magic in TAMP register */
+	if (readl(TAMP_BACKUP_MAGIC_NUMBER))
+		writel(0xFFFFFFFF, TAMP_BACKUP_MAGIC_NUMBER);
+	/*
+	 * ROM code need a first SGI0 after core reset
+	 * core is ready when magic is set to 0 in ROM code
+	 */
+	while (readl(TAMP_BACKUP_MAGIC_NUMBER))
+		stm32mp_raise_sgi0(cpu);
+
+	/* store target PC and context id*/
+	psci_save(cpu, pc, context_id);
+
+	/* write entrypoint in backup RAM register */
+	writel((u32)&psci_cpu_entry, TAMP_BACKUP_BRANCH_ADDRESS);
+	psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON_PENDING);
+
+	/* write magic number in backup register */
+	if (cpu == 0x01)
+		writel(BOOT_API_A7_CORE1_MAGIC_NUMBER,
+		       TAMP_BACKUP_MAGIC_NUMBER);
+	else
+		writel(BOOT_API_A7_CORE0_MAGIC_NUMBER,
+		       TAMP_BACKUP_MAGIC_NUMBER);
+
+	/* Generate an IT to start the core */
+	stm32mp_raise_sgi0(cpu);
+
+	return ARM_PSCI_RET_SUCCESS;
+}
+
+s32 __secure psci_cpu_off(void)
+{
+	u32 cpu;
+
+	cpu = psci_get_cpu_id();
+
+	psci_cpu_off_common();
+	psci_set_state(cpu, PSCI_AFFINITY_LEVEL_OFF);
+
+	/* reset core: wfi is managed by BootRom */
+	if (cpu == 0x01)
+		writel(RCC_MP_GRSTCSETR_MPUP1RST, RCC_MP_GRSTCSETR);
+	else
+		writel(RCC_MP_GRSTCSETR_MPUP0RST, RCC_MP_GRSTCSETR);
+
+	/* just waiting reset */
+	while (1)
+		wfi();
+}
+
+void __secure psci_system_reset(void)
+{
+	/* System reset */
+	writel(RCC_MP_GRSTCSETR_MPSYSRST, RCC_MP_GRSTCSETR);
+	/* just waiting reset */
+	while (1)
+		wfi();
+}
+
+void __secure psci_system_off(void)
+{
+	/* System Off is not managed, waiting user power off
+	 * TODO: handle I2C write in PMIC Main Control register bit 0 = SWOFF
+	 */
+	while (1)
+		wfi();
+}
+
+static void __secure secure_udelay(unsigned int delay)
+{
+	u32 freq = cp15_read_cntfrq() / 1000000;
+	u64 start, end;
+
+	delay *= freq;
+
+	asm volatile("mrrc p15, 0, %Q0, %R0, c14" : "=r" (start));
+	for (;;) {
+		asm volatile("mrrc p15, 0, %Q0, %R0, c14" : "=r" (end));
+		if ((end - start) > delay)
+			break;
+	}
+}
+
+static int __secure secure_waitbits(u32 reg, u32 mask, u32 val)
+{
+	u32 freq = cp15_read_cntfrq() / 1000000;
+	u32 delay = 500 * freq;	/* 500 us */
+	u64 start, end;
+	u32 tmp;
+
+	asm volatile("mrrc p15, 0, %Q0, %R0, c14" : "=r" (start));
+	for (;;) {
+		tmp = readl(reg);
+		tmp &= mask;
+		if ((tmp & val) == val)
+			return 0;
+		asm volatile("mrrc p15, 0, %Q0, %R0, c14" : "=r" (end));
+		if ((end - start) > delay)
+			return -ETIMEDOUT;
+	}
+}
+
+static void __secure ddr_sr_mode_ssr(u32 *saved_pwrctl)
+{
+	setbits_le32(STM32_RCC_BASE + RCC_DDRITFCR,
+		     RCC_DDRITFCR_DDRC1LPEN | RCC_DDRITFCR_DDRC1EN |
+		     RCC_DDRITFCR_DDRC2LPEN | RCC_DDRITFCR_DDRC2EN |
+		     RCC_DDRITFCR_DDRCAPBLPEN | RCC_DDRITFCR_DDRPHYCAPBLPEN |
+		     RCC_DDRITFCR_DDRCAPBEN | RCC_DDRITFCR_DDRPHYCAPBEN |
+		     RCC_DDRITFCR_DDRPHYCEN);
+
+	clrbits_le32(STM32_RCC_BASE + RCC_DDRITFCR,
+		     RCC_DDRITFCR_AXIDCGEN | RCC_DDRITFCR_DDRCKMOD_MASK);
+
+	/* Disable HW LP interface of uMCTL2 */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_HWLPCTL,
+		     DDRCTRL_HWLPCTL_HW_LP_EN);
+
+	/* Configure Automatic LP modes of uMCTL2 */
+	clrsetbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PWRTMG,
+			DDRCTRL_PWRTMG_SELFREF_TO_X32_MASK,
+			DDRCTRL_PWRTMG_SELFREF_TO_X32_0);
+
+	/* Save PWRCTL register to restart ASR after suspend (if applicable) */
+	*saved_pwrctl = readl(STM32_DDRCTRL_BASE + DDRCTRL_PWRCTL);
+
+	/*
+	 * Disable Clock disable with LP modes
+	 * (used in RUN mode for LPDDR2 with specific timing).
+	 */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PWRCTL,
+		     DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE);
+
+	/* Disable automatic Self-Refresh mode */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PWRCTL,
+		     DDRCTRL_PWRCTL_SELFREF_EN);
+}
+
+static void __secure ddr_sr_mode_restore(u32 saved_pwrctl)
+{
+	saved_pwrctl &= DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE |
+			DDRCTRL_PWRCTL_SELFREF_EN;
+
+	/* Restore ASR mode in case it was enabled before suspend. */
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PWRCTL, saved_pwrctl);
+}
+
+static int __secure ddr_sw_self_refresh_in(void)
+{
+	int ret;
+
+	clrbits_le32(STM32_RCC_BASE + RCC_DDRITFCR, RCC_DDRITFCR_AXIDCGEN);
+
+	/* Blocks AXI ports from taking anymore transactions */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PCTRL_0,
+		     DDRCTRL_PCTRL_N_PORT_EN);
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PCTRL_1,
+		     DDRCTRL_PCTRL_N_PORT_EN);
+
+	/*
+	 * Waits unit all AXI ports are idle
+	 * Poll PSTAT.rd_port_busy_n = 0
+	 * Poll PSTAT.wr_port_busy_n = 0
+	 */
+	ret = secure_waitbits(STM32_DDRCTRL_BASE + DDRCTRL_PSTAT,
+			      DDRCTRL_PSTAT_RD_PORT_BUSY_0 |
+			      DDRCTRL_PSTAT_RD_PORT_BUSY_1 |
+			      DDRCTRL_PSTAT_WR_PORT_BUSY_0 |
+			      DDRCTRL_PSTAT_WR_PORT_BUSY_1, 0);
+	if (ret)
+		goto pstat_failed;
+
+	/* SW Self-Refresh entry */
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PWRCTL, DDRCTRL_PWRCTL_SELFREF_SW);
+
+	/*
+	 * Wait operating mode change in self-refresh mode
+	 * with STAT.operating_mode[1:0]==11.
+	 * Ensure transition to self-refresh was due to software
+	 * by checking also that STAT.selfref_type[1:0]=2.
+	 */
+	ret = secure_waitbits(STM32_DDRCTRL_BASE + DDRCTRL_STAT,
+			      DDRCTRL_STAT_OPERATING_MODE_MASK |
+			      DDRCTRL_STAT_SELFREF_TYPE_MASK,
+			      DDRCTRL_STAT_OPERATING_MODE_SR |
+			      DDRCTRL_STAT_SELFREF_TYPE_SR);
+	if (ret)
+		goto selfref_sw_failed;
+
+	/* IOs powering down (PUBL registers) */
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACPDD);
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACPDR);
+
+	clrsetbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR,
+			DDRPHYC_ACIOCR_CKPDD_MASK,
+			DDRPHYC_ACIOCR_CKPDD_0);
+
+	clrsetbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR,
+			DDRPHYC_ACIOCR_CKPDR_MASK,
+			DDRPHYC_ACIOCR_CKPDR_0);
+
+	clrsetbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR,
+			DDRPHYC_ACIOCR_CSPDD_MASK,
+			DDRPHYC_ACIOCR_CSPDD_0);
+
+	/* Disable command/address output driver */
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACOE);
+
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDD);
+
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDR);
+
+	clrsetbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DSGCR,
+			DDRPHYC_DSGCR_ODTPDD_MASK,
+			DDRPHYC_DSGCR_ODTPDD_0);
+
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DSGCR, DDRPHYC_DSGCR_NL2PD);
+
+	clrsetbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DSGCR,
+			DDRPHYC_DSGCR_CKEPDD_MASK,
+			DDRPHYC_DSGCR_CKEPDD_0);
+
+	/* Disable PZQ cell (PUBL register) */
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ZQ0CR0, DDRPHYC_ZQ0CRN_ZQPD);
+
+	/* Set latch */
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DSGCR, DDRPHYC_DSGCR_CKOE);
+
+	/* Additional delay to avoid early latch */
+	secure_udelay(10);
+
+	/* Activate sw retention in PWRCTRL */
+	setbits_le32(STM32_PWR_BASE + PWR_CR3, PWR_CR3_DDRRETEN);
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL ref clock */
+	setbits_le32(STM32_RCC_BASE + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+
+	/* Disable all DLLs: GLITCH window */
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACDLLCR, DDRPHYC_ACDLLCR_DLLDIS);
+
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DX0DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DX1DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DX2DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DX3DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL output clock */
+	clrbits_le32(STM32_RCC_BASE + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+
+	/* Deactivate all DDR clocks */
+	clrbits_le32(STM32_RCC_BASE + RCC_DDRITFCR,
+		     RCC_DDRITFCR_DDRC1EN | RCC_DDRITFCR_DDRC2EN |
+		     RCC_DDRITFCR_DDRCAPBEN | RCC_DDRITFCR_DDRPHYCAPBEN);
+
+	return 0;
+
+selfref_sw_failed:
+	/* This bit should be cleared to restore DDR in its previous state */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PWRCTL,
+		     DDRCTRL_PWRCTL_SELFREF_SW);
+
+pstat_failed:
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PCTRL_0,
+		     DDRCTRL_PCTRL_N_PORT_EN);
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PCTRL_1,
+		     DDRCTRL_PCTRL_N_PORT_EN);
+
+	return -EINVAL;
+};
+
+static void __secure ddr_sw_self_refresh_exit(void)
+{
+	int ret;
+
+	/* Enable all clocks */
+	setbits_le32(STM32_RCC_BASE + RCC_DDRITFCR,
+		     RCC_DDRITFCR_DDRC1EN | RCC_DDRITFCR_DDRC2EN |
+		     RCC_DDRITFCR_DDRPHYCEN | RCC_DDRITFCR_DDRPHYCAPBEN |
+		     RCC_DDRITFCR_DDRCAPBEN);
+
+	/* Handshake */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_SWCTL, DDRCTRL_SWCTL_SW_DONE);
+
+	/* Mask dfi_init_complete_en */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_DFIMISC,
+		     DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+
+	/* Ack */
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_SWCTL, DDRCTRL_SWCTL_SW_DONE);
+	ret = secure_waitbits(STM32_DDRCTRL_BASE + DDRCTRL_SWSTAT,
+			      DDRCTRL_SWSTAT_SW_DONE_ACK,
+			      DDRCTRL_SWSTAT_SW_DONE_ACK);
+	if (ret)
+		hang();
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL ref clock */
+	setbits_le32(STM32_RCC_BASE + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+
+	/* Enable all DLLs: GLITCH window */
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACDLLCR,
+		     DDRPHYC_ACDLLCR_DLLDIS);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DX0DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DX1DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DX2DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DX3DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	/* Additional delay to avoid early DLL clock switch */
+	secure_udelay(50);
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL ref clock */
+	clrbits_le32(STM32_RCC_BASE + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACDLLCR, DDRPHYC_ACDLLCR_DLLSRST);
+
+	secure_udelay(10);
+
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACDLLCR, DDRPHYC_ACDLLCR_DLLSRST);
+
+	/* PHY partial init: (DLL lock and ITM reset) */
+	writel(DDRPHYC_PIR_DLLSRST | DDRPHYC_PIR_DLLLOCK |
+	       DDRPHYC_PIR_ITMSRST | DDRPHYC_PIR_INIT,
+	       STM32_DDRPHYC_BASE + DDRPHYC_PIR);
+
+	/* Need to wait at least 10 clock cycles before accessing PGSR */
+	secure_udelay(1);
+
+	/* Pool end of init */
+	ret = secure_waitbits(STM32_DDRPHYC_BASE + DDRPHYC_PGSR,
+			      DDRPHYC_PGSR_IDONE, DDRPHYC_PGSR_IDONE);
+	if (ret)
+		hang();
+
+	/* Handshake */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_SWCTL, DDRCTRL_SWCTL_SW_DONE);
+
+	/* Unmask dfi_init_complete_en to uMCTL2 */
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_DFIMISC, DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+
+	/* Ack */
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_SWCTL, DDRCTRL_SWCTL_SW_DONE);
+	ret = secure_waitbits(STM32_DDRCTRL_BASE + DDRCTRL_SWSTAT,
+			      DDRCTRL_SWSTAT_SW_DONE_ACK,
+			      DDRCTRL_SWSTAT_SW_DONE_ACK);
+	if (ret)
+		hang();
+
+	/* Deactivate sw retention in PWR */
+	clrbits_le32(STM32_PWR_BASE + PWR_CR3, PWR_CR3_DDRRETEN);
+
+	/* Enable PZQ cell (PUBL register) */
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ZQ0CR0, DDRPHYC_ZQ0CRN_ZQPD);
+
+	/* Enable pad drivers */
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACPDD);
+
+	/* Enable command/address output driver */
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACOE);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_CKPDD_MASK);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_CSPDD_MASK);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDD);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDR);
+
+	/* Release latch */
+	setbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DSGCR, DDRPHYC_DSGCR_CKOE);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DSGCR, DDRPHYC_DSGCR_ODTPDD_MASK);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DSGCR, DDRPHYC_DSGCR_NL2PD);
+
+	clrbits_le32(STM32_DDRPHYC_BASE + DDRPHYC_DSGCR, DDRPHYC_DSGCR_CKEPDD_MASK);
+
+	/* Remove selfrefresh */
+	clrbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PWRCTL, DDRCTRL_PWRCTL_SELFREF_SW);
+
+	/* Wait operating_mode == normal */
+	ret = secure_waitbits(STM32_DDRCTRL_BASE + DDRCTRL_STAT,
+			      DDRCTRL_STAT_OPERATING_MODE_MASK,
+			      DDRCTRL_STAT_OPERATING_MODE_NORMAL);
+	if (ret)
+		hang();
+
+	/* AXI ports are no longer blocked from taking transactions */
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PCTRL_0, DDRCTRL_PCTRL_N_PORT_EN);
+	setbits_le32(STM32_DDRCTRL_BASE + DDRCTRL_PCTRL_1, DDRCTRL_PCTRL_N_PORT_EN);
+
+	setbits_le32(STM32_RCC_BASE + RCC_DDRITFCR, RCC_DDRITFCR_AXIDCGEN);
+}
+
+void __secure psci_system_suspend(u32 __always_unused function_id,
+				  u32 ep, u32 context_id)
+{
+	u32 saved_mcudivr, saved_pll3cr, saved_pll4cr, saved_mssckselr;
+	u32 gicd_addr = stm32mp_get_gicd_base_address();
+	bool iwdg1_wake = false;
+	bool iwdg2_wake = false;
+	bool other_wake = false;
+	u32 saved_pwrctl, reg;
+	u32 gic_enabled[8];
+	u32 irqs;
+	int i;
+
+	/* Cache enable mask of all 256 SPI */
+	for (i = 0; i < ARRAY_SIZE(gic_enabled); i++)
+		gic_enabled[i] = readl(gicd_addr + GICD_ISENABLERn + 0x4 + 4 * i);
+
+	/* Disable IO compensation */
+
+	/* Place current APSRC/ANSRC into RAPSRC/RANSRC */
+	reg = readl(STM32_SYSCFG_BASE + SYSCFG_CMPCR);
+	reg >>= 8;
+	reg &= 0xff << 16;
+	reg |= SYSCFG_CMPCR_SW_CTRL;
+	writel(reg, STM32_SYSCFG_BASE + SYSCFG_CMPCR);
+	writel(SYSCFG_CMPENR_MPUEN, STM32_SYSCFG_BASE + SYSCFG_CMPENCLRR);
+
+	writel(RCC_MP_CIFR_WKUPF, STM32_RCC_BASE + RCC_MP_CIFR);
+	setbits_le32(STM32_RCC_BASE + RCC_MP_CIER, RCC_MP_CIFR_WKUPF);
+
+	setbits_le32(STM32_PWR_BASE + PWR_MPUCR,
+		     PWR_MPUCR_CSSF | PWR_MPUCR_CSTDBYDIS);
+
+	saved_mcudivr = readl(STM32_RCC_BASE + RCC_MCUDIVR);
+	saved_pll3cr = readl(STM32_RCC_BASE + RCC_PLL3CR);
+	saved_pll4cr = readl(STM32_RCC_BASE + RCC_PLL4CR);
+	saved_mssckselr = readl(STM32_RCC_BASE + RCC_MSSCKSELR);
+
+	psci_v7_flush_dcache_all();
+	ddr_sr_mode_ssr(&saved_pwrctl);
+	ddr_sw_self_refresh_in();
+	setbits_le32(STM32_PWR_BASE + PWR_CR3, PWR_CR3_DDRSREN);
+	writel(0x3, STM32_RCC_BASE + RCC_MP_SREQSETR);
+
+	/* Ping the IWDG before entering suspend */
+	iwdg1_wake = !!(gic_enabled[4] & BIT(22));	/* SPI 150 */
+	iwdg2_wake = !!(gic_enabled[4] & BIT(23));	/* SPI 151 */
+
+	for (;;) {
+		/* Ping IWDG1 and ACK pretimer IRQ */
+		if (iwdg1_wake) {
+			writel(IWDG_KR_RELOAD_KEY, STM32_IWDG1_BASE + IWDG_KR);
+			writel(IWDG_EWCR_EWIC, STM32_IWDG1_BASE + IWDG_EWCR);
+		}
+
+		/* Ping IWDG2 and ACK pretimer IRQ */
+		if (iwdg2_wake) {
+			writel(IWDG_KR_RELOAD_KEY, STM32_IWDG2_BASE + IWDG_KR);
+			writel(IWDG_EWCR_EWIC, STM32_IWDG2_BASE + IWDG_EWCR);
+		}
+
+		iwdg1_wake = false;
+		iwdg2_wake = false;
+
+		/* Zzz, enter stop mode */
+		asm volatile(
+			"isb\n"
+			"dsb\n"
+			"wfi\n");
+
+		/* Determine the wake up source */
+		for (i = 0; i < ARRAY_SIZE(gic_enabled); i++) {
+			irqs = readl(gicd_addr + GICR_IGROUPMODRn + 0x4 + 4 * i);
+			irqs &= gic_enabled[i];
+			if (!irqs)
+				continue;
+
+			/* Test whether IWDG pretimeout triggered the wake up. */
+			if (i == 4) {	/* SPI Num 128..159 */
+				iwdg1_wake = !!(irqs & BIT(22));	/* SPI 150 */
+				iwdg2_wake = !!(irqs & BIT(23));	/* SPI 151 */
+				irqs &= ~(BIT(22) | BIT(23));
+			}
+
+			/* Test whether there is any other wake up trigger. */
+			if (irqs) {
+				other_wake = true;
+				break;
+			}
+		}
+
+		/* Other wake up triggers pending, let OS deal with all of it. */
+		if (other_wake)
+			break;
+	}
+
+	writel(0x3, STM32_RCC_BASE + RCC_MP_SREQCLRR);
+	ddr_sw_self_refresh_exit();
+	ddr_sr_mode_restore(saved_pwrctl);
+
+	writel(saved_mcudivr, STM32_RCC_BASE + RCC_MCUDIVR);
+	writel(saved_pll3cr, STM32_RCC_BASE + RCC_PLL3CR);
+	writel(saved_pll4cr, STM32_RCC_BASE + RCC_PLL4CR);
+	writel(saved_mssckselr, STM32_RCC_BASE + RCC_MSSCKSELR);
+
+	writel(SYSCFG_CMPENR_MPUEN, STM32_SYSCFG_BASE + SYSCFG_CMPENSETR);
+	clrbits_le32(STM32_SYSCFG_BASE + SYSCFG_CMPCR, SYSCFG_CMPCR_SW_CTRL);
+}
diff --git a/arch/arm/mach-stm32mp/stm32mp1/pwr_regulator.c b/arch/arm/mach-stm32mp/stm32mp1/pwr_regulator.c
new file mode 100644
index 0000000000..846637ab16
--- /dev/null
+++ b/arch/arm/mach-stm32mp/stm32mp1/pwr_regulator.c
@@ -0,0 +1,279 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#define LOG_CATEGORY UCLASS_REGULATOR
+
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <syscon.h>
+#include <asm/io.h>
+#include <dm/device_compat.h>
+#include <dm/device-internal.h>
+#include <linux/bitops.h>
+#include <linux/err.h>
+#include <power/pmic.h>
+#include <power/regulator.h>
+
+#define STM32MP_PWR_CR3 0xc
+#define STM32MP_PWR_CR3_USB33DEN BIT(24)
+#define STM32MP_PWR_CR3_USB33RDY BIT(26)
+#define STM32MP_PWR_CR3_REG18DEN BIT(28)
+#define STM32MP_PWR_CR3_REG18RDY BIT(29)
+#define STM32MP_PWR_CR3_REG11DEN BIT(30)
+#define STM32MP_PWR_CR3_REG11RDY BIT(31)
+
+struct stm32mp_pwr_reg_info {
+	u32 enable;
+	u32 ready;
+	char *name;
+};
+
+struct stm32mp_pwr_priv {
+	fdt_addr_t base;
+};
+
+static int stm32mp_pwr_write(struct udevice *dev, uint reg,
+			     const uint8_t *buff, int len)
+{
+	struct stm32mp_pwr_priv *priv = dev_get_priv(dev);
+	u32 val = *(u32 *)buff;
+
+	if (len != 4)
+		return -EINVAL;
+
+	writel(val, priv->base + STM32MP_PWR_CR3);
+
+	return 0;
+}
+
+static int stm32mp_pwr_read(struct udevice *dev, uint reg, uint8_t *buff,
+			    int len)
+{
+	struct stm32mp_pwr_priv *priv = dev_get_priv(dev);
+
+	if (len != 4)
+		return -EINVAL;
+
+	*(u32 *)buff = readl(priv->base + STM32MP_PWR_CR3);
+
+	return 0;
+}
+
+static int stm32mp_pwr_of_to_plat(struct udevice *dev)
+{
+	struct stm32mp_pwr_priv *priv = dev_get_priv(dev);
+
+	priv->base = dev_read_addr(dev);
+	if (priv->base == FDT_ADDR_T_NONE)
+		return -EINVAL;
+
+	return 0;
+}
+
+static const struct pmic_child_info pwr_children_info[] = {
+	{ .prefix = "reg", .driver = "stm32mp_pwr_regulator"},
+	{ .prefix = "usb", .driver = "stm32mp_pwr_regulator"},
+	{ },
+};
+
+static int stm32mp_pwr_bind(struct udevice *dev)
+{
+	int children;
+
+	children = pmic_bind_children(dev, dev_ofnode(dev), pwr_children_info);
+	if (!children)
+		dev_dbg(dev, "no child found\n");
+
+	return 0;
+}
+
+static struct dm_pmic_ops stm32mp_pwr_ops = {
+	.read = stm32mp_pwr_read,
+	.write = stm32mp_pwr_write,
+};
+
+static const struct udevice_id stm32mp_pwr_ids[] = {
+	{ .compatible = "st,stm32mp1,pwr-reg" },
+	{ }
+};
+
+U_BOOT_DRIVER(stm32mp_pwr_pmic) = {
+	.name = "stm32mp_pwr_pmic",
+	.id = UCLASS_PMIC,
+	.of_match = stm32mp_pwr_ids,
+	.bind = stm32mp_pwr_bind,
+	.ops = &stm32mp_pwr_ops,
+	.of_to_plat = stm32mp_pwr_of_to_plat,
+	.priv_auto	= sizeof(struct stm32mp_pwr_priv),
+};
+
+static const struct stm32mp_pwr_reg_info stm32mp_pwr_reg11 = {
+	.enable = STM32MP_PWR_CR3_REG11DEN,
+	.ready = STM32MP_PWR_CR3_REG11RDY,
+	.name = "reg11"
+};
+
+static const struct stm32mp_pwr_reg_info stm32mp_pwr_reg18 = {
+	.enable = STM32MP_PWR_CR3_REG18DEN,
+	.ready = STM32MP_PWR_CR3_REG18RDY,
+	.name = "reg18"
+};
+
+static const struct stm32mp_pwr_reg_info stm32mp_pwr_usb33 = {
+	.enable = STM32MP_PWR_CR3_USB33DEN,
+	.ready = STM32MP_PWR_CR3_USB33RDY,
+	.name = "usb33"
+};
+
+static const struct stm32mp_pwr_reg_info *stm32mp_pwr_reg_infos[] = {
+	&stm32mp_pwr_reg11,
+	&stm32mp_pwr_reg18,
+	&stm32mp_pwr_usb33,
+	NULL
+};
+
+static int stm32mp_pwr_regulator_probe(struct udevice *dev)
+{
+	const struct stm32mp_pwr_reg_info **p = stm32mp_pwr_reg_infos;
+	struct dm_regulator_uclass_plat *uc_pdata;
+
+	uc_pdata = dev_get_uclass_plat(dev);
+
+	while (*p) {
+		int rc;
+
+		rc = dev_read_stringlist_search(dev, "regulator-name",
+						(*p)->name);
+		if (rc >= 0) {
+			dev_dbg(dev, "found regulator %s\n", (*p)->name);
+			break;
+		} else if (rc != -ENODATA) {
+			return rc;
+		}
+		p++;
+	}
+	if (!*p) {
+		int i = 0;
+		const char *s;
+
+		dev_dbg(dev, "regulator ");
+		while (dev_read_string_index(dev, "regulator-name",
+					     i++, &s) >= 0)
+			dev_dbg(dev, "%s'%s' ", (i > 1) ? ", " : "", s);
+		dev_dbg(dev, "%s not supported\n", (i > 2) ? "are" : "is");
+		return -EINVAL;
+	}
+
+	uc_pdata->type = REGULATOR_TYPE_FIXED;
+	dev_set_priv(dev, (void *)*p);
+
+	return 0;
+}
+
+static int stm32mp_pwr_regulator_set_value(struct udevice *dev, int uV)
+{
+	struct dm_regulator_uclass_plat *uc_pdata;
+
+	uc_pdata = dev_get_uclass_plat(dev);
+	if (!uc_pdata)
+		return -ENXIO;
+
+	if (uc_pdata->min_uV != uV) {
+		dev_dbg(dev, "Invalid uV=%d for: %s\n", uV, uc_pdata->name);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int stm32mp_pwr_regulator_get_value(struct udevice *dev)
+{
+	struct dm_regulator_uclass_plat *uc_pdata;
+
+	uc_pdata = dev_get_uclass_plat(dev);
+	if (!uc_pdata)
+		return -ENXIO;
+
+	if (uc_pdata->min_uV != uc_pdata->max_uV) {
+		dev_dbg(dev, "Invalid constraints for: %s\n", uc_pdata->name);
+		return -EINVAL;
+	}
+
+	return uc_pdata->min_uV;
+}
+
+static int stm32mp_pwr_regulator_get_enable(struct udevice *dev)
+{
+	const struct stm32mp_pwr_reg_info *p = dev_get_priv(dev);
+	int rc;
+	u32 reg;
+
+	rc = pmic_read(dev->parent, 0, (uint8_t *)&reg, sizeof(reg));
+	if (rc)
+		return rc;
+
+	dev_dbg(dev, "%s id %s\n", p->name, (reg & p->enable) ? "on" : "off");
+
+	return (reg & p->enable) != 0;
+}
+
+static int stm32mp_pwr_regulator_set_enable(struct udevice *dev, bool enable)
+{
+	const struct stm32mp_pwr_reg_info *p = dev_get_priv(dev);
+	int rc;
+	u32 reg;
+	u32 time_start;
+
+	dev_dbg(dev, "Turning %s %s\n", enable ? "on" : "off", p->name);
+
+	rc = pmic_read(dev->parent, 0, (uint8_t *)&reg, sizeof(reg));
+	if (rc)
+		return rc;
+
+	/* if regulator is already in the wanted state, nothing to do */
+	if (!!(reg & p->enable) == enable)
+		return 0;
+
+	reg &= ~p->enable;
+	if (enable)
+		reg |= p->enable;
+
+	rc = pmic_write(dev->parent, 0, (uint8_t *)&reg, sizeof(reg));
+	if (rc)
+		return rc;
+
+	if (!enable)
+		return 0;
+
+	/* waiting ready for enable */
+	time_start = get_timer(0);
+	while (1) {
+		rc = pmic_read(dev->parent, 0, (uint8_t *)&reg, sizeof(reg));
+		if (rc)
+			return rc;
+		if (reg & p->ready)
+			break;
+		if (get_timer(time_start) > CONFIG_SYS_HZ) {
+			dev_dbg(dev, "%s: timeout\n", p->name);
+			return -ETIMEDOUT;
+		}
+	}
+	return 0;
+}
+
+static const struct dm_regulator_ops stm32mp_pwr_regulator_ops = {
+	.set_value  = stm32mp_pwr_regulator_set_value,
+	.get_value  = stm32mp_pwr_regulator_get_value,
+	.get_enable = stm32mp_pwr_regulator_get_enable,
+	.set_enable = stm32mp_pwr_regulator_set_enable,
+};
+
+U_BOOT_DRIVER(stm32mp_pwr_regulator) = {
+	.name = "stm32mp_pwr_regulator",
+	.id = UCLASS_REGULATOR,
+	.ops = &stm32mp_pwr_regulator_ops,
+	.probe = stm32mp_pwr_regulator_probe,
+};
diff --git a/arch/arm/mach-stm32mp/stm32mp1/spl.c b/arch/arm/mach-stm32mp/stm32mp1/spl.c
new file mode 100644
index 0000000000..6c79259b2c
--- /dev/null
+++ b/arch/arm/mach-stm32mp/stm32mp1/spl.c
@@ -0,0 +1,252 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#define LOG_CATEGORY LOGC_ARCH
+
+#include <common.h>
+#include <cpu_func.h>
+#include <dm.h>
+#include <hang.h>
+#include <init.h>
+#include <log.h>
+#include <ram.h>
+#include <spl.h>
+#include <asm/cache.h>
+#include <asm/global_data.h>
+#include <asm/io.h>
+#include <asm/arch/sys_proto.h>
+#include <mach/tzc.h>
+#include <linux/libfdt.h>
+
+u32 spl_boot_device(void)
+{
+	u32 boot_mode;
+
+	boot_mode = get_bootmode();
+
+	switch (boot_mode) {
+	case BOOT_FLASH_SD_1:
+	case BOOT_FLASH_EMMC_1:
+		return BOOT_DEVICE_MMC1;
+	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_DFU;
+	case BOOT_FLASH_NAND_FMC:
+		return BOOT_DEVICE_NAND;
+	case BOOT_FLASH_NOR_QSPI:
+		return BOOT_DEVICE_SPI;
+	case BOOT_FLASH_SPINAND_1:
+		return BOOT_DEVICE_NONE; /* SPINAND not supported in SPL */
+	}
+
+	return BOOT_DEVICE_MMC1;
+}
+
+u32 spl_mmc_boot_mode(struct mmc *mmc, const u32 boot_device)
+{
+	return MMCSD_MODE_RAW;
+}
+
+#ifdef CONFIG_SYS_MMCSD_RAW_MODE_U_BOOT_USE_PARTITION
+int spl_mmc_boot_partition(const u32 boot_device)
+{
+	switch (boot_device) {
+	case BOOT_DEVICE_MMC1:
+		return CONFIG_SYS_MMCSD_RAW_MODE_U_BOOT_PARTITION;
+	case BOOT_DEVICE_MMC2:
+		return CONFIG_SYS_MMCSD_RAW_MODE_U_BOOT_PARTITION_MMC2;
+	default:
+		return -EINVAL;
+	}
+}
+#endif
+
+#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)
+		log_info("Model: %s\n", model);
+}
+#endif
+
+__weak int board_early_init_f(void)
+{
+	return 0;
+}
+
+uint32_t stm32mp_get_dram_size(void)
+{
+	struct ram_info ram;
+	struct udevice *dev;
+	int ret;
+
+	if (uclass_get_device(UCLASS_RAM, 0, &dev))
+		return 0;
+
+	ret = ram_get_info(dev, &ram);
+	if (ret)
+		return 0;
+
+	return ram.size;
+}
+
+static int optee_get_reserved_memory(uint32_t *start, uint32_t *size)
+{
+	fdt_addr_t fdt_mem_size;
+	fdt_addr_t fdt_start;
+	ofnode node;
+
+	node = ofnode_path("/reserved-memory/optee");
+	if (!ofnode_valid(node))
+		return 0;
+
+	fdt_start = ofnode_get_addr_size(node, "reg", &fdt_mem_size);
+	*start = fdt_start;
+	*size = fdt_mem_size;
+	return (fdt_start < 0) ? fdt_start : 0;
+}
+
+#define CFG_SHMEM_SIZE			0x200000
+#define STM32_TZC_NSID_ALL		0xffff
+#define STM32_TZC_FILTER_ALL		3
+
+void stm32_init_tzc_for_optee(void)
+{
+	const uint32_t dram_size = stm32mp_get_dram_size();
+	const uintptr_t dram_top = STM32_DDR_BASE + (dram_size - 1);
+	uint32_t optee_base, optee_size, tee_shmem_base;
+	const uintptr_t tzc = STM32_TZC_BASE;
+	int ret;
+
+	if (dram_size == 0)
+		panic("Cannot determine DRAM size from devicetree\n");
+
+	ret = optee_get_reserved_memory(&optee_base, &optee_size);
+	if (ret < 0 || optee_size <= CFG_SHMEM_SIZE)
+		panic("Invalid OPTEE reserved memory in devicetree\n");
+
+	tee_shmem_base = optee_base + optee_size - CFG_SHMEM_SIZE;
+
+	const struct tzc_region optee_config[] = {
+		{
+			.base = STM32_DDR_BASE,
+			.top = optee_base - 1,
+			.sec_mode = TZC_ATTR_SEC_NONE,
+			.nsec_id = STM32_TZC_NSID_ALL,
+			.filters_mask = STM32_TZC_FILTER_ALL,
+		}, {
+			.base = optee_base,
+			.top = tee_shmem_base - 1,
+			.sec_mode = TZC_ATTR_SEC_RW,
+			.nsec_id = 0,
+			.filters_mask = STM32_TZC_FILTER_ALL,
+		}, {
+			.base = tee_shmem_base,
+			.top = dram_top,
+			.sec_mode = TZC_ATTR_SEC_NONE,
+			.nsec_id = STM32_TZC_NSID_ALL,
+			.filters_mask = STM32_TZC_FILTER_ALL,
+		}, {
+			.top = 0,
+		}
+	};
+
+	flush_dcache_all();
+
+	tzc_configure(tzc, optee_config);
+	tzc_dump_config(tzc);
+
+	dcache_disable();
+}
+
+void spl_board_prepare_for_optee(void *fdt)
+{
+	stm32_init_tzc_for_optee();
+}
+
+void board_init_f(ulong dummy)
+{
+	struct udevice *dev;
+	int ret;
+
+	arch_cpu_init();
+	mach_cpu_init();
+
+	ret = spl_early_init();
+	if (ret) {
+		log_debug("spl_early_init() failed: %d\n", ret);
+		hang();
+	}
+
+	ret = uclass_get_device(UCLASS_CLK, 0, &dev);
+	if (ret) {
+		log_debug("Clock init failed: %d\n", ret);
+		hang();
+	}
+
+	ret = uclass_get_device(UCLASS_RESET, 0, &dev);
+	if (ret) {
+		log_debug("Reset init failed: %d\n", ret);
+		hang();
+	}
+
+	ret = uclass_get_device(UCLASS_PINCTRL, 0, &dev);
+	if (ret) {
+		log_debug("%s: Cannot find pinctrl device\n", __func__);
+		hang();
+	}
+
+	/* enable console uart printing */
+	preloader_console_init();
+
+	ret = board_early_init_f();
+	if (ret) {
+		log_debug("board_early_init_f() failed: %d\n", ret);
+		hang();
+	}
+
+	ret = uclass_get_device(UCLASS_RAM, 0, &dev);
+	if (ret) {
+		log_err("DRAM init failed: %d\n", ret);
+		hang();
+	}
+
+	/*
+	 * activate cache on DDR only when DDR is fully initialized
+	 * to avoid speculative access and issue in get_ram_size()
+	 */
+	if (!CONFIG_IS_ENABLED(SYS_DCACHE_OFF))
+		mmu_set_region_dcache_behaviour(STM32_DDR_BASE,
+						CONFIG_DDR_CACHEABLE_SIZE,
+						DCACHE_DEFAULT_OPTION);
+}
+
+void spl_board_prepare_for_boot(void)
+{
+	dcache_disable();
+}
+
+void spl_board_prepare_for_linux(void)
+{
+	dcache_disable();
+}
diff --git a/arch/arm/mach-stm32mp/stm32mp1/stm32mp13x.c b/arch/arm/mach-stm32mp/stm32mp1/stm32mp13x.c
new file mode 100644
index 0000000000..845d973ad1
--- /dev/null
+++ b/arch/arm/mach-stm32mp/stm32mp1/stm32mp13x.c
@@ -0,0 +1,138 @@
+// SPDX-License-Identifier: GPL-2.0-or-later OR BSD-3-Clause
+/*
+ * Copyright (C) 2022, STMicroelectronics - All Rights Reserved
+ */
+
+#define LOG_CATEGORY LOGC_ARCH
+
+#include <common.h>
+#include <log.h>
+#include <syscon.h>
+#include <asm/io.h>
+#include <asm/arch/stm32.h>
+#include <asm/arch/sys_proto.h>
+
+/* SYSCFG register */
+#define SYSCFG_IDC_OFFSET	0x380
+#define SYSCFG_IDC_DEV_ID_MASK	GENMASK(11, 0)
+#define SYSCFG_IDC_DEV_ID_SHIFT	0
+#define SYSCFG_IDC_REV_ID_MASK	GENMASK(31, 16)
+#define SYSCFG_IDC_REV_ID_SHIFT	16
+
+/* Device Part Number (RPN) = OTP_DATA1 lower 11 bits */
+#define RPN_SHIFT	0
+#define RPN_MASK	GENMASK(11, 0)
+
+static u32 read_idc(void)
+{
+	void *syscfg = syscon_get_first_range(STM32MP_SYSCON_SYSCFG);
+
+	return readl(syscfg + SYSCFG_IDC_OFFSET);
+}
+
+u32 get_cpu_dev(void)
+{
+	return (read_idc() & SYSCFG_IDC_DEV_ID_MASK) >> SYSCFG_IDC_DEV_ID_SHIFT;
+}
+
+u32 get_cpu_rev(void)
+{
+	return (read_idc() & SYSCFG_IDC_REV_ID_MASK) >> SYSCFG_IDC_REV_ID_SHIFT;
+}
+
+/* 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 (get_cpu_dev() << 16) | get_cpu_rpn();
+}
+
+int get_eth_nb(void)
+{
+	int nb_eth = 2;
+
+	switch (get_cpu_type()) {
+	case CPU_STM32MP131Dxx:
+		fallthrough;
+	case CPU_STM32MP131Cxx:
+		fallthrough;
+	case CPU_STM32MP131Axx:
+		nb_eth = 1;
+		break;
+	default:
+		nb_eth = 2;
+		break;
+	}
+
+	return nb_eth;
+}
+
+void get_soc_name(char name[SOC_NAME_SIZE])
+{
+	char *cpu_s, *cpu_r;
+
+	/* MPUs Part Numbers */
+	switch (get_cpu_type()) {
+	case CPU_STM32MP135Fxx:
+		cpu_s = "135F";
+		break;
+	case CPU_STM32MP135Dxx:
+		cpu_s = "135D";
+		break;
+	case CPU_STM32MP135Cxx:
+		cpu_s = "135C";
+		break;
+	case CPU_STM32MP135Axx:
+		cpu_s = "135A";
+		break;
+	case CPU_STM32MP133Fxx:
+		cpu_s = "133F";
+		break;
+	case CPU_STM32MP133Dxx:
+		cpu_s = "133D";
+		break;
+	case CPU_STM32MP133Cxx:
+		cpu_s = "133C";
+		break;
+	case CPU_STM32MP133Axx:
+		cpu_s = "133A";
+		break;
+	case CPU_STM32MP131Fxx:
+		cpu_s = "131F";
+		break;
+	case CPU_STM32MP131Dxx:
+		cpu_s = "131D";
+		break;
+	case CPU_STM32MP131Cxx:
+		cpu_s = "131C";
+		break;
+	case CPU_STM32MP131Axx:
+		cpu_s = "131A";
+		break;
+	default:
+		cpu_s = "????";
+		break;
+	}
+
+	/* REVISION */
+	switch (get_cpu_rev()) {
+	case CPU_REV1:
+		cpu_r = "A";
+		break;
+	case CPU_REV1_1:
+		cpu_r = "Z";
+		break;
+	case CPU_REV1_2:
+		cpu_r = "Y";
+		break;
+	default:
+		cpu_r = "?";
+		break;
+	}
+
+	snprintf(name, SOC_NAME_SIZE, "STM32MP%s Rev.%s", cpu_s, cpu_r);
+}
diff --git a/arch/arm/mach-stm32mp/stm32mp1/stm32mp15x.c b/arch/arm/mach-stm32mp/stm32mp1/stm32mp15x.c
new file mode 100644
index 0000000000..afc56b02ee
--- /dev/null
+++ b/arch/arm/mach-stm32mp/stm32mp1/stm32mp15x.c
@@ -0,0 +1,344 @@
+// SPDX-License-Identifier: GPL-2.0-or-later OR BSD-3-Clause
+/*
+ * Copyright (C) 2021, STMicroelectronics - All Rights Reserved
+ */
+
+#define LOG_CATEGORY LOGC_ARCH
+
+#include <common.h>
+#include <env.h>
+#include <log.h>
+#include <asm/io.h>
+#include <asm/arch/bsec.h>
+#include <asm/arch/stm32.h>
+#include <asm/arch/sys_proto.h>
+#include <dm/device.h>
+#include <dm/uclass.h>
+
+/* RCC register */
+#define RCC_TZCR		(STM32_RCC_BASE + 0x00)
+#define RCC_BDCR		(STM32_RCC_BASE + 0x0140)
+#define RCC_MP_APB5ENSETR	(STM32_RCC_BASE + 0x0208)
+#define RCC_MP_AHB5ENSETR	(STM32_RCC_BASE + 0x0210)
+#define RCC_DBGCFGR		(STM32_RCC_BASE + 0x080C)
+
+#define RCC_BDCR_VSWRST		BIT(31)
+#define RCC_BDCR_RTCSRC		GENMASK(17, 16)
+
+#define RCC_DBGCFGR_DBGCKEN	BIT(8)
+
+/* DBGMCU register */
+#define DBGMCU_IDC		(STM32_DBGMCU_BASE + 0x00)
+#define DBGMCU_APB4FZ1		(STM32_DBGMCU_BASE + 0x2C)
+#define DBGMCU_APB4FZ1_IWDG2	BIT(2)
+
+/* Security register */
+#define ETZPC_TZMA1_SIZE	(STM32_ETZPC_BASE + 0x04)
+#define ETZPC_DECPROT0		(STM32_ETZPC_BASE + 0x10)
+
+#define TZC_GATE_KEEPER		(STM32_TZC_BASE + 0x008)
+#define TZC_REGION_ATTRIBUTE0	(STM32_TZC_BASE + 0x110)
+#define TZC_REGION_ID_ACCESS0	(STM32_TZC_BASE + 0x114)
+
+#define TAMP_CR1		(STM32_TAMP_BASE + 0x00)
+
+#define PWR_CR1			(STM32_PWR_BASE + 0x00)
+#define PWR_MCUCR		(STM32_PWR_BASE + 0x14)
+#define PWR_CR1_DBP		BIT(8)
+#define PWR_MCUCR_SBF		BIT(6)
+
+/* GPIOZ registers */
+#define GPIOZ_SECCFGR		0x54004030
+
+/* DBGMCU register */
+#define DBGMCU_IDC		(STM32_DBGMCU_BASE + 0x00)
+#define DBGMCU_IDC_DEV_ID_MASK	GENMASK(11, 0)
+#define DBGMCU_IDC_DEV_ID_SHIFT	0
+#define DBGMCU_IDC_REV_ID_MASK	GENMASK(31, 16)
+#define DBGMCU_IDC_REV_ID_SHIFT	16
+
+/* boot interface from Bootrom
+ * - boot instance = bit 31:16
+ * - boot device = bit 15:0
+ */
+#define BOOTROM_PARAM_ADDR	0x2FFC0078
+#define BOOTROM_MODE_MASK	GENMASK(15, 0)
+#define BOOTROM_MODE_SHIFT	0
+#define BOOTROM_INSTANCE_MASK	 GENMASK(31, 16)
+#define BOOTROM_INSTANCE_SHIFT	16
+
+/* 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 => STM32MP15_PKG defines
+ * - 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)
+
+static void security_init(void)
+{
+	/* Disable the backup domain write protection */
+	/* the protection is enable at each reset by hardware */
+	/* And must be disable by software */
+	setbits_le32(PWR_CR1, PWR_CR1_DBP);
+
+	while (!(readl(PWR_CR1) & PWR_CR1_DBP))
+		;
+
+	/* If RTC clock isn't enable so this is a cold boot then we need
+	 * to reset the backup domain
+	 */
+	if (!(readl(RCC_BDCR) & RCC_BDCR_RTCSRC)) {
+		setbits_le32(RCC_BDCR, RCC_BDCR_VSWRST);
+		while (!(readl(RCC_BDCR) & RCC_BDCR_VSWRST))
+			;
+		clrbits_le32(RCC_BDCR, RCC_BDCR_VSWRST);
+	}
+
+	/* allow non secure access in Write/Read for all peripheral */
+	writel(GENMASK(25, 0), ETZPC_DECPROT0);
+
+	/* Open SYSRAM for no secure access */
+	writel(0x0, ETZPC_TZMA1_SIZE);
+
+	/* enable TZC1 TZC2 clock */
+	writel(BIT(11) | BIT(12), RCC_MP_APB5ENSETR);
+
+	/* Region 0 set to no access by default */
+	/* bit 0 / 16 => nsaid0 read/write Enable
+	 * bit 1 / 17 => nsaid1 read/write Enable
+	 * ...
+	 * bit 15 / 31 => nsaid15 read/write Enable
+	 */
+	writel(0xFFFFFFFF, TZC_REGION_ID_ACCESS0);
+	/* bit 30 / 31 => Secure Global Enable : write/read */
+	/* bit 0 / 1 => Region Enable for filter 0/1 */
+	writel(BIT(0) | BIT(1) | BIT(30) | BIT(31), TZC_REGION_ATTRIBUTE0);
+
+	/* Enable Filter 0 and 1 */
+	setbits_le32(TZC_GATE_KEEPER, BIT(0) | BIT(1));
+
+	/* RCC trust zone deactivated */
+	writel(0x0, RCC_TZCR);
+
+	/* TAMP: deactivate the internal tamper
+	 * Bit 23 ITAMP8E: monotonic counter overflow
+	 * Bit 20 ITAMP5E: RTC calendar overflow
+	 * Bit 19 ITAMP4E: HSE monitoring
+	 * Bit 18 ITAMP3E: LSE monitoring
+	 * Bit 16 ITAMP1E: RTC power domain supply monitoring
+	 */
+	writel(0x0, TAMP_CR1);
+
+	/* GPIOZ: deactivate the security */
+	writel(BIT(0), RCC_MP_AHB5ENSETR);
+	writel(0x0, GPIOZ_SECCFGR);
+}
+
+/*
+ * Debug init
+ */
+void dbgmcu_init(void)
+{
+	/*
+	 * Freeze IWDG2 if Cortex-A7 is in debug mode
+	 * done in TF-A for TRUSTED boot and
+	 * DBGMCU access is controlled by BSEC_DENABLE.DBGSWENABLE
+	 */
+	if (bsec_dbgswenable()) {
+		setbits_le32(RCC_DBGCFGR, RCC_DBGCFGR_DBGCKEN);
+		setbits_le32(DBGMCU_APB4FZ1, DBGMCU_APB4FZ1_IWDG2);
+	}
+}
+
+void spl_board_init(void)
+{
+	struct udevice *dev;
+	int ret;
+
+	dbgmcu_init();
+
+	/* force probe of BSEC driver to shadow the upper OTP */
+	ret = uclass_get_device_by_driver(UCLASS_MISC, DM_DRIVER_GET(stm32mp_bsec), &dev);
+	if (ret)
+		log_warning("BSEC probe failed: %d\n", ret);
+}
+
+/* get bootmode from ROM code boot context: saved in TAMP register */
+static void update_bootmode(void)
+{
+	u32 boot_mode;
+	u32 bootrom_itf = readl(BOOTROM_PARAM_ADDR);
+	u32 bootrom_device, bootrom_instance;
+
+	/* enable TAMP clock = RTCAPBEN */
+	writel(BIT(8), RCC_MP_APB5ENSETR);
+
+	/* read bootrom context */
+	bootrom_device =
+		(bootrom_itf & BOOTROM_MODE_MASK) >> BOOTROM_MODE_SHIFT;
+	bootrom_instance =
+		(bootrom_itf & BOOTROM_INSTANCE_MASK) >> BOOTROM_INSTANCE_SHIFT;
+	boot_mode =
+		((bootrom_device << BOOT_TYPE_SHIFT) & BOOT_TYPE_MASK) |
+		((bootrom_instance << BOOT_INSTANCE_SHIFT) &
+		 BOOT_INSTANCE_MASK);
+
+	/* save the boot mode in TAMP backup register */
+	clrsetbits_le32(TAMP_BOOT_CONTEXT,
+			TAMP_BOOT_MODE_MASK,
+			boot_mode << TAMP_BOOT_MODE_SHIFT);
+}
+
+/* weak function: STM32MP15x mach init for boot without TFA */
+void stm32mp_cpu_init(void)
+{
+	if (IS_ENABLED(CONFIG_SPL_BUILD)) {
+		security_init();
+		update_bootmode();
+	}
+
+	/* reset copro state in SPL, when used, or in U-Boot */
+	if (!IS_ENABLED(CONFIG_SPL) || IS_ENABLED(CONFIG_SPL_BUILD)) {
+		/* Reset Coprocessor state unless it wakes up from Standby power mode */
+		if (!(readl(PWR_MCUCR) & PWR_MCUCR_SBF)) {
+			writel(TAMP_COPRO_STATE_OFF, TAMP_COPRO_STATE);
+			writel(0, TAMP_COPRO_RSC_TBL_ADDRESS);
+		}
+	}
+}
+
+static u32 read_idc(void)
+{
+	/* DBGMCU access is controlled by BSEC_DENABLE.DBGSWENABLE */
+	if (bsec_dbgswenable()) {
+		setbits_le32(RCC_DBGCFGR, RCC_DBGCFGR_DBGCKEN);
+
+		return readl(DBGMCU_IDC);
+	}
+
+	return CPU_DEV_STM32MP15; /* STM32MP15x and unknown revision */
+}
+
+u32 get_cpu_dev(void)
+{
+	return (read_idc() & DBGMCU_IDC_DEV_ID_MASK) >> DBGMCU_IDC_DEV_ID_SHIFT;
+}
+
+u32 get_cpu_rev(void)
+{
+	return (read_idc() & DBGMCU_IDC_REV_ID_MASK) >> DBGMCU_IDC_REV_ID_SHIFT;
+}
+
+/* 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 (get_cpu_dev() << 16) | get_cpu_rpn();
+}
+
+int get_eth_nb(void)
+{
+	return 1;
+}
+
+/* Get Package options from OTP */
+u32 get_cpu_package(void)
+{
+	return get_otp(BSEC_OTP_PKG, PKG_SHIFT, PKG_MASK);
+}
+
+static const char * const soc_type[] = {
+	"????",
+	"151C", "151A", "151F", "151D",
+	"153C", "153A", "153F", "153D",
+	"157C", "157A", "157F", "157D"
+};
+
+static const char * const soc_pkg[] = { "??", "AD", "AC", "AB", "AA" };
+static const char * const soc_rev[] = { "?", "A", "B", "Z", "Y"};
+
+static void get_cpu_string_offsets(unsigned int *type, unsigned int *pkg,
+				   unsigned int *rev)
+{
+	u32 cpu_type = get_cpu_type();
+	u32 ct = cpu_type & ~(BIT(7) | BIT(0));
+	u32 cm = ((cpu_type & BIT(7)) >> 6) | (cpu_type & BIT(0));
+
+	/* Bits 0 and 7 are the ACDF, 00:C 01:A 10:F 11:D */
+	switch (ct) {
+	case CPU_STM32MP151Cxx:
+		*type = cm + 1;
+		break;
+	case CPU_STM32MP153Cxx:
+		*type = cm + 5;
+		break;
+	case CPU_STM32MP157Cxx:
+		*type = cm + 9;
+		break;
+	default:
+		*type = 0;
+		break;
+	}
+
+	/* Package */
+	*pkg = get_cpu_package();
+	if (*pkg > STM32MP15_PKG_AA_LBGA448)
+		*pkg = STM32MP15_PKG_UNKNOWN;
+
+	/* Revision */
+	switch (get_cpu_rev()) {
+	case CPU_REV1:
+		*rev = 1;
+		break;
+	case CPU_REV2:
+		*rev = 2;
+		break;
+	case CPU_REV2_1:
+		*rev = 3;
+		break;
+	case CPU_REV2_2:
+		*rev = 4;
+		break;
+	default:
+		*rev = 0;
+		break;
+	}
+}
+
+void get_soc_name(char name[SOC_NAME_SIZE])
+{
+	unsigned int type, pkg, rev;
+
+	get_cpu_string_offsets(&type, &pkg, &rev);
+
+	snprintf(name, SOC_NAME_SIZE, "STM32MP%s%s Rev.%s",
+		 soc_type[type], soc_pkg[pkg], soc_rev[rev]);
+}
+
+static void setup_soc_type_pkg_rev(void)
+{
+	unsigned int type, pkg, rev;
+
+	get_cpu_string_offsets(&type, &pkg, &rev);
+
+	env_set("soc_type", soc_type[type]);
+	env_set("soc_pkg", soc_pkg[pkg]);
+	env_set("soc_rev", soc_rev[rev]);
+}
+
+/* weak function called in arch_misc_init */
+void stm32mp_misc_init(void)
+{
+	setup_soc_type_pkg_rev();
+}
diff --git a/arch/arm/mach-stm32mp/stm32mp1/tzc400.c b/arch/arm/mach-stm32mp/stm32mp1/tzc400.c
new file mode 100644
index 0000000000..cdc4a40eda
--- /dev/null
+++ b/arch/arm/mach-stm32mp/stm32mp1/tzc400.c
@@ -0,0 +1,136 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Simple API for configuring TrustZone memory restrictions for TZC400
+ */
+
+#define LOG_CATEGORY LOGC_ARCH
+
+#include <linux/iopoll.h>
+#include <mach/tzc.h>
+
+#define TZC_TIMEOUT_US		100
+
+#define TZC_BUILD_CONFIG	0x00
+#define TZC_ACTION		0x04
+#define TZC_ACTION_NONE		0
+#define TZC_ACTION_ERR		1
+#define TZC_ACTION_INT		2
+#define TZC_ACTION_INT_ERR	3
+#define TZC_GATE_KEEPER		0x08
+
+#define TZC_REGION0_OFFSET	0x100
+#define TZC_REGION_CFG_SIZE	0x20
+#define TZC_REGION1_OFFSET	0x120
+#define TZC_REGION_BASE		0x00
+#define TZC_REGION_TOP		0x08
+#define TZC_REGION_ATTRIBUTE	0x10
+#define TZC_REGION_ACCESS	0x14
+
+static uint32_t tzc_read(uintptr_t tzc, size_t reg)
+{
+	return readl(tzc + reg);
+}
+
+static void tzc_write(uintptr_t tzc, size_t reg, uint32_t val)
+{
+	writel(val, tzc + reg);
+}
+
+static uint16_t tzc_config_get_active_filters(const struct tzc_region *cfg)
+{
+	uint16_t active_filters = 0;
+
+	for ( ; cfg->top != 0; cfg++)
+		active_filters |= cfg->filters_mask;
+
+	return active_filters;
+}
+
+int tzc_configure(uintptr_t tzc, const struct tzc_region *cfg)
+{
+	uintptr_t region = tzc + TZC_REGION1_OFFSET;
+	uint32_t nsid, attr_reg, active_filters;
+	int ret;
+
+	active_filters = tzc_config_get_active_filters(cfg);
+	if (active_filters == 0)
+		return -EINVAL;
+
+	ret = tzc_disable_filters(tzc, active_filters);
+	if (ret < 0)
+		return ret;
+
+	for ( ; cfg->top != 0; cfg++, region += TZC_REGION_CFG_SIZE) {
+		attr_reg = (cfg->sec_mode & 0x03) << 30;
+		attr_reg |= (cfg->filters_mask & 0x03) << 0;
+		nsid = cfg->nsec_id & 0xffff;
+		nsid |= nsid << 16;
+
+		tzc_write(region, TZC_REGION_BASE, cfg->base);
+		tzc_write(region, TZC_REGION_TOP, cfg->top);
+		tzc_write(region, TZC_REGION_ACCESS, nsid);
+		tzc_write(region, TZC_REGION_ATTRIBUTE, attr_reg);
+	}
+
+	tzc_write(tzc, TZC_ACTION, TZC_ACTION_ERR);
+	return tzc_enable_filters(tzc, active_filters);
+}
+
+int tzc_disable_filters(uintptr_t tzc, uint16_t filters_mask)
+{
+	uint32_t gate = tzc_read(tzc, TZC_GATE_KEEPER);
+	uint32_t filter_status = filters_mask << 16;
+
+	gate &= ~filters_mask;
+	tzc_write(tzc, TZC_GATE_KEEPER, gate);
+
+	return readl_poll_timeout(tzc + TZC_GATE_KEEPER, gate,
+				 (gate & filter_status) == 0, TZC_TIMEOUT_US);
+}
+
+int tzc_enable_filters(uintptr_t tzc, uint16_t filters_mask)
+{
+	uint32_t gate = tzc_read(tzc, TZC_GATE_KEEPER);
+	uint32_t filter_status = filters_mask << 16;
+
+	gate |= filters_mask;
+	tzc_write(tzc, TZC_GATE_KEEPER, gate);
+
+	return readl_poll_timeout(tzc + TZC_GATE_KEEPER, gate,
+				 (gate & filter_status) == filter_status,
+				 TZC_TIMEOUT_US);
+}
+
+static const char *sec_access_str_from_attr(uint32_t attr)
+{
+	const char *const sec_mode[] = { "none", "RO  ", "WO  ", "RW  " };
+
+	return sec_mode[(attr >> 30) & 0x03];
+}
+
+void tzc_dump_config(uintptr_t tzc)
+{
+	uint32_t build_config, base, top, attr, nsaid;
+	int num_regions, i;
+	uintptr_t region;
+
+	build_config = tzc_read(tzc, TZC_BUILD_CONFIG);
+	num_regions = ((build_config >> 0) & 0x1f) + 1;
+
+	for (i = 0; i < num_regions; i++) {
+		region = tzc + TZC_REGION0_OFFSET + i * TZC_REGION_CFG_SIZE;
+
+		base = tzc_read(region, TZC_REGION_BASE);
+		top = tzc_read(region, TZC_REGION_TOP);
+		attr = tzc_read(region, TZC_REGION_ATTRIBUTE);
+		nsaid = tzc_read(region, TZC_REGION_ACCESS);
+
+		if (attr == 0 && nsaid == 0)
+			continue;
+
+		log_info("TZC region %u: %08x->%08x - filters 0x%x\n",
+			 i, base, top, (attr >> 0) & 0xf);
+		log_info("\t Secure access %s NSAID %08x\n",
+			 sec_access_str_from_attr(attr), nsaid);
+	}
+}