From da84a97b7083d65144ed658e0f0def1232b7852c Mon Sep 17 00:00:00 2001
From: christophe montaud <christophe.montaud@st.com>
Date: Wed, 30 Jan 2019 10:41:28 +0100
Subject: [PATCH] st updates r1

---
 core/arch/arm/fdts/stm32mp15-ddr.dtsi              |  153 ++
 .../arm/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi   |  121 ++
 .../arm/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi   |  122 ++
 core/arch/arm/fdts/stm32mp157-pinctrl.dtsi         |  350 +++++
 core/arch/arm/fdts/stm32mp157a-dk1.dts             |  380 +++++
 core/arch/arm/fdts/stm32mp157c-dk2.dts             |   16 +
 core/arch/arm/fdts/stm32mp157c-ed1.dts             |  381 +++++
 core/arch/arm/fdts/stm32mp157c-ev1.dts             |   67 +
 core/arch/arm/fdts/stm32mp157c-security.dtsi       |   71 +
 core/arch/arm/fdts/stm32mp157c.dtsi                |  371 +++++
 core/arch/arm/fdts/stm32mp157caa-pinctrl.dtsi      |   90 ++
 core/arch/arm/fdts/stm32mp157cab-pinctrl.dtsi      |   62 +
 core/arch/arm/fdts/stm32mp157cac-pinctrl.dtsi      |   78 +
 core/arch/arm/fdts/stm32mp157cad-pinctrl.dtsi      |   62 +
 core/arch/arm/include/arm32.h                      |   14 +-
 core/arch/arm/include/kernel/delay.h               |    8 +
 core/arch/arm/include/mm/core_mmu.h                |    3 +
 core/arch/arm/include/sm/pm.h                      |    4 +
 core/arch/arm/kernel/delay.c                       |   40 +-
 core/arch/arm/kernel/generic_boot.c                |   36 +-
 core/arch/arm/mm/core_mmu.c                        |    3 +
 core/arch/arm/mm/mobj.c                            |    3 +-
 core/arch/arm/plat-stm32mp1/boot_api.h             |    2 +
 core/arch/arm/plat-stm32mp1/conf.mk                |   81 +-
 core/arch/arm/plat-stm32mp1/drivers/stm32_reset.c  |   60 +
 core/arch/arm/plat-stm32mp1/drivers/stm32_reset.h  |   15 +
 .../arm/plat-stm32mp1/drivers/stm32mp1_calib.c     |  457 ++++++
 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c | 1527 ++++++++++++++++++
 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.h |   62 +
 .../arm/plat-stm32mp1/drivers/stm32mp1_clkfunc.c   |  340 ++++
 .../arm/plat-stm32mp1/drivers/stm32mp1_clkfunc.h   |   33 +
 .../arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.c |  513 ++++++
 .../arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.h |  205 +++
 .../arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c |  614 ++++++++
 .../arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h |   33 +
 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.c |   21 +
 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h |   48 +
 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.c |   51 +
 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.h |  539 +++++++
 core/arch/arm/plat-stm32mp1/drivers/sub.mk         |    8 +
 core/arch/arm/plat-stm32mp1/link.mk                |    6 +-
 core/arch/arm/plat-stm32mp1/main.c                 |  578 ++++++-
 core/arch/arm/plat-stm32mp1/platform_config.h      |  183 ++-
 core/arch/arm/plat-stm32mp1/pm/context.c           |  510 ++++++
 core/arch/arm/plat-stm32mp1/pm/context.h           |  100 ++
 .../arm/plat-stm32mp1/pm/context_asm_defines.c     |   28 +
 core/arch/arm/plat-stm32mp1/pm/low_power.c         |  431 ++++++
 core/arch/arm/plat-stm32mp1/pm/pm_helpers.S        |  635 ++++++++
 core/arch/arm/plat-stm32mp1/pm/power.h             |   26 +
 core/arch/arm/plat-stm32mp1/pm/power_config.c      |  212 +++
 core/arch/arm/plat-stm32mp1/pm/psci.c              |  427 +++++
 core/arch/arm/plat-stm32mp1/pm/sub.mk              |    7 +
 core/arch/arm/plat-stm32mp1/reset.S                |   28 +-
 core/arch/arm/plat-stm32mp1/scripts/stm32image.py  |   19 +-
 core/arch/arm/plat-stm32mp1/service/bsec_svc.c     |   65 +
 core/arch/arm/plat-stm32mp1/service/bsec_svc.h     |   14 +
 .../arch/arm/plat-stm32mp1/service/low_power_svc.c |  153 ++
 .../arch/arm/plat-stm32mp1/service/low_power_svc.h |   22 +
 core/arch/arm/plat-stm32mp1/service/pwr_svc.c      |  117 ++
 core/arch/arm/plat-stm32mp1/service/pwr_svc.h      |   11 +
 core/arch/arm/plat-stm32mp1/service/rcc_svc.c      |  440 ++++++
 core/arch/arm/plat-stm32mp1/service/rcc_svc.h      |   11 +
 core/arch/arm/plat-stm32mp1/service/stm32mp1_smc.h |  211 +++
 .../arm/plat-stm32mp1/service/stm32mp1_svc_setup.c |  129 ++
 core/arch/arm/plat-stm32mp1/service/sub.mk         |    7 +
 core/arch/arm/plat-stm32mp1/shared_resources.c     | 1007 ++++++++++++
 core/arch/arm/plat-stm32mp1/stm32_util.h           |  287 ++++
 core/arch/arm/plat-stm32mp1/stm32mp1_dt.c          |  338 ++++
 core/arch/arm/plat-stm32mp1/stm32mp_dt.h           |   39 +
 core/arch/arm/plat-stm32mp1/stm32mp_pm.h           |   49 +
 core/arch/arm/plat-stm32mp1/sub.mk                 |    6 +
 core/arch/arm/sm/pm_a32.S                          |   83 +-
 core/arch/arm/tee/entry_std.c                      |    2 +-
 core/drivers/gic.c                                 |  210 ++-
 core/drivers/stm32_bsec.c                          |  824 ++++++++++
 core/drivers/stm32_etzpc.c                         |  337 ++++
 core/drivers/stm32_gpio.c                          |  417 +++++
 core/drivers/stm32_i2c.c                           | 1629 ++++++++++++++++++++
 core/drivers/stm32_iwdg.c                          |  308 ++++
 core/drivers/stm32_rng.c                           |  200 +++
 core/drivers/stm32_rtc.c                           |  503 ++++++
 core/drivers/stm32_timer.c                         |  272 ++++
 core/drivers/stm32_uart.c                          |  142 +-
 core/drivers/stpmic1.c                             |  954 ++++++++++++
 core/drivers/sub.mk                                |    9 +
 core/include/drivers/gic.h                         |   29 +
 core/include/drivers/stm32_bsec.h                  |  143 ++
 core/include/drivers/stm32_etzpc.h                 |   75 +
 core/include/drivers/stm32_gpio.h                  |  107 ++
 core/include/drivers/stm32_i2c.h                   |  377 +++++
 core/include/drivers/stm32_iwdg.h                  |   17 +
 core/include/drivers/stm32_rng.h                   |   14 +
 core/include/drivers/stm32_rtc.h                   |   73 +
 core/include/drivers/stm32_timer.h                 |   25 +
 core/include/drivers/stm32_uart.h                  |   10 +-
 core/include/drivers/stpmic1.h                     |  228 +++
 core/include/dt-bindings/clock/stm32mp1-clks.h     |  252 +++
 core/include/dt-bindings/clock/stm32mp1-clksrc.h   |  284 ++++
 core/include/dt-bindings/etzpc/stm32-etzpc.h       |  108 ++
 .../dt-bindings/interrupt-controller/arm-gic.h     |   21 +
 core/include/dt-bindings/pinctrl/stm32-pinfunc.h   |   41 +
 core/include/dt-bindings/power/stm32mp1-power.h    |   19 +
 core/include/dt-bindings/reset/stm32mp1-resets.h   |  108 ++
 core/include/kernel/interrupt.h                    |   15 +
 core/kernel/console.c                              |    9 +-
 core/kernel/interrupt.c                            |   10 +
 core/lib/libfdt/fdt_ro.c                           |  154 +-
 core/lib/libfdt/fdt_rw.c                           |   11 +-
 core/lib/libfdt/fdt_wip.c                          |   29 +-
 core/lib/libfdt/include/fdt.h                      |    8 +
 core/lib/libfdt/include/libfdt.h                   |  247 ++-
 core/secure_dt.mk                                  |  107 ++
 core/sub.mk                                        |    7 +
 lib/libutils/ext/include/util.h                    |    7 +
 mk/config.mk                                       |   23 +-
 scripts/bin_to_c.py                                |   58 +
 116 files changed, 21451 insertions(+), 195 deletions(-)
 create mode 100644 core/arch/arm/fdts/stm32mp15-ddr.dtsi
 create mode 100644 core/arch/arm/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi
 create mode 100644 core/arch/arm/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi
 create mode 100644 core/arch/arm/fdts/stm32mp157-pinctrl.dtsi
 create mode 100644 core/arch/arm/fdts/stm32mp157a-dk1.dts
 create mode 100644 core/arch/arm/fdts/stm32mp157c-dk2.dts
 create mode 100644 core/arch/arm/fdts/stm32mp157c-ed1.dts
 create mode 100644 core/arch/arm/fdts/stm32mp157c-ev1.dts
 create mode 100644 core/arch/arm/fdts/stm32mp157c-security.dtsi
 create mode 100644 core/arch/arm/fdts/stm32mp157c.dtsi
 create mode 100644 core/arch/arm/fdts/stm32mp157caa-pinctrl.dtsi
 create mode 100644 core/arch/arm/fdts/stm32mp157cab-pinctrl.dtsi
 create mode 100644 core/arch/arm/fdts/stm32mp157cac-pinctrl.dtsi
 create mode 100644 core/arch/arm/fdts/stm32mp157cad-pinctrl.dtsi
 create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32_reset.c
 create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32_reset.h
 create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_calib.c
 create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c
 create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.h
 create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clkfunc.c
 create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clkfunc.h
 create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.c
 create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.h
 create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c
 create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h
 create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.c
 create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h
 create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.c
 create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.h
 create mode 100644 core/arch/arm/plat-stm32mp1/drivers/sub.mk
 create mode 100644 core/arch/arm/plat-stm32mp1/pm/context.c
 create mode 100644 core/arch/arm/plat-stm32mp1/pm/context.h
 create mode 100644 core/arch/arm/plat-stm32mp1/pm/context_asm_defines.c
 create mode 100644 core/arch/arm/plat-stm32mp1/pm/low_power.c
 create mode 100644 core/arch/arm/plat-stm32mp1/pm/pm_helpers.S
 create mode 100644 core/arch/arm/plat-stm32mp1/pm/power.h
 create mode 100644 core/arch/arm/plat-stm32mp1/pm/power_config.c
 create mode 100644 core/arch/arm/plat-stm32mp1/pm/psci.c
 create mode 100644 core/arch/arm/plat-stm32mp1/pm/sub.mk
 create mode 100644 core/arch/arm/plat-stm32mp1/service/bsec_svc.c
 create mode 100644 core/arch/arm/plat-stm32mp1/service/bsec_svc.h
 create mode 100644 core/arch/arm/plat-stm32mp1/service/low_power_svc.c
 create mode 100644 core/arch/arm/plat-stm32mp1/service/low_power_svc.h
 create mode 100644 core/arch/arm/plat-stm32mp1/service/pwr_svc.c
 create mode 100644 core/arch/arm/plat-stm32mp1/service/pwr_svc.h
 create mode 100644 core/arch/arm/plat-stm32mp1/service/rcc_svc.c
 create mode 100644 core/arch/arm/plat-stm32mp1/service/rcc_svc.h
 create mode 100644 core/arch/arm/plat-stm32mp1/service/stm32mp1_smc.h
 create mode 100644 core/arch/arm/plat-stm32mp1/service/stm32mp1_svc_setup.c
 create mode 100644 core/arch/arm/plat-stm32mp1/service/sub.mk
 create mode 100644 core/arch/arm/plat-stm32mp1/shared_resources.c
 create mode 100644 core/arch/arm/plat-stm32mp1/stm32_util.h
 create mode 100644 core/arch/arm/plat-stm32mp1/stm32mp1_dt.c
 create mode 100644 core/arch/arm/plat-stm32mp1/stm32mp_dt.h
 create mode 100644 core/arch/arm/plat-stm32mp1/stm32mp_pm.h
 create mode 100644 core/drivers/stm32_bsec.c
 create mode 100644 core/drivers/stm32_etzpc.c
 create mode 100644 core/drivers/stm32_gpio.c
 create mode 100644 core/drivers/stm32_i2c.c
 create mode 100644 core/drivers/stm32_iwdg.c
 create mode 100644 core/drivers/stm32_rng.c
 create mode 100644 core/drivers/stm32_rtc.c
 create mode 100644 core/drivers/stm32_timer.c
 create mode 100644 core/drivers/stpmic1.c
 create mode 100644 core/include/drivers/stm32_bsec.h
 create mode 100644 core/include/drivers/stm32_etzpc.h
 create mode 100644 core/include/drivers/stm32_gpio.h
 create mode 100644 core/include/drivers/stm32_i2c.h
 create mode 100644 core/include/drivers/stm32_iwdg.h
 create mode 100644 core/include/drivers/stm32_rng.h
 create mode 100644 core/include/drivers/stm32_rtc.h
 create mode 100644 core/include/drivers/stm32_timer.h
 create mode 100644 core/include/drivers/stpmic1.h
 create mode 100644 core/include/dt-bindings/clock/stm32mp1-clks.h
 create mode 100644 core/include/dt-bindings/clock/stm32mp1-clksrc.h
 create mode 100644 core/include/dt-bindings/etzpc/stm32-etzpc.h
 create mode 100644 core/include/dt-bindings/interrupt-controller/arm-gic.h
 create mode 100644 core/include/dt-bindings/pinctrl/stm32-pinfunc.h
 create mode 100644 core/include/dt-bindings/power/stm32mp1-power.h
 create mode 100644 core/include/dt-bindings/reset/stm32mp1-resets.h
 create mode 100644 core/secure_dt.mk
 create mode 100755 scripts/bin_to_c.py

diff --git a/core/arch/arm/fdts/stm32mp15-ddr.dtsi b/core/arch/arm/fdts/stm32mp15-ddr.dtsi
new file mode 100644
index 0000000..1a5c51c
--- /dev/null
+++ b/core/arch/arm/fdts/stm32mp15-ddr.dtsi
@@ -0,0 +1,153 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+/ {
+	soc {
+		ddr: ddr@5A003000{
+
+			compatible = "st,stm32mp1-ddr";
+
+			reg = <0x5A003000 0x550
+			       0x5A004000 0x234>;
+
+			clocks = <&rcc AXIDCG>,
+				 <&rcc DDRC1>,
+				 <&rcc DDRC2>,
+				 <&rcc DDRPHYC>,
+				 <&rcc DDRCAPB>,
+				 <&rcc DDRPHYCAPB>;
+
+			clock-names = "axidcg",
+				      "ddrc1",
+				      "ddrc2",
+				      "ddrphyc",
+				      "ddrcapb",
+				      "ddrphycapb";
+
+			st,mem-name = DDR_MEM_NAME;
+			st,mem-speed = <DDR_MEM_SPEED>;
+			st,mem-size = <DDR_MEM_SIZE>;
+
+			st,ctl-reg = <
+				DDR_MSTR
+				DDR_MRCTRL0
+				DDR_MRCTRL1
+				DDR_DERATEEN
+				DDR_DERATEINT
+				DDR_PWRCTL
+				DDR_PWRTMG
+				DDR_HWLPCTL
+				DDR_RFSHCTL0
+				DDR_RFSHCTL3
+				DDR_CRCPARCTL0
+				DDR_ZQCTL0
+				DDR_DFITMG0
+				DDR_DFITMG1
+				DDR_DFILPCFG0
+				DDR_DFIUPD0
+				DDR_DFIUPD1
+				DDR_DFIUPD2
+				DDR_DFIPHYMSTR
+				DDR_ODTMAP
+				DDR_DBG0
+				DDR_DBG1
+				DDR_DBGCMD
+				DDR_POISONCFG
+				DDR_PCCFG
+			>;
+
+			st,ctl-timing = <
+				DDR_RFSHTMG
+				DDR_DRAMTMG0
+				DDR_DRAMTMG1
+				DDR_DRAMTMG2
+				DDR_DRAMTMG3
+				DDR_DRAMTMG4
+				DDR_DRAMTMG5
+				DDR_DRAMTMG6
+				DDR_DRAMTMG7
+				DDR_DRAMTMG8
+				DDR_DRAMTMG14
+				DDR_ODTCFG
+			>;
+
+			st,ctl-map = <
+				DDR_ADDRMAP1
+				DDR_ADDRMAP2
+				DDR_ADDRMAP3
+				DDR_ADDRMAP4
+				DDR_ADDRMAP5
+				DDR_ADDRMAP6
+				DDR_ADDRMAP9
+				DDR_ADDRMAP10
+				DDR_ADDRMAP11
+			>;
+
+			st,ctl-perf = <
+				DDR_SCHED
+				DDR_SCHED1
+				DDR_PERFHPR1
+				DDR_PERFLPR1
+				DDR_PERFWR1
+				DDR_PCFGR_0
+				DDR_PCFGW_0
+				DDR_PCFGQOS0_0
+				DDR_PCFGQOS1_0
+				DDR_PCFGWQOS0_0
+				DDR_PCFGWQOS1_0
+				DDR_PCFGR_1
+				DDR_PCFGW_1
+				DDR_PCFGQOS0_1
+				DDR_PCFGQOS1_1
+				DDR_PCFGWQOS0_1
+				DDR_PCFGWQOS1_1
+			>;
+
+			st,phy-reg = <
+				DDR_PGCR
+				DDR_ACIOCR
+				DDR_DXCCR
+				DDR_DSGCR
+				DDR_DCR
+				DDR_ODTCR
+				DDR_ZQ0CR1
+				DDR_DX0GCR
+				DDR_DX1GCR
+				DDR_DX2GCR
+				DDR_DX3GCR
+			>;
+
+			st,phy-timing = <
+				DDR_PTR0
+				DDR_PTR1
+				DDR_PTR2
+				DDR_DTPR0
+				DDR_DTPR1
+				DDR_DTPR2
+				DDR_MR0
+				DDR_MR1
+				DDR_MR2
+				DDR_MR3
+			>;
+
+			st,phy-cal = <
+				DDR_DX0DLLCR
+				DDR_DX0DQTR
+				DDR_DX0DQSTR
+				DDR_DX1DLLCR
+				DDR_DX1DQTR
+				DDR_DX1DQSTR
+				DDR_DX2DLLCR
+				DDR_DX2DQTR
+				DDR_DX2DQSTR
+				DDR_DX3DLLCR
+				DDR_DX3DQTR
+				DDR_DX3DQSTR
+			>;
+
+			status = "okay";
+		};
+	};
+};
diff --git a/core/arch/arm/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi b/core/arch/arm/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi
new file mode 100644
index 0000000..16b8cf6
--- /dev/null
+++ b/core/arch/arm/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi
@@ -0,0 +1,121 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+/* STM32MP157C DK1/DK2 BOARD configuration
+ * 1x DDR3L 4Gb, 16-bit, 533MHz.
+ * Reference used NT5CC256M16DP-DI from NANYA
+ *
+ * DDR type / Platform	DDR3/3L
+ * freq		533MHz
+ * width	16
+ * datasheet	0  = MT41J256M16-187 / DDR3-1066 bin G
+ * DDR density	4
+ * timing mode	optimized
+ * Scheduling/QoS options : type = 2
+ * address mapping : RBC
+ * Tc > + 85C : N
+ */
+
+#define DDR_MEM_NAME "DDR3-1066/888 bin G 1x4Gb 533MHz v1.41"
+#define DDR_MEM_SPEED 533000
+#define DDR_MEM_SIZE 0x20000000
+
+#define DDR_MSTR 0x00041401
+#define DDR_MRCTRL0 0x00000010
+#define DDR_MRCTRL1 0x00000000
+#define DDR_DERATEEN 0x00000000
+#define DDR_DERATEINT 0x00800000
+#define DDR_PWRCTL 0x00000000
+#define DDR_PWRTMG 0x00400010
+#define DDR_HWLPCTL 0x00000000
+#define DDR_RFSHCTL0 0x00210000
+#define DDR_RFSHCTL3 0x00000000
+#define DDR_RFSHTMG 0x0081008B
+#define DDR_CRCPARCTL0 0x00000000
+#define DDR_DRAMTMG0 0x121B2414
+#define DDR_DRAMTMG1 0x000A041C
+#define DDR_DRAMTMG2 0x0608090F
+#define DDR_DRAMTMG3 0x0050400C
+#define DDR_DRAMTMG4 0x08040608
+#define DDR_DRAMTMG5 0x06060403
+#define DDR_DRAMTMG6 0x02020002
+#define DDR_DRAMTMG7 0x00000202
+#define DDR_DRAMTMG8 0x00001005
+#define DDR_DRAMTMG14 0x000000A0
+#define DDR_ZQCTL0 0xC2000040
+#define DDR_DFITMG0 0x02060105
+#define DDR_DFITMG1 0x00000202
+#define DDR_DFILPCFG0 0x07000000
+#define DDR_DFIUPD0 0xC0400003
+#define DDR_DFIUPD1 0x00000000
+#define DDR_DFIUPD2 0x00000000
+#define DDR_DFIPHYMSTR 0x00000000
+#define DDR_ADDRMAP1 0x00070707
+#define DDR_ADDRMAP2 0x00000000
+#define DDR_ADDRMAP3 0x1F000000
+#define DDR_ADDRMAP4 0x00001F1F
+#define DDR_ADDRMAP5 0x06060606
+#define DDR_ADDRMAP6 0x0F060606
+#define DDR_ADDRMAP9 0x00000000
+#define DDR_ADDRMAP10 0x00000000
+#define DDR_ADDRMAP11 0x00000000
+#define DDR_ODTCFG 0x06000600
+#define DDR_ODTMAP 0x00000001
+#define DDR_SCHED 0x00000C01
+#define DDR_SCHED1 0x00000000
+#define DDR_PERFHPR1 0x01000001
+#define DDR_PERFLPR1 0x08000200
+#define DDR_PERFWR1 0x08000400
+#define DDR_DBG0 0x00000000
+#define DDR_DBG1 0x00000000
+#define DDR_DBGCMD 0x00000000
+#define DDR_POISONCFG 0x00000000
+#define DDR_PCCFG 0x00000010
+#define DDR_PCFGR_0 0x00010000
+#define DDR_PCFGW_0 0x00000000
+#define DDR_PCFGQOS0_0 0x02100C03
+#define DDR_PCFGQOS1_0 0x00800100
+#define DDR_PCFGWQOS0_0 0x01100C03
+#define DDR_PCFGWQOS1_0 0x01000200
+#define DDR_PCFGR_1 0x00010000
+#define DDR_PCFGW_1 0x00000000
+#define DDR_PCFGQOS0_1 0x02100C03
+#define DDR_PCFGQOS1_1 0x00800040
+#define DDR_PCFGWQOS0_1 0x01100C03
+#define DDR_PCFGWQOS1_1 0x01000200
+#define DDR_PGCR 0x01442E02
+#define DDR_PTR0 0x0022AA5B
+#define DDR_PTR1 0x04841104
+#define DDR_PTR2 0x042DA068
+#define DDR_ACIOCR 0x10400812
+#define DDR_DXCCR 0x00000C40
+#define DDR_DSGCR 0xF200001F
+#define DDR_DCR 0x0000000B
+#define DDR_DTPR0 0x38D488D0
+#define DDR_DTPR1 0x098B00D8
+#define DDR_DTPR2 0x10023600
+#define DDR_MR0 0x00000840
+#define DDR_MR1 0x00000000
+#define DDR_MR2 0x00000208
+#define DDR_MR3 0x00000000
+#define DDR_ODTCR 0x00010000
+#define DDR_ZQ0CR1 0x00000038
+#define DDR_DX0GCR 0x0000CE81
+#define DDR_DX0DLLCR 0x40000000
+#define DDR_DX0DQTR 0xFFFFFFFF
+#define DDR_DX0DQSTR 0x3DB02000
+#define DDR_DX1GCR 0x0000CE81
+#define DDR_DX1DLLCR 0x40000000
+#define DDR_DX1DQTR 0xFFFFFFFF
+#define DDR_DX1DQSTR 0x3DB02000
+#define DDR_DX2GCR 0x0000CE81
+#define DDR_DX2DLLCR 0x40000000
+#define DDR_DX2DQTR 0xFFFFFFFF
+#define DDR_DX2DQSTR 0x3DB02000
+#define DDR_DX3GCR 0x0000CE81
+#define DDR_DX3DLLCR 0x40000000
+#define DDR_DX3DQTR 0xFFFFFFFF
+#define DDR_DX3DQSTR 0x3DB02000
+
+#include "stm32mp15-ddr.dtsi"
diff --git a/core/arch/arm/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi b/core/arch/arm/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi
new file mode 100644
index 0000000..82e7104
--- /dev/null
+++ b/core/arch/arm/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi
@@ -0,0 +1,122 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+/* STM32MP157C ED1 BOARD configuration
+ * 2x DDR3L 4Gb each, 16-bit, 533MHz, Single Die Package in flyby topology.
+ * Reference used NT5CC256M16DP-DI from NANYA
+ *
+ * DDR type / Platform	DDR3/3L
+ * freq		533MHz
+ * width	32
+ * datasheet	0  = MT41J256M16-187 / DDR3-1066 bin G
+ * DDR density	8
+ * timing mode	optimized
+ * Scheduling/QoS options : type = 2
+ * address mapping : RBC
+ * Tc > + 85C : N
+ */
+
+#define DDR_MEM_NAME "DDR3-1066/888 bin G 2x4Gb 533MHz v1.41"
+#define DDR_MEM_SPEED 533000
+#define DDR_MEM_SIZE 0x40000000
+
+#define DDR_MSTR 0x00040401
+#define DDR_MRCTRL0 0x00000010
+#define DDR_MRCTRL1 0x00000000
+#define DDR_DERATEEN 0x00000000
+#define DDR_DERATEINT 0x00800000
+#define DDR_PWRCTL 0x00000000
+#define DDR_PWRTMG 0x00400010
+#define DDR_HWLPCTL 0x00000000
+#define DDR_RFSHCTL0 0x00210000
+#define DDR_RFSHCTL3 0x00000000
+#define DDR_RFSHTMG 0x0081008B
+#define DDR_CRCPARCTL0 0x00000000
+#define DDR_DRAMTMG0 0x121B2414
+#define DDR_DRAMTMG1 0x000A041C
+#define DDR_DRAMTMG2 0x0608090F
+#define DDR_DRAMTMG3 0x0050400C
+#define DDR_DRAMTMG4 0x08040608
+#define DDR_DRAMTMG5 0x06060403
+#define DDR_DRAMTMG6 0x02020002
+#define DDR_DRAMTMG7 0x00000202
+#define DDR_DRAMTMG8 0x00001005
+#define DDR_DRAMTMG14 0x000000A0
+#define DDR_ZQCTL0 0xC2000040
+#define DDR_DFITMG0 0x02060105
+#define DDR_DFITMG1 0x00000202
+#define DDR_DFILPCFG0 0x07000000
+#define DDR_DFIUPD0 0xC0400003
+#define DDR_DFIUPD1 0x00000000
+#define DDR_DFIUPD2 0x00000000
+#define DDR_DFIPHYMSTR 0x00000000
+#define DDR_ADDRMAP1 0x00080808
+#define DDR_ADDRMAP2 0x00000000
+#define DDR_ADDRMAP3 0x00000000
+#define DDR_ADDRMAP4 0x00001F1F
+#define DDR_ADDRMAP5 0x07070707
+#define DDR_ADDRMAP6 0x0F070707
+#define DDR_ADDRMAP9 0x00000000
+#define DDR_ADDRMAP10 0x00000000
+#define DDR_ADDRMAP11 0x00000000
+#define DDR_ODTCFG 0x06000600
+#define DDR_ODTMAP 0x00000001
+#define DDR_SCHED 0x00000C01
+#define DDR_SCHED1 0x00000000
+#define DDR_PERFHPR1 0x01000001
+#define DDR_PERFLPR1 0x08000200
+#define DDR_PERFWR1 0x08000400
+#define DDR_DBG0 0x00000000
+#define DDR_DBG1 0x00000000
+#define DDR_DBGCMD 0x00000000
+#define DDR_POISONCFG 0x00000000
+#define DDR_PCCFG 0x00000010
+#define DDR_PCFGR_0 0x00010000
+#define DDR_PCFGW_0 0x00000000
+#define DDR_PCFGQOS0_0 0x02100C03
+#define DDR_PCFGQOS1_0 0x00800100
+#define DDR_PCFGWQOS0_0 0x01100C03
+#define DDR_PCFGWQOS1_0 0x01000200
+#define DDR_PCFGR_1 0x00010000
+#define DDR_PCFGW_1 0x00000000
+#define DDR_PCFGQOS0_1 0x02100C03
+#define DDR_PCFGQOS1_1 0x00800040
+#define DDR_PCFGWQOS0_1 0x01100C03
+#define DDR_PCFGWQOS1_1 0x01000200
+#define DDR_PGCR 0x01442E02
+#define DDR_PTR0 0x0022AA5B
+#define DDR_PTR1 0x04841104
+#define DDR_PTR2 0x042DA068
+#define DDR_ACIOCR 0x10400812
+#define DDR_DXCCR 0x00000C40
+#define DDR_DSGCR 0xF200001F
+#define DDR_DCR 0x0000000B
+#define DDR_DTPR0 0x38D488D0
+#define DDR_DTPR1 0x098B00D8
+#define DDR_DTPR2 0x10023600
+#define DDR_MR0 0x00000840
+#define DDR_MR1 0x00000000
+#define DDR_MR2 0x00000208
+#define DDR_MR3 0x00000000
+#define DDR_ODTCR 0x00010000
+#define DDR_ZQ0CR1 0x00000038
+#define DDR_DX0GCR 0x0000CE81
+#define DDR_DX0DLLCR 0x40000000
+#define DDR_DX0DQTR 0xFFFFFFFF
+#define DDR_DX0DQSTR 0x3DB02000
+#define DDR_DX1GCR 0x0000CE81
+#define DDR_DX1DLLCR 0x40000000
+#define DDR_DX1DQTR 0xFFFFFFFF
+#define DDR_DX1DQSTR 0x3DB02000
+#define DDR_DX2GCR 0x0000CE81
+#define DDR_DX2DLLCR 0x40000000
+#define DDR_DX2DQTR 0xFFFFFFFF
+#define DDR_DX2DQSTR 0x3DB02000
+#define DDR_DX3GCR 0x0000CE81
+#define DDR_DX3DLLCR 0x40000000
+#define DDR_DX3DQTR 0xFFFFFFFF
+#define DDR_DX3DQSTR 0x3DB02000
+
+#include "stm32mp15-ddr.dtsi"
diff --git a/core/arch/arm/fdts/stm32mp157-pinctrl.dtsi b/core/arch/arm/fdts/stm32mp157-pinctrl.dtsi
new file mode 100644
index 0000000..8037e4f
--- /dev/null
+++ b/core/arch/arm/fdts/stm32mp157-pinctrl.dtsi
@@ -0,0 +1,350 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2017 - All Rights Reserved
+ * Author: Ludovic Barre <ludovic.barre@st.com> for STMicroelectronics.
+ */
+#include <dt-bindings/pinctrl/stm32-pinfunc.h>
+
+/ {
+	soc {
+		pinctrl: pin-controller@50002000 {
+			#address-cells = <1>;
+			#size-cells = <1>;
+			compatible = "st,stm32mp157-pinctrl";
+			ranges = <0 0x50002000 0xa400>;
+			pins-are-numbered;
+
+			gpioa: gpio@50002000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x0 0x400>;
+				clocks = <&rcc GPIOA>;
+				st,bank-name = "GPIOA";
+				status = "disabled";
+			};
+
+			gpiob: gpio@50003000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x1000 0x400>;
+				clocks = <&rcc GPIOB>;
+				st,bank-name = "GPIOB";
+				status = "disabled";
+			};
+
+			gpioc: gpio@50004000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x2000 0x400>;
+				clocks = <&rcc GPIOC>;
+				st,bank-name = "GPIOC";
+				status = "disabled";
+			};
+
+			gpiod: gpio@50005000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x3000 0x400>;
+				clocks = <&rcc GPIOD>;
+				st,bank-name = "GPIOD";
+				status = "disabled";
+			};
+
+			gpioe: gpio@50006000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x4000 0x400>;
+				clocks = <&rcc GPIOE>;
+				st,bank-name = "GPIOE";
+				status = "disabled";
+			};
+
+			gpiof: gpio@50007000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x5000 0x400>;
+				clocks = <&rcc GPIOF>;
+				st,bank-name = "GPIOF";
+				status = "disabled";
+			};
+
+			gpiog: gpio@50008000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x6000 0x400>;
+				clocks = <&rcc GPIOG>;
+				st,bank-name = "GPIOG";
+				status = "disabled";
+			};
+
+			gpioh: gpio@50009000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x7000 0x400>;
+				clocks = <&rcc GPIOH>;
+				st,bank-name = "GPIOH";
+				status = "disabled";
+			};
+
+			gpioi: gpio@5000a000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x8000 0x400>;
+				clocks = <&rcc GPIOI>;
+				st,bank-name = "GPIOI";
+				status = "disabled";
+			};
+
+			gpioj: gpio@5000b000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x9000 0x400>;
+				clocks = <&rcc GPIOJ>;
+				st,bank-name = "GPIOJ";
+				status = "disabled";
+			};
+
+			gpiok: gpio@5000c000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0xa000 0x400>;
+				clocks = <&rcc GPIOK>;
+				st,bank-name = "GPIOK";
+				status = "disabled";
+			};
+
+			qspi_bk1_pins_a: qspi-bk1-0 {
+				pins1 {
+					pinmux = <STM32_PINMUX('F', 8, AF10)>, /* QSPI_BK1_IO0 */
+						 <STM32_PINMUX('F', 9, AF10)>, /* QSPI_BK1_IO1 */
+						 <STM32_PINMUX('F', 7, AF9)>, /* QSPI_BK1_IO2 */
+						 <STM32_PINMUX('F', 6, AF9)>; /* QSPI_BK1_IO3 */
+					bias-disable;
+					drive-push-pull;
+					slew-rate = <1>;
+				};
+				pins2 {
+					pinmux = <STM32_PINMUX('B', 6, AF10)>; /* QSPI_BK1_NCS */
+					bias-pull-up;
+					drive-push-pull;
+					slew-rate = <1>;
+				};
+			};
+
+			qspi_bk2_pins_a: qspi-bk2-0 {
+				pins1 {
+					pinmux = <STM32_PINMUX('H', 2, AF9)>, /* QSPI_BK2_IO0 */
+						 <STM32_PINMUX('H', 3, AF9)>, /* QSPI_BK2_IO1 */
+						 <STM32_PINMUX('G', 10, AF11)>, /* QSPI_BK2_IO2 */
+						 <STM32_PINMUX('G', 7, AF11)>; /* QSPI_BK2_IO3 */
+					bias-disable;
+					drive-push-pull;
+					slew-rate = <1>;
+				};
+				pins2 {
+					pinmux = <STM32_PINMUX('C', 0, AF10)>; /* QSPI_BK2_NCS */
+					bias-pull-up;
+					drive-push-pull;
+					slew-rate = <1>;
+				};
+			};
+
+			qspi_clk_pins_a: qspi-clk-0 {
+				pins {
+					pinmux = <STM32_PINMUX('F', 10, AF9)>; /* QSPI_CLK */
+					bias-disable;
+					drive-push-pull;
+					slew-rate = <3>;
+				};
+			};
+
+			sdmmc1_b4_pins_a: sdmmc1-b4-0 {
+				pins1 {
+					pinmux = <STM32_PINMUX('C', 8, AF12)>, /* SDMMC1_D0 */
+						 <STM32_PINMUX('C', 9, AF12)>, /* SDMMC1_D1 */
+						 <STM32_PINMUX('C', 10, AF12)>, /* SDMMC1_D2 */
+						 <STM32_PINMUX('C', 11, AF12)>, /* SDMMC1_D3 */
+						 <STM32_PINMUX('D', 2, AF12)>; /* SDMMC1_CMD */
+					slew-rate = <1>;
+					drive-push-pull;
+					bias-disable;
+				};
+				pins2 {
+					pinmux = <STM32_PINMUX('C', 12, AF12)>; /* SDMMC1_CK */
+					slew-rate = <2>;
+					drive-push-pull;
+					bias-disable;
+				};
+			};
+
+			sdmmc1_dir_pins_a: sdmmc1-dir-0 {
+				pins1 {
+					pinmux = <STM32_PINMUX('F', 2, AF11)>, /* SDMMC1_D0DIR */
+						 <STM32_PINMUX('C', 7, AF8)>, /* SDMMC1_D123DIR */
+						 <STM32_PINMUX('B', 9, AF11)>; /* SDMMC1_CDIR */
+					slew-rate = <1>;
+					drive-push-pull;
+					bias-pull-up;
+				};
+				pins2{
+					pinmux = <STM32_PINMUX('E', 4, AF8)>; /* SDMMC1_CKIN */
+					bias-pull-up;
+				};
+			};
+
+			sdmmc1_dir_pins_b: sdmmc1-dir-1 {
+				pins1 {
+					pinmux = <STM32_PINMUX('E', 12, AF8)>, /* SDMMC1_D0DIR */
+						 <STM32_PINMUX('E', 14, AF11)>, /* SDMMC1_D123DIR */
+						 <STM32_PINMUX('B', 9, AF11)>; /* SDMMC1_CDIR */
+					slew-rate = <3>;
+					drive-push-pull;
+					bias-pull-up;
+				};
+				pins2 {
+					pinmux = <STM32_PINMUX('E', 4, AF8)>; /* SDMMC1_CKIN */
+					bias-pull-up;
+				};
+			};
+
+			sdmmc2_b4_pins_a: sdmmc2-b4-0 {
+				pins1 {
+					pinmux = <STM32_PINMUX('B', 14, AF9)>, /* SDMMC2_D0 */
+						 <STM32_PINMUX('B', 15, AF9)>, /* SDMMC2_D1 */
+						 <STM32_PINMUX('B', 3, AF9)>, /* SDMMC2_D2 */
+						 <STM32_PINMUX('B', 4, AF9)>, /* SDMMC2_D3 */
+						 <STM32_PINMUX('G', 6, AF10)>; /* SDMMC2_CMD */
+					slew-rate = <1>;
+					drive-push-pull;
+					bias-pull-up;
+				};
+				pins2 {
+					pinmux = <STM32_PINMUX('E', 3, AF9)>; /* SDMMC2_CK */
+					slew-rate = <2>;
+					drive-push-pull;
+					bias-pull-up;
+				};
+			};
+
+			sdmmc2_d47_pins_a: sdmmc2-d47-0 {
+				pins {
+					pinmux = <STM32_PINMUX('A', 8, AF9)>, /* SDMMC2_D4 */
+						 <STM32_PINMUX('A', 9, AF10)>, /* SDMMC2_D5 */
+						 <STM32_PINMUX('E', 5, AF9)>, /* SDMMC2_D6 */
+						 <STM32_PINMUX('D', 3, AF9)>; /* SDMMC2_D7 */
+					slew-rate = <1>;
+					drive-push-pull;
+					bias-pull-up;
+				};
+			};
+
+			uart4_pins_a: uart4-0 {
+				pins1 {
+					pinmux = <STM32_PINMUX('G', 11, AF6)>; /* UART4_TX */
+					bias-disable;
+					drive-push-pull;
+					slew-rate = <0>;
+				};
+				pins2 {
+					pinmux = <STM32_PINMUX('B', 2, AF8)>; /* UART4_RX */
+					bias-disable;
+				};
+			};
+
+			uart7_pins_a: uart7-0 {
+				pins1 {
+					pinmux = <STM32_PINMUX('E', 8, AF7)>; /* USART7_TX */
+					bias-disable;
+					drive-push-pull;
+					slew-rate = <0>;
+				};
+				pins2 {
+					pinmux = <STM32_PINMUX('E', 7, AF7)>; /* USART7_RX */
+					bias-disable;
+				};
+			};
+
+			usart3_pins_a: usart3-0 {
+				pins1 {
+					pinmux = <STM32_PINMUX('B', 10, AF7)>, /* USART3_TX */
+						 <STM32_PINMUX('G', 8, AF8)>; /* USART3_RTS */
+					bias-disable;
+					drive-push-pull;
+					slew-rate = <0>;
+				};
+				pins2 {
+					pinmux = <STM32_PINMUX('B', 12, AF8)>, /* USART3_RX */
+						 <STM32_PINMUX('I', 10, AF8)>; /* USART3_CTS_NSS */
+					bias-disable;
+				};
+			};
+
+			usart3_pins_b: usart3-1 {
+				pins1 {
+					pinmux = <STM32_PINMUX('B', 10, AF7)>, /* USART3_TX */
+						 <STM32_PINMUX('G', 8, AF8)>; /* USART3_RTS */
+					bias-disable;
+					drive-push-pull;
+					slew-rate = <0>;
+				};
+				pins2 {
+					pinmux = <STM32_PINMUX('B', 12, AF8)>, /* USART3_RX */
+						 <STM32_PINMUX('B', 13, AF7)>; /* USART3_CTS_NSS */
+					bias-disable;
+				};
+			};
+		};
+
+		pinctrl_z: pin-controller-z@54004000 {
+			#address-cells = <1>;
+			#size-cells = <1>;
+			compatible = "st,stm32mp157-z-pinctrl";
+			ranges = <0 0x54004000 0x400>;
+			pins-are-numbered;
+
+			gpioz: gpio@54004000 {
+				gpio-controller;
+				#gpio-cells = <2>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0 0x400>;
+				clocks = <&rcc GPIOZ>;
+				st,bank-name = "GPIOZ";
+				st,bank-ioport = <11>;
+				status = "disabled";
+			};
+
+			i2c4_pins_a: i2c4-0 {
+				pins {
+					pinmux = <STM32_PINMUX('Z', 4, AF6)>, /* I2C4_SCL */
+						 <STM32_PINMUX('Z', 5, AF6)>; /* I2C4_SDA */
+					bias-disable;
+					drive-open-drain;
+					slew-rate = <0>;
+				};
+			};
+		};
+	};
+};
diff --git a/core/arch/arm/fdts/stm32mp157a-dk1.dts b/core/arch/arm/fdts/stm32mp157a-dk1.dts
new file mode 100644
index 0000000..e42bdcd
--- /dev/null
+++ b/core/arch/arm/fdts/stm32mp157a-dk1.dts
@@ -0,0 +1,380 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2018-2019 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com>.
+ */
+
+/dts-v1/;
+
+#include "stm32mp157c.dtsi"
+#include "stm32mp157cac-pinctrl.dtsi"
+
+/ {
+	model = "STMicroelectronics STM32MP157A-DK1 Discovery Board";
+	compatible = "st,stm32mp157a-dk1", "st,stm32mp157";
+
+	aliases {
+		serial0 = &uart4;
+		serial1 = &usart3;
+		serial2 = &uart7;
+	};
+
+	chosen {
+		stdout-path = "serial0:115200n8";
+};
+
+};
+
+&clk_hse {
+	st,digbypass;
+};
+
+&i2c4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&i2c4_pins_a>;
+	i2c-scl-rising-time-ns = <185>;
+	i2c-scl-falling-time-ns = <20>;
+	status = "okay";
+
+	pmic: stpmic@33 {
+		compatible = "st,stpmic1";
+		reg = <0x33>;
+		interrupts-extended = <&exti_pwr 55 IRQ_TYPE_EDGE_FALLING>;
+		interrupt-controller;
+		#interrupt-cells = <2>;
+		status = "okay";
+
+		st,main-control-register = <0x04>;
+		st,vin-control-register = <0xc0>;
+		st,usb-control-register = <0x20>;
+
+		regulators {
+			compatible = "st,stpmic1-regulators";
+
+			ldo1-supply = <&v3v3>;
+			ldo3-supply = <&vdd_ddr>;
+			ldo6-supply = <&v3v3>;
+
+			vddcore: buck1 {
+				regulator-name = "vddcore";
+				regulator-min-microvolt = <1200000>;
+				regulator-max-microvolt = <1350000>;
+				regulator-always-on;
+				regulator-initial-mode = <0>;
+				regulator-over-current-protection;
+			};
+
+			vdd_ddr: buck2 {
+				regulator-name = "vdd_ddr";
+				regulator-min-microvolt = <1350000>;
+				regulator-max-microvolt = <1350000>;
+				regulator-always-on;
+				regulator-initial-mode = <0>;
+				regulator-over-current-protection;
+			};
+
+			vdd: buck3 {
+				regulator-name = "vdd";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+				st,mask-reset;
+				regulator-initial-mode = <0>;
+				regulator-over-current-protection;
+			};
+
+			v3v3: buck4 {
+				regulator-name = "v3v3";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+				regulator-over-current-protection;
+				regulator-initial-mode = <0>;
+			};
+
+			v1v8_audio: ldo1 {
+				regulator-name = "v1v8_audio";
+				regulator-min-microvolt = <1800000>;
+				regulator-max-microvolt = <1800000>;
+				regulator-always-on;
+			};
+
+			v3v3_hdmi: ldo2 {
+				regulator-name = "v3v3_hdmi";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+			};
+
+			vtt_ddr: ldo3 {
+				regulator-name = "vtt_ddr";
+				regulator-min-microvolt = <500000>;
+				regulator-max-microvolt = <750000>;
+				regulator-always-on;
+				regulator-over-current-protection;
+			};
+
+			vdd_usb: ldo4 {
+				regulator-name = "vdd_usb";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+			};
+
+			vdda: ldo5 {
+				regulator-name = "vdda";
+				regulator-min-microvolt = <2900000>;
+				regulator-max-microvolt = <2900000>;
+				regulator-boot-on;
+			};
+
+			v1v2_hdmi: ldo6 {
+				regulator-name = "v1v2_hdmi";
+				regulator-min-microvolt = <1200000>;
+				regulator-max-microvolt = <1200000>;
+				regulator-always-on;
+			};
+
+			vref_ddr: vref_ddr {
+				regulator-name = "vref_ddr";
+				regulator-always-on;
+				regulator-over-current-protection;
+			};
+		};
+	};
+};
+
+&iwdg2 {
+	timeout-sec = <32>;
+	status = "okay";
+};
+
+&pwr {
+	pwr-supply = <&vdd>;
+};
+
+/* RNG1 is used by non secure at run time and by secure (low power sequences) */
+&rng1 {
+	status = "okay";
+	secure-status = "okay";
+};
+
+&rtc {
+	status = "okay";
+};
+
+&sdmmc1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&sdmmc1_b4_pins_a>;
+	broken-cd;
+	st,neg-edge;
+	bus-width = <4>;
+	vmmc-supply = <&v3v3>;
+	status = "okay";
+};
+
+&uart4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&uart4_pins_a>;
+	status = "okay";
+};
+
+&uart7 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&uart7_pins_a>;
+	status = "disabled";
+};
+
+&usart3 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&usart3_pins_b>;
+	status = "disabled";
+};
+
+
+/* ATF Specific */
+#include <dt-bindings/clock/stm32mp1-clksrc.h>
+#include <dt-bindings/power/stm32mp1-power.h>
+#include "stm32mp15-ddr3-1x4Gb-1066-binG.dtsi"
+#include "stm32mp157c-security.dtsi"
+
+/ {
+	aliases {
+		gpio0 = &gpioa;
+		gpio1 = &gpiob;
+		gpio2 = &gpioc;
+		gpio3 = &gpiod;
+		gpio4 = &gpioe;
+		gpio5 = &gpiof;
+		gpio6 = &gpiog;
+		gpio7 = &gpioh;
+		gpio8 = &gpioi;
+		gpio25 = &gpioz;
+		i2c3 = &i2c4;
+	};
+};
+
+/* CLOCK presence */
+&rcc {
+	secure-status = "okay";
+	st,hsi-cal;
+	st,csi-cal;
+	st,cal-sec = <60>;
+};
+
+/* Security specific */
+&etzpc {
+	st,decprot = <
+		DECPROT(STM32MP1_ETZPC_USART1_ID, DECPROT_NS_RW, DECPROT_UNLOCK)
+		DECPROT(STM32MP1_ETZPC_SPI6_ID, DECPROT_NS_RW, DECPROT_UNLOCK)
+		DECPROT(STM32MP1_ETZPC_I2C4_ID, DECPROT_NS_RW, DECPROT_UNLOCK)
+		DECPROT(STM32MP1_ETZPC_I2C6_ID, DECPROT_NS_RW, DECPROT_UNLOCK)
+		DECPROT(STM32MP1_ETZPC_RNG1_ID, DECPROT_NS_RW, DECPROT_UNLOCK)
+		DECPROT(STM32MP1_ETZPC_HASH1_ID, DECPROT_NS_RW, DECPROT_UNLOCK)
+		DECPROT(STM32MP1_ETZPC_CRYP1_ID, DECPROT_NS_RW, DECPROT_UNLOCK)
+	>;
+};
+
+&iwdg2 {
+	secure-status = "okay";
+};
+
+&pwr {
+	system_suspend_supported_soc_modes = <
+		STM32_PM_CSLEEP_RUN
+		STM32_PM_CSTOP_ALLOW_LP_STOP
+		STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR
+	>;
+
+	system_off_soc_mode = <STM32_PM_CSTOP_ALLOW_STANDBY_DDR_OFF>;
+};
+
+&timers15 {
+	secure-status = "okay";
+	st,hsi-cal-input = <7>;
+	st,csi_cal-input = <8>;
+};
+
+/* Low-power states of regulators */
+&vddcore {
+	lp-stop {
+		regulator-on-in-suspend;
+		regulator-suspend-microvolt = <1200000>;
+	};
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vdd_ddr {
+	lp-stop {
+		regulator-suspend-microvolt = <1350000>;
+		regulator-on-in-suspend;
+	};
+	standby-ddr-sr {
+		regulator-suspend-microvolt = <1350000>;
+		regulator-on-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vdd {
+	lp-stop {
+		regulator-suspend-microvolt = <3300000>;
+		regulator-on-in-suspend;
+	};
+	standby-ddr-sr {
+		regulator-suspend-microvolt = <3300000>;
+		regulator-on-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-suspend-microvolt = <3300000>;
+		regulator-on-in-suspend;
+	};
+};
+
+&v3v3 {
+	lp-stop {
+		regulator-suspend-microvolt = <3300000>;
+		regulator-on-in-suspend;
+	};
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&v1v8_audio {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&v3v3_hdmi {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vtt_ddr {
+	lp-stop {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vdd_usb {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vdda {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&v1v2_hdmi {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vref_ddr {
+	lp-stop {
+		regulator-on-in-suspend;
+	};
+	standby-ddr-sr {
+		regulator-on-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
diff --git a/core/arch/arm/fdts/stm32mp157c-dk2.dts b/core/arch/arm/fdts/stm32mp157c-dk2.dts
new file mode 100644
index 0000000..fdcf4c8
--- /dev/null
+++ b/core/arch/arm/fdts/stm32mp157c-dk2.dts
@@ -0,0 +1,16 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2018 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com>.
+ */
+
+/dts-v1/;
+
+#include "stm32mp157a-dk1.dts"
+
+/ {
+	model = "STMicroelectronics STM32MP157C-DK2 Discovery Board";
+	compatible = "st,stm32mp157c-dk2", "st,stm32mp157";
+
+};
+
diff --git a/core/arch/arm/fdts/stm32mp157c-ed1.dts b/core/arch/arm/fdts/stm32mp157c-ed1.dts
new file mode 100644
index 0000000..8462d23
--- /dev/null
+++ b/core/arch/arm/fdts/stm32mp157c-ed1.dts
@@ -0,0 +1,381 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2017-2019 - All Rights Reserved
+ * Author: Ludovic Barre <ludovic.barre@st.com> for STMicroelectronics.
+ */
+/dts-v1/;
+
+#include "stm32mp157c.dtsi"
+#include "stm32mp157caa-pinctrl.dtsi"
+
+/ {
+	model = "STMicroelectronics STM32MP157C eval daughter";
+	compatible = "st,stm32mp157c-ed1", "st,stm32mp157";
+
+	chosen {
+		stdout-path = "serial0:115200n8";
+	};
+
+	aliases {
+		serial0 = &uart4;
+	};
+};
+
+&clk_hse {
+	st,digbypass;
+};
+
+&i2c4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&i2c4_pins_a>;
+	i2c-scl-rising-time-ns = <185>;
+	i2c-scl-falling-time-ns = <20>;
+	status = "okay";
+
+	pmic: stpmic@33 {
+		compatible = "st,stpmic1";
+		reg = <0x33>;
+		interrupts-extended = <&exti_pwr 55 IRQ_TYPE_EDGE_FALLING>;
+		interrupt-controller;
+		#interrupt-cells = <2>;
+		status = "okay";
+
+		st,main-control-register = <0x04>;
+		st,vin-control-register = <0xc0>;
+		st,usb-control-register = <0x20>;
+
+		regulators {
+			compatible = "st,stpmic1-regulators";
+
+			ldo1-supply = <&v3v3>;
+			ldo2-supply = <&v3v3>;
+			ldo3-supply = <&vdd_ddr>;
+			ldo5-supply = <&v3v3>;
+			ldo6-supply = <&v3v3>;
+
+			vddcore: buck1 {
+				regulator-name = "vddcore";
+				regulator-min-microvolt = <1200000>;
+				regulator-max-microvolt = <1350000>;
+				regulator-always-on;
+				regulator-initial-mode = <0>;
+				regulator-over-current-protection;
+			};
+
+			vdd_ddr: buck2 {
+				regulator-name = "vdd_ddr";
+				regulator-min-microvolt = <1350000>;
+				regulator-max-microvolt = <1350000>;
+				regulator-always-on;
+				regulator-initial-mode = <0>;
+				regulator-over-current-protection;
+			};
+
+			vdd: buck3 {
+				regulator-name = "vdd";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+				st,mask-reset;
+				regulator-initial-mode = <0>;
+				regulator-over-current-protection;
+			};
+
+			v3v3: buck4 {
+				regulator-name = "v3v3";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+				regulator-over-current-protection;
+				regulator-initial-mode = <0>;
+			};
+
+			vdda: ldo1 {
+				regulator-name = "vdda";
+				regulator-min-microvolt = <2900000>;
+				regulator-max-microvolt = <2900000>;
+			};
+
+			v2v8: ldo2 {
+				regulator-name = "v2v8";
+				regulator-min-microvolt = <2800000>;
+				regulator-max-microvolt = <2800000>;
+			};
+
+			vtt_ddr: ldo3 {
+				regulator-name = "vtt_ddr";
+				regulator-min-microvolt = <500000>;
+				regulator-max-microvolt = <750000>;
+				regulator-always-on;
+				regulator-over-current-protection;
+			};
+
+			vdd_usb: ldo4 {
+				regulator-name = "vdd_usb";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+			};
+
+			vdd_sd: ldo5 {
+				regulator-name = "vdd_sd";
+				regulator-min-microvolt = <2900000>;
+				regulator-max-microvolt = <2900000>;
+				regulator-boot-on;
+			};
+
+			v1v8: ldo6 {
+				regulator-name = "v1v8";
+				regulator-min-microvolt = <1800000>;
+				regulator-max-microvolt = <1800000>;
+			};
+
+			vref_ddr: vref_ddr {
+				regulator-name = "vref_ddr";
+				regulator-always-on;
+				regulator-over-current-protection;
+			};
+		};
+	};
+};
+
+&iwdg2 {
+	timeout-sec = <32>;
+	status = "okay";
+};
+
+&pwr {
+	pwr-supply = <&vdd>;
+};
+
+/* RNG1 is used by non secure at run time and by secure (low power sequences) */
+&rng1 {
+	status = "okay";
+	secure-status = "okay";
+};
+
+&rtc {
+	status = "okay";
+};
+
+&sdmmc1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&sdmmc1_b4_pins_a &sdmmc1_dir_pins_a>;
+	broken-cd;
+	st,sig-dir;
+	st,neg-edge;
+	st,use-ckin;
+	bus-width = <4>;
+	vmmc-supply = <&vdd_sd>;
+	sd-uhs-sdr12;
+	sd-uhs-sdr25;
+	sd-uhs-sdr50;
+	sd-uhs-ddr50;
+	sd-uhs-sdr104;
+	status = "okay";
+};
+
+&sdmmc2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&sdmmc2_b4_pins_a &sdmmc2_d47_pins_a>;
+	non-removable;
+	no-sd;
+	no-sdio;
+	st,neg-edge;
+	bus-width = <8>;
+	vmmc-supply = <&v3v3>;
+	vqmmc-supply = <&v3v3>;
+	mmc-ddr-3_3v;
+	status = "okay";
+};
+
+&uart4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&uart4_pins_a>;
+	status = "okay";
+};
+
+/* ATF Specific */
+#include <dt-bindings/clock/stm32mp1-clksrc.h>
+#include <dt-bindings/power/stm32mp1-power.h>
+#include "stm32mp15-ddr3-2x4Gb-1066-binG.dtsi"
+#include "stm32mp157c-security.dtsi"
+
+/ {
+	aliases {
+		gpio0 = &gpioa;
+		gpio1 = &gpiob;
+		gpio2 = &gpioc;
+		gpio3 = &gpiod;
+		gpio4 = &gpioe;
+		gpio5 = &gpiof;
+		gpio6 = &gpiog;
+		gpio7 = &gpioh;
+		gpio8 = &gpioi;
+		gpio9 = &gpioj;
+		gpio10 = &gpiok;
+		gpio25 = &gpioz;
+		i2c3 = &i2c4;
+	};
+};
+
+/* CLOCK presence */
+&rcc {
+	secure-status = "okay";
+	st,hsi-cal;
+	st,csi-cal;
+	st,cal-sec = <60>;
+};
+
+/* Security specific */
+&etzpc {
+	st,decprot = <
+		DECPROT(STM32MP1_ETZPC_USART1_ID, DECPROT_NS_RW, DECPROT_UNLOCK)
+		DECPROT(STM32MP1_ETZPC_SPI6_ID, DECPROT_NS_RW, DECPROT_UNLOCK)
+		DECPROT(STM32MP1_ETZPC_I2C4_ID, DECPROT_NS_RW, DECPROT_UNLOCK)
+		DECPROT(STM32MP1_ETZPC_I2C6_ID, DECPROT_NS_RW, DECPROT_UNLOCK)
+		DECPROT(STM32MP1_ETZPC_RNG1_ID, DECPROT_NS_RW, DECPROT_UNLOCK)
+		DECPROT(STM32MP1_ETZPC_HASH1_ID, DECPROT_NS_RW, DECPROT_UNLOCK)
+		DECPROT(STM32MP1_ETZPC_CRYP1_ID, DECPROT_NS_RW, DECPROT_UNLOCK)
+	>;
+};
+
+&iwdg2 {
+	secure-status = "okay";
+};
+
+&pwr {
+	system_suspend_supported_soc_modes = <
+		STM32_PM_CSLEEP_RUN
+		STM32_PM_CSTOP_ALLOW_LP_STOP
+		STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR
+	>;
+
+	system_off_soc_mode = <STM32_PM_CSTOP_ALLOW_STANDBY_DDR_OFF>;
+};
+
+&timers15 {
+	secure-status = "okay";
+	st,hsi-cal-input = <7>;
+	st,csi_cal-input = <8>;
+};
+
+/* Low-power states of regulators */
+&vddcore {
+	lp-stop {
+		regulator-on-in-suspend;
+		regulator-suspend-microvolt = <1200000>;
+	};
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vdd_ddr {
+	lp-stop {
+		regulator-suspend-microvolt = <1350000>;
+		regulator-on-in-suspend;
+	};
+	standby-ddr-sr {
+		regulator-suspend-microvolt = <1350000>;
+		regulator-on-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vdd {
+	lp-stop {
+		regulator-suspend-microvolt = <3300000>;
+		regulator-on-in-suspend;
+	};
+	standby-ddr-sr {
+		regulator-suspend-microvolt = <3300000>;
+		regulator-on-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-suspend-microvolt = <3300000>;
+		regulator-on-in-suspend;
+	};
+};
+
+&v3v3 {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vdda {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&v2v8 {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vtt_ddr {
+	lp-stop {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vdd_usb {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vdd_sd {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&v1v8 {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vref_ddr {
+	lp-stop {
+		regulator-on-in-suspend;
+	};
+	standby-ddr-sr {
+		regulator-on-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
diff --git a/core/arch/arm/fdts/stm32mp157c-ev1.dts b/core/arch/arm/fdts/stm32mp157c-ev1.dts
new file mode 100644
index 0000000..cfde8ed
--- /dev/null
+++ b/core/arch/arm/fdts/stm32mp157c-ev1.dts
@@ -0,0 +1,67 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2017 - All Rights Reserved
+ * Author: Ludovic Barre <ludovic.barre@st.com> for STMicroelectronics.
+ */
+/dts-v1/;
+
+#include "stm32mp157c-ed1.dts"
+
+/ {
+	model = "STMicroelectronics STM32MP157C eval daughter on eval mother";
+	compatible = "st,stm32mp157c-ev1", "st,stm32mp157c-ed1", "st,stm32mp157";
+
+	chosen {
+		stdout-path = "serial0:115200n8";
+	};
+
+	aliases {
+		serial1 = &usart3;
+	};
+};
+
+&fmc {
+	status = "okay";
+	#address-cells = <1>;
+	#size-cells = <0>;
+
+	nand: nand@0 {
+		reg = <0>;
+		nand-on-flash-bbt;
+		#address-cells = <1>;
+		#size-cells = <1>;
+	};
+};
+
+&qspi {
+	pinctrl-names = "default";
+	pinctrl-0 = <&qspi_clk_pins_a &qspi_bk1_pins_a &qspi_bk2_pins_a>;
+	reg = <0x58003000 0x1000>, <0x70000000 0x4000000>;
+	#address-cells = <1>;
+	#size-cells = <0>;
+	status = "okay";
+
+	flash0: mx66l51235l@0 {
+		compatible = "jedec,spi-nor";
+		reg = <0>;
+		spi-rx-bus-width = <4>;
+		spi-max-frequency = <108000000>;
+		#address-cells = <1>;
+		#size-cells = <1>;
+	};
+
+	flash1: mx66l51235l@1 {
+		compatible = "jedec,spi-nor";
+		reg = <1>;
+		spi-rx-bus-width = <4>;
+		spi-max-frequency = <108000000>;
+		#address-cells = <1>;
+		#size-cells = <1>;
+	};
+};
+
+&usart3 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&usart3_pins_a>;
+	status = "disabled";
+};
diff --git a/core/arch/arm/fdts/stm32mp157c-security.dtsi b/core/arch/arm/fdts/stm32mp157c-security.dtsi
new file mode 100644
index 0000000..bff1043
--- /dev/null
+++ b/core/arch/arm/fdts/stm32mp157c-security.dtsi
@@ -0,0 +1,71 @@
+/*
+ * Copyright : STMicroelectronics 2017
+ *
+ * SPDX-License-Identifier:	GPL-2.0+	BSD-3-Clause
+ */
+
+#include <dt-bindings/etzpc/stm32-etzpc.h>
+
+/ {
+	soc {
+		iwdg1: iwdg@5C003000 {
+			compatible = "st,stm32mp1-iwdg";
+			reg = <0x5C003000 0x400>;
+			clocks = <&rcc IWDG1>, <&rcc CK_LSI>;
+			clock-names = "pclk", "lsi";
+			interrupts = <GIC_SPI 150 IRQ_TYPE_LEVEL_HIGH>;
+			status = "disabled";
+			secure-status = "disabled";
+		};
+
+		etzpc: etzpc@5C007000 {
+			compatible = "st,stm32-etzpc";
+			reg = <0x5C007000 0x400>;
+			clocks = <&rcc TZPC>;
+			status = "disabled";
+			secure-status = "okay";
+		};
+
+		stgen: stgen@5C008000 {
+			compatible = "st,stm32-stgen";
+			reg = <0x5C008000 0x1000>;
+		};
+	};
+};
+
+&bsec {
+	mac_addr: mac_addr@e4 {
+		reg = <0xe4 0x6>;
+	};
+	/* Spare field to align on 32-bit OTP granularity  */
+	spare_ns_ea: spare_ns_ea@ea {
+		reg = <0xea 0x2>;
+	};
+	board_id: board_id@ec {
+		reg = <0xec 0x4>;
+	};
+};
+
+&iwdg2 {
+	secure-interrupts = <GIC_SPI 151 IRQ_TYPE_LEVEL_HIGH>;
+};
+
+&rcc {
+	secure-interrupts = <GIC_SPI 145 IRQ_TYPE_LEVEL_HIGH>;
+	interrupt-names = "wakeup";
+};
+
+&sdmmc1 {
+	compatible = "st,stm32-sdmmc2";
+};
+
+&sdmmc2 {
+	compatible = "st,stm32-sdmmc2";
+};
+
+&tamp {
+	compatible = "st,stm32-tamp";
+	clocks = <&rcc RTCAPB>;
+	interrupts = <GIC_SPI 197 IRQ_TYPE_LEVEL_HIGH>;
+	secure-status= "disabled";
+};
diff --git a/core/arch/arm/fdts/stm32mp157c.dtsi b/core/arch/arm/fdts/stm32mp157c.dtsi
new file mode 100644
index 0000000..06c2cf1
--- /dev/null
+++ b/core/arch/arm/fdts/stm32mp157c.dtsi
@@ -0,0 +1,371 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2017-2019 - All Rights Reserved
+ * Author: Ludovic Barre <ludovic.barre@st.com> for STMicroelectronics.
+ */
+#include <dt-bindings/interrupt-controller/arm-gic.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <dt-bindings/reset/stm32mp1-resets.h>
+
+/ {
+	#address-cells = <1>;
+	#size-cells = <1>;
+
+	intc: interrupt-controller@a0021000 {
+		compatible = "arm,cortex-a7-gic";
+		#interrupt-cells = <3>;
+		interrupt-controller;
+		reg = <0xa0021000 0x1000>,
+		      <0xa0022000 0x2000>;
+	};
+
+	clocks {
+		clk_hse: clk-hse {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <24000000>;
+		};
+
+		clk_hsi: clk-hsi {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <64000000>;
+		};
+
+		clk_lse: clk-lse {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <32768>;
+		};
+
+		clk_lsi: clk-lsi {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <32000>;
+		};
+
+		clk_csi: clk-csi {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <4000000>;
+		};
+
+		clk_i2s_ckin: i2s_ckin {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <0>;
+		};
+
+		clk_dsi_phy: ck_dsi_phy {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <0>;
+		};
+	};
+
+	soc {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		interrupt-parent = <&intc>;
+		ranges;
+
+		timers12: timer@40006000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "st,stm32-timers";
+			reg = <0x40006000 0x400>;
+			clocks = <&rcc TIM12_K>;
+			clock-names = "int";
+			status = "disabled";
+		};
+
+		usart2: serial@4000e000 {
+			compatible = "st,stm32h7-uart";
+			reg = <0x4000e000 0x400>;
+			clocks = <&rcc USART2_K>;
+			resets = <&rcc USART2_R>;
+			status = "disabled";
+		};
+
+		usart3: serial@4000f000 {
+			compatible = "st,stm32h7-uart";
+			reg = <0x4000f000 0x400>;
+			clocks = <&rcc USART3_K>;
+			resets = <&rcc USART3_R>;
+			status = "disabled";
+		};
+
+		uart4: serial@40010000 {
+			compatible = "st,stm32h7-uart";
+			reg = <0x40010000 0x400>;
+			clocks = <&rcc UART4_K>;
+			resets = <&rcc UART4_R>;
+			status = "disabled";
+		};
+
+		uart5: serial@40011000 {
+			compatible = "st,stm32h7-uart";
+			reg = <0x40011000 0x400>;
+			clocks = <&rcc UART5_K>;
+			resets = <&rcc UART5_R>;
+			status = "disabled";
+		};
+
+
+		uart7: serial@40018000 {
+			compatible = "st,stm32h7-uart";
+			reg = <0x40018000 0x400>;
+			clocks = <&rcc UART7_K>;
+			resets = <&rcc UART7_R>;
+			status = "disabled";
+		};
+
+		uart8: serial@40019000 {
+			compatible = "st,stm32h7-uart";
+			reg = <0x40019000 0x400>;
+			clocks = <&rcc UART8_K>;
+			resets = <&rcc UART8_R>;
+			status = "disabled";
+		};
+
+		usart6: serial@44003000 {
+			compatible = "st,stm32h7-uart";
+			reg = <0x44003000 0x400>;
+			clocks = <&rcc USART6_K>;
+			resets = <&rcc USART6_R>;
+			status = "disabled";
+		};
+
+		timers15: timer@44006000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "st,stm32-timers";
+			reg = <0x44006000 0x400>;
+			clocks = <&rcc TIM15_K>;
+			clock-names = "int";
+			status = "disabled";
+		};
+
+		sdmmc3: sdmmc@48004000 {
+			compatible = "arm,pl18x", "arm,primecell";
+			arm,primecell-periphid = <0x00253180>;
+			reg = <0x48004000 0x400>, <0x48005000 0x400>;
+			clocks = <&rcc SDMMC3_K>;
+			clock-names = "apb_pclk";
+			resets = <&rcc SDMMC3_R>;
+			cap-sd-highspeed;
+			cap-mmc-highspeed;
+			max-frequency = <120000000>;
+			status = "disabled";
+		};
+
+		usbotg_hs: usb-otg@49000000 {
+			compatible = "st,stm32mp1-hsotg", "snps,dwc2";
+			reg = <0x49000000 0x10000>;
+			clocks = <&rcc USBO_K>;
+			clock-names = "otg";
+			resets = <&rcc USBO_R>;
+			reset-names = "dwc2";
+			status = "disabled";
+		};
+
+		rcc: rcc@50000000 {
+			compatible = "st,stm32mp1-rcc", "syscon";
+			reg = <0x50000000 0x1000>;
+			#clock-cells = <1>;
+			#reset-cells = <1>;
+			interrupts = <GIC_SPI 5 IRQ_TYPE_LEVEL_HIGH>;
+		};
+
+		pwr: pwr@50001000 {
+			compatible = "st,stm32mp1-pwr", "syscon", "simple-mfd";
+			reg = <0x50001000 0x400>;
+		};
+
+		exti: interrupt-controller@5000d000 {
+			compatible = "st,stm32mp1-exti", "syscon";
+			interrupt-controller;
+			#interrupt-cells = <2>;
+			reg = <0x5000d000 0x400>;
+
+			/* exti_pwr is an extra interrupt controller used for
+			 * EXTI 55 to 60. It's mapped on pwr interrupt
+			 * controller.
+			 */
+			exti_pwr: exti-pwr {
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				interrupt-parent = <&pwr>;
+				st,irq-number = <6>;
+			};
+		};
+
+		syscfg: syscon@50020000 {
+			compatible = "st,stm32mp157-syscfg", "syscon";
+			reg = <0x50020000 0x400>;
+			clocks = <&rcc SYSCFG>;
+		};
+
+		cryp1: cryp@54001000 {
+			compatible = "st,stm32mp1-cryp";
+			reg = <0x54001000 0x400>;
+			interrupts = <GIC_SPI 79 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&rcc CRYP1>;
+			resets = <&rcc CRYP1_R>;
+			status = "disabled";
+		};
+
+		hash1: hash@54002000 {
+			compatible = "st,stm32f756-hash";
+			reg = <0x54002000 0x400>;
+			interrupts = <GIC_SPI 80 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&rcc HASH1>;
+			resets = <&rcc HASH1_R>;
+			status = "disabled";
+		};
+
+		rng1: rng@54003000 {
+			compatible = "st,stm32-rng";
+			reg = <0x54003000 0x400>;
+			clocks = <&rcc RNG1_K>;
+			resets = <&rcc RNG1_R>;
+			status = "disabled";
+		};
+
+		fmc: nand-controller@58002000 {
+			compatible = "st,stm32mp15-fmc2";
+			reg = <0x58002000 0x1000>,
+			      <0x80000000 0x1000>,
+			      <0x88010000 0x1000>,
+			      <0x88020000 0x1000>,
+			      <0x81000000 0x1000>,
+			      <0x89010000 0x1000>,
+			      <0x89020000 0x1000>;
+			clocks = <&rcc FMC_K>;
+			resets = <&rcc FMC_R>;
+			status = "disabled";
+		};
+
+		qspi: qspi@58003000 {
+			compatible = "st,stm32f469-qspi";
+			reg = <0x58003000 0x1000>, <0x70000000 0x10000000>;
+			reg-names = "qspi", "qspi_mm";
+			clocks = <&rcc QSPI_K>;
+			resets = <&rcc QSPI_R>;
+			status = "disabled";
+		};
+
+		sdmmc1: sdmmc@58005000 {
+			compatible = "arm,pl18x", "arm,primecell";
+			arm,primecell-periphid = <0x00253180>;
+			reg = <0x58005000 0x1000>, <0x58006000 0x1000>;
+			clocks = <&rcc SDMMC1_K>;
+			clock-names = "apb_pclk";
+			resets = <&rcc SDMMC1_R>;
+			cap-sd-highspeed;
+			cap-mmc-highspeed;
+			max-frequency = <120000000>;
+			status = "disabled";
+		};
+
+		sdmmc2: sdmmc@58007000 {
+			compatible = "arm,pl18x", "arm,primecell";
+			arm,primecell-periphid = <0x00253180>;
+			reg = <0x58007000 0x1000>, <0x58008000 0x1000>;
+			clocks = <&rcc SDMMC2_K>;
+			clock-names = "apb_pclk";
+			resets = <&rcc SDMMC2_R>;
+			cap-sd-highspeed;
+			cap-mmc-highspeed;
+			max-frequency = <120000000>;
+			status = "disabled";
+		};
+
+		iwdg2: watchdog@5a002000 {
+			compatible = "st,stm32mp1-iwdg";
+			reg = <0x5a002000 0x400>;
+			clocks = <&rcc IWDG2>, <&rcc CK_LSI>;
+			clock-names = "pclk", "lsi";
+			status = "disabled";
+		};
+
+		usart1: serial@5c000000 {
+			compatible = "st,stm32h7-uart";
+			reg = <0x5c000000 0x400>;
+			interrupt-names = "event", "wakeup";
+			interrupts-extended = <&intc GIC_SPI 37 IRQ_TYPE_LEVEL_HIGH>,
+					      <&exti 26 1>;
+			clocks = <&rcc USART1_K>;
+			resets = <&rcc USART1_R>;
+			status = "disabled";
+		};
+
+		spi6: spi@5c001000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "st,stm32h7-spi";
+			reg = <0x5c001000 0x400>;
+			interrupts = <GIC_SPI 86 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&rcc SPI6_K>;
+			resets = <&rcc SPI6_R>;
+			status = "disabled";
+		};
+
+		i2c4: i2c@5c002000 {
+			compatible = "st,stm32f7-i2c";
+			reg = <0x5c002000 0x400>;
+			interrupt-names = "event", "error", "wakeup";
+			interrupts-extended = <&intc GIC_SPI 95 IRQ_TYPE_LEVEL_HIGH>,
+					      <&intc GIC_SPI 96 IRQ_TYPE_LEVEL_HIGH>,
+					      <&exti 24 1>;
+			clocks = <&rcc I2C4_K>;
+			resets = <&rcc I2C4_R>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+			status = "disabled";
+		};
+
+		rtc: rtc@5c004000 {
+			compatible = "st,stm32mp1-rtc";
+			reg = <0x5c004000 0x400>;
+			clocks = <&rcc RTCAPB>, <&rcc RTC>;
+			clock-names = "pclk", "rtc_ck";
+			interrupts-extended = <&intc GIC_SPI 3 IRQ_TYPE_LEVEL_HIGH>,
+					      <&exti 19 1>;
+			status = "disabled";
+		};
+
+		bsec: nvmem@5c005000 {
+			compatible = "st,stm32mp15-bsec";
+			reg = <0x5c005000 0x400>;
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ts_cal1: calib@5c {
+				reg = <0x5c 0x2>;
+			};
+			ts_cal2: calib@5e {
+				reg = <0x5e 0x2>;
+			};
+		};
+
+		i2c6: i2c@5c009000 {
+			compatible = "st,stm32f7-i2c";
+			reg = <0x5c009000 0x400>;
+			interrupt-names = "event", "error", "wakeup";
+			interrupts-extended = <&intc GIC_SPI 135 IRQ_TYPE_LEVEL_HIGH>,
+					      <&intc GIC_SPI 136 IRQ_TYPE_LEVEL_HIGH>,
+					      <&exti 54 1>;
+			clocks = <&rcc I2C6_K>;
+			resets = <&rcc I2C6_R>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+			status = "disabled";
+		};
+
+		tamp: tamp@5c00a000 {
+			compatible = "simple-bus", "syscon", "simple-mfd";
+			reg = <0x5c00a000 0x400>;
+		};
+	};
+};
diff --git a/core/arch/arm/fdts/stm32mp157caa-pinctrl.dtsi b/core/arch/arm/fdts/stm32mp157caa-pinctrl.dtsi
new file mode 100644
index 0000000..9b9cd08
--- /dev/null
+++ b/core/arch/arm/fdts/stm32mp157caa-pinctrl.dtsi
@@ -0,0 +1,90 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2018 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com>
+ */
+
+#include "stm32mp157-pinctrl.dtsi"
+/ {
+	soc {
+		pinctrl: pin-controller@50002000 {
+			st,package = <STM32MP157CAA>;
+
+			gpioa: gpio@50002000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 0 16>;
+			};
+
+			gpiob: gpio@50003000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 16 16>;
+			};
+
+			gpioc: gpio@50004000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 32 16>;
+			};
+
+			gpiod: gpio@50005000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 48 16>;
+			};
+
+			gpioe: gpio@50006000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 64 16>;
+			};
+
+			gpiof: gpio@50007000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 80 16>;
+			};
+
+			gpiog: gpio@50008000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 96 16>;
+			};
+
+			gpioh: gpio@50009000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 112 16>;
+			};
+
+			gpioi: gpio@5000a000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 128 16>;
+			};
+
+			gpioj: gpio@5000b000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 144 16>;
+			};
+
+			gpiok: gpio@5000c000 {
+				status = "okay";
+				ngpios = <8>;
+				gpio-ranges = <&pinctrl 0 160 8>;
+			};
+		};
+
+		pinctrl_z: pin-controller-z@54004000 {
+			st,package = <STM32MP157CAA>;
+
+			gpioz: gpio@54004000 {
+				status = "okay";
+				ngpios = <8>;
+				gpio-ranges = <&pinctrl_z 0 400 8>;
+			};
+		};
+	};
+};
diff --git a/core/arch/arm/fdts/stm32mp157cab-pinctrl.dtsi b/core/arch/arm/fdts/stm32mp157cab-pinctrl.dtsi
new file mode 100644
index 0000000..c570cf9
--- /dev/null
+++ b/core/arch/arm/fdts/stm32mp157cab-pinctrl.dtsi
@@ -0,0 +1,62 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2018 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com>
+ */
+
+#include "stm32mp157-pinctrl.dtsi"
+/ {
+	soc {
+		pinctrl: pin-controller@50002000 {
+			st,package = <STM32MP157CAB>;
+
+			gpioa: gpio@50002000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 0 16>;
+			};
+
+			gpiob: gpio@50003000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 16 16>;
+			};
+
+			gpioc: gpio@50004000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 32 16>;
+			};
+
+			gpiod: gpio@50005000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 48 16>;
+			};
+
+			gpioe: gpio@50006000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 64 16>;
+			};
+
+			gpiof: gpio@50007000 {
+				status = "okay";
+				ngpios = <6>;
+				gpio-ranges = <&pinctrl 6 86 6>;
+			};
+
+			gpiog: gpio@50008000 {
+				status = "okay";
+				ngpios = <10>;
+				gpio-ranges = <&pinctrl 6 102 10>;
+			};
+
+			gpioh: gpio@50009000 {
+				status = "okay";
+				ngpios = <2>;
+				gpio-ranges = <&pinctrl 0 112 2>;
+			};
+		};
+	};
+};
diff --git a/core/arch/arm/fdts/stm32mp157cac-pinctrl.dtsi b/core/arch/arm/fdts/stm32mp157cac-pinctrl.dtsi
new file mode 100644
index 0000000..777f991
--- /dev/null
+++ b/core/arch/arm/fdts/stm32mp157cac-pinctrl.dtsi
@@ -0,0 +1,78 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2018 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com>
+ */
+
+#include "stm32mp157-pinctrl.dtsi"
+/ {
+	soc {
+		pinctrl: pin-controller@50002000 {
+			st,package = <STM32MP157CAC>;
+
+			gpioa: gpio@50002000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 0 16>;
+			};
+
+			gpiob: gpio@50003000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 16 16>;
+			};
+
+			gpioc: gpio@50004000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 32 16>;
+			};
+
+			gpiod: gpio@50005000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 48 16>;
+			};
+
+			gpioe: gpio@50006000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 64 16>;
+			};
+
+			gpiof: gpio@50007000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 80 16>;
+			};
+
+			gpiog: gpio@50008000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 96 16>;
+			};
+
+			gpioh: gpio@50009000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 112 16>;
+			};
+
+			gpioi: gpio@5000a000 {
+				status = "okay";
+				ngpios = <12>;
+				gpio-ranges = <&pinctrl 0 128 12>;
+			};
+		};
+
+		pinctrl_z: pin-controller-z@54004000 {
+			st,package = <STM32MP157CAC>;
+
+			gpioz: gpio@54004000 {
+				status = "okay";
+				ngpios = <8>;
+				gpio-ranges = <&pinctrl_z 0 400 8>;
+			};
+		};
+	};
+};
diff --git a/core/arch/arm/fdts/stm32mp157cad-pinctrl.dtsi b/core/arch/arm/fdts/stm32mp157cad-pinctrl.dtsi
new file mode 100644
index 0000000..c4c303a
--- /dev/null
+++ b/core/arch/arm/fdts/stm32mp157cad-pinctrl.dtsi
@@ -0,0 +1,62 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2018 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com>
+ */
+
+#include "stm32mp157-pinctrl.dtsi"
+/ {
+	soc {
+		pinctrl: pin-controller@50002000 {
+			st,package = <STM32MP157CAD>;
+
+			gpioa: gpio@50002000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 0 16>;
+			};
+
+			gpiob: gpio@50003000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 16 16>;
+			};
+
+			gpioc: gpio@50004000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 32 16>;
+			};
+
+			gpiod: gpio@50005000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 48 16>;
+			};
+
+			gpioe: gpio@50006000 {
+				status = "okay";
+				ngpios = <16>;
+				gpio-ranges = <&pinctrl 0 64 16>;
+			};
+
+			gpiof: gpio@50007000 {
+				status = "okay";
+				ngpios = <6>;
+				gpio-ranges = <&pinctrl 6 86 6>;
+			};
+
+			gpiog: gpio@50008000 {
+				status = "okay";
+				ngpios = <10>;
+				gpio-ranges = <&pinctrl 6 102 10>;
+			};
+
+			gpioh: gpio@50009000 {
+				status = "okay";
+				ngpios = <2>;
+				gpio-ranges = <&pinctrl 0 112 2>;
+			};
+		};
+	};
+};
diff --git a/core/arch/arm/include/arm32.h b/core/arch/arm/include/arm32.h
index e7c3b2d..0dbd889 100644
--- a/core/arch/arm/include/arm32.h
+++ b/core/arch/arm/include/arm32.h
@@ -1,12 +1,13 @@
 /* SPDX-License-Identifier: BSD-2-Clause */
 /*
- * Copyright (c) 2016, Linaro Limited
+ * Copyright (c) 2016-2018, Linaro Limited
  * Copyright (c) 2014, STMicroelectronics International N.V.
  */
 
 #ifndef ARM32_H
 #define ARM32_H
 
+#include <arm.h>
 #include <sys/cdefs.h>
 #include <stdint.h>
 #include <util.h>
@@ -159,6 +160,17 @@
 #define IDPFR1_GENTIMER_SHIFT        16
 #define IDPFR1_GENTIMER_MASK         (0xF << IDPFR1_GENTIMER_SHIFT)
 
+/* Generic timer registers and fields */
+#define CNTCR_OFFSET		0x000
+#define CNTSR_OFFSET		0x004
+#define CNTCVL_OFFSET		0x008
+#define CNTCVU_OFFSET		0x00C
+#define CNTFID_OFFSET		0x020
+
+#define CNTCR_EN		BIT(0)
+#define CNTCR_HDBG		BIT(1)
+#define CNTCR_FCREQ(x)		((x) << 8)
+
 #ifndef ASM
 #include <generated/arm32_sysreg.h>
 #ifdef CFG_ARM_GICV3
diff --git a/core/arch/arm/include/kernel/delay.h b/core/arch/arm/include/kernel/delay.h
index 72ac873..2c47f0a 100644
--- a/core/arch/arm/include/kernel/delay.h
+++ b/core/arch/arm/include/kernel/delay.h
@@ -1,5 +1,6 @@
 /* SPDX-License-Identifier: BSD-2-Clause */
 /*
+ * Copyright (c) 2018, Linaro Limited
  * Copyright (C) 2017, Fuzhou Rockchip Electronics Co., Ltd.
  * All rights reserved.
  *
@@ -29,7 +30,14 @@
 #ifndef __KERNEL_DELAY_H
 #define __KERNEL_DELAY_H
 
+#include <stdbool.h>
+#include <stdint.h>
+
 void udelay(uint32_t us);
 void mdelay(uint32_t ms);
 
+uint64_t utimeout_init(uint32_t us);
+bool utimeout_elapsed(uint32_t us, uint64_t ref);
+unsigned int utimeout_elapsed_us(uint32_t us, uint64_t reference);
+
 #endif
diff --git a/core/arch/arm/include/mm/core_mmu.h b/core/arch/arm/include/mm/core_mmu.h
index 64994d3..cec3a42 100644
--- a/core/arch/arm/include/mm/core_mmu.h
+++ b/core/arch/arm/include/mm/core_mmu.h
@@ -101,6 +101,7 @@
  * MEM_AREA_NSEC_SHM: NonSecure shared RAM between NSec and TEE.
  * MEM_AREA_RAM_NSEC: NonSecure RAM storing data
  * MEM_AREA_RAM_SEC:  Secure RAM storing some secrets
+ * MEM_AREA_ROM_SEC:  Secure read only memory storing some secrets
  * MEM_AREA_IO_NSEC:  NonSecure HW mapped registers
  * MEM_AREA_IO_SEC:   Secure HW mapped registers
  * MEM_AREA_RES_VASPACE: Reserved virtual memory space
@@ -121,6 +122,7 @@ enum teecore_memtypes {
 	MEM_AREA_NSEC_SHM,
 	MEM_AREA_RAM_NSEC,
 	MEM_AREA_RAM_SEC,
+	MEM_AREA_ROM_SEC,
 	MEM_AREA_IO_NSEC,
 	MEM_AREA_IO_SEC,
 	MEM_AREA_RES_VASPACE,
@@ -146,6 +148,7 @@ static inline const char *teecore_memtype_name(enum teecore_memtypes type)
 		[MEM_AREA_NSEC_SHM] = "NSEC_SHM",
 		[MEM_AREA_RAM_NSEC] = "RAM_NSEC",
 		[MEM_AREA_RAM_SEC] = "RAM_SEC",
+		[MEM_AREA_ROM_SEC] = "ROM_SEC",
 		[MEM_AREA_IO_NSEC] = "IO_NSEC",
 		[MEM_AREA_IO_SEC] = "IO_SEC",
 		[MEM_AREA_RES_VASPACE] = "RES_VASPACE",
diff --git a/core/arch/arm/include/sm/pm.h b/core/arch/arm/include/sm/pm.h
index 939f966..90f031a 100644
--- a/core/arch/arm/include/sm/pm.h
+++ b/core/arch/arm/include/sm/pm.h
@@ -34,7 +34,11 @@
 struct sm_pm_ctx {
 	uint32_t sp;
 	paddr_t cpu_resume_addr;
+#ifdef CFG_WITH_LPAE
+	uint32_t suspend_regs[18];
+#else
 	uint32_t suspend_regs[16];
+#endif
 };
 
 /* suspend/resume core functions */
diff --git a/core/arch/arm/kernel/delay.c b/core/arch/arm/kernel/delay.c
index 2321b78..ae62187 100644
--- a/core/arch/arm/kernel/delay.c
+++ b/core/arch/arm/kernel/delay.c
@@ -1,5 +1,6 @@
 // SPDX-License-Identifier: BSD-2-Clause
 /*
+ * Copyright (c) 2018, Linaro Limited
  * Copyright (C) 2017, Fuzhou Rockchip Electronics Co., Ltd.
  * All rights reserved.
  *
@@ -29,12 +30,15 @@
 #include <arm.h>
 #include <kernel/delay.h>
 
+#define US2CNT(m)	(((uint64_t)m * (uint64_t)read_cntfrq()) / 1000000ULL)
+#define CNT2US(m)	((uint32_t)(((uint64_t)m * 1000000ULL) / read_cntfrq()))
+
 void udelay(uint32_t us)
 {
 	uint64_t start, target;
 
 	start = read_cntpct();
-	target = ((uint64_t)read_cntfrq() * us) / 1000000ULL;
+	target = US2CNT(us);
 
 	while (read_cntpct() - start <= target)
 		;
@@ -44,3 +48,37 @@ void mdelay(uint32_t ms)
 {
 	udelay(1000 * ms);
 }
+
+uint64_t utimeout_init(uint32_t us)
+{
+	return read_cntpct() + US2CNT(us);
+}
+
+bool utimeout_elapsed(uint32_t us, uint64_t reference)
+{
+	uint64_t origin = reference - US2CNT(us);
+	uint64_t now = read_cntpct();
+
+	if (origin < reference)
+		return now < origin || now > reference;
+
+	return now < origin && now > reference;
+}
+
+unsigned int utimeout_elapsed_us(uint32_t us, uint64_t reference)
+{
+	uint64_t origin = reference - US2CNT(us);
+	uint64_t now = read_cntpct();
+
+	if (origin < reference) {
+		if (now < origin || now > reference)
+			return CNT2US(now - reference);
+
+		return 0;
+	}
+
+	if (now < origin && now > reference)
+		return CNT2US(now - reference);
+
+	return 0;
+}
diff --git a/core/arch/arm/kernel/generic_boot.c b/core/arch/arm/kernel/generic_boot.c
index e5f0f3d..612eab4 100644
--- a/core/arch/arm/kernel/generic_boot.c
+++ b/core/arch/arm/kernel/generic_boot.c
@@ -73,6 +73,9 @@ uint32_t sem_cpu_sync[CFG_TEE_CORE_NB_CORE];
 KEEP_PAGER(sem_cpu_sync);
 #endif
 
+#ifdef CFG_STATIC_SECURE_DT
+extern uint8_t static_secure_dtb[];
+#endif
 #ifdef CFG_DT
 static void *dt_blob_addr;
 #endif
@@ -471,7 +474,7 @@ static void init_runtime(unsigned long pageable_part __unused)
 }
 #endif
 
-#ifdef CFG_DT
+#if defined(CFG_DT) && !defined(CFG_STATIC_SECURE_DT)
 void *get_dt_blob(void)
 {
 	assert(cpu_mmu_enabled());
@@ -836,23 +839,42 @@ static void update_fdt(void)
 		panic();
 	}
 }
+#endif /*CFG_DT && !CFG_STATIC_SECURE_DT*/
 
-#else
-static void init_fdt(unsigned long phys_fdt __unused)
+#if defined(CFG_DT) && defined(CFG_STATIC_SECURE_DT)
+void *get_dt_blob(void)
 {
+	assert(cpu_mmu_enabled());
+
+	if (!dt_blob_addr) {
+		if (fdt_check_header((void *)&static_secure_dtb[0]))
+			panic("Invalid static DTB");
+
+		dt_blob_addr = (void *)&static_secure_dtb[0];
+	}
+
+	return dt_blob_addr;
 }
+#endif
 
-static void update_fdt(void)
+#ifndef CFG_DT
+void *get_dt_blob(void)
 {
+	return NULL;
 }
+#endif
 
+#if !defined(CFG_DT) || defined(CFG_STATIC_SECURE_DT)
 static void reset_dt_references(void)
 {
 }
 
-void *get_dt_blob(void)
+static void init_fdt(unsigned long phys_fdt __unused)
+{
+}
+
+static void update_fdt(void)
 {
-	return NULL;
 }
 
 static struct core_mmu_phys_mem *get_memory(void *fdt __unused,
@@ -861,7 +883,7 @@ static struct core_mmu_phys_mem *get_memory(void *fdt __unused,
 	return NULL;
 }
 
-#endif /*!CFG_DT*/
+#endif /*!CFG_DT || CFG_STATIC_SECURE_DT*/
 
 static void discover_nsec_memory(void)
 {
diff --git a/core/arch/arm/mm/core_mmu.c b/core/arch/arm/mm/core_mmu.c
index f69069e..3337ca6 100644
--- a/core/arch/arm/mm/core_mmu.c
+++ b/core/arch/arm/mm/core_mmu.c
@@ -627,6 +627,8 @@ uint32_t core_mmu_type_to_attr(enum teecore_memtypes t)
 		return attr | TEE_MATTR_PRW | cached;
 	case MEM_AREA_RAM_SEC:
 		return attr | TEE_MATTR_SECURE | TEE_MATTR_PRW | cached;
+	case MEM_AREA_ROM_SEC:
+		return attr | TEE_MATTR_SECURE | TEE_MATTR_PR | cached;
 	case MEM_AREA_RES_VASPACE:
 	case MEM_AREA_SHM_VASPACE:
 		return 0;
@@ -1014,6 +1016,7 @@ void core_init_mmu_map(void)
 		case MEM_AREA_IO_SEC:
 		case MEM_AREA_IO_NSEC:
 		case MEM_AREA_RAM_SEC:
+		case MEM_AREA_ROM_SEC:
 		case MEM_AREA_RAM_NSEC:
 		case MEM_AREA_RES_VASPACE:
 		case MEM_AREA_SHM_VASPACE:
diff --git a/core/arch/arm/mm/mobj.c b/core/arch/arm/mm/mobj.c
index b2bdf6b..789a88d 100644
--- a/core/arch/arm/mm/mobj.c
+++ b/core/arch/arm/mm/mobj.c
@@ -52,6 +52,7 @@ static void *mobj_phys_get_va(struct mobj *mobj, size_t offset)
 
 	return (void *)(moph->va + offset);
 }
+KEEP_PAGER(mobj_phys_get_va);
 
 static TEE_Result mobj_phys_get_pa(struct mobj *mobj, size_t offs,
 				   size_t granule, paddr_t *pa)
@@ -230,7 +231,7 @@ static void *mobj_mm_get_va(struct mobj *mobj, size_t offs)
 	return mobj_get_va(to_mobj_mm(mobj)->parent_mobj,
 			   mobj_mm_offs(mobj, offs));
 }
-
+KEEP_PAGER(mobj_mm_get_va);
 
 static TEE_Result mobj_mm_get_pa(struct mobj *mobj, size_t offs,
 				    size_t granule, paddr_t *pa)
diff --git a/core/arch/arm/plat-stm32mp1/boot_api.h b/core/arch/arm/plat-stm32mp1/boot_api.h
index 62e38b5..a7daffd 100644
--- a/core/arch/arm/plat-stm32mp1/boot_api.h
+++ b/core/arch/arm/plat-stm32mp1/boot_api.h
@@ -14,7 +14,9 @@
 #define BCKR_CORE1_MAGIC_NUMBER			4
 
 /* Value for BCKR_CORE1_MAGIC_NUMBER entry */
+#define BOOT_API_A7_CORE0_MAGIC_NUMBER		0xca7face0
 #define BOOT_API_A7_CORE1_MAGIC_NUMBER		0xca7face1
+#define BOOT_API_A7_RESET_MAGIC_NUMBER		0xca7dead0
 
 /* Backup register #5: physical address of core1 entry at boot up */
 #define BCKR_CORE1_BRANCH_ADDRESS		5
diff --git a/core/arch/arm/plat-stm32mp1/conf.mk b/core/arch/arm/plat-stm32mp1/conf.mk
index fb0ca5a..0940ce3 100644
--- a/core/arch/arm/plat-stm32mp1/conf.mk
+++ b/core/arch/arm/plat-stm32mp1/conf.mk
@@ -1,9 +1,16 @@
 PLATFORM_FLAVOR ?= stm32mp157c
+STM32_BOARD ?= ev1
 
+# 1GB and 512MB DDR target do not locate secure DDR at the same place.
+#
+flavorlist-1G = stm32mp157c-ev1 stm32mp157c-ed1
+flavorlist-512M = stm32mp157a-dk1 stm32mp157c-dk2
+
+# Generic stm32mp1 configuration directives
+#
 include core/arch/arm/cpu/cortex-a7.mk
 ta-targets = ta_arm32
 
-$(call force,CFG_TEE_CORE_NB_CORE,2)
 $(call force,CFG_ARM32_core,y)
 $(call force,CFG_BOOT_SECONDARY_REQUEST,y)
 $(call force,CFG_GENERIC_BOOT,y)
@@ -11,23 +18,85 @@ $(call force,CFG_GIC,y)
 $(call force,CFG_INIT_CNTVOFF,y)
 $(call force,CFG_PM_STUBS,y)
 $(call force,CFG_PSCI_ARM32,y)
+$(call force,CFG_PM_ARM32,y)
 $(call force,CFG_SECONDARY_INIT_CNTFRQ,y)
 $(call force,CFG_SECURE_TIME_SOURCE_CNTPCT,y)
 $(call force,CFG_WITH_SOFTWARE_PRNG,y)
+$(call force,CFG_SM_PLATFORM_HANDLER,y)
+
+ifneq ($(CFG_SECURE_DT),)
+$(call force,CFG_DT,y)
+$(call force,CFG_STATIC_SECURE_DT,y)
+endif
+
+CFG_TEE_CORE_NB_CORE ?= 2
+
+ifneq (,$(filter $(CFG_SECURE_DT),$(flavorlist-512M)))
+CFG_TZDRAM_START ?= 0xde000000
+CFG_SHMEM_START  ?= 0xdfe00000
+endif
 
 CFG_TZSRAM_START ?= 0x2ffc0000
 CFG_TZSRAM_SIZE  ?= 0x00040000
 CFG_TZDRAM_START ?= 0xfe000000
 CFG_TZDRAM_SIZE  ?= 0x01e00000
-CFG_SHMEM_SIZE   ?= 0x00200000
 CFG_SHMEM_START  ?= 0xffe00000
+CFG_SHMEM_SIZE   ?= 0x00200000
 
-CFG_WITH_PAGER		?= y
-CFG_WITH_LPAE		?= y
-CFG_WITH_STACK_CANARIES	?= y
+CFG_CORE_HEAP_SIZE ?= 49152
+CFG_WITH_PAGER ?= y
+CFG_WITH_LPAE ?= y
+CFG_WITH_STACK_CANARIES ?= y
+CFG_MMAP_REGIONS ?= 23
 
+ifneq ($(CFG_DT),y)
+# Some drivers mandate DT support
+$(call force,CFG_STPMIC1,n)
+$(call force,CFG_STM32_I2C,n)
+$(call force,CFG_STM32_IWDG,n)
+$(call force,CFG_STM32_RNG,n)
+$(call force,CFG_STM32_TIMER,n)
+$(call force,CFG_STM32_CLOCKSRC_CALIB,n)
+endif
+
+$(call force,CFG_STM32_BSEC,y)
+$(call force,CFG_STM32_CRYP,y)
+$(call force,CFG_STM32_ETZPC,y)
+CFG_STM32_GPIO ?= y
+CFG_STM32_I2C ?= y
+CFG_STM32_IWDG ?= y
 CFG_STM32_UART ?= y
+CFG_STM32_RNG ?= y
+$(call force,CFG_STM32_RTC,y)
+CFG_STM32_TIMER ?= y
+
+CFG_STPMIC1 ?= y
+
+$(call force,CFG_STM32_BSEC_SIP,y)
+$(call force,CFG_STM32_RCC_SIP,y)
+CFG_STM32_PWR_SIP ?= y
+CFG_STM32_POWER_SERVICES ?= y
+CFG_STM32_CLOCKSRC_CALIB ?= y
+
+ifeq ($(CFG_STPMIC1),y)
+$(call force,CFG_STM32_I2C,y)
+$(call force,CFG_STM32_GPIO,y)
+endif
+
+ifeq ($(CFG_STM32_CLOCKSRC_CALIB),y)
+$(call force,CFG_STM32_TIMER,y)
+endif
+
+# Get a static mapping for the non secure low DDR (save 4kB of unpaged memory)
+CFG_STM32MP_MAP_NSEC_LOW_DDR ?= y
 
-# Default enable the test facitilites
+# Default enable some test facitilites
 CFG_TEE_CORE_EMBED_INTERNAL_TESTS ?= y
 CFG_WITH_STATS ?= y
+CFG_UNWIND ?= n
+# Non secure UART and GPIO/pinctrl for the output console
+CFG_WITH_NSEC_GPIOS ?= y
+CFG_WITH_NSEC_UARTS ?= y
+CFG_FORCE_CONSOLE_ON_SUSPEND ?= n
+# UART instance used for early console (0 disables early console)
+CFG_STM32_EARLY_CONSOLE_UART ?= 4
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32_reset.c b/core/arch/arm/plat-stm32mp1/drivers/stm32_reset.c
new file mode 100644
index 0000000..b406167
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32_reset.c
@@ -0,0 +1,60 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <drivers/stm32mp1_rcc.h>
+#include <drivers/stm32_reset.h>
+#include <io.h>
+#include <kernel/delay.h>
+#include <kernel/panic.h>
+#include <util.h>
+
+#define RST_CLR_OFFSET		4U
+#define RESET_TIMEOUT_US	1000
+
+static size_t id2reg_offset(unsigned int reset_id)
+{
+	return ((reset_id & GENMASK_32(31, 5)) >> 5) * sizeof(uint32_t);
+}
+
+static uint8_t id2reg_bit_pos(unsigned int reset_id)
+{
+	return (uint8_t)(reset_id & GENMASK_32(4,0));
+}
+
+void stm32_reset_assert(unsigned int reset_id)
+{
+	size_t offset = id2reg_offset(reset_id);
+	uint32_t bitmsk = BIT(id2reg_bit_pos(reset_id));
+	uint64_t timeout_ref;
+	uintptr_t rcc_base = stm32_rcc_base();
+
+	write32(bitmsk, rcc_base + offset);
+
+	timeout_ref = utimeout_init(RESET_TIMEOUT_US);
+
+	while (!(read32(rcc_base + offset) & bitmsk)) {
+		if (utimeout_elapsed(RESET_TIMEOUT_US, timeout_ref)) {
+			panic("Reset timeout");
+		}
+	}
+}
+
+void stm32_reset_deassert(unsigned int reset_id)
+{
+	size_t offset = id2reg_offset(reset_id) + RST_CLR_OFFSET;
+	uint32_t bitmsk = BIT(id2reg_bit_pos(reset_id));
+	uint64_t timeout_ref;
+	uintptr_t rcc_base = stm32_rcc_base();
+
+	write32(bitmsk, rcc_base + offset);
+
+	timeout_ref = utimeout_init(RESET_TIMEOUT_US);
+
+	while (read32(rcc_base + offset) & bitmsk) {
+		if (utimeout_elapsed(RESET_TIMEOUT_US, timeout_ref)) {
+			panic("Reset timeout");
+		}
+	}
+}
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32_reset.h b/core/arch/arm/plat-stm32mp1/drivers/stm32_reset.h
new file mode 100644
index 0000000..577ba9b
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32_reset.h
@@ -0,0 +1,15 @@
+/*
+ * Copyright (c) 2018, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef __STM32_RESET_H__
+#define __STM32_RESET_H__
+
+#include <stdint.h>
+
+void stm32_reset_assert(uint32_t reset_id);
+void stm32_reset_deassert(uint32_t reset_id);
+
+#endif /* __STM32MP1_RESET_H__ */
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_calib.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_calib.c
new file mode 100644
index 0000000..583bf94
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_calib.c
@@ -0,0 +1,457 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2018, STMicroelectronics
+ */
+
+#include <arm.h>
+#include <drivers/stm32_timer.h>
+#include <drivers/stm32mp1_clk.h>
+#include <drivers/stm32mp1_clkfunc.h>
+#include <drivers/stm32mp1_rcc.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <initcall.h>
+#include <keep.h>
+#include <kernel/generic_boot.h>
+#include <kernel/interrupt.h>
+#include <kernel/panic.h>
+#include <mm/core_memprot.h>
+#include <stm32_util.h>
+#include <stm32mp_dt.h>
+#include <stm32mp_pm.h>
+#include <string.h>
+
+#define CAL_MAX_RETRY		20U
+
+/* List of forbiden values for HSI and CSI */
+static uint16_t fbv_hsi[] = {
+	512, 480, 448, 416, 384, 352, 320, 288,
+	256, 224, 192, 160, 128, 96, 64, 32, 0,
+};
+static uint16_t fbv_csi[] = {
+	256, 240, 224, 208, 192, 176, 160, 144,
+	128, 112, 96, 80, 64, 48, 32, 16, 0,
+};
+
+struct stm32mp1_trim_boundary_t {
+	unsigned int x1;	/* Max boundary trim value around forbidden value */
+	unsigned int x2;	/* Min boundary trim value around forbidden value */
+};
+
+struct stm32mp1_clk_cal {
+	uint16_t *fbv;
+	unsigned int cal_ref;
+	int trim_max;
+	int trim_min;
+	unsigned int boundary_max;
+	unsigned long ref_freq;
+	unsigned int freq_margin;
+	unsigned long (*get_freq)(void);
+	void (*set_trim)(unsigned int cal);
+	unsigned int (*get_trim)(void);
+	struct stm32mp1_trim_boundary_t boundary[16];
+};
+
+static void hsi_set_trim(unsigned int cal);
+static unsigned int hsi_get_trimed_cal(void);
+static void csi_set_trim(unsigned int cal);
+static unsigned int csi_get_trimed_cal(void);
+
+static struct stm32mp1_clk_cal *hsi_calib;
+static struct stm32mp1_clk_cal *csi_calib;
+
+static const struct stm32mp1_clk_cal hsi_calib_config = {
+	.fbv = fbv_hsi,
+	.trim_max = 63,
+	.trim_min = -64,
+	.ref_freq = 0,
+	.freq_margin = 1,
+	.set_trim = hsi_set_trim,
+	.get_trim = hsi_get_trimed_cal,
+};
+
+static const struct stm32mp1_clk_cal csi_calib_config = {
+	.fbv = fbv_csi,
+	.trim_max = 15,
+	.trim_min = -16,
+	.ref_freq = 0,
+	.freq_margin = 2,
+	.set_trim = csi_set_trim,
+	.get_trim = csi_get_trimed_cal,
+};
+
+static void hsi_set_trim(unsigned int cal)
+{
+	int clk_trim = (int)cal - (int)hsi_calib->cal_ref;
+	uint32_t trim = ((uint32_t)clk_trim << RCC_HSICFGR_HSITRIM_SHIFT) &
+			RCC_HSICFGR_HSITRIM_MASK;
+
+	mmio_clrsetbits_32(stm32_rcc_base() + RCC_HSICFGR,
+			   RCC_HSICFGR_HSITRIM_MASK, trim);
+}
+KEEP_PAGER(hsi_set_trim);
+
+static unsigned int hsi_get_trimed_cal(void)
+{
+	uint32_t utrim = (mmio_read_32(stm32_rcc_base() + RCC_HSICFGR) &
+			  RCC_HSICFGR_HSITRIM_MASK) >>
+			 RCC_HSICFGR_HSITRIM_SHIFT;
+	int trim = (int)utrim - hsi_calib->trim_max;
+
+	if (trim + (int)hsi_calib->cal_ref < 0)
+		return 0;
+
+	return hsi_calib->cal_ref + trim;
+}
+KEEP_PAGER(hsi_get_trimed_cal);
+
+static void csi_set_trim(unsigned int cal)
+{
+	int clk_trim = (int)cal - (int)csi_calib->cal_ref +
+		       csi_calib->trim_max + 1;
+	uint32_t trim = ((uint32_t)clk_trim << RCC_CSICFGR_CSITRIM_SHIFT) &
+			RCC_CSICFGR_CSITRIM_MASK;
+
+	mmio_clrsetbits_32(stm32_rcc_base() + RCC_CSICFGR,
+			   RCC_CSICFGR_CSITRIM_MASK, trim);
+}
+KEEP_PAGER(csi_set_trim);
+
+static unsigned int csi_get_trimed_cal(void)
+{
+	uint32_t trim = (mmio_read_32(stm32_rcc_base() + RCC_CSICFGR) &
+			 RCC_CSICFGR_CSITRIM_MASK) >>
+			RCC_CSICFGR_CSITRIM_SHIFT;
+
+	return (int)trim - csi_calib->trim_max + (int)csi_calib->cal_ref - 1;
+}
+KEEP_PAGER(csi_get_trimed_cal);
+
+static unsigned int trim_increase(struct stm32mp1_clk_cal *clk_cal,
+				  unsigned int cal)
+{
+	struct stm32mp1_trim_boundary_t *boundary;
+	unsigned int new_cal;
+	int i;
+
+	/* By default: last calibration value */
+	new_cal = cal;
+
+	/* Start from Lowest cal value */
+	for (i = (int)clk_cal->boundary_max - 1; i >= 0; i--) {
+		boundary = &clk_cal->boundary[i];
+
+		if (cal < boundary->x2) {
+			new_cal = boundary->x2;
+			break;
+		}
+
+		if ((cal >= boundary->x2) && (cal < boundary->x1)) {
+			new_cal = cal + 1;
+			break;
+		}
+	}
+
+	return new_cal;
+}
+
+static unsigned int trim_decrease(struct stm32mp1_clk_cal *clk_cal,
+				  unsigned int cal)
+{
+	struct stm32mp1_trim_boundary_t *boundary;
+	unsigned int new_cal;
+	unsigned int i;
+
+	/* By default: last calibration value */
+	new_cal = cal;
+
+	/* Start from Highest cal value */
+	for (i = 0; i < clk_cal->boundary_max; i++) {
+		boundary = &clk_cal->boundary[i];
+
+		if (cal > boundary->x1) {
+			new_cal = boundary->x1;
+			break;
+		}
+
+		if ((cal > boundary->x2) && (cal <= boundary->x1)) {
+			new_cal = cal - 1;
+			break;
+		}
+	}
+
+	return new_cal;
+}
+
+static void rcc_calibration(struct stm32mp1_clk_cal *clk_cal)
+{
+	unsigned long margin = (clk_cal->ref_freq * clk_cal->freq_margin) / 100;
+	unsigned long min = clk_cal->ref_freq - margin;
+	unsigned long max = clk_cal->ref_freq + margin;
+	unsigned long freq = clk_cal->get_freq();
+	int cal = clk_cal->get_trim();
+	unsigned int nb_retries;
+
+	for (nb_retries = 0; nb_retries < CAL_MAX_RETRY; nb_retries++) {
+		if ((freq >= min) && (freq <= max)) {
+			break;
+		}
+
+		if (freq < min) {
+			cal = trim_increase(clk_cal, cal);
+		} else {
+			cal = trim_decrease(clk_cal, cal);
+		}
+
+		clk_cal->set_trim(cal);
+
+		freq = clk_cal->get_freq();
+	}
+
+	if ((freq < min) || (freq > max)) {
+		DMSG("Calibration failed");
+		panic("Calibration");
+	}
+}
+
+static void save_trim(struct stm32mp1_clk_cal *clk_cal,
+		      unsigned int i, unsigned int x1, unsigned int x2)
+{
+	clk_cal->boundary[i].x1 = x1;
+	clk_cal->boundary[i].x2 = x2;
+}
+
+static int trim_find_prev_boundary(struct stm32mp1_clk_cal *clk_cal,
+				   unsigned int x1)
+{
+	unsigned int x = x1;
+	unsigned long freq;
+
+	clk_cal->set_trim(x1 + 1);
+	freq = clk_cal->get_freq();
+
+	while (x >= (clk_cal->cal_ref + clk_cal->trim_min)) {
+		x--;
+		clk_cal->set_trim(x);
+
+		if (clk_cal->get_freq() <= freq) {
+			break;
+		}
+	};
+
+	return x;
+}
+
+static void trim_table_init(struct stm32mp1_clk_cal *clk_cal)
+{
+	uint16_t *trim_fbv = clk_cal->fbv;
+	unsigned int min;
+	unsigned int max;
+	int boundary = 0;
+	int i = 0;
+
+	max = clk_cal->cal_ref + clk_cal->trim_max;
+	min = clk_cal->cal_ref + clk_cal->trim_min;
+
+	while (trim_fbv[i]) {
+		unsigned int x;
+		unsigned int x1 = trim_fbv[i];
+		unsigned int x2 = trim_fbv[i + 1];
+
+		if ((max <= x2) || (min >= x1)) {
+			i++;
+			if (boundary != 0) {
+				goto out;
+			}
+			continue;
+		}
+
+		/* Take forbiden value + 1 */
+		x2 = x2 + 1;
+		if (x2 < min) {
+			x2 = min;
+		}
+
+		if (boundary == 0) {
+			/* Save first boundary */
+			save_trim(clk_cal, boundary, max, x2);
+			boundary++;
+			i++;
+			continue;
+		}
+
+		x = trim_find_prev_boundary(clk_cal, x1);
+		/* Save boundary values */
+		save_trim(clk_cal, boundary, x - 1, x2);
+		boundary++;
+		i++;
+	};
+out:
+	clk_cal->boundary_max = boundary;
+}
+
+/* Timer countdown/delay argument for the target calibration periodicity */
+static uint32_t timer_val;
+
+#define CNTP_CTL_ENABLE		BIT(0)
+#define CNTP_CTL_IMASK		BIT(1)
+#define CNTP_CTL_ISTATUS	BIT(2)
+
+static void arm_timer(void)
+{
+	if (!timer_val)
+		return;
+
+	write_cntp_ctl(read_cntp_ctl() & ~(CNTP_CTL_ENABLE | CNTP_CTL_IMASK));
+	write_cntp_tval(timer_val);
+	write_cntp_ctl(read_cntp_ctl() | CNTP_CTL_ENABLE);
+}
+
+static void arm_timer_with_period(uint32_t period_sec)
+{
+	timer_val = period_sec * read_cntfrq();
+
+	arm_timer();
+}
+
+static void calib_period(void)
+{
+	(void)stm32mp_start_clock_calib(CK_HSI);
+	(void)stm32mp_start_clock_calib(CK_CSI);
+
+	arm_timer();
+}
+
+static enum itr_return arm_cntp_it_handler(struct itr_handler *handler __unused)
+{
+	if (timer_val)
+		calib_period();
+
+	return ITRR_HANDLED;
+}
+static struct itr_handler arm_cntp_handler = {
+	.it = GIC_SPI_SEC_PHY_TIMER,
+	.handler = arm_cntp_it_handler,
+};
+KEEP_PAGER(arm_cntp_handler);
+
+static void timer_pm(enum pm_op op, void *handle __unused)
+{
+	if (op != PM_OP_RESUME || !timer_val)
+		return;
+
+	calib_period();
+}
+KEEP_PAGER(timer_pm);
+
+static TEE_Result init_arm_cntp_timer(void)
+{
+	itr_add(&arm_cntp_handler);
+	itr_enable(arm_cntp_handler.it);
+
+	stm32mp_register_pm_cb(timer_pm, NULL);
+
+	return TEE_SUCCESS;
+}
+driver_init(init_arm_cntp_timer);
+
+static void init_periodic_calibration(void *fdt, int node)
+{
+	uint32_t period = fdt_read_uint32_default(fdt, node, "st,cal-sec", 0);
+
+	DMSG("Calib period %us", period);
+	arm_timer_with_period(period);
+}
+
+int stm32mp_start_clock_calib(unsigned int clock_id)
+{
+	struct stm32mp1_clk_cal *clk_calib;
+
+	switch (clock_id) {
+	case CK_HSI:
+		clk_calib = hsi_calib;
+		break;
+	case CK_CSI:
+		clk_calib = csi_calib;
+		break;
+	default:
+		DMSG("Cannot calibrate clock %u", clock_id);
+		return 1;
+	}
+
+	if (clk_calib->ref_freq == 0U)
+		return 1;
+
+	DMSG("%s", clock_id == CK_HSI ? "HSI" : "CSI");
+	rcc_calibration(clk_calib);
+
+	return 0;
+}
+
+static void init_hsi_calibration(void *fdt, int node)
+{
+	if (!fdt_getprop(fdt, node, "st,hsi-cal", NULL))
+		return;
+
+	hsi_calib = calloc(1, sizeof(*hsi_calib));
+	assert(hsi_calib);
+	memcpy(hsi_calib, &hsi_calib_config, sizeof(*hsi_calib));
+
+	stm32_timer_freq_func(&hsi_calib->get_freq, HSI_CAL);
+	assert(hsi_calib->get_freq);
+
+	hsi_calib->ref_freq = stm32mp1_clk_get_rate(CK_HSI);
+
+	hsi_calib->cal_ref = (mmio_read_32(stm32_rcc_base() + RCC_HSICFGR) &
+			       RCC_HSICFGR_HSICAL_MASK) >>
+			      RCC_HSICFGR_HSICAL_SHIFT;
+
+	trim_table_init(hsi_calib);
+
+	hsi_calib->set_trim(hsi_calib->cal_ref);
+
+	stm32mp_start_clock_calib(CK_HSI);
+}
+
+static void init_csi_calibration(void *fdt, int node)
+{
+	if (!fdt_getprop(fdt, node, "st,csi-cal", NULL))
+		return;
+
+	csi_calib = calloc(1, sizeof(*csi_calib));
+	assert(csi_calib);
+	memcpy(csi_calib, &csi_calib_config, sizeof(*csi_calib));
+
+	stm32_timer_freq_func(&csi_calib->get_freq, CSI_CAL);
+	assert(csi_calib->get_freq);
+
+	csi_calib->ref_freq = stm32mp1_clk_get_rate(CK_CSI);
+
+	csi_calib->cal_ref = (mmio_read_32(stm32_rcc_base() + RCC_CSICFGR) &
+			      RCC_CSICFGR_CSICAL_MASK) >>
+			     RCC_CSICFGR_CSICAL_SHIFT;
+
+	trim_table_init(csi_calib);
+
+	csi_calib->set_trim(csi_calib->cal_ref);
+
+	stm32mp_start_clock_calib(CK_CSI);
+}
+
+static TEE_Result init_stm32mp1_calib(void)
+{
+	void *fdt;
+	int rcc_node = -1;
+
+	fdt = get_dt_blob();
+	if (fdt)
+		rcc_node = fdt_get_rcc_node(fdt);
+	if (rcc_node < 0)
+		panic();
+
+	init_hsi_calibration(fdt, rcc_node);
+	init_csi_calibration(fdt, rcc_node);
+	init_periodic_calibration(fdt, rcc_node);
+
+	return TEE_SUCCESS;
+}
+driver_init(init_stm32mp1_calib);
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c
new file mode 100644
index 0000000..acf5d60
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c
@@ -0,0 +1,1527 @@
+// SPDX-License-Identifier:	GPL-2.0+	BSD-3-Clause
+/*
+ * Copyright (C) 2017-2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <assert.h>
+#include <initcall.h>
+#include <kernel/delay.h>
+#include <kernel/dt.h>
+#include <drivers/stm32mp1_clk.h>
+#include <drivers/stm32mp1_rcc.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <dt-bindings/clock/stm32mp1-clksrc.h>
+#include <initcall.h>
+#include <io.h>
+#include <keep.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <kernel/spinlock.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stm32_util.h>
+#include <stm32mp_dt.h>
+#include <stm32mp_pm.h>
+#include <trace.h>
+#include <util.h>
+
+#ifdef CFG_DT
+#include <drivers/stm32mp1_clkfunc.h>
+#include <libfdt.h>
+#endif
+
+enum stm32mp1_parent_id {
+/* Oscillators are defined in enum stm32mp_osc_id */
+
+/* Other parent source */
+	_HSI_KER = NB_OSC,
+	_HSE_KER,
+	_HSE_KER_DIV2,
+	_CSI_KER,
+	_PLL1_P,
+	_PLL1_Q,
+	_PLL1_R,
+	_PLL2_P,
+	_PLL2_Q,
+	_PLL2_R,
+	_PLL3_P,
+	_PLL3_Q,
+	_PLL3_R,
+	_PLL4_P,
+	_PLL4_Q,
+	_PLL4_R,
+	_ACLK,
+	_PCLK1,
+	_PCLK2,
+	_PCLK3,
+	_PCLK4,
+	_PCLK5,
+	_HCLK6,
+	_HCLK2,
+	_CK_PER,
+	_CK_MPU,
+	_CK_MCU,
+	_PARENT_NB,
+	_UNKNOWN_ID = 0xff,
+};
+
+/* Lists only the parent clock we are interested in */
+enum stm32mp1_parent_sel {
+	_STGEN_SEL,
+	_I2C46_SEL,
+	_SPI6_SEL,
+	_USART1_SEL,
+	_RNG1_SEL,
+	_UART6_SEL,
+	_UART24_SEL,
+	_UART35_SEL,
+	_UART78_SEL,
+	_ASS_SEL,
+	_MSS_SEL,
+	_USBPHY_SEL,
+	_USBO_SEL,
+	_PARENT_SEL_NB,
+	_UNKNOWN_SEL = 0xff,
+};
+
+enum stm32mp1_pll_id {
+	_PLL1,
+	_PLL2,
+	_PLL3,
+	_PLL4,
+	_PLL_NB
+};
+
+enum stm32mp1_div_id {
+	_DIV_P,
+	_DIV_Q,
+	_DIV_R,
+	_DIV_NB,
+};
+
+enum stm32mp1_clksrc_id {
+	CLKSRC_MPU,
+	CLKSRC_AXI,
+	CLKSRC_MCU,
+	CLKSRC_PLL12,
+	CLKSRC_PLL3,
+	CLKSRC_PLL4,
+	CLKSRC_RTC,
+	CLKSRC_MCO1,
+	CLKSRC_MCO2,
+	CLKSRC_NB
+};
+
+enum stm32mp1_clkdiv_id {
+	CLKDIV_MPU,
+	CLKDIV_AXI,
+	CLKDIV_MCU,
+	CLKDIV_APB1,
+	CLKDIV_APB2,
+	CLKDIV_APB3,
+	CLKDIV_APB4,
+	CLKDIV_APB5,
+	CLKDIV_RTC,
+	CLKDIV_MCO1,
+	CLKDIV_MCO2,
+	CLKDIV_NB
+};
+
+enum stm32mp1_pllcfg {
+	PLLCFG_M,
+	PLLCFG_N,
+	PLLCFG_P,
+	PLLCFG_Q,
+	PLLCFG_R,
+	PLLCFG_O,
+	PLLCFG_NB
+};
+
+enum stm32mp1_pllcsg {
+	PLLCSG_MOD_PER,
+	PLLCSG_INC_STEP,
+	PLLCSG_SSCG_MODE,
+	PLLCSG_NB
+};
+
+enum stm32mp1_plltype {
+	PLL_800,
+	PLL_1600,
+	PLL_TYPE_NB
+};
+
+struct stm32mp1_pll {
+	uint8_t refclk_min;
+	uint8_t refclk_max;
+	uint8_t divn_max;
+};
+
+struct stm32mp1_clk_gate {
+	uint16_t offset;
+	uint8_t bit;
+	uint8_t index;
+	uint8_t set_clr;
+	uint8_t sel; /* Relates to enum stm32mp1_parent_sel */
+	uint8_t fixed; /* Relates to enum stm32mp1_parent_id */
+};
+
+struct stm32mp1_clk_sel {
+	uint16_t offset;
+	uint8_t src;
+	uint8_t msk;
+	uint8_t nb_parent;
+	const uint8_t *parent;
+};
+
+#define REFCLK_SIZE 4
+struct stm32mp1_clk_pll {
+	enum stm32mp1_plltype plltype;
+	uint16_t rckxselr;
+	uint16_t pllxcfgr1;
+	uint16_t pllxcfgr2;
+	uint16_t pllxfracr;
+	uint16_t pllxcr;
+	uint16_t pllxcsgr;
+	enum stm32mp_osc_id refclk[REFCLK_SIZE];
+};
+
+/* Clocks with selectable source and not set/clr register access */
+#define _CLK_SELEC(off, b, idx, s)			\
+	{						\
+		.offset = (off),			\
+		.bit = (b),				\
+		.index = (idx),				\
+		.set_clr = 0,				\
+		.sel = (s),				\
+		.fixed = _UNKNOWN_ID,			\
+	}
+
+/* Clocks with fixed source and not set/clr register access */
+#define _CLK_FIXED(off, b, idx, f)			\
+	{						\
+		.offset = (off),			\
+		.bit = (b),				\
+		.index = (idx),				\
+		.set_clr = 0,				\
+		.sel = _UNKNOWN_SEL,			\
+		.fixed = (f),				\
+	}
+
+/* Clocks with selectable source and set/clr register access */
+#define _CLK_SC_SELEC(off, b, idx, s)			\
+	{						\
+		.offset = (off),			\
+		.bit = (b),				\
+		.index = (idx),				\
+		.set_clr = 1,				\
+		.sel = (s),				\
+		.fixed = _UNKNOWN_ID,			\
+	}
+
+/* Clocks with fixed source and set/clr register access */
+#define _CLK_SC_FIXED(off, b, idx, f)			\
+	{						\
+		.offset = (off),			\
+		.bit = (b),				\
+		.index = (idx),				\
+		.set_clr = 1,				\
+		.sel = _UNKNOWN_SEL,			\
+		.fixed = (f),				\
+	}
+
+/*
+ * Clocks with selectable source and set/clr register access
+ * and enable bit position defined by a label (argument b)
+ */
+#define _CLK_SC2_SELEC(off, b, idx, s)			\
+	{						\
+		.offset = (off),			\
+		.index = (idx),				\
+		.bit = off ## _ ## b ## _POS,		\
+		.set_clr = 1,				\
+		.sel = (s),				\
+		.fixed = _UNKNOWN_ID,			\
+	}
+#define _CLK_SC2_FIXED(off, b, idx, f)			\
+	{						\
+		.offset = (off),			\
+		.index = (idx),				\
+		.bit = off ## _ ## b ## _POS,		\
+		.set_clr = 1,				\
+		.sel = _UNKNOWN_SEL,			\
+		.fixed = (f),				\
+	}
+
+#define _CLK_PARENT(idx, off, s, m, p)			\
+	[(idx)] = {					\
+		.offset = (off),			\
+		.src = (s),				\
+		.msk = (m),				\
+		.parent = (p),				\
+		.nb_parent = ARRAY_SIZE(p)		\
+	}
+
+#define _CLK_PLL(idx, type, off1, off2, off3,		\
+		 off4, off5, off6,			\
+		 p1, p2, p3, p4)			\
+	[(idx)] = {					\
+		.plltype = (type),			\
+		.rckxselr = (off1),			\
+		.pllxcfgr1 = (off2),			\
+		.pllxcfgr2 = (off3),			\
+		.pllxfracr = (off4),			\
+		.pllxcr = (off5),			\
+		.pllxcsgr = (off6),			\
+		.refclk[0] = (p1),			\
+		.refclk[1] = (p2),			\
+		.refclk[2] = (p3),			\
+		.refclk[3] = (p4),			\
+	}
+
+static const uint8_t stm32mp1_clks[][2] = {
+	{ CK_PER, _CK_PER },
+	{ CK_MPU, _CK_MPU },
+	{ CK_AXI, _ACLK },
+	{ CK_MCU, _CK_MCU },
+	{ CK_HSE, _HSE },
+	{ CK_CSI, _CSI },
+	{ CK_LSI, _LSI },
+	{ CK_LSE, _LSE },
+	{ CK_HSI, _HSI },
+	{ CK_HSE_DIV2, _HSE_KER_DIV2 },
+};
+
+#define NB_GATES	ARRAY_SIZE(stm32mp1_clk_gate)
+
+static const struct stm32mp1_clk_gate stm32mp1_clk_gate[] = {
+	_CLK_FIXED(RCC_DDRITFCR, 0, DDRC1, _ACLK),
+	_CLK_FIXED(RCC_DDRITFCR, 1, DDRC1LP, _ACLK),
+	_CLK_FIXED(RCC_DDRITFCR, 2, DDRC2, _ACLK),
+	_CLK_FIXED(RCC_DDRITFCR, 3, DDRC2LP, _ACLK),
+	_CLK_FIXED(RCC_DDRITFCR, 4, DDRPHYC, _PLL2_R),
+	_CLK_FIXED(RCC_DDRITFCR, 5, DDRPHYCLP, _PLL2_R),
+	_CLK_FIXED(RCC_DDRITFCR, 6, DDRCAPB, _PCLK4),
+	_CLK_FIXED(RCC_DDRITFCR, 7, DDRCAPBLP, _PCLK4),
+	_CLK_FIXED(RCC_DDRITFCR, 8, AXIDCG, _ACLK),
+	_CLK_FIXED(RCC_DDRITFCR, 9, DDRPHYCAPB, _PCLK4),
+	_CLK_FIXED(RCC_DDRITFCR, 10, DDRPHYCAPBLP, _PCLK4),
+
+	_CLK_SC2_SELEC(RCC_MP_APB5ENSETR, SPI6EN, SPI6_K, _SPI6_SEL),
+	_CLK_SC2_SELEC(RCC_MP_APB5ENSETR, I2C4EN, I2C4_K, _I2C46_SEL),
+	_CLK_SC2_SELEC(RCC_MP_APB5ENSETR, I2C6EN, I2C6_K, _I2C46_SEL),
+	_CLK_SC2_SELEC(RCC_MP_APB5ENSETR, USART1EN, USART1_K, _USART1_SEL),
+	_CLK_SC2_FIXED(RCC_MP_APB5ENSETR, RTCAPBEN, RTCAPB, _PCLK5),
+	_CLK_SC2_FIXED(RCC_MP_APB5ENSETR, TZC1EN, TZC1, _PCLK5),
+	_CLK_SC2_FIXED(RCC_MP_APB5ENSETR, TZC2EN, TZC2, _PCLK5),
+	_CLK_SC2_FIXED(RCC_MP_APB5ENSETR, TZPCEN, TZPC, _PCLK5),
+	_CLK_SC2_FIXED(RCC_MP_APB5ENSETR, IWDG1APBEN, IWDG1, _PCLK5),
+	_CLK_SC2_FIXED(RCC_MP_APB5ENSETR, BSECEN, BSEC, _PCLK5),
+	_CLK_SC2_SELEC(RCC_MP_APB5ENSETR, STGENEN, STGEN_K, _STGEN_SEL),
+
+	_CLK_SC2_FIXED(RCC_MP_AHB5ENSETR, GPIOZEN, GPIOZ, _PCLK5),
+	_CLK_SC2_FIXED(RCC_MP_AHB5ENSETR, CRYP1EN, CRYP1, _PCLK5),
+	_CLK_SC2_FIXED(RCC_MP_AHB5ENSETR, HASH1EN, HASH1, _PCLK5),
+	_CLK_SC2_SELEC(RCC_MP_AHB5ENSETR, RNG1EN, RNG1_K, _RNG1_SEL),
+	_CLK_SC2_FIXED(RCC_MP_AHB5ENSETR, BKPSRAMEN, BKPSRAM, _PCLK5),
+
+	/* Non-secure clocks */
+#ifdef CFG_WITH_NSEC_GPIOS
+	_CLK_SC_FIXED(RCC_MP_AHB4ENSETR, 0, GPIOA, _UNKNOWN_ID),
+	_CLK_SC_FIXED(RCC_MP_AHB4ENSETR, 1, GPIOB, _UNKNOWN_ID),
+	_CLK_SC_FIXED(RCC_MP_AHB4ENSETR, 2, GPIOC, _UNKNOWN_ID),
+	_CLK_SC_FIXED(RCC_MP_AHB4ENSETR, 3, GPIOD, _UNKNOWN_ID),
+	_CLK_SC_FIXED(RCC_MP_AHB4ENSETR, 4, GPIOE, _UNKNOWN_ID),
+	_CLK_SC_FIXED(RCC_MP_AHB4ENSETR, 5, GPIOF, _UNKNOWN_ID),
+	_CLK_SC_FIXED(RCC_MP_AHB4ENSETR, 6, GPIOG, _UNKNOWN_ID),
+	_CLK_SC_FIXED(RCC_MP_AHB4ENSETR, 7, GPIOH, _UNKNOWN_ID),
+	_CLK_SC_FIXED(RCC_MP_AHB4ENSETR, 8, GPIOI, _UNKNOWN_ID),
+	_CLK_SC_FIXED(RCC_MP_AHB4ENSETR, 9, GPIOJ, _UNKNOWN_ID),
+	_CLK_SC_FIXED(RCC_MP_AHB4ENSETR, 10, GPIOK, _UNKNOWN_ID),
+#endif
+#ifdef CFG_WITH_NSEC_UARTS
+	_CLK_SC_SELEC(RCC_MP_APB1ENSETR, 14, USART2_K, _UART24_SEL),
+	_CLK_SC_SELEC(RCC_MP_APB1ENSETR, 15, USART3_K, _UART35_SEL),
+	_CLK_SC_SELEC(RCC_MP_APB1ENSETR, 16, UART4_K, _UART24_SEL),
+	_CLK_SC_SELEC(RCC_MP_APB1ENSETR, 17, UART5_K, _UART35_SEL),
+	_CLK_SC_SELEC(RCC_MP_APB1ENSETR, 18, UART7_K, _UART78_SEL),
+	_CLK_SC_SELEC(RCC_MP_APB1ENSETR, 19, UART8_K, _UART78_SEL),
+	_CLK_SC_SELEC(RCC_MP_APB2ENSETR, 13, USART6_K, _UART6_SEL),
+#endif
+	_CLK_SC_SELEC(RCC_MP_APB4ENSETR, 8, DDRPERFM, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(RCC_MP_APB4ENSETR, 15, IWDG2, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(RCC_MP_APB4ENSETR, 16, USBPHY_K, _USBPHY_SEL),
+	_CLK_SC_SELEC(RCC_MP_AHB2ENSETR, 8, USBO_K, _USBO_SEL),
+	_CLK_SELEC(RCC_DBGCFGR, 8, CK_DBG, _UNKNOWN_SEL),
+	_CLK_SC_FIXED(RCC_MP_APB1ENSETR, 6, TIM12_K, _PCLK1),
+	_CLK_SC_FIXED(RCC_MP_APB2ENSETR, 2, TIM15_K, _PCLK2),
+};
+KEEP_PAGER(stm32mp1_clk_gate);
+
+/* Parents for secure aware clocks in the xxxSELR value ordering */
+static const uint8_t stgen_parents[] = {
+	_HSI_KER, _HSE_KER
+};
+
+static const uint8_t i2c46_parents[] = {
+	_PCLK5, _PLL3_Q, _HSI_KER, _CSI_KER
+};
+
+static const uint8_t spi6_parents[] = {
+	_PCLK5, _PLL4_Q, _HSI_KER, _CSI_KER, _HSE_KER, _PLL3_Q
+};
+
+static const uint8_t usart1_parents[] = {
+	_PCLK5, _PLL3_Q, _HSI_KER, _CSI_KER, _PLL4_Q, _HSE_KER
+};
+
+static const uint8_t rng1_parents[] = {
+	_CSI, _PLL4_R, _LSE, _LSI
+};
+
+/* Parents for (some) non-secure clocks */
+static const uint8_t uart6_parents[] = {
+	_PCLK2, _PLL4_Q, _HSI_KER, _CSI_KER, _HSE_KER
+};
+
+static const uint8_t uart234578_parents[] = {
+	_PCLK1, _PLL4_Q, _HSI_KER, _CSI_KER, _HSE_KER
+};
+
+static const uint8_t ass_parents[] = {
+	_HSI, _HSE, _PLL2
+};
+
+static const uint8_t mss_parents[] = {
+	_HSI, _HSE, _CSI, _PLL3
+};
+
+static const uint8_t usbphy_parents[] = {
+	_HSE_KER, _PLL4_R, _HSE_KER_DIV2
+};
+
+static const uint8_t usbo_parents[] = {
+	_PLL4_R, _USB_PHY_48
+};
+
+static const struct stm32mp1_clk_sel stm32mp1_clk_sel[_PARENT_SEL_NB] = {
+	/* Secure aware clocks */
+	_CLK_PARENT(_STGEN_SEL, RCC_STGENCKSELR, 0, 0x3, stgen_parents),
+	_CLK_PARENT(_I2C46_SEL, RCC_I2C46CKSELR, 0, 0x7, i2c46_parents),
+	_CLK_PARENT(_SPI6_SEL, RCC_SPI6CKSELR, 0, 0x7, spi6_parents),
+	_CLK_PARENT(_USART1_SEL, RCC_UART1CKSELR, 0, 0x7, usart1_parents),
+	_CLK_PARENT(_RNG1_SEL, RCC_RNG1CKSELR, 0, 0x3, rng1_parents),
+	/* Always non-secure clocks (maybe used in some way in secure world) */
+	_CLK_PARENT(_UART6_SEL, RCC_UART6CKSELR, 0, 0x7, uart6_parents),
+	_CLK_PARENT(_UART24_SEL, RCC_UART24CKSELR, 0, 0x7, uart234578_parents),
+	_CLK_PARENT(_UART35_SEL, RCC_UART35CKSELR, 0, 0x7, uart234578_parents),
+	_CLK_PARENT(_UART78_SEL, RCC_UART78CKSELR, 0, 0x7, uart234578_parents),
+	_CLK_PARENT(_ASS_SEL, RCC_ASSCKSELR, 0, 0x3, ass_parents),
+	_CLK_PARENT(_MSS_SEL, RCC_MSSCKSELR, 0, 0x3, mss_parents),
+	_CLK_PARENT(_USBPHY_SEL, RCC_USBCKSELR, 0, 0x3, usbphy_parents),
+	_CLK_PARENT(_USBO_SEL, RCC_USBCKSELR, 4, 0x1, usbo_parents),
+};
+
+/* PLLNCFGR2 register divider by output */
+static const uint8_t pllncfgr2[_DIV_NB] = {
+	[_DIV_P] = RCC_PLLNCFGR2_DIVP_SHIFT,
+	[_DIV_Q] = RCC_PLLNCFGR2_DIVQ_SHIFT,
+	[_DIV_R] = RCC_PLLNCFGR2_DIVR_SHIFT,
+};
+
+static const struct stm32mp1_clk_pll stm32mp1_clk_pll[_PLL_NB] = {
+	_CLK_PLL(_PLL1, PLL_1600,
+		 RCC_RCK12SELR, RCC_PLL1CFGR1, RCC_PLL1CFGR2,
+		 RCC_PLL1FRACR, RCC_PLL1CR, RCC_PLL1CSGR,
+		 _HSI, _HSE, _UNKNOWN_OSC_ID, _UNKNOWN_OSC_ID),
+	_CLK_PLL(_PLL2, PLL_1600,
+		 RCC_RCK12SELR, RCC_PLL2CFGR1, RCC_PLL2CFGR2,
+		 RCC_PLL2FRACR, RCC_PLL2CR, RCC_PLL2CSGR,
+		 _HSI, _HSE, _UNKNOWN_OSC_ID, _UNKNOWN_OSC_ID),
+	_CLK_PLL(_PLL3, PLL_800,
+		 RCC_RCK3SELR, RCC_PLL3CFGR1, RCC_PLL3CFGR2,
+		 RCC_PLL3FRACR, RCC_PLL3CR, RCC_PLL3CSGR,
+		 _HSI, _HSE, _CSI, _UNKNOWN_OSC_ID),
+	_CLK_PLL(_PLL4, PLL_800,
+		 RCC_RCK4SELR, RCC_PLL4CFGR1, RCC_PLL4CFGR2,
+		 RCC_PLL4FRACR, RCC_PLL4CR, RCC_PLL4CSGR,
+		 _HSI, _HSE, _CSI, _I2S_CKIN),
+};
+
+/* Prescaler table lookups for clock computation */
+/* div = /1 /2 /4 /8 / 16 /64 /128 /512 */
+static const uint8_t stm32mp1_mcu_div[16] = {
+	0, 1, 2, 3, 4, 6, 7, 8, 9, 9, 9, 9, 9, 9, 9, 9
+};
+
+/* div = /1 /2 /4 /8 /16 : same divider for PMU and APBX */
+#define stm32mp1_mpu_div stm32mp1_mpu_apbx_div
+#define stm32mp1_apbx_div stm32mp1_mpu_apbx_div
+static const uint8_t stm32mp1_mpu_apbx_div[8] = {
+	0, 1, 2, 3, 4, 4, 4, 4
+};
+
+/* div = /1 /2 /3 /4 */
+static const uint8_t stm32mp1_axi_div[8] = {
+	1, 2, 3, 4, 4, 4, 4, 4
+};
+
+#if TRACE_LEVEL >= TRACE_DEBUG
+static const char *const __maybe_unused stm32mp1_clk_parent_name[_PARENT_NB] = {
+	[_HSI] = "HSI",
+	[_HSE] = "HSE",
+	[_CSI] = "CSI",
+	[_LSI] = "LSI",
+	[_LSE] = "LSE",
+	[_I2S_CKIN] = "I2S_CKIN",
+	[_HSI_KER] = "HSI_KER",
+	[_HSE_KER] = "HSE_KER",
+	[_HSE_KER_DIV2] = "HSE_KER_DIV2",
+	[_CSI_KER] = "CSI_KER",
+	[_PLL1_P] = "PLL1_P",
+	[_PLL1_Q] = "PLL1_Q",
+	[_PLL1_R] = "PLL1_R",
+	[_PLL2_P] = "PLL2_P",
+	[_PLL2_Q] = "PLL2_Q",
+	[_PLL2_R] = "PLL2_R",
+	[_PLL3_P] = "PLL3_P",
+	[_PLL3_Q] = "PLL3_Q",
+	[_PLL3_R] = "PLL3_R",
+	[_PLL4_P] = "PLL4_P",
+	[_PLL4_Q] = "PLL4_Q",
+	[_PLL4_R] = "PLL4_R",
+	[_ACLK] = "ACLK",
+	[_PCLK1] = "PCLK1",
+	[_PCLK2] = "PCLK2",
+	[_PCLK3] = "PCLK3",
+	[_PCLK4] = "PCLK4",
+	[_PCLK5] = "PCLK5",
+	[_HCLK6] = "KCLK6",
+	[_HCLK2] = "HCLK2",
+	[_CK_PER] = "CK_PER",
+	[_CK_MPU] = "CK_MPU",
+	[_CK_MCU] = "CK_MCU",
+	[_USB_PHY_48] = "USB_PHY_48",
+};
+#endif
+
+/* RCC clock device driver private */
+static unsigned long stm32mp1_osc[NB_OSC];
+static unsigned int gate_refcounts[NB_GATES];
+static unsigned int refcount_lock;
+
+static const struct stm32mp1_clk_gate *gate_ref(unsigned int idx)
+{
+	return &stm32mp1_clk_gate[idx];
+}
+
+static const struct stm32mp1_clk_sel *clk_sel_ref(unsigned int idx)
+{
+	return &stm32mp1_clk_sel[idx];
+}
+
+static const struct stm32mp1_clk_pll *pll_ref(unsigned int idx)
+{
+	return &stm32mp1_clk_pll[idx];
+}
+
+static unsigned int get_id_from_rcc_bit(unsigned int offset, unsigned int bit)
+{
+	unsigned int idx;
+
+	for (idx = 0; idx < NB_GATES; idx++) {
+		const struct stm32mp1_clk_gate *gate = gate_ref(idx);
+
+		if ((offset == gate->offset) && (bit == gate->bit)) {
+			return gate->index;
+		}
+
+		if ((gate->set_clr != 0U) &&
+		    (offset == (gate->offset + RCC_MP_ENCLRR_OFFSET)) &&
+		    (bit == gate->bit)) {
+			return gate->index;
+		}
+	}
+
+	/* Currently only supported gated clocks */
+	return ~0U;
+}
+
+static unsigned long stm32mp1_clk_get_fixed(enum stm32mp_osc_id idx)
+{
+	if (idx >= NB_OSC) {
+		DMSG("clk id %d not found", idx);
+		return 0;
+	}
+
+	return stm32mp1_osc[idx];
+}
+
+static int stm32mp1_clk_get_gated_id(unsigned long id)
+{
+	unsigned int i;
+
+	for (i = 0; i < NB_GATES; i++) {
+		if (gate_ref(i)->index == id) {
+			return i;
+		}
+	}
+
+	DMSG("clk id %lu not found", id);
+	return -1;
+}
+
+static enum stm32mp1_parent_sel stm32mp1_clk_get_sel(int i)
+{
+	return (enum stm32mp1_parent_sel)(gate_ref(i)->sel);
+}
+
+static enum stm32mp1_parent_id stm32mp1_clk_get_fixed_parent(int i)
+{
+	return (enum stm32mp1_parent_id)gate_ref(i)->fixed;
+}
+
+static int stm32mp1_clk_get_parent(unsigned long id)
+{
+	const struct stm32mp1_clk_sel *sel;
+	unsigned int j;
+	uint32_t p_sel;
+	int i;
+	enum stm32mp1_parent_id p;
+	enum stm32mp1_parent_sel s;
+	uintptr_t rcc_base = stm32_rcc_base();
+
+	for (j = 0U; j < ARRAY_SIZE(stm32mp1_clks); j++) {
+		if (stm32mp1_clks[j][0] == id) {
+			return (int)stm32mp1_clks[j][1];
+		}
+	}
+
+	i = stm32mp1_clk_get_gated_id(id);
+	if (i < 0) {
+		panic();
+	}
+
+	p = stm32mp1_clk_get_fixed_parent(i);
+	if (p < _PARENT_NB) {
+		return (int)p;
+	}
+
+	s = stm32mp1_clk_get_sel(i);
+	if (s == _UNKNOWN_SEL) {
+		return -1;
+	}
+	if (s >= _PARENT_SEL_NB) {
+		panic();
+	}
+
+	sel = clk_sel_ref(s);
+	p_sel = (mmio_read_32(rcc_base + sel->offset) >> sel->src) & sel->msk;
+	if (p_sel < sel->nb_parent) {
+		return (int)sel->parent[p_sel];
+	}
+
+	DMSG("No parent selected for clk %lu", id);
+	return -1;
+}
+
+static unsigned long stm32mp1_pll_get_fref(const struct stm32mp1_clk_pll *pll)
+{
+	uint32_t selr = mmio_read_32(stm32_rcc_base() + pll->rckxselr);
+	uint32_t src = selr & RCC_SELR_REFCLK_SRC_MASK;
+
+	return stm32mp1_clk_get_fixed(pll->refclk[src]);
+}
+
+/*
+ * pll_get_fvco() : return the VCO or (VCO / 2) frequency for the requested PLL
+ * - PLL1 & PLL2 => return VCO / 2 with Fpll_y_ck = FVCO / 2 * (DIVy + 1)
+ * - PLL3 & PLL4 => return VCO     with Fpll_y_ck = FVCO / (DIVy + 1)
+ * => in all cases Fpll_y_ck = pll_get_fvco() / (DIVy + 1)
+ */
+static unsigned long stm32mp1_pll_get_fvco(const struct stm32mp1_clk_pll *pll)
+{
+	unsigned long refclk, fvco;
+	uint32_t cfgr1, fracr, divm, divn;
+
+	cfgr1 = mmio_read_32(stm32_rcc_base() + pll->pllxcfgr1);
+	fracr = mmio_read_32(stm32_rcc_base() + pll->pllxfracr);
+
+	divm = (cfgr1 & (RCC_PLLNCFGR1_DIVM_MASK)) >> RCC_PLLNCFGR1_DIVM_SHIFT;
+	divn = cfgr1 & RCC_PLLNCFGR1_DIVN_MASK;
+
+	refclk = stm32mp1_pll_get_fref(pll);
+
+	/*
+	 * With FRACV :
+	 *   Fvco = Fck_ref * ((DIVN + 1) + FRACV / 2^13) / (DIVM + 1)
+	 * Without FRACV
+	 *   Fvco = Fck_ref * ((DIVN + 1) / (DIVM + 1)
+	 */
+	if ((fracr & RCC_PLLNFRACR_FRACLE) != 0U) {
+		unsigned long long numerator;
+		unsigned long long denominator;
+		uint32_t fracv = (fracr & RCC_PLLNFRACR_FRACV_MASK) >>
+				 RCC_PLLNFRACR_FRACV_SHIFT;
+
+		numerator = (((unsigned long long)divn + 1U) << 13) + fracv;
+		numerator = refclk * numerator;
+		denominator = ((unsigned long long)divm + 1U) << 13;
+		fvco = (unsigned long)(numerator / denominator);
+	} else {
+		fvco = (unsigned long)(refclk * (divn + 1U) / (divm + 1U));
+	}
+
+	return fvco;
+}
+
+static unsigned long stm32mp1_read_pll_freq(enum stm32mp1_pll_id pll_id,
+					    enum stm32mp1_div_id div_id)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	unsigned long dfout;
+	uint32_t cfgr2, divy;
+
+	if (div_id >= _DIV_NB) {
+		return 0;
+	}
+
+	cfgr2 = mmio_read_32(stm32_rcc_base() + pll->pllxcfgr2);
+	divy = (cfgr2 >> pllncfgr2[div_id]) & RCC_PLLNCFGR2_DIVX_MASK;
+
+	dfout = stm32mp1_pll_get_fvco(pll) / (divy + 1U);
+
+	return dfout;
+}
+
+static unsigned long get_clock_rate(int p)
+{
+	uint32_t reg, clkdiv;
+	unsigned long clock = 0;
+	uintptr_t rcc_base = stm32_rcc_base();
+
+	switch (p) {
+	case _CK_MPU:
+	/* MPU sub system */
+		reg = mmio_read_32(rcc_base + RCC_MPCKSELR);
+		switch (reg & RCC_SELR_SRC_MASK) {
+		case RCC_MPCKSELR_HSI:
+			clock = stm32mp1_clk_get_fixed(_HSI);
+			break;
+		case RCC_MPCKSELR_HSE:
+			clock = stm32mp1_clk_get_fixed(_HSE);
+			break;
+		case RCC_MPCKSELR_PLL:
+			clock = stm32mp1_read_pll_freq(_PLL1, _DIV_P);
+			break;
+		case RCC_MPCKSELR_PLL_MPUDIV:
+			clock = stm32mp1_read_pll_freq(_PLL1, _DIV_P);
+
+			reg = mmio_read_32(rcc_base + RCC_MPCKDIVR);
+			clkdiv = reg & RCC_MPUDIV_MASK;
+			if (clkdiv != 0U) {
+				clock /= stm32mp1_mpu_div[clkdiv];
+			}
+			break;
+		default:
+			break;
+		}
+		break;
+	/* AXI sub system */
+	case _ACLK:
+	case _HCLK2:
+	case _HCLK6:
+	case _PCLK4:
+	case _PCLK5:
+		reg = mmio_read_32(rcc_base + RCC_ASSCKSELR);
+		switch (reg & RCC_SELR_SRC_MASK) {
+		case RCC_ASSCKSELR_HSI:
+			clock = stm32mp1_clk_get_fixed(_HSI);
+			break;
+		case RCC_ASSCKSELR_HSE:
+			clock = stm32mp1_clk_get_fixed(_HSE);
+			break;
+		case RCC_ASSCKSELR_PLL:
+			clock = stm32mp1_read_pll_freq(_PLL2, _DIV_P);
+			break;
+		default:
+			break;
+		}
+
+		/* System clock divider */
+		reg = mmio_read_32(rcc_base + RCC_AXIDIVR);
+		clock /= stm32mp1_axi_div[reg & RCC_AXIDIV_MASK];
+
+		switch (p) {
+		case _PCLK4:
+			reg = mmio_read_32(rcc_base + RCC_APB4DIVR);
+			clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
+			break;
+		case _PCLK5:
+			reg = mmio_read_32(rcc_base + RCC_APB5DIVR);
+			clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
+			break;
+		default:
+			break;
+		}
+		break;
+	/* MCU sub system */
+	case _CK_MCU:
+	case _PCLK1:
+	case _PCLK2:
+	case _PCLK3:
+		reg = mmio_read_32(rcc_base + RCC_MSSCKSELR);
+		switch (reg & RCC_SELR_SRC_MASK) {
+		case RCC_MSSCKSELR_HSI:
+			clock = stm32mp1_clk_get_fixed(_HSI);
+			break;
+		case RCC_MSSCKSELR_HSE:
+			clock = stm32mp1_clk_get_fixed(_HSE);
+			break;
+		case RCC_MSSCKSELR_CSI:
+			clock = stm32mp1_clk_get_fixed(_CSI);
+			break;
+		case RCC_MSSCKSELR_PLL:
+			clock = stm32mp1_read_pll_freq(_PLL3, _DIV_P);
+			break;
+		default:
+			break;
+		}
+
+		/* MCU clock divider */
+		reg = mmio_read_32(rcc_base + RCC_MCUDIVR);
+		clock >>= stm32mp1_mcu_div[reg & RCC_MCUDIV_MASK];
+
+		switch (p) {
+		case _PCLK1:
+			reg = mmio_read_32(rcc_base + RCC_APB1DIVR);
+			clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
+			break;
+		case _PCLK2:
+			reg = mmio_read_32(rcc_base + RCC_APB2DIVR);
+			clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
+			break;
+		case _PCLK3:
+			reg = mmio_read_32(rcc_base + RCC_APB3DIVR);
+			clock >>= stm32mp1_apbx_div[reg & RCC_APBXDIV_MASK];
+			break;
+		case _CK_MCU:
+		default:
+			break;
+		}
+		break;
+	case _CK_PER:
+		reg = mmio_read_32(rcc_base + RCC_CPERCKSELR);
+		switch (reg & RCC_SELR_SRC_MASK) {
+		case RCC_CPERCKSELR_HSI:
+			clock = stm32mp1_clk_get_fixed(_HSI);
+			break;
+		case RCC_CPERCKSELR_HSE:
+			clock = stm32mp1_clk_get_fixed(_HSE);
+			break;
+		case RCC_CPERCKSELR_CSI:
+			clock = stm32mp1_clk_get_fixed(_CSI);
+			break;
+		default:
+			break;
+		}
+		break;
+	case _HSI:
+	case _HSI_KER:
+		clock = stm32mp1_clk_get_fixed(_HSI);
+		break;
+	case _CSI:
+	case _CSI_KER:
+		clock = stm32mp1_clk_get_fixed(_CSI);
+		break;
+	case _HSE:
+	case _HSE_KER:
+		clock = stm32mp1_clk_get_fixed(_HSE);
+		break;
+	case _HSE_KER_DIV2:
+		clock = stm32mp1_clk_get_fixed(_HSE) >> 1;
+		break;
+	case _LSI:
+		clock = stm32mp1_clk_get_fixed(_LSI);
+		break;
+	case _LSE:
+		clock = stm32mp1_clk_get_fixed(_LSE);
+		break;
+	/* PLL */
+	case _PLL1_P:
+		clock = stm32mp1_read_pll_freq(_PLL1, _DIV_P);
+		break;
+	case _PLL1_Q:
+		clock = stm32mp1_read_pll_freq(_PLL1, _DIV_Q);
+		break;
+	case _PLL1_R:
+		clock = stm32mp1_read_pll_freq(_PLL1, _DIV_R);
+		break;
+	case _PLL2_P:
+		clock = stm32mp1_read_pll_freq(_PLL2, _DIV_P);
+		break;
+	case _PLL2_Q:
+		clock = stm32mp1_read_pll_freq(_PLL2, _DIV_Q);
+		break;
+	case _PLL2_R:
+		clock = stm32mp1_read_pll_freq(_PLL2, _DIV_R);
+		break;
+	case _PLL3_P:
+		clock = stm32mp1_read_pll_freq(_PLL3, _DIV_P);
+		break;
+	case _PLL3_Q:
+		clock = stm32mp1_read_pll_freq(_PLL3, _DIV_Q);
+		break;
+	case _PLL3_R:
+		clock = stm32mp1_read_pll_freq(_PLL3, _DIV_R);
+		break;
+	case _PLL4_P:
+		clock = stm32mp1_read_pll_freq(_PLL4, _DIV_P);
+		break;
+	case _PLL4_Q:
+		clock = stm32mp1_read_pll_freq(_PLL4, _DIV_Q);
+		break;
+	case _PLL4_R:
+		clock = stm32mp1_read_pll_freq(_PLL4, _DIV_R);
+		break;
+	/* Other */
+	case _USB_PHY_48:
+		clock = stm32mp1_clk_get_fixed(_USB_PHY_48);
+		break;
+	default:
+		break;
+	}
+
+	return clock;
+}
+
+static void __clk_enable(struct stm32mp1_clk_gate const *gate)
+{
+	uintptr_t base = stm32_rcc_base();
+	uint32_t bit = BIT(gate->bit);
+
+	if (gate->set_clr != 0U) {
+		mmio_write_32(base + gate->offset, bit);
+	} else {
+		io_mask32_stm32shregs(base + gate->offset, bit, bit);
+	}
+
+	FMSG("Clock %u has been enabled", gate->index);
+}
+
+static void __clk_disable(struct stm32mp1_clk_gate const *gate)
+{
+	uintptr_t base = stm32_rcc_base();
+	uint32_t bit = BIT(gate->bit);
+
+	if (gate->set_clr != 0U) {
+		mmio_write_32(base + gate->offset + RCC_MP_ENCLRR_OFFSET, bit);
+	} else {
+		io_mask32_stm32shregs(base + gate->offset, 0, bit);
+	}
+
+	FMSG("Clock %u has been disabled", gate->index);
+}
+
+static bool __clk_is_enabled(struct stm32mp1_clk_gate const *gate)
+{
+	uintptr_t base = stm32_rcc_base();
+
+	return mmio_read_32(base + gate->offset) & BIT(gate->bit);
+}
+
+bool stm32mp1_clk_is_enabled(unsigned long id)
+{
+	int i = stm32mp1_clk_get_gated_id(id);
+
+	if (i < 0) {
+		return false;
+	}
+
+	return __clk_is_enabled(gate_ref(i));
+}
+
+unsigned int stm32mp1_clk_get_refcount(unsigned long id)
+{
+	int i = stm32mp1_clk_get_gated_id(id);
+
+	return gate_refcounts[i];
+}
+
+void __stm32mp1_clk_enable(unsigned long id, bool secure)
+{
+	int i = stm32mp1_clk_get_gated_id(id);
+	uint32_t exceptions;
+
+	if (i < 0) {
+		DMSG("Invalid clock %lu: %d", id, i);
+		panic();
+	}
+
+	exceptions = may_spin_lock(&refcount_lock);
+
+	if (incr_shrefcnt(&gate_refcounts[i], secure) != 0) {
+		__clk_enable(gate_ref(i));
+	}
+
+	may_spin_unlock(&refcount_lock, exceptions);
+}
+
+void __stm32mp1_clk_disable(unsigned long id, bool secure)
+{
+	int i = stm32mp1_clk_get_gated_id(id);
+	uint32_t exceptions;
+
+	if (i < 0) {
+		DMSG("Invalid clock %lu: %d", id, i);
+		panic();
+	}
+
+	exceptions = may_spin_lock(&refcount_lock);
+
+	if (decr_shrefcnt(&gate_refcounts[i], secure) != 0) {
+		__clk_disable(gate_ref(i));
+	}
+
+	may_spin_unlock(&refcount_lock, exceptions);
+}
+
+static long get_timer_rate(long parent_rate, unsigned int apb_bus)
+{
+	uint32_t timgxpre;
+	uint32_t apbxdiv;
+	uintptr_t rcc_base = stm32_rcc_base();
+
+	switch (apb_bus) {
+	case 1:
+		apbxdiv = mmio_read_32(rcc_base + RCC_APB1DIVR) &
+			  RCC_APBXDIV_MASK;
+		timgxpre = mmio_read_32(rcc_base + RCC_TIMG1PRER) &
+			   RCC_TIMGXPRER_TIMGXPRE;
+		break;
+	case 2:
+		apbxdiv = mmio_read_32(rcc_base + RCC_APB2DIVR) &
+			  RCC_APBXDIV_MASK;
+		timgxpre = mmio_read_32(rcc_base + RCC_TIMG2PRER) &
+			   RCC_TIMGXPRER_TIMGXPRE;
+		break;
+	default:
+		panic();
+		break;
+	}
+
+	if (apbxdiv == 0) {
+		return parent_rate;
+	}
+
+	return parent_rate * (timgxpre + 1) * 2;
+}
+
+unsigned long stm32mp1_clk_get_rate(unsigned long id)
+{
+	int p;
+	unsigned long rate;
+
+	p = stm32mp1_clk_get_parent(id);
+	if (p < 0) {
+		return 0;
+	}
+
+	rate = get_clock_rate(p);
+
+	if ((id >= TIM2_K) && (id <= TIM14_K)) {
+		rate = get_timer_rate(rate, 1);
+	}
+	if ((id >= TIM1_K) && (id <= TIM17_K)) {
+		rate = get_timer_rate(rate, 2);
+	}
+
+	return rate;
+}
+
+#ifdef CFG_DT
+static void stm32mp1_osc_clk_init(const char *name,
+				  enum stm32mp_osc_id index)
+{
+	uint32_t frequency;
+	void *fdt;
+
+	fdt = get_dt_blob();
+	if (fdt == NULL) {
+		panic();
+	}
+
+	stm32mp1_osc[index] = 0;
+
+	if (fdt_osc_read_freq(fdt, name, &frequency) == 0) {
+		stm32mp1_osc[index] = frequency;
+	}
+}
+
+static void stm32mp1_osc_init(void)
+{
+	enum stm32mp_osc_id i;
+	char **name __maybe_unused = (char **)&stm32mp_osc_node_label[0];
+
+	for (i = (enum stm32mp_osc_id)0 ; i < NB_OSC; i++) {
+		stm32mp1_osc_clk_init(stm32mp_osc_node_label[i], i);
+		DMSG("Osc %s frequency: %lu", name[i], stm32mp1_osc[i]);
+	}
+}
+#else
+static void stm32mp1_osc_init(void)
+{
+}
+#endif
+
+/*
+ * Lookup platform clock from enable bit location in RCC registers.
+ * Return a valid clock ID on success, return ~0 on error.
+ */
+unsigned long stm32mp1_clk_rcc2id(size_t offset, size_t bit)
+{
+	return get_id_from_rcc_bit(offset, bit);
+}
+
+/*
+ * Get the parent ID of the target parent clock, for tagging as secure
+ * shared clock dependencies.
+ */
+static int get_parent_id_parent(unsigned int parent_id)
+{
+	enum stm32mp1_parent_sel s = _UNKNOWN_SEL;
+	enum stm32mp1_pll_id pll_id;
+	uint32_t p_sel;
+
+	switch (parent_id) {
+	case _ACLK:
+	case _PCLK4:
+	case _PCLK5:
+		s = _ASS_SEL;
+		break;
+	case _PLL1_P:
+	case _PLL1_Q:
+	case _PLL1_R:
+		pll_id = _PLL1;
+		break;
+	case _PLL2_P:
+	case _PLL2_Q:
+	case _PLL2_R:
+		pll_id = _PLL2;
+		break;
+	case _PLL3_P:
+	case _PLL3_Q:
+	case _PLL3_R:
+		pll_id = _PLL3;
+		break;
+	case _PLL4_P:
+	case _PLL4_Q:
+	case _PLL4_R:
+		pll_id = _PLL4;
+		break;
+	case _PCLK1:
+	case _PCLK2:
+	case _HCLK2:
+	case _HCLK6:
+	case _CK_PER:
+	case _CK_MPU:
+	case _CK_MCU:
+	case _USB_PHY_48:
+		/* We do not expected to access these */
+		panic();
+		break;
+	default:
+		/* Other parents have no parent */
+		return -1;
+	}
+
+	if (s != _UNKNOWN_SEL) {
+		const struct stm32mp1_clk_sel *sel = clk_sel_ref(s);
+		uintptr_t rcc_base = stm32_rcc_base();
+
+		p_sel = (mmio_read_32(rcc_base + sel->offset) >> sel->src) &
+			sel->msk;
+
+		if (p_sel < sel->nb_parent) {
+			return (int)sel->parent[p_sel];
+		}
+	} else {
+		const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+
+		p_sel = mmio_read_32(stm32_rcc_base() + pll->rckxselr) &
+			RCC_SELR_REFCLK_SRC_MASK;
+
+		if (pll->refclk[p_sel] != _UNKNOWN_OSC_ID) {
+			return (int)pll->refclk[p_sel];
+		}
+	}
+
+	FMSG("No parent selected for %s", stm32mp1_clk_parent_name[parent_id]);
+	return -1;
+}
+
+static void secure_parent_clocks(unsigned long parent_id)
+{
+	int grandparent_id;
+
+	switch (parent_id) {
+	/* Secure only the parents for these clocks */
+	case _ACLK:
+	case _HCLK2:
+	case _HCLK6:
+	case _PCLK4:
+	case _PCLK5:
+		break;
+	/* PLLs */
+	case _PLL1_P:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_PLL1_P);
+		break;
+	case _PLL1_Q:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_PLL1_Q);
+		break;
+	case _PLL1_R:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_PLL1_R);
+		break;
+
+	case _PLL2_P:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_PLL2_P);
+		break;
+	case _PLL2_Q:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_PLL2_Q);
+		break;
+	case _PLL2_R:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_PLL2_R);
+		break;
+
+	case _PLL3_P:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_PLL3_P);
+		break;
+	case _PLL3_Q:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_PLL3_Q);
+		break;
+	case _PLL3_R:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_PLL3_R);
+		break;
+
+	/* Source clocks */
+	case _HSI:
+	case _HSI_KER:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_HSI);
+		break;
+	case _LSI:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_LSI);
+		break;
+	case _CSI:
+	case _CSI_KER:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_CSI);
+		break;
+	case _HSE:
+	case _HSE_KER:
+	case _HSE_KER_DIV2:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_HSE);
+		break;
+	case _LSE:
+		stm32mp_register_secure_periph(STM32MP1_SHRES_LSE);
+		break;
+
+	default:
+		panic();
+	}
+
+	grandparent_id = get_parent_id_parent(parent_id);
+	if (grandparent_id >= 0) {
+		secure_parent_clocks(grandparent_id);
+	}
+}
+
+void stm32mp_register_clock_parents_secure(unsigned long clock_id)
+{
+	int parent_id;
+
+	switch (clock_id) {
+	case PLL1:
+		parent_id = get_parent_id_parent(_PLL1_P);
+		break;
+	case PLL2:
+		parent_id = get_parent_id_parent(_PLL2_P);
+		break;
+	case PLL3:
+		parent_id = get_parent_id_parent(_PLL3_P);
+		break;
+	case PLL4:
+		EMSG("PLL4 cannot be secure");
+		panic();
+	default:
+		/* Others are expected gateable clock */
+		parent_id = stm32mp1_clk_get_parent(clock_id);
+		break;
+	}
+
+	if (parent_id < 0) {
+		DMSG("No parent for clock %lu", clock_id);
+		panic();
+	}
+
+	secure_parent_clocks(parent_id);
+}
+
+#ifdef CFG_DT
+/*
+ * Check that the device tree does not provide clock tree configuration
+ * information. Such configuration would not be applied since the early boot
+ * loader is in charge of configuring the clock tree and enabling the PLLs.
+ */
+static void init_clock_tree_from_dt(void)
+{
+	void *fdt;
+	uintptr_t rcc_base = stm32_rcc_base();
+	int node = -1;
+	unsigned int i;
+	int len;
+	int ignored = 0;
+
+	fdt = get_dt_blob();
+	if (fdt != NULL) {
+		node = fdt_get_rcc_node(fdt);
+	}
+
+	if ((fdt == NULL) || (node < 0)) {
+		panic("RCC DT");
+	}
+
+	if ((_fdt_get_status(fdt, node) & DT_STATUS_OK_SEC) == 0) {
+		panic("RCC disabled");
+	}
+
+	assert(virt_to_phys((void *)stm32_rcc_base()) ==
+	       fdt_rcc_read_addr(fdt));
+
+	/* Expect booting from a secure setup */
+	if ((mmio_read_32(rcc_base + RCC_TZCR) & RCC_TZCR_TZEN) == 0) {
+		panic("RCC TZC[TZEN]");
+	}
+
+	/* Get oscillator frequency to handle freq get/set operations */
+	stm32mp1_osc_init();
+
+	node = fdt_get_rcc_node(fdt);
+	assert(node >= 0);
+
+	/*
+	 * OP-TEE core is not in charge of the clock tree configuration.
+	 * This is expected from an earlier boot stage. Modifying the clock
+	 * tree here may jeopardize the already configured clock tree.
+	 * The sequence below ignores such DT directives with a friendly
+	 * debug trace.
+	 */
+	if (fdt_getprop(fdt, node, "st,clksrc", &len)) {
+		DMSG("Ignore source clocks configuration from DT");
+		ignored++;
+	}
+	if (fdt_getprop(fdt, node, "st,clkdiv", &len)) {
+		DMSG("Ignore clock divisors configuration from DT");
+		ignored++;
+	}
+	if (fdt_getprop(fdt, node, "st,pkcs", &len)) {
+		DMSG("Ignore peripheral clocks tree configuration from DT");
+		ignored++;
+	}
+	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
+		char name[12];
+
+		snprintf(name, sizeof(name), "st,pll@%d", i);
+		node = fdt_rcc_subnode_offset(fdt, name);
+
+		if (node <= 0) {
+			continue;
+		}
+
+		if (fdt_getprop(fdt, node, "cfg", &len) ||
+		    fdt_getprop(fdt, node, "frac", &len)) {
+			DMSG("Ignore PLL%u configurations from DT", i);
+			ignored++;
+		}
+	}
+
+	if (ignored != 0) {
+		IMSG("DT clock tree configurations were ignored");
+	}
+}
+#else
+static void init_clock_tree_from_dt(void)
+{
+	uintptr_t rcc_base = stm32_rcc_base();
+
+	/* Expect booting from a secure setup */
+	if ((mmio_read_32(rcc_base + RCC_TZCR) & RCC_TZCR_TZEN) == 0) {
+		panic("RCC TZC[TZEN]");
+	}
+}
+#endif /*CFG_DT*/
+
+/* Sync secure clock refcount after all drivers probe/inits,  */
+void stm32mp_update_earlyboot_clocks_state(void)
+{
+	unsigned int idx;
+
+	for (idx = 0; idx < NB_GATES; idx++) {
+		unsigned long clock_id = gate_ref(idx)->index;
+
+		/*
+		 * Drop non-secure refcount set on shareable clocks that are
+		 * not shared. Secure clock should not hold a non-secure
+		 * refcount. Non-secure clock cannot hold any refcount.
+		 */
+		if (__clk_is_enabled(gate_ref(idx)) &&
+		    stm32mp_clock_is_shareable(clock_id) &&
+		    !stm32mp_clock_is_shared(clock_id)) {
+			stm32mp1_clk_disable_non_secure(clock_id);
+		}
+
+		/*
+		 * Disable secure clocks enabled from early boot but not explicitly
+		 * enabled from the secure world.
+		 */
+		if (__clk_is_enabled(gate_ref(idx)) &&
+		    !stm32mp_clock_is_non_secure(clock_id) &&
+		    !gate_refcounts[idx]) {
+			__clk_disable(gate_ref(idx));
+		}
+	}
+
+	/* Dump clocks state */
+	for (idx = 0; idx < NB_GATES; idx++) {
+		unsigned long __maybe_unused clock_id = gate_ref(idx)->index;
+		int __maybe_unused p = stm32mp1_clk_get_parent(clock_id);
+
+		FMSG("stm32mp clock %3lu is %sabled (refcnt %d) (parent %d %s)",
+			clock_id,
+			__clk_is_enabled(gate_ref(idx)) ? "en" : "dis",
+			gate_refcounts[idx],
+			p, p < 0 ? "n.a" : stm32mp1_clk_parent_name[p]);
+	}
+}
+
+/* Set a non-secure refcount on shareable clock that were enabled from boot */
+static void sync_earlyboot_clocks_state(void)
+{
+	unsigned int idx;
+
+	for (idx = 0; idx < NB_GATES; idx++) {
+		assert(!gate_refcounts[idx]);
+	}
+
+	/*
+	 * Set a non-secure refcount for shareable clocks enabled from boot.
+	 * It will be dropped after core inits for secure-only clocks.
+	 */
+	for (idx = 0; idx < NB_GATES; idx++) {
+		struct stm32mp1_clk_gate const *gate = gate_ref(idx);
+
+		if (__clk_is_enabled(gate) &&
+		    stm32mp_clock_is_shareable(gate->index)) {
+			gate_refcounts[idx] = SHREFCNT_NONSECURE_FLAG;
+		}
+	}
+
+	/*
+	 * Register secure clock parents and init a refcount for
+	 * secure only resources that are not registered from a driver probe.
+	 * - DDR controller and phy clocks.
+	 * - TZC400, ETZPC and STGEN clocks.
+	 * - RTCAPB clocks on multi-core
+	 */
+	stm32mp_register_clock_parents_secure(DDRC1);
+	stm32mp1_clk_enable_secure(DDRC1);
+	stm32mp_register_clock_parents_secure(DDRC1LP);
+	stm32mp1_clk_enable_secure(DDRC1LP);
+	stm32mp_register_clock_parents_secure(DDRC2);
+	stm32mp1_clk_enable_secure(DDRC2);
+	stm32mp_register_clock_parents_secure(DDRC2LP);
+	stm32mp1_clk_enable_secure(DDRC2LP);
+	stm32mp_register_clock_parents_secure(DDRPHYC);
+	stm32mp1_clk_enable_secure(DDRPHYC);
+	stm32mp_register_clock_parents_secure(DDRPHYCLP);
+	stm32mp1_clk_enable_secure(DDRPHYCLP);
+	stm32mp_register_clock_parents_secure(DDRCAPB);
+	stm32mp1_clk_enable_secure(DDRCAPB);
+	stm32mp_register_clock_parents_secure(AXIDCG);
+	stm32mp1_clk_enable_secure(AXIDCG);
+	stm32mp_register_clock_parents_secure(DDRPHYCAPB);
+	stm32mp1_clk_enable_secure(DDRPHYCAPB);
+	stm32mp_register_clock_parents_secure(DDRPHYCAPBLP);
+	stm32mp1_clk_enable_secure(DDRPHYCAPBLP);
+
+	stm32mp_register_clock_parents_secure(TZPC);
+	stm32mp1_clk_enable_secure(TZPC);
+	stm32mp_register_clock_parents_secure(TZC1);
+	stm32mp1_clk_enable_secure(TZC1);
+	stm32mp_register_clock_parents_secure(TZC2);
+	stm32mp1_clk_enable_secure(TZC2);
+	stm32mp_register_clock_parents_secure(STGEN_K);
+	stm32mp1_clk_enable_secure(STGEN_K);
+
+	stm32mp_register_clock_parents_secure(BSEC);
+	stm32mp1_clk_enable_secure(BSEC);
+
+	stm32mp_register_clock_parents_secure(BKPSRAM);
+
+	stm32mp_register_clock_parents_secure(RTCAPB);
+
+#if CFG_TEE_CORE_NB_CORE > 1
+	stm32mp1_clk_enable_secure(RTCAPB);
+#endif
+
+	/* The low power sequences mandates RNG1 and CRYP1 support */
+	stm32mp_register_clock_parents_secure(RNG1_K);
+	stm32mp_register_clock_parents_secure(CRYP1);
+}
+
+static void _clock_resume(void)
+{
+	unsigned int idx;
+
+	/* Sync secure and shared clocks physical state on functional state */
+	for (idx = 0; idx < NB_GATES; idx++) {
+		struct stm32mp1_clk_gate const *gate = gate_ref(idx);
+
+		if (stm32mp_clock_is_non_secure(gate->index)) {
+			continue;
+		}
+
+		if (gate_refcounts[idx]) {
+			DMSG("Force clock %d enable", gate->index);
+			__clk_enable(gate);
+		} else {
+			DMSG("Force clock %d disable", gate->index);
+			__clk_disable(gate);
+		}
+	}
+}
+
+void stm32mp_clock_suspend_resume(enum pm_op op)
+{
+	switch (op) {
+	case PM_OP_SUSPEND:
+		/* Nothing to do */
+		break;
+	case PM_OP_RESUME:
+		_clock_resume();
+		break;
+	default:
+		panic();
+	}
+}
+
+static TEE_Result stm32mp1_clk_probe(void)
+{
+	init_clock_tree_from_dt();
+
+	sync_earlyboot_clocks_state();
+
+	return TEE_SUCCESS;
+}
+/* Setup clock support before driver initialization */
+service_init(stm32mp1_clk_probe);
+
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.h b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.h
new file mode 100644
index 0000000..783d81d
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.h
@@ -0,0 +1,62 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef __STM32MP1_CLK_H__
+#define __STM32MP1_CLK_H__
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+enum stm32mp_osc_id {
+	_HSI,
+	_HSE,
+	_CSI,
+	_LSI,
+	_LSE,
+	_I2S_CKIN,
+	_USB_PHY_48,
+	NB_OSC,
+	_UNKNOWN_OSC_ID = 0xFF
+};
+
+void __stm32mp1_clk_enable(unsigned long id, bool caller_is_secure);
+void __stm32mp1_clk_disable(unsigned long id, bool caller_is_secure);
+bool stm32mp1_clk_is_enabled(unsigned long id);
+
+static inline void stm32mp1_clk_enable_non_secure(unsigned long id)
+{
+	__stm32mp1_clk_enable(id, false);
+}
+
+static inline void stm32mp1_clk_enable_secure(unsigned long id)
+{
+	__stm32mp1_clk_enable(id, true);
+}
+
+static inline void stm32mp1_clk_disable_non_secure(unsigned long id)
+{
+	__stm32mp1_clk_disable(id, false);
+}
+
+static inline void stm32mp1_clk_disable_secure(unsigned long id)
+{
+	__stm32mp1_clk_disable(id, true);
+}
+
+unsigned int stm32mp1_clk_get_refcount(unsigned long id);
+
+unsigned long stm32mp1_clk_get_rate(unsigned long id);
+
+unsigned long stm32mp1_clk_rcc2id(size_t offset, size_t bit);
+
+void stm32mp_register_clock_parents_secure(unsigned long id);
+
+void stm32mp_update_earlyboot_clocks_state(void);
+
+void stm32mp1_clock_suspend(void);
+void stm32mp1_clock_resume(void);
+
+#endif /* __STM32MP1_CLK_H__ */
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clkfunc.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clkfunc.c
new file mode 100644
index 0000000..c83d561
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clkfunc.c
@@ -0,0 +1,340 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <dt-bindings/clock/stm32mp1-clksrc.h>
+#include <kernel/dt.h>
+#include <kernel/generic_boot.h>
+#include <libfdt.h>
+#include <stm32_util.h>
+#include <drivers/stm32mp1_clk.h>
+#include <drivers/stm32mp1_clkfunc.h>
+#include <stm32mp_dt.h>
+
+#define DT_RCC_NODE_NAME	"rcc@50000000"
+#define DT_RCC_CLK_COMPAT	"st,stm32mp1-rcc"
+#define DT_RCC_COMPAT		"syscon"
+#define DT_STGEN_COMPAT		"st,stm32-stgen"
+#define DT_UART_COMPAT		"st,stm32h7-uart"
+
+const char *stm32mp_osc_node_label[NB_OSC] = {
+	[_LSI] = "clk-lsi",
+	[_LSE] = "clk-lse",
+	[_HSI] = "clk-hsi",
+	[_HSE] = "clk-hse",
+	[_CSI] = "clk-csi",
+	[_I2S_CKIN] = "i2s_ckin",
+	[_USB_PHY_48] = "ck_usbo_48m"
+};
+
+/*******************************************************************************
+ * This function reads the frequency of an oscillator from its name.
+ * It reads the value indicated inside the device tree.
+ * Returns 0 on success, and a negative FDT/ERRNO error code on failure.
+ * On success, value is stored in the second parameter.
+ ******************************************************************************/
+int fdt_osc_read_freq(void *fdt, const char *name, uint32_t *freq)
+{
+	int node, subnode;
+
+	node = fdt_path_offset(fdt, "/clocks");
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		const char *cchar;
+		int ret;
+
+		cchar = fdt_get_name(fdt, subnode, &ret);
+		if (cchar == NULL) {
+			return ret;
+		}
+
+		if (strncmp(cchar, name, (size_t)ret) == 0) {
+			const fdt32_t *cuint;
+
+			cuint = fdt_getprop(fdt, subnode, "clock-frequency",
+					    &ret);
+			if (cuint == NULL) {
+				return ret;
+			}
+
+			*freq = fdt32_to_cpu(*cuint);
+
+			return 0;
+		}
+	}
+
+	/* Oscillator not found, freq=0 */
+	*freq = 0;
+	return 0;
+}
+
+/*******************************************************************************
+ * This function checks the presence of an oscillator property from its id.
+ * The search is done inside the device tree.
+ * Returns true/false regarding search result.
+ ******************************************************************************/
+bool fdt_osc_read_bool(void *fdt, enum stm32mp_osc_id osc_id,
+			const char *prop_name)
+{
+	int node, subnode;
+
+	if (osc_id >= NB_OSC) {
+		return false;
+	}
+
+	node = fdt_path_offset(fdt, "/clocks");
+	if (node < 0) {
+		return false;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		const char *cchar;
+		int ret;
+
+		cchar = fdt_get_name(fdt, subnode, &ret);
+		if (cchar == NULL) {
+			return false;
+		}
+
+		if (strncmp(cchar, stm32mp_osc_node_label[osc_id],
+			    (size_t)ret) != 0) {
+			continue;
+		}
+
+		if (fdt_getprop(fdt, subnode, prop_name, NULL) != NULL) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+/*******************************************************************************
+ * This function reads a value of a oscillator property from its id.
+ * Returns value on success, and a default value if property not found.
+ * Default value is passed as parameter.
+ ******************************************************************************/
+uint32_t fdt_osc_read_uint32_default(void *fdt, enum stm32mp_osc_id osc_id,
+				     const char *prop_name, uint32_t dflt_value)
+{
+	int node, subnode;
+
+	if (osc_id >= NB_OSC) {
+		return dflt_value;
+	}
+
+	node = fdt_path_offset(fdt, "/clocks");
+	if (node < 0) {
+		return dflt_value;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		const char *cchar;
+		int ret;
+
+		cchar = fdt_get_name(fdt, subnode, &ret);
+		if (cchar == NULL) {
+			return dflt_value;
+		}
+
+		if (strncmp(cchar, stm32mp_osc_node_label[osc_id],
+			    (size_t)ret) != 0) {
+			continue;
+		}
+
+		return fdt_read_uint32_default(fdt, subnode, prop_name,
+					       dflt_value);
+	}
+
+	return dflt_value;
+}
+
+/*******************************************************************************
+ * This function reads the rcc base address.
+ * It reads the value indicated inside the device tree.
+ * Returns address on success, and 0 on failure.
+ ******************************************************************************/
+uint32_t fdt_rcc_read_addr(void *fdt)
+{
+	int node, subnode;
+
+	node = fdt_path_offset(fdt, "/soc");
+	if (node < 0) {
+		return 0;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		const char *cchar;
+		int ret;
+
+		cchar = fdt_get_name(fdt, subnode, &ret);
+		if (cchar == NULL) {
+			return 0;
+		}
+
+		if (strncmp(cchar, DT_RCC_NODE_NAME, (size_t)ret) == 0) {
+			const fdt32_t *cuint;
+
+			cuint = fdt_getprop(fdt, subnode, "reg", NULL);
+			if (cuint == NULL) {
+				return 0;
+			}
+
+			return fdt32_to_cpu(*cuint);
+		}
+	}
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function returns the RCC node in the device tree.
+ ******************************************************************************/
+int fdt_get_rcc_node(void *fdt)
+{
+	return fdt_node_offset_by_compatible(fdt, -1, DT_RCC_CLK_COMPAT);
+}
+
+/*******************************************************************************
+ * This function reads a series of parameters in rcc-clk section.
+ * It reads the values indicated inside the device tree, from property name.
+ * The number of parameters is also indicated as entry parameter.
+ * Returns 0 on success, and a negative FDT/ERRNO error code on failure.
+ * On success, values are stored at the second parameter address.
+ ******************************************************************************/
+int fdt_rcc_read_uint32_array(void *fdt, const char *prop_name,
+			      uint32_t *array, uint32_t count)
+{
+	int node = fdt_get_rcc_node(fdt);
+
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return fdt_read_uint32_array(fdt, node, prop_name, array, count);
+}
+
+/*******************************************************************************
+ * This function gets the subnode offset in rcc-clk section from its name.
+ * It reads the values indicated inside the device tree.
+ * Returns offset on success, and a negative FDT/ERRNO error code on failure.
+ ******************************************************************************/
+int fdt_rcc_subnode_offset(void *fdt, const char *name)
+{
+	int node, subnode;
+
+	node = fdt_get_rcc_node(fdt);
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	subnode = fdt_subnode_offset(fdt, node, name);
+	if (subnode <= 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return subnode;
+}
+
+/*******************************************************************************
+ * This function gets the pointer to a rcc-clk property from its name.
+ * It reads the values indicated inside the device tree.
+ * Length of the property is stored in the second parameter.
+ * Returns pointer on success, and NULL value on failure.
+ ******************************************************************************/
+const fdt32_t *fdt_rcc_read_prop(void *fdt, const char *prop_name, int *lenp)
+{
+	const fdt32_t *cuint;
+	int node, len;
+
+	node = fdt_get_rcc_node(fdt);
+	if (node < 0) {
+		return NULL;
+	}
+
+	cuint = fdt_getprop(fdt, node, prop_name, &len);
+	if (cuint == NULL) {
+		return NULL;
+	}
+
+	*lenp = len;
+	return cuint;
+}
+
+/*******************************************************************************
+ * This function reads the stgen base address.
+ * It reads the value indicated inside the device tree.
+ * Returns address on success, and NULL value on failure.
+ ******************************************************************************/
+uintptr_t get_stgen_base(void)
+{
+	int node;
+	const fdt32_t *cuint;
+	void *fdt;
+
+	fdt = get_dt_blob();
+	if (fdt == NULL) {
+		return 0;
+	}
+
+	node = fdt_node_offset_by_compatible(fdt, -1, DT_STGEN_COMPAT);
+	if (node < 0) {
+		return 0;
+	}
+
+	cuint = fdt_getprop(fdt, node, "reg", NULL);
+	if (cuint == NULL) {
+		return 0;
+	}
+
+	return fdt32_to_cpu(*cuint);
+}
+
+/*******************************************************************************
+ * This function gets the frequency of the specified uart instance.
+ * From this instance, all the uarts nodes in DT are parsed, and the register
+ * base is compared to the instance. If match between these two values, then
+ * the clock source is read from the DT and we deduce the frequency.
+ * Returns clock frequency on success, 0 value on failure.
+ ******************************************************************************/
+unsigned long get_uart_clock_freq(uint32_t instance)
+{
+	int node;
+	void *fdt;
+
+	fdt = get_dt_blob();
+	if (fdt == NULL) {
+		return 0;
+	}
+
+	/* Check for UART nodes */
+	node = fdt_node_offset_by_compatible(fdt, -1, DT_UART_COMPAT);
+	while (node != -FDT_ERR_NOTFOUND) {
+		const fdt32_t *cuint;
+
+		cuint = fdt_getprop(fdt, node, "reg", NULL);
+		if (cuint == NULL)
+			goto next;
+
+		if ((uint32_t)fdt32_to_cpu(*cuint) == instance) {
+			unsigned long clk_id;
+
+			cuint = fdt_getprop(fdt, node, "clocks", NULL);
+			if (cuint == NULL)
+				goto next;
+
+			cuint++;
+			clk_id = (unsigned long)(fdt32_to_cpu(*cuint));
+
+			return stm32mp1_clk_get_rate(clk_id);
+		}
+next:
+		node = fdt_node_offset_by_compatible(fdt, node, DT_UART_COMPAT);
+	}
+
+	return 0;
+}
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clkfunc.h b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clkfunc.h
new file mode 100644
index 0000000..cbb489b
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clkfunc.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef __STM32MP1_CLKFUNC_H__
+#define __STM32MP1_CLKFUNC_H__
+
+#include <libfdt.h>
+#include <stdbool.h>
+
+extern const char *stm32mp_osc_node_label[NB_OSC];
+
+int fdt_osc_read_freq(void *fdt, const char *name, uint32_t *freq);
+bool fdt_osc_read_bool(void *fdt, enum stm32mp_osc_id osc_id,
+			const char *prop_name);
+uint32_t fdt_osc_read_uint32_default(void *fdt, enum stm32mp_osc_id osc_id,
+				     const char *prop_name,
+				     uint32_t dflt_value);
+
+int fdt_get_rcc_node(void *fdt);
+uint32_t fdt_rcc_read_addr(void *fdt);
+int fdt_rcc_read_uint32_array(void *fdt, const char *prop_name,
+			      uint32_t *array, uint32_t count);
+int fdt_rcc_subnode_offset(void *fdt, const char *name);
+const fdt32_t *fdt_rcc_read_prop(void *fdt, const char *prop_name, int *lenp);
+bool fdt_get_rcc_secure_status(void *fdt);
+
+uintptr_t get_stgen_base(void);
+
+unsigned long get_uart_clock_freq(uint32_t instance);
+
+#endif /* __STM32MP1_CLKFUNC_H__ */
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.c
new file mode 100644
index 0000000..03ded28
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.c
@@ -0,0 +1,513 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ */
+
+#include <arm32.h>
+#include <boot_api.h>
+#include <drivers/stm32_rtc.h>
+#include <drivers/stm32mp1_clk.h>
+#include <drivers/stm32mp1_ddrc.h>
+#include <drivers/stm32mp1_pwr.h>
+#include <drivers/stm32mp1_rcc.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <kernel/delay.h>
+#include <kernel/panic.h>
+#include <io.h>
+#include <mm/core_mmu.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <stm32_util.h>
+#include <string.h>
+#include <trace.h>
+
+#define TIMEOUT_500US		(500 * 1000)
+
+static uintptr_t get_ddrctrl_base(void)
+{
+	static void *va;
+
+	if (!cpu_mmu_enabled()) {
+		return DDRCTRL_BASE;
+	}
+
+	if (!va) {
+		va = phys_to_virt(DDRCTRL_BASE, MEM_AREA_IO_SEC);
+	}
+
+	return (uintptr_t)va;
+}
+
+static uintptr_t get_ddrphy_base(void)
+{
+	static void *va;
+
+	if (!cpu_mmu_enabled()) {
+		return DDRPHYC_BASE;
+	}
+
+	if (!va) {
+		va = phys_to_virt(DDRPHYC_BASE, MEM_AREA_IO_SEC);
+	}
+
+	return (uintptr_t)va;
+}
+
+static void ddr_disable_clock(void)
+{
+	uintptr_t rcc_base = stm32_rcc_base();
+
+	/* Disable all clocks */
+	mmio_clrbits_32(rcc_base + RCC_DDRITFCR,
+			RCC_DDRITFCR_DDRC1EN |
+			RCC_DDRITFCR_DDRC2EN |
+			RCC_DDRITFCR_DDRPHYCAPBEN |
+			RCC_DDRITFCR_DDRCAPBEN);
+}
+
+static void ddr_enable_clock(void)
+{
+	uintptr_t rcc_base = stm32_rcc_base();
+
+	/* Enable all clocks */
+	mmio_setbits_32(rcc_base + RCC_DDRITFCR,
+			RCC_DDRITFCR_DDRC1EN |
+			RCC_DDRITFCR_DDRC2EN |
+			RCC_DDRITFCR_DDRPHYCEN |
+			RCC_DDRITFCR_DDRPHYCAPBEN |
+			RCC_DDRITFCR_DDRCAPBEN);
+}
+
+static void do_sw_handshake(void)
+{
+	uintptr_t ddrctrl_base = get_ddrctrl_base();
+
+	mmio_clrbits_32(ddrctrl_base + DDRCTRL_SWCTL, DDRCTRL_SWCTL_SW_DONE);
+}
+
+static void do_sw_ack(void)
+{
+	uint64_t to_ref;
+	uintptr_t ddrctrl_base = get_ddrctrl_base();
+
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_SWCTL, DDRCTRL_SWCTL_SW_DONE);
+
+	to_ref = utimeout_init(TIMEOUT_500US);
+	while ((mmio_read_32(ddrctrl_base + DDRCTRL_SWSTAT) &
+		DDRCTRL_SWSTAT_SW_DONE_ACK) == 0U) {
+		if (utimeout_elapsed(TIMEOUT_500US, to_ref)) {
+			panic();
+		}
+	}
+}
+
+static int ddr_sw_self_refresh_in(void)
+{
+	uint64_t to_ref;
+	uint32_t operating_mode;
+	uint32_t selref_type;
+	uint8_t op_mode_changed = 0;
+	uintptr_t pwr_base = stm32_pwr_base();
+	uintptr_t rcc_base = stm32_rcc_base();
+	uintptr_t ddrctrl_base = get_ddrctrl_base();
+	uintptr_t ddrphy_base = get_ddrphy_base();
+
+	mmio_clrbits_32(rcc_base + RCC_DDRITFCR, RCC_DDRITFCR_AXIDCGEN);
+
+	/* Blocks AXI ports from taking anymore transactions */
+	mmio_clrbits_32(ddrctrl_base + DDRCTRL_PCTRL_0,
+			DDRCTRL_PCTRL_N_PORT_EN);
+	mmio_clrbits_32(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
+	 */
+	to_ref = utimeout_init(TIMEOUT_500US);
+	while (mmio_read_32(ddrctrl_base + DDRCTRL_PSTAT)) {
+		if (utimeout_elapsed(TIMEOUT_500US, to_ref)) {
+			goto pstat_failed;
+		}
+	}
+
+	/* SW Self-Refresh entry */
+	mmio_setbits_32(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.
+	 */
+	to_ref = utimeout_init(TIMEOUT_500US);
+	while (!utimeout_elapsed(TIMEOUT_500US, to_ref)) {
+		uint32_t stat = mmio_read_32(ddrctrl_base + DDRCTRL_STAT);
+
+		operating_mode = stat & DDRCTRL_STAT_OPERATING_MODE_MASK;
+		selref_type = stat & DDRCTRL_STAT_SELFREF_TYPE_MASK;
+
+		if ((operating_mode == DDRCTRL_STAT_OPERATING_MODE_SR) &&
+		    (selref_type == DDRCTRL_STAT_SELFREF_TYPE_SR)) {
+			op_mode_changed = 1;
+			break;
+		}
+	}
+
+	if (op_mode_changed == 0U)
+		goto selfref_sw_failed;
+
+	/* IOs powering down (PUBL registers) */
+	mmio_setbits_32(ddrphy_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACPDD);
+
+	mmio_setbits_32(ddrphy_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACPDR);
+
+	mmio_clrsetbits_32(ddrphy_base + DDRPHYC_ACIOCR,
+			   DDRPHYC_ACIOCR_CKPDD_MASK,
+			   DDRPHYC_ACIOCR_CKPDD_0);
+
+	mmio_clrsetbits_32(ddrphy_base + DDRPHYC_ACIOCR,
+			   DDRPHYC_ACIOCR_CKPDR_MASK,
+			   DDRPHYC_ACIOCR_CKPDR_0);
+
+	mmio_clrsetbits_32(ddrphy_base + DDRPHYC_ACIOCR,
+			   DDRPHYC_ACIOCR_CSPDD_MASK,
+			   DDRPHYC_ACIOCR_CSPDD_0);
+
+	mmio_setbits_32(ddrphy_base + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDD);
+
+	mmio_setbits_32(ddrphy_base + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDR);
+
+	mmio_clrsetbits_32(ddrphy_base + DDRPHYC_DSGCR,
+			   DDRPHYC_DSGCR_ODTPDD_MASK,
+			   DDRPHYC_DSGCR_ODTPDD_0);
+
+	mmio_setbits_32(ddrphy_base + DDRPHYC_DSGCR, DDRPHYC_DSGCR_NL2PD);
+
+	mmio_clrsetbits_32(ddrphy_base + DDRPHYC_DSGCR,
+			   DDRPHYC_DSGCR_CKEPDD_MASK,
+			   DDRPHYC_DSGCR_CKEPDD_0);
+
+	/* Disable PZQ cell (PUBL register) */
+	mmio_setbits_32(ddrphy_base + DDRPHYC_ZQ0CR0, DDRPHYC_ZQ0CRN_ZQPD);
+
+	/* Activate sw retention in PWRCTRL */
+	mmio_setbits_32(pwr_base + PWR_CR3_OFF, PWR_CR3_DDRRETEN);
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL ref clock */
+	mmio_setbits_32(rcc_base + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+
+	/* Disable all DLLs: GLITCH window */
+	mmio_setbits_32(ddrphy_base + DDRPHYC_ACDLLCR,
+			DDRPHYC_ACDLLCR_DLLDIS);
+
+	mmio_setbits_32(ddrphy_base + DDRPHYC_DX0DLLCR,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+
+	mmio_setbits_32(ddrphy_base + DDRPHYC_DX1DLLCR,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+
+	mmio_setbits_32(ddrphy_base + DDRPHYC_DX2DLLCR,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+
+	mmio_setbits_32(ddrphy_base + DDRPHYC_DX3DLLCR,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL output clock */
+	mmio_clrbits_32(rcc_base + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+
+	/* Disable all clocks */
+	ddr_disable_clock();
+
+	return 0;
+
+selfref_sw_failed:
+	/* This bit should be cleared to restore DDR in its previous state */
+	mmio_clrbits_32(ddrctrl_base + DDRCTRL_PWRCTL,
+			DDRCTRL_PWRCTL_SELFREF_SW);
+
+pstat_failed:
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_PCTRL_0,
+			DDRCTRL_PCTRL_N_PORT_EN);
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_PCTRL_1,
+			DDRCTRL_PCTRL_N_PORT_EN);
+
+	return -1;
+}
+
+int ddr_sw_self_refresh_exit(void)
+{
+	uint64_t to_ref;
+	uintptr_t rcc_base = stm32_rcc_base();
+	uintptr_t pwr_base = stm32_pwr_base();
+	uintptr_t ddrctrl_base = get_ddrctrl_base();
+	uintptr_t ddrphy_base = get_ddrphy_base();
+
+	/* Enable all clocks */
+	ddr_enable_clock();
+
+	do_sw_handshake();
+
+	/* Mask dfi_init_complete_en */
+	mmio_clrbits_32(ddrctrl_base + DDRCTRL_DFIMISC,
+			DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+
+	do_sw_ack();
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL ref clock */
+	mmio_setbits_32(rcc_base + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+
+	/* Enable all DLLs: GLITCH window */
+	mmio_clrbits_32(ddrphy_base + DDRPHYC_ACDLLCR,
+			DDRPHYC_ACDLLCR_DLLDIS);
+
+	mmio_clrbits_32(ddrphy_base + DDRPHYC_DX0DLLCR,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+
+	mmio_clrbits_32(ddrphy_base + DDRPHYC_DX1DLLCR,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+
+	mmio_clrbits_32(ddrphy_base + DDRPHYC_DX2DLLCR,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+
+	mmio_clrbits_32(ddrphy_base + DDRPHYC_DX3DLLCR,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+
+	/* Additional delay to avoid early DLL clock switch */
+	udelay(10);
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL ref clock */
+	mmio_clrbits_32(rcc_base + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+	mmio_clrbits_32(ddrphy_base + DDRPHYC_ACDLLCR, DDRPHYC_ACDLLCR_DLLSRST);
+
+	udelay(10);
+
+	mmio_setbits_32(ddrphy_base + DDRPHYC_ACDLLCR,
+			DDRPHYC_ACDLLCR_DLLSRST);
+
+	/* PHY partial init: (DLL lock and ITM reset) */
+	mmio_write_32(ddrphy_base + DDRPHYC_PIR,
+		      DDRPHYC_PIR_DLLSRST | DDRPHYC_PIR_DLLLOCK |
+		      DDRPHYC_PIR_ITMSRST | DDRPHYC_PIR_INIT);
+
+	/* Need to wait at least 10 clock cycles before accessing PGSR */
+	udelay(1);
+
+	to_ref = utimeout_init(TIMEOUT_500US);
+	while ((mmio_read_32(ddrphy_base + DDRPHYC_PGSR) &
+		DDRPHYC_PGSR_IDONE) == 0U) {
+		if (utimeout_elapsed(TIMEOUT_500US, to_ref)) {
+			return -1;
+		}
+	}
+
+	do_sw_handshake();
+
+	/* Unmask dfi_init_complete_en to uMCTL2 */
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_DFIMISC,
+			DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+
+	do_sw_ack();
+
+	/* Deactivate sw retention in PWR */
+	mmio_clrbits_32(pwr_base + PWR_CR3_OFF, PWR_CR3_DDRRETEN);
+
+	/* Enable PZQ cell (PUBL register) */
+	mmio_clrbits_32(ddrphy_base + DDRPHYC_ZQ0CR0, DDRPHYC_ZQ0CRN_ZQPD);
+
+	/* Enable pad drivers */
+	mmio_clrbits_32(ddrphy_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACPDD);
+
+	mmio_clrbits_32(ddrphy_base + DDRPHYC_ACIOCR,
+			DDRPHYC_ACIOCR_CKPDD_MASK);
+
+	mmio_clrbits_32(ddrphy_base + DDRPHYC_ACIOCR,
+			DDRPHYC_ACIOCR_CSPDD_MASK);
+
+	mmio_clrbits_32(ddrphy_base + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDD);
+
+	mmio_clrbits_32(ddrphy_base + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDR);
+
+	mmio_clrbits_32(ddrphy_base + DDRPHYC_DSGCR, DDRPHYC_DSGCR_ODTPDD_MASK);
+
+	mmio_clrbits_32(ddrphy_base + DDRPHYC_DSGCR, DDRPHYC_DSGCR_NL2PD);
+
+	mmio_clrbits_32(ddrphy_base + DDRPHYC_DSGCR, DDRPHYC_DSGCR_CKEPDD_MASK);
+
+	/* Remove selfrefresh */
+	mmio_clrbits_32(ddrctrl_base + DDRCTRL_PWRCTL,
+			DDRCTRL_PWRCTL_SELFREF_SW);
+
+	/* Wait operating_mode == normal */
+	to_ref = utimeout_init(TIMEOUT_500US);
+
+	while ((mmio_read_32(ddrctrl_base + DDRCTRL_STAT) &
+		DDRCTRL_STAT_OPERATING_MODE_MASK) !=
+	       DDRCTRL_STAT_OPERATING_MODE_NORMAL) {
+		if (utimeout_elapsed(TIMEOUT_500US, to_ref)) {
+			return -1;
+		}
+	}
+
+	/* AXI ports are no longer blocked from taking transactions */
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_PCTRL_0,
+			DDRCTRL_PCTRL_N_PORT_EN);
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_PCTRL_1,
+			DDRCTRL_PCTRL_N_PORT_EN);
+
+	mmio_setbits_32(rcc_base + RCC_DDRITFCR, RCC_DDRITFCR_AXIDCGEN);
+
+	return 0;
+}
+
+uint32_t get_ddrphy_calibration(void)
+{
+	uintptr_t ddrphy_base = get_ddrphy_base();
+	uint32_t zcal = mmio_read_32(ddrphy_base + DDRPHYC_ZQ0CR0);
+
+	return (zcal & DDRPHYC_ZQ0CRN_ZDATA_MASK) >> DDRPHYC_ZQ0CRN_ZDATA_SHIFT;
+}
+
+int ddr_standby_sr_entry(uint32_t *zq0cr0_zdata)
+{
+	uintptr_t pwr_base = stm32_pwr_base();
+	uintptr_t ddrphy_base = get_ddrphy_base();
+
+	/* Save IOs calibration values */
+	if (zq0cr0_zdata != NULL) {
+		*zq0cr0_zdata = mmio_read_32(ddrphy_base + DDRPHYC_ZQ0CR0) &
+				DDRPHYC_ZQ0CRN_ZDATA_MASK;
+	}
+
+	/* Put DDR in Self-Refresh */
+	if (ddr_sw_self_refresh_in() != 0) {
+		return -1;
+	}
+
+	/* Enable I/O retention mode in standby */
+	mmio_setbits_32(pwr_base + PWR_CR3_OFF, PWR_CR3_DDRSREN);
+
+	return 0;
+}
+
+void ddr_sr_mode_ssr(void)
+{
+	uintptr_t rcc_ddritfcr = stm32_rcc_base() + RCC_DDRITFCR;
+	uintptr_t ddrctrl_base = get_ddrctrl_base();
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC1LPEN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC2LPEN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC1EN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC2EN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRCAPBLPEN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRPHYCAPBLPEN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRCAPBEN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRPHYCAPBEN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRPHYCEN);
+
+	mmio_clrbits_32(rcc_ddritfcr, RCC_DDRITFCR_AXIDCGEN);
+
+	mmio_clrbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRCKMOD_MASK);
+
+	/* Disable HW LP interface of uMCTL2 */
+	mmio_clrbits_32(ddrctrl_base + DDRCTRL_HWLPCTL,
+			DDRCTRL_HWLPCTL_HW_LP_EN);
+
+	/* Configure Automatic LP modes of uMCTL2 */
+	mmio_clrsetbits_32(ddrctrl_base + DDRCTRL_PWRTMG,
+			   DDRCTRL_PWRTMG_SELFREF_TO_X32_MASK,
+			   DDRCTRL_PWRTMG_SELFREF_TO_X32_0);
+
+	/*
+	 * Disable Clock disable with LP modes
+	 * (used in RUN mode for LPDDR2 with specific timing).
+	 */
+	mmio_clrbits_32(ddrctrl_base + DDRCTRL_PWRCTL,
+			DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE);
+
+	/* Disable automatic Self-Refresh mode */
+	mmio_clrbits_32(ddrctrl_base + DDRCTRL_PWRCTL,
+			DDRCTRL_PWRCTL_SELFREF_EN);
+}
+
+void ddr_sr_mode_asr(void)
+{
+	uintptr_t rcc_ddritfcr = stm32_rcc_base() + RCC_DDRITFCR;
+	uintptr_t ddrctrl_base = get_ddrctrl_base();
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_AXIDCGEN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC1LPEN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC2LPEN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRPHYCLPEN);
+
+	mmio_clrsetbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRCKMOD_MASK,
+			   RCC_DDRITFCR_DDRCKMOD_ASR1);
+
+	/* Enable HW LP interface of uMCTL2 */
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_HWLPCTL,
+			DDRCTRL_HWLPCTL_HW_LP_EN);
+
+	/* Configure Automatic LP modes of uMCTL2 */
+	mmio_clrsetbits_32(ddrctrl_base + DDRCTRL_PWRTMG,
+			   DDRCTRL_PWRTMG_SELFREF_TO_X32_MASK,
+			   DDRCTRL_PWRTMG_SELFREF_TO_X32_0);
+
+	/*
+	 * Enable Clock disable with LP modes
+	 * (used in RUN mode for LPDDR2 with specific timing).
+	 */
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_PWRCTL,
+			DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE);
+
+	/* Enable automatic Self-Refresh for ASR mode */
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_PWRCTL,
+			DDRCTRL_PWRCTL_SELFREF_EN);
+}
+
+void ddr_sr_mode_hsr(void)
+{
+	uintptr_t rcc_ddritfcr = stm32_rcc_base() + RCC_DDRITFCR;
+	uintptr_t ddrctrl_base = get_ddrctrl_base();
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_AXIDCGEN);
+
+	mmio_clrbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC1LPEN);
+
+	mmio_clrbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC2LPEN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRPHYCLPEN);
+
+	mmio_clrsetbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRCKMOD_MASK,
+			   RCC_DDRITFCR_DDRCKMOD_HSR1);
+
+	/* Enable HW LP interface of uMCTL2 */
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_HWLPCTL,
+			DDRCTRL_HWLPCTL_HW_LP_EN);
+
+	/* Configure Automatic LP modes of uMCTL2 */
+	mmio_clrsetbits_32(ddrctrl_base + DDRCTRL_PWRTMG,
+			   DDRCTRL_PWRTMG_SELFREF_TO_X32_MASK,
+			   DDRCTRL_PWRTMG_SELFREF_TO_X32_0);
+
+	/*
+	 * Enable Clock disable with LP modes
+	 * (used in RUN mode for LPDDR2 with specific timing).
+	 */
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_PWRCTL,
+			DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE);
+}
+
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.h b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.h
new file mode 100644
index 0000000..59014b4
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.h
@@ -0,0 +1,205 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef __STM32MP1_DDRC_H__
+#define __STM32MP1_DDRC_H__
+
+#include <util.h>
+
+/* DDR Controller */
+/* DDR Controller registers offsets */
+#define DDRCTRL_MSTR				0x000
+#define DDRCTRL_STAT				0x004
+#define DDRCTRL_MRCTRL0				0x010
+#define DDRCTRL_MRSTAT				0x018
+#define DDRCTRL_PWRCTL				0x030
+#define DDRCTRL_PWRTMG				0x034
+#define DDRCTRL_HWLPCTL				0x038
+#define DDRCTRL_RFSHCTL3			0x060
+#define DDRCTRL_RFSHTMG				0x064
+#define DDRCTRL_INIT0				0x0D0
+#define DDRCTRL_DFIMISC				0x1B0
+#define DDRCTRL_DBG1				0x304
+#define DDRCTRL_DBGCAM				0x308
+#define DDRCTRL_DBGCMD				0x30C
+#define DDRCTRL_DBGSTAT				0x310
+#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_MSTR_DDR3			BIT(0)
+#define DDRCTRL_MSTR_LPDDR2			BIT(2)
+#define DDRCTRL_MSTR_LPDDR3			BIT(3)
+#define DDRCTRL_MSTR_DATA_BUS_WIDTH_MASK	GENMASK_32(13, 12)
+#define DDRCTRL_MSTR_DATA_BUS_WIDTH_FULL	0
+#define DDRCTRL_MSTR_DATA_BUS_WIDTH_HALF	BIT(12)
+#define DDRCTRL_MSTR_DATA_BUS_WIDTH_QUARTER	BIT(13)
+#define DDRCTRL_MSTR_DLL_OFF_MODE		BIT(15)
+
+#define DDRCTRL_STAT_OPERATING_MODE_MASK	GENMASK_32(2, 0)
+#define DDRCTRL_STAT_OPERATING_MODE_NORMAL	BIT(0)
+#define DDRCTRL_STAT_OPERATING_MODE_SR		(BIT(0) | BIT(1))
+#define DDRCTRL_STAT_SELFREF_TYPE_MASK		GENMASK_32(5, 4)
+#define DDRCTRL_STAT_SELFREF_TYPE_ASR		(BIT(4) | BIT(5))
+#define DDRCTRL_STAT_SELFREF_TYPE_SR		BIT(5)
+
+#define DDRCTRL_MRCTRL0_MR_TYPE_WRITE		0
+/* only one rank supported */
+#define DDRCTRL_MRCTRL0_MR_RANK_SHIFT		4
+#define DDRCTRL_MRCTRL0_MR_RANK_ALL	\
+					BIT(DDRCTRL_MRCTRL0_MR_RANK_SHIFT)
+#define DDRCTRL_MRCTRL0_MR_ADDR_SHIFT		12
+#define DDRCTRL_MRCTRL0_MR_ADDR_MASK		GENMASK_32(15, 12)
+#define DDRCTRL_MRCTRL0_MR_WR			BIT(31)
+
+#define DDRCTRL_MRSTAT_MR_WR_BUSY		BIT(0)
+
+#define DDRCTRL_PWRCTL_SELFREF_EN		BIT(0)
+#define DDRCTRL_PWRCTL_POWERDOWN_EN		BIT(1)
+#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_32(19, 12)
+#define DDRCTRL_PWRTMG_SELFREF_TO_X32_0		BIT(16)
+
+#define DDRCTRL_RFSHCTL3_DIS_AUTO_REFRESH	BIT(0)
+
+#define DDRCTRL_HWLPCTL_HW_LP_EN		BIT(0)
+
+#define DDRCTRL_RFSHTMG_T_RFC_NOM_X1_X32_MASK	GENMASK_32(27, 16)
+#define DDRCTRL_RFSHTMG_T_RFC_NOM_X1_X32_SHIFT	16
+
+#define DDRCTRL_INIT0_SKIP_DRAM_INIT_MASK	GENMASK_32(31, 30)
+#define DDRCTRL_INIT0_SKIP_DRAM_INIT_NORMAL	BIT(30)
+
+#define DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN	BIT(0)
+
+#define DDRCTRL_DBG1_DIS_HIF			BIT(1)
+
+#define DDRCTRL_DBGCAM_WR_DATA_PIPELINE_EMPTY	BIT(29)
+#define DDRCTRL_DBGCAM_RD_DATA_PIPELINE_EMPTY	BIT(28)
+#define DDRCTRL_DBGCAM_DBG_WR_Q_EMPTY		BIT(26)
+#define DDRCTRL_DBGCAM_DBG_LPR_Q_DEPTH		GENMASK_32(12, 8)
+#define DDRCTRL_DBGCAM_DBG_HPR_Q_DEPTH		GENMASK_32(4, 0)
+
+#define DDRCTRL_DBGCAM_DATA_PIPELINE_EMPTY \
+		(DDRCTRL_DBGCAM_WR_DATA_PIPELINE_EMPTY | \
+		 DDRCTRL_DBGCAM_RD_DATA_PIPELINE_EMPTY)
+
+#define DDRCTRL_DBGCAM_DBG_Q_DEPTH \
+		(DDRCTRL_DBGCAM_DBG_WR_Q_EMPTY | \
+		 DDRCTRL_DBGCAM_DBG_LPR_Q_DEPTH | \
+		 DDRCTRL_DBGCAM_DBG_HPR_Q_DEPTH)
+
+#define DDRCTRL_DBGCMD_RANK0_REFRESH		BIT(0)
+
+#define DDRCTRL_DBGSTAT_RANK0_REFRESH_BUSY	BIT(0)
+
+#define DDRCTRL_SWCTL_SW_DONE			BIT(0)
+
+#define DDRCTRL_SWSTAT_SW_DONE_ACK		BIT(0)
+
+#define DDRCTRL_PCTRL_N_PORT_EN			BIT(0)
+
+/* DDR PHY registers offsets */
+#define DDRPHYC_PIR				0x004
+#define DDRPHYC_PGCR				0x008
+#define DDRPHYC_PGSR				0x00C
+#define DDRPHYC_DLLGCR				0x010
+#define DDRPHYC_ACDLLCR				0x014
+#define DDRPHYC_PTR0				0x018
+#define DDRPHYC_ACIOCR				0x024
+#define DDRPHYC_DXCCR				0x028
+#define DDRPHYC_DSGCR				0x02C
+#define DDRPHYC_ZQ0CR0				0x180
+#define DDRPHYC_DX0GCR				0x1C0
+#define DDRPHYC_DX0DLLCR			0x1CC
+#define DDRPHYC_DX1GCR				0x200
+#define DDRPHYC_DX1DLLCR			0x20C
+#define DDRPHYC_DX2GCR				0x240
+#define DDRPHYC_DX2DLLCR			0x24C
+#define DDRPHYC_DX3GCR				0x280
+#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_ZCAL			BIT(3)
+#define DDRPHYC_PIR_ITMSRST			BIT(4)
+#define DDRPHYC_PIR_DRAMRST			BIT(5)
+#define DDRPHYC_PIR_DRAMINIT			BIT(6)
+#define DDRPHYC_PIR_QSTRN			BIT(7)
+#define DDRPHYC_PIR_ICPC			BIT(16)
+#define DDRPHYC_PIR_ZCALBYP			BIT(30)
+#define DDRPHYC_PIR_INITSTEPS_MASK		GENMASK(31, 7)
+
+#define DDRPHYC_PGCR_DFTCMP			BIT(2)
+#define DDRPHYC_PGCR_PDDISDX			BIT(24)
+#define DDRPHYC_PGCR_RFSHDT_MASK		GENMASK_32(28, 25)
+
+#define DDRPHYC_PGSR_IDONE			BIT(0)
+#define DDRPHYC_PGSR_DTERR			BIT(5)
+#define DDRPHYC_PGSR_DTIERR			BIT(6)
+#define DDRPHYC_PGSR_DFTERR			BIT(7)
+#define DDRPHYC_PGSR_RVERR			BIT(8)
+#define DDRPHYC_PGSR_RVEIRR			BIT(9)
+
+#define DDRPHYC_DLLGCR_BPS200			BIT(23)
+
+#define DDRPHYC_ACDLLCR_DLLSRST			BIT(30)
+#define DDRPHYC_ACDLLCR_DLLDIS			BIT(31)
+
+#define DDRPHYC_PTR0_TDLLSRST_OFFSET		0
+#define DDRPHYC_PTR0_TDLLSRST_MASK		GENMASK_32(5, 0)
+#define DDRPHYC_PTR0_TDLLLOCK_OFFSET		6
+#define DDRPHYC_PTR0_TDLLLOCK_MASK		GENMASK_32(17, 6)
+#define DDRPHYC_PTR0_TITMSRST_OFFSET		18
+#define DDRPHYC_PTR0_TITMSRST_MASK		GENMASK_32(21, 18)
+
+#define DDRPHYC_ACIOCR_ACPDD			BIT(3)
+#define DDRPHYC_ACIOCR_ACPDR			BIT(4)
+#define DDRPHYC_ACIOCR_CKPDD_MASK		GENMASK_32(10, 8)
+#define DDRPHYC_ACIOCR_CKPDD_0			BIT(8)
+#define DDRPHYC_ACIOCR_CKPDR_MASK		GENMASK_32(13, 11)
+#define DDRPHYC_ACIOCR_CKPDR_0			BIT(11)
+#define DDRPHYC_ACIOCR_CSPDD_MASK		GENMASK_32(21, 18)
+#define DDRPHYC_ACIOCR_CSPDD_0			BIT(18)
+#define DDRPHYC_ACIOCR_RSTPDD			BIT(27)
+#define DDRPHYC_ACIOCR_RSTPDR			BIT(28)
+
+#define DDRPHYC_DXCCR_DXPDD			BIT(2)
+#define DDRPHYC_DXCCR_DXPDR			BIT(3)
+
+#define DDRPHYC_DSGCR_CKEPDD_MASK		GENMASK_32(19, 16)
+#define DDRPHYC_DSGCR_CKEPDD_0			BIT(16)
+#define DDRPHYC_DSGCR_ODTPDD_MASK		GENMASK_32(23, 20)
+#define DDRPHYC_DSGCR_ODTPDD_0			BIT(20)
+#define DDRPHYC_DSGCR_NL2PD			BIT(24)
+
+#define DDRPHYC_ZQ0CRN_ZDATA_MASK		GENMASK_32(27, 0)
+#define DDRPHYC_ZQ0CRN_ZDATA_SHIFT		0
+#define DDRPHYC_ZQ0CRN_ZDEN			BIT(28)
+#define DDRPHYC_ZQ0CRN_ZQPD			BIT(31)
+
+#define DDRPHYC_DXNGCR_DXEN			BIT(0)
+
+#define DDRPHYC_DXNDLLCR_DLLSRST		BIT(30)
+#define DDRPHYC_DXNDLLCR_DLLDIS			BIT(31)
+#define DDRPHYC_DXNDLLCR_SDPHASE_MASK		GENMASK(17, 14)
+#define DDRPHYC_DXNDLLCR_SDPHASE_SHIFT		14
+
+uint32_t get_ddrphy_calibration(void);
+
+int ddr_sw_self_refresh_exit(void);
+int ddr_standby_sr_entry(uint32_t *zq0cr0_zdata);
+void ddr_sr_mode_ssr(void);
+void ddr_sr_mode_asr(void);
+void ddr_sr_mode_hsr(void);
+
+#endif /*__STM32MP1_DDRC_H__*/
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c
new file mode 100644
index 0000000..ed38575
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c
@@ -0,0 +1,614 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <kernel/delay.h>
+#include <drivers/stm32_i2c.h>
+#include <drivers/stm32mp1_clk.h>
+#include <drivers/stm32mp1_pmic.h>
+#include <drivers/stpmic1.h>
+#include <io.h>
+#include <keep.h>
+#include <kernel/dt.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <libfdt.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <stdbool.h>
+#include <stm32_util.h>
+#include <stm32mp_dt.h>
+#include <stm32mp_pm.h>
+#include <trace.h>
+#include <util.h>
+
+#define STPMIC1_LDO12356_OUTPUT_MASK	(uint8_t)(GENMASK_32(6, 2))
+#define STPMIC1_LDO12356_OUTPUT_SHIFT	2
+#define STPMIC1_LDO3_MODE		(uint8_t)(BIT(7))
+#define STPMIC1_LDO3_DDR_SEL		31U
+#define STPMIC1_LDO3_1800000		(9U << STPMIC1_LDO12356_OUTPUT_SHIFT)
+
+#define STPMIC1_BUCK_OUTPUT_SHIFT	2
+#define STPMIC1_BUCK3_1V8		(39U << STPMIC1_BUCK_OUTPUT_SHIFT)
+
+#define MODE_STANDBY                    8U
+
+#define STPMIC1_DEFAULT_START_UP_DELAY_MS	1
+
+static struct i2c_handle_s i2c_handle;
+static uint32_t pmic_i2c_addr;
+
+bool stm32mp_with_pmic(void)
+{
+	return (i2c_handle.dt_status & DT_STATUS_OK_SEC) != 0;
+}
+
+static int dt_get_pmic_node(void *fdt)
+{
+	return fdt_node_offset_by_compatible(fdt, -1, "st,stpmic1");
+}
+
+static int dt_pmic_status(void)
+{
+	void *fdt = get_dt_blob();
+
+	if (fdt) {
+		int node = dt_get_pmic_node(fdt);
+
+		if (node > 0) {
+			return _fdt_get_status(fdt, node);
+		}
+	}
+
+	return -1;
+}
+
+static bool dt_pmic_is_secure(void)
+{
+	int status = dt_pmic_status();
+
+	return ((unsigned)status == DT_STATUS_OK_SEC) &&
+	       (i2c_handle.dt_status == DT_STATUS_OK_SEC);
+}
+
+/*
+ * @idx: Private identifier provided by the target PMIC driver
+ * @flags: Operations expected when entering a low power sequence
+ * @voltage: Target voltage to apply during low power sequences
+ */
+struct regu_bo_config {
+	uint8_t flags;
+	struct stpmic1_bo_cfg cfg;
+};
+
+#define REGU_BO_FLAG_ENABLE_REGU		BIT(0)
+#define REGU_BO_FLAG_SET_VOLTAGE		BIT(1)
+#define REGU_BO_FLAG_PULL_DOWN			BIT(2)
+#define REGU_BO_FLAG_MASK_RESET			BIT(3)
+
+static struct regu_bo_config *regu_bo_config;
+static size_t regu_bo_count;
+
+static int save_boot_on_config(void)
+{
+	int pmic_node, regulators_node, regulator_node;
+	void *fdt;
+
+	assert(!regu_bo_config && !regu_bo_count);
+
+	fdt = get_dt_blob();
+	if (fdt == NULL) {
+		panic();
+	}
+
+	pmic_node = dt_get_pmic_node(fdt);
+	if (pmic_node < 0) {
+		panic();
+	}
+
+	regulators_node = fdt_subnode_offset(fdt, pmic_node, "regulators");
+
+	fdt_for_each_subnode(regulator_node, fdt, regulators_node) {
+		const fdt32_t *cuint;
+		const char *name;
+		struct regu_bo_config regu_cfg;
+		uint16_t mv;
+
+		if (fdt_getprop(fdt, regulator_node, "regulator-boot-on",
+				NULL) == NULL) {
+			continue;
+		}
+
+		memset(&regu_cfg, 0, sizeof(regu_cfg));
+		name = fdt_get_name(fdt, regulator_node, NULL);
+
+		regu_cfg.flags |= REGU_BO_FLAG_ENABLE_REGU;
+
+		if (fdt_getprop(fdt, regulator_node, "regulator-pull-down",
+				NULL) != NULL) {
+			stpmic1_bo_pull_down_cfg(name, &regu_cfg.cfg);
+			regu_cfg.flags |= REGU_BO_FLAG_PULL_DOWN;
+		}
+
+		if (fdt_getprop(fdt, regulator_node, "st,mask-reset",
+				NULL) != NULL) {
+			stpmic1_bo_mask_reset_cfg(name, &regu_cfg.cfg);
+			regu_cfg.flags |= REGU_BO_FLAG_MASK_RESET;
+		}
+
+		cuint = fdt_getprop(fdt, regulator_node,
+				    "regulator-min-microvolt", NULL);
+		if (cuint != NULL) {
+			/* DT uses microvolts, whereas driver awaits millivolts */
+			mv = (uint16_t)(fdt32_to_cpu(*cuint) / 1000U);
+
+			if (!stpmic1_bo_voltage_cfg(name, mv, &regu_cfg.cfg)) {
+				regu_cfg.flags |= REGU_BO_FLAG_SET_VOLTAGE;
+			}
+		}
+
+		/* Save config in the Boot On configuration list */
+		regu_bo_count++;
+		regu_bo_config = realloc(regu_bo_config,
+					 regu_bo_count * sizeof(regu_cfg));
+		if (regu_bo_config == NULL) {
+			panic();
+		}
+
+		memcpy(&regu_bo_config[regu_bo_count - 1], &regu_cfg,
+			sizeof(regu_cfg));
+	}
+
+	return 0;
+}
+
+void stm32mp_pmic_apply_boot_on_config(void)
+{
+	size_t i;
+
+	for (i = 0; i < regu_bo_count; i++) {
+		struct regu_bo_config *regu_cfg = &regu_bo_config[i];
+
+		if (regu_cfg->flags & REGU_BO_FLAG_SET_VOLTAGE) {
+			if (stpmic1_bo_voltage_unpg(&regu_cfg->cfg)) {
+				panic();
+			}
+		}
+
+		if (regu_cfg->flags & REGU_BO_FLAG_ENABLE_REGU) {
+			if (stpmic1_bo_enable_unpg(&regu_cfg->cfg)) {
+				panic();
+			}
+		}
+
+		if (regu_cfg->flags & REGU_BO_FLAG_PULL_DOWN) {
+			if (stpmic1_bo_pull_down_unpg(&regu_cfg->cfg)) {
+				panic();
+			}
+		}
+
+		if (regu_cfg->flags & REGU_BO_FLAG_MASK_RESET) {
+			if (stpmic1_bo_mask_reset_unpg(&regu_cfg->cfg)) {
+				panic();
+			}
+		}
+	}
+}
+
+/*
+ * @idx: Private identifier provided by the target PMIC driver
+ * @flags: Operations expected when entering a low power sequence
+ * @voltage: Target voltage to apply during low power sequences
+ */
+struct regu_lp_config {
+	uint8_t flags;
+	struct stpmic1_lp_cfg cfg;
+};
+
+#define REGU_LP_FLAG_LOAD_PWRCTRL	BIT(0)
+#define REGU_LP_FLAG_ON_IN_SUSPEND	BIT(1)
+#define REGU_LP_FLAG_OFF_IN_SUSPEND	BIT(2)
+#define REGU_LP_FLAG_SET_VOLTAGE	BIT(3)
+#define REGU_LP_FLAG_MODE_STANDBY	BIT(4)
+
+struct regu_lp_state {
+	const char *name;
+	size_t cfg_count;
+	struct regu_lp_config *cfg;
+};
+
+#define REGU_LP_STATE_DISK		0
+#define REGU_LP_STATE_STANDBY		1
+#define REGU_LP_STATE_MEM		2
+#define REGU_LP_STATE_COUNT		3
+
+static struct regu_lp_state regu_lp_state[REGU_LP_STATE_COUNT] = {
+	[REGU_LP_STATE_DISK] = { .name = "standby-ddr-off", },
+	[REGU_LP_STATE_STANDBY] = { .name = "standby-ddr-sr", },
+	[REGU_LP_STATE_MEM] = { .name = "lp-stop", },
+};
+
+static unsigned int regu_lp_state2idx(const char *name)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(regu_lp_state); i++) {
+		struct regu_lp_state *state = &regu_lp_state[i];
+
+		if (!strncmp(name, state->name, strlen(state->name))) {
+			return i;
+		}
+	}
+
+	panic();
+}
+
+static int save_low_power_config(const char *lp_state)
+{
+	int pmic_node, regulators_node, regulator_node;
+	void *fdt;
+	unsigned int state_idx = regu_lp_state2idx(lp_state);
+	struct regu_lp_state *state = &regu_lp_state[state_idx];
+
+	assert(!state->cfg && !state->cfg_count);
+
+	fdt = get_dt_blob();
+	if (fdt == NULL) {
+		panic();
+	}
+
+	pmic_node = dt_get_pmic_node(fdt);
+	if (pmic_node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	regulators_node = fdt_subnode_offset(fdt, pmic_node, "regulators");
+
+	fdt_for_each_subnode(regulator_node, fdt, regulators_node) {
+		const fdt32_t *cuint;
+		const char *reg_name;
+		int regulator_state_node;
+		struct regu_lp_config *regu_cfg;
+
+		state->cfg_count++;
+		state->cfg = realloc(state->cfg,
+				     state->cfg_count * sizeof(*state->cfg));
+		if (state->cfg == NULL) {
+			panic();
+		}
+
+		regu_cfg = &state->cfg[state->cfg_count - 1];
+
+		memset(regu_cfg, 0, sizeof(*regu_cfg));
+
+		reg_name = fdt_get_name(fdt, regulator_node, NULL);
+
+		if (stpmic1_lp_cfg(reg_name, &regu_cfg->cfg) != 0) {
+			EMSG("Invalid regu name %s", reg_name);
+			return -1;
+		}
+
+		/*
+		 * Always copy active configuration (Control register) to
+		 * PWRCTRL Control register, even if regulator_state_node
+		 * does not exist.
+		 */
+		regu_cfg->flags |= REGU_LP_FLAG_LOAD_PWRCTRL;
+
+		/* Then apply configs from regulator_state_node */
+		regulator_state_node = fdt_subnode_offset(fdt,
+							  regulator_node,
+							  lp_state);
+		if (regulator_state_node <= 0) {
+			continue;
+		}
+
+		if (fdt_getprop(fdt, regulator_state_node,
+				"regulator-on-in-suspend", NULL) != NULL) {
+			regu_cfg->flags |= REGU_LP_FLAG_ON_IN_SUSPEND;
+		}
+
+		if (fdt_getprop(fdt, regulator_state_node,
+				"regulator-off-in-suspend", NULL) != NULL) {
+			regu_cfg->flags |= REGU_LP_FLAG_OFF_IN_SUSPEND;
+		}
+
+		cuint = fdt_getprop(fdt, regulator_state_node,
+				    "regulator-suspend-microvolt", NULL);
+		if (cuint != NULL) {
+			uint32_t mv = fdt32_to_cpu(*cuint) / 1000U;
+
+			if (stpmic1_lp_voltage_cfg(reg_name, mv,
+						    &regu_cfg->cfg) == 0) {
+				regu_cfg->flags |= REGU_LP_FLAG_SET_VOLTAGE;
+			}
+		}
+
+		cuint = fdt_getprop(fdt, regulator_state_node,
+				    "regulator-mode", NULL);
+		if (cuint != NULL) {
+			if (fdt32_to_cpu(*cuint) == MODE_STANDBY) {
+				regu_cfg->flags |= REGU_LP_FLAG_MODE_STANDBY;
+			}
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * int stm32mp_pmic_set_lp_config(char *lp_state)
+ *
+ * Load the low power configuration stored in regu_lp_state[].
+ */
+void stm32mp_pmic_apply_lp_config(const char *lp_state)
+{
+	unsigned int state_idx = regu_lp_state2idx(lp_state);
+	struct regu_lp_state *state = &regu_lp_state[state_idx];
+	size_t i;
+
+	if (stpmic1_powerctrl_on() != 0) {
+		panic();
+	}
+
+	for (i = 0; i < state->cfg_count; i++) {
+		struct stpmic1_lp_cfg *cfg = &state->cfg[i].cfg;
+
+		if ((state->cfg[i].flags & REGU_LP_FLAG_LOAD_PWRCTRL) != 0) {
+			if (stpmic1_lp_load_unpg(cfg) != 0) {
+				panic();
+			}
+		}
+
+		if ((state->cfg[i].flags & REGU_LP_FLAG_ON_IN_SUSPEND) != 0) {
+			if (stpmic1_lp_on_off_unpg(cfg, 1) != 0) {
+				panic();
+			}
+		}
+
+		if ((state->cfg[i].flags & REGU_LP_FLAG_OFF_IN_SUSPEND) != 0) {
+			if (stpmic1_lp_on_off_unpg(cfg, 0) != 0) {
+				panic();
+			}
+		}
+
+		if ((state->cfg[i].flags & REGU_LP_FLAG_SET_VOLTAGE) != 0) {
+			if (stpmic1_lp_voltage_unpg(cfg) != 0) {
+				panic();
+			}
+		}
+
+		if ((state->cfg[i].flags & REGU_LP_FLAG_MODE_STANDBY) != 0) {
+			if (stpmic1_lp_mode_unpg(cfg, 1) != 0) {
+				panic();
+			}
+		}
+	}
+}
+
+static int save_power_configurations(void)
+{
+	unsigned int i;
+
+	if (save_boot_on_config() != 0) {
+		return -1;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(regu_lp_state); i++) {
+		if (save_low_power_config(regu_lp_state[i].name) != 0) {
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * Get PMIC and its I2C bus configuration from the device tree.
+ * Return 0 on success, negative on error, 1 if no PMIC node is defined.
+ */
+static int dt_pmic_i2c_config(struct dt_node_info *i2c_info,
+			      struct stm32_pinctrl **pinctrl,
+			      size_t *pinctrl_count,
+			      struct stm32_i2c_init_s *init)
+{
+	int pmic_node;
+	int i2c_node;
+	void *fdt;
+	const fdt32_t *cuint;
+
+	fdt = get_dt_blob();
+	if (!fdt) {
+		return -1;
+	}
+
+	pmic_node = dt_get_pmic_node(fdt);
+	if (pmic_node < 0) {
+		return 1;
+	}
+
+	cuint = fdt_getprop(fdt, pmic_node, "reg", NULL);
+	if (cuint == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	pmic_i2c_addr = fdt32_to_cpu(*cuint) << 1;
+	if (pmic_i2c_addr > UINT16_MAX) {
+		return -1;
+	}
+
+	i2c_node = fdt_parent_offset(fdt, pmic_node);
+	if (i2c_node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	fdt_fill_device_info(fdt, i2c_info, i2c_node);
+	if (i2c_info->base == 0U) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return stm32_i2c_get_setup_from_fdt(fdt, i2c_node, init,
+					    pinctrl, pinctrl_count);
+}
+
+/*
+ * PMIC and resource initialization
+ */
+
+/* Return true if PMIC is available, false if not found, panics on errors */
+static bool initialize_pmic_i2c(void)
+{
+	int ret;
+	struct dt_node_info i2c_info;
+	struct i2c_handle_s *i2c = &i2c_handle;
+	struct stm32_pinctrl *pinctrl;
+	size_t pinctrl_count;
+	struct stm32_i2c_init_s i2c_init;
+
+	ret = dt_pmic_i2c_config(&i2c_info, &pinctrl, &pinctrl_count,
+				 &i2c_init);
+	if (ret < 0) {
+		EMSG("I2C configuration failed %d\n", ret);
+		panic();
+	}
+	if (ret != 0) {
+		return false;
+	}
+
+	/* Initialize PMIC I2C */
+	i2c->pbase = i2c_info.base;
+	i2c->vbase = (uintptr_t)phys_to_virt(i2c_info.base, MEM_AREA_IO_SEC);
+	assert(i2c->vbase);
+	i2c->dt_status = i2c_info.status;
+	i2c->clock = i2c_info.clock;
+	i2c_init.own_address1 = pmic_i2c_addr;
+	i2c_init.addressing_mode = I2C_ADDRESSINGMODE_7BIT;
+	i2c_init.dual_address_mode = I2C_DUALADDRESS_DISABLE;
+	i2c_init.own_address2 = 0;
+	i2c_init.own_address2_masks = I2C_OAR2_OA2NOMASK;
+	i2c_init.general_call_mode = I2C_GENERALCALL_DISABLE;
+	i2c_init.no_stretch_mode = I2C_NOSTRETCH_DISABLE;
+	i2c_init.analog_filter = 1;
+	i2c_init.digital_filter_coef = 0;
+
+	i2c->pinctrl = pinctrl;
+	i2c->pinctrl_count = pinctrl_count;
+
+	stm32mp_get_pmic();
+
+	ret = stm32_i2c_init(i2c, &i2c_init);
+	if (ret != 0) {
+		EMSG("Cannot initialize I2C %x (%d)\n", i2c->pbase, ret);
+		panic();
+	}
+
+	if (!stm32_i2c_is_device_ready(i2c, pmic_i2c_addr, 1,
+				       I2C_TIMEOUT_BUSY_MS)) {
+		EMSG("I2C device not ready\n");
+		panic();
+	}
+
+	stpmic1_bind_i2c(i2c, (uint16_t)pmic_i2c_addr);
+
+	stm32mp_put_pmic();
+
+	return true;
+}
+
+/*
+ * Automated suspend/resume at system suspend/resume is expected
+ * only when the PMIC is secure. If it is non secure, only atomic
+ * execution context acn get/put the PMIC resources.
+ */
+static void pmic_suspend_resume(enum pm_op op, void __unused *handle)
+{
+	switch (op) {
+	case PM_OP_SUSPEND:
+		stm32_i2c_suspend(&i2c_handle);
+		break;
+	case PM_OP_RESUME:
+		stm32_i2c_resume(&i2c_handle);
+		break;
+	default:
+		panic();
+	}
+}
+KEEP_PAGER(pmic_suspend_resume);
+
+/* stm32mp_get/put_pmic allows secure atomic sequences to use non secure PMIC */
+void stm32mp_get_pmic(void)
+{
+	stm32_i2c_resume(&i2c_handle);
+}
+
+void stm32mp_put_pmic(void)
+{
+	stm32_i2c_suspend(&i2c_handle);
+}
+
+static void register_non_secure_pmic(void)
+{
+	size_t n;
+
+	if (!i2c_handle.pbase) {
+		return;
+	}
+
+	stm32mp_register_non_secure_periph_iomem(i2c_handle.pbase);
+	stm32mp_register_clock_parents_secure(i2c_handle.clock);
+
+	for (n = 0; n < i2c_handle.pinctrl_count; n++) {
+		stm32mp_register_non_secure_gpio(i2c_handle.pinctrl[n].bank,
+						 i2c_handle.pinctrl[n].pin);
+	}
+}
+
+static void register_secure_pmic(void)
+{
+	size_t n;
+
+	stm32mp_register_pm_cb(pmic_suspend_resume, NULL);
+	stm32mp_register_secure_periph_iomem(i2c_handle.pbase);
+
+	for (n = 0; n < i2c_handle.pinctrl_count; n++) {
+		stm32mp_register_secure_gpio(i2c_handle.pinctrl[n].bank,
+					     i2c_handle.pinctrl[n].pin);
+	}
+}
+
+static TEE_Result initialize_pmic(void)
+{
+	unsigned long pmic_version;
+
+	if (!initialize_pmic_i2c()) {
+		DMSG("No PMIC");
+		register_non_secure_pmic();
+		return TEE_SUCCESS;
+	}
+
+	stm32mp_get_pmic();
+
+	if (stpmic1_get_version(&pmic_version)) {
+		panic("Failed to access PMIC");
+	}
+
+	DMSG("PMIC version = 0x%02lx", pmic_version);
+	stpmic1_dump_regulators();
+
+	if (save_power_configurations() != 0) {
+		panic();
+	}
+
+	if (dt_pmic_is_secure()) {
+		register_secure_pmic();
+	} else {
+		register_non_secure_pmic();
+	}
+
+	stm32mp_put_pmic();
+
+	return TEE_SUCCESS;
+}
+driver_init(initialize_pmic);
+
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h
new file mode 100644
index 0000000..f50943d
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef __STM32MP1_PMIC_H__
+#define __STM32MP1_PMIC_H__
+
+#include <kernel/panic.h>
+
+#ifdef CFG_STPMIC1
+void stm32mp_pmic_apply_boot_on_config(void);
+void stm32mp_pmic_apply_lp_config(const char *lp_state);
+void stm32mp_get_pmic(void);
+void stm32mp_put_pmic(void);
+#else
+void stm32mp_pmic_apply_boot_on_config(void)
+{
+}
+void stm32mp_pmic_apply_lp_config(const char *lp_state)
+{
+}
+void stm32mp_get_pmic(void)
+{
+	panic();
+}
+void stm32mp_put_pmic(void)
+{
+	panic();
+}
+#endif
+
+#endif /*__STM32MP1_PMIC_H__*/
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.c
new file mode 100644
index 0000000..ce60f50
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.c
@@ -0,0 +1,21 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics
+ */
+
+#include <drivers/stm32mp1_pwr.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+
+uintptr_t stm32_pwr_base(void)
+{
+	static void *va;
+
+	if (!cpu_mmu_enabled())
+		return PWR_BASE;
+
+	if (!va)
+		va = phys_to_virt(PWR_BASE, MEM_AREA_IO_SEC);
+
+	return (uintptr_t)va;
+}
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h
new file mode 100644
index 0000000..1d44e8b
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h
@@ -0,0 +1,48 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics
+ */
+
+#ifndef __STM32MP1_PWR_H__
+#define __STM32MP1_PWR_H__
+
+#include <util.h>
+
+#define PWR_CR1_OFF		0x00
+#define PWR_CR2_OFF		0x08
+#define PWR_CR3_OFF		0x0c
+#define PWR_MPUCR_OFF		0x10
+#define PWR_WKUPCR_OFF		0x20
+#define PWR_MPUWKUPENR_OFF	0x28
+
+#define PWR_OFFSET_MASK		GENMASK_32(5, 0)
+
+#define PWR_CR1_LPDS		BIT(0)
+#define PWR_CR1_LPCFG		BIT(1)
+#define PWR_CR1_LVDS		BIT(2)
+#define PWR_CR1_DBP		BIT(8)
+
+#define PWR_CR2_BREN		BIT(0)
+#define PWR_CR2_RREN		BIT(1)
+
+#define PWR_CR3_VBE		BIT(8)
+#define PWR_CR3_VBRS		BIT(9)
+#define PWR_CR3_DDRSREN		BIT(10)
+#define PWR_CR3_DDRSRDIS	BIT(11)
+#define PWR_CR3_DDRRETEN	BIT(12)
+#define PWR_CR3_USB33DEN	BIT(24)
+#define PWR_CR3_REG18EN		BIT(28)
+#define PWR_CR3_REG11EN		BIT(30)
+
+#define PWR_MPUCR_PDDS		BIT(0)
+#define PWR_MPUCR_CSTDBYDIS	BIT(3)
+#define PWR_MPUCR_CSSF		BIT(9)
+
+#define PWR_WKUPCR_MASK		(GENMASK_32(27, 16) | \
+				 GENMASK_32(13, 8) | GENMASK_32(5, 0))
+
+#define PWR_MPUWKUPENR_MASK	GENMASK_32(5, 0)
+
+uintptr_t stm32_pwr_base(void);
+
+#endif /*__STM32MP1_PWR_H__*/
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.c
new file mode 100644
index 0000000..a9df116
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.c
@@ -0,0 +1,51 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics
+ */
+
+#include <io.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <drivers/stm32mp1_rcc.h>
+#include <stm32_util.h>
+
+uintptr_t stm32_rcc_base(void)
+{
+	static void *va;
+
+	if (!cpu_mmu_enabled()) {
+		return RCC_BASE;
+	}
+
+	if (!va) {
+		va = phys_to_virt(RCC_BASE, MEM_AREA_IO_SEC);
+	}
+
+	return (uintptr_t)va;
+}
+
+void stm32_rcc_secure(int enable)
+{
+	uintptr_t base = stm32_rcc_base();
+
+	if (enable != 0) {
+		io_mask32_stm32shregs(base + RCC_TZCR, RCC_TZCR_TZEN,
+				      RCC_TZCR_TZEN);
+	} else {
+		io_mask32_stm32shregs(base + RCC_TZCR, 0, RCC_TZCR_TZEN);
+	}
+}
+
+bool stm32_rcc_is_secure(void)
+{
+	uintptr_t base = stm32_rcc_base();
+
+	return read32(base + RCC_TZCR) & RCC_TZCR_TZEN;
+}
+
+bool stm32_rcc_is_mckprot(void)
+{
+	uintptr_t base = stm32_rcc_base();
+
+	return read32(base + RCC_TZCR) & RCC_TZCR_MCKPROT;
+}
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.h b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.h
new file mode 100644
index 0000000..6a7a665
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.h
@@ -0,0 +1,539 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics
+ */
+
+#ifndef __STM32MP1_RCC_H__
+#define __STM32MP1_RCC_H__
+
+#include <stdbool.h>
+#include <util.h>
+
+#define RCC_TZCR			0x00
+#define RCC_OCENSETR			0x0C
+#define RCC_OCENCLRR			0x10
+#define RCC_HSICFGR			0x18
+#define RCC_CSICFGR			0x1C
+#define RCC_MPCKSELR			0x20
+#define RCC_ASSCKSELR			0x24
+#define RCC_RCK12SELR			0x28
+#define RCC_MPCKDIVR			0x2C
+#define RCC_AXIDIVR			0x30
+#define RCC_APB4DIVR			0x3C
+#define RCC_APB5DIVR			0x40
+#define RCC_RTCDIVR			0x44
+#define RCC_MSSCKSELR			0x48
+#define RCC_PLL1CR			0x80
+#define RCC_PLL1CFGR1			0x84
+#define RCC_PLL1CFGR2			0x88
+#define RCC_PLL1FRACR			0x8C
+#define RCC_PLL1CSGR			0x90
+#define RCC_PLL2CR			0x94
+#define RCC_PLL2CFGR1			0x98
+#define RCC_PLL2CFGR2			0x9C
+#define RCC_PLL2FRACR			0xA0
+#define RCC_PLL2CSGR			0xA4
+#define RCC_I2C46CKSELR			0xC0
+#define RCC_SPI6CKSELR			0xC4
+#define RCC_UART1CKSELR			0xC8
+#define RCC_RNG1CKSELR			0xCC
+#define RCC_CPERCKSELR			0xD0
+#define RCC_STGENCKSELR			0xD4
+#define RCC_DDRITFCR			0xD8
+#define RCC_MP_BOOTCR			0x100
+#define RCC_MP_SREQSETR			0x104
+#define RCC_MP_SREQCLRR			0x108
+#define RCC_MP_GCR			0x10C
+#define RCC_MP_APRSTCR			0x110
+#define RCC_MP_APRSTSR			0x114
+#define RCC_BDCR			0x140
+#define RCC_RDLSICR			0x144
+#define RCC_APB4RSTSETR			0x180
+#define RCC_APB4RSTCLRR			0x184
+#define RCC_APB5RSTSETR			0x188
+#define RCC_APB5RSTCLRR			0x18C
+#define RCC_AHB5RSTSETR			0x190
+#define RCC_AHB5RSTCLRR			0x194
+#define RCC_AHB6RSTSETR			0x198
+#define RCC_AHB6RSTCLRR			0x19C
+#define RCC_TZAHB6RSTSETR		0x1A0
+#define RCC_TZAHB6RSTCLRR		0x1A4
+#define RCC_MP_APB4ENSETR		0x200
+#define RCC_MP_APB4ENCLRR		0x204
+#define RCC_MP_APB5ENSETR		0x208
+#define RCC_MP_APB5ENCLRR		0x20C
+#define RCC_MP_AHB5ENSETR		0x210
+#define RCC_MP_AHB5ENCLRR		0x214
+#define RCC_MP_AHB6ENSETR		0x218
+#define RCC_MP_AHB6ENCLRR		0x21C
+#define RCC_MP_TZAHB6ENSETR		0x220
+#define RCC_MP_TZAHB6ENCLRR		0x224
+#define RCC_MC_APB4ENSETR		0x280
+#define RCC_MC_APB4ENCLRR		0x284
+#define RCC_MC_APB5ENSETR		0x288
+#define RCC_MC_APB5ENCLRR		0x28C
+#define RCC_MC_AHB5ENSETR		0x290
+#define RCC_MC_AHB5ENCLRR		0x294
+#define RCC_MC_AHB6ENSETR		0x298
+#define RCC_MC_AHB6ENCLRR		0x29C
+#define RCC_MP_APB4LPENSETR		0x300
+#define RCC_MP_APB4LPENCLRR		0x304
+#define RCC_MP_APB5LPENSETR		0x308
+#define RCC_MP_APB5LPENCLRR		0x30C
+#define RCC_MP_AHB5LPENSETR		0x310
+#define RCC_MP_AHB5LPENCLRR		0x314
+#define RCC_MP_AHB6LPENSETR		0x318
+#define RCC_MP_AHB6LPENCLRR		0x31C
+#define RCC_MP_TZAHB6LPENSETR		0x320
+#define RCC_MP_TZAHB6LPENCLRR		0x324
+#define RCC_MC_APB4LPENSETR		0x380
+#define RCC_MC_APB4LPENCLRR		0x384
+#define RCC_MC_APB5LPENSETR		0x388
+#define RCC_MC_APB5LPENCLRR		0x38C
+#define RCC_MC_AHB5LPENSETR		0x390
+#define RCC_MC_AHB5LPENCLRR		0x394
+#define RCC_MC_AHB6LPENSETR		0x398
+#define RCC_MC_AHB6LPENCLRR		0x39C
+#define RCC_BR_RSTSCLRR			0x400
+#define RCC_MP_GRSTCSETR		0x404
+#define RCC_MP_RSTSCLRR			0x408
+#define RCC_MP_IWDGFZSETR		0x40C
+#define RCC_MP_IWDGFZCLRR		0x410
+#define RCC_MP_CIER			0x414
+#define RCC_MP_CIFR			0x418
+#define RCC_PWRLPDLYCR			0x41C
+#define RCC_MP_RSTSSETR			0x420
+#define RCC_MCO1CFGR			0x800
+#define RCC_MCO2CFGR			0x804
+#define RCC_OCRDYR			0x808
+#define RCC_DBGCFGR			0x80C
+#define RCC_RCK3SELR			0x820
+#define RCC_RCK4SELR			0x824
+#define RCC_TIMG1PRER			0x828
+#define RCC_TIMG2PRER			0x82C
+#define RCC_MCUDIVR			0x830
+#define RCC_APB1DIVR			0x834
+#define RCC_APB2DIVR			0x838
+#define RCC_APB3DIVR			0x83C
+#define RCC_PLL3CR			0x880
+#define RCC_PLL3CFGR1			0x884
+#define RCC_PLL3CFGR2			0x888
+#define RCC_PLL3FRACR			0x88C
+#define RCC_PLL3CSGR			0x890
+#define RCC_PLL4CR			0x894
+#define RCC_PLL4CFGR1			0x898
+#define RCC_PLL4CFGR2			0x89C
+#define RCC_PLL4FRACR			0x8A0
+#define RCC_PLL4CSGR			0x8A4
+#define RCC_I2C12CKSELR			0x8C0
+#define RCC_I2C35CKSELR			0x8C4
+#define RCC_SAI1CKSELR			0x8C8
+#define RCC_SAI2CKSELR			0x8CC
+#define RCC_SAI3CKSELR			0x8D0
+#define RCC_SAI4CKSELR			0x8D4
+#define RCC_SPI2S1CKSELR		0x8D8
+#define RCC_SPI2S23CKSELR		0x8DC
+#define RCC_SPI45CKSELR			0x8E0
+#define RCC_UART6CKSELR			0x8E4
+#define RCC_UART24CKSELR		0x8E8
+#define RCC_UART35CKSELR		0x8EC
+#define RCC_UART78CKSELR		0x8F0
+#define RCC_SDMMC12CKSELR		0x8F4
+#define RCC_SDMMC3CKSELR		0x8F8
+#define RCC_ETHCKSELR			0x8FC
+#define RCC_QSPICKSELR			0x900
+#define RCC_FMCCKSELR			0x904
+#define RCC_FDCANCKSELR			0x90C
+#define RCC_SPDIFCKSELR			0x914
+#define RCC_CECCKSELR			0x918
+#define RCC_USBCKSELR			0x91C
+#define RCC_RNG2CKSELR			0x920
+#define RCC_DSICKSELR			0x924
+#define RCC_ADCCKSELR			0x928
+#define RCC_LPTIM45CKSELR		0x92C
+#define RCC_LPTIM23CKSELR		0x930
+#define RCC_LPTIM1CKSELR		0x934
+#define RCC_APB1RSTSETR			0x980
+#define RCC_APB1RSTCLRR			0x984
+#define RCC_APB2RSTSETR			0x988
+#define RCC_APB2RSTCLRR			0x98C
+#define RCC_APB3RSTSETR			0x990
+#define RCC_APB3RSTCLRR			0x994
+#define RCC_AHB2RSTSETR			0x998
+#define RCC_AHB2RSTCLRR			0x99C
+#define RCC_AHB3RSTSETR			0x9A0
+#define RCC_AHB3RSTCLRR			0x9A4
+#define RCC_AHB4RSTSETR			0x9A8
+#define RCC_AHB4RSTCLRR			0x9AC
+#define RCC_MP_APB1ENSETR		0xA00
+#define RCC_MP_APB1ENCLRR		0xA04
+#define RCC_MP_APB2ENSETR		0xA08
+#define RCC_MP_APB2ENCLRR		0xA0C
+#define RCC_MP_APB3ENSETR		0xA10
+#define RCC_MP_APB3ENCLRR		0xA14
+#define RCC_MP_AHB2ENSETR		0xA18
+#define RCC_MP_AHB2ENCLRR		0xA1C
+#define RCC_MP_AHB3ENSETR		0xA20
+#define RCC_MP_AHB3ENCLRR		0xA24
+#define RCC_MP_AHB4ENSETR		0xA28
+#define RCC_MP_AHB4ENCLRR		0xA2C
+#define RCC_MP_MLAHBENSETR		0xA38
+#define RCC_MP_MLAHBENCLRR		0xA3C
+#define RCC_MC_APB1ENSETR		0xA80
+#define RCC_MC_APB1ENCLRR		0xA84
+#define RCC_MC_APB2ENSETR		0xA88
+#define RCC_MC_APB2ENCLRR		0xA8C
+#define RCC_MC_APB3ENSETR		0xA90
+#define RCC_MC_APB3ENCLRR		0xA94
+#define RCC_MC_AHB2ENSETR		0xA98
+#define RCC_MC_AHB2ENCLRR		0xA9C
+#define RCC_MC_AHB3ENSETR		0xAA0
+#define RCC_MC_AHB3ENCLRR		0xAA4
+#define RCC_MC_AHB4ENSETR		0xAA8
+#define RCC_MC_AHB4ENCLRR		0xAAC
+#define RCC_MC_AXIMENSETR		0xAB0
+#define RCC_MC_AXIMENCLRR		0xAB4
+#define RCC_MC_MLAHBENSETR		0xAB8
+#define RCC_MC_MLAHBENCLRR		0xABC
+#define RCC_MP_APB1LPENSETR		0xB00
+#define RCC_MP_APB1LPENCLRR		0xB04
+#define RCC_MP_APB2LPENSETR		0xB08
+#define RCC_MP_APB2LPENCLRR		0xB0C
+#define RCC_MP_APB3LPENSETR		0xB10
+#define RCC_MP_APB3LPENCLRR		0xB14
+#define RCC_MP_AHB2LPENSETR		0xB18
+#define RCC_MP_AHB2LPENCLRR		0xB1C
+#define RCC_MP_AHB3LPENSETR		0xB20
+#define RCC_MP_AHB3LPENCLRR		0xB24
+#define RCC_MP_AHB4LPENSETR		0xB28
+#define RCC_MP_AHB4LPENCLRR		0xB2C
+#define RCC_MP_AXIMLPENSETR		0xB30
+#define RCC_MP_AXIMLPENCLRR		0xB34
+#define RCC_MP_MLAHBLPENSETR		0xB38
+#define RCC_MP_MLAHBLPENCLRR		0xB3C
+#define RCC_MC_APB1LPENSETR		0xB80
+#define RCC_MC_APB1LPENCLRR		0xB84
+#define RCC_MC_APB2LPENSETR		0xB88
+#define RCC_MC_APB2LPENCLRR		0xB8C
+#define RCC_MC_APB3LPENSETR		0xB90
+#define RCC_MC_APB3LPENCLRR		0xB94
+#define RCC_MC_AHB2LPENSETR		0xB98
+#define RCC_MC_AHB2LPENCLRR		0xB9C
+#define RCC_MC_AHB3LPENSETR		0xBA0
+#define RCC_MC_AHB3LPENCLRR		0xBA4
+#define RCC_MC_AHB4LPENSETR		0xBA8
+#define RCC_MC_AHB4LPENCLRR		0xBAC
+#define RCC_MC_AXIMLPENSETR		0xBB0
+#define RCC_MC_AXIMLPENCLRR		0xBB4
+#define RCC_MC_MLAHBLPENSETR		0xBB8
+#define RCC_MC_MLAHBLPENCLRR		0xBBC
+#define RCC_MC_RSTSCLRR			0xC00
+#define RCC_MC_CIER			0xC14
+#define RCC_MC_CIFR			0xC18
+#define RCC_VERR			0xFF4
+#define RCC_IDR				0xFF8
+#define RCC_SIDR			0xFFC
+
+#define RCC_OFFSET_MASK			GENMASK_32(11, 0)
+
+/* Values for RCC_TZCR register */
+#define RCC_TZCR_TZEN			BIT(0)
+#define RCC_TZCR_MCKPROT		BIT(1)
+
+/* Used for most of RCC_<x>SELR registers */
+#define RCC_SELR_SRC_MASK		GENMASK_32(2, 0)
+#define RCC_SELR_REFCLK_SRC_MASK	GENMASK_32(1, 0)
+#define RCC_SELR_SRCRDY			BIT(31)
+
+/* Values of RCC_MPCKSELR register */
+#define RCC_MPCKSELR_HSI		0x00000000
+#define RCC_MPCKSELR_HSE		0x00000001
+#define RCC_MPCKSELR_PLL		0x00000002
+#define RCC_MPCKSELR_PLL_MPUDIV		0x00000003
+
+/* Values of RCC_ASSCKSELR register */
+#define RCC_ASSCKSELR_HSI		0x00000000
+#define RCC_ASSCKSELR_HSE		0x00000001
+#define RCC_ASSCKSELR_PLL		0x00000002
+
+/* Values of RCC_MSSCKSELR register */
+#define RCC_MSSCKSELR_HSI		0x00000000
+#define RCC_MSSCKSELR_HSE		0x00000001
+#define RCC_MSSCKSELR_CSI		0x00000002
+#define RCC_MSSCKSELR_PLL		0x00000003
+
+/* Values of RCC_CPERCKSELR register */
+#define RCC_CPERCKSELR_HSI		0x00000000
+#define RCC_CPERCKSELR_CSI		0x00000001
+#define RCC_CPERCKSELR_HSE		0x00000002
+
+/* Used for most of DIVR register: max div for RTC */
+#define RCC_DIVR_DIV_MASK		GENMASK_32(5, 0)
+#define RCC_DIVR_DIVRDY			BIT(31)
+
+/* Masks for specific DIVR registers */
+#define RCC_APBXDIV_MASK		GENMASK_32(2, 0)
+#define RCC_MPUDIV_MASK			GENMASK_32(2, 0)
+#define RCC_AXIDIV_MASK			GENMASK_32(2, 0)
+#define RCC_MCUDIV_MASK			GENMASK_32(3, 0)
+
+/* Used for TIMER Prescaler */
+#define RCC_TIMGXPRER_TIMGXPRE		BIT(0)
+
+/* Offset between RCC_MP_xxxENSETR and RCC_MP_xxxENCLRR registers */
+#define RCC_MP_ENCLRR_OFFSET		4u
+
+/* Fields of RCC_BDCR register */
+#define RCC_BDCR_LSEON			BIT(0)
+#define RCC_BDCR_LSEBYP			BIT(1)
+#define RCC_BDCR_LSERDY			BIT(2)
+#define RCC_BDCR_DIGBYP			BIT(3)
+#define RCC_BDCR_LSEDRV_MASK		GENMASK_32(5, 4)
+#define RCC_BDCR_LSEDRV_SHIFT		4
+#define RCC_BDCR_LSECSSON		BIT(8)
+#define RCC_BDCR_RTCCKEN		BIT(20)
+#define RCC_BDCR_RTCSRC_MASK		GENMASK_32(17, 16)
+#define RCC_BDCR_RTCSRC_SHIFT		16
+#define RCC_BDCR_VSWRST			BIT(31)
+
+/* Fields of RCC_RDLSICR register */
+#define RCC_RDLSICR_LSION		BIT(0)
+#define RCC_RDLSICR_LSIRDY		BIT(1)
+
+/* Used for all RCC_PLL<n>CR registers */
+#define RCC_PLLNCR_PLLON		BIT(0)
+#define RCC_PLLNCR_PLLRDY		BIT(1)
+#define RCC_PLLNCR_SSCG_CTRL		BIT(2)
+#define RCC_PLLNCR_DIVPEN		BIT(4)
+#define RCC_PLLNCR_DIVQEN		BIT(5)
+#define RCC_PLLNCR_DIVREN		BIT(6)
+#define RCC_PLLNCR_DIVEN_SHIFT		4
+
+/* Used for all RCC_PLL<n>CFGR1 registers */
+#define RCC_PLLNCFGR1_DIVM_SHIFT	16
+#define RCC_PLLNCFGR1_DIVM_MASK		GENMASK_32(21, 16)
+#define RCC_PLLNCFGR1_DIVN_SHIFT	0
+#define RCC_PLLNCFGR1_DIVN_MASK		GENMASK_32(8, 0)
+/* Only for PLL3 and PLL4 */
+#define RCC_PLLNCFGR1_IFRGE_SHIFT	24
+#define RCC_PLLNCFGR1_IFRGE_MASK	GENMASK_32(25, 24)
+
+/* Used for all RCC_PLL<n>CFGR2 registers */
+#define RCC_PLLNCFGR2_DIVX_MASK		GENMASK_32(6, 0)
+#define RCC_PLLNCFGR2_DIVP_SHIFT	0
+#define RCC_PLLNCFGR2_DIVP_MASK		GENMASK_32(6, 0)
+#define RCC_PLLNCFGR2_DIVQ_SHIFT	8
+#define RCC_PLLNCFGR2_DIVQ_MASK		GENMASK_32(14, 8)
+#define RCC_PLLNCFGR2_DIVR_SHIFT	16
+#define RCC_PLLNCFGR2_DIVR_MASK		GENMASK_32(22, 16)
+
+/* Used for all RCC_PLL<n>FRACR registers */
+#define RCC_PLLNFRACR_FRACV_SHIFT	3
+#define RCC_PLLNFRACR_FRACV_MASK	GENMASK_32(15, 3)
+#define RCC_PLLNFRACR_FRACLE		BIT(16)
+
+/* Used for all RCC_PLL<n>CSGR registers */
+#define RCC_PLLNCSGR_INC_STEP_SHIFT	16
+#define RCC_PLLNCSGR_INC_STEP_MASK	GENMASK_32(30, 16)
+#define RCC_PLLNCSGR_MOD_PER_SHIFT	0
+#define RCC_PLLNCSGR_MOD_PER_MASK	GENMASK_32(12, 0)
+#define RCC_PLLNCSGR_SSCG_MODE_SHIFT	15
+#define RCC_PLLNCSGR_SSCG_MODE_MASK	BIT(15)
+
+/* Used for RCC_OCENSETR and RCC_OCENCLRR registers */
+#define RCC_OCENR_HSION			BIT(0)
+#define RCC_OCENR_HSIKERON		BIT(1)
+#define RCC_OCENR_CSION			BIT(4)
+#define RCC_OCENR_CSIKERON		BIT(5)
+#define RCC_OCENR_DIGBYP		BIT(7)
+#define RCC_OCENR_HSEON			BIT(8)
+#define RCC_OCENR_HSEKERON		BIT(9)
+#define RCC_OCENR_HSEBYP		BIT(10)
+#define RCC_OCENR_HSECSSON		BIT(11)
+
+/* Fields of RCC_OCRDYR register */
+#define RCC_OCRDYR_HSIRDY		BIT(0)
+#define RCC_OCRDYR_HSIDIVRDY		BIT(2)
+#define RCC_OCRDYR_CSIRDY		BIT(4)
+#define RCC_OCRDYR_HSERDY		BIT(8)
+
+/* Fields of RCC_DDRITFCR register */
+#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_DDRCAPBRST		BIT(14)
+#define RCC_DDRITFCR_DDRCAXIRST		BIT(15)
+#define RCC_DDRITFCR_DDRCORERST		BIT(16)
+#define RCC_DDRITFCR_DPHYAPBRST		BIT(17)
+#define RCC_DDRITFCR_DPHYRST		BIT(18)
+#define RCC_DDRITFCR_DPHYCTLRST		BIT(19)
+#define RCC_DDRITFCR_DDRCKMOD_MASK	GENMASK_32(22, 20)
+#define RCC_DDRITFCR_DDRCKMOD_SHIFT	20
+#define RCC_DDRITFCR_DDRCKMOD_SSR	0
+#define RCC_DDRITFCR_DDRCKMOD_ASR1	BIT(20)
+#define RCC_DDRITFCR_DDRCKMOD_HSR1	BIT(21)
+#define RCC_DDRITFCR_GSKPCTRL		BIT(24)
+
+/* Fields of RCC_HSICFGR register */
+#define RCC_HSICFGR_HSIDIV_MASK		GENMASK_32(1, 0)
+#define RCC_HSICFGR_HSITRIM_SHIFT	8
+#define RCC_HSICFGR_HSITRIM_MASK	GENMASK_32(14, 8)
+#define RCC_HSICFGR_HSICAL_SHIFT	16
+#define RCC_HSICFGR_HSICAL_MASK		GENMASK_32(27, 16)
+
+/* Fields of RCC_CSICFGR register */
+#define RCC_CSICFGR_CSITRIM_SHIFT	8
+#define RCC_CSICFGR_CSITRIM_MASK	GENMASK_32(12, 8)
+#define RCC_CSICFGR_CSICAL_SHIFT	16
+#define RCC_CSICFGR_CSICAL_MASK		GENMASK_32(23, 16)
+
+/* Used for RCC_MCO related operations */
+#define RCC_MCOCFG_MCOON		BIT(12)
+#define RCC_MCOCFG_MCODIV_MASK		GENMASK_32(7, 4)
+#define RCC_MCOCFG_MCODIV_SHIFT		4
+#define RCC_MCOCFG_MCOSRC_MASK		GENMASK_32(2, 0)
+
+/* Fields of RCC_DBGCFGR register */
+#define RCC_DBGCFGR_DBGCKEN		BIT(8)
+
+/* RCC register fields for reset reasons */
+#define RCC_MP_RSTSCLRR_PORRSTF		BIT(0)
+#define RCC_MP_RSTSCLRR_BORRSTF		BIT(1)
+#define RCC_MP_RSTSCLRR_PADRSTF		BIT(2)
+#define RCC_MP_RSTSCLRR_HCSSRSTF	BIT(3)
+#define RCC_MP_RSTSCLRR_VCORERSTF	BIT(4)
+#define RCC_MP_RSTSCLRR_MPSYSRSTF	BIT(6)
+#define RCC_MP_RSTSCLRR_MCSYSRSTF	BIT(7)
+#define RCC_MP_RSTSCLRR_IWDG1RSTF	BIT(8)
+#define RCC_MP_RSTSCLRR_IWDG2RSTF	BIT(9)
+#define RCC_MP_RSTSCLRR_STDBYRSTF	BIT(11)
+#define RCC_MP_RSTSCLRR_CSTDBYRSTF	BIT(12)
+
+/* Global Reset Register */
+#define RCC_MP_GRSTCSETR_MPSYSRST	BIT(0)
+#define RCC_MP_GRSTCSETR_MCURST		BIT(1)
+#define RCC_MP_GRSTCSETR_MPUP0RST	BIT(4)
+#define RCC_MP_GRSTCSETR_MPUP1RST	BIT(5)
+
+/* Clock Source Interrupt Flag Register */
+#define RCC_MP_CIFR_LSIRDYF		BIT(0)
+#define RCC_MP_CIFR_LSERDYF		BIT(1)
+#define RCC_MP_CIFR_HSIRDYF		BIT(2)
+#define RCC_MP_CIFR_HSERDYF		BIT(3)
+#define RCC_MP_CIFR_CSIRDYF		BIT(4)
+#define RCC_MP_CIFR_PLL1DYF		BIT(8)
+#define RCC_MP_CIFR_PLL2DYF		BIT(9)
+#define RCC_MP_CIFR_PLL3DYF		BIT(10)
+#define RCC_MP_CIFR_PLL4DYF		BIT(11)
+#define RCC_MP_CIFR_LSECSSF		BIT(16)
+#define RCC_MP_CIFR_WKUPF		BIT(20)
+#define RCC_MP_CIFR_MASK		(RCC_MP_CIFR_LSIRDYF | RCC_MP_CIFR_LSERDYF | \
+				 RCC_MP_CIFR_HSIRDYF | RCC_MP_CIFR_HSERDYF | \
+				 RCC_MP_CIFR_CSIRDYF | RCC_MP_CIFR_PLL1DYF | \
+				 RCC_MP_CIFR_PLL2DYF | RCC_MP_CIFR_PLL3DYF | \
+				 RCC_MP_CIFR_PLL4DYF | RCC_MP_CIFR_LSECSSF | \
+				 RCC_MP_CIFR_WKUPF)
+
+/* Stop Request Set Register */
+#define RCC_MP_SREQSETR_STPREQ_P0	BIT(0)
+#define RCC_MP_SREQSETR_STPREQ_P1	BIT(1)
+
+/* Stop Request Clear Register */
+#define RCC_MP_SREQCLRR_STPREQ_P0	BIT(0)
+#define RCC_MP_SREQCLRR_STPREQ_P1	BIT(1)
+
+/* RCC_MP_APB5RST(SET|CLR)R bit fields */
+#define RCC_APB5RSTSETR_SPI6RST		BIT(0)
+#define RCC_APB5RSTSETR_I2C4RST		BIT(2)
+#define RCC_APB5RSTSETR_I2C6RST		BIT(3)
+#define RCC_APB5RSTSETR_USART1RST	BIT(4)
+#define RCC_APB5RSTSETR_STGENRST	BIT(20)
+
+/* RCC_MP_APB5EN(SET|CLR)R bit fields */
+#define RCC_MP_APB5ENSETR_SPI6EN_POS		0
+#define RCC_MP_APB5ENSETR_I2C4EN_POS		2
+#define RCC_MP_APB5ENSETR_I2C6EN_POS		3
+#define RCC_MP_APB5ENSETR_USART1EN_POS		4
+#define RCC_MP_APB5ENSETR_RTCAPBEN_POS		8
+#define RCC_MP_APB5ENSETR_TZC1EN_POS		11
+#define RCC_MP_APB5ENSETR_TZC2EN_POS		12
+#define RCC_MP_APB5ENSETR_TZPCEN_POS		13
+#define RCC_MP_APB5ENSETR_IWDG1APBEN_POS	15
+#define RCC_MP_APB5ENSETR_BSECEN_POS		16
+#define RCC_MP_APB5ENSETR_STGENEN_POS		20
+
+#define RCC_MP_APB5ENSETR_SPI6EN	BIT(RCC_MP_APB5ENSETR_SPI6EN_POS)
+#define RCC_MP_APB5ENSETR_I2C4EN	BIT(RCC_MP_APB5ENSETR_I2C4EN_POS)
+#define RCC_MP_APB5ENSETR_I2C6EN	BIT(RCC_MP_APB5ENSETR_I2C6EN_POS)
+#define RCC_MP_APB5ENSETR_USART1EN	BIT(RCC_MP_APB5ENSETR_USART1EN_POS)
+#define RCC_MP_APB5ENSETR_RTCAPBEN	BIT(RCC_MP_APB5ENSETR_RTCAPBEN_POS)
+#define RCC_MP_APB5ENSETR_TZC1EN	BIT(RCC_MP_APB5ENSETR_TZC1EN_POS)
+#define RCC_MP_APB5ENSETR_TZC2EN	BIT(RCC_MP_APB5ENSETR_TZC2EN_POS)
+#define RCC_MP_APB5ENSETR_TZPCEN	BIT(RCC_MP_APB5ENSETR_TZPCEN_POS)
+#define RCC_MP_APB5ENSETR_IWDG1APBEN	BIT(RCC_MP_APB5ENSETR_IWDG1APBEN_POS)
+#define RCC_MP_APB5ENSETR_BSECEN	BIT(RCC_MP_APB5ENSETR_BSECEN_POS)
+#define RCC_MP_APB5ENSETR_STGENEN	BIT(RCC_MP_APB5ENSETR_STGENEN_POS)
+
+/* RCC_MP_APB5LPEN(SET|CLR)R bit fields */
+#define RCC_MP_APB5LPENSETR_SPI6LPEN		BIT(0)
+#define RCC_MP_APB5LPENSETR_I2C4LPEN		BIT(2)
+#define RCC_MP_APB5LPENSETR_I2C6LPEN		BIT(3)
+#define RCC_MP_APB5LPENSETR_USART1LPEN		BIT(4)
+#define RCC_MP_APB5LPENSETR_RTCAPBLPEN		BIT(8)
+#define RCC_MP_APB5LPENSETR_TZC1LPEN		BIT(11)
+#define RCC_MP_APB5LPENSETR_TZC2LPEN		BIT(12)
+#define RCC_MP_APB5LPENSETR_TZPCLPEN		BIT(13)
+#define RCC_MP_APB5LPENSETR_IWDG1APBLPEN	BIT(15)
+#define RCC_MP_APB5LPENSETR_BSECLPEN		BIT(16)
+#define RCC_MP_APB5LPENSETR_STGENLPEN		BIT(20)
+#define RCC_MP_APB5LPENSETR_STGENSTPEN		BIT(21)
+
+/* RCC_MP_AHB5RST(SET|CLR)R bit fields */
+#define RCC_AHB5RSTSETR_GPIOZRST		BIT(0)
+#define RCC_AHB5RSTSETR_CRYP1RST		BIT(4)
+#define RCC_AHB5RSTSETR_HASH1RST		BIT(5)
+#define RCC_AHB5RSTSETR_RNG1RST			BIT(6)
+#define RCC_AHB5RSTSETR_AXIMCRST		BIT(16)
+
+/* RCC_MP_AHB5EN(SET|CLR)R bit fields */
+#define RCC_MP_AHB5ENSETR_GPIOZEN_POS		0
+#define RCC_MP_AHB5ENSETR_CRYP1EN_POS		4
+#define RCC_MP_AHB5ENSETR_HASH1EN_POS		5
+#define RCC_MP_AHB5ENSETR_RNG1EN_POS		6
+#define RCC_MP_AHB5ENSETR_BKPSRAMEN_POS		8
+#define RCC_MP_AHB5ENSETR_AXIMCEN_POS		16
+
+#define RCC_MP_AHB5ENSETR_GPIOZEN	BIT(RCC_MP_AHB5ENSETR_GPIOZEN_POS)
+#define RCC_MP_AHB5ENSETR_CRYP1EN	BIT(RCC_MP_AHB5ENSETR_CRYP1EN_POS)
+#define RCC_MP_AHB5ENSETR_HASH1EN	BIT(RCC_MP_AHB5ENSETR_HASH1EN_POS)
+#define RCC_MP_AHB5ENSETR_RNG1EN	BIT(RCC_MP_AHB5ENSETR_RNG1EN_POS)
+#define RCC_MP_AHB5ENSETR_BKPSRAMEN	BIT(RCC_MP_AHB5ENSETR_BKPSRAMEN_POS)
+#define RCC_MP_AHB5ENSETR_AXIMCEN	BIT(RCC_MP_AHB5ENSETR_AXIMCEN_POS)
+
+/* RCC_MP_AHB5LPEN(SET|CLR)R bit fields */
+#define RCC_MP_AHB5LPENSETR_GPIOZLPEN		BIT(0)
+#define RCC_MP_AHB5LPENSETR_CRYP1LPEN		BIT(4)
+#define RCC_MP_AHB5LPENSETR_HASH1LPEN		BIT(5)
+#define RCC_MP_AHB5LPENSETR_RNG1LPEN		BIT(6)
+#define RCC_MP_AHB5LPENSETR_BKPSRAMLPEN		BIT(8)
+
+/* RCC_MP_IWDGFZ(SET|CLR)R bit fields */
+#define RCC_MP_IWDGFZSETR_IWDG1			BIT(0)
+#define RCC_MP_IWDGFZSETR_IWDG2			BIT(1)
+
+#ifndef ASM
+uintptr_t stm32_rcc_base(void);
+void stm32_rcc_secure(int enable);
+bool stm32_rcc_is_secure(void);
+bool stm32_rcc_is_mckprot(void);
+#endif
+
+#endif /*__STM32MP1_RCC_H__*/
diff --git a/core/arch/arm/plat-stm32mp1/drivers/sub.mk b/core/arch/arm/plat-stm32mp1/drivers/sub.mk
new file mode 100644
index 0000000..c668efa
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/sub.mk
@@ -0,0 +1,8 @@
+srcs-y 	+= stm32mp1_pwr.c
+srcs-y 	+= stm32mp1_rcc.c
+srcs-$(CFG_STM32_CLOCKSRC_CALIB) += stm32mp1_calib.c
+srcs-y 	+= stm32_reset.c
+srcs-$(CFG_STPMIC1) += stm32mp1_pmic.c
+srcs-y += stm32mp1_clk.c
+srcs-$(CFG_DT) += stm32mp1_clkfunc.c
+srcs-y += stm32mp1_ddrc.c
diff --git a/core/arch/arm/plat-stm32mp1/link.mk b/core/arch/arm/plat-stm32mp1/link.mk
index 97e22ea..01a9b8e 100644
--- a/core/arch/arm/plat-stm32mp1/link.mk
+++ b/core/arch/arm/plat-stm32mp1/link.mk
@@ -11,14 +11,14 @@ endef
 all: $(link-out-dir)/tee-header_v2.stm32
 cleanfiles += $(link-out-dir)/tee-header_v2.stm32
 $(link-out-dir)/tee-header_v2.stm32: $(link-out-dir)/tee-header_v2.bin
-	$(stm32image_cmd) --source $< --dest $@
+	$(stm32image_cmd) --source $< --dest $@ --bintype 0x20
 
 all: $(link-out-dir)/tee-pager_v2.stm32
 cleanfiles += $(link-out-dir)/tee-pager_v2.stm32
 $(link-out-dir)/tee-pager_v2.stm32: $(link-out-dir)/tee-pager_v2.bin
-	$(stm32image_cmd) --source $< --dest $@
+	$(stm32image_cmd) --source $< --dest $@ --bintype 0x21
 
 all: $(link-out-dir)/tee-pageable_v2.stm32
 cleanfiles += $(link-out-dir)/tee-pageable_v2.stm32
 $(link-out-dir)/tee-pageable_v2.stm32: $(link-out-dir)/tee-pageable_v2.bin
-	$(stm32image_cmd) --source $< --dest $@
+	$(stm32image_cmd) --source $< --dest $@ --bintype 0x22
diff --git a/core/arch/arm/plat-stm32mp1/main.c b/core/arch/arm/plat-stm32mp1/main.c
index f45e3cd..1921678 100644
--- a/core/arch/arm/plat-stm32mp1/main.c
+++ b/core/arch/arm/plat-stm32mp1/main.c
@@ -4,32 +4,105 @@
  * Copyright (c) 2016-2018, Linaro Limited
  */
 
+#include <arm.h>
 #include <boot_api.h>
 #include <console.h>
 #include <drivers/gic.h>
+#include <drivers/stm32_bsec.h>
+#include <drivers/stm32_etzpc.h>
+#include <drivers/stm32_iwdg.h>
 #include <drivers/stm32_uart.h>
+#include <drivers/stm32mp1_clk.h>
+#include <drivers/stm32mp1_rcc.h>
+#include <dt-bindings/etzpc/stm32-etzpc.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <io.h>
+#include <keep.h>
 #include <kernel/generic_boot.h>
+#include <kernel/interrupt.h>
 #include <kernel/misc.h>
 #include <kernel/panic.h>
 #include <kernel/pm_stubs.h>
+#include <kernel/spinlock.h>
+#include <mm/core_mmu.h>
 #include <mm/core_memprot.h>
 #include <platform_config.h>
+#include <sm/optee_smc.h>
 #include <sm/psci.h>
+#include <tee/arch_svc.h>
 #include <tee/entry_std.h>
 #include <tee/entry_fast.h>
+#include <stdint.h>
+#include <stm32_util.h>
+#include <stm32mp_dt.h>
+#include <stm32mp_pm.h>
+#include <string.h>
+#include <trace.h>
+#include <util.h>
 
-register_phys_mem(MEM_AREA_IO_NSEC, CONSOLE_UART_BASE, CONSOLE_UART_SIZE);
+#ifdef CFG_WITH_NSEC_GPIOS
+register_phys_mem(MEM_AREA_IO_NSEC, GPIOS_NSEC_BASE, GPIOS_NSEC_SIZE);
+#endif
+register_phys_mem(MEM_AREA_IO_NSEC, RNG1_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_NSEC, IWDG1_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_NSEC, IWDG2_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_NSEC, RTC_BASE, SMALL_PAGE_SIZE);
+#ifdef CFG_WITH_NSEC_UARTS
+register_phys_mem(MEM_AREA_IO_NSEC, USART1_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_NSEC, USART2_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_NSEC, USART3_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_NSEC, UART4_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_NSEC, UART5_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_NSEC, USART6_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_NSEC, UART7_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_NSEC, UART8_BASE, SMALL_PAGE_SIZE);
+#endif
 
-register_phys_mem(MEM_AREA_IO_SEC, GIC_BASE, GIC_SIZE);
-register_phys_mem(MEM_AREA_IO_SEC, BKP_REGS_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, GIC_IOMEM_BASE, GIC_IOMEM_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, TAMP_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, RCC_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, PWR_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, ETZPC_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, BSEC_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, I2C4_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, STGEN_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, GPIOZ_BASE,
+		  STM32MP1_GPIOZ_MAX_COUNT * SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, RNG1_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, IWDG1_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, RTC_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, DDRCTRL_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, DDRPHYC_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, BKPSRAM_BASE, SMALL_PAGE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, USART1_BASE, SMALL_PAGE_SIZE);
 
-static struct gic_data gic_data;
-static struct console_pdata console_data;
+register_phys_mem(MEM_AREA_ROM_SEC, TEE_RAM_START, TEE_RAM_PH_SIZE);
 
-static void main_fiq(void)
+register_ddr(DDR_BASE, STM32MP1_DDR_SIZE_DFLT);
+
+#ifdef CFG_STM32MP_MAP_NSEC_LOW_DDR
+register_phys_mem(MEM_AREA_RAM_NSEC, DDR_BASE, SMALL_PAGE_SIZE);
+#endif
+
+#define _ID2STR(id)		(#id)
+#define ID2STR(id)		_ID2STR(id)
+
+static TEE_Result platform_banner(void)
 {
-	gic_it_handle(&gic_data);
+#ifdef CFG_DT
+	IMSG("Platform stm32mp1: flavor %s - device tree %s",
+		ID2STR(PLATFORM_FLAVOR), ID2STR(CFG_SECURE_DT));
+	IMSG("Model: %s", fdt_get_board_model(get_dt_blob()));
+#else
+	IMSG("Platform stm32mp1: flavor %s - no device tree",
+		ID2STR(PLATFORM_FLAVOR));
+#endif
+
+	return TEE_SUCCESS;
 }
+service_init(platform_banner);
+
+static void main_fiq(void);
 
 static const struct thread_handlers handlers = {
 	.std_smc = tee_entry_std,
@@ -48,10 +121,132 @@ const struct thread_handlers *generic_boot_get_handlers(void)
 	return &handlers;
 }
 
+/*
+ * Console
+ *
+ * We cannot use the generic serial_console support since probing
+ * the console requires the platform clock driver to be already
+ * up and ready which is done only once service_init are completed.
+ *
+ * If the console uses a non secure UART, its clock might have been
+ * disabled by the non secure world, in which case secure traces must
+ * be dropped unless what the driver may stall since the UART FIFO is
+ * never emptied.
+ */
+#define CONSOLE_WITHOUT_CLOCK_MAGIC	~0UL
+
+static struct stm32_uart_pdata console_data;
+static struct serial_chip *serial_console;
+
 void console_init(void)
 {
-	stm32_uart_init(&console_data, CONSOLE_UART_BASE);
+	/* Early console initialization before MMU setup */
+	struct uart {
+		uintptr_t pa;
+		bool secure;
+	} uarts[] = {
+		[0] = { .pa = 0 },
+		[1] = { .pa = USART1_BASE, .secure = true, },
+		[2] = { .pa = USART2_BASE, .secure = false, },
+		[3] = { .pa = USART3_BASE, .secure = false, },
+		[4] = { .pa = UART4_BASE, .secure = false, },
+		[5] = { .pa = UART5_BASE, .secure = false, },
+		[6] = { .pa = USART6_BASE, .secure = false, },
+		[7] = { .pa = UART7_BASE, .secure = false, },
+		[8] = { .pa = UART8_BASE, .secure = false, },
+	};
+
+	COMPILE_TIME_ASSERT(CFG_STM32_EARLY_CONSOLE_UART < ARRAY_SIZE(uarts));
+	assert(!cpu_mmu_enabled());
+
+	if (!uarts[CFG_STM32_EARLY_CONSOLE_UART].pa) {
+		return;
+	}
+
+	/* No clock/PINCTRL yet bound to the UART console */
+	console_data.clock = CONSOLE_WITHOUT_CLOCK_MAGIC;
+
+	console_data.secure = uarts[CFG_STM32_EARLY_CONSOLE_UART].secure;
+	stm32_uart_init(&console_data, uarts[CFG_STM32_EARLY_CONSOLE_UART].pa);
+
+#ifdef CFG_FORCE_CONSOLE_ON_SUSPEND
+	serial_console = &console_data.chip;
+#else
 	register_serial_console(&console_data.chip);
+#endif
+
+	IMSG("Early console on UART#%u", CFG_STM32_EARLY_CONSOLE_UART);
+}
+
+#ifdef CFG_FORCE_CONSOLE_ON_SUSPEND
+void console_putc(int ch)
+{
+	if (!serial_console)
+		return;
+
+	stm32_pinctrl_load_active_cfg(console_data.pinctrl,
+				      console_data.pinctrl_count);
+
+        if (ch == '\n')
+                serial_console->ops->putc(serial_console, '\r');
+
+	serial_console->ops->putc(serial_console, ch);
+}
+
+void console_flush(void)
+{
+	if (!serial_console)
+		return;
+
+	serial_console->ops->flush(serial_console);
+}
+#endif
+
+#if CFG_DT
+/* Probe console once clocks inits (service_init level) are completed */
+static TEE_Result stm32_uart_console_probe(void)
+{
+	void *fdt;
+	int node;
+	struct stm32_uart_pdata *pd = NULL;
+
+	fdt = get_dt_blob();
+	if (fdt == NULL) {
+		panic();
+	}
+
+	node = fdt_get_stdout_node_offset(fdt);
+	if (node >= 0) {
+		pd = probe_uart_from_dt_node(fdt, node);
+	}
+
+	if (pd) {
+		serial_console = NULL;
+		dmb();
+		memcpy(&console_data, pd, sizeof(*pd));
+		dmb();
+		serial_console = &console_data.chip;
+		free(pd);
+		IMSG("UART console probed from DT (%ssecure)",
+			pd->secure ? "" : "non ");
+	} else {
+		IMSG("No UART console probed from DT");
+		serial_console = NULL;
+	}
+
+	return TEE_SUCCESS;
+}
+service_init_late(stm32_uart_console_probe);
+#endif
+
+/*
+ * GIC init, used also for primary/secondary boot core wake completion
+ */
+static struct gic_data gic_data;
+
+static void main_fiq(void)
+{
+	gic_it_handle(&gic_data);
 }
 
 void main_init_gic(void)
@@ -59,75 +254,378 @@ void main_init_gic(void)
 	void *gicc_base;
 	void *gicd_base;
 
-	gicc_base = phys_to_virt(GIC_BASE + GICC_OFFSET, MEM_AREA_IO_SEC);
-	gicd_base = phys_to_virt(GIC_BASE + GICD_OFFSET, MEM_AREA_IO_SEC);
+	gicc_base = phys_to_virt(GIC_IOMEM_BASE + GICC_OFFSET, MEM_AREA_IO_SEC);
+	gicd_base = phys_to_virt(GIC_IOMEM_BASE + GICD_OFFSET, MEM_AREA_IO_SEC);
 	if (!gicc_base || !gicd_base)
 		panic();
 
 	gic_init(&gic_data, (vaddr_t)gicc_base, (vaddr_t)gicd_base);
 	itr_init(&gic_data.chip);
+
+#ifdef CFG_PSCI_ARM32
+	stm32mp_register_online_cpu();
+#endif
 }
 
 void main_secondary_init_gic(void)
 {
 	gic_cpu_init(&gic_data);
+
+#if CFG_TEE_CORE_NB_CORE == 2
+	/* Secondary core release constraint on APB5 clock */
+	write32(BOOT_API_A7_RESET_MAGIC_NUMBER,
+		stm32mp_bkpreg(BCKR_CORE1_MAGIC_NUMBER));
+	stm32mp1_clk_disable_secure(RTCAPB);
+#endif
+
+#ifdef CFG_PSCI_ARM32
+	stm32mp_register_online_cpu();
+#endif
+}
+
+/* Specific GIC suspend/resume function called aside registered PM handlers */
+void stm32mp_gic_suspend_resume(enum pm_op op)
+{
+	switch (op) {
+	case PM_OP_SUSPEND:
+		gic_suspend(&gic_data);
+		break;
+	case PM_OP_RESUME:
+		gic_resume(&gic_data);
+		break;
+	default:
+		panic();
+	}
+}
+KEEP_PAGER(stm32mp_gic_suspend_resume);
+
+/* stm32mp1 low power sequence needs straight access to GIC */
+uintptr_t get_gicc_base(void)
+{
+	uintptr_t pbase = GIC_IOMEM_BASE + GICC_OFFSET;
+
+	if (cpu_mmu_enabled())
+		return (uintptr_t)phys_to_virt(pbase, MEM_AREA_IO_SEC);
+
+	return pbase;
+}
+
+uintptr_t get_gicd_base(void)
+{
+	uintptr_t pbase = GIC_IOMEM_BASE + GICD_OFFSET;
+
+	if (cpu_mmu_enabled())
+		return (uintptr_t)phys_to_virt(pbase, MEM_AREA_IO_SEC);
+
+	return pbase;
+}
+
+/*
+ * Various platform specific functions
+ */
+unsigned int stm32mp_get_otp_max(void)
+{
+	return STM32MP1_OTP_MAX_ID;
+}
+unsigned int stm32mp_get_otp_upper_start(void)
+{
+	return STM32MP1_UPPER_OTP_START;
+}
+
+bool __weak stm32mp_with_pmic(void)
+{
+	return false;
 }
 
 /*
- * SMP boot support and access to the mailbox
+ * SIP and other platform specific services
  */
-#define GIC_SEC_SGI_0		8
+bool sm_platform_handler(struct sm_ctx *ctx)
+{
+	uint32_t *a0 = (uint32_t *)(&ctx->nsec.r0);
+	uint32_t *a1 = (uint32_t *)(&ctx->nsec.r1);
+	uint32_t *a2 = (uint32_t *)(&ctx->nsec.r2);
+	uint32_t *a3 = (uint32_t *)(&ctx->nsec.r3);
+
+	if (!OPTEE_SMC_IS_FAST_CALL(*a0))
+		return true;
+
+	switch (OPTEE_SMC_OWNER_NUM(*a0)) {
+	case OPTEE_SMC_OWNER_SIP:
+		return stm32_sip_service(ctx, a0, a1, a2, a3);
+	case OPTEE_SMC_OWNER_OEM:
+		return stm32_oem_service(ctx, a0, a1, a2, a3);
+	default:
+		return true;
+	}
+}
 
-static vaddr_t bckreg_base(void)
+/* SoC versioning util */
+uint32_t stm32mp1_dbgmcu_get_chip_version(void)
+{
+	uintptr_t base = DBGMCU_BASE;
+
+	if (cpu_mmu_enabled())
+		base = (uintptr_t)phys_to_virt(DBGMCU_BASE, MEM_AREA_IO_SEC);
+
+	return read32(base + DBGMCU_IDC) >> 16;
+}
+
+static uintptr_t stm32_tamp_base(void)
 {
 	static void *va;
 
 	if (!cpu_mmu_enabled())
-		return BKP_REGS_BASE + BKP_REGISTER_OFF;
+		return TAMP_BASE;
 
 	if (!va)
-		va = phys_to_virt(BKP_REGS_BASE + BKP_REGISTER_OFF,
-				  MEM_AREA_IO_SEC);
+		va = phys_to_virt(TAMP_BASE, MEM_AREA_IO_SEC);
 
-	return (vaddr_t)va;
+	return (uintptr_t)va;
 }
 
-static uint32_t *bckreg_address(unsigned int idx)
+static uintptr_t bkpreg_base(void)
 {
-	return (uint32_t *)bckreg_base() + idx;
+	return stm32_tamp_base() + TAMP_BKP_REGISTER_OFF;
 }
 
-static void release_secondary_early_hpen(size_t pos)
+uintptr_t stm32mp_bkpreg(unsigned int idx)
 {
-	uint32_t *p_entry = bckreg_address(BCKR_CORE1_BRANCH_ADDRESS);
-	uint32_t *p_magic = bckreg_address(BCKR_CORE1_MAGIC_NUMBER);
+	return bkpreg_base() + (idx * sizeof(uint32_t));
+}
 
-	*p_entry = TEE_LOAD_ADDR;
-	*p_magic = BOOT_API_A7_CORE1_MAGIC_NUMBER;
+uintptr_t stm32mp1_bkpsram_base(void)
+{
+	static void *va;
 
-	dmb();
-	isb();
-	itr_raise_sgi(GIC_SEC_SGI_0, BIT(pos));
+	if (!cpu_mmu_enabled())
+		return BKPSRAM_BASE;
+
+	if (!va)
+		va = phys_to_virt(BKPSRAM_BASE, MEM_AREA_IO_SEC);
+
+	return (uintptr_t)va;
+}
+
+uintptr_t stm32_get_stgen_base(void)
+{
+	static uintptr_t va;
+
+	if (!cpu_mmu_enabled())
+		return STGEN_BASE;
+
+	if (!va)
+		va = (uintptr_t)phys_to_virt(STGEN_BASE, MEM_AREA_IO_SEC);
+
+	return va;
 }
 
-int psci_cpu_on(uint32_t core_id, uint32_t entry, uint32_t context_id)
+uintptr_t stm32_get_gpio_bank_base(unsigned int bank)
 {
-	size_t pos = get_core_pos_mpidr(core_id);
-	static bool core_is_released[CFG_TEE_CORE_NB_CORE];
+	/* Non secure banks and mapped together, same for secure banks */
+	static uintptr_t gpiox_va;
+	static uintptr_t gpioz_va;
+
+	switch (bank) {
+	case GPIO_BANK_A:
+	case GPIO_BANK_B:
+	case GPIO_BANK_C:
+	case GPIO_BANK_D:
+	case GPIO_BANK_E:
+	case GPIO_BANK_F:
+	case GPIO_BANK_G:
+	case GPIO_BANK_H:
+	case GPIO_BANK_I:
+	case GPIO_BANK_J:
+	case GPIO_BANK_K:
+		if (!gpiox_va && cpu_mmu_enabled())
+			gpiox_va = (uintptr_t)phys_to_virt(GPIOS_NSEC_BASE,
+							   MEM_AREA_IO_NSEC);
 
-	if (!pos || pos >= CFG_TEE_CORE_NB_CORE)
-		return PSCI_RET_INVALID_PARAMETERS;
+		if (cpu_mmu_enabled())
+			return gpiox_va + (bank * GPIO_BANK_OFFSET);
 
-	DMSG("core pos: %zu: ns_entry %#" PRIx32, pos, entry);
+		return GPIOS_NSEC_BASE + (bank * GPIO_BANK_OFFSET);
 
-	if (core_is_released[pos]) {
-		DMSG("core %zu already released", pos);
-		return PSCI_RET_DENIED;
+	case GPIO_BANK_Z:
+		if (!gpioz_va && cpu_mmu_enabled())
+			gpioz_va = (uintptr_t)phys_to_virt(GPIOZ_BASE,
+							   MEM_AREA_IO_SEC);
+
+		if (cpu_mmu_enabled())
+			return gpioz_va;
+
+		return GPIOZ_BASE;
+	default:
+		panic();
+	}
+}
+
+uint32_t stm32_get_gpio_bank_offset(unsigned int bank)
+{
+	if (bank == GPIO_BANK_Z) {
+		return 0;
+	} else {
+		return bank * GPIO_BANK_OFFSET;
+	}
+}
+
+/* Return clock ID on success, negative value on error */
+int stm32_get_gpio_bank_clock(unsigned int bank)
+{
+	switch (bank) {
+	case GPIO_BANK_A:
+	case GPIO_BANK_B:
+	case GPIO_BANK_C:
+	case GPIO_BANK_D:
+	case GPIO_BANK_E:
+	case GPIO_BANK_F:
+	case GPIO_BANK_G:
+	case GPIO_BANK_H:
+	case GPIO_BANK_I:
+	case GPIO_BANK_J:
+	case GPIO_BANK_K:
+		return (int)GPIOA + (bank - GPIO_BANK_A);
+	case GPIO_BANK_Z:
+		return (int)GPIOZ;
+	default:
+		panic();
 	}
-	core_is_released[pos] = true;
+}
+
+int stm32mp_iwdg_irq2instance(size_t irq)
+{
+	int instance = (int)irq - STM32MP1_IRQ_IWDG1;
+
+	assert((instance >= IWDG1_INST) && (instance <= IWDG2_INST));
+	return instance;
+}
+
+size_t stm32mp_iwdg_instance2irq(int instance)
+{
+	return (size_t)(instance + STM32MP1_IRQ_IWDG1);
+}
 
-	generic_boot_set_core_ns_entry(pos, entry, context_id);
-	release_secondary_early_hpen(pos);
+unsigned int stm32mp_iwdg_iomem2instance(uintptr_t pbase)
+{
+	switch (pbase) {
+	case IWDG1_BASE:
+		return IWDG1_INST;
+	case IWDG2_BASE:
+		return IWDG2_INST;
+	default:
+		panic();
+	}
+}
+
+unsigned long stm32_get_iwdg_otp_config(uintptr_t pbase)
+{
+	unsigned int idx;
+	unsigned long iwdg_cfg = 0;
+	uint32_t otp_value;
+
+	idx = stm32mp_iwdg_iomem2instance(pbase);
+
+	if (bsec_read_otp(&otp_value, HW2_OTP))
+		panic();
+
+	if (otp_value & BIT(idx + HW2_OTP_IWDG_HW_ENABLE_SHIFT))
+		iwdg_cfg |= IWDG_HW_ENABLED;
+
+	if (!(otp_value & BIT(idx + HW2_OTP_IWDG_FZ_STOP_SHIFT)))
+		iwdg_cfg |= IWDG_ENABLE_ON_STOP;
+
+	if (!(otp_value & BIT(idx + HW2_OTP_IWDG_FZ_STANDBY_SHIFT)))
+		iwdg_cfg |= IWDG_ENABLE_ON_STANDBY;
+
+	return iwdg_cfg;
+}
+
+uintptr_t stm32mp_get_etzpc_base(void)
+{
+	return ETZPC_BASE;
+}
+
+/*
+ * This function allows to split bindings between platform and ETZPC
+ * HW mapping. If this conversion was done at driver level, the driver
+ * should include all supported platform bindings. ETZPC may be used on
+ * other platforms.
+ */
+enum etzpc_decprot_attributes stm32mp_etzpc_binding2decprot(uint32_t mode)
+{
+	switch (mode) {
+	case DECPROT_S_RW:
+		return TZPC_DECPROT_S_RW;
+	case DECPROT_NS_R_S_W:
+		return TZPC_DECPROT_NS_R_S_W;
+	case DECPROT_MCU_ISOLATION:
+		return TZPC_DECPROT_MCU_ISOLATION;
+	case DECPROT_NS_RW:
+		return TZPC_DECPROT_NS_RW;
+	default:
+		panic();
+	}
+}
+
+#ifdef CFG_STM32_RTC
+/*******************************************************************************
+ * This function determines the number of needed RTC calendar read operations
+ * to get consistent values (1 or 2 depending of clock frequencies).
+ * If APB1 frequency is less than 7 times the RTC one, the software has to
+ * read the calendar time and date register twice.
+ * This function computes each of them and does the comparison.
+ * Returns true if read twice is needed, false else.
+ ******************************************************************************/
+bool stm32_rtc_get_read_twice(void)
+{
+	unsigned long apb1_freq;
+	unsigned long rtc_freq = 0;
+	uint32_t apb1_div;
+	uintptr_t rcc_base = stm32_rcc_base();
+
+	/* Compute RTC frequency */
+	switch ((mmio_read_32(rcc_base + RCC_BDCR) &
+		 RCC_BDCR_RTCSRC_MASK) >> RCC_BDCR_RTCSRC_SHIFT) {
+	case 1:
+		rtc_freq = stm32mp1_clk_get_rate(CK_LSE);
+		break;
+	case 2:
+		rtc_freq = stm32mp1_clk_get_rate(CK_LSI);
+		break;
+	case 3:
+		rtc_freq = stm32mp1_clk_get_rate(CK_HSE);
+		rtc_freq /= (mmio_read_32(rcc_base + RCC_RTCDIVR) &
+			     RCC_DIVR_DIV_MASK) + 1U;
+		break;
+	default:
+		/* Forbidden values: consider only one needed read here */
+		panic();
+	}
+
+	/* Compute APB1 frequency */
+	apb1_div = mmio_read_32(rcc_base + RCC_APB1DIVR) & RCC_APBXDIV_MASK;
+	apb1_freq = stm32mp1_clk_get_rate(CK_MCU) >> apb1_div;
+
+	/* Compare APB1 and 7*RTC frequencies */
+	return apb1_freq < (rtc_freq * 7U);
+}
+#endif
+
+uint32_t may_spin_lock(unsigned int *lock)
+{
+	if (!lock || !cpu_mmu_enabled()) {
+		return 0;
+	}
+
+	return cpu_spin_lock_xsave(lock);
+}
+
+void may_spin_unlock(unsigned int *lock, uint32_t exceptions)
+{
+	if (!lock || !cpu_mmu_enabled()) {
+		return;
+	}
 
-	return PSCI_RET_SUCCESS;
+	cpu_spin_unlock_xrestore(lock, exceptions);
 }
diff --git a/core/arch/arm/plat-stm32mp1/platform_config.h b/core/arch/arm/plat-stm32mp1/platform_config.h
index 6f5dd97..e8d1660 100644
--- a/core/arch/arm/plat-stm32mp1/platform_config.h
+++ b/core/arch/arm/plat-stm32mp1/platform_config.h
@@ -8,22 +8,185 @@
 
 #include <mm/generic_ram_layout.h>
 
+/* Enable/disable use of the core0 reset control from RCC */
+#undef STM32MP1_USE_MPU0_RESET
+
 /* Make stacks aligned to data cache line length */
 #define STACK_ALIGNMENT			32
 
-#define GIC_BASE			0xA0021000ul
-#define GIC_SIZE			0x2000
-#define GICC_OFFSET			0x1000
-#define GICD_OFFSET			0x0000
+#if defined(CFG_WITH_PAGER)
+#if defined(CFG_WITH_LPAE)
+/*
+ * Optimize unpaged memory size:
+ * - one table for the level2 table for overall vmem range
+ * - two tables for TEE RAM fine grain mapping [2ffc.0000 301f.ffff]
+ * - one table for internal RAMs (PM: ROMed core TEE RAM & DDR first page)
+ * - one table for a 2MByte dynamiq shared virtual memory (SHM_VASPACE)
+ */
+#define MAX_XLAT_TABLES			5
+#else
+/*
+ * Optimize unpaged memory size:
+ * - two tables for TEE RAM mapping [2ffc.0000 300f.ffff]
+ * - one table for secure internal RAMs (PM: ROMed core TEE RAM)
+ * - one table for non-secure internal RAMs (PM: DDR first page)
+ * - two tables for a 2MByte dynamiq shared virtual memory (SHM_VASPACE)
+ */
+#define MAX_XLAT_TABLES			6
+#endif /*CFG_WITH_LPAE*/
+#else
+/* Be generous, there is plenty of secure DDR */
+#define MAX_XLAT_TABLES			10
+#endif /*CFG_WITH_PAGER*/
+
+/* Expected platform default size, if not found in device tree */
+#define STM32MP1_DDR_SIZE_DFLT		(1 * 1024 * 1024 * 1024)
 
-#define BKP_REGS_BASE			0x5C00A000
-#define BKP_REGISTER_OFF		0x100
+#define GIC_IOMEM_BASE			0xa0021000ul
+#define GIC_IOMEM_SIZE			0x00007000
+#define DDR_BASE			0xc0000000ul
+#define SYSRAM_BASE			0x2ffc0000
+#define BKPSRAM_BASE			0x54000000
 
+#define BSEC_BASE			0x5c005000
+#define CRYP1_BASE			0x54001000
+#define DBGMCU_BASE			0x50081000
+#define DDRCTRL_BASE			0x5a003000
+#define DDRPHYC_BASE			0x5a004000
+#define ETZPC_BASE			0x5c007000
+#define EXTI_BASE			0x5000D000
+#define GPIOA_BASE			0x50002000
+#define GPIOB_BASE			0x50003000
+#define GPIOC_BASE			0x50004000
+#define GPIOD_BASE			0x50005000
+#define GPIOE_BASE			0x50006000
+#define GPIOF_BASE			0x50007000
+#define GPIOG_BASE			0x50008000
+#define GPIOH_BASE			0x50009000
+#define GPIOI_BASE			0x5000a000
+#define GPIOJ_BASE			0x5000b000
+#define GPIOK_BASE			0x5000c000
+#define GPIOZ_BASE			0x54004000
+#define HASH1_BASE			0x54002000
+#define I2C4_BASE			0x5c002000
+#define I2C6_BASE			0x5c009000
+#define IWDG1_BASE			0x5c003000
+#define IWDG2_BASE			0x5a002000
+#define PWR_BASE			0x50001000
+#define RCC_BASE			0x50000000
+#define RNG1_BASE			0x54003000
+#define RTC_BASE			0x5c004000
+#define SPI6_BASE			0x5c001000
+#define STGEN_BASE			0x5C008000
+#define TAMP_BASE			0x5c00a000
+#define USART1_BASE			0x5c000000
+#define USART2_BASE			0x4000e000
+#define USART3_BASE			0x4000f000
 #define UART4_BASE			0x40010000
-#define STM32MP1_DEBUG_USART_BASE	UART4_BASE
-#define GIC_SPI_UART4			84
+#define UART5_BASE			0x40011000
+#define USART6_BASE			0x44003000
+#define UART7_BASE			0x40018000
+#define UART8_BASE			0x40019000
+#define TZC_BASE			0x5c006000
+
+/* BSEC OTP resources */
+#define STM32MP1_OTP_MAX_ID		0x5FU
+#define STM32MP1_UPPER_OTP_START	0x20U
+
+#define OTP_MAX_SIZE			(STM32MP1_OTP_MAX_ID + 1U)
+
+#define DATA0_OTP			0
+#define PART_NUMBER_OTP			1
+#define MONOTONIC_OTP			4
+#define NAND_OTP			9
+#define UID0_OTP			13
+#define UID1_OTP			14
+#define UID2_OTP			15
+#define HW2_OTP				18
+
+#define DATA0_OTP_SECURED		BIT(6)
+
+#define HW2_OTP_IWDG_HW_ENABLE_SHIFT		3
+#define HW2_OTP_IWDG_FZ_STOP_SHIFT		5
+#define HW2_OTP_IWDG_FZ_STANDBY_SHIFT		7
+
+/*
+ * GPIO banks: 11 non secure banks (A to K) and 1 secure bank (Z)
+ * Bank register's base address is computed from the bank ID listed here.
+ */
+#define GPIOS_NSEC_COUNT		11
+#define GPIOS_NSEC_BASE			GPIOA_BASE
+#define GPIOS_NSEC_SIZE			(GPIOS_NSEC_COUNT * SMALL_PAGE_SIZE)
+
+#define STM32MP1_GPIOZ_MAX_COUNT	1
+#define STM32MP1_GPIOZ_PIN_MAX_COUNT	8
+
+#define GPIO_BANK_OFFSET		0x1000U
+
+/* Bank IDs used in GPIO driver API */
+#define GPIO_BANK_A			0U
+#define GPIO_BANK_B			1U
+#define GPIO_BANK_C			2U
+#define GPIO_BANK_D			3U
+#define GPIO_BANK_E			4U
+#define GPIO_BANK_F			5U
+#define GPIO_BANK_G			6U
+#define GPIO_BANK_H			7U
+#define GPIO_BANK_I			8U
+#define GPIO_BANK_J			9U
+#define GPIO_BANK_K			10U
+#define GPIO_BANK_Z			25U
+
+/* IWDG resources */
+#define IWDG1_INST			0
+#define IWDG2_INST			1
+
+#define STM32MP1_IRQ_IWDG1		182U
+#define STM32MP1_IRQ_IWDG2		183U
+
+/* TAMP resources */
+#define TAMP_BKP_REGISTER_OFF		0x100
+
+/* RCC platform resources */
+#define RCC_WAKEUP_IT			177
+
+/* SoC revision */
+#define STM32MP1_REV_A			0x00001000
+#define STM32MP1_REV_B			0x00002000
+
+/* DBGMCU resources */
+#define DBGMCU_IDC			0x0
+
+/* BKPSRAM layout */
+#define BKPSRAM_SIZE			SMALL_PAGE_SIZE
+#define BKPSRAM_PM_OFFSET		0x000
+#define BKPSRAM_PM_SIZE			(BKPSRAM_PM_MAILBOX_SIZE + \
+						BKPSRAM_PM_CONTEXT_SIZE)
+
+#define BKPSRAM_PM_MAILBOX_OFFSET	BKPSRAM_PM_OFFSET
+#define BKPSRAM_PM_MAILBOX_SIZE		0x100
+#define BKPSRAM_PM_CONTEXT_OFFSET	(BKPSRAM_PM_MAILBOX_OFFSET + \
+						BKPSRAM_PM_MAILBOX_SIZE)
+#define BKPSRAM_PM_CONTEXT_SIZE		0x700
+
+/* SYSRAM */
+#define SYSRAM_SIZE			0x40000
+
+/* GIC resources */
+#define GICD_OFFSET			0x00000000
+#define GICC_OFFSET			0x00001000
+#define GICH_OFFSET			0x00003000
+#define GICV_OFFSET			0x00005000
+
+#define GIC_NON_SEC_SGI_0		0
+#define GIC_SEC_SGI_0			8
+#define GIC_SEC_SGI_1			9
+#define GIC_SPI_SEC_PHY_TIMER		29
+
+#define TARGET_CPU0_GIC_MASK		BIT(0)
+#define TARGET_CPU1_GIC_MASK		BIT(1)
+#define TARGET_CPUS_GIC_MASK		GENMASK_32(CFG_TEE_CORE_NB_CORE - 1, 0)
 
-#define CONSOLE_UART_BASE		STM32MP1_DEBUG_USART_BASE
-#define CONSOLE_UART_SIZE		1024
+#define STM32MP_GIC_PRIORITY_CSTOP	0xc0
 
 #endif /*PLATFORM_CONFIG_H*/
diff --git a/core/arch/arm/plat-stm32mp1/pm/context.c b/core/arch/arm/plat-stm32mp1/pm/context.c
new file mode 100644
index 0000000..2ce72b3
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/pm/context.c
@@ -0,0 +1,510 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2018, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ */
+
+#include <arm32.h>
+#include <boot_api.h>
+#include <drivers/gic.h>
+#include <drivers/stm32_rng.h>
+#include <drivers/stm32_rtc.h>
+#include <drivers/stm32mp1_clk.h>
+#include <drivers/stm32mp1_ddrc.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <dt-bindings/power/stm32mp1-power.h>
+#include <generated/context_asm_defines.h>
+#include <initcall.h>
+#include <kernel/cache_helpers.h>
+#include <kernel/delay.h>
+#include <keep.h>
+#include <kernel/panic.h>
+#include <mm/core_memprot.h>
+#include <mm/mobj.h>
+#include <platform_config.h>
+#include <stdlib.h>
+#include <stm32_util.h>
+#include <stm32mp_pm.h>
+#include <string.h>
+
+#include "context.h"
+#include "power.h"
+
+#define TRAINING_AREA_SIZE		64
+
+#define STANDBY_CONTEXT_MAGIC		(0x00010000 + TRAINING_AREA_SIZE)
+
+/*
+ * Context saved in TEE RAM during lower power sequence.
+ * Can be allocated if to big for static allocation.
+ *
+ * @stgen_cnt_h: Upper 32bit of the STGEN counter
+ * @stgen_cnt_l: Lower 32bit of the STGEN counter
+ * @rtc: RTC time read at suspend
+ */
+struct pm_context {
+	uint32_t stgen_cnt_h;
+	uint32_t stgen_cnt_l;
+	struct stm32_rtc_calendar rtc;
+};
+
+static struct pm_context plat_ctx;
+
+/*
+ * BKPSRAM contains a mailbox used with early boot stages for resume sequence.
+ * The mailbox content data that must be restored before OP-TEE is resumed.
+ *
+ * @magic: magic value read by early boot stage for consistency
+ * @zq0cr0_zdata: DDRPHY configuration to be restored.
+ * @ddr_training_backup: DDR area saved at suspend and backed up at resume
+ */
+struct pm_mailbox {
+	uint32_t magic;
+	uint32_t core0_resume_ep;
+	uint32_t zq0cr0_zdata;
+	uint8_t ddr_training_backup[TRAINING_AREA_SIZE];
+};
+
+/*
+ * BKPSRAM contains OP-TEE resume instruction sequence which restores
+ * TEE RAM content. The BKPSRAM contains restoration materials
+ * (key, tag) and the resume entry point in restored TEE RAM.
+ */
+static struct retram_resume_ctx *get_retram_resume_ctx(void)
+{
+	uintptr_t bkpsram_base = stm32mp1_bkpsram_base();
+	uintptr_t context_base = bkpsram_base + BKPSRAM_PM_CONTEXT_OFFSET;
+
+	return (struct retram_resume_ctx *)context_base;
+}
+
+static struct pm_mailbox *get_pm_mailbox(void)
+{
+	uintptr_t bkpsram_base = stm32mp1_bkpsram_base();
+	uintptr_t mailbox_base = bkpsram_base + BKPSRAM_PM_MAILBOX_OFFSET;
+
+	return (struct pm_mailbox *)mailbox_base;
+}
+
+#if TRACE_LEVEL >= TRACE_DEBUG
+static void __maybe_unused dump_context(void)
+{
+	struct pm_mailbox *mailbox = get_pm_mailbox();
+	struct retram_resume_ctx *ctx = get_retram_resume_ctx();
+
+	stm32_clock_enable(RTCAPB);
+
+	DMSG("Backup registers: address 0x%" PRIx32 ", magic 0x%" PRIx32,
+		*(uint32_t *)stm32mp_bkpreg(BCKR_CORE1_BRANCH_ADDRESS),
+		*(uint32_t *)stm32mp_bkpreg(BCKR_CORE1_MAGIC_NUMBER));
+
+	stm32_clock_disable(RTCAPB);
+
+	stm32_clock_enable(BKPSRAM);
+
+	DMSG("BKPSRAM mailbox:  0x%" PRIx32 ", zd 0x%" PRIx32 ", ep 0x%" PRIx32,
+		mailbox->magic, mailbox->zq0cr0_zdata,
+		mailbox->core0_resume_ep);
+
+	DMSG("BKPSRAM context:  teeram backup @%" PRIx32 ", resume @0x%" PRIx32,
+		ctx->teeram_bkp_pa, ctx->resume_pa);
+
+	stm32_clock_disable(BKPSRAM);
+}
+#else
+static void __maybe_unused dump_context(void)
+{
+}
+#endif
+
+/*
+ * Save and restore functions
+ */
+static void save_time(void)
+{
+        uintptr_t stgen = stm32_get_stgen_base();
+
+	plat_ctx.stgen_cnt_h = read32(stgen + CNTCVU_OFFSET);
+	plat_ctx.stgen_cnt_l = read32(stgen + CNTCVL_OFFSET);
+	if (plat_ctx.stgen_cnt_l < 10) {
+		plat_ctx.stgen_cnt_h = read32(stgen + CNTCVU_OFFSET);
+	}
+
+	stm32_rtc_get_calendar(&plat_ctx.rtc);
+}
+
+#if TRACE_LEVEL >= TRACE_DEBUG
+static void print_ccm_decryption_duration(void)
+{
+	uintptr_t stgen = stm32_get_stgen_base();
+	struct retram_resume_ctx *ctx = get_retram_resume_ctx();
+
+
+	stm32_clock_enable(BKPSRAM);
+
+	DMSG("CCM decryption duration %llums",
+		((unsigned long long)ctx->stgen_cnt * 1000) /
+		mmio_read_32(stgen + CNTFID_OFFSET));
+
+	stm32_clock_enable(BKPSRAM);
+}
+#else
+static void print_ccm_decryption_duration(void)
+{
+}
+#endif
+
+static void restore_time(void)
+{
+	struct stm32_rtc_calendar current_calendar;
+	unsigned long long stdby_time_in_ms;
+	unsigned long long cnt;
+	uintptr_t stgen = stm32_get_stgen_base();
+	struct retram_resume_ctx __maybe_unused *ctx = get_retram_resume_ctx();
+
+	stm32_rtc_get_calendar(&current_calendar);
+	stdby_time_in_ms = stm32_rtc_diff_calendar(&current_calendar,
+						   &plat_ctx.rtc);
+
+	cnt = ((uint64_t)plat_ctx.stgen_cnt_h << 32) | plat_ctx.stgen_cnt_l;
+	cnt += (stdby_time_in_ms * mmio_read_32(stgen + CNTFID_OFFSET)) / 1000U;
+
+	mmio_clrbits_32(stgen + CNTCR_OFFSET, CNTCR_EN);
+	mmio_write_32(stgen + CNTCVL_OFFSET, (uint32_t)cnt);
+	mmio_write_32(stgen + CNTCVU_OFFSET, (uint32_t)(cnt >> 32));
+	mmio_setbits_32(stgen + CNTCR_OFFSET, CNTCR_EN);
+
+	print_ccm_decryption_duration();
+}
+
+static bool __maybe_unused pm_cb_is_valid(void (*cb)(enum pm_op op, void *hdl),
+					  void *hdl)
+{
+	void *cb_voidp = (void *)(uintptr_t)cb;
+	paddr_t cb_phy = virt_to_phys(cb_voidp);
+	paddr_t hdl_phy = virt_to_phys(hdl);
+	bool __maybe_unused valid;
+
+	valid = (phys_to_virt(cb_phy, MEM_AREA_TEE_RAM_RX) == cb_voidp) &&
+		((phys_to_virt(hdl_phy, MEM_AREA_TEE_RAM_RX) == hdl) ||
+		 (phys_to_virt(hdl_phy, MEM_AREA_TEE_RAM_RO) == hdl) ||
+		 (phys_to_virt(hdl_phy, MEM_AREA_TEE_RAM_RW) == hdl));
+
+	if (!valid) {
+		EMSG("pm_cb mandates unpaged arguments %p %p", cb_voidp, hdl);
+	}
+
+	return valid;
+}
+
+struct pm_cb {
+	void (*callback)(enum pm_op op, void *handle);
+	void *handle;
+};
+static struct pm_cb *pm_cb;
+static size_t pm_cb_count;
+
+void stm32mp_register_pm_cb(void (*callback)(enum pm_op op, void *handle),
+			    void *handle)
+{
+	assert(pm_cb_is_valid(callback, handle));
+
+	pm_cb_count++;
+	pm_cb = realloc(pm_cb, sizeof(struct pm_cb) * pm_cb_count);
+	if (!pm_cb){
+		panic();
+	}
+
+	pm_cb[pm_cb_count - 1].callback = callback;
+	pm_cb[pm_cb_count - 1].handle = handle;
+}
+
+static void save_soc_context(void)
+{
+	const enum pm_op suspend = PM_OP_SUSPEND;
+	size_t n;
+
+	for (n = 0; n < pm_cb_count; n++) {
+		pm_cb[n].callback(suspend, pm_cb[n].handle);
+	}
+
+	/* Suspend core services */
+	stm32mp_gic_suspend_resume(suspend);
+	stm32mp_clock_suspend_resume(suspend);
+}
+
+static void restore_soc_context(void)
+{
+	const enum pm_op resume = PM_OP_RESUME;
+	size_t n;
+
+	/* Resume core services */
+	stm32mp_gic_suspend_resume(resume);
+	stm32mp_clock_suspend_resume(resume);
+
+	for (n = 0; n < pm_cb_count; n++) {
+		pm_cb[n].callback(resume, pm_cb[n].handle);
+	}
+}
+
+uintptr_t stm32mp_pm_retram_resume_ep(void)
+{
+	struct retram_resume_ctx *ctx = get_retram_resume_ctx();
+
+	return (uintptr_t)&ctx->resume_sequence;
+}
+
+/* Clear the content of the PM mailbox */
+void stm32mp_pm_wipe_context(void)
+{
+	struct retram_resume_ctx *ctx = get_retram_resume_ctx();
+	struct pm_mailbox *mailbox = get_pm_mailbox();
+
+	stm32_clock_enable(BKPSRAM);
+
+	memset(ctx, 0xa5, sizeof(*ctx));
+	memset(mailbox, 0xa5, sizeof(*mailbox));
+
+	stm32_clock_disable(BKPSRAM);
+}
+
+static struct mobj *teeram_bkp_mobj;
+
+static void init_retram_resume_resources(void)
+{
+	struct retram_resume_ctx *ctx = get_retram_resume_ctx();
+	const size_t size = (uintptr_t)stm32mp_bkpsram_image_end -
+			    (uintptr_t)stm32mp_bkpsram_resume;
+	paddr_t __maybe_unused pa;
+
+	COMPILE_TIME_ASSERT(sizeof(struct pm_mailbox) <
+			    BKPSRAM_PM_MAILBOX_SIZE);
+	COMPILE_TIME_ASSERT(sizeof(struct retram_resume_ctx) <
+			    BKPSRAM_PM_CONTEXT_SIZE);
+	assert((sizeof(*ctx) + size) < BKPSRAM_PM_CONTEXT_SIZE);
+
+	teeram_bkp_mobj = mobj_mm_alloc(mobj_sec_ddr, TEE_RAM_PH_SIZE,
+					&tee_mm_sec_ddr);
+	if (teeram_bkp_mobj == NULL) {
+		panic();
+	}
+	assert((mobj_get_va(teeram_bkp_mobj, 0) != NULL) &&
+	       (mobj_get_pa(teeram_bkp_mobj, 0, 0, &pa) == 0));
+
+	stm32_clock_enable(BKPSRAM);
+	memset(ctx, 0, sizeof(*ctx));
+	stm32_clock_disable(BKPSRAM);
+}
+
+/*
+ * When returning from STANDBY, the 64 first bytes of DDR will be overwritten
+ * during DDR DQS training. This area must then be saved before going to
+ * standby in the PM mailbox with the earlier boot stages.
+ */
+static void save_ddr_training_area(void)
+{
+	struct pm_mailbox *mailbox = get_pm_mailbox();
+	size_t size = sizeof(mailbox->ddr_training_backup);
+	struct mobj __maybe_unused *mobj = NULL;
+	paddr_t pa = DDR_BASE;
+	void *va = phys_to_virt(pa, MEM_AREA_RAM_NSEC);
+
+#if !defined(CFG_STM32MP_MAP_NSEC_LOW_DDR)
+	/* Config switch helps not requesting mobj_mapped_shm_alloc() unpaged */
+	if (!va) {
+		mobj = mobj_mapped_shm_alloc(&pa, SMALL_PAGE_SIZE, 0, 0);
+		va = mobj_get_va(mobj, 0);
+	}
+#endif
+
+	memcpy(&mailbox->ddr_training_backup[0], va, size);
+
+	mobj_free(mobj);
+}
+
+static void load_earlyboot_pm_mailbox(void)
+{
+	struct pm_mailbox *mailbox = get_pm_mailbox();
+
+	COMPILE_TIME_ASSERT(sizeof(struct pm_mailbox) <
+			    BKPSRAM_PM_MAILBOX_SIZE);
+
+	assert(stm32_clock_is_enabled(BKPSRAM));
+
+	memset(mailbox, 0, sizeof(*mailbox));
+
+	mailbox->zq0cr0_zdata = get_ddrphy_calibration();
+
+	save_ddr_training_area();
+}
+
+#ifdef CFG_STM32_RNG
+/*
+ * CRYP relies on standard format for CCM IV/B0/CRT0 data. Our sequence uses
+ * no AAD, 4 bytes to encode the payload byte size and a 11 byte nonce.
+ */
+#define PM_CCM_Q			4
+#define PM_CCM_Q_FLAGS			(PM_CCM_Q - 1)
+#define PM_CCM_TAG_LEN			16
+#define PM_CCM_TAG_FLAGS		(((PM_CCM_TAG_LEN - 2) / 2) << 3)
+
+static void save_teeram_in_ddr(void)
+{
+	struct retram_resume_ctx *ctx = get_retram_resume_ctx();
+	size_t __maybe_unused size = (uintptr_t)stm32mp_bkpsram_image_end -
+				     (uintptr_t)stm32mp_bkpsram_resume;
+	paddr_t pa;
+	struct ccm_unpg_ctx *ccm = &ctx->ccm_ctx;
+	void *teeram = phys_to_virt(TEE_RAM_START, MEM_AREA_ROM_SEC);
+	void *teeram_bkp = mobj_get_va(teeram_bkp_mobj, 0);
+
+	COMPILE_TIME_ASSERT(PM_CTX_CCM_KEY_SIZE == sizeof(ccm->key));
+	COMPILE_TIME_ASSERT(PM_CTX_CCM_CTR1_SIZE == sizeof(ccm->ctr1));
+	COMPILE_TIME_ASSERT(PM_CTX_CCM_B0_SIZE == sizeof(ccm->b0));
+	COMPILE_TIME_ASSERT(PM_CTX_CCM_CTR0_SIZE == sizeof(ccm->ctr0));
+	COMPILE_TIME_ASSERT(PM_CTX_CCM_TAG_SIZE == sizeof(ccm->tag));
+
+	assert(stm32_clock_is_enabled(BKPSRAM) &&
+	       stm32_clock_is_enabled(CRYP1));
+
+	memcpy(ctx->resume_sequence,
+	       (void *)(uintptr_t)stm32mp_bkpsram_resume, size);
+
+	memset(ctx, 0, sizeof(*ctx));
+	ctx->resume_pa = virt_to_phys((void *)(uintptr_t)stm32mp_sysram_resume);
+	if (mobj_get_pa(teeram_bkp_mobj, 0, 0, &pa)) {
+		panic();
+	}
+	ctx->teeram_bkp_pa = (uint32_t)pa;
+	ctx->cryp1_base = (uint32_t)phys_to_virt(CRYP1_BASE, MEM_AREA_IO_SEC);
+	ctx->rcc_base = (uint32_t)phys_to_virt(RCC_BASE, MEM_AREA_IO_SEC);
+	ctx->stgen_base = (uint32_t)phys_to_virt(STGEN_BASE, MEM_AREA_IO_SEC);
+
+	if (stm32_rng_read((uint8_t *)ccm->key, sizeof(ccm->key))) {
+		panic();
+	}
+
+	assert(((PM_CCM_TAG_FLAGS & ~0x38U) | (PM_CCM_Q_FLAGS & ~0x07U)) == 0);
+	COMPILE_TIME_ASSERT(PM_CCM_Q <= 4);
+	COMPILE_TIME_ASSERT(TEE_RAM_PH_SIZE > UINT16_MAX);
+	COMPILE_TIME_ASSERT(TEE_RAM_PH_SIZE < UINT32_MAX);
+
+	if (stm32_rng_read((uint8_t *)ccm->ctr1, sizeof(ccm->ctr1))) {
+		panic();
+	}
+	ccm->ctr1[0] &= GENMASK_32(24, 0);
+	memcpy(ccm->b0, ccm->ctr1, sizeof(ccm->b0));
+	memcpy(ccm->ctr0, ccm->ctr1, sizeof(ccm->ctr0));
+
+	ccm->ctr0[0] |= PM_CCM_Q_FLAGS << 24;
+	ccm->ctr0[3] = 0;
+	ccm->ctr1[0] |= PM_CCM_Q_FLAGS << 24;
+	ccm->ctr1[3] = 1;
+	ccm->b0[0] |= (PM_CCM_Q_FLAGS | PM_CCM_TAG_FLAGS) << 24;
+	ccm->b0[3] = TEE_RAM_PH_SIZE;
+
+	stm32mp_ccm_encrypt_teeram(ctx, teeram_bkp, teeram, TEE_RAM_PH_SIZE);
+	dcache_clean_range(teeram_bkp, TEE_RAM_PH_SIZE);
+
+	memcpy(ctx->ccm_ref_tag, ccm->tag, sizeof(ctx->ccm_ref_tag));
+
+	DMSG("CCM encryption duration %llums",
+		((unsigned long long)ctx->stgen_cnt * 1000) /
+		mmio_read_32(ctx->stgen_base + CNTFID_OFFSET));
+	ctx->stgen_cnt = 0;
+}
+#else
+static void save_teeram_in_ddr(void)
+{
+	panic("Mandates RNG support");
+}
+#endif /*CFG_STM32_RNG*/
+
+/* Finalize the PM mailbox now that everything is loaded */
+static void enable_pm_mailbox(unsigned int suspend)
+{
+	struct pm_mailbox *mailbox = get_pm_mailbox();
+	uint32_t magic = BOOT_API_A7_CORE0_MAGIC_NUMBER;
+	uint32_t hint = 0;
+
+	assert(stm32_clock_is_enabled(BKPSRAM) &&
+	       stm32_clock_is_enabled(RTCAPB));
+
+	if (suspend) {
+		hint = virt_to_phys(&get_retram_resume_ctx()->resume_sequence);
+	}
+
+	write32(magic, stm32mp_bkpreg(BCKR_CORE1_MAGIC_NUMBER));
+	write32(hint, stm32mp_bkpreg(BCKR_CORE1_BRANCH_ADDRESS));
+
+	mailbox->core0_resume_ep = hint;
+	mailbox->magic = STANDBY_CONTEXT_MAGIC;
+}
+
+static void gate_pm_context_clocks(bool enable)
+{
+	static bool clocks_enabled;
+
+	if (enable) {
+		assert(!clocks_enabled);
+		stm32_clock_enable(BKPSRAM);
+		stm32_clock_enable(RTCAPB);
+		stm32_clock_enable(CRYP1);
+		clocks_enabled = true;
+	} else {
+		/* Suspended TEE RAM state left the clocks enabled */
+		if (clocks_enabled) {
+			stm32_clock_disable(BKPSRAM);
+			stm32_clock_disable(RTCAPB);
+			stm32_clock_disable(CRYP1);
+			clocks_enabled = false;
+		}
+	}
+}
+
+/*
+ * Context (TEE RAM content + peripherals) must be restored
+ * only if system may reach STANDBY state.
+ */
+void stm32mp_pm_save_context(unsigned int soc_mode)
+{
+	save_time();
+
+	if (!need_to_backup_cpu_context(soc_mode)) {
+		return;
+	}
+
+	gate_pm_context_clocks(true);
+	load_earlyboot_pm_mailbox();
+	save_soc_context();
+	save_teeram_in_ddr();
+	enable_pm_mailbox(1);
+}
+
+void stm32mp_pm_restore_context(unsigned int soc_mode)
+{
+	if (need_to_backup_cpu_context(soc_mode)) {
+		restore_soc_context();
+		gate_pm_context_clocks(false);
+	}
+
+	restore_time();
+}
+
+void stm32mp_pm_shutdown_context(void)
+{
+	gate_pm_context_clocks(true);
+	load_earlyboot_pm_mailbox();
+	enable_pm_mailbox(0);
+	gate_pm_context_clocks(false);
+}
+
+static TEE_Result init_pm_support(void)
+{
+	init_retram_resume_resources();
+
+	stm32mp_pm_wipe_context();
+
+	return TEE_SUCCESS;
+}
+driver_init(init_pm_support);
diff --git a/core/arch/arm/plat-stm32mp1/pm/context.h b/core/arch/arm/plat-stm32mp1/pm/context.h
new file mode 100644
index 0000000..9070fac
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/pm/context.h
@@ -0,0 +1,100 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ */
+
+#ifndef __STM32MP_PM_CONTEXT_H__
+#define __STM32MP_PM_CONTEXT_H__
+
+#ifndef ASM
+#include <compiler.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stm32mp_pm.h>
+#endif
+
+#define PM_CTX_CCM_KEY_SIZE		32
+#define PM_CTX_CCM_CTR1_SIZE		16
+#define PM_CTX_CCM_B0_SIZE		16
+#define PM_CTX_CCM_CTR0_SIZE		16
+#define PM_CTX_CCM_TAG_SIZE		16
+
+#ifndef ASM
+/*
+ * All materials for the CCM sequence using CRYP support are preloaded
+ * in this specific structure. Note that the sequence does not use AAD.
+ *
+ * @key: AES key material buffer
+ * @ctr1: Preformatted 128bit CTR1 block
+ * @ctr1: Preformatted 128bit B0 block
+ * @ctr1: Preformatted 128bit CTR0 block
+ * @tag: Buffer where the generated CCM tag is stored
+ */
+struct ccm_unpg_ctx {
+	uint32_t key[PM_CTX_CCM_KEY_SIZE / sizeof(uint32_t)];
+	uint32_t ctr1[PM_CTX_CCM_CTR1_SIZE / sizeof(uint32_t)];
+	uint32_t b0[PM_CTX_CCM_B0_SIZE / sizeof(uint32_t)];
+	uint32_t ctr0[PM_CTX_CCM_CTR0_SIZE / sizeof(uint32_t)];
+	uint32_t tag[PM_CTX_CCM_TAG_SIZE / sizeof(uint32_t)];
+};
+
+/*
+ * This structure is used by pm_helpers.S at early resume from retention RAM.
+ * It is defined here and used by context_asm_defines.c to generate offset
+ * macros for the assembly implementation in pm_helpers.S.
+ *
+ * To lower the memory footprint of suspend sequence, The same function is
+ * used for encryption (executed from TEE RAM with MMU enabled) and for
+ * decryption (executed from BKPSRAM with MMU disabled). Therefore some
+ * required addresses are provided by the caller through this structure
+ * especially some SoC interface registers that are likely to have different
+ * physical and virtual addresses.
+ *
+ * @resume_pa: OP-TEE resume physical entry in TEE RAM (once restored)
+ * @teeram_bkp_pa: Physical base address in TEE RAM backup in DDR
+ * @cryp1_base: Base address of the CRYP1 registers (physical or virtual)
+ * @rcc_base: Base address of the RCC registers (physical or virtual)
+ * @stgen_base: Base address of the STGEN registers (physical or virtual)
+ * @stgen_cnt: STGEN cycle counter backup cell and measure of cycles spent
+ * @ccm_ref_tag: 128bit arrays storing tag generated during encryption
+ * @ccm_ctx: Structure storing CCM configuration and generated tag
+ * @resume_sequence: Code/data array for the BKPSRAM resume sequence
+ */
+struct retram_resume_ctx {
+	uint32_t resume_pa;
+	uint32_t teeram_bkp_pa;
+	uint32_t cryp1_base;
+	uint32_t rcc_base;
+	uint32_t stgen_base;
+	uint32_t stgen_cnt;
+	uint8_t ccm_ref_tag[PM_CTX_CCM_TAG_SIZE];
+	struct ccm_unpg_ctx ccm_ctx;
+	/* Last start the resume routine ARM (32bit) instructions sequence */
+	uint32_t resume_sequence[];
+};
+
+extern const uint8_t stm32mp_bkpsram_image_end[];
+void stm32mp_bkpsram_resume(void);
+void stm32mp_sysram_resume(void);
+
+void stm32mp_cpu_reset_state(void);
+
+void stm32mp_pm_save_context(unsigned int soc_mode);
+void stm32mp_pm_restore_context(unsigned int soc_mode);
+void stm32mp_pm_shutdown_context(void);
+void stm32mp_pm_wipe_context(void);
+
+int stm32mp1_set_pm_domain_state(enum stm32mp1_pm_domain domain, bool status);
+
+uint32_t stm32mp1_get_lp_soc_mode(uint32_t psci_mode);
+int stm32mp1_set_lp_deepest_soc_mode(uint32_t psci_mode, uint32_t soc_mode);
+
+uintptr_t stm32mp_pm_retram_resume_ep(void);
+
+int stm32mp_ccm_encrypt_teeram(struct retram_resume_ctx *ctx,
+			       void *dst, void *src, size_t size);
+int stm32mp_ccm_decrypt_teeram(struct retram_resume_ctx *ctx,
+			       void *dst, void *src, size_t size);
+#endif /*ASM*/
+
+#endif /*__STM32MP_PM_CONTEXT_H__*/
diff --git a/core/arch/arm/plat-stm32mp1/pm/context_asm_defines.c b/core/arch/arm/plat-stm32mp1/pm/context_asm_defines.c
new file mode 100644
index 0000000..34c797f
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/pm/context_asm_defines.c
@@ -0,0 +1,28 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2018, STMicroelectronics
+ * Copyright (c) 2018, Linaro Limited
+ */
+
+#include <gen-asm-defines.h>
+
+#include "context.h"
+
+#define OFFSET_OF_CTX_STRUCT(_f)	offsetof(struct retram_resume_ctx, _f)
+#define OFFSET_OF_CMM_CTX_STRUCT(_f)	(OFFSET_OF_CTX_STRUCT(ccm_ctx) + \
+					 offsetof(struct ccm_unpg_ctx, _f))
+DEFINES
+{
+	DEFINE(PM_CTX_RESUME_PA, OFFSET_OF_CTX_STRUCT(resume_pa));
+	DEFINE(PM_CTX_TEERAM_BKP_PA, OFFSET_OF_CTX_STRUCT(teeram_bkp_pa));
+	DEFINE(PM_CTX_CRYP1_BASE, OFFSET_OF_CTX_STRUCT(cryp1_base));
+	DEFINE(PM_CTX_RCC_BASE, OFFSET_OF_CTX_STRUCT(rcc_base));
+	DEFINE(PM_CTX_STGEN_BASE, OFFSET_OF_CTX_STRUCT(stgen_base));
+	DEFINE(PM_CTX_STGEN_CNT, OFFSET_OF_CTX_STRUCT(stgen_cnt));
+	DEFINE(PM_CTX_CCM_KEY, OFFSET_OF_CMM_CTX_STRUCT(key));
+	DEFINE(PM_CTX_CCM_CTR1, OFFSET_OF_CMM_CTX_STRUCT(ctr1));
+	DEFINE(PM_CTX_CCM_B0, OFFSET_OF_CMM_CTX_STRUCT(b0));
+	DEFINE(PM_CTX_CCM_CTR0, OFFSET_OF_CMM_CTX_STRUCT(ctr0));
+	DEFINE(PM_CTX_CCM_TAG, OFFSET_OF_CMM_CTX_STRUCT(tag));
+	DEFINE(PM_CTX_CCM_REF_TAG, OFFSET_OF_CTX_STRUCT(ccm_ref_tag));
+}
diff --git a/core/arch/arm/plat-stm32mp1/pm/low_power.c b/core/arch/arm/plat-stm32mp1/pm/low_power.c
new file mode 100644
index 0000000..bd28014
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/pm/low_power.c
@@ -0,0 +1,431 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <arm.h>
+#include <assert.h>
+#include <boot_api.h>
+#include <console.h>
+#include <drivers/gic.h>
+#include <drivers/stm32_iwdg.h>
+#include <drivers/stm32_rtc.h>
+#include <drivers/stm32mp1_ddrc.h>
+#include <drivers/stm32mp1_pmic.h>
+#include <drivers/stm32mp1_pwr.h>
+#include <drivers/stm32mp1_rcc.h>
+#include <drivers/stpmic1.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <dt-bindings/power/stm32mp1-power.h>
+#include <initcall.h>
+#include <io.h>
+#include <keep.h>
+#include <kernel/cache_helpers.h>
+#include <kernel/delay.h>
+#include <kernel/misc.h>
+#include <kernel/panic.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <stdbool.h>
+#include <stm32mp_pm.h>
+#include <stm32_util.h>
+#include <trace.h>
+
+#include "context.h"
+#include "power.h"
+
+static uint8_t gicd_rcc_wakeup;
+static uint8_t gicc_pmr;
+static bool ddr_in_selfrefresh;
+
+struct pwr_lp_config {
+	uint32_t pwr_cr1;
+	uint32_t pwr_mpucr;
+	const char *regul_suspend_node_name;
+};
+
+#define PWR_CR1_MASK	(PWR_CR1_LPDS | PWR_CR1_LPCFG | PWR_CR1_LVDS)
+#define PWR_MPUCR_MASK	(PWR_MPUCR_CSTDBYDIS | PWR_MPUCR_CSSF | PWR_MPUCR_PDDS)
+
+static const struct pwr_lp_config config_pwr[STM32_PM_MAX_SOC_MODE] = {
+	[STM32_PM_CSLEEP_RUN] = {
+		.pwr_cr1 = 0U,
+		.pwr_mpucr = 0U,
+		.regul_suspend_node_name = NULL,
+	},
+	[STM32_PM_CSTOP_ALLOW_STOP] = {
+		.pwr_cr1 = 0U,
+		.pwr_mpucr = PWR_MPUCR_CSTDBYDIS | PWR_MPUCR_CSSF,
+		.regul_suspend_node_name = NULL,
+	},
+	[STM32_PM_CSTOP_ALLOW_LP_STOP] = {
+		.pwr_cr1 = PWR_CR1_LPDS,
+		.pwr_mpucr = PWR_MPUCR_CSTDBYDIS | PWR_MPUCR_CSSF,
+		.regul_suspend_node_name = "lp-stop",
+	},
+	[STM32_PM_CSTOP_ALLOW_LPLV_STOP] = {
+		.pwr_cr1 = PWR_CR1_LVDS | PWR_CR1_LPDS | PWR_CR1_LPCFG,
+		.pwr_mpucr = PWR_MPUCR_CSTDBYDIS | PWR_MPUCR_CSSF,
+		.regul_suspend_node_name = "lplv-stop",
+	},
+	[STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR] = {
+		.pwr_cr1 = 0U,
+		.pwr_mpucr = PWR_MPUCR_CSTDBYDIS | PWR_MPUCR_CSSF |
+			PWR_MPUCR_PDDS,
+		.regul_suspend_node_name = "standby-ddr-sr",
+	},
+	[STM32_PM_CSTOP_ALLOW_STANDBY_DDR_OFF] = {
+		.pwr_cr1 = 0U,
+		.pwr_mpucr = PWR_MPUCR_CSTDBYDIS | PWR_MPUCR_CSSF |
+			PWR_MPUCR_PDDS,
+		.regul_suspend_node_name = "standby-ddr-off",
+	},
+	[STM32_PM_SHUTDOWN] = {
+		.pwr_cr1 = 0U,
+		.pwr_mpucr = 0U,
+		.regul_suspend_node_name = "standby-ddr-off",
+	},
+};
+
+static void set_rcc_it_priority(uint8_t *it_prio, uint8_t *pmr)
+{
+	*it_prio = itr_set_ipriority(RCC_WAKEUP_IT, GIC_HIGHEST_SEC_PRIORITY);
+	*pmr = itr_set_pmr(STM32MP_GIC_PRIORITY_CSTOP);
+}
+
+static void restore_rcc_it_priority(uint8_t it_prio, uint8_t pmr)
+{
+	(void)itr_set_ipriority(RCC_WAKEUP_IT, it_prio);
+	(void)itr_set_pmr(pmr);
+}
+
+static void stm32_apply_pmic_suspend_config(uint32_t mode)
+{
+	if (stm32mp_with_pmic()) {
+		const char *name = config_pwr[mode].regul_suspend_node_name;
+
+		assert(mode < ARRAY_SIZE(config_pwr));
+		stm32mp_get_pmic();
+		stm32mp_pmic_apply_lp_config(name);
+		stm32mp_pmic_apply_boot_on_config(); // ??? should be done at wakeup only?
+		stm32mp_put_pmic();
+	}
+}
+
+#define CONSOLE_FLUSH_DELAY_MS		10
+
+#if TRACE_LEVEL >= TRACE_DEBUG
+static void wait_console_flushed(void)
+{
+	console_flush();
+	mdelay(CONSOLE_FLUSH_DELAY_MS);
+}
+#else
+static void wait_console_flushed(void)
+{
+}
+#endif
+
+static void cpu_wfi(void)
+{
+	dsb();
+	isb();
+	wfi();
+}
+
+void stm32_pm_cpu_wfi(void)
+{
+	wait_console_flushed();
+	cpu_wfi();
+}
+
+/*If IWDG is not supported, provide a stubbed weak watchdog kicker */
+void __weak stm32_iwdg_refresh(uint32_t __unused instance)
+{
+}
+
+/*
+ * stm32_enter_cstop - Prepare CSTOP mode
+ *
+ * @mode - Target low power mode
+ * Return 0 if succeed to suspend, non 0 else.
+ */
+int stm32_enter_cstop(uint32_t mode)
+{
+	uint32_t pwr_cr1 = config_pwr[mode].pwr_cr1;
+	uintptr_t pwr_base = stm32_pwr_base();
+	uintptr_t rcc_base = stm32_rcc_base();
+	int rc;
+
+	stm32_apply_pmic_suspend_config(mode);
+
+	if (stm32mp_with_pmic() && (mode == STM32_PM_CSTOP_ALLOW_LP_STOP)) {
+		pwr_cr1 |= PWR_CR1_LPCFG;
+	}
+
+	/* Workaround for non secure cache issue: this should not be needed */
+	dcache_op_all(DCACHE_OP_CLEAN_INV);
+
+	/* Clear RCC interrupt before enabling it */
+	mmio_setbits_32(rcc_base + RCC_MP_CIFR, RCC_MP_CIFR_WKUPF);
+
+	/* Enable RCC Wake-up */
+	mmio_setbits_32(rcc_base + RCC_MP_CIER, RCC_MP_CIFR_WKUPF);
+
+	/* Configure low power mode */
+	mmio_clrsetbits_32(pwr_base + PWR_MPUCR_OFF, PWR_MPUCR_MASK,
+			   config_pwr[mode].pwr_mpucr);
+	mmio_clrsetbits_32(pwr_base + PWR_CR1_OFF, PWR_CR1_MASK, pwr_cr1);
+
+	/* Clear RCC pending interrupt flags */
+	mmio_write_32(rcc_base + RCC_MP_CIFR, RCC_MP_CIFR_MASK);
+
+	/* Request CSTOP mode to RCC */
+	mmio_setbits_32(rcc_base + RCC_MP_SREQSETR,
+			RCC_MP_SREQSETR_STPREQ_P0 | RCC_MP_SREQSETR_STPREQ_P1);
+
+	stm32_iwdg_refresh(IWDG2_INST);
+
+	set_rcc_it_priority(&gicd_rcc_wakeup, &gicc_pmr);
+
+	rc = ddr_standby_sr_entry(NULL);
+
+	ddr_in_selfrefresh = (rc == 0);
+
+	return rc;
+}
+
+/*
+ * stm32_exit_cstop - Exit from CSTOP mode
+ */
+void stm32_exit_cstop(void)
+{
+	uintptr_t rcc_base = stm32_rcc_base();
+
+	if (ddr_in_selfrefresh) {
+		if (ddr_sw_self_refresh_exit() != 0) {
+			panic();
+		}
+		ddr_in_selfrefresh = false;
+	}
+
+	restore_rcc_it_priority(gicd_rcc_wakeup, gicc_pmr);
+
+	/* Disable STOP request */
+	mmio_setbits_32(rcc_base + RCC_MP_SREQCLRR,
+			RCC_MP_SREQSETR_STPREQ_P0 | RCC_MP_SREQSETR_STPREQ_P1);
+
+	/* Disable RCC Wake-up */
+	mmio_clrbits_32(rcc_base + RCC_MP_CIER, RCC_MP_CIFR_WKUPF);
+
+	dsb();
+	isb();
+}
+
+static void __noreturn reset_cores(void)
+{
+	uintptr_t rcc_base = stm32_rcc_base();
+	uint32_t reset_mask;
+	uint32_t target_mask;
+
+	if (get_core_pos() == 0) {
+		reset_mask = RCC_MP_GRSTCSETR_MPUP0RST;
+		target_mask = TARGET_CPU1_GIC_MASK;
+	} else {
+		reset_mask = RCC_MP_GRSTCSETR_MPUP1RST;
+		target_mask = TARGET_CPU0_GIC_MASK;
+	}
+
+	itr_raise_sgi(GIC_SEC_SGI_1, target_mask);
+	dcache_op_all(DCACHE_OP_CLEAN_INV);
+	write32(reset_mask, rcc_base + RCC_MP_GRSTCSETR);
+	cpu_wfi();
+	panic("Cores reset");
+}
+
+/*
+ * stm32_pm_cpus_reset - Reset only cpus
+ */
+void __noreturn stm32_cores_reset(void)
+{
+	reset_cores();
+}
+KEEP_PAGER(stm32_cores_reset);
+
+/*
+ * stm32_enter_cstop_shutdown - Shutdown CPUs to target low power mode
+ * @mode - Target low power mode
+ */
+void __noreturn stm32_enter_cstop_shutdown(uint32_t mode)
+{
+	switch (mode) {
+	case STM32_PM_SHUTDOWN:
+		if (stm32mp_with_pmic()) {
+			wait_console_flushed();
+			stm32mp_get_pmic();
+			stpmic1_switch_off();
+			udelay(100);
+		}
+		break;
+	case STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR:
+	case STM32_PM_CSTOP_ALLOW_STANDBY_DDR_OFF:
+#ifdef STM32MP1_USE_MPU0_RESET
+		stm32mp_pm_shutdown_context();
+		stm32_enter_cstop(mode);
+		cpu_wfi();
+		reset_cores();
+#else
+		if (stm32mp_with_pmic()) {
+			wait_console_flushed();
+			stm32mp_get_pmic();
+			stpmic1_switch_off();
+			udelay(100);
+		}
+#endif
+		break;
+	default:
+		break;
+	}
+
+	panic();
+}
+
+/*
+ * stm32_enter_cstop_reset - Reset CPUs to target low power mode
+ * @mode - Target low power mode
+ */
+void __noreturn stm32_enter_cstop_reset(uint32_t mode)
+{
+	uintptr_t rcc_base = stm32_rcc_base();
+
+	switch (mode) {
+	case STM32_PM_SHUTDOWN:
+		write32(RCC_MP_GRSTCSETR_MPSYSRST, rcc_base + RCC_MP_GRSTCSETR);
+		udelay(100);
+		break;
+	default:
+#ifdef STM32MP1_USE_MPU0_RESET
+		reset_cores();
+#else
+		IMSG("Forced system reset");
+		wait_console_flushed();
+		write32(RCC_MP_GRSTCSETR_MPSYSRST, rcc_base + RCC_MP_GRSTCSETR);
+		udelay(100);
+#endif
+		break;
+	}
+
+	panic();
+}
+
+/*
+ * stm32_enter_csleep - enter CSLEEP state while WFI and exit in CRUN
+ *
+ * Configure PWR for CSLEEP state. CPU shall execute a WFI and return
+ * once a interrupt is pending.
+ */
+void stm32_enter_csleep(void)
+{
+	uintptr_t pwr_base = stm32_pwr_base();
+
+	mmio_clrsetbits_32(pwr_base + PWR_MPUCR_OFF, PWR_MPUCR_MASK,
+			   config_pwr[STM32_PM_CSLEEP_RUN].pwr_mpucr);
+	mmio_clrsetbits_32(pwr_base + PWR_CR1_OFF, PWR_CR1_MASK,
+			   config_pwr[STM32_PM_CSLEEP_RUN].pwr_cr1);
+
+	stm32_pm_cpu_wfi();
+}
+
+/*
+ * Secure interrupts used in the low power sequences
+ */
+#define GICC_IAR		0x00C
+#define GICC_IAR_IT_ID_MASK	0x3ff
+#define GICC_EOIR		0x010
+
+/* RCC Wakeup interrupt is used to wake from suspeneded mode */
+static enum itr_return rcc_wakeup_it_handler(struct itr_handler *hdl __unused)
+{
+	/* This interrupt is not expected to be handled */
+	panic("RCC  wakeup interrupt");
+	return ITRR_HANDLED;
+}
+
+static struct itr_handler rcc_wakeup_handler = {
+	.it = RCC_WAKEUP_IT,
+	.handler = rcc_wakeup_it_handler,
+};
+KEEP_PAGER(rcc_wakeup_handler);
+
+/* SGI9 (secure SGI 1) informs targeted CPU it shall reset */
+static enum itr_return sgi9_it_handler(struct itr_handler *handler)
+{
+	uintptr_t rcc_base = stm32_rcc_base();
+	uint32_t reset_mask;
+	uintptr_t gicc_base = get_gicc_base();
+
+	write32(handler->it, gicc_base + GICC_EOIR);
+
+	if (get_core_pos() == 0) {
+		reset_mask = RCC_MP_GRSTCSETR_MPUP0RST;
+	} else {
+		reset_mask = RCC_MP_GRSTCSETR_MPUP1RST;
+	}
+
+	dcache_op_all(DCACHE_OP_CLEAN_INV);
+	write32(reset_mask, rcc_base + RCC_MP_GRSTCSETR);
+	cpu_wfi();
+	panic("Core reset");
+
+	return ITRR_HANDLED;
+}
+
+static struct itr_handler sgi9_reset_handler = {
+	.it = GIC_SEC_SGI_1,
+	.handler = sgi9_it_handler,
+};
+KEEP_PAGER(sgi9_reset_handler);
+
+static TEE_Result init_low_power(void)
+{
+	uintptr_t pwr_base = stm32_pwr_base();
+
+	itr_add(&rcc_wakeup_handler);
+	itr_enable(rcc_wakeup_handler.it);
+
+	itr_add(&sgi9_reset_handler);
+	itr_enable(sgi9_reset_handler.it);
+
+	/* Enable retention for BKPSRAM and BKPREG */
+	io_mask32(pwr_base + PWR_CR2_OFF,
+		  PWR_CR2_BREN | PWR_CR2_RREN, PWR_CR2_BREN | PWR_CR2_RREN);
+
+	return TEE_SUCCESS;
+}
+service_init(init_low_power);
+
+/*
+ * CPU low power sequences
+ */
+void __noreturn stm32_pm_cpu_power_down_wfi(void)
+{
+	if (get_core_pos() == 0) {
+		void (*reset_ep)(void) = stm32mp_sysram_resume;
+
+		wait_console_flushed();
+
+		dsb();
+		isb();
+		wfi();
+		/* STANDBY not reached: resume from retained SYSRAM */
+		stm32_exit_cstop();
+		stm32mp_cpu_reset_state();
+		reset_ep();
+		panic();
+	}
+
+	dcache_op_level1(DCACHE_OP_CLEAN);
+	write32(RCC_MP_GRSTCSETR_MPUP1RST, stm32_rcc_base() + RCC_MP_GRSTCSETR);
+	cpu_wfi();
+	panic();
+}
diff --git a/core/arch/arm/plat-stm32mp1/pm/pm_helpers.S b/core/arch/arm/plat-stm32mp1/pm/pm_helpers.S
new file mode 100644
index 0000000..91c7580
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/pm/pm_helpers.S
@@ -0,0 +1,635 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (c) 2018, STMicroelectronics
+ * Copyright (c) 2017 NXP
+ * Copyright (c) 2016-2017, ARM Limited and Contributors. All rights reserved.
+ */
+
+#include <arm.h>
+#include <arm32_macros.S>
+#include <asm.S>
+#include <boot_api.h>
+#include <drivers/stm32mp1_rcc.h>
+#include <generated/context_asm_defines.h>
+#include <kernel/cache_helpers.h>
+#include <kernel/tz_proc_def.h>
+#include <kernel/tz_ssvce_def.h>
+#include <kernel/unwind.h>
+#include <platform_config.h>
+#include <mm/core_mmu.h>
+#include <util.h>
+
+#include "context.h"
+
+/*
+ * Right bit shift distance to reach timeout from a 1s STGEN freq count
+ * Value N relates to 1000ms / 2^N, i.e 7 relates to 7.8125ms=~8ms
+ */
+#define CCM_TIMEOUT_128MS		2
+#define CCM_TIMEOUT_8MS			7
+#define CCM_TIMEOUT_1MS			10
+#define CCM_TIMEOUT_16US		16
+#define CCM_TIMEOUT			CCM_TIMEOUT_8MS
+
+/*
+ * CRYP interface register used for AES CCM
+ */
+#define CRYP_CR				0x000
+#define CRYP_SR				0x004
+#define CRYP_DIN			0x008
+#define CRYP_DOUT			0x00c
+#define CRYP_KEYR_BASE			0x020
+#define CRYP_IVR_BASE			0x040
+
+#define CRYP_CR_ALGODIR_DECRYPT		BIT(2)
+#define CRYP_CR_ALGOMODE_MASK		(BIT(19) | GENMASK_32(5, 3))
+#define CRYP_CR_ALGOMODE(m)		(((m & BIT(3)) << 16) | (m & 0x7) << 3)
+#define ALGOMODE_AES_CCM		0x9
+#define CRYP_CR_DATATYPE_SHIFT		6
+#define CRYP_CR_DATATYPE_8BIT		(2 << CRYP_CR_DATATYPE_SHIFT)
+#define CRYP_CR_KEYSIZE_SHIFT		8
+#define CRYP_CR_KEYSIZE_256BIT		(2U << CRYP_CR_KEYSIZE_SHIFT)
+#define CRYP_CR_CRYPEN			BIT(15)
+#define CRYP_CR_FFLUSH			BIT(14)
+#define CRYP_CR_GCM_CCMPH_SHIFT		16
+#define CRYP_CR_PHASE_MASK		(0x3 << CRYP_CR_GCM_CCMPH_SHIFT)
+#define CRYP_CR_INIT_PHASE		(0 << CRYP_CR_GCM_CCMPH_SHIFT)
+#define CRYP_CR_HEADER_PHASE		(1 << CRYP_CR_GCM_CCMPH_SHIFT)
+#define CRYP_CR_PAYLOAD_PHASE		(2 << CRYP_CR_GCM_CCMPH_SHIFT)
+#define CRYP_CR_FINAL_PHASE		(3 << CRYP_CR_GCM_CCMPH_SHIFT)
+
+#define CRYP_SR_BUSY			BIT(4)
+#define CRYP_SR_OFFU			BIT(3)
+#define CRYP_SR_OFNE			BIT(2)
+#define CRYP_SR_IFNF			BIT(1)
+#define CRYP_SR_IFEM			BIT(0)
+
+	/* Bound of the binary image loaded in retained memory */
+	.global stm32mp_bkpsram_image_end
+
+/*
+ * stm32mp_bkpsram_resume - Restore TEE RAM from backup memory and resume into
+ *
+ * This function executes at early resume from suspend state. It is the
+ * entrypoint of the OP-TEE provided to early boot stage when SoC wakes.
+ * This code is located in a retained memory, MMU disabled. This function
+ * shall restore TEE RAM content for OP-TEE to resume execution. Once
+ * TEE RAM is restored, this function branches to the resident resume entry
+ * point in TEE_RAM. This function and its resources shall execute in place.
+ */
+FUNC stm32mp_bkpsram_resume , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	/*
+	 * Almost all sequences here expect PM context structure base address
+	 * from CPU register r11.
+	 */
+	mov_imm	r11, (BKPSRAM_BASE + BKPSRAM_PM_CONTEXT_OFFSET)
+
+	/* stm32mp_ccm_teeram needs some HW interface base addresss */
+	mov_imm r0, CRYP1_BASE
+	str	r0, [r11, #PM_CTX_CRYP1_BASE]
+	mov_imm r0, RCC_BASE
+	str	r0, [r11, #PM_CTX_RCC_BASE]
+	mov_imm r0, STGEN_BASE
+	str	r0, [r11, #PM_CTX_STGEN_BASE]
+
+	bl	_clear_early_mailbox
+	bl	_prepare_time
+
+	mov_imm r0, TEE_RAM_START
+	ldr	r1, [r11, #PM_CTX_TEERAM_BKP_PA]
+	mov_imm r2, TEE_RAM_PH_SIZE
+	mov_imm r3, 1
+	bl	stm32mp_ccm_teeram
+	cmp	r0, #0
+	bne	_failed
+
+	/* Compare the generated and reference tags */
+	add	r8, r11, #PM_CTX_CCM_TAG
+	add	r9, r11, #PM_CTX_CCM_REF_TAG
+	ldm	r8, {r2-r5}
+	ldm	r9, {r6-r9}
+	mov	r0, #0
+	cmp	r2, r6
+	addeq	r0, #1
+	cmp	r3, r7
+	addeq	r0, #1
+	cmp	r4, r8
+	addeq	r0, #1
+	cmp	r5, r9
+	addeq	r0, #1
+	cmp	r0, #4
+	bne	_failed
+	bl	_save_resume_time
+
+	/* Resume into the restored TEE RAM */
+	ldr	r1, [r11, #PM_CTX_RESUME_PA]
+	bx	r1
+
+_failed:
+	/* Clear context including key and reference tag */
+	mov	r0, #0xa5
+	mov_imm	r12, BKPSRAM_PM_CONTEXT_SIZE
+	add	r12, r11, r12
+1:	str	r0, [r11], #4
+	cmp	r11, r12
+	blt	1b
+	b	.
+
+	/*
+	 * _clear_early_mailbox - Wipe mailbox in case of reset
+	 *
+	 * Sratches r0-r4.
+	 * All other CPU registers are preserved.
+	 */
+_clear_early_mailbox:
+	/* Clear the backup registers (first enable RTCAPB clock) */
+	mov_imm r0, (RCC_BASE + RCC_MP_APB5ENSETR)
+	mov_imm	r2, RCC_MP_APB5ENSETR_RTCAPBEN
+	ldr	r1, [r0]
+	ands	r1, r1, r2
+	moveq	r1, r2
+	movne	r1, #0
+	str	r2, [r0]
+	mov_imm	r2, (TAMP_BASE + TAMP_BKP_REGISTER_OFF)
+	mov_imm	r3, (BCKR_CORE1_MAGIC_NUMBER * 4)
+	mov_imm	r4, BOOT_API_A7_RESET_MAGIC_NUMBER
+	str	r4, [r2, r3]
+	mov_imm	r3, (BCKR_CORE1_BRANCH_ADDRESS * 4)
+	mov	r4, #0
+	str	r4, [r2, r3]
+	/* Restore RTCAPB clock initial state */
+	str	r1, [r0, #RCC_MP_ENCLRR_OFFSET]
+	bx	lr
+
+	/*
+	 * prepare_time - save/reset cycle counter to prevent later overflow
+	 *
+	 * Save current 32bit lower counter and reset to 0 so that later
+	 * timeout test do not need to care about overflow.
+	 *
+	 * Expects r11 is context base and lr is return address.
+	 * Scrathes r0-r2.
+	 * All other CPU registers are preserved.
+	 */
+_prepare_time:
+	ldr	r2, [r11, #PM_CTX_STGEN_BASE]
+	/* Disable STGEN counter */
+	ldr	r1, [r2, #CNTCR_OFFSET]
+	bic	r1, r1, #CNTCR_EN
+	str	r1, [r2, #CNTCR_OFFSET]
+1:	ldr	r1, [r2, #CNTSR_OFFSET]
+	tst	r1, #CNTCR_EN
+	bne	1b
+	/* Save and reset STGEN counter */
+	ldr	r0, [r2, #CNTCVL_OFFSET]
+	str	r0, [r11, #PM_CTX_STGEN_CNT]
+	mov	r0, #0
+	str	r0, [r2, #CNTCVL_OFFSET]
+	ldr	r0, [r2, #CNTCVU_OFFSET]
+	str	r0, [r2, #CNTCVU_OFFSET]
+	/* Enable STGEN counter */
+	ldr	r1, [r2, #CNTCR_OFFSET]
+	orr	r1, r1, #CNTCR_EN
+	str	r1, [r2, #CNTCR_OFFSET]
+	bx	lr
+
+	/*
+	 * save_resume_time - save time spent and restore STGEN cycle counter
+	 *
+	 * Restore STGEN counter to initial value incremented by the current
+	 * count. Note 32bit upper may need to be incremented.
+	 *
+	 * Expects r11 is context base and lr is return address.
+	 * Scrathes r0-r3.
+	 * All other CPU registers are preserved.
+	 */
+_save_resume_time:
+	/* Compute update STGEN counter 32bit LSB value */
+	ldr	r2, [r11, #PM_CTX_STGEN_BASE]
+	ldr	r0, [r11, #PM_CTX_STGEN_CNT]
+	ldr	r3, [r2, #CNTCVL_OFFSET]
+	str	r3, [r11, #PM_CTX_STGEN_CNT]
+	adds	r0, r0, r3
+	/* Disable STGEN  */
+	ldr	r1, [r2, #CNTCR_OFFSET]
+	bic	r1, r1, #CNTCR_EN
+	str	r1, [r2, #CNTCR_OFFSET]
+1:	ldr	r1, [r2, #CNTSR_OFFSET]
+	tst	r1, #CNTCR_EN
+	bne	1b
+	/* Update counter (increment 32bit MSB if requried) */
+	str	r0, [r2, #CNTCVL_OFFSET]
+	ldr	r0, [r2, #CNTCVU_OFFSET]
+	addcs	r0, r0, #1
+	str	r0, [r2, #CNTCVU_OFFSET]	/* Write CNTCVU value ... */
+	ldr	r0, [r2, #CNTCVU_OFFSET]	/* ... and wait it is set */
+	/* Enable STGEN  */
+	ldr	r0, [r2, #CNTCR_OFFSET]
+	orr	r0, r0, #CNTCR_EN
+	str	r0, [r2, #CNTCR_OFFSET]
+	bx	lr
+
+	/*
+	 * _setup_cryp1 - Enable CRYP1 hardware: reset & clock
+	 * _reset_cryp1 - Reset CRYP1 hardware
+	 *
+	 * Function call before and after CCM sequence. Note that the CRYP1
+	 * clock remain enabled. It is disabled later by the resume sequence.
+	 *
+	 * Expects r11 is context base and lr is return address.
+	 * Scratches r0-r3.
+	 */
+_setup_cryp1:
+	ldr	r1, [r11, #PM_CTX_RCC_BASE]
+	mov_imm r0, RCC_MP_AHB5ENSETR_CRYP1EN
+	str	r0, [r1, #RCC_MP_AHB5ENSETR]
+	/* Intentionnally fall through reset_cryp1 */
+_reset_cryp1:
+	ldr	r3, [r11, #PM_CTX_RCC_BASE]
+	mov_imm r0, RCC_AHB5RSTSETR_CRYP1RST
+	str	r0, [r3, #RCC_AHB5RSTSETR]
+1:	ldr	r1, [r3, #RCC_AHB5RSTSETR]
+	ands	r1, r1, r0
+	beq	1b
+	mov_imm r0, RCC_AHB5RSTSETR_CRYP1RST
+	str	r0, [r3, #RCC_AHB5RSTCLRR]
+1:	ldr	r1, [r3, #RCC_AHB5RSTSETR]
+	ands	r1, r1, r0
+	bne	1b
+	bx	lr
+
+	/*
+	 * _ccm_arm_8ms_timeout - Init 8ms threshold for _ccm_failed_on_timeout
+	 * _ccm_fail_on_timeout - Check STGEN counter against timeout threshold
+	 *
+	 * These function are used by the macro wait_flag_timeout_8ms. The
+	 * former loads the timeout in CPU register r0 while the later get the
+	 * timeout counter threshold from CPU register r0.
+	 *
+	 * Expect r11 is context base and lr is return address.
+	 * Scratch r0-r1.
+	 * All other CPU registers are preserved.
+	 */
+_ccm_arm_8ms_timeout:
+	ldr	r1, [r11, #PM_CTX_STGEN_BASE]
+	ldr	r0, [r1, #CNTFID_OFFSET]
+	lsrs	r0, r0, #CCM_TIMEOUT
+	moveq	r0, #1
+	ldr	r1, [r1, #CNTCVL_OFFSET]
+	adds	r0, r0, r1
+	bcs	_ccm_failed
+	bx	lr
+
+_ccm_fail_on_timeout:
+	ldr	r1, [r11, #PM_CTX_STGEN_BASE]
+	ldr	r1, [r1, #CNTCVL_OFFSET]
+	cmp	r1, r0
+	bge	_ccm_failed
+	bx	lr
+
+	/*
+	 * Macro WAIT_FLAG_TIMEOUT compares timeout threshold (r0) with
+	 * current time and branches the CCM failure entry on timeout.
+	 * It is assumed the 32bit timestamps cannot overflow.
+	 */
+	.macro WAIT_FLAG_TIMEOUT register_offset, bit_mask, awaited_mask
+		bl	_ccm_arm_8ms_timeout
+	1:
+		bl	_ccm_fail_on_timeout
+		ldr	r1, [r10, #(\register_offset)]
+		and	r1, r1, #(\bit_mask)
+		cmp	r1, #(\awaited_mask)
+		bne	1b
+	.endm
+
+/*
+ * stm32mp_ccm_teeram - Size optimzed unpaged CCM encryption/decryption
+ *
+ * This sequence encrypts or decrypts a input block using AES CCM with a
+ * 256bit key and no AAD and generates the CCM tag. The key, CTR0, CTR1
+ * and B0 block are read from PM context structure. The generated tag is
+ * stored in the PM context structure.
+ *
+ * This function is executed from TEE RAM during suspend sequence to generate
+ * the encrypted data and the tag. This function is also executed from BKPSRAM
+ * called with MMU disabled. Therefore this sequence shall be comply with
+ * position independent code constraints.
+ *
+ * Expects at entry:
+ * lr = caller return address
+ * r11 = retram_resume_ctx structure base address
+ * r0 = Destination buffer for the output data (ciphertext or plaintext)
+ * r1 = Source buffer for the input data (plaintext or ciphertext)
+ * r2 = Input (and output) data size in bytes
+ * r3 = 1 if decrypting, 0 if encrypting
+ */
+stm32mp_ccm_teeram:
+	/*
+	 * Use of the CPU registers in the whole stm32mp_ccm_teeram sequence
+	 *
+	 * sp: preserved, not used
+	 * lr: scratch register used to call subroutines.
+	 * r12: saves the caller link register for final return
+	 * r11: context from BKPSRAM
+	 * r10: CRYP1 base address
+	 * r9: destination buffer
+	 * r8: source buffer to cipher
+	 * r7: data byte counter
+	 * r0-r6 are scratch registers
+	 */
+	mov	r12, lr
+	ldr	r10, [r11, #PM_CTX_CRYP1_BASE]
+	mov	r9, r0
+	mov	r8, r1
+	mov	r7, r2
+	mov	r6, r3
+
+	bl	_setup_cryp1
+
+	mov_imm	r0, (CRYP_CR_ALGOMODE(ALGOMODE_AES_CCM) | \
+		     CRYP_CR_DATATYPE_8BIT | CRYP_CR_FFLUSH | \
+		     CRYP_CR_KEYSIZE_256BIT)
+	cmp	r6, #0
+	orrne	r0, r0, #CRYP_CR_ALGODIR_DECRYPT
+	str	r0, [r10, #CRYP_CR]
+
+	/* Check data alignment (addresses and size) */
+	ands	r0, r7, #0x0F
+	bne	_ccm_failed
+	ands	r0, r8, #0x03
+	bne	_ccm_failed
+	ands	r0, r9, #0x03
+	bne	_ccm_failed
+
+	ldr	r0, [r11, #PM_CTX_CCM_KEY]
+	str	r0, [r10, #CRYP_KEYR_BASE]
+	ldr	r0, [r11, #(PM_CTX_CCM_KEY + 4)]
+	str	r0, [r10, #(CRYP_KEYR_BASE + 4)]
+	ldr	r0, [r11, #(PM_CTX_CCM_KEY + 8)]
+	str	r0, [r10, #(CRYP_KEYR_BASE + 8)]
+	ldr	r0, [r11, #(PM_CTX_CCM_KEY + 12)]
+	str	r0, [r10, #(CRYP_KEYR_BASE + 12)]
+	ldr	r0, [r11, #(PM_CTX_CCM_KEY + 16)]
+	str	r0, [r10, #(CRYP_KEYR_BASE + 16)]
+	ldr	r0, [r11, #(PM_CTX_CCM_KEY + 20)]
+	str	r0, [r10, #(CRYP_KEYR_BASE + 20)]
+	ldr	r0, [r11, #(PM_CTX_CCM_KEY + 24)]
+	str	r0, [r10, #(CRYP_KEYR_BASE + 24)]
+	ldr	r0, [r11, #(PM_CTX_CCM_KEY + 28)]
+	str	r0, [r10, #(CRYP_KEYR_BASE + 28)]
+
+	ldr	r0, [r11, #PM_CTX_CCM_CTR1]
+	str	r0, [r10, #CRYP_IVR_BASE]
+	ldr	r0, [r11, #(PM_CTX_CCM_CTR1 + 4)]
+	str	r0, [r10, #(CRYP_IVR_BASE + 4)]
+	ldr	r0, [r11, #(PM_CTX_CCM_CTR1 + 8)]
+	str	r0, [r10, #(CRYP_IVR_BASE + 8)]
+	ldr	r0, [r11, #(PM_CTX_CCM_CTR1 + 12)]
+	str	r0, [r10, #(CRYP_IVR_BASE + 12)]
+
+	/* Setup CRYP for the CCM Init Phase */
+	ldr	r0, [r10, #CRYP_CR]
+	orr	r0, r0, #(CRYP_CR_CRYPEN | CRYP_CR_INIT_PHASE)
+	str	r0, [r10, #CRYP_CR]
+	ldr	r0, [r10, #CRYP_CR]
+
+	ldr	r0, [r11, #PM_CTX_CCM_B0]
+	str	r0, [r10, #CRYP_DIN]
+	ldr	r0, [r11, #(PM_CTX_CCM_B0 + 4)]
+	str	r0, [r10, #CRYP_DIN]
+	ldr	r0, [r11, #(PM_CTX_CCM_B0 + 8)]
+	str	r0, [r10, #CRYP_DIN]
+	ldr	r0, [r11, #(PM_CTX_CCM_B0 + 12)]
+	str	r0, [r10, #CRYP_DIN]
+
+	WAIT_FLAG_TIMEOUT CRYP_CR, CRYP_CR_CRYPEN, 0
+
+	/* Setup CRYP for the CCM Payload phase */
+	ldr	r0, [r10, #CRYP_CR]
+	bic	r0, r0, #CRYP_CR_PHASE_MASK
+	orr	r0, r0, #CRYP_CR_PAYLOAD_PHASE
+	orr	r0, r0, #CRYP_CR_CRYPEN
+	str	r0, [r10, #CRYP_CR]
+	ldr	r0, [r10, #CRYP_CR]
+
+_next_block:
+	WAIT_FLAG_TIMEOUT CRYP_SR, CRYP_SR_IFEM, CRYP_SR_IFEM
+
+	/* Feed input data, r8 stores the current source buffer */
+	ldr	r0, [r8], #4
+	str	r0, [r10, #CRYP_DIN]
+	ldr	r0, [r8], #4
+	str	r0, [r10, #CRYP_DIN]
+	ldr	r0, [r8], #4
+	str	r0, [r10, #CRYP_DIN]
+	ldr	r0, [r8], #4
+	str	r0, [r10, #CRYP_DIN]
+
+	WAIT_FLAG_TIMEOUT CRYP_SR, CRYP_SR_OFNE, CRYP_SR_OFNE
+
+	/* Store output data, r9 stores the current source buffer */
+	ldr	r0, [r10, #CRYP_DOUT]
+	str	r0, [r9], #4
+	ldr	r0, [r10, #CRYP_DOUT]
+	str	r0, [r9], #4
+	ldr	r0, [r10, #CRYP_DOUT]
+	str	r0, [r9], #4
+	/* Before last 32bit word, the output FIFO shall not be empty */
+	ldr	r0, [r10, #CRYP_SR]
+	ands	r0, r0, #CRYP_SR_OFNE
+	beq	_ccm_failed
+	/* After last 32bit word for this 128block, FIFO shall be empty */
+	ldr	r0, [r10, #CRYP_DOUT]
+	str	r0, [r9], #4
+	ldr	r0, [r10, #CRYP_SR]
+	ands	r0, r0, #CRYP_SR_OFNE
+	bne	_ccm_failed
+
+	/* Another round if remaining data */
+	subs	r7, r7, #16
+	bne	_next_block;
+
+	WAIT_FLAG_TIMEOUT CRYP_SR, CRYP_SR_BUSY, 0
+
+	/*
+	 * Data processing completed, now remains the tag generation.
+	 * Here expect SR[IFNF]=SR[OFNE]=1 and all others bits are 0.
+	 */
+	ldr	r0, [r10, #CRYP_SR]
+	cmp	r0, #(CRYP_SR_IFEM | CRYP_SR_IFNF)
+	bne	_ccm_failed
+
+	/* Setup CRYP1 for the CCM Final Phase */
+	ldr	r0, [r10, #CRYP_CR]
+	bic	r0, r0, #CRYP_CR_CRYPEN
+	str	r0, [r10, #CRYP_CR]
+	ldr	r0, [r10, #CRYP_CR]
+	bic	r0, r0, #CRYP_CR_PHASE_MASK
+	bic	r0, r0, #CRYP_CR_ALGODIR_DECRYPT
+	orr	r0, r0, #CRYP_CR_FINAL_PHASE
+	orr	r0, r0, #CRYP_CR_CRYPEN
+	str	r0, [r10, #CRYP_CR]
+	ldr	r0, [r10, #CRYP_CR]
+
+	/* Load CTR0 to generate the tag */
+	ldr	r0, [r11, #PM_CTX_CCM_CTR0]
+	str	r0, [r10, #CRYP_DIN]
+	ldr	r0, [r11, #(PM_CTX_CCM_CTR0 + 4)]
+	str	r0, [r10, #CRYP_DIN]
+	ldr	r0, [r11, #(PM_CTX_CCM_CTR0 + 8)]
+	str	r0, [r10, #CRYP_DIN]
+	ldr	r0, [r11, #(PM_CTX_CCM_CTR0 + 12)]
+	str	r0, [r10, #CRYP_DIN]
+
+	WAIT_FLAG_TIMEOUT CRYP_SR, CRYP_SR_OFNE, CRYP_SR_OFNE
+
+	/* Store generated tag in the PM_CTX structure */
+	ldr	r0, [r10, #CRYP_DOUT]
+	str	r0, [r11, #PM_CTX_CCM_TAG]
+	ldr	r0, [r10, #CRYP_DOUT]
+	str	r0, [r11, #(PM_CTX_CCM_TAG + 4)]
+	ldr	r0, [r10, #CRYP_DOUT]
+	str	r0, [r11, #(PM_CTX_CCM_TAG + 8)]
+	/* Before last 32bit word, the output FIFO shall not be empty */
+	ldr	r0, [r10, #CRYP_SR]
+	ands	r0, r0, #CRYP_SR_OFNE
+	beq	_ccm_failed
+	/* After last 32bit word for this 128block, FIFO shall be empty */
+	ldr	r0, [r10, #CRYP_DOUT]
+	str	r0, [r11, #(PM_CTX_CCM_TAG + 12)]
+	ldr	r0, [r10, #CRYP_SR]
+	ands	r0, r0, #CRYP_SR_OFNE
+	bne	_ccm_failed
+
+	/* Successful return */
+	bl	_reset_cryp1
+	mov	r0, #0
+	bx	r12
+
+_ccm_failed:
+	bl	_reset_cryp1
+	mov	r0, #1
+	bx	r12
+
+/* End address of the PIC resume sequence copy in retained RAM */
+stm32mp_bkpsram_image_end:
+	nop
+
+UNWIND(	.fnend)
+END_FUNC stm32mp_bkpsram_resume
+
+/*
+ * int stm32mp_ccm_encrypt_teeram(ctx, dst, src, len)
+ */
+FUNC stm32mp_ccm_encrypt_teeram , :
+UNWIND(	.fnstart)
+	push	{r4-r12, lr}
+UNWIND(	.save	{r4-r12, lr})
+	mov	r11, r0
+	mov	r0, r1
+	mov	r1, r2
+	mov	r2, r3
+	mov	r3, #0
+	push	{r0-r3}
+	bl	_prepare_time
+	pop	{r0-r3}
+	bl	stm32mp_ccm_teeram
+	bl	_save_resume_time
+	pop	{r4-r12, pc}
+UNWIND(	.fnend)
+END_FUNC stm32mp_ccm_encrypt_teeram
+
+/*
+ * int stm32mp_ccm_decrypt_teeram(ctx, cryp_base, dst, src)
+ */
+FUNC stm32mp_ccm_decrypt_teeram , :
+UNWIND(	.fnstart)
+	push	{r4-r12, lr}
+UNWIND(	.save	{r4-r12, lr})
+	mov	r11, r0
+	mov	r0, r1
+	mov	r1, r2
+	mov	r2, r3
+	mov	r3, #1
+	push	{r0-r3}
+	bl	_prepare_time
+	pop	{r0-r3}
+	bl	stm32mp_ccm_teeram
+	bl	_save_resume_time
+	pop	{r4-r12, pc}
+UNWIND(	.fnend)
+END_FUNC stm32mp_ccm_decrypt_teeram
+
+/*
+ * stm32mp_sysram_resume - Resume OP-TEE execution
+ *
+ * This function is the entry point of OP-TEE core resume sequence in the TEE
+ * RAM. When TEE RAM is lost during a power cycle, stm32mp_bkpsram_resume() is
+ * called to restore TEE RAM content and branch to this stm32mp_sysram_resume()
+ * routine.
+ *
+ * This function calls the OP-TEE core generic PM resume API
+ * sm_pm_cpu_resume().
+ */
+FUNC stm32mp_sysram_resume, :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	/* Invalidate the data cache */
+	mov     r0, #0	@ ; write the cache size selection register to be
+	write_csselr r0	@ ; sure we address the data cache
+	isb		@ ; isb to sync the change to the cachesizeid reg
+
+	mov     r0, #0	@ ; set way number to 0
+_inv_nextway:
+	mov     r1, #0	@ ; set line number (=index) to 0
+_inv_nextline:
+	orr     r2, r0, r1	@ ; construct way/index value
+	write_dcisw r2		@ ; inval data or unified cache line by set/way
+	add     r1, r1, #1 << LINE_FIELD_OFFSET	@ ; increment the index
+	cmp     r1, #1 << LINE_FIELD_OVERFLOW	@ ; overflow out of set field?
+	bne     _inv_nextline
+	add     r0, r0, #1 << WAY_FIELD_OFFSET	@ ; increment the way number
+	cmp     r0, #0				@ ; overflow out of way field?
+	bne     _inv_nextway
+
+	dsb
+	isb
+
+	/* Resume sequence executes in Monitor mode */
+	cps	#CPSR_MODE_MON
+
+	blx	plat_cpu_reset_early
+	b	sm_pm_cpu_resume
+UNWIND(	.fnend)
+END_FUNC stm32mp_sysram_resume
+
+/*
+ * stm32mp_cpu_reset_state - set CPU in a reset like state
+ *
+ * Disable CPU env (interrupts, cache, SMP, MMU) and return.
+ * Preserve the execution mode in CPSR.
+ */
+FUNC stm32mp_cpu_reset_state, :
+UNWIND(	.fnstart)
+	push	{r12, lr}
+UNWIND(	.save	{r12, lr})
+
+	cpsid	aif
+
+	bl	psci_armv7_cpu_off
+
+	write_bpiall
+	isb
+	dsb
+	read_sctlr r0
+	bic	r0, r0, #SCTLR_M
+	bic	r0, r0, #SCTLR_I
+	write_sctlr r0
+	isb
+	dsb	sy
+
+	pop	{r12, pc}
+UNWIND(	.fnend)
+END_FUNC stm32mp_cpu_reset_state
diff --git a/core/arch/arm/plat-stm32mp1/pm/power.h b/core/arch/arm/plat-stm32mp1/pm/power.h
new file mode 100644
index 0000000..21ca7be
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/pm/power.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ */
+
+#ifndef __STM32MP_PM_POWER_H__
+#define __STM32MP_PM_POWER_H__
+
+#include <compiler.h>
+#include <stdint.h>
+#include <stddef.h>
+
+bool need_to_backup_cpu_context(unsigned int soc_mode);
+
+void stm32_enter_csleep(void);
+
+int stm32_enter_cstop(uint32_t mode);
+void stm32_exit_cstop(void);
+
+void stm32_enter_cstop_shutdown(uint32_t mode) __noreturn;
+void stm32_enter_cstop_reset(uint32_t mode) __noreturn;
+
+void stm32_pm_cpu_power_down_wfi(void) __noreturn;
+void stm32_pm_cpu_wfi(void);
+
+#endif /*__STM32MP_PM_POWER_H__*/
diff --git a/core/arch/arm/plat-stm32mp1/pm/power_config.c b/core/arch/arm/plat-stm32mp1/pm/power_config.c
new file mode 100644
index 0000000..7845ede
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/pm/power_config.c
@@ -0,0 +1,212 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <assert.h>
+#include <dt-bindings/power/stm32mp1-power.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <stm32mp_dt.h>
+#include <stm32mp_pm.h>
+#include <trace.h>
+#include <util.h>
+
+#ifdef CFG_DT
+#include <libfdt.h>
+#endif
+
+#include "context.h"
+#include "power.h"
+
+#define DT_PWR_COMPAT			"st,stm32mp1-pwr"
+#define SYSTEM_SUSPEND_SUPPORTED_MODES	"system_suspend_supported_soc_modes"
+#define SYSTEM_OFF_MODE			"system_off_soc_mode"
+
+static uint32_t deepest_suspend_mode = STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR;
+static uint32_t system_off_mode = STM32_PM_SHUTDOWN;
+
+/* Default initialized to NOT supported until set from DT directives */
+static uint8_t stm32mp1_supported_soc_modes[STM32_PM_MAX_SOC_MODE];
+
+/* Init with all domains ON */
+static bool stm32mp1_pm_dom[STM32MP1_PD_MAX_PM_DOMAIN] = {
+	[STM32MP1_PD_VSW] = false,
+	[STM32MP1_PD_CORE_RET] = false,
+	[STM32MP1_PD_CORE] = false
+};
+
+bool need_to_backup_cpu_context(unsigned int soc_mode)
+{
+	switch (soc_mode) {
+	case STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR:
+		return true;
+	case STM32_PM_CSLEEP_RUN:
+	case STM32_PM_CSTOP_ALLOW_STOP:
+	case STM32_PM_CSTOP_ALLOW_LP_STOP:
+	case STM32_PM_CSTOP_ALLOW_LPLV_STOP:
+	case STM32_PM_CSTOP_ALLOW_STANDBY_DDR_OFF:
+	case STM32_PM_SHUTDOWN:
+		return false;
+	default:
+		EMSG("Invalid mode 0x%x", soc_mode);
+		panic();
+	}
+}
+
+#ifdef CFG_DT
+static int dt_get_pwr_node(void *fdt)
+{
+	return fdt_node_offset_by_compatible(fdt, -1, DT_PWR_COMPAT);
+}
+#endif
+
+static bool get_pm_domain_state(uint8_t mode)
+{
+	bool res = true;
+	enum stm32mp1_pm_domain id = STM32MP1_PD_MAX_PM_DOMAIN;
+
+	while (res && (id > mode)) {
+		id--;
+		res &= stm32mp1_pm_dom[id];
+	}
+
+	return res;
+}
+
+int stm32mp1_set_pm_domain_state(enum stm32mp1_pm_domain domain, bool status)
+{
+	if (domain >= STM32MP1_PD_MAX_PM_DOMAIN) {
+		return -1;
+	}
+
+	stm32mp1_pm_dom[domain] = status;
+
+	return 0;
+}
+
+#ifdef CFG_DT
+static void save_supported_mode(void *fdt, int pwr_node)
+{
+	int len;
+	uint32_t count;
+	unsigned int i;
+	uint32_t supported[ARRAY_SIZE(stm32mp1_supported_soc_modes)];
+	const void *prop;
+
+	prop = fdt_getprop(fdt, pwr_node, SYSTEM_SUSPEND_SUPPORTED_MODES, &len);
+	if (prop == NULL) {
+		panic();
+	}
+
+	count = (uint32_t)len / sizeof(uint32_t);
+	if (count > STM32_PM_MAX_SOC_MODE) {
+		panic();
+	}
+
+	if (fdt_read_uint32_array(fdt, pwr_node, SYSTEM_SUSPEND_SUPPORTED_MODES,
+				  &supported[0], count) < 0) {
+		panic("PWR DT");
+	}
+
+	for (i = 0; i < count; i++) {
+		if (supported[i] >= STM32_PM_MAX_SOC_MODE) {
+			panic("Invalid mode");
+		}
+		stm32mp1_supported_soc_modes[supported[i]] = true;
+	}
+}
+#endif
+
+static bool is_supported_mode(uint32_t soc_mode)
+{
+	assert(soc_mode < ARRAY_SIZE(stm32mp1_supported_soc_modes));
+	return stm32mp1_supported_soc_modes[soc_mode] == 1;
+}
+
+uint32_t stm32mp1_get_lp_soc_mode(uint32_t psci_mode)
+{
+	uint32_t mode;
+
+	if (psci_mode == PSCI_MODE_SYSTEM_OFF) {
+		return system_off_mode;
+	}
+
+	mode = deepest_suspend_mode;
+
+	if ((mode == STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR) &&
+	    ((!get_pm_domain_state(STM32MP1_PD_CORE_RET)) ||
+	     (!is_supported_mode(mode)))) {
+		mode = STM32_PM_CSTOP_ALLOW_LPLV_STOP;
+	}
+
+	if ((mode == STM32_PM_CSTOP_ALLOW_LPLV_STOP) &&
+	    ((!get_pm_domain_state(STM32MP1_PD_CORE)) ||
+	     (!is_supported_mode(mode)))) {
+		mode = STM32_PM_CSTOP_ALLOW_LP_STOP;
+	}
+
+	if ((mode == STM32_PM_CSTOP_ALLOW_LP_STOP) &&
+	    (!is_supported_mode(mode))) {
+		mode = STM32_PM_CSTOP_ALLOW_STOP;
+	}
+
+	if ((mode == STM32_PM_CSTOP_ALLOW_STOP) &&
+	    (!is_supported_mode(mode))) {
+		mode = STM32_PM_CSLEEP_RUN;
+	}
+
+	return mode;
+}
+
+int stm32mp1_set_lp_deepest_soc_mode(uint32_t psci_mode, uint32_t soc_mode)
+{
+	if (soc_mode >= STM32_PM_MAX_SOC_MODE) {
+		return -1;
+	}
+
+	if (psci_mode == PSCI_MODE_SYSTEM_SUSPEND) {
+		deepest_suspend_mode = soc_mode;
+	}
+
+	if (psci_mode == PSCI_MODE_SYSTEM_OFF) {
+		system_off_mode = soc_mode;
+	}
+
+	return 0;
+}
+
+#ifdef CFG_DT
+static TEE_Result stm32mp1_init_lp_states(void)
+{
+	void *fdt;
+	int pwr_node = -1;
+	const fdt32_t *cuint = NULL;
+
+	fdt = get_dt_blob();
+	if (fdt != NULL) {
+		pwr_node = dt_get_pwr_node(fdt);
+	}
+	if (pwr_node >= 0) {
+		cuint = fdt_getprop(fdt, pwr_node, SYSTEM_OFF_MODE, NULL);
+	}
+	if ((fdt == NULL) || (pwr_node < 0) || (cuint == NULL)) {
+		IMSG("No power configuration found in DT");
+		return TEE_SUCCESS;
+	}
+
+	system_off_mode = fdt32_to_cpu(*cuint);
+
+	/* Initialize suspend support to the deepest possible mode */
+	deepest_suspend_mode = STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR;
+
+	save_supported_mode(fdt, pwr_node);
+
+	DMSG("Power configuration: shutdown to %u, suspend to %u",
+	     stm32mp1_get_lp_soc_mode(PSCI_MODE_SYSTEM_OFF),
+	     stm32mp1_get_lp_soc_mode(PSCI_MODE_SYSTEM_SUSPEND));
+
+	return TEE_SUCCESS;
+}
+service_init(stm32mp1_init_lp_states);
+#endif
diff --git a/core/arch/arm/plat-stm32mp1/pm/psci.c b/core/arch/arm/plat-stm32mp1/pm/psci.c
new file mode 100644
index 0000000..6e8219e
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/pm/psci.c
@@ -0,0 +1,427 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics
+ * Copyright (c) 2015-2018, ARM Limited and Contributors. All rights reserved.
+ */
+
+#include <arm.h>
+#include <boot_api.h>
+#include <drivers/stm32_etzpc.h>
+#include <drivers/stm32_rng.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <dt-bindings/power/stm32mp1-power.h>
+#include <initcall.h>
+#include <io.h>
+#include <keep.h>
+#include <kernel/cache_helpers.h>
+#include <kernel/delay.h>
+#include <kernel/generic_boot.h>
+#include <kernel/interrupt.h>
+#include <kernel/misc.h>
+#include <kernel/panic.h>
+#include <kernel/spinlock.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <sm/pm.h>
+#include <sm/psci.h>
+#include <stm32_util.h>
+#include <stm32mp_pm.h>
+#include <string.h>
+#include <trace.h>
+#include <util.h>
+
+#include "context.h"
+#include "power.h"
+
+/*
+ * SMP boot support and access to the mailbox
+ */
+#define CORE_OFF		0
+#define CORE_AWAKE		1
+#define CORE_RET		2
+#define CORE_ON			3
+
+static int core_state[CFG_TEE_CORE_NB_CORE];
+static unsigned int __maybe_unused state_lock = SPINLOCK_UNLOCK;
+
+static uint32_t __maybe_unused lock_state_access(void)
+{
+	return may_spin_lock(&state_lock);
+}
+
+static void __maybe_unused unlock_state_access(uint32_t exceptions)
+{
+	may_spin_unlock(&state_lock, exceptions);
+}
+
+int psci_affinity_info(uint32_t affinity, uint32_t lowest_affinity_level)
+{
+	unsigned int pos = get_core_pos_mpidr(affinity);
+
+	DMSG("core %zu, state %u", pos, core_state[pos]);
+
+	if ((pos >= CFG_TEE_CORE_NB_CORE) ||
+	    (lowest_affinity_level > PSCI_AFFINITY_LEVEL_ON)) {
+		return PSCI_RET_INVALID_PARAMETERS;
+	}
+
+	switch (core_state[pos]) {
+	case CORE_OFF:
+	case CORE_RET:
+		return PSCI_AFFINITY_LEVEL_OFF;
+	case CORE_AWAKE:
+		return PSCI_AFFINITY_LEVEL_ON_PENDING;
+	case CORE_ON:
+		return PSCI_AFFINITY_LEVEL_ON;
+	default:
+		panic();
+	}
+}
+
+#if CFG_TEE_CORE_NB_CORE == 1
+/*
+ * Function called when a CPU is booted through the OP-TEE.
+ * All cores shall register when online.
+ */
+void stm32mp_register_online_cpu(void)
+{
+	assert((core_state[0] == CORE_OFF) ||
+	       (core_state[0] == CORE_RET));
+	core_state[0] = CORE_ON;
+}
+#else
+void stm32mp_register_online_cpu(void)
+{
+	size_t pos = get_core_pos();
+	uint32_t excep = lock_state_access();
+
+	if (pos == 0) {
+		assert((core_state[pos] == CORE_OFF) ||
+		       (core_state[pos] == CORE_RET));
+	} else {
+		if (core_state[pos] != CORE_AWAKE) {
+			core_state[pos] = CORE_OFF;
+			unlock_state_access(excep);
+			stm32_pm_cpu_power_down_wfi();
+			/* No return */
+		}
+	}
+
+	core_state[pos] = CORE_ON;
+	unlock_state_access(excep);
+}
+
+#define GICD_SGIR		0xF00
+static void raise_sgi0_as_secure(void)
+{
+	dsb_ishst();
+	write32(GIC_NON_SEC_SGI_0 | SHIFT_U32(TARGET_CPU1_GIC_MASK, 16),
+		get_gicd_base() + GICD_SGIR);
+}
+
+static void release_secondary_early_hpen(size_t pos __unused)
+{
+	/* Need to send SIG#0 over Group0 after individual core 1 reset */
+	raise_sgi0_as_secure();
+	udelay(20);
+
+	stm32_clock_enable(RTCAPB);
+
+	write32(TEE_LOAD_ADDR,
+		stm32mp_bkpreg(BCKR_CORE1_BRANCH_ADDRESS));
+	write32(BOOT_API_A7_CORE1_MAGIC_NUMBER,
+		stm32mp_bkpreg(BCKR_CORE1_MAGIC_NUMBER));
+
+	stm32_clock_disable(RTCAPB);
+
+	dsb_ishst();
+	itr_raise_sgi(GIC_SEC_SGI_0, TARGET_CPU1_GIC_MASK);
+}
+
+/* Override default psci_cpu_on() with platform specific sequence */
+int psci_cpu_on(uint32_t core_id, uint32_t entry, uint32_t context_id)
+{
+	size_t pos = get_core_pos_mpidr(core_id);
+	uint32_t excep;
+	int rc;
+
+	if (!pos || pos >= CFG_TEE_CORE_NB_CORE)
+		return PSCI_RET_INVALID_PARAMETERS;
+
+	DMSG("core %zu, ns_entry 0x%" PRIx32 ", state %u",
+		pos, entry, core_state[pos]);
+
+	excep = lock_state_access();
+
+	switch (core_state[pos]) {
+	case CORE_ON:
+		rc = PSCI_RET_ALREADY_ON;
+		break;
+	case CORE_AWAKE:
+		rc = PSCI_RET_ON_PENDING;
+		break;
+	case CORE_RET:
+		rc = PSCI_RET_DENIED;
+		break;
+	case CORE_OFF:
+		core_state[pos] = CORE_AWAKE;
+		rc = PSCI_RET_SUCCESS;
+		break;
+	default:
+		panic();
+	}
+
+	unlock_state_access(excep);
+
+	if (rc == PSCI_RET_SUCCESS) {
+		generic_boot_set_core_ns_entry(pos, entry, context_id);
+		release_secondary_early_hpen(pos);
+	}
+
+	return rc;
+}
+
+/* Override default psci_cpu_off() with platform specific sequence */
+int psci_cpu_off(void)
+{
+	unsigned int pos = get_core_pos();
+	uint32_t excep;
+
+	if (pos == 0) {
+		EMSG("PSCI_CPU_OFF not supported for core0, use system_off");
+		return PSCI_RET_INTERNAL_FAILURE;
+	}
+
+	DMSG("core %u", pos);
+
+	excep = lock_state_access();
+
+	assert(core_state[pos] == CORE_ON);
+	core_state[pos] = CORE_OFF;
+
+	unlock_state_access(excep);
+
+	/* Enable BKPREG access for the disabled CPU */
+	stm32_clock_enable(RTCAPB);
+
+	thread_mask_exceptions(THREAD_EXCP_ALL);
+	stm32_pm_cpu_power_down_wfi();
+	panic();
+}
+#endif
+
+static int enter_cstop_suspend(unsigned int soc_mode)
+{
+	int rc = 1;
+
+	if (read_isr() != 0) {
+		return rc;
+	}
+
+	if (stm32_enter_cstop(soc_mode)) {
+		goto resume;
+	}
+
+	if (need_to_backup_cpu_context(soc_mode)) {
+		stm32_pm_cpu_power_down_wfi();
+	} else {
+		stm32_pm_cpu_wfi();
+		rc = 0;
+	}
+
+resume:
+	stm32_exit_cstop();
+
+	return rc;
+}
+
+static int plat_suspend(uint32_t arg)
+{
+	unsigned int soc_mode = arg;
+	size_t pos = get_core_pos();
+	int rc = 1;
+
+	if (read_isr() != 0) {
+		return rc;
+	}
+
+	/* No need to lock state access as CPU is alone when here */
+	assert(core_state[pos] == CORE_ON);
+	core_state[pos] = CORE_RET;
+
+	stm32mp_pm_save_context(soc_mode);
+
+	rc = enter_cstop_suspend(soc_mode);
+
+	stm32mp_pm_restore_context(soc_mode);
+	stm32mp_pm_wipe_context();
+
+	assert(core_state[pos] == CORE_RET);
+	core_state[pos] = CORE_ON;
+
+	return rc;
+}
+
+static void plat_resume(uint32_t arg)
+{
+	unsigned int soc_mode = arg;
+
+	plat_cpu_reset_late();
+	main_init_gic();
+
+	assert(core_state[get_core_pos()] == CORE_ON);
+
+	stm32mp_pm_restore_context(soc_mode);
+}
+
+#if !defined(CFG_STM32_RNG)
+static bool plat_can_suspend(void)
+{
+	/* RNG is mandated for suspending TEE RAM content */
+	return false;
+}
+#else
+#if CFG_TEE_CORE_NB_CORE == 1
+static bool plat_can_suspend(void)
+{
+	return true;
+}
+#else /*CFG_TEE_CORE_NB_CORE==1*/
+static bool plat_can_suspend(void)
+{
+	size_t pos = get_core_pos();
+	size_t n;
+	uint32_t excep;
+	bool rc = true;
+
+	excep = lock_state_access();
+
+	for (n = 0; n < ARRAY_SIZE(core_state); n++) {
+		if (n == pos) {
+			continue;
+		}
+		if (core_state[n] == CORE_AWAKE) {
+			/* State core as lost and proceed suspend */
+			core_state[n] = CORE_OFF;
+		}
+		if (core_state[n] != CORE_OFF) {
+			rc = false;
+		}
+	}
+
+	unlock_state_access(excep);
+
+	return rc;
+}
+#endif /*CFG_TEE_CORE_NB_CORE==1*/
+#endif /*CFG_STM32_RNG*/
+
+/* Override default psci_system_suspend() with platform specific sequence */
+int psci_system_suspend(uintptr_t entry, uint32_t context_id __unused,
+			struct sm_nsec_ctx *nsec)
+{
+	int ret = PSCI_RET_INVALID_PARAMETERS;
+	uint32_t soc_mode;
+	int pos = get_core_pos();
+
+	DMSG("core %u", pos);
+
+	if (!plat_can_suspend()) {
+		return PSCI_RET_DENIED;
+	}
+
+	soc_mode = stm32mp1_get_lp_soc_mode(PSCI_MODE_SYSTEM_SUSPEND);
+
+	switch (soc_mode) {
+	case STM32_PM_CSLEEP_RUN:
+		stm32_enter_csleep();
+		nsec->mon_lr = (uint32_t)entry;
+		return PSCI_RET_SUCCESS;
+	case STM32_PM_SHUTDOWN:
+		stm32_enter_cstop_shutdown(soc_mode);
+		panic();
+	default:
+		/* Others are suspended mode: at least some context to backup */
+		break;
+	}
+
+	assert(cpu_mmu_enabled() && core_state[pos] == CORE_ON);
+
+	if (need_to_backup_cpu_context(soc_mode)) {
+		sm_save_unbanked_regs(&nsec->ub_regs);
+		/*
+		 * sm_pm_cpu_suspend(arg, func) saves the CPU core context in TEE RAM
+		 * then calls func(arg) to run the platform lower power sequence.
+		 *
+		 * If platform fails to suspend, sm_pm_cpu_suspend() returns with a
+		 * non null return code. When sm_pm_cpu_suspend() returns 0 platform
+		 * context must be restored.
+		 */
+		ret = sm_pm_cpu_suspend((uint32_t)soc_mode, plat_suspend);
+		if (ret == 0) {
+			plat_resume((uint32_t)soc_mode);
+			sm_restore_unbanked_regs(&nsec->ub_regs);
+		}
+	} else {
+		ret = plat_suspend((uint32_t)soc_mode);
+	}
+
+	if (ret == 0) {
+		nsec->mon_lr = (uint32_t)entry;
+		IMSG("Resumed");
+		return PSCI_RET_SUCCESS;
+	}
+
+	return PSCI_RET_INTERNAL_FAILURE;
+}
+
+/* Override default psci_system_off() with platform specific sequence */
+void __noreturn psci_system_off(void)
+{
+	uint32_t soc_mode = stm32mp1_get_lp_soc_mode(PSCI_MODE_SYSTEM_OFF);
+
+	DMSG("core %u", get_core_pos());
+
+	stm32_enter_cstop_shutdown(soc_mode);
+}
+
+/* Override default psci_system_reset() with platform specific sequence */
+void __noreturn psci_system_reset(void)
+{
+	uint32_t soc_mode = stm32mp1_get_lp_soc_mode(PSCI_MODE_SYSTEM_OFF);
+
+	DMSG("core %u", get_core_pos());
+
+	stm32_enter_cstop_reset(soc_mode);
+}
+
+void __noreturn stm32mp_platform_reset(int __unused cpu)
+{
+	psci_system_reset();
+}
+
+/* Override default psci_cpu_on() with platform supported features */
+int psci_features(uint32_t psci_fid)
+{
+	switch (psci_fid) {
+	case PSCI_PSCI_FEATURES:
+	case PSCI_VERSION:
+#if CFG_TEE_CORE_NB_CORE > 1
+	case PSCI_CPU_ON:
+	case PSCI_CPU_OFF:
+#endif
+	case PSCI_SYSTEM_SUSPEND:
+	case PSCI_SYSTEM_RESET:
+	case PSCI_SYSTEM_OFF:
+		return PSCI_RET_SUCCESS;
+	default:
+		return PSCI_RET_NOT_SUPPORTED;
+	}
+}
+
+/* Override default psci_version() to enable PSCI_VERSION_1_0 API */
+uint32_t psci_version(void)
+{
+	return PSCI_VERSION_1_0;
+}
+
diff --git a/core/arch/arm/plat-stm32mp1/pm/sub.mk b/core/arch/arm/plat-stm32mp1/pm/sub.mk
new file mode 100644
index 0000000..d8930a4
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/pm/sub.mk
@@ -0,0 +1,7 @@
+asm-defines-y += context_asm_defines.c
+
+srcs-y += context.c
+srcs-y += low_power.c
+srcs-y += pm_helpers.S
+srcs-y += power_config.c
+srcs-$(CFG_PSCI_ARM32) += psci.c
diff --git a/core/arch/arm/plat-stm32mp1/reset.S b/core/arch/arm/plat-stm32mp1/reset.S
index 69ab151..abf8b9a 100644
--- a/core/arch/arm/plat-stm32mp1/reset.S
+++ b/core/arch/arm/plat-stm32mp1/reset.S
@@ -23,8 +23,34 @@ UNWIND(	.fnstart)
 	mov_imm	r1, STM32MP1_NSACR_PRESERVE_MASK
 	and	r0, r0, r1
 	write_nsacr r0
+	isb
+
+	read_actlr r0
+	orr	r0, r0, #ACTLR_SMP
+	write_actlr r0
+
+	/*
+	 * Always reset CNTVOFF for the dear non secure world.
+	 * This operation requires being in Monitor mode and
+	 * non secure state.
+	 */
+	mrs	r1, cpsr
+	cps	#CPSR_MODE_MON
+	isb
+
+	read_scr r2
+	orr	r0, r2, #SCR_NS
+	write_scr r0
+	isb
+
+	mov	r0, #0
+	write_cntvoff r0, r0
 
+	write_scr r2
 	isb
-	bx	lr
+	msr	cpsr, r1
+	isb
+
+	bx lr
 UNWIND(	.fnend)
 END_FUNC plat_cpu_reset_early
diff --git a/core/arch/arm/plat-stm32mp1/scripts/stm32image.py b/core/arch/arm/plat-stm32mp1/scripts/stm32image.py
index 55363af..2def5b8 100755
--- a/core/arch/arm/plat-stm32mp1/scripts/stm32image.py
+++ b/core/arch/arm/plat-stm32mp1/scripts/stm32image.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/usr/bin/env python
 # SPDX-License-Identifier: BSD-2-Clause
 #
 # Copyright (c) 2017-2018, STMicroelectronics
@@ -35,7 +35,7 @@ def stm32image_checksum(dest_fd, sizedest):
         return csum
 
 
-def stm32image_set_header(dest_fd, load, entry):
+def stm32image_set_header(dest_fd, load, entry, bintype):
         sizedest = get_size(dest_fd)
 
         checksum = stm32image_checksum(dest_fd, sizedest)
@@ -68,11 +68,12 @@ def stm32image_set_header(dest_fd, load, entry):
         dest_fd.write(b'\x00' * 64)
 
         # Padding
-        dest_fd.write(b'\x00' * 84)
+        dest_fd.write(b'\x00' * 83)
+        dest_fd.write(struct.pack('<B', bintype))
         dest_fd.close()
 
 
-def stm32image_create_header_file(source, dest, load, entry):
+def stm32image_create_header_file(source, dest, load, entry, bintype):
         dest_fd = open(dest, 'w+b')
         src_fd = open(source, 'rb')
 
@@ -86,7 +87,7 @@ def stm32image_create_header_file(source, dest, load, entry):
 
         src_fd.close()
 
-        stm32image_set_header(dest_fd, load, entry)
+        stm32image_set_header(dest_fd, load, entry, bintype)
 
         dest_fd.close()
 
@@ -113,6 +114,10 @@ def get_args():
                             required=True, type=int_parse,
                             help='Entry point')
 
+        parser.add_argument('--bintype',
+                            required=True, type=int_parse,
+                            help='Binary identification')
+
         return parser.parse_args()
 
 
@@ -122,11 +127,13 @@ def main():
         destination_file = args.dest
         load_address = args.load
         entry_point = args.entry
+        binary_type = args.bintype
 
         stm32image_create_header_file(source_file,
                                       destination_file,
                                       load_address,
-                                      entry_point)
+                                      entry_point,
+                                      binary_type)
 
 
 if __name__ == "__main__":
diff --git a/core/arch/arm/plat-stm32mp1/service/bsec_svc.c b/core/arch/arm/plat-stm32mp1/service/bsec_svc.c
new file mode 100644
index 0000000..1ce7195
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/service/bsec_svc.c
@@ -0,0 +1,65 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2016-2018, STMicroelectronics
+ */
+
+#include <trace.h>
+#include "bsec_svc.h"
+#include "stm32mp1_smc.h"
+
+uint32_t bsec_main(uint32_t x1, uint32_t x2, uint32_t x3, uint32_t *out)
+{
+	int result;
+	uint32_t tmp;
+
+	if (bsec_check_nsec_access_rights(x2) != BSEC_OK) {
+		return BSEC_ERROR;
+	}
+
+	switch (x1) {
+	case STM32_SIP_BSEC_READ_SHADOW:
+		result = bsec_read_otp(out, x2);
+		FMSG("read shadow @%" PRIx32 " = %" PRIx32, x2, *out);
+		break;
+	case STM32_SIP_BSEC_PROG_OTP:
+		FMSG("program @%" PRIx32, x2);
+		result = bsec_program_otp(x3, x2);
+		break;
+	case STM32_SIP_BSEC_WRITE_SHADOW:
+		FMSG("write shadow @%" PRIx32, x2);
+		result = bsec_write_otp(x3, x2);
+		break;
+	case STM32_SIP_BSEC_READ_OTP:
+		result = bsec_read_otp(&tmp, x2);
+		if (result != BSEC_OK) {
+			break;
+		}
+
+		result = bsec_shadow_register(x2);
+		if (result != BSEC_OK) {
+			break;
+		}
+
+		result = bsec_read_otp(out, x2);
+		if (result != BSEC_OK) {
+			break;
+		}
+
+		result = bsec_write_otp(tmp, x2);
+		FMSG("read @%" PRIx32 " = %" PRIx32, x2, *out);
+		break;
+	default:
+		EMSG("Invalid %" PRIx32, x1);
+		result = BSEC_ERROR;
+		break;
+	}
+
+	switch (result) {
+	case BSEC_OK:
+		return STM32_SIP_OK;
+	case BSEC_INVALID_PARAM:
+		return STM32_SIP_INVALID_PARAMS;
+	default:
+		return STM32_SIP_FAILED;
+	}
+}
diff --git a/core/arch/arm/plat-stm32mp1/service/bsec_svc.h b/core/arch/arm/plat-stm32mp1/service/bsec_svc.h
new file mode 100644
index 0000000..4f1babe
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/service/bsec_svc.h
@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2016-2018, STMicroelectronics
+ */
+
+#ifndef __STM32MP1_BSEC_SVC_H__
+#define __STM32MP1_BSEC_SVC_H__
+
+#include <drivers/stm32_bsec.h>
+
+uint32_t bsec_main(uint32_t x1, uint32_t x2, uint32_t x3,
+		   uint32_t *ret_otp_value);
+
+#endif /*__STM32MP1_BSEC_SVC_H__*/
diff --git a/core/arch/arm/plat-stm32mp1/service/low_power_svc.c b/core/arch/arm/plat-stm32mp1/service/low_power_svc.c
new file mode 100644
index 0000000..626f390
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/service/low_power_svc.c
@@ -0,0 +1,153 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ */
+
+#include <arm32.h>
+#include <drivers/stm32mp1_ddrc.h>
+#include <dt-bindings/power/stm32mp1-power.h>
+#include <kernel/delay.h>
+#include <kernel/generic_boot.h>
+#include <kernel/interrupt.h>
+#include <kernel/misc.h>
+#include <kernel/panic.h>
+#include <platform_config.h>
+#include <sm/pm.h>
+#include <sm/psci.h>
+#include <stm32mp_pm.h>
+#include <stm32_util.h>
+#include <trace.h>
+
+#include "stm32mp1_smc.h"
+#include "low_power_svc.h"
+#include "../pm/power.h"
+#include "../pm/context.h"
+
+
+#undef DDR_SR_TEST
+
+#ifdef DDR_SR_TEST
+uint32_t sr_mode_scv_handler(uint32_t __maybe_unused x1, uint32_t x2)
+{
+	unsigned int mode = x2;
+
+	DMSG("DDR selfrefresh mode 0x%" PRIx32 ", 0x%" PRIx32, mode, x1);
+
+	switch (mode) {
+	case STM32_SIP_SR_MODE_SSR:
+		ddr_sr_mode_ssr();
+		break;
+	case STM32_SIP_SR_MODE_ASR:
+		ddr_sr_mode_asr();
+		break;
+	case STM32_SIP_SR_MODE_HSR:
+		ddr_sr_mode_hsr();
+		break;
+	default:
+		return STM32_SIP_INVALID_PARAMS;
+	}
+
+	return STM32_SIP_OK;
+}
+#else
+uint32_t sr_mode_scv_handler(uint32_t __unused x1, uint32_t __unused x2)
+{
+	return STM32_SIP_NOT_SUPPORTED;
+}
+#endif
+
+uint32_t cstop_scv_handler(struct sm_ctx __unused *ctx, uint32_t __unused x1,
+			   uint32_t __unused x2, uint32_t __unused x3)
+{
+	DMSG("core %u", get_core_pos());
+
+	stm32mp1_set_lp_deepest_soc_mode(PSCI_MODE_SYSTEM_SUSPEND,
+					 STM32_PM_CSTOP_ALLOW_LPLV_STOP);
+
+	return (psci_system_suspend(ctx->nsec.mon_lr, 0, &ctx->nsec) == 0) ?
+		STM32_SIP_OK : STM32_SIP_FAILED;
+}
+
+uint32_t standby_scv_handler(struct sm_ctx *ctx, uint32_t __unused x1,
+			     uint32_t __unused x2, uint32_t x3)
+{
+	uint32_t nsec_resume_ep = x3;
+
+	DMSG("core %u", get_core_pos());
+
+	if (nsec_resume_ep == 0U) {
+		shutdown_scv_handler();
+		panic();
+	}
+
+	stm32mp1_set_lp_deepest_soc_mode(PSCI_MODE_SYSTEM_SUSPEND,
+					 STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR);
+
+	return (psci_system_suspend(nsec_resume_ep, 0, &ctx->nsec) == 0) ?
+		STM32_SIP_OK : STM32_SIP_FAILED;
+}
+
+uint32_t shutdown_scv_handler(void)
+{
+	DMSG("core %u", get_core_pos());
+
+	if (!stm32mp_with_pmic()) {
+		return STM32_SIP_NOT_SUPPORTED;
+	}
+
+	psci_system_off();
+	panic();
+}
+
+uint32_t pm_domain_scv_handler(uint32_t id, uint32_t enable)
+{
+	unsigned int pd = id;
+
+	DMSG("%sable PD %u", enable != 0 ? "En" : "Dis", pd);
+
+	switch (pd) {
+	case STM32MP1_PD_VSW:
+	case STM32MP1_PD_CORE_RET:
+	case STM32MP1_PD_CORE:
+		break;
+	default:
+		return STM32_SIP_INVALID_PARAMS;
+	}
+
+	stm32mp1_set_pm_domain_state(pd, enable);
+
+	return STM32_SIP_OK;
+}
+
+#ifdef CFG_TEE_CORE_DEBUG
+uint32_t pm_set_lp_state_scv_handler(uint32_t request, uint32_t state)
+{
+	uint32_t power_mode;
+
+	switch (request) {
+	case STM32_OEM_LP_FORCE_SUSPEND_PARAMS:
+		DMSG("Set suspend mode to %u", state);
+		power_mode = PSCI_MODE_SYSTEM_SUSPEND;
+		break;
+	case STM32_OEM_LP_FORCE_OFF_PARAMS:
+		DMSG("Set off mode to %u", state);
+		power_mode = PSCI_MODE_SYSTEM_OFF;
+		break;
+	default:
+		return STM32_OEM_INVALID_PARAMS;
+	}
+
+	if (stm32mp1_set_lp_deepest_soc_mode(power_mode, state) < 0) {
+		return STM32_OEM_FAILED;
+	}
+
+	return STM32_OEM_OK;
+}
+#else
+uint32_t pm_set_lp_state_scv_handler(uint32_t __unused mode,
+				     uint32_t __unused state)
+{
+	return STM32_SIP_NOT_SUPPORTED;
+}
+#endif
diff --git a/core/arch/arm/plat-stm32mp1/service/low_power_svc.h b/core/arch/arm/plat-stm32mp1/service/low_power_svc.h
new file mode 100644
index 0000000..5681ea2
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/service/low_power_svc.h
@@ -0,0 +1,22 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2018, STMicroelectronics
+ */
+
+#ifndef LOW_POWER_SVC_H
+#define LOW_POWER_SVC_H
+
+#include <sm/sm.h>
+#include <stdint.h>
+
+uint32_t sr_mode_scv_handler(uint32_t x1, uint32_t x2);
+uint32_t cstop_scv_handler(struct sm_ctx *nsec,
+			   uint32_t x1, uint32_t x2, uint32_t x3);
+uint32_t standby_scv_handler(struct sm_ctx *nsec,
+			     uint32_t x1, uint32_t x2, uint32_t x3);
+uint32_t shutdown_scv_handler(void);
+uint32_t pm_domain_scv_handler(uint32_t x1, uint32_t x2);
+
+uint32_t pm_set_lp_state_scv_handler(uint32_t x1, uint32_t x2);
+
+#endif /* LOW_POWER_SVC_H */
diff --git a/core/arch/arm/plat-stm32mp1/service/pwr_svc.c b/core/arch/arm/plat-stm32mp1/service/pwr_svc.c
new file mode 100644
index 0000000..9c314b2
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/service/pwr_svc.c
@@ -0,0 +1,117 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics
+ */
+
+#include <inttypes.h>
+#include <drivers/stm32mp1_pwr.h>
+#include <kernel/panic.h>
+#include <io.h>
+#include <kernel/spinlock.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <platform_config.h>
+#include <stm32_util.h>
+#include <trace.h>
+
+#include "pwr_svc.h"
+#include "stm32mp1_smc.h"
+
+struct pwr_reg_prop {
+	uint32_t offset;
+	uint32_t mask;
+};
+
+#define PWR_ALLOWED_MASK(_off, _mask)	{ .offset = (_off), .mask = (_mask), }
+
+static const struct pwr_reg_prop allowed_regs[] = {
+	PWR_ALLOWED_MASK(PWR_CR3_OFF, PWR_CR3_VBE | PWR_CR3_VBRS |
+				      PWR_CR3_USB33DEN |
+				      PWR_CR3_REG18EN | PWR_CR3_REG11EN),
+	PWR_ALLOWED_MASK(PWR_WKUPCR_OFF, PWR_WKUPCR_MASK),
+	PWR_ALLOWED_MASK(PWR_MPUWKUPENR_OFF, PWR_MPUWKUPENR_MASK),
+};
+
+static unsigned int lock = SPINLOCK_UNLOCK;
+
+static uint32_t pwr_regs_lock(void)
+{
+	return may_spin_lock(&lock);
+}
+
+static void pwr_regs_unlock(uint32_t exceptions)
+{
+	may_spin_unlock(&lock, exceptions);
+}
+
+uint32_t pwr_scv_handler(uint32_t x1, uint32_t x2, uint32_t x3)
+{
+	uint32_t req = x1;
+	uint32_t offset = x2;
+	uint32_t value = x3;
+	uint32_t va;
+	uint32_t allowed;
+	uint32_t i;
+	uint32_t ret;
+	uint32_t exc;
+
+	/*
+	 * Argument x2 can be either the register physical address of the
+	 * register offset toward PWR_BASE.
+	 */
+	if ((offset & ~PWR_OFFSET_MASK) != 0) {
+		if ((offset & ~PWR_OFFSET_MASK) != PWR_BASE) {
+			return STM32_SIP_INVALID_PARAMS;
+		}
+
+		offset &= PWR_OFFSET_MASK;
+	}
+
+	DMSG_RAW("PWR service: %s 0x%" PRIx32 " at offset 0x%" PRIx32,
+		req == STM32_SIP_REG_WRITE ? "write" :
+		req == STM32_SIP_REG_SET ? "set" : "clear",
+		value, offset);
+
+	exc = pwr_regs_lock();
+
+	for (i = 0; i < ARRAY_SIZE(allowed_regs); i++) {
+		if (offset != allowed_regs[i].offset) {
+			continue;
+		}
+
+		va = stm32_pwr_base() + offset;
+		allowed = allowed_regs[i].mask;
+		value &= allowed;
+
+		switch (req) {
+		case STM32_SIP_REG_WRITE:
+			io_mask32_stm32shregs(va, value, allowed);
+			FMSG("wrt off %" PRIx32 "=%" PRIx32 " => %" PRIx32,
+				offset, value, read32(va));
+			ret = STM32_SIP_OK;
+			break;
+		case STM32_SIP_REG_SET:
+			io_mask32_stm32shregs(va, value, value);
+			FMSG("set off %" PRIx32 "=%" PRIx32 " => %" PRIx32,
+				offset, value, read32(va));
+			ret = STM32_SIP_OK;
+			break;
+		case STM32_SIP_REG_CLEAR:
+			io_mask32_stm32shregs(va, 0, value);
+			FMSG("clr off %" PRIx32 "=%" PRIx32 " => %" PRIx32,
+				offset, value, read32(va));
+			ret = STM32_SIP_OK;
+			break;
+		default:
+			ret = STM32_SIP_INVALID_PARAMS;
+		}
+
+		pwr_regs_unlock(exc);
+
+		return ret;
+	}
+
+	pwr_regs_unlock(exc);
+
+	return STM32_SIP_INVALID_PARAMS;
+}
diff --git a/core/arch/arm/plat-stm32mp1/service/pwr_svc.h b/core/arch/arm/plat-stm32mp1/service/pwr_svc.h
new file mode 100644
index 0000000..011cac4
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/service/pwr_svc.h
@@ -0,0 +1,11 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics
+ */
+
+#ifndef __PWR_SVC_H__
+#define __PWR_SVC_H__
+
+uint32_t pwr_scv_handler(uint32_t x1, uint32_t x2, uint32_t x3);
+
+#endif /* __PWR_SVC_H__*/
diff --git a/core/arch/arm/plat-stm32mp1/service/rcc_svc.c b/core/arch/arm/plat-stm32mp1/service/rcc_svc.c
new file mode 100644
index 0000000..7bdaea8
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/service/rcc_svc.c
@@ -0,0 +1,440 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics
+ */
+
+#include <drivers/stm32mp1_clk.h>
+#include <drivers/stm32mp1_rcc.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <inttypes.h>
+#include <kernel/panic.h>
+#include <io.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <platform_config.h>
+#include <stm32_util.h>
+#include <trace.h>
+
+#include "rcc_svc.h"
+#include "stm32mp1_smc.h"
+
+#define STD_REG			0
+#define SET_REG			1
+#define CLR_REG			2
+
+static void shared_clk_request(uint32_t request,
+			       uint32_t offset, uint32_t value)
+{
+	unsigned int id;
+	unsigned int bit;
+	uint32_t enable_bits = 0;
+	int clr_std_set = STD_REG;
+
+	switch (request) {
+	case STM32_SIP_REG_WRITE:
+	case STM32_SIP_REG_SET:
+	case STM32_SIP_REG_CLEAR:
+		break;
+	default:
+		return;
+	}
+
+	switch (offset) {
+	case RCC_MP_APB5ENSETR:
+		clr_std_set = SET_REG;
+		/* Non secure backup registers requires RTCAPB clock */
+		enable_bits |= RCC_MP_APB5ENSETR_RTCAPBEN;
+		break;
+	case RCC_MP_APB5ENCLRR:
+		clr_std_set = CLR_REG;
+		/* Non secure backup registers requires RTCAPB clock */
+		enable_bits |= RCC_MP_APB5ENSETR_RTCAPBEN;
+		break;
+
+	case RCC_MP_AHB5ENSETR:
+		clr_std_set = SET_REG;
+		if (stm32mp_gpio_bank_is_shared(GPIO_BANK_Z)) {
+			enable_bits |= RCC_MP_AHB5ENSETR_GPIOZEN;
+		}
+		break;
+	case RCC_MP_AHB5ENCLRR:
+		clr_std_set = CLR_REG;
+		if (stm32mp_gpio_bank_is_shared(GPIO_BANK_Z)) {
+			enable_bits |= RCC_MP_AHB5ENSETR_GPIOZEN;
+		}
+		break;
+	default:
+		return;
+	}
+
+	if ((clr_std_set != STD_REG) && (request == STM32_SIP_REG_CLEAR))
+		return;
+
+	/*
+	 * Parse bit that relate to a functional clock.
+	 * Call stm32mp1_clk_enable/disable_non_secure() for that clock
+	 * according to request (write/set/clear) and target register
+	 * (write or set/clear).
+	 */
+	for (bit = 0; enable_bits && bit < 32; bit++) {
+
+		if (!(BIT(bit) & enable_bits))
+			continue;
+
+		id = stm32mp1_clk_rcc2id(offset, bit);
+		if (id == ~0U)
+			panic();
+
+		switch (clr_std_set) {
+		case SET_REG:
+			if (BIT(bit) & value) {
+				DMSG("Enable non-secure clock %u", id);
+				stm32mp1_clk_enable_non_secure(id);
+			}
+			break;
+		case CLR_REG:
+			if (BIT(bit) & value) {
+				DMSG("Disable non-secure clock %u", id);
+				stm32mp1_clk_disable_non_secure(id);
+			}
+			break;
+		default:
+			/* Standard registers case */
+			switch (request) {
+			case STM32_SIP_REG_WRITE:
+				if (BIT(bit) & value) {
+					DMSG("Enable non-secure clock %u", id);
+					stm32mp1_clk_enable_non_secure(id);
+				} else {
+					DMSG("Disable non-secure clock %u", id);
+					stm32mp1_clk_disable_non_secure(id);
+				}
+				break;
+			case STM32_SIP_REG_SET:
+				if (BIT(bit) & value) {
+					DMSG("Enable non-secure clock %u", id);
+					stm32mp1_clk_enable_non_secure(id);
+				}
+				break;
+			case STM32_SIP_REG_CLEAR:
+				if (BIT(bit) & value) {
+					DMSG("Disable non-secure clock %u", id);
+					stm32mp1_clk_disable_non_secure(id);
+				}
+				break;
+			default:
+				return;
+			}
+			break;
+		}
+
+		enable_bits &= ~BIT(bit);
+	}
+}
+
+static bool offset_is_clear_register(uint32_t offset)
+{
+	switch (offset) {
+	case RCC_OCENCLRR:
+	case RCC_MP_SREQCLRR:
+	case RCC_APB5RSTCLRR:
+	case RCC_AHB5RSTCLRR:
+	case RCC_MP_APB5ENCLRR:
+	case RCC_MP_AHB5ENCLRR:
+	case RCC_MP_APB5LPENCLRR:
+	case RCC_MP_AHB5LPENCLRR:
+	case RCC_MP_IWDGFZCLRR:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static void access_allowed_mask(uint32_t request, uint32_t offset,
+				uint32_t value, uint32_t allowed_mask)
+{
+	uint32_t va = stm32_rcc_base() + offset;
+
+	switch (request) {
+	case STM32_SIP_REG_WRITE:
+		if (offset_is_clear_register(offset)) {
+			/* CLR registers show the SET state, not the CLR state */
+			write32(value & allowed_mask, va);
+		} else {
+			io_mask32_stm32shregs(va, value, allowed_mask);
+		}
+		FMSG("wrt 0x%" PRIx32 "=0x%" PRIx32 " => 0x%" PRIx32,
+			offset, value, read32(va));
+		break;
+
+	case STM32_SIP_REG_SET:
+		if (offset_is_clear_register(offset)) {
+			/* CLR registers show the SET state, not the CLR state */
+			write32(value & allowed_mask, va);
+		} else {
+			io_mask32_stm32shregs(va, value, value & allowed_mask);
+		}
+		FMSG("set 0x%" PRIx32 "=0x%" PRIx32 " => 0x%" PRIx32,
+			offset, value, read32(va));
+		break;
+
+	case STM32_SIP_REG_CLEAR:
+		if (offset_is_clear_register(offset)) {
+			/* Nothing to do on CLR registers */
+		} else {
+			io_mask32_stm32shregs(va, 0, value & allowed_mask);
+		}
+		FMSG("clear 0x%" PRIx32 "=0x%" PRIx32 " => 0x%" PRIx32,
+			offset, value, read32(va));
+		break;
+
+	default:
+		break;
+	}
+}
+
+static void raw_allowed_access_request(uint32_t request,
+				       uint32_t offset, uint32_t value)
+{
+	uint32_t allowed_mask = 0;
+
+	/* Use UINT32_MAX if no secure restriction on register access */
+	switch (offset) {
+	case RCC_OCENSETR:
+	case RCC_OCENCLRR:
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_HSI)) {
+			allowed_mask |= RCC_OCENR_HSION | RCC_OCENR_HSIKERON;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_CSI)) {
+			allowed_mask |= RCC_OCENR_CSION | RCC_OCENR_CSIKERON;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_HSE)) {
+			allowed_mask |= RCC_OCENR_HSEON | RCC_OCENR_HSEKERON |
+					RCC_OCENR_HSEBYP | RCC_OCENR_HSECSSON |
+					RCC_OCENR_DIGBYP;
+		}
+		break;
+
+	case RCC_HSICFGR:
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_HSI)) {
+			allowed_mask = UINT32_MAX;
+		}
+		break;
+
+	case RCC_CSICFGR:
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_CSI)) {
+			allowed_mask = UINT32_MAX;
+		}
+		break;
+
+	case RCC_MP_CIER:
+	case RCC_MP_CIFR:
+		/* RCC_MP_CIFR_xxxRDYF matches CIER and CIFR bit mapping */
+		allowed_mask |= RCC_MP_CIFR_WKUPF | RCC_MP_CIFR_PLL4DYF;
+
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_LSI)) {
+			allowed_mask |= RCC_MP_CIFR_LSIRDYF;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_LSE)) {
+			allowed_mask |= RCC_MP_CIFR_LSERDYF;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_HSI)) {
+			allowed_mask |= RCC_MP_CIFR_HSIRDYF;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_HSE)) {
+			allowed_mask |= RCC_MP_CIFR_HSERDYF;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_CSI)) {
+			allowed_mask |= RCC_MP_CIFR_CSIRDYF;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_PLL1)) {
+			allowed_mask |= RCC_MP_CIFR_PLL1DYF;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_PLL2)) {
+			allowed_mask |= RCC_MP_CIFR_PLL2DYF;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_PLL3)) {
+			allowed_mask |= RCC_MP_CIFR_PLL3DYF;
+		}
+		break;
+
+	case RCC_PLL1CR:
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_PLL1_P)) {
+			allowed_mask |= RCC_PLLNCR_DIVPEN;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_PLL1_Q)) {
+			allowed_mask |= RCC_PLLNCR_DIVQEN;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_PLL1_R)) {
+			allowed_mask |= RCC_PLLNCR_DIVREN;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_PLL1)) {
+			allowed_mask |= RCC_PLLNCR_PLLON | RCC_PLLNCR_PLLRDY |
+					RCC_PLLNCR_SSCG_CTRL;
+		}
+		break;
+
+	case RCC_PLL2CR:
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_PLL2_P)) {
+			allowed_mask |= RCC_PLLNCR_DIVPEN;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_PLL2_Q)) {
+			allowed_mask |= RCC_PLLNCR_DIVQEN;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_PLL2_R)) {
+			allowed_mask |= RCC_PLLNCR_DIVREN;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_PLL2)) {
+			allowed_mask |= RCC_PLLNCR_PLLON | RCC_PLLNCR_PLLRDY |
+					RCC_PLLNCR_SSCG_CTRL;
+		}
+		break;
+
+	case RCC_PLL3CR:
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_PLL3_P)) {
+			allowed_mask |= RCC_PLLNCR_DIVPEN;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_PLL3_Q)) {
+			allowed_mask |= RCC_PLLNCR_DIVQEN;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_PLL3_R)) {
+			allowed_mask |= RCC_PLLNCR_DIVREN;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_PLL3)) {
+			allowed_mask |= RCC_PLLNCR_PLLON | RCC_PLLNCR_PLLRDY |
+					RCC_PLLNCR_SSCG_CTRL;
+		}
+		break;
+
+	case RCC_MP_BOOTCR:		/* Allowed MPU/MCU reboot cfg */
+	case RCC_MP_GCR:		/* Allowed MPU/MCU reboot cfg */
+	case RCC_MP_GRSTCSETR:		/* Allowed MCU and system reset */
+	case RCC_BR_RSTSCLRR:		/* Allowed system reset status */
+	case RCC_MC_RSTSCLRR:		/* Allowed system reset status */
+	case RCC_MP_RSTSCLRR:		/* Allowed system reset status */
+		allowed_mask = UINT32_MAX;
+		break;
+	case RCC_APB5RSTSETR:
+	case RCC_APB5RSTCLRR:
+	case RCC_MP_APB5ENSETR:
+	case RCC_MP_APB5ENCLRR:
+	case RCC_MP_APB5LPENSETR:
+	case RCC_MP_APB5LPENCLRR:
+		/*
+		 * SPI6/I2C4/I2C6/USART1/RTC/IWDG1 resources may be non secure.
+		 * TZPC/TZC/BSEC/STGEN resources are secure only.
+		 * Bit mask RCC_MP_APB5ENSETR_xxxEN fits EN, RST and LPEN.
+		 */
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_SPI6)) {
+			allowed_mask |= RCC_MP_APB5ENSETR_SPI6EN;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_I2C4)) {
+			allowed_mask |= RCC_MP_APB5ENSETR_I2C4EN;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_I2C6)) {
+			allowed_mask |= RCC_MP_APB5ENSETR_I2C6EN;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_USART1)) {
+			allowed_mask |= RCC_MP_APB5ENSETR_USART1EN;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_IWDG1)) {
+			allowed_mask |= RCC_MP_APB5ENSETR_IWDG1APBEN;
+		}
+		break;
+	case RCC_AHB5RSTSETR:
+	case RCC_AHB5RSTCLRR:
+	case RCC_MP_AHB5ENSETR:
+	case RCC_MP_AHB5ENCLRR:
+	case RCC_MP_AHB5LPENSETR:
+	case RCC_MP_AHB5LPENCLRR:
+		/*
+		 * RNG1/HASH1/CRYP1/GPIOZ/AXIMC resources are accessible if related
+		 * BKPSRAM resources are reserved to secure services.
+		 * Bit mask RCC_MP_AHB5ENSETR_xxxEN fits EN, RST and LPEN.
+		 */
+		if (stm32mp_gpio_bank_is_non_secure(GPIO_BANK_Z)) {
+			allowed_mask |= RCC_MP_AHB5ENSETR_GPIOZEN;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_CRYP1)) {
+			allowed_mask |= RCC_MP_AHB5ENSETR_CRYP1EN;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_HASH1)) {
+			allowed_mask |= RCC_MP_AHB5ENSETR_HASH1EN;
+		}
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_RNG1)) {
+			allowed_mask |= RCC_MP_AHB5ENSETR_RNG1EN;
+		}
+		break;
+	case RCC_RTCDIVR:
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_RTC)) {
+			allowed_mask = UINT32_MAX;
+		}
+		break;
+	case RCC_I2C46CKSELR:
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_I2C4) &&
+		    stm32mp_periph_is_non_secure(STM32MP1_SHRES_I2C6)) {
+			allowed_mask = UINT32_MAX;
+		}
+		break;
+	case RCC_SPI6CKSELR:
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_SPI6)) {
+			allowed_mask = UINT32_MAX;
+		}
+		break;
+	case RCC_UART1CKSELR:
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_USART1)) {
+			allowed_mask = UINT32_MAX;
+		}
+		break;
+	case RCC_RNG1CKSELR:
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_RNG1)) {
+			allowed_mask = UINT32_MAX;
+		}
+		break;
+	case RCC_MP_IWDGFZSETR:
+	case RCC_MP_IWDGFZCLRR:
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_IWDG1)) {
+			allowed_mask |= RCC_MP_IWDGFZSETR_IWDG1;
+		}
+		allowed_mask |= RCC_MP_IWDGFZSETR_IWDG2;
+		break;
+	default:
+		return;
+	}
+
+	if (allowed_mask != 0U) {
+		access_allowed_mask(request, offset, value, allowed_mask);
+	}
+}
+
+uint32_t rcc_scv_handler(uint32_t x1, uint32_t x2, uint32_t x3)
+{
+	uint32_t request = x1;
+	uint32_t offset = x2;
+	uint32_t value = x3;
+
+	/*
+	 * Argument x2 can be either the register physical address of the
+	 * register offset toward RCC_BASE.
+	 */
+	if ((offset & ~RCC_OFFSET_MASK) != 0) {
+		if ((offset & ~RCC_OFFSET_MASK) != RCC_BASE) {
+			return STM32_SIP_INVALID_PARAMS;
+		}
+
+		offset &= RCC_OFFSET_MASK;
+	}
+
+	DMSG_RAW("RCC service: %s 0x%" PRIx32 " at offset 0x%" PRIx32,
+		request == STM32_SIP_REG_WRITE ? "write" :
+		request == STM32_SIP_REG_SET ? "set" : "clear",
+		value, offset);
+
+	/* Some clocks may be managed by some secure services */
+	shared_clk_request(request, offset, value);
+
+	/* RCC controls for non secure resource may be accessed straight */
+	raw_allowed_access_request(request, offset, value);
+
+	return STM32_SIP_OK;
+}
diff --git a/core/arch/arm/plat-stm32mp1/service/rcc_svc.h b/core/arch/arm/plat-stm32mp1/service/rcc_svc.h
new file mode 100644
index 0000000..f198ebd
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/service/rcc_svc.h
@@ -0,0 +1,11 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics
+ */
+
+#ifndef __RCC_SVC_H__
+#define __RCC_SVC_H__
+
+uint32_t rcc_scv_handler(uint32_t x1, uint32_t x2, uint32_t x3);
+
+#endif /*__RCC_SVC_H__*/
diff --git a/core/arch/arm/plat-stm32mp1/service/stm32mp1_smc.h b/core/arch/arm/plat-stm32mp1/service/stm32mp1_smc.h
new file mode 100644
index 0000000..2db7b10
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/service/stm32mp1_smc.h
@@ -0,0 +1,211 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2016-2018, STMicroelectronics
+ * Copyright (c) 2018, Linaro Limited
+ */
+#ifndef __STM32MP1_SMC_H__
+#define __STM32MP1_SMC_H__
+
+/*
+ * SIP Functions
+ */
+#define STM32_SIP_SVC_VERSION_MAJOR		0x0
+#define STM32_SIP_SVC_VERSION_MINOR		0x1
+
+/* SIP service generic return codes */
+#define STM32_SIP_OK				0x0
+#define STM32_SIP_NOT_SUPPORTED			0xffffffffU
+#define STM32_SIP_FAILED			0xfffffffeU
+#define STM32_SIP_INVALID_PARAMS		0xfffffffdU
+
+/*
+ * 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
+ */
+
+/*
+ * SIP function STM32_SIP_FUNC_CALL_COUNT
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) dummy value 0
+ */
+#define STM32_SIP_FUNC_CALL_COUNT		0xff00
+
+/*
+ * SIP function STM32_SIP_FUNC_UID
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) Lowest 32bit of the stm32mp1 SIP service UUID
+ * Argument a1: (output) Next 32bit of the stm32mp1 SIP service UUID
+ * Argument a2: (output) Next 32bit of the stm32mp1 SIP service UUID
+ * Argument a3: (output) Last 32bit of the stm32mp1 SIP service UUID
+ */
+#define STM32_SIP_FUNC_UID			0xff01
+
+/*
+ * SIP function STM32_SIP_FUNC_VERSION
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) STM32 SIP service major
+ * Argument a1: (output) STM32 SIP service minor
+ */
+#define STM32_SIP_FUNC_VERSION			0xff03
+
+/*
+ * SIP function STM32_SIP_FUNC_RCC
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) status return code
+ * Argument a1: (input) Service ID (STM32_SIP_REG_xxx)
+ * Argument a2: (input) register offset or physical address
+ *		(output) register read value, if applicable
+ * Argument a3: (input) register target value if applicable
+ */
+#define STM32_SIP_FUNC_RCC			0x1000
+
+/*
+ * SIP function STM32_SIP_FUNC_PWR
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) status return code
+ * Argument a1: (input) Service ID (STM32_SIP_REG_xxx)
+ * Argument a2: (input) register offset or physical address
+ *		(output) register read value, if applicable
+ * Argument a3: (input) register target value if applicable
+ */
+#define STM32_SIP_FUNC_PWR			0x1001
+
+/* Service ID for STM32_SIP_FUNC_RCC/_PWR */
+#define STM32_SIP_REG_READ			0x0
+#define STM32_SIP_REG_WRITE			0x1
+#define STM32_SIP_REG_SET			0x2
+#define STM32_SIP_REG_CLEAR			0x3
+
+/*
+ * SIP functions STM32_SIP_FUNC_BSEC
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) status return code
+ * Argument a1: (input) Clock ID (from DT clock bindings)
+ */
+#define STM32_SIP_RCC_CAL			0x1002
+
+/*
+ * SIP functions STM32_SIP_FUNC_BSEC
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) status return code
+ * Argument a1: (input) Service ID (STM32_SIP_BSEC_xxx)
+ * Argument a2: (input) OTP index
+ *		(output) OTP read value, if applicable
+ * Argument a3: (input) OTP value if applicable
+ */
+#define STM32_SIP_FUNC_BSEC			0x1003
+
+/* Service ID for STM32_SIP_FUNC_BSEC */
+#define STM32_SIP_BSEC_READ_SHADOW		0x1
+#define STM32_SIP_BSEC_PROG_OTP			0x2
+#define STM32_SIP_BSEC_WRITE_SHADOW		0x3
+#define STM32_SIP_BSEC_READ_OTP			0x4
+
+/*
+ * SIP functions STM32_SIP_FUNC_SR_MODE
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) status return code
+ * Argument a1: (unused)
+ * Argument a2: (input) Target selfrefresh mode
+ */
+#define STM32_SIP_FUNC_SR_MODE			0x1004
+
+/* DDR Self-Refresh modes */
+#define STM32_SIP_SR_MODE_SSR			0x0
+#define STM32_SIP_SR_MODE_ASR			0x1
+#define STM32_SIP_SR_MODE_HSR			0x2
+
+/*
+ * SIP functions STM32_SIP_FUNC_CSTOP
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) status return code
+ * Argument a1: (unused)
+ * Argument a2: (unused)
+ * Argument a3: (input) Target SoC mode
+ */
+#define STM32_SIP_FUNC_CSTOP			0x1005
+
+/* Valid SoC modes used for CSTOP, */
+#define STM32_SIP_CSLEEP_RUN			0x0
+#define STM32_SIP_CSTOP_ALLOW_STOP		0x1
+#define STM32_SIP_CSTOP_ALLOW_LP_STOP		0x2
+#define STM32_SIP_CSTOP_ALLOW_LPLV_STOP		0x3
+#define STM32_SIP_CSTOP_ALLOW_STANDBY		0x4
+#define STM32_SIP_CSTOP_ALLOW_STANDBY_DDR_OFF	0x5
+#define STM32_SIP_CSTOP_SHUTDOWN		0x6
+
+/*
+ * SIP functions STM32_SIP_FUNC_STANDBY
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) status return code
+ * Argument a1: (unused)
+ * Argument a2: (unused)
+ * Argument a3: (input) non null only for DDR off standby
+ */
+#define STM32_SIP_FUNC_STANDBY			0x1006
+
+/*
+ * SIP function STM32_SIP_FUNC_SHUTDOWN
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) status return code
+ */
+#define STM32_SIP_FUNC_SHUTDOWN			0x1007
+
+/*
+ * SIP function STM32_SIP_FUNC_PD_DOMAIN
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) status return code
+ * Argument a2: (index) ID of target power domain to be enabled/disabled
+ * Argument a3: (input) 0 to disable, 1 to eanble target domain
+ */
+#define STM32_SIP_FUNC_PD_DOMAIN		0x1008
+
+/* Valid IDs for power domain for function STM32_SIP_FUNC_PD_DOMAIN */
+#define STM32_SIP_PD_VSW			0x0
+#define STM32_SIP_PD_CORE_RET			0x1
+#define STM32_SIP_PD_CORE			0x2
+#define STM32_SIP_PD_MAX_PM_DOMAIN		0x3
+
+/*
+ * OEM Functions
+ */
+#define STM32_OEM_SVC_VERSION_MAJOR		0x0
+#define STM32_OEM_SVC_VERSION_MINOR		0x1
+
+/* OEM service generic return codes */
+#define STM32_OEM_OK				0x0
+#define STM32_OEM_NOT_SUPPORTED			0xffffffffU
+#define STM32_OEM_FAILED			0xfffffffeU
+#define STM32_OEM_INVALID_PARAMS		0xfffffffdU
+
+/*
+ * OEM function STM32_OEM_FUNC_LP_FORCE_PARAMS
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) status return code
+ * Argument a2: (input) ID of the mode: suspend or shutdown (off)
+ * Argument a3: (input) ID of the power state to be reached for the mode
+ *                      Refer to stm32mp1 power bindings.
+ */
+#define STM32_OEM_FUNC_LP_FORCE_PARAMS		0x0f800
+
+/* Valid IDs for power mode configured with STM32_OEM_FUNC_LP_FORCE_PARAMS */
+#define STM32_OEM_LP_FORCE_SUSPEND_PARAMS	0
+#define STM32_OEM_LP_FORCE_OFF_PARAMS		1
+
+#endif /* __STM32MP1_SMC_H__*/
diff --git a/core/arch/arm/plat-stm32mp1/service/stm32mp1_svc_setup.c b/core/arch/arm/plat-stm32mp1/service/stm32mp1_svc_setup.c
new file mode 100644
index 0000000..3c7958c
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/service/stm32mp1_svc_setup.c
@@ -0,0 +1,129 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics
+ */
+
+#include <arm.h>
+#include <assert.h>
+#include <drivers/stm32_bsec.h>
+#include <kernel/thread.h>
+#include <sm/optee_smc.h>
+#include <sm/sm.h>
+#include <stm32_util.h>
+#include <stdint.h>
+#include <string.h>
+#include <tee_api_types.h>
+#include <trace.h>
+
+#include "bsec_svc.h"
+#include "low_power_svc.h"
+#include "pwr_svc.h"
+#include "rcc_svc.h"
+#include "stm32mp1_smc.h"
+
+static uint32_t __maybe_unused calib_scv_handler(uint32_t x1)
+{
+	unsigned long clock_id = x1;
+
+	return (stm32mp_start_clock_calib(clock_id) == 0) ?
+		STM32_SIP_OK : STM32_SIP_FAILED;
+}
+
+/* STM32 SiP Service UUID */
+static const TEE_UUID stm32mp1_sip_svc_uid = {
+	0x50aa78a7, 0x9bf4, 0x4a14,
+	{ 0x8a, 0x5e, 0x26, 0x4d, 0x59, 0x94, 0xc2, 0x14 }
+};
+
+static void get_sip_func_uid(uint32_t *a0, uint32_t *a1,
+			     uint32_t *a2, uint32_t *a3)
+{
+	const void *uid = &stm32mp1_sip_svc_uid;
+
+	memcpy(a0, (char *)uid, sizeof(uint32_t));
+	memcpy(a1, (char *)uid + sizeof(uint32_t), sizeof(uint32_t));
+	memcpy(a2, (char *)uid + (sizeof(uint32_t) * 2), sizeof(uint32_t));
+	memcpy(a3, (char *)uid + (sizeof(uint32_t) * 3), sizeof(uint32_t));
+}
+
+bool stm32_sip_service(struct sm_ctx __unused *ctx,
+		       uint32_t *a0, uint32_t *a1, uint32_t *a2, uint32_t *a3)
+{
+	switch (OPTEE_SMC_FUNC_NUM(*a0)) {
+	case STM32_SIP_FUNC_CALL_COUNT:
+		/* This service is meaningless, return a dummy value */
+		*a0 = 0;
+		break;
+	case STM32_SIP_FUNC_VERSION:
+		*a0 = STM32_SIP_SVC_VERSION_MAJOR;
+		*a1 = STM32_SIP_SVC_VERSION_MINOR;
+		break;
+	case STM32_SIP_FUNC_UID:
+		get_sip_func_uid(a0, a1, a2, a3);
+		break;
+#ifdef CFG_STM32_BSEC_SIP
+	case STM32_SIP_FUNC_BSEC:
+		*a0 = bsec_main(*a1, *a2, *a3, a1);
+		break;
+#endif
+#ifdef CFG_STM32_RCC_SIP
+	case STM32_SIP_FUNC_RCC:
+		*a0 = rcc_scv_handler(*a1, *a2, *a3);
+		break;
+#endif
+#ifdef CFG_STM32_CLOCKSRC_CALIB
+	case STM32_SIP_RCC_CAL:
+		*a0 = calib_scv_handler(*a1);
+		break;
+#endif
+#ifdef CFG_STM32_PWR_SIP
+	case STM32_SIP_FUNC_PWR:
+		*a0 = pwr_scv_handler(*a1, *a2, *a3);
+		break;
+#endif
+#ifdef CFG_STM32_POWER_SERVICES
+	case STM32_SIP_FUNC_SR_MODE:
+		*a0 = sr_mode_scv_handler(*a1, *a2);
+		break;
+
+	case STM32_SIP_FUNC_CSTOP:
+		*a0 = cstop_scv_handler(ctx, *a1, *a2, *a3);
+		break;
+
+	case STM32_SIP_FUNC_STANDBY:
+		*a0 = standby_scv_handler(ctx, *a1, *a2, *a3);
+		break;
+
+	case STM32_SIP_FUNC_SHUTDOWN:
+		*a0 = shutdown_scv_handler();
+		break;
+
+	case STM32_SIP_FUNC_PD_DOMAIN:
+		*a0 = pm_domain_scv_handler(*a1, *a2);
+		break;
+#endif
+
+	default:
+		return true;
+	}
+
+	return false;
+}
+
+bool stm32_oem_service(struct sm_ctx __unused *ctx,
+		       uint32_t *a0, uint32_t *a1 __maybe_unused,
+		       uint32_t *a2 __maybe_unused, uint32_t *a3 __unused)
+{
+	switch (OPTEE_SMC_FUNC_NUM(*a0)) {
+#ifdef CFG_STM32_POWER_SERVICES
+	case STM32_OEM_FUNC_LP_FORCE_PARAMS:
+		*a0 = pm_set_lp_state_scv_handler(*a1, *a2);
+		break;
+#endif
+
+	default:
+		return true;
+	}
+
+	return false;
+}
diff --git a/core/arch/arm/plat-stm32mp1/service/sub.mk b/core/arch/arm/plat-stm32mp1/service/sub.mk
new file mode 100644
index 0000000..0797b8f
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/service/sub.mk
@@ -0,0 +1,7 @@
+global-incdirs-y += .
+
+srcs-y += stm32mp1_svc_setup.c
+srcs-$(CFG_STM32_BSEC_SIP) += bsec_svc.c
+srcs-$(CFG_STM32_PWR_SIP) += pwr_svc.c
+srcs-$(CFG_STM32_RCC_SIP) += rcc_svc.c
+srcs-$(CFG_STM32_POWER_SERVICES) += low_power_svc.c
diff --git a/core/arch/arm/plat-stm32mp1/shared_resources.c b/core/arch/arm/plat-stm32mp1/shared_resources.c
new file mode 100644
index 0000000..0eb929c
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/shared_resources.c
@@ -0,0 +1,1007 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics
+ */
+
+#include <drivers/stm32_gpio.h>
+#include <drivers/stm32_etzpc.h>
+#include <drivers/stm32mp1_clk.h>
+#include <drivers/stm32mp1_rcc.h>
+#include <dt-bindings/etzpc/stm32-etzpc.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <initcall.h>
+#include <io.h>
+#include <keep.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <kernel/spinlock.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <stm32_util.h>
+#include <stm32mp_dt.h>
+#include <stm32mp_pm.h>
+#include <stdbool.h>
+#include <string.h>
+
+static unsigned int shregs_lock = SPINLOCK_UNLOCK;
+static bool registering_locked;
+
+/* Shared resource lock: assume not required if MMU is disabled */
+uint32_t lock_stm32shregs(void)
+{
+	return may_spin_lock(&shregs_lock);
+}
+
+void unlock_stm32shregs(uint32_t exceptions)
+{
+	may_spin_unlock(&shregs_lock, exceptions);
+}
+
+/* Shared register access: upon shared resource lock */
+void io_mask32_stm32shregs(uintptr_t va, uint32_t value, uint32_t mask)
+{
+	uint32_t exceptions = lock_stm32shregs();
+
+	io_mask32(va, value, mask);
+
+	unlock_stm32shregs(exceptions);
+}
+
+/*
+ * Generic clock enable/disable from secure world.
+ * Some drivers may use non secure resources in specific execution context:
+ * when the other SMP core(s) are offline and non secure is never reached.
+ * In such cases, drivers shall enable/disable the HW clock only if it was not
+ * left enabled by the non secure world.
+ *
+ * During driver initializations, before registering_locked is locked, all
+ * driver simply enable/disable the clock as if the peripheral was secure.
+ */
+void stm32_clock_enable(unsigned long id)
+{
+	if (registering_locked) {
+		if (stm32mp_clock_is_non_secure(id)) {
+			assert(!stm32mp1_clk_get_refcount(id));
+
+			if (stm32mp1_clk_is_enabled(id)) {
+				return;
+			}
+		}
+	}
+
+	stm32mp1_clk_enable_secure(id);
+}
+
+void stm32_clock_disable(unsigned long id)
+{
+	if (registering_locked) {
+		if (stm32mp_clock_is_non_secure(id)) {
+			if (!stm32mp1_clk_get_refcount(id)) {
+				return;
+			}
+		}
+	}
+
+	stm32mp1_clk_disable_secure(id);
+}
+
+/*
+ * Shared peripherals and resources.
+ * Defines resource that may be non secure, secure or shared.
+ * May be a device, a bus, a clock, a memory.
+ *
+ * State default to PERIPH_UNREGISTERED resource is not explicitly
+ * set here.
+ *
+ * Resource driver not built (upon CFG_xxx), the resource defaults
+ * to non secure ownership.
+ *
+ * Each IO of the GPIOZ IO can be secure or non secure.
+ * When the GPIO driver is enabled, the GPIOZ bank is fully non secure
+ * only if each IO is non secure and the GPIOZ bank is shared if it
+ * includes secure and non secure IOs.
+ *
+ * BKPSRAM is assumed shared.
+ * DDR control (DDRC and DDRPHY) is secure.
+ * Inits will define the resource state according the device tree
+ * and the driver initialization sequences.
+ *
+ * The platform initialization uses these information to set the ETZPC
+ * configuration. Non secure services (as clocks or regulator accesses)
+ * rely on these information to drive the related service execution.
+ */
+#define SHRES_NON_SECURE		3
+#define SHRES_SHARED			2
+#define SHRES_SECURE			1
+#define SHRES_UNREGISTERED		0
+
+static uint8_t shres_state[STM32MP1_SHRES_COUNT] = {
+#if !defined(CFG_STM32_IWDG)
+	[STM32MP1_SHRES_IWDG1] = SHRES_NON_SECURE,
+#endif
+#if !defined(CFG_STM32_UART)
+	[STM32MP1_SHRES_USART1] = SHRES_NON_SECURE,
+#endif
+#if !defined(CFG_STM32_SPI)
+	[STM32MP1_SHRES_SPI6] = SHRES_NON_SECURE,
+#endif
+#if !defined(CFG_STM32_I2C)
+	[STM32MP1_SHRES_I2C4] = SHRES_NON_SECURE,
+	[STM32MP1_SHRES_I2C6] = SHRES_NON_SECURE,
+#endif
+#if !defined(CFG_STM32_GPIO)
+	[STM32MP1_SHRES_GPIOZ(0)] = SHRES_NON_SECURE,
+	[STM32MP1_SHRES_GPIOZ(1)] = SHRES_NON_SECURE,
+	[STM32MP1_SHRES_GPIOZ(2)] = SHRES_NON_SECURE,
+	[STM32MP1_SHRES_GPIOZ(3)] = SHRES_NON_SECURE,
+	[STM32MP1_SHRES_GPIOZ(4)] = SHRES_NON_SECURE,
+	[STM32MP1_SHRES_GPIOZ(5)] = SHRES_NON_SECURE,
+	[STM32MP1_SHRES_GPIOZ(6)] = SHRES_NON_SECURE,
+	[STM32MP1_SHRES_GPIOZ(7)] = SHRES_NON_SECURE,
+#endif
+#if !defined(CFG_STM32_RNG)
+	[STM32MP1_SHRES_RNG1] = SHRES_NON_SECURE,
+#endif
+#if !defined(CFG_STM32_HASH)
+	[STM32MP1_SHRES_HASH1] = SHRES_NON_SECURE,
+#endif
+#if !defined(CFG_STM32_CRYP)
+	[STM32MP1_SHRES_CRYP1] = SHRES_NON_SECURE,
+#endif
+#if !defined(CFG_STM32_RTC)
+	[STM32MP1_SHRES_RTC] = SHRES_NON_SECURE,
+#endif
+};
+
+#if CFG_TEE_CORE_LOG_LEVEL > 0
+static const char *shres2str_id_tbl[STM32MP1_SHRES_COUNT] = {
+	[STM32MP1_SHRES_GPIOZ(0)] = "GPIOZ0",
+	[STM32MP1_SHRES_GPIOZ(1)] = "GPIOZ1",
+	[STM32MP1_SHRES_GPIOZ(2)] = "GPIOZ2",
+	[STM32MP1_SHRES_GPIOZ(3)] = "GPIOZ3",
+	[STM32MP1_SHRES_GPIOZ(4)] = "GPIOZ4",
+	[STM32MP1_SHRES_GPIOZ(5)] = "GPIOZ5",
+	[STM32MP1_SHRES_GPIOZ(6)] = "GPIOZ6",
+	[STM32MP1_SHRES_GPIOZ(7)] = "GPIOZ7",
+	[STM32MP1_SHRES_IWDG1] = "IWDG1",
+	[STM32MP1_SHRES_USART1] = "USART1",
+	[STM32MP1_SHRES_SPI6] = "SPI6",
+	[STM32MP1_SHRES_I2C4] = "I2C4",
+	[STM32MP1_SHRES_RNG1] = "RNG1",
+	[STM32MP1_SHRES_HASH1] = "HASH1",
+	[STM32MP1_SHRES_CRYP1] = "CRYP1",
+	[STM32MP1_SHRES_I2C6] = "I2C6",
+	[STM32MP1_SHRES_RTC] = "RTC",
+	[STM32MP1_SHRES_MCU] = "MCU",
+	[STM32MP1_SHRES_HSI] = "HSI",
+	[STM32MP1_SHRES_LSI] = "LSI",
+	[STM32MP1_SHRES_HSE] = "HSE",
+	[STM32MP1_SHRES_LSE] = "LSE",
+	[STM32MP1_SHRES_CSI] = "CSI",
+	[STM32MP1_SHRES_PLL1] = "PLL1",
+	[STM32MP1_SHRES_PLL1_P] = "PLL1_P",
+	[STM32MP1_SHRES_PLL1_Q] = "PLL1_Q",
+	[STM32MP1_SHRES_PLL1_R] = "PLL1_R",
+	[STM32MP1_SHRES_PLL2] = "PLL2",
+	[STM32MP1_SHRES_PLL2_P] = "PLL2_P",
+	[STM32MP1_SHRES_PLL2_Q] = "PLL2_Q",
+	[STM32MP1_SHRES_PLL2_R] = "PLL2_R",
+	[STM32MP1_SHRES_PLL3] = "PLL3",
+	[STM32MP1_SHRES_PLL3_P] = "PLL3_P",
+	[STM32MP1_SHRES_PLL3_Q] = "PLL3_Q",
+	[STM32MP1_SHRES_PLL3_R] = "PLL3_R",
+};
+
+static const char *shres2str_id(unsigned int id)
+{
+	return shres2str_id_tbl[id];
+}
+
+static const char *shres2str_state_tbl[4] = {
+	[SHRES_SHARED] = "shared",
+	[SHRES_NON_SECURE] = "non secure",
+	[SHRES_SECURE] = "secure",
+	[SHRES_UNREGISTERED] = "unregistered",
+};
+
+static const char *shres2str_state(unsigned int id)
+{
+	return shres2str_state_tbl[id];
+}
+#else
+static __maybe_unused const char *shres2str_id(unsigned int __unused id)
+{
+	return NULL;
+}
+
+static __maybe_unused const char *shres2str_state(unsigned int __unused id)
+{
+	return NULL;
+}
+#endif
+
+struct shres2decprot {
+	unsigned int shres_id;
+	unsigned int decprot_id;
+	const char *decprot_str;
+};
+
+#define SHRES2DECPROT(shres, decprot, str) {	\
+		.shres_id = shres,		\
+		.decprot_id = decprot,		\
+		.decprot_str = str,		\
+	}
+
+#define SHRES_INVALID		~0U
+static const struct shres2decprot shres2decprot_tbl[] = {
+	SHRES2DECPROT(STM32MP1_SHRES_IWDG1, STM32MP1_ETZPC_IWDG1_ID, "IWDG1"),
+	SHRES2DECPROT(STM32MP1_SHRES_USART1, STM32MP1_ETZPC_USART1_ID, "UART1"),
+	SHRES2DECPROT(STM32MP1_SHRES_SPI6, STM32MP1_ETZPC_SPI6_ID, "SPI6"),
+	SHRES2DECPROT(STM32MP1_SHRES_I2C4, STM32MP1_ETZPC_I2C4_ID, "I2C4"),
+	SHRES2DECPROT(STM32MP1_SHRES_RNG1, STM32MP1_ETZPC_RNG1_ID, "RNG1"),
+	SHRES2DECPROT(STM32MP1_SHRES_HASH1, STM32MP1_ETZPC_HASH1_ID, "HASH1"),
+	SHRES2DECPROT(STM32MP1_SHRES_CRYP1, STM32MP1_ETZPC_CRYP1_ID, "CRYP1"),
+	SHRES2DECPROT(STM32MP1_SHRES_I2C6, STM32MP1_ETZPC_I2C6_ID, "I2C6"),
+	/* Below are specific IDs without a 1-to-1 mapping to SHRES IDs */
+	SHRES2DECPROT(SHRES_INVALID, STM32MP1_ETZPC_STGENC_ID, "STGEN"),
+	SHRES2DECPROT(SHRES_INVALID, STM32MP1_ETZPC_BKPSRAM_ID, "BKPSRAM"),
+	SHRES2DECPROT(SHRES_INVALID, STM32MP1_ETZPC_GPIOZ_ID, "GPIOZ"),
+	SHRES2DECPROT(SHRES_INVALID, STM32MP1_ETZPC_DDRCTRL_ID, "DDRCTRL"),
+	SHRES2DECPROT(SHRES_INVALID, STM32MP1_ETZPC_DDRPHYC_ID, "DDRPHY"),
+};
+
+static unsigned int decprot2shres(unsigned int decprot_id)
+{
+	size_t i;
+
+	for (i = 0; i < ARRAY_SIZE(shres2decprot_tbl); i++) {
+		if (shres2decprot_tbl[i].decprot_id == decprot_id) {
+			return shres2decprot_tbl[i].shres_id;
+		}
+	}
+
+	DMSG("No share resource ID %u", decprot_id);
+	return SHRES_INVALID;
+}
+
+static const char *decprot2str(unsigned int decprot_id)
+{
+	size_t i;
+
+	for (i = 0; i < ARRAY_SIZE(shres2decprot_tbl); i++) {
+		if (shres2decprot_tbl[i].decprot_id == decprot_id) {
+			return shres2decprot_tbl[i].decprot_str;
+		}
+	}
+
+	EMSG("Invalid ID %u", decprot_id);
+	panic();
+}
+
+/* GPIOZ bank may have several number of pins */
+#if CFG_DT
+static int gpioz_nbpin = -1;
+
+static unsigned int get_gpioz_nbpin_unpg(void)
+{
+	if (gpioz_nbpin >= 0) {
+		return (unsigned int)gpioz_nbpin;
+	}
+
+	panic();
+}
+KEEP_PAGER(get_gpioz_nbpin_unpg);
+
+static unsigned int get_gpioz_nbpin(void)
+{
+	if (gpioz_nbpin < 0) {
+		void *fdt;
+
+		fdt = get_dt_blob();
+		if (fdt == NULL) {
+			panic();
+		}
+
+		gpioz_nbpin = fdt_get_gpioz_nbpins_from_dt(fdt);
+		assert((gpioz_nbpin == 0) ||
+		       (gpioz_nbpin == STM32MP1_GPIOZ_PIN_MAX_COUNT));
+	}
+
+	return (unsigned int)gpioz_nbpin;
+}
+#else
+static unsigned int get_gpioz_nbpin_unpg(void)
+{
+	return STM32MP1_GPIOZ_PIN_MAX_COUNT;
+}
+
+static unsigned int get_gpioz_nbpin(void)
+{
+	return get_gpioz_nbpin_unpg();
+}
+#endif
+
+static bool shareable_resource(unsigned int id)
+{
+	switch (id) {
+	default:
+		/* Currently no shareable resource */
+		return false;
+	}
+}
+
+static bool mckprot_resource(unsigned int id)
+{
+	switch (id) {
+	case STM32MP1_SHRES_MCU:
+	case STM32MP1_SHRES_PLL3:
+	case STM32MP1_SHRES_PLL3_P:
+	case STM32MP1_SHRES_PLL3_Q:
+	case STM32MP1_SHRES_PLL3_R:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static void register_periph(unsigned int id, unsigned int state)
+{
+	assert(id < STM32MP1_SHRES_COUNT &&
+	       state > SHRES_UNREGISTERED &&
+	       state <= SHRES_NON_SECURE);
+
+	if (registering_locked) {
+		panic();
+	}
+
+	if ((state == SHRES_SHARED && !shareable_resource(id)) ||
+	    ((shres_state[id] != SHRES_UNREGISTERED) &&
+	     (shres_state[id] != state))) {
+		DMSG("Cannot change %s from %s to %s",
+		     shres2str_id(id),
+		     shres2str_state(shres_state[id]),
+		     shres2str_state(state));
+		panic();
+	}
+
+	if (shres_state[id] == SHRES_UNREGISTERED) {
+		DMSG("Register %s as %s",
+			shres2str_id(id), shres2str_state(state));
+	}
+
+	switch (id) {
+	case STM32MP1_SHRES_GPIOZ(0):
+	case STM32MP1_SHRES_GPIOZ(1):
+	case STM32MP1_SHRES_GPIOZ(2):
+	case STM32MP1_SHRES_GPIOZ(3):
+	case STM32MP1_SHRES_GPIOZ(4):
+	case STM32MP1_SHRES_GPIOZ(5):
+	case STM32MP1_SHRES_GPIOZ(6):
+	case STM32MP1_SHRES_GPIOZ(7):
+		if ((id - STM32MP1_SHRES_GPIOZ(0)) >= get_gpioz_nbpin()) {
+			EMSG("Invalid GPIO pin %u, %u pin(s) available",
+			     id - STM32MP1_SHRES_GPIOZ(0), get_gpioz_nbpin());
+			panic();
+		}
+		break;
+	default:
+		break;
+	}
+
+	shres_state[id] = (uint8_t)state;
+
+	/* Explore clock tree to lock dependencies */
+	if ((state == SHRES_SECURE) || (state == SHRES_SHARED)) {
+		switch (id) {
+		case STM32MP1_SHRES_GPIOZ(0):
+		case STM32MP1_SHRES_GPIOZ(1):
+		case STM32MP1_SHRES_GPIOZ(2):
+		case STM32MP1_SHRES_GPIOZ(3):
+		case STM32MP1_SHRES_GPIOZ(4):
+		case STM32MP1_SHRES_GPIOZ(5):
+		case STM32MP1_SHRES_GPIOZ(6):
+		case STM32MP1_SHRES_GPIOZ(7):
+			stm32mp_register_clock_parents_secure(GPIOZ);
+			break;
+		case STM32MP1_SHRES_IWDG1:
+			stm32mp_register_clock_parents_secure(IWDG1);
+			break;
+		case STM32MP1_SHRES_USART1:
+			stm32mp_register_clock_parents_secure(USART1_K);
+			break;
+		case STM32MP1_SHRES_SPI6:
+			stm32mp_register_clock_parents_secure(SPI6_K);
+			break;
+		case STM32MP1_SHRES_I2C4:
+			stm32mp_register_clock_parents_secure(I2C4_K);
+			break;
+		case STM32MP1_SHRES_RNG1:
+			stm32mp_register_clock_parents_secure(RNG1_K);
+			break;
+		case STM32MP1_SHRES_HASH1:
+			stm32mp_register_clock_parents_secure(HASH1);
+			break;
+		case STM32MP1_SHRES_CRYP1:
+			stm32mp_register_clock_parents_secure(CRYP1);
+			break;
+		case STM32MP1_SHRES_I2C6:
+			stm32mp_register_clock_parents_secure(I2C6_K);
+			break;
+		case STM32MP1_SHRES_RTC:
+			stm32mp_register_clock_parents_secure(RTC);
+			break;
+		case STM32MP1_SHRES_PLL1_P:
+		case STM32MP1_SHRES_PLL1_Q:
+		case STM32MP1_SHRES_PLL1_R:
+			register_periph(STM32MP1_SHRES_PLL1, SHRES_SECURE);
+			stm32mp_register_clock_parents_secure(PLL1);
+			break;
+		case STM32MP1_SHRES_PLL2_P:
+		case STM32MP1_SHRES_PLL2_Q:
+		case STM32MP1_SHRES_PLL2_R:
+			register_periph(STM32MP1_SHRES_PLL2, SHRES_SECURE);
+			stm32mp_register_clock_parents_secure(PLL2);
+			break;
+		case STM32MP1_SHRES_PLL3_P:
+		case STM32MP1_SHRES_PLL3_Q:
+		case STM32MP1_SHRES_PLL3_R:
+			register_periph(STM32MP1_SHRES_PLL3, SHRES_SECURE);
+			stm32mp_register_clock_parents_secure(PLL3);
+			break;
+		default:
+			/* No expected resource dependency */
+			break;
+		}
+	}
+}
+
+/* Register resource by ID */
+void stm32mp_register_secure_periph(unsigned int id)
+{
+	register_periph(id, SHRES_SECURE);
+}
+
+void stm32mp_register_shared_periph(unsigned int id)
+{
+	register_periph(id, SHRES_SHARED);
+}
+
+void stm32mp_register_non_secure_periph(unsigned int id)
+{
+	register_periph(id, SHRES_NON_SECURE);
+}
+
+/* Register resource by IO memory base address */
+static void register_periph_iomem(uintptr_t base, unsigned int state)
+{
+	unsigned int id;
+
+	switch (base) {
+	case IWDG1_BASE:
+		id = STM32MP1_SHRES_IWDG1;
+		break;
+	case USART1_BASE:
+		id = STM32MP1_SHRES_USART1;
+		break;
+	case SPI6_BASE:
+		id = STM32MP1_SHRES_SPI6;
+		break;
+	case I2C4_BASE:
+		id = STM32MP1_SHRES_I2C4;
+		break;
+	case I2C6_BASE:
+		id = STM32MP1_SHRES_I2C6;
+		break;
+	case RTC_BASE:
+		id = STM32MP1_SHRES_RTC;
+		break;
+	case RNG1_BASE:
+		id = STM32MP1_SHRES_RNG1;
+		break;
+	case CRYP1_BASE:
+		id = STM32MP1_SHRES_CRYP1;
+		break;
+	case HASH1_BASE:
+		id = STM32MP1_SHRES_HASH1;
+		break;
+
+#ifdef CFG_WITH_NSEC_GPIOS
+	case GPIOA_BASE:
+	case GPIOB_BASE:
+	case GPIOC_BASE:
+	case GPIOD_BASE:
+	case GPIOE_BASE:
+	case GPIOF_BASE:
+	case GPIOG_BASE:
+	case GPIOH_BASE:
+	case GPIOI_BASE:
+	case GPIOJ_BASE:
+	case GPIOK_BASE:
+#endif
+#ifdef CFG_WITH_NSEC_UARTS
+	case USART2_BASE:
+	case USART3_BASE:
+	case UART4_BASE:
+	case UART5_BASE:
+	case USART6_BASE:
+	case UART7_BASE:
+	case UART8_BASE:
+#endif
+	case IWDG2_BASE:
+		/* Allow drivers to register some non secure resources */
+		DMSG("IO for non secure resource 0x%lx", base);
+		if (state != SHRES_NON_SECURE)
+			panic();
+
+		return;
+
+	default:
+		panic();
+		break;
+	}
+
+	register_periph(id, state);
+}
+
+void stm32mp_register_secure_periph_iomem(uintptr_t base)
+{
+	register_periph_iomem(base, SHRES_SECURE);
+}
+
+void stm32mp_register_non_secure_periph_iomem(uintptr_t base)
+{
+	register_periph_iomem(base, SHRES_NON_SECURE);
+}
+
+/* Register GPIO resource */
+void stm32mp_register_secure_gpio(unsigned int bank, unsigned int pin)
+{
+	switch (bank) {
+	case GPIO_BANK_Z:
+		register_periph(STM32MP1_SHRES_GPIOZ(pin), SHRES_SECURE);
+		break;
+	default:
+		EMSG("GPIO bank %u cannot be secured", bank);
+		panic();
+	}
+}
+
+void stm32mp_register_non_secure_gpio(unsigned int bank, unsigned int pin)
+{
+	switch (bank) {
+	case GPIO_BANK_Z:
+		register_periph(STM32MP1_SHRES_GPIOZ(pin), SHRES_NON_SECURE);
+		break;
+	default:
+		break;
+	}
+}
+
+void stm32mp_register_etzpc_decprot(unsigned int id,
+				    enum etzpc_decprot_attributes attr)
+{
+	unsigned int state = SHRES_SECURE;
+	unsigned int id_shres;
+
+	switch (attr) {
+	case TZPC_DECPROT_S_RW:
+		break;
+	case TZPC_DECPROT_NS_R_S_W:
+	case TZPC_DECPROT_MCU_ISOLATION:
+	case TZPC_DECPROT_NS_RW:
+		state = SHRES_NON_SECURE;
+		break;
+	default:
+		panic();
+	}
+
+	switch (id) {
+	case STM32MP1_ETZPC_GPIOZ_ID:
+		if (state == SHRES_SECURE) {
+			/* GPIOZ cannot be hardened from the ETZPC */
+			panic();
+		}
+		break;
+	default:
+		id_shres = decprot2shres(id);
+		if (id_shres == SHRES_INVALID) {
+			if (state != SHRES_NON_SECURE)
+				panic();
+		} else {
+			register_periph(id_shres, state);
+		}
+		break;
+	}
+}
+
+/* Get resource state: these accesses lock the registering support */
+static void lock_registering(void)
+{
+	registering_locked = true;
+}
+
+bool stm32mp_periph_is_shared(unsigned long id)
+{
+	lock_registering();
+
+	return shres_state[id] == SHRES_SHARED;
+}
+
+bool stm32mp_periph_is_non_secure(unsigned long id)
+{
+	lock_registering();
+
+	return shres_state[id] == SHRES_NON_SECURE;
+}
+
+bool stm32mp_periph_is_secure(unsigned long id)
+{
+	lock_registering();
+
+	return shres_state[id] == SHRES_SECURE;
+}
+
+bool stm32mp_periph_is_unregistered(unsigned long id)
+{
+	lock_registering();
+
+	return shres_state[id] == SHRES_UNREGISTERED;
+}
+
+bool stm32mp_gpio_bank_is_shared(unsigned int bank)
+{
+	unsigned int non_secure = 0;
+	unsigned int i;
+
+	lock_registering();
+
+	if (bank != GPIO_BANK_Z) {
+		return false;
+	}
+
+	for (i = 0; i < get_gpioz_nbpin_unpg(); i++) {
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_GPIOZ(i)) ||
+		    stm32mp_periph_is_unregistered(STM32MP1_SHRES_GPIOZ(i))) {
+			non_secure++;
+		}
+	}
+
+	return (non_secure != 0) && (non_secure < get_gpioz_nbpin_unpg());
+}
+
+bool stm32mp_gpio_bank_is_non_secure(unsigned int bank)
+{
+	unsigned int non_secure = 0;
+	unsigned int i;
+
+	lock_registering();
+
+	if (bank != GPIO_BANK_Z) {
+		return true;
+	}
+
+	for (i = 0; i < get_gpioz_nbpin_unpg(); i++) {
+		if (stm32mp_periph_is_non_secure(STM32MP1_SHRES_GPIOZ(i)) ||
+		    stm32mp_periph_is_unregistered(STM32MP1_SHRES_GPIOZ(i))) {
+			non_secure++;
+		}
+	}
+
+	return non_secure == get_gpioz_nbpin_unpg();
+}
+
+bool stm32mp_gpio_bank_is_secure(unsigned int bank)
+{
+	unsigned int secure = 0;
+	unsigned int i;
+
+	lock_registering();
+
+	if (bank != GPIO_BANK_Z) {
+		return false;
+	}
+
+	for (i = 0; i < get_gpioz_nbpin_unpg(); i++) {
+		if (stm32mp_periph_is_secure(STM32MP1_SHRES_GPIOZ(i))) {
+			secure++;
+		}
+	}
+
+	return secure == get_gpioz_nbpin_unpg();
+}
+
+bool stm32mp_clock_is_shareable(unsigned long clock_id)
+{
+	switch (clock_id) {
+	case GPIOZ:
+		return get_gpioz_nbpin() > 0;
+	case RTCAPB:
+		return true;
+	default:
+		return false;
+	}
+}
+
+bool stm32mp_clock_is_shared(unsigned long clock_id)
+{
+	lock_registering();
+
+	switch (clock_id) {
+	case GPIOZ:
+		if (get_gpioz_nbpin_unpg() > 0) {
+			return stm32mp_gpio_bank_is_shared(GPIO_BANK_Z);
+		} else {
+			return false;
+		}
+	case RTCAPB:
+		/* RTCAPB is shared for non secure backup registers */
+		return true;
+	default:
+		return false;
+	}
+}
+
+bool stm32mp_clock_is_non_secure(unsigned long clock_id)
+{
+	unsigned int shres_id;
+
+	lock_registering();
+
+	if (stm32mp_clock_is_shared(clock_id)) {
+		return false;
+	}
+
+	switch (clock_id) {
+	case BSEC:
+	case BKPSRAM:
+	case TZPC:
+	case TZC1:
+	case TZC2:
+	case STGEN_K:
+	case DDRC1:
+	case DDRC1LP:
+	case DDRC2:
+	case DDRC2LP:
+	case DDRPHYC:
+	case DDRPHYCLP:
+	case DDRCAPB:
+	case DDRCAPBLP:
+	case AXIDCG:
+	case DDRPHYCAPB:
+	case DDRPHYCAPBLP:
+		return false;
+	case IWDG1:
+		shres_id = STM32MP1_SHRES_IWDG1;
+		break;
+	case USART1_K:
+		shres_id = STM32MP1_SHRES_USART1;
+		break;
+	case SPI6_K:
+		shres_id = STM32MP1_SHRES_SPI6;
+		break;
+	case I2C4_K:
+		shres_id = STM32MP1_SHRES_I2C4;
+		break;
+	case RNG1_K:
+		shres_id = STM32MP1_SHRES_RNG1;
+		break;
+	case HASH1:
+		shres_id = STM32MP1_SHRES_HASH1;
+		break;
+	case CRYP1:
+		shres_id = STM32MP1_SHRES_CRYP1;
+		break;
+	case I2C6_K:
+		shres_id = STM32MP1_SHRES_I2C6;
+		break;
+	case RTC:
+		shres_id = STM32MP1_SHRES_RTC;
+		break;
+	default:
+		return true;
+	}
+
+	return stm32mp_periph_is_non_secure(shres_id);
+}
+
+/* ETZPC configuration at drivers initialization completion */
+static enum etzpc_decprot_attributes decprot_periph_attr(unsigned int id)
+{
+	switch (id) {
+	case STM32MP1_SHRES_GPIOZ(0):
+	case STM32MP1_SHRES_GPIOZ(1):
+	case STM32MP1_SHRES_GPIOZ(2):
+	case STM32MP1_SHRES_GPIOZ(3):
+	case STM32MP1_SHRES_GPIOZ(4):
+	case STM32MP1_SHRES_GPIOZ(5):
+	case STM32MP1_SHRES_GPIOZ(6):
+	case STM32MP1_SHRES_GPIOZ(7):
+		assert((id - STM32MP1_SHRES_GPIOZ(0)) < get_gpioz_nbpin_unpg());
+		return TZPC_DECPROT_NS_RW;
+	default:
+		if (stm32mp_periph_is_non_secure(id)) {
+			return TZPC_DECPROT_NS_RW;
+		}
+
+		return TZPC_DECPROT_S_RW;
+	}
+}
+
+static bool check_decprot(unsigned int id, enum etzpc_decprot_attributes exp)
+{
+	enum etzpc_decprot_attributes cur = etzpc_get_decprot(id);
+
+	if (cur == exp) {
+		return true;
+	}
+
+	switch (exp) {
+	case TZPC_DECPROT_NS_RW:
+		if (cur == TZPC_DECPROT_S_RW) {
+#ifdef CFG_DT
+			IMSG("Warning ETZPC: %s could be non secure",
+			     decprot2str(id));
+#else
+			etzpc_configure_decprot(id, TZPC_DECPROT_NS_RW);
+			IMSG("Warning ETZPC: %s forced non secure",
+			     decprot2str(id));
+#endif
+		}
+		return true;
+
+	case TZPC_DECPROT_S_RW:
+		EMSG("ETZPC DECPROT: %s (%u) expected secure but DECPROT=%d",
+		     decprot2str(id), id, cur);
+		break;
+
+	case TZPC_DECPROT_NS_R_S_W:
+	case TZPC_DECPROT_MCU_ISOLATION:
+	default:
+		panic();
+	}
+
+	return false;
+}
+
+static void check_etzpc_secure_configuration(void)
+{
+	bool error = false;
+
+	assert(registering_locked);
+
+	error |= !check_decprot(STM32MP1_ETZPC_STGENC_ID,
+				TZPC_DECPROT_S_RW);
+
+	error |= !check_decprot(STM32MP1_ETZPC_BKPSRAM_ID,
+				TZPC_DECPROT_S_RW);
+
+	error |= !check_decprot(STM32MP1_ETZPC_IWDG1_ID,
+				decprot_periph_attr(STM32MP1_SHRES_IWDG1));
+
+	error |= !check_decprot(STM32MP1_ETZPC_USART1_ID,
+				decprot_periph_attr(STM32MP1_SHRES_USART1));
+
+	error |= !check_decprot(STM32MP1_ETZPC_SPI6_ID,
+				decprot_periph_attr(STM32MP1_SHRES_SPI6));
+
+	error |= !check_decprot(STM32MP1_ETZPC_I2C4_ID,
+				decprot_periph_attr(STM32MP1_SHRES_I2C4));
+
+	error |= !check_decprot(STM32MP1_ETZPC_RNG1_ID,
+				decprot_periph_attr(STM32MP1_SHRES_RNG1));
+
+	error |= !check_decprot(STM32MP1_ETZPC_HASH1_ID,
+				decprot_periph_attr(STM32MP1_SHRES_HASH1));
+
+	error |= !check_decprot(STM32MP1_ETZPC_CRYP1_ID,
+				decprot_periph_attr(STM32MP1_SHRES_CRYP1));
+
+	error |= !check_decprot(STM32MP1_ETZPC_DDRCTRL_ID,
+				TZPC_DECPROT_S_RW);
+
+	error |= !check_decprot(STM32MP1_ETZPC_DDRPHYC_ID,
+				TZPC_DECPROT_S_RW);
+
+	error |= !check_decprot(STM32MP1_ETZPC_I2C6_ID,
+				decprot_periph_attr(STM32MP1_SHRES_I2C6));
+
+	if (!error) {
+		return;
+	}
+
+	panic();
+}
+
+static void check_rcc_secure_configuration(void)
+{
+	bool secure = stm32_rcc_is_secure();
+	bool mckprot = stm32_rcc_is_mckprot();
+	unsigned int n;
+	unsigned int error = 0;
+
+	for (n = 0; n < ARRAY_SIZE(shres_state); n++) {
+		if  ((shres_state[n] == SHRES_SECURE) ||
+		     (shres_state[n] == SHRES_SHARED)) {
+			if ((mckprot_resource(n) && (!mckprot)) || !secure) {
+				EMSG("RCC %s MCKPROT %s and %s (%u) secure",
+				      secure ? "secure" : "non secure",
+				      mckprot ? "set" : "not set",
+				      shres2str_id(n), n);
+				error++;
+			}
+		}
+	}
+
+	if (error != 0U) {
+		panic();
+	}
+}
+
+static void gpio_secure_suspend_resume(enum pm_op op, void __unused *handle)
+{
+	unsigned int pin;
+
+	switch (op) {
+	case PM_OP_SUSPEND:
+		return;
+	case PM_OP_RESUME:
+		break;
+	default:
+		panic();
+	}
+
+	/* Release secure hardening of non secure pins at resume */
+	for (pin = 0; pin < get_gpioz_nbpin_unpg(); pin++) {
+		unsigned int id = STM32MP1_SHRES_GPIOZ(pin);
+
+		if (stm32mp_periph_is_non_secure(id) ||
+		    stm32mp_periph_is_unregistered(id)) {
+			stm32_gpio_set_secure_cfg(GPIO_BANK_Z, pin, false);
+		}
+	}
+}
+KEEP_PAGER(gpio_secure_suspend_resume);
+
+static void check_gpio_secure_configuration(void)
+{
+	unsigned int pin;
+	bool secure;
+
+	for (pin = 0; pin < get_gpioz_nbpin(); pin++) {
+		secure = stm32mp_periph_is_secure(STM32MP1_SHRES_GPIOZ(pin));
+		stm32_gpio_set_secure_cfg(GPIO_BANK_Z, pin, secure);
+	}
+
+	stm32mp_register_pm_cb(gpio_secure_suspend_resume, NULL);
+}
+
+static TEE_Result stm32mp1_init_drivers(void)
+{
+	size_t id;
+
+	registering_locked = true;
+
+	for (id = 0; id < STM32MP1_SHRES_COUNT; id++) {
+		uint8_t *state = &shres_state[id];
+
+#if TRACE_LEVEL == TRACE_INFO
+		/* Display only the secure and shared resources */
+		if ((*state == SHRES_NON_SECURE) ||
+		    ((*state == SHRES_UNREGISTERED))) {
+			continue;
+		}
+#endif
+
+		IMSG("stm32mp %-8s (%2u): %-14s",
+		     shres2str_id(id), id, shres2str_state(*state));
+	}
+
+	stm32mp_update_earlyboot_clocks_state();
+
+	check_rcc_secure_configuration();
+	check_etzpc_secure_configuration();
+	check_gpio_secure_configuration();
+
+	return TEE_SUCCESS;
+}
+driver_init_late(stm32mp1_init_drivers);
diff --git a/core/arch/arm/plat-stm32mp1/stm32_util.h b/core/arch/arm/plat-stm32mp1/stm32_util.h
new file mode 100644
index 0000000..0164855
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/stm32_util.h
@@ -0,0 +1,287 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics
+ */
+
+#ifndef __STM32_UTIL_H__
+#define __STM32_UTIL_H__
+
+#include <io.h>
+#include <drivers/stm32_etzpc.h>
+#include <drivers/stm32mp1_clk.h>
+#include <kernel/panic.h>
+#include <mm/core_mmu.h>
+#include <sm/sm.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+/* SoC versioning */
+uint32_t stm32mp1_dbgmcu_get_chip_version(void);
+
+/* SiP & OEM platform services */
+bool stm32_sip_service(struct sm_ctx *ctx,
+		       uint32_t *a0, uint32_t *a1, uint32_t *a2, uint32_t *a3);
+bool stm32_oem_service(struct sm_ctx *ctx,
+		       uint32_t *a0, uint32_t *a1, uint32_t *a2, uint32_t *a3);
+
+
+/* Platform util for the STGEN driver */
+uintptr_t stm32_get_stgen_base(void);
+
+/* Platform util for the GIC */
+uintptr_t get_gicc_base(void);
+uintptr_t get_gicd_base(void);
+
+/* Platform util for clock gating. ID refers to clock DT bindings ID. */
+void stm32_clock_enable(unsigned long id);
+void stm32_clock_disable(unsigned long id);
+
+static inline unsigned long stm32_clock_get_rate(unsigned long id)
+{
+	return stm32mp1_clk_get_rate(id);
+}
+
+static inline unsigned long stm32_clock_is_enabled(unsigned long id)
+{
+	return stm32mp1_clk_is_enabled(id);
+}
+
+/* Platform util for the GPIO driver */
+uintptr_t stm32_get_gpio_bank_base(unsigned int bank);
+uint32_t stm32_get_gpio_bank_offset(unsigned int bank);
+int stm32_get_gpio_bank_clock(unsigned int bank);
+
+/* Platform util for the IWDG driver */
+unsigned long stm32_get_iwdg_otp_config(uintptr_t pbase);
+int stm32mp_iwdg_irq2instance(size_t irq);
+size_t stm32mp_iwdg_instance2irq(int instance);
+unsigned int stm32mp_iwdg_iomem2instance(uintptr_t pbase);
+
+/* Platform util for the BSEC driver */
+unsigned int stm32mp_get_otp_max(void);
+unsigned int stm32mp_get_otp_upper_start(void);
+
+/* Platform util for the ETZPC driver */
+uintptr_t stm32mp_get_etzpc_base(void);
+enum etzpc_decprot_attributes stm32mp_etzpc_binding2decprot(uint32_t mode);
+
+/* Platform util for the RTC driver */
+bool stm32_rtc_get_read_twice(void);
+
+/* Backup registers and RAM utils */
+uintptr_t stm32mp_bkpreg(unsigned int idx);
+
+uintptr_t stm32mp1_bkpsram_base(void);
+
+/* Platform util for PMIC support */
+bool stm32mp_with_pmic(void);
+
+/* Power management service */
+void stm32mp_register_online_cpu(void);
+
+/*
+ * Lock/unlock access to shared registers
+ *
+ * @lock - NULL or pointer to spin lock
+ */
+uint32_t lock_stm32shregs(void);
+void unlock_stm32shregs(uint32_t exceptions);
+
+void io_mask32_stm32shregs(uintptr_t va, uint32_t value, uint32_t mask);
+
+static inline void stm32shregs_setbits(uintptr_t va, uint32_t value)
+{
+	io_mask32_stm32shregs(va, value, value);
+}
+
+static inline void stm32shregs_clrbits(uintptr_t va, uint32_t value)
+{
+	io_mask32_stm32shregs(va, 0, value);
+}
+
+static inline void stm32shregs_clrsetbits(uintptr_t va, uint32_t mask,
+					  uint32_t value)
+{
+	io_mask32_stm32shregs(va, value, mask);
+}
+
+/*
+ * Generic spinlock function that bypass spinlock if MMU is disabled or
+ * lock is NULL.
+ */
+uint32_t may_spin_lock(unsigned int *lock);
+void may_spin_unlock(unsigned int *lock, uint32_t exceptions);
+
+/* Reset function for early watchdog management */
+void stm32mp_platform_reset(int cpu);
+
+/* Clock calibration */
+int stm32mp_start_clock_calib(unsigned int clock_id);
+
+/*
+ * Shared reference counter: increments by 2 on secure increment
+ * request, decrements by 2 on secure decrement request. Bit #0
+ * is set to 1 on non-secure increment request and reset to 0 on
+ * non-secure decrement request. These counter initializes to
+ * either 0, 1 or 2 upon their expect default state.
+ * Increment refcount and return if incremented from 0.
+ * Counters saturates once above UINT_MAX / 2.
+ */
+#define SHREFCNT_NONSECURE_FLAG		0x1ul
+#define SHREFCNT_SECURE_STEP		0x2ul
+#define SHREFCNT_MAX			(UINT_MAX / 2)
+
+/* Return 1 if refcnt decrements to 0, else return 0 */
+static inline int incr_shrefcnt(unsigned int *refcnt, bool secure)
+{
+	int rc = !*refcnt;
+
+	if (secure) {
+		if (*refcnt < SHREFCNT_MAX) {
+			*refcnt += SHREFCNT_SECURE_STEP;
+			assert(*refcnt < SHREFCNT_MAX);
+		}
+	} else {
+		*refcnt |= SHREFCNT_NONSECURE_FLAG;
+	}
+
+	return rc;
+}
+
+/* Return 1 if refcnt decrements to 0, else return 0 */
+static inline int decr_shrefcnt(unsigned int *refcnt, bool secure)
+{
+	int  rc = 0;
+
+	if (secure) {
+		if (*refcnt < SHREFCNT_MAX) {
+			if (*refcnt < SHREFCNT_SECURE_STEP) {
+				panic();
+			}
+
+			*refcnt -= SHREFCNT_SECURE_STEP;
+			rc = !*refcnt;
+		}
+	} else {
+		rc = (*refcnt == SHREFCNT_NONSECURE_FLAG);
+		*refcnt &= ~SHREFCNT_NONSECURE_FLAG;
+	}
+
+	return rc;
+}
+
+static inline int incr_refcnt(unsigned int *refcnt)
+{
+	return incr_shrefcnt(refcnt, true);
+}
+
+static inline int decr_refcnt(unsigned int *refcnt)
+{
+	return decr_shrefcnt(refcnt, true);
+}
+
+#define STM32MP1_SHRES_GPIOZ(i)		(STM32MP1_SHRES_GPIOZ_0 + i)
+
+enum stm32mp_shres {
+	STM32MP1_SHRES_GPIOZ_0 = 0,
+	STM32MP1_SHRES_GPIOZ_1,
+	STM32MP1_SHRES_GPIOZ_2,
+	STM32MP1_SHRES_GPIOZ_3,
+	STM32MP1_SHRES_GPIOZ_4,
+	STM32MP1_SHRES_GPIOZ_5,
+	STM32MP1_SHRES_GPIOZ_6,
+	STM32MP1_SHRES_GPIOZ_7,
+	STM32MP1_SHRES_IWDG1,
+	STM32MP1_SHRES_USART1,
+	STM32MP1_SHRES_SPI6,
+	STM32MP1_SHRES_I2C4,
+	STM32MP1_SHRES_RNG1,
+	STM32MP1_SHRES_HASH1,
+	STM32MP1_SHRES_CRYP1,
+	STM32MP1_SHRES_I2C6,
+	STM32MP1_SHRES_RTC,
+	STM32MP1_SHRES_MCU,
+	STM32MP1_SHRES_HSI,
+	STM32MP1_SHRES_LSI,
+	STM32MP1_SHRES_HSE,
+	STM32MP1_SHRES_LSE,
+	STM32MP1_SHRES_CSI,
+	STM32MP1_SHRES_PLL1,
+	STM32MP1_SHRES_PLL1_P,
+	STM32MP1_SHRES_PLL1_Q,
+	STM32MP1_SHRES_PLL1_R,
+	STM32MP1_SHRES_PLL2,
+	STM32MP1_SHRES_PLL2_P,
+	STM32MP1_SHRES_PLL2_Q,
+	STM32MP1_SHRES_PLL2_R,
+	STM32MP1_SHRES_PLL3,
+	STM32MP1_SHRES_PLL3_P,
+	STM32MP1_SHRES_PLL3_Q,
+	STM32MP1_SHRES_PLL3_R,
+
+	STM32MP1_SHRES_COUNT
+};
+
+void stm32mp_register_secure_periph(unsigned int id);
+void stm32mp_register_shared_periph(unsigned int id);
+void stm32mp_register_non_secure_periph(unsigned int id);
+void stm32mp_register_secure_periph_iomem(uintptr_t base);
+void stm32mp_register_non_secure_periph_iomem(uintptr_t base);
+void stm32mp_register_secure_gpio(unsigned int bank, unsigned int pin);
+void stm32mp_register_non_secure_gpio(unsigned int bank, unsigned int pin);
+void stm32mp_register_etzpc_decprot(unsigned int id,
+				    enum etzpc_decprot_attributes attr);
+
+bool stm32mp_periph_is_shared(unsigned long id);
+bool stm32mp_periph_is_non_secure(unsigned long id);
+bool stm32mp_periph_is_secure(unsigned long id);
+bool stm32mp_periph_is_unregistered(unsigned long id);
+
+bool stm32mp_gpio_bank_is_shared(unsigned int bank);
+bool stm32mp_gpio_bank_is_non_secure(unsigned int bank);
+bool stm32mp_gpio_bank_is_secure(unsigned int bank);
+
+bool stm32mp_clock_is_shareable(unsigned long clock_id);
+bool stm32mp_clock_is_shared(unsigned long clock_id);
+bool stm32mp_clock_is_non_secure(unsigned long clock_id);
+
+/*
+ * Set bit fields, clear bit flieds, clear bits and set bits utils
+ */
+static inline void mmio_write_8(uintptr_t addr, uint8_t value)
+{
+	write8(value, addr);
+}
+
+static inline uint8_t mmio_read_8(uintptr_t addr)
+{
+	return read8(addr);
+}
+
+static inline void mmio_write_32(uintptr_t addr, uint32_t value)
+{
+	write32(value, addr);
+}
+
+static inline uint32_t mmio_read_32(uintptr_t addr)
+{
+	return read32(addr);
+}
+
+static inline void mmio_setbits_32(uintptr_t addr, uint32_t mask)
+{
+	write32(read32(addr) | mask, addr);
+}
+
+static inline void mmio_clrbits_32(uintptr_t addr, uint32_t mask)
+{
+	write32(read32(addr) & ~mask, addr);
+}
+
+static inline void mmio_clrsetbits_32(uintptr_t addr, uint32_t clear_mask,
+				      uint32_t set_mask)
+{
+	write32((read32(addr) & ~clear_mask) | set_mask, addr);
+}
+
+#endif /*__STM32_UTIL_H__*/
diff --git a/core/arch/arm/plat-stm32mp1/stm32mp1_dt.c b/core/arch/arm/plat-stm32mp1/stm32mp1_dt.c
new file mode 100644
index 0000000..9ce6ba8
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/stm32mp1_dt.c
@@ -0,0 +1,338 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2018, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ */
+
+#include <assert.h>
+#include <drivers/stm32mp1_clk.h>
+#include <drivers/stm32mp1_clkfunc.h>
+#include <drivers/stm32_gpio.h>
+#include <kernel/dt.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <libfdt.h>
+#include <platform_config.h>
+#include <stm32_util.h>
+#include <stm32mp_dt.h>
+#include <trace.h>
+
+/*******************************************************************************
+ * This function check the presence of a node (generic use of fdt library).
+ * Returns true if present else returns false.
+ ******************************************************************************/
+bool fdt_check_node(void *fdt, int node)
+{
+	int len;
+	const char *cchar;
+
+	cchar = fdt_get_name(fdt, node, &len);
+
+	return (cchar != NULL) && (len >= 0);
+}
+
+/*******************************************************************************
+ * This function reads a value of a node property (generic use of fdt
+ * library).
+ * Returns value if success, and a default value if property not found.
+ * Default value is passed as parameter.
+ ******************************************************************************/
+uint32_t fdt_read_uint32_default(void *fdt, int node, const char *prop_name,
+				 uint32_t dflt_value)
+{
+	const fdt32_t *cuint;
+	int lenp;
+
+	cuint = fdt_getprop(fdt, node, prop_name, &lenp);
+	if (cuint == NULL) {
+		return dflt_value;
+	}
+
+	return fdt32_to_cpu(*cuint);
+}
+
+/*******************************************************************************
+ * This function reads a series of parameters in a node property
+ * (generic use of fdt library).
+ * It reads the values inside the device tree, from property name and node.
+ * The number of parameters is also indicated as entry parameter.
+ * Returns 0 if success, and a negative value else.
+ * If success, values are stored at the third parameter address.
+ ******************************************************************************/
+int fdt_read_uint32_array(void *fdt, int node, const char *prop_name,
+			  uint32_t *array, uint32_t count)
+{
+	const fdt32_t *cuint;
+	int len;
+	uint32_t i;
+
+	cuint = fdt_getprop(fdt, node, prop_name, &len);
+	if (cuint == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	if ((uint32_t)len != (count * sizeof(uint32_t))) {
+		return -FDT_ERR_BADLAYOUT;
+	}
+
+	for (i = 0; i < ((uint32_t)len / sizeof(uint32_t)); i++) {
+		*array = fdt32_to_cpu(*cuint);
+		array++;
+		cuint++;
+	}
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function gets the clock ID of the given node.
+ * It reads the value indicated inside the device tree.
+ * Returns ID on success and a negative FDT error code on failure.
+ ******************************************************************************/
+int fdt_get_clock_id(void *fdt, int node)
+{
+	const fdt32_t *cuint;
+
+	cuint = fdt_getprop(fdt, node, "clocks", NULL);
+	if (cuint == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	cuint++;
+	return (int)fdt32_to_cpu(*cuint);
+}
+
+/*******************************************************************************
+ * This function gets the clock ID of the given node using clock-names.
+ * It reads the value indicated inside the device tree.
+ * Returns ID on success and a negative FDT error code on failure.
+ ******************************************************************************/
+int fdt_get_clock_id_by_name(void *fdt, int node, const char *name)
+{
+	const fdt32_t *cuint;
+	int index;
+	int len;
+
+	index = fdt_stringlist_search(fdt, node, "clock-names", name);
+	if (index < 0) {
+		return index;
+	}
+
+	cuint = fdt_getprop(fdt, node, "clocks", &len);
+	if (cuint == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	if ((index * (int)sizeof(uint32_t)) > len) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	cuint += (index << 1) + 1;
+	return (int)fdt32_to_cpu(*cuint);
+}
+
+/*******************************************************************************
+ * This function fills the generic information from a given node.
+ ******************************************************************************/
+void fdt_fill_device_info(void *fdt, struct dt_node_info *info, int node)
+{
+	const fdt32_t *cuint;
+
+	cuint = fdt_getprop(fdt, node, "reg", NULL);
+	if (cuint != NULL) {
+		info->base = fdt32_to_cpu(*cuint);
+	} else {
+		info->base = 0;
+	}
+
+	cuint = fdt_getprop(fdt, node, "clocks", NULL);
+	if (cuint != NULL) {
+		cuint++;
+		info->clock = (int)fdt32_to_cpu(*cuint);
+	} else {
+		info->clock = -1;
+	}
+
+	cuint = fdt_getprop(fdt, node, "resets", NULL);
+	if (cuint != NULL) {
+		cuint++;
+		info->reset = (int)fdt32_to_cpu(*cuint);
+	} else {
+		info->reset = -1;
+	}
+
+	info->status = _fdt_get_status(fdt, node);
+}
+
+/*******************************************************************************
+ * This function retrieve the generic information from DT.
+ * Returns node if success, and a negative value else.
+ ******************************************************************************/
+int fdt_get_node(void *fdt, struct dt_node_info *info, int offset,
+		const char *compat)
+{
+	int node;
+
+	node = fdt_node_offset_by_compatible(fdt, offset, compat);
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	fdt_fill_device_info(fdt, info, node);
+
+	return node;
+}
+
+/*******************************************************************************
+ * This function gets the stdout path node.
+ * It reads the value indicated inside the device tree.
+ * Returns node on success and a negative FDT error code on failure.
+ ******************************************************************************/
+int fdt_get_stdout_node_offset(void *fdt)
+{
+	int node;
+	const char *cchar;
+
+	node = fdt_path_offset(fdt, "/secure-chosen");
+	if (node < 0) {
+		node = fdt_path_offset(fdt, "/chosen");
+		if (node < 0) {
+			return -FDT_ERR_NOTFOUND;
+		}
+	}
+
+	cchar = fdt_getprop(fdt, node, "stdout-path", NULL);
+	if (cchar == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	node = -FDT_ERR_NOTFOUND;
+	if (strchr(cchar, (int)':') != NULL) {
+		const char *name;
+		char *str = (char *)cchar;
+		int len = 0;
+
+		while (strncmp(":", str, 1)) {
+			len++;
+			str++;
+		}
+
+		name = fdt_get_alias_namelen(fdt, cchar, len);
+
+		if (name != NULL) {
+			node = fdt_path_offset(fdt, name);
+		}
+	} else {
+		node = fdt_path_offset(fdt, cchar);
+	}
+
+	return node;
+}
+
+/*******************************************************************************
+ * This function gets DDR size information from the DT.
+ * Returns value in bytes if success, and STM32MP1_DDR_SIZE_DFLT else.
+ ******************************************************************************/
+uint32_t fdt_get_ddr_size(void *fdt)
+{
+	int node;
+
+	node = fdt_node_offset_by_compatible(fdt, -1, DT_DDR_COMPAT);
+	if (node < 0) {
+		IMSG("%s: Cannot read DDR node in DT\n", __func__);
+		return STM32MP1_DDR_SIZE_DFLT;
+	}
+
+	return fdt_read_uint32_default(fdt, node, "st,mem-size",
+				       STM32MP1_DDR_SIZE_DFLT);
+}
+
+/*******************************************************************************
+ * This function retrieves board model from DT.
+ * Returns string taken from model node, NULL otherwise
+ ******************************************************************************/
+const char *fdt_get_board_model(void *fdt)
+{
+	int node = fdt_path_offset(fdt, "/");
+
+	if (node < 0) {
+		return NULL;
+	}
+
+	return (const char *)fdt_getprop(fdt, node, "model", NULL);
+}
+
+/*******************************************************************************
+ * This function gets GPIO bank PINCTRL node information from the DT.
+ * Returns node value.
+ ******************************************************************************/
+int fdt_get_gpio_bank_pinctrl_node(void *fdt, unsigned int bank)
+{
+	switch (bank) {
+	case GPIO_BANK_A:
+	case GPIO_BANK_B:
+	case GPIO_BANK_C:
+	case GPIO_BANK_D:
+	case GPIO_BANK_E:
+	case GPIO_BANK_F:
+	case GPIO_BANK_G:
+	case GPIO_BANK_H:
+	case GPIO_BANK_I:
+	case GPIO_BANK_J:
+	case GPIO_BANK_K:
+		return fdt_path_offset(fdt, "/soc/pin-controller");
+	case GPIO_BANK_Z:
+		return fdt_path_offset(fdt, "/soc/pin-controller-z");
+	default:
+		panic();
+	}
+}
+
+/*******************************************************************************
+ * This function gets GPIOZ pin number information from the DT.
+ * It also checks node consistency.
+ ******************************************************************************/
+int fdt_get_gpioz_nbpins_from_dt(void *fdt)
+{
+	int pinctrl_node;
+	int pinctrl_subnode;
+
+	pinctrl_node = fdt_get_gpio_bank_pinctrl_node(fdt, GPIO_BANK_Z);
+	if (pinctrl_node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	fdt_for_each_subnode(pinctrl_subnode, fdt, pinctrl_node) {
+		uint32_t bank_offset;
+		const fdt32_t *cuint;
+
+		if (fdt_getprop(fdt, pinctrl_subnode,
+				"gpio-controller", NULL) == NULL) {
+			continue;
+		}
+
+		cuint = fdt_getprop(fdt, pinctrl_subnode, "reg", NULL);
+		if (cuint == NULL) {
+			continue;
+		}
+
+		bank_offset = stm32_get_gpio_bank_offset(GPIO_BANK_Z);
+		if (fdt32_to_cpu(*cuint) != bank_offset) {
+			continue;
+		}
+
+		if (_fdt_get_status(fdt, pinctrl_subnode) ==
+		     DT_STATUS_DISABLED) {
+			return 0;
+		}
+
+		cuint = fdt_getprop(fdt, pinctrl_subnode, "ngpios", NULL);
+		if (cuint == NULL) {
+			return -FDT_ERR_NOTFOUND;
+		}
+
+		return (int)fdt32_to_cpu(*cuint);
+	}
+
+	return 0;
+}
diff --git a/core/arch/arm/plat-stm32mp1/stm32mp_dt.h b/core/arch/arm/plat-stm32mp1/stm32mp_dt.h
new file mode 100644
index 0000000..ec8aa4d
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/stm32mp_dt.h
@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2018, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ */
+
+#ifndef __STM32MP1_DT_H__
+#define __STM32MP1_DT_H__
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define DT_DDR_COMPAT		"st,stm32mp1-ddr"
+
+struct dt_node_info {
+	uint32_t base;
+	int32_t clock;
+	int32_t reset;
+	unsigned int status;
+};
+
+bool fdt_check_node(void *fdt, int node);
+uint32_t fdt_read_uint32_default(void *fdt, int node, const char *prop_name,
+				 uint32_t dflt_value);
+int fdt_read_uint32_array(void *fdt, int node, const char *prop_name,
+			  uint32_t *array, uint32_t count);
+void fdt_fill_device_info(void *fdt, struct dt_node_info *info, int node);
+int fdt_get_node(void *fdt, struct dt_node_info *info, int offset,
+		 const char *compat);
+int fdt_get_stdout_node_offset(void *fdt);
+uint32_t fdt_get_ddr_size(void *fdt);
+const char *fdt_get_board_model(void *fdt);
+
+int fdt_get_clock_id(void *fdt, int node);
+int fdt_get_clock_id_by_name(void *fdt, int node, const char *name);
+int fdt_get_gpio_bank_pinctrl_node(void *fdt, unsigned int bank);
+int fdt_get_gpioz_nbpins_from_dt(void *fdt);
+
+#endif /* __STM32MP1_DT_H__ */
diff --git a/core/arch/arm/plat-stm32mp1/stm32mp_pm.h b/core/arch/arm/plat-stm32mp1/stm32mp_pm.h
new file mode 100644
index 0000000..8395ccc
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/stm32mp_pm.h
@@ -0,0 +1,49 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics
+ */
+#ifndef __STM32MP_PM_H__
+#define __STM32MP_PM_H__
+
+#ifndef ASM
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+#define PSCI_MODE_SYSTEM_SUSPEND	0
+#define PSCI_MODE_SYSTEM_OFF		1
+
+enum stm32mp1_pm_domain {
+	STM32MP1_PD_VSW,
+	STM32MP1_PD_CORE_RET,
+	STM32MP1_PD_CORE,
+	STM32MP1_PD_MAX_PM_DOMAIN
+};
+
+enum pm_op {
+	PM_OP_SUSPEND,
+	PM_OP_RESUME,
+};
+
+/*
+ * Drivers can register a callback for the suspend and resume sequences and
+ * a private cookie passed as argument to the callback.
+ * The same callback is used for suspend and resume. First argument of the
+ * callback defines whether the device shall suspend or resume.
+ *
+ * Driver should tag its callback resource as unpaged (i.e KEEP_PAGER())
+ * since the callback is called from an unpaged execution context.
+ */
+void stm32mp_register_pm_cb(void (*callback)(enum pm_op op, void *handle),
+			    void *handle);
+
+void stm32_cores_reset(void);
+
+void stm32mp_gic_suspend_resume(enum pm_op op);
+void stm32mp_clock_suspend_resume(enum pm_op op);
+
+#endif /*ASM*/
+
+#endif /*__STM32MP_PM_H__*/
+
diff --git a/core/arch/arm/plat-stm32mp1/sub.mk b/core/arch/arm/plat-stm32mp1/sub.mk
index 4a559ea..c5a0deb 100644
--- a/core/arch/arm/plat-stm32mp1/sub.mk
+++ b/core/arch/arm/plat-stm32mp1/sub.mk
@@ -2,3 +2,9 @@ global-incdirs-y += .
 
 srcs-y += main.c
 srcs-y += reset.S
+srcs-y += shared_resources.c
+srcs-$(CFG_DT) += stm32mp1_dt.c
+
+subdirs-y += service
+subdirs-y += drivers
+subdirs-y += pm
diff --git a/core/arch/arm/sm/pm_a32.S b/core/arch/arm/sm/pm_a32.S
index 9421b2d..8a59cd2 100644
--- a/core/arch/arm/sm/pm_a32.S
+++ b/core/arch/arm/sm/pm_a32.S
@@ -69,25 +69,30 @@ a9_suspend:
 a7_suspend:
 	read_fcseidr r4
 	read_tpidruro r5
-	stmia	r0!, {r4 - r5}
-	read_dacr r4
+	read_dacr r6
+	stmia	r0!, {r4 - r6}
 #ifdef CFG_WITH_LPAE
-#error "Not supported"
+	read_ttbr0_64bit r4, r5
+	read_ttbr1_64bit r6, r7
+	read_mair0 r8
+	read_mair1 r9
+	stmia	r0!, {r4 - r9}
 #else
-	read_ttbr0 r5
-	read_ttbr1 r6
-	read_ttbcr r7
+	read_ttbr0 r4
+	read_ttbr1 r5
+	read_prrr r6
+	read_nmrr r7
+	stmia	r0!, {r4 - r7}
 #endif
-	read_sctlr r8
-	read_actlr r9
-	read_cpacr r10
-	read_mvbar r11
-	stmia	r0!, {r4 - r11}
-	read_prrr r4
-	read_nmrr r5
-	read_vbar r6
-	read_nsacr r7
-	stmia	r0, {r4 - r7}
+	read_ttbcr r4
+	read_actlr r5
+	read_cpacr r6
+	read_mvbar r7
+	read_vbar r8
+	read_nsacr r9
+	read_sctlr r10
+	stmia	r0, {r4 - r10}
+
 	pop	{r4 - r11}
 	bx	lr
 UNWIND(	.fnend)
@@ -154,50 +159,50 @@ UNWIND(	.cantunwind)
 	cmp	r5, r4
 	beq	a7_resume
 
-	/*
-	 * A9 needs PCR/DIAG
-	 */
+	/* A9 needs PCR/DIAG */
 	ldmia   r0!, {r4 - r5}
 	write_pcr r4
 	write_diag r5
-
 a7_resume:
-	/* v7 resume */
+	/* Armv7 generic resume */
 	mov	ip, #0
 	/* Invalidate icache to PoU */
 	write_iciallu
 	/* set reserved context */
 	write_contextidr ip
-	ldmia	r0!, {r4 - r5}
+	ldmia	r0!, {r4 - r6}
 	write_fcseidr r4
 	write_tpidruro r5
-	ldmia	r0!, {r4 - r11}
 	/* Invalidate entire TLB */
 	write_tlbiall
-	write_dacr r4
+	write_dacr r6
 #ifdef CFG_WITH_LPAE
-#error "Not supported -"
+	ldmia	r0!, {r4 - r9}
+	write_ttbr0_64bit r4, r5
+	write_ttbr1_64bit r6, r7
+	write_mair0 r8
+	write_mair1 r9
 #else
-	write_ttbr0 r5
-	write_ttbr1 r6
-	write_ttbcr r7
+	ldmia	r0!, {r4 - r7}
+	write_ttbr0 r4
+	write_ttbr1 r5
+	write_prrr r6
+	write_nmrr r7
 #endif
-
-	ldmia	r0, {r4 - r7}
-	write_prrr r4
-	write_nmrr r5
-	write_vbar r6
-	write_nsacr r7
-
-	write_actlr r9
-	write_cpacr r10
-	write_mvbar r11
+	ldmia	r0!, {r4 - r10}
+	write_ttbcr r4
+	write_actlr r5
+	write_cpacr r6
+	write_mvbar r7
+	write_vbar r8
+	write_nsacr r9
 	write_bpiall
 	isb
 	dsb
 	/* MMU will be enabled here */
-	write_sctlr r8
+	write_sctlr r10
 	isb
+
 	mov	r0, #0
 	b	suspend_return
 UNWIND(	.fnend)
diff --git a/core/arch/arm/tee/entry_std.c b/core/arch/arm/tee/entry_std.c
index 92fdf0f..e0836ed 100644
--- a/core/arch/arm/tee/entry_std.c
+++ b/core/arch/arm/tee/entry_std.c
@@ -602,4 +602,4 @@ static TEE_Result default_mobj_init(void)
 	return TEE_SUCCESS;
 }
 
-driver_init_late(default_mobj_init);
+service_init(default_mobj_init);
diff --git a/core/drivers/gic.c b/core/drivers/gic.c
index 9baffee..a000bff 100644
--- a/core/drivers/gic.c
+++ b/core/drivers/gic.c
@@ -1,5 +1,6 @@
 // SPDX-License-Identifier: BSD-2-Clause
 /*
+ * Copyright (c) 2015-2018, ARM Limited and Contributors. All rights reserved.
  * Copyright (c) 2016-2017, Linaro Limited
  * Copyright (c) 2014, STMicroelectronics International N.V.
  */
@@ -7,12 +8,14 @@
 #include <arm.h>
 #include <assert.h>
 #include <drivers/gic.h>
+#include <io.h>
 #include <keep.h>
 #include <kernel/interrupt.h>
 #include <kernel/panic.h>
+#include <malloc.h>
 #include <util.h>
-#include <io.h>
 #include <trace.h>
+#include <string.h>
 
 /* Offsets from gic.gicc_base */
 #define GICC_CTLR		(0x000)
@@ -32,8 +35,12 @@
 #define GICD_ICENABLER(n)	(0x180 + (n) * 4)
 #define GICD_ISPENDR(n)		(0x200 + (n) * 4)
 #define GICD_ICPENDR(n)		(0x280 + (n) * 4)
+#define GICD_ISACTIVER(n)	(0x300 + (n) * 4)
+#define GICD_ICACTIVER(n)	(0x380 + (n) * 4)
 #define GICD_IPRIORITYR(n)	(0x400 + (n) * 4)
 #define GICD_ITARGETSR(n)	(0x800 + (n) * 4)
+#define GICD_ICFGR(n)		(0xC00 + (n) * 4)
+#define GICD_NSACR(n)		(0xE00 + (n) * 4)
 #define GICD_SGIR		(0xF00)
 
 #define GICD_CTLR_ENABLEGRP0	(1 << 0)
@@ -74,6 +81,12 @@ static void gic_op_raise_sgi(struct itr_chip *chip, size_t it,
 static void gic_op_set_affinity(struct itr_chip *chip, size_t it,
 			uint8_t cpu_mask);
 
+#if !defined(CFG_ARM_GICV3)
+static uint8_t gic_op_set_pmr(struct itr_chip *chip, uint8_t mask);
+static uint8_t gic_op_set_ipriority(struct itr_chip *chip, size_t it,
+			uint8_t mask);
+#endif
+
 static const struct itr_ops gic_ops = {
 	.add = gic_op_add,
 	.enable = gic_op_enable,
@@ -81,6 +94,10 @@ static const struct itr_ops gic_ops = {
 	.raise_pi = gic_op_raise_pi,
 	.raise_sgi = gic_op_raise_sgi,
 	.set_affinity = gic_op_set_affinity,
+#if !defined(CFG_ARM_GICV3)
+	.set_pmr = gic_op_set_pmr,
+	.set_ipriority = gic_op_set_ipriority,
+#endif
 };
 KEEP_PAGER(gic_ops);
 
@@ -146,11 +163,11 @@ void gic_cpu_init(struct gic_data *gd)
 	 * allow the Non-secure world to adjust the priority mask itself
 	 */
 #if defined(CFG_ARM_GICV3)
-	write_icc_pmr(0x80);
+	write_icc_pmr(GIC_HIGHEST_NS_PRIORITY);
 	write_icc_ctlr(GICC_CTLR_ENABLEGRP0 | GICC_CTLR_ENABLEGRP1 |
 		       GICC_CTLR_FIQEN);
 #else
-	write32(0x80, gd->gicc_base + GICC_PMR);
+	write32(GIC_HIGHEST_NS_PRIORITY, gd->gicc_base + GICC_PMR);
 
 	/* Enable GIC */
 	write32(GICC_CTLR_ENABLEGRP0 | GICC_CTLR_ENABLEGRP1 | GICC_CTLR_FIQEN,
@@ -189,11 +206,11 @@ void gic_init(struct gic_data *gd, vaddr_t gicc_base __maybe_unused,
 	 * allow the Non-secure world to adjust the priority mask itself
 	 */
 #if defined(CFG_ARM_GICV3)
-	write_icc_pmr(0x80);
+	write_icc_pmr(GIC_HIGHEST_NS_PRIORITY);
 	write_icc_ctlr(GICC_CTLR_ENABLEGRP0 | GICC_CTLR_ENABLEGRP1 |
 		       GICC_CTLR_FIQEN);
 #else
-	write32(0x80, gd->gicc_base + GICC_PMR);
+	write32(GIC_HIGHEST_NS_PRIORITY, gd->gicc_base + GICC_PMR);
 
 	/* Enable GIC */
 	write32(GICC_CTLR_ENABLEGRP0 | GICC_CTLR_ENABLEGRP1 | GICC_CTLR_FIQEN,
@@ -254,9 +271,20 @@ static void gic_it_set_prio(struct gic_data *gd, size_t it, uint8_t prio)
 {
 	size_t idx __maybe_unused = it / NUM_INTS_PER_REG;
 	uint32_t mask __maybe_unused = 1 << (it % NUM_INTS_PER_REG);
+	bool group0 = ((read32(gd->gicd_base + GICD_IGROUPR(idx)) & mask) == 0);
 
-	/* Assigned to group0 */
-	assert(!(read32(gd->gicd_base + GICD_IGROUPR(idx)) & mask));
+	/*
+	 * Check priority against ARM recommendation: Group1 interrupts always
+	 * have a lower priority than group0 interrupts.
+	 * Note, lower numerical values have higher priorities so the comparison
+	 * checks below are reversed from what might be expected.
+	 */
+	if (group0) {
+		assert(prio <= GIC_LOWEST_SEC_PRIORITY);
+	} else {
+		assert(prio >= GIC_HIGHEST_NS_PRIORITY &&
+		       prio <= GIC_LOWEST_NS_PRIORITY);
+	}
 
 	/* Set prio it to selected CPUs */
 	DMSG("prio: writing 0x%x to 0x%" PRIxVA,
@@ -264,6 +292,34 @@ static void gic_it_set_prio(struct gic_data *gd, size_t it, uint8_t prio)
 	write8(prio, gd->gicd_base + GICD_IPRIORITYR(0) + it);
 }
 
+static uint8_t gic_op_set_pmr(struct itr_chip *chip, uint8_t mask)
+{
+	struct gic_data *gd = container_of(chip, struct gic_data, chip);
+	uint32_t pmr = read32(gd->gicc_base + GICC_PMR);
+
+	/*
+	 * Order memory updates w.r.t. PMR write, and ensure they're visible
+	 * before potential out of band interrupt trigger because of PMR update.
+	 */
+	dsb_ishst();
+	write32(mask, gd->gicc_base + GICC_PMR);
+	dsb_ishst();
+
+	return (uint8_t)pmr;
+}
+
+static uint8_t gic_op_set_ipriority(struct itr_chip *chip, size_t it,
+			uint8_t mask)
+{
+	struct gic_data *gd = container_of(chip, struct gic_data, chip);
+	uint8_t prio = read8(gd->gicd_base + GICD_IPRIORITYR(0) + it);
+
+	gic_it_set_prio(gd, it, mask);
+
+	return prio;
+}
+
+
 static void gic_it_enable(struct gic_data *gd, size_t it)
 {
 	size_t idx = it / NUM_INTS_PER_REG;
@@ -472,3 +528,143 @@ static void gic_op_set_affinity(struct itr_chip *chip, size_t it,
 
 	gic_it_set_cpu_mask(gd, it, cpu_mask);
 }
+
+#define IT_PM_GPOUP1_BIT	BIT(0)
+#define IT_PM_ENABLE_BIT	BIT(1)
+#define IT_PM_PENDING_BIT	BIT(2)
+#define IT_PM_ACTIVE_BIT	BIT(3)
+#define IT_PM_CONFIG_MASK	GENMASK_32(1, 0)
+
+struct gic_it_pm {
+	uint16_t it;
+	uint8_t flags;
+	uint8_t iprio;
+	uint8_t itarget;
+	uint8_t icfg;
+};
+
+static void gic_save_it(struct gic_data *gd,
+			unsigned int it, struct gic_it_pm *pm)
+{
+	size_t idx;
+	uint32_t bit_mask = BIT(it % NUM_INTS_PER_REG);
+	uint32_t shift2 = it % (NUM_INTS_PER_REG / 2);
+	uint32_t shift8 = it % (NUM_INTS_PER_REG / 8);
+	uint32_t data32;
+
+	assert(it < UINT16_MAX);
+	pm->it = (uint16_t)it;
+
+	idx = it / NUM_INTS_PER_REG;
+
+	pm->flags = 0;
+	if ((read32(gd->gicd_base + GICD_IGROUPR(idx)) & bit_mask) != 0) {
+		pm->flags |= IT_PM_GPOUP1_BIT;
+	}
+	if ((read32(gd->gicd_base + GICD_ISENABLER(idx)) & bit_mask) != 0) {
+		pm->flags |= IT_PM_ENABLE_BIT;
+	}
+	if ((read32(gd->gicd_base + GICD_ISPENDR(idx)) & bit_mask) != 0) {
+		pm->flags |= IT_PM_PENDING_BIT;
+	}
+	if ((read32(gd->gicd_base + GICD_ISACTIVER(idx)) & bit_mask) != 0) {
+		pm->flags |= IT_PM_ACTIVE_BIT;
+	}
+
+	idx = (8 * it) / NUM_INTS_PER_REG;
+
+	data32 = read32(gd->gicd_base + GICD_IPRIORITYR(idx)) >> shift8;
+	pm->iprio = (uint8_t)data32;
+
+	data32 = read32(gd->gicd_base + GICD_ITARGETSR(idx)) >> shift8;
+	pm->itarget = (uint8_t)data32;
+
+	/* Note: ICFGR is RAO for SPIs and PPIs */
+	idx = (2 * it) / NUM_INTS_PER_REG;
+	data32 = read32(gd->gicd_base + GICD_ICFGR(idx)) >> shift2;
+	pm->icfg = (uint8_t)data32 & IT_PM_CONFIG_MASK;
+}
+
+static void gic_restore_it(struct gic_data *gd, struct gic_it_pm *pm)
+{
+	size_t idx;
+	unsigned int it = (unsigned int)pm->it;
+	uint32_t mask = BIT(it % NUM_INTS_PER_REG);
+	uint32_t shift2 = it % (NUM_INTS_PER_REG / 2);
+	uint32_t shift8 = it % (NUM_INTS_PER_REG / 8);
+
+	idx = it / NUM_INTS_PER_REG;
+
+	io_mask32(gd->gicd_base + GICD_IGROUPR(idx),
+		  (pm->flags & IT_PM_GPOUP1_BIT) != 0 ? mask : 0, mask);
+
+	io_mask32(gd->gicd_base + GICD_ISENABLER(idx),
+		  (pm->flags & IT_PM_ENABLE_BIT) != 0 ? mask : 0, mask);
+
+	io_mask32(gd->gicd_base + GICD_ISPENDR(idx),
+		  (pm->flags & IT_PM_PENDING_BIT) != 0 ? mask : 0, mask);
+
+	io_mask32(gd->gicd_base + GICD_ISACTIVER(idx),
+		  (pm->flags & IT_PM_ACTIVE_BIT) != 0 ? mask : 0, mask);
+
+	idx = (8 * it) / NUM_INTS_PER_REG;
+
+	io_mask32(gd->gicd_base + GICD_IPRIORITYR(idx),
+		  (uint32_t)pm->iprio << shift8, UINT8_MAX << shift8);
+
+	io_mask32(gd->gicd_base + GICD_ITARGETSR(idx),
+		  (uint32_t)pm->itarget << shift8, UINT8_MAX << shift8);
+
+	/* Note: ICFGR is WI for SPIs and PPIs */
+	idx = (2 * it) / NUM_INTS_PER_REG;
+	io_mask32(gd->gicd_base + GICD_ICFGR(idx),
+		  (uint32_t)pm->icfg << shift2, IT_PM_CONFIG_MASK << shift2);
+}
+
+static bool it_is_group0(struct gic_data *gd, unsigned int it)
+{
+	size_t idx = it / NUM_INTS_PER_REG;
+	uint32_t groupr = read32(gd->gicd_base + GICD_IGROUPR(idx));
+	uint32_t bit_mask = BIT(it % NUM_INTS_PER_REG);
+
+	return (groupr & bit_mask) == 0;
+}
+
+/* Save the configuration for interrupts in group0 only */
+void gic_suspend(struct gic_data *gd)
+{
+	struct gic_pm *pm = &gd->pm;
+	unsigned int n;
+	size_t count;
+
+	for (count = 0, n = 0; n <= gd->max_it; n++) {
+		if (it_is_group0(gd, n)) {
+			count++;
+		}
+	}
+	pm->count = count;
+
+	if (count == 0) {
+		return;
+	}
+
+	pm->pm_cfg = realloc(pm->pm_cfg, count * sizeof(*pm->pm_cfg));
+	assert(pm->pm_cfg != NULL);
+
+	for (count = 0, n = 0; n <= gd->max_it; n++) {
+		if (it_is_group0(gd, n)) {
+			gic_save_it(gd, n, &pm->pm_cfg[count]);
+			count++;
+		}
+	}
+}
+
+void gic_resume(struct gic_data *gd)
+{
+	struct gic_pm *pm = &gd->pm;
+	size_t n;
+
+	for (n = 0; n < pm->count; n++) {
+		gic_restore_it(gd, &pm->pm_cfg[n]);
+	}
+}
diff --git a/core/drivers/stm32_bsec.c b/core/drivers/stm32_bsec.c
new file mode 100644
index 0000000..3355e0a
--- /dev/null
+++ b/core/drivers/stm32_bsec.c
@@ -0,0 +1,824 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics
+ */
+
+#include <assert.h>
+#include <drivers/stm32_bsec.h>
+#include <io.h>
+#include <kernel/dt.h>
+#include <kernel/generic_boot.h>
+#include <kernel/spinlock.h>
+#include <limits.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <stdint.h>
+#include <stm32_util.h>
+#include <stm32mp_dt.h>
+#include <util.h>
+
+#ifdef CFG_DT
+#include <libfdt.h>
+#endif
+
+#define BSEC_COMPAT	"st,stm32mp15-bsec"
+#define BITS_PER_WORD	(CHAR_BIT * sizeof(uint32_t))
+#define OTP_ACCESS_SIZE	(ROUNDUP(OTP_MAX_SIZE, BITS_PER_WORD) / BITS_PER_WORD)
+
+static uint32_t __maybe_unused otp_nsec_access[OTP_ACCESS_SIZE];
+
+static uint32_t bsec_power_safmem(bool enable);
+
+/* Bsec access protection */
+static unsigned int lock = SPINLOCK_UNLOCK;
+
+static uint32_t bsec_lock(void)
+{
+	return may_spin_lock(&lock);
+}
+
+static void bsec_unlock(uint32_t exceptions)
+{
+	may_spin_unlock(&lock, exceptions);
+}
+
+#ifdef CFG_DT
+static int bsec_get_dt_node(void *fdt, struct dt_node_info *info)
+{
+	int node;
+
+	node = fdt_get_node(fdt, info, -1, BSEC_COMPAT);
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return node;
+}
+
+static void enable_non_secure_access(uint32_t otp)
+{
+	otp_nsec_access[otp / BITS_PER_WORD] |= BIT(otp % BITS_PER_WORD);
+
+	if (bsec_shadow_register(otp) != BSEC_OK) {
+		panic();
+	}
+}
+
+static bool non_secure_can_access(uint32_t otp)
+{
+	return (otp_nsec_access[otp / BITS_PER_WORD] &
+		BIT(otp % BITS_PER_WORD)) != 0;
+}
+
+static int bsec_dt_otp_nsec_access(void *fdt, int bsec_node)
+{
+	int bsec_subnode;
+
+	fdt_for_each_subnode(bsec_subnode, fdt, bsec_node) {
+		const fdt32_t *cuint;
+		uint32_t reg;
+		uint32_t i;
+		uint32_t size;
+		uint8_t status;
+
+		cuint = fdt_getprop(fdt, bsec_subnode, "reg", NULL);
+		if (cuint == NULL) {
+			panic();
+		}
+
+		reg = fdt32_to_cpu(*cuint) / sizeof(uint32_t);
+		if (reg < STM32MP1_UPPER_OTP_START) {
+			continue;
+		}
+
+		status = _fdt_get_status(fdt, bsec_subnode);
+		if ((status & DT_STATUS_OK_NSEC) == 0U)  {
+			continue;
+		}
+
+		size = fdt32_to_cpu(*(cuint + 1)) / sizeof(uint32_t);
+
+		if ((fdt32_to_cpu(*(cuint + 1)) % sizeof(uint32_t)) != 0) {
+			size++;
+		}
+
+		for (i = reg; i < (reg + size); i++) {
+			enable_non_secure_access(i);
+		}
+	}
+
+	return 0;
+}
+#endif
+
+static uint32_t otp_bank_offset(uint32_t otp)
+{
+	assert(otp <= stm32mp_get_otp_max());
+
+	return ((otp & ~BSEC_OTP_MASK) >> BSEC_OTP_BANK_SHIFT) *
+		sizeof(uint32_t);
+}
+
+static uintptr_t bsec_get_base(void)
+{
+	static void *va;
+
+	if (!cpu_mmu_enabled())
+		return BSEC_BASE;
+
+	if (!va)
+		va = phys_to_virt(BSEC_BASE, MEM_AREA_IO_SEC);
+
+	return (vaddr_t)va;
+}
+
+/*
+ * bsec_check_error
+ * otp : OTP number.
+ * return value : BSEC_OK if no error.
+ */
+static uint32_t bsec_check_error(uint32_t otp)
+{
+	uint32_t bit = BIT(otp & BSEC_OTP_MASK);
+	uint32_t bank = otp_bank_offset(otp);
+
+	if ((read32(bsec_get_base() + BSEC_DISTURBED_OFF + bank) & bit) != 0U) {
+		return BSEC_DISTURBED;
+	}
+
+	if ((read32(bsec_get_base() + BSEC_ERROR_OFF + bank) & bit) != 0U) {
+		return BSEC_ERROR;
+	}
+
+	return BSEC_OK;
+}
+
+/*
+ * bsec_shadow_register: copy SAFMEM OTP to BSEC data.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_shadow_register(uint32_t otp)
+{
+	uint32_t result;
+	uint32_t exc;
+
+	if (otp > stm32mp_get_otp_max()) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	/* Check if shadowing of OTP is locked */
+	if (bsec_read_sr_lock(otp)) {
+		IMSG("BSEC: OTP locked, register will not be refreshed");
+	}
+
+	result = bsec_power_safmem(true);
+	if (result != BSEC_OK) {
+		return result;
+	}
+
+	exc = bsec_lock();
+
+	write32(otp | BSEC_READ, bsec_get_base() + BSEC_OTP_CTRL_OFF);
+
+	while ((bsec_get_status() & BSEC_MODE_BUSY_MASK) != 0U) {
+		;
+	}
+
+	result = bsec_check_error(otp);
+
+	bsec_unlock(exc);
+
+	bsec_power_safmem(false);
+
+	return result;
+}
+
+/*
+ * bsec_read_otp: read an OTP data value.
+ * val: read value.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_read_otp(uint32_t *val, uint32_t otp)
+{
+	uint32_t exc;
+	uint32_t result;
+
+	if (otp > stm32mp_get_otp_max()) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	exc = bsec_lock();
+
+	*val = read32(bsec_get_base() + BSEC_OTP_DATA_OFF +
+		      (otp * sizeof(uint32_t)));
+
+	result = bsec_check_error(otp);
+
+	bsec_unlock(exc);
+
+	return result;
+}
+
+/*
+ * bsec_write_otp: write value in BSEC data register.
+ * val: value to write.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_write_otp(uint32_t val, uint32_t otp)
+{
+	uint32_t exc;
+	uint32_t result;
+
+	if (otp > stm32mp_get_otp_max()) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	/* Check if programming of OTP is locked */
+	if (bsec_read_sw_lock(otp)) {
+		IMSG("BSEC: OTP locked, write will be ignored");
+	}
+
+	exc = bsec_lock();
+
+	write32(val, bsec_get_base() + BSEC_OTP_DATA_OFF +
+		(otp * sizeof(uint32_t)));
+
+	result = bsec_check_error(otp);
+
+	bsec_unlock(exc);
+
+	return result;
+}
+
+/*
+ * bsec_program_otp: program a bit in SAFMEM after the prog.
+ *	The OTP data is not refreshed.
+ * val: value to program.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_program_otp(uint32_t val, uint32_t otp)
+{
+	uint32_t result;
+	uint32_t exc;
+
+	if (otp > stm32mp_get_otp_max()) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	/* Check if programming of OTP is locked */
+	if (bsec_read_sp_lock(otp)) {
+		IMSG("BSEC: OTP locked, prog will be ignored");
+	}
+
+	if ((read32(bsec_get_base() + BSEC_OTP_LOCK_OFF) &
+	    BIT(BSEC_LOCK_PROGRAM)) != 0U) {
+		IMSG("BSEC: GPLOCK activated, prog will be ignored");
+	}
+
+	result = bsec_power_safmem(true);
+	if (result != BSEC_OK) {
+		return result;
+	}
+
+	exc = bsec_lock();
+
+	write32(val, bsec_get_base() + BSEC_OTP_WRDATA_OFF);
+	write32(otp | BSEC_WRITE, bsec_get_base() + BSEC_OTP_CTRL_OFF);
+
+	while ((bsec_get_status() & BSEC_MODE_BUSY_MASK) != 0U) {
+		;
+	}
+
+	if ((bsec_get_status() & BSEC_MODE_PROGFAIL_MASK) != 0U) {
+		result = BSEC_PROG_FAIL;
+	} else {
+		result = bsec_check_error(otp);
+	}
+
+	bsec_unlock(exc);
+
+	bsec_power_safmem(false);
+
+	return result;
+}
+
+/*
+ * bsec_permanent_lock_otp: permanent lock of OTP in SAFMEM.
+ * otp: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_permanent_lock_otp(uint32_t otp)
+{
+	uint32_t result;
+	uint32_t data;
+	uint32_t addr;
+	uint32_t exc;
+
+	if (otp > stm32mp_get_otp_max()) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	result = bsec_power_safmem(true);
+	if (result != BSEC_OK) {
+		return result;
+	}
+
+	if (otp < stm32mp_get_otp_upper_start()) {
+		addr = otp >> ADDR_LOWER_OTP_PERLOCK_SHIFT;
+		data = DATA_LOWER_OTP_PERLOCK_BIT <<
+		       ((otp & DATA_LOWER_OTP_PERLOCK_MASK) << 1U);
+	} else {
+		addr = (otp >> ADDR_UPPER_OTP_PERLOCK_SHIFT) + 2U;
+		data = DATA_UPPER_OTP_PERLOCK_BIT <<
+		       (otp & DATA_UPPER_OTP_PERLOCK_MASK);
+	}
+
+	exc = bsec_lock();
+
+	write32(data, bsec_get_base() + BSEC_OTP_WRDATA_OFF);
+	write32(addr | BSEC_WRITE | BSEC_LOCK,
+		bsec_get_base() + BSEC_OTP_CTRL_OFF);
+
+	while ((bsec_get_status() & BSEC_MODE_BUSY_MASK) != 0U) {
+		;
+	}
+
+	if ((bsec_get_status() & BSEC_MODE_PROGFAIL_MASK) != 0U) {
+		result = BSEC_PROG_FAIL;
+	} else {
+		result = bsec_check_error(otp);
+	}
+
+	bsec_unlock(exc);
+
+	bsec_power_safmem(false);
+
+	return result;
+}
+
+/*
+ * bsec_write_debug_conf: write value in debug feature
+ *	to enable/disable debug service.
+ * val: value to write.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_write_debug_conf(uint32_t val)
+{
+	uint32_t result = BSEC_ERROR;
+	uint32_t masked_val = val & BSEC_DEN_ALL_MSK;
+	unsigned int exc;
+
+	exc = bsec_lock();
+
+	write32(val, bsec_get_base() + BSEC_DEN_OFF);
+
+	if ((read32(bsec_get_base() + BSEC_DEN_OFF) ^ masked_val) == 0U) {
+		result = BSEC_OK;
+	}
+
+	bsec_unlock(exc);
+
+	return result;
+}
+
+/*
+ * bsec_read_debug_conf : read debug configuration.
+ */
+uint32_t bsec_read_debug_conf(void)
+{
+	return read32(bsec_get_base() + BSEC_DEN_OFF);
+}
+
+/*
+ * bsec_get_status : return status register value.
+ */
+uint32_t bsec_get_status(void)
+{
+	return read32(bsec_get_base() + BSEC_OTP_STATUS_OFF);
+}
+
+/*
+ * bsec_get_hw_conf : return hardware configuration.
+ */
+uint32_t bsec_get_hw_conf(void)
+{
+	return read32(bsec_get_base() + BSEC_IPHW_CFG_OFF);
+}
+
+/*
+ * bsec_get_version : return BSEC version.
+ */
+uint32_t bsec_get_version(void)
+{
+	return read32(bsec_get_base() + BSEC_IPVR_OFF);
+}
+
+/*
+ * bsec_get_id : return BSEC ID.
+ */
+uint32_t bsec_get_id(void)
+{
+	return read32(bsec_get_base() + BSEC_IP_ID_OFF);
+}
+
+/*
+ * bsec_get_magic_id : return BSEC magic number.
+ */
+uint32_t bsec_get_magic_id(void)
+{
+	return read32(bsec_get_base() + BSEC_IP_MAGIC_ID_OFF);
+}
+
+/*
+ * bsec_write_sr_lock: write shadow-read lock.
+ * otp: OTP number.
+ * value: value to write in the register.
+ *	Must be always 1.
+ * return: true if OTP is locked, else false.
+ */
+bool bsec_write_sr_lock(uint32_t otp, uint32_t value)
+{
+	bool result = false;
+	uint32_t bank = otp_bank_offset(otp);
+	uint32_t bank_value;
+	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+	uint32_t exc;
+
+	exc = bsec_lock();
+
+	bank_value = read32(bsec_get_base() + BSEC_SRLOCK_OFF + bank);
+
+	if ((bank_value & otp_mask) == value) {
+		/*
+		 * In case of write don't need to write,
+		 * the lock is already set.
+		 */
+		if (value != 0U) {
+			result = true;
+		}
+	} else {
+		if (value != 0U) {
+			bank_value = bank_value | otp_mask;
+		} else {
+			bank_value = bank_value & ~otp_mask;
+		}
+
+		/*
+		 * We can write 0 in all other OTP
+		 * if the lock is activated in one of other OTP.
+		 * Write 0 has no effect.
+		 */
+		write32(bank_value, bsec_get_base() + BSEC_SRLOCK_OFF + bank);
+		result = true;
+	}
+
+	bsec_unlock(exc);
+
+	return result;
+}
+
+/*
+ * bsec_read_sr_lock: read shadow-read lock.
+ * otp: OTP number.
+ * return: true if otp is locked, else false.
+ */
+bool bsec_read_sr_lock(uint32_t otp)
+{
+	uint32_t bank = otp_bank_offset(otp);
+	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+	uint32_t bank_value = read32(bsec_get_base() + BSEC_SRLOCK_OFF + bank);
+
+	return (bank_value & otp_mask) != 0U;
+}
+
+/*
+ * bsec_write_sw_lock: write shadow-write lock.
+ * otp: OTP number.
+ * value: Value to write in the register.
+ *	Must be always 1.
+ * return: true if OTP is locked, else false.
+ */
+bool bsec_write_sw_lock(uint32_t otp, uint32_t value)
+{
+	bool result = false;
+	uint32_t bank = otp_bank_offset(otp);
+	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+	uint32_t bank_value;
+	unsigned int exc;
+
+	exc = bsec_lock();
+
+	bank_value = read32(bsec_get_base() + BSEC_SWLOCK_OFF + bank);
+
+	if ((bank_value & otp_mask) == value) {
+		/*
+		 * In case of write don't need to write,
+		 * the lock is already set.
+		 */
+		if (value != 0U) {
+			result = true;
+		}
+	} else {
+		if (value != 0U) {
+			bank_value = bank_value | otp_mask;
+		} else {
+			bank_value = bank_value & ~otp_mask;
+		}
+
+		/*
+		 * We can write 0 in all other OTP
+		 * if the lock is activated in one of other OTP.
+		 * Write 0 has no effect.
+		 */
+		write32(bank_value, bsec_get_base() + BSEC_SWLOCK_OFF + bank);
+		result = true;
+	}
+
+	bsec_unlock(exc);
+
+	return result;
+}
+
+/*
+ * bsec_read_sw_lock: read shadow-write lock.
+ * otp: OTP number.
+ * return: true if OTP is locked, else false.
+ */
+bool bsec_read_sw_lock(uint32_t otp)
+{
+	uint32_t bank = otp_bank_offset(otp);
+	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+	uint32_t bank_value = read32(bsec_get_base() + BSEC_SWLOCK_OFF + bank);
+
+	return (bank_value & otp_mask) != 0U;
+}
+
+/*
+ * bsec_write_sp_lock: write shadow-program lock.
+ * otp: OTP number.
+ * value: Value to write in the register.
+ *	Must be always 1.
+ * return: true if OTP is locked, else false.
+ */
+bool bsec_write_sp_lock(uint32_t otp, uint32_t value)
+{
+	bool result = false;
+	uint32_t bank = otp_bank_offset(otp);
+	uint32_t bank_value;
+	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+	unsigned int exc;
+
+	exc = bsec_lock();
+
+	bank_value = read32(bsec_get_base() + BSEC_SPLOCK_OFF + bank);
+
+	if ((bank_value & otp_mask) == value) {
+		/*
+		 * In case of write don't need to write,
+		 * the lock is already set.
+		 */
+		if (value != 0U) {
+			result = true;
+		}
+	} else {
+		if (value != 0U) {
+			bank_value = bank_value | otp_mask;
+		} else {
+			bank_value = bank_value & ~otp_mask;
+		}
+
+		/*
+		 * We can write 0 in all other OTP
+		 * if the lock is activated in one of other OTP.
+		 * Write 0 has no effect.
+		 */
+		write32(bank_value, bsec_get_base() + BSEC_SPLOCK_OFF + bank);
+		result = true;
+	}
+
+	bsec_unlock(exc);
+
+	return result;
+}
+
+/*
+ * bsec_read_sp_lock: read shadow-program lock.
+ * otp: OTP number.
+ * return: true if OTP is locked, else false.
+ */
+bool bsec_read_sp_lock(uint32_t otp)
+{
+	uint32_t bank = otp_bank_offset(otp);
+	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+	uint32_t bank_value = read32(bsec_get_base() + BSEC_SPLOCK_OFF + bank);
+
+	return (bank_value & otp_mask) != 0U;
+}
+
+/*
+ * bsec_wr_lock: Read permanent lock status.
+ * otp: OTP number.
+ * return: true if OTP is locked, else false.
+ */
+bool bsec_wr_lock(uint32_t otp)
+{
+	uint32_t bank = otp_bank_offset(otp);
+	uint32_t lock_bit = BIT(otp & BSEC_OTP_MASK);
+
+	if ((read32(bsec_get_base() + BSEC_WRLOCK_OFF + bank) &
+	     lock_bit) != 0U) {
+		/*
+		 * In case of write don't need to write,
+		 * the lock is already set.
+		 */
+		return true;
+	}
+
+	return false;
+}
+
+/*
+ * bsec_otp_lock: Lock Upper OTP or Global programming or debug enable
+ * service: Service to lock see header file.
+ * value: Value to write must always set to 1 (only use for debug purpose).
+ * return: BSEC_OK if succeed.
+ */
+uint32_t bsec_otp_lock(uint32_t service, uint32_t value)
+{
+	uintptr_t reg = bsec_get_base() + BSEC_OTP_LOCK_OFF;
+
+	switch (service) {
+	case BSEC_LOCK_UPPER_OTP:
+		write32(value << BSEC_LOCK_UPPER_OTP, reg);
+		break;
+	case BSEC_LOCK_DEBUG:
+		write32(value << BSEC_LOCK_DEBUG, reg);
+		break;
+	case BSEC_LOCK_PROGRAM:
+		write32(value << BSEC_LOCK_PROGRAM, reg);
+		break;
+	default:
+		return BSEC_INVALID_PARAM;
+	}
+
+	return BSEC_OK;
+}
+
+static uint32_t enable_power(void)
+{
+	size_t cntdown;
+
+	io_mask32(bsec_get_base() + BSEC_OTP_CONF_OFF, BSEC_CONF_POWER_UP_MASK,
+		  BSEC_CONF_POWER_UP_MASK);
+
+	for (cntdown = BSEC_TIMEOUT_VALUE; cntdown; cntdown--) {
+		if (bsec_get_status() & BSEC_MODE_PWR_MASK) {
+			break;
+		}
+	}
+
+	return cntdown ? BSEC_OK : BSEC_TIMEOUT;
+}
+
+static uint32_t disable_power(void)
+{
+	size_t cntdown;
+
+	io_mask32(bsec_get_base() + BSEC_OTP_CONF_OFF, 0,
+		  BSEC_CONF_POWER_UP_MASK);
+
+	for (cntdown = BSEC_TIMEOUT_VALUE; cntdown; cntdown--) {
+		if (!(bsec_get_status() & BSEC_MODE_PWR_MASK)) {
+			break;
+		}
+	}
+
+	return cntdown ? BSEC_OK : BSEC_TIMEOUT;
+}
+
+/*
+ * bsec_power_safmem: Activate or deactivate SAFMEM power.
+ * power: true to power up, false to power down.
+ * return: BSEC_OK if succeed.
+ */
+static uint32_t bsec_power_safmem(bool enable)
+{
+	static unsigned int refcnt = ~0UL;
+	uint32_t result = BSEC_OK;
+	uint32_t exc = 0;
+
+	/* Get the initial state */
+	if (refcnt == ~0UL) {
+		refcnt = !!(bsec_get_status() & BSEC_MODE_PWR_MASK);
+		DMSG("Reset SAFMEM refcnt to %u", refcnt);
+	}
+
+	exc = bsec_lock();
+
+	if (enable && (incr_refcnt(&refcnt) != 0U)) {
+		result = enable_power();
+	}
+
+	if (!enable && (decr_refcnt(&refcnt) != 0U)) {
+		result = disable_power();
+	}
+
+	bsec_unlock(exc);
+
+	return result;
+}
+
+/*
+ * bsec_mode_is_closed_device: read OTP secure sub-mode.
+ * return: false if open_device and true of closed_device.
+ */
+bool bsec_mode_is_closed_device(void)
+{
+	uint32_t value;
+
+	if ((bsec_shadow_register(DATA0_OTP) != BSEC_OK) ||
+	    (bsec_read_otp(&value, DATA0_OTP) != BSEC_OK)) {
+		return true;
+	}
+
+	return (value & DATA0_OTP_SECURED) == DATA0_OTP_SECURED;
+}
+
+/*
+ * bsec_shadow_read_otp: Load OTP from SAFMEM and provide its value
+ * otp_value: read value.
+ * word: OTP number.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_shadow_read_otp(uint32_t *otp_value, uint32_t word)
+{
+	uint32_t result;
+
+	result = bsec_shadow_register(word);
+	if (result != BSEC_OK) {
+		EMSG("BSEC: %u Shadowing Error %i\n", word, result);
+		return result;
+	}
+
+	result = bsec_read_otp(otp_value, word);
+	if (result != BSEC_OK) {
+		EMSG("BSEC: %u Read Error %i\n", word, result);
+	}
+
+	return result;
+}
+
+/*
+ * bsec_check_nsec_access_rights: check non-secure access rights to target OTP.
+ * otp: OTP number.
+ * return: BSEC_OK if authorized access.
+ */
+uint32_t bsec_check_nsec_access_rights(uint32_t otp)
+{
+	if (otp > stm32mp_get_otp_max()) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	if (otp >= stm32mp_get_otp_upper_start()) {
+		/* Check if BSEC is in OTP-SECURED closed_device state. */
+		if (bsec_mode_is_closed_device()) {
+#ifdef CFG_DT
+			if (!non_secure_can_access(otp)) {
+				return BSEC_ERROR;
+			}
+#else
+			return BSEC_ERROR;
+#endif
+		}
+	}
+
+	return BSEC_OK;
+}
+
+#ifdef CFG_DT
+static TEE_Result initialize_bsec(void)
+{
+	void *fdt;
+	int node;
+	struct dt_node_info bsec_info;
+
+	fdt = get_dt_blob();
+	if (fdt == NULL) {
+		panic();
+	}
+
+	node = bsec_get_dt_node(fdt, &bsec_info);
+	if (node < 0) {
+		panic();
+	}
+
+	bsec_dt_otp_nsec_access(fdt, node);
+
+	return TEE_SUCCESS;
+}
+driver_init(initialize_bsec);
+#endif
diff --git a/core/drivers/stm32_etzpc.c b/core/drivers/stm32_etzpc.c
new file mode 100644
index 0000000..fcb32e6
--- /dev/null
+++ b/core/drivers/stm32_etzpc.c
@@ -0,0 +1,337 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2015-2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2018, STMicroelectronics
+ */
+
+#include <assert.h>
+#include <drivers/stm32_etzpc.h>
+#include <kernel/dt.h>
+#include <kernel/generic_boot.h>
+#include <initcall.h>
+#include <io.h>
+#include <keep.h>
+#include <kernel/panic.h>
+#include <mm/core_memprot.h>
+#include <stdint.h>
+#include <stm32_util.h>
+#include <stm32mp_dt.h>
+#include <stm32mp_pm.h>
+#include <util.h>
+
+#ifdef CFG_DT
+#include <libfdt.h>
+#endif
+
+#define ETZPC_COMPAT		"st,stm32-etzpc"
+#define ETZPC_LOCK_MASK		0x1U
+#define ETZPC_MODE_SHIFT	8
+#define ETZPC_MODE_MASK		GENMASK_32(1, 0)
+#define ETZPC_ID_SHIFT		16
+#define ETZPC_ID_MASK		GENMASK_32(7, 0)
+
+/* ID Registers */
+#define ETZPC_TZMA0_SIZE		0x000U
+#define ETZPC_DECPROT0			0x010U
+#define ETZPC_DECPROT_LOCK0		0x030U
+#define ETZPC_HWCFGR			0x3F0U
+#define ETZPC_VERR			0x3F4U
+
+/* ID Registers fields */
+#define ETZPC_TZMA0_SIZE_LOCK		BIT(31)
+#define ETZPC_DECPROT0_MASK		GENMASK_32(1, 0)
+#define ETZPC_HWCFGR_NUM_TZMA_SHIFT	0
+#define ETZPC_HWCFGR_NUM_PER_SEC_SHIFT	8
+#define ETZPC_HWCFGR_NUM_AHB_SEC_SHIFT	16
+#define ETZPC_HWCFGR_CHUNCKS1N4_SHIFT	24
+
+#define DECPROT_SHIFT			1
+#define IDS_PER_DECPROT_REGS		16U
+#define IDS_PER_DECPROT_LOCK_REGS	32U
+
+#define ETZPC_TZMA_ALL_SECURE		0x3FF
+#define ETZPC_TZMA_ALL_NO_SECURE	0x000
+
+/*
+ * etzpc_instance.
+ * base : register virtual base address set during init given by user
+ * pbase : register physical base address set during init given by user
+ * chunk_size : supported TZMA size steps
+ * num_tzma: number of TZMA zone read from register at init
+ * num_ahb_sec : number of securable AHB master zone read from register
+ * num_per_sec : number of securable AHB & APB Peripherals read from register
+ * revision : IP revision read from register at init
+ * periph_cfg : buffer storing holding the DECPROT configuration for peripherals
+ */
+struct etzpc_instance {
+	uintptr_t base;
+	paddr_t pbase;
+	uint8_t chunck_size;
+	uint8_t num_tzma;
+	uint8_t num_per_sec;
+	uint8_t num_ahb_sec;
+	uint8_t revision;
+	uint8_t *periph_cfg;
+};
+
+/* Only 1 instance of the ETZPC is expected per platform */
+static struct etzpc_instance etzpc_dev;
+
+#ifdef CFG_DT
+struct dt_id_attr {
+	/* The effective size of the array is meaningless here */
+	fdt32_t id_attr[1];
+};
+#endif
+
+/*
+ * Implementation uses uint8_t to store each securable DECPROT configuration.
+ * When resuming from deep suspend, the DECPROT configurations are restored.
+ */
+#define PERIPH_LOCK_BIT		BIT(7)
+#define PERIPH_ATTR_MASK	GENMASK_32(2, 0)
+
+static uintptr_t etzpc_base(void)
+{
+	if (!cpu_mmu_enabled())
+		return etzpc_dev.pbase;
+
+	if (!etzpc_dev.base)
+		etzpc_dev.base = (uintptr_t)phys_to_virt(etzpc_dev.pbase,
+							 MEM_AREA_IO_SEC);
+
+	return etzpc_dev.base;
+}
+
+static bool valid_decprot_id(unsigned int id)
+{
+	return id < (unsigned int)etzpc_dev.num_per_sec;
+}
+
+static bool valid_tzma_id(unsigned int id)
+{
+	return id < (unsigned int)etzpc_dev.num_tzma;
+}
+
+#ifdef CFG_DT
+static int etzpc_dt_conf_decprot(void *fdt, int node)
+{
+	const struct dt_id_attr *conf_list;
+	unsigned int i;
+	int len = 0;
+
+	conf_list = (const struct dt_id_attr *)fdt_getprop(fdt, node,
+							   "st,decprot", &len);
+	if (conf_list == NULL) {
+		IMSG("No ETZPC configuration in DT, use default");
+		return 0;
+	}
+
+	for (i = 0; i < len / sizeof(uint32_t); i++) {
+		enum etzpc_decprot_attributes attr;
+		uint32_t value;
+		uint32_t id;
+		uint32_t mode;
+
+		value = fdt32_to_cpu(conf_list->id_attr[i]);
+
+		id = ((value >> ETZPC_ID_SHIFT) & ETZPC_ID_MASK);
+		if (!valid_decprot_id(id)) {
+			EMSG("Invalid DECPROT %" PRIu32, id);
+			return -1;
+		}
+
+		mode = (value >> ETZPC_MODE_SHIFT) & ETZPC_MODE_MASK;
+		attr = stm32mp_etzpc_binding2decprot(mode);
+
+		stm32mp_register_etzpc_decprot(id, attr);
+
+		etzpc_configure_decprot(id, attr);
+
+		if ((value & ETZPC_LOCK_MASK) != 0U) {
+			etzpc_lock_decprot(id);
+		}
+	}
+
+	return 0;
+}
+#endif
+
+void etzpc_configure_decprot(uint32_t decprot_id,
+			     enum etzpc_decprot_attributes decprot_attr)
+{
+	uintptr_t offset = 4U * (decprot_id / IDS_PER_DECPROT_REGS);
+	uint32_t shift = (decprot_id % IDS_PER_DECPROT_REGS) << DECPROT_SHIFT;
+	uint32_t masked_decprot = (uint32_t)decprot_attr & ETZPC_DECPROT0_MASK;
+	uintptr_t base = etzpc_base();
+
+	assert(valid_decprot_id(decprot_id));
+
+	mmio_clrsetbits_32(base + ETZPC_DECPROT0 + offset,
+			   (uint32_t)ETZPC_DECPROT0_MASK << shift,
+			   masked_decprot << shift);
+
+	etzpc_dev.periph_cfg[decprot_id] = (uint8_t)decprot_attr;
+
+	assert((decprot_attr & ~PERIPH_ATTR_MASK) == 0);
+	COMPILE_TIME_ASSERT(TZPC_DECPROT_MAX <= UINT8_MAX);
+}
+
+enum etzpc_decprot_attributes etzpc_get_decprot(uint32_t decprot_id)
+{
+	uintptr_t offset = 4U * (decprot_id / IDS_PER_DECPROT_REGS);
+	uint32_t shift = (decprot_id % IDS_PER_DECPROT_REGS) << DECPROT_SHIFT;
+	uintptr_t base_decprot = etzpc_base() + offset;
+	uint32_t value;
+
+	assert(valid_decprot_id(decprot_id));
+
+	value = (read32(base_decprot + ETZPC_DECPROT0) >> shift) &
+		ETZPC_DECPROT0_MASK;
+
+	return (enum etzpc_decprot_attributes)value;
+}
+
+void etzpc_lock_decprot(uint32_t decprot_id)
+{
+	uintptr_t offset = 4U * (decprot_id / IDS_PER_DECPROT_LOCK_REGS);
+	uint32_t shift = BIT(decprot_id % IDS_PER_DECPROT_LOCK_REGS);
+	uintptr_t base_decprot = etzpc_base() + offset;
+
+	assert(valid_decprot_id(decprot_id));
+
+	write32(shift, base_decprot + ETZPC_DECPROT_LOCK0);
+
+	etzpc_dev.periph_cfg[decprot_id] |= PERIPH_LOCK_BIT;
+}
+
+void etzpc_configure_tzma(uint32_t tzma_id, uint16_t tzma_value)
+{
+	assert(valid_tzma_id(tzma_id));
+
+	write32(tzma_value, etzpc_base() + ETZPC_TZMA0_SIZE +
+		(sizeof(uint32_t) * tzma_id));
+}
+
+uint16_t etzpc_get_tzma(uint32_t tzma_id)
+{
+	assert(valid_tzma_id(tzma_id));
+
+	return read32(etzpc_base() + ETZPC_TZMA0_SIZE +
+		      (sizeof(uint32_t) * tzma_id));
+}
+
+void etzpc_lock_tzma(uint32_t tzma_id)
+{
+	assert(valid_tzma_id(tzma_id));
+
+	mmio_setbits_32(etzpc_base() + ETZPC_TZMA0_SIZE +
+			(sizeof(uint32_t) * tzma_id), ETZPC_TZMA0_SIZE_LOCK);
+}
+
+bool etzpc_get_lock_tzma(uint32_t tzma_id)
+{
+	uint32_t tzma_size;
+
+	assert(valid_tzma_id(tzma_id));
+
+	tzma_size = read32(etzpc_base() + ETZPC_TZMA0_SIZE +
+			   (sizeof(uint32_t) * tzma_id));
+
+	return (tzma_size & ETZPC_TZMA0_SIZE_LOCK) != 0;
+}
+
+static void etzpc_suspend_resume(enum pm_op op, void __unused *handle)
+{
+	unsigned int n;
+
+	if (op == PM_OP_SUSPEND) {
+		return;
+	}
+
+	/* OP-TEE owns the whole in SYSRAM */
+	etzpc_configure_tzma(1, ETZPC_TZMA_ALL_SECURE);
+	etzpc_lock_tzma(1);
+
+	for (n = 0; n < etzpc_dev.num_per_sec; n++) {
+		unsigned int attr = etzpc_dev.periph_cfg[n] & PERIPH_ATTR_MASK;
+
+		etzpc_configure_decprot(n, (enum etzpc_decprot_attributes)attr);
+
+		if (etzpc_dev.periph_cfg[n] & PERIPH_LOCK_BIT) {
+			etzpc_lock_decprot(n);
+		}
+	}
+}
+KEEP_PAGER(etzpc_suspend_resume);
+
+static TEE_Result etzpc_init(void)
+{
+	void *fdt __maybe_unused;
+	int node __maybe_unused;
+	struct dt_node_info etzpc_info __maybe_unused;
+	uintptr_t base;
+	uint32_t hwcfg;
+	size_t n;
+
+#ifdef CFG_DT
+	fdt = get_dt_blob();
+	if (!fdt) {
+		panic();
+	}
+
+	node = fdt_get_node(fdt, &etzpc_info, -1, ETZPC_COMPAT);
+	if (node >= 0) {
+		etzpc_dev.pbase = etzpc_info.base;
+		assert(etzpc_dev.pbase == stm32mp_get_etzpc_base());
+
+		if (etzpc_info.status != DT_STATUS_OK_SEC) {
+			EMSG("ETZPC DT status");
+			panic();
+		}
+	}
+#endif
+
+	if (!etzpc_dev.pbase) {
+		etzpc_dev.pbase = stm32mp_get_etzpc_base();
+	}
+
+	base = etzpc_base();
+
+	hwcfg = read32(base + ETZPC_HWCFGR);
+	etzpc_dev.num_tzma = (uint8_t)(hwcfg >> ETZPC_HWCFGR_NUM_TZMA_SHIFT);
+	etzpc_dev.num_per_sec = (uint8_t)(hwcfg >>
+					  ETZPC_HWCFGR_NUM_PER_SEC_SHIFT);
+	etzpc_dev.num_ahb_sec = (uint8_t)(hwcfg >>
+					  ETZPC_HWCFGR_NUM_AHB_SEC_SHIFT);
+	etzpc_dev.chunck_size = (uint8_t)(hwcfg >>
+					  ETZPC_HWCFGR_CHUNCKS1N4_SHIFT);
+
+	etzpc_dev.revision = read8(base + ETZPC_VERR);
+
+	etzpc_dev.periph_cfg = calloc(etzpc_dev.num_per_sec,
+				      sizeof(*etzpc_dev.periph_cfg));
+	if (etzpc_dev.periph_cfg == NULL) {
+		panic();
+	}
+	for (n = 0; n < etzpc_dev.num_per_sec; n++) {
+		etzpc_configure_decprot(n, etzpc_get_decprot(n));
+	}
+
+	DMSG("ETZPC version 0x%" PRIx8, etzpc_dev.revision);
+
+#ifdef CFG_DT
+	if (etzpc_dt_conf_decprot(fdt, node)) {
+		panic();
+	}
+#endif
+
+	/* OP-TEE owns the whole in SYSRAM */
+	etzpc_configure_tzma(1, ETZPC_TZMA_ALL_SECURE);
+	etzpc_lock_tzma(1);
+
+	stm32mp_register_pm_cb(etzpc_suspend_resume, NULL);
+
+	return TEE_SUCCESS;
+}
+driver_init(etzpc_init);
diff --git a/core/drivers/stm32_gpio.c b/core/drivers/stm32_gpio.c
new file mode 100644
index 0000000..cb18271
--- /dev/null
+++ b/core/drivers/stm32_gpio.c
@@ -0,0 +1,417 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2016-2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <assert.h>
+#include <drivers/stm32_gpio.h>
+#include <io.h>
+#include <kernel/dt.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <stdbool.h>
+#include <stm32_util.h>
+#include <stm32mp_dt.h>
+#include <trace.h>
+#include <util.h>
+
+#ifdef CFG_DT
+#include <libfdt.h>
+#endif
+
+#define DT_GPIO_BANK_SHIFT	12
+#define DT_GPIO_BANK_MASK	GENMASK_32(16, 12)
+#define DT_GPIO_PIN_SHIFT	8
+#define DT_GPIO_PIN_MASK	GENMASK_32(11, 8)
+#define DT_GPIO_MODE_MASK	GENMASK_32(7, 0)
+
+static void get_gpio_cfg(uint32_t bank, uint32_t pin, struct gpio_cfg *cfg)
+{
+	uintptr_t base = stm32_get_gpio_bank_base(bank);
+	int clock = stm32_get_gpio_bank_clock(bank);
+
+	stm32_clock_enable((unsigned long)clock);
+
+	cfg->moder = (mmio_read_32(base + GPIO_MODE_OFFSET) >> (pin << 1)) &
+			GPIO_MODE_MAX;
+
+	cfg->otyper = (mmio_read_32(base + GPIO_TYPE_OFFSET) >> pin) & 1;
+	cfg->ospeedr = (mmio_read_32(base +  GPIO_SPEED_OFFSET) >> (pin << 1)) &
+			GPIO_SPEED_MAX;
+	cfg->pupdr = (mmio_read_32(base +  GPIO_PUPD_OFFSET) >> (pin << 1)) &
+			GPIO_PULL_MAX;
+
+	cfg->odr = (mmio_read_32(base + GPIO_ODR_OFFSET) >> (pin << 1)) & 1;
+
+	if (pin < GPIO_ALT_LOWER_LIMIT) {
+		cfg->afr = (mmio_read_32(base + GPIO_AFRL_OFFSET) >>
+				(pin << 2)) & GPIO_ALTERNATE_MAX;
+	} else {
+		cfg->afr = (mmio_read_32(base + GPIO_AFRH_OFFSET) >>
+				((pin - GPIO_ALT_LOWER_LIMIT) << 2)) &
+				GPIO_ALTERNATE_MAX;
+	}
+
+	stm32_clock_disable((unsigned long)clock);
+}
+
+static void set_gpio_cfg(uint32_t bank, uint32_t pin, struct gpio_cfg *cfg)
+{
+	uintptr_t base = stm32_get_gpio_bank_base(bank);
+	int clock = stm32_get_gpio_bank_clock(bank);
+
+	stm32_clock_enable((unsigned long)clock);
+
+	mmio_clrsetbits_32(base + GPIO_MODE_OFFSET,
+			   GPIO_MODE_MAX << (pin << 1),
+			   cfg->moder << (pin << 1));
+
+	mmio_clrsetbits_32(base + GPIO_TYPE_OFFSET,
+			   BIT(pin),
+			   cfg->otyper << pin);
+
+	mmio_clrsetbits_32(base + GPIO_SPEED_OFFSET,
+			   GPIO_SPEED_MAX << (pin << 1),
+			   cfg->ospeedr << (pin << 1));
+
+	mmio_clrsetbits_32(base + GPIO_PUPD_OFFSET,
+			   BIT(pin),
+			   cfg->pupdr << (pin << 1));
+
+	if (pin < GPIO_ALT_LOWER_LIMIT) {
+		mmio_clrsetbits_32(base + GPIO_AFRL_OFFSET,
+				   GPIO_ALTERNATE_MAX << (pin << 2),
+				   cfg->afr << (pin << 2));
+	} else {
+		size_t shift = (pin - GPIO_ALT_LOWER_LIMIT) << 2;
+
+		mmio_clrsetbits_32(base + GPIO_AFRH_OFFSET,
+				   GPIO_ALTERNATE_MAX << shift,
+				   cfg->afr << shift);
+	}
+
+	mmio_clrsetbits_32(base + GPIO_ODR_OFFSET,
+			   BIT(pin), cfg->odr << pin);
+
+	stm32_clock_disable((unsigned long)clock);
+}
+
+/*
+ * stm32_gpio_set_output_level - Set level of an output GPIO instance
+ *
+ * @bank: GPIO bank
+ * @pin: GPIO pin position in bank
+ * @level: target level, either 0 (level low) or non zero (level high)
+ */
+void stm32_gpio_set_output_level(uint32_t bank, uint32_t pin, int level)
+{
+	uintptr_t base = stm32_get_gpio_bank_base(bank);
+	int clock = stm32_get_gpio_bank_clock(bank);
+
+	assert(pin <= GPIO_PIN_MAX);
+
+	stm32_clock_enable((unsigned long)clock);
+
+	if (level) {
+		write32(BIT(pin), base + GPIO_BSRR_OFFSET);
+	} else {
+		write32(BIT(pin + 16), base + GPIO_BSRR_OFFSET);
+	}
+
+	stm32_clock_disable((unsigned long)clock);
+}
+
+/*
+ * stm32_pinctrl_load_active_cfg - Load the PINCTRLs active configuration
+ *
+ * @pinctrl: array of PINCTRL configurations to apply
+ * @cnt: number of elements in array pinctrl
+ */
+void stm32_pinctrl_load_active_cfg(struct stm32_pinctrl *pinctrl, size_t cnt)
+{
+	size_t n;
+
+	for (n = 0; n < cnt; n++) {
+		set_gpio_cfg(pinctrl[n].bank, pinctrl[n].pin,
+			     &pinctrl[n].active_cfg);
+	}
+}
+
+/*
+ * stm32_pinctrl_load_standby_cfg - Load the PINCTRLs standby configuration
+ *
+ * @pinctrl: array of PINCTRL configurations to apply
+ * @cnt: number of elements in array pinctrl
+ */
+void stm32_pinctrl_load_standby_cfg(struct stm32_pinctrl *pinctrl, size_t cnt)
+{
+	size_t n;
+
+	for (n = 0; n < cnt; n++) {
+		set_gpio_cfg(pinctrl[n].bank, pinctrl[n].pin,
+			     &pinctrl[n].standby_cfg);
+	}
+}
+
+/*
+ * stm32_pinctrl_store_standby_cfg - Save current PINCTRLs config as standby
+ *
+ * @pinctrl: array of PINCTRL configurations to store
+ * @cnt: number of elements in array pinctrl
+ */
+void stm32_pinctrl_store_standby_cfg(struct stm32_pinctrl *pinctrl, size_t cnt)
+{
+	size_t n;
+
+	for (n = 0; n < cnt; n++) {
+		get_gpio_cfg(pinctrl[n].bank, pinctrl[n].pin,
+			     &pinctrl[n].standby_cfg);
+	}
+}
+
+#ifdef CFG_DT
+/* Return GPIO bank node if status is okay in DT, else return 0 */
+static int ckeck_gpio_bank(void *fdt, uint32_t bank, int pinctrl_node)
+{
+	int pinctrl_subnode;
+
+	fdt_for_each_subnode(pinctrl_subnode, fdt, pinctrl_node) {
+		uint32_t bank_offset;
+		const fdt32_t *cuint;
+
+		if (fdt_getprop(fdt, pinctrl_subnode,
+				"gpio-controller", NULL) == NULL) {
+			continue;
+		}
+
+		cuint = fdt_getprop(fdt, pinctrl_subnode, "reg", NULL);
+		if (cuint == NULL) {
+			continue;
+		}
+
+		bank_offset = stm32_get_gpio_bank_offset(bank);
+
+		if ((fdt32_to_cpu(*cuint) == bank_offset) &&
+		    (_fdt_get_status(fdt,pinctrl_subnode) !=
+		     DT_STATUS_DISABLED)) {
+			return pinctrl_subnode;
+		}
+	}
+
+	return 0;
+}
+
+static int get_pinctrl_from_fdt(void *fdt, int node,
+				struct stm32_pinctrl *pinctrl, size_t count)
+{
+	const fdt32_t *cuint, *slewrate;
+	int len;
+	int pinctrl_node;
+	int bank_node;
+	int clk;
+	uint32_t i;
+	uint32_t speed = GPIO_SPEED_LOW;
+	uint32_t pull = GPIO_NO_PULL;
+	size_t found = 0;
+
+	cuint = fdt_getprop(fdt, node, "pinmux", &len);
+	if (cuint == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	pinctrl_node = fdt_parent_offset(fdt, fdt_parent_offset(fdt, node));
+	if (pinctrl_node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	slewrate = fdt_getprop(fdt, node, "slew-rate", NULL);
+	if (slewrate != NULL) {
+		speed = fdt32_to_cpu(*slewrate);
+	}
+
+	if (fdt_getprop(fdt, node, "bias-pull-up", NULL) != NULL) {
+		pull = GPIO_PULL_UP;
+	} else if (fdt_getprop(fdt, node, "bias-pull-down", NULL) != NULL) {
+		pull = GPIO_PULL_DOWN;
+	} else {
+		DMSG("No bias configured in node %d\n", node);
+	}
+
+	for (i = 0; i < ((uint32_t)len / sizeof(uint32_t)); i++) {
+		uint32_t pincfg;
+		uint32_t bank;
+		uint32_t pin;
+		uint32_t mode;
+		uint32_t alternate = GPIO_ALTERNATE_0;
+
+		pincfg = fdt32_to_cpu(*cuint);
+		cuint++;
+
+		bank = (pincfg & DT_GPIO_BANK_MASK) >> DT_GPIO_BANK_SHIFT;
+
+		pin = (pincfg & DT_GPIO_PIN_MASK) >> DT_GPIO_PIN_SHIFT;
+
+		mode = pincfg & DT_GPIO_MODE_MASK;
+
+		switch (mode) {
+		case 0:
+			mode = GPIO_MODE_INPUT;
+			break;
+		case 1:
+		case 2:
+		case 3:
+		case 4:
+		case 5:
+		case 6:
+		case 7:
+		case 8:
+		case 9:
+		case 10:
+		case 11:
+		case 12:
+		case 13:
+		case 14:
+		case 15:
+		case 16:
+			alternate = mode - 1U;
+			mode = GPIO_MODE_ALTERNATE;
+			break;
+		case 17:
+			mode = GPIO_MODE_ANALOG;
+			break;
+		default:
+			mode = GPIO_MODE_OUTPUT;
+			break;
+		}
+
+		if (fdt_getprop(fdt, node, "drive-open-drain", NULL) != NULL) {
+			mode |= GPIO_OPEN_DRAIN;
+		}
+
+		bank_node = ckeck_gpio_bank(fdt, bank, pinctrl_node);
+		if (bank_node == 0) {
+			panic("PINCTRL inconsistent in DT");
+		}
+
+		clk = fdt_get_clock_id(fdt, bank_node);
+		if (clk < 0) {
+			return -FDT_ERR_NOTFOUND;
+		}
+
+		/* Platform knows the clock: assert it is okay */
+		assert(clk == stm32_get_gpio_bank_clock(bank));
+		stm32_clock_enable((unsigned int)clk);
+		stm32_clock_disable((unsigned int)clk);
+
+		if (found < count) {
+			struct stm32_pinctrl *cfg = &pinctrl[found];
+
+			cfg->bank = (uint8_t)bank;
+			cfg->pin = (uint8_t)pin;
+			cfg->active_cfg.moder = mode & ~GPIO_OPEN_DRAIN;
+			cfg->active_cfg.otyper = mode & GPIO_OPEN_DRAIN ? 1 : 0;
+			cfg->active_cfg.ospeedr = speed;
+			cfg->active_cfg.pupdr = pull;
+			cfg->active_cfg.odr = 0;
+			cfg->active_cfg.afr = alternate;
+			cfg->standby_cfg.moder = GPIO_MODE_ANALOG;
+			cfg->standby_cfg.pupdr = GPIO_NO_PULL;
+		}
+
+		found++;
+	}
+
+	return (int)found;
+}
+
+/*
+ * stm32_pinctrl_fdt_get_pinctrl - get PINCTRL config from a DT device node
+ *
+ * Argument cfg can be set to NULL or count to 0: the function will return only
+ * the number of PINCTRL instances found in the device tree for the target
+ * device.
+ *
+ * If more instances than count are found then the function returns the
+ * effective number of PINCTRL found but will fill array cfg only according
+ * to the value of count.
+ *
+ * @fdt: device tree
+ * @node: device node in the device tree
+ * @cfg: NULL or pointer to array of struct stm32_pinctrl
+ * @count: number of elements pointed by argument cfg
+ *
+ * Return the number of PINCTRL instances found and a negative value on error
+ */
+int stm32_pinctrl_fdt_get_pinctrl(void *fdt, int device_node,
+				  struct stm32_pinctrl *cfg, size_t count)
+{
+	const fdt32_t *cuint;
+	int lenp;
+	uint32_t i;
+	size_t found = 0;
+
+	cuint = fdt_getprop(fdt, device_node, "pinctrl-0", &lenp);
+	if (cuint == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	for (i = 0; i < ((uint32_t)lenp / 4U); i++) {
+		int node;
+		int subnode;
+
+		node = fdt_node_offset_by_phandle(fdt, fdt32_to_cpu(*cuint));
+		if (node < 0) {
+			return -FDT_ERR_NOTFOUND;
+		}
+
+		fdt_for_each_subnode(subnode, fdt, node) {
+			size_t n;
+			int rc;
+
+			if (count > found) {
+				n = count - found;
+			} else {
+				n = 0;
+			}
+
+			rc = get_pinctrl_from_fdt(fdt, subnode, &cfg[found], n);
+			if (rc < 0) {
+				return rc;
+			}
+
+			found += (size_t)rc;
+		}
+
+		cuint++;
+	}
+
+	return (int)found;
+}
+#endif /*CFG_DT*/
+
+/*
+ * stm32_gpio_set_secure_cfg - Set secure field of an output GPIO instance
+ *
+ * @bank: GPIO bank
+ * @pin: GPIO pin position in bank
+ * @secure: Secure field, either false (non secure) or true (secure)
+ */
+void stm32_gpio_set_secure_cfg(uint32_t bank, uint32_t pin, bool secure)
+{
+	uintptr_t base = stm32_get_gpio_bank_base(bank);
+	int clock = stm32_get_gpio_bank_clock(bank);
+
+	assert(pin <= GPIO_PIN_MAX);
+
+	assert(!(secure && stm32mp_gpio_bank_is_non_secure(bank)));
+
+	stm32_clock_enable((unsigned long)clock);
+
+	if (secure) {
+		mmio_setbits_32(base + GPIO_SECR_OFFSET, BIT(pin));
+	} else {
+		mmio_clrbits_32(base + GPIO_SECR_OFFSET, BIT(pin));
+	}
+
+	stm32_clock_disable((unsigned long)clock);
+}
diff --git a/core/drivers/stm32_i2c.c b/core/drivers/stm32_i2c.c
new file mode 100644
index 0000000..f7f0f70
--- /dev/null
+++ b/core/drivers/stm32_i2c.c
@@ -0,0 +1,1629 @@
+// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
+/*
+ * Copyright (c) 2016-2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <arm32.h>
+#include <drivers/stm32_i2c.h>
+#include <drivers/stm32_gpio.h>
+#include <io.h>
+#include <kernel/delay.h>
+#include <kernel/dt.h>
+#include <kernel/panic.h>
+#include <libfdt.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stm32_util.h>
+
+/* STM32 I2C registers offsets */
+#define I2C_CR1			0x00U
+#define I2C_CR2			0x04U
+#define I2C_OAR1		0x08U
+#define I2C_OAR2		0x0CU
+#define I2C_TIMINGR		0x10U
+#define I2C_TIMEOUTR		0x14U
+#define I2C_ISR			0x18U
+#define I2C_ICR			0x1CU
+#define I2C_PECR		0x20U
+#define I2C_RXDR		0x24U
+#define I2C_TXDR		0x28U
+
+#define TIMINGR_CLEAR_MASK	0xF0FFFFFFU
+
+#define MAX_NBYTE_SIZE		255U
+
+#define I2C_NSEC_PER_SEC	1000000000L
+
+/*
+ * struct i2c_spec_s - Private I2C timing specifications.
+ * @rate: I2C bus speed (Hz)
+ * @rate_min: 80% of I2C bus speed (Hz)
+ * @rate_max: 120% of I2C bus speed (Hz)
+ * @fall_max: Max fall time of both SDA and SCL signals (ns)
+ * @rise_max: Max rise time of both SDA and SCL signals (ns)
+ * @hddat_min: Min data hold time (ns)
+ * @vddat_max: Max data valid time (ns)
+ * @sudat_min: Min data setup time (ns)
+ * @l_min: Min low period of the SCL clock (ns)
+ * @h_min: Min high period of the SCL clock (ns)
+ */
+struct i2c_spec_s {
+	uint32_t rate;
+	uint32_t rate_min;
+	uint32_t rate_max;
+	uint32_t fall_max;
+	uint32_t rise_max;
+	uint32_t hddat_min;
+	uint32_t vddat_max;
+	uint32_t sudat_min;
+	uint32_t l_min;
+	uint32_t h_min;
+};
+
+/*
+ * struct i2c_timing_s - Private I2C output parameters.
+ * @scldel: Data setup time
+ * @sdadel: Data hold time
+ * @sclh: SCL high period (master mode)
+ * @sclh: SCL low period (master mode)
+ * @is_saved: True if relating to a configuration candidate
+ */
+struct i2c_timing_s {
+	uint8_t scldel;
+	uint8_t sdadel;
+	uint8_t sclh;
+	uint8_t scll;
+	bool is_saved;
+};
+
+/**
+ * All these values are coming from I2C Specification, Version 6.0, 4th of
+ * April 2014.
+ *
+ * Table10. Characteristics of the SDA and SCL bus lines for Standard, Fast,
+ * and Fast-mode Plus I2C-bus devices.
+ */
+static const struct i2c_spec_s i2c_specs[] = {
+	[I2C_SPEED_STANDARD] = {
+		.rate = STANDARD_RATE,
+		.rate_min = 8000,
+		.rate_max = 120000,
+		.fall_max = 300,
+		.rise_max = 1000,
+		.hddat_min = 0,
+		.vddat_max = 3450,
+		.sudat_min = 250,
+		.l_min = 4700,
+		.h_min = 4000,
+	},
+	[I2C_SPEED_FAST] = {
+		.rate = FAST_RATE,
+		.rate_min = 320000,
+		.rate_max = 480000,
+		.fall_max = 300,
+		.rise_max = 300,
+		.hddat_min = 0,
+		.vddat_max = 900,
+		.sudat_min = 100,
+		.l_min = 1300,
+		.h_min = 600,
+	},
+	[I2C_SPEED_FAST_PLUS] = {
+		.rate = FAST_PLUS_RATE,
+		.rate_min = 800000,
+		.rate_max = 1200000,
+		.fall_max = 100,
+		.rise_max = 120,
+		.hddat_min = 0,
+		.vddat_max = 450,
+		.sudat_min = 50,
+		.l_min = 500,
+		.h_min = 260,
+	},
+};
+
+static int i2c_request_memory_write(struct i2c_handle_s *hi2c,
+				    uint16_t dev_addr, uint16_t mem_addr,
+				    uint16_t mem_add_size,
+				    uint64_t tick_to, uint64_t tick_start);
+static int i2c_request_memory_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+				   uint16_t mem_addr, uint16_t mem_add_size,
+				   uint64_t tick_to, uint64_t tick_start);
+
+/* Private functions to handle flags during polling transfer */
+static int i2c_wait_flag(struct i2c_handle_s *hi2c, uint32_t flag,
+			 uint8_t awaited_value,
+			 uint64_t tick_to, uint64_t tick_start);
+static int i2c_wait_txis(struct i2c_handle_s *hi2c,
+			 uint64_t tick_to, uint64_t tick_start);
+static int i2c_wait_stop(struct i2c_handle_s *hi2c,
+			 uint64_t tick_to, uint64_t tick_start);
+static int i2c_ack_failed(struct i2c_handle_s *hi2c,
+			  uint64_t tick_to, uint64_t tick_start);
+
+/* Private function to flush TXDR register */
+static void i2c_flush_txdr(struct i2c_handle_s *hi2c);
+
+/* Private function to start, restart or stop a transfer */
+static void i2c_transfer_config(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+				uint16_t size, uint32_t i2c_mode,
+				uint32_t request);
+
+static uintptr_t get_base(struct i2c_handle_s *hi2c)
+{
+	if (!cpu_mmu_enabled())
+		return hi2c->pbase;
+
+	return hi2c->vbase;
+}
+
+static bool i2c_is_secure(struct i2c_handle_s *hi2c)
+{
+	return hi2c->dt_status == DT_STATUS_OK_SEC;
+}
+
+static uint64_t ms2tick(uint32_t timeout_ms)
+{
+	return ((uint64_t)timeout_ms * read_cntfrq()) / 1000;
+}
+
+static uint64_t timeout_start(void)
+{
+	return read_cntpct();
+}
+
+static bool timeout_elapsed(uint64_t tick_start, uint64_t tick_to)
+{
+	return (tick_to != 0U) && ((read_cntpct() - tick_start) > tick_to);
+}
+
+static void notif_i2c_timeout(struct i2c_handle_s *hi2c)
+{
+	hi2c->i2c_err |= I2C_ERROR_TIMEOUT;
+	hi2c->i2c_mode = I2C_MODE_NONE;
+	hi2c->i2c_state = I2C_STATE_READY;
+}
+
+static void save_cfg(struct i2c_handle_s *hi2c, struct i2c_cfg *cfg)
+{
+	uintptr_t base = get_base(hi2c);
+
+	stm32_clock_enable(hi2c->clock);
+
+	cfg->cr1 = mmio_read_32(base + I2C_CR1);
+	cfg->cr2 = mmio_read_32(base + I2C_CR2);
+	cfg->oar1 = mmio_read_32(base + I2C_OAR1);
+	cfg->oar2 = mmio_read_32(base + I2C_OAR2);
+	cfg->timingr = mmio_read_32(base + I2C_TIMINGR);
+
+	stm32_clock_disable(hi2c->clock);
+}
+
+static void restore_cfg(struct i2c_handle_s *hi2c, struct i2c_cfg *cfg)
+{
+	uintptr_t base = get_base(hi2c);
+
+	if (hi2c->lock) {
+		panic();
+	}
+
+	stm32_clock_enable(hi2c->clock);
+
+	mmio_clrbits_32(base + I2C_CR1, I2C_CR1_PE);
+	mmio_write_32(base + I2C_TIMINGR, cfg->timingr & TIMINGR_CLEAR_MASK);
+	mmio_write_32(base + I2C_OAR1, cfg->oar1);
+	mmio_write_32(base + I2C_CR2, cfg->cr2);
+	mmio_write_32(base + I2C_OAR2, cfg->oar2);
+	mmio_write_32(base + I2C_CR1, cfg->cr1 & ~I2C_CR1_PE);
+	mmio_setbits_32(base + I2C_CR1, cfg->cr1 & I2C_CR1_PE);
+
+	stm32_clock_disable(hi2c->clock);
+}
+
+static void __maybe_unused dump_cfg(struct i2c_cfg *cfg __maybe_unused)
+{
+	DMSG("CR1:  %x", (unsigned)cfg->cr1);
+	DMSG("CR2:  %x", (unsigned)cfg->cr2);
+	DMSG("OAR1: %x", (unsigned)cfg->oar1);
+	DMSG("OAR2: %x", (unsigned)cfg->oar2);
+	DMSG("TIM:  %x", (unsigned)cfg->timingr);
+}
+
+static void __maybe_unused dump_i2c(struct i2c_handle_s *hi2c)
+{
+	uintptr_t __maybe_unused base = get_base(hi2c);
+
+	stm32_clock_enable(hi2c->clock);
+
+	DMSG("CR1:  %x", (unsigned)mmio_read_32(base + I2C_CR1));
+	DMSG("CR2:  %x", (unsigned)mmio_read_32(base + I2C_CR2));
+	DMSG("OAR1: %x", (unsigned)mmio_read_32(base + I2C_OAR1));
+	DMSG("OAR2: %x", (unsigned)mmio_read_32(base + I2C_OAR2));
+	DMSG("TIM:  %x", (unsigned)mmio_read_32(base + I2C_TIMINGR));
+
+	stm32_clock_disable(hi2c->clock);
+}
+
+/*
+ * @brief  Compute the I2C device timings.
+ * @param  init: Ref to the initialization configuration structure
+ * @param  clock_src: I2C clock source frequency (Hz)
+ * @param  timing: Pointer to the final computed timing result
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_compute_timing(struct stm32_i2c_init_s *init,
+			      uint32_t clock_src, uint32_t *timing)
+{
+	enum i2c_speed_e mode = init->speed_mode;
+	uint32_t speed_freq;
+	uint32_t i2cclk = UDIV_ROUND_NEAREST(I2C_NSEC_PER_SEC, clock_src);
+	uint32_t i2cbus;
+	uint32_t p_prev = I2C_TIMINGR_PRESC_MAX;
+	uint32_t af_delay_min;
+	uint32_t af_delay_max;
+	uint32_t dnf_delay;
+	uint32_t tsync;
+	uint32_t clk_min;
+	uint32_t clk_max;
+	int clk_error_prev;
+	uint16_t p;
+	uint16_t l;
+	uint16_t a;
+	uint16_t h;
+	unsigned int sdadel_min;
+	unsigned int sdadel_max;
+	unsigned int scldel_min;
+	unsigned int delay;
+	int s = -1;
+	struct i2c_timing_s solutions[I2C_TIMINGR_PRESC_MAX];
+
+	switch (mode) {
+	case I2C_SPEED_STANDARD:
+	case I2C_SPEED_FAST:
+	case I2C_SPEED_FAST_PLUS:
+		break;
+	default:
+		EMSG("I2C speed out of bound {%d/%d}\n",
+		     mode, I2C_SPEED_FAST_PLUS);
+		return -1;
+	}
+
+	speed_freq = i2c_specs[mode].rate;
+	i2cbus = UDIV_ROUND_NEAREST(I2C_NSEC_PER_SEC, speed_freq);
+	clk_error_prev = INT_MAX;
+
+	if ((init->rise_time > i2c_specs[mode].rise_max) ||
+	    (init->fall_time > i2c_specs[mode].fall_max)) {
+		EMSG(" I2C timings out of bound Rise{%d>%d}/Fall{%d>%d}\n",
+		     init->rise_time, i2c_specs[mode].rise_max,
+		     init->fall_time, i2c_specs[mode].fall_max);
+		return -1;
+	}
+
+	if (init->digital_filter_coef > STM32_I2C_DIGITAL_FILTER_MAX) {
+		EMSG("DNF out of bound %d/%d\n",
+		     init->digital_filter_coef, STM32_I2C_DIGITAL_FILTER_MAX);
+		return -1;
+	}
+
+	/*  Analog and Digital Filters */
+	af_delay_min = (init->analog_filter ?
+			STM32_I2C_ANALOG_FILTER_DELAY_MIN : 0);
+	af_delay_max = (init->analog_filter ?
+			STM32_I2C_ANALOG_FILTER_DELAY_MAX : 0);
+	dnf_delay = init->digital_filter_coef * i2cclk;
+
+	sdadel_min = i2c_specs[mode].hddat_min + init->fall_time;
+	delay = af_delay_min - ((init->digital_filter_coef + 3) * i2cclk);
+	if (SUB_OVERFLOW(sdadel_min, delay ,&sdadel_min))
+		sdadel_min = 0;
+
+	sdadel_max = i2c_specs[mode].vddat_max - init->rise_time;
+	delay = af_delay_max - ((init->digital_filter_coef + 4) * i2cclk);
+	if (SUB_OVERFLOW(sdadel_max, delay ,&sdadel_max))
+		sdadel_max = 0;
+
+	scldel_min = init->rise_time + i2c_specs[mode].sudat_min;
+
+	DMSG("I2C SDADEL(min/max): %u/%u, SCLDEL(Min): %u\n",
+	     sdadel_min, sdadel_max, scldel_min);
+
+	memset(&solutions, 0, sizeof(solutions));
+
+	/* Compute possible values for PRESC, SCLDEL and SDADEL */
+	for (p = 0; p < I2C_TIMINGR_PRESC_MAX; p++) {
+		for (l = 0; l < I2C_TIMINGR_SCLDEL_MAX; l++) {
+			uint32_t scldel = (l + 1) * (p + 1) * i2cclk;
+
+			if (scldel < scldel_min) {
+				continue;
+			}
+
+			for (a = 0; a < I2C_TIMINGR_SDADEL_MAX; a++) {
+				uint32_t sdadel = (a * (p + 1) + 1) * i2cclk;
+
+				if ((sdadel >= sdadel_min) &&
+				    (sdadel <= sdadel_max) &&
+				    (p != p_prev)) {
+					solutions[p].scldel = l;
+					solutions[p].sdadel = a;
+					solutions[p].is_saved = true;
+					p_prev = p;
+					break;
+				}
+			}
+
+			if (p_prev == p) {
+				break;
+			}
+		}
+	}
+
+	if (p_prev == I2C_TIMINGR_PRESC_MAX) {
+		EMSG(" I2C no Prescaler solution\n");
+		return -1;
+	}
+
+	tsync = af_delay_min + dnf_delay + (2 * i2cclk);
+	clk_max = I2C_NSEC_PER_SEC / i2c_specs[mode].rate_min;
+	clk_min = I2C_NSEC_PER_SEC / i2c_specs[mode].rate_max;
+
+	/*
+	 * Among prescaler possibilities discovered above figures out SCL Low
+	 * and High Period. Provided:
+	 * - SCL Low Period has to be higher than Low Period of the SCL Clock
+	 *   defined by I2C Specification. I2C Clock has to be lower than
+	 *   (SCL Low Period - Analog/Digital filters) / 4.
+	 * - SCL High Period has to be lower than High Period of the SCL Clock
+	 *   defined by I2C Specification.
+	 * - I2C Clock has to be lower than SCL High Period.
+	 */
+	for (p = 0; p < I2C_TIMINGR_PRESC_MAX; p++) {
+		uint32_t prescaler = (p + 1) * i2cclk;
+
+		if (!solutions[p].is_saved) {
+			continue;
+		}
+
+		for (l = 0; l < I2C_TIMINGR_SCLL_MAX; l++) {
+			uint32_t tscl_l = ((l + 1) * prescaler) + tsync;
+
+			if ((tscl_l < i2c_specs[mode].l_min) ||
+			    (i2cclk >=
+			     ((tscl_l - af_delay_min - dnf_delay) / 4))) {
+				continue;
+			}
+
+			for (h = 0; h < I2C_TIMINGR_SCLH_MAX; h++) {
+				uint32_t tscl_h = ((h + 1) * prescaler) + tsync;
+				uint32_t tscl = tscl_l + tscl_h +
+						init->rise_time +
+						init->fall_time;
+
+				if ((tscl >= clk_min) && (tscl <= clk_max) &&
+				    (tscl_h >= i2c_specs[mode].h_min) &&
+				    (i2cclk < tscl_h)) {
+					int clk_error = tscl - i2cbus;
+
+					if (clk_error < 0) {
+						clk_error = -clk_error;
+					}
+
+					if (clk_error < clk_error_prev) {
+						clk_error_prev = clk_error;
+						solutions[p].scll = l;
+						solutions[p].sclh = h;
+						s = p;
+					}
+				}
+			}
+		}
+	}
+
+	if (s < 0) {
+		EMSG(" I2C no solution at all\n");
+		return -1;
+	}
+
+	/* Finalize timing settings */
+	*timing = I2C_SET_TIMINGR_PRESC(s) |
+		   I2C_SET_TIMINGR_SCLDEL(solutions[s].scldel) |
+		   I2C_SET_TIMINGR_SDADEL(solutions[s].sdadel) |
+		   I2C_SET_TIMINGR_SCLH(solutions[s].sclh) |
+		   I2C_SET_TIMINGR_SCLL(solutions[s].scll);
+
+	DMSG("I2C TIMINGR (PRESC/SCLDEL/SDADEL): %i/%i/%i\n",
+		s, solutions[s].scldel, solutions[s].sdadel);
+	DMSG("I2C TIMINGR (SCLH/SCLL): %i/%i\n",
+		solutions[s].sclh, solutions[s].scll);
+	DMSG("I2C TIMINGR: 0x%x\n", *timing);
+
+	return 0;
+}
+
+/*
+ * @brief  Setup the I2C device timings.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  init: Ref to the initialization configuration structure
+ * @param  timing: Pointer to the final computed timing result
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_setup_timing(struct i2c_handle_s *hi2c,
+			    struct stm32_i2c_init_s *init,
+			    uint32_t *timing)
+{
+	int rc = 0;
+	uint32_t clock_src;
+
+	clock_src = stm32_clock_get_rate(hi2c->clock);
+	if (clock_src == 0U) {
+		EMSG("I2C clock rate is 0\n");
+		return -1;
+	}
+
+	do {
+		rc = i2c_compute_timing(init, clock_src, timing);
+		if (rc != 0) {
+			EMSG("Failed to compute I2C timings\n");
+			if (init->speed_mode > I2C_SPEED_STANDARD) {
+				init->speed_mode--;
+				IMSG("Downgrade I2C speed to %uHz)\n",
+				     i2c_specs[init->speed_mode].rate);
+			} else {
+				break;
+			}
+		}
+	} while (rc != 0);
+
+	if (rc != 0) {
+		EMSG("Impossible to compute I2C timings\n");
+		return rc;
+	}
+
+	DMSG("I2C Speed Mode(%i), Freq(%i), Clk Source(%i)\n",
+	     init->speed_mode, i2c_specs[init->speed_mode].rate, clock_src);
+	DMSG("I2C Rise(%i) and Fall(%i) Time\n",
+	     init->rise_time, init->fall_time);
+	DMSG("I2C Analog Filter(%s), DNF(%i)\n",
+	     (init->analog_filter ? "On" : "Off"), init->digital_filter_coef);
+
+	return 0;
+}
+
+/*
+ * @brief  Configure I2C Analog noise filter.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C peripheral.
+ * @param  analog_filter: New state of the Analog filter
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_config_analog_filter(struct i2c_handle_s *hi2c,
+				    uint32_t analog_filter)
+{
+	uintptr_t base = get_base(hi2c);
+
+	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+		return -1;
+	}
+
+	hi2c->lock = 1;
+
+	hi2c->i2c_state = I2C_STATE_BUSY;
+
+	/* Disable the selected I2C peripheral */
+	mmio_clrbits_32(base + I2C_CR1, I2C_CR1_PE);
+
+	/* Reset I2Cx ANOFF bit */
+	mmio_clrbits_32(base + I2C_CR1, I2C_CR1_ANFOFF);
+
+	/* Set analog filter bit*/
+	mmio_setbits_32(base + I2C_CR1, analog_filter);
+
+	/* Enable the selected I2C peripheral */
+	mmio_setbits_32(base + I2C_CR1, I2C_CR1_PE);
+
+	hi2c->i2c_state = I2C_STATE_READY;
+
+	hi2c->lock = 0;
+
+	return 0;
+}
+
+/*
+ * @brief  Get I2C setup information from the device tree and set pinctrl
+ *         configuration.
+ * @param  fdt: Pointer to the device tree
+ * @param  node: I2C node offset
+ * @param  init: Ref to the initialization configuration structure
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_get_setup_from_fdt(void *fdt, int node,
+				 struct stm32_i2c_init_s *init,
+				 struct stm32_pinctrl **pinctrl,
+				 size_t *pinctrl_count)
+{
+	const fdt32_t *cuint;
+	int count;
+
+	cuint = fdt_getprop(fdt, node, "i2c-scl-rising-time-ns", NULL);
+	if (cuint == NULL) {
+		init->rise_time = STM32_I2C_RISE_TIME_DEFAULT;
+	} else {
+		init->rise_time = fdt32_to_cpu(*cuint);
+	}
+
+	cuint = fdt_getprop(fdt, node, "i2c-scl-falling-time-ns", NULL);
+	if (cuint == NULL) {
+		init->fall_time = STM32_I2C_FALL_TIME_DEFAULT;
+	} else {
+		init->fall_time = fdt32_to_cpu(*cuint);
+	}
+
+	cuint = fdt_getprop(fdt, node, "clock-frequency", NULL);
+	if (cuint == NULL) {
+		init->speed_mode = STM32_I2C_SPEED_DEFAULT;
+	} else {
+		switch (fdt32_to_cpu(*cuint)) {
+		case STANDARD_RATE:
+			init->speed_mode = I2C_SPEED_STANDARD;
+			break;
+		case FAST_RATE:
+			init->speed_mode = I2C_SPEED_FAST;
+			break;
+		case FAST_PLUS_RATE:
+			init->speed_mode = I2C_SPEED_FAST_PLUS;
+			break;
+		default:
+			init->speed_mode = STM32_I2C_SPEED_DEFAULT;
+			break;
+		}
+	}
+
+	count = stm32_pinctrl_fdt_get_pinctrl(fdt, node, NULL, 0);
+	if (count <= 0) {
+		*pinctrl = NULL;
+		*pinctrl_count = 0;
+		return count;
+	}
+
+	if (count > 2) {
+		panic("Too many PINCTRLs found");
+	}
+
+	*pinctrl = calloc(count, sizeof(**pinctrl));
+	if (!*pinctrl) {
+		panic();
+	}
+
+	*pinctrl_count = stm32_pinctrl_fdt_get_pinctrl(fdt, node,
+						       *pinctrl, count);
+	assert(*pinctrl_count == (unsigned int)count);
+
+	return 0;
+}
+
+/*
+ * @brief  Initialize the I2C device.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  init_data: Ref to the initialization configuration structure
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_init(struct i2c_handle_s *hi2c,
+		   struct stm32_i2c_init_s *init_data)
+{
+	int rc = 0;
+	uint32_t timing;
+	uintptr_t base = get_base(hi2c);
+
+	if (hi2c == NULL) {
+		return -1;
+	}
+
+	if (hi2c->i2c_state == I2C_STATE_RESET) {
+		hi2c->lock = 0;
+	}
+
+	hi2c->i2c_state = I2C_STATE_BUSY;
+
+	rc = i2c_setup_timing(hi2c, init_data, &timing);
+	if (rc != 0) {
+		return rc;
+	}
+
+	stm32_clock_enable(hi2c->clock);
+
+	/* Disable the selected I2C peripheral */
+	mmio_clrbits_32(base + I2C_CR1, I2C_CR1_PE);
+
+	/* Configure I2Cx: Frequency range */
+	mmio_write_32(base + I2C_TIMINGR, timing & TIMINGR_CLEAR_MASK);
+
+	/* Disable Own Address1 before set the Own Address1 configuration */
+	mmio_clrbits_32(base + I2C_OAR1, I2C_OAR1_OA1EN);
+
+	/* Configure I2Cx: Own Address1 and ack own address1 mode */
+	if (init_data->addressing_mode == I2C_ADDRESSINGMODE_7BIT) {
+		mmio_write_32(base + I2C_OAR1,
+			      I2C_OAR1_OA1EN | init_data->own_address1);
+	} else { /* I2C_ADDRESSINGMODE_10BIT */
+		mmio_write_32(base + I2C_OAR1,
+			      I2C_OAR1_OA1EN | I2C_OAR1_OA1MODE |
+			      init_data->own_address1);
+	}
+
+	mmio_write_32(base + I2C_CR2, 0);
+
+	/* Configure I2Cx: Addressing Master mode */
+	if (init_data->addressing_mode == I2C_ADDRESSINGMODE_10BIT) {
+		mmio_setbits_32(base + I2C_CR2, I2C_CR2_ADD10);
+	}
+
+	/*
+	 * Enable the AUTOEND by default, and enable NACK
+	 * (should be disabled only during Slave process).
+	 */
+	mmio_setbits_32(base + I2C_CR2, I2C_CR2_AUTOEND | I2C_CR2_NACK);
+
+	/* Disable Own Address2 before set the Own Address2 configuration */
+	mmio_clrbits_32(base + I2C_OAR2, I2C_DUALADDRESS_ENABLE);
+
+	/* Configure I2Cx: Dual mode and Own Address2 */
+	mmio_write_32(base + I2C_OAR2,
+		      init_data->dual_address_mode |
+		      init_data->own_address2 |
+		      (init_data->own_address2_masks << 8));
+
+	/* Configure I2Cx: Generalcall and NoStretch mode */
+	mmio_write_32(base + I2C_CR1,
+		      init_data->general_call_mode |
+		      init_data->no_stretch_mode);
+
+	/* Enable the selected I2C peripheral */
+	mmio_setbits_32(base + I2C_CR1, I2C_CR1_PE);
+
+	hi2c->i2c_err = I2C_ERROR_NONE;
+	hi2c->i2c_state = I2C_STATE_READY;
+	hi2c->i2c_mode = I2C_MODE_NONE;
+
+	rc = i2c_config_analog_filter(hi2c, init_data->analog_filter ?
+						I2C_ANALOGFILTER_ENABLE :
+						I2C_ANALOGFILTER_DISABLE);
+	if (rc != 0) {
+		EMSG("Cannot initialize I2C analog filter (%d)\n", rc);
+		stm32_clock_disable(hi2c->clock);
+		return rc;
+	}
+
+	stm32_clock_disable(hi2c->clock);
+
+	return rc;
+}
+
+/*
+ * @brief  Generic function to write an amount of data in blocking mode
+ *         (for Memory Mode and Master Mode)
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  mem_addr: Internal memory address (if Memory Mode)
+ * @param  mem_add_size: Size of internal memory address (if Memory Mode)
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @param  mode: Communication mode
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_write(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+		     uint16_t mem_addr, uint16_t mem_add_size,
+		     uint8_t *p_data, uint16_t size, uint32_t timeout_ms,
+		     enum i2c_mode_e mode)
+{
+	uint64_t tick_start;
+	uintptr_t base = get_base(hi2c);
+	int rc = -1;
+	uint64_t tick_to = ms2tick(timeout_ms);
+	uint8_t *p_buff = p_data;
+	size_t xfer_size;
+	size_t xfer_count = size;
+
+	if ((mode != I2C_MODE_MASTER) && (mode != I2C_MODE_MEM)) {
+		return -1;
+	}
+
+	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+		return -1;
+	}
+
+	if ((p_data == NULL) || (size == 0U)) {
+		return -1;
+	}
+
+	stm32_clock_enable(hi2c->clock);
+
+	hi2c->lock = 1;
+
+	tick_start = timeout_start();
+
+	if (i2c_wait_flag(hi2c, I2C_FLAG_BUSY, 1, ms2tick(I2C_TIMEOUT_BUSY_MS),
+			  tick_start) != 0) {
+		goto bail;
+	}
+
+	hi2c->i2c_state = I2C_STATE_BUSY_TX;
+	hi2c->i2c_mode = mode;
+	hi2c->i2c_err = I2C_ERROR_NONE;
+
+	if (mode == I2C_MODE_MEM) {
+		/* In Memory Mode, Send Slave Address and Memory Address */
+		if (i2c_request_memory_write(hi2c, dev_addr,
+					     mem_addr, mem_add_size,
+					     tick_to, tick_start) != 0) {
+			goto bail;
+		}
+
+		if (xfer_count > MAX_NBYTE_SIZE) {
+			xfer_size = MAX_NBYTE_SIZE;
+			i2c_transfer_config(hi2c, dev_addr, xfer_size,
+					    I2C_RELOAD_MODE, I2C_NO_STARTSTOP);
+		} else {
+			xfer_size = xfer_count;
+			i2c_transfer_config(hi2c, dev_addr, xfer_size,
+					    I2C_AUTOEND_MODE, I2C_NO_STARTSTOP);
+		}
+	} else {
+		/* In Master Mode, Send Slave Address */
+		if (xfer_count > MAX_NBYTE_SIZE) {
+			xfer_size = MAX_NBYTE_SIZE;
+			i2c_transfer_config(hi2c, dev_addr, xfer_size,
+					    I2C_RELOAD_MODE,
+					    I2C_GENERATE_START_WRITE);
+		} else {
+			xfer_size = xfer_count;
+			i2c_transfer_config(hi2c, dev_addr, xfer_size,
+					    I2C_AUTOEND_MODE,
+					    I2C_GENERATE_START_WRITE);
+		}
+	}
+
+	do {
+		if (i2c_wait_txis(hi2c, tick_to, tick_start) != 0) {
+			goto bail;
+		}
+
+		mmio_write_8(base + I2C_TXDR, *p_buff);
+		p_buff++;
+		xfer_count--;
+		xfer_size--;
+
+		if ((xfer_count != 0U) && (xfer_size == 0U)) {
+			/* Wait until TCR flag is set */
+			if (i2c_wait_flag(hi2c, I2C_FLAG_TCR, 0, tick_to,
+					  tick_start) != 0) {
+				goto bail;
+			}
+
+			if (xfer_count > MAX_NBYTE_SIZE) {
+				xfer_size = MAX_NBYTE_SIZE;
+				i2c_transfer_config(hi2c, dev_addr,
+						    xfer_size,
+						    I2C_RELOAD_MODE,
+						    I2C_NO_STARTSTOP);
+			} else {
+				xfer_size = xfer_count;
+				i2c_transfer_config(hi2c, dev_addr,
+						    xfer_size,
+						    I2C_AUTOEND_MODE,
+						    I2C_NO_STARTSTOP);
+			}
+		}
+
+	} while (xfer_count > 0U);
+
+	/*
+	 * No need to Check TC flag, with AUTOEND mode the stop
+	 * is automatically generated.
+	 * Wait until STOPF flag is reset.
+	 */
+	if (i2c_wait_stop(hi2c, tick_to, tick_start) != 0) {
+		goto bail;
+	}
+
+	mmio_write_32(base + I2C_ICR, I2C_FLAG_STOPF);
+
+	mmio_clrbits_32(base + I2C_CR2, I2C_RESET_CR2);
+
+	hi2c->i2c_state = I2C_STATE_READY;
+	hi2c->i2c_mode = I2C_MODE_NONE;
+
+	rc = 0;
+
+bail:
+	hi2c->lock = 0;
+	stm32_clock_disable(hi2c->clock);
+
+	return rc;
+}
+
+/*
+ * @brief  Write an amount of data in blocking mode to a specific memory
+ *         address.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  mem_addr: Internal memory address
+ * @param  mem_add_size: Size of internal memory address
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_mem_write(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			uint16_t mem_addr, uint16_t mem_add_size,
+			uint8_t *p_data, uint16_t size, uint32_t timeout_ms)
+{
+	return i2c_write(hi2c, dev_addr, mem_addr, mem_add_size,
+			 p_data, size, timeout_ms, I2C_MODE_MEM);
+}
+
+/*
+ * @brief  Transmits in master mode an amount of data in blocking mode.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_master_transmit(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			      uint8_t *p_data, uint16_t size,
+			      uint32_t timeout_ms)
+{
+	return i2c_write(hi2c, dev_addr, 0, 0,
+			 p_data, size, timeout_ms, I2C_MODE_MASTER);
+}
+
+/*
+ * Optimized 1 byte read/write function for unpaged sequences.
+ * 8-bit addressing mode / single byte transferred / use default I2C timeout.
+ * 'unpg88' refers to 8bit address/8bit value.
+ */
+int stm32_i2c_read_write_membyte(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			      unsigned int mem_addr, uint8_t *p_data,
+			      bool write)
+{
+	uint64_t tick_start;
+	uintptr_t base = get_base(hi2c);
+	int rc = 1;
+	uint64_t tick_to = ms2tick(I2C_TIMEOUT_BUSY_MS);
+	uint8_t *p_buff = p_data;
+
+	if ((hi2c->i2c_state != I2C_STATE_READY) ||
+	    (hi2c->lock != 0U) ||
+	    (p_data == NULL)) {
+		return 1;
+	}
+
+	stm32_clock_enable(hi2c->clock);
+
+	hi2c->lock = 1;
+
+	tick_start = timeout_start();
+	if (i2c_wait_flag(hi2c, I2C_FLAG_BUSY, 1, tick_to, tick_start) != 0) {
+		goto bail;
+	}
+
+	hi2c->i2c_state = write ? I2C_STATE_BUSY_TX : I2C_STATE_BUSY_RX;
+	hi2c->i2c_mode = I2C_MODE_MEM;
+	hi2c->i2c_err = I2C_ERROR_NONE;
+
+	i2c_transfer_config(hi2c, dev_addr, I2C_MEMADD_SIZE_8BIT,
+			    write ? I2C_RELOAD_MODE : I2C_SOFTEND_MODE,
+			    I2C_GENERATE_START_WRITE);
+
+	tick_start = timeout_start();
+	if (i2c_wait_txis(hi2c, tick_to, tick_start) != 0) {
+		goto bail;
+	}
+
+	mmio_write_8(base + I2C_TXDR, (uint8_t)(mem_addr & 0x0FFU));
+
+	tick_start = timeout_start();
+	if (i2c_wait_flag(hi2c, write ? I2C_FLAG_TCR : I2C_FLAG_TC, 0,
+			  tick_to, tick_start) != 0) {
+		goto bail;
+	}
+
+	i2c_transfer_config(hi2c, dev_addr, I2C_MEMADD_SIZE_8BIT,
+			    I2C_AUTOEND_MODE,
+			    write ? I2C_NO_STARTSTOP : I2C_GENERATE_START_READ);
+
+	tick_start = timeout_start();
+	if (write) {
+		if (i2c_wait_txis(hi2c, tick_to, tick_start) != 0) {
+			goto bail;
+		}
+		mmio_write_8(base + I2C_TXDR, *p_buff);
+
+	} else {
+		if (i2c_wait_flag(hi2c, I2C_FLAG_RXNE, 0,
+				  tick_to, tick_start) != 0) {
+			goto bail;
+		}
+		*p_buff = mmio_read_8(base + I2C_RXDR);
+	}
+
+	tick_start = timeout_start();
+	if (i2c_wait_stop(hi2c, tick_to, tick_start) != 0) {
+		goto bail;
+	}
+
+	mmio_write_32(base + I2C_ICR, I2C_FLAG_STOPF);
+	mmio_clrbits_32(base + I2C_CR2, I2C_RESET_CR2);
+
+	hi2c->i2c_state = I2C_STATE_READY;
+	hi2c->i2c_mode = I2C_MODE_NONE;
+
+	rc = 0;
+
+bail:
+	hi2c->lock = 0;
+	stm32_clock_disable(hi2c->clock);
+
+	return rc;
+}
+
+/*
+ * @brief  Generic function to read an amount of data in blocking mode
+ *         (for Memory Mode and Master Mode)
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  mem_addr: Internal memory address (if Memory Mode)
+ * @param  mem_add_size: Size of internal memory address (if Memory Mode)
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @param  mode: Communication mode
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+		    uint16_t mem_addr, uint16_t mem_add_size,
+		    uint8_t *p_data, uint16_t size, uint32_t timeout_ms,
+		    enum i2c_mode_e mode)
+{
+	uintptr_t base = get_base(hi2c);
+	uint64_t tick_start;
+	int rc = -1;
+	uint64_t tick_to = ms2tick(timeout_ms);
+	uint8_t *p_buff = p_data;
+	size_t xfer_count = size;
+	size_t xfer_size;
+
+	if ((mode != I2C_MODE_MASTER) && (mode != I2C_MODE_MEM)) {
+		return -1;
+	}
+
+	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+		return -1;
+	}
+
+	if ((p_data == NULL) || (size == 0U)) {
+		return  -1;
+	}
+
+	stm32_clock_enable(hi2c->clock);
+
+	hi2c->lock = 1;
+
+	tick_start = timeout_start();
+	if (i2c_wait_flag(hi2c, I2C_FLAG_BUSY, 1, ms2tick(I2C_TIMEOUT_BUSY_MS),
+			  tick_start) != 0) {
+		goto bail;
+	}
+
+	hi2c->i2c_state = I2C_STATE_BUSY_RX;
+	hi2c->i2c_mode = mode;
+	hi2c->i2c_err = I2C_ERROR_NONE;
+
+	if (mode == I2C_MODE_MEM) {
+		/* Send Memory Address */
+		if (i2c_request_memory_read(hi2c, dev_addr,
+					    mem_addr, mem_add_size,
+					    tick_to, tick_start) != 0) {
+			goto bail;
+		}
+	}
+
+	/*
+	 * Send Slave Address.
+	 * Set NBYTES to write and reload if xfer_count > MAX_NBYTE_SIZE
+	 * and generate RESTART.
+	 */
+	if (xfer_count > MAX_NBYTE_SIZE) {
+		xfer_size = MAX_NBYTE_SIZE;
+		i2c_transfer_config(hi2c, dev_addr, xfer_size,
+				    I2C_RELOAD_MODE, I2C_GENERATE_START_READ);
+	} else {
+		xfer_size = xfer_count;
+		i2c_transfer_config(hi2c, dev_addr, xfer_size,
+				    I2C_AUTOEND_MODE, I2C_GENERATE_START_READ);
+	}
+
+	do {
+		if (i2c_wait_flag(hi2c, I2C_FLAG_RXNE, 0, tick_to,
+				  tick_start) != 0) {
+			goto bail;
+		}
+
+		*p_buff = mmio_read_8(base + I2C_RXDR);
+		p_buff++;
+		xfer_size--;
+		xfer_count--;
+
+		if ((xfer_count != 0U) && (xfer_size == 0U)) {
+			if (i2c_wait_flag(hi2c, I2C_FLAG_TCR, 0, tick_to,
+					  tick_start) != 0) {
+				goto bail;
+			}
+
+			if (xfer_count > MAX_NBYTE_SIZE) {
+				xfer_size = MAX_NBYTE_SIZE;
+				i2c_transfer_config(hi2c, dev_addr,
+						    xfer_size,
+						    I2C_RELOAD_MODE,
+						    I2C_NO_STARTSTOP);
+			} else {
+				xfer_size = xfer_count;
+				i2c_transfer_config(hi2c, dev_addr,
+						    xfer_size,
+						    I2C_AUTOEND_MODE,
+						    I2C_NO_STARTSTOP);
+			}
+		}
+	} while (xfer_count > 0U);
+
+	/*
+	 * No need to Check TC flag, with AUTOEND mode the stop
+	 * is automatically generated.
+	 * Wait until STOPF flag is reset.
+	 */
+	if (i2c_wait_stop(hi2c, tick_to, tick_start) != 0) {
+		goto bail;
+	}
+
+	mmio_write_32(base + I2C_ICR, I2C_FLAG_STOPF);
+
+	mmio_clrbits_32(base + I2C_CR2, I2C_RESET_CR2);
+
+	hi2c->i2c_state = I2C_STATE_READY;
+	hi2c->i2c_mode = I2C_MODE_NONE;
+
+	rc = 0;
+
+bail:
+	hi2c->lock = 0;
+	stm32_clock_disable(hi2c->clock);
+
+	return rc;
+}
+
+/*
+ * @brief  Read an amount of data in blocking mode from a specific memory
+ *	   address.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  mem_addr: Internal memory address
+ * @param  mem_add_size: Size of internal memory address
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_mem_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+		       uint16_t mem_addr, uint16_t mem_add_size,
+		       uint8_t *p_data, uint16_t size, uint32_t timeout_ms)
+{
+	return i2c_read(hi2c, dev_addr, mem_addr, mem_add_size,
+			p_data, size, timeout_ms, I2C_MODE_MEM);
+}
+
+/*
+ * @brief  Receives in master mode an amount of data in blocking mode.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  p_data: Pointer to data buffer
+ * @param  size: Amount of data to be sent
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @retval 0 if OK, negative value else
+ */
+int stm32_i2c_master_receive(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			     uint8_t *p_data, uint16_t size,
+			     uint32_t timeout_ms)
+{
+	return i2c_read(hi2c, dev_addr, 0, 0,
+			p_data, size, timeout_ms, I2C_MODE_MASTER);
+}
+
+/*
+ * @brief  Checks if target device is ready for communication.
+ * @note   This function is used with Memory devices
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  trials: Number of trials
+ * @param  timeout_ms: Timeout duration in milliseconds
+ * @retval True if device is ready, false else
+ */
+bool stm32_i2c_is_device_ready(struct i2c_handle_s *hi2c,
+			       uint16_t dev_addr, uint32_t trials,
+			       uint32_t timeout_ms)
+{
+	uintptr_t base = get_base(hi2c);
+	uint32_t i2c_trials = 0U;
+	bool rc = false;
+	uint64_t tick_to = ms2tick(timeout_ms);
+
+	if ((hi2c->i2c_state != I2C_STATE_READY) || (hi2c->lock != 0U)) {
+		return rc;
+	}
+
+	stm32_clock_enable(hi2c->clock);
+
+	hi2c->lock = 1;
+	hi2c->i2c_mode = I2C_MODE_NONE;
+
+	if ((mmio_read_32(base + I2C_ISR) & I2C_FLAG_BUSY) != 0U) {
+		goto bail;
+	}
+
+	hi2c->i2c_state = I2C_STATE_BUSY;
+	hi2c->i2c_err = I2C_ERROR_NONE;
+
+	do {
+		uint64_t tick_start;
+		uint32_t isr;
+
+		/* Generate Start */
+		if ((mmio_read_32(base + I2C_OAR1) & I2C_OAR1_OA1MODE) == 0) {
+			mmio_write_32(base + I2C_CR2,
+				      (((uint32_t)dev_addr & I2C_CR2_SADD) |
+				       I2C_CR2_START | I2C_CR2_AUTOEND) &
+				      ~I2C_CR2_RD_WRN);
+		} else {
+			mmio_write_32(base + I2C_CR2,
+				      (((uint32_t)dev_addr & I2C_CR2_SADD) |
+				       I2C_CR2_START | I2C_CR2_ADD10) &
+				      ~I2C_CR2_RD_WRN);
+		}
+
+		/*
+		 * No need to Check TC flag, with AUTOEND mode the stop
+		 * is automatically generated.
+		 * Wait until STOPF flag is set or a NACK flag is set.
+		 */
+		tick_start = timeout_start();
+		do {
+			if (timeout_elapsed(tick_start, tick_to)) {
+				notif_i2c_timeout(hi2c);
+				goto bail;
+			}
+
+			isr = mmio_read_32(base + I2C_ISR);
+		} while ((isr & (I2C_FLAG_STOPF | I2C_FLAG_AF)) == 0);
+
+		if ((mmio_read_32(base + I2C_ISR) & I2C_FLAG_AF) == 0U) {
+			if (i2c_wait_flag(hi2c, I2C_FLAG_STOPF, 0, tick_to,
+					  tick_start) != 0) {
+				goto bail;
+			}
+
+			mmio_write_32(base + I2C_ICR, I2C_FLAG_STOPF);
+
+			hi2c->i2c_state = I2C_STATE_READY;
+
+			rc = true;
+			goto bail;
+		}
+
+		if (i2c_wait_flag(hi2c, I2C_FLAG_STOPF, 0, tick_to,
+				  tick_start) != 0) {
+			goto bail;
+		}
+
+		mmio_write_32(base + I2C_ICR, I2C_FLAG_AF);
+
+		mmio_write_32(base + I2C_ICR, I2C_FLAG_STOPF);
+
+		if (i2c_trials == trials) {
+			mmio_setbits_32(base + I2C_CR2, I2C_CR2_STOP);
+
+			if (i2c_wait_flag(hi2c, I2C_FLAG_STOPF, 0, tick_to,
+					  tick_start) != 0) {
+				goto bail;
+			}
+
+			mmio_write_32(base + I2C_ICR, I2C_FLAG_STOPF);
+		}
+
+		i2c_trials++;
+	} while (i2c_trials < trials);
+
+	notif_i2c_timeout(hi2c);
+
+bail:
+#ifdef STM32_I2C_MAYBE_NON_SECURE
+	if (rc) {
+		/* Save the active secure configuraton */
+		save_cfg(hi2c, &hi2c->sec_cfg);
+	}
+#endif
+
+	hi2c->lock = 0;
+	stm32_clock_disable(hi2c->clock);
+
+	return rc;
+}
+
+/*
+ * @brief  Master sends target device address followed by internal memory
+ *	   address for write request.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  mem_addr: Internal memory address
+ * @param  mem_add_size: Size of internal memory address
+ * @param  tick_to: Tick timeout duration
+ * @param  tick_start: Tick start value
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_request_memory_write(struct i2c_handle_s *hi2c,
+				    uint16_t dev_addr, uint16_t mem_addr,
+				    uint16_t mem_add_size, uint64_t tick_to,
+				    uint64_t tick_start)
+{
+	uintptr_t base = get_base(hi2c);
+
+	i2c_transfer_config(hi2c, dev_addr, mem_add_size, I2C_RELOAD_MODE,
+			    I2C_GENERATE_START_WRITE);
+
+	if (i2c_wait_txis(hi2c, tick_to, tick_start) != 0) {
+		return -1;
+	}
+
+	if (mem_add_size == I2C_MEMADD_SIZE_8BIT) {
+		/* Send Memory Address */
+		mmio_write_8(base + I2C_TXDR,
+			     (uint8_t)(mem_addr & 0x00FFU));
+	} else {
+		/* Send MSB of Memory Address */
+		mmio_write_8(base + I2C_TXDR,
+			     (uint8_t)((mem_addr & 0xFF00U) >> 8));
+
+		if (i2c_wait_txis(hi2c, tick_to, tick_start) != 0) {
+			return -1;
+		}
+
+		/* Send LSB of Memory Address */
+		mmio_write_8(base + I2C_TXDR,
+			     (uint8_t)(mem_addr & 0x00FFU));
+	}
+
+	if (i2c_wait_flag(hi2c, I2C_FLAG_TCR, 0, tick_to, tick_start) != 0) {
+		return -1;
+	}
+
+	return 0;
+}
+
+/*
+ * @brief  Master sends target device address followed by internal memory
+ *	   address for read request.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  dev_addr: Target device address
+ * @param  mem_addr: Internal memory address
+ * @param  mem_add_size: Size of internal memory address
+ * @param  tick_to: Tick timeout duration
+ * @param  tick_start: Tick start value
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_request_memory_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+				   uint16_t mem_addr, uint16_t mem_add_size,
+				   uint64_t tick_to, uint64_t tick_start)
+{
+	uintptr_t base = get_base(hi2c);
+
+	i2c_transfer_config(hi2c, dev_addr, mem_add_size, I2C_SOFTEND_MODE,
+			    I2C_GENERATE_START_WRITE);
+
+	if (i2c_wait_txis(hi2c, tick_to, tick_start) != 0) {
+		return -1;
+	}
+
+	if (mem_add_size == I2C_MEMADD_SIZE_8BIT) {
+		/* Send Memory Address */
+		mmio_write_8(base + I2C_TXDR,
+			     (uint8_t)(mem_addr & 0x00FFU));
+	} else {
+		/* Send MSB of Memory Address */
+		mmio_write_8(base + I2C_TXDR,
+			     (uint8_t)((mem_addr & 0xFF00U) >> 8));
+
+		if (i2c_wait_txis(hi2c, tick_to, tick_start) != 0) {
+			return -1;
+		}
+
+		/* Send LSB of Memory Address */
+		mmio_write_8(base + I2C_TXDR,
+			     (uint8_t)(mem_addr & 0x00FFU));
+	}
+
+	if (i2c_wait_flag(hi2c, I2C_FLAG_TC, 0, tick_to, tick_start) != 0) {
+		return -1;
+	}
+
+	return 0;
+}
+
+/*
+ * @brief  I2C Tx data register flush process.
+ * @param  hi2c: I2C handle
+ * @retval None
+ */
+static void i2c_flush_txdr(struct i2c_handle_s *hi2c)
+{
+	uintptr_t base = get_base(hi2c);
+
+	/*
+	 * If a pending TXIS flag is set,
+	 * write a dummy data in TXDR to clear it.
+	 */
+	if ((mmio_read_32(base + I2C_ISR) & I2C_FLAG_TXIS) != 0U) {
+		mmio_write_32(base + I2C_TXDR, 0);
+	}
+
+	/* Flush TX register if not empty */
+	if ((mmio_read_32(base + I2C_ISR) & I2C_FLAG_TXE) == 0U) {
+		mmio_setbits_32(base + I2C_ISR, I2C_FLAG_TXE);
+	}
+}
+
+/*
+ * @brief  This function handles I2C Communication timeout.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  flag: Specifies the I2C flag to check
+ * @param  awaited_value: The awaited bit value for the flag (0 or 1)
+ * @param  tick_to: Tick timeout duration
+ * @param  tick_start: Tick start value
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_wait_flag(struct i2c_handle_s *hi2c, uint32_t flag,
+			 uint8_t awaited_value, uint64_t tick_to,
+			 uint64_t tick_start)
+{
+	for ( ; ; ) {
+		uint32_t isr = read32(get_base(hi2c) + I2C_ISR);
+
+		if (!!(isr & flag) != !!awaited_value) {
+			return 0;
+		}
+
+		if (timeout_elapsed(tick_start, tick_to)) {
+			notif_i2c_timeout(hi2c);
+			hi2c->lock = 0;
+
+			return -1;
+		}
+	}
+}
+
+/*
+ * @brief  This function handles I2C Communication timeout for specific usage
+ *	   of TXIS flag.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  tick_to: Tick timeout duration
+ * @param  tick_start: Tick start value
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_wait_txis(struct i2c_handle_s *hi2c, uint64_t tick_to,
+			 uint64_t tick_start)
+{
+	while ((read32(get_base(hi2c) + I2C_ISR) & I2C_FLAG_TXIS) == 0U) {
+		if (i2c_ack_failed(hi2c, tick_to, tick_start) != 0) {
+			return -1;
+		}
+
+		if (timeout_elapsed(tick_start, tick_to)) {
+			notif_i2c_timeout(hi2c);
+			hi2c->lock = 0;
+
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * @brief  This function handles I2C Communication timeout for specific
+ *	   usage of STOP flag.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  tick_to: Tick timeout duration
+ * @param  tick_start: Tick start value
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_wait_stop(struct i2c_handle_s *hi2c, uint64_t tick_to,
+			 uint64_t tick_start)
+{
+	while ((read32(get_base(hi2c) + I2C_ISR) & I2C_FLAG_STOPF) == 0U) {
+		if (i2c_ack_failed(hi2c, tick_to, tick_start) != 0) {
+			return -1;
+		}
+
+		if (timeout_elapsed(tick_start, tick_to)) {
+			notif_i2c_timeout(hi2c);
+			hi2c->lock = 0;
+
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * @brief  This function handles Acknowledge failed detection during
+ *	   an I2C Communication.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  tick_to: Tick timeout duration
+ * @param  tick_start: Tick start value
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_ack_failed(struct i2c_handle_s *hi2c, uint64_t tick_to,
+			  uint64_t tick_start)
+{
+	uintptr_t base = get_base(hi2c);
+
+	if ((mmio_read_32(base + I2C_ISR) & I2C_FLAG_AF) == 0U) {
+		return 0;
+	}
+
+	/*
+	 * Wait until STOP Flag is reset.
+	 * AutoEnd should be initiate after AF.
+	 */
+	while ((mmio_read_32(base + I2C_ISR) & I2C_FLAG_STOPF) == 0U) {
+		if (timeout_elapsed(tick_start, tick_to)) {
+			notif_i2c_timeout(hi2c);
+			hi2c->lock = 0;
+
+			return -1;
+		}
+	}
+
+	mmio_write_32(base + I2C_ICR, I2C_FLAG_AF);
+
+	mmio_write_32(base + I2C_ICR, I2C_FLAG_STOPF);
+
+	i2c_flush_txdr(hi2c);
+
+	mmio_clrbits_32(base + I2C_CR2, I2C_RESET_CR2);
+
+	hi2c->i2c_err |= I2C_ERROR_AF;
+	hi2c->i2c_state = I2C_STATE_READY;
+	hi2c->i2c_mode = I2C_MODE_NONE;
+
+	hi2c->lock = 0;
+
+	return -1;
+}
+
+/*
+ * @brief  Handles I2Cx communication when starting transfer or during transfer
+ *	   (TC or TCR flag are set).
+ * @param  hi2c: I2C handle
+ * @param  dev_addr: Specifies the slave address to be programmed
+ * @param  size: Specifies the number of bytes to be programmed.
+ *   This parameter must be a value between 0 and 255.
+ * @param  i2c_mode: New state of the I2C START condition generation.
+ *   This parameter can be one of the following values:
+ *     @arg @ref I2C_RELOAD_MODE: Enable Reload mode.
+ *     @arg @ref I2C_AUTOEND_MODE: Enable Automatic end mode.
+ *     @arg @ref I2C_SOFTEND_MODE: Enable Software end mode.
+ * @param  request: New state of the I2C START condition generation.
+ *   This parameter can be one of the following values:
+ *     @arg @ref I2C_NO_STARTSTOP: Don't Generate stop and start condition.
+ *     @arg @ref I2C_GENERATE_STOP: Generate stop condition
+ *                                  (size should be set to 0).
+ *     @arg @ref I2C_GENERATE_START_READ: Generate Restart for read request.
+ *     @arg @ref I2C_GENERATE_START_WRITE: Generate Restart for write request.
+ * @retval None
+ */
+static void i2c_transfer_config(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+				uint16_t size, uint32_t i2c_mode,
+				uint32_t request)
+{
+	uint32_t clr_value, set_value;
+
+	clr_value = (I2C_CR2_SADD | I2C_CR2_NBYTES | I2C_CR2_RELOAD |
+		     I2C_CR2_AUTOEND | I2C_CR2_START | I2C_CR2_STOP) |
+		(I2C_CR2_RD_WRN & (request >> (31U - I2C_CR2_RD_WRN_OFFSET)));
+
+	set_value = ((uint32_t)dev_addr & I2C_CR2_SADD) |
+		(((uint32_t)size << I2C_CR2_NBYTES_OFFSET) & I2C_CR2_NBYTES) |
+		i2c_mode | request;
+
+	mmio_clrsetbits_32(get_base(hi2c) + I2C_CR2, clr_value, set_value);
+}
+
+#ifdef STM32_I2C_MAYBE_NON_SECURE
+/*
+ * Secure side needs the PMIC. If it is secure, it can be freely used.
+ * If the PMIC is non secure, the configuration must be save/restored
+ * when used by the secure side.
+ */
+#endif
+
+void stm32_i2c_resume(struct i2c_handle_s *hi2c)
+{
+	if (hi2c->i2c_state == I2C_STATE_READY) {
+		return;
+	}
+
+	if ((hi2c->i2c_state != I2C_STATE_RESET) &&
+	    (hi2c->i2c_state != I2C_STATE_SUSPENDED)) {
+		panic();
+	}
+
+#ifdef STM32_I2C_MAYBE_NON_SECURE
+	if (!i2c_is_secure(hi2c)) {
+		stm32_pinctrl_store_standby_cfg(hi2c->pinctrl,
+						hi2c->pinctrl_count);
+		save_cfg(hi2c, &hi2c->alt_cfg);
+	}
+#endif
+
+	stm32_pinctrl_load_active_cfg(hi2c->pinctrl, hi2c->pinctrl_count);
+
+	if (hi2c->i2c_state == I2C_STATE_RESET) {
+		/* This is no valid I2C configuration loaded yet */
+		return;
+	}
+
+	restore_cfg(hi2c, &hi2c->sec_cfg);
+
+	hi2c->i2c_state = I2C_STATE_READY;
+}
+
+void stm32_i2c_suspend(struct i2c_handle_s *hi2c)
+{
+	if ((hi2c->i2c_state == I2C_STATE_SUSPENDED)) {
+		return;
+	}
+
+	if ((hi2c->i2c_state != I2C_STATE_READY)) {
+		panic();
+	}
+
+	save_cfg(hi2c, &hi2c->sec_cfg);
+
+	stm32_pinctrl_load_standby_cfg(hi2c->pinctrl,
+				       hi2c->pinctrl_count);
+
+#ifdef STM32_I2C_MAYBE_NON_SECURE
+	if (!i2c_is_secure(hi2c)) {
+		restore_cfg(hi2c, &hi2c->alt_cfg);
+	}
+#endif
+
+	hi2c->i2c_state = I2C_STATE_SUSPENDED;
+}
diff --git a/core/drivers/stm32_iwdg.c b/core/drivers/stm32_iwdg.c
new file mode 100644
index 0000000..b2c8a68
--- /dev/null
+++ b/core/drivers/stm32_iwdg.c
@@ -0,0 +1,308 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ */
+
+#include <assert.h>
+#include <drivers/stm32_iwdg.h>
+#include <io.h>
+#include <keep.h>
+#include <kernel/delay.h>
+#include <kernel/dt.h>
+#include <kernel/generic_boot.h>
+#include <kernel/interrupt.h>
+#include <kernel/misc.h>
+#include <kernel/panic.h>
+#include <libfdt.h>
+#include <mm/core_memprot.h>
+#include <stm32_util.h>
+#include <stm32mp_dt.h>
+#include <stm32mp_pm.h>
+#include <string.h>
+#include <trace.h>
+
+/* IWDG Compatibility */
+#define IWDG_COMPAT		"st,stm32mp1-iwdg"
+#define IWDG_TIMEOUT_US		100000U
+
+/* IWDG registers offsets */
+#define IWDG_KR_OFFSET		0x00U
+#define IWDG_PR_OFFSET		0x04U
+#define IWDG_RLR_OFFSET		0x08U
+#define IWDG_SR_OFFSET		0x0CU
+#define IWDG_EWCR_OFFSET	0x14U
+
+/* Registers values */
+#define IWDG_KR_ACCESS_KEY	0x5555
+#define IWDG_KR_RELOAD_KEY	0xAAAA
+#define IWDG_KR_START_KEY	0xCCCC
+
+#define IWDG_PR_DIV_4		0x00
+#define IWDG_PR_DIV_256		0x06
+
+#define IWDG_RLR_MAX_VAL	0xFFF
+
+#define IWDG_SR_EWU		BIT(3)
+
+#define IWDG_EWCR_EWIE		BIT(15)
+#define IWDG_EWCR_EWIC		BIT(14)
+#define IWDG_EWCR_EWIT_MASK	GENMASK_32(11, 0)
+
+struct stm32_iwdg_instance {
+	uintptr_t pbase;
+	uintptr_t vbase;
+	unsigned long clock;
+	uint8_t instance;
+	uint8_t flags;
+};
+
+static struct stm32_iwdg_instance *stm32_iwdg;
+static size_t stm32_iwdg_count;
+
+static uintptr_t get_base(struct stm32_iwdg_instance *iwdg)
+{
+	if (!cpu_mmu_enabled()) {
+		return iwdg->pbase;
+	}
+
+	return iwdg->vbase;
+}
+
+static int stm32_iwdg_get_dt_node(void *fdt, struct dt_node_info *info,
+				  int offset)
+{
+	int node;
+
+	node = fdt_get_node(fdt, info, offset, IWDG_COMPAT);
+	if (node < 0) {
+		if (offset == -1) {
+			DMSG("No IDWG found");
+		}
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return node;
+}
+
+static struct stm32_iwdg_instance *get_iwdg(unsigned int instance)
+{
+	size_t i;
+
+	for (i = 0; i < stm32_iwdg_count; i++) {
+		if (stm32_iwdg[i].instance == instance) {
+			return &stm32_iwdg[i];
+		}
+	}
+
+	return NULL;
+}
+
+static enum itr_return stm32_iwdg_it_handler(struct itr_handler *handler)
+{
+	unsigned int __maybe_unused cpu = get_core_pos();
+	int instance = stm32mp_iwdg_irq2instance(handler->it);
+	struct stm32_iwdg_instance *iwdg = get_iwdg(instance);
+	uintptr_t iwdg_base = get_base(iwdg);
+
+	DMSG("CPU %u IT Watchdog %d\n", cpu, instance + 1);
+
+	stm32_iwdg_refresh(instance);
+
+	stm32_clock_enable(iwdg->clock);
+
+	mmio_setbits_32(iwdg_base + IWDG_EWCR_OFFSET, IWDG_EWCR_EWIC);
+
+	stm32_clock_disable(iwdg->clock);
+
+	stm32_cores_reset();
+
+	return ITRR_HANDLED;
+}
+KEEP_PAGER(stm32_iwdg_it_handler);
+
+static int stm32_iwdg_get_secure_timeout(void *fdt, int node)
+{
+	const fdt32_t *cuint;
+
+	cuint = fdt_getprop(fdt, node, "secure-timeout-sec", NULL);
+	if (cuint == NULL) {
+		return -1;
+	}
+
+	return (int)fdt32_to_cpu(*cuint);
+}
+
+static int stm32_iwdg_conf_etimeout(void *fdt, int node,
+				    struct stm32_iwdg_instance *iwdg)
+{
+	int id_lsi;
+	int dt_secure_timeout = stm32_iwdg_get_secure_timeout(fdt, node);
+	uint32_t reload, status;
+	uint64_t timeout_ref;
+	unsigned long long reload_ll;
+	uintptr_t iwdg_base = get_base(iwdg);
+	struct itr_handler *itr;
+
+	if (dt_secure_timeout < 0) {
+		return 0;
+	}
+
+	if (dt_secure_timeout == 0) {
+		return -1;
+	}
+
+	id_lsi = fdt_get_clock_id_by_name(fdt, node, "lsi");
+	if (id_lsi < 0) {
+		return -1;
+	}
+
+	/* Prescaler fix to 256 */
+	reload_ll = (unsigned long long)dt_secure_timeout *
+		    stm32mp1_clk_get_rate(id_lsi);
+	reload = ((uint32_t)(reload_ll >> 8) - 1U) & IWDG_EWCR_EWIT_MASK;
+
+	stm32_clock_enable(iwdg->clock);
+
+	write32(IWDG_KR_START_KEY, iwdg_base + IWDG_KR_OFFSET);
+	write32(IWDG_KR_ACCESS_KEY, iwdg_base + IWDG_KR_OFFSET);
+	write32(IWDG_PR_DIV_256, iwdg_base + IWDG_PR_OFFSET);
+	write32(IWDG_EWCR_EWIE | reload, iwdg_base + IWDG_EWCR_OFFSET);
+
+	timeout_ref = utimeout_init(IWDG_TIMEOUT_US);
+	do {
+		status = read32(iwdg_base + IWDG_SR_OFFSET) & IWDG_SR_EWU;
+		if (utimeout_elapsed(IWDG_TIMEOUT_US, timeout_ref)) {
+			stm32_clock_disable(iwdg->clock);
+			return -1;
+		}
+	} while (status != 0U);
+
+	stm32_clock_disable(iwdg->clock);
+
+	itr = calloc(1, sizeof(struct itr_handler));
+	if (itr == NULL) {
+		panic("out of memory");
+	}
+
+	itr->it = stm32mp_iwdg_instance2irq(iwdg->instance);
+	itr->handler = stm32_iwdg_it_handler;
+	itr_add(itr);
+	itr_enable(itr->it);
+
+	return 0;
+}
+
+void stm32_iwdg_refresh(uint32_t instance)
+{
+	struct stm32_iwdg_instance *iwdg = get_iwdg(instance);
+	uintptr_t iwdg_base = get_base(iwdg);
+
+	assert(iwdg);
+
+	stm32_clock_enable(iwdg->clock);
+
+	write32(IWDG_KR_RELOAD_KEY, iwdg_base + IWDG_KR_OFFSET);
+
+	stm32_clock_disable(iwdg->clock);
+}
+
+static TEE_Result iwdg_init(void)
+{
+	int node = -1;
+	int res;
+	struct dt_node_info dt_info;
+	void *fdt;
+	size_t count;
+
+	fdt = get_dt_blob();
+	if (!fdt) {
+		panic();
+	}
+
+	assert((stm32_iwdg == NULL) && (stm32_iwdg_count == 0));
+	count = 0;
+
+	for (node = stm32_iwdg_get_dt_node(fdt, &dt_info, node);
+	     node != -FDT_ERR_NOTFOUND;
+	     node = stm32_iwdg_get_dt_node(fdt, &dt_info, node)) {
+		struct stm32_iwdg_instance iwdg;
+		enum teecore_memtypes memtype;
+		uint32_t hw_init;
+
+		memset(&iwdg, 0, sizeof(iwdg));
+		iwdg.pbase = dt_info.base;
+		iwdg.clock = (unsigned long)dt_info.clock;
+		iwdg.instance = (uint8_t)stm32mp_iwdg_iomem2instance(iwdg.pbase);
+
+		memtype = ((dt_info.status & DT_STATUS_OK_NSEC) != 0) ?
+			  MEM_AREA_IO_NSEC : MEM_AREA_IO_SEC;
+		iwdg.vbase = (uintptr_t)phys_to_virt(iwdg.pbase, memtype);
+
+		/* DT can specify low power cases */
+		if (fdt_getprop(fdt, node, "stm32,enable-on-stop", NULL) !=
+		    NULL) {
+			iwdg.flags |= IWDG_ENABLE_ON_STOP;
+		}
+
+		if (fdt_getprop(fdt, node, "stm32,enable-on-standby", NULL) !=
+		    NULL) {
+			iwdg.flags |= IWDG_ENABLE_ON_STANDBY;
+		}
+
+		hw_init = stm32_get_iwdg_otp_config(iwdg.pbase);
+
+		if ((hw_init & IWDG_HW_ENABLED) != 0) {
+			if (dt_info.status == DT_STATUS_DISABLED) {
+				panic("IWDG HW enabled");
+			}
+			iwdg.flags |= IWDG_HW_ENABLED;
+		}
+
+		if ((hw_init & IWDG_ENABLE_ON_STOP) != 0) {
+			iwdg.flags |= IWDG_ENABLE_ON_STOP;
+		}
+
+		if ((hw_init & IWDG_ENABLE_ON_STANDBY) != 0) {
+			iwdg.flags |= IWDG_ENABLE_ON_STANDBY;
+		}
+
+		if (dt_info.status == DT_STATUS_DISABLED) {
+			continue;
+		}
+
+		DMSG("IWDG%u found, %ssecure", iwdg.instance + 1,
+		     ((dt_info.status & DT_STATUS_OK_NSEC) != 0) ? "non " : "");
+
+		if ((dt_info.status & DT_STATUS_OK_NSEC) != 0) {
+			stm32mp_register_non_secure_periph_iomem(iwdg.pbase);
+		} else {
+			stm32mp_register_secure_periph_iomem(iwdg.pbase);
+		}
+
+		stm32_clock_enable(iwdg.clock);
+		stm32_clock_disable(iwdg.clock);
+
+		res = stm32_iwdg_conf_etimeout(fdt, node, &iwdg);
+		if (res != 0) {
+			EMSG("IWDG%x early timeout config failed (%d)\n",
+			     iwdg.instance + 1, res);
+			panic();
+		}
+
+		stm32_iwdg = realloc(stm32_iwdg, (count + 1) * sizeof(iwdg));
+		if (stm32_iwdg == NULL) {
+			panic("out of memory");
+		}
+
+		memcpy(&stm32_iwdg[count], &iwdg, sizeof(iwdg));
+		count++;
+	}
+
+	stm32_iwdg_count = count;
+
+	DMSG("%u IWDG instance%s found", count, count > 1 ? "s" : "");
+
+	return TEE_SUCCESS;
+}
+driver_init(iwdg_init);
diff --git a/core/drivers/stm32_rng.c b/core/drivers/stm32_rng.c
new file mode 100644
index 0000000..75bbef7
--- /dev/null
+++ b/core/drivers/stm32_rng.c
@@ -0,0 +1,200 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <assert.h>
+#include <drivers/stm32_rng.h>
+#include <drivers/stm32_reset.h>
+#include <io.h>
+#include <libfdt.h>
+#include <platform_config.h>
+#include <kernel/dt.h>
+#include <kernel/delay.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <libfdt.h>
+#include <mm/core_memprot.h>
+#include <stdbool.h>
+#include <stm32mp_dt.h>
+#include <stm32_util.h>
+
+#define DT_RNG_COMPAT		"st,stm32-rng"
+#define RNG_CR			0x00U
+#define RNG_SR			0x04U
+#define RNG_DR			0x08U
+
+#define RNG_CR_RNGEN		BIT(2)
+#define RNG_CR_IE		BIT(3)
+#define RNG_CR_CED		BIT(5)
+
+#define RNG_SR_DRDY		BIT(0)
+#define RNG_SR_CECS		BIT(1)
+#define RNG_SR_SECS		BIT(2)
+#define RNG_SR_CEIS		BIT(5)
+#define RNG_SR_SEIS		BIT(6)
+
+#define RNG_TIMEOUT_US		100000
+
+struct stm32_rng_instance {
+	uintptr_t pbase;
+	uintptr_t vbase;
+	unsigned long clock;
+};
+
+static struct stm32_rng_instance *stm32_rng;
+
+static uintptr_t get_base(void)
+{
+	if (!cpu_mmu_enabled()) {
+		return stm32_rng->pbase;
+	}
+
+	return stm32_rng->vbase;
+}
+
+int stm32_rng_read(uint8_t *out, size_t size)
+{
+	uint8_t *buf = out;
+	size_t len = size;
+	uint64_t timeout_ref;
+	uint32_t data32;
+	uintptr_t rng_base = get_base();
+	int rc = 0;
+	int count;
+
+	if (stm32_rng == 0) {
+		return -1;
+	}
+
+	stm32_clock_enable(stm32_rng->clock);
+
+	if ((read32(rng_base + RNG_CR) & RNG_CR_RNGEN) == 0U) {
+		write32(RNG_CR_RNGEN | RNG_CR_CED, rng_base + RNG_CR);
+	}
+
+	while (len != 0) {
+		timeout_ref = utimeout_init(RNG_TIMEOUT_US);
+		do {
+			uint32_t status = read32(rng_base + RNG_SR);
+
+			if ((status & (RNG_SR_SECS | RNG_SR_SEIS)) != 0U) {
+				size_t i;
+
+				/* Recommended by the SoC reference manual */
+				io_mask32(rng_base + RNG_SR, 0, RNG_SR_SEIS);
+				dmb();
+				for (i = 12; i != 0; i--) {
+					(void)read32(rng_base + RNG_DR);
+				}
+				dmb();
+
+				if ((read32(rng_base + RNG_SR) & RNG_SR_SEIS) !=
+				    0U) {
+					panic("RNG noise");
+				}
+			}
+
+			if (utimeout_elapsed(RNG_TIMEOUT_US, timeout_ref)) {
+				rc = -1;
+				goto bail;
+			}
+		} while ((read32(rng_base + RNG_SR) & RNG_SR_DRDY) == 0U);
+
+		count = 4;
+		while (len != 0) {
+			data32 = read32(rng_base + RNG_DR);
+			count--;
+
+			memcpy(buf, &data32, MIN(len, sizeof(uint32_t)));
+			buf += MIN(len, sizeof(uint32_t));
+			len -= MIN(len, sizeof(uint32_t));
+
+			if (count == 0) {
+				break;
+			}
+		}
+	}
+
+bail:
+	stm32_clock_disable(stm32_rng->clock);
+
+	if (rc != 0) {
+		memset(out, 0, buf - out);
+	}
+
+	return rc;
+}
+
+static TEE_Result stm32_rng_init(void)
+{
+	void *fdt;
+	struct dt_node_info dt_rng;
+	int node;
+	uint8_t __maybe_unused test[43];
+	enum teecore_memtypes memtype;
+
+	fdt = get_dt_blob();
+	if (!fdt) {
+		panic();
+	}
+
+	node = fdt_get_node(fdt, &dt_rng, -1, DT_RNG_COMPAT);
+	if (node < 0) {
+		return TEE_SUCCESS;
+	}
+
+	if ((dt_rng.status & DT_STATUS_OK_SEC) == 0) {
+		return TEE_SUCCESS;
+	}
+
+	assert(dt_rng.base == RNG1_BASE);
+
+	if (stm32_rng) {
+		panic();
+	}
+	stm32_rng = calloc(1, sizeof(*stm32_rng));
+	if (!stm32_rng) {
+		panic();
+	}
+
+	stm32_rng->pbase = dt_rng.base;
+
+	if ((dt_rng.status & DT_STATUS_OK_NSEC) != 0) {
+		memtype = MEM_AREA_IO_NSEC;
+		stm32mp_register_non_secure_periph_iomem(stm32_rng->pbase);
+	} else {
+		memtype = MEM_AREA_IO_SEC;
+		stm32mp_register_secure_periph_iomem(stm32_rng->pbase);
+	}
+
+	stm32_rng->vbase = (uintptr_t)phys_to_virt(stm32_rng->pbase, memtype);
+
+	if (dt_rng.clock < 0) {
+		panic();
+	}
+	stm32_rng->clock = (unsigned long)dt_rng.clock;
+
+	stm32_clock_enable(stm32_rng->clock);
+	stm32_clock_disable(stm32_rng->clock);
+
+	if (dt_rng.reset >= 0) {
+		stm32_reset_assert((unsigned long)dt_rng.reset);
+		udelay(20);
+		stm32_reset_deassert((unsigned long)dt_rng.reset);
+	}
+
+	DMSG("Init RNG done");
+
+#if TRACE_LEVEL >= TRACE_DEBUG
+	memset(test, 0xa5, sizeof(test));
+	if (stm32_rng_read(test, sizeof(test))) {
+		panic("RNG test");
+	}
+
+	DHEXDUMP(test, sizeof(test));
+#endif
+
+	return TEE_SUCCESS;
+}
+driver_init(stm32_rng_init);
diff --git a/core/drivers/stm32_rtc.c b/core/drivers/stm32_rtc.c
new file mode 100644
index 0000000..709fafa
--- /dev/null
+++ b/core/drivers/stm32_rtc.c
@@ -0,0 +1,503 @@
+/*
+ * Copyright (c) 2018, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arm32.h>
+#include <drivers/stm32_rtc.h>
+#include <initcall.h>
+#include <kernel/dt.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <mm/core_memprot.h>
+#include <stm32_util.h>
+#include <stm32mp_dt.h>
+
+#define RTC_COMPAT		"st,stm32mp1-rtc"
+
+#define RTC_TR_SU_MASK		0x0000000FU
+#define RTC_TR_ST_MASK		0x00000070U
+#define RTC_TR_ST_SHIFT		4
+#define RTC_TR_MNU_MASK		0x00000F00U
+#define RTC_TR_MNU_SHIFT	8
+#define RTC_TR_MNT_MASK		0x00007000U
+#define RTC_TR_MNT_SHIFT	12
+#define RTC_TR_HU_MASK		0x000F0000U
+#define RTC_TR_HU_SHIFT		16
+#define RTC_TR_HT_MASK		0x00300000U
+#define RTC_TR_HT_SHIFT		20
+#define RTC_TR_PM		BIT(22)
+
+#define RTC_DR_DU_MASK		0x0000000FU
+#define RTC_DR_DT_MASK		0x00000030U
+#define RTC_DR_DT_SHIFT		4
+#define RTC_DR_MU_MASK		0x00000F00U
+#define RTC_DR_MU_SHIFT		8
+#define RTC_DR_MT		BIT(12)
+#define RTC_DR_MT_SHIFT		12
+#define RTC_DR_WDU_MASK		0x0000E000U
+#define RTC_DR_WDU_SHIFT	13
+#define RTC_DR_YU_MASK		0x000F0000U
+#define RTC_DR_YU_SHIFT		16
+#define RTC_DR_YT_MASK		0x00F00000U
+#define RTC_DR_YT_SHIFT		20
+
+#define RTC_SSR_SS_MASK		0x0000FFFFU
+
+#define RTC_ICSR_RSF		BIT(5)
+
+#define RTC_PRER_PREDIV_S_MASK	0x0000FFFFU
+
+#define RTC_CR_BYPSHAD		BIT(5)
+#define RTC_CR_BYPSHAD_SHIFT	5
+#define RTC_CR_TAMPTS		BIT(25)
+
+#define RTC_SMCR_TS_DPROT	BIT(3)
+#define RTC_SR_TSF		BIT(3)
+#define RTC_SCR_CTSF		BIT(3)
+#define RTC_SR_TSOVF		BIT(4)
+#define RTC_SCR_CTSOVF		BIT(4)
+
+#define RTC_TSDR_MU_MASK	0x00000F00U
+#define RTC_TSDR_MU_SHIFT	8
+#define RTC_TSDR_DT_MASK	0x00000030U
+#define RTC_TSDR_DT_SHIFT	4
+#define RTC_TSDR_DU_MASK	0x0000000FU
+#define RTC_TSDR_DU_SHIFT	0
+
+#define RTC_WPR_KEY1		0xCA
+#define RTC_WPR_KEY2		0x53
+#define RTC_WPR_KEY_LOCK	0xFF
+
+#define RTC_FLAGS_READ_TWICE	BIT(0)
+#define RTC_FLAGS_SECURE	BIT(1)
+
+struct rtc_device {
+	uintptr_t pbase;
+	uintptr_t vbase;
+	uint16_t clock;
+	uint8_t flags;
+};
+
+struct rtc_device rtc_dev;
+
+static uintptr_t get_base(void)
+{
+	if (!cpu_mmu_enabled())
+		return rtc_dev.pbase;
+
+	return rtc_dev.vbase;
+}
+
+static void stm32_rtc_write_unprotect(void)
+{
+	uintptr_t rtc_base = get_base();
+
+	mmio_write_32(rtc_base + RTC_WPR, RTC_WPR_KEY1);
+	mmio_write_32(rtc_base + RTC_WPR, RTC_WPR_KEY2);
+}
+
+static void stm32_rtc_write_protect(void)
+{
+	uintptr_t rtc_base = get_base();
+
+	mmio_write_32(rtc_base + RTC_WPR, RTC_WPR_KEY_LOCK);
+}
+
+/*******************************************************************************
+ * This function gets the BYPSHAD bit value of the RTC_CR register.
+ * It will determine if we need to reset RTC_ISCR.RSF after each RTC calendar
+ * read, and also wait for RTC_ISCR.RSF=1 before next read.
+ * Returns true or false depending on the bit value.
+ ******************************************************************************/
+static bool stm32_rtc_get_bypshad(void)
+{
+	return ((mmio_read_32(get_base() + RTC_CR) & RTC_CR_BYPSHAD) >>
+		RTC_CR_BYPSHAD_SHIFT) != 0U;
+}
+
+/*******************************************************************************
+ * This function reads the RTC calendar register values.
+ * If shadow registers are not bypassed, then a reset/poll is done.
+ ******************************************************************************/
+static void stm32_rtc_read_calendar(struct stm32_rtc_calendar *calendar)
+{
+	uintptr_t rtc_base = get_base();
+	bool bypshad = stm32_rtc_get_bypshad();
+
+	if (!bypshad) {
+		mmio_clrbits_32((uint32_t)(rtc_base + RTC_ICSR), RTC_ICSR_RSF);
+		while ((mmio_read_32(rtc_base + RTC_ICSR) & RTC_ICSR_RSF) !=
+		       RTC_ICSR_RSF) {
+			;
+		}
+	}
+
+	calendar->ssr = mmio_read_32(rtc_base + RTC_SSR);
+	calendar->tr = mmio_read_32(rtc_base + RTC_TR);
+	calendar->dr = mmio_read_32(rtc_base + RTC_DR);
+}
+
+/*******************************************************************************
+ * This function fill the rtc_time structure based on rtc_calendar register.
+ ******************************************************************************/
+static void stm32_rtc_get_time(struct stm32_rtc_calendar *cal,
+			       struct stm32_rtc_time *tm)
+{
+	tm->hour = (((cal->tr & RTC_TR_HT_MASK) >> RTC_TR_HT_SHIFT) * 10U) +
+		((cal->tr & RTC_TR_HU_MASK) >> RTC_TR_HU_SHIFT);
+
+	if ((cal->tr & RTC_TR_PM) != 0U) {
+		tm->hour += 12U;
+	}
+
+	tm->min = (((cal->tr & RTC_TR_MNT_MASK) >> RTC_TR_MNT_SHIFT) * 10U) +
+		  ((cal->tr & RTC_TR_MNU_MASK) >> RTC_TR_MNU_SHIFT);
+	tm->sec = (((cal->tr & RTC_TR_ST_MASK) >> RTC_TR_ST_SHIFT) * 10U) +
+		  (cal->tr & RTC_TR_SU_MASK);
+}
+
+/*******************************************************************************
+ * This function fill the rtc_time structure with the given date register.
+ ******************************************************************************/
+static void stm32_rtc_get_date(struct stm32_rtc_calendar *cal,
+			       struct stm32_rtc_time *tm)
+{
+	tm->wday = (((cal->dr & RTC_DR_WDU_MASK) >> RTC_DR_WDU_SHIFT));
+
+	tm->day = (((cal->dr & RTC_DR_DT_MASK) >> RTC_DR_DT_SHIFT) * 10U) +
+		  (cal->dr & RTC_DR_DU_MASK);
+
+	tm->month = (((cal->dr & RTC_DR_MT) >> RTC_DR_MT_SHIFT) * 10U) +
+		    ((cal->dr & RTC_DR_MU_MASK) >> RTC_DR_MU_SHIFT);
+
+	tm->year = (((cal->dr & RTC_DR_YT_MASK) >> RTC_DR_YT_SHIFT) * 10U) +
+		   ((cal->dr & RTC_DR_YU_MASK) >> RTC_DR_YU_SHIFT) + 2000U;
+}
+
+/*******************************************************************************
+ * This function reads the RTC timestamp register values and update time
+ * structure with the corresponding value.
+ ******************************************************************************/
+static void stm32_rtc_read_timestamp(struct stm32_rtc_time *time)
+{
+	struct stm32_rtc_calendar cal_tamp;
+	uintptr_t rtc_base = get_base();
+
+	cal_tamp.tr = mmio_read_32(rtc_base + RTC_TSTR);
+	cal_tamp.dr = mmio_read_32(rtc_base + RTC_TSDR);
+	stm32_rtc_get_time(&cal_tamp, time);
+	stm32_rtc_get_date(&cal_tamp, time);
+}
+
+/*******************************************************************************
+ * This function gets the RTC calendar register values.
+ * It takes into account the need of reading twice or not, depending on
+ * frequencies previously setted, and the bypass or not of the shadow
+ * registers. This service is exposed externally.
+ ******************************************************************************/
+void stm32_rtc_get_calendar(struct stm32_rtc_calendar *calendar)
+{
+	stm32_clock_enable(rtc_dev.clock);
+
+	stm32_rtc_read_calendar(calendar);
+
+	if (rtc_dev.flags & RTC_FLAGS_READ_TWICE) {
+		uint32_t tr_save = calendar->tr;
+
+		stm32_rtc_read_calendar(calendar);
+
+		if (calendar->tr != tr_save) {
+			stm32_rtc_read_calendar(calendar);
+		}
+	}
+
+	stm32_clock_disable(rtc_dev.clock);
+}
+
+/*******************************************************************************
+ * This function computes the second fraction in milliseconds.
+ * The returned value is a uint32_t between 0 and 1000.
+ ******************************************************************************/
+static uint32_t stm32_rtc_get_second_fraction(struct stm32_rtc_calendar *cal)
+{
+	uint32_t prediv_s = mmio_read_32(get_base() + RTC_PRER) &
+			    RTC_PRER_PREDIV_S_MASK;
+	uint32_t ss = cal->ssr & RTC_SSR_SS_MASK;
+
+	return ((prediv_s - ss) * 1000U) / (prediv_s + 1U);
+}
+
+/*******************************************************************************
+ * This function computes the fraction difference between two timestamps.
+ * Here again the returned value is in milliseconds.
+ ******************************************************************************/
+static unsigned long long stm32_rtc_diff_frac(struct stm32_rtc_calendar *cur,
+					      struct stm32_rtc_calendar *ref)
+{
+	unsigned long long val_r;
+	unsigned long long val_c;
+
+	val_r = stm32_rtc_get_second_fraction(ref);
+	val_c = stm32_rtc_get_second_fraction(cur);
+
+	if (val_c >= val_r) {
+		return val_c - val_r;
+	} else {
+		return 1000U - val_r + val_c;
+	}
+}
+
+/*******************************************************************************
+ * This function computes the time difference between two timestamps.
+ * It includes seconds, minutes and hours.
+ * Here again the returned value is in milliseconds.
+ ******************************************************************************/
+static unsigned long long stm32_rtc_diff_time(struct stm32_rtc_time *current,
+					      struct stm32_rtc_time *ref)
+{
+	signed long long diff_in_s;
+	signed long long curr_s;
+	signed long long ref_s;
+
+	curr_s = (signed long long)current->sec +
+		 (((signed long long)current->min +
+		  (((signed long long)current->hour * 60))) * 60);
+
+	ref_s = (signed long long)ref->sec +
+		(((signed long long)ref->min +
+		 (((signed long long)ref->hour * 60))) * 60);
+
+	diff_in_s = curr_s - ref_s;
+	if (diff_in_s < 0) {
+		diff_in_s += 24 * 60 * 60;
+	}
+
+	return (unsigned long long)diff_in_s * 1000U;
+}
+
+/*******************************************************************************
+ * This function determines if the year is leap or not.
+ * Returned value is true or false.
+ ******************************************************************************/
+static bool stm32_is_a_leap_year(uint32_t year)
+{
+	return ((year % 4U) == 0U) &&
+	       (((year % 100U) != 0U) || ((year % 400U) == 0U));
+}
+
+/*******************************************************************************
+ * This function computes the date difference between two timestamps.
+ * It includes days, months, years, with exceptions.
+ * Here again the returned value is in milliseconds.
+ ******************************************************************************/
+static unsigned long long stm32_rtc_diff_date(struct stm32_rtc_time *current,
+					      struct stm32_rtc_time *ref)
+{
+	uint32_t diff_in_days = 0;
+	uint32_t m;
+	static const uint8_t month_len[NB_MONTHS] = {
+		31, 28, 31, 30, 31, 30,
+		31, 31, 30, 31, 30, 31
+	};
+
+	/* Get the number of non-entire month days */
+	if (current->day >= ref->day) {
+		diff_in_days += current->day - ref->day;
+	} else {
+		diff_in_days += (uint32_t)month_len[ref->month - 1U] -
+				ref->day + current->day;
+	}
+
+	/* Get the number of entire months, and compute the related days */
+	if (current->month > (ref->month + 1U)) {
+		for (m = (ref->month + 1U); (m < current->month) &&
+		     (m < 12U); m++) {
+			diff_in_days += (uint32_t)month_len[m - 1U];
+		}
+	}
+
+	if (current->month < (ref->month - 1U)) {
+		for (m = 1U; (m < current->month) && (m < 12U); m++) {
+			diff_in_days += (uint32_t)month_len[m - 1U];
+		}
+
+		for (m = (ref->month + 1U); m < 12U; m++) {
+			diff_in_days += (uint32_t)month_len[m - 1U];
+		}
+	}
+
+	/* Get complete years */
+	if (current->year > (ref->year + 1U)) {
+		diff_in_days += (current->year - ref->year - 1U) * 365U;
+	}
+
+	/* Particular cases: leap years (one day more) */
+	if (diff_in_days > 0U) {
+		if (current->year == ref->year) {
+			if (stm32_is_a_leap_year(current->year)) {
+				if ((ref->month <= 2U) &&
+				    (current->month >= 3U) &&
+				    (current->day <= 28U)) {
+					diff_in_days++;
+				}
+			}
+		} else {
+			uint32_t y;
+
+			/* Ref year is leap */
+			if ((stm32_is_a_leap_year(ref->year)) &&
+			    (ref->month <= 2U) && (ref->day <= 28U)) {
+				diff_in_days++;
+			}
+
+			/* Current year is leap */
+			if ((stm32_is_a_leap_year(current->year)) &&
+			    (current->month >= 3U)) {
+				diff_in_days++;
+			}
+
+			/* Interleaved years are leap */
+			for (y = ref->year + 1U; y < current->year; y++) {
+				if (stm32_is_a_leap_year(y)) {
+					diff_in_days++;
+				}
+			}
+		}
+	}
+
+	return (24ULL * 60U * 60U * 1000U) * (unsigned long long)diff_in_days;
+}
+
+/*******************************************************************************
+ * This function computes the date difference between two rtc value.
+ * Here again the returned value is in milliseconds.
+ ******************************************************************************/
+unsigned long long stm32_rtc_diff_calendar(struct stm32_rtc_calendar *cur,
+					   struct stm32_rtc_calendar *ref)
+{
+	unsigned long long diff_in_ms = 0;
+	struct stm32_rtc_time curr_t;
+	struct stm32_rtc_time ref_t;
+
+	stm32_clock_enable(rtc_dev.clock);
+
+	stm32_rtc_get_date(cur, &curr_t);
+	stm32_rtc_get_date(ref, &ref_t);
+	stm32_rtc_get_time(cur, &curr_t);
+	stm32_rtc_get_time(ref, &ref_t);
+
+	diff_in_ms += stm32_rtc_diff_frac(cur, ref);
+	diff_in_ms += stm32_rtc_diff_time(&curr_t, &ref_t);
+	diff_in_ms += stm32_rtc_diff_date(&curr_t, &ref_t);
+
+	stm32_clock_disable(rtc_dev.clock);
+
+	return diff_in_ms;
+}
+
+/*******************************************************************************
+ * This function fill the RTC timestamp structure.
+ ******************************************************************************/
+void stm32_rtc_get_timestamp(struct stm32_rtc_time *tamp_ts)
+{
+	uintptr_t rtc_base = get_base();
+
+	stm32_clock_enable(rtc_dev.clock);
+
+	if ((mmio_read_32(rtc_base + RTC_SR) & RTC_SR_TSF) != 0U) {
+		/* Print timestamp for tamper event */
+		stm32_rtc_read_timestamp(tamp_ts);
+		mmio_setbits_32(rtc_base + RTC_SCR, RTC_SCR_CTSF);
+		if ((mmio_read_32(rtc_base + RTC_SR) & RTC_SR_TSOVF) !=
+		    0U) {
+			/* Overflow detected */
+			mmio_setbits_32(rtc_base + RTC_SCR, RTC_SCR_CTSOVF);
+		}
+	}
+
+	stm32_clock_disable(rtc_dev.clock);
+}
+
+/*******************************************************************************
+ * This function enable the timestamp bit for tamper and secure timestamp
+ * access.
+ ******************************************************************************/
+void stm32_rtc_set_tamper_timestamp(void)
+{
+	uintptr_t rtc_base = get_base();
+
+	stm32_clock_enable(rtc_dev.clock);
+
+	stm32_rtc_write_unprotect();
+
+	/* Enable tamper timestamper */
+	mmio_setbits_32(rtc_base + RTC_CR, RTC_CR_TAMPTS);
+
+	/* Secure Timestamp bit */
+	mmio_clrbits_32(rtc_base + RTC_SMCR, RTC_SMCR_TS_DPROT);
+
+	stm32_rtc_write_protect();
+
+	stm32_clock_disable(rtc_dev.clock);
+}
+
+/*******************************************************************************
+ * This function return state of tamper timestamp.
+ ******************************************************************************/
+bool stm32_rtc_is_timestamp_enable(void)
+{
+	bool ret;
+
+	stm32_clock_enable(rtc_dev.clock);
+
+	ret = (mmio_read_32(get_base() + RTC_CR) & RTC_CR_TAMPTS) != 0U;
+
+	stm32_clock_disable(rtc_dev.clock);
+
+	return ret;
+}
+
+#ifdef CFG_DT
+/*******************************************************************************
+ * RTC initialisation function.
+ ******************************************************************************/
+static TEE_Result stm32_rtc_init(void)
+{
+	int node;
+	struct dt_node_info dt_info;
+	void *fdt = get_dt_blob();
+
+	if (!fdt) {
+		panic();
+	}
+
+	node = fdt_get_node(fdt, &dt_info, -1, RTC_COMPAT);
+	if (node < 0) {
+		return node;
+	}
+
+	rtc_dev.pbase = dt_info.base;
+
+	if (dt_info.status == DT_STATUS_OK_SEC) {
+		rtc_dev.flags |= RTC_FLAGS_SECURE;
+		stm32mp_register_secure_periph_iomem(rtc_dev.pbase);
+		rtc_dev.vbase = (uintptr_t)phys_to_virt(rtc_dev.pbase,
+							MEM_AREA_IO_SEC);
+	} else {
+		stm32mp_register_non_secure_periph_iomem(rtc_dev.pbase);
+		rtc_dev.vbase = (uintptr_t)phys_to_virt(rtc_dev.pbase,
+							MEM_AREA_IO_NSEC);
+	}
+
+	rtc_dev.clock = (unsigned long)dt_info.clock;
+
+	if (stm32_rtc_get_read_twice()) {
+		rtc_dev.flags |= RTC_FLAGS_READ_TWICE;
+	}
+
+	return 0;
+}
+driver_init(stm32_rtc_init);
+#endif
diff --git a/core/drivers/stm32_timer.c b/core/drivers/stm32_timer.c
new file mode 100644
index 0000000..be03577
--- /dev/null
+++ b/core/drivers/stm32_timer.c
@@ -0,0 +1,272 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2018, STMicroelectronics
+ */
+
+#include <arm.h>
+#include <drivers/serial.h>
+#include <drivers/stm32_timer.h>
+#include <initcall.h>
+#include <keep.h>
+#include <kernel/delay.h>
+#include <kernel/dt.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <libfdt.h>
+#include <mm/core_memprot.h>
+#include <stm32_util.h>
+#include <stm32mp_dt.h>
+
+#define TIM_CR1			0x00U		/* Control Register 1      */
+#define TIM_CR2			0x04U		/* Control Register 2      */
+#define TIM_SMCR		0x08U		/* Slave mode control reg  */
+#define TIM_DIER		0x0CU		/* DMA/interrupt register  */
+#define TIM_SR			0x10U		/* Status register	   */
+#define TIM_EGR			0x14U		/* Event Generation Reg    */
+#define TIM_CCMR1		0x18U		/* Capt/Comp 1 Mode Reg    */
+#define TIM_CCMR2		0x1CU		/* Capt/Comp 2 Mode Reg    */
+#define TIM_CCER		0x20U		/* Capt/Comp Enable Reg    */
+#define TIM_CNT			0x24U		/* Counter		   */
+#define TIM_PSC			0x28U		/* Prescaler               */
+#define TIM_ARR			0x2CU		/* Auto-Reload Register    */
+#define TIM_CCR1		0x34U		/* Capt/Comp Register 1    */
+#define TIM_CCR2		0x38U		/* Capt/Comp Register 2    */
+#define TIM_CCR3		0x3CU		/* Capt/Comp Register 3    */
+#define TIM_CCR4		0x40U		/* Capt/Comp Register 4    */
+#define TIM_BDTR		0x44U		/* Break and Dead-Time Reg */
+#define TIM_DCR			0x48U		/* DMA control register    */
+#define TIM_DMAR		0x4CU		/* DMA transfer register   */
+#define TIM_AF1			0x60U		/* Alt Function Reg 1      */
+#define TIM_AF2			0x64U		/* Alt Function Reg 2      */
+#define TIM_TISEL		0x68U		/* Input Selection         */
+
+#define TIM_CR1_CEN		BIT(0)
+#define TIM_SMCR_SMS		GENMASK_32(2, 0) /* Slave mode selection */
+#define TIM_SMCR_TS		GENMASK_32(6, 4) /* Trigger selection */
+#define TIM_CCMR_CC1S_TI1	BIT(0)		/* IC1/IC3 selects TI1/TI3 */
+#define TIM_CCMR_CC1S_TI2	BIT(1)		/* IC1/IC3 selects TI2/TI4 */
+#define TIM_CCMR_CC2S_TI2	BIT(8)		/* IC2/IC4 selects TI2/TI4 */
+#define TIM_CCMR_CC2S_TI1	BIT(9)		/* IC2/IC4 selects TI1/TI3 */
+#define TIM_CCER_CC1E		BIT(0)		/* Capt/Comp 1  out Ena    */
+#define TIM_CCER_CC1P		BIT(1)		/* Capt/Comp 1  Polarity   */
+#define TIM_CCER_CC1NP		BIT(3)		/* Capt/Comp 1N Polarity   */
+#define TIM_SR_UIF		BIT(0)		/* UIF interrupt flag      */
+#define TIM_SR_CC1IF		BIT(1)		/* CC1 interrupt flag      */
+#define TIM_TISEL_TI1SEL_MASK	GENMASK_32(3, 0)
+#define TIM_SMCR_SMS_RESET	0x4U
+#define TIM_SMCR_TS_SHIFT	4U
+#define TIM_SMCR_TS_TI1FP1	0x5U
+
+#define TIM_COMPAT		"st,stm32-timers"
+#define TIM_TIMEOUT_US		100000
+#define TIM_TIMEOUT_STEP_US	10
+#define TIM_PRESCAL_HSI		10U
+#define TIM_PRESCAL_CSI		7U
+
+struct stm32_timer_instance {
+	struct io_pa_va base;
+	unsigned long clk;
+	unsigned long freq;
+	uint8_t cal_input;
+};
+
+static uintptr_t timer_base(struct stm32_timer_instance *timer)
+{
+	return (uintptr_t)io_pa_or_va(&timer->base);
+}
+
+/* Currently support HSI and CSI calibratrion */
+#define TIM_MAX_INSTANCE	2
+
+static struct stm32_timer_instance stm32_timer[TIM_MAX_INSTANCE];
+
+static int timer_get_dt_node(void *fdt, struct dt_node_info *info, int offset)
+{
+	int node;
+
+	node = fdt_get_node(fdt, info, offset, TIM_COMPAT);
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return node;
+}
+
+static void timer_config(struct stm32_timer_instance *timer)
+{
+	uintptr_t base = timer_base(timer);
+
+	stm32_clock_enable(timer->clk);
+
+	timer->freq = stm32_clock_get_rate(timer->clk);
+
+	if ((mmio_read_32(base + TIM_TISEL) & TIM_TISEL_TI1SEL_MASK) !=
+	    timer->cal_input) {
+		mmio_clrsetbits_32(base + TIM_CCMR1,
+				   TIM_CCMR_CC1S_TI1 | TIM_CCMR_CC1S_TI2,
+				   TIM_CCMR_CC1S_TI1);
+
+		mmio_clrbits_32(base + TIM_CCER,
+				TIM_CCER_CC1P | TIM_CCER_CC1NP);
+
+		mmio_clrsetbits_32(base + TIM_SMCR,
+				   TIM_SMCR_TS | TIM_SMCR_SMS,
+				   (TIM_SMCR_TS_TI1FP1 << TIM_SMCR_TS_SHIFT) |
+				   TIM_SMCR_SMS_RESET);
+
+		mmio_write_32(base + TIM_TISEL, timer->cal_input);
+		mmio_setbits_32(base + TIM_CR1, TIM_CR1_CEN);
+		mmio_setbits_32(base + TIM_CCER, TIM_CCER_CC1E);
+	}
+
+	stm32_clock_disable(timer->clk);
+}
+
+static uint32_t timer_start_capture(struct stm32_timer_instance *timer)
+{
+	uint64_t timeout_ref;
+	uint32_t counter = 0U;
+	uint32_t old_counter = 0U;
+	int twice;
+	uintptr_t base = timer_base(timer);
+
+	timer_config(timer);
+
+	stm32_clock_enable(timer->clk);
+
+	mmio_write_32(base + TIM_SR, 0U);
+
+	timeout_ref = utimeout_init(TIM_TIMEOUT_US);
+
+	while ((mmio_read_32(base + TIM_SR) & TIM_SR_UIF) == 0U) {
+		if (utimeout_elapsed(TIM_TIMEOUT_US, timeout_ref)) {
+			goto bail;
+		}
+	}
+
+	mmio_write_32(base + TIM_SR, 0U);
+
+	for (twice = 0; (twice < 2) || (counter != old_counter); twice++) {
+		timeout_ref = utimeout_init(TIM_TIMEOUT_US);
+
+		while ((mmio_read_32(base + TIM_SR) & TIM_SR_CC1IF) == 0U) {
+			if (utimeout_elapsed(TIM_TIMEOUT_US, timeout_ref)) {
+				counter = 0U;
+				goto bail;
+			}
+		}
+
+		old_counter = counter;
+		counter = mmio_read_32(base + TIM_CCR1);
+	}
+
+bail:
+	stm32_clock_disable(timer->clk);
+
+	return counter;
+}
+
+unsigned long stm32_timer_hsi_freq(void)
+{
+	struct stm32_timer_instance *timer = &stm32_timer[HSI_CAL];
+	uint32_t counter = 0;
+
+	if (timer->base.pa != 0) {
+		counter = timer_start_capture(timer);
+	}
+
+	return (counter == 0) ? 0 : (timer->freq / counter) << TIM_PRESCAL_HSI;
+}
+KEEP_PAGER(stm32_timer_hsi_freq);
+
+unsigned long stm32_timer_csi_freq(void)
+{
+	struct stm32_timer_instance *timer = &stm32_timer[CSI_CAL];
+	uint32_t counter = 0;
+
+	if (timer->base.pa != 0) {
+		counter = timer_start_capture(timer);
+	}
+
+	return (counter == 0U) ? 0 : (timer->freq / counter) << TIM_PRESCAL_CSI;
+}
+KEEP_PAGER(stm32_timer_csi_freq);
+
+static void _init_stm32_timer(void)
+{
+	void *fdt = get_dt_blob();
+	struct dt_node_info dt_timer;
+	int node = -1;
+	static bool inited;
+
+	if (inited)
+		return;
+	inited = true;
+
+	if (!fdt) {
+		panic();
+	}
+
+	for (node = timer_get_dt_node(fdt, &dt_timer, node);
+	     node != -FDT_ERR_NOTFOUND;
+	     node = timer_get_dt_node(fdt, &dt_timer, node)) {
+		struct stm32_timer_instance *timer;
+		const uint32_t *cuint;
+
+		if (!(dt_timer.status & DT_STATUS_OK_SEC)) {
+			continue;
+		}
+
+		cuint = fdt_getprop(fdt, node, "st,hsi-cal-input", NULL);
+		if (cuint != NULL) {
+			timer = &stm32_timer[HSI_CAL];
+			timer->base.pa = dt_timer.base;
+			timer->clk = dt_timer.clock;
+			timer->freq = stm32_clock_get_rate(timer->clk);
+			timer->cal_input = (uint8_t)fdt32_to_cpu(*cuint);
+			timer_config(timer);
+		}
+
+		cuint = fdt_getprop(fdt, node, "st,csi_cal-input", NULL);
+		if (cuint != NULL) {
+			timer = &stm32_timer[CSI_CAL];
+			timer->base.pa = dt_timer.base;
+			timer->clk = dt_timer.clock;
+			timer->freq = stm32_clock_get_rate(timer->clk);
+			timer->cal_input = (uint8_t)fdt32_to_cpu(*cuint);
+			timer_config(timer);
+		}
+	}
+}
+
+void stm32_timer_freq_func(unsigned long (**timer_freq_cb)(void),
+			   enum timer_cal type)
+{
+	_init_stm32_timer();
+
+	*timer_freq_cb = NULL;
+
+	switch (type) {
+	case HSI_CAL:
+		if (stm32_timer[HSI_CAL].base.pa != 0) {
+			*timer_freq_cb = stm32_timer_hsi_freq;
+		}
+		break;
+
+	case CSI_CAL:
+		if (stm32_timer[CSI_CAL].base.pa != 0) {
+			*timer_freq_cb = stm32_timer_csi_freq;
+		}
+		break;
+	default:
+		panic();
+	}
+}
+
+static TEE_Result init_stm32_timer(void)
+{
+	_init_stm32_timer();
+
+	return TEE_SUCCESS;
+}
+driver_init(init_stm32_timer);
diff --git a/core/drivers/stm32_uart.c b/core/drivers/stm32_uart.c
index 1febd84..4afbac8 100644
--- a/core/drivers/stm32_uart.c
+++ b/core/drivers/stm32_uart.c
@@ -4,10 +4,18 @@
  */
 
 #include <compiler.h>
+#include <console.h>
 #include <drivers/serial.h>
 #include <drivers/stm32_uart.h>
+#include <initcall.h>
 #include <io.h>
 #include <keep.h>
+#include <kernel/delay.h>
+#include <kernel/dt.h>
+#include <kernel/generic_boot.h>
+#include <stm32_util.h>
+#include <stm32mp_dt.h>
+#include <string.h>
 #include <util.h>
 
 #define UART_REG_CR1			0x00	/* Control register 1 */
@@ -21,6 +29,11 @@
 #define UART_REG_TDR			0x28	/* Transmit data register */
 #define UART_REG_PRESC			0x2c	/* Prescaler register */
 
+#define STM32_UART_IOMEM_SIZE		0x400
+
+#define PUTC_TIMEOUT_US			1000
+#define FLUSH_TIMEOUT_US		16000
+
 /*
  * Uart Interrupt & status register bits
  *
@@ -36,26 +49,52 @@
 
 static vaddr_t loc_chip_to_base(struct serial_chip *chip)
 {
-	struct console_pdata *pd =
-		container_of(chip, struct console_pdata, chip);
-
-	return io_pa_or_va(&pd->base);
+	struct stm32_uart_pdata *pd =
+		container_of(chip, struct stm32_uart_pdata, chip);
+
+	if (cpu_mmu_enabled()) {
+		if (!pd->base.va) {
+			pd->base.va = (vaddr_t)phys_to_virt(pd->base.pa,
+							    pd->secure ?
+							    MEM_AREA_IO_SEC :
+							    MEM_AREA_IO_NSEC);
+			assert(pd->base.va);
+		}
+
+		return pd->base.va;
+	}
+
+	return pd->base.pa;
 }
 
 static void loc_flush(struct serial_chip *chip)
 {
 	vaddr_t base = loc_chip_to_base(chip);
-
-	while (!(read32(base + UART_REG_ISR) & USART_ISR_TXFE))
-		;
+	uint64_t timeout_ref = utimeout_init(FLUSH_TIMEOUT_US);
+
+	while (!(read32(base + UART_REG_ISR) & USART_ISR_TXFE)) {
+		if (utimeout_elapsed(FLUSH_TIMEOUT_US, timeout_ref)) {
+			if (read32(base + UART_REG_ISR) & USART_ISR_TXFE) {
+				break;
+			}
+			return;
+		}
+	}
 }
 
 static void loc_putc(struct serial_chip *chip, int ch)
 {
 	vaddr_t base = loc_chip_to_base(chip);
+	uint64_t timeout_ref = utimeout_init(PUTC_TIMEOUT_US);
 
-	while (!(read32(base + UART_REG_ISR) & USART_ISR_TXE_TXFNF))
-		;
+	while (!(read32(base + UART_REG_ISR) & USART_ISR_TXE_TXFNF)) {
+		if (utimeout_elapsed(PUTC_TIMEOUT_US, timeout_ref)) {
+			if (read32(base + UART_REG_ISR) & USART_ISR_TXE_TXFNF) {
+				break;
+			}
+			return;
+		}
+	}
 
 	write32(ch, base + UART_REG_TDR);
 }
@@ -77,17 +116,94 @@ static int loc_getchar(struct serial_chip *chip)
 	return read32(base + UART_REG_RDR) & 0xff;
 }
 
-static const struct serial_ops serial_ops = {
+static const struct serial_ops stm32_uart_serial_ops = {
 	.flush = loc_flush,
 	.putc = loc_putc,
 	.have_rx_data = loc_have_rx_data,
 	.getchar = loc_getchar,
 
 };
-KEEP_PAGER(serial_ops);
+KEEP_PAGER(stm32_uart_serial_ops);
 
-void stm32_uart_init(struct console_pdata *pd, vaddr_t base)
+void stm32_uart_init(struct stm32_uart_pdata *pd, vaddr_t base)
 {
 	pd->base.pa = base;
-	pd->chip.ops = &serial_ops;
+	pd->chip.ops = &stm32_uart_serial_ops;
+}
+
+#ifdef CFG_DT
+static void register_secure_uart(struct stm32_uart_pdata *pd)
+{
+	size_t n;
+
+	stm32mp_register_secure_periph_iomem(pd->base.pa);
+	for (n = 0; n < pd->pinctrl_count; n++) {
+		stm32mp_register_secure_gpio(pd->pinctrl[n].bank,
+					     pd->pinctrl[n].pin);
+	}
+}
+
+static void register_non_secure_uart(struct stm32_uart_pdata *pd)
+{
+	size_t n;
+
+	stm32mp_register_non_secure_periph_iomem(pd->base.pa);
+	for (n = 0; n < pd->pinctrl_count; n++) {
+		stm32mp_register_non_secure_gpio(pd->pinctrl[n].bank,
+						 pd->pinctrl[n].pin);
+	}
 }
+
+struct stm32_uart_pdata *probe_uart_from_dt_node(void *fdt, int node)
+{
+	struct stm32_uart_pdata *pd;
+	struct dt_node_info info;
+	struct stm32_pinctrl *pinctrl_cfg = NULL;
+	int count;
+
+	fdt_fill_device_info(fdt, &info, node);
+
+	if (info.status == DT_STATUS_DISABLED) {
+		return NULL;
+	}
+
+	pd = calloc(1, sizeof(*pd));
+	if (!pd) {
+		panic();
+	}
+
+	pd->secure = (info.status == DT_STATUS_OK_SEC);
+	pd->base.pa = info.base;
+
+	if (info.clock < 0) {
+		panic();
+	}
+	pd->clock = (unsigned int)info.clock;
+
+	count = stm32_pinctrl_fdt_get_pinctrl(fdt, node, NULL, 0);
+	if (count < 0) {
+		panic();
+	}
+	if (count != 0) {
+		pinctrl_cfg = calloc(count, sizeof(*pinctrl_cfg));
+		if (!pinctrl_cfg) {
+			panic();
+		}
+		stm32_pinctrl_fdt_get_pinctrl(fdt, node, pinctrl_cfg, count);
+		stm32_pinctrl_load_active_cfg(pinctrl_cfg, count);
+	}
+	pd->pinctrl = pinctrl_cfg;
+	pd->pinctrl_count = count;
+
+	stm32_uart_init(pd, info.base);
+
+	if (pd->secure) {
+		register_secure_uart(pd);
+	} else {
+		register_non_secure_uart(pd);
+	}
+
+	return pd;
+}
+#endif /*CFG_DT*/
+
diff --git a/core/drivers/stpmic1.c b/core/drivers/stpmic1.c
new file mode 100644
index 0000000..3aa3afc
--- /dev/null
+++ b/core/drivers/stpmic1.c
@@ -0,0 +1,954 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2016-2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <drivers/stpmic1.h>
+#include <kernel/panic.h>
+#include <platform_config.h>
+#include <stdint.h>
+#include <string.h>
+#include <trace.h>
+#include <assert.h>
+
+#define I2C_TIMEOUT_MS		25
+
+struct regul_struct {
+	const char *dt_node_name;
+	const uint16_t *voltage_table;
+	uint8_t voltage_table_size;
+	uint8_t control_reg;
+	uint8_t low_power_reg;
+	uint8_t pull_down_reg;
+	uint8_t pull_down;
+	uint8_t mask_reset_reg;
+	uint8_t mask_reset;
+};
+
+static struct i2c_handle_s *pmic_i2c_handle;
+static uint16_t pmic_i2c_addr;
+
+/* Voltage tables in mV */
+static const uint16_t buck1_voltage_table[] = {
+	725,
+	725,
+	725,
+	725,
+	725,
+	725,
+	750,
+	775,
+	800,
+	825,
+	850,
+	875,
+	900,
+	925,
+	950,
+	975,
+	1000,
+	1025,
+	1050,
+	1075,
+	1100,
+	1125,
+	1150,
+	1175,
+	1200,
+	1225,
+	1250,
+	1275,
+	1300,
+	1325,
+	1350,
+	1375,
+	1400,
+	1425,
+	1450,
+	1475,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+	1500,
+};
+
+static const uint16_t buck2_voltage_table[] = {
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1050,
+	1050,
+	1100,
+	1100,
+	1150,
+	1150,
+	1200,
+	1200,
+	1250,
+	1250,
+	1300,
+	1300,
+	1350,
+	1350,
+	1400,
+	1400,
+	1450,
+	1450,
+	1500,
+};
+
+static const uint16_t buck3_voltage_table[] = {
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1000,
+	1100,
+	1100,
+	1100,
+	1100,
+	1200,
+	1200,
+	1200,
+	1200,
+	1300,
+	1300,
+	1300,
+	1300,
+	1400,
+	1400,
+	1400,
+	1400,
+	1500,
+	1600,
+	1700,
+	1800,
+	1900,
+	2000,
+	2100,
+	2200,
+	2300,
+	2400,
+	2500,
+	2600,
+	2700,
+	2800,
+	2900,
+	3000,
+	3100,
+	3200,
+	3300,
+	3400,
+};
+
+static const uint16_t buck4_voltage_table[] = {
+	600,
+	625,
+	650,
+	675,
+	700,
+	725,
+	750,
+	775,
+	800,
+	825,
+	850,
+	875,
+	900,
+	925,
+	950,
+	975,
+	1000,
+	1025,
+	1050,
+	1075,
+	1100,
+	1125,
+	1150,
+	1175,
+	1200,
+	1225,
+	1250,
+	1275,
+	1300,
+	1300,
+	1350,
+	1350,
+	1400,
+	1400,
+	1450,
+	1450,
+	1500,
+	1600,
+	1700,
+	1800,
+	1900,
+	2000,
+	2100,
+	2200,
+	2300,
+	2400,
+	2500,
+	2600,
+	2700,
+	2800,
+	2900,
+	3000,
+	3100,
+	3200,
+	3300,
+	3400,
+	3500,
+	3600,
+	3700,
+	3800,
+	3900,
+};
+
+static const uint16_t ldo1_voltage_table[] = {
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1800,
+	1900,
+	2000,
+	2100,
+	2200,
+	2300,
+	2400,
+	2500,
+	2600,
+	2700,
+	2800,
+	2900,
+	3000,
+	3100,
+	3200,
+	3300,
+};
+
+static const uint16_t ldo2_voltage_table[] = {
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1800,
+	1900,
+	2000,
+	2100,
+	2200,
+	2300,
+	2400,
+	2500,
+	2600,
+	2700,
+	2800,
+	2900,
+	3000,
+	3100,
+	3200,
+	3300,
+};
+
+static const uint16_t ldo3_voltage_table[] = {
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1800,
+	1900,
+	2000,
+	2100,
+	2200,
+	2300,
+	2400,
+	2500,
+	2600,
+	2700,
+	2800,
+	2900,
+	3000,
+	3100,
+	3200,
+	3300,
+	3300,
+	3300,
+	3300,
+	3300,
+	3300,
+	3300,
+	0xFFFF, /* VREFDDR */
+};
+
+static const uint16_t ldo5_voltage_table[] = {
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1700,
+	1800,
+	1900,
+	2000,
+	2100,
+	2200,
+	2300,
+	2400,
+	2500,
+	2600,
+	2700,
+	2800,
+	2900,
+	3000,
+	3100,
+	3200,
+	3300,
+	3400,
+	3500,
+	3600,
+	3700,
+	3800,
+	3900,
+};
+
+static const uint16_t ldo6_voltage_table[] = {
+	900,
+	1000,
+	1100,
+	1200,
+	1300,
+	1400,
+	1500,
+	1600,
+	1700,
+	1800,
+	1900,
+	2000,
+	2100,
+	2200,
+	2300,
+	2400,
+	2500,
+	2600,
+	2700,
+	2800,
+	2900,
+	3000,
+	3100,
+	3200,
+	3300,
+};
+
+static const uint16_t ldo4_voltage_table[] = {
+	3300,
+};
+
+static const uint16_t vref_ddr_voltage_table[] = {
+	3300,
+};
+
+/* Table of Regulators in PMIC SoC */
+static const struct regul_struct regulators_table[] = {
+	{
+		.dt_node_name	= "buck1",
+		.voltage_table	= buck1_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(buck1_voltage_table),
+		.control_reg	= BUCK1_CONTROL_REG,
+		.low_power_reg	= BUCK1_PWRCTRL_REG,
+		.pull_down_reg	= BUCK_PULL_DOWN_REG,
+		.pull_down	= BUCK1_PULL_DOWN_SHIFT,
+		.mask_reset_reg	= MASK_RESET_BUCK_REG,
+		.mask_reset	= BUCK1_MASK_RESET_SHIFT,
+	},
+	{
+		.dt_node_name	= "buck2",
+		.voltage_table	= buck2_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(buck2_voltage_table),
+		.control_reg	= BUCK2_CONTROL_REG,
+		.low_power_reg	= BUCK2_PWRCTRL_REG,
+		.pull_down_reg	= BUCK_PULL_DOWN_REG,
+		.pull_down	= BUCK2_PULL_DOWN_SHIFT,
+		.mask_reset_reg	= MASK_RESET_BUCK_REG,
+		.mask_reset	= BUCK2_MASK_RESET_SHIFT,
+	},
+	{
+		.dt_node_name	= "buck3",
+		.voltage_table	= buck3_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(buck3_voltage_table),
+		.control_reg	= BUCK3_CONTROL_REG,
+		.low_power_reg	= BUCK3_PWRCTRL_REG,
+		.pull_down_reg	= BUCK_PULL_DOWN_REG,
+		.pull_down	= BUCK3_PULL_DOWN_SHIFT,
+		.mask_reset_reg	= MASK_RESET_BUCK_REG,
+		.mask_reset	= BUCK3_MASK_RESET_SHIFT,
+	},
+	{
+		.dt_node_name	= "buck4",
+		.voltage_table	= buck4_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(buck4_voltage_table),
+		.control_reg	= BUCK4_CONTROL_REG,
+		.low_power_reg	= BUCK4_PWRCTRL_REG,
+		.pull_down_reg	= BUCK_PULL_DOWN_REG,
+		.pull_down	= BUCK4_PULL_DOWN_SHIFT,
+		.mask_reset_reg	= MASK_RESET_BUCK_REG,
+		.mask_reset	= BUCK4_MASK_RESET_SHIFT,
+	},
+	{
+		.dt_node_name	= "ldo1",
+		.voltage_table	= ldo1_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo1_voltage_table),
+		.control_reg	= LDO1_CONTROL_REG,
+		.low_power_reg	= LDO1_PWRCTRL_REG,
+		.mask_reset_reg	= MASK_RESET_LDO_REG,
+		.mask_reset	= LDO1_MASK_RESET_SHIFT,
+	},
+	{
+		.dt_node_name	= "ldo2",
+		.voltage_table	= ldo2_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo2_voltage_table),
+		.control_reg	= LDO2_CONTROL_REG,
+		.low_power_reg	= LDO2_PWRCTRL_REG,
+		.mask_reset_reg	= MASK_RESET_LDO_REG,
+		.mask_reset	= LDO2_MASK_RESET_SHIFT,
+	},
+	{
+		.dt_node_name	= "ldo3",
+		.voltage_table	= ldo3_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo3_voltage_table),
+		.control_reg	= LDO3_CONTROL_REG,
+		.low_power_reg	= LDO3_PWRCTRL_REG,
+		.mask_reset_reg	= MASK_RESET_LDO_REG,
+		.mask_reset	= LDO3_MASK_RESET_SHIFT,
+	},
+	{
+		.dt_node_name	= "ldo4",
+		.voltage_table	= ldo4_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo4_voltage_table),
+		.control_reg	= LDO4_CONTROL_REG,
+		.low_power_reg	= LDO4_PWRCTRL_REG,
+		.mask_reset_reg	= MASK_RESET_LDO_REG,
+		.mask_reset	= LDO4_MASK_RESET_SHIFT,
+	},
+	{
+		.dt_node_name	= "ldo5",
+		.voltage_table	= ldo5_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo5_voltage_table),
+		.control_reg	= LDO5_CONTROL_REG,
+		.low_power_reg	= LDO5_PWRCTRL_REG,
+		.mask_reset_reg	= MASK_RESET_LDO_REG,
+		.mask_reset	= LDO5_MASK_RESET_SHIFT,
+	},
+	{
+		.dt_node_name	= "ldo6",
+		.voltage_table	= ldo6_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(ldo6_voltage_table),
+		.control_reg	= LDO6_CONTROL_REG,
+		.low_power_reg	= LDO6_PWRCTRL_REG,
+		.mask_reset_reg	= MASK_RESET_LDO_REG,
+		.mask_reset	= LDO6_MASK_RESET_SHIFT,
+	},
+	{
+		.dt_node_name	= "vref_ddr",
+		.voltage_table	= vref_ddr_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(vref_ddr_voltage_table),
+		.control_reg	= VREF_DDR_CONTROL_REG,
+		.low_power_reg	= VREF_DDR_PWRCTRL_REG,
+		.mask_reset_reg	= MASK_RESET_LDO_REG,
+		.mask_reset	= VREF_DDR_MASK_RESET_SHIFT,
+	},
+};
+
+#define MAX_REGUL	ARRAY_SIZE(regulators_table)
+
+static const struct regul_struct *get_regulator_data(const char *name)
+{
+	uint8_t i;
+
+	for (i = 0 ; i < MAX_REGUL ; i++) {
+		if (strncmp(name, regulators_table[i].dt_node_name,
+			    strlen(regulators_table[i].dt_node_name)) == 0) {
+			return &regulators_table[i];
+		}
+	}
+
+	/* Regulator not found */
+	panic();
+	return NULL;
+}
+
+static uint8_t voltage_to_index(const char *name, uint16_t millivolts)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+	uint8_t i;
+
+	for (i = 0 ; i < regul->voltage_table_size ; i++) {
+		if (regul->voltage_table[i] == millivolts) {
+			return i;
+		}
+	}
+
+	/* Voltage not found */
+	panic();
+
+	return 0;
+}
+
+int stpmic1_powerctrl_on(void)
+{
+	return stpmic1_register_update(MAIN_CONTROL_REG, PWRCTRL_PIN_VALID,
+				       PWRCTRL_PIN_VALID);
+}
+
+int stpmic1_switch_off(void)
+{
+	return stpmic1_register_update(MAIN_CONTROL_REG, 1,
+				       SOFTWARE_SWITCH_OFF_ENABLED);
+}
+
+int stpmic1_regulator_enable(const char *name)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	return stpmic1_register_update(regul->control_reg, BIT(0), BIT(0));
+}
+
+int stpmic1_regulator_disable(const char *name)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	return stpmic1_register_update(regul->control_reg, 0, BIT(0));
+}
+
+uint8_t stpmic1_is_regulator_enabled(const char *name)
+{
+	uint8_t val;
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	if (stpmic1_register_read(regul->control_reg, &val) != 0) {
+		panic();
+	}
+
+	return (val & 0x1U);
+}
+
+int stpmic1_regulator_voltage_set(const char *name, uint16_t millivolts)
+{
+	uint8_t voltage_index = voltage_to_index(name, millivolts);
+	const struct regul_struct *regul = get_regulator_data(name);
+	uint8_t mask;
+
+	/* Voltage can be set for buck<N> or ldo<N> (except ldo4) regulators */
+	if (strncmp(name, "buck", 4) == 0) {
+		mask = BUCK_VOLTAGE_MASK;
+	} else if ((strncmp(name, "ldo", 3) == 0) &&
+		   (strncmp(name, "ldo4", 4) != 0)) {
+		mask = LDO_VOLTAGE_MASK;
+	} else {
+		return 0;
+	}
+
+	return stpmic1_register_update(regul->control_reg,
+				       voltage_index << LDO_BUCK_VOLTAGE_SHIFT,
+				       mask);
+}
+
+int stpmic1_regulator_pull_down_set(const char *name)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	if (regul->pull_down_reg != 0) {
+		return stpmic1_register_update(regul->pull_down_reg,
+					       BIT(regul->pull_down),
+					       LDO_BUCK_PULL_DOWN_MASK <<
+					       regul->pull_down);
+	}
+
+	return 0;
+}
+
+int stpmic1_regulator_mask_reset_set(const char *name)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	return stpmic1_register_update(regul->mask_reset_reg,
+				       BIT(regul->mask_reset),
+				       LDO_BUCK_RESET_MASK <<
+				       regul->mask_reset);
+}
+
+int stpmic1_bo_enable_unpg(struct stpmic1_bo_cfg *cfg)
+{
+	return stpmic1_register_update(cfg->ctrl_reg, BIT(0), BIT(0));
+}
+
+/* Returns 1 if no configuration are expected applied at runtime, 0 otherwise */
+int stpmic1_bo_voltage_cfg(const char *name, uint16_t millivolts,
+			   struct stpmic1_bo_cfg *cfg)
+{
+	uint8_t voltage_index = voltage_to_index(name, millivolts);
+	const struct regul_struct *regul = get_regulator_data(name);
+	uint8_t mask;
+
+	/* Voltage can be set for buck<N> or ldo<N> (except ldo4) regulators */
+	if (strncmp(name, "buck", 4) == 0) {
+		mask = BUCK_VOLTAGE_MASK;
+	} else if ((strncmp(name, "ldo", 3) == 0) &&
+		   (strncmp(name, "ldo4", 4) != 0)) {
+		mask = LDO_VOLTAGE_MASK;
+	} else {
+		return 1;
+	}
+
+	cfg->ctrl_reg = regul->control_reg;
+	cfg->value = voltage_index << LDO_BUCK_VOLTAGE_SHIFT;
+	cfg->mask = mask;
+
+	return 0;
+}
+
+int stpmic1_bo_voltage_unpg(struct stpmic1_bo_cfg *cfg)
+{
+	return stpmic1_register_update(cfg->ctrl_reg, cfg->value, cfg->mask);
+}
+
+int stpmic1_bo_pull_down_cfg(const char *name, struct stpmic1_bo_cfg *cfg)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	cfg->pd_reg = regul->pull_down_reg;
+	cfg->pd_value = BIT(regul->pull_down);
+	cfg->pd_mask = LDO_BUCK_PULL_DOWN_MASK << regul->pull_down;
+
+	return 0;
+}
+
+int stpmic1_bo_pull_down_unpg(struct stpmic1_bo_cfg *cfg)
+{
+	return stpmic1_register_update(cfg->pd_reg, cfg->pd_value,
+				       cfg->pd_mask);
+}
+
+int stpmic1_bo_mask_reset_cfg(const char *name, struct stpmic1_bo_cfg *cfg)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	cfg->mrst_reg = regul->mask_reset_reg;
+	cfg->mrst_value = BIT(regul->mask_reset);
+	cfg->mrst_mask = LDO_BUCK_RESET_MASK << regul->mask_reset;
+
+	return 0;
+}
+
+int stpmic1_bo_mask_reset_unpg(struct stpmic1_bo_cfg *cfg)
+{
+	return stpmic1_register_update(cfg->mrst_reg, cfg->mrst_value,
+				       cfg->mrst_mask);
+}
+
+int stpmic1_regulator_voltage_get(const char *name)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+	uint8_t value;
+	uint8_t mask;
+
+	/* Voltage can be set for buck<N> or ldo<N> (except ldo4) regulators */
+	if (strncmp(name, "buck", 4) == 0) {
+		mask = BUCK_VOLTAGE_MASK;
+	} else if ((strncmp(name, "ldo", 3) == 0) &&
+		   (strncmp(name, "ldo4", 4) != 0)) {
+		mask = LDO_VOLTAGE_MASK;
+	} else {
+		return 0;
+	}
+
+	if (stpmic1_register_read(regul->control_reg, &value))
+		return -1;
+
+	value = (value & mask) >> LDO_BUCK_VOLTAGE_SHIFT;
+
+	if (value > regul->voltage_table_size)
+		return -1;
+
+	return (int)regul->voltage_table[value];
+}
+
+int stpmic1_lp_copy_reg(const char *name)
+{
+	uint8_t val;
+	int status;
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	status = stpmic1_register_read(regul->control_reg, &val);
+	if (status != 0) {
+		return status;
+	}
+
+	return stpmic1_register_write(regul->low_power_reg, val);
+}
+
+int stpmic1_lp_cfg(const char *name, struct stpmic1_lp_cfg *cfg)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	cfg->ctrl_reg = regul->control_reg;
+	cfg->lp_reg = regul->low_power_reg;
+
+	return 0;
+}
+
+int stpmic1_lp_load_unpg(struct stpmic1_lp_cfg *cfg)
+{
+	uint8_t val;
+	int status;
+
+	status = stpmic1_register_read(cfg->ctrl_reg, &val);
+	if (status == 0) {
+		status = stpmic1_register_write(cfg->lp_reg, val);
+	}
+	return status;
+}
+
+int stpmic1_lp_reg_on_off(const char *name, uint8_t enable)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	return stpmic1_register_update(regul->low_power_reg, enable,
+				       LDO_BUCK_ENABLE_MASK);
+}
+
+int stpmic1_lp_on_off_unpg(struct stpmic1_lp_cfg *cfg, int enable)
+{
+	assert(enable == 0 || enable == 1);
+	return stpmic1_register_update(cfg->lp_reg, enable,
+				       LDO_BUCK_ENABLE_MASK);
+}
+
+int stpmic1_lp_set_mode(const char *name, uint8_t hplp)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	return stpmic1_register_update(regul->low_power_reg,
+				       hplp << LDO_BUCK_HPLP_SHIFT,
+				       LDO_BUCK_HPLP_ENABLE_MASK);
+}
+
+int stpmic1_lp_mode_unpg(struct stpmic1_lp_cfg *cfg, unsigned int mode)
+{
+	assert(mode == 0 || mode == 1);
+	return stpmic1_register_update(cfg->lp_reg,
+				       mode << LDO_BUCK_HPLP_SHIFT,
+				       LDO_BUCK_HPLP_ENABLE_MASK);
+}
+
+int stpmic1_lp_set_voltage(const char *name, uint16_t millivolts)
+{
+	uint8_t voltage_index = voltage_to_index(name, millivolts);
+	const struct regul_struct *regul = get_regulator_data(name);
+	uint8_t mask;
+
+	/* Voltage can be set for buck<N> or ldo<N> (except ldo4) regulators */
+	if (strncmp(name, "buck", 4) == 0) {
+		mask = BUCK_VOLTAGE_MASK;
+	} else if ((strncmp(name, "ldo", 3) == 0) &&
+		   (strncmp(name, "ldo4", 4) != 0)) {
+		mask = LDO_VOLTAGE_MASK;
+	} else {
+		return 0;
+	}
+
+	return stpmic1_register_update(regul->low_power_reg, voltage_index << 2,
+				       mask);
+}
+
+/* Returns 1 if no configuration are expected applied at runtime, 0 otherwise */
+int stpmic1_lp_voltage_cfg(const char *name, uint16_t millivolts,
+			   struct stpmic1_lp_cfg *cfg)
+
+{
+	uint8_t voltage_index = voltage_to_index(name, millivolts);
+	const struct regul_struct *regul = get_regulator_data(name);
+	uint8_t mask;
+
+	/* Voltage can be set for buck<N> or ldo<N> (except ldo4) regulators */
+	if (strncmp(name, "buck", 4) == 0) {
+		mask = BUCK_VOLTAGE_MASK;
+	} else if ((strncmp(name, "ldo", 3) == 0) &&
+		   (strncmp(name, "ldo4", 4) != 0)) {
+		mask = LDO_VOLTAGE_MASK;
+	} else {
+		return 1;
+	}
+
+	assert(cfg->lp_reg == regul->low_power_reg);
+	cfg->value = voltage_index << 2;
+	cfg->mask = mask;
+
+	return 0;
+}
+
+int stpmic1_lp_voltage_unpg(struct stpmic1_lp_cfg *cfg)
+{
+	return stpmic1_register_update(cfg->lp_reg, cfg->value,	cfg->mask);
+}
+
+int stpmic1_register_read(uint8_t register_id,  uint8_t *value)
+{
+	struct i2c_handle_s *i2c = pmic_i2c_handle;
+
+	return stm32_i2c_read_write_membyte(i2c, pmic_i2c_addr,
+					    register_id, value, false);
+}
+
+int stpmic1_register_write(uint8_t register_id, uint8_t value)
+{
+	int status;
+	struct i2c_handle_s *i2c = pmic_i2c_handle;
+
+	status = stm32_i2c_read_write_membyte(i2c, pmic_i2c_addr,
+					      register_id, &value, true);
+
+#ifdef CFG_TEE_CORE_DEBUG
+	if (status != 0) {
+		return status;
+	}
+
+	if ((register_id != WATCHDOG_CONTROL_REG) && (register_id <= 0x40U)) {
+		uint8_t readval;
+
+		status = stpmic1_register_read(register_id, &readval);
+		if (status != 0) {
+			return status;
+		}
+
+		if (readval != value) {
+			return -1;
+		}
+	}
+#endif
+
+	return status;
+}
+
+int stpmic1_register_update(uint8_t register_id, uint8_t value, uint8_t mask)
+{
+	int status;
+	uint8_t val;
+
+	status = stpmic1_register_read(register_id, &val);
+	if (status != 0) {
+		return status;
+	}
+
+	val = (val & ~mask) | (value & mask);
+
+	return stpmic1_register_write(register_id, val);
+}
+
+void stpmic1_bind_i2c(struct i2c_handle_s *i2c_handle, uint16_t i2c_addr)
+{
+	pmic_i2c_handle = i2c_handle;
+	pmic_i2c_addr = i2c_addr;
+}
+
+void stpmic1_dump_regulators(void)
+{
+	size_t i;
+	char __maybe_unused const *name;
+
+	for (i = 0 ; i < MAX_REGUL ; i++) {
+		name = regulators_table[i].dt_node_name;
+
+		DMSG("PMIC regul %s: %sable, %dmV",
+			name,
+			stpmic1_is_regulator_enabled(name) ? "en" : "dis",
+			stpmic1_regulator_voltage_get(name));
+	}
+}
+
+int stpmic1_get_version(unsigned long *version)
+{
+	int rc;
+	uint8_t read_val;
+
+	rc = stpmic1_register_read(VERSION_STATUS_REG, &read_val);
+	if (rc) {
+		return -1;
+	}
+
+	*version = (unsigned long)read_val;
+
+	return 0;
+}
diff --git a/core/drivers/sub.mk b/core/drivers/sub.mk
index b2bcf15..41f01fa 100644
--- a/core/drivers/sub.mk
+++ b/core/drivers/sub.mk
@@ -19,4 +19,13 @@ srcs-$(CFG_DRA7_RNG) += dra7_rng.c
 srcs-$(CFG_STIH_UART) += stih_asc.c
 srcs-$(CFG_ATMEL_UART) += atmel_uart.c
 srcs-$(CFG_MVEBU_UART) += mvebu_uart.c
+srcs-$(CFG_STM32_BSEC) += stm32_bsec.c
+srcs-$(CFG_STM32_ETZPC) += stm32_etzpc.c
+srcs-$(CFG_STM32_GPIO) += stm32_gpio.c
+srcs-$(CFG_STM32_I2C) += stm32_i2c.c
+srcs-$(CFG_STM32_IWDG) += stm32_iwdg.c
+srcs-$(CFG_STM32_RNG) += stm32_rng.c
+srcs-$(CFG_STM32_RTC) += stm32_rtc.c
+srcs-$(CFG_STM32_TIMER) += stm32_timer.c
 srcs-$(CFG_STM32_UART) += stm32_uart.c
+srcs-$(CFG_STPMIC1) += stpmic1.c
diff --git a/core/include/drivers/gic.h b/core/include/drivers/gic.h
index a4f60b7..df1e9af 100644
--- a/core/include/drivers/gic.h
+++ b/core/include/drivers/gic.h
@@ -9,14 +9,39 @@
 #include <types_ext.h>
 #include <kernel/interrupt.h>
 
+/* Constants to categorize priorities */
+#define GIC_HIGHEST_SEC_PRIORITY	0x0U
+#define GIC_LOWEST_SEC_PRIORITY		0x7fU
+#define GIC_HIGHEST_NS_PRIORITY		0x80U
+#define GIC_LOWEST_NS_PRIORITY		0xfeU
+/* 0xff would disable all interrupts */
+
 #define GIC_DIST_REG_SIZE	0x10000
 #define GIC_CPU_REG_SIZE	0x10000
 
+#if !defined(CFG_WITH_ARM_TRUSTED_FW) && defined(CFG_ARM_GICV3)
+#error CFG_ARM_GICV3 is not supported without CFG_WITH_ARM_TRUSTED_FW
+#endif
+
+struct gic_it_pm;
+
+/*
+ * Save and restore some interrupts configuration during low power sequences.
+ * This is used on platforms using OP-TEE secure monitor.
+ */
+struct gic_pm {
+	struct gic_it_pm *pm_cfg;
+	size_t count;
+};
+
 struct gic_data {
 	vaddr_t gicc_base;
 	vaddr_t gicd_base;
 	size_t max_it;
 	struct itr_chip chip;
+#if !defined(CFG_WITH_ARM_TRUSTED_FW)
+	struct gic_pm pm;
+#endif
 };
 
 /*
@@ -34,4 +59,8 @@ void gic_cpu_init(struct gic_data *gd);
 void gic_it_handle(struct gic_data *gd);
 
 void gic_dump_state(struct gic_data *gd);
+
+void gic_suspend(struct gic_data *gd);
+void gic_resume(struct gic_data *gd);
+
 #endif /*__DRIVERS_GIC_H*/
diff --git a/core/include/drivers/stm32_bsec.h b/core/include/drivers/stm32_bsec.h
new file mode 100644
index 0000000..de4d20b
--- /dev/null
+++ b/core/include/drivers/stm32_bsec.h
@@ -0,0 +1,143 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2015-2018, STMicroelectronics
+ */
+
+#ifndef __STM32_BSEC_H__
+#define __STM32_BSEC_H__
+
+#include <stdint.h>
+#include <io.h>
+
+#define BSEC_OTP_MASK			GENMASK_32(4, 0)
+#define BSEC_OTP_BANK_SHIFT		5
+
+#define BSEC_TIMEOUT_VALUE		0xFFFF
+
+#define ADDR_LOWER_OTP_PERLOCK_SHIFT	3
+#define DATA_LOWER_OTP_PERLOCK_BIT	3
+#define DATA_LOWER_OTP_PERLOCK_MASK	GENMASK_32(2, 0)
+#define ADDR_UPPER_OTP_PERLOCK_SHIFT	4
+#define DATA_UPPER_OTP_PERLOCK_BIT	1
+#define DATA_UPPER_OTP_PERLOCK_MASK	GENMASK_32(3, 0)
+
+/* Return status */
+#define BSEC_OK				0U
+#define BSEC_ERROR			0xFFFFFFFFU
+#define BSEC_DISTURBED			0xFFFFFFFEU
+#define BSEC_FEATURE_LOCK		0xFFFFFFFDU
+#define BSEC_INVALID_PARAM		0xFFFFFFFCU
+#define BSEC_PROG_FAIL			0xFFFFFFFBU
+#define BSEC_LOCK_FAIL			0xFFFFFFFAU
+#define BSEC_WRITE_FAIL			0xFFFFFFF9U
+#define BSEC_SHADOW_FAIL		0xFFFFFFF8U
+#define BSEC_TIMEOUT			0xFFFFFFF7U
+
+/* BSEC REGISTER OFFSET (base relative) */
+#define BSEC_OTP_CONF_OFF		0x000U
+#define BSEC_OTP_CTRL_OFF		0x004U
+#define BSEC_OTP_WRDATA_OFF		0x008U
+#define BSEC_OTP_STATUS_OFF		0x00CU
+#define BSEC_OTP_LOCK_OFF		0x010U
+#define BSEC_DEN_OFF			0x014U
+#define BSEC_FEN_OFF			0x018U
+#define BSEC_DISTURBED_OFF		0x01CU
+#define BSEC_DISTURBED1_OFF		0x020U
+#define BSEC_DISTURBED2_OFF		0x024U
+#define BSEC_ERROR_OFF			0x034U
+#define BSEC_ERROR1_OFF			0x038U
+#define BSEC_ERROR2_OFF			0x03CU
+#define BSEC_WRLOCK_OFF			0x04CU
+#define BSEC_WRLOCK1_OFF		0x050U
+#define BSEC_WRLOCK2_OFF		0x054U
+#define BSEC_SPLOCK_OFF			0x064U
+#define BSEC_SPLOCK1_OFF		0x068U
+#define BSEC_SPLOCK2_OFF		0x06CU
+#define BSEC_SWLOCK_OFF			0x07CU
+#define BSEC_SWLOCK1_OFF		0x080U
+#define BSEC_SWLOCK2_OFF		0x084U
+#define BSEC_SRLOCK_OFF			0x094U
+#define BSEC_SRLOCK1_OFF		0x098U
+#define BSEC_SRLOCK2_OFF		0x09CU
+#define BSEC_JTAG_IN_OFF		0x0ACU
+#define BSEC_JTAG_OUT_OFF		0x0B0U
+#define BSEC_SCRATCH_OFF		0x0B4U
+#define BSEC_OTP_DATA_OFF		0x200U
+#define BSEC_IPHW_CFG_OFF		0xFF0U
+#define BSEC_IPVR_OFF			0xFF4U
+#define BSEC_IP_ID_OFF			0xFF8U
+#define BSEC_IP_MAGIC_ID_OFF		0xFFCU
+
+/* BSEC_CONFIGURATION Register */
+#define BSEC_CONF_POWER_UP_MASK		BIT(0)
+#define BSEC_CONF_POWER_UP_SHIFT	0
+#define BSEC_CONF_FRQ_MASK		GENMASK_32(2, 1)
+#define BSEC_CONF_FRQ_SHIFT		1
+#define BSEC_CONF_PRG_WIDTH_MASK	GENMASK_32(6, 3)
+#define BSEC_CONF_PRG_WIDTH_SHIFT	3
+#define BSEC_CONF_TREAD_MASK		GENMASK_32(8, 7)
+#define BSEC_CONF_TREAD_SHIFT		7
+
+/* BSEC_CONTROL Register */
+#define BSEC_READ			0x000U
+#define BSEC_WRITE			0x100U
+#define BSEC_LOCK			0x200U
+
+/* STATUS Register */
+#define BSEC_MODE_STATUS_MASK		GENMASK_32(2, 0)
+#define BSEC_MODE_BUSY_MASK		BIT(3)
+#define BSEC_MODE_PROGFAIL_MASK		BIT(4)
+#define BSEC_MODE_PWR_MASK		BIT(5)
+#define BSEC_MODE_BIST1_LOCK_MASK	BIT(6)
+#define BSEC_MODE_BIST2_LOCK_MASK	BIT(7)
+
+/* Debug */
+#define BSEC_HDPEN			BIT(4)
+#define BSEC_SPIDEN			BIT(5)
+#define BSEC_SPINDEN			BIT(6)
+#define BSEC_DBGSWGEN			BIT(10)
+#define BSEC_DEN_ALL_MSK		GENMASK_32(10, 0)
+
+/*
+ * OTP Lock services definition
+ * Value must corresponding to the bit number in the register
+ */
+#define BSEC_LOCK_UPPER_OTP		0x00
+#define BSEC_LOCK_DEBUG			0x02
+#define BSEC_LOCK_PROGRAM		0x03
+
+/* Values for struct bsec_config::freq */
+#define FREQ_10_20_MHZ			0x0
+#define FREQ_20_30_MHZ			0x1
+#define FREQ_30_45_MHZ			0x2
+#define FREQ_45_67_MHZ			0x3
+
+uint32_t bsec_shadow_register(uint32_t otp);
+uint32_t bsec_read_otp(uint32_t *val, uint32_t otp);
+uint32_t bsec_write_otp(uint32_t val, uint32_t otp);
+uint32_t bsec_program_otp(uint32_t val, uint32_t otp);
+uint32_t bsec_permanent_lock_otp(uint32_t otp);
+
+uint32_t bsec_write_debug_conf(uint32_t val);
+uint32_t bsec_read_debug_conf(void);
+
+uint32_t bsec_get_status(void);
+uint32_t bsec_get_hw_conf(void);
+uint32_t bsec_get_version(void);
+uint32_t bsec_get_id(void);
+uint32_t bsec_get_magic_id(void);
+
+bool bsec_write_sr_lock(uint32_t otp, uint32_t value);
+bool bsec_read_sr_lock(uint32_t otp);
+bool bsec_write_sw_lock(uint32_t otp, uint32_t value);
+bool bsec_read_sw_lock(uint32_t otp);
+bool bsec_write_sp_lock(uint32_t otp, uint32_t value);
+bool bsec_read_sp_lock(uint32_t otp);
+bool bsec_wr_lock(uint32_t otp);
+uint32_t bsec_otp_lock(uint32_t service, uint32_t value);
+
+bool bsec_mode_is_closed_device(void);
+uint32_t bsec_shadow_read_otp(uint32_t *otp_value, uint32_t word);
+uint32_t bsec_check_nsec_access_rights(uint32_t otp);
+
+#endif /*__STM32_BSEC_H__*/
diff --git a/core/include/drivers/stm32_etzpc.h b/core/include/drivers/stm32_etzpc.h
new file mode 100644
index 0000000..bd34955
--- /dev/null
+++ b/core/include/drivers/stm32_etzpc.h
@@ -0,0 +1,75 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2018, STMicroelectronics
+ */
+
+#ifndef __STM32_ETZPC_H__
+#define __STM32_ETZPC_H__
+
+#include <io.h>
+#include <stdint.h>
+
+#define ETZPC_V1_0			0x10
+#define ETZPC_V2_0			0x20
+
+/*
+ * Define security level for each peripheral (DECPROT)
+ */
+enum etzpc_decprot_attributes {
+	TZPC_DECPROT_S_RW = 0,
+	TZPC_DECPROT_NS_R_S_W = 1,
+	TZPC_DECPROT_MCU_ISOLATION = 2,
+	TZPC_DECPROT_NS_RW = 3,
+	TZPC_DECPROT_MAX = 4,
+};
+
+/*
+ * etzpc_configure_decprot : Load a DECPROT configuration
+ * decprot_id : ID of the IP
+ * decprot_attr : Restriction access attributes
+ */
+void etzpc_configure_decprot(uint32_t decprot_id,
+			     enum etzpc_decprot_attributes decprot_attr);
+
+/*
+ * etzpc_get_decprot : Get the DECPROT attribute
+ * decprot_id : ID of the IP
+ * return : Attribute of this DECPROT
+ */
+enum etzpc_decprot_attributes etzpc_get_decprot(uint32_t decprot_id);
+
+/*
+ * etzpc_lock_decprot : Lock access to the DECPROT attributes
+ * decprot_id : ID of the IP
+ */
+void etzpc_lock_decprot(uint32_t decprot_id);
+
+/*
+ * etzpc_configure_tzma : Configure the target TZMA read only size
+ * tzma_id : ID of the memory
+ * tzma_value : read-only size
+ */
+void etzpc_configure_tzma(uint32_t tzma_id, uint16_t tzma_value);
+
+/*
+ * etzpc_get_tzma : Get the target TZMA read only size
+ * tzma_id : TZMA ID
+ * return : Size of read only size
+ */
+uint16_t etzpc_get_tzma(uint32_t tzma_id);
+
+/*
+ * etzpc_lock_tzma : Lock the target TZMA
+ * tzma_id : TZMA ID
+ */
+void etzpc_lock_tzma(uint32_t tzma_id);
+
+/*
+ * etzpc_get_lock_tzma : Return the lock status of the target TZMA
+ * tzma_id : TZMA ID
+ * return : True if TZMA is locked, false otherwise
+ */
+bool etzpc_get_lock_tzma(uint32_t tzma_id);
+
+#endif /*__STM32_ETZPC_H__*/
diff --git a/core/include/drivers/stm32_gpio.h b/core/include/drivers/stm32_gpio.h
new file mode 100644
index 0000000..67de10f
--- /dev/null
+++ b/core/include/drivers/stm32_gpio.h
@@ -0,0 +1,107 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2015-2018, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef __STM32_GPIO_H__
+#define __STM32_GPIO_H__
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stddef.h>
+
+#define GPIO_MODE_OFFSET	0x00U
+#define GPIO_TYPE_OFFSET	0x04U
+#define GPIO_SPEED_OFFSET	0x08U
+#define GPIO_PUPD_OFFSET	0x0CU
+#define GPIO_ODR_OFFSET		0x14U
+#define GPIO_BSRR_OFFSET	0x18U
+#define GPIO_AFRL_OFFSET	0x20U
+#define GPIO_AFRH_OFFSET	0x24U
+#define GPIO_SECR_OFFSET	0x30U
+
+#define GPIO_ALT_LOWER_LIMIT	0x08U
+
+#define GPIO_PIN_0		0x00U
+#define GPIO_PIN_1		0x01U
+#define GPIO_PIN_2		0x02U
+#define GPIO_PIN_3		0x03U
+#define GPIO_PIN_4		0x04U
+#define GPIO_PIN_5		0x05U
+#define GPIO_PIN_6		0x06U
+#define GPIO_PIN_7		0x07U
+#define GPIO_PIN_8		0x08U
+#define GPIO_PIN_9		0x09U
+#define GPIO_PIN_10		0x0AU
+#define GPIO_PIN_11		0x0BU
+#define GPIO_PIN_12		0x0CU
+#define GPIO_PIN_13		0x0DU
+#define GPIO_PIN_14		0x0EU
+#define GPIO_PIN_15		0x0FU
+#define GPIO_PIN_MAX		GPIO_PIN_15
+
+#define GPIO_ALTERNATE_0	0x00
+#define GPIO_ALTERNATE_1	0x01
+#define GPIO_ALTERNATE_2	0x02
+#define GPIO_ALTERNATE_3	0x03
+#define GPIO_ALTERNATE_4	0x04
+#define GPIO_ALTERNATE_5	0x05
+#define GPIO_ALTERNATE_6	0x06
+#define GPIO_ALTERNATE_7	0x07
+#define GPIO_ALTERNATE_8	0x08
+#define GPIO_ALTERNATE_9	0x09
+#define GPIO_ALTERNATE_10	0x0A
+#define GPIO_ALTERNATE_11	0x0B
+#define GPIO_ALTERNATE_12	0x0C
+#define GPIO_ALTERNATE_13	0x0D
+#define GPIO_ALTERNATE_14	0x0E
+#define GPIO_ALTERNATE_15	0x0F
+#define GPIO_ALTERNATE_MAX	0x0FU
+
+#define GPIO_MODE_INPUT		0x00
+#define GPIO_MODE_OUTPUT	0x01
+#define GPIO_MODE_ALTERNATE	0x02
+#define GPIO_MODE_ANALOG	0x03
+#define GPIO_MODE_MAX		0x03U
+
+#define GPIO_OPEN_DRAIN		0x10U
+
+#define GPIO_SPEED_LOW		0x00
+#define GPIO_SPEED_MEDIUM	0x01
+#define GPIO_SPEED_HIGH		0x02
+#define GPIO_SPEED_VERY_HIGH	0x03
+#define GPIO_SPEED_MAX		0x03U
+
+#define GPIO_NO_PULL		0x00
+#define GPIO_PULL_UP		0x01
+#define GPIO_PULL_DOWN		0x02
+#define GPIO_PULL_MAX		0x03U
+
+struct gpio_cfg {
+	uint16_t moder:		2;
+	uint16_t otyper:	1;
+	uint16_t ospeedr:	2;
+	uint16_t pupdr:		2;
+	uint16_t odr:		1;
+	uint16_t afr:		4;
+};
+
+struct stm32_pinctrl {
+	uint8_t bank;
+	uint8_t pin;
+	struct gpio_cfg active_cfg;
+	struct gpio_cfg standby_cfg;
+};
+
+void stm32_pinctrl_load_active_cfg(struct stm32_pinctrl *pinctrl, size_t cnt);
+void stm32_pinctrl_load_standby_cfg(struct stm32_pinctrl *pinctrl, size_t cnt);
+void stm32_pinctrl_store_standby_cfg(struct stm32_pinctrl *pinctrl, size_t cnt);
+
+int stm32_pinctrl_fdt_get_pinctrl(void *fdt, int node,
+				  struct stm32_pinctrl *cfg, size_t count);
+
+void stm32_gpio_set_output_level(uint32_t bank, uint32_t pin, int high);
+
+void stm32_gpio_set_secure_cfg(uint32_t bank, uint32_t pin, bool secure);
+
+#endif /*__STM32_GPIO_H__*/
diff --git a/core/include/drivers/stm32_i2c.h b/core/include/drivers/stm32_i2c.h
new file mode 100644
index 0000000..3c6e326
--- /dev/null
+++ b/core/include/drivers/stm32_i2c.h
@@ -0,0 +1,377 @@
+/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
+/*
+ * Copyright (c) 2016-2018, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef __STM32_I2C_H
+#define __STM32_I2C_H
+
+#include <drivers/stm32_gpio.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <util.h>
+
+/*
+ * When STM32_I2C_MAYBE_NON_SECURE is defined, the I2C bus maybe
+ * non secure and yet used by the secure world. In such case,
+ * secure world may use the I2C only in atomic conditions
+ * where the non secure world cannot be executed.
+ */
+#define STM32_I2C_MAYBE_NON_SECURE
+
+/* Bit definition for I2C_CR1 register */
+#define I2C_CR1_PE			BIT(0)
+#define I2C_CR1_TXIE			BIT(1)
+#define I2C_CR1_RXIE			BIT(2)
+#define I2C_CR1_ADDRIE			BIT(3)
+#define I2C_CR1_NACKIE			BIT(4)
+#define I2C_CR1_STOPIE			BIT(5)
+#define I2C_CR1_TCIE			BIT(6)
+#define I2C_CR1_ERRIE			BIT(7)
+#define I2C_CR1_DNF			GENMASK_32(11, 8)
+#define I2C_CR1_ANFOFF			BIT(12)
+#define I2C_CR1_SWRST			BIT(13)
+#define I2C_CR1_TXDMAEN			BIT(14)
+#define I2C_CR1_RXDMAEN			BIT(15)
+#define I2C_CR1_SBC			BIT(16)
+#define I2C_CR1_NOSTRETCH		BIT(17)
+#define I2C_CR1_WUPEN			BIT(18)
+#define I2C_CR1_GCEN			BIT(19)
+#define I2C_CR1_SMBHEN			BIT(22)
+#define I2C_CR1_SMBDEN			BIT(21)
+#define I2C_CR1_ALERTEN			BIT(22)
+#define I2C_CR1_PECEN			BIT(23)
+
+/* Bit definition for I2C_CR2 register */
+#define I2C_CR2_SADD			GENMASK_32(9, 0)
+#define I2C_CR2_RD_WRN			BIT(10)
+#define I2C_CR2_RD_WRN_OFFSET		10U
+#define I2C_CR2_ADD10			BIT(11)
+#define I2C_CR2_HEAD10R			BIT(12)
+#define I2C_CR2_START			BIT(13)
+#define I2C_CR2_STOP			BIT(14)
+#define I2C_CR2_NACK			BIT(15)
+#define I2C_CR2_NBYTES			GENMASK_32(23, 16)
+#define I2C_CR2_NBYTES_OFFSET		16U
+#define I2C_CR2_RELOAD			BIT(24)
+#define I2C_CR2_AUTOEND			BIT(25)
+#define I2C_CR2_PECBYTE			BIT(26)
+
+/* Bit definition for I2C_OAR1 register */
+#define I2C_OAR1_OA1			GENMASK_32(9, 0)
+#define I2C_OAR1_OA1MODE		BIT(10)
+#define I2C_OAR1_OA1EN			BIT(15)
+
+/* Bit definition for I2C_OAR2 register */
+#define I2C_OAR2_OA2			GENMASK_32(7, 1)
+#define I2C_OAR2_OA2MSK			GENMASK_32(10, 8)
+#define I2C_OAR2_OA2NOMASK		0
+#define I2C_OAR2_OA2MASK01		BIT(8)
+#define I2C_OAR2_OA2MASK02		BIT(9)
+#define I2C_OAR2_OA2MASK03		GENMASK_32(9, 8)
+#define I2C_OAR2_OA2MASK04		BIT(10)
+#define I2C_OAR2_OA2MASK05		(BIT(8) | BIT(10))
+#define I2C_OAR2_OA2MASK06		(BIT(9) | BIT(10))
+#define I2C_OAR2_OA2MASK07		GENMASK_32(10, 8)
+#define I2C_OAR2_OA2EN			BIT(15)
+
+/* Bit definition for I2C_TIMINGR register */
+#define I2C_TIMINGR_SCLL		GENMASK_32(7, 0)
+#define I2C_TIMINGR_SCLH		GENMASK_32(15, 8)
+#define I2C_TIMINGR_SDADEL		GENMASK_32(19, 16)
+#define I2C_TIMINGR_SCLDEL		GENMASK_32(23, 20)
+#define I2C_TIMINGR_PRESC		GENMASK_32(31, 28)
+#define I2C_TIMINGR_SCLL_MAX		(I2C_TIMINGR_SCLL + 1)
+#define I2C_TIMINGR_SCLH_MAX		((I2C_TIMINGR_SCLH >> 8) + 1)
+#define I2C_TIMINGR_SDADEL_MAX		((I2C_TIMINGR_SDADEL >> 16) + 1)
+#define I2C_TIMINGR_SCLDEL_MAX		((I2C_TIMINGR_SCLDEL >> 20) + 1)
+#define I2C_TIMINGR_PRESC_MAX		((I2C_TIMINGR_PRESC >> 28) + 1)
+#define I2C_SET_TIMINGR_SCLL(n)		((n) & \
+					 (I2C_TIMINGR_SCLL_MAX - 1))
+#define I2C_SET_TIMINGR_SCLH(n)		(((n) & \
+					  (I2C_TIMINGR_SCLH_MAX - 1)) << 8)
+#define I2C_SET_TIMINGR_SDADEL(n)	(((n) & \
+					  (I2C_TIMINGR_SDADEL_MAX - 1)) << 16)
+#define I2C_SET_TIMINGR_SCLDEL(n)	(((n) & \
+					  (I2C_TIMINGR_SCLDEL_MAX - 1)) << 20)
+#define I2C_SET_TIMINGR_PRESC(n)	(((n) & \
+					  (I2C_TIMINGR_PRESC_MAX - 1)) << 28)
+
+/* Bit definition for I2C_TIMEOUTR register */
+#define I2C_TIMEOUTR_TIMEOUTA		GENMASK_32(11, 0)
+#define I2C_TIMEOUTR_TIDLE		BIT(12)
+#define I2C_TIMEOUTR_TIMOUTEN		BIT(15)
+#define I2C_TIMEOUTR_TIMEOUTB		GENMASK_32(27, 16)
+#define I2C_TIMEOUTR_TEXTEN		BIT(31)
+
+/* Bit definition for I2C_ISR register */
+#define I2C_ISR_TXE			BIT(0)
+#define I2C_ISR_TXIS			BIT(1)
+#define I2C_ISR_RXNE			BIT(2)
+#define I2C_ISR_ADDR			BIT(3)
+#define I2C_ISR_NACKF			BIT(4)
+#define I2C_ISR_STOPF			BIT(5)
+#define I2C_ISR_TC			BIT(6)
+#define I2C_ISR_TCR			BIT(7)
+#define I2C_ISR_BERR			BIT(8)
+#define I2C_ISR_ARLO			BIT(9)
+#define I2C_ISR_OVR			BIT(10)
+#define I2C_ISR_PECERR			BIT(11)
+#define I2C_ISR_TIMEOUT			BIT(12)
+#define I2C_ISR_ALERT			BIT(13)
+#define I2C_ISR_BUSY			BIT(15)
+#define I2C_ISR_DIR			BIT(16)
+#define I2C_ISR_ADDCODE			GENMASK_32(23, 17)
+
+/* Bit definition for I2C_ICR register */
+#define I2C_ICR_ADDRCF			BIT(3)
+#define I2C_ICR_NACKCF			BIT(4)
+#define I2C_ICR_STOPCF			BIT(5)
+#define I2C_ICR_BERRCF			BIT(8)
+#define I2C_ICR_ARLOCF			BIT(9)
+#define I2C_ICR_OVRCF			BIT(10)
+#define I2C_ICR_PECCF			BIT(11)
+#define I2C_ICR_TIMOUTCF		BIT(12)
+#define I2C_ICR_ALERTCF			BIT(13)
+
+enum i2c_speed_e {
+	I2C_SPEED_STANDARD,	/* 100 kHz */
+	I2C_SPEED_FAST,		/* 400 kHz */
+	I2C_SPEED_FAST_PLUS,	/* 1 MHz   */
+};
+
+#define STANDARD_RATE				100000
+#define FAST_RATE				400000
+#define FAST_PLUS_RATE				1000000
+
+struct stm32_i2c_init_s {
+	uint32_t own_address1;		/*
+					 * Specifies the first device own
+					 * address. This parameter can be a
+					 * 7-bit or 10-bit address.
+					 */
+
+	uint32_t addressing_mode;	/*
+					 * Specifies if 7-bit or 10-bit
+					 * addressing mode is selected.
+					 * This parameter can be a value of
+					 * @ref I2C_ADDRESSING_MODE.
+					 */
+
+	uint32_t dual_address_mode;	/*
+					 * Specifies if dual addressing mode is
+					 * selected.
+					 * This parameter can be a value of @ref
+					 * I2C_DUAL_ADDRESSING_MODE.
+					 */
+
+	uint32_t own_address2;		/*
+					 * Specifies the second device own
+					 * address if dual addressing mode is
+					 * selected. This parameter can be a
+					 * 7-bit address.
+					 */
+
+	uint32_t own_address2_masks;	/*
+					 * Specifies the acknowledge mask
+					 * address second device own address
+					 * if dual addressing mode is selected
+					 * This parameter can be a value of @ref
+					 * I2C_OWN_ADDRESS2_MASKS.
+					 */
+
+	uint32_t general_call_mode;	/*
+					 * Specifies if general call mode is
+					 * selected.
+					 * This parameter can be a value of @ref
+					 * I2C_GENERAL_CALL_ADDRESSING_MODE.
+					 */
+
+	uint32_t no_stretch_mode;	/*
+					 * Specifies if nostretch mode is
+					 * selected.
+					 * This parameter can be a value of @ref
+					 * I2C_NOSTRETCH_MODE.
+					 */
+
+	uint32_t rise_time;		/*
+					 * Specifies the SCL clock pin rising
+					 * time in nanoseconds.
+					 */
+
+	uint32_t fall_time;		/*
+					 * Specifies the SCL clock pin falling
+					 * time in nanoseconds.
+					 */
+
+	enum i2c_speed_e speed_mode;	/*
+					 * Specifies the I2C clock source
+					 * frequency mode.
+					 * This parameter can be a value of @ref
+					 * i2c_speed_mode_e.
+					 */
+
+	int analog_filter;		/*
+					 * Specifies if the I2C analog noise
+					 * filter is selected.
+					 * This parameter can be 0 (filter
+					 * off), all other values mean filter
+					 * on.
+					 */
+
+	uint8_t digital_filter_coef;	/*
+					 * Specifies the I2C digital noise
+					 * filter coefficient.
+					 * This parameter can be a value
+					 * between 0 and
+					 * STM32_I2C_DIGITAL_FILTER_MAX.
+					 */
+};
+
+enum i2c_state_e {
+	I2C_STATE_RESET          = 0x00U,	/* Not yet initialized */
+	I2C_STATE_READY          = 0x20U,	/* Ready for use */
+	I2C_STATE_BUSY           = 0x24U,	/* Internal process ongoing */
+	I2C_STATE_BUSY_TX        = 0x21U,	/* Data Transmission ongoing */
+	I2C_STATE_BUSY_RX        = 0x22U,	/* Data Reception ongoing */
+	I2C_STATE_SUSPENDED	 = 0xF0U,	/* Bus is supended */
+};
+
+enum i2c_mode_e {
+	I2C_MODE_NONE   = 0x00U,	/* No active communication */
+	I2C_MODE_MASTER = 0x10U,	/* Communication in Master Mode */
+	I2C_MODE_SLAVE  = 0x20U,	/* Communication in Slave Mode */
+	I2C_MODE_MEM    = 0x40U		/* Communication in Memory Mode */
+
+};
+
+#define I2C_ERROR_NONE		0x00000000U	/* No error */
+#define I2C_ERROR_BERR		0x00000001U	/* BERR error */
+#define I2C_ERROR_ARLO		0x00000002U	/* ARLO error */
+#define I2C_ERROR_AF		0x00000004U	/* ACKF error */
+#define I2C_ERROR_OVR		0x00000008U	/* OVR error  */
+#define I2C_ERROR_DMA		0x00000010U	/* DMA transfer error */
+#define I2C_ERROR_TIMEOUT	0x00000020U	/* Timeout error */
+#define I2C_ERROR_SIZE		0x00000040U	/* Size Management error */
+
+#ifdef STM32_I2C_MAYBE_NON_SECURE
+struct i2c_cfg {
+	uint32_t timingr;
+	uint32_t oar1;
+	uint32_t oar2;
+	uint32_t cr1;
+	uint32_t cr2;
+};
+#endif
+
+struct i2c_handle_s {
+	uintptr_t pbase;			/* Registers phys base address */
+	uintptr_t vbase;			/* Registers virt base address */
+	unsigned int dt_status;			/* DT nsec/sec status */
+	unsigned int clock;			/* Clock reference */
+	uint8_t lock;				/* Locking object (FIXME: really usefull?) */
+	enum i2c_state_e i2c_state;		/* Communication state */
+	enum i2c_mode_e i2c_mode;		/* Communication mode */
+	uint32_t i2c_err;			/* Error code */
+	struct stm32_pinctrl *pinctrl;		/* PINCTRLs configuration for the I2C PINs */
+	size_t pinctrl_count;			/* Number of PINCTRLs elements */
+	struct i2c_cfg sec_cfg;			/* Context for secure usage */
+#ifdef STM32_I2C_MAYBE_NON_SECURE
+	struct i2c_cfg alt_cfg;			/* Alternate usage context */
+#endif
+};
+
+#define I2C_ADDRESSINGMODE_7BIT			0x00000001U
+#define I2C_ADDRESSINGMODE_10BIT		0x00000002U
+
+#define I2C_DUALADDRESS_DISABLE			0x00000000U
+#define I2C_DUALADDRESS_ENABLE			I2C_OAR2_OA2EN
+
+#define I2C_GENERALCALL_DISABLE			0x00000000U
+#define I2C_GENERALCALL_ENABLE			I2C_CR1_GCEN
+
+#define I2C_NOSTRETCH_DISABLE			0x00000000U
+#define I2C_NOSTRETCH_ENABLE			I2C_CR1_NOSTRETCH
+
+#define I2C_MEMADD_SIZE_8BIT			0x00000001U
+#define I2C_MEMADD_SIZE_16BIT			0x00000002U
+
+#define I2C_RELOAD_MODE				I2C_CR2_RELOAD
+#define I2C_AUTOEND_MODE			I2C_CR2_AUTOEND
+#define I2C_SOFTEND_MODE			0x00000000U
+
+#define I2C_NO_STARTSTOP			0x00000000U
+#define I2C_GENERATE_STOP			(BIT(31) | I2C_CR2_STOP)
+#define I2C_GENERATE_START_READ			(BIT(31) | I2C_CR2_START | \
+						 I2C_CR2_RD_WRN)
+#define I2C_GENERATE_START_WRITE		(BIT(31) | I2C_CR2_START)
+
+#define I2C_FLAG_TXE			I2C_ISR_TXE
+#define I2C_FLAG_TXIS			I2C_ISR_TXIS
+#define I2C_FLAG_RXNE			I2C_ISR_RXNE
+#define I2C_FLAG_ADDR			I2C_ISR_ADDR
+#define I2C_FLAG_AF			I2C_ISR_NACKF
+#define I2C_FLAG_STOPF			I2C_ISR_STOPF
+#define I2C_FLAG_TC			I2C_ISR_TC
+#define I2C_FLAG_TCR			I2C_ISR_TCR
+#define I2C_FLAG_BERR			I2C_ISR_BERR
+#define I2C_FLAG_ARLO			I2C_ISR_ARLO
+#define I2C_FLAG_OVR			I2C_ISR_OVR
+#define I2C_FLAG_PECERR			I2C_ISR_PECERR
+#define I2C_FLAG_TIMEOUT		I2C_ISR_TIMEOUT
+#define I2C_FLAG_ALERT			I2C_ISR_ALERT
+#define I2C_FLAG_BUSY			I2C_ISR_BUSY
+#define I2C_FLAG_DIR			I2C_ISR_DIR
+
+#define I2C_RESET_CR2			(I2C_CR2_SADD | I2C_CR2_HEAD10R | \
+					 I2C_CR2_NBYTES | I2C_CR2_RELOAD  | \
+					 I2C_CR2_RD_WRN)
+
+#define I2C_TIMEOUT_BUSY_MS		25U
+
+#define I2C_ANALOGFILTER_ENABLE		0x00000000U
+#define I2C_ANALOGFILTER_DISABLE	I2C_CR1_ANFOFF
+
+/* STM32 specific defines */
+#define STM32_I2C_RISE_TIME_DEFAULT		25	/* ns */
+#define STM32_I2C_FALL_TIME_DEFAULT		10	/* ns */
+#define STM32_I2C_SPEED_DEFAULT			I2C_SPEED_STANDARD
+#define STM32_I2C_ANALOG_FILTER_DELAY_MIN	50	/* ns */
+#define STM32_I2C_ANALOG_FILTER_DELAY_MAX	260	/* ns */
+#define STM32_I2C_DIGITAL_FILTER_MAX		16
+
+int stm32_i2c_get_setup_from_fdt(void *fdt, int node,
+				 struct stm32_i2c_init_s *init,
+				 struct stm32_pinctrl **pinctrl,
+				 size_t *pinctrl_count);
+
+int stm32_i2c_init(struct i2c_handle_s *hi2c,
+		   struct stm32_i2c_init_s *init_data);
+
+int stm32_i2c_mem_write(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			uint16_t mem_addr, uint16_t mem_add_size,
+			uint8_t *p_data, uint16_t size, uint32_t timeout_ms);
+
+int stm32_i2c_mem_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+		       uint16_t mem_addr, uint16_t mem_add_size,
+		       uint8_t *p_data, uint16_t size, uint32_t timeout_ms);
+
+int stm32_i2c_master_transmit(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			      uint8_t *p_data, uint16_t size,
+			      uint32_t timeout_ms);
+
+int stm32_i2c_master_receive(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			     uint8_t *p_data, uint16_t size,
+			     uint32_t timeout_ms);
+
+bool stm32_i2c_is_device_ready(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			       uint32_t trials, uint32_t timeout_ms);
+
+int stm32_i2c_read_write_membyte(struct i2c_handle_s *hi2c, uint16_t dev_addr,
+			      unsigned int mem_addr, uint8_t *p_data,
+			      bool write);
+
+void stm32_i2c_suspend(struct i2c_handle_s *hi2c);
+void stm32_i2c_resume(struct i2c_handle_s *hi2c);
+
+#endif /* __STM32_I2C_H */
diff --git a/core/include/drivers/stm32_iwdg.h b/core/include/drivers/stm32_iwdg.h
new file mode 100644
index 0000000..b649dee
--- /dev/null
+++ b/core/include/drivers/stm32_iwdg.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef __STM32_IWDG_H__
+#define __STM32_IWDG_H__
+
+#include <stdint.h>
+
+#define IWDG_HW_ENABLED			BIT(0)
+#define IWDG_ENABLE_ON_STOP		BIT(1)
+#define IWDG_ENABLE_ON_STANDBY		BIT(2)
+
+void stm32_iwdg_refresh(uint32_t instance);
+
+#endif /*__STM32_IWDG_H__*/
diff --git a/core/include/drivers/stm32_rng.h b/core/include/drivers/stm32_rng.h
new file mode 100644
index 0000000..1ba098f
--- /dev/null
+++ b/core/include/drivers/stm32_rng.h
@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef __STM32_RNG_H__
+#define __STM32_RNG_H__
+
+#include <stdint.h>
+#include <stddef.h>
+
+int stm32_rng_read(uint8_t *out, size_t size);
+
+#endif /*__STM32_RNG__*/
diff --git a/core/include/drivers/stm32_rtc.h b/core/include/drivers/stm32_rtc.h
new file mode 100644
index 0000000..39a262c
--- /dev/null
+++ b/core/include/drivers/stm32_rtc.h
@@ -0,0 +1,73 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ */
+
+#ifndef __PLAT_RTC_H__
+#define __PLAT_RTC_H__
+
+#include <stdbool.h>
+
+#define RTC_TR		0x00U
+#define RTC_DR		0x04U
+#define RTC_SSR		0x08U
+#define RTC_ICSR	0x0CU
+#define RTC_PRER	0x10U
+#define RTC_WUTR	0x14U
+#define RTC_CR		0x18U
+#define RTC_SMCR	0x20U
+#define RTC_WPR		0x24U
+#define RTC_CALR	0x28U
+#define RTC_SHIFTR	0x2CU
+#define RTC_TSTR	0x30U
+#define RTC_TSDR	0x34U
+#define RTC_TSSSR	0x38U
+#define RTC_ALRMAR	0x40U
+#define RTC_ALRMASSR	0x44U
+#define RTC_ALRMBR	0x48U
+#define RTC_ALRMBSSR	0x4CU
+#define RTC_SR		0x50U
+#define RTC_SCR		0x5CU
+#define RTC_OR		0x60U
+
+struct stm32_rtc_calendar {
+	uint32_t ssr;
+	uint32_t tr;
+	uint32_t dr;
+};
+
+enum months {
+	JANUARY = 1,
+	FEBRUARY,
+	MARCH,
+	APRIL,
+	MAY,
+	JUNE,
+	JULY,
+	AUGUST,
+	SEPTEMBER,
+	OCTOBER,
+	NOVEMBER,
+	DECEMBER,
+	NB_MONTHS = 12
+};
+
+struct stm32_rtc_time {
+	uint32_t hour;
+	uint32_t min;
+	uint32_t sec;
+	uint32_t wday;
+	uint32_t day;
+	enum months month;
+	uint32_t year;
+};
+
+void stm32_rtc_get_calendar(struct stm32_rtc_calendar *calendar);
+unsigned long long stm32_rtc_diff_calendar(struct stm32_rtc_calendar *current,
+					   struct stm32_rtc_calendar *ref);
+void stm32_rtc_set_tamper_timestamp(void);
+bool stm32_rtc_is_timestamp_enable(void);
+void stm32_rtc_get_timestamp(struct stm32_rtc_time *tamp_ts);
+
+#endif /* __PLAT_RTC_H__ */
diff --git a/core/include/drivers/stm32_timer.h b/core/include/drivers/stm32_timer.h
new file mode 100644
index 0000000..b7e6751
--- /dev/null
+++ b/core/include/drivers/stm32_timer.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef STM32_TIMER_H
+#define STM32_TIMER_H
+
+enum timer_cal {
+	HSI_CAL = 0,
+	CSI_CAL
+};
+
+unsigned long stm32_timer_hsi_freq(void);
+unsigned long stm32_timer_csi_freq(void);
+
+/*
+ * Get the timer frequence callback function for a target clock calibration
+ * @timer_freq_cb - Output callback function
+ * @type - Target clock calibration ID
+ */
+void stm32_timer_freq_func(unsigned long (**timer_freq_cb)(void),
+			   enum timer_cal type);
+
+#endif /* STM32_TIMER_H */
diff --git a/core/include/drivers/stm32_uart.h b/core/include/drivers/stm32_uart.h
index 58c7077..6ddb8c4 100644
--- a/core/include/drivers/stm32_uart.h
+++ b/core/include/drivers/stm32_uart.h
@@ -7,12 +7,18 @@
 #define __STM32_UART_H__
 
 #include <drivers/serial.h>
+#include <drivers/stm32_gpio.h>
 
-struct console_pdata {
+struct stm32_uart_pdata {
 	struct io_pa_va base;
 	struct serial_chip chip;
+	bool secure;
+	struct stm32_pinctrl *pinctrl;
+	size_t pinctrl_count;
+	unsigned long clock;
 };
 
-void stm32_uart_init(struct console_pdata *pd, vaddr_t base);
+void stm32_uart_init(struct stm32_uart_pdata *pd, vaddr_t base);
+struct stm32_uart_pdata *probe_uart_from_dt_node(void *fdt, int node);
 
 #endif /*__STM32_UART_H__*/
diff --git a/core/include/drivers/stpmic1.h b/core/include/drivers/stpmic1.h
new file mode 100644
index 0000000..3073662
--- /dev/null
+++ b/core/include/drivers/stpmic1.h
@@ -0,0 +1,228 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2016-2018, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef __STPMIC1_H__
+#define __STPMIC1_H__
+
+#include <drivers/stm32_i2c.h>
+#include <util.h>
+
+#define TURN_ON_REG			0x1U
+#define TURN_OFF_REG			0x2U
+#define ICC_LDO_TURN_OFF_REG		0x3U
+#define ICC_BUCK_TURN_OFF_REG		0x4U
+#define RESET_STATUS_REG		0x5U
+#define VERSION_STATUS_REG		0x6U
+#define MAIN_CONTROL_REG		0x10U
+#define PADS_PULL_REG			0x11U
+#define BUCK_PULL_DOWN_REG		0x12U
+#define LDO14_PULL_DOWN_REG		0x13U
+#define LDO56_PULL_DOWN_REG		0x14U
+#define VIN_CONTROL_REG			0x15U
+#define PONKEY_TIMER_REG		0x16U
+#define MASK_RANK_BUCK_REG		0x17U
+#define MASK_RESET_BUCK_REG		0x18U
+#define MASK_RANK_LDO_REG		0x19U
+#define MASK_RESET_LDO_REG		0x1AU
+#define WATCHDOG_CONTROL_REG		0x1BU
+#define WATCHDOG_TIMER_REG		0x1CU
+#define BUCK_ICC_TURNOFF_REG		0x1DU
+#define LDO_ICC_TURNOFF_REG		0x1EU
+#define BUCK_APM_CONTROL_REG		0x1FU
+#define BUCK1_CONTROL_REG		0x20U
+#define BUCK2_CONTROL_REG		0x21U
+#define BUCK3_CONTROL_REG		0x22U
+#define BUCK4_CONTROL_REG		0x23U
+#define VREF_DDR_CONTROL_REG		0x24U
+#define LDO1_CONTROL_REG		0x25U
+#define LDO2_CONTROL_REG		0x26U
+#define LDO3_CONTROL_REG		0x27U
+#define LDO4_CONTROL_REG		0x28U
+#define LDO5_CONTROL_REG		0x29U
+#define LDO6_CONTROL_REG		0x2AU
+#define BUCK1_PWRCTRL_REG		0x30U
+#define BUCK2_PWRCTRL_REG		0x31U
+#define BUCK3_PWRCTRL_REG		0x32U
+#define BUCK4_PWRCTRL_REG		0x33U
+#define VREF_DDR_PWRCTRL_REG		0x34U
+#define LDO1_PWRCTRL_REG		0x35U
+#define LDO2_PWRCTRL_REG		0x36U
+#define LDO3_PWRCTRL_REG		0x37U
+#define LDO4_PWRCTRL_REG		0x38U
+#define LDO5_PWRCTRL_REG		0x39U
+#define LDO6_PWRCTRL_REG		0x3AU
+#define FREQUENCY_SPREADING_REG		0x3BU
+#define USB_CONTROL_REG			0x40U
+#define ITLATCH1_REG			0x50U
+#define ITLATCH2_REG			0x51U
+#define ITLATCH3_REG			0x52U
+#define ITLATCH4_REG			0x53U
+#define ITSETLATCH1_REG			0x60U
+#define ITSETLATCH2_REG			0x61U
+#define ITSETLATCH3_REG			0x62U
+#define ITSETLATCH4_REG			0x63U
+#define ITCLEARLATCH1_REG		0x70U
+#define ITCLEARLATCH2_REG		0x71U
+#define ITCLEARLATCH3_REG		0x72U
+#define ITCLEARLATCH4_REG		0x73U
+#define ITMASK1_REG			0x80U
+#define ITMASK2_REG			0x81U
+#define ITMASK3_REG			0x82U
+#define ITMASK4_REG			0x83U
+#define ITSETMASK1_REG			0x90U
+#define ITSETMASK2_REG			0x91U
+#define ITSETMASK3_REG			0x92U
+#define ITSETMASK4_REG			0x93U
+#define ITCLEARMASK1_REG		0xA0U
+#define ITCLEARMASK2_REG		0xA1U
+#define ITCLEARMASK3_REG		0xA2U
+#define ITCLEARMASK4_REG		0xA3U
+#define ITSOURCE1_REG			0xB0U
+#define ITSOURCE2_REG			0xB1U
+#define ITSOURCE3_REG			0xB2U
+#define ITSOURCE4_REG			0xB3U
+
+/* Registers masks */
+#define LDO_VOLTAGE_MASK		0x7CU
+#define BUCK_VOLTAGE_MASK		0xFCU
+#define LDO_BUCK_VOLTAGE_SHIFT		2
+#define LDO_BUCK_ENABLE_MASK		0x01U
+#define LDO_BUCK_HPLP_ENABLE_MASK	0x02U
+#define LDO_BUCK_HPLP_SHIFT		1
+#define LDO_BUCK_RANK_MASK		0x01U
+#define LDO_BUCK_RESET_MASK		0x01U
+#define LDO_BUCK_PULL_DOWN_MASK		0x03U
+
+/* Pull down register */
+#define BUCK1_PULL_DOWN_SHIFT		0
+#define BUCK2_PULL_DOWN_SHIFT		2
+#define BUCK3_PULL_DOWN_SHIFT		4
+#define BUCK4_PULL_DOWN_SHIFT		6
+#define VREF_DDR_PULL_DOWN_SHIFT	4
+
+/* Buck Mask reset register */
+#define BUCK1_MASK_RESET_SHIFT		0
+#define BUCK2_MASK_RESET_SHIFT		1
+#define BUCK3_MASK_RESET_SHIFT		2
+#define BUCK4_MASK_RESET_SHIFT		3
+
+/* LDO Mask reset register */
+#define LDO1_MASK_RESET_SHIFT		0
+#define LDO2_MASK_RESET_SHIFT		1
+#define LDO3_MASK_RESET_SHIFT		2
+#define LDO4_MASK_RESET_SHIFT		3
+#define LDO5_MASK_RESET_SHIFT		4
+#define LDO6_MASK_RESET_SHIFT		5
+#define VREF_DDR_MASK_RESET_SHIFT	6
+
+/* Main PMIC Control Register (MAIN_CONTROL_REG) */
+#define ICC_EVENT_ENABLED		BIT(4)
+#define PWRCTRL_POLARITY_HIGH		BIT(3)
+#define PWRCTRL_PIN_VALID		BIT(2)
+#define RESTART_REQUEST_ENABLED		BIT(1)
+#define SOFTWARE_SWITCH_OFF_ENABLED	BIT(0)
+
+/* Main PMIC PADS Control Register (PADS_PULL_REG) */
+#define WAKEUP_DETECTOR_DISABLED	BIT(4)
+#define PWRCTRL_PD_ACTIVE		BIT(3)
+#define PWRCTRL_PU_ACTIVE		BIT(2)
+#define WAKEUP_PD_ACTIVE		BIT(1)
+#define PONKEY_PU_ACTIVE		BIT(0)
+
+/* Main PMIC VINLOW Control Register (VIN_CONTROL_REGC DMSC) */
+#define SWIN_DETECTOR_ENABLED		BIT(7)
+#define SWOUT_DETECTOR_ENABLED          BIT(6)
+#define VINLOW_HYST_MASK		0x3
+#define VINLOW_HYST_SHIFT		4
+#define VINLOW_THRESHOLD_MASK		0x7
+#define VINLOW_THRESHOLD_SHIFT		1
+#define VINLOW_ENABLED			0x01
+#define VINLOW_CTRL_REG_MASK		0xFF
+
+/* USB Control Register */
+#define BOOST_OVP_DISABLED		BIT(7)
+#define VBUS_OTG_DETECTION_DISABLED	BIT(6)
+#define OCP_LIMIT_HIGH			BIT(3)
+#define SWIN_SWOUT_ENABLED		BIT(2)
+#define USBSW_OTG_SWITCH_ENABLED	BIT(1)
+
+int stpmic1_powerctrl_on(void);
+int stpmic1_switch_off(void);
+
+int stpmic1_register_read(uint8_t register_id, uint8_t *value);
+int stpmic1_register_write(uint8_t register_id, uint8_t value);
+int stpmic1_register_update(uint8_t register_id, uint8_t value, uint8_t mask);
+
+int stpmic1_regulator_enable(const char *name);
+int stpmic1_regulator_disable(const char *name);
+uint8_t stpmic1_is_regulator_enabled(const char *name);
+
+int stpmic1_regulator_voltage_set(const char *name, uint16_t millivolts);
+int stpmic1_regulator_voltage_get(const char *name);
+int stpmic1_regulator_mask_reset_set(const char *name);
+
+int stpmic1_lp_copy_reg(const char *name);
+int stpmic1_lp_reg_on_off(const char *name, uint8_t enable);
+int stpmic1_lp_set_mode(const char *name, uint8_t hplp);
+int stpmic1_lp_set_voltage(const char *name, uint16_t millivolts);
+
+/*
+ * The STPMIC1 is accessed during low power sequence in unpaged
+ * execution context. To prevent adding an unpaged constraint on
+ * STPMIC1 regulator definitions, conversion tables and device tree
+ * content, the regulators configurations are read from device tree
+ * at boot time and saved in memory for being applied at runtime
+ * without needing pager support.
+ *
+ * There are 2 types of regulator configuration loaded during such
+ * low power and unpaged sequences: boot-on (bo) configuration and
+ * low power (lp) configuration.
+ */
+struct stpmic1_bo_cfg {
+	uint8_t ctrl_reg;
+	uint8_t value;
+	uint8_t mask;
+	uint8_t pd_reg;
+	uint8_t pd_value;
+	uint8_t pd_mask;
+	uint8_t mrst_reg;
+	uint8_t mrst_value;
+	uint8_t mrst_mask;
+};
+
+struct stpmic1_lp_cfg {
+	uint8_t ctrl_reg;
+	uint8_t lp_reg;
+	uint8_t value;
+	uint8_t mask;
+};
+
+int stpmic1_bo_enable_unpg(struct stpmic1_bo_cfg *cfg);
+int stpmic1_bo_voltage_cfg(const char *name, uint16_t millivolts,
+			   struct stpmic1_bo_cfg *cfg);
+int stpmic1_bo_voltage_unpg(struct stpmic1_bo_cfg *cfg);
+
+int stpmic1_bo_pull_down_cfg(const char *name,
+			     struct stpmic1_bo_cfg *cfg);
+int stpmic1_bo_pull_down_unpg(struct stpmic1_bo_cfg *cfg);
+
+int stpmic1_bo_mask_reset_cfg(const char *name, struct stpmic1_bo_cfg *cfg);
+int stpmic1_bo_mask_reset_unpg(struct stpmic1_bo_cfg *cfg);
+
+int stpmic1_lp_cfg(const char *name, struct stpmic1_lp_cfg *cfg);
+int stpmic1_lp_load_unpg(struct stpmic1_lp_cfg *cfg);
+int stpmic1_lp_on_off_unpg(struct stpmic1_lp_cfg *cfg, int enable);
+int stpmic1_lp_mode_unpg(struct stpmic1_lp_cfg *cfg,
+			     unsigned int mode);
+int stpmic1_lp_voltage_cfg(const char *name, uint16_t millivolts,
+			   struct stpmic1_lp_cfg *cfg);
+int stpmic1_lp_voltage_unpg(struct stpmic1_lp_cfg *cfg);
+
+void stpmic1_bind_i2c(struct i2c_handle_s *i2c_handle, uint16_t i2c_addr);
+
+int stpmic1_get_version(unsigned long *version);
+void stpmic1_dump_regulators(void);
+
+#endif /*__STPMIC1_H__*/
diff --git a/core/include/dt-bindings/clock/stm32mp1-clks.h b/core/include/dt-bindings/clock/stm32mp1-clks.h
new file mode 100644
index 0000000..f5254cf
--- /dev/null
+++ b/core/include/dt-bindings/clock/stm32mp1-clks.h
@@ -0,0 +1,252 @@
+/* SPDX-License-Identifier: GPL-2.0 or BSD-3-Clause */
+/*
+ * Copyright (C) STMicroelectronics 2018 - All Rights Reserved
+ * Author: Gabriel Fernandez <gabriel.fernandez@st.com> for STMicroelectronics.
+ */
+
+#ifndef _DT_BINDINGS_STM32MP1_CLKS_H_
+#define _DT_BINDINGS_STM32MP1_CLKS_H_
+
+/* OSCILLATOR clocks */
+#define CK_HSE		0
+#define CK_CSI		1
+#define CK_LSI		2
+#define CK_LSE		3
+#define CK_HSI		4
+#define CK_HSE_DIV2	5
+
+/* Bus clocks */
+#define TIM2		6
+#define TIM3		7
+#define TIM4		8
+#define TIM5		9
+#define TIM6		10
+#define TIM7		11
+#define TIM12		12
+#define TIM13		13
+#define TIM14		14
+#define LPTIM1		15
+#define SPI2		16
+#define SPI3		17
+#define USART2		18
+#define USART3		19
+#define UART4		20
+#define UART5		21
+#define UART7		22
+#define UART8		23
+#define I2C1		24
+#define I2C2		25
+#define I2C3		26
+#define I2C5		27
+#define SPDIF		28
+#define CEC		29
+#define DAC12		30
+#define MDIO		31
+#define TIM1		32
+#define TIM8		33
+#define TIM15		34
+#define TIM16		35
+#define TIM17		36
+#define SPI1		37
+#define SPI4		38
+#define SPI5		39
+#define USART6		40
+#define SAI1		41
+#define SAI2		42
+#define SAI3		43
+#define DFSDM		44
+#define FDCAN		45
+#define LPTIM2		46
+#define LPTIM3		47
+#define LPTIM4		48
+#define LPTIM5		49
+#define SAI4		50
+#define SYSCFG		51
+#define VREF		52
+#define TMPSENS		53
+#define PMBCTRL		54
+#define HDP		55
+#define LTDC		56
+#define DSI		57
+#define IWDG2		58
+#define USBPHY		59
+#define STGENRO		60
+#define SPI6		61
+#define I2C4		62
+#define I2C6		63
+#define USART1		64
+#define RTCAPB		65
+#define TZC1		66
+#define TZPC		67
+#define IWDG1		68
+#define BSEC		69
+#define STGEN		70
+#define DMA1		71
+#define DMA2		72
+#define DMAMUX		73
+#define ADC12		74
+#define USBO		75
+#define SDMMC3		76
+#define DCMI		77
+#define CRYP2		78
+#define HASH2		79
+#define RNG2		80
+#define CRC2		81
+#define HSEM		82
+#define IPCC		83
+#define GPIOA		84
+#define GPIOB		85
+#define GPIOC		86
+#define GPIOD		87
+#define GPIOE		88
+#define GPIOF		89
+#define GPIOG		90
+#define GPIOH		91
+#define GPIOI		92
+#define GPIOJ		93
+#define GPIOK		94
+#define GPIOZ		95
+#define CRYP1		96
+#define HASH1		97
+#define RNG1		98
+#define BKPSRAM		99
+#define MDMA		100
+#define GPU		101
+#define ETHCK		102
+#define ETHTX		103
+#define ETHRX		104
+#define ETHMAC		105
+#define FMC		106
+#define QSPI		107
+#define SDMMC1		108
+#define SDMMC2		109
+#define CRC1		110
+#define USBH		111
+#define ETHSTP		112
+#define TZC2		113
+
+/* Kernel clocks */
+#define SDMMC1_K	118
+#define SDMMC2_K	119
+#define SDMMC3_K	120
+#define FMC_K		121
+#define QSPI_K		122
+#define ETHCK_K		123
+#define RNG1_K		124
+#define RNG2_K		125
+#define GPU_K		126
+#define USBPHY_K	127
+#define STGEN_K		128
+#define SPDIF_K		129
+#define SPI1_K		130
+#define SPI2_K		131
+#define SPI3_K		132
+#define SPI4_K		133
+#define SPI5_K		134
+#define SPI6_K		135
+#define CEC_K		136
+#define I2C1_K		137
+#define I2C2_K		138
+#define I2C3_K		139
+#define I2C4_K		140
+#define I2C5_K		141
+#define I2C6_K		142
+#define LPTIM1_K	143
+#define LPTIM2_K	144
+#define LPTIM3_K	145
+#define LPTIM4_K	146
+#define LPTIM5_K	147
+#define USART1_K	148
+#define USART2_K	149
+#define USART3_K	150
+#define UART4_K		151
+#define UART5_K		152
+#define USART6_K	153
+#define UART7_K		154
+#define UART8_K		155
+#define DFSDM_K		156
+#define FDCAN_K		157
+#define SAI1_K		158
+#define SAI2_K		159
+#define SAI3_K		160
+#define SAI4_K		161
+#define ADC12_K		162
+#define DSI_K		163
+#define DSI_PX		164
+#define ADFSDM_K	165
+#define USBO_K		166
+#define LTDC_PX		167
+#define DAC12_K		168
+#define ETHPTP_K	169
+
+/* PLL */
+#define PLL1		176
+#define PLL2		177
+#define PLL3		178
+#define PLL4		179
+
+/* ODF */
+#define PLL1_P		180
+#define PLL1_Q		181
+#define PLL1_R		182
+#define PLL2_P		183
+#define PLL2_Q		184
+#define PLL2_R		185
+#define PLL3_P		186
+#define PLL3_Q		187
+#define PLL3_R		188
+#define PLL4_P		189
+#define PLL4_Q		190
+#define PLL4_R		191
+
+/* AUX */
+#define RTC		192
+
+/* MCLK */
+#define CK_PER		193
+#define CK_MPU		194
+#define CK_AXI		195
+#define CK_MCU		196
+
+/* Time base */
+#define TIM2_K		197
+#define TIM3_K		198
+#define TIM4_K		199
+#define TIM5_K		200
+#define TIM6_K		201
+#define TIM7_K		202
+#define TIM12_K		203
+#define TIM13_K		204
+#define TIM14_K		205
+#define TIM1_K		206
+#define TIM8_K		207
+#define TIM15_K		208
+#define TIM16_K		209
+#define TIM17_K		210
+
+/* MCO clocks */
+#define CK_MCO1		211
+#define CK_MCO2		212
+
+/* TRACE & DEBUG clocks */
+#define DBG		213
+#define CK_DBG		214
+#define CK_TRACE	215
+
+/* DDR */
+#define DDRC1		220
+#define DDRC1LP		221
+#define DDRC2		222
+#define DDRC2LP		223
+#define DDRPHYC		224
+#define DDRPHYCLP	225
+#define DDRCAPB		226
+#define DDRCAPBLP	227
+#define AXIDCG		228
+#define DDRPHYCAPB	229
+#define DDRPHYCAPBLP	230
+#define DDRPERFM	231
+
+#define STM32MP1_LAST_CLK 232
+
+#endif /* _DT_BINDINGS_STM32MP1_CLKS_H_ */
diff --git a/core/include/dt-bindings/clock/stm32mp1-clksrc.h b/core/include/dt-bindings/clock/stm32mp1-clksrc.h
new file mode 100644
index 0000000..de7d160
--- /dev/null
+++ b/core/include/dt-bindings/clock/stm32mp1-clksrc.h
@@ -0,0 +1,284 @@
+/*
+ * Copyright (C) 2017, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier:	GPL-2.0+	BSD-3-Clause
+ */
+
+#ifndef _DT_BINDINGS_CLOCK_STM32MP1_CLKSRC_H_
+#define _DT_BINDINGS_CLOCK_STM32MP1_CLKSRC_H_
+
+/* PLL output is enable when x=1, with x=p,q or r */
+#define PQR(p, q, r)	(((p) & 1) | (((q) & 1) << 1) | (((r) & 1) << 2))
+
+/* st,clksrc: mandatory clock source */
+
+#define CLK_MPU_HSI		0x00000200
+#define CLK_MPU_HSE		0x00000201
+#define CLK_MPU_PLL1P		0x00000202
+#define CLK_MPU_PLL1P_DIV	0x00000203
+
+#define CLK_AXI_HSI		0x00000240
+#define CLK_AXI_HSE		0x00000241
+#define CLK_AXI_PLL2P		0x00000242
+
+#define CLK_MCU_HSI		0x00000480
+#define CLK_MCU_HSE		0x00000481
+#define CLK_MCU_CSI		0x00000482
+#define CLK_MCU_PLL3P		0x00000483
+
+#define CLK_PLL12_HSI		0x00000280
+#define CLK_PLL12_HSE		0x00000281
+
+#define CLK_PLL3_HSI		0x00008200
+#define CLK_PLL3_HSE		0x00008201
+#define CLK_PLL3_CSI		0x00008202
+
+#define CLK_PLL4_HSI		0x00008240
+#define CLK_PLL4_HSE		0x00008241
+#define CLK_PLL4_CSI		0x00008242
+#define CLK_PLL4_I2SCKIN	0x00008243
+
+#define CLK_RTC_DISABLED	0x00001400
+#define CLK_RTC_LSE		0x00001401
+#define CLK_RTC_LSI		0x00001402
+#define CLK_RTC_HSE		0x00001403
+
+#define CLK_MCO1_HSI		0x00008000
+#define CLK_MCO1_HSE		0x00008001
+#define CLK_MCO1_CSI		0x00008002
+#define CLK_MCO1_LSI		0x00008003
+#define CLK_MCO1_LSE		0x00008004
+#define CLK_MCO1_DISABLED	0x0000800F
+
+#define CLK_MCO2_MPU		0x00008040
+#define CLK_MCO2_AXI		0x00008041
+#define CLK_MCO2_MCU		0x00008042
+#define CLK_MCO2_PLL4P		0x00008043
+#define CLK_MCO2_HSE		0x00008044
+#define CLK_MCO2_HSI		0x00008045
+#define CLK_MCO2_DISABLED	0x0000804F
+
+/* st,pkcs: peripheral kernel clock source */
+
+#define CLK_I2C12_PCLK1		0x00008C00
+#define CLK_I2C12_PLL4R		0x00008C01
+#define CLK_I2C12_HSI		0x00008C02
+#define CLK_I2C12_CSI		0x00008C03
+#define CLK_I2C12_DISABLED	0x00008C07
+
+#define CLK_I2C35_PCLK1		0x00008C40
+#define CLK_I2C35_PLL4R		0x00008C41
+#define CLK_I2C35_HSI		0x00008C42
+#define CLK_I2C35_CSI		0x00008C43
+#define CLK_I2C35_DISABLED	0x00008C47
+
+#define CLK_I2C46_PCLK5		0x00000C00
+#define CLK_I2C46_PLL3Q		0x00000C01
+#define CLK_I2C46_HSI		0x00000C02
+#define CLK_I2C46_CSI		0x00000C03
+#define CLK_I2C46_DISABLED	0x00000C07
+
+#define CLK_SAI1_PLL4Q		0x00008C80
+#define CLK_SAI1_PLL3Q		0x00008C81
+#define CLK_SAI1_I2SCKIN	0x00008C82
+#define CLK_SAI1_CKPER		0x00008C83
+#define CLK_SAI1_PLL3R		0x00008C84
+#define CLK_SAI1_DISABLED	0x00008C87
+
+#define CLK_SAI2_PLL4Q		0x00008CC0
+#define CLK_SAI2_PLL3Q		0x00008CC1
+#define CLK_SAI2_I2SCKIN	0x00008CC2
+#define CLK_SAI2_CKPER		0x00008CC3
+#define CLK_SAI2_SPDIF		0x00008CC4
+#define CLK_SAI2_PLL3R		0x00008CC5
+#define CLK_SAI2_DISABLED	0x00008CC7
+
+#define CLK_SAI3_PLL4Q		0x00008D00
+#define CLK_SAI3_PLL3Q		0x00008D01
+#define CLK_SAI3_I2SCKIN	0x00008D02
+#define CLK_SAI3_CKPER		0x00008D03
+#define CLK_SAI3_PLL3R		0x00008D04
+#define CLK_SAI3_DISABLED	0x00008D07
+
+#define CLK_SAI4_PLL4Q		0x00008D40
+#define CLK_SAI4_PLL3Q		0x00008D41
+#define CLK_SAI4_I2SCKIN	0x00008D42
+#define CLK_SAI4_CKPER		0x00008D43
+#define CLK_SAI4_PLL3R		0x00008D44
+#define CLK_SAI4_DISABLED	0x00008D47
+
+#define CLK_SPI2S1_PLL4P	0x00008D80
+#define CLK_SPI2S1_PLL3Q	0x00008D81
+#define CLK_SPI2S1_I2SCKIN	0x00008D82
+#define CLK_SPI2S1_CKPER	0x00008D83
+#define CLK_SPI2S1_PLL3R	0x00008D84
+#define CLK_SPI2S1_DISABLED	0x00008D87
+
+#define CLK_SPI2S23_PLL4P	0x00008DC0
+#define CLK_SPI2S23_PLL3Q	0x00008DC1
+#define CLK_SPI2S23_I2SCKIN	0x00008DC2
+#define CLK_SPI2S23_CKPER	0x00008DC3
+#define CLK_SPI2S23_PLL3R	0x00008DC4
+#define CLK_SPI2S23_DISABLED	0x00008DC7
+
+#define CLK_SPI45_PCLK2		0x00008E00
+#define CLK_SPI45_PLL4Q		0x00008E01
+#define CLK_SPI45_HSI		0x00008E02
+#define CLK_SPI45_CSI		0x00008E03
+#define CLK_SPI45_HSE		0x00008E04
+#define CLK_SPI45_DISABLED	0x00008E07
+
+#define CLK_SPI6_PCLK5		0x00000C40
+#define CLK_SPI6_PLL4Q		0x00000C41
+#define CLK_SPI6_HSI		0x00000C42
+#define CLK_SPI6_CSI		0x00000C43
+#define CLK_SPI6_HSE		0x00000C44
+#define CLK_SPI6_PLL3Q		0x00000C45
+#define CLK_SPI6_DISABLED	0x00000C47
+
+#define CLK_UART6_PCLK2		0x00008E40
+#define CLK_UART6_PLL4Q		0x00008E41
+#define CLK_UART6_HSI		0x00008E42
+#define CLK_UART6_CSI		0x00008E43
+#define CLK_UART6_HSE		0x00008E44
+#define CLK_UART6_DISABLED	0x00008E47
+
+#define CLK_UART24_PCLK1	0x00008E80
+#define CLK_UART24_PLL4Q	0x00008E81
+#define CLK_UART24_HSI		0x00008E82
+#define CLK_UART24_CSI		0x00008E83
+#define CLK_UART24_HSE		0x00008E84
+#define CLK_UART24_DISABLED	0x00008E87
+
+#define CLK_UART35_PCLK1	0x00008EC0
+#define CLK_UART35_PLL4Q	0x00008EC1
+#define CLK_UART35_HSI		0x00008EC2
+#define CLK_UART35_CSI		0x00008EC3
+#define CLK_UART35_HSE		0x00008EC4
+#define CLK_UART35_DISABLED	0x00008EC7
+
+#define CLK_UART78_PCLK1	0x00008F00
+#define CLK_UART78_PLL4Q	0x00008F01
+#define CLK_UART78_HSI		0x00008F02
+#define CLK_UART78_CSI		0x00008F03
+#define CLK_UART78_HSE		0x00008F04
+#define CLK_UART78_DISABLED	0x00008F07
+
+#define CLK_UART1_PCLK5		0x00000C80
+#define CLK_UART1_PLL3Q		0x00000C81
+#define CLK_UART1_HSI		0x00000C82
+#define CLK_UART1_CSI		0x00000C83
+#define CLK_UART1_PLL4Q		0x00000C84
+#define CLK_UART1_HSE		0x00000C85
+#define CLK_UART1_DISABLED	0x00000C87
+
+#define CLK_SDMMC12_HCLK6	0x00008F40
+#define CLK_SDMMC12_PLL3R	0x00008F41
+#define CLK_SDMMC12_PLL4P	0x00008F42
+#define CLK_SDMMC12_HSI		0x00008F43
+#define CLK_SDMMC12_DISABLED	0x00008F47
+
+#define CLK_SDMMC3_HCLK2	0x00008F80
+#define CLK_SDMMC3_PLL3R	0x00008F81
+#define CLK_SDMMC3_PLL4P	0x00008F82
+#define CLK_SDMMC3_HSI		0x00008F83
+#define CLK_SDMMC3_DISABLED	0x00008F87
+
+#define CLK_ETH_PLL4P		0x00008FC0
+#define CLK_ETH_PLL3Q		0x00008FC1
+#define CLK_ETH_DISABLED	0x00008FC3
+
+#define CLK_QSPI_ACLK		0x00009000
+#define CLK_QSPI_PLL3R		0x00009001
+#define CLK_QSPI_PLL4P		0x00009002
+#define CLK_QSPI_CKPER		0x00009003
+
+#define CLK_FMC_ACLK		0x00009040
+#define CLK_FMC_PLL3R		0x00009041
+#define CLK_FMC_PLL4P		0x00009042
+#define CLK_FMC_CKPER		0x00009043
+
+#define CLK_FDCAN_HSE		0x000090C0
+#define CLK_FDCAN_PLL3Q		0x000090C1
+#define CLK_FDCAN_PLL4Q		0x000090C2
+#define CLK_FDCAN_PLL4R		0x000090C3
+
+#define CLK_SPDIF_PLL4P		0x00009140
+#define CLK_SPDIF_PLL3Q		0x00009141
+#define CLK_SPDIF_HSI		0x00009142
+#define CLK_SPDIF_DISABLED	0x00009143
+
+#define CLK_CEC_LSE		0x00009180
+#define CLK_CEC_LSI		0x00009181
+#define CLK_CEC_CSI_DIV122	0x00009182
+#define CLK_CEC_DISABLED	0x00009183
+
+#define CLK_USBPHY_HSE		0x000091C0
+#define CLK_USBPHY_PLL4R	0x000091C1
+#define CLK_USBPHY_HSE_DIV2	0x000091C2
+#define CLK_USBPHY_DISABLED	0x000091C3
+
+#define CLK_USBO_PLL4R		0x800091C0
+#define CLK_USBO_USBPHY		0x800091C1
+
+#define CLK_RNG1_CSI		0x00000CC0
+#define CLK_RNG1_PLL4R		0x00000CC1
+#define CLK_RNG1_LSE		0x00000CC2
+#define CLK_RNG1_LSI		0x00000CC3
+
+#define CLK_RNG2_CSI		0x00009200
+#define CLK_RNG2_PLL4R		0x00009201
+#define CLK_RNG2_LSE		0x00009202
+#define CLK_RNG2_LSI		0x00009203
+
+#define CLK_CKPER_HSI		0x00000D00
+#define CLK_CKPER_CSI		0x00000D01
+#define CLK_CKPER_HSE		0x00000D02
+#define CLK_CKPER_DISABLED	0x00000D03
+
+#define CLK_STGEN_HSI		0x00000D40
+#define CLK_STGEN_HSE		0x00000D41
+#define CLK_STGEN_DISABLED	0x00000D43
+
+#define CLK_DSI_DSIPLL		0x00009240
+#define CLK_DSI_PLL4P		0x00009241
+
+#define CLK_ADC_PLL4R		0x00009280
+#define CLK_ADC_CKPER		0x00009281
+#define CLK_ADC_PLL3Q		0x00009282
+#define CLK_ADC_DISABLED	0x00009283
+
+#define CLK_LPTIM45_PCLK3	0x000092C0
+#define CLK_LPTIM45_PLL4P	0x000092C1
+#define CLK_LPTIM45_PLL3Q	0x000092C2
+#define CLK_LPTIM45_LSE		0x000092C3
+#define CLK_LPTIM45_LSI		0x000092C4
+#define CLK_LPTIM45_CKPER	0x000092C5
+#define CLK_LPTIM45_DISABLED	0x000092C7
+
+#define CLK_LPTIM23_PCLK3	0x00009300
+#define CLK_LPTIM23_PLL4Q	0x00009301
+#define CLK_LPTIM23_CKPER	0x00009302
+#define CLK_LPTIM23_LSE		0x00009303
+#define CLK_LPTIM23_LSI		0x00009304
+#define CLK_LPTIM23_DISABLED	0x00009307
+
+#define CLK_LPTIM1_PCLK1	0x00009340
+#define CLK_LPTIM1_PLL4P	0x00009341
+#define CLK_LPTIM1_PLL3Q	0x00009342
+#define CLK_LPTIM1_LSE		0x00009343
+#define CLK_LPTIM1_LSI		0x00009344
+#define CLK_LPTIM1_CKPER	0x00009345
+#define CLK_LPTIM1_DISABLED	0x00009347
+
+/* define for st,pll /csg */
+#define SSCG_MODE_CENTER_SPREAD	0
+#define SSCG_MODE_DOWN_SPREAD	1
+
+/* define for st,drive */
+#define LSEDRV_LOWEST		0
+#define LSEDRV_MEDIUM_LOW	1
+#define LSEDRV_MEDIUM_HIGH	2
+#define LSEDRV_HIGHEST		3
+
+#endif
diff --git a/core/include/dt-bindings/etzpc/stm32-etzpc.h b/core/include/dt-bindings/etzpc/stm32-etzpc.h
new file mode 100644
index 0000000..f498651
--- /dev/null
+++ b/core/include/dt-bindings/etzpc/stm32-etzpc.h
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) 2017, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier:	GPL-2.0+	BSD-3-Clause
+ */
+
+#ifndef _DT_BINDINGS_STM32_ETZPC_H
+#define _DT_BINDINGS_STM32_ETZPC_H
+
+/*  define DECPROT modes */
+#define DECPROT_S_RW		0x0
+#define DECPROT_NS_R_S_W	0x1
+#define DECPROT_MCU_ISOLATION	0x2
+#define DECPROT_NS_RW		0x3
+
+/*  define DECPROT lock */
+#define DECPROT_UNLOCK		0x0
+#define DECPROT_LOCK		0x1
+
+/* define ETZPC ID */
+#define STM32MP1_ETZPC_STGENC_ID	0
+#define STM32MP1_ETZPC_BKPSRAM_ID	1
+#define STM32MP1_ETZPC_IWDG1_ID		2
+#define STM32MP1_ETZPC_USART1_ID	3
+#define STM32MP1_ETZPC_SPI6_ID		4
+#define STM32MP1_ETZPC_I2C4_ID		5
+#define STM32MP1_ETZPC_GPIOZ_ID		6
+#define STM32MP1_ETZPC_RNG1_ID		7
+#define STM32MP1_ETZPC_HASH1_ID		8
+#define STM32MP1_ETZPC_CRYP1_ID		9
+#define STM32MP1_ETZPC_DDRCTRL_ID	10
+#define STM32MP1_ETZPC_DDRPHYC_ID	11
+#define STM32MP1_ETZPC_I2C6_ID		12
+#define STM32MP1_ETZPC_TIM2_ID		16
+#define STM32MP1_ETZPC_TIM3_ID		17
+#define STM32MP1_ETZPC_TIM4_ID		18
+#define STM32MP1_ETZPC_TIM5_ID		19
+#define STM32MP1_ETZPC_TIM6_ID		20
+#define STM32MP1_ETZPC_TIM7_ID		21
+#define STM32MP1_ETZPC_TIM12_ID		22
+#define STM32MP1_ETZPC_TIM13_ID		23
+#define STM32MP1_ETZPC_TIM14_ID		24
+#define STM32MP1_ETZPC_LPTIM1_ID	25
+#define STM32MP1_ETZPC_WWDG1_ID		26
+#define STM32MP1_ETZPC_SPI2_ID		27
+#define STM32MP1_ETZPC_SPI3_ID		28
+#define STM32MP1_ETZPC_SPDIFRX_ID	29
+#define STM32MP1_ETZPC_USART2_ID	30
+#define STM32MP1_ETZPC_USART3_ID	31
+#define STM32MP1_ETZPC_UART4_ID		32
+#define STM32MP1_ETZPC_UART5_ID		33
+#define STM32MP1_ETZPC_I2C1_ID		34
+#define STM32MP1_ETZPC_I2C2_ID		35
+#define STM32MP1_ETZPC_I2C3_ID		36
+#define STM32MP1_ETZPC_I2C5_ID		37
+#define STM32MP1_ETZPC_CEC_ID		38
+#define STM32MP1_ETZPC_DAC_ID		39
+#define STM32MP1_ETZPC_UART7_ID		40
+#define STM32MP1_ETZPC_UART8_ID		41
+#define STM32MP1_ETZPC_MDIOS_ID		44
+#define STM32MP1_ETZPC_TIM1_ID		48
+#define STM32MP1_ETZPC_TIM8_ID		49
+#define STM32MP1_ETZPC_USART6_ID	51
+#define STM32MP1_ETZPC_SPI1_ID		52
+#define STM32MP1_ETZPC_SPI4_ID		53
+#define STM32MP1_ETZPC_TIM15_ID		54
+#define STM32MP1_ETZPC_TIM16_ID		55
+#define STM32MP1_ETZPC_TIM17_ID		56
+#define STM32MP1_ETZPC_SPI5_ID		57
+#define STM32MP1_ETZPC_SAI1_ID		58
+#define STM32MP1_ETZPC_SAI2_ID		59
+#define STM32MP1_ETZPC_SAI3_ID		60
+#define STM32MP1_ETZPC_DFSDM_ID		61
+#define STM32MP1_ETZPC_TT_FDCAN_ID	62
+#define STM32MP1_ETZPC_LPTIM2_ID	64
+#define STM32MP1_ETZPC_LPTIM3_ID	65
+#define STM32MP1_ETZPC_LPTIM4_ID	66
+#define STM32MP1_ETZPC_LPTIM5_ID	67
+#define STM32MP1_ETZPC_SAI4_ID		68
+#define STM32MP1_ETZPC_VREFBUF_ID	69
+#define STM32MP1_ETZPC_DCMI_ID		70
+#define STM32MP1_ETZPC_CRC2_ID		71
+#define STM32MP1_ETZPC_ADC_ID		72
+#define STM32MP1_ETZPC_HASH2_ID		73
+#define STM32MP1_ETZPC_RNG2_ID		74
+#define STM32MP1_ETZPC_CRYP2_ID		75
+#define STM32MP1_ETZPC_SRAM1_ID		80
+#define STM32MP1_ETZPC_SRAM2_ID		81
+#define STM32MP1_ETZPC_SRAM3_ID		82
+#define STM32MP1_ETZPC_SRAM4_ID		83
+#define STM32MP1_ETZPC_RETRAM_ID	84
+#define STM32MP1_ETZPC_OTG_ID		85
+#define STM32MP1_ETZPC_SDMMC3_ID	86
+#define STM32MP1_ETZPC_DLYBSD3_ID	87
+#define STM32MP1_ETZPC_DMA1_ID		88
+#define STM32MP1_ETZPC_DMA2_ID		89
+#define STM32MP1_ETZPC_DMAMUX_ID	90
+#define STM32MP1_ETZPC_FMC_ID		91
+#define STM32MP1_ETZPC_QSPI_ID		92
+#define STM32MP1_ETZPC_DLYBQ_ID		93
+#define STM32MP1_ETZPC_ETH_ID		94
+
+#define STM32MP1_ETZPC_MAX_ID		96
+
+#define DECPROT(ip_id, mode, lock) ((ip_id << 16) | (mode << 8) | lock)
+
+#endif /* _DT_BINDINGS_STM32_ETZPC_H */
+
diff --git a/core/include/dt-bindings/interrupt-controller/arm-gic.h b/core/include/dt-bindings/interrupt-controller/arm-gic.h
new file mode 100644
index 0000000..6863d5e
--- /dev/null
+++ b/core/include/dt-bindings/interrupt-controller/arm-gic.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0 or BSD-3-Clause */
+/*
+ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef _DT_BINDINGS_INTERRUPT_CONTROLLER_ARM_GIC_H
+#define _DT_BINDINGS_INTERRUPT_CONTROLLER_ARM_GIC_H
+
+/* Interrupt specifier cell 0 */
+
+#define GIC_SPI 0
+#define GIC_PPI 1
+
+#define IRQ_TYPE_NONE		0
+#define IRQ_TYPE_EDGE_RISING	1
+#define IRQ_TYPE_EDGE_FALLING	2
+#define IRQ_TYPE_EDGE_BOTH	(IRQ_TYPE_EDGE_FALLING | IRQ_TYPE_EDGE_RISING)
+#define IRQ_TYPE_LEVEL_HIGH	4
+#define IRQ_TYPE_LEVEL_LOW	8
+
+#endif
diff --git a/core/include/dt-bindings/pinctrl/stm32-pinfunc.h b/core/include/dt-bindings/pinctrl/stm32-pinfunc.h
new file mode 100644
index 0000000..7f6e4b9
--- /dev/null
+++ b/core/include/dt-bindings/pinctrl/stm32-pinfunc.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause) */
+/*
+ * Copyright (C) STMicroelectronics 2017 - All Rights Reserved
+ * Author: Torgue Alexandre <alexandre.torgue@st.com> for STMicroelectronics.
+ */
+
+#ifndef _DT_BINDINGS_STM32_PINFUNC_H
+#define _DT_BINDINGS_STM32_PINFUNC_H
+
+/*  define PIN modes */
+#define GPIO	0x0
+#define AF0	0x1
+#define AF1	0x2
+#define AF2	0x3
+#define AF3	0x4
+#define AF4	0x5
+#define AF5	0x6
+#define AF6	0x7
+#define AF7	0x8
+#define AF8	0x9
+#define AF9	0xa
+#define AF10	0xb
+#define AF11	0xc
+#define AF12	0xd
+#define AF13	0xe
+#define AF14	0xf
+#define AF15	0x10
+#define ANALOG	0x11
+
+/* define Pins number*/
+#define PIN_NO(port, line)	(((port) - 'A') * 0x10 + (line))
+
+#define STM32_PINMUX(port, line, mode) (((PIN_NO(port, line)) << 8) | (mode))
+
+/*  package information */
+#define STM32MP157CAA	0x1
+#define STM32MP157CAB	0x2
+#define STM32MP157CAC	0x4
+#define STM32MP157CAD	0x8
+
+#endif /* _DT_BINDINGS_STM32_PINFUNC_H */
diff --git a/core/include/dt-bindings/power/stm32mp1-power.h b/core/include/dt-bindings/power/stm32mp1-power.h
new file mode 100644
index 0000000..bfb7f78
--- /dev/null
+++ b/core/include/dt-bindings/power/stm32mp1-power.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0 or BSD-3-Clause */
+/*
+ * Copyright (C) STMicroelectronics 2018 - All Rights Reserved
+ * Author: Yann Gautier <yann.gautier@st.com> for STMicroelectronics.
+ */
+
+#ifndef DT_BINDINGS_STM32MP1_POWER_H
+#define DT_BINDINGS_STM32MP1_POWER_H
+
+#define STM32_PM_CSLEEP_RUN			0
+#define STM32_PM_CSTOP_ALLOW_STOP		1
+#define STM32_PM_CSTOP_ALLOW_LP_STOP		2
+#define STM32_PM_CSTOP_ALLOW_LPLV_STOP		3
+#define STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR	4
+#define STM32_PM_CSTOP_ALLOW_STANDBY_DDR_OFF	5
+#define STM32_PM_SHUTDOWN			6
+#define STM32_PM_MAX_SOC_MODE			7
+
+#endif /* DT_BINDINGS_STM32MP1_POWER_H */
diff --git a/core/include/dt-bindings/reset/stm32mp1-resets.h b/core/include/dt-bindings/reset/stm32mp1-resets.h
new file mode 100644
index 0000000..f0c3aae
--- /dev/null
+++ b/core/include/dt-bindings/reset/stm32mp1-resets.h
@@ -0,0 +1,108 @@
+/* SPDX-License-Identifier: GPL-2.0 or BSD-3-Clause */
+/*
+ * Copyright (C) STMicroelectronics 2018 - All Rights Reserved
+ * Author: Gabriel Fernandez <gabriel.fernandez@st.com> for STMicroelectronics.
+ */
+
+#ifndef _DT_BINDINGS_STM32MP1_RESET_H_
+#define _DT_BINDINGS_STM32MP1_RESET_H_
+
+#define LTDC_R		3072
+#define DSI_R		3076
+#define DDRPERFM_R	3080
+#define USBPHY_R	3088
+#define SPI6_R		3136
+#define I2C4_R		3138
+#define I2C6_R		3139
+#define USART1_R	3140
+#define STGEN_R		3156
+#define GPIOZ_R		3200
+#define CRYP1_R		3204
+#define HASH1_R		3205
+#define RNG1_R		3206
+#define AXIM_R		3216
+#define GPU_R		3269
+#define ETHMAC_R	3274
+#define FMC_R		3276
+#define QSPI_R		3278
+#define SDMMC1_R	3280
+#define SDMMC2_R	3281
+#define CRC1_R		3284
+#define USBH_R		3288
+#define MDMA_R		3328
+#define MCU_R		8225
+#define TIM2_R		19456
+#define TIM3_R		19457
+#define TIM4_R		19458
+#define TIM5_R		19459
+#define TIM6_R		19460
+#define TIM7_R		19461
+#define TIM12_R		16462
+#define TIM13_R		16463
+#define TIM14_R		16464
+#define LPTIM1_R	19465
+#define SPI2_R		19467
+#define SPI3_R		19468
+#define USART2_R	19470
+#define USART3_R	19471
+#define UART4_R		19472
+#define UART5_R		19473
+#define UART7_R		19474
+#define UART8_R		19475
+#define I2C1_R		19477
+#define I2C2_R		19478
+#define I2C3_R		19479
+#define I2C5_R		19480
+#define SPDIF_R		19482
+#define CEC_R		19483
+#define DAC12_R		19485
+#define MDIO_R		19847
+#define TIM1_R		19520
+#define TIM8_R		19521
+#define TIM15_R		19522
+#define TIM16_R		19523
+#define TIM17_R		19524
+#define SPI1_R		19528
+#define SPI4_R		19529
+#define SPI5_R		19530
+#define USART6_R	19533
+#define SAI1_R		19536
+#define SAI2_R		19537
+#define SAI3_R		19538
+#define DFSDM_R		19540
+#define FDCAN_R		19544
+#define LPTIM2_R	19584
+#define LPTIM3_R	19585
+#define LPTIM4_R	19586
+#define LPTIM5_R	19587
+#define SAI4_R		19592
+#define SYSCFG_R	19595
+#define VREF_R		19597
+#define TMPSENS_R	19600
+#define PMBCTRL_R	19601
+#define DMA1_R		19648
+#define DMA2_R		19649
+#define DMAMUX_R	19650
+#define ADC12_R		19653
+#define USBO_R		19656
+#define SDMMC3_R	19664
+#define CAMITF_R	19712
+#define CRYP2_R		19716
+#define HASH2_R		19717
+#define RNG2_R		19718
+#define CRC2_R		19719
+#define HSEM_R		19723
+#define MBOX_R		19724
+#define GPIOA_R		19776
+#define GPIOB_R		19777
+#define GPIOC_R		19778
+#define GPIOD_R		19779
+#define GPIOE_R		19780
+#define GPIOF_R		19781
+#define GPIOG_R		19782
+#define GPIOH_R		19783
+#define GPIOI_R		19784
+#define GPIOJ_R		19785
+#define GPIOK_R		19786
+
+#endif /* _DT_BINDINGS_STM32MP1_RESET_H_ */
diff --git a/core/include/kernel/interrupt.h b/core/include/kernel/interrupt.h
index d7b8abd..5a7e18d 100644
--- a/core/include/kernel/interrupt.h
+++ b/core/include/kernel/interrupt.h
@@ -23,6 +23,11 @@ struct itr_ops {
 		uint8_t cpu_mask);
 	void (*set_affinity)(struct itr_chip *chip, size_t it,
 		uint8_t cpu_mask);
+#if !defined(CFG_ARM_GICV3)
+	uint8_t (*set_pmr)(struct itr_chip *chip, uint8_t mask);
+	uint8_t (*set_ipriority)(struct itr_chip *chip, size_t it,
+					uint8_t mask);
+#endif
 };
 
 enum itr_return {
@@ -57,4 +62,14 @@ void itr_raise_sgi(size_t it, uint8_t cpu_mask);
  */
 void itr_set_affinity(size_t it, uint8_t cpu_mask);
 
+/*
+ * Set the Priority Mask Regarding and return its previous value
+ */
+uint8_t itr_set_pmr(uint8_t mask);
+
+/*
+ * Set the targe tinterrupt priority mask and return its previous value
+ */
+uint8_t itr_set_ipriority(size_t it, uint8_t mask);
+
 #endif /*__KERNEL_INTERRUPT_H*/
diff --git a/core/kernel/console.c b/core/kernel/console.c
index 1f11503..98fde7c 100644
--- a/core/kernel/console.c
+++ b/core/kernel/console.c
@@ -65,8 +65,13 @@ void configure_console_from_dt(void)
 		return;
 
 	offs = fdt_path_offset(fdt, "/secure-chosen");
-	if (offs < 0)
-		return;
+	if (offs < 0) {
+		/* Fallback to node /chosen */
+		offs = fdt_path_offset(fdt, "/chosen");
+		if (offs < 0) {
+			return;
+		}
+	}
 	prop = fdt_get_property(fdt, offs, "stdout-path", NULL);
 	if (!prop) {
 		/*
diff --git a/core/kernel/interrupt.c b/core/kernel/interrupt.c
index cff1f80..ce27aca 100644
--- a/core/kernel/interrupt.c
+++ b/core/kernel/interrupt.c
@@ -78,3 +78,13 @@ void itr_set_affinity(size_t it, uint8_t cpu_mask)
 {
 	itr_chip->ops->set_affinity(itr_chip, it, cpu_mask);
 }
+
+uint8_t itr_set_pmr(uint8_t mask)
+{
+	return itr_chip->ops->set_pmr(itr_chip, mask);
+}
+
+uint8_t itr_set_ipriority(size_t it, uint8_t mask)
+{
+	return itr_chip->ops->set_ipriority(itr_chip, it, mask);
+}
diff --git a/core/lib/libfdt/fdt_ro.c b/core/lib/libfdt/fdt_ro.c
index 34110da..79b1b6d 100644
--- a/core/lib/libfdt/fdt_ro.c
+++ b/core/lib/libfdt/fdt_ro.c
@@ -89,6 +89,32 @@ static int _fdt_string_eq(const void *fdt, int stroffset,
 	return (strlen(p) == len) && (memcmp(p, s, len) == 0);
 }
 
+uint32_t fdt_get_max_phandle(const void *fdt)
+{
+	uint32_t max_phandle = 0;
+	int offset;
+
+	for (offset = fdt_next_node(fdt, -1, NULL);;
+	     offset = fdt_next_node(fdt, offset, NULL)) {
+		uint32_t phandle;
+
+		if (offset == -FDT_ERR_NOTFOUND)
+			return max_phandle;
+
+		if (offset < 0)
+			return (uint32_t)-1;
+
+		phandle = fdt_get_phandle(fdt, offset);
+		if (phandle == (uint32_t)-1)
+			continue;
+
+		if (phandle > max_phandle)
+			max_phandle = phandle;
+	}
+
+	return 0;
+}
+
 int fdt_get_mem_rsv(const void *fdt, int n, uint64_t *address, uint64_t *size)
 {
 	FDT_CHECK_HEADER(fdt);
@@ -155,9 +181,9 @@ int fdt_subnode_offset(const void *fdt, int parentoffset,
 	return fdt_subnode_offset_namelen(fdt, parentoffset, name, strlen(name));
 }
 
-int fdt_path_offset(const void *fdt, const char *path)
+int fdt_path_offset_namelen(const void *fdt, const char *path, int namelen)
 {
-	const char *end = path + strlen(path);
+	const char *end = path + namelen;
 	const char *p = path;
 	int offset = 0;
 
@@ -165,7 +191,7 @@ int fdt_path_offset(const void *fdt, const char *path)
 
 	/* see if we have an alias */
 	if (*path != '/') {
-		const char *q = strchr(path, '/');
+		const char *q = memchr(path, '/', end - p);
 
 		if (!q)
 			q = end;
@@ -178,14 +204,15 @@ int fdt_path_offset(const void *fdt, const char *path)
 		p = q;
 	}
 
-	while (*p) {
+	while (p < end) {
 		const char *q;
 
-		while (*p == '/')
+		while (*p == '/') {
 			p++;
-		if (! *p)
-			return offset;
-		q = strchr(p, '/');
+			if (p == end)
+				return offset;
+		}
+		q = memchr(p, '/', end - p);
 		if (! q)
 			q = end;
 
@@ -199,6 +226,11 @@ int fdt_path_offset(const void *fdt, const char *path)
 	return offset;
 }
 
+int fdt_path_offset(const void *fdt, const char *path)
+{
+	return fdt_path_offset_namelen(fdt, path, strlen(path));
+}
+
 const char *fdt_get_name(const void *fdt, int nodeoffset, int *len)
 {
 	const struct fdt_node_header *nh = _fdt_offset_ptr(fdt, nodeoffset);
@@ -533,6 +565,106 @@ int fdt_stringlist_contains(const char *strlist, int listlen, const char *str)
 	return 0;
 }
 
+int fdt_stringlist_count(const void *fdt, int nodeoffset, const char *property)
+{
+	const char *list, *end;
+	int length, count = 0;
+
+	list = fdt_getprop(fdt, nodeoffset, property, &length);
+	if (!list)
+		return -length;
+
+	end = list + length;
+
+	while (list < end) {
+		length = strnlen(list, end - list) + 1;
+
+		/* Abort if the last string isn't properly NUL-terminated. */
+		if (list + length > end)
+			return -FDT_ERR_BADVALUE;
+
+		list += length;
+		count++;
+	}
+
+	return count;
+}
+
+int fdt_stringlist_search(const void *fdt, int nodeoffset, const char *property,
+			  const char *string)
+{
+	int length, len, idx = 0;
+	const char *list, *end;
+
+	list = fdt_getprop(fdt, nodeoffset, property, &length);
+	if (!list)
+		return -length;
+
+	len = strlen(string) + 1;
+	end = list + length;
+
+	while (list < end) {
+		length = strnlen(list, end - list) + 1;
+
+		/* Abort if the last string isn't properly NUL-terminated. */
+		if (list + length > end)
+			return -FDT_ERR_BADVALUE;
+
+		if (length == len && memcmp(list, string, length) == 0)
+			return idx;
+
+		list += length;
+		idx++;
+	}
+
+	return -FDT_ERR_NOTFOUND;
+}
+
+const char *fdt_stringlist_get(const void *fdt, int nodeoffset,
+			       const char *property, int idx,
+			       int *lenp)
+{
+	const char *list, *end;
+	int length;
+
+	list = fdt_getprop(fdt, nodeoffset, property, &length);
+	if (!list) {
+		if (lenp)
+			*lenp = length;
+
+		return NULL;
+	}
+
+	end = list + length;
+
+	while (list < end) {
+		length = strnlen(list, end - list) + 1;
+
+		/* Abort if the last string isn't properly NUL-terminated. */
+		if (list + length > end) {
+			if (lenp)
+				*lenp = -FDT_ERR_BADVALUE;
+
+			return NULL;
+		}
+
+		if (idx == 0) {
+			if (lenp)
+				*lenp = length - 1;
+
+			return list;
+		}
+
+		list += length;
+		idx--;
+	}
+
+	if (lenp)
+		*lenp = -FDT_ERR_NOTFOUND;
+
+	return NULL;
+}
+
 int fdt_node_check_compatible(const void *fdt, int nodeoffset,
 			      const char *compatible)
 {
@@ -542,10 +674,8 @@ int fdt_node_check_compatible(const void *fdt, int nodeoffset,
 	prop = fdt_getprop(fdt, nodeoffset, "compatible", &len);
 	if (!prop)
 		return len;
-	if (fdt_stringlist_contains(prop, len, compatible))
-		return 0;
-	else
-		return 1;
+
+	return !fdt_stringlist_contains(prop, len, compatible);
 }
 
 int fdt_node_offset_by_compatible(const void *fdt, int startoffset,
diff --git a/core/lib/libfdt/fdt_rw.c b/core/lib/libfdt/fdt_rw.c
index 1785a51..15897b7 100644
--- a/core/lib/libfdt/fdt_rw.c
+++ b/core/lib/libfdt/fdt_rw.c
@@ -102,6 +102,8 @@ static int _fdt_splice(void *fdt, void *splicepoint, int oldlen, int newlen)
 
 	if (((p + oldlen) < p) || ((p + oldlen) > end))
 		return -FDT_ERR_BADOFFSET;
+	if ((p < (char *)fdt) || ((end - oldlen + newlen) < (char *)fdt))
+		return -FDT_ERR_BADOFFSET;
 	if ((end - oldlen + newlen) > ((char *)fdt + fdt_totalsize(fdt)))
 		return -FDT_ERR_NOSPACE;
 	memmove(p + newlen, p + oldlen, end - p - oldlen);
@@ -190,17 +192,13 @@ int fdt_add_mem_rsv(void *fdt, uint64_t address, uint64_t size)
 int fdt_del_mem_rsv(void *fdt, int n)
 {
 	struct fdt_reserve_entry *re = _fdt_mem_rsv_w(fdt, n);
-	int err;
 
 	FDT_RW_CHECK_HEADER(fdt);
 
 	if (n >= fdt_num_mem_rsv(fdt))
 		return -FDT_ERR_NOTFOUND;
 
-	err = _fdt_splice_mem_rsv(fdt, re, 1, 0);
-	if (err)
-		return err;
-	return 0;
+	return _fdt_splice_mem_rsv(fdt, re, 1, 0);
 }
 
 static int _fdt_resize_property(void *fdt, int nodeoffset, const char *name,
@@ -286,8 +284,7 @@ int fdt_setprop(void *fdt, int nodeoffset, const char *name,
 	if (err)
 		return err;
 
-	if (len)
-		memcpy(prop->data, val, len);
+	memcpy(prop->data, val, len);
 	return 0;
 }
 
diff --git a/core/lib/libfdt/fdt_wip.c b/core/lib/libfdt/fdt_wip.c
index c97c5f7..ee9df5c 100644
--- a/core/lib/libfdt/fdt_wip.c
+++ b/core/lib/libfdt/fdt_wip.c
@@ -56,21 +56,42 @@
 
 #include "libfdt_internal.h"
 
+int fdt_setprop_inplace_namelen_partial(void *fdt, int nodeoffset,
+					const char *name, int namelen,
+					uint32_t idx, const void *val,
+					int len)
+{
+	void *propval;
+	int proplen;
+
+	propval = fdt_getprop_namelen_w(fdt, nodeoffset, name, namelen,
+					&proplen);
+	if (!propval)
+		return proplen;
+
+	if (proplen < (len + idx))
+		return -FDT_ERR_NOSPACE;
+
+	memcpy((char *)propval + idx, val, len);
+	return 0;
+}
+
 int fdt_setprop_inplace(void *fdt, int nodeoffset, const char *name,
 			const void *val, int len)
 {
-	void *propval;
+	const void *propval;
 	int proplen;
 
-	propval = fdt_getprop_w(fdt, nodeoffset, name, &proplen);
+	propval = fdt_getprop(fdt, nodeoffset, name, &proplen);
 	if (! propval)
 		return proplen;
 
 	if (proplen != len)
 		return -FDT_ERR_NOSPACE;
 
-	memcpy(propval, val, len);
-	return 0;
+	return fdt_setprop_inplace_namelen_partial(fdt, nodeoffset, name,
+						   strlen(name), 0,
+						   val, len);
 }
 
 static void _fdt_nop_region(void *start, int len)
diff --git a/core/lib/libfdt/include/fdt.h b/core/lib/libfdt/include/fdt.h
index 520bdcf..faa8046 100644
--- a/core/lib/libfdt/include/fdt.h
+++ b/core/lib/libfdt/include/fdt.h
@@ -53,8 +53,16 @@
  *     EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
+/*
+ * Portions copyright (c) 2016-2017, ARM Limited and Contributors.
+ * All rights reserved.
+ */
+
 #ifndef __ASSEMBLY__
 
+#include <libfdt_env.h>
+
+
 struct fdt_header {
 	fdt32_t magic;			 /* magic word FDT_MAGIC */
 	fdt32_t totalsize;		 /* total size of DT block */
diff --git a/core/lib/libfdt/include/libfdt.h b/core/lib/libfdt/include/libfdt.h
index 01a9065..5ce9bfb 100644
--- a/core/lib/libfdt/include/libfdt.h
+++ b/core/lib/libfdt/include/libfdt.h
@@ -52,6 +52,11 @@
  *     EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
+/*
+ * Portions copyright (c) 2016-2017, ARM Limited and Contributors.
+ * All rights reserved.
+ */
+
 #include <libfdt_env.h>
 #include <fdt.h>
 
@@ -122,7 +127,12 @@
 	/* FDT_ERR_BADNCELLS: Device tree has a #address-cells, #size-cells
 	 * or similar property with a bad format or value */
 
-#define FDT_ERR_MAX		14
+#define FDT_ERR_BADVALUE	15
+	/* FDT_ERR_BADVALUE: Device tree has a property with an unexpected
+	 * value. For example: a property expected to contain a string list
+	 * is not NUL-terminated within the length of its value. */
+
+#define FDT_ERR_MAX		15
 
 /**********************************************************************/
 /* Low-level functions (you probably don't need these)                */
@@ -164,27 +174,55 @@ int fdt_first_subnode(const void *fdt, int offset);
  */
 int fdt_next_subnode(const void *fdt, int offset);
 
+/**
+ * fdt_for_each_subnode - iterate over all subnodes of a parent
+ *
+ * @node:	child node (int, lvalue)
+ * @fdt:	FDT blob (const void *)
+ * @parent:	parent node (int)
+ *
+ * This is actually a wrapper around a for loop and would be used like so:
+ *
+ *	fdt_for_each_subnode(node, fdt, parent) {
+ *		Use node
+ *		...
+ *	}
+ *
+ *	if ((node < 0) && (node != -FDT_ERR_NOT_FOUND)) {
+ *		Error handling
+ *	}
+ *
+ * Note that this is implemented as a macro and @node is used as
+ * iterator in the loop. The parent variable be constant or even a
+ * literal.
+ *
+ */
+#define fdt_for_each_subnode(node, fdt, parent)		\
+	for (node = fdt_first_subnode(fdt, parent);	\
+	     node >= 0;					\
+	     node = fdt_next_subnode(fdt, node))
+
 /**********************************************************************/
 /* General functions                                                  */
 /**********************************************************************/
 
 #define fdt_get_header(fdt, field) \
 	(fdt32_to_cpu(((const struct fdt_header *)(fdt))->field))
-#define fdt_magic(fdt) 			(fdt_get_header(fdt, magic))
+#define fdt_magic(fdt)			(fdt_get_header(fdt, magic))
 #define fdt_totalsize(fdt)		(fdt_get_header(fdt, totalsize))
 #define fdt_off_dt_struct(fdt)		(fdt_get_header(fdt, off_dt_struct))
 #define fdt_off_dt_strings(fdt)		(fdt_get_header(fdt, off_dt_strings))
 #define fdt_off_mem_rsvmap(fdt)		(fdt_get_header(fdt, off_mem_rsvmap))
 #define fdt_version(fdt)		(fdt_get_header(fdt, version))
-#define fdt_last_comp_version(fdt) 	(fdt_get_header(fdt, last_comp_version))
-#define fdt_boot_cpuid_phys(fdt) 	(fdt_get_header(fdt, boot_cpuid_phys))
-#define fdt_size_dt_strings(fdt) 	(fdt_get_header(fdt, size_dt_strings))
+#define fdt_last_comp_version(fdt)	(fdt_get_header(fdt, last_comp_version))
+#define fdt_boot_cpuid_phys(fdt)	(fdt_get_header(fdt, boot_cpuid_phys))
+#define fdt_size_dt_strings(fdt)	(fdt_get_header(fdt, size_dt_strings))
 #define fdt_size_dt_struct(fdt)		(fdt_get_header(fdt, size_dt_struct))
 
 #define __fdt_set_hdr(name) \
 	static inline void fdt_set_##name(void *fdt, uint32_t val) \
 	{ \
-		struct fdt_header *fdth = (struct fdt_header*)fdt; \
+		struct fdt_header *fdth = (struct fdt_header *)fdt; \
 		fdth->name = cpu_to_fdt32(val); \
 	}
 __fdt_set_hdr(magic)
@@ -255,6 +293,21 @@ int fdt_move(const void *fdt, void *buf, int bufsize);
 const char *fdt_string(const void *fdt, int stroffset);
 
 /**
+ * fdt_get_max_phandle - retrieves the highest phandle in a tree
+ * @fdt: pointer to the device tree blob
+ *
+ * fdt_get_max_phandle retrieves the highest phandle in the given
+ * device tree. This will ignore badly formatted phandles, or phandles
+ * with a value of 0 or -1.
+ *
+ * returns:
+ *      the highest phandle on success
+ *      0, if no phandle was found in the device tree
+ *      -1, if an error occurred
+ */
+uint32_t fdt_get_max_phandle(const void *fdt);
+
+/**
  * fdt_num_mem_rsv - retrieve the number of memory reserve map entries
  * @fdt: pointer to the device tree blob
  *
@@ -314,8 +367,9 @@ int fdt_subnode_offset_namelen(const void *fdt, int parentoffset,
  * returns:
  *	structure block offset of the requested subnode (>=0), on success
  *	-FDT_ERR_NOTFOUND, if the requested subnode does not exist
- *	-FDT_ERR_BADOFFSET, if parentoffset did not point to an FDT_BEGIN_NODE tag
- *      -FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADOFFSET, if parentoffset did not point to an FDT_BEGIN_NODE
+ *		tag
+ *	-FDT_ERR_BADMAGIC,
  *	-FDT_ERR_BADVERSION,
  *	-FDT_ERR_BADSTATE,
  *	-FDT_ERR_BADSTRUCTURE,
@@ -324,6 +378,17 @@ int fdt_subnode_offset_namelen(const void *fdt, int parentoffset,
 int fdt_subnode_offset(const void *fdt, int parentoffset, const char *name);
 
 /**
+ * fdt_path_offset_namelen - find a tree node by its full path
+ * @fdt: pointer to the device tree blob
+ * @path: full path of the node to locate
+ * @namelen: number of characters of path to consider
+ *
+ * Identical to fdt_path_offset(), but only consider the first namelen
+ * characters of path as the path name.
+ */
+int fdt_path_offset_namelen(const void *fdt, const char *path, int namelen);
+
+/**
  * fdt_path_offset - find a tree node by its full path
  * @fdt: pointer to the device tree blob
  * @path: full path of the node to locate
@@ -336,7 +401,8 @@ int fdt_subnode_offset(const void *fdt, int parentoffset, const char *name);
  * address).
  *
  * returns:
- *	structure block offset of the node with the requested path (>=0), on success
+ *	structure block offset of the node with the requested path (>=0), on
+ *		success
  *	-FDT_ERR_BADPATH, given path does not begin with '/' or is invalid
  *	-FDT_ERR_NOTFOUND, if the requested node does not exist
  *      -FDT_ERR_BADMAGIC,
@@ -360,10 +426,12 @@ int fdt_path_offset(const void *fdt, const char *path);
  *
  * returns:
  *	pointer to the node's name, on success
- *		If lenp is non-NULL, *lenp contains the length of that name (>=0)
+ *		If lenp is non-NULL, *lenp contains the length of that name
+ *			(>=0)
  *	NULL, on error
  *		if lenp is non-NULL *lenp contains an error code (<0):
- *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE
+ *			tag
  *		-FDT_ERR_BADMAGIC,
  *		-FDT_ERR_BADVERSION,
  *		-FDT_ERR_BADSTATE, standard meanings
@@ -412,6 +480,33 @@ int fdt_first_property_offset(const void *fdt, int nodeoffset);
 int fdt_next_property_offset(const void *fdt, int offset);
 
 /**
+ * fdt_for_each_property_offset - iterate over all properties of a node
+ *
+ * @property_offset:	property offset (int, lvalue)
+ * @fdt:		FDT blob (const void *)
+ * @node:		node offset (int)
+ *
+ * This is actually a wrapper around a for loop and would be used like so:
+ *
+ *	fdt_for_each_property_offset(property, fdt, node) {
+ *		Use property
+ *		...
+ *	}
+ *
+ *	if ((property < 0) && (property != -FDT_ERR_NOT_FOUND)) {
+ *		Error handling
+ *	}
+ *
+ * Note that this is implemented as a macro and property is used as
+ * iterator in the loop. The node variable can be constant or even a
+ * literal.
+ */
+#define fdt_for_each_property_offset(property, fdt, node)	\
+	for (property = fdt_first_property_offset(fdt, node);	\
+	     property >= 0;					\
+	     property = fdt_next_property_offset(fdt, property))
+
+/**
  * fdt_get_property_by_offset - retrieve the property at a given offset
  * @fdt: pointer to the device tree blob
  * @offset: offset of the property to retrieve
@@ -447,8 +542,8 @@ const struct fdt_property *fdt_get_property_by_offset(const void *fdt,
  * @namelen: number of characters of name to consider
  * @lenp: pointer to an integer variable (will be overwritten) or NULL
  *
- * Identical to fdt_get_property_namelen(), but only examine the first
- * namelen characters of name for matching the property name.
+ * Identical to fdt_get_property(), but only examine the first namelen
+ * characters of name for matching the property name.
  */
 const struct fdt_property *fdt_get_property_namelen(const void *fdt,
 						    int nodeoffset,
@@ -475,7 +570,8 @@ const struct fdt_property *fdt_get_property_namelen(const void *fdt,
  *	NULL, on error
  *		if lenp is non-NULL, *lenp contains an error code (<0):
  *		-FDT_ERR_NOTFOUND, node does not have named property
- *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE
+ *			tag
  *		-FDT_ERR_BADMAGIC,
  *		-FDT_ERR_BADVERSION,
  *		-FDT_ERR_BADSTATE,
@@ -539,6 +635,13 @@ const void *fdt_getprop_by_offset(const void *fdt, int offset,
  */
 const void *fdt_getprop_namelen(const void *fdt, int nodeoffset,
 				const char *name, int namelen, int *lenp);
+static inline void *fdt_getprop_namelen_w(void *fdt, int nodeoffset,
+					  const char *name, int namelen,
+					  int *lenp)
+{
+	return (void *)(uintptr_t)fdt_getprop_namelen(fdt, nodeoffset, name,
+						      namelen, lenp);
+}
 
 /**
  * fdt_getprop - retrieve the value of a given property
@@ -560,7 +663,8 @@ const void *fdt_getprop_namelen(const void *fdt, int nodeoffset,
  *	NULL, on error
  *		if lenp is non-NULL, *lenp contains an error code (<0):
  *		-FDT_ERR_NOTFOUND, node does not have named property
- *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE
+ *			tag
  *		-FDT_ERR_BADMAGIC,
  *		-FDT_ERR_BADVERSION,
  *		-FDT_ERR_BADSTATE,
@@ -632,7 +736,7 @@ const char *fdt_get_alias(const void *fdt, const char *name);
  *	0, on success
  *		buf contains the absolute path of the node at
  *		nodeoffset, as a NUL-terminated string.
- * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
  *	-FDT_ERR_NOSPACE, the path of the given node is longer than (bufsize-1)
  *		characters and will not fit in the given buffer.
  *	-FDT_ERR_BADMAGIC,
@@ -662,11 +766,11 @@ int fdt_get_path(const void *fdt, int nodeoffset, char *buf, int buflen);
  * structure from the start to nodeoffset.
  *
  * returns:
-
  *	structure block offset of the node at node offset's ancestor
  *		of depth supernodedepth (>=0), on success
- * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
-*	-FDT_ERR_NOTFOUND, supernodedepth was greater than the depth of nodeoffset
+ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_NOTFOUND, supernodedepth was greater than the depth of
+ *		nodeoffset
  *	-FDT_ERR_BADMAGIC,
  *	-FDT_ERR_BADVERSION,
  *	-FDT_ERR_BADSTATE,
@@ -688,7 +792,7 @@ int fdt_supernode_atdepth_offset(const void *fdt, int nodeoffset,
  *
  * returns:
  *	depth of the node at nodeoffset (>=0), on success
- * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
  *	-FDT_ERR_BADMAGIC,
  *	-FDT_ERR_BADVERSION,
  *	-FDT_ERR_BADSTATE,
@@ -711,7 +815,7 @@ int fdt_node_depth(const void *fdt, int nodeoffset);
  * returns:
  *	structure block offset of the parent of the node at nodeoffset
  *		(>=0), on success
- * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
  *	-FDT_ERR_BADMAGIC,
  *	-FDT_ERR_BADVERSION,
  *	-FDT_ERR_BADSTATE,
@@ -751,7 +855,7 @@ int fdt_parent_offset(const void *fdt, int nodeoffset);
  *		 on success
  *	-FDT_ERR_NOTFOUND, no node matching the criterion exists in the
  *		tree after startoffset
- * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
  *	-FDT_ERR_BADMAGIC,
  *	-FDT_ERR_BADVERSION,
  *	-FDT_ERR_BADSTATE,
@@ -798,7 +902,7 @@ int fdt_node_offset_by_phandle(const void *fdt, uint32_t phandle);
  *	1, if the node has a 'compatible' property, but it does not list
  *		the given string
  *	-FDT_ERR_NOTFOUND, if the given node has no 'compatible' property
- * 	-FDT_ERR_BADOFFSET, if nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_BADOFFSET, if nodeoffset does not refer to a BEGIN_NODE tag
  *	-FDT_ERR_BADMAGIC,
  *	-FDT_ERR_BADVERSION,
  *	-FDT_ERR_BADSTATE,
@@ -835,7 +939,7 @@ int fdt_node_check_compatible(const void *fdt, int nodeoffset,
  *		 on success
  *	-FDT_ERR_NOTFOUND, no node matching the criterion exists in the
  *		tree after startoffset
- * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
  *	-FDT_ERR_BADMAGIC,
  *	-FDT_ERR_BADVERSION,
  *	-FDT_ERR_BADSTATE,
@@ -858,6 +962,68 @@ int fdt_node_offset_by_compatible(const void *fdt, int startoffset,
  */
 int fdt_stringlist_contains(const char *strlist, int listlen, const char *str);
 
+/**
+ * fdt_stringlist_count - count the number of strings in a string list
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of a tree node
+ * @property: name of the property containing the string list
+ * @return:
+ *   the number of strings in the given property
+ *   -FDT_ERR_BADVALUE if the property value is not NUL-terminated
+ *   -FDT_ERR_NOTFOUND if the property does not exist
+ */
+int fdt_stringlist_count(const void *fdt, int nodeoffset, const char *property);
+
+/**
+ * fdt_stringlist_search - find a string in a string list and return its index
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of a tree node
+ * @property: name of the property containing the string list
+ * @string: string to look up in the string list
+ *
+ * Note that it is possible for this function to succeed on property values
+ * that are not NUL-terminated. That's because the function will stop after
+ * finding the first occurrence of @string. This can for example happen with
+ * small-valued cell properties, such as #address-cells, when searching for
+ * the empty string.
+ *
+ * @return:
+ *   the index of the string in the list of strings
+ *   -FDT_ERR_BADVALUE if the property value is not NUL-terminated
+ *   -FDT_ERR_NOTFOUND if the property does not exist or does not contain
+ *                     the given string
+ */
+int fdt_stringlist_search(const void *fdt, int nodeoffset, const char *property,
+			  const char *string);
+
+/**
+ * fdt_stringlist_get() - obtain the string at a given index in a string list
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of a tree node
+ * @property: name of the property containing the string list
+ * @index: index of the string to return
+ * @lenp: return location for the string length or an error code on failure
+ *
+ * Note that this will successfully extract strings from properties with
+ * non-NUL-terminated values. For example on small-valued cell properties
+ * this function will return the empty string.
+ *
+ * If non-NULL, the length of the string (on success) or a negative error-code
+ * (on failure) will be stored in the integer pointer to by lenp.
+ *
+ * @return:
+ *   A pointer to the string at the given index in the string list or NULL on
+ *   failure. On success the length of the string will be stored in the memory
+ *   location pointed to by the lenp parameter, if non-NULL. On failure one of
+ *   the following negative error codes will be returned in the lenp parameter
+ *   (if non-NULL):
+ *     -FDT_ERR_BADVALUE if the property value is not NUL-terminated
+ *     -FDT_ERR_NOTFOUND if the property does not exist
+ */
+const char *fdt_stringlist_get(const void *fdt, int nodeoffset,
+			       const char *property, int index,
+			       int *lenp);
+
 /**********************************************************************/
 /* Read-only functions (addressing related)                           */
 /**********************************************************************/
@@ -883,7 +1049,8 @@ int fdt_stringlist_contains(const char *strlist, int listlen, const char *str);
  * returns:
  *	0 <= n < FDT_MAX_NCELLS, on success
  *      2, if the node has no #address-cells property
- *      -FDT_ERR_BADNCELLS, if the node has a badly formatted or invalid #address-cells property
+ *      -FDT_ERR_BADNCELLS, if the node has a badly formatted or invalid
+ *		#address-cells property
  *	-FDT_ERR_BADMAGIC,
  *	-FDT_ERR_BADVERSION,
  *	-FDT_ERR_BADSTATE,
@@ -903,7 +1070,8 @@ int fdt_address_cells(const void *fdt, int nodeoffset);
  * returns:
  *	0 <= n < FDT_MAX_NCELLS, on success
  *      2, if the node has no #address-cells property
- *      -FDT_ERR_BADNCELLS, if the node has a badly formatted or invalid #size-cells property
+ *      -FDT_ERR_BADNCELLS, if the node has a badly formatted or invalid
+ *		#size-cells property
  *	-FDT_ERR_BADMAGIC,
  *	-FDT_ERR_BADVERSION,
  *	-FDT_ERR_BADSTATE,
@@ -918,6 +1086,27 @@ int fdt_size_cells(const void *fdt, int nodeoffset);
 /**********************************************************************/
 
 /**
+ * fdt_setprop_inplace_namelen_partial - change a property's value,
+ *                                       but not its size
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @namelen: number of characters of name to consider
+ * @idx: index of the property to change in the array
+ * @val: pointer to data to replace the property value with
+ * @len: length of the property value
+ *
+ * Identical to fdt_setprop_inplace(), but modifies the given property
+ * starting from the given index, and using only the first characters
+ * of the name. It is useful when you want to manipulate only one value of
+ * an array and you have a string that doesn't end with \0.
+ */
+int fdt_setprop_inplace_namelen_partial(void *fdt, int nodeoffset,
+					const char *name, int namelen,
+					uint32_t idx, const void *val,
+					int len);
+
+/**
  * fdt_setprop_inplace - change a property's value, but not its size
  * @fdt: pointer to the device tree blob
  * @nodeoffset: offset of the node whose property to change
@@ -1527,9 +1716,11 @@ int fdt_add_subnode_namelen(void *fdt, int parentoffset,
  * change the offsets of some existing nodes.
 
  * returns:
- *	structure block offset of the created nodeequested subnode (>=0), on success
+ *	structure block offset of the created nodeequested subnode (>=0), on
+ *		success
  *	-FDT_ERR_NOTFOUND, if the requested subnode does not exist
- *	-FDT_ERR_BADOFFSET, if parentoffset did not point to an FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADOFFSET, if parentoffset did not point to an FDT_BEGIN_NODE
+ *		tag
  *	-FDT_ERR_EXISTS, if the node at parentoffset already has a subnode of
  *		the given name
  *	-FDT_ERR_NOSPACE, if there is insufficient free space in the
diff --git a/core/secure_dt.mk b/core/secure_dt.mk
new file mode 100644
index 0000000..16ea741
--- /dev/null
+++ b/core/secure_dt.mk
@@ -0,0 +1,107 @@
+#
+# Copyright (c) 2015-2018, ARM Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+DTC_FLAGS += -I dts -O dtb
+DTC := dtc
+
+define MAKE_PREREQ_DIR
+ifneq (${1},${2})
+${1} :
+	@mkdir -p "${1}"
+
+endif
+endef
+
+# Convert device tree source file names to matching blobs
+#   $(1) = input dts
+define SOURCES_TO_DTBS
+        $(notdir $(patsubst %.dts,%.dtb,$(filter %.dts,$(1))))
+endef
+
+# MAKE_FDT_DIRS macro creates the prerequisite directories that host the
+# FDT binaries
+#   $(1) = output directory
+#   $(2) = input dts
+define MAKE_FDT_DIRS
+        $(eval DTBS       := $(addprefix $(1)/,$(call SOURCES_TO_DTBS,$(2))))
+        $(eval TEMP_DTB_DIRS := $(sort $(dir ${DTBS})))
+        # The $(dir ) function leaves a trailing / on the directory names
+        # Rip off the / to match directory names with make rule targets.
+        $(eval DTB_DIRS   := $(patsubst %/,%,$(TEMP_DTB_DIRS)))
+
+$(eval $(foreach objd,${DTB_DIRS},$(call MAKE_PREREQ_DIR,${objd},${out-dir})))
+
+fdt_dirs: ${DTB_DIRS}
+endef
+
+# MAKE_DTB generate the Flattened device tree binary
+#   $(1) = output directory
+#   $(2) = input dts
+define MAKE_DTB
+
+# List of DTB file(s) to generate, based on DTS file basename list
+$(eval DTBOBJ := $(addprefix $(1)/,$(call SOURCES_TO_DTBS,$(2))))
+# List of the pre-compiled DTS file(s)
+$(eval DTSPRE := $(addprefix $(1)/,$(patsubst %.dts,%.pre.dts,$(notdir $(2)))))
+# Dependencies of the pre-compiled DTS file(s) on its source and included files
+$(eval DTSDEP := $(patsubst %.dtb,%.o.d,$(DTBOBJ)))
+# Dependencies of the DT compilation on its pre-compiled DTS
+$(eval DTBDEP := $(patsubst %.dtb,%.d,$(DTBOBJ)))
+
+$(DTBOBJ): $(2) $(filter-out %.d,$(MAKEFILE_LIST)) | fdt_dirs
+	@echo "  CPP     $$<"
+	$(eval DTBS       := $(addprefix $(1)/,$(call SOURCES_TO_DTBS,$(2))))
+	@$(CPP$(sm)) $$(CPPFLAGS) -Icore/include/ -x assembler-with-cpp \
+		-E -ffreestanding -MT $(DTBS) -MMD -MF $(DTSDEP) -o $(DTSPRE) $$<
+	@echo "  DTC     $$<"
+	@$(DTC) $$(DTC_FLAGS) -d $(DTBDEP) -o $$@ $(DTSPRE)
+
+-include $(DTBDEP)
+-include $(DTSDEP)
+endef
+
+# MAKE_DTBS builds flattened device tree sources
+#   $(1) = output directory
+#   $(2) = list of flattened device tree source files
+define MAKE_DTBS
+        $(eval DTBOBJS := $(filter %.dts,$(2)))
+        $(eval REMAIN := $(filter-out %.dts,$(2)))
+        $(and $(REMAIN),$(error FDT_SOURCES contain non-DTS files: $(REMAIN)))
+        $(eval $(foreach obj,$(DTBOBJS),$(call MAKE_DTB,$(1),$(obj))))
+        $(eval $(call MAKE_FDT_DIRS,$(1),$(2)))
+
+dtbs: $(DTBS)
+all: dtbs
+endef
+
+# Generating the DTB from the DTS and integrating into OP-TEE core.
+#
+# CFG_SECURE_DT provides the DTS base name. It is looked up as file
+# name as $(CFG_SECURE_DT).dts from the platform sub directory fdts/.
+#
+# Build precompiles $(CFG_SECURE_DT).dts to resolve pre compilation features
+# (build directive, file inclusion, ...) then generates $(CFG_SECURE_DT).dtb.
+#
+# If CFG_STATIC_SECURE_DT is enabled, $(CFG_SECURE_DT).dtb content is wrapped
+# into C source file builtin_secure_dtb.c that defines the DTB byte array
+# CFG_STATIC_SECURE_DT expects.
+
+DTB_FILE_NAME 	:= $(CFG_SECURE_DT).dtb
+FDT_SOURCES 	:= $(addprefix $(arch-dir)/fdts/, $(CFG_SECURE_DT).dts)
+
+DTC_FLAGS 	+= -Wno-unit_address_vs_reg
+
+$(eval $(call MAKE_DTBS,$(out-dir)/core/fdts,$(FDT_SOURCES)))
+
+ifeq ($(CFG_STATIC_SECURE_DT),y)
+gensrcs-y += builtin_secure_dtb
+produce-builtin_secure_dtb = fdts/builtin_secure_dtb.c
+depends-builtin_secure_dtb = $(DTBOBJ) scripts/ta_bin_to_c.py
+recipe-builtin_secure_dtb = scripts/bin_to_c.py --dtb $(DTBOBJ) \
+						--label static_secure_dtb \
+						--out $(out-dir)/core/fdts/builtin_secure_dtb.c
+cleanfiles += $(out-dir)/core/fdts/builtin_secure_dtb.c
+endif
diff --git a/core/sub.mk b/core/sub.mk
index 9aee1b4..3119b58 100644
--- a/core/sub.mk
+++ b/core/sub.mk
@@ -26,3 +26,10 @@ $(foreach f, $(EARLY_TA_PATHS), $(eval $(call process_early_ta,$(f))))
 $(foreach f, $(CFG_IN_TREE_EARLY_TAS), $(eval $(call \
 	process_early_ta,$(out-dir)/ta/$(f).stripped.elf)))
 endif
+
+#
+# Secure device tree support
+#
+ifneq ($(CFG_SECURE_DT),)
+include core/secure_dt.mk
+endif
diff --git a/lib/libutils/ext/include/util.h b/lib/libutils/ext/include/util.h
index 4561758..2509b07 100644
--- a/lib/libutils/ext/include/util.h
+++ b/lib/libutils/ext/include/util.h
@@ -49,9 +49,16 @@
 
 /* Round down the even multiple of size, size has to be a multiple of 2 */
 #define ROUNDDOWN(v, size) ((v) & ~((__typeof__(v))(size) - 1))
+
+/* Unsigned integer division with nearest rounding variant */
+#define UDIV_ROUND_NEAREST(x, y) \
+	(__extension__ ({ __typeof__(x) _x = (x); \
+	  __typeof__(y) _y = (y); \
+	  (_x + (_y / 2)) / _y; }))
 #else
 #define ROUNDUP(x, y)			((((x) + (y) - 1) / (y)) * (y))
 #define ROUNDDOWN(x, y)		(((x) / (y)) * (y))
+#define UDIV_ROUND_NEAREST(x, y)	(((x) + ((y) / 2)) / (y))
 #endif
 
 /* x has to be of an unsigned type */
diff --git a/mk/config.mk b/mk/config.mk
index e0c050a..21ba469 100644
--- a/mk/config.mk
+++ b/mk/config.mk
@@ -236,10 +236,25 @@ CFG_CORE_SANITIZE_UNDEFINED ?= n
 CFG_CORE_SANITIZE_KADDRESS ?= n
 
 # Device Tree support
-# When enabled, the TEE _start function expects to find the address of a
-# Device Tree Blob (DTB) in register r2. The DT parsing code relies on
-# libfdt.  Currently only used to add the optee node and a reserved-memory
-# node for shared memory.
+#
+# - When CFG_DT is enabled, the libfdt is embedded in the core. It allows
+#   to parse a device tree.
+# - When CFG_STATIC_SECURE_DT is enabled, TEE core embeds a statically linked
+#   device tree blob.
+# - When CFG_SECURE_DT is defined, it defines the base name of the device tree
+#   source file from which a device tree blob is generated at core build.
+#
+# If CFG_DT is enabled and CFG_STATIC_SECURE_DT is disabled, the TEE _start
+# function expects to find the address of a Device Tree Blob (DTB) in
+# register r2 and is currently only used to add the optee node and a
+# reserved-memory node for shared memory.
+ifneq ($(CFG_SECURE_DT),)
+$(call force,CFG_STATIC_SECURE_DT,y)
+endif
+CFG_STATIC_SECURE_DT ?= n
+ifeq ($(CFG_STATIC_SECURE_DT),y)
+$(call force,CFG_DT,y)
+endif
 CFG_DT ?= n
 
 # Maximum size of the Device Tree Blob, has to be large enough to allow
diff --git a/scripts/bin_to_c.py b/scripts/bin_to_c.py
new file mode 100755
index 0000000..6fe7772
--- /dev/null
+++ b/scripts/bin_to_c.py
@@ -0,0 +1,58 @@
+#!/usr/bin/env python
+# SPDX-License-Identifier: BSD-2-Clause
+#
+# Copyright (c) 2018, Linaro Limited
+#
+
+import argparse
+import array
+import os
+import re
+
+def get_args():
+
+	parser = argparse.ArgumentParser(description='Converts a binary '
+		'file into C source file defining binary data as a'
+		'constant byte array.')
+
+	parser.add_argument('--dtb', required=True,
+		help='Path to the input binary file')
+
+	parser.add_argument('--label', required=True,
+		help='Label for the generated table in the C source file.')
+
+	parser.add_argument('--out', required=True,
+		help='Path of the output C file')
+
+	return parser.parse_args()
+
+def main():
+
+	args = get_args();
+
+	with open(args.dtb, 'rb') as indata:
+		bytes = indata.read()
+		size = len(bytes)
+
+	f = open(args.out, 'w')
+	f.write('/* Generated from ' + args.dtb + ' by ' +
+		os.path.basename(__file__) + ' */\n\n')
+	f.write('#include <compiler.h>\n');
+	f.write('#include <stdint.h>\n');
+	f.write('__extension__ const uint8_t ' + args.label + '[] ' +
+		' __aligned(__alignof__(uint64_t)) = {\n')
+	i = 0
+	while i < size:
+		if i % 8 == 0:
+			f.write('\t\t');
+		f.write('0x' + '{:02x}'.format(ord(bytes[i])) + ',')
+		i = i + 1
+		if i % 8 == 0 or i == size:
+			f.write('\n')
+		else:
+			f.write(' ')
+	f.write('};\n');
+	f.close()
+
+if __name__ == "__main__":
+	main()
-- 
2.7.4

