From c85389468d5ee9f06de67f10783bc015117caf67 Mon Sep 17 00:00:00 2001
From: Romuald JEANNE <romuald.jeanne@st.com>
Date: Mon, 15 Mar 2021 17:27:07 +0100
Subject: [PATCH] 3.12.0-stm32mp-r1

Signed-off-by: Romuald JEANNE <romuald.jeanne@st.com>
---
 CONTRIBUTING.md                               |   30 +
 core/arch/arm/dts/stm32mp15-pinctrl.dtsi      |  957 ++----------
 core/arch/arm/dts/stm32mp151.dtsi             | 1368 +++--------------
 core/arch/arm/dts/stm32mp153.dtsi             |   31 +-
 core/arch/arm/dts/stm32mp157.dtsi             |   24 -
 core/arch/arm/dts/stm32mp157a-dk1.dts         |   29 +-
 core/arch/arm/dts/stm32mp157a-ed1.dts         |   38 +
 core/arch/arm/dts/stm32mp157a-ev1.dts         |   24 +
 core/arch/arm/dts/stm32mp157c-dk2.dts         |   95 +-
 core/arch/arm/dts/stm32mp157c-ed1.dts         |  377 +----
 core/arch/arm/dts/stm32mp157c-ev1.dts         |  349 +----
 core/arch/arm/dts/stm32mp157d-dk1.dts         |   45 +
 core/arch/arm/dts/stm32mp157d-ed1.dts         |   39 +
 core/arch/arm/dts/stm32mp157d-ev1.dts         |   23 +
 core/arch/arm/dts/stm32mp157f-dk2.dts         |   51 +
 core/arch/arm/dts/stm32mp157f-ed1.dts         |   43 +
 core/arch/arm/dts/stm32mp157f-ev1.dts         |   23 +
 core/arch/arm/dts/stm32mp15xa.dtsi            |   13 +
 core/arch/arm/dts/stm32mp15xc.dtsi            |    3 +
 core/arch/arm/dts/stm32mp15xd.dtsi            |   19 +
 core/arch/arm/dts/stm32mp15xf.dtsi            |   21 +
 core/arch/arm/dts/stm32mp15xx-dkx.dtsi        |  708 ++++-----
 core/arch/arm/dts/stm32mp15xx-edx.dtsi        |  517 +++++++
 core/arch/arm/dts/stm32mp15xx-evx.dtsi        |   73 +
 core/arch/arm/dts/stm32mp15xxaa-pinctrl.dtsi  |    1 +
 core/arch/arm/dts/stm32mp15xxac-pinctrl.dtsi  |    1 +
 core/arch/arm/include/arm32.h                 |   11 +-
 core/arch/arm/include/kernel/tlb_helpers.h    |    9 +-
 core/arch/arm/include/mm/core_mmu.h           |   15 +
 core/arch/arm/include/mm/pgt_cache.h          |    5 +-
 core/arch/arm/include/sm/pm.h                 |    4 +
 core/arch/arm/mm/core_mmu.c                   |   35 +-
 core/arch/arm/mm/mobj.c                       |    5 +-
 core/arch/arm/mm/pgt_cache.c                  |   56 +-
 core/arch/arm/plat-stm32mp1/boot_api.h        |    2 +
 core/arch/arm/plat-stm32mp1/conf.mk           |   74 +-
 .../plat-stm32mp1/drivers/stm32mp1_calib.c    |  507 ++++++
 .../arm/plat-stm32mp1/drivers/stm32mp1_ddrc.c |  469 ++++++
 .../arm/plat-stm32mp1/drivers/stm32mp1_ddrc.h |  219 +++
 .../arm/plat-stm32mp1/drivers/stm32mp1_pmic.c |   50 +-
 .../arm/plat-stm32mp1/drivers/stm32mp1_pmic.h |    8 +-
 .../arm/plat-stm32mp1/drivers/stm32mp1_pwr.h  |   33 +-
 .../plat-stm32mp1/drivers/stm32mp1_syscfg.c   |   14 +-
 core/arch/arm/plat-stm32mp1/drivers/sub.mk    |    3 +-
 core/arch/arm/plat-stm32mp1/link.mk           |    2 +
 core/arch/arm/plat-stm32mp1/main.c            |  354 ++++-
 .../nsec-service/low_power_svc.c              |  152 ++
 .../nsec-service/low_power_svc.h              |   55 +
 .../arm/plat-stm32mp1/nsec-service/pwr_svc.c  |   89 ++
 .../arm/plat-stm32mp1/nsec-service/pwr_svc.h  |   22 +
 .../arm/plat-stm32mp1/nsec-service/rcc_svc.c  |  139 ++
 .../arm/plat-stm32mp1/nsec-service/rcc_svc.h  |   30 +
 .../plat-stm32mp1/nsec-service/stm32mp1_smc.h |  196 ++-
 .../nsec-service/stm32mp1_svc_setup.c         |   79 +-
 .../arm/plat-stm32mp1/nsec-service/sub.mk     |    3 +
 core/arch/arm/plat-stm32mp1/plat_tzc400.c     |   52 +-
 core/arch/arm/plat-stm32mp1/platform_config.h |  139 +-
 core/arch/arm/plat-stm32mp1/pm/context.c      |  522 +++++++
 core/arch/arm/plat-stm32mp1/pm/context.h      |  102 ++
 .../plat-stm32mp1/pm/context_asm_defines.c    |   28 +
 core/arch/arm/plat-stm32mp1/pm/low_power.c    |  626 ++++++++
 core/arch/arm/plat-stm32mp1/pm/pm_helpers.S   |  729 +++++++++
 core/arch/arm/plat-stm32mp1/pm/power.h        |   27 +
 core/arch/arm/plat-stm32mp1/pm/power_config.c |  252 +++
 core/arch/arm/plat-stm32mp1/pm/psci.c         |  234 ++-
 core/arch/arm/plat-stm32mp1/pm/sub.mk         |    6 +
 core/arch/arm/plat-stm32mp1/remoteproc_pta.c  |  528 +++++++
 core/arch/arm/plat-stm32mp1/reset.S           |   28 +-
 core/arch/arm/plat-stm32mp1/rproc_pub_key.h   |   16 +
 core/arch/arm/plat-stm32mp1/scmi_server.c     |   96 +-
 .../arch/arm/plat-stm32mp1/shared_resources.c |  156 +-
 core/arch/arm/plat-stm32mp1/stm32_util.h      |   77 +-
 core/arch/arm/plat-stm32mp1/stm32mp_pm.h      |   21 +
 core/arch/arm/plat-stm32mp1/sub.mk            |    9 +
 core/arch/arm/sm/pm_a32.S                     |   83 +-
 core/arch/arm/tee/entry_std.c                 |    2 +-
 .../clk/clk-stm32mp15.c}                      | 1345 +++++++++++++++-
 core/drivers/clk/clk.c                        |   58 +
 core/drivers/clk/sub.mk                       |    2 +
 core/drivers/gic.c                            |  225 ++-
 core/drivers/scmi-msg/clock.c                 |    4 +-
 core/drivers/stm32_bsec.c                     |  111 +-
 core/drivers/stm32_etzpc.c                    |   47 +
 core/drivers/stm32_gpio.c                     |   23 +-
 core/drivers/stm32_i2c.c                      |   37 +-
 core/drivers/stm32_iwdg.c                     |  314 ++++
 core/drivers/stm32_rng.c                      |    7 +-
 core/drivers/stm32_rtc.c                      |  446 ++++++
 core/drivers/stm32_tim.c                      |  298 ++++
 core/drivers/sub.mk                           |    4 +
 core/include/drivers/clk.h                    |   35 +
 core/include/drivers/gic.h                    |   24 +
 core/include/drivers/stm32_bsec.h             |   16 +
 core/include/drivers/stm32_iwdg.h             |   17 +
 core/include/drivers/stm32_rtc.h              |   60 +
 core/include/drivers/stm32_tim.h              |   25 +
 .../drivers/stm32mp1_rcc.h                    |   38 +-
 .../dt-bindings/clock/stm32mp1-clksrc.h       |  284 ++++
 .../dt-bindings/power/stm32mp1-power.h        |   19 +
 .../dt-bindings/soc/st,stm32-etzpc.h}         |   35 +-
 core/include/kernel/interrupt.h               |   15 +
 core/include/kernel/panic.h                   |    6 +
 core/kernel/interrupt.c                       |   10 +
 core/kernel/panic.c                           |   23 +-
 core/mm/vm.c                                  |   42 +-
 core/sub.mk                                   |    7 +-
 keys/default_rproc.pem                        |   27 +
 lib/libutee/include/remoteproc_pta.h          |  157 ++
 mk/config.mk                                  |    4 +
 mk/gcc.mk                                     |    6 +-
 scripts/sign_rproc_fw.py                      |  416 +++++
 ta/remoteproc/Makefile                        |   18 +
 ta/remoteproc/elf_parser.c                    |  186 +++
 ta/remoteproc/include/elf32.h                 |  243 +++
 ta/remoteproc/include/elf_common.h            | 1014 ++++++++++++
 ta/remoteproc/include/elf_parser.h            |   59 +
 ta/remoteproc/include/ta_remoteproc.h         |   65 +
 .../include/user_ta_header_defines.h          |   29 +
 ta/remoteproc/remoteproc_core.c               |  781 ++++++++++
 ta/remoteproc/sub.mk                          |    3 +
 ta/remoteproc/user_ta.mk                      |    1 +
 121 files changed, 14243 insertions(+), 3618 deletions(-)
 create mode 100644 CONTRIBUTING.md
 create mode 100644 core/arch/arm/dts/stm32mp157a-ed1.dts
 create mode 100644 core/arch/arm/dts/stm32mp157a-ev1.dts
 create mode 100644 core/arch/arm/dts/stm32mp157d-dk1.dts
 create mode 100644 core/arch/arm/dts/stm32mp157d-ed1.dts
 create mode 100644 core/arch/arm/dts/stm32mp157d-ev1.dts
 create mode 100644 core/arch/arm/dts/stm32mp157f-dk2.dts
 create mode 100644 core/arch/arm/dts/stm32mp157f-ed1.dts
 create mode 100644 core/arch/arm/dts/stm32mp157f-ev1.dts
 create mode 100644 core/arch/arm/dts/stm32mp15xa.dtsi
 create mode 100644 core/arch/arm/dts/stm32mp15xd.dtsi
 create mode 100644 core/arch/arm/dts/stm32mp15xf.dtsi
 create mode 100644 core/arch/arm/dts/stm32mp15xx-edx.dtsi
 create mode 100644 core/arch/arm/dts/stm32mp15xx-evx.dtsi
 create mode 100644 core/arch/arm/plat-stm32mp1/drivers/stm32mp1_calib.c
 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/nsec-service/low_power_svc.c
 create mode 100644 core/arch/arm/plat-stm32mp1/nsec-service/low_power_svc.h
 create mode 100644 core/arch/arm/plat-stm32mp1/nsec-service/pwr_svc.c
 create mode 100644 core/arch/arm/plat-stm32mp1/nsec-service/pwr_svc.h
 create mode 100644 core/arch/arm/plat-stm32mp1/nsec-service/rcc_svc.c
 create mode 100644 core/arch/arm/plat-stm32mp1/nsec-service/rcc_svc.h
 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/remoteproc_pta.c
 create mode 100644 core/arch/arm/plat-stm32mp1/rproc_pub_key.h
 create mode 100644 core/arch/arm/plat-stm32mp1/stm32mp_pm.h
 rename core/{arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c => drivers/clk/clk-stm32mp15.c} (50%)
 create mode 100644 core/drivers/clk/clk.c
 create mode 100644 core/drivers/clk/sub.mk
 create mode 100644 core/drivers/stm32_iwdg.c
 create mode 100644 core/drivers/stm32_rtc.c
 create mode 100644 core/drivers/stm32_tim.c
 create mode 100644 core/include/drivers/clk.h
 create mode 100644 core/include/drivers/stm32_iwdg.h
 create mode 100644 core/include/drivers/stm32_rtc.h
 create mode 100644 core/include/drivers/stm32_tim.h
 rename core/{arch/arm/plat-stm32mp1 => include}/drivers/stm32mp1_rcc.h (95%)
 create mode 100644 core/include/dt-bindings/clock/stm32mp1-clksrc.h
 create mode 100644 core/include/dt-bindings/power/stm32mp1-power.h
 rename core/{arch/arm/plat-stm32mp1/drivers/stm32mp1_etzpc.h => include/dt-bindings/soc/st,stm32-etzpc.h} (83%)
 create mode 100644 keys/default_rproc.pem
 create mode 100644 lib/libutee/include/remoteproc_pta.h
 create mode 100755 scripts/sign_rproc_fw.py
 create mode 100644 ta/remoteproc/Makefile
 create mode 100644 ta/remoteproc/elf_parser.c
 create mode 100644 ta/remoteproc/include/elf32.h
 create mode 100644 ta/remoteproc/include/elf_common.h
 create mode 100644 ta/remoteproc/include/elf_parser.h
 create mode 100644 ta/remoteproc/include/ta_remoteproc.h
 create mode 100644 ta/remoteproc/include/user_ta_header_defines.h
 create mode 100644 ta/remoteproc/remoteproc_core.c
 create mode 100644 ta/remoteproc/sub.mk
 create mode 100644 ta/remoteproc/user_ta.mk

diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
new file mode 100644
index 000000000..3d1bacd78
--- /dev/null
+++ b/CONTRIBUTING.md
@@ -0,0 +1,30 @@
+﻿# Contributing guide
+
+This document serves as a checklist before contributing to this repository. It includes links to read up on if topics are unclear to you.
+
+This guide mainly focuses on the proper use of Git.
+
+## 1. Issues
+
+STM32MPU projects do not activate "Github issues" feature for the time being. If you need to report an issue or question about this project deliverables, you can report them using [ ST Support Center ](https://my.st.com/ols#/ols/newrequest)  or  [ ST Community MPU Forum ](https://community.st.com/s/topic/0TO0X0000003u2AWAQ/stm32-mpus).
+
+## 2. Pull Requests
+
+STMicrolectronics is happy to receive contributions from the community, based on an initial Contributor License Agreement (CLA) procedure.
+
+* If you are an individual writing original source code and you are sure **you own the intellectual property**, then you need to sign an Individual CLA (https://cla.st.com).
+* If you work for a company that wants also to allow you to contribute with your work, your company needs to provide a Corporate CLA (https://cla.st.com) mentioning your GitHub account name.
+* If you are not sure that a CLA (Individual or Corporate) has been signed for your GitHub account you can check here (https://cla.st.com).
+
+Please note that:
+* The Corporate CLA will always take precedence over the Individual CLA.
+* One CLA submission is sufficient, for any project proposed by STMicroelectronics.
+
+__How to proceed__
+
+* We recommend to fork the project in your GitHub account to further develop your contribution. Please use the latest commit version.
+* Please, submit one Pull Request for one new feature or proposal. This will ease the analysis and final merge if accepted.
+
+__Note__
+
+Merge will not be done directly in GitHub but it will need first to follow internal integration process before public deliver in a standard release. The Pull request will stay open until it is merged and delivered.
diff --git a/core/arch/arm/dts/stm32mp15-pinctrl.dtsi b/core/arch/arm/dts/stm32mp15-pinctrl.dtsi
index 0237d4dda..16c40cbff 100644
--- a/core/arch/arm/dts/stm32mp15-pinctrl.dtsi
+++ b/core/arch/arm/dts/stm32mp15-pinctrl.dtsi
@@ -6,162 +6,6 @@
 #include <dt-bindings/pinctrl/stm32-pinfunc.h>
 
 &pinctrl {
-	adc1_in6_pins_a: adc1-in6 {
-		pins {
-			pinmux = <STM32_PINMUX('F', 12, ANALOG)>;
-		};
-	};
-
-	adc12_ain_pins_a: adc12-ain-0 {
-		pins {
-			pinmux = <STM32_PINMUX('C', 3, ANALOG)>, /* ADC1 in13 */
-				 <STM32_PINMUX('F', 12, ANALOG)>, /* ADC1 in6 */
-				 <STM32_PINMUX('F', 13, ANALOG)>, /* ADC2 in2 */
-				 <STM32_PINMUX('F', 14, ANALOG)>; /* ADC2 in6 */
-		};
-	};
-
-	adc12_usb_cc_pins_a: adc12-usb-cc-pins-0 {
-		pins {
-			pinmux = <STM32_PINMUX('A', 4, ANALOG)>, /* ADC12 in18 */
-				 <STM32_PINMUX('A', 5, ANALOG)>; /* ADC12 in19 */
-		};
-	};
-
-	cec_pins_a: cec-0 {
-		pins {
-			pinmux = <STM32_PINMUX('A', 15, AF4)>;
-			bias-disable;
-			drive-open-drain;
-			slew-rate = <0>;
-		};
-	};
-
-	cec_pins_sleep_a: cec-sleep-0 {
-		pins {
-			pinmux = <STM32_PINMUX('A', 15, ANALOG)>; /* HDMI_CEC */
-		};
-	};
-
-	cec_pins_b: cec-1 {
-		pins {
-			pinmux = <STM32_PINMUX('B', 6, AF5)>;
-			bias-disable;
-			drive-open-drain;
-			slew-rate = <0>;
-		};
-	};
-
-	cec_pins_sleep_b: cec-sleep-1 {
-		pins {
-			pinmux = <STM32_PINMUX('B', 6, ANALOG)>; /* HDMI_CEC */
-		};
-	};
-
-	dac_ch1_pins_a: dac-ch1 {
-		pins {
-			pinmux = <STM32_PINMUX('A', 4, ANALOG)>;
-		};
-	};
-
-	dac_ch2_pins_a: dac-ch2 {
-		pins {
-			pinmux = <STM32_PINMUX('A', 5, ANALOG)>;
-		};
-	};
-
-	dcmi_pins_a: dcmi-0 {
-		pins {
-			pinmux = <STM32_PINMUX('H', 8,  AF13)>,/* DCMI_HSYNC */
-				 <STM32_PINMUX('B', 7,  AF13)>,/* DCMI_VSYNC */
-				 <STM32_PINMUX('A', 6,  AF13)>,/* DCMI_PIXCLK */
-				 <STM32_PINMUX('H', 9,  AF13)>,/* DCMI_D0 */
-				 <STM32_PINMUX('H', 10, AF13)>,/* DCMI_D1 */
-				 <STM32_PINMUX('H', 11, AF13)>,/* DCMI_D2 */
-				 <STM32_PINMUX('H', 12, AF13)>,/* DCMI_D3 */
-				 <STM32_PINMUX('H', 14, AF13)>,/* DCMI_D4 */
-				 <STM32_PINMUX('I', 4,  AF13)>,/* DCMI_D5 */
-				 <STM32_PINMUX('B', 8,  AF13)>,/* DCMI_D6 */
-				 <STM32_PINMUX('E', 6,  AF13)>,/* DCMI_D7 */
-				 <STM32_PINMUX('I', 1,  AF13)>,/* DCMI_D8 */
-				 <STM32_PINMUX('H', 7,  AF13)>,/* DCMI_D9 */
-				 <STM32_PINMUX('I', 3,  AF13)>,/* DCMI_D10 */
-				 <STM32_PINMUX('H', 15, AF13)>;/* DCMI_D11 */
-			bias-disable;
-		};
-	};
-
-	dcmi_sleep_pins_a: dcmi-sleep-0 {
-		pins {
-			pinmux = <STM32_PINMUX('H', 8,  ANALOG)>,/* DCMI_HSYNC */
-				 <STM32_PINMUX('B', 7,  ANALOG)>,/* DCMI_VSYNC */
-				 <STM32_PINMUX('A', 6,  ANALOG)>,/* DCMI_PIXCLK */
-				 <STM32_PINMUX('H', 9,  ANALOG)>,/* DCMI_D0 */
-				 <STM32_PINMUX('H', 10, ANALOG)>,/* DCMI_D1 */
-				 <STM32_PINMUX('H', 11, ANALOG)>,/* DCMI_D2 */
-				 <STM32_PINMUX('H', 12, ANALOG)>,/* DCMI_D3 */
-				 <STM32_PINMUX('H', 14, ANALOG)>,/* DCMI_D4 */
-				 <STM32_PINMUX('I', 4,  ANALOG)>,/* DCMI_D5 */
-				 <STM32_PINMUX('B', 8,  ANALOG)>,/* DCMI_D6 */
-				 <STM32_PINMUX('E', 6,  ANALOG)>,/* DCMI_D7 */
-				 <STM32_PINMUX('I', 1,  ANALOG)>,/* DCMI_D8 */
-				 <STM32_PINMUX('H', 7,  ANALOG)>,/* DCMI_D9 */
-				 <STM32_PINMUX('I', 3,  ANALOG)>,/* DCMI_D10 */
-				 <STM32_PINMUX('H', 15, ANALOG)>;/* DCMI_D11 */
-		};
-	};
-
-	ethernet0_rgmii_pins_a: rgmii-0 {
-		pins1 {
-			pinmux = <STM32_PINMUX('G', 5, AF11)>, /* ETH_RGMII_CLK125 */
-				 <STM32_PINMUX('G', 4, AF11)>, /* ETH_RGMII_GTX_CLK */
-				 <STM32_PINMUX('G', 13, AF11)>, /* ETH_RGMII_TXD0 */
-				 <STM32_PINMUX('G', 14, AF11)>, /* ETH_RGMII_TXD1 */
-				 <STM32_PINMUX('C', 2, AF11)>, /* ETH_RGMII_TXD2 */
-				 <STM32_PINMUX('E', 2, AF11)>, /* ETH_RGMII_TXD3 */
-				 <STM32_PINMUX('B', 11, AF11)>, /* ETH_RGMII_TX_CTL */
-				 <STM32_PINMUX('C', 1, AF11)>; /* ETH_MDC */
-			bias-disable;
-			drive-push-pull;
-			slew-rate = <2>;
-		};
-		pins2 {
-			pinmux = <STM32_PINMUX('A', 2, AF11)>; /* ETH_MDIO */
-			bias-disable;
-			drive-push-pull;
-			slew-rate = <0>;
-		};
-		pins3 {
-			pinmux = <STM32_PINMUX('C', 4, AF11)>, /* ETH_RGMII_RXD0 */
-				 <STM32_PINMUX('C', 5, AF11)>, /* ETH_RGMII_RXD1 */
-				 <STM32_PINMUX('B', 0, AF11)>, /* ETH_RGMII_RXD2 */
-				 <STM32_PINMUX('B', 1, AF11)>, /* ETH_RGMII_RXD3 */
-				 <STM32_PINMUX('A', 1, AF11)>, /* ETH_RGMII_RX_CLK */
-				 <STM32_PINMUX('A', 7, AF11)>; /* ETH_RGMII_RX_CTL */
-			bias-disable;
-		};
-	};
-
-	ethernet0_rgmii_pins_sleep_a: rgmii-sleep-0 {
-		pins1 {
-			pinmux = <STM32_PINMUX('G', 5, ANALOG)>, /* ETH_RGMII_CLK125 */
-				 <STM32_PINMUX('G', 4, ANALOG)>, /* ETH_RGMII_GTX_CLK */
-				 <STM32_PINMUX('G', 13, ANALOG)>, /* ETH_RGMII_TXD0 */
-				 <STM32_PINMUX('G', 14, ANALOG)>, /* ETH_RGMII_TXD1 */
-				 <STM32_PINMUX('C', 2, ANALOG)>, /* ETH_RGMII_TXD2 */
-				 <STM32_PINMUX('E', 2, ANALOG)>, /* ETH_RGMII_TXD3 */
-				 <STM32_PINMUX('B', 11, ANALOG)>, /* ETH_RGMII_TX_CTL */
-				 <STM32_PINMUX('A', 2, ANALOG)>, /* ETH_MDIO */
-				 <STM32_PINMUX('C', 1, ANALOG)>, /* ETH_MDC */
-				 <STM32_PINMUX('C', 4, ANALOG)>, /* ETH_RGMII_RXD0 */
-				 <STM32_PINMUX('C', 5, ANALOG)>, /* ETH_RGMII_RXD1 */
-				 <STM32_PINMUX('B', 0, ANALOG)>, /* ETH_RGMII_RXD2 */
-				 <STM32_PINMUX('B', 1, ANALOG)>, /* ETH_RGMII_RXD3 */
-				 <STM32_PINMUX('A', 1, ANALOG)>, /* ETH_RGMII_RX_CLK */
-				 <STM32_PINMUX('A', 7, ANALOG)>; /* ETH_RGMII_RX_CTL */
-		};
-	};
-
 	fmc_pins_a: fmc-0 {
 		pins1 {
 			pinmux = <STM32_PINMUX('D', 4, AF12)>, /* FMC_NOE */
@@ -187,412 +31,6 @@
 		};
 	};
 
-	fmc_sleep_pins_a: fmc-sleep-0 {
-		pins {
-			pinmux = <STM32_PINMUX('D', 4, ANALOG)>, /* FMC_NOE */
-				 <STM32_PINMUX('D', 5, ANALOG)>, /* FMC_NWE */
-				 <STM32_PINMUX('D', 11, ANALOG)>, /* FMC_A16_FMC_CLE */
-				 <STM32_PINMUX('D', 12, ANALOG)>, /* FMC_A17_FMC_ALE */
-				 <STM32_PINMUX('D', 14, ANALOG)>, /* FMC_D0 */
-				 <STM32_PINMUX('D', 15, ANALOG)>, /* FMC_D1 */
-				 <STM32_PINMUX('D', 0, ANALOG)>, /* FMC_D2 */
-				 <STM32_PINMUX('D', 1, ANALOG)>, /* FMC_D3 */
-				 <STM32_PINMUX('E', 7, ANALOG)>, /* FMC_D4 */
-				 <STM32_PINMUX('E', 8, ANALOG)>, /* FMC_D5 */
-				 <STM32_PINMUX('E', 9, ANALOG)>, /* FMC_D6 */
-				 <STM32_PINMUX('E', 10, ANALOG)>, /* FMC_D7 */
-				 <STM32_PINMUX('D', 6, ANALOG)>, /* FMC_NWAIT */
-				 <STM32_PINMUX('G', 9, ANALOG)>; /* FMC_NE2_FMC_NCE */
-		};
-	};
-
-	i2c1_pins_a: i2c1-0 {
-		pins {
-			pinmux = <STM32_PINMUX('D', 12, AF5)>, /* I2C1_SCL */
-				 <STM32_PINMUX('F', 15, AF5)>; /* I2C1_SDA */
-			bias-disable;
-			drive-open-drain;
-			slew-rate = <0>;
-		};
-	};
-
-	i2c1_pins_sleep_a: i2c1-1 {
-		pins {
-			pinmux = <STM32_PINMUX('D', 12, ANALOG)>, /* I2C1_SCL */
-				 <STM32_PINMUX('F', 15, ANALOG)>; /* I2C1_SDA */
-		};
-	};
-
-	i2c1_pins_b: i2c1-2 {
-		pins {
-			pinmux = <STM32_PINMUX('F', 14, AF5)>, /* I2C1_SCL */
-				 <STM32_PINMUX('F', 15, AF5)>; /* I2C1_SDA */
-			bias-disable;
-			drive-open-drain;
-			slew-rate = <0>;
-		};
-	};
-
-	i2c1_pins_sleep_b: i2c1-3 {
-		pins {
-			pinmux = <STM32_PINMUX('F', 14, ANALOG)>, /* I2C1_SCL */
-				 <STM32_PINMUX('F', 15, ANALOG)>; /* I2C1_SDA */
-		};
-	};
-
-	i2c2_pins_a: i2c2-0 {
-		pins {
-			pinmux = <STM32_PINMUX('H', 4, AF4)>, /* I2C2_SCL */
-				 <STM32_PINMUX('H', 5, AF4)>; /* I2C2_SDA */
-			bias-disable;
-			drive-open-drain;
-			slew-rate = <0>;
-		};
-	};
-
-	i2c2_pins_sleep_a: i2c2-1 {
-		pins {
-			pinmux = <STM32_PINMUX('H', 4, ANALOG)>, /* I2C2_SCL */
-				 <STM32_PINMUX('H', 5, ANALOG)>; /* I2C2_SDA */
-		};
-	};
-
-	i2c2_pins_b1: i2c2-2 {
-		pins {
-			pinmux = <STM32_PINMUX('H', 5, AF4)>; /* I2C2_SDA */
-			bias-disable;
-			drive-open-drain;
-			slew-rate = <0>;
-		};
-	};
-
-	i2c2_pins_sleep_b1: i2c2-3 {
-		pins {
-			pinmux = <STM32_PINMUX('H', 5, ANALOG)>; /* I2C2_SDA */
-		};
-	};
-
-	i2c5_pins_a: i2c5-0 {
-		pins {
-			pinmux = <STM32_PINMUX('A', 11, AF4)>, /* I2C5_SCL */
-				 <STM32_PINMUX('A', 12, AF4)>; /* I2C5_SDA */
-			bias-disable;
-			drive-open-drain;
-			slew-rate = <0>;
-		};
-	};
-
-	i2c5_pins_sleep_a: i2c5-1 {
-		pins {
-			pinmux = <STM32_PINMUX('A', 11, ANALOG)>, /* I2C5_SCL */
-				 <STM32_PINMUX('A', 12, ANALOG)>; /* I2C5_SDA */
-
-		};
-	};
-
-	i2s2_pins_a: i2s2-0 {
-		pins {
-			pinmux = <STM32_PINMUX('I', 3, AF5)>, /* I2S2_SDO */
-				 <STM32_PINMUX('B', 9, AF5)>, /* I2S2_WS */
-				 <STM32_PINMUX('A', 9, AF5)>; /* I2S2_CK */
-			slew-rate = <1>;
-			drive-push-pull;
-			bias-disable;
-		};
-	};
-
-	i2s2_pins_sleep_a: i2s2-1 {
-		pins {
-			pinmux = <STM32_PINMUX('I', 3, ANALOG)>, /* I2S2_SDO */
-				 <STM32_PINMUX('B', 9, ANALOG)>, /* I2S2_WS */
-				 <STM32_PINMUX('A', 9, ANALOG)>; /* I2S2_CK */
-		};
-	};
-
-	ltdc_pins_a: ltdc-a-0 {
-		pins {
-			pinmux = <STM32_PINMUX('G',  7, AF14)>, /* LCD_CLK */
-				 <STM32_PINMUX('I', 10, AF14)>, /* LCD_HSYNC */
-				 <STM32_PINMUX('I',  9, AF14)>, /* LCD_VSYNC */
-				 <STM32_PINMUX('F', 10, AF14)>, /* LCD_DE */
-				 <STM32_PINMUX('H',  2, AF14)>, /* LCD_R0 */
-				 <STM32_PINMUX('H',  3, AF14)>, /* LCD_R1 */
-				 <STM32_PINMUX('H',  8, AF14)>, /* LCD_R2 */
-				 <STM32_PINMUX('H',  9, AF14)>, /* LCD_R3 */
-				 <STM32_PINMUX('H', 10, AF14)>, /* LCD_R4 */
-				 <STM32_PINMUX('C',  0, AF14)>, /* LCD_R5 */
-				 <STM32_PINMUX('H', 12, AF14)>, /* LCD_R6 */
-				 <STM32_PINMUX('E', 15, AF14)>, /* LCD_R7 */
-				 <STM32_PINMUX('E',  5, AF14)>, /* LCD_G0 */
-				 <STM32_PINMUX('E',  6, AF14)>, /* LCD_G1 */
-				 <STM32_PINMUX('H', 13, AF14)>, /* LCD_G2 */
-				 <STM32_PINMUX('H', 14, AF14)>, /* LCD_G3 */
-				 <STM32_PINMUX('H', 15, AF14)>, /* LCD_G4 */
-				 <STM32_PINMUX('I',  0, AF14)>, /* LCD_G5 */
-				 <STM32_PINMUX('I',  1, AF14)>, /* LCD_G6 */
-				 <STM32_PINMUX('I',  2, AF14)>, /* LCD_G7 */
-				 <STM32_PINMUX('D',  9, AF14)>, /* LCD_B0 */
-				 <STM32_PINMUX('G', 12, AF14)>, /* LCD_B1 */
-				 <STM32_PINMUX('G', 10, AF14)>, /* LCD_B2 */
-				 <STM32_PINMUX('D', 10, AF14)>, /* LCD_B3 */
-				 <STM32_PINMUX('I',  4, AF14)>, /* LCD_B4 */
-				 <STM32_PINMUX('A',  3, AF14)>, /* LCD_B5 */
-				 <STM32_PINMUX('B',  8, AF14)>, /* LCD_B6 */
-				 <STM32_PINMUX('D',  8, AF14)>; /* LCD_B7 */
-			bias-disable;
-			drive-push-pull;
-			slew-rate = <1>;
-		};
-	};
-
-	ltdc_pins_sleep_a: ltdc-a-1 {
-		pins {
-			pinmux = <STM32_PINMUX('G',  7, ANALOG)>, /* LCD_CLK */
-				 <STM32_PINMUX('I', 10, ANALOG)>, /* LCD_HSYNC */
-				 <STM32_PINMUX('I',  9, ANALOG)>, /* LCD_VSYNC */
-				 <STM32_PINMUX('F', 10, ANALOG)>, /* LCD_DE */
-				 <STM32_PINMUX('H',  2, ANALOG)>, /* LCD_R0 */
-				 <STM32_PINMUX('H',  3, ANALOG)>, /* LCD_R1 */
-				 <STM32_PINMUX('H',  8, ANALOG)>, /* LCD_R2 */
-				 <STM32_PINMUX('H',  9, ANALOG)>, /* LCD_R3 */
-				 <STM32_PINMUX('H', 10, ANALOG)>, /* LCD_R4 */
-				 <STM32_PINMUX('C',  0, ANALOG)>, /* LCD_R5 */
-				 <STM32_PINMUX('H', 12, ANALOG)>, /* LCD_R6 */
-				 <STM32_PINMUX('E', 15, ANALOG)>, /* LCD_R7 */
-				 <STM32_PINMUX('E',  5, ANALOG)>, /* LCD_G0 */
-				 <STM32_PINMUX('E',  6, ANALOG)>, /* LCD_G1 */
-				 <STM32_PINMUX('H', 13, ANALOG)>, /* LCD_G2 */
-				 <STM32_PINMUX('H', 14, ANALOG)>, /* LCD_G3 */
-				 <STM32_PINMUX('H', 15, ANALOG)>, /* LCD_G4 */
-				 <STM32_PINMUX('I',  0, ANALOG)>, /* LCD_G5 */
-				 <STM32_PINMUX('I',  1, ANALOG)>, /* LCD_G6 */
-				 <STM32_PINMUX('I',  2, ANALOG)>, /* LCD_G7 */
-				 <STM32_PINMUX('D',  9, ANALOG)>, /* LCD_B0 */
-				 <STM32_PINMUX('G', 12, ANALOG)>, /* LCD_B1 */
-				 <STM32_PINMUX('G', 10, ANALOG)>, /* LCD_B2 */
-				 <STM32_PINMUX('D', 10, ANALOG)>, /* LCD_B3 */
-				 <STM32_PINMUX('I',  4, ANALOG)>, /* LCD_B4 */
-				 <STM32_PINMUX('A',  3, ANALOG)>, /* LCD_B5 */
-				 <STM32_PINMUX('B',  8, ANALOG)>, /* LCD_B6 */
-				 <STM32_PINMUX('D',  8, ANALOG)>; /* LCD_B7 */
-		};
-	};
-
-	ltdc_pins_b: ltdc-b-0 {
-		pins {
-			pinmux = <STM32_PINMUX('I', 14, AF14)>, /* LCD_CLK */
-				 <STM32_PINMUX('I', 12, AF14)>, /* LCD_HSYNC */
-				 <STM32_PINMUX('I', 13, AF14)>, /* LCD_VSYNC */
-				 <STM32_PINMUX('K',  7, AF14)>, /* LCD_DE */
-				 <STM32_PINMUX('I', 15, AF14)>, /* LCD_R0 */
-				 <STM32_PINMUX('J',  0, AF14)>, /* LCD_R1 */
-				 <STM32_PINMUX('J',  1, AF14)>, /* LCD_R2 */
-				 <STM32_PINMUX('J',  2, AF14)>, /* LCD_R3 */
-				 <STM32_PINMUX('J',  3, AF14)>, /* LCD_R4 */
-				 <STM32_PINMUX('J',  4, AF14)>, /* LCD_R5 */
-				 <STM32_PINMUX('J',  5, AF14)>, /* LCD_R6 */
-				 <STM32_PINMUX('J',  6, AF14)>, /* LCD_R7 */
-				 <STM32_PINMUX('J',  7, AF14)>, /* LCD_G0 */
-				 <STM32_PINMUX('J',  8, AF14)>, /* LCD_G1 */
-				 <STM32_PINMUX('J',  9, AF14)>, /* LCD_G2 */
-				 <STM32_PINMUX('J', 10, AF14)>, /* LCD_G3 */
-				 <STM32_PINMUX('J', 11, AF14)>, /* LCD_G4 */
-				 <STM32_PINMUX('K',  0, AF14)>, /* LCD_G5 */
-				 <STM32_PINMUX('K',  1, AF14)>, /* LCD_G6 */
-				 <STM32_PINMUX('K',  2, AF14)>, /* LCD_G7 */
-				 <STM32_PINMUX('J', 12, AF14)>, /* LCD_B0 */
-				 <STM32_PINMUX('J', 13, AF14)>, /* LCD_B1 */
-				 <STM32_PINMUX('J', 14, AF14)>, /* LCD_B2 */
-				 <STM32_PINMUX('J', 15, AF14)>, /* LCD_B3 */
-				 <STM32_PINMUX('K',  3, AF14)>, /* LCD_B4 */
-				 <STM32_PINMUX('K',  4, AF14)>, /* LCD_B5 */
-				 <STM32_PINMUX('K',  5, AF14)>, /* LCD_B6 */
-				 <STM32_PINMUX('K',  6, AF14)>; /* LCD_B7 */
-			bias-disable;
-			drive-push-pull;
-			slew-rate = <1>;
-		};
-	};
-
-	ltdc_pins_sleep_b: ltdc-b-1 {
-		pins {
-			pinmux = <STM32_PINMUX('I', 14, ANALOG)>, /* LCD_CLK */
-				 <STM32_PINMUX('I', 12, ANALOG)>, /* LCD_HSYNC */
-				 <STM32_PINMUX('I', 13, ANALOG)>, /* LCD_VSYNC */
-				 <STM32_PINMUX('K',  7, ANALOG)>, /* LCD_DE */
-				 <STM32_PINMUX('I', 15, ANALOG)>, /* LCD_R0 */
-				 <STM32_PINMUX('J',  0, ANALOG)>, /* LCD_R1 */
-				 <STM32_PINMUX('J',  1, ANALOG)>, /* LCD_R2 */
-				 <STM32_PINMUX('J',  2, ANALOG)>, /* LCD_R3 */
-				 <STM32_PINMUX('J',  3, ANALOG)>, /* LCD_R4 */
-				 <STM32_PINMUX('J',  4, ANALOG)>, /* LCD_R5 */
-				 <STM32_PINMUX('J',  5, ANALOG)>, /* LCD_R6 */
-				 <STM32_PINMUX('J',  6, ANALOG)>, /* LCD_R7 */
-				 <STM32_PINMUX('J',  7, ANALOG)>, /* LCD_G0 */
-				 <STM32_PINMUX('J',  8, ANALOG)>, /* LCD_G1 */
-				 <STM32_PINMUX('J',  9, ANALOG)>, /* LCD_G2 */
-				 <STM32_PINMUX('J', 10, ANALOG)>, /* LCD_G3 */
-				 <STM32_PINMUX('J', 11, ANALOG)>, /* LCD_G4 */
-				 <STM32_PINMUX('K',  0, ANALOG)>, /* LCD_G5 */
-				 <STM32_PINMUX('K',  1, ANALOG)>, /* LCD_G6 */
-				 <STM32_PINMUX('K',  2, ANALOG)>, /* LCD_G7 */
-				 <STM32_PINMUX('J', 12, ANALOG)>, /* LCD_B0 */
-				 <STM32_PINMUX('J', 13, ANALOG)>, /* LCD_B1 */
-				 <STM32_PINMUX('J', 14, ANALOG)>, /* LCD_B2 */
-				 <STM32_PINMUX('J', 15, ANALOG)>, /* LCD_B3 */
-				 <STM32_PINMUX('K',  3, ANALOG)>, /* LCD_B4 */
-				 <STM32_PINMUX('K',  4, ANALOG)>, /* LCD_B5 */
-				 <STM32_PINMUX('K',  5, ANALOG)>, /* LCD_B6 */
-				 <STM32_PINMUX('K',  6, ANALOG)>; /* LCD_B7 */
-		};
-	};
-
-	m_can1_pins_a: m-can1-0 {
-		pins1 {
-			pinmux = <STM32_PINMUX('H', 13, AF9)>; /* CAN1_TX */
-			slew-rate = <1>;
-			drive-push-pull;
-			bias-disable;
-		};
-		pins2 {
-			pinmux = <STM32_PINMUX('I', 9, AF9)>; /* CAN1_RX */
-			bias-disable;
-		};
-	};
-
-	m_can1_sleep_pins_a: m_can1-sleep-0 {
-		pins {
-			pinmux = <STM32_PINMUX('H', 13, ANALOG)>, /* CAN1_TX */
-				 <STM32_PINMUX('I', 9, ANALOG)>; /* CAN1_RX */
-		};
-	};
-
-	pwm1_pins_a: pwm1-0 {
-		pins {
-			pinmux = <STM32_PINMUX('E', 9, AF1)>, /* TIM1_CH1 */
-				 <STM32_PINMUX('E', 11, AF1)>, /* TIM1_CH2 */
-				 <STM32_PINMUX('E', 14, AF1)>; /* TIM1_CH4 */
-			bias-pull-down;
-			drive-push-pull;
-			slew-rate = <0>;
-		};
-	};
-
-	pwm1_sleep_pins_a: pwm1-sleep-0 {
-		pins {
-			pinmux = <STM32_PINMUX('E', 9, ANALOG)>, /* TIM1_CH1 */
-				 <STM32_PINMUX('E', 11, ANALOG)>, /* TIM1_CH2 */
-				 <STM32_PINMUX('E', 14, ANALOG)>; /* TIM1_CH4 */
-		};
-	};
-
-	pwm2_pins_a: pwm2-0 {
-		pins {
-			pinmux = <STM32_PINMUX('A', 3, AF1)>; /* TIM2_CH4 */
-			bias-pull-down;
-			drive-push-pull;
-			slew-rate = <0>;
-		};
-	};
-
-	pwm2_sleep_pins_a: pwm2-sleep-0 {
-		pins {
-			pinmux = <STM32_PINMUX('A', 3, ANALOG)>; /* TIM2_CH4 */
-		};
-	};
-
-	pwm3_pins_a: pwm3-0 {
-		pins {
-			pinmux = <STM32_PINMUX('C', 7, AF2)>; /* TIM3_CH2 */
-			bias-pull-down;
-			drive-push-pull;
-			slew-rate = <0>;
-		};
-	};
-
-	pwm3_sleep_pins_a: pwm3-sleep-0 {
-		pins {
-			pinmux = <STM32_PINMUX('C', 7, ANALOG)>; /* TIM3_CH2 */
-		};
-	};
-
-	pwm4_pins_a: pwm4-0 {
-		pins {
-			pinmux = <STM32_PINMUX('D', 14, AF2)>, /* TIM4_CH3 */
-				 <STM32_PINMUX('D', 15, AF2)>; /* TIM4_CH4 */
-			bias-pull-down;
-			drive-push-pull;
-			slew-rate = <0>;
-		};
-	};
-
-	pwm4_sleep_pins_a: pwm4-sleep-0 {
-		pins {
-			pinmux = <STM32_PINMUX('D', 14, ANALOG)>, /* TIM4_CH3 */
-				 <STM32_PINMUX('D', 15, ANALOG)>; /* TIM4_CH4 */
-		};
-	};
-
-	pwm4_pins_b: pwm4-1 {
-		pins {
-			pinmux = <STM32_PINMUX('D', 13, AF2)>; /* TIM4_CH2 */
-			bias-pull-down;
-			drive-push-pull;
-			slew-rate = <0>;
-		};
-	};
-
-	pwm4_sleep_pins_b: pwm4-sleep-1 {
-		pins {
-			pinmux = <STM32_PINMUX('D', 13, ANALOG)>; /* TIM4_CH2 */
-		};
-	};
-
-	pwm5_pins_a: pwm5-0 {
-		pins {
-			pinmux = <STM32_PINMUX('H', 11, AF2)>; /* TIM5_CH2 */
-			bias-pull-down;
-			drive-push-pull;
-			slew-rate = <0>;
-		};
-	};
-
-	pwm5_sleep_pins_a: pwm5-sleep-0 {
-		pins {
-			pinmux = <STM32_PINMUX('H', 11, ANALOG)>; /* TIM5_CH2 */
-		};
-	};
-
-	pwm8_pins_a: pwm8-0 {
-		pins {
-			pinmux = <STM32_PINMUX('I', 2, AF3)>; /* TIM8_CH4 */
-			bias-pull-down;
-			drive-push-pull;
-			slew-rate = <0>;
-		};
-	};
-
-	pwm8_sleep_pins_a: pwm8-sleep-0 {
-		pins {
-			pinmux = <STM32_PINMUX('I', 2, ANALOG)>; /* TIM8_CH4 */
-		};
-	};
-
-	pwm12_pins_a: pwm12-0 {
-		pins {
-			pinmux = <STM32_PINMUX('H', 6, AF2)>; /* TIM12_CH1 */
-			bias-pull-down;
-			drive-push-pull;
-			slew-rate = <0>;
-		};
-	};
-
-	pwm12_sleep_pins_a: pwm12-sleep-0 {
-		pins {
-			pinmux = <STM32_PINMUX('H', 6, ANALOG)>; /* TIM12_CH1 */
-		};
-	};
-
 	qspi_clk_pins_a: qspi-clk-0 {
 		pins {
 			pinmux = <STM32_PINMUX('F', 10, AF9)>; /* QSPI_CLK */
@@ -602,12 +40,6 @@
 		};
 	};
 
-	qspi_clk_sleep_pins_a: qspi-clk-sleep-0 {
-		pins {
-			pinmux = <STM32_PINMUX('F', 10, ANALOG)>; /* QSPI_CLK */
-		};
-	};
-
 	qspi_bk1_pins_a: qspi-bk1-0 {
 		pins1 {
 			pinmux = <STM32_PINMUX('F', 8, AF10)>, /* QSPI_BK1_IO0 */
@@ -626,16 +58,6 @@
 		};
 	};
 
-	qspi_bk1_sleep_pins_a: qspi-bk1-sleep-0 {
-		pins {
-			pinmux = <STM32_PINMUX('F', 8, ANALOG)>, /* QSPI_BK1_IO0 */
-				 <STM32_PINMUX('F', 9, ANALOG)>, /* QSPI_BK1_IO1 */
-				 <STM32_PINMUX('F', 7, ANALOG)>, /* QSPI_BK1_IO2 */
-				 <STM32_PINMUX('F', 6, ANALOG)>, /* QSPI_BK1_IO3 */
-				 <STM32_PINMUX('B', 6, ANALOG)>; /* QSPI_BK1_NCS */
-		};
-	};
-
 	qspi_bk2_pins_a: qspi-bk2-0 {
 		pins1 {
 			pinmux = <STM32_PINMUX('H', 2, AF9)>, /* QSPI_BK2_IO0 */
@@ -654,86 +76,9 @@
 		};
 	};
 
-	qspi_bk2_sleep_pins_a: qspi-bk2-sleep-0 {
+	rtc_out2_rmp_pins_a: rtc-out2-rmp-pins-0 {
 		pins {
-			pinmux = <STM32_PINMUX('H', 2, ANALOG)>, /* QSPI_BK2_IO0 */
-				 <STM32_PINMUX('H', 3, ANALOG)>, /* QSPI_BK2_IO1 */
-				 <STM32_PINMUX('G', 10, ANALOG)>, /* QSPI_BK2_IO2 */
-				 <STM32_PINMUX('G', 7, ANALOG)>, /* QSPI_BK2_IO3 */
-				 <STM32_PINMUX('C', 0, ANALOG)>; /* QSPI_BK2_NCS */
-		};
-	};
-
-	sai2a_pins_a: sai2a-0 {
-		pins {
-			pinmux = <STM32_PINMUX('I', 5, AF10)>, /* SAI2_SCK_A */
-				 <STM32_PINMUX('I', 6, AF10)>, /* SAI2_SD_A */
-				 <STM32_PINMUX('I', 7, AF10)>, /* SAI2_FS_A */
-				 <STM32_PINMUX('E', 0, AF10)>; /* SAI2_MCLK_A */
-			slew-rate = <0>;
-			drive-push-pull;
-			bias-disable;
-		};
-	};
-
-	sai2a_sleep_pins_a: sai2a-1 {
-		pins {
-			pinmux = <STM32_PINMUX('I', 5, ANALOG)>, /* SAI2_SCK_A */
-				 <STM32_PINMUX('I', 6, ANALOG)>, /* SAI2_SD_A */
-				 <STM32_PINMUX('I', 7, ANALOG)>, /* SAI2_FS_A */
-				 <STM32_PINMUX('E', 0, ANALOG)>; /* SAI2_MCLK_A */
-		};
-	};
-
-	sai2b_pins_a: sai2b-0 {
-		pins1 {
-			pinmux = <STM32_PINMUX('E', 12, AF10)>, /* SAI2_SCK_B */
-				 <STM32_PINMUX('E', 13, AF10)>, /* SAI2_FS_B */
-				 <STM32_PINMUX('E', 14, AF10)>; /* SAI2_MCLK_B */
-			slew-rate = <0>;
-			drive-push-pull;
-			bias-disable;
-		};
-		pins2 {
-			pinmux = <STM32_PINMUX('F', 11, AF10)>; /* SAI2_SD_B */
-			bias-disable;
-		};
-	};
-
-	sai2b_sleep_pins_a: sai2b-1 {
-		pins {
-			pinmux = <STM32_PINMUX('F', 11, ANALOG)>, /* SAI2_SD_B */
-				 <STM32_PINMUX('E', 12, ANALOG)>, /* SAI2_SCK_B */
-				 <STM32_PINMUX('E', 13, ANALOG)>, /* SAI2_FS_B */
-				 <STM32_PINMUX('E', 14, ANALOG)>; /* SAI2_MCLK_B */
-		};
-	};
-
-	sai2b_pins_b: sai2b-2 {
-		pins {
-			pinmux = <STM32_PINMUX('F', 11, AF10)>; /* SAI2_SD_B */
-			bias-disable;
-		};
-	};
-
-	sai2b_sleep_pins_b: sai2b-3 {
-		pins {
-			pinmux = <STM32_PINMUX('F', 11, ANALOG)>; /* SAI2_SD_B */
-		};
-	};
-
-	sai4a_pins_a: sai4a-0 {
-		pins {
-			pinmux = <STM32_PINMUX('B', 5, AF10)>; /* SAI4_SD_A */
-			slew-rate = <0>;
-			drive-push-pull;
-			bias-disable;
-		};
-	};
-
-	sai4a_sleep_pins_a: sai4a-1 {
-		pins {
-			pinmux = <STM32_PINMUX('B', 5, ANALOG)>; /* SAI4_SD_A */
+			pinmux = <STM32_PINMUX('I', 8, ANALOG)>; /* RTC_OUT2_RMP */
 		};
 	};
 
@@ -756,41 +101,6 @@
 		};
 	};
 
-	sdmmc1_b4_od_pins_a: sdmmc1-b4-od-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 */
-			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;
-		};
-		pins3 {
-			pinmux = <STM32_PINMUX('D', 2, AF12)>; /* SDMMC1_CMD */
-			slew-rate = <1>;
-			drive-open-drain;
-			bias-disable;
-		};
-	};
-
-	sdmmc1_b4_sleep_pins_a: sdmmc1-b4-sleep-0 {
-		pins {
-			pinmux = <STM32_PINMUX('C', 8, ANALOG)>, /* SDMMC1_D0 */
-				 <STM32_PINMUX('C', 9, ANALOG)>, /* SDMMC1_D1 */
-				 <STM32_PINMUX('C', 10, ANALOG)>, /* SDMMC1_D2 */
-				 <STM32_PINMUX('C', 11, ANALOG)>, /* SDMMC1_D3 */
-				 <STM32_PINMUX('C', 12, ANALOG)>, /* SDMMC1_CK */
-				 <STM32_PINMUX('D', 2, ANALOG)>; /* SDMMC1_CMD */
-		};
-	};
-
 	sdmmc1_dir_pins_a: sdmmc1-dir-0 {
 		pins1 {
 			pinmux = <STM32_PINMUX('F', 2, AF11)>, /* SDMMC1_D0DIR */
@@ -806,15 +116,6 @@
 		};
 	};
 
-	sdmmc1_dir_sleep_pins_a: sdmmc1-dir-sleep-0 {
-		pins {
-			pinmux = <STM32_PINMUX('F', 2, ANALOG)>, /* SDMMC1_D0DIR */
-				 <STM32_PINMUX('C', 7, ANALOG)>, /* SDMMC1_D123DIR */
-				 <STM32_PINMUX('B', 9, ANALOG)>, /* SDMMC1_CDIR */
-				 <STM32_PINMUX('E', 4, ANALOG)>; /* SDMMC1_CKIN */
-		};
-	};
-
 	sdmmc2_b4_pins_a: sdmmc2-b4-0 {
 		pins1 {
 			pinmux = <STM32_PINMUX('B', 14, AF9)>, /* SDMMC2_D0 */
@@ -834,230 +135,220 @@
 		};
 	};
 
-	sdmmc2_b4_od_pins_a: sdmmc2-b4-od-0 {
+	sdmmc2_b4_pins_b: sdmmc2-b4-1 {
 		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('B', 4, AF9)>, /* SDMMC2_D3 */
+				 <STM32_PINMUX('G', 6, AF10)>; /* SDMMC2_CMD */
 			slew-rate = <1>;
 			drive-push-pull;
-			bias-pull-up;
+			bias-disable;
 		};
 		pins2 {
 			pinmux = <STM32_PINMUX('E', 3, AF9)>; /* SDMMC2_CK */
 			slew-rate = <2>;
 			drive-push-pull;
-			bias-pull-up;
-		};
-		pins3 {
-			pinmux = <STM32_PINMUX('G', 6, AF10)>; /* SDMMC2_CMD */
-			slew-rate = <1>;
-			drive-open-drain;
-			bias-pull-up;
+			bias-disable;
 		};
 	};
 
-	sdmmc2_b4_sleep_pins_a: sdmmc2-b4-sleep-0 {
+	sdmmc2_d47_pins_a: sdmmc2-d47-0 {
 		pins {
-			pinmux = <STM32_PINMUX('B', 14, ANALOG)>, /* SDMMC2_D0 */
-				 <STM32_PINMUX('B', 15, ANALOG)>, /* SDMMC2_D1 */
-				 <STM32_PINMUX('B', 3, ANALOG)>, /* SDMMC2_D2 */
-				 <STM32_PINMUX('B', 4, ANALOG)>, /* SDMMC2_D3 */
-				 <STM32_PINMUX('E', 3, ANALOG)>, /* SDMMC2_CK */
-				 <STM32_PINMUX('G', 6, ANALOG)>; /* SDMMC2_CMD */
+			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;
 		};
 	};
 
-	sdmmc2_b4_pins_b: sdmmc2-b4-1 {
+	uart4_pins_a: uart4-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;
+			pinmux = <STM32_PINMUX('G', 11, AF6)>; /* UART4_TX */
 			bias-disable;
+			drive-push-pull;
+			slew-rate = <0>;
 		};
 		pins2 {
-			pinmux = <STM32_PINMUX('E', 3, AF9)>; /* SDMMC2_CK */
-			slew-rate = <2>;
-			drive-push-pull;
+			pinmux = <STM32_PINMUX('B', 2, AF8)>; /* UART4_RX */
 			bias-disable;
 		};
 	};
 
-	sdmmc2_b4_od_pins_b: sdmmc2-b4-od-1 {
+	uart4_pins_b: uart4-1 {
 		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 */
-			slew-rate = <1>;
-			drive-push-pull;
+			pinmux = <STM32_PINMUX('D', 1, AF8)>; /* UART4_TX */
 			bias-disable;
-		};
-		pins2 {
-			pinmux = <STM32_PINMUX('E', 3, AF9)>; /* SDMMC2_CK */
-			slew-rate = <2>;
 			drive-push-pull;
-			bias-disable;
+			slew-rate = <0>;
 		};
-		pins3 {
-			pinmux = <STM32_PINMUX('G', 6, AF10)>; /* SDMMC2_CMD */
-			slew-rate = <1>;
-			drive-open-drain;
+		pins2 {
+			pinmux = <STM32_PINMUX('B', 2, AF8)>; /* UART4_RX */
 			bias-disable;
 		};
 	};
 
-	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>;
+	uart7_pins_a: uart7-0 {
+		pins1 {
+			pinmux = <STM32_PINMUX('E', 8, AF7)>; /* UART7_TX */
+			bias-disable;
 			drive-push-pull;
-			bias-pull-up;
+			slew-rate = <0>;
+		};
+		pins2 {
+			pinmux = <STM32_PINMUX('E', 7, AF7)>, /* UART7_RX */
+				 <STM32_PINMUX('E', 10, AF7)>, /* UART7_CTS */
+				 <STM32_PINMUX('E', 9, AF7)>; /* UART7_RTS */
+			bias-disable;
 		};
 	};
 
-	sdmmc2_d47_sleep_pins_a: sdmmc2-d47-sleep-0 {
-		pins {
-			pinmux = <STM32_PINMUX('A', 8, ANALOG)>, /* SDMMC2_D4 */
-				 <STM32_PINMUX('A', 9, ANALOG)>, /* SDMMC2_D5 */
-				 <STM32_PINMUX('E', 5, ANALOG)>, /* SDMMC2_D6 */
-				 <STM32_PINMUX('D', 3, ANALOG)>; /* SDMMC2_D7 */
+	uart7_pins_b: uart7-1 {
+		pins1 {
+			pinmux = <STM32_PINMUX('F', 7, AF7)>; /* UART7_TX */
+			bias-disable;
+			drive-push-pull;
+			slew-rate = <0>;
+		};
+		pins2 {
+			pinmux = <STM32_PINMUX('F', 6, AF7)>; /* UART7_RX */
+			bias-disable;
 		};
 	};
 
-	sdmmc3_b4_pins_a: sdmmc3-b4-0 {
+	uart7_pins_c: uart7-2 {
 		pins1 {
-			pinmux = <STM32_PINMUX('F', 0, AF9)>, /* SDMMC3_D0 */
-				 <STM32_PINMUX('F', 4, AF9)>, /* SDMMC3_D1 */
-				 <STM32_PINMUX('F', 5, AF9)>, /* SDMMC3_D2 */
-				 <STM32_PINMUX('D', 7, AF10)>, /* SDMMC3_D3 */
-				 <STM32_PINMUX('F', 1, AF9)>; /* SDMMC3_CMD */
-			slew-rate = <1>;
+			pinmux = <STM32_PINMUX('E', 8, AF7)>; /* UART7_TX */
+			bias-disable;
 			drive-push-pull;
-			bias-pull-up;
+			slew-rate = <0>;
 		};
 		pins2 {
-			pinmux = <STM32_PINMUX('G', 15, AF10)>; /* SDMMC3_CK */
-			slew-rate = <2>;
-			drive-push-pull;
+			pinmux = <STM32_PINMUX('E', 7, AF7)>; /* UART7_RX */
 			bias-pull-up;
 		};
 	};
 
-	sdmmc3_b4_od_pins_a: sdmmc3-b4-od-0 {
+	uart8_pins_a: uart8-0 {
 		pins1 {
-			pinmux = <STM32_PINMUX('F', 0, AF9)>, /* SDMMC3_D0 */
-				 <STM32_PINMUX('F', 4, AF9)>, /* SDMMC3_D1 */
-				 <STM32_PINMUX('F', 5, AF9)>, /* SDMMC3_D2 */
-				 <STM32_PINMUX('D', 7, AF10)>; /* SDMMC3_D3 */
-			slew-rate = <1>;
+			pinmux = <STM32_PINMUX('E', 1, AF8)>; /* UART8_TX */
+			bias-disable;
 			drive-push-pull;
-			bias-pull-up;
+			slew-rate = <0>;
 		};
 		pins2 {
-			pinmux = <STM32_PINMUX('G', 15, AF10)>; /* SDMMC3_CK */
-			slew-rate = <2>;
-			drive-push-pull;
-			bias-pull-up;
-		};
-		pins3 {
-			pinmux = <STM32_PINMUX('F', 1, AF9)>; /* SDMMC2_CMD */
-			slew-rate = <1>;
-			drive-open-drain;
-			bias-pull-up;
+			pinmux = <STM32_PINMUX('E', 0, AF8)>; /* UART8_RX */
+			bias-disable;
 		};
 	};
 
-	sdmmc3_b4_sleep_pins_a: sdmmc3-b4-sleep-0 {
-		pins {
-			pinmux = <STM32_PINMUX('F', 0, ANALOG)>, /* SDMMC3_D0 */
-				 <STM32_PINMUX('F', 4, ANALOG)>, /* SDMMC3_D1 */
-				 <STM32_PINMUX('F', 5, ANALOG)>, /* SDMMC3_D2 */
-				 <STM32_PINMUX('D', 7, ANALOG)>, /* SDMMC3_D3 */
-				 <STM32_PINMUX('G', 15, ANALOG)>, /* SDMMC3_CK */
-				 <STM32_PINMUX('F', 1, ANALOG)>; /* SDMMC3_CMD */
+	usart2_pins_a: usart2-0 {
+		pins1 {
+			pinmux = <STM32_PINMUX('F', 5, AF7)>, /* USART2_TX */
+				 <STM32_PINMUX('D', 4, AF7)>; /* USART2_RTS */
+			bias-disable;
+			drive-push-pull;
+			slew-rate = <0>;
+		};
+		pins2 {
+			pinmux = <STM32_PINMUX('D', 6, AF7)>, /* USART2_RX */
+				 <STM32_PINMUX('D', 3, AF7)>; /* USART2_CTS_NSS */
+			bias-disable;
 		};
 	};
 
-	spdifrx_pins_a: spdifrx-0 {
-		pins {
-			pinmux = <STM32_PINMUX('G', 12, AF8)>; /* SPDIF_IN1 */
+	usart2_pins_b: usart2-1 {
+		pins1 {
+			pinmux = <STM32_PINMUX('F', 5, AF7)>, /* USART2_TX */
+				 <STM32_PINMUX('A', 1, AF7)>; /* USART2_RTS */
+			bias-disable;
+			drive-push-pull;
+			slew-rate = <0>;
+		};
+		pins2 {
+			pinmux = <STM32_PINMUX('F', 4, AF7)>, /* USART2_RX */
+				 <STM32_PINMUX('E', 15, AF7)>; /* USART2_CTS_NSS */
 			bias-disable;
 		};
 	};
 
-	spdifrx_sleep_pins_a: spdifrx-1 {
-		pins {
-			pinmux = <STM32_PINMUX('G', 12, ANALOG)>; /* SPDIF_IN1 */
+	usart2_pins_c: usart2-2 {
+		pins1 {
+			pinmux = <STM32_PINMUX('D', 5, AF7)>, /* USART2_TX */
+				 <STM32_PINMUX('D', 4, AF7)>; /* USART2_RTS */
+			bias-disable;
+			drive-push-pull;
+			slew-rate = <3>;
+		};
+		pins2 {
+			pinmux = <STM32_PINMUX('D', 6, AF7)>, /* USART2_RX */
+				 <STM32_PINMUX('D', 3, AF7)>; /* USART2_CTS_NSS */
+			bias-disable;
 		};
 	};
 
-	uart4_pins_a: uart4-0 {
+	usart3_pins_a: usart3-0 {
 		pins1 {
-			pinmux = <STM32_PINMUX('G', 11, AF6)>; /* UART4_TX */
+			pinmux = <STM32_PINMUX('B', 10, AF7)>; /* USART3_TX */
 			bias-disable;
 			drive-push-pull;
 			slew-rate = <0>;
 		};
 		pins2 {
-			pinmux = <STM32_PINMUX('B', 2, AF8)>; /* UART4_RX */
+			pinmux = <STM32_PINMUX('B', 12, AF8)>; /* USART3_RX */
 			bias-disable;
 		};
 	};
 
-	uart4_pins_b: uart4-1 {
+	usart3_pins_b: usart3-1 {
 		pins1 {
-			pinmux = <STM32_PINMUX('D', 1, AF8)>; /* UART4_TX */
+			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', 2, AF8)>; /* UART4_RX */
+			pinmux = <STM32_PINMUX('B', 12, AF8)>, /* USART3_RX */
+				 <STM32_PINMUX('I', 10, AF8)>; /* USART3_CTS_NSS */
 			bias-disable;
 		};
 	};
 
-	uart7_pins_a: uart7-0 {
+	usart3_pins_c: usart3-2 {
 		pins1 {
-			pinmux = <STM32_PINMUX('E', 8, AF7)>; /* UART4_TX */
+			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('E', 7, AF7)>, /* UART4_RX */
-				 <STM32_PINMUX('E', 10, AF7)>, /* UART4_CTS */
-				 <STM32_PINMUX('E', 9, AF7)>; /* UART4_RTS */
-			bias-disable;
+			pinmux = <STM32_PINMUX('B', 12, AF8)>, /* USART3_RX */
+				 <STM32_PINMUX('B', 13, AF7)>; /* USART3_CTS_NSS */
+			bias-pull-up;
 		};
 	};
-};
 
-&pinctrl_z {
-	i2c2_pins_b2: i2c2-0 {
+	usbotg_hs_pins_a: usbotg-hs-0 {
 		pins {
-			pinmux = <STM32_PINMUX('Z', 0, AF3)>; /* I2C2_SCL */
-			bias-disable;
-			drive-open-drain;
-			slew-rate = <0>;
+			pinmux = <STM32_PINMUX('A', 10, ANALOG)>; /* OTG_ID */
 		};
 	};
 
-	i2c2_pins_sleep_b2: i2c2-1 {
+	usbotg_fs_dp_dm_pins_a: usbotg-fs-dp-dm-0 {
 		pins {
-			pinmux = <STM32_PINMUX('Z', 0, ANALOG)>; /* I2C2_SCL */
+			pinmux = <STM32_PINMUX('A', 11, ANALOG)>, /* OTG_FS_DM */
+				 <STM32_PINMUX('A', 12, ANALOG)>; /* OTG_FS_DP */
 		};
 	};
+};
 
+&pinctrl_z {
 	i2c4_pins_a: i2c4-0 {
 		pins {
 			pinmux = <STM32_PINMUX('Z', 4, AF6)>, /* I2C4_SCL */
@@ -1067,26 +358,4 @@
 			slew-rate = <0>;
 		};
 	};
-
-	i2c4_pins_sleep_a: i2c4-1 {
-		pins {
-			pinmux = <STM32_PINMUX('Z', 4, ANALOG)>, /* I2C4_SCL */
-				 <STM32_PINMUX('Z', 5, ANALOG)>; /* I2C4_SDA */
-		};
-	};
-
-	spi1_pins_a: spi1-0 {
-		pins1 {
-			pinmux = <STM32_PINMUX('Z', 0, AF5)>, /* SPI1_SCK */
-				 <STM32_PINMUX('Z', 2, AF5)>; /* SPI1_MOSI */
-			bias-disable;
-			drive-push-pull;
-			slew-rate = <1>;
-		};
-
-		pins2 {
-			pinmux = <STM32_PINMUX('Z', 1, AF5)>; /* SPI1_MISO */
-			bias-disable;
-		};
-	};
 };
diff --git a/core/arch/arm/dts/stm32mp151.dtsi b/core/arch/arm/dts/stm32mp151.dtsi
index d8ac70292..714d94710 100644
--- a/core/arch/arm/dts/stm32mp151.dtsi
+++ b/core/arch/arm/dts/stm32mp151.dtsi
@@ -19,9 +19,41 @@
 			compatible = "arm,cortex-a7";
 			device_type = "cpu";
 			reg = <0>;
+			clocks = <&rcc CK_MPU>;
+			clock-names = "cpu";
+			operating-points-v2 = <&cpu0_opp_table>;
+			nvmem-cells = <&part_number_otp>;
+			nvmem-cell-names = "part_number";
 		};
 	};
 
+	cpu0_opp_table: cpu0-opp-table {
+		compatible = "operating-points-v2";
+		opp-shared;
+	};
+
+	nvmem_layout: nvmem_layout@0 {
+		compatible = "st,stm32-nvmem-layout";
+
+		nvmem-cells = <&cfg0_otp>,
+			      <&part_number_otp>,
+			      <&monotonic_otp>,
+			      <&nand_otp>,
+			      <&uid_otp>,
+			      <&package_otp>,
+			      <&hw2_otp>,
+			      <&pkh_otp>;
+
+		nvmem-cell-names = "cfg0_otp",
+				   "part_number_otp",
+				   "monotonic_otp",
+				   "nand_otp",
+				   "uid_otp",
+				   "package_otp",
+				   "hw2_otp",
+				   "pkh_otp";
+	};
+
 	psci {
 		compatible = "arm,psci-1.0";
 		method = "smc";
@@ -35,15 +67,6 @@
 		      <0xa0022000 0x2000>;
 	};
 
-	timer {
-		compatible = "arm,armv7-timer";
-		interrupts = <GIC_PPI 13 (GIC_CPU_MASK_SIMPLE(4) | IRQ_TYPE_LEVEL_LOW)>,
-			     <GIC_PPI 14 (GIC_CPU_MASK_SIMPLE(4) | IRQ_TYPE_LEVEL_LOW)>,
-			     <GIC_PPI 11 (GIC_CPU_MASK_SIMPLE(4) | IRQ_TYPE_LEVEL_LOW)>,
-			     <GIC_PPI 10 (GIC_CPU_MASK_SIMPLE(4) | IRQ_TYPE_LEVEL_LOW)>;
-		interrupt-parent = <&intc>;
-	};
-
 	clocks {
 		clk_hse: clk-hse {
 			#clock-cells = <0>;
@@ -76,37 +99,6 @@
 		};
 	};
 
-	thermal-zones {
-		cpu_thermal: cpu-thermal {
-			polling-delay-passive = <0>;
-			polling-delay = <0>;
-			thermal-sensors = <&dts>;
-
-			trips {
-				cpu_alert1: cpu-alert1 {
-					temperature = <85000>;
-					hysteresis = <0>;
-					type = "passive";
-				};
-
-				cpu-crit {
-					temperature = <120000>;
-					hysteresis = <0>;
-					type = "critical";
-				};
-			};
-
-			cooling-maps {
-			};
-		};
-	};
-
-	booster: regulator-booster {
-		compatible = "st,stm32mp1-booster";
-		st,syscfg = <&syscfg>;
-		status = "disabled";
-	};
-
 	soc {
 		compatible = "simple-bus";
 		#address-cells = <1>;
@@ -114,175 +106,6 @@
 		interrupt-parent = <&intc>;
 		ranges;
 
-		timers2: timer@40000000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32-timers";
-			reg = <0x40000000 0x400>;
-			clocks = <&rcc TIM2_K>;
-			clock-names = "int";
-			dmas = <&dmamux1 18 0x400 0x1>,
-			       <&dmamux1 19 0x400 0x1>,
-			       <&dmamux1 20 0x400 0x1>,
-			       <&dmamux1 21 0x400 0x1>,
-			       <&dmamux1 22 0x400 0x1>;
-			dma-names = "ch1", "ch2", "ch3", "ch4", "up";
-			status = "disabled";
-
-			pwm {
-				compatible = "st,stm32-pwm";
-				#pwm-cells = <3>;
-				status = "disabled";
-			};
-
-			timer@1 {
-				compatible = "st,stm32h7-timer-trigger";
-				reg = <1>;
-				status = "disabled";
-			};
-
-			counter {
-				compatible = "st,stm32-timer-counter";
-				status = "disabled";
-			};
-		};
-
-		timers3: timer@40001000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32-timers";
-			reg = <0x40001000 0x400>;
-			clocks = <&rcc TIM3_K>;
-			clock-names = "int";
-			dmas = <&dmamux1 23 0x400 0x1>,
-			       <&dmamux1 24 0x400 0x1>,
-			       <&dmamux1 25 0x400 0x1>,
-			       <&dmamux1 26 0x400 0x1>,
-			       <&dmamux1 27 0x400 0x1>,
-			       <&dmamux1 28 0x400 0x1>;
-			dma-names = "ch1", "ch2", "ch3", "ch4", "up", "trig";
-			status = "disabled";
-
-			pwm {
-				compatible = "st,stm32-pwm";
-				#pwm-cells = <3>;
-				status = "disabled";
-			};
-
-			timer@2 {
-				compatible = "st,stm32h7-timer-trigger";
-				reg = <2>;
-				status = "disabled";
-			};
-
-			counter {
-				compatible = "st,stm32-timer-counter";
-				status = "disabled";
-			};
-		};
-
-		timers4: timer@40002000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32-timers";
-			reg = <0x40002000 0x400>;
-			clocks = <&rcc TIM4_K>;
-			clock-names = "int";
-			dmas = <&dmamux1 29 0x400 0x1>,
-			       <&dmamux1 30 0x400 0x1>,
-			       <&dmamux1 31 0x400 0x1>,
-			       <&dmamux1 32 0x400 0x1>;
-			dma-names = "ch1", "ch2", "ch3", "ch4";
-			status = "disabled";
-
-			pwm {
-				compatible = "st,stm32-pwm";
-				#pwm-cells = <3>;
-				status = "disabled";
-			};
-
-			timer@3 {
-				compatible = "st,stm32h7-timer-trigger";
-				reg = <3>;
-				status = "disabled";
-			};
-
-			counter {
-				compatible = "st,stm32-timer-counter";
-				status = "disabled";
-			};
-		};
-
-		timers5: timer@40003000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32-timers";
-			reg = <0x40003000 0x400>;
-			clocks = <&rcc TIM5_K>;
-			clock-names = "int";
-			dmas = <&dmamux1 55 0x400 0x1>,
-			       <&dmamux1 56 0x400 0x1>,
-			       <&dmamux1 57 0x400 0x1>,
-			       <&dmamux1 58 0x400 0x1>,
-			       <&dmamux1 59 0x400 0x1>,
-			       <&dmamux1 60 0x400 0x1>;
-			dma-names = "ch1", "ch2", "ch3", "ch4", "up", "trig";
-			status = "disabled";
-
-			pwm {
-				compatible = "st,stm32-pwm";
-				#pwm-cells = <3>;
-				status = "disabled";
-			};
-
-			timer@4 {
-				compatible = "st,stm32h7-timer-trigger";
-				reg = <4>;
-				status = "disabled";
-			};
-
-			counter {
-				compatible = "st,stm32-timer-counter";
-				status = "disabled";
-			};
-		};
-
-		timers6: timer@40004000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32-timers";
-			reg = <0x40004000 0x400>;
-			clocks = <&rcc TIM6_K>;
-			clock-names = "int";
-			dmas = <&dmamux1 69 0x400 0x1>;
-			dma-names = "up";
-			status = "disabled";
-
-			timer@5 {
-				compatible = "st,stm32h7-timer-trigger";
-				reg = <5>;
-				status = "disabled";
-			};
-		};
-
-		timers7: timer@40005000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32-timers";
-			reg = <0x40005000 0x400>;
-			clocks = <&rcc TIM7_K>;
-			clock-names = "int";
-			dmas = <&dmamux1 70 0x400 0x1>;
-			dma-names = "up";
-			status = "disabled";
-
-			timer@6 {
-				compatible = "st,stm32h7-timer-trigger";
-				reg = <6>;
-				status = "disabled";
-			};
-		};
-
 		timers12: timer@40006000 {
 			#address-cells = <1>;
 			#size-cells = <0>;
@@ -291,152 +114,7 @@
 			clocks = <&rcc TIM12_K>;
 			clock-names = "int";
 			status = "disabled";
-
-			pwm {
-				compatible = "st,stm32-pwm";
-				#pwm-cells = <3>;
-				status = "disabled";
-			};
-
-			timer@11 {
-				compatible = "st,stm32h7-timer-trigger";
-				reg = <11>;
-				status = "disabled";
-			};
-		};
-
-		timers13: timer@40007000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32-timers";
-			reg = <0x40007000 0x400>;
-			clocks = <&rcc TIM13_K>;
-			clock-names = "int";
-			status = "disabled";
-
-			pwm {
-				compatible = "st,stm32-pwm";
-				#pwm-cells = <3>;
-				status = "disabled";
-			};
-
-			timer@12 {
-				compatible = "st,stm32h7-timer-trigger";
-				reg = <12>;
-				status = "disabled";
-			};
-		};
-
-		timers14: timer@40008000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32-timers";
-			reg = <0x40008000 0x400>;
-			clocks = <&rcc TIM14_K>;
-			clock-names = "int";
-			status = "disabled";
-
-			pwm {
-				compatible = "st,stm32-pwm";
-				#pwm-cells = <3>;
-				status = "disabled";
-			};
-
-			timer@13 {
-				compatible = "st,stm32h7-timer-trigger";
-				reg = <13>;
-				status = "disabled";
-			};
-		};
-
-		lptimer1: timer@40009000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32-lptimer";
-			reg = <0x40009000 0x400>;
-			clocks = <&rcc LPTIM1_K>;
-			clock-names = "mux";
-			status = "disabled";
-
-			pwm {
-				compatible = "st,stm32-pwm-lp";
-				#pwm-cells = <3>;
-				status = "disabled";
-			};
-
-			trigger@0 {
-				compatible = "st,stm32-lptimer-trigger";
-				reg = <0>;
-				status = "disabled";
-			};
-
-			counter {
-				compatible = "st,stm32-lptimer-counter";
-				status = "disabled";
-			};
-		};
-
-		spi2: spi@4000b000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32h7-spi";
-			reg = <0x4000b000 0x400>;
-			interrupts = <GIC_SPI 36 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc SPI2_K>;
-			resets = <&rcc SPI2_R>;
-			dmas = <&dmamux1 39 0x400 0x05>,
-			       <&dmamux1 40 0x400 0x05>;
-			dma-names = "rx", "tx";
-			status = "disabled";
-		};
-
-		i2s2: audio-controller@4000b000 {
-			compatible = "st,stm32h7-i2s";
-			#sound-dai-cells = <0>;
-			reg = <0x4000b000 0x400>;
-			interrupts = <GIC_SPI 36 IRQ_TYPE_LEVEL_HIGH>;
-			dmas = <&dmamux1 39 0x400 0x01>,
-			       <&dmamux1 40 0x400 0x01>;
-			dma-names = "rx", "tx";
-			status = "disabled";
-		};
-
-		spi3: spi@4000c000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32h7-spi";
-			reg = <0x4000c000 0x400>;
-			interrupts = <GIC_SPI 51 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc SPI3_K>;
-			resets = <&rcc SPI3_R>;
-			dmas = <&dmamux1 61 0x400 0x05>,
-			       <&dmamux1 62 0x400 0x05>;
-			dma-names = "rx", "tx";
-			status = "disabled";
-		};
-
-		i2s3: audio-controller@4000c000 {
-			compatible = "st,stm32h7-i2s";
-			#sound-dai-cells = <0>;
-			reg = <0x4000c000 0x400>;
-			interrupts = <GIC_SPI 51 IRQ_TYPE_LEVEL_HIGH>;
-			dmas = <&dmamux1 61 0x400 0x01>,
-			       <&dmamux1 62 0x400 0x01>;
-			dma-names = "rx", "tx";
-			status = "disabled";
-		};
-
-		spdifrx: audio-controller@4000d000 {
-			compatible = "st,stm32h7-spdifrx";
-			#sound-dai-cells = <0>;
-			reg = <0x4000d000 0x400>;
-			clocks = <&rcc SPDIF_K>;
-			clock-names = "kclk";
-			interrupts = <GIC_SPI 97 IRQ_TYPE_LEVEL_HIGH>;
-			dmas = <&dmamux1 93 0x400 0x01>,
-			       <&dmamux1 94 0x400 0x01>;
-			dma-names = "rx", "rx-ctrl";
-			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		usart2: serial@4000e000 {
@@ -444,6 +122,7 @@
 			reg = <0x4000e000 0x400>;
 			interrupts = <GIC_SPI 38 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc USART2_K>;
+			resets = <&rcc USART2_R>;
 			status = "disabled";
 		};
 
@@ -452,14 +131,17 @@
 			reg = <0x4000f000 0x400>;
 			interrupts = <GIC_SPI 39 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc USART3_K>;
+			resets = <&rcc USART3_R>;
 			status = "disabled";
 		};
 
 		uart4: serial@40010000 {
 			compatible = "st,stm32h7-uart";
 			reg = <0x40010000 0x400>;
-			interrupts = <GIC_SPI 52 IRQ_TYPE_LEVEL_HIGH>;
+			interrupts-extended = <&exti 30 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc UART4_K>;
+			resets = <&rcc UART4_R>;
+			wakeup-source;
 			status = "disabled";
 		};
 
@@ -468,99 +150,16 @@
 			reg = <0x40011000 0x400>;
 			interrupts = <GIC_SPI 53 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc UART5_K>;
+			resets = <&rcc UART5_R>;
 			status = "disabled";
 		};
 
-		i2c1: i2c@40012000 {
-			compatible = "st,stm32f7-i2c";
-			reg = <0x40012000 0x400>;
-			interrupt-names = "event", "error";
-			interrupts = <GIC_SPI 31 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 32 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc I2C1_K>;
-			resets = <&rcc I2C1_R>;
-			#address-cells = <1>;
-			#size-cells = <0>;
-			status = "disabled";
-		};
-
-		i2c2: i2c@40013000 {
-			compatible = "st,stm32f7-i2c";
-			reg = <0x40013000 0x400>;
-			interrupt-names = "event", "error";
-			interrupts = <GIC_SPI 33 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 34 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc I2C2_K>;
-			resets = <&rcc I2C2_R>;
-			#address-cells = <1>;
-			#size-cells = <0>;
-			status = "disabled";
-		};
-
-		i2c3: i2c@40014000 {
-			compatible = "st,stm32f7-i2c";
-			reg = <0x40014000 0x400>;
-			interrupt-names = "event", "error";
-			interrupts = <GIC_SPI 72 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 73 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc I2C3_K>;
-			resets = <&rcc I2C3_R>;
-			#address-cells = <1>;
-			#size-cells = <0>;
-			status = "disabled";
-		};
-
-		i2c5: i2c@40015000 {
-			compatible = "st,stm32f7-i2c";
-			reg = <0x40015000 0x400>;
-			interrupt-names = "event", "error";
-			interrupts = <GIC_SPI 107 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 108 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc I2C5_K>;
-			resets = <&rcc I2C5_R>;
-			#address-cells = <1>;
-			#size-cells = <0>;
-			status = "disabled";
-		};
-
-		cec: cec@40016000 {
-			compatible = "st,stm32-cec";
-			reg = <0x40016000 0x400>;
-			interrupts = <GIC_SPI 94 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc CEC_K>, <&clk_lse>;
-			clock-names = "cec", "hdmi-cec";
-			status = "disabled";
-		};
-
-		dac: dac@40017000 {
-			compatible = "st,stm32h7-dac-core";
-			reg = <0x40017000 0x400>;
-			clocks = <&rcc DAC12>;
-			clock-names = "pclk";
-			#address-cells = <1>;
-			#size-cells = <0>;
-			status = "disabled";
-
-			dac1: dac@1 {
-				compatible = "st,stm32-dac";
-				#io-channels-cells = <1>;
-				reg = <1>;
-				status = "disabled";
-			};
-
-			dac2: dac@2 {
-				compatible = "st,stm32-dac";
-				#io-channels-cells = <1>;
-				reg = <2>;
-				status = "disabled";
-			};
-		};
-
 		uart7: serial@40018000 {
 			compatible = "st,stm32h7-uart";
 			reg = <0x40018000 0x400>;
 			interrupts = <GIC_SPI 82 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc UART7_K>;
+			resets = <&rcc UART7_R>;
 			status = "disabled";
 		};
 
@@ -569,125 +168,16 @@
 			reg = <0x40019000 0x400>;
 			interrupts = <GIC_SPI 83 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc UART8_K>;
+			resets = <&rcc UART8_R>;
 			status = "disabled";
 		};
 
-		timers1: timer@44000000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32-timers";
-			reg = <0x44000000 0x400>;
-			clocks = <&rcc TIM1_K>;
-			clock-names = "int";
-			dmas = <&dmamux1 11 0x400 0x1>,
-			       <&dmamux1 12 0x400 0x1>,
-			       <&dmamux1 13 0x400 0x1>,
-			       <&dmamux1 14 0x400 0x1>,
-			       <&dmamux1 15 0x400 0x1>,
-			       <&dmamux1 16 0x400 0x1>,
-			       <&dmamux1 17 0x400 0x1>;
-			dma-names = "ch1", "ch2", "ch3", "ch4",
-				    "up", "trig", "com";
-			status = "disabled";
-
-			pwm {
-				compatible = "st,stm32-pwm";
-				#pwm-cells = <3>;
-				status = "disabled";
-			};
-
-			timer@0 {
-				compatible = "st,stm32h7-timer-trigger";
-				reg = <0>;
-				status = "disabled";
-			};
-
-			counter {
-				compatible = "st,stm32-timer-counter";
-				status = "disabled";
-			};
-		};
-
-		timers8: timer@44001000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32-timers";
-			reg = <0x44001000 0x400>;
-			clocks = <&rcc TIM8_K>;
-			clock-names = "int";
-			dmas = <&dmamux1 47 0x400 0x1>,
-			       <&dmamux1 48 0x400 0x1>,
-			       <&dmamux1 49 0x400 0x1>,
-			       <&dmamux1 50 0x400 0x1>,
-			       <&dmamux1 51 0x400 0x1>,
-			       <&dmamux1 52 0x400 0x1>,
-			       <&dmamux1 53 0x400 0x1>;
-			dma-names = "ch1", "ch2", "ch3", "ch4",
-				    "up", "trig", "com";
-			status = "disabled";
-
-			pwm {
-				compatible = "st,stm32-pwm";
-				#pwm-cells = <3>;
-				status = "disabled";
-			};
-
-			timer@7 {
-				compatible = "st,stm32h7-timer-trigger";
-				reg = <7>;
-				status = "disabled";
-			};
-
-			counter {
-				compatible = "st,stm32-timer-counter";
-				status = "disabled";
-			};
-		};
-
 		usart6: serial@44003000 {
 			compatible = "st,stm32h7-uart";
 			reg = <0x44003000 0x400>;
 			interrupts = <GIC_SPI 71 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc USART6_K>;
-			status = "disabled";
-		};
-
-		spi1: spi@44004000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32h7-spi";
-			reg = <0x44004000 0x400>;
-			interrupts = <GIC_SPI 35 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc SPI1_K>;
-			resets = <&rcc SPI1_R>;
-			dmas = <&dmamux1 37 0x400 0x05>,
-			       <&dmamux1 38 0x400 0x05>;
-			dma-names = "rx", "tx";
-			status = "disabled";
-		};
-
-		i2s1: audio-controller@44004000 {
-			compatible = "st,stm32h7-i2s";
-			#sound-dai-cells = <0>;
-			reg = <0x44004000 0x400>;
-			interrupts = <GIC_SPI 35 IRQ_TYPE_LEVEL_HIGH>;
-			dmas = <&dmamux1 37 0x400 0x01>,
-			       <&dmamux1 38 0x400 0x01>;
-			dma-names = "rx", "tx";
-			status = "disabled";
-		};
-
-		spi4: spi@44005000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32h7-spi";
-			reg = <0x44005000 0x400>;
-			interrupts = <GIC_SPI 84 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc SPI4_K>;
-			resets = <&rcc SPI4_R>;
-			dmas = <&dmamux1 83 0x400 0x05>,
-			       <&dmamux1 84 0x400 0x05>;
-			dma-names = "rx", "tx";
+			resets = <&rcc USART6_R>;
 			status = "disabled";
 		};
 
@@ -698,401 +188,42 @@
 			reg = <0x44006000 0x400>;
 			clocks = <&rcc TIM15_K>;
 			clock-names = "int";
-			dmas = <&dmamux1 105 0x400 0x1>,
-			       <&dmamux1 106 0x400 0x1>,
-			       <&dmamux1 107 0x400 0x1>,
-			       <&dmamux1 108 0x400 0x1>;
-			dma-names = "ch1", "up", "trig", "com";
-			status = "disabled";
-
-			pwm {
-				compatible = "st,stm32-pwm";
-				#pwm-cells = <3>;
-				status = "disabled";
-			};
-
-			timer@14 {
-				compatible = "st,stm32h7-timer-trigger";
-				reg = <14>;
-				status = "disabled";
-			};
-		};
-
-		timers16: timer@44007000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32-timers";
-			reg = <0x44007000 0x400>;
-			clocks = <&rcc TIM16_K>;
-			clock-names = "int";
-			dmas = <&dmamux1 109 0x400 0x1>,
-			       <&dmamux1 110 0x400 0x1>;
-			dma-names = "ch1", "up";
-			status = "disabled";
-
-			pwm {
-				compatible = "st,stm32-pwm";
-				#pwm-cells = <3>;
-				status = "disabled";
-			};
-			timer@15 {
-				compatible = "st,stm32h7-timer-trigger";
-				reg = <15>;
-				status = "disabled";
-			};
-		};
-
-		timers17: timer@44008000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32-timers";
-			reg = <0x44008000 0x400>;
-			clocks = <&rcc TIM17_K>;
-			clock-names = "int";
-			dmas = <&dmamux1 111 0x400 0x1>,
-			       <&dmamux1 112 0x400 0x1>;
-			dma-names = "ch1", "up";
-			status = "disabled";
-
-			pwm {
-				compatible = "st,stm32-pwm";
-				#pwm-cells = <3>;
-				status = "disabled";
-			};
-
-			timer@16 {
-				compatible = "st,stm32h7-timer-trigger";
-				reg = <16>;
-				status = "disabled";
-			};
-		};
-
-		spi5: spi@44009000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32h7-spi";
-			reg = <0x44009000 0x400>;
-			interrupts = <GIC_SPI 85 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc SPI5_K>;
-			resets = <&rcc SPI5_R>;
-			dmas = <&dmamux1 85 0x400 0x05>,
-			       <&dmamux1 86 0x400 0x05>;
-			dma-names = "rx", "tx";
-			status = "disabled";
-		};
-
-		sai1: sai@4400a000 {
-			compatible = "st,stm32h7-sai";
-			#address-cells = <1>;
-			#size-cells = <1>;
-			ranges = <0 0x4400a000 0x400>;
-			reg = <0x4400a000 0x4>, <0x4400a3f0 0x10>;
-			interrupts = <GIC_SPI 87 IRQ_TYPE_LEVEL_HIGH>;
-			resets = <&rcc SAI1_R>;
-			status = "disabled";
-
-			sai1a: audio-controller@4400a004 {
-				#sound-dai-cells = <0>;
-
-				compatible = "st,stm32-sai-sub-a";
-				reg = <0x4 0x1c>;
-				clocks = <&rcc SAI1_K>;
-				clock-names = "sai_ck";
-				dmas = <&dmamux1 87 0x400 0x01>;
-				status = "disabled";
-			};
-
-			sai1b: audio-controller@4400a024 {
-				#sound-dai-cells = <0>;
-				compatible = "st,stm32-sai-sub-b";
-				reg = <0x24 0x1c>;
-				clocks = <&rcc SAI1_K>;
-				clock-names = "sai_ck";
-				dmas = <&dmamux1 88 0x400 0x01>;
-				status = "disabled";
-			};
-		};
-
-		sai2: sai@4400b000 {
-			compatible = "st,stm32h7-sai";
-			#address-cells = <1>;
-			#size-cells = <1>;
-			ranges = <0 0x4400b000 0x400>;
-			reg = <0x4400b000 0x4>, <0x4400b3f0 0x10>;
-			interrupts = <GIC_SPI 91 IRQ_TYPE_LEVEL_HIGH>;
-			resets = <&rcc SAI2_R>;
-			status = "disabled";
-
-			sai2a: audio-controller@4400b004 {
-				#sound-dai-cells = <0>;
-				compatible = "st,stm32-sai-sub-a";
-				reg = <0x4 0x1c>;
-				clocks = <&rcc SAI2_K>;
-				clock-names = "sai_ck";
-				dmas = <&dmamux1 89 0x400 0x01>;
-				status = "disabled";
-			};
-
-			sai2b: audio-controller@4400b024 {
-				#sound-dai-cells = <0>;
-				compatible = "st,stm32-sai-sub-b";
-				reg = <0x24 0x1c>;
-				clocks = <&rcc SAI2_K>;
-				clock-names = "sai_ck";
-				dmas = <&dmamux1 90 0x400 0x01>;
-				status = "disabled";
-			};
-		};
-
-		sai3: sai@4400c000 {
-			compatible = "st,stm32h7-sai";
-			#address-cells = <1>;
-			#size-cells = <1>;
-			ranges = <0 0x4400c000 0x400>;
-			reg = <0x4400c000 0x4>, <0x4400c3f0 0x10>;
-			interrupts = <GIC_SPI 114 IRQ_TYPE_LEVEL_HIGH>;
-			resets = <&rcc SAI3_R>;
-			status = "disabled";
-
-			sai3a: audio-controller@4400c004 {
-				#sound-dai-cells = <0>;
-				compatible = "st,stm32-sai-sub-a";
-				reg = <0x04 0x1c>;
-				clocks = <&rcc SAI3_K>;
-				clock-names = "sai_ck";
-				dmas = <&dmamux1 113 0x400 0x01>;
-				status = "disabled";
-			};
-
-			sai3b: audio-controller@4400c024 {
-				#sound-dai-cells = <0>;
-				compatible = "st,stm32-sai-sub-b";
-				reg = <0x24 0x1c>;
-				clocks = <&rcc SAI3_K>;
-				clock-names = "sai_ck";
-				dmas = <&dmamux1 114 0x400 0x01>;
-				status = "disabled";
-			};
-		};
-
-		dfsdm: dfsdm@4400d000 {
-			compatible = "st,stm32mp1-dfsdm";
-			reg = <0x4400d000 0x800>;
-			clocks = <&rcc DFSDM_K>;
-			clock-names = "dfsdm";
-			#address-cells = <1>;
-			#size-cells = <0>;
-			status = "disabled";
-
-			dfsdm0: filter@0 {
-				compatible = "st,stm32-dfsdm-adc";
-				#io-channel-cells = <1>;
-				reg = <0>;
-				interrupts = <GIC_SPI 110 IRQ_TYPE_LEVEL_HIGH>;
-				dmas = <&dmamux1 101 0x400 0x01>;
-				dma-names = "rx";
-				status = "disabled";
-			};
-
-			dfsdm1: filter@1 {
-				compatible = "st,stm32-dfsdm-adc";
-				#io-channel-cells = <1>;
-				reg = <1>;
-				interrupts = <GIC_SPI 111 IRQ_TYPE_LEVEL_HIGH>;
-				dmas = <&dmamux1 102 0x400 0x01>;
-				dma-names = "rx";
-				status = "disabled";
-			};
-
-			dfsdm2: filter@2 {
-				compatible = "st,stm32-dfsdm-adc";
-				#io-channel-cells = <1>;
-				reg = <2>;
-				interrupts = <GIC_SPI 112 IRQ_TYPE_LEVEL_HIGH>;
-				dmas = <&dmamux1 103 0x400 0x01>;
-				dma-names = "rx";
-				status = "disabled";
-			};
-
-			dfsdm3: filter@3 {
-				compatible = "st,stm32-dfsdm-adc";
-				#io-channel-cells = <1>;
-				reg = <3>;
-				interrupts = <GIC_SPI 113 IRQ_TYPE_LEVEL_HIGH>;
-				dmas = <&dmamux1 104 0x400 0x01>;
-				dma-names = "rx";
-				status = "disabled";
-			};
-
-			dfsdm4: filter@4 {
-				compatible = "st,stm32-dfsdm-adc";
-				#io-channel-cells = <1>;
-				reg = <4>;
-				interrupts = <GIC_SPI 115 IRQ_TYPE_LEVEL_HIGH>;
-				dmas = <&dmamux1 91 0x400 0x01>;
-				dma-names = "rx";
-				status = "disabled";
-			};
-
-			dfsdm5: filter@5 {
-				compatible = "st,stm32-dfsdm-adc";
-				#io-channel-cells = <1>;
-				reg = <5>;
-				interrupts = <GIC_SPI 126 IRQ_TYPE_LEVEL_HIGH>;
-				dmas = <&dmamux1 92 0x400 0x01>;
-				dma-names = "rx";
-				status = "disabled";
-			};
-		};
-
-		dma1: dma-controller@48000000 {
-			compatible = "st,stm32-dma";
-			reg = <0x48000000 0x400>;
-			interrupts = <GIC_SPI 11 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 12 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 13 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 14 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 15 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 16 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 17 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 47 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc DMA1>;
-			#dma-cells = <4>;
-			st,mem2mem;
-			dma-requests = <8>;
-		};
-
-		dma2: dma-controller@48001000 {
-			compatible = "st,stm32-dma";
-			reg = <0x48001000 0x400>;
-			interrupts = <GIC_SPI 56 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 57 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 58 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 59 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 60 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 68 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 69 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 70 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc DMA2>;
-			#dma-cells = <4>;
-			st,mem2mem;
-			dma-requests = <8>;
-		};
-
-		dmamux1: dma-router@48002000 {
-			compatible = "st,stm32h7-dmamux";
-			reg = <0x48002000 0x1c>;
-			#dma-cells = <3>;
-			dma-requests = <128>;
-			dma-masters = <&dma1 &dma2>;
-			dma-channels = <16>;
-			clocks = <&rcc DMAMUX>;
-		};
-
-		adc: adc@48003000 {
-			compatible = "st,stm32mp1-adc-core";
-			reg = <0x48003000 0x400>;
-			interrupts = <GIC_SPI 18 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 90 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc ADC12>, <&rcc ADC12_K>;
-			clock-names = "bus", "adc";
-			interrupt-controller;
-			st,syscfg = <&syscfg>;
-			#interrupt-cells = <1>;
-			#address-cells = <1>;
-			#size-cells = <0>;
-			status = "disabled";
-
-			adc1: adc@0 {
-				compatible = "st,stm32mp1-adc";
-				#io-channel-cells = <1>;
-				reg = <0x0>;
-				interrupt-parent = <&adc>;
-				interrupts = <0>;
-				dmas = <&dmamux1 9 0x400 0x01>;
-				dma-names = "rx";
-				status = "disabled";
-			};
-
-			adc2: adc@100 {
-				compatible = "st,stm32mp1-adc";
-				#io-channel-cells = <1>;
-				reg = <0x100>;
-				interrupt-parent = <&adc>;
-				interrupts = <1>;
-				dmas = <&dmamux1 10 0x400 0x01>;
-				dma-names = "rx";
-				status = "disabled";
-			};
-		};
-
-		sdmmc3: sdmmc@48004000 {
-			compatible = "arm,pl18x", "arm,primecell";
-			arm,primecell-periphid = <0x10153180>;
-			reg = <0x48004000 0x400>;
-			interrupts = <GIC_SPI 137 IRQ_TYPE_LEVEL_HIGH>;
-			interrupt-names = "cmd_irq";
-			clocks = <&rcc SDMMC3_K>;
-			clock-names = "apb_pclk";
-			resets = <&rcc SDMMC3_R>;
-			cap-sd-highspeed;
-			cap-mmc-highspeed;
-			max-frequency = <120000000>;
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		usbotg_hs: usb-otg@49000000 {
-			compatible = "snps,dwc2";
+			compatible = "st,stm32mp15-hsotg", "snps,dwc2";
 			reg = <0x49000000 0x10000>;
 			clocks = <&rcc USBO_K>;
 			clock-names = "otg";
 			resets = <&rcc USBO_R>;
 			reset-names = "dwc2";
 			interrupts = <GIC_SPI 98 IRQ_TYPE_LEVEL_HIGH>;
-			g-rx-fifo-size = <256>;
+			g-rx-fifo-size = <512>;
 			g-np-tx-fifo-size = <32>;
-			g-tx-fifo-size = <128 128 64 64 64 64 32 32>;
+			g-tx-fifo-size = <256 16 16 16 16 16 16 16>;
 			dr_mode = "otg";
-			status = "disabled";
-		};
-
-		ipcc: mailbox@4c001000 {
-			compatible = "st,stm32mp1-ipcc";
-			#mbox-cells = <1>;
-			reg = <0x4c001000 0x400>;
-			st,proc-id = <0>;
-			interrupts-extended =
-				<&intc GIC_SPI 100 IRQ_TYPE_LEVEL_HIGH>,
-				<&intc GIC_SPI 101 IRQ_TYPE_LEVEL_HIGH>,
-				<&exti 61 1>;
-			interrupt-names = "rx", "tx", "wakeup";
-			clocks = <&rcc IPCC>;
-			wakeup-source;
-			status = "disabled";
-		};
-
-		dcmi: dcmi@4c006000 {
-			compatible = "st,stm32-dcmi";
-			reg = <0x4c006000 0x400>;
-			interrupts = <GIC_SPI 78 IRQ_TYPE_LEVEL_HIGH>;
-			resets = <&rcc CAMITF_R>;
-			clocks = <&rcc DCMI>;
-			clock-names = "mclk";
-			dmas = <&dmamux1 75 0x400 0x0d>;
-			dma-names = "tx";
+			usb33d-supply = <&usb33>;
 			status = "disabled";
 		};
 
 		rcc: rcc@50000000 {
-			compatible = "st,stm32mp1-rcc", "syscon";
+			compatible = "st,stm32mp1-rcc-secure", "st,stm32mp1-rcc", "syscon";
 			reg = <0x50000000 0x1000>;
+			#address-cells = <1>;
+			#size-cells = <0>;
 			#clock-cells = <1>;
 			#reset-cells = <1>;
+			interrupts = <GIC_SPI 5 IRQ_TYPE_LEVEL_HIGH>;
+			secure-interrupts = <GIC_SPI 145 IRQ_TYPE_LEVEL_HIGH>;
+			secure-interrupt-names = "wakeup";
 		};
 
 		pwr_regulators: pwr@50001000 {
 			compatible = "st,stm32mp1,pwr-reg";
 			reg = <0x50001000 0x10>;
+			st,tzcr = <&rcc 0x0 0x1>;
 
 			reg11: reg11 {
 				regulator-name = "reg11";
@@ -1113,11 +244,35 @@
 			};
 		};
 
+		pwr_mcu: pwr_mcu@50001014 {
+			compatible = "st,stm32mp151-pwr-mcu", "syscon";
+			reg = <0x50001014 0x4>;
+		};
+
+		pwr_irq: pwr@50001020 {
+			compatible = "st,stm32mp1-pwr";
+			reg = <0x50001020 0x100>;
+			interrupts = <GIC_SPI 149 IRQ_TYPE_LEVEL_HIGH>;
+			interrupt-controller;
+			#interrupt-cells = <3>;
+		};
+
 		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_irq>;
+				st,irq-number = <6>;
+			};
 		};
 
 		syscfg: syscon@50020000 {
@@ -1126,143 +281,14 @@
 			clocks = <&rcc SYSCFG>;
 		};
 
-		lptimer2: timer@50021000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32-lptimer";
-			reg = <0x50021000 0x400>;
-			clocks = <&rcc LPTIM2_K>;
-			clock-names = "mux";
-			status = "disabled";
-
-			pwm {
-				compatible = "st,stm32-pwm-lp";
-				#pwm-cells = <3>;
-				status = "disabled";
-			};
-
-			trigger@1 {
-				compatible = "st,stm32-lptimer-trigger";
-				reg = <1>;
-				status = "disabled";
-			};
-
-			counter {
-				compatible = "st,stm32-lptimer-counter";
-				status = "disabled";
-			};
-		};
-
-		lptimer3: timer@50022000 {
-			#address-cells = <1>;
-			#size-cells = <0>;
-			compatible = "st,stm32-lptimer";
-			reg = <0x50022000 0x400>;
-			clocks = <&rcc LPTIM3_K>;
-			clock-names = "mux";
-			status = "disabled";
-
-			pwm {
-				compatible = "st,stm32-pwm-lp";
-				#pwm-cells = <3>;
-				status = "disabled";
-			};
-
-			trigger@2 {
-				compatible = "st,stm32-lptimer-trigger";
-				reg = <2>;
-				status = "disabled";
-			};
-		};
-
-		lptimer4: timer@50023000 {
-			compatible = "st,stm32-lptimer";
-			reg = <0x50023000 0x400>;
-			clocks = <&rcc LPTIM4_K>;
-			clock-names = "mux";
-			status = "disabled";
-
-			pwm {
-				compatible = "st,stm32-pwm-lp";
-				#pwm-cells = <3>;
-				status = "disabled";
-			};
-		};
-
-		lptimer5: timer@50024000 {
-			compatible = "st,stm32-lptimer";
-			reg = <0x50024000 0x400>;
-			clocks = <&rcc LPTIM5_K>;
-			clock-names = "mux";
-			status = "disabled";
-
-			pwm {
-				compatible = "st,stm32-pwm-lp";
-				#pwm-cells = <3>;
-				status = "disabled";
-			};
-		};
-
-		vrefbuf: vrefbuf@50025000 {
-			compatible = "st,stm32-vrefbuf";
-			reg = <0x50025000 0x8>;
-			regulator-min-microvolt = <1500000>;
-			regulator-max-microvolt = <2500000>;
-			clocks = <&rcc VREF>;
-			status = "disabled";
-		};
-
-		sai4: sai@50027000 {
-			compatible = "st,stm32h7-sai";
-			#address-cells = <1>;
-			#size-cells = <1>;
-			ranges = <0 0x50027000 0x400>;
-			reg = <0x50027000 0x4>, <0x500273f0 0x10>;
-			interrupts = <GIC_SPI 146 IRQ_TYPE_LEVEL_HIGH>;
-			resets = <&rcc SAI4_R>;
-			status = "disabled";
-
-			sai4a: audio-controller@50027004 {
-				#sound-dai-cells = <0>;
-				compatible = "st,stm32-sai-sub-a";
-				reg = <0x04 0x1c>;
-				clocks = <&rcc SAI4_K>;
-				clock-names = "sai_ck";
-				dmas = <&dmamux1 99 0x400 0x01>;
-				status = "disabled";
-			};
-
-			sai4b: audio-controller@50027024 {
-				#sound-dai-cells = <0>;
-				compatible = "st,stm32-sai-sub-b";
-				reg = <0x24 0x1c>;
-				clocks = <&rcc SAI4_K>;
-				clock-names = "sai_ck";
-				dmas = <&dmamux1 100 0x400 0x01>;
-				status = "disabled";
-			};
-		};
-
-		dts: thermal@50028000 {
-			compatible = "st,stm32-thermal";
-			reg = <0x50028000 0x100>;
-			interrupts = <GIC_SPI 147 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc TMPSENS>;
-			clock-names = "pclk";
-			#thermal-sensor-cells = <0>;
-			status = "disabled";
-		};
-
 		hash1: hash@54002000 {
 			compatible = "st,stm32f756-hash";
 			reg = <0x54002000 0x400>;
 			interrupts = <GIC_SPI 80 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc HASH1>;
 			resets = <&rcc HASH1_R>;
-			dmas = <&mdma1 31 0x10 0x1000A02 0x0 0x0>;
-			dma-names = "in";
-			dma-maxburst = <2>;
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		rng1: rng@54003000 {
@@ -1271,35 +297,37 @@
 			clocks = <&rcc RNG1_K>;
 			resets = <&rcc RNG1_R>;
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
-		mdma1: dma-controller@58000000 {
-			compatible = "st,stm32h7-mdma";
-			reg = <0x58000000 0x1000>;
-			interrupts = <GIC_SPI 122 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc MDMA>;
-			#dma-cells = <5>;
-			dma-channels = <32>;
-			dma-requests = <48>;
-		};
-
-		fmc: nand-controller@58002000 {
-			compatible = "st,stm32mp15-fmc2";
-			reg = <0x58002000 0x1000>,
-			      <0x80000000 0x1000>,
-			      <0x88010000 0x1000>,
-			      <0x88020000 0x1000>,
-			      <0x81000000 0x1000>,
-			      <0x89010000 0x1000>,
-			      <0x89020000 0x1000>;
-			interrupts = <GIC_SPI 48 IRQ_TYPE_LEVEL_HIGH>;
-			dmas = <&mdma1 20 0x10 0x12000a02 0x0 0x0>,
-			       <&mdma1 20 0x10 0x12000a08 0x0 0x0>,
-			       <&mdma1 21 0x10 0x12000a0a 0x0 0x0>;
-			dma-names = "tx", "rx", "ecc";
+		fmc: memory-controller@58002000 {
+			#address-cells = <2>;
+			#size-cells = <1>;
+			compatible = "st,stm32mp1-fmc2-ebi";
+			reg = <0x58002000 0x1000>;
 			clocks = <&rcc FMC_K>;
 			resets = <&rcc FMC_R>;
 			status = "disabled";
+
+			ranges = <0 0 0x60000000 0x04000000>, /* EBI CS 1 */
+				 <1 0 0x64000000 0x04000000>, /* EBI CS 2 */
+				 <2 0 0x68000000 0x04000000>, /* EBI CS 3 */
+				 <3 0 0x6c000000 0x04000000>, /* EBI CS 4 */
+				 <4 0 0x80000000 0x10000000>; /* NAND */
+
+			nand-controller@4,0 {
+				#address-cells = <1>;
+				#size-cells = <0>;
+				compatible = "st,stm32mp1-fmc2-nfc";
+				reg = <4 0x00000000 0x1000>,
+				      <4 0x08010000 0x1000>,
+				      <4 0x08020000 0x1000>,
+				      <4 0x01000000 0x1000>,
+				      <4 0x09010000 0x1000>,
+				      <4 0x09020000 0x1000>;
+				interrupts = <GIC_SPI 48 IRQ_TYPE_LEVEL_HIGH>;
+				status = "disabled";
+			};
 		};
 
 		qspi: spi@58003000 {
@@ -1307,18 +335,17 @@
 			reg = <0x58003000 0x1000>, <0x70000000 0x10000000>;
 			reg-names = "qspi", "qspi_mm";
 			interrupts = <GIC_SPI 92 IRQ_TYPE_LEVEL_HIGH>;
-			dmas = <&mdma1 22 0x10 0x100002 0x0 0x0>,
-			       <&mdma1 22 0x10 0x100008 0x0 0x0>;
-			dma-names = "tx", "rx";
 			clocks = <&rcc QSPI_K>;
 			resets = <&rcc QSPI_R>;
+			#address-cells = <1>;
+			#size-cells = <0>;
 			status = "disabled";
 		};
 
 		sdmmc1: sdmmc@58005000 {
-			compatible = "arm,pl18x", "arm,primecell";
-			arm,primecell-periphid = <0x10153180>;
-			reg = <0x58005000 0x1000>;
+			compatible = "st,stm32-sdmmc2", "arm,pl18x", "arm,primecell";
+			arm,primecell-periphid = <0x00253180>;
+			reg = <0x58005000 0x1000>, <0x58006000 0x1000>;
 			interrupts = <GIC_SPI 49 IRQ_TYPE_LEVEL_HIGH>;
 			interrupt-names = "cmd_irq";
 			clocks = <&rcc SDMMC1_K>;
@@ -1331,9 +358,9 @@
 		};
 
 		sdmmc2: sdmmc@58007000 {
-			compatible = "arm,pl18x", "arm,primecell";
-			arm,primecell-periphid = <0x10153180>;
-			reg = <0x58007000 0x1000>;
+			compatible = "st,stm32-sdmmc2", "arm,pl18x", "arm,primecell";
+			arm,primecell-periphid = <0x00253180>;
+			reg = <0x58007000 0x1000>, <0x58008000 0x1000>;
 			interrupts = <GIC_SPI 124 IRQ_TYPE_LEVEL_HIGH>;
 			interrupt-names = "cmd_irq";
 			clocks = <&rcc SDMMC2_K>;
@@ -1345,87 +372,26 @@
 			status = "disabled";
 		};
 
-		crc1: crc@58009000 {
-			compatible = "st,stm32f7-crc";
-			reg = <0x58009000 0x400>;
-			clocks = <&rcc CRC1>;
-			status = "disabled";
-		};
-
-		stmmac_axi_config_0: stmmac-axi-config {
-			snps,wr_osr_lmt = <0x7>;
-			snps,rd_osr_lmt = <0x7>;
-			snps,blen = <0 0 0 0 16 8 4>;
-		};
-
-		ethernet0: ethernet@5800a000 {
-			compatible = "st,stm32mp1-dwmac", "snps,dwmac-4.20a";
-			reg = <0x5800a000 0x2000>;
-			reg-names = "stmmaceth";
-			interrupts-extended = <&intc GIC_SPI 61 IRQ_TYPE_LEVEL_HIGH>;
-			interrupt-names = "macirq";
-			clock-names = "stmmaceth",
-				      "mac-clk-tx",
-				      "mac-clk-rx",
-				      "ethstp";
-			clocks = <&rcc ETHMAC>,
-				 <&rcc ETHTX>,
-				 <&rcc ETHRX>,
-				 <&rcc ETHSTP>;
-			st,syscon = <&syscfg 0x4>;
-			snps,mixed-burst;
-			snps,pbl = <2>;
-			snps,en-tx-lpi-clockgating;
-			snps,axi-config = <&stmmac_axi_config_0>;
-			snps,tso;
-			status = "disabled";
-		};
-
-		usbh_ohci: usbh-ohci@5800c000 {
-			compatible = "generic-ohci";
-			reg = <0x5800c000 0x1000>;
-			clocks = <&rcc USBH>;
-			resets = <&rcc USBH_R>;
-			interrupts = <GIC_SPI 74 IRQ_TYPE_LEVEL_HIGH>;
-			status = "disabled";
-		};
-
-		usbh_ehci: usbh-ehci@5800d000 {
-			compatible = "generic-ehci";
-			reg = <0x5800d000 0x1000>;
-			clocks = <&rcc USBH>;
-			resets = <&rcc USBH_R>;
-			interrupts = <GIC_SPI 75 IRQ_TYPE_LEVEL_HIGH>;
-			companion = <&usbh_ohci>;
-			status = "disabled";
-		};
-
-		ltdc: display-controller@5a001000 {
-			compatible = "st,stm32-ltdc";
-			reg = <0x5a001000 0x400>;
-			interrupts = <GIC_SPI 88 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 89 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc LTDC_PX>;
-			clock-names = "lcd";
-			resets = <&rcc LTDC_R>;
-			status = "disabled";
-		};
-
 		iwdg2: watchdog@5a002000 {
 			compatible = "st,stm32mp1-iwdg";
 			reg = <0x5a002000 0x400>;
+			secure-interrupts = <GIC_SPI 151 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc IWDG2>, <&rcc CK_LSI>;
 			clock-names = "pclk", "lsi";
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		usbphyc: usbphyc@5a006000 {
 			#address-cells = <1>;
 			#size-cells = <0>;
+			#clock-cells = <0>;
 			compatible = "st,stm32mp1-usbphyc";
 			reg = <0x5a006000 0x1000>;
 			clocks = <&rcc USBPHY_K>;
 			resets = <&rcc USBPHY_R>;
+			vdda1v1-supply = <&reg11>;
+			vdda1v8-supply = <&reg18>;
 			status = "disabled";
 
 			usbphyc_port0: usb-phy@0 {
@@ -1446,6 +412,7 @@
 			clocks = <&rcc USART1_K>;
 			resets = <&rcc USART1_R>;
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		spi6: spi@5c001000 {
@@ -1456,22 +423,34 @@
 			interrupts = <GIC_SPI 86 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc SPI6_K>;
 			resets = <&rcc SPI6_R>;
-			dmas = <&mdma1 34 0x0 0x40008 0x0 0x0>,
-			       <&mdma1 35 0x0 0x40002 0x0 0x0>;
-			dma-names = "rx", "tx";
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		i2c4: i2c@5c002000 {
-			compatible = "st,stm32f7-i2c";
+			compatible = "st,stm32mp15-i2c";
 			reg = <0x5c002000 0x400>;
 			interrupt-names = "event", "error";
-			interrupts = <GIC_SPI 95 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 96 IRQ_TYPE_LEVEL_HIGH>;
+			interrupts-extended = <&exti 24 IRQ_TYPE_LEVEL_HIGH>,
+					      <&intc GIC_SPI 96 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc I2C4_K>;
 			resets = <&rcc I2C4_R>;
 			#address-cells = <1>;
 			#size-cells = <0>;
+			st,syscfg-fmp = <&syscfg 0x4 0x8>;
+			wakeup-source;
+			status = "disabled";
+			secure-status = "disabled";
+		};
+
+		iwdg1: watchdog@5c003000 {
+			compatible = "st,stm32mp1-iwdg";
+			reg = <0x5C003000 0x400>;
+			interrupts = <GIC_SPI 150 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&rcc IWDG1>, <&rcc CK_LSI>;
+			clock-names = "pclk", "lsi";
+			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		rtc: rtc@5c004000 {
@@ -1479,8 +458,9 @@
 			reg = <0x5c004000 0x400>;
 			clocks = <&rcc RTCAPB>, <&rcc RTC>;
 			clock-names = "pclk", "rtc_ck";
-			interrupts = <GIC_SPI 3 IRQ_TYPE_LEVEL_HIGH>;
+			interrupts-extended = <&exti 19 IRQ_TYPE_LEVEL_HIGH>;
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		bsec: efuse@5c005000 {
@@ -1488,13 +468,38 @@
 			reg = <0x5c005000 0x400>;
 			#address-cells = <1>;
 			#size-cells = <1>;
+
+			cfg0_otp: cfg0_otp@0 {
+				reg = <0x0 0x1>;
+			};
+			part_number_otp: part_number_otp@4 {
+				reg = <0x4 0x1>;
+			};
+			monotonic_otp: monotonic_otp@10 {
+				reg = <0x10 0x4>;
+			};
+			nand_otp: nand_otp@24 {
+				reg = <0x24 0x4>;
+			};
+			uid_otp: uid_otp@34 {
+				reg = <0x34 0xc>;
+			};
+			package_otp: package_otp@40 {
+				reg = <0x40 0x4>;
+			};
+			hw2_otp: hw2_otp@48 {
+				reg = <0x48 0x4>;
+			};
 			ts_cal1: calib@5c {
 				reg = <0x5c 0x2>;
 			};
 			ts_cal2: calib@5e {
 				reg = <0x5e 0x2>;
 			};
-			mac_addr: mac_addr@e4 {
+			pkh_otp: pkh_otp@60 {
+				reg = <0x60 0x20>;
+			};
+			ethernet_mac_address: mac@e4 {
 				reg = <0xe4 0x8>;
 				st,non-secure-otp;
 			};
@@ -1508,17 +513,32 @@
 			secure-status = "okay";
 		};
 
+		stgen: stgen@5c008000 {
+			compatible = "st,stm32-stgen";
+			reg = <0x5C008000 0x1000>;
+		};
+
 		i2c6: i2c@5c009000 {
-			compatible = "st,stm32f7-i2c";
+			compatible = "st,stm32mp15-i2c";
 			reg = <0x5c009000 0x400>;
 			interrupt-names = "event", "error";
-			interrupts = <GIC_SPI 135 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 136 IRQ_TYPE_LEVEL_HIGH>;
+			interrupts-extended = <&exti 54 IRQ_TYPE_LEVEL_HIGH>,
+					      <&intc GIC_SPI 136 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc I2C6_K>;
 			resets = <&rcc I2C6_R>;
 			#address-cells = <1>;
 			#size-cells = <0>;
+			st,syscfg-fmp = <&syscfg 0x4 0x20>;
+			wakeup-source;
 			status = "disabled";
+			secure-status = "disabled";
+		};
+
+		tamp: tamp@5c00a000 {
+			compatible = "st,stm32-tamp", "simple-bus", "syscon", "simple-mfd";
+			reg = <0x5c00a000 0x400>;
+			secure-interrupts = <GIC_SPI 197 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&rcc RTCAPB>;
 		};
 
 		/*
@@ -1675,28 +695,8 @@
 				st,bank-name = "GPIOZ";
 				st,bank-ioport = <11>;
 				status = "disabled";
+				secure-status = "disabled";
 			};
 		};
 	};
-
-	mlahb: ahb {
-		compatible = "st,mlahb", "simple-bus";
-		#address-cells = <1>;
-		#size-cells = <1>;
-		ranges;
-		dma-ranges = <0x00000000 0x38000000 0x10000>,
-			     <0x10000000 0x10000000 0x60000>,
-			     <0x30000000 0x30000000 0x60000>;
-
-		m4_rproc: m4@10000000 {
-			compatible = "st,stm32mp1-m4";
-			reg = <0x10000000 0x40000>,
-			      <0x30000000 0x40000>,
-			      <0x38000000 0x10000>;
-			resets = <&rcc MCU_R>;
-			st,syscfg-holdboot = <&rcc 0x10C 0x1>;
-			st,syscfg-tz = <&rcc 0x000 0x1>;
-			status = "disabled";
-		};
-	};
 };
diff --git a/core/arch/arm/dts/stm32mp153.dtsi b/core/arch/arm/dts/stm32mp153.dtsi
index 2d759fc60..617380a52 100644
--- a/core/arch/arm/dts/stm32mp153.dtsi
+++ b/core/arch/arm/dts/stm32mp153.dtsi
@@ -12,34 +12,9 @@
 			compatible = "arm,cortex-a7";
 			device_type = "cpu";
 			reg = <1>;
-		};
-	};
-
-	soc {
-		m_can1: can@4400e000 {
-			compatible = "bosch,m_can";
-			reg = <0x4400e000 0x400>, <0x44011000 0x1400>;
-			reg-names = "m_can", "message_ram";
-			interrupts = <GIC_SPI 19 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 21 IRQ_TYPE_LEVEL_HIGH>;
-			interrupt-names = "int0", "int1";
-			clocks = <&rcc CK_HSE>, <&rcc FDCAN_K>;
-			clock-names = "hclk", "cclk";
-			bosch,mram-cfg = <0x0 0 0 32 0 0 2 2>;
-			status = "disabled";
-		};
-
-		m_can2: can@4400f000 {
-			compatible = "bosch,m_can";
-			reg = <0x4400f000 0x400>, <0x44011000 0x2800>;
-			reg-names = "m_can", "message_ram";
-			interrupts = <GIC_SPI 20 IRQ_TYPE_LEVEL_HIGH>,
-				     <GIC_SPI 22 IRQ_TYPE_LEVEL_HIGH>;
-			interrupt-names = "int0", "int1";
-			clocks = <&rcc CK_HSE>, <&rcc FDCAN_K>;
-			clock-names = "hclk", "cclk";
-			bosch,mram-cfg = <0x1400 0 0 32 0 0 2 2>;
-			status = "disabled";
+			clocks = <&rcc CK_MPU>;
+			clock-names = "cpu";
+			operating-points-v2 = <&cpu0_opp_table>;
 		};
 	};
 };
diff --git a/core/arch/arm/dts/stm32mp157.dtsi b/core/arch/arm/dts/stm32mp157.dtsi
index 3f0a4a91c..c83402907 100644
--- a/core/arch/arm/dts/stm32mp157.dtsi
+++ b/core/arch/arm/dts/stm32mp157.dtsi
@@ -5,27 +5,3 @@
  */
 
 #include "stm32mp153.dtsi"
-
-/ {
-	soc {
-		gpu: gpu@59000000 {
-			compatible = "vivante,gc";
-			reg = <0x59000000 0x800>;
-			interrupts = <GIC_SPI 109 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc GPU>, <&rcc GPU_K>;
-			clock-names = "bus" ,"core";
-			resets = <&rcc GPU_R>;
-			status = "disabled";
-		};
-
-		dsi: dsi@5a000000 {
-			compatible = "st,stm32-dsi";
-			reg = <0x5a000000 0x800>;
-			clocks = <&rcc DSI_K>, <&clk_hse>, <&rcc DSI_PX>;
-			clock-names = "pclk", "ref", "px_clk";
-			resets = <&rcc DSI_R>;
-			reset-names = "apb";
-			status = "disabled";
-		};
-	};
-};
diff --git a/core/arch/arm/dts/stm32mp157a-dk1.dts b/core/arch/arm/dts/stm32mp157a-dk1.dts
index bc9ee69e6..5d5c0a5f7 100644
--- a/core/arch/arm/dts/stm32mp157a-dk1.dts
+++ b/core/arch/arm/dts/stm32mp157a-dk1.dts
@@ -7,17 +7,20 @@
 /dts-v1/;
 
 #include "stm32mp157.dtsi"
+#include "stm32mp15xa.dtsi"
 #include "stm32mp15-pinctrl.dtsi"
 #include "stm32mp15xxac-pinctrl.dtsi"
 #include "stm32mp15xx-dkx.dtsi"
+#include <dt-bindings/soc/st,stm32-etzpc.h>
 
 / {
 	model = "STMicroelectronics STM32MP157A-DK1 Discovery Board";
 	compatible = "st,stm32mp157a-dk1", "st,stm32mp157";
 
 	aliases {
-		ethernet0 = &ethernet0;
 		serial0 = &uart4;
+		serial1 = &usart3;
+		serial2 = &uart7;
 	};
 
 	chosen {
@@ -25,14 +28,18 @@
 	};
 };
 
-&rcc {
-	status = "okay";
-	secure-status = "disable";
-};
-
-&bsec {
-	board_id: board_id@ec {
-		reg = <0xec 0x4>;
-		st,non-secure-otp;
-	};
+&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_DDRCTRL_ID, DECPROT_NS_R_S_W, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_DDRPHYC_ID, DECPROT_NS_R_S_W, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_STGENC_ID, DECPROT_S_RW, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_BKPSRAM_ID, DECPROT_S_RW, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_IWDG1_ID, DECPROT_S_RW, DECPROT_LOCK)
+	>;
 };
diff --git a/core/arch/arm/dts/stm32mp157a-ed1.dts b/core/arch/arm/dts/stm32mp157a-ed1.dts
new file mode 100644
index 000000000..1527b642a
--- /dev/null
+++ b/core/arch/arm/dts/stm32mp157a-ed1.dts
@@ -0,0 +1,38 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
+ */
+/dts-v1/;
+
+#include "stm32mp157.dtsi"
+#include "stm32mp15xa.dtsi"
+#include "stm32mp15-pinctrl.dtsi"
+#include "stm32mp15xxaa-pinctrl.dtsi"
+#include "stm32mp15xx-edx.dtsi"
+#include <dt-bindings/soc/st,stm32-etzpc.h>
+
+/ {
+	model = "STMicroelectronics STM32MP157A eval daughter";
+	compatible = "st,stm32mp157a-ed1", "st,stm32mp157";
+
+	chosen {
+		stdout-path = "serial0:115200n8";
+	};
+};
+
+&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_DDRCTRL_ID, DECPROT_NS_R_S_W, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_DDRPHYC_ID, DECPROT_NS_R_S_W, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_STGENC_ID, DECPROT_S_RW, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_BKPSRAM_ID, DECPROT_S_RW, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_IWDG1_ID, DECPROT_S_RW, DECPROT_LOCK)
+	>;
+};
diff --git a/core/arch/arm/dts/stm32mp157a-ev1.dts b/core/arch/arm/dts/stm32mp157a-ev1.dts
new file mode 100644
index 000000000..3cb35698a
--- /dev/null
+++ b/core/arch/arm/dts/stm32mp157a-ev1.dts
@@ -0,0 +1,24 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
+ */
+/dts-v1/;
+
+#include "stm32mp157a-ed1.dts"
+#include "stm32mp15xx-evx.dtsi"
+
+/ {
+	model = "STMicroelectronics STM32MP157A eval daughter on eval mother";
+	compatible = "st,stm32mp157a-ev1", "st,stm32mp157a-ed1", "st,stm32mp157";
+
+	chosen {
+		stdout-path = "serial0:115200n8";
+	};
+
+	aliases {
+		serial0 = &uart4;
+		serial1 = &usart3;
+	};
+};
+
diff --git a/core/arch/arm/dts/stm32mp157c-dk2.dts b/core/arch/arm/dts/stm32mp157c-dk2.dts
index c5b226de6..ff5c4509f 100644
--- a/core/arch/arm/dts/stm32mp157c-dk2.dts
+++ b/core/arch/arm/dts/stm32mp157c-dk2.dts
@@ -11,14 +11,17 @@
 #include "stm32mp15-pinctrl.dtsi"
 #include "stm32mp15xxac-pinctrl.dtsi"
 #include "stm32mp15xx-dkx.dtsi"
+#include <dt-bindings/soc/st,stm32-etzpc.h>
 
 / {
 	model = "STMicroelectronics STM32MP157C-DK2 Discovery Board";
 	compatible = "st,stm32mp157c-dk2", "st,stm32mp157";
 
 	aliases {
-		ethernet0 = &ethernet0;
 		serial0 = &uart4;
+		serial1 = &usart3;
+		serial2 = &uart7;
+		serial3 = &usart2;
 	};
 
 	chosen {
@@ -26,81 +29,23 @@
 	};
 };
 
-&dsi {
-	#address-cells = <1>;
-	#size-cells = <0>;
+&cryp1 {
 	status = "okay";
-	phy-dsi-supply = <&reg18>;
-
-	ports {
-		#address-cells = <1>;
-		#size-cells = <0>;
-
-		port@0 {
-			reg = <0>;
-			dsi_in: endpoint {
-				remote-endpoint = <&ltdc_ep1_out>;
-			};
-		};
-
-		port@1 {
-			reg = <1>;
-			dsi_out: endpoint {
-				remote-endpoint = <&panel_in>;
-			};
-		};
-	};
-
-	panel@0 {
-		compatible = "orisetech,otm8009a";
-		reg = <0>;
-		reset-gpios = <&gpioe 4 GPIO_ACTIVE_LOW>;
-		power-supply = <&v3v3>;
-		status = "okay";
-
-		port {
-			panel_in: endpoint {
-				remote-endpoint = <&dsi_out>;
-			};
-		};
-	};
-};
-
-&i2c1 {
-	touchscreen@38 {
-		compatible = "focaltech,ft6236";
-		reg = <0x38>;
-		interrupts = <2 2>;
-		interrupt-parent = <&gpiof>;
-		interrupt-controller;
-		touchscreen-size-x = <480>;
-		touchscreen-size-y = <800>;
-		status = "okay";
-	};
 };
 
-&ltdc {
-	status = "okay";
-
-	port {
-		#address-cells = <1>;
-		#size-cells = <0>;
-
-		ltdc_ep1_out: endpoint@1 {
-			reg = <1>;
-			remote-endpoint = <&dsi_in>;
-		};
-	};
-};
-
-&rcc {
-	status = "okay";
-	secure-status = "disable";
-};
-
-&bsec {
-	board_id: board_id@ec {
-		reg = <0xec 0x4>;
-		st,non-secure-otp;
-	};
+&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)
+		DECPROT(STM32MP1_ETZPC_DDRCTRL_ID, DECPROT_NS_R_S_W, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_DDRPHYC_ID, DECPROT_NS_R_S_W, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_STGENC_ID, DECPROT_S_RW, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_BKPSRAM_ID, DECPROT_S_RW, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_IWDG1_ID, DECPROT_S_RW, DECPROT_LOCK)
+	>;
 };
diff --git a/core/arch/arm/dts/stm32mp157c-ed1.dts b/core/arch/arm/dts/stm32mp157c-ed1.dts
index ea301c550..8d8014751 100644
--- a/core/arch/arm/dts/stm32mp157c-ed1.dts
+++ b/core/arch/arm/dts/stm32mp157c-ed1.dts
@@ -1,7 +1,7 @@
 // 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.
+ * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
  */
 /dts-v1/;
 
@@ -9,8 +9,8 @@
 #include "stm32mp15xc.dtsi"
 #include "stm32mp15-pinctrl.dtsi"
 #include "stm32mp15xxaa-pinctrl.dtsi"
-#include <dt-bindings/gpio/gpio.h>
-#include <dt-bindings/mfd/st,stpmic1.h>
+#include "stm32mp15xx-edx.dtsi"
+#include <dt-bindings/soc/st,stm32-etzpc.h>
 
 / {
 	model = "STMicroelectronics STM32MP157C eval daughter";
@@ -19,362 +19,25 @@
 	chosen {
 		stdout-path = "serial0:115200n8";
 	};
-
-	memory@c0000000 {
-		device_type = "memory";
-		reg = <0xC0000000 0x40000000>;
-	};
-
-	reserved-memory {
-		#address-cells = <1>;
-		#size-cells = <1>;
-		ranges;
-
-		mcuram2: mcuram2@10000000 {
-			compatible = "shared-dma-pool";
-			reg = <0x10000000 0x40000>;
-			no-map;
-		};
-
-		vdev0vring0: vdev0vring0@10040000 {
-			compatible = "shared-dma-pool";
-			reg = <0x10040000 0x1000>;
-			no-map;
-		};
-
-		vdev0vring1: vdev0vring1@10041000 {
-			compatible = "shared-dma-pool";
-			reg = <0x10041000 0x1000>;
-			no-map;
-		};
-
-		vdev0buffer: vdev0buffer@10042000 {
-			compatible = "shared-dma-pool";
-			reg = <0x10042000 0x4000>;
-			no-map;
-		};
-
-		mcuram: mcuram@30000000 {
-			compatible = "shared-dma-pool";
-			reg = <0x30000000 0x40000>;
-			no-map;
-		};
-
-		retram: retram@38000000 {
-			compatible = "shared-dma-pool";
-			reg = <0x38000000 0x10000>;
-			no-map;
-		};
-
-		gpu_reserved: gpu@e8000000 {
-			reg = <0xe8000000 0x8000000>;
-			no-map;
-		};
-	};
-
-	aliases {
-		serial0 = &uart4;
-	};
-
-	sd_switch: regulator-sd_switch {
-		compatible = "regulator-gpio";
-		regulator-name = "sd_switch";
-		regulator-min-microvolt = <1800000>;
-		regulator-max-microvolt = <2900000>;
-		regulator-type = "voltage";
-		regulator-always-on;
-
-		gpios = <&gpiof 14 GPIO_ACTIVE_HIGH>;
-		gpios-states = <0>;
-		states = <1800000 0x1>,
-			 <2900000 0x0>;
-	};
-};
-
-&adc {
-	/* ANA0, ANA1 are dedicated pins and don't need pinctrl: only in6. */
-	pinctrl-0 = <&adc1_in6_pins_a>;
-	pinctrl-names = "default";
-	vdd-supply = <&vdd>;
-	vdda-supply = <&vdda>;
-	vref-supply = <&vdda>;
-	status = "disabled";
-	adc1: adc@0 {
-		st,adc-channels = <0 1 6>;
-		/* 16.5 ck_cycles sampling time */
-		st,min-sample-time-nsecs = <400>;
-		status = "okay";
-	};
-};
-
-&dac {
-	pinctrl-names = "default";
-	pinctrl-0 = <&dac_ch1_pins_a &dac_ch2_pins_a>;
-	vref-supply = <&vdda>;
-	status = "disabled";
-	dac1: dac@1 {
-		status = "okay";
-	};
-	dac2: dac@2 {
-		status = "okay";
-	};
-};
-
-&dts {
-	status = "okay";
-};
-
-&gpu {
-	contiguous-area = <&gpu_reserved>;
-	status = "okay";
-};
-
-&i2c4 {
-	pinctrl-names = "default";
-	pinctrl-0 = <&i2c4_pins_a>;
-	i2c-scl-rising-time-ns = <185>;
-	i2c-scl-falling-time-ns = <20>;
-	status = "okay";
-	/* spare dmas for other usage */
-	/delete-property/dmas;
-	/delete-property/dma-names;
-
-	pmic: stpmic@33 {
-		compatible = "st,stpmic1";
-		reg = <0x33>;
-		interrupts-extended = <&gpioa 0 IRQ_TYPE_EDGE_FALLING>;
-		interrupt-controller;
-		#interrupt-cells = <2>;
-		status = "okay";
-
-		regulators {
-			compatible = "st,stpmic1-regulators";
-			ldo1-supply = <&v3v3>;
-			ldo2-supply = <&v3v3>;
-			ldo3-supply = <&vdd_ddr>;
-			ldo5-supply = <&v3v3>;
-			ldo6-supply = <&v3v3>;
-			pwr_sw1-supply = <&bst_out>;
-			pwr_sw2-supply = <&bst_out>;
-
-			vddcore: buck1 {
-				regulator-name = "vddcore";
-				regulator-min-microvolt = <1200000>;
-				regulator-max-microvolt = <1350000>;
-				regulator-always-on;
-				regulator-initial-mode = <0>;
-				regulator-over-current-protection;
-			};
-
-			vdd_ddr: buck2 {
-				regulator-name = "vdd_ddr";
-				regulator-min-microvolt = <1350000>;
-				regulator-max-microvolt = <1350000>;
-				regulator-always-on;
-				regulator-initial-mode = <0>;
-				regulator-over-current-protection;
-			};
-
-			vdd: buck3 {
-				regulator-name = "vdd";
-				regulator-min-microvolt = <3300000>;
-				regulator-max-microvolt = <3300000>;
-				regulator-always-on;
-				st,mask-reset;
-				regulator-initial-mode = <0>;
-				regulator-over-current-protection;
-			};
-
-			v3v3: buck4 {
-				regulator-name = "v3v3";
-				regulator-min-microvolt = <3300000>;
-				regulator-max-microvolt = <3300000>;
-				regulator-always-on;
-				regulator-over-current-protection;
-				regulator-initial-mode = <0>;
-			};
-
-			vdda: ldo1 {
-				regulator-name = "vdda";
-				regulator-min-microvolt = <2900000>;
-				regulator-max-microvolt = <2900000>;
-				interrupts = <IT_CURLIM_LDO1 0>;
-			};
-
-			v2v8: ldo2 {
-				regulator-name = "v2v8";
-				regulator-min-microvolt = <2800000>;
-				regulator-max-microvolt = <2800000>;
-				interrupts = <IT_CURLIM_LDO2 0>;
-			};
-
-			vtt_ddr: ldo3 {
-				regulator-name = "vtt_ddr";
-				regulator-min-microvolt = <500000>;
-				regulator-max-microvolt = <750000>;
-				regulator-always-on;
-				regulator-over-current-protection;
-			};
-
-			vdd_usb: ldo4 {
-				regulator-name = "vdd_usb";
-				regulator-min-microvolt = <3300000>;
-				regulator-max-microvolt = <3300000>;
-				interrupts = <IT_CURLIM_LDO4 0>;
-			};
-
-			vdd_sd: ldo5 {
-				regulator-name = "vdd_sd";
-				regulator-min-microvolt = <2900000>;
-				regulator-max-microvolt = <2900000>;
-				interrupts = <IT_CURLIM_LDO5 0>;
-				regulator-boot-on;
-			};
-
-			v1v8: ldo6 {
-				regulator-name = "v1v8";
-				regulator-min-microvolt = <1800000>;
-				regulator-max-microvolt = <1800000>;
-				interrupts = <IT_CURLIM_LDO6 0>;
-			};
-
-			vref_ddr: vref_ddr {
-				regulator-name = "vref_ddr";
-				regulator-always-on;
-				regulator-over-current-protection;
-			};
-
-			bst_out: boost {
-				regulator-name = "bst_out";
-				interrupts = <IT_OCP_BOOST 0>;
-			};
-
-			vbus_otg: pwr_sw1 {
-				regulator-name = "vbus_otg";
-				interrupts = <IT_OCP_OTG 0>;
-			 };
-
-			 vbus_sw: pwr_sw2 {
-				regulator-name = "vbus_sw";
-				interrupts = <IT_OCP_SWOUT 0>;
-				regulator-active-discharge = <1>;
-			 };
-		};
-
-		onkey {
-			compatible = "st,stpmic1-onkey";
-			interrupts = <IT_PONKEY_F 0>, <IT_PONKEY_R 0>;
-			interrupt-names = "onkey-falling", "onkey-rising";
-			power-off-time-sec = <10>;
-			status = "okay";
-		};
-
-		watchdog {
-			compatible = "st,stpmic1-wdt";
-			status = "disabled";
-		};
-	};
-};
-
-&ipcc {
-	status = "okay";
-};
-
-&iwdg2 {
-	timeout-sec = <32>;
-	status = "okay";
-};
-
-&m4_rproc {
-	memory-region = <&retram>, <&mcuram>, <&mcuram2>, <&vdev0vring0>,
-			<&vdev0vring1>, <&vdev0buffer>;
-	mboxes = <&ipcc 0>, <&ipcc 1>, <&ipcc 2>;
-	mbox-names = "vq0", "vq1", "shutdown";
-	interrupt-parent = <&exti>;
-	interrupts = <68 1>;
-	status = "okay";
-};
-
-&pwr_regulators {
-	vdd-supply = <&vdd>;
-	vdd_3v3_usbfs-supply = <&vdd_usb>;
-};
-
-&rng1 {
-	status = "okay";
 };
 
-&rtc {
+&cryp1 {
 	status = "okay";
 };
 
-&sdmmc1 {
-	pinctrl-names = "default", "opendrain", "sleep";
-	pinctrl-0 = <&sdmmc1_b4_pins_a &sdmmc1_dir_pins_a>;
-	pinctrl-1 = <&sdmmc1_b4_od_pins_a &sdmmc1_dir_pins_a>;
-	pinctrl-2 = <&sdmmc1_b4_sleep_pins_a &sdmmc1_dir_sleep_pins_a>;
-	broken-cd;
-	st,sig-dir;
-	st,neg-edge;
-	st,use-ckin;
-	bus-width = <4>;
-	vmmc-supply = <&vdd_sd>;
-	vqmmc-supply = <&sd_switch>;
-	status = "okay";
-};
-
-&sdmmc2 {
-	pinctrl-names = "default", "opendrain", "sleep";
-	pinctrl-0 = <&sdmmc2_b4_pins_a &sdmmc2_d47_pins_a>;
-	pinctrl-1 = <&sdmmc2_b4_od_pins_a &sdmmc2_d47_pins_a>;
-	pinctrl-2 = <&sdmmc2_b4_sleep_pins_a &sdmmc2_d47_sleep_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";
-};
-
-&timers6 {
-	status = "okay";
-	/* spare dmas for other usage */
-	/delete-property/dmas;
-	/delete-property/dma-names;
-	timer@5 {
-		status = "okay";
-	};
-};
-
-&uart4 {
-	pinctrl-names = "default";
-	pinctrl-0 = <&uart4_pins_a>;
-	status = "okay";
-};
-
-&usbphyc_port0 {
-	phy-supply = <&vdd_usb>;
-	vdda1v1-supply = <&reg11>;
-	vdda1v8-supply = <&reg18>;
-};
-
-&usbphyc_port1 {
-	phy-supply = <&vdd_usb>;
-	vdda1v1-supply = <&reg11>;
-	vdda1v8-supply = <&reg18>;
-};
-
-&rcc {
-	status = "okay";
-	secure-status = "disable";
-};
-
-&bsec {
-	board_id: board_id@ec {
-		reg = <0xec 0x4>;
-		st,non-secure-otp;
-	};
+&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)
+		DECPROT(STM32MP1_ETZPC_DDRCTRL_ID, DECPROT_NS_R_S_W, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_DDRPHYC_ID, DECPROT_NS_R_S_W, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_STGENC_ID, DECPROT_S_RW, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_BKPSRAM_ID, DECPROT_S_RW, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_IWDG1_ID, DECPROT_S_RW, DECPROT_LOCK)
+	>;
 };
diff --git a/core/arch/arm/dts/stm32mp157c-ev1.dts b/core/arch/arm/dts/stm32mp157c-ev1.dts
index fd8c958b0..d12d30093 100644
--- a/core/arch/arm/dts/stm32mp157c-ev1.dts
+++ b/core/arch/arm/dts/stm32mp157c-ev1.dts
@@ -1,13 +1,12 @@
 // 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.
+ * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
  */
 /dts-v1/;
 
 #include "stm32mp157c-ed1.dts"
-#include <dt-bindings/gpio/gpio.h>
-/* #include <dt-bindings/input/input.h> Remove due to BSD license issue */
+#include "stm32mp15xx-evx.dtsi"
 
 / {
 	model = "STMicroelectronics STM32MP157C eval daughter on eval mother";
@@ -19,346 +18,6 @@
 
 	aliases {
 		serial0 = &uart4;
-		ethernet0 = &ethernet0;
+		serial1 = &usart3;
 	};
-
-	clocks {
-		clk_ext_camera: clk-ext-camera {
-			#clock-cells = <0>;
-			compatible = "fixed-clock";
-			clock-frequency = <24000000>;
-		};
-	};
-
-	joystick {
-		compatible = "gpio-keys";
-		pinctrl-0 = <&joystick_pins>;
-		pinctrl-names = "default";
-		button-0 {
-			label = "JoySel";
-			/* linux,code = <KEY_ENTER>; BSD license issue */
-			interrupt-parent = <&stmfx_pinctrl>;
-			interrupts = <0 IRQ_TYPE_EDGE_RISING>;
-		};
-		button-1 {
-			label = "JoyDown";
-			/* linux,code = <KEY_DOWN>; BSD license issue */
-			interrupt-parent = <&stmfx_pinctrl>;
-			interrupts = <1 IRQ_TYPE_EDGE_RISING>;
-		};
-		button-2 {
-			label = "JoyLeft";
-			/* linux,code = <KEY_LEFT>; BSD license issue */
-			interrupt-parent = <&stmfx_pinctrl>;
-			interrupts = <2 IRQ_TYPE_EDGE_RISING>;
-		};
-		button-3 {
-			label = "JoyRight";
-			/* linux,code = <KEY_RIGHT>; BSD license issue */
-			interrupt-parent = <&stmfx_pinctrl>;
-			interrupts = <3 IRQ_TYPE_EDGE_RISING>;
-		};
-		button-4 {
-			label = "JoyUp";
-			/* linux,code = <KEY_UP>; BSD license issue */
-			interrupt-parent = <&stmfx_pinctrl>;
-			interrupts = <4 IRQ_TYPE_EDGE_RISING>;
-		};
-	};
-
-	panel_backlight: panel-backlight {
-		compatible = "gpio-backlight";
-		gpios = <&gpiod 13 GPIO_ACTIVE_LOW>;
-		default-on;
-		status = "okay";
-	};
-};
-
-&cec {
-	pinctrl-names = "default";
-	pinctrl-0 = <&cec_pins_a>;
-	status = "okay";
-};
-
-&dcmi {
-	status = "okay";
-	pinctrl-names = "default", "sleep";
-	pinctrl-0 = <&dcmi_pins_a>;
-	pinctrl-1 = <&dcmi_sleep_pins_a>;
-
-	port {
-		dcmi_0: endpoint {
-			remote-endpoint = <&ov5640_0>;
-			bus-width = <8>;
-			hsync-active = <0>;
-			vsync-active = <0>;
-			pclk-sample = <1>;
-		};
-	};
-};
-
-&dsi {
-	#address-cells = <1>;
-	#size-cells = <0>;
-	phy-dsi-supply = <&reg18>;
-	status = "okay";
-
-	ports {
-		#address-cells = <1>;
-		#size-cells = <0>;
-
-		port@0 {
-			reg = <0>;
-			dsi_in: endpoint {
-				remote-endpoint = <&ltdc_ep0_out>;
-			};
-		};
-
-		port@1 {
-			reg = <1>;
-			dsi_out: endpoint {
-				remote-endpoint = <&dsi_panel_in>;
-			};
-		};
-	};
-
-	panel-dsi@0 {
-		compatible = "raydium,rm68200";
-		reg = <0>;
-		reset-gpios = <&gpiof 15 GPIO_ACTIVE_LOW>;
-		backlight = <&panel_backlight>;
-		power-supply = <&v3v3>;
-		status = "okay";
-
-		port {
-			dsi_panel_in: endpoint {
-				remote-endpoint = <&dsi_out>;
-			};
-		};
-	};
-};
-
-&ethernet0 {
-	status = "okay";
-	pinctrl-0 = <&ethernet0_rgmii_pins_a>;
-	pinctrl-1 = <&ethernet0_rgmii_pins_sleep_a>;
-	pinctrl-names = "default", "sleep";
-	phy-mode = "rgmii-id";
-	max-speed = <1000>;
-	phy-handle = <&phy0>;
-
-	mdio0 {
-		#address-cells = <1>;
-		#size-cells = <0>;
-		compatible = "snps,dwmac-mdio";
-		phy0: ethernet-phy@0 {
-			reg = <0>;
-		};
-	};
-};
-
-&fmc {
-	pinctrl-names = "default", "sleep";
-	pinctrl-0 = <&fmc_pins_a>;
-	pinctrl-1 = <&fmc_sleep_pins_a>;
-	status = "okay";
-	#address-cells = <1>;
-	#size-cells = <0>;
-
-	nand@0 {
-		reg = <0>;
-		nand-on-flash-bbt;
-		#address-cells = <1>;
-		#size-cells = <1>;
-	};
-};
-
-&i2c2 {
-	pinctrl-names = "default";
-	pinctrl-0 = <&i2c2_pins_a>;
-	i2c-scl-rising-time-ns = <185>;
-	i2c-scl-falling-time-ns = <20>;
-	status = "okay";
-
-	ov5640: camera@3c {
-		compatible = "ovti,ov5640";
-		reg = <0x3c>;
-		clocks = <&clk_ext_camera>;
-		clock-names = "xclk";
-		DOVDD-supply = <&v2v8>;
-		powerdown-gpios = <&stmfx_pinctrl 18 (GPIO_ACTIVE_HIGH | GPIO_PUSH_PULL)>;
-		reset-gpios = <&stmfx_pinctrl 19 (GPIO_ACTIVE_LOW | GPIO_PUSH_PULL)>;
-		rotation = <180>;
-		status = "okay";
-
-		port {
-			ov5640_0: endpoint {
-				remote-endpoint = <&dcmi_0>;
-				bus-width = <8>;
-				data-shift = <2>; /* lines 9:2 are used */
-				hsync-active = <0>;
-				vsync-active = <0>;
-				pclk-sample = <1>;
-			};
-		};
-	};
-
-	stmfx: stmfx@42 {
-		compatible = "st,stmfx-0300";
-		reg = <0x42>;
-		interrupts = <8 IRQ_TYPE_EDGE_RISING>;
-		interrupt-parent = <&gpioi>;
-		vdd-supply = <&v3v3>;
-
-		stmfx_pinctrl: stmfx-pin-controller {
-			compatible = "st,stmfx-0300-pinctrl";
-			gpio-controller;
-			#gpio-cells = <2>;
-			interrupt-controller;
-			#interrupt-cells = <2>;
-			gpio-ranges = <&stmfx_pinctrl 0 0 24>;
-
-			joystick_pins: joystick {
-				pins = "gpio0", "gpio1", "gpio2", "gpio3", "gpio4";
-				bias-pull-down;
-			};
-		};
-	};
-};
-
-&i2c5 {
-	pinctrl-names = "default";
-	pinctrl-0 = <&i2c5_pins_a>;
-	i2c-scl-rising-time-ns = <185>;
-	i2c-scl-falling-time-ns = <20>;
-	status = "okay";
-};
-
-&ltdc {
-	status = "okay";
-
-	port {
-		#address-cells = <1>;
-		#size-cells = <0>;
-
-		ltdc_ep0_out: endpoint@0 {
-			reg = <0>;
-			remote-endpoint = <&dsi_in>;
-		};
-	};
-};
-
-&m_can1 {
-	pinctrl-names = "default", "sleep";
-	pinctrl-0 = <&m_can1_pins_a>;
-	pinctrl-1 = <&m_can1_sleep_pins_a>;
-	status = "okay";
-};
-
-&qspi {
-	pinctrl-names = "default", "sleep";
-	pinctrl-0 = <&qspi_clk_pins_a &qspi_bk1_pins_a &qspi_bk2_pins_a>;
-	pinctrl-1 = <&qspi_clk_sleep_pins_a &qspi_bk1_sleep_pins_a &qspi_bk2_sleep_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>;
-	};
-};
-
-&sdmmc3 {
-	pinctrl-names = "default", "opendrain", "sleep";
-	pinctrl-0 = <&sdmmc3_b4_pins_a>;
-	pinctrl-1 = <&sdmmc3_b4_od_pins_a>;
-	pinctrl-2 = <&sdmmc3_b4_sleep_pins_a>;
-	broken-cd;
-	st,neg-edge;
-	bus-width = <4>;
-	vmmc-supply = <&v3v3>;
-	status = "disabled";
-};
-
-&spi1 {
-	pinctrl-names = "default";
-	pinctrl-0 = <&spi1_pins_a>;
-	status = "disabled";
-};
-
-&timers2 {
-	/* spare dmas for other usage (un-delete to enable pwm capture) */
-	/delete-property/dmas;
-	/delete-property/dma-names;
-	status = "disabled";
-	pwm {
-		pinctrl-0 = <&pwm2_pins_a>;
-		pinctrl-1 = <&pwm2_sleep_pins_a>;
-		pinctrl-names = "default", "sleep";
-		status = "okay";
-	};
-	timer@1 {
-		status = "okay";
-	};
-};
-
-&timers8 {
-	/delete-property/dmas;
-	/delete-property/dma-names;
-	status = "disabled";
-	pwm {
-		pinctrl-0 = <&pwm8_pins_a>;
-		pinctrl-1 = <&pwm8_sleep_pins_a>;
-		pinctrl-names = "default", "sleep";
-		status = "okay";
-	};
-	timer@7 {
-		status = "okay";
-	};
-};
-
-&timers12 {
-	/delete-property/dmas;
-	/delete-property/dma-names;
-	status = "disabled";
-	pwm {
-		pinctrl-0 = <&pwm12_pins_a>;
-		pinctrl-1 = <&pwm12_sleep_pins_a>;
-		pinctrl-names = "default", "sleep";
-		status = "okay";
-	};
-	timer@11 {
-		status = "okay";
-	};
-};
-
-&usbh_ehci {
-	phys = <&usbphyc_port0>;
-	status = "okay";
-};
-
-&usbotg_hs {
-	dr_mode = "peripheral";
-	phys = <&usbphyc_port1 0>;
-	phy-names = "usb2-phy";
-	status = "okay";
-};
-
-&usbphyc {
-	status = "okay";
 };
diff --git a/core/arch/arm/dts/stm32mp157d-dk1.dts b/core/arch/arm/dts/stm32mp157d-dk1.dts
new file mode 100644
index 000000000..79297b831
--- /dev/null
+++ b/core/arch/arm/dts/stm32mp157d-dk1.dts
@@ -0,0 +1,45 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
+ */
+
+/dts-v1/;
+
+#include "stm32mp157.dtsi"
+#include "stm32mp15xd.dtsi"
+#include "stm32mp15-pinctrl.dtsi"
+#include "stm32mp15xxac-pinctrl.dtsi"
+#include "stm32mp15xx-dkx.dtsi"
+#include <dt-bindings/soc/st,stm32-etzpc.h>
+
+/ {
+	model = "STMicroelectronics STM32MP157D-DK1 Discovery Board";
+	compatible = "st,stm32mp157d-dk1", "st,stm32mp157";
+
+	aliases {
+		serial0 = &uart4;
+		serial1 = &usart3;
+		serial2 = &uart7;
+	};
+
+	chosen {
+		stdout-path = "serial0:115200n8";
+	};
+};
+
+&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_DDRCTRL_ID, DECPROT_NS_R_S_W, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_DDRPHYC_ID, DECPROT_NS_R_S_W, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_STGENC_ID, DECPROT_S_RW, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_BKPSRAM_ID, DECPROT_S_RW, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_IWDG1_ID, DECPROT_S_RW, DECPROT_LOCK)
+	>;
+};
diff --git a/core/arch/arm/dts/stm32mp157d-ed1.dts b/core/arch/arm/dts/stm32mp157d-ed1.dts
new file mode 100644
index 000000000..2c67ec0ac
--- /dev/null
+++ b/core/arch/arm/dts/stm32mp157d-ed1.dts
@@ -0,0 +1,39 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
+ */
+/dts-v1/;
+
+#include "stm32mp157.dtsi"
+#include "stm32mp15xd.dtsi"
+#include "stm32mp15-pinctrl.dtsi"
+#include "stm32mp15xxaa-pinctrl.dtsi"
+#include "stm32mp15xx-edx.dtsi"
+#include <dt-bindings/soc/st,stm32-etzpc.h>
+
+/ {
+	model = "STMicroelectronics STM32MP157D eval daughter";
+	compatible = "st,stm32mp157d-ed1", "st,stm32mp157";
+
+	chosen {
+		stdout-path = "serial0:115200n8";
+	};
+};
+
+
+&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_DDRCTRL_ID, DECPROT_NS_R_S_W, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_DDRPHYC_ID, DECPROT_NS_R_S_W, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_STGENC_ID, DECPROT_S_RW, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_BKPSRAM_ID, DECPROT_S_RW, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_IWDG1_ID, DECPROT_S_RW, DECPROT_LOCK)
+	>;
+};
diff --git a/core/arch/arm/dts/stm32mp157d-ev1.dts b/core/arch/arm/dts/stm32mp157d-ev1.dts
new file mode 100644
index 000000000..4a40f5fe5
--- /dev/null
+++ b/core/arch/arm/dts/stm32mp157d-ev1.dts
@@ -0,0 +1,23 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
+ */
+/dts-v1/;
+
+#include "stm32mp157d-ed1.dts"
+#include "stm32mp15xx-evx.dtsi"
+
+/ {
+	model = "STMicroelectronics STM32MP157D eval daughter on eval mother";
+	compatible = "st,stm32mp157d-ev1", "st,stm32mp157d-ed1", "st,stm32mp157";
+
+	chosen {
+		stdout-path = "serial0:115200n8";
+	};
+
+	aliases {
+		serial0 = &uart4;
+		serial1 = &usart3;
+	};
+};
diff --git a/core/arch/arm/dts/stm32mp157f-dk2.dts b/core/arch/arm/dts/stm32mp157f-dk2.dts
new file mode 100644
index 000000000..680ca0f6e
--- /dev/null
+++ b/core/arch/arm/dts/stm32mp157f-dk2.dts
@@ -0,0 +1,51 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
+ */
+
+/dts-v1/;
+
+#include "stm32mp157.dtsi"
+#include "stm32mp15xf.dtsi"
+#include "stm32mp15-pinctrl.dtsi"
+#include "stm32mp15xxac-pinctrl.dtsi"
+#include "stm32mp15xx-dkx.dtsi"
+#include <dt-bindings/soc/st,stm32-etzpc.h>
+
+/ {
+	model = "STMicroelectronics STM32MP157F-DK2 Discovery Board";
+	compatible = "st,stm32mp157f-dk2", "st,stm32mp157";
+
+	aliases {
+		serial0 = &uart4;
+		serial1 = &usart3;
+		serial2 = &uart7;
+		serial3 = &usart2;
+	};
+
+	chosen {
+		stdout-path = "serial0:115200n8";
+	};
+};
+
+&cryp1 {
+	status = "okay";
+};
+
+&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)
+		DECPROT(STM32MP1_ETZPC_DDRCTRL_ID, DECPROT_NS_R_S_W, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_DDRPHYC_ID, DECPROT_NS_R_S_W, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_STGENC_ID, DECPROT_S_RW, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_BKPSRAM_ID, DECPROT_S_RW, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_IWDG1_ID, DECPROT_S_RW, DECPROT_LOCK)
+	>;
+};
diff --git a/core/arch/arm/dts/stm32mp157f-ed1.dts b/core/arch/arm/dts/stm32mp157f-ed1.dts
new file mode 100644
index 000000000..1aa26cdbb
--- /dev/null
+++ b/core/arch/arm/dts/stm32mp157f-ed1.dts
@@ -0,0 +1,43 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
+ */
+/dts-v1/;
+
+#include "stm32mp157.dtsi"
+#include "stm32mp15xf.dtsi"
+#include "stm32mp15-pinctrl.dtsi"
+#include "stm32mp15xxaa-pinctrl.dtsi"
+#include "stm32mp15xx-edx.dtsi"
+#include <dt-bindings/soc/st,stm32-etzpc.h>
+
+/ {
+	model = "STMicroelectronics STM32MP157F eval daughter";
+	compatible = "st,stm32mp157f-ed1", "st,stm32mp157";
+
+	chosen {
+		stdout-path = "serial0:115200n8";
+	};
+};
+
+&cryp1 {
+	status = "okay";
+};
+
+&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)
+		DECPROT(STM32MP1_ETZPC_DDRCTRL_ID, DECPROT_NS_R_S_W, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_DDRPHYC_ID, DECPROT_NS_R_S_W, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_STGENC_ID, DECPROT_S_RW, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_BKPSRAM_ID, DECPROT_S_RW, DECPROT_LOCK)
+		DECPROT(STM32MP1_ETZPC_IWDG1_ID, DECPROT_S_RW, DECPROT_LOCK)
+	>;
+};
diff --git a/core/arch/arm/dts/stm32mp157f-ev1.dts b/core/arch/arm/dts/stm32mp157f-ev1.dts
new file mode 100644
index 000000000..caf5dfe11
--- /dev/null
+++ b/core/arch/arm/dts/stm32mp157f-ev1.dts
@@ -0,0 +1,23 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
+ */
+/dts-v1/;
+
+#include "stm32mp157f-ed1.dts"
+#include "stm32mp15xx-evx.dtsi"
+
+/ {
+	model = "STMicroelectronics STM32MP157F eval daughter on eval mother";
+	compatible = "st,stm32mp157f-ev1", "st,stm32mp157f-ed1", "st,stm32mp157";
+
+	chosen {
+		stdout-path = "serial0:115200n8";
+	};
+
+	aliases {
+		serial0 = &uart4;
+		serial1 = &usart3;
+	};
+};
diff --git a/core/arch/arm/dts/stm32mp15xa.dtsi b/core/arch/arm/dts/stm32mp15xa.dtsi
new file mode 100644
index 000000000..5ed7e594f
--- /dev/null
+++ b/core/arch/arm/dts/stm32mp15xa.dtsi
@@ -0,0 +1,13 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
+ */
+
+&cpu0_opp_table {
+		opp-650000000 {
+			opp-hz = /bits/ 64 <650000000>;
+			opp-microvolt = <1200000>;
+			opp-supported-hw = <0x1>;
+		};
+};
diff --git a/core/arch/arm/dts/stm32mp15xc.dtsi b/core/arch/arm/dts/stm32mp15xc.dtsi
index b06a55a2f..68d822d8c 100644
--- a/core/arch/arm/dts/stm32mp15xc.dtsi
+++ b/core/arch/arm/dts/stm32mp15xc.dtsi
@@ -4,6 +4,8 @@
  * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
  */
 
+#include "stm32mp15xa.dtsi"
+
 / {
 	soc {
 		cryp1: cryp@54001000 {
@@ -13,6 +15,7 @@
 			clocks = <&rcc CRYP1>;
 			resets = <&rcc CRYP1_R>;
 			status = "disabled";
+			secure-status = "disabled";
 		};
 	};
 };
diff --git a/core/arch/arm/dts/stm32mp15xd.dtsi b/core/arch/arm/dts/stm32mp15xd.dtsi
new file mode 100644
index 000000000..18b05ee38
--- /dev/null
+++ b/core/arch/arm/dts/stm32mp15xd.dtsi
@@ -0,0 +1,19 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
+ */
+
+&cpu0_opp_table {
+		opp-800000000 {
+			opp-hz = /bits/ 64 <800000000>;
+			opp-microvolt = <1350000>;
+			opp-supported-hw = <0x2>;
+		};
+		opp-400000000 {
+			opp-hz = /bits/ 64 <400000000>;
+			opp-microvolt = <1200000>;
+			opp-supported-hw = <0x2>;
+			opp-suspend;
+		};
+};
diff --git a/core/arch/arm/dts/stm32mp15xf.dtsi b/core/arch/arm/dts/stm32mp15xf.dtsi
new file mode 100644
index 000000000..526a1627c
--- /dev/null
+++ b/core/arch/arm/dts/stm32mp15xf.dtsi
@@ -0,0 +1,21 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
+ */
+
+#include "stm32mp15xd.dtsi"
+
+/ {
+	soc {
+		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";
+			secure-status = "disabled";
+		};
+	};
+};
diff --git a/core/arch/arm/dts/stm32mp15xx-dkx.dtsi b/core/arch/arm/dts/stm32mp15xx-dkx.dtsi
index f6672e87a..236cff932 100644
--- a/core/arch/arm/dts/stm32mp15xx-dkx.dtsi
+++ b/core/arch/arm/dts/stm32mp15xx-dkx.dtsi
@@ -4,8 +4,8 @@
  * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
  */
 
-#include <dt-bindings/gpio/gpio.h>
-#include <dt-bindings/mfd/st,stpmic1.h>
+#include <dt-bindings/clock/stm32mp1-clksrc.h>
+#include <dt-bindings/power/stm32mp1-power.h>
 
 / {
 	memory@c0000000 {
@@ -13,206 +13,36 @@
 		reg = <0xc0000000 0x20000000>;
 	};
 
-	reserved-memory {
-		#address-cells = <1>;
-		#size-cells = <1>;
-		ranges;
-
-		mcuram2: mcuram2@10000000 {
-			compatible = "shared-dma-pool";
-			reg = <0x10000000 0x40000>;
-			no-map;
-		};
-
-		vdev0vring0: vdev0vring0@10040000 {
-			compatible = "shared-dma-pool";
-			reg = <0x10040000 0x1000>;
-			no-map;
-		};
-
-		vdev0vring1: vdev0vring1@10041000 {
-			compatible = "shared-dma-pool";
-			reg = <0x10041000 0x1000>;
-			no-map;
-		};
-
-		vdev0buffer: vdev0buffer@10042000 {
-			compatible = "shared-dma-pool";
-			reg = <0x10042000 0x4000>;
-			no-map;
-		};
-
-		mcuram: mcuram@30000000 {
-			compatible = "shared-dma-pool";
-			reg = <0x30000000 0x40000>;
-			no-map;
-		};
-
-		retram: retram@38000000 {
-			compatible = "shared-dma-pool";
-			reg = <0x38000000 0x10000>;
-			no-map;
-		};
-
-		gpu_reserved: gpu@d4000000 {
-			reg = <0xd4000000 0x4000000>;
-			no-map;
-		};
-	};
-
-	led {
-		compatible = "gpio-leds";
-		blue {
-			label = "heartbeat";
-			gpios = <&gpiod 11 GPIO_ACTIVE_HIGH>;
-			linux,default-trigger = "heartbeat";
-			default-state = "off";
-		};
-	};
-
-	sound {
-		compatible = "audio-graph-card";
-		label = "STM32MP1-DK";
-		routing =
-			"Playback" , "MCLK",
-			"Capture" , "MCLK",
-			"MICL" , "Mic Bias";
-		dais = <&sai2a_port &sai2b_port &i2s2_port>;
-		status = "okay";
+	vin: vin {
+		compatible = "regulator-fixed";
+		regulator-name = "vin";
+		regulator-min-microvolt = <5000000>;
+		regulator-max-microvolt = <5000000>;
+		regulator-always-on;
 	};
 };
 
-&adc {
-	pinctrl-names = "default";
-	pinctrl-0 = <&adc12_ain_pins_a>, <&adc12_usb_cc_pins_a>;
-	vdd-supply = <&vdd>;
-	vdda-supply = <&vdd>;
-	vref-supply = <&vrefbuf>;
-	status = "disabled";
-	adc1: adc@0 {
-		/*
-		 * Type-C USB_PWR_CC1 & USB_PWR_CC2 on in18 & in19.
-		 * Use at least 5 * RC time, e.g. 5 * (Rp + Rd) * C:
-		 * 5 * (56 + 47kOhms) * 5pF => 2.5us.
-		 * Use arbitrary margin here (e.g. 5us).
-		 */
-		st,min-sample-time-nsecs = <5000>;
-		/* AIN connector, USB Type-C CC1 & CC2 */
-		st,adc-channels = <0 1 6 13 18 19>;
-		status = "okay";
-	};
-	adc2: adc@100 {
-		/* AIN connector, USB Type-C CC1 & CC2 */
-		st,adc-channels = <0 1 2 6 18 19>;
-		st,min-sample-time-nsecs = <5000>;
-		status = "okay";
+&bsec {
+	board_id: board_id@ec {
+		reg = <0xec 0x4>;
+		st,non-secure-otp;
 	};
 };
 
-&cec {
-	pinctrl-names = "default", "sleep";
-	pinctrl-0 = <&cec_pins_b>;
-	pinctrl-1 = <&cec_pins_sleep_b>;
-	status = "okay";
+&clk_hse {
+	st,digbypass;
 };
 
-&ethernet0 {
-	status = "okay";
-	pinctrl-0 = <&ethernet0_rgmii_pins_a>;
-	pinctrl-1 = <&ethernet0_rgmii_pins_sleep_a>;
-	pinctrl-names = "default", "sleep";
-	phy-mode = "rgmii-id";
-	max-speed = <1000>;
-	phy-handle = <&phy0>;
-
-	mdio0 {
-		#address-cells = <1>;
-		#size-cells = <0>;
-		compatible = "snps,dwmac-mdio";
-		phy0: ethernet-phy@0 {
-			reg = <0>;
-		};
-	};
+&cpu0{
+	cpu-supply = <&vddcore>;
 };
 
-&gpu {
-	contiguous-area = <&gpu_reserved>;
-	status = "okay";
+&cpu1{
+	cpu-supply = <&vddcore>;
 };
 
-&i2c1 {
-	pinctrl-names = "default", "sleep";
-	pinctrl-0 = <&i2c1_pins_a>;
-	pinctrl-1 = <&i2c1_pins_sleep_a>;
-	i2c-scl-rising-time-ns = <100>;
-	i2c-scl-falling-time-ns = <7>;
+&hash1 {
 	status = "okay";
-	/delete-property/dmas;
-	/delete-property/dma-names;
-
-	hdmi-transmitter@39 {
-		compatible = "sil,sii9022";
-		reg = <0x39>;
-		iovcc-supply = <&v3v3_hdmi>;
-		cvcc12-supply = <&v1v2_hdmi>;
-		reset-gpios = <&gpioa 10 GPIO_ACTIVE_LOW>;
-		interrupts = <1 IRQ_TYPE_EDGE_FALLING>;
-		interrupt-parent = <&gpiog>;
-		#sound-dai-cells = <0>;
-		status = "okay";
-
-		ports {
-			#address-cells = <1>;
-			#size-cells = <0>;
-
-			port@0 {
-				reg = <0>;
-				sii9022_in: endpoint {
-					remote-endpoint = <&ltdc_ep0_out>;
-				};
-			};
-
-			port@3 {
-				reg = <3>;
-				sii9022_tx_endpoint: endpoint {
-					remote-endpoint = <&i2s2_endpoint>;
-				};
-			};
-		};
-	};
-
-	cs42l51: cs42l51@4a {
-		compatible = "cirrus,cs42l51";
-		reg = <0x4a>;
-		#sound-dai-cells = <0>;
-		VL-supply = <&v3v3>;
-		VD-supply = <&v1v8_audio>;
-		VA-supply = <&v1v8_audio>;
-		VAHP-supply = <&v1v8_audio>;
-		reset-gpios = <&gpiog 9 GPIO_ACTIVE_LOW>;
-		clocks = <&sai2a>;
-		clock-names = "MCLK";
-		status = "okay";
-
-		cs42l51_port: port {
-			#address-cells = <1>;
-			#size-cells = <0>;
-
-			cs42l51_tx_endpoint: endpoint@0 {
-				reg = <0>;
-				remote-endpoint = <&sai2a_endpoint>;
-				frame-master;
-				bitclock-master;
-			};
-
-			cs42l51_rx_endpoint: endpoint@1 {
-				reg = <1>;
-				remote-endpoint = <&sai2b_endpoint>;
-				frame-master;
-				bitclock-master;
-			};
-		};
-	};
 };
 
 &i2c4 {
@@ -220,24 +50,33 @@
 	pinctrl-0 = <&i2c4_pins_a>;
 	i2c-scl-rising-time-ns = <185>;
 	i2c-scl-falling-time-ns = <20>;
+	clock-frequency = <400000>;
 	status = "okay";
-	/* spare dmas for other usage */
-	/delete-property/dmas;
-	/delete-property/dma-names;
+	secure-status = "okay";
 
 	pmic: stpmic@33 {
 		compatible = "st,stpmic1";
 		reg = <0x33>;
-		interrupts-extended = <&gpioa 0 IRQ_TYPE_EDGE_FALLING>;
+		interrupts-extended = <&exti_pwr 55 IRQ_TYPE_EDGE_FALLING>;
 		interrupt-controller;
 		#interrupt-cells = <2>;
 		status = "okay";
+		secure-status = "okay";
 
 		regulators {
 			compatible = "st,stpmic1-regulators";
+			buck1-supply = <&vin>;
+			buck2-supply = <&vin>;
+			buck3-supply = <&vin>;
+			buck4-supply = <&vin>;
 			ldo1-supply = <&v3v3>;
+			ldo2-supply = <&vin>;
 			ldo3-supply = <&vdd_ddr>;
+			ldo4-supply = <&vin>;
+			ldo5-supply = <&vin>;
 			ldo6-supply = <&v3v3>;
+			vref_ddr-supply = <&vin>;
+			boost-supply = <&vin>;
 			pwr_sw1-supply = <&bst_out>;
 			pwr_sw2-supply = <&bst_out>;
 
@@ -283,7 +122,6 @@
 				regulator-min-microvolt = <1800000>;
 				regulator-max-microvolt = <1800000>;
 				regulator-always-on;
-				interrupts = <IT_CURLIM_LDO1 0>;
 			};
 
 			v3v3_hdmi: ldo2 {
@@ -291,7 +129,6 @@
 				regulator-min-microvolt = <3300000>;
 				regulator-max-microvolt = <3300000>;
 				regulator-always-on;
-				interrupts = <IT_CURLIM_LDO2 0>;
 			};
 
 			vtt_ddr: ldo3 {
@@ -304,16 +141,12 @@
 
 			vdd_usb: ldo4 {
 				regulator-name = "vdd_usb";
-				regulator-min-microvolt = <3300000>;
-				regulator-max-microvolt = <3300000>;
-				interrupts = <IT_CURLIM_LDO4 0>;
 			};
 
 			vdda: ldo5 {
 				regulator-name = "vdda";
 				regulator-min-microvolt = <2900000>;
 				regulator-max-microvolt = <2900000>;
-				interrupts = <IT_CURLIM_LDO5 0>;
 				regulator-boot-on;
 			};
 
@@ -322,7 +155,6 @@
 				regulator-min-microvolt = <1200000>;
 				regulator-max-microvolt = <1200000>;
 				regulator-always-on;
-				interrupts = <IT_CURLIM_LDO6 0>;
 			};
 
 			vref_ddr: vref_ddr {
@@ -331,295 +163,339 @@
 				regulator-over-current-protection;
 			};
 
-			 bst_out: boost {
+			bst_out: boost {
 				regulator-name = "bst_out";
-				interrupts = <IT_OCP_BOOST 0>;
-			 };
+			};
 
 			vbus_otg: pwr_sw1 {
 				regulator-name = "vbus_otg";
-				interrupts = <IT_OCP_OTG 0>;
-			 };
+			};
 
-			 vbus_sw: pwr_sw2 {
+			vbus_sw: pwr_sw2 {
 				regulator-name = "vbus_sw";
-				interrupts = <IT_OCP_SWOUT 0>;
 				regulator-active-discharge = <1>;
-			 };
-		};
-
-		onkey {
-			compatible = "st,stpmic1-onkey";
-			interrupts = <IT_PONKEY_F 0>, <IT_PONKEY_R 0>;
-			interrupt-names = "onkey-falling", "onkey-rising";
-			power-off-time-sec = <10>;
-			status = "okay";
-		};
-
-		watchdog {
-			compatible = "st,stpmic1-wdt";
-			status = "disabled";
+			};
 		};
 	};
 };
 
-&i2s2 {
-	clocks = <&rcc SPI2>, <&rcc SPI2_K>, <&rcc PLL3_Q>, <&rcc PLL3_R>;
-	clock-names = "pclk", "i2sclk", "x8k", "x11k";
-	pinctrl-names = "default", "sleep";
-	pinctrl-0 = <&i2s2_pins_a>;
-	pinctrl-1 = <&i2s2_pins_sleep_a>;
+&iwdg2 {
+	timeout-sec = <32>;
+	secure-timeout-sec = <5>;
 	status = "okay";
-
-	i2s2_port: port {
-		i2s2_endpoint: endpoint {
-			remote-endpoint = <&sii9022_tx_endpoint>;
-			format = "i2s";
-			mclk-fs = <256>;
-		};
-	};
+	secure-status = "okay";
 };
 
-&ipcc {
-	status = "okay";
+&nvmem_layout {
+	nvmem-cells = <&cfg0_otp>,
+		      <&part_number_otp>,
+		      <&monotonic_otp>,
+		      <&nand_otp>,
+		      <&uid_otp>,
+		      <&package_otp>,
+		      <&hw2_otp>,
+		      <&pkh_otp>,
+		      <&board_id>;
+
+	nvmem-cell-names = "cfg0_otp",
+			   "part_number_otp",
+			   "monotonic_otp",
+			   "nand_otp",
+			   "uid_otp",
+			   "package_otp",
+			   "hw2_otp",
+			   "pkh_otp",
+			   "board_id";
 };
 
-&iwdg2 {
-	timeout-sec = <32>;
-	status = "okay";
+&pwr_regulators {
+	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>;
+	vdd-supply = <&vdd>;
+	vdd_3v3_usbfs-supply = <&vdd_usb>;
 };
 
-&ltdc {
-	pinctrl-names = "default", "sleep";
-	pinctrl-0 = <&ltdc_pins_a>;
-	pinctrl-1 = <&ltdc_pins_sleep_a>;
-	status = "okay";
+&rcc {
+	st,hsi-cal;
+	st,csi-cal;
+	st,cal-sec = <60>;
+	st,clksrc = <
+		CLK_MPU_PLL1P
+		CLK_AXI_PLL2P
+		CLK_MCU_PLL3P
+		CLK_PLL12_HSE
+		CLK_PLL3_HSE
+		CLK_PLL4_HSE
+		CLK_RTC_LSE
+		CLK_MCO1_DISABLED
+		CLK_MCO2_DISABLED
+	>;
+
+	st,clkdiv = <
+		1 /*MPU*/
+		0 /*AXI*/
+		0 /*MCU*/
+		1 /*APB1*/
+		1 /*APB2*/
+		1 /*APB3*/
+		1 /*APB4*/
+		2 /*APB5*/
+		23 /*RTC*/
+		0 /*MCO1*/
+		0 /*MCO2*/
+	>;
+
+	st,pkcs = <
+		CLK_CKPER_HSE
+		CLK_FMC_ACLK
+		CLK_QSPI_ACLK
+		CLK_ETH_DISABLED
+		CLK_SDMMC12_PLL4P
+		CLK_DSI_DSIPLL
+		CLK_STGEN_HSE
+		CLK_USBPHY_HSE
+		CLK_SPI2S1_PLL3Q
+		CLK_SPI2S23_PLL3Q
+		CLK_SPI45_HSI
+		CLK_SPI6_HSI
+		CLK_I2C46_HSI
+		CLK_SDMMC3_PLL4P
+		CLK_USBO_USBPHY
+		CLK_ADC_CKPER
+		CLK_CEC_LSE
+		CLK_I2C12_HSI
+		CLK_I2C35_HSI
+		CLK_UART1_HSI
+		CLK_UART24_HSI
+		CLK_UART35_HSI
+		CLK_UART6_HSI
+		CLK_UART78_HSI
+		CLK_SPDIF_PLL4P
+		CLK_FDCAN_PLL4R
+		CLK_SAI1_PLL3Q
+		CLK_SAI2_PLL3Q
+		CLK_SAI3_PLL3Q
+		CLK_SAI4_PLL3Q
+		CLK_RNG1_LSI
+		CLK_RNG2_LSI
+		CLK_LPTIM1_PCLK1
+		CLK_LPTIM23_PCLK3
+		CLK_LPTIM45_LSE
+	>;
+
+	/* VCO = 1066.0 MHz => P = 266 (AXI), Q = 533 (GPU), R = 533 (DDR) */
+	pll2: st,pll@1 {
+		compatible = "st,stm32mp1-pll";
+		reg = <1>;
+		cfg = <2 65 1 0 0 PQR(1,1,1)>;
+		frac = <0x1400>;
+	};
 
-	port {
-		#address-cells = <1>;
-		#size-cells = <0>;
+	/* VCO = 417.8 MHz => P = 209, Q = 24, R = 11 */
+	pll3: st,pll@2 {
+		compatible = "st,stm32mp1-pll";
+		reg = <2>;
+		cfg = <1 33 1 16 36 PQR(1,1,1)>;
+		frac = <0x1a04>;
+	};
 
-		ltdc_ep0_out: endpoint@0 {
-			reg = <0>;
-			remote-endpoint = <&sii9022_in>;
-		};
+	/* VCO = 594.0 MHz => P = 99, Q = 74, R = 74 */
+	pll4: st,pll@3 {
+		compatible = "st,stm32mp1-pll";
+		reg = <3>;
+		cfg = <3 98 5 7 7 PQR(1,1,1)>;
 	};
 };
 
-&m4_rproc {
-	memory-region = <&retram>, <&mcuram>, <&mcuram2>, <&vdev0vring0>,
-			<&vdev0vring1>, <&vdev0buffer>;
-	mboxes = <&ipcc 0>, <&ipcc 1>, <&ipcc 2>;
-	mbox-names = "vq0", "vq1", "shutdown";
-	interrupt-parent = <&exti>;
-	interrupts = <68 1>;
+&rng1 {
 	status = "okay";
+	secure-status = "okay";
 };
 
-&pwr_regulators {
-	vdd-supply = <&vdd>;
-	vdd_3v3_usbfs-supply = <&vdd_usb>;
+&rtc {
+	status = "okay";
+	secure-status = "okay";
 };
 
-&rng1 {
+&sdmmc1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&sdmmc1_b4_pins_a>;
+	disable-wp;
+	st,neg-edge;
+	bus-width = <4>;
+	vmmc-supply = <&v3v3>;
 	status = "okay";
 };
 
-&rtc {
-	status = "okay";
+&timers15 {
+	secure-status = "okay";
+	st,hsi-cal-input = <7>;
+	st,csi-cal-input = <8>;
 };
 
-&sai2 {
-	clocks = <&rcc SAI2>, <&rcc PLL3_Q>, <&rcc PLL3_R>;
-	clock-names = "pclk", "x8k", "x11k";
-	pinctrl-names = "default", "sleep";
-	pinctrl-0 = <&sai2a_pins_a>, <&sai2b_pins_b>;
-	pinctrl-1 = <&sai2a_sleep_pins_a>, <&sai2b_sleep_pins_b>;
+&uart4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&uart4_pins_a>;
 	status = "okay";
+};
 
-	sai2a: audio-controller@4400b004 {
-		#clock-cells = <0>;
-		dma-names = "tx";
-		clocks = <&rcc SAI2_K>;
-		clock-names = "sai_ck";
-		status = "okay";
-
-		sai2a_port: port {
-			sai2a_endpoint: endpoint {
-				remote-endpoint = <&cs42l51_tx_endpoint>;
-				format = "i2s";
-				mclk-fs = <256>;
-				dai-tdm-slot-num = <2>;
-				dai-tdm-slot-width = <32>;
-			};
-		};
-	};
+&uart7 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&uart7_pins_c>;
+	status = "disabled";
+};
 
-	sai2b: audio-controller@4400b024 {
-		dma-names = "rx";
-		st,sync = <&sai2a 2>;
-		clocks = <&rcc SAI2_K>, <&sai2a>;
-		clock-names = "sai_ck", "MCLK";
-		status = "okay";
+&usart3 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&usart3_pins_c>;
+	uart-has-rtscts;
+	status = "disabled";
+};
 
-		sai2b_port: port {
-			sai2b_endpoint: endpoint {
-				remote-endpoint = <&cs42l51_rx_endpoint>;
-				format = "i2s";
-				mclk-fs = <256>;
-				dai-tdm-slot-num = <2>;
-				dai-tdm-slot-width = <32>;
-			};
-		};
-	};
+&usbotg_hs {
+	phys = <&usbphyc_port1 0>;
+	phy-names = "usb2-phy";
+	usb-role-switch;
+	status = "okay";
 };
 
-&sdmmc1 {
-	pinctrl-names = "default", "opendrain", "sleep";
-	pinctrl-0 = <&sdmmc1_b4_pins_a>;
-	pinctrl-1 = <&sdmmc1_b4_od_pins_a>;
-	pinctrl-2 = <&sdmmc1_b4_sleep_pins_a>;
-	broken-cd;
-	st,neg-edge;
-	bus-width = <4>;
-	vmmc-supply = <&v3v3>;
+&usbphyc {
 	status = "okay";
 };
 
-&sdmmc3 {
-	pinctrl-names = "default", "opendrain", "sleep";
-	pinctrl-0 = <&sdmmc3_b4_pins_a>;
-	pinctrl-1 = <&sdmmc3_b4_od_pins_a>;
-	pinctrl-2 = <&sdmmc3_b4_sleep_pins_a>;
-	broken-cd;
-	st,neg-edge;
-	bus-width = <4>;
-	vmmc-supply = <&v3v3>;
-	status = "disabled";
+&usbphyc_port0 {
+	phy-supply = <&vdd_usb>;
 };
 
-&timers1 {
-	/* spare dmas for other usage */
-	/delete-property/dmas;
-	/delete-property/dma-names;
-	status = "disabled";
-	pwm {
-		pinctrl-0 = <&pwm1_pins_a>;
-		pinctrl-1 = <&pwm1_sleep_pins_a>;
-		pinctrl-names = "default", "sleep";
-		status = "okay";
-	};
-	timer@0 {
-		status = "okay";
-	};
+&usbphyc_port1 {
+	phy-supply = <&vdd_usb>;
 };
 
-&timers3 {
-	/delete-property/dmas;
-	/delete-property/dma-names;
-	status = "disabled";
-	pwm {
-		pinctrl-0 = <&pwm3_pins_a>;
-		pinctrl-1 = <&pwm3_sleep_pins_a>;
-		pinctrl-names = "default", "sleep";
-		status = "okay";
+/* Low-power states of regulators */
+&v1v2_hdmi {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
 	};
-	timer@2 {
-		status = "okay";
+	standby-ddr-off {
+		regulator-off-in-suspend;
 	};
 };
 
-&timers4 {
-	/delete-property/dmas;
-	/delete-property/dma-names;
-	status = "disabled";
-	pwm {
-		pinctrl-0 = <&pwm4_pins_a &pwm4_pins_b>;
-		pinctrl-1 = <&pwm4_sleep_pins_a &pwm4_sleep_pins_b>;
-		pinctrl-names = "default", "sleep";
-		status = "okay";
+&v1v8_audio {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
 	};
-	timer@3 {
-		status = "okay";
+	standby-ddr-off {
+		regulator-off-in-suspend;
 	};
 };
 
-&timers5 {
-	/delete-property/dmas;
-	/delete-property/dma-names;
-	status = "disabled";
-	pwm {
-		pinctrl-0 = <&pwm5_pins_a>;
-		pinctrl-1 = <&pwm5_sleep_pins_a>;
-		pinctrl-names = "default", "sleep";
-		status = "okay";
+&v3v3 {
+	lp-stop {
+		regulator-suspend-microvolt = <3300000>;
+		regulator-on-in-suspend;
 	};
-	timer@4 {
-		status = "okay";
+	standby-ddr-sr {
+		regulator-off-in-suspend;
 	};
-};
-
-&timers6 {
-	/delete-property/dmas;
-	/delete-property/dma-names;
-	status = "disabled";
-	timer@5 {
-		status = "okay";
+	standby-ddr-off {
+		regulator-off-in-suspend;
 	};
 };
 
-&timers12 {
-	/delete-property/dmas;
-	/delete-property/dma-names;
-	status = "disabled";
-	pwm {
-		pinctrl-0 = <&pwm12_pins_a>;
-		pinctrl-1 = <&pwm12_sleep_pins_a>;
-		pinctrl-names = "default", "sleep";
-		status = "okay";
+&v3v3_hdmi {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
 	};
-	timer@11 {
-		status = "okay";
+	standby-ddr-off {
+		regulator-off-in-suspend;
 	};
 };
 
-&uart4 {
-	pinctrl-names = "default";
-	pinctrl-0 = <&uart4_pins_a>;
-	status = "okay";
+&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;
+	};
 };
 
-&usbh_ehci {
-	phys = <&usbphyc_port0>;
-	status = "okay";
+&vdda {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
 };
 
-&usbotg_hs {
-	dr_mode = "peripheral";
-	phys = <&usbphyc_port1 0>;
-	phy-names = "usb2-phy";
-	status = "okay";
+&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;
+	};
 };
 
-&usbphyc {
-	status = "okay";
+&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;
+	};
 };
 
-&usbphyc_port0 {
-	phy-supply = <&vdd_usb>;
-	vdda1v1-supply = <&reg11>;
-	vdda1v8-supply = <&reg18>;
+&vdd_usb {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
 };
 
-&usbphyc_port1 {
-	phy-supply = <&vdd_usb>;
-	vdda1v1-supply = <&reg11>;
-	vdda1v8-supply = <&reg18>;
+&vref_ddr {
+	lp-stop {
+		regulator-on-in-suspend;
+	};
+	standby-ddr-sr {
+		regulator-on-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
 };
 
-&vrefbuf {
-	regulator-min-microvolt = <2500000>;
-	regulator-max-microvolt = <2500000>;
-	vdda-supply = <&vdd>;
-	status = "okay";
+&vtt_ddr {
+	lp-stop {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
 };
diff --git a/core/arch/arm/dts/stm32mp15xx-edx.dtsi b/core/arch/arm/dts/stm32mp15xx-edx.dtsi
new file mode 100644
index 000000000..ef51b00af
--- /dev/null
+++ b/core/arch/arm/dts/stm32mp15xx-edx.dtsi
@@ -0,0 +1,517 @@
+// 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/clock/stm32mp1-clksrc.h>
+#include <dt-bindings/power/stm32mp1-power.h>
+
+/ {
+	memory@c0000000 {
+		device_type = "memory";
+		reg = <0xC0000000 0x40000000>;
+	};
+
+	aliases {
+		serial0 = &uart4;
+	};
+
+	vin: vin {
+		compatible = "regulator-fixed";
+		regulator-name = "vin";
+		regulator-min-microvolt = <5000000>;
+		regulator-max-microvolt = <5000000>;
+		regulator-always-on;
+	};
+};
+
+&bsec {
+	board_id: board_id@ec {
+		reg = <0xec 0x4>;
+		st,non-secure-otp;
+	};
+};
+
+&clk_hse {
+	st,digbypass;
+};
+
+&cpu0{
+	cpu-supply = <&vddcore>;
+};
+
+&cpu1{
+	cpu-supply = <&vddcore>;
+};
+
+&hash1 {
+	status = "okay";
+};
+
+&i2c4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&i2c4_pins_a>;
+	i2c-scl-rising-time-ns = <185>;
+	i2c-scl-falling-time-ns = <20>;
+	clock-frequency = <400000>;
+	status = "okay";
+	secure-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";
+		secure-status = "okay";
+
+		regulators {
+			compatible = "st,stpmic1-regulators";
+			buck1-supply = <&vin>;
+			buck2-supply = <&vin>;
+			buck3-supply = <&vin>;
+			buck4-supply = <&vin>;
+			ldo1-supply = <&v3v3>;
+			ldo2-supply = <&v3v3>;
+			ldo3-supply = <&vdd_ddr>;
+			ldo4-supply = <&vin>;
+			ldo5-supply = <&v3v3>;
+			ldo6-supply = <&v3v3>;
+			vref_ddr-supply = <&vin>;
+			boost-supply = <&vin>;
+			pwr_sw1-supply = <&bst_out>;
+			pwr_sw2-supply = <&bst_out>;
+
+			vddcore: buck1 {
+				regulator-name = "vddcore";
+				regulator-min-microvolt = <1200000>;
+				regulator-max-microvolt = <1350000>;
+				regulator-always-on;
+				regulator-initial-mode = <0>;
+				regulator-over-current-protection;
+			};
+
+			vdd_ddr: buck2 {
+				regulator-name = "vdd_ddr";
+				regulator-min-microvolt = <1350000>;
+				regulator-max-microvolt = <1350000>;
+				regulator-always-on;
+				regulator-initial-mode = <0>;
+				regulator-over-current-protection;
+			};
+
+			vdd: buck3 {
+				regulator-name = "vdd";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+				st,mask-reset;
+				regulator-initial-mode = <0>;
+				regulator-over-current-protection;
+			};
+
+			v3v3: buck4 {
+				regulator-name = "v3v3";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+				regulator-over-current-protection;
+				regulator-initial-mode = <0>;
+			};
+
+			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";
+			};
+
+			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;
+			};
+
+			bst_out: boost {
+				regulator-name = "bst_out";
+			};
+
+			vbus_otg: pwr_sw1 {
+				regulator-name = "vbus_otg";
+			};
+
+			vbus_sw: pwr_sw2 {
+				regulator-name = "vbus_sw";
+				regulator-active-discharge = <1>;
+			};
+		};
+	};
+};
+
+&iwdg2 {
+	timeout-sec = <32>;
+	secure-timeout-sec = <5>;
+	status = "okay";
+	secure-status = "okay";
+};
+
+&nvmem_layout {
+	nvmem-cells = <&cfg0_otp>,
+		      <&part_number_otp>,
+		      <&monotonic_otp>,
+		      <&nand_otp>,
+		      <&uid_otp>,
+		      <&package_otp>,
+		      <&hw2_otp>,
+		      <&pkh_otp>,
+		      <&board_id>;
+
+	nvmem-cell-names = "cfg0_otp",
+			   "part_number_otp",
+			   "monotonic_otp",
+			   "nand_otp",
+			   "uid_otp",
+			   "package_otp",
+			   "hw2_otp",
+			   "pkh_otp",
+			   "board_id";
+};
+
+&pwr_regulators {
+	system_suspend_supported_soc_modes = <
+		STM32_PM_CSLEEP_RUN
+		STM32_PM_CSTOP_ALLOW_LP_STOP
+		STM32_PM_CSTOP_ALLOW_LPLV_STOP
+		STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR
+	>;
+	system_off_soc_mode = <STM32_PM_CSTOP_ALLOW_STANDBY_DDR_OFF>;
+	vdd-supply = <&vdd>;
+	vdd_3v3_usbfs-supply = <&vdd_usb>;
+};
+
+&rcc {
+	st,hsi-cal;
+	st,csi-cal;
+	st,cal-sec = <60>;
+	st,clksrc = <
+		CLK_MPU_PLL1P
+		CLK_AXI_PLL2P
+		CLK_MCU_PLL3P
+		CLK_PLL12_HSE
+		CLK_PLL3_HSE
+		CLK_PLL4_HSE
+		CLK_RTC_LSE
+		CLK_MCO1_DISABLED
+		CLK_MCO2_DISABLED
+	>;
+
+	st,clkdiv = <
+		1 /*MPU*/
+		0 /*AXI*/
+		0 /*MCU*/
+		1 /*APB1*/
+		1 /*APB2*/
+		1 /*APB3*/
+		1 /*APB4*/
+		2 /*APB5*/
+		23 /*RTC*/
+		0 /*MCO1*/
+		0 /*MCO2*/
+	>;
+
+	st,pkcs = <
+		CLK_CKPER_HSE
+		CLK_FMC_ACLK
+		CLK_QSPI_ACLK
+		CLK_ETH_DISABLED
+		CLK_SDMMC12_PLL4P
+		CLK_DSI_DSIPLL
+		CLK_STGEN_HSE
+		CLK_USBPHY_HSE
+		CLK_SPI2S1_PLL3Q
+		CLK_SPI2S23_PLL3Q
+		CLK_SPI45_HSI
+		CLK_SPI6_HSI
+		CLK_I2C46_HSI
+		CLK_SDMMC3_PLL4P
+		CLK_USBO_USBPHY
+		CLK_ADC_CKPER
+		CLK_CEC_LSE
+		CLK_I2C12_HSI
+		CLK_I2C35_HSI
+		CLK_UART1_HSI
+		CLK_UART24_HSI
+		CLK_UART35_HSI
+		CLK_UART6_HSI
+		CLK_UART78_HSI
+		CLK_SPDIF_PLL4P
+		CLK_FDCAN_PLL4R
+		CLK_SAI1_PLL3Q
+		CLK_SAI2_PLL3Q
+		CLK_SAI3_PLL3Q
+		CLK_SAI4_PLL3Q
+		CLK_RNG1_LSI
+		CLK_RNG2_LSI
+		CLK_LPTIM1_PCLK1
+		CLK_LPTIM23_PCLK3
+		CLK_LPTIM45_LSE
+	>;
+
+	/* VCO = 1066.0 MHz => P = 266 (AXI), Q = 533 (GPU), R = 533 (DDR) */
+	pll2: st,pll@1 {
+		compatible = "st,stm32mp1-pll";
+		reg = <1>;
+		cfg = <2 65 1 0 0 PQR(1,1,1)>;
+		frac = <0x1400>;
+	};
+
+	/* VCO = 417.8 MHz => P = 209, Q = 24, R = 11 */
+	pll3: st,pll@2 {
+		compatible = "st,stm32mp1-pll";
+		reg = <2>;
+		cfg = <1 33 1 16 36 PQR(1,1,1)>;
+		frac = <0x1a04>;
+	};
+
+	/* VCO = 594.0 MHz => P = 99, Q = 74, R = 74 */
+	pll4: st,pll@3 {
+		compatible = "st,stm32mp1-pll";
+		reg = <3>;
+		cfg = <3 98 5 7 7 PQR(1,1,1)>;
+	};
+};
+
+&rng1 {
+	status = "okay";
+	secure-status = "okay";
+};
+
+&rtc {
+	status = "okay";
+	secure-status = "okay";
+};
+
+&sdmmc1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&sdmmc1_b4_pins_a &sdmmc1_dir_pins_a>;
+	disable-wp;
+	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;
+	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 = <&vdd>;
+	mmc-ddr-3_3v;
+	status = "okay";
+};
+
+&timers15 {
+	secure-status = "okay";
+	st,hsi-cal-input = <7>;
+	st,csi-cal-input = <8>;
+};
+
+&uart4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&uart4_pins_a>;
+	status = "okay";
+};
+
+&usbotg_hs {
+	vbus-supply = <&vbus_otg>;
+};
+
+&usbphyc_port0 {
+	phy-supply = <&vdd_usb>;
+};
+
+&usbphyc_port1 {
+	phy-supply = <&vdd_usb>;
+};
+
+/* Low-power states of regulators */
+&v1v8 {
+	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;
+	};
+};
+
+&v3v3 {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vdd {
+	lp-stop {
+		regulator-suspend-microvolt = <3300000>;
+		regulator-on-in-suspend;
+	};
+	lplv-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;
+	};
+};
+
+&vdda {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vddcore {
+	lp-stop {
+		regulator-on-in-suspend;
+		regulator-suspend-microvolt = <1200000>;
+	};
+	lplv-stop {
+		regulator-on-in-suspend;
+		regulator-suspend-microvolt = <900000>;
+	};
+	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;
+	};
+	lplv-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_sd {
+	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;
+	};
+};
+
+&vref_ddr {
+	lp-stop {
+		regulator-on-in-suspend;
+	};
+	lplv-stop {
+		regulator-on-in-suspend;
+	};
+	standby-ddr-sr {
+		regulator-on-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vtt_ddr {
+	lp-stop {
+		regulator-off-in-suspend;
+	};
+	lplv-stop {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
diff --git a/core/arch/arm/dts/stm32mp15xx-evx.dtsi b/core/arch/arm/dts/stm32mp15xx-evx.dtsi
new file mode 100644
index 000000000..ce1982d45
--- /dev/null
+++ b/core/arch/arm/dts/stm32mp15xx-evx.dtsi
@@ -0,0 +1,73 @@
+// 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.
+ */
+
+&fmc {
+	pinctrl-names = "default";
+	pinctrl-0 = <&fmc_pins_a>;
+	status = "okay";
+
+	nand-controller@4,0 {
+		status = "okay";
+
+		nand@0 {
+			reg = <0>;
+			nand-on-flash-bbt;
+			#address-cells = <1>;
+			#size-cells = <1>;
+		};
+	};
+};
+
+&i2c4 {
+	pmic: stpmic@33 {
+		regulators {
+			v1v8: ldo6 {
+				regulator-enable-ramp-delay = <300000>;
+			};
+		};
+	};
+};
+
+&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>;
+	};
+};
+
+&timers12 {
+	status = "disabled";
+};
+
+&usart3 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&usart3_pins_b>;
+	uart-has-rtscts;
+	status = "disabled";
+};
+
+&usbotg_hs {
+	pinctrl-0 = <&usbotg_hs_pins_a>;
+	pinctrl-names = "default";
+	phys = <&usbphyc_port1 0>;
+	phy-names = "usb2-phy";
+	status = "okay";
+};
+
+&usbphyc {
+	status = "okay";
+};
diff --git a/core/arch/arm/dts/stm32mp15xxaa-pinctrl.dtsi b/core/arch/arm/dts/stm32mp15xxaa-pinctrl.dtsi
index 04f7a43ad..4f4130eff 100644
--- a/core/arch/arm/dts/stm32mp15xxaa-pinctrl.dtsi
+++ b/core/arch/arm/dts/stm32mp15xxaa-pinctrl.dtsi
@@ -79,6 +79,7 @@
 
 	gpioz: gpio@54004000 {
 		status = "okay";
+		secure-status = "okay";
 		ngpios = <8>;
 		gpio-ranges = <&pinctrl_z 0 400 8>;
 	};
diff --git a/core/arch/arm/dts/stm32mp15xxac-pinctrl.dtsi b/core/arch/arm/dts/stm32mp15xxac-pinctrl.dtsi
index 7eaa245f4..866c050da 100644
--- a/core/arch/arm/dts/stm32mp15xxac-pinctrl.dtsi
+++ b/core/arch/arm/dts/stm32mp15xxac-pinctrl.dtsi
@@ -67,6 +67,7 @@
 
 	gpioz: gpio@54004000 {
 		status = "okay";
+		secure-status = "okay";
 		ngpios = <8>;
 		gpio-ranges = <&pinctrl_z 0 400 8>;
 	};
diff --git a/core/arch/arm/include/arm32.h b/core/arch/arm/include/arm32.h
index f28b73fda..2f67cc762 100644
--- a/core/arch/arm/include/arm32.h
+++ b/core/arch/arm/include/arm32.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: BSD-2-Clause */
 /*
- * Copyright (c) 2016, Linaro Limited
+ * Copyright (c) 2016-2019, Linaro Limited
  * Copyright (c) 2014, STMicroelectronics International N.V.
  */
 
@@ -163,6 +163,15 @@
 #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)
+
 #ifndef __ASSEMBLER__
 #include <generated/arm32_sysreg.h>
 #ifdef CFG_ARM_GICV3
diff --git a/core/arch/arm/include/kernel/tlb_helpers.h b/core/arch/arm/include/kernel/tlb_helpers.h
index d33dd5c01..2dbcbcf08 100644
--- a/core/arch/arm/include/kernel/tlb_helpers.h
+++ b/core/arch/arm/include/kernel/tlb_helpers.h
@@ -25,11 +25,10 @@ static inline void tlbi_mva_allasid_nosync(vaddr_t va)
 #endif
 }
 
-static inline void tlbi_mva_asid(vaddr_t va, uint32_t asid)
+static inline void tlbi_mva_asid_nosync(vaddr_t va, uint32_t asid)
 {
 	uint32_t a = asid & TLBI_ASID_MASK;
 
-	dsb_ishst();
 #ifdef ARM64
 	tlbi_vale1is((va >> TLBI_MVA_SHIFT) | SHIFT_U64(a, TLBI_ASID_SHIFT));
 	tlbi_vale1is((va >> TLBI_MVA_SHIFT) |
@@ -38,6 +37,12 @@ static inline void tlbi_mva_asid(vaddr_t va, uint32_t asid)
 	write_tlbimvais((va & ~(BIT32(TLBI_MVA_SHIFT) - 1)) | a);
 	write_tlbimvais((va & ~(BIT32(TLBI_MVA_SHIFT) - 1)) | a | 1);
 #endif
+}
+
+static inline void tlbi_mva_asid(vaddr_t va, uint32_t asid)
+{
+	dsb_ishst();
+	tlbi_mva_asid_nosync(va, asid);
 	dsb_ish();
 	isb();
 }
diff --git a/core/arch/arm/include/mm/core_mmu.h b/core/arch/arm/include/mm/core_mmu.h
index 33ee2ebd7..bf8e68607 100644
--- a/core/arch/arm/include/mm/core_mmu.h
+++ b/core/arch/arm/include/mm/core_mmu.h
@@ -97,6 +97,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_EXT_DT:   Memory loads external device tree
@@ -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_EXT_DT,
@@ -150,6 +152,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_EXT_DT] = "EXT_DT",
@@ -629,6 +632,18 @@ enum teecore_tlb_op {
 /* TLB invalidation for a range of virtual address */
 void tlbi_mva_range(vaddr_t va, size_t size, size_t granule);
 
+/*
+ * tlbi_mva_range_asid() - Invalidate TLB for virtual address range for
+ *			   a specific ASID
+ * @va:		start virtual address, must be a multiple of @granule
+ * @len:	length in bytes of range, must be a multiple of @granule
+ * @granule:	granularity of mapping, supported values are
+ *		CORE_MMU_PGDIR_SIZE or SMALL_PAGE_SIZE. This value must
+ *		match the actual mappings.
+ * @asid:	Address space identifier
+ */
+void tlbi_mva_range_asid(vaddr_t va, size_t len, size_t granule, uint32_t asid);
+
 /* deprecated: please call straight tlbi_all() and friends */
 int core_tlb_maintenance(int op, unsigned long a) __deprecated;
 
diff --git a/core/arch/arm/include/mm/pgt_cache.h b/core/arch/arm/include/mm/pgt_cache.h
index d19414e48..433dc3920 100644
--- a/core/arch/arm/include/mm/pgt_cache.h
+++ b/core/arch/arm/include/mm/pgt_cache.h
@@ -23,8 +23,8 @@ struct ts_ctx;
 
 struct pgt {
 	void *tbl;
-#if defined(CFG_PAGED_USER_TA)
 	vaddr_t vabase;
+#if defined(CFG_PAGED_USER_TA)
 	struct ts_ctx *ctx;
 	size_t num_used_entries;
 #endif
@@ -61,6 +61,8 @@ void pgt_alloc(struct pgt_cache *pgt_cache, struct ts_ctx *owning_ctx,
 	       vaddr_t begin, vaddr_t last);
 void pgt_free(struct pgt_cache *pgt_cache, bool save_ctx);
 
+void pgt_clear_ctx_range(struct pgt_cache *pgt_cache, struct ts_ctx *ctx,
+			 vaddr_t begin, vaddr_t end);
 #ifdef CFG_PAGED_USER_TA
 void pgt_flush_ctx_range(struct pgt_cache *pgt_cache, struct ts_ctx *ctx,
 			 vaddr_t begin, vaddr_t last);
@@ -73,6 +75,7 @@ static inline void pgt_flush_ctx_range(struct pgt_cache *pgt_cache __unused,
 }
 #endif
 
+
 void pgt_init(void);
 
 #if defined(CFG_PAGED_USER_TA)
diff --git a/core/arch/arm/include/sm/pm.h b/core/arch/arm/include/sm/pm.h
index 939f966e8..90f031a2b 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/mm/core_mmu.c b/core/arch/arm/mm/core_mmu.c
index 4f27b2499..489dae220 100644
--- a/core/arch/arm/mm/core_mmu.c
+++ b/core/arch/arm/mm/core_mmu.c
@@ -388,6 +388,7 @@ void core_mmu_set_discovered_nsec_ddr(struct core_mmu_phys_mem *start,
 			carve_out_phys_mem(&m, &num_elems, map->pa, map->size);
 			break;
 		case MEM_AREA_EXT_DT:
+		case MEM_AREA_RAM_NSEC:
 		case MEM_AREA_RES_VASPACE:
 		case MEM_AREA_SHM_VASPACE:
 		case MEM_AREA_TA_VASPACE:
@@ -490,7 +491,6 @@ static bool pbuf_is_sdp_mem(paddr_t pbuf __unused, size_t len __unused)
 
 /* Check special memories comply with registered memories */
 static void verify_special_mem_areas(struct tee_mmap_region *mem_map,
-				     size_t len,
 				     const struct core_mmu_phys_mem *start,
 				     const struct core_mmu_phys_mem *end,
 				     const char *area_name __maybe_unused)
@@ -498,7 +498,6 @@ static void verify_special_mem_areas(struct tee_mmap_region *mem_map,
 	const struct core_mmu_phys_mem *mem;
 	const struct core_mmu_phys_mem *mem2;
 	struct tee_mmap_region *mmap;
-	size_t n;
 
 	if (start == end) {
 		DMSG("No %s memory area defined", area_name);
@@ -524,9 +523,18 @@ static void verify_special_mem_areas(struct tee_mmap_region *mem_map,
 	/*
 	 * Check memories do not intersect any mapped memory.
 	 * This is called before reserved VA space is loaded in mem_map.
+	 *
+	 * Exceptions are the memory areas that maps with the same attributes
+	 * as for example MEM_AREA_RAM_NSEC and MEM_AREA_NSEC_SHM
+	 * which may overlap since they are used for the same purpose
+	 * except that MEM_AREA_NSEC_SHM is always mapped and
+	 * MEM_AREA_RAM_NSEC only uses a dynamic mapping.
 	 */
 	for (mem = start; mem < end; mem++) {
-		for (mmap = mem_map, n = 0; n < len; mmap++, n++) {
+		for (mmap = mem_map; mmap->type != MEM_AREA_END; mmap++) {
+			if (core_mmu_type_to_attr(mem->type) ==
+			    core_mmu_type_to_attr(mmap->type))
+				continue;
 			if (core_is_buffer_intersect(mem->addr, mem->size,
 						     mmap->pa, mmap->size)) {
 				MSG_MEM_INSTERSECT(mem->addr, mem->size,
@@ -650,6 +658,8 @@ uint32_t core_mmu_type_to_attr(enum teecore_memtypes t)
 	case MEM_AREA_RAM_SEC:
 	case MEM_AREA_SEC_RAM_OVERALL:
 		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;
@@ -841,8 +851,7 @@ static size_t collect_mem_ranges(struct tee_mmap_region *memory_map,
 	}
 
 	if (IS_ENABLED(CFG_SECURE_DATA_PATH))
-		verify_special_mem_areas(memory_map, num_elems,
-					 phys_sdp_mem_begin,
+		verify_special_mem_areas(memory_map, phys_sdp_mem_begin,
 					 phys_sdp_mem_end, "SDP");
 
 	add_va_space(memory_map, num_elems, MEM_AREA_RES_VASPACE,
@@ -1145,6 +1154,7 @@ static void check_mem_map(struct tee_mmap_region *map)
 		case MEM_AREA_IO_NSEC:
 		case MEM_AREA_EXT_DT:
 		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:
@@ -1395,6 +1405,21 @@ void tlbi_mva_range(vaddr_t va, size_t size, size_t granule)
 	isb();
 }
 
+void tlbi_mva_range_asid(vaddr_t va, size_t len, size_t granule, uint32_t asid)
+{
+	assert(granule == CORE_MMU_PGDIR_SIZE || granule == SMALL_PAGE_SIZE);
+	assert(!(va & (granule - 1)) && !(len & (granule - 1)));
+
+	dsb_ishst();
+	while (len) {
+		tlbi_mva_asid_nosync(va, asid);
+		len -= granule;
+		va += granule;
+	}
+	dsb_ish();
+	isb();
+}
+
 TEE_Result cache_op_inner(enum cache_op op, void *va, size_t len)
 {
 	switch (op) {
diff --git a/core/arch/arm/mm/mobj.c b/core/arch/arm/mm/mobj.c
index 7a129cf5a..b14e93939 100644
--- a/core/arch/arm/mm/mobj.c
+++ b/core/arch/arm/mm/mobj.c
@@ -49,6 +49,7 @@ static void *mobj_phys_get_va(struct mobj *mobj, size_t offset)
 
 	return (void *)(moph->va + offset);
 }
+DECLARE_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)
@@ -228,7 +229,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));
 }
-
+DECLARE_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)
@@ -657,4 +658,4 @@ static TEE_Result mobj_init(void)
 	return TEE_SUCCESS;
 }
 
-driver_init_late(mobj_init);
+service_init(mobj_init);
diff --git a/core/arch/arm/mm/pgt_cache.c b/core/arch/arm/mm/pgt_cache.c
index 3e854934d..988c1a351 100644
--- a/core/arch/arm/mm/pgt_cache.c
+++ b/core/arch/arm/mm/pgt_cache.c
@@ -413,13 +413,65 @@ static void pgt_free_unlocked(struct pgt_cache *pgt_cache,
 	}
 }
 
-static struct pgt *pop_from_some_list(vaddr_t vabase __unused,
+static struct pgt *pop_from_some_list(vaddr_t vabase,
 				      struct ts_ctx *ctx __unused)
 {
-	return pop_from_free_list();
+	struct pgt *p = pop_from_free_list();
+
+	if (p)
+		p->vabase = vabase;
+
+	return p;
 }
 #endif /*!CFG_PAGED_USER_TA*/
 
+static void clear_ctx_range_from_list(struct pgt_cache *pgt_cache,
+				      void *ctx __maybe_unused,
+				      vaddr_t begin, vaddr_t end)
+{
+	struct pgt *p = NULL;
+#ifdef CFG_WITH_LPAE
+	uint64_t *tbl = NULL;
+#else
+	uint32_t *tbl = NULL;
+#endif
+	unsigned int idx = 0;
+	unsigned int n = 0;
+
+	SLIST_FOREACH(p, pgt_cache, link) {
+		vaddr_t b = MAX(p->vabase, begin);
+		vaddr_t e = MIN(p->vabase + CORE_MMU_PGDIR_SIZE, end);
+
+#ifdef CFG_PAGED_USER_TA
+		if (p->ctx != ctx)
+			continue;
+#endif
+		if (b >= e)
+			continue;
+
+		tbl = p->tbl;
+		idx = (b - p->vabase) / SMALL_PAGE_SIZE;
+		n = (e - b) / SMALL_PAGE_SIZE;
+		memset(tbl + idx, 0, n * sizeof(*tbl));
+	}
+}
+
+void pgt_clear_ctx_range(struct pgt_cache *pgt_cache, struct ts_ctx *ctx,
+			 vaddr_t begin, vaddr_t end)
+{
+	mutex_lock(&pgt_mu);
+
+	if (pgt_cache)
+		clear_ctx_range_from_list(pgt_cache, ctx, begin, end);
+#ifdef CFG_PAGED_USER_TA
+	clear_ctx_range_from_list(&pgt_cache_list, ctx, begin, end);
+#endif
+
+	mutex_unlock(&pgt_mu);
+}
+
+
+
 static bool pgt_alloc_unlocked(struct pgt_cache *pgt_cache, struct ts_ctx *ctx,
 			       vaddr_t begin, vaddr_t last)
 {
diff --git a/core/arch/arm/plat-stm32mp1/boot_api.h b/core/arch/arm/plat-stm32mp1/boot_api.h
index 62e38b588..a7daffd44 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 baf3fe49a..b1d41c92a 100644
--- a/core/arch/arm/plat-stm32mp1/conf.mk
+++ b/core/arch/arm/plat-stm32mp1/conf.mk
@@ -1,22 +1,41 @@
 # 1GB and 512MB DDR targets do not locate secure DDR at the same place.
 flavor_dts_file-157A_DK1 = stm32mp157a-dk1.dts
+flavor_dts_file-157A_ED1 = stm32mp157a-ed1.dts
+flavor_dts_file-157A_EV1 = stm32mp157a-ev1.dts
 flavor_dts_file-157C_DK2 = stm32mp157c-dk2.dts
 flavor_dts_file-157C_ED1 = stm32mp157c-ed1.dts
 flavor_dts_file-157C_EV1 = stm32mp157c-ev1.dts
+flavor_dts_file-157D_DK1 = stm32mp157d-dk1.dts
+flavor_dts_file-157D_ED1 = stm32mp157d-ed1.dts
+flavor_dts_file-157D_EV1 = stm32mp157d-ev1.dts
+flavor_dts_file-157F_DK2 = stm32mp157f-dk2.dts
+flavor_dts_file-157F_ED1 = stm32mp157f-ed1.dts
+flavor_dts_file-157F_EV1 = stm32mp157f-ev1.dts
 
-flavorlist-cryp-512M = $(flavor_dts_file-157C_DK2)
+flavorlist-cryp-512M = $(flavor_dts_file-157C_DK2) \
+		       $(flavor_dts_file-157F_DK2)
 
-flavorlist-no_cryp-512M = $(flavor_dts_file-157A_DK1)
+flavorlist-no_cryp-512M = $(flavor_dts_file-157A_DK1) \
+			  $(flavor_dts_file-157D_DK1)
 
 flavorlist-cryp-1G = $(flavor_dts_file-157C_ED1) \
-		     $(flavor_dts_file-157C_EV1)
+		     $(flavor_dts_file-157C_EV1) \
+		     $(flavor_dts_file-157F_ED1) \
+		     $(flavor_dts_file-157F_EV1)
 
-flavorlist-no_cryp = $(flavorlist-no_cryp-512M)
+flavorlist-no_cryp-1G = $(flavor_dts_file-157A_ED1) \
+			$(flavor_dts_file-157A_EV1) \
+			$(flavor_dts_file-157D_ED1) \
+			$(flavor_dts_file-157D_EV1)
+
+flavorlist-no_cryp = $(flavorlist-no_cryp-512M) \
+		     $(flavorlist-no_cryp-1G)
 
 flavorlist-512M = $(flavorlist-cryp-512M) \
 		  $(flavorlist-no_cryp-512M)
 
-flavorlist-1G = $(flavorlist-cryp-1G)
+flavorlist-1G = $(flavorlist-cryp-1G) \
+		$(flavorlist-no_cryp-1G)
 
 ifneq ($(PLATFORM_FLAVOR),)
 ifeq ($(flavor_dts_file-$(PLATFORM_FLAVOR)),)
@@ -31,9 +50,14 @@ endif
 
 include core/arch/arm/cpu/cortex-a7.mk
 
+$(call force,CFG_CLK_DRIVER,y)
+$(call force,CFG_ARM_GIC_PM,y)
 $(call force,CFG_BOOT_SECONDARY_REQUEST,y)
 $(call force,CFG_GIC,y)
 $(call force,CFG_INIT_CNTVOFF,y)
+$(call force,CFG_PM,y)
+$(call force,CFG_PM_ARM32,y)
+$(call force,CFG_PM_STUBS,y)
 $(call force,CFG_PSCI_ARM32,y)
 $(call force,CFG_SCMI_MSG_DRIVERS,y)
 $(call force,CFG_SCMI_MSG_CLOCK,y)
@@ -52,6 +76,8 @@ CFG_SHMEM_START  ?= 0xdfe00000
 CFG_DRAM_SIZE    ?= 0x20000000
 endif
 
+CFG_DTB_MAX_SIZE ?= 0x20000
+
 CFG_TZSRAM_START ?= 0x2ffc0000
 CFG_TZSRAM_SIZE  ?= 0x0003f000
 CFG_STM32MP1_SCMI_SHM_BASE ?= 0x2ffff000
@@ -65,23 +91,45 @@ CFG_DRAM_SIZE    ?= 0x40000000
 CFG_TEE_CORE_NB_CORE ?= 2
 CFG_WITH_PAGER ?= y
 CFG_WITH_LPAE ?= y
-CFG_MMAP_REGIONS ?= 23
+CFG_MMAP_REGIONS ?= 30
+CFG_CORE_HEAP_SIZE ?= 49152
+CFG_DTB_MAX_SIZE = 0x20000
+
+# Disable early TA compression to limit HEAP size
+CFG_EARLY_TA_COMPRESS ?= n
+
+# Embed public part of this key in OP-TEE OS
+CFG_RPROC_SIGN_KEY ?= keys/default_rproc.pem
 
 ifeq ($(CFG_EMBED_DTB_SOURCE_FILE),)
 # Some drivers mandate DT support
+$(call force,CFG_STM32_CLKCALIB,n)
 $(call force,CFG_STM32_I2C,n)
+$(call force,CFG_STM32_IWDG,n)
+$(call force,CFG_STM32_TIM,n)
 $(call force,CFG_STPMIC1,n)
 endif
 
+CFG_RPROC_PTA ?= n
 CFG_STM32_BSEC ?= y
+CFG_STM32_CLKCALIB ?= y
+CFG_STM32_CRYP ?= y
 CFG_STM32_ETZPC ?= y
 CFG_STM32_GPIO ?= y
 CFG_STM32_I2C ?= y
+CFG_STM32_IWDG ?= y
 CFG_STM32_RNG ?= y
+CFG_STM32_RTC ?= y
+CFG_STM32_TIM ?= y
 CFG_STM32_UART ?= y
+CFG_STM32MP15_CLK ?= y
 CFG_STPMIC1 ?= y
 CFG_TZC400 ?= y
 
+ifeq ($(CFG_STM32_CLKCALIB),y)
+$(call force,CFG_STM32_TIM,y)
+endif
+
 ifeq ($(CFG_STPMIC1),y)
 $(call force,CFG_STM32_I2C,y)
 $(call force,CFG_STM32_GPIO,y)
@@ -92,12 +140,23 @@ CFG_STM32MP_PANIC_ON_TZC_PERM_VIOLATION ?= y
 
 # SiP/OEM service for non-secure world
 CFG_STM32_BSEC_SIP ?= y
+CFG_STM32_CLKCALIB_SIP ?= y
+CFG_STM32_LOWPOWER_SIP ?= $(CFG_PM)
+CFG_STM32_PWR_SIP ?= y
+CFG_STM32_RCC_SIP ?= y
+
+# Default use stm32mp1 PM mailbox context version 2
+# Use CFG_STM32MP15_PM_CONTEX_VERSION=1 to force version 0 when dealing with
+# a TF-A firmware that supports version 1 of the context mailbox.
+CFG_STM32MP15_PM_CONTEX_VERSION ?= 2
 
 # Default enable some test facitilites
 CFG_TEE_CORE_EMBED_INTERNAL_TESTS ?= y
 CFG_WITH_STATS ?= y
+CFG_WERROR ?= y
 
 # Default disable some support for pager memory size constraint
+CFG_TEE_CORE_LOG_LEVEL ?= 2
 CFG_TEE_CORE_DEBUG ?= n
 CFG_UNWIND ?= n
 CFG_LOCKDEP ?= n
@@ -108,3 +167,6 @@ CFG_WITH_NSEC_GPIOS ?= y
 CFG_WITH_NSEC_UARTS ?= y
 # UART instance used for early console (0 disables early console)
 CFG_STM32_EARLY_CONSOLE_UART ?= 4
+
+# Generate the STM32 files
+CFG_STM32MP15x_STM32IMAGE ?= n
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 000000000..d52f638eb
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_calib.c
@@ -0,0 +1,507 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2018-2020, STMicroelectronics
+ */
+
+#include <arm.h>
+#include <drivers/clk.h>
+#include <drivers/stm32_tim.h>
+#include <drivers/stm32mp1_rcc.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <initcall.h>
+#include <io.h>
+#include <libfdt.h>
+#include <keep.h>
+#include <kernel/boot.h>
+#include <kernel/delay.h>
+#include <kernel/dt.h>
+#include <kernel/interrupt.h>
+#include <kernel/panic.h>
+#include <kernel/pm.h>
+#include <limits.h>
+#include <mm/core_memprot.h>
+#include <stm32_util.h>
+#include <string.h>
+
+#define CALIBRATION_TIMEOUT_US		10000
+
+/* List of forbiden values for HSI and CSI */
+static const uint16_t fbv_hsi[] = {
+	512, 480, 448, 416, 384, 352, 320, 288,
+	256, 224, 192, 160, 128, 96, 64, 32, 0,
+};
+static const 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 max;	/* Max boundary trim value around forbidden value */
+	unsigned int min;	/* Min boundary trim value around forbidden value */
+};
+
+struct stm32mp1_clk_cal {
+	const 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 = 5,
+	.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 = 8,
+	.set_trim = csi_set_trim,
+	.get_trim = csi_get_trimed_cal,
+};
+
+static int get_signed_value(uint8_t val)
+{
+	return (int8_t)(val << 1) / 2;
+}
+
+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;
+
+	io_clrsetbits32(stm32_rcc_base() + RCC_HSICFGR,
+			RCC_HSICFGR_HSITRIM_MASK, trim);
+}
+DECLARE_KEEP_PAGER(hsi_set_trim);
+
+static unsigned int hsi_get_trimed_cal(void)
+{
+	uint32_t utrim = (io_read32(stm32_rcc_base() + RCC_HSICFGR) &
+			  RCC_HSICFGR_HSITRIM_MASK) >>
+			 RCC_HSICFGR_HSITRIM_SHIFT;
+	int trim = get_signed_value((uint8_t)utrim);
+
+	if (trim + (int)hsi_calib->cal_ref < 0)
+		return 0;
+
+	return hsi_calib->cal_ref + trim;
+}
+DECLARE_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;
+
+	io_clrsetbits32(stm32_rcc_base() + RCC_CSICFGR,
+			RCC_CSICFGR_CSITRIM_MASK, trim);
+}
+DECLARE_KEEP_PAGER(csi_set_trim);
+
+static unsigned int csi_get_trimed_cal(void)
+{
+	uint32_t trim = (io_read32(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;
+}
+DECLARE_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 = NULL;
+	unsigned int new_cal = 0;
+	int i = 0;
+
+	/* 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->min) {
+			new_cal = boundary->min;
+			break;
+		}
+
+		if ((cal >= boundary->min) && (cal < boundary->max)) {
+			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 = NULL;
+	unsigned int new_cal = 0;
+	unsigned int i = 0;
+
+	/* 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->max) {
+			new_cal = boundary->max;
+			break;
+		}
+
+		if ((cal > boundary->min) && (cal <= boundary->max)) {
+			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) / 1000;
+	unsigned long min = clk_cal->ref_freq - margin;
+	unsigned long max = clk_cal->ref_freq + margin;
+	unsigned long freq = clk_cal->get_freq();
+	int trim = 0;
+	int new_trim = 0;
+	unsigned long conv = 0;
+	unsigned long min_conv = ULONG_MAX;
+	uint64_t timeout_ref = 0;
+
+	if ((freq >= min) && (freq <= max))
+		return;
+
+	trim = clk_cal->get_trim();
+	timeout_ref = timeout_init_us(CALIBRATION_TIMEOUT_US);
+	do {
+		if (freq < clk_cal->ref_freq)
+			new_trim = trim_increase(clk_cal, trim);
+		else
+			new_trim = trim_decrease(clk_cal, trim);
+
+		clk_cal->set_trim(new_trim);
+		freq = clk_cal->get_freq();
+		if (freq == 0) {
+			/* Calibration will be stopped */
+			clk_cal->ref_freq = 0U;
+			return;
+		}
+		conv = (clk_cal->ref_freq < freq) ?
+			freq - clk_cal->ref_freq : clk_cal->ref_freq - freq;
+		if (conv < min_conv) {
+			min_conv = conv;
+			trim = new_trim;
+		}
+
+		if (timeout_elapsed(timeout_ref))
+			break;
+
+	} while (conv == min_conv);
+
+	clk_cal->set_trim(trim);
+	freq = clk_cal->get_freq();
+	if ((freq < min) || (freq > max)) {
+		EMSG("%s Calibration : Freq %lu , trim %i\n",
+		     (clk_cal->set_trim == hsi_set_trim) ? "HSI" : "CSI",
+		     freq, trim);
+	}
+}
+
+static void save_trim(struct stm32mp1_clk_cal *clk_cal,
+		      unsigned int i, unsigned int max, unsigned int min)
+{
+	clk_cal->boundary[i].max = max;
+	clk_cal->boundary[i].min = min;
+}
+
+static int trim_find_prev_boundary(struct stm32mp1_clk_cal *clk_cal,
+				   unsigned int x1)
+{
+	unsigned int x = x1;
+	unsigned long freq = 0;
+
+	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)
+{
+	const uint16_t *trim_fbv = clk_cal->fbv;
+	unsigned int min = 0;
+	unsigned int max = 0;
+	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 = 0;
+		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();
+
+	if (timer_val > INT32_MAX)
+		timer_val = INT32_MAX;
+
+	DMSG("Calibration timeout set to %"PRIu32, timer_val / 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,
+};
+DECLARE_KEEP_PAGER(arm_cntp_handler);
+
+static TEE_Result timer_pm(enum pm_op op, uint32_t pm_hint __unused,
+			   const struct pm_callback_handle *handle __unused)
+{
+	if (op == PM_OP_RESUME && timer_val)
+		calib_period();
+
+	return TEE_SUCCESS;
+}
+DECLARE_KEEP_PAGER(timer_pm);
+
+static TEE_Result init_arm_cntp_timer(void)
+{
+	itr_add(&arm_cntp_handler);
+	itr_enable(arm_cntp_handler.it);
+
+	register_pm_driver_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 = 0;
+        int lenp = 0;
+        const fdt32_t *cuint = fdt_getprop(fdt, node, "st,cal-sec", &lenp);
+
+	if (cuint)
+		period = fdt32_to_cpu(*cuint);
+
+	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 = NULL;
+
+	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 int init_hsi_calibration(void *fdt, int node)
+{
+	if (!fdt_getprop(fdt, node, "st,hsi-cal", NULL))
+		return 0;
+
+	hsi_calib = calloc(1, sizeof(*hsi_calib));
+	assert(hsi_calib);
+	memcpy(hsi_calib, &hsi_calib_config, sizeof(*hsi_calib));
+
+	stm32_tim_freq_func(&hsi_calib->get_freq, HSI_CAL);
+	if (hsi_calib->get_freq == NULL) {
+		free(hsi_calib);
+		return -1;
+	}
+
+	hsi_calib->ref_freq = clk_get_rate(CK_HSI);
+
+	hsi_calib->cal_ref = (io_read32(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);
+	return 1;
+}
+
+static int init_csi_calibration(void *fdt, int node)
+{
+	if (!fdt_getprop(fdt, node, "st,csi-cal", NULL))
+		return 0;
+
+	csi_calib = calloc(1, sizeof(*csi_calib));
+	assert(csi_calib);
+	memcpy(csi_calib, &csi_calib_config, sizeof(*csi_calib));
+
+	stm32_tim_freq_func(&csi_calib->get_freq, CSI_CAL);
+	if (csi_calib->get_freq == NULL) {
+		free(csi_calib);
+		return -1;
+	}
+
+	csi_calib->ref_freq = clk_get_rate(CK_CSI);
+
+	csi_calib->cal_ref = (io_read32(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);
+	return 1;
+}
+
+static TEE_Result init_stm32mp1_calib(void)
+{
+	void *fdt = NULL;
+	int rcc_node = 0;
+	int res_csi = 0;
+	int res_hsi = 0;
+
+	fdt = get_embedded_dt();
+	if (!fdt)
+		panic();
+
+	rcc_node = fdt_node_offset_by_compatible(fdt, -1, DT_RCC_CLK_COMPAT);
+	if (rcc_node < 0)
+		panic();
+
+	res_hsi = init_hsi_calibration(fdt, rcc_node);
+	if (res_hsi < 0)
+		panic("HSI calibration init failed");
+	res_csi = init_csi_calibration(fdt, rcc_node);
+	if (res_csi < 0)
+		panic("CSI calibration init failed");
+	if (res_csi || res_hsi)
+		init_periodic_calibration(fdt, rcc_node);
+
+	return TEE_SUCCESS;
+}
+driver_init(init_stm32mp1_calib);
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 000000000..1c2d9b2c8
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.c
@@ -0,0 +1,469 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2020, 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_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_memprot.h>
+#include <platform_config.h>
+#include <string.h>
+#include <trace.h>
+
+#define TIMEOUT_500US		500
+
+static enum stm32mp1_ddr_sr_mode saved_ddr_sr_mode;
+
+static vaddr_t get_ddrctrl_base(void)
+{
+	static struct io_pa_va base __nex_data = { .pa = DDRCTRL_BASE };
+
+	return io_pa_or_va(&base);
+}
+
+static vaddr_t get_ddrphy_base(void)
+{
+	static struct io_pa_va base __nex_data = { .pa = DDRPHYC_BASE };
+
+	return io_pa_or_va(&base);
+}
+
+static void ddr_disable_clock(void)
+{
+	vaddr_t rcc_base = stm32_rcc_base();
+
+	/* Disable all clocks */
+	io_clrbits32(rcc_base + RCC_DDRITFCR,
+		     RCC_DDRITFCR_DDRC1EN |
+		     RCC_DDRITFCR_DDRC2EN |
+		     RCC_DDRITFCR_DDRPHYCAPBEN |
+		     RCC_DDRITFCR_DDRCAPBEN);
+}
+
+static void ddr_enable_clock(void)
+{
+	vaddr_t rcc_base = stm32_rcc_base();
+
+	/* Enable all clocks */
+	io_setbits32(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)
+{
+	vaddr_t ddrctrl_base = get_ddrctrl_base();
+
+	io_clrbits32(ddrctrl_base + DDRCTRL_SWCTL, DDRCTRL_SWCTL_SW_DONE);
+}
+
+static void do_sw_ack(void)
+{
+	uint64_t timeout_ref = 0;
+	vaddr_t ddrctrl_base = get_ddrctrl_base();
+
+	io_setbits32(ddrctrl_base + DDRCTRL_SWCTL, DDRCTRL_SWCTL_SW_DONE);
+
+	timeout_ref = timeout_init_us(TIMEOUT_500US);
+	while (!timeout_elapsed(timeout_ref))
+		if (io_read32(ddrctrl_base + DDRCTRL_SWSTAT) &
+		    DDRCTRL_SWSTAT_SW_DONE_ACK)
+			return;
+
+	panic();
+}
+
+static int ddr_sw_self_refresh_in(void)
+{
+	uint64_t timeout_ref = 0;
+	uint32_t operating_mode = 0;
+	uint32_t selref_type = 0;
+	uint8_t op_mode_changed = 0;
+	vaddr_t pwr_base = stm32_pwr_base();
+	vaddr_t rcc_base = stm32_rcc_base();
+	vaddr_t ddrctrl_base = get_ddrctrl_base();
+	vaddr_t ddrphy_base = get_ddrphy_base();
+
+	io_clrbits32(rcc_base + RCC_DDRITFCR, RCC_DDRITFCR_AXIDCGEN);
+
+	/* Blocks AXI ports from taking anymore transactions */
+	io_clrbits32(ddrctrl_base + DDRCTRL_PCTRL_0, DDRCTRL_PCTRL_N_PORT_EN);
+	io_clrbits32(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
+	 */
+	timeout_ref = timeout_init_us(TIMEOUT_500US);
+	while (io_read32(ddrctrl_base + DDRCTRL_PSTAT))
+		if (timeout_elapsed(timeout_ref))
+			goto pstat_failed;
+
+	/* SW Self-Refresh entry */
+	io_setbits32(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.
+	 */
+	timeout_ref = timeout_init_us(TIMEOUT_500US);
+	while (!timeout_elapsed(timeout_ref)) {
+		uint32_t stat = io_read32(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) */
+	io_setbits32(ddrphy_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACPDD);
+	io_setbits32(ddrphy_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACPDR);
+	io_clrsetbits32(ddrphy_base + DDRPHYC_ACIOCR,
+			DDRPHYC_ACIOCR_CKPDD_MASK, DDRPHYC_ACIOCR_CKPDD_0);
+	io_clrsetbits32(ddrphy_base + DDRPHYC_ACIOCR,
+			DDRPHYC_ACIOCR_CKPDR_MASK, DDRPHYC_ACIOCR_CKPDR_0);
+	io_clrsetbits32(ddrphy_base + DDRPHYC_ACIOCR,
+			DDRPHYC_ACIOCR_CSPDD_MASK, DDRPHYC_ACIOCR_CSPDD_0);
+	io_clrbits32(ddrphy_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACOE);
+	io_setbits32(ddrphy_base + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDD);
+	io_setbits32(ddrphy_base + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDR);
+	io_clrsetbits32(ddrphy_base + DDRPHYC_DSGCR,
+			DDRPHYC_DSGCR_ODTPDD_MASK, DDRPHYC_DSGCR_ODTPDD_0);
+	io_setbits32(ddrphy_base + DDRPHYC_DSGCR, DDRPHYC_DSGCR_NL2PD);
+	io_clrsetbits32(ddrphy_base + DDRPHYC_DSGCR,
+			DDRPHYC_DSGCR_CKEPDD_MASK, DDRPHYC_DSGCR_CKEPDD_0);
+
+	/* Disable PZQ cell (PUBL register) */
+	io_setbits32(ddrphy_base + DDRPHYC_ZQ0CR0, DDRPHYC_ZQ0CRN_ZQPD);
+
+	/* Activate sw retention in PWRCTRL */
+	io_setbits32(pwr_base + PWR_CR3_OFF, PWR_CR3_DDRRETEN);
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL ref clock */
+	io_setbits32(rcc_base + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+
+	/* Disable all DLLs: GLITCH window */
+	io_setbits32(ddrphy_base + DDRPHYC_ACDLLCR, DDRPHYC_ACDLLCR_DLLDIS);
+	io_setbits32(ddrphy_base + DDRPHYC_DX0DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+	io_setbits32(ddrphy_base + DDRPHYC_DX1DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+	io_setbits32(ddrphy_base + DDRPHYC_DX2DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+	io_setbits32(ddrphy_base + DDRPHYC_DX3DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL output clock */
+	io_clrbits32(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 */
+	io_clrbits32(ddrctrl_base + DDRCTRL_PWRCTL, DDRCTRL_PWRCTL_SELFREF_SW);
+
+pstat_failed:
+	io_setbits32(ddrctrl_base + DDRCTRL_PCTRL_0, DDRCTRL_PCTRL_N_PORT_EN);
+	io_setbits32(ddrctrl_base + DDRCTRL_PCTRL_1, DDRCTRL_PCTRL_N_PORT_EN);
+
+	return -1;
+}
+
+static int ddr_sw_self_refresh_exit(void)
+{
+	uint64_t timeout_ref = 0;
+	vaddr_t rcc_base = stm32_rcc_base();
+	vaddr_t pwr_base = stm32_pwr_base();
+	vaddr_t ddrctrl_base = get_ddrctrl_base();
+	vaddr_t ddrphy_base = get_ddrphy_base();
+
+	/* Enable all clocks */
+	ddr_enable_clock();
+
+	do_sw_handshake();
+
+	/* Mask dfi_init_complete_en */
+	io_clrbits32(ddrctrl_base + DDRCTRL_DFIMISC,
+		     DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+
+	do_sw_ack();
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL ref clock */
+	io_setbits32(rcc_base + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+
+	/* Enable all DLLs: GLITCH window */
+	io_clrbits32(ddrphy_base + DDRPHYC_ACDLLCR, DDRPHYC_ACDLLCR_DLLDIS);
+	io_clrbits32(ddrphy_base + DDRPHYC_DX0DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+	io_clrbits32(ddrphy_base + DDRPHYC_DX1DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+	io_clrbits32(ddrphy_base + DDRPHYC_DX2DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+	io_clrbits32(ddrphy_base + DDRPHYC_DX3DLLCR, DDRPHYC_DXNDLLCR_DLLDIS);
+
+	/* Additional delay to avoid early DLL clock switch */
+	udelay(50);
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL ref clock */
+	io_clrbits32(rcc_base + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+	io_clrbits32(ddrphy_base + DDRPHYC_ACDLLCR, DDRPHYC_ACDLLCR_DLLSRST);
+	udelay(10);
+	io_setbits32(ddrphy_base + DDRPHYC_ACDLLCR, DDRPHYC_ACDLLCR_DLLSRST);
+
+	/* PHY partial init: (DLL lock and ITM reset) */
+	io_write32(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(10);
+
+	timeout_ref = timeout_init_us(TIMEOUT_500US);
+	while (!(io_read32(ddrphy_base + DDRPHYC_PGSR) & DDRPHYC_PGSR_IDONE))
+		if (timeout_elapsed(timeout_ref))
+			return -1;
+
+	do_sw_handshake();
+
+	/* Unmask dfi_init_complete_en to uMCTL2 */
+	io_setbits32(ddrctrl_base + DDRCTRL_DFIMISC,
+		     DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+
+	do_sw_ack();
+
+	/* Deactivate sw retention in PWR */
+	io_clrbits32(pwr_base + PWR_CR3_OFF, PWR_CR3_DDRRETEN);
+
+	/* Enable PZQ cell (PUBL register) */
+	io_clrbits32(ddrphy_base + DDRPHYC_ZQ0CR0, DDRPHYC_ZQ0CRN_ZQPD);
+
+	/* Enable pad drivers */
+	io_clrbits32(ddrphy_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACPDD);
+	io_setbits32(ddrphy_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACOE);
+	io_clrbits32(ddrphy_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_CKPDD_MASK);
+	io_clrbits32(ddrphy_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_CSPDD_MASK);
+	io_clrbits32(ddrphy_base + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDD);
+	io_clrbits32(ddrphy_base + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDR);
+	io_clrbits32(ddrphy_base + DDRPHYC_DSGCR, DDRPHYC_DSGCR_ODTPDD_MASK);
+	io_clrbits32(ddrphy_base + DDRPHYC_DSGCR, DDRPHYC_DSGCR_NL2PD);
+	io_clrbits32(ddrphy_base + DDRPHYC_DSGCR, DDRPHYC_DSGCR_CKEPDD_MASK);
+
+	/* Remove selfrefresh */
+	io_clrbits32(ddrctrl_base + DDRCTRL_PWRCTL, DDRCTRL_PWRCTL_SELFREF_SW);
+
+	/* Wait operating_mode == normal */
+	timeout_ref = timeout_init_us(TIMEOUT_500US);
+	while (1) {
+		if ((io_read32(ddrctrl_base + DDRCTRL_STAT) &
+		     DDRCTRL_STAT_OPERATING_MODE_MASK) ==
+	            DDRCTRL_STAT_OPERATING_MODE_NORMAL)
+			break;
+
+		if (timeout_elapsed(timeout_ref))
+			return -1;
+	}
+
+	/* AXI ports are no longer blocked from taking transactions */
+	io_setbits32(ddrctrl_base + DDRCTRL_PCTRL_0, DDRCTRL_PCTRL_N_PORT_EN);
+	io_setbits32(ddrctrl_base + DDRCTRL_PCTRL_1, DDRCTRL_PCTRL_N_PORT_EN);
+
+	io_setbits32(rcc_base + RCC_DDRITFCR, RCC_DDRITFCR_AXIDCGEN);
+
+	return 0;
+}
+
+uint32_t get_ddrphy_calibration(void)
+{
+	vaddr_t ddrphy_base = get_ddrphy_base();
+	uint32_t zcal = io_read32(ddrphy_base + DDRPHYC_ZQ0CR0);
+
+	return (zcal & DDRPHYC_ZQ0CRN_ZDATA_MASK) >> DDRPHYC_ZQ0CRN_ZDATA_SHIFT;
+}
+
+int ddr_standby_sr_entry(void)
+{
+	vaddr_t pwr_base = stm32_pwr_base();
+
+	if (ddr_sw_self_refresh_in())
+		return -1;
+
+	/* Enable I/O retention mode in standby */
+	io_setbits32(pwr_base + PWR_CR3_OFF, PWR_CR3_DDRSREN);
+
+	return 0;
+}
+
+int ddr_standby_sr_exit(void)
+{
+	return ddr_sw_self_refresh_exit();
+}
+
+static void ddr_sr_mode_ssr(void)
+{
+	vaddr_t rcc_ddritfcr = stm32_rcc_base() + RCC_DDRITFCR;
+	vaddr_t ddrctrl_base = get_ddrctrl_base();
+
+	io_setbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRC1LPEN);
+	io_setbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRC2LPEN);
+	io_setbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRC1EN);
+	io_setbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRC2EN);
+	io_setbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRCAPBLPEN);
+	io_setbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRPHYCAPBLPEN);
+	io_setbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRCAPBEN);
+	io_setbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRPHYCAPBEN);
+	io_setbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRPHYCEN);
+	io_clrbits32(rcc_ddritfcr, RCC_DDRITFCR_AXIDCGEN);
+	io_clrbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRCKMOD_MASK);
+
+	/* Disable HW LP interface of uMCTL2 */
+	io_clrbits32(ddrctrl_base + DDRCTRL_HWLPCTL, DDRCTRL_HWLPCTL_HW_LP_EN);
+
+	/* Configure Automatic LP modes of uMCTL2 */
+	io_clrsetbits32(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).
+	 */
+	io_clrbits32(ddrctrl_base + DDRCTRL_PWRCTL,
+		     DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE);
+
+	/* Disable automatic Self-Refresh mode */
+	io_clrbits32(ddrctrl_base + DDRCTRL_PWRCTL,
+		     DDRCTRL_PWRCTL_SELFREF_EN);
+}
+
+static void ddr_sr_mode_asr(void)
+{
+	vaddr_t rcc_ddritfcr = stm32_rcc_base() + RCC_DDRITFCR;
+	vaddr_t ddrctrl_base = get_ddrctrl_base();
+
+	io_setbits32(rcc_ddritfcr, RCC_DDRITFCR_AXIDCGEN);
+	io_setbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRC1LPEN);
+	io_setbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRC2LPEN);
+	io_setbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRPHYCLPEN);
+	io_clrsetbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRCKMOD_MASK,
+			RCC_DDRITFCR_DDRCKMOD_ASR1);
+
+	/* Enable HW LP interface of uMCTL2 */
+	io_setbits32(ddrctrl_base + DDRCTRL_HWLPCTL, DDRCTRL_HWLPCTL_HW_LP_EN);
+
+	/* Configure Automatic LP modes of uMCTL2 */
+	io_clrsetbits32(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).
+	 */
+	io_setbits32(ddrctrl_base + DDRCTRL_PWRCTL,
+		     DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE);
+
+	/* Enable automatic Self-Refresh for ASR mode */
+	io_setbits32(ddrctrl_base + DDRCTRL_PWRCTL,
+		     DDRCTRL_PWRCTL_SELFREF_EN);
+}
+
+static void ddr_sr_mode_hsr(void)
+{
+	vaddr_t rcc_ddritfcr = stm32_rcc_base() + RCC_DDRITFCR;
+	vaddr_t ddrctrl_base = get_ddrctrl_base();
+
+	io_setbits32(rcc_ddritfcr, RCC_DDRITFCR_AXIDCGEN);
+	io_clrbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRC1LPEN);
+	io_clrbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRC2LPEN);
+	io_setbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRPHYCLPEN);
+	io_clrsetbits32(rcc_ddritfcr, RCC_DDRITFCR_DDRCKMOD_MASK,
+			   RCC_DDRITFCR_DDRCKMOD_HSR1);
+
+	/* Enable HW LP interface of uMCTL2 */
+	io_setbits32(ddrctrl_base + DDRCTRL_HWLPCTL, DDRCTRL_HWLPCTL_HW_LP_EN);
+
+	/* Configure Automatic LP modes of uMCTL2 */
+	io_clrsetbits32(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).
+	 */
+	io_setbits32(ddrctrl_base + DDRCTRL_PWRCTL,
+		     DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE);
+}
+
+static enum stm32mp1_ddr_sr_mode ddr_read_sr_mode(void)
+{
+	uint32_t pwrctl = io_read32(get_ddrctrl_base() + DDRCTRL_PWRCTL);
+	uint32_t mask = DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE |
+			DDRCTRL_PWRCTL_SELFREF_EN;
+
+	switch (pwrctl & mask) {
+	case 0U:
+		return DDR_SSR_MODE;
+
+	case DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE:
+		return DDR_HSR_MODE;
+
+	case DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE | DDRCTRL_PWRCTL_SELFREF_EN:
+		return DDR_ASR_MODE;
+
+	default:
+		return DDR_SR_MODE_INVALID;
+	}
+}
+
+static void ddr_set_sr_mode(enum stm32mp1_ddr_sr_mode mode)
+{
+	switch (mode) {
+	case DDR_SSR_MODE:
+		ddr_sr_mode_ssr();
+		break;
+
+	case DDR_HSR_MODE:
+		ddr_sr_mode_hsr();
+		break;
+
+	case DDR_ASR_MODE:
+		ddr_sr_mode_asr();
+		break;
+
+	default:
+		EMSG("Unknown Self Refresh mode\n");
+		panic();
+	}
+}
+
+void ddr_save_sr_mode(enum stm32mp1_ddr_sr_mode mode)
+{
+	/* Save current mode before setting new one */
+	saved_ddr_sr_mode = ddr_read_sr_mode();
+	ddr_set_sr_mode(mode);
+}
+
+void ddr_restore_sr_mode(void)
+{
+	ddr_set_sr_mode(saved_ddr_sr_mode);
+}
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 000000000..e2d809bcd
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_ddrc.h
@@ -0,0 +1,219 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2017-2020, 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_ACOE			BIT(1)
+#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
+
+/*  DDR Self Refresh (SR) modes */
+enum stm32mp1_ddr_sr_mode {
+	DDR_SR_MODE_INVALID = 0,
+	DDR_SSR_MODE,
+	DDR_HSR_MODE,
+	DDR_ASR_MODE,
+};
+
+void ddr_save_sr_mode(enum stm32mp1_ddr_sr_mode mode);
+void ddr_restore_sr_mode(void);
+
+/* Return 32bit calibration value used for DDRPHY */
+uint32_t get_ddrphy_calibration(void);
+
+/*
+ * Entry/exit DDR selfrefresh mode
+ * Return 0 on success and a non-null value on error
+ */
+int ddr_standby_sr_entry(void);
+int ddr_standby_sr_exit(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
index 8d18ff94b..a7d755a0c 100644
--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.c
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: BSD-3-Clause
 /*
- * Copyright (c) 2017-2020, STMicroelectronics
+ * Copyright (c) 2017-2021, STMicroelectronics
  */
 
 #include <drivers/stm32_i2c.h>
@@ -36,6 +36,8 @@ static uint32_t pmic_i2c_addr;
 
 /* CPU voltage supplier if found */
 static char cpu_supply_name[PMIC_REGU_SUPPLY_NAME_LEN];
+/* USB voltage supplier if found */
+static char *usb_supply_name;
 
 bool stm32mp_with_pmic(void)
 {
@@ -372,6 +374,49 @@ const char *stm32mp_pmic_get_cpu_supply_name(void)
 	return cpu_supply_name;
 }
 
+/* Return a libfdt compliant status value */
+static int save_usb_supply_name(void)
+{
+	void *fdt = NULL;
+	int node = 0;
+	int subnode = 0;
+	const fdt32_t *cuint = NULL;
+	const char *name = NULL;
+
+	fdt = get_embedded_dt();
+	if (!fdt)
+		panic();
+
+	node = fdt_node_offset_by_compatible(fdt, -1, "st,stm32mp1-usbphyc");
+	if (node < 0)
+		return -FDT_ERR_NOTFOUND;
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		cuint = fdt_getprop(fdt, subnode, "phy-supply", NULL);
+		if (cuint)
+			break;
+	}
+	if (!cuint)
+		return -FDT_ERR_NOTFOUND;
+
+	node = fdt_node_offset_by_phandle(fdt, fdt32_to_cpu(*cuint));
+	if (node < 0)
+		return -FDT_ERR_NOTFOUND;
+
+	name = fdt_get_name(fdt, node, NULL);
+	assert(name);
+	usb_supply_name = strdup(name);
+	if (!usb_supply_name)
+		panic();
+
+	return 0;
+}
+
+const char *stm32mp_pmic_get_usb_supply_name(void)
+{
+	return usb_supply_name;
+}
+
 /* Preallocate not that much regu references */
 static char *nsec_access_regu_name[PMIC_REGU_COUNT];
 
@@ -452,6 +497,9 @@ static void parse_regulator_fdt_nodes(void)
 
 	if (save_cpu_supply_name())
 		DMSG("No CPU supply provided");
+
+	if (save_usb_supply_name())
+		DMSG("No USB supply provided");
 }
 
 /*
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h
index 4625af125..f91654733 100644
--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pmic.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: BSD-3-Clause */
 /*
- * Copyright (c) 2017-2020, STMicroelectronics
+ * Copyright (c) 2017-2021, STMicroelectronics
  */
 
 #ifndef __STM32MP1_PMIC_H__
@@ -15,6 +15,7 @@ void stm32mp_get_pmic(void);
 void stm32mp_put_pmic(void);
 int stm32mp_dt_pmic_status(void);
 const char *stm32mp_pmic_get_cpu_supply_name(void);
+const char *stm32mp_pmic_get_usb_supply_name(void);
 #else
 static inline void stm32mp_pmic_apply_boot_on_config(void)
 {
@@ -43,6 +44,11 @@ static inline const char *stm32mp_pmic_get_cpu_supply_name(void)
 {
 	return NULL;
 }
+
+static inline const char *stm32mp_pmic_get_usb_supply_name(void)
+{
+	return NULL;
+}
 #endif
 
 #endif /*__STM32MP1_PMIC_H__*/
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h
index 9cedd9321..5783a5ca9 100644
--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_pwr.h
@@ -13,10 +13,41 @@
 #define PWR_CR2_OFF		0x08
 #define PWR_CR3_OFF		0x0c
 #define PWR_MPUCR_OFF		0x10
+#define PWR_MCUCR_OFF		0x14
 #define PWR_WKUPCR_OFF		0x20
 #define PWR_MPUWKUPENR_OFF	0x28
 
-#define PWR_OFFSET_MASK		0x3fUL
+#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_CR2_BRRDY		BIT(16)
+#define PWR_CR2_RRRDY		BIT(17)
+
+#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_MCUCR_PDDS		BIT(0)
+
+#define PWR_WKUPCR_MASK		(GENMASK_32(27, 16) | \
+				 GENMASK_32(13, 8) | GENMASK_32(5, 0))
+
+#define PWR_MPUWKUPENR_MASK	GENMASK_32(5, 0)
 
 enum pwr_regulator {
 	PWR_REG11 = 0,
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_syscfg.c b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_syscfg.c
index 37d2cb0dc..345a19807 100644
--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_syscfg.c
+++ b/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_syscfg.c
@@ -5,6 +5,7 @@
 
 #include <dt-bindings/clock/stm32mp1-clks.h>
 #include <initcall.h>
+#include <drivers/clk.h>
 #include <kernel/delay.h>
 #include <mm/core_memprot.h>
 #include <stm32_util.h>
@@ -17,6 +18,7 @@
  */
 #define SYSCFG_CMPCR				0x20U
 #define SYSCFG_CMPENSETR			0x24U
+#define SYSCFG_CMPENCLRR			0x28U
 
 /*
  * SYSCFG_CMPCR Register
@@ -47,8 +49,8 @@ void stm32mp_syscfg_enable_io_compensation(void)
 	vaddr_t syscfg_base = get_syscfg_base();
 	uint64_t timeout_ref = 0;
 
-	stm32_clock_enable(CK_CSI);
-	stm32_clock_enable(SYSCFG);
+	clk_enable(CK_CSI);
+	clk_enable(SYSCFG);
 
 	io_setbits32(syscfg_base + SYSCFG_CMPENSETR, SYSCFG_CMPENSETR_MPU_EN);
 
@@ -71,6 +73,9 @@ void stm32mp_syscfg_disable_io_compensation(void)
 	vaddr_t syscfg_base = get_syscfg_base();
 	uint32_t value = 0;
 
+	 /* No refcount balance needed on non-secure SYSCFG clock */
+	clk_enable(SYSCFG);
+
 	value = io_read32(syscfg_base + SYSCFG_CMPCR) >>
 		SYSCFG_CMPCR_ANSRC_SHIFT;
 
@@ -84,10 +89,9 @@ void stm32mp_syscfg_disable_io_compensation(void)
 
 	DMSG("SYSCFG.cmpcr = %#"PRIx32, io_read32(syscfg_base + SYSCFG_CMPCR));
 
-	io_clrbits32(syscfg_base + SYSCFG_CMPENSETR, SYSCFG_CMPENSETR_MPU_EN);
+	io_setbits32(syscfg_base + SYSCFG_CMPENCLRR, SYSCFG_CMPENSETR_MPU_EN);
 
-	stm32_clock_disable(SYSCFG);
-	stm32_clock_disable(CK_CSI);
+	clk_disable(CK_CSI);
 }
 
 static TEE_Result stm32mp1_iocomp(void)
diff --git a/core/arch/arm/plat-stm32mp1/drivers/sub.mk b/core/arch/arm/plat-stm32mp1/drivers/sub.mk
index 42b6893f0..0b814a410 100644
--- a/core/arch/arm/plat-stm32mp1/drivers/sub.mk
+++ b/core/arch/arm/plat-stm32mp1/drivers/sub.mk
@@ -1,4 +1,5 @@
-srcs-y 	+= stm32mp1_clk.c
+srcs-$(CFG_STM32_CLKCALIB) += stm32mp1_calib.c
+srcs-y += stm32mp1_ddrc.c
 srcs-$(CFG_STPMIC1) += stm32mp1_pmic.c
 srcs-y 	+= stm32mp1_pwr.c
 srcs-y 	+= stm32mp1_rcc.c
diff --git a/core/arch/arm/plat-stm32mp1/link.mk b/core/arch/arm/plat-stm32mp1/link.mk
index 01a9b8ed5..fa1465a45 100644
--- a/core/arch/arm/plat-stm32mp1/link.mk
+++ b/core/arch/arm/plat-stm32mp1/link.mk
@@ -1,5 +1,6 @@
 include core/arch/arm/kernel/link.mk
 
+ifeq ($(CFG_STM32MP15x_STM32IMAGE),y)
 # Create stm32 formatted images from the native binary images
 
 define stm32image_cmd
@@ -22,3 +23,4 @@ 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 $@ --bintype 0x22
+endif
diff --git a/core/arch/arm/plat-stm32mp1/main.c b/core/arch/arm/plat-stm32mp1/main.c
index 9dca331c7..bf2e394fb 100644
--- a/core/arch/arm/plat-stm32mp1/main.c
+++ b/core/arch/arm/plat-stm32mp1/main.c
@@ -1,23 +1,30 @@
 // SPDX-License-Identifier: BSD-2-Clause
 /*
- * Copyright (c) 2017-2018, STMicroelectronics
  * Copyright (c) 2016-2018, Linaro Limited
+ * Copyright (c) 2017-2021, STMicroelectronics
  */
 
 #include <boot_api.h>
 #include <config.h>
 #include <console.h>
+#include <drivers/clk.h>
 #include <drivers/gic.h>
 #include <drivers/stm32_etzpc.h>
-#include <drivers/stm32mp1_etzpc.h>
+#include <drivers/stm32_iwdg.h>
 #include <drivers/stm32_uart.h>
+#include <drivers/stm32mp1_pmic.h>
+#include <drivers/stm32mp1_rcc.h>
+#include <drivers/stpmic1.h>
 #include <dt-bindings/clock/stm32mp1-clks.h>
+#include <dt-bindings/soc/st,stm32-etzpc.h>
+#include <io.h>
 #include <kernel/boot.h>
 #include <kernel/dt.h>
 #include <kernel/interrupt.h>
 #include <kernel/misc.h>
 #include <kernel/panic.h>
 #include <kernel/spinlock.h>
+#include <kernel/tee_misc.h>
 #include <mm/core_memprot.h>
 #include <platform_config.h>
 #include <sm/psci.h>
@@ -29,7 +36,10 @@ register_phys_mem_pgdir(MEM_AREA_IO_NSEC, GPIOS_NSEC_BASE, GPIOS_NSEC_SIZE);
 #endif
 register_phys_mem_pgdir(MEM_AREA_IO_NSEC, I2C4_BASE, SMALL_PAGE_SIZE);
 register_phys_mem_pgdir(MEM_AREA_IO_NSEC, I2C6_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_NSEC, IWDG1_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_NSEC, IWDG2_BASE, SMALL_PAGE_SIZE);
 register_phys_mem_pgdir(MEM_AREA_IO_NSEC, RNG1_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_NSEC, RTC_BASE, SMALL_PAGE_SIZE);
 #ifdef CFG_WITH_NSEC_UARTS
 register_phys_mem_pgdir(MEM_AREA_IO_NSEC, USART1_BASE, SMALL_PAGE_SIZE);
 register_phys_mem_pgdir(MEM_AREA_IO_NSEC, USART2_BASE, SMALL_PAGE_SIZE);
@@ -41,17 +51,38 @@ register_phys_mem_pgdir(MEM_AREA_IO_NSEC, UART7_BASE, SMALL_PAGE_SIZE);
 register_phys_mem_pgdir(MEM_AREA_IO_NSEC, UART8_BASE, SMALL_PAGE_SIZE);
 #endif
 
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, BKPSRAM_BASE, SMALL_PAGE_SIZE);
 register_phys_mem_pgdir(MEM_AREA_IO_SEC, BSEC_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, DBGMCU_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, DDRCTRL_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, DDRPHYC_BASE, SMALL_PAGE_SIZE);
 register_phys_mem_pgdir(MEM_AREA_IO_SEC, ETZPC_BASE, SMALL_PAGE_SIZE);
 register_phys_mem_pgdir(MEM_AREA_IO_SEC, GIC_BASE, GIC_SIZE);
 register_phys_mem_pgdir(MEM_AREA_IO_SEC, GPIOZ_BASE, SMALL_PAGE_SIZE);
 register_phys_mem_pgdir(MEM_AREA_IO_SEC, I2C4_BASE, SMALL_PAGE_SIZE);
 register_phys_mem_pgdir(MEM_AREA_IO_SEC, I2C6_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, IWDG1_BASE, SMALL_PAGE_SIZE);
 register_phys_mem_pgdir(MEM_AREA_IO_SEC, PWR_BASE, SMALL_PAGE_SIZE);
 register_phys_mem_pgdir(MEM_AREA_IO_SEC, RCC_BASE, SMALL_PAGE_SIZE);
 register_phys_mem_pgdir(MEM_AREA_IO_SEC, RNG1_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, RTC_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, STGEN_BASE, SMALL_PAGE_SIZE);
 register_phys_mem_pgdir(MEM_AREA_IO_SEC, SYSCFG_BASE, SMALL_PAGE_SIZE);
 register_phys_mem_pgdir(MEM_AREA_IO_SEC, TAMP_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, TIM1_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, TIM2_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, TIM3_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, TIM4_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, TIM5_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, TIM6_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, TIM7_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, TIM8_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, TIM12_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, TIM13_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, TIM14_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, TIM15_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, TIM16_BASE, SMALL_PAGE_SIZE);
+register_phys_mem_pgdir(MEM_AREA_IO_SEC, TIM17_BASE, SMALL_PAGE_SIZE);
 register_phys_mem_pgdir(MEM_AREA_IO_SEC, TZC_BASE, SMALL_PAGE_SIZE);
 register_phys_mem_pgdir(MEM_AREA_IO_SEC, USART1_BASE, SMALL_PAGE_SIZE);
 
@@ -66,6 +97,20 @@ register_dynamic_shm(DDR_BASE, CFG_TZDRAM_START - DDR_BASE);
 register_dynamic_shm(TZDRAM_END, DRAM_END - TZDRAM_END);
 #endif
 
+/* Map non-secure DDR bottom for the low power sequence */
+register_phys_mem(MEM_AREA_RAM_NSEC, DDR_BASE, SMALL_PAGE_SIZE);
+
+#ifdef CFG_RPROC_PTA
+/* Map MCU RETRAM as read write for Cortex-M4 firmware management */
+register_phys_mem(MEM_AREA_IO_SEC, RETRAM_BASE, RETRAM_SIZE);
+
+/* Map MCU SRAM as read write for Cortex-M4 firmware management */
+register_phys_mem(MEM_AREA_IO_SEC, MCUSRAM_BASE, MCUSRAM_SIZE);
+#endif
+
+/* Map TEE physical RAM as read-only for content storage when suspending */
+register_phys_mem(MEM_AREA_ROM_SEC, TEE_RAM_START, TEE_RAM_PH_SIZE);
+
 #define _ID2STR(id)		(#id)
 #define ID2STR(id)		_ID2STR(id)
 
@@ -168,8 +213,106 @@ static TEE_Result init_console_from_dt(void)
 
 /* Probe console from DT once clock inits (service init level) are completed */
 service_init_late(init_console_from_dt);
+
+static TEE_Result initialize_pll1_settings(void)
+{
+	uint32_t cpu_voltage = 0U;
+	int ret = 0;
+
+	if (stm32mp1_clk_pll1_settings_are_valid())
+		return TEE_SUCCESS;
+
+	if (stm32mp_dt_pmic_status() > 0) {
+		stm32mp_get_pmic();
+
+		ret = stpmic1_regulator_voltage_get(
+				stm32mp_pmic_get_cpu_supply_name());
+		if (ret < 0)
+			return ret;
+
+		cpu_voltage = (uint32_t)ret;
+
+		stm32mp_put_pmic();
+	}
+
+	if (stm32mp1_clk_compute_all_pll1_settings(cpu_voltage))
+		panic();
+
+	return TEE_SUCCESS;
+}
+
+/* Compute PLL1 settings once PMIC init is completed */
+driver_init_late(initialize_pll1_settings);
+
+static TEE_Result disable_usb_phy_regulator(void)
+{
+	if (stm32mp_dt_pmic_status() > 0) {
+		const char *name = stm32mp_pmic_get_usb_supply_name();
+
+		if (!name)
+			return TEE_SUCCESS;
+
+		stm32mp_get_pmic();
+		if (stpmic1_regulator_disable(name))
+			panic();
+
+		stm32mp_put_pmic();
+	}
+
+	return TEE_SUCCESS;
+}
+
+/* Disable USB regulator to avoid initialization issues */
+boot_final(disable_usb_phy_regulator);
 #endif
 
+static uintptr_t stm32_dbgmcu_base(void)
+{
+	static void *va;
+
+	if (!cpu_mmu_enabled())
+		return DBGMCU_BASE;
+
+	if (!va)
+		va = phys_to_virt(DBGMCU_BASE, MEM_AREA_IO_SEC);
+
+	return (uintptr_t)va;
+}
+
+/* SoC versioning util, returns default ID if can't access DBGMCU */
+TEE_Result stm32mp1_dbgmcu_get_chip_version(uint32_t *chip_version)
+{
+	uint32_t id = STM32MP1_CHIP_DEFAULT_VERSION;
+
+	if (!chip_version)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (stm32_bsec_read_debug_conf() & BSEC_DBGSWGEN)
+		id = (io_read32(stm32_dbgmcu_base() + DBGMCU_IDC) &
+		      DBGMCU_IDC_REV_ID_MASK) >> DBGMCU_IDC_REV_ID_SHIFT;
+
+	*chip_version = id;
+
+	return TEE_SUCCESS;
+}
+
+/* SoC device ID util, returns default ID if can't access DBGMCU */
+TEE_Result stm32mp1_dbgmcu_get_chip_dev_id(uint32_t *chip_dev_id)
+{
+	uint32_t id = STM32MP1_CHIP_ID;
+
+	if (!chip_dev_id)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (stm32_bsec_read_debug_conf() & BSEC_DBGSWGEN)
+		id = io_read32(stm32_dbgmcu_base() + DBGMCU_IDC) &
+		     DBGMCU_IDC_DEV_ID_MASK;
+
+	*chip_dev_id = id;
+
+	return TEE_SUCCESS;
+}
+
 /*
  * GIC init, used also for primary/secondary boot core wake completion
  */
@@ -212,7 +355,11 @@ static TEE_Result init_stm32mp1_drivers(void)
 			     (SYSRAM_SEC_SIZE >= CFG_TZSRAM_SIZE)));
 
 	etzpc_configure_tzma(1, SYSRAM_SEC_SIZE >> SMALL_PAGE_SHIFT);
+#ifdef STM32MP1_USE_MPU0_RESET
+	/* BootROM needs unlocked for independent reset */
+#else
 	etzpc_lock_tzma(1);
+#endif
 
 	return TEE_SUCCESS;
 }
@@ -241,12 +388,20 @@ void stm32mp_get_bsec_static_cfg(struct stm32_bsec_static_cfg *cfg)
 
 bool stm32mp_is_closed_device(void)
 {
-	uint32_t otp = 0;
+	uint32_t otp_id = 0;
+	size_t bit_len = 0;
+	uint32_t otp_value = 0;
 	TEE_Result result = TEE_ERROR_GENERIC;
 
+	if (stm32_bsec_find_otp_in_nvmem_layout(CFG0_OTP, &otp_id, &bit_len))
+		panic();
+
+	if (bit_len != 8)
+		panic();
+
 	/* Non closed_device platform expects fuse well programmed to 0 */
-	result = stm32_bsec_shadow_read_otp(&otp, DATA0_OTP);
-	if (!result && !(otp & BIT(DATA0_OTP_SECURED_POS)))
+	result = stm32_bsec_shadow_read_otp(&otp_value, otp_id);
+	if (!result && !(otp_value & BIT(CFG0_OTP_SECURED_POS)))
 		return false;
 
 	return true;
@@ -290,6 +445,20 @@ vaddr_t stm32mp_bkpreg(unsigned int idx)
 	return bkpreg_base() + (idx * sizeof(uint32_t));
 }
 
+vaddr_t stm32mp_bkpsram_base(void)
+{
+	struct io_pa_va base = { .pa = BKPSRAM_BASE };
+
+	return io_pa_or_va(&base);
+}
+
+vaddr_t stm32mp_stgen_base(void)
+{
+	struct io_pa_va base = { .pa = STGEN_BASE };
+
+	return io_pa_or_va(&base);
+}
+
 vaddr_t stm32_get_gpio_bank_base(unsigned int bank)
 {
 	static struct io_pa_va gpios_nsec_base = { .pa = GPIOS_NSEC_BASE };
@@ -322,3 +491,178 @@ unsigned int stm32_get_gpio_bank_clock(unsigned int bank)
 	assert(bank <= GPIO_BANK_K);
 	return GPIOA + bank;
 }
+
+static int get_part_number(uint32_t *part_nb)
+{
+	uint32_t part_number = 0;
+	uint32_t dev_id = 0;
+	uint32_t otp = 0;
+	size_t bit_len = 0;
+
+	assert(part_nb);
+
+	if (stm32mp1_dbgmcu_get_chip_dev_id(&dev_id))
+		return -1;
+
+	if (stm32_bsec_find_otp_in_nvmem_layout(PART_NUMBER_OTP,
+						&otp, &bit_len))
+		return -1;
+
+	if (bit_len != 8)
+		panic();
+
+	if (stm32_bsec_read_otp(&part_number, otp))
+		return -1;
+
+	part_number = (part_number & PART_NUMBER_OTP_PART_MASK) >>
+		      PART_NUMBER_OTP_PART_SHIFT;
+
+	*part_nb = part_number | (dev_id << 16);
+
+	return 0;
+}
+
+bool stm32mp_supports_cpu_opp(uint32_t opp_id)
+{
+	uint32_t part_number = 0;
+	uint32_t id = 0;
+
+	if (get_part_number(&part_number)) {
+		DMSG("Cannot get part number\n");
+		panic();
+	}
+
+	switch (opp_id) {
+	case PLAT_OPP_ID1:
+	case PLAT_OPP_ID2:
+		id = opp_id;
+		break;
+	default:
+		return false;
+	}
+
+	switch (part_number) {
+	case STM32MP157F_PART_NB:
+	case STM32MP157D_PART_NB:
+	case STM32MP153F_PART_NB:
+	case STM32MP153D_PART_NB:
+	case STM32MP151F_PART_NB:
+	case STM32MP151D_PART_NB:
+		return true;
+	default:
+		return id == PLAT_OPP_ID1;
+	}
+}
+
+enum etzpc_decprot_attributes stm32mp_etzpc_binding2decprot(uint32_t mode)
+{
+	switch (mode) {
+	case DECPROT_S_RW:
+		return ETZPC_DECPROT_S_RW;
+	case DECPROT_NS_R_S_W:
+		return ETZPC_DECPROT_NS_R_S_W;
+	case DECPROT_MCU_ISOLATION:
+		return ETZPC_DECPROT_MCU_ISOLATION;
+	case DECPROT_NS_RW:
+		return ETZPC_DECPROT_NS_RW;
+	default:
+		panic();
+	}
+}
+
+unsigned int stm32mp_iwdg_irq2instance(size_t irq)
+{
+	int id = irq - STM32MP1_IRQ_IWDG1;
+
+	assert(id >= IWDG1_INST && id <= IWDG2_INST);
+	return (unsigned int)id;
+}
+
+size_t stm32mp_iwdg_instance2irq(unsigned int instance)
+{
+	return instance + STM32MP1_IRQ_IWDG1;
+}
+
+unsigned int stm32mp_iwdg_iomem2instance(vaddr_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(vaddr_t pbase)
+{
+	unsigned int idx = 0;
+	unsigned long iwdg_cfg = 0;
+	uint32_t otp_id = 0;
+	size_t bit_len = 0;
+	uint32_t otp_value = 0;
+
+	idx = stm32mp_iwdg_iomem2instance(pbase);
+
+	if (stm32_bsec_find_otp_in_nvmem_layout(HW2_OTP, &otp_id, &bit_len))
+		panic();
+
+	if (bit_len != 32)
+		panic();
+
+	if (stm32_bsec_read_otp(&otp_value, otp_id))
+		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_DISABLE_ON_STOP;
+
+	if (otp_value & BIT(idx + HW2_OTP_IWDG_FZ_STANDBY_SHIFT))
+		iwdg_cfg |= IWDG_DISABLE_ON_STANDBY;
+
+	return iwdg_cfg;
+}
+
+#ifdef CFG_STM32_RTC
+/*
+ * Return true if RTC needs to be read twice not once for a reliable value.
+ *
+ * This function determines the number of needed RTC calendar read operations
+ * to get consistent values: RTC may need to be read twice depending on 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.
+ */
+bool stm32_rtc_get_read_twice(void)
+{
+	unsigned long apb1_freq = 0;
+	unsigned long rtc_freq = 0;
+	uint32_t apb1_div = 0;
+	vaddr_t rcc_base = stm32_rcc_base();
+
+	switch ((io_read32(rcc_base + RCC_BDCR) &
+		 RCC_BDCR_RTCSRC_MASK) >> RCC_BDCR_RTCSRC_SHIFT) {
+	case 1:
+		rtc_freq = clk_get_rate(CK_LSE);
+		break;
+	case 2:
+		rtc_freq = clk_get_rate(CK_LSI);
+		break;
+	case 3:
+		rtc_freq = clk_get_rate(CK_HSE);
+		rtc_freq /= (io_read32(rcc_base + RCC_RTCDIVR) &
+			     RCC_DIVR_DIV_MASK) + 1U;
+		break;
+	default:
+		panic();
+	}
+
+	apb1_div = io_read32(rcc_base + RCC_APB1DIVR) & RCC_APBXDIV_MASK;
+	apb1_freq = clk_get_rate(CK_MCU) >> apb1_div;
+
+	return apb1_freq < (rtc_freq * 7U);
+}
+#endif
diff --git a/core/arch/arm/plat-stm32mp1/nsec-service/low_power_svc.c b/core/arch/arm/plat-stm32mp1/nsec-service/low_power_svc.c
new file mode 100644
index 000000000..486977baa
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/nsec-service/low_power_svc.c
@@ -0,0 +1,152 @@
+// 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/boot.h>
+#include <kernel/delay.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_SVC_SR_MODE_SSR:
+		ddr_sr_mode_ssr();
+		break;
+	case STM32_SIP_SVC_SR_MODE_ASR:
+		ddr_sr_mode_asr();
+		break;
+	case STM32_SIP_SVC_SR_MODE_HSR:
+		ddr_sr_mode_hsr();
+		break;
+	default:
+		return STM32_SIP_SVC_INVALID_PARAMS;
+	}
+
+	return STM32_SIP_SVC_OK;
+}
+#else
+uint32_t sr_mode_scv_handler(uint32_t __unused x1, uint32_t __unused x2)
+{
+	return STM32_SIP_SVC_FAILED;
+}
+#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_SVC_OK : STM32_SIP_SVC_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_SVC_OK : STM32_SIP_SVC_FAILED;
+}
+
+uint32_t shutdown_scv_handler(void)
+{
+	DMSG("core %u", get_core_pos());
+
+	if (!stm32mp_with_pmic()) {
+		return STM32_SIP_SVC_FAILED;
+	}
+
+	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_SVC_INVALID_PARAMS;
+	}
+
+	stm32mp1_set_pm_domain_state(pd, enable);
+
+	return STM32_SIP_SVC_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_SVC_LP_FORCE_SUSPEND_PARAMS:
+		DMSG("Set suspend mode to %u", state);
+		power_mode = PSCI_MODE_SYSTEM_SUSPEND;
+		break;
+	case STM32_OEM_SVC_LP_FORCE_OFF_PARAMS:
+		DMSG("Set off mode to %u", state);
+		power_mode = PSCI_MODE_SYSTEM_OFF;
+		break;
+	default:
+		return STM32_OEM_SVC_INVALID_PARAMS;
+	}
+
+	if (stm32mp1_set_lp_deepest_soc_mode(power_mode, state) < 0) {
+		return STM32_OEM_SVC_FAILED;
+	}
+
+	return STM32_OEM_SVC_OK;
+}
+#else
+uint32_t pm_set_lp_state_scv_handler(uint32_t __unused mode,
+				     uint32_t __unused state)
+{
+	return STM32_SIP_SVC_FAILED;
+}
+#endif
diff --git a/core/arch/arm/plat-stm32mp1/nsec-service/low_power_svc.h b/core/arch/arm/plat-stm32mp1/nsec-service/low_power_svc.h
new file mode 100644
index 000000000..3d7d247d9
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/nsec-service/low_power_svc.h
@@ -0,0 +1,55 @@
+/* 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>
+
+#ifdef CFG_STM32_LOWPOWER_SIP
+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);
+#else
+uint32_t sr_mode_scv_handler(uint32_t x1 __unused, uint32_t x2 __unused)
+{
+	return STM32_SIP_SVC_FAILED;
+}
+
+uint32_t cstop_scv_handler(struct sm_ctx *nsec __unused, uint32_t x1 __unused,
+			   uint32_t x2 __unused, uint32_t x3 __unused)
+{
+	return STM32_SIP_SVC_FAILED;
+}
+
+uint32_t standby_scv_handler(struct sm_ctx *nsec __unused, uint32_t x1 __unused,
+			     uint32_t x2 __unused, uint32_t x3 __unused)
+{
+	return STM32_SIP_SVC_FAILED;
+}
+
+uint32_t shutdown_scv_handler(void)
+{
+	return STM32_SIP_SVC_FAILED;
+}
+
+uint32_t pm_domain_scv_handler(uint32_t x1 __unused, uint32_t x2 __unused)
+{
+	return STM32_SIP_SVC_FAILED;
+}
+
+uint32_t pm_set_lp_state_scv_handler(uint32_t x1 __unused, uint32_t x2 __unused)
+{
+	return STM32_SIP_SVC_FAILED;
+}
+#endif /* CFG_STM32_LOWPOWER_SIP */
+#endif /* LOW_POWER_SVC_H */
diff --git a/core/arch/arm/plat-stm32mp1/nsec-service/pwr_svc.c b/core/arch/arm/plat-stm32mp1/nsec-service/pwr_svc.c
new file mode 100644
index 000000000..6c78d8203
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/nsec-service/pwr_svc.c
@@ -0,0 +1,89 @@
+// 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 <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),
+};
+
+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;
+	vaddr_t va = 0;
+	uint32_t allowed = 0;
+	unsigned int i = 0;
+
+	/*
+	 * 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_SVC_INVALID_PARAMS;
+
+		offset &= PWR_OFFSET_MASK;
+	}
+
+	DMSG("PWR service: %s 0x%" PRIx32 " at offset 0x%" PRIx32,
+		req == STM32_SIP_SVC_REG_WRITE ? "write" :
+		req == STM32_SIP_SVC_REG_SET ? "set" : "clear",
+		value, offset);
+
+	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_SVC_REG_WRITE:
+			io_mask32_stm32shregs(va, value, allowed);
+			FMSG("wrt off %" PRIx32 "=%" PRIx32 " => %" PRIx32,
+				offset, value, io_read32(va));
+			return STM32_SIP_SVC_OK;
+		case STM32_SIP_SVC_REG_SET:
+			io_setbits32_stm32shregs(va, value);
+			FMSG("set off %" PRIx32 "=%" PRIx32 " => %" PRIx32,
+				offset, value, io_read32(va));
+			return STM32_SIP_SVC_OK;
+		case STM32_SIP_SVC_REG_CLEAR:
+			io_clrbits32_stm32shregs(va, value);
+			FMSG("clr off %" PRIx32 "=%" PRIx32 " => %" PRIx32,
+				offset, value, io_read32(va));
+			return STM32_SIP_SVC_OK;
+		default:
+			return STM32_SIP_SVC_INVALID_PARAMS;
+		}
+	}
+
+	return STM32_SIP_SVC_INVALID_PARAMS;
+}
diff --git a/core/arch/arm/plat-stm32mp1/nsec-service/pwr_svc.h b/core/arch/arm/plat-stm32mp1/nsec-service/pwr_svc.h
new file mode 100644
index 000000000..429d62071
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/nsec-service/pwr_svc.h
@@ -0,0 +1,22 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics
+ */
+
+#ifndef __PWR_SVC_H__
+#define __PWR_SVC_H__
+
+#include <stm32mp1_smc.h>
+
+#ifdef CFG_STM32_PWR_SIP
+uint32_t pwr_scv_handler(uint32_t x1, uint32_t x2, uint32_t x3);
+#else
+static inline uint32_t pwr_scv_handler(uint32_t x1 __unused,
+				       uint32_t x2 __unused,
+				       uint32_t x3 __unused)
+{
+	return STM32_SIP_SVC_FAILED;
+}
+#endif
+
+#endif /* __PWR_SVC_H__*/
diff --git a/core/arch/arm/plat-stm32mp1/nsec-service/rcc_svc.c b/core/arch/arm/plat-stm32mp1/nsec-service/rcc_svc.c
new file mode 100644
index 000000000..b4307b065
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/nsec-service/rcc_svc.c
@@ -0,0 +1,139 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2019, STMicroelectronics
+ */
+
+#include <drivers/stm32mp1_rcc.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <inttypes.h>
+#include <io.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <stm32_util.h>
+#include <trace.h>
+
+#include "rcc_svc.h"
+#include "stm32mp1_smc.h"
+
+static bool offset_is_clear_register(uint32_t __maybe_unused offset)
+{
+	/* All allowed registers are non set/clear registers  */
+	return false;
+}
+
+static void access_allowed_mask(uint32_t request, uint32_t offset,
+				uint32_t value, uint32_t allowed_mask)
+{
+	vaddr_t va = stm32_rcc_base() + offset;
+
+	if (!allowed_mask)
+		return;
+
+	switch (request) {
+	case STM32_SIP_SVC_REG_WRITE:
+		if (offset_is_clear_register(offset)) {
+			/* CLR registers show SET state, not CLR state */
+			io_write32(va, value & allowed_mask);
+		} else {
+			io_mask32_stm32shregs(va, value, allowed_mask);
+		}
+		FMSG("wrt 0x%" PRIx32 "=0x%" PRIx32 " => 0x%" PRIx32,
+			offset, value, io_read32(va));
+		break;
+
+	case STM32_SIP_SVC_REG_SET:
+		if (offset_is_clear_register(offset)) {
+			/* CLR registers show SET state, not CLR state */
+			io_write32(va, value & allowed_mask);
+		} else {
+			io_setbits32_stm32shregs(va, value & allowed_mask);
+		}
+		FMSG("set 0x%" PRIx32 "=0x%" PRIx32 " => 0x%" PRIx32,
+			offset, value, io_read32(va));
+		break;
+
+	case STM32_SIP_SVC_REG_CLEAR:
+		/* Nothing to do on CLR registers */
+		if (!offset_is_clear_register(offset))
+			io_clrbits32_stm32shregs(va, value & allowed_mask);
+		FMSG("clear 0x%" PRIx32 "=0x%" PRIx32 " => 0x%" PRIx32,
+			offset, value, io_read32(va));
+		break;
+
+	default:
+		break;
+	}
+}
+
+static uint32_t raw_allowed_access_request(uint32_t request,
+					   uint32_t offset, uint32_t value)
+{
+	uint32_t allowed_mask = 0;
+
+	switch (offset) {
+	case RCC_MP_CIER:
+	case RCC_MP_CIFR:
+		allowed_mask = RCC_MP_CIFR_WKUPF;
+		break;
+	case RCC_MP_GCR:
+		allowed_mask = RCC_MP_GCR_BOOT_MCU;
+		break;
+	default:
+		return STM32_SIP_SVC_INVALID_PARAMS;
+	}
+
+	access_allowed_mask(request, offset, value, allowed_mask);
+
+	return STM32_SIP_SVC_OK;
+}
+
+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) {
+		if ((offset & ~RCC_OFFSET_MASK) != RCC_BASE)
+			return STM32_SIP_SVC_INVALID_PARAMS;
+
+		offset &= RCC_OFFSET_MASK;
+	}
+
+	DMSG_RAW("RCC service: %s 0x%" PRIx32 " at offset 0x%" PRIx32,
+		 request == STM32_SIP_SVC_REG_WRITE ? "write" :
+		 request == STM32_SIP_SVC_REG_SET ? "set" : "clear",
+		 value, offset);
+
+	return raw_allowed_access_request(request, offset, value);
+}
+
+uint32_t rcc_opp_scv_handler(uint32_t x1, uint32_t x2, uint32_t *res)
+{
+	uint32_t cmd = x1;
+	uint32_t opp = x2 / 1000U; /* KHz */
+
+	switch (cmd) {
+	case STM32_SIP_SVC_RCC_OPP_SET:
+		if (stm32mp1_set_opp_khz(opp))
+			return STM32_SIP_SVC_FAILED;
+		break;
+
+	case STM32_SIP_SVC_RCC_OPP_ROUND:
+		if(stm32mp1_round_opp_khz(&opp))
+			return STM32_SIP_SVC_FAILED;
+
+		if (MUL_OVERFLOW(opp, 1000, res))
+			return STM32_SIP_SVC_FAILED;
+		break;
+
+	default:
+		return STM32_SIP_SVC_INVALID_PARAMS;
+	}
+
+	return STM32_SIP_SVC_OK;
+}
diff --git a/core/arch/arm/plat-stm32mp1/nsec-service/rcc_svc.h b/core/arch/arm/plat-stm32mp1/nsec-service/rcc_svc.h
new file mode 100644
index 000000000..873458f49
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/nsec-service/rcc_svc.h
@@ -0,0 +1,30 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (c) 2017-2019, STMicroelectronics
+ */
+
+#ifndef RCC_SVC_H
+#define RCC_SVC_H
+
+#include <stm32mp1_smc.h>
+
+#ifdef CFG_STM32_RCC_SIP
+uint32_t rcc_scv_handler(uint32_t x1, uint32_t x2, uint32_t x3);
+uint32_t rcc_opp_scv_handler(uint32_t x1, uint32_t x2, uint32_t *res);
+#else
+static inline uint32_t rcc_scv_handler(uint32_t x1 __unused,
+				       uint32_t x2 __unused,
+				       uint32_t x3 __unused)
+{
+	return STM32_SIP_SVC_FAILED;
+}
+static inline uint32_t rcc_opp_scv_handler(uint32_t x1 __unused,
+					   uint32_t x2 __unused,
+					   uint32_t *res __unused)
+
+{
+	return STM32_SIP_SVC_FAILED;
+}
+#endif
+
+#endif /*RCC_SVC_H*/
diff --git a/core/arch/arm/plat-stm32mp1/nsec-service/stm32mp1_smc.h b/core/arch/arm/plat-stm32mp1/nsec-service/stm32mp1_smc.h
index 3fbde31fe..a1f0e5859 100644
--- a/core/arch/arm/plat-stm32mp1/nsec-service/stm32mp1_smc.h
+++ b/core/arch/arm/plat-stm32mp1/nsec-service/stm32mp1_smc.h
@@ -14,7 +14,7 @@
 #define STM32_SIP_SVC_VERSION_MAJOR	0x0
 #define STM32_SIP_SVC_VERSION_MINOR	0x1
 
-#define STM32_SIP_SVC_FUNCTION_COUNT	0x3
+#define STM32_SIP_SVC_FUNCTION_COUNT	12
 
 /* STM32 SIP service generic return codes */
 #define STM32_SIP_SVC_OK		0x0
@@ -71,6 +71,51 @@
  */
 #define STM32_SIP_SVC_FUNC_VERSION		0xff03
 
+/*
+ * SIP function STM32_SIP_SVC_FUNC_RCC
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) status return code
+ * Argument a1: (input) Service ID (STM32_SIP_SVC_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_SVC_FUNC_RCC			0x1000
+
+/* Service ID for STM32_SIP_FUNC_RCC */
+#define STM32_SIP_SVC_REG_READ			0x0
+#define STM32_SIP_SVC_REG_WRITE			0x1
+#define STM32_SIP_SVC_REG_SET			0x2
+#define STM32_SIP_SVC_REG_CLEAR			0x3
+
+/*
+ * SIP functions STM32_SIP_SVC_FUNC_CAL
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) status return code
+ * Argument a1: (input) Clock ID (from DT clock bindings)
+ */
+#define STM32_SIP_SVC_FUNC_CAL			0x1002
+
+/*
+ * SIP functions 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_SVC_FUNC_PWR			0x1001
+
+/* Service ID for STM32_SIP_SVC_FUNC_RCC/_PWR */
+#define STM32_SIP_SVC_REG_READ			0x0
+#define STM32_SIP_SVC_REG_WRITE			0x1
+#define STM32_SIP_SVC_REG_SET			0x2
+#define STM32_SIP_SVC_REG_CLEAR			0x3
+
 /*
  * SIP functions STM32_SIP_SVC_FUNC_BSEC
  *
@@ -92,6 +137,91 @@
 /* reserved for STM32_SIP_SVC_SMC_WRITE_ALL	0x6 */
 #define STM32_SIP_SVC_BSEC_WRLOCK_OTP		0x7
 
+/*
+ * 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
+
+/*
+ * SIP function STM32_SIP_FUNC_RCC_OPP.
+ *
+ * Argument a0: (input) SMCC ID.
+ *		(output) Status return code.
+ * Argument a1: (input) Service ID (STM32_SIP_RCC_OPP_xxx).
+ *		(output) Rounded frequency, if applicable.
+ * Argument a2: (input) Requested frequency.
+ */
+#define STM32_SIP_SVC_FUNC_RCC_OPP			0x1009
+
+/* Service ID for STM32_SIP_FUNC_RCC_OPP */
+#define STM32_SIP_SVC_RCC_OPP_SET			0x0
+#define STM32_SIP_SVC_RCC_OPP_ROUND			0x1
+
 /*
  * SIP function STM32_SIP_SVC_FUNC_SCMI_AGENT0
  * SIP function STM32_SIP_SVC_FUNC_SCMI_AGENT1
@@ -103,4 +233,68 @@
 #define STM32_SIP_SVC_FUNC_SCMI_AGENT0		0x2000
 #define STM32_SIP_SVC_FUNC_SCMI_AGENT1		0x2001
 
+/*
+ * OEM Functions
+ */
+#define STM32_OEM_SVC_VERSION_MAJOR		0x0
+#define STM32_OEM_SVC_VERSION_MINOR		0x1
+
+#define STM32_OEM_SVC_FUNCTION_COUNT	1
+
+/* Use the same UID as for SiP service */
+#define STM32_OEM_SVC_UID_0			STM32_SIP_SVC_UID_0
+#define STM32_OEM_SVC_UID_1			STM32_SIP_SVC_UID_1
+#define STM32_OEM_SVC_UID_2			STM32_SIP_SVC_UID_2
+#define STM32_OEM_SVC_UID_3			STM32_SIP_SVC_UID_3
+
+
+/* OEM service generic return codes */
+#define STM32_OEM_SVC_OK			0x0
+#define STM32_OEM_SVC_NOT_SUPPORTED		0xffffffffU
+#define STM32_OEM_SVC_FAILED			0xfffffffeU
+#define STM32_OEM_SVC_INVALID_PARAMS		0xfffffffdU
+
+/*
+ * OEM function STM32_OEM_FUNC_CALL_COUNT
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) Count of defined function IDs
+ */
+#define STM32_OEM_SVC_FUNC_CALL_COUNT		0xff00
+
+/*
+ * OEM function STM32_OEM_SVC_FUNC_UID
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) Lowest 32bit of the stm32mp1 OEM service UUID
+ * Argument a1: (output) Next 32bit of the stm32mp1 OEM service UUID
+ * Argument a2: (output) Next 32bit of the stm32mp1 OEM service UUID
+ * Argument a3: (output) Last 32bit of the stm32mp1 OEM service UUID
+ */
+#define STM32_OEM_SVC_FUNC_UID			0xff01
+
+/*
+ * OEM function STM32_OEM_FUNC_VERSION
+ *
+ * Argument a0: (input) SMCC ID
+ *		(output) STM32 OEM service major
+ * Argument a1: (output) STM32 OEM service minor
+ */
+#define STM32_OEM_SVC_FUNC_VERSION		0xff03
+
+/*
+ * OEM function STM32_OEM_SVC_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_SVC_FUNC_LP_FORCE_PARAMS	0x0f800
+
+/* Valid IDs for power mode in STM32_OEM_SVC_FUNC_LP_FORCE_PARAMS */
+#define STM32_OEM_SVC_LP_FORCE_SUSPEND_PARAMS	0
+#define STM32_OEM_SVC_LP_FORCE_OFF_PARAMS	1
+
 #endif /* __STM32MP1_SMC_H__*/
diff --git a/core/arch/arm/plat-stm32mp1/nsec-service/stm32mp1_svc_setup.c b/core/arch/arm/plat-stm32mp1/nsec-service/stm32mp1_svc_setup.c
index 49d23ff02..f5f131df6 100644
--- a/core/arch/arm/plat-stm32mp1/nsec-service/stm32mp1_svc_setup.c
+++ b/core/arch/arm/plat-stm32mp1/nsec-service/stm32mp1_svc_setup.c
@@ -7,11 +7,32 @@
 #include <drivers/scmi-msg.h>
 #include <sm/optee_smc.h>
 #include <sm/sm.h>
+#include <stm32_util.h>
 
 #include "bsec_svc.h"
+#include "low_power_svc.h"
+#include "pwr_svc.h"
+#include "rcc_svc.h"
 #include "stm32mp1_smc.h"
 
-static enum sm_handler_ret sip_service(struct sm_ctx *ctx __unused,
+#ifdef CFG_STM32_CLKCALIB_SIP
+static uint32_t calib_scv_handler(uint32_t x1)
+{
+	unsigned long clock_id = x1;
+
+	if (stm32mp_start_clock_calib(clock_id))
+		return STM32_SIP_SVC_FAILED;
+
+	return STM32_SIP_SVC_OK;
+}
+#else
+static uint32_t calib_scv_handler(uint32_t __unused x1)
+{
+	return STM32_SIP_SVC_FAILED;
+}
+#endif
+
+static enum sm_handler_ret sip_service(struct sm_ctx *ctx,
 				       struct thread_smc_args *args)
 {
 	switch (OPTEE_SMC_FUNC_NUM(args->a0)) {
@@ -39,6 +60,60 @@ static enum sm_handler_ret sip_service(struct sm_ctx *ctx __unused,
 	case STM32_SIP_SVC_FUNC_BSEC:
 		bsec_main(args);
 		break;
+	case STM32_SIP_SVC_FUNC_RCC:
+		args->a0 = rcc_scv_handler(args->a1, args->a2, args->a3);
+		break;
+        case STM32_SIP_SVC_FUNC_RCC_OPP:
+                args->a0 = rcc_opp_scv_handler(args->a1, args->a2, &args->a1);
+                break;
+	case STM32_SIP_SVC_FUNC_CAL:
+		args->a0 = calib_scv_handler(args->a1);
+		break;
+	case STM32_SIP_SVC_FUNC_PWR:
+		args->a0 = pwr_scv_handler(args->a1, args->a2, args->a3);
+		break;
+	case STM32_SIP_FUNC_SR_MODE:
+		args->a0 = sr_mode_scv_handler(args->a1, args->a2);
+		break;
+	case STM32_SIP_FUNC_CSTOP:
+		args->a0 = cstop_scv_handler(ctx, args->a1, args->a2, args->a3);
+		break;
+	case STM32_SIP_FUNC_STANDBY:
+		args->a0 = standby_scv_handler(ctx, args->a1, args->a2, args->a3);
+		break;
+	case STM32_SIP_FUNC_SHUTDOWN:
+		args->a0 = shutdown_scv_handler();
+		break;
+	case STM32_SIP_FUNC_PD_DOMAIN:
+		args->a0 = pm_domain_scv_handler(args->a1, args->a2);
+		break;
+	default:
+		return SM_HANDLER_PENDING_SMC;
+	}
+
+	return SM_HANDLER_SMC_HANDLED;
+}
+
+static enum sm_handler_ret oem_service(struct sm_ctx *ctx __unused,
+				       struct thread_smc_args *args)
+{
+	switch (OPTEE_SMC_FUNC_NUM(args->a0)) {
+	case STM32_OEM_SVC_FUNC_CALL_COUNT:
+		args->a0 = STM32_OEM_SVC_FUNCTION_COUNT;
+		break;
+	case STM32_OEM_SVC_FUNC_VERSION:
+		args->a0 = STM32_OEM_SVC_VERSION_MAJOR;
+		args->a1 = STM32_OEM_SVC_VERSION_MINOR;
+		break;
+	case STM32_OEM_SVC_FUNC_UID:
+		args->a0 = STM32_OEM_SVC_UID_0;
+		args->a1 = STM32_OEM_SVC_UID_1;
+		args->a2 = STM32_OEM_SVC_UID_2;
+		args->a3 = STM32_OEM_SVC_UID_3;
+		break;
+	case STM32_OEM_SVC_FUNC_LP_FORCE_PARAMS:
+		args->a0 = pm_set_lp_state_scv_handler(args->a1, args->a2);
+		break;
 	default:
 		return SM_HANDLER_PENDING_SMC;
 	}
@@ -56,6 +131,8 @@ enum sm_handler_ret sm_platform_handler(struct sm_ctx *ctx)
 	switch (OPTEE_SMC_OWNER_NUM(args->a0)) {
 	case OPTEE_SMC_OWNER_SIP:
 		return sip_service(ctx, args);
+	case OPTEE_SMC_OWNER_OEM:
+		return oem_service(ctx, args);
 	default:
 		return SM_HANDLER_PENDING_SMC;
 	}
diff --git a/core/arch/arm/plat-stm32mp1/nsec-service/sub.mk b/core/arch/arm/plat-stm32mp1/nsec-service/sub.mk
index 6e1abcb4a..acccda16a 100644
--- a/core/arch/arm/plat-stm32mp1/nsec-service/sub.mk
+++ b/core/arch/arm/plat-stm32mp1/nsec-service/sub.mk
@@ -2,3 +2,6 @@ global-incdirs-y += .
 
 srcs-y += stm32mp1_svc_setup.c
 srcs-$(CFG_STM32_BSEC_SIP) += bsec_svc.c
+srcs-$(CFG_STM32_LOWPOWER_SIP) += low_power_svc.c
+srcs-$(CFG_STM32_PWR_SIP) += pwr_svc.c
+srcs-$(CFG_STM32_RCC_SIP) += rcc_svc.c
diff --git a/core/arch/arm/plat-stm32mp1/plat_tzc400.c b/core/arch/arm/plat-stm32mp1/plat_tzc400.c
index bce923711..23c7170e5 100644
--- a/core/arch/arm/plat-stm32mp1/plat_tzc400.c
+++ b/core/arch/arm/plat-stm32mp1/plat_tzc400.c
@@ -33,39 +33,45 @@ static struct itr_handler tzc_itr_handler = {
 };
 DECLARE_KEEP_PAGER(tzc_itr_handler);
 
-static bool tzc_region_is_non_secure(unsigned int i, vaddr_t base, size_t size)
+static bool tzc_find_region(vaddr_t base, size_t size, struct tzc_region_config *region_cfg)
+{
+	uint8_t i = 1;
+
+	while (true) {
+		if (tzc_get_region_config(i++, region_cfg) != TEE_SUCCESS)
+			return false;
+
+		if (region_cfg->base <= base && region_cfg->top >= (base + size - 1))
+			return true;
+	}
+}
+
+static bool tzc_region_is_non_secure(vaddr_t base, size_t size)
 {
 	struct tzc_region_config region_cfg = { };
 	uint32_t ns_cpu_mask = TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_A7_ID);
 	uint32_t filters_mask = GENMASK_32(1, 0);
 
-	if (tzc_get_region_config(i, &region_cfg))
-		panic();
-
-	return region_cfg.base == base && region_cfg.top == (base + size - 1) &&
-	       region_cfg.sec_attr == TZC_REGION_S_NONE &&
-	       (region_cfg.ns_device_access & ns_cpu_mask) == ns_cpu_mask &&
-	       region_cfg.filters == filters_mask;
+	return tzc_find_region(base, size, &region_cfg) && 
+		region_cfg.sec_attr == TZC_REGION_S_NONE &&
+		(region_cfg.ns_device_access & ns_cpu_mask) == ns_cpu_mask &&
+		region_cfg.filters == filters_mask;
 }
 
-static bool tzc_region_is_secure(unsigned int i, vaddr_t base, size_t size)
+static bool tzc_region_is_secure(vaddr_t base, size_t size)
 {
 	struct tzc_region_config region_cfg = { };
 	uint32_t filters_mask = GENMASK_32(1, 0);
 
-	if (tzc_get_region_config(i, &region_cfg))
-		panic();
-
-	return region_cfg.base == base && region_cfg.top == (base + size - 1) &&
-	       region_cfg.sec_attr == TZC_REGION_S_RDWR &&
-	       region_cfg.ns_device_access == 0 &&
-	       region_cfg.filters == filters_mask;
+	return tzc_find_region(base, size, &region_cfg) &&
+		region_cfg.sec_attr == TZC_REGION_S_RDWR &&
+		region_cfg.ns_device_access == 0 &&
+		region_cfg.filters == filters_mask;
 }
 
 static TEE_Result init_stm32mp1_tzc(void)
 {
 	void *base = phys_to_virt(TZC_BASE, MEM_AREA_IO_SEC);
-	unsigned int region_index = 1;
 	const uint64_t dram_start = DDR_BASE;
 	const uint64_t dram_end = dram_start + CFG_DRAM_SIZE;
 	const uint64_t tzdram_start = CFG_TZDRAM_START;
@@ -83,21 +89,15 @@ static TEE_Result init_stm32mp1_tzc(void)
 	 * expectations.
 	 */
 	if (dram_start < tzdram_start) {
-		if (!tzc_region_is_non_secure(region_index, dram_start,
-					      tzdram_start - dram_start))
+		if (!tzc_region_is_non_secure(dram_start, tzdram_start - dram_start))
 			panic("Unexpected TZC area on non-secure region");
-
-		region_index++;
 	}
 
-	if (!tzc_region_is_secure(region_index, tzdram_start, tzdram_size))
+	if (!tzc_region_is_secure(tzdram_start, tzdram_size))
 		panic("Unexpected TZC configuration on secure region");
 
 	if (tzdram_end < dram_end) {
-		region_index++;
-
-		if (!tzc_region_is_non_secure(region_index, tzdram_end,
-					      dram_end - tzdram_end))
+		if (!tzc_region_is_non_secure(tzdram_end, dram_end - tzdram_end))
 			panic("Unexpected TZC area on non-secure region");
 	}
 
diff --git a/core/arch/arm/plat-stm32mp1/platform_config.h b/core/arch/arm/plat-stm32mp1/platform_config.h
index cf68f4510..0ea5efc32 100644
--- a/core/arch/arm/plat-stm32mp1/platform_config.h
+++ b/core/arch/arm/plat-stm32mp1/platform_config.h
@@ -8,14 +8,47 @@
 
 #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
 
+#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 a 2MByte dynamic shared virtual memory (SHM_VASPACE)
+ */
+#define MAX_XLAT_TABLES			4
+#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 with this setup that has plenty of secure RAM */
+#define MAX_XLAT_TABLES			10
+#endif /*CFG_WITH_PAGER*/
+
 /* SoC interface registers base address */
+#define MCUSRAM_BASE			0x30000000ul
+#define RETRAM_BASE			0x38000000ul
+#define BKPSRAM_BASE			0x54000000
 #define BSEC_BASE			0x5c005000
 #define ETZPC_BASE			0x5c007000
 #define CRYP1_BASE			0x54001000
+#define DBGMCU_BASE			0x50081000
 #define DDR_BASE			0xc0000000ul
+#define DDRCTRL_BASE			0x5a003000
+#define DDRPHYC_BASE			0x5a004000
 #define GIC_BASE			0xa0021000ul
 #define GPIOA_BASE			0x50002000
 #define GPIOB_BASE			0x50003000
@@ -39,9 +72,24 @@
 #define RNG1_BASE			0x54003000
 #define RTC_BASE			0x5c004000
 #define SPI6_BASE			0x5c001000
+#define STGEN_BASE			0x5c008000
 #define SYSCFG_BASE			0x50020000
 #define SYSRAM_BASE			0x2ffc0000
 #define TAMP_BASE			0x5c00a000
+#define TIM1_BASE			0x44000000
+#define TIM2_BASE			0x40000000
+#define TIM3_BASE			0x40001000
+#define TIM4_BASE			0x40002000
+#define TIM5_BASE			0x40003000
+#define TIM6_BASE			0x40004000
+#define TIM7_BASE			0x40005000
+#define TIM8_BASE			0x44001000
+#define TIM12_BASE			0x40006000
+#define TIM13_BASE			0x40007000
+#define TIM14_BASE			0x40008000
+#define TIM15_BASE			0x44006000
+#define TIM16_BASE			0x44007000
+#define TIM17_BASE			0x44008000
 #define TZC_BASE			0x5c006000
 #define UART1_BASE			0x5c000000
 #define UART2_BASE			0x4000e000
@@ -52,6 +100,9 @@
 #define UART7_BASE			0x40018000
 #define UART8_BASE			0x40019000
 
+/* DDR expected size if not found in device tree */
+#define STM32MP1_DDR_SIZE_DFLT		(1 * 1024 * 1024 * 1024)
+
 /* Console configuration */
 #define STM32MP1_DEBUG_USART_BASE	UART4_BASE
 #define GIC_SPI_UART4			84
@@ -65,16 +116,21 @@
 
 #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 CFG0_OTP			"cfg0_otp"
+#define CFG0_OTP_SECURED_POS		6
+
+#define HW2_OTP				"hw2_otp"
+#define HW2_OTP_IWDG_HW_ENABLE_SHIFT	3
+#define HW2_OTP_IWDG_FZ_STOP_SHIFT	5
+#define HW2_OTP_IWDG_FZ_STANDBY_SHIFT	7
 
-#define DATA0_OTP_SECURED_POS		6
+#define PART_NUMBER_OTP			"part_number_otp"
+#define PART_NUMBER_OTP_PART_MASK	GENMASK_32(7, 0)
+#define PART_NUMBER_OTP_PART_SHIFT	0
+
+#define HW2_OTP_IWDG_HW_ENABLE_SHIFT		3
+#define HW2_OTP_IWDG_FZ_STOP_SHIFT		5
+#define HW2_OTP_IWDG_FZ_STANDBY_SHIFT		7
 
 /* GIC resources */
 #define GIC_SIZE			0x2000
@@ -84,11 +140,14 @@
 #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 STM32MP_GIC_PRIORITY_CSTOP	0xc0
+
 /*
  * 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.
@@ -116,6 +175,16 @@
 #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
+
+/* RCC platform resources */
+#define RCC_WAKEUP_IT			177
+
 /* TAMP resources */
 #define TAMP_BKP_REGISTER_OFF		0x100
 
@@ -140,6 +209,24 @@
 #define USART3_BASE			UART3_BASE
 #define USART6_BASE			UART6_BASE
 
+/* DBGMCU resources */
+#define DBGMCU_IDC			0x0
+#define DBGMCU_IDC_DEV_ID_MASK		GENMASK_32(11, 0)
+#define DBGMCU_IDC_REV_ID_MASK		GENMASK_32(31, 16)
+#define DBGMCU_IDC_REV_ID_SHIFT		16
+
+/* BKPSRAM layout */
+#define BKPSRAM_SIZE			0x1000
+#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		0xF00
+
 /* SYSRAM layout */
 #define SYSRAM_SIZE			0x40000
 #define SYSRAM_NS_SIZE			(SYSRAM_SIZE - SYSRAM_SEC_SIZE)
@@ -153,4 +240,38 @@
 #define SYSRAM_SEC_SIZE		SYSRAM_SIZE
 #endif
 
+/* RETRAM layout */
+#define RETRAM_SIZE			0x10000
+
+/* MCUSRAM layout */
+#define MCUSRAM_SIZE			0x60000
+
+/* SoC part numbers and revisions */
+#define STM32MP1_CHIP_ID		0x500
+
+#define STM32MP157C_PART_NB		0x05000000
+#define STM32MP157A_PART_NB		0x05000001
+#define STM32MP153C_PART_NB		0x05000024
+#define STM32MP153A_PART_NB		0x05000025
+#define STM32MP151C_PART_NB		0x0500002E
+#define STM32MP151A_PART_NB		0x0500002F
+#define STM32MP157F_PART_NB		0x05000080
+#define STM32MP157D_PART_NB		0x05000081
+#define STM32MP153F_PART_NB		0x050000A4
+#define STM32MP153D_PART_NB		0x050000A5
+#define STM32MP151F_PART_NB		0x050000AE
+#define STM32MP151D_PART_NB		0x050000AF
+
+#define STM32MP1_CHIP_DEFAULT_VERSION	0
+
+#define STM32MP1_REV_A			0x00001000
+#define STM32MP1_REV_B			0x00002000
+#define STM32MP1_REV_Z			0x00002001
+
+/* OPP */
+#define PLAT_OPP_ID1			1U
+#define PLAT_OPP_ID2			2U
+#define PLAT_MAX_OPP_NB			2U
+#define PLAT_MAX_PLLCFG_NB		6U
+
 #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 000000000..8dddb0c02
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/pm/context.c
@@ -0,0 +1,522 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2018-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ */
+
+#include <arm32.h>
+#include <boot_api.h>
+#include <drivers/clk.h>
+#include <drivers/gic.h>
+#include <drivers/stm32_rng.h>
+#include <drivers/stm32_rtc.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 <io.h>
+#include <kernel/cache_helpers.h>
+#include <kernel/delay.h>
+#include <keep.h>
+#include <kernel/panic.h>
+#include <kernel/pm.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
+
+/*
+ * STANDBY_CONTEXT_MAGIC0:
+ * Context provides magic, resume entry, zq0cr0 zdata and DDR training buffer.
+ *
+ * STANDBY_CONTEXT_MAGIC1:
+ * Context provides magic, resume entry, zq0cr0 zdata, DDR training buffer
+ * and PLL1 dual OPP settings structure (86 bytes).
+ */
+#define STANDBY_CONTEXT_MAGIC0		(0x0001 << 16)
+#define STANDBY_CONTEXT_MAGIC1		(0x0002 << 16)
+
+#if CFG_STM32MP15_PM_CONTEX_VERSION == 1
+#define STANDBY_CONTEXT_MAGIC	(STANDBY_CONTEXT_MAGIC0 | TRAINING_AREA_SIZE)
+#elif CFG_STM32MP15_PM_CONTEX_VERSION == 2
+#define STANDBY_CONTEXT_MAGIC	(STANDBY_CONTEXT_MAGIC1 | TRAINING_AREA_SIZE)
+#else
+#error Invalid value for CFG_STM32MP15_PM_CONTEX_VERSION
+#endif
+
+#if (PLAT_MAX_OPP_NB != 2) || (PLAT_MAX_PLLCFG_NB != 6)
+#error STANDBY_CONTEXT_MAGIC1 does not support expected PLL1 settings
+#endif
+
+/* pll_settings structure size definitions (reference to clock driver) */
+#define PLL1_SETTINGS_SIZE		(((PLAT_MAX_OPP_NB * \
+					  (PLAT_MAX_PLLCFG_NB + 3)) + 1) * \
+					 sizeof(uint32_t))
+
+/*
+ * 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];
+#if CFG_STM32MP15_PM_CONTEX_VERSION >= 2
+	uint8_t pll1_settings[PLL1_SETTINGS_SIZE];
+#endif
+};
+
+/*
+ * 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)
+{
+	vaddr_t bkpsram_base = stm32mp_bkpsram_base();
+	vaddr_t context_base = bkpsram_base + BKPSRAM_PM_CONTEXT_OFFSET;
+
+	return (struct retram_resume_ctx *)context_base;
+}
+
+static struct pm_mailbox *get_pm_mailbox(void)
+{
+	vaddr_t bkpsram_base = stm32mp_bkpsram_base();
+	vaddr_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();
+
+	clk_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));
+
+	clk_disable(RTCAPB);
+
+	clk_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);
+
+	clk_disable(BKPSRAM);
+}
+#else
+static void __maybe_unused dump_context(void)
+{
+}
+#endif
+
+/*
+ * Save and restore functions
+ */
+static void save_time(void)
+{
+        vaddr_t stgen = stm32mp_stgen_base();
+
+	plat_ctx.stgen_cnt_h = io_read32(stgen + CNTCVU_OFFSET);
+	plat_ctx.stgen_cnt_l = io_read32(stgen + CNTCVL_OFFSET);
+	if (plat_ctx.stgen_cnt_l < 10)
+		plat_ctx.stgen_cnt_h = io_read32(stgen + CNTCVU_OFFSET);
+
+	clk_enable(RTC);
+	stm32_rtc_get_calendar(&plat_ctx.rtc);
+}
+
+#if TRACE_LEVEL >= TRACE_DEBUG
+static void print_ccm_decryption_duration(void)
+{
+	vaddr_t stgen = stm32mp_stgen_base();
+	struct retram_resume_ctx *ctx = get_retram_resume_ctx();
+
+	clk_enable(BKPSRAM);
+
+	DMSG("CCM decryption duration %llums",
+		((unsigned long long)ctx->stgen_cnt * 1000) /
+		io_read32(stgen + CNTFID_OFFSET));
+
+	clk_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 = 0;
+	unsigned long long cnt = 0;
+	vaddr_t stgen = stm32mp_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 * io_read32(stgen + CNTFID_OFFSET)) / 1000U;
+
+	io_clrbits32(stgen + CNTCR_OFFSET, CNTCR_EN);
+	io_write32(stgen + CNTCVL_OFFSET, (uint32_t)cnt);
+	io_write32(stgen + CNTCVU_OFFSET, (uint32_t)(cnt >> 32));
+	io_setbits32(stgen + CNTCR_OFFSET, CNTCR_EN);
+
+	/* Balance clock enable(RTC) at save_time() */
+	clk_disable(RTC);
+
+	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 *)(vaddr_t)cb;
+	paddr_t cb_phy = virt_to_phys(cb_voidp);
+	paddr_t hdl_phy = virt_to_phys(hdl);
+	bool valid = false;
+
+	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;
+}
+
+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();
+
+	clk_enable(BKPSRAM);
+
+	memset(ctx, 0xa5, sizeof(*ctx));
+	memset(mailbox, 0xa5, sizeof(*mailbox));
+
+	clk_disable(BKPSRAM);
+}
+
+static struct mobj *teeram_bkp_mobj;
+
+static void init_retram_resume_resources(void)
+{
+	struct retram_resume_ctx *ctx = get_retram_resume_ctx();
+	size_t __maybe_unused csize = 0;
+	paddr_t __maybe_unused pa = 0;
+
+	COMPILE_TIME_ASSERT(sizeof(struct pm_mailbox) <
+			    BKPSRAM_PM_MAILBOX_SIZE);
+	COMPILE_TIME_ASSERT(sizeof(struct retram_resume_ctx) <
+			    BKPSRAM_PM_CONTEXT_SIZE);
+	csize = (vaddr_t)stm32mp_bkpsram_image_end -
+		(vaddr_t)stm32mp_bkpsram_resume;
+	assert((sizeof(*ctx) + csize) < 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)
+		panic();
+
+	assert((mobj_get_va(teeram_bkp_mobj, 0) != NULL) &&
+	       (mobj_get_pa(teeram_bkp_mobj, 0, 0, &pa) == 0));
+
+	clk_enable(BKPSRAM);
+	memset(ctx, 0, sizeof(*ctx));
+	clk_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);
+	paddr_t pa = DDR_BASE;
+	void *va = phys_to_virt(pa, MEM_AREA_RAM_NSEC);
+
+	memcpy(&mailbox->ddr_training_backup[0], va, size);
+
+}
+
+/*
+ * When returning from STANDBY, warm boot boot stage needs to access to PLL1
+ * settings. This avoids to re-compute them and optimizes performances. This
+ * structure must then be saved before going to STANDBY in the PM mailbox
+ * shared with the warm boot boot stage.
+ */
+#if CFG_STM32MP15_PM_CONTEX_VERSION >= 2
+static void save_pll1_settings(void)
+{
+	struct pm_mailbox *mailbox = get_pm_mailbox();
+	size_t size = sizeof(mailbox->pll1_settings);
+	uint8_t *data = &mailbox->pll1_settings[0];
+
+	stm32mp1_clk_lp_save_opp_pll1_settings(data, size);
+}
+#endif
+
+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(clk_is_enabled(BKPSRAM));
+
+	memset(mailbox, 0, sizeof(*mailbox));
+
+	mailbox->zq0cr0_zdata = get_ddrphy_calibration();
+
+	save_ddr_training_area();
+
+#if CFG_STM32MP15_PM_CONTEX_VERSION >= 2
+	save_pll1_settings();
+#endif
+}
+
+#if defined(CFG_STM32_RNG) && defined(CFG_STM32_CRYP)
+/*
+ * 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 = (vaddr_t)stm32mp_bkpsram_image_end -
+				     (vaddr_t)stm32mp_bkpsram_resume;
+	paddr_t pa = 0;
+	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(clk_is_enabled(BKPSRAM) &&
+	       clk_is_enabled(CRYP1));
+
+	memcpy(ctx->resume_sequence,
+	       (void *)(vaddr_t)stm32mp_bkpsram_resume, size);
+
+	memset(ctx, 0, sizeof(*ctx));
+	ctx->resume_pa = virt_to_phys((void *)(vaddr_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) /
+		io_read32(ctx->stgen_base + CNTFID_OFFSET));
+	ctx->stgen_cnt = 0;
+}
+#else
+static void save_teeram_in_ddr(void)
+{
+	panic("Mandates RNG and CRYP 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 = 0;
+	uint32_t hint = 0;
+
+	assert(clk_is_enabled(BKPSRAM) &&
+	       clk_is_enabled(RTCAPB));
+
+	if (suspend) {
+		magic = BOOT_API_A7_CORE0_MAGIC_NUMBER;
+		mailbox->magic = STANDBY_CONTEXT_MAGIC;
+
+		hint = virt_to_phys(&get_retram_resume_ctx()->resume_sequence);
+	} else {
+		mailbox->magic = 0;
+	}
+
+	io_write32(stm32mp_bkpreg(BCKR_CORE1_MAGIC_NUMBER), magic);
+	io_write32(stm32mp_bkpreg(BCKR_CORE1_BRANCH_ADDRESS), hint);
+
+	mailbox->core0_resume_ep = hint;
+}
+
+static void gate_pm_context_clocks(bool enable)
+{
+	static bool clocks_enabled;
+
+	if (enable) {
+		assert(!clocks_enabled);
+		clk_enable(BKPSRAM);
+		clk_enable(RTCAPB);
+		clk_enable(CRYP1);
+		clocks_enabled = true;
+		return;
+	}
+
+	/* Suspended TEE RAM state left the clocks enabled */
+	if (clocks_enabled) {
+		clk_disable(BKPSRAM);
+		clk_disable(RTCAPB);
+		clk_disable(CRYP1);
+		clocks_enabled = false;
+	}
+}
+
+/*
+ * Context (TEE RAM content + peripherals) must be restored
+ * only if system may reach STANDBY state.
+ */
+TEE_Result stm32mp_pm_save_context(unsigned int soc_mode)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	save_time();
+
+	if (!need_to_backup_cpu_context(soc_mode)) {
+		if (need_to_backup_stop_context(soc_mode))
+			stm32mp1_clk_save_context_for_stop();
+
+		return TEE_SUCCESS;
+	}
+
+	gate_pm_context_clocks(true);
+	load_earlyboot_pm_mailbox();
+	res = pm_change_state(PM_OP_SUSPEND, 0);
+	if (res)
+		return res;
+
+	save_teeram_in_ddr();
+	enable_pm_mailbox(1);
+
+	return TEE_SUCCESS;
+}
+
+void stm32mp_pm_restore_context(unsigned int soc_mode)
+{
+	if (need_to_backup_cpu_context(soc_mode)) {
+		if (pm_change_state(PM_OP_RESUME, 0))
+			panic();
+
+		gate_pm_context_clocks(false);
+	} else if (need_to_backup_stop_context(soc_mode)) {
+		stm32mp1_clk_restore_context_for_stop();
+	}
+
+	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 000000000..a806d685b
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/pm/context.h
@@ -0,0 +1,102 @@
+/* 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 __ASSEMBLER__
+#include <compiler.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <stm32mp_pm.h>
+#include <tee_api_types.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 __ASSEMBLER__
+/*
+ * 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);
+
+TEE_Result 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 /*__ASSEMBLER__*/
+
+#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 000000000..34c797f52
--- /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 000000000..f06d8273d
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/pm/low_power.c
@@ -0,0 +1,626 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
+ */
+
+#include <arm.h>
+#include <assert.h>
+#include <boot_api.h>
+#include <console.h>
+#include <drivers/gic.h>
+#include <drivers/stm32_etzpc.h>
+#include <drivers/stm32_iwdg.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 <dt-bindings/reset/stm32mp1-resets.h>
+#include <dt-bindings/soc/st,stm32-etzpc.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 <sm/psci.h>
+#include <stdbool.h>
+#include <stm32mp_pm.h>
+#include <stm32_util.h>
+#include <trace.h>
+
+#include "context.h"
+#include "power.h"
+
+#define TIMEOUT_US_1MS		1000
+
+#define PWRLP_TEMPO_5_HSI	5
+
+static uint8_t gicd_rcc_wakeup;
+static uint8_t gicc_pmr;
+
+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)
+{
+	const char *name = config_pwr[mode].regul_suspend_node_name;
+
+	assert(mode < ARRAY_SIZE(config_pwr));
+
+	if (stm32mp_with_pmic() && name) {
+		stm32mp_get_pmic();
+		stm32mp_pmic_apply_lp_config(name);
+		stm32mp_pmic_apply_boot_on_config();
+		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)
+{
+}
+
+#define ARM_CNTXCTL_IMASK	BIT(1)
+
+static void stm32mp_mask_timer(void)
+{
+	/* Mask timer interrupts */
+	write_cntp_ctl(read_cntp_ctl() | ARM_CNTXCTL_IMASK);
+	write_cntv_ctl(read_cntv_ctl() | ARM_CNTXCTL_IMASK);
+}
+
+/*
+ * stm32_enter_cstop - Prepare CSTOP mode
+ *
+ * @mode - Target low power mode
+ */
+void stm32_enter_cstop(uint32_t mode)
+{
+	uint32_t pwr_cr1 = config_pwr[mode].pwr_cr1;
+	vaddr_t pwr_base = stm32_pwr_base();
+	vaddr_t rcc_base = stm32_rcc_base();
+
+	stm32mp_syscfg_disable_io_compensation();
+
+	/* Save Self-Refresh (SR) mode and switch to Software SR mode */
+	ddr_save_sr_mode(DDR_SSR_MODE);
+
+	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 */
+	io_setbits32(rcc_base + RCC_MP_CIFR, RCC_MP_CIFR_WKUPF);
+
+	/* Enable RCC Wake-up */
+	io_setbits32(rcc_base + RCC_MP_CIER, RCC_MP_CIFR_WKUPF);
+
+	/* Configure low power mode */
+	io_clrsetbits32(pwr_base + PWR_MPUCR_OFF, PWR_MPUCR_MASK,
+			config_pwr[mode].pwr_mpucr);
+	io_clrsetbits32(pwr_base + PWR_CR1_OFF, PWR_CR1_MASK, pwr_cr1);
+
+	/* Clear RCC pending interrupt flags */
+	io_write32(rcc_base + RCC_MP_CIFR, RCC_MP_CIFR_MASK);
+
+	/* Request CSTOP mode to RCC */
+	io_setbits32(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);
+
+	if (ddr_standby_sr_entry() != 0)
+		panic();
+
+	if (mode == STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR) {
+		/* Keep retention and backup RAM content in standby */
+		io_setbits32(pwr_base + PWR_CR2_OFF, PWR_CR2_BREN |
+			     PWR_CR2_RREN);
+
+		while ((io_read32(pwr_base + PWR_CR2_OFF) &
+			(PWR_CR2_BRRDY | PWR_CR2_RRRDY)) == 0U)
+			;
+	}
+}
+
+/*
+ * stm32_exit_cstop - Exit from CSTOP mode
+ */
+void stm32_exit_cstop(void)
+{
+	vaddr_t rcc_base = stm32_rcc_base();
+
+	if (ddr_standby_sr_exit())
+		panic();
+
+	/* Restore Self-Refresh mode saved in stm32_enter_cstop() */
+	ddr_restore_sr_mode();
+
+	restore_rcc_it_priority(gicd_rcc_wakeup, gicc_pmr);
+
+	/* Disable STOP request */
+	io_setbits32(rcc_base + RCC_MP_SREQCLRR,
+			RCC_MP_SREQSETR_STPREQ_P0 | RCC_MP_SREQSETR_STPREQ_P1);
+
+	/* Disable RCC Wake-up */
+	io_clrbits32(rcc_base + RCC_MP_CIER, RCC_MP_CIFR_WKUPF);
+
+	dsb();
+	isb();
+
+	/* Disable retention and backup RAM content after stop */
+	io_clrbits32(stm32_pwr_base() + PWR_CR2_OFF, PWR_CR2_BREN | PWR_CR2_RREN);
+
+	stm32mp_syscfg_enable_io_compensation();
+}
+
+/*
+ * GIC support required in low power sequences and reset sequences
+ */
+#define GICC_IAR			0x00C
+#define GICC_IT_ID_MASK			0x3ff
+#define GICC_EOIR			0x010
+#define GICC_HPPIR			0x018
+#define GICC_AHPPIR			0x028
+#define GIC_PENDING_G1_INTID		1022U
+#define GIC_SPURIOUS_INTERRUPT		1023U
+#define GIC_NUM_INTS_PER_REG		32
+#define GIC_MAX_SPI_ID			1020
+#define GICD_ICENABLER(n)		(0x180 + (n) * 4)
+
+static void clear_pending_interrupts(void)
+{
+	uint32_t id = 0;
+	vaddr_t gicc_base = get_gicc_base();
+	vaddr_t gicd_base = get_gicd_base();
+
+	do {
+		id = io_read32(gicc_base + GICC_HPPIR) & GICC_IT_ID_MASK;
+
+		/*
+		 * Find out which interrupt it is under the
+		 * assumption that the GICC_CTLR.AckCtl bit is 0.
+		 */
+		if (id == GIC_PENDING_G1_INTID)
+			id = io_read32(gicc_base + GICC_AHPPIR) & GICC_IT_ID_MASK;
+
+		if (id < GIC_MAX_SPI_ID) {
+			size_t idx = id / GIC_NUM_INTS_PER_REG;
+			uint32_t mask = 1 << (id % GIC_NUM_INTS_PER_REG);
+
+			io_write32(gicc_base + GICC_EOIR, id);
+
+			io_write32(gicd_base + GICD_ICENABLER(idx), mask);
+
+			dsb_ishst();
+		}
+	} while (id < GIC_MAX_SPI_ID);
+}
+
+void stm32mp_gic_set_end_of_interrupt(uint32_t it)
+{
+	vaddr_t gicc_base = get_gicc_base();
+
+	io_write32(gicc_base + GICC_EOIR, it);
+}
+
+static void __noreturn wait_cpu_reset(void)
+{
+#ifdef STM32MP1_USE_MPU0_RESET
+	dcache_op_all(DCACHE_OP_CLEAN_INV);
+	write_sctlr(read_sctlr() & ~SCTLR_C);
+	dcache_op_all(DCACHE_OP_CLEAN_INV);
+	__asm__("clrex");
+
+	dsb();
+	isb();
+#else
+	psci_armv7_cpu_off();
+#endif
+
+	for ( ; ; ) {
+		clear_pending_interrupts();
+		wfi();
+	}
+}
+
+#ifdef STM32MP1_USE_MPU0_RESET
+/*
+ * tzc_source_ip contains the TZC transaction source IPs that need to be reset
+ * before a C-A7 subsystem is reset (i.e. independent reset):
+ * - C-A7 subsystem is reset separately later in the sequence,
+ * - C-M4 subsystem is not concerned here,
+ * - DAP is excluded for debug purpose,
+ * - IPs are stored with their ETZPC IDs (STM32MP1_ETZPC_MAX_ID if not
+ *   applicable) because some of them need to be reset only if they are not
+ *   configured in MCU isolation mode inside ETZPC device tree.
+ */
+struct tzc_source_ip {
+	uint16_t reset_id;
+	uint16_t clock_id;
+	uint32_t decprot_id;
+};
+
+#define _TZC_FIXED(res, clk)				\
+	{						\
+		.reset_id = (res),			\
+		.clock_id = (clk),			\
+		.decprot_id = STM32MP1_ETZPC_MAX_ID,	\
+	}
+
+#define _TZC_COND(res, clk, decprot)			\
+	{						\
+		.reset_id = (res),			\
+		.clock_id = (clk),			\
+		.decprot_id = (decprot),		\
+	}
+
+static const struct tzc_source_ip __maybe_unused tzc_source_ip[] = {
+	_TZC_FIXED(LTDC_R, LTDC_PX),
+	_TZC_FIXED(GPU_R, GPU),
+	_TZC_FIXED(USBH_R, USBH),
+	_TZC_FIXED(SDMMC1_R, SDMMC1_K),
+	_TZC_FIXED(SDMMC2_R, SDMMC2_K),
+	_TZC_FIXED(MDMA_R, MDMA),
+	_TZC_COND(USBO_R, USBO_K, STM32MP1_ETZPC_OTG_ID),
+	_TZC_COND(SDMMC3_R, SDMMC3_K, STM32MP1_ETZPC_SDMMC3_ID),
+	_TZC_COND(ETHMAC_R, ETHMAC, STM32MP1_ETZPC_ETH_ID),
+	_TZC_COND(DMA1_R, DMA1, STM32MP1_ETZPC_DMA1_ID),
+	_TZC_COND(DMA2_R, DMA2, STM32MP1_ETZPC_DMA2_ID),
+};
+
+static void reset_peripherals(void)
+{
+	vaddr_t rcc_base = stm32_rcc_base();
+	size_t __maybe_unused id = 0;
+
+	for (id = 0U; id < ARRAY_SIZE(tzc_source_ip); id++) {
+		const struct tzc_source_ip *tzc = &tzc_source_ip[id];
+
+		if (!stm32_clock_is_enabled(tzc->clock_id) ||
+		    ((tzc->decprot_id != STM32MP1_ETZPC_MAX_ID) &&
+		     (etzpc_get_decprot(tzc->decprot_id) ==
+		      ETZPC_DECPROT_MCU_ISOLATION)))
+			continue;
+
+		if (tzc->reset_id != GPU_R) {
+			stm32_reset_assert(tzc->reset_id, TIMEOUT_US_1MS);
+			stm32_reset_deassert(tzc->reset_id, TIMEOUT_US_1MS);
+		} else {
+			/* GPU reset automatically cleared by hardware */
+			io_setbits32(rcc_base + RCC_AHB6RSTSETR,
+				     RCC_AHB6RSTSETR_GPURST);
+		}
+	}
+}
+#endif /* STM32MP1_USE_MPU0_RESET */
+
+static void __noreturn reset_cores(void)
+{
+	vaddr_t rcc_base = stm32_rcc_base();
+	uint32_t reset_mask = RCC_MP_GRSTCSETR_MPUP0RST |
+			      RCC_MP_GRSTCSETR_MPUP1RST;
+	uint32_t target_mask = 0;
+
+	/* Mask timer interrupts */
+	stm32mp_mask_timer();
+
+#ifdef STM32MP1_USE_MPU0_RESET
+	reset_peripherals();
+#endif
+
+	if (get_core_pos() == 0)
+		target_mask = TARGET_CPU1_GIC_MASK;
+	else
+		target_mask = TARGET_CPU0_GIC_MASK;
+
+	itr_raise_sgi(GIC_SEC_SGI_1, target_mask);
+
+	clear_pending_interrupts();
+
+	io_write32(rcc_base + RCC_MP_GRSTCSETR, reset_mask);
+
+	wait_cpu_reset();
+}
+
+/*
+ * stm32_pm_cpus_reset - Reset only cpus
+ */
+void __noreturn stm32_cores_reset(void)
+{
+	reset_cores();
+}
+DECLARE_KEEP_PAGER(stm32_cores_reset);
+
+static __maybe_unused void reset_other_core(void)
+{
+	vaddr_t rcc_base = stm32_rcc_base();
+	uint32_t reset_mask = 0;
+	uint32_t target_mask = 0;
+
+	if (get_core_pos() == 0) {
+		reset_mask = RCC_MP_GRSTCSETR_MPUP1RST;
+		target_mask = TARGET_CPU1_GIC_MASK;
+	} else {
+		reset_mask = RCC_MP_GRSTCSETR_MPUP0RST;
+		target_mask = TARGET_CPU0_GIC_MASK;
+	}
+
+	itr_raise_sgi(GIC_SEC_SGI_1, target_mask);
+
+	io_write32(rcc_base + RCC_MP_GRSTCSETR, reset_mask);
+}
+
+/*
+ * 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();
+		reset_other_core();
+		stm32_enter_cstop(mode);
+		dsb();
+		isb();
+		for ( ; ; )
+			wfi();
+#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)
+{
+	vaddr_t rcc = stm32_rcc_base();
+
+	switch (mode) {
+	case STM32_PM_SHUTDOWN:
+		io_write32(rcc + RCC_MP_GRSTCSETR, RCC_MP_GRSTCSETR_MPSYSRST);
+		udelay(100);
+		break;
+	default:
+		IMSG("Forced system reset");
+		wait_console_flushed();
+		io_write32(rcc + RCC_MP_GRSTCSETR, RCC_MP_GRSTCSETR_MPSYSRST);
+		udelay(100);
+		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)
+{
+	vaddr_t pwr_base = stm32_pwr_base();
+
+	io_clrsetbits32(pwr_base + PWR_MPUCR_OFF, PWR_MPUCR_MASK,
+			config_pwr[STM32_PM_CSLEEP_RUN].pwr_mpucr);
+	io_clrsetbits32(pwr_base + PWR_CR1_OFF, PWR_CR1_MASK,
+			config_pwr[STM32_PM_CSLEEP_RUN].pwr_cr1);
+
+	stm32_pm_cpu_wfi();
+}
+
+/* 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,
+};
+DECLARE_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)
+{
+	stm32mp_mask_timer();
+
+	stm32mp_gic_set_end_of_interrupt(handler->it);
+
+	clear_pending_interrupts();
+
+	wait_cpu_reset();
+
+	panic("Core reset");
+
+	return ITRR_HANDLED;
+}
+
+static struct itr_handler sgi9_reset_handler = {
+	.it = GIC_SEC_SGI_1,
+	.handler = sgi9_it_handler,
+};
+DECLARE_KEEP_PAGER(sgi9_reset_handler);
+
+static TEE_Result init_low_power(void)
+{
+	vaddr_t pwr_base = stm32_pwr_base();
+	vaddr_t rcc_base = stm32_rcc_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);
+
+	/*
+	 * Configure Standby mode available for MCU by default
+	 * and allow to switch in standby SoC in all case
+	 */
+	io_setbits32(pwr_base + PWR_MCUCR_OFF, PWR_MCUCR_PDDS);
+
+	/* Disable STOP request */
+	io_setbits32(rcc_base + RCC_MP_SREQCLRR,
+		     RCC_MP_SREQSETR_STPREQ_P0 | RCC_MP_SREQSETR_STPREQ_P1);
+
+	/* Wait 5 HSI periods before re-enabling PLLs after STOP modes */
+	io_clrsetbits32(rcc_base + RCC_PWRLPDLYCR, RCC_PWRLPDLYCR_PWRLP_DLY_MASK,
+			PWRLP_TEMPO_5_HSI);
+
+	return TEE_SUCCESS;
+}
+service_init(init_low_power);
+
+/*
+ * CPU low power sequences
+ */
+void __noreturn stm32_pm_cpu_power_down_wfi(void)
+{
+	vaddr_t rcc_base = stm32_rcc_base();
+
+	if (get_core_pos() == 0) {
+		void (*reset_ep)(void) = stm32mp_sysram_resume;
+
+		stm32_pm_cpu_wfi();
+
+		/* STANDBY not reached: resume from retained SYSRAM */
+		stm32_exit_cstop();
+		stm32mp_cpu_reset_state();
+		reset_ep();
+		panic();
+	}
+
+	dcache_op_level1(DCACHE_OP_CLEAN);
+	io_write32(rcc_base + RCC_MP_GRSTCSETR, RCC_MP_GRSTCSETR_MPUP1RST);
+	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 000000000..63142a08e
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/pm/pm_helpers.S
@@ -0,0 +1,729 @@
+/* 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 <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)
+
+/*
+ * Enable TRACE_SYSRAM_RESTORE to get some UART console traces
+ * at resume time.
+ */
+#if defined(TRACE_SYSRAM_RESTORE)
+
+#define UART_BASE			UART4_BASE
+#define UART_ISR_OFF			0x1c
+#define UART_TDR_OFF			0x28
+#define USART_ISR_TXE_TXFNF		(1<< 7)
+
+	.macro PRINT_CHAR _reg0, _reg1, _char
+		/* Trace only at resume when MMU is OFF */
+		read_sctlr \_reg0
+		ands	\_reg0, #SCTLR_M
+	101:
+		bne	102f
+		mov_imm	\_reg0, UART4_BASE
+		ldr	\_reg1, [\_reg0, #UART_ISR_OFF]
+		ands	\_reg1, #USART_ISR_TXE_TXFNF
+		beq 101b
+		mov_imm	\_reg1, (\_char)
+		str	\_reg1, [\_reg0, #UART_TDR_OFF]
+	102:
+	.endm
+#else
+	.macro PRINT_CHAR _reg0, _reg1, _char
+	.endm
+#endif
+
+	/* 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(	.cantunwind)
+
+	PRINT_CHAR r0, r1, '0'
+
+	/*
+	 * 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
+
+	PRINT_CHAR r0, r1, 'T'
+	PRINT_CHAR r0, r1, 'a'
+	PRINT_CHAR r0, r1, 'g'
+	PRINT_CHAR r0, r1, '\n'
+
+	/* 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
+
+	PRINT_CHAR r1, r2, 'O'
+	PRINT_CHAR r1, r2, 'k'
+	PRINT_CHAR r1, r2, '\n'
+
+	/* Resume into the restored TEE RAM */
+	ldr	r1, [r11, #PM_CTX_RESUME_PA]
+	bx	r1
+
+_failed:
+	PRINT_CHAR r0, r12, 'F'
+	PRINT_CHAR r0, r12, '\n'
+
+	/* 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_on_timeout
+	bx	lr
+
+_ccm_fail_on_timeout:
+
+	ldr	r1, [r11, #PM_CTX_STGEN_BASE]
+	ldr	r1, [r1, #CNTCVL_OFFSET]
+	cmp	r1, r0
+	bge	_ccm_failed_on_timeout
+	bx	lr
+
+_ccm_failed_on_timeout:
+	PRINT_CHAR r0, r1, 'T'
+	PRINT_CHAR r0, r1, 'o'
+	PRINT_CHAR r0, r1, '\n'
+	b	_ccm_failed
+
+	/*
+	 * 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
+
+	PRINT_CHAR r0, r1, '1'
+
+	bl	_setup_cryp1
+
+	PRINT_CHAR r0, r1, '2'
+
+	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]
+
+	PRINT_CHAR r0, r1, '3'
+
+	/* 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
+
+	PRINT_CHAR r0, r1, '4'
+
+	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]
+
+	PRINT_CHAR r0, r1, '5'
+
+	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]
+
+	PRINT_CHAR r0, r1, '\n'
+
+_next_block:
+	PRINT_CHAR r0, r1, 'b'
+
+	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;
+
+	PRINT_CHAR r0, r1, '\n'
+	PRINT_CHAR r0, r1, '6'
+
+	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
+
+	PRINT_CHAR r0, r1, '7'
+
+	/* 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]
+
+	PRINT_CHAR r0, r1, '8'
+
+	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
+
+	PRINT_CHAR r0, r1, '9'
+
+	/* Successful return */
+	bl	_reset_cryp1
+	mov	r0, #0
+	bx	r12
+
+_ccm_failed:
+
+	PRINT_CHAR r0, r1, 'K'
+	PRINT_CHAR r0, r1, 'O'
+
+	bl	_reset_cryp1
+	mov	r0, #1
+	bx	r12
+
+/* End address of the PIC resume sequence copy in retained RAM */
+stm32mp_bkpsram_image_end:
+	nop
+
+END_FUNC stm32mp_bkpsram_resume
+
+/*
+ * int stm32mp_ccm_encrypt_teeram(ctx, dst, src, len)
+ */
+FUNC stm32mp_ccm_encrypt_teeram , :
+	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}
+END_FUNC stm32mp_ccm_encrypt_teeram
+
+/*
+ * int stm32mp_ccm_decrypt_teeram(ctx, cryp_base, dst, src)
+ */
+FUNC stm32mp_ccm_decrypt_teeram , :
+	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}
+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(	.cantunwind)
+	/* Invalidate the data cache */
+
+	read_clidr r2
+	ubfx	r3, r2, #CLIDR_LOC_SHIFT, #CLIDR_FIELD_WIDTH
+	lsl	r3, r3, #CSSELR_LEVEL_SHIFT
+	mov	r1, #0
+
+loop1:
+	add	r10, r1, r1, LSR #1	// Work out 3x current cache level
+	mov	r12, r2, LSR r10	// extract cache type bits from clidr
+	and	r12, r12, #7   		// mask the bits for current cache only
+	cmp	r12, #2			// see what cache we have at this level
+	blo	level_done      	// no cache or only instruction cache at this level
+
+	write_csselr r1			// select current cache level in csselr
+	isb				// isb to sych the new cssr&csidr
+	read_ccsidr r12			// read the new ccsidr
+	and	r10, r12, #7   		// extract the length of the cache lines
+	add	r10, r10, #4        	// add 4 (r10 = line length offset)
+	ubfx	r4, r12, #3, #10	// r4 = maximum way number (right aligned)
+	clz	r5, r4            	// r5 = the bit position of the way size increment
+	mov	r9, r4			// r9 working copy of the aligned max way number
+
+loop2:
+	ubfx	r7, r12, #13, #15	// r7 = max set number (right aligned)
+
+loop3:
+	orr	r0, r1, r9, LSL r5	// factor in the way number and cache level into r0
+	orr	r0, r0, r7, LSL r10	// factor in the set number
+
+	write_dcisw r0
+
+	subs	r7, r7, #1              // decrement the set number
+	bhs	loop3
+	subs	r9, r9, #1              // decrement the way number
+	bhs	loop2
+level_done:
+	add	r1, r1, #2		// increment the cache number
+	cmp	r3, r1
+	dsb	sy			// ensure completion of previous cache maintenance instruction
+	bhi	loop1
+
+	mov	r6, #0
+	write_csselr r6			//select cache level 0 in csselr
+	dsb	sy
+	isb
+
+	/* Resume sequence executes in Monitor mode */
+	cps	#CPSR_MODE_MON
+
+	blx	plat_cpu_reset_early
+	b	sm_pm_cpu_resume
+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, :
+	push	{r12, lr}
+UNWIND(	.save	{r12, lr})
+
+	cpsid	aif
+
+	bl	psci_armv7_cpu_off
+
+	write_bpiall
+	dsb
+	isb
+	read_sctlr r0
+	bic	r0, r0, #SCTLR_M
+	bic	r0, r0, #SCTLR_I
+	write_sctlr r0
+	dsb	sy
+	isb
+
+	pop	{r12, pc}
+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 000000000..4a5578c63
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/pm/power.h
@@ -0,0 +1,27 @@
+/* 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);
+bool need_to_backup_stop_context(unsigned int soc_mode);
+
+void stm32_enter_csleep(void);
+
+void 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 000000000..61e4f6800
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/pm/power_config.c
@@ -0,0 +1,252 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ */
+
+#include <assert.h>
+#include <config.h>
+#include <dt-bindings/power/stm32mp1-power.h>
+#include <kernel/boot.h>
+#include <kernel/panic.h>
+#include <libfdt.h>
+#include <stm32mp_pm.h>
+#include <trace.h>
+#include <util.h>
+
+#include "context.h"
+#include "power.h"
+
+#define DT_PWR_COMPAT			"st,stm32mp1,pwr-reg"
+#define SYSTEM_SUSPEND_SUPPORTED_MODES	"system_suspend_supported_soc_modes"
+#define SYSTEM_OFF_MODE			"system_off_soc_mode"
+
+static uint32_t deepest_suspend_mode;
+static uint32_t system_off_mode;
+static uint8_t stm32mp1_supported_soc_modes[STM32_PM_MAX_SOC_MODE];
+
+/* Boot 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();
+	}
+}
+
+bool need_to_backup_stop_context(unsigned int soc_mode)
+{
+	switch (soc_mode) {
+	case STM32_PM_CSTOP_ALLOW_STOP:
+	case STM32_PM_CSTOP_ALLOW_LP_STOP:
+	case STM32_PM_CSTOP_ALLOW_LPLV_STOP:
+		return true;
+	default:
+		return false;
+	}
+}
+
+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 int fdt_read_uint32_array(void *fdt, int node, const char *prop_name,
+				 uint32_t *array, uint32_t count)
+{
+	const fdt32_t *cuint = NULL;
+	int len = 0;
+	uint32_t i = 0;
+
+	cuint = fdt_getprop(fdt, node, prop_name, &len);
+	if (!cuint)
+		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;
+}
+
+static void save_supported_mode(void *fdt, int pwr_node)
+{
+	int len = 0;
+	uint32_t count = 0;
+	unsigned int i = 0;
+	uint32_t supported[ARRAY_SIZE(stm32mp1_supported_soc_modes)] = { };
+	const void *prop = 0;
+
+	prop = fdt_getprop(fdt, pwr_node, SYSTEM_SUSPEND_SUPPORTED_MODES, &len);
+	if (!prop)
+		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 = 0;
+
+	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 (!IS_ENABLED(CFG_STM32_CRYP) &&
+		    deepest_suspend_mode == STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR)
+			deepest_suspend_mode = STM32_PM_CSTOP_ALLOW_LPLV_STOP;
+	}
+
+	if (psci_mode == PSCI_MODE_SYSTEM_OFF)
+		system_off_mode = soc_mode;
+
+	return 0;
+}
+
+#ifdef CFG_DT
+static int dt_get_pwr_node(void *fdt)
+{
+	return fdt_node_offset_by_compatible(fdt, -1, DT_PWR_COMPAT);
+}
+
+static TEE_Result stm32mp1_init_lp_states(void)
+{
+	void *fdt = NULL;
+	int pwr_node = -1;
+	const fdt32_t *cuint = NULL;
+
+	fdt = get_embedded_dt();
+	if (fdt)
+		pwr_node = dt_get_pwr_node(fdt);
+
+	if (pwr_node >= 0)
+		cuint = fdt_getprop(fdt, pwr_node, SYSTEM_OFF_MODE, NULL);
+
+	if (!fdt || (pwr_node < 0) || !cuint) {
+		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 */
+	if (IS_ENABLED(CFG_STM32_CRYP))
+		deepest_suspend_mode = STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR;
+	else
+		deepest_suspend_mode = STM32_PM_CSTOP_ALLOW_LPLV_STOP;
+
+	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;
+}
+#else
+static TEE_Result stm32mp1_init_lp_states(void)
+{
+	if (IS_ENABLED(CFG_STM32_CRYP))
+		deepest_suspend_mode = STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR;
+	else
+		deepest_suspend_mode = STM32_PM_CSTOP_ALLOW_LPLV_STOP;
+
+	system_off_mode = STM32_PM_SHUTDOWN;
+
+	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;
+}
+#endif
+service_init(stm32mp1_init_lp_states);
diff --git a/core/arch/arm/plat-stm32mp1/pm/psci.c b/core/arch/arm/plat-stm32mp1/pm/psci.c
index 9c472c796..65f583d2e 100644
--- a/core/arch/arm/plat-stm32mp1/pm/psci.c
+++ b/core/arch/arm/plat-stm32mp1/pm/psci.c
@@ -1,16 +1,23 @@
 // SPDX-License-Identifier: BSD-2-Clause
 /*
- * Copyright (c) 2017-2018, STMicroelectronics
+ * Copyright (c) 2017-2020, STMicroelectronics
  */
 
 #include <arm.h>
 #include <boot_api.h>
+#include <config.h>
 #include <console.h>
+#include <drivers/clk.h>
+#include <drivers/stm32_etzpc.h>
+#include <drivers/stm32_rng.h>
 #include <drivers/stm32mp1_pmic.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/boot.h>
@@ -20,10 +27,17 @@
 #include <kernel/spinlock.h>
 #include <mm/core_memprot.h>
 #include <platform_config.h>
+#include <sm/pm.h>
 #include <sm/psci.h>
 #include <sm/std_smc.h>
 #include <stm32_util.h>
+#include <stm32mp_pm.h>
+#include <string.h>
 #include <trace.h>
+#include <util.h>
+
+#include "context.h"
+#include "power.h"
 
 #define CONSOLE_FLUSH_DELAY_MS		10
 
@@ -57,10 +71,9 @@ int psci_affinity_info(uint32_t affinity, uint32_t lowest_affinity_level)
 
 	DMSG("core %zu, state %u", pos, core_state[pos]);
 
-	if ((pos >= CFG_TEE_CORE_NB_CORE) ||
-	    (lowest_affinity_level > PSCI_AFFINITY_LEVEL_ON)) {
+	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:
@@ -82,38 +95,33 @@ int psci_affinity_info(uint32_t affinity, uint32_t lowest_affinity_level)
  */
 void stm32mp_register_online_cpu(void)
 {
-	assert(core_state[0] == CORE_OFF);
+	assert(core_state[0] == CORE_OFF || core_state[0] == CORE_RET);
 	core_state[0] = CORE_ON;
 }
 #else
-static void __noreturn stm32_pm_cpu_power_down_wfi(void)
-{
-	dcache_op_level1(DCACHE_OP_CLEAN);
-
-	io_write32(stm32_rcc_base() + RCC_MP_GRSTCSETR,
-		   RCC_MP_GRSTCSETR_MPUP1RST);
-
-	dsb();
-	isb();
-	wfi();
-	panic();
-}
-
 void stm32mp_register_online_cpu(void)
 {
 	size_t pos = get_core_pos();
 	uint32_t exceptions = lock_state_access();
 
 	if (pos == 0) {
-		assert(core_state[pos] == CORE_OFF);
+		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(exceptions);
-			stm32_pm_cpu_power_down_wfi();
+			if (IS_ENABLED(CFG_PM))
+				stm32_pm_cpu_power_down_wfi();
 			panic();
 		}
-		stm32_clock_disable(RTCAPB);
+
+		/* Clear hold in pen flag */
+		io_write32(stm32mp_bkpreg(BCKR_CORE1_MAGIC_NUMBER),
+			   BOOT_API_A7_RESET_MAGIC_NUMBER);
+
+		/* Balance BKPREG clock gating */
+		clk_disable(RTCAPB);
 	}
 
 	core_state[pos] = CORE_ON;
@@ -134,11 +142,16 @@ static void release_secondary_early_hpen(size_t __unused pos)
 	raise_sgi0_as_secure();
 	udelay(20);
 
+	clk_enable(RTCAPB);
+
 	io_write32(stm32mp_bkpreg(BCKR_CORE1_BRANCH_ADDRESS),
 		   TEE_LOAD_ADDR);
 	io_write32(stm32mp_bkpreg(BCKR_CORE1_MAGIC_NUMBER),
 		   BOOT_API_A7_CORE1_MAGIC_NUMBER);
 
+	dsb_ishst();
+	clk_disable(RTCAPB);
+
 	dsb_ishst();
 	itr_raise_sgi(GIC_SEC_SGI_0, TARGET_CPU1_GIC_MASK);
 }
@@ -207,44 +220,188 @@ int psci_cpu_off(void)
 	unlock_state_access(exceptions);
 
 	/* Enable BKPREG access for the disabled CPU */
-	stm32_clock_enable(RTCAPB);
+	clk_enable(RTCAPB);
 
 	thread_mask_exceptions(THREAD_EXCP_ALL);
-	stm32_pm_cpu_power_down_wfi();
+	if (IS_ENABLED(CFG_PM))
+		stm32_pm_cpu_power_down_wfi();
 	panic();
 }
 #endif
 
-/* Override default psci_system_off() with platform specific sequence */
-void __noreturn psci_system_off(void)
+#ifdef CFG_PM
+static int enter_cstop_suspend(unsigned int soc_mode)
 {
-	DMSG("core %u", get_core_pos());
+	int rc = 1;
+
+	if (read_isr())
+		return rc;
+
+	stm32_enter_cstop(soc_mode);
+
+	if (need_to_backup_cpu_context(soc_mode)) {
+		stm32_pm_cpu_power_down_wfi();
+	} else {
+		stm32_pm_cpu_wfi();
+		rc = 0;
+	}
+
+	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())
+		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;
+
+	if (stm32mp_pm_save_context(soc_mode) == TEE_SUCCESS)
+		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;
+
+	stm32mp_register_online_cpu();
+
+	assert(core_state[get_core_pos()] == CORE_ON);
+
+	stm32mp_pm_restore_context(soc_mode);
+}
 
-	if (TRACE_LEVEL >= TRACE_DEBUG) {
-		console_flush();
-		mdelay(CONSOLE_FLUSH_DELAY_MS);
+static bool plat_can_suspend(void)
+{
+	size_t pos = get_core_pos();
+	size_t n = 0;
+	uint32_t exceptions = 0;
+	bool rc = true;
+
+	if (!IS_ENABLED(CFG_STM32_RNG))
+		return false;
+
+	if (CFG_TEE_CORE_NB_CORE == 1)
+		return true;
+
+	exceptions = 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(exceptions);
+
+	return rc;
+}
+
+/* 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 = 0;
+	int __maybe_unused 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;
 	}
 
-	if (stm32mp_with_pmic()) {
-		stm32mp_get_pmic();
-		stpmic1_switch_off();
-		udelay(100);
+	assert(cpu_mmu_enabled() && core_state[pos] == CORE_ON);
+
+	if (need_to_backup_cpu_context(soc_mode)) {
+		if (!IS_ENABLED(CFG_STM32_CRYP))
+			return PSCI_RET_DENIED;
+
+		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);
 	}
 
-	panic();
+	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)
 {
-	vaddr_t rcc_base = stm32_rcc_base();
+	uint32_t soc_mode = stm32mp1_get_lp_soc_mode(PSCI_MODE_SYSTEM_OFF);
 
 	DMSG("core %u", get_core_pos());
 
-	io_write32(rcc_base + RCC_MP_GRSTCSETR, RCC_MP_GRSTCSETR_MPSYSRST);
-	udelay(100);
-	panic();
+	stm32_enter_cstop_reset(soc_mode);
 }
+#endif /*CFG_PM*/
+
 
 /* Override default psci_cpu_on() with platform supported features */
 int psci_features(uint32_t psci_fid)
@@ -261,6 +418,7 @@ int psci_features(uint32_t psci_fid)
 			return PSCI_RET_SUCCESS;
 		return PSCI_RET_NOT_SUPPORTED;
 	case PSCI_SYSTEM_OFF:
+	case PSCI_SYSTEM_SUSPEND:
 		if (stm32mp_with_pmic())
 			return PSCI_RET_SUCCESS;
 		return PSCI_RET_NOT_SUPPORTED;
diff --git a/core/arch/arm/plat-stm32mp1/pm/sub.mk b/core/arch/arm/plat-stm32mp1/pm/sub.mk
index c8ae8f915..d652958e3 100644
--- a/core/arch/arm/plat-stm32mp1/pm/sub.mk
+++ b/core/arch/arm/plat-stm32mp1/pm/sub.mk
@@ -1 +1,7 @@
+asm-defines-y += context_asm_defines.c
+
+srcs-$(CFG_PM) += context.c
+srcs-$(CFG_PM) += low_power.c
+srcs-$(CFG_PM) += pm_helpers.S
+srcs-$(CFG_PM) += power_config.c
 srcs-$(CFG_PSCI_ARM32) += psci.c
diff --git a/core/arch/arm/plat-stm32mp1/remoteproc_pta.c b/core/arch/arm/plat-stm32mp1/remoteproc_pta.c
new file mode 100644
index 000000000..27b0dfa07
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/remoteproc_pta.c
@@ -0,0 +1,528 @@
+ // SPDX-License-Identifier: BSD-2-Clause
+ /*
+  * Copyright (C) 2020, STMicroelectronics - All Rights Reserved
+  */
+
+#include <crypto/crypto.h>
+#include <drivers/clk.h>
+#include <drivers/stm32_etzpc.h>
+#include <drivers/stm32mp1_rcc.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <dt-bindings/reset/stm32mp1-resets.h>
+#include <dt-bindings/soc/st,stm32-etzpc.h>
+#include <initcall.h>
+#include <kernel/pseudo_ta.h>
+#include <kernel/user_ta.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <remoteproc_pta.h>
+#include <rproc_pub_key.h>
+#include <stm32_util.h>
+#include <string.h>
+
+#define PTA_NAME "remoteproc.pta"
+
+#define STM32_M4_FW_ID		0
+
+/* Firmware states */
+enum rproc_load_state {
+	REMOTEPROC_OFF = 0,
+	REMOTEPROC_ON,
+};
+
+/*
+ * struct rproc_ta_etzpc_rams - memory protection strategy table
+ * @pa - Memory physical base address from current CPU space
+ * @size - Memory region byte size
+ * @etzpc_id - associated ETZPC identifier.
+ * @attr - memory access permission according to @etzpc_decprot_attributes
+ */
+struct rproc_ta_etzpc_rams {
+	paddr_t pa;
+	size_t size;
+	uint32_t etzpc_id;
+	enum etzpc_decprot_attributes attr;
+};
+
+/*
+ * struct rproc_ta_memory_region - Represent a remote processor memory mapping
+ * @pa - Memory physical base address from current CPU space
+ * @da - Memory physical base address from remote processor space
+ * @size - Memory region byte size
+ */
+struct rproc_ta_memory_region {
+	paddr_t pa;
+	paddr_t da;
+	size_t size;
+};
+
+static const struct rproc_ta_etzpc_rams rproc_ta_mp1_m4_rams[] = {
+	/* MCU SRAM 1*/
+	{
+		.pa = MCUSRAM_BASE,
+		.size = 0x20000,
+		.etzpc_id = STM32MP1_ETZPC_SRAM1_ID,
+		.attr = ETZPC_DECPROT_MCU_ISOLATION,
+	},
+	/* MCU SRAM 2*/
+	{
+		.pa = MCUSRAM_BASE + 0x20000,
+		.size = 0x20000,
+		.etzpc_id = STM32MP1_ETZPC_SRAM2_ID,
+		.attr = ETZPC_DECPROT_MCU_ISOLATION,
+	},
+
+	/* MCU SRAM 3*/
+	{
+	/* Used as shared memory between the NS and the coprocessor */
+		.pa = MCUSRAM_BASE + 0x40000,
+		.size = 0x10000,
+		.etzpc_id = STM32MP1_ETZPC_SRAM3_ID,
+		.attr = ETZPC_DECPROT_NS_RW,
+	},
+	/* MCU SRAM 4*/
+	/* Not used reserved by NS for MDMA */
+	{
+		.pa = MCUSRAM_BASE + 0x50000,
+		.size = 0x10000,
+		.etzpc_id = STM32MP1_ETZPC_SRAM4_ID,
+		.attr = ETZPC_DECPROT_NS_RW,
+	},
+
+	/* MCU RETRAM */
+	{
+		.pa = RETRAM_BASE,
+		.size = RETRAM_SIZE,
+		.etzpc_id = STM32MP1_ETZPC_RETRAM_ID,
+		.attr = ETZPC_DECPROT_MCU_ISOLATION,
+	},
+};
+
+static const struct rproc_ta_memory_region rproc_ta_mp1_m4_mems[] = {
+	/* MCU SRAM */
+	{ .pa = MCUSRAM_BASE, .da = 0x10000000, .size = MCUSRAM_SIZE },
+	/* Alias of the MCU SRAM */
+	{ .pa = MCUSRAM_BASE, .da = 0x30000000, .size = MCUSRAM_SIZE },
+	/* RETRAM */
+	{ .pa = RETRAM_BASE, .da = 0x00000000, .size = RETRAM_SIZE },
+};
+
+static enum rproc_load_state rproc_ta_state;
+
+static TEE_Result rproc_pta_capabilities(uint32_t pt,
+					 TEE_Param params[TEE_NUM_PARAMS])
+{
+	const uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+						TEE_PARAM_TYPE_VALUE_OUTPUT,
+						TEE_PARAM_TYPE_VALUE_OUTPUT,
+						TEE_PARAM_TYPE_NONE);
+
+	if (pt != exp_pt)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Support only ELF format */
+	params[1].value.a = PTA_REMOTEPROC_ELF_FMT;
+
+	/*
+	 * Due to stm32mp1 pager, secure memory is too expensive. Support hash
+	 * protected image only, so that firmware image can be loaded from
+	 * non-secure memory.
+	 */
+	params[2].value.a = PTA_REMOTEPROC_FW_WITH_HASH_TABLE;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result da_to_pa(paddr_t da, size_t size, paddr_t *pa)
+{
+	const struct rproc_ta_memory_region *mems = rproc_ta_mp1_m4_mems;
+	size_t i = 0;
+
+	DMSG("da addr: %#"PRIxPA" size: %zu", da, size);
+
+	for (i = 0; i < ARRAY_SIZE(rproc_ta_mp1_m4_mems); i++) {
+		if (da >= mems[i].da &&
+		    (da + size) <= (mems[i].da + mems[i].size)) {
+			*pa = da - mems[i].da + mems[i].pa;
+			DMSG("da %#"PRIxPA" to pa %#"PRIxPA, da, *pa);
+
+			return TEE_SUCCESS;
+		}
+	}
+
+	return TEE_ERROR_ACCESS_DENIED;
+}
+
+static TEE_Result rproc_pta_load_segment(uint32_t pt,
+					 TEE_Param params[TEE_NUM_PARAMS])
+{
+	const uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+						TEE_PARAM_TYPE_MEMREF_INPUT,
+						TEE_PARAM_TYPE_VALUE_INPUT,
+						TEE_PARAM_TYPE_MEMREF_INPUT);
+	TEE_Result res = TEE_ERROR_GENERIC;
+	paddr_t pa = 0;
+	uint8_t *dst = 0;
+	uint8_t *src = params[1].memref.buffer;
+	size_t size = params[1].memref.size;
+	uint8_t *hash = params[3].memref.buffer;
+	paddr_t da = (paddr_t)reg_pair_to_64(params[2].value.b,
+					     params[2].value.a);
+
+	if (pt != exp_pt)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (!hash || params[3].memref.size != TEE_SHA256_HASH_SIZE)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Only STM32_M4_FW_ID supported */
+	if (params[0].value.a != STM32_M4_FW_ID) {
+		EMSG("Unsupported firmware ID %#"PRIx32, params[0].value.a);
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	if (rproc_ta_state != REMOTEPROC_OFF)
+		return TEE_ERROR_BAD_STATE;
+
+	/* Get the physical address in A7 mapping */
+	res = da_to_pa(da, size, &pa);
+	if (res)
+		return res;
+
+	/* Get the associated va */
+	dst = (void *)core_mmu_get_va(pa, MEM_AREA_IO_SEC);
+
+	/* Copy the segment to the remote processor memory*/
+	memcpy(dst, src, size);
+
+	/* Verify that loaded segment is valid */
+	res = hash_sha256_check(hash, dst, size);
+	if (res)
+		memset(dst, 0, size);
+
+	return res;
+}
+
+static TEE_Result rproc_pta_set_memory(uint32_t pt,
+				       TEE_Param params[TEE_NUM_PARAMS])
+{
+	const uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+						TEE_PARAM_TYPE_VALUE_INPUT,
+						TEE_PARAM_TYPE_VALUE_INPUT,
+						TEE_PARAM_TYPE_VALUE_INPUT);
+	TEE_Result res = TEE_ERROR_GENERIC;
+	paddr_t pa = 0;
+	vaddr_t dst = 0;
+	paddr_t da = params[1].value.a;
+	size_t size = params[2].value.a;
+	char value = (char)params[3].value.a;
+
+	if (pt != exp_pt)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Only STM32_M4_FW_ID supported */
+	if (params[0].value.a != STM32_M4_FW_ID) {
+		EMSG("Unsupported firmware ID %#"PRIx32, params[0].value.a);
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	if (rproc_ta_state != REMOTEPROC_OFF)
+		return TEE_ERROR_BAD_STATE;
+
+	/* Get the physical address in CPU mapping */
+	res = da_to_pa(da, size, &pa);
+	if (res)
+		return res;
+
+	dst = core_mmu_get_va(pa, MEM_AREA_IO_SEC);
+
+	memset((void *)dst, value, size);
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result rproc_pta_da_to_pa(uint32_t pt,
+				     TEE_Param params[TEE_NUM_PARAMS])
+{
+	const uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+						TEE_PARAM_TYPE_VALUE_INPUT,
+						TEE_PARAM_TYPE_VALUE_INPUT,
+						TEE_PARAM_TYPE_VALUE_OUTPUT);
+	TEE_Result res = TEE_ERROR_GENERIC;
+	paddr_t da = params[1].value.a;
+	size_t size = params[2].value.a;
+	paddr_t pa = 0;
+
+	DMSG("Conversion for address %#"PRIxPA" size %zu", da, size);
+
+	if (pt != exp_pt)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Only STM32_M4_FW_ID supported */
+	if (params[0].value.a != STM32_M4_FW_ID) {
+		EMSG("Unsupported firmware ID %#"PRIx32, params[0].value.a);
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	/* Target address is expected 32bit, ensure 32bit MSB are zero */
+	if (params[1].value.b || params[2].value.b)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = da_to_pa(da, size, &pa);
+	if (res)
+		return res;
+
+	reg_pair_from_64((uint64_t)pa, &params[3].value.b, &params[3].value.a);
+
+	return TEE_SUCCESS;
+}
+
+static void rproc_pta_mem_protect(bool secure_access)
+{
+	unsigned int i = 0;
+	const struct rproc_ta_etzpc_rams *ram = NULL;
+
+	/*
+	 * MCU RAM banks access permissions for MCU memories depending on
+	 * rproc_ta_mp1_m4_rams[].
+	 * If memory bank is declared as MCU isolated:
+	 *     if secure_access then set to secure world read/write permission
+	 *     else set to MCU isolated
+	 * else apply memory permission as defined in rproc_ta_mp1_m4_rams[].
+	 */
+	for (i = 0; i < ARRAY_SIZE(rproc_ta_mp1_m4_rams); i++) {
+		enum etzpc_decprot_attributes attr = ETZPC_DECPROT_MAX;
+
+		ram = &rproc_ta_mp1_m4_rams[i];
+		attr = ram->attr;
+
+		if (secure_access && attr == ETZPC_DECPROT_MCU_ISOLATION)
+			attr = ETZPC_DECPROT_S_RW;
+
+		etzpc_configure_decprot(ram->etzpc_id, attr);
+	}
+}
+
+static TEE_Result rproc_pta_start(uint32_t pt,
+				  TEE_Param params[TEE_NUM_PARAMS])
+{
+	const uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+						TEE_PARAM_TYPE_NONE,
+						TEE_PARAM_TYPE_NONE,
+						TEE_PARAM_TYPE_NONE);
+	vaddr_t rcc_base = stm32_rcc_base();
+
+	if (pt != exp_pt)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Only STM32_M4_FW_ID supported */
+	if (params[0].value.a != STM32_M4_FW_ID) {
+		EMSG("Unsupported firmware ID %#"PRIx32, params[0].value.a);
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	if (rproc_ta_state != REMOTEPROC_OFF)
+		return TEE_ERROR_BAD_STATE;
+
+	clk_enable(CK_MCU);
+
+	/* Configure the Cortex-M4 RAMs as expected to run the firmware */
+	rproc_pta_mem_protect(false);
+
+	/*
+	 * The firmware is started by deasserting the hold boot and
+	 * asserting back to avoid auto restart on a crash.
+	 * No need to release the MCU reset as it is automatically released by
+	 * the hardware.
+	 */
+	io_setbits32(rcc_base + RCC_MP_GCR, RCC_MP_GCR_BOOT_MCU);
+	io_clrbits32(rcc_base + RCC_MP_GCR, RCC_MP_GCR_BOOT_MCU);
+
+	rproc_ta_state = REMOTEPROC_ON;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result rproc_pta_stop(uint32_t pt,
+				 TEE_Param params[TEE_NUM_PARAMS])
+{
+	const uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+						TEE_PARAM_TYPE_NONE,
+						TEE_PARAM_TYPE_NONE,
+						TEE_PARAM_TYPE_NONE);
+	const struct rproc_ta_etzpc_rams *ram = NULL;
+	vaddr_t rcc_base = stm32_rcc_base();
+	unsigned int i = 0;
+
+	if (pt != exp_pt)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Only STM32_M4_FW_ID supported */
+	if (params[0].value.a != STM32_M4_FW_ID) {
+		EMSG("Unsupported firmware ID %#"PRIx32, params[0].value.a);
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	if (rproc_ta_state != REMOTEPROC_ON)
+		return TEE_ERROR_BAD_STATE;
+
+	/* The firmware is stopped (reset with holdboot is active) */
+	io_clrbits32(rcc_base + RCC_MP_GCR, RCC_MP_GCR_BOOT_MCU);
+
+	stm32_reset_set(MCU_R);
+
+	clk_disable(CK_MCU);
+
+	/*
+	 * Cortex-M4 memories are cleaned and access rights restored for the
+	 * secure context.
+	 */
+	rproc_pta_mem_protect(true);
+	for (i = 0; i < ARRAY_SIZE(rproc_ta_mp1_m4_rams); i++) {
+		ram = &rproc_ta_mp1_m4_rams[i];
+		if (ram->attr == ETZPC_DECPROT_MCU_ISOLATION) {
+			memset((void *)core_mmu_get_va(ram->pa,
+						       MEM_AREA_IO_SEC),
+			       0, ram->size);
+		}
+	}
+	rproc_ta_state = REMOTEPROC_OFF;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result rproc_pta_verify_rsa_signature(TEE_Param *hash,
+						 TEE_Param *sig, uint32_t algo)
+{
+	struct rsa_public_key key = { };
+	TEE_Result res = TEE_ERROR_GENERIC;
+	uint32_t e = TEE_U32_TO_BIG_ENDIAN(rproc_pub_key_exponent);
+	size_t hash_size = (size_t)hash->memref.size;
+	size_t sig_size = (size_t)sig->memref.size;
+
+	res = crypto_acipher_alloc_rsa_public_key(&key, sig_size);
+	if (res)
+		return TEE_ERROR_SECURITY;
+
+	res = crypto_bignum_bin2bn((uint8_t *)&e, sizeof(e), key.e);
+	if (res)
+		goto out;
+
+	res = crypto_bignum_bin2bn(rproc_pub_key_modulus,
+				   rproc_pub_key_modulus_size, key.n);
+	if (res)
+		goto out;
+
+	res = crypto_acipher_rsassa_verify(algo, &key, hash_size,
+					   hash->memref.buffer, hash_size,
+					   sig->memref.buffer, sig_size);
+
+out:
+	crypto_acipher_free_rsa_public_key(&key);
+
+	return res;
+}
+
+static TEE_Result rproc_pta_verify_digest(uint32_t pt,
+					  TEE_Param params[TEE_NUM_PARAMS])
+{
+	struct rproc_pta_key_info *keyinfo = NULL;
+	const uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+						TEE_PARAM_TYPE_MEMREF_INPUT,
+						TEE_PARAM_TYPE_MEMREF_INPUT,
+						TEE_PARAM_TYPE_MEMREF_INPUT);
+
+	if (pt != exp_pt)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Only STM32_M4_FW_ID supported */
+	if (params[0].value.a != STM32_M4_FW_ID) {
+		EMSG("Unsupported firmware ID %#"PRIx32, params[0].value.a);
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	if (rproc_ta_state != REMOTEPROC_OFF)
+		return TEE_ERROR_BAD_STATE;
+
+	keyinfo = params[1].memref.buffer;
+
+	if (!keyinfo ||
+	    RPROC_PTA_GET_KEYINFO_SIZE(keyinfo) != params[1].memref.size)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (keyinfo->algo != TEE_ALG_RSASSA_PKCS1_V1_5_SHA256)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	return rproc_pta_verify_rsa_signature(&params[2], &params[3],
+					      keyinfo->algo);
+}
+
+static TEE_Result rproc_pta_invoke_command(void *pSessionContext __unused,
+					   uint32_t cmd_id,
+					   uint32_t param_types,
+					   TEE_Param params[TEE_NUM_PARAMS])
+{
+	switch (cmd_id) {
+	case PTA_REMOTEPROC_HW_CAPABILITIES:
+		return rproc_pta_capabilities(param_types, params);
+	case PTA_REMOTEPROC_LOAD_SEGMENT_SHA256:
+		return rproc_pta_load_segment(param_types, params);
+	case PTA_REMOTEPROC_SET_MEMORY:
+		return rproc_pta_set_memory(param_types, params);
+	case PTA_REMOTEPROC_FIRMWARE_START:
+		return rproc_pta_start(param_types, params);
+	case PTA_REMOTEPROC_FIRMWARE_STOP:
+		return rproc_pta_stop(param_types, params);
+	case PTA_REMOTEPROC_FIRMWARE_DA_TO_PA:
+		return rproc_pta_da_to_pa(param_types, params);
+	case PTA_REMOTEPROC_VERIFY_DIGEST:
+		return rproc_pta_verify_digest(param_types, params);
+	default:
+		break;
+	}
+
+	return TEE_ERROR_NOT_IMPLEMENTED;
+}
+
+/*
+ * Trusted Application entry points
+ */
+static TEE_Result
+	rproc_pta_open_session(uint32_t param_types __unused,
+			       TEE_Param params[TEE_NUM_PARAMS] __unused,
+			       void **sess_ctx __unused)
+{
+	struct tee_ta_session *s = tee_ta_get_calling_session();
+
+	/* Check that we're called from a user TA */
+	if (!s)
+		return TEE_ERROR_ACCESS_DENIED;
+
+	if (!is_user_ta_ctx(s->ctx))
+		return TEE_ERROR_ACCESS_DENIED;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result rproc_pta_init(void)
+{
+	vaddr_t rcc_base = stm32_rcc_base();
+
+	/* Configure the Cortex-M4 rams access right for secure context only */
+	rproc_pta_mem_protect(true);
+
+	/* Initialise the context */
+	rproc_ta_state = REMOTEPROC_OFF;
+
+	/* Ensure that the MCU is HOLD */
+	io_clrbits32(rcc_base + RCC_MP_GCR, RCC_MP_GCR_BOOT_MCU);
+	stm32_reset_set(MCU_R);
+
+	return TEE_SUCCESS;
+}
+service_init_late(rproc_pta_init);
+
+pseudo_ta_register(.uuid = PTA_REMOTEPROC_UUID, .name = PTA_NAME,
+		   .flags = PTA_DEFAULT_FLAGS,
+		   .invoke_command_entry_point = rproc_pta_invoke_command,
+		   .open_session_entry_point = rproc_pta_open_session);
diff --git a/core/arch/arm/plat-stm32mp1/reset.S b/core/arch/arm/plat-stm32mp1/reset.S
index 8a1ab6494..0a11d915a 100644
--- a/core/arch/arm/plat-stm32mp1/reset.S
+++ b/core/arch/arm/plat-stm32mp1/reset.S
@@ -3,7 +3,7 @@
  * Copyright (c) 2018, STMicroelectronics
  */
 
-#include <arm32.h>
+#include <arm.h>
 #include <arm32_macros.S>
 #include <asm.S>
 
@@ -21,7 +21,33 @@ FUNC plat_cpu_reset_early , :
 	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
+	msr	cpsr, r1
+	isb
+
 	bx	lr
 END_FUNC plat_cpu_reset_early
diff --git a/core/arch/arm/plat-stm32mp1/rproc_pub_key.h b/core/arch/arm/plat-stm32mp1/rproc_pub_key.h
new file mode 100644
index 000000000..9b1db4271
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/rproc_pub_key.h
@@ -0,0 +1,16 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef RPROC_PUB_KEY_H
+#define RPROC_PUB_KEY_H
+
+#include <types_ext.h>
+
+extern const uint32_t rproc_pub_key_exponent;
+extern const uint8_t rproc_pub_key_modulus[];
+extern const size_t rproc_pub_key_modulus_size;
+
+#endif /*RPROC_PUB_KEY_H*/
+
diff --git a/core/arch/arm/plat-stm32mp1/scmi_server.c b/core/arch/arm/plat-stm32mp1/scmi_server.c
index dce8c169e..169e69ee7 100644
--- a/core/arch/arm/plat-stm32mp1/scmi_server.c
+++ b/core/arch/arm/plat-stm32mp1/scmi_server.c
@@ -1,9 +1,10 @@
 // SPDX-License-Identifier: BSD-2-Clause
 /*
- * Copyright (c) 2019, STMicroelectronics
+ * Copyright (c) 2019-2020, STMicroelectronics
  */
 #include <assert.h>
 #include <compiler.h>
+#include <drivers/clk.h>
 #include <drivers/scmi-msg.h>
 #include <drivers/scmi.h>
 #include <drivers/stm32mp1_pmic.h>
@@ -13,6 +14,7 @@
 #include <dt-bindings/regulator/st,stm32mp15-regulator.h>
 #include <dt-bindings/reset/stm32mp1-resets.h>
 #include <initcall.h>
+#include <kernel/pm.h>
 #include <mm/core_memprot.h>
 #include <mm/core_mmu.h>
 #include <platform_config.h>
@@ -312,9 +314,22 @@ const char *plat_scmi_clock_get_name(unsigned int agent_id,
 	return clock->name;
 }
 
-int32_t plat_scmi_clock_rates_array(unsigned int agent_id, unsigned int scmi_id,
-				    size_t start_index, unsigned long *array,
-				    size_t *nb_elts)
+int32_t plat_scmi_clock_rates_array(unsigned int agent_id __unused,
+				    unsigned int scmi_id __unused,
+				    size_t start_index __unused,
+				    unsigned long *array __unused,
+				    size_t *nb_elts __unused)
+{
+	/*
+	 * Explicitly do not expose clock rates by array since not
+	 * fully supported by Linux kernel as of v5.4.24.
+	 */
+	return SCMI_NOT_SUPPORTED;
+}
+
+int32_t plat_scmi_clock_rates_by_step(unsigned int agent_id,
+				      unsigned int scmi_id,
+				      unsigned long *array)
 {
 	struct stm32_scmi_clk *clock = find_clock(agent_id, scmi_id);
 
@@ -324,16 +339,48 @@ int32_t plat_scmi_clock_rates_array(unsigned int agent_id, unsigned int scmi_id,
 	if (!stm32mp_nsec_can_access_clock(clock->clock_id))
 		return SCMI_DENIED;
 
-	/* Exposed clocks are currently fixed rate clocks */
-	if (start_index)
-		return SCMI_INVALID_PARAMETERS;
+	switch (scmi_id) {
+	case CK_SCMI0_MPU:
+		/*
+		 * Pretend we support all rates for MPU clock,
+		 * CLOCK_RATE_SET will reject unsupported rates.
+		 */
+		array[0] = 0U;
+		array[1] = UINT32_MAX;
+		array[2] = 1U;
+		break;
+	default:
+		array[0] = clk_get_rate(clock->clock_id);
+		array[1] = array[0];
+		array[2] = 0U;
+		break;
+	}
 
-	if (!array)
-		*nb_elts = 1;
-	else if (*nb_elts == 1)
-		*array = stm32_clock_get_rate(clock->clock_id);
-	else
-		return SCMI_GENERIC_ERROR;
+	return SCMI_SUCCESS;
+}
+
+int32_t plat_scmi_clock_set_rate(unsigned int agent_id, unsigned int scmi_id,
+				 unsigned long rate)
+{
+	struct stm32_scmi_clk *clock = find_clock(agent_id, scmi_id);
+	int ret = SCMI_DENIED;
+
+	if (!clock)
+		return SCMI_NOT_FOUND;
+	if (!stm32mp_nsec_can_access_clock(clock->clock_id))
+		return SCMI_DENIED;
+
+	switch (scmi_id) {
+	case CK_SCMI0_MPU:
+		ret = stm32mp1_set_opp_khz(rate / 1000);
+		if (ret)
+			return SCMI_INVALID_PARAMETERS;
+		break;
+	default:
+		if (rate != clk_get_rate(clock->clock_id))
+			return SCMI_INVALID_PARAMETERS;
+		break;
+	}
 
 	return SCMI_SUCCESS;
 }
@@ -346,7 +393,7 @@ unsigned long plat_scmi_clock_get_rate(unsigned int agent_id,
 	if (!clock || !stm32mp_nsec_can_access_clock(clock->clock_id))
 		return 0;
 
-	return stm32_clock_get_rate(clock->clock_id);
+	return clk_get_rate(clock->clock_id);
 }
 
 int32_t plat_scmi_clock_get_state(unsigned int agent_id, unsigned int scmi_id)
@@ -373,13 +420,13 @@ int32_t plat_scmi_clock_set_state(unsigned int agent_id, unsigned int scmi_id,
 	if (enable_not_disable) {
 		if (!clock->enabled) {
 			DMSG("SCMI clock %u enable", scmi_id);
-			stm32_clock_enable(clock->clock_id);
+			clk_enable(clock->clock_id);
 			clock->enabled = true;
 		}
 	} else {
 		if (clock->enabled) {
 			DMSG("SCMI clock %u disable", scmi_id);
-			stm32_clock_disable(clock->clock_id);
+			clk_disable(clock->clock_id);
 			clock->enabled = false;
 		}
 	}
@@ -483,6 +530,19 @@ int32_t plat_scmi_rd_set_state(unsigned int agent_id, unsigned int scmi_id,
 	return SCMI_SUCCESS;
 }
 
+static TEE_Result stm32_scmi_pm(enum pm_op op, unsigned int pm_hint __unused,
+				const struct pm_callback_handle *hdl __unused)
+{
+	size_t i = 0;
+
+	if (op == PM_OP_RESUME)
+		for (i = 0; i < ARRAY_SIZE(scmi_channel); i++)
+			scmi_smt_init_agent_channel(&scmi_channel[i]);
+
+	return TEE_SUCCESS;
+}
+DECLARE_KEEP_PAGER(stm32_scmi_pm);
+
 /*
  * Platform SCMI voltage domains
  */
@@ -810,6 +870,8 @@ static TEE_Result stm32mp1_init_scmi_server(void)
 	size_t i = 0;
 	size_t j = 0;
 
+	register_pm_driver_cb(stm32_scmi_pm, NULL);
+
 	for (i = 0; i < ARRAY_SIZE(scmi_channel); i++) {
 		struct scmi_msg_channel *chan = &scmi_channel[i];
 
@@ -834,7 +896,7 @@ static TEE_Result stm32mp1_init_scmi_server(void)
 			/* Sync SCMI clocks with their targeted initial state */
 			if (clk->enabled &&
 			    stm32mp_nsec_can_access_clock(clk->clock_id))
-				stm32_clock_enable(clk->clock_id);
+				clk_enable(clk->clock_id);
 		}
 
 		for (j = 0; j < res->rd_count; j++) {
diff --git a/core/arch/arm/plat-stm32mp1/shared_resources.c b/core/arch/arm/plat-stm32mp1/shared_resources.c
index 3d1609353..7ad3d7b9c 100644
--- a/core/arch/arm/plat-stm32mp1/shared_resources.c
+++ b/core/arch/arm/plat-stm32mp1/shared_resources.c
@@ -1,14 +1,14 @@
 // SPDX-License-Identifier: BSD-3-Clause
 /*
- * Copyright (c) 2017-2019, STMicroelectronics
+ * Copyright (c) 2017-2020, STMicroelectronics
  */
 
 #include <drivers/stm32_etzpc.h>
 #include <drivers/stm32_gpio.h>
-#include <drivers/stm32mp1_etzpc.h>
 #include <drivers/stm32mp1_rcc.h>
 #include <dt-bindings/clock/stm32mp1-clks.h>
 #include <dt-bindings/reset/stm32mp1-resets.h>
+#include <dt-bindings/soc/st,stm32-etzpc.h>
 #include <initcall.h>
 #include <io.h>
 #include <keep.h>
@@ -147,6 +147,9 @@ static const char __maybe_unused *shres2str_id_tbl[STM32MP1_SHRES_COUNT] = {
 
 static __maybe_unused const char *shres2str_id(enum stm32mp_shres id)
 {
+	if (id >= ARRAY_SIZE(shres2str_id_tbl))
+		panic("Invalid shared resource ID");
+
 	return shres2str_id_tbl[id];
 }
 
@@ -407,6 +410,91 @@ void stm32mp_register_non_secure_gpio(unsigned int bank, unsigned int pin)
 	}
 }
 
+#ifdef CFG_STM32_ETZPC
+struct shres2decprot {
+	enum stm32mp_shres shres_id;
+	unsigned int decprot_id;
+};
+
+#define SHRES2DECPROT(shres, decprot) {		\
+		.shres_id = shres,		\
+		.decprot_id = decprot,		\
+	}
+
+static const struct shres2decprot shres2decprot_tbl[] = {
+	SHRES2DECPROT(STM32MP1_SHRES_IWDG1, STM32MP1_ETZPC_IWDG1_ID),
+	SHRES2DECPROT(STM32MP1_SHRES_USART1, STM32MP1_ETZPC_USART1_ID),
+	SHRES2DECPROT(STM32MP1_SHRES_SPI6, STM32MP1_ETZPC_SPI6_ID),
+	SHRES2DECPROT(STM32MP1_SHRES_I2C4, STM32MP1_ETZPC_I2C4_ID),
+	SHRES2DECPROT(STM32MP1_SHRES_RNG1, STM32MP1_ETZPC_RNG1_ID),
+	SHRES2DECPROT(STM32MP1_SHRES_HASH1, STM32MP1_ETZPC_HASH1_ID),
+	SHRES2DECPROT(STM32MP1_SHRES_CRYP1, STM32MP1_ETZPC_CRYP1_ID),
+	SHRES2DECPROT(STM32MP1_SHRES_I2C6, STM32MP1_ETZPC_I2C6_ID),
+};
+
+static enum stm32mp_shres decprot2shres(unsigned int decprot_id)
+{
+	size_t i = 0;
+
+	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 shared resource for DECPROT ID %u", decprot_id);
+	return STM32MP1_SHRES_COUNT;
+}
+
+void stm32mp_register_etzpc_decprot(unsigned int id,
+				    enum etzpc_decprot_attributes attr)
+{
+	enum shres_state state = SHRES_SECURE;
+	bool write_secure = false;
+	unsigned int id_shres = STM32MP1_SHRES_COUNT;
+
+	switch (attr) {
+	case ETZPC_DECPROT_S_RW:
+		state = SHRES_SECURE;
+		break;
+	case ETZPC_DECPROT_NS_R_S_W:
+	case ETZPC_DECPROT_MCU_ISOLATION:
+	case ETZPC_DECPROT_NS_RW:
+		state = SHRES_NON_SECURE;
+		break;
+	default:
+		panic();
+	}
+
+	write_secure = attr == ETZPC_DECPROT_NS_R_S_W ||
+		       attr == ETZPC_DECPROT_S_RW;
+
+	switch (id) {
+	case STM32MP1_ETZPC_STGENC_ID:
+	case STM32MP1_ETZPC_BKPSRAM_ID:
+		if (state != SHRES_SECURE)
+			panic("ETZPC: STGEN & BKSRAM should be secure");
+		break;
+	case STM32MP1_ETZPC_DDRCTRL_ID:
+	case STM32MP1_ETZPC_DDRPHYC_ID:
+		/* We assume these must always be write-only to secure world */
+		if (!write_secure)
+			panic("ETZPC: DDRCTRL & DDRPHY should be write secure");
+		break;
+	default:
+		id_shres = decprot2shres(id);
+		if (id_shres >= STM32MP1_SHRES_COUNT) {
+			if (write_secure) {
+				DMSG("ETZPC: cannot secure DECPROT %s",
+				     shres2str_id(id_shres));
+				panic();
+			}
+		} else {
+			register_periph(id_shres, state);
+		}
+		break;
+	}
+}
+#endif /*CFG_STM32_ETZPC*/
+
 static void lock_registering(void)
 {
 	registering_locked = true;
@@ -614,13 +702,39 @@ static void config_lock_decprot(uint32_t decprot_id,
 		etzpc_lock_decprot(decprot_id);
 }
 
+static bool decprot_is_write_secure(uint32_t decprot_id)
+{
+	enum etzpc_decprot_attributes attr = etzpc_get_decprot(decprot_id);
+
+	return attr == ETZPC_DECPROT_S_RW ||
+	       attr == ETZPC_DECPROT_NS_R_S_W;
+}
+
+static bool decprot_is_secure(uint32_t decprot_id)
+{
+	enum etzpc_decprot_attributes attr = etzpc_get_decprot(decprot_id);
+
+	return attr == ETZPC_DECPROT_S_RW;
+}
+
 static void set_etzpc_secure_configuration(void)
 {
 	/* Some peripherals shall be secure */
-	config_lock_decprot(STM32MP1_ETZPC_STGENC_ID, ETZPC_DECPROT_S_RW);
-	config_lock_decprot(STM32MP1_ETZPC_BKPSRAM_ID, ETZPC_DECPROT_S_RW);
-	config_lock_decprot(STM32MP1_ETZPC_DDRCTRL_ID, ETZPC_DECPROT_S_RW);
-	config_lock_decprot(STM32MP1_ETZPC_DDRPHYC_ID, ETZPC_DECPROT_S_RW);
+	if (!decprot_is_secure(STM32MP1_ETZPC_STGENC_ID))
+		config_lock_decprot(STM32MP1_ETZPC_STGENC_ID,
+				    ETZPC_DECPROT_S_RW);
+
+	if (!decprot_is_secure(STM32MP1_ETZPC_BKPSRAM_ID))
+		config_lock_decprot(STM32MP1_ETZPC_BKPSRAM_ID,
+				    ETZPC_DECPROT_S_RW);
+
+	if (!decprot_is_write_secure(STM32MP1_ETZPC_DDRCTRL_ID))
+		config_lock_decprot(STM32MP1_ETZPC_DDRCTRL_ID,
+				    ETZPC_DECPROT_NS_R_S_W);
+
+	if (!decprot_is_write_secure(STM32MP1_ETZPC_DDRPHYC_ID))
+		config_lock_decprot(STM32MP1_ETZPC_DDRPHYC_ID,
+				    ETZPC_DECPROT_NS_R_S_W);
 
 	/* Configure ETZPC with peripheral registering */
 	config_lock_decprot(STM32MP1_ETZPC_IWDG1_ID,
@@ -651,27 +765,35 @@ static void check_rcc_secure_configuration(void)
 {
 	bool secure = stm32_rcc_is_secure();
 	bool mckprot = stm32_rcc_is_mckprot();
+	bool need_secure = false;
+	bool need_mckprot = false;
 	enum stm32mp_shres id = STM32MP1_SHRES_COUNT;
-	bool have_error = false;
 
-	if (stm32mp_is_closed_device() && !secure)
-		panic();
+	if (stm32mp_is_closed_device() && !secure) {
+		DMSG("stm32mp1: closed device mandates OP-TEE to secure RCC");
+		need_secure = true;
+	}
 
 	for (id = 0; id < STM32MP1_SHRES_COUNT; id++) {
 		if  (shres_state[id] != SHRES_SECURE)
 			continue;
 
-		if ((mckprot_resource(id) && !mckprot) || !secure) {
-			EMSG("RCC %s MCKPROT %s and %s (%u) secure",
-			      secure ? "secure" : "non-secure",
-			      mckprot ? "set" : "not set",
-			      shres2str_id(id), id);
-			have_error = true;
-		}
+		need_secure = true;
+		if (mckprot_resource(id))
+			need_mckprot = true;
+
+		if ((mckprot_resource(id) && !mckprot) || !secure)
+			EMSG("Error: RCC TZEN=%u MCKPROT=%u while %s is secure",
+			      secure, mckprot, shres2str_id(id));
 	}
 
-	if (have_error)
+	DMSG("RCC/PWR secure hardening: TZEN %sable, MCKPROT %sable",
+	     secure ? "en" : "dis", mckprot ? "en" : "dis");
+
+	if (need_secure && !secure)
 		panic();
+
+	stm32mp1_clk_mcuss_protect(need_mckprot);
 }
 
 static void set_gpio_secure_configuration(void)
diff --git a/core/arch/arm/plat-stm32mp1/stm32_util.h b/core/arch/arm/plat-stm32mp1/stm32_util.h
index 6e4ac7499..efb364998 100644
--- a/core/arch/arm/plat-stm32mp1/stm32_util.h
+++ b/core/arch/arm/plat-stm32mp1/stm32_util.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: BSD-3-Clause */
 /*
- * Copyright (c) 2018-2019, STMicroelectronics
+ * Copyright (c) 2018-2020, STMicroelectronics
  */
 
 #ifndef __STM32_UTIL_H__
@@ -9,11 +9,23 @@
 #include <assert.h>
 #include <drivers/stm32_bsec.h>
 #include <kernel/panic.h>
+#include <stdbool.h>
 #include <stdint.h>
 #include <types_ext.h>
 
+/* SoC versioning and device ID */
+TEE_Result stm32mp1_dbgmcu_get_chip_version(uint32_t *chip_version);
+TEE_Result stm32mp1_dbgmcu_get_chip_dev_id(uint32_t *chip_dev_id);
+
+/* OPP service */
+bool stm32mp_supports_cpu_opp(uint32_t opp_id);
+
 /* Backup registers and RAM utils */
 vaddr_t stm32mp_bkpreg(unsigned int idx);
+vaddr_t stm32mp_bkpsram_base(void);
+
+/* Platform util for the STGEN driver */
+vaddr_t stm32mp_stgen_base(void);
 
 /*
  * SYSCFG IO compensation.
@@ -25,6 +37,7 @@ void stm32mp_syscfg_disable_io_compensation(void);
 /* Platform util for the GIC */
 vaddr_t get_gicc_base(void);
 vaddr_t get_gicd_base(void);
+void stm32mp_gic_set_end_of_interrupt(uint32_t it);
 
 /*
  * Platform util functions for the GPIO driver
@@ -43,6 +56,18 @@ vaddr_t stm32_get_gpio_bank_base(unsigned int bank);
 unsigned int stm32_get_gpio_bank_offset(unsigned int bank);
 unsigned int stm32_get_gpio_bank_clock(unsigned int bank);
 
+/*
+ * Platform util for the IWDG driver
+ */
+
+/* Get IWDG_* enable flags mask from watchdog configuration read from fuses */
+unsigned long stm32_get_iwdg_otp_config(vaddr_t pbase);
+
+/* Conversion between IWDG instance IDs and hardware resources */
+size_t stm32mp_iwdg_instance2irq(unsigned int instance);
+unsigned int stm32mp_iwdg_irq2instance(size_t irq);
+unsigned int stm32mp_iwdg_iomem2instance(vaddr_t pbase);
+
 /* Platform util for PMIC support */
 bool stm32mp_with_pmic(void);
 
@@ -62,15 +87,6 @@ static inline void stm32mp_register_online_cpu(void)
 uint32_t may_spin_lock(unsigned int *lock);
 void may_spin_unlock(unsigned int *lock, uint32_t exceptions);
 
-/*
- * Util for clock gating and to get clock rate for stm32 and platform drivers
- * @id: Target clock ID, ID used in clock DT bindings
- */
-void stm32_clock_enable(unsigned long id);
-void stm32_clock_disable(unsigned long id);
-unsigned long stm32_clock_get_rate(unsigned long id);
-bool stm32_clock_is_enabled(unsigned long id);
-
 /* Return true if @clock_id is shared by secure and non-secure worlds */
 bool stm32mp_nsec_can_access_clock(unsigned long clock_id);
 
@@ -84,6 +100,22 @@ static inline bool stm32mp_nsec_can_access_pmic_regu(const char *name __unused)
 }
 #endif
 
+/* PM sequences specific to SoC STOP mode support */
+void stm32mp1_clk_save_context_for_stop(void);
+void stm32mp1_clk_restore_context_for_stop(void);
+
+/* Protect the MCU clock subsytem */
+void stm32mp1_clk_mcuss_protect(bool enable);
+
+/*
+ * Util for PLL1 settings management based on DT OPP table content.
+ */
+int stm32mp1_clk_compute_all_pll1_settings(uint32_t buck1_voltage);
+void stm32mp1_clk_lp_save_opp_pll1_settings(uint8_t *data, size_t size);
+bool stm32mp1_clk_pll1_settings_are_valid(void);
+int stm32mp1_set_opp_khz(uint32_t freq_khz);
+int stm32mp1_round_opp_khz(uint32_t *freq_khz);
+
 /*
  * Util for reset signal assertion/desassertion for stm32 and platform drivers
  * @id: Target peripheral ID, ID used in reset DT bindings
@@ -123,6 +155,27 @@ struct stm32_bsec_static_cfg {
 
 void stm32mp_get_bsec_static_cfg(struct stm32_bsec_static_cfg *cfg);
 
+/* Reset function for early watchdog management */
+void stm32mp_platform_reset(int cpu);
+
+/* Clock calibration. Returns 0 on success */
+#ifdef CFG_STM32_CLKCALIB
+int stm32mp_start_clock_calib(unsigned int clock_id);
+#else
+static inline int stm32mp_start_clock_calib(unsigned int clock_id __unused)
+{
+	return 1;
+}
+#endif
+
+/* Platform util for the RTC driver */
+bool stm32_rtc_get_read_twice(void);
+
+/* Platform util for the ETZPC driver */
+
+/* Convert a ETZPC mode from DT binding to ETZPC DECPROT configuration */
+enum etzpc_decprot_attributes stm32mp_etzpc_binding2decprot(uint32_t mode);
+
 /*
  * Return true if platform is in closed_device mode
  */
@@ -294,4 +347,8 @@ bool stm32mp_gpio_bank_is_non_secure(unsigned int bank);
 /* Register parent clocks of @clock (ID used in clock DT bindings) as secure */
 void stm32mp_register_clock_parents_secure(unsigned long clock_id);
 
+/* Register a resource identified by a ETZPC DECPROT identifier */
+void stm32mp_register_etzpc_decprot(unsigned int id,
+				    enum etzpc_decprot_attributes attr);
+
 #endif /*__STM32_UTIL_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 000000000..4265db9dd
--- /dev/null
+++ b/core/arch/arm/plat-stm32mp1/stm32mp_pm.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (c) 2017-2018, STMicroelectronics
+ */
+#ifndef __STM32MP_PM_H__
+#define __STM32MP_PM_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
+};
+
+void __noreturn stm32_cores_reset(void);
+
+#endif /*__STM32MP_PM_H__*/
+
diff --git a/core/arch/arm/plat-stm32mp1/sub.mk b/core/arch/arm/plat-stm32mp1/sub.mk
index 69df1ced5..ae2041741 100644
--- a/core/arch/arm/plat-stm32mp1/sub.mk
+++ b/core/arch/arm/plat-stm32mp1/sub.mk
@@ -6,6 +6,15 @@ srcs-$(CFG_STM32_RNG) += rng_seed.c
 srcs-y += scmi_server.c
 srcs-y += shared_resources.c
 srcs-$(CFG_TZC400) += plat_tzc400.c
+srcs-$(CFG_RPROC_PTA) += remoteproc_pta.c
+
+ifeq ($(CFG_RPROC_PTA),y)
+gensrcs-y += rproc_pub_key
+produce-rproc_pub_key = rproc_pub_key.c
+depends-rproc_pub_key = $(CFG_RPROC_SIGN_KEY) scripts/pem_to_pub_c.py
+recipe-rproc_pub_key = $(PYTHON3) scripts/pem_to_pub_c.py --prefix rproc_pub_key \
+        --key $(CFG_RPROC_SIGN_KEY) --out $(sub-dir-out)/rproc_pub_key.c
+endif
 
 subdirs-y += drivers
 subdirs-y += nsec-service
diff --git a/core/arch/arm/sm/pm_a32.S b/core/arch/arm/sm/pm_a32.S
index a927b1e77..3e0080af4 100644
--- a/core/arch/arm/sm/pm_a32.S
+++ b/core/arch/arm/sm/pm_a32.S
@@ -65,25 +65,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
 END_FUNC sm_pm_cpu_do_suspend
@@ -146,50 +151,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
 END_FUNC sm_pm_cpu_do_resume
diff --git a/core/arch/arm/tee/entry_std.c b/core/arch/arm/tee/entry_std.c
index 424c4c677..92db391d4 100644
--- a/core/arch/arm/tee/entry_std.c
+++ b/core/arch/arm/tee/entry_std.c
@@ -588,4 +588,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/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c b/core/drivers/clk/clk-stm32mp15.c
similarity index 50%
rename from core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c
rename to core/drivers/clk/clk-stm32mp15.c
index c8dbbb7ae..794fd5e75 100644
--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_clk.c
+++ b/core/drivers/clk/clk-stm32mp15.c
@@ -1,25 +1,47 @@
 // SPDX-License-Identifier: (BSD-3-Clause OR GPL-2.0+)
 /*
- * Copyright (C) 2018-2019, STMicroelectronics
+ * Copyright (C) 2018-2020, STMicroelectronics
  */
 
 #include <assert.h>
+#include <drivers/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/delay.h>
 #include <kernel/dt.h>
 #include <kernel/boot.h>
 #include <kernel/panic.h>
+#include <kernel/pm.h>
 #include <kernel/spinlock.h>
 #include <libfdt.h>
 #include <platform_config.h>
 #include <stdio.h>
 #include <stm32_util.h>
+#include <tee_api_types.h>
 #include <trace.h>
 #include <util.h>
 
+#define DT_OPP_COMPAT		"operating-points-v2"
+
+/* PLL settings computation related definitions */
+#define POST_DIVM_MIN	8000000
+#define POST_DIVM_MAX	16000000
+#define DIVM_MIN	0
+#define DIVM_MAX	63
+#define DIVN_MIN	24
+#define DIVN_MAX	99
+#define DIVP_MIN	0
+#define DIVP_MAX	127
+#define FRAC_MAX	8192
+#define VCO_MIN		800000000
+#define VCO_MAX		1600000000
+
+#define PLL1_SETTINGS_VALID_ID		0x504C4C31 /* "PLL1" */
+
 /* Identifiers for root oscillators */
 enum stm32mp_osc_id {
 	_HSI = 0,
@@ -85,11 +107,14 @@ enum stm32mp1_parent_sel {
 	_UART24_SEL,
 	_UART35_SEL,
 	_UART78_SEL,
+	_SDMMC12_SEL,
+	_SDMMC3_SEL,
 	_AXISS_SEL,
 	_MCUSS_SEL,
 	_USBPHY_SEL,
 	_USBO_SEL,
 	_RTC_SEL,
+	_MPU_SEL,
 	_PARENT_SEL_NB,
 	_UNKNOWN_SEL = 0xff,
 };
@@ -160,6 +185,16 @@ enum stm32mp1_div_id {
 	_DIV_NB,
 };
 
+enum stm32mp1_pllcfg {
+	PLLCFG_M,
+	PLLCFG_N,
+	PLLCFG_P,
+	PLLCFG_Q,
+	PLLCFG_R,
+	PLLCFG_O,
+	PLLCFG_NB
+};
+
 enum stm32mp1_plltype {
 	PLL_800,
 	PLL_1600,
@@ -212,6 +247,21 @@ struct stm32mp1_clk_pll {
 	enum stm32mp_osc_id refclk[REFCLK_SIZE];
 };
 
+struct stm32mp1_pll {
+	uint8_t refclk_min;
+	uint8_t refclk_max;
+	uint8_t divn_max;
+};
+
+/* Compact structure of 32bit cells, copied raw when suspending */
+struct stm32mp1_pll_settings {
+	uint32_t valid_id;
+	uint32_t freq[PLAT_MAX_OPP_NB];
+	uint32_t volt[PLAT_MAX_OPP_NB];
+	uint32_t cfg[PLAT_MAX_OPP_NB][PLAT_MAX_PLLCFG_NB];
+	uint32_t frac[PLAT_MAX_OPP_NB];
+};
+
 #define N_S	0	/* Non-secure can access RCC interface */
 #define SEC	1	/* RCC[TZEN] protects RCC interface */
 
@@ -380,6 +430,18 @@ static const struct stm32mp1_clk_gate stm32mp1_clk_gate[] = {
 	_CLK_SC_FIXED(N_S, RCC_MP_APB3ENSETR, 11, SYSCFG, _UNKNOWN_ID),
 	_CLK_SC_SELEC(N_S, RCC_MP_APB4ENSETR, 8, DDRPERFM, _UNKNOWN_SEL),
 	_CLK_SC_SELEC(N_S, RCC_MP_APB4ENSETR, 15, IWDG2, _UNKNOWN_SEL),
+#ifdef STM32MP1_USE_MPU0_RESET
+	_CLK_SC_SELEC(N_S, RCC_MP_APB4ENSETR, 0, LTDC_PX, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB2ENSETR, 0, DMA1, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB2ENSETR, 1, DMA2, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB2ENSETR, 8, USBO_K, _USBO_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB2ENSETR, 16, SDMMC3_K, _SDMMC3_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 5, GPU, _UNKNOWN_SEL),
+	_CLK_SC_FIXED(N_S, RCC_MP_AHB6ENSETR, 10, ETHMAC, _ACLK),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 16, SDMMC1_K, _SDMMC12_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 17, SDMMC2_K, _SDMMC12_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 24, USBH, _UNKNOWN_SEL),
+#endif
 
 	_CLK_SELEC(N_S, RCC_DBGCFGR, 8, CK_DBG, _UNKNOWN_SEL),
 };
@@ -406,6 +468,10 @@ static const uint8_t rng1_parents[] = {
 	_CSI, _PLL4_R, _LSE, _LSI
 };
 
+static const uint8_t mpu_parents[] = {
+	_HSI, _HSE, _PLL1_P, _PLL1_P /* specific div */
+};
+
 /* Parents for (some) non-secure clocks */
 #ifdef CFG_WITH_NSEC_UARTS
 static const uint8_t uart6_parents[] = {
@@ -429,6 +495,24 @@ static const uint8_t rtc_parents[] = {
 	_UNKNOWN_ID, _LSE, _LSI, _HSE
 };
 
+#ifdef STM32MP1_USE_MPU0_RESET
+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 uint8_t sdmmc12_parents[] = {
+	_HCLK6, _PLL3_R, _PLL4_P, _HSI_KER
+};
+
+static const uint8_t sdmmc3_parents[] = {
+	_HCLK2, _PLL3_R, _PLL4_P, _HSI_KER
+};
+#endif
+
 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),
@@ -437,6 +521,9 @@ static const struct stm32mp1_clk_sel stm32mp1_clk_sel[_PARENT_SEL_NB] = {
 	_CLK_PARENT(_USART1_SEL, RCC_UART1CKSELR, 0, 0x7, usart1_parents),
 	_CLK_PARENT(_RNG1_SEL, RCC_RNG1CKSELR, 0, 0x3, rng1_parents),
 	_CLK_PARENT(_RTC_SEL, RCC_BDCR, 0, 0x3, rtc_parents),
+	_CLK_PARENT(_MPU_SEL, RCC_MPCKSELR, 0, 0x3, mpu_parents),
+	_CLK_PARENT(_AXISS_SEL, RCC_ASSCKSELR, 0, 0x3, axiss_parents),
+	_CLK_PARENT(_MCUSS_SEL, RCC_MSSCKSELR, 0, 0x3, mcuss_parents),
 	/* Always non-secure clocks (maybe used in some way in secure world) */
 #ifdef CFG_WITH_NSEC_UARTS
 	_CLK_PARENT(_UART6_SEL, RCC_UART6CKSELR, 0, 0x7, uart6_parents),
@@ -444,8 +531,26 @@ static const struct stm32mp1_clk_sel stm32mp1_clk_sel[_PARENT_SEL_NB] = {
 	_CLK_PARENT(_UART35_SEL, RCC_UART35CKSELR, 0, 0x7, uart234578_parents),
 	_CLK_PARENT(_UART78_SEL, RCC_UART78CKSELR, 0, 0x7, uart234578_parents),
 #endif
-	_CLK_PARENT(_AXISS_SEL, RCC_ASSCKSELR, 0, 0x3, axiss_parents),
-	_CLK_PARENT(_MCUSS_SEL, RCC_MSSCKSELR, 0, 0x3, mcuss_parents),
+#ifdef STM32MP1_USE_MPU0_RESET
+	_CLK_PARENT(_SDMMC12_SEL, RCC_SDMMC12CKSELR, 0, 0x7, sdmmc12_parents),
+	_CLK_PARENT(_SDMMC3_SEL, RCC_SDMMC3CKSELR, 0, 0x7, sdmmc3_parents),
+	_CLK_PARENT(_USBPHY_SEL, RCC_USBCKSELR, 0, 0x3, usbphy_parents),
+	_CLK_PARENT(_USBO_SEL, RCC_USBCKSELR, 4, 0x1, usbo_parents),
+#endif
+};
+
+/* Define characteristics of PLL according type */
+static const struct stm32mp1_pll stm32mp1_pll[PLL_TYPE_NB] = {
+	[PLL_800] = {
+		.refclk_min = 4,
+		.refclk_max = 16,
+		.divn_max = 99,
+	},
+	[PLL_1600] = {
+		.refclk_min = 8,
+		.refclk_max = 16,
+		.divn_max = 199,
+	},
 };
 
 /* PLLNCFGR2 register divider by output */
@@ -549,6 +654,10 @@ static unsigned long osc_frequency(enum stm32mp_osc_id idx)
 static unsigned int gate_refcounts[NB_GATES];
 static unsigned int refcount_lock;
 
+/* Storage of the precomputed SoC settings for PLL1 various OPPs */
+static struct stm32mp1_pll_settings pll1_settings;
+static uint32_t current_opp_khz;
+
 static const struct stm32mp1_clk_gate *gate_ref(unsigned int idx)
 {
 	return &stm32mp1_clk_gate[idx];
@@ -591,7 +700,7 @@ 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)
+static int __clk_get_parent(unsigned long id)
 {
 	const struct stm32mp1_clk_sel *sel = NULL;
 	enum stm32mp1_parent_id parent_id = 0;
@@ -701,7 +810,153 @@ static unsigned long stm32mp1_read_pll_freq(enum stm32mp1_pll_id pll_id,
 	return dfout;
 }
 
-static unsigned long get_clock_rate(int p)
+static void pll_start(enum stm32mp1_pll_id pll_id)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uint32_t pllxcr = stm32_rcc_base() + pll->pllxcr;
+
+	if (io_read32(pllxcr) & RCC_PLLNCR_PLLON)
+		return;
+
+	io_clrsetbits32(pllxcr,	RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN |
+			RCC_PLLNCR_DIVREN, RCC_PLLNCR_PLLON);
+}
+
+#define PLLRDY_TIMEOUT_US	(200 * 1000)
+
+static int pll_output(enum stm32mp1_pll_id pll_id, uint32_t output)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uint32_t pllxcr = stm32_rcc_base() + pll->pllxcr;
+	uint64_t start = 0;
+
+	start = timeout_init_us(PLLRDY_TIMEOUT_US);
+	/* Wait PLL lock */
+	while (!(io_read32(pllxcr) & RCC_PLLNCR_PLLRDY))
+		if (timeout_elapsed(start)) {
+			EMSG("PLL%d start failed @ 0x%"PRIx32": 0x%"PRIx32,
+			     pll_id, pllxcr, io_read32(pllxcr));
+			return -1;
+		}
+
+	/* Start the requested output */
+	io_setbits32(pllxcr, output << RCC_PLLNCR_DIVEN_SHIFT);
+
+	return 0;
+}
+
+static int pll_stop(enum stm32mp1_pll_id pll_id)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uint32_t pllxcr = stm32_rcc_base() + pll->pllxcr;
+	uint64_t start = 0;
+
+	/* Stop all output */
+	io_clrbits32(pllxcr, RCC_PLLNCR_DIVPEN | RCC_PLLNCR_DIVQEN |
+		     RCC_PLLNCR_DIVREN);
+
+	/* Stop PLL */
+	io_clrbits32(pllxcr, RCC_PLLNCR_PLLON);
+
+	start = timeout_init_us(PLLRDY_TIMEOUT_US);
+	/* Wait PLL stopped */
+	while (!(io_read32(pllxcr) & RCC_PLLNCR_PLLRDY))
+		if (timeout_elapsed(start)) {
+			EMSG("PLL%d stop failed @ 0x%"PRIx32": 0x%"PRIx32,
+			     pll_id, pllxcr, io_read32(pllxcr));
+
+			return -1;
+		}
+
+	return 0;
+}
+
+static uint32_t pll_compute_pllxcfgr2(uint32_t *pllcfg)
+{
+	uint32_t value = 0;
+
+	value = (pllcfg[PLLCFG_P] << RCC_PLLNCFGR2_DIVP_SHIFT) &
+		RCC_PLLNCFGR2_DIVP_MASK;
+	value |= (pllcfg[PLLCFG_Q] << RCC_PLLNCFGR2_DIVQ_SHIFT) &
+		 RCC_PLLNCFGR2_DIVQ_MASK;
+	value |= (pllcfg[PLLCFG_R] << RCC_PLLNCFGR2_DIVR_SHIFT) &
+		 RCC_PLLNCFGR2_DIVR_MASK;
+
+	return value;
+}
+
+static void pll_config_output(enum stm32mp1_pll_id pll_id, uint32_t *pllcfg)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uintptr_t rcc_base = stm32_rcc_base();
+	uint32_t value = 0;
+
+	value = pll_compute_pllxcfgr2(pllcfg);
+
+	io_write32(rcc_base + pll->pllxcfgr2, value);
+}
+
+static int pll_compute_pllxcfgr1(const struct stm32mp1_clk_pll *pll,
+				 uint32_t *pllcfg, uint32_t *cfgr1)
+{
+	uint32_t rcc_base = stm32_rcc_base();
+	enum stm32mp1_plltype type = pll->plltype;
+	unsigned long refclk = 0;
+	uint32_t ifrge = 0;
+	uint32_t src = 0;
+
+	src = io_read32(rcc_base + pll->rckxselr) &
+	      RCC_SELR_REFCLK_SRC_MASK;
+
+	refclk = osc_frequency(pll->refclk[src]) /
+		 (pllcfg[PLLCFG_M] + 1U);
+
+	if ((refclk < (stm32mp1_pll[type].refclk_min * 1000000U)) ||
+	    (refclk > (stm32mp1_pll[type].refclk_max * 1000000U)))
+		return -1;
+
+	if ((type == PLL_800) && (refclk >= 8000000U))
+		ifrge = 1U;
+
+	*cfgr1 = (pllcfg[PLLCFG_N] << RCC_PLLNCFGR1_DIVN_SHIFT) &
+		 RCC_PLLNCFGR1_DIVN_MASK;
+	*cfgr1 |= (pllcfg[PLLCFG_M] << RCC_PLLNCFGR1_DIVM_SHIFT) &
+		  RCC_PLLNCFGR1_DIVM_MASK;
+	*cfgr1 |= (ifrge << RCC_PLLNCFGR1_IFRGE_SHIFT) &
+		  RCC_PLLNCFGR1_IFRGE_MASK;
+
+	return 0;
+}
+
+static int pll_config(enum stm32mp1_pll_id pll_id, uint32_t *pllcfg,
+		      uint32_t fracv)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uint32_t rcc_base = stm32_rcc_base();
+	uint32_t value = 0;
+	int ret = 0;
+
+	ret = pll_compute_pllxcfgr1(pll, pllcfg, &value);
+	if (ret)
+		return ret;
+
+	io_write32(rcc_base + pll->pllxcfgr1, value);
+
+	/* Fractional configuration */
+	io_write32(rcc_base + pll->pllxfracr, value);
+
+	/* Frac must be enabled only once its configuration is loaded */
+	value = fracv << RCC_PLLNFRACR_FRACV_SHIFT;
+	io_write32(rcc_base + pll->pllxfracr, value);
+	value = io_read32(rcc_base + pll->pllxfracr);
+	io_write32(rcc_base + pll->pllxfracr, value | RCC_PLLNFRACR_FRACLE);
+
+	pll_config_output(pll_id, pllcfg);
+
+	return 0;
+}
+
+static unsigned long __clk_get_parent_rate(enum stm32mp1_parent_id p)
 {
 	uint32_t reg = 0;
 	unsigned long clock = 0;
@@ -964,7 +1219,7 @@ static bool clock_is_always_on(unsigned long id)
 	}
 }
 
-bool stm32_clock_is_enabled(unsigned long id)
+static bool clk_stm32_is_enabled(unsigned long id)
 {
 	int i = 0;
 
@@ -978,13 +1233,13 @@ bool stm32_clock_is_enabled(unsigned long id)
 	return __clk_is_enabled(gate_ref(i));
 }
 
-void stm32_clock_enable(unsigned long id)
+static TEE_Result clk_stm32_enable(unsigned long id)
 {
 	int i = 0;
 	uint32_t exceptions = 0;
 
 	if (clock_is_always_on(id))
-		return;
+		return TEE_SUCCESS;
 
 	i = stm32mp1_clk_get_gated_id(id);
 	if (i < 0) {
@@ -995,7 +1250,7 @@ void stm32_clock_enable(unsigned long id)
 	if (gate_is_non_secure(gate_ref(i))) {
 		/* Enable non-secure clock w/o any refcounting */
 		__clk_enable(gate_ref(i));
-		return;
+		return TEE_SUCCESS;
 	}
 
 	exceptions = may_spin_lock(&refcount_lock);
@@ -1006,9 +1261,11 @@ void stm32_clock_enable(unsigned long id)
 	gate_refcounts[i]++;
 
 	may_spin_unlock(&refcount_lock, exceptions);
+
+	return TEE_SUCCESS;
 }
 
-void stm32_clock_disable(unsigned long id)
+static void clk_stm32_disable(unsigned long id)
 {
 	int i = 0;
 	uint32_t exceptions = 0;
@@ -1067,16 +1324,16 @@ static long get_timer_rate(long parent_rate, unsigned int apb_bus)
 	return parent_rate * (timgxpre + 1) * 2;
 }
 
-unsigned long stm32_clock_get_rate(unsigned long id)
+static unsigned long clk_stm32_get_rate(unsigned long id)
 {
-	int p = 0;
+	enum stm32mp1_parent_id p = _UNKNOWN_ID;
 	unsigned long rate = 0;
 
-	p = stm32mp1_clk_get_parent(id);
+	p = __clk_get_parent(id);
 	if (p < 0)
 		return 0;
 
-	rate = get_clock_rate(p);
+	rate = __clk_get_parent_rate(p);
 
 	if ((id >= TIM2_K) && (id <= TIM14_K))
 		rate = get_timer_rate(rate, 1);
@@ -1210,7 +1467,7 @@ static void secure_parent_clocks(unsigned long parent_id)
 
 void stm32mp_register_clock_parents_secure(unsigned long clock_id)
 {
-	int parent_id = stm32mp1_clk_get_parent(clock_id);
+	enum stm32mp1_parent_id parent_id = __clk_get_parent(clock_id);
 
 	if (parent_id < 0) {
 		DMSG("No parent for clock %lu", clock_id);
@@ -1220,6 +1477,14 @@ void stm32mp_register_clock_parents_secure(unsigned long clock_id)
 	secure_parent_clocks(parent_id);
 }
 
+static const struct clk_ops stm32mp_clk_ops = {
+	.enable		= clk_stm32_enable,
+	.disable	= clk_stm32_disable,
+	.is_enabled	= clk_stm32_is_enabled,
+	.get_rate	= clk_stm32_get_rate,
+};
+DECLARE_KEEP_PAGER(stm32mp_clk_ops);
+
 #ifdef CFG_EMBED_DTB
 static const char *stm32mp_osc_node_label[NB_OSC] = {
 	[_LSI] = "clk-lsi",
@@ -1291,15 +1556,15 @@ static void enable_static_secure_clocks(void)
 	};
 
 	for (idx = 0; idx < ARRAY_SIZE(secure_enable); idx++) {
-		stm32_clock_enable(secure_enable[idx]);
+		clk_stm32_enable(secure_enable[idx]);
 		stm32mp_register_clock_parents_secure(secure_enable[idx]);
 	}
 
 	if (CFG_TEE_CORE_NB_CORE > 1)
-		stm32_clock_enable(RTCAPB);
+		clk_stm32_enable(RTCAPB);
 }
 
-static TEE_Result stm32mp1_clk_early_init(void)
+static void stm32mp1_clk_early_init(void)
 {
 	void *fdt = NULL;
 	int node = 0;
@@ -1308,18 +1573,21 @@ static TEE_Result stm32mp1_clk_early_init(void)
 	int ignored = 0;
 
 	fdt = get_embedded_dt();
-	node = fdt_node_offset_by_compatible(fdt, -1, DT_RCC_CLK_COMPAT);
-
-	if (node < 0 || _fdt_reg_base_address(fdt, node) != RCC_BASE)
-		panic();
-
-	if (_fdt_get_status(fdt, node) & DT_STATUS_OK_SEC) {
-		io_setbits32(stm32_rcc_base() + RCC_TZCR, RCC_TZCR_TZEN);
+	node = fdt_node_offset_by_compatible(fdt, -1, DT_RCC_SEC_CLK_COMPAT);
+
+	if (node < 0 || _fdt_reg_base_address(fdt, node) != RCC_BASE) {
+		/* Check non secure compatible */
+		node = fdt_node_offset_by_compatible(fdt, -1,
+						     DT_RCC_CLK_COMPAT);
+		if (node < 0 || _fdt_reg_base_address(fdt, node) != RCC_BASE) {
+			panic();
+		} else {
+			io_clrbits32(stm32_rcc_base() + RCC_TZCR,
+				     RCC_TZCR_TZEN);
+			IMSG("RCC is non secure");
+		}
 	} else {
-		if (io_read32(stm32_rcc_base() + RCC_TZCR) & RCC_TZCR_TZEN)
-			panic("Refuse to release RCC[TZEN]");
-
-		IMSG("RCC is non-secure");
+		io_setbits32(stm32_rcc_base() + RCC_TZCR, RCC_TZCR_TZEN);
 	}
 
 	get_osc_freq_from_dt(fdt);
@@ -1360,11 +1628,1024 @@ static TEE_Result stm32mp1_clk_early_init(void)
 
 	if (ignored != 0)
 		IMSG("DT clock tree configurations were ignored");
+}
 
-	enable_static_secure_clocks();
+/*
+ * Gets OPP parameters (frequency in KHz and voltage in mV) from an OPP table
+ * subnode. Platform HW support capabilities are also checked.
+ */
+static int get_opp_freqvolt_from_dt_subnode(void *fdt, int subnode,
+					    uint32_t *freq_khz,
+					    uint32_t *voltage_mv)
+{
+	const fdt64_t *cuint64 = NULL;
+	const fdt32_t *cuint32 = NULL;
+	uint64_t read_freq_64 = 0;
+	uint32_t read_voltage_32 = 0;
+
+	assert(freq_khz);
+	assert(voltage_mv);
+
+	cuint32 = fdt_getprop(fdt, subnode, "opp-supported-hw", NULL);
+	if (cuint32)
+		if (!stm32mp_supports_cpu_opp(fdt32_to_cpu(*cuint32))) {
+			DMSG("Invalid opp-supported-hw 0x%"PRIx32,
+			     fdt32_to_cpu(*cuint32));
+			return -FDT_ERR_BADVALUE;
+		}
 
-	return TEE_SUCCESS;
+	cuint64 = fdt_getprop(fdt, subnode, "opp-hz", NULL);
+	if (!cuint64) {
+		DMSG("Missing opp-hz");
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	/* Frequency value expressed in KHz must fit on 32 bits */
+	read_freq_64 = fdt64_to_cpu(*cuint64) / 1000ULL;
+	if (read_freq_64 > (uint64_t)UINT32_MAX) {
+		DMSG("Invalid opp-hz %"PRIu64, read_freq_64);
+		return -FDT_ERR_BADVALUE;
+	}
+
+	cuint32 = fdt_getprop(fdt, subnode, "opp-microvolt", NULL);
+	if (!cuint32) {
+		DMSG("Missing opp-microvolt");
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	/* Millivolt value must fit on 16 bits */
+	read_voltage_32 = fdt32_to_cpu(*cuint32) / 1000U;
+	if (read_voltage_32 > UINT16_MAX) {
+		DMSG("Invalid opp-microvolt %"PRIu32, read_voltage_32);
+		return -FDT_ERR_BADVALUE;
+	}
+
+	*freq_khz = (uint32_t)read_freq_64;
+
+	*voltage_mv = read_voltage_32;
+
+	return 0;
+}
+
+/*
+ * Parses OPP table in DT and finds all parameters supported by the HW
+ * platform. If found, the corresponding frequency and voltage values are
+ * respectively stored in @pll1_settings structure.
+ * Note that @*count has to be set by caller to the effective size allocated
+ * for both tables. Its value is then replaced by the number of filled elements.
+ */
+static int get_all_opp_freqvolt_from_dt(uint32_t *count)
+{
+	void *fdt = NULL;
+	int node = 0;
+	int subnode = 0;
+	uint32_t idx = 0;
+
+	assert(count);
+
+	fdt = get_embedded_dt();
+	node = fdt_node_offset_by_compatible(fdt, -1, DT_OPP_COMPAT);
+	if (node < 0)
+		return node;
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		uint32_t read_freq = 0;
+		uint32_t read_voltage = 0;
+
+		if (get_opp_freqvolt_from_dt_subnode(fdt, subnode, &read_freq,
+						     &read_voltage))
+			continue;
+
+		if (idx >= *count)
+			return -FDT_ERR_NOSPACE;
+
+		pll1_settings.freq[idx] = read_freq;
+		pll1_settings.volt[idx] = read_voltage;
+		idx++;
+	}
+
+	if (!idx)
+		return -FDT_ERR_NOTFOUND;
+
+	*count = idx;
+
+	return 0;
+}
+
+static int clk_compute_pll1_settings(unsigned long input_freq, int idx)
+{
+	unsigned long post_divm = 0;
+	unsigned long long output_freq = pll1_settings.freq[idx] * 1000U;
+	unsigned long long freq = 0;
+	unsigned long long vco = 0;
+	int divm = 0;
+	int divn = 0;
+	int divp = 0;
+	int frac = 0;
+	int i = 0;
+	unsigned int diff = 0;
+	unsigned int best_diff = UINT_MAX;
+
+	/* Following parameters have always the same value */
+	pll1_settings.cfg[idx][PLLCFG_Q] = 0;
+	pll1_settings.cfg[idx][PLLCFG_R] = 0;
+	pll1_settings.cfg[idx][PLLCFG_O] = PQR(1, 0, 0);
+
+	for (divm = DIVM_MAX; divm >= DIVM_MIN; divm--)	{
+		post_divm = input_freq / (unsigned long)(divm + 1);
+
+		if ((post_divm < POST_DIVM_MIN) ||
+		    (post_divm > POST_DIVM_MAX))
+			continue;
+
+		for (divp = DIVP_MIN; divp <= DIVP_MAX; divp++) {
+
+			freq = output_freq * (divm + 1) * (divp + 1);
+
+			divn = (int)((freq / input_freq) - 1);
+			if ((divn < DIVN_MIN) || (divn > DIVN_MAX))
+				continue;
+
+			frac = (int)(((freq * FRAC_MAX) / input_freq) -
+				     ((divn + 1) * FRAC_MAX));
+
+			/* 2 loops to refine the fractional part */
+			for (i = 2; i != 0; i--) {
+				if (frac > FRAC_MAX)
+					break;
+
+				vco = (post_divm * (divn + 1)) +
+				      ((post_divm * (unsigned long long)frac) /
+				       FRAC_MAX);
+
+				if ((vco < (VCO_MIN / 2)) ||
+				    (vco > (VCO_MAX / 2))) {
+					frac++;
+					continue;
+				}
+
+				freq = vco / (divp + 1);
+				if (output_freq < freq)
+					diff = (unsigned int)(freq -
+							      output_freq);
+				else
+					diff = (unsigned int)(output_freq -
+							      freq);
+
+				if (diff < best_diff)  {
+					pll1_settings.cfg[idx][PLLCFG_M] = divm;
+					pll1_settings.cfg[idx][PLLCFG_N] = divn;
+					pll1_settings.cfg[idx][PLLCFG_P] = divp;
+					pll1_settings.frac[idx] = frac;
+
+					if (!diff)
+						return 0;
+
+					best_diff = diff;
+				}
+
+				frac++;
+			}
+		}
+	}
+
+	if (best_diff == UINT_MAX) {
+		pll1_settings.cfg[idx][PLLCFG_O] = 0;
+		return -1;
+	}
+
+	return 0;
+}
+
+static int clk_get_pll1_settings(uint32_t clksrc, int index)
+{
+	unsigned long input_freq = 0;
+	unsigned int i = 0;
+
+	for (i = 0; i < PLAT_MAX_OPP_NB; i++)
+		if (pll1_settings.freq[i] == pll1_settings.freq[index])
+			break;
+
+	if (((i == PLAT_MAX_OPP_NB) &&
+	     !stm32mp1_clk_pll1_settings_are_valid()) ||
+	    ((i < PLAT_MAX_OPP_NB) && !pll1_settings.cfg[i][PLLCFG_O])) {
+		/*
+		 * Either PLL1 settings structure is completely empty,
+		 * or these settings are not yet computed: do it.
+		 */
+		switch (clksrc) {
+		case CLK_PLL12_HSI:
+			input_freq = clk_stm32_get_rate(CK_HSI);
+			break;
+		case CLK_PLL12_HSE:
+			input_freq = clk_stm32_get_rate(CK_HSE);
+			break;
+		default:
+			panic();
+		}
+
+		return clk_compute_pll1_settings(input_freq, index);
+	}
+
+	if (i < PLAT_MAX_OPP_NB) {
+		if (pll1_settings.cfg[i][PLLCFG_O])
+			return 0;
+
+		/*
+		 * Index is in range and PLL1 settings are computed:
+		 * use content to answer to the request.
+		 */
+		memcpy(&pll1_settings.cfg[index][0], &pll1_settings.cfg[i][0],
+		       sizeof(uint32_t) * PLAT_MAX_PLLCFG_NB);
+		pll1_settings.frac[index] = pll1_settings.frac[i];
+
+		return 0;
+	}
+
+	return -1;
+}
+
+static int clk_save_current_pll1_settings(uint32_t buck1_voltage)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(_PLL1);
+	uint32_t rcc_base = stm32_rcc_base();
+	uint32_t freq = 0;
+	unsigned int i = 0;
+
+	freq = UDIV_ROUND_NEAREST(clk_stm32_get_rate(CK_MPU), 1000L);
+
+	for (i = 0; i < PLAT_MAX_OPP_NB; i++)
+		if (pll1_settings.freq[i] == freq)
+			break;
+
+	if ((i == PLAT_MAX_OPP_NB) ||
+	    ((pll1_settings.volt[i] != buck1_voltage) && buck1_voltage))
+		return -1;
+
+	pll1_settings.cfg[i][PLLCFG_M] = (io_read32(rcc_base + pll->pllxcfgr1) &
+					  RCC_PLLNCFGR1_DIVM_MASK) >>
+					 RCC_PLLNCFGR1_DIVM_SHIFT;
+
+	pll1_settings.cfg[i][PLLCFG_N] = (io_read32(rcc_base + pll->pllxcfgr1) &
+					  RCC_PLLNCFGR1_DIVN_MASK) >>
+					 RCC_PLLNCFGR1_DIVN_SHIFT;
+
+	pll1_settings.cfg[i][PLLCFG_P] = (io_read32(rcc_base + pll->pllxcfgr2) &
+					  RCC_PLLNCFGR2_DIVP_MASK) >>
+					 RCC_PLLNCFGR2_DIVP_SHIFT;
+
+	pll1_settings.cfg[i][PLLCFG_Q] = (io_read32(rcc_base + pll->pllxcfgr2) &
+					  RCC_PLLNCFGR2_DIVQ_MASK) >>
+					 RCC_PLLNCFGR2_DIVQ_SHIFT;
+
+	pll1_settings.cfg[i][PLLCFG_R] = (io_read32(rcc_base + pll->pllxcfgr2) &
+					  RCC_PLLNCFGR2_DIVR_MASK) >>
+					 RCC_PLLNCFGR2_DIVR_SHIFT;
+
+	pll1_settings.cfg[i][PLLCFG_O] = io_read32(rcc_base + pll->pllxcr) >>
+					 RCC_PLLNCR_DIVEN_SHIFT;
+
+	pll1_settings.frac[i] = (io_read32(rcc_base + pll->pllxfracr) &
+				 RCC_PLLNFRACR_FRACV_MASK) >>
+				RCC_PLLNFRACR_FRACV_SHIFT;
+
+	return i;
+}
+
+static uint32_t stm32mp1_clk_get_pll1_current_clksrc(void)
+{
+	uint32_t value = 0;
+	const struct stm32mp1_clk_pll *pll = pll_ref(_PLL1);
+	uint32_t rcc_base = stm32_rcc_base();
+
+	value = io_read32(rcc_base + pll->rckxselr);
+
+	switch (value & RCC_SELR_REFCLK_SRC_MASK) {
+	case 0:
+		return CLK_PLL12_HSI;
+	case 1:
+		return CLK_PLL12_HSE;
+	default:
+		panic();
+	}
+}
+
+int stm32mp1_clk_compute_all_pll1_settings(uint32_t buck1_voltage)
+{
+	unsigned int i = 0;
+	int ret = 0;
+	int index = 0;
+	uint32_t count = PLAT_MAX_OPP_NB;
+	uint32_t clksrc = 0;
+
+	ret = get_all_opp_freqvolt_from_dt(&count);
+	switch (ret) {
+	case 0:
+		break;
+	case -FDT_ERR_NOTFOUND:
+		DMSG("Cannot find all OPP info in DT: use default settings.");
+		return 0;
+	default:
+		EMSG("Inconsistent OPP settings found in DT, ignored.");
+		return 0;
+	}
+
+	index = clk_save_current_pll1_settings(buck1_voltage);
+
+	clksrc = stm32mp1_clk_get_pll1_current_clksrc();
+
+	for (i = 0; i < count; i++) {
+		if (index >= 0 && i == (unsigned int)index)
+			continue;
+
+		ret = clk_get_pll1_settings(clksrc, i);
+		if (ret != 0)
+			return ret;
+	}
+
+	pll1_settings.valid_id = PLL1_SETTINGS_VALID_ID;
+
+	return 0;
+}
+
+void stm32mp1_clk_lp_save_opp_pll1_settings(uint8_t *data, size_t size)
+{
+	if ((size != sizeof(pll1_settings)) ||
+	    !stm32mp1_clk_pll1_settings_are_valid())
+		panic();
+
+	memcpy(data, &pll1_settings, size);
+}
+
+bool stm32mp1_clk_pll1_settings_are_valid(void)
+{
+	return pll1_settings.valid_id == PLL1_SETTINGS_VALID_ID;
+}
+#else
+static void stm32mp1_clk_early_init(void)
+{
+	vaddr_t rcc_base = stm32_rcc_base();
+
+	/* Expect booting from a secure setup */
+	if ((io_read32(rcc_base + RCC_TZCR) & RCC_TZCR_TZEN) == 0)
+		panic("RCC TZC[TZEN]");
+}
+
+int stm32mp1_clk_compute_all_pll1_settings(uint32_t buck1_voltage __unused)
+{
+	return 0;
+}
+
+void stm32mp1_clk_lp_save_opp_pll1_settings(uint8_t *data __unused,
+					    size_t size __unused)
+{
+}
+
+bool stm32mp1_clk_pll1_settings_are_valid(void)
+{
+	return false;
 }
 
-service_init(stm32mp1_clk_early_init);
+static void enable_static_secure_clocks(void)
+{
+}
 #endif /*CFG_EMBED_DTB*/
+
+/* Start MPU OPP */
+#define CLKSRC_TIMEOUT_US	(200 * 1000)
+#define CLKDIV_TIMEOUT_US	(200 * 1000)
+#define CLK_MPU_PLL1P		0x00000202
+#define CLK_MPU_PLL1P_DIV	0x00000203
+
+static int stm32mp1_set_clksrc(unsigned int clksrc)
+{
+	uintptr_t address = stm32_rcc_base() + (clksrc >> 4);
+	uint64_t timeout_ref = 0;
+
+	io_clrsetbits32(address, RCC_SELR_SRC_MASK, clksrc & RCC_SELR_SRC_MASK);
+
+	timeout_ref = timeout_init_us(CLKSRC_TIMEOUT_US);
+	while ((io_read32(address) & RCC_SELR_SRCRDY) == 0U) {
+		if (timeout_elapsed(timeout_ref)) {
+			EMSG("CLKSRC %u start failed @ 0x%"PRIxPTR": 0x%"PRIx32,
+			      clksrc, address, io_read32(address));
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+static int stm32mp1_set_clkdiv(unsigned int clkdiv, uintptr_t address)
+{
+	uint64_t timeout_ref = 0;
+
+	io_clrsetbits32(address, RCC_DIVR_DIV_MASK, clkdiv & RCC_DIVR_DIV_MASK);
+
+	timeout_ref = timeout_init_us(CLKDIV_TIMEOUT_US);
+	while ((io_read32(address) & RCC_DIVR_DIVRDY) == 0U) {
+		if (timeout_elapsed(timeout_ref)) {
+			EMSG("CLKDIV 0x%x start failed @ 0x%"PRIxPTR": 0x%"PRIx32,
+			     clkdiv, address, io_read32(address));
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * Check if PLL1 can be configured on the fly.
+ * @result  (-1) => config on the fly is not possible.
+ *          (0)  => config on the fly is possible.
+ *          (+1) => same parameters as those in place, no need to reconfig.
+ * Return value is 0 if no error.
+ */
+static int is_pll_config_on_the_fly(enum stm32mp1_pll_id pll_id,
+				    uint32_t *pllcfg, uint32_t fracv,
+				    int *result)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uintptr_t rcc_base = stm32_rcc_base();
+	uint32_t fracr = 0;
+	uint32_t value = 0;
+	int ret = 0;
+
+	ret = pll_compute_pllxcfgr1(pll, pllcfg, &value);
+	if (ret)
+		return ret;
+
+	if (io_read32(rcc_base + pll->pllxcfgr1) != value) {
+		/* Different DIVN/DIVM, can't config on the fly */
+		*result = -1;
+		return 0;
+	}
+
+	*result = true;
+
+	fracr = fracv << RCC_PLLNFRACR_FRACV_SHIFT;
+	fracr |= RCC_PLLNFRACR_FRACLE;
+	value = pll_compute_pllxcfgr2(pllcfg);
+
+	if ((io_read32(rcc_base + pll->pllxfracr) == fracr) &&
+	    (io_read32(rcc_base + pll->pllxcfgr2) == value))
+		/* Same parameters, no need to config */
+		*result = 1;
+	else
+		*result = 0;
+
+	return 0;
+}
+
+static int stm32mp1_get_mpu_div(uint32_t freq_khz)
+{
+	unsigned long freq_pll1_p;
+	unsigned long div;
+
+	freq_pll1_p = __clk_get_parent_rate(_PLL1_P) / 1000UL;
+	if ((freq_pll1_p % freq_khz) != 0U)
+		return -1;
+
+	div = freq_pll1_p / freq_khz;
+
+	switch (div) {
+	case 1UL:
+	case 2UL:
+	case 4UL:
+	case 8UL:
+	case 16UL:
+		return __builtin_ffs(div) - 1;
+	default:
+		return -1;
+	}
+}
+
+/* Configure PLL1 from input frequency OPP parameters */
+static int pll1_config_from_opp_khz(uint32_t freq_khz)
+{
+	unsigned int idx = 0;
+	int ret = 0;
+	int div = 0;
+	int config_on_the_fly = -1;
+
+	for (idx = 0; idx < PLAT_MAX_OPP_NB; idx++)
+		if (pll1_settings.freq[idx] == freq_khz)
+			break;
+
+	if (idx == PLAT_MAX_OPP_NB)
+		return -1;
+
+	div = stm32mp1_get_mpu_div(freq_khz);
+	switch (div) {
+	case -1:
+		break;
+	case 0:
+		return stm32mp1_set_clksrc(CLK_MPU_PLL1P);
+	default:
+		ret = stm32mp1_set_clkdiv(div, stm32_rcc_base() +
+					  RCC_MPCKDIVR);
+		if (ret == 0)
+			ret = stm32mp1_set_clksrc(CLK_MPU_PLL1P_DIV);
+
+		return ret;
+	}
+
+	ret = is_pll_config_on_the_fly(_PLL1, &pll1_settings.cfg[idx][0],
+				       pll1_settings.frac[idx],
+				       &config_on_the_fly);
+	if (ret)
+		return ret;
+
+	if (config_on_the_fly == 1)
+		return 0;
+
+	if (config_on_the_fly == -1) {
+		/* Switch to HSI and stop PLL1 before reconfiguration */
+		ret = stm32mp1_set_clksrc(CLK_MPU_HSI);
+		if (ret)
+			return ret;
+
+		ret = pll_stop(_PLL1);
+		if (ret)
+			return ret;
+	}
+
+	ret = pll_config(_PLL1, &pll1_settings.cfg[idx][0],
+			 pll1_settings.frac[idx]);
+	if (ret)
+		return ret;
+
+	if (config_on_the_fly == -1) {
+		/* Start PLL1 and switch back to after reconfiguration */
+		pll_start(_PLL1);
+
+		ret = pll_output(_PLL1, pll1_settings.cfg[idx][PLLCFG_O]);
+		if (ret)
+			return ret;
+
+		ret = stm32mp1_set_clksrc(CLK_MPU_PLL1P);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static void save_current_opp(void)
+{
+	unsigned long freq_khz = UDIV_ROUND_NEAREST(clk_stm32_get_rate(CK_MPU),
+						    1000UL);
+	if (freq_khz > (unsigned long)UINT32_MAX)
+		panic();
+
+	current_opp_khz = (uint32_t)freq_khz;
+}
+
+int stm32mp1_set_opp_khz(uint32_t freq_khz)
+{
+	uint32_t mpu_src = 0;
+
+	if (freq_khz == current_opp_khz)
+		return 0;
+
+	if (!stm32mp1_clk_pll1_settings_are_valid()) {
+		/*
+		 * No OPP table in DT or an error occurred during PLL1
+		 * settings computation, system can only work on current
+		 * operating point so return error.
+		 */
+		return -1;
+	}
+
+	/* Check that PLL1 is MPU clock source */
+	mpu_src = io_read32(stm32_rcc_base() + RCC_MPCKSELR) &
+		RCC_SELR_SRC_MASK;
+	if ((mpu_src != RCC_MPCKSELR_PLL) &&
+	    (mpu_src != RCC_MPCKSELR_PLL_MPUDIV))
+		return -1;
+
+	if (pll1_config_from_opp_khz(freq_khz)) {
+		/* Restore original value */
+		if (pll1_config_from_opp_khz(current_opp_khz)) {
+			EMSG("No CPU operating point can be set");
+			panic();
+		}
+
+		return -1;
+	}
+
+	current_opp_khz = freq_khz;
+
+	return 0;
+}
+
+int stm32mp1_round_opp_khz(uint32_t *freq_khz)
+{
+	unsigned int i = 0;
+	uint32_t round_opp = 0;
+
+	if (!stm32mp1_clk_pll1_settings_are_valid()) {
+		/*
+		 * No OPP table in DT, or an error occurred during PLL1
+		 * settings computation, system can only work on current
+		 * operating point, so return current CPU frequency.
+		 */
+		*freq_khz = current_opp_khz;
+
+		return 0;
+	}
+
+	for (i = 0; i < PLAT_MAX_OPP_NB; i++)
+		if ((pll1_settings.freq[i] <= *freq_khz) &&
+		    (pll1_settings.freq[i] > round_opp))
+			round_opp = pll1_settings.freq[i];
+
+	*freq_khz = round_opp;
+
+	return 0;
+}
+/* End PMU OPP */
+
+#ifdef CFG_PM
+struct soc_stop_context {
+	uint32_t pll3cr;
+	uint32_t pll4cr;
+	uint32_t mssckselr;
+	uint32_t mcudivr;
+};
+
+static struct soc_stop_context soc_stop_ctx;
+
+static void save_pll34_state(void)
+{
+	uintptr_t rcc_base = stm32_rcc_base();
+	struct soc_stop_context *ctx = &soc_stop_ctx;
+
+	ctx->pll3cr = io_read32(rcc_base + RCC_PLL3CR);
+	ctx->pll4cr = io_read32(rcc_base + RCC_PLL4CR);
+}
+
+static void save_mcu_subsys_clocks(void)
+{
+	uintptr_t rcc_base = stm32_rcc_base();
+	struct soc_stop_context *ctx = &soc_stop_ctx;
+
+	ctx->mssckselr = io_read32(rcc_base + RCC_MSSCKSELR);
+	ctx->mcudivr = io_read32(rcc_base + RCC_MCUDIVR) &
+		       RCC_MCUDIV_MASK;
+}
+
+static void restore_pll34_state(void)
+{
+	struct soc_stop_context *ctx = &soc_stop_ctx;
+
+	/* Let PLL4 start while we're starting and waiting for PLL3 */
+	if (ctx->pll4cr & RCC_PLLNCR_PLLON)
+		pll_start(_PLL4);
+
+	if (ctx->pll3cr & RCC_PLLNCR_PLLON) {
+		pll_start(_PLL3);
+		if (pll_output(_PLL3, ctx->pll3cr >> RCC_PLLNCR_DIVEN_SHIFT)) {
+			EMSG("Failed to restore PLL3");
+			panic();
+		}
+	}
+
+	if (ctx->pll4cr & RCC_PLLNCR_PLLON) {
+		if (pll_output(_PLL4, ctx->pll4cr >> RCC_PLLNCR_DIVEN_SHIFT)) {
+			EMSG("Failed to restore PLL4");
+			panic();
+		}
+	}
+}
+
+static void restore_mcu_subsys_clocks(void)
+{
+	uintptr_t rcc_base = stm32_rcc_base();
+	struct soc_stop_context *ctx = &soc_stop_ctx;
+
+	io_write32(rcc_base + RCC_MSSCKSELR, ctx->mssckselr);
+
+	if (stm32mp1_set_clkdiv(ctx->mcudivr, rcc_base + RCC_MCUDIVR)) {
+		EMSG("Failed to restore MCUDIVR");
+		panic();
+	}
+}
+
+/*
+ * Sequence to save/restore the non-secure configuration.
+ * Restoring clocks and muxes need IPs to run on kernel clock
+ * hence on configuration is restored at resume, kernel clock
+ * should be disable: this mandates secure access.
+ *
+ * backup_mux*_cfg for the clock muxes.
+ * backup_clock_sc_cfg for the set/clear clock gating registers
+ * backup_clock_cfg for the regular full write registers
+ */
+
+struct backup_mux_cfg {
+	uint16_t offset;
+	uint8_t value;
+	uint8_t bit_len;
+};
+
+#define MUXCFG(_offset, _bit_len) \
+	{ .offset = (_offset), .bit_len = (_bit_len) }
+
+struct backup_mux_cfg backup_mux0_cfg[] = {
+	MUXCFG(RCC_SDMMC12CKSELR, 3),
+	MUXCFG(RCC_SPI2S23CKSELR, 3),
+	MUXCFG(RCC_SPI45CKSELR, 3),
+	MUXCFG(RCC_I2C12CKSELR, 3),
+	MUXCFG(RCC_I2C35CKSELR, 3),
+	MUXCFG(RCC_LPTIM23CKSELR, 3),
+	MUXCFG(RCC_LPTIM45CKSELR, 3),
+	MUXCFG(RCC_UART24CKSELR, 3),
+	MUXCFG(RCC_UART35CKSELR, 3),
+	MUXCFG(RCC_UART78CKSELR, 3),
+	MUXCFG(RCC_SAI1CKSELR, 3),
+	MUXCFG(RCC_ETHCKSELR, 2),
+	MUXCFG(RCC_I2C46CKSELR, 3),
+	MUXCFG(RCC_RNG2CKSELR, 2),
+	MUXCFG(RCC_SDMMC3CKSELR, 3),
+	MUXCFG(RCC_FMCCKSELR, 2),
+	MUXCFG(RCC_QSPICKSELR, 2),
+	MUXCFG(RCC_USBCKSELR, 2),
+	MUXCFG(RCC_SPDIFCKSELR, 2),
+	MUXCFG(RCC_SPI2S1CKSELR, 3),
+	MUXCFG(RCC_CECCKSELR, 2),
+	MUXCFG(RCC_LPTIM1CKSELR, 3),
+	MUXCFG(RCC_UART6CKSELR, 3),
+	MUXCFG(RCC_FDCANCKSELR, 2),
+	MUXCFG(RCC_SAI2CKSELR, 3),
+	MUXCFG(RCC_SAI3CKSELR,  3),
+	MUXCFG(RCC_SAI4CKSELR, 3),
+	MUXCFG(RCC_ADCCKSELR, 2),
+	MUXCFG(RCC_DSICKSELR, 1),
+	MUXCFG(RCC_CPERCKSELR, 2),
+	MUXCFG(RCC_RNG1CKSELR, 2),
+	MUXCFG(RCC_STGENCKSELR, 2),
+	MUXCFG(RCC_UART1CKSELR, 3),
+	MUXCFG(RCC_SPI6CKSELR, 3),
+};
+
+struct backup_mux_cfg backup_mux4_cfg[] = {
+	MUXCFG(RCC_USBCKSELR, 1),
+};
+
+static void backup_mux_cfg(void)
+{
+	struct backup_mux_cfg *cfg = backup_mux0_cfg;
+	size_t count = ARRAY_SIZE(backup_mux0_cfg);
+	size_t i = 0;
+	uintptr_t base = stm32_rcc_base();
+
+	for (i = 0; i < count; i++)
+		cfg[i].value = io_read32(base + cfg[i].offset) &
+				GENMASK_32(cfg[i].bit_len - 1, 0);
+
+	cfg = backup_mux4_cfg;
+	count = ARRAY_SIZE(backup_mux4_cfg);
+
+	for (i = 0; i < count; i++)
+		cfg[i].value = io_read32(base + cfg[i].offset) &
+				GENMASK_32(4 + cfg[i].bit_len - 1, 4);
+}
+
+static void restore_mux_cfg(void)
+{
+	struct backup_mux_cfg *cfg = backup_mux0_cfg;
+	size_t count = ARRAY_SIZE(backup_mux0_cfg);
+	size_t i = 0;
+	uintptr_t base = stm32_rcc_base();
+
+	for (i = 0; i < count; i++)
+		io_clrsetbits32(base + cfg[i].offset,
+				GENMASK_32(cfg[i].bit_len - 1, 0),
+				cfg[i].value);
+
+	cfg = backup_mux4_cfg;
+	count = ARRAY_SIZE(backup_mux4_cfg);
+
+	for (i = 0; i < count; i++)
+		 io_clrsetbits32(base + cfg[i].offset,
+				 GENMASK_32(4 + cfg[i].bit_len - 1, 4),
+				 cfg[i].value);
+}
+
+/* Structure is used for set/clear registers and for regular registers */
+struct backup_clock_cfg {
+	uint32_t offset;
+	uint32_t value;
+};
+
+static struct backup_clock_cfg backup_clock_sc_cfg[] = {
+	{ .offset = RCC_MP_APB1ENSETR },
+	{ .offset = RCC_MP_APB2ENSETR },
+	{ .offset = RCC_MP_APB3ENSETR },
+	{ .offset = RCC_MP_APB4ENSETR },
+	{ .offset = RCC_MP_APB5ENSETR },
+	{ .offset = RCC_MP_AHB2ENSETR },
+	{ .offset = RCC_MP_AHB3ENSETR },
+	{ .offset = RCC_MP_AHB4ENSETR },
+	{ .offset = RCC_MP_AHB5ENSETR },
+	{ .offset = RCC_MP_AHB6ENSETR },
+	{ .offset = RCC_MP_MLAHBENSETR },
+};
+
+static struct backup_clock_cfg backup_clock_cfg[] = {
+	{ .offset = RCC_TZCR},
+	{ .offset = RCC_MCO1CFGR },
+	{ .offset = RCC_MCO2CFGR },
+	{ .offset = RCC_PLL3CR },
+	{ .offset = RCC_PLL4CR },
+	{ .offset = RCC_PLL4CFGR2 },
+	{ .offset = RCC_MCUDIVR },
+	{ .offset = RCC_MSSCKSELR },
+};
+
+static void backup_sc_cfg(void)
+{
+	struct backup_clock_cfg *cfg = backup_clock_sc_cfg;
+	size_t count = ARRAY_SIZE(backup_clock_sc_cfg);
+	size_t i = 0;
+	uintptr_t base = stm32_rcc_base();
+
+	for (i = 0; i < count; i++)
+		cfg[i].value = io_read32(base + cfg[i].offset);
+}
+
+static void restore_sc_cfg(void)
+{
+	struct backup_clock_cfg *cfg = backup_clock_sc_cfg;
+	size_t count = ARRAY_SIZE(backup_clock_sc_cfg);
+	size_t i = 0;
+	uintptr_t base = stm32_rcc_base();
+
+	for (i = 0; i < count; i++) {
+		io_write32(base + cfg[i].offset, cfg[i].value);
+		io_write32(base + cfg[i].offset + RCC_MP_ENCLRR_OFFSET,
+			   ~cfg[i].value);
+	}
+}
+
+static void backup_regular_cfg(void)
+{
+	struct backup_clock_cfg *cfg = backup_clock_cfg;
+	size_t count = ARRAY_SIZE(backup_clock_cfg);
+	size_t i = 0;
+	uintptr_t base = stm32_rcc_base();
+
+	for (i = 0; i < count; i++)
+		cfg[i].value = io_read32(base + cfg[i].offset);
+}
+
+static void restore_regular_cfg(void)
+{
+	struct backup_clock_cfg *cfg = backup_clock_cfg;
+	size_t count = ARRAY_SIZE(backup_clock_cfg);
+	size_t i = 0;
+	uintptr_t base = stm32_rcc_base();
+
+	for (i = 0; i < count; i++)
+		io_write32(base + cfg[i].offset, cfg[i].value);
+}
+
+static void disable_kernel_clocks(void)
+{
+	const uint32_t ker_mask = RCC_OCENR_HSIKERON |
+				  RCC_OCENR_CSIKERON |
+				  RCC_OCENR_HSEKERON;
+
+	/* Disable all ck_xxx_ker clocks */
+	io_write32(stm32_rcc_base() + RCC_OCENCLRR, ker_mask);
+}
+
+static void enable_kernel_clocks(void)
+{
+	uintptr_t rcc_base = stm32_rcc_base();
+	uint32_t reg = 0;
+	const uint32_t ker_mask = RCC_OCENR_HSIKERON |
+				  RCC_OCENR_CSIKERON |
+				  RCC_OCENR_HSEKERON;
+
+	/* Enable ck_xxx_ker clocks if ck_xxx was on */
+	reg = io_read32(rcc_base + RCC_OCENSETR) << 1;
+	io_write32(rcc_base + RCC_OCENSETR, reg & ker_mask);
+}
+
+static void clear_rcc_reset_status(void)
+{
+	/* Clear reset status fields */
+	io_write32(stm32_rcc_base() + RCC_MP_RSTSCLRR, 0);
+}
+
+void stm32mp1_clk_save_context_for_stop(void)
+{
+	enable_kernel_clocks();
+	save_mcu_subsys_clocks();
+	save_pll34_state();
+}
+
+void stm32mp1_clk_restore_context_for_stop(void)
+{
+	restore_pll34_state();
+	/* Restore MCU clock source after PLL3 is ready */
+	restore_mcu_subsys_clocks();
+	disable_kernel_clocks();
+}
+
+void stm32mp1_clk_mcuss_protect(bool enable)
+{
+	uintptr_t rcc_base = stm32_rcc_base();
+
+	if (enable)
+		io_setbits32(rcc_base + RCC_TZCR, RCC_TZCR_MCKPROT);
+	else
+		io_clrbits32(rcc_base + RCC_TZCR, RCC_TZCR_MCKPROT);
+}
+
+static void stm32_clock_suspend(void)
+{
+	backup_regular_cfg();
+	backup_sc_cfg();
+	backup_mux_cfg();
+	save_pll34_state();
+
+	enable_kernel_clocks();
+	clear_rcc_reset_status();
+}
+
+static void stm32_clock_resume(void)
+{
+	unsigned int idx = 0;
+
+	restore_pll34_state();
+	restore_mux_cfg();
+	restore_sc_cfg();
+	restore_regular_cfg();
+
+	/* 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 (gate_is_non_secure(gate))
+			continue;
+
+		if (gate_refcounts[idx]) {
+			DMSG("Force clock %d enable", gate->clock_id);
+			__clk_enable(gate);
+		} else {
+			DMSG("Force clock %d disable", gate->clock_id);
+			__clk_disable(gate);
+		}
+	}
+
+	disable_kernel_clocks();
+}
+
+static TEE_Result stm32_clock_pm(enum pm_op op, unsigned int pm_hint __unused,
+				 const struct pm_callback_handle *hdl __unused)
+{
+	if (op == PM_OP_SUSPEND)
+		stm32_clock_suspend();
+	else
+		stm32_clock_resume();
+
+	return TEE_SUCCESS;
+}
+DECLARE_KEEP_PAGER(stm32_clock_pm);
+#else
+static TEE_Result stm32_clock_pm(enum pm_op op __unused,
+				 unsigned int pm_hint __unused,
+				 const struct pm_callback_handle *hdl __unused)
+{
+	return TEE_ERROR_SECURITY;
+}
+#endif /*CFG_PM*/
+
+static void init_non_secure_rcc(void)
+{
+	uintptr_t rcc_base = stm32_rcc_base();
+
+	/*  Clear all interrupt flags and core stop requests */
+	io_write32(rcc_base + RCC_MP_CIFR, 0x110F1F);
+	io_write32(rcc_base + RCC_MP_SREQCLRR, 0x3);
+}
+
+static TEE_Result stm32_clk_probe(void)
+{
+	assert(PLLCFG_NB == PLAT_MAX_PLLCFG_NB);
+
+	stm32mp1_clk_early_init();
+	enable_static_secure_clocks();
+	save_current_opp();
+	init_non_secure_rcc();
+	register_pm_core_service_cb(stm32_clock_pm, NULL);
+
+	clk_provider_register(&stm32mp_clk_ops);
+
+	return TEE_SUCCESS;
+}
+/* Setup clock support before driver initialization */
+service_init(stm32_clk_probe);
diff --git a/core/drivers/clk/clk.c b/core/drivers/clk/clk.c
new file mode 100644
index 000000000..f6e5d8ecf
--- /dev/null
+++ b/core/drivers/clk/clk.c
@@ -0,0 +1,58 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ * Author(s): Ludovic Barre, <ludovic.barre@st.com> for STMicroelectronics.
+ */
+
+#include <assert.h>
+#include <drivers/clk.h>
+#include <stdbool.h>
+
+static const struct clk_ops *ops;
+
+TEE_Result clk_enable(unsigned long id)
+{
+	assert(ops && ops->enable);
+
+	return ops->enable(id);
+}
+
+void clk_disable(unsigned long id)
+{
+	assert(ops && ops->disable);
+
+	ops->disable(id);
+}
+
+unsigned long clk_get_rate(unsigned long id)
+{
+	assert(ops && ops->get_rate);
+
+	return ops->get_rate(id);
+}
+
+unsigned long clk_get_parent(unsigned long id)
+{
+	assert(ops);
+
+	if (ops->get_parent)
+		return ops->get_parent(id);
+
+	return CLK_UNKNOWN_ID;
+}
+
+bool clk_is_enabled(unsigned long id)
+{
+	assert(ops && ops->is_enabled);
+
+	return ops->is_enabled(id);
+}
+
+void clk_provider_register(const struct clk_ops *ops_ptr)
+{
+	assert(!ops && ops_ptr && ops_ptr->enable &&
+	       ops_ptr->disable && ops_ptr->get_rate &&
+	       ops_ptr->is_enabled);
+
+	ops = ops_ptr;
+}
diff --git a/core/drivers/clk/sub.mk b/core/drivers/clk/sub.mk
new file mode 100644
index 000000000..4caed574b
--- /dev/null
+++ b/core/drivers/clk/sub.mk
@@ -0,0 +1,2 @@
+srcs-$(CFG_CLK_DRIVER) += clk.c
+srcs-$(CFG_STM32MP15_CLK) += clk-stm32mp15.c
diff --git a/core/drivers/gic.c b/core/drivers/gic.c
index 0bad97486..3f1a606ed 100644
--- a/core/drivers/gic.c
+++ b/core/drivers/gic.c
@@ -7,12 +7,15 @@
 #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 <kernel/pm.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)
@@ -33,9 +36,11 @@
 #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_IPRIORITYR(n)	(0x400 + (n) * 4)
 #define GICD_ITARGETSR(n)	(0x800 + (n) * 4)
 #define GICD_IGROUPMODR(n)	(0xd00 + (n) * 4)
+#define GICD_ICFGR(n)		(0xc00 + (n) * 4)
 #define GICD_SGIR		(0xF00)
 
 #define GICD_CTLR_ENABLEGRP0	(1 << 0)
@@ -76,6 +81,25 @@ 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_GIC_PM)
+static void gic_pm_add_it(struct gic_data *gd, unsigned int it);
+static void gic_pm_register(struct gic_data *gd);
+#else
+static void gic_pm_add_it(struct gic_data *gd __unused,
+			  unsigned int it __unused)
+{
+}
+static void gic_pm_register(struct gic_data *gd __unused)
+{
+}
+#endif
+
+#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,
@@ -83,6 +107,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
 };
 DECLARE_KEEP_PAGER(gic_ops);
 
@@ -148,10 +176,10 @@ 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_igrpen1(1);
 #else
-	io_write32(gd->gicc_base + GICC_PMR, 0x80);
+	io_write32(gd->gicc_base + GICC_PMR, GIC_HIGHEST_NS_PRIORITY);
 
 	/* Enable GIC */
 	io_write32(gd->gicc_base + GICC_CTLR,
@@ -160,13 +188,10 @@ void gic_cpu_init(struct gic_data *gd)
 #endif
 }
 
-void gic_init(struct gic_data *gd, vaddr_t gicc_base __maybe_unused,
-	      vaddr_t gicd_base)
+void gic_init_setup(struct gic_data *gd)
 {
 	size_t n;
 
-	gic_init_base_addr(gd, gicc_base, gicd_base);
-
 	for (n = 0; n <= gd->max_it / NUM_INTS_PER_REG; n++) {
 		/* Disable interrupts */
 		io_write32(gd->gicd_base + GICD_ICENABLER(n), 0xffffffff);
@@ -191,11 +216,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_igrpen1(1);
 	io_setbits32(gd->gicd_base + GICD_CTLR, GICD_CTLR_ENABLEGRP1S);
 #else
-	io_write32(gd->gicc_base + GICC_PMR, 0x80);
+	io_write32(gd->gicc_base + GICC_PMR, GIC_HIGHEST_NS_PRIORITY);
 
 	/* Enable GIC */
 	io_write32(gd->gicc_base + GICC_CTLR, GICC_CTLR_FIQEN |
@@ -212,6 +237,14 @@ void gic_init_base_addr(struct gic_data *gd, vaddr_t gicc_base __maybe_unused,
 	gd->gicd_base = gicd_base;
 	gd->max_it = probe_max_it(gicc_base, gicd_base);
 	gd->chip.ops = &gic_ops;
+
+	gic_pm_register(gd);
+}
+
+void gic_init(struct gic_data *gd, vaddr_t gicc_base, vaddr_t gicd_base)
+{
+	gic_init_base_addr(gd, gicc_base, gicd_base);
+	gic_init_setup(gd);
 }
 
 static void gic_it_add(struct gic_data *gd, size_t it)
@@ -236,7 +269,8 @@ static void gic_it_set_cpu_mask(struct gic_data *gd, size_t it,
 {
 	size_t idx __maybe_unused = it / NUM_INTS_PER_REG;
 	uint32_t mask __maybe_unused = 1 << (it % NUM_INTS_PER_REG);
-	uint32_t target, target_shift;
+	uint32_t target = 0;
+	uint32_t target_shift = 0;
 	vaddr_t itargetsr = gd->gicd_base +
 			    GICD_ITARGETSR(it / NUM_TARGETS_PER_REG);
 
@@ -258,15 +292,39 @@ 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);
 
-	/* Assigned to group0 */
-	assert(!(io_read32(gd->gicd_base + GICD_IGROUPR(idx)) & mask));
-
 	/* Set prio it to selected CPUs */
 	DMSG("prio: writing 0x%x to 0x%" PRIxVA,
 		prio, gd->gicd_base + GICD_IPRIORITYR(0) + it);
 	io_write8(gd->gicd_base + GICD_IPRIORITYR(0) + it, prio);
 }
 
+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 = io_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();
+	io_write32(gd->gicc_base + GICC_PMR, mask);
+	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 = io_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;
@@ -391,8 +449,8 @@ void gic_dump_state(struct gic_data *gd)
 
 void gic_it_handle(struct gic_data *gd)
 {
-	uint32_t iar;
-	uint32_t id;
+	uint32_t iar = 0;
+	uint32_t id = 0;
 
 	iar = gic_read_iar(gd);
 	id = iar & GICC_IAR_IT_ID_MASK;
@@ -417,6 +475,8 @@ static void gic_op_add(struct itr_chip *chip, size_t it,
 	/* Set the CPU mask to deliver interrupts to any online core */
 	gic_it_set_cpu_mask(gd, it, 0xff);
 	gic_it_set_prio(gd, it, 0x1);
+
+	gic_pm_add_it(gd, it);
 }
 
 static void gic_op_enable(struct itr_chip *chip, size_t it)
@@ -462,6 +522,7 @@ static void gic_op_raise_sgi(struct itr_chip *chip, size_t it,
 	else
 		gic_it_raise_sgi(gd, it, cpu_mask, 0);
 }
+
 static void gic_op_set_affinity(struct itr_chip *chip, size_t it,
 			uint8_t cpu_mask)
 {
@@ -472,3 +533,137 @@ static void gic_op_set_affinity(struct itr_chip *chip, size_t it,
 
 	gic_it_set_cpu_mask(gd, it, cpu_mask);
 }
+
+#if defined(CFG_ARM_GIC_PM)
+/*
+ * Save/restore interrupts registered from the gic_op_add_it() handler
+ */
+#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)
+
+/*
+ * @it - interrupt ID/number
+ * @flags - bitflag IT_PM_*_BIT
+ * @iprio - 8bit prio from IPRIORITYR
+ * @itarget - 8bit target from ITARGETR
+ * @icfg - 2bit configuration from ICFGR and IT_PM_CONFIG_MASK
+ */
+struct gic_it_pm {
+	uint16_t it;
+	uint8_t flags;
+	uint8_t iprio;
+	uint8_t itarget;
+	uint8_t icfg;
+};
+
+static void gic_pm_add_it(struct gic_data *gd, unsigned int it)
+{
+	struct gic_pm *pm = &gd->pm;
+
+	pm->count++;
+	pm->pm_cfg = realloc(pm->pm_cfg, pm->count * sizeof(*pm->pm_cfg));
+	if (!pm->pm_cfg)
+		panic();
+
+	pm->pm_cfg[pm->count - 1] = (struct gic_it_pm){ .it = it };
+}
+
+static void gic_save_it(struct gic_data *gd, struct gic_it_pm *pm)
+{
+	unsigned int it = pm->it;
+	size_t idx = 0;
+	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 = 0;
+
+	pm->flags = 0;
+	idx = it / NUM_INTS_PER_REG;
+
+	if (io_read32(gd->gicd_base + GICD_IGROUPR(idx)) & bit_mask)
+		pm->flags |= IT_PM_GPOUP1_BIT;
+	if (io_read32(gd->gicd_base + GICD_ISENABLER(idx)) & bit_mask)
+		pm->flags |= IT_PM_ENABLE_BIT;
+	if (io_read32(gd->gicd_base + GICD_ISPENDR(idx)) & bit_mask)
+		pm->flags |= IT_PM_PENDING_BIT;
+	if (io_read32(gd->gicd_base + GICD_ISACTIVER(idx)) & bit_mask)
+		pm->flags |= IT_PM_ACTIVE_BIT;
+
+	idx = (8 * it) / NUM_INTS_PER_REG;
+
+	data32 = io_read32(gd->gicd_base + GICD_IPRIORITYR(idx)) >> shift8;
+	pm->iprio = (uint8_t)data32;
+
+	data32 = io_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 = io_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)
+{
+	unsigned int it = pm->it;
+	size_t idx = it / NUM_INTS_PER_REG;
+	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);
+
+	io_mask32(gd->gicd_base + GICD_IGROUPR(idx),
+		  (pm->flags & IT_PM_GPOUP1_BIT) ? mask : 0, mask);
+
+	io_mask32(gd->gicd_base + GICD_ISENABLER(idx),
+		  (pm->flags & IT_PM_ENABLE_BIT) ? mask : 0, mask);
+
+	io_mask32(gd->gicd_base + GICD_ISPENDR(idx),
+		  (pm->flags & IT_PM_PENDING_BIT) ? mask : 0, mask);
+
+	io_mask32(gd->gicd_base + GICD_ISACTIVER(idx),
+		  (pm->flags & IT_PM_ACTIVE_BIT) ? 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 TEE_Result gic_pm(enum pm_op op, unsigned int pm_hint __unused,
+			 const struct pm_callback_handle *handle)
+{
+	void (*sequence)(struct gic_data *gd, struct gic_it_pm *pm) = NULL;
+	struct gic_it_pm *cfg = NULL;
+	unsigned int n = 0;
+	struct gic_data *gd = (struct gic_data *)PM_CALLBACK_GET_HANDLE(handle);
+	struct gic_pm *pm = &gd->pm;
+
+	if (op == PM_OP_SUSPEND) {
+		sequence = gic_save_it;
+	} else {
+		gic_init_setup(gd);
+		sequence = gic_restore_it;
+	}
+	for (n = 0, cfg = pm->pm_cfg; n < pm->count; n++, cfg++)
+		sequence(gd, cfg);
+
+	return TEE_SUCCESS;
+}
+DECLARE_KEEP_PAGER(gic_pm);
+
+static void gic_pm_register(struct gic_data *gd)
+{
+	register_pm_core_service_cb(gic_pm, gd);
+}
+#endif /*CFG_ARM_GIC_PM*/
diff --git a/core/drivers/scmi-msg/clock.c b/core/drivers/scmi-msg/clock.c
index 6fe62477e..0361f8828 100644
--- a/core/drivers/scmi-msg/clock.c
+++ b/core/drivers/scmi-msg/clock.c
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: BSD-3-Clause
 /*
- * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
  * Copyright (c) 2019, Linaro Limited
+ * Copyright (c) 2015-2020, Arm Limited and Contributors. All rights reserved.
  */
 #include <assert.h>
 #include <confine_array_index.h>
@@ -357,7 +357,7 @@ static const scmi_msg_handler_t scmi_clock_handler_table[] = {
 	[SCMI_CLOCK_CONFIG_SET] = scmi_clock_config_set,
 };
 
-static bool message_id_is_supported(size_t message_id)
+static bool message_id_is_supported(unsigned int message_id)
 {
 	return message_id < ARRAY_SIZE(scmi_clock_handler_table) &&
 	       scmi_clock_handler_table[message_id];
diff --git a/core/drivers/stm32_bsec.c b/core/drivers/stm32_bsec.c
index 2b1584ff5..7d389a061 100644
--- a/core/drivers/stm32_bsec.c
+++ b/core/drivers/stm32_bsec.c
@@ -91,10 +91,6 @@
 #define BSEC_MODE_BIST2_LOCK_MASK	BIT(7)
 
 /* BSEC_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)
 
 /*
@@ -546,6 +542,40 @@ bool stm32_bsec_nsec_can_access_otp(uint32_t otp_id)
 	       nsec_access_granted(otp_id - otp_upper_base());
 }
 
+struct nvmem_layout {
+	char *name;
+	uint32_t otp_id;
+	size_t bit_len;
+};
+
+static struct nvmem_layout *nvmem_layout;
+static size_t nvmem_layout_count;
+
+TEE_Result stm32_bsec_find_otp_in_nvmem_layout(const char *name,
+					       uint32_t *otp_id,
+					       size_t *otp_bit_len)
+{
+	size_t i = 0;
+
+	if (!name)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	for (i = 0; i < nvmem_layout_count; i++) {
+		if (!nvmem_layout[i].name || strcmp(name, nvmem_layout[i].name))
+			continue;
+
+		if (otp_id)
+			*otp_id = nvmem_layout[i].otp_id;
+
+		if (otp_bit_len)
+			*otp_bit_len = nvmem_layout[i].bit_len;
+
+		return TEE_SUCCESS;
+	}
+
+	return TEE_ERROR_ITEM_NOT_FOUND;
+}
+
 #ifdef CFG_DT
 static void enable_nsec_access(unsigned int otp_id)
 {
@@ -624,6 +654,77 @@ static void bsec_dt_otp_nsec_access(void *fdt, int bsec_node)
 	}
 }
 
+static void save_dt_nvmem_layout(void *fdt)
+{
+	const fdt32_t *cells = NULL;
+	int i = 0;
+	int cell_nb = 0;
+	int nvmem_node = 0;
+
+	nvmem_node = fdt_node_offset_by_compatible(fdt, -1,
+						   "st,stm32-nvmem-layout");
+	if (nvmem_node < 0)
+		return;
+
+	cells = fdt_getprop(fdt, nvmem_node, "nvmem-cells", &cell_nb);
+	if (!cells)
+		cell_nb = 0;
+
+	cell_nb /= sizeof(uint32_t);
+
+	i = fdt_stringlist_count(fdt, nvmem_node, "nvmem-cell-names");
+	if (i < 0)
+		i = 0;
+
+	if (cell_nb != i)
+		panic("Inconsistent NVMEM layout");
+
+	nvmem_layout = calloc(cell_nb, sizeof(*nvmem_layout));
+	if (!nvmem_layout)
+		panic();
+
+	nvmem_layout_count = (size_t)cell_nb;
+
+	for (i = 0; i < cell_nb; i++) {
+		const fdt32_t *cuint = NULL;
+		const char *string = NULL;
+		int len = 0;
+		int node = 0;
+		struct nvmem_layout *layout_cell = &nvmem_layout[i];
+
+		node = fdt_node_offset_by_phandle(fdt,
+						  fdt32_to_cpu(*(cells + i)));
+		if (node < 0) {
+			IMSG("Malformed nvmem_layout node: ignored");
+			continue;
+		}
+
+		cuint = fdt_getprop(fdt, node, "reg", &len);
+		if (!cuint || (len != (2 * (int)sizeof(uint32_t))))  {
+			IMSG("Malformed nvmem_layout node: ignored");
+			continue;
+		}
+
+		if (fdt32_to_cpu(*cuint) % sizeof(uint32_t)) {
+			IMSG("Misaligned nvmem_layout element: ignored");
+			continue;
+		}
+		layout_cell->otp_id = fdt32_to_cpu(*cuint) / sizeof(uint32_t);
+		layout_cell->bit_len = fdt32_to_cpu(*(cuint + 1)) * CHAR_BIT;
+
+		string = fdt_stringlist_get(fdt, nvmem_node, "nvmem-cell-names",
+					    i, &len);
+		if (!string || !len)
+			continue;
+
+		layout_cell->name = calloc(1, len + 1);
+		if (!layout_cell->name)
+			panic();
+
+		memcpy(layout_cell->name, string, len);
+	}
+}
+
 static void initialize_bsec_from_dt(void)
 {
 	void *fdt = NULL;
@@ -642,6 +743,8 @@ static void initialize_bsec_from_dt(void)
 		panic();
 
 	bsec_dt_otp_nsec_access(fdt, node);
+
+	save_dt_nvmem_layout(fdt);
 }
 #else
 static void initialize_bsec_from_dt(void)
diff --git a/core/drivers/stm32_etzpc.c b/core/drivers/stm32_etzpc.c
index 3482f3431..5573503a1 100644
--- a/core/drivers/stm32_etzpc.c
+++ b/core/drivers/stm32_etzpc.c
@@ -26,10 +26,16 @@
 #include <kernel/pm.h>
 #include <libfdt.h>
 #include <mm/core_memprot.h>
+#include <stm32_util.h>
 #include <util.h>
 
 /* Devicetree compatibility */
 #define ETZPC_COMPAT			"st,stm32-etzpc"
+#define ETZPC_LOCK_MASK			BIT(0)
+#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
@@ -316,6 +322,45 @@ void stm32_etzpc_init(paddr_t base)
 }
 
 #ifdef CFG_DT
+struct dt_id_attr {
+	/* The effective size of the array is meaningless here */
+	fdt32_t id_attr[1];
+};
+
+static void etzpc_dt_conf_decprot(void *fdt, int node)
+{
+	const struct dt_id_attr *conf_list = NULL;
+	size_t i = 0;
+	int len = 0;
+
+	conf_list = (const struct dt_id_attr *)fdt_getprop(fdt, node,
+							   "st,decprot", &len);
+	if (conf_list == NULL) {
+		DMSG("No ETZPC DECPROT configuration in DT");
+		return;
+	}
+
+	for (i = 0; i < len / sizeof(uint32_t); i++) {
+		uint32_t value = fdt32_to_cpu(conf_list->id_attr[i]);
+		uint32_t id = (value >> ETZPC_ID_SHIFT) & ETZPC_ID_MASK;
+		uint32_t mode = (value >> ETZPC_MODE_SHIFT) & ETZPC_MODE_MASK;
+		bool lock = value & ETZPC_LOCK_MASK;
+		enum etzpc_decprot_attributes attr = ETZPC_DECPROT_MAX;
+
+		if (!valid_decprot_id(id)) {
+			DMSG("Invalid DECPROT %"PRIu32, id);
+			panic();
+		}
+
+		attr = stm32mp_etzpc_binding2decprot(mode);
+		stm32mp_register_etzpc_decprot(id, attr);
+		etzpc_configure_decprot(id, attr);
+
+		if (lock)
+			etzpc_lock_decprot(id);
+	}
+}
+
 static TEE_Result init_etzpc_from_dt(void)
 {
 	void *fdt = get_embedded_dt();
@@ -339,6 +384,8 @@ static TEE_Result init_etzpc_from_dt(void)
 
 	init_device_from_hw_config(&etzpc_dev, pbase);
 
+	etzpc_dt_conf_decprot(fdt, node);
+
 	return TEE_SUCCESS;
 }
 
diff --git a/core/drivers/stm32_gpio.c b/core/drivers/stm32_gpio.c
index 8f7927d90..83c7b8b4e 100644
--- a/core/drivers/stm32_gpio.c
+++ b/core/drivers/stm32_gpio.c
@@ -1,12 +1,13 @@
 // SPDX-License-Identifier: BSD-3-Clause
 /*
- * Copyright (c) 2017-2019, STMicroelectronics
+ * Copyright (c) 2017-2020, STMicroelectronics
  *
  * STM32 GPIO driver is used as pin controller for stm32mp SoCs.
  * The driver API is defined in header file stm32_gpio.h.
  */
 
 #include <assert.h>
+#include <drivers/clk.h>
 #include <drivers/stm32_gpio.h>
 #include <io.h>
 #include <kernel/dt.h>
@@ -54,7 +55,7 @@ static void get_gpio_cfg(uint32_t bank, uint32_t pin, struct gpio_cfg *cfg)
 	vaddr_t base = stm32_get_gpio_bank_base(bank);
 	unsigned int clock = stm32_get_gpio_bank_clock(bank);
 
-	stm32_clock_enable(clock);
+	clk_enable(clock);
 
 	/*
 	 * Save GPIO configuration bits spread over the few bank registers.
@@ -84,7 +85,7 @@ static void get_gpio_cfg(uint32_t bank, uint32_t pin, struct gpio_cfg *cfg)
 			    ((pin - GPIO_ALT_LOWER_LIMIT) << 2)) &
 			   GPIO_ALTERNATE_MASK;
 
-	stm32_clock_disable(clock);
+	clk_disable(clock);
 }
 
 /* Apply GPIO (@bank/@pin) configuration described by @cfg */
@@ -94,7 +95,7 @@ static void set_gpio_cfg(uint32_t bank, uint32_t pin, struct gpio_cfg *cfg)
 	unsigned int clock = stm32_get_gpio_bank_clock(bank);
 	uint32_t exceptions = cpu_spin_lock_xsave(&gpio_lock);
 
-	stm32_clock_enable(clock);
+	clk_enable(clock);
 
 	/* Load GPIO MODE value, 2bit value shifted by twice the pin number */
 	io_clrsetbits32(base + GPIO_MODER_OFFSET,
@@ -129,7 +130,7 @@ static void set_gpio_cfg(uint32_t bank, uint32_t pin, struct gpio_cfg *cfg)
 	/* Load GPIO Output direction confuguration, 1bit */
 	io_clrsetbits32(base + GPIO_ODR_OFFSET, BIT(pin), cfg->od << pin);
 
-	stm32_clock_disable(clock);
+	clk_disable(clock);
 	cpu_spin_unlock_xrestore(&gpio_lock, exceptions);
 }
 
@@ -395,12 +396,12 @@ int stm32_gpio_get_input_level(unsigned int bank, unsigned int pin)
 
 	assert(valid_gpio_config(bank, pin, true));
 
-	stm32_clock_enable(clock);
+	clk_enable(clock);
 
 	if (io_read32(base + GPIO_IDR_OFFSET) == BIT(pin))
 		rc = 1;
 
-	stm32_clock_disable(clock);
+	clk_disable(clock);
 
 	return rc;
 }
@@ -412,14 +413,14 @@ void stm32_gpio_set_output_level(unsigned int bank, unsigned int pin, int level)
 
 	assert(valid_gpio_config(bank, pin, false));
 
-	stm32_clock_enable(clock);
+	clk_enable(clock);
 
 	if (level)
 		io_write32(base + GPIO_BSRR_OFFSET, BIT(pin));
 	else
 		io_write32(base + GPIO_BSRR_OFFSET, BIT(pin + 16));
 
-	stm32_clock_disable(clock);
+	clk_disable(clock);
 }
 
 void stm32_gpio_set_secure_cfg(unsigned int bank, unsigned int pin, bool secure)
@@ -428,13 +429,13 @@ void stm32_gpio_set_secure_cfg(unsigned int bank, unsigned int pin, bool secure)
 	unsigned int clock = stm32_get_gpio_bank_clock(bank);
 	uint32_t exceptions = cpu_spin_lock_xsave(&gpio_lock);
 
-	stm32_clock_enable(clock);
+	clk_enable(clock);
 
 	if (secure)
 		io_setbits32(base + GPIO_SECR_OFFSET, BIT(pin));
 	else
 		io_clrbits32(base + GPIO_SECR_OFFSET, BIT(pin));
 
-	stm32_clock_disable(clock);
+	clk_disable(clock);
 	cpu_spin_unlock_xrestore(&gpio_lock, exceptions);
 }
diff --git a/core/drivers/stm32_i2c.c b/core/drivers/stm32_i2c.c
index 790a5cab6..2807cc42a 100644
--- a/core/drivers/stm32_i2c.c
+++ b/core/drivers/stm32_i2c.c
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: (GPL-2.0 OR BSD-3-Clause)
 /*
- * Copyright (c) 2017-2019, STMicroelectronics
+ * Copyright (c) 2017-2020, STMicroelectronics
  *
  * The driver API is defined in header file stm32_i2c.h.
  *
@@ -10,6 +10,7 @@
  */
 
 #include <arm.h>
+#include <drivers/clk.h>
 #include <drivers/stm32_i2c.h>
 #include <io.h>
 #include <kernel/delay.h>
@@ -313,7 +314,7 @@ static void save_cfg(struct i2c_handle_s *hi2c, struct i2c_cfg *cfg)
 {
 	vaddr_t base = get_base(hi2c);
 
-	stm32_clock_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	cfg->cr1 = io_read32(base + I2C_CR1);
 	cfg->cr2 = io_read32(base + I2C_CR2);
@@ -321,14 +322,14 @@ static void save_cfg(struct i2c_handle_s *hi2c, struct i2c_cfg *cfg)
 	cfg->oar2 = io_read32(base + I2C_OAR2);
 	cfg->timingr = io_read32(base + I2C_TIMINGR);
 
-	stm32_clock_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 }
 
 static void restore_cfg(struct i2c_handle_s *hi2c, struct i2c_cfg *cfg)
 {
 	vaddr_t base = get_base(hi2c);
 
-	stm32_clock_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	io_clrbits32(base + I2C_CR1, I2C_CR1_PE);
 	io_write32(base + I2C_TIMINGR, cfg->timingr & TIMINGR_CLEAR_MASK);
@@ -338,7 +339,7 @@ static void restore_cfg(struct i2c_handle_s *hi2c, struct i2c_cfg *cfg)
 	io_write32(base + I2C_CR1, cfg->cr1 & ~I2C_CR1_PE);
 	io_setbits32(base + I2C_CR1, cfg->cr1 & I2C_CR1_PE);
 
-	stm32_clock_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 }
 
 static void __maybe_unused dump_cfg(struct i2c_cfg *cfg __maybe_unused)
@@ -354,7 +355,7 @@ static void __maybe_unused dump_i2c(struct i2c_handle_s *hi2c)
 {
 	vaddr_t __maybe_unused base = get_base(hi2c);
 
-	stm32_clock_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	DMSG("CR1:  %#"PRIx32, io_read32(base + I2C_CR1));
 	DMSG("CR2:  %#"PRIx32, io_read32(base + I2C_CR2));
@@ -362,7 +363,7 @@ static void __maybe_unused dump_i2c(struct i2c_handle_s *hi2c)
 	DMSG("OAR2: %#"PRIx32, io_read32(base + I2C_OAR2));
 	DMSG("TIM:  %#"PRIx32, io_read32(base + I2C_TIMINGR));
 
-	stm32_clock_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 }
 
 /*
@@ -597,7 +598,7 @@ static int i2c_setup_timing(struct i2c_handle_s *hi2c,
 
 	assert(i2c_specs_is_consistent());
 
-	clock_src = stm32_clock_get_rate(hi2c->clock);
+	clock_src = clk_get_rate(hi2c->clock);
 	if (!clock_src) {
 		DMSG("Null I2C clock rate");
 		return -1;
@@ -759,7 +760,7 @@ int stm32_i2c_init(struct i2c_handle_s *hi2c,
 	if (rc)
 		return rc;
 
-	stm32_clock_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 	base = get_base(hi2c);
 	hi2c->i2c_state = I2C_STATE_BUSY;
 
@@ -819,7 +820,7 @@ int stm32_i2c_init(struct i2c_handle_s *hi2c,
 	if (rc)
 		DMSG("I2C analog filter error %d", rc);
 
-	stm32_clock_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 
 	return rc;
 }
@@ -1074,7 +1075,7 @@ static int i2c_write(struct i2c_handle_s *hi2c, struct i2c_request *request,
 	if (!p_data || !size)
 		return -1;
 
-	stm32_clock_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	timeout_ref = timeout_init_us(I2C_TIMEOUT_BUSY_MS * 1000);
 	if (wait_isr_event(hi2c, I2C_ISR_BUSY, 0, timeout_ref))
@@ -1161,7 +1162,7 @@ static int i2c_write(struct i2c_handle_s *hi2c, struct i2c_request *request,
 	rc = 0;
 
 bail:
-	stm32_clock_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 
 	return rc;
 }
@@ -1207,7 +1208,7 @@ int stm32_i2c_read_write_membyte(struct i2c_handle_s *hi2c, uint16_t dev_addr,
 	if (hi2c->i2c_state != I2C_STATE_READY || !p_data)
 		return -1;
 
-	stm32_clock_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	timeout_ref = timeout_init_us(I2C_TIMEOUT_BUSY_US);
 	if (wait_isr_event(hi2c, I2C_ISR_BUSY, 0, timeout_ref))
@@ -1264,7 +1265,7 @@ int stm32_i2c_read_write_membyte(struct i2c_handle_s *hi2c, uint16_t dev_addr,
 	rc = 0;
 
 bail:
-	stm32_clock_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 
 	return rc;
 }
@@ -1297,7 +1298,7 @@ static int i2c_read(struct i2c_handle_s *hi2c, struct i2c_request *request,
 	if (!p_data || !size)
 		return -1;
 
-	stm32_clock_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	timeout_ref = timeout_init_us(I2C_TIMEOUT_BUSY_MS * 1000);
 	if (wait_isr_event(hi2c, I2C_ISR_BUSY, 0, timeout_ref))
@@ -1374,7 +1375,7 @@ static int i2c_read(struct i2c_handle_s *hi2c, struct i2c_request *request,
 	rc = 0;
 
 bail:
-	stm32_clock_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 
 	return rc;
 }
@@ -1417,7 +1418,7 @@ bool stm32_i2c_is_device_ready(struct i2c_handle_s *hi2c, uint32_t dev_addr,
 	if (hi2c->i2c_state != I2C_STATE_READY)
 		return rc;
 
-	stm32_clock_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	if (io_read32(base + I2C_ISR) & I2C_ISR_BUSY)
 		goto bail;
@@ -1489,7 +1490,7 @@ bool stm32_i2c_is_device_ready(struct i2c_handle_s *hi2c, uint32_t dev_addr,
 	notif_i2c_timeout(hi2c);
 
 bail:
-	stm32_clock_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 
 	return rc;
 }
diff --git a/core/drivers/stm32_iwdg.c b/core/drivers/stm32_iwdg.c
new file mode 100644
index 000000000..335d1a2cb
--- /dev/null
+++ b/core/drivers/stm32_iwdg.c
@@ -0,0 +1,314 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
+ */
+
+#include <assert.h>
+#include <drivers/clk.h>
+#include <drivers/stm32_iwdg.h>
+#include <io.h>
+#include <keep.h>
+#include <kernel/boot.h>
+#include <kernel/delay.h>
+#include <kernel/dt.h>
+#include <kernel/interrupt.h>
+#include <kernel/misc.h>
+#include <kernel/panic.h>
+#include <kernel/pm.h>
+#include <libfdt.h>
+#include <mm/core_memprot.h>
+#include <stm32_util.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 {
+	struct io_pa_va base;
+	unsigned long clock;
+	uint8_t instance;
+	uint8_t flags;
+};
+
+static struct stm32_iwdg_instance *stm32_iwdg;
+static size_t stm32_iwdg_count;
+
+static vaddr_t get_base(struct stm32_iwdg_instance *iwdg)
+{
+	return io_pa_or_va(&iwdg->base);
+}
+
+static int stm32_iwdg_get_dt_node(void *fdt, struct dt_node_info *info,
+				  int offset)
+{
+	int node = fdt_node_offset_by_compatible(fdt, offset, IWDG_COMPAT);
+
+	if (node < 0) {
+		if (offset == -1)
+			DMSG("No IDWG found");
+
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	_fdt_fill_device_info(fdt, info, node);
+
+	return node;
+}
+
+static struct stm32_iwdg_instance *get_iwdg(unsigned int instance)
+{
+	size_t i = 0;
+
+	assert(instance <= UINT8_MAX);
+	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();
+	unsigned int instance = stm32mp_iwdg_irq2instance(handler->it);
+	struct stm32_iwdg_instance *iwdg = get_iwdg(instance);
+	vaddr_t iwdg_base = get_base(iwdg);
+
+	DMSG("CPU %u IT Watchdog %d\n", cpu, instance + 1);
+
+	stm32_iwdg_refresh(instance);
+
+	clk_enable(iwdg->clock);
+
+	io_setbits32(iwdg_base + IWDG_EWCR_OFFSET, IWDG_EWCR_EWIC);
+
+	clk_disable(iwdg->clock);
+
+#ifdef CFG_PM
+	/*
+	 * Ack interrupt as we do not return from next call.
+	 * And interrupt is no more considered as pending here.
+	 */
+	stm32mp_gic_set_end_of_interrupt(handler->it);
+
+	stm32_cores_reset();
+#else
+	panic("Watchdog");
+#endif
+
+	return ITRR_HANDLED;
+}
+DECLARE_KEEP_PAGER(stm32_iwdg_it_handler);
+
+static int stm32_iwdg_get_secure_timeout(void *fdt, int node)
+{
+	const fdt32_t *cuint = NULL;
+
+	cuint = fdt_getprop(fdt, node, "secure-timeout-sec", NULL);
+	if (!cuint)
+		return -1;
+
+	return (int)fdt32_to_cpu(*cuint);
+}
+
+static int fdt_get_clock_id_by_name(void *fdt, int node, const char *name)
+{
+	const fdt32_t *cuint = NULL;
+	int index = 0;
+	int len = 0;
+
+	index = fdt_stringlist_search(fdt, node, "clock-names", name);
+	if (index < 0)
+		return index;
+
+	cuint = fdt_getprop(fdt, node, "clocks", &len);
+	if (!cuint)
+		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);
+}
+
+static TEE_Result stm32_iwdg_conf_etimeout(void *fdt, int node,
+					   struct stm32_iwdg_instance *iwdg)
+{
+	int id_lsi = 0;
+	int dt_secure_timeout = stm32_iwdg_get_secure_timeout(fdt, node);
+	uint32_t reload = 0;
+	uint32_t status = 0;
+	uint64_t timeout_ref = 0;
+	unsigned long long reload_ll = 0;
+	vaddr_t iwdg_base = get_base(iwdg);
+	struct itr_handler *itr = NULL;
+
+	if (dt_secure_timeout < 0)
+		return TEE_SUCCESS;
+
+	if (dt_secure_timeout == 0)
+		return TEE_ERROR_GENERIC;
+
+	id_lsi = fdt_get_clock_id_by_name(fdt, node, "lsi");
+	if (id_lsi < 0)
+		return TEE_ERROR_GENERIC;
+
+	/* Prescaler fix to 256 */
+	reload_ll = (unsigned long long)dt_secure_timeout *
+		    clk_get_rate(id_lsi);
+	reload = ((uint32_t)(reload_ll >> 8) - 1) & IWDG_EWCR_EWIT_MASK;
+
+	clk_enable(iwdg->clock);
+
+	io_write32(iwdg_base + IWDG_KR_OFFSET, IWDG_KR_START_KEY);
+	io_write32(iwdg_base + IWDG_KR_OFFSET, IWDG_KR_ACCESS_KEY);
+	io_write32(iwdg_base + IWDG_PR_OFFSET, IWDG_PR_DIV_256);
+	io_write32(iwdg_base + IWDG_EWCR_OFFSET, reload | IWDG_EWCR_EWIE);
+
+	timeout_ref = timeout_init_us(IWDG_TIMEOUT_US);
+	while (!timeout_elapsed(timeout_ref))
+		if (!(io_read32(iwdg_base + IWDG_SR_OFFSET) & IWDG_SR_EWU))
+			break;
+
+	status = io_read32(iwdg_base + IWDG_SR_OFFSET) & IWDG_SR_EWU;
+	clk_disable(iwdg->clock);
+	if (status)
+		return TEE_ERROR_GENERIC;
+
+	itr = calloc(1, sizeof(*itr));
+	if (!itr)
+		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 TEE_SUCCESS;
+}
+
+void stm32_iwdg_refresh(unsigned int instance)
+{
+	struct stm32_iwdg_instance *iwdg = get_iwdg(instance);
+
+	if (!iwdg)
+		return;
+
+	clk_enable(iwdg->clock);
+
+	io_write32(get_base(iwdg) + IWDG_KR_OFFSET, IWDG_KR_RELOAD_KEY);
+
+	clk_disable(iwdg->clock);
+}
+
+static TEE_Result iwdg_init(void)
+{
+	int node = -1;
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct dt_node_info dt_info = { };
+	void *fdt = NULL;
+	size_t count = 0;
+
+	fdt = get_embedded_dt();
+	if (!fdt)
+		panic();
+
+	assert(!stm32_iwdg && !stm32_iwdg_count);
+
+	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 = MEM_AREA_MAXTYPE;
+		uint32_t hw_init = 0;
+
+		iwdg.base.pa = dt_info.reg;
+		iwdg.clock = (unsigned long)dt_info.clock;
+		iwdg.instance = stm32mp_iwdg_iomem2instance(iwdg.base.pa);
+
+		memtype = ((dt_info.status & DT_STATUS_OK_NSEC) != 0) ?
+			  MEM_AREA_IO_NSEC : MEM_AREA_IO_SEC;
+		iwdg.base.va = (vaddr_t)phys_to_virt(iwdg.base.pa, memtype);
+
+		/* DT can specify low power cases */
+		if (!fdt_getprop(fdt, node, "stm32,enable-on-stop", NULL))
+			iwdg.flags |= IWDG_DISABLE_ON_STOP;
+
+		if (!fdt_getprop(fdt, node, "stm32,enable-on-standby", NULL))
+			iwdg.flags |= IWDG_DISABLE_ON_STANDBY;
+
+		hw_init = stm32_get_iwdg_otp_config(iwdg.base.pa);
+
+		if (hw_init & IWDG_HW_ENABLED) {
+			if (dt_info.status == DT_STATUS_DISABLED)
+				panic("IWDG HW enabled");
+
+			iwdg.flags |= IWDG_HW_ENABLED;
+		}
+
+		if (hw_init & IWDG_DISABLE_ON_STOP)
+			iwdg.flags |= IWDG_DISABLE_ON_STOP;
+
+		if (hw_init & IWDG_DISABLE_ON_STANDBY)
+			iwdg.flags |= IWDG_DISABLE_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) ? "non-" : "");
+
+		if (dt_info.status & DT_STATUS_OK_NSEC)
+			stm32mp_register_non_secure_periph_iomem(iwdg.base.pa);
+		else
+			stm32mp_register_secure_periph_iomem(iwdg.base.pa);
+
+		res = stm32_iwdg_conf_etimeout(fdt, node, &iwdg);
+		if (res) {
+			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)
+			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
index d55f28f19..e95bde8ad 100644
--- a/core/drivers/stm32_rng.c
+++ b/core/drivers/stm32_rng.c
@@ -1,9 +1,10 @@
 // SPDX-License-Identifier: BSD-3-Clause
 /*
- * Copyright (c) 2018-2019, STMicroelectronics
+ * Copyright (c) 2018-2020, STMicroelectronics
  */
 
 #include <assert.h>
+#include <drivers/clk.h>
 #include <drivers/stm32_rng.h>
 #include <io.h>
 #include <kernel/delay.h>
@@ -129,7 +130,7 @@ static void gate_rng(bool enable, struct stm32_rng_instance *dev)
 	if (enable) {
 		/* incr_refcnt return non zero if resource shall be enabled */
 		if (incr_refcnt(&dev->refcount)) {
-			stm32_clock_enable(dev->clock);
+			clk_enable(dev->clock);
 			io_write32(rng_cr, 0);
 			io_write32(rng_cr, RNG_CR_RNGEN | RNG_CR_CED);
 		}
@@ -137,7 +138,7 @@ static void gate_rng(bool enable, struct stm32_rng_instance *dev)
 		/* decr_refcnt return non zero if resource shall be disabled */
 		if (decr_refcnt(&dev->refcount)) {
 			io_write32(rng_cr, 0);
-			stm32_clock_disable(dev->clock);
+			clk_disable(dev->clock);
 		}
 	}
 
diff --git a/core/drivers/stm32_rtc.c b/core/drivers/stm32_rtc.c
new file mode 100644
index 000000000..79ab627e6
--- /dev/null
+++ b/core/drivers/stm32_rtc.c
@@ -0,0 +1,446 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2018-2020, STMicroelectronics - All Rights Reserved
+ *
+ */
+
+#include <arm32.h>
+#include <drivers/clk.h>
+#include <drivers/stm32_rtc.h>
+#include <initcall.h>
+#include <io.h>
+#include <kernel/boot.h>
+#include <kernel/dt.h>
+#include <kernel/panic.h>
+#include <libfdt.h>
+#include <mm/core_memprot.h>
+#include <stm32_util.h>
+
+#define RTC_COMPAT		"st,stm32mp1-rtc"
+
+#define RTC_TR			0x00
+#define RTC_DR			0x04
+#define RTC_SSR			0x08
+#define RTC_ICSR		0x0C
+#define RTC_PRER		0x10
+#define RTC_WUTR		0x14
+#define RTC_CR			0x18
+#define RTC_SMCR		0x20
+#define RTC_WPR			0x24
+#define RTC_CALR		0x28
+#define RTC_SHIFTR		0x2C
+#define RTC_TSTR		0x30
+#define RTC_TSDR		0x34
+#define RTC_TSSSR		0x38
+#define RTC_ALRMAR		0x40
+#define RTC_ALRMASSR		0x44
+#define RTC_ALRMBR		0x48
+#define RTC_ALRMBSSR		0x4C
+#define RTC_SR			0x50
+#define RTC_SCR			0x5C
+#define RTC_OR			0x60
+
+#define RTC_TR_SU_MASK		GENMASK_32(3, 0)
+#define RTC_TR_ST_MASK		GENMASK_32(6, 4)
+#define RTC_TR_ST_SHIFT		4
+#define RTC_TR_MNU_MASK		GENMASK_32(11, 8)
+#define RTC_TR_MNU_SHIFT	8
+#define RTC_TR_MNT_MASK		GENMASK_32(14, 12)
+#define RTC_TR_MNT_SHIFT	12
+#define RTC_TR_HU_MASK		GENMASK_32(19, 16)
+#define RTC_TR_HU_SHIFT		16
+#define RTC_TR_HT_MASK		GENMASK_32(21, 20)
+#define RTC_TR_HT_SHIFT		20
+#define RTC_TR_PM		BIT(22)
+
+#define RTC_DR_DU_MASK		GENMASK_32(3, 0)
+#define RTC_DR_DT_MASK		GENMASK_32(5, 4)
+#define RTC_DR_DT_SHIFT		4
+#define RTC_DR_MU_MASK		GENMASK_32(11, 8)
+#define RTC_DR_MU_SHIFT		8
+#define RTC_DR_MT		BIT(12)
+#define RTC_DR_MT_SHIFT		12
+#define RTC_DR_WDU_MASK		GENMASK_32(15, 13)
+#define RTC_DR_WDU_SHIFT	13
+#define RTC_DR_YU_MASK		GENMASK_32(19, 16)
+#define RTC_DR_YU_SHIFT		16
+#define RTC_DR_YT_MASK		GENMASK_32(23, 20)
+#define RTC_DR_YT_SHIFT		20
+
+#define RTC_SSR_SS_MASK		GENMASK_32(15, 0)
+
+#define RTC_ICSR_RSF		BIT(5)
+
+#define RTC_PRER_PREDIV_S_MASK	GENMASK_32(15, 0)
+
+#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	GENMASK_32(11, 8)
+#define RTC_TSDR_MU_SHIFT	8
+#define RTC_TSDR_DT_MASK	GENMASK_32(5, 4)
+#define RTC_TSDR_DT_SHIFT	4
+#define RTC_TSDR_DU_MASK	GENMASK_32(3, 0)
+#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 {
+	struct io_pa_va base;
+	uint16_t clock;
+	uint8_t flags;
+};
+
+/* Expect a single RTC instance */
+struct rtc_device rtc_dev;
+
+static vaddr_t get_base(void)
+{
+	assert(rtc_dev.base.pa);
+	return io_pa_or_va(&rtc_dev.base);
+}
+
+static void stm32_rtc_write_unprotect(void)
+{
+	vaddr_t rtc_base = get_base();
+
+	io_write32(rtc_base + RTC_WPR, RTC_WPR_KEY1);
+	io_write32(rtc_base + RTC_WPR, RTC_WPR_KEY2);
+}
+
+static void stm32_rtc_write_protect(void)
+{
+	vaddr_t rtc_base = get_base();
+
+	io_write32(rtc_base + RTC_WPR, RTC_WPR_KEY_LOCK);
+}
+
+static bool stm32_rtc_get_bypshad(void)
+{
+	return io_read32(get_base() + RTC_CR) & RTC_CR_BYPSHAD;
+}
+
+/* Get calendar data from RTC devicecalendar valueregister values */
+static void stm32_rtc_read_calendar(struct stm32_rtc_calendar *calendar)
+{
+	vaddr_t rtc_base = get_base();
+	bool bypshad = stm32_rtc_get_bypshad();
+
+	if (!bypshad) {
+		/* Shadow RTC registers */
+		io_clrbits32(rtc_base + RTC_ICSR, RTC_ICSR_RSF);
+		while (!(io_read32(rtc_base + RTC_ICSR) & RTC_ICSR_RSF))
+			;
+	}
+
+	calendar->ssr = io_read32(rtc_base + RTC_SSR);
+	calendar->tr = io_read32(rtc_base + RTC_TR);
+	calendar->dr = io_read32(rtc_base + RTC_DR);
+}
+
+/* Fill the RTC timestamp structure from a given RTC time-in-day value */
+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) * 10) +
+		   ((cal->tr & RTC_TR_HU_MASK) >> RTC_TR_HU_SHIFT);
+
+	if (cal->tr & RTC_TR_PM)
+		tm->hour += 12;
+
+	tm->min = (((cal->tr & RTC_TR_MNT_MASK) >> RTC_TR_MNT_SHIFT) * 10) +
+		  ((cal->tr & RTC_TR_MNU_MASK) >> RTC_TR_MNU_SHIFT);
+	tm->sec = (((cal->tr & RTC_TR_ST_MASK) >> RTC_TR_ST_SHIFT) * 10) +
+		  (cal->tr & RTC_TR_SU_MASK);
+}
+
+/* Fill the RTC timestamp structure from a given RTC date value */
+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) * 10) +
+		  (cal->dr & RTC_DR_DU_MASK);
+
+	tm->month = (((cal->dr & RTC_DR_MT) >> RTC_DR_MT_SHIFT) * 10) +
+		    ((cal->dr & RTC_DR_MU_MASK) >> RTC_DR_MU_SHIFT);
+
+	tm->year = (((cal->dr & RTC_DR_YT_MASK) >> RTC_DR_YT_SHIFT) * 10) +
+		   ((cal->dr & RTC_DR_YU_MASK) >> RTC_DR_YU_SHIFT) + 2000;
+}
+
+/* Update time value with RTC timestamp */
+static void stm32_rtc_read_timestamp(struct stm32_rtc_time *time)
+{
+	struct stm32_rtc_calendar cal_tamp = { };
+	vaddr_t rtc_base = get_base();
+
+	cal_tamp.tr = io_read32(rtc_base + RTC_TSTR);
+	cal_tamp.dr = io_read32(rtc_base + RTC_TSDR);
+	stm32_rtc_get_time(&cal_tamp, time);
+	stm32_rtc_get_date(&cal_tamp, time);
+}
+
+void stm32_rtc_get_calendar(struct stm32_rtc_calendar *calendar)
+{
+	clk_enable(rtc_dev.clock);
+
+	stm32_rtc_read_calendar(calendar);
+
+	/* RTC may need to be read twice, depending of clocks configuration */
+	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);
+	}
+
+	clk_disable(rtc_dev.clock);
+}
+
+/* Return difference in milliseconds on second fraction */
+static uint32_t stm32_rtc_get_second_fraction(struct stm32_rtc_calendar *cal)
+{
+	uint32_t prediv_s = io_read32(get_base() + RTC_PRER) &
+			    RTC_PRER_PREDIV_S_MASK;
+	uint32_t ss = cal->ssr & RTC_SSR_SS_MASK;
+
+	return ((prediv_s - ss) * 1000) / (prediv_s + 1);
+}
+
+/* Return absolute difference in milliseconds on second fraction */
+static signed long long stm32_rtc_diff_frac(struct stm32_rtc_calendar *cur,
+					    struct stm32_rtc_calendar *ref)
+{
+	return (signed long long)stm32_rtc_get_second_fraction(cur) -
+	       (signed long long)stm32_rtc_get_second_fraction(ref);
+}
+
+/* Return absolute difference in milliseconds on seconds-in-day fraction */
+static signed long long stm32_rtc_diff_time(struct stm32_rtc_time *current,
+					    struct stm32_rtc_time *ref)
+{
+	signed long long curr_s = 0;
+	signed long long ref_s = 0;
+
+	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);
+
+	return (curr_s - ref_s) * 1000U;
+}
+
+static bool stm32_is_a_leap_year(uint32_t year)
+{
+	return ((year % 4) == 0) &&
+	       (((year % 100) != 0) || ((year % 400) == 0));
+}
+
+/* Return absolute difference in milliseconds on day-in-year fraction */
+static signed 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 = 0;
+	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 += month_len[ref->month - 1] -
+				ref->day + current->day;
+
+	/* Get the number of entire months, and compute the related days */
+	if (current->month > (ref->month + 1))
+		for (m = ref->month + 1; m < current->month && m < 12; m++)
+			diff_in_days += month_len[m - 1];
+
+	if (current->month < (ref->month - 1)) {
+		for (m = 1; m < current->month && m < 12; m++)
+			diff_in_days += month_len[m - 1];
+
+		for (m = ref->month + 1; m < 12; m++)
+			diff_in_days += month_len[m - 1];
+	}
+
+	/* Get complete years */
+	if (current->year > (ref->year + 1))
+		diff_in_days += (current->year - ref->year - 1) * 365;
+
+	/* Particular cases: leap years (one day more) */
+	if (diff_in_days > 0) {
+		if (current->year == ref->year) {
+			if (stm32_is_a_leap_year(current->year) &&
+			    ref->month <= 2 &&
+			    current->month >= 3 && current->day <= 28)
+				diff_in_days++;
+		} else {
+			uint32_t y = 0;
+
+			/* Ref year is leap */
+			if (stm32_is_a_leap_year(ref->year) &&
+			    ref->month <= 2 && ref->day <= 28)
+				diff_in_days++;
+
+			/* Current year is leap */
+			if (stm32_is_a_leap_year(current->year) &&
+			    current->month >= 3)
+				diff_in_days++;
+
+			/* Interleaved years are leap */
+			for (y = ref->year + 1; y < current->year; y++)
+				if (stm32_is_a_leap_year(y))
+					diff_in_days++;
+		}
+	}
+
+	return (24 * 60 * 60 * 1000) * (signed long long)diff_in_days;
+}
+
+unsigned long long stm32_rtc_diff_calendar(struct stm32_rtc_calendar *cur,
+					   struct stm32_rtc_calendar *ref)
+{
+	signed long long diff_in_ms = 0;
+	struct stm32_rtc_time curr_t = { };
+	struct stm32_rtc_time ref_t = { };
+
+	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);
+
+	return (unsigned long long)diff_in_ms;
+}
+
+void stm32_rtc_get_timestamp(struct stm32_rtc_time *tamp_ts)
+{
+	vaddr_t rtc_base = get_base();
+
+	clk_enable(rtc_dev.clock);
+
+	if (io_read32(rtc_base + RTC_SR) & RTC_SR_TSF) {
+		/* Timestamp for tamper event */
+		stm32_rtc_read_timestamp(tamp_ts);
+		io_setbits32(rtc_base + RTC_SCR, RTC_SCR_CTSF);
+
+		/* Overflow detection */
+		if (io_read32(rtc_base + RTC_SR) & RTC_SR_TSOVF)
+			io_setbits32(rtc_base + RTC_SCR, RTC_SCR_CTSOVF);
+	}
+
+	clk_disable(rtc_dev.clock);
+}
+
+void stm32_rtc_set_tamper_timestamp(void)
+{
+	vaddr_t rtc_base = get_base();
+
+	clk_enable(rtc_dev.clock);
+
+	stm32_rtc_write_unprotect();
+
+	/* Enable tamper timestamper */
+	io_setbits32(rtc_base + RTC_CR, RTC_CR_TAMPTS);
+
+	/* Secure Timestamp bit */
+	io_clrbits32(rtc_base + RTC_SMCR, RTC_SMCR_TS_DPROT);
+
+	stm32_rtc_write_protect();
+
+	clk_disable(rtc_dev.clock);
+}
+
+bool stm32_rtc_is_timestamp_enable(void)
+{
+	bool ret = false;
+
+	clk_enable(rtc_dev.clock);
+
+	ret = io_read32(get_base() + RTC_CR) & RTC_CR_TAMPTS;
+
+	clk_disable(rtc_dev.clock);
+
+	return ret;
+}
+
+#ifdef CFG_DT
+static unsigned int get_second_clock(void *fdt, int offs)
+{
+	const fdt32_t *cuint = NULL;
+	int len = 0;
+
+	cuint = fdt_getprop(fdt, offs, "clocks", &len);
+	if (len == 3) {
+		EMSG("RTC expects 2 clocks, %d found", len);
+		return DT_INFO_INVALID_CLOCK;
+	}
+
+	return fdt32_to_cpu(*(cuint + 3));
+}
+
+static TEE_Result stm32_rtc_init(void)
+{
+	int node = 0;
+	struct dt_node_info dt_info = { };
+	void *fdt = get_embedded_dt();
+
+	if (!fdt)
+		panic();
+
+	node = fdt_node_offset_by_compatible(fdt, -1, RTC_COMPAT);
+	if (node < 0)
+		return TEE_SUCCESS;
+
+	_fdt_fill_device_info(fdt, &dt_info, node);
+
+	rtc_dev.base.pa = dt_info.reg;
+
+	if (dt_info.status == DT_STATUS_OK_SEC) {
+		rtc_dev.flags |= RTC_FLAGS_SECURE;
+		stm32mp_register_secure_periph_iomem(rtc_dev.base.pa);
+		rtc_dev.base.va = (vaddr_t)phys_to_virt(rtc_dev.base.pa,
+							MEM_AREA_IO_SEC);
+		/* Unbalanced clock enable: keep RTC running */
+		clk_enable(get_second_clock(fdt, node));
+	} else {
+		stm32mp_register_non_secure_periph_iomem(rtc_dev.base.pa);
+		rtc_dev.base.va = (vaddr_t)phys_to_virt(rtc_dev.base.pa,
+							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_tim.c b/core/drivers/stm32_tim.c
new file mode 100644
index 000000000..635e13824
--- /dev/null
+++ b/core/drivers/stm32_tim.c
@@ -0,0 +1,298 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2018-2020, STMicroelectronics
+ */
+
+#include <arm.h>
+#include <drivers/clk.h>
+#include <drivers/serial.h>
+#include <drivers/stm32_tim.h>
+#include <initcall.h>
+#include <io.h>
+#include <keep.h>
+#include <kernel/boot.h>
+#include <kernel/delay.h>
+#include <kernel/dt.h>
+#include <kernel/panic.h>
+#include <libfdt.h>
+#include <mm/core_memprot.h>
+#include <stm32_util.h>
+
+#define TIM_CR1			0x00		/* Control Register 1      */
+#define TIM_CR2			0x04		/* Control Register 2      */
+#define TIM_SMCR		0x08		/* Slave mode control reg  */
+#define TIM_DIER		0x0C		/* DMA/interrupt register  */
+#define TIM_SR			0x10		/* Status register	   */
+#define TIM_EGR			0x14		/* Event Generation Reg    */
+#define TIM_CCMR1		0x18		/* Capt/Comp 1 Mode Reg    */
+#define TIM_CCMR2		0x1C		/* Capt/Comp 2 Mode Reg    */
+#define TIM_CCER		0x20		/* Capt/Comp Enable Reg    */
+#define TIM_CNT			0x24		/* Counter		   */
+#define TIM_PSC			0x28		/* Prescaler               */
+#define TIM_ARR			0x2C		/* Auto-Reload Register    */
+#define TIM_CCR1		0x34		/* Capt/Comp Register 1    */
+#define TIM_CCR2		0x38		/* Capt/Comp Register 2    */
+#define TIM_CCR3		0x3C		/* Capt/Comp Register 3    */
+#define TIM_CCR4		0x40		/* Capt/Comp Register 4    */
+#define TIM_BDTR		0x44		/* Break and Dead-Time Reg */
+#define TIM_DCR			0x48		/* DMA control register    */
+#define TIM_DMAR		0x4C		/* DMA transfer register   */
+#define TIM_AF1			0x60		/* Alt Function Reg 1      */
+#define TIM_AF2			0x64		/* Alt Function Reg 2      */
+#define TIM_TISEL		0x68		/* 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	BIT(2)
+#define TIM_SMCR_TS_SHIFT	4
+#define TIM_SMCR_TS_TI1FP1	0x5
+
+#define TIM_COMPAT		"st,stm32-timers"
+#define TIM_TIMEOUT_US		100000
+#define TIM_TIMEOUT_STEP_US	10
+#define TIM_PRESCAL_HSI		10
+#define TIM_PRESCAL_CSI		7
+#define TIM_MIN_FREQ_CALIB	50000000U
+#define TIM_THRESHOLD		1U
+
+struct stm32_tim_instance {
+	struct io_pa_va base;
+	unsigned long clk;
+	unsigned long freq;
+	uint8_t cal_input;
+};
+
+static vaddr_t timer_base(struct stm32_tim_instance *timer)
+{
+	return io_pa_or_va(&timer->base);
+}
+
+/* Currently support HSI and CSI calibratrion */
+#define TIM_MAX_INSTANCE	2
+static struct stm32_tim_instance stm32_tim[TIM_MAX_INSTANCE];
+
+static int timer_get_dt_node(void *fdt, struct dt_node_info *info, int offset)
+{
+	int node = fdt_node_offset_by_compatible(fdt, offset, TIM_COMPAT);
+
+	if (node < 0)
+		return -FDT_ERR_NOTFOUND;
+
+	_fdt_fill_device_info(fdt, info, node);
+
+	return node;
+}
+
+static int timer_config(struct stm32_tim_instance *timer)
+{
+	vaddr_t base = timer_base(timer);
+
+	clk_enable(timer->clk);
+
+	timer->freq = clk_get_rate(timer->clk);
+	if (timer->freq < TIM_MIN_FREQ_CALIB) {
+		EMSG("Calibration: timer not accurate enough");
+		clk_disable(timer->clk);
+		return -1;
+	}
+
+	if ((io_read32(base + TIM_TISEL) & TIM_TISEL_TI1SEL_MASK) !=
+	    timer->cal_input) {
+		io_clrsetbits32(base + TIM_CCMR1,
+				TIM_CCMR_CC1S_TI1 | TIM_CCMR_CC1S_TI2,
+				TIM_CCMR_CC1S_TI1);
+
+		io_clrbits32(base + TIM_CCER,
+			     TIM_CCER_CC1P | TIM_CCER_CC1NP);
+
+		io_clrsetbits32(base + TIM_SMCR,
+				TIM_SMCR_TS | TIM_SMCR_SMS,
+				(TIM_SMCR_TS_TI1FP1 << TIM_SMCR_TS_SHIFT) |
+				TIM_SMCR_SMS_RESET);
+
+		io_write32(base + TIM_TISEL, timer->cal_input);
+		io_setbits32(base + TIM_CR1, TIM_CR1_CEN);
+		io_setbits32(base + TIM_CCER, TIM_CCER_CC1E);
+	}
+
+	clk_disable(timer->clk);
+
+	return 0;
+}
+
+static uint32_t timer_start_capture(struct stm32_tim_instance *timer)
+{
+	uint64_t timeout_ref = 0;
+	uint64_t timeout_conv = 0;
+	uint32_t counter = 0;
+	uint32_t old_counter = 0;
+	vaddr_t base = timer_base(timer);
+
+	if (timer_config(timer))
+		return 0;
+
+	clk_enable(timer->clk);
+
+	io_write32(base + TIM_SR, 0);
+
+	timeout_ref = timeout_init_us(TIM_TIMEOUT_US / TIM_TIMEOUT_STEP_US);
+
+	while (!timeout_elapsed(timeout_ref))
+		if (io_read32(base + TIM_SR) & TIM_SR_UIF)
+			break;
+	if (!(io_read32(base + TIM_SR) & TIM_SR_UIF))
+		goto bail;
+
+	io_write32(base + TIM_SR, 0);
+
+	timeout_conv = timeout_init_us(TIM_TIMEOUT_US);
+
+	do {
+		if (timeout_elapsed(timeout_conv)) {
+			counter = 0;
+			goto bail;
+		}
+
+		timeout_ref = timeout_init_us(TIM_TIMEOUT_US /
+					      TIM_TIMEOUT_STEP_US);
+		while (!timeout_elapsed(timeout_ref))
+			if (io_read32(base + TIM_SR) & TIM_SR_CC1IF)
+				break;
+
+		if (!(io_read32(base + TIM_SR) & TIM_SR_CC1IF)) {
+			counter = 0;
+			goto bail;
+		}
+
+		old_counter = counter;
+		counter = io_read32(base + TIM_CCR1);
+	} while (MAX(counter, old_counter) - MIN(counter, old_counter) >
+		 TIM_THRESHOLD);
+
+bail:
+	clk_disable(timer->clk);
+
+	return counter;
+}
+
+unsigned long stm32_tim_hsi_freq(void)
+{
+	struct stm32_tim_instance *timer = &stm32_tim[HSI_CAL];
+	uint32_t counter = 0;
+
+	if (timer->base.pa)
+		counter = timer_start_capture(timer);
+
+	if (!counter)
+		return 0;
+
+	return (timer->freq / counter) << TIM_PRESCAL_HSI;
+}
+DECLARE_KEEP_PAGER(stm32_tim_hsi_freq);
+
+unsigned long stm32_tim_csi_freq(void)
+{
+	struct stm32_tim_instance *timer = &stm32_tim[CSI_CAL];
+	uint32_t counter = 0;
+
+	if (timer->base.pa)
+		counter = timer_start_capture(timer);
+
+	if (!counter)
+		return 0;
+
+	return (timer->freq / counter) << TIM_PRESCAL_CSI;
+}
+DECLARE_KEEP_PAGER(stm32_tim_csi_freq);
+
+static void _init_stm32_tim(void)
+{
+	void *fdt = get_embedded_dt();
+	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_tim_instance *timer = NULL;
+		const uint32_t *cuint = NULL;
+
+		if (!(dt_timer.status & DT_STATUS_OK_SEC))
+			continue;
+
+		cuint = fdt_getprop(fdt, node, "st,hsi-cal-input", NULL);
+		if (cuint) {
+			timer = &stm32_tim[HSI_CAL];
+			timer->base.pa = dt_timer.reg;
+			timer->clk = dt_timer.clock;
+			timer->freq = clk_get_rate(timer->clk);
+			timer->cal_input = fdt32_to_cpu(*cuint);
+			if (timer_config(timer)) {
+				timer->base.pa = 0;
+				continue;
+			}
+		}
+
+		cuint = fdt_getprop(fdt, node, "st,csi-cal-input", NULL);
+		if (cuint) {
+			timer = &stm32_tim[CSI_CAL];
+			timer->base.pa = dt_timer.reg;
+			timer->clk = dt_timer.clock;
+			timer->freq = clk_get_rate(timer->clk);
+			timer->cal_input = fdt32_to_cpu(*cuint);
+			if (timer_config(timer)) {
+				timer->base.pa = 0;
+				continue;
+			}
+		}
+	}
+}
+
+void stm32_tim_freq_func(unsigned long (**timer_freq_cb)(void),
+			 enum stm32_tim_cal type)
+{
+	_init_stm32_tim();
+
+	*timer_freq_cb = NULL;
+
+	switch (type) {
+	case HSI_CAL:
+		if (stm32_tim[HSI_CAL].base.pa)
+			*timer_freq_cb = stm32_tim_hsi_freq;
+		break;
+
+	case CSI_CAL:
+		if (stm32_tim[CSI_CAL].base.pa)
+			*timer_freq_cb = stm32_tim_csi_freq;
+		break;
+	default:
+		panic();
+	}
+}
+
+static TEE_Result init_stm32_tim(void)
+{
+	_init_stm32_tim();
+
+	return TEE_SUCCESS;
+}
+driver_init(init_stm32_tim);
diff --git a/core/drivers/sub.mk b/core/drivers/sub.mk
index 25d69eea8..0d3b81e21 100644
--- a/core/drivers/sub.mk
+++ b/core/drivers/sub.mk
@@ -26,13 +26,17 @@ 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_TIM) += stm32_tim.c
 srcs-$(CFG_STM32_UART) += stm32_uart.c
 srcs-$(CFG_STPMIC1) += stpmic1.c
 srcs-$(CFG_BCM_HWRNG) += bcm_hwrng.c
 srcs-$(CFG_BCM_SOTP) += bcm_sotp.c
 srcs-$(CFG_BCM_GPIO) += bcm_gpio.c
 
+subdirs-y += clk
 subdirs-y += crypto
 subdirs-$(CFG_BNXT_FW) += bnxt
 subdirs-$(CFG_SCMI_MSG_DRIVERS) += scmi-msg
diff --git a/core/include/drivers/clk.h b/core/include/drivers/clk.h
new file mode 100644
index 000000000..8c0c44b34
--- /dev/null
+++ b/core/include/drivers/clk.h
@@ -0,0 +1,35 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef CLK_H
+#define CLK_H
+
+#include <stdbool.h>
+#include <tee_api_types.h>
+
+#define CLK_UNKNOWN_ID	ULONG_MAX
+
+/*
+ * Minimal generic clock framework where platform is expected implement a
+ * single clock provider and each individual clock identified with a unique
+ * unsigned long identifier.
+ */
+struct clk_ops {
+	TEE_Result (*enable)(unsigned long id);
+	void (*disable)(unsigned long id);
+	unsigned long (*get_rate)(unsigned long id);
+	unsigned long (*get_parent)(unsigned long id);
+	bool (*is_enabled)(unsigned long id);
+};
+
+TEE_Result clk_enable(unsigned long id);
+void clk_disable(unsigned long id);
+unsigned long clk_get_rate(unsigned long id);
+bool clk_is_enabled(unsigned long id);
+unsigned long clk_get_parent(unsigned long id);
+
+void clk_provider_register(const struct clk_ops *ops);
+
+#endif /* CLK_H */
diff --git a/core/include/drivers/gic.h b/core/include/drivers/gic.h
index f9bb28ec1..c8da6d849 100644
--- a/core/include/drivers/gic.h
+++ b/core/include/drivers/gic.h
@@ -9,17 +9,38 @@
 #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
 #define GIC_SGI(x)		(x)
 #define GIC_PPI(x)		((x) + 16)
 #define GIC_SPI(x)		((x) + 32)
 
+/*
+ * Save and restore some interrupts configuration during low power sequences.
+ * This is used on platforms using OP-TEE secure monitor.
+ */
+struct gic_it_pm;
+
+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_ARM_GIC_PM)
+	struct gic_pm pm;
+#endif
 };
 
 /*
@@ -31,6 +52,9 @@ void gic_init(struct gic_data *gd, vaddr_t gicc_base, vaddr_t gicd_base);
 /* initial base address only */
 void gic_init_base_addr(struct gic_data *gd, vaddr_t gicc_base,
 			vaddr_t gicd_base);
+/* Setup GIC default configuration */
+void gic_init_setup(struct gic_data *gd);
+
 /* initial cpu if only, mainly use for secondary cpu setup cpu interface */
 void gic_cpu_init(struct gic_data *gd);
 
diff --git a/core/include/drivers/stm32_bsec.h b/core/include/drivers/stm32_bsec.h
index b6afbf8c4..9fe85bc98 100644
--- a/core/include/drivers/stm32_bsec.h
+++ b/core/include/drivers/stm32_bsec.h
@@ -10,6 +10,12 @@
 #include <stdint.h>
 #include <tee_api.h>
 
+/* BSEC_DEBUG */
+#define BSEC_HDPEN			BIT(4)
+#define BSEC_SPIDEN			BIT(5)
+#define BSEC_SPINDEN			BIT(6)
+#define BSEC_DBGSWGEN			BIT(10)
+
 /*
  * Load OTP from SAFMEM and provide its value
  * @value: Output read value
@@ -147,4 +153,14 @@ TEE_Result stm32_bsec_otp_lock(uint32_t service);
  */
 bool stm32_bsec_nsec_can_access_otp(uint32_t otp_id);
 
+/*
+ * Find and get OTP location from its name.
+ * @name: sub-node name to look up.
+ * @otp_id: pointer to read OTP number or NULL.
+ * @otp_bit_len: pointer to read OTP length in bits or NULL.
+ * Return a TEE_Result compliant status
+ */
+TEE_Result stm32_bsec_find_otp_in_nvmem_layout(const char *name,
+					       uint32_t *otp_id,
+					       size_t *otp_bit_len);
 #endif /*__STM32_BSEC_H*/
diff --git a/core/include/drivers/stm32_iwdg.h b/core/include/drivers/stm32_iwdg.h
new file mode 100644
index 000000000..f06325812
--- /dev/null
+++ b/core/include/drivers/stm32_iwdg.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2018-2019, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef __STM32_IWDG_H__
+#define __STM32_IWDG_H__
+
+#include <stdint.h>
+
+#define IWDG_HW_ENABLED			BIT(0)
+#define IWDG_DISABLE_ON_STOP		BIT(1)
+#define IWDG_DISABLE_ON_STANDBY		BIT(2)
+
+void stm32_iwdg_refresh(unsigned int instance);
+
+#endif /*__STM32_IWDG_H__*/
diff --git a/core/include/drivers/stm32_rtc.h b/core/include/drivers/stm32_rtc.h
new file mode 100644
index 000000000..b0f4ef858
--- /dev/null
+++ b/core/include/drivers/stm32_rtc.h
@@ -0,0 +1,60 @@
+/* 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>
+
+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;
+};
+
+/* Get calendar formated time from RTC device */
+void stm32_rtc_get_calendar(struct stm32_rtc_calendar *calendar);
+
+/* Return time diff in milliseconds between current and reference time */
+unsigned long long stm32_rtc_diff_calendar(struct stm32_rtc_calendar *cur,
+					   struct stm32_rtc_calendar *ref);
+
+/* Enable tamper and secure timestamp access in RTC */
+void stm32_rtc_set_tamper_timestamp(void);
+
+/* Retrun true if and only if RTC timestamp is enable */
+bool stm32_rtc_is_timestamp_enable(void);
+
+/* Get RTC timestamp for current time */
+void stm32_rtc_get_timestamp(struct stm32_rtc_time *tamp_ts);
+
+#endif /* __PLAT_RTC_H__ */
diff --git a/core/include/drivers/stm32_tim.h b/core/include/drivers/stm32_tim.h
new file mode 100644
index 000000000..2373561a3
--- /dev/null
+++ b/core/include/drivers/stm32_tim.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2018-2019, STMicroelectronics
+ */
+
+#ifndef STM32_TIM_H
+#define STM32_TIM_H
+
+enum stm32_tim_cal {
+	HSI_CAL = 0,
+	CSI_CAL
+};
+
+unsigned long stm32_tim_hsi_freq(void);
+unsigned long stm32_tim_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_tim_freq_func(unsigned long (**timer_freq_cb)(void),
+			 enum stm32_tim_cal type);
+
+#endif /* STM32_TIM_H */
diff --git a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.h b/core/include/drivers/stm32mp1_rcc.h
similarity index 95%
rename from core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.h
rename to core/include/drivers/stm32mp1_rcc.h
index 4dedbad27..6c92257b6 100644
--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_rcc.h
+++ b/core/include/drivers/stm32mp1_rcc.h
@@ -1,13 +1,11 @@
 /* SPDX-License-Identifier: BSD-3-Clause */
 /*
- * Copyright (c) 2017-2018, STMicroelectronics
+ * Copyright (c) 2017-2020, STMicroelectronics
  */
 
 #ifndef __STM32MP1_RCC_H__
 #define __STM32MP1_RCC_H__
 
-#include <io.h>
-#include <stdbool.h>
 #include <util.h>
 
 #define RCC_TZCR			0x00
@@ -392,7 +390,8 @@
 #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)
+#define RCC_HSICFGR_HSICAL_MASK		GENMASK_32(24, 16)
+#define RCC_HSICFGR_HSICAL_TEMP_MASK	GENMASK_32(27, 25)
 
 /* Fields of RCC_CSICFGR register */
 #define RCC_CSICFGR_CSITRIM_SHIFT	8
@@ -455,6 +454,9 @@
 #define RCC_MP_SREQCLRR_STPREQ_P0	BIT(0)
 #define RCC_MP_SREQCLRR_STPREQ_P1	BIT(1)
 
+/* Values of RCC_PWRLPDLYCR register */
+#define RCC_PWRLPDLYCR_PWRLP_DLY_MASK	GENMASK_32(21, 0)
+
 /* Global Control Register */
 #define RCC_MP_GCR_BOOT_MCU		BIT(0)
 
@@ -511,6 +513,9 @@
 #define RCC_AHB5RSTSETR_RNG1RST			BIT(6)
 #define RCC_AHB5RSTSETR_AXIMCRST		BIT(16)
 
+/* RCC_MP_AHB6RST(SET|CLR)R bit fields */
+#define RCC_AHB6RSTSETR_GPURST			BIT(5)
+
 /* RCC_MP_AHB5EN(SET|CLR)R bit fields */
 #define RCC_MP_AHB5ENSETR_GPIOZEN_POS		0
 #define RCC_MP_AHB5ENSETR_CRYP1EN_POS		4
@@ -534,26 +539,43 @@
 #define RCC_MP_AHB5LPENSETR_BKPSRAMLPEN		BIT(8)
 
 /* RCC_MP_TZAHB6EN(SET|CLR)R bit fields */
-#define RCC_MP_TZAHB6ENSETR_MDMA_POS	0
-#define RCC_MP_TZAHB6ENSETR_MDMA	BIT(RCC_MP_TZAHB6ENSETR_MDMA_POS)
+#define RCC_MP_TZAHB6ENSETR_MDMA_POS		0
+#define RCC_MP_TZAHB6ENSETR_MDMA		\
+					BIT(RCC_MP_TZAHB6ENSETR_MDMA_POS)
 
 /* RCC_MP_IWDGFZ(SET|CLR)R bit fields */
 #define RCC_MP_IWDGFZSETR_IWDG1			BIT(0)
 #define RCC_MP_IWDGFZSETR_IWDG2			BIT(1)
 
 #define DT_RCC_CLK_COMPAT	"st,stm32mp1-rcc"
+#define DT_RCC_SEC_CLK_COMPAT	"st,stm32mp1-rcc-secure"
 
 #ifndef __ASSEMBLER__
+#include <io.h>
+#include <stdbool.h>
+#include <types_ext.h>
+
 vaddr_t stm32_rcc_base(void);
 
 static inline bool stm32_rcc_is_secure(void)
 {
-	return io_read32(stm32_rcc_base() + RCC_TZCR) & RCC_TZCR_TZEN;
+	static int state = -1;
+
+	if (state < 0)
+		state = io_read32(stm32_rcc_base() + RCC_TZCR) & RCC_TZCR_TZEN;
+
+	return state;
 }
 
 static inline bool stm32_rcc_is_mckprot(void)
 {
-	return io_read32(stm32_rcc_base() + RCC_TZCR) & RCC_TZCR_MCKPROT;
+	const uint32_t mask = RCC_TZCR_TZEN | RCC_TZCR_MCKPROT;
+	static int state = -1;
+
+	if (state < 0)
+		state = (io_read32(stm32_rcc_base() + RCC_TZCR) & mask) == mask;
+
+	return state;
 }
 #endif /*__ASSEMBLER__*/
 
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 000000000..de7d1604c
--- /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/power/stm32mp1-power.h b/core/include/dt-bindings/power/stm32mp1-power.h
new file mode 100644
index 000000000..bfb7f785e
--- /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/arch/arm/plat-stm32mp1/drivers/stm32mp1_etzpc.h b/core/include/dt-bindings/soc/st,stm32-etzpc.h
similarity index 83%
rename from core/arch/arm/plat-stm32mp1/drivers/stm32mp1_etzpc.h
rename to core/include/dt-bindings/soc/st,stm32-etzpc.h
index 338a39283..6678b8e66 100644
--- a/core/arch/arm/plat-stm32mp1/drivers/stm32mp1_etzpc.h
+++ b/core/include/dt-bindings/soc/st,stm32-etzpc.h
@@ -1,26 +1,35 @@
-/* SPDX-License-Identifier: GPL-2.0 or BSD-3-Clause */
 /*
- * Copyright (C) 2018-2019, STMicroelectronics
+ * Copyright (C) 2017, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier:	GPL-2.0+	BSD-3-Clause
  */
 
-#ifndef __STM32MP1_ETZPC_H
-#define __STM32MP1_ETZPC_H
+#ifndef _DT_BINDINGS_STM32_ETZPC_H
+#define _DT_BINDINGS_STM32_ETZPC_H
 
-/* Define DECPROT IDs for stm32mp1 familly */
+/*  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
-/* 13-15 Reserved */
 #define STM32MP1_ETZPC_TIM2_ID		16
 #define STM32MP1_ETZPC_TIM3_ID		17
 #define STM32MP1_ETZPC_TIM4_ID		18
@@ -47,12 +56,9 @@
 #define STM32MP1_ETZPC_DAC_ID		39
 #define STM32MP1_ETZPC_UART7_ID		40
 #define STM32MP1_ETZPC_UART8_ID		41
-/* 42-43 Reserved */
 #define STM32MP1_ETZPC_MDIOS_ID		44
-/* 45-47 Reserved */
 #define STM32MP1_ETZPC_TIM1_ID		48
 #define STM32MP1_ETZPC_TIM8_ID		49
-/* 50 Reserved */
 #define STM32MP1_ETZPC_USART6_ID	51
 #define STM32MP1_ETZPC_SPI1_ID		52
 #define STM32MP1_ETZPC_SPI4_ID		53
@@ -65,7 +71,6 @@
 #define STM32MP1_ETZPC_SAI3_ID		60
 #define STM32MP1_ETZPC_DFSDM_ID		61
 #define STM32MP1_ETZPC_TT_FDCAN_ID	62
-/* 63 Reserved */
 #define STM32MP1_ETZPC_LPTIM2_ID	64
 #define STM32MP1_ETZPC_LPTIM3_ID	65
 #define STM32MP1_ETZPC_LPTIM4_ID	66
@@ -78,7 +83,6 @@
 #define STM32MP1_ETZPC_HASH2_ID		73
 #define STM32MP1_ETZPC_RNG2_ID		74
 #define STM32MP1_ETZPC_CRYP2_ID		75
-/* 76-79 Reserved */
 #define STM32MP1_ETZPC_SRAM1_ID		80
 #define STM32MP1_ETZPC_SRAM2_ID		81
 #define STM32MP1_ETZPC_SRAM3_ID		82
@@ -94,7 +98,10 @@
 #define STM32MP1_ETZPC_QSPI_ID		92
 #define STM32MP1_ETZPC_DLYBQ_ID		93
 #define STM32MP1_ETZPC_ETH_ID		94
-/* 95 Reserved */
+
 #define STM32MP1_ETZPC_MAX_ID		96
 
-#endif /*__STM32MP1_ETZPC_H*/
+#define DECPROT(id, mode, lock)		(((id) << 16) | ((mode) << 8) | (lock))
+
+#endif /* _DT_BINDINGS_STM32_ETZPC_H */
+
diff --git a/core/include/kernel/interrupt.h b/core/include/kernel/interrupt.h
index 849e987f9..796eded29 100644
--- a/core/include/kernel/interrupt.h
+++ b/core/include/kernel/interrupt.h
@@ -25,6 +25,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 {
@@ -66,4 +71,14 @@ void itr_set_affinity(size_t it, uint8_t cpu_mask);
  */
 void itr_core_handler(void);
 
+/*
+ * 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/include/kernel/panic.h b/core/include/kernel/panic.h
index b9a56dc2c..0f0d0794e 100644
--- a/core/include/kernel/panic.h
+++ b/core/include/kernel/panic.h
@@ -8,6 +8,12 @@
 
 #include <compiler.h>
 
+/*
+ * Platform can define a panic sequence to trap cpu/reset core or system
+ * after eventual debug trace.
+ */
+void plat_panic(void);
+
 /* debug disabled => __FILE__, ... and panic message are not built. */
 #if defined(CFG_TEE_CORE_DEBUG)
 #define __panic(str)	__do_panic(__FILE__, __LINE__, __func__, str)
diff --git a/core/kernel/interrupt.c b/core/kernel/interrupt.c
index 961ce6be3..376012556 100644
--- a/core/kernel/interrupt.c
+++ b/core/kernel/interrupt.c
@@ -88,3 +88,13 @@ void __weak __noreturn itr_core_handler(void)
 {
 	panic("Secure interrupt handler not defined");
 }
+
+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/kernel/panic.c b/core/kernel/panic.c
index f30c6ffe7..023f4be5d 100644
--- a/core/kernel/panic.c
+++ b/core/kernel/panic.c
@@ -7,8 +7,21 @@
 #include <kernel/panic.h>
 #include <kernel/thread.h>
 #include <kernel/unwind.h>
+#include <stdbool.h>
 #include <trace.h>
 
+static void __noreturn stall_cpu(void)
+{
+	while (true)
+		;
+}
+
+void __weak __noreturn plat_panic(void)
+{
+	/* abort current execution */
+	stall_cpu();
+}
+
 void __do_panic(const char *file __maybe_unused,
 		const int line __maybe_unused,
 		const char *func __maybe_unused,
@@ -17,8 +30,6 @@ void __do_panic(const char *file __maybe_unused,
 	/* disable prehemption */
 	(void)thread_mask_exceptions(THREAD_EXCP_ALL);
 
-	/* TODO: notify other cores */
-
 	/* trace: Panic ['panic-string-message' ]at FILE:LINE [<FUNCTION>]" */
 	if (!file && !func && !msg)
 		EMSG_RAW("Panic");
@@ -29,7 +40,9 @@ void __do_panic(const char *file __maybe_unused,
 			 func ? "<" : "", func ? func : "", func ? ">" : "");
 
 	print_kernel_stack();
-	/* abort current execution */
-	while (1)
-		;
+
+	plat_panic();
+
+	EMSG("platform failed to abord execution");
+	stall_cpu();
 }
diff --git a/core/mm/vm.c b/core/mm/vm.c
index 27ffa18af..95080a700 100644
--- a/core/mm/vm.c
+++ b/core/mm/vm.c
@@ -162,14 +162,26 @@ static TEE_Result alloc_pgt(struct user_mode_ctx *uctx)
 	return TEE_SUCCESS;
 }
 
-static void maybe_free_pgt(struct user_mode_ctx *uctx, struct vm_region *r)
+static void rem_um_region(struct user_mode_ctx *uctx, struct vm_region *r)
 {
-	struct thread_specific_data *tsd = NULL;
+	struct thread_specific_data *tsd = thread_get_tsd();
 	struct pgt_cache *pgt_cache = NULL;
 	vaddr_t begin = ROUNDDOWN(r->va, CORE_MMU_PGDIR_SIZE);
 	vaddr_t last = ROUNDUP(r->va + r->size, CORE_MMU_PGDIR_SIZE);
 	struct vm_region *r2 = NULL;
 
+	if (uctx->ts_ctx == tsd->ctx)
+		pgt_cache = &tsd->pgt_cache;
+
+	if (mobj_is_paged(r->mobj)) {
+		tee_pager_rem_um_region(uctx, r->va, r->size);
+	} else {
+		pgt_clear_ctx_range(pgt_cache, uctx->ts_ctx, r->va,
+				    r->va + r->size);
+		tlbi_mva_range_asid(r->va, r->size, SMALL_PAGE_SIZE,
+				    uctx->vm_info.asid);
+	}
+
 	r2 = TAILQ_NEXT(r, link);
 	if (r2)
 		last = MIN(last, ROUNDDOWN(r2->va, CORE_MMU_PGDIR_SIZE));
@@ -183,10 +195,6 @@ static void maybe_free_pgt(struct user_mode_ctx *uctx, struct vm_region *r)
 	if (begin >= last)
 		return;
 
-	tsd = thread_get_tsd();
-	if (uctx->ts_ctx == tsd->ctx)
-		pgt_cache = &tsd->pgt_cache;
-
 	pgt_flush_ctx_range(pgt_cache, uctx->ts_ctx, r->va, r->va + r->size);
 }
 
@@ -539,9 +547,7 @@ TEE_Result vm_remap(struct user_mode_ctx *uctx, vaddr_t *new_va, vaddr_t old_va,
 		if (r->va + r->size > old_va + len)
 			break;
 		r_next = TAILQ_NEXT(r, link);
-		if (fobj)
-			tee_pager_rem_um_region(uctx, r->va, r->size);
-		maybe_free_pgt(uctx, r);
+		rem_um_region(uctx, r);
 		TAILQ_REMOVE(&uctx->vm_info.regions, r, link);
 		TAILQ_INSERT_TAIL(&regs, r, link);
 	}
@@ -774,21 +780,13 @@ TEE_Result vm_unmap(struct user_mode_ctx *uctx, vaddr_t va, size_t len)
 	while (true) {
 		r_next = TAILQ_NEXT(r, link);
 		unmap_end_va = r->va + r->size;
-		if (mobj_is_paged(r->mobj))
-			tee_pager_rem_um_region(uctx, r->va, r->size);
-		maybe_free_pgt(uctx, r);
+		rem_um_region(uctx, r);
 		umap_remove_region(&uctx->vm_info, r);
 		if (!r_next || unmap_end_va == end_va)
 			break;
 		r = r_next;
 	}
 
-	/*
-	 * Synchronize change to translation tables. Even though the pager
-	 * case unmaps immediately we may still free a translation table.
-	 */
-	vm_set_ctx(uctx->ts_ctx);
-
 	return TEE_SUCCESS;
 }
 
@@ -843,9 +841,7 @@ void vm_clean_param(struct user_mode_ctx *uctx)
 
 	TAILQ_FOREACH_SAFE(r, &uctx->vm_info.regions, link, next_r) {
 		if (r->flags & VM_FLAG_EPHEMERAL) {
-			if (mobj_is_paged(r->mobj))
-				tee_pager_rem_um_region(uctx, r->va, r->size);
-			maybe_free_pgt(uctx, r);
+			rem_um_region(uctx, r);
 			umap_remove_region(&uctx->vm_info, r);
 		}
 	}
@@ -1055,9 +1051,7 @@ void vm_rem_rwmem(struct user_mode_ctx *uctx, struct mobj *mobj, vaddr_t va)
 
 	TAILQ_FOREACH(r, &uctx->vm_info.regions, link) {
 		if (r->mobj == mobj && r->va == va) {
-			if (mobj_is_paged(r->mobj))
-				tee_pager_rem_um_region(uctx, r->va, r->size);
-			maybe_free_pgt(uctx, r);
+			rem_um_region(uctx, r);
 			umap_remove_region(&uctx->vm_info, r);
 			return;
 		}
diff --git a/core/sub.mk b/core/sub.mk
index 5a17cd923..faaf21c74 100644
--- a/core/sub.mk
+++ b/core/sub.mk
@@ -20,13 +20,16 @@ recipe-ldelf = $(PYTHON3) scripts/gen_ldelf_hex.py --input $(out-dir)/ldelf/ldel
 endif
 
 ifeq ($(CFG_WITH_USER_TA)-$(CFG_EARLY_TA),y-y)
+ifeq ($(CFG_EARLY_TA_COMPRESS),y)
+early-ta-compress = --compress
+endif
 define process_early_ta
 early-ta-$1-uuid := $(firstword $(subst ., ,$(notdir $1)))
 gensrcs-y += early-ta-$1
 produce-early-ta-$1 = early_ta_$$(early-ta-$1-uuid).c
 depends-early-ta-$1 = $1 scripts/ts_bin_to_c.py
-recipe-early-ta-$1 = $(PYTHON3) scripts/ts_bin_to_c.py --compress --ta $1 \
-		--out $(sub-dir-out)/early_ta_$$(early-ta-$1-uuid).c
+recipe-early-ta-$1 = $(PYTHON3) scripts/ts_bin_to_c.py $(early-ta-compress) \
+		--ta $1 --out $(sub-dir-out)/early_ta_$$(early-ta-$1-uuid).c
 endef
 $(foreach f, $(EARLY_TA_PATHS), $(eval $(call process_early_ta,$(f))))
 $(foreach f, $(CFG_IN_TREE_EARLY_TAS), $(eval $(call \
diff --git a/keys/default_rproc.pem b/keys/default_rproc.pem
new file mode 100644
index 000000000..d54476f35
--- /dev/null
+++ b/keys/default_rproc.pem
@@ -0,0 +1,27 @@
+-----BEGIN RSA PRIVATE KEY-----
+MIIEpAIBAAKCAQEA1TD3AMwyJhzsliOhEStPrCrgnG9Gy8z+OCtialVngW/rkqQB
+cb/oOdiMPjfLoSio8zaUA86mTHo6DINZIopTZvs9pysnpvrOwbjCVRXTcIhgM/YZ
+GyN9c+Qo5fCbOlCsxD0PG40hP4O0dktWyzmWQfjy0+9BDAyMoW59lPeYZcJAKSWT
+M10V5h3JTabA4dqVroeztuTow3ftNImNuzMFYDqGDUcJy0EdluRsBfhOOKXE7ZaQ
+RXnwY9CTCGqgsuNKwE1g8evkseaLcJk4/JpVFOgZp4fUgsxU6EBRD2i+C+Jq9KXg
+qBcK0QwXNr2IwG3i76QmLzGGkpW7bKPn/QhGMQIDAQABAoIBAQCHBnAqzSmmAgPG
+Q+KroSbhAHcqHUBVrAwHP1Mhzd20mVI2mjFf/g/zMzf/4A7Uj5ASGqs8jhG9tlw1
+uKsnuTyBqPavfiGrHIb/IynSAfTc/UMRJflYuu2mDQfqOq3WDWqfD50V8hjwxVXy
+5lyecma8ehQyLwKfwwL+66AWTYr0Rx+OdXkGdGj1SXbJU39nv7UH8ZggpICFhUXO
+r7NgJr2UOyhEje4stTGSb86SrvxHRm07JG1WSOJ2GV0EBhWNqbRtsnvH1NKNm03m
+yM+zMbqvtdg1Wkfcfxtx6h7MGHhUUIHwotDoKgZ1lz1EdVWk8clmlKCLFEBxcwIp
+sdbrodnBAoGBAPFQ1J7B+3eDBJjgrEFu9soygrjulhiA2+3qrra95RyuIE0wE8CO
+DPjAGHOnxAoUt8H+TEGF2Wo2HVQauz2ElPOlIxQctr3w2Xpi1DibKEdooQLRlaSS
+LHWAxTLZj9EI3NuYhdRvFHUW2ExEqCADOKY4+1xRSXuIbIkaLMaah8R9AoGBAOIq
+BEiqoaQM/EA4z+m7RuOjc1PtA0qo9DJIEb4Mj92gEGzSHCdNN+eDCKbqyd8BGo/w
+Pf6N8+dotjYvzlz3MqwfV1aczvQqbfkivzahO7GlK2KmwwLG3Vm8dZAZCZKEglOg
+ex6h/L8gOgqSP+C4OmdEU4jdA24BSHr+1ZArHPrFAoGBAL27l/DbBCSLVun8fHNW
+E6QW4sEUld7eMg12H7h+xc0u+ya1TlJvXbOXFaKZnYFvmKtmjf5WhwMDWTvvaJiN
+za9jf5kommXtIJEhc0quc5TxpubYcpfadipM/L9mX7UzCrN90Hueeq81LwuIT8gb
+wEaxNrD3GJeQRAXoFpxwk57hAoGAaeqLfwyKDq4WJG120VtnY4xUomVJOVnOow2l
+YX+4kG45wvzTOoSrPbzb/G/QgqOdsPMt1VzdcO5VByN0XY1XKcyztlhRg3+raRWg
+vxDbR+K2Ysj+YvqHB1N/KzDOjtOHxWpOvpXWLBwHkpPTXoZos5wIEvyOcqIfM5rM
+oWvPcpECgYBCtncnPQQJfYcFebw3+rzm9OodF/s6G9afOrzqgEk/0Z6mi20x1c23
+dzcZUpLl9p7fCFg0iz9NwLUYR9tZ/4zy+J4ZET7PduxoLG3m1TVzxSJM+dU+GtQi
+fBcdQuC6od9K9MJD2egwmes/I+aWhrIAatrG2iMtrOTG5N0mUMUc+w==
+-----END RSA PRIVATE KEY-----
diff --git a/lib/libutee/include/remoteproc_pta.h b/lib/libutee/include/remoteproc_pta.h
new file mode 100644
index 000000000..cede192eb
--- /dev/null
+++ b/lib/libutee/include/remoteproc_pta.h
@@ -0,0 +1,157 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef __REMOTEPROC_PTA_H
+#define __REMOTEPROC_PTA_H
+
+#include <util.h>
+
+/*
+ * Interface to the pseudo TA which provides platform implementation
+ * of the remote processor management
+ */
+
+#define PTA_REMOTEPROC_UUID { 0x54af4a68, 0x19be, 0x40d7, \
+		{ 0xbb, 0xe6, 0x89, 0x50, 0x35, 0x0a, 0x87, 0x44 } }
+
+/* Firmware format */
+#define PTA_REMOTEPROC_PROPRIETARY_FMT		BIT32(0)
+#define PTA_REMOTEPROC_ELF_FMT			BIT32(1)
+
+/* Firmware image protection */
+/* The platorm supports copy of the input firmware image in secure memory */
+#define PTA_REMOTEPROC_FW_SECURE_COPY		BIT32(0)
+/* The platorm supports load of segment with hash protection */
+#define PTA_REMOTEPROC_FW_WITH_HASH_TABLE	BIT32(1)
+/* The platorm is able to change access to secure the firmware input image */
+#define PTA_REMOTEPROC_FW_MEMORY_PROTECTION	BIT32(2)
+
+/**
+ * struct rproc_pta_key_info - public key information
+ * @algo:	Algorithm, defined by public key algorithms TEE_ALG_*
+ *		from TEE Internal API specification
+ * @info_size:	Byte size of the @info
+ * @info:	Append key information data
+ */
+struct rproc_pta_key_info {
+	uint32_t algo;
+	uint32_t info_size;
+	char info[];
+};
+
+static inline size_t
+	rproc_pta_get_keyinfo_size(struct rproc_pta_key_info *keyinf)
+{
+	size_t s = 0;
+
+	if (!keyinf || ADD_OVERFLOW(sizeof(*keyinf), keyinf->info_size, &s))
+		return 0;
+
+	return s;
+}
+
+#define RPROC_PTA_GET_KEYINFO_SIZE(x)	rproc_pta_get_keyinfo_size((x))
+
+/*
+ * Platform capabilities.
+ *
+ * Get Platform firmware loader service capabilities.
+ *
+ * [in]  params[0].value.a:	Unique 32bit firmware identifier
+ * [out] params[1].value.a:	Firmware format (PTA_REMOTEPROC_*_FMT)
+ * [out] params[2].value.a:	Image protection method (PTA_REMOTEPROC_FW_*)
+ */
+#define PTA_REMOTEPROC_HW_CAPABILITIES		1
+
+/*
+ * Firmware loading.
+ *
+ * Optional service to implement only in case of proprietary format.
+ *
+ * [in]  params[0].value.a:	Unique 32bit firmware identifier
+ * [in]  params[1].memref:	Loadable firmware image
+ */
+#define PTA_REMOTEPROC_FIRMWARE_LOAD		2
+
+/*
+ * Load a segment with a SHA256 hash.
+ *
+ * This command is used when the platform secure memory is too expensive to
+ * save the whole firmware image in secure memory. Upon segment load, a
+ * successful completion ensures the loaded image complies with the provided
+ * hash.
+ *
+ * [in]  params[0].value.a:	Unique 32bit firmware identifier
+ * [in]  params[1].memref:	Section data to load
+ * [in]  params[2].value.a:	32bit LSB load device segment address
+ * [in]  params[2].value.b:	32bit MSB load device segment address
+ * [in]  params[3].memref:	Expected hash (SHA256) of the payload
+ */
+#define PTA_REMOTEPROC_LOAD_SEGMENT_SHA256	3
+
+/*
+ * Memory set.
+ *
+ * Fill a remote device memory with requested value. this is use for instance
+ * to clear a memory on the remote firmware load.
+ *
+ * [in]  params[0].value.a:	Unique 32bit firmware identifier
+ * [in]  params[1].value.a:	32bit LSB device memory address
+ * [in]  params[1].value.b:	32bit MSB device memory address
+ * [in]  params[2].value.a:	32bit LSB device memory size
+ * [in]  params[2].value.b:	32bit MSB device memory size
+ * [in]  params[3].value.a:	Byte value to be set
+ */
+#define PTA_REMOTEPROC_SET_MEMORY		4
+
+/*
+ * Firmware start.
+ *
+ * Start up a successfully remote processor firmware.
+ *
+ * [in]  params[0].value.a:	Unique 32bit firmware identifier
+ */
+#define PTA_REMOTEPROC_FIRMWARE_START		5
+
+/*
+ * Firmware stop.
+ *
+ * Stop of the remote processor firmware and release/clean resources.
+ * After the command successful completion, remote processor firmware must be
+ * reloaded prior being started again.
+ *
+ * [in]  params[0].value.a:	Unique 32bit firmware identifier
+ */
+#define PTA_REMOTEPROC_FIRMWARE_STOP		6
+
+/*
+ * Firmware device to physical address conversion.
+ *
+ * Return the physical address corresponding to an address got from the
+ * firmware address layout.
+ *
+ * [in]  params[0].value.a:	Unique 32bit firmware identifier
+ * [in]  params[1].value.a:	32bit LSB Device memory address
+ * [in]  params[1].value.b:	32bit MSB Device memory address
+ * [in]  params[2].value.a:	32bit LSB Device memory size
+ * [in]  params[2].value.b:	32bit MSB Device memory size
+ * [out] params[3].value.a:	32bit LSB converted physical address
+ * [out] params[3].value.b:	32bit MSB converted physical address
+ */
+#define PTA_REMOTEPROC_FIRMWARE_DA_TO_PA	7
+
+/*
+ * Verify the firmware digest against a signature
+ *
+ * Return TEE_SUCCESS if the signature is verified,  else an error
+ *
+ * [in]  params[0].value.a:	Unique 32bit firmware identifier
+ * [in]  params[1].memref:	Key information (refer to @rproc_pta_key_info)
+ * [in]  params[2].memref:	Digest of the firmware authenticated data
+ * [in]  params[3].memref:	Signature of the firmware authenticated data
+ */
+#define PTA_REMOTEPROC_VERIFY_DIGEST	8
+
+#endif /* __REMOTEPROC_PTA_H */
diff --git a/mk/config.mk b/mk/config.mk
index 72f13fba4..a14c4f832 100644
--- a/mk/config.mk
+++ b/mk/config.mk
@@ -306,6 +306,9 @@ $(eval $(call cfg-depends-all,CFG_REE_FS_TA_BUFFERED,CFG_REE_FS_TA))
 # in-tree TAs. CFG_IN_TREE_EARLY_TAS is formatted as:
 # <name-of-ta>/<uuid>
 # for instance avb/023f8f1a-292a-432b-8fc4-de8471358067
+#
+# By default the early TAs are compressed in the TEE binary, it is possible to
+# not compress them with CFG_EARLY_TA_COMPRESS=n
 ifneq ($(EARLY_TA_PATHS)$(CFG_IN_TREE_EARLY_TAS),)
 $(call force,CFG_EARLY_TA,y)
 $(call force,CFG_EMBEDDED_TS,y)
@@ -323,6 +326,7 @@ endif
 ifeq ($(CFG_EMBEDDED_TS),y)
 $(call force,CFG_ZLIB,y)
 endif
+CFG_EARLY_TA_COMPRESS ?= y
 
 # Enable paging, requires SRAM, can't be enabled by default
 CFG_WITH_PAGER ?= n
diff --git a/mk/gcc.mk b/mk/gcc.mk
index adc77a24f..81bfa78ad 100644
--- a/mk/gcc.mk
+++ b/mk/gcc.mk
@@ -13,11 +13,11 @@ nostdinc$(sm)	:= -nostdinc -isystem $(shell $(CC$(sm)) \
 			-print-file-name=include 2> /dev/null)
 
 # Get location of libgcc from gcc
-libgcc$(sm)  	:= $(shell $(CC$(sm)) $(CFLAGS$(arch-bits-$(sm))) \
+libgcc$(sm)  	:= $(shell $(CC$(sm)) $(LIBGCC_LOCATE_CFLAGS) $(CFLAGS$(arch-bits-$(sm))) \
 			-print-libgcc-file-name 2> /dev/null)
-libstdc++$(sm)	:= $(shell $(CXX$(sm)) $(CXXFLAGS$(arch-bits-$(sm))) $(comp-cxxflags$(sm)) \
+libstdc++$(sm)	:= $(shell $(CXX$(sm)) $(LIBGCC_LOCATE_CFLAGS) $(CXXFLAGS$(arch-bits-$(sm))) $(comp-cxxflags$(sm)) \
 			-print-file-name=libstdc++.a 2> /dev/null)
-libgcc_eh$(sm)	:= $(shell $(CXX$(sm)) $(CXXFLAGS$(arch-bits-$(sm))) $(comp-cxxflags$(sm)) \
+libgcc_eh$(sm)	:= $(shell $(CXX$(sm)) $(LIBGCC_LOCATE_CFLAGS) $(CXXFLAGS$(arch-bits-$(sm))) $(comp-cxxflags$(sm)) \
 			-print-file-name=libgcc_eh.a 2> /dev/null)
 
 # Define these to something to discover accidental use
diff --git a/scripts/sign_rproc_fw.py b/scripts/sign_rproc_fw.py
new file mode 100755
index 000000000..4248ed280
--- /dev/null
+++ b/scripts/sign_rproc_fw.py
@@ -0,0 +1,416 @@
+#!/usr/bin/env python3
+# SPDX-License-Identifier: BSD-2-Clause
+#
+# Copyright (C) 2020, STMicroelectronics - All Rights Reserved
+#
+
+try:
+    from elftools.elf.elffile import ELFFile
+    from elftools.elf.sections import SymbolTableSection
+    from elftools.elf.enums import ENUM_P_TYPE_BASE
+    from elftools.elf.enums import *
+except ImportError:
+    print("""
+***
+ERROR: pyelftools python module is not installed or version < 0.25.
+***
+""")
+    raise
+
+from Cryptodome.Hash import SHA256
+from Cryptodome.Signature import pkcs1_15
+from Cryptodome.PublicKey import RSA
+from Cryptodome.Signature import DSS
+from Cryptodome.PublicKey import ECC
+import os
+import sys
+import struct
+import logging
+import binascii
+
+
+logging.basicConfig(stream=sys.stderr, level=logging.INFO)
+
+ENUM_HASH_TYPE = dict(
+    SHA256=1,
+)
+
+ENUM_SIGNATURE_TYPE = dict(
+    RSA=1,
+    ECC=2,
+)
+
+ENUM_BINARY_TYPE = dict(
+    ELF=1,
+)
+
+
+def dump_buffer(buf, step=16, name="", logger=logging.info, indent=""):
+    logger("%s%s:" % (indent, name))
+    for i in range(0, len(buf), step):
+        logger("%s    " % (indent) + " ".
+               join(["%02X" % c for c in buf[i:i+step]]))
+    logger("\n")
+
+
+class RSA_Signature(object):
+
+    def __init__(self, key):
+        self._hasher = SHA256.new()
+        self.signer = pkcs1_15.new(key)
+
+    def hash_compute(self, segment):
+        self._hasher.update(segment)
+
+    def sign(self):
+        return self.signer.sign(self._hasher)
+
+
+class ECC_Signature(object):
+
+    def __init__(self, key):
+        self._hasher = SHA256.new()
+        self.signer = DSS.new(key, 'fips-186-3')
+
+    def hash_compute(self, segment):
+        self._hasher.update(segment)
+
+    def sign(self):
+        return self.signer.sign(self._hasher)
+
+
+Signature = {
+        1: RSA_Signature,
+        2: ECC_Signature,
+}
+
+
+class SegmentHashStruct:
+    pass
+
+
+class SegmentHash(object):
+    '''
+        Hash table based on Elf program segments
+    '''
+    def __init__(self, img):
+        self._num_segments = img.num_segments()
+        self._pack_fmt = '<%dL' % 8
+        self.img = img
+        self.hashProgTable = bytes()
+        self._offset = 0
+
+    def get_table(self):
+        '''
+            Create a segment hash table containing for each segment:
+                - the segments header
+                - a hash of the segment
+        '''
+        h = SHA256.new()
+        seg = SegmentHashStruct()
+        self.size = (h.digest_size + 32) * self._num_segments
+        logging.debug("hash section size %d" % self.size)
+        del h
+        self.buf = bytearray(self.size)
+        self._bufview_ = memoryview(self.buf)
+
+        for i in range(self._num_segments):
+            h = SHA256.new()
+            segment = self.img.get_segment(i)
+            seg.header = self.img.get_segment(i).header
+            logging.debug("compute hash for segment offset %s" % seg.header)
+            h.update(segment.data())
+            seg.hash = h.digest()
+            logging.debug("hash computed: %s" % seg.hash)
+            del h
+            struct.pack_into('<I', self._bufview_, self._offset,
+                             ENUM_P_TYPE_BASE[seg.header.p_type])
+            self._offset += 4
+            struct.pack_into('<7I', self._bufview_, self._offset,
+                             seg.header.p_offset, seg.header.p_vaddr,
+                             seg.header.p_paddr, seg.header.p_filesz,
+                             seg.header.p_memsz, seg.header.p_flags,
+                             seg.header.p_align)
+            self._offset += 28
+            struct.pack_into('<32B', self._bufview_, self._offset, *seg.hash)
+            self._offset += 32
+        dump_buffer(self.buf, name='hash table', indent="\t")
+        return self.buf
+
+
+class ImageHeader(object):
+    '''
+        Image header
+    '''
+
+    magic = 'HELF'   # SHDR_MAGIC
+    version = 1
+    sign_type = 1     # SHA256
+    img_type = 1     # ELF
+
+    MAGIC_OFFSET = 0
+    VERSION_OFFSET = 4
+    LENGTH_OFFSET = 8
+    SIGNATURE_LEN_OFFSET = 12
+    SIGNATURE_OFFSET_OFFSET = 16
+    SIGNATURE_TYPE_OFFSET = 20
+    HASH_LEN_OFFSET = 24
+    HASH_OFFSET_OFFSET = 28
+    HASH_TYPE_OFFSET = 32
+    PUBLIC_INFO_LEN_OFFSET = 36
+    PUBLIC_INFO_OFFSET_OFFSET = 40
+    IMG_LEN_OFFSET = 44
+    IMG_OFFSET_OFFSET = 48
+    IMG_TYPE_OFFSET = 52
+
+    def __init__(self):
+        self.size = 56
+
+        self.magic = 0x3543A468
+        self.version = 1
+        self.length = 0
+        self.sign_length = 0
+        self.sign_offset = 0
+        self.sign_type = 0
+        self.hash_length = 0
+        self.hash_offset = 0
+        self.hash_type = 0
+        self.key_length = 0
+        self.key_offset = 0
+        self.img_length = 0
+        self.img_offset = 0
+        self.img_type = 0
+
+        self.shdr = struct.pack('<IIIIIIIIIIIIII',
+                                self.magic, self.version, self.length,
+                                self.sign_length, self.sign_offset,
+                                self.sign_type, self.hash_length,
+                                self.hash_offset, self.hash_type,
+                                self.key_length, self.key_offset,
+                                self.img_length, self.img_offset,
+                                self.img_type)
+
+    def dump(self):
+        logging.info("\tMAGIC\t\t= %08X" % (self.magic))
+        logging.info("\tHEADER_VERSION\t= %08X" % (self.version))
+        logging.info("\tHEADER_LENGTH\t= %08X" % (self.length))
+        logging.info("\tSIGN_LENGTH\t= %08X" % (self.sign_length))
+        logging.info("\tSIGN_OFFSET\t= %08X" % (self.sign_offset))
+        logging.info("\tSIGN_TYPE\t= %08X" % (self.sign_type))
+        logging.info("\tHASH_LENGTH\t= %08X" % (self.hash_length))
+        logging.info("\tHASH_OFFSET\t= %08X" % (self.hash_offset))
+        logging.info("\tHASH_TYPE\t= %08X" % (self.hash_type))
+        logging.info("\tPKEY_LENGTH\t= %08X" % (self.key_length))
+        logging.info("\tPKEY_OFFSET\t= %08X" % (self.key_offset))
+        logging.info("\tIMAGE_LENGTH\t= %08X" % (self.img_length))
+        logging.info("\tIMAGE_OFFSET\t= %08X" % (self.img_offset))
+        logging.info("\tIMAGE_TYPE\t= %08X" % (self.img_type))
+
+    def get_packed(self):
+        return struct.pack('<IIIIIIIIIIIIII',
+                           self.magic, self.version, self.length,
+                           self.sign_length, self.sign_offset, self.sign_type,
+                           self.hash_length, self.hash_offset, self.hash_type,
+                           self.key_length, self.key_offset, self.img_length,
+                           self.img_offset, self.img_type)
+
+
+def get_args(logger):
+    from argparse import ArgumentParser, RawDescriptionHelpFormatter
+    import textwrap
+    command_base = ['sign']
+    command_choices = command_base
+    default_key = os.path.abspath(os.path.dirname(__file__)) + \
+        '/../keys/default_rproc.pem'
+
+    parser = ArgumentParser(
+        description='Sign a remote processor firmware loadable by OP-TEE.',
+        usage='\n   %(prog)s command [ arguments ]\n\n'
+
+        '   command:\n' +
+        '     sign      Generate signed loadable binary \n' +
+        '                 Takes arguments --in, --out --key\n' +
+        '   %(prog)s --help  show available commands and arguments\n\n',
+        formatter_class=RawDescriptionHelpFormatter,
+        epilog=textwrap.dedent(
+            '''If no key is specified, the script will default try to ''' +
+            '''use the following private key:''') + '\n' + default_key
+        )
+    parser.add_argument(
+        'command', choices=command_choices, nargs='?',
+        default='sign',
+        help='Command, one of [' + ', '.join(command_base) + ']')
+    parser.add_argument('--key', required=False,
+                        help='Name of signing key file',
+                        default=default_key,
+                        dest='keyf')
+    parser.add_argument('--key_info', required=False,
+                        help='Name file containing extra key information',
+                        dest='key_infof')
+    parser.add_argument('--key_type', required=False,
+                        help='Type of signing key: should be RSA or ECC',
+                        default='RSA',
+                        dest='key_type')
+    parser.add_argument(
+        '--in', required=True, dest='inf',
+        help='Name of firmware input file')
+    parser.add_argument(
+        '--out', required=False, dest='outf',
+        help='Name of the signed firmware output file,' +
+             ' default <in base name>.sig')
+
+    parsed = parser.parse_args()
+
+    # Set defaults for optional arguments.
+
+    if parsed.outf is None:
+        parsed.outf = str(parsed.inf)+'.sig'
+
+    return parsed
+
+
+def rsa_key(keyf):
+    return RSA.importKey(open(keyf).read())
+
+
+def ecc_key(keyf):
+    return ECC.import_key(open(keyf).read())
+
+
+key_type = {
+        1: rsa_key,
+        2: ecc_key,
+}
+
+
+def rsa_sig_size(key):
+    return key.size_in_bytes()
+
+
+def ecc_sig_size(key):
+    # to be improve...
+    # DSA size is N/4  so 64 for DSA (L,N) = (2048, 256)
+    return 64
+
+
+sig_size_type = {
+        1: rsa_sig_size,
+        2: ecc_sig_size,
+}
+
+
+def main():
+    from Cryptodome.Signature import pss
+    from Cryptodome.Hash import SHA256
+    from Cryptodome.PublicKey import RSA
+    import base64
+    import logging
+    import struct
+
+    logging.basicConfig()
+    logger = logging.getLogger(os.path.basename(__file__))
+
+    args = get_args(logger)
+
+    # Initialise the header */
+    s_header = ImageHeader()
+
+    get_key = key_type.get(ENUM_SIGNATURE_TYPE[args.key_type],
+                           lambda: "Invalid sign type")
+    key = get_key(args.keyf)
+
+    if not key.has_private():
+        logger.error('Provided key cannot be used for signing, ' +
+                     'please use offline-signing mode.')
+        sys.exit(1)
+
+    # Firmware image
+    input_file = open(args.inf, 'rb')
+    img = ELFFile(input_file)
+
+    # need to reopen the file to get the raw data
+    with open(args.inf, 'rb') as f:
+        bin_img = f.read()
+    img_size = len(bin_img)
+    logging.debug("image size %d" % img_size)
+    s_header.img_length = img_size
+    s_header.img_type = ENUM_BINARY_TYPE['ELF']
+
+    # Hash table chunk
+    h = SHA256.new()
+
+    # Compute the hash table
+    hash_table = SegmentHash(img)
+    hash = hash_table.get_table()
+
+    s_header.hash_offset = s_header.size
+    s_header.hash_length = hash_table.size
+    s_header.hash_type = ENUM_HASH_TYPE['SHA256']
+    # Get padding to align on 64 bytes
+    hash_align = s_header.hash_length % 8
+
+    # Key information chunk
+    if args.key_infof:
+        with open(args.key_infof, 'rb') as f:
+            key_info = f.read()
+        s_header.key_length = sys.getsizeof(key_info)
+        s_header.key_offset = s_header.hash_offset + s_header.hash_length + \
+            hash_align
+        # Get padding to align on 64 bytes
+        key_info_align = s_header.key_length % 8
+    else:
+        key_info_align = 0
+
+    # Signature chunk
+    s_header.sign_type = ENUM_SIGNATURE_TYPE[args.key_type]
+
+    sign_size = sig_size_type.get(ENUM_SIGNATURE_TYPE[args.key_type],
+                                  lambda: "Invalid sign type")(key)
+    s_header.sign_length = sign_size
+
+    if args.key_infof:
+        s_header.sign_offset = s_header.key_offset + s_header.key_length + \
+                           key_info_align
+    else:
+        s_header.sign_offset = s_header.hash_offset + s_header.hash_length + \
+                               hash_align
+
+    s_header.img_offset = s_header.sign_offset + sign_size
+
+    s_header.length = s_header.size + s_header.hash_length + hash_align + \
+        s_header.key_length + key_info_align + s_header.sign_length
+
+    header = s_header.get_packed()
+
+    # Generate signature
+    signer = Signature.get(ENUM_SIGNATURE_TYPE[args.key_type])(key)
+
+    signer.hash_compute(header)
+    signer.hash_compute(bytes(hash))
+    if args.key_infof:
+        signer.hash_compute(key_info)
+
+    signature = signer.sign()
+    if len(signature) != sign_size:
+        raise Exception(("Actual signature length is not equal to ",
+                         "the computed one: {} != {}".
+                         format(len(signature), sign_size)))
+
+    s_header.dump()
+
+    with open(args.outf, 'wb') as f:
+        f.write(header)
+        f.write(hash)
+        if hash_align:
+            f.write(bytearray(hash_align))
+        if args.key_infof:
+            if key_info_align:
+                f.write(key_info)
+                f.write(bytearray(key_info_align))
+        f.write(signature)
+        f.write(bytearray(sign_size - s_header.sign_length))
+        f.write(bin_img)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/ta/remoteproc/Makefile b/ta/remoteproc/Makefile
new file mode 100644
index 000000000..0d1a5010c
--- /dev/null
+++ b/ta/remoteproc/Makefile
@@ -0,0 +1,18 @@
+# The UUID for the Trusted Application
+BINARY=80a4c275-0a47-4905-8285-1486a9771a08
+
+ifdef TA_CROSS_COMPILE
+CROSS_COMPILE ?= $(TA_CROSS_COMPILE)
+endif
+export CROSS_COMPILE
+
+CFG_TEE_TA_LOG_LEVEL ?= 2
+CPPFLAGS += -DCFG_TEE_TA_LOG_LEVEL=$(CFG_TEE_TA_LOG_LEVEL)
+
+-include $(TA_DEV_KIT_DIR)/mk/ta_dev_kit.mk
+
+ifeq ($(wildcard $(TA_DEV_KIT_DIR)/mk/ta_dev_kit.mk), )
+clean:
+	@echo 'Note: $$(TA_DEV_KIT_DIR)/mk/ta_dev_kit.mk not found, cannot clean TA'
+	@echo 'Note: TA_DEV_KIT_DIR=$(TA_DEV_KIT_DIR)'
+endif
diff --git a/ta/remoteproc/elf_parser.c b/ta/remoteproc/elf_parser.c
new file mode 100644
index 000000000..da3e6f4ae
--- /dev/null
+++ b/ta/remoteproc/elf_parser.c
@@ -0,0 +1,186 @@
+ // SPDX-License-Identifier: BSD-2-Clause
+ /*
+  * Copyright (C) 2020, STMicroelectronics - All Rights Reserved
+  */
+
+#include <assert.h>
+#include <elf_parser.h>
+#include <string.h>
+#include <trace.h>
+#include <types_ext.h>
+#include <util.h>
+
+static bool va_in_fwm_image_range(void *va, uint8_t *fw, size_t fw_size)
+{
+	uint8_t *vaddr = va;
+
+	assert(fw + fw_size >= fw);
+	return vaddr >= fw && vaddr < fw + fw_size;
+}
+
+/*
+ * e32_parse_ehdr() - Check and parse the ELF header
+ *
+ * fw:   Firmware ELF file image
+ * size: Byte size of firmware ELF file image
+ */
+TEE_Result e32_parse_ehdr(uint8_t *fw, size_t size)
+{
+	Elf32_Ehdr *ehdr = (Elf32_Ehdr *)(void *)fw;
+
+	if (!fw || !ALIGNMENT_IS_OK(fw, uint32_t)) {
+		EMSG("Invalid fw address %p", fw);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	if (size < sizeof(Elf32_Ehdr) ||
+	    size < (ehdr->e_shoff + sizeof(Elf32_Shdr)))
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	if (!IS_ELF(*ehdr) ||
+	    ehdr->e_ident[EI_VERSION] != EV_CURRENT ||
+	    ehdr->e_ident[EI_CLASS] != ELFCLASS32 ||
+	    ehdr->e_phentsize != sizeof(Elf32_Phdr) ||
+	    ehdr->e_shentsize != sizeof(Elf32_Shdr)) {
+		EMSG("Invalid header");
+
+		return TEE_ERROR_BAD_FORMAT;
+	}
+
+	if (ehdr->e_phnum == 0) {
+		EMSG("No loadable segment found");
+		return TEE_ERROR_BAD_FORMAT;
+	}
+
+	return TEE_SUCCESS;
+}
+
+/*
+ * e32_parser_load_elf_image - simple ELF loader
+ * fw:		Firmware ELF file image
+ * fw_size:	Firmware ELF file image byte size
+ * load_seg:	Callback for loading a firmware image segment into device memory
+ * priv_data:	Private data passed to @load_seg callback.
+ */
+TEE_Result e32_parser_load_elf_image(uint8_t *fw, size_t fw_size,
+				     TEE_Result (*load_seg)(uint8_t *src,
+							    uint32_t size,
+							    uint32_t da,
+							    uint32_t mem_size,
+							    void *priv),
+				     void *priv_data)
+{
+	Elf32_Ehdr *ehdr = (Elf32_Ehdr *)(void *)fw;
+	Elf32_Phdr *phdr = (void *)((int8_t *)ehdr + ehdr->e_phoff);
+	TEE_Result res = TEE_SUCCESS;
+	unsigned int i = 0;
+
+	if (!load_seg)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (!ALIGNMENT_IS_OK(phdr, uint32_t) ||
+	    !va_in_fwm_image_range(phdr, fw, fw_size))
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	for (i = 0; i < ehdr->e_phnum; i++, phdr++) {
+		uint32_t dst = phdr->p_paddr;
+		uint8_t *src = NULL;
+
+		if (!va_in_fwm_image_range((void *)((vaddr_t)(phdr + 1) - 1),
+					   fw, fw_size))
+			return TEE_ERROR_CORRUPT_OBJECT;
+
+		if (phdr->p_type != PT_LOAD)
+			continue;
+
+		src = (uint8_t *)fw + phdr->p_offset;
+
+		if (!va_in_fwm_image_range(src, fw, fw_size) ||
+		    !va_in_fwm_image_range(src + phdr->p_filesz, fw, fw_size))
+			return TEE_ERROR_CORRUPT_OBJECT;
+
+		res = load_seg(src, phdr->p_filesz, dst, phdr->p_memsz,
+			       priv_data);
+		if (res)
+			return res;
+	}
+
+	return TEE_SUCCESS;
+}
+
+/* Helper to find resource table in an ELF image */
+int e32_parser_find_rsc_table(uint8_t *fw, size_t fw_size,
+			      Elf32_Addr *rsc_addr, Elf32_Word *rsc_size)
+{
+	Elf32_Shdr *shdr = NULL;
+	int i = 0;
+	char *name_table = NULL;
+	struct resource_table *table = NULL;
+	Elf32_Ehdr *ehdr = (Elf32_Ehdr *)(void *)fw;
+	uint8_t *elf_data = fw;
+
+	shdr = (void *)(fw + ehdr->e_shoff);
+	if (!ALIGNMENT_IS_OK(shdr, uint32_t) ||
+	    !va_in_fwm_image_range(shdr, fw, fw_size))
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	name_table = (char *)elf_data + shdr[ehdr->e_shstrndx].sh_offset;
+	if (!va_in_fwm_image_range(name_table, fw, fw_size))
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	for (i = 0; i < ehdr->e_shnum; i++, shdr++) {
+		size_t size = shdr->sh_size;
+		size_t offset = shdr->sh_offset;
+
+		if (!va_in_fwm_image_range(shdr, fw, fw_size))
+			return TEE_ERROR_CORRUPT_OBJECT;
+
+		if (strcmp(name_table + shdr->sh_name, ".resource_table"))
+			continue;
+
+		if (!shdr->sh_size) {
+			IMSG("Ignore empty resource table section");
+			return TEE_ERROR_NO_DATA;
+		}
+
+		if (offset + size > fw_size || offset + size < size) {
+			EMSG("Resource table truncated");
+			return TEE_ERROR_BAD_FORMAT;
+		}
+
+		if (sizeof(struct resource_table) > size) {
+			EMSG("No header found in resource table");
+			return TEE_ERROR_BAD_FORMAT;
+		}
+
+		table = (struct resource_table *)(void *)(elf_data + offset);
+		if (!ALIGNMENT_IS_OK(table, uint32_t))
+			return TEE_ERROR_CORRUPT_OBJECT;
+
+		if (table->ver != 1) {
+			EMSG("Unsupported fw version %"PRId32, table->ver);
+			return TEE_ERROR_BAD_FORMAT;
+		}
+
+		if (table->reserved[0] || table->reserved[1]) {
+			EMSG("Non zero reserved bytes");
+			return TEE_ERROR_BAD_FORMAT;
+		}
+
+		if (table->num * sizeof(*table->offset) +
+		    sizeof(struct resource_table) > size) {
+			EMSG("Resource table incomplete");
+			return TEE_ERROR_BAD_FORMAT;
+		}
+
+		DMSG("Resource table address %#"PRIx32", size %"PRIu32,
+		     shdr->sh_addr, shdr->sh_size);
+
+		*rsc_addr = shdr->sh_addr;
+		*rsc_size = shdr->sh_size;
+
+		return TEE_SUCCESS;
+	}
+
+	return TEE_ERROR_NO_DATA;
+}
diff --git a/ta/remoteproc/include/elf32.h b/ta/remoteproc/include/elf32.h
new file mode 100644
index 000000000..2806c615d
--- /dev/null
+++ b/ta/remoteproc/include/elf32.h
@@ -0,0 +1,243 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*-
+ * Copyright (c) 1996-1998 John D. Polstra.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#ifndef _SYS_ELF32_H_
+#define _SYS_ELF32_H_ 1
+
+#include <elf_common.h>
+#include <stdint.h>
+
+/*
+ * ELF definitions common to all 32-bit architectures.
+ */
+
+typedef uint32_t	Elf32_Addr;
+typedef uint16_t	Elf32_Half;
+typedef uint32_t	Elf32_Off;
+typedef int32_t		Elf32_Sword;
+typedef uint32_t	Elf32_Word;
+typedef uint64_t	Elf32_Lword;
+
+typedef Elf32_Word	Elf32_Hashelt;
+
+/* Non-standard class-dependent datatype used for abstraction. */
+typedef Elf32_Word	Elf32_Size;
+typedef Elf32_Sword	Elf32_Ssize;
+
+/*
+ * ELF header.
+ */
+
+typedef struct {
+	unsigned char	e_ident[EI_NIDENT];	/* File identification. */
+	Elf32_Half	e_type;		/* File type. */
+	Elf32_Half	e_machine;	/* Machine architecture. */
+	Elf32_Word	e_version;	/* ELF format version. */
+	Elf32_Addr	e_entry;	/* Entry point. */
+	Elf32_Off	e_phoff;	/* Program header file offset. */
+	Elf32_Off	e_shoff;	/* Section header file offset. */
+	Elf32_Word	e_flags;	/* Architecture-specific flags. */
+	Elf32_Half	e_ehsize;	/* Size of ELF header in bytes. */
+	Elf32_Half	e_phentsize;	/* Size of program header entry. */
+	Elf32_Half	e_phnum;	/* Number of program header entries. */
+	Elf32_Half	e_shentsize;	/* Size of section header entry. */
+	Elf32_Half	e_shnum;	/* Number of section header entries. */
+	Elf32_Half	e_shstrndx;	/* Section name strings section. */
+} Elf32_Ehdr;
+
+/*
+ * Section header.
+ */
+
+typedef struct {
+	Elf32_Word	sh_name;	/* Section name (index into the
+					   section header string table). */
+	Elf32_Word	sh_type;	/* Section type. */
+	Elf32_Word	sh_flags;	/* Section flags. */
+	Elf32_Addr	sh_addr;	/* Address in memory image. */
+	Elf32_Off	sh_offset;	/* Offset in file. */
+	Elf32_Word	sh_size;	/* Size in bytes. */
+	Elf32_Word	sh_link;	/* Index of a related section. */
+	Elf32_Word	sh_info;	/* Depends on section type. */
+	Elf32_Word	sh_addralign;	/* Alignment in bytes. */
+	Elf32_Word	sh_entsize;	/* Size of each entry in section. */
+} Elf32_Shdr;
+
+/*
+ * Program header.
+ */
+
+typedef struct {
+	Elf32_Word	p_type;		/* Entry type. */
+	Elf32_Off	p_offset;	/* File offset of contents. */
+	Elf32_Addr	p_vaddr;	/* Virtual address in memory image. */
+	Elf32_Addr	p_paddr;	/* Physical address (not used). */
+	Elf32_Word	p_filesz;	/* Size of contents in file. */
+	Elf32_Word	p_memsz;	/* Size of contents in memory. */
+	Elf32_Word	p_flags;	/* Access permission flags. */
+	Elf32_Word	p_align;	/* Alignment in memory and file. */
+} Elf32_Phdr;
+
+/*
+ * Dynamic structure.  The ".dynamic" section contains an array of them.
+ */
+
+typedef struct {
+	Elf32_Sword	d_tag;		/* Entry type. */
+	union {
+		Elf32_Word	d_val;	/* Integer value. */
+		Elf32_Addr	d_ptr;	/* Address value. */
+	} d_un;
+} Elf32_Dyn;
+
+/*
+ * Relocation entries.
+ */
+
+/* Relocations that don't need an addend field. */
+typedef struct {
+	Elf32_Addr	r_offset;	/* Location to be relocated. */
+	Elf32_Word	r_info;		/* Relocation type and symbol index. */
+} Elf32_Rel;
+
+/* Relocations that need an addend field. */
+typedef struct {
+	Elf32_Addr	r_offset;	/* Location to be relocated. */
+	Elf32_Word	r_info;		/* Relocation type and symbol index. */
+	Elf32_Sword	r_addend;	/* Addend. */
+} Elf32_Rela;
+
+/* Macros for accessing the fields of r_info. */
+#define ELF32_R_SYM(info)	((info) >> 8)
+#define ELF32_R_TYPE(info)	((unsigned char)(info))
+
+/* Macro for constructing r_info from field values. */
+#define ELF32_R_INFO(sym, type)	(((sym) << 8) + (unsigned char)(type))
+
+/*
+ *	Note entry header
+ */
+typedef Elf_Note Elf32_Nhdr;
+
+/*
+ *	Move entry
+ */
+typedef struct {
+	Elf32_Lword	m_value;	/* symbol value */
+	Elf32_Word 	m_info;		/* size + index */
+	Elf32_Word	m_poffset;	/* symbol offset */
+	Elf32_Half	m_repeat;	/* repeat count */
+	Elf32_Half	m_stride;	/* stride info */
+} Elf32_Move;
+
+/*
+ *	The macros compose and decompose values for Move.r_info
+ *
+ *	sym = ELF32_M_SYM(M.m_info)
+ *	size = ELF32_M_SIZE(M.m_info)
+ *	M.m_info = ELF32_M_INFO(sym, size)
+ */
+#define	ELF32_M_SYM(info)	((info)>>8)
+#define	ELF32_M_SIZE(info)	((unsigned char)(info))
+#define	ELF32_M_INFO(sym, size)	(((sym)<<8)+(unsigned char)(size))
+
+/*
+ *	Hardware/Software capabilities entry
+ */
+typedef struct {
+	Elf32_Word	c_tag;		/* how to interpret value */
+	union {
+		Elf32_Word	c_val;
+		Elf32_Addr	c_ptr;
+	} c_un;
+} Elf32_Cap;
+
+/*
+ * Symbol table entries.
+ */
+
+typedef struct {
+	Elf32_Word	st_name;	/* String table index of name. */
+	Elf32_Addr	st_value;	/* Symbol value. */
+	Elf32_Word	st_size;	/* Size of associated object. */
+	unsigned char	st_info;	/* Type and binding information. */
+	unsigned char	st_other;	/* Reserved (not used). */
+	Elf32_Half	st_shndx;	/* Section index of symbol. */
+} Elf32_Sym;
+
+/* Macros for accessing the fields of st_info. */
+#define ELF32_ST_BIND(info)		((info) >> 4)
+#define ELF32_ST_TYPE(info)		((info) & 0xf)
+
+/* Macro for constructing st_info from field values. */
+#define ELF32_ST_INFO(bind, type)	(((bind) << 4) + ((type) & 0xf))
+
+/* Macro for accessing the fields of st_other. */
+#define ELF32_ST_VISIBILITY(oth)	((oth) & 0x3)
+
+/* Structures used by Sun & GNU symbol versioning. */
+typedef struct {
+	Elf32_Half	vd_version;
+	Elf32_Half	vd_flags;
+	Elf32_Half	vd_ndx;
+	Elf32_Half	vd_cnt;
+	Elf32_Word	vd_hash;
+	Elf32_Word	vd_aux;
+	Elf32_Word	vd_next;
+} Elf32_Verdef;
+
+typedef struct {
+	Elf32_Word	vda_name;
+	Elf32_Word	vda_next;
+} Elf32_Verdaux;
+
+typedef struct {
+	Elf32_Half	vn_version;
+	Elf32_Half	vn_cnt;
+	Elf32_Word	vn_file;
+	Elf32_Word	vn_aux;
+	Elf32_Word	vn_next;
+} Elf32_Verneed;
+
+typedef struct {
+	Elf32_Word	vna_hash;
+	Elf32_Half	vna_flags;
+	Elf32_Half	vna_other;
+	Elf32_Word	vna_name;
+	Elf32_Word	vna_next;
+} Elf32_Vernaux;
+
+typedef Elf32_Half Elf32_Versym;
+
+typedef struct {
+	Elf32_Half	si_boundto;	/* direct bindings - symbol bound to */
+	Elf32_Half	si_flags;	/* per symbol flags */
+} Elf32_Syminfo;
+
+#endif /* !_SYS_ELF32_H_ */
diff --git a/ta/remoteproc/include/elf_common.h b/ta/remoteproc/include/elf_common.h
new file mode 100644
index 000000000..a1da0ef80
--- /dev/null
+++ b/ta/remoteproc/include/elf_common.h
@@ -0,0 +1,1014 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*-
+ * Copyright (c) 2000, 2001, 2008, 2011, David E. O'Brien
+ * Copyright (c) 1998 John D. Polstra.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#ifndef _SYS_ELF_COMMON_H_
+#define	_SYS_ELF_COMMON_H_ 1
+
+#include <stdint.h>
+
+/*
+ * ELF definitions that are independent of architecture or word size.
+ */
+
+#ifndef __ASSEMBLER__
+/*
+ * Note header.  The ".note" section contains an array of notes.  Each
+ * begins with this header, aligned to a word boundary.  Immediately
+ * following the note header is n_namesz bytes of name, padded to the
+ * next word boundary.  Then comes n_descsz bytes of descriptor, again
+ * padded to a word boundary.  The values of n_namesz and n_descsz do
+ * not include the padding.
+ */
+
+typedef struct {
+	uint32_t	n_namesz;	/* Length of name. */
+	uint32_t	n_descsz;	/* Length of descriptor. */
+	uint32_t	n_type;		/* Type of this note. */
+} Elf_Note;
+
+/*
+ * The header for GNU-style hash sections.
+ */
+
+typedef struct {
+	uint32_t	gh_nbuckets;	/* Number of hash buckets. */
+	uint32_t	gh_symndx;	/* First visible symbol in .dynsym. */
+	uint32_t	gh_maskwords;	/* #maskwords used in bloom filter. */
+	uint32_t	gh_shift2;	/* Bloom filter shift count. */
+} Elf_GNU_Hash_Header;
+#endif /*__ASSEMBLER__*/
+
+/* Indexes into the e_ident array.  Keep synced with
+   http://www.sco.com/developers/gabi/latest/ch4.eheader.html */
+#define	EI_MAG0		0	/* Magic number, byte 0. */
+#define	EI_MAG1		1	/* Magic number, byte 1. */
+#define	EI_MAG2		2	/* Magic number, byte 2. */
+#define	EI_MAG3		3	/* Magic number, byte 3. */
+#define	EI_CLASS	4	/* Class of machine. */
+#define	EI_DATA		5	/* Data format. */
+#define	EI_VERSION	6	/* ELF format version. */
+#define	EI_OSABI	7	/* Operating system / ABI identification */
+#define	EI_ABIVERSION	8	/* ABI version */
+#define	OLD_EI_BRAND	8	/* Start of architecture identification. */
+#define	EI_PAD		9	/* Start of padding (per SVR4 ABI). */
+#define	EI_NIDENT	16	/* Size of e_ident array. */
+
+/* Values for the magic number bytes. */
+#define	ELFMAG0		0x7f
+#define	ELFMAG1		'E'
+#define	ELFMAG2		'L'
+#define	ELFMAG3		'F'
+#define	ELFMAG		"\177ELF"	/* magic string */
+#define	SELFMAG		4		/* magic string size */
+
+/* Values for e_ident[EI_VERSION] and e_version. */
+#define	EV_NONE		0
+#define	EV_CURRENT	1
+
+/* Values for e_ident[EI_CLASS]. */
+#define	ELFCLASSNONE	0	/* Unknown class. */
+#define	ELFCLASS32	1	/* 32-bit architecture. */
+#define	ELFCLASS64	2	/* 64-bit architecture. */
+
+/* Values for e_ident[EI_DATA]. */
+#define	ELFDATANONE	0	/* Unknown data format. */
+#define	ELFDATA2LSB	1	/* 2's complement little-endian. */
+#define	ELFDATA2MSB	2	/* 2's complement big-endian. */
+
+/* Values for e_ident[EI_OSABI]. */
+#define	ELFOSABI_NONE		0	/* UNIX System V ABI */
+#define	ELFOSABI_HPUX		1	/* HP-UX operating system */
+#define	ELFOSABI_NETBSD		2	/* NetBSD */
+#define	ELFOSABI_LINUX		3	/* GNU/Linux */
+#define	ELFOSABI_HURD		4	/* GNU/Hurd */
+#define	ELFOSABI_86OPEN		5	/* 86Open common IA32 ABI */
+#define	ELFOSABI_SOLARIS	6	/* Solaris */
+#define	ELFOSABI_AIX		7	/* AIX */
+#define	ELFOSABI_IRIX		8	/* IRIX */
+#define	ELFOSABI_FREEBSD	9	/* FreeBSD */
+#define	ELFOSABI_TRU64		10	/* TRU64 UNIX */
+#define	ELFOSABI_MODESTO	11	/* Novell Modesto */
+#define	ELFOSABI_OPENBSD	12	/* OpenBSD */
+#define	ELFOSABI_OPENVMS	13	/* Open VMS */
+#define	ELFOSABI_NSK		14	/* HP Non-Stop Kernel */
+#define	ELFOSABI_AROS		15	/* Amiga Research OS */
+#define	ELFOSABI_ARM		97	/* ARM */
+#define	ELFOSABI_STANDALONE	255	/* Standalone (embedded) application */
+
+#define	ELFOSABI_SYSV		ELFOSABI_NONE	/* symbol used in old spec */
+#define	ELFOSABI_MONTEREY	ELFOSABI_AIX	/* Monterey */
+
+/* e_ident */
+#define	IS_ELF(ehdr)	((ehdr).e_ident[EI_MAG0] == ELFMAG0 && \
+			 (ehdr).e_ident[EI_MAG1] == ELFMAG1 && \
+			 (ehdr).e_ident[EI_MAG2] == ELFMAG2 && \
+			 (ehdr).e_ident[EI_MAG3] == ELFMAG3)
+
+/* Values for e_type. */
+#define	ET_NONE		0	/* Unknown type. */
+#define	ET_REL		1	/* Relocatable. */
+#define	ET_EXEC		2	/* Executable. */
+#define	ET_DYN		3	/* Shared object. */
+#define	ET_CORE		4	/* Core file. */
+#define	ET_LOOS		0xfe00	/* First operating system specific. */
+#define	ET_HIOS		0xfeff	/* Last operating system-specific. */
+#define	ET_LOPROC	0xff00	/* First processor-specific. */
+#define	ET_HIPROC	0xffff	/* Last processor-specific. */
+
+/* Values for e_machine. */
+#define	EM_NONE		0	/* Unknown machine. */
+#define	EM_M32		1	/* AT&T WE32100. */
+#define	EM_SPARC	2	/* Sun SPARC. */
+#define	EM_386		3	/* Intel i386. */
+#define	EM_68K		4	/* Motorola 68000. */
+#define	EM_88K		5	/* Motorola 88000. */
+#define	EM_860		7	/* Intel i860. */
+#define	EM_MIPS		8	/* MIPS R3000 Big-Endian only. */
+#define	EM_S370		9	/* IBM System/370. */
+#define	EM_MIPS_RS3_LE	10	/* MIPS R3000 Little-Endian. */
+#define	EM_PARISC	15	/* HP PA-RISC. */
+#define	EM_VPP500	17	/* Fujitsu VPP500. */
+#define	EM_SPARC32PLUS	18	/* SPARC v8plus. */
+#define	EM_960		19	/* Intel 80960. */
+#define	EM_PPC		20	/* PowerPC 32-bit. */
+#define	EM_PPC64	21	/* PowerPC 64-bit. */
+#define	EM_S390		22	/* IBM System/390. */
+#define	EM_V800		36	/* NEC V800. */
+#define	EM_FR20		37	/* Fujitsu FR20. */
+#define	EM_RH32		38	/* TRW RH-32. */
+#define	EM_RCE		39	/* Motorola RCE. */
+#define	EM_ARM		40	/* ARM. */
+#define	EM_SH		42	/* Hitachi SH. */
+#define	EM_SPARCV9	43	/* SPARC v9 64-bit. */
+#define	EM_TRICORE	44	/* Siemens TriCore embedded processor. */
+#define	EM_ARC		45	/* Argonaut RISC Core. */
+#define	EM_H8_300	46	/* Hitachi H8/300. */
+#define	EM_H8_300H	47	/* Hitachi H8/300H. */
+#define	EM_H8S		48	/* Hitachi H8S. */
+#define	EM_H8_500	49	/* Hitachi H8/500. */
+#define	EM_IA_64	50	/* Intel IA-64 Processor. */
+#define	EM_MIPS_X	51	/* Stanford MIPS-X. */
+#define	EM_COLDFIRE	52	/* Motorola ColdFire. */
+#define	EM_68HC12	53	/* Motorola M68HC12. */
+#define	EM_MMA		54	/* Fujitsu MMA. */
+#define	EM_PCP		55	/* Siemens PCP. */
+#define	EM_NCPU		56	/* Sony nCPU. */
+#define	EM_NDR1		57	/* Denso NDR1 microprocessor. */
+#define	EM_STARCORE	58	/* Motorola Star*Core processor. */
+#define	EM_ME16		59	/* Toyota ME16 processor. */
+#define	EM_ST100	60	/* STMicroelectronics ST100 processor. */
+#define	EM_TINYJ	61	/* Advanced Logic Corp. TinyJ processor. */
+#define	EM_X86_64	62	/* Advanced Micro Devices x86-64 */
+#define	EM_AMD64	EM_X86_64	/* Advanced Micro Devices x86-64 (compat) */
+#define	EM_PDSP		63	/* Sony DSP Processor. */
+#define	EM_FX66		66	/* Siemens FX66 microcontroller. */
+#define	EM_ST9PLUS	67	/* STMicroelectronics ST9+ 8/16
+				   microcontroller. */
+#define	EM_ST7		68	/* STmicroelectronics ST7 8-bit
+				   microcontroller. */
+#define	EM_68HC16	69	/* Motorola MC68HC16 microcontroller. */
+#define	EM_68HC11	70	/* Motorola MC68HC11 microcontroller. */
+#define	EM_68HC08	71	/* Motorola MC68HC08 microcontroller. */
+#define	EM_68HC05	72	/* Motorola MC68HC05 microcontroller. */
+#define	EM_SVX		73	/* Silicon Graphics SVx. */
+#define	EM_ST19		74	/* STMicroelectronics ST19 8-bit mc. */
+#define	EM_VAX		75	/* Digital VAX. */
+#define	EM_CRIS		76	/* Axis Communications 32-bit embedded
+				   processor. */
+#define	EM_JAVELIN	77	/* Infineon Technologies 32-bit embedded
+				   processor. */
+#define	EM_FIREPATH	78	/* Element 14 64-bit DSP Processor. */
+#define	EM_ZSP		79	/* LSI Logic 16-bit DSP Processor. */
+#define	EM_MMIX		80	/* Donald Knuth's educational 64-bit proc. */
+#define	EM_HUANY	81	/* Harvard University machine-independent
+				   object files. */
+#define	EM_PRISM	82	/* SiTera Prism. */
+#define	EM_AVR		83	/* Atmel AVR 8-bit microcontroller. */
+#define	EM_FR30		84	/* Fujitsu FR30. */
+#define	EM_D10V		85	/* Mitsubishi D10V. */
+#define	EM_D30V		86	/* Mitsubishi D30V. */
+#define	EM_V850		87	/* NEC v850. */
+#define	EM_M32R		88	/* Mitsubishi M32R. */
+#define	EM_MN10300	89	/* Matsushita MN10300. */
+#define	EM_MN10200	90	/* Matsushita MN10200. */
+#define	EM_PJ		91	/* picoJava. */
+#define	EM_OPENRISC	92	/* OpenRISC 32-bit embedded processor. */
+#define	EM_ARC_A5	93	/* ARC Cores Tangent-A5. */
+#define	EM_XTENSA	94	/* Tensilica Xtensa Architecture. */
+#define	EM_VIDEOCORE	95	/* Alphamosaic VideoCore processor. */
+#define	EM_TMM_GPP	96	/* Thompson Multimedia General Purpose
+				   Processor. */
+#define	EM_NS32K	97	/* National Semiconductor 32000 series. */
+#define	EM_TPC		98	/* Tenor Network TPC processor. */
+#define	EM_SNP1K	99	/* Trebia SNP 1000 processor. */
+#define	EM_ST200	100	/* STMicroelectronics ST200 microcontroller. */
+#define	EM_IP2K		101	/* Ubicom IP2xxx microcontroller family. */
+#define	EM_MAX		102	/* MAX Processor. */
+#define	EM_CR		103	/* National Semiconductor CompactRISC
+				   microprocessor. */
+#define	EM_F2MC16	104	/* Fujitsu F2MC16. */
+#define	EM_MSP430	105	/* Texas Instruments embedded microcontroller
+				   msp430. */
+#define	EM_BLACKFIN	106	/* Analog Devices Blackfin (DSP) processor. */
+#define	EM_SE_C33	107	/* S1C33 Family of Seiko Epson processors. */
+#define	EM_SEP		108	/* Sharp embedded microprocessor. */
+#define	EM_ARCA		109	/* Arca RISC Microprocessor. */
+#define	EM_UNICORE	110	/* Microprocessor series from PKU-Unity Ltd.
+				   and MPRC of Peking University */
+#define	EM_AARCH64	183	/* AArch64 (64-bit ARM) */
+
+/* Non-standard or deprecated. */
+#define	EM_486		6	/* Intel i486. */
+#define	EM_MIPS_RS4_BE	10	/* MIPS R4000 Big-Endian */
+#define	EM_ALPHA_STD	41	/* Digital Alpha (standard value). */
+#define	EM_ALPHA	0x9026	/* Alpha (written in the absence of an ABI) */
+
+/* e_flags for EM_ARM */
+#define EF_ARM_ABI_VERSION	0x05000000	/* ABI version 5 */
+#define EF_ARM_ABIMASK		0xFF000000
+#define EF_ARM_BE8		0x00800000
+#define EF_ARM_ABI_FLOAT_HARD	0x00000400	/* ABI version 5 and later */
+#define EF_ARM_ABI_FLOAT_SOFT	0x00000200	/* ABI version 5 and later */
+
+/* Special section indexes. */
+#define	SHN_UNDEF	     0		/* Undefined, missing, irrelevant. */
+#define	SHN_LORESERVE	0xff00		/* First of reserved range. */
+#define	SHN_LOPROC	0xff00		/* First processor-specific. */
+#define	SHN_HIPROC	0xff1f		/* Last processor-specific. */
+#define	SHN_LOOS	0xff20		/* First operating system-specific. */
+#define	SHN_HIOS	0xff3f		/* Last operating system-specific. */
+#define	SHN_ABS		0xfff1		/* Absolute values. */
+#define	SHN_COMMON	0xfff2		/* Common data. */
+#define	SHN_XINDEX	0xffff		/* Escape -- index stored elsewhere. */
+#define	SHN_HIRESERVE	0xffff		/* Last of reserved range. */
+
+/* sh_type */
+#define	SHT_NULL		0	/* inactive */
+#define	SHT_PROGBITS		1	/* program defined information */
+#define	SHT_SYMTAB		2	/* symbol table section */
+#define	SHT_STRTAB		3	/* string table section */
+#define	SHT_RELA		4	/* relocation section with addends */
+#define	SHT_HASH		5	/* symbol hash table section */
+#define	SHT_DYNAMIC		6	/* dynamic section */
+#define	SHT_NOTE		7	/* note section */
+#define	SHT_NOBITS		8	/* no space section */
+#define	SHT_REL			9	/* relocation section - no addends */
+#define	SHT_SHLIB		10	/* reserved - purpose unknown */
+#define	SHT_DYNSYM		11	/* dynamic symbol table section */
+#define	SHT_INIT_ARRAY		14	/* Initialization function pointers. */
+#define	SHT_FINI_ARRAY		15	/* Termination function pointers. */
+#define	SHT_PREINIT_ARRAY	16	/* Pre-initialization function ptrs. */
+#define	SHT_GROUP		17	/* Section group. */
+#define	SHT_SYMTAB_SHNDX	18	/* Section indexes (see SHN_XINDEX). */
+#define	SHT_LOOS		0x60000000	/* First of OS specific semantics */
+#define	SHT_LOSUNW		0x6ffffff4
+#define	SHT_SUNW_dof		0x6ffffff4
+#define	SHT_SUNW_cap		0x6ffffff5
+#define	SHT_SUNW_SIGNATURE	0x6ffffff6
+#define	SHT_GNU_HASH		0x6ffffff6
+#define	SHT_GNU_LIBLIST		0x6ffffff7
+#define	SHT_SUNW_ANNOTATE	0x6ffffff7
+#define	SHT_SUNW_DEBUGSTR	0x6ffffff8
+#define	SHT_SUNW_DEBUG		0x6ffffff9
+#define	SHT_SUNW_move		0x6ffffffa
+#define	SHT_SUNW_COMDAT		0x6ffffffb
+#define	SHT_SUNW_syminfo	0x6ffffffc
+#define	SHT_SUNW_verdef		0x6ffffffd
+#define	SHT_GNU_verdef		0x6ffffffd	/* Symbol versions provided */
+#define	SHT_SUNW_verneed	0x6ffffffe
+#define	SHT_GNU_verneed		0x6ffffffe	/* Symbol versions required */
+#define	SHT_SUNW_versym		0x6fffffff
+#define	SHT_GNU_versym		0x6fffffff	/* Symbol version table */
+#define	SHT_HISUNW		0x6fffffff
+#define	SHT_HIOS		0x6fffffff	/* Last of OS specific semantics */
+#define	SHT_LOPROC		0x70000000	/* reserved range for processor */
+#define	SHT_AMD64_UNWIND	0x70000001	/* unwind information */
+#define	SHT_ARM_EXIDX		0x70000001	/* Exception index table. */
+#define	SHT_ARM_PREEMPTMAP	0x70000002	/* BPABI DLL dynamic linking
+						   pre-emption map. */
+#define	SHT_ARM_ATTRIBUTES	0x70000003	/* Object file compatibility
+						   attributes. */
+#define	SHT_ARM_DEBUGOVERLAY	0x70000004	/* See DBGOVL for details. */
+#define	SHT_ARM_OVERLAYSECTION	0x70000005	/* See DBGOVL for details. */
+#define	SHT_MIPS_REGINFO	0x70000006
+#define	SHT_MIPS_OPTIONS	0x7000000d
+#define	SHT_MIPS_DWARF		0x7000001e	/* MIPS gcc uses MIPS_DWARF */
+#define	SHT_HIPROC		0x7fffffff	/* specific section header types */
+#define	SHT_LOUSER		0x80000000	/* reserved range for application */
+#define	SHT_HIUSER		0xffffffff	/* specific indexes */
+
+/* Flags for sh_flags. */
+#define	SHF_WRITE		0x1	/* Section contains writable data. */
+#define	SHF_ALLOC		0x2	/* Section occupies memory. */
+#define	SHF_EXECINSTR		0x4	/* Section contains instructions. */
+#define	SHF_MERGE		0x10	/* Section may be merged. */
+#define	SHF_STRINGS		0x20	/* Section contains strings. */
+#define	SHF_INFO_LINK		0x40	/* sh_info holds section index. */
+#define	SHF_LINK_ORDER		0x80	/* Special ordering requirements. */
+#define	SHF_OS_NONCONFORMING	0x100	/* OS-specific processing required. */
+#define	SHF_GROUP		0x200	/* Member of section group. */
+#define	SHF_TLS			0x400	/* Section contains TLS data. */
+#define	SHF_MASKOS	0x0ff00000	/* OS-specific semantics. */
+#define	SHF_MASKPROC	0xf0000000	/* Processor-specific semantics. */
+
+/* Values for p_type. */
+#define	PT_NULL		0	/* Unused entry. */
+#define	PT_LOAD		1	/* Loadable segment. */
+#define	PT_DYNAMIC	2	/* Dynamic linking information segment. */
+#define	PT_INTERP	3	/* Pathname of interpreter. */
+#define	PT_NOTE		4	/* Auxiliary information. */
+#define	PT_SHLIB	5	/* Reserved (not used). */
+#define	PT_PHDR		6	/* Location of program header itself. */
+#define	PT_TLS		7	/* Thread local storage segment */
+#define	PT_LOOS		0x60000000	/* First OS-specific. */
+#define	PT_SUNW_UNWIND	0x6464e550	/* amd64 UNWIND program header */
+#define	PT_GNU_EH_FRAME	0x6474e550
+#define	PT_GNU_STACK	0x6474e551
+#define	PT_GNU_RELRO	0x6474e552
+#define	PT_LOSUNW	0x6ffffffa
+#define	PT_SUNWBSS	0x6ffffffa	/* Sun Specific segment */
+#define	PT_SUNWSTACK	0x6ffffffb	/* describes the stack segment */
+#define	PT_SUNWDTRACE	0x6ffffffc	/* private */
+#define	PT_SUNWCAP	0x6ffffffd	/* hard/soft capabilities segment */
+#define	PT_HISUNW	0x6fffffff
+#define	PT_HIOS		0x6fffffff	/* Last OS-specific. */
+#define	PT_LOPROC	0x70000000	/* First processor-specific type. */
+#define PT_ARM_EXIDX	0x70000001	/* .ARM.exidx segment */
+#define	PT_HIPROC	0x7fffffff	/* Last processor-specific type. */
+
+/* Values for p_flags. */
+#define	PF_X		0x1		/* Executable. */
+#define	PF_W		0x2		/* Writable. */
+#define	PF_R		0x4		/* Readable. */
+#define	PF_MASKOS	0x0ff00000	/* Operating system-specific. */
+#define	PF_MASKPROC	0xf0000000	/* Processor-specific. */
+
+/* Extended program header index. */
+#define	PN_XNUM		0xffff
+
+/* Values for d_tag. */
+#define	DT_NULL		0	/* Terminating entry. */
+#define	DT_NEEDED	1	/* String table offset of a needed shared
+				   library. */
+#define	DT_PLTRELSZ	2	/* Total size in bytes of PLT relocations. */
+#define	DT_PLTGOT	3	/* Processor-dependent address. */
+#define	DT_HASH		4	/* Address of symbol hash table. */
+#define	DT_STRTAB	5	/* Address of string table. */
+#define	DT_SYMTAB	6	/* Address of symbol table. */
+#define	DT_RELA		7	/* Address of ElfNN_Rela relocations. */
+#define	DT_RELASZ	8	/* Total size of ElfNN_Rela relocations. */
+#define	DT_RELAENT	9	/* Size of each ElfNN_Rela relocation entry. */
+#define	DT_STRSZ	10	/* Size of string table. */
+#define	DT_SYMENT	11	/* Size of each symbol table entry. */
+#define	DT_INIT		12	/* Address of initialization function. */
+#define	DT_FINI		13	/* Address of finalization function. */
+#define	DT_SONAME	14	/* String table offset of shared object
+				   name. */
+#define	DT_RPATH	15	/* String table offset of library path. [sup] */
+#define	DT_SYMBOLIC	16	/* Indicates "symbolic" linking. [sup] */
+#define	DT_REL		17	/* Address of ElfNN_Rel relocations. */
+#define	DT_RELSZ	18	/* Total size of ElfNN_Rel relocations. */
+#define	DT_RELENT	19	/* Size of each ElfNN_Rel relocation. */
+#define	DT_PLTREL	20	/* Type of relocation used for PLT. */
+#define	DT_DEBUG	21	/* Reserved (not used). */
+#define	DT_TEXTREL	22	/* Indicates there may be relocations in
+				   non-writable segments. [sup] */
+#define	DT_JMPREL	23	/* Address of PLT relocations. */
+#define	DT_BIND_NOW	24	/* [sup] */
+#define	DT_INIT_ARRAY	25	/* Address of the array of pointers to
+				   initialization functions */
+#define	DT_FINI_ARRAY	26	/* Address of the array of pointers to
+				   termination functions */
+#define	DT_INIT_ARRAYSZ	27	/* Size in bytes of the array of
+				   initialization functions. */
+#define	DT_FINI_ARRAYSZ	28	/* Size in bytes of the array of
+				   termination functions. */
+#define	DT_RUNPATH	29	/* String table offset of a null-terminated
+				   library search path string. */
+#define	DT_FLAGS	30	/* Object specific flag values. */
+#define	DT_ENCODING	32	/* Values greater than or equal to DT_ENCODING
+				   and less than DT_LOOS follow the rules for
+				   the interpretation of the d_un union
+				   as follows: even == 'd_ptr', odd == 'd_val'
+				   or none */
+#define	DT_PREINIT_ARRAY 32	/* Address of the array of pointers to
+				   pre-initialization functions. */
+#define	DT_PREINIT_ARRAYSZ 33	/* Size in bytes of the array of
+				   pre-initialization functions. */
+#define	DT_MAXPOSTAGS	34	/* number of positive tags */
+#define	DT_LOOS		0x6000000d	/* First OS-specific */
+#define	DT_SUNW_AUXILIARY	0x6000000d	/* symbol auxiliary name */
+#define	DT_SUNW_RTLDINF		0x6000000e	/* ld.so.1 info (private) */
+#define	DT_SUNW_FILTER		0x6000000f	/* symbol filter name */
+#define	DT_SUNW_CAP		0x60000010	/* hardware/software */
+#define	DT_HIOS		0x6ffff000	/* Last OS-specific */
+
+/*
+ * DT_* entries which fall between DT_VALRNGHI & DT_VALRNGLO use the
+ * Dyn.d_un.d_val field of the Elf*_Dyn structure.
+ */
+#define	DT_VALRNGLO	0x6ffffd00
+#define	DT_CHECKSUM	0x6ffffdf8	/* elf checksum */
+#define	DT_PLTPADSZ	0x6ffffdf9	/* pltpadding size */
+#define	DT_MOVEENT	0x6ffffdfa	/* move table entry size */
+#define	DT_MOVESZ	0x6ffffdfb	/* move table size */
+#define	DT_FEATURE_1	0x6ffffdfc	/* feature holder */
+#define	DT_POSFLAG_1	0x6ffffdfd	/* flags for DT_* entries, effecting */
+					/*	the following DT_* entry. */
+					/*	See DF_P1_* definitions */
+#define	DT_SYMINSZ	0x6ffffdfe	/* syminfo table size (in bytes) */
+#define	DT_SYMINENT	0x6ffffdff	/* syminfo entry size (in bytes) */
+#define	DT_VALRNGHI	0x6ffffdff
+
+/*
+ * DT_* entries which fall between DT_ADDRRNGHI & DT_ADDRRNGLO use the
+ * Dyn.d_un.d_ptr field of the Elf*_Dyn structure.
+ *
+ * If any adjustment is made to the ELF object after it has been
+ * built, these entries will need to be adjusted.
+ */
+#define	DT_ADDRRNGLO	0x6ffffe00
+#define	DT_GNU_HASH	0x6ffffef5	/* GNU-style hash table */
+#define	DT_CONFIG	0x6ffffefa	/* configuration information */
+#define	DT_DEPAUDIT	0x6ffffefb	/* dependency auditing */
+#define	DT_AUDIT	0x6ffffefc	/* object auditing */
+#define	DT_PLTPAD	0x6ffffefd	/* pltpadding (sparcv9) */
+#define	DT_MOVETAB	0x6ffffefe	/* move table */
+#define	DT_SYMINFO	0x6ffffeff	/* syminfo table */
+#define	DT_ADDRRNGHI	0x6ffffeff
+
+#define	DT_VERSYM	0x6ffffff0	/* Address of versym section. */
+#define	DT_RELACOUNT	0x6ffffff9	/* number of RELATIVE relocations */
+#define	DT_RELCOUNT	0x6ffffffa	/* number of RELATIVE relocations */
+#define	DT_FLAGS_1	0x6ffffffb	/* state flags - see DF_1_* defs */
+#define	DT_VERDEF	0x6ffffffc	/* Address of verdef section. */
+#define	DT_VERDEFNUM	0x6ffffffd	/* Number of elems in verdef section */
+#define	DT_VERNEED	0x6ffffffe	/* Address of verneed section. */
+#define	DT_VERNEEDNUM	0x6fffffff	/* Number of elems in verneed section */
+
+#define	DT_LOPROC	0x70000000	/* First processor-specific type. */
+#define	DT_DEPRECATED_SPARC_REGISTER	0x7000001
+#define	DT_AUXILIARY	0x7ffffffd	/* shared library auxiliary name */
+#define	DT_USED		0x7ffffffe	/* ignored - same as needed */
+#define	DT_FILTER	0x7fffffff	/* shared library filter name */
+#define	DT_HIPROC	0x7fffffff	/* Last processor-specific type. */
+
+/* Values for DT_FLAGS */
+#define	DF_ORIGIN	0x0001	/* Indicates that the object being loaded may
+				   make reference to the $ORIGIN substitution
+				   string */
+#define	DF_SYMBOLIC	0x0002	/* Indicates "symbolic" linking. */
+#define	DF_TEXTREL	0x0004	/* Indicates there may be relocations in
+				   non-writable segments. */
+#define	DF_BIND_NOW	0x0008	/* Indicates that the dynamic linker should
+				   process all relocations for the object
+				   containing this entry before transferring
+				   control to the program. */
+#define	DF_STATIC_TLS	0x0010	/* Indicates that the shared object or
+				   executable contains code using a static
+				   thread-local storage scheme. */
+
+/* Values for DT_FLAGS_1 */
+#define	DF_1_BIND_NOW	0x00000001	/* Same as DF_BIND_NOW */
+#define	DF_1_GLOBAL	0x00000002	/* Set the RTLD_GLOBAL for object */
+#define	DF_1_NODELETE	0x00000008	/* Set the RTLD_NODELETE for object */
+#define	DF_1_LOADFLTR	0x00000010	/* Immediate loading of filtees */
+#define	DF_1_NOOPEN     0x00000040	/* Do not allow loading on dlopen() */
+#define	DF_1_ORIGIN	0x00000080	/* Process $ORIGIN */
+#define	DF_1_INTERPOSE	0x00000400	/* Interpose all objects but main */
+#define	DF_1_NODEFLIB	0x00000800	/* Do not search default paths */
+
+/* Values for n_type.  Used in core files. */
+#define	NT_PRSTATUS	1	/* Process status. */
+#define	NT_FPREGSET	2	/* Floating point registers. */
+#define	NT_PRPSINFO	3	/* Process state info. */
+#define	NT_THRMISC	7	/* Thread miscellaneous info. */
+#define	NT_PROCSTAT_PROC	8	/* Procstat proc data. */
+#define	NT_PROCSTAT_FILES	9	/* Procstat files data. */
+#define	NT_PROCSTAT_VMMAP	10	/* Procstat vmmap data. */
+#define	NT_PROCSTAT_GROUPS	11	/* Procstat groups data. */
+#define	NT_PROCSTAT_UMASK	12	/* Procstat umask data. */
+#define	NT_PROCSTAT_RLIMIT	13	/* Procstat rlimit data. */
+#define	NT_PROCSTAT_OSREL	14	/* Procstat osreldate data. */
+#define	NT_PROCSTAT_PSSTRINGS	15	/* Procstat ps_strings data. */
+#define	NT_PROCSTAT_AUXV	16	/* Procstat auxv data. */
+
+/* Symbol Binding - ELFNN_ST_BIND - st_info */
+#define	STB_LOCAL	0	/* Local symbol */
+#define	STB_GLOBAL	1	/* Global symbol */
+#define	STB_WEAK	2	/* like global - lower precedence */
+#define	STB_LOOS	10	/* Reserved range for operating system */
+#define	STB_HIOS	12	/*   specific semantics. */
+#define	STB_LOPROC	13	/* reserved range for processor */
+#define	STB_HIPROC	15	/*   specific semantics. */
+
+/* Symbol type - ELFNN_ST_TYPE - st_info */
+#define	STT_NOTYPE	0	/* Unspecified type. */
+#define	STT_OBJECT	1	/* Data object. */
+#define	STT_FUNC	2	/* Function. */
+#define	STT_SECTION	3	/* Section. */
+#define	STT_FILE	4	/* Source file. */
+#define	STT_COMMON	5	/* Uninitialized common block. */
+#define	STT_TLS		6	/* TLS object. */
+#define	STT_NUM		7
+#define	STT_LOOS	10	/* Reserved range for operating system */
+#define	STT_GNU_IFUNC	10
+#define	STT_HIOS	12	/*   specific semantics. */
+#define	STT_LOPROC	13	/* reserved range for processor */
+#define	STT_HIPROC	15	/*   specific semantics. */
+
+/* Symbol visibility - ELFNN_ST_VISIBILITY - st_other */
+#define	STV_DEFAULT	0x0	/* Default visibility (see binding). */
+#define	STV_INTERNAL	0x1	/* Special meaning in relocatable objects. */
+#define	STV_HIDDEN	0x2	/* Not visible. */
+#define	STV_PROTECTED	0x3	/* Visible but not preemptible. */
+#define	STV_EXPORTED	0x4
+#define	STV_SINGLETON	0x5
+#define	STV_ELIMINATE	0x6
+
+/* Special symbol table indexes. */
+#define	STN_UNDEF	0	/* Undefined symbol index. */
+
+/* Symbol versioning flags. */
+#define	VER_DEF_CURRENT	1
+#define	VER_DEF_IDX(x)	VER_NDX(x)
+
+#define	VER_FLG_BASE	0x01
+#define	VER_FLG_WEAK	0x02
+
+#define	VER_NEED_CURRENT	1
+#define	VER_NEED_WEAK	(1u << 15)
+#define	VER_NEED_HIDDEN	VER_NDX_HIDDEN
+#define	VER_NEED_IDX(x)	VER_NDX(x)
+
+#define	VER_NDX_LOCAL	0
+#define	VER_NDX_GLOBAL	1
+#define	VER_NDX_GIVEN	2
+
+#define	VER_NDX_HIDDEN	(1u << 15)
+#define	VER_NDX(x)	((x) & ~(1u << 15))
+
+#define	CA_SUNW_NULL	0
+#define	CA_SUNW_HW_1	1		/* first hardware capabilities entry */
+#define	CA_SUNW_SF_1	2		/* first software capabilities entry */
+
+/*
+ * Syminfo flag values
+ */
+#define	SYMINFO_FLG_DIRECT	0x0001	/* symbol ref has direct association */
+					/*	to object containing defn. */
+#define	SYMINFO_FLG_PASSTHRU	0x0002	/* ignored - see SYMINFO_FLG_FILTER */
+#define	SYMINFO_FLG_COPY	0x0004	/* symbol is a copy-reloc */
+#define	SYMINFO_FLG_LAZYLOAD	0x0008	/* object containing defn should be */
+					/*	lazily-loaded */
+#define	SYMINFO_FLG_DIRECTBIND	0x0010	/* ref should be bound directly to */
+					/*	object containing defn. */
+#define	SYMINFO_FLG_NOEXTDIRECT	0x0020	/* don't let an external reference */
+					/*	directly bind to this symbol */
+#define	SYMINFO_FLG_FILTER	0x0002	/* symbol ref is associated to a */
+#define	SYMINFO_FLG_AUXILIARY	0x0040	/* 	standard or auxiliary filter */
+
+/*
+ * Syminfo.si_boundto values.
+ */
+#define	SYMINFO_BT_SELF		0xffff	/* symbol bound to self */
+#define	SYMINFO_BT_PARENT	0xfffe	/* symbol bound to parent */
+#define	SYMINFO_BT_NONE		0xfffd	/* no special symbol binding */
+#define	SYMINFO_BT_EXTERN	0xfffc	/* symbol defined as external */
+#define	SYMINFO_BT_LOWRESERVE	0xff00	/* beginning of reserved entries */
+
+/*
+ * Syminfo version values.
+ */
+#define	SYMINFO_NONE		0	/* Syminfo version */
+#define	SYMINFO_CURRENT		1
+#define	SYMINFO_NUM		2
+
+/*
+ * Relocation types.
+ *
+ * All machine architectures are defined here to allow tools on one to
+ * handle others.
+ */
+
+#define	R_386_NONE		0	/* No relocation. */
+#define	R_386_32		1	/* Add symbol value. */
+#define	R_386_PC32		2	/* Add PC-relative symbol value. */
+#define	R_386_GOT32		3	/* Add PC-relative GOT offset. */
+#define	R_386_PLT32		4	/* Add PC-relative PLT offset. */
+#define	R_386_COPY		5	/* Copy data from shared object. */
+#define	R_386_GLOB_DAT		6	/* Set GOT entry to data address. */
+#define	R_386_JMP_SLOT		7	/* Set GOT entry to code address. */
+#define	R_386_RELATIVE		8	/* Add load address of shared object. */
+#define	R_386_GOTOFF		9	/* Add GOT-relative symbol address. */
+#define	R_386_GOTPC		10	/* Add PC-relative GOT table address. */
+#define	R_386_TLS_TPOFF		14	/* Negative offset in static TLS block */
+#define	R_386_TLS_IE		15	/* Absolute address of GOT for -ve static TLS */
+#define	R_386_TLS_GOTIE		16	/* GOT entry for negative static TLS block */
+#define	R_386_TLS_LE		17	/* Negative offset relative to static TLS */
+#define	R_386_TLS_GD		18	/* 32 bit offset to GOT (index,off) pair */
+#define	R_386_TLS_LDM		19	/* 32 bit offset to GOT (index,zero) pair */
+#define	R_386_TLS_GD_32		24	/* 32 bit offset to GOT (index,off) pair */
+#define	R_386_TLS_GD_PUSH	25	/* pushl instruction for Sun ABI GD sequence */
+#define	R_386_TLS_GD_CALL	26	/* call instruction for Sun ABI GD sequence */
+#define	R_386_TLS_GD_POP	27	/* popl instruction for Sun ABI GD sequence */
+#define	R_386_TLS_LDM_32	28	/* 32 bit offset to GOT (index,zero) pair */
+#define	R_386_TLS_LDM_PUSH	29	/* pushl instruction for Sun ABI LD sequence */
+#define	R_386_TLS_LDM_CALL	30	/* call instruction for Sun ABI LD sequence */
+#define	R_386_TLS_LDM_POP	31	/* popl instruction for Sun ABI LD sequence */
+#define	R_386_TLS_LDO_32	32	/* 32 bit offset from start of TLS block */
+#define	R_386_TLS_IE_32		33	/* 32 bit offset to GOT static TLS offset entry */
+#define	R_386_TLS_LE_32		34	/* 32 bit offset within static TLS block */
+#define	R_386_TLS_DTPMOD32	35	/* GOT entry containing TLS index */
+#define	R_386_TLS_DTPOFF32	36	/* GOT entry containing TLS offset */
+#define	R_386_TLS_TPOFF32	37	/* GOT entry of -ve static TLS offset */
+#define	R_386_IRELATIVE		42	/* PLT entry resolved indirectly at runtime */
+
+#define	R_AARCH64_ABS64		257
+#define	R_AARCH64_GLOB_DAT	1025	/* Set GOT entry to data address. */
+#define	R_AARCH64_JUMP_SLOT	1026	/* Set GOT entry to code address. */
+#define	R_AARCH64_RELATIVE 	1027
+
+#define	R_ARM_NONE		0	/* No relocation. */
+#define	R_ARM_PC24		1
+#define	R_ARM_ABS32		2
+#define	R_ARM_REL32		3
+#define	R_ARM_PC13		4
+#define	R_ARM_ABS16		5
+#define	R_ARM_ABS12		6
+#define	R_ARM_THM_ABS5		7
+#define	R_ARM_ABS8		8
+#define	R_ARM_SBREL32		9
+#define	R_ARM_THM_PC22		10
+#define	R_ARM_THM_PC8		11
+#define	R_ARM_AMP_VCALL9	12
+#define	R_ARM_SWI24		13
+#define	R_ARM_THM_SWI8		14
+#define	R_ARM_XPC25		15
+#define	R_ARM_THM_XPC22		16
+/* TLS relocations */
+#define	R_ARM_TLS_DTPMOD32	17	/* ID of module containing symbol */
+#define	R_ARM_TLS_DTPOFF32	18	/* Offset in TLS block */
+#define	R_ARM_TLS_TPOFF32	19	/* Offset in static TLS block */
+#define	R_ARM_COPY		20	/* Copy data from shared object. */
+#define	R_ARM_GLOB_DAT		21	/* Set GOT entry to data address. */
+#define	R_ARM_JUMP_SLOT		22	/* Set GOT entry to code address. */
+#define	R_ARM_RELATIVE		23	/* Add load address of shared object. */
+#define	R_ARM_GOTOFF		24	/* Add GOT-relative symbol address. */
+#define	R_ARM_GOTPC		25	/* Add PC-relative GOT table address. */
+#define	R_ARM_GOT32		26	/* Add PC-relative GOT offset. */
+#define	R_ARM_PLT32		27	/* Add PC-relative PLT offset. */
+#define	R_ARM_GNU_VTENTRY	100
+#define	R_ARM_GNU_VTINHERIT	101
+#define	R_ARM_RSBREL32		250
+#define	R_ARM_THM_RPC22		251
+#define	R_ARM_RREL32		252
+#define	R_ARM_RABS32		253
+#define	R_ARM_RPC24		254
+#define	R_ARM_RBASE		255
+
+/*	Name			Value	   Field	Calculation */
+#define	R_IA_64_NONE		0	/* None */
+#define	R_IA_64_IMM14		0x21	/* immediate14	S + A */
+#define	R_IA_64_IMM22		0x22	/* immediate22	S + A */
+#define	R_IA_64_IMM64		0x23	/* immediate64	S + A */
+#define	R_IA_64_DIR32MSB	0x24	/* word32 MSB	S + A */
+#define	R_IA_64_DIR32LSB	0x25	/* word32 LSB	S + A */
+#define	R_IA_64_DIR64MSB	0x26	/* word64 MSB	S + A */
+#define	R_IA_64_DIR64LSB	0x27	/* word64 LSB	S + A */
+#define	R_IA_64_GPREL22		0x2a	/* immediate22	@gprel(S + A) */
+#define	R_IA_64_GPREL64I	0x2b	/* immediate64	@gprel(S + A) */
+#define	R_IA_64_GPREL32MSB	0x2c	/* word32 MSB	@gprel(S + A) */
+#define	R_IA_64_GPREL32LSB	0x2d	/* word32 LSB	@gprel(S + A) */
+#define	R_IA_64_GPREL64MSB	0x2e	/* word64 MSB	@gprel(S + A) */
+#define	R_IA_64_GPREL64LSB	0x2f	/* word64 LSB	@gprel(S + A) */
+#define	R_IA_64_LTOFF22		0x32	/* immediate22	@ltoff(S + A) */
+#define	R_IA_64_LTOFF64I	0x33	/* immediate64	@ltoff(S + A) */
+#define	R_IA_64_PLTOFF22	0x3a	/* immediate22	@pltoff(S + A) */
+#define	R_IA_64_PLTOFF64I	0x3b	/* immediate64	@pltoff(S + A) */
+#define	R_IA_64_PLTOFF64MSB	0x3e	/* word64 MSB	@pltoff(S + A) */
+#define	R_IA_64_PLTOFF64LSB	0x3f	/* word64 LSB	@pltoff(S + A) */
+#define	R_IA_64_FPTR64I		0x43	/* immediate64	@fptr(S + A) */
+#define	R_IA_64_FPTR32MSB	0x44	/* word32 MSB	@fptr(S + A) */
+#define	R_IA_64_FPTR32LSB	0x45	/* word32 LSB	@fptr(S + A) */
+#define	R_IA_64_FPTR64MSB	0x46	/* word64 MSB	@fptr(S + A) */
+#define	R_IA_64_FPTR64LSB	0x47	/* word64 LSB	@fptr(S + A) */
+#define	R_IA_64_PCREL60B	0x48	/* immediate60 form1 S + A - P */
+#define	R_IA_64_PCREL21B	0x49	/* immediate21 form1 S + A - P */
+#define	R_IA_64_PCREL21M	0x4a	/* immediate21 form2 S + A - P */
+#define	R_IA_64_PCREL21F	0x4b	/* immediate21 form3 S + A - P */
+#define	R_IA_64_PCREL32MSB	0x4c	/* word32 MSB	S + A - P */
+#define	R_IA_64_PCREL32LSB	0x4d	/* word32 LSB	S + A - P */
+#define	R_IA_64_PCREL64MSB	0x4e	/* word64 MSB	S + A - P */
+#define	R_IA_64_PCREL64LSB	0x4f	/* word64 LSB	S + A - P */
+#define	R_IA_64_LTOFF_FPTR22	0x52	/* immediate22	@ltoff(@fptr(S + A)) */
+#define	R_IA_64_LTOFF_FPTR64I	0x53	/* immediate64	@ltoff(@fptr(S + A)) */
+#define	R_IA_64_LTOFF_FPTR32MSB	0x54	/* word32 MSB	@ltoff(@fptr(S + A)) */
+#define	R_IA_64_LTOFF_FPTR32LSB	0x55	/* word32 LSB	@ltoff(@fptr(S + A)) */
+#define	R_IA_64_LTOFF_FPTR64MSB	0x56	/* word64 MSB	@ltoff(@fptr(S + A)) */
+#define	R_IA_64_LTOFF_FPTR64LSB	0x57	/* word64 LSB	@ltoff(@fptr(S + A)) */
+#define	R_IA_64_SEGREL32MSB	0x5c	/* word32 MSB	@segrel(S + A) */
+#define	R_IA_64_SEGREL32LSB	0x5d	/* word32 LSB	@segrel(S + A) */
+#define	R_IA_64_SEGREL64MSB	0x5e	/* word64 MSB	@segrel(S + A) */
+#define	R_IA_64_SEGREL64LSB	0x5f	/* word64 LSB	@segrel(S + A) */
+#define	R_IA_64_SECREL32MSB	0x64	/* word32 MSB	@secrel(S + A) */
+#define	R_IA_64_SECREL32LSB	0x65	/* word32 LSB	@secrel(S + A) */
+#define	R_IA_64_SECREL64MSB	0x66	/* word64 MSB	@secrel(S + A) */
+#define	R_IA_64_SECREL64LSB	0x67	/* word64 LSB	@secrel(S + A) */
+#define	R_IA_64_REL32MSB	0x6c	/* word32 MSB	BD + A */
+#define	R_IA_64_REL32LSB	0x6d	/* word32 LSB	BD + A */
+#define	R_IA_64_REL64MSB	0x6e	/* word64 MSB	BD + A */
+#define	R_IA_64_REL64LSB	0x6f	/* word64 LSB	BD + A */
+#define	R_IA_64_LTV32MSB	0x74	/* word32 MSB	S + A */
+#define	R_IA_64_LTV32LSB	0x75	/* word32 LSB	S + A */
+#define	R_IA_64_LTV64MSB	0x76	/* word64 MSB	S + A */
+#define	R_IA_64_LTV64LSB	0x77	/* word64 LSB	S + A */
+#define	R_IA_64_PCREL21BI	0x79	/* immediate21 form1 S + A - P */
+#define	R_IA_64_PCREL22		0x7a	/* immediate22	S + A - P */
+#define	R_IA_64_PCREL64I	0x7b	/* immediate64	S + A - P */
+#define	R_IA_64_IPLTMSB		0x80	/* function descriptor MSB special */
+#define	R_IA_64_IPLTLSB		0x81	/* function descriptor LSB speciaal */
+#define	R_IA_64_SUB		0x85	/* immediate64	A - S */
+#define	R_IA_64_LTOFF22X	0x86	/* immediate22	special */
+#define	R_IA_64_LDXMOV		0x87	/* immediate22	special */
+#define	R_IA_64_TPREL14		0x91	/* imm14	@tprel(S + A) */
+#define	R_IA_64_TPREL22		0x92	/* imm22	@tprel(S + A) */
+#define	R_IA_64_TPREL64I	0x93	/* imm64	@tprel(S + A) */
+#define	R_IA_64_TPREL64MSB	0x96	/* word64 MSB	@tprel(S + A) */
+#define	R_IA_64_TPREL64LSB	0x97	/* word64 LSB	@tprel(S + A) */
+#define	R_IA_64_LTOFF_TPREL22	0x9a	/* imm22	@ltoff(@tprel(S+A)) */
+#define	R_IA_64_DTPMOD64MSB	0xa6	/* word64 MSB	@dtpmod(S + A) */
+#define	R_IA_64_DTPMOD64LSB	0xa7	/* word64 LSB	@dtpmod(S + A) */
+#define	R_IA_64_LTOFF_DTPMOD22	0xaa	/* imm22	@ltoff(@dtpmod(S+A)) */
+#define	R_IA_64_DTPREL14	0xb1	/* imm14	@dtprel(S + A) */
+#define	R_IA_64_DTPREL22	0xb2	/* imm22	@dtprel(S + A) */
+#define	R_IA_64_DTPREL64I	0xb3	/* imm64	@dtprel(S + A) */
+#define	R_IA_64_DTPREL32MSB	0xb4	/* word32 MSB	@dtprel(S + A) */
+#define	R_IA_64_DTPREL32LSB	0xb5	/* word32 LSB	@dtprel(S + A) */
+#define	R_IA_64_DTPREL64MSB	0xb6	/* word64 MSB	@dtprel(S + A) */
+#define	R_IA_64_DTPREL64LSB	0xb7	/* word64 LSB	@dtprel(S + A) */
+#define	R_IA_64_LTOFF_DTPREL22	0xba	/* imm22	@ltoff(@dtprel(S+A)) */
+
+#define	R_MIPS_NONE	0	/* No reloc */
+#define	R_MIPS_16	1	/* Direct 16 bit */
+#define	R_MIPS_32	2	/* Direct 32 bit */
+#define	R_MIPS_REL32	3	/* PC relative 32 bit */
+#define	R_MIPS_26	4	/* Direct 26 bit shifted */
+#define	R_MIPS_HI16	5	/* High 16 bit */
+#define	R_MIPS_LO16	6	/* Low 16 bit */
+#define	R_MIPS_GPREL16	7	/* GP relative 16 bit */
+#define	R_MIPS_LITERAL	8	/* 16 bit literal entry */
+#define	R_MIPS_GOT16	9	/* 16 bit GOT entry */
+#define	R_MIPS_PC16	10	/* PC relative 16 bit */
+#define	R_MIPS_CALL16	11	/* 16 bit GOT entry for function */
+#define	R_MIPS_GPREL32	12	/* GP relative 32 bit */
+#define	R_MIPS_64	18	/* Direct 64 bit */
+#define	R_MIPS_GOTHI16	21	/* GOT HI 16 bit */
+#define	R_MIPS_GOTLO16	22	/* GOT LO 16 bit */
+#define	R_MIPS_CALLHI16 30	/* upper 16 bit GOT entry for function */
+#define	R_MIPS_CALLLO16 31	/* lower 16 bit GOT entry for function */
+
+#define	R_PPC_NONE		0	/* No relocation. */
+#define	R_PPC_ADDR32		1
+#define	R_PPC_ADDR24		2
+#define	R_PPC_ADDR16		3
+#define	R_PPC_ADDR16_LO		4
+#define	R_PPC_ADDR16_HI		5
+#define	R_PPC_ADDR16_HA		6
+#define	R_PPC_ADDR14		7
+#define	R_PPC_ADDR14_BRTAKEN	8
+#define	R_PPC_ADDR14_BRNTAKEN	9
+#define	R_PPC_REL24		10
+#define	R_PPC_REL14		11
+#define	R_PPC_REL14_BRTAKEN	12
+#define	R_PPC_REL14_BRNTAKEN	13
+#define	R_PPC_GOT16		14
+#define	R_PPC_GOT16_LO		15
+#define	R_PPC_GOT16_HI		16
+#define	R_PPC_GOT16_HA		17
+#define	R_PPC_PLTREL24		18
+#define	R_PPC_COPY		19
+#define	R_PPC_GLOB_DAT		20
+#define	R_PPC_JMP_SLOT		21
+#define	R_PPC_RELATIVE		22
+#define	R_PPC_LOCAL24PC		23
+#define	R_PPC_UADDR32		24
+#define	R_PPC_UADDR16		25
+#define	R_PPC_REL32		26
+#define	R_PPC_PLT32		27
+#define	R_PPC_PLTREL32		28
+#define	R_PPC_PLT16_LO		29
+#define	R_PPC_PLT16_HI		30
+#define	R_PPC_PLT16_HA		31
+#define	R_PPC_SDAREL16		32
+#define	R_PPC_SECTOFF		33
+#define	R_PPC_SECTOFF_LO	34
+#define	R_PPC_SECTOFF_HI	35
+#define	R_PPC_SECTOFF_HA	36
+
+/*
+ * 64-bit relocations
+ */
+#define	R_PPC64_ADDR64		38
+#define	R_PPC64_ADDR16_HIGHER	39
+#define	R_PPC64_ADDR16_HIGHERA	40
+#define	R_PPC64_ADDR16_HIGHEST	41
+#define	R_PPC64_ADDR16_HIGHESTA	42
+#define	R_PPC64_UADDR64		43
+#define	R_PPC64_REL64		44
+#define	R_PPC64_PLT64		45
+#define	R_PPC64_PLTREL64	46
+#define	R_PPC64_TOC16		47
+#define	R_PPC64_TOC16_LO	48
+#define	R_PPC64_TOC16_HI	49
+#define	R_PPC64_TOC16_HA	50
+#define	R_PPC64_TOC		51
+#define	R_PPC64_DTPMOD64	68
+#define	R_PPC64_TPREL64		73
+#define	R_PPC64_DTPREL64	78
+
+/*
+ * TLS relocations
+ */
+#define	R_PPC_TLS		67
+#define	R_PPC_DTPMOD32		68
+#define	R_PPC_TPREL16		69
+#define	R_PPC_TPREL16_LO	70
+#define	R_PPC_TPREL16_HI	71
+#define	R_PPC_TPREL16_HA	72
+#define	R_PPC_TPREL32		73
+#define	R_PPC_DTPREL16		74
+#define	R_PPC_DTPREL16_LO	75
+#define	R_PPC_DTPREL16_HI	76
+#define	R_PPC_DTPREL16_HA	77
+#define	R_PPC_DTPREL32		78
+#define	R_PPC_GOT_TLSGD16	79
+#define	R_PPC_GOT_TLSGD16_LO	80
+#define	R_PPC_GOT_TLSGD16_HI	81
+#define	R_PPC_GOT_TLSGD16_HA	82
+#define	R_PPC_GOT_TLSLD16	83
+#define	R_PPC_GOT_TLSLD16_LO	84
+#define	R_PPC_GOT_TLSLD16_HI	85
+#define	R_PPC_GOT_TLSLD16_HA	86
+#define	R_PPC_GOT_TPREL16	87
+#define	R_PPC_GOT_TPREL16_LO	88
+#define	R_PPC_GOT_TPREL16_HI	89
+#define	R_PPC_GOT_TPREL16_HA	90
+
+/*
+ * The remaining relocs are from the Embedded ELF ABI, and are not in the
+ *  SVR4 ELF ABI.
+ */
+
+#define	R_PPC_EMB_NADDR32	101
+#define	R_PPC_EMB_NADDR16	102
+#define	R_PPC_EMB_NADDR16_LO	103
+#define	R_PPC_EMB_NADDR16_HI	104
+#define	R_PPC_EMB_NADDR16_HA	105
+#define	R_PPC_EMB_SDAI16	106
+#define	R_PPC_EMB_SDA2I16	107
+#define	R_PPC_EMB_SDA2REL	108
+#define	R_PPC_EMB_SDA21		109
+#define	R_PPC_EMB_MRKREF	110
+#define	R_PPC_EMB_RELSEC16	111
+#define	R_PPC_EMB_RELST_LO	112
+#define	R_PPC_EMB_RELST_HI	113
+#define	R_PPC_EMB_RELST_HA	114
+#define	R_PPC_EMB_BIT_FLD	115
+#define	R_PPC_EMB_RELSDA	116
+
+#define	R_SPARC_NONE		0
+#define	R_SPARC_8		1
+#define	R_SPARC_16		2
+#define	R_SPARC_32		3
+#define	R_SPARC_DISP8		4
+#define	R_SPARC_DISP16		5
+#define	R_SPARC_DISP32		6
+#define	R_SPARC_WDISP30		7
+#define	R_SPARC_WDISP22		8
+#define	R_SPARC_HI22		9
+#define	R_SPARC_22		10
+#define	R_SPARC_13		11
+#define	R_SPARC_LO10		12
+#define	R_SPARC_GOT10		13
+#define	R_SPARC_GOT13		14
+#define	R_SPARC_GOT22		15
+#define	R_SPARC_PC10		16
+#define	R_SPARC_PC22		17
+#define	R_SPARC_WPLT30		18
+#define	R_SPARC_COPY		19
+#define	R_SPARC_GLOB_DAT	20
+#define	R_SPARC_JMP_SLOT	21
+#define	R_SPARC_RELATIVE	22
+#define	R_SPARC_UA32		23
+#define	R_SPARC_PLT32		24
+#define	R_SPARC_HIPLT22		25
+#define	R_SPARC_LOPLT10		26
+#define	R_SPARC_PCPLT32		27
+#define	R_SPARC_PCPLT22		28
+#define	R_SPARC_PCPLT10		29
+#define	R_SPARC_10		30
+#define	R_SPARC_11		31
+#define	R_SPARC_64		32
+#define	R_SPARC_OLO10		33
+#define	R_SPARC_HH22		34
+#define	R_SPARC_HM10		35
+#define	R_SPARC_LM22		36
+#define	R_SPARC_PC_HH22		37
+#define	R_SPARC_PC_HM10		38
+#define	R_SPARC_PC_LM22		39
+#define	R_SPARC_WDISP16		40
+#define	R_SPARC_WDISP19		41
+#define	R_SPARC_GLOB_JMP	42
+#define	R_SPARC_7		43
+#define	R_SPARC_5		44
+#define	R_SPARC_6		45
+#define	R_SPARC_DISP64		46
+#define	R_SPARC_PLT64		47
+#define	R_SPARC_HIX22		48
+#define	R_SPARC_LOX10		49
+#define	R_SPARC_H44		50
+#define	R_SPARC_M44		51
+#define	R_SPARC_L44		52
+#define	R_SPARC_REGISTER	53
+#define	R_SPARC_UA64		54
+#define	R_SPARC_UA16		55
+#define	R_SPARC_TLS_GD_HI22	56
+#define	R_SPARC_TLS_GD_LO10	57
+#define	R_SPARC_TLS_GD_ADD	58
+#define	R_SPARC_TLS_GD_CALL	59
+#define	R_SPARC_TLS_LDM_HI22	60
+#define	R_SPARC_TLS_LDM_LO10	61
+#define	R_SPARC_TLS_LDM_ADD	62
+#define	R_SPARC_TLS_LDM_CALL	63
+#define	R_SPARC_TLS_LDO_HIX22	64
+#define	R_SPARC_TLS_LDO_LOX10	65
+#define	R_SPARC_TLS_LDO_ADD	66
+#define	R_SPARC_TLS_IE_HI22	67
+#define	R_SPARC_TLS_IE_LO10	68
+#define	R_SPARC_TLS_IE_LD	69
+#define	R_SPARC_TLS_IE_LDX	70
+#define	R_SPARC_TLS_IE_ADD	71
+#define	R_SPARC_TLS_LE_HIX22	72
+#define	R_SPARC_TLS_LE_LOX10	73
+#define	R_SPARC_TLS_DTPMOD32	74
+#define	R_SPARC_TLS_DTPMOD64	75
+#define	R_SPARC_TLS_DTPOFF32	76
+#define	R_SPARC_TLS_DTPOFF64	77
+#define	R_SPARC_TLS_TPOFF32	78
+#define	R_SPARC_TLS_TPOFF64	79
+
+#define	R_X86_64_NONE		0	/* No relocation. */
+#define	R_X86_64_64		1	/* Add 64 bit symbol value. */
+#define	R_X86_64_PC32		2	/* PC-relative 32 bit signed sym value. */
+#define	R_X86_64_GOT32		3	/* PC-relative 32 bit GOT offset. */
+#define	R_X86_64_PLT32		4	/* PC-relative 32 bit PLT offset. */
+#define	R_X86_64_COPY		5	/* Copy data from shared object. */
+#define	R_X86_64_GLOB_DAT	6	/* Set GOT entry to data address. */
+#define	R_X86_64_JMP_SLOT	7	/* Set GOT entry to code address. */
+#define	R_X86_64_RELATIVE	8	/* Add load address of shared object. */
+#define	R_X86_64_GOTPCREL	9	/* Add 32 bit signed pcrel offset to GOT. */
+#define	R_X86_64_32		10	/* Add 32 bit zero extended symbol value */
+#define	R_X86_64_32S		11	/* Add 32 bit sign extended symbol value */
+#define	R_X86_64_16		12	/* Add 16 bit zero extended symbol value */
+#define	R_X86_64_PC16		13	/* Add 16 bit signed extended pc relative symbol value */
+#define	R_X86_64_8		14	/* Add 8 bit zero extended symbol value */
+#define	R_X86_64_PC8		15	/* Add 8 bit signed extended pc relative symbol value */
+#define	R_X86_64_DTPMOD64	16	/* ID of module containing symbol */
+#define	R_X86_64_DTPOFF64	17	/* Offset in TLS block */
+#define	R_X86_64_TPOFF64	18	/* Offset in static TLS block */
+#define	R_X86_64_TLSGD		19	/* PC relative offset to GD GOT entry */
+#define	R_X86_64_TLSLD		20	/* PC relative offset to LD GOT entry */
+#define	R_X86_64_DTPOFF32	21	/* Offset in TLS block */
+#define	R_X86_64_GOTTPOFF	22	/* PC relative offset to IE GOT entry */
+#define	R_X86_64_TPOFF32	23	/* Offset in static TLS block */
+#define	R_X86_64_IRELATIVE	37
+
+#endif /* !_SYS_ELF_COMMON_H_ */
diff --git a/ta/remoteproc/include/elf_parser.h b/ta/remoteproc/include/elf_parser.h
new file mode 100644
index 000000000..5950db971
--- /dev/null
+++ b/ta/remoteproc/include/elf_parser.h
@@ -0,0 +1,59 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef ELF_PARSER
+#define ELF_PARSER
+
+#include <elf32.h>
+#include <stdint.h>
+#include <tee_api_types.h>
+
+/**
+ * struct resource_table - firmware resource table header
+ * @ver: version number
+ * @num: number of resource entries
+ * @reserved: reserved (must be zero)
+ * @offset: array of offsets pointing at the various resource entries
+ *
+ * A resource table is essentially a list of system resources required
+ * by the remote processor. It may also include configuration entries.
+ * If needed, the remote processor firmware should contain this table
+ * as a dedicated ".resource_table" ELF section.
+ *
+ * This structure shall be consistent with the Linux kernel structure
+ * definition from include/linux/remoteproc.h.
+ */
+struct resource_table {
+	uint32_t ver;
+	uint32_t num;
+	uint32_t reserved[2];
+	uint32_t offset[];
+} __packed;
+
+struct fw_elf32 {
+	uintptr_t e_entry;
+	uintptr_t e_phoff;
+	uintptr_t e_shoff;
+	uint32_t  e_phnum;
+	uint32_t  e_shnum;
+	uint32_t  e_phentsize;
+	uint32_t  e_shentsize;
+
+	Elf32_Phdr *phdr;
+	Elf32_Shdr *shdr;
+};
+
+TEE_Result e32_parse_ehdr(uint8_t *fw, size_t size);
+TEE_Result e32_parser_load_elf_image(uint8_t *fw,  size_t fw_size,
+				     TEE_Result (*load_seg)(uint8_t *src,
+							    uint32_t size,
+							    uint32_t da,
+							    uint32_t mem_size,
+							    void *priv),
+				     void *priv_data);
+int e32_parser_find_rsc_table(uint8_t *fw, size_t fw_size, Elf32_Addr *rsc_addr,
+			      Elf32_Word *rsc_size);
+
+#endif /*ELF_PARSER*/
diff --git a/ta/remoteproc/include/ta_remoteproc.h b/ta/remoteproc/include/ta_remoteproc.h
new file mode 100644
index 000000000..a51ba5f70
--- /dev/null
+++ b/ta/remoteproc/include/ta_remoteproc.h
@@ -0,0 +1,65 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef TA_RPROC_FW_H
+#define TA_RPROC_FW_H
+
+/*
+ * This UUID is generated with uuidgen
+ * the ITU-T UUID generator at http://www.itu.int/ITU-T/asn1/uuid.html
+ */
+#define TA_REMOTEPROC_UUID \
+	{ 0x80a4c275, 0x0a47, 0x4905, \
+		{ 0x82, 0x85, 0x14, 0x86, 0xa9, 0x77, 0x1a, 0x08} }
+
+/* The function IDs implemented in this TA */
+
+/*
+ * Authentication of the firmware and load in the remote processor memory.
+ *
+ * [in]  params[0].value.a:	unique 32bit identifier of the firmware
+ * [in]  params[1].memref:	buffer containing the image of the firmware
+ */
+#define TA_RPROC_FW_CMD_LOAD_FW		1
+
+/*
+ * Start the remote processor.
+ *
+ * [in]  params[0].value.a:	unique 32bit identifier of the firmware
+ */
+#define TA_RPROC_FW_CMD_START_FW	2
+
+/*
+ * Stop the remote processor.
+ *
+ * [in]  params[0].value.a:	unique 32bit identifier of the firmware
+ */
+#define TA_RPROC_FW_CMD_STOP_FW		3
+
+/*
+ * Return the physical address of the resource table, or 0 if not found
+ * No check is done to verify that the address returned is accessible by the
+ * non secure world. If the resource table is loaded in a protected memory,
+ * then accesses from non-secure world will likely fail.
+ *
+ * [in]  params[0].value.a:	unique 32bit identifier of the firmware
+ * [out] params[1].value.a:	32bit LSB resource table memory address
+ * [out] params[1].value.b:	32bit MSB resource table memory address
+ * [out] params[2].value.a:	32bit LSB resource table memory size
+ * [out] params[2].value.b:	32bit MSB resource table memory size
+ */
+#define TA_RPROC_FW_CMD_GET_RSC_TABLE	4
+
+/*
+ * Get remote processor firmware core dump. If found, return either
+ * TEE_SUCCESS on successful completion or TEE_ERROR_SHORT_BUFFER if output
+ * buffer is too short to store the core dump.
+ *
+ * [in]  params[0].value.a:	unique 32bit identifier of the firmware
+ * [out] params[1].memref:	Core dump, if found
+ */
+#define TA_RPROC_FW_CMD_GET_COREDUMP	5
+
+#endif /*TA_RPROC_FW_H*/
diff --git a/ta/remoteproc/include/user_ta_header_defines.h b/ta/remoteproc/include/user_ta_header_defines.h
new file mode 100644
index 000000000..c2ef1b21b
--- /dev/null
+++ b/ta/remoteproc/include/user_ta_header_defines.h
@@ -0,0 +1,29 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#ifndef USER_TA_HEADER_DEFINES_H
+#define USER_TA_HEADER_DEFINES_H
+
+#include <ta_remoteproc.h>
+
+#define TA_UUID				TA_REMOTEPROC_UUID
+
+#define TA_FLAGS			(TA_FLAG_DEVICE_ENUM | \
+					 TA_FLAG_SINGLE_INSTANCE | \
+					 TA_FLAG_INSTANCE_KEEP_ALIVE)
+
+/* Provisioned stack size */
+#define TA_STACK_SIZE			(4 * 1024)
+
+/* Provisioned heap size for TEE_Malloc() and friends */
+#define TA_DATA_SIZE			(4 * 1024)
+
+/* The gpd.ta.version property */
+#define TA_VERSION	"1.0"
+
+/* The gpd.ta.description property */
+#define TA_DESCRIPTION	"remote processor firmware management"
+
+#endif /* USER_TA_HEADER_DEFINES_H */
diff --git a/ta/remoteproc/remoteproc_core.c b/ta/remoteproc/remoteproc_core.c
new file mode 100644
index 000000000..454edf2fb
--- /dev/null
+++ b/ta/remoteproc/remoteproc_core.c
@@ -0,0 +1,781 @@
+ // SPDX-License-Identifier: BSD-2-Clause
+ /*
+  * Copyright (C) 2020-2021, STMicroelectronics - All Rights Reserved
+  */
+
+#include <elf_parser.h>
+#include <remoteproc_pta.h>
+#include <string.h>
+#include <sys/queue.h>
+#include <ta_remoteproc.h>
+#include <tee_internal_api.h>
+#include <tee_internal_api_extensions.h>
+#include <types_ext.h>
+#include <utee_defines.h>
+
+/* Firmware state */
+enum remoteproc_state {
+	REMOTEPROC_OFF,
+	REMOTEPROC_LOADED,
+	REMOTEPROC_STARTED,
+};
+
+#define RPROC_HDR_MAGIC		0x3543A468 /*random value */
+#define HEADER_VERSION		1
+
+/* Supported signature algorithm */
+enum remoteproc_sign_type {
+	RPROC_RSASSA_PKCS1_v1_5_SHA256 = 1,
+	RPROC_ECDSA_SHA256 = 2,
+};
+
+/*
+ * struct remoteproc_segment - program header with hash structure
+ * @phdr: program header
+ * @hash: hash associated to the program segment.
+ */
+struct remoteproc_segment {
+	Elf32_Phdr phdr;
+	unsigned char hash[TEE_SHA256_HASH_SIZE];
+};
+
+/*
+ * struct remoteproc_fw_hdr - firmware header
+ * @magic:        Magic number, must be equal to RPROC_HDR_MAGIC
+ * @version:      Version of the header (must be 1)
+ * @hdr_length:   Total header byte length including chunks
+ * @sign_length:  Signature chunk byte length
+ * @sign_offset:  Signature chunk byte offset from header start
+ * @sign_type:    Signature type
+ * @phhdr_length: Program header with hashes byte size, possibly 0
+ * @phhdr_offset: Program header with hashes byte offset, 0 if not used
+ * @phhdr_type:   Program header with hash type or 0 if not used
+ * @key_length:   Authentication key info byte length, possibly 0
+ * @key_offset:   Authentication key info byte offset, 0 if not used
+ * @img_length:   Firmware image chunk byte length
+ * @img_offset:   Firmware image chunk byte offset
+ * @img_type:     Firmware image type
+ */
+struct remoteproc_fw_hdr {
+	uint32_t magic;
+	uint32_t version;
+	uint32_t hdr_length;
+	uint32_t sign_length;
+	uint32_t sign_offset;
+	uint32_t sign_type;
+	uint32_t phhdr_length;
+	uint32_t phhdr_offset;
+	uint32_t phhdr_type;
+	uint32_t key_length;
+	uint32_t key_offset;
+	uint32_t img_length;
+	uint32_t img_offset;
+	uint32_t img_type;
+};
+
+/*
+ * struct remoteproc_sig_algo - signature algorithm information
+ * @sign_type: Header signature type
+ * @id:        Signature algorigthm identifier TEE_ALG_*
+ * @hash_len:  Signature hash length
+ */
+struct remoteproc_sig_algo {
+	enum remoteproc_sign_type sign_type;
+	uint32_t id;
+	size_t hash_len;
+};
+
+/*
+ * struct remoteproc_context - firmware context
+ * @fw_id:       Unique Id of the firmware
+ * @hdr:         Location of a secure copy of the firmware header
+ * @fw_img:      Firmware image
+ * @fw_img_size: Byte size of the firmware image
+ * @rsc_pa:      Physical address of the firmware resource table
+ * @rsc_size:    Byte size of the firmware resource table
+ * @state:       Remote-processor state
+ * @hw_fmt:      Image format capabilities of the remoteproc PTA
+ * @hw_img_prot: Image protection capabilities of the remoteproc PTA
+ * @link:        Linked list element
+ */
+struct remoteproc_context {
+	uint32_t fw_id;
+	struct remoteproc_fw_hdr *hdr;
+	uint8_t *fw_img;
+	size_t fw_img_size;
+	paddr_t rsc_pa;
+	uint32_t rsc_size;
+	enum remoteproc_state state;
+	uint32_t hw_fmt;
+	uint32_t hw_img_prot;
+	TAILQ_ENTRY(remoteproc_context) link;
+};
+
+TAILQ_HEAD(remoteproc_firmware_head, remoteproc_context);
+
+static struct remoteproc_firmware_head firmware_head =
+	TAILQ_HEAD_INITIALIZER(firmware_head);
+
+static const struct remoteproc_sig_algo rproc_ta_sign_algo[] = {
+	{
+		.sign_type = RPROC_RSASSA_PKCS1_v1_5_SHA256,
+		.id = TEE_ALG_RSASSA_PKCS1_V1_5_SHA256,
+		.hash_len = TEE_SHA256_HASH_SIZE,
+	},
+	{
+		.sign_type = RPROC_ECDSA_SHA256,
+		.id = TEE_ALG_ECDSA_P256,
+		.hash_len = TEE_SHA256_HASH_SIZE,
+	},
+};
+
+static size_t session_refcount;
+static TEE_TASessionHandle pta_session;
+
+static void remoteproc_header_dump(struct remoteproc_fw_hdr __maybe_unused *hdr)
+{
+	DMSG("magic :\t%#"PRIx32, hdr->magic);
+	DMSG("version :\t%#"PRIx32, hdr->version);
+	DMSG("hdr_length :\t%#"PRIx32, hdr->hdr_length);
+	DMSG("sign_length :\t%#"PRIx32, hdr->sign_length);
+	DMSG("sign_offset :\t%#"PRIx32, hdr->sign_offset);
+	DMSG("sign_type :\t%#"PRIx32, hdr->sign_type);
+	DMSG("phhdr_length :\t%#"PRIx32, hdr->phhdr_length);
+	DMSG("phhdr_offset :\t%#"PRIx32, hdr->phhdr_offset);
+	DMSG("phhdr_type :\t%#"PRIx32, hdr->phhdr_type);
+	DMSG("key_length :\t%#"PRIx32, hdr->key_length);
+	DMSG("key_offset :\t%#"PRIx32, hdr->key_offset);
+	DMSG("img_length :\t%#"PRIx32, hdr->img_length);
+	DMSG("img_offset :\t%#"PRIx32, hdr->img_offset);
+	DMSG("img_type :\t%#"PRIx32, hdr->img_type);
+}
+
+static struct remoteproc_context *remoteproc_find_firmware(uint32_t fw_id)
+{
+	struct remoteproc_context *ctx = NULL;
+
+	TAILQ_FOREACH(ctx, &firmware_head, link) {
+		if (ctx->fw_id == fw_id)
+			return ctx;
+	}
+
+	return NULL;
+}
+
+static struct remoteproc_context *remoteproc_add_firmware(uint32_t fw_id)
+{
+	struct remoteproc_context *ctx = NULL;
+
+	ctx = TEE_Malloc(sizeof(*ctx), TEE_MALLOC_FILL_ZERO);
+	if (!ctx)
+		return NULL;
+
+	ctx->fw_id = fw_id;
+
+	TAILQ_INSERT_TAIL(&firmware_head, ctx, link);
+
+	return ctx;
+}
+
+static const struct remoteproc_sig_algo *remoteproc_get_algo(uint32_t sign_type)
+{
+	unsigned int i = 0;
+
+	for (i = 0; i < ARRAY_SIZE(rproc_ta_sign_algo); i++)
+		if (sign_type == rproc_ta_sign_algo[i].sign_type)
+			return &rproc_ta_sign_algo[i];
+
+	return NULL;
+}
+
+static TEE_Result remoteproc_pta_verify(struct remoteproc_context *ctx,
+					const struct remoteproc_sig_algo  *algo,
+					char *hash, uint32_t hash_len)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct remoteproc_fw_hdr *hdr = ctx->hdr;
+	struct rproc_pta_key_info *keyinfo = NULL;
+	uint32_t param_types = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+					       TEE_PARAM_TYPE_MEMREF_INPUT,
+					       TEE_PARAM_TYPE_MEMREF_INPUT,
+					       TEE_PARAM_TYPE_MEMREF_INPUT);
+	TEE_Param params[TEE_NUM_PARAMS] = { };
+
+	keyinfo = TEE_Malloc(sizeof(*keyinfo) + hdr->key_length, 0);
+	if (!keyinfo)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	keyinfo->algo = algo->id;
+	keyinfo->info_size = hdr->key_length;
+	memcpy(keyinfo->info, (uint8_t *)hdr + hdr->key_offset,
+	       hdr->key_length);
+
+	params[0].value.a = ctx->fw_id;
+	params[1].memref.buffer = keyinfo;
+	params[1].memref.size = RPROC_PTA_GET_KEYINFO_SIZE(keyinfo);
+	params[2].memref.buffer = hash;
+	params[2].memref.size = hash_len;
+	params[3].memref.buffer = (uint8_t *)hdr + hdr->sign_offset;
+	params[3].memref.size = hdr->sign_length;
+
+	res = TEE_InvokeTACommand(pta_session, TEE_TIMEOUT_INFINITE,
+				  PTA_REMOTEPROC_VERIFY_DIGEST,
+				  param_types, params, NULL);
+	if (res != TEE_SUCCESS)
+		EMSG("Failed to verify signature, res = %#"PRIx32, res);
+
+	TEE_Free(keyinfo);
+
+	return res;
+}
+
+static TEE_Result remoteproc_save_fw_header(struct remoteproc_context *ctx,
+					    void *fw_orig,
+					    uint32_t fw_orig_size)
+{
+	struct remoteproc_fw_hdr *hdr = fw_orig;
+
+	remoteproc_header_dump(hdr);
+
+	if (fw_orig_size <= sizeof(*hdr) || fw_orig_size <= hdr->hdr_length)
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	ctx->hdr = TEE_Malloc(hdr->hdr_length, TEE_MALLOC_FILL_ZERO);
+	if (!ctx->hdr)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	memcpy(ctx->hdr, fw_orig, hdr->hdr_length);
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result remoteproc_verify_signature(struct remoteproc_context *ctx)
+{
+	TEE_OperationHandle op = TEE_HANDLE_NULL;
+	struct remoteproc_fw_hdr *hdr = ctx->hdr;
+	const struct remoteproc_sig_algo  *algo = NULL;
+	TEE_Result res = TEE_ERROR_GENERIC;
+	char *hash = NULL;
+	uint32_t hash_len = 0;
+
+	algo = remoteproc_get_algo(hdr->sign_type);
+	if (!algo) {
+		EMSG("Unsupported signature type %d", hdr->sign_type);
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	/* Compute the header hash */
+	hash_len = algo->hash_len;
+	hash = TEE_Malloc(hash_len, 0);
+	if (!hash)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = TEE_AllocateOperation(&op, TEE_ALG_SHA256, TEE_MODE_DIGEST, 0);
+	if (res != TEE_SUCCESS)
+		goto free_hash;
+
+	res = TEE_DigestDoFinal(op, hdr, hdr->sign_offset, hash, &hash_len);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	/*
+	 * TODO:
+	 * Provide alternative to verify the signature in the TA. This could
+	 * be done for instance by getting the key object from secure storage.
+	 */
+
+	/* By default ask the pta to verify the signature. */
+	res = remoteproc_pta_verify(ctx, algo, hash, hash_len);
+
+out:
+	TEE_FreeOperation(op);
+free_hash:
+	TEE_Free(hash);
+
+	return res;
+}
+
+static TEE_Result remoteproc_verify_header(struct remoteproc_context *ctx)
+{
+	struct remoteproc_fw_hdr *hdr = ctx->hdr;
+	uint32_t hdr_size = 0;
+	uint32_t chunk_size = 0;
+	uint32_t alignment = 0;
+
+	if (hdr->magic != RPROC_HDR_MAGIC)
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	if (hdr->version != HEADER_VERSION)
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	/*
+	 * The offsets are aligned to 64 bits format. The hdr_length takes into
+	 * account these alignments while the length of each chunks are the
+	 * effective length,excluding the alignment padding bytes.
+	 */
+	alignment = hdr->sign_length % sizeof(uint64_t) +
+		    hdr->phhdr_length % sizeof(uint64_t) +
+		    hdr->key_length % sizeof(uint64_t);
+
+	if (ADD_OVERFLOW(sizeof(*hdr), hdr->sign_length, &hdr_size) ||
+	    ADD_OVERFLOW(hdr_size, hdr->phhdr_length, &hdr_size) ||
+	    ADD_OVERFLOW(hdr_size, hdr->key_length, &hdr_size) ||
+	    ADD_OVERFLOW(hdr_size, alignment, &hdr_size) ||
+	    hdr->hdr_length != hdr_size)
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	if (ADD_OVERFLOW(hdr->sign_offset, hdr->sign_length, &chunk_size) ||
+	    chunk_size > hdr_size ||
+	    ADD_OVERFLOW(hdr->key_offset, hdr->key_length, &chunk_size) ||
+	    chunk_size > hdr_size ||
+	    ADD_OVERFLOW(hdr->phhdr_offset, hdr->phhdr_length, &chunk_size) ||
+	    chunk_size > hdr_size)
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	if (hdr->phhdr_length % sizeof(struct remoteproc_segment))
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	return remoteproc_verify_signature(ctx);
+}
+
+static TEE_Result get_rproc_pta_capabilities(struct remoteproc_context *ctx)
+{
+	uint32_t param_types = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+					       TEE_PARAM_TYPE_VALUE_OUTPUT,
+					       TEE_PARAM_TYPE_VALUE_OUTPUT,
+					       TEE_PARAM_TYPE_NONE);
+	TEE_Param params[TEE_NUM_PARAMS] = { };
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	params[0].value.a = ctx->fw_id;
+
+	res = TEE_InvokeTACommand(pta_session, TEE_TIMEOUT_INFINITE,
+				  PTA_REMOTEPROC_HW_CAPABILITIES,
+				  param_types, params, NULL);
+	if (res)
+		return res;
+
+	ctx->hw_fmt = params[1].value.a;
+	ctx->hw_img_prot = params[2].value.a;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result remoteproc_verify_firmware(struct remoteproc_context *ctx,
+					     uint8_t *fw_orig,
+					     uint32_t fw_orig_size)
+{
+	struct remoteproc_fw_hdr *hdr = NULL;
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	res = get_rproc_pta_capabilities(ctx);
+	if (res)
+		return res;
+
+	/* Secure the firmware image depending on strategy */
+	if (!(ctx->hw_img_prot & PTA_REMOTEPROC_FW_WITH_HASH_TABLE) ||
+	    ctx->hw_fmt != PTA_REMOTEPROC_ELF_FMT) {
+		/*
+		 * Only hash table for ELF format support implemented
+		 * in a first step.
+		 */
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	}
+
+	res = remoteproc_save_fw_header(ctx, fw_orig, fw_orig_size);
+	if (res)
+		return res;
+
+	res = remoteproc_verify_header(ctx);
+	if (res)
+		goto free_hdr;
+
+	/* Store location of the loadable binary in non-secure memory */
+	hdr = ctx->hdr;
+	ctx->fw_img_size = hdr->img_length;
+	ctx->fw_img = fw_orig + hdr->img_offset;
+
+	DMSG("Firmware image addr: %p size: %zu", ctx->fw_img,
+	     ctx->fw_img_size);
+
+free_hdr:
+	TEE_Free(ctx->hdr);
+
+	return res;
+}
+
+static paddr_t remoteproc_da_to_pa(uint32_t da, uint32_t size, void *priv)
+{
+	struct remoteproc_context *ctx = priv;
+	uint32_t param_types = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+					       TEE_PARAM_TYPE_VALUE_INPUT,
+					       TEE_PARAM_TYPE_VALUE_INPUT,
+					       TEE_PARAM_TYPE_VALUE_OUTPUT);
+	TEE_Param params[TEE_NUM_PARAMS] = { };
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	params[0].value.a = ctx->fw_id;
+	params[1].value.a = da;
+	params[2].value.a = size;
+
+	res = TEE_InvokeTACommand(pta_session, TEE_TIMEOUT_INFINITE,
+				  PTA_REMOTEPROC_FIRMWARE_DA_TO_PA,
+				  param_types, params, NULL);
+	if (res != TEE_SUCCESS) {
+		EMSG("Failed to translate device address %#"PRIx32, da);
+		return 0;
+	}
+
+	return (paddr_t)reg_pair_to_64(params[3].value.b, params[3].value.a);
+}
+
+static TEE_Result remoteproc_parse_rsc_table(struct remoteproc_context *ctx)
+{
+	uint32_t da = 0;
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	res = e32_parser_find_rsc_table(ctx->fw_img, ctx->fw_img_size,
+					&da, &ctx->rsc_size);
+	if (res == TEE_ERROR_NO_DATA) {
+		/* Firmware without resource table */
+		ctx->rsc_size = 0;
+		ctx->rsc_pa = 0;
+		return TEE_SUCCESS;
+	}
+	if (res)
+		return res;
+
+	if (da) {
+		DMSG("Resource table device address %#"PRIx32" size %zu",
+		     da, ctx->rsc_size);
+
+		ctx->rsc_pa = remoteproc_da_to_pa(da, ctx->rsc_size, ctx);
+		if (!ctx->rsc_pa)
+			return TEE_ERROR_ACCESS_DENIED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result get_segment_hash(struct remoteproc_context *ctx, uint8_t *src,
+				   uint32_t size, uint32_t da,
+				   uint32_t mem_size, unsigned char **hash)
+{
+	struct remoteproc_fw_hdr *hdr = ctx->hdr;
+	struct remoteproc_segment *peh = NULL;
+	unsigned int i = 0;
+	unsigned int nb_entry = hdr->phhdr_length / sizeof(*peh);
+
+	peh = (void *)((uint8_t *)hdr + hdr->phhdr_offset);
+
+	for (i = 0; i < nb_entry; peh++, i++) {
+		if (peh->phdr.p_paddr != da)
+			continue;
+
+		/*
+		 * Segment is read from a non secure memory. Crosscheck it using
+		 * the hash table to verify that the segment has not been
+		 * corrupted.
+		 */
+		if (peh->phdr.p_type != PT_LOAD)
+			return TEE_ERROR_CORRUPT_OBJECT;
+
+		if (peh->phdr.p_filesz != size || peh->phdr.p_memsz != mem_size)
+			return TEE_ERROR_CORRUPT_OBJECT;
+
+		if ((Elf32_Off)(src - ctx->fw_img) !=  peh->phdr.p_offset)
+			return TEE_ERROR_CORRUPT_OBJECT;
+
+		*hash = peh->hash;
+
+		return TEE_SUCCESS;
+	}
+
+	return TEE_ERROR_NO_DATA;
+}
+
+static TEE_Result remoteproc_load_segment(uint8_t *src, uint32_t size,
+					  uint32_t da, uint32_t mem_size,
+					  void *priv)
+{
+	struct remoteproc_context *ctx = priv;
+	uint32_t param_types = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+					       TEE_PARAM_TYPE_MEMREF_INPUT,
+					       TEE_PARAM_TYPE_VALUE_INPUT,
+					       TEE_PARAM_TYPE_MEMREF_INPUT);
+	TEE_Param params[TEE_NUM_PARAMS] = { };
+	TEE_Result res = TEE_ERROR_GENERIC;
+	unsigned char *hash = NULL;
+
+	/*
+	 * Invoke platform remoteproc PTA to load the segment in remote
+	 * processor memory which is not mapped in the TA space.
+	 */
+
+	DMSG("Load segment %#"PRIx32" size %"PRIu32" (%"PRIu32")", da, size,
+	     mem_size);
+
+	res = get_segment_hash(ctx, src, size, da, mem_size, &hash);
+	if (res)
+		return res;
+
+	params[0].value.a = ctx->fw_id;
+	params[1].memref.buffer = src;
+	params[1].memref.size = size;
+	params[2].value.a = da;
+	params[3].memref.buffer = hash;
+	params[3].memref.size = TEE_SHA256_HASH_SIZE;
+
+	res = TEE_InvokeTACommand(pta_session, TEE_TIMEOUT_INFINITE,
+				  PTA_REMOTEPROC_LOAD_SEGMENT_SHA256,
+				  param_types, params, NULL);
+	if (res != TEE_SUCCESS) {
+		EMSG("Fails to load segment, res = 0x%x", res);
+		return res;
+	}
+
+	/* Fill the rest of the memory with 0 */
+	if (size < mem_size) {
+		param_types = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+					      TEE_PARAM_TYPE_VALUE_INPUT,
+					      TEE_PARAM_TYPE_VALUE_INPUT,
+					      TEE_PARAM_TYPE_VALUE_INPUT);
+		params[1].value.a = da + size;
+		params[2].value.a = mem_size - size;
+		params[3].value.a = 0;
+
+		res = TEE_InvokeTACommand(pta_session, TEE_TIMEOUT_INFINITE,
+					  PTA_REMOTEPROC_SET_MEMORY,
+					  param_types, params, NULL);
+		if (res != TEE_SUCCESS)
+			EMSG("Fails to clear segment, res = 0x%x", res);
+	}
+
+	return res;
+}
+
+static TEE_Result remoteproc_load_elf(struct remoteproc_context *ctx)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	res = e32_parse_ehdr(ctx->fw_img, ctx->fw_img_size);
+	if (res) {
+		EMSG("Failed to parse firmware, res = %#"PRIx32, res);
+		return res;
+	}
+
+	return e32_parser_load_elf_image(ctx->fw_img, ctx->fw_img_size,
+					 remoteproc_load_segment, ctx);
+}
+
+static TEE_Result remoteproc_load_fw(uint32_t pt,
+				     TEE_Param params[TEE_NUM_PARAMS])
+{
+	const uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+						TEE_PARAM_TYPE_MEMREF_INPUT,
+						TEE_PARAM_TYPE_NONE,
+						TEE_PARAM_TYPE_NONE);
+	struct remoteproc_context *ctx = NULL;
+	uint32_t fw_id = params[0].value.a;
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	if (pt != exp_pt)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ctx = remoteproc_find_firmware(fw_id);
+	if (!ctx)
+		ctx = remoteproc_add_firmware(fw_id);
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	if (ctx->state != REMOTEPROC_OFF)
+		return TEE_ERROR_BAD_STATE;
+
+	if (!params[1].memref.buffer || !params[1].memref.size)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	DMSG("Got base addr: %p size %zu", params[1].memref.buffer,
+	     params[1].memref.size);
+
+	res = remoteproc_verify_firmware(ctx, params[1].memref.buffer,
+					 params[1].memref.size);
+	if (res) {
+		EMSG("Can't Authenticate the firmware, res = %#"PRIx32, res);
+		goto out;
+	}
+
+	res = remoteproc_load_elf(ctx);
+	if (res)
+		goto out;
+
+	/* Take opportunity to get the resource table address */
+	res = remoteproc_parse_rsc_table(ctx);
+	if (res == TEE_SUCCESS)
+		ctx->state = REMOTEPROC_LOADED;
+
+out:
+	/* Clear reference to firmware image from shared memory */
+	ctx->fw_img = NULL;
+	ctx->fw_img_size =  0;
+
+	return res;
+}
+
+static TEE_Result remoteproc_start_fw(uint32_t pt,
+				      TEE_Param params[TEE_NUM_PARAMS])
+{
+	const uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+						TEE_PARAM_TYPE_NONE,
+						TEE_PARAM_TYPE_NONE,
+						TEE_PARAM_TYPE_NONE);
+	struct remoteproc_context *ctx = NULL;
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	if (pt != exp_pt)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ctx = remoteproc_find_firmware(params[0].value.a);
+	if (!ctx)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	switch (ctx->state) {
+	case REMOTEPROC_OFF:
+		res = TEE_ERROR_BAD_STATE;
+		break;
+	case REMOTEPROC_STARTED:
+		res =  TEE_SUCCESS;
+		break;
+	case REMOTEPROC_LOADED:
+		res = TEE_InvokeTACommand(pta_session, TEE_TIMEOUT_INFINITE,
+					  PTA_REMOTEPROC_FIRMWARE_START,
+					  pt, params, NULL);
+		if (res == TEE_SUCCESS)
+			ctx->state = REMOTEPROC_STARTED;
+		break;
+	default:
+		res = TEE_ERROR_BAD_STATE;
+	}
+
+	return res;
+}
+
+static TEE_Result remoteproc_stop_fw(uint32_t pt,
+				     TEE_Param params[TEE_NUM_PARAMS])
+{
+	const uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+						TEE_PARAM_TYPE_NONE,
+						TEE_PARAM_TYPE_NONE,
+						TEE_PARAM_TYPE_NONE);
+	struct remoteproc_context *ctx = NULL;
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	if (pt != exp_pt)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ctx = remoteproc_find_firmware(params[0].value.a);
+	if (!ctx)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	switch (ctx->state) {
+	case REMOTEPROC_LOADED:
+		res = TEE_ERROR_BAD_STATE;
+		break;
+	case REMOTEPROC_OFF:
+		res = TEE_SUCCESS;
+		break;
+	case REMOTEPROC_STARTED:
+		res = TEE_InvokeTACommand(pta_session, TEE_TIMEOUT_INFINITE,
+					  PTA_REMOTEPROC_FIRMWARE_STOP,
+					  pt, params, NULL);
+		if (res == TEE_SUCCESS)
+			ctx->state = REMOTEPROC_OFF;
+		break;
+	default:
+		res = TEE_ERROR_BAD_STATE;
+	}
+
+	return res;
+}
+
+static TEE_Result remoteproc_get_rsc_table(uint32_t pt,
+					   TEE_Param params[TEE_NUM_PARAMS])
+{
+	const uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+						TEE_PARAM_TYPE_VALUE_OUTPUT,
+						TEE_PARAM_TYPE_VALUE_OUTPUT,
+						TEE_PARAM_TYPE_NONE);
+	struct remoteproc_context *ctx = NULL;
+
+	if (pt != exp_pt)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ctx = remoteproc_find_firmware(params[0].value.a);
+	if (!ctx)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (ctx->state == REMOTEPROC_OFF)
+		return TEE_ERROR_BAD_STATE;
+
+	reg_pair_from_64((uint64_t)ctx->rsc_pa,
+			 &params[1].value.b, &params[1].value.a);
+	reg_pair_from_64((uint64_t)ctx->rsc_size,
+			 &params[2].value.b, &params[2].value.a);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result TA_CreateEntryPoint(void)
+{
+	return TEE_SUCCESS;
+}
+
+void TA_DestroyEntryPoint(void)
+{
+}
+
+TEE_Result TA_OpenSessionEntryPoint(uint32_t pt __unused,
+				    TEE_Param params[TEE_NUM_PARAMS] __unused,
+				    void **sess __unused)
+{
+	static const TEE_UUID uuid = PTA_REMOTEPROC_UUID;
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	if (!session_refcount) {
+		res = TEE_OpenTASession(&uuid, TEE_TIMEOUT_INFINITE, 0, NULL,
+					&pta_session, NULL);
+		if (res)
+			return res;
+	}
+
+	session_refcount++;
+
+	return TEE_SUCCESS;
+}
+
+void TA_CloseSessionEntryPoint(void *sess __unused)
+{
+	session_refcount--;
+
+	if (!session_refcount)
+		TEE_CloseTASession(pta_session);
+}
+
+TEE_Result TA_InvokeCommandEntryPoint(void *sess __unused, uint32_t cmd_id,
+				      uint32_t pt,
+				      TEE_Param params[TEE_NUM_PARAMS])
+{
+	switch (cmd_id) {
+	case TA_RPROC_FW_CMD_LOAD_FW:
+		return remoteproc_load_fw(pt, params);
+	case TA_RPROC_FW_CMD_START_FW:
+		return remoteproc_start_fw(pt, params);
+	case TA_RPROC_FW_CMD_STOP_FW:
+		return remoteproc_stop_fw(pt, params);
+	case TA_RPROC_FW_CMD_GET_RSC_TABLE:
+		return remoteproc_get_rsc_table(pt, params);
+	case TA_RPROC_FW_CMD_GET_COREDUMP:
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	default:
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+}
diff --git a/ta/remoteproc/sub.mk b/ta/remoteproc/sub.mk
new file mode 100644
index 000000000..caca5901a
--- /dev/null
+++ b/ta/remoteproc/sub.mk
@@ -0,0 +1,3 @@
+global-incdirs-y += include
+srcs-y += remoteproc_core.c
+srcs-y += elf_parser.c
diff --git a/ta/remoteproc/user_ta.mk b/ta/remoteproc/user_ta.mk
new file mode 100644
index 000000000..6db23f705
--- /dev/null
+++ b/ta/remoteproc/user_ta.mk
@@ -0,0 +1 @@
+user-ta-uuid := 80a4c275-0a47-4905-8285-1486a9771a08
\ No newline at end of file
-- 
2.17.1

