From 423ad55f481adefb98fdd57f391e185213733ec6 Mon Sep 17 00:00:00 2001
From: Romuald JEANNE <romuald.jeanne@st.com>
Date: Mon, 15 Mar 2021 17:31:45 +0100
Subject: [PATCH] st update v2.4-r1.0.0

---
 .editorconfig                                 |    6 +-
 CONTRIBUTING.md                               |   30 +
 Makefile                                      |   10 +-
 bl1/aarch32/bl1_entrypoint.S                  |   14 +-
 bl2/aarch32/bl2_el3_entrypoint.S              |    5 +-
 bl2/aarch32/bl2_el3_exceptions.S              |    9 +
 bl2/aarch32/bl2_entrypoint.S                  |    9 +
 bl2/bl2_image_load_v2.c                       |   14 +-
 bl2u/aarch32/bl2u_entrypoint.S                |    9 +
 bl32/sp_min/aarch32/entrypoint.S              |   22 +-
 bl32/sp_min/sp_min.ld.S                       |   10 +-
 common/aarch32/debug.S                        |   52 +-
 common/bl_common.c                            |    2 +-
 common/fdt_fixup.c                            |   44 +-
 docs/devicetree/bindings/arm/secure.txt       |   53 +
 .../bindings/clock/st,stm32mp1-rcc.txt        |  496 +++++
 docs/devicetree/bindings/i2c/i2c-stm32.txt    |   54 +
 .../memory-controllers/st,stm32mp1-ddr.txt    |  301 +++
 docs/devicetree/bindings/mmc/mmci.txt         |   72 +
 .../bindings/mmc/st,stm32-sdmmc2.txt          |   22 +
 .../bindings/power/st,stm32mp1-pwr.txt        |   43 +
 docs/devicetree/bindings/power/st,stpmic1.txt |   94 +
 .../bindings/reset/st,stm32mp1-rcc.txt        |    6 +
 docs/devicetree/bindings/rng/st,stm32-rng.txt |   23 +
 .../bindings/serial/st,stm32-usart.txt        |   88 +
 .../bindings/soc/st,stm32-etzpc.txt           |   56 +
 .../bindings/soc/st,stm32-romem.txt           |   74 +
 .../bindings/soc/st,stm32-stgen.txt           |   18 +
 .../devicetree/bindings/soc/st,stm32-tamp.txt |   22 +
 .../bindings/watchdog/st,stm32-iwdg.txt       |   28 +
 docs/getting_started/porting-guide.rst        |   25 +
 docs/plat/stm32mp1.rst                        |  169 +-
 drivers/arm/tzc/tzc400.c                      |  135 +-
 drivers/arm/tzc/tzc_common_private.h          |   21 +
 drivers/auth/auth_mod.c                       |   14 +
 drivers/auth/mbedtls/mbedtls_common.mk        |    2 +-
 drivers/auth/mbedtls/mbedtls_x509_parser.c    |    4 +-
 drivers/auth/tbbr/tbbr_cot_bl1.c              |   15 -
 drivers/auth/tbbr/tbbr_cot_bl2.c              |    1 +
 drivers/auth/tbbr/tbbr_cot_common.c           |   15 +
 drivers/clk/clk.c                             |   64 +
 drivers/io/io_mtd.c                           |   68 +-
 drivers/mmc/mmc.c                             |   67 +-
 drivers/mtd/nand/core.c                       |   43 +-
 drivers/mtd/nor/spi_nor.c                     |   20 +-
 drivers/st/bsec/{bsec.c => bsec2.c}           |  534 +++--
 drivers/st/clk/stm32mp1_calib.c               |  536 +++++
 drivers/st/clk/stm32mp1_clk.c                 | 1829 ++++++++++++++---
 drivers/st/clk/stm32mp_clkfunc.c              |  220 +-
 drivers/st/crypto/stm32_hash.c                |   19 +-
 drivers/st/ddr/stm32mp1_ddr.c                 |  331 ++-
 drivers/st/ddr/stm32mp1_ddr_helpers.c         |  593 +++++-
 drivers/st/ddr/stm32mp1_ram.c                 |  120 +-
 drivers/st/etzpc/etzpc.c                      |   50 +-
 drivers/st/fmc/stm32_fmc2_nand.c              |    5 +-
 drivers/st/gpio/stm32_gpio.c                  |    9 +-
 drivers/st/i2c/stm32_i2c.c                    |  460 ++++-
 drivers/st/io/io_stm32image.c                 |  196 +-
 drivers/st/iwdg/stm32_iwdg.c                  |  151 +-
 drivers/st/mmc/stm32_sdmmc2.c                 |  125 +-
 drivers/st/pmic/stm32mp_pmic.c                |  484 ++++-
 drivers/st/pmic/stpmic1.c                     |  116 +-
 .../st/regulator/stm32mp_dummy_regulator.c    |   27 +
 drivers/st/regulator/stm32mp_regulator.c      |   38 +
 drivers/st/reset/stm32mp1_reset.c             |   31 +-
 drivers/st/rng/stm32_rng.c                    |  193 ++
 drivers/st/rtc/stm32_rtc.c                    |  480 +++++
 drivers/st/spi/stm32_qspi.c                   |    5 +-
 drivers/st/tamper/stm32_tamp.c                |  379 ++++
 drivers/st/timer/stm32_timer.c                |  323 +++
 drivers/st/uart/aarch32/stm32_console.S       |   23 +-
 drivers/st/uart/stm32_uart.c                  |  405 ++++
 drivers/st/usb_dwc2/usb_dwc2.c                | 1094 ++++++++++
 fdts/stm32mp15-bl2.dtsi                       |  116 ++
 fdts/stm32mp15-bl32.dtsi                      |   42 +
 fdts/stm32mp15-ddr-1g-fw-config.dts           |   63 +
 fdts/stm32mp15-ddr-512m-fw-config.dts         |   63 +
 fdts/stm32mp15-ddr.dtsi                       |   12 +
 fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi      |    4 +-
 fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi      |    4 +-
 fdts/stm32mp15-pinctrl.dtsi                   |   87 +-
 fdts/stm32mp15-ssp-bl2.dtsi                   |  125 ++
 fdts/stm32mp151.dtsi                          |   95 +-
 fdts/stm32mp153.dtsi                          |    1 +
 fdts/stm32mp157a-avenger96-fw-config.dts      |    6 +
 fdts/stm32mp157a-avenger96.dts                |   20 +
 fdts/stm32mp157a-dk1-fw-config.dts            |    6 +
 fdts/stm32mp157a-dk1.dts                      |   18 +
 fdts/stm32mp157a-ed1-fw-config.dts            |    6 +
 fdts/stm32mp157a-ed1.dts                      |   38 +
 fdts/stm32mp157a-ev1-fw-config.dts            |    6 +
 fdts/stm32mp157a-ev1.dts                      |   24 +
 fdts/stm32mp157c-dk2-fw-config.dts            |    6 +
 fdts/stm32mp157c-dk2.dts                      |   18 +
 fdts/stm32mp157c-ed1-fw-config.dts            |    6 +
 fdts/stm32mp157c-ed1.dts                      |  329 +--
 fdts/stm32mp157c-ev1-fw-config.dts            |    6 +
 fdts/stm32mp157c-ev1.dts                      |   47 +-
 fdts/stm32mp157d-dk1-fw-config.dts            |    6 +
 fdts/stm32mp157d-dk1.dts                      |   45 +
 fdts/stm32mp157d-ed1-fw-config.dts            |    6 +
 fdts/stm32mp157d-ed1.dts                      |   39 +
 fdts/stm32mp157d-ev1-fw-config.dts            |    6 +
 fdts/stm32mp157d-ev1.dts                      |   23 +
 fdts/stm32mp157f-dk2-fw-config.dts            |    6 +
 fdts/stm32mp157f-dk2.dts                      |   51 +
 fdts/stm32mp157f-ed1-fw-config.dts            |    6 +
 fdts/stm32mp157f-ed1.dts                      |   43 +
 fdts/stm32mp157f-ev1-fw-config.dts            |    6 +
 fdts/stm32mp157f-ev1.dts                      |   23 +
 fdts/stm32mp15xa.dtsi                         |   13 +
 fdts/stm32mp15xc.dtsi                         |    3 +
 fdts/stm32mp15xd.dtsi                         |   19 +
 fdts/stm32mp15xf.dtsi                         |   21 +
 fdts/stm32mp15xx-dkx.dtsi                     |  180 +-
 fdts/stm32mp15xx-edx.dtsi                     |  518 +++++
 fdts/stm32mp15xx-evx.dtsi                     |   73 +
 fdts/stm32mp15xxaa-pinctrl.dtsi               |    3 +-
 fdts/stm32mp15xxab-pinctrl.dtsi               |    2 +-
 fdts/stm32mp15xxac-pinctrl.dtsi               |    3 +-
 fdts/stm32mp15xxad-pinctrl.dtsi               |    2 +-
 include/arch/aarch32/arch.h                   |   18 +-
 include/arch/aarch32/arch_helpers.h           |    4 +
 include/arch/aarch32/el3_common_macros.S      |   42 +-
 include/arch/aarch64/arch.h                   |    6 +-
 include/common/bl_common.ld.h                 |    9 +
 include/common/tbbr/cot_def.h                 |    2 +-
 include/drivers/arm/tzc400.h                  |   44 +-
 include/drivers/auth/tbbr_cot_common.h        |    1 +
 include/drivers/clk.h                         |   28 +
 include/drivers/io/io_mtd.h                   |   13 +-
 include/drivers/mmc.h                         |   27 +-
 include/drivers/nand.h                        |   12 +-
 include/drivers/st/bsec.h                     |  159 +-
 include/drivers/st/bsec2_reg.h                |   98 +
 include/drivers/st/stm32_i2c.h                |   41 +-
 include/drivers/st/stm32_iwdg.h               |    1 +
 include/drivers/st/stm32_rng.h                |   13 +
 include/drivers/st/stm32_rtc.h                |   78 +
 include/drivers/st/stm32_sdmmc2.h             |    2 +
 include/drivers/st/stm32_tamp.h               |  163 ++
 include/drivers/st/stm32_timer.h              |   21 +
 include/drivers/st/stm32_uart.h               |  170 ++
 include/drivers/st/stm32mp1_calib.h           |   20 +
 include/drivers/st/stm32mp1_clk.h             |   52 +-
 include/drivers/st/stm32mp1_ddr.h             |   13 +-
 include/drivers/st/stm32mp1_ddr_helpers.h     |   20 +-
 include/drivers/st/stm32mp1_ddr_regs.h        |   14 +-
 include/drivers/st/stm32mp1_pwr.h             |   21 +-
 include/drivers/st/stm32mp1_ram.h             |    3 +-
 include/drivers/st/stm32mp1_rcc.h             |   32 +-
 include/drivers/st/stm32mp_clkfunc.h          |   19 +-
 include/drivers/st/stm32mp_dummy_regulator.h  |   14 +
 include/drivers/st/stm32mp_pmic.h             |   41 +-
 include/drivers/st/stm32mp_regulator.h        |   31 +
 include/drivers/st/stm32mp_reset.h            |   14 +-
 include/drivers/st/stpmic1.h                  |   29 +-
 include/drivers/st/usb_dwc2.h                 |   19 +
 include/dt-bindings/clock/stm32mp1-clks.h     |    6 +
 include/dt-bindings/power/stm32mp1-power.h    |   19 +
 include/dt-bindings/reset/stm32mp1-resets.h   |    2 +
 include/dt-bindings/soc/st,stm32-etzpc.h      |   86 +
 include/dt-bindings/soc/stm32mp1-tzc400.h     |   37 +
 include/lib/optee_utils.h                     |    1 +
 include/lib/psci/psci.h                       |    1 +
 include/lib/usb/usb_core.h                    |  277 +++
 include/lib/usb/usb_st_dfu.h                  |   85 +
 include/lib/utils_def.h                       |   10 +
 include/plat/common/platform.h                |    9 +-
 lib/aarch32/misc_helpers.S                    |  126 ++
 lib/optee/optee_utils.c                       |   37 +-
 lib/psci/psci_private.h                       |    1 -
 lib/usb/usb_core.c                            |  838 ++++++++
 lib/usb/usb_st_dfu.c                          |  537 +++++
 make_helpers/defaults.mk                      |    8 +-
 plat/common/aarch32/platform_helpers.S        |   34 +
 plat/common/plat_bl_common.c                  |    2 +-
 plat/st/common/bl2_io_storage.c               |  506 ++---
 plat/st/common/bl2_stm32_io_storage.c         |  769 +++++++
 plat/st/common/include/stm32cubeprogrammer.h  |   67 +
 plat/st/common/include/stm32mp_auth.h         |   19 -
 plat/st/common/include/stm32mp_common.h       |   53 +-
 plat/st/common/include/stm32mp_dt.h           |   16 +-
 plat/st/common/include/stm32mp_fconf_getter.h |   29 +
 plat/st/common/include/stm32mp_io_storage.h   |   23 +
 .../st/common/include/stm32mp_shres_helpers.h |   65 +-
 plat/st/common/stm32_gic.c                    |  223 ++
 plat/st/common/stm32cubeprogrammer_uart.c     |  690 +++++++
 plat/st/common/stm32cubeprogrammer_usb.c      |  352 ++++
 plat/st/common/stm32mp_auth.c                 |   90 -
 plat/st/common/stm32mp_common.c               |  158 +-
 plat/st/common/stm32mp_cot.c                  |  114 +
 plat/st/common/stm32mp_crypto_lib.c           |  447 ++++
 plat/st/common/stm32mp_dt.c                   |  347 +++-
 plat/st/common/stm32mp_fconf_io.c             |  136 ++
 plat/st/common/stm32mp_img_parser_lib.c       |   75 +
 plat/st/common/stm32mp_shres_helpers.c        |   63 +
 plat/st/common/stm32mp_trusted_boot.c         |  143 ++
 plat/st/stm32mp1/bl2_plat_setup.c             |  516 ++++-
 plat/st/stm32mp1/include/boot_api.h           |  552 ++++-
 plat/st/stm32mp1/include/platform_def.h       |   40 +-
 .../include/stm32mp15_mbedtls_config.h        |  119 ++
 plat/st/stm32mp1/include/stm32mp1_context.h   |   29 +-
 .../stm32mp1/include/stm32mp1_critic_power.h  |   22 +
 plat/st/stm32mp1/include/stm32mp1_low_power.h |   23 +
 .../stm32mp1/include/stm32mp1_power_config.h  |   29 +
 plat/st/stm32mp1/include/stm32mp1_private.h   |   33 +-
 .../include/stm32mp1_shared_resources.h       |   17 +
 plat/st/stm32mp1/include/stm32mp1_smc.h       |   87 +-
 plat/st/stm32mp1/plat_bl2_mem_params_desc.c   |   71 +-
 .../stm32mp1/plat_bl2_stm32_mem_params_desc.c |  111 +
 plat/st/stm32mp1/plat_image_load.c            |   35 +-
 plat/st/stm32mp1/platform.mk                  |  258 ++-
 plat/st/stm32mp1/services/bsec_svc.c          |  455 +++-
 plat/st/stm32mp1/services/low_power_svc.c     |   22 +
 plat/st/stm32mp1/services/low_power_svc.h     |   14 +
 plat/st/stm32mp1/services/pwr_svc.c           |  102 +
 plat/st/stm32mp1/services/pwr_svc.h           |   12 +
 plat/st/stm32mp1/services/rcc_svc.c           |  169 ++
 plat/st/stm32mp1/services/rcc_svc.h           |   14 +
 .../st/stm32mp1/services/stm32mp1_svc_setup.c |   34 +-
 plat/st/stm32mp1/sp_min/sp_min-stm32mp1.mk    |   30 +-
 plat/st/stm32mp1/sp_min/sp_min_setup.c        |  487 ++++-
 plat/st/stm32mp1/stm32mp1.S                   |    4 +-
 plat/st/stm32mp1/stm32mp1.ld.S                |   14 +-
 plat/st/stm32mp1/stm32mp1_boot_device.c       |    6 +-
 plat/st/stm32mp1/stm32mp1_context.c           |  483 ++++-
 plat/st/stm32mp1/stm32mp1_critic_power.c      |   91 +
 .../stm32mp1/stm32mp1_critic_power_wrapper.S  |   62 +
 plat/st/stm32mp1/stm32mp1_dbgmcu.c            |   48 +-
 plat/st/stm32mp1/stm32mp1_def.h               |  438 ++--
 plat/st/stm32mp1/stm32mp1_fconf_firewall.c    |  124 ++
 plat/st/stm32mp1/stm32mp1_gic.c               |   92 -
 plat/st/stm32mp1/stm32mp1_helper.S            |  135 +-
 plat/st/stm32mp1/stm32mp1_low_power.c         |  455 ++++
 plat/st/stm32mp1/stm32mp1_pm.c                |  107 +-
 plat/st/stm32mp1/stm32mp1_power_config.c      |  223 ++
 plat/st/stm32mp1/stm32mp1_private.c           |  625 +++++-
 plat/st/stm32mp1/stm32mp1_scmi.c              |  131 +-
 plat/st/stm32mp1/stm32mp1_security.c          |  119 +-
 plat/st/stm32mp1/stm32mp1_shared_resources.c  |  211 +-
 plat/st/stm32mp1/stm32mp1_ssp.c               | 1039 ++++++++++
 plat/st/stm32mp1/stm32mp1_ssp.mk              |   84 +
 plat/st/stm32mp1/stm32mp1_syscfg.c            |   51 +-
 plat/st/stm32mp1/stm32mp1_usb.c               |  491 +++++
 tools/cert_create/Makefile                    |    8 +-
 tools/cert_create/include/key.h               |    6 +-
 tools/cert_create/src/key.c                   |   20 +-
 tools/cert_create/src/main.c                  |    3 +-
 tools/encrypt_fw/Makefile                     |    8 +-
 tools/fiptool/Makefile                        |    8 +-
 tools/stm32image/stm32image.c                 |   46 +-
 252 files changed, 26563 insertions(+), 3015 deletions(-)
 create mode 100644 CONTRIBUTING.md
 create mode 100644 docs/devicetree/bindings/arm/secure.txt
 create mode 100644 docs/devicetree/bindings/clock/st,stm32mp1-rcc.txt
 create mode 100644 docs/devicetree/bindings/i2c/i2c-stm32.txt
 create mode 100644 docs/devicetree/bindings/memory-controllers/st,stm32mp1-ddr.txt
 create mode 100644 docs/devicetree/bindings/mmc/mmci.txt
 create mode 100644 docs/devicetree/bindings/mmc/st,stm32-sdmmc2.txt
 create mode 100644 docs/devicetree/bindings/power/st,stm32mp1-pwr.txt
 create mode 100644 docs/devicetree/bindings/power/st,stpmic1.txt
 create mode 100644 docs/devicetree/bindings/reset/st,stm32mp1-rcc.txt
 create mode 100644 docs/devicetree/bindings/rng/st,stm32-rng.txt
 create mode 100644 docs/devicetree/bindings/serial/st,stm32-usart.txt
 create mode 100644 docs/devicetree/bindings/soc/st,stm32-etzpc.txt
 create mode 100644 docs/devicetree/bindings/soc/st,stm32-romem.txt
 create mode 100644 docs/devicetree/bindings/soc/st,stm32-stgen.txt
 create mode 100644 docs/devicetree/bindings/soc/st,stm32-tamp.txt
 create mode 100644 docs/devicetree/bindings/watchdog/st,stm32-iwdg.txt
 create mode 100644 drivers/clk/clk.c
 rename drivers/st/bsec/{bsec.c => bsec2.c} (62%)
 create mode 100644 drivers/st/clk/stm32mp1_calib.c
 create mode 100644 drivers/st/regulator/stm32mp_dummy_regulator.c
 create mode 100644 drivers/st/regulator/stm32mp_regulator.c
 create mode 100644 drivers/st/rng/stm32_rng.c
 create mode 100644 drivers/st/rtc/stm32_rtc.c
 create mode 100644 drivers/st/tamper/stm32_tamp.c
 create mode 100644 drivers/st/timer/stm32_timer.c
 create mode 100644 drivers/st/uart/stm32_uart.c
 create mode 100644 drivers/st/usb_dwc2/usb_dwc2.c
 create mode 100644 fdts/stm32mp15-bl2.dtsi
 create mode 100644 fdts/stm32mp15-bl32.dtsi
 create mode 100644 fdts/stm32mp15-ddr-1g-fw-config.dts
 create mode 100644 fdts/stm32mp15-ddr-512m-fw-config.dts
 create mode 100644 fdts/stm32mp15-ssp-bl2.dtsi
 create mode 100644 fdts/stm32mp157a-avenger96-fw-config.dts
 create mode 100644 fdts/stm32mp157a-dk1-fw-config.dts
 create mode 100644 fdts/stm32mp157a-ed1-fw-config.dts
 create mode 100644 fdts/stm32mp157a-ed1.dts
 create mode 100644 fdts/stm32mp157a-ev1-fw-config.dts
 create mode 100644 fdts/stm32mp157a-ev1.dts
 create mode 100644 fdts/stm32mp157c-dk2-fw-config.dts
 create mode 100644 fdts/stm32mp157c-ed1-fw-config.dts
 create mode 100644 fdts/stm32mp157c-ev1-fw-config.dts
 create mode 100644 fdts/stm32mp157d-dk1-fw-config.dts
 create mode 100644 fdts/stm32mp157d-dk1.dts
 create mode 100644 fdts/stm32mp157d-ed1-fw-config.dts
 create mode 100644 fdts/stm32mp157d-ed1.dts
 create mode 100644 fdts/stm32mp157d-ev1-fw-config.dts
 create mode 100644 fdts/stm32mp157d-ev1.dts
 create mode 100644 fdts/stm32mp157f-dk2-fw-config.dts
 create mode 100644 fdts/stm32mp157f-dk2.dts
 create mode 100644 fdts/stm32mp157f-ed1-fw-config.dts
 create mode 100644 fdts/stm32mp157f-ed1.dts
 create mode 100644 fdts/stm32mp157f-ev1-fw-config.dts
 create mode 100644 fdts/stm32mp157f-ev1.dts
 create mode 100644 fdts/stm32mp15xa.dtsi
 create mode 100644 fdts/stm32mp15xd.dtsi
 create mode 100644 fdts/stm32mp15xf.dtsi
 create mode 100644 fdts/stm32mp15xx-edx.dtsi
 create mode 100644 fdts/stm32mp15xx-evx.dtsi
 create mode 100644 include/drivers/clk.h
 create mode 100644 include/drivers/st/bsec2_reg.h
 create mode 100644 include/drivers/st/stm32_rng.h
 create mode 100644 include/drivers/st/stm32_rtc.h
 create mode 100644 include/drivers/st/stm32_tamp.h
 create mode 100644 include/drivers/st/stm32_timer.h
 create mode 100644 include/drivers/st/stm32_uart.h
 create mode 100644 include/drivers/st/stm32mp1_calib.h
 create mode 100644 include/drivers/st/stm32mp_dummy_regulator.h
 create mode 100644 include/drivers/st/stm32mp_regulator.h
 create mode 100644 include/drivers/st/usb_dwc2.h
 create mode 100644 include/dt-bindings/power/stm32mp1-power.h
 create mode 100644 include/dt-bindings/soc/stm32mp1-tzc400.h
 create mode 100644 include/lib/usb/usb_core.h
 create mode 100644 include/lib/usb/usb_st_dfu.h
 create mode 100644 lib/usb/usb_core.c
 create mode 100644 lib/usb/usb_st_dfu.c
 create mode 100644 plat/st/common/bl2_stm32_io_storage.c
 create mode 100644 plat/st/common/include/stm32cubeprogrammer.h
 delete mode 100644 plat/st/common/include/stm32mp_auth.h
 create mode 100644 plat/st/common/include/stm32mp_fconf_getter.h
 create mode 100644 plat/st/common/include/stm32mp_io_storage.h
 create mode 100644 plat/st/common/stm32_gic.c
 create mode 100644 plat/st/common/stm32cubeprogrammer_uart.c
 create mode 100644 plat/st/common/stm32cubeprogrammer_usb.c
 delete mode 100644 plat/st/common/stm32mp_auth.c
 create mode 100644 plat/st/common/stm32mp_cot.c
 create mode 100644 plat/st/common/stm32mp_crypto_lib.c
 create mode 100644 plat/st/common/stm32mp_fconf_io.c
 create mode 100644 plat/st/common/stm32mp_img_parser_lib.c
 create mode 100644 plat/st/common/stm32mp_shres_helpers.c
 create mode 100644 plat/st/common/stm32mp_trusted_boot.c
 create mode 100644 plat/st/stm32mp1/include/stm32mp15_mbedtls_config.h
 create mode 100644 plat/st/stm32mp1/include/stm32mp1_critic_power.h
 create mode 100644 plat/st/stm32mp1/include/stm32mp1_low_power.h
 create mode 100644 plat/st/stm32mp1/include/stm32mp1_power_config.h
 create mode 100644 plat/st/stm32mp1/plat_bl2_stm32_mem_params_desc.c
 create mode 100644 plat/st/stm32mp1/services/low_power_svc.c
 create mode 100644 plat/st/stm32mp1/services/low_power_svc.h
 create mode 100644 plat/st/stm32mp1/services/pwr_svc.c
 create mode 100644 plat/st/stm32mp1/services/pwr_svc.h
 create mode 100644 plat/st/stm32mp1/services/rcc_svc.c
 create mode 100644 plat/st/stm32mp1/services/rcc_svc.h
 create mode 100644 plat/st/stm32mp1/stm32mp1_critic_power.c
 create mode 100644 plat/st/stm32mp1/stm32mp1_critic_power_wrapper.S
 create mode 100644 plat/st/stm32mp1/stm32mp1_fconf_firewall.c
 delete mode 100644 plat/st/stm32mp1/stm32mp1_gic.c
 create mode 100644 plat/st/stm32mp1/stm32mp1_low_power.c
 create mode 100644 plat/st/stm32mp1/stm32mp1_power_config.c
 create mode 100644 plat/st/stm32mp1/stm32mp1_ssp.c
 create mode 100644 plat/st/stm32mp1/stm32mp1_ssp.mk
 create mode 100644 plat/st/stm32mp1/stm32mp1_usb.c

diff --git a/.editorconfig b/.editorconfig
index f523ca19da..12f786de5a 100644
--- a/.editorconfig
+++ b/.editorconfig
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2017-2019, Arm Limited and Contributors. All rights reserved.
+# Copyright (c) 2017-2020, Arm Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -38,10 +38,10 @@ indent_style = tab
 insert_final_newline = true
 
 # [LCS] Chapter 2: Breaking long lines and strings
-#       "The limit on the length of lines is 80 columns"
+#       "The limit on the length of lines is 100 columns"
 #   This is a "soft" requirement for Arm-TF, and should not be the sole
 #   reason for changes.
-max_line_length = 80
+max_line_length = 100
 
 # [LCS] Chapter 1: Indentation
 #       "Tabs are 8 characters"
diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
new file mode 100644
index 0000000000..3d1bacd78a
--- /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/Makefile b/Makefile
index 5c9186ece3..db60400b8b 100644
--- a/Makefile
+++ b/Makefile
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2013-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2013-2021, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -548,11 +548,9 @@ endif
 endif
 	BL31_CFLAGS	+=	-fpie
 	BL31_LDFLAGS	+=	$(PIE_LDFLAGS)
-ifeq ($(ARCH),aarch64)
 	BL32_CFLAGS	+=	-fpie
 	BL32_LDFLAGS	+=	$(PIE_LDFLAGS)
 endif
-endif
 
 ifeq (${ARCH},aarch64)
 BL1_CPPFLAGS += -DIMAGE_AT_EL3
@@ -918,6 +916,7 @@ $(eval $(call assert_booleans,\
         RAS_TRAP_LOWER_EL_ERR_ACCESS \
         COT_DESC_IN_DTB \
         USE_SP804_TIMER \
+        AARCH32_EXCEPTION_DEBUG \
 )))
 
 $(eval $(call assert_numerics,\
@@ -1007,6 +1006,7 @@ $(eval $(call add_defines,\
         RAS_TRAP_LOWER_EL_ERR_ACCESS \
         COT_DESC_IN_DTB \
         USE_SP804_TIMER \
+        AARCH32_EXCEPTION_DEBUG \
 )))
 
 ifeq (${SANITIZE_UB},trap)
@@ -1224,7 +1224,7 @@ certtool: ${CRTTOOL}
 
 .PHONY: ${CRTTOOL}
 ${CRTTOOL}:
-	${Q}${MAKE} PLAT=${PLAT} USE_TBBR_DEFS=${USE_TBBR_DEFS} COT=${COT} OPENSSL_DIR=${OPENSSL_DIR} CRTTOOL=${CRTTOOL} --no-print-directory -C ${CRTTOOLPATH}
+	${Q}${MAKE} PLAT=${PLAT} USE_TBBR_DEFS=${USE_TBBR_DEFS} COT=${COT} CRTTOOL=${CRTTOOL} --no-print-directory -C ${CRTTOOLPATH}
 	@${ECHO_BLANK_LINE}
 	@echo "Built $@ successfully"
 	@${ECHO_BLANK_LINE}
@@ -1299,7 +1299,7 @@ enctool: ${ENCTOOL}
 
 .PHONY: ${ENCTOOL}
 ${ENCTOOL}:
-	${Q}${MAKE} PLAT=${PLAT} BUILD_INFO=0 OPENSSL_DIR=${OPENSSL_DIR} ENCTOOL=${ENCTOOL} --no-print-directory -C ${ENCTOOLPATH}
+	${Q}${MAKE} PLAT=${PLAT} BUILD_INFO=0 ENCTOOL=${ENCTOOL} --no-print-directory -C ${ENCTOOLPATH}
 	@${ECHO_BLANK_LINE}
 	@echo "Built $@ successfully"
 	@${ECHO_BLANK_LINE}
diff --git a/bl1/aarch32/bl1_entrypoint.S b/bl1/aarch32/bl1_entrypoint.S
index 6a155660b6..09ad3b035e 100644
--- a/bl1/aarch32/bl1_entrypoint.S
+++ b/bl1/aarch32/bl1_entrypoint.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -21,10 +21,19 @@
 	 */
 vector_base bl1_vector_table
 	b	bl1_entrypoint
+#if AARCH32_EXCEPTION_DEBUG
+	b	report_undef_inst	/* Undef */
+#else
 	b	report_exception	/* Undef */
+#endif
 	b	bl1_aarch32_smc_handler	/* SMC call */
+#if AARCH32_EXCEPTION_DEBUG
+	b	report_prefetch_abort	/* Prefetch abort */
+	b	report_data_abort	/* Data abort */
+#else
 	b	report_exception	/* Prefetch abort */
 	b	report_exception	/* Data abort */
+#endif
 	b	report_exception	/* Reserved */
 	b	report_exception	/* IRQ */
 	b	report_exception	/* FIQ */
@@ -49,7 +58,8 @@ func bl1_entrypoint
 		_secondary_cold_boot=!COLD_BOOT_SINGLE_CPU	\
 		_init_memory=1					\
 		_init_c_runtime=1				\
-		_exception_vectors=bl1_vector_table
+		_exception_vectors=bl1_vector_table		\
+		_pie_fixup_size=0
 
 	/* -----------------------------------------------------
 	 * Perform BL1 setup
diff --git a/bl2/aarch32/bl2_el3_entrypoint.S b/bl2/aarch32/bl2_el3_entrypoint.S
index 2e851e61a4..49d48980c0 100644
--- a/bl2/aarch32/bl2_el3_entrypoint.S
+++ b/bl2/aarch32/bl2_el3_entrypoint.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -26,7 +26,8 @@ func bl2_entrypoint
                 _secondary_cold_boot=!COLD_BOOT_SINGLE_CPU      \
                 _init_memory=1                                  \
                 _init_c_runtime=1                               \
-                _exception_vectors=bl2_vector_table
+                _exception_vectors=bl2_vector_table		\
+		_pie_fixup_size=0
 
 	/*
 	 * Restore parameters of boot rom
diff --git a/bl2/aarch32/bl2_el3_exceptions.S b/bl2/aarch32/bl2_el3_exceptions.S
index 087b6656dc..dff4e36a4a 100644
--- a/bl2/aarch32/bl2_el3_exceptions.S
+++ b/bl2/aarch32/bl2_el3_exceptions.S
@@ -12,10 +12,19 @@
 
 vector_base bl2_vector_table
 	b	bl2_entrypoint
+#if AARCH32_EXCEPTION_DEBUG
+	b	report_undef_inst	/* Undef */
+#else
 	b	report_exception	/* Undef */
+#endif
 	b	report_exception	/* SVC call */
+#if AARCH32_EXCEPTION_DEBUG
+	b	report_prefetch_abort	/* Prefetch abort */
+	b	report_data_abort	/* Data abort */
+#else
 	b	report_exception	/* Prefetch abort */
 	b	report_exception	/* Data abort */
+#endif
 	b	report_exception	/* Reserved */
 	b	report_exception	/* IRQ */
 	b	report_exception	/* FIQ */
diff --git a/bl2/aarch32/bl2_entrypoint.S b/bl2/aarch32/bl2_entrypoint.S
index 102fd2f514..bfd721ca1d 100644
--- a/bl2/aarch32/bl2_entrypoint.S
+++ b/bl2/aarch32/bl2_entrypoint.S
@@ -14,10 +14,19 @@
 
 vector_base bl2_vector_table
 	b	bl2_entrypoint
+#if AARCH32_EXCEPTION_DEBUG
+	b	report_undef_inst	/* Undef */
+#else
 	b	report_exception	/* Undef */
+#endif
 	b	report_exception	/* SVC call */
+#if AARCH32_EXCEPTION_DEBUG
+	b	report_prefetch_abort	/* Prefetch abort */
+	b	report_data_abort	/* Data abort */
+#else
 	b	report_exception	/* Prefetch abort */
 	b	report_exception	/* Data abort */
+#endif
 	b	report_exception	/* Reserved */
 	b	report_exception	/* IRQ */
 	b	report_exception	/* FIQ */
diff --git a/bl2/bl2_image_load_v2.c b/bl2/bl2_image_load_v2.c
index 48c9beca6c..4fb0e030d0 100644
--- a/bl2/bl2_image_load_v2.c
+++ b/bl2/bl2_image_load_v2.c
@@ -74,17 +74,17 @@ struct entry_point_info *bl2_load_images(void)
 				      bl2_node_info->image_id, err);
 				plat_error_handler(err);
 			}
+
+			/* Allow platform to handle image information. */
+			err = bl2_plat_handle_post_image_load(bl2_node_info->image_id);
+			if (err != 0) {
+				ERROR("BL2: Failure in post image load handling (%i)\n", err);
+				plat_error_handler(err);
+			}
 		} else {
 			INFO("BL2: Skip loading image id %d\n", bl2_node_info->image_id);
 		}
 
-		/* Allow platform to handle image information. */
-		err = bl2_plat_handle_post_image_load(bl2_node_info->image_id);
-		if (err != 0) {
-			ERROR("BL2: Failure in post image load handling (%i)\n", err);
-			plat_error_handler(err);
-		}
-
 		/* Go to next image */
 		bl2_node_info = bl2_node_info->next_load_info;
 	}
diff --git a/bl2u/aarch32/bl2u_entrypoint.S b/bl2u/aarch32/bl2u_entrypoint.S
index 6391f537cd..426176d98e 100644
--- a/bl2u/aarch32/bl2u_entrypoint.S
+++ b/bl2u/aarch32/bl2u_entrypoint.S
@@ -14,10 +14,19 @@
 
 vector_base bl2u_vector_table
 	b	bl2u_entrypoint
+#if AARCH32_EXCEPTION_DEBUG
+	b	report_undef_inst	/* Undef */
+#else
 	b	report_exception	/* Undef */
+#endif
 	b	report_exception	/* SVC call */
+#if AARCH32_EXCEPTION_DEBUG
+	b	report_prefetch_abort	/* Prefetch abort */
+	b	report_data_abort	/* Data abort */
+#else
 	b	report_exception	/* Prefetch abort */
 	b	report_exception	/* Data abort */
+#endif
 	b	report_exception	/* Reserved */
 	b	report_exception	/* IRQ */
 	b	report_exception	/* FIQ */
diff --git a/bl32/sp_min/aarch32/entrypoint.S b/bl32/sp_min/aarch32/entrypoint.S
index f3a1e440b7..b73943aca8 100644
--- a/bl32/sp_min/aarch32/entrypoint.S
+++ b/bl32/sp_min/aarch32/entrypoint.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -23,6 +23,8 @@
 	.globl	sp_min_handle_smc
 	.globl	sp_min_handle_fiq
 
+#define FIXUP_SIZE	((BL32_LIMIT) - (BL32_BASE))
+
 	.macro route_fiq_to_sp_min reg
 		/* -----------------------------------------------------
 		 * FIQs are secure interrupts trapped by Monitor and non
@@ -47,10 +49,19 @@
 
 vector_base sp_min_vector_table
 	b	sp_min_entrypoint
+#if AARCH32_EXCEPTION_DEBUG
+	b	report_undef_inst	/* Undef */
+#else
 	b	plat_panic_handler	/* Undef */
+#endif
 	b	sp_min_handle_smc	/* Syscall */
+#if AARCH32_EXCEPTION_DEBUG
+	b	report_prefetch_abort	/* Prefetch abort */
+	b	report_data_abort	/* Data abort */
+#else
 	b	plat_panic_handler	/* Prefetch abort */
 	b	plat_panic_handler	/* Data abort */
+#endif
 	b	plat_panic_handler	/* Reserved */
 	b	plat_panic_handler	/* IRQ */
 	b	sp_min_handle_fiq	/* FIQ */
@@ -87,7 +98,8 @@ func sp_min_entrypoint
 		_secondary_cold_boot=0				\
 		_init_memory=0					\
 		_init_c_runtime=1				\
-		_exception_vectors=sp_min_vector_table
+		_exception_vectors=sp_min_vector_table		\
+		_pie_fixup_size=FIXUP_SIZE
 
 	/* ---------------------------------------------------------------------
 	 * Relay the previous bootloader's arguments to the platform layer
@@ -106,7 +118,8 @@ func sp_min_entrypoint
 		_secondary_cold_boot=!COLD_BOOT_SINGLE_CPU	\
 		_init_memory=1					\
 		_init_c_runtime=1				\
-		_exception_vectors=sp_min_vector_table
+		_exception_vectors=sp_min_vector_table		\
+		_pie_fixup_size=FIXUP_SIZE
 
 	/* ---------------------------------------------------------------------
 	 * For RESET_TO_SP_MIN systems, BL32 (SP_MIN) is the first bootloader
@@ -306,7 +319,8 @@ func sp_min_warm_entrypoint
 		_secondary_cold_boot=0				\
 		_init_memory=0					\
 		_init_c_runtime=0				\
-		_exception_vectors=sp_min_vector_table
+		_exception_vectors=sp_min_vector_table		\
+		_pie_fixup_size=0
 
 	/*
 	 * We're about to enable MMU and participate in PSCI state coordination.
diff --git a/bl32/sp_min/sp_min.ld.S b/bl32/sp_min/sp_min.ld.S
index f202c7ada8..175305aa07 100644
--- a/bl32/sp_min/sp_min.ld.S
+++ b/bl32/sp_min/sp_min.ld.S
@@ -92,6 +92,7 @@ SECTIONS
     __RW_START__ = . ;
 
     DATA_SECTION >RAM
+    RELA_SECTION >RAM
 
 #ifdef BL32_PROGBITS_LIMIT
     ASSERT(. <= BL32_PROGBITS_LIMIT, "BL32 progbits has exceeded its limit.")
@@ -101,8 +102,6 @@ SECTIONS
     BSS_SECTION >RAM
     XLAT_TABLE_SECTION >RAM
 
-     __BSS_SIZE__ = SIZEOF(.bss);
-
 #if USE_COHERENT_MEM
     /*
      * The base address of the coherent memory section must be page-aligned (4K)
@@ -128,9 +127,6 @@ SECTIONS
         . = ALIGN(PAGE_SIZE);
         __COHERENT_RAM_END__ = .;
     } >RAM
-
-    __COHERENT_RAM_UNALIGNED_SIZE__ =
-        __COHERENT_RAM_END_UNALIGNED__ - __COHERENT_RAM_START__;
 #endif
 
     /*
@@ -141,5 +137,9 @@ SECTIONS
 
     __BL32_END__ = .;
 
+    /DISCARD/ : {
+        *(.dynsym .dynstr .hash .gnu.hash)
+    }
+
     ASSERT(. <= BL32_LIMIT, "BL32 image has exceeded its limit.")
 }
diff --git a/common/aarch32/debug.S b/common/aarch32/debug.S
index 9d410df07e..69b1ed4ed5 100644
--- a/common/aarch32/debug.S
+++ b/common/aarch32/debug.S
@@ -14,6 +14,11 @@
 	.globl	asm_assert
 	.globl	do_panic
 	.globl	report_exception
+#if AARCH32_EXCEPTION_DEBUG
+	.globl	report_undef_inst
+	.globl	report_prefetch_abort
+	.globl	report_data_abort
+#endif
 
 /* Since the max decimal input number is 65536 */
 #define MAX_DEC_DIVISOR		10000
@@ -113,10 +118,10 @@ endfunc asm_assert
 
 /*
  * This function prints a string from address in r4
- * Clobber: lr, r0 - r4
+ * Clobber: lr, r0 - r4, r7
  */
 func asm_print_str
-	mov	r3, lr
+	mov	r7, lr
 1:
 	ldrb	r0, [r4], #0x1
 	cmp	r0, #0
@@ -124,20 +129,20 @@ func asm_print_str
 	bl	plat_crash_console_putc
 	b	1b
 2:
-	bx	r3
+	bx	r7
 endfunc asm_print_str
 
 /*
  * This function prints a hexadecimal number in r4.
  * In: r4 = the hexadecimal to print.
- * Clobber: lr, r0 - r3, r5
+ * Clobber: lr, r0 - r3, r5, r7
  */
 func asm_print_hex
 	mov	r5, #32  /* No of bits to convert to ascii */
 
 	/* Convert to ascii number of bits in r5 */
 asm_print_hex_bits:
-	mov	r3, lr
+	mov	r7, lr
 1:
 	sub	r5, r5, #4
 	lsr	r0, r4, r5
@@ -153,7 +158,7 @@ asm_print_hex_bits:
 	bl	plat_crash_console_putc
 	cmp	r5, #0
 	bne	1b
-	bx	r3
+	bx	r7
 endfunc asm_print_hex
 
 	/***********************************************************
@@ -205,3 +210,38 @@ func report_exception
 	bl	plat_report_exception
 	no_ret	plat_panic_handler
 endfunc report_exception
+
+#if AARCH32_EXCEPTION_DEBUG
+	/***********************************************************
+	 * This function is called from the vector table for
+	 * undefined instruction. The lr_und is given as an
+	 * argument to platform handler.
+	 ***********************************************************/
+func report_undef_inst
+	mrs	r0, lr_und
+	bl	plat_report_undef_inst
+	no_ret	plat_panic_handler
+endfunc report_undef_inst
+
+	/***********************************************************
+	 * This function is called from the vector table for
+	 * unhandled exceptions. The lr_abt is given as an
+	 * argument to platform handler.
+	 ***********************************************************/
+func report_prefetch_abort
+	mrs	r0, lr_abt
+	bl	plat_report_prefetch_abort
+	no_ret	plat_panic_handler
+endfunc report_prefetch_abort
+
+	/***********************************************************
+	 * This function is called from the vector table for
+	 * unhandled exceptions. The lr_abt is given as an
+	 * argument to platform handler.
+	 ***********************************************************/
+func report_data_abort
+	mrs	r0, lr_abt
+	bl	plat_report_data_abort
+	no_ret	plat_panic_handler
+endfunc report_data_abort
+#endif
diff --git a/common/bl_common.c b/common/bl_common.c
index f17afcb115..22a0daf18c 100644
--- a/common/bl_common.c
+++ b/common/bl_common.c
@@ -241,7 +241,7 @@ int load_auth_image(unsigned int image_id, image_info_t *image_data)
 
 	do {
 		err = load_auth_image_internal(image_id, image_data);
-	} while ((err != 0) && (plat_try_next_boot_source() != 0));
+	} while ((err != 0) && (plat_try_next_boot_source(image_id) != 0));
 
 	return err;
 }
diff --git a/common/fdt_fixup.c b/common/fdt_fixup.c
index e88a550080..f6d5963890 100644
--- a/common/fdt_fixup.c
+++ b/common/fdt_fixup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -195,23 +195,51 @@ int fdt_add_reserved_memory(void *dtb, const char *node_name,
 			    uintptr_t base, size_t size)
 {
 	int offs = fdt_path_offset(dtb, "/reserved-memory");
-	uint32_t addresses[3];
+	uint32_t addr_len = sizeof(base) / sizeof(uint32_t);
+	uint32_t size_len = sizeof(size) / sizeof(uint32_t);
+	uint32_t addresses[4];
 
 	if (offs < 0) {			/* create if not existing yet */
 		offs = fdt_add_subnode(dtb, 0, "reserved-memory");
 		if (offs < 0)
 			return offs;
-		fdt_setprop_u32(dtb, offs, "#address-cells", 2);
-		fdt_setprop_u32(dtb, offs, "#size-cells", 1);
+
+		fdt_setprop_u32(dtb, offs, "#address-cells", addr_len);
+		fdt_setprop_u32(dtb, offs, "#size-cells", size_len);
 		fdt_setprop(dtb, offs, "ranges", NULL, 0);
+	} else {
+		const fdt32_t *prop;
+		int len;
+
+		prop = fdt_getprop(dtb, offs, "#address-cells", &len);
+		if ((prop == NULL) || (fdt32_to_cpu(*prop) != addr_len)) {
+			return -1;
+		}
+
+		prop = fdt_getprop(dtb, offs, "#size-cells", &len);
+		if ((prop == NULL) || (fdt32_to_cpu(*prop) != size_len)) {
+			return -1;
+		}
+	}
+
+	if (addr_len == 1U) {
+		addresses[0] = cpu_to_fdt32(base & 0xffffffff);
+	} else {
+		addresses[0] = cpu_to_fdt32(HIGH_BITS(base));
+		addresses[1] = cpu_to_fdt32(base & 0xffffffff);
+	}
+
+	if (size_len == 1U) {
+		addresses[addr_len] = cpu_to_fdt32(size & 0xffffffff);
+	} else {
+		addresses[addr_len] = cpu_to_fdt32(HIGH_BITS(size));
+		addresses[addr_len + 1U] = cpu_to_fdt32(size & 0xffffffff);
 	}
 
-	addresses[0] = cpu_to_fdt32(HIGH_BITS(base));
-	addresses[1] = cpu_to_fdt32(base & 0xffffffff);
-	addresses[2] = cpu_to_fdt32(size & 0xffffffff);
 	offs = fdt_add_subnode(dtb, offs, node_name);
 	fdt_setprop(dtb, offs, "no-map", NULL, 0);
-	fdt_setprop(dtb, offs, "reg", addresses, 12);
+	fdt_setprop(dtb, offs, "reg", addresses,
+		    (addr_len + size_len) * sizeof(uint32_t));
 
 	return 0;
 }
diff --git a/docs/devicetree/bindings/arm/secure.txt b/docs/devicetree/bindings/arm/secure.txt
new file mode 100644
index 0000000000..e31303fb23
--- /dev/null
+++ b/docs/devicetree/bindings/arm/secure.txt
@@ -0,0 +1,53 @@
+* ARM Secure world bindings
+
+ARM CPUs with TrustZone support have two distinct address spaces,
+"Normal" and "Secure". Most devicetree consumers (including the Linux
+kernel) are not TrustZone aware and run entirely in either the Normal
+world or the Secure world. However some devicetree consumers are
+TrustZone aware and need to be able to determine whether devices are
+visible only in the Secure address space, only in the Normal address
+space, or visible in both. (One example of that situation would be a
+virtual machine which boots Secure firmware and wants to tell the
+firmware about the layout of the machine via devicetree.)
+
+The general principle of the naming scheme for Secure world bindings
+is that any property that needs a different value in the Secure world
+can be supported by prefixing the property name with "secure-". So for
+instance "secure-foo" would override "foo". For property names with
+a vendor prefix, the Secure variant of "vendor,foo" would be
+"vendor,secure-foo". If there is no "secure-" property then the Secure
+world value is the same as specified for the Normal world by the
+non-prefixed property. However, only the properties listed below may
+validly have "secure-" versions; this list will be enlarged on a
+case-by-case basis.
+
+Defining the bindings in this way means that a device tree which has
+been annotated to indicate the presence of Secure-only devices can
+still be processed unmodified by existing Non-secure software (and in
+particular by the kernel).
+
+Note that it is still valid for bindings intended for purely Secure
+world consumers (like kernels that run entirely in Secure) to simply
+describe the view of Secure world using the standard bindings. These
+secure- bindings only need to be used where both the Secure and Normal
+world views need to be described in a single device tree.
+
+Valid Secure world properties:
+
+- secure-status : specifies whether the device is present and usable
+  in the secure world. The combination of this with "status" allows
+  the various possible combinations of device visibility to be
+  specified. If "secure-status" is not specified it defaults to the
+  same value as "status"; if "status" is not specified either then
+  both default to "okay". This means the following combinations are
+  possible:
+
+   /* Neither specified: default to visible in both S and NS */
+   secure-status = "okay";                          /* visible in both */
+   status = "okay";                                 /* visible in both */
+   status = "okay"; secure-status = "okay";         /* visible in both */
+   secure-status = "disabled";                      /* NS-only */
+   status = "okay"; secure-status = "disabled";     /* NS-only */
+   status = "disabled"; secure-status = "okay";     /* S-only */
+   status = "disabled";                             /* disabled in both */
+   status = "disabled"; secure-status = "disabled"; /* disabled in both */
diff --git a/docs/devicetree/bindings/clock/st,stm32mp1-rcc.txt b/docs/devicetree/bindings/clock/st,stm32mp1-rcc.txt
new file mode 100644
index 0000000000..7d2b5be9d7
--- /dev/null
+++ b/docs/devicetree/bindings/clock/st,stm32mp1-rcc.txt
@@ -0,0 +1,496 @@
+STMicroelectronics STM32 Peripheral Reset Clock Controller
+==========================================================
+
+The RCC IP is both a reset and a clock controller.
+
+RCC makes also power management (resume/supend and wakeup interrupt).
+
+Please also refer to reset.txt for common reset controller binding usage.
+
+Please also refer to clock-bindings.txt for common clock controller
+binding usage.
+
+
+Required properties:
+- compatible: "st,stm32mp1-rcc", "syscon"
+- reg: should be register base and length as documented in the datasheet
+- #clock-cells: 1, device nodes should specify the clock in their
+  "clocks" property, containing a phandle to the clock device node,
+  an index specifying the clock to use.
+- #reset-cells: Shall be 1
+- interrupts: Should contain a general interrupt line.
+- secure-interrupts: Should contain a interrupt line to the wake-up of
+                     processor (CSTOP).
+- secure-status: Relates to RCC TZ_ENABLE configuration to restrict RCC access.
+
+Example:
+	rcc: rcc@50000000 {
+		compatible = "st,stm32mp1-rcc", "syscon";
+		reg = <0x50000000 0x1000>;
+		#clock-cells = <1>;
+		#reset-cells = <1>;
+		interrupts = <GIC_SPI 5 IRQ_TYPE_LEVEL_HIGH>;
+		secure-interrupts = <GIC_SPI 145 IRQ_TYPE_LEVEL_HIGH>;
+	};
+
+Specifying clocks
+=================
+
+All available clocks are defined as preprocessor macros in
+dt-bindings/clock/stm32mp1-clks.h header and can be used in device
+tree sources.
+
+Specifying softreset control of devices
+=======================================
+
+Device nodes should specify the reset channel required in their "resets"
+property, containing a phandle to the reset device node and an index specifying
+which channel to use.
+The index is the bit number within the RCC registers bank, starting from RCC
+base address.
+It is calculated as: index = register_offset / 4 * 32 + bit_offset.
+Where bit_offset is the bit offset within the register.
+
+For example on STM32MP1, for LTDC reset:
+ ltdc = APB4_RSTSETR_offset / 4 * 32 + LTDC_bit_offset
+      = 0x180 / 4 * 32 + 0 = 3072
+
+The list of valid indices for STM32MP1 is available in:
+include/dt-bindings/reset-controller/stm32mp1-resets.h
+
+This file implements defines like:
+#define LTDC_R	3072
+
+
+Defining clock source distribution with property st,clksrc
+==========================================================
+
+- st,clksrc : The clock sources configuration array in a platform specific
+              order.
+
+  Property can be used to configure the clock distribution tree.
+  When used, it shall describe the whole distribution tree.
+
+  For the STM32MP15x family there are 9 clock sources selector which are
+  configured in the following order:
+	MPU AXI MCU PLL12 PLL3 PLL4 RTC MCO1 MCO2
+
+  Clock source configuration values are defined by macros CLK_<NAME>_<SOURCE>
+  from dt-bindings/clock/stm32mp1-clksrc.h.
+
+  Example:
+	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
+	>;
+
+Defining clock dividers with property st,clkdiv
+===============================================
+
+- st,clkdiv : The clock main dividers value specified in an array
+              in a platform specific order.
+
+  When used, it shall describe the whole clock dividers tree.
+
+  Property can be used to configure the clock main dividers value.
+  When used, it shall describe the whole clock dividers tree.
+
+  For the STM32MP15x family there are 11 dividers values expected.
+  They shall be configured in the following order:
+		MPU AXI MCU APB1 APB2 APB3 APB4 APB5 RTC MCO1 MCO2
+
+  The each divider value uses the DIV coding defined in RCC associated
+  register RCC_xxxDIVR. In most cases, it is:
+		0x0: not divided
+		0x1: division by 2
+		0x2: division by 4
+		0x3: division by 8
+		...
+
+  Note that for RTC MCO1 MCO2, the coding is different:
+		0x0: not divided
+		0x1: division by 2
+		0x2: division by 3
+		0x3: division by 4
+		...
+
+  Example:
+	st,clkdiv = <
+		1 /*MPU*/
+		0 /*AXI*/
+		0 /*MCU*/
+		1 /*APB1*/
+		1 /*APB2*/
+		1 /*APB3*/
+		1 /*APB4*/
+		2 /*APB5*/
+		23 /*RTC*/
+		0 /*MCO1*/
+		0 /*MCO2*/
+	>;
+
+Optional Properties:
+Defining peripherals kernel clock tree distribution with property st,pkcs
+=========================================================================
+
+- st,pkcs : used to configure the peripherals kernel clock selection.
+
+  The property is a list of peripheral kernel clock source identifiers defined
+  by macros CLK_<KERNEL-CLOCK>_<PARENT-CLOCK> as defined by header file
+  dt-bindings/clock/stm32mp1-clksrc.h.
+
+  st,pkcs may not list all the kernel clocks and has no ordering requirements.
+
+  Example:
+	st,pkcs = <
+		CLK_STGEN_HSE
+		CLK_CKPER_HSI
+		CLK_USBPHY_PLL2P
+		CLK_DSI_PLL2Q
+		CLK_I2C46_HSI
+		CLK_UART1_HSI
+		CLK_UART24_HSI
+	>;
+
+Defining peripheral PLL frequencies
+========================================================
+
+- children for a PLL static configuration with "st,stm32mp1-pll" compatible
+
+  Each PLL children nodes for PLL1 to PLL4 (see ref manual for details)
+  are listed with associated reg 0 to 3.
+
+  PLL2, PLL3 or PLL4 are off when their associated nodes are absent or
+  deactivated.
+
+  The configuration of PLL1, the source clock of Cortex-A7 core, with st,pll@0
+  node, is optional as TF-A automatically selects the most suitable operating
+  point for the platform.
+  The node st,pll@0 node should be absent; it is only used if you want to
+  override the PLL1 properties computed by TF-A (clock spreading for example).
+
+  Here are the available properties for each PLL node:
+    - compatible: should be "st,stm32mp1-pll"
+
+    - reg: index of the pll instance
+
+    - cfg: The parameters for PLL configuration in the following order:
+           DIVM DIVN DIVP DIVQ DIVR Output.
+
+	DIVx values are defined as in RCC spec:
+		0x0: bypass (division by 1)
+		0x1: division by 2
+		0x2: division by 3
+		0x3: division by 4
+		...
+
+	Output contains a bitfield for each output value (1:ON/0:OFF)
+		BIT(0) => output P : DIVPEN
+		BIT(1) => output Q : DIVQEN
+		BIT(2) => output R : DIVREN
+	  NB: macro PQR(p,q,r) can be used to build this value
+	      with p,q,r = 0 or 1.
+
+    - frac: Fractional part of the multiplication factor
+            (optional, PLL is in integer mode when absent).
+
+    - csg: Clock Spreading Generator (optional) with parameters in the
+	   following order: MOD_PER INC_STEP SSCG_MODE.
+
+	MOD_PER: Modulation Period Adjustment
+	INC_STEP: Modulation Depth Adjustment
+	SSCG_MODE: Spread spectrum clock generator mode, with associated
+		   defined from stm32mp1-clksrc.h:
+			- SSCG_MODE_CENTER_SPREAD = 0
+			- SSCG_MODE_DOWN_SPREAD = 1
+
+    Example:
+	st,pll@0 {
+		compatible = "st,stm32mp1-pll";
+		reg = <0>;
+		cfg = <1 53 0 0 0 1>;
+		frac = <0x810>;
+	};
+	st,pll@1 {
+		compatible = "st,stm32mp1-pll";
+		reg = <1>;
+		cfg = <1 43 1 0 0 PQR(0,1,1)>;
+		csg = <10 20 1>;
+	};
+	st,pll@2 {
+		compatible = "st,stm32mp1-pll";
+		reg = <2>;
+		cfg = <2 85 3 13 3 0>;
+		csg = <10 20 SSCG_MODE_CENTER_SPREAD>;
+	};
+	st,pll@3 {
+		compatible = "st,stm32mp1-pll";
+		reg = <3>;
+		cfg = <2 78 4 7 9 3>;
+	};
+
+Fixed clocks description
+========================
+
+The clock tree is also based on 5 fixed-clock in clocks node
+used to define the state of associated ST32MP1 oscillators:
+  - clk-lsi
+  - clk-lse
+  - clk-hsi
+  - clk-hse
+  - clk-csi
+
+At boot the clock tree initialization will
+  - enable oscillators present in device tree and not disabled
+    (node with status="disabled"),
+  - disable HSI oscillator if the node is absent (always activated by bootrom)
+    and not disabled (node with status="disabled").
+
+Optional properties :
+
+a) for external oscillator: "clk-lse", "clk-hse"
+
+  4 optional fields are managed
+  - "st,bypass" configures the oscillator bypass mode (HSEBYP, LSEBYP)
+  - "st,digbypass" configures the bypass mode as full-swing digital
+    signal (DIGBYP)
+  - "st,css" activates the clock security system (HSECSSON, LSECSSON)
+  - "st,drive" (only for LSE) contains the value of the drive for the
+     oscillator (see LSEDRV_ defined in the file
+     dt-bindings/clock/stm32mp1-clksrc.h)
+
+  Example board file:
+	/ {
+		clocks {
+			clk_hse: clk-hse {
+				#clock-cells = <0>;
+				compatible = "fixed-clock";
+				clock-frequency = <64000000>;
+				st,bypass;
+			};
+
+			clk_lse: clk-lse {
+				#clock-cells = <0>;
+				compatible = "fixed-clock";
+				clock-frequency = <32768>;
+				st,css;
+				st,drive = <LSEDRV_LOWEST>;
+			};
+	};
+
+b) for internal oscillator: "clk-hsi"
+
+  Internally HSI clock is fixed to 64MHz for STM32MP157 SoC.
+  In device tree, clk-hsi is the clock after HSIDIV (clk_hsi in RCC
+  doc). So this clock frequency is used to compute the expected HSI_DIV
+  for the clock tree initialization.
+
+  Example with HSIDIV = /1:
+	/ {
+		clocks {
+			clk_hsi: clk-hsi {
+				#clock-cells = <0>;
+				compatible = "fixed-clock";
+				clock-frequency = <64000000>;
+			};
+	};
+
+  Example with HSIDIV = /2
+	/ {
+		clocks {
+			clk_hsi: clk-hsi {
+				#clock-cells = <0>;
+				compatible = "fixed-clock";
+				clock-frequency = <32000000>;
+			};
+	};
+
+HSI & CSI calibration
+========================
+
+Calibration is an optional feature that may be enabled from device tree. It
+allows to request calibration of the HSI or the CSI clocks from several means:
+	- SiP SMC service
+	- Periodic calibration every X seconds
+	- Interrupt raised by the MCU
+
+This feature requires that a HW timer is assigned to the calibration sequence.
+
+Dedicated secure interrupt must be defined using "mcu_sev" name to start a
+calibration on detection of an interrupt raised by MCU.
+
+- st,hsi-cal: used to enable HSI clock calibration feature.
+
+- st,csi-cal; used to enable CSI clock calibration feature.
+
+- st,cal-sec: used to enable periodic calibration every specified seconds from
+  secure monitor. Time must be given in seconds. If not specified, calibration
+  is processed for each incoming request.
+
+Example:
+	&rcc {
+		st,hsi-cal;
+		st,csi-cal;
+		st,cal-sec = <15>;
+		secure-interrupts = <GIC_SPI 144 IRQ_TYPE_LEVEL_HIGH>,
+				<GIC_SPI 145 IRQ_TYPE_LEVEL_HIGH>;
+		secure-interrupt-names = "mcu_sev", "wakeup";
+	};
+
+
+Example of clock tree initialization
+====================================
+
+/ {
+	clocks {
+		clk_hse: clk-hse {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <24000000>;
+			st,digbypass;
+		};
+
+		clk_hsi: clk-hsi {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <64000000>;
+		};
+
+		clk_lse: clk-lse {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <32768>;
+		};
+
+		clk_lsi: clk-lsi {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <32000>;
+		};
+
+		clk_csi: clk-csi {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <4000000>;
+		};
+	};
+
+	soc {
+
+		rcc: rcc@50000000 {
+			compatible = "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";
+			secure-status = "okay";
+
+			st,clksrc = <
+				CLK_MPU_PLL1P
+				CLK_AXI_PLL2P
+				CLK_MCU_PLL3P
+				CLK_PLL12_HSE
+				CLK_PLL3_HSE
+				CLK_PLL4_HSE
+				CLK_RTC_LSE
+				CLK_MCO1_DISABLED
+				CLK_MCO2_DISABLED
+			>;
+
+			st,clkdiv = <
+				1 /*MPU*/
+				0 /*AXI*/
+				0 /*MCU*/
+				1 /*APB1*/
+				1 /*APB2*/
+				1 /*APB3*/
+				1 /*APB4*/
+				2 /*APB5*/
+				23 /*RTC*/
+				0 /*MCO1*/
+				0 /*MCO2*/
+			>;
+
+			st,pkcs = <
+				CLK_CKPER_HSE
+				CLK_FMC_ACLK
+				CLK_QSPI_ACLK
+				CLK_ETH_DISABLED
+				CLK_SDMMC12_PLL4P
+				CLK_DSI_DSIPLL
+				CLK_STGEN_HSE
+				CLK_USBPHY_HSE
+				CLK_SPI2S1_PLL3Q
+				CLK_SPI2S23_PLL3Q
+				CLK_SPI45_HSI
+				CLK_SPI6_HSI
+				CLK_I2C46_HSI
+				CLK_SDMMC3_PLL4P
+				CLK_USBO_USBPHY
+				CLK_ADC_CKPER
+				CLK_CEC_LSE
+				CLK_I2C12_HSI
+				CLK_I2C35_HSI
+				CLK_UART1_HSI
+				CLK_UART24_HSI
+				CLK_UART35_HSI
+				CLK_UART6_HSI
+				CLK_UART78_HSI
+				CLK_SPDIF_PLL4P
+				CLK_FDCAN_PLL4Q
+				CLK_SAI1_PLL3Q
+				CLK_SAI2_PLL3Q
+				CLK_SAI3_PLL3Q
+				CLK_SAI4_PLL3Q
+				CLK_RNG1_LSI
+				CLK_RNG2_LSI
+				CLK_LPTIM1_PCLK1
+				CLK_LPTIM23_PCLK3
+				CLK_LPTIM45_LSE
+			>;
+
+			/* VCO = 1300.0 MHz => P = 650 (CPU) */
+			pll1: st,pll@0 {
+				compatible = "st,stm32mp1-pll";
+				reg = <0>;
+				cfg = <2 80 0 0 0 PQR(1,0,0)>;
+				frac = <0x800>;
+			};
+
+			/* 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)>;
+			};
+		};
+	};
+};
diff --git a/docs/devicetree/bindings/i2c/i2c-stm32.txt b/docs/devicetree/bindings/i2c/i2c-stm32.txt
new file mode 100644
index 0000000000..68aefa6dc4
--- /dev/null
+++ b/docs/devicetree/bindings/i2c/i2c-stm32.txt
@@ -0,0 +1,54 @@
+* I2C controller embedded in STMicroelectronics STM32 I2C platform
+
+Required properties :
+- compatible : Must be one of the following
+  - "st,stm32f7-i2c"
+- reg : Offset and length of the register set for the device
+- resets: Must contain the phandle to the reset controller.
+- clocks: Must contain the input clock of the I2C instance.
+- A pinctrl state named "default" must be defined to set pins in mode of
+  operation for I2C transfer. An optional pinctrl state named "sleep" has to
+  be defined as well as to put I2C in low power mode in suspend mode.
+- #address-cells = <1>;
+- #size-cells = <0>;
+
+Optional properties :
+- clock-frequency : Desired I2C bus clock frequency in Hz. If not specified,
+  the default 100 kHz frequency will be used.
+- i2c-scl-rising-time-ns : Only for STM32F7, I2C SCL Rising time for the board
+  (default: 25)
+- i2c-scl-falling-time-ns : Only for STM32F7, I2C SCL Falling time for the board
+  (default: 10)
+  I2C Timings are derived from these 2 values
+- st,syscfg-fmp:  Only for STM32F7, use to set Fast Mode Plus bit within SYSCFG
+  whether Fast Mode Plus speed is selected by slave.
+	1st cell : phandle to syscfg
+	2nd cell : register offset within SYSCFG
+	3rd cell : register bitmask for FMP bit
+
+Example :
+
+	i2c@40005400 {
+		compatible = "st,stm32f4-i2c";
+		#address-cells = <1>;
+		#size-cells = <0>;
+		reg = <0x40005400 0x400>;
+		resets = <&rcc 277>;
+		clocks = <&rcc 0 149>;
+		pinctrl-0 = <&i2c1_sda_pin>, <&i2c1_scl_pin>;
+		pinctrl-names = "default";
+	};
+
+	i2c@40005400 {
+		compatible = "st,stm32f7-i2c";
+		#address-cells = <1>;
+		#size-cells = <0>;
+		reg = <0x40005400 0x400>;
+		resets = <&rcc STM32F7_APB1_RESET(I2C1)>;
+		clocks = <&rcc 1 CLK_I2C1>;
+		pinctrl-0 = <&i2c1_sda_pin>, <&i2c1_scl_pin>;
+		pinctrl-1 =  <&i2c1_sda_pin_sleep>, <&i2c1_scl_pin_sleep>;
+		pinctrl-names = "default", "sleep";
+		st,syscfg-fmp = <&syscfg 0x4 0x1>;
+	};
+
diff --git a/docs/devicetree/bindings/memory-controllers/st,stm32mp1-ddr.txt b/docs/devicetree/bindings/memory-controllers/st,stm32mp1-ddr.txt
new file mode 100644
index 0000000000..ac6a7df432
--- /dev/null
+++ b/docs/devicetree/bindings/memory-controllers/st,stm32mp1-ddr.txt
@@ -0,0 +1,301 @@
+ST,stm32mp1 DDR3/LPDDR2/LPDDR3 Controller (DDRCTRL and DDRPHYC)
+
+--------------------
+Required properties:
+--------------------
+- compatible	: Should be "st,stm32mp1-ddr"
+- reg		: controleur (DDRCTRL) and phy (DDRPHYC) base address
+- clocks	: controller clocks handle
+- clock-names	: associated controller clock names
+		  the "ddrphyc" clock is used to check the DDR frequency
+		  at phy level according the expected value in "mem-speed" field
+
+the next attributes are DDR parameters, they are generated by DDR tools
+included in STM32 Cube tool
+
+info attributes:
+----------------
+- st,mem-name	: name for DDR configuration, simple string for information
+- st,mem-speed	: DDR expected speed for the setting in kHz
+- st,mem-size	: DDR mem size in byte
+
+
+controlleur attributes:
+-----------------------
+- st,ctl-reg	: controleur values depending of the DDR type
+		  (DDR3/LPDDR2/LPDDR3)
+	for STM32MP15x: 25 values are requested in this order
+		MSTR
+		MRCTRL0
+		MRCTRL1
+		DERATEEN
+		DERATEINT
+		PWRCTL
+		PWRTMG
+		HWLPCTL
+		RFSHCTL0
+		RFSHCTL3
+		CRCPARCTL0
+		ZQCTL0
+		DFITMG0
+		DFITMG1
+		DFILPCFG0
+		DFIUPD0
+		DFIUPD1
+		DFIUPD2
+		DFIPHYMSTR
+		ODTMAP
+		DBG0
+		DBG1
+		DBGCMD
+		POISONCFG
+		PCCFG
+
+- st,ctl-timing	: controleur values depending of frequency and timing parameter
+		  of DDR
+	for STM32MP15x: 12 values are requested in this order
+		RFSHTMG
+		DRAMTMG0
+		DRAMTMG1
+		DRAMTMG2
+		DRAMTMG3
+		DRAMTMG4
+		DRAMTMG5
+		DRAMTMG6
+		DRAMTMG7
+		DRAMTMG8
+		DRAMTMG14
+		ODTCFG
+
+- st,ctl-map	: controleur values depending of address mapping
+	for STM32MP15x: 9 values are requested in this order
+		ADDRMAP1
+		ADDRMAP2
+		ADDRMAP3
+		ADDRMAP4
+		ADDRMAP5
+		ADDRMAP6
+		ADDRMAP9
+		ADDRMAP10
+		ADDRMAP11
+
+- st,ctl-perf	: controleur values depending of performance and scheduling
+	for STM32MP15x: 17 values are requested in this order
+		SCHED
+		SCHED1
+		PERFHPR1
+		PERFLPR1
+		PERFWR1
+		PCFGR_0
+		PCFGW_0
+		PCFGQOS0_0
+		PCFGQOS1_0
+		PCFGWQOS0_0
+		PCFGWQOS1_0
+		PCFGR_1
+		PCFGW_1
+		PCFGQOS0_1
+		PCFGQOS1_1
+		PCFGWQOS0_1
+		PCFGWQOS1_1
+
+phyc attributes:
+----------------
+- st,phy-reg	: phy values depending of the DDR type (DDR3/LPDDR2/LPDDR3)
+	for STM32MP15x: 11 values are requested in this order
+		PGCR
+		ACIOCR
+		DXCCR
+		DSGCR
+		DCR
+		ODTCR
+		ZQ0CR1
+		DX0GCR
+		DX1GCR
+		DX2GCR
+		DX3GCR
+
+- st,phy-timing	: phy values depending of frequency and timing parameter of DDR
+	for STM32MP15x: 10 values are requested in this order
+		PTR0
+		PTR1
+		PTR2
+		DTPR0
+		DTPR1
+		DTPR2
+		MR0
+		MR1
+		MR2
+		MR3
+
+- st,phy-cal	: phy cal depending of calibration or tuning of DDR
+	This parameter is optional; when it is absent the built-in PHY
+	calibration is done.
+	for STM32MP15x: 12 values are requested in this order
+		DX0DLLCR
+		DX0DQTR
+		DX0DQSTR
+		DX1DLLCR
+		DX1DQTR
+		DX1DQSTR
+		DX2DLLCR
+		DX2DQTR
+		DX2DQSTR
+		DX3DLLCR
+		DX3DQTR
+		DX3DQSTR
+
+Example:
+
+/ {
+	soc {
+		u-boot,dm-spl;
+
+		ddr: ddr@0x5A003000{
+			u-boot,dm-spl;
+			u-boot,dm-pre-reloc;
+
+			compatible = "st,stm32mp1-ddr";
+
+			reg = <0x5A003000 0x550
+			       0x5A004000 0x234>;
+
+			clocks = <&rcc_clk AXIDCG>,
+				 <&rcc_clk DDRC1>,
+				 <&rcc_clk DDRC2>,
+				 <&rcc_clk DDRPHYC>,
+				 <&rcc_clk DDRCAPB>,
+				 <&rcc_clk DDRPHYCAPB>;
+
+			clock-names = "axidcg",
+				      "ddrc1",
+				      "ddrc2",
+				      "ddrphyc",
+				      "ddrcapb",
+				      "ddrphycapb";
+
+			st,mem-name = "DDR3 2x4Gb 533MHz";
+			st,mem-speed = <533000>;
+			st,mem-size = <0x40000000>;
+
+			st,ctl-reg = <
+				0x00040401 /*MSTR*/
+				0x00000010 /*MRCTRL0*/
+				0x00000000 /*MRCTRL1*/
+				0x00000000 /*DERATEEN*/
+				0x00800000 /*DERATEINT*/
+				0x00000000 /*PWRCTL*/
+				0x00400010 /*PWRTMG*/
+				0x00000000 /*HWLPCTL*/
+				0x00210000 /*RFSHCTL0*/
+				0x00000000 /*RFSHCTL3*/
+				0x00000000 /*CRCPARCTL0*/
+				0xC2000040 /*ZQCTL0*/
+				0x02050105 /*DFITMG0*/
+				0x00000202 /*DFITMG1*/
+				0x07000000 /*DFILPCFG0*/
+				0xC0400003 /*DFIUPD0*/
+				0x00000000 /*DFIUPD1*/
+				0x00000000 /*DFIUPD2*/
+				0x00000000 /*DFIPHYMSTR*/
+				0x00000001 /*ODTMAP*/
+				0x00000000 /*DBG0*/
+				0x00000000 /*DBG1*/
+				0x00000000 /*DBGCMD*/
+				0x00000000 /*POISONCFG*/
+				0x00000010 /*PCCFG*/
+			>;
+
+			st,ctl-timing = <
+				0x0080008A /*RFSHTMG*/
+				0x121B2414 /*DRAMTMG0*/
+				0x000D041B /*DRAMTMG1*/
+				0x0607080E /*DRAMTMG2*/
+				0x0050400C /*DRAMTMG3*/
+				0x07040407 /*DRAMTMG4*/
+				0x06060303 /*DRAMTMG5*/
+				0x02020002 /*DRAMTMG6*/
+				0x00000202 /*DRAMTMG7*/
+				0x00001005 /*DRAMTMG8*/
+				0x000D041B /*DRAMTMG1*/4
+				0x06000600 /*ODTCFG*/
+			>;
+
+			st,ctl-map = <
+				0x00080808 /*ADDRMAP1*/
+				0x00000000 /*ADDRMAP2*/
+				0x00000000 /*ADDRMAP3*/
+				0x00001F1F /*ADDRMAP4*/
+				0x07070707 /*ADDRMAP5*/
+				0x0F070707 /*ADDRMAP6*/
+				0x00000000 /*ADDRMAP9*/
+				0x00000000 /*ADDRMAP10*/
+				0x00000000 /*ADDRMAP11*/
+			>;
+
+			st,ctl-perf = <
+				0x00001201 /*SCHED*/
+				0x00001201 /*SCHED*/1
+				0x01000001 /*PERFHPR1*/
+				0x08000200 /*PERFLPR1*/
+				0x08000400 /*PERFWR1*/
+				0x00010000 /*PCFGR_0*/
+				0x00000000 /*PCFGW_0*/
+				0x02100B03 /*PCFGQOS0_0*/
+				0x00800100 /*PCFGQOS1_0*/
+				0x01100B03 /*PCFGWQOS0_0*/
+				0x01000200 /*PCFGWQOS1_0*/
+				0x00010000 /*PCFGR_1*/
+				0x00000000 /*PCFGW_1*/
+				0x02100B03 /*PCFGQOS0_1*/
+				0x00800000 /*PCFGQOS1_1*/
+				0x01100B03 /*PCFGWQOS0_1*/
+				0x01000200 /*PCFGWQOS1_1*/
+			>;
+
+			st,phy-reg = <
+				0x01442E02 /*PGCR*/
+				0x10400812 /*ACIOCR*/
+				0x00000C40 /*DXCCR*/
+				0xF200001F /*DSGCR*/
+				0x0000000B /*DCR*/
+				0x00010000 /*ODTCR*/
+				0x0000007B /*ZQ0CR1*/
+				0x0000CE81 /*DX0GCR*/
+				0x0000CE81 /*DX1GCR*/
+				0x0000CE81 /*DX2GCR*/
+				0x0000CE81 /*DX3GCR*/
+			>;
+
+			st,phy-timing = <
+				0x0022A41B /*PTR0*/
+				0x047C0740 /*PTR1*/
+				0x042D9C80 /*PTR2*/
+				0x369477D0 /*DTPR0*/
+				0x098A00D8 /*DTPR1*/
+				0x10023600 /*DTPR2*/
+				0x00000830 /*MR0*/
+				0x00000000 /*MR1*/
+				0x00000208 /*MR2*/
+				0x00000000 /*MR3*/
+			>;
+
+			st,phy-cal = <
+				0x40000000 /*DX0DLLCR*/
+				0xFFFFFFFF /*DX0DQTR*/
+				0x3DB02000 /*DX0DQSTR*/
+				0x40000000 /*DX1DLLCR*/
+				0xFFFFFFFF /*DX1DQTR*/
+				0x3DB02000 /*DX1DQSTR*/
+				0x40000000 /*DX2DLLCR*/
+				0xFFFFFFFF /*DX2DQTR*/
+				0x3DB02000 /*DX2DQSTR*/
+				0x40000000 /*DX3DLLCR*/
+				0xFFFFFFFF /*DX3DQTR*/
+				0x3DB02000 /*DX3DQSTR*/
+			>;
+
+			status = "okay";
+		};
+	};
+};
diff --git a/docs/devicetree/bindings/mmc/mmci.txt b/docs/devicetree/bindings/mmc/mmci.txt
new file mode 100644
index 0000000000..6d3c626e01
--- /dev/null
+++ b/docs/devicetree/bindings/mmc/mmci.txt
@@ -0,0 +1,72 @@
+* ARM PrimeCell MultiMedia Card Interface (MMCI) PL180/1
+
+The ARM PrimeCell MMCI PL180 and PL181 provides an interface for
+reading and writing to MultiMedia and SD cards alike.
+
+This file documents differences between the core properties described
+by mmc.txt and the properties used by the mmci driver. Using "st" as
+the prefix for a property, indicates support by the ST Micro variant.
+
+Required properties:
+- compatible             : contains "arm,pl18x", "arm,primecell".
+- vmmc-supply            : phandle to the regulator device tree node, mentioned
+                           as the VCC/VDD supply in the eMMC/SD specs.
+
+Optional properties:
+- arm,primecell-periphid : contains the PrimeCell Peripheral ID, it overrides
+                           the ID provided by the HW
+- resets                 : phandle to internal reset line.
+			   Should be defined for sdmmc variant.
+- vqmmc-supply           : phandle to the regulator device tree node, mentioned
+                           as the VCCQ/VDD_IO supply in the eMMC/SD specs.
+specific for ux500 variant:
+- st,sig-dir-dat0        : bus signal direction pin used for DAT[0].
+- st,sig-dir-dat2        : bus signal direction pin used for DAT[2].
+- st,sig-dir-dat31       : bus signal direction pin used for DAT[3] and DAT[1].
+- st,sig-dir-dat74       : bus signal direction pin used for DAT[4] to DAT[7].
+- st,sig-dir-cmd         : cmd signal direction pin used for CMD.
+- st,sig-pin-fbclk       : feedback clock signal pin used.
+
+specific for sdmmc variant:
+- st,sig-dir             : signal direction polarity used for cmd, dat0 dat123.
+- st,neg-edge            : data & command phase relation, generated on
+                           sd clock falling edge.
+- st,use-ckin            : use ckin pin from an external driver to sample
+                           the receive data (example: with voltage
+			   switch transceiver).
+
+Deprecated properties:
+- mmc-cap-mmc-highspeed  : indicates whether MMC is high speed capable.
+- mmc-cap-sd-highspeed   : indicates whether SD is high speed capable.
+
+Example:
+
+sdi0_per1@80126000 {
+	compatible = "arm,pl18x", "arm,primecell";
+	reg = <0x80126000 0x1000>;
+	interrupts = <0 60 IRQ_TYPE_LEVEL_HIGH>;
+
+	dmas = <&dma 29 0 0x2>, /* Logical - DevToMem */
+	       <&dma 29 0 0x0>; /* Logical - MemToDev */
+	dma-names = "rx", "tx";
+
+	clocks = <&prcc_kclk 1 5>, <&prcc_pclk 1 5>;
+	clock-names = "sdi", "apb_pclk";
+
+	max-frequency = <100000000>;
+	bus-width = <4>;
+	cap-sd-highspeed;
+	cap-mmc-highspeed;
+	cd-gpios  = <&gpio2 31 0x4>; // 95
+	st,sig-dir-dat0;
+	st,sig-dir-dat2;
+	st,sig-dir-cmd;
+	st,sig-pin-fbclk;
+
+	vmmc-supply = <&ab8500_ldo_aux3_reg>;
+	vqmmc-supply = <&vmmci>;
+
+	pinctrl-names = "default", "sleep";
+	pinctrl-0 = <&sdi0_default_mode>;
+	pinctrl-1 = <&sdi0_sleep_mode>;
+};
diff --git a/docs/devicetree/bindings/mmc/st,stm32-sdmmc2.txt b/docs/devicetree/bindings/mmc/st,stm32-sdmmc2.txt
new file mode 100644
index 0000000000..51576a3844
--- /dev/null
+++ b/docs/devicetree/bindings/mmc/st,stm32-sdmmc2.txt
@@ -0,0 +1,22 @@
+* STMicroelectronics STM32 SDMMC2 controller
+
+The highspeed MMC host controller on STM32 soc family
+provides an interface for MMC, SD and SDIO types of memory cards.
+
+This file documents differences between the core properties described
+by mmci.txt and the properties used by the sdmmc2 driver.
+
+Required properties:
+ - compatible: should be one of:
+   "st,stm32-sdmmc2"
+
+Example:
+	sdmmc1: sdmmc@0x58005000 {
+		compatible = "st,stm32-sdmmc2";
+		reg = <0x58005000 0x1000>;
+		clocks = <&rcc SDMMC1_K>;
+		resets = <&rcc SDMMC1_R>;
+		cap-sd-highspeed;
+		cap-mmc-highspeed;
+		status = "disabled";
+	};
diff --git a/docs/devicetree/bindings/power/st,stm32mp1-pwr.txt b/docs/devicetree/bindings/power/st,stm32mp1-pwr.txt
new file mode 100644
index 0000000000..22779b05af
--- /dev/null
+++ b/docs/devicetree/bindings/power/st,stm32mp1-pwr.txt
@@ -0,0 +1,43 @@
+STMicroelectronics STM32MP1 Power Management Controller
+=======================================================
+
+The PWR IP is responsible for handling the power related resources such as
+clocks, power supplies and resets. It provides 6 wake-up pins that are handled
+by an interrupt-controller. Wake-up pin can be used to wake-up from STANDBY SoC
+state.
+
+Required properties:
+- compatible should be: "st,stm32mp1-pwr", "st,stm32-pwr"
+- reg: should be register base and length as documented in the
+  datasheet
+
+Optional Properties:
+- Nodes corresponding to PSCI commands issued by kernel:
+  - system_suspend_supported_soc_modes: list of supported SoC modes in suspend
+  - system_off_soc_mode: SoC mode for shutdown
+  - st,retram-enabled-in-standby-ddr-sr: enable retram during standby-ddr-sr
+
+The list of SoC modes is in include/dt-bindings/power/stm32mp1-power.h:
+  - modes for system_suspend
+	1 -> STM32_PM_CSTOP_ALLOW_STOP
+	2 -> STM32_PM_CSTOP_ALLOW_LP_STOP
+	3 -> STM32_PM_CSTOP_ALLOW_LPLV_STOP
+	4 -> STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR
+  - modes for system_off
+	6 -> STM32_PM_CSTOP_ALLOW_STANDBY_DDR_OFF
+	7 -> STM32_PM_SHUTDOWN
+
+Example:
+
+pwr: pwr@50001000 {
+	compatible = "st,stm32mp1-pwr", "st,stm32-pwr", "syscon", "simple-mfd";
+	reg = <0x50001000 0x400>;
+
+	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>;
+};
diff --git a/docs/devicetree/bindings/power/st,stpmic1.txt b/docs/devicetree/bindings/power/st,stpmic1.txt
new file mode 100644
index 0000000000..83307d23b4
--- /dev/null
+++ b/docs/devicetree/bindings/power/st,stpmic1.txt
@@ -0,0 +1,94 @@
+* STMicroelectronics STPMIC1 Power Management IC
+
+Required parent device properties:
+- compatible:			"st,stpmic1"
+- reg:				The I2C slave address for the STPMIC1 chip.
+- interrupts:			The interrupt lines the device is connected to.
+				The second interrupt is used for wake-up.
+- #interrupt-cells:		Should be 2.
+- interrupt-controller: 	Describes the STPMIC1 as an interrupt
+  controller (has its own domain). Interrupt number are the following:
+	/* Interrupt Register 1 (0x50 for latch) */
+	IT_SWOUT_R=0
+	IT_SWOUT_F=1
+	IT_VBUS_OTG_R=2
+	IT_VBUS_OTG_F=3
+	IT_WAKEUP_R=4
+	IT_WAKEUP_F=5
+	IT_PONKEY_R=6
+	IT_PONKEY_F=7
+	/* Interrupt Register 2 (0x51 for latch) */
+	IT_OVP_BOOST=8
+	IT_OCP_BOOST=9
+	IT_OCP_SWOUT=10
+	IT_OCP_OTG=11
+	IT_CURLIM_BUCK4=12
+	IT_CURLIM_BUCK3=13
+	IT_CURLIM_BUCK2=14
+	IT_CURLIM_BUCK1=15
+	/* Interrupt Register 3 (0x52 for latch) */
+	IT_SHORT_SWOUT=16
+	IT_SHORT_SWOTG=17
+	IT_CURLIM_LDO6=18
+	IT_CURLIM_LDO5=19
+	IT_CURLIM_LDO4=20
+	IT_CURLIM_LDO3=21
+	IT_CURLIM_LDO2=22
+	IT_CURLIM_LDO1=23
+	/* Interrupt Register 3 (0x52 for latch) */
+	IT_SWIN_R=24
+	IT_SWIN_F=25
+	IT_RESERVED_1=26
+	IT_RESERVED_2=27
+	IT_VINLOW_R=28
+	IT_VINLOW_F=29
+	IT_TWARN_R=30
+	IT_TWARN_F=31
+
+STPMIC1 consists in a varied group of sub-devices.
+Each sub-device binding is be described in own documentation file.
+
+Device			 Description
+------			------------
+st,stpmic1-onkey	: Power on key, see ../input/st,stpmic1-onkey.txt
+st,stpmic1-regulators	: Regulators, see ../regulator/st,stpmic1-regulator.txt
+st,stpmic1-wdt		: Watchdog, see ../watchdog/st,stpmic1-wdt.txt
+
+Example:
+
+pmic: pmic@33 {
+	compatible = "st,stpmic1";
+	reg = <0x33>;
+	interrupt-parent = <&gpioa>;
+	interrupts = <0 2>;
+	interrupt-controller;
+	#interrupt-cells = <2>;
+
+	onkey {
+		compatible = "st,stpmic1-onkey";
+		interrupts = <IT_PONKEY_F 0>,<IT_PONKEY_R 1>;
+		interrupt-names = "onkey-falling", "onkey-rising";
+		power-off-time-sec = <10>;
+	};
+
+	watchdog {
+		compatible = "st,stpmic1-wdt";
+	};
+
+	regulators {
+		compatible = "st,stpmic1-regulators";
+
+		vdd_core: buck1 {
+			regulator-name = "vdd_core";
+			regulator-boot-on;
+			regulator-min-microvolt = <700000>;
+			regulator-max-microvolt = <1200000>;
+		};
+		vdd: buck3 {
+			regulator-name = "vdd";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+			regulator-boot-on;
+			regulator-pull-down;
+		};
+	};
diff --git a/docs/devicetree/bindings/reset/st,stm32mp1-rcc.txt b/docs/devicetree/bindings/reset/st,stm32mp1-rcc.txt
new file mode 100644
index 0000000000..b4edaf7c7f
--- /dev/null
+++ b/docs/devicetree/bindings/reset/st,stm32mp1-rcc.txt
@@ -0,0 +1,6 @@
+STMicroelectronics STM32MP1 Peripheral Reset Controller
+=======================================================
+
+The RCC IP is both a reset and a clock controller.
+
+Please see Documentation/devicetree/bindings/clock/st,stm32mp1-rcc.txt
diff --git a/docs/devicetree/bindings/rng/st,stm32-rng.txt b/docs/devicetree/bindings/rng/st,stm32-rng.txt
new file mode 100644
index 0000000000..3c613d7914
--- /dev/null
+++ b/docs/devicetree/bindings/rng/st,stm32-rng.txt
@@ -0,0 +1,23 @@
+STMicroelectronics STM32 HW RNG
+===============================
+
+The STM32 hardware random number generator is a simple fixed purpose IP and
+is fully separated from other crypto functions.
+
+Required properties:
+
+- compatible : Should be "st,stm32-rng"
+- reg : Should be register base and length as documented in the datasheet
+- clocks : The clock needed to enable the RNG
+
+Optional properties:
+- resets : The reset to properly start RNG
+- clock-error-detect : Enable the clock detection management
+
+Example:
+
+	rng: rng@50060800 {
+		compatible = "st,stm32-rng";
+		reg = <0x50060800 0x400>;
+		clocks = <&rcc 0 38>;
+	};
diff --git a/docs/devicetree/bindings/serial/st,stm32-usart.txt b/docs/devicetree/bindings/serial/st,stm32-usart.txt
new file mode 100644
index 0000000000..08b499045a
--- /dev/null
+++ b/docs/devicetree/bindings/serial/st,stm32-usart.txt
@@ -0,0 +1,88 @@
+* STMicroelectronics STM32 USART
+
+Required properties:
+- compatible: can be either:
+  - "st,stm32-uart",
+  - "st,stm32f7-uart",
+  - "st,stm32h7-uart".
+  depending is compatible with stm32(f4), stm32f7 or stm32h7.
+- reg: The address and length of the peripheral registers space
+- interrupts:
+  - The interrupt line for the USART instance,
+  - An optional wake-up interrupt.
+- interrupt-names: Contains "event" for the USART interrupt line.
+- clocks: The input clock of the USART instance
+
+Optional properties:
+- resets: Must contain the phandle to the reset controller.
+- pinctrl-names: Set to "default". An additional "sleep" state can be defined
+  to set pins in sleep state when in low power. In case the device is used as
+  a wakeup source, "idle" state is defined in order to keep RX pin active.
+  For a console device, an optional state "no_console_suspend" can be defined
+  to enable console messages during suspend. Typically, "no_console_suspend" and
+  "default" states can refer to the same pin configuration.
+- pinctrl-n: Phandle(s) pointing to pin configuration nodes.
+  For Pinctrl properties see ../pinctrl/pinctrl-bindings.txt
+- st,hw-flow-ctrl: bool flag to enable hardware flow control.
+- rs485-rts-delay, rs485-rx-during-tx, rs485-rts-active-low,
+  linux,rs485-enabled-at-boot-time: see rs485.txt.
+- dmas: phandle(s) to DMA controller node(s). Refer to stm32-dma.txt
+- dma-names: "rx" and/or "tx"
+- wakeup-source: bool flag to indicate this device has wakeup capabilities
+- interrupt-names : Should contain "wakeup" if optional wake-up interrupt is
+  used.
+
+Note for dma using:
+- "tx" dma can be used without any constraint since it uses single
+dma transfers.
+- "rx" dma using requires some attention:
+   1) if you cannot anticipate the length of your received packets
+   and if your usart device embeds an internal fifo, then DON'T use
+   dma mode.
+   2) if you enable dma mode WITHOUT mdma intermediate copy (cf.
+   stm32-dma.txt), then the availability of the received data will
+   depend on the dma driver policy and it may be delayed until dma
+   internal fifo is full. The usart driver will see this checking
+   the dma residue when rx interrupt (RXNE or RTO) occurs.
+   3) if you enable dma mode WITH mdma intermediate copy (cf.
+   stm32-dma.txt) then the usart driver will never see the dma
+   residue becoming smaller than RX_BUF_P but it will get its
+   rx dma complete callback called when the cyclic transfer period
+   (RX_BUF_P) is reached.
+The three possibilities above are ordered from the most cpu time
+consuming one to the least one. The counterpart of this optimisation
+is the reception granularity achievable by the usart driver, from
+one byte up to RX_BUF_P.
+
+Examples:
+usart4: serial@40004c00 {
+	compatible = "st,stm32-uart";
+	reg = <0x40004c00 0x400>;
+	interrupts = <52>;
+	clocks = <&clk_pclk1>;
+	pinctrl-names = "default", "sleep", "idle", "no_console_suspend";
+	pinctrl-0 = <&pinctrl_usart4>;
+	pinctrl-1 = <&pinctrl_usart4_sleep>;
+	pinctrl-2 = <&pinctrl_usart4_idle>;
+	pinctrl-3 = <&pinctrl_usart4>;
+};
+
+usart2: serial@40004400 {
+	compatible = "st,stm32-uart";
+	reg = <0x40004400 0x400>;
+	interrupts = <38>;
+	clocks = <&clk_pclk1>;
+	st,hw-flow-ctrl;
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_usart2 &pinctrl_usart2_rtscts>;
+};
+
+usart1: serial@40011000 {
+	compatible = "st,stm32-uart";
+	reg = <0x40011000 0x400>;
+	interrupts = <37>;
+	clocks = <&rcc 0 164>;
+	dmas = <&dma2 2 4 0x414 0x0>,
+	       <&dma2 7 4 0x414 0x0>;
+	dma-names = "rx", "tx";
+};
diff --git a/docs/devicetree/bindings/soc/st,stm32-etzpc.txt b/docs/devicetree/bindings/soc/st,stm32-etzpc.txt
new file mode 100644
index 0000000000..a2ac263cec
--- /dev/null
+++ b/docs/devicetree/bindings/soc/st,stm32-etzpc.txt
@@ -0,0 +1,56 @@
+STM32 ETZPC
+---------------------------------
+
+Required properties:
+- compatible: should be "st,stm32-etzpc"
+- reg: physical base address and length of the registers set for the device
+- clocks: reference to the clock entry
+
+Optional property:
+- st,decprot: Configure option to properly set firewall for IPs.
+
+Examples:
+etzpc: etzpc@5C007000 {
+			compatible = "st,stm32-etzpc";
+			reg = <0x5C007000 0x400>;
+			clocks = <&rcc TZPC>;
+			status = "disabled";
+			secure-status = "okay";
+		};
+
+Firewall specifications
+=======================
+
+DECPROT macro must be used to properly configure IP firewalling. It must
+specify ID, domain and locking register status.
+
+The macro is defined in the binding header file [1].
+
+Example:
+	... {
+	st,decprot = <
+	DECPROT(STM32MP1_ETZPC_I2C4_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_GPIOZ_ID, DECPROT_NS_RW,	DECPROT_UNLOCK)>;
+	};
+
+Specify Peripheral IDs
+=======================
+
+Each peripheral is identified with a specific ID. Each platform defines the
+identifiers relevant to that platform. Peripheral IDs are defined in [1].
+
+Specify domain
+==============
+Firewall controls peripherals in specific domains:
+
+DECPROT_S_RW		0x0  -> Read/write Secure
+DECPROT_NS_R_S_W	0x1  -> Non secure read / Read/write Secure
+DECPROT_MCU_ISOLATION	0x2  -> MCU access only
+DECPROT_NS_RW		0x3  -> Non secure read/write
+
+
+[1] include/dt-bindings/soc/st,stm32-etzpc.h
+
diff --git a/docs/devicetree/bindings/soc/st,stm32-romem.txt b/docs/devicetree/bindings/soc/st,stm32-romem.txt
new file mode 100644
index 0000000000..c430fb84d7
--- /dev/null
+++ b/docs/devicetree/bindings/soc/st,stm32-romem.txt
@@ -0,0 +1,74 @@
+STMicroelectronics STM32 Factory-programmed data device tree bindings
+
+This represents STM32 Factory-programmed read only non-volatile area: locked
+flash, OTP, read-only HW regs... This contains various information such as:
+analog calibration data for temperature sensor (e.g. TS_CAL1, TS_CAL2),
+internal vref (VREFIN_CAL), unique device ID...
+
+Required properties:
+- compatible:		Should be one of:
+			"st,stm32-romem"
+			"st,stm32mp15-bsec"
+- reg:			Offset and length of factory-programmed area.
+- #address-cells:	Should be '<1>'.
+- #size-cells:		Should be '<1>'.
+
+Optional Data cells:
+- Must be child nodes as described in nvmem.txt.
+
+Optional-properties:
+- "st,non-secure-otp" specifies that the OTP can be accessed by non-secure
+  world through secure world services. Only useful for upper OTPs. This
+  property mandates 32-bit granularity of the related nvmem area, that is
+  offset and length are both multiple of 4.
+
+Example on stm32f4:
+	romem: nvmem@1fff7800 {
+		compatible = "st,stm32-romem";
+		reg = <0x1fff7800 0x400>;
+		#address-cells = <1>;
+		#size-cells = <1>;
+
+		/* Data cells: ts_cal1 at 0x1fff7a2c */
+		ts_cal1: calib@22c {
+			reg = <0x22c 0x2>;
+		};
+		...
+	};
+
+Example on stm32mp1:
+	bsec: nvmem@5c005000 {
+		...
+		mac_addr: mac_addr@e4 {
+			reg = <0xe4 0x8>;
+			st,non-secure-otp;
+		};
+		...
+	};
+
+The nvmem_layout node gathers all nvmem platform-dependent layout information,
+including OTP names and phandles, in order to allow easy accesses for data
+consumers, using pre-defined string in nvmem-cell-names property.
+
+Required properties:
+- compatible:		"st,stm32-nvmem-layout"
+- nvmem-cells and nvmem-cell-names, as described in nvmem.txt.
+
+Example on stm32mp1:
+	nvmem_layout: nvmem_layout@0 {
+		compatible = "st,stm32-nvmem-layout";
+		nvmem-cells = <&part_number_otp>,
+			      ...
+			      ;
+		nvmem-cell-names = "part_number_otp",
+				   ...
+				   ;
+	};
+
+	bsec: nvmem@5c005000 {
+		...
+		part_number_otp: part_number_otp@4 {
+			reg = <0x4 0x1>;
+		};
+		...
+	};
diff --git a/docs/devicetree/bindings/soc/st,stm32-stgen.txt b/docs/devicetree/bindings/soc/st,stm32-stgen.txt
new file mode 100644
index 0000000000..dbd962ebc4
--- /dev/null
+++ b/docs/devicetree/bindings/soc/st,stm32-stgen.txt
@@ -0,0 +1,18 @@
+STMicroelectronics STM32 STGEN
+===============================
+
+The STM32 System Generic Counter generate a time count value. This
+is a 64 bits wide counter.
+
+Required properties:
+
+- compatible : Should be "st,stm32-stgen"
+- reg : Should be register base and length as documented in the datasheet
+
+Example:
+
+	stgen: stgen@5C008000 {
+		compatible = "st,stm32-stgen";
+		reg = <0x5C008000 0x1000>;
+		status = "okay";
+	};
diff --git a/docs/devicetree/bindings/soc/st,stm32-tamp.txt b/docs/devicetree/bindings/soc/st,stm32-tamp.txt
new file mode 100644
index 0000000000..4d21c6b8a3
--- /dev/null
+++ b/docs/devicetree/bindings/soc/st,stm32-tamp.txt
@@ -0,0 +1,22 @@
+STM32 TAMPER
+---------------------------------
+
+Required properties:
+- compatible: should be "st,stm32-tamp"
+- reg: physical base address and length of the registers set for the device
+- clocks: reference to the clock entry
+- secure-status: Required to properly disable/enable secure IP
+
+Optional property:
+- st,out3-pc13: Configure option register to map OUT3 on PC13
+- wakeup-source : Configure tamp as wakeup-src
+
+Examples:
+tamp: tamp@5C00A000 {
+	compatible = "st,stm32-tamp";
+	reg = <0x5C00A000 0x100>;
+	clocks = <&rcc_clk RTCAPB>;
+	st,out3-pc13;
+	wakeup-source;
+	secure-status = "okay";
+};
diff --git a/docs/devicetree/bindings/watchdog/st,stm32-iwdg.txt b/docs/devicetree/bindings/watchdog/st,stm32-iwdg.txt
new file mode 100644
index 0000000000..2453603a17
--- /dev/null
+++ b/docs/devicetree/bindings/watchdog/st,stm32-iwdg.txt
@@ -0,0 +1,28 @@
+STM32 Independent WatchDoG (IWDG)
+---------------------------------
+
+Required properties:
+- compatible: should be "st,stm32mp1-iwdg".
+- reg: physical base address and length of the registers set for the device.
+- clocks: reference to the clock entry lsi. Additional pclk clock entry.
+  is required only for st,stm32mp1-iwdg.
+- clock-names: name of the clocks used.
+  "pclk", "lsi" for st,stm32mp1-iwdg.
+
+Optional properties:
+- timeout-sec: Watchdog timeout value in seconds.
+- secure-timeout-sec: Watchdog early timeout management in seconds.
+- stm32,enable-on-stop: Keep watchdog enable during stop.
+- stm32,enable-on-standby: Keep watchdog enable durung standby.
+- secure-status: Required to properly enable/disable secure IP.
+
+Examples:
+
+iwdg2: iwdg@5a002000 {
+	compatible = "st,stm32mp1-iwdg";
+	reg = <0x5a002000 0x400>;
+	clocks = <&rcc IWDG2>, <&clk_lsi>;
+	clock-names = "pclk", "lsi";
+	instance = <2>;
+	timeout-sec = <30>;
+};
diff --git a/docs/getting_started/porting-guide.rst b/docs/getting_started/porting-guide.rst
index 19e26e4ea1..ba816fae58 100644
--- a/docs/getting_started/porting-guide.rst
+++ b/docs/getting_started/porting-guide.rst
@@ -796,6 +796,31 @@ The function returns 0 on success. Any other value means the counter value
 either could not be updated or the authentication image descriptor indicates
 that it is not allowed to be updated.
 
+Function: plat_get_hashed_pk()
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+::
+
+    Argument : void *, unsigned int, void **, unsigned int *
+    Return   : int
+
+This function is optional when Trusted Board Boot is enabled, and only
+used if the platform saves a hash of the ROTPK.
+First argument is the BER ROTPK.
+Second argument is its size.
+Third argument is used to return a pointer to a buffer, which hash should
+be the one saved in OTP
+Fourth argument is a pointer to return its size
+
+Most platforms save the hash of the BER ROTPK, but some may save the hash of
+a non encapsulated public key or a platform specific encapsulated ROT public
+key. Defining this function allows to transform the BER ROTPK used to verify
+the signature to the buffer (a platform specific encapsulated public key) which
+hash is saved in OTP.
+
+The function returns 0 on success. Any other value means the expected
+public key buffer cannot be extracted from the BER public key.
+
 Common mandatory function modifications
 ---------------------------------------
 
diff --git a/docs/plat/stm32mp1.rst b/docs/plat/stm32mp1.rst
index f597460db2..fb7afdbbb0 100644
--- a/docs/plat/stm32mp1.rst
+++ b/docs/plat/stm32mp1.rst
@@ -37,6 +37,15 @@ The TF-A image must be properly formatted with a STM32 header structure
 for ROM code is able to load this image.
 Tool stm32image can be used to prepend this header to the generated TF-A binary.
 
+Boot with FIP
+~~~~~~~~~~~~~
+The use of FIP is now the recommended way to boot STM32MP1 platform.
+Only BL2 (with STM32 header) is loaded by ROM code. The other binaries are
+inside the FIP binary: BL32 (SP_min or OP-TEE), U-Boot and their respective
+device tree blobs.
+
+STM32IMAGE bootchain (deprecated)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 At compilation step, BL2, BL32 and DTB file are linked together in a single
 binary. The stm32image tool is also generated and the header is added to TF-A
 binary. This binary file with header is named tf-a-stm32mp157c-ev1.stm32.
@@ -55,15 +64,17 @@ Memory mapping
                |       ...       |
                |                 |
     0x2FFC0000 +-----------------+ \
-               |                 | |
+               |     BL32 DTB    | |
+    0x2FFC5000 +-----------------+ |
+               |       BL32      | |
+    0x2FFDF000 +-----------------+ |
                |       ...       | |
-               |                 | |
-    0x2FFD8000 +-----------------+ |
-               |    TF-A DTB     | | Embedded SRAM
-    0x2FFDC000 +-----------------+ |
+    0x2FFE3000 +-----------------+ |
+               |     BL2 DTB     | | Embedded SRAM
+    0x2FFEA000 +-----------------+ |
                |       BL2       | |
-    0x2FFEF000 +-----------------+ |
-               |       BL32      | |
+    0x2FFFF000 +-----------------+ |
+               |  SCMI mailbox   | |
     0x30000000 +-----------------+ /
                |                 |
                |       ...       |
@@ -95,41 +106,169 @@ Build Instructions
 ------------------
 Boot media(s) supported by BL2 must be specified in the build command.
 Available storage medias are:
+
 - ``STM32MP_SDMMC``
 - ``STM32MP_EMMC``
 - ``STM32MP_RAW_NAND``
 - ``STM32MP_SPI_NAND``
 - ``STM32MP_SPI_NOR``
 
-To build with SP_min and support for all bootable devices:
+Boot with FIP
+~~~~~~~~~~~~~
+You need to build BL2, BL32 (SP_min or OP-TEE) and BL33 (U-Boot) before building FIP binary.
+
+U-Boot
+______
 
 .. code:: bash
 
-    make CROSS_COMPILE=arm-linux-gnueabihf- PLAT=stm32mp1 ARCH=aarch32 ARM_ARCH_MAJOR=7 AARCH32_SP=sp_min STM32MP_SDMMC=1 STM32MP_EMMC=1 STM32MP_RAW_NAND=1 STM32MP_SPI_NAND=1
-    STM32MP_SPI_NOR=1 DTB_FILE_NAME=stm32mp157c-ev1.dtb
     cd <u-boot_directory>
     make stm32mp15_trusted_defconfig
     make DEVICE_TREE=stm32mp157c-ev1 all
 
-To build TF-A with OP-TEE support for all bootable devices:
+OP-TEE (optional)
+_________________
+
 .. code:: bash
 
-    make CROSS_COMPILE=arm-linux-gnueabihf- PLAT=stm32mp1 ARCH=aarch32 ARM_ARCH_MAJOR=7 AARCH32_SP=optee STM32MP_SDMMC=1 STM32MP_EMMC=1 STM32MP_RAW_NAND=1 STM32MP_SPI_NAND=1 STM32MP_SPI_NOR=1 DTB_FILE_NAME=stm32mp157c-ev1.dtb
     cd <optee_directory>
-    make CROSS_COMPILE=arm-linux-gnueabihf- ARCH=arm PLATFORM=stm32mp1 CFG_EMBED_DTB_SOURCE_FILE=stm32mp157c-ev1.dts
+    make CROSS_COMPILE=arm-linux-gnueabihf- ARCH=arm PLATFORM=stm32mp1 \
+        CFG_EMBED_DTB_SOURCE_FILE=stm32mp157c-ev1.dts
+
+
+TF-A BL32 (SP_min)
+__________________
+If you choose not to use OP-TEE, you can use TF-A SP_min.
+To build TF-A BL32, and its device tree file:
+
+.. code:: bash
+
+    make CROSS_COMPILE=arm-none-eabi- PLAT=stm32mp1 ARCH=aarch32 ARM_ARCH_MAJOR=7 \
+        AARCH32_SP=sp_min DTB_FILE_NAME=stm32mp157c-ev1.dtb bl32 dtbs
+
+TF-A BL2
+________
+To build TF-A BL2 with its STM32 header for SD-card boot:
+
+.. code:: bash
+
+    make CROSS_COMPILE=arm-none-eabi- PLAT=stm32mp1 ARCH=aarch32 ARM_ARCH_MAJOR=7 \
+        DTB_FILE_NAME=stm32mp157c-ev1.dtb
+
+This BL2 is independent of the BL32 used (SP_min or OP-TEE)
+
+
+FIP
+___
+With BL32 SP_min:
+
+.. code:: bash
+
+    make CROSS_COMPILE=arm-none-eabi- PLAT=stm32mp1 ARCH=aarch32 ARM_ARCH_MAJOR=7 \
+        AARCH32_SP=sp_min \
+        DTB_FILE_NAME=stm32mp157c-ev1.dtb \
+        BL33=<u-boot_directory>/u-boot-nodtb.bin \
+        BL33_CFG=<u-boot_directory>/u-boot.dtb \
+        fip
+
+With OP-TEE:
+
+.. code:: bash
+
+    make CROSS_COMPILE=arm-none-eabi- PLAT=stm32mp1 ARCH=aarch32 ARM_ARCH_MAJOR=7 \
+        DTB_FILE_NAME=stm32mp157c-ev1.dtb \
+        BL33=<u-boot_directory>/u-boot-nodtb.bin \
+        BL33_CFG=<u-boot_directory>/u-boot.dtb \
+        BL32=<optee_directory>/tee-header_v2.bin \
+        BL32_EXTRA1=<optee_directory>/tee-pager_v2.bin
+        BL32_EXTRA2=<optee_directory>/tee-pageable_v2.bin
+        fip
+
+Trusted Boot Board
+__________________
+
+.. code:: shell
+
+    tools/cert_create/cert_create -n --rot-key "build/stm32mp1/debug/rot_key.pem" \
+        --tfw-nvctr 0 \
+        --ntfw-nvctr 0 \
+        --key-alg ecdsa --hash-alg sha256 \
+        --trusted-key-cert build/stm32mp1/cert_images/trusted-key-cert.key-crt \
+        --tb-fw=build/stm32mp1/debug/bl2.bin \
+        --tb-fw-cert build/stm32mp1/cert_images/trusted-boot-fw.key-crt\
+        --tos-fw <optee_directory>/tee-header_v2.bin \
+        --tos-fw-cert build/stm32mp1/cert_images/tee-header_v2.bin.crt \
+        --tos-fw-key-cert build/stm32mp1/cert_images/tee-header_v2.bin.key-crt \
+        --tos-fw-extra1 <optee_directory>/tee-pager_v2.bin \
+        --tos-fw-extra2 <optee_directory>/tee-pageable_v2.bin \
+        --nt-fw <u-boot_directory>/u-boot-nodtb.bin \
+        --nt-fw-cert build/stm32mp1/cert_images/u-boot.bin.crt \
+        --nt-fw-key-cert build/stm32mp1/cert_images/u-boot.bin.key-crt \
+        --hw-config <u-boot_directory>/u-boot.dtb
+        --fw-config build/stm32mp1/debug/fdts/fw-config.dtb
+
+    tools/fiptool/fiptool create --tos-fw <optee_directory>/tee-header_v2.bin \
+        --tos-fw-extra1 <optee_directory>/tee-pager_v2.bin \
+        --tos-fw-extra2 <optee_directory>/tee-pageable_v2.bin \
+        --nt-fw <u-boot_directory>/u-boot-nodtb.bin \
+        --hw-config <u-boot_directory>/uboot-nodtb.dtb \
+        --tos-fw-cert build/stm32mp1/cert_images/tee-header_v2.bin.crt \
+        --tos-fw-key-cert build/stm32mp1/cert_images/tee-header_v2.bin.key-crt \
+        --nt-fw-cert build/stm32mp1/cert_images/u-boot.bin.crt \
+        --nt-fw-key-cert build/stm32mp1/cert_images/u-boot.bin.key-crt \
+        --trusted-key-cert build/stm32mp1/cert_images/trusted-key-cert.key-crt \
+        --tb-fw-cert build/stm32mp1/cert_images/trusted-boot-fw.key-crt stm32mp1.fip
+
+
+STM32IMAGE bootchain (deprecated)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+You need to add the following flag to the make command:
+``STM32MP_USE_STM32IMAGE=1``
+
+To build with SP_min and support for SD-card boot:
+
+.. code:: bash
+
+    make CROSS_COMPILE=arm-linux-gnueabihf- PLAT=stm32mp1 ARCH=aarch32 ARM_ARCH_MAJOR=7 \
+        AARCH32_SP=sp_min STM32MP_SDMMC=1 DTB_FILE_NAME=stm32mp157c-ev1.dtb \
+        STM32MP_USE_STM32IMAGE=1
+
     cd <u-boot_directory>
     make stm32mp15_trusted_defconfig
     make DEVICE_TREE=stm32mp157c-ev1 all
 
+To build TF-A with OP-TEE support for SD-card boot:
 
-The following build options are supported:
+.. code:: bash
 
-- ``ENABLE_STACK_PROTECTOR``: To enable the stack protection.
+    make CROSS_COMPILE=arm-linux-gnueabihf- PLAT=stm32mp1 ARCH=aarch32 ARM_ARCH_MAJOR=7 \
+        AARCH32_SP=optee STM32MP_SDMMC=1 DTB_FILE_NAME=stm32mp157c-ev1.dtb \
+        STM32MP_USE_STM32IMAGE=1
+
+    cd <optee_directory>
+    make CROSS_COMPILE=arm-linux-gnueabihf- ARCH=arm PLATFORM=stm32mp1 \
+        CFG_EMBED_DTB_SOURCE_FILE=stm32mp157c-ev1.dts
+
+    cd <u-boot_directory>
+    make stm32mp15_trusted_defconfig
+    make DEVICE_TREE=stm32mp157c-ev1 all
 
 
 Populate SD-card
 ----------------
 
+Boot with FIP
+~~~~~~~~~~~~~
+The SD-card has to be formated with GPT.
+It should contain at least those partitions:
+
+- fsbl: to copy the tf-a-stm32mp157c-ev1.stm32 binary (BL2)
+- fip: which contains the FIP binary
+
+Usually, two copies of fsbl are used (fsbl1 and fsbl2) instead of one partition fsbl.
+
+STM32IMAGE bootchain (deprecated)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 The SD-card has to be formated with GPT.
 It should contain at least those partitions:
 
diff --git a/drivers/arm/tzc/tzc400.c b/drivers/arm/tzc/tzc400.c
index 95a5e7f77f..fa32a695e0 100644
--- a/drivers/arm/tzc/tzc400.c
+++ b/drivers/arm/tzc/tzc400.c
@@ -10,6 +10,7 @@
 #include <common/debug.h>
 #include <drivers/arm/tzc400.h>
 #include <lib/mmio.h>
+#include <lib/utils_def.h>
 
 #include "tzc_common_private.h"
 
@@ -60,18 +61,54 @@ static inline void _tzc400_write_gate_keeper(uintptr_t base, unsigned int val)
 					GATE_KEEPER_OS_SHIFT) &		\
 					GATE_KEEPER_OS_MASK)
 
-
 /* Define common core functions used across different TZC peripherals. */
 DEFINE_TZC_COMMON_WRITE_ACTION(400, 400)
 DEFINE_TZC_COMMON_WRITE_REGION_BASE(400, 400)
 DEFINE_TZC_COMMON_WRITE_REGION_TOP(400, 400)
 DEFINE_TZC_COMMON_WRITE_REGION_ATTRIBUTES(400, 400)
 DEFINE_TZC_COMMON_WRITE_REGION_ID_ACCESS(400, 400)
+DEFINE_TZC_COMMON_UPDATE_FILTERS(400, 400)
 DEFINE_TZC_COMMON_CONFIGURE_REGION0(400)
 DEFINE_TZC_COMMON_CONFIGURE_REGION(400)
 
+static inline void tzc400_clear_it(long base, uint32_t filter)
+{
+	mmio_write_32(base + INT_CLEAR, BIT(filter));
+}
+
+static inline uint32_t tzc400_get_int_by_filter(long base, uint32_t filter)
+{
+	return (mmio_read_32(base + INT_STATUS) & BIT(filter));
+}
+
+#if DEBUG
+static unsigned long tzc400_get_fail_address(long base, uint32_t filter)
+{
+	unsigned long fail_address;
+
+	fail_address = mmio_read_32(base + FAIL_ADDRESS_LOW_OFF +
+				    (filter * FILTER_OFFSET));
+#ifdef __aarch64__
+	fail_address += mmio_read_32(base + FAIL_ADDRESS_HIGH_OFF +
+				     (filter * FILTER_OFFSET)) << 32;
+#endif
+
+	return fail_address;
+}
+
+static inline uint32_t tzc400_get_fail_id(long base, uint32_t filter)
+{
+	return mmio_read_32(base + FAIL_ID + (filter * FILTER_OFFSET));
+}
+
+static inline uint32_t tzc400_get_fail_control(long base, uint32_t filter)
+{
+	return mmio_read_32(base + FAIL_CONTROL_OFF + (filter * FILTER_OFFSET));
+}
+#endif
+
 static unsigned int _tzc400_get_gate_keeper(uintptr_t base,
-				unsigned int filter)
+					    unsigned int filter)
 {
 	unsigned int open_status;
 
@@ -81,9 +118,8 @@ static unsigned int _tzc400_get_gate_keeper(uintptr_t base,
 }
 
 /* This function is not MP safe. */
-static void _tzc400_set_gate_keeper(uintptr_t base,
-				unsigned int filter,
-				int val)
+static void _tzc400_set_gate_keeper(uintptr_t base, unsigned int filter,
+				    int val)
 {
 	unsigned int open_status;
 
@@ -151,7 +187,7 @@ void tzc400_init(uintptr_t base)
  * changed. This function only changes the access permissions.
  */
 void tzc400_configure_region0(unsigned int sec_attr,
-			   unsigned int ns_device_access)
+			      unsigned int ns_device_access)
 {
 	assert(tzc400.base != 0U);
 	assert(sec_attr <= TZC_REGION_S_RDWR);
@@ -168,11 +204,11 @@ void tzc400_configure_region0(unsigned int sec_attr,
  * for this region (see comment for that function).
  */
 void tzc400_configure_region(unsigned int filters,
-			  unsigned int region,
-			  unsigned long long region_base,
-			  unsigned long long region_top,
-			  unsigned int sec_attr,
-			  unsigned int nsaid_permissions)
+			     unsigned int region,
+			     unsigned long long region_base,
+			     unsigned long long region_top,
+			     unsigned int sec_attr,
+			     unsigned int nsaid_permissions)
 {
 	assert(tzc400.base != 0U);
 
@@ -185,7 +221,7 @@ void tzc400_configure_region(unsigned int filters,
 	 * the max and expected case.
 	 */
 	assert((region_top <= (UINT64_MAX >> (64U - tzc400.addr_width))) &&
-		(region_base < region_top));
+	       (region_base < region_top));
 
 	/* region_base and (region_top + 1) must be 4KB aligned */
 	assert(((region_base | (region_top + 1U)) & (4096U - 1U)) == 0U);
@@ -193,8 +229,16 @@ void tzc400_configure_region(unsigned int filters,
 	assert(sec_attr <= TZC_REGION_S_RDWR);
 
 	_tzc400_configure_region(tzc400.base, filters, region, region_base,
-						region_top,
-						sec_attr, nsaid_permissions);
+				 region_top, sec_attr, nsaid_permissions);
+}
+
+void tzc400_update_filters(unsigned int region, unsigned int filters)
+{
+	/* Do range checks on filters and regions. */
+	assert(((filters >> tzc400.num_filters) == 0U) &&
+	       (region < tzc400.num_regions));
+
+	_tzc400_update_filters(tzc400.base, region, tzc400.num_filters, filters);
 }
 
 void tzc400_enable_filters(void)
@@ -217,8 +261,8 @@ void tzc400_enable_filters(void)
 			 * See the 'ARM (R) CoreLink TM TZC-400 TrustZone (R)
 			 * Address Space Controller' Technical Reference Manual.
 			 */
-			ERROR("TZC-400 : Filter %d Gatekeeper already"
-				" enabled.\n", filter);
+			ERROR("TZC-400: Filter %d Gatekeeper already enabled\n",
+			      filter);
 			panic();
 		}
 		_tzc400_set_gate_keeper(tzc400.base, filter, 1);
@@ -238,3 +282,62 @@ void tzc400_disable_filters(void)
 	for (filter = 0; filter < tzc400.num_filters; filter++)
 		_tzc400_set_gate_keeper(tzc400.base, filter, 0);
 }
+
+void tzc400_it_handler(void)
+{
+	uint32_t filter;
+	uint32_t filter_it_pending = tzc400.num_filters;
+#if DEBUG
+	uint32_t control_fail;
+	uint32_t fail_id;
+	unsigned long address_fail;
+#endif
+
+	assert(tzc400.base != 0U);
+
+	/* first display information conerning the fault access */
+	for (filter = 0U; (filter < tzc400.num_filters) &&
+	     (filter_it_pending == tzc400.num_filters); filter++) {
+		if (tzc400_get_int_by_filter(tzc400.base, filter)) {
+			filter_it_pending = filter;
+		}
+	}
+
+	if (filter_it_pending == tzc400.num_filters) {
+		ERROR("Error no IT pending!");
+		panic();
+	}
+
+#if DEBUG
+	address_fail = tzc400_get_fail_address(tzc400.base, filter_it_pending);
+	ERROR("Illegal access to 0x%lx:\n", address_fail);
+
+	fail_id = tzc400_get_fail_id(tzc400.base, filter_it_pending);
+	ERROR("\tFAIL_ID = 0x%x\n", fail_id);
+
+	control_fail = tzc400_get_fail_control(tzc400.base, filter_it_pending);
+
+	if (((control_fail & BIT(FAIL_CONTROL_NS_SHIFT)) >> FAIL_CONTROL_NS_SHIFT) ==
+	    FAIL_CONTROL_NS_NONSECURE) {
+		ERROR("\tNon-Secure\n");
+	} else {
+		ERROR("\tSecure\n");
+	}
+
+	if (((control_fail & BIT(FAIL_CONTROL_PRIV_SHIFT)) >> FAIL_CONTROL_PRIV_SHIFT) ==
+	    FAIL_CONTROL_PRIV_PRIV) {
+		ERROR("\tPrivilege\n");
+	} else {
+		ERROR("\tUnprivilege\n");
+	}
+
+	if (((control_fail & BIT(FAIL_CONTROL_DIR_SHIFT)) >> FAIL_CONTROL_DIR_SHIFT) ==
+	    FAIL_CONTROL_DIR_WRITE) {
+		ERROR("\tWrite\n");
+	} else {
+		ERROR("\tRead\n");
+	}
+#endif
+
+	tzc400_clear_it(tzc400.base, filter_it_pending);
+}
diff --git a/drivers/arm/tzc/tzc_common_private.h b/drivers/arm/tzc/tzc_common_private.h
index 1d99077ad1..b2af140fb7 100644
--- a/drivers/arm/tzc/tzc_common_private.h
+++ b/drivers/arm/tzc/tzc_common_private.h
@@ -89,6 +89,27 @@
 			val);						\
 	}
 
+/*
+ * It is used to modify the filters status for a defined region.
+ */
+#define DEFINE_TZC_COMMON_UPDATE_FILTERS(fn_name, macro_name)		\
+	static inline void _tzc##fn_name##_update_filters(		\
+						uintptr_t base,		\
+						unsigned int region_no,	\
+						unsigned int nbfilters, \
+						unsigned int filters)	\
+	{								\
+		uint32_t filters_mask = GENMASK(nbfilters - 1U, 0);	\
+									\
+		mmio_clrsetbits_32(base +				\
+			TZC_REGION_OFFSET(				\
+				TZC_##macro_name##_REGION_SIZE,		\
+				region_no) +				\
+			TZC_##macro_name##_REGION_ATTR_0_OFFSET,	\
+			filters_mask << TZC_REGION_ATTR_F_EN_SHIFT,	\
+			filters << TZC_REGION_ATTR_F_EN_SHIFT);		\
+	}
+
 /*
  * It is used to program region 0 ATTRIBUTES and ACCESS register.
  */
diff --git a/drivers/auth/auth_mod.c b/drivers/auth/auth_mod.c
index 91ee1bea97..a84787fc39 100644
--- a/drivers/auth/auth_mod.c
+++ b/drivers/auth/auth_mod.c
@@ -30,6 +30,7 @@
 	} while (0)
 
 #pragma weak plat_set_nv_ctr2
+#pragma weak plat_get_hashed_pk
 
 
 static int cmp_auth_param_type_desc(const auth_param_type_desc_t *a,
@@ -201,6 +202,10 @@ static int auth_signature(const auth_method_param_sig_t *param,
 			NOTICE("ROTPK is not deployed on platform. "
 				"Skipping ROTPK verification.\n");
 		} else {
+			/* platform may store the hash of a prefixed, suffixed or modified pk */
+			rc = plat_get_hashed_pk(pk_ptr, pk_len, &pk_ptr, &pk_len);
+			return_if_error(rc);
+
 			/* Ask the crypto-module to verify the key hash */
 			rc = crypto_mod_verify_hash(pk_ptr, pk_len,
 				    pk_hash_ptr, pk_hash_len);
@@ -292,6 +297,15 @@ int plat_set_nv_ctr2(void *cookie, const auth_img_desc_t *img_desc __unused,
 	return plat_set_nv_ctr(cookie, nv_ctr);
 }
 
+int plat_get_hashed_pk(void *full_pk_ptr, unsigned int full_pk_len,
+		       void **hashed_pk_ptr, unsigned int *hashed_pk_len)
+{
+	*hashed_pk_ptr = full_pk_ptr;
+	*hashed_pk_len = full_pk_len;
+
+	return 0;
+}
+
 /*
  * Return the parent id in the output parameter '*parent_id'
  *
diff --git a/drivers/auth/mbedtls/mbedtls_common.mk b/drivers/auth/mbedtls/mbedtls_common.mk
index 53ebe30b63..ace4312cf3 100644
--- a/drivers/auth/mbedtls/mbedtls_common.mk
+++ b/drivers/auth/mbedtls/mbedtls_common.mk
@@ -16,7 +16,7 @@ endif
 MBEDTLS_INC		=	-I${MBEDTLS_DIR}/include
 
 # Specify mbed TLS configuration file
-MBEDTLS_CONFIG_FILE	:=	"<drivers/auth/mbedtls/mbedtls_config.h>"
+MBEDTLS_CONFIG_FILE	?=	"<drivers/auth/mbedtls/mbedtls_config.h>"
 $(eval $(call add_define,MBEDTLS_CONFIG_FILE))
 
 MBEDTLS_SOURCES	+=		drivers/auth/mbedtls/mbedtls_common.c
diff --git a/drivers/auth/mbedtls/mbedtls_x509_parser.c b/drivers/auth/mbedtls/mbedtls_x509_parser.c
index 129566bd69..7b3ecd1a31 100644
--- a/drivers/auth/mbedtls/mbedtls_x509_parser.c
+++ b/drivers/auth/mbedtls/mbedtls_x509_parser.c
@@ -114,10 +114,10 @@ static int get_ext(const char *oid, void **ext, unsigned int *ext_len)
 		oid_len = mbedtls_oid_get_numeric_string(oid_str,
 							 MAX_OID_STR_LEN,
 							 &extn_oid);
-		if (oid_len == MBEDTLS_ERR_OID_BUF_TOO_SMALL) {
+		if ((oid_len == MBEDTLS_ERR_OID_BUF_TOO_SMALL) || (oid_len < 0)) {
 			return IMG_PARSER_ERR;
 		}
-		if ((oid_len == strlen(oid_str)) && !strcmp(oid, oid_str)) {
+		if (((size_t)oid_len == strlen(oid_str)) && !strcmp(oid, oid_str)) {
 			*ext = (void *)p;
 			*ext_len = (unsigned int)len;
 			return IMG_PARSER_OK;
diff --git a/drivers/auth/tbbr/tbbr_cot_bl1.c b/drivers/auth/tbbr/tbbr_cot_bl1.c
index e4c92213ae..15a35431d6 100644
--- a/drivers/auth/tbbr/tbbr_cot_bl1.c
+++ b/drivers/auth/tbbr/tbbr_cot_bl1.c
@@ -150,21 +150,6 @@ static const auth_img_desc_t tb_fw_config = {
 	}
 };
 
-static const auth_img_desc_t fw_config = {
-	.img_id = FW_CONFIG_ID,
-	.img_type = IMG_RAW,
-	.parent = &trusted_boot_fw_cert,
-	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
-		[0] = {
-			.type = AUTH_METHOD_HASH,
-			.param.hash = {
-				.data = &raw_data,
-				.hash = &fw_config_hash
-			}
-		}
-	}
-};
-
 /*
  * TBBR Chain of trust definition
  */
diff --git a/drivers/auth/tbbr/tbbr_cot_bl2.c b/drivers/auth/tbbr/tbbr_cot_bl2.c
index 65a0478abf..de7ad8f902 100644
--- a/drivers/auth/tbbr/tbbr_cot_bl2.c
+++ b/drivers/auth/tbbr/tbbr_cot_bl2.c
@@ -671,6 +671,7 @@ static const auth_img_desc_t * const cot_desc[] = {
 	[NON_TRUSTED_FW_CONTENT_CERT_ID]	=	&non_trusted_fw_content_cert,
 	[BL33_IMAGE_ID]				=	&bl33_image,
 	[NT_FW_CONFIG_ID]			=	&nt_fw_config,
+	[FW_CONFIG_ID]				=	&fw_config,
 #if defined(SPD_spmd)
 	[SIP_SP_CONTENT_CERT_ID]		=	&sip_sp_content_cert,
 	[SP_PKG1_ID]				=	&sp_pkg1,
diff --git a/drivers/auth/tbbr/tbbr_cot_common.c b/drivers/auth/tbbr/tbbr_cot_common.c
index ff3f22de15..279f30e2e2 100644
--- a/drivers/auth/tbbr/tbbr_cot_common.c
+++ b/drivers/auth/tbbr/tbbr_cot_common.c
@@ -124,3 +124,18 @@ const auth_img_desc_t hw_config = {
 		}
 	}
 };
+
+const auth_img_desc_t fw_config = {
+	.img_id = FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_boot_fw_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &fw_config_hash
+			}
+		}
+	}
+};
diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
new file mode 100644
index 0000000000..9336b88b4f
--- /dev/null
+++ b/drivers/clk/clk.c
@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ * Author(s): Ludovic Barre, <ludovic.barre@st.com> for STMicroelectronics.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+
+#include <drivers/clk.h>
+
+static const clk_ops_t *ops;
+
+int clk_enable(unsigned long id)
+{
+	assert((ops != NULL) && (ops->enable != NULL));
+
+	return ops->enable(id);
+}
+
+void clk_disable(unsigned long id)
+{
+	assert((ops != NULL) && (ops->disable != NULL));
+
+	ops->disable(id);
+}
+
+unsigned long clk_get_rate(unsigned long id)
+{
+	assert((ops != NULL) && (ops->get_rate != NULL));
+
+	return ops->get_rate(id);
+}
+
+int clk_get_parent(unsigned long id)
+{
+	assert((ops != NULL) && (ops->get_parent != NULL));
+
+	return ops->get_parent(id);
+}
+
+bool clk_is_enabled(unsigned long id)
+{
+	assert((ops != NULL) && (ops->is_enabled != NULL));
+
+	return ops->is_enabled(id);
+}
+
+/*
+ * Initialize the clk. The fields in the provided clk
+ * ops pointer must be valid.
+ */
+void clk_register(const clk_ops_t *ops_ptr)
+{
+	assert((ops_ptr != NULL) && (ops_ptr->enable != NULL) &&
+	       (ops_ptr->disable != NULL) &&
+	       (ops_ptr->get_rate != NULL) &&
+	       (ops_ptr->get_parent != NULL) &&
+	       (ops_ptr->is_enabled != NULL));
+
+	ops = ops_ptr;
+}
diff --git a/drivers/io/io_mtd.c b/drivers/io/io_mtd.c
index 7575fa2503..a9f41a18c6 100644
--- a/drivers/io/io_mtd.c
+++ b/drivers/io/io_mtd.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,8 +18,9 @@
 typedef struct {
 	io_mtd_dev_spec_t	*dev_spec;
 	uintptr_t		base;
-	unsigned long long	offset;		/* Offset in bytes */
-	unsigned long long	size;	/* Size of device in bytes */
+	unsigned long long	pos;		/* Offset in bytes */
+	unsigned long long	size;		/* Size of device in bytes */
+	unsigned long long	extra_offset;	/* Extra offset in bytes */
 } mtd_dev_state_t;
 
 io_type_t device_type_mtd(void);
@@ -110,16 +111,47 @@ static int free_dev_info(io_dev_info_t *dev_info)
 	return 0;
 }
 
+static int mtd_add_extra_offset(mtd_dev_state_t *cur, size_t *extra_offset)
+{
+	io_mtd_ops_t *ops = &cur->dev_spec->ops;
+	int ret;
+
+	if (ops->seek == NULL) {
+		return 0;
+	}
+
+	ret = ops->seek(cur->base, cur->pos, extra_offset);
+	if (ret != 0) {
+		ERROR("%s: Seek error %d\n", __func__, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
 static int mtd_open(io_dev_info_t *dev_info, const uintptr_t spec,
 		    io_entity_t *entity)
 {
 	mtd_dev_state_t *cur;
+	io_block_spec_t *region;
+	size_t extra_offset = 0U;
+	int ret;
 
 	assert((dev_info->info != 0UL) && (entity->info == 0UL));
 
+	region = (io_block_spec_t *)spec;
 	cur = (mtd_dev_state_t *)dev_info->info;
 	entity->info = (uintptr_t)cur;
-	cur->offset = 0U;
+	cur->base = region->offset;
+	cur->pos = 0U;
+	cur->extra_offset = 0U;
+
+	ret = mtd_add_extra_offset(cur, &extra_offset);
+	if (ret != 0) {
+		return ret;
+	}
+
+	cur->base += extra_offset;
 
 	return 0;
 }
@@ -128,6 +160,8 @@ static int mtd_open(io_dev_info_t *dev_info, const uintptr_t spec,
 static int mtd_seek(io_entity_t *entity, int mode, signed long long offset)
 {
 	mtd_dev_state_t *cur;
+	size_t extra_offset = 0U;
+	int ret;
 
 	assert((entity->info != (uintptr_t)NULL) && (offset >= 0));
 
@@ -140,22 +174,29 @@ static int mtd_seek(io_entity_t *entity, int mode, signed long long offset)
 			return -EINVAL;
 		}
 
-		cur->offset = offset;
+		cur->pos = offset;
 		break;
 	case IO_SEEK_CUR:
-		if (((cur->offset + (unsigned long long)offset) >=
+		if (((cur->base + cur->pos + (unsigned long long)offset) >=
 		     cur->size) ||
-		    ((cur->offset + (unsigned long long)offset) <
-		     cur->offset)) {
+		    ((cur->base + cur->pos + (unsigned long long)offset) <
+		     cur->base + cur->pos)) {
 			return -EINVAL;
 		}
 
-		cur->offset += (unsigned long long)offset;
+		cur->pos += (unsigned long long)offset;
 		break;
 	default:
 		return -EINVAL;
 	}
 
+	ret = mtd_add_extra_offset(cur, &extra_offset);
+	if (ret != 0) {
+		return ret;
+	}
+
+	cur->extra_offset = extra_offset;
+
 	return 0;
 }
 
@@ -174,18 +215,19 @@ static int mtd_read(io_entity_t *entity, uintptr_t buffer, size_t length,
 	assert(ops->read != NULL);
 
 	VERBOSE("Read at %llx into %lx, length %zi\n",
-		cur->offset, buffer, length);
-	if ((cur->offset + length) > cur->dev_spec->device_size) {
+		cur->base + cur->pos, buffer, length);
+	if ((cur->base + cur->pos + length) > cur->dev_spec->device_size) {
 		return -EINVAL;
 	}
 
-	ret = ops->read(cur->offset, buffer, length, out_length);
+	ret = ops->read(cur->base + cur->pos + cur->extra_offset, buffer,
+			length, out_length);
 	if (ret < 0) {
 		return ret;
 	}
 
 	assert(*out_length == length);
-	cur->offset += *out_length;
+	cur->pos += *out_length;
 
 	return 0;
 }
diff --git a/drivers/mmc/mmc.c b/drivers/mmc/mmc.c
index b5f6a10d38..42243ea096 100644
--- a/drivers/mmc/mmc.c
+++ b/drivers/mmc/mmc.c
@@ -25,6 +25,7 @@
 static const struct mmc_ops *ops;
 static unsigned int mmc_ocr_value;
 static struct mmc_csd_emmc mmc_csd;
+static struct sd_switch_status sd_switch_func_status;
 static unsigned char mmc_ext_csd[512] __aligned(16);
 static unsigned int mmc_flags;
 static struct mmc_device_info *mmc_dev_info;
@@ -44,6 +45,11 @@ static bool is_cmd23_enabled(void)
 	return ((mmc_flags & MMC_FLAG_CMD23) != 0U);
 }
 
+static bool is_sd_cmd6_enabled(void)
+{
+	return ((mmc_flags & MMC_FLAG_SD_CMD6) != 0U);
+}
+
 static int mmc_send_cmd(unsigned int idx, unsigned int arg,
 			unsigned int r_type, unsigned int *r_data)
 {
@@ -327,6 +333,33 @@ static int mmc_fill_device_info(void)
 	return 0;
 }
 
+static int sd_switch(unsigned char mode, unsigned char group,
+		     unsigned char func)
+{
+	unsigned int group_shift = (group - 1U) * 4U;
+	unsigned int group_mask = GENMASK(group_shift + 3U,  group_shift);
+	unsigned int arg;
+	int ret = 0;
+
+	ret = ops->prepare(0, (uintptr_t)&sd_switch_func_status,
+			   sizeof(sd_switch_func_status));
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* MMC CMD6: SWITCH_FUNC */
+	arg = (mode << 31) | GENMASK(23, 0);
+	arg &= ~group_mask;
+	arg |= func << group_shift;
+	ret = mmc_send_cmd(MMC_CMD(6), arg, MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	return ops->read(0, (uintptr_t)&sd_switch_func_status,
+			 sizeof(sd_switch_func_status));
+}
+
 static int sd_send_op_cond(void)
 {
 	int n;
@@ -494,7 +527,39 @@ static int mmc_enumerate(unsigned int clk, unsigned int bus_width)
 		return ret;
 	}
 
-	return mmc_fill_device_info();
+	ret = mmc_fill_device_info();
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (is_sd_cmd6_enabled() &&
+	    (mmc_dev_info->mmc_dev_type == MMC_IS_SD_HC)) {
+		/* Try to switch to High Speed Mode */
+		ret = sd_switch(SD_SWITCH_FUNC_CHECK, 1U, 1U);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((sd_switch_func_status.support_g1 & BIT(9)) == 0U) {
+			/* High speed not supported, keep default speed */
+			return 0;
+		}
+
+		ret = sd_switch(SD_SWITCH_FUNC_SWITCH, 1U, 1U);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((sd_switch_func_status.sel_g2_g1 & 0x1U) == 0U) {
+			/* Cannot switch to high speed, keep default speed */
+			return 0;
+		}
+
+		mmc_dev_info->max_bus_freq = 50000000U;
+		ret = ops->set_ios(clk, bus_width);
+	}
+
+	return ret;
 }
 
 size_t mmc_read_blocks(int lba, uintptr_t buf, size_t size)
diff --git a/drivers/mtd/nand/core.c b/drivers/mtd/nand/core.c
index 44b001e35b..d961c24630 100644
--- a/drivers/mtd/nand/core.c
+++ b/drivers/mtd/nand/core.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2019-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -112,6 +112,47 @@ int nand_read(unsigned int offset, uintptr_t buffer, size_t length,
 	return 0;
 }
 
+int nand_seek_bb(uintptr_t base, unsigned int offset, size_t *extra_offset)
+{
+	unsigned int block;
+	unsigned int offset_block;
+	unsigned int max_block;
+	int is_bad;
+	size_t count_bb = 0U;
+
+	block = base / nand_dev.block_size;
+
+	if (offset != 0U) {
+		offset_block = (base + offset - 1U) / nand_dev.block_size;
+	} else {
+		offset_block = block;
+	}
+
+	max_block = nand_dev.size / nand_dev.block_size;
+
+	while (block <= offset_block) {
+		if (offset_block >= max_block) {
+			return -EIO;
+		}
+
+		is_bad = nand_dev.mtd_block_is_bad(block);
+		if (is_bad < 0) {
+			return is_bad;
+		}
+
+		if (is_bad == 1) {
+			count_bb++;
+			offset_block++;
+		}
+
+		block++;
+	}
+
+	*extra_offset = count_bb * nand_dev.block_size;
+
+	return 0;
+}
+
 struct nand_device *get_nand_device(void)
 {
 	return &nand_dev;
diff --git a/drivers/mtd/nor/spi_nor.c b/drivers/mtd/nor/spi_nor.c
index 108f893d3f..2ed562a677 100644
--- a/drivers/mtd/nor/spi_nor.c
+++ b/drivers/mtd/nor/spi_nor.c
@@ -103,7 +103,7 @@ static int spi_nor_ready(void)
 			0 : 1;
 	}
 
-	return (((sr & SR_WIP) != 0U) ? 1 : 0);
+	return (((sr & SR_WIP) == 0U) ? 0 : 1);
 }
 
 static int spi_nor_wait_ready(void)
@@ -131,7 +131,7 @@ static int spi_nor_macronix_quad_enable(void)
 		return ret;
 	}
 
-	if ((sr & SR_QUAD_EN_MX) == 0U) {
+	if ((sr & SR_QUAD_EN_MX) != 0U) {
 		return 0;
 	}
 
@@ -141,7 +141,7 @@ static int spi_nor_macronix_quad_enable(void)
 	}
 
 	sr |= SR_QUAD_EN_MX;
-	ret = spi_nor_reg(SPI_NOR_OP_WRSR, &sr, 1, SPI_MEM_DATA_OUT);
+	ret = spi_nor_reg(SPI_NOR_OP_WRSR, &sr, 1U, SPI_MEM_DATA_OUT);
 	if (ret != 0) {
 		return ret;
 	}
@@ -168,7 +168,7 @@ static int spi_nor_write_sr_cr(uint8_t *sr_cr)
 		return ret;
 	}
 
-	ret = spi_nor_reg(SPI_NOR_OP_WRSR, sr_cr, 2, SPI_MEM_DATA_OUT);
+	ret = spi_nor_reg(SPI_NOR_OP_WRSR, sr_cr, 2U, SPI_MEM_DATA_OUT);
 	if (ret != 0) {
 		return -EINVAL;
 	}
@@ -230,7 +230,7 @@ static int spi_nor_clean_bar(void)
 	}
 
 	return spi_nor_reg(nor_dev.bank_write_cmd, &nor_dev.selected_bank,
-			   1, SPI_MEM_DATA_OUT);
+			   1U, SPI_MEM_DATA_OUT);
 }
 
 static int spi_nor_write_bar(uint32_t offset)
@@ -248,7 +248,7 @@ static int spi_nor_write_bar(uint32_t offset)
 	}
 
 	ret = spi_nor_reg(nor_dev.bank_write_cmd, &selected_bank,
-			  1, SPI_MEM_DATA_OUT);
+			  1U, SPI_MEM_DATA_OUT);
 	if (ret != 0) {
 		return ret;
 	}
@@ -260,11 +260,11 @@ static int spi_nor_write_bar(uint32_t offset)
 
 static int spi_nor_read_bar(void)
 {
-	uint8_t selected_bank = 0;
+	uint8_t selected_bank = 0U;
 	int ret;
 
 	ret = spi_nor_reg(nor_dev.bank_read_cmd, &selected_bank,
-			  1, SPI_MEM_DATA_IN);
+			  1U, SPI_MEM_DATA_IN);
 	if (ret != 0) {
 		return ret;
 	}
@@ -280,7 +280,7 @@ int spi_nor_read(unsigned int offset, uintptr_t buffer, size_t length,
 	size_t remain_len;
 	int ret;
 
-	*length_read = 0;
+	*length_read = 0U;
 	nor_dev.read_op.addr.val = offset;
 	nor_dev.read_op.data.buf = (void *)buffer;
 
@@ -339,7 +339,7 @@ int spi_nor_init(unsigned long long *size, unsigned int *erase_size)
 		return -EINVAL;
 	}
 
-	assert(nor_dev.size != 0);
+	assert(nor_dev.size != 0U);
 
 	if (nor_dev.size > BANK_SIZE) {
 		nor_dev.flags |= SPI_NOR_USE_BANK;
diff --git a/drivers/st/bsec/bsec.c b/drivers/st/bsec/bsec2.c
similarity index 62%
rename from drivers/st/bsec/bsec.c
rename to drivers/st/bsec/bsec2.c
index 01c369edcd..de00a65812 100644
--- a/drivers/st/bsec/bsec.c
+++ b/drivers/st/bsec/bsec2.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,11 +14,13 @@
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <drivers/st/bsec.h>
+#include <drivers/st/bsec2_reg.h>
 #include <lib/mmio.h>
 #include <lib/spinlock.h>
 
-#define BSEC_IP_VERSION_1_0	0x10
-#define BSEC_COMPAT		"st,stm32mp15-bsec"
+#define BSEC_IP_VERSION_1_1	U(0x11)
+#define BSEC_IP_VERSION_2_0	U(0x20)
+#define BSEC_IP_ID_2		U(0x100032)
 
 #define OTP_ACCESS_SIZE (round_up(OTP_MAX_SIZE, __WORD_BIT) / __WORD_BIT)
 
@@ -44,11 +46,12 @@ static void bsec_unlock(void)
 	}
 }
 
+#if defined(IMAGE_BL32)
 static int bsec_get_dt_node(struct dt_node_info *info)
 {
 	int node;
 
-	node = dt_get_node(info, -1, BSEC_COMPAT);
+	node = dt_get_node(info, -1, DT_BSEC_COMPAT);
 	if (node < 0) {
 		return -FDT_ERR_NOTFOUND;
 	}
@@ -56,7 +59,6 @@ static int bsec_get_dt_node(struct dt_node_info *info)
 	return node;
 }
 
-#if defined(IMAGE_BL32)
 static void enable_non_secure_access(uint32_t otp)
 {
 	otp_nsec_access[otp / __WORD_BIT] |= BIT(otp % __WORD_BIT);
@@ -78,39 +80,85 @@ static int bsec_dt_otp_nsec_access(void *fdt, int bsec_node)
 
 	fdt_for_each_subnode(bsec_subnode, fdt, bsec_node) {
 		const fdt32_t *cuint;
-		uint32_t reg;
+		uint32_t otp;
 		uint32_t i;
 		uint32_t size;
-		uint8_t status;
+		uint32_t offset;
+		uint32_t length;
 
 		cuint = fdt_getprop(fdt, bsec_subnode, "reg", NULL);
 		if (cuint == NULL) {
 			panic();
 		}
 
-		reg = fdt32_to_cpu(*cuint) / sizeof(uint32_t);
-		if (reg < STM32MP1_UPPER_OTP_START) {
-			continue;
+		offset = fdt32_to_cpu(*cuint);
+		cuint++;
+		length = fdt32_to_cpu(*cuint);
+
+		otp = offset / sizeof(uint32_t);
+
+		if (otp < STM32MP1_UPPER_OTP_START) {
+			unsigned int otp_end = round_up(offset + length,
+						       sizeof(uint32_t)) /
+					       sizeof(uint32_t);
+
+			if (otp_end > STM32MP1_UPPER_OTP_START) {
+				/*
+				 * OTP crosses Lower/Upper boundary, consider
+				 * only the upper part.
+				 */
+				otp = STM32MP1_UPPER_OTP_START;
+				length -= (STM32MP1_UPPER_OTP_START *
+					   sizeof(uint32_t)) - offset;
+				offset = STM32MP1_UPPER_OTP_START *
+					 sizeof(uint32_t);
+
+				WARN("OTP crosses Lower/Upper boundary\n");
+			} else {
+				continue;
+			}
 		}
 
-		status = fdt_get_status(bsec_subnode);
-		if ((status & DT_NON_SECURE) == 0U)  {
+		if ((fdt_getprop(fdt, bsec_subnode,
+				 "st,non-secure-otp", NULL)) == NULL) {
 			continue;
 		}
 
-		size = fdt32_to_cpu(*(cuint + 1)) / sizeof(uint32_t);
-
-		if ((fdt32_to_cpu(*(cuint + 1)) % sizeof(uint32_t)) != 0) {
-			size++;
+		if (((offset % sizeof(uint32_t)) != 0) ||
+		    ((length % sizeof(uint32_t)) != 0)) {
+			ERROR("Unaligned non-secure OTP\n");
+			panic();
 		}
 
-		for (i = reg; i < (reg + size); i++) {
+		size = length / sizeof(uint32_t);
+
+		for (i = otp; i < (otp + size); i++) {
 			enable_non_secure_access(i);
 		}
 	}
 
 	return 0;
 }
+
+static void bsec_late_init(void)
+{
+	void *fdt;
+	int node;
+	struct dt_node_info bsec_info;
+
+	if (fdt_get_address(&fdt) == 0) {
+		panic();
+	}
+
+	node = bsec_get_dt_node(&bsec_info);
+	if (node < 0) {
+		panic();
+	}
+
+	assert(bsec_base == bsec_info.base);
+
+	bsec_dt_otp_nsec_access(fdt, node);
+}
 #endif
 
 static uint32_t otp_bank_offset(uint32_t otp)
@@ -121,19 +169,30 @@ static uint32_t otp_bank_offset(uint32_t otp)
 	       sizeof(uint32_t);
 }
 
-static uint32_t bsec_check_error(uint32_t otp)
+/*
+ * bsec_check_error: check BSEC error status.
+ * otp: OTP number.
+ * check_disturbed: check only error (false),
+ *	or error and disturbed status (true).
+ * return value: BSEC_OK if no error.
+ */
+static uint32_t bsec_check_error(uint32_t otp, bool check_disturbed)
 {
 	uint32_t bit = BIT(otp & BSEC_OTP_MASK);
 	uint32_t bank = otp_bank_offset(otp);
 
-	if ((mmio_read_32(bsec_base + BSEC_DISTURBED_OFF + bank) & bit) != 0U) {
-		return BSEC_DISTURBED;
-	}
-
 	if ((mmio_read_32(bsec_base + BSEC_ERROR_OFF + bank) & bit) != 0U) {
 		return BSEC_ERROR;
 	}
 
+	if (!check_disturbed) {
+		return BSEC_OK;
+	}
+
+	if ((mmio_read_32(bsec_base + BSEC_DISTURBED_OFF + bank) & bit) != 0U) {
+		return BSEC_DISTURBED;
+	}
+
 	return BSEC_OK;
 }
 
@@ -143,23 +202,16 @@ static uint32_t bsec_check_error(uint32_t otp)
  */
 uint32_t bsec_probe(void)
 {
-	void *fdt;
-	int node;
-	struct dt_node_info bsec_info;
-
-	if (fdt_get_address(&fdt) == 0) {
-		panic();
-	}
+	bsec_base = BSEC_BASE;
 
-	node = bsec_get_dt_node(&bsec_info);
-	if (node < 0) {
+	if ((((bsec_get_version() & BSEC_IPVR_MSK) != BSEC_IP_VERSION_1_1) &&
+	     ((bsec_get_version() & BSEC_IPVR_MSK) != BSEC_IP_VERSION_2_0)) ||
+	    (bsec_get_id() != BSEC_IP_ID_2)) {
 		panic();
 	}
 
-	bsec_base = bsec_info.base;
-
 #if defined(IMAGE_BL32)
-	bsec_dt_otp_nsec_access(fdt, node);
+	bsec_late_init();
 #endif
 	return BSEC_OK;
 }
@@ -251,6 +303,79 @@ uint32_t bsec_get_config(struct bsec_config *cfg)
 	return BSEC_OK;
 }
 
+/*
+ * bsec_find_otp_name_in_dt: get OTP ID and length in DT.
+ * name: sub-node name to look up.
+ * otp: pointer to read OTP number or NULL.
+ * otp_len: pointer to read OTP length in bits or NULL.
+ * return value: BSEC_OK if no error.
+ */
+uint32_t bsec_find_otp_name_in_dt(const char *name, uint32_t *otp,
+				  uint32_t *otp_len)
+{
+	void *fdt;
+	int node;
+	int index, len;
+	const fdt32_t *cuint;
+
+	if ((name == NULL) || (otp == NULL)) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	if (fdt_get_address(&fdt) == 0) {
+		panic();
+	}
+
+	node = fdt_node_offset_by_compatible(fdt, -1, DT_NVMEM_LAYOUT_COMPAT);
+	if (node < 0) {
+		return BSEC_ERROR;
+	}
+
+	index = fdt_stringlist_search(fdt, node, "nvmem-cell-names", name);
+	if (index < 0) {
+		return BSEC_ERROR;
+	}
+
+	cuint = fdt_getprop(fdt, node, "nvmem-cells", &len);
+	if (cuint == NULL) {
+		return BSEC_ERROR;
+	}
+
+	if ((index * (int)sizeof(uint32_t)) > len) {
+		return BSEC_ERROR;
+	}
+
+	cuint += index;
+
+	node = fdt_node_offset_by_phandle(fdt, fdt32_to_cpu(*cuint));
+	if (node < 0) {
+		ERROR("Malformed nvmem_layout node: ignored\n");
+		return BSEC_ERROR;
+	}
+
+	cuint = fdt_getprop(fdt, node, "reg", &len);
+	if ((cuint == NULL) || (len != (2 * (int)sizeof(uint32_t)))) {
+		ERROR("Malformed nvmem_layout node: ignored\n");
+		return BSEC_ERROR;
+	}
+
+	if (fdt32_to_cpu(*cuint) % sizeof(uint32_t)) {
+		ERROR("Misaligned nvmem_layout element: ignored\n");
+		return BSEC_ERROR;
+	}
+
+	if (otp != NULL) {
+		*otp = fdt32_to_cpu(*cuint) / sizeof(uint32_t);
+	}
+
+	if (otp_len != NULL) {
+		cuint++;
+		*otp_len = fdt32_to_cpu(*cuint) * CHAR_BIT;
+	}
+
+	return BSEC_OK;
+}
+
 /*
  * bsec_shadow_register: copy SAFMEM OTP to BSEC data.
  * otp: OTP number.
@@ -259,14 +384,16 @@ uint32_t bsec_get_config(struct bsec_config *cfg)
 uint32_t bsec_shadow_register(uint32_t otp)
 {
 	uint32_t result;
+	bool value;
 	bool power_up = false;
 
-	if (otp > STM32MP1_OTP_MAX_ID) {
-		return BSEC_INVALID_PARAM;
+	result = bsec_read_sr_lock(otp, &value);
+	if (result != BSEC_OK) {
+		ERROR("BSEC: %u Sticky-read bit read Error %i\n", otp, result);
+		return result;
 	}
 
-	/* Check if shadowing of OTP is locked */
-	if (bsec_read_sr_lock(otp)) {
+	if (value) {
 		VERBOSE("BSEC: OTP %i is locked and will not be refreshed\n",
 			otp);
 	}
@@ -283,14 +410,13 @@ uint32_t bsec_shadow_register(uint32_t otp)
 
 	bsec_lock();
 
-	/* Set BSEC_OTP_CTRL_OFF and set ADDR with the OTP value */
 	mmio_write_32(bsec_base + BSEC_OTP_CTRL_OFF, otp | BSEC_READ);
 
 	while ((bsec_get_status() & BSEC_MODE_BUSY_MASK) != 0U) {
 		;
 	}
 
-	result = bsec_check_error(otp);
+	result = bsec_check_error(otp, true);
 
 	bsec_unlock();
 
@@ -311,22 +437,14 @@ uint32_t bsec_shadow_register(uint32_t otp)
  */
 uint32_t bsec_read_otp(uint32_t *val, uint32_t otp)
 {
-	uint32_t result;
-
 	if (otp > STM32MP1_OTP_MAX_ID) {
 		return BSEC_INVALID_PARAM;
 	}
 
-	bsec_lock();
-
 	*val = mmio_read_32(bsec_base + BSEC_OTP_DATA_OFF +
 			    (otp * sizeof(uint32_t)));
 
-	result = bsec_check_error(otp);
-
-	bsec_unlock();
-
-	return result;
+	return BSEC_OK;
 }
 
 /*
@@ -338,24 +456,25 @@ uint32_t bsec_read_otp(uint32_t *val, uint32_t otp)
 uint32_t bsec_write_otp(uint32_t val, uint32_t otp)
 {
 	uint32_t result;
+	bool value;
 
-	if (otp > STM32MP1_OTP_MAX_ID) {
-		return BSEC_INVALID_PARAM;
+	result = bsec_read_sw_lock(otp, &value);
+	if (result != BSEC_OK) {
+		ERROR("BSEC: %u Sticky-write bit read Error %i\n", otp, result);
+		return result;
 	}
 
-	/* Check if programming of OTP is locked */
-	if (bsec_read_sw_lock(otp)) {
+	if (value) {
 		VERBOSE("BSEC: OTP %i is locked and write will be ignored\n",
 			otp);
 	}
 
+	/* Ensure integrity of each register access sequence */
 	bsec_lock();
 
 	mmio_write_32(bsec_base + BSEC_OTP_DATA_OFF +
 		      (otp * sizeof(uint32_t)), val);
 
-	result = bsec_check_error(otp);
-
 	bsec_unlock();
 
 	return result;
@@ -372,14 +491,23 @@ uint32_t bsec_program_otp(uint32_t val, uint32_t otp)
 {
 	uint32_t result;
 	bool power_up = false;
+	bool sp_lock, perm_lock;
 
-	if (otp > STM32MP1_OTP_MAX_ID) {
-		return BSEC_INVALID_PARAM;
+	result = bsec_read_sp_lock(otp, &sp_lock);
+	if (result != BSEC_OK) {
+		ERROR("BSEC: %u Sticky-prog bit read Error %i\n", otp, result);
+		return result;
+	}
+
+	result = bsec_read_permanent_lock(otp, &perm_lock);
+	if (result != BSEC_OK) {
+		ERROR("BSEC: %u permanent bit read Error %i\n", otp, result);
+		return result;
 	}
 
-	/* Check if programming of OTP is locked */
-	if (bsec_read_sp_lock(otp)) {
+	if (sp_lock || perm_lock) {
 		WARN("BSEC: OTP locked, prog will be ignored\n");
+		return BSEC_PROG_FAIL;
 	}
 
 	if ((mmio_read_32(bsec_base + BSEC_OTP_LOCK_OFF) &
@@ -399,10 +527,8 @@ uint32_t bsec_program_otp(uint32_t val, uint32_t otp)
 
 	bsec_lock();
 
-	/* Set value in write register */
 	mmio_write_32(bsec_base + BSEC_OTP_WRDATA_OFF, val);
 
-	/* Set BSEC_OTP_CTRL_OFF and set ADDR with the OTP value */
 	mmio_write_32(bsec_base + BSEC_OTP_CTRL_OFF, otp | BSEC_WRITE);
 
 	while ((bsec_get_status() & BSEC_MODE_BUSY_MASK) != 0U) {
@@ -412,7 +538,7 @@ uint32_t bsec_program_otp(uint32_t val, uint32_t otp)
 	if ((bsec_get_status() & BSEC_MODE_PROGFAIL_MASK) != 0U) {
 		result = BSEC_PROG_FAIL;
 	} else {
-		result = bsec_check_error(otp);
+		result = bsec_check_error(otp, true);
 	}
 
 	bsec_unlock();
@@ -464,10 +590,8 @@ uint32_t bsec_permanent_lock_otp(uint32_t otp)
 
 	bsec_lock();
 
-	/* Set value in write register */
 	mmio_write_32(bsec_base + BSEC_OTP_WRDATA_OFF, data);
 
-	/* Set BSEC_OTP_CTRL_OFF and set ADDR with the OTP value */
 	mmio_write_32(bsec_base + BSEC_OTP_CTRL_OFF,
 		      addr | BSEC_WRITE | BSEC_LOCK);
 
@@ -478,7 +602,7 @@ uint32_t bsec_permanent_lock_otp(uint32_t otp)
 	if ((bsec_get_status() & BSEC_MODE_PROGFAIL_MASK) != 0U) {
 		result = BSEC_PROG_FAIL;
 	} else {
-		result = bsec_check_error(otp);
+		result = bsec_check_error(otp, false);
 	}
 
 	bsec_unlock();
@@ -493,37 +617,46 @@ uint32_t bsec_permanent_lock_otp(uint32_t otp)
 }
 
 /*
- * bsec_write_debug_conf: write value in debug feature
+ * bsec_write_debug_conf: write value in debug feature.
  *	to enable/disable debug service.
  * val: value to write.
- * return value: BSEC_OK if no error.
+ * return value: none.
  */
-uint32_t bsec_write_debug_conf(uint32_t val)
+void bsec_write_debug_conf(uint32_t val)
 {
-	uint32_t result = BSEC_ERROR;
-	uint32_t masked_val = val & BSEC_DEN_ALL_MSK;
-
 	bsec_lock();
-
-	mmio_write_32(bsec_base + BSEC_DEN_OFF, masked_val);
-
-	if ((mmio_read_32(bsec_base + BSEC_DEN_OFF) ^ masked_val) == 0U) {
-		result = BSEC_OK;
-	}
-
+	mmio_write_32(bsec_base + BSEC_DEN_OFF, val & BSEC_DEN_ALL_MSK);
 	bsec_unlock();
-
-	return result;
 }
 
 /*
- * bsec_read_debug_conf: read debug configuration.
+ * bsec_read_debug_conf: return debug configuration register value.
  */
 uint32_t bsec_read_debug_conf(void)
 {
 	return mmio_read_32(bsec_base + BSEC_DEN_OFF);
 }
 
+/*
+ * bsec_write_scratch: write value in scratch register.
+ * val: value to write.
+ * return value: none.
+ */
+void bsec_write_scratch(uint32_t val)
+{
+	bsec_lock();
+	mmio_write_32(bsec_base + BSEC_SCRATCH_OFF, val);
+	bsec_unlock();
+}
+
+/*
+ * bsec_read_scratch: return scratch register value.
+ */
+uint32_t bsec_read_scratch(void)
+{
+	return mmio_read_32(bsec_base + BSEC_SCRATCH_OFF);
+}
+
 /*
  * bsec_get_status: return status register value.
  */
@@ -533,7 +666,7 @@ uint32_t bsec_get_status(void)
 }
 
 /*
- * bsec_get_hw_conf: return hardware configuration.
+ * bsec_get_hw_conf: return hardware configuration register value.
  */
 uint32_t bsec_get_hw_conf(void)
 {
@@ -541,7 +674,7 @@ uint32_t bsec_get_hw_conf(void)
 }
 
 /*
- * bsec_get_version: return BSEC version.
+ * bsec_get_version: return BSEC version register value.
  */
 uint32_t bsec_get_version(void)
 {
@@ -549,7 +682,7 @@ uint32_t bsec_get_version(void)
 }
 
 /*
- * bsec_get_id: return BSEC ID.
+ * bsec_get_id: return BSEC ID register value.
  */
 uint32_t bsec_get_id(void)
 {
@@ -557,7 +690,7 @@ uint32_t bsec_get_id(void)
 }
 
 /*
- * bsec_get_magic_id: return BSEC magic number.
+ * bsec_get_magic_id: return BSEC magic number register value.
  */
 uint32_t bsec_get_magic_id(void)
 {
@@ -565,229 +698,178 @@ uint32_t bsec_get_magic_id(void)
 }
 
 /*
- * bsec_write_sr_lock: write shadow-read lock.
+ * bsec_set_sr_lock: set shadow-read lock.
  * otp: OTP number.
- * value: value to write in the register.
- *	Must be always 1.
- * return: true if OTP is locked, else false.
+ * return value: BSEC_OK if no error.
  */
-bool bsec_write_sr_lock(uint32_t otp, uint32_t value)
+uint32_t bsec_set_sr_lock(uint32_t otp)
 {
-	bool result = false;
 	uint32_t bank = otp_bank_offset(otp);
-	uint32_t bank_value;
 	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
 
-	bsec_lock();
-
-	bank_value = mmio_read_32(bsec_base + BSEC_SRLOCK_OFF + bank);
-
-	if ((bank_value & otp_mask) == value) {
-		/*
-		 * In case of write don't need to write,
-		 * the lock is already set.
-		 */
-		if (value != 0U) {
-			result = true;
-		}
-	} else {
-		if (value != 0U) {
-			bank_value = bank_value | otp_mask;
-		} else {
-			bank_value = bank_value & ~otp_mask;
-		}
-
-		/*
-		 * We can write 0 in all other OTP
-		 * if the lock is activated in one of other OTP.
-		 * Write 0 has no effect.
-		 */
-		mmio_write_32(bsec_base + BSEC_SRLOCK_OFF + bank, bank_value);
-		result = true;
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
 	}
 
+	bsec_lock();
+	mmio_write_32(bsec_base + BSEC_SRLOCK_OFF + bank, otp_mask);
 	bsec_unlock();
 
-	return result;
+	return BSEC_OK;
 }
 
 /*
  * bsec_read_sr_lock: read shadow-read lock.
  * otp: OTP number.
- * return: true if otp is locked, else false.
+ * value: read value (true or false).
+ * return value: BSEC_OK if no error.
  */
-bool bsec_read_sr_lock(uint32_t otp)
+uint32_t bsec_read_sr_lock(uint32_t otp, bool *value)
 {
 	uint32_t bank = otp_bank_offset(otp);
 	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
-	uint32_t bank_value = mmio_read_32(bsec_base + BSEC_SRLOCK_OFF + bank);
+	uint32_t bank_value;
+
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bank_value = mmio_read_32(bsec_base + BSEC_SRLOCK_OFF + bank);
+
+	*value = ((bank_value & otp_mask) != 0U);
 
-	return (bank_value & otp_mask) != 0U;
+	return BSEC_OK;
 }
 
 /*
- * bsec_write_sw_lock: write shadow-write lock.
+ * bsec_set_sw_lock: set shadow-write lock.
  * otp: OTP number.
- * value: Value to write in the register.
- *	Must be always 1.
- * return: true if OTP is locked, else false.
+ * return value: BSEC_OK if no error.
  */
-bool bsec_write_sw_lock(uint32_t otp, uint32_t value)
+uint32_t bsec_set_sw_lock(uint32_t otp)
 {
-	bool result = false;
 	uint32_t bank = otp_bank_offset(otp);
 	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
-	uint32_t bank_value;
-
-	bsec_lock();
 
-	bank_value = mmio_read_32(bsec_base + BSEC_SWLOCK_OFF + bank);
-
-	if ((bank_value & otp_mask) == value) {
-		/*
-		 * In case of write don't need to write,
-		 * the lock is already set.
-		 */
-		if (value != 0U) {
-			result = true;
-		}
-	} else {
-		if (value != 0U) {
-			bank_value = bank_value | otp_mask;
-		} else {
-			bank_value = bank_value & ~otp_mask;
-		}
-
-		/*
-		 * We can write 0 in all other OTP
-		 * if the lock is activated in one of other OTP.
-		 * Write 0 has no effect.
-		 */
-		mmio_write_32(bsec_base + BSEC_SWLOCK_OFF + bank, bank_value);
-		result = true;
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
 	}
 
+	bsec_lock();
+	mmio_write_32(bsec_base + BSEC_SWLOCK_OFF + bank, otp_mask);
 	bsec_unlock();
 
-	return result;
+	return BSEC_OK;
 }
 
 /*
  * bsec_read_sw_lock: read shadow-write lock.
  * otp: OTP number.
- * return: true if OTP is locked, else false.
+ * value: read value (true or false).
+ * return value: BSEC_OK if no error.
  */
-bool bsec_read_sw_lock(uint32_t otp)
+uint32_t bsec_read_sw_lock(uint32_t otp, bool *value)
 {
 	uint32_t bank = otp_bank_offset(otp);
 	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
-	uint32_t bank_value = mmio_read_32(bsec_base + BSEC_SWLOCK_OFF + bank);
+	uint32_t bank_value;
 
-	return (bank_value & otp_mask) != 0U;
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bank_value = mmio_read_32(bsec_base + BSEC_SWLOCK_OFF + bank);
+
+	*value = ((bank_value & otp_mask) != 0U);
+
+	return BSEC_OK;
 }
 
 /*
- * bsec_write_sp_lock: write shadow-program lock.
+ * bsec_set_sp_lock: set shadow-program lock.
  * otp: OTP number.
- * value: Value to write in the register.
- *	Must be always 1.
- * return: true if OTP is locked, else false.
+ * return value: BSEC_OK if no error.
  */
-bool bsec_write_sp_lock(uint32_t otp, uint32_t value)
+uint32_t bsec_set_sp_lock(uint32_t otp)
 {
-	bool result = false;
 	uint32_t bank = otp_bank_offset(otp);
-	uint32_t bank_value;
 	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
 
-	bsec_lock();
-
-	bank_value = mmio_read_32(bsec_base + BSEC_SPLOCK_OFF + bank);
-
-	if ((bank_value & otp_mask) == value) {
-		/*
-		 * In case of write don't need to write,
-		 * the lock is already set.
-		 */
-		if (value != 0U) {
-			result = true;
-		}
-	} else {
-		if (value != 0U) {
-			bank_value = bank_value | otp_mask;
-		} else {
-			bank_value = bank_value & ~otp_mask;
-		}
-
-		/*
-		 * We can write 0 in all other OTP
-		 * if the lock is activated in one of other OTP.
-		 * Write 0 has no effect.
-		 */
-		mmio_write_32(bsec_base + BSEC_SPLOCK_OFF + bank, bank_value);
-		result = true;
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
 	}
 
+	bsec_lock();
+	mmio_write_32(bsec_base + BSEC_SPLOCK_OFF + bank, otp_mask);
 	bsec_unlock();
 
-	return result;
+	return BSEC_OK;
 }
 
 /*
  * bsec_read_sp_lock: read shadow-program lock.
  * otp: OTP number.
- * return: true if OTP is locked, else false.
+ * value: read value (true or false).
+ * return value: BSEC_OK if no error.
  */
-bool bsec_read_sp_lock(uint32_t otp)
+uint32_t bsec_read_sp_lock(uint32_t otp, bool *value)
 {
 	uint32_t bank = otp_bank_offset(otp);
 	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
-	uint32_t bank_value = mmio_read_32(bsec_base + BSEC_SPLOCK_OFF + bank);
+	uint32_t bank_value;
+
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
+	}
+
+	bank_value = mmio_read_32(bsec_base + BSEC_SPLOCK_OFF + bank);
+
+	*value = ((bank_value & otp_mask) != 0U);
 
-	return (bank_value & otp_mask) != 0U;
+	return BSEC_OK;
 }
 
 /*
- * bsec_wr_lock: Read permanent lock status.
+ * bsec_read_permanent_lock: Read permanent lock status.
  * otp: OTP number.
- * return: true if OTP is locked, else false.
+ * value: read value (true or false).
+ * return value: BSEC_OK if no error.
  */
-bool bsec_wr_lock(uint32_t otp)
+uint32_t bsec_read_permanent_lock(uint32_t otp, bool *value)
 {
 	uint32_t bank = otp_bank_offset(otp);
-	uint32_t lock_bit = BIT(otp & BSEC_OTP_MASK);
+	uint32_t otp_mask = BIT(otp & BSEC_OTP_MASK);
+	uint32_t bank_value;
 
-	if ((mmio_read_32(bsec_base + BSEC_WRLOCK_OFF + bank) &
-	     lock_bit) != 0U) {
-		/*
-		 * In case of write don't need to write,
-		 * the lock is already set.
-		 */
-		return true;
+	if (otp > STM32MP1_OTP_MAX_ID) {
+		return BSEC_INVALID_PARAM;
 	}
 
-	return false;
+	bank_value = mmio_read_32(bsec_base + BSEC_WRLOCK_OFF + bank);
+
+	*value = ((bank_value & otp_mask) != 0U);
+
+	return BSEC_OK;
 }
 
 /*
- * bsec_otp_lock: Lock Upper OTP or Global programming or debug enable
- * service: Service to lock see header file.
- * value: Value to write must always set to 1 (only use for debug purpose).
- * return: BSEC_OK if succeed.
+ * bsec_otp_lock: Lock Upper OTP or Global Programming or Debug Enable.
+ * service: Service to lock, see header file.
+ * return value: BSEC_OK if no error.
  */
-uint32_t bsec_otp_lock(uint32_t service, uint32_t value)
+uint32_t bsec_otp_lock(uint32_t service)
 {
 	uintptr_t reg = bsec_base + BSEC_OTP_LOCK_OFF;
 
 	switch (service) {
 	case BSEC_LOCK_UPPER_OTP:
-		mmio_write_32(reg, value << BSEC_LOCK_UPPER_OTP);
+		mmio_write_32(reg, BIT(BSEC_LOCK_UPPER_OTP));
 		break;
 	case BSEC_LOCK_DEBUG:
-		mmio_write_32(reg, value << BSEC_LOCK_DEBUG);
+		mmio_write_32(reg, BIT(BSEC_LOCK_DEBUG));
 		break;
 	case BSEC_LOCK_PROGRAM:
-		mmio_write_32(reg, value << BSEC_LOCK_PROGRAM);
+		mmio_write_32(reg, BIT(BSEC_LOCK_PROGRAM));
 		break;
 	default:
 		return BSEC_INVALID_PARAM;
@@ -799,7 +881,7 @@ uint32_t bsec_otp_lock(uint32_t service, uint32_t value)
 /*
  * bsec_power_safmem: Activate or deactivate SAFMEM power.
  * power: true to power up, false to power down.
- * return: BSEC_OK if succeed.
+ * return value: BSEC_OK if no error.
  */
 static uint32_t bsec_power_safmem(bool power)
 {
@@ -818,7 +900,6 @@ static uint32_t bsec_power_safmem(bool power)
 
 	mmio_write_32(bsec_base + BSEC_OTP_CONF_OFF, register_val);
 
-	/* Waiting loop */
 	if (power) {
 		while (((bsec_get_status() & BSEC_MODE_PWR_MASK) == 0U) &&
 		       (timeout != 0U)) {
@@ -841,7 +922,7 @@ static uint32_t bsec_power_safmem(bool power)
 }
 
 /*
- * bsec_shadow_read_otp: Load OTP from SAFMEM and provide its value
+ * bsec_shadow_read_otp: Load OTP from SAFMEM and provide its value.
  * otp_value: read value.
  * word: OTP number.
  * return value: BSEC_OK if no error.
@@ -867,7 +948,7 @@ uint32_t bsec_shadow_read_otp(uint32_t *otp_value, uint32_t word)
 /*
  * bsec_check_nsec_access_rights: check non-secure access rights to target OTP.
  * otp: OTP number.
- * return: BSEC_OK if authorized access.
+ * return value: BSEC_OK if authorized access.
  */
 uint32_t bsec_check_nsec_access_rights(uint32_t otp)
 {
@@ -877,11 +958,8 @@ uint32_t bsec_check_nsec_access_rights(uint32_t otp)
 	}
 
 	if (otp >= STM32MP1_UPPER_OTP_START) {
-		/* Check if BSEC is in OTP-SECURED closed_device state. */
-		if (stm32mp_is_closed_device()) {
-			if (!non_secure_can_access(otp)) {
-				return BSEC_ERROR;
-			}
+		if (!non_secure_can_access(otp)) {
+			return BSEC_ERROR;
 		}
 	}
 #endif
diff --git a/drivers/st/clk/stm32mp1_calib.c b/drivers/st/clk/stm32mp1_calib.c
new file mode 100644
index 0000000000..11a334303c
--- /dev/null
+++ b/drivers/st/clk/stm32mp1_calib.c
@@ -0,0 +1,536 @@
+/*
+ * Copyright (C) 2019-2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <limits.h>
+#include <stdint.h>
+#include <stdio.h>
+
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/generic_delay_timer.h>
+#include <drivers/st/stm32_timer.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <lib/mmio.h>
+#include <lib/spinlock.h>
+#include <lib/utils_def.h>
+#include <plat/common/platform.h>
+
+#define TIMEOUT_10MS	10000
+#define CALIB_TIMEOUT	TIMEOUT_10MS
+
+struct stm32mp1_trim_boundary_t {
+	/* Max boundary trim value around forbidden value */
+	unsigned int x1;
+	/* Min boundary trim value around forbidden value */
+	unsigned int x2;
+};
+
+struct stm32mp1_clk_cal {
+	uint16_t *fbv;
+	unsigned int cal_ref;
+	int trim_max;
+	int trim_min;
+	unsigned int boundary_max;
+	unsigned long ref_freq;
+	unsigned int freq_margin;
+	unsigned long (*get_freq)(void);
+	void (*set_trim)(unsigned int cal);
+	unsigned int (*get_trim)(void);
+	struct stm32mp1_trim_boundary_t boundary[16];
+};
+
+/* RCC Wakeup status */
+static bool rcc_wakeup;
+
+/* List of forbiden values for HSI */
+static uint16_t fbv_hsi[] = {
+	512,
+	480,
+	448,
+	416,
+	384,
+	352,
+	320,
+	288,
+	256,
+	224,
+	192,
+	160,
+	128,
+	96,
+	64,
+	32,
+	0
+};
+
+/* List of forbiden values for CSI */
+static uint16_t fbv_csi[] = {
+	256,
+	240,
+	224,
+	208,
+	192,
+	176,
+	160,
+	144,
+	128,
+	112,
+	96,
+	80,
+	64,
+	48,
+	32,
+	16,
+	0
+};
+
+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 stm32mp1_clk_cal_hsi = {
+	.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 struct stm32mp1_clk_cal stm32mp1_clk_cal_csi = {
+	.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 uint32_t timer_val;
+
+/*
+ * HSI Calibration part
+ */
+static int get_signed_value(uint8_t val)
+{
+	return ((int8_t)(val << 1)) >> 1;
+}
+
+static void hsi_set_trim(unsigned int cal)
+{
+	int clk_trim = (int)cal - (int)stm32mp1_clk_cal_hsi.cal_ref;
+	uint32_t trim = ((uint32_t)clk_trim << RCC_HSICFGR_HSITRIM_SHIFT) &
+			RCC_HSICFGR_HSITRIM_MASK;
+
+	mmio_clrsetbits_32(stm32mp_rcc_base() + RCC_HSICFGR,
+			   RCC_HSICFGR_HSITRIM_MASK, trim);
+}
+
+static unsigned int hsi_get_trimed_cal(void)
+{
+	uint32_t utrim = (mmio_read_32(stm32mp_rcc_base() + RCC_HSICFGR) &
+			  RCC_HSICFGR_HSITRIM_MASK) >>
+			 RCC_HSICFGR_HSITRIM_SHIFT;
+
+	int trim = get_signed_value((uint8_t)utrim);
+
+	if (trim + (int)stm32mp1_clk_cal_hsi.cal_ref < 0) {
+		return 0;
+	}
+
+	return stm32mp1_clk_cal_hsi.cal_ref + trim;
+}
+
+static void csi_set_trim(unsigned int cal)
+{
+	int clk_trim = (int)cal - (int)stm32mp1_clk_cal_csi.cal_ref +
+		       stm32mp1_clk_cal_csi.trim_max + 1;
+	uint32_t trim = ((uint32_t)clk_trim << RCC_CSICFGR_CSITRIM_SHIFT) &
+			RCC_CSICFGR_CSITRIM_MASK;
+
+	mmio_clrsetbits_32(stm32mp_rcc_base() + RCC_CSICFGR,
+			   RCC_CSICFGR_CSITRIM_MASK, trim);
+}
+
+static unsigned int csi_get_trimed_cal(void)
+{
+	uint32_t trim = (mmio_read_32(stm32mp_rcc_base() + RCC_CSICFGR) &
+			 RCC_CSICFGR_CSITRIM_MASK) >>
+			RCC_CSICFGR_CSITRIM_SHIFT;
+
+	return (int)trim - stm32mp1_clk_cal_csi.trim_max +
+		(int)stm32mp1_clk_cal_csi.cal_ref - 1;
+}
+
+static unsigned int trim_increase(struct stm32mp1_clk_cal *clk_cal,
+				  unsigned int cal)
+{
+	struct stm32mp1_trim_boundary_t *boundary;
+	unsigned int new_cal;
+	int i;
+
+	/* By default: last calibration value */
+	new_cal = cal;
+
+	/* Start from Lowest cal value */
+	for (i = (int)clk_cal->boundary_max - 1; i >= 0; i--) {
+		boundary = &clk_cal->boundary[i];
+
+		if (cal < boundary->x2) {
+			new_cal = boundary->x2;
+			break;
+		}
+
+		if ((cal >= boundary->x2) && (cal < boundary->x1)) {
+			new_cal = cal + 1;
+			break;
+		}
+	}
+
+	return new_cal;
+}
+
+static unsigned int trim_decrease(struct stm32mp1_clk_cal *clk_cal,
+				  unsigned int cal)
+{
+	struct stm32mp1_trim_boundary_t *boundary;
+	unsigned int new_cal;
+	unsigned int i;
+
+	/* By default: last calibration value */
+	new_cal = cal;
+
+	/* Start from Highest cal value */
+	for (i = 0; i < clk_cal->boundary_max; i++) {
+		boundary = &clk_cal->boundary[i];
+
+		if (cal > boundary->x1) {
+			new_cal = boundary->x1;
+			break;
+		}
+
+		if ((cal > boundary->x2) && (cal <= boundary->x1)) {
+			new_cal = cal - 1;
+			break;
+		}
+	}
+
+	return new_cal;
+}
+
+static void rcc_calibration(struct stm32mp1_clk_cal *clk_cal)
+{
+	unsigned long freq = clk_cal->get_freq();
+	unsigned long min = clk_cal->ref_freq -
+		((clk_cal->ref_freq * clk_cal->freq_margin) / 1000);
+	unsigned long max = clk_cal->ref_freq +
+		((clk_cal->ref_freq * clk_cal->freq_margin) / 1000);
+	int trim, new_trim;
+	unsigned long conv;
+	unsigned long min_conv = ULONG_MAX;
+	uint64_t start;
+
+	if ((freq >= min) && (freq <= max)) {
+		return;
+	}
+
+	trim = clk_cal->get_trim();
+	start = timeout_init_us(CALIB_TIMEOUT);
+	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 == 0U) {
+			/* 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(start)) {
+			break;
+		}
+
+	} while (conv == min_conv);
+
+	clk_cal->set_trim(trim);
+	freq = clk_cal->get_freq();
+
+	if ((freq < min) || (freq > max)) {
+		ERROR("%s Calibration : Freq %lu, trim %i\n",
+		      (clk_cal->set_trim == hsi_set_trim) ? "HSI" : "CSI",
+		      freq, trim);
+#if DEBUG
+		/*
+		 * Show the steps around the selected trim value
+		 * to correct the margin if needed
+		 */
+		new_trim = trim_decrease(clk_cal, trim);
+		clk_cal->set_trim(new_trim);
+		ERROR("%s Calibration : Freq %lu, trim %i\n",
+		      (clk_cal->set_trim == hsi_set_trim) ?
+		      "HSI" : "CSI", clk_cal->get_freq(), new_trim);
+
+		new_trim = trim_increase(clk_cal, trim);
+		clk_cal->set_trim(new_trim);
+		ERROR("%s Calibration : Freq %lu, trim %i\n",
+		      (clk_cal->set_trim == hsi_set_trim) ?
+		      "HSI" : "CSI", clk_cal->get_freq(), new_trim);
+#endif
+	}
+}
+
+static void save_trim(struct stm32mp1_clk_cal *clk_cal,
+		      unsigned int i, unsigned int x1, unsigned int x2)
+{
+	clk_cal->boundary[i].x1 = x1;
+	clk_cal->boundary[i].x2 = x2;
+}
+
+static int trim_find_prev_boundary(struct stm32mp1_clk_cal *clk_cal,
+				   unsigned int x1)
+{
+	unsigned int x = x1;
+	unsigned long freq;
+
+	clk_cal->set_trim(x1 + 1);
+	freq = clk_cal->get_freq();
+
+	while (x >= (clk_cal->cal_ref + clk_cal->trim_min)) {
+		x--;
+		clk_cal->set_trim(x);
+
+		if (clk_cal->get_freq() <= freq) {
+			break;
+		}
+	};
+
+	return x;
+}
+
+static void trim_table_init(struct stm32mp1_clk_cal *clk_cal)
+{
+	uint16_t *trim_fbv = clk_cal->fbv;
+	unsigned int min;
+	unsigned int max;
+	int boundary = 0;
+	int i = 0;
+
+	max = clk_cal->cal_ref + clk_cal->trim_max;
+	min = clk_cal->cal_ref + clk_cal->trim_min;
+
+	while (trim_fbv[i]) {
+		unsigned int x;
+		unsigned int x1 = trim_fbv[i];
+		unsigned int x2 = trim_fbv[i + 1];
+
+		if ((max <= x2) || (min >= x1)) {
+			i++;
+			if (boundary != 0) {
+				goto out;
+			}
+			continue;
+		}
+
+		/* Take forbiden value + 1 */
+		x2 = x2 + 1;
+		if (x2 < min) {
+			x2 = min;
+		}
+
+		if (boundary == 0) {
+			/* Save first boundary */
+			save_trim(clk_cal, boundary, max, x2);
+			boundary++;
+			i++;
+			continue;
+		}
+
+		x = trim_find_prev_boundary(clk_cal, x1);
+		/* Save boundary values */
+		save_trim(clk_cal, boundary, x - 1, x2);
+		boundary++;
+		i++;
+	};
+out:
+	clk_cal->boundary_max = boundary;
+}
+
+bool stm32mp1_calib_get_wakeup(void)
+{
+	return rcc_wakeup;
+}
+
+void stm32mp1_calib_set_wakeup(bool state)
+{
+	rcc_wakeup = state;
+}
+
+void stm32mp1_calib_it_handler(uint32_t id)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	switch (id) {
+	case STM32MP1_IRQ_RCC_WAKEUP:
+		plat_ic_set_priority_mask(GIC_HIGHEST_NS_PRIORITY);
+		mmio_setbits_32(rcc_base + RCC_MP_CIFR, RCC_MP_CIFR_WKUPF);
+		stm32mp1_calib_set_wakeup(true);
+		return;
+
+	case STM32MP1_IRQ_MCU_SEV:
+		stm32mp1_calib_set_wakeup(false);
+		if ((mmio_read_32(EXTI_BASE + EXTI_RPR3) &
+		     EXTI_RPR3_RPIF65) != 0U) {
+			mmio_setbits_32(EXTI_BASE + EXTI_RPR3,
+					EXTI_RPR3_RPIF65);
+		}
+
+		if ((mmio_read_32(EXTI_BASE + EXTI_FPR3) &
+		     EXTI_FPR3_FPIF65) != 0U) {
+			mmio_setbits_32(EXTI_BASE + EXTI_FPR3,
+					EXTI_FPR3_FPIF65);
+		}
+
+		break;
+
+	case ARM_IRQ_SEC_PHY_TIMER:
+	default:
+		break;
+	}
+
+	if (stm32mp1_clk_cal_hsi.ref_freq != 0U) {
+		rcc_calibration(&stm32mp1_clk_cal_hsi);
+	}
+
+	if (stm32mp1_clk_cal_csi.ref_freq != 0U) {
+		rcc_calibration(&stm32mp1_clk_cal_csi);
+	}
+
+	if (timer_val != 0U) {
+		write_cntp_tval(timer_val);
+	}
+}
+
+int stm32mp1_calib_start_hsi_cal(void)
+{
+	if (stm32mp1_clk_cal_hsi.ref_freq == 0U) {
+		return -ENOENT;
+	}
+
+	rcc_calibration(&stm32mp1_clk_cal_hsi);
+	return 0;
+}
+
+int stm32mp1_calib_start_csi_cal(void)
+{
+	if (stm32mp1_clk_cal_csi.ref_freq == 0U) {
+		return -ENOENT;
+	}
+
+	rcc_calibration(&stm32mp1_clk_cal_csi);
+	return 0;
+}
+
+static void init_hsi_cal(void)
+{
+	int len;
+
+	if (fdt_rcc_read_prop("st,hsi-cal", &len) == NULL) {
+		return;
+	}
+
+	stm32_timer_freq_func(&stm32mp1_clk_cal_hsi.get_freq, HSI_CAL);
+	if (stm32mp1_clk_cal_hsi.get_freq == NULL) {
+		return;
+	}
+
+	stm32mp1_clk_cal_hsi.ref_freq = clk_get_rate(CK_HSI);
+
+	/* Read initial value */
+	stm32mp1_clk_cal_hsi.cal_ref =
+		((mmio_read_32(stm32mp_rcc_base() + RCC_HSICFGR)
+		  & RCC_HSICFGR_HSICAL_MASK) >> RCC_HSICFGR_HSICAL_SHIFT);
+
+	trim_table_init(&stm32mp1_clk_cal_hsi);
+
+	stm32mp1_clk_cal_hsi.set_trim(stm32mp1_clk_cal_hsi.cal_ref);
+
+	rcc_calibration(&stm32mp1_clk_cal_hsi);
+}
+
+static void init_csi_cal(void)
+{
+	int len;
+
+	if (fdt_rcc_read_prop("st,csi-cal", &len) == NULL) {
+		return;
+	}
+
+	stm32_timer_freq_func(&stm32mp1_clk_cal_csi.get_freq, CSI_CAL);
+	if (stm32mp1_clk_cal_csi.get_freq == NULL) {
+		return;
+	}
+
+	stm32mp1_clk_cal_csi.ref_freq = clk_get_rate(CK_CSI);
+
+	/* Read initial value */
+	stm32mp1_clk_cal_csi.cal_ref =
+		((mmio_read_32(stm32mp_rcc_base() + RCC_CSICFGR) &
+		  RCC_CSICFGR_CSICAL_MASK) >> RCC_CSICFGR_CSICAL_SHIFT);
+
+	trim_table_init(&stm32mp1_clk_cal_csi);
+
+	stm32mp1_clk_cal_csi.set_trim(stm32mp1_clk_cal_csi.cal_ref);
+
+	rcc_calibration(&stm32mp1_clk_cal_csi);
+}
+
+void stm32mp1_calib_init(void)
+{
+	init_hsi_cal();
+	init_csi_cal();
+
+	timer_val = fdt_rcc_read_uint32_default("st,cal-sec", 0) *
+		plat_get_syscnt_freq2();
+
+	if (timer_val > INT32_MAX) {
+		timer_val = INT32_MAX;
+	}
+
+	if (timer_val != 0U) {
+		/* Load & enable timer */
+		INFO("Set calibration timer to %u sec\n",
+		     timer_val / plat_get_syscnt_freq2());
+		write_cntp_tval(timer_val);
+		write_cntp_ctl(BIT(0));
+	};
+
+	if (fdt_rcc_enable_it("mcu_sev") < 0) {
+		VERBOSE("No MCU calibration\n");
+	}
+}
diff --git a/drivers/st/clk/stm32mp1_clk.c b/drivers/st/clk/stm32mp1_clk.c
index 564bd87989..00954cb2e8 100644
--- a/drivers/st/clk/stm32mp1_clk.c
+++ b/drivers/st/clk/stm32mp1_clk.c
@@ -6,6 +6,7 @@
 
 #include <assert.h>
 #include <errno.h>
+#include <limits.h>
 #include <stdint.h>
 #include <stdio.h>
 
@@ -17,8 +18,9 @@
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
-#include <drivers/generic_delay_timer.h>
+#include <drivers/st/stm32_timer.h>
 #include <drivers/st/stm32mp_clkfunc.h>
 #include <drivers/st/stm32mp1_clk.h>
 #include <drivers/st/stm32mp1_rcc.h>
@@ -49,6 +51,19 @@ const char *stm32mp_osc_node_label[NB_OSC] = {
 	[_I2S_CKIN] = "i2s_ckin",
 };
 
+/* 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
+
 enum stm32mp1_parent_id {
 /* Oscillators are defined in enum stm32mp_osc_id */
 
@@ -239,6 +254,7 @@ struct stm32mp1_clk_gate {
 	uint8_t bit;
 	uint8_t index;
 	uint8_t set_clr;
+	uint8_t secure;
 	uint8_t sel; /* Relates to enum stm32mp1_parent_sel */
 	uint8_t fixed; /* Relates to enum stm32mp1_parent_id */
 };
@@ -263,46 +279,59 @@ struct stm32mp1_clk_pll {
 	enum stm32mp_osc_id refclk[REFCLK_SIZE];
 };
 
+/* 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];
+};
+
 /* Clocks with selectable source and non set/clr register access */
-#define _CLK_SELEC(off, b, idx, s)			\
+#define _CLK_SELEC(sec, off, b, idx, s)			\
 	{						\
 		.offset = (off),			\
 		.bit = (b),				\
 		.index = (idx),				\
 		.set_clr = 0,				\
+		.secure = (sec),			\
 		.sel = (s),				\
 		.fixed = _UNKNOWN_ID,			\
 	}
 
 /* Clocks with fixed source and non set/clr register access */
-#define _CLK_FIXED(off, b, idx, f)			\
+#define _CLK_FIXED(sec, off, b, idx, f)			\
 	{						\
 		.offset = (off),			\
 		.bit = (b),				\
 		.index = (idx),				\
 		.set_clr = 0,				\
+		.secure = (sec),			\
 		.sel = _UNKNOWN_SEL,			\
 		.fixed = (f),				\
 	}
 
 /* Clocks with selectable source and set/clr register access */
-#define _CLK_SC_SELEC(off, b, idx, s)			\
+#define _CLK_SC_SELEC(sec, off, b, idx, s)			\
 	{						\
 		.offset = (off),			\
 		.bit = (b),				\
 		.index = (idx),				\
 		.set_clr = 1,				\
+		.secure = (sec),			\
 		.sel = (s),				\
 		.fixed = _UNKNOWN_ID,			\
 	}
 
 /* Clocks with fixed source and set/clr register access */
-#define _CLK_SC_FIXED(off, b, idx, f)			\
+#define _CLK_SC_FIXED(sec, off, b, idx, f)			\
 	{						\
 		.offset = (off),			\
 		.bit = (b),				\
 		.index = (idx),				\
 		.set_clr = 1,				\
+		.secure = (sec),			\
 		.sel = _UNKNOWN_SEL,			\
 		.fixed = (f),				\
 	}
@@ -336,81 +365,105 @@ struct stm32mp1_clk_pll {
 
 #define NB_GATES	ARRAY_SIZE(stm32mp1_clk_gate)
 
+#define SEC		1
+#define N_S		0
+
 static const struct stm32mp1_clk_gate stm32mp1_clk_gate[] = {
-	_CLK_FIXED(RCC_DDRITFCR, 0, DDRC1, _ACLK),
-	_CLK_FIXED(RCC_DDRITFCR, 1, DDRC1LP, _ACLK),
-	_CLK_FIXED(RCC_DDRITFCR, 2, DDRC2, _ACLK),
-	_CLK_FIXED(RCC_DDRITFCR, 3, DDRC2LP, _ACLK),
-	_CLK_FIXED(RCC_DDRITFCR, 4, DDRPHYC, _PLL2_R),
-	_CLK_FIXED(RCC_DDRITFCR, 5, DDRPHYCLP, _PLL2_R),
-	_CLK_FIXED(RCC_DDRITFCR, 6, DDRCAPB, _PCLK4),
-	_CLK_FIXED(RCC_DDRITFCR, 7, DDRCAPBLP, _PCLK4),
-	_CLK_FIXED(RCC_DDRITFCR, 8, AXIDCG, _ACLK),
-	_CLK_FIXED(RCC_DDRITFCR, 9, DDRPHYCAPB, _PCLK4),
-	_CLK_FIXED(RCC_DDRITFCR, 10, DDRPHYCAPBLP, _PCLK4),
-
-	_CLK_SC_FIXED(RCC_MP_APB1ENSETR, 6, TIM12_K, _PCLK1),
-	_CLK_SC_SELEC(RCC_MP_APB1ENSETR, 14, USART2_K, _UART24_SEL),
-	_CLK_SC_SELEC(RCC_MP_APB1ENSETR, 15, USART3_K, _UART35_SEL),
-	_CLK_SC_SELEC(RCC_MP_APB1ENSETR, 16, UART4_K, _UART24_SEL),
-	_CLK_SC_SELEC(RCC_MP_APB1ENSETR, 17, UART5_K, _UART35_SEL),
-	_CLK_SC_SELEC(RCC_MP_APB1ENSETR, 18, UART7_K, _UART78_SEL),
-	_CLK_SC_SELEC(RCC_MP_APB1ENSETR, 19, UART8_K, _UART78_SEL),
-	_CLK_SC_SELEC(RCC_MP_APB1ENSETR, 21, I2C1_K, _I2C12_SEL),
-	_CLK_SC_SELEC(RCC_MP_APB1ENSETR, 22, I2C2_K, _I2C12_SEL),
-	_CLK_SC_SELEC(RCC_MP_APB1ENSETR, 23, I2C3_K, _I2C35_SEL),
-	_CLK_SC_SELEC(RCC_MP_APB1ENSETR, 24, I2C5_K, _I2C35_SEL),
-
-	_CLK_SC_FIXED(RCC_MP_APB2ENSETR, 2, TIM15_K, _PCLK2),
-	_CLK_SC_SELEC(RCC_MP_APB2ENSETR, 13, USART6_K, _UART6_SEL),
-
-	_CLK_SC_FIXED(RCC_MP_APB3ENSETR, 11, SYSCFG, _UNKNOWN_ID),
-
-	_CLK_SC_SELEC(RCC_MP_APB4ENSETR, 8, DDRPERFM, _UNKNOWN_SEL),
-	_CLK_SC_SELEC(RCC_MP_APB4ENSETR, 15, IWDG2, _UNKNOWN_SEL),
-	_CLK_SC_SELEC(RCC_MP_APB4ENSETR, 16, USBPHY_K, _USBPHY_SEL),
-
-	_CLK_SC_SELEC(RCC_MP_APB5ENSETR, 0, SPI6_K, _SPI6_SEL),
-	_CLK_SC_SELEC(RCC_MP_APB5ENSETR, 2, I2C4_K, _I2C46_SEL),
-	_CLK_SC_SELEC(RCC_MP_APB5ENSETR, 3, I2C6_K, _I2C46_SEL),
-	_CLK_SC_SELEC(RCC_MP_APB5ENSETR, 4, USART1_K, _UART1_SEL),
-	_CLK_SC_FIXED(RCC_MP_APB5ENSETR, 8, RTCAPB, _PCLK5),
-	_CLK_SC_FIXED(RCC_MP_APB5ENSETR, 11, TZC1, _PCLK5),
-	_CLK_SC_FIXED(RCC_MP_APB5ENSETR, 12, TZC2, _PCLK5),
-	_CLK_SC_FIXED(RCC_MP_APB5ENSETR, 13, TZPC, _PCLK5),
-	_CLK_SC_FIXED(RCC_MP_APB5ENSETR, 15, IWDG1, _PCLK5),
-	_CLK_SC_FIXED(RCC_MP_APB5ENSETR, 16, BSEC, _PCLK5),
-	_CLK_SC_SELEC(RCC_MP_APB5ENSETR, 20, STGEN_K, _STGEN_SEL),
-
-	_CLK_SC_SELEC(RCC_MP_AHB2ENSETR, 8, USBO_K, _USBO_SEL),
-	_CLK_SC_SELEC(RCC_MP_AHB2ENSETR, 16, SDMMC3_K, _SDMMC3_SEL),
-
-	_CLK_SC_SELEC(RCC_MP_AHB4ENSETR, 0, GPIOA, _UNKNOWN_SEL),
-	_CLK_SC_SELEC(RCC_MP_AHB4ENSETR, 1, GPIOB, _UNKNOWN_SEL),
-	_CLK_SC_SELEC(RCC_MP_AHB4ENSETR, 2, GPIOC, _UNKNOWN_SEL),
-	_CLK_SC_SELEC(RCC_MP_AHB4ENSETR, 3, GPIOD, _UNKNOWN_SEL),
-	_CLK_SC_SELEC(RCC_MP_AHB4ENSETR, 4, GPIOE, _UNKNOWN_SEL),
-	_CLK_SC_SELEC(RCC_MP_AHB4ENSETR, 5, GPIOF, _UNKNOWN_SEL),
-	_CLK_SC_SELEC(RCC_MP_AHB4ENSETR, 6, GPIOG, _UNKNOWN_SEL),
-	_CLK_SC_SELEC(RCC_MP_AHB4ENSETR, 7, GPIOH, _UNKNOWN_SEL),
-	_CLK_SC_SELEC(RCC_MP_AHB4ENSETR, 8, GPIOI, _UNKNOWN_SEL),
-	_CLK_SC_SELEC(RCC_MP_AHB4ENSETR, 9, GPIOJ, _UNKNOWN_SEL),
-	_CLK_SC_SELEC(RCC_MP_AHB4ENSETR, 10, GPIOK, _UNKNOWN_SEL),
-
-	_CLK_SC_FIXED(RCC_MP_AHB5ENSETR, 0, GPIOZ, _PCLK5),
-	_CLK_SC_FIXED(RCC_MP_AHB5ENSETR, 4, CRYP1, _PCLK5),
-	_CLK_SC_FIXED(RCC_MP_AHB5ENSETR, 5, HASH1, _PCLK5),
-	_CLK_SC_SELEC(RCC_MP_AHB5ENSETR, 6, RNG1_K, _RNG1_SEL),
-	_CLK_SC_FIXED(RCC_MP_AHB5ENSETR, 8, BKPSRAM, _PCLK5),
-
-	_CLK_SC_SELEC(RCC_MP_AHB6ENSETR, 12, FMC_K, _FMC_SEL),
-	_CLK_SC_SELEC(RCC_MP_AHB6ENSETR, 14, QSPI_K, _QSPI_SEL),
-	_CLK_SC_SELEC(RCC_MP_AHB6ENSETR, 16, SDMMC1_K, _SDMMC12_SEL),
-	_CLK_SC_SELEC(RCC_MP_AHB6ENSETR, 17, SDMMC2_K, _SDMMC12_SEL),
-	_CLK_SC_SELEC(RCC_MP_AHB6ENSETR, 24, USBH, _UNKNOWN_SEL),
-
-	_CLK_SELEC(RCC_BDCR, 20, RTC, _RTC_SEL),
-	_CLK_SELEC(RCC_DBGCFGR, 8, CK_DBG, _UNKNOWN_SEL),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 0, DDRC1, _ACLK),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 1, DDRC1LP, _ACLK),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 2, DDRC2, _ACLK),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 3, DDRC2LP, _ACLK),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 4, DDRPHYC, _PLL2_R),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 5, DDRPHYCLP, _PLL2_R),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 6, DDRCAPB, _PCLK4),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 7, DDRCAPBLP, _PCLK4),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 8, AXIDCG, _ACLK),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 9, DDRPHYCAPB, _PCLK4),
+	_CLK_FIXED(SEC, RCC_DDRITFCR, 10, DDRPHYCAPBLP, _PCLK4),
+
+#if defined(IMAGE_BL32)
+	_CLK_SC_FIXED(N_S, RCC_MP_APB1ENSETR, 6, TIM12_K, _PCLK1),
+#endif
+#if defined(IMAGE_BL2)
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 14, USART2_K, _UART24_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 15, USART3_K, _UART35_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 16, UART4_K, _UART24_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 17, UART5_K, _UART35_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 18, UART7_K, _UART78_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB1ENSETR, 19, UART8_K, _UART78_SEL),
+#endif
+
+#if defined(IMAGE_BL32)
+	_CLK_SC_FIXED(N_S, RCC_MP_APB2ENSETR, 2, TIM15_K, _PCLK2),
+#endif
+#if defined(IMAGE_BL2)
+	_CLK_SC_SELEC(N_S, RCC_MP_APB2ENSETR, 13, USART6_K, _UART6_SEL),
+#endif
+
+	_CLK_SC_FIXED(N_S, RCC_MP_APB3ENSETR, 11, SYSCFG, _UNKNOWN_ID),
+
+#if defined(IMAGE_BL32)
+	_CLK_SC_SELEC(N_S, RCC_MP_APB4ENSETR, 0, LTDC_PX, _UNKNOWN_SEL),
+#endif
+	_CLK_SC_SELEC(N_S, RCC_MP_APB4ENSETR, 8, DDRPERFM, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB4ENSETR, 15, IWDG2, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_APB4ENSETR, 16, USBPHY_K, _USBPHY_SEL),
+
+	_CLK_SC_SELEC(SEC, RCC_MP_APB5ENSETR, 0, SPI6_K, _SPI6_SEL),
+	_CLK_SC_SELEC(SEC, RCC_MP_APB5ENSETR, 2, I2C4_K, _I2C46_SEL),
+	_CLK_SC_SELEC(SEC, RCC_MP_APB5ENSETR, 3, I2C6_K, _I2C46_SEL),
+	_CLK_SC_SELEC(SEC, RCC_MP_APB5ENSETR, 4, USART1_K, _UART1_SEL),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 8, RTCAPB, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 11, TZC1, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 12, TZC2, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 13, TZPC, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 15, IWDG1, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_APB5ENSETR, 16, BSEC, _PCLK5),
+	_CLK_SC_SELEC(SEC, RCC_MP_APB5ENSETR, 20, STGEN_K, _STGEN_SEL),
+
+	_CLK_SELEC(SEC, RCC_BDCR, 20, RTC, _RTC_SEL),
+
+#if defined(IMAGE_BL32)
+	_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),
+#endif
+
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 0, GPIOA, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 1, GPIOB, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 2, GPIOC, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 3, GPIOD, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 4, GPIOE, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 5, GPIOF, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 6, GPIOG, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 7, GPIOH, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 8, GPIOI, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 9, GPIOJ, _UNKNOWN_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB4ENSETR, 10, GPIOK, _UNKNOWN_SEL),
+
+	_CLK_SC_FIXED(SEC, RCC_MP_AHB5ENSETR, 0, GPIOZ, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_AHB5ENSETR, 4, CRYP1, _PCLK5),
+	_CLK_SC_FIXED(SEC, RCC_MP_AHB5ENSETR, 5, HASH1, _PCLK5),
+	_CLK_SC_SELEC(SEC, RCC_MP_AHB5ENSETR, 6, RNG1_K, _RNG1_SEL),
+	_CLK_SC_FIXED(SEC, RCC_MP_AHB5ENSETR, 8, BKPSRAM, _PCLK5),
+
+#if defined(IMAGE_BL32)
+	_CLK_SC_FIXED(SEC, RCC_MP_TZAHB6ENSETR, 0, MDMA, _ACLK),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 5, GPU, _UNKNOWN_SEL),
+	_CLK_SC_FIXED(N_S, RCC_MP_AHB6ENSETR, 10, ETHMAC, _ACLK),
+#endif
+#if defined(IMAGE_BL2)
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 12, FMC_K, _FMC_SEL),
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 14, QSPI_K, _QSPI_SEL),
+#endif
+	_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),
+#if defined(IMAGE_BL32)
+	_CLK_SC_SELEC(N_S, RCC_MP_AHB6ENSETR, 24, USBH, _UNKNOWN_SEL),
+#endif
+
+	_CLK_SELEC(N_S, RCC_DBGCFGR, 8, CK_DBG, _UNKNOWN_SEL),
 };
 
 static const uint8_t i2c12_parents[] = {
@@ -465,12 +518,12 @@ static const uint8_t fmc_parents[] = {
 	_ACLK, _PLL3_R, _PLL4_P, _CK_PER
 };
 
-static const uint8_t ass_parents[] = {
-	_HSI, _HSE, _PLL2
+static const uint8_t axiss_parents[] = {
+	_HSI, _HSE, _PLL2_P
 };
 
-static const uint8_t mss_parents[] = {
-	_HSI, _HSE, _CSI, _PLL3
+static const uint8_t mcuss_parents[] = {
+	_HSI, _HSE, _CSI, _PLL3_P
 };
 
 static const uint8_t usbphy_parents[] = {
@@ -512,14 +565,13 @@ static const struct stm32mp1_clk_sel stm32mp1_clk_sel[_PARENT_SEL_NB] = {
 	_CLK_PARENT_SEL(SDMMC3, RCC_SDMMC3CKSELR, sdmmc3_parents),
 	_CLK_PARENT_SEL(QSPI, RCC_QSPICKSELR, qspi_parents),
 	_CLK_PARENT_SEL(FMC, RCC_FMCCKSELR, fmc_parents),
-	_CLK_PARENT_SEL(AXIS, RCC_ASSCKSELR, ass_parents),
-	_CLK_PARENT_SEL(MCUS, RCC_MSSCKSELR, mss_parents),
+	_CLK_PARENT_SEL(AXIS, RCC_ASSCKSELR, axiss_parents),
+	_CLK_PARENT_SEL(MCUS, RCC_MSSCKSELR, mcuss_parents),
 	_CLK_PARENT_SEL(USBPHY, RCC_USBCKSELR, usbphy_parents),
 	_CLK_PARENT_SEL(USBO, RCC_USBCKSELR, usbo_parents),
 };
 
 /* Define characteristic of PLL according type */
-#define DIVN_MIN	24
 static const struct stm32mp1_pll stm32mp1_pll[PLL_TYPE_NB] = {
 	[PLL_800] = {
 		.refclk_min = 4,
@@ -614,17 +666,55 @@ static const char * const stm32mp1_clk_parent_name[_PARENT_NB] __unused = {
 	[_USB_PHY_48] = "USB_PHY_48",
 };
 
+static const char *
+const stm32mp1_clk_parent_sel_name[_PARENT_SEL_NB] __unused = {
+	[_I2C12_SEL] = "I2C12",
+	[_I2C35_SEL] = "I2C35",
+	[_STGEN_SEL] = "STGEN",
+	[_I2C46_SEL] = "I2C46",
+	[_SPI6_SEL] = "SPI6",
+	[_UART1_SEL] = "USART1",
+	[_RNG1_SEL] = "RNG1",
+	[_UART6_SEL] = "UART6",
+	[_UART24_SEL] = "UART24",
+	[_UART35_SEL] = "UART35",
+	[_UART78_SEL] = "UART78",
+	[_SDMMC12_SEL] = "SDMMC12",
+	[_SDMMC3_SEL] = "SDMMC3",
+	[_QSPI_SEL] = "QSPI",
+	[_FMC_SEL] = "FMC",
+	[_AXIS_SEL] = "AXISS",
+	[_MCUS_SEL] = "MCUSS",
+	[_USBPHY_SEL] = "USBPHY",
+	[_USBO_SEL] = "USBO",
+};
+
 /* RCC clock device driver private */
 static unsigned long stm32mp1_osc[NB_OSC];
 static struct spinlock reg_lock;
 static unsigned int gate_refcounts[NB_GATES];
 static struct spinlock refcount_lock;
+static struct stm32mp1_pll_settings pll1_settings;
+static uint32_t current_opp_khz;
+static uint32_t pll3cr;
+static uint32_t pll4cr;
+static uint32_t mssckselr;
+static uint32_t mcudivr;
+#if STM32MP_SP_MIN_IN_DDR
+static uint32_t mpapb_iwdg1;
+static uint32_t mpapb_iwdg2;
+#endif
 
 static const struct stm32mp1_clk_gate *gate_ref(unsigned int idx)
 {
 	return &stm32mp1_clk_gate[idx];
 }
 
+static bool gate_is_non_secure(const struct stm32mp1_clk_gate *gate)
+{
+	return gate->secure == N_S;
+}
+
 static const struct stm32mp1_clk_sel *clk_sel_ref(unsigned int idx)
 {
 	return &stm32mp1_clk_sel[idx];
@@ -747,6 +837,12 @@ static int stm32mp1_clk_get_parent(unsigned long id)
 	p_sel = (mmio_read_32(rcc_base + sel->offset) &
 		 (sel->msk << sel->src)) >> sel->src;
 	if (p_sel < sel->nb_parent) {
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+		VERBOSE("%s: %s clock is the parent %s of clk id %ld\n",
+			__func__,
+			stm32mp1_clk_parent_name[sel->parent[p_sel]],
+			stm32mp1_clk_parent_sel_name[s], id);
+#endif
 		return (int)sel->parent[p_sel];
 	}
 
@@ -847,9 +943,7 @@ static unsigned long get_clock_rate(int p)
 
 			reg = mmio_read_32(rcc_base + RCC_MPCKDIVR);
 			clkdiv = reg & RCC_MPUDIV_MASK;
-			if (clkdiv != 0U) {
-				clock /= stm32mp1_mpu_div[clkdiv];
-			}
+			clock >>= stm32mp1_mpu_div[clkdiv];
 			break;
 		default:
 			break;
@@ -1057,17 +1151,6 @@ static bool __clk_is_enabled(struct stm32mp1_clk_gate const *gate)
 	return mmio_read_32(rcc_base + gate->offset) & BIT(gate->bit);
 }
 
-unsigned int stm32mp1_clk_get_refcount(unsigned long id)
-{
-	int i = stm32mp1_clk_get_gated_id(id);
-
-	if (i < 0) {
-		panic();
-	}
-
-	return gate_refcounts[i];
-}
-
 /* Oscillators and PLLs are not gated at runtime */
 static bool clock_is_always_on(unsigned long id)
 {
@@ -1086,17 +1169,19 @@ static bool clock_is_always_on(unsigned long id)
 	case PLL3_P:
 	case PLL3_Q:
 	case PLL3_R:
+	case CK_AXI:
+	case CK_MPU:
+	case CK_MCU:
 		return true;
 	default:
 		return false;
 	}
 }
 
-void __stm32mp1_clk_enable(unsigned long id, bool secure)
+static void __stm32mp1_clk_enable(unsigned long id, bool with_refcnt)
 {
 	const struct stm32mp1_clk_gate *gate;
 	int i;
-	unsigned int *refcnt;
 
 	if (clock_is_always_on(id)) {
 		return;
@@ -1109,22 +1194,38 @@ void __stm32mp1_clk_enable(unsigned long id, bool secure)
 	}
 
 	gate = gate_ref(i);
-	refcnt = &gate_refcounts[i];
+
+	if (!with_refcnt) {
+		__clk_enable(gate);
+		return;
+	}
+
+#if defined(IMAGE_BL32)
+	if (gate_is_non_secure(gate)) {
+		/* Enable non-secure clock w/o any refcounting */
+		__clk_enable(gate);
+		return;
+	}
+#endif
 
 	stm32mp1_clk_lock(&refcount_lock);
 
-	if (stm32mp_incr_shrefcnt(refcnt, secure) != 0) {
+	if (gate_refcounts[i] == 0) {
 		__clk_enable(gate);
 	}
 
+	gate_refcounts[i]++;
+	if (gate_refcounts[i] == UINT_MAX) {
+		panic();
+	}
+
 	stm32mp1_clk_unlock(&refcount_lock);
 }
 
-void __stm32mp1_clk_disable(unsigned long id, bool secure)
+static void __stm32mp1_clk_disable(unsigned long id, bool with_refcnt)
 {
 	const struct stm32mp1_clk_gate *gate;
 	int i;
-	unsigned int *refcnt;
 
 	if (clock_is_always_on(id)) {
 		return;
@@ -1137,28 +1238,56 @@ void __stm32mp1_clk_disable(unsigned long id, bool secure)
 	}
 
 	gate = gate_ref(i);
-	refcnt = &gate_refcounts[i];
+
+	if (!with_refcnt) {
+		__clk_disable(gate);
+		return;
+	}
+
+#if defined(IMAGE_BL32)
+	if (gate_is_non_secure(gate)) {
+		/* Don't disable non-secure clocks */
+		return;
+	}
+#endif
 
 	stm32mp1_clk_lock(&refcount_lock);
 
-	if (stm32mp_decr_shrefcnt(refcnt, secure) != 0) {
+	if (gate_refcounts[i] == 0) {
+		panic();
+	}
+	gate_refcounts[i]--;
+
+	if (gate_refcounts[i] == 0) {
 		__clk_disable(gate);
 	}
 
 	stm32mp1_clk_unlock(&refcount_lock);
 }
 
-void stm32mp_clk_enable(unsigned long id)
+static int stm32mp_clk_enable(unsigned long id)
 {
 	__stm32mp1_clk_enable(id, true);
+
+	return 0;
 }
 
-void stm32mp_clk_disable(unsigned long id)
+static void stm32mp_clk_disable(unsigned long id)
 {
 	__stm32mp1_clk_disable(id, true);
 }
 
-bool stm32mp_clk_is_enabled(unsigned long id)
+void stm32mp1_clk_force_enable(unsigned long id)
+{
+	__stm32mp1_clk_enable(id, false);
+}
+
+void stm32mp1_clk_force_disable(unsigned long id)
+{
+	__stm32mp1_clk_disable(id, false);
+}
+
+static bool stm32mp_clk_is_enabled(unsigned long id)
 {
 	int i;
 
@@ -1174,15 +1303,55 @@ bool stm32mp_clk_is_enabled(unsigned long id)
 	return __clk_is_enabled(gate_ref(i));
 }
 
-unsigned long stm32mp_clk_get_rate(unsigned long id)
+static unsigned long stm32mp_clk_get_rate(unsigned long id)
 {
+	uintptr_t rcc_base = stm32mp_rcc_base();
 	int p = stm32mp1_clk_get_parent(id);
+	uint32_t prescaler, timpre;
+	unsigned long parent_rate;
 
 	if (p < 0) {
 		return 0;
 	}
 
-	return get_clock_rate(p);
+	parent_rate = get_clock_rate(p);
+
+	switch (id) {
+	case TIM2_K:
+	case TIM3_K:
+	case TIM4_K:
+	case TIM5_K:
+	case TIM6_K:
+	case TIM7_K:
+	case TIM12_K:
+	case TIM13_K:
+	case TIM14_K:
+		prescaler = mmio_read_32(rcc_base + RCC_APB1DIVR) &
+			    RCC_APBXDIV_MASK;
+		timpre = mmio_read_32(rcc_base + RCC_TIMG1PRER) &
+			 RCC_TIMGXPRER_TIMGXPRE;
+		break;
+
+	case TIM1_K:
+	case TIM8_K:
+	case TIM15_K:
+	case TIM16_K:
+	case TIM17_K:
+		prescaler = mmio_read_32(rcc_base + RCC_APB2DIVR) &
+			    RCC_APBXDIV_MASK;
+		timpre = mmio_read_32(rcc_base + RCC_TIMG2PRER) &
+			 RCC_TIMGXPRER_TIMGXPRE;
+		break;
+
+	default:
+		return parent_rate;
+	}
+
+	if (prescaler == 0U) {
+		return parent_rate;
+	}
+
+	return parent_rate * (timpre + 1U) * 2U;
 }
 
 static void stm32mp1_ls_osc_set(bool enable, uint32_t offset, uint32_t mask_on)
@@ -1299,6 +1468,13 @@ static void stm32mp1_hse_enable(bool bypass, bool digbyp, bool css)
 	if (css) {
 		mmio_write_32(rcc_base + RCC_OCENSETR, RCC_OCENR_HSECSSON);
 	}
+
+#if defined(STM32MP_USB) || defined(STM32MP_UART)
+	if ((mmio_read_32(rcc_base + RCC_OCENSETR) & RCC_OCENR_HSEBYP) &&
+	    (!(digbyp || bypass))) {
+		panic();
+	}
+#endif
 }
 
 static void stm32mp1_csi_set(bool enable)
@@ -1498,11 +1674,8 @@ static int stm32mp1_pll_stop(enum stm32mp1_pll_id pll_id)
 	return 0;
 }
 
-static void stm32mp1_pll_config_output(enum stm32mp1_pll_id pll_id,
-				       uint32_t *pllcfg)
+static uint32_t stm32mp1_pll_compute_pllxcfgr2(uint32_t *pllcfg)
 {
-	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
-	uintptr_t rcc_base = stm32mp_rcc_base();
 	uint32_t value;
 
 	value = (pllcfg[PLLCFG_P] << RCC_PLLNCFGR2_DIVP_SHIFT) &
@@ -1511,21 +1684,33 @@ static void stm32mp1_pll_config_output(enum stm32mp1_pll_id pll_id,
 		 RCC_PLLNCFGR2_DIVQ_MASK;
 	value |= (pllcfg[PLLCFG_R] << RCC_PLLNCFGR2_DIVR_SHIFT) &
 		 RCC_PLLNCFGR2_DIVR_MASK;
-	mmio_write_32(rcc_base + pll->pllxcfgr2, value);
+
+	return value;
 }
 
-static int stm32mp1_pll_config(enum stm32mp1_pll_id pll_id,
-			       uint32_t *pllcfg, uint32_t fracv)
+static void stm32mp1_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 = stm32mp_rcc_base();
+	uint32_t value;
+
+	value = stm32mp1_pll_compute_pllxcfgr2(pllcfg);
+
+	mmio_write_32(rcc_base + pll->pllxcfgr2, value);
+}
+
+static int stm32mp1_pll_compute_pllxcfgr1(const struct stm32mp1_clk_pll *pll,
+					  uint32_t *pllcfg, uint32_t *cfgr1)
+{
 	uintptr_t rcc_base = stm32mp_rcc_base();
 	enum stm32mp1_plltype type = pll->plltype;
 	unsigned long refclk;
 	uint32_t ifrge = 0;
-	uint32_t src, value;
+	uint32_t src;
 
 	src = mmio_read_32(rcc_base + pll->rckxselr) &
-		RCC_SELR_REFCLK_SRC_MASK;
+	      RCC_SELR_REFCLK_SRC_MASK;
 
 	refclk = stm32mp1_clk_get_fixed(pll->refclk[src]) /
 		 (pllcfg[PLLCFG_M] + 1U);
@@ -1539,23 +1724,39 @@ static int stm32mp1_pll_config(enum stm32mp1_pll_id pll_id,
 		ifrge = 1U;
 	}
 
-	value = (pllcfg[PLLCFG_N] << RCC_PLLNCFGR1_DIVN_SHIFT) &
-		RCC_PLLNCFGR1_DIVN_MASK;
-	value |= (pllcfg[PLLCFG_M] << RCC_PLLNCFGR1_DIVM_SHIFT) &
-		 RCC_PLLNCFGR1_DIVM_MASK;
-	value |= (ifrge << RCC_PLLNCFGR1_IFRGE_SHIFT) &
-		 RCC_PLLNCFGR1_IFRGE_MASK;
+	*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 stm32mp1_pll_config(enum stm32mp1_pll_id pll_id,
+			       uint32_t *pllcfg, uint32_t fracv)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(pll_id);
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uint32_t value;
+	int ret;
+
+	ret = stm32mp1_pll_compute_pllxcfgr1(pll, pllcfg, &value);
+	if (ret != 0) {
+		return ret;
+	}
+
 	mmio_write_32(rcc_base + pll->pllxcfgr1, value);
 
 	/* Fractional configuration */
 	value = 0;
 	mmio_write_32(rcc_base + pll->pllxfracr, value);
 
+	/*  Frac must be enabled only once its configuration is loaded */
 	value = fracv << RCC_PLLNFRACR_FRACV_SHIFT;
 	mmio_write_32(rcc_base + pll->pllxfracr, value);
-
-	value |= RCC_PLLNFRACR_FRACLE;
-	mmio_write_32(rcc_base + pll->pllxfracr, value);
+	mmio_setbits_32(rcc_base + pll->pllxfracr, RCC_PLLNFRACR_FRACLE);
 
 	stm32mp1_pll_config_output(pll_id, pllcfg);
 
@@ -1662,48 +1863,41 @@ static void stm32mp1_set_rtcsrc(unsigned int clksrc, bool lse_css)
 	}
 }
 
-static void stm32mp1_stgen_config(void)
+/*******************************************************************************
+ * This function determines the number of needed RTC calendar read operations
+ * to get consistent values (1 or 2 depending on clock frequencies).
+ * If APB1 frequency is lower than 7 times the RTC one, the software has to
+ * read the calendar time and date registers twice.
+ * Returns true if read twice is needed, false else.
+ ******************************************************************************/
+bool stm32mp1_rtc_get_read_twice(void)
 {
-	uint32_t cntfid0;
-	unsigned long rate;
-	unsigned long long counter;
-
-	cntfid0 = mmio_read_32(STGEN_BASE + CNTFID_OFF);
-	rate = get_clock_rate(stm32mp1_clk_get_parent(STGEN_K));
+	unsigned long apb1_freq;
+	uint32_t rtc_freq;
+	uint32_t apb1_div;
+	uintptr_t rcc_base = stm32mp_rcc_base();
 
-	if (cntfid0 == rate) {
-		return;
+	switch ((mmio_read_32(rcc_base + RCC_BDCR) &
+		 RCC_BDCR_RTCSRC_MASK) >> RCC_BDCR_RTCSRC_SHIFT) {
+	case 1:
+		rtc_freq = stm32mp_clk_get_rate(CK_LSE);
+		break;
+	case 2:
+		rtc_freq = stm32mp_clk_get_rate(CK_LSI);
+		break;
+	case 3:
+		rtc_freq = stm32mp_clk_get_rate(CK_HSE);
+		rtc_freq /= (mmio_read_32(rcc_base + RCC_RTCDIVR) &
+			     RCC_DIVR_DIV_MASK) + 1U;
+		break;
+	default:
+		panic();
 	}
 
-	mmio_clrbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
-	counter = (unsigned long long)mmio_read_32(STGEN_BASE + CNTCVL_OFF);
-	counter |= ((unsigned long long)mmio_read_32(STGEN_BASE + CNTCVU_OFF)) << 32;
-	counter = (counter * rate / cntfid0);
-
-	mmio_write_32(STGEN_BASE + CNTCVL_OFF, (uint32_t)counter);
-	mmio_write_32(STGEN_BASE + CNTCVU_OFF, (uint32_t)(counter >> 32));
-	mmio_write_32(STGEN_BASE + CNTFID_OFF, rate);
-	mmio_setbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
-
-	write_cntfrq((u_register_t)rate);
-
-	/* Need to update timer with new frequency */
-	generic_delay_timer_init();
-}
-
-void stm32mp1_stgen_increment(unsigned long long offset_in_ms)
-{
-	unsigned long long cnt;
-
-	cnt = ((unsigned long long)mmio_read_32(STGEN_BASE + CNTCVU_OFF) << 32) |
-		mmio_read_32(STGEN_BASE + CNTCVL_OFF);
-
-	cnt += (offset_in_ms * mmio_read_32(STGEN_BASE + CNTFID_OFF)) / 1000U;
+	apb1_div = mmio_read_32(rcc_base + RCC_APB1DIVR) & RCC_APBXDIV_MASK;
+	apb1_freq = stm32mp_clk_get_rate(CK_MCU) >> apb1_div;
 
-	mmio_clrbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
-	mmio_write_32(STGEN_BASE + CNTCVL_OFF, (uint32_t)cnt);
-	mmio_write_32(STGEN_BASE + CNTCVU_OFF, (uint32_t)(cnt >> 32));
-	mmio_setbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
+	return apb1_freq < (rtc_freq * 7U);
 }
 
 static void stm32mp1_pkcs_config(uint32_t pkcs)
@@ -1720,152 +1914,740 @@ static void stm32mp1_pkcs_config(uint32_t pkcs)
 	mmio_clrsetbits_32(address, mask, value);
 }
 
-int stm32mp1_clk_init(void)
+static bool clk_pll1_settings_are_valid(void)
 {
-	uintptr_t rcc_base = stm32mp_rcc_base();
-	unsigned int clksrc[CLKSRC_NB];
-	unsigned int clkdiv[CLKDIV_NB];
-	unsigned int pllcfg[_PLL_NB][PLLCFG_NB];
-	int plloff[_PLL_NB];
-	int ret, len;
-	enum stm32mp1_pll_id i;
-	bool lse_css = false;
-	bool pll3_preserve = false;
-	bool pll4_preserve = false;
-	bool pll4_bootrom = false;
-	const fdt32_t *pkcs_cell;
-	void *fdt;
+	return pll1_settings.valid_id == PLL1_SETTINGS_VALID_ID;
+}
 
-	if (fdt_get_address(&fdt) == 0) {
-		return false;
-	}
+int stm32mp1_round_opp_khz(uint32_t *freq_khz)
+{
+	unsigned int i;
+	uint32_t round_opp = 0U;
+
+	if (!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;
 
-	/* Check status field to disable security */
-	if (!fdt_get_rcc_secure_status()) {
-		mmio_write_32(rcc_base + RCC_TZCR, 0);
+		return 0;
 	}
 
-	ret = fdt_rcc_read_uint32_array("st,clksrc", (uint32_t)CLKSRC_NB,
-					clksrc);
-	if (ret < 0) {
-		return -FDT_ERR_NOTFOUND;
+	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];
+		}
 	}
 
-	ret = fdt_rcc_read_uint32_array("st,clkdiv", (uint32_t)CLKDIV_NB,
-					clkdiv);
-	if (ret < 0) {
-		return -FDT_ERR_NOTFOUND;
+	*freq_khz = round_opp;
+
+	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, no need to reconfigure.
+ * Return value is 0 if no error.
+ */
+static int stm32mp1_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 = stm32mp_rcc_base();
+	uint32_t fracr;
+	uint32_t value;
+	int ret;
+
+	ret = stm32mp1_pll_compute_pllxcfgr1(pll, pllcfg, &value);
+	if (ret != 0) {
+		return ret;
 	}
 
-	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
-		char name[12];
+	if (mmio_read_32(rcc_base + pll->pllxcfgr1) != value) {
+		/* Different DIVN/DIVM, can't config on the fly */
+		*result = -1;
+		return 0;
+	}
 
-		snprintf(name, sizeof(name), "st,pll@%d", i);
-		plloff[i] = fdt_rcc_subnode_offset(name);
+	*result = true;
 
-		if (!fdt_check_node(plloff[i])) {
-			continue;
-		}
+	fracr = fracv << RCC_PLLNFRACR_FRACV_SHIFT;
+	fracr |= RCC_PLLNFRACR_FRACLE;
+	value = stm32mp1_pll_compute_pllxcfgr2(pllcfg);
 
-		ret = fdt_read_uint32_array(fdt, plloff[i], "cfg",
-					    (int)PLLCFG_NB, pllcfg[i]);
-		if (ret < 0) {
-			return -FDT_ERR_NOTFOUND;
-		}
+	if ((mmio_read_32(rcc_base + pll->pllxfracr) == fracr) &&
+	    (mmio_read_32(rcc_base + pll->pllxcfgr2) == value)) {
+		/* Same parameters, no need to config */
+		*result = 1;
+	} else {
+		*result = 0;
 	}
 
-	stm32mp1_mco_csg(clksrc[CLKSRC_MCO1], clkdiv[CLKDIV_MCO1]);
-	stm32mp1_mco_csg(clksrc[CLKSRC_MCO2], clkdiv[CLKDIV_MCO2]);
+	return 0;
+}
 
-	/*
-	 * Switch ON oscillator found in device-tree.
-	 * Note: HSI already ON after BootROM stage.
-	 */
-	if (stm32mp1_osc[_LSI] != 0U) {
-		stm32mp1_lsi_set(true);
+static int stm32mp1_get_mpu_div(uint32_t freq_khz)
+{
+	unsigned long freq_pll1_p;
+	unsigned long div;
+
+	freq_pll1_p = get_clock_rate(_PLL1_P) / 1000UL;
+	if ((freq_pll1_p % freq_khz) != 0U) {
+		return -1;
 	}
-	if (stm32mp1_osc[_LSE] != 0U) {
-		bool bypass, digbyp;
-		uint32_t lsedrv;
 
-		bypass = fdt_osc_read_bool(_LSE, "st,bypass");
-		digbyp = fdt_osc_read_bool(_LSE, "st,digbypass");
-		lse_css = fdt_osc_read_bool(_LSE, "st,css");
-		lsedrv = fdt_osc_read_uint32_default(_LSE, "st,drive",
-						     LSEDRV_MEDIUM_HIGH);
-		stm32mp1_lse_enable(bypass, digbyp, lsedrv);
+	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;
 	}
-	if (stm32mp1_osc[_HSE] != 0U) {
-		bool bypass, digbyp, css;
+}
 
-		bypass = fdt_osc_read_bool(_HSE, "st,bypass");
-		digbyp = fdt_osc_read_bool(_HSE, "st,digbypass");
-		css = fdt_osc_read_bool(_HSE, "st,css");
-		stm32mp1_hse_enable(bypass, digbyp, css);
+static int stm32mp1_pll1_config_from_opp_khz(uint32_t freq_khz)
+{
+	unsigned int i;
+	int ret;
+	int div;
+	int config_on_the_fly = -1;
+
+	for (i = 0; i < PLAT_MAX_OPP_NB; i++) {
+		if (pll1_settings.freq[i] == freq_khz) {
+			break;
+		}
 	}
-	/*
-	 * CSI is mandatory for automatic I/O compensation (SYSCFG_CMPCR)
-	 * => switch on CSI even if node is not present in device tree
-	 */
-	stm32mp1_csi_set(true);
 
-	/* Come back to HSI */
-	ret = stm32mp1_set_clksrc(CLK_MPU_HSI);
-	if (ret != 0) {
-		return ret;
+	if (i == PLAT_MAX_OPP_NB) {
+		return -ENXIO;
 	}
-	ret = stm32mp1_set_clksrc(CLK_AXI_HSI);
-	if (ret != 0) {
+
+	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, stm32mp_rcc_base() +
+					  RCC_MPCKDIVR);
+		if (ret == 0) {
+			ret = stm32mp1_set_clksrc(CLK_MPU_PLL1P_DIV);
+		}
 		return ret;
 	}
-	ret = stm32mp1_set_clksrc(CLK_MCU_HSI);
+
+	ret = stm32mp1_is_pll_config_on_the_fly(_PLL1, &pll1_settings.cfg[i][0],
+						pll1_settings.frac[i],
+						&config_on_the_fly);
 	if (ret != 0) {
 		return ret;
 	}
 
-	if ((mmio_read_32(rcc_base + RCC_MP_RSTSCLRR) &
-	     RCC_MP_RSTSCLRR_MPUP0RSTF) != 0) {
-		pll3_preserve = stm32mp1_check_pll_conf(_PLL3,
-							clksrc[CLKSRC_PLL3],
-							pllcfg[_PLL3],
-							plloff[_PLL3]);
-		pll4_preserve = stm32mp1_check_pll_conf(_PLL4,
-							clksrc[CLKSRC_PLL4],
-							pllcfg[_PLL4],
-							plloff[_PLL4]);
+	if (config_on_the_fly == 1) {
+		/*  No need to reconfigure, setup already OK */
+		return 0;
 	}
 
-	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
-		if (((i == _PLL3) && pll3_preserve) ||
-		    ((i == _PLL4) && pll4_preserve)) {
-			continue;
-		}
-
-		ret = stm32mp1_pll_stop(i);
+	if (config_on_the_fly == -1) {
+		/* Switch to HSI and stop PLL1 before reconfiguration */
+		ret = stm32mp1_set_clksrc(CLK_MPU_HSI);
 		if (ret != 0) {
 			return ret;
 		}
-	}
 
-	/* Configure HSIDIV */
-	if (stm32mp1_osc[_HSI] != 0U) {
-		ret = stm32mp1_hsidiv(stm32mp1_osc[_HSI]);
+		ret = stm32mp1_pll_stop(_PLL1);
 		if (ret != 0) {
 			return ret;
 		}
-		stm32mp1_stgen_config();
 	}
 
-	/* Select DIV */
-	/* No ready bit when MPUSRC != CLK_MPU_PLL1P_DIV, MPUDIV is disabled */
-	mmio_write_32(rcc_base + RCC_MPCKDIVR,
-		      clkdiv[CLKDIV_MPU] & RCC_DIVR_DIV_MASK);
-	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_AXI], rcc_base + RCC_AXIDIVR);
+	ret = stm32mp1_pll_config(_PLL1, &pll1_settings.cfg[i][0],
+				  pll1_settings.frac[i]);
 	if (ret != 0) {
 		return ret;
 	}
-	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB4], rcc_base + RCC_APB4DIVR);
-	if (ret != 0) {
+
+	if (config_on_the_fly == -1) {
+		/* Start PLL1 and switch back to after reconfiguration */
+		stm32mp1_pll_start(_PLL1);
+
+		ret = stm32mp1_pll_output(_PLL1,
+					  pll1_settings.cfg[i][PLLCFG_O]);
+		if (ret != 0) {
+			return ret;
+		}
+
+		ret = stm32mp1_set_clksrc(CLK_MPU_PLL1P);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+int stm32mp1_set_opp_khz(uint32_t freq_khz)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uint32_t mpu_src;
+
+	if (freq_khz == current_opp_khz) {
+		/* OPP already set, nothing to do */
+		return 0;
+	}
+
+	if (!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 -EACCES;
+	}
+
+	/* Check that PLL1 is MPU clock source */
+	mpu_src = mmio_read_32(rcc_base + RCC_MPCKSELR) & RCC_SELR_SRC_MASK;
+	if ((mpu_src != RCC_MPCKSELR_PLL) &&
+	    (mpu_src != RCC_MPCKSELR_PLL_MPUDIV)) {
+		return -EPERM;
+	}
+
+	if (stm32mp1_pll1_config_from_opp_khz(freq_khz) != 0) {
+		/* Restore original value */
+		if (stm32mp1_pll1_config_from_opp_khz(current_opp_khz) != 0) {
+			ERROR("No CPU operating point can be set\n");
+			panic();
+		}
+
+		return -EIO;
+	}
+
+	current_opp_khz = freq_khz;
+
+	return 0;
+}
+
+static int clk_get_pll_settings_from_dt(int plloff, unsigned int *pllcfg,
+					uint32_t *fracv, uint32_t *csg,
+					bool *csg_set)
+{
+	void *fdt;
+	int ret;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	ret = fdt_read_uint32_array(fdt, plloff, "cfg", (uint32_t)PLLCFG_NB,
+				    pllcfg);
+	if (ret < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	*fracv = fdt_read_uint32_default(fdt, plloff, "frac", 0);
+
+	ret = fdt_read_uint32_array(fdt, plloff, "csg", (uint32_t)PLLCSG_NB,
+				    csg);
+
+	*csg_set = (ret == 0);
+
+	if (ret == -FDT_ERR_NOTFOUND) {
+		ret = 0;
+	}
+
+	return ret;
+}
+
+static int clk_compute_pll1_settings(unsigned long input_freq,
+				     uint32_t freq_khz,
+				     uint32_t *pllcfg, uint32_t *fracv)
+{
+	unsigned long long output_freq = freq_khz * 1000U;
+	unsigned long long freq;
+	unsigned long long vco;
+	int divm;
+	int divn;
+	int divp;
+	int frac;
+	int i;
+	unsigned int diff;
+	unsigned int best_diff = UINT_MAX;
+
+	/* Following parameters have always the same value */
+	pllcfg[PLLCFG_Q] = 0;
+	pllcfg[PLLCFG_R] = 0;
+	pllcfg[PLLCFG_O] = PQR(1, 0, 0);
+
+	for (divm = DIVM_MAX; divm >= DIVM_MIN; divm--)	{
+		unsigned long 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)  {
+					pllcfg[PLLCFG_M] = divm;
+					pllcfg[PLLCFG_N] = divn;
+					pllcfg[PLLCFG_P] = divp;
+					*fracv = frac;
+
+					if (diff == 0) {
+						return 0;
+					}
+
+					best_diff = diff;
+				}
+
+				frac++;
+			}
+		}
+	}
+
+	if (best_diff == UINT_MAX) {
+		return -1;
+	}
+
+	return 0;
+}
+
+static int clk_get_pll1_settings(uint32_t clksrc, uint32_t freq_khz,
+				 uint32_t *pllcfg, uint32_t *fracv)
+{
+	unsigned int i;
+
+	assert(pllcfg != NULL);
+	assert(fracv != NULL);
+
+	for (i = 0; i < PLAT_MAX_OPP_NB; i++) {
+		if (pll1_settings.freq[i] == freq_khz) {
+			break;
+		}
+	}
+
+	if (((i == PLAT_MAX_OPP_NB) && (pll1_settings.valid_id == 0U)) ||
+	    ((i < PLAT_MAX_OPP_NB) &&
+	     (pll1_settings.cfg[i][PLLCFG_O] == 0U))) {
+		unsigned long input_freq;
+
+		/*
+		 * Either PLL1 settings structure is completely empty,
+		 * or these settings are not yet computed: do it.
+		 */
+		switch (clksrc) {
+		case CLK_PLL12_HSI:
+			input_freq = stm32mp_clk_get_rate(CK_HSI);
+			break;
+		case CLK_PLL12_HSE:
+			input_freq = stm32mp_clk_get_rate(CK_HSE);
+			break;
+		default:
+			panic();
+		}
+
+		return clk_compute_pll1_settings(input_freq, freq_khz, pllcfg,
+						 fracv);
+	}
+
+	if ((i < PLAT_MAX_OPP_NB) &&
+	    (pll1_settings.cfg[i][PLLCFG_O] != 0U)) {
+		/*
+		 * Index is in range and PLL1 settings are computed:
+		 * use content to answer to the request.
+		 */
+		memcpy(pllcfg, &pll1_settings.cfg[i][0],
+		       sizeof(uint32_t) * PLAT_MAX_PLLCFG_NB);
+		*fracv = pll1_settings.frac[i];
+
+		return 0;
+	}
+
+	return -1;
+}
+
+int stm32mp1_clk_get_maxfreq_opp(uint32_t *freq_khz,
+				 uint32_t *voltage_mv)
+{
+	unsigned int i;
+	uint32_t freq = 0U;
+	uint32_t voltage = 0U;
+
+	assert(freq_khz != NULL);
+	assert(voltage_mv != NULL);
+
+	if (!clk_pll1_settings_are_valid()) {
+		return -1;
+	}
+
+	for (i = 0; i < PLAT_MAX_OPP_NB; i++) {
+		if (pll1_settings.freq[i] > freq) {
+			freq = pll1_settings.freq[i];
+			voltage = pll1_settings.volt[i];
+		}
+	}
+
+	if ((freq == 0U) || (voltage == 0U)) {
+		return -1;
+	}
+
+	*freq_khz = freq;
+	*voltage_mv = voltage;
+
+	return 0;
+}
+
+static int clk_save_current_pll1_settings(uint32_t buck1_voltage)
+{
+	const struct stm32mp1_clk_pll *pll = pll_ref(_PLL1);
+	uint32_t rcc_base = stm32mp_rcc_base();
+	uint32_t freq;
+	unsigned int i;
+
+	freq = udiv_round_nearest(stm32mp_clk_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 != 0U))) {
+		return -1;
+	}
+
+	pll1_settings.cfg[i][PLLCFG_M] =
+		(mmio_read_32(rcc_base + pll->pllxcfgr1) &
+		 RCC_PLLNCFGR1_DIVM_MASK) >> RCC_PLLNCFGR1_DIVM_SHIFT;
+
+	pll1_settings.cfg[i][PLLCFG_N] =
+		(mmio_read_32(rcc_base + pll->pllxcfgr1) &
+		 RCC_PLLNCFGR1_DIVN_MASK) >> RCC_PLLNCFGR1_DIVN_SHIFT;
+
+	pll1_settings.cfg[i][PLLCFG_P] =
+		(mmio_read_32(rcc_base + pll->pllxcfgr2) &
+		 RCC_PLLNCFGR2_DIVP_MASK) >> RCC_PLLNCFGR2_DIVP_SHIFT;
+
+	pll1_settings.cfg[i][PLLCFG_Q] =
+		(mmio_read_32(rcc_base + pll->pllxcfgr2) &
+		 RCC_PLLNCFGR2_DIVQ_MASK) >> RCC_PLLNCFGR2_DIVQ_SHIFT;
+
+	pll1_settings.cfg[i][PLLCFG_R] =
+		(mmio_read_32(rcc_base + pll->pllxcfgr2) &
+		 RCC_PLLNCFGR2_DIVR_MASK) >> RCC_PLLNCFGR2_DIVR_SHIFT;
+
+	pll1_settings.cfg[i][PLLCFG_O] =
+		mmio_read_32(rcc_base + pll->pllxcr) >>
+		RCC_PLLNCR_DIVEN_SHIFT;
+
+	pll1_settings.frac[i] =
+		(mmio_read_32(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;
+	const struct stm32mp1_clk_pll *pll = pll_ref(_PLL1);
+	uint32_t rcc_base = stm32mp_rcc_base();
+
+	value = mmio_read_32(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)
+{
+	int i;
+	int ret;
+	int index;
+	uint32_t count = PLAT_MAX_OPP_NB;
+	uint32_t clksrc;
+
+	ret = dt_get_all_opp_freqvolt(&count, pll1_settings.freq,
+				      pll1_settings.volt);
+	switch (ret) {
+	case 0:
+		break;
+	case -FDT_ERR_NOTFOUND:
+		VERBOSE("Cannot find OPP table in DT, use default settings.\n");
+		return 0;
+	default:
+		ERROR("Inconsistent OPP settings found in DT, ignored.\n");
+		return 0;
+	}
+
+	index = clk_save_current_pll1_settings(buck1_voltage);
+
+	clksrc = stm32mp1_clk_get_pll1_current_clksrc();
+
+	for (i = 0; i < (int)count; i++) {
+		if (i == index) {
+			continue;
+		}
+
+		ret = clk_get_pll1_settings(clksrc, pll1_settings.freq[i],
+					    &pll1_settings.cfg[i][0],
+					    &pll1_settings.frac[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) || !clk_pll1_settings_are_valid()) {
+		panic();
+	}
+
+	memcpy(data, &pll1_settings, size);
+}
+
+void stm32mp1_clk_lp_load_opp_pll1_settings(uint8_t *data, size_t size)
+{
+	if (size != sizeof(pll1_settings)) {
+		panic();
+	}
+
+	memcpy(&pll1_settings, data, size);
+}
+
+int stm32mp1_clk_init(uint32_t pll1_freq_khz)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uint32_t pllfracv[_PLL_NB];
+	uint32_t pllcsg[_PLL_NB][PLLCSG_NB];
+	unsigned int clksrc[CLKSRC_NB];
+	unsigned int clkdiv[CLKDIV_NB];
+	unsigned int pllcfg[_PLL_NB][PLLCFG_NB];
+	int plloff[_PLL_NB];
+	int ret, len;
+	enum stm32mp1_pll_id i;
+	bool pllcsg_set[_PLL_NB];
+	bool pllcfg_valid[_PLL_NB];
+	bool lse_css = false;
+	bool pll3_preserve = false;
+	bool pll4_preserve = false;
+	bool pll4_bootrom = false;
+	const fdt32_t *pkcs_cell;
+	void *fdt;
+	int stgen_p = stm32mp1_clk_get_parent((int)STGEN_K);
+	int usbphy_p = stm32mp1_clk_get_parent((int)USBPHY_K);
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	ret = fdt_rcc_read_uint32_array("st,clksrc", (uint32_t)CLKSRC_NB,
+					clksrc);
+	if (ret < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	ret = fdt_rcc_read_uint32_array("st,clkdiv", (uint32_t)CLKDIV_NB,
+					clkdiv);
+	if (ret < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
+		char name[12];
+
+		snprintf(name, sizeof(name), "st,pll@%d", i);
+		plloff[i] = fdt_rcc_subnode_offset(name);
+
+		pllcfg_valid[i] = fdt_check_node(plloff[i]);
+		if (pllcfg_valid[i]) {
+			ret = clk_get_pll_settings_from_dt(plloff[i], pllcfg[i],
+							   &pllfracv[i],
+							   pllcsg[i],
+							   &pllcsg_set[i]);
+			if (ret != 0) {
+				return ret;
+			}
+
+			continue;
+		}
+
+		if ((i == _PLL1) && (pll1_freq_khz != 0U)) {
+			ret = clk_get_pll1_settings(clksrc[CLKSRC_PLL12],
+						    pll1_freq_khz,
+						    pllcfg[i], &pllfracv[i]);
+			if (ret != 0) {
+				return ret;
+			}
+
+			pllcfg_valid[i] = true;
+		}
+	}
+
+	stm32mp1_mco_csg(clksrc[CLKSRC_MCO1], clkdiv[CLKDIV_MCO1]);
+	stm32mp1_mco_csg(clksrc[CLKSRC_MCO2], clkdiv[CLKDIV_MCO2]);
+
+	/*
+	 * Switch ON oscillator found in device-tree.
+	 * Note: HSI already ON after BootROM stage.
+	 */
+	if (stm32mp1_osc[_LSI] != 0U) {
+		stm32mp1_lsi_set(true);
+	}
+	if (stm32mp1_osc[_LSE] != 0U) {
+		const char *name = stm32mp_osc_node_label[_LSE];
+		bool bypass, digbyp;
+		uint32_t lsedrv;
+
+		bypass = fdt_clk_read_bool(name, "st,bypass");
+		digbyp = fdt_clk_read_bool(name, "st,digbypass");
+		lse_css = fdt_clk_read_bool(name, "st,css");
+		lsedrv = fdt_clk_read_uint32_default(name, "st,drive",
+						     LSEDRV_MEDIUM_HIGH);
+		stm32mp1_lse_enable(bypass, digbyp, lsedrv);
+	}
+	if (stm32mp1_osc[_HSE] != 0U) {
+		const char *name = stm32mp_osc_node_label[_HSE];
+		bool bypass, digbyp, css;
+
+		bypass = fdt_clk_read_bool(name, "st,bypass");
+		digbyp = fdt_clk_read_bool(name, "st,digbypass");
+		css = fdt_clk_read_bool(name, "st,css");
+		stm32mp1_hse_enable(bypass, digbyp, css);
+	}
+	/*
+	 * CSI is mandatory for automatic I/O compensation (SYSCFG_CMPCR)
+	 * => switch on CSI even if node is not present in device tree
+	 */
+	stm32mp1_csi_set(true);
+
+	/* Come back to HSI */
+	ret = stm32mp1_set_clksrc(CLK_MPU_HSI);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clksrc(CLK_AXI_HSI);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clksrc(CLK_MCU_HSI);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if ((mmio_read_32(rcc_base + RCC_MP_RSTSCLRR) &
+	     RCC_MP_RSTSCLRR_MPUP0RSTF) != 0) {
+		pll3_preserve = stm32mp1_check_pll_conf(_PLL3,
+							clksrc[CLKSRC_PLL3],
+							pllcfg[_PLL3],
+							plloff[_PLL3]);
+		pll4_preserve = stm32mp1_check_pll_conf(_PLL4,
+							clksrc[CLKSRC_PLL4],
+							pllcfg[_PLL4],
+							plloff[_PLL4]);
+	}
+	/* Don't initialize PLL4, when used by BOOTROM */
+	if ((get_boot_device() == BOOT_DEVICE_USB) &&
+	    ((stgen_p == (int)_PLL4_R) || (usbphy_p == (int)_PLL4_R))) {
+		pll4_bootrom = true;
+		pll4_preserve = true;
+	}
+
+	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
+		if (((i == _PLL3) && pll3_preserve) ||
+		    ((i == _PLL4) && pll4_preserve)) {
+			continue;
+		}
+
+		ret = stm32mp1_pll_stop(i);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	/* Configure HSIDIV */
+	if (stm32mp1_osc[_HSI] != 0U) {
+		ret = stm32mp1_hsidiv(stm32mp1_osc[_HSI]);
+		if (ret != 0) {
+			return ret;
+		}
+
+		stm32mp_stgen_config(stm32mp_clk_get_rate(STGEN_K));
+	}
+
+	/* Select DIV */
+	/* No ready bit when MPUSRC != CLK_MPU_PLL1P_DIV, MPUDIV is disabled */
+	mmio_write_32(rcc_base + RCC_MPCKDIVR,
+		      clkdiv[CLKDIV_MPU] & RCC_DIVR_DIV_MASK);
+	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_AXI], rcc_base + RCC_AXIDIVR);
+	if (ret != 0) {
+		return ret;
+	}
+	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB4], rcc_base + RCC_APB4DIVR);
+	if (ret != 0) {
 		return ret;
 	}
 	ret = stm32mp1_set_clkdiv(clkdiv[CLKDIV_APB5], rcc_base + RCC_APB5DIVR);
@@ -1915,15 +2697,12 @@ int stm32mp1_clk_init(void)
 
 	/* Configure and start PLLs */
 	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
-		uint32_t fracv;
-		uint32_t csg[PLLCSG_NB];
-
 		if (((i == _PLL3) && pll3_preserve) ||
 		    ((i == _PLL4) && pll4_preserve && !pll4_bootrom)) {
 			continue;
 		}
 
-		if (!fdt_check_node(plloff[i])) {
+		if (!pllcfg_valid[i]) {
 			continue;
 		}
 
@@ -1933,25 +2712,20 @@ int stm32mp1_clk_init(void)
 			continue;
 		}
 
-		fracv = fdt_read_uint32_default(fdt, plloff[i], "frac", 0);
-
-		ret = stm32mp1_pll_config(i, pllcfg[i], fracv);
+		ret = stm32mp1_pll_config(i, pllcfg[i], pllfracv[i]);
 		if (ret != 0) {
 			return ret;
 		}
-		ret = fdt_read_uint32_array(fdt, plloff[i], "csg",
-					    (uint32_t)PLLCSG_NB, csg);
-		if (ret == 0) {
-			stm32mp1_pll_csg(i, csg);
-		} else if (ret != -FDT_ERR_NOTFOUND) {
-			return ret;
+
+		if (pllcsg_set[i]) {
+			stm32mp1_pll_csg(i, pllcsg[i]);
 		}
 
 		stm32mp1_pll_start(i);
 	}
 	/* Wait and start PLLs ouptut when ready */
 	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
-		if (!fdt_check_node(plloff[i])) {
+		if (!pllcfg_valid[i]) {
 			continue;
 		}
 
@@ -1985,6 +2759,11 @@ int stm32mp1_clk_init(void)
 	if (pkcs_cell != NULL) {
 		bool ckper_disabled = false;
 		uint32_t j;
+		uint32_t usbreg_bootrom = 0U;
+
+		if (pll4_bootrom) {
+			usbreg_bootrom = mmio_read_32(rcc_base + RCC_USBCKSELR);
+		}
 
 		for (j = 0; j < ((uint32_t)len / sizeof(uint32_t)); j++) {
 			uint32_t pkcs = fdt32_to_cpu(pkcs_cell[j]);
@@ -2005,13 +2784,33 @@ int stm32mp1_clk_init(void)
 		if (ckper_disabled) {
 			stm32mp1_pkcs_config(CLK_CKPER_DISABLED);
 		}
+
+		if (pll4_bootrom) {
+			uint32_t usbreg_value, usbreg_mask;
+			const struct stm32mp1_clk_sel *sel;
+
+			sel = clk_sel_ref(_USBPHY_SEL);
+			usbreg_mask = (uint32_t)sel->msk << sel->src;
+			sel = clk_sel_ref(_USBO_SEL);
+			usbreg_mask |= (uint32_t)sel->msk << sel->src;
+
+			usbreg_value = mmio_read_32(rcc_base + RCC_USBCKSELR) &
+				       usbreg_mask;
+			usbreg_bootrom &= usbreg_mask;
+			if (usbreg_bootrom != usbreg_value) {
+				VERBOSE("forbidden new USB clk path\n");
+				VERBOSE("vs bootrom on USB boot\n");
+				return -FDT_ERR_BADVALUE;
+			}
+		}
 	}
 
 	/* Switch OFF HSI if not found in device-tree */
 	if (stm32mp1_osc[_HSI] == 0U) {
 		stm32mp1_hsi_set(false);
 	}
-	stm32mp1_stgen_config();
+
+	stm32mp_stgen_config(stm32mp_clk_get_rate(STGEN_K));
 
 	/* Software Self-Refresh mode (SSR) during DDR initilialization */
 	mmio_clrsetbits_32(rcc_base + RCC_DDRITFCR,
@@ -2200,6 +2999,429 @@ void stm32mp1_register_clock_parents_secure(unsigned long clock_id)
 }
 #endif /* STM32MP_SHARED_RESOURCES */
 
+/*
+ * 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) }
+
+static 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),
+};
+
+static struct backup_mux_cfg backup_mux4_cfg[] = {
+	MUXCFG(RCC_USBCKSELR, 1),
+};
+
+static void backup_mux_cfg(void)
+{
+	uintptr_t base = stm32mp_rcc_base();
+	struct backup_mux_cfg *cfg;
+	size_t i;
+
+	cfg = backup_mux0_cfg;
+	for (i = 0U; i < ARRAY_SIZE(backup_mux0_cfg); i++) {
+		cfg[i].value = mmio_read_32(base + cfg[i].offset) &
+			       GENMASK_32(cfg[i].bit_len - 1U, 0U);
+	}
+
+	cfg = backup_mux4_cfg;
+	for (i = 0U; i < ARRAY_SIZE(backup_mux4_cfg); i++) {
+		cfg[i].value = mmio_read_32(base + cfg[i].offset) &
+			       GENMASK_32(4U + cfg[i].bit_len - 1U, 4U);
+	}
+}
+
+static void restore_mux_cfg(void)
+{
+	uintptr_t base = stm32mp_rcc_base();
+	struct backup_mux_cfg *cfg;
+	size_t i;
+
+	cfg = backup_mux0_cfg;
+	for (i = 0U; i < ARRAY_SIZE(backup_mux0_cfg); i++) {
+		uint32_t mask = GENMASK_32(cfg[i].bit_len - 1U, 0U);
+		uint32_t value = cfg[i].value & mask;
+
+		mmio_clrsetbits_32(base + cfg[i].offset, mask, value);
+	}
+
+	cfg = backup_mux4_cfg;
+	for (i = 0U; i < ARRAY_SIZE(backup_mux4_cfg); i++) {
+		uint32_t mask = GENMASK_32(4U + cfg[i].bit_len - 1U, 4U);
+		uint32_t value = cfg[i].value & mask;
+
+		mmio_clrsetbits_32(base + cfg[i].offset, mask, 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_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);
+	uintptr_t base = stm32mp_rcc_base();
+	size_t i;
+
+	for (i = 0U; i < count; i++) {
+		cfg[i].value = mmio_read_32(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);
+	uintptr_t base = stm32mp_rcc_base();
+	size_t i;
+
+	for (i = 0U; i < count; i++) {
+		mmio_write_32(base + cfg[i].offset, cfg[i].value);
+		mmio_write_32(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);
+	uintptr_t base = stm32mp_rcc_base();
+	size_t i;
+
+	for (i = 0U; i < count; i++) {
+		cfg[i].value = mmio_read_32(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);
+	uintptr_t base = stm32mp_rcc_base();
+	size_t i;
+
+	for (i = 0U; i < count; i++) {
+		mmio_write_32(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 */
+	mmio_write_32(stm32mp_rcc_base() + RCC_OCENCLRR, ker_mask);
+}
+
+static void enable_kernel_clocks(void)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uint32_t reg;
+	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 = mmio_read_32(rcc_base + RCC_OCENSETR) << 1U;
+	mmio_write_32(rcc_base + RCC_OCENSETR, reg & ker_mask);
+}
+
+static void clear_rcc_reset_status(void)
+{
+	/* Clear reset status fields */
+	mmio_write_32(stm32mp_rcc_base() + RCC_MP_RSTSCLRR, 0U);
+}
+
+void save_clock_pm_context(void)
+{
+	size_t offset = 0U;
+
+	stm32mp1_pm_save_clock_cfg(offset,
+				   (uint8_t *)backup_mux0_cfg,
+				   sizeof(backup_mux0_cfg));
+	offset += sizeof(backup_mux0_cfg);
+
+	stm32mp1_pm_save_clock_cfg(offset,
+				   (uint8_t *)backup_mux4_cfg,
+				   sizeof(backup_mux4_cfg));
+	offset += sizeof(backup_mux4_cfg);
+
+	stm32mp1_pm_save_clock_cfg(offset,
+				   (uint8_t *)backup_clock_sc_cfg,
+				   sizeof(backup_clock_sc_cfg));
+	offset += sizeof(backup_clock_sc_cfg);
+
+	stm32mp1_pm_save_clock_cfg(offset,
+				   (uint8_t *)backup_clock_cfg,
+				   sizeof(backup_clock_cfg));
+	offset += sizeof(backup_clock_cfg);
+
+	stm32mp1_pm_save_clock_cfg(offset,
+				   (uint8_t *)gate_refcounts,
+				   sizeof(gate_refcounts));
+}
+
+void restore_clock_pm_context(void)
+{
+	size_t offset = 0U;
+
+	stm32mp1_pm_restore_clock_cfg(offset,
+				      (uint8_t *)backup_mux0_cfg,
+				      sizeof(backup_mux0_cfg));
+	offset += sizeof(backup_mux0_cfg);
+
+	stm32mp1_pm_restore_clock_cfg(offset,
+				      (uint8_t *)backup_mux4_cfg,
+				      sizeof(backup_mux4_cfg));
+	offset += sizeof(backup_mux4_cfg);
+
+	stm32mp1_pm_restore_clock_cfg(offset,
+				      (uint8_t *)backup_clock_sc_cfg,
+				      sizeof(backup_clock_sc_cfg));
+	offset += sizeof(backup_clock_sc_cfg);
+
+	stm32mp1_pm_restore_clock_cfg(offset,
+				      (uint8_t *)backup_clock_cfg,
+				      sizeof(backup_clock_cfg));
+	offset += sizeof(backup_clock_cfg);
+
+	stm32mp1_pm_restore_clock_cfg(offset,
+				      (uint8_t *)gate_refcounts,
+				      sizeof(gate_refcounts));
+}
+
+void stm32mp1_clock_suspend(void)
+{
+	backup_regular_cfg();
+	backup_sc_cfg();
+	backup_mux_cfg();
+	clear_rcc_reset_status();
+}
+
+void stm32mp1_clock_resume(void)
+{
+	unsigned int idx;
+
+	restore_mux_cfg();
+	restore_sc_cfg();
+	restore_regular_cfg();
+
+	/* Sync secure and shared clocks physical state on functional state */
+	for (idx = 0U; idx < NB_GATES; idx++) {
+		struct stm32mp1_clk_gate const *gate = gate_ref(idx);
+
+		if (gate_is_non_secure(gate)) {
+			continue;
+		}
+
+		if (gate_refcounts[idx] != 0U) {
+			VERBOSE("Resume clock %d enable\n", gate->index);
+			__clk_enable(gate);
+		} else {
+			VERBOSE("Resume clock %d disable\n", gate->index);
+			__clk_disable(gate);
+		}
+	}
+
+	disable_kernel_clocks();
+}
+
+void stm32mp1_clock_stopmode_save(void)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	/* Save registers not restored after STOP mode */
+	pll3cr = mmio_read_32(rcc_base + RCC_PLL3CR);
+	pll4cr = mmio_read_32(rcc_base + RCC_PLL4CR);
+	mssckselr = mmio_read_32(rcc_base + RCC_MSSCKSELR);
+	mcudivr = mmio_read_32(rcc_base + RCC_MCUDIVR) & RCC_MCUDIV_MASK;
+#if STM32MP_SP_MIN_IN_DDR
+	mpapb_iwdg2 = (mmio_read_32(rcc_base + RCC_MP_APB4ENSETR) &
+		    RCC_MP_APB4ENSETR_IWDG2APBEN);
+	mpapb_iwdg1 = (mmio_read_32(rcc_base + RCC_MP_APB5ENSETR) &
+		    RCC_MP_APB5ENSETR_IWDG1APBEN);
+#endif
+
+	enable_kernel_clocks();
+}
+
+static bool pll_is_running(uint32_t pll_offset)
+{
+	uintptr_t pll_cr = stm32mp_rcc_base() + pll_offset;
+
+	return (mmio_read_32(pll_cr) & RCC_PLLNCR_PLLON) != 0U;
+}
+
+static bool pll_was_running(uint32_t saved_value)
+{
+	return (saved_value & RCC_PLLNCR_PLLON) != 0U;
+}
+
+int stm32mp1_clock_stopmode_resume(void)
+{
+	int res;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	if (pll_was_running(pll4cr) && !pll_is_running(RCC_PLL4CR)) {
+		stm32mp1_pll_start(_PLL4);
+	}
+
+	if (pll_was_running(pll3cr)) {
+		if (!pll_is_running(RCC_PLL3CR)) {
+			stm32mp1_pll_start(_PLL3);
+		}
+
+		res = stm32mp1_pll_output(_PLL3,
+					  pll3cr >> RCC_PLLNCR_DIVEN_SHIFT);
+		if (res != 0) {
+			return res;
+		}
+	}
+
+	if (pll_was_running(pll4cr)) {
+		res = stm32mp1_pll_output(_PLL4,
+					  pll4cr >> RCC_PLLNCR_DIVEN_SHIFT);
+		if (res != 0) {
+			return res;
+		}
+	}
+
+	/* Restore MCU clock src after PLL3 RDY */
+	mmio_write_32(rcc_base + RCC_MSSCKSELR, mssckselr);
+
+	/* Restore MCUDIV */
+	res = stm32mp1_set_clkdiv(mcudivr, rcc_base + RCC_MCUDIVR);
+	if (res != 0) {
+		return res;
+	}
+
+#if STM32MP_SP_MIN_IN_DDR
+	/* Restore IWDG clock */
+	mmio_clrsetbits_32(rcc_base + RCC_MP_APB5ENSETR,
+			   RCC_MP_APB5ENSETR_IWDG1APBEN,
+			   mpapb_iwdg1);
+
+	mmio_clrsetbits_32(rcc_base + RCC_MP_APB4ENSETR,
+			   RCC_MP_APB4ENSETR_IWDG2APBEN,
+			   mpapb_iwdg2);
+#endif
+
+	disable_kernel_clocks();
+
+	return 0;
+}
+
+void stm32mp1_clk_mcuss_protect(bool enable)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	if (enable) {
+		mmio_setbits_32(rcc_base + RCC_TZCR, RCC_TZCR_MCKPROT);
+	} else {
+		mmio_clrbits_32(rcc_base + RCC_TZCR, RCC_TZCR_MCKPROT);
+	}
+}
+
+/* Sync secure clock refcount after all drivers probe/inits,  */
+void stm32mp1_dump_clocks_state(void)
+{
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+	unsigned int idx;
+
+	/* Dump clocks state */
+	for (idx = 0U; idx < NB_GATES; idx++) {
+		const struct stm32mp1_clk_gate *gate = gate_ref(idx);
+		unsigned long __unused clock_id = gate->index;
+		unsigned int __unused refcnt = gate_refcounts[idx];
+		int __unused p = stm32mp1_clk_get_parent(clock_id);
+
+		VERBOSE("stm32mp1 clk %lu %sabled (refcnt %d) (parent %d %s)\n",
+			clock_id, __clk_is_enabled(gate) ? "en" : "dis",
+			refcnt, p,
+			p < 0 ? "n.a" : stm32mp1_clk_parent_sel_name[p]);
+	}
+#endif
+}
+
 static void sync_earlyboot_clocks_state(void)
 {
 	unsigned int idx;
@@ -2210,6 +3432,7 @@ static void sync_earlyboot_clocks_state(void)
 		DDRC2, DDRC2LP,
 		DDRCAPB, DDRPHYCAPB, DDRPHYCAPBLP,
 		DDRPHYC, DDRPHYCLP,
+		RTCAPB,
 		TZC1, TZC2,
 		TZPC,
 		STGEN_K,
@@ -2218,17 +3441,41 @@ static void sync_earlyboot_clocks_state(void)
 	for (idx = 0U; idx < ARRAY_SIZE(secure_enable); idx++) {
 		stm32mp_clk_enable(secure_enable[idx]);
 	}
-
-	if (!stm32mp_is_single_core()) {
-		stm32mp1_clk_enable_secure(RTCAPB);
-	}
 }
 
+static const clk_ops_t stm32mp_clk_ops = {
+	.enable		= stm32mp_clk_enable,
+	.disable	= stm32mp_clk_disable,
+	.is_enabled	= stm32mp_clk_is_enabled,
+	.get_rate	= stm32mp_clk_get_rate,
+	.get_parent	= stm32mp1_clk_get_parent,
+};
+
 int stm32mp1_clk_probe(void)
 {
+	unsigned long freq_khz;
+
+	assert(PLLCFG_NB == PLAT_MAX_PLLCFG_NB);
+
+#if defined(IMAGE_BL32)
+	if (!fdt_get_rcc_secure_state()) {
+		mmio_write_32(stm32mp_rcc_base() + RCC_TZCR, 0U);
+	}
+#endif
+
 	stm32mp1_osc_init();
 
 	sync_earlyboot_clocks_state();
 
+	/* Save current CPU operating point value */
+	freq_khz = udiv_round_nearest(stm32mp_clk_get_rate(CK_MPU), 1000UL);
+	if (freq_khz > (unsigned long)UINT32_MAX) {
+		panic();
+	}
+
+	current_opp_khz = (uint32_t)freq_khz;
+
+	clk_register(&stm32mp_clk_ops);
+
 	return 0;
 }
diff --git a/drivers/st/clk/stm32mp_clkfunc.c b/drivers/st/clk/stm32mp_clkfunc.c
index 8333f6dfbf..a119bbf386 100644
--- a/drivers/st/clk/stm32mp_clkfunc.c
+++ b/drivers/st/clk/stm32mp_clkfunc.c
@@ -10,9 +10,15 @@
 
 #include <platform_def.h>
 
+#include <arch_helpers.h>
 #include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
+#include <drivers/generic_delay_timer.h>
 #include <drivers/st/stm32_gpio.h>
 #include <drivers/st/stm32mp_clkfunc.h>
+#include <lib/mmio.h>
+
+#define DT_UART_COMPAT		"st,stm32h7-uart"
 
 /*
  * Get the frequency of an oscillator from its name in device tree.
@@ -43,7 +49,8 @@ int fdt_osc_read_freq(const char *name, uint32_t *freq)
 			return ret;
 		}
 
-		if (strncmp(cchar, name, (size_t)ret) == 0) {
+		if ((strncmp(cchar, name, (size_t)ret) == 0) &&
+		    (fdt_get_status(subnode) != DT_DISABLED)) {
 			const fdt32_t *cuint;
 
 			cuint = fdt_getprop(fdt, subnode, "clock-frequency",
@@ -69,7 +76,7 @@ int fdt_osc_read_freq(const char *name, uint32_t *freq)
  * @param prop_name: property name
  * @return: true/false regarding search result.
  */
-bool fdt_osc_read_bool(enum stm32mp_osc_id osc_id, const char *prop_name)
+bool fdt_clk_read_bool(const char *node_label, const char *prop_name)
 {
 	int node, subnode;
 	void *fdt;
@@ -78,10 +85,6 @@ bool fdt_osc_read_bool(enum stm32mp_osc_id osc_id, const char *prop_name)
 		return false;
 	}
 
-	if (osc_id >= NB_OSC) {
-		return false;
-	}
-
 	node = fdt_path_offset(fdt, "/clocks");
 	if (node < 0) {
 		return false;
@@ -96,8 +99,7 @@ bool fdt_osc_read_bool(enum stm32mp_osc_id osc_id, const char *prop_name)
 			return false;
 		}
 
-		if (strncmp(cchar, stm32mp_osc_node_label[osc_id],
-			    (size_t)ret) != 0) {
+		if (strncmp(cchar, node_label, (size_t)ret) != 0) {
 			continue;
 		}
 
@@ -110,13 +112,13 @@ bool fdt_osc_read_bool(enum stm32mp_osc_id osc_id, const char *prop_name)
 }
 
 /*
- * Get the value of a oscillator property from its ID.
- * @param osc_id: oscillator ID
+ * Get the value of a oscillator property from its name.
+ * @param node_label: oscillator name
  * @param prop_name: property name
  * @param dflt_value: default value
  * @return oscillator value on success, default value if property not found.
  */
-uint32_t fdt_osc_read_uint32_default(enum stm32mp_osc_id osc_id,
+uint32_t fdt_clk_read_uint32_default(const char *node_label,
 				     const char *prop_name, uint32_t dflt_value)
 {
 	int node, subnode;
@@ -126,10 +128,6 @@ uint32_t fdt_osc_read_uint32_default(enum stm32mp_osc_id osc_id,
 		return dflt_value;
 	}
 
-	if (osc_id >= NB_OSC) {
-		return dflt_value;
-	}
-
 	node = fdt_path_offset(fdt, "/clocks");
 	if (node < 0) {
 		return dflt_value;
@@ -144,8 +142,7 @@ uint32_t fdt_osc_read_uint32_default(enum stm32mp_osc_id osc_id,
 			return dflt_value;
 		}
 
-		if (strncmp(cchar, stm32mp_osc_node_label[osc_id],
-			    (size_t)ret) != 0) {
+		if (strncmp(cchar, node_label, (size_t)ret) != 0) {
 			continue;
 		}
 
@@ -161,9 +158,15 @@ uint32_t fdt_osc_read_uint32_default(enum stm32mp_osc_id osc_id,
  * @param fdt: Device tree reference
  * @return: Node offset or a negative value on error
  */
-int fdt_get_rcc_node(void *fdt)
+static int fdt_get_rcc_node(void *fdt)
 {
-	return fdt_node_offset_by_compatible(fdt, -1, DT_RCC_CLK_COMPAT);
+	static int node;
+
+	if (node <= 0) {
+		node = fdt_node_offset_by_compatible(fdt, -1, DT_RCC_CLK_COMPAT);
+	}
+
+	return node;
 }
 
 /*
@@ -191,6 +194,29 @@ int fdt_rcc_read_uint32_array(const char *prop_name, uint32_t count,
 	return fdt_read_uint32_array(fdt, node, prop_name, count, array);
 }
 
+/*******************************************************************************
+ * This function reads a property rcc-clk section.
+ * It reads the values indicated inside the device tree, from property name.
+ * Returns dflt_value if property is not found, and a property value on
+ * success.
+ ******************************************************************************/
+uint32_t fdt_rcc_read_uint32_default(const char *prop_name, uint32_t dflt_value)
+{
+	int node;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return dflt_value;
+	}
+
+	node = fdt_get_rcc_node(fdt);
+	if (node < 0) {
+		return dflt_value;
+	}
+
+	return fdt_read_uint32_default(fdt, node, prop_name, dflt_value);
+}
+
 /*
  * Get the subnode offset in rcc-clk section from its name in device tree
  * @param name: name of the RCC property
@@ -249,24 +275,38 @@ const fdt32_t *fdt_rcc_read_prop(const char *prop_name, int *lenp)
 }
 
 /*
- * Get the secure status for rcc node in device tree.
- * @return: true if rcc is available from secure world, false if not.
+ * Get the secure state for rcc node in device tree.
+ * @return: true if rcc is configured for secure world access, false if not.
  */
-bool fdt_get_rcc_secure_status(void)
+bool fdt_get_rcc_secure_state(void)
 {
-	int node;
 	void *fdt;
 
 	if (fdt_get_address(&fdt) == 0) {
 		return false;
 	}
 
-	node = fdt_get_rcc_node(fdt);
-	if (node < 0) {
+	if (fdt_node_offset_by_compatible(fdt, -1, DT_RCC_SEC_CLK_COMPAT) < 0) {
 		return false;
 	}
 
-	return !!(fdt_get_status(node) & DT_SECURE);
+	return true;
+}
+
+/*
+ * This function gets interrupt name.
+ * It reads the values indicated the enabling status.
+ * Returns 0 if success, and a negative value else.
+ */
+int fdt_rcc_enable_it(const char *name)
+{
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	return stm32_gic_enable_spi(fdt_get_rcc_node(fdt), name);
 }
 
 /*
@@ -291,3 +331,131 @@ int fdt_get_clock_id(int node)
 	cuint++;
 	return (int)fdt32_to_cpu(*cuint);
 }
+
+/*******************************************************************************
+ * This function gets the clock ID of the given node using clock-names.
+ * It reads the value indicated inside the device tree.
+ * Returns ID on success, and a negative FDT/ERRNO error code on failure.
+ ******************************************************************************/
+int fdt_get_clock_id_by_name(int node, const char *name)
+{
+	const fdt32_t *cuint;
+	void *fdt;
+	int index, len;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	index = fdt_stringlist_search(fdt, node, "clock-names", name);
+	if (index < 0) {
+		return index;
+	}
+
+	cuint = fdt_getprop(fdt, node, "clocks", &len);
+	if (cuint == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	if ((index * (int)sizeof(uint32_t)) > len) {
+		return -FDT_ERR_BADVALUE;
+	}
+
+	cuint += (index << 1) + 1;
+	return (int)fdt32_to_cpu(*cuint);
+}
+
+/*
+ * Get the frequency of the specified UART instance.
+ * @param instance: UART interface registers base address.
+ * @return: clock frequency on success, 0 value on failure.
+ */
+unsigned long fdt_get_uart_clock_freq(uintptr_t instance)
+{
+	void *fdt;
+	int node;
+	int clk_id;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return 0UL;
+	}
+
+	/* Check for UART nodes */
+	node = dt_match_instance_by_compatible(DT_UART_COMPAT, instance);
+	if (node < 0) {
+		return 0UL;
+	}
+
+	clk_id = fdt_get_clock_id(node);
+	if (clk_id < 0) {
+		return 0UL;
+	}
+
+	return clk_get_rate((unsigned long)clk_id);
+}
+
+/*******************************************************************************
+ * This function checks if PLL1 hard-coded settings have been defined in DT.
+ * Returns true if PLL1 node is found and enabled, false if not.
+ ******************************************************************************/
+bool fdt_is_pll1_predefined(void)
+{
+	return fdt_check_node(fdt_rcc_subnode_offset(DT_PLL1_NODE_NAME));
+}
+
+/*******************************************************************************
+ * This function configures and restores the STGEN counter depending on the
+ * connected clock.
+ ******************************************************************************/
+void stm32mp_stgen_config(unsigned long rate)
+{
+	uint32_t cntfid0;
+	unsigned long long counter;
+
+	cntfid0 = mmio_read_32(STGEN_BASE + CNTFID_OFF);
+
+	if (cntfid0 == rate) {
+		return;
+	}
+
+	mmio_clrbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
+	counter = stm32mp_stgen_get_counter() * rate / cntfid0;
+
+	mmio_write_32(STGEN_BASE + CNTCVL_OFF, (uint32_t)counter);
+	mmio_write_32(STGEN_BASE + CNTCVU_OFF, (uint32_t)(counter >> 32));
+	mmio_write_32(STGEN_BASE + CNTFID_OFF, rate);
+	mmio_setbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
+
+	write_cntfrq_el0((u_register_t)rate);
+
+	/* Need to update timer with new frequency */
+	generic_delay_timer_init();
+}
+
+/*******************************************************************************
+ * This function returns the STGEN counter value.
+ ******************************************************************************/
+unsigned long long stm32mp_stgen_get_counter(void)
+{
+	return (((unsigned long long)mmio_read_32(STGEN_BASE + CNTCVU_OFF) << 32) |
+		mmio_read_32(STGEN_BASE + CNTCVL_OFF));
+}
+
+/*******************************************************************************
+ * This function restores the STGEN counter value.
+ * It takes a first input value as a counter backup value to be restored and a
+ * offset in ms to be added.
+ ******************************************************************************/
+void stm32mp_stgen_restore_counter(unsigned long long value,
+				   unsigned long long offset_in_ms)
+{
+	unsigned long long cnt;
+
+	cnt = value + ((offset_in_ms *
+			mmio_read_32(STGEN_BASE + CNTFID_OFF)) / 1000U);
+
+	mmio_clrbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
+	mmio_write_32(STGEN_BASE + CNTCVL_OFF, (uint32_t)cnt);
+	mmio_write_32(STGEN_BASE + CNTCVU_OFF, (uint32_t)(cnt >> 32));
+	mmio_setbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN);
+}
diff --git a/drivers/st/crypto/stm32_hash.c b/drivers/st/crypto/stm32_hash.c
index 317fd9eb88..9ee64a57e7 100644
--- a/drivers/st/crypto/stm32_hash.c
+++ b/drivers/st/crypto/stm32_hash.c
@@ -14,6 +14,7 @@
 
 #include <arch_helpers.h>
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/st/stm32_hash.h>
 #include <drivers/st/stm32mp_reset.h>
@@ -189,7 +190,7 @@ int stm32_hash_update(const uint8_t *buffer, size_t length)
 		return 0;
 	}
 
-	stm32mp_clk_enable(stm32_hash.clock);
+	clk_enable(stm32_hash.clock);
 
 	if (stm32_remain.length != 0U) {
 		uint32_t copysize;
@@ -231,7 +232,7 @@ int stm32_hash_update(const uint8_t *buffer, size_t length)
 	}
 
 exit:
-	stm32mp_clk_disable(stm32_hash.clock);
+	clk_disable(stm32_hash.clock);
 
 	return ret;
 }
@@ -240,12 +241,12 @@ int stm32_hash_final(uint8_t *digest)
 {
 	int ret;
 
-	stm32mp_clk_enable(stm32_hash.clock);
+	clk_enable(stm32_hash.clock);
 
 	if (stm32_remain.length != 0U) {
 		ret = hash_write_data(stm32_remain.buffer);
 		if (ret != 0) {
-			stm32mp_clk_disable(stm32_hash.clock);
+			clk_disable(stm32_hash.clock);
 			return ret;
 		}
 
@@ -260,7 +261,7 @@ int stm32_hash_final(uint8_t *digest)
 
 	ret = hash_get_digest(digest);
 
-	stm32mp_clk_disable(stm32_hash.clock);
+	clk_disable(stm32_hash.clock);
 
 	return ret;
 }
@@ -280,11 +281,11 @@ int stm32_hash_final_update(const uint8_t *buffer, uint32_t length,
 
 void stm32_hash_init(enum stm32_hash_algo_mode mode)
 {
-	stm32mp_clk_enable(stm32_hash.clock);
+	clk_enable(stm32_hash.clock);
 
 	hash_hw_init(mode);
 
-	stm32mp_clk_disable(stm32_hash.clock);
+	clk_disable(stm32_hash.clock);
 
 	zeromem(&stm32_remain, sizeof(stm32_remain));
 }
@@ -321,7 +322,7 @@ int stm32_hash_register(void)
 	stm32_hash.base = hash_info.base;
 	stm32_hash.clock = hash_info.clock;
 
-	stm32mp_clk_enable(stm32_hash.clock);
+	clk_enable(stm32_hash.clock);
 
 	if (hash_info.reset >= 0) {
 		uint32_t id = (uint32_t)hash_info.reset;
@@ -335,7 +336,7 @@ int stm32_hash_register(void)
 		}
 	}
 
-	stm32mp_clk_disable(stm32_hash.clock);
+	clk_disable(stm32_hash.clock);
 
 	return 0;
 }
diff --git a/drivers/st/ddr/stm32mp1_ddr.c b/drivers/st/ddr/stm32mp1_ddr.c
index 7d89d027e6..7e8041743e 100644
--- a/drivers/st/ddr/stm32mp1_ddr.c
+++ b/drivers/st/ddr/stm32mp1_ddr.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2018-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (C) 2018-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
  */
@@ -12,6 +12,7 @@
 #include <arch.h>
 #include <arch_helpers.h>
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/st/stm32mp_pmic.h>
 #include <drivers/st/stm32mp1_ddr.h>
@@ -29,6 +30,7 @@ struct reg_desc {
 
 #define INVALID_OFFSET	0xFFU
 
+#define TIMESLOT_US_1US	1U
 #define TIMEOUT_US_1S	1000000U
 
 #define DDRCTL_REG(x, y)					\
@@ -45,8 +47,34 @@ struct reg_desc {
 		.par_offset = offsetof(struct y, x)		\
 	}
 
+/*
+ * PARAMETERS: value get from device tree :
+ *             size / order need to be aligned with binding
+ *             modification NOT ALLOWED !!!
+ */
+#define DDRCTL_REG_REG_SIZE	25	/* st,ctl-reg */
+#define DDRCTL_REG_TIMING_SIZE	12	/* st,ctl-timing */
+#define DDRCTL_REG_MAP_SIZE	9	/* st,ctl-map */
+#if STM32MP_DDR_DUAL_AXI_PORT
+#define DDRCTL_REG_PERF_SIZE	17	/* st,ctl-perf */
+#else
+#define DDRCTL_REG_PERF_SIZE	11	/* st,ctl-perf */
+#endif
+
+#if STM32MP_DDR_DUAL_AXI_PORT
+#define DDRPHY_REG_REG_SIZE	11	/* st,phy-reg */
+#else
+#define DDRPHY_REG_REG_SIZE	9	/* st,phy-reg */
+#endif
+#define	DDRPHY_REG_TIMING_SIZE	10	/* st,phy-timing */
+#if STM32MP_DDR_DUAL_AXI_PORT
+#define	DDRPHY_REG_CAL_SIZE	12	/* st,phy-cal */
+#else
+#define	DDRPHY_REG_CAL_SIZE	6	/* st,phy-cal */
+#endif
+
 #define DDRCTL_REG_REG(x)	DDRCTL_REG(x, stm32mp1_ddrctrl_reg)
-static const struct reg_desc ddr_reg[] = {
+static const struct reg_desc ddr_reg[DDRCTL_REG_REG_SIZE] = {
 	DDRCTL_REG_REG(mstr),
 	DDRCTL_REG_REG(mrctrl0),
 	DDRCTL_REG_REG(mrctrl1),
@@ -75,7 +103,7 @@ static const struct reg_desc ddr_reg[] = {
 };
 
 #define DDRCTL_REG_TIMING(x)	DDRCTL_REG(x, stm32mp1_ddrctrl_timing)
-static const struct reg_desc ddr_timing[] = {
+static const struct reg_desc ddr_timing[DDRCTL_REG_TIMING_SIZE] = {
 	DDRCTL_REG_TIMING(rfshtmg),
 	DDRCTL_REG_TIMING(dramtmg0),
 	DDRCTL_REG_TIMING(dramtmg1),
@@ -91,7 +119,7 @@ static const struct reg_desc ddr_timing[] = {
 };
 
 #define DDRCTL_REG_MAP(x)	DDRCTL_REG(x, stm32mp1_ddrctrl_map)
-static const struct reg_desc ddr_map[] = {
+static const struct reg_desc ddr_map[DDRCTL_REG_MAP_SIZE] = {
 	DDRCTL_REG_MAP(addrmap1),
 	DDRCTL_REG_MAP(addrmap2),
 	DDRCTL_REG_MAP(addrmap3),
@@ -104,7 +132,7 @@ static const struct reg_desc ddr_map[] = {
 };
 
 #define DDRCTL_REG_PERF(x)	DDRCTL_REG(x, stm32mp1_ddrctrl_perf)
-static const struct reg_desc ddr_perf[] = {
+static const struct reg_desc ddr_perf[DDRCTL_REG_PERF_SIZE] = {
 	DDRCTL_REG_PERF(sched),
 	DDRCTL_REG_PERF(sched1),
 	DDRCTL_REG_PERF(perfhpr1),
@@ -116,16 +144,18 @@ static const struct reg_desc ddr_perf[] = {
 	DDRCTL_REG_PERF(pcfgqos1_0),
 	DDRCTL_REG_PERF(pcfgwqos0_0),
 	DDRCTL_REG_PERF(pcfgwqos1_0),
+#if STM32MP_DDR_DUAL_AXI_PORT
 	DDRCTL_REG_PERF(pcfgr_1),
 	DDRCTL_REG_PERF(pcfgw_1),
 	DDRCTL_REG_PERF(pcfgqos0_1),
 	DDRCTL_REG_PERF(pcfgqos1_1),
 	DDRCTL_REG_PERF(pcfgwqos0_1),
 	DDRCTL_REG_PERF(pcfgwqos1_1),
+#endif
 };
 
 #define DDRPHY_REG_REG(x)	DDRPHY_REG(x, stm32mp1_ddrphy_reg)
-static const struct reg_desc ddrphy_reg[] = {
+static const struct reg_desc ddrphy_reg[DDRPHY_REG_REG_SIZE] = {
 	DDRPHY_REG_REG(pgcr),
 	DDRPHY_REG_REG(aciocr),
 	DDRPHY_REG_REG(dxccr),
@@ -135,12 +165,14 @@ static const struct reg_desc ddrphy_reg[] = {
 	DDRPHY_REG_REG(zq0cr1),
 	DDRPHY_REG_REG(dx0gcr),
 	DDRPHY_REG_REG(dx1gcr),
+#if STM32MP_DDR_DUAL_AXI_PORT
 	DDRPHY_REG_REG(dx2gcr),
 	DDRPHY_REG_REG(dx3gcr),
+#endif
 };
 
 #define DDRPHY_REG_TIMING(x)	DDRPHY_REG(x, stm32mp1_ddrphy_timing)
-static const struct reg_desc ddrphy_timing[] = {
+static const struct reg_desc ddrphy_timing[DDRPHY_REG_TIMING_SIZE] = {
 	DDRPHY_REG_TIMING(ptr0),
 	DDRPHY_REG_TIMING(ptr1),
 	DDRPHY_REG_TIMING(ptr2),
@@ -154,51 +186,26 @@ static const struct reg_desc ddrphy_timing[] = {
 };
 
 #define DDRPHY_REG_CAL(x)	DDRPHY_REG(x, stm32mp1_ddrphy_cal)
-static const struct reg_desc ddrphy_cal[] = {
+static const struct reg_desc ddrphy_cal[DDRPHY_REG_CAL_SIZE] = {
 	DDRPHY_REG_CAL(dx0dllcr),
 	DDRPHY_REG_CAL(dx0dqtr),
 	DDRPHY_REG_CAL(dx0dqstr),
 	DDRPHY_REG_CAL(dx1dllcr),
 	DDRPHY_REG_CAL(dx1dqtr),
 	DDRPHY_REG_CAL(dx1dqstr),
+#if STM32MP_DDR_DUAL_AXI_PORT
 	DDRPHY_REG_CAL(dx2dllcr),
 	DDRPHY_REG_CAL(dx2dqtr),
 	DDRPHY_REG_CAL(dx2dqstr),
 	DDRPHY_REG_CAL(dx3dllcr),
 	DDRPHY_REG_CAL(dx3dqtr),
 	DDRPHY_REG_CAL(dx3dqstr),
+#endif
 };
 
-#define DDR_REG_DYN(x)						\
-	{							\
-		.name = #x,					\
-		.offset = offsetof(struct stm32mp1_ddrctl, x),	\
-		.par_offset = INVALID_OFFSET \
-	}
-
-static const struct reg_desc ddr_dyn[] = {
-	DDR_REG_DYN(stat),
-	DDR_REG_DYN(init0),
-	DDR_REG_DYN(dfimisc),
-	DDR_REG_DYN(dfistat),
-	DDR_REG_DYN(swctl),
-	DDR_REG_DYN(swstat),
-	DDR_REG_DYN(pctrl_0),
-	DDR_REG_DYN(pctrl_1),
-};
-
-#define DDRPHY_REG_DYN(x)					\
-	{							\
-		.name = #x,					\
-		.offset = offsetof(struct stm32mp1_ddrphy, x),	\
-		.par_offset = INVALID_OFFSET			\
-	}
-
-static const struct reg_desc ddrphy_dyn[] = {
-	DDRPHY_REG_DYN(pir),
-	DDRPHY_REG_DYN(pgsr),
-};
-
+/*
+ * REGISTERS ARRAY: used to parse device tree and interactive mode
+ */
 enum reg_type {
 	REG_REG,
 	REG_TIMING,
@@ -207,12 +214,6 @@ enum reg_type {
 	REGPHY_REG,
 	REGPHY_TIMING,
 	REGPHY_CAL,
-/*
- * Dynamic registers => managed in driver or not changed,
- * can be dumped in interactive mode.
- */
-	REG_DYN,
-	REGPHY_DYN,
 	REG_TYPE_NB
 };
 
@@ -233,55 +234,43 @@ static const struct ddr_reg_info ddr_registers[REG_TYPE_NB] = {
 	[REG_REG] = {
 		.name = "static",
 		.desc = ddr_reg,
-		.size = ARRAY_SIZE(ddr_reg),
+		.size = DDRCTL_REG_REG_SIZE,
 		.base = DDR_BASE
 	},
 	[REG_TIMING] = {
 		.name = "timing",
 		.desc = ddr_timing,
-		.size = ARRAY_SIZE(ddr_timing),
+		.size = DDRCTL_REG_TIMING_SIZE,
 		.base = DDR_BASE
 	},
 	[REG_PERF] = {
 		.name = "perf",
 		.desc = ddr_perf,
-		.size = ARRAY_SIZE(ddr_perf),
+		.size = DDRCTL_REG_PERF_SIZE,
 		.base = DDR_BASE
 	},
 	[REG_MAP] = {
 		.name = "map",
 		.desc = ddr_map,
-		.size = ARRAY_SIZE(ddr_map),
+		.size = DDRCTL_REG_MAP_SIZE,
 		.base = DDR_BASE
 	},
 	[REGPHY_REG] = {
 		.name = "static",
 		.desc = ddrphy_reg,
-		.size = ARRAY_SIZE(ddrphy_reg),
+		.size = DDRPHY_REG_REG_SIZE,
 		.base = DDRPHY_BASE
 	},
 	[REGPHY_TIMING] = {
 		.name = "timing",
 		.desc = ddrphy_timing,
-		.size = ARRAY_SIZE(ddrphy_timing),
+		.size = DDRPHY_REG_TIMING_SIZE,
 		.base = DDRPHY_BASE
 	},
 	[REGPHY_CAL] = {
 		.name = "cal",
 		.desc = ddrphy_cal,
-		.size = ARRAY_SIZE(ddrphy_cal),
-		.base = DDRPHY_BASE
-	},
-	[REG_DYN] = {
-		.name = "dyn",
-		.desc = ddr_dyn,
-		.size = ARRAY_SIZE(ddr_dyn),
-		.base = DDR_BASE
-	},
-	[REGPHY_DYN] = {
-		.name = "dyn",
-		.desc = ddrphy_dyn,
-		.size = ARRAY_SIZE(ddrphy_dyn),
+		.size = DDRPHY_REG_CAL_SIZE,
 		.base = DDRPHY_BASE
 	},
 };
@@ -627,7 +616,7 @@ static void stm32mp1_ddr3_dll_off(struct ddr_info *priv)
 	 */
 
 	/* Change Bypass Mode Frequency Range */
-	if (stm32mp_clk_get_rate(DDRPHYC) < 100000000U) {
+	if (clk_get_rate(DDRPHYC) < 100000000U) {
 		mmio_clrbits_32((uintptr_t)&priv->phy->dllgcr,
 				DDRPHYC_DLLGCR_BPS200);
 	} else {
@@ -641,10 +630,12 @@ static void stm32mp1_ddr3_dll_off(struct ddr_info *priv)
 			DDRPHYC_DXNDLLCR_DLLDIS);
 	mmio_setbits_32((uintptr_t)&priv->phy->dx1dllcr,
 			DDRPHYC_DXNDLLCR_DLLDIS);
+#if STM32MP_DDR_DUAL_AXI_PORT
 	mmio_setbits_32((uintptr_t)&priv->phy->dx2dllcr,
 			DDRPHYC_DXNDLLCR_DLLDIS);
 	mmio_setbits_32((uintptr_t)&priv->phy->dx3dllcr,
 			DDRPHYC_DXNDLLCR_DLLDIS);
+#endif
 
 	/* 12. Exit the self-refresh state by setting PWRCTL.selfref_sw = 0. */
 	mmio_clrbits_32((uintptr_t)&priv->ctl->pwrctl,
@@ -675,7 +666,8 @@ static void stm32mp1_refresh_disable(struct stm32mp1_ddrctl *ctl)
 	/* Quasi-dynamic register update*/
 	mmio_setbits_32((uintptr_t)&ctl->rfshctl3,
 			DDRCTRL_RFSHCTL3_DIS_AUTO_REFRESH);
-	mmio_clrbits_32((uintptr_t)&ctl->pwrctl, DDRCTRL_PWRCTL_POWERDOWN_EN);
+	mmio_clrbits_32((uintptr_t)&ctl->pwrctl, DDRCTRL_PWRCTL_POWERDOWN_EN |
+						 DDRCTRL_PWRCTL_SELFREF_EN);
 	mmio_clrbits_32((uintptr_t)&ctl->dfimisc,
 			DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
 	stm32mp1_wait_sw_done_ack(ctl);
@@ -693,11 +685,92 @@ static void stm32mp1_refresh_restore(struct stm32mp1_ddrctl *ctl,
 		mmio_setbits_32((uintptr_t)&ctl->pwrctl,
 				DDRCTRL_PWRCTL_POWERDOWN_EN);
 	}
+	if ((pwrctl & DDRCTRL_PWRCTL_SELFREF_EN) != 0U) {
+		mmio_setbits_32((uintptr_t)&ctl->pwrctl,
+				DDRCTRL_PWRCTL_SELFREF_EN);
+	}
 	mmio_setbits_32((uintptr_t)&ctl->dfimisc,
 			DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
 	stm32mp1_wait_sw_done_ack(ctl);
 }
 
+static void stm32mp1_refresh_cmd(struct stm32mp1_ddrctl *ctl)
+{
+	uint32_t dbgstat;
+
+	do {
+		dbgstat = mmio_read_32((uintptr_t)&ctl->dbgstat);
+	} while ((dbgstat & DDRCTRL_DBGSTAT_RANK0_REFRESH_BUSY) != 0U);
+
+	mmio_setbits_32((uintptr_t)&ctl->dbgcmd, DDRCTRL_DBGCMD_RANK0_REFRESH);
+}
+
+/* Refresh compensation by forcing refresh command
+ * Rule1: Tref should be always < tREFW ? R x tREBW/8
+ * Rule2: refcomp = RU(Tref/tREFI)  = RU(RxTref/tREFW)
+ */
+static
+void stm32mp1_refresh_compensation(const struct stm32mp1_ddr_config *config,
+				   struct stm32mp1_ddrctl *ctl,
+				   uint64_t start)
+{
+	uint32_t tck_ps;
+	uint64_t time_us, tref, trefi, refcomp, i;
+
+	time_us = timeout_init_us(0) - start;
+	tck_ps = 1000000000U / config->info.speed;
+	if (tck_ps == 0U) {
+		return;
+	}
+	/* ref = refresh time in tck */
+	tref = time_us * 1000000U / tck_ps;
+	trefi = ((mmio_read_32((uintptr_t)&ctl->rfshtmg) &
+		  DDRCTRL_RFSHTMG_T_RFC_NOM_X1_X32_MASK)
+		 >> DDRCTRL_RFSHTMG_T_RFC_NOM_X1_X32_SHIFT) * 32U;
+	if (trefi == 0U) {
+		return;
+	}
+
+	/* div round up : number of refresh to compensate */
+	refcomp = (tref + trefi - 1U) / trefi;
+
+	for (i = 0; i < refcomp; i++) {
+		stm32mp1_refresh_cmd(ctl);
+	}
+}
+
+static void stm32mp1_self_refresh_zcal(struct ddr_info *priv, uint32_t zdata)
+{
+	/* sequence for PUBL I/O Data Retention during Power-Down */
+
+	/* 10. Override ZQ calibration with previously (pre-retention)
+	 *     calibrated values. This is done by writing 1 to ZQ0CRN.ZDEN
+	 *     and the override data to ZQ0CRN.ZDATA.
+	 */
+	mmio_setbits_32((uintptr_t)&priv->phy->zq0cr0, DDRPHYC_ZQ0CRN_ZDEN);
+
+	mmio_clrsetbits_32((uintptr_t)&priv->phy->zq0cr0,
+			   DDRPHYC_ZQ0CRN_ZDATA_MASK,
+			   zdata << DDRPHYC_ZQ0CRN_ZDATA_SHIFT);
+
+	/* 11. De-assert the PHY_top data retention enable signals
+	 *     (ret_en or ret_en_i/ret_en_n_i).
+	 */
+	mmio_setbits_32((uintptr_t)(priv->pwr) + PWR_CR3, PWR_CR3_DDRSRDIS);
+	mmio_clrbits_32((uintptr_t)(priv->pwr) + PWR_CR3, PWR_CR3_DDRRETEN);
+
+	/* 12. Remove ZQ calibration override by writing 0 to ZQ0CRN.ZDEN. */
+	mmio_clrbits_32((uintptr_t)&priv->phy->zq0cr0, DDRPHYC_ZQ0CRN_ZDEN);
+
+	/* 13. Trigger ZQ calibration by writing 1 to PIR.INIT
+	 *     and '1' to PIR.ZCAL
+	 */
+	/* 14. Wait for ZQ calibration to finish by polling a 1 status
+	 * on PGSR.IDONE.
+	 */
+	stm32mp1_ddrphy_init(priv->phy, DDRPHYC_PIR_ZCAL);
+}
+
 static int board_ddr_power_init(enum ddr_type ddr_type)
 {
 	if (dt_pmic_status() > 0) {
@@ -710,7 +783,7 @@ static int board_ddr_power_init(enum ddr_type ddr_type)
 void stm32mp1_ddr_init(struct ddr_info *priv,
 		       struct stm32mp1_ddr_config *config)
 {
-	uint32_t pir;
+	uint32_t pir, ddr_reten;
 	int ret = -EINVAL;
 
 	if ((config->c_reg.mstr & DDRCTRL_MSTR_DDR3) != 0U) {
@@ -730,6 +803,25 @@ void stm32mp1_ddr_init(struct ddr_info *priv,
 	VERBOSE("name = %s\n", config->info.name);
 	VERBOSE("speed = %d kHz\n", config->info.speed);
 	VERBOSE("size  = 0x%x\n", config->info.size);
+	if (config->self_refresh) {
+		VERBOSE("sel-refresh exit (zdata = 0x%x)\n", config->zdata);
+	}
+
+	/* Check DDR PHY pads retention */
+	ddr_reten = mmio_read_32((uint32_t)(priv->pwr) + PWR_CR3) &
+		    PWR_CR3_DDRRETEN;
+	if (config->self_refresh) {
+		if (ddr_reten == 0U) {
+			VERBOSE("self-refresh aborted: no retention\n");
+			config->self_refresh = false;
+		}
+	}
+
+	if (!config->self_refresh) {
+		VERBOSE("disable DDR PHY retention\n");
+		mmio_setbits_32((uint32_t)(priv->pwr) + PWR_CR3, PWR_CR3_DDRSRDIS);
+		mmio_clrbits_32((uint32_t)(priv->pwr) + PWR_CR3, PWR_CR3_DDRRETEN);
+	}
 
 	/* DDR INIT SEQUENCE */
 
@@ -790,6 +882,12 @@ void stm32mp1_ddr_init(struct ddr_info *priv,
 	set_reg(priv, REG_TIMING, &config->c_timing);
 	set_reg(priv, REG_MAP, &config->c_map);
 
+	/* Keep the controller in self-refresh mode */
+	if (config->self_refresh) {
+		mmio_setbits_32((uintptr_t)&priv->ctl->pwrctl,
+				DDRCTRL_PWRCTL_SELFREF_SW);
+	}
+
 	/* Skip CTRL init, SDRAM init is done by PHY PUBL */
 	mmio_clrsetbits_32((uintptr_t)&priv->ctl->init0,
 			   DDRCTRL_INIT0_SKIP_DRAM_INIT_MASK,
@@ -811,7 +909,9 @@ void stm32mp1_ddr_init(struct ddr_info *priv,
 	 */
 	set_reg(priv, REGPHY_REG, &config->p_reg);
 	set_reg(priv, REGPHY_TIMING, &config->p_timing);
-	set_reg(priv, REGPHY_CAL, &config->p_cal);
+	if (config->p_cal_present) {
+		set_reg(priv, REGPHY_CAL, &config->p_cal);
+	}
 
 	/* DDR3 = don't set DLLOFF for init mode */
 	if ((config->c_reg.mstr &
@@ -843,8 +943,20 @@ void stm32mp1_ddr_init(struct ddr_info *priv,
 		pir |= DDRPHYC_PIR_DRAMRST; /* Only for DDR3 */
 	}
 
+	/* Treat self-refresh exit : hot boot */
+	if (config->self_refresh) {
+		/* DDR in self refresh mode, remove zcal & reset & init */
+		pir &= ~(DDRPHYC_PIR_ZCAL & DDRPHYC_PIR_DRAMRST
+			 & DDRPHYC_PIR_DRAMINIT);
+		pir |= DDRPHYC_PIR_ZCALBYP;
+	}
+
 	stm32mp1_ddrphy_init(priv->phy, pir);
 
+	if (config->self_refresh) {
+		stm32mp1_self_refresh_zcal(priv, config->zdata);
+	}
+
 	/*
 	 *  6. SET DFIMISC.dfi_init_complete_en to 1
 	 *  Enable quasi-dynamic register programming.
@@ -865,6 +977,13 @@ void stm32mp1_ddr_init(struct ddr_info *priv,
 	 */
 
 	/* Wait uMCTL2 ready */
+
+	/* Trigger self-refresh exit */
+	if (config->self_refresh) {
+		mmio_clrbits_32((uintptr_t)&priv->ctl->pwrctl,
+				DDRCTRL_PWRCTL_SELFREF_SW);
+	}
+
 	stm32mp1_wait_operating_mode(priv, DDRCTRL_STAT_OPERATING_MODE_NORMAL);
 
 	/* Switch to DLL OFF mode */
@@ -872,37 +991,53 @@ void stm32mp1_ddr_init(struct ddr_info *priv,
 		stm32mp1_ddr3_dll_off(priv);
 	}
 
-	VERBOSE("DDR DQS training : ");
-
-	/*
-	 *  8. Disable Auto refresh and power down by setting
-	 *    - RFSHCTL3.dis_au_refresh = 1
-	 *    - PWRCTL.powerdown_en = 0
-	 *    - DFIMISC.dfiinit_complete_en = 0
-	 */
-	stm32mp1_refresh_disable(priv->ctl);
-
-	/*
-	 *  9. Program PUBL PGCR to enable refresh during training
-	 *     and rank to train
-	 *     not done => keep the programed value in PGCR
-	 */
-
-	/*
-	 * 10. configure PUBL PIR register to specify which training step
-	 * to run
-	 * Warning : RVTRN  is not supported by this PUBL
-	 */
-	stm32mp1_ddrphy_init(priv->phy, DDRPHYC_PIR_QSTRN);
-
-	/* 11. monitor PUB PGSR.IDONE to poll cpmpletion of training sequence */
-	stm32mp1_ddrphy_idone_wait(priv->phy);
+	if (config->p_cal_present) {
+		VERBOSE("DDR DQS training skipped.\n");
+	} else {
+		uint64_t time;
+
+		VERBOSE("DDR DQS training.\n");
+
+		time = timeout_init_us(0);
+
+		/*
+		 *  8. Disable Auto refresh and power down by setting
+		 *    - RFSHCTL3.dis_au_refresh = 1
+		 *    - PWRCTL.powerdown_en = 0
+		 *    - DFIMISC.dfiinit_complete_en = 0
+		 */
+		stm32mp1_refresh_disable(priv->ctl);
+
+		/*
+		 *  9. Program PUBL PGCR to enable refresh during training
+		 *     and rank to train
+		 *     not done => keep the programed value in PGCR
+		 */
+
+		/*
+		 * 10. configure PUBL PIR register to specify which training
+		 *     step to run
+		 * Warning : RVTRN  is not supported by this PUBL
+		 */
+		stm32mp1_ddrphy_init(priv->phy, DDRPHYC_PIR_QSTRN);
+
+		/* 11. monitor PUB PGSR.IDONE to poll cpmpletion of training
+		 *     sequence
+		 */
+		stm32mp1_ddrphy_idone_wait(priv->phy);
+
+		/* Refresh compensation: forcing refresh command */
+		if (config->self_refresh) {
+			stm32mp1_refresh_compensation(config, priv->ctl, time);
+		}
 
-	/*
-	 * 12. set back registers in step 8 to the orginal values if desidered
-	 */
-	stm32mp1_refresh_restore(priv->ctl, config->c_reg.rfshctl3,
-				 config->c_reg.pwrctl);
+		/*
+		 * 12. set back registers in step 8 to the orginal values
+		 *     if desidered
+		 */
+		stm32mp1_refresh_restore(priv->ctl, config->c_reg.rfshctl3,
+					 config->c_reg.pwrctl);
+	}
 
 	/* Enable uMCTL2 AXI port 0 */
 	mmio_setbits_32((uintptr_t)&priv->ctl->pctrl_0,
@@ -911,10 +1046,12 @@ void stm32mp1_ddr_init(struct ddr_info *priv,
 		(uintptr_t)&priv->ctl->pctrl_0,
 		mmio_read_32((uintptr_t)&priv->ctl->pctrl_0));
 
+#if STM32MP_DDR_DUAL_AXI_PORT
 	/* Enable uMCTL2 AXI port 1 */
 	mmio_setbits_32((uintptr_t)&priv->ctl->pctrl_1,
 			DDRCTRL_PCTRL_N_PORT_EN);
 	VERBOSE("[0x%lx] pctrl_1 = 0x%x\n",
 		(uintptr_t)&priv->ctl->pctrl_1,
 		mmio_read_32((uintptr_t)&priv->ctl->pctrl_1));
+#endif
 }
diff --git a/drivers/st/ddr/stm32mp1_ddr_helpers.c b/drivers/st/ddr/stm32mp1_ddr_helpers.c
index fcb4cfcfdf..dab63d191b 100644
--- a/drivers/st/ddr/stm32mp1_ddr_helpers.c
+++ b/drivers/st/ddr/stm32mp1_ddr_helpers.c
@@ -1,24 +1,615 @@
 /*
- * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #include <platform_def.h>
 
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp1_ddr.h>
 #include <drivers/st/stm32mp1_ddr_helpers.h>
+#include <drivers/st/stm32mp1_ddr_regs.h>
 #include <lib/mmio.h>
 
+#define TIMEOUT_500US	500U
+
+static enum stm32mp1_ddr_sr_mode saved_ddr_sr_mode;
+
 void ddr_enable_clock(void)
 {
 	stm32mp1_clk_rcc_regs_lock();
 
 	mmio_setbits_32(stm32mp_rcc_base() + RCC_DDRITFCR,
 			RCC_DDRITFCR_DDRC1EN |
+#if STM32MP_DDR_DUAL_AXI_PORT
 			RCC_DDRITFCR_DDRC2EN |
+#endif
 			RCC_DDRITFCR_DDRPHYCEN |
 			RCC_DDRITFCR_DDRPHYCAPBEN |
 			RCC_DDRITFCR_DDRCAPBEN);
 
 	stm32mp1_clk_rcc_regs_unlock();
 }
+
+static void do_sw_handshake(void)
+{
+	uintptr_t ddrctrl_base = stm32mp_ddrctrl_base();
+
+	mmio_clrbits_32(ddrctrl_base + DDRCTRL_SWCTL, DDRCTRL_SWCTL_SW_DONE);
+}
+
+static void do_sw_ack(void)
+{
+	uint64_t timeout;
+	uintptr_t ddrctrl_base = stm32mp_ddrctrl_base();
+
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_SWCTL, DDRCTRL_SWCTL_SW_DONE);
+
+	timeout = timeout_init_us(TIMEOUT_500US);
+	while ((mmio_read_32(ddrctrl_base + DDRCTRL_SWSTAT) &
+		DDRCTRL_SWSTAT_SW_DONE_ACK) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			panic();
+		}
+	}
+}
+
+static int ddr_sw_self_refresh_in(void)
+{
+	uint64_t timeout;
+	uint32_t stat;
+	uint32_t operating_mode;
+	uint32_t selref_type;
+	uint8_t op_mode_changed = 0;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uintptr_t pwr_base = stm32mp_pwr_base();
+	uintptr_t ddrctrl_base = stm32mp_ddrctrl_base();
+	uintptr_t ddrphyc_base = stm32mp_ddrphyc_base();
+
+	stm32mp1_clk_rcc_regs_lock();
+
+	mmio_clrbits_32(rcc_base + RCC_DDRITFCR, RCC_DDRITFCR_AXIDCGEN);
+
+	stm32mp1_clk_rcc_regs_unlock();
+
+	/* Blocks AXI ports from taking anymore transactions */
+	mmio_clrbits_32(ddrctrl_base + DDRCTRL_PCTRL_0,
+			DDRCTRL_PCTRL_N_PORT_EN);
+#if STM32MP_DDR_DUAL_AXI_PORT
+	mmio_clrbits_32(ddrctrl_base + DDRCTRL_PCTRL_1,
+			DDRCTRL_PCTRL_N_PORT_EN);
+#endif
+
+	/* Waits unit all AXI ports are idle
+	 * Poll PSTAT.rd_port_busy_n = 0
+	 * Poll PSTAT.wr_port_busy_n = 0
+	 */
+	timeout = timeout_init_us(TIMEOUT_500US);
+	while (mmio_read_32(ddrctrl_base + DDRCTRL_PSTAT)) {
+		if (timeout_elapsed(timeout)) {
+			goto pstat_failed;
+		}
+	}
+	/* SW Self-Refresh entry */
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_PWRCTL,
+			DDRCTRL_PWRCTL_SELFREF_SW);
+
+	/* Wait operating mode change in self-refresh mode
+	 * with STAT.operating_mode[1:0]==11.
+	 * Ensure transition to self-refresh was due to software
+	 * by checking also that STAT.selfref_type[1:0]=2.
+	 */
+	timeout = timeout_init_us(TIMEOUT_500US);
+	while (!timeout_elapsed(timeout)) {
+		stat = mmio_read_32(ddrctrl_base + DDRCTRL_STAT);
+		operating_mode = stat & DDRCTRL_STAT_OPERATING_MODE_MASK;
+		selref_type = stat & DDRCTRL_STAT_SELFREF_TYPE_MASK;
+
+		if ((operating_mode == DDRCTRL_STAT_OPERATING_MODE_SR) &&
+		    (selref_type == DDRCTRL_STAT_SELFREF_TYPE_SR)) {
+			op_mode_changed = 1;
+			break;
+		}
+	}
+
+	if (op_mode_changed == 0U)
+		goto selfref_sw_failed;
+
+	/* IOs powering down (PUBL registers) */
+	mmio_setbits_32(ddrphyc_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACPDD);
+
+	mmio_setbits_32(ddrphyc_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACPDR);
+
+	mmio_clrsetbits_32(ddrphyc_base + DDRPHYC_ACIOCR,
+			   DDRPHYC_ACIOCR_CKPDD_MASK,
+			   DDRPHYC_ACIOCR_CKPDD_0);
+
+	mmio_clrsetbits_32(ddrphyc_base + DDRPHYC_ACIOCR,
+			   DDRPHYC_ACIOCR_CKPDR_MASK,
+			   DDRPHYC_ACIOCR_CKPDR_0);
+
+	mmio_clrsetbits_32(ddrphyc_base + DDRPHYC_ACIOCR,
+			   DDRPHYC_ACIOCR_CSPDD_MASK,
+			   DDRPHYC_ACIOCR_CSPDD_0);
+
+	/* Disable command/address output driver */
+	mmio_clrbits_32(ddrphyc_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACOE);
+
+	mmio_setbits_32(ddrphyc_base + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDD);
+
+	mmio_setbits_32(ddrphyc_base + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDR);
+
+	mmio_clrsetbits_32(ddrphyc_base + DDRPHYC_DSGCR,
+			   DDRPHYC_DSGCR_ODTPDD_MASK,
+			   DDRPHYC_DSGCR_ODTPDD_0);
+
+	mmio_setbits_32(ddrphyc_base + DDRPHYC_DSGCR, DDRPHYC_DSGCR_NL2PD);
+
+	mmio_clrsetbits_32(ddrphyc_base + DDRPHYC_DSGCR,
+			   DDRPHYC_DSGCR_CKEPDD_MASK,
+			   DDRPHYC_DSGCR_CKEPDD_0);
+
+	/* Disable PZQ cell (PUBL register) */
+	mmio_setbits_32(ddrphyc_base + DDRPHYC_ZQ0CR0, DDRPHYC_ZQ0CRN_ZQPD);
+
+	/* Set latch */
+	mmio_clrbits_32(ddrphyc_base + DDRPHYC_DSGCR, DDRPHYC_DSGCR_CKOE);
+
+	/* Additional delay to avoid early latch */
+	udelay(10);
+
+	/* Activate sw retention in PWRCTRL */
+	stm32mp_pwr_regs_lock();
+	mmio_setbits_32(pwr_base + PWR_CR3, PWR_CR3_DDRRETEN);
+	stm32mp_pwr_regs_unlock();
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL ref clock */
+	stm32mp1_clk_rcc_regs_lock();
+	mmio_setbits_32(rcc_base + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+	stm32mp1_clk_rcc_regs_unlock();
+
+	/* Disable all DLLs: GLITCH window */
+	mmio_setbits_32(ddrphyc_base + DDRPHYC_ACDLLCR,
+			DDRPHYC_ACDLLCR_DLLDIS);
+
+	mmio_setbits_32(ddrphyc_base + DDRPHYC_DX0DLLCR,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+
+	mmio_setbits_32(ddrphyc_base + DDRPHYC_DX1DLLCR,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+
+#if STM32MP_DDR_DUAL_AXI_PORT
+	mmio_setbits_32(ddrphyc_base + DDRPHYC_DX2DLLCR,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+
+	mmio_setbits_32(ddrphyc_base + DDRPHYC_DX3DLLCR,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+#endif
+
+	stm32mp1_clk_rcc_regs_lock();
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL output clock */
+	mmio_clrbits_32(rcc_base + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+
+	/* Deactivate all DDR clocks */
+	mmio_clrbits_32(rcc_base + RCC_DDRITFCR,
+			RCC_DDRITFCR_DDRC1EN |
+#if STM32MP_DDR_DUAL_AXI_PORT
+			RCC_DDRITFCR_DDRC2EN |
+#endif
+			RCC_DDRITFCR_DDRCAPBEN |
+			RCC_DDRITFCR_DDRPHYCAPBEN);
+
+	stm32mp1_clk_rcc_regs_unlock();
+
+	return 0;
+
+selfref_sw_failed:
+	/* This bit should be cleared to restore DDR in its previous state */
+	mmio_clrbits_32(ddrctrl_base + DDRCTRL_PWRCTL,
+			DDRCTRL_PWRCTL_SELFREF_SW);
+
+pstat_failed:
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_PCTRL_0,
+			DDRCTRL_PCTRL_N_PORT_EN);
+#if STM32MP_DDR_DUAL_AXI_PORT
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_PCTRL_1,
+			DDRCTRL_PCTRL_N_PORT_EN);
+#endif
+
+	return -1;
+}
+
+int ddr_sw_self_refresh_exit(void)
+{
+	uint64_t timeout;
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	uintptr_t pwr_base = stm32mp_pwr_base();
+	uintptr_t ddrctrl_base = stm32mp_ddrctrl_base();
+	uintptr_t ddrphyc_base = stm32mp_ddrphyc_base();
+
+	/* Enable all clocks */
+	ddr_enable_clock();
+
+	do_sw_handshake();
+
+	/* Mask dfi_init_complete_en */
+	mmio_clrbits_32(ddrctrl_base + DDRCTRL_DFIMISC,
+			DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+
+	do_sw_ack();
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL ref clock */
+	stm32mp1_clk_rcc_regs_lock();
+	mmio_setbits_32(rcc_base + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+	stm32mp1_clk_rcc_regs_unlock();
+
+	/* Enable all DLLs: GLITCH window */
+	mmio_clrbits_32(ddrphyc_base + DDRPHYC_ACDLLCR,
+			DDRPHYC_ACDLLCR_DLLDIS);
+
+	mmio_clrbits_32(ddrphyc_base + DDRPHYC_DX0DLLCR,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+
+	mmio_clrbits_32(ddrphyc_base + DDRPHYC_DX1DLLCR,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+
+#if STM32MP_DDR_DUAL_AXI_PORT
+	mmio_clrbits_32(ddrphyc_base + DDRPHYC_DX2DLLCR,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+
+	mmio_clrbits_32(ddrphyc_base + DDRPHYC_DX3DLLCR,
+			DDRPHYC_DXNDLLCR_DLLDIS);
+#endif
+
+	/* Additional delay to avoid early DLL clock switch */
+	udelay(50);
+
+	/* Switch controller clocks (uMCTL2/PUBL) to DLL ref clock */
+	stm32mp1_clk_rcc_regs_lock();
+	mmio_clrbits_32(rcc_base + RCC_DDRITFCR, RCC_DDRITFCR_GSKPCTRL);
+	stm32mp1_clk_rcc_regs_unlock();
+
+	mmio_clrbits_32(ddrphyc_base + DDRPHYC_ACDLLCR,
+			DDRPHYC_ACDLLCR_DLLSRST);
+
+	udelay(10);
+
+	mmio_setbits_32(ddrphyc_base + DDRPHYC_ACDLLCR,
+			DDRPHYC_ACDLLCR_DLLSRST);
+
+	/* PHY partial init: (DLL lock and ITM reset) */
+	mmio_write_32(ddrphyc_base + DDRPHYC_PIR,
+		      DDRPHYC_PIR_DLLSRST | DDRPHYC_PIR_DLLLOCK |
+		      DDRPHYC_PIR_ITMSRST | DDRPHYC_PIR_INIT);
+
+	/* Need to wait at least 10 clock cycles before accessing PGSR */
+	udelay(1);
+
+	/* Pool end of init */
+	timeout = timeout_init_us(TIMEOUT_500US);
+
+	while ((mmio_read_32(ddrphyc_base + DDRPHYC_PGSR) &
+		DDRPHYC_PGSR_IDONE) == 0U) {
+		if (timeout_elapsed(timeout)) {
+			return -1;
+		}
+	}
+
+	do_sw_handshake();
+
+	/* Unmask dfi_init_complete_en to uMCTL2 */
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_DFIMISC,
+			DDRCTRL_DFIMISC_DFI_INIT_COMPLETE_EN);
+
+	do_sw_ack();
+
+	/* Deactivate sw retention in PWR */
+	stm32mp_pwr_regs_lock();
+	mmio_clrbits_32(pwr_base + PWR_CR3, PWR_CR3_DDRRETEN);
+	stm32mp_pwr_regs_unlock();
+
+	/* Enable PZQ cell (PUBL register) */
+	mmio_clrbits_32(ddrphyc_base + DDRPHYC_ZQ0CR0, DDRPHYC_ZQ0CRN_ZQPD);
+
+	/* Enable pad drivers */
+	mmio_clrbits_32(ddrphyc_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACPDD);
+
+	/* Enable command/address output driver */
+	mmio_setbits_32(ddrphyc_base + DDRPHYC_ACIOCR, DDRPHYC_ACIOCR_ACOE);
+
+	mmio_clrbits_32(ddrphyc_base + DDRPHYC_ACIOCR,
+			DDRPHYC_ACIOCR_CKPDD_MASK);
+
+	mmio_clrbits_32(ddrphyc_base + DDRPHYC_ACIOCR,
+			DDRPHYC_ACIOCR_CSPDD_MASK);
+
+	mmio_clrbits_32(ddrphyc_base + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDD);
+
+	mmio_clrbits_32(ddrphyc_base + DDRPHYC_DXCCR, DDRPHYC_DXCCR_DXPDR);
+
+	/* Release latch */
+	mmio_setbits_32(ddrphyc_base + DDRPHYC_DSGCR, DDRPHYC_DSGCR_CKOE);
+
+	mmio_clrbits_32(ddrphyc_base + DDRPHYC_DSGCR,
+			DDRPHYC_DSGCR_ODTPDD_MASK);
+
+	mmio_clrbits_32(ddrphyc_base + DDRPHYC_DSGCR, DDRPHYC_DSGCR_NL2PD);
+
+	mmio_clrbits_32(ddrphyc_base + DDRPHYC_DSGCR,
+			DDRPHYC_DSGCR_CKEPDD_MASK);
+
+	/* Remove selfrefresh */
+	mmio_clrbits_32(ddrctrl_base + DDRCTRL_PWRCTL,
+			DDRCTRL_PWRCTL_SELFREF_SW);
+
+	/* Wait operating_mode == normal */
+	timeout = timeout_init_us(TIMEOUT_500US);
+	while ((mmio_read_32(ddrctrl_base + DDRCTRL_STAT) &
+		DDRCTRL_STAT_OPERATING_MODE_MASK) !=
+	       DDRCTRL_STAT_OPERATING_MODE_NORMAL) {
+		if (timeout_elapsed(timeout)) {
+			return -1;
+		}
+	}
+
+	/* AXI ports are no longer blocked from taking transactions */
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_PCTRL_0,
+			DDRCTRL_PCTRL_N_PORT_EN);
+#if STM32MP_DDR_DUAL_AXI_PORT
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_PCTRL_1,
+			DDRCTRL_PCTRL_N_PORT_EN);
+#endif
+
+	stm32mp1_clk_rcc_regs_lock();
+
+	mmio_setbits_32(rcc_base + RCC_DDRITFCR, RCC_DDRITFCR_AXIDCGEN);
+
+	stm32mp1_clk_rcc_regs_unlock();
+
+	return 0;
+}
+
+uint32_t ddr_get_io_calibration_val(void)
+{
+	uintptr_t ddrphyc_base = stm32mp_ddrphyc_base();
+
+	return mmio_read_32(ddrphyc_base + DDRPHYC_ZQ0CR0) &
+		DDRPHYC_ZQ0CRN_ZDATA_MASK;
+}
+
+int ddr_standby_sr_entry(void)
+{
+	uintptr_t pwr_base = stm32mp_pwr_base();
+
+	/* Put DDR in Self-Refresh */
+	if (ddr_sw_self_refresh_in() != 0) {
+		return -1;
+	}
+
+	/* Enable I/O retention mode in standby */
+	stm32mp_pwr_regs_lock();
+	mmio_setbits_32(pwr_base + PWR_CR3, PWR_CR3_DDRSREN);
+	stm32mp_pwr_regs_unlock();
+
+	return 0;
+}
+
+static void ddr_sr_mode_ssr(void)
+{
+	uintptr_t rcc_ddritfcr = stm32mp_rcc_base() + RCC_DDRITFCR;
+	uintptr_t ddrctrl_base = stm32mp_ddrctrl_base();
+
+	stm32mp1_clk_rcc_regs_lock();
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC1LPEN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC1EN);
+
+#if STM32MP_DDR_DUAL_AXI_PORT
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC2LPEN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC2EN);
+#endif
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRCAPBLPEN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRPHYCAPBLPEN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRCAPBEN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRPHYCAPBEN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRPHYCEN);
+
+	mmio_clrbits_32(rcc_ddritfcr, RCC_DDRITFCR_AXIDCGEN);
+
+	mmio_clrbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRCKMOD_MASK);
+
+	stm32mp1_clk_rcc_regs_unlock();
+
+	/* Disable HW LP interface of uMCTL2 */
+	mmio_clrbits_32(ddrctrl_base + DDRCTRL_HWLPCTL,
+			DDRCTRL_HWLPCTL_HW_LP_EN);
+
+	/* Configure Automatic LP modes of uMCTL2 */
+	mmio_clrsetbits_32(ddrctrl_base + DDRCTRL_PWRTMG,
+			   DDRCTRL_PWRTMG_SELFREF_TO_X32_MASK,
+			   DDRCTRL_PWRTMG_SELFREF_TO_X32_0);
+
+	/*
+	 * Disable Clock disable with LP modes
+	 * (used in RUN mode for LPDDR2 with specific timing).
+	 */
+	mmio_clrbits_32(ddrctrl_base + DDRCTRL_PWRCTL,
+			DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE);
+
+	/* Disable automatic Self-Refresh mode */
+	mmio_clrbits_32(ddrctrl_base + DDRCTRL_PWRCTL,
+			DDRCTRL_PWRCTL_SELFREF_EN);
+}
+
+static void ddr_sr_mode_asr(void)
+{
+	uintptr_t rcc_ddritfcr = stm32mp_rcc_base() + RCC_DDRITFCR;
+	uintptr_t ddrctrl_base = stm32mp_ddrctrl_base();
+
+	stm32mp1_clk_rcc_regs_lock();
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_AXIDCGEN);
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC1LPEN);
+
+#if STM32MP_DDR_DUAL_AXI_PORT
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC2LPEN);
+#endif
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRPHYCLPEN);
+
+	mmio_clrsetbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRCKMOD_MASK,
+			   RCC_DDRITFCR_DDRCKMOD_ASR1);
+
+	stm32mp1_clk_rcc_regs_unlock();
+
+	/* Enable HW LP interface of uMCTL2 */
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_HWLPCTL,
+			DDRCTRL_HWLPCTL_HW_LP_EN);
+
+	/* Configure Automatic LP modes of uMCTL2 */
+	mmio_clrsetbits_32(ddrctrl_base + DDRCTRL_PWRTMG,
+			   DDRCTRL_PWRTMG_SELFREF_TO_X32_MASK,
+			   DDRCTRL_PWRTMG_SELFREF_TO_X32_0);
+
+	/*
+	 * Enable Clock disable with LP modes
+	 * (used in RUN mode for LPDDR2 with specific timing).
+	 */
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_PWRCTL,
+			DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE);
+
+	/* Enable automatic Self-Refresh for ASR mode */
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_PWRCTL,
+			DDRCTRL_PWRCTL_SELFREF_EN);
+}
+
+static void ddr_sr_mode_hsr(void)
+{
+	uintptr_t rcc_ddritfcr = stm32mp_rcc_base() + RCC_DDRITFCR;
+	uintptr_t ddrctrl_base = stm32mp_ddrctrl_base();
+
+	stm32mp1_clk_rcc_regs_lock();
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_AXIDCGEN);
+
+	mmio_clrbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC1LPEN);
+
+#if STM32MP_DDR_DUAL_AXI_PORT
+	mmio_clrbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRC2LPEN);
+#endif
+
+	mmio_setbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRPHYCLPEN);
+
+	mmio_clrsetbits_32(rcc_ddritfcr, RCC_DDRITFCR_DDRCKMOD_MASK,
+			   RCC_DDRITFCR_DDRCKMOD_HSR1);
+
+	stm32mp1_clk_rcc_regs_unlock();
+
+	/* Enable HW LP interface of uMCTL2 */
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_HWLPCTL,
+			DDRCTRL_HWLPCTL_HW_LP_EN);
+
+	/* Configure Automatic LP modes of uMCTL2 */
+	mmio_clrsetbits_32(ddrctrl_base + DDRCTRL_PWRTMG,
+			   DDRCTRL_PWRTMG_SELFREF_TO_X32_MASK,
+			   DDRCTRL_PWRTMG_SELFREF_TO_X32_0);
+
+	/*
+	 * Enable Clock disable with LP modes
+	 * (used in RUN mode for LPDDR2 with specific timing).
+	 */
+	mmio_setbits_32(ddrctrl_base + DDRCTRL_PWRCTL,
+			DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE);
+}
+
+enum stm32mp1_ddr_sr_mode ddr_read_sr_mode(void)
+{
+	uint32_t pwrctl = mmio_read_32(stm32mp_ddrctrl_base() + DDRCTRL_PWRCTL);
+
+	switch (pwrctl & (DDRCTRL_PWRCTL_EN_DFI_DRAM_CLK_DISABLE |
+			  DDRCTRL_PWRCTL_SELFREF_EN)) {
+	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;
+	}
+}
+
+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:
+		ERROR("Unknown Self Refresh mode\n");
+		panic();
+	}
+}
+
+void ddr_save_sr_mode(void)
+{
+	saved_ddr_sr_mode = ddr_read_sr_mode();
+}
+
+void ddr_restore_sr_mode(void)
+{
+	ddr_set_sr_mode(saved_ddr_sr_mode);
+}
+
+bool ddr_is_nonsecured_area(uintptr_t address, uint32_t length)
+{
+	uint64_t pa;
+
+	write_ats1cpw(address);
+
+	isb();
+
+	pa = read64_par();
+
+	if  ((((pa >> PAR_NS_SHIFT) & PAR_NS_MASK) != PAR_NS_MASK) ||
+	     (((pa >> PAR_F_SHIFT) & PAR_F_MASK) == PAR_F_MASK)) {
+		return false;
+	}
+
+	write_ats1cpw(address + length - 1U);
+
+	isb();
+
+	pa = read64_par();
+
+	if  ((((pa >> PAR_NS_SHIFT) & PAR_NS_MASK) == PAR_NS_MASK) &&
+	     (((pa >> PAR_F_SHIFT) & PAR_F_MASK) != PAR_F_MASK)) {
+		return true;
+	}
+
+	return false;
+}
diff --git a/drivers/st/ddr/stm32mp1_ram.c b/drivers/st/ddr/stm32mp1_ram.c
index b21c8949fe..3918eb7be5 100644
--- a/drivers/st/ddr/stm32mp1_ram.c
+++ b/drivers/st/ddr/stm32mp1_ram.c
@@ -13,6 +13,7 @@
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
 #include <drivers/st/stm32mp1_ddr.h>
 #include <drivers/st/stm32mp1_ddr_helpers.h>
 #include <drivers/st/stm32mp1_ram.h>
@@ -22,6 +23,7 @@
 #define DDR_ANTIPATTERN	0x55555555U
 
 static struct ddr_info ddr_priv_data;
+static bool ddr_self_refresh;
 
 int stm32mp1_ddr_clk_enable(struct ddr_info *priv, uint32_t mem_speed)
 {
@@ -29,7 +31,7 @@ int stm32mp1_ddr_clk_enable(struct ddr_info *priv, uint32_t mem_speed)
 
 	ddr_enable_clock();
 
-	ddrphy_clk = stm32mp_clk_get_rate(DDRPHYC);
+	ddrphy_clk = clk_get_rate(DDRPHYC);
 
 	VERBOSE("DDR: mem_speed (%d kHz), RCC %ld kHz\n",
 		mem_speed, ddrphy_clk / 1000U);
@@ -50,6 +52,26 @@ int stm32mp1_ddr_clk_enable(struct ddr_info *priv, uint32_t mem_speed)
 	return 0;
 }
 
+/*******************************************************************************
+ * This function tests a simple read/write access to the DDR.
+ * Note that the previous content is restored after test.
+ * Returns 0 if success, and address value else.
+ ******************************************************************************/
+static uint32_t ddr_test_rw_access(void)
+{
+	uint32_t saved_value = mmio_read_32(STM32MP_DDR_BASE);
+
+	mmio_write_32(STM32MP_DDR_BASE, DDR_PATTERN);
+
+	if (mmio_read_32(STM32MP_DDR_BASE) != DDR_PATTERN) {
+		return (uint32_t)STM32MP_DDR_BASE;
+	}
+
+	mmio_write_32(STM32MP_DDR_BASE, saved_value);
+
+	return 0;
+}
+
 /*******************************************************************************
  * This function tests the DDR data bus wiring.
  * This is inspired from the Data Bus Test algorithm written by Michael Barr
@@ -172,20 +194,23 @@ static int stm32mp1_ddr_setup(void)
 	uint32_t uret, idx;
 	void *fdt;
 
-#define PARAM(x, y)							\
+#define PARAM(x, y, z)							\
 	{								\
 		.name = x,						\
 		.offset = offsetof(struct stm32mp1_ddr_config, y),	\
-		.size = sizeof(config.y) / sizeof(uint32_t)		\
+		.size = sizeof(config.y) / sizeof(uint32_t),		\
+		.present = z						\
 	}
 
-#define CTL_PARAM(x) PARAM("st,ctl-"#x, c_##x)
-#define PHY_PARAM(x) PARAM("st,phy-"#x, p_##x)
+#define CTL_PARAM(x) PARAM("st,ctl-"#x, c_##x, NULL)
+#define PHY_PARAM(x) PARAM("st,phy-"#x, p_##x, NULL)
+#define PHY_PARAM_OPT(x) PARAM("st,phy-"#x, p_##x, &config.p_##x##_present)
 
 	const struct {
 		const char *name; /* Name in DT */
 		const uint32_t offset; /* Offset in config struct */
 		const uint32_t size;   /* Size of parameters */
+		bool * const present;  /* presence indication for opt */
 	} param[] = {
 		CTL_PARAM(reg),
 		CTL_PARAM(timing),
@@ -193,7 +218,7 @@ static int stm32mp1_ddr_setup(void)
 		CTL_PARAM(perf),
 		PHY_PARAM(reg),
 		PHY_PARAM(timing),
-		PHY_PARAM(cal)
+		PHY_PARAM_OPT(cal)
 	};
 
 	if (fdt_get_address(&fdt) == 0) {
@@ -231,11 +256,27 @@ static int stm32mp1_ddr_setup(void)
 
 		VERBOSE("%s: %s[0x%x] = %d\n", __func__,
 			param[idx].name, param[idx].size, ret);
-		if (ret != 0) {
-			ERROR("%s: Cannot read %s\n",
-			      __func__, param[idx].name);
+		if ((ret != 0) &&
+		    ((ret != -FDT_ERR_NOTFOUND) ||
+		     (param[idx].present == NULL))) {
+			ERROR("%s: Cannot read %s, error=%d\n",
+			      __func__, param[idx].name, ret);
 			return -EINVAL;
 		}
+		if (param[idx].present != NULL) {
+			/* save presence of optional parameters */
+			*(param[idx].present) = true;
+			if (ret == -FDT_ERR_NOTFOUND) {
+				*(param[idx].present) = false;
+			}
+		}
+	}
+
+	config.self_refresh = false;
+
+	if (stm32mp1_is_wakeup_from_standby()) {
+		config.self_refresh = true;
+		config.zdata = stm32_get_zdata_from_context();
 	}
 
 	/* Disable axidcg clock gating during init */
@@ -255,34 +296,61 @@ static int stm32mp1_ddr_setup(void)
 		panic();
 	}
 
-	uret = ddr_test_data_bus();
-	if (uret != 0U) {
-		ERROR("DDR data bus test: can't access memory @ 0x%x\n",
-		      uret);
-		panic();
-	}
+	if (config.self_refresh) {
+		uret = ddr_test_rw_access();
+		if (uret != 0U) {
+			ERROR("DDR rw test: Can't access memory @ 0x%x\n",
+			      uret);
+			panic();
+		}
 
-	uret = ddr_test_addr_bus();
-	if (uret != 0U) {
-		ERROR("DDR addr bus test: can't access memory @ 0x%x\n",
-		      uret);
-		panic();
-	}
+		/* Restore area overwritten by training */
+		stm32_restore_ddr_training_area();
+	} else {
+		uret = ddr_test_data_bus();
+		if (uret != 0U) {
+			ERROR("DDR data bus test: can't access memory @ 0x%x\n",
+			      uret);
+			panic();
+		}
 
-	uret = ddr_check_size();
-	if (uret < config.info.size) {
-		ERROR("DDR size: 0x%x does not match DT config: 0x%x\n",
-		      uret, config.info.size);
-		panic();
+		uret = ddr_test_addr_bus();
+		if (uret != 0U) {
+			ERROR("DDR addr bus test: can't access memory @ 0x%x\n",
+			      uret);
+			panic();
+		}
+
+		uret = ddr_check_size();
+		if (uret < config.info.size) {
+			ERROR("DDR size: 0x%x does not match DT config: 0x%x\n",
+			      uret, config.info.size);
+			panic();
+		}
 	}
 
+	/*
+	 * Initialization sequence has configured DDR registers with settings.
+	 * The Self Refresh (SR) mode corresponding to these settings has now
+	 * to be set.
+	 */
+	ddr_set_sr_mode(ddr_read_sr_mode());
+
 	if (stm32mp_unmap_ddr() != 0) {
 		panic();
 	}
 
+	/* Save DDR self_refresh state */
+	ddr_self_refresh = config.self_refresh;
+
 	return 0;
 }
 
+bool stm32mp1_ddr_is_restored(void)
+{
+	return ddr_self_refresh;
+}
+
 int stm32mp1_ddr_probe(void)
 {
 	struct ddr_info *priv = &ddr_priv_data;
diff --git a/drivers/st/etzpc/etzpc.c b/drivers/st/etzpc/etzpc.c
index ff52a22d9d..eb1c79cf7b 100644
--- a/drivers/st/etzpc/etzpc.c
+++ b/drivers/st/etzpc/etzpc.c
@@ -66,6 +66,10 @@ struct etzpc_instance {
 /* Only 1 instance of the ETZPC is expected per platform */
 static struct etzpc_instance etzpc_dev;
 
+struct dt_id_attr {
+	fdt32_t id_attr[STM32MP1_ETZPC_MAX_ID];
+};
+
 /*
  * Implementation uses uint8_t to store each securable DECPROT configuration.
  * When resuming from deep suspend, the DECPROT configurations are restored.
@@ -85,6 +89,50 @@ static bool valid_tzma_id(unsigned int id)
 }
 #endif
 
+static int etzpc_dt_conf_decprot(int node)
+{
+	const struct dt_id_attr *conf_list;
+	void *fdt;
+	unsigned int i;
+	int len = 0;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	conf_list = (const struct dt_id_attr *)fdt_getprop(fdt, node,
+							   "st,decprot", &len);
+	if (conf_list == NULL) {
+		INFO("No ETZPC configuration in DT, use default\n");
+		return 0;
+	}
+
+	for (i = 0U; i < (unsigned int)len / sizeof(uint32_t); i++) {
+		enum etzpc_decprot_attributes attr;
+		uint32_t value;
+		uint32_t id;
+		uint32_t mode;
+
+		value = fdt32_to_cpu(conf_list->id_attr[i]);
+
+		id = ((value >> ETZPC_ID_SHIFT) & ETZPC_ID_MASK);
+		assert(valid_decprot_id(id));
+
+		mode = (value >> ETZPC_MODE_SHIFT) & ETZPC_MODE_MASK;
+		attr = stm32mp_etzpc_binding2decprot(mode);
+
+		stm32mp1_register_etzpc_decprot(id, attr);
+
+		etzpc_configure_decprot(id, attr);
+
+		if ((value & ETZPC_LOCK_MASK) != 0U) {
+			etzpc_lock_decprot(id);
+		}
+	}
+
+	return 0;
+}
+
 /*
  * etzpc_configure_decprot : Load a DECPROT configuration
  * decprot_id : ID of the IP
@@ -254,5 +302,5 @@ int etzpc_init(void)
 
 	VERBOSE("ETZPC version 0x%x", etzpc_dev.revision);
 
-	return 0;
+	return etzpc_dt_conf_decprot(node);
 }
diff --git a/drivers/st/fmc/stm32_fmc2_nand.c b/drivers/st/fmc/stm32_fmc2_nand.c
index a58a243ad6..503e259876 100644
--- a/drivers/st/fmc/stm32_fmc2_nand.c
+++ b/drivers/st/fmc/stm32_fmc2_nand.c
@@ -14,6 +14,7 @@
 #include <platform_def.h>
 
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/raw_nand.h>
 #include <drivers/st/stm32_fmc2_nand.h>
@@ -162,7 +163,7 @@ static uintptr_t fmc2_base(void)
 static void stm32_fmc2_nand_setup_timing(void)
 {
 	struct stm32_fmc2_nand_timings tims;
-	unsigned long hclk = stm32mp_clk_get_rate(stm32_fmc2.clock_id);
+	unsigned long hclk = clk_get_rate(stm32_fmc2.clock_id);
 	unsigned long hclkp = FMC2_PSEC_PER_MSEC / (hclk / 1000U);
 	unsigned long timing, tar, tclr, thiz, twait;
 	unsigned long tset_mem, tset_att, thold_mem, thold_att;
@@ -918,7 +919,7 @@ int stm32_fmc2_init(void)
 	}
 
 	/* Enable Clock */
-	stm32mp_clk_enable(stm32_fmc2.clock_id);
+	clk_enable(stm32_fmc2.clock_id);
 
 	/* Reset IP */
 	ret = stm32mp_reset_assert(stm32_fmc2.reset_id, TIMEOUT_US_1_MS);
diff --git a/drivers/st/gpio/stm32_gpio.c b/drivers/st/gpio/stm32_gpio.c
index 7d63262d71..75707e63a1 100644
--- a/drivers/st/gpio/stm32_gpio.c
+++ b/drivers/st/gpio/stm32_gpio.c
@@ -14,6 +14,7 @@
 
 #include <common/bl_common.h>
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/st/stm32_gpio.h>
 #include <drivers/st/stm32mp_clkfunc.h>
 #include <lib/mmio.h>
@@ -208,7 +209,7 @@ void set_gpio(uint32_t bank, uint32_t pin, uint32_t mode, uint32_t speed,
 
 	assert(pin <= GPIO_PIN_MAX);
 
-	stm32mp_clk_enable(clock);
+	clk_enable(clock);
 
 	mmio_clrbits_32(base + GPIO_MODE_OFFSET,
 			((uint32_t)GPIO_MODE_MASK << (pin << 1)));
@@ -254,7 +255,7 @@ void set_gpio(uint32_t bank, uint32_t pin, uint32_t mode, uint32_t speed,
 	VERBOSE("GPIO %u mode alternate high to 0x%x\n", bank,
 		mmio_read_32(base + GPIO_AFRH_OFFSET));
 
-	stm32mp_clk_disable(clock);
+	clk_disable(clock);
 
 	if (status == DT_SECURE) {
 		stm32mp_register_secure_gpio(bank, pin);
@@ -273,7 +274,7 @@ void set_gpio_secure_cfg(uint32_t bank, uint32_t pin, bool secure)
 
 	assert(pin <= GPIO_PIN_MAX);
 
-	stm32mp_clk_enable(clock);
+	clk_enable(clock);
 
 	if (secure) {
 		mmio_setbits_32(base + GPIO_SECR_OFFSET, BIT(pin));
@@ -281,5 +282,5 @@ void set_gpio_secure_cfg(uint32_t bank, uint32_t pin, bool secure)
 		mmio_clrbits_32(base + GPIO_SECR_OFFSET, BIT(pin));
 	}
 
-	stm32mp_clk_disable(clock);
+	clk_disable(clock);
 }
diff --git a/drivers/st/i2c/stm32_i2c.c b/drivers/st/i2c/stm32_i2c.c
index ed880522b0..3e4b96a2c3 100644
--- a/drivers/st/i2c/stm32_i2c.c
+++ b/drivers/st/i2c/stm32_i2c.c
@@ -1,10 +1,11 @@
 /*
- * Copyright (c) 2016-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2016-2020, STMicroelectronics - All Rights Reserved
  *
- * SPDX-License-Identifier: BSD-3-Clause
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
  */
 
 #include <errno.h>
+#include <limits.h>
 #include <stdbool.h>
 #include <stdlib.h>
 
@@ -13,6 +14,8 @@
 #include <platform_def.h>
 
 #include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/st/stm32_gpio.h>
 #include <drivers/st/stm32_i2c.h>
@@ -38,8 +41,87 @@
 
 #define I2C_NSEC_PER_SEC	1000000000L
 
-/* I2C Timing hard-coded value, for I2C clock source is HSI at 64MHz */
-#define I2C_TIMING			0x10D07DB5
+/*
+ * struct i2c_spec_s - Private I2C timing specifications.
+ * @rate: I2C bus speed (Hz)
+ * @fall_max: Max fall time of both SDA and SCL signals (ns)
+ * @rise_max: Max rise time of both SDA and SCL signals (ns)
+ * @hddat_min: Min data hold time (ns)
+ * @vddat_max: Max data valid time (ns)
+ * @sudat_min: Min data setup time (ns)
+ * @l_min: Min low period of the SCL clock (ns)
+ * @h_min: Min high period of the SCL clock (ns)
+ */
+struct i2c_spec_s {
+	uint32_t rate;
+	uint32_t fall_max;
+	uint32_t rise_max;
+	uint32_t hddat_min;
+	uint32_t vddat_max;
+	uint32_t sudat_min;
+	uint32_t l_min;
+	uint32_t h_min;
+};
+
+/*
+ * struct i2c_timing_s - Private I2C output parameters.
+ * @scldel: Data setup time
+ * @sdadel: Data hold time
+ * @sclh: SCL high period (master mode)
+ * @sclh: SCL low period (master mode)
+ * @is_saved: True if relating to a configuration candidate
+ */
+struct i2c_timing_s {
+	uint8_t scldel;
+	uint8_t sdadel;
+	uint8_t sclh;
+	uint8_t scll;
+	bool is_saved;
+};
+
+/*
+ * I2C specification values as per version 6.0, 4th of April 2014 [1],
+ * table 10 page 48: Characteristics of the SDA and SCL bus lines for
+ * Standard, Fast, and Fast-mode Plus I2C-bus devices.
+ *
+ * [1] https://www.i2c-bus.org/specification/
+ */
+static const struct i2c_spec_s i2c_specs[] = {
+	/* Standard - 100KHz */
+	{
+		.rate = STANDARD_RATE,
+		.fall_max = 300,
+		.rise_max = 1000,
+		.hddat_min = 0,
+		.vddat_max = 3450,
+		.sudat_min = 250,
+		.l_min = 4700,
+		.h_min = 4000,
+	},
+	/* Fast - 400KHz */
+	{
+		.rate = FAST_RATE,
+		.fall_max = 300,
+		.rise_max = 300,
+		.hddat_min = 0,
+		.vddat_max = 900,
+		.sudat_min = 100,
+		.l_min = 1300,
+		.h_min = 600,
+	},
+	/* FastPlus - 1MHz */
+	{
+		.rate = FAST_PLUS_RATE,
+		.fall_max = 100,
+		.rise_max = 120,
+		.hddat_min = 0,
+		.vddat_max = 450,
+		.sudat_min = 50,
+		.l_min = 500,
+		.h_min = 260,
+	},
+};
+
 
 static void notif_i2c_timeout(struct i2c_handle_s *hi2c)
 {
@@ -48,6 +130,298 @@ static void notif_i2c_timeout(struct i2c_handle_s *hi2c)
 	hi2c->i2c_state = I2C_STATE_READY;
 }
 
+static const struct i2c_spec_s *get_specs(uint32_t rate)
+{
+	size_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(i2c_specs); i++) {
+		if (rate <= i2c_specs[i].rate) {
+			return &i2c_specs[i];
+		}
+	}
+
+	/* NOT REACHED */
+	return NULL;
+}
+
+#define RATE_MIN(rate)	(((rate) / 100U) * 80U)
+/*
+ * @brief  Compute the I2C device timings.
+ * @param  init: Ref to the initialization configuration structure
+ * @param  clock_src: I2C clock source frequency (Hz)
+ * @param  timing: Pointer to the final computed timing result
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_compute_timing(struct stm32_i2c_init_s *init,
+			      uint32_t clock_src, uint32_t *timing)
+{
+	const struct i2c_spec_s *specs;
+	uint32_t speed_freq;
+	uint32_t i2cclk = udiv_round_nearest(I2C_NSEC_PER_SEC, clock_src);
+	uint32_t i2cbus;
+	uint32_t p_prev = I2C_TIMINGR_PRESC_MAX;
+	uint32_t af_delay_min;
+	uint32_t af_delay_max;
+	uint32_t dnf_delay;
+	uint32_t tsync;
+	uint32_t clk_min;
+	uint32_t clk_max;
+	int clk_error_prev;
+	uint16_t p;
+	uint16_t l;
+	uint16_t a;
+	uint16_t h;
+	int sdadel_min;
+	int sdadel_max;
+	uint32_t sdadel_min_u;
+	uint32_t sdadel_max_u;
+	uint32_t scldel_min;
+	int s = -1;
+	struct i2c_timing_s solutions[I2C_TIMINGR_PRESC_MAX];
+
+	specs = get_specs(init->bus_rate);
+	if (specs == NULL) {
+		ERROR("I2C speed out of bound {%d}\n", init->bus_rate);
+		return -EINVAL;
+	}
+
+	speed_freq = specs->rate;
+	i2cbus = udiv_round_nearest(I2C_NSEC_PER_SEC, speed_freq);
+	clk_error_prev = INT_MAX;
+
+	if ((init->rise_time > specs->rise_max) ||
+	    (init->fall_time > specs->fall_max)) {
+		ERROR(" I2C timings out of bound Rise{%d>%d}/Fall{%d>%d}\n",
+		      init->rise_time, specs->rise_max,
+		      init->fall_time, specs->fall_max);
+		return -EINVAL;
+	}
+
+	if (init->digital_filter_coef > STM32_I2C_DIGITAL_FILTER_MAX) {
+		ERROR("DNF out of bound %d/%d\n",
+		      init->digital_filter_coef, STM32_I2C_DIGITAL_FILTER_MAX);
+		return -EINVAL;
+	}
+
+	/*  Analog and Digital Filters */
+	af_delay_min = (init->analog_filter ?
+			STM32_I2C_ANALOG_FILTER_DELAY_MIN : 0);
+	af_delay_max = (init->analog_filter ?
+			STM32_I2C_ANALOG_FILTER_DELAY_MAX : 0);
+	dnf_delay = init->digital_filter_coef * i2cclk;
+
+	sdadel_min = specs->hddat_min + init->fall_time -
+		     af_delay_min - ((init->digital_filter_coef + 3) * i2cclk);
+
+	sdadel_max = specs->vddat_max - init->rise_time -
+		     af_delay_max - ((init->digital_filter_coef + 4) * i2cclk);
+
+	scldel_min = init->rise_time + specs->sudat_min;
+
+	if (sdadel_min < 0) {
+		sdadel_min_u = 0;
+	} else {
+		sdadel_min_u = (uint32_t)sdadel_min;
+	}
+
+	if (sdadel_max < 0) {
+		sdadel_max_u = 0;
+	} else {
+		sdadel_max_u = (uint32_t)sdadel_max;
+	}
+
+	VERBOSE("I2C SDADEL(min/max): %u/%u, SCLDEL(Min): %u\n",
+		sdadel_min_u, sdadel_max_u, scldel_min);
+
+	zeromem(&solutions, sizeof(solutions));
+
+	/* Compute possible values for PRESC, SCLDEL and SDADEL */
+	for (p = 0; p < I2C_TIMINGR_PRESC_MAX; p++) {
+		for (l = 0; l < I2C_TIMINGR_SCLDEL_MAX; l++) {
+			uint32_t scldel = (l + 1) * (p + 1) * i2cclk;
+
+			if (scldel < scldel_min) {
+				continue;
+			}
+
+			for (a = 0; a < I2C_TIMINGR_SDADEL_MAX; a++) {
+				uint32_t sdadel = (a * (p + 1) + 1) * i2cclk;
+
+				if ((sdadel >= sdadel_min_u) &&
+				    (sdadel <= sdadel_max_u) &&
+				    (p != p_prev)) {
+					solutions[p].scldel = l;
+					solutions[p].sdadel = a;
+					solutions[p].is_saved = true;
+					p_prev = p;
+					break;
+				}
+			}
+
+			if (p_prev == p) {
+				break;
+			}
+		}
+	}
+
+	if (p_prev == I2C_TIMINGR_PRESC_MAX) {
+		ERROR(" I2C no Prescaler solution\n");
+		return -EPERM;
+	}
+
+	tsync = af_delay_min + dnf_delay + (2 * i2cclk);
+	clk_max = I2C_NSEC_PER_SEC / RATE_MIN(specs->rate);
+	clk_min = I2C_NSEC_PER_SEC / specs->rate;
+
+	/*
+	 * Among prescaler possibilities discovered above figures out SCL Low
+	 * and High Period. Provided:
+	 * - SCL Low Period has to be higher than Low Period of the SCL Clock
+	 *   defined by I2C Specification. I2C Clock has to be lower than
+	 *   (SCL Low Period - Analog/Digital filters) / 4.
+	 * - SCL High Period has to be lower than High Period of the SCL Clock
+	 *   defined by I2C Specification.
+	 * - I2C Clock has to be lower than SCL High Period.
+	 */
+	for (p = 0; p < I2C_TIMINGR_PRESC_MAX; p++) {
+		uint32_t prescaler = (p + 1) * i2cclk;
+
+		if (!solutions[p].is_saved) {
+			continue;
+		}
+
+		for (l = 0; l < I2C_TIMINGR_SCLL_MAX; l++) {
+			uint32_t tscl_l = ((l + 1) * prescaler) + tsync;
+
+			if ((tscl_l < specs->l_min) ||
+			    (i2cclk >=
+			     ((tscl_l - af_delay_min - dnf_delay) / 4))) {
+				continue;
+			}
+
+			for (h = 0; h < I2C_TIMINGR_SCLH_MAX; h++) {
+				uint32_t tscl_h = ((h + 1) * prescaler) + tsync;
+				uint32_t tscl = tscl_l + tscl_h +
+						init->rise_time +
+						init->fall_time;
+
+				if ((tscl >= clk_min) && (tscl <= clk_max) &&
+				    (tscl_h >= specs->h_min) &&
+				    (i2cclk < tscl_h)) {
+					int clk_error = tscl - i2cbus;
+
+					if (clk_error < 0) {
+						clk_error = -clk_error;
+					}
+
+					if (clk_error < clk_error_prev) {
+						clk_error_prev = clk_error;
+						solutions[p].scll = l;
+						solutions[p].sclh = h;
+						s = p;
+					}
+				}
+			}
+		}
+	}
+
+	if (s < 0) {
+		ERROR(" I2C no solution at all\n");
+		return -EPERM;
+	}
+
+	/* Finalize timing settings */
+	*timing = I2C_SET_TIMINGR_PRESC(s) |
+		  I2C_SET_TIMINGR_SCLDEL(solutions[s].scldel) |
+		  I2C_SET_TIMINGR_SDADEL(solutions[s].sdadel) |
+		  I2C_SET_TIMINGR_SCLH(solutions[s].sclh) |
+		  I2C_SET_TIMINGR_SCLL(solutions[s].scll);
+
+	VERBOSE("I2C TIMINGR (PRESC/SCLDEL/SDADEL): %i/%i/%i\n",
+		s, solutions[s].scldel, solutions[s].sdadel);
+	VERBOSE("I2C TIMINGR (SCLH/SCLL): %i/%i\n",
+		solutions[s].sclh, solutions[s].scll);
+	VERBOSE("I2C TIMINGR: 0x%x\n", *timing);
+
+	return 0;
+}
+
+static uint32_t get_lower_rate(uint32_t rate)
+{
+	int i;
+
+	for (i = ARRAY_SIZE(i2c_specs) - 1; i >= 0; i--) {
+		if (rate > i2c_specs[i].rate) {
+			return i2c_specs[i].rate;
+		}
+	}
+
+	return i2c_specs[0].rate;
+}
+
+/*
+ * @brief  Setup the I2C device timings.
+ * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
+ *               the configuration information for the specified I2C.
+ * @param  init: Ref to the initialization configuration structure
+ * @param  timing: Pointer to the final computed timing result
+ * @retval 0 if OK, negative value else
+ */
+static int i2c_setup_timing(struct i2c_handle_s *hi2c,
+			    struct stm32_i2c_init_s *init,
+			    uint32_t *timing)
+{
+	int rc = 0;
+	uint32_t clock_src;
+
+	clock_src = (uint32_t)clk_get_rate(hi2c->clock);
+	if (clock_src == 0U) {
+		ERROR("I2C clock rate is 0\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * If the timing has already been computed, and the frequency is the
+	 * same as when it was computed, then use the saved timing.
+	 */
+	if (clock_src == hi2c->saved_frequency) {
+		*timing = hi2c->saved_timing;
+		return 0;
+	}
+
+	do {
+		rc = i2c_compute_timing(init, clock_src, timing);
+		if (rc != 0) {
+			ERROR("Failed to compute I2C timings\n");
+			if (init->bus_rate > STANDARD_RATE) {
+				init->bus_rate = get_lower_rate(init->bus_rate);
+				WARN("Downgrade I2C speed to %uHz)\n",
+				     init->bus_rate);
+			} else {
+				break;
+			}
+		}
+	} while (rc != 0);
+
+	if (rc != 0) {
+		ERROR("Impossible to compute I2C timings\n");
+		return rc;
+	}
+
+	VERBOSE("I2C Freq(%i), Clk Source(%i)\n",
+		init->bus_rate, clock_src);
+	VERBOSE("I2C Rise(%i) and Fall(%i) Time\n",
+		init->rise_time, init->fall_time);
+	VERBOSE("I2C Analog Filter(%s), DNF(%i)\n",
+		(init->analog_filter ? "On" : "Off"),
+		init->digital_filter_coef);
+
+	hi2c->saved_timing = *timing;
+	hi2c->saved_frequency = clock_src;
+
+	return 0;
+}
+
 /*
  * @brief  Configure I2C Analog noise filter.
  * @param  hi2c: Pointer to a struct i2c_handle_s structure that contains
@@ -88,49 +462,35 @@ static int i2c_config_analog_filter(struct i2c_handle_s *hi2c,
 /*
  * @brief  Get I2C setup information from the device tree and set pinctrl
  *         configuration.
- * @param  fdt: Pointer to the device tree
  * @param  node: I2C node offset
  * @param  init: Ref to the initialization configuration structure
  * @retval 0 if OK, negative value else
  */
-int stm32_i2c_get_setup_from_fdt(void *fdt, int node,
-				 struct stm32_i2c_init_s *init)
+int stm32_i2c_get_setup_from_fdt(int node, struct stm32_i2c_init_s *init)
 {
-	const fdt32_t *cuint;
+	uint32_t read_val;
+	void *fdt;
 
-	cuint = fdt_getprop(fdt, node, "i2c-scl-rising-time-ns", NULL);
-	if (cuint == NULL) {
-		init->rise_time = STM32_I2C_RISE_TIME_DEFAULT;
-	} else {
-		init->rise_time = fdt32_to_cpu(*cuint);
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
 	}
 
-	cuint = fdt_getprop(fdt, node, "i2c-scl-falling-time-ns", NULL);
-	if (cuint == NULL) {
-		init->fall_time = STM32_I2C_FALL_TIME_DEFAULT;
-	} else {
-		init->fall_time = fdt32_to_cpu(*cuint);
-	}
+	init->rise_time = fdt_read_uint32_default(fdt, node,
+						  "i2c-scl-rising-time-ns",
+						  STM32_I2C_RISE_TIME_DEFAULT);
 
-	cuint = fdt_getprop(fdt, node, "clock-frequency", NULL);
-	if (cuint == NULL) {
-		init->speed_mode = STM32_I2C_SPEED_DEFAULT;
-	} else {
-		switch (fdt32_to_cpu(*cuint)) {
-		case STANDARD_RATE:
-			init->speed_mode = I2C_SPEED_STANDARD;
-			break;
-		case FAST_RATE:
-			init->speed_mode = I2C_SPEED_FAST;
-			break;
-		case FAST_PLUS_RATE:
-			init->speed_mode = I2C_SPEED_FAST_PLUS;
-			break;
-		default:
-			init->speed_mode = STM32_I2C_SPEED_DEFAULT;
-			break;
-		}
+	init->fall_time = fdt_read_uint32_default(fdt, node,
+						  "i2c-scl-falling-time-ns",
+						  STM32_I2C_FALL_TIME_DEFAULT);
+
+	read_val = fdt_read_uint32_default(fdt, node, "clock-frequency",
+					   STANDARD_RATE);
+	if (read_val > FAST_PLUS_RATE) {
+		ERROR("Invalid bus speed (%i > %i)\n", read_val,
+		      FAST_PLUS_RATE);
+		return -FDT_ERR_BADVALUE;
 	}
+	init->bus_rate = read_val;
 
 	return dt_set_pinctrl_config(node);
 }
@@ -146,7 +506,7 @@ int stm32_i2c_init(struct i2c_handle_s *hi2c,
 		   struct stm32_i2c_init_s *init_data)
 {
 	int rc = 0;
-	uint32_t timing = I2C_TIMING;
+	uint32_t timing;
 
 	if (hi2c == NULL) {
 		return -ENOENT;
@@ -158,7 +518,12 @@ int stm32_i2c_init(struct i2c_handle_s *hi2c,
 
 	hi2c->i2c_state = I2C_STATE_BUSY;
 
-	stm32mp_clk_enable(hi2c->clock);
+	rc = i2c_setup_timing(hi2c, init_data, &timing);
+	if (rc != 0) {
+		return rc;
+	}
+
+	clk_enable(hi2c->clock);
 
 	/* Disable the selected I2C peripheral */
 	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_PE);
@@ -220,11 +585,11 @@ int stm32_i2c_init(struct i2c_handle_s *hi2c,
 						I2C_ANALOGFILTER_DISABLE);
 	if (rc != 0) {
 		ERROR("Cannot initialize I2C analog filter (%d)\n", rc);
-		stm32mp_clk_disable(hi2c->clock);
+		clk_disable(hi2c->clock);
 		return rc;
 	}
 
-	stm32mp_clk_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 
 	return rc;
 }
@@ -548,7 +913,7 @@ static int i2c_write(struct i2c_handle_s *hi2c, uint16_t dev_addr,
 		return -EINVAL;
 	}
 
-	stm32mp_clk_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	hi2c->lock = 1;
 
@@ -648,7 +1013,7 @@ static int i2c_write(struct i2c_handle_s *hi2c, uint16_t dev_addr,
 
 bail:
 	hi2c->lock = 0;
-	stm32mp_clk_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 
 	return rc;
 }
@@ -729,7 +1094,7 @@ static int i2c_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
 		return  -EINVAL;
 	}
 
-	stm32mp_clk_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	hi2c->lock = 1;
 
@@ -817,7 +1182,7 @@ static int i2c_read(struct i2c_handle_s *hi2c, uint16_t dev_addr,
 
 bail:
 	hi2c->lock = 0;
-	stm32mp_clk_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 
 	return rc;
 }
@@ -882,7 +1247,7 @@ bool stm32_i2c_is_device_ready(struct i2c_handle_s *hi2c,
 		return rc;
 	}
 
-	stm32mp_clk_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	hi2c->lock = 1;
 	hi2c->i2c_mode = I2C_MODE_NONE;
@@ -974,8 +1339,7 @@ bool stm32_i2c_is_device_ready(struct i2c_handle_s *hi2c,
 
 bail:
 	hi2c->lock = 0;
-	stm32mp_clk_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 
 	return rc;
 }
-
diff --git a/drivers/st/io/io_stm32image.c b/drivers/st/io/io_stm32image.c
index 3e377cd483..ccd3379a39 100644
--- a/drivers/st/io/io_stm32image.c
+++ b/drivers/st/io/io_stm32image.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -182,53 +182,26 @@ static int stm32image_partition_size(io_entity_t *entity, size_t *length)
 		return result;
 	}
 
-	/* Reset magic header value */
-	header->magic = 0;
-
-	while (header->magic == 0U) {
-		result = io_seek(backend_handle, IO_SEEK_SET, *stm32_img);
-		if (result != 0) {
-			ERROR("%s: io_seek (%i)\n", __func__, result);
-			break;
-		}
-
-		result = io_read(backend_handle, (uintptr_t)header,
-				 MAX_LBA_SIZE, (size_t *)&bytes_read);
-		if (result != 0) {
-			if (current_part->bkp_offset == 0U) {
-				ERROR("%s: io_read (%i)\n", __func__, result);
-			}
-			header->magic = 0;
-		}
-
-		if ((header->magic != BOOT_API_IMAGE_HEADER_MAGIC_NB) ||
-		    (header->binary_type != current_part->binary_type) ||
-		    (header->image_length >= stm32image_dev.device_size)) {
-			VERBOSE("%s: partition %s not found at %x\n",
-				__func__, current_part->name, *stm32_img);
-
-			if (current_part->bkp_offset == 0U) {
-				result = -ENOMEM;
-				break;
-			}
-
-			/* Header not correct, check next offset for backup */
-			*stm32_img += current_part->bkp_offset;
-			if (*stm32_img > stm32image_dev.device_size) {
-				/* No backup found, end of device reached */
-				WARN("%s : partition %s not found\n",
-				     __func__, current_part->name);
-				result = -ENOMEM;
-				break;
-			}
-			header->magic = 0;
-		}
+	result = io_seek(backend_handle, IO_SEEK_SET, *stm32_img);
+	if (result != 0) {
+		ERROR("%s: io_seek (%i)\n", __func__, result);
+		goto out;
 	}
 
-	io_close(backend_handle);
-
+	result = io_read(backend_handle, (uintptr_t)header,
+			 MAX_LBA_SIZE, &bytes_read);
 	if (result != 0) {
-		return result;
+		ERROR("%s: io_read (%i)\n", __func__, result);
+		goto out;
+	}
+
+	if ((header->magic != BOOT_API_IMAGE_HEADER_MAGIC_NB) ||
+	    (header->binary_type != current_part->binary_type) ||
+	    (header->image_length >= stm32image_dev.device_size)) {
+		VERBOSE("%s: partition %s not found at %x\n",
+			__func__, current_part->name, *stm32_img);
+		result = -ENOMEM;
+		goto out;
 	}
 
 	if (header->image_length < stm32image_dev.lba_size) {
@@ -239,17 +212,24 @@ static int stm32image_partition_size(io_entity_t *entity, size_t *length)
 
 	INFO("STM32 Image size : %lu\n", (unsigned long)*length);
 
-	return 0;
+out:
+	io_close(backend_handle);
+
+	return result;
 }
 
 /* Read data from a partition */
 static int stm32image_partition_read(io_entity_t *entity, uintptr_t buffer,
 				     size_t length, size_t *length_read)
 {
-	int result;
+	int offset;
+	int local_length;
+	uintptr_t backend_handle;
+	int result = -EINVAL;
 	uint8_t *local_buffer;
 	boot_api_image_header_t *header =
 		(boot_api_image_header_t *)first_lba_buffer;
+	size_t hdr_sz = sizeof(boot_api_image_header_t);
 
 	assert(entity != NULL);
 	assert(buffer != 0U);
@@ -258,99 +238,53 @@ static int stm32image_partition_read(io_entity_t *entity, uintptr_t buffer,
 	local_buffer = (uint8_t *)buffer;
 	*length_read = 0U;
 
-	while (*length_read == 0U) {
-		int offset;
-		int local_length;
-		uintptr_t backend_handle;
-
-		if (header->magic != BOOT_API_IMAGE_HEADER_MAGIC_NB) {
-			/* Check for backup as image is corrupted */
-			if (current_part->bkp_offset == 0U) {
-				result = -ENOMEM;
-				break;
-			}
-
-			*stm32_img += current_part->bkp_offset;
-			if (*stm32_img >= stm32image_dev.device_size) {
-				/* End of device reached */
-				result = -ENOMEM;
-				break;
-			}
-
-			local_buffer = (uint8_t *)buffer;
-
-			result = stm32image_partition_size(entity, &length);
-			if (result != 0) {
-				break;
-			}
-		}
+#if TRUSTED_BOARD_BOOT
+	stm32mp_save_loaded_header(header);
+#endif
 
-		/* Part of image already loaded with the header */
-		memcpy(local_buffer, (uint8_t *)first_lba_buffer +
-		       sizeof(boot_api_image_header_t),
-		       MAX_LBA_SIZE - sizeof(boot_api_image_header_t));
-		local_buffer += MAX_LBA_SIZE - sizeof(boot_api_image_header_t);
-		offset = MAX_LBA_SIZE;
-
-		/* New image length to be read */
-		local_length = round_up(length -
-					((MAX_LBA_SIZE) -
-					 sizeof(boot_api_image_header_t)),
-					stm32image_dev.lba_size);
-
-		if ((header->load_address != 0U) &&
-		    (header->load_address != buffer)) {
-			ERROR("Wrong load address\n");
-			panic();
-		}
+	/* Part of image already loaded with the header */
+	memcpy(local_buffer, (uint8_t *)first_lba_buffer + hdr_sz,
+	       MAX_LBA_SIZE - hdr_sz);
+	local_buffer += MAX_LBA_SIZE - hdr_sz;
+	offset = MAX_LBA_SIZE;
 
-		result = io_open(backend_dev_handle, backend_image_spec,
-				 &backend_handle);
+	/* New image length to be read */
+	local_length = round_up(length - ((MAX_LBA_SIZE) - hdr_sz),
+				stm32image_dev.lba_size);
 
-		if (result != 0) {
-			ERROR("%s: io_open (%i)\n", __func__, result);
-			break;
-		}
-
-		result = io_seek(backend_handle, IO_SEEK_SET,
-				 *stm32_img + offset);
-
-		if (result != 0) {
-			ERROR("%s: io_seek (%i)\n", __func__, result);
-			*length_read = 0;
-			io_close(backend_handle);
-			break;
-		}
-
-		result = io_read(backend_handle, (uintptr_t)local_buffer,
-				 local_length, length_read);
+	if ((header->load_address != 0U) && (header->load_address != buffer)) {
+		ERROR("Wrong load address\n");
+		panic();
+	}
 
-		/* Adding part of size already read from header */
-		*length_read += MAX_LBA_SIZE - sizeof(boot_api_image_header_t);
+	result = io_open(backend_dev_handle, backend_image_spec,
+			 &backend_handle);
+	if (result != 0) {
+		ERROR("%s: io_open (%i)\n", __func__, result);
+		return result;
+	}
 
-		if (result != 0) {
-			ERROR("%s: io_read (%i)\n", __func__, result);
-			*length_read = 0;
-			header->magic = 0;
-			continue;
-		}
+	result = io_seek(backend_handle, IO_SEEK_SET, *stm32_img + offset);
+	if (result != 0) {
+		ERROR("%s: io_seek (%i)\n", __func__, result);
+		goto out;
+	}
 
-		result = stm32mp_check_header(header, buffer);
-		if (result != 0) {
-			ERROR("Header check failed\n");
-			*length_read = 0;
-			header->magic = 0;
-		}
+	result = io_read(backend_handle, (uintptr_t)local_buffer,
+			 local_length, length_read);
+	if (result != 0) {
+		ERROR("%s: io_read (%i)\n", __func__, result);
+		goto out;
+	}
 
-		result = stm32mp_auth_image(header, buffer);
-		if (result != 0) {
-			ERROR("Authentication Failed (%i)\n", result);
-			return result;
-		}
+	/* Adding part of size already read from header */
+	*length_read += MAX_LBA_SIZE - hdr_sz;
 
-		io_close(backend_handle);
-	}
+	inv_dcache_range(round_up((uintptr_t)(local_buffer + length - hdr_sz),
+				  CACHE_WRITEBACK_GRANULE), *length_read - length + hdr_sz);
 
+out:
+	io_close(backend_handle);
 	return result;
 }
 
diff --git a/drivers/st/iwdg/stm32_iwdg.c b/drivers/st/iwdg/stm32_iwdg.c
index c052b4dfbe..f68f95371f 100644
--- a/drivers/st/iwdg/stm32_iwdg.c
+++ b/drivers/st/iwdg/stm32_iwdg.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -15,6 +15,7 @@
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <drivers/arm/gicv2.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/st/stm32_iwdg.h>
 #include <drivers/st/stm32mp_clkfunc.h>
@@ -22,11 +23,30 @@
 #include <lib/utils.h>
 #include <plat/common/platform.h>
 
+#define IWDG_TIMEOUT_MS		U(100)
+
 /* 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(11, 0)
 
 struct stm32_iwdg_instance {
 	uintptr_t base;
@@ -52,6 +72,122 @@ static int stm32_iwdg_get_dt_node(struct dt_node_info *info, int offset)
 	return node;
 }
 
+#if defined(IMAGE_BL32)
+void __dead2 stm32_iwdg_it_handler(int id)
+{
+	struct stm32_iwdg_instance *iwdg;
+	unsigned int instance;
+
+	for (instance = 0; instance < IWDG_MAX_INSTANCE; instance++) {
+		if (stm32_iwdg[instance].num_irq == id) {
+			break;
+		}
+	}
+
+	if (instance == IWDG_MAX_INSTANCE) {
+		panic();
+	}
+
+	iwdg = &stm32_iwdg[instance];
+
+#if DEBUG
+	INFO("CPU %x IT Watchdog %u\n", plat_my_core_pos(), instance + 1U);
+
+	stm32mp_dump_core_registers(true);
+#endif
+	stm32_iwdg_refresh();
+
+	clk_enable(iwdg->clock);
+
+	mmio_clrsetbits_32(iwdg->base + IWDG_EWCR_OFFSET,
+			   IWDG_EWCR_EWIE, IWDG_EWCR_EWIC);
+
+	clk_disable(iwdg->clock);
+
+	/* Ack interrupt as we do not return from next call */
+	gicv2_end_of_interrupt(id);
+
+#if DEBUG
+	if (!stm32mp_is_single_core()) {
+		unsigned int sec_cpu = (plat_my_core_pos() == STM32MP_PRIMARY_CPU) ?
+			STM32MP_SECONDARY_CPU : STM32MP_PRIMARY_CPU;
+
+		gicv2_raise_sgi(ARM_IRQ_SEC_SGI_1, sec_cpu);
+	}
+#endif
+
+	for ( ; ; ) {
+		;
+	}
+}
+
+static int stm32_iwdg_get_secure_timeout(int node)
+{
+	void *fdt;
+	const fdt32_t *cuint;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -1;
+	}
+
+	cuint = fdt_getprop(fdt, node, "secure-timeout-sec", NULL);
+	if (cuint == NULL) {
+		return -1;
+	}
+
+	return (int)fdt32_to_cpu(*cuint);
+}
+
+static int stm32_iwdg_conf_etimeout(int node, struct stm32_iwdg_instance *iwdg)
+{
+	int id_lsi;
+	int dt_secure_timeout = stm32_iwdg_get_secure_timeout(node);
+	uint32_t reload, status;
+	unsigned int timeout = IWDG_TIMEOUT_MS;
+	unsigned long long reload_ll;
+
+	if (dt_secure_timeout < 0) {
+		return 0;
+	}
+
+	if (dt_secure_timeout == 0) {
+		return -EINVAL;
+	}
+
+	id_lsi = fdt_get_clock_id_by_name(node, "lsi");
+	if (id_lsi < 0) {
+		return -EINVAL;
+	}
+
+	/* Prescaler fix to 256 */
+	reload_ll = (unsigned long long)dt_secure_timeout * clk_get_rate(id_lsi);
+	reload = ((uint32_t)(reload_ll >> 8) - 1U) & IWDG_EWCR_EWIT_MASK;
+
+	clk_enable(iwdg->clock);
+
+	mmio_write_32(iwdg->base + IWDG_KR_OFFSET, IWDG_KR_START_KEY);
+	mmio_write_32(iwdg->base + IWDG_KR_OFFSET, IWDG_KR_ACCESS_KEY);
+	mmio_write_32(iwdg->base + IWDG_PR_OFFSET, IWDG_PR_DIV_256);
+	mmio_write_32(iwdg->base + IWDG_EWCR_OFFSET, IWDG_EWCR_EWIE | reload);
+
+	do {
+		status = mmio_read_32(iwdg->base + IWDG_SR_OFFSET) &
+			 IWDG_SR_EWU;
+		timeout--;
+		mdelay(1);
+	} while ((status != 0U) && (timeout != 0U));
+
+	iwdg->num_irq = stm32_gic_enable_spi(node, NULL);
+	if (iwdg->num_irq < 0) {
+		panic();
+	}
+
+	clk_disable(iwdg->clock);
+
+	return (timeout == 0U) ? -ETIMEDOUT : 0;
+}
+#endif
+
 void stm32_iwdg_refresh(void)
 {
 	uint8_t i;
@@ -61,12 +197,12 @@ void stm32_iwdg_refresh(void)
 
 		/* 0x00000000 is not a valid address for IWDG peripherals */
 		if (iwdg->base != 0U) {
-			stm32mp_clk_enable(iwdg->clock);
+			clk_enable(iwdg->clock);
 
 			mmio_write_32(iwdg->base + IWDG_KR_OFFSET,
 				      IWDG_KR_RELOAD_KEY);
 
-			stm32mp_clk_disable(iwdg->clock);
+			clk_disable(iwdg->clock);
 		}
 	}
 }
@@ -74,6 +210,7 @@ void stm32_iwdg_refresh(void)
 int stm32_iwdg_init(void)
 {
 	int node = -1;
+	int __unused res;
 	struct dt_node_info dt_info;
 	void *fdt;
 	uint32_t __unused count = 0;
@@ -143,6 +280,14 @@ int stm32_iwdg_init(void)
 			stm32mp_register_secure_periph_iomem(iwdg->base);
 		}
 
+#if defined(IMAGE_BL32)
+		res = stm32_iwdg_conf_etimeout(node, iwdg);
+		if (res != 0) {
+			ERROR("IWDG%x early timeout config failed (%d)\n",
+			      idx + 1, res);
+			return res;
+		}
+#endif
 #if defined(IMAGE_BL2)
 		if (stm32_iwdg_shadow_update(idx, iwdg->flags) != BSEC_OK) {
 			return -1;
diff --git a/drivers/st/mmc/stm32_sdmmc2.c b/drivers/st/mmc/stm32_sdmmc2.c
index cff3a344f7..8c203a4c03 100644
--- a/drivers/st/mmc/stm32_sdmmc2.c
+++ b/drivers/st/mmc/stm32_sdmmc2.c
@@ -15,6 +15,7 @@
 #include <arch.h>
 #include <arch_helpers.h>
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/mmc.h>
 #include <drivers/st/stm32_gpio.h>
@@ -50,6 +51,7 @@
 
 /* SDMMC power control register */
 #define SDMMC_POWER_PWRCTRL		GENMASK(1, 0)
+#define SDMMC_POWER_PWRCTRL_PWR_CYCLE	BIT(1)
 #define SDMMC_POWER_DIRPOL		BIT(4)
 
 /* SDMMC clock control register */
@@ -117,6 +119,13 @@
 #define TIMEOUT_US_10_MS		10000U
 #define TIMEOUT_US_1_S			1000000U
 
+/* Power cycle delays in ms */
+#define VCC_POWER_OFF_DELAY		2
+#define VCC_POWER_ON_DELAY		2
+#define POWER_CYCLE_DELAY		2
+#define POWER_OFF_DELAY			2
+#define POWER_ON_DELAY			1
+
 #define DT_SDMMC2_COMPAT		"st,stm32-sdmmc2"
 
 static void stm32_sdmmc2_init(void);
@@ -138,12 +147,35 @@ static const struct mmc_ops stm32_sdmmc2_ops = {
 
 static struct stm32_sdmmc2_params sdmmc2_params;
 
+static bool next_cmd_is_acmd;
+
 #pragma weak plat_sdmmc2_use_dma
 bool plat_sdmmc2_use_dma(unsigned int instance, unsigned int memory)
 {
 	return false;
 }
 
+static void dump_registers(void)
+{
+	uintptr_t base = sdmmc2_params.reg_base;
+
+	INFO("SDMMC_POWER    = 0x%x\n", mmio_read_32(base + SDMMC_POWER));
+	INFO("SDMMC_CLKCR    = 0x%x\n", mmio_read_32(base + SDMMC_CLKCR));
+	INFO("SDMMC_ARGR     = 0x%x\n", mmio_read_32(base + SDMMC_ARGR));
+	INFO("SDMMC_CMDR     = 0x%x\n", mmio_read_32(base + SDMMC_CMDR));
+	INFO("SDMMC_RESPCMDR = 0x%x\n", mmio_read_32(base + SDMMC_RESPCMDR));
+	INFO("SDMMC_RESP1R   = 0x%x\n", mmio_read_32(base + SDMMC_RESP1R));
+	INFO("SDMMC_RESP2R   = 0x%x\n", mmio_read_32(base + SDMMC_RESP2R));
+	INFO("SDMMC_RESP3R   = 0x%x\n", mmio_read_32(base + SDMMC_RESP3R));
+	INFO("SDMMC_RESP4R   = 0x%x\n", mmio_read_32(base + SDMMC_RESP4R));
+	INFO("SDMMC_DTIMER   = 0x%x\n", mmio_read_32(base + SDMMC_DTIMER));
+	INFO("SDMMC_DLENR    = 0x%x\n", mmio_read_32(base + SDMMC_DLENR));
+	INFO("SDMMC_DCTRLR   = 0x%x\n", mmio_read_32(base + SDMMC_DCTRLR));
+	INFO("SDMMC_DCNTR    = 0x%x\n", mmio_read_32(base + SDMMC_DCNTR));
+	INFO("SDMMC_MASKR    = 0x%x\n", mmio_read_32(base + SDMMC_MASKR));
+	INFO("SDMMC_ACKTIMER = 0x%x\n", mmio_read_32(base + SDMMC_ACKTIMER));
+}
+
 static void stm32_sdmmc2_init(void)
 {
 	uint32_t clock_div;
@@ -154,6 +186,26 @@ static void stm32_sdmmc2_init(void)
 		freq = MIN(sdmmc2_params.max_freq, freq);
 	}
 
+	if (sdmmc2_params.vmmc_regu.id != -1) {
+		stm32mp_regulator_register(&sdmmc2_params.vmmc_regu);
+		stm32mp_regulator_disable(&sdmmc2_params.vmmc_regu);
+	}
+
+	mdelay(VCC_POWER_OFF_DELAY);
+
+	mmio_write_32(base + SDMMC_POWER,
+		      SDMMC_POWER_PWRCTRL_PWR_CYCLE | sdmmc2_params.dirpol);
+	mdelay(POWER_CYCLE_DELAY);
+
+	if (sdmmc2_params.vmmc_regu.id != -1) {
+		stm32mp_regulator_enable(&sdmmc2_params.vmmc_regu);
+	}
+
+	mdelay(VCC_POWER_ON_DELAY);
+
+	mmio_write_32(base + SDMMC_POWER, sdmmc2_params.dirpol);
+	mdelay(POWER_OFF_DELAY);
+
 	clock_div = div_round_up(sdmmc2_params.clk_rate, freq * 2U);
 
 	mmio_write_32(base + SDMMC_CLKCR, SDMMC_CLKCR_HWFC_EN | clock_div |
@@ -163,7 +215,7 @@ static void stm32_sdmmc2_init(void)
 	mmio_write_32(base + SDMMC_POWER,
 		      SDMMC_POWER_PWRCTRL | sdmmc2_params.dirpol);
 
-	mdelay(1);
+	mdelay(POWER_ON_DELAY);
 }
 
 static int stm32_sdmmc2_stop_transfer(void)
@@ -221,6 +273,20 @@ static int stm32_sdmmc2_send_cmd_req(struct mmc_cmd *cmd)
 	case MMC_CMD(1):
 		arg_reg |= OCR_POWERUP;
 		break;
+	case MMC_CMD(6):
+		if ((sdmmc2_params.device_info->mmc_dev_type == MMC_IS_SD_HC) &&
+		    (!next_cmd_is_acmd)) {
+			cmd_reg |= SDMMC_CMDR_CMDTRANS;
+			if (sdmmc2_params.use_dma) {
+				flags_data |= SDMMC_STAR_DCRCFAIL |
+					SDMMC_STAR_DTIMEOUT |
+					SDMMC_STAR_DATAEND |
+					SDMMC_STAR_RXOVERR |
+					SDMMC_STAR_IDMATE |
+					SDMMC_STAR_DBCKEND;
+			}
+		}
+		break;
 	case MMC_CMD(8):
 		if (sdmmc2_params.device_info->mmc_dev_type == MMC_IS_EMMC) {
 			cmd_reg |= SDMMC_CMDR_CMDTRANS;
@@ -258,6 +324,8 @@ static int stm32_sdmmc2_send_cmd_req(struct mmc_cmd *cmd)
 		break;
 	}
 
+	next_cmd_is_acmd = (cmd->cmd_idx == MMC_CMD(55));
+
 	mmio_write_32(base + SDMMC_ICR, SDMMC_STATIC_FLAGS);
 
 	/*
@@ -265,8 +333,7 @@ static int stm32_sdmmc2_send_cmd_req(struct mmc_cmd *cmd)
 	 * Skip CMD55 as the next command could be data related, and
 	 * the register could have been set in prepare function.
 	 */
-	if (((cmd_reg & SDMMC_CMDR_CMDTRANS) == 0U) &&
-	    (cmd->cmd_idx != MMC_CMD(55))) {
+	if (((cmd_reg & SDMMC_CMDR_CMDTRANS) == 0U) && !next_cmd_is_acmd) {
 		mmio_write_32(base + SDMMC_DCTRLR, 0U);
 	}
 
@@ -563,6 +630,7 @@ static int stm32_sdmmc2_read(int lba, uintptr_t buf, size_t size)
 		if ((status & error_flags) != 0U) {
 			ERROR("%s: Read error (status = %x)\n", __func__,
 			      status);
+			dump_registers();
 			mmio_write_32(base + SDMMC_DCTRLR,
 				      SDMMC_DCTRLR_FIFORST);
 
@@ -580,6 +648,7 @@ static int stm32_sdmmc2_read(int lba, uintptr_t buf, size_t size)
 		if (timeout_elapsed(timeout)) {
 			ERROR("%s: timeout 1s (status = %x)\n",
 			      __func__, status);
+			dump_registers();
 			mmio_write_32(base + SDMMC_ICR,
 				      SDMMC_STATIC_FLAGS);
 
@@ -628,6 +697,7 @@ static int stm32_sdmmc2_dt_get_config(void)
 	int sdmmc_node;
 	void *fdt = NULL;
 	const fdt32_t *cuint;
+	struct dt_node_info dt_info;
 
 	if (fdt_get_address(&fdt) == 0) {
 		return -FDT_ERR_NOTFOUND;
@@ -637,27 +707,14 @@ static int stm32_sdmmc2_dt_get_config(void)
 		return -FDT_ERR_NOTFOUND;
 	}
 
-	sdmmc_node = fdt_node_offset_by_compatible(fdt, -1, DT_SDMMC2_COMPAT);
-
-	while (sdmmc_node != -FDT_ERR_NOTFOUND) {
-		cuint = fdt_getprop(fdt, sdmmc_node, "reg", NULL);
-		if (cuint == NULL) {
-			continue;
-		}
-
-		if (fdt32_to_cpu(*cuint) == sdmmc2_params.reg_base) {
-			break;
-		}
-
-		sdmmc_node = fdt_node_offset_by_compatible(fdt, sdmmc_node,
-							   DT_SDMMC2_COMPAT);
-	}
-
+	sdmmc_node = dt_match_instance_by_compatible(DT_SDMMC2_COMPAT,
+						     sdmmc2_params.reg_base);
 	if (sdmmc_node == -FDT_ERR_NOTFOUND) {
 		return -FDT_ERR_NOTFOUND;
 	}
 
-	if (fdt_get_status(sdmmc_node) == DT_DISABLED) {
+	dt_fill_device_info(&dt_info, sdmmc_node);
+	if (dt_info.status == DT_DISABLED) {
 		return -FDT_ERR_NOTFOUND;
 	}
 
@@ -665,21 +722,8 @@ static int stm32_sdmmc2_dt_get_config(void)
 		return -FDT_ERR_BADVALUE;
 	}
 
-	cuint = fdt_getprop(fdt, sdmmc_node, "clocks", NULL);
-	if (cuint == NULL) {
-		return -FDT_ERR_NOTFOUND;
-	}
-
-	cuint++;
-	sdmmc2_params.clock_id = fdt32_to_cpu(*cuint);
-
-	cuint = fdt_getprop(fdt, sdmmc_node, "resets", NULL);
-	if (cuint == NULL) {
-		return -FDT_ERR_NOTFOUND;
-	}
-
-	cuint++;
-	sdmmc2_params.reset_id = fdt32_to_cpu(*cuint);
+	sdmmc2_params.clock_id = dt_info.clock;
+	sdmmc2_params.reset_id = dt_info.reset;
 
 	if ((fdt_getprop(fdt, sdmmc_node, "st,use-ckin", NULL)) != NULL) {
 		sdmmc2_params.pin_ckin = SDMMC_CLKCR_SELCLKRX_0;
@@ -714,6 +758,11 @@ static int stm32_sdmmc2_dt_get_config(void)
 		sdmmc2_params.max_freq = fdt32_to_cpu(*cuint);
 	}
 
+	cuint = fdt_getprop(fdt, sdmmc_node, "vmmc-supply", NULL);
+	if (cuint != NULL) {
+		sdmmc2_params.vmmc_regu.id = fdt32_to_cpu(*cuint);
+	}
+
 	return 0;
 }
 
@@ -734,12 +783,14 @@ int stm32_sdmmc2_mmc_init(struct stm32_sdmmc2_params *params)
 
 	memcpy(&sdmmc2_params, params, sizeof(struct stm32_sdmmc2_params));
 
+	sdmmc2_params.vmmc_regu.id = -1;
+
 	if (stm32_sdmmc2_dt_get_config() != 0) {
 		ERROR("%s: DT error\n", __func__);
 		return -ENOMEM;
 	}
 
-	stm32mp_clk_enable(sdmmc2_params.clock_id);
+	clk_enable(sdmmc2_params.clock_id);
 
 	rc = stm32mp_reset_assert(sdmmc2_params.reset_id, TIMEOUT_US_1_MS);
 	if (rc != 0) {
@@ -752,7 +803,7 @@ int stm32_sdmmc2_mmc_init(struct stm32_sdmmc2_params *params)
 	}
 	mdelay(1);
 
-	sdmmc2_params.clk_rate = stm32mp_clk_get_rate(sdmmc2_params.clock_id);
+	sdmmc2_params.clk_rate = clk_get_rate(sdmmc2_params.clock_id);
 	sdmmc2_params.device_info->ocr_voltage = OCR_3_2_3_3 | OCR_3_3_3_4;
 
 	return mmc_init(&stm32_sdmmc2_ops, sdmmc2_params.clk_rate,
diff --git a/drivers/st/pmic/stm32mp_pmic.c b/drivers/st/pmic/stm32mp_pmic.c
index b2bb482f9d..4e505debdf 100644
--- a/drivers/st/pmic/stm32mp_pmic.c
+++ b/drivers/st/pmic/stm32mp_pmic.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <assert.h>
 #include <errno.h>
 
 #include <libfdt.h>
@@ -22,36 +23,56 @@
 #define STPMIC1_LDO12356_OUTPUT_SHIFT	2
 #define STPMIC1_LDO3_MODE		(uint8_t)(BIT(7))
 #define STPMIC1_LDO3_DDR_SEL		31U
-#define STPMIC1_LDO3_1800000		(9U << STPMIC1_LDO12356_OUTPUT_SHIFT)
 
 #define STPMIC1_BUCK_OUTPUT_SHIFT	2
 #define STPMIC1_BUCK3_1V8		(39U << STPMIC1_BUCK_OUTPUT_SHIFT)
 
+#define REGULATOR_MODE_STANDBY		8U
+
 #define STPMIC1_DEFAULT_START_UP_DELAY_MS	1
 
+#define CMD_GET_MIN_VOLTAGE		0U
+#define CMD_CONFIG_BOOT_ON		1U
+#define CMD_CONFIG_LP			2U
+
 static struct i2c_handle_s i2c_handle;
 static uint32_t pmic_i2c_addr;
 
 static int dt_get_pmic_node(void *fdt)
 {
-	return fdt_node_offset_by_compatible(fdt, -1, "st,stpmic1");
+	static int node = -FDT_ERR_BADOFFSET;
+
+	if (node == -FDT_ERR_BADOFFSET) {
+		node = fdt_node_offset_by_compatible(fdt, -1, "st,stpmic1");
+	}
+
+	return node;
 }
 
 int dt_pmic_status(void)
 {
+	static int status = -FDT_ERR_BADVALUE;
 	int node;
 	void *fdt;
 
+	if (status != -FDT_ERR_BADVALUE) {
+		return status;
+	}
+
 	if (fdt_get_address(&fdt) == 0) {
 		return -ENOENT;
 	}
 
 	node = dt_get_pmic_node(fdt);
 	if (node <= 0) {
-		return -FDT_ERR_NOTFOUND;
+		status = -FDT_ERR_NOTFOUND;
+
+		return status;
 	}
 
-	return fdt_get_status(node);
+	status = (int)fdt_get_status(node);
+
+	return status;
 }
 
 static bool dt_pmic_is_secure(void)
@@ -63,53 +84,173 @@ static bool dt_pmic_is_secure(void)
 	       (i2c_handle.dt_status == DT_SECURE);
 }
 
-/*
- * Get PMIC and its I2C bus configuration from the device tree.
- * Return 0 on success, negative on error, 1 if no PMIC node is found.
- */
-static int dt_pmic_i2c_config(struct dt_node_info *i2c_info,
-			      struct stm32_i2c_init_s *init)
+static int dt_pmic_get_regulator_voltage(void *fdt, int node,
+					 uint16_t *min_mv, uint16_t *max_mv)
 {
-	int pmic_node, i2c_node;
-	void *fdt;
 	const fdt32_t *cuint;
 
-	if (fdt_get_address(&fdt) == 0) {
-		return -ENOENT;
+	cuint = fdt_getprop(fdt, node, "regulator-min-microvolt", NULL);
+	if (cuint == NULL) {
+		return -FDT_ERR_NOTFOUND;
 	}
 
-	pmic_node = dt_get_pmic_node(fdt);
-	if (pmic_node < 0) {
-		return 1;
+	if (min_mv != NULL) {
+		*min_mv = (uint16_t)(fdt32_to_cpu(*cuint) / 1000U);
 	}
 
-	cuint = fdt_getprop(fdt, pmic_node, "reg", NULL);
+	cuint = fdt_getprop(fdt, node, "regulator-max-microvolt", NULL);
 	if (cuint == NULL) {
 		return -FDT_ERR_NOTFOUND;
 	}
 
-	pmic_i2c_addr = fdt32_to_cpu(*cuint) << 1;
-	if (pmic_i2c_addr > UINT16_MAX) {
-		return -EINVAL;
+	if (max_mv != NULL) {
+		*max_mv = (uint16_t)(fdt32_to_cpu(*cuint) / 1000U);
 	}
 
-	i2c_node = fdt_parent_offset(fdt, pmic_node);
-	if (i2c_node < 0) {
-		return -FDT_ERR_NOTFOUND;
+	return 0;
+}
+
+static int pmic_config_boot_on(void *fdt, int node, const char *regu_name)
+{
+	uint16_t voltage = 0U;
+	uint16_t voltage_min;
+	uint16_t voltage_max;
+	int status;
+	int pmic_voltage;
+
+	if ((fdt_getprop(fdt, node, "regulator-boot-on", NULL) == NULL) &&
+	    (fdt_getprop(fdt, node, "regulator-always-on", NULL) == NULL)) {
+		return 0;
 	}
 
-	dt_fill_device_info(i2c_info, i2c_node);
-	if (i2c_info->base == 0U) {
-		return -FDT_ERR_NOTFOUND;
+	if (fdt_getprop(fdt, node, "regulator-pull-down", NULL) != NULL) {
+
+		status = stpmic1_regulator_pull_down_set(regu_name);
+		if (status < 0) {
+			return status;
+		}
+	}
+
+	if (fdt_getprop(fdt, node, "st,mask-reset", NULL) != NULL) {
+
+		status = stpmic1_regulator_mask_reset_set(regu_name);
+		if (status < 0) {
+			return status;
+		}
+	}
+
+	if (dt_pmic_get_regulator_voltage(fdt, node, &voltage_min,
+					  &voltage_max) < 0) {
+		return 0;
+	}
+
+	pmic_voltage = stpmic1_regulator_voltage_get(regu_name);
+	if (pmic_voltage < 0) {
+		return pmic_voltage;
+	}
+
+	if ((uint16_t)pmic_voltage < voltage_min) {
+		voltage = voltage_min;
+	}
+
+	if ((uint16_t)pmic_voltage > voltage_max) {
+		voltage = voltage_max;
+	}
+
+	/* Only re-program voltage if not in the range provided in DT. */
+	if (voltage != 0U) {
+		status = stpmic1_regulator_voltage_set(regu_name, voltage);
+		if (status < 0) {
+			return status;
+		}
+	}
+
+	if (!stpmic1_is_regulator_enabled(regu_name)) {
+		status = stpmic1_regulator_enable(regu_name);
+		if (status < 0) {
+			return status;
+		}
 	}
 
-	return stm32_i2c_get_setup_from_fdt(fdt, i2c_node, init);
+	return 0;
+}
+
+#if defined(IMAGE_BL32)
+static int pmic_config_lp(void *fdt, int node, const char *node_name,
+			  const char *regu_name)
+{
+	int status;
+	const fdt32_t *cuint;
+	int regulator_state_node;
+
+	status = stpmic1_powerctrl_on();
+	if (status < 0) {
+		return status;
+	};
+
+	/*
+	 * First, copy active configuration (Control register) to
+	 * PWRCTRL Control register, even if regulator_state_node
+	 * does not exist.
+	 */
+	status = stpmic1_lp_copy_reg(regu_name);
+	if (status < 0) {
+		return status;
+	}
+
+	/* Then apply configs from regulator_state_node */
+	regulator_state_node = fdt_subnode_offset(fdt, node, node_name);
+	if (regulator_state_node <= 0) {
+		return 0;
+	}
+
+	if (fdt_getprop(fdt, regulator_state_node, "regulator-on-in-suspend",
+			NULL) != NULL) {
+		status = stpmic1_lp_reg_on_off(regu_name, 1);
+		if (status < 0) {
+			return status;
+		}
+	}
+
+	if (fdt_getprop(fdt, regulator_state_node, "regulator-off-in-suspend",
+			NULL) != NULL) {
+		status = stpmic1_lp_reg_on_off(regu_name, 0);
+		if (status < 0) {
+			return status;
+		}
+	}
+
+	cuint = fdt_getprop(fdt, regulator_state_node,
+			    "regulator-suspend-microvolt", NULL);
+	if (cuint != NULL) {
+		uint16_t voltage = (uint16_t)(fdt32_to_cpu(*cuint) / 1000U);
+
+		status = stpmic1_lp_set_voltage(regu_name, voltage);
+		if (status < 0) {
+			return status;
+		}
+	}
+
+	cuint = fdt_getprop(fdt, regulator_state_node, "regulator-mode", NULL);
+	if (cuint != NULL) {
+		if (fdt32_to_cpu(*cuint) == REGULATOR_MODE_STANDBY) {
+			status = stpmic1_lp_set_mode(regu_name, 1);
+			if (status < 0) {
+				return status;
+			}
+		}
+	}
+
+	return 0;
 }
+#endif
 
-int dt_pmic_configure_boot_on_regulators(void)
+static int pmic_operate(uint8_t command, const char *node_name,
+			uint16_t *voltage_mv)
 {
-	int pmic_node, regulators_node, regulator_node;
+	int pmic_node, regulators_node, subnode;
 	void *fdt;
+	int ret = -EIO;
 
 	if (fdt_get_address(&fdt) == 0) {
 		return -ENOENT;
@@ -117,70 +258,166 @@ int dt_pmic_configure_boot_on_regulators(void)
 
 	pmic_node = dt_get_pmic_node(fdt);
 	if (pmic_node < 0) {
-		return -FDT_ERR_NOTFOUND;
+		return -ENOENT;
 	}
 
 	regulators_node = fdt_subnode_offset(fdt, pmic_node, "regulators");
+	if (regulators_node < 0) {
+		return -ENOENT;
+	}
 
-	fdt_for_each_subnode(regulator_node, fdt, regulators_node) {
-		const fdt32_t *cuint;
-		const char *node_name = fdt_get_name(fdt, regulator_node, NULL);
-		uint16_t voltage;
-		int status;
-
-#if defined(IMAGE_BL2)
-		if ((fdt_getprop(fdt, regulator_node, "regulator-boot-on",
-				 NULL) == NULL) &&
-		    (fdt_getprop(fdt, regulator_node, "regulator-always-on",
-				 NULL) == NULL)) {
-#else
-		if (fdt_getprop(fdt, regulator_node, "regulator-boot-on",
-				NULL) == NULL) {
-#endif
-			continue;
-		}
+	fdt_for_each_subnode(subnode, fdt, regulators_node) {
+		const char *regu_name = fdt_get_name(fdt, subnode, NULL);
 
-		if (fdt_getprop(fdt, regulator_node, "regulator-pull-down",
-				NULL) != NULL) {
+		switch (command) {
+		case CMD_GET_MIN_VOLTAGE:
+			assert(node_name != NULL);
+			assert(voltage_mv != NULL);
 
-			status = stpmic1_regulator_pull_down_set(node_name);
-			if (status != 0) {
-				return status;
+			if (strcmp(regu_name, node_name) != 0) {
+				continue;
 			}
-		}
 
-		if (fdt_getprop(fdt, regulator_node, "st,mask-reset",
-				NULL) != NULL) {
+			ret = dt_pmic_get_regulator_voltage(fdt, subnode,
+							    voltage_mv, NULL);
+			if (ret < 0) {
+				return -ENXIO;
+			}
 
-			status = stpmic1_regulator_mask_reset_set(node_name);
-			if (status != 0) {
-				return status;
+			return ret;
+
+		case CMD_CONFIG_BOOT_ON:
+			ret = pmic_config_boot_on(fdt, subnode, regu_name);
+			if (ret < 0) {
+				return ret;
 			}
+			break;
+
+#if defined(IMAGE_BL32)
+		case CMD_CONFIG_LP:
+			assert(node_name != NULL);
+
+			ret = pmic_config_lp(fdt, subnode, node_name,
+					     regu_name);
+			if (ret < 0) {
+				return ret;
+			}
+			break;
+#endif
+
+		default:
+			return -EINVAL;
+		}
+	}
+
+	return ret;
+}
+
+/*
+ * Get PMIC and its I2C bus configuration from the device tree.
+ * Return 0 on success, negative on error, 1 if no PMIC node is defined.
+ */
+static int dt_pmic_i2c_config(struct dt_node_info *i2c_info,
+			      struct stm32_i2c_init_s *init)
+{
+	static int i2c_node = -FDT_ERR_NOTFOUND;
+
+	if (i2c_node == -FDT_ERR_NOTFOUND) {
+		void *fdt;
+		int pmic_node;
+		const fdt32_t *cuint;
+
+		if (fdt_get_address(&fdt) == 0) {
+			return -FDT_ERR_NOTFOUND;
+		}
+
+		pmic_node = dt_get_pmic_node(fdt);
+		if (pmic_node < 0) {
+			return 1;
 		}
 
-		cuint = fdt_getprop(fdt, regulator_node,
-				    "regulator-min-microvolt", NULL);
+		cuint = fdt_getprop(fdt, pmic_node, "reg", NULL);
 		if (cuint == NULL) {
-			continue;
+			return -FDT_ERR_NOTFOUND;
 		}
 
-		/* DT uses microvolts, whereas driver awaits millivolts */
-		voltage = (uint16_t)(fdt32_to_cpu(*cuint) / 1000U);
+		pmic_i2c_addr = fdt32_to_cpu(*cuint) << 1;
+		if (pmic_i2c_addr > UINT16_MAX) {
+			return -FDT_ERR_BADVALUE;
+		}
 
-		status = stpmic1_regulator_voltage_set(node_name, voltage);
-		if (status != 0) {
-			return status;
+		i2c_node = fdt_parent_offset(fdt, pmic_node);
+		if (i2c_node < 0) {
+			return -FDT_ERR_NOTFOUND;
 		}
+	}
 
-		if (stpmic1_is_regulator_enabled(node_name) == 0U) {
-			status = stpmic1_regulator_enable(node_name);
-			if (status != 0) {
-				return status;
-			}
+	dt_fill_device_info(i2c_info, i2c_node);
+	if (i2c_info->base == 0U) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return stm32_i2c_get_setup_from_fdt(i2c_node, init);
+}
+
+int pmic_configure_boot_on_regulators(void)
+{
+	return pmic_operate(CMD_CONFIG_BOOT_ON, NULL, NULL);
+}
+
+int pmic_set_lp_config(const char *node_name)
+{
+	return pmic_operate(CMD_CONFIG_LP, node_name, NULL);
+}
+
+int dt_pmic_find_supply(const char **supply_name, const char *regu_name)
+{
+	int pmic_node, regulators_node, subnode;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	pmic_node = dt_get_pmic_node(fdt);
+	if (pmic_node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	regulators_node = fdt_subnode_offset(fdt, pmic_node, "regulators");
+	if (regulators_node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, regulators_node) {
+		const char *name;
+
+		name = fdt_getprop(fdt, subnode, "regulator-name", NULL);
+		if ((name != NULL) &&
+		    (strcmp(name, regu_name) == 0)) {
+			*supply_name = fdt_get_name(fdt, subnode, NULL);
+			return 0;
 		}
 	}
 
-	return 0;
+	return -FDT_ERR_NOTFOUND;
+}
+
+int pmic_set_regulator_min_voltage(const char *regu_name)
+{
+	int rc = -ENOENT;
+	const char *supply_name;
+
+	if (dt_pmic_find_supply(&supply_name, regu_name) == 0) {
+		uint16_t min_mv;
+
+		rc = pmic_operate(CMD_GET_MIN_VOLTAGE, supply_name, &min_mv);
+		if (rc == 0) {
+			rc = stpmic1_regulator_voltage_set(supply_name, min_mv);
+		}
+	}
+
+	return rc;
 }
 
 bool initialize_pmic_i2c(void)
@@ -204,6 +441,7 @@ bool initialize_pmic_i2c(void)
 	i2c->i2c_base_addr		= i2c_info.base;
 	i2c->dt_status			= i2c_info.status;
 	i2c->clock			= i2c_info.clock;
+	i2c->i2c_state			= I2C_STATE_RESET;
 	i2c_init.own_address1		= pmic_i2c_addr;
 	i2c_init.addressing_mode	= I2C_ADDRESSINGMODE_7BIT;
 	i2c_init.dual_address_mode	= I2C_DUALADDRESS_DISABLE;
@@ -245,16 +483,83 @@ static void register_pmic_shared_peripherals(void)
 	}
 }
 
-void initialize_pmic(void)
+static int pmic_regulator_enable(struct stm32mp_regulator *regu)
 {
-	unsigned long pmic_version;
+	void *fdt;
+	const char *node_name;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	node_name = fdt_get_name(fdt, fdt_node_offset_by_phandle(fdt, regu->id),
+				 NULL);
+
+	return stpmic1_regulator_enable(node_name);
+}
+
+static int pmic_regulator_disable(struct stm32mp_regulator *regu)
+{
+	void *fdt;
+	const char *node_name;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	node_name = fdt_get_name(fdt, fdt_node_offset_by_phandle(fdt, regu->id),
+				 NULL);
+
+	return stpmic1_regulator_disable(node_name);
+}
+
+static const struct stm32mp_regulator_ops pmic_regu_ops = {
+	.enable = pmic_regulator_enable,
+	.disable = pmic_regulator_disable,
+};
+
+bool is_pmic_regulator(struct stm32mp_regulator *regu)
+{
+	void *fdt;
+	int parent_node;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return false;
+	}
+
+	parent_node = fdt_parent_offset(fdt,
+					fdt_node_offset_by_phandle(fdt,
+								   regu->id));
+	return (fdt_node_check_compatible(fdt, parent_node,
+					  "st,stpmic1-regulators") == 0);
+}
+
+void bind_pmic_regulator(struct stm32mp_regulator *regu)
+{
+	regu->ops = &pmic_regu_ops;
+}
 
+void initialize_pmic(void)
+{
 	if (!initialize_pmic_i2c()) {
 		VERBOSE("No PMIC\n");
 		return;
 	}
 
 	register_pmic_shared_peripherals();
+}
+
+void configure_pmic(void)
+{
+	if (pmic_configure_boot_on_regulators() < 0) {
+		panic();
+	};
+}
+
+#if DEBUG
+void print_pmic_info_and_debug(void)
+{
+	unsigned long pmic_version;
 
 	if (stpmic1_get_version(&pmic_version) != 0) {
 		ERROR("Failed to access PMIC\n");
@@ -263,19 +568,20 @@ void initialize_pmic(void)
 
 	INFO("PMIC version = 0x%02lx\n", pmic_version);
 	stpmic1_dump_regulators();
-
-#if defined(IMAGE_BL2)
-	if (dt_pmic_configure_boot_on_regulators() != 0) {
-		panic();
-	};
-#endif
 }
+#endif
 
 int pmic_ddr_power_init(enum ddr_type ddr_type)
 {
 	bool buck3_at_1v8 = false;
 	uint8_t read_val;
 	int status;
+	uint16_t buck2_mv;
+	uint16_t ldo3_mv;
+
+	if (pmic_operate(CMD_GET_MIN_VOLTAGE, "buck2", &buck2_mv) != 0) {
+		return -EPERM;
+	}
 
 	switch (ddr_type) {
 	case STM32MP_DDR3:
@@ -295,7 +601,7 @@ int pmic_ddr_power_init(enum ddr_type ddr_type)
 			return status;
 		}
 
-		status = stpmic1_regulator_voltage_set("buck2", 1350);
+		status = stpmic1_regulator_voltage_set("buck2", buck2_mv);
 		if (status != 0) {
 			return status;
 		}
@@ -345,7 +651,6 @@ int pmic_ddr_power_init(enum ddr_type ddr_type)
 
 		read_val &= ~STPMIC1_LDO3_MODE;
 		read_val &= ~STPMIC1_LDO12356_OUTPUT_MASK;
-		read_val |= STPMIC1_LDO3_1800000;
 		if (buck3_at_1v8) {
 			read_val |= STPMIC1_LDO3_MODE;
 		}
@@ -355,7 +660,16 @@ int pmic_ddr_power_init(enum ddr_type ddr_type)
 			return status;
 		}
 
-		status = stpmic1_regulator_voltage_set("buck2", 1200);
+		if (pmic_operate(CMD_GET_MIN_VOLTAGE, "ldo3", &ldo3_mv) != 0) {
+			return -EPERM;
+		}
+
+		status = stpmic1_regulator_voltage_set("ldo3", ldo3_mv);
+		if (status != 0) {
+			return status;
+		}
+
+		status = stpmic1_regulator_voltage_set("buck2", buck2_mv);
 		if (status != 0) {
 			return status;
 		}
diff --git a/drivers/st/pmic/stpmic1.c b/drivers/st/pmic/stpmic1.c
index 9999630545..a1db120dc1 100644
--- a/drivers/st/pmic/stpmic1.c
+++ b/drivers/st/pmic/stpmic1.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2016-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2016-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <errno.h>
 #include <string.h>
 
 #include <common/debug.h>
@@ -419,6 +420,10 @@ static const uint16_t vref_ddr_voltage_table[] = {
 	3300,
 };
 
+static const uint16_t fixed_5v_voltage_table[] = {
+	5000,
+};
+
 /* Table of Regulators in PMIC SoC */
 static const struct regul_struct regulators_table[] = {
 	{
@@ -528,6 +533,27 @@ static const struct regul_struct regulators_table[] = {
 		.mask_reset_reg	= MASK_RESET_LDO_REG,
 		.mask_reset	= VREF_DDR_MASK_RESET,
 	},
+	{
+		.dt_node_name	= "boost",
+		.voltage_table	= fixed_5v_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(fixed_5v_voltage_table),
+		.control_reg	= USB_CONTROL_REG,
+		.mask_reset	= BOOST_ENABLED,
+	},
+	{
+		.dt_node_name	= "pwr_sw1",
+		.voltage_table	= fixed_5v_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(fixed_5v_voltage_table),
+		.control_reg	= USB_CONTROL_REG,
+		.mask_reset	= USBSW_OTG_SWITCH_ENABLED,
+	},
+	{
+		.dt_node_name	= "pwr_sw2",
+		.voltage_table	= fixed_5v_voltage_table,
+		.voltage_table_size = ARRAY_SIZE(fixed_5v_voltage_table),
+		.control_reg	= USB_CONTROL_REG,
+		.mask_reset	= SWIN_SWOUT_ENABLED,
+	},
 };
 
 #define MAX_REGUL	ARRAY_SIZE(regulators_table)
@@ -581,17 +607,19 @@ int stpmic1_regulator_enable(const char *name)
 {
 	const struct regul_struct *regul = get_regulator_data(name);
 
-	return stpmic1_register_update(regul->control_reg, BIT(0), BIT(0));
+	return stpmic1_register_update(regul->control_reg, LDO_BUCK_ENABLE_MASK,
+				       LDO_BUCK_ENABLE_MASK);
 }
 
 int stpmic1_regulator_disable(const char *name)
 {
 	const struct regul_struct *regul = get_regulator_data(name);
 
-	return stpmic1_register_update(regul->control_reg, 0, BIT(0));
+	return stpmic1_register_update(regul->control_reg, 0,
+				       LDO_BUCK_ENABLE_MASK);
 }
 
-uint8_t stpmic1_is_regulator_enabled(const char *name)
+bool stpmic1_is_regulator_enabled(const char *name)
 {
 	uint8_t val;
 	const struct regul_struct *regul = get_regulator_data(name);
@@ -600,7 +628,7 @@ uint8_t stpmic1_is_regulator_enabled(const char *name)
 		panic();
 	}
 
-	return (val & 0x1U);
+	return (val & LDO_BUCK_ENABLE_MASK) == LDO_BUCK_ENABLE_MASK;
 }
 
 int stpmic1_regulator_voltage_set(const char *name, uint16_t millivolts)
@@ -648,11 +676,68 @@ int stpmic1_regulator_mask_reset_set(const char *name)
 				       regul->mask_reset);
 }
 
+/* Low-power functions */
+int stpmic1_lp_copy_reg(const char *name)
+{
+	uint8_t val;
+	int status;
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	if (regul->low_power_reg == 0U) {
+		return 0;
+	}
+
+	status = stpmic1_register_read(regul->control_reg, &val);
+	if (status != 0) {
+		return status;
+	}
+
+	return stpmic1_register_write(regul->low_power_reg, val);
+}
+
+int stpmic1_lp_reg_on_off(const char *name, uint8_t enable)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	return stpmic1_register_update(regul->low_power_reg, enable,
+				       LDO_BUCK_ENABLE_MASK);
+}
+
+int stpmic1_lp_set_mode(const char *name, uint8_t hplp)
+{
+	const struct regul_struct *regul = get_regulator_data(name);
+
+	return stpmic1_register_update(regul->low_power_reg,
+				       hplp << LDO_BUCK_HPLP_SHIFT,
+				       LDO_BUCK_HPLP_ENABLE_MASK);
+}
+
+int stpmic1_lp_set_voltage(const char *name, uint16_t millivolts)
+{
+	uint8_t voltage_index = voltage_to_index(name, millivolts);
+	const struct regul_struct *regul = get_regulator_data(name);
+	uint8_t mask;
+
+	/* Voltage can be set for buck<N> or ldo<N> (except ldo4) regulators */
+	if (strncmp(name, "buck", 4) == 0) {
+		mask = BUCK_VOLTAGE_MASK;
+	} else if ((strncmp(name, "ldo", 3) == 0) &&
+		   (strncmp(name, "ldo4", 4) != 0)) {
+		mask = LDO_VOLTAGE_MASK;
+	} else {
+		return 0;
+	}
+
+	return stpmic1_register_update(regul->low_power_reg, voltage_index << 2,
+				       mask);
+}
+
 int stpmic1_regulator_voltage_get(const char *name)
 {
 	const struct regul_struct *regul = get_regulator_data(name);
 	uint8_t value;
 	uint8_t mask;
+	int status;
 
 	/* Voltage can be set for buck<N> or ldo<N> (except ldo4) regulators */
 	if (strncmp(name, "buck", 4) == 0) {
@@ -664,13 +749,16 @@ int stpmic1_regulator_voltage_get(const char *name)
 		return 0;
 	}
 
-	if (stpmic1_register_read(regul->control_reg, &value))
-		return -1;
+	status = stpmic1_register_read(regul->control_reg, &value);
+	if (status < 0) {
+		return status;
+	}
 
 	value = (value & mask) >> LDO_BUCK_VOLTAGE_SHIFT;
 
-	if (value > regul->voltage_table_size)
-		return -1;
+	if (value > regul->voltage_table_size) {
+		return -ERANGE;
+	}
 
 	return (int)regul->voltage_table[value];
 }
@@ -706,7 +794,7 @@ int stpmic1_register_write(uint8_t register_id, uint8_t value)
 		}
 
 		if (readval != value) {
-			return -1;
+			return -EIO;
 		}
 	}
 #endif
@@ -751,12 +839,12 @@ void stpmic1_dump_regulators(void)
 
 int stpmic1_get_version(unsigned long *version)
 {
-	int rc;
 	uint8_t read_val;
+	int status;
 
-	rc = stpmic1_register_read(VERSION_STATUS_REG, &read_val);
-	if (rc) {
-		return -1;
+	status = stpmic1_register_read(VERSION_STATUS_REG, &read_val);
+	if (status < 0) {
+		return status;
 	}
 
 	*version = (unsigned long)read_val;
diff --git a/drivers/st/regulator/stm32mp_dummy_regulator.c b/drivers/st/regulator/stm32mp_dummy_regulator.c
new file mode 100644
index 0000000000..1003aba054
--- /dev/null
+++ b/drivers/st/regulator/stm32mp_dummy_regulator.c
@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) 2019, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <drivers/st/stm32mp_dummy_regulator.h>
+
+static int dummy_regulator_enable(struct stm32mp_regulator *regu)
+{
+	return 0;
+}
+
+static int dummy_regulator_disable(struct stm32mp_regulator *regu)
+{
+	return 0;
+}
+
+static const struct stm32mp_regulator_ops dummy_regu_ops = {
+	.enable = dummy_regulator_enable,
+	.disable = dummy_regulator_disable,
+};
+
+void bind_dummy_regulator(struct stm32mp_regulator *regu)
+{
+	regu->ops = &dummy_regu_ops;
+}
diff --git a/drivers/st/regulator/stm32mp_regulator.c b/drivers/st/regulator/stm32mp_regulator.c
new file mode 100644
index 0000000000..f0e4a4ae5c
--- /dev/null
+++ b/drivers/st/regulator/stm32mp_regulator.c
@@ -0,0 +1,38 @@
+/*
+ * Copyright (C) 2019, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <drivers/st/stm32mp_regulator.h>
+
+#pragma weak plat_bind_regulator
+int plat_bind_regulator(struct stm32mp_regulator *regu)
+{
+	return -1;
+}
+
+int stm32mp_regulator_enable(struct stm32mp_regulator *regu)
+{
+	assert((regu->ops != NULL) && (regu->ops->enable != NULL));
+
+	return regu->ops->enable(regu);
+}
+
+int stm32mp_regulator_disable(struct stm32mp_regulator *regu)
+{
+	assert((regu->ops != NULL) && (regu->ops->disable != NULL));
+
+	if (regu->always_on) {
+		return 0;
+	}
+
+	return regu->ops->disable(regu);
+}
+
+int stm32mp_regulator_register(struct stm32mp_regulator *regu)
+{
+	return plat_bind_regulator(regu);
+}
diff --git a/drivers/st/reset/stm32mp1_reset.c b/drivers/st/reset/stm32mp1_reset.c
index 98c8dcf710..a803923fca 100644
--- a/drivers/st/reset/stm32mp1_reset.c
+++ b/drivers/st/reset/stm32mp1_reset.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2018-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -67,3 +67,32 @@ int stm32mp_reset_deassert(uint32_t id, unsigned int to_us)
 
 	return 0;
 }
+
+void stm32mp_reset_assert_deassert_to_mcu(bool assert_not_deassert)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	/*
+	 * The RCC_MP_GCR is a read/write register.
+	 * Assert the MCU HOLD_BOOT means clear the BOOT_MCU bit
+	 * Deassert the MCU HOLD_BOOT means set the BOOT_MCU the bit
+	 */
+	if (assert_not_deassert) {
+		mmio_clrbits_32(rcc_base + RCC_MP_GCR, RCC_MP_GCR_BOOT_MCU);
+	} else {
+		mmio_setbits_32(rcc_base + RCC_MP_GCR, RCC_MP_GCR_BOOT_MCU);
+	}
+}
+
+void __dead2 stm32mp_system_reset(void)
+{
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	mmio_setbits_32(rcc_base + RCC_MP_GRSTCSETR,
+			RCC_MP_GRSTCSETR_MPSYSRST);
+
+	/* Loop in case system reset is not immediately caught */
+	for ( ; ; ) {
+		;
+	}
+}
diff --git a/drivers/st/rng/stm32_rng.c b/drivers/st/rng/stm32_rng.c
new file mode 100644
index 0000000000..461d50c814
--- /dev/null
+++ b/drivers/st/rng/stm32_rng.c
@@ -0,0 +1,193 @@
+/*
+ * Copyright (c) 2018-2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#include <arch_helpers.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_rng.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
+
+#define DT_RNG_COMPAT		"st,stm32-rng"
+#define RNG_CR			0x00U
+#define RNG_SR			0x04U
+#define RNG_DR			0x08U
+
+#define RNG_CR_RNGEN		BIT(2)
+#define RNG_CR_IE		BIT(3)
+#define RNG_CR_CED		BIT(5)
+
+#define RNG_SR_DRDY		BIT(0)
+#define RNG_SR_CECS		BIT(1)
+#define RNG_SR_SECS		BIT(2)
+#define RNG_SR_CEIS		BIT(5)
+#define RNG_SR_SEIS		BIT(6)
+
+#define RNG_TIMEOUT_US		100000
+#define RNG_TIMEOUT_STEP_US	10
+
+#define TIMEOUT_US_1MS		U(1000)
+
+struct stm32_rng_instance {
+	uintptr_t base;
+	unsigned long clock;
+};
+
+static struct stm32_rng_instance stm32_rng;
+
+/*
+ * stm32_rng_read - Read a number of random bytes from RNG
+ * out: pointer to the output buffer
+ * size: number of bytes to be read
+ * Return 0 on success, non-0 on failure
+ */
+int stm32_rng_read(uint8_t *out, uint32_t size)
+{
+	uint8_t *buf = out;
+	uint32_t len = size;
+	int nb_tries;
+	uint32_t data32;
+	int rc = 0;
+	int count;
+
+	if (stm32_rng.base == 0U) {
+		return -EPERM;
+	}
+
+	clk_enable(stm32_rng.clock);
+
+	if ((mmio_read_32(stm32_rng.base + RNG_CR) & RNG_CR_RNGEN) == 0U) {
+		mmio_write_32(stm32_rng.base + RNG_CR,
+			      RNG_CR_RNGEN | RNG_CR_CED);
+	}
+
+	while (len != 0U) {
+		nb_tries = RNG_TIMEOUT_US / RNG_TIMEOUT_STEP_US;
+		do {
+			uint32_t status = mmio_read_32(stm32_rng.base + RNG_SR);
+
+			if ((status & (RNG_SR_SECS | RNG_SR_SEIS)) != 0U) {
+				uint8_t i;
+
+				/* Recommended by the SoC reference manual */
+				mmio_clrbits_32(stm32_rng.base + RNG_SR,
+						RNG_SR_SEIS);
+				dmb();
+				for (i = 12; i != 0; i--) {
+					(void)mmio_read_32(stm32_rng.base +
+							   RNG_DR);
+				}
+				dmb();
+
+				if ((mmio_read_32(stm32_rng.base + RNG_SR) &
+				     RNG_SR_SEIS) != 0U) {
+					ERROR("RNG noise\n");
+					panic();
+				}
+			}
+
+			udelay(RNG_TIMEOUT_STEP_US);
+			nb_tries--;
+			if (nb_tries == 0) {
+				rc = -ETIMEDOUT;
+				goto bail;
+			}
+		} while ((mmio_read_32(stm32_rng.base + RNG_SR) &
+			  RNG_SR_DRDY) == 0U);
+
+		count = 4;
+		while (len != 0U) {
+			data32 = mmio_read_32(stm32_rng.base + RNG_DR);
+			count--;
+
+			memcpy(buf, &data32, MIN(len, sizeof(uint32_t)));
+			buf += MIN(len, sizeof(uint32_t));
+			len -= MIN(len, sizeof(uint32_t));
+
+			if (count == 0) {
+				break;
+			}
+		}
+	}
+
+bail:
+	clk_disable(stm32_rng.clock);
+
+	if (rc != 0) {
+		memset(out, 0, buf - out);
+	}
+
+	return rc;
+}
+
+/*
+ * stm32_rng_init: Initialize rng from DT
+ * return 0 on success, negative value on failure
+ */
+int stm32_rng_init(void)
+{
+	void *fdt;
+	struct dt_node_info dt_rng;
+	int node;
+
+	if (fdt_get_address(&fdt) == 0) {
+		panic();
+	}
+
+	node = dt_get_node(&dt_rng, -1, DT_RNG_COMPAT);
+	if (node < 0) {
+		return 0;
+	}
+
+	if ((dt_rng.status & DT_SECURE) == 0) {
+		return 0;
+	}
+
+	assert(dt_rng.base != 0U);
+
+	stm32_rng.base = dt_rng.base;
+
+	if ((dt_rng.status & DT_NON_SECURE) == DT_NON_SECURE) {
+		stm32mp_register_non_secure_periph_iomem(stm32_rng.base);
+	} else {
+		stm32mp_register_secure_periph_iomem(stm32_rng.base);
+	}
+
+	if (dt_rng.clock < 0) {
+		panic();
+	}
+	stm32_rng.clock = (unsigned long)dt_rng.clock;
+
+	if (dt_rng.reset >= 0) {
+		int ret;
+
+		ret = stm32mp_reset_assert((unsigned long)dt_rng.reset,
+					   TIMEOUT_US_1MS);
+		if (ret != 0) {
+			panic();
+		}
+
+		udelay(20);
+
+		ret = stm32mp_reset_deassert((unsigned long)dt_rng.reset,
+					     TIMEOUT_US_1MS);
+		if (ret != 0) {
+			panic();
+		}
+	}
+
+	VERBOSE("Init RNG done\n");
+
+	return 0;
+}
diff --git a/drivers/st/rtc/stm32_rtc.c b/drivers/st/rtc/stm32_rtc.c
new file mode 100644
index 0000000000..58d599b17a
--- /dev/null
+++ b/drivers/st/rtc/stm32_rtc.c
@@ -0,0 +1,480 @@
+/*
+ * Copyright (c) 2018-2019, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <platform_def.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/clk.h>
+#include <drivers/st/stm32_rtc.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <lib/mmio.h>
+#include <lib/spinlock.h>
+
+#define RTC_COMPAT		"st,stm32mp1-rtc"
+
+#define RTC_TR_SU_MASK		GENMASK(3, 0)
+#define RTC_TR_ST_MASK		GENMASK(6, 4)
+#define RTC_TR_ST_SHIFT		4
+#define RTC_TR_MNU_MASK		GENMASK(11, 8)
+#define RTC_TR_MNU_SHIFT	8
+#define RTC_TR_MNT_MASK		GENMASK(14, 12)
+#define RTC_TR_MNT_SHIFT	12
+#define RTC_TR_HU_MASK		GENMASK(19, 16)
+#define RTC_TR_HU_SHIFT		16
+#define RTC_TR_HT_MASK		GENMASK(21, 20)
+#define RTC_TR_HT_SHIFT		20
+#define RTC_TR_PM		BIT(22)
+
+#define RTC_DR_DU_MASK		GENMASK(3, 0)
+#define RTC_DR_DT_MASK		GENMASK(5, 4)
+#define RTC_DR_DT_SHIFT		4
+#define RTC_DR_MU_MASK		GENMASK(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(15, 13)
+#define RTC_DR_WDU_SHIFT	13
+#define RTC_DR_YU_MASK		GENMASK(19, 16)
+#define RTC_DR_YU_SHIFT		16
+#define RTC_DR_YT_MASK		GENMASK(23, 20)
+#define RTC_DR_YT_SHIFT		20
+
+#define RTC_SSR_SS_MASK		GENMASK(15, 0)
+
+#define RTC_ICSR_ALRAWF		BIT(0)
+#define RTC_ICSR_RSF		BIT(5)
+
+#define RTC_PRER_PREDIV_S_MASK	GENMASK(14, 0)
+
+#define RTC_CR_BYPSHAD		BIT(5)
+#define RTC_CR_BYPSHAD_SHIFT	5
+#define RTC_CR_ALRAE		BIT(8)
+#define RTC_CR_ALRAIE		BIT(12)
+#define RTC_CR_TAMPTS		BIT(25)
+
+#define RTC_SMCR_TS_DPROT	BIT(3)
+
+#define RTC_TSDR_DU_MASK	GENMASK(3, 0)
+#define RTC_TSDR_DU_SHIFT	0
+#define RTC_TSDR_DT_MASK	GENMASK(5, 4)
+#define RTC_TSDR_DT_SHIFT	4
+#define RTC_TSDR_MU_MASK	GENMASK(11, 8)
+#define RTC_TSDR_MU_SHIFT	8
+
+#define RTC_ALRMAR_DU_SHIFT	24
+
+#define RTC_SR_TSF		BIT(3)
+#define RTC_SR_TSOVF		BIT(4)
+
+#define RTC_SCR_CTSF		BIT(3)
+#define RTC_SCR_CTSOVF		BIT(4)
+
+#define RTC_WPR_KEY1		0xCA
+#define RTC_WPR_KEY2		0x53
+#define RTC_WPR_KEY_LOCK	0xFF
+
+static struct dt_node_info rtc_dev;
+
+static struct spinlock lock;
+
+void stm32_rtc_regs_lock(void)
+{
+	if (stm32mp_lock_available()) {
+		spin_lock(&lock);
+	}
+}
+
+void stm32_rtc_regs_unlock(void)
+{
+	if (stm32mp_lock_available()) {
+		spin_unlock(&lock);
+	}
+}
+
+static void stm32_rtc_write_unprotect(void)
+{
+	mmio_write_32(rtc_dev.base + RTC_WPR, RTC_WPR_KEY1);
+	mmio_write_32(rtc_dev.base + RTC_WPR, RTC_WPR_KEY2);
+}
+
+static void stm32_rtc_write_protect(void)
+{
+	mmio_write_32(rtc_dev.base + RTC_WPR, RTC_WPR_KEY_LOCK);
+}
+
+/*******************************************************************************
+ * This function gets the BYPSHAD bit value of the RTC_CR register.
+ * It will determine if we need to reset RTC_ISCR.RSF after each RTC calendar
+ * read, and also wait for RTC_ISCR.RSF=1 before next read.
+ * Returns true or false depending on the bit value.
+ ******************************************************************************/
+static bool stm32_rtc_get_bypshad(void)
+{
+	return ((mmio_read_32(rtc_dev.base + RTC_CR) & RTC_CR_BYPSHAD) >>
+		RTC_CR_BYPSHAD_SHIFT) != 0U;
+}
+
+/*******************************************************************************
+ * This function reads the RTC calendar register values.
+ * If shadow registers are not bypassed, then a reset/poll is done.
+ ******************************************************************************/
+static void stm32_rtc_read_calendar(struct stm32_rtc_calendar *calendar)
+{
+	bool bypshad = stm32_rtc_get_bypshad();
+
+	if (!bypshad) {
+		mmio_clrbits_32((uint32_t)(rtc_dev.base + RTC_ICSR),
+				RTC_ICSR_RSF);
+		while ((mmio_read_32(rtc_dev.base + RTC_ICSR) & RTC_ICSR_RSF) !=
+		       RTC_ICSR_RSF) {
+			;
+		}
+	}
+
+	calendar->ssr = mmio_read_32(rtc_dev.base + RTC_SSR);
+	calendar->tr = mmio_read_32(rtc_dev.base + RTC_TR);
+	calendar->dr = mmio_read_32(rtc_dev.base + RTC_DR);
+}
+
+/*******************************************************************************
+ * This function fill the rtc_time structure based on rtc_calendar register.
+ ******************************************************************************/
+static void stm32_rtc_get_time(struct stm32_rtc_calendar *cal,
+			       struct stm32_rtc_time *tm)
+{
+	assert(cal != NULL);
+	assert(tm != NULL);
+
+	tm->hour = (((cal->tr & RTC_TR_HT_MASK) >> RTC_TR_HT_SHIFT) * 10U) +
+		((cal->tr & RTC_TR_HU_MASK) >> RTC_TR_HU_SHIFT);
+
+	if ((cal->tr & RTC_TR_PM) != 0U) {
+		tm->hour += 12U;
+	}
+
+	tm->min = (((cal->tr & RTC_TR_MNT_MASK) >> RTC_TR_MNT_SHIFT) * 10U) +
+		  ((cal->tr & RTC_TR_MNU_MASK) >> RTC_TR_MNU_SHIFT);
+	tm->sec = (((cal->tr & RTC_TR_ST_MASK) >> RTC_TR_ST_SHIFT) * 10U) +
+		  (cal->tr & RTC_TR_SU_MASK);
+}
+
+/*******************************************************************************
+ * This function fill the rtc_time structure with the given date register.
+ ******************************************************************************/
+static void stm32_rtc_get_date(struct stm32_rtc_calendar *cal,
+			       struct stm32_rtc_time *tm)
+{
+	assert(cal != NULL);
+	assert(tm != NULL);
+
+	tm->wday = (((cal->dr & RTC_DR_WDU_MASK) >> RTC_DR_WDU_SHIFT));
+
+	tm->day = (((cal->dr & RTC_DR_DT_MASK) >> RTC_DR_DT_SHIFT) * 10U) +
+		  (cal->dr & RTC_DR_DU_MASK);
+
+	tm->month = (((cal->dr & RTC_DR_MT) >> RTC_DR_MT_SHIFT) * 10U) +
+		    ((cal->dr & RTC_DR_MU_MASK) >> RTC_DR_MU_SHIFT);
+
+	tm->year = (((cal->dr & RTC_DR_YT_MASK) >> RTC_DR_YT_SHIFT) * 10U) +
+		   ((cal->dr & RTC_DR_YU_MASK) >> RTC_DR_YU_SHIFT) + 2000U;
+}
+
+/*******************************************************************************
+ * This function reads the RTC timestamp register values and update time
+ * structure with the corresponding value.
+ ******************************************************************************/
+static void stm32_rtc_read_timestamp(struct stm32_rtc_time *time)
+{
+	assert(time != NULL);
+
+	struct stm32_rtc_calendar cal_tamp;
+
+	cal_tamp.tr = mmio_read_32(rtc_dev.base + RTC_TSTR);
+	cal_tamp.dr = mmio_read_32(rtc_dev.base + RTC_TSDR);
+	stm32_rtc_get_time(&cal_tamp, time);
+	stm32_rtc_get_date(&cal_tamp, time);
+}
+
+/*******************************************************************************
+ * This function gets the RTC calendar register values.
+ * It takes into account the need of reading twice or not, depending on
+ * frequencies previously setted, and the bypass or not of the shadow
+ * registers. This service is exposed externally.
+ ******************************************************************************/
+void stm32_rtc_get_calendar(struct stm32_rtc_calendar *calendar)
+{
+	bool read_twice = stm32mp1_rtc_get_read_twice();
+
+	stm32_rtc_regs_lock();
+	clk_enable(rtc_dev.clock);
+
+	stm32_rtc_read_calendar(calendar);
+
+	if (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);
+	stm32_rtc_regs_unlock();
+}
+
+/*******************************************************************************
+ * This function computes the second fraction in milliseconds.
+ * The returned value is a uint32_t between 0 and 1000.
+ ******************************************************************************/
+static uint32_t stm32_rtc_get_second_fraction(struct stm32_rtc_calendar *cal)
+{
+	uint32_t prediv_s = mmio_read_32(rtc_dev.base + RTC_PRER) &
+			    RTC_PRER_PREDIV_S_MASK;
+	uint32_t ss = cal->ssr & RTC_SSR_SS_MASK;
+
+	return ((prediv_s - ss) * 1000U) / (prediv_s + 1U);
+}
+
+/*******************************************************************************
+ * This function computes the fraction difference between two timestamps.
+ * Here again the returned value is in milliseconds.
+ ******************************************************************************/
+static 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);
+}
+
+/*******************************************************************************
+ * This function computes the time difference between two timestamps.
+ * It includes seconds, minutes and hours.
+ * Here again the returned value is in milliseconds.
+ ******************************************************************************/
+static signed long long stm32_rtc_diff_time(struct stm32_rtc_time *current,
+					    struct stm32_rtc_time *ref)
+{
+	signed long long curr_s;
+	signed long long ref_s;
+
+	curr_s = (signed long long)current->sec +
+		 (((signed long long)current->min +
+		  (((signed long long)current->hour * 60))) * 60);
+
+	ref_s = (signed long long)ref->sec +
+		(((signed long long)ref->min +
+		 (((signed long long)ref->hour * 60))) * 60);
+
+	return (curr_s - ref_s) * 1000;
+}
+
+/*******************************************************************************
+ * This function determines if the year is leap or not.
+ * Returned value is true or false.
+ ******************************************************************************/
+static bool stm32_is_a_leap_year(uint32_t year)
+{
+	return ((year % 4U) == 0U) &&
+	       (((year % 100U) != 0U) || ((year % 400U) == 0U));
+}
+
+/*******************************************************************************
+ * This function computes the date difference between two timestamps.
+ * It includes days, months, years, with exceptions.
+ * Here again the returned value is in milliseconds.
+ ******************************************************************************/
+static 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;
+	static const uint8_t month_len[NB_MONTHS] = {
+		31, 28, 31, 30, 31, 30,
+		31, 31, 30, 31, 30, 31
+	};
+
+	/* Get the number of non-entire month days */
+	if (current->day >= ref->day) {
+		diff_in_days += current->day - ref->day;
+	} else {
+		diff_in_days += (uint32_t)month_len[ref->month - 1U] -
+				ref->day + current->day;
+	}
+
+	/* Get the number of entire months, and compute the related days */
+	if (current->month > (ref->month + 1U)) {
+		for (m = (ref->month + 1U); (m < current->month) &&
+		     (m < 12U); m++) {
+			diff_in_days += (uint32_t)month_len[m - 1U];
+		}
+	}
+
+	if (current->month < (ref->month - 1U)) {
+		for (m = 1U; (m < current->month) && (m < 12U); m++) {
+			diff_in_days += (uint32_t)month_len[m - 1U];
+		}
+
+		for (m = (ref->month + 1U); m < 12U; m++) {
+			diff_in_days += (uint32_t)month_len[m - 1U];
+		}
+	}
+
+	/* Get complete years */
+	if (current->year > (ref->year + 1U)) {
+		diff_in_days += (current->year - ref->year - 1U) * 365U;
+	}
+
+	/* Particular cases: leap years (one day more) */
+	if (diff_in_days > 0U) {
+		if (current->year == ref->year) {
+			if (stm32_is_a_leap_year(current->year)) {
+				if ((ref->month <= 2U) &&
+				    (current->month >= 3U) &&
+				    (current->day <= 28U)) {
+					diff_in_days++;
+				}
+			}
+		} else {
+			uint32_t y;
+
+			/* Ref year is leap */
+			if ((stm32_is_a_leap_year(ref->year)) &&
+			    (ref->month <= 2U) && (ref->day <= 28U)) {
+				diff_in_days++;
+			}
+
+			/* Current year is leap */
+			if ((stm32_is_a_leap_year(current->year)) &&
+			    (current->month >= 3U)) {
+				diff_in_days++;
+			}
+
+			/* Interleaved years are leap */
+			for (y = ref->year + 1U; y < current->year; y++) {
+				if (stm32_is_a_leap_year(y)) {
+					diff_in_days++;
+				}
+			}
+		}
+	}
+
+	return (24 * 60 * 60 * 1000) * (signed long long)diff_in_days;
+}
+
+/*******************************************************************************
+ * This function computes the date difference between two rtc value.
+ * Here again the returned value is in milliseconds.
+ ******************************************************************************/
+unsigned long long stm32_rtc_diff_calendar(struct stm32_rtc_calendar *cur,
+					   struct stm32_rtc_calendar *ref)
+{
+	signed long long diff_in_ms = 0;
+	struct stm32_rtc_time curr_t;
+	struct stm32_rtc_time ref_t;
+
+	clk_enable(rtc_dev.clock);
+
+	stm32_rtc_get_date(cur, &curr_t);
+	stm32_rtc_get_date(ref, &ref_t);
+	stm32_rtc_get_time(cur, &curr_t);
+	stm32_rtc_get_time(ref, &ref_t);
+
+	diff_in_ms += stm32_rtc_diff_frac(cur, ref);
+	diff_in_ms += stm32_rtc_diff_time(&curr_t, &ref_t);
+	diff_in_ms += stm32_rtc_diff_date(&curr_t, &ref_t);
+
+	clk_disable(rtc_dev.clock);
+
+	return (unsigned long long)diff_in_ms;
+}
+
+/*******************************************************************************
+ * This function fill the RTC timestamp structure.
+ ******************************************************************************/
+void stm32_rtc_get_timestamp(struct stm32_rtc_time *tamp_ts)
+{
+	stm32_rtc_regs_lock();
+	clk_enable(rtc_dev.clock);
+
+	if ((mmio_read_32(rtc_dev.base + RTC_SR) & RTC_SR_TSF) != 0U) {
+		/* Print timestamp for tamper event */
+		stm32_rtc_read_timestamp(tamp_ts);
+		mmio_setbits_32(rtc_dev.base + RTC_SCR, RTC_SCR_CTSF);
+		if ((mmio_read_32(rtc_dev.base + RTC_SR) & RTC_SR_TSOVF) !=
+		    0U) {
+			/* Overflow detected */
+			mmio_setbits_32(rtc_dev.base + RTC_SCR, RTC_SCR_CTSOVF);
+		}
+	}
+
+	clk_disable(rtc_dev.clock);
+	stm32_rtc_regs_unlock();
+}
+
+/*******************************************************************************
+ * This function enable the timestamp bit for tamper and secure timestamp
+ * access.
+ ******************************************************************************/
+void stm32_rtc_set_tamper_timestamp(void)
+{
+	stm32_rtc_regs_lock();
+	clk_enable(rtc_dev.clock);
+
+	stm32_rtc_write_unprotect();
+
+	/* Enable tamper timestamper */
+	mmio_setbits_32(rtc_dev.base + RTC_CR, RTC_CR_TAMPTS);
+
+	/* Secure Timestamp bit */
+	mmio_clrbits_32(rtc_dev.base + RTC_SMCR, RTC_SMCR_TS_DPROT);
+
+	stm32_rtc_write_protect();
+
+	clk_disable(rtc_dev.clock);
+	stm32_rtc_regs_unlock();
+}
+
+/*******************************************************************************
+ * This function return state of tamper timestamp.
+ ******************************************************************************/
+bool stm32_rtc_is_timestamp_enable(void)
+{
+	bool ret;
+
+	clk_enable(rtc_dev.clock);
+
+	ret = (mmio_read_32(rtc_dev.base + RTC_CR) & RTC_CR_TAMPTS) != 0U;
+
+	clk_disable(rtc_dev.clock);
+
+	return ret;
+}
+
+/*******************************************************************************
+ * RTC initialisation function.
+ ******************************************************************************/
+int stm32_rtc_init(void)
+{
+	int node;
+
+	node = dt_get_node(&rtc_dev, -1, RTC_COMPAT);
+	if (node < 0) {
+		return node;
+	}
+
+	if (rtc_dev.status == DT_SECURE) {
+		stm32mp_register_secure_periph_iomem(rtc_dev.base);
+	} else {
+		stm32mp_register_non_secure_periph_iomem(rtc_dev.base);
+	}
+
+	return 0;
+}
diff --git a/drivers/st/spi/stm32_qspi.c b/drivers/st/spi/stm32_qspi.c
index d67f8313f3..966716d68f 100644
--- a/drivers/st/spi/stm32_qspi.c
+++ b/drivers/st/spi/stm32_qspi.c
@@ -10,6 +10,7 @@
 
 #include <common/debug.h>
 #include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/spi_mem.h>
 #include <drivers/st/stm32_gpio.h>
@@ -363,7 +364,7 @@ static void stm32_qspi_release_bus(void)
 
 static int stm32_qspi_set_speed(unsigned int hz)
 {
-	unsigned long qspi_clk = stm32mp_clk_get_rate(stm32_qspi.clock_id);
+	unsigned long qspi_clk = clk_get_rate(stm32_qspi.clock_id);
 	uint32_t prescaler = UINT8_MAX;
 	uint32_t csht;
 	int ret;
@@ -493,7 +494,7 @@ int stm32_qspi_init(void)
 	stm32_qspi.clock_id = (unsigned long)info.clock;
 	stm32_qspi.reset_id = (unsigned int)info.reset;
 
-	stm32mp_clk_enable(stm32_qspi.clock_id);
+	clk_enable(stm32_qspi.clock_id);
 
 	ret = stm32mp_reset_assert(stm32_qspi.reset_id, TIMEOUT_US_1_MS);
 	if (ret != 0) {
diff --git a/drivers/st/tamper/stm32_tamp.c b/drivers/st/tamper/stm32_tamp.c
new file mode 100644
index 0000000000..c9a9e9aef8
--- /dev/null
+++ b/drivers/st/tamper/stm32_tamp.c
@@ -0,0 +1,379 @@
+/*
+ * Copyright (c) 2018-2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#include <drivers/clk.h>
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32_rng.h>
+#include <drivers/st/stm32_rtc.h>
+#include <drivers/st/stm32_tamp.h>
+#include <lib/mmio.h>
+
+#define DT_TAMP_COMPAT			"st,stm32-tamp"
+/* STM32 Registers */
+#define STM32_TAMP_CR1			0x00U
+#define STM32_TAMP_CR2			0x04U
+#define STM32_TAMP_FLTCR		0x0CU
+#define STM32_TAMP_ATCR			0x10U
+#define STM32_TAMP_ATSEEDR		0x14U
+#define STM32_TAMP_ATOR			0x18U
+#define STM32_TAMP_SMCR			0x20U
+#define STM32_TAMP_IER			0x2CU
+#define STM32_TAMP_SR			0x30U
+#define STM32_TAMP_SCR			0x3CU
+#define STM32_TAMP_COUNTR		0x40U
+#define STM32_TAMP_OR			0x50U
+#define STM32_TAMP_HWCFGR2		0x3ECU
+#define STM32_TAMP_HWCFGR1		0x3F0U
+#define STM32_TAMP_VERR			0x3F4U
+#define STM32_TAMP_IPIDR		0x3F8U
+#define STM32_TAMP_SIDR			0x3FCU
+
+/* STM32_TAMP_FLTCR bit fields */
+#define STM32_TAMP_FLTCR_TAMPFREQ	GENMASK(2, 0)
+#define STM32_TAMP_FLTCR_TAMPFLT	GENMASK(4, 3)
+#define STM32_TAMP_FLTCR_TAMPPRCH	GENMASK(6, 5)
+#define STM32_TAMP_FLTCR_TAMPPUDIS	BIT(7)
+
+/* STM32_TAMP_ATCR bit fields */
+#define STM32_TAMP_ATCR_ATCKSEL		GENMASK(18, 16)
+#define STM32_TAMP_ATCR_ATPER		GENMASK(26, 24)
+#define STM32_TAMP_ATCR_ATOSHARE	BIT(30)
+#define STM32_TAMP_ATCR_FLTEN		BIT(31)
+
+/* STM32_TAMP_ATOR bit fields */
+#define STM32_TAMP_PRNG			GENMASK(7, 0)
+#define STM32_TAMP_SEEDF		BIT(14)
+#define STM32_TAMP_INITS		BIT(15)
+
+/* STM32_TAMP_IER bit fields */
+#define STM32_TAMP_IER_TAMPXIE_ALL	GENMASK(7, 0)
+#define STM32_TAMP_IER_ITAMPXIE_ALL	GENMASK(31, 16)
+
+/* STM32_TAMP_SR bit fields */
+#define STM32_TAMP_SR_TAMPXF_MASK	GENMASK(7, 0)
+#define STM32_TAMP_SR_ITAMPXF_MASK	GENMASK(31, 16)
+
+/* STM32_TAMP_SMCR but fields */
+#define STM32_TAMP_SMCR_DPROT		BIT(31)
+
+/* STM32_TAMP_CFGR bit fields */
+#define STM32_TAMP_OR_OUT3RMP		BIT(0)
+
+/* STM32_TAMP_HWCFGR2 bit fields */
+#define STM32_TAMP_HWCFGR2_TZ		GENMASK(11, 8)
+#define STM32_TAMP_HWCFGR2_OR		GENMASK(7, 0)
+
+/* STM32_TAMP_HWCFGR1 bit fields */
+#define STM32_TAMP_HWCFGR1_BKPREG	GENMASK(7, 0)
+#define STM32_TAMP_HWCFGR1_TAMPER	GENMASK(11, 8)
+#define STM32_TAMP_HWCFGR1_ACTIVE	GENMASK(15, 12)
+#define STM32_TAMP_HWCFGR1_INTERN	GENMASK(31, 16)
+
+/* STM32_TAMP_VERR bit fields */
+#define STM32_TAMP_VERR_MINREV		GENMASK(3, 0)
+#define STM32_TAMP_VERR_MAJREV		GENMASK(7, 4)
+
+#define STM32_TAMP_MAX_INTERNAL		16U
+#define STM32_TAMP_MAX_EXTERNAL		8U
+
+struct stm32_tamp_instance {
+	uintptr_t base;
+	uint32_t clock;
+	uint32_t hwconf1;
+	uint32_t hwconf2;
+	uint16_t int_nbtamp;
+	uint8_t ext_nbtamp;
+	struct stm32_tamp_int *int_tamp;
+	struct stm32_tamp_ext *ext_tamp;
+};
+
+static struct stm32_tamp_instance stm32_tamp;
+
+static void stm32_tamp_set_secured(unsigned long base)
+{
+	mmio_clrbits_32(base + STM32_TAMP_SMCR, STM32_TAMP_SMCR_DPROT);
+}
+
+static void stm32_tamp_configure_or(unsigned long base, uint32_t out3)
+{
+	mmio_setbits_32(base + STM32_TAMP_OR, out3);
+}
+
+static int stm32_tamp_seed_init(unsigned long base)
+{
+	/*  Need RNG access */
+	uint32_t timeout = 100;
+	uint8_t idx;
+
+	for (idx = 0; idx < 4U; idx++) {
+		uint32_t rnd;
+
+		if (stm32_rng_read((uint8_t *)&rnd, sizeof(uint32_t)) != 0) {
+			return -1;
+		}
+
+		VERBOSE("Seed init %u\n", rnd);
+		mmio_write_32(base + STM32_TAMP_ATSEEDR, rnd);
+	}
+
+	while (((mmio_read_32(base + STM32_TAMP_ATOR) &
+		 STM32_TAMP_SEEDF) != 0U) &&
+	       (timeout != 0U)) {
+		timeout--;
+	}
+
+	if (timeout == 0U) {
+		return -1;
+	}
+
+	return 0;
+}
+
+static void stm32_tamp_reset_register(unsigned long base)
+{
+	/* Disable all internal tamper */
+	mmio_write_32(base + STM32_TAMP_CR1, 0U);
+
+	/* Disable all external tamper */
+	mmio_write_32(base + STM32_TAMP_CR2, 0U);
+
+	/* Clean configuration registers */
+	mmio_write_32(base + STM32_TAMP_FLTCR, 0U);
+	mmio_write_32(base + STM32_TAMP_ATCR, 0U);
+	mmio_clrbits_32(base + STM32_TAMP_SMCR, STM32_TAMP_SMCR_DPROT);
+
+	/* Clean Tamper IT */
+	mmio_write_32(base + STM32_TAMP_IER, 0U);
+	mmio_write_32(base + STM32_TAMP_SCR, ~0U);
+
+	mmio_clrbits_32(base + STM32_TAMP_OR, STM32_TAMP_OR_OUT3RMP);
+}
+
+void stm32_tamp_write_mcounter(void)
+{
+	mmio_write_32(stm32_tamp.base + STM32_TAMP_COUNTR, 1U);
+}
+
+void stm32_tamp_configure_internal(struct stm32_tamp_int *tamper_list,
+				   uint16_t nb_tamper)
+{
+	uint16_t i;
+
+	assert(nb_tamper < STM32_TAMP_MAX_INTERNAL);
+
+	for (i = 0; i < nb_tamper; i++) {
+		int id = tamper_list[i].id;
+		uint32_t u_id;
+
+		if (id == -1) {
+			continue;
+		}
+
+		u_id = (uint32_t)id;
+
+		if ((stm32_tamp.hwconf1 & BIT(u_id + 16U)) != 0U) {
+			mmio_setbits_32(stm32_tamp.base + STM32_TAMP_CR1,
+					BIT(u_id + 16U));
+			mmio_setbits_32(stm32_tamp.base + STM32_TAMP_IER,
+					BIT(u_id + 16U));
+		}
+	}
+
+	stm32_tamp.int_tamp = tamper_list;
+	stm32_tamp.int_nbtamp = nb_tamper;
+}
+
+void stm32_tamp_configure_external(struct stm32_tamp_ext *ext_tamper_list,
+				   uint8_t nb_tamper, uint32_t passive_conf,
+				   uint32_t active_conf)
+{
+	/* External configuration */
+	uint8_t i, active_tamp = 0;
+
+	assert(nb_tamper < STM32_TAMP_MAX_EXTERNAL);
+
+	/*  Enable external Tamp */
+	for (i = 0; i < nb_tamper; i++) {
+		int id = ext_tamper_list[i].id;
+		uint32_t reg = 0, u_id;
+
+		if (id == -1) {
+			continue;
+		}
+
+		u_id = (uint32_t)id;
+
+		mmio_setbits_32(stm32_tamp.base + STM32_TAMP_CR1,
+				BIT(u_id));
+
+		if (ext_tamper_list[i].mode == TAMP_TRIG_ON) {
+			reg |= BIT(u_id + 24U);
+		}
+
+		if (ext_tamper_list[i].mode == TAMP_ACTIVE) {
+			active_tamp |= BIT(u_id);
+		}
+
+		if (ext_tamper_list[i].erase != 0U) {
+			reg |= BIT(u_id);
+		}
+
+		if (ext_tamper_list[i].evt_mask != 0U) {
+			reg |= BIT(u_id + 16U);
+		} else {
+			mmio_setbits_32(stm32_tamp.base + STM32_TAMP_IER,
+					BIT(u_id));
+		}
+
+		mmio_setbits_32(stm32_tamp.base + STM32_TAMP_CR2, reg);
+	}
+
+	/*  Filter mode register set */
+	mmio_write_32(stm32_tamp.base + STM32_TAMP_FLTCR, passive_conf);
+
+	/*  Active mode configuration */
+	if (active_tamp != 0U) {
+		mmio_write_32(stm32_tamp.base + STM32_TAMP_ATCR,
+			      active_conf | active_tamp);
+		if (stm32_tamp_seed_init(stm32_tamp.base) != 0) {
+			ERROR("Active tamper: SEED not initialized\n");
+			panic();
+		}
+	}
+
+	stm32_tamp.ext_tamp = ext_tamper_list;
+	stm32_tamp.ext_nbtamp = nb_tamper;
+}
+
+void stm32_tamp_it_handler(void)
+{
+	uint32_t it = mmio_read_32(stm32_tamp.base + STM32_TAMP_SR);
+	uint8_t tamp = 0;
+	struct stm32_rtc_time tamp_ts;
+	struct stm32_tamp_int *int_list = stm32_tamp.int_tamp;
+	struct stm32_tamp_ext *ext_list = stm32_tamp.ext_tamp;
+
+	if (stm32_rtc_is_timestamp_enable()) {
+		stm32_rtc_get_timestamp(&tamp_ts);
+		INFO("Tamper Event Occurred\n");
+		INFO("Date : %u/%u\n \t Time : %u:%u:%u\n",
+		     tamp_ts.day, tamp_ts.month, tamp_ts.hour,
+		     tamp_ts.min, tamp_ts.sec);
+	}
+
+	/*  Internal tamper interrupt */
+	if ((it & STM32_TAMP_IER_ITAMPXIE_ALL) == 0U) {
+		goto tamp_ext;
+	}
+
+	while ((it != 0U) && (tamp < stm32_tamp.int_nbtamp)) {
+		uint32_t int_id = (uint32_t)int_list[tamp].id;
+
+		if ((it & BIT(int_id + 16U)) != 0U) {
+			mmio_setbits_32(stm32_tamp.base + STM32_TAMP_SCR, BIT(int_id + 16U));
+			it &= ~BIT(int_id + 16U);
+			if (int_list[tamp].func != NULL) {
+				int_list[tamp].func(int_id);
+			}
+		}
+		tamp++;
+	}
+
+tamp_ext:
+	tamp = 0;
+	/* External tamper interrupt */
+	if ((it == 0U) || ((it & STM32_TAMP_IER_TAMPXIE_ALL) == 0U)) {
+		return;
+	}
+
+	while ((it != 0U) && (tamp < stm32_tamp.ext_nbtamp)) {
+		uint32_t ext_id = (uint32_t)ext_list[tamp].id;
+
+		if ((it & BIT(ext_id)) != 0U) {
+			if (ext_list[tamp].func != NULL) {
+				ext_list[tamp].func(ext_id);
+			}
+
+			mmio_setbits_32(stm32_tamp.base + STM32_TAMP_SCR,
+					BIT(ext_id));
+			it &= ~BIT(ext_id);
+		}
+		tamp++;
+	}
+}
+
+int stm32_tamp_init(void)
+{
+	int node;
+	struct dt_node_info dt_tamp;
+	void *fdt;
+	uint32_t rev __unused;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -EPERM;
+	}
+
+	node = dt_get_node(&dt_tamp, -1, DT_TAMP_COMPAT);
+	if (node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	assert(dt_tamp.base != 0U);
+	assert(dt_tamp.clock != -1);
+
+	stm32_tamp.base = dt_tamp.base;
+	stm32_tamp.clock = (uint32_t)dt_tamp.clock;
+
+	/* Init Tamp clock */
+	clk_enable(stm32_tamp.clock);
+
+	/* Reset Tamp register without modifying backup registers conf */
+	stm32_tamp_reset_register(stm32_tamp.base);
+
+	/* Check if TAMP is enabled */
+	if ((dt_tamp.status != DT_SECURE) &&
+	    (dt_tamp.status != DT_SHARED)) {
+		return 0;
+	}
+
+	stm32_tamp.hwconf1 = mmio_read_32(stm32_tamp.base + STM32_TAMP_HWCFGR1);
+	stm32_tamp.hwconf2 = mmio_read_32(stm32_tamp.base + STM32_TAMP_HWCFGR2);
+
+	rev = mmio_read_32(stm32_tamp.base + STM32_TAMP_VERR);
+	VERBOSE("STM32 TAMPER V%u.%u\n", (rev & STM32_TAMP_VERR_MAJREV) >> 4,
+		rev & STM32_TAMP_VERR_MINREV);
+
+	if ((stm32_tamp.hwconf2 & STM32_TAMP_HWCFGR2_TZ) == 0U) {
+		ERROR("Tamper IP doesn't support trustzone");
+		return -EPERM;
+	}
+
+	stm32_tamp_set_secured(stm32_tamp.base);
+
+	if (dt_set_pinctrl_config(node) != -FDT_ERR_NOTFOUND) {
+		if (fdt_getprop(fdt, node, "st,out3-pc13", NULL) != NULL) {
+			stm32_tamp_configure_or(stm32_tamp.base, 1);
+		}
+	}
+
+	if (stm32_gic_enable_spi(node, NULL) < 0) {
+		panic();
+	}
+
+	if (fdt_getprop(fdt, node, "wakeup-source", NULL) != NULL) {
+		mmio_setbits_32(EXTI_BASE + EXTI_TZENR1, EXTI_TZENR1_TZEN18);
+		mmio_setbits_32(EXTI_BASE + EXTI_C1IMR1, EXTI_IMR1_IM18);
+	}
+
+	return 1;
+}
diff --git a/drivers/st/timer/stm32_timer.c b/drivers/st/timer/stm32_timer.c
new file mode 100644
index 0000000000..1899707f76
--- /dev/null
+++ b/drivers/st/timer/stm32_timer.c
@@ -0,0 +1,323 @@
+/*
+ * Copyright (c) 2018-2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_timer.h>
+#include <lib/mmio.h>
+
+#define TIM_CR1			0x00U		/* Control Register 1      */
+#define TIM_CR2			0x04U		/* Control Register 2      */
+#define TIM_SMCR		0x08U		/* Slave mode control reg  */
+#define TIM_DIER		0x0CU		/* DMA/interrupt register  */
+#define TIM_SR			0x10U		/* Status register	   */
+#define TIM_EGR			0x14U		/* Event Generation Reg    */
+#define TIM_CCMR1		0x18U		/* Capt/Comp 1 Mode Reg    */
+#define TIM_CCMR2		0x1CU		/* Capt/Comp 2 Mode Reg    */
+#define TIM_CCER		0x20U		/* Capt/Comp Enable Reg    */
+#define TIM_CNT			0x24U		/* Counter		   */
+#define TIM_PSC			0x28U		/* Prescaler               */
+#define TIM_ARR			0x2CU		/* Auto-Reload Register    */
+#define TIM_CCR1		0x34U		/* Capt/Comp Register 1    */
+#define TIM_CCR2		0x38U		/* Capt/Comp Register 2    */
+#define TIM_CCR3		0x3CU		/* Capt/Comp Register 3    */
+#define TIM_CCR4		0x40U		/* Capt/Comp Register 4    */
+#define TIM_BDTR		0x44U		/* Break and Dead-Time Reg */
+#define TIM_DCR			0x48U		/* DMA control register    */
+#define TIM_DMAR		0x4CU		/* DMA transfer register   */
+#define TIM_AF1			0x60U		/* Alt Function Reg 1      */
+#define TIM_AF2			0x64U		/* Alt Function Reg 2      */
+#define TIM_TISEL		0x68U		/* Input Selection         */
+
+#define TIM_CR1_CEN		BIT(0)
+#define TIM_SMCR_SMS		GENMASK(2, 0)	/* Slave mode selection */
+#define TIM_SMCR_TS		GENMASK(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(3, 0)
+#define TIM_SMCR_SMS_RESET	0x4U
+#define TIM_SMCR_TS_SHIFT	4U
+#define TIM_SMCR_TS_TI1FP1	0x5U
+
+#define TIM_COMPAT		"st,stm32-timers"
+#define TIM_TIMEOUT_US		100000
+#define TIM_TIMEOUT_STEP_US	10
+#define TIM_PRESCAL_HSI		10U
+#define TIM_PRESCAL_CSI		7U
+#define TIM_MIN_FREQ_CALIB	50000000U
+#define TIM_THRESHOLD		1U
+
+struct stm32_timer_instance {
+	uintptr_t base;
+	unsigned long clk;
+	unsigned long freq;
+	uint8_t cal_input;
+};
+
+static struct stm32_timer_instance stm32_timer[TIM_MAX_INSTANCE];
+
+static int stm32_timer_get_dt_node(struct dt_node_info *info, int offset)
+{
+	int node;
+
+	node = dt_get_node(info, offset, TIM_COMPAT);
+	if (node < 0) {
+		if (offset == -1) {
+			WARN("%s: No TIMER found\n", __func__);
+		}
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return node;
+}
+
+static int stm32_timer_config(struct stm32_timer_instance *timer)
+{
+	clk_enable(timer->clk);
+
+	timer->freq = clk_get_rate(timer->clk);
+
+	if (timer->freq < TIM_MIN_FREQ_CALIB) {
+		WARN("Timer is not accurate enough for calibration\n");
+		clk_disable(timer->clk);
+		return -EINVAL;
+	}
+
+	if ((mmio_read_32(timer->base + TIM_TISEL) & TIM_TISEL_TI1SEL_MASK) !=
+	    timer->cal_input) {
+		mmio_clrsetbits_32(timer->base + TIM_CCMR1,
+				   TIM_CCMR_CC1S_TI1 | TIM_CCMR_CC1S_TI2,
+				   TIM_CCMR_CC1S_TI1);
+
+		mmio_clrbits_32(timer->base + TIM_CCER,
+				TIM_CCER_CC1P | TIM_CCER_CC1NP);
+
+		mmio_clrsetbits_32(timer->base + TIM_SMCR,
+				   TIM_SMCR_TS | TIM_SMCR_SMS,
+				   (TIM_SMCR_TS_TI1FP1 << TIM_SMCR_TS_SHIFT) |
+				   TIM_SMCR_SMS_RESET);
+
+		mmio_write_32(timer->base + TIM_TISEL, timer->cal_input);
+		mmio_setbits_32(timer->base + TIM_CR1, TIM_CR1_CEN);
+		mmio_setbits_32(timer->base + TIM_CCER, TIM_CCER_CC1E);
+	}
+
+	clk_disable(timer->clk);
+
+	return 0;
+}
+
+static uint32_t stm32_timer_start_capture(struct stm32_timer_instance *timer)
+{
+	uint32_t timeout = TIM_TIMEOUT_US / TIM_TIMEOUT_STEP_US;
+	uint32_t counter = 0U;
+	uint32_t old_counter;
+	uint64_t conv_timeout;
+
+	if (stm32_timer_config(timer) < 0) {
+		return 0U;
+	}
+
+	clk_enable(timer->clk);
+
+	mmio_write_32(timer->base + TIM_SR, 0U);
+	while (((mmio_read_32(timer->base + TIM_SR) &
+		 TIM_SR_UIF) == 0U) && (timeout != 0U)) {
+		udelay(TIM_TIMEOUT_STEP_US);
+		timeout--;
+	}
+
+	if (timeout == 0U) {
+		goto out;
+	}
+
+	mmio_write_32(timer->base + TIM_SR, 0U);
+
+	conv_timeout = timeout_init_us(TIM_TIMEOUT_US);
+	do {
+		if (timeout_elapsed(conv_timeout)) {
+			WARN("Timer counter not stable\n");
+			timeout = 0U;
+			goto out;
+		}
+
+		timeout = TIM_TIMEOUT_US / TIM_TIMEOUT_STEP_US;
+		while (((mmio_read_32(timer->base + TIM_SR) &
+			 TIM_SR_CC1IF) == 0U) && (timeout != 0U)) {
+			udelay(TIM_TIMEOUT_STEP_US);
+			timeout--;
+		}
+
+		if (timeout == 0U) {
+			goto out;
+		}
+
+		old_counter = counter;
+		counter = mmio_read_32(timer->base + TIM_CCR1);
+	} while ((MAX(counter, old_counter) - MIN(counter, old_counter)) >
+		 TIM_THRESHOLD);
+
+out:
+	clk_disable(timer->clk);
+
+	if (timeout == 0U) {
+		return 0U;
+	}
+
+	return counter;
+}
+
+unsigned long stm32_timer_hsi_freq(void)
+{
+	struct stm32_timer_instance *timer = &stm32_timer[HSI_CAL];
+	unsigned long hsi_freq;
+	uint32_t counter;
+
+	if (timer->base == 0) {
+		return 0;
+	}
+
+	counter = stm32_timer_start_capture(timer);
+	VERBOSE("Counter value %i\n", counter);
+
+	if (counter == 0U) {
+		return 0;
+	}
+
+	hsi_freq = (timer->freq / counter) << TIM_PRESCAL_HSI;
+
+	return hsi_freq;
+}
+
+unsigned long stm32_timer_csi_freq(void)
+{
+	struct stm32_timer_instance *timer = &stm32_timer[CSI_CAL];
+	unsigned long csi_freq;
+	uint32_t counter;
+
+	if (timer->base == 0) {
+		return 0;
+	}
+
+	counter = stm32_timer_start_capture(timer);
+	VERBOSE("Counter value %i\n", counter);
+
+	if (counter == 0U) {
+		return 0;
+	}
+
+	csi_freq = (timer->freq / counter) << TIM_PRESCAL_CSI;
+
+	return csi_freq;
+}
+
+/*
+ * Get the timer frequence callback function for a target clock calibration
+ * @timer_freq_cb - Output callback function
+ * @type - Target clock calibration ID
+ */
+void stm32_timer_freq_func(unsigned long (**timer_freq_cb)(void),
+			   enum timer_cal type)
+{
+	switch (type) {
+	case HSI_CAL:
+		if (stm32_timer[HSI_CAL].base != 0) {
+			*timer_freq_cb = stm32_timer_hsi_freq;
+		}
+		break;
+
+	case CSI_CAL:
+		if (stm32_timer[CSI_CAL].base != 0) {
+			*timer_freq_cb = stm32_timer_csi_freq;
+		}
+		break;
+
+	default:
+		panic();
+	}
+}
+
+/*
+ * Initialize timer from DT
+ * return 0 if disabled, 1 if enabled, else < 0
+ */
+int stm32_timer_init(void)
+{
+	void *fdt;
+	struct dt_node_info dt_timer;
+	int node = -1;
+	uint8_t nb_timer = 0;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -EPERM;
+	}
+
+	for (node = stm32_timer_get_dt_node(&dt_timer, node);
+	     node != -FDT_ERR_NOTFOUND;
+	     node = stm32_timer_get_dt_node(&dt_timer, node)) {
+
+		if (dt_timer.status == DT_SECURE) {
+			struct stm32_timer_instance *timer;
+			const fdt32_t *cuint;
+
+			nb_timer++;
+
+			cuint = fdt_getprop(fdt, node, "st,hsi-cal-input",
+					    NULL);
+			if (cuint != NULL) {
+				timer = &stm32_timer[HSI_CAL];
+				timer->base = dt_timer.base;
+				timer->clk = dt_timer.clock;
+				timer->freq = clk_get_rate(timer->clk);
+				timer->cal_input =
+					(uint8_t)fdt32_to_cpu(*cuint);
+				if (stm32_timer_config(timer) < 0) {
+					timer->base = 0;
+					continue;
+				}
+			}
+
+			cuint = fdt_getprop(fdt, node, "st,csi-cal-input",
+					    NULL);
+			if (cuint != NULL) {
+				timer = &stm32_timer[CSI_CAL];
+				timer->base = dt_timer.base;
+				timer->clk = dt_timer.clock;
+				timer->freq = clk_get_rate(timer->clk);
+				timer->cal_input =
+					(uint8_t)fdt32_to_cpu(*cuint);
+				if (stm32_timer_config(timer) < 0) {
+					timer->base = 0;
+					continue;
+				}
+			}
+		}
+	}
+
+	VERBOSE("%u TIMER instance%s found\n", nb_timer,
+		nb_timer > 1 ? "s" : "");
+
+	if (nb_timer == 0U) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return 0;
+}
diff --git a/drivers/st/uart/aarch32/stm32_console.S b/drivers/st/uart/aarch32/stm32_console.S
index 686b18b969..2cb04a88fc 100644
--- a/drivers/st/uart/aarch32/stm32_console.S
+++ b/drivers/st/uart/aarch32/stm32_console.S
@@ -122,17 +122,15 @@ register_fail:
 	pop	{r4, pc}
 endfunc console_stm32_register
 
-	/* ---------------------------------------------------------------
+	/* --------------------------------------------------------
 	 * int console_core_putc(int c, uintptr_t base_addr)
-	 *
-	 * Function to output a character over the console. It returns the
-	 * character printed on success or -1 on error.
-	 *
+	 * Function to output a character over the console. It
+	 * returns the character printed on success or -1 on error.
 	 * In : r0 - character to be printed
 	 *      r1 - console base address
 	 * Out : return -1 on error else return character.
-	 * Clobber list : r2
-	 * ---------------------------------------------------------------
+	 * Clobber list : r2, r3
+	 * --------------------------------------------------------
 	 */
 func console_stm32_core_putc
 	/* Check the input parameter */
@@ -140,13 +138,19 @@ func console_stm32_core_putc
 	beq	putc_error
 
 	/* Check Transmit Data Register Empty */
+	mov	r3, #USART_TIMEOUT
 txe_loop:
+	subs	r3, r3, #1
+	beq	putc_error
 	ldr	r2, [r1, #USART_ISR]
 	tst	r2, #USART_ISR_TXE
 	beq	txe_loop
 	str	r0, [r1, #USART_TDR]
 	/* Check transmit complete flag */
+	mov	r3, #USART_TIMEOUT
 tc_loop:
+	subs	r3, r3, #1
+	beq	putc_error
 	ldr	r2, [r1, #USART_ISR]
 	tst	r2, #USART_ISR_TC
 	beq	tc_loop
@@ -200,7 +204,7 @@ endfunc console_stm32_core_getc
 	 *
 	 * In : r0 - console base address
 	 * Out : void.
-	 * Clobber list : r0, r1
+	 * Clobber list : r0, r1, r2
 	 * ---------------------------------------------------------------
 	 */
 func console_stm32_core_flush
@@ -209,7 +213,10 @@ func console_stm32_core_flush
 	ASM_ASSERT(ne)
 #endif /* ENABLE_ASSERTIONS */
 	/* Check Transmit Data Register Empty */
+	mov	r2, #USART_TIMEOUT
 txe_loop_3:
+	subs	r2, r2, #1
+	beq	plat_panic_handler
 	ldr	r1, [r0, #USART_ISR]
 	tst	r1, #USART_ISR_TXE
 	beq	txe_loop_3
diff --git a/drivers/st/uart/stm32_uart.c b/drivers/st/uart/stm32_uart.c
new file mode 100644
index 0000000000..a0d21dcf33
--- /dev/null
+++ b/drivers/st/uart/stm32_uart.c
@@ -0,0 +1,405 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <common/bl_common.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <drivers/st/stm32_uart.h>
+#include <drivers/st/stm32_uart_regs.h>
+#include <lib/mmio.h>
+
+/* UART time-out value */
+#define STM32_UART_TIMEOUT_US	20000U
+
+/* Mask to clear ALL the configuration register */
+
+#define STM32_UART_CR1_FIELDS \
+		(USART_CR1_M | USART_CR1_PCE | USART_CR1_PS | USART_CR1_TE | \
+		 USART_CR1_RE | USART_CR1_OVER8 | USART_CR1_FIFOEN)
+
+#define STM32_UART_CR2_FIELDS \
+		(USART_CR2_SLVEN | USART_CR2_DIS_NSS | USART_CR2_ADDM7 | \
+		 USART_CR2_LBDL | USART_CR2_LBDIE | USART_CR2_LBCL | \
+		 USART_CR2_CPHA | USART_CR2_CPOL | USART_CR2_CLKEN | \
+		 USART_CR2_STOP | USART_CR2_LINEN | USART_CR2_SWAP | \
+		 USART_CR2_RXINV | USART_CR2_TXINV | USART_CR2_DATAINV | \
+		 USART_CR2_MSBFIRST | USART_CR2_ABREN | USART_CR2_ABRMODE | \
+		 USART_CR2_RTOEN | USART_CR2_ADD)
+
+#define STM32_UART_CR3_FIELDS \
+		(USART_CR3_EIE | USART_CR3_IREN | USART_CR3_IRLP | \
+		 USART_CR3_HDSEL | USART_CR3_NACK | USART_CR3_SCEN | \
+		 USART_CR3_DMAR | USART_CR3_DMAT | USART_CR3_RTSE | \
+		 USART_CR3_CTSE | USART_CR3_CTSIE | USART_CR3_ONEBIT | \
+		 USART_CR3_OVRDIS | USART_CR3_DDRE | USART_CR3_DEM | \
+		 USART_CR3_DEP | USART_CR3_SCARCNT | USART_CR3_WUS | \
+		 USART_CR3_WUFIE | USART_CR3_TXFTIE | USART_CR3_TCBGTIE | \
+		 USART_CR3_RXFTCFG | USART_CR3_RXFTIE | USART_CR3_TXFTCFG)
+
+#define STM32_UART_ISR_ERRORS	 \
+		(USART_ISR_ORE | USART_ISR_NE |  USART_ISR_FE | USART_ISR_PE)
+
+static const uint16_t presc_table[STM32_UART_PRESCALER_MAX + 1] = {
+	1U, 2U, 4U, 6U, 8U, 10U, 12U, 16U, 32U, 64U, 128U, 256U
+};
+
+/* @brief  BRR division operation to set BRR register in 8-bit oversampling
+ * mode.
+ * @param  clockfreq: UART clock.
+ * @param  baud_rate: Baud rate set by the user.
+ * @param  prescaler: UART prescaler value.
+ * @retval Division result.
+ */
+static uint32_t uart_div_sampling8(unsigned long clockfreq,
+				   uint32_t baud_rate,
+				   uint32_t prescaler)
+{
+	uint32_t scaled_freq = clockfreq / presc_table[prescaler];
+
+	return ((scaled_freq * 2) + (baud_rate / 2)) / baud_rate;
+
+}
+
+/* @brief  BRR division operation to set BRR register in 16-bit oversampling
+ * mode.
+ * @param  clockfreq: UART clock.
+ * @param  baud_rate: Baud rate set by the user.
+ * @param  prescaler: UART prescaler value.
+ * @retval Division result.
+ */
+static uint32_t uart_div_sampling16(unsigned long clockfreq,
+				    uint32_t baud_rate,
+				    uint32_t prescaler)
+{
+	uint32_t scaled_freq = clockfreq / presc_table[prescaler];
+
+	return (scaled_freq + (baud_rate / 2)) / baud_rate;
+
+}
+
+/*
+ * @brief  Return the UART clock frequency.
+ * @param  huart: UART handle.
+ * @retval Frequency value in Hz.
+ */
+static unsigned long uart_get_clock_freq(struct stm32_uart_handle_s *huart)
+{
+	return fdt_get_uart_clock_freq((uintptr_t)huart->base);
+}
+
+/*
+ * @brief  Configure the UART peripheral.
+ * @param  huart: UART handle.
+ * @retval UART status.
+ */
+static int uart_set_config(struct stm32_uart_handle_s *huart,
+			   const struct stm32_uart_init_s *init)
+{
+	uint32_t tmpreg;
+	unsigned long clockfreq;
+	uint32_t brrtemp;
+
+	/*
+	 * ---------------------- USART CR1 Configuration --------------------
+	 * Clear M, PCE, PS, TE, RE and OVER8 bits and configure
+	 * the UART word length, parity, mode and oversampling:
+	 * - set the M bits according to init->word_length value,
+	 * - set PCE and PS bits according to init->parity value,
+	 * - set TE and RE bits according to init->mode value,
+	 * - set OVER8 bit according to init->over_sampling value.
+	 */
+	tmpreg = init->word_length |
+		 init->parity |
+		 init->mode |
+		 init->over_sampling |
+		 init->fifo_mode;
+	mmio_clrsetbits_32(huart->base + USART_CR1, STM32_UART_CR1_FIELDS, tmpreg);
+
+	/*
+	 * --------------------- USART CR2 Configuration ---------------------
+	 * Configure the UART Stop Bits: Set STOP[13:12] bits according
+	 * to init->stop_bits value.
+	 */
+	mmio_clrsetbits_32(huart->base + USART_CR2, STM32_UART_CR2_FIELDS,
+			   init->stop_bits);
+
+	/*
+	 * --------------------- USART CR3 Configuration ---------------------
+	 * Configure:
+	 * - UART HardWare Flow Control: set CTSE and RTSE bits according
+	 *   to init->hw_flow_control value,
+	 * - one-bit sampling method versus three samples' majority rule
+	 *   according to init->one_bit_sampling (not applicable to
+	 *   LPUART),
+	 * - set TXFTCFG bit according to init->tx_fifo_threshold value,
+	 * - set RXFTCFG bit according to init->rx_fifo_threshold value.
+	 */
+	tmpreg = init->hw_flow_control | init->one_bit_sampling;
+
+	if (init->fifo_mode == USART_CR1_FIFOEN) {
+		tmpreg |= init->tx_fifo_threshold |
+			  init->rx_fifo_threshold;
+	}
+
+	mmio_clrsetbits_32(huart->base + USART_CR3, STM32_UART_CR3_FIELDS, tmpreg);
+
+	/*
+	 * --------------------- USART PRESC Configuration -------------------
+	 * Configure UART Clock Prescaler : set PRESCALER according to
+	 * init->prescaler value.
+	 */
+	assert(init->prescaler <= STM32_UART_PRESCALER_MAX);
+	mmio_clrsetbits_32(huart->base + USART_PRESC, USART_PRESC_PRESCALER,
+			   init->prescaler);
+
+	/*---------------------- USART BRR configuration --------------------*/
+	clockfreq = uart_get_clock_freq(huart);
+	if (clockfreq == 0UL) {
+		return -ENODEV;
+	}
+
+	if (init->over_sampling == STM32_UART_OVERSAMPLING_8) {
+		uint32_t usartdiv = uart_div_sampling8(clockfreq,
+						       init->baud_rate,
+						       init->prescaler);
+
+		brrtemp = (usartdiv & USART_BRR_DIV_MANTISSA) |
+			  ((usartdiv & USART_BRR_DIV_FRACTION) >> 1);
+	} else {
+		brrtemp = uart_div_sampling16(clockfreq,
+					      init->baud_rate,
+					      init->prescaler) &
+			  (USART_BRR_DIV_FRACTION | USART_BRR_DIV_MANTISSA);
+	}
+	mmio_write_32(huart->base + USART_BRR, brrtemp);
+
+	return 0U;
+}
+
+/*
+ * @brief  Handle UART communication timeout.
+ * @param  huart: UART handle.
+ * @param  flag: Specifies the UART flag to check.
+ * @retval UART status.
+ */
+static int stm32_uart_wait_flag(struct stm32_uart_handle_s *huart, uint32_t flag)
+{
+	uint64_t timeout_ref = timeout_init_us(STM32_UART_TIMEOUT_US);
+
+	while ((mmio_read_32(huart->base + USART_ISR) & flag) == 0U) {
+		if (timeout_elapsed(timeout_ref)) {
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0U;
+}
+
+/*
+ * @brief  Check the UART idle State.
+ * @param  huart: UART handle.
+ * @retval UART status.
+ */
+static int stm32_uart_check_idle(struct stm32_uart_handle_s *huart)
+{
+	int ret;
+
+	/* Check if the transmitter is enabled */
+	if ((mmio_read_32(huart->base + USART_CR1) & USART_CR1_TE) == USART_CR1_TE) {
+		ret = stm32_uart_wait_flag(huart, USART_ISR_TEACK);
+		if (ret != 0U) {
+			return ret;
+		}
+	}
+
+	/* Check if the receiver is enabled */
+	if ((mmio_read_32(huart->base + USART_CR1) & USART_CR1_RE) == USART_CR1_RE) {
+		ret = stm32_uart_wait_flag(huart, USART_ISR_REACK);
+		if (ret != 0U) {
+			return ret;
+		}
+	}
+
+	return 0U;
+}
+
+/*
+ * @brief  Compute RDR register mask depending on word length.
+ * @param  huart: UART handle.
+ * @retval Mask value.
+ */
+static unsigned int stm32_uart_rdr_mask(const struct stm32_uart_init_s *init)
+{
+	unsigned int mask;
+
+	switch (init->word_length) {
+	case STM32_UART_WORDLENGTH_9B:
+		mask = GENMASK(8, 0);
+		break;
+	case STM32_UART_WORDLENGTH_8B:
+		mask = GENMASK(7, 0);
+		break;
+	case STM32_UART_WORDLENGTH_7B:
+		mask = GENMASK(6, 0);
+		break;
+	default:
+		return 0U;
+	}
+
+	if (init->parity != STM32_UART_PARITY_NONE) {
+		mask >>= 1;
+	}
+
+	return mask;
+}
+
+/*
+ * @brief  Stop the UART.
+ * @param  base: UART base address.
+ */
+void stm32_uart_stop(uintptr_t base)
+{
+	mmio_clrbits_32(base + USART_CR1, USART_CR1_UE);
+}
+
+/*
+ * @brief  Initialize UART.
+ * @param  huart: UART handle.
+ * @param  base_addr: base address of UART.
+ * @param  init: UART initialization parameter.
+ * @retval UART status.
+ */
+
+int stm32_uart_init(struct stm32_uart_handle_s *huart,
+		    uintptr_t base_addr,
+		    const struct stm32_uart_init_s *init)
+{
+	int ret;
+
+	if (huart == NULL || init == NULL || base_addr == 0U) {
+		return -EINVAL;
+	}
+
+	huart->base = base_addr;
+
+	/* Disable the peripheral */
+	stm32_uart_stop(huart->base);
+
+	/* Computation of UART mask to apply to RDR register */
+	huart->rdr_mask = stm32_uart_rdr_mask(init);
+
+	/* Init the peripheral */
+	ret = uart_set_config(huart, init);
+	if (ret != 0U) {
+		return ret;
+	}
+
+	/* Enable the peripheral */
+	mmio_setbits_32(huart->base + USART_CR1, USART_CR1_UE);
+
+	/* TEACK and/or REACK to check */
+	return stm32_uart_check_idle(huart);
+}
+
+/*
+ * @brief  Check interrupt and status errors.
+ * @retval True if error detected, false otherwise.
+ */
+static bool stm32_uart_error_detected(struct stm32_uart_handle_s *huart)
+{
+	return (mmio_read_32(huart->base + USART_ISR) & STM32_UART_ISR_ERRORS) != 0U;
+}
+
+/*
+ * @brief  Clear status errors.
+ */
+static void stm32_uart_error_clear(struct stm32_uart_handle_s *huart)
+{
+	mmio_write_32(huart->base + USART_ICR, STM32_UART_ISR_ERRORS);
+}
+
+/*
+ * @brief  Transmit one data in no blocking mode
+ * @param  huart: UART handle.
+ * @param  c: data to sent.
+ * @retval UART status.
+ */
+int stm32_uart_putc(struct stm32_uart_handle_s *huart, int c)
+{
+	int ret;
+
+	if (huart == NULL) {
+		return -EINVAL;
+	}
+
+	ret = stm32_uart_wait_flag(huart, USART_ISR_TXE);
+	if (ret != 0) {
+		return ret;
+
+	}
+	mmio_write_32(huart->base + USART_TDR, c);
+	if (stm32_uart_error_detected(huart)) {
+		stm32_uart_error_clear(huart);
+		return -EFAULT;
+	}
+
+	return 0U;
+}
+
+/*
+ * @brief  Flush TX Transmit fifo
+ * @param  huart: UART handle.
+ * @retval UART status.
+ */
+int stm32_uart_flush(struct stm32_uart_handle_s *huart)
+{
+	int ret;
+
+	if (huart == NULL) {
+		return -EINVAL;
+	}
+	ret = stm32_uart_wait_flag(huart, USART_ISR_TXE);
+	if (ret != 0) {
+		return ret;
+
+	}
+	ret = stm32_uart_wait_flag(huart, USART_ISR_TC);
+
+	return ret;
+}
+
+/*
+ * @brief  Receive a data in no blocking mode.
+ * @retval value if >0 or UART status.
+ */
+int stm32_uart_getc(struct stm32_uart_handle_s *huart)
+{
+	uint32_t data;
+
+	if (huart == NULL) {
+		return -EINVAL;
+	}
+
+	/* check if data is available */
+	if ((mmio_read_32(huart->base + USART_ISR) & USART_ISR_RXNE) == 0U) {
+		return -EAGAIN;
+	}
+
+	data = mmio_read_32(huart->base + USART_RDR) & huart->rdr_mask;
+
+	if (stm32_uart_error_detected(huart)) {
+		stm32_uart_error_clear(huart);
+		return -EFAULT;
+	}
+
+	return data;
+}
diff --git a/drivers/st/usb_dwc2/usb_dwc2.c b/drivers/st/usb_dwc2/usb_dwc2.c
new file mode 100644
index 0000000000..fc3f771c7c
--- /dev/null
+++ b/drivers/st/usb_dwc2/usb_dwc2.c
@@ -0,0 +1,1094 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <platform_def.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/usb_dwc2.h>
+#include <lib/mmio.h>
+
+#define USB_OTG_MODE_DEVICE			0U
+#define USB_OTG_MODE_HOST			1U
+#define USB_OTG_MODE_DRD			2U
+
+#define EP_TYPE_CTRL				0U
+#define EP_TYPE_ISOC				1U
+#define EP_TYPE_BULK				2U
+#define EP_TYPE_INTR				3U
+
+#define USBD_FIFO_FLUSH_TIMEOUT_US		1000U
+#define EP0_FIFO_SIZE				64U
+
+/* OTG registers offsets */
+#define OTG_GOTGINT				0x004U
+#define OTG_GAHBCFG				0x008U
+#define OTG_GUSBCFG				0x00CU
+#define OTG_GRSTCTL				0x010U
+#define OTG_GINTSTS				0x014U
+#define OTG_GINTMSK				0x018U
+#define OTG_GRXSTSP				0x020U
+#define OTG_GLPMCFG				0x054U
+#define OTG_DCFG				0x800U
+#define OTG_DCTL				0x804U
+#define OTG_DSTS				0x808U
+#define OTG_DIEPMSK				0x810U
+#define OTG_DOEPMSK				0x814U
+#define OTG_DAINT				0x818U
+#define OTG_DAINTMSK				0x81CU
+#define OTG_DIEPEMPMSK				0x834U
+
+/* Definitions for OTG_DIEPx registers */
+#define OTG_DIEP_BASE				0x900U
+#define OTG_DIEP_SIZE				0x20U
+#define OTG_DIEPCTL				0x00U
+#define OTG_DIEPINT				0x08U
+#define OTG_DIEPTSIZ				0x10U
+#define OTG_DIEPDMA				0x14U
+#define OTG_DTXFSTS				0x18U
+#define OTG_DIEP_MAX_NB				9U
+
+/* Definitions for OTG_DOEPx registers */
+#define OTG_DOEP_BASE				0xB00U
+#define OTG_DOEP_SIZE				0x20U
+#define OTG_DOEPCTL				0x00U
+#define OTG_DOEPINT				0x08U
+#define OTG_DOEPTSIZ				0x10U
+#define OTG_DOEPDMA				0x14U
+#define OTG_D0EP_MAX_NB				9U
+
+/* Definitions for OTG_DAINT registers */
+#define OTG_DAINT_OUT_MASK			GENMASK(31, 16)
+#define OTG_DAINT_OUT_SHIFT			16U
+#define OTG_DAINT_IN_MASK			GENMASK(15, 0)
+#define OTG_DAINT_IN_SHIFT			0U
+
+#define OTG_DAINT_EP0_IN			BIT(16)
+#define OTG_DAINT_EP0_OUT			BIT(0)
+
+/* Definitions for FIFOs */
+#define OTG_FIFO_BASE				0x1000U
+#define OTG_FIFO_SIZE				0x1000U
+
+/* Bit definitions for OTG_GOTGINT register */
+#define OTG_GOTGINT_SEDET			BIT(2)
+
+/* Bit definitions for OTG_GAHBCFG register */
+#define OTG_GAHBCFG_GINT			BIT(0)
+
+/* Bit definitions for OTG_GUSBCFG register */
+#define OTG_GUSBCFG_TRDT			GENMASK(13, 10)
+#define OTG_GUSBCFG_TRDT_SHIFT			10U
+
+#define USBD_HS_TRDT_VALUE			9U
+
+/* Bit definitions for OTG_GRSTCTL register */
+#define OTG_GRSTCTL_RXFFLSH			BIT(4)
+#define OTG_GRSTCTL_TXFFLSH			BIT(5)
+#define OTG_GRSTCTL_TXFNUM_SHIFT		6U
+
+/* Bit definitions for OTG_GINTSTS register */
+#define OTG_GINTSTS_CMOD			BIT(0)
+#define OTG_GINTSTS_MMIS			BIT(1)
+#define OTG_GINTSTS_OTGINT			BIT(2)
+#define OTG_GINTSTS_SOF				BIT(3)
+#define OTG_GINTSTS_RXFLVL			BIT(4)
+#define OTG_GINTSTS_USBSUSP			BIT(11)
+#define OTG_GINTSTS_USBRST			BIT(12)
+#define OTG_GINTSTS_ENUMDNE			BIT(13)
+#define OTG_GINTSTS_IEPINT			BIT(18)
+#define OTG_GINTSTS_OEPINT			BIT(19)
+#define OTG_GINTSTS_IISOIXFR			BIT(20)
+#define OTG_GINTSTS_IPXFR_INCOMPISOOUT		BIT(21)
+#define OTG_GINTSTS_LPMINT			BIT(27)
+#define OTG_GINTSTS_SRQINT			BIT(30)
+#define OTG_GINTSTS_WKUPINT			BIT(31)
+
+/* Bit definitions for OTG_GRXSTSP register */
+#define OTG_GRXSTSP_EPNUM			GENMASK(3, 0)
+#define OTG_GRXSTSP_BCNT			GENMASK(14, 4)
+#define OTG_GRXSTSP_BCNT_SHIFT			4U
+#define OTG_GRXSTSP_PKTSTS			GENMASK(20, 17)
+#define OTG_GRXSTSP_PKTSTS_SHIFT		17U
+
+#define STS_GOUT_NAK				1U
+#define STS_DATA_UPDT				2U
+#define STS_XFER_COMP				3U
+#define STS_SETUP_COMP				4U
+#define STS_SETUP_UPDT				6U
+
+/* Bit definitions for OTG_GLPMCFG register */
+#define OTG_GLPMCFG_BESL			GENMASK(5, 2)
+
+/* Bit definitions for OTG_DCFG register */
+#define OTG_DCFG_DAD				GENMASK(10, 4)
+#define OTG_DCFG_DAD_SHIFT			4U
+
+/* Bit definitions for OTG_DCTL register */
+#define OTG_DCTL_RWUSIG				BIT(0)
+#define OTG_DCTL_SDIS				BIT(1)
+#define OTG_DCTL_CGINAK				BIT(8)
+
+/* Bit definitions for OTG_DSTS register */
+#define OTG_DSTS_SUSPSTS			BIT(0)
+#define OTG_DSTS_ENUMSPD_MASK			GENMASK(2, 1)
+#define OTG_DSTS_FNSOF0				BIT(8)
+
+#define OTG_DSTS_ENUMSPD(val)			((val) << 1)
+#define OTG_DSTS_ENUMSPD_HS_PHY_30MHZ_OR_60MHZ	OTG_DSTS_ENUMSPD(0U)
+#define OTG_DSTS_ENUMSPD_FS_PHY_30MHZ_OR_60MHZ	OTG_DSTS_ENUMSPD(1U)
+#define OTG_DSTS_ENUMSPD_LS_PHY_6MHZ		OTG_DSTS_ENUMSPD(2U)
+#define OTG_DSTS_ENUMSPD_FS_PHY_48MHZ		OTG_DSTS_ENUMSPD(3U)
+
+/* Bit definitions for OTG_DIEPMSK register */
+#define OTG_DIEPMSK_XFRCM			BIT(0)
+#define OTG_DIEPMSK_EPDM			BIT(1)
+#define OTG_DIEPMSK_TOM				BIT(3)
+
+/* Bit definitions for OTG_DOEPMSK register */
+#define OTG_DOEPMSK_XFRCM			BIT(0)
+#define OTG_DOEPMSK_EPDM			BIT(1)
+#define OTG_DOEPMSK_STUPM			BIT(3)
+
+/* Bit definitions for OTG_DIEPCTLx registers */
+#define OTG_DIEPCTL_MPSIZ			GENMASK(10, 0)
+#define OTG_DIEPCTL_STALL			BIT(21)
+#define OTG_DIEPCTL_CNAK			BIT(26)
+#define OTG_DIEPCTL_SD0PID_SEVNFRM		BIT(28)
+#define OTG_DIEPCTL_SODDFRM			BIT(29)
+#define OTG_DIEPCTL_EPDIS			BIT(30)
+#define OTG_DIEPCTL_EPENA			BIT(31)
+
+/* Bit definitions for OTG_DIEPINTx registers */
+#define OTG_DIEPINT_XFRC			BIT(0)
+#define OTG_DIEPINT_EPDISD			BIT(1)
+#define OTG_DIEPINT_TOC				BIT(3)
+#define OTG_DIEPINT_ITTXFE			BIT(4)
+#define OTG_DIEPINT_INEPNE			BIT(6)
+#define OTG_DIEPINT_TXFE			BIT(7)
+#define OTG_DIEPINT_TXFE_SHIFT			7U
+
+#define OTG_DIEPINT_MASK			(BIT(13) | BIT(11) | GENMASK(9, 0))
+
+/* Bit definitions for OTG_DIEPTSIZx registers */
+#define OTG_DIEPTSIZ_XFRSIZ			GENMASK(18, 0)
+#define OTG_DIEPTSIZ_PKTCNT			GENMASK(28, 19)
+#define OTG_DIEPTSIZ_PKTCNT_SHIFT		19U
+#define OTG_DIEPTSIZ_MCNT_MASK			GENMASK(30, 29)
+#define OTG_DIEPTSIZ_MCNT_DATA0			BIT(29)
+
+#define OTG_DIEPTSIZ_PKTCNT_1			BIT(19)
+
+/* Bit definitions for OTG_DTXFSTSx registers */
+#define OTG_DTXFSTS_INEPTFSAV			GENMASK(15, 0)
+
+/* Bit definitions for OTG_DOEPCTLx registers */
+#define OTG_DOEPCTL_STALL			BIT(21)
+#define OTG_DOEPCTL_CNAK			BIT(26)
+#define OTG_DOEPCTL_SD0PID_SEVNFRM		BIT(28) /* other than endpoint 0 */
+#define OTG_DOEPCTL_SD1PID_SODDFRM		BIT(29) /* other than endpoint 0 */
+#define OTG_DOEPCTL_EPDIS			BIT(30)
+#define OTG_DOEPCTL_EPENA			BIT(31)
+
+/* Bit definitions for OTG_DOEPTSIZx registers */
+#define OTG_DOEPTSIZ_XFRSIZ			GENMASK(18, 0)
+#define OTG_DOEPTSIZ_PKTCNT			GENMASK(28, 19)
+#define OTG_DOEPTSIZ_RXDPID_STUPCNT		GENMASK(30, 29)
+
+/* Bit definitions for OTG_DOEPINTx registers */
+#define OTG_DOEPINT_XFRC			BIT(0)
+#define OTG_DOEPINT_STUP			BIT(3)
+#define OTG_DOEPINT_OTEPDIS			BIT(4)
+
+#define OTG_DOEPINT_MASK			(GENMASK(15, 12) | GENMASK(9, 8) | GENMASK(6, 0))
+
+#define EP_NB					15U
+#define EP_ALL					0x10U
+
+/*
+ * @brief  Flush TX FIFO.
+ * @param  handle: PCD handle.
+ * @param  num: FIFO number.
+ *	   This parameter can be a value from 1 to 15 or EP_ALL.
+ *	   EP_ALL= 0x10 means Flush all TX FIFOs
+ * @retval USB status.
+ */
+static usb_status_t usb_dwc2_flush_tx_fifo(void *handle, uint32_t num)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint64_t timeout = timeout_init_us(USBD_FIFO_FLUSH_TIMEOUT_US);
+
+	mmio_write_32(usb_base_addr + OTG_GRSTCTL,
+		      OTG_GRSTCTL_TXFFLSH | (uint32_t)(num << OTG_GRSTCTL_TXFNUM_SHIFT));
+
+	while ((mmio_read_32(usb_base_addr + OTG_GRSTCTL) &
+		OTG_GRSTCTL_TXFFLSH) == OTG_GRSTCTL_TXFFLSH)
+		if (timeout_elapsed(timeout)) {
+			return USBD_TIMEOUT;
+		}
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  Flush RX FIFO.
+ * @param  handle: PCD handle.
+ * @retval USB status.
+ */
+static usb_status_t usb_dwc2_flush_rx_fifo(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint64_t timeout = timeout_init_us(USBD_FIFO_FLUSH_TIMEOUT_US);
+
+	mmio_write_32(usb_base_addr + OTG_GRSTCTL, OTG_GRSTCTL_RXFFLSH);
+
+	while ((mmio_read_32(usb_base_addr + OTG_GRSTCTL) &
+		 OTG_GRSTCTL_RXFFLSH) == OTG_GRSTCTL_RXFFLSH)
+		if (timeout_elapsed(timeout)) {
+			return USBD_TIMEOUT;
+		}
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  Return the global USB interrupt status.
+ * @param  handle: PCD handle.
+ * @retval Interrupt register value.
+ */
+static uint32_t usb_dwc2_read_int(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	return mmio_read_32(usb_base_addr + OTG_GINTSTS) &
+	       mmio_read_32(usb_base_addr + OTG_GINTMSK);
+}
+
+/*
+ * @brief  Return the USB device OUT endpoints interrupt.
+ * @param  handle: PCD handle.
+ * @retval Device OUT endpoint interrupts.
+ */
+static uint32_t usb_dwc2_all_out_ep_int(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	return ((mmio_read_32(usb_base_addr + OTG_DAINT) &
+		 mmio_read_32(usb_base_addr + OTG_DAINTMSK)) &
+		OTG_DAINT_OUT_MASK) >> OTG_DAINT_OUT_SHIFT;
+}
+
+/*
+ * @brief  Return the USB device IN endpoints interrupt.
+ * @param  handle: PCD handle.
+ * @retval Device IN endpoint interrupts.
+ */
+static uint32_t usb_dwc2_all_in_ep_int(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	return ((mmio_read_32(usb_base_addr + OTG_DAINT) &
+		 mmio_read_32(usb_base_addr + OTG_DAINTMSK)) &
+		OTG_DAINT_IN_MASK) >> OTG_DAINT_IN_SHIFT;
+}
+
+/*
+ * @brief  Return Device OUT EP interrupt register.
+ * @param  handle: PCD handle.
+ * @param  epnum: Endpoint number.
+ *         This parameter can be a value from 0 to 15.
+ * @retval Device OUT EP Interrupt register.
+ */
+static uint32_t usb_dwc2_out_ep_int(void *handle, uint8_t epnum)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	return mmio_read_32(usb_base_addr + OTG_DOEP_BASE +
+			    (epnum * OTG_DOEP_SIZE) + OTG_DOEPINT) &
+	       mmio_read_32(usb_base_addr + OTG_DOEPMSK);
+}
+
+/*
+ * @brief  Return Device IN EP interrupt register.
+ * @param  handle: PCD handle.
+ * @param  epnum: Endpoint number.
+ *         This parameter can be a value from 0 to 15.
+ * @retval Device IN EP Interrupt register.
+ */
+static uint32_t usb_dwc2_in_ep_int(void *handle, uint8_t epnum)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t msk;
+	uint32_t emp;
+
+	msk = mmio_read_32(usb_base_addr + OTG_DIEPMSK);
+	emp = mmio_read_32(usb_base_addr + OTG_DIEPEMPMSK);
+	msk |= ((emp >> epnum) << OTG_DIEPINT_TXFE_SHIFT) & OTG_DIEPINT_TXFE;
+
+	return mmio_read_32(usb_base_addr + OTG_DIEP_BASE +
+			    (epnum * OTG_DIEP_SIZE) + OTG_DIEPINT) & msk;
+}
+
+/*
+ * @brief  Return USB core mode.
+ * @param  handle: PCD handle.
+ * @retval Core mode.
+ *         This parameter can be one of the these values:
+ *          0 : Host.
+ *          1 : Device.
+ */
+static uint32_t usb_dwc2_get_mode(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	return mmio_read_32(usb_base_addr + OTG_GINTSTS) & OTG_GINTSTS_CMOD;
+}
+
+/*
+ * @brief  Activate EP0 for detup transactions.
+ * @param  handle: PCD handle.
+ * @retval USB status.
+ */
+static usb_status_t usb_dwc2_activate_setup(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uintptr_t reg_offset = usb_base_addr + OTG_DIEP_BASE;
+
+	/* Set the MPS of the IN EP based on the enumeration speed */
+	mmio_clrbits_32(reg_offset + OTG_DIEPCTL, OTG_DIEPCTL_MPSIZ);
+
+	if ((mmio_read_32(usb_base_addr + OTG_DSTS) & OTG_DSTS_ENUMSPD_MASK) ==
+	    OTG_DSTS_ENUMSPD_LS_PHY_6MHZ) {
+		mmio_setbits_32(reg_offset + OTG_DIEPCTL, 3U);
+	}
+
+	mmio_setbits_32(usb_base_addr + OTG_DCTL, OTG_DCTL_CGINAK);
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  Prepare the EP0 to start the first control setup.
+ * @param  handle: Selected device.
+ * @retval USB status.
+ */
+static usb_status_t usb_dwc2_ep0_out_start(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uintptr_t reg_offset = usb_base_addr + OTG_DIEP_BASE + OTG_DIEPTSIZ;
+	uint32_t reg_value = 0U;
+
+	/* PKTCNT = 1 and XFRSIZ = 24 bytes for endpoint 0 */
+	reg_value |= OTG_DIEPTSIZ_PKTCNT_1;
+	reg_value |= (EP0_FIFO_SIZE & OTG_DIEPTSIZ_XFRSIZ);
+	reg_value |= OTG_DOEPTSIZ_RXDPID_STUPCNT;
+
+	mmio_write_32(reg_offset, reg_value);
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  Write a packet into the TX FIFO associated with the EP/channel.
+ * @param  handle: Selected device.
+ * @param  src: Pointer to source buffer.
+ * @param  ch_ep_num: Endpoint or host channel number.
+ * @param  len: Number of bytes to write.
+ * @retval USB status.
+ */
+static usb_status_t usb_dwc2_write_packet(void *handle, uint8_t *src,
+					  uint8_t ch_ep_num, uint16_t len)
+{
+	uint32_t reg_offset;
+	uint32_t count32b = (len + 3U) / 4U;
+	uint32_t i;
+
+	reg_offset = (uintptr_t)handle + OTG_FIFO_BASE +
+		     (ch_ep_num * OTG_FIFO_SIZE);
+
+	for (i = 0U; i < count32b; i++) {
+		uint32_t src_copy = 0U;
+		uint32_t j;
+
+		/* Data written to FIFO need to be 4 bytes aligned */
+		for (j = 0U; j < 4U; j++) {
+			src_copy += (*(src + j)) << (8U * j);
+		}
+
+		mmio_write_32(reg_offset, src_copy);
+		src += 4U;
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  Read a packet from the RX FIFO associated with the EP/channel.
+ * @param  handle: Selected device.
+ * @param  src: Source pointer.
+ * @param  ch_ep_num: Endpoint or host channel number.
+ * @param  len: Number of bytes to read.
+ * @retval Pointer to destination buffer.
+ */
+static void *usb_dwc2_read_packet(void *handle, uint8_t *dest, uint16_t len)
+{
+	uint32_t reg_offset;
+	uint32_t count32b = (len + 3U) / 4U;
+	uint32_t i;
+
+	VERBOSE("read packet length %i to 0x%lx\n", len, (uintptr_t)dest);
+
+	reg_offset = (uintptr_t)handle + OTG_FIFO_BASE;
+
+	for (i = 0U; i < count32b; i++) {
+		*(uint32_t *)dest = mmio_read_32(reg_offset);
+		dest += 4U;
+		dsb();
+	}
+
+	return (void *)dest;
+}
+
+/*
+ * @brief  Setup and start a transfer over an EP.
+ * @param  handle: Selected device
+ * @param  ep: Pointer to endpoint structure.
+ * @retval USB status.
+ */
+static usb_status_t usb_dwc2_ep_start_xfer(void *handle, usbd_ep_t *ep)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t reg_offset;
+	uint32_t reg_value;
+	uint32_t clear_value;
+
+	if (ep->is_in) {
+		reg_offset = usb_base_addr + OTG_DIEP_BASE + (ep->num * OTG_DIEP_SIZE);
+		clear_value = OTG_DIEPTSIZ_PKTCNT | OTG_DIEPTSIZ_XFRSIZ;
+		if (ep->xfer_len == 0U) {
+			reg_value = OTG_DIEPTSIZ_PKTCNT_1;
+		} else {
+			/*
+			 * Program the transfer size and packet count
+			 * as follows:
+			 * xfersize = N * maxpacket + short_packet
+			 * pktcnt = N + (short_packet exist ? 1 : 0)
+			 */
+			reg_value = (OTG_DIEPTSIZ_PKTCNT &
+				     (((ep->xfer_len + ep->maxpacket - 1U) /
+				       ep->maxpacket) << OTG_DIEPTSIZ_PKTCNT_SHIFT))
+				    | ep->xfer_len;
+
+			if (ep->type == EP_TYPE_ISOC) {
+				clear_value |= OTG_DIEPTSIZ_MCNT_MASK;
+				reg_value |= OTG_DIEPTSIZ_MCNT_DATA0;
+			}
+		}
+
+		mmio_clrsetbits_32(reg_offset + OTG_DIEPTSIZ, clear_value, reg_value);
+
+		if ((ep->type != EP_TYPE_ISOC) && (ep->xfer_len > 0U)) {
+			/* Enable the TX FIFO empty interrupt for this EP */
+			mmio_setbits_32(usb_base_addr + OTG_DIEPEMPMSK, BIT(ep->num));
+		}
+
+		/* EP enable, IN data in FIFO */
+		reg_value = OTG_DIEPCTL_CNAK | OTG_DIEPCTL_EPENA;
+
+		if (ep->type == EP_TYPE_ISOC) {
+			if ((mmio_read_32(usb_base_addr + OTG_DSTS) & OTG_DSTS_FNSOF0) == 0U) {
+				reg_value |= OTG_DIEPCTL_SODDFRM;
+			} else {
+				reg_value |= OTG_DIEPCTL_SD0PID_SEVNFRM;
+			}
+		}
+
+		mmio_setbits_32(reg_offset + OTG_DIEPCTL, reg_value);
+
+		if (ep->type == EP_TYPE_ISOC) {
+			usb_dwc2_write_packet(handle, ep->xfer_buff, ep->num, ep->xfer_len);
+		}
+	} else {
+		reg_offset = usb_base_addr + OTG_DOEP_BASE + (ep->num * OTG_DOEP_SIZE);
+		/*
+		 * Program the transfer size and packet count as follows:
+		 * pktcnt = N
+		 * xfersize = N * maxpacket
+		 */
+		if (ep->xfer_len == 0U) {
+			reg_value = ep->maxpacket | OTG_DIEPTSIZ_PKTCNT_1;
+		} else {
+			uint16_t pktcnt = (ep->xfer_len + ep->maxpacket - 1U) / ep->maxpacket;
+
+			reg_value = (pktcnt << OTG_DIEPTSIZ_PKTCNT_SHIFT) |
+				    (ep->maxpacket * pktcnt);
+		}
+
+		mmio_clrsetbits_32(reg_offset + OTG_DOEPTSIZ,
+				   OTG_DOEPTSIZ_XFRSIZ & OTG_DOEPTSIZ_PKTCNT,
+				   reg_value);
+
+		/* EP enable */
+		reg_value = OTG_DOEPCTL_CNAK | OTG_DOEPCTL_EPENA;
+
+		if (ep->type == EP_TYPE_ISOC) {
+			if ((mmio_read_32(usb_base_addr + OTG_DSTS) & OTG_DSTS_FNSOF0) == 0U) {
+				reg_value |= OTG_DOEPCTL_SD1PID_SODDFRM;
+			} else {
+				reg_value |= OTG_DOEPCTL_SD0PID_SEVNFRM;
+			}
+		}
+
+		mmio_setbits_32(reg_offset + OTG_DOEPCTL, reg_value);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  Setup and start a transfer over the EP0.
+ * @param  handle: Selected device.
+ * @param  ep: Pointer to endpoint structure.
+ * @retval USB status.
+ */
+static usb_status_t usb_dwc2_ep0_start_xfer(void *handle, usbd_ep_t *ep)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t reg_offset;
+	uint32_t reg_value;
+
+	if (ep->is_in) {
+		reg_offset = usb_base_addr + OTG_DIEP_BASE +
+			     (ep->num * OTG_DIEP_SIZE);
+
+		if (ep->xfer_len == 0U) {
+			reg_value = OTG_DIEPTSIZ_PKTCNT_1;
+		} else {
+			/*
+			 * Program the transfer size and packet count
+			 * as follows:
+			 * xfersize = N * maxpacket + short_packet
+			 * pktcnt = N + (short_packet exist ? 1 : 0)
+			 */
+
+			if (ep->xfer_len > ep->maxpacket) {
+				ep->xfer_len = ep->maxpacket;
+			}
+
+			reg_value = OTG_DIEPTSIZ_PKTCNT_1 | ep->xfer_len;
+		}
+
+		mmio_clrsetbits_32(reg_offset + OTG_DIEPTSIZ,
+				   OTG_DIEPTSIZ_XFRSIZ | OTG_DIEPTSIZ_PKTCNT,
+				   reg_value);
+
+		/* Enable the TX FIFO empty interrupt for this EP */
+		if (ep->xfer_len > 0U) {
+			mmio_setbits_32(usb_base_addr +	OTG_DIEPEMPMSK,
+					BIT(ep->num));
+		}
+
+		/* EP enable, IN data in FIFO */
+		mmio_setbits_32(reg_offset + OTG_DIEPCTL,
+				OTG_DIEPCTL_CNAK | OTG_DIEPCTL_EPENA);
+	} else {
+		reg_offset = usb_base_addr + OTG_DOEP_BASE +
+			     (ep->num * OTG_DOEP_SIZE);
+
+		/*
+		 * Program the transfer size and packet count as follows:
+		 * pktcnt = N
+		 * xfersize = N * maxpacket
+		 */
+		if (ep->xfer_len > 0U) {
+			ep->xfer_len = ep->maxpacket;
+		}
+
+		reg_value = OTG_DIEPTSIZ_PKTCNT_1 | ep->maxpacket;
+
+		mmio_clrsetbits_32(reg_offset + OTG_DIEPTSIZ,
+				   OTG_DIEPTSIZ_XFRSIZ | OTG_DIEPTSIZ_PKTCNT,
+				   reg_value);
+
+		/* EP enable */
+		mmio_setbits_32(reg_offset + OTG_DOEPCTL,
+				OTG_DOEPCTL_CNAK | OTG_DOEPCTL_EPENA);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  Set a stall condition over an EP.
+ * @param  handle: Selected device.
+ * @param  ep: Pointer to endpoint structure.
+ * @retval USB status.
+ */
+static usb_status_t usb_dwc2_ep_set_stall(void *handle, usbd_ep_t *ep)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t reg_offset;
+	uint32_t reg_value;
+
+	if (ep->is_in) {
+		reg_offset = usb_base_addr + OTG_DIEP_BASE +
+			     (ep->num * OTG_DIEP_SIZE);
+		reg_value = mmio_read_32(reg_offset + OTG_DIEPCTL);
+
+		if ((reg_value & OTG_DIEPCTL_EPENA) == 0U) {
+			reg_value &= ~OTG_DIEPCTL_EPDIS;
+		}
+
+		reg_value |= OTG_DIEPCTL_STALL;
+
+		mmio_write_32(reg_offset + OTG_DIEPCTL, reg_value);
+	} else {
+		reg_offset = usb_base_addr + OTG_DOEP_BASE +
+			     (ep->num * OTG_DOEP_SIZE);
+		reg_value = mmio_read_32(reg_offset + OTG_DOEPCTL);
+
+		if ((reg_value & OTG_DOEPCTL_EPENA) == 0U) {
+			reg_value &= ~OTG_DOEPCTL_EPDIS;
+		}
+
+		reg_value |= OTG_DOEPCTL_STALL;
+
+		mmio_write_32(reg_offset + OTG_DOEPCTL, reg_value);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  Stop the USB device mode.
+ * @param  handle: Selected device.
+ * @retval USB status.
+ */
+static usb_status_t usb_dwc2_stop_device(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t i;
+
+	/* Disable Int */
+	mmio_clrbits_32(usb_base_addr + OTG_GAHBCFG, OTG_GAHBCFG_GINT);
+
+	/* Clear pending interrupts */
+	for (i = 0U; i < EP_NB; i++) {
+		mmio_write_32(usb_base_addr + OTG_DIEP_BASE + (i * OTG_DIEP_SIZE) + OTG_DIEPINT,
+			      OTG_DIEPINT_MASK);
+		mmio_write_32(usb_base_addr + OTG_DOEP_BASE + (i * OTG_DOEP_SIZE) + OTG_DOEPINT,
+			      OTG_DOEPINT_MASK);
+	}
+
+	mmio_write_32(usb_base_addr + OTG_DAINT, OTG_DAINT_IN_MASK | OTG_DAINT_OUT_MASK);
+
+	/* Clear interrupt masks */
+	mmio_write_32(usb_base_addr + OTG_DIEPMSK, 0U);
+	mmio_write_32(usb_base_addr + OTG_DOEPMSK, 0U);
+	mmio_write_32(usb_base_addr + OTG_DAINTMSK, 0U);
+
+	/* Flush the FIFO */
+	usb_dwc2_flush_rx_fifo(handle);
+	usb_dwc2_flush_tx_fifo(handle, EP_ALL);
+
+	/* Disconnect the USB device by disabling the pull-up/pull-down. */
+	mmio_setbits_32((uintptr_t)handle + OTG_DCTL, OTG_DCTL_SDIS);
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  Stop the USB device mode.
+ * @param  handle: Selected device.
+ * @param  address: New device address to be assigned.
+ *         This parameter can be a value from 0 to 255.
+ * @retval USB status.
+ */
+static usb_status_t usb_dwc2_set_address(void *handle, uint8_t address)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	mmio_clrsetbits_32(usb_base_addr + OTG_DCFG,
+			   OTG_DCFG_DAD,
+			   address << OTG_DCFG_DAD_SHIFT);
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  Check FIFO for the next packet to be loaded.
+ * @param  handle: Selected device.
+ * @param  epnum : Endpoint number.
+ * @param  xfer_len: Block length.
+ * @param  xfer_count: Number of blocks.
+ * @param  maxpacket: Max packet length.
+ * @param  xfer_buff: Buffer pointer.
+ * @retval USB status.
+ */
+static usb_status_t usb_dwc2_write_empty_tx_fifo(void *handle,
+						 uint32_t epnum,
+						 uint32_t xfer_len,
+						 uint32_t *xfer_count,
+						 uint32_t maxpacket,
+						 uint8_t **xfer_buff)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t reg_offset;
+	int32_t len;
+	uint32_t len32b;
+	usb_status_t ret;
+
+	len = xfer_len - *xfer_count;
+
+	if ((len > 0) && ((uint32_t)len > maxpacket)) {
+		len = maxpacket;
+	}
+
+	len32b = (len + 3U) / 4U;
+
+	reg_offset = usb_base_addr + OTG_DIEP_BASE + (epnum * OTG_DIEP_SIZE);
+
+	while (((mmio_read_32(reg_offset + OTG_DTXFSTS) &
+		OTG_DTXFSTS_INEPTFSAV) > len32b) &&
+	       (*xfer_count < xfer_len) && (xfer_len != 0U)) {
+		/* Write the FIFO */
+		len = xfer_len - *xfer_count;
+
+		if ((len > 0) && ((uint32_t)len > maxpacket)) {
+			len = maxpacket;
+		}
+
+		len32b = (len + 3U) / 4U;
+
+		ret = usb_dwc2_write_packet(handle, *xfer_buff, epnum, len);
+		if (ret != USBD_OK) {
+			return ret;
+		}
+
+		*xfer_buff  += len;
+		*xfer_count += len;
+	}
+
+	if (len <= 0) {
+		mmio_clrbits_32(usb_base_addr + OTG_DIEPEMPMSK, BIT(epnum));
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  Handle PCD interrupt request.
+ * @param  handle: PCD handle.
+ * @param  param: Pointer to information updated by the IT handling.
+ * @retval Action to do after IT handling.
+ */
+static usb_action_t usb_dwc2_it_handler(void *handle, uint32_t *param)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+	uint32_t ep_intr;
+	uint32_t epint;
+	uint32_t epnum;
+	uint32_t temp;
+	usb_status_t ret;
+
+	if (usb_dwc2_get_mode(handle) != USB_OTG_MODE_DEVICE) {
+		return USB_NOTHING;
+	}
+
+	/* Avoid spurious interrupt */
+	if (usb_dwc2_read_int(handle) == 0U) {
+		return USB_NOTHING;
+	}
+
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_MMIS) != 0U) {
+		/* Incorrect mode, acknowledge the interrupt */
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_MMIS);
+	}
+
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_OEPINT) != 0U) {
+		uint32_t reg_offset;
+
+		/* Read in the device interrupt bits */
+		ep_intr = usb_dwc2_all_out_ep_int(handle);
+		epnum = 0U;
+		while ((ep_intr & BIT(0)) != BIT(0)) {
+			epnum++;
+			ep_intr >>= 1;
+		}
+
+		reg_offset = usb_base_addr + OTG_DOEP_BASE + (epnum * OTG_DOEP_SIZE) + OTG_DOEPINT;
+
+		epint = usb_dwc2_out_ep_int(handle, epnum);
+
+		if ((epint & OTG_DOEPINT_XFRC) == OTG_DOEPINT_XFRC) {
+			mmio_write_32(reg_offset, OTG_DOEPINT_XFRC);
+			*param = epnum;
+
+			return USB_DATA_OUT;
+		}
+
+		if ((epint & OTG_DOEPINT_STUP) == OTG_DOEPINT_STUP) {
+			/* Inform  that a setup packet is available */
+			mmio_write_32(reg_offset, OTG_DOEPINT_STUP);
+
+			return USB_SETUP;
+		}
+
+		if ((epint & OTG_DOEPINT_OTEPDIS) == OTG_DOEPINT_OTEPDIS) {
+			mmio_write_32(reg_offset, OTG_DOEPINT_OTEPDIS);
+		}
+	}
+
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_IEPINT) != 0U) {
+		uint32_t reg_offset;
+
+		/* Read in the device interrupt bits */
+		ep_intr = usb_dwc2_all_in_ep_int(handle);
+		epnum = 0U;
+		while ((ep_intr & BIT(0)) != BIT(0)) {
+			epnum++;
+			ep_intr >>= 1;
+		}
+
+		reg_offset = usb_base_addr + OTG_DIEP_BASE + (epnum * OTG_DIEP_SIZE) + OTG_DIEPINT;
+
+		epint = usb_dwc2_in_ep_int(handle, epnum);
+
+		if ((epint & OTG_DIEPINT_XFRC) == OTG_DIEPINT_XFRC) {
+			mmio_clrbits_32(usb_base_addr + OTG_DIEPEMPMSK, BIT(epnum));
+			mmio_write_32(reg_offset, OTG_DIEPINT_XFRC);
+			*param = epnum;
+
+			return USB_DATA_IN;
+		}
+
+		if ((epint & OTG_DIEPINT_TOC) == OTG_DIEPINT_TOC) {
+			mmio_write_32(reg_offset, OTG_DIEPINT_TOC);
+		}
+
+		if ((epint & OTG_DIEPINT_ITTXFE) == OTG_DIEPINT_ITTXFE) {
+			mmio_write_32(reg_offset, OTG_DIEPINT_ITTXFE);
+		}
+
+		if ((epint & OTG_DIEPINT_INEPNE) == OTG_DIEPINT_INEPNE) {
+			mmio_write_32(reg_offset, OTG_DIEPINT_INEPNE);
+		}
+
+		if ((epint & OTG_DIEPINT_EPDISD) == OTG_DIEPINT_EPDISD) {
+			mmio_write_32(reg_offset, OTG_DIEPINT_EPDISD);
+		}
+
+		if ((epint & OTG_DIEPINT_TXFE) == OTG_DIEPINT_TXFE) {
+			*param = epnum;
+
+			return USB_WRITE_EMPTY;
+		}
+	}
+
+	/* Handle resume interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_WKUPINT) != 0U) {
+		INFO("handle USB : Resume\n");
+
+		/* Clear the remote wake-up signaling */
+		mmio_clrbits_32(usb_base_addr + OTG_DCTL, OTG_DCTL_RWUSIG);
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_WKUPINT);
+
+		return USB_RESUME;
+	}
+
+	/* Handle suspend interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_USBSUSP) != 0U) {
+		INFO("handle USB : Suspend int\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_USBSUSP);
+
+		if ((mmio_read_32(usb_base_addr + OTG_DSTS) &
+		     OTG_DSTS_SUSPSTS) == OTG_DSTS_SUSPSTS) {
+			return USB_SUSPEND;
+		}
+	}
+
+	/* Handle LPM interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_LPMINT) != 0U) {
+		INFO("handle USB : LPM int enter in suspend\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_LPMINT);
+		*param = (mmio_read_32(usb_base_addr + OTG_GLPMCFG) &
+			  OTG_GLPMCFG_BESL) >> 2;
+
+		return USB_LPM;
+	}
+
+	/* Handle reset interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_USBRST) != 0U) {
+		INFO("handle USB : Reset\n");
+
+		mmio_clrbits_32(usb_base_addr + OTG_DCTL, OTG_DCTL_RWUSIG);
+
+		usb_dwc2_flush_tx_fifo(handle, 0U);
+
+		mmio_write_32(usb_base_addr + OTG_DAINT, OTG_DAINT_IN_MASK | OTG_DAINT_OUT_MASK);
+		mmio_setbits_32(usb_base_addr + OTG_DAINTMSK, OTG_DAINT_EP0_IN | OTG_DAINT_EP0_OUT);
+
+		mmio_setbits_32(usb_base_addr + OTG_DOEPMSK, OTG_DOEPMSK_STUPM |
+							     OTG_DOEPMSK_XFRCM |
+							     OTG_DOEPMSK_EPDM);
+		mmio_setbits_32(usb_base_addr + OTG_DIEPMSK, OTG_DIEPMSK_TOM |
+							     OTG_DIEPMSK_XFRCM |
+							     OTG_DIEPMSK_EPDM);
+
+		/* Set default address to 0 */
+		mmio_clrbits_32(usb_base_addr + OTG_DCFG, OTG_DCFG_DAD);
+
+		/* Setup EP0 to receive SETUP packets */
+		ret = usb_dwc2_ep0_out_start(handle);
+		if (ret != USBD_OK) {
+			return ret;
+		}
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_USBRST);
+
+		return USB_RESET;
+	}
+
+	/* Handle enumeration done interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_ENUMDNE) != 0U) {
+		ret = usb_dwc2_activate_setup(handle);
+		if (ret != USBD_OK) {
+			return ret;
+		}
+
+		mmio_clrbits_32(usb_base_addr + OTG_GUSBCFG, OTG_GUSBCFG_TRDT);
+
+		mmio_setbits_32(usb_base_addr + OTG_GUSBCFG,
+				(USBD_HS_TRDT_VALUE << OTG_GUSBCFG_TRDT_SHIFT) & OTG_GUSBCFG_TRDT);
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_ENUMDNE);
+
+		return USB_ENUM_DONE;
+	}
+
+	/* Handle RXQLevel interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_RXFLVL) != 0U) {
+		mmio_clrbits_32(usb_base_addr + OTG_GINTMSK,
+				OTG_GINTSTS_RXFLVL);
+
+		temp = mmio_read_32(usb_base_addr + OTG_GRXSTSP);
+
+		*param = temp & OTG_GRXSTSP_EPNUM;
+		*param |= (temp & OTG_GRXSTSP_BCNT) << (USBD_OUT_COUNT_SHIFT -
+							OTG_GRXSTSP_BCNT_SHIFT);
+
+		if (((temp & OTG_GRXSTSP_PKTSTS) >> OTG_GRXSTSP_PKTSTS_SHIFT) == STS_DATA_UPDT) {
+			if ((temp & OTG_GRXSTSP_BCNT) != 0U) {
+				mmio_setbits_32(usb_base_addr + OTG_GINTMSK, OTG_GINTSTS_RXFLVL);
+
+				return USB_READ_DATA_PACKET;
+			}
+		} else if (((temp & OTG_GRXSTSP_PKTSTS) >> OTG_GRXSTSP_PKTSTS_SHIFT) ==
+			    STS_SETUP_UPDT) {
+			mmio_setbits_32(usb_base_addr + OTG_GINTMSK, OTG_GINTSTS_RXFLVL);
+
+			return USB_READ_SETUP_PACKET;
+		}
+
+		mmio_setbits_32(usb_base_addr + OTG_GINTMSK, OTG_GINTSTS_RXFLVL);
+	}
+
+	/* Handle SOF interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_SOF) != 0U) {
+		INFO("handle USB : SOF\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_SOF);
+
+		return USB_SOF;
+	}
+
+	/* Handle incomplete ISO IN interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_IISOIXFR) != 0U) {
+		INFO("handle USB : ISO IN\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS,
+			      OTG_GINTSTS_IISOIXFR);
+	}
+
+	/* Handle incomplete ISO OUT interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_IPXFR_INCOMPISOOUT) !=
+	    0U) {
+		INFO("handle USB : ISO OUT\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS,
+			      OTG_GINTSTS_IPXFR_INCOMPISOOUT);
+	}
+
+	/* Handle connection event interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_SRQINT) != 0U) {
+		INFO("handle USB : Connect\n");
+
+		mmio_write_32(usb_base_addr + OTG_GINTSTS, OTG_GINTSTS_SRQINT);
+	}
+
+	/* Handle disconnection event interrupt */
+	if ((usb_dwc2_read_int(handle) & OTG_GINTSTS_OTGINT) != 0U) {
+		INFO("handle USB : Disconnect\n");
+
+		temp = mmio_read_32(usb_base_addr + OTG_GOTGINT);
+
+		if ((temp & OTG_GOTGINT_SEDET) == OTG_GOTGINT_SEDET) {
+			return USB_DISCONNECT;
+		}
+	}
+
+	return USB_NOTHING;
+}
+
+/*
+ * @brief  Start the usb device mode
+ * @param  usb_core_handle: USB core driver handle.
+ * @retval None.
+ * return  USB status.
+ */
+static usb_status_t usb_dwc2_start_device(void *handle)
+{
+	uintptr_t usb_base_addr = (uintptr_t)handle;
+
+	mmio_clrbits_32(usb_base_addr + OTG_DCTL, OTG_DCTL_SDIS);
+	mmio_setbits_32(usb_base_addr + OTG_GAHBCFG, OTG_GAHBCFG_GINT);
+
+	return USBD_OK;
+}
+
+static const usb_driver_t usb_dwc2driver = {
+	.ep0_out_start = usb_dwc2_ep0_out_start,
+	.ep_start_xfer = usb_dwc2_ep_start_xfer,
+	.ep0_start_xfer = usb_dwc2_ep0_start_xfer,
+	.write_packet = usb_dwc2_write_packet,
+	.read_packet = usb_dwc2_read_packet,
+	.ep_set_stall = usb_dwc2_ep_set_stall,
+	.start_device = usb_dwc2_start_device,
+	.stop_device = usb_dwc2_stop_device,
+	.set_address = usb_dwc2_set_address,
+	.write_empty_tx_fifo = usb_dwc2_write_empty_tx_fifo,
+	.it_handler = usb_dwc2_it_handler
+};
+
+/*
+ * @brief  Initialize USB DWC2 driver.
+ * @param  usb_core_handle: USB core driver handle.
+ * @param  pcd_handle: PCD handle.
+ * @param  base_register: USB global register base address.
+ * @retval None.
+ */
+void usb_dwc2_init_driver(usb_handle_t *usb_core_handle,
+			  pcd_handle_t *pcd_handle,
+			  void *base_register)
+{
+	register_usb_driver(usb_core_handle, pcd_handle, &usb_dwc2driver,
+			    base_register);
+}
diff --git a/fdts/stm32mp15-bl2.dtsi b/fdts/stm32mp15-bl2.dtsi
new file mode 100644
index 0000000000..4e3573fb12
--- /dev/null
+++ b/fdts/stm32mp15-bl2.dtsi
@@ -0,0 +1,116 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2020 - All Rights Reserved
+ */
+
+/ {
+#if !(STM32MP_EMMC || STM32MP_SDMMC)
+	aliases {
+		/delete-property/ mmc0;
+	};
+#endif
+
+	cpus {
+		/delete-node/ cpu@1;
+	};
+
+	/delete-node/ psci;
+
+	soc {
+		/delete-node/ timer@40006000;
+		/delete-node/ timer@44006000;
+		/delete-node/ pwr_mcu@50001014;
+		/delete-node/ cryp@54001000;
+		/delete-node/ rng@54003000;
+		/delete-node/ spi@5c001000;
+		/delete-node/ rtc@5c004000;
+		/delete-node/ etzpc@5c007000;
+		/delete-node/ stgen@5c008000;
+		/delete-node/ i2c@5c009000;
+		/delete-node/ tamp@5c00a000;
+#if !(STM32MP_EMMC || STM32MP_SDMMC)
+		/delete-node/ sdmmc@58005000;
+		/delete-node/ sdmmc@58007000;
+#endif
+#if !STM32MP_RAW_NAND
+		/delete-node/ memory-controller@58002000;
+#endif
+#if !(STM32MP_SPI_NAND || STM32MP_SPI_NOR)
+		/delete-node/ spi@58003000;
+#endif
+#if !STM32MP_USB_PROGRAMMER
+		/delete-node/ usb-otg@49000000;
+		/delete-node/ usbphyc@5a006000;
+#endif
+
+		pin-controller@50002000 {
+			/delete-node/ rtc-out2-rmp-pins-0;
+#if !(STM32MP_EMMC || STM32MP_SDMMC)
+			/delete-node/ sdmmc1-b4-0;
+			/delete-node/ sdmmc1-dir-0;
+			/delete-node/ sdmmc2-b4-0;
+			/delete-node/ sdmmc2-b4-1;
+			/delete-node/ sdmmc2-d47-0;
+#endif
+#if !STM32MP_RAW_NAND
+			/delete-node/ fmc-0;
+#endif
+#if !(STM32MP_SPI_NAND || STM32MP_SPI_NOR)
+			/delete-node/ qspi-clk-0;
+			/delete-node/ qspi-bk1-0;
+			/delete-node/ qspi-bk2-0;
+#endif
+#if !STM32MP_USB_PROGRAMMER
+			/delete-node/ usbotg_hs-0;
+			/delete-node/ usbotg-fs-dp-dm-0;
+#endif
+		};
+	};
+
+#if !STM32MP_USE_STM32IMAGE
+	/*
+	 * UUID's here are UUID RFC 4122 compliant meaning fieds are stored in
+	 * network order (big endian)
+	 */
+
+	st-io_policies {
+		fip-handles {
+			compatible = "st,io-fip-handle";
+			fw_cfg_uuid = <0x5807e16a 0x845947be 0x8ed5648e 0x8dddab0e>;
+			bl32_uuid = <0x05d0e189 0x53dc1347 0x8d2b500a 0x4b7a3e38>;
+			bl32_extra1_uuid = <0x0b70c29b 0x2a5a7840 0x9f650a56 0x82738288>;
+			bl32_extra2_uuid = <0x8ea87bb1 0xcfa23f4d 0x85fde7bb 0xa50220d9>;
+			bl33_uuid = <0xd6d0eea7 0xfcead54b 0x97829934 0xf234b6e4>;
+			hw_cfg_uuid = <0x08b8f1d9 0xc9cf9349 0xa9626fbc 0x6b7265cc>;
+			tos_fw_cfg_uuid = <0x26257c1a 0xdbc67f47 0x8d96c4c4 0xb0248021>;
+			nt_fw_cfg_uuid = <0x28da9815 0x93e87e44 0xac661aaf 0x801550f9>;
+#if TRUSTED_BOARD_BOOT
+			t_key_cert_uuid = <0x827ee890 0xf860e411 0xa1b4777a 0x21b4f94c>;
+			t_boot_fw_cert_uuid = <0xd6e269ea 0x5d63e411 0x8d8c9fba 0xbe9956a5>;
+			tos_fw_key_cert_uuid = <0x9477d603 0xfb60e411 0x85ddb710 0x5b8cee04>;
+			nt_fw_key_cert_uuid = <0x8ad5832a 0xfb60e411 0x8aafdf30 0xbbc49859>;
+			tos_fw_content_cert_uuid = <0xa49f4411 0x5e63e411 0x87283f05 0x722af33d>;
+			nt_fw_content_cert_uuid = <0x8ec4c1f3 0x5d63e411 0xa7a987ee 0x40b23fa7>;
+#endif
+		};
+	};
+
+#if TRUSTED_BOARD_BOOT
+	tb_fw-config {
+		compatible = "arm,tb_fw";
+
+		/* Disable authentication for development */
+		disable_auth = <0x0>;
+
+		/*
+		 * The following two entries are placeholders for Mbed TLS
+		 * heap information.
+		 */
+		mbedtls_heap_addr = <0x0 0x0>;
+		mbedtls_heap_size = <0x0>;
+	};
+
+#include "cot_descriptors.dtsi"
+#endif
+#endif /* !STM32MP_USE_STM32IMAGE */
+};
diff --git a/fdts/stm32mp15-bl32.dtsi b/fdts/stm32mp15-bl32.dtsi
new file mode 100644
index 0000000000..d237f0d275
--- /dev/null
+++ b/fdts/stm32mp15-bl32.dtsi
@@ -0,0 +1,42 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2020 - All Rights Reserved
+ */
+
+/ {
+	aliases {
+		/delete-property/ mmc0;
+	};
+
+	cpus {
+		/delete-node/ cpu@1;
+	};
+
+	/delete-node/ psci;
+
+	soc {
+		/delete-node/ usb-otg@49000000;
+		/delete-node/ hash@54002000;
+		/delete-node/ memory-controller@58002000;
+		/delete-node/ spi@58003000;
+		/delete-node/ sdmmc@58005000;
+		/delete-node/ sdmmc@58007000;
+		/delete-node/ spi@5c001000;
+		/delete-node/ stgen@5c008000;
+		/delete-node/ i2c@5c009000;
+
+		pin-controller@50002000 {
+			/delete-node/ fmc-0;
+			/delete-node/ qspi-clk-0;
+			/delete-node/ qspi-bk1-0;
+			/delete-node/ qspi-bk2-0;
+			/delete-node/ sdmmc1-b4-0;
+			/delete-node/ sdmmc1-dir-0;
+			/delete-node/ sdmmc2-b4-0;
+			/delete-node/ sdmmc2-b4-1;
+			/delete-node/ sdmmc2-d47-0;
+			/delete-node/ usbotg_hs-0;
+			/delete-node/ usbotg-fs-dp-dm-0;
+		};
+	};
+};
diff --git a/fdts/stm32mp15-ddr-1g-fw-config.dts b/fdts/stm32mp15-ddr-1g-fw-config.dts
new file mode 100644
index 0000000000..c871463062
--- /dev/null
+++ b/fdts/stm32mp15-ddr-1g-fw-config.dts
@@ -0,0 +1,63 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#include <platform_def.h>
+
+#include <common/tbbr/tbbr_img_def.h>
+#include <dt-bindings/soc/stm32mp1-tzc400.h>
+
+/dts-v1/;
+
+/ {
+	dtb-registry {
+		compatible = "fconf,dyn_cfg-dtb_registry";
+
+		hw-config {
+			load-address = <0x0 STM32MP_HW_CONFIG_BASE>;
+			max-size = <STM32MP_HW_CONFIG_MAX_SIZE>;
+			id = <HW_CONFIG_ID>;
+		};
+
+		nt_fw {
+			load-address = <0x0 STM32MP_BL33_BASE>;
+			max-size = <STM32MP_BL33_MAX_SIZE>;
+			id = <BL33_IMAGE_ID>;
+		};
+
+#ifdef AARCH32_SP_OPTEE
+		tos_fw {
+			load-address = <0x0 0x2FFC0000>;
+			max-size = <0x0001F000>;
+			id = <BL32_IMAGE_ID>;
+		};
+#else
+		tos_fw {
+			load-address = <0x0 STM32MP_BL32_BASE>;
+			max-size = <STM32MP_BL32_SIZE>;
+			id = <BL32_IMAGE_ID>;
+		};
+
+		tos_fw-config {
+			load-address = <0x0 STM32MP_BL32_DTB_BASE>;
+			max-size = <STM32MP_BL32_DTB_SIZE>;
+			id = <TOS_FW_CONFIG_ID>;
+		};
+#endif
+	};
+
+	st-mem-firewall {
+		compatible = "st,mem-firewall";
+#ifdef AARCH32_SP_OPTEE
+		memory-ranges = <
+			0xc0000000 0x3e000000 TZC_REGION_S_NONE TZC_REGION_NSEC_ALL_ACCESS_RDWR
+			0xfe000000 0x01e00000 TZC_REGION_S_RDWR 0
+			0xffe00000 0x00200000 TZC_REGION_S_NONE
+			TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_A7_ID)>;
+#else
+		memory-ranges = <
+			0xc0000000 0x40000000 TZC_REGION_S_NONE TZC_REGION_NSEC_ALL_ACCESS_RDWR>;
+#endif
+	};
+};
diff --git a/fdts/stm32mp15-ddr-512m-fw-config.dts b/fdts/stm32mp15-ddr-512m-fw-config.dts
new file mode 100644
index 0000000000..3d0722181a
--- /dev/null
+++ b/fdts/stm32mp15-ddr-512m-fw-config.dts
@@ -0,0 +1,63 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#include <platform_def.h>
+
+#include <common/tbbr/tbbr_img_def.h>
+#include <dt-bindings/soc/stm32mp1-tzc400.h>
+
+/dts-v1/;
+
+/ {
+	dtb-registry {
+		compatible = "fconf,dyn_cfg-dtb_registry";
+
+		hw-config {
+			load-address = <0x0 STM32MP_HW_CONFIG_BASE>;
+			max-size = <STM32MP_HW_CONFIG_MAX_SIZE>;
+			id = <HW_CONFIG_ID>;
+		};
+
+		nt_fw {
+			load-address = <0x0 STM32MP_BL33_BASE>;
+			max-size = <STM32MP_BL33_MAX_SIZE>;
+			id = <BL33_IMAGE_ID>;
+		};
+
+#ifdef AARCH32_SP_OPTEE
+		tos_fw {
+			load-address = <0x0 0x2FFC0000>;
+			max-size = <0x0001F000>;
+			id = <BL32_IMAGE_ID>;
+		};
+#else
+		tos_fw {
+			load-address = <0x0 STM32MP_BL32_BASE>;
+			max-size = <STM32MP_BL32_SIZE>;
+			id = <BL32_IMAGE_ID>;
+		};
+
+		tos_fw-config {
+			load-address = <0x0 STM32MP_BL32_DTB_BASE>;
+			max-size = <STM32MP_BL32_DTB_SIZE>;
+			id = <TOS_FW_CONFIG_ID>;
+		};
+#endif
+	};
+
+	st-mem-firewall {
+		compatible = "st,mem-firewall";
+#ifdef AARCH32_SP_OPTEE
+		memory-ranges = <
+			0xc0000000 0x1e000000 TZC_REGION_S_NONE TZC_REGION_NSEC_ALL_ACCESS_RDWR
+			0xde000000 0x01e00000 TZC_REGION_S_RDWR 0
+			0xdfe00000 0x00200000 TZC_REGION_S_NONE
+			TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_A7_ID)>;
+#else
+		memory-ranges = <
+			0xc0000000 0x20000000 TZC_REGION_S_NONE TZC_REGION_NSEC_ALL_ACCESS_RDWR>;
+#endif
+	};
+};
diff --git a/fdts/stm32mp15-ddr.dtsi b/fdts/stm32mp15-ddr.dtsi
index 4825691f90..734943c0ea 100644
--- a/fdts/stm32mp15-ddr.dtsi
+++ b/fdts/stm32mp15-ddr.dtsi
@@ -14,14 +14,18 @@
 
 			clocks = <&rcc AXIDCG>,
 				 <&rcc DDRC1>,
+#if STM32MP_DDR_DUAL_AXI_PORT
 				 <&rcc DDRC2>,
+#endif
 				 <&rcc DDRPHYC>,
 				 <&rcc DDRCAPB>,
 				 <&rcc DDRPHYCAPB>;
 
 			clock-names = "axidcg",
 				      "ddrc1",
+#if STM32MP_DDR_DUAL_AXI_PORT
 				      "ddrc2",
+#endif
 				      "ddrphyc",
 				      "ddrcapb",
 				      "ddrphycapb";
@@ -97,12 +101,14 @@
 				DDR_PCFGQOS1_0
 				DDR_PCFGWQOS0_0
 				DDR_PCFGWQOS1_0
+#if STM32MP_DDR_DUAL_AXI_PORT
 				DDR_PCFGR_1
 				DDR_PCFGW_1
 				DDR_PCFGQOS0_1
 				DDR_PCFGQOS1_1
 				DDR_PCFGWQOS0_1
 				DDR_PCFGWQOS1_1
+#endif
 			>;
 
 			st,phy-reg = <
@@ -115,8 +121,10 @@
 				DDR_ZQ0CR1
 				DDR_DX0GCR
 				DDR_DX1GCR
+#if STM32MP_DDR_DUAL_AXI_PORT
 				DDR_DX2GCR
 				DDR_DX3GCR
+#endif
 			>;
 
 			st,phy-timing = <
@@ -132,6 +140,7 @@
 				DDR_MR3
 			>;
 
+#ifdef DDR_PHY_CAL_SKIP
 			st,phy-cal = <
 				DDR_DX0DLLCR
 				DDR_DX0DQTR
@@ -139,13 +148,16 @@
 				DDR_DX1DLLCR
 				DDR_DX1DQTR
 				DDR_DX1DQSTR
+#if STM32MP_DDR_DUAL_AXI_PORT
 				DDR_DX2DLLCR
 				DDR_DX2DQTR
 				DDR_DX2DQSTR
 				DDR_DX3DLLCR
 				DDR_DX3DQTR
 				DDR_DX3DQSTR
+#endif
 			>;
+#endif
 
 			status = "okay";
 		};
diff --git a/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi b/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi
index c0fc1f772e..127053b86a 100644
--- a/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi
+++ b/fdts/stm32mp15-ddr3-1x4Gb-1066-binG.dtsi
@@ -61,13 +61,13 @@
 #define DDR_DBGCMD 0x00000000
 #define DDR_POISONCFG 0x00000000
 #define DDR_PCCFG 0x00000010
-#define DDR_PCFGR_0 0x00010000
+#define DDR_PCFGR_0 0x00000000
 #define DDR_PCFGW_0 0x00000000
 #define DDR_PCFGQOS0_0 0x02100C03
 #define DDR_PCFGQOS1_0 0x00800100
 #define DDR_PCFGWQOS0_0 0x01100C03
 #define DDR_PCFGWQOS1_0 0x01000200
-#define DDR_PCFGR_1 0x00010000
+#define DDR_PCFGR_1 0x00000000
 #define DDR_PCFGW_1 0x00000000
 #define DDR_PCFGQOS0_1 0x02100C03
 #define DDR_PCFGQOS1_1 0x00800040
diff --git a/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi b/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi
index fc226d2544..5ae861fee1 100644
--- a/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi
+++ b/fdts/stm32mp15-ddr3-2x4Gb-1066-binG.dtsi
@@ -61,13 +61,13 @@
 #define DDR_DBGCMD 0x00000000
 #define DDR_POISONCFG 0x00000000
 #define DDR_PCCFG 0x00000010
-#define DDR_PCFGR_0 0x00010000
+#define DDR_PCFGR_0 0x00000000
 #define DDR_PCFGW_0 0x00000000
 #define DDR_PCFGQOS0_0 0x02100C03
 #define DDR_PCFGQOS1_0 0x00800100
 #define DDR_PCFGWQOS0_0 0x01100C03
 #define DDR_PCFGWQOS1_0 0x01000200
-#define DDR_PCFGR_1 0x00010000
+#define DDR_PCFGR_1 0x00000000
 #define DDR_PCFGW_1 0x00000000
 #define DDR_PCFGQOS0_1 0x02100C03
 #define DDR_PCFGQOS1_1 0x00800040
diff --git a/fdts/stm32mp15-pinctrl.dtsi b/fdts/stm32mp15-pinctrl.dtsi
index d3d1744ec4..16c40cbff9 100644
--- a/fdts/stm32mp15-pinctrl.dtsi
+++ b/fdts/stm32mp15-pinctrl.dtsi
@@ -194,33 +194,89 @@
 
 	uart7_pins_a: uart7-0 {
 		pins1 {
-			pinmux = <STM32_PINMUX('E', 8, AF7)>; /* UART4_TX */
+			pinmux = <STM32_PINMUX('E', 8, AF7)>; /* UART7_TX */
 			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 */
+			pinmux = <STM32_PINMUX('E', 7, AF7)>, /* UART7_RX */
+				 <STM32_PINMUX('E', 10, AF7)>, /* UART7_CTS */
+				 <STM32_PINMUX('E', 9, AF7)>; /* UART7_RTS */
 			bias-disable;
 		};
 	};
 
 	uart7_pins_b: uart7-1 {
 		pins1 {
-			pinmux = <STM32_PINMUX('E', 8, AF7)>; /* USART7_TX */
+			pinmux = <STM32_PINMUX('F', 7, AF7)>; /* UART7_TX */
 			bias-disable;
 			drive-push-pull;
 			slew-rate = <0>;
 		};
 		pins2 {
-			pinmux = <STM32_PINMUX('E', 7, AF7)>; /* USART7_RX */
+			pinmux = <STM32_PINMUX('F', 6, AF7)>; /* UART7_RX */
+			bias-disable;
+		};
+	};
+
+	uart7_pins_c: uart7-2 {
+		pins1 {
+			pinmux = <STM32_PINMUX('E', 8, AF7)>; /* UART7_TX */
+			bias-disable;
+			drive-push-pull;
+			slew-rate = <0>;
+		};
+		pins2 {
+			pinmux = <STM32_PINMUX('E', 7, AF7)>; /* UART7_RX */
+			bias-pull-up;
+		};
+	};
+
+	uart8_pins_a: uart8-0 {
+		pins1 {
+			pinmux = <STM32_PINMUX('E', 1, AF8)>; /* UART8_TX */
+			bias-disable;
+			drive-push-pull;
+			slew-rate = <0>;
+		};
+		pins2 {
+			pinmux = <STM32_PINMUX('E', 0, AF8)>; /* UART8_RX */
 			bias-disable;
 		};
 	};
 
 	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;
+		};
+	};
+
+	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;
+		};
+	};
+
+	usart2_pins_c: usart2-2 {
 		pins1 {
 			pinmux = <STM32_PINMUX('D', 5, AF7)>, /* USART2_TX */
 				 <STM32_PINMUX('D', 4, AF7)>; /* USART2_RTS */
@@ -236,6 +292,19 @@
 	};
 
 	usart3_pins_a: usart3-0 {
+		pins1 {
+			pinmux = <STM32_PINMUX('B', 10, AF7)>; /* USART3_TX */
+			bias-disable;
+			drive-push-pull;
+			slew-rate = <0>;
+		};
+		pins2 {
+			pinmux = <STM32_PINMUX('B', 12, AF8)>; /* USART3_RX */
+			bias-disable;
+		};
+	};
+
+	usart3_pins_b: usart3-1 {
 		pins1 {
 			pinmux = <STM32_PINMUX('B', 10, AF7)>, /* USART3_TX */
 				 <STM32_PINMUX('G', 8, AF8)>; /* USART3_RTS */
@@ -250,7 +319,7 @@
 		};
 	};
 
-	usart3_pins_b: usart3-1 {
+	usart3_pins_c: usart3-2 {
 		pins1 {
 			pinmux = <STM32_PINMUX('B', 10, AF7)>, /* USART3_TX */
 				 <STM32_PINMUX('G', 8, AF8)>; /* USART3_RTS */
@@ -261,11 +330,11 @@
 		pins2 {
 			pinmux = <STM32_PINMUX('B', 12, AF8)>, /* USART3_RX */
 				 <STM32_PINMUX('B', 13, AF7)>; /* USART3_CTS_NSS */
-			bias-disable;
+			bias-pull-up;
 		};
 	};
 
-	usbotg_hs_pins_a: usbotg_hs-0 {
+	usbotg_hs_pins_a: usbotg-hs-0 {
 		pins {
 			pinmux = <STM32_PINMUX('A', 10, ANALOG)>; /* OTG_ID */
 		};
diff --git a/fdts/stm32mp15-ssp-bl2.dtsi b/fdts/stm32mp15-ssp-bl2.dtsi
new file mode 100644
index 0000000000..107f6e2166
--- /dev/null
+++ b/fdts/stm32mp15-ssp-bl2.dtsi
@@ -0,0 +1,125 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) STMicroelectronics 2020 - All Rights Reserved
+ */
+
+/ {
+	cpus {
+		/delete-node/ cpu@1;
+	};
+
+#if STM32MP_USB_PROGRAMMER
+	aliases {
+		/delete-property/ serial1;
+	};
+#endif
+
+	/delete-node/ cpu0_opp_table;
+
+	nvmem_layout@0 {
+	/delete-property/ nvmem-cells;
+	/delete-property/ nvmem-cell-names;
+
+	nvmem-cells = <&cfg0_otp>,
+		      <&part_number_otp>,
+		      <&monotonic_otp>,
+		      <&nand_otp>,
+		      <&uid_otp>,
+		      <&package_otp>,
+		      <&hw2_otp>,
+		      <&pkh_otp>,
+		      <&cfg2_otp>,
+		      <&ssp_otp>,
+		      <&chip_otp>,
+		      <&rma_otp>;
+
+	nvmem-cell-names = "cfg0_otp",
+			   "part_number_otp",
+			   "monotonic_otp",
+			   "nand_otp",
+			   "uid_otp",
+			   "package_otp",
+			   "hw2_otp",
+			   "pkh_otp",
+			   "cfg2_otp",
+			   "ssp_otp",
+			   "chip_otp",
+			   "rma_otp";
+	};
+
+	/delete-node/ psci;
+
+	soc {
+		efuse@5c005000 {
+			cfg2_otp: cfg2_otp@8 {
+				reg = <0x8 0x4>;
+			};
+
+			ssp_otp: ssp_otp@20 {
+				reg = <0x20 0x4>;
+			};
+
+			chip_otp: chip_otp@a0 {
+				reg = <0xa0 0x40>;
+			};
+
+			rma_otp: rma_otp@e0 {
+				reg = <0xe0 0x4>;
+			};
+		};
+
+		/delete-node/ timer@40006000;
+		/delete-node/ timer@44006000;
+		/delete-node/ pwr_mcu@50001014;
+		/delete-node/ cryp@54001000;
+		/delete-node/ rng@54003000;
+		/delete-node/ memory-controller@58002000;
+		/delete-node/ spi@58003000;
+		/delete-node/ sdmmc@58005000;
+		/delete-node/ sdmmc@58007000;
+		/delete-node/ ddr@5a003000;
+		/delete-node/ spi@5c001000;
+		/delete-node/ rtc@5c004000;
+		/delete-node/ etzpc@5c007000;
+		/delete-node/ stgen@5c008000;
+		/delete-node/ i2c@5c009000;
+		/delete-node/ tamp@5c00a000;
+#if STM32MP_USB_PROGRAMMER
+		/delete-node/ serial@4000e000;
+		/delete-node/ serial@4000f000;
+		/delete-node/ serial@40011000;
+		/delete-node/ serial@40018000;
+		/delete-node/ serial@40019000;
+		/delete-node/ serial@44003000;
+		/delete-node/ serial@5c000000;
+#endif
+#if STM32MP_UART_PROGRAMMER
+		/delete-node/ usb-otg@49000000;
+		/delete-node/ usbphyc@5a006000;
+#endif
+
+		pin-controller@50002000 {
+			/delete-node/ fmc-0;
+			/delete-node/ qspi-clk-0;
+			/delete-node/ qspi-bk1-0;
+			/delete-node/ qspi-bk2-0;
+			/delete-node/ rtc-out2-rmp-pins-0;
+			/delete-node/ sdmmc1-b4-0;
+			/delete-node/ sdmmc1-dir-0;
+			/delete-node/ sdmmc2-b4-0;
+			/delete-node/ sdmmc2-b4-1;
+			/delete-node/ sdmmc2-d47-0;
+#if STM32MP_USB_PROGRAMMER
+			/delete-node/ uart7-0;
+			/delete-node/ uart7-1;
+			/delete-node/ usart2-0;
+			/delete-node/ usart3-0;
+			/delete-node/ usart3-1;
+#endif
+#if STM32MP_UART_PROGRAMMER
+			/delete-node/ usbotg_hs-0;
+			/delete-node/ usbotg-fs-dp-dm-0;
+#endif
+		};
+	};
+};
diff --git a/fdts/stm32mp151.dtsi b/fdts/stm32mp151.dtsi
index 8f175a6492..714d94710f 100644
--- a/fdts/stm32mp151.dtsi
+++ b/fdts/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";
@@ -82,12 +114,13 @@
 			clocks = <&rcc TIM12_K>;
 			clock-names = "int";
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		usart2: serial@4000e000 {
 			compatible = "st,stm32h7-uart";
 			reg = <0x4000e000 0x400>;
-			interrupts-extended = <&exti 27 IRQ_TYPE_LEVEL_HIGH>;
+			interrupts = <GIC_SPI 38 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc USART2_K>;
 			resets = <&rcc USART2_R>;
 			status = "disabled";
@@ -96,7 +129,7 @@
 		usart3: serial@4000f000 {
 			compatible = "st,stm32h7-uart";
 			reg = <0x4000f000 0x400>;
-			interrupts-extended = <&exti 28 IRQ_TYPE_LEVEL_HIGH>;
+			interrupts = <GIC_SPI 39 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc USART3_K>;
 			resets = <&rcc USART3_R>;
 			status = "disabled";
@@ -115,7 +148,7 @@
 		uart5: serial@40011000 {
 			compatible = "st,stm32h7-uart";
 			reg = <0x40011000 0x400>;
-			interrupts-extended = <&exti 31 IRQ_TYPE_LEVEL_HIGH>;
+			interrupts = <GIC_SPI 53 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc UART5_K>;
 			resets = <&rcc UART5_R>;
 			status = "disabled";
@@ -124,7 +157,7 @@
 		uart7: serial@40018000 {
 			compatible = "st,stm32h7-uart";
 			reg = <0x40018000 0x400>;
-			interrupts-extended = <&exti 32 IRQ_TYPE_LEVEL_HIGH>;
+			interrupts = <GIC_SPI 82 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc UART7_K>;
 			resets = <&rcc UART7_R>;
 			status = "disabled";
@@ -133,7 +166,7 @@
 		uart8: serial@40019000 {
 			compatible = "st,stm32h7-uart";
 			reg = <0x40019000 0x400>;
-			interrupts-extended = <&exti 33 IRQ_TYPE_LEVEL_HIGH>;
+			interrupts = <GIC_SPI 83 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc UART8_K>;
 			resets = <&rcc UART8_R>;
 			status = "disabled";
@@ -142,7 +175,7 @@
 		usart6: serial@44003000 {
 			compatible = "st,stm32h7-uart";
 			reg = <0x44003000 0x400>;
-			interrupts-extended = <&exti 29 IRQ_TYPE_LEVEL_HIGH>;
+			interrupts = <GIC_SPI 71 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc USART6_K>;
 			resets = <&rcc USART6_R>;
 			status = "disabled";
@@ -156,16 +189,17 @@
 			clocks = <&rcc TIM15_K>;
 			clock-names = "int";
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		usbotg_hs: usb-otg@49000000 {
-			compatible = "st,stm32mp1-hsotg", "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-extended = <&exti 44 IRQ_TYPE_LEVEL_HIGH>;
+			interrupts = <GIC_SPI 98 IRQ_TYPE_LEVEL_HIGH>;
 			g-rx-fifo-size = <512>;
 			g-np-tx-fifo-size = <32>;
 			g-tx-fifo-size = <256 16 16 16 16 16 16 16>;
@@ -175,7 +209,7 @@
 		};
 
 		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>;
@@ -254,6 +288,7 @@
 			clocks = <&rcc HASH1>;
 			resets = <&rcc HASH1_R>;
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		rng1: rng@54003000 {
@@ -262,6 +297,7 @@
 			clocks = <&rcc RNG1_K>;
 			resets = <&rcc RNG1_R>;
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		fmc: memory-controller@58002000 {
@@ -301,6 +337,8 @@
 			interrupts = <GIC_SPI 92 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&rcc QSPI_K>;
 			resets = <&rcc QSPI_R>;
+			#address-cells = <1>;
+			#size-cells = <0>;
 			status = "disabled";
 		};
 
@@ -341,6 +379,7 @@
 			clocks = <&rcc IWDG2>, <&rcc CK_LSI>;
 			clock-names = "pclk", "lsi";
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		usbphyc: usbphyc@5a006000 {
@@ -373,6 +412,7 @@
 			clocks = <&rcc USART1_K>;
 			resets = <&rcc USART1_R>;
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		spi6: spi@5c001000 {
@@ -384,6 +424,7 @@
 			clocks = <&rcc SPI6_K>;
 			resets = <&rcc SPI6_R>;
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		i2c4: i2c@5c002000 {
@@ -399,6 +440,7 @@
 			st,syscfg-fmp = <&syscfg 0x4 0x8>;
 			wakeup-source;
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		iwdg1: watchdog@5c003000 {
@@ -408,6 +450,7 @@
 			clocks = <&rcc IWDG1>, <&rcc CK_LSI>;
 			clock-names = "pclk", "lsi";
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		rtc: rtc@5c004000 {
@@ -417,19 +460,49 @@
 			clock-names = "pclk", "rtc_ck";
 			interrupts-extended = <&exti 19 IRQ_TYPE_LEVEL_HIGH>;
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
-		bsec: nvmem@5c005000 {
+		bsec: efuse@5c005000 {
 			compatible = "st,stm32mp15-bsec";
 			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>;
 			};
+			pkh_otp: pkh_otp@60 {
+				reg = <0x60 0x20>;
+			};
+			ethernet_mac_address: mac@e4 {
+				reg = <0xe4 0x8>;
+				st,non-secure-otp;
+			};
 		};
 
 		etzpc: etzpc@5c007000 {
@@ -458,6 +531,7 @@
 			st,syscfg-fmp = <&syscfg 0x4 0x20>;
 			wakeup-source;
 			status = "disabled";
+			secure-status = "disabled";
 		};
 
 		tamp: tamp@5c00a000 {
@@ -621,6 +695,7 @@
 				st,bank-name = "GPIOZ";
 				st,bank-ioport = <11>;
 				status = "disabled";
+				secure-status = "disabled";
 			};
 		};
 	};
diff --git a/fdts/stm32mp153.dtsi b/fdts/stm32mp153.dtsi
index 0a0bb8dc19..617380a52f 100644
--- a/fdts/stm32mp153.dtsi
+++ b/fdts/stm32mp153.dtsi
@@ -14,6 +14,7 @@
 			reg = <1>;
 			clocks = <&rcc CK_MPU>;
 			clock-names = "cpu";
+			operating-points-v2 = <&cpu0_opp_table>;
 		};
 	};
 };
diff --git a/fdts/stm32mp157a-avenger96-fw-config.dts b/fdts/stm32mp157a-avenger96-fw-config.dts
new file mode 100644
index 0000000000..10f9402c4a
--- /dev/null
+++ b/fdts/stm32mp157a-avenger96-fw-config.dts
@@ -0,0 +1,6 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#include "stm32mp15-ddr-1g-fw-config.dts"
diff --git a/fdts/stm32mp157a-avenger96.dts b/fdts/stm32mp157a-avenger96.dts
index b967736e47..57c1b02d46 100644
--- a/fdts/stm32mp157a-avenger96.dts
+++ b/fdts/stm32mp157a-avenger96.dts
@@ -10,9 +10,11 @@
 /dts-v1/;
 
 #include "stm32mp157.dtsi"
+#include "stm32mp15xa.dtsi"
 #include "stm32mp15-pinctrl.dtsi"
 #include "stm32mp15xxac-pinctrl.dtsi"
 #include <dt-bindings/clock/stm32mp1-clksrc.h>
+#include <dt-bindings/soc/st,stm32-etzpc.h>
 #include "stm32mp15-ddr3-2x4Gb-1066-binG.dtsi"
 
 / {
@@ -35,6 +37,22 @@
 	};
 };
 
+&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)
+	>;
+};
+
 &i2c4 {
 	pinctrl-names = "default";
 	pinctrl-0 = <&i2c4_pins_a>;
@@ -273,10 +291,12 @@
 
 &rng1 {
 	status = "okay";
+	secure-status = "okay";
 };
 
 &rtc {
 	status = "okay";
+	secure-status = "okay";
 };
 
 &sdmmc1 {
diff --git a/fdts/stm32mp157a-dk1-fw-config.dts b/fdts/stm32mp157a-dk1-fw-config.dts
new file mode 100644
index 0000000000..256d0db935
--- /dev/null
+++ b/fdts/stm32mp157a-dk1-fw-config.dts
@@ -0,0 +1,6 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#include "stm32mp15-ddr-512m-fw-config.dts"
diff --git a/fdts/stm32mp157a-dk1.dts b/fdts/stm32mp157a-dk1.dts
index a73bef8ee4..5d5c0a5f74 100644
--- a/fdts/stm32mp157a-dk1.dts
+++ b/fdts/stm32mp157a-dk1.dts
@@ -7,9 +7,11 @@
 /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";
@@ -25,3 +27,19 @@
 		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/fdts/stm32mp157a-ed1-fw-config.dts b/fdts/stm32mp157a-ed1-fw-config.dts
new file mode 100644
index 0000000000..10f9402c4a
--- /dev/null
+++ b/fdts/stm32mp157a-ed1-fw-config.dts
@@ -0,0 +1,6 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#include "stm32mp15-ddr-1g-fw-config.dts"
diff --git a/fdts/stm32mp157a-ed1.dts b/fdts/stm32mp157a-ed1.dts
new file mode 100644
index 0000000000..1527b642a4
--- /dev/null
+++ b/fdts/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/fdts/stm32mp157a-ev1-fw-config.dts b/fdts/stm32mp157a-ev1-fw-config.dts
new file mode 100644
index 0000000000..10f9402c4a
--- /dev/null
+++ b/fdts/stm32mp157a-ev1-fw-config.dts
@@ -0,0 +1,6 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#include "stm32mp15-ddr-1g-fw-config.dts"
diff --git a/fdts/stm32mp157a-ev1.dts b/fdts/stm32mp157a-ev1.dts
new file mode 100644
index 0000000000..3cb35698ad
--- /dev/null
+++ b/fdts/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/fdts/stm32mp157c-dk2-fw-config.dts b/fdts/stm32mp157c-dk2-fw-config.dts
new file mode 100644
index 0000000000..256d0db935
--- /dev/null
+++ b/fdts/stm32mp157c-dk2-fw-config.dts
@@ -0,0 +1,6 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#include "stm32mp15-ddr-512m-fw-config.dts"
diff --git a/fdts/stm32mp157c-dk2.dts b/fdts/stm32mp157c-dk2.dts
index be8300e9e0..ff5c4509fe 100644
--- a/fdts/stm32mp157c-dk2.dts
+++ b/fdts/stm32mp157c-dk2.dts
@@ -11,6 +11,7 @@
 #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";
@@ -31,3 +32,20 @@
 &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/fdts/stm32mp157c-ed1-fw-config.dts b/fdts/stm32mp157c-ed1-fw-config.dts
new file mode 100644
index 0000000000..10f9402c4a
--- /dev/null
+++ b/fdts/stm32mp157c-ed1-fw-config.dts
@@ -0,0 +1,6 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#include "stm32mp15-ddr-1g-fw-config.dts"
diff --git a/fdts/stm32mp157c-ed1.dts b/fdts/stm32mp157c-ed1.dts
index a6b98b7d93..8d80147518 100644
--- a/fdts/stm32mp157c-ed1.dts
+++ b/fdts/stm32mp157c-ed1.dts
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
 /*
- * Copyright (C) STMicroelectronics 2017-2019 - All Rights Reserved
- * Author: Ludovic Barre <ludovic.barre@st.com> for STMicroelectronics.
+ * 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/clock/stm32mp1-clksrc.h>
-#include "stm32mp15-ddr3-2x4Gb-1066-binG.dtsi"
+#include "stm32mp15xx-edx.dtsi"
+#include <dt-bindings/soc/st,stm32-etzpc.h>
 
 / {
 	model = "STMicroelectronics STM32MP157C eval daughter";
@@ -19,318 +19,25 @@
 	chosen {
 		stdout-path = "serial0:115200n8";
 	};
-
-
-	memory@c0000000 {
-		device_type = "memory";
-		reg = <0xC0000000 0x40000000>;
-	};
-
-	aliases {
-		serial0 = &uart4;
-	};
-};
-
-&bsec {
-	board_id: board_id@ec {
-		reg = <0xec 0x4>;
-		status = "okay";
-		secure-status = "okay";
-	};
-};
-
-&clk_hse {
-	st,digbypass;
-};
-
-&cpu0 {
-	cpu-supply = <&vddcore>;
-};
-
-&cpu1 {
-	cpu-supply = <&vddcore>;
 };
 
 &cryp1 {
-	status="okay";
-};
-
-&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";
-
-	pmic: stpmic@33 {
-		compatible = "st,stpmic1";
-		reg = <0x33>;
-		interrupts-extended = <&exti_pwr 55 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>;
-			};
-
-			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;
-			};
-
-			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>;
-			 };
-		};
-
-		onkey {
-			compatible = "st,stpmic1-onkey";
-			power-off-time-sec = <10>;
-			status = "okay";
-		};
-
-		watchdog {
-			compatible = "st,stpmic1-wdt";
-			status = "disabled";
-		};
-	};
-};
-
-&iwdg2 {
-	timeout-sec = <32>;
 	status = "okay";
 };
 
-&pwr_regulators {
-	vdd-supply = <&vdd>;
-	vdd_3v3_usbfs-supply = <&vdd_usb>;
-};
-
-&rcc {
-	secure-status = "disabled";
-	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
+&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)
 	>;
-
-	/* VCO = 1300.0 MHz => P = 650 (CPU) */
-	pll1: st,pll@0 {
-		cfg = < 2 80 0 0 0 PQR(1,0,0) >;
-		frac = < 0x800 >;
-	};
-
-	/* VCO = 1066.0 MHz => P = 266 (AXI), Q = 533 (GPU), R = 533 (DDR) */
-	pll2: st,pll@1 {
-		cfg = < 2 65 1 0 0 PQR(1,1,1) >;
-		frac = < 0x1400 >;
-	};
-
-	/* VCO = 417.8 MHz => P = 209, Q = 24, R = 11 */
-	pll3: st,pll@2 {
-		cfg = < 1 33 1 16 36 PQR(1,1,1) >;
-		frac = < 0x1a04 >;
-	};
-
-	/* VCO = 594.0 MHz => P = 99, Q = 74, R = 74 */
-	pll4: st,pll@3 {
-		cfg = < 3 98 5 7 7 PQR(1,1,1) >;
-	};
-};
-
-&rng1 {
-	status = "okay";
-};
-
-&rtc {
-	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";
-};
-
-&uart4 {
-	pinctrl-names = "default";
-	pinctrl-0 = <&uart4_pins_a>;
-	status = "okay";
 };
diff --git a/fdts/stm32mp157c-ev1-fw-config.dts b/fdts/stm32mp157c-ev1-fw-config.dts
new file mode 100644
index 0000000000..10f9402c4a
--- /dev/null
+++ b/fdts/stm32mp157c-ev1-fw-config.dts
@@ -0,0 +1,6 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#include "stm32mp15-ddr-1g-fw-config.dts"
diff --git a/fdts/stm32mp157c-ev1.dts b/fdts/stm32mp157c-ev1.dts
index c5d12e3b23..d12d30093c 100644
--- a/fdts/stm32mp157c-ev1.dts
+++ b/fdts/stm32mp157c-ev1.dts
@@ -1,11 +1,12 @@
 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
 /*
- * Copyright (C) STMicroelectronics 2017-2019 - All Rights Reserved
- * Author: Ludovic Barre <ludovic.barre@st.com> for STMicroelectronics.
+ * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
  */
 /dts-v1/;
 
 #include "stm32mp157c-ed1.dts"
+#include "stm32mp15xx-evx.dtsi"
 
 / {
 	model = "STMicroelectronics STM32MP157C eval daughter on eval mother";
@@ -16,47 +17,7 @@
 	};
 
 	aliases {
+		serial0 = &uart4;
 		serial1 = &usart3;
 	};
 };
-
-&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>;
-		};
-	};
-};
-
-&qspi {
-	pinctrl-names = "default";
-	pinctrl-0 = <&qspi_clk_pins_a &qspi_bk1_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>;
-	};
-};
-
-&usart3 {
-	pinctrl-names = "default";
-	pinctrl-0 = <&usart3_pins_a>;
-	status = "disabled";
-};
diff --git a/fdts/stm32mp157d-dk1-fw-config.dts b/fdts/stm32mp157d-dk1-fw-config.dts
new file mode 100644
index 0000000000..256d0db935
--- /dev/null
+++ b/fdts/stm32mp157d-dk1-fw-config.dts
@@ -0,0 +1,6 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#include "stm32mp15-ddr-512m-fw-config.dts"
diff --git a/fdts/stm32mp157d-dk1.dts b/fdts/stm32mp157d-dk1.dts
new file mode 100644
index 0000000000..79297b831f
--- /dev/null
+++ b/fdts/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/fdts/stm32mp157d-ed1-fw-config.dts b/fdts/stm32mp157d-ed1-fw-config.dts
new file mode 100644
index 0000000000..10f9402c4a
--- /dev/null
+++ b/fdts/stm32mp157d-ed1-fw-config.dts
@@ -0,0 +1,6 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#include "stm32mp15-ddr-1g-fw-config.dts"
diff --git a/fdts/stm32mp157d-ed1.dts b/fdts/stm32mp157d-ed1.dts
new file mode 100644
index 0000000000..2c67ec0acb
--- /dev/null
+++ b/fdts/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/fdts/stm32mp157d-ev1-fw-config.dts b/fdts/stm32mp157d-ev1-fw-config.dts
new file mode 100644
index 0000000000..10f9402c4a
--- /dev/null
+++ b/fdts/stm32mp157d-ev1-fw-config.dts
@@ -0,0 +1,6 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#include "stm32mp15-ddr-1g-fw-config.dts"
diff --git a/fdts/stm32mp157d-ev1.dts b/fdts/stm32mp157d-ev1.dts
new file mode 100644
index 0000000000..4a40f5fe51
--- /dev/null
+++ b/fdts/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/fdts/stm32mp157f-dk2-fw-config.dts b/fdts/stm32mp157f-dk2-fw-config.dts
new file mode 100644
index 0000000000..256d0db935
--- /dev/null
+++ b/fdts/stm32mp157f-dk2-fw-config.dts
@@ -0,0 +1,6 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#include "stm32mp15-ddr-512m-fw-config.dts"
diff --git a/fdts/stm32mp157f-dk2.dts b/fdts/stm32mp157f-dk2.dts
new file mode 100644
index 0000000000..680ca0f6e7
--- /dev/null
+++ b/fdts/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/fdts/stm32mp157f-ed1-fw-config.dts b/fdts/stm32mp157f-ed1-fw-config.dts
new file mode 100644
index 0000000000..10f9402c4a
--- /dev/null
+++ b/fdts/stm32mp157f-ed1-fw-config.dts
@@ -0,0 +1,6 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#include "stm32mp15-ddr-1g-fw-config.dts"
diff --git a/fdts/stm32mp157f-ed1.dts b/fdts/stm32mp157f-ed1.dts
new file mode 100644
index 0000000000..1aa26cdbbe
--- /dev/null
+++ b/fdts/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/fdts/stm32mp157f-ev1-fw-config.dts b/fdts/stm32mp157f-ev1-fw-config.dts
new file mode 100644
index 0000000000..10f9402c4a
--- /dev/null
+++ b/fdts/stm32mp157f-ev1-fw-config.dts
@@ -0,0 +1,6 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+#include "stm32mp15-ddr-1g-fw-config.dts"
diff --git a/fdts/stm32mp157f-ev1.dts b/fdts/stm32mp157f-ev1.dts
new file mode 100644
index 0000000000..caf5dfe11a
--- /dev/null
+++ b/fdts/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/fdts/stm32mp15xa.dtsi b/fdts/stm32mp15xa.dtsi
new file mode 100644
index 0000000000..5ed7e594f4
--- /dev/null
+++ b/fdts/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/fdts/stm32mp15xc.dtsi b/fdts/stm32mp15xc.dtsi
index b06a55a2fa..68d822d8c1 100644
--- a/fdts/stm32mp15xc.dtsi
+++ b/fdts/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/fdts/stm32mp15xd.dtsi b/fdts/stm32mp15xd.dtsi
new file mode 100644
index 0000000000..18b05ee380
--- /dev/null
+++ b/fdts/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/fdts/stm32mp15xf.dtsi b/fdts/stm32mp15xf.dtsi
new file mode 100644
index 0000000000..526a1627cf
--- /dev/null
+++ b/fdts/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/fdts/stm32mp15xx-dkx.dtsi b/fdts/stm32mp15xx-dkx.dtsi
index 52b914b84e..e5166706ee 100644
--- a/fdts/stm32mp15xx-dkx.dtsi
+++ b/fdts/stm32mp15xx-dkx.dtsi
@@ -5,6 +5,7 @@
  */
 
 #include <dt-bindings/clock/stm32mp1-clksrc.h>
+#include <dt-bindings/power/stm32mp1-power.h>
 #include "stm32mp15-ddr3-1x4Gb-1066-binG.dtsi"
 
 / {
@@ -52,6 +53,7 @@
 	i2c-scl-falling-time-ns = <20>;
 	clock-frequency = <400000>;
 	status = "okay";
+	secure-status = "okay";
 
 	pmic: stpmic@33 {
 		compatible = "st,stpmic1";
@@ -60,6 +62,7 @@
 		interrupt-controller;
 		#interrupt-cells = <2>;
 		status = "okay";
+		secure-status = "okay";
 
 		regulators {
 			compatible = "st,stpmic1-regulators";
@@ -139,9 +142,6 @@
 
 			vdd_usb: ldo4 {
 				regulator-name = "vdd_usb";
-				regulator-min-microvolt = <3300000>;
-				regulator-max-microvolt = <3300000>;
-				regulator-always-on;
 			};
 
 			vdda: ldo5 {
@@ -182,17 +182,48 @@
 
 &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_STANDBY_DDR_SR
+	>;
+	system_off_soc_mode = <STM32_PM_CSTOP_ALLOW_STANDBY_DDR_OFF>;
 	vdd-supply = <&vdd>;
 	vdd_3v3_usbfs-supply = <&vdd_usb>;
 };
 
 &rcc {
-	secure-status = "disabled";
+	st,hsi-cal;
+	st,csi-cal;
+	st,cal-sec = <60>;
 	st,clksrc = <
 		CLK_MPU_PLL1P
 		CLK_AXI_PLL2P
@@ -257,14 +288,6 @@
 		CLK_LPTIM45_LSE
 	>;
 
-	/* VCO = 1300.0 MHz => P = 650 (CPU) */
-	pll1: st,pll@0 {
-		compatible = "st,stm32mp1-pll";
-		reg = <0>;
-		cfg = < 2 80 0 0 0 PQR(1,0,0) >;
-		frac = < 0x800 >;
-	};
-
 	/* VCO = 1066.0 MHz => P = 266 (AXI), Q = 533 (GPU), R = 533 (DDR) */
 	pll2: st,pll@1 {
 		compatible = "st,stm32mp1-pll";
@@ -291,10 +314,12 @@
 
 &rng1 {
 	status = "okay";
+	secure-status = "okay";
 };
 
 &rtc {
 	status = "okay";
+	secure-status = "okay";
 };
 
 &sdmmc1 {
@@ -309,6 +334,8 @@
 
 &timers15 {
 	secure-status = "okay";
+	st,hsi-cal-input = <7>;
+	st,csi-cal-input = <8>;
 };
 
 &uart4 {
@@ -319,13 +346,13 @@
 
 &uart7 {
 	pinctrl-names = "default";
-	pinctrl-0 = <&uart7_pins_b>;
+	pinctrl-0 = <&uart7_pins_c>;
 	status = "disabled";
 };
 
 &usart3 {
 	pinctrl-names = "default";
-	pinctrl-0 = <&usart3_pins_b>;
+	pinctrl-0 = <&usart3_pins_c>;
 	uart-has-rtscts;
 	status = "disabled";
 };
@@ -348,3 +375,128 @@
 &usbphyc_port1 {
 	phy-supply = <&vdd_usb>;
 };
+
+/* Low-power states of regulators */
+&v1v2_hdmi {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&v1v8_audio {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&v3v3 {
+	lp-stop {
+		regulator-suspend-microvolt = <3300000>;
+		regulator-on-in-suspend;
+	};
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&v3v3_hdmi {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&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;
+	};
+};
+
+&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>;
+	};
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vdd_ddr {
+	lp-stop {
+		regulator-suspend-microvolt = <1350000>;
+		regulator-on-in-suspend;
+	};
+	standby-ddr-sr {
+		regulator-suspend-microvolt = <1350000>;
+		regulator-on-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vdd_usb {
+	standby-ddr-sr {
+		regulator-off-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&vref_ddr {
+	lp-stop {
+		regulator-on-in-suspend;
+	};
+	standby-ddr-sr {
+		regulator-on-in-suspend;
+	};
+	standby-ddr-off {
+		regulator-off-in-suspend;
+	};
+};
+
+&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/fdts/stm32mp15xx-edx.dtsi b/fdts/stm32mp15xx-edx.dtsi
new file mode 100644
index 0000000000..540f5b9d97
--- /dev/null
+++ b/fdts/stm32mp15xx-edx.dtsi
@@ -0,0 +1,518 @@
+// 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>
+#include "stm32mp15-ddr3-2x4Gb-1066-binG.dtsi"
+
+/ {
+	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/fdts/stm32mp15xx-evx.dtsi b/fdts/stm32mp15xx-evx.dtsi
new file mode 100644
index 0000000000..ce1982d45a
--- /dev/null
+++ b/fdts/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/fdts/stm32mp15xxaa-pinctrl.dtsi b/fdts/stm32mp15xxaa-pinctrl.dtsi
index 64e566bf82..4f4130effd 100644
--- a/fdts/stm32mp15xxaa-pinctrl.dtsi
+++ b/fdts/stm32mp15xxaa-pinctrl.dtsi
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
 /*
  * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
- * Author: Alexandre Torgue <alexandre.torgue@st.com>
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
  */
 
 &pinctrl {
@@ -79,6 +79,7 @@
 
 	gpioz: gpio@54004000 {
 		status = "okay";
+		secure-status = "okay";
 		ngpios = <8>;
 		gpio-ranges = <&pinctrl_z 0 400 8>;
 	};
diff --git a/fdts/stm32mp15xxab-pinctrl.dtsi b/fdts/stm32mp15xxab-pinctrl.dtsi
index d29af8986f..328dad140e 100644
--- a/fdts/stm32mp15xxab-pinctrl.dtsi
+++ b/fdts/stm32mp15xxab-pinctrl.dtsi
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
 /*
  * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
- * Author: Alexandre Torgue <alexandre.torgue@st.com>
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
  */
 
 &pinctrl {
diff --git a/fdts/stm32mp15xxac-pinctrl.dtsi b/fdts/stm32mp15xxac-pinctrl.dtsi
index 5d8199fd19..866c050daf 100644
--- a/fdts/stm32mp15xxac-pinctrl.dtsi
+++ b/fdts/stm32mp15xxac-pinctrl.dtsi
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
 /*
  * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
- * Author: Alexandre Torgue <alexandre.torgue@st.com>
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
  */
 
 &pinctrl {
@@ -67,6 +67,7 @@
 
 	gpioz: gpio@54004000 {
 		status = "okay";
+		secure-status = "okay";
 		ngpios = <8>;
 		gpio-ranges = <&pinctrl_z 0 400 8>;
 	};
diff --git a/fdts/stm32mp15xxad-pinctrl.dtsi b/fdts/stm32mp15xxad-pinctrl.dtsi
index 023f5404c4..b63e207de2 100644
--- a/fdts/stm32mp15xxad-pinctrl.dtsi
+++ b/fdts/stm32mp15xxad-pinctrl.dtsi
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
 /*
  * Copyright (C) STMicroelectronics 2019 - All Rights Reserved
- * Author: Alexandre Torgue <alexandre.torgue@st.com>
+ * Author: Alexandre Torgue <alexandre.torgue@st.com> for STMicroelectronics.
  */
 
 &pinctrl {
diff --git a/include/arch/aarch32/arch.h b/include/arch/aarch32/arch.h
index db8938ff1f..9cf8c8ee80 100644
--- a/include/arch/aarch32/arch.h
+++ b/include/arch/aarch32/arch.h
@@ -461,13 +461,13 @@
  * system level implementation of the Generic Timer.
  ******************************************************************************/
 /* Physical Count register. */
-#define CNTPCT_LO		U(0x0)
+#define CNTBASEN_CNTPCT_LO		U(0x0)
 /* Counter Frequency register. */
 #define CNTBASEN_CNTFRQ		U(0x10)
 /* Physical Timer CompareValue register. */
-#define CNTP_CVAL_LO		U(0x20)
+#define CNTBASEN_CNTP_CVAL_LO	U(0x20)
 /* Physical Timer Control register. */
-#define CNTP_CTL		U(0x2c)
+#define CNTBASEN_CNTP_CTL	U(0x2c)
 
 /* Physical timer control register bit fields shifts and masks */
 #define CNTP_CTL_ENABLE_SHIFT   0
@@ -526,6 +526,9 @@
 #define HSTR		p15, 4, c1, c1, 3
 #define CNTHCTL		p15, 4, c14, c1, 0
 #define CNTKCTL		p15, 0, c14, c1, 0
+#define CNTP_TVAL	p15, 0, c14, c2, 0
+#define CNTP_CTL	p15, 0, c14, c2, 1
+#define CNTV_CTL	p15, 0, c14, c3, 1
 #define VPIDR		p15, 4, c0, c0, 0
 #define VMPIDR		p15, 4, c0, c0, 5
 #define ISR		p15, 0, c12, c1, 0
@@ -535,6 +538,7 @@
 #define HTCR		p15, 4, c2, c0, 2
 #define HMAIR0		p15, 4, c10, c2, 0
 #define ATS1CPR		p15, 0, c7, c8, 0
+#define ATS1CPW		p15, 0, c7, c8, 1
 #define ATS1HR		p15, 4, c7, c8, 0
 #define DBGOSDLR	p14, 0, c1, c3, 4
 
@@ -585,6 +589,12 @@
 #define ICC_ASGI1R_EL1_64	p15, 1, c12
 #define ICC_SGI0R_EL1_64	p15, 2, c12
 
+/* Fault registers. The format is: coproc, opt1, CRn, CRm, opt2 */
+#define DFSR		p15, 0, c5, c0, 0
+#define IFSR		p15, 0, c5, c0, 1
+#define DFAR		p15, 0, c6, c0, 0
+#define IFAR		p15, 0, c6, c0, 2
+
 /*******************************************************************************
  * Definitions of MAIR encodings for device and normal memory
  ******************************************************************************/
@@ -638,6 +648,8 @@
 /* PAR fields */
 #define PAR_F_SHIFT	U(0)
 #define PAR_F_MASK	ULL(0x1)
+#define PAR_NS_SHIFT	U(9)
+#define PAR_NS_MASK	U(0x1)
 #define PAR_ADDR_SHIFT	U(12)
 #define PAR_ADDR_MASK	(BIT_64(40) - ULL(1)) /* 40-bits-wide page address */
 
diff --git a/include/arch/aarch32/arch_helpers.h b/include/arch/aarch32/arch_helpers.h
index 82efb188a4..9998034396 100644
--- a/include/arch/aarch32/arch_helpers.h
+++ b/include/arch/aarch32/arch_helpers.h
@@ -248,6 +248,9 @@ DEFINE_COPROCR_RW_FUNCS_64(ttbr1, TTBR1_64)
 DEFINE_COPROCR_RW_FUNCS_64(cntvoff, CNTVOFF_64)
 DEFINE_COPROCR_RW_FUNCS(csselr, CSSELR)
 DEFINE_COPROCR_RW_FUNCS(hstr, HSTR)
+DEFINE_COPROCR_RW_FUNCS(cntp_tval, CNTP_TVAL)
+DEFINE_COPROCR_RW_FUNCS(cntp_ctl, CNTP_CTL)
+DEFINE_COPROCR_RW_FUNCS(cntv_ctl, CNTV_CTL)
 DEFINE_COPROCR_RW_FUNCS(cnthp_ctl_el2, CNTHP_CTL)
 DEFINE_COPROCR_RW_FUNCS(cnthp_tval_el2, CNTHP_TVAL)
 DEFINE_COPROCR_RW_FUNCS_64(cnthp_cval_el2, CNTHP_CVAL_64)
@@ -290,6 +293,7 @@ DEFINE_COPROCR_READ_FUNC(pmcr, PMCR)
  * Address translation
  */
 DEFINE_COPROCR_WRITE_FUNC(ats1cpr, ATS1CPR)
+DEFINE_COPROCR_WRITE_FUNC(ats1cpw, ATS1CPW)
 DEFINE_COPROCR_WRITE_FUNC(ats1hr, ATS1HR)
 DEFINE_COPROCR_RW_FUNCS_64(par, PAR_64)
 
diff --git a/include/arch/aarch32/el3_common_macros.S b/include/arch/aarch32/el3_common_macros.S
index 4fd746d5a4..6bbcde2e0a 100644
--- a/include/arch/aarch32/el3_common_macros.S
+++ b/include/arch/aarch32/el3_common_macros.S
@@ -10,6 +10,9 @@
 #include <arch.h>
 #include <asm_macros.S>
 #include <assert_macros.S>
+#include <lib/xlat_tables/xlat_tables_defs.h>
+
+#define PAGE_START_MASK		~(PAGE_SIZE_MASK)
 
 	/*
 	 * Helper macro to initialise EL3 registers we care about.
@@ -199,11 +202,18 @@
  *
  * _exception_vectors:
  *	Address of the exception vectors to program in the VBAR_EL3 register.
+ *
+ * _pie_fixup_size:
+ *	Size of memory region to fixup Global Descriptor Table (GDT).
+ *
+ *	A non-zero value is expected when firmware needs GDT to be fixed-up.
+ *
  * -----------------------------------------------------------------------------
  */
 	.macro el3_entrypoint_common					\
 		_init_sctlr, _warm_boot_mailbox, _secondary_cold_boot,	\
-		_init_memory, _init_c_runtime, _exception_vectors
+		_init_memory, _init_c_runtime, _exception_vectors,	\
+		_pie_fixup_size
 
 	/* Make sure we are in Secure Mode */
 #if ENABLE_ASSERTIONS
@@ -255,6 +265,27 @@
 		bxne	r0
 	.endif /* _warm_boot_mailbox */
 
+	.if \_pie_fixup_size
+#if ENABLE_PIE
+		/*
+		 * ------------------------------------------------------------
+		 * If PIE is enabled fixup the Global descriptor Table only
+		 * once during primary core cold boot path.
+		 *
+		 * Compile time base address, required for fixup, is calculated
+		 * using "pie_fixup" label present within first page.
+		 * ------------------------------------------------------------
+		 */
+	pie_fixup:
+		ldr	r0, =pie_fixup
+		ldr	r1, =PAGE_START_MASK
+		and	r0, r0, r1
+		mov_imm	r1, \_pie_fixup_size
+		add	r1, r1, r0
+		bl	fixup_gdt_reloc
+#endif /* ENABLE_PIE */
+	.endif /* _pie_fixup_size */
+
 	/* ---------------------------------------------------------------------
 	 * Set the exception vectors (VBAR/MVBAR).
 	 * ---------------------------------------------------------------------
@@ -335,12 +366,14 @@
 		 */
 		mov	r7, r12
 		ldr	r0, =__BSS_START__
-		ldr	r1, =__BSS_SIZE__
+		ldr	r1, =__BSS_END__
+		sub 	r1, r1, r0
 		bl	zeromem
 
 #if USE_COHERENT_MEM
 		ldr	r0, =__COHERENT_RAM_START__
-		ldr	r1, =__COHERENT_RAM_UNALIGNED_SIZE__
+		ldr	r1, =__COHERENT_RAM_END_UNALIGNED__
+		sub 	r1, r1, r0
 		bl	zeromem
 #endif
 
@@ -354,7 +387,8 @@
 		 */
 		ldr	r0, =__DATA_RAM_START__
 		ldr	r1, =__DATA_ROM_START__
-		ldr	r2, =__DATA_SIZE__
+		ldr	r2, =__DATA_RAM_END__
+		sub 	r2, r2, r0
 		bl	memcpy4
 #endif
 	.endif /* _init_c_runtime */
diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index 33e1134dd5..f50a5a57c3 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -735,13 +735,13 @@
  * system level implementation of the Generic Timer.
  ******************************************************************************/
 /* Physical Count register. */
-#define CNTPCT_LO		U(0x0)
+#define CNTBASEN_CNTPCT_LO		U(0x0)
 /* Counter Frequency register. */
 #define CNTBASEN_CNTFRQ		U(0x10)
 /* Physical Timer CompareValue register. */
-#define CNTP_CVAL_LO		U(0x20)
+#define CNTBASEN_CNTP_CVAL_LO	U(0x20)
 /* Physical Timer Control register. */
-#define CNTP_CTL		U(0x2c)
+#define CNTBASEN_CNTP_CTL	U(0x2c)
 
 /* PMCR_EL0 definitions */
 #define PMCR_EL0_RESET_VAL	U(0x0)
diff --git a/include/common/bl_common.ld.h b/include/common/bl_common.ld.h
index ab3391aa21..cb71d9aea8 100644
--- a/include/common/bl_common.ld.h
+++ b/include/common/bl_common.ld.h
@@ -105,12 +105,21 @@
  * .rela.dyn needs to come after .data for the read-elf utility to parse
  * this section correctly.
  */
+#if __aarch64__
 #define RELA_SECTION					\
 	.rela.dyn : ALIGN(STRUCT_ALIGN) {		\
 		__RELA_START__ = .;			\
 		*(.rela*)				\
 		__RELA_END__ = .;			\
 	}
+#else
+#define RELA_SECTION					\
+	.rel.dyn : ALIGN(STRUCT_ALIGN) {		\
+		__RELA_START__ = .;			\
+		*(.rel*)				\
+		__RELA_END__ = .;			\
+	}
+#endif
 
 #if !(defined(IMAGE_BL31) && RECLAIM_INIT_CODE)
 #define STACK_SECTION					\
diff --git a/include/common/tbbr/cot_def.h b/include/common/tbbr/cot_def.h
index 6ce7f80c17..0d2d9acfe5 100644
--- a/include/common/tbbr/cot_def.h
+++ b/include/common/tbbr/cot_def.h
@@ -35,7 +35,7 @@
 #error "Invalid value for TF_MBEDTLS_KEY_SIZE"
 #endif
 #else /* Only using ECDSA keys. */
-#define PK_DER_LEN                      91
+#define PK_DER_LEN                      92
 #endif
 
 #if TF_MBEDTLS_HASH_ALG_ID == TF_MBEDTLS_SHA256
diff --git a/include/drivers/arm/tzc400.h b/include/drivers/arm/tzc400.h
index 32aeb03502..88c9237cf1 100644
--- a/include/drivers/arm/tzc400.h
+++ b/include/drivers/arm/tzc400.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -32,7 +32,6 @@
 #define BUILD_CONFIG_AW_MASK			U(0x3f)
 #define BUILD_CONFIG_NR_SHIFT			0
 #define BUILD_CONFIG_NR_MASK			U(0x1f)
-
 /*
  * Number of gate keepers is implementation defined. But we know the max for
  * this device is 4. Get implementation details from BUILD_CONFIG.
@@ -65,8 +64,8 @@
 #define FAIL_CONTROL_NS_SECURE			U(0)
 #define FAIL_CONTROL_NS_NONSECURE		U(1)
 #define FAIL_CONTROL_PRIV_SHIFT			20
-#define FAIL_CONTROL_PRIV_PRIV			U(0)
-#define FAIL_CONTROL_PRIV_UNPRIV		U(1)
+#define FAIL_CONTROL_PRIV_UNPRIV		U(0)
+#define FAIL_CONTROL_PRIV_PRIV			U(1)
 
 /*
  * FAIL_ID_ID_MASK depends on AID_WIDTH which is platform specific.
@@ -93,6 +92,8 @@
 #define TZC_400_REGION_SIZE			U(0x20)
 #define TZC_400_ACTION_OFF			U(0x4)
 
+#define FILTER_OFFSET				U(0x10)
+
 #ifndef __ASSEMBLER__
 
 #include <cdefs.h>
@@ -103,39 +104,39 @@
  ******************************************************************************/
 void tzc400_init(uintptr_t base);
 void tzc400_configure_region0(unsigned int sec_attr,
-			   unsigned int ns_device_access);
+			      unsigned int ns_device_access);
 void tzc400_configure_region(unsigned int filters,
-			  unsigned int region,
-			  unsigned long long region_base,
-			  unsigned long long region_top,
-			  unsigned int sec_attr,
-			  unsigned int nsaid_permissions);
+			     unsigned int region,
+			     unsigned long long region_base,
+			     unsigned long long region_top,
+			     unsigned int sec_attr,
+			     unsigned int nsaid_permissions);
+void tzc400_update_filters(unsigned int region, unsigned int filters);
 void tzc400_set_action(unsigned int action);
 void tzc400_enable_filters(void);
 void tzc400_disable_filters(void);
+void tzc400_it_handler(void);
 
 static inline void tzc_init(uintptr_t base)
 {
 	tzc400_init(base);
 }
 
-static inline void tzc_configure_region0(
-			unsigned int sec_attr,
-			unsigned int ns_device_access)
+static inline void tzc_configure_region0(unsigned int sec_attr,
+					 unsigned int ns_device_access)
 {
 	tzc400_configure_region0(sec_attr, ns_device_access);
 }
 
-static inline void tzc_configure_region(
-			  unsigned int filters,
-			  unsigned int region,
-			  unsigned long long region_base,
-			  unsigned long long region_top,
-			  unsigned int sec_attr,
-			  unsigned int ns_device_access)
+static inline void tzc_configure_region(unsigned int filters,
+					unsigned int region,
+					unsigned long long region_base,
+					unsigned long long region_top,
+					unsigned int sec_attr,
+					unsigned int ns_device_access)
 {
 	tzc400_configure_region(filters, region, region_base,
-			region_top, sec_attr, ns_device_access);
+				region_top, sec_attr, ns_device_access);
 }
 
 static inline void tzc_set_action(unsigned int action)
@@ -143,7 +144,6 @@ static inline void tzc_set_action(unsigned int action)
 	tzc400_set_action(action);
 }
 
-
 static inline void tzc_enable_filters(void)
 {
 	tzc400_enable_filters();
diff --git a/include/drivers/auth/tbbr_cot_common.h b/include/drivers/auth/tbbr_cot_common.h
index a51faee1aa..21bcd520a4 100644
--- a/include/drivers/auth/tbbr_cot_common.h
+++ b/include/drivers/auth/tbbr_cot_common.h
@@ -25,5 +25,6 @@ extern auth_param_type_desc_t fw_config_hash;
 
 extern const auth_img_desc_t trusted_boot_fw_cert;
 extern const auth_img_desc_t hw_config;
+extern const auth_img_desc_t fw_config;
 
 #endif /* TBBR_COT_COMMON_H */
diff --git a/include/drivers/clk.h b/include/drivers/clk.h
new file mode 100644
index 0000000000..960dce742a
--- /dev/null
+++ b/include/drivers/clk.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CLK_H
+#define CLK_H
+
+#include <stdbool.h>
+
+typedef struct clk_ops {
+	int (*enable)(unsigned long id);
+	void (*disable)(unsigned long id);
+	unsigned long (*get_rate)(unsigned long id);
+	int (*get_parent)(unsigned long id);
+	bool (*is_enabled)(unsigned long id);
+} clk_ops_t;
+
+int 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);
+int clk_get_parent(unsigned long id);
+
+void clk_register(const clk_ops_t *ops);
+
+#endif /* CLK_H */
diff --git a/include/drivers/io/io_mtd.h b/include/drivers/io/io_mtd.h
index 1395ff6019..b2c90205d4 100644
--- a/include/drivers/io/io_mtd.h
+++ b/include/drivers/io/io_mtd.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -44,11 +44,22 @@ typedef struct io_mtd_ops {
 	 * Return 0 on success, a negative error code otherwise.
 	 */
 	int (*write)(unsigned int offset, uintptr_t buffer, size_t length);
+
+	/*
+	 * Look for an offset to be added to the given offset.
+	 *
+	 * @base: Base address of the area.
+	 * @offset: Offset in bytes to start read operation.
+	 * @extra_offset: [out] Offset to be added to the previous offset.
+	 * Return 0 on success, a negative error code otherwise.
+	 */
+	int (*seek)(uintptr_t base, unsigned int offset, size_t *extra_offset);
 } io_mtd_ops_t;
 
 typedef struct io_mtd_dev_spec {
 	unsigned long long device_size;
 	unsigned int erase_size;
+	size_t offset;
 	io_mtd_ops_t ops;
 } io_mtd_dev_spec_t;
 
diff --git a/include/drivers/mmc.h b/include/drivers/mmc.h
index 7611f019a1..2b4f5ab8b3 100644
--- a/include/drivers/mmc.h
+++ b/include/drivers/mmc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -104,6 +104,7 @@
 #define MMC_STATE_SLP			10
 
 #define MMC_FLAG_CMD23			(U(1) << 0)
+#define MMC_FLAG_SD_CMD6		(U(1) << 1)
 
 #define CMD8_CHECK_PATTERN		U(0xAA)
 #define VHS_2_7_3_6_V			BIT(8)
@@ -111,6 +112,9 @@
 #define SD_SCR_BUS_WIDTH_1		BIT(8)
 #define SD_SCR_BUS_WIDTH_4		BIT(10)
 
+#define SD_SWITCH_FUNC_CHECK		0U
+#define SD_SWITCH_FUNC_SWITCH		1U
+
 struct mmc_cmd {
 	unsigned int	cmd_idx;
 	unsigned int	cmd_arg;
@@ -210,6 +214,27 @@ struct mmc_csd_sd_v2 {
 	unsigned int		csd_structure:		2;
 };
 
+struct sd_switch_status {
+	unsigned short		max_current;
+	unsigned short		support_g6;
+	unsigned short		support_g5;
+	unsigned short		support_g4;
+	unsigned short		support_g3;
+	unsigned short		support_g2;
+	unsigned short		support_g1;
+	unsigned char		sel_g6_g5;
+	unsigned char		sel_g4_g3;
+	unsigned char		sel_g2_g1;
+	unsigned char		data_struct_ver;
+	unsigned short		busy_g6;
+	unsigned short		busy_g5;
+	unsigned short		busy_g4;
+	unsigned short		busy_g3;
+	unsigned short		busy_g2;
+	unsigned short		busy_g1;
+	unsigned short		reserved[17];
+};
+
 enum mmc_device_type {
 	MMC_IS_EMMC,
 	MMC_IS_SD,
diff --git a/include/drivers/nand.h b/include/drivers/nand.h
index 1dbb008f9c..862e1792a4 100644
--- a/include/drivers/nand.h
+++ b/include/drivers/nand.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2019-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -45,6 +45,16 @@ struct nand_device {
 int nand_read(unsigned int offset, uintptr_t buffer, size_t length,
 	      size_t *length_read);
 
+/*
+ * Look for an extra offset to be added in case of bad blocks
+ *
+ * @base: Base address of the area
+ * @offset: Byte offset to read from in device
+ * @extra_offset: [out] Extra offset to be added if bad blocks are found
+ * Return: 0 on success, a negative errno on failure
+ */
+int nand_seek_bb(uintptr_t base, unsigned int offset, size_t *extra_offset);
+
 /*
  * Get NAND device instance
  *
diff --git a/include/drivers/st/bsec.h b/include/drivers/st/bsec.h
index d833e7ab27..273a4e98ac 100644
--- a/include/drivers/st/bsec.h
+++ b/include/drivers/st/bsec.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2015-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,13 +19,6 @@
 #define BSEC_OTP_BANK_SHIFT		5
 #define BSEC_TIMEOUT_VALUE		0xFFFF
 
-#define ADDR_LOWER_OTP_PERLOCK_SHIFT	0x03
-#define DATA_LOWER_OTP_PERLOCK_BIT	0x03U /* 2 significants bits are used */
-#define DATA_LOWER_OTP_PERLOCK_MASK	GENMASK(2, 0)
-#define ADDR_UPPER_OTP_PERLOCK_SHIFT	0x04
-#define DATA_UPPER_OTP_PERLOCK_BIT	0x01U /* 1 significants bits are used */
-#define DATA_UPPER_OTP_PERLOCK_MASK	GENMASK(3, 0)
-
 /*
  * Return status
  */
@@ -35,110 +28,34 @@
 #define BSEC_INVALID_PARAM		0xFFFFFFFCU
 #define BSEC_PROG_FAIL			0xFFFFFFFBU
 #define BSEC_LOCK_FAIL			0xFFFFFFFAU
-#define BSEC_WRITE_FAIL			0xFFFFFFF9U
-#define BSEC_SHADOW_FAIL		0xFFFFFFF8U
-#define BSEC_TIMEOUT			0xFFFFFFF7U
-
-/*
- * BSEC REGISTER OFFSET (base relative)
- */
-#define BSEC_OTP_CONF_OFF		0x000U
-#define BSEC_OTP_CTRL_OFF		0x004U
-#define BSEC_OTP_WRDATA_OFF		0x008U
-#define BSEC_OTP_STATUS_OFF		0x00CU
-#define BSEC_OTP_LOCK_OFF		0x010U
-#define BSEC_DEN_OFF			0x014U
-#define BSEC_DISTURBED_OFF		0x01CU
-#define BSEC_DISTURBED1_OFF		0x020U
-#define BSEC_DISTURBED2_OFF		0x024U
-#define BSEC_ERROR_OFF			0x034U
-#define BSEC_ERROR1_OFF			0x038U
-#define BSEC_ERROR2_OFF			0x03CU
-#define BSEC_WRLOCK_OFF			0x04CU /* Safmem permanent lock */
-#define BSEC_WRLOCK1_OFF		0x050U
-#define BSEC_WRLOCK2_OFF		0x054U
-#define BSEC_SPLOCK_OFF			0x064U /* Program safmem sticky lock */
-#define BSEC_SPLOCK1_OFF		0x068U
-#define BSEC_SPLOCK2_OFF		0x06CU
-#define BSEC_SWLOCK_OFF			0x07CU /* Write in OTP sticky lock */
-#define BSEC_SWLOCK1_OFF		0x080U
-#define BSEC_SWLOCK2_OFF		0x084U
-#define BSEC_SRLOCK_OFF			0x094U /* Shadowing sticky lock */
-#define BSEC_SRLOCK1_OFF		0x098U
-#define BSEC_SRLOCK2_OFF		0x09CU
-#define BSEC_JTAG_IN_OFF		0x0ACU
-#define BSEC_JTAG_OUT_OFF		0x0B0U
-#define BSEC_SCRATCH_OFF		0x0B4U
-#define BSEC_OTP_DATA_OFF		0x200U
-#define BSEC_IPHW_CFG_OFF		0xFF0U
-#define BSEC_IPVR_OFF			0xFF4U
-#define BSEC_IP_ID_OFF			0xFF8U
-#define BSEC_IP_MAGIC_ID_OFF		0xFFCU
-
-/*
- * BSEC_CONFIGURATION Register
- */
-#define BSEC_CONF_POWER_UP_MASK		BIT(0)
-#define BSEC_CONF_POWER_UP_SHIFT	0
-#define BSEC_CONF_FRQ_MASK		GENMASK(2, 1)
-#define BSEC_CONF_FRQ_SHIFT		1
-#define BSEC_CONF_PRG_WIDTH_MASK	GENMASK(6, 3)
-#define BSEC_CONF_PRG_WIDTH_SHIFT	3
-#define BSEC_CONF_TREAD_MASK		GENMASK(8, 7)
-#define BSEC_CONF_TREAD_SHIFT		7
-
-/*
- * BSEC_CONTROL Register
- */
-#define BSEC_READ			0x000U
-#define BSEC_WRITE			0x100U
-#define BSEC_LOCK			0x200U
-
-/*
- * BSEC_OTP_LOCK register
- */
-#define UPPER_OTP_LOCK_MASK		BIT(0)
-#define UPPER_OTP_LOCK_SHIFT		0
-#define DENREG_LOCK_MASK		BIT(2)
-#define DENREG_LOCK_SHIFT		2
-#define GPLOCK_LOCK_MASK		BIT(4)
-#define GPLOCK_LOCK_SHIFT		4
+#define BSEC_TIMEOUT			0xFFFFFFF9U
+#define BSEC_RETRY			0xFFFFFFF8U
+#define BSEC_NOT_SUPPORTED		0xFFFFFFF7U
+#define BSEC_WRITE_LOCKED		0xFFFFFFF6U
+#define BSEC_ERROR_INVALID_FVR		0xFFFFFFF5U
 
 /*
- * BSEC_OTP_STATUS Register
+ * OTP MODE
  */
-#define BSEC_MODE_STATUS_MASK		GENMASK(2, 0)
-#define BSEC_MODE_BUSY_MASK		BIT(3)
-#define BSEC_MODE_PROGFAIL_MASK		BIT(4)
-#define BSEC_MODE_PWR_MASK		BIT(5)
-#define BSEC_MODE_BIST1_LOCK_MASK	BIT(6)
-#define BSEC_MODE_BIST2_LOCK_MASK	BIT(7)
-
-/* OTP MODE*/
 #define BSEC_MODE_OPEN1			0x00
 #define BSEC_MODE_SECURED		0x01
 #define BSEC_MODE_OPEN2			0x02
 #define BSEC_MODE_INVALID		0x04
 
-/* BSEC_DENABLE Register */
-#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(10, 0)
-
-/* BSEC_FENABLE Register */
-#define BSEC_FEN_ALL_MSK		GENMASK(14, 0)
-
 /*
- * OTP Lock services definition
- * Value must corresponding to the bit number in the register
+ * OTP Lock services definition.
+ * Value must corresponding to the bit number in the register.
+ * Special case: (bit number << 1) for BSEC3.
  */
 #define BSEC_LOCK_UPPER_OTP		0x00
+#define BSEC_LOCK_GWLOCK		0x01
 #define BSEC_LOCK_DEBUG			0x02
 #define BSEC_LOCK_PROGRAM		0x03
+#define BSEC_LOCK_KVLOCK		0x04
 
-/* Values for struct bsec_config::freq */
+/*
+ * Values for struct bsec_config::freq
+ */
 #define FREQ_10_20_MHZ			0x0
 #define FREQ_20_30_MHZ			0x1
 #define FREQ_30_45_MHZ			0x2
@@ -146,22 +63,28 @@
 
 /*
  * Device info structure, providing device-specific functions and a means of
- * adding driver-specific state
+ * adding driver-specific state.
  */
 struct bsec_config {
+	uint8_t den_lock;	/*
+				 * Debug enable sticky lock
+				 * 1 debug enable is locked until next reset
+				 */
+
+	/*  BSEC2 only */
 	uint8_t tread;		/* SAFMEM Reading current level default 0 */
 	uint8_t pulse_width;	/* SAFMEM Programming pulse width default 1 */
-	uint8_t freq;		/* SAFMEM CLOCK see freq value define
+	uint8_t freq;		/*
+				 * SAFMEM CLOCK see freq value define
 				 * default FREQ_45_67_MHZ
 				 */
 	uint8_t power;		/* Power up SAFMEM. 1 power up, 0 power off */
-	uint8_t prog_lock;	/* Programming Sticky lock
+	uint8_t prog_lock;	/*
+				 * Programming Sticky lock
 				 * 1 programming is locked until next reset
 				 */
-	uint8_t den_lock;	/* Debug enable sticky lock
-				 * 1 debug enable is locked until next reset
-				 */
-	uint8_t upper_otp_lock;	/* Shadowing of upper OTP sticky lock
+	uint8_t upper_otp_lock;	/*
+				 * Shadowing of upper OTP sticky lock
 				 * 1 shadowing of upper OTP is locked
 				 * until next reset
 				 */
@@ -173,16 +96,20 @@ uint32_t bsec_get_base(void);
 uint32_t bsec_set_config(struct bsec_config *cfg);
 uint32_t bsec_get_config(struct bsec_config *cfg);
 
+uint32_t bsec_find_otp_name_in_dt(const char *name, uint32_t *otp,
+				  uint32_t *otp_len);
+
 uint32_t bsec_shadow_register(uint32_t otp);
 uint32_t bsec_read_otp(uint32_t *val, uint32_t otp);
 uint32_t bsec_write_otp(uint32_t val, uint32_t otp);
 uint32_t bsec_program_otp(uint32_t val, uint32_t otp);
 uint32_t bsec_permanent_lock_otp(uint32_t otp);
 
-uint32_t bsec_write_debug_conf(uint32_t val);
+void bsec_write_debug_conf(uint32_t val);
 uint32_t bsec_read_debug_conf(void);
-uint32_t bsec_write_feature_conf(uint32_t val);
-uint32_t bsec_read_feature_conf(uint32_t *val);
+
+void bsec_write_scratch(uint32_t val);
+uint32_t bsec_read_scratch(void);
 
 uint32_t bsec_get_status(void);
 uint32_t bsec_get_hw_conf(void);
@@ -190,14 +117,14 @@ uint32_t bsec_get_version(void);
 uint32_t bsec_get_id(void);
 uint32_t bsec_get_magic_id(void);
 
-bool bsec_write_sr_lock(uint32_t otp, uint32_t value);
-bool bsec_read_sr_lock(uint32_t otp);
-bool bsec_write_sw_lock(uint32_t otp, uint32_t value);
-bool bsec_read_sw_lock(uint32_t otp);
-bool bsec_write_sp_lock(uint32_t otp, uint32_t value);
-bool bsec_read_sp_lock(uint32_t otp);
-bool bsec_wr_lock(uint32_t otp);
-uint32_t bsec_otp_lock(uint32_t service, uint32_t value);
+uint32_t bsec_set_sr_lock(uint32_t otp);
+uint32_t bsec_read_sr_lock(uint32_t otp, bool *value);
+uint32_t bsec_set_sw_lock(uint32_t otp);
+uint32_t bsec_read_sw_lock(uint32_t otp, bool *value);
+uint32_t bsec_set_sp_lock(uint32_t otp);
+uint32_t bsec_read_sp_lock(uint32_t otp, bool *value);
+uint32_t bsec_read_permanent_lock(uint32_t otp, bool *value);
+uint32_t bsec_otp_lock(uint32_t service);
 
 uint32_t bsec_shadow_read_otp(uint32_t *otp_value, uint32_t word);
 uint32_t bsec_check_nsec_access_rights(uint32_t otp);
diff --git a/include/drivers/st/bsec2_reg.h b/include/drivers/st/bsec2_reg.h
new file mode 100644
index 0000000000..9da9526031
--- /dev/null
+++ b/include/drivers/st/bsec2_reg.h
@@ -0,0 +1,98 @@
+/*
+ * Copyright (c) 2019, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef BSEC2_REG_H
+#define BSEC2_REG_H
+
+#include <lib/utils_def.h>
+
+/* IP configuration */
+#define ADDR_LOWER_OTP_PERLOCK_SHIFT	0x03
+#define DATA_LOWER_OTP_PERLOCK_BIT	0x03U /* 2 significants bits are used */
+#define DATA_LOWER_OTP_PERLOCK_MASK	GENMASK(2, 0)
+#define ADDR_UPPER_OTP_PERLOCK_SHIFT	0x04
+#define DATA_UPPER_OTP_PERLOCK_BIT	0x01U /* 1 significants bits are used */
+#define DATA_UPPER_OTP_PERLOCK_MASK	GENMASK(3, 0)
+
+/* BSEC REGISTER OFFSET (base relative) */
+#define BSEC_OTP_CONF_OFF		U(0x000)
+#define BSEC_OTP_CTRL_OFF		U(0x004)
+#define BSEC_OTP_WRDATA_OFF		U(0x008)
+#define BSEC_OTP_STATUS_OFF		U(0x00C)
+#define BSEC_OTP_LOCK_OFF		U(0x010)
+#define BSEC_DEN_OFF			U(0x014)
+#define BSEC_DISTURBED_OFF		U(0x01C)
+#define BSEC_DISTURBED1_OFF		U(0x020)
+#define BSEC_DISTURBED2_OFF		U(0x024)
+#define BSEC_ERROR_OFF			U(0x034)
+#define BSEC_ERROR1_OFF			U(0x038)
+#define BSEC_ERROR2_OFF			U(0x03C)
+#define BSEC_WRLOCK_OFF			U(0x04C) /* Safmem permanent lock */
+#define BSEC_WRLOCK1_OFF		U(0x050)
+#define BSEC_WRLOCK2_OFF		U(0x054)
+#define BSEC_SPLOCK_OFF			U(0x064) /* Program safmem sticky lock */
+#define BSEC_SPLOCK1_OFF		U(0x068)
+#define BSEC_SPLOCK2_OFF		U(0x06C)
+#define BSEC_SWLOCK_OFF			U(0x07C) /* Write in OTP sticky lock */
+#define BSEC_SWLOCK1_OFF		U(0x080)
+#define BSEC_SWLOCK2_OFF		U(0x084)
+#define BSEC_SRLOCK_OFF			U(0x094) /* Shadowing sticky lock */
+#define BSEC_SRLOCK1_OFF		U(0x098)
+#define BSEC_SRLOCK2_OFF		U(0x09C)
+#define BSEC_JTAG_IN_OFF		U(0x0AC)
+#define BSEC_JTAG_OUT_OFF		U(0x0B0)
+#define BSEC_SCRATCH_OFF		U(0x0B4)
+#define BSEC_OTP_DATA_OFF		U(0x200)
+#define BSEC_IPHW_CFG_OFF		U(0xFF0)
+#define BSEC_IPVR_OFF			U(0xFF4)
+#define BSEC_IP_ID_OFF			U(0xFF8)
+#define BSEC_IP_MAGIC_ID_OFF		U(0xFFC)
+
+/* BSEC_CONFIGURATION Register */
+#define BSEC_CONF_POWER_UP_MASK		BIT(0)
+#define BSEC_CONF_POWER_UP_SHIFT	0
+#define BSEC_CONF_FRQ_MASK		GENMASK(2, 1)
+#define BSEC_CONF_FRQ_SHIFT		1
+#define BSEC_CONF_PRG_WIDTH_MASK	GENMASK(6, 3)
+#define BSEC_CONF_PRG_WIDTH_SHIFT	3
+#define BSEC_CONF_TREAD_MASK		GENMASK(8, 7)
+#define BSEC_CONF_TREAD_SHIFT		7
+
+/* BSEC_CONTROL Register */
+#define BSEC_READ			0
+#define BSEC_WRITE			BIT(8)
+#define BSEC_LOCK			BIT(9)
+
+/* BSEC_OTP_LOCK register */
+#define UPPER_OTP_LOCK_MASK		BIT(0)
+#define UPPER_OTP_LOCK_SHIFT		0
+#define DENREG_LOCK_MASK		BIT(2)
+#define DENREG_LOCK_SHIFT		2
+#define GPLOCK_LOCK_MASK		BIT(4)
+#define GPLOCK_LOCK_SHIFT		4
+
+/* BSEC_OTP_STATUS Register */
+#define BSEC_MODE_STATUS_MASK		GENMASK(2, 0)
+#define BSEC_MODE_BUSY_MASK		BIT(3)
+#define BSEC_MODE_PROGFAIL_MASK		BIT(4)
+#define BSEC_MODE_PWR_MASK		BIT(5)
+#define BSEC_MODE_BIST1_LOCK_MASK	BIT(6)
+#define BSEC_MODE_BIST2_LOCK_MASK	BIT(7)
+
+/* BSEC_DENABLE Register */
+#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(10, 0)
+
+/* BSEC_FENABLE Register */
+#define BSEC_FEN_ALL_MSK		GENMASK(14, 0)
+
+/* BSEC_IPVR Register */
+#define BSEC_IPVR_MSK			GENMASK(7, 0)
+
+#endif /* BSEC2_REG_H */
diff --git a/include/drivers/st/stm32_i2c.h b/include/drivers/st/stm32_i2c.h
index 170d4cf815..e1a3782b2e 100644
--- a/include/drivers/st/stm32_i2c.h
+++ b/include/drivers/st/stm32_i2c.h
@@ -1,7 +1,7 @@
 /*
  * Copyright (c) 2016-2019, STMicroelectronics - All Rights Reserved
  *
- * SPDX-License-Identifier: BSD-3-Clause
+ * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
  */
 
 #ifndef STM32_I2C_H
@@ -73,6 +73,21 @@
 #define I2C_TIMINGR_SDADEL		GENMASK(19, 16)
 #define I2C_TIMINGR_SCLDEL		GENMASK(23, 20)
 #define I2C_TIMINGR_PRESC		GENMASK(31, 28)
+#define I2C_TIMINGR_SCLL_MAX		(I2C_TIMINGR_SCLL + 1)
+#define I2C_TIMINGR_SCLH_MAX		((I2C_TIMINGR_SCLH >> 8) + 1)
+#define I2C_TIMINGR_SDADEL_MAX		((I2C_TIMINGR_SDADEL >> 16) + 1)
+#define I2C_TIMINGR_SCLDEL_MAX		((I2C_TIMINGR_SCLDEL >> 20) + 1)
+#define I2C_TIMINGR_PRESC_MAX		((I2C_TIMINGR_PRESC >> 28) + 1)
+#define I2C_SET_TIMINGR_SCLL(n)		((n) & \
+					 (I2C_TIMINGR_SCLL_MAX - 1))
+#define I2C_SET_TIMINGR_SCLH(n)		(((n) & \
+					  (I2C_TIMINGR_SCLH_MAX - 1)) << 8)
+#define I2C_SET_TIMINGR_SDADEL(n)	(((n) & \
+					  (I2C_TIMINGR_SDADEL_MAX - 1)) << 16)
+#define I2C_SET_TIMINGR_SCLDEL(n)	(((n) & \
+					  (I2C_TIMINGR_SCLDEL_MAX - 1)) << 20)
+#define I2C_SET_TIMINGR_PRESC(n)	(((n) & \
+					  (I2C_TIMINGR_PRESC_MAX - 1)) << 28)
 
 /* Bit definition for I2C_TIMEOUTR register */
 #define I2C_TIMEOUTR_TIMEOUTA		GENMASK(11, 0)
@@ -111,15 +126,9 @@
 #define I2C_ICR_TIMOUTCF		BIT(12)
 #define I2C_ICR_ALERTCF			BIT(13)
 
-enum i2c_speed_e {
-	I2C_SPEED_STANDARD,	/* 100 kHz */
-	I2C_SPEED_FAST,		/* 400 kHz */
-	I2C_SPEED_FAST_PLUS,	/* 1 MHz   */
-};
-
-#define STANDARD_RATE				100000
-#define FAST_RATE				400000
-#define FAST_PLUS_RATE				1000000
+#define STANDARD_RATE			100000
+#define FAST_RATE			400000
+#define FAST_PLUS_RATE			1000000
 
 struct stm32_i2c_init_s {
 	uint32_t own_address1;		/*
@@ -181,12 +190,7 @@ struct stm32_i2c_init_s {
 					 * time in nanoseconds.
 					 */
 
-	enum i2c_speed_e speed_mode;	/*
-					 * Specifies the I2C clock source
-					 * frequency mode.
-					 * This parameter can be a value of @ref
-					 * i2c_speed_mode_e.
-					 */
+	uint32_t bus_rate;		/* Specifies the I2C clock frequency */
 
 	int analog_filter;		/*
 					 * Specifies if the I2C analog noise
@@ -238,6 +242,8 @@ struct i2c_handle_s {
 	enum i2c_state_e i2c_state;		/* Communication state    */
 	enum i2c_mode_e i2c_mode;		/* Communication mode     */
 	uint32_t i2c_err;			/* Error code             */
+	uint32_t saved_timing;			/* Saved timing value     */
+	uint32_t saved_frequency;		/* Saved frequency value  */
 };
 
 #define I2C_ADDRESSINGMODE_7BIT		0x00000001U
@@ -299,8 +305,7 @@ struct i2c_handle_s {
 #define STM32_I2C_ANALOG_FILTER_DELAY_MAX	260	/* ns */
 #define STM32_I2C_DIGITAL_FILTER_MAX		16
 
-int stm32_i2c_get_setup_from_fdt(void *fdt, int node,
-				 struct stm32_i2c_init_s *init);
+int stm32_i2c_get_setup_from_fdt(int node, struct stm32_i2c_init_s *init);
 int stm32_i2c_init(struct i2c_handle_s *hi2c,
 		   struct stm32_i2c_init_s *init_data);
 int stm32_i2c_mem_write(struct i2c_handle_s *hi2c, uint16_t dev_addr,
diff --git a/include/drivers/st/stm32_iwdg.h b/include/drivers/st/stm32_iwdg.h
index bad25244a8..c0c009779e 100644
--- a/include/drivers/st/stm32_iwdg.h
+++ b/include/drivers/st/stm32_iwdg.h
@@ -15,5 +15,6 @@
 
 int stm32_iwdg_init(void);
 void stm32_iwdg_refresh(void);
+void __dead2 stm32_iwdg_it_handler(int id);
 
 #endif /* STM32_IWDG_H */
diff --git a/include/drivers/st/stm32_rng.h b/include/drivers/st/stm32_rng.h
new file mode 100644
index 0000000000..a644118656
--- /dev/null
+++ b/include/drivers/st/stm32_rng.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright (c) 2018, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef STM32_RNG_H
+#define STM32_RNG_H
+
+int stm32_rng_read(uint8_t *out, uint32_t size);
+int stm32_rng_init(void);
+
+#endif /* STM32_RNG_H */
diff --git a/include/drivers/st/stm32_rtc.h b/include/drivers/st/stm32_rtc.h
new file mode 100644
index 0000000000..128dd2d14a
--- /dev/null
+++ b/include/drivers/st/stm32_rtc.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef STM32_RTC_H
+#define STM32_RTC_H
+
+#include <stdbool.h>
+
+#define RTC_TR		0x00U
+#define RTC_DR		0x04U
+#define RTC_SSR		0x08U
+#define RTC_ICSR	0x0CU
+#define RTC_PRER	0x10U
+#define RTC_WUTR	0x14U
+#define RTC_CR		0x18U
+#define RTC_SMCR	0x20U
+#define RTC_WPR		0x24U
+#define RTC_CALR	0x28U
+#define RTC_SHIFTR	0x2CU
+#define RTC_TSTR	0x30U
+#define RTC_TSDR	0x34U
+#define RTC_TSSSR	0x38U
+#define RTC_ALRMAR	0x40U
+#define RTC_ALRMASSR	0x44U
+#define RTC_ALRMBR	0x48U
+#define RTC_ALRMBSSR	0x4CU
+#define RTC_SR		0x50U
+#define RTC_SCR		0x5CU
+#define RTC_OR		0x60U
+
+struct stm32_rtc_calendar {
+	uint32_t ssr;
+	uint32_t tr;
+	uint32_t dr;
+};
+
+enum months {
+	JANUARY = 1,
+	FEBRUARY,
+	MARCH,
+	APRIL,
+	MAY,
+	JUNE,
+	JULY,
+	AUGUST,
+	SEPTEMBER,
+	OCTOBER,
+	NOVEMBER,
+	DECEMBER,
+	NB_MONTHS = 12
+};
+
+struct stm32_rtc_time {
+	uint32_t hour;
+	uint32_t min;
+	uint32_t sec;
+	uint32_t wday;
+	uint32_t day;
+	enum months month;
+	uint32_t year;
+};
+
+void stm32_rtc_get_calendar(struct stm32_rtc_calendar *calendar);
+unsigned long long stm32_rtc_diff_calendar(struct stm32_rtc_calendar *current,
+					   struct stm32_rtc_calendar *ref);
+void stm32_rtc_set_tamper_timestamp(void);
+bool stm32_rtc_is_timestamp_enable(void);
+void stm32_rtc_get_timestamp(struct stm32_rtc_time *tamp_ts);
+int stm32_rtc_init(void);
+
+/* SMP protection on RTC registers access */
+void stm32_rtc_regs_lock(void);
+void stm32_rtc_regs_unlock(void);
+
+#endif /* STM32_RTC_H */
diff --git a/include/drivers/st/stm32_sdmmc2.h b/include/drivers/st/stm32_sdmmc2.h
index 4853208c2b..5b4bd0e167 100644
--- a/include/drivers/st/stm32_sdmmc2.h
+++ b/include/drivers/st/stm32_sdmmc2.h
@@ -10,6 +10,7 @@
 #include <stdbool.h>
 
 #include <drivers/mmc.h>
+#include <drivers/st/stm32mp_regulator.h>
 
 struct stm32_sdmmc2_params {
 	uintptr_t		reg_base;
@@ -24,6 +25,7 @@ struct stm32_sdmmc2_params {
 	unsigned int		reset_id;
 	unsigned int		max_freq;
 	bool			use_dma;
+	struct stm32mp_regulator vmmc_regu;
 };
 
 unsigned long long stm32_sdmmc2_mmc_get_device_size(void);
diff --git a/include/drivers/st/stm32_tamp.h b/include/drivers/st/stm32_tamp.h
new file mode 100644
index 0000000000..424cbb205a
--- /dev/null
+++ b/include/drivers/st/stm32_tamp.h
@@ -0,0 +1,163 @@
+/*
+ * Copyright (c) 2014-2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef STM32_TAMP_H
+#define STM32_TAMP_H
+
+/* Internal Tamper */
+enum stm32_tamp_int_id {
+	ITAMP1 = 0,
+	ITAMP2,
+	ITAMP3,
+	ITAMP4,
+	ITAMP5,
+	ITAMP6,
+	ITAMP7,
+	ITAMP8,
+	ITAMP9,
+	ITAMP10,
+	ITAMP11,
+	ITAMP12,
+	ITAMP13,
+	ITAMP14,
+	ITAMP15,
+	ITAMP16
+};
+
+/* External Tamper */
+enum stm32_tamp_ext_id {
+	EXT_TAMP1 = 0,
+	EXT_TAMP2,
+	EXT_TAMP3,
+	EXT_TAMP4,
+	EXT_TAMP5,
+	EXT_TAMP6,
+	EXT_TAMP7,
+	EXT_TAMP8
+};
+
+enum stm32_tamp_state {
+	TAMP_DISABLE = 0,
+	TAMP_ENABLE
+};
+
+#define TAMP_UNUSED	{.id = -1}
+
+/* define TAMPER modes */
+#define TAMP_TRIG_OFF		0x0U
+#define TAMP_TRIG_ON		0x1U
+#define TAMP_ACTIVE		0x2U
+#define TAMP_ERASE		0x0U
+#define TAMP_NOERASE		0x1U
+#define TAMP_NO_EVT_MASK	0x0U
+#define TAMP_EVT_MASK		0x1U
+
+/* define Passive FILTER mode */
+#define TAMP_FILTER_PRECHARGE		0x0U
+#define TAMP_FILTER_PULL_UP_DISABLE	0x1U
+#define TAMP_FILTER_DURATION_1_CYCLE	0x0U
+#define TAMP_FILTER_DURATION_2_CYCLES	0x1U
+#define TAMP_FILTER_DURATION_3_CYCLES	0x2U
+#define TAMP_FILTER_DURATION_4_CYCLES	0x3U
+#define TAMP_FILTER_COUNT_1		0x0U
+#define TAMP_FILTER_COUNT_2		0x1U
+#define TAMP_FILTER_COUNT_4		0x2U
+#define TAMP_FILTER_COUNT_8		0x3U
+#define TAMP_FILTER_SAMPLING_32768	0x0U
+#define TAMP_FILTER_SAMPLING_16384	0x1U
+#define TAMP_FILTER_SAMPLING_8192	0x2U
+#define TAMP_FILTER_SAMPLING_4096	0x3U
+#define TAMP_FILTER_SAMPLING_2048	0x4U
+#define TAMP_FILTER_SAMPLING_1024	0x5U
+#define TAMP_FILTER_SAMPLING_512	0x6U
+#define TAMP_FILTER_SAMPLING_256	0x7U
+
+/*  define active filter */
+#define TAMP_ACTIVE_FILTER_OFF		0x0U
+#define TAMP_ACTIVE_FILTER_ON		0x1U
+#define TAMP_ACTIVE_ATO_DEDICATED	0x0U
+#define TAMP_ACTIVE_ATO_TAMPOUTSEL	0x1U
+#define TAMP_ACTIVE_APER_1_OUTPUT	0x0U
+#define TAMP_ACTIVE_APER_2_OUTPUTS	0x1U
+#define TAMP_ACTIVE_APER_3_4_OUTPUTS	0x2U
+#define TAMP_ACTIVE_APER_5_OUTPUTS	0x3U
+#define TAMP_ACTIVE_CKSEL_DIV_0		0x0U
+#define TAMP_ACTIVE_CKSEL_DIV_2		0x1U
+#define TAMP_ACTIVE_CKSEL_DIV_4		0x2U
+#define TAMP_ACTIVE_CKSEL_DIV_8		0x3U
+#define TAMP_ACTIVE_CKSEL_DIV_16	0x4U
+#define TAMP_ACTIVE_CKSEL_DIV_32	0x5U
+#define TAMP_ACTIVE_CKSEL_DIV_64	0x6U
+#define TAMP_ACTIVE_CKSEL_DIV_128	0x7U
+#define TAMP_ACTIVE_ATOSEL_OUT1_(X)	(0x0U << ((X) * 2))
+#define TAMP_ACTIVE_ATOSEL_OUT2_(X)	(0x1U << ((X) * 2))
+#define TAMP_ACTIVE_ATOSEL_OUT3_(X)	(0x2U << ((X) * 2))
+#define TAMP_ACTIVE_ATOSEL_OUT4_(X)	(0x3U << ((X) * 2))
+
+#define TAMP_EXT(tamp_id, trig, erase, mask) (((tamp_id) << 3) | ((trig) << 2)\
+					      | ((erase) << 1) | (mask))
+
+#define TAMP_FLTCR(precharge, duration, count, sample) (((precharge) << 7) |\
+							((duration) << 5) |\
+							((count) << 3) |\
+							(sample))
+
+#define TAMP_ACT(filter, ato, aper, atcksel, atosel) (((filter) << 31) |\
+						      ((ato) << 30) |\
+						      ((aper) << 24) |\
+						      ((atcksel) << 16) |\
+						      (atosel) << 8)
+
+struct stm32_tamp_int {
+	int id;
+	void (*func)(int id);
+};
+
+struct stm32_tamp_ext {
+	int id;
+	uint8_t mode;
+	uint8_t erase;
+	uint8_t evt_mask;
+	void (*func)(int id);
+};
+
+/*
+ * stm32_tamp_write_mcounter : Increase monotonic counter
+ */
+void stm32_tamp_write_mcounter(void);
+
+/*
+ * stm32_tamp_it_handler : Interrupt handler
+ */
+void stm32_tamp_it_handler(void);
+
+/*
+ * stm32_tamp_configure_internal: Configure internal tamper
+ * tamper_list: List of tamper to enable
+ * nb_tamper: Number of tamper in list
+ */
+void stm32_tamp_configure_internal(struct stm32_tamp_int *tamper_list,
+				   uint16_t nb_tamper);
+
+/*
+ * stm32_tamp_configure_external: Configure external tamper and associated
+ * configuration for filtering
+ * ext_tamp_list: List of external tamper to configure
+ * nb_tamper: Number of tamper in list
+ * passive_conf: Filter configuration
+ * active_conf: Configuration for active tamper
+ */
+void stm32_tamp_configure_external(struct stm32_tamp_ext *ext_tamper_list,
+				   uint8_t nb_tamper, uint32_t passive_conf,
+				   uint32_t active_conf);
+
+/*
+ * stm32_tamp_init: Initialize tamper from DT
+ * return 0 if disabled, 1 if enabled, else < 0
+ */
+int stm32_tamp_init(void);
+
+#endif /* STM32_TAMP_H */
diff --git a/include/drivers/st/stm32_timer.h b/include/drivers/st/stm32_timer.h
new file mode 100644
index 0000000000..0e2eb91fe9
--- /dev/null
+++ b/include/drivers/st/stm32_timer.h
@@ -0,0 +1,21 @@
+/*
+ * Copyright (c) 2018-2019, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef STM32_TIMER_H
+#define STM32_TIMER_H
+
+enum timer_cal {
+	HSI_CAL = 0,
+	CSI_CAL
+};
+
+unsigned long stm32_timer_hsi_freq(void);
+unsigned long stm32_timer_csi_freq(void);
+void stm32_timer_freq_func(unsigned long (**timer_freq_cb)(void),
+			   enum timer_cal type);
+int stm32_timer_init(void);
+
+#endif /* STM32_TIMER_H */
diff --git a/include/drivers/st/stm32_uart.h b/include/drivers/st/stm32_uart.h
new file mode 100644
index 0000000000..f00da852fe
--- /dev/null
+++ b/include/drivers/st/stm32_uart.h
@@ -0,0 +1,170 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef STM32_UART_H
+#define STM32_UART_H
+
+/* UART word length */
+#define STM32_UART_WORDLENGTH_7B		USART_CR1_M1
+#define STM32_UART_WORDLENGTH_8B		0x00000000U
+#define STM32_UART_WORDLENGTH_9B		USART_CR1_M0
+
+/* UART number of stop bits */
+#define STM32_UART_STOPBITS_0_5			USART_CR2_STOP_0
+#define STM32_UART_STOPBITS_1			0x00000000U
+#define STM32_UART_STOPBITS_1_5			(USART_CR2_STOP_0 | USART_CR2_STOP_1)
+#define STM32_UART_STOPBITS_2			USART_CR2_STOP_1
+
+/* UART parity */
+#define STM32_UART_PARITY_NONE			0x00000000U
+#define STM32_UART_PARITY_EVEN			USART_CR1_PCE
+#define STM32_UART_PARITY_ODD			(USART_CR1_PCE | USART_CR1_PS)
+
+/* UART transfer mode */
+#define STM32_UART_MODE_RX			USART_CR1_RE
+#define STM32_UART_MODE_TX			USART_CR1_TE
+#define STM32_UART_MODE_TX_RX			(USART_CR1_TE | USART_CR1_RE)
+
+/* UART hardware flow control */
+#define STM32_UART_HWCONTROL_NONE		0x00000000U
+#define STM32_UART_HWCONTROL_RTS		USART_CR3_RTSE
+#define STM32_UART_HWCONTROL_CTS		USART_CR3_CTSE
+#define STM32_UART_HWCONTROL_RTS_CTS		(USART_CR3_RTSE | USART_CR3_CTSE)
+
+/* UART over sampling */
+#define STM32_UART_OVERSAMPLING_16		0x00000000U
+#define STM32_UART_OVERSAMPLING_8		USART_CR1_OVER8
+
+/* UART prescaler */
+#define STM32_UART_PRESCALER_DIV1		0x00000000U
+#define STM32_UART_PRESCALER_DIV2		0x00000001U
+#define STM32_UART_PRESCALER_DIV4		0x00000002U
+#define STM32_UART_PRESCALER_DIV6		0x00000003U
+#define STM32_UART_PRESCALER_DIV8		0x00000004U
+#define STM32_UART_PRESCALER_DIV10		0x00000005U
+#define STM32_UART_PRESCALER_DIV12		0x00000006U
+#define STM32_UART_PRESCALER_DIV16		0x00000007U
+#define STM32_UART_PRESCALER_DIV32		0x00000008U
+#define STM32_UART_PRESCALER_DIV64		0x00000009U
+#define STM32_UART_PRESCALER_DIV128		0x0000000AU
+#define STM32_UART_PRESCALER_DIV256		0x0000000BU
+#define STM32_UART_PRESCALER_MAX		STM32_UART_PRESCALER_DIV256
+
+/* UART fifo mode */
+#define STM32_UART_FIFOMODE_EN			USART_CR1_FIFOEN
+#define STM32_UART_FIFOMODE_DIS			0x00000000U
+
+/* UART TXFIFO threshold level */
+#define STM32_UART_TXFIFO_THRESHOLD_1EIGHTHFULL		0x00000000U
+#define STM32_UART_TXFIFO_THRESHOLD_1QUARTERFUL		USART_CR3_TXFTCFG_0
+#define STM32_UART_TXFIFO_THRESHOLD_HALFFULL		USART_CR3_TXFTCFG_1
+#define STM32_UART_TXFIFO_THRESHOLD_3QUARTERSFULL	(USART_CR3_TXFTCFG_0 | USART_CR3_TXFTCFG_1)
+#define STM32_UART_TXFIFO_THRESHOLD_7EIGHTHFULL		USART_CR3_TXFTCFG_2
+#define STM32_UART_TXFIFO_THRESHOLD_EMPTY		(USART_CR3_TXFTCFG_2 | USART_CR3_TXFTCFG_0)
+
+/* UART RXFIFO threshold level */
+#define STM32_UART_RXFIFO_THRESHOLD_1EIGHTHFULL		0x00000000U
+#define STM32_UART_RXFIFO_THRESHOLD_1QUARTERFULL	USART_CR3_RXFTCFG_0
+#define STM32_UART_RXFIFO_THRESHOLD_HALFFULL		USART_CR3_RXFTCFG_1
+#define STM32_UART_RXFIFO_THRESHOLD_3QUARTERSFULL	(USART_CR3_RXFTCFG_0 | USART_CR3_RXFTCFG_1)
+#define STM32_UART_RXFIFO_THRESHOLD_7EIGHTHFULL		USART_CR3_RXFTCFG_2
+#define STM32_UART_RXFIFO_THRESHOLD_FULL		(USART_CR3_RXFTCFG_2 | USART_CR3_RXFTCFG_0)
+
+struct stm32_uart_init_s {
+	uint32_t baud_rate;		/*
+					 * Configures the UART communication
+					 * baud rate.
+					 */
+
+	uint32_t word_length;		/*
+					 * Specifies the number of data bits
+					 * transmitted or received in a frame.
+					 * This parameter can be a value of
+					 * @ref STM32_UART_WORDLENGTH_*.
+					 */
+
+	uint32_t stop_bits;		/*
+					 * Specifies the number of stop bits
+					 * transmitted. This parameter can be
+					 * a value of @ref STM32_UART_STOPBITS_*.
+					 */
+
+	uint32_t parity;		/*
+					 * Specifies the parity mode.
+					 * This parameter can be a value of
+					 * @ref STM32_UART_PARITY_*.
+					 */
+
+	uint32_t mode;			/*
+					 * Specifies whether the receive or
+					 * transmit mode is enabled or
+					 * disabled. This parameter can be a
+					 * value of @ref @ref STM32_UART_MODE_*.
+					 */
+
+	uint32_t hw_flow_control;	/*
+					 * Specifies whether the hardware flow
+					 * control mode is enabled or
+					 * disabled. This parameter can be a
+					 * value of @ref STM32_UARTHWCONTROL_*.
+					 */
+
+	uint32_t over_sampling;		/*
+					 * Specifies whether the over sampling
+					 * 8 is enabled or disabled.
+					 * This parameter can be a value of
+					 * @ref STM32_UART_OVERSAMPLING_*.
+					 */
+
+	uint32_t one_bit_sampling;	/*
+					 * Specifies whether a single sample
+					 * or three samples' majority vote is
+					 * selected. This parameter can be 0
+					 * or USART_CR3_ONEBIT.
+					 */
+
+	uint32_t prescaler;		/*
+					 * Specifies the prescaler value used
+					 * to divide the UART clock source.
+					 * This parameter can be a value of
+					 * @ref STM32_UART_PRESCALER_*.
+					 */
+
+	uint32_t fifo_mode;		/*
+					 * Specifies if the FIFO mode will be
+					 * used. This parameter can be a value
+					 * of @ref STM32_UART_FIFOMODE_*.
+					 */
+
+	uint32_t tx_fifo_threshold;	/*
+					 * Specifies the TXFIFO threshold
+					 * level. This parameter can be a
+					 * value of @ref
+					 * STM32_UART_TXFIFO_THRESHOLD_*.
+					 */
+
+	uint32_t rx_fifo_threshold;	/*
+					 * Specifies the RXFIFO threshold
+					 * level. This parameter can be a
+					 * value of @ref
+					 * STM32_UART_RXFIFO_THRESHOLD_*.
+					 */
+};
+
+struct stm32_uart_handle_s {
+	uint32_t base;
+	uint32_t rdr_mask;
+};
+
+int stm32_uart_init(struct stm32_uart_handle_s *huart,
+		    uintptr_t base_addr,
+		    const struct stm32_uart_init_s *init);
+void stm32_uart_stop(uintptr_t base_addr);
+int stm32_uart_putc(struct stm32_uart_handle_s *huart, int c);
+int stm32_uart_flush(struct stm32_uart_handle_s *huart);
+int stm32_uart_getc(struct stm32_uart_handle_s *huart);
+
+#endif /* STM32_UART_H */
diff --git a/include/drivers/st/stm32mp1_calib.h b/include/drivers/st/stm32mp1_calib.h
new file mode 100644
index 0000000000..ef69cb4563
--- /dev/null
+++ b/include/drivers/st/stm32mp1_calib.h
@@ -0,0 +1,20 @@
+/*
+ * Copyright (c) 2018-2019, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef STM32MP1_CALIB_H
+#define STM32MP1_CALIB_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+bool stm32mp1_calib_get_wakeup(void);
+void stm32mp1_calib_set_wakeup(bool state);
+void stm32mp1_calib_it_handler(uint32_t id);
+int stm32mp1_calib_start_hsi_cal(void);
+int stm32mp1_calib_start_csi_cal(void);
+void stm32mp1_calib_init(void);
+
+#endif /* STM32MP1_CLK_H */
diff --git a/include/drivers/st/stm32mp1_clk.h b/include/drivers/st/stm32mp1_clk.h
index c46892b78e..c62fb20784 100644
--- a/include/drivers/st/stm32mp1_clk.h
+++ b/include/drivers/st/stm32mp1_clk.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2018-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -22,42 +22,44 @@ enum stm32mp_osc_id {
 
 extern const char *stm32mp_osc_node_label[NB_OSC];
 
+#define PLL1_SETTINGS_VALID_ID	U(0x504C4C31) /* "PLL1" */
+
 int stm32mp1_clk_probe(void);
-int stm32mp1_clk_init(void);
+int stm32mp1_clk_init(uint32_t pll1_freq_mhz);
+
+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);
+void stm32mp1_clk_lp_load_opp_pll1_settings(uint8_t *data, size_t size);
+
+int stm32mp1_clk_get_maxfreq_opp(uint32_t *freq_mhz, uint32_t *voltage_mv);
 
 bool stm32mp1_rcc_is_secure(void);
 bool stm32mp1_rcc_is_mckprot(void);
 
-void __stm32mp1_clk_enable(unsigned long id, bool caller_is_secure);
-void __stm32mp1_clk_disable(unsigned long id, bool caller_is_secure);
+void stm32mp1_clk_force_enable(unsigned long id);
+void stm32mp1_clk_force_disable(unsigned long id);
 
-static inline void stm32mp1_clk_enable_non_secure(unsigned long id)
-{
-	__stm32mp1_clk_enable(id, false);
-}
+bool stm32mp1_rtc_get_read_twice(void);
+
+/* SMP protection on RCC registers access */
+void stm32mp1_clk_rcc_regs_lock(void);
+void stm32mp1_clk_rcc_regs_unlock(void);
 
-static inline void stm32mp1_clk_enable_secure(unsigned long id)
-{
-	__stm32mp1_clk_enable(id, true);
-}
+int stm32mp1_round_opp_khz(uint32_t *freq_khz);
+int stm32mp1_set_opp_khz(uint32_t freq_khz);
 
-static inline void stm32mp1_clk_disable_non_secure(unsigned long id)
-{
-	__stm32mp1_clk_disable(id, false);
-}
+void stm32mp1_clock_suspend(void);
+void stm32mp1_clock_resume(void);
 
-static inline void stm32mp1_clk_disable_secure(unsigned long id)
-{
-	__stm32mp1_clk_disable(id, true);
-}
+void stm32mp1_clock_stopmode_save(void);
+int stm32mp1_clock_stopmode_resume(void);
 
-unsigned int stm32mp1_clk_get_refcount(unsigned long id);
+void restore_clock_pm_context(void);
+void save_clock_pm_context(void);
 
-/* SMP protection on RCC registers access */
-void stm32mp1_clk_rcc_regs_lock(void);
-void stm32mp1_clk_rcc_regs_unlock(void);
+void stm32mp1_clk_mcuss_protect(bool enable);
 
-void stm32mp1_stgen_increment(unsigned long long offset_in_ms);
+void stm32mp1_dump_clocks_state(void);
 
 #ifdef STM32MP_SHARED_RESOURCES
 void stm32mp1_register_clock_parents_secure(unsigned long id);
diff --git a/include/drivers/st/stm32mp1_ddr.h b/include/drivers/st/stm32mp1_ddr.h
index 4ab37d6b44..245ad52cc2 100644
--- a/include/drivers/st/stm32mp1_ddr.h
+++ b/include/drivers/st/stm32mp1_ddr.h
@@ -8,9 +8,6 @@
 #define STM32MP1_DDR_H
 
 #include <stdbool.h>
-#include <stdint.h>
-
-#define DT_DDR_COMPAT	"st,stm32mp1-ddr"
 
 struct stm32mp1_ddr_size {
 	uint64_t base;
@@ -101,12 +98,14 @@ struct stm32mp1_ddrctrl_perf {
 	uint32_t pcfgqos1_0;
 	uint32_t pcfgwqos0_0;
 	uint32_t pcfgwqos1_0;
+#if STM32MP_DDR_DUAL_AXI_PORT
 	uint32_t pcfgr_1;
 	uint32_t pcfgw_1;
 	uint32_t pcfgqos0_1;
 	uint32_t pcfgqos1_1;
 	uint32_t pcfgwqos0_1;
 	uint32_t pcfgwqos1_1;
+#endif
 };
 
 struct stm32mp1_ddrphy_reg {
@@ -119,8 +118,10 @@ struct stm32mp1_ddrphy_reg {
 	uint32_t zq0cr1;
 	uint32_t dx0gcr;
 	uint32_t dx1gcr;
+#if STM32MP_DDR_DUAL_AXI_PORT
 	uint32_t dx2gcr;
 	uint32_t dx3gcr;
+#endif
 };
 
 struct stm32mp1_ddrphy_timing {
@@ -143,12 +144,14 @@ struct stm32mp1_ddrphy_cal {
 	uint32_t dx1dllcr;
 	uint32_t dx1dqtr;
 	uint32_t dx1dqstr;
+#if STM32MP_DDR_DUAL_AXI_PORT
 	uint32_t dx2dllcr;
 	uint32_t dx2dqtr;
 	uint32_t dx2dqstr;
 	uint32_t dx3dllcr;
 	uint32_t dx3dqtr;
 	uint32_t dx3dqstr;
+#endif
 };
 
 struct stm32mp1_ddr_info {
@@ -166,9 +169,13 @@ struct stm32mp1_ddr_config {
 	struct stm32mp1_ddrphy_reg p_reg;
 	struct stm32mp1_ddrphy_timing p_timing;
 	struct stm32mp1_ddrphy_cal p_cal;
+	bool p_cal_present;
+	bool self_refresh;
+	uint32_t zdata;
 };
 
 int stm32mp1_ddr_clk_enable(struct ddr_info *priv, uint32_t mem_speed);
 void stm32mp1_ddr_init(struct ddr_info *priv,
 		       struct stm32mp1_ddr_config *config);
+
 #endif /* STM32MP1_DDR_H */
diff --git a/include/drivers/st/stm32mp1_ddr_helpers.h b/include/drivers/st/stm32mp1_ddr_helpers.h
index 38f24152a9..f09f05d555 100644
--- a/include/drivers/st/stm32mp1_ddr_helpers.h
+++ b/include/drivers/st/stm32mp1_ddr_helpers.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,6 +7,24 @@
 #ifndef STM32MP1_DDR_HELPERS_H
 #define STM32MP1_DDR_HELPERS_H
 
+#include <stdbool.h>
+#include <stdint.h>
+
+enum stm32mp1_ddr_sr_mode {
+	DDR_SR_MODE_INVALID = 0,
+	DDR_SSR_MODE,
+	DDR_HSR_MODE,
+	DDR_ASR_MODE,
+};
+
 void ddr_enable_clock(void);
+int ddr_sw_self_refresh_exit(void);
+uint32_t ddr_get_io_calibration_val(void);
+int ddr_standby_sr_entry(void);
+enum stm32mp1_ddr_sr_mode ddr_read_sr_mode(void);
+void ddr_set_sr_mode(enum stm32mp1_ddr_sr_mode mode);
+void ddr_save_sr_mode(void);
+void ddr_restore_sr_mode(void);
+bool ddr_is_nonsecured_area(uintptr_t address, uint32_t length);
 
 #endif /* STM32MP1_DDR_HELPERS_H */
diff --git a/include/drivers/st/stm32mp1_ddr_regs.h b/include/drivers/st/stm32mp1_ddr_regs.h
index 342239a52d..e298fcf77b 100644
--- a/include/drivers/st/stm32mp1_ddr_regs.h
+++ b/include/drivers/st/stm32mp1_ddr_regs.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
  */
@@ -128,6 +128,7 @@ struct stm32mp1_ddrctl {
 	uint32_t pcfgwqos1_0;	/* 0x4a0 Write QoS Configuration 1 */
 	uint8_t reserved4a4[0x4b4 - 0x4a4];
 
+#if STM32MP_DDR_DUAL_AXI_PORT
 	/* PORT 1 */
 	uint32_t pcfgr_1;	/* 0x4b4 Configuration Read */
 	uint32_t pcfgw_1;	/* 0x4b8 Configuration Write */
@@ -137,6 +138,7 @@ struct stm32mp1_ddrctl {
 	uint32_t pcfgqos1_1;	/* 0x548 Read QoS Configuration 1 */
 	uint32_t pcfgwqos0_1;	/* 0x54c Write QoS Configuration 0 */
 	uint32_t pcfgwqos1_1;	/* 0x550 Write QoS Configuration 1 */
+#endif
 } __packed;
 
 /* DDR Physical Interface Control (DDRPHYC) registers*/
@@ -214,6 +216,7 @@ struct stm32mp1_ddrphy {
 	uint32_t dx1dqtr;	/* 0x210 Byte lane 1 DQ Timing */
 	uint32_t dx1dqstr;	/* 0x214 Byte lane 1 QS Timing */
 	uint8_t res6[0x240 - 0x218];	/* 0x218 */
+#if STM32MP_DDR_DUAL_AXI_PORT
 	uint32_t dx2gcr;	/* 0x240 Byte lane 2 General Configuration */
 	uint32_t dx2gsr0;	/* 0x244 Byte lane 2 General Status 0 */
 	uint32_t dx2gsr1;	/* 0x248 Byte lane 2 General Status 1 */
@@ -227,6 +230,7 @@ struct stm32mp1_ddrphy {
 	uint32_t dx3dllcr;	/* 0x28c Byte lane 3 DLL Control */
 	uint32_t dx3dqtr;	/* 0x290 Byte lane 3 DQ Timing */
 	uint32_t dx3dqstr;	/* 0x294 Byte lane 3 QS Timing */
+#endif
 } __packed;
 
 /* DDR Controller registers offsets */
@@ -249,7 +253,9 @@ struct stm32mp1_ddrphy {
 #define DDRCTRL_SWSTAT				0x324
 #define DDRCTRL_PSTAT				0x3FC
 #define DDRCTRL_PCTRL_0				0x490
+#if STM32MP_DDR_DUAL_AXI_PORT
 #define DDRCTRL_PCTRL_1				0x540
+#endif
 
 /* DDR Controller Register fields */
 #define DDRCTRL_MSTR_DDR3			BIT(0)
@@ -284,7 +290,7 @@ struct stm32mp1_ddrphy {
 #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(19, 12)
+#define DDRCTRL_PWRTMG_SELFREF_TO_X32_MASK	GENMASK(23, 16)
 #define DDRCTRL_PWRTMG_SELFREF_TO_X32_0		BIT(16)
 
 #define DDRCTRL_RFSHCTL3_DIS_AUTO_REFRESH	BIT(0)
@@ -339,10 +345,12 @@ struct stm32mp1_ddrphy {
 #define DDRPHYC_DX0DLLCR			0x1CC
 #define DDRPHYC_DX1GCR				0x200
 #define DDRPHYC_DX1DLLCR			0x20C
+#if STM32MP_DDR_DUAL_AXI_PORT
 #define DDRPHYC_DX2GCR				0x240
 #define DDRPHYC_DX2DLLCR			0x24C
 #define DDRPHYC_DX3GCR				0x280
 #define DDRPHYC_DX3DLLCR			0x28C
+#endif
 
 /* DDR PHY Register fields */
 #define DDRPHYC_PIR_INIT			BIT(0)
@@ -380,6 +388,7 @@ struct stm32mp1_ddrphy {
 #define DDRPHYC_PTR0_TITMSRST_OFFSET		18
 #define DDRPHYC_PTR0_TITMSRST_MASK		GENMASK(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(10, 8)
@@ -399,6 +408,7 @@ struct stm32mp1_ddrphy {
 #define DDRPHYC_DSGCR_ODTPDD_MASK		GENMASK(23, 20)
 #define DDRPHYC_DSGCR_ODTPDD_0			BIT(20)
 #define DDRPHYC_DSGCR_NL2PD			BIT(24)
+#define DDRPHYC_DSGCR_CKOE			BIT(28)
 
 #define DDRPHYC_ZQ0CRN_ZDATA_MASK		GENMASK(27, 0)
 #define DDRPHYC_ZQ0CRN_ZDATA_SHIFT		0
diff --git a/include/drivers/st/stm32mp1_pwr.h b/include/drivers/st/stm32mp1_pwr.h
index e17df44fb7..9b662f2d1c 100644
--- a/include/drivers/st/stm32mp1_pwr.h
+++ b/include/drivers/st/stm32mp1_pwr.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -13,20 +13,39 @@
 #define PWR_CR2			U(0x08)
 #define PWR_CR3			U(0x0C)
 #define PWR_MPUCR		U(0x10)
+#define PWR_MCUCR		U(0x14)
 #define PWR_WKUPCR		U(0x20)
 #define PWR_MPUWKUPENR		U(0x28)
 
+#define PWR_OFFSET_MASK		GENMASK(9, 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(27, 16) | GENMASK(13, 8) | GENMASK(5, 0)
+
+#define PWR_MPUWKUPENR_MASK	GENMASK(5, 0)
+
 #endif /* STM32MP1_PWR_H */
diff --git a/include/drivers/st/stm32mp1_ram.h b/include/drivers/st/stm32mp1_ram.h
index 38360e7595..adecd409df 100644
--- a/include/drivers/st/stm32mp1_ram.h
+++ b/include/drivers/st/stm32mp1_ram.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2018, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2015-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,6 +7,7 @@
 #ifndef STM32MP1_RAM_H
 #define STM32MP1_RAM_H
 
+bool stm32mp1_ddr_is_restored(void);
 int stm32mp1_ddr_probe(void);
 
 #endif /* STM32MP1_RAM_H */
diff --git a/include/drivers/st/stm32mp1_rcc.h b/include/drivers/st/stm32mp1_rcc.h
index 2ffc3b2bc2..feaac43a13 100644
--- a/include/drivers/st/stm32mp1_rcc.h
+++ b/include/drivers/st/stm32mp1_rcc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2015-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -394,7 +394,8 @@
 #define RCC_HSICFGR_HSITRIM_SHIFT	8
 #define RCC_HSICFGR_HSITRIM_MASK	GENMASK(14, 8)
 #define RCC_HSICFGR_HSICAL_SHIFT	16
-#define RCC_HSICFGR_HSICAL_MASK		GENMASK(27, 16)
+#define RCC_HSICFGR_HSICAL_MASK		GENMASK(24, 16)
+#define RCC_HSICFGR_HSICAL_TEMP_MASK	GENMASK(27, 25)
 
 /* Fields of RCC_CSICFGR register */
 #define RCC_CSICFGR_CSITRIM_SHIFT	8
@@ -453,12 +454,18 @@
 #define RCC_MP_SREQCLRR_STPREQ_P0	BIT(0)
 #define RCC_MP_SREQCLRR_STPREQ_P1	BIT(1)
 
+/* Global Control Register */
+#define RCC_MP_GCR_BOOT_MCU		BIT(0)
+
 /* Values of RCC_UART24CKSELR register */
 #define RCC_UART24CKSELR_HSI		0x00000002
 
 /* Values of RCC_MP_APB1ENSETR register */
 #define RCC_MP_APB1ENSETR_UART4EN	BIT(16)
 
+/* Values of RCC_MP_APB4ENSETR register */
+#define RCC_MP_APB4ENSETR_IWDG2APBEN	BIT(15)
+
 /* Values of RCC_MP_APB5ENSETR register */
 #define RCC_MP_APB5ENSETR_SPI6EN	BIT(0)
 #define RCC_MP_APB5ENSETR_I2C4EN	BIT(2)
@@ -466,8 +473,15 @@
 #define RCC_MP_APB5ENSETR_USART1EN	BIT(4)
 #define RCC_MP_APB5ENSETR_RTCAPBEN	BIT(8)
 #define RCC_MP_APB5ENSETR_IWDG1APBEN	BIT(15)
+#define RCC_MP_APB5ENSETR_STGENEN	BIT(20)
 
 /* Values of RCC_MP_AHB4ENSETR register */
+#define RCC_MP_AHB4ENSETR_GPIOAEN	BIT(0)
+#define RCC_MP_AHB4ENSETR_GPIOBEN	BIT(1)
+#define RCC_MP_AHB4ENSETR_GPIOCEN	BIT(2)
+#define RCC_MP_AHB4ENSETR_GPIODEN	BIT(3)
+#define RCC_MP_AHB4ENSETR_GPIOEEN	BIT(4)
+#define RCC_MP_AHB4ENSETR_GPIOFEN	BIT(5)
 #define RCC_MP_AHB4ENSETR_GPIOGEN	BIT(6)
 #define RCC_MP_AHB4ENSETR_GPIOHEN	BIT(7)
 
@@ -477,6 +491,12 @@
 #define RCC_MP_AHB5ENSETR_HASH1EN	BIT(5)
 #define RCC_MP_AHB5ENSETR_RNG1EN	BIT(6)
 
+/* Values of RCC_AHB6RSTSETR register */
+#define RCC_AHB6RSTSETR_GPURST		BIT(5)
+
+/* Values of RCC_MP_APB5LPENSETR register */
+#define RCC_MP_APB5LPENSETR_STGENSTPEN	BIT(21)
+
 /* Values of RCC_MP_IWDGFZSETR register */
 #define RCC_MP_IWDGFZSETR_IWDG1		BIT(0)
 #define RCC_MP_IWDGFZSETR_IWDG2		BIT(1)
@@ -562,4 +582,12 @@
 #define RCC_USBCKSELR_USBOSRC_MASK		BIT(4)
 #define RCC_USBCKSELR_USBOSRC_SHIFT		4
 
+/* RCC_MPCKSELR register fields */
+#define RCC_MPCKSELR_MPUSRC_MASK		GENMASK(1, 0)
+#define RCC_MPCKSELR_MPUSRC_SHIFT		0
+
+/* RCC_CPERCKSELR register fields */
+#define RCC_CPERCKSELR_PERSRC_MASK		GENMASK(1, 0)
+#define RCC_CPERCKSELR_PERSRC_SHIFT		0
+
 #endif /* STM32MP1_RCC_H */
diff --git a/include/drivers/st/stm32mp_clkfunc.h b/include/drivers/st/stm32mp_clkfunc.h
index c7e0b6e6fb..1386fa6e29 100644
--- a/include/drivers/st/stm32mp_clkfunc.h
+++ b/include/drivers/st/stm32mp_clkfunc.h
@@ -14,18 +14,29 @@
 #include <platform_def.h>
 
 int fdt_osc_read_freq(const char *name, uint32_t *freq);
-bool fdt_osc_read_bool(enum stm32mp_osc_id osc_id, const char *prop_name);
-uint32_t fdt_osc_read_uint32_default(enum stm32mp_osc_id osc_id,
+bool fdt_clk_read_bool(const char *node_label, const char *prop_name);
+uint32_t fdt_clk_read_uint32_default(const char *node_label,
 				     const char *prop_name,
 				     uint32_t dflt_value);
 
-int fdt_get_rcc_node(void *fdt);
 int fdt_rcc_read_uint32_array(const char *prop_name, uint32_t count,
 			      uint32_t *array);
+uint32_t fdt_rcc_read_uint32_default(const char *prop_name,
+				     uint32_t dflt_value);
 int fdt_rcc_subnode_offset(const char *name);
 const fdt32_t *fdt_rcc_read_prop(const char *prop_name, int *lenp);
-bool fdt_get_rcc_secure_status(void);
+bool fdt_get_rcc_secure_state(void);
+int fdt_rcc_enable_it(const char *name);
 
 int fdt_get_clock_id(int node);
+int fdt_get_clock_id_by_name(int node, const char *name);
+unsigned long fdt_get_uart_clock_freq(uintptr_t instance);
+
+bool fdt_is_pll1_predefined(void);
+
+void stm32mp_stgen_config(unsigned long rate);
+void stm32mp_stgen_restore_counter(unsigned long long value,
+				   unsigned long long offset_in_ms);
+unsigned long long stm32mp_stgen_get_counter(void);
 
 #endif /* STM32MP_CLKFUNC_H */
diff --git a/include/drivers/st/stm32mp_dummy_regulator.h b/include/drivers/st/stm32mp_dummy_regulator.h
new file mode 100644
index 0000000000..6804192ba0
--- /dev/null
+++ b/include/drivers/st/stm32mp_dummy_regulator.h
@@ -0,0 +1,14 @@
+/*
+ * Copyright (C) 2019, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef STM32MP_DUMMY_REGULATOR_H
+#define STM32MP_DUMMY_REGULATOR_H
+
+#include <drivers/st/stm32mp_regulator.h>
+
+void bind_dummy_regulator(struct stm32mp_regulator *regu);
+
+#endif /* STM32MP_DUMMY_REGULATOR_H */
diff --git a/include/drivers/st/stm32mp_pmic.h b/include/drivers/st/stm32mp_pmic.h
index 984cd60143..898a28b44c 100644
--- a/include/drivers/st/stm32mp_pmic.h
+++ b/include/drivers/st/stm32mp_pmic.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -11,6 +11,8 @@
 
 #include <platform_def.h>
 
+#include <drivers/st/stm32mp_regulator.h>
+
 /*
  * dt_pmic_status - Check PMIC status from device tree
  *
@@ -25,7 +27,24 @@ int dt_pmic_status(void);
  *
  * Returns 0 on success, and negative values on errors
  */
-int dt_pmic_configure_boot_on_regulators(void);
+int pmic_configure_boot_on_regulators(void);
+
+int pmic_set_lp_config(const char *node_name);
+
+/*
+ * dt_pmic_find_supply - Find the supply name related to a regulator name
+ *
+ * Returns 0 on success, and negative values on errors
+ */
+int dt_pmic_find_supply(const char **supply_name, const char *regu_name);
+
+/*
+ * pmic_set_regulator_min_voltage - Set target supply to its device tree
+ * "regulator-min-microvolt" value.
+ *
+ * Returns 0 on success, and negative values on errors
+ */
+int pmic_set_regulator_min_voltage(const char *regu_name);
 
 /*
  * initialize_pmic_i2c - Initialize I2C for the PMIC control
@@ -41,6 +60,24 @@ bool initialize_pmic_i2c(void);
  */
 void initialize_pmic(void);
 
+/*
+ * configure_pmic - PMIC configuration function, called at platform init
+ *
+ * Panics on errors
+ */
+void configure_pmic(void);
+
+#if DEBUG
+void print_pmic_info_and_debug(void);
+#else
+static inline void print_pmic_info_and_debug(void)
+{
+}
+#endif
+
+bool is_pmic_regulator(struct stm32mp_regulator *regu);
+void bind_pmic_regulator(struct stm32mp_regulator *regu);
+
 /*
  * pmic_ddr_power_init - Initialize regulators required for DDR
  *
diff --git a/include/drivers/st/stm32mp_regulator.h b/include/drivers/st/stm32mp_regulator.h
new file mode 100644
index 0000000000..7a66b97ba7
--- /dev/null
+++ b/include/drivers/st/stm32mp_regulator.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) 2019, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef STM32MP_REGULATOR_H
+#define STM32MP_REGULATOR_H
+
+#include <stdbool.h>
+
+struct stm32mp_regulator;
+
+struct stm32mp_regulator_ops {
+	int (*enable)(struct stm32mp_regulator *regu);
+	int (*disable)(struct stm32mp_regulator *regu);
+};
+
+struct stm32mp_regulator {
+	const struct stm32mp_regulator_ops *ops;
+	int id;
+	bool always_on;
+};
+
+int stm32mp_regulator_enable(struct stm32mp_regulator *regu);
+int stm32mp_regulator_disable(struct stm32mp_regulator *regu);
+int stm32mp_regulator_register(struct stm32mp_regulator *regu);
+
+int plat_bind_regulator(struct stm32mp_regulator *regu);
+
+#endif /* STM32MP_REGULATOR_H */
diff --git a/include/drivers/st/stm32mp_reset.h b/include/drivers/st/stm32mp_reset.h
index 84448050d7..8c43c0b5b1 100644
--- a/include/drivers/st/stm32mp_reset.h
+++ b/include/drivers/st/stm32mp_reset.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2018-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -47,4 +47,16 @@ static inline void stm32mp_reset_release(uint32_t reset_id)
 	(void)stm32mp_reset_deassert(reset_id, 0U);
 }
 
+/*
+ * Manage reset control for the MCU reset
+ *
+ * @assert_not_deassert: reset requested state
+ */
+void stm32mp_reset_assert_deassert_to_mcu(bool assert_not_deassert);
+
+/*
+ * Manage system reset control
+ */
+void __dead2 stm32mp_system_reset(void);
+
 #endif /* STM32MP_RESET_H */
diff --git a/include/drivers/st/stpmic1.h b/include/drivers/st/stpmic1.h
index f7e293b189..5c8933d84d 100644
--- a/include/drivers/st/stpmic1.h
+++ b/include/drivers/st/stpmic1.h
@@ -86,15 +86,15 @@
 #define ITSOURCE4_REG			0xB3U
 
 /* Registers masks */
-#define LDO_VOLTAGE_MASK		0x7CU
-#define BUCK_VOLTAGE_MASK		0xFCU
+#define LDO_VOLTAGE_MASK		GENMASK(6, 2)
+#define BUCK_VOLTAGE_MASK		GENMASK(7, 2)
 #define LDO_BUCK_VOLTAGE_SHIFT		2
-#define LDO_BUCK_ENABLE_MASK		0x01U
-#define LDO_BUCK_HPLP_ENABLE_MASK	0x02U
+#define LDO_BUCK_ENABLE_MASK		BIT(0)
+#define LDO_BUCK_HPLP_ENABLE_MASK	BIT(1)
 #define LDO_BUCK_HPLP_SHIFT		1
-#define LDO_BUCK_RANK_MASK		0x01U
-#define LDO_BUCK_RESET_MASK		0x01U
-#define LDO_BUCK_PULL_DOWN_MASK		0x03U
+#define LDO_BUCK_RANK_MASK		BIT(0)
+#define LDO_BUCK_RESET_MASK		BIT(0)
+#define LDO_BUCK_PULL_DOWN_MASK		GENMASK(1, 0)
 
 /* Pull down register */
 #define BUCK1_PULL_DOWN_SHIFT		0
@@ -135,12 +135,12 @@
 /* Main PMIC VINLOW Control Register (VIN_CONTROL_REGC DMSC) */
 #define SWIN_DETECTOR_ENABLED		BIT(7)
 #define SWOUT_DETECTOR_ENABLED          BIT(6)
-#define VINLOW_HYST_MASK		0x3
+#define VINLOW_HYST_MASK		GENMASK(1, 0)
 #define VINLOW_HYST_SHIFT		4
-#define VINLOW_THRESHOLD_MASK		0x7
+#define VINLOW_THRESHOLD_MASK		GENMASK(2, 0)
 #define VINLOW_THRESHOLD_SHIFT		1
-#define VINLOW_ENABLED			0x01
-#define VINLOW_CTRL_REG_MASK		0xFF
+#define VINLOW_ENABLED			BIT(0)
+#define VINLOW_CTRL_REG_MASK		GENMASK(7, 0)
 
 /* USB Control Register */
 #define BOOST_OVP_DISABLED		BIT(7)
@@ -148,6 +148,7 @@
 #define OCP_LIMIT_HIGH			BIT(3)
 #define SWIN_SWOUT_ENABLED		BIT(2)
 #define USBSW_OTG_SWITCH_ENABLED	BIT(1)
+#define BOOST_ENABLED			BIT(0)
 
 int stpmic1_powerctrl_on(void);
 int stpmic1_switch_off(void);
@@ -156,11 +157,15 @@ int stpmic1_register_write(uint8_t register_id, uint8_t value);
 int stpmic1_register_update(uint8_t register_id, uint8_t value, uint8_t mask);
 int stpmic1_regulator_enable(const char *name);
 int stpmic1_regulator_disable(const char *name);
-uint8_t stpmic1_is_regulator_enabled(const char *name);
+bool stpmic1_is_regulator_enabled(const char *name);
 int stpmic1_regulator_voltage_set(const char *name, uint16_t millivolts);
 int stpmic1_regulator_voltage_get(const char *name);
 int stpmic1_regulator_pull_down_set(const char *name);
 int stpmic1_regulator_mask_reset_set(const char *name);
+int stpmic1_lp_copy_reg(const char *name);
+int stpmic1_lp_reg_on_off(const char *name, uint8_t enable);
+int stpmic1_lp_set_mode(const char *name, uint8_t hplp);
+int stpmic1_lp_set_voltage(const char *name, uint16_t millivolts);
 void stpmic1_bind_i2c(struct i2c_handle_s *i2c_handle, uint16_t i2c_addr);
 
 int stpmic1_get_version(unsigned long *version);
diff --git a/include/drivers/st/usb_dwc2.h b/include/drivers/st/usb_dwc2.h
new file mode 100644
index 0000000000..58a0e4b6e5
--- /dev/null
+++ b/include/drivers/st/usb_dwc2.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2015-2019, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef USB_DWC2_H
+#define USB_DWC2_H
+
+#include <lib/usb/usb_core.h>
+
+#define USB_MAX_ENDPOINT_NB			0x10
+
+void usb_dwc2_init_driver(usb_handle_t *usb_core_handle,
+			  pcd_handle_t *pcd_handle,
+			  void *base_register);
+
+#endif /* USB_DWC2_H */
+
diff --git a/include/dt-bindings/clock/stm32mp1-clks.h b/include/dt-bindings/clock/stm32mp1-clks.h
index 67e66b23fd..cfda43310b 100644
--- a/include/dt-bindings/clock/stm32mp1-clks.h
+++ b/include/dt-bindings/clock/stm32mp1-clks.h
@@ -179,6 +179,12 @@
 #define DAC12_K		168
 #define ETHPTP_K	169
 
+#define PCLK1		170
+#define PCLK2		171
+#define PCLK3		172
+#define PCLK4		173
+#define PCLK5		174
+
 /* PLL */
 #define PLL1		176
 #define PLL2		177
diff --git a/include/dt-bindings/power/stm32mp1-power.h b/include/dt-bindings/power/stm32mp1-power.h
new file mode 100644
index 0000000000..d588dd71f3
--- /dev/null
+++ b/include/dt-bindings/power/stm32mp1-power.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0 or BSD-3-Clause */
+/*
+ * Copyright (C) 2018-2019, STMicroelectronics - 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/include/dt-bindings/reset/stm32mp1-resets.h b/include/dt-bindings/reset/stm32mp1-resets.h
index bc71924faa..f3a0ed3178 100644
--- a/include/dt-bindings/reset/stm32mp1-resets.h
+++ b/include/dt-bindings/reset/stm32mp1-resets.h
@@ -7,6 +7,7 @@
 #ifndef _DT_BINDINGS_STM32MP1_RESET_H_
 #define _DT_BINDINGS_STM32MP1_RESET_H_
 
+#define MCU_HOLD_BOOT_R	2144
 #define LTDC_R		3072
 #define DSI_R		3076
 #define DDRPERFM_R	3080
@@ -117,5 +118,6 @@
 #define RST_SCMI0_RNG1		8
 #define RST_SCMI0_MDMA		9
 #define RST_SCMI0_MCU		10
+#define RST_SCMI0_MCU_HOLD_BOOT	11
 
 #endif /* _DT_BINDINGS_STM32MP1_RESET_H_ */
diff --git a/include/dt-bindings/soc/st,stm32-etzpc.h b/include/dt-bindings/soc/st,stm32-etzpc.h
index 3f9fb3b12f..199c83154d 100644
--- a/include/dt-bindings/soc/st,stm32-etzpc.h
+++ b/include/dt-bindings/soc/st,stm32-etzpc.h
@@ -17,4 +17,90 @@
 #define DECPROT_UNLOCK		0x0
 #define DECPROT_LOCK		0x1
 
+/* 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_RNG1_ID		7
+#define STM32MP1_ETZPC_HASH1_ID		8
+#define STM32MP1_ETZPC_CRYP1_ID		9
+#define STM32MP1_ETZPC_DDRCTRL_ID	10
+#define STM32MP1_ETZPC_DDRPHYC_ID	11
+#define STM32MP1_ETZPC_I2C6_ID		12
+#define STM32MP1_ETZPC_TIM2_ID		16
+#define STM32MP1_ETZPC_TIM3_ID		17
+#define STM32MP1_ETZPC_TIM4_ID		18
+#define STM32MP1_ETZPC_TIM5_ID		19
+#define STM32MP1_ETZPC_TIM6_ID		20
+#define STM32MP1_ETZPC_TIM7_ID		21
+#define STM32MP1_ETZPC_TIM12_ID		22
+#define STM32MP1_ETZPC_TIM13_ID		23
+#define STM32MP1_ETZPC_TIM14_ID		24
+#define STM32MP1_ETZPC_LPTIM1_ID	25
+#define STM32MP1_ETZPC_WWDG1_ID		26
+#define STM32MP1_ETZPC_SPI2_ID		27
+#define STM32MP1_ETZPC_SPI3_ID		28
+#define STM32MP1_ETZPC_SPDIFRX_ID	29
+#define STM32MP1_ETZPC_USART2_ID	30
+#define STM32MP1_ETZPC_USART3_ID	31
+#define STM32MP1_ETZPC_UART4_ID		32
+#define STM32MP1_ETZPC_UART5_ID		33
+#define STM32MP1_ETZPC_I2C1_ID		34
+#define STM32MP1_ETZPC_I2C2_ID		35
+#define STM32MP1_ETZPC_I2C3_ID		36
+#define STM32MP1_ETZPC_I2C5_ID		37
+#define STM32MP1_ETZPC_CEC_ID		38
+#define STM32MP1_ETZPC_DAC_ID		39
+#define STM32MP1_ETZPC_UART7_ID		40
+#define STM32MP1_ETZPC_UART8_ID		41
+#define STM32MP1_ETZPC_MDIOS_ID		44
+#define STM32MP1_ETZPC_TIM1_ID		48
+#define STM32MP1_ETZPC_TIM8_ID		49
+#define STM32MP1_ETZPC_USART6_ID	51
+#define STM32MP1_ETZPC_SPI1_ID		52
+#define STM32MP1_ETZPC_SPI4_ID		53
+#define STM32MP1_ETZPC_TIM15_ID		54
+#define STM32MP1_ETZPC_TIM16_ID		55
+#define STM32MP1_ETZPC_TIM17_ID		56
+#define STM32MP1_ETZPC_SPI5_ID		57
+#define STM32MP1_ETZPC_SAI1_ID		58
+#define STM32MP1_ETZPC_SAI2_ID		59
+#define STM32MP1_ETZPC_SAI3_ID		60
+#define STM32MP1_ETZPC_DFSDM_ID		61
+#define STM32MP1_ETZPC_TT_FDCAN_ID	62
+#define STM32MP1_ETZPC_LPTIM2_ID	64
+#define STM32MP1_ETZPC_LPTIM3_ID	65
+#define STM32MP1_ETZPC_LPTIM4_ID	66
+#define STM32MP1_ETZPC_LPTIM5_ID	67
+#define STM32MP1_ETZPC_SAI4_ID		68
+#define STM32MP1_ETZPC_VREFBUF_ID	69
+#define STM32MP1_ETZPC_DCMI_ID		70
+#define STM32MP1_ETZPC_CRC2_ID		71
+#define STM32MP1_ETZPC_ADC_ID		72
+#define STM32MP1_ETZPC_HASH2_ID		73
+#define STM32MP1_ETZPC_RNG2_ID		74
+#define STM32MP1_ETZPC_CRYP2_ID		75
+#define STM32MP1_ETZPC_SRAM1_ID		80
+#define STM32MP1_ETZPC_SRAM2_ID		81
+#define STM32MP1_ETZPC_SRAM3_ID		82
+#define STM32MP1_ETZPC_SRAM4_ID		83
+#define STM32MP1_ETZPC_RETRAM_ID	84
+#define STM32MP1_ETZPC_OTG_ID		85
+#define STM32MP1_ETZPC_SDMMC3_ID	86
+#define STM32MP1_ETZPC_DLYBSD3_ID	87
+#define STM32MP1_ETZPC_DMA1_ID		88
+#define STM32MP1_ETZPC_DMA2_ID		89
+#define STM32MP1_ETZPC_DMAMUX_ID	90
+#define STM32MP1_ETZPC_FMC_ID		91
+#define STM32MP1_ETZPC_QSPI_ID		92
+#define STM32MP1_ETZPC_DLYBQ_ID		93
+#define STM32MP1_ETZPC_ETH_ID		94
+
+#define STM32MP1_ETZPC_MAX_ID		96
+
+#define DECPROT(id, mode, lock)		(((id) << 16) | ((mode) << 8) | (lock))
+
 #endif /* _DT_BINDINGS_STM32_ETZPC_H */
diff --git a/include/dt-bindings/soc/stm32mp1-tzc400.h b/include/dt-bindings/soc/stm32mp1-tzc400.h
new file mode 100644
index 0000000000..88fbcdd814
--- /dev/null
+++ b/include/dt-bindings/soc/stm32mp1-tzc400.h
@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
+/*
+ * Copyright (C) 2020, STMicroelectronics - All Rights Reserved
+ * Author(s): Lionel DEBIEVE <lionel.debieve@st.com> for STMicroelectronics.
+ */
+
+#ifndef _DT_BINDINGS_STM32MP1_TZC400_H
+#define _DT_BINDINGS_STM32MP1_TZC400_H
+
+#include <drivers/arm/tzc_common.h>
+
+#define STM32MP1_TZC_A7_ID		U(0)
+#define STM32MP1_TZC_M4_ID		U(1)
+#define STM32MP1_TZC_LCD_ID		U(3)
+#define STM32MP1_TZC_GPU_ID		U(4)
+#define STM32MP1_TZC_MDMA_ID		U(5)
+#define STM32MP1_TZC_DMA_ID		U(6)
+#define STM32MP1_TZC_USB_HOST_ID	U(7)
+#define STM32MP1_TZC_USB_OTG_ID		U(8)
+#define STM32MP1_TZC_SDMMC_ID		U(9)
+#define STM32MP1_TZC_ETH_ID		U(10)
+#define STM32MP1_TZC_DAP_ID		U(15)
+
+#define TZC_REGION_NSEC_ALL_ACCESS_RDWR \
+	(TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_A7_ID) | \
+	 TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_GPU_ID) | \
+	 TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_LCD_ID) | \
+	 TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_MDMA_ID) | \
+	 TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_M4_ID) | \
+	 TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_DMA_ID) | \
+	 TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_USB_HOST_ID) | \
+	 TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_USB_OTG_ID) | \
+	 TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_SDMMC_ID) | \
+	 TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_ETH_ID) | \
+	 TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_DAP_ID))
+
+#endif /* _DT_BINDINGS_STM32MP1_TZC400_H */
diff --git a/include/lib/optee_utils.h b/include/lib/optee_utils.h
index 6067caff42..ba44f998ec 100644
--- a/include/lib/optee_utils.h
+++ b/include/lib/optee_utils.h
@@ -9,6 +9,7 @@
 
 #include <common/bl_common.h>
 
+int get_optee_header_ep(entry_point_info_t *header_ep, uintptr_t *pc);
 int parse_optee_header(entry_point_info_t *header_ep,
 	image_info_t *pager_image_info,
 	image_info_t *paged_image_info);
diff --git a/include/lib/psci/psci.h b/include/lib/psci/psci.h
index b56e98b5f3..9f2a80ebeb 100644
--- a/include/lib/psci/psci.h
+++ b/include/lib/psci/psci.h
@@ -349,6 +349,7 @@ int psci_node_hw_state(u_register_t target_cpu,
 int psci_features(unsigned int psci_fid);
 void __dead2 psci_power_down_wfi(void);
 void psci_arch_setup(void);
+unsigned int psci_is_last_on_cpu(void);
 
 #endif /*__ASSEMBLER__*/
 
diff --git a/include/lib/usb/usb_core.h b/include/lib/usb/usb_core.h
new file mode 100644
index 0000000000..dbc311d8bb
--- /dev/null
+++ b/include/lib/usb/usb_core.h
@@ -0,0 +1,277 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef USB_CORE_H
+#define USB_CORE_H
+
+#include <stdint.h>
+
+#include <lib/utils_def.h>
+
+#define USBD_MAX_NUM_INTERFACES			1
+#define USBD_MAX_NUM_CONFIGURATION		1
+
+#define USB_LEN_DEV_QUALIFIER_DESC		0x0A
+#define USB_LEN_DEV_DESC			0x12
+#define USB_LEN_CFG_DESC			0x09
+#define USB_LEN_IF_DESC				0x09
+#define USB_LEN_EP_DESC				0x07
+#define USB_LEN_OTG_DESC			0x03
+#define USB_LEN_LANGID_STR_DESC			0x04
+#define USB_LEN_OTHER_SPEED_DESC_SIZ		0x09
+
+#define USBD_IDX_LANGID_STR			0x00
+#define USBD_IDX_MFC_STR			0x01
+#define USBD_IDX_PRODUCT_STR			0x02
+#define USBD_IDX_SERIAL_STR			0x03
+#define USBD_IDX_CONFIG_STR			0x04
+#define USBD_IDX_INTERFACE_STR			0x05
+#define USBD_IDX_USER0_STR			0x06
+
+#define USB_REQ_TYPE_STANDARD			0x00
+#define USB_REQ_TYPE_CLASS			0x20
+#define USB_REQ_TYPE_VENDOR			0x40
+#define USB_REQ_TYPE_MASK			0x60
+
+#define USB_REQ_RECIPIENT_DEVICE		0x00
+#define USB_REQ_RECIPIENT_INTERFACE		0x01
+#define USB_REQ_RECIPIENT_ENDPOINT		0x02
+#define USB_REQ_RECIPIENT_MASK			0x1F
+
+#define USB_REQ_DIRECTION			0x80
+
+#define USB_REQ_GET_STATUS			0x00
+#define USB_REQ_CLEAR_FEATURE			0x01
+#define USB_REQ_SET_FEATURE			0x03
+#define USB_REQ_SET_ADDRESS			0x05
+#define USB_REQ_GET_DESCRIPTOR			0x06
+#define USB_REQ_SET_DESCRIPTOR			0x07
+#define USB_REQ_GET_CONFIGURATION		0x08
+#define USB_REQ_SET_CONFIGURATION		0x09
+#define USB_REQ_GET_INTERFACE			0x0A
+#define USB_REQ_SET_INTERFACE			0x0B
+#define USB_REQ_SYNCH_FRAME			0x0C
+
+#define USB_DESC_TYPE_DEVICE			0x01
+#define USB_DESC_TYPE_CONFIGURATION		0x02
+#define USB_DESC_TYPE_STRING			0x03
+#define USB_DESC_TYPE_INTERFACE			0x04
+#define USB_DESC_TYPE_ENDPOINT			0x05
+#define USB_DESC_TYPE_DEVICE_QUALIFIER		0x06
+#define USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION	0x07
+#define USB_DESC_TYPE_BOS			0x0F
+
+#define USB_CONFIG_REMOTE_WAKEUP		2
+#define USB_CONFIG_SELF_POWERED			1
+
+#define USB_FEATURE_EP_HALT			0
+#define USB_FEATURE_REMOTE_WAKEUP		1
+#define USB_FEATURE_TEST_MODE			2
+
+#define USB_DEVICE_CAPABITY_TYPE		0x10
+
+#define USB_MAX_EP0_SIZE			64
+
+/*  Device Status */
+#define USBD_STATE_DEFAULT			1
+#define USBD_STATE_ADDRESSED			2
+#define USBD_STATE_CONFIGURED			3
+#define USBD_STATE_SUSPENDED			4
+
+/*  EP0 State */
+#define USBD_EP0_IDLE				0
+#define USBD_EP0_SETUP				1
+#define USBD_EP0_DATA_IN			2
+#define USBD_EP0_DATA_OUT			3
+#define USBD_EP0_STATUS_IN			4
+#define USBD_EP0_STATUS_OUT			5
+#define USBD_EP0_STALL				6
+
+#define USBD_EP_TYPE_CTRL			0
+#define USBD_EP_TYPE_ISOC			1
+#define USBD_EP_TYPE_BULK			2
+#define USBD_EP_TYPE_INTR			3
+
+#define USB_OTG_SPEED_HIGH			0
+#define USB_OTG_SPEED_HIGH_IN_FULL		1
+#define USB_OTG_SPEED_LOW			2
+#define USB_OTG_SPEED_FULL			3
+
+#define  USBD_OUT_EPNUM_MASK			GENMASK(15, 0)
+#define  USBD_OUT_COUNT_MASK			GENMASK(31, 16)
+#define  USBD_OUT_COUNT_SHIFT			16U
+
+#define LOBYTE(x)	((uint8_t)((x) & 0x00FF))
+#define HIBYTE(x)	((uint8_t)(((x) & 0xFF00) >> 8))
+
+typedef struct {
+	uint8_t bm_request;
+	uint8_t b_request;
+	uint16_t value;
+	uint16_t index;
+	uint16_t length;
+} usb_setup_req_t;
+
+struct usb_handle;
+
+typedef struct {
+	uint8_t (*init)(struct usb_handle *pdev, uint8_t cfgidx);
+	uint8_t (*de_init)(struct usb_handle *pdev, uint8_t cfgidx);
+	/* Control Endpoints*/
+	uint8_t (*setup)(struct usb_handle *pdev, usb_setup_req_t  *req);
+	uint8_t (*ep0_tx_sent)(struct usb_handle *pdev);
+	uint8_t (*ep0_rx_ready)(struct usb_handle *pdev);
+	/* Class Specific Endpoints*/
+	uint8_t (*data_in)(struct usb_handle *pdev, uint8_t epnum);
+	uint8_t (*data_out)(struct usb_handle *pdev, uint8_t epnum);
+	uint8_t (*sof)(struct usb_handle *pdev);
+	uint8_t (*iso_in_incomplete)(struct usb_handle *pdev, uint8_t epnum);
+	uint8_t (*iso_out_incomplete)(struct usb_handle *pdev, uint8_t epnum);
+} usb_class_t;
+
+/* Following USB Device status */
+typedef enum {
+	USBD_OK = 0,
+	USBD_BUSY,
+	USBD_FAIL,
+	USBD_TIMEOUT
+} usb_status_t;
+
+/* Action to do after IT handling */
+typedef enum {
+	USB_NOTHING = 0,
+	USB_DATA_OUT,
+	USB_DATA_IN,
+	USB_SETUP,
+	USB_ENUM_DONE,
+	USB_READ_DATA_PACKET,
+	USB_READ_SETUP_PACKET,
+	USB_RESET,
+	USB_RESUME,
+	USB_SUSPEND,
+	USB_LPM,
+	USB_SOF,
+	USB_DISCONNECT,
+	USB_WRITE_EMPTY
+} usb_action_t;
+
+/* USB Device descriptors structure */
+typedef struct {
+	uint8_t *(*get_device_desc)(uint16_t *length);
+	uint8_t *(*get_lang_id_desc)(uint16_t *length);
+	uint8_t *(*get_manufacturer_desc)(uint16_t *length);
+	uint8_t *(*get_product_desc)(uint16_t *length);
+	uint8_t *(*get_serial_desc)(uint16_t *length);
+	uint8_t *(*get_configuration_desc)(uint16_t *length);
+	uint8_t *(*get_interface_desc)(uint16_t *length);
+	uint8_t *(*get_usr_desc)(uint8_t index, uint16_t *length);
+	uint8_t *(*get_config_desc)(uint16_t *length);
+	uint8_t *(*get_device_qualifier_desc)(uint16_t *length);
+} usb_desc_t;
+
+/* USB Device handle structure */
+typedef struct {
+	uint32_t status;
+	uint32_t total_length;
+	uint32_t rem_length;
+	uint32_t maxpacket;
+} usb_endpoint_t;
+
+typedef struct {
+	uint8_t num; /* Endpoint number
+		      * This parameter must be a number between Min_Data = 1
+		      * and Max_Data = 15
+		      */
+	bool is_in; /* Endpoint direction */
+	uint8_t type; /* Endpoint type */
+	uint32_t maxpacket; /* Endpoint Max packet size
+			     * This parameter must be a number between
+			     * Min_Data = 0 and Max_Data = 64KB
+			     */
+	uint8_t *xfer_buff; /* Pointer to transfer buffer */
+	uint32_t xfer_len; /* Current transfer length */
+	uint32_t xfer_count; /* Partial transfer length in case of multi
+			      * packet transfer
+			      */
+} usbd_ep_t;
+
+typedef enum {
+	LPM_L0 = 0x00, /* on */
+	LPM_L1 = 0x01, /* LPM L1 sleep */
+	LPM_L2 = 0x02, /* suspend */
+	LPM_L3 = 0x03, /* off */
+} pcd_lpm_state_t;
+
+/* USB Device descriptors structure */
+typedef struct {
+	usb_status_t (*ep0_out_start)(void *handle);
+	usb_status_t (*ep_start_xfer)(void *handle, usbd_ep_t *ep);
+	usb_status_t (*ep0_start_xfer)(void *handle, usbd_ep_t *ep);
+	usb_status_t (*write_packet)(void *handle, uint8_t *src,
+				     uint8_t ch_ep_num, uint16_t len);
+	void *(*read_packet)(void *handle, uint8_t *dest, uint16_t len);
+	usb_status_t (*ep_set_stall)(void *handle, usbd_ep_t *ep);
+	usb_status_t (*start_device)(void *handle);
+	usb_status_t (*stop_device)(void *handle);
+	usb_status_t (*set_address)(void *handle, uint8_t address);
+	usb_status_t (*write_empty_tx_fifo)(void *handle,
+					    uint32_t epnum, uint32_t xfer_len,
+					    uint32_t *xfer_count,
+					    uint32_t maxpacket,
+					    uint8_t **xfer_buff);
+	usb_action_t (*it_handler)(void *handle, uint32_t *param);
+} usb_driver_t;
+
+/* USB Peripheral Controller Drivers */
+typedef struct {
+	void *instance; /* Register base address */
+	usbd_ep_t in_ep[15]; /* IN endpoint parameters */
+	usbd_ep_t out_ep[15]; /* OUT endpoint parameters */
+	uint32_t setup[12]; /* Setup packet buffer */
+	pcd_lpm_state_t lpm_state; /* LPM State */
+} pcd_handle_t;
+
+/* USB Device handle structure */
+typedef struct usb_handle {
+	uint8_t id;
+	uint32_t dev_config;
+	uint32_t dev_config_status;
+	usb_endpoint_t ep_in[15];
+	usb_endpoint_t ep_out[15];
+	uint32_t ep0_state;
+	uint32_t ep0_data_len;
+	uint8_t dev_state;
+	uint8_t dev_old_state;
+	uint8_t dev_address;
+	uint32_t dev_remote_wakeup;
+	usb_setup_req_t request;
+	const usb_desc_t *desc;
+	usb_class_t *class;
+	void *class_data;
+	void *user_data;
+	pcd_handle_t *data;
+	const usb_driver_t *driver;
+} usb_handle_t;
+
+usb_status_t usb_core_handle_it(usb_handle_t *pdev);
+usb_status_t usb_core_receive(usb_handle_t *pdev, uint8_t ep_addr,
+			      uint8_t *p_buf, uint32_t len);
+usb_status_t usb_core_transmit(usb_handle_t *pdev, uint8_t ep_addr,
+			       uint8_t *p_buf, uint32_t len);
+usb_status_t usb_core_receive_ep0(usb_handle_t *pdev, uint8_t *p_buf,
+				  uint32_t len);
+usb_status_t usb_core_transmit_ep0(usb_handle_t *pdev, uint8_t *p_buf,
+				   uint32_t len);
+void usb_core_ctl_error(usb_handle_t *pdev);
+usb_status_t usb_core_start(usb_handle_t *pdev);
+usb_status_t usb_core_stop(usb_handle_t *pdev);
+usb_status_t register_usb_driver(usb_handle_t *pdev, pcd_handle_t *pcd_handle,
+				 const usb_driver_t *driver,
+				 void *driver_handle);
+usb_status_t register_platform(usb_handle_t *pdev,
+			       const usb_desc_t *plat_call_back);
+
+#endif /* USB_CORE_H */
diff --git a/include/lib/usb/usb_st_dfu.h b/include/lib/usb/usb_st_dfu.h
new file mode 100644
index 0000000000..bc043c091f
--- /dev/null
+++ b/include/lib/usb/usb_st_dfu.h
@@ -0,0 +1,85 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef USB_ST_DFU_H
+#define USB_ST_DFU_H
+
+#include <stdint.h>
+
+#include <lib/usb/usb_core.h>
+
+#define DFU_DESCRIPTOR_TYPE		0x21
+
+/* Max DFU Packet Size = 1024 bytes */
+#define USBD_DFU_XFER_SIZE		1024
+
+#define TRANSFER_SIZE_BYTES(size) \
+	((uint8_t)((size) & 0xFF)), /* XFERSIZEB0 */\
+	((uint8_t)((size) >> 8))    /* XFERSIZEB1 */
+
+/* Descriptor of DFU interface 0 Alternate setting n */
+#define USBD_DFU_IF_DESC(n)	0x09, /* Interface Descriptor size */\
+				USB_DESC_TYPE_INTERFACE, /* descriptor type */\
+				0x00, /* Number of Interface */\
+				(n), /* Alternate setting */\
+				0x00, /* bNumEndpoints*/\
+				0xFE, /* Application Specific Class Code */\
+				0x01, /* Device Firmware Upgrade Code */\
+				0x02, /* DFU mode protocol */ \
+				USBD_IDX_USER0_STR + (n) /* iInterface:
+							  * Index of string
+							  * descriptor
+							  */
+
+/* DFU1.1 Standard */
+#define USB_DFU_VERSION			0x0110
+#define USB_DFU_ITF_SIZ			9
+#define USB_DFU_DESC_SIZ(itf)		(USB_DFU_ITF_SIZ * ((itf) + 2))
+
+/* bmAttribute :
+ * bitCanDnload = 1(bit 0)
+ * bitCanUpload = 1(bit 1)
+ * bitManifestationTolerant = 1 (bit 2)
+ * bitWillDetach = 1(bit 3)
+ * Reserved (bit4-6)
+ * bitAcceleratedST = 0(bit 7)
+ */
+#define DFU_BM_ATTRIBUTE		0x0F
+
+#define DFU_MEDIA_STATE_READY		0x00
+#define DFU_MEDIA_STATE_WRITTEN		0x01
+#define DFU_MEDIA_STATE_ERROR		0x02
+
+#define DFU_STATUS_SIZE			6U
+
+typedef void (*p_function)(void);
+
+/* Callback for media access */
+typedef struct {
+	int (*upload)(uint8_t alt, uintptr_t *buffer, uint32_t *len,
+		      void *user_data);
+	int (*download)(uint8_t alt, uintptr_t *buffer, uint32_t *len,
+			void *user_data);
+	int (*manifestation)(uint8_t alt, void *user_data);
+} usb_dfu_media_t;
+
+/* Internal DFU handle */
+typedef struct {
+	uint8_t status[DFU_STATUS_SIZE];
+	uint8_t dev_state;
+	uint8_t dev_status;
+	uint32_t alt_setting;
+	const usb_dfu_media_t *callback;
+} usb_dfu_handle_t;
+
+void usb_dfu_register(usb_handle_t *pdev, usb_dfu_handle_t *phandle);
+
+int usb_dfu_loop(usb_handle_t *pdev, const usb_dfu_media_t *pmedia);
+
+/* Function provided by plat */
+usb_handle_t *usb_dfu_plat_init(void);
+
+#endif /* USB_ST_DFU_H */
diff --git a/include/lib/utils_def.h b/include/lib/utils_def.h
index 2d0e9c08eb..bf93444505 100644
--- a/include/lib/utils_def.h
+++ b/include/lib/utils_def.h
@@ -64,6 +64,16 @@
 	((val) + _div - (__typeof__(div)) 1) / _div;		\
 })
 
+/*
+ * Macro for unsigned integer division with nearest rounding variant.
+ * Default integer division rounds down.
+ */
+#define udiv_round_nearest(x, y) __extension__ ({	\
+	__typeof__(x) _x = (x);				\
+	__typeof__(y) _y = (y);				\
+	(_x + (_y / 2)) / _y;				\
+})
+
 #define MIN(x, y) __extension__ ({	\
 	__typeof__(x) _x = (x);		\
 	__typeof__(y) _y = (y);		\
diff --git a/include/plat/common/platform.h b/include/plat/common/platform.h
index ebcc855774..ab05be1aed 100644
--- a/include/plat/common/platform.h
+++ b/include/plat/common/platform.h
@@ -109,6 +109,11 @@ unsigned int plat_ic_get_interrupt_id(unsigned int raw);
  ******************************************************************************/
 uintptr_t plat_get_my_stack(void);
 void plat_report_exception(unsigned int exception_type);
+#if AARCH32_EXCEPTION_DEBUG
+void plat_report_undef_inst(unsigned int fault_address);
+void plat_report_prefetch_abort(unsigned int fault_address);
+void plat_report_data_abort(unsigned int fault_address);
+#endif
 int plat_crash_console_init(void);
 int plat_crash_console_putc(int c);
 void plat_crash_console_flush(void);
@@ -116,7 +121,7 @@ void plat_error_handler(int err) __dead2;
 void plat_panic_handler(void) __dead2;
 const char *plat_log_get_prefix(unsigned int log_level);
 void bl2_plat_preload_setup(void);
-int plat_try_next_boot_source(void);
+int plat_try_next_boot_source(unsigned int image_id);
 
 /*******************************************************************************
  * Mandatory BL1 functions
@@ -284,6 +289,8 @@ int plat_get_nv_ctr(void *cookie, unsigned int *nv_ctr);
 int plat_set_nv_ctr(void *cookie, unsigned int nv_ctr);
 int plat_set_nv_ctr2(void *cookie, const struct auth_img_desc_s *img_desc,
 		unsigned int nv_ctr);
+int plat_get_hashed_pk(void *full_pk_ptr, unsigned int full_pk_len,
+		       void **hashed_pk_ptr, unsigned int *hash_pk_len);
 int get_mbedtls_heap_helper(void **heap_addr, size_t *heap_size);
 int plat_get_enc_key_info(enum fw_enc_status_t fw_enc_status, uint8_t *key,
 			  size_t *key_len, unsigned int *flags,
diff --git a/lib/aarch32/misc_helpers.S b/lib/aarch32/misc_helpers.S
index e9734ac2c6..aea975c0ad 100644
--- a/lib/aarch32/misc_helpers.S
+++ b/lib/aarch32/misc_helpers.S
@@ -7,6 +7,8 @@
 #include <arch.h>
 #include <asm_macros.S>
 #include <assert_macros.S>
+#include <common/bl_common.h>
+#include <lib/xlat_tables/xlat_tables_defs.h>
 
 	.globl	smc
 	.globl	zeromem
@@ -14,6 +16,9 @@
 	.globl	memcpy4
 	.globl	disable_mmu_icache_secure
 	.globl	disable_mmu_secure
+	.globl	fixup_gdt_reloc
+
+#define PAGE_START_MASK		~(PAGE_SIZE_MASK)
 
 func smc
 	/*
@@ -187,3 +192,124 @@ func disable_mmu_icache_secure
 	ldr	r1, =(SCTLR_M_BIT | SCTLR_C_BIT | SCTLR_I_BIT)
 	b	do_disable_mmu
 endfunc disable_mmu_icache_secure
+
+/* ---------------------------------------------------------------------------
+ * Helper to fixup Global Descriptor table (GDT) and dynamic relocations
+ * (.rel.dyn) at runtime.
+ *
+ * This function is meant to be used when the firmware is compiled with -fpie
+ * and linked with -pie options. We rely on the linker script exporting
+ * appropriate markers for start and end of the section. For GOT, we
+ * expect __GOT_START__ and __GOT_END__. Similarly for .rela.dyn, we expect
+ * __RELA_START__ and __RELA_END__.
+ *
+ * The function takes the limits of the memory to apply fixups to as
+ * arguments (which is usually the limits of the relocable BL image).
+ *   r0 -  the start of the fixup region
+ *   r1 -  the limit of the fixup region
+ * These addresses have to be 4KB page aligned.
+ * ---------------------------------------------------------------------------
+ */
+
+/* Relocation codes */
+#define R_ARM_RELATIVE 	23
+
+func fixup_gdt_reloc
+	mov	r6, r0
+	mov	r7, r1
+
+#if ENABLE_ASSERTIONS
+	/* Test if the limits are 4K aligned */
+	orr	r0, r0, r1
+	mov	r1, #(PAGE_SIZE_MASK)
+	tst	r0, r1
+	ASM_ASSERT(eq)
+#endif
+	/*
+	 * Calculate the offset based on return address in lr.
+	 * Assume that this function is called within a page at the start of
+	 * fixup region.
+	 */
+	ldr	r1, =PAGE_START_MASK
+	and	r2, lr, r1
+	subs	r0, r2, r6	/* Diff(S) = Current Address - Compiled Address */
+	beq	3f		/* Diff(S) = 0. No relocation needed */
+
+	ldr	r1, =__GOT_START__
+	add	r1, r1, r0
+	ldr	r2, =__GOT_END__
+	add	r2, r2, r0
+
+	/*
+	 * GOT is an array of 32_bit addresses which must be fixed up as
+	 * new_addr = old_addr + Diff(S).
+	 * The new_addr is the address currently the binary is executing from
+	 * and old_addr is the address at compile time.
+	 */
+1:	ldr	r3, [r1]
+
+	/* Skip adding offset if address is < lower limit */
+	cmp	r3, r6
+	blo	2f
+
+	/* Skip adding offset if address is > upper limit */
+	cmp	r3, r7
+	bhi	2f
+	add	r3, r3, r0
+	str	r3, [r1]
+
+2:	add	r1, r1, #4
+	cmp	r1, r2
+	blo	1b
+
+	/* Starting dynamic relocations. Use ldr to get RELA_START and END */
+3:	ldr	r1, =__RELA_START__
+	add	r1, r1, r0
+	ldr	r2, =__RELA_END__
+	add	r2, r2, r0
+
+	/*
+	 * According to ELF-32 specification, the RELA data structure is as
+	 * follows:
+	 *	typedef struct {
+	 *		Elf32_Addr r_offset;
+	 *		Elf32_Xword r_info;
+	 *	} Elf32_Rela;
+	 *
+	 * r_offset is address of reference
+	 * r_info is symbol index and type of relocation (in this case
+	 * code 23  which corresponds to R_ARM_RELATIVE).
+	 *
+	 * Size of Elf32_Rela structure is 8 bytes.
+	 */
+
+	/* Skip R_ARM_NONE entry with code 0 */
+1:	ldr	r3, [r1, #4]
+	ands	r3, r3, #0xff
+	beq	2f
+
+#if ENABLE_ASSERTIONS
+	/* Assert that the relocation type is R_ARM_RELATIVE */
+	cmp	r3, #R_ARM_RELATIVE
+	ASM_ASSERT(eq)
+#endif
+	ldr	r3, [r1]	/* r_offset */
+	add	r3, r0, r3	/* Diff(S) + r_offset */
+	ldr 	r4, [r3]
+
+	/* Skip adding offset if address is < lower limit */
+	cmp	r4, r6
+	blo	2f
+
+	/* Skip adding offset if address is >= upper limit */
+	cmp	r4, r7
+	bhs	2f
+
+	add 	r4, r0, r4
+	str	r4, [r3]
+
+2:	add	r1, r1, #8
+	cmp	r1, r2
+	blo	1b
+	bx	lr
+endfunc fixup_gdt_reloc
diff --git a/lib/optee/optee_utils.c b/lib/optee/optee_utils.c
index 0ad108242e..18dbfe319a 100644
--- a/lib/optee/optee_utils.c
+++ b/lib/optee/optee_utils.c
@@ -86,7 +86,7 @@ static int parse_optee_image(image_info_t *image_info,
 	 * -1 indicates loader decided address; take our pre-mapped area
 	 * for current image since arm-tf could not allocate memory dynamically
 	 */
-	if (init_load_addr == -1)
+	if (init_load_addr == (uintptr_t)-1)
 		init_load_addr = image_info->image_base;
 
 	/* Check that the default end address doesn't overflow */
@@ -129,6 +129,36 @@ static int parse_optee_image(image_info_t *image_info,
 	return 0;
 }
 
+/*******************************************************************************
+ * Parse the OPTEE header for an executable entry point address.
+ * Return 1 on success, 0 on failure.
+ ******************************************************************************/
+int get_optee_header_ep(entry_point_info_t *header_ep, uintptr_t *pc)
+{
+	optee_header_t *optee_header;
+	uint32_t num;
+
+	assert(pc && header_ep && header_ep->pc);
+	optee_header = (optee_header_t *)header_ep->pc;
+
+	if (!tee_validate_header(optee_header))
+		return 0;
+
+	for (num = 0U; num < optee_header->nb_images; num++) {
+		optee_image_t *optee_image =
+			&optee_header->optee_image_list[num];
+
+		if (optee_image->image_id != OPTEE_PAGER_IMAGE_ID)
+			continue;
+
+		*pc = ((uint64_t)optee_image->load_addr_hi << 32) |
+			optee_image->load_addr_lo;
+		return 1;
+	}
+
+	return 0;
+}
+
 /*******************************************************************************
  * Parse the OPTEE header
  * Return 0 on success or a negative error code otherwise.
@@ -139,7 +169,8 @@ int parse_optee_header(entry_point_info_t *header_ep,
 
 {
 	optee_header_t *header;
-	int num, ret;
+	uint32_t num;
+	int ret;
 
 	assert(header_ep);
 	header = (optee_header_t *)header_ep->pc;
@@ -182,7 +213,7 @@ int parse_optee_header(entry_point_info_t *header_ep,
 	}
 
 	/* Parse OPTEE image */
-	for (num = 0; num < header->nb_images; num++) {
+	for (num = 0U; num < header->nb_images; num++) {
 		if (header->optee_image_list[num].image_id ==
 				OPTEE_PAGER_IMAGE_ID) {
 			ret = parse_optee_image(pager_image_info,
diff --git a/lib/psci/psci_private.h b/lib/psci/psci_private.h
index e2dcfa8b1e..deb1d2d815 100644
--- a/lib/psci/psci_private.h
+++ b/lib/psci/psci_private.h
@@ -286,7 +286,6 @@ unsigned int psci_find_max_off_lvl(const psci_power_state_t *state_info);
 unsigned int psci_find_target_suspend_lvl(const psci_power_state_t *state_info);
 void psci_set_pwr_domains_to_run(unsigned int end_pwrlvl);
 void psci_print_power_domain_map(void);
-unsigned int psci_is_last_on_cpu(void);
 int psci_spd_migrate_info(u_register_t *mpidr);
 void psci_do_pwrdown_sequence(unsigned int power_level);
 
diff --git a/lib/usb/usb_core.c b/lib/usb/usb_core.c
new file mode 100644
index 0000000000..1f33095af4
--- /dev/null
+++ b/lib/usb/usb_core.c
@@ -0,0 +1,838 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <lib/usb/usb_core.h>
+
+/* define for field bEndpointAddress */
+#define EP_DIR_MASK		BIT(7)
+#define EP_DIR_IN		BIT(7)
+#define EP_NUM_MASK		GENMASK(3, 0)
+
+#define EP0_IN			(0U | EP_DIR_IN)
+#define EP0_OUT			0U
+
+/* USB address between 1 through 127 = 0x7F mask */
+#define ADDRESS_MASK		GENMASK(6, 0)
+
+/*
+ * @brief  Set a STALL condition over an endpoint
+ * @param  pdev: USB handle
+ * @param  ep_addr: endpoint address
+ * @retval HAL status
+ */
+static usb_status_t usb_core_set_stall(usb_handle_t *pdev, uint8_t ep_addr)
+{
+	usbd_ep_t *ep;
+	pcd_handle_t *hpcd = (pcd_handle_t *)pdev->data;
+	uint8_t num;
+
+	num = ep_addr & EP_NUM_MASK;
+	if ((EP_DIR_MASK & ep_addr) == EP_DIR_IN) {
+		ep = &hpcd->in_ep[num];
+		ep->is_in = true;
+	} else {
+		ep = &hpcd->out_ep[num];
+		ep->is_in = false;
+	}
+	ep->num = num;
+
+	pdev->driver->ep_set_stall(hpcd->instance, ep);
+	if (num == 0U) {
+		pdev->driver->ep0_out_start(hpcd->instance);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_get_desc
+ *         Handle Get Descriptor requests
+ * pdev : device instance
+ * req : usb request
+ * return : status
+ */
+static void usb_core_get_desc(usb_handle_t *pdev, usb_setup_req_t *req)
+{
+	uint16_t len;
+	uint8_t *pbuf;
+	uint8_t desc_type = HIBYTE(req->value);
+	uint8_t desc_idx = LOBYTE(req->value);
+
+	switch (desc_type) {
+	case USB_DESC_TYPE_DEVICE:
+		pbuf = pdev->desc->get_device_desc(&len);
+		break;
+
+	case USB_DESC_TYPE_CONFIGURATION:
+		pbuf = (uint8_t *)pdev->desc->get_config_desc(&len);
+		pbuf[1] = USB_DESC_TYPE_CONFIGURATION;
+		break;
+
+	case USB_DESC_TYPE_STRING:
+		switch (desc_idx) {
+		case USBD_IDX_LANGID_STR:
+			pbuf = pdev->desc->get_lang_id_desc(&len);
+			break;
+
+		case USBD_IDX_MFC_STR:
+			pbuf = pdev->desc->get_manufacturer_desc(&len);
+			break;
+
+		case USBD_IDX_PRODUCT_STR:
+			pbuf = pdev->desc->get_product_desc(&len);
+			break;
+
+		case USBD_IDX_SERIAL_STR:
+			pbuf = pdev->desc->get_serial_desc(&len);
+			break;
+
+		case USBD_IDX_CONFIG_STR:
+			pbuf = pdev->desc->get_configuration_desc(&len);
+			break;
+
+		case USBD_IDX_INTERFACE_STR:
+			pbuf = pdev->desc->get_interface_desc(&len);
+			break;
+
+		/* for all USER string */
+		case USBD_IDX_USER0_STR:
+		default:
+			pbuf = pdev->desc->get_usr_desc(desc_idx - USBD_IDX_USER0_STR, &len);
+			break;
+		}
+		break;
+
+	case USB_DESC_TYPE_DEVICE_QUALIFIER:
+		pbuf = (uint8_t *)pdev->desc->get_device_qualifier_desc(&len);
+		break;
+
+	case USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION:
+		pbuf = (uint8_t *)pdev->desc->get_config_desc(&len);
+		pbuf[1] = USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION;
+		break;
+
+	default:
+		ERROR("Unknown request %i\n", desc_type);
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	if ((len != 0U) && (req->length != 0U)) {
+		len = MIN(len, req->length);
+
+		/* Start the transfer */
+		usb_core_transmit_ep0(pdev, pbuf, len);
+	}
+}
+
+/*
+ * usb_core_set_config
+ *         Handle Set device configuration request
+ * pdev : device instance
+ * req : usb request
+ * return : status
+ */
+static void usb_core_set_config(usb_handle_t *pdev, usb_setup_req_t *req)
+{
+	static uint8_t cfgidx;
+
+	cfgidx = LOBYTE(req->value);
+
+	if (cfgidx > USBD_MAX_NUM_CONFIGURATION) {
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	switch (pdev->dev_state) {
+	case USBD_STATE_ADDRESSED:
+		if (cfgidx != 0U) {
+			pdev->dev_config = cfgidx;
+			pdev->dev_state = USBD_STATE_CONFIGURED;
+			if (!pdev->class) {
+				usb_core_ctl_error(pdev);
+				return;
+			}
+			/* Set configuration  and Start the Class */
+			if (pdev->class->init(pdev, cfgidx) != 0U) {
+				usb_core_ctl_error(pdev);
+				return;
+			}
+		}
+		break;
+
+	case USBD_STATE_CONFIGURED:
+		if (cfgidx == 0U) {
+			pdev->dev_state = USBD_STATE_ADDRESSED;
+			pdev->dev_config = cfgidx;
+			pdev->class->de_init(pdev, cfgidx);
+		} else if (cfgidx != pdev->dev_config) {
+			if (pdev->class != NULL) {
+				usb_core_ctl_error(pdev);
+				return;
+			}
+			/* Clear old configuration */
+			pdev->class->de_init(pdev, pdev->dev_config);
+			/* Set new configuration */
+			pdev->dev_config = cfgidx;
+			/* Set configuration and start the Class*/
+			if (pdev->class->init(pdev, cfgidx) != 0U) {
+				usb_core_ctl_error(pdev);
+				return;
+			}
+		}
+		break;
+
+	default:
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	/* Send status */
+	usb_core_transmit_ep0(pdev, NULL, 0U);
+}
+
+/*
+ * usb_core_get_status
+ *         Handle Get Status request
+ * pdev : device instance
+ * req : usb request
+ * return : status
+ */
+static void usb_core_get_status(usb_handle_t *pdev, usb_setup_req_t *req)
+{
+	if ((pdev->dev_state != USBD_STATE_ADDRESSED) &&
+	    (pdev->dev_state != USBD_STATE_CONFIGURED)) {
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	pdev->dev_config_status = USB_CONFIG_SELF_POWERED;
+
+	if (pdev->dev_remote_wakeup != 0U) {
+		pdev->dev_config_status |= USB_CONFIG_REMOTE_WAKEUP;
+	}
+
+	/* Start the transfer */
+	usb_core_transmit_ep0(pdev, (uint8_t *)&pdev->dev_config_status, 2U);
+}
+
+/*
+ * usb_core_set_address
+ *         Set device address
+ * pdev : device instance
+ * req : usb request
+ * return : status
+ */
+static void usb_core_set_address(usb_handle_t *pdev, usb_setup_req_t *req)
+{
+	uint8_t dev_addr;
+
+	if ((req->index != 0U) || (req->length != 0U)) {
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	dev_addr = req->value & ADDRESS_MASK;
+	if (pdev->dev_state != USBD_STATE_DEFAULT) {
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	pdev->dev_address = dev_addr;
+	pdev->driver->set_address(((pcd_handle_t *)(pdev->data))->instance, dev_addr);
+
+	/* Send status */
+	usb_core_transmit_ep0(pdev, NULL, 0U);
+
+	if (dev_addr != 0U) {
+		pdev->dev_state  = USBD_STATE_ADDRESSED;
+	} else {
+		pdev->dev_state  = USBD_STATE_DEFAULT;
+	}
+}
+
+/*
+ * usb_core_dev_req
+ *         Handle standard usb device requests
+ * pdev : device instance
+ * req : usb request
+ * return : status
+ */
+static usb_status_t usb_core_dev_req(usb_handle_t *pdev, usb_setup_req_t *req)
+{
+	VERBOSE("receive request %i\n", req->b_request);
+	switch (req->b_request) {
+	case USB_REQ_GET_DESCRIPTOR:
+		usb_core_get_desc(pdev, req);
+		break;
+
+	case USB_REQ_SET_CONFIGURATION:
+		usb_core_set_config(pdev, req);
+		break;
+
+	case USB_REQ_GET_STATUS:
+		usb_core_get_status(pdev, req);
+		break;
+
+	case USB_REQ_SET_ADDRESS:
+		usb_core_set_address(pdev, req);
+		break;
+
+	case USB_REQ_GET_CONFIGURATION:
+	case USB_REQ_SET_FEATURE:
+	case USB_REQ_CLEAR_FEATURE:
+	default:
+		ERROR("NOT SUPPORTED %i\n", req->b_request);
+		usb_core_ctl_error(pdev);
+		break;
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_itf_req
+ *         Handle standard usb interface requests
+ * pdev : device instance
+ * req : usb request
+ * return : status
+ */
+static usb_status_t usb_core_itf_req(usb_handle_t *pdev, usb_setup_req_t *req)
+{
+	if (pdev->dev_state != USBD_STATE_CONFIGURED) {
+		usb_core_ctl_error(pdev);
+		return USBD_OK;
+	}
+
+	if (LOBYTE(req->index) <= USBD_MAX_NUM_INTERFACES) {
+		pdev->class->setup(pdev, req);
+
+		if (req->length == 0U) {
+			usb_core_transmit_ep0(pdev, NULL, 0U);
+		}
+	} else {
+		usb_core_ctl_error(pdev);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  USBD_ParseSetupRequest
+ *         Copy buffer into setup structure
+ * @param  pdev: device instance
+ * @param  req: usb request
+ * @retval None
+ */
+static void usb_core_parse_req(usb_setup_req_t *req, uint8_t *pdata)
+{
+	req->bm_request = pdata[0];
+	req->b_request = pdata[1];
+	req->value = pdata[2] + (pdata[3] << 8);
+	req->index = pdata[4] + (pdata[5] << 8);
+	req->length = pdata[6] + (pdata[7] << 8);
+}
+
+/*
+ * usb_core_setup_stage
+ *         Handle the setup stage
+ * pdev: device instance
+ * return : status
+ */
+static usb_status_t usb_core_setup_stage(usb_handle_t *pdev, uint8_t *psetup)
+{
+	usb_core_parse_req(&pdev->request, psetup);
+
+	pdev->ep0_state = USBD_EP0_SETUP;
+	pdev->ep0_data_len = pdev->request.length;
+
+	switch (pdev->request.bm_request & USB_REQ_RECIPIENT_MASK) {
+	case USB_REQ_RECIPIENT_DEVICE:
+		usb_core_dev_req(pdev, &pdev->request);
+		break;
+
+	case USB_REQ_RECIPIENT_INTERFACE:
+		usb_core_itf_req(pdev, &pdev->request);
+		break;
+
+	case USB_REQ_RECIPIENT_ENDPOINT:
+	default:
+		ERROR("receive unsupported request %i",
+		      pdev->request.bm_request & USB_REQ_RECIPIENT_MASK);
+		usb_core_set_stall(pdev, pdev->request.bm_request & USB_REQ_DIRECTION);
+		return USBD_FAIL;
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_data_out
+ *         Handle data OUT stage
+ * pdev: device instance
+ * epnum: endpoint index
+ * return : status
+ */
+static usb_status_t usb_core_data_out(usb_handle_t *pdev, uint8_t epnum,
+				      uint8_t *pdata)
+{
+	usb_endpoint_t *pep;
+
+	if (epnum == 0U) {
+		pep = &pdev->ep_out[0];
+		if (pdev->ep0_state == USBD_EP0_DATA_OUT) {
+			if (pep->rem_length > pep->maxpacket) {
+				pep->rem_length -= pep->maxpacket;
+
+				usb_core_receive(pdev, 0U, pdata,
+						 MIN(pep->rem_length,
+						     pep->maxpacket));
+			} else {
+				if (pdev->class->ep0_rx_ready &&
+				    (pdev->dev_state == USBD_STATE_CONFIGURED))
+					pdev->class->ep0_rx_ready(pdev);
+
+				usb_core_transmit_ep0(pdev, NULL, 0U);
+			}
+		}
+	} else if (pdev->class->data_out != NULL &&
+		   (pdev->dev_state == USBD_STATE_CONFIGURED)) {
+		pdev->class->data_out(pdev, epnum);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_data_in
+ *         Handle data in stage
+ * pdev: device instance
+ * epnum: endpoint index
+ * return : status
+ */
+static usb_status_t usb_core_data_in(usb_handle_t *pdev, uint8_t epnum,
+				     uint8_t *pdata)
+{
+	if (epnum == 0U) {
+		usb_endpoint_t *pep = &pdev->ep_in[0];
+
+		if (pdev->ep0_state == USBD_EP0_DATA_IN) {
+			if (pep->rem_length > pep->maxpacket) {
+				pep->rem_length -= pep->maxpacket;
+
+				usb_core_transmit(pdev, 0U, pdata,
+						  pep->rem_length);
+
+				/* Prepare endpoint for premature
+				 * end of transfer
+				 */
+				usb_core_receive(pdev, 0U, NULL, 0U);
+			} else {
+				/* Last packet is MPS multiple,
+				 * so send ZLP packet
+				 */
+				if ((pep->total_length % pep->maxpacket == 0U) &&
+				    (pep->total_length >= pep->maxpacket) &&
+				    (pep->total_length < pdev->ep0_data_len)) {
+					usb_core_transmit(pdev, 0U, NULL, 0U);
+
+					pdev->ep0_data_len = 0U;
+
+					/* Prepare endpoint for premature
+					 * end of transfer
+					 */
+					usb_core_receive(pdev, 0U, NULL, 0U);
+				} else {
+					if (pdev->class->ep0_tx_sent != NULL &&
+					    (pdev->dev_state ==
+					     USBD_STATE_CONFIGURED))
+						pdev->class->ep0_tx_sent(pdev);
+
+					/* Start the transfer */
+					usb_core_receive_ep0(pdev, NULL, 0U);
+				}
+			}
+		}
+	} else if (pdev->class->data_in != NULL &&
+		  (pdev->dev_state == USBD_STATE_CONFIGURED)) {
+		pdev->class->data_in(pdev, epnum);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_suspend
+ *         Handle suspend event
+ * pdev : device instance
+ * return : status
+ */
+
+static usb_status_t usb_core_suspend(usb_handle_t  *pdev)
+{
+	INFO("USB Suspend mode\n");
+	pdev->dev_old_state =  pdev->dev_state;
+	pdev->dev_state  = USBD_STATE_SUSPENDED;
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_resume
+ *         Handle resume event
+ * pdev : device instance
+ * return : status
+ */
+
+static usb_status_t usb_core_resume(usb_handle_t *pdev)
+{
+	INFO("USB Resume\n");
+	pdev->dev_state = pdev->dev_old_state;
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_sof
+ *         Handle SOF event
+ * pdev : device instance
+ * return : status
+ */
+
+static usb_status_t usb_core_sof(usb_handle_t *pdev)
+{
+	if (pdev->dev_state == USBD_STATE_CONFIGURED) {
+		if (pdev->class->sof != NULL)
+			pdev->class->sof(pdev);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_disconnect
+ *         Handle device disconnection event
+ * pdev : device instance
+ * return : status
+ */
+static usb_status_t usb_core_disconnect(usb_handle_t *pdev)
+{
+	/* Free class resources */
+	pdev->dev_state = USBD_STATE_DEFAULT;
+	pdev->class->de_init(pdev, pdev->dev_config);
+
+	return USBD_OK;
+}
+
+usb_status_t usb_core_handle_it(usb_handle_t *pdev)
+{
+	uint32_t param = 0U;
+	uint32_t len = 0U;
+	usbd_ep_t *ep;
+
+	switch (pdev->driver->it_handler(pdev->data->instance, &param)) {
+	case USB_DATA_OUT:
+		usb_core_data_out(pdev, param,
+				  pdev->data->out_ep[param].xfer_buff);
+		break;
+
+	case USB_DATA_IN:
+		usb_core_data_in(pdev, param,
+				 pdev->data->in_ep[param].xfer_buff);
+		break;
+
+	case USB_SETUP:
+		usb_core_setup_stage(pdev, (uint8_t *)pdev->data->setup);
+		break;
+
+	case USB_ENUM_DONE:
+		break;
+
+	case USB_READ_DATA_PACKET:
+		ep = &pdev->data->out_ep[param &  USBD_OUT_EPNUM_MASK];
+		len = (param &  USBD_OUT_COUNT_MASK) >> USBD_OUT_COUNT_SHIFT;
+		pdev->driver->read_packet(pdev->data->instance,
+					  ep->xfer_buff, len);
+		ep->xfer_buff += len;
+		ep->xfer_count += len;
+		break;
+
+	case USB_READ_SETUP_PACKET:
+		ep = &pdev->data->out_ep[param &  USBD_OUT_EPNUM_MASK];
+		len = (param &  USBD_OUT_COUNT_MASK) >> 0x10;
+		pdev->driver->read_packet(pdev->data->instance,
+					  (uint8_t *)pdev->data->setup, 8);
+		ep->xfer_count += len;
+		break;
+
+	case USB_RESET:
+		pdev->dev_state = USBD_STATE_DEFAULT;
+		break;
+
+	case USB_RESUME:
+		if (pdev->data->lpm_state == LPM_L1) {
+			pdev->data->lpm_state = LPM_L0;
+		} else {
+			usb_core_resume(pdev);
+		}
+		break;
+
+	case USB_SUSPEND:
+		usb_core_suspend(pdev);
+		break;
+
+	case USB_LPM:
+		if (pdev->data->lpm_state == LPM_L0) {
+			pdev->data->lpm_state = LPM_L1;
+		} else {
+			usb_core_suspend(pdev);
+		}
+		break;
+
+	case USB_SOF:
+		usb_core_sof(pdev);
+		break;
+
+	case USB_DISCONNECT:
+		usb_core_disconnect(pdev);
+		break;
+
+	case USB_WRITE_EMPTY:
+		pdev->driver->write_empty_tx_fifo(pdev->data->instance, param,
+				     pdev->data->in_ep[param].xfer_len,
+				     (uint32_t *)&pdev->data->in_ep[param].xfer_count,
+				     pdev->data->in_ep[param].maxpacket,
+				     &pdev->data->in_ep[param].xfer_buff);
+		break;
+
+	case USB_NOTHING:
+	default:
+		break;
+	}
+
+	return USBD_OK;
+}
+
+/**
+ * @brief  Receive an amount of data
+ * @param  pdev: USB handle
+ * @param  ep_addr: endpoint address
+ * @param  pBuf: pointer to the reception buffer
+ * @param  len: amount of data to be received
+ * @retval status
+ */
+usb_status_t usb_core_receive(usb_handle_t *pdev, uint8_t ep_addr,
+			      uint8_t *buf, uint32_t len)
+{
+	usbd_ep_t *ep;
+	pcd_handle_t *hpcd = (pcd_handle_t *)pdev->data;
+	uint8_t num;
+
+	num = ep_addr & EP_NUM_MASK;
+	ep = &hpcd->out_ep[num];
+
+	/* Setup and start the Xfer */
+	ep->xfer_buff = buf;
+	ep->xfer_len = len;
+	ep->xfer_count = 0U;
+	ep->is_in = false;
+	ep->num = num;
+
+	if (num == 0U) {
+		pdev->driver->ep0_start_xfer(hpcd->instance, ep);
+	} else {
+		pdev->driver->ep_start_xfer(hpcd->instance, ep);
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  Send an amount of data
+ * @param  pdev: USB handle
+ * @param  ep_addr: endpoint address
+ * @param  pBuf: pointer to the transmission buffer
+ * @param  len: amount of data to be sent
+ * @retval status
+ */
+usb_status_t usb_core_transmit(usb_handle_t *pdev, uint8_t ep_addr,
+			       uint8_t *buf, uint32_t len)
+{
+	usbd_ep_t *ep;
+	pcd_handle_t *hpcd = (pcd_handle_t *)pdev->data;
+	uint8_t num;
+
+	num = ep_addr & EP_NUM_MASK;
+	ep = &hpcd->in_ep[num];
+
+	/* Setup and start the Xfer */
+	ep->xfer_buff = buf;
+	ep->xfer_len = len;
+	ep->xfer_count = 0U;
+	ep->is_in = true;
+	ep->num = num;
+
+	if (num == 0U) {
+		pdev->driver->ep0_start_xfer(hpcd->instance, ep);
+	} else {
+		pdev->driver->ep_start_xfer(hpcd->instance, ep);
+	}
+
+	return USBD_OK;
+}
+
+/**
+ * @brief  Receive an amount of data on ep0
+ * @param  pdev: USB handle
+ * @param  buf: pointer to the reception buffer
+ * @param  len: amount of data to be received
+ * @retval status
+ */
+usb_status_t usb_core_receive_ep0(usb_handle_t *pdev, uint8_t *buf,
+				  uint32_t len)
+{
+	/* Prepare the reception of the buffer over EP0 */
+	if (len != 0U) {
+		pdev->ep0_state = USBD_EP0_DATA_OUT;
+	} else {
+		pdev->ep0_state = USBD_EP0_STATUS_OUT;
+	}
+
+	pdev->ep_out[0].total_length = len;
+	pdev->ep_out[0].rem_length = len;
+
+	/* Start the transfer */
+	return usb_core_receive(pdev, 0U, buf, len);
+}
+
+/*
+ * @brief  Send an amount of data on ep0
+ * @param  pdev: USB handle
+ * @param  buf: pointer to the transmission buffer
+ * @param  len: amount of data to be sent
+ * @retval status
+ */
+usb_status_t usb_core_transmit_ep0(usb_handle_t *pdev, uint8_t *buf,
+				   uint32_t len)
+{
+	/* Set EP0 State */
+	if (len != 0U) {
+		pdev->ep0_state = USBD_EP0_DATA_IN;
+	} else {
+		pdev->ep0_state = USBD_EP0_STATUS_IN;
+	}
+
+	pdev->ep_in[0].total_length = len;
+	pdev->ep_in[0].rem_length = len;
+
+	/* Start the transfer */
+	return usb_core_transmit(pdev, 0U, buf, len);
+}
+
+/*
+ * @brief  usb_core_ctl_error
+ *         Handle USB low level error
+ * @param  pdev: device instance
+ * @param  req: usb request
+ * @retval None
+ */
+
+void usb_core_ctl_error(usb_handle_t *pdev)
+{
+	ERROR("%s : Send an ERROR\n", __func__);
+	usb_core_set_stall(pdev, EP0_IN);
+	usb_core_set_stall(pdev, EP0_OUT);
+}
+
+/*
+ * usb_core_start
+ *         Start the USB device core.
+ * pdev: Device Handle
+ * return : USBD Status
+ */
+usb_status_t usb_core_start(usb_handle_t *pdev)
+{
+	/* Start the low level driver */
+	pdev->driver->start_device(pdev->data->instance);
+
+	return USBD_OK;
+}
+
+/*
+ * usb_core_stop
+ *         Stop the USB device core.
+ * pdev: Device Handle
+ * return : USBD Status
+ */
+usb_status_t usb_core_stop(usb_handle_t *pdev)
+{
+	/* Free class resources */
+	pdev->class->de_init(pdev, pdev->dev_config);
+
+	/* Stop the low level driver */
+	pdev->driver->stop_device(pdev->data->instance);
+
+	return USBD_OK;
+}
+
+/*
+ * register_usb_driver
+ *         Stop the USB device core.
+ * pdev: Device Handle
+ * @param  hpcd: PCD handle
+ * @param  driver: USB driver
+ * @param  driver_handle: USB driver handle
+ * return : USBD Status
+ */
+usb_status_t register_usb_driver(usb_handle_t *pdev, pcd_handle_t *pcd_handle,
+				 const usb_driver_t *driver,
+				 void *driver_handle)
+{
+	uint8_t i;
+
+	assert(pdev != NULL);
+	assert(pcd_handle != NULL);
+	assert(driver != NULL);
+	assert(driver_handle != NULL);
+
+	/* Free class resources */
+	pdev->driver = driver;
+	pdev->data = pcd_handle;
+	pdev->data->instance = driver_handle;
+	pdev->dev_state = USBD_STATE_DEFAULT;
+	pdev->ep0_state = USBD_EP0_IDLE;
+
+	/* Copy endpoint information */
+	for (i = 0U; i < 15U; i++) {
+		pdev->ep_in[i].maxpacket = pdev->data->in_ep[i].maxpacket;
+		pdev->ep_out[i].maxpacket = pdev->data->out_ep[i].maxpacket;
+	}
+
+	return USBD_OK;
+}
+
+/*
+ * register_platform
+ *         Register the USB device core.
+ * pdev: Device Handle
+ * plat_call_back: callback
+ * return : USBD Status
+ */
+usb_status_t register_platform(usb_handle_t *pdev,
+			       const usb_desc_t *plat_call_back)
+{
+	assert(pdev != NULL);
+	assert(plat_call_back != NULL);
+
+	/* Save platform info in class resources */
+	pdev->desc = plat_call_back;
+
+	return USBD_OK;
+}
diff --git a/lib/usb/usb_st_dfu.c b/lib/usb/usb_st_dfu.c
new file mode 100644
index 0000000000..18f5415990
--- /dev/null
+++ b/lib/usb/usb_st_dfu.c
@@ -0,0 +1,537 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <common/debug.h>
+#include <lib/usb/usb_st_dfu.h>
+
+/* DFU Requests  DFU states */
+#define APP_STATE_IDLE			0
+#define APP_STATE_DETACH		1
+#define DFU_STATE_IDLE			2
+#define DFU_STATE_DNLOAD_SYNC		3
+#define DFU_STATE_DNLOAD_BUSY		4
+#define DFU_STATE_DNLOAD_IDLE		5
+#define DFU_STATE_MANIFEST_SYNC		6
+#define DFU_STATE_MANIFEST		7
+#define DFU_STATE_MANIFEST_WAIT_RESET	8
+#define DFU_STATE_UPLOAD_IDLE		9
+#define DFU_STATE_ERROR			10
+
+/* DFU errors */
+#define DFU_ERROR_NONE			0x00
+#define DFU_ERROR_TARGET		0x01
+#define DFU_ERROR_FILE			0x02
+#define DFU_ERROR_WRITE			0x03
+#define DFU_ERROR_ERASE			0x04
+#define DFU_ERROR_CHECK_ERASED		0x05
+#define DFU_ERROR_PROG			0x06
+#define DFU_ERROR_VERIFY		0x07
+#define DFU_ERROR_ADDRESS		0x08
+#define DFU_ERROR_NOTDONE		0x09
+#define DFU_ERROR_FIRMWARE		0x0A
+#define DFU_ERROR_VENDOR		0x0B
+#define DFU_ERROR_USB			0x0C
+#define DFU_ERROR_POR			0x0D
+#define DFU_ERROR_UNKNOWN		0x0E
+#define DFU_ERROR_STALLEDPKT		0x0F
+
+typedef enum {
+	DFU_DETACH = 0,
+	DFU_DNLOAD,
+	DFU_UPLOAD,
+	DFU_GETSTATUS,
+	DFU_CLRSTATUS,
+	DFU_GETSTATE,
+	DFU_ABORT
+} dfu_request_t;
+
+static bool usb_dfu_detach_req;
+
+/*
+ * @brief  usb_dfu_init
+ *         Initialize the DFU interface
+ * @param  pdev: device instance
+ * @param  cfgidx: Configuration index
+ * @retval status
+ */
+static uint8_t usb_dfu_init(usb_handle_t *pdev, uint8_t cfgidx)
+{
+	/* Nothing to do in this stage */
+	return USBD_OK;
+}
+
+/**
+ * @brief  usb_dfu_de_init
+ *         De-Initialize the DFU layer
+ * @param  pdev: device instance
+ * @param  cfgidx: Configuration index
+ * @retval status
+ */
+static uint8_t usb_dfu_de_init(usb_handle_t *pdev, uint8_t cfgidx)
+{
+	/* Nothing to do in this stage */
+	return USBD_OK;
+}
+
+/*
+ * @brief  usb_dfu_data_in
+ *         handle data IN Stage
+ * @param  pdev: device instance
+ * @param  epnum: endpoint index
+ * @retval status
+ */
+static uint8_t  usb_dfu_data_in(usb_handle_t *pdev, uint8_t epnum)
+{
+	(void)pdev;
+	(void)epnum;
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  usb_dfu_ep0_rx_ready
+ *         handle EP0 Rx Ready event
+ * @param  pdev: device instance
+ * @retval status
+ */
+static uint8_t usb_dfu_ep0_rx_ready(usb_handle_t *pdev)
+{
+	(void)pdev;
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  usb_dfu_ep0_tx_ready
+ *         handle EP0 TRx Ready event
+ * @param  pdev: device instance
+ * @retval status
+ */
+static uint8_t usb_dfu_ep0_tx_ready(usb_handle_t *pdev)
+{
+	return USBD_OK;
+}
+
+/*
+ * @brief  usb_dfu_sof
+ *         handle SOF event
+ * @param  pdev: device instance
+ * @retval status
+ */
+static uint8_t usb_dfu_sof(usb_handle_t *pdev)
+{
+	(void)pdev;
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  usb_dfu_iso_in_incomplete
+ *         handle data ISO IN Incomplete event
+ * @param  pdev: device instance
+ * @param  epnum: endpoint index
+ * @retval status
+ */
+static uint8_t usb_dfu_iso_in_incomplete(usb_handle_t *pdev, uint8_t epnum)
+{
+	(void)pdev;
+	(void)epnum;
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  usb_dfu_iso_out_incomplete
+ *         handle data ISO OUT Incomplete event
+ * @param  pdev: device instance
+ * @param  epnum: endpoint index
+ * @retval status
+ */
+static uint8_t usb_dfu_iso_out_incomplete(usb_handle_t *pdev, uint8_t epnum)
+{
+	(void)pdev;
+	(void)epnum;
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  usb_dfu_data_out
+ *         handle data OUT Stage
+ * @param  pdev: device instance
+ * @param  epnum: endpoint index
+ * @retval status
+ */
+static uint8_t usb_dfu_data_out(usb_handle_t *pdev, uint8_t epnum)
+{
+	(void)pdev;
+	(void)epnum;
+
+	return USBD_OK;
+}
+
+/*
+ * @brief  usb_dfu_detach
+ *         Handles the DFU DETACH request.
+ * @param  pdev: device instance
+ * @param  req: pointer to the request structure.
+ * @retval None.
+ */
+static void usb_dfu_detach(usb_handle_t *pdev, usb_setup_req_t *req)
+{
+	usb_dfu_handle_t *hdfu = (usb_dfu_handle_t *)pdev->class_data;
+
+	INFO("Receive DFU Detach\n");
+
+	if ((hdfu->dev_state == DFU_STATE_IDLE) ||
+	    (hdfu->dev_state == DFU_STATE_DNLOAD_SYNC) ||
+	    (hdfu->dev_state == DFU_STATE_DNLOAD_IDLE) ||
+	    (hdfu->dev_state == DFU_STATE_MANIFEST_SYNC) ||
+	    (hdfu->dev_state == DFU_STATE_UPLOAD_IDLE)) {
+		/* Update the state machine */
+		hdfu->dev_state = DFU_STATE_IDLE;
+		hdfu->dev_status = DFU_ERROR_NONE;
+	}
+
+	usb_dfu_detach_req = true;
+}
+
+/*
+ * @brief  usb_dfu_download
+ *         Handles the DFU DNLOAD request.
+ * @param  pdev: device instance
+ * @param  req: pointer to the request structure
+ * @retval None
+ */
+static void usb_dfu_download(usb_handle_t *pdev, usb_setup_req_t *req)
+{
+	usb_dfu_handle_t *hdfu = (usb_dfu_handle_t *)pdev->class_data;
+	uintptr_t data_ptr;
+	uint32_t length;
+	int ret;
+
+	/* Data setup request */
+	if (req->length > 0) {
+		/* Unsupported state */
+		if ((hdfu->dev_state != DFU_STATE_IDLE) &&
+		    (hdfu->dev_state != DFU_STATE_DNLOAD_IDLE)) {
+			/* Call the error management function (command will be nacked) */
+			usb_core_ctl_error(pdev);
+			return;
+		}
+
+		/* Get the data address */
+		length = req->length;
+		ret = hdfu->callback->download(hdfu->alt_setting, &data_ptr,
+					       &length, pdev->user_data);
+		if (ret == 0U) {
+			/* Update the state machine */
+			hdfu->dev_state = DFU_STATE_DNLOAD_SYNC;
+			/* Start the transfer */
+			usb_core_receive_ep0(pdev, (uint8_t *)data_ptr, length);
+		} else {
+			usb_core_ctl_error(pdev);
+		}
+	} else {
+		/* End of DNLOAD operation*/
+		if (hdfu->dev_state != DFU_STATE_DNLOAD_IDLE) {
+			/* Call the error management function (command will be nacked) */
+			usb_core_ctl_error(pdev);
+			return;
+		}
+		/* End of DNLOAD operation*/
+		hdfu->dev_state = DFU_STATE_MANIFEST_SYNC;
+		ret = hdfu->callback->manifestation(hdfu->alt_setting, pdev->user_data);
+		if (ret == 0U) {
+			hdfu->dev_state = DFU_STATE_MANIFEST_SYNC;
+		} else {
+			usb_core_ctl_error(pdev);
+		}
+	}
+}
+
+/*
+ * @brief  usb_dfu_upload
+ *         Handles the DFU UPLOAD request.
+ * @param  pdev: instance
+ * @param  req: pointer to the request structure
+ * @retval status
+ */
+static void usb_dfu_upload(usb_handle_t *pdev, usb_setup_req_t *req)
+{
+	usb_dfu_handle_t *hdfu = (usb_dfu_handle_t *)pdev->class_data;
+	uintptr_t data_ptr;
+	uint32_t length;
+	int ret;
+
+	/* Data setup request */
+	if (req->length == 0) {
+		/* No Data setup request */
+		hdfu->dev_state = DFU_STATE_IDLE;
+		return;
+	}
+
+	/* Unsupported state */
+	if ((hdfu->dev_state != DFU_STATE_IDLE) && (hdfu->dev_state != DFU_STATE_UPLOAD_IDLE)) {
+		ERROR("UPLOAD : Unsupported State\n");
+		/* Call the error management function (command will be nacked) */
+		usb_core_ctl_error(pdev);
+		return;
+	}
+
+	/* Update the data address */
+	length = req->length;
+	ret = hdfu->callback->upload(hdfu->alt_setting, &data_ptr, &length, pdev->user_data);
+	if (ret == 0U) {
+		/* Short frame */
+		hdfu->dev_state = (req->length > length) ? DFU_STATE_IDLE : DFU_STATE_UPLOAD_IDLE;
+
+		/* Start the transfer */
+		usb_core_transmit_ep0(pdev, (uint8_t *)data_ptr, length);
+	} else {
+		ERROR("UPLOAD : bad block %i on alt %i\n", req->value, req->index);
+		hdfu->dev_state = DFU_STATE_ERROR;
+		hdfu->dev_status = DFU_ERROR_STALLEDPKT;
+
+		/* Call the error management function (command will be nacked) */
+		usb_core_ctl_error(pdev);
+	}
+}
+
+/*
+ * @brief  usb_dfu_get_status
+ *         Handles the DFU GETSTATUS request.
+ * @param  pdev: instance
+ * @retval status
+ */
+static void usb_dfu_get_status(usb_handle_t *pdev)
+{
+	usb_dfu_handle_t *hdfu = (usb_dfu_handle_t *)pdev->class_data;
+
+	hdfu->status[0] = hdfu->dev_status;	/* bStatus */
+	hdfu->status[1] = 0;			/* bwPollTimeout[3]; */
+	hdfu->status[2] = 0;
+	hdfu->status[3] = 0;
+	hdfu->status[4] = hdfu->dev_state;	/* bState */
+	hdfu->status[5] = 0;			/* iString */
+
+	/* next step */
+	switch (hdfu->dev_state) {
+	case DFU_STATE_DNLOAD_SYNC:
+		hdfu->dev_state = DFU_STATE_DNLOAD_IDLE;
+		break;
+	case DFU_STATE_MANIFEST_SYNC:
+		/* We're MainfestationTolerant */
+		hdfu->status[4] = DFU_STATE_MANIFEST;
+		hdfu->status[1] = 1U; /* bwPollTimeout = 1ms */
+		hdfu->dev_state = DFU_STATE_IDLE;
+		break;
+
+	default:
+		break;
+	}
+
+	/* Start the transfer */
+	usb_core_transmit_ep0(pdev, (uint8_t *)&hdfu->status[0], sizeof(hdfu->status));
+}
+
+/*
+ * @brief  usb_dfu_clear_status
+ *         Handles the DFU CLRSTATUS request.
+ * @param  pdev: device instance
+ * @retval status
+ */
+static void usb_dfu_clear_status(usb_handle_t *pdev)
+{
+	usb_dfu_handle_t *hdfu = (usb_dfu_handle_t *)pdev->class_data;
+
+	if (hdfu->dev_state == DFU_STATE_ERROR) {
+		hdfu->dev_state = DFU_STATE_IDLE;
+		hdfu->dev_status = DFU_ERROR_NONE;
+	} else {
+		/* State Error */
+		hdfu->dev_state = DFU_STATE_ERROR;
+		hdfu->dev_status = DFU_ERROR_UNKNOWN;
+	}
+}
+
+/*
+ * @brief  usb_dfu_get_state
+ *         Handles the DFU GETSTATE request.
+ * @param  pdev: device instance
+ * @retval None
+ */
+static void usb_dfu_get_state(usb_handle_t *pdev)
+{
+	usb_dfu_handle_t *hdfu = (usb_dfu_handle_t *)pdev->class_data;
+
+	/* Return the current state of the DFU interface */
+	usb_core_transmit_ep0(pdev, &hdfu->dev_state, 1);
+}
+
+/*
+ * @brief  usb_dfu_abort
+ *         Handles the DFU ABORT request.
+ * @param  pdev: device instance
+ * @retval None
+ */
+static void usb_dfu_abort(usb_handle_t *pdev)
+{
+	usb_dfu_handle_t *hdfu = (usb_dfu_handle_t *)pdev->class_data;
+
+	if ((hdfu->dev_state == DFU_STATE_IDLE) ||
+	    (hdfu->dev_state == DFU_STATE_DNLOAD_SYNC) ||
+	    (hdfu->dev_state == DFU_STATE_DNLOAD_IDLE) ||
+	    (hdfu->dev_state == DFU_STATE_MANIFEST_SYNC) ||
+	    (hdfu->dev_state == DFU_STATE_UPLOAD_IDLE)) {
+		hdfu->dev_state = DFU_STATE_IDLE;
+		hdfu->dev_status = DFU_ERROR_NONE;
+	}
+}
+
+/*
+ * @brief  usb_dfu_setup
+ *         Handle the DFU specific requests
+ * @param  pdev: instance
+ * @param  req: usb requests
+ * @retval status
+ */
+static uint8_t usb_dfu_setup(usb_handle_t *pdev, usb_setup_req_t *req)
+{
+	uint8_t *pbuf = NULL;
+	uint16_t len = 0U;
+	uint8_t ret = USBD_OK;
+	usb_dfu_handle_t *hdfu = (usb_dfu_handle_t *)pdev->class_data;
+
+	switch (req->bm_request & USB_REQ_TYPE_MASK) {
+	case USB_REQ_TYPE_CLASS:
+		switch (req->b_request) {
+		case DFU_DNLOAD:
+			usb_dfu_download(pdev, req);
+			break;
+
+		case DFU_UPLOAD:
+			usb_dfu_upload(pdev, req);
+			break;
+
+		case DFU_GETSTATUS:
+			usb_dfu_get_status(pdev);
+			break;
+
+		case DFU_CLRSTATUS:
+			usb_dfu_clear_status(pdev);
+			break;
+
+		case DFU_GETSTATE:
+			usb_dfu_get_state(pdev);
+			break;
+
+		case DFU_ABORT:
+			usb_dfu_abort(pdev);
+			break;
+
+		case DFU_DETACH:
+			usb_dfu_detach(pdev, req);
+			break;
+
+		default:
+			ERROR("unkwon request %x on alternate %i\n",
+			      req->b_request, hdfu->alt_setting);
+			usb_core_ctl_error(pdev);
+			ret = USBD_FAIL;
+			break;
+		}
+		break;
+	case USB_REQ_TYPE_STANDARD:
+		switch (req->b_request) {
+		case USB_REQ_GET_DESCRIPTOR:
+			if (HIBYTE(req->value) == DFU_DESCRIPTOR_TYPE) {
+				pbuf = pdev->desc->get_config_desc(&len);
+				/* DFU descriptor at the end of the USB */
+				pbuf += len - 9U;
+				len = 9U;
+				len = MIN(len, req->length);
+			}
+
+			/* Start the transfer */
+			usb_core_transmit_ep0(pdev, pbuf, len);
+
+			break;
+
+		case USB_REQ_GET_INTERFACE:
+			/* Start the transfer */
+			usb_core_transmit_ep0(pdev, (uint8_t *)&hdfu->alt_setting, 1U);
+			break;
+
+		case USB_REQ_SET_INTERFACE:
+			hdfu->alt_setting = LOBYTE(req->value);
+			break;
+
+		default:
+			usb_core_ctl_error(pdev);
+			ret = USBD_FAIL;
+			break;
+		}
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+static const usb_class_t USBD_DFU_initvalue = {
+	.init = usb_dfu_init,
+	.de_init = usb_dfu_de_init,
+	.setup = usb_dfu_setup,
+	.ep0_tx_sent = usb_dfu_ep0_tx_ready,
+	.ep0_rx_ready = usb_dfu_ep0_rx_ready,
+	.data_in = usb_dfu_data_in,
+	.data_out = usb_dfu_data_out,
+	.sof = usb_dfu_sof,
+	.iso_in_incomplete = usb_dfu_iso_in_incomplete,
+	.iso_out_incomplete = usb_dfu_iso_out_incomplete,
+};
+
+void usb_dfu_register(usb_handle_t *pdev, usb_dfu_handle_t *phandle)
+{
+	pdev->class = (usb_class_t *)&USBD_DFU_initvalue;
+	pdev->class_data = phandle;
+
+	phandle->dev_state = DFU_STATE_IDLE;
+	phandle->dev_status = DFU_ERROR_NONE;
+}
+
+int usb_dfu_loop(usb_handle_t *pdev, const usb_dfu_media_t *pmedia)
+{
+	uint32_t it_count;
+	usb_status_t ret;
+	usb_dfu_handle_t *hdfu = (usb_dfu_handle_t *)pdev->class_data;
+
+	hdfu->callback = pmedia;
+	usb_dfu_detach_req = false;
+	/* Continue to handle USB core IT to assure complete data transmission */
+	it_count = 100U;
+
+	/* DFU infinite loop until DETACH_REQ */
+	while (it_count != 0U) {
+		ret = usb_core_handle_it(pdev);
+		if (ret != USBD_OK) {
+			return -EIO;
+		}
+
+		/* detach request received */
+		if (usb_dfu_detach_req) {
+			it_count--;
+		}
+	}
+
+	return 0;
+}
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index 578bd59876..4d1f63b4b4 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2016-2020, ARM Limited. All rights reserved.
+# Copyright (c) 2016-2021, ARM Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -126,6 +126,9 @@ ENC_KEY	:= 1234567890abcdef1234567890abcdef1234567890abcdef1234567890abcdef
 # Default dummy nonce for firmware encryption
 ENC_NONCE			:= 1234567890abcdef12345678
 
+# Flag to enable exception debug for AARCH32
+AARCH32_EXCEPTION_DEBUG		:= 0
+
 # Build flag to treat usage of deprecated platform and framework APIs as error.
 ERROR_DEPRECATED		:= 0
 
@@ -323,8 +326,5 @@ RAS_TRAP_LOWER_EL_ERR_ACCESS	:= 0
 # Build option to create cot descriptors using fconf
 COT_DESC_IN_DTB			:= 0
 
-# Build option to provide openssl directory path
-OPENSSL_DIR			:= /usr
-
 # Build option to use the SP804 timer instead of the generic one
 USE_SP804_TIMER			:= 0
diff --git a/plat/common/aarch32/platform_helpers.S b/plat/common/aarch32/platform_helpers.S
index 5b9cb59146..d2c20b4a7d 100644
--- a/plat/common/aarch32/platform_helpers.S
+++ b/plat/common/aarch32/platform_helpers.S
@@ -8,6 +8,11 @@
 #include <asm_macros.S>
 
 	.weak	plat_report_exception
+#if AARCH32_EXCEPTION_DEBUG
+	.weak	plat_report_undef_inst
+	.weak	plat_report_prefetch_abort
+	.weak	plat_report_data_abort
+#endif
 	.weak	plat_reset_handler
 	.weak	plat_disable_acp
 	.weak	bl1_plat_prepare_exit
@@ -23,6 +28,35 @@ func plat_report_exception
 	bx	lr
 endfunc plat_report_exception
 
+#if AARCH32_EXCEPTION_DEBUG
+	/* -----------------------------------------------------
+	 * Placeholder function which should be redefined by
+	 * each platform.
+	 * -----------------------------------------------------
+	 */
+func plat_report_undef_inst
+	bx	lr
+endfunc plat_report_undef_inst
+
+	/* -----------------------------------------------------
+	 * Placeholder function which should be redefined by
+	 * each platform.
+	 * -----------------------------------------------------
+	 */
+func plat_report_prefetch_abort
+	bx	lr
+endfunc plat_report_prefetch_abort
+
+	/* -----------------------------------------------------
+	 * Placeholder function which should be redefined by
+	 * each platform.
+	 * -----------------------------------------------------
+	 */
+func plat_report_data_abort
+	bx	lr
+endfunc plat_report_data_abort
+#endif
+
 	/* -----------------------------------------------------
 	 * Placeholder function which should be redefined by
 	 * each platform.
diff --git a/plat/common/plat_bl_common.c b/plat/common/plat_bl_common.c
index 89b77ba6ce..5bbd73bd94 100644
--- a/plat/common/plat_bl_common.c
+++ b/plat/common/plat_bl_common.c
@@ -69,7 +69,7 @@ int bl2_plat_handle_post_image_load(unsigned int image_id)
 	return 0;
 }
 
-int plat_try_next_boot_source(void)
+int plat_try_next_boot_source(unsigned int image_id)
 {
 	return 0;
 }
diff --git a/plat/st/common/bl2_io_storage.c b/plat/st/common/bl2_io_storage.c
index 3ec7d4048a..23794db963 100644
--- a/plat/st/common/bl2_io_storage.c
+++ b/plat/st/common/bl2_io_storage.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -13,7 +13,8 @@
 #include <common/debug.h>
 #include <drivers/io/io_block.h>
 #include <drivers/io/io_driver.h>
-#include <drivers/io/io_dummy.h>
+#include <drivers/io/io_fip.h>
+#include <drivers/io/io_memmap.h>
 #include <drivers/io/io_mtd.h>
 #include <drivers/io/io_storage.h>
 #include <drivers/mmc.h>
@@ -22,28 +23,29 @@
 #include <drivers/spi_nand.h>
 #include <drivers/spi_nor.h>
 #include <drivers/st/io_mmc.h>
-#include <drivers/st/io_stm32image.h>
 #include <drivers/st/stm32_fmc2_nand.h>
 #include <drivers/st/stm32_qspi.h>
 #include <drivers/st/stm32_sdmmc2.h>
+#include <lib/fconf/fconf.h>
 #include <lib/mmio.h>
 #include <lib/utils.h>
+#include <lib/usb/usb_core.h>
+#include <lib/usb/usb_st_dfu.h>
 #include <plat/common/platform.h>
+#include <tools_share/firmware_image_package.h>
+
+#include <stm32cubeprogrammer.h>
+
+#include <stm32mp_fconf_getter.h>
 
 /* IO devices */
-static const io_dev_connector_t *dummy_dev_con;
-static uintptr_t dummy_dev_handle;
-static uintptr_t dummy_dev_spec;
+uintptr_t fip_dev_handle;
+uintptr_t storage_dev_handle;
 
-static uintptr_t image_dev_handle;
-static uintptr_t storage_dev_handle;
+static const io_dev_connector_t *fip_dev_con;
+static uint32_t nand_bkp_offset;
 
 #if STM32MP_SDMMC || STM32MP_EMMC
-static io_block_spec_t gpt_block_spec = {
-	.offset = 0,
-	.length = 34 * MMC_BLOCK_SIZE, /* Size of GPT table */
-};
-
 static uint32_t block_buffer[MMC_BLOCK_SIZE] __aligned(MMC_BLOCK_SIZE);
 
 static const io_block_dev_spec_t mmc_block_dev_spec = {
@@ -76,6 +78,7 @@ static io_mtd_dev_spec_t nand_dev_spec = {
 	.ops = {
 		.init = nand_raw_init,
 		.read = nand_read,
+		.seek = nand_seek_bb
 	},
 };
 
@@ -87,6 +90,7 @@ static io_mtd_dev_spec_t spi_nand_dev_spec = {
 	.ops = {
 		.init = spi_nand_init,
 		.read = nand_read,
+		.seek = nand_seek_bb
 	},
 };
 #endif
@@ -95,146 +99,21 @@ static io_mtd_dev_spec_t spi_nand_dev_spec = {
 static const io_dev_connector_t *spi_dev_con;
 #endif
 
-#ifdef AARCH32_SP_OPTEE
-static const struct stm32image_part_info optee_header_partition_spec = {
-	.name = OPTEE_HEADER_IMAGE_NAME,
-	.binary_type = OPTEE_HEADER_BINARY_TYPE,
-};
-
-static const struct stm32image_part_info optee_pager_partition_spec = {
-	.name = OPTEE_PAGER_IMAGE_NAME,
-	.binary_type = OPTEE_PAGER_BINARY_TYPE,
-};
-
-static const struct stm32image_part_info optee_paged_partition_spec = {
-	.name = OPTEE_PAGED_IMAGE_NAME,
-	.binary_type = OPTEE_PAGED_BINARY_TYPE,
-};
-#else
-static const io_block_spec_t bl32_block_spec = {
-	.offset = BL32_BASE,
-	.length = STM32MP_BL32_SIZE
-};
-#endif
-
-static const io_block_spec_t bl2_block_spec = {
-	.offset = BL2_BASE,
-	.length = STM32MP_BL2_SIZE,
-};
-
-static const struct stm32image_part_info bl33_partition_spec = {
-	.name = BL33_IMAGE_NAME,
-	.binary_type = BL33_BINARY_TYPE,
-};
-
-enum {
-	IMG_IDX_BL33,
-#ifdef AARCH32_SP_OPTEE
-	IMG_IDX_OPTEE_HEADER,
-	IMG_IDX_OPTEE_PAGER,
-	IMG_IDX_OPTEE_PAGED,
+#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
+static const io_dev_connector_t *memmap_dev_con;
 #endif
-	IMG_IDX_NUM
-};
 
-static struct stm32image_device_info stm32image_dev_info_spec __unused = {
-	.lba_size = MMC_BLOCK_SIZE,
-	.part_info[IMG_IDX_BL33] = {
-		.name = BL33_IMAGE_NAME,
-		.binary_type = BL33_BINARY_TYPE,
-	},
-#ifdef AARCH32_SP_OPTEE
-	.part_info[IMG_IDX_OPTEE_HEADER] = {
-		.name = OPTEE_HEADER_IMAGE_NAME,
-		.binary_type = OPTEE_HEADER_BINARY_TYPE,
-	},
-	.part_info[IMG_IDX_OPTEE_PAGER] = {
-		.name = OPTEE_PAGER_IMAGE_NAME,
-		.binary_type = OPTEE_PAGER_BINARY_TYPE,
-	},
-	.part_info[IMG_IDX_OPTEE_PAGED] = {
-		.name = OPTEE_PAGED_IMAGE_NAME,
-		.binary_type = OPTEE_PAGED_BINARY_TYPE,
-	},
-#endif
-};
-
-static io_block_spec_t stm32image_block_spec = {
+io_block_spec_t image_block_spec = {
 	.offset = 0,
 	.length = 0,
 };
 
-static const io_dev_connector_t *stm32image_dev_con __unused;
-
-static int open_dummy(const uintptr_t spec);
-static int open_image(const uintptr_t spec);
-static int open_storage(const uintptr_t spec);
-
-struct plat_io_policy {
-	uintptr_t *dev_handle;
-	uintptr_t image_spec;
-	int (*check)(const uintptr_t spec);
-};
-
-static const struct plat_io_policy policies[] = {
-	[BL2_IMAGE_ID] = {
-		.dev_handle = &dummy_dev_handle,
-		.image_spec = (uintptr_t)&bl2_block_spec,
-		.check = open_dummy
-	},
-#ifdef AARCH32_SP_OPTEE
-	[BL32_IMAGE_ID] = {
-		.dev_handle = &image_dev_handle,
-		.image_spec = (uintptr_t)&optee_header_partition_spec,
-		.check = open_image
-	},
-	[BL32_EXTRA1_IMAGE_ID] = {
-		.dev_handle = &image_dev_handle,
-		.image_spec = (uintptr_t)&optee_pager_partition_spec,
-		.check = open_image
-	},
-	[BL32_EXTRA2_IMAGE_ID] = {
-		.dev_handle = &image_dev_handle,
-		.image_spec = (uintptr_t)&optee_paged_partition_spec,
-		.check = open_image
-	},
-#else
-	[BL32_IMAGE_ID] = {
-		.dev_handle = &dummy_dev_handle,
-		.image_spec = (uintptr_t)&bl32_block_spec,
-		.check = open_dummy
-	},
-#endif
-	[BL33_IMAGE_ID] = {
-		.dev_handle = &image_dev_handle,
-		.image_spec = (uintptr_t)&bl33_partition_spec,
-		.check = open_image
-	},
-#if STM32MP_SDMMC || STM32MP_EMMC
-	[GPT_IMAGE_ID] = {
-		.dev_handle = &storage_dev_handle,
-		.image_spec = (uintptr_t)&gpt_block_spec,
-		.check = open_storage
-	},
-#endif
-	[STM32_IMAGE_ID] = {
-		.dev_handle = &storage_dev_handle,
-		.image_spec = (uintptr_t)&stm32image_block_spec,
-		.check = open_storage
-	}
-};
-
-static int open_dummy(const uintptr_t spec)
-{
-	return io_dev_init(dummy_dev_handle, 0);
-}
-
-static int open_image(const uintptr_t spec)
+int open_fip(const uintptr_t spec)
 {
-	return io_dev_init(image_dev_handle, 0);
+	return io_dev_init(fip_dev_handle, (uintptr_t)FIP_IMAGE_ID);
 }
 
-static int open_storage(const uintptr_t spec)
+int open_storage(const uintptr_t spec)
 {
 	return io_dev_init(storage_dev_handle, 0);
 }
@@ -248,17 +127,24 @@ static void print_boot_device(boot_api_context_t *boot_context)
 	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_EMMC:
 		INFO("Using EMMC\n");
 		break;
-	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NOR_QSPI:
-		INFO("Using QSPI NOR\n");
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NOR_SPI:
+		INFO("Using SPI NOR\n");
 		break;
 	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_FMC:
 		INFO("Using FMC NAND\n");
 		break;
-	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_QSPI:
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_SPI:
 		INFO("Using SPI NAND\n");
 		break;
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART:
+		INFO("Using UART\n");
+		break;
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB:
+		INFO("Using USB\n");
+		break;
 	default:
-		ERROR("Boot interface not found\n");
+		ERROR("Boot interface %u not found\n",
+		      boot_context->boot_interface_selected);
 		panic();
 		break;
 	}
@@ -273,11 +159,8 @@ static void boot_mmc(enum mmc_device_type mmc_dev_type,
 		     uint16_t boot_interface_instance)
 {
 	int io_result __unused;
-	uint8_t idx;
-	struct stm32image_part_info *part;
 	struct stm32_sdmmc2_params params;
 	struct mmc_device_info device_info;
-	const partition_entry_t *entry;
 
 	zeromem(&device_info, sizeof(struct mmc_device_info));
 	zeromem(&params, sizeof(struct stm32_sdmmc2_params));
@@ -304,6 +187,10 @@ static void boot_mmc(enum mmc_device_type mmc_dev_type,
 		break;
 	}
 
+	if (mmc_dev_type == MMC_IS_SD) {
+		params.flags = MMC_FLAG_SD_CMD6;
+	}
+
 	params.device_info = &device_info;
 	if (stm32_sdmmc2_mmc_init(&params) != 0) {
 		ERROR("SDMMC%u init failed\n", boot_interface_instance);
@@ -319,44 +206,6 @@ static void boot_mmc(enum mmc_device_type mmc_dev_type,
 	io_result = io_dev_open(mmc_dev_con, (uintptr_t)&mmc_block_dev_spec,
 				&storage_dev_handle);
 	assert(io_result == 0);
-
-	partition_init(GPT_IMAGE_ID);
-
-	io_result = io_dev_close(storage_dev_handle);
-	assert(io_result == 0);
-
-	stm32image_dev_info_spec.device_size =
-		stm32_sdmmc2_mmc_get_device_size();
-
-	for (idx = 0U; idx < IMG_IDX_NUM; idx++) {
-		part = &stm32image_dev_info_spec.part_info[idx];
-		entry = get_partition_entry(part->name);
-		if (entry == NULL) {
-			ERROR("Partition %s not found\n", part->name);
-			panic();
-		}
-
-		part->part_offset = entry->start;
-		part->bkp_offset = 0U;
-	}
-
-	/*
-	 * Re-open MMC with io_mmc, for better perfs compared to
-	 * io_block.
-	 */
-	io_result = register_io_dev_mmc(&mmc_dev_con);
-	assert(io_result == 0);
-
-	io_result = io_dev_open(mmc_dev_con, 0, &storage_dev_handle);
-	assert(io_result == 0);
-
-	io_result = register_io_dev_stm32image(&stm32image_dev_con);
-	assert(io_result == 0);
-
-	io_result = io_dev_open(stm32image_dev_con,
-				(uintptr_t)&stm32image_dev_info_spec,
-				&image_dev_handle);
-	assert(io_result == 0);
 }
 #endif /* STM32MP_SDMMC || STM32MP_EMMC */
 
@@ -364,8 +213,6 @@ static void boot_mmc(enum mmc_device_type mmc_dev_type,
 static void boot_spi_nor(boot_api_context_t *boot_context)
 {
 	int io_result __unused;
-	uint8_t idx;
-	struct stm32image_part_info *part;
 
 	io_result = stm32_qspi_init();
 	assert(io_result == 0);
@@ -378,38 +225,6 @@ static void boot_spi_nor(boot_api_context_t *boot_context)
 				(uintptr_t)&spi_nor_dev_spec,
 				&storage_dev_handle);
 	assert(io_result == 0);
-
-	stm32image_dev_info_spec.device_size = spi_nor_dev_spec.device_size;
-
-	idx = IMG_IDX_BL33;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NOR_BL33_OFFSET;
-	part->bkp_offset = 0U;
-
-#ifdef AARCH32_SP_OPTEE
-	idx = IMG_IDX_OPTEE_HEADER;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NOR_TEEH_OFFSET;
-	part->bkp_offset = 0U;
-
-	idx = IMG_IDX_OPTEE_PAGED;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NOR_TEED_OFFSET;
-	part->bkp_offset = 0U;
-
-	idx = IMG_IDX_OPTEE_PAGER;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NOR_TEEX_OFFSET;
-	part->bkp_offset = 0U;
-#endif
-
-	io_result = register_io_dev_stm32image(&stm32image_dev_con);
-	assert(io_result == 0);
-
-	io_result = io_dev_open(stm32image_dev_con,
-				(uintptr_t)&stm32image_dev_info_spec,
-				&image_dev_handle);
-	assert(io_result == 0);
 }
 #endif /* STM32MP_SPI_NOR */
 
@@ -417,8 +232,6 @@ static void boot_spi_nor(boot_api_context_t *boot_context)
 static void boot_fmc2_nand(boot_api_context_t *boot_context)
 {
 	int io_result __unused;
-	uint8_t idx;
-	struct stm32image_part_info *part;
 
 	io_result = stm32_fmc2_init();
 	assert(io_result == 0);
@@ -432,37 +245,7 @@ static void boot_fmc2_nand(boot_api_context_t *boot_context)
 				&storage_dev_handle);
 	assert(io_result == 0);
 
-	stm32image_dev_info_spec.device_size = nand_dev_spec.device_size;
-
-	idx = IMG_IDX_BL33;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NAND_BL33_OFFSET;
-	part->bkp_offset = nand_dev_spec.erase_size;
-
-#ifdef AARCH32_SP_OPTEE
-	idx = IMG_IDX_OPTEE_HEADER;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NAND_TEEH_OFFSET;
-	part->bkp_offset = nand_dev_spec.erase_size;
-
-	idx = IMG_IDX_OPTEE_PAGED;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NAND_TEED_OFFSET;
-	part->bkp_offset = nand_dev_spec.erase_size;
-
-	idx = IMG_IDX_OPTEE_PAGER;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NAND_TEEX_OFFSET;
-	part->bkp_offset = nand_dev_spec.erase_size;
-#endif
-
-	io_result = register_io_dev_stm32image(&stm32image_dev_con);
-	assert(io_result == 0);
-
-	io_result = io_dev_open(stm32image_dev_con,
-				(uintptr_t)&stm32image_dev_info_spec,
-				&image_dev_handle);
-	assert(io_result == 0);
+	nand_bkp_offset = nand_dev_spec.erase_size;
 }
 #endif /* STM32MP_RAW_NAND */
 
@@ -470,8 +253,6 @@ static void boot_fmc2_nand(boot_api_context_t *boot_context)
 static void boot_spi_nand(boot_api_context_t *boot_context)
 {
 	int io_result __unused;
-	uint8_t idx;
-	struct stm32image_part_info *part;
 
 	io_result = stm32_qspi_init();
 	assert(io_result == 0);
@@ -485,40 +266,57 @@ static void boot_spi_nand(boot_api_context_t *boot_context)
 				&storage_dev_handle);
 	assert(io_result == 0);
 
-	stm32image_dev_info_spec.device_size =
-		spi_nand_dev_spec.device_size;
-
-	idx = IMG_IDX_BL33;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NAND_BL33_OFFSET;
-	part->bkp_offset = spi_nand_dev_spec.erase_size;
-
-#ifdef AARCH32_SP_OPTEE
-	idx = IMG_IDX_OPTEE_HEADER;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NAND_TEEH_OFFSET;
-	part->bkp_offset = spi_nand_dev_spec.erase_size;
-
-	idx = IMG_IDX_OPTEE_PAGED;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NAND_TEED_OFFSET;
-	part->bkp_offset = spi_nand_dev_spec.erase_size;
-
-	idx = IMG_IDX_OPTEE_PAGER;
-	part = &stm32image_dev_info_spec.part_info[idx];
-	part->part_offset = STM32MP_NAND_TEEX_OFFSET;
-	part->bkp_offset = spi_nand_dev_spec.erase_size;
-#endif
+	nand_bkp_offset = spi_nand_dev_spec.erase_size;
+}
+#endif /* STM32MP_SPI_NAND */
+
+#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
+static void mmap_io_setup(void)
+{
+	int io_result __unused;
 
-	io_result = register_io_dev_stm32image(&stm32image_dev_con);
+	io_result = register_io_dev_memmap(&memmap_dev_con);
 	assert(io_result == 0);
 
-	io_result = io_dev_open(stm32image_dev_con,
-				(uintptr_t)&stm32image_dev_info_spec,
-				&image_dev_handle);
+	io_result = io_dev_open(memmap_dev_con, (uintptr_t)NULL,
+				&storage_dev_handle);
 	assert(io_result == 0);
 }
-#endif /* STM32MP_SPI_NAND */
+#endif
+
+#if STM32MP_UART_PROGRAMMER
+static void stm32cubeprogrammer_uart(unsigned int image_id)
+{
+	int ret __unused;
+	boot_api_context_t *boot_context =
+		(boot_api_context_t *)stm32mp_get_boot_ctx_address();
+	uintptr_t uart_base;
+
+	uart_base = get_uart_address(boot_context->boot_interface_instance);
+	ret = stm32cubeprog_uart_load(image_id, uart_base, FLASHLAYOUT_BASE, FLASHLAYOUT_SIZE,
+				      DWL_BUFFER_BASE, DWL_BUFFER_SIZE);
+	assert(ret == 0);
+
+	flush_dcache_range(FLASHLAYOUT_BASE, FLASHLAYOUT_SIZE);
+}
+#endif
+
+#if STM32MP_USB_PROGRAMMER
+static void stm32cubeprogrammer_usb(unsigned int image_id)
+{
+	usb_handle_t *pdev;
+	int ret __unused;
+
+	/* init USB on platform */
+	pdev = usb_dfu_plat_init();
+
+	ret = stm32cubeprog_usb_load(image_id, pdev, FLASHLAYOUT_BASE, FLASHLAYOUT_SIZE,
+				     DWL_BUFFER_BASE, DWL_BUFFER_SIZE);
+	assert(ret == 0);
+
+	flush_dcache_range(FLASHLAYOUT_BASE, FLASHLAYOUT_SIZE);
+}
+#endif
 
 void stm32mp_io_setup(void)
 {
@@ -534,12 +332,11 @@ void stm32mp_io_setup(void)
 		     boot_context->boot_partition_used_toboot);
 	}
 
-	io_result = register_io_dev_dummy(&dummy_dev_con);
+	io_result = register_io_dev_fip(&fip_dev_con);
 	assert(io_result == 0);
 
-	io_result = io_dev_open(dummy_dev_con, dummy_dev_spec,
-				&dummy_dev_handle);
-	assert(io_result == 0);
+	io_result = io_dev_open(fip_dev_con, (uintptr_t)NULL,
+				&fip_dev_handle);
 
 	switch (boot_context->boot_interface_selected) {
 #if STM32MP_SDMMC
@@ -555,7 +352,7 @@ void stm32mp_io_setup(void)
 		break;
 #endif
 #if STM32MP_SPI_NOR
-	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NOR_QSPI:
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NOR_SPI:
 		dmbsy();
 		boot_spi_nor(boot_context);
 		break;
@@ -567,19 +364,106 @@ void stm32mp_io_setup(void)
 		break;
 #endif
 #if STM32MP_SPI_NAND
-	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_QSPI:
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_SPI:
 		dmbsy();
 		boot_spi_nand(boot_context);
 		break;
 #endif
-
+#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
+#if STM32MP_UART_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART:
+#endif
+#if STM32MP_USB_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB:
+#endif
+		dmbsy();
+		mmap_io_setup();
+		break;
+#endif
 	default:
 		ERROR("Boot interface %d not supported\n",
 		      boot_context->boot_interface_selected);
+		panic();
 		break;
 	}
 }
 
+int bl2_plat_handle_pre_image_load(unsigned int image_id)
+{
+	static bool gpt_init_done __unused;
+	uint16_t boot_itf = stm32mp_get_boot_itf_selected();
+
+	switch (boot_itf) {
+#if STM32MP_SDMMC || STM32MP_EMMC
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_SD:
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_EMMC:
+		if (!gpt_init_done) {
+			const partition_entry_t *entry;
+
+			partition_init(GPT_IMAGE_ID);
+			entry = get_partition_entry(FIP_IMAGE_NAME);
+			if (entry == NULL) {
+				ERROR("Could NOT find the %s partition!\n",
+				      FIP_IMAGE_NAME);
+				return -ENOENT;
+			}
+
+			image_block_spec.offset = entry->start;
+			image_block_spec.length = entry->length;
+
+			gpt_init_done = true;
+		}
+
+		break;
+#endif
+
+#if STM32MP_RAW_NAND || STM32MP_SPI_NAND
+#if STM32MP_RAW_NAND
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_FMC:
+#endif
+#if STM32MP_SPI_NAND
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_SPI:
+#endif
+		image_block_spec.offset = STM32MP_NAND_FIP_OFFSET;
+		break;
+#endif
+
+#if STM32MP_SPI_NOR
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NOR_SPI:
+		image_block_spec.offset = STM32MP_NOR_FIP_OFFSET;
+		break;
+#endif
+
+#if STM32MP_UART_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART:
+		if (image_id == FW_CONFIG_ID) {
+			stm32cubeprogrammer_uart(FIP_IMAGE_ID);
+			/* BL33 at SSBL load address */
+			image_block_spec.offset = DWL_BUFFER_BASE;
+			image_block_spec.length = DWL_BUFFER_SIZE;
+		}
+		break;
+#endif
+
+#if STM32MP_USB_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB:
+		if (image_id == FW_CONFIG_ID) {
+			stm32cubeprogrammer_usb(FIP_IMAGE_ID);
+			/* BL33 at SSBL load address */
+			image_block_spec.offset = DWL_BUFFER_BASE;
+			image_block_spec.length = DWL_BUFFER_SIZE;
+		}
+		break;
+#endif
+
+	default:
+		ERROR("FIP Not found\n");
+		panic();
+	}
+
+	return 0;
+}
+
 /*
  * Return an IO device handle and specification which can be used to access
  * an image. Use this to enforce platform load policy.
@@ -590,9 +474,7 @@ int plat_get_image_source(unsigned int image_id, uintptr_t *dev_handle,
 	int rc;
 	const struct plat_io_policy *policy;
 
-	assert(image_id < ARRAY_SIZE(policies));
-
-	policy = &policies[image_id];
+	policy = FCONF_GET_PROPERTY(stm32mp, io_policies, image_id);
 	rc = policy->check(policy->image_spec);
 	if (rc == 0) {
 		*image_spec = policy->image_spec;
@@ -601,3 +483,33 @@ int plat_get_image_source(unsigned int image_id, uintptr_t *dev_handle,
 
 	return rc;
 }
+
+/*
+ * This function shall return 0 if it cannot find an alternate
+ * image to be loaded or it returns 1 otherwise.
+ */
+int plat_try_next_boot_source(unsigned int image_id)
+{
+	static unsigned int backup_id;
+	static unsigned int backup_nb;
+
+	/* No backup available */
+	if (nand_bkp_offset == 0U) {
+		return 0;
+	}
+
+	if (backup_id != image_id) {
+		backup_nb = 0;
+		backup_id = image_id;
+	}
+
+	backup_nb++;
+
+	if (backup_nb >= PLATFORM_MTD_BACKUP_BLOCKS) {
+		return 0;
+	}
+
+	image_block_spec.offset += nand_bkp_offset;
+
+	return 1;
+}
diff --git a/plat/st/common/bl2_stm32_io_storage.c b/plat/st/common/bl2_stm32_io_storage.c
new file mode 100644
index 0000000000..5e7ecfad48
--- /dev/null
+++ b/plat/st/common/bl2_stm32_io_storage.c
@@ -0,0 +1,769 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <common/desc_image_load.h>
+#include <drivers/io/io_block.h>
+#include <drivers/io/io_driver.h>
+#include <drivers/io/io_dummy.h>
+#include <drivers/io/io_memmap.h>
+#include <drivers/io/io_mtd.h>
+#include <drivers/io/io_storage.h>
+#include <drivers/mmc.h>
+#include <drivers/partition/partition.h>
+#include <drivers/raw_nand.h>
+#include <drivers/spi_nand.h>
+#include <drivers/spi_nor.h>
+#include <drivers/st/io_mmc.h>
+#include <drivers/st/io_stm32image.h>
+#include <drivers/st/stm32_fmc2_nand.h>
+#include <drivers/st/stm32_qspi.h>
+#include <drivers/st/stm32_sdmmc2.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+#include <lib/usb/usb_core.h>
+#include <lib/usb/usb_st_dfu.h>
+#include <plat/common/platform.h>
+
+#include <stm32cubeprogrammer.h>
+
+/* IO devices */
+#ifndef AARCH32_SP_OPTEE
+static const io_dev_connector_t *dummy_dev_con;
+static uintptr_t dummy_dev_handle;
+static uintptr_t dummy_dev_spec;
+#endif
+
+static uintptr_t image_dev_handle;
+static uintptr_t storage_dev_handle;
+
+#if STM32MP_SDMMC || STM32MP_EMMC
+static io_block_spec_t gpt_block_spec = {
+	.offset = 0,
+	.length = 34 * MMC_BLOCK_SIZE, /* Size of GPT table */
+};
+
+static uint32_t block_buffer[MMC_BLOCK_SIZE] __aligned(MMC_BLOCK_SIZE);
+
+static const io_block_dev_spec_t mmc_block_dev_spec = {
+	/* It's used as temp buffer in block driver */
+	.buffer = {
+		.offset = (size_t)&block_buffer,
+		.length = MMC_BLOCK_SIZE,
+	},
+	.ops = {
+		.read = mmc_read_blocks,
+		.write = NULL,
+	},
+	.block_size = MMC_BLOCK_SIZE,
+};
+
+static const io_dev_connector_t *mmc_dev_con;
+#endif /* STM32MP_SDMMC || STM32MP_EMMC */
+
+#if STM32MP_SPI_NOR
+static io_mtd_dev_spec_t spi_nor_dev_spec = {
+	.ops = {
+		.init = spi_nor_init,
+		.read = spi_nor_read,
+	},
+};
+#endif
+
+#if STM32MP_RAW_NAND
+static io_mtd_dev_spec_t nand_dev_spec = {
+	.ops = {
+		.init = nand_raw_init,
+		.read = nand_read,
+	},
+};
+
+static const io_dev_connector_t *nand_dev_con;
+#endif
+
+#if STM32MP_SPI_NAND
+static io_mtd_dev_spec_t spi_nand_dev_spec = {
+	.ops = {
+		.init = spi_nand_init,
+		.read = nand_read,
+	},
+};
+#endif
+
+#if STM32MP_SPI_NAND || STM32MP_SPI_NOR
+static const io_dev_connector_t *spi_dev_con;
+#endif
+
+#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
+static const io_dev_connector_t *memmap_dev_con;
+#endif
+
+#ifdef AARCH32_SP_OPTEE
+static const struct stm32image_part_info optee_header_partition_spec = {
+	.name = OPTEE_HEADER_IMAGE_NAME,
+	.binary_type = OPTEE_HEADER_BINARY_TYPE,
+};
+
+static const struct stm32image_part_info optee_core_partition_spec = {
+	.name = OPTEE_CORE_IMAGE_NAME,
+	.binary_type = OPTEE_CORE_BINARY_TYPE,
+};
+
+static const struct stm32image_part_info optee_paged_partition_spec = {
+	.name = OPTEE_PAGED_IMAGE_NAME,
+	.binary_type = OPTEE_PAGED_BINARY_TYPE,
+};
+#else
+static const io_block_spec_t bl32_block_spec = {
+	.offset = BL32_BASE,
+	.length = STM32MP_BL32_SIZE
+};
+#endif
+
+static const struct stm32image_part_info bl33_partition_spec = {
+	.name = BL33_IMAGE_NAME,
+	.binary_type = BL33_BINARY_TYPE,
+};
+
+enum {
+	IMG_IDX_BL33,
+#ifdef AARCH32_SP_OPTEE
+	IMG_IDX_OPTEE_HEADER,
+	IMG_IDX_OPTEE_CORE,
+	IMG_IDX_OPTEE_PAGED,
+#endif
+	IMG_IDX_NUM
+};
+
+static struct stm32image_device_info stm32image_dev_info_spec __unused = {
+	.lba_size = MMC_BLOCK_SIZE,
+	.part_info[IMG_IDX_BL33] = {
+		.name = BL33_IMAGE_NAME,
+		.binary_type = BL33_BINARY_TYPE,
+	},
+#ifdef AARCH32_SP_OPTEE
+	.part_info[IMG_IDX_OPTEE_HEADER] = {
+		.name = OPTEE_HEADER_IMAGE_NAME,
+		.binary_type = OPTEE_HEADER_BINARY_TYPE,
+	},
+	.part_info[IMG_IDX_OPTEE_CORE] = {
+		.name = OPTEE_CORE_IMAGE_NAME,
+		.binary_type = OPTEE_CORE_BINARY_TYPE,
+	},
+	.part_info[IMG_IDX_OPTEE_PAGED] = {
+		.name = OPTEE_PAGED_IMAGE_NAME,
+		.binary_type = OPTEE_PAGED_BINARY_TYPE,
+	},
+#endif
+};
+
+static io_block_spec_t image_block_spec = {
+	.offset = 0,
+	.length = 0,
+};
+
+static const io_dev_connector_t *stm32image_dev_con __unused;
+
+#ifndef AARCH32_SP_OPTEE
+static int open_dummy(const uintptr_t spec);
+#endif
+static int open_image(const uintptr_t spec);
+static int open_storage(const uintptr_t spec);
+
+struct plat_io_policy {
+	uintptr_t *dev_handle;
+	uintptr_t image_spec;
+	int (*check)(const uintptr_t spec);
+};
+
+static const struct plat_io_policy policies[] = {
+#ifdef AARCH32_SP_OPTEE
+	[BL32_IMAGE_ID] = {
+		.dev_handle = &image_dev_handle,
+		.image_spec = (uintptr_t)&optee_header_partition_spec,
+		.check = open_image
+	},
+	[BL32_EXTRA1_IMAGE_ID] = {
+		.dev_handle = &image_dev_handle,
+		.image_spec = (uintptr_t)&optee_core_partition_spec,
+		.check = open_image
+	},
+	[BL32_EXTRA2_IMAGE_ID] = {
+		.dev_handle = &image_dev_handle,
+		.image_spec = (uintptr_t)&optee_paged_partition_spec,
+		.check = open_image
+	},
+#else
+	[BL32_IMAGE_ID] = {
+		.dev_handle = &dummy_dev_handle,
+		.image_spec = (uintptr_t)&bl32_block_spec,
+		.check = open_dummy
+	},
+#endif
+	[BL33_IMAGE_ID] = {
+		.dev_handle = &image_dev_handle,
+		.image_spec = (uintptr_t)&bl33_partition_spec,
+		.check = open_image
+	},
+#if STM32MP_SDMMC || STM32MP_EMMC
+	[GPT_IMAGE_ID] = {
+		.dev_handle = &storage_dev_handle,
+		.image_spec = (uintptr_t)&gpt_block_spec,
+		.check = open_storage
+	},
+#endif
+	[STM32_IMAGE_ID] = {
+		.dev_handle = &storage_dev_handle,
+		.image_spec = (uintptr_t)&image_block_spec,
+		.check = open_storage
+	},
+};
+
+#ifndef AARCH32_SP_OPTEE
+static int open_dummy(const uintptr_t spec)
+{
+	return io_dev_init(dummy_dev_handle, 0);
+}
+#endif
+
+static int open_image(const uintptr_t spec)
+{
+	return io_dev_init(image_dev_handle, 0);
+}
+
+static int open_storage(const uintptr_t spec)
+{
+	return io_dev_init(storage_dev_handle, 0);
+}
+
+static void print_boot_device(boot_api_context_t *boot_context)
+{
+	switch (boot_context->boot_interface_selected) {
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_SD:
+		INFO("Using SDMMC\n");
+		break;
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_EMMC:
+		INFO("Using EMMC\n");
+		break;
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NOR_SPI:
+		INFO("Using SPI NOR\n");
+		break;
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_FMC:
+		INFO("Using FMC NAND\n");
+		break;
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_SPI:
+		INFO("Using SPI NAND\n");
+		break;
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART:
+		INFO("Using UART\n");
+		break;
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB:
+		INFO("Using USB\n");
+		break;
+	default:
+		ERROR("Boot interface %u not found\n",
+		      boot_context->boot_interface_selected);
+		panic();
+		break;
+	}
+
+	if (boot_context->boot_interface_instance != 0U) {
+		INFO("  Instance %d\n", boot_context->boot_interface_instance);
+	}
+}
+
+static void stm32image_io_setup(void)
+{
+	int io_result __unused;
+
+	io_result = register_io_dev_stm32image(&stm32image_dev_con);
+	assert(io_result == 0);
+
+	io_result = io_dev_open(stm32image_dev_con,
+				(uintptr_t)&stm32image_dev_info_spec,
+				&image_dev_handle);
+	assert(io_result == 0);
+}
+
+#if STM32MP_SDMMC || STM32MP_EMMC
+static void boot_mmc(enum mmc_device_type mmc_dev_type,
+		     uint16_t boot_interface_instance)
+{
+	int io_result __unused;
+	uint8_t idx;
+	struct stm32image_part_info *part;
+	struct stm32_sdmmc2_params params;
+	struct mmc_device_info device_info;
+	const partition_entry_t *entry;
+
+	zeromem(&device_info, sizeof(struct mmc_device_info));
+	zeromem(&params, sizeof(struct stm32_sdmmc2_params));
+
+	device_info.mmc_dev_type = mmc_dev_type;
+
+	switch (boot_interface_instance) {
+	case 1:
+		params.reg_base = STM32MP_SDMMC1_BASE;
+		break;
+	case 2:
+		params.reg_base = STM32MP_SDMMC2_BASE;
+		break;
+	case 3:
+		params.reg_base = STM32MP_SDMMC3_BASE;
+		break;
+	default:
+		WARN("SDMMC instance not found, using default\n");
+		if (mmc_dev_type == MMC_IS_SD) {
+			params.reg_base = STM32MP_SDMMC1_BASE;
+		} else {
+			params.reg_base = STM32MP_SDMMC2_BASE;
+		}
+		break;
+	}
+
+	if (mmc_dev_type == MMC_IS_SD) {
+		params.flags = MMC_FLAG_SD_CMD6;
+	}
+
+	params.device_info = &device_info;
+	if (stm32_sdmmc2_mmc_init(&params) != 0) {
+		ERROR("SDMMC%u init failed\n", boot_interface_instance);
+		panic();
+	}
+
+	/* Open MMC as a block device to read GPT table */
+	io_result = register_io_dev_block(&mmc_dev_con);
+	if (io_result != 0) {
+		panic();
+	}
+
+	io_result = io_dev_open(mmc_dev_con, (uintptr_t)&mmc_block_dev_spec,
+				&storage_dev_handle);
+	assert(io_result == 0);
+
+	partition_init(GPT_IMAGE_ID);
+
+	io_result = io_dev_close(storage_dev_handle);
+	assert(io_result == 0);
+
+	stm32image_dev_info_spec.device_size =
+		stm32_sdmmc2_mmc_get_device_size();
+
+	for (idx = 0U; idx < IMG_IDX_NUM; idx++) {
+		part = &stm32image_dev_info_spec.part_info[idx];
+		entry = get_partition_entry(part->name);
+		if (entry == NULL) {
+			ERROR("Partition %s not found\n", part->name);
+			panic();
+		}
+
+		part->part_offset = entry->start;
+		part->bkp_offset = 0U;
+	}
+
+	/*
+	 * Re-open MMC with io_mmc, for better perfs compared to
+	 * io_block.
+	 */
+	io_result = register_io_dev_mmc(&mmc_dev_con);
+	assert(io_result == 0);
+
+	io_result = io_dev_open(mmc_dev_con, 0, &storage_dev_handle);
+	assert(io_result == 0);
+}
+#endif /* STM32MP_SDMMC || STM32MP_EMMC */
+
+#if STM32MP_SPI_NOR
+static void boot_spi_nor(boot_api_context_t *boot_context)
+{
+	int io_result __unused;
+	uint8_t idx;
+	struct stm32image_part_info *part;
+
+	io_result = stm32_qspi_init();
+	assert(io_result == 0);
+
+	io_result = register_io_dev_mtd(&spi_dev_con);
+	assert(io_result == 0);
+
+	/* Open connections to device */
+	io_result = io_dev_open(spi_dev_con,
+				(uintptr_t)&spi_nor_dev_spec,
+				&storage_dev_handle);
+	assert(io_result == 0);
+
+	stm32image_dev_info_spec.device_size = spi_nor_dev_spec.device_size;
+
+	idx = IMG_IDX_BL33;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NOR_BL33_OFFSET;
+	part->bkp_offset = 0U;
+
+#ifdef AARCH32_SP_OPTEE
+	idx = IMG_IDX_OPTEE_HEADER;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NOR_TEEH_OFFSET;
+	part->bkp_offset = 0U;
+
+	idx = IMG_IDX_OPTEE_PAGED;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NOR_TEED_OFFSET;
+	part->bkp_offset = 0U;
+
+	idx = IMG_IDX_OPTEE_CORE;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NOR_TEEX_OFFSET;
+	part->bkp_offset = 0U;
+#endif
+}
+#endif /* STM32MP_SPI_NOR */
+
+#if STM32MP_RAW_NAND
+static void boot_fmc2_nand(boot_api_context_t *boot_context)
+{
+	int io_result __unused;
+	uint8_t idx;
+	struct stm32image_part_info *part;
+
+	io_result = stm32_fmc2_init();
+	assert(io_result == 0);
+
+	/* Register the IO device on this platform */
+	io_result = register_io_dev_mtd(&nand_dev_con);
+	assert(io_result == 0);
+
+	/* Open connections to device */
+	io_result = io_dev_open(nand_dev_con, (uintptr_t)&nand_dev_spec,
+				&storage_dev_handle);
+	assert(io_result == 0);
+
+	stm32image_dev_info_spec.device_size = nand_dev_spec.device_size;
+
+	idx = IMG_IDX_BL33;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NAND_BL33_OFFSET;
+	part->bkp_offset = nand_dev_spec.erase_size;
+
+#ifdef AARCH32_SP_OPTEE
+	idx = IMG_IDX_OPTEE_HEADER;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NAND_TEEH_OFFSET;
+	part->bkp_offset = nand_dev_spec.erase_size;
+
+	idx = IMG_IDX_OPTEE_PAGED;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NAND_TEED_OFFSET;
+	part->bkp_offset = nand_dev_spec.erase_size;
+
+	idx = IMG_IDX_OPTEE_CORE;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NAND_TEEX_OFFSET;
+	part->bkp_offset = nand_dev_spec.erase_size;
+#endif
+}
+#endif /* STM32MP_RAW_NAND */
+
+#if STM32MP_SPI_NAND
+static void boot_spi_nand(boot_api_context_t *boot_context)
+{
+	int io_result __unused;
+	uint8_t idx;
+	struct stm32image_part_info *part;
+
+	io_result = stm32_qspi_init();
+	assert(io_result == 0);
+
+	io_result = register_io_dev_mtd(&spi_dev_con);
+	assert(io_result == 0);
+
+	/* Open connections to device */
+	io_result = io_dev_open(spi_dev_con,
+				(uintptr_t)&spi_nand_dev_spec,
+				&storage_dev_handle);
+	assert(io_result == 0);
+
+	stm32image_dev_info_spec.device_size = spi_nand_dev_spec.device_size;
+
+	idx = IMG_IDX_BL33;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NAND_BL33_OFFSET;
+	part->bkp_offset = spi_nand_dev_spec.erase_size;
+
+#ifdef AARCH32_SP_OPTEE
+	idx = IMG_IDX_OPTEE_HEADER;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NAND_TEEH_OFFSET;
+	part->bkp_offset = spi_nand_dev_spec.erase_size;
+
+	idx = IMG_IDX_OPTEE_PAGED;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NAND_TEED_OFFSET;
+	part->bkp_offset = spi_nand_dev_spec.erase_size;
+
+	idx = IMG_IDX_OPTEE_CORE;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = STM32MP_NAND_TEEX_OFFSET;
+	part->bkp_offset = spi_nand_dev_spec.erase_size;
+#endif
+}
+#endif /* STM32MP_SPI_NAND */
+
+#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
+static void mmap_io_setup(void)
+{
+	int io_result __unused;
+
+	io_result = register_io_dev_memmap(&memmap_dev_con);
+	assert(io_result == 0);
+
+	io_result = io_dev_open(memmap_dev_con, (uintptr_t)NULL,
+				&storage_dev_handle);
+	assert(io_result == 0);
+}
+
+static void stm32image_mmap_setup(void)
+{
+	uint8_t idx;
+	struct stm32image_part_info *part;
+
+	stm32image_dev_info_spec.device_size = DWL_BUFFER_SIZE;
+
+	idx = IMG_IDX_BL33;
+	part = &stm32image_dev_info_spec.part_info[idx];
+	part->part_offset = 0;
+	part->bkp_offset = 0;
+}
+#endif
+
+#if STM32MP_UART_PROGRAMMER
+static void stm32cubeprogrammer_uart(unsigned int image_id)
+{
+	int ret __unused;
+	boot_api_context_t *boot_context =
+		(boot_api_context_t *)stm32mp_get_boot_ctx_address();
+	uintptr_t uart_base;
+
+	uart_base = get_uart_address(boot_context->boot_interface_instance);
+	ret = stm32cubeprog_uart_load(image_id, uart_base, FLASHLAYOUT_BASE, FLASHLAYOUT_SIZE,
+				      DWL_BUFFER_BASE, DWL_BUFFER_SIZE);
+	assert(ret == 0);
+
+	flush_dcache_range(FLASHLAYOUT_BASE, FLASHLAYOUT_SIZE);
+}
+#endif
+
+#if STM32MP_USB_PROGRAMMER
+static void stm32cubeprogrammer_usb(unsigned int image_id)
+{
+	usb_handle_t *pdev;
+	int ret __unused;
+
+	/* init USB on platform */
+	pdev = usb_dfu_plat_init();
+
+	ret = stm32cubeprog_usb_load(image_id, pdev, FLASHLAYOUT_BASE, FLASHLAYOUT_SIZE,
+				     DWL_BUFFER_BASE, DWL_BUFFER_SIZE);
+	assert(ret == 0);
+
+	flush_dcache_range(FLASHLAYOUT_BASE, FLASHLAYOUT_SIZE);
+}
+#endif
+
+void stm32mp_io_setup(void)
+{
+	int io_result __unused;
+	boot_api_context_t *boot_context =
+		(boot_api_context_t *)stm32mp_get_boot_ctx_address();
+
+	print_boot_device(boot_context);
+
+	if ((boot_context->boot_partition_used_toboot == 1U) ||
+	    (boot_context->boot_partition_used_toboot == 2U)) {
+		INFO("Boot used partition fsbl%d\n",
+		     boot_context->boot_partition_used_toboot);
+	}
+
+#ifndef AARCH32_SP_OPTEE
+	io_result = register_io_dev_dummy(&dummy_dev_con);
+	assert(io_result == 0);
+
+	io_result = io_dev_open(dummy_dev_con, dummy_dev_spec,
+				&dummy_dev_handle);
+	assert(io_result == 0);
+#endif
+
+	switch (boot_context->boot_interface_selected) {
+#if STM32MP_SDMMC
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_SD:
+		dmbsy();
+		boot_mmc(MMC_IS_SD, boot_context->boot_interface_instance);
+		stm32image_io_setup();
+		break;
+#endif
+#if STM32MP_EMMC
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_EMMC:
+		dmbsy();
+		boot_mmc(MMC_IS_EMMC, boot_context->boot_interface_instance);
+		stm32image_io_setup();
+		break;
+#endif
+#if STM32MP_SPI_NOR
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NOR_SPI:
+		dmbsy();
+		boot_spi_nor(boot_context);
+		stm32image_io_setup();
+		break;
+#endif
+#if STM32MP_RAW_NAND
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_FMC:
+		dmbsy();
+		boot_fmc2_nand(boot_context);
+		stm32image_io_setup();
+		break;
+#endif
+#if STM32MP_SPI_NAND
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_SPI:
+		dmbsy();
+		boot_spi_nand(boot_context);
+		stm32image_io_setup();
+		break;
+#endif
+#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
+#if STM32MP_UART_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART:
+#endif
+#if STM32MP_USB_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB:
+#endif
+		dmbsy();
+		mmap_io_setup();
+		stm32image_mmap_setup();
+		stm32image_io_setup();
+		break;
+#endif
+	default:
+		ERROR("Boot interface %d not supported\n",
+		      boot_context->boot_interface_selected);
+		panic();
+		break;
+	}
+}
+
+int bl2_plat_handle_pre_image_load(unsigned int image_id)
+{
+	boot_api_context_t *boot_context =
+		(boot_api_context_t *)stm32mp_get_boot_ctx_address();
+
+	switch (boot_context->boot_interface_selected) {
+#if STM32MP_UART_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART:
+		if (image_id == BL33_IMAGE_ID) {
+			stm32cubeprogrammer_uart(STM32_IMAGE_ID);
+			/* BL33 at SSBL load address */
+			image_block_spec.offset = DWL_BUFFER_BASE;
+			image_block_spec.length = DWL_BUFFER_SIZE;
+		}
+		break;
+#endif
+#if STM32MP_USB_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB:
+		if (image_id == BL33_IMAGE_ID) {
+			stm32cubeprogrammer_usb(STM32_IMAGE_ID);
+			/* BL33 at SSBL load address */
+			image_block_spec.offset = DWL_BUFFER_BASE;
+			image_block_spec.length = DWL_BUFFER_SIZE;
+		}
+		break;
+#endif
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+/*
+ * Return an IO device handle and specification which can be used to access
+ * an image. Use this to enforce platform load policy.
+ */
+int plat_get_image_source(unsigned int image_id, uintptr_t *dev_handle,
+			  uintptr_t *image_spec)
+{
+	int rc;
+	const struct plat_io_policy *policy;
+
+	assert(image_id < ARRAY_SIZE(policies));
+
+	policy = &policies[image_id];
+	rc = policy->check(policy->image_spec);
+	if (rc == 0) {
+		*image_spec = policy->image_spec;
+		*dev_handle = *(policy->dev_handle);
+	}
+
+	return rc;
+}
+
+/*
+ * This function shall return 0 if it cannot find an alternate
+ * image to be loaded and any non-zero value otherwise.
+ */
+int plat_try_next_boot_source(unsigned int image_id)
+{
+	int io_result __unused;
+	const struct stm32image_part_info *partition_spec;
+	struct stm32image_part_info *part;
+	const struct plat_io_policy *policy;
+	uint32_t idx;
+	static unsigned int backup_nb;
+	static unsigned int backup_id = MAX_NUMBER_IDS;
+
+	assert(image_id < ARRAY_SIZE(policies));
+
+	if (backup_id != image_id) {
+		backup_id = image_id;
+		backup_nb = 0;
+	}
+
+	backup_nb++;
+
+	if (backup_nb >= PLATFORM_MTD_BACKUP_BLOCKS) {
+		return 0;
+	}
+
+	policy = &policies[image_id];
+	partition_spec = (struct stm32image_part_info *)policy->image_spec;
+	for (idx = 0U; idx < STM32_PART_NUM; idx++) {
+		part = &stm32image_dev_info_spec.part_info[idx];
+		if (part->binary_type == partition_spec->binary_type) {
+			break;
+		}
+	}
+
+	assert(idx < STM32_PART_NUM);
+
+	if (part->bkp_offset == 0U) {
+		return 0;
+	}
+
+	part->part_offset += part->bkp_offset;
+	/*
+	 * Reopen the io_dev as it was closed in the load_auth_image()
+	 * sequence.
+	 */
+	io_result = io_dev_open(stm32image_dev_con,
+				(uintptr_t)&stm32image_dev_info_spec,
+				&image_dev_handle);
+	assert(io_result == 0);
+
+	return 1;
+}
diff --git a/plat/st/common/include/stm32cubeprogrammer.h b/plat/st/common/include/stm32cubeprogrammer.h
new file mode 100644
index 0000000000..947ad43aea
--- /dev/null
+++ b/plat/st/common/include/stm32cubeprogrammer.h
@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2015-2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef STM32CUBEPROGRAMMER_H
+#define STM32CUBEROGRAMMER_H
+
+/* Phase definition */
+#define PHASE_FLASHLAYOUT	0U
+#define PHASE_FSBL1		1U
+#define PHASE_FSBL2		2U
+#define PHASE_SSBL		3U
+#define PHASE_CMD		0xF1U
+#define PHASE_SSP		0xF3U
+#define PHASE_RESET		0xFFU
+
+/* Command definition */
+#define GET_CMD_COMMAND		0x00U
+#define GET_VER_COMMAND		0x01U
+#define GET_ID_COMMAND		0x02U
+#define PHASE_COMMAND		0x03U
+#define READ_PART_COMMAND	0x12U
+#define START_COMMAND		0x21U
+#define DOWNLOAD_COMMAND	0x31U
+
+/* Answer defines  */
+#define INIT_BYTE		0x7FU
+#define ACK_BYTE		0x79U
+#define NACK_BYTE		0x1FU
+#define ABORT			0x5FU
+
+#define DEVICE_ID_BYTE1		0x05U
+#define DEVICE_ID_BYTE2		0x00U
+
+/* Functions provided by plat */
+uint8_t usb_dfu_get_phase(uint8_t alt);
+
+typedef struct usb_handle usb_handle_t;
+int stm32cubeprog_usb_load(unsigned int image_id,
+			   usb_handle_t *usb_core_handle,
+			   uintptr_t flashlayout_base,
+			   size_t flashlayout_len,
+			   uintptr_t ssbl_base,
+			   size_t ssbl_len);
+
+int stm32cubeprog_uart_load(unsigned int image_id,
+			    uintptr_t instance,
+			    uintptr_t flashlayout_base,
+			    size_t flashlayout_len,
+			    uintptr_t ssbl_base,
+			    size_t ssbl_len);
+
+int stm32cubeprog_usb_ssp(usb_handle_t *usb_core_handle,
+			  uintptr_t cert_base,
+			  size_t cert_len,
+			  uintptr_t ssp_base,
+			  size_t ssp_len);
+
+int stm32cubeprog_uart_ssp(uintptr_t instance,
+			   uintptr_t cert_base,
+			   size_t cert_len,
+			   uintptr_t ssp_base,
+			   size_t ssp_len);
+
+#endif /* STM32CUBEROGRAMMER_H */
diff --git a/plat/st/common/include/stm32mp_auth.h b/plat/st/common/include/stm32mp_auth.h
deleted file mode 100644
index 3075d18ac7..0000000000
--- a/plat/st/common/include/stm32mp_auth.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*
- * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef STM32MP_AUTH_H
-#define STM32MP_AUTH_H
-
-struct stm32mp_auth_ops {
-	uint32_t (*check_key)(uint8_t *pubkey_in, uint8_t *pubkey_out);
-	uint32_t (*verify_signature)(uint8_t *hash_in, uint8_t *pubkey_in,
-				     uint8_t *signature, uint32_t ecc_algo);
-};
-
-void stm32mp_init_auth(struct stm32mp_auth_ops *init_ptr);
-int stm32mp_auth_image(boot_api_image_header_t *header, uintptr_t buffer);
-
-#endif /* STM32MP_AUTH_H */
diff --git a/plat/st/common/include/stm32mp_common.h b/plat/st/common/include/stm32mp_common.h
index feeb4a790d..022e22742a 100644
--- a/plat/st/common/include/stm32mp_common.h
+++ b/plat/st/common/include/stm32mp_common.h
@@ -7,16 +7,26 @@
 #ifndef STM32MP_COMMON_H
 #define STM32MP_COMMON_H
 
+#include <cdefs.h>
 #include <stdbool.h>
 
 #include <platform_def.h>
 
+void __dead2 stm32mp_plat_reset(int cpu);
+
 /* Functions to save and get boot context address given by ROM code */
 void stm32mp_save_boot_ctx_address(uintptr_t address);
 uintptr_t stm32mp_get_boot_ctx_address(void);
+uint16_t stm32mp_get_boot_itf_selected(void);
+uint32_t stm32mp_get_boot_action(void);
 
 bool stm32mp_is_single_core(void);
 bool stm32mp_is_closed_device(void);
+bool stm32mp_is_auth_supported(void);
+
+const char *stm32mp_get_cpu_supply_name(void);
+const char *stm32mp_get_vdd_supply_name(void);
+const char *stm32mp_get_usb_phy_supply_name(void);
 
 /* Return the base address of the DDR controller */
 uintptr_t stm32mp_ddrctrl_base(void);
@@ -30,9 +40,22 @@ uintptr_t stm32mp_pwr_base(void);
 /* Return the base address of the RCC peripheral */
 uintptr_t stm32mp_rcc_base(void);
 
+void stm32_gic_pcpu_init(void);
+void stm32_gic_init(void);
+int stm32_gic_enable_spi(int node, const char *name);
+
 /* Check MMU status to allow spinlock use */
 bool stm32mp_lock_available(void);
 
+/* SMP protection on PWR registers access */
+void stm32mp_pwr_regs_lock(void);
+void stm32mp_pwr_regs_unlock(void);
+
+int stm32_get_otp_index(const char *otp_name, uint32_t *otp_idx,
+			uint32_t *otp_len);
+int stm32_get_otp_value(const char *otp_name, uint32_t *otp_val);
+int stm32_get_otp_value_from_idx(const uint32_t otp_idx, uint32_t *otp_val);
+
 /* Get IWDG platform instance ID from peripheral IO memory base address */
 uint32_t stm32_iwdg_get_instance(uintptr_t base);
 
@@ -44,6 +67,11 @@ uint32_t stm32_iwdg_get_otp_config(uint32_t iwdg_inst);
 uint32_t stm32_iwdg_shadow_update(uint32_t iwdg_inst, uint32_t flags);
 #endif
 
+#if STM32MP_UART_PROGRAMMER || defined(IMAGE_BL32)
+/* Get the UART address from its instance number */
+uintptr_t get_uart_address(uint32_t instance_nb);
+#endif
+
 /*
  * Platform util functions for the GPIO driver
  * @bank: Target GPIO bank ID as per DT bindings
@@ -64,24 +92,26 @@ uint32_t stm32_get_gpio_bank_offset(unsigned int bank);
 /* Return node offset for target GPIO bank ID @bank or a FDT error code */
 int stm32_get_gpio_bank_pinctrl_node(void *fdt, unsigned int bank);
 
+/* Get the chip revision */
+int stm32mp_get_chip_version(uint32_t *chip_version);
+
+/* Get SOC name */
+#define STM32_SOC_NAME_SIZE 20
+void stm32mp_get_soc_name(char name[STM32_SOC_NAME_SIZE]);
+
 /* Print CPU information */
 void stm32mp_print_cpuinfo(void);
 
 /* Print board information */
 void stm32mp_print_boardinfo(void);
 
-/*
- * Util for clock gating and to get clock rate for stm32 and platform drivers
- * @id: Target clock ID, ID used in clock DT bindings
- */
-bool stm32mp_clk_is_enabled(unsigned long id);
-void stm32mp_clk_enable(unsigned long id);
-void stm32mp_clk_disable(unsigned long id);
-unsigned long stm32mp_clk_get_rate(unsigned long id);
+/* Check HW CPU OPP support */
+bool stm32mp_supports_cpu_opp(uint32_t opp_id);
 
 /* Initialise the IO layer and register platform IO devices */
 void stm32mp_io_setup(void);
 
+#if STM32MP_USE_STM32IMAGE
 /*
  * Check that the STM32 header of a .stm32 binary image is valid
  * @param header: pointer to the stm32 image header
@@ -89,6 +119,13 @@ void stm32mp_io_setup(void);
  * @return: 0 on success, negative value in case of error
  */
 int stm32mp_check_header(boot_api_image_header_t *header, uintptr_t buffer);
+#endif
+
+#if TRUSTED_BOARD_BOOT
+void stm32mp_save_loaded_header(void *header);
+void stm32mp_delete_loaded_header(void);
+boot_api_image_header_t *stm32mp_get_loaded_header(void);
+#endif
 
 /* Functions to map DDR in MMU with non-cacheable attribute, and unmap it */
 int stm32mp_map_ddr_non_cacheable(void);
diff --git a/plat/st/common/include/stm32mp_dt.h b/plat/st/common/include/stm32mp_dt.h
index e3b4e597ee..0ff30a3827 100644
--- a/plat/st/common/include/stm32mp_dt.h
+++ b/plat/st/common/include/stm32mp_dt.h
@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,9 @@
 #define STM32MP_DT_H
 
 #include <stdbool.h>
+#include <stdint.h>
+
+#include <libfdt.h>
 
 #define DT_DISABLED		U(0)
 #define DT_NON_SECURE		U(1)
@@ -25,16 +28,25 @@ struct dt_node_info {
 /*******************************************************************************
  * Function and variable prototypes
  ******************************************************************************/
-int dt_open_and_check(void);
+int dt_open_and_check(uintptr_t dt_addr);
 int fdt_get_address(void **fdt_addr);
 bool fdt_check_node(int node);
 uint8_t fdt_get_status(int node);
+int fdt_get_interrupt(int node, const fdt32_t **array, int *len,
+		      bool *extended);
 int dt_set_stdout_pinctrl(void);
 void dt_fill_device_info(struct dt_node_info *info, int node);
 int dt_get_node(struct dt_node_info *info, int offset, const char *compat);
 int dt_get_stdout_uart_info(struct dt_node_info *info);
+int dt_match_instance_by_compatible(const char *compatible, uintptr_t address);
 uint32_t dt_get_ddr_size(void);
+int dt_get_max_opp_freqvolt(uint32_t *freq_khz, uint32_t *voltage_mv);
+int dt_get_all_opp_freqvolt(uint32_t *count, uint32_t *freq_khz_array,
+			    uint32_t *voltage_mv_array);
 uint32_t dt_get_pwr_vdd_voltage(void);
+const char *dt_get_vdd_regulator_name(void);
+const char *dt_get_cpu_regulator_name(void);
+const char *dt_get_usb_phy_regulator_name(void);
 const char *dt_get_board_model(void);
 int fdt_get_gpio_bank_pin_count(unsigned int bank);
 
diff --git a/plat/st/common/include/stm32mp_fconf_getter.h b/plat/st/common/include/stm32mp_fconf_getter.h
new file mode 100644
index 0000000000..09d853f8f3
--- /dev/null
+++ b/plat/st/common/include/stm32mp_fconf_getter.h
@@ -0,0 +1,29 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef STM32MP_FCONF_GETTER
+#define STM32MP_FCONF_GETTER
+
+#include <assert.h>
+
+#include <lib/fconf/fconf.h>
+
+/* IO policies */
+#define stm32mp__io_policies_getter(id) __extension__ ({	\
+	assert((id) < MAX_NUMBER_IDS);				\
+	&policies[id];						\
+})
+
+struct plat_io_policy {
+	uintptr_t *dev_handle;
+	uintptr_t image_spec;
+	int (*check)(const uintptr_t spec);
+};
+
+extern struct plat_io_policy policies[];
+int fconf_populate_stm32mp_io_policies(uintptr_t config);
+
+#endif /* STM32MP_FCONF_GETTER */
diff --git a/plat/st/common/include/stm32mp_io_storage.h b/plat/st/common/include/stm32mp_io_storage.h
new file mode 100644
index 0000000000..48418a5bf6
--- /dev/null
+++ b/plat/st/common/include/stm32mp_io_storage.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef STM32MP_IO_STORAGE_H
+#define STM32MP_IO_STORAGE_H
+
+#include <stdint.h>
+
+#include <drivers/io/io_storage.h>
+
+/* IO devices handle */
+extern uintptr_t storage_dev_handle;
+extern uintptr_t fip_dev_handle;
+
+extern io_block_spec_t image_block_spec;
+
+/* Function declarations */
+int open_fip(const uintptr_t spec);
+int open_storage(const uintptr_t spec);
+
+#endif /* STM32MP_IO_STORAGE_H */
diff --git a/plat/st/common/include/stm32mp_shres_helpers.h b/plat/st/common/include/stm32mp_shres_helpers.h
index 8b786cc040..8b048284c6 100644
--- a/plat/st/common/include/stm32mp_shres_helpers.h
+++ b/plat/st/common/include/stm32mp_shres_helpers.h
@@ -12,63 +12,16 @@
 #include <common/debug.h>
 
 /*
- * Shared reference counter: increments by 2 on secure increment
- * request, decrements by 2 on secure decrement request. Bit #0
- * is set to 1 on non-secure increment request and reset to 0 on
- * non-secure decrement request. The counter initializes to
- * either 0, 1 or 2 upon their expect default state.
- * Counters saturates once above UINT_MAX / 2.
+ * Lock/unlock access to shared registers
+ *
+ * @lock - NULL or pointer to spin lock
  */
-#define SHREFCNT_NONSECURE_FLAG		0x1UL
-#define SHREFCNT_SECURE_STEP		0x2UL
-#define SHREFCNT_MAX			(UINT32_MAX / 2)
-
-/* Return 1 if refcnt increments from 0, else return 0 */
-static inline int stm32mp_incr_shrefcnt(unsigned int *refcnt, bool secure)
-{
-	int rc = !*refcnt;
-
-	if (secure) {
-		*refcnt += SHREFCNT_SECURE_STEP;
-		if (*refcnt >= SHREFCNT_MAX) {
-			panic();
-		}
-	} else {
-		*refcnt |= SHREFCNT_NONSECURE_FLAG;
-	}
-
-	return rc;
-}
-
-/* Return 1 if refcnt decrements to 0, else return 0 */
-static inline int stm32mp_decr_shrefcnt(unsigned int *refcnt, bool secure)
-{
-	int  rc = 0;
-
-	if (secure) {
-		if (*refcnt < SHREFCNT_MAX) {
-			if (*refcnt < SHREFCNT_SECURE_STEP) {
-				panic();
-			}
-			*refcnt -= SHREFCNT_SECURE_STEP;
-			rc = !*refcnt;
-		}
-	} else {
-		rc = (*refcnt == SHREFCNT_NONSECURE_FLAG) ? 1 : 0;
-		*refcnt &= ~SHREFCNT_NONSECURE_FLAG;
-	}
-
-	return rc;
-}
-
-static inline int stm32mp_incr_refcnt(unsigned int *refcnt)
-{
-	return stm32mp_incr_shrefcnt(refcnt, true);
-}
 
-static inline int stm32mp_decr_refcnt(unsigned int *refcnt)
-{
-	return stm32mp_decr_shrefcnt(refcnt, true);
-}
+void stm32mp_lock_shregs(void);
+void stm32mp_unlock_shregs(void);
+void stm32mp_mmio_clrsetbits_32_shregs(uintptr_t addr, uint32_t clear,
+				       uint32_t set);
+void stm32mp_mmio_clrbits_32_shregs(uintptr_t addr, uint32_t clear);
+void stm32mp_mmio_setbits_32_shregs(uintptr_t addr, uint32_t set);
 
 #endif /* STM32MP_SHRES_HELPERS_H */
diff --git a/plat/st/common/stm32_gic.c b/plat/st/common/stm32_gic.c
new file mode 100644
index 0000000000..ec3e3525cf
--- /dev/null
+++ b/plat/st/common/stm32_gic.c
@@ -0,0 +1,223 @@
+/*
+ * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <libfdt.h>
+
+#include <platform_def.h>
+
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/arm/gicv2.h>
+#include <dt-bindings/interrupt-controller/arm-gic.h>
+#include <lib/utils.h>
+#include <plat/common/platform.h>
+
+struct stm32_gic_instance {
+	uint32_t cells;
+	uint32_t phandle_node;
+};
+
+/******************************************************************************
+ * On a GICv2 system, the Group 1 secure interrupts are treated as Group 0
+ * interrupts.
+ *****************************************************************************/
+static const interrupt_prop_t stm32_interrupt_props[] = {
+	PLATFORM_G1S_PROPS(GICV2_INTR_GROUP0),
+	PLATFORM_G0_PROPS(GICV2_INTR_GROUP0)
+};
+
+/* Fix target_mask_array as secondary core is not able to initialize it */
+static unsigned int target_mask_array[PLATFORM_CORE_COUNT] = {1, 2};
+
+static gicv2_driver_data_t platform_gic_data = {
+	.interrupt_props = stm32_interrupt_props,
+	.interrupt_props_num = ARRAY_SIZE(stm32_interrupt_props),
+	.target_masks = target_mask_array,
+	.target_masks_num = ARRAY_SIZE(target_mask_array),
+};
+
+static struct stm32_gic_instance stm32_gic;
+
+static uint32_t enable_gic_interrupt(const fdt32_t *array)
+{
+	unsigned int id, cfg;
+
+	switch (fdt32_to_cpu(*array)) {
+	case GIC_SPI:
+		id = MIN_SPI_ID;
+		break;
+
+	case GIC_PPI:
+		id = MIN_PPI_ID;
+		break;
+
+	default:
+		id = MIN_SGI_ID;
+		break;
+	}
+
+	id += fdt32_to_cpu(*(array + 1));
+	cfg = (fdt32_to_cpu(*(array + 2)) < IRQ_TYPE_LEVEL_HIGH) ?
+		GIC_INTR_CFG_EDGE : GIC_INTR_CFG_LEVEL;
+
+	if ((id >= MIN_SPI_ID) && (id <= MAX_SPI_ID)) {
+		VERBOSE("Enable IT %i\n", id);
+		gicv2_set_interrupt_type(id, GICV2_INTR_GROUP0);
+		gicv2_set_interrupt_priority(id, STM32MP_IRQ_SEC_SPI_PRIO);
+		gicv2_set_spi_routing(id, STM32MP_PRIMARY_CPU);
+		gicv2_interrupt_set_cfg(id, cfg);
+		gicv2_enable_interrupt(id);
+	}
+
+	return id;
+}
+
+static void find_next_interrupt(const fdt32_t **array)
+{
+	int node;
+	const fdt32_t *cuint;
+	void *fdt;
+
+	assert(fdt32_to_cpu(**array) != stm32_gic.phandle_node);
+
+	if (fdt_get_address(&fdt) == 0) {
+		panic();
+	}
+
+	node = fdt_node_offset_by_phandle(fdt, fdt32_to_cpu(**array));
+	if (node < 0) {
+		panic();
+	}
+
+	cuint = fdt_getprop(fdt, node, "#interrupt-cells", NULL);
+	if (cuint == NULL) {
+		panic();
+	}
+
+	*array += fdt32_to_cpu(*cuint) + 1;
+}
+
+void stm32_gic_init(void)
+{
+	int node;
+	void *fdt;
+	const fdt32_t *cuint;
+	struct dt_node_info dt_gic;
+
+	if (fdt_get_address(&fdt) == 0) {
+		panic();
+	}
+
+	node = dt_get_node(&dt_gic, -1, "arm,cortex-a7-gic");
+	if (node < 0) {
+		panic();
+	}
+
+	platform_gic_data.gicd_base = dt_gic.base;
+
+	cuint = fdt_getprop(fdt, node, "reg", NULL);
+	if (cuint == NULL) {
+		panic();
+	}
+
+	platform_gic_data.gicc_base = fdt32_to_cpu(*(cuint + 2));
+
+	cuint = fdt_getprop(fdt, node, "#interrupt-cells", NULL);
+	if (cuint == NULL) {
+		panic();
+	}
+
+	stm32_gic.cells = fdt32_to_cpu(*cuint);
+
+	stm32_gic.phandle_node = fdt_get_phandle(fdt, node);
+	if (stm32_gic.phandle_node == 0U) {
+		panic();
+	}
+
+	gicv2_driver_init(&platform_gic_data);
+	gicv2_distif_init();
+
+	stm32_gic_pcpu_init();
+}
+
+void stm32_gic_pcpu_init(void)
+{
+	gicv2_pcpu_distif_init();
+	gicv2_set_pe_target_mask(plat_my_core_pos());
+	gicv2_cpuif_enable();
+}
+
+int stm32_gic_enable_spi(int node, const char *name)
+{
+	const fdt32_t *cuint;
+	void *fdt;
+	int res, len;
+	int index = -1;
+	int i = 0;
+	int id = -1;
+	bool extended;
+	const fdt32_t *t_array, *max;
+
+	if (fdt_get_address(&fdt) == 0) {
+		panic();
+	}
+
+	cuint = fdt_getprop(fdt, node, "interrupt-parent", NULL);
+	if (cuint != NULL) {
+		if (stm32_gic.phandle_node != fdt32_to_cpu(*cuint)) {
+			return -FDT_ERR_NOTFOUND;
+		}
+	}
+
+	if (name != NULL) {
+		switch (fdt_get_status(node)) {
+		case DT_SECURE:
+			index = fdt_stringlist_search(fdt, node,
+						      "interrupt-names", name);
+			break;
+		default:
+			index = fdt_stringlist_search(fdt, node,
+						      "secure-interrupt-names",
+						      name);
+			break;
+		}
+
+		if (index < 0) {
+			return index;
+		}
+	}
+
+	res = fdt_get_interrupt(node, &t_array, &len, &extended);
+	if (res < 0) {
+		return res;
+	}
+
+	max = t_array + (len / sizeof(uint32_t));
+
+	while ((t_array < max) && ((i <= index) || (index == -1))) {
+		if (!extended) {
+			if ((index == -1) || (i == index)) {
+				id = enable_gic_interrupt(t_array);
+			}
+			t_array += stm32_gic.cells;
+		} else {
+			if (fdt32_to_cpu(*t_array) == stm32_gic.phandle_node) {
+				t_array++;
+				if ((index == -1) || (i == index)) {
+					id = enable_gic_interrupt(t_array);
+				}
+				t_array += stm32_gic.cells;
+			} else {
+				find_next_interrupt(&t_array);
+			}
+		}
+		i++;
+	}
+
+	return id;
+}
diff --git a/plat/st/common/stm32cubeprogrammer_uart.c b/plat/st/common/stm32cubeprogrammer_uart.c
new file mode 100644
index 0000000000..3fd16aa39c
--- /dev/null
+++ b/plat/st/common/stm32cubeprogrammer_uart.c
@@ -0,0 +1,690 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_iwdg.h>
+#include <drivers/st/stm32_uart.h>
+#include <drivers/st/stm32_uart_regs.h>
+#include <lib/mmio.h>
+#include <tools_share/firmware_image_package.h>
+
+#include <stm32cubeprogrammer.h>
+
+#define PROGRAMMER_TIMEOUT_US	20000U
+
+/* USART bootloader protocol version V4.0 */
+#define USART_BL_VERSION	0x40
+#define UNDEFINED_DOWN_ADDR	0xFFFFFFFF
+
+static const uint8_t command_tab[] = {
+	GET_CMD_COMMAND,
+	GET_VER_COMMAND,
+	GET_ID_COMMAND,
+	PHASE_COMMAND,
+#if STM32MP_SSP
+	READ_PART_COMMAND,
+#endif
+	START_COMMAND,
+	DOWNLOAD_COMMAND
+};
+
+/* STM32CubeProgrammer over UART handle */
+struct stm32prog_uart_handle_s {
+	struct stm32_uart_handle_s uart;
+	uint32_t packet;
+	uint8_t *addr;
+	uint32_t len;
+	uint8_t phase;
+#if STM32MP_SSP
+	uintptr_t cert_base;
+	size_t cert_len;
+#endif
+	/* error msg buffer: max 255 in UART protocol, reduced in TF-A */
+	uint8_t error[64];
+} handle;
+
+/* Trace and handle unrecoverable UART protocol error */
+#define STM32PROG_ERROR(...) \
+	{ \
+		ERROR(__VA_ARGS__); \
+		if (handle.phase != PHASE_RESET) { \
+			snprintf((char *)&handle.error, sizeof(handle.error), __VA_ARGS__); \
+			handle.phase = PHASE_RESET; \
+			handle.addr = (uint8_t *)UNDEFINED_DOWN_ADDR; \
+			handle.len = 0U; \
+			handle.packet = 0U; \
+		} \
+	}
+
+static int uart_write(const uint8_t *addr, uint16_t size)
+{
+	while (size) {
+		if (stm32_uart_putc(&handle.uart, *addr) != 0) {
+			return -EIO;
+		}
+
+		size--;
+		addr++;
+	}
+
+	return 0;
+}
+
+static int uart_write_8(uint8_t byte)
+{
+	return stm32_uart_putc(&handle.uart, byte);
+}
+
+static int uart_write_32(uint32_t value)
+{
+	return uart_write((uint8_t *)&value, 4U);
+}
+
+static int uart_read_8(uint8_t *byte)
+{
+	int ret;
+	uint64_t timeout_ref = timeout_init_us(PROGRAMMER_TIMEOUT_US);
+
+	do {
+		ret = stm32_uart_getc(&handle.uart);
+		if (ret == -EAGAIN) {
+			if (timeout_elapsed(timeout_ref)) {
+				return -ETIMEDOUT;
+			}
+		} else if (ret < 0) {
+			return ret;
+		}
+	} while (ret == -EAGAIN);
+
+	*byte = (uint8_t)ret;
+
+	return 0;
+}
+
+static int uart_flush_and_nack(void)
+{
+	int ret;
+
+	/* read all pending data */
+	do {
+		ret = stm32_uart_getc(&handle.uart);
+	} while (ret >= 0);
+
+	return uart_write_8(NACK_BYTE);
+}
+
+static inline int is_valid_header(fip_toc_header_t *header)
+{
+	if ((header->name == TOC_HEADER_NAME) && (header->serial_number != 0)) {
+		return 1;
+	} else {
+		return 0;
+	}
+}
+
+static int uart_receive_command(uint8_t *command)
+{
+	uint8_t byte = 0U;
+	uint8_t xor = 0U;
+	unsigned int count;
+	bool found = false;
+	int ret;
+
+	ret = uart_read_8(&byte);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* handle reconnection request */
+	if (byte == INIT_BYTE) {
+		*command = byte;
+		return 0;
+	}
+
+	for (count = 0U; count < ARRAY_SIZE(command_tab); count++) {
+		if (command_tab[count] == byte) {
+			found = true;
+			break;
+		}
+	}
+
+	if (!found) {
+		VERBOSE("UART: Command unknown (byte=0x%x)\n", byte);
+		return -EPROTO;
+	}
+
+	ret = uart_read_8(&xor);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if ((byte ^ xor) != 0xFF) {
+		VERBOSE("UART: Command XOR check fail (byte=0x%x, xor=0x%x)\n",
+			byte, xor);
+		return -EPROTO;
+	}
+
+	*command = byte;
+
+	return 0;
+}
+
+static int get_cmd_command(void)
+{
+	int ret;
+	const uint8_t msg[2] = {
+		sizeof(command_tab), /* Length of data - 1 */
+		USART_BL_VERSION
+	};
+
+	ret = uart_write(msg, sizeof(msg));
+	if (ret != 0) {
+		return ret;
+	}
+
+	return uart_write(command_tab, sizeof(command_tab));
+}
+
+static int get_version_command(void)
+{
+	return uart_write_8(STM32_TF_VERSION);
+}
+
+static int get_id_command(void)
+{
+	const uint8_t msg[3] = {
+		sizeof(msg) - 1,
+		DEVICE_ID_BYTE1,
+		DEVICE_ID_BYTE2
+	};
+
+	return uart_write(msg, sizeof(msg));
+}
+
+static int uart_send_phase(uint32_t address)
+{
+	int ret;
+	uint8_t msg_size = 5U; /* Length of data - 1 */
+	uint8_t error_size = 0U;
+
+	/* additionnal information only for RESET phase */
+	if (handle.phase == PHASE_RESET) {
+		error_size = strnlen((char *)&handle.error, sizeof(handle.error));
+	}
+
+	ret = uart_write_8(msg_size + error_size);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* Send the ID of next partition */
+	ret = uart_write_8(handle.phase);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* Destination address */
+	ret = uart_write_32(address);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = uart_write_8(error_size);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* Additional information: message error */
+	if (error_size > 0U) {
+		ret = uart_write(handle.error, error_size);
+	}
+
+	return ret;
+}
+
+static int uart_download_part(void)
+{
+	uint8_t operation = 0U;
+	uint8_t xor;
+	uint8_t byte = 0U;
+	uint32_t packet_number = 0U;
+	uint32_t packet_size = 0U;
+	uint32_t i = 0;
+	int ret;
+
+	/* Get operation number */
+	ret = uart_read_8(&operation);
+	if (ret != 0) {
+		return ret;
+	}
+
+	xor = operation;
+
+	/* Get packet Number */
+	for (i = 3U; i > 0U; i--) {
+		ret = uart_read_8(&byte);
+		if (ret != 0) {
+			return ret;
+		}
+
+		xor ^= byte;
+		packet_number = (packet_number << 8) | byte;
+	}
+
+	if (packet_number != handle.packet) {
+		WARN("UART: Bad packet number receive: %i, expected %i\n",
+		     packet_number, handle.packet);
+		return -EPROTO;
+	}
+
+	/* Checksum */
+	ret = uart_read_8(&byte);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (xor != byte) {
+		VERBOSE("UART: Download Command checksum xor: %x, received %x\n",
+			xor, byte);
+		return -EPROTO;
+	}
+
+	ret = uart_write_8(ACK_BYTE);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = uart_read_8(&byte);
+	if (ret != 0) {
+		return ret;
+	}
+
+	xor = byte;
+	packet_size = byte + 1U;
+	if (handle.len < packet_size) {
+		STM32PROG_ERROR("Download overflow at %p\n", handle.addr + packet_size);
+		return 0;
+	}
+
+	for (i = 0U; i < packet_size; i++) {
+		ret = uart_read_8(&byte);
+		if (ret != 0) {
+			return ret;
+		}
+
+		*(handle.addr + i) = byte;
+		xor ^= byte;
+	}
+
+	/* Checksum */
+	ret = uart_read_8(&byte) != 0;
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (xor != byte) {
+		VERBOSE("UART: Download Data checksum xor: %x, received %x\n",
+			xor, byte);
+		return -EPROTO;
+	}
+
+	/* packet treated */
+	handle.packet++;
+	handle.addr += packet_size;
+	handle.len -= packet_size;
+
+	return 0;
+}
+
+static int uart_start_cmd(unsigned int image_id, uintptr_t buffer)
+{
+	uint8_t byte = 0U;
+	uint8_t xor = 0U;
+	int8_t i;
+	uint32_t start_address = 0U;
+	int ret;
+
+	/* Get address */
+	for (i = 4; i > 0; i--) {
+		ret = uart_read_8(&byte);
+		if (ret != 0) {
+			return ret;
+		}
+
+		xor ^= byte;
+		start_address = (start_address << 8) | byte;
+	}
+
+	/* Checksum */
+	ret = uart_read_8(&byte);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (xor != byte) {
+		VERBOSE("UART: Start Command checksum xor: %x, received %x\n",
+			xor, byte);
+		return -EPROTO;
+	}
+
+	if (start_address != UNDEFINED_DOWN_ADDR) {
+		STM32PROG_ERROR("Invalid start at %x, for phase %d\n",
+				start_address, handle.phase);
+		return 0;
+	}
+
+#if !STM32MP_USE_STM32IMAGE
+	if (image_id == FIP_IMAGE_ID) {
+		if (!is_valid_header((fip_toc_header_t *)buffer)) {
+			STM32PROG_ERROR("FIP Header check failed at phase %d\n",
+					(uint32_t)buffer);
+			return -EIO;
+		}
+
+		VERBOSE("FIP header looks OK.\n");
+	}
+#else
+	if (image_id == STM32_IMAGE_ID) {
+		/* Verify header and checksum payload */
+		ret = stm32mp_check_header((boot_api_image_header_t *)buffer,
+					   buffer + sizeof(boot_api_image_header_t));
+		if (ret != 0U) {
+			STM32PROG_ERROR("STM32IMAGE check error at %x\n",
+					(uint32_t)buffer);
+			return -EIO;
+		}
+
+		VERBOSE("STM32 header looks OK.\n");
+	}
+#endif
+
+	return 0;
+}
+
+#if STM32MP_SSP
+static int uart_read_part(void)
+{
+	uint8_t byte = 0U;
+	uint8_t xor = 0U;
+	uint8_t partid = 0U;
+	uint16_t size = 0U;
+	uint32_t start_address = 0U;
+	uint32_t i;
+	size_t length;
+	uint8_t *buffer;
+
+	/* Get partition id */
+	if (uart_read_8(&partid) != 0) {
+		return -EIO;
+	}
+
+	if ((partid != PHASE_FLASHLAYOUT) && (partid != PHASE_SSP)) {
+		return -EPERM;
+	}
+
+	xor = partid;
+
+	/* Get address */
+	for (i = 4U; i > 0U; i--) {
+		if (uart_read_8(&byte) != 0) {
+			return -EIO;
+		}
+
+		xor ^= byte;
+		start_address = (start_address << 8) | byte;
+	}
+
+	/* Checksum */
+	if (uart_read_8(&byte) != 0) {
+		return -EIO;
+	}
+
+	if (xor != byte) {
+		WARN("UART: Start cmd: address checksum: %x != %x\n",
+		     xor, byte);
+		return -EPROTO;
+	}
+	/* OFFSET != 0 not supported */
+	if (start_address != 0U) {
+		return -EIO;
+	}
+
+	uart_write_8(ACK_BYTE);
+
+	/* Get number of bytes to send */
+	if (uart_read_8(&byte) != 0) {
+		return -EIO;
+	}
+
+	xor = byte;
+
+	/* Send Size + 1 */
+	size = byte++;
+
+	/* Checksum */
+	if (uart_read_8(&byte) != 0) {
+		return -EIO;
+	}
+
+	if ((xor ^ byte) != 0xFF) {
+		WARN("UART: Start cmd: length checksum: %x != %x\n", xor, byte);
+		return -EPROTO;
+	}
+
+	uart_write_8(ACK_BYTE);
+
+	if (partid != PHASE_SSP) {
+		WARN("Not supported\n");
+		return -EPROTO;
+	}
+
+	length = handle.cert_len;
+	buffer = (uint8_t *)handle.cert_base;
+
+	for (i = 0U; i < length; i++, buffer++) {
+		uart_write_8(*buffer);
+	}
+	for (; i < size; i++) {
+		uart_write_8(0x0);
+	}
+
+	return 0;
+}
+#endif /* STM32MP_SSP */
+
+static int uart_read(unsigned int image_id, uint8_t id, uintptr_t buffer, size_t length)
+{
+	bool start_done = false;
+	int ret;
+	uint8_t command = 0U;
+
+	handle.phase = id;
+	handle.packet = 0U;
+	handle.addr = (uint8_t *)buffer;
+	handle.len = length;
+
+	INFO("UART: read phase %i at 0x%lx size 0x%x\n",
+	     id, buffer, length);
+	while (!start_done) {
+
+		stm32_iwdg_refresh();
+
+		ret = uart_receive_command(&command);
+		if (ret != 0U) {
+			/* delay to wait STM32CubeProgrammer end of transmission */
+			mdelay(2);
+
+			ret = uart_flush_and_nack();
+			if (ret != 0U) {
+				return ret;
+			}
+
+			continue;
+		}
+
+		uart_write_8(ACK_BYTE);
+
+		switch (command) {
+		case INIT_BYTE:
+			INFO("UART: Connected\n");
+			/* Nothing to do */
+			continue;
+
+		case GET_CMD_COMMAND:
+			ret = get_cmd_command();
+			break;
+
+		case GET_VER_COMMAND:
+			ret = get_version_command();
+			break;
+
+		case GET_ID_COMMAND:
+			ret = get_id_command();
+			break;
+
+		case PHASE_COMMAND:
+			ret = uart_send_phase((uint32_t)buffer);
+			if ((ret == 0U) && (handle.phase == PHASE_RESET)) {
+				start_done = true;
+				INFO("UART: Reset\n");
+			}
+			break;
+
+		case DOWNLOAD_COMMAND:
+			ret = uart_download_part();
+			break;
+#if STM32MP_SSP
+		case READ_PART_COMMAND:
+			ret = uart_read_part();
+			break;
+#endif
+		case START_COMMAND:
+			ret = uart_start_cmd(image_id, buffer);
+			if ((ret == 0U) && (handle.phase == id)) {
+				INFO("UART: Start phase %d\n", handle.phase);
+#if STM32MP_SSP
+				if (handle.phase == PHASE_SSP) {
+					handle.phase = PHASE_RESET;
+					break;
+				}
+#endif
+				start_done = true;
+			}
+			break;
+
+		default:
+			/* Not supported command  */
+			WARN("UART: Unknown command\n");
+			ret = -EINVAL;
+			break;
+		}
+
+		if (ret == 0U) {
+			ret = uart_write_8(ACK_BYTE);
+		} else {
+			ret = uart_flush_and_nack();
+		}
+
+		if (ret != 0U) {
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/* Init UART: 115200, 8bit 1stop parity even and enable FIFO mode */
+const struct stm32_uart_init_s init = {
+	.baud_rate = U(115200),
+	.word_length = STM32_UART_WORDLENGTH_9B,
+	.stop_bits = STM32_UART_STOPBITS_1,
+	.parity = STM32_UART_PARITY_EVEN,
+	.hw_flow_control = STM32_UART_HWCONTROL_NONE,
+	.mode = STM32_UART_MODE_TX_RX,
+	.over_sampling = STM32_UART_OVERSAMPLING_16,
+	.fifo_mode = STM32_UART_FIFOMODE_EN,
+};
+
+#if STM32MP_SSP
+int stm32cubeprog_uart_ssp(uintptr_t instance,
+			   uintptr_t cert_base,
+			   size_t cert_len,
+			   uintptr_t ssp_base,
+			   size_t ssp_len)
+{
+	int ret;
+
+	if (stm32_uart_init(&handle.uart, instance, &init) != 0U) {
+		return -EIO;
+	}
+
+	/* NACK to synchronize STM32CubeProgrammer */
+	ret = uart_flush_and_nack();
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (cert_base == UNDEFINED_DOWN_ADDR) {
+		/* Send Provisioning message to programmer for reboot */
+		STM32PROG_ERROR("Provisioning\n");
+	} else {
+		handle.cert_base = cert_base;
+		handle.cert_len  = cert_len;
+		handle.phase = PHASE_SSP;
+	}
+
+	return uart_read(MAX_IMAGE_IDS, handle.phase, ssp_base, ssp_len);
+
+}
+#endif
+
+int stm32cubeprog_uart_load(unsigned int image_id,
+			    uintptr_t instance,
+			    uintptr_t flashlayout_base,
+			    size_t flashlayout_len,
+			    uintptr_t ssbl_base,
+			    size_t ssbl_len)
+{
+	int ret;
+
+	if (stm32_uart_init(&handle.uart, instance, &init) != 0U) {
+		return -EIO;
+	}
+
+	/*
+	 * The following NACK_BYTE is written because STM32CubeProgrammer has
+	 * already sent its command before TF-A has reached this point, and
+	 * because FIFO was not configured by BootROM.
+	 * The byte in the UART_RX register is then the checksum and not the
+	 * command. NACK_BYTE has to be written, so that the programmer will
+	 * re-send the good command.
+	 */
+	ret = uart_flush_and_nack();
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* Get FlashLayout with PhaseId=0 */
+	if (flashlayout_len > 0U) {
+		ret = uart_read(STM32_IMAGE_ID, PHASE_FLASHLAYOUT,
+				flashlayout_base, flashlayout_len);
+		if (ret != 0U) {
+			return ret;
+		}
+	}
+
+	ret = uart_read(image_id, PHASE_SSBL, ssbl_base, ssbl_len);
+
+	return ret;
+}
diff --git a/plat/st/common/stm32cubeprogrammer_usb.c b/plat/st/common/stm32cubeprogrammer_usb.c
new file mode 100644
index 0000000000..eefe6413b5
--- /dev/null
+++ b/plat/st/common/stm32cubeprogrammer_usb.c
@@ -0,0 +1,352 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <lib/usb/usb_st_dfu.h>
+#include <tools_share/firmware_image_package.h>
+
+#include <stm32cubeprogrammer.h>
+
+/* Undefined download address */
+#define UNDEFINED_DOWN_ADDR	0xFFFFFFFF
+
+#define USB_STATE_READY		0
+#define USB_STATE_WRITTEN	1
+
+#define USB_DFU_MAX_XFER_SIZE	USBD_DFU_XFER_SIZE
+
+typedef struct {
+	unsigned int image_id;
+	uint8_t phase;
+	uintptr_t base;
+	size_t len;
+	uintptr_t address;
+	/* parameter */
+	uintptr_t ssbl_base;
+	size_t ssbl_len;
+#if STM32MP_SSP
+	uintptr_t cert_base;
+	size_t cert_len;
+#endif
+	/* working buffer */
+	uint8_t buffer[255];
+} dfu_state_t;
+
+static dfu_state_t dfu_state;
+
+#define DFU_ERROR(...) \
+	{ \
+		ERROR(__VA_ARGS__); \
+		if (dfu->phase != PHASE_RESET) { \
+			snprintf((char *)&dfu->buffer[9], \
+				 sizeof(dfu->buffer) - 9, __VA_ARGS__); \
+			dfu->phase = PHASE_RESET; \
+			dfu->address = UNDEFINED_DOWN_ADDR; \
+			dfu->len = 0; \
+		} \
+	}
+
+static inline bool is_valid_header(fip_toc_header_t *header)
+{
+	if ((header->name == TOC_HEADER_NAME) && (header->serial_number != 0U)) {
+		return true;
+	}
+
+	return false;
+}
+
+static int dfu_callback_upload(uint8_t alt, uintptr_t *buffer, uint32_t *len,
+			       void *user_data)
+{
+	int result = 0;
+	uint32_t length = 0;
+	dfu_state_t *dfu = (dfu_state_t *)user_data;
+
+	switch (usb_dfu_get_phase(alt)) {
+	case PHASE_CMD:
+		/* Get Pḧase */
+#if STM32MP_SSP
+		if (dfu->phase == PHASE_SSP) {
+			dfu->buffer[0] = PHASE_FLASHLAYOUT;
+		} else {
+			dfu->buffer[0] = dfu->phase;
+		}
+#else
+		dfu->buffer[0] = dfu->phase;
+#endif
+		dfu->buffer[1] = (uint8_t)(dfu->address);
+		dfu->buffer[2] = (uint8_t)(dfu->address >> 8);
+		dfu->buffer[3] = (uint8_t)(dfu->address >> 16);
+		dfu->buffer[4] = (uint8_t)(dfu->address >> 24);
+		dfu->buffer[5] = 0x00;
+		dfu->buffer[6] = 0x00;
+		dfu->buffer[7] = 0x00;
+		dfu->buffer[8] = 0x00;
+		length = 9;
+		if (dfu->phase == PHASE_FLASHLAYOUT &&
+		    dfu->address == UNDEFINED_DOWN_ADDR) {
+			INFO("Send detach request\n");
+			dfu->buffer[9] = 0x01;
+			length = 10;
+		}
+		if (dfu->phase == PHASE_RESET) {
+			length = 8 + strnlen((char *)&dfu->buffer[9],
+					     sizeof(dfu->buffer) - 9);
+		}
+		break;
+
+#if STM32MP_SSP
+	case PHASE_SSP:
+		/* Fix phase to flashlayout phase */
+		dfu->buffer[0] = PHASE_FLASHLAYOUT;
+		dfu->buffer[1] = (uint8_t)(dfu_state.cert_base);
+		dfu->buffer[2] = (uint8_t)(dfu_state.cert_base >> 8);
+		dfu->buffer[3] = (uint8_t)(dfu_state.cert_base >> 16);
+		dfu->buffer[4] = (uint8_t)(dfu_state.cert_base >> 24);
+		dfu->buffer[5] = 0x00;
+		dfu->buffer[6] = 0x00;
+		dfu->buffer[7] = 0x00;
+		dfu->buffer[8] = 0x00;
+		length = 9U;
+
+		if ((length + dfu_state.cert_len) <= sizeof(dfu->buffer)) {
+			memcpy(&dfu->buffer[9], (uint8_t *)dfu_state.cert_base,
+			       dfu_state.cert_len);
+			length += dfu_state.cert_len;
+		}
+
+		break;
+#endif
+	default:
+		DFU_ERROR("phase ID :%i, alternate %i for phase %i\n",
+			  dfu->phase, alt, usb_dfu_get_phase(alt));
+		result = -EIO;
+		break;
+	}
+
+	if (result == 0) {
+		*len = length;
+		*buffer = (uintptr_t)dfu->buffer;
+	}
+
+	return result;
+}
+
+static int dfu_callback_download(uint8_t alt, uintptr_t *buffer, uint32_t *len,
+				 void *user_data)
+{
+	dfu_state_t *dfu = (dfu_state_t *)user_data;
+
+	if ((dfu->phase != usb_dfu_get_phase(alt)) ||
+	    (dfu->address == UNDEFINED_DOWN_ADDR)) {
+		DFU_ERROR("phase ID :%i, alternate %i, address %x\n",
+			  dfu->phase, alt, (uint32_t)dfu->address);
+		return -EIO;
+	}
+
+	VERBOSE("Download %d %lx %x\n", alt, dfu->address, *len);
+	*buffer = dfu->address;
+	dfu->address += *len;
+
+	if (dfu->address - dfu->base > dfu->len) {
+		return  -EIO;
+	}
+
+	return 0;
+}
+
+static int dfu_callback_manifestation(uint8_t alt, void *user_data)
+{
+#if STM32MP_USE_STM32IMAGE
+	int result;
+#endif
+	boot_api_image_header_t *header __unused;
+	dfu_state_t *dfu = (dfu_state_t *)user_data;
+
+	if (dfu->phase != usb_dfu_get_phase(alt)) {
+		ERROR("Manifestation phase ID :%i, alternate %i, address %lx\n",
+		      dfu->phase, alt, dfu->address);
+		return -EIO;
+	}
+
+	INFO("phase ID :%i, Manifestation %d at %lx\n",
+	     dfu->phase, alt, dfu->address);
+	switch (dfu->phase) {
+#if STM32MP_SSP
+	case PHASE_SSP:
+		/* Configure End with request detach */
+		dfu->phase = PHASE_FLASHLAYOUT;
+		dfu->address = UNDEFINED_DOWN_ADDR;
+		dfu->len = 0;
+		break;
+#else
+	case PHASE_FLASHLAYOUT:
+		header = (boot_api_image_header_t *)(dfu->base);
+
+		/* TODO check data flush */
+		flush_dcache_range((unsigned long)header,
+				   header->image_length +
+				   sizeof(boot_api_image_header_t));
+
+#if STM32MP_USE_STM32IMAGE
+		/* Verify header and checksum payload */
+		INFO("Flashlayout Header check at %lx\n",
+		     (uintptr_t)header);
+		result = stm32mp_check_header(header,
+					      (unsigned long)header +
+					      sizeof(boot_api_image_header_t));
+		if (result != 0) {
+			DFU_ERROR("Header check failed for phase %d\n", alt);
+			return -EIO;
+		}
+#endif
+		/* Configure U-Boot loading */
+		dfu->phase = PHASE_SSBL;
+		dfu->address = dfu->ssbl_base;
+		dfu->base = dfu->ssbl_base;
+		dfu->len = dfu->ssbl_len;
+		break;
+
+	case PHASE_SSBL:
+#if !STM32MP_USE_STM32IMAGE
+		if (dfu->image_id == FIP_IMAGE_ID) {
+			if (!is_valid_header((fip_toc_header_t *)dfu->base)) {
+				DFU_ERROR("FIP Header check failed for phase %d\n", alt);
+				return -EIO;
+			}
+
+			VERBOSE("FIP header looks OK.\n");
+		}
+#else
+		if (dfu->image_id == STM32_IMAGE_ID) {
+			header = (boot_api_image_header_t *)dfu->base;
+			/* Verify header and checksum payload */
+			result = stm32mp_check_header(header,
+						      dfu->base +
+						      sizeof(boot_api_image_header_t));
+			if (result != 0) {
+				DFU_ERROR("STM32 Header check failed for phase %d\n", alt);
+				return -EIO;
+			}
+
+			VERBOSE("STM32 header looks OK.\n");
+		}
+#endif
+		/* Configure End with request detach */
+		dfu->phase = PHASE_FLASHLAYOUT;
+		dfu->address = UNDEFINED_DOWN_ADDR;
+		dfu->len = 0;
+		break;
+#endif /* STM32MP_SSP */
+	default:
+		DFU_ERROR("Unknown phase\n");
+	}
+
+	return 0;
+}
+
+/* Open a connection to the USB device */
+static const usb_dfu_media_t usb_dfu_fops = {
+	.upload = dfu_callback_upload,
+	.download = dfu_callback_download,
+	.manifestation = dfu_callback_manifestation,
+};
+
+#if STM32MP_SSP
+int stm32cubeprog_usb_ssp(usb_handle_t *usb_core_handle,
+			  uintptr_t cert_base,
+			  size_t cert_len,
+			  uintptr_t ssp_base,
+			  size_t ssp_len)
+{
+	int ret;
+
+	usb_core_handle->user_data = (void *)&dfu_state;
+
+	INFO("DFU USB START...\n");
+	ret = usb_core_start(usb_core_handle);
+	if (ret != USBD_OK) {
+		return -EIO;
+	}
+
+	if (cert_base == UNDEFINED_DOWN_ADDR) {
+		dfu_state_t *dfu = (dfu_state_t *)usb_core_handle->user_data;
+
+		/* Send Provisioning message to programmer for reboot */
+		DFU_ERROR("Provisioning\n");
+	} else {
+		dfu_state.phase = PHASE_SSP;
+		dfu_state.image_id = MAX_IMAGE_IDS;
+		dfu_state.address = ssp_base;
+		dfu_state.base = ssp_base;
+		dfu_state.len = ssp_len;
+		dfu_state.cert_base = cert_base;
+		dfu_state.cert_len = cert_len;
+	}
+
+	ret = usb_dfu_loop(usb_core_handle, &usb_dfu_fops);
+	if (ret != USBD_OK) {
+		return -EIO;
+	}
+
+	INFO("DFU USB STOP...\n");
+	ret = usb_core_stop(usb_core_handle);
+	if (ret != USBD_OK) {
+		return -EIO;
+	}
+
+	return 0;
+}
+#endif
+
+int stm32cubeprog_usb_load(unsigned int image_id,
+			   usb_handle_t *usb_core_handle,
+			   uintptr_t flashlayout_base,
+			   size_t flashlayout_len,
+			   uintptr_t ssbl_base,
+			   size_t ssbl_len)
+{
+	int ret;
+
+	usb_core_handle->user_data = (void *)&dfu_state;
+
+	INFO("DFU USB START...\n");
+	ret = usb_core_start(usb_core_handle);
+	if (ret != USBD_OK) {
+		return -EIO;
+	}
+
+	dfu_state.image_id = image_id;
+	dfu_state.ssbl_base = ssbl_base;
+	dfu_state.ssbl_len = ssbl_len;
+
+	if (flashlayout_len) {
+		dfu_state.phase = PHASE_FLASHLAYOUT;
+		dfu_state.address = flashlayout_base;
+		dfu_state.base = flashlayout_base;
+		dfu_state.len = flashlayout_len;
+	} else {
+		dfu_state.phase = PHASE_SSBL;
+		dfu_state.address = ssbl_base;
+		dfu_state.base = ssbl_base;
+		dfu_state.len = ssbl_len;
+	}
+
+	ret = usb_dfu_loop(usb_core_handle, &usb_dfu_fops);
+	if (ret != USBD_OK) {
+		return -EIO;
+	}
+
+	INFO("DFU USB STOP...\n");
+	ret = usb_core_stop(usb_core_handle);
+	if (ret != USBD_OK) {
+		return -EIO;
+	}
+
+	return 0;
+}
diff --git a/plat/st/common/stm32mp_auth.c b/plat/st/common/stm32mp_auth.c
deleted file mode 100644
index 0ef6d54548..0000000000
--- a/plat/st/common/stm32mp_auth.c
+++ /dev/null
@@ -1,90 +0,0 @@
-/*
- * Copyright (c) 2019, STMicroelectronics - All Rights Reserved
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <errno.h>
-
-#include <platform_def.h>
-
-#include <common/debug.h>
-#include <drivers/io/io_storage.h>
-#include <drivers/st/bsec.h>
-#include <drivers/st/stm32_hash.h>
-#include <lib/xlat_tables/xlat_tables_v2.h>
-#include <plat/common/platform.h>
-
-static const struct stm32mp_auth_ops *auth_ops;
-
-void stm32mp_init_auth(struct stm32mp_auth_ops *init_ptr)
-{
-	if ((init_ptr == NULL) ||
-	    (init_ptr->check_key == NULL) ||
-	    (init_ptr->verify_signature == NULL) ||
-	    (stm32_hash_register() != 0)) {
-		panic();
-	}
-
-	auth_ops = init_ptr;
-}
-
-int stm32mp_auth_image(boot_api_image_header_t *header, uintptr_t buffer)
-{
-	int ret;
-	uint8_t image_hash[BOOT_API_SHA256_DIGEST_SIZE_IN_BYTES];
-	uint32_t header_skip_cksum = sizeof(header->magic) +
-				     sizeof(header->image_signature) +
-				     sizeof(header->payload_checksum);
-
-	/* Check Security Status */
-	if (!stm32mp_is_closed_device()) {
-		if (header->option_flags != 0U) {
-			WARN("Skip signature check (header option)\n");
-			return 0;
-		}
-		INFO("Check signature on Open device\n");
-	}
-
-	ret = mmap_add_dynamic_region(STM32MP_ROM_BASE, STM32MP_ROM_BASE,
-				      STM32MP_ROM_SIZE, MT_CODE | MT_SECURE);
-	if (ret != 0) {
-		return ret;
-	}
-
-	/* Check Public Key */
-	if (auth_ops->check_key(header->ecc_pubk, NULL) != BOOT_API_RETURN_OK) {
-		ret = -EINVAL;
-		goto err;
-	}
-
-	/* Compute end of header hash and payload hash */
-	stm32_hash_init(HASH_SHA256);
-
-	ret = stm32_hash_update((uint8_t *)&header->header_version,
-				sizeof(boot_api_image_header_t) -
-				header_skip_cksum);
-	if (ret != 0) {
-		ERROR("Hash of header failed, %i\n", ret);
-		goto err;
-	}
-
-	ret = stm32_hash_final_update((uint8_t *)buffer,
-			       header->image_length, image_hash);
-	if (ret != 0) {
-		ERROR("Hash of payload failed\n");
-		goto err;
-	}
-
-	/* Verify signature */
-	if (auth_ops->verify_signature(image_hash, header->ecc_pubk,
-				       header->image_signature,
-				       header->ecc_algo_type) !=
-	    BOOT_API_RETURN_OK) {
-		ret = -EINVAL;
-	}
-
-err:
-	mmap_remove_dynamic_region(STM32MP_ROM_BASE, STM32MP_ROM_SIZE);
-	return ret;
-}
diff --git a/plat/st/common/stm32mp_common.c b/plat/st/common/stm32mp_common.c
index 89d8078386..3f385ca317 100644
--- a/plat/st/common/stm32mp_common.c
+++ b/plat/st/common/stm32mp_common.c
@@ -12,9 +12,16 @@
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <drivers/st/stm32mp_clkfunc.h>
+#include <drivers/st/stm32mp_pmic.h>
+#include <lib/spinlock.h>
+#include <lib/utils.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
+#define HEADER_VERSION_MAJOR_MASK	GENMASK(23, 16)
+
+static struct spinlock lock;
+
 uintptr_t plat_get_ns_image_entrypoint(void)
 {
 	return BL33_BASE;
@@ -25,11 +32,23 @@ unsigned int plat_get_syscnt_freq2(void)
 	return read_cntfrq_el0();
 }
 
+#pragma weak stm32mp_plat_reset
+void __dead2 stm32mp_plat_reset(int cpu)
+{
+	panic();
+}
+
 static uintptr_t boot_ctx_address;
+static uint16_t boot_itf_selected;
+static uint32_t boot_action_saved;
 
 void stm32mp_save_boot_ctx_address(uintptr_t address)
 {
+	boot_api_context_t *boot_context = (boot_api_context_t *)address;
+
 	boot_ctx_address = address;
+	boot_itf_selected = boot_context->boot_interface_selected;
+	boot_action_saved = boot_context->boot_action;
 }
 
 uintptr_t stm32mp_get_boot_ctx_address(void)
@@ -37,6 +56,16 @@ uintptr_t stm32mp_get_boot_ctx_address(void)
 	return boot_ctx_address;
 }
 
+uint16_t stm32mp_get_boot_itf_selected(void)
+{
+	return boot_itf_selected;
+}
+
+uint32_t stm32mp_get_boot_action(void)
+{
+	return boot_action_saved;
+}
+
 uintptr_t stm32mp_ddrctrl_base(void)
 {
 	return DDRCTRL_BASE;
@@ -65,45 +94,148 @@ bool stm32mp_lock_available(void)
 	return (read_sctlr() & c_m_bits) == c_m_bits;
 }
 
-int stm32mp_check_header(boot_api_image_header_t *header, uintptr_t buffer)
+void stm32mp_pwr_regs_lock(void)
+{
+	if (stm32mp_lock_available()) {
+		spin_lock(&lock);
+	}
+}
+
+void stm32mp_pwr_regs_unlock(void)
 {
-	uint32_t i;
-	uint32_t img_checksum = 0U;
+	if (stm32mp_lock_available()) {
+		spin_unlock(&lock);
+	}
+}
 
+#if STM32MP_USE_STM32IMAGE
+int stm32mp_check_header(boot_api_image_header_t *header, uintptr_t buffer)
+{
 	/*
 	 * Check header/payload validity:
 	 *	- Header magic
 	 *	- Header version
-	 *	- Payload checksum
+	 *	- Payload checksum if no signature verification
 	 */
 	if (header->magic != BOOT_API_IMAGE_HEADER_MAGIC_NB) {
 		ERROR("Header magic\n");
 		return -EINVAL;
 	}
 
-	if (header->header_version != BOOT_API_HEADER_VERSION) {
+	if ((header->header_version & HEADER_VERSION_MAJOR_MASK) !=
+	    (BOOT_API_HEADER_VERSION & HEADER_VERSION_MAJOR_MASK)) {
 		ERROR("Header version\n");
 		return -EINVAL;
 	}
 
-	for (i = 0U; i < header->image_length; i++) {
-		img_checksum += *(uint8_t *)(buffer + i);
-	}
+	if (header->option_flags == 1U) {
+		uint32_t i;
+		uint32_t img_checksum = 0U;
 
-	if (header->payload_checksum != img_checksum) {
-		ERROR("Checksum: 0x%x (awaited: 0x%x)\n", img_checksum,
-		      header->payload_checksum);
-		return -EINVAL;
+		for (i = 0U; i < header->image_length; i++) {
+			img_checksum += *(uint8_t *)(buffer + i);
+		}
+
+		if (header->payload_checksum != img_checksum) {
+			ERROR("Checksum: 0x%x (awaited: 0x%x)\n", img_checksum,
+			      header->payload_checksum);
+			return -EINVAL;
+		}
 	}
 
 	return 0;
 }
+#endif
+
+/* Return CPU supply name */
+const char *stm32mp_get_cpu_supply_name(void)
+{
+	const char *regulator;
+	const char *supply = NULL;
+
+	regulator = dt_get_cpu_regulator_name();
+	if (regulator == NULL) {
+		return NULL;
+	}
+
+	if (dt_pmic_status() > 0) {
+		if (dt_pmic_find_supply(&supply, regulator) != 0) {
+			return NULL;
+		}
+	}
+
+	return supply;
+}
+
+/* Return VDD supply name */
+const char *stm32mp_get_vdd_supply_name(void)
+{
+	const char *supply = NULL;
+
+	if (dt_pmic_status() > 0) {
+		const char *regulator = dt_get_vdd_regulator_name();
+
+		if (regulator != NULL) {
+			dt_pmic_find_supply(&supply, regulator);
+		}
+	}
+
+	return supply;
+}
+
+/* Return USB phy supply name */
+const char *stm32mp_get_usb_phy_supply_name(void)
+{
+	const char *supply = NULL;
+
+	if (dt_pmic_status() > 0) {
+		const char *regulator = dt_get_usb_phy_regulator_name();
+
+		if (regulator != NULL) {
+			dt_pmic_find_supply(&supply, regulator);
+		}
+	}
+
+	return supply;
+}
+
+#if TRUSTED_BOARD_BOOT && STM32MP_USE_STM32IMAGE
+/* Save pointer to last loaded header */
+static boot_api_image_header_t *latest_stm32_header;
+
+/* Save last loaded header */
+void stm32mp_save_loaded_header(void *header)
+{
+	assert(latest_stm32_header == NULL);
+
+	latest_stm32_header = header;
+}
+
+/* Discard last loaded header */
+void stm32mp_delete_loaded_header(void)
+{
+	if (latest_stm32_header == NULL) {
+		return;
+	}
+
+	zeromem(latest_stm32_header, sizeof(boot_api_image_header_t));
+	latest_stm32_header = NULL;
+}
+
+/* Get last loaded header */
+boot_api_image_header_t *stm32mp_get_loaded_header(void)
+{
+	assert(latest_stm32_header != NULL);
+
+	return latest_stm32_header;
+}
+#endif /* TRUSTED_BOARD_BOOT */
 
 int stm32mp_map_ddr_non_cacheable(void)
 {
 	return  mmap_add_dynamic_region(STM32MP_DDR_BASE, STM32MP_DDR_BASE,
 					STM32MP_DDR_MAX_SIZE,
-					MT_NON_CACHEABLE | MT_RW | MT_NS);
+					MT_NON_CACHEABLE | MT_RW | MT_SECURE);
 }
 
 int stm32mp_unmap_ddr(void)
diff --git a/plat/st/common/stm32mp_cot.c b/plat/st/common/stm32mp_cot.c
new file mode 100644
index 0000000000..5f673fde78
--- /dev/null
+++ b/plat/st/common/stm32mp_cot.c
@@ -0,0 +1,114 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <platform_def.h>
+
+#include <drivers/auth/auth_mod.h>
+#include <plat/common/platform.h>
+
+static auth_param_type_desc_t stm32_header_pk =
+	AUTH_PARAM_TYPE_DESC(AUTH_PARAM_PUB_KEY, 0);
+static auth_param_type_desc_t stm32_header_sig =
+	AUTH_PARAM_TYPE_DESC(AUTH_PARAM_SIG, 0);
+static auth_param_type_desc_t stm32_header_sig_alg =
+	AUTH_PARAM_TYPE_DESC(AUTH_PARAM_SIG_ALG, 0);
+static auth_param_type_desc_t stm32_load =
+	AUTH_PARAM_TYPE_DESC(AUTH_PARAM_RAW_DATA, 0);
+
+#if defined(AARCH32_SP_OPTEE)
+static const auth_img_desc_t bl32_image = {
+	.img_id = BL32_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &stm32_header_pk,
+				.sig = &stm32_header_sig,
+				.alg = &stm32_header_sig_alg,
+				.data = &stm32_load
+			}
+		},
+	},
+};
+
+static const auth_img_desc_t bl32_extra1_image = {
+	.img_id = BL32_EXTRA1_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &stm32_header_pk,
+				.sig = &stm32_header_sig,
+				.alg = &stm32_header_sig_alg,
+				.data = &stm32_load
+			}
+		},
+	},
+};
+
+static const auth_img_desc_t bl32_extra2_image = {
+	.img_id = BL32_EXTRA2_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &stm32_header_pk,
+				.sig = &stm32_header_sig,
+				.alg = &stm32_header_sig_alg,
+				.data = &stm32_load
+			}
+		},
+	},
+};
+#else
+static const auth_img_desc_t bl32_image = {
+	.img_id = BL32_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_NONE, /* Already verified by BL1
+						   * as loaded in the same time
+						   * as BL2
+						   */
+		}
+	},
+};
+#endif
+
+static const auth_img_desc_t bl33_image = {
+	.img_id = BL33_IMAGE_ID,
+	.img_type = IMG_PLAT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &stm32_header_pk,
+				.sig = &stm32_header_sig,
+				.alg = &stm32_header_sig_alg,
+				.data = &stm32_load
+			}
+		},
+	},
+};
+
+static const auth_img_desc_t * const cot_desc[] = {
+	[BL32_IMAGE_ID] = &bl32_image,
+#if defined(AARCH32_SP_OPTEE)
+	[BL32_EXTRA1_IMAGE_ID] = &bl32_extra1_image,
+	[BL32_EXTRA2_IMAGE_ID] = &bl32_extra2_image,
+#endif
+	[BL33_IMAGE_ID] = &bl33_image,
+};
+
+REGISTER_COT(cot_desc);
diff --git a/plat/st/common/stm32mp_crypto_lib.c b/plat/st/common/stm32mp_crypto_lib.c
new file mode 100644
index 0000000000..de9601d2b8
--- /dev/null
+++ b/plat/st/common/stm32mp_crypto_lib.c
@@ -0,0 +1,447 @@
+/*
+ * Copyright (c) 2020-2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#if !STM32MP_USE_STM32IMAGE
+#include <mbedtls/asn1.h>
+#include <mbedtls/md.h>
+#include <mbedtls/oid.h>
+#include <mbedtls/platform.h>
+#include <mbedtls/x509.h>
+#endif
+
+#include <platform_def.h>
+
+#include <common/debug.h>
+#include <drivers/auth/crypto_mod.h>
+#include <drivers/io/io_storage.h>
+#include <drivers/st/bsec.h>
+#include <drivers/st/stm32_hash.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <plat/common/platform.h>
+
+#define CRYPTO_HASH_MAX_SIZE	32U
+#define CRYPTO_SIGN_MAX_SIZE	64U
+#define CRYPTO_PUBKEY_MAX_SIZE	64U
+
+struct stm32mp_auth_ops {
+	uint32_t (*verify_signature)(uint8_t *hash_in, uint8_t *pubkey_in,
+				     uint8_t *signature, uint32_t ecc_algo);
+};
+
+static struct stm32mp_auth_ops auth_ops;
+
+static void crypto_lib_init(void)
+{
+	boot_api_context_t *boot_context =
+		(boot_api_context_t *)stm32mp_get_boot_ctx_address();
+
+	if (!stm32mp_is_auth_supported()) {
+		return;
+	}
+
+	auth_ops.verify_signature =
+		boot_context->bootrom_ecdsa_verify_signature;
+
+	if (stm32_hash_register() != 0) {
+		panic();
+	}
+}
+
+#if STM32MP_USE_STM32IMAGE
+static int crypto_verify_signature(void *data_ptr, unsigned int data_len,
+				   void *sig_ptr, unsigned int sig_len,
+				   void *sig_alg, unsigned int sig_alg_len,
+				   void *pk_ptr, unsigned int pk_len)
+{
+	uint8_t image_hash[BOOT_API_SHA256_DIGEST_SIZE_IN_BYTES];
+	uint32_t option_flags;
+	uint32_t ecc_algo_type;
+	uint32_t header_len;
+	int result;
+	boot_api_image_header_t *header = stm32mp_get_loaded_header();
+
+	header_len = sizeof(boot_api_image_header_t) - sizeof(header->magic) -
+		     sizeof(header->image_signature) -
+		     sizeof(header->payload_checksum);
+
+	if ((((size_t)sig_alg % __alignof__(uint32_t)) != 0) ||
+	    (sig_alg_len != sizeof(option_flags) + sizeof(ecc_algo_type))) {
+		return -EINVAL;
+	}
+
+	option_flags = ((uint32_t *)sig_alg)[0];
+	ecc_algo_type = ((uint32_t *)sig_alg)[1];
+
+	/* Check security status */
+	if (!stm32mp_is_closed_device()) {
+		if (option_flags != 0U) {
+			WARN("Skip signature check (header option)\n");
+			stm32mp_delete_loaded_header();
+			return 0;
+		}
+		INFO("Check signature on Open device\n");
+	}
+
+	/* Check key/sign size */
+	if ((pk_len != BOOT_API_ECDSA_PUB_KEY_LEN_IN_BYTES) ||
+	    (sig_len != BOOT_API_ECDSA_SIGNATURE_LEN_IN_BYTES)) {
+		return -EINVAL;
+	}
+
+	result = mmap_add_dynamic_region(STM32MP_ROM_BASE, STM32MP_ROM_BASE,
+					 STM32MP_ROM_SIZE_2MB_ALIGNED, MT_CODE | MT_SECURE);
+	if (result != 0) {
+		return result;
+	}
+
+	/* Compute hash for the data covered by the signature */
+	stm32_hash_init(HASH_SHA256);
+
+	result = stm32_hash_update((void *)&header->header_version, header_len);
+	if (result != 0) {
+		VERBOSE("Hash of header failed, %i\n", result);
+		goto out;
+	}
+
+	result = stm32_hash_final_update((uint8_t *)data_ptr,
+					 data_len, image_hash);
+	if (result != 0) {
+		VERBOSE("Hash of payload failed, %i\n", result);
+		goto out;
+	}
+
+	/* Verify signature */
+	if (auth_ops.verify_signature(image_hash, pk_ptr, sig_ptr,
+				      ecc_algo_type) != BOOT_API_RETURN_OK) {
+		result = -EAUTH;
+	}
+
+out:
+	mmap_remove_dynamic_region(STM32MP_ROM_BASE, STM32MP_ROM_SIZE_2MB_ALIGNED);
+	if (result != 0) {
+		stm32mp_delete_loaded_header();
+	}
+
+	return result;
+}
+
+static int crypto_verify_hash(void *data_ptr, unsigned int data_len,
+			      void *digest_info_ptr,
+			      unsigned int digest_info_len)
+{
+	int ret;
+	uint8_t calc_hash[BOOT_API_SHA256_DIGEST_SIZE_IN_BYTES];
+
+	if (digest_info_len != BOOT_API_SHA256_DIGEST_SIZE_IN_BYTES) {
+		VERBOSE("%s: unexpected digest_len\n", __func__);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	stm32_hash_init(HASH_SHA256);
+	ret = stm32_hash_final_update(data_ptr, data_len, calc_hash);
+	if (ret != 0) {
+		VERBOSE("%s: hash failed\n", __func__);
+		goto out;
+	}
+
+	ret = memcmp(calc_hash, digest_info_ptr, digest_info_len);
+	if (ret != 0) {
+		VERBOSE("%s: not expected digest\n", __func__);
+		ret = -EAUTH;
+	}
+
+out:
+	/* Clean header as no more used */
+	stm32mp_delete_loaded_header();
+
+	return ret;
+}
+#else /* STM32MP_USE_STM32IMAGE */
+int get_plain_pk_from_asn1(void *pk_ptr, unsigned int pk_len, void **plain_pk,
+			   unsigned int *len, int *pk_alg)
+{
+	int ret;
+	mbedtls_pk_context mbedtls_pk = {0};
+	unsigned char *p, *end;
+	mbedtls_asn1_buf alg_params = {0};
+	mbedtls_asn1_buf alg_oid = {0};
+
+	*plain_pk = NULL;
+	*len = 0U;
+
+	/* Parse the public key */
+	mbedtls_pk_init(&mbedtls_pk);
+	p = (unsigned char *)pk_ptr;
+	end = (unsigned char *)(p + pk_len);
+
+	ret =  mbedtls_asn1_get_tag(&p, end, len,
+				    MBEDTLS_ASN1_CONSTRUCTED | MBEDTLS_ASN1_SEQUENCE);
+	if (ret != 0) {
+		return -EINVAL;
+	}
+
+	end = p + *len;
+	ret = mbedtls_asn1_get_alg(&p, end, &alg_oid, &alg_params);
+	if (ret != 0) {
+		VERBOSE("%s: mbedtls_asn1_get_alg (%d)\n", __func__, ret);
+		return -EINVAL;
+	}
+
+	if (pk_alg != NULL) {
+		if ((strlen(MBEDTLS_OID_EC_GRP_SECP256R1) == alg_params.len) &&
+		    (memcmp(MBEDTLS_OID_EC_GRP_SECP256R1, alg_params.p, alg_params.len) == 0)) {
+			*pk_alg = BOOT_API_ECDSA_ALGO_TYPE_P256NIST;
+		} else {
+			*pk_alg = BOOT_API_ECDSA_ALGO_TYPE_BRAINPOOL256;
+		}
+	}
+
+	ret = mbedtls_asn1_get_bitstring_null(&p, end, len);
+	if (ret != 0) {
+		VERBOSE("%s: mbedtls_asn1_get_bitstring_null (%d)\n", __func__, ret);
+		return -EINVAL;
+	}
+
+	/* we remove the ident (0x04) first byte.  */
+	if ((*len < 1U) || (p[0] !=  MBEDTLS_ASN1_OCTET_STRING)) {
+		VERBOSE("%s: not expected len or tag\n", __func__);
+		return -EINVAL;
+	}
+
+	*len = *len - 1U;
+	*plain_pk = p + 1U;
+
+	return 0;
+}
+
+int plat_get_hashed_pk(void *full_pk_ptr, unsigned int full_pk_len,
+		       void **hashed_pk_ptr, unsigned int *hashed_pk_len)
+{
+	return get_plain_pk_from_asn1(full_pk_ptr, full_pk_len, hashed_pk_ptr, hashed_pk_len, NULL);
+}
+
+static int get_plain_digest_from_asn1(void *digest_ptr, unsigned int digest_len,
+				      uint8_t **out, size_t *out_len, mbedtls_md_type_t *md_alg)
+{
+	int ret;
+	mbedtls_asn1_buf hash_oid, params;
+	size_t len;
+	unsigned char *p, *end;
+
+	*out = NULL;
+	*out_len = 0U;
+
+	/* Digest info should be an MBEDTLS_ASN1_SEQUENCE */
+	p = (unsigned char *)digest_ptr;
+	end = p + digest_len;
+	ret = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
+				   MBEDTLS_ASN1_SEQUENCE);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* Get the hash algorithm */
+	ret = mbedtls_asn1_get_alg(&p, end, &hash_oid, &params);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = mbedtls_oid_get_md_alg(&hash_oid, md_alg);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_OCTET_STRING);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* Length of hash must match the algorithm's size */
+	if (len != BOOT_API_SHA256_DIGEST_SIZE_IN_BYTES) {
+		return -1;
+	}
+
+	*out = p;
+	*out_len = len;
+
+	return 0;
+}
+
+static int crypto_verify_signature(void *data_ptr, unsigned int data_len,
+				   void *sig_ptr, unsigned int sig_len,
+				   void *sig_alg, unsigned int sig_alg_len,
+				   void *pk_ptr, unsigned int pk_len)
+{
+	uint8_t image_hash[CRYPTO_HASH_MAX_SIZE];
+	uint8_t sig[CRYPTO_SIGN_MAX_SIZE];
+	uint8_t my_pk[CRYPTO_PUBKEY_MAX_SIZE];
+	int ret;
+	size_t len;
+	mbedtls_asn1_sequence seq;
+	unsigned char *p, *end;
+	int curve_id;
+	mbedtls_asn1_buf sig_oid, sig_params;
+	mbedtls_md_type_t md_alg;
+	mbedtls_pk_type_t pk_alg;
+
+	/* Get pointers to signature OID and parameters */
+	p = (unsigned char *)sig_alg;
+	end = (unsigned char *)(p + sig_alg_len);
+	ret = mbedtls_asn1_get_alg(&p, end, &sig_oid, &sig_params);
+	if (ret != 0) {
+		VERBOSE("%s: mbedtls_asn1_get_alg (%d)\n", __func__, ret);
+		return CRYPTO_ERR_SIGNATURE;
+	}
+
+	/* Get the actual signature algorithm (MD + PK) */
+	ret = mbedtls_oid_get_sig_alg(&sig_oid, &md_alg, &pk_alg);
+	if (ret != 0) {
+		VERBOSE("%s: mbedtls_oid_get_sig_alg (%d)\n", __func__, ret);
+		return CRYPTO_ERR_SIGNATURE;
+	}
+
+	if ((md_alg != MBEDTLS_MD_SHA256) || (pk_alg != MBEDTLS_PK_ECDSA)) {
+		VERBOSE("%s: md_alg=%d pk_alg=%d\n", __func__, md_alg, pk_alg);
+		return CRYPTO_ERR_SIGNATURE;
+	}
+
+	ret = get_plain_pk_from_asn1(pk_ptr, pk_len, &pk_ptr, &pk_len, &curve_id);
+	if (ret != 0) {
+		VERBOSE("%s: get_plain_pk_from_asn1 (%d)\n", __func__, ret);
+		return CRYPTO_ERR_SIGNATURE;
+	}
+
+	/* We expect a known pk_len */
+	if (pk_len != sizeof(my_pk)) {
+		VERBOSE("%s: pk_len=%d sizeof(my_pk)=%d)\n", __func__, pk_len, sizeof(my_pk));
+		return CRYPTO_ERR_SIGNATURE;
+	}
+
+	/* Need to copy as auth_ops.verify_signature
+	 * expects aligned public key.
+	 */
+	memcpy(my_pk, pk_ptr, sizeof(my_pk));
+
+	/* Get the signature (bitstring) */
+	p = (unsigned char *)sig_ptr;
+	end = (unsigned char *)(p + sig_len);
+	ret = mbedtls_asn1_get_bitstring_null(&p, end, &len);
+	if (ret != 0) {
+		VERBOSE("%s: mbedtls_asn1_get_bitstring_null (%d)\n", __func__, ret);
+		return CRYPTO_ERR_SIGNATURE;
+	}
+
+	/* Get r and s from sequence */
+	ret = mbedtls_asn1_get_sequence_of(&p, end, &seq, MBEDTLS_ASN1_INTEGER);
+	if (ret != 0) {
+		VERBOSE("%s: mbedtls_asn1_get_sequence_of (%d)\n", __func__, ret);
+		return CRYPTO_ERR_SIGNATURE;
+	}
+
+	/* We expect only 2 integers (r and s) from the sequence */
+	if (seq.next->next != NULL) {
+		mbedtls_asn1_sequence *cur = seq.next;
+		mbedtls_asn1_sequence *next;
+
+		VERBOSE("%s: nb seq != 2\n", __func__);
+		/* Free all the sequences */
+		while (cur != NULL) {
+			next = cur->next;
+			mbedtls_free(cur);
+			cur = next;
+		}
+
+		return CRYPTO_ERR_SIGNATURE;
+	}
+
+	/* Integer sequence may (sometime) start with 0x00 as MSB, but we can only
+	 * manage exactly 2*32 bytes, we remove this higher byte
+	 * if there are not 00, we will fail either.
+	 */
+	memcpy(sig, seq.buf.p + seq.buf.len - sizeof(sig) / 2U, sizeof(sig) / 2U);
+	memcpy(sig +  sizeof(sig) / 2U,
+	       seq.next->buf.p + seq.next->buf.len - sizeof(sig) / 2U,
+	       sizeof(sig) / 2U);
+	/* Need to free allocated 'next' in mbedtls_asn1_get_sequence_of */
+	mbedtls_free(seq.next);
+
+	/* Compute hash for the data covered by the signature */
+	stm32_hash_init(HASH_SHA256);
+
+	ret = stm32_hash_final_update((uint8_t *)data_ptr, data_len, image_hash);
+	if (ret != 0) {
+		VERBOSE("%s: stm32_hash_final_update (%d)\n", __func__, ret);
+		return CRYPTO_ERR_SIGNATURE;
+	}
+
+	ret = mmap_add_dynamic_region(STM32MP_ROM_BASE, STM32MP_ROM_BASE,
+				      STM32MP_ROM_SIZE_2MB_ALIGNED, MT_CODE | MT_SECURE);
+	if (ret != 0) {
+		VERBOSE("%s: mmap_add_dynamic_region (%d)\n", __func__, ret);
+		return CRYPTO_ERR_SIGNATURE;
+	}
+
+	ret = auth_ops.verify_signature(image_hash, my_pk, sig, curve_id);
+	if (ret != BOOT_API_RETURN_OK) {
+		VERBOSE("%s: auth_ops.verify_signature (%d)\n", __func__, ret);
+		ret = CRYPTO_ERR_SIGNATURE;
+	} else {
+		ret = 0;
+	}
+
+	mmap_remove_dynamic_region(STM32MP_ROM_BASE, STM32MP_ROM_SIZE_2MB_ALIGNED);
+
+	return ret;
+}
+
+static int crypto_verify_hash(void *data_ptr, unsigned int data_len,
+			      void *digest_info_ptr,
+			      unsigned int digest_info_len)
+{
+	int ret;
+	uint8_t calc_hash[BOOT_API_SHA256_DIGEST_SIZE_IN_BYTES];
+	unsigned char *p;
+	mbedtls_md_type_t md_alg;
+	size_t len;
+
+	/* we receive an asn1 encapsulated digest, we flatten it */
+	ret = get_plain_digest_from_asn1(digest_info_ptr,
+					 digest_info_len, &p, &len,
+					 &md_alg);
+	if ((ret != 0) || (md_alg != MBEDTLS_MD_SHA256)) {
+		return CRYPTO_ERR_HASH;
+	}
+
+	digest_info_ptr = p;
+	digest_info_len = len;
+
+	stm32_hash_init(HASH_SHA256);
+
+	ret = stm32_hash_final_update(data_ptr, data_len, calc_hash);
+	if (ret != 0) {
+		VERBOSE("%s: hash failed\n", __func__);
+		return CRYPTO_ERR_HASH;
+	}
+
+	ret = memcmp(calc_hash, digest_info_ptr, digest_info_len);
+	if (ret != 0) {
+		VERBOSE("%s: not expected digest\n", __func__);
+		ret = CRYPTO_ERR_HASH;
+	}
+
+	return ret;
+}
+#endif
+
+REGISTER_CRYPTO_LIB("stm32_crypto_lib",
+		    crypto_lib_init,
+		    crypto_verify_signature,
+		    crypto_verify_hash,
+		    NULL);
diff --git a/plat/st/common/stm32mp_dt.c b/plat/st/common/stm32mp_dt.c
index 391e5f0547..a3584cbc12 100644
--- a/plat/st/common/stm32mp_dt.c
+++ b/plat/st/common/stm32mp_dt.c
@@ -14,25 +14,22 @@
 #include <common/debug.h>
 #include <common/fdt_wrappers.h>
 #include <drivers/st/stm32_gpio.h>
-#include <drivers/st/stm32mp1_ddr.h>
-#include <drivers/st/stm32mp1_ram.h>
 
 #include <stm32mp_dt.h>
 
-static int fdt_checked;
-
-static void *fdt = (void *)(uintptr_t)STM32MP_DTB_BASE;
+static void *fdt;
 
 /*******************************************************************************
  * This function checks device tree file with its header.
  * Returns 0 on success and a negative FDT error code on failure.
  ******************************************************************************/
-int dt_open_and_check(void)
+int dt_open_and_check(uintptr_t dt_addr)
 {
-	int ret = fdt_check_header(fdt);
+	int ret;
 
+	ret = fdt_check_header((void *)dt_addr);
 	if (ret == 0) {
-		fdt_checked = 1;
+		fdt = (void *)dt_addr;
 	}
 
 	return ret;
@@ -45,11 +42,13 @@ int dt_open_and_check(void)
  ******************************************************************************/
 int fdt_get_address(void **fdt_addr)
 {
-	if (fdt_checked == 1) {
-		*fdt_addr = fdt;
+	if (fdt == NULL) {
+		return 0;
 	}
 
-	return fdt_checked;
+	*fdt_addr = fdt;
+
+	return 1;
 }
 
 /*******************************************************************************
@@ -115,6 +114,37 @@ static int fdt_get_node_parent_address_cells(int node)
 }
 #endif
 
+/*******************************************************************************
+ * This function return interrupts from node.
+ ******************************************************************************/
+int fdt_get_interrupt(int node, const fdt32_t **array, int *len, bool *extended)
+{
+	uint8_t status = fdt_get_status(node);
+
+	*extended = false;
+
+	switch (status) {
+	case DT_SECURE:
+		*array = fdt_getprop(fdt, node, "interrupts-extended", len);
+		if (*array == NULL) {
+			*array = fdt_getprop(fdt, node, "interrupts", len);
+		} else {
+			*extended = true;
+		}
+		break;
+
+	default:
+		*array = fdt_getprop(fdt, node, "secure-interrupts", len);
+		break;
+	}
+
+	if (*array == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	return 0;
+}
+
 /*******************************************************************************
  * This function gets the stdout pin configuration information from the DT.
  * And then calls the sub-function to treat it and set GPIO registers.
@@ -203,21 +233,223 @@ int dt_get_stdout_uart_info(struct dt_node_info *info)
 	return node;
 }
 
+/*******************************************************************************
+ * This function returns the node offset matching compatible string in the DT,
+ * and also matching the reg property with the given address.
+ * Returns value on success, and error value on failure.
+ ******************************************************************************/
+int dt_match_instance_by_compatible(const char *compatible, uintptr_t address)
+{
+	int node;
+
+	for (node = fdt_node_offset_by_compatible(fdt, -1, compatible);
+	     node != -FDT_ERR_NOTFOUND;
+	     node = fdt_node_offset_by_compatible(fdt, node, compatible)) {
+		const fdt32_t *cuint;
+
+		assert(fdt_get_node_parent_address_cells(node) == 1);
+
+		cuint = fdt_getprop(fdt, node, "reg", NULL);
+		if (cuint == NULL) {
+			continue;
+		}
+
+		if ((uintptr_t)fdt32_to_cpu(*cuint) == address) {
+			return node;
+		}
+	}
+
+	return -FDT_ERR_NOTFOUND;
+}
+
 /*******************************************************************************
  * This function gets DDR size information from the DT.
  * Returns value in bytes on success, and 0 on failure.
  ******************************************************************************/
 uint32_t dt_get_ddr_size(void)
 {
+	static uint32_t size;
 	int node;
 
+	if (size != 0U) {
+		return size;
+	}
+
 	node = fdt_node_offset_by_compatible(fdt, -1, DT_DDR_COMPAT);
 	if (node < 0) {
-		INFO("%s: Cannot read DDR node in DT\n", __func__);
 		return 0;
 	}
 
-	return fdt_read_uint32_default(fdt, node, "st,mem-size", 0);
+	size = fdt_read_uint32_default(fdt, node, "st,mem-size", 0U);
+
+	flush_dcache_range((uintptr_t)&size, sizeof(uint32_t));
+
+	return size;
+}
+
+/*******************************************************************************
+ * This function gets OPP table node from the DT.
+ * Returns node offset on success and a negative FDT error code on failure.
+ ******************************************************************************/
+static int dt_get_opp_table_node(void)
+{
+	return fdt_node_offset_by_compatible(fdt, -1, DT_OPP_COMPAT);
+}
+
+/*******************************************************************************
+ * This function gets OPP parameters (frequency in KHz and voltage in mV) from
+ * an OPP table subnode. Platform HW support capabilities are also checked.
+ * Returns 0 on success and a negative FDT error code on failure.
+ ******************************************************************************/
+static int dt_get_opp_freqvolt_from_subnode(int subnode, uint32_t *freq_khz,
+					    uint32_t *voltage_mv)
+{
+	const fdt64_t *cuint64;
+	const fdt32_t *cuint32;
+	uint64_t read_freq_64;
+	uint32_t read_voltage_32;
+
+	assert(freq_khz != NULL);
+	assert(voltage_mv != NULL);
+
+	cuint32 = fdt_getprop(fdt, subnode, "opp-supported-hw", NULL);
+	if (cuint32 != NULL) {
+		if (!stm32mp_supports_cpu_opp(fdt32_to_cpu(*cuint32))) {
+			VERBOSE("Invalid opp-supported-hw 0x%x\n",
+				fdt32_to_cpu(*cuint32));
+			return -FDT_ERR_BADVALUE;
+		}
+	}
+
+	cuint64 = fdt_getprop(fdt, subnode, "opp-hz", NULL);
+	if (cuint64 == NULL) {
+		VERBOSE("Missing opp-hz\n");
+		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) {
+		VERBOSE("Invalid opp-hz %llu\n", read_freq_64);
+		return -FDT_ERR_BADVALUE;
+	}
+
+	cuint32 = fdt_getprop(fdt, subnode, "opp-microvolt", NULL);
+	if (cuint32 == NULL) {
+		VERBOSE("Missing opp-microvolt\n");
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	/* Millivolt value must fit on 16 bits */
+	read_voltage_32 = fdt32_to_cpu(*cuint32) / 1000U;
+	if (read_voltage_32 > (uint32_t)UINT16_MAX) {
+		VERBOSE("Invalid opp-microvolt %u\n", read_voltage_32);
+		return -FDT_ERR_BADVALUE;
+	}
+
+	*freq_khz = (uint32_t)read_freq_64;
+
+	*voltage_mv = read_voltage_32;
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function parses OPP table in DT and finds the parameters for the
+ * highest frequency supported by the HW platform.
+ * If found, the new frequency and voltage values override the original ones.
+ * Returns 0 on success and a negative FDT error code on failure.
+ ******************************************************************************/
+int dt_get_max_opp_freqvolt(uint32_t *freq_khz, uint32_t *voltage_mv)
+{
+	int node;
+	int subnode;
+	uint32_t freq = 0U;
+	uint32_t voltage = 0U;
+
+	assert(freq_khz != NULL);
+	assert(voltage_mv != NULL);
+
+	node = dt_get_opp_table_node();
+	if (node < 0) {
+		return node;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		uint32_t read_freq;
+		uint32_t read_voltage;
+
+		if (dt_get_opp_freqvolt_from_subnode(subnode, &read_freq,
+						     &read_voltage) != 0) {
+			continue;
+		}
+
+		if (read_freq > freq) {
+			freq = read_freq;
+			voltage = read_voltage;
+		}
+	}
+
+	if ((freq == 0U) || (voltage == 0U)) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	*freq_khz = freq;
+	*voltage_mv = voltage;
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function 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 @*freq_khz_array and @*voltage_mv_array.
+ * 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.
+ * Returns 0 on success and a negative FDT error code on failure.
+ ******************************************************************************/
+int dt_get_all_opp_freqvolt(uint32_t *count, uint32_t *freq_khz_array,
+			    uint32_t *voltage_mv_array)
+{
+	int node;
+	int subnode;
+	uint32_t idx = 0U;
+
+	assert(count != NULL);
+	assert(freq_khz_array != NULL);
+	assert(voltage_mv_array != NULL);
+
+	node = dt_get_opp_table_node();
+	if (node < 0) {
+		return node;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		uint32_t read_freq;
+		uint32_t read_voltage;
+
+		if (dt_get_opp_freqvolt_from_subnode(subnode, &read_freq,
+						     &read_voltage) != 0) {
+			continue;
+		}
+
+		if (idx >= *count) {
+			return -FDT_ERR_NOSPACE;
+		}
+
+		freq_khz_array[idx] = read_freq;
+		voltage_mv_array[idx] = read_voltage;
+		idx++;
+	}
+
+	if (idx == 0U) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	*count = idx;
+
+	return 0;
 }
 
 /*******************************************************************************
@@ -226,22 +458,15 @@ uint32_t dt_get_ddr_size(void)
  ******************************************************************************/
 uint32_t dt_get_pwr_vdd_voltage(void)
 {
-	int node, pwr_regulators_node;
+	int node;
 	const fdt32_t *cuint;
 
 	node = fdt_node_offset_by_compatible(fdt, -1, DT_PWR_COMPAT);
 	if (node < 0) {
-		INFO("%s: Cannot read PWR node in DT\n", __func__);
 		return 0;
 	}
 
-	pwr_regulators_node = fdt_subnode_offset(fdt, node, "pwr-regulators");
-	if (pwr_regulators_node < 0) {
-		INFO("%s: Cannot read pwr-regulators node in DT\n", __func__);
-		return 0;
-	}
-
-	cuint = fdt_getprop(fdt, pwr_regulators_node, "vdd-supply", NULL);
+	cuint = fdt_getprop(fdt, node, "vdd-supply", NULL);
 	if (cuint == NULL) {
 		return 0;
 	}
@@ -259,6 +484,84 @@ uint32_t dt_get_pwr_vdd_voltage(void)
 	return fdt32_to_cpu(*cuint);
 }
 
+/*******************************************************************************
+ * This function return the real regulator name from DT.
+ ******************************************************************************/
+static const char *dt_get_regulator_name(int node, const char *regu_name)
+{
+	const fdt32_t *cuint;
+
+	if ((node < 0) || (regu_name == NULL)) {
+		return NULL;
+	}
+
+	cuint = fdt_getprop(fdt, node, regu_name, NULL);
+	if (cuint == NULL) {
+		return NULL;
+	}
+
+	node = fdt_node_offset_by_phandle(fdt, fdt32_to_cpu(*cuint));
+	if (node < 0) {
+		return NULL;
+	}
+
+	return (const char *)fdt_getprop(fdt, node, "regulator-name", NULL);
+}
+
+/*******************************************************************************
+ * This function retrieves VDD regulator name from DT.
+ * Returns string taken from supply node, NULL otherwise.
+ ******************************************************************************/
+const char *dt_get_vdd_regulator_name(void)
+{
+	int node = fdt_node_offset_by_compatible(fdt, -1, DT_PWR_COMPAT);
+
+	if (node < 0) {
+		return NULL;
+	}
+
+	return dt_get_regulator_name(node, "vdd-supply");
+}
+
+/*******************************************************************************
+ * This function retrieves CPU regulator name from DT.
+ * Returns string taken from supply node, NULL otherwise.
+ ******************************************************************************/
+const char *dt_get_cpu_regulator_name(void)
+{
+	int node = fdt_path_offset(fdt, "/cpus/cpu@0");
+
+	if (node < 0) {
+		return NULL;
+	}
+
+	return dt_get_regulator_name(node, "cpu-supply");
+}
+
+/*******************************************************************************
+ * This function retrieves USB phy regulator name from DT.
+ * Returns string taken from supply node, NULL otherwise.
+ ******************************************************************************/
+const char *dt_get_usb_phy_regulator_name(void)
+{
+	int node = fdt_node_offset_by_compatible(fdt, -1, DT_USBPHYC_COMPAT);
+	int subnode;
+	const char *reg_name = NULL;
+
+	if (node < 0) {
+		return NULL;
+	}
+
+	fdt_for_each_subnode(subnode, fdt, node) {
+		reg_name = dt_get_regulator_name(subnode, "phy-supply");
+		if (reg_name != NULL) {
+			return reg_name;
+		}
+	}
+
+	return NULL;
+}
+
 /*******************************************************************************
  * This function retrieves board model from DT
  * Returns string taken from model node, NULL otherwise
diff --git a/plat/st/common/stm32mp_fconf_io.c b/plat/st/common/stm32mp_fconf_io.c
new file mode 100644
index 0000000000..d07067531f
--- /dev/null
+++ b/plat/st/common/stm32mp_fconf_io.c
@@ -0,0 +1,136 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/io/io_storage.h>
+#include <drivers/mmc.h>
+#include <lib/fconf/fconf.h>
+#include <lib/object_pool.h>
+#include <libfdt.h>
+#include <tools_share/firmware_image_package.h>
+
+#include <platform_def.h>
+
+#include <stm32mp_io_storage.h>
+#include <stm32mp_fconf_getter.h>
+
+#if STM32MP_SDMMC || STM32MP_EMMC
+static io_block_spec_t gpt_block_spec = {
+	.offset = 0,
+	.length = 34 * MMC_BLOCK_SIZE, /* Size of GPT table */
+};
+#endif
+
+/* By default, STM32 platforms load images from the FIP */
+struct plat_io_policy policies[MAX_NUMBER_IDS] = {
+	[FIP_IMAGE_ID] = {
+		&storage_dev_handle,
+		(uintptr_t)&image_block_spec,
+		open_storage
+	},
+#if STM32MP_SDMMC || STM32MP_EMMC
+	[GPT_IMAGE_ID] = {
+		&storage_dev_handle,
+		(uintptr_t)&gpt_block_spec,
+		open_storage
+	},
+#endif
+};
+
+#if TRUSTED_BOARD_BOOT
+#define FCONF_ST_IO_UUID_NUMBER	U(14)
+#else
+#define FCONF_ST_IO_UUID_NUMBER	U(8)
+#endif
+
+static io_uuid_spec_t fconf_stm32mp_uuids[FCONF_ST_IO_UUID_NUMBER];
+static OBJECT_POOL_ARRAY(fconf_stm32mp_uuids_pool, fconf_stm32mp_uuids);
+
+struct policies_load_info {
+	unsigned int image_id;
+	const char *name;
+};
+
+/* image id to property name table */
+static const struct policies_load_info load_info[FCONF_ST_IO_UUID_NUMBER] = {
+	{FW_CONFIG_ID, "fw_cfg_uuid"},
+	{BL32_IMAGE_ID, "bl32_uuid"},
+	{BL32_EXTRA1_IMAGE_ID, "bl32_extra1_uuid"},
+	{BL32_EXTRA2_IMAGE_ID, "bl32_extra2_uuid"},
+	{BL33_IMAGE_ID, "bl33_uuid"},
+	{HW_CONFIG_ID, "hw_cfg_uuid"},
+	{TOS_FW_CONFIG_ID, "tos_fw_cfg_uuid"},
+	{NT_FW_CONFIG_ID, "nt_fw_cfg_uuid"},
+#if TRUSTED_BOARD_BOOT
+	{TRUSTED_BOOT_FW_CERT_ID, "t_boot_fw_cert_uuid"},
+	{TRUSTED_KEY_CERT_ID, "t_key_cert_uuid"},
+	{TRUSTED_OS_FW_KEY_CERT_ID, "tos_fw_key_cert_uuid"},
+	{NON_TRUSTED_FW_KEY_CERT_ID, "nt_fw_key_cert_uuid"},
+	{TRUSTED_OS_FW_CONTENT_CERT_ID, "tos_fw_content_cert_uuid"},
+	{NON_TRUSTED_FW_CONTENT_CERT_ID, "nt_fw_content_cert_uuid"},
+#endif /* TRUSTED_BOARD_BOOT */
+};
+
+int fconf_populate_stm32mp_io_policies(uintptr_t config)
+{
+	int node;
+	unsigned int i;
+
+	/* As libfdt uses void *, we can't avoid this cast */
+	const void *dtb = (void *)config;
+
+	/* Assert the node offset point to "st,io-fip-handle" compatible property */
+	const char *compatible_str = "st,io-fip-handle";
+
+	node = fdt_node_offset_by_compatible(dtb, -1, compatible_str);
+	if (node < 0) {
+		ERROR("FCONF: Can't find %s compatible in dtb\n", compatible_str);
+		return node;
+	}
+
+	/* Locate the uuid cells and read the value for all the load info uuid */
+	for (i = 0U; i < FCONF_ST_IO_UUID_NUMBER; i++) {
+		union uuid_helper_t uuid_helper;
+		io_uuid_spec_t *uuid_ptr;
+		int err;
+		unsigned int j;
+
+		uuid_ptr = pool_alloc(&fconf_stm32mp_uuids_pool);
+		err = fdt_read_uint32_array(dtb, node, load_info[i].name,
+					    4, uuid_helper.word);
+		if (err < 0) {
+			WARN("FCONF: Read cell failed for %s\n", load_info[i].name);
+			return err;
+		}
+
+		/* Convert uuid from big endian to little endian */
+		for (j = 0U; j < 4U; j++) {
+			uuid_helper.word[j] =
+				((uuid_helper.word[j] >> 24U) & 0xff) |
+				((uuid_helper.word[j] << 8U) & 0xff0000) |
+				((uuid_helper.word[j] >> 8U) & 0xff00) |
+				((uuid_helper.word[j] << 24U) & 0xff000000);
+		}
+
+		VERBOSE("FCONF: stm32mp-io_policies.%s found with value = 0x%x 0x%x 0x%x 0x%x\n",
+			load_info[i].name,
+			uuid_helper.word[0], uuid_helper.word[1],
+			uuid_helper.word[2], uuid_helper.word[3]);
+
+		uuid_ptr->uuid = uuid_helper.uuid_struct;
+		policies[load_info[i].image_id].image_spec = (uintptr_t)uuid_ptr;
+		policies[load_info[i].image_id].dev_handle = &fip_dev_handle;
+		policies[load_info[i].image_id].check = open_fip;
+	}
+
+	return 0;
+}
+
+FCONF_REGISTER_POPULATOR(TB_FW, stm32mp_io, fconf_populate_stm32mp_io_policies);
diff --git a/plat/st/common/stm32mp_img_parser_lib.c b/plat/st/common/stm32mp_img_parser_lib.c
new file mode 100644
index 0000000000..f4c8bd642f
--- /dev/null
+++ b/plat/st/common/stm32mp_img_parser_lib.c
@@ -0,0 +1,75 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+
+#include <platform_def.h>
+
+#include <common/debug.h>
+#include <drivers/auth/img_parser_mod.h>
+#include <plat/common/platform.h>
+
+static void img_lib_init(void)
+{
+}
+
+static int img_check_integrity(void *img, unsigned int img_len)
+{
+	return stm32mp_check_header(stm32mp_get_loaded_header(),
+				    (uintptr_t)img);
+}
+
+static int img_get_auth_param(const auth_param_type_desc_t *type_desc,
+			      void *img, unsigned int img_len, void **param,
+			      unsigned int *param_len)
+{
+	boot_api_image_header_t *image_header = stm32mp_get_loaded_header();
+
+	switch (type_desc->type) {
+	case AUTH_PARAM_SIG:
+		*param_len = sizeof(image_header->image_signature);
+		*param = &image_header->image_signature;
+		break;
+	case AUTH_PARAM_SIG_ALG:
+		*param_len = sizeof(image_header->option_flags) +
+			sizeof(image_header->ecc_algo_type);
+		*param = &image_header->option_flags;
+		/*
+		 * Store option_flags and ecc_alog_type in same param
+		 * structure because they both have the same header fields.
+		 */
+		break;
+	case AUTH_PARAM_PUB_KEY:
+		*param_len = sizeof(image_header->ecc_pubk);
+		*param = &image_header->ecc_pubk;
+		break;
+	case AUTH_PARAM_RAW_DATA:
+		if (type_desc->cookie == NULL) {
+			*param_len = image_header->image_length;
+			*param = img;
+		} else {
+			return -EINVAL;
+		}
+		break;
+	case AUTH_PARAM_NV_CTR:
+		if (type_desc->cookie == NULL) {
+			*param_len = sizeof(image_header->image_version);
+			*param = &image_header->image_version;
+		} else {
+			return -EINVAL;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+REGISTER_IMG_PARSER_LIB(IMG_PLAT, "stm32_img_parser_lib",
+			img_lib_init,
+			img_check_integrity,
+			img_get_auth_param);
diff --git a/plat/st/common/stm32mp_shres_helpers.c b/plat/st/common/stm32mp_shres_helpers.c
new file mode 100644
index 0000000000..12633e47d2
--- /dev/null
+++ b/plat/st/common/stm32mp_shres_helpers.c
@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) 2018-2019, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <platform_def.h>
+
+#include <arch_helpers.h>
+#include <lib/mmio.h>
+#include <lib/spinlock.h>
+
+#include <stm32mp_shres_helpers.h>
+
+static struct spinlock shregs_lock;
+
+void stm32mp_lock_shregs(void)
+{
+	if (stm32mp_lock_available() == 0U) {
+		return;
+	}
+
+	/* Assume interrupts are masked */
+	spin_lock(&shregs_lock);
+}
+
+void stm32mp_unlock_shregs(void)
+{
+	if (stm32mp_lock_available() == 0U) {
+		return;
+	}
+
+	spin_unlock(&shregs_lock);
+}
+
+/* Shared register access: upon shared resource lock */
+void stm32mp_mmio_clrsetbits_32_shregs(uintptr_t addr, uint32_t clear,
+				       uint32_t set)
+{
+	stm32mp_lock_shregs();
+
+	mmio_clrsetbits_32(addr, clear, set);
+
+	stm32mp_unlock_shregs();
+}
+
+void stm32mp_mmio_clrbits_32_shregs(uintptr_t addr, uint32_t clear)
+{
+	stm32mp_lock_shregs();
+
+	mmio_clrbits_32(addr, clear);
+
+	stm32mp_unlock_shregs();
+}
+
+void stm32mp_mmio_setbits_32_shregs(uintptr_t addr, uint32_t set)
+{
+	stm32mp_lock_shregs();
+
+	mmio_setbits_32(addr, set);
+
+	stm32mp_unlock_shregs();
+}
diff --git a/plat/st/common/stm32mp_trusted_boot.c b/plat/st/common/stm32mp_trusted_boot.c
new file mode 100644
index 0000000000..1ea30abf9b
--- /dev/null
+++ b/plat/st/common/stm32mp_trusted_boot.c
@@ -0,0 +1,143 @@
+/*
+ * Copyright (c) 2020-2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <endian.h>
+#include <errno.h>
+#include <limits.h>
+
+#include <platform_def.h>
+
+#include <common/debug.h>
+#include <common/tbbr/cot_def.h>
+#include <lib/fconf/fconf.h>
+#include <lib/fconf/fconf_dyn_cfg_getter.h>
+#include <lib/fconf/fconf_tbbr_getter.h>
+#include <plat/common/platform.h>
+
+#if STM32MP_USE_STM32IMAGE
+static uint8_t root_pk_hash[BOOT_API_SHA256_DIGEST_SIZE_IN_BYTES];
+#else
+static uint8_t der_sha256_header[] = {0x30, 0x31, 0x30, 0x0d, 0x06, 0x09, 0x60,
+	0x86, 0x48, 0x01, 0x65, 0x03, 0x04, 0x02, 0x01, 0x05, 0x00, 0x04, 0x20};
+static uint8_t root_pk_hash[HASH_DER_LEN];
+#endif
+
+int plat_get_rotpk_info(void *cookie, void **key_ptr, unsigned int *key_len,
+			unsigned int *flags)
+{
+	uint32_t otp_idx;
+	uint32_t otp_val;
+	uint32_t len;
+	size_t i;
+	size_t start_copy_idx = 0U;
+
+	if (cookie != NULL) {
+		return -EINVAL;
+	}
+
+	if (stm32_get_otp_index(PKH_OTP, &otp_idx, &len) != 0) {
+		VERBOSE("get_rot_pk_hash: get index error\n");
+		return -EINVAL;
+	}
+	if (len != (CHAR_BIT * BOOT_API_SHA256_DIGEST_SIZE_IN_BYTES)) {
+		VERBOSE("get_rot_pk_hash: length Error\n");
+		return -EINVAL;
+	}
+
+#if !STM32MP_USE_STM32IMAGE
+	memcpy(root_pk_hash, der_sha256_header, sizeof(der_sha256_header));
+	start_copy_idx = sizeof(der_sha256_header);
+#endif
+
+	for (i = 0U; i < BOOT_API_SHA256_DIGEST_SIZE_IN_BYTES / sizeof(uint32_t); i++) {
+		uint32_t temp;
+
+		if (stm32_get_otp_value_from_idx(otp_idx + i, &otp_val) != 0) {
+			return -EINVAL;
+		}
+
+		temp = bswap32(otp_val);
+		memcpy(root_pk_hash + i * sizeof(uint32_t) + start_copy_idx, &temp, sizeof(temp));
+	}
+
+#if STM32MP_USE_STM32IMAGE
+	*key_len = BOOT_API_SHA256_DIGEST_SIZE_IN_BYTES;
+#else
+	*key_len = HASH_DER_LEN;
+#endif
+	*key_ptr = &root_pk_hash;
+	*flags = ROTPK_IS_HASH;
+
+	if (!stm32mp_is_closed_device()) {
+		/* Check if key hash values in OTP are 0 or 0xFFFFFFFFF programmed : Invalid Key */
+		uint32_t res;
+		uint32_t rootpk;
+		uint8_t *proot_pk = root_pk_hash;
+		uint8_t idx = sizeof(uint32_t);
+
+#if !STM32MP_USE_STM32IMAGE
+		idx += sizeof(der_sha256_header);
+		proot_pk = root_pk_hash + sizeof(der_sha256_header);
+#endif
+		memcpy(&res, proot_pk, sizeof(uint32_t));
+		if ((res == 0U) || (res == 0xFFFFFFFFU)) {
+			while (idx < ARRAY_SIZE(root_pk_hash)) {
+				memcpy(&rootpk, (proot_pk + idx), sizeof(uint32_t));
+				if (res != rootpk) {
+					return 0;
+				}
+
+				idx += sizeof(uint32_t);
+			}
+
+			*flags |= ROTPK_NOT_DEPLOYED;
+		}
+	}
+
+	return 0;
+}
+
+int plat_get_nv_ctr(void *cookie, unsigned int *nv_ctr)
+{
+#if STM32MP_USE_STM32IMAGE
+	if (cookie != NULL) {
+		return -EINVAL;
+	}
+#endif
+
+	/*
+	 * This monotonic counter is the counter used by ROM code
+	 * to identify BL2.
+	 */
+	if (stm32_get_otp_value(MONOTONIC_OTP, nv_ctr) == 0) {
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+int plat_set_nv_ctr(void *cookie, unsigned int nv_ctr)
+{
+	return -EINVAL;
+}
+
+#if !STM32MP_USE_STM32IMAGE
+int plat_get_mbedtls_heap(void **heap_addr, size_t *heap_size)
+{
+	assert(heap_addr != NULL);
+	assert(heap_size != NULL);
+
+#if STM32MP_USE_EXTERNAL_HEAP
+	/* Retrieve the already allocated heap's info from DTB */
+	*heap_addr = FCONF_GET_PROPERTY(tbbr, dyn_config, mbedtls_heap_addr);
+	*heap_size = FCONF_GET_PROPERTY(tbbr, dyn_config, mbedtls_heap_size);
+
+	return 0;
+#else
+	return get_mbedtls_heap_helper(heap_addr, heap_size);
+#endif
+}
+#endif
diff --git a/plat/st/stm32mp1/bl2_plat_setup.c b/plat/st/stm32mp1/bl2_plat_setup.c
index e09ce63c21..13ce3042a5 100644
--- a/plat/st/stm32mp1/bl2_plat_setup.c
+++ b/plat/st/stm32mp1/bl2_plat_setup.c
@@ -1,10 +1,11 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #include <assert.h>
+#include <errno.h>
 #include <string.h>
 
 #include <platform_def.h>
@@ -13,28 +14,51 @@
 #include <common/bl_common.h>
 #include <common/debug.h>
 #include <common/desc_image_load.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/generic_delay_timer.h>
 #include <drivers/st/bsec.h>
 #include <drivers/st/stm32_console.h>
 #include <drivers/st/stm32_iwdg.h>
+#include <drivers/st/stm32_uart.h>
+#include <drivers/st/stm32mp_clkfunc.h>
 #include <drivers/st/stm32mp_pmic.h>
 #include <drivers/st/stm32mp_reset.h>
 #include <drivers/st/stm32mp1_clk.h>
 #include <drivers/st/stm32mp1_pwr.h>
 #include <drivers/st/stm32mp1_ram.h>
+#include <drivers/st/stpmic1.h>
+#include <lib/fconf/fconf.h>
+#include <lib/fconf/fconf_dyn_cfg_getter.h>
 #include <lib/mmio.h>
 #include <lib/optee_utils.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
+#include <boot_api.h>
 #include <stm32mp1_context.h>
+#include <stm32mp1_critic_power.h>
 #include <stm32mp1_dbgmcu.h>
 
 #define RESET_TIMEOUT_US_1MS		1000U
 
+static const char debug_msg[626] = {
+	"***************************************************\n"
+	"** NOTICE   NOTICE   NOTICE   NOTICE   NOTICE    **\n"
+	"**                                               **\n"
+	"** DEBUG ACCESS PORT IS OPEN!                    **\n"
+	"** This boot image is only for debugging purpose **\n"
+	"** and is unsafe for production use.             **\n"
+	"**                                               **\n"
+	"** If you see this message and you are not       **\n"
+	"** debugging report this immediately to your     **\n"
+	"** vendor!                                       **\n"
+	"**                                               **\n"
+	"***************************************************\n"
+};
+
 static console_t console;
-static struct stm32mp_auth_ops stm32mp1_auth_ops;
+static enum boot_device_e boot_device = BOOT_DEVICE_BOARD;
 
 static void print_reset_reason(void)
 {
@@ -121,6 +145,11 @@ static void print_reset_reason(void)
 	ERROR("  Unidentified reset reason\n");
 }
 
+enum boot_device_e get_boot_device(void)
+{
+	return boot_device;
+}
+
 void bl2_el3_early_platform_setup(u_register_t arg0,
 				  u_register_t arg1 __unused,
 				  u_register_t arg2 __unused,
@@ -132,11 +161,6 @@ void bl2_el3_early_platform_setup(u_register_t arg0,
 void bl2_platform_setup(void)
 {
 	int ret;
-	uint32_t ddr_ns_size;
-
-	if (dt_pmic_status() > 0) {
-		initialize_pmic();
-	}
 
 	ret = stm32mp1_ddr_probe();
 	if (ret < 0) {
@@ -144,29 +168,150 @@ void bl2_platform_setup(void)
 		panic();
 	}
 
-	ddr_ns_size = stm32mp_get_ddr_ns_size();
-	assert(ddr_ns_size > 0U);
-
-	/* Map non secure DDR for BL33 load, now with cacheable attribute */
-	ret = mmap_add_dynamic_region(STM32MP_DDR_BASE, STM32MP_DDR_BASE,
-				      ddr_ns_size, MT_MEMORY | MT_RW | MT_NS);
-	assert(ret == 0);
-
+#if STM32MP_USE_STM32IMAGE
 #ifdef AARCH32_SP_OPTEE
 	INFO("BL2 runs OP-TEE setup\n");
-
-	/* Map secure DDR for OP-TEE paged area */
-	ret = mmap_add_dynamic_region(STM32MP_DDR_BASE + ddr_ns_size,
-				      STM32MP_DDR_BASE + ddr_ns_size,
-				      STM32MP_DDR_S_SIZE,
-				      MT_MEMORY | MT_RW | MT_SECURE);
-	assert(ret == 0);
-
-	/* Initialize tzc400 after DDR initialization */
-	stm32mp1_security_setup();
 #else
 	INFO("BL2 runs SP_MIN setup\n");
 #endif
+#endif
+
+	if (!stm32mp1_ddr_is_restored()) {
+		uint32_t bkpr_core1_magic =
+			tamp_bkpr(BOOT_API_CORE1_MAGIC_NUMBER_TAMP_BCK_REG_IDX);
+		uint32_t bkpr_core1_addr =
+			tamp_bkpr(BOOT_API_CORE1_BRANCH_ADDRESS_TAMP_BCK_REG_IDX);
+
+		/* Clear backup register */
+		mmio_write_32(bkpr_core1_addr, 0);
+		/* Clear backup register magic */
+		mmio_write_32(bkpr_core1_magic, 0);
+
+		/* Clear the context in BKPSRAM */
+		stm32_clean_context();
+
+		if (dt_pmic_status() > 0) {
+			configure_pmic();
+		}
+	}
+
+	/* Map DDR for binary load, now with cacheable attribute */
+	ret = mmap_add_dynamic_region(STM32MP_DDR_BASE, STM32MP_DDR_BASE,
+				      STM32MP_DDR_MAX_SIZE, MT_MEMORY | MT_RW | MT_SECURE);
+	if (ret < 0) {
+		ERROR("DDR mapping: error %d\n", ret);
+		panic();
+	}
+}
+
+static void update_monotonic_counter(void)
+{
+	uint32_t version;
+	uint32_t otp;
+
+	CASSERT(STM32_TF_VERSION <= MAX_MONOTONIC_VALUE,
+		assert_stm32mp1_monotonic_counter_reach_max);
+
+	/* Check if monotonic counter needs to be incremented */
+	if (stm32_get_otp_index(MONOTONIC_OTP, &otp, NULL) != 0) {
+		panic();
+	}
+
+	if (stm32_get_otp_value(MONOTONIC_OTP, &version) != 0) {
+		panic();
+	}
+
+	if ((version + 1U) < BIT(STM32_TF_VERSION)) {
+		uint32_t result;
+
+		/* Need to increment the monotonic counter. */
+		version = BIT(STM32_TF_VERSION) - 1U;
+
+		result = bsec_program_otp(version, otp);
+		if (result != BSEC_OK) {
+			ERROR("BSEC: MONOTONIC_OTP program Error %i\n",
+			      result);
+			panic();
+		}
+		INFO("Monotonic counter has been incremented (value 0x%x)\n",
+		     version);
+	}
+}
+
+static void initialize_clock(void)
+{
+	uint32_t voltage_mv = 0U;
+	uint32_t freq_khz = 0U;
+	int ret = 0;
+
+	if (stm32mp1_is_wakeup_from_standby()) {
+		ret = stm32_get_pll1_settings_from_context();
+	}
+
+	/*
+	 * If no pre-defined PLL1 settings in DT, find the highest frequency
+	 * in the OPP table (in DT, compatible with plaform capabilities, or
+	 * in structure restored in RAM), and set related CPU supply voltage.
+	 * If PLL1 settings found in DT, we consider CPU supply voltage in DT
+	 * is consistent with it.
+	 */
+	if ((ret == 0) && !fdt_is_pll1_predefined()) {
+		if (stm32mp1_is_wakeup_from_standby()) {
+			ret = stm32mp1_clk_get_maxfreq_opp(&freq_khz,
+							   &voltage_mv);
+		} else {
+			ret = dt_get_max_opp_freqvolt(&freq_khz, &voltage_mv);
+		}
+
+		if (ret != 0) {
+			panic();
+		}
+
+		if (dt_pmic_status() > 0) {
+			int read_voltage;
+			const char *name;
+
+			name = stm32mp_get_cpu_supply_name();
+			if (name == NULL) {
+				panic();
+			}
+
+			read_voltage = stpmic1_regulator_voltage_get(name);
+			if (read_voltage < 0) {
+				panic();
+			}
+
+			if (voltage_mv != (uint32_t)read_voltage) {
+				if (stpmic1_regulator_voltage_set(name,
+						(uint16_t)voltage_mv) != 0) {
+					panic();
+				}
+			}
+		}
+	}
+
+	if (stm32mp1_clk_init(freq_khz) < 0) {
+		panic();
+	}
+}
+
+static void reset_uart(uint32_t reset)
+{
+	int ret;
+
+	ret = stm32mp_reset_assert(reset, RESET_TIMEOUT_US_1MS);
+	if (ret != 0) {
+		panic();
+	}
+
+	udelay(2);
+
+	ret = stm32mp_reset_deassert(reset, RESET_TIMEOUT_US_1MS);
+	if (ret != 0) {
+		panic();
+	}
+
+	mdelay(1);
 }
 
 void bl2_el3_plat_arch_setup(void)
@@ -179,16 +324,20 @@ void bl2_el3_plat_arch_setup(void)
 	uint32_t clk_rate;
 	uintptr_t pwr_base;
 	uintptr_t rcc_base;
+	bool serial_uart_interface __unused =
+				(boot_context->boot_interface_selected ==
+				 BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART);
+	uintptr_t uart_prog_addr __unused;
+
+	if (bsec_probe() != 0) {
+		panic();
+	}
 
 	mmap_add_region(BL_CODE_BASE, BL_CODE_BASE,
 			BL_CODE_END - BL_CODE_BASE,
 			MT_CODE | MT_SECURE);
 
-#ifdef AARCH32_SP_OPTEE
-	mmap_add_region(STM32MP_OPTEE_BASE, STM32MP_OPTEE_BASE,
-			STM32MP_OPTEE_SIZE,
-			MT_MEMORY | MT_RW | MT_SECURE);
-#else
+#if STM32MP_USE_STM32IMAGE && !defined(AARCH32_SP_OPTEE)
 	/* Prevent corruption of preloaded BL32 */
 	mmap_add_region(BL32_BASE, BL32_BASE,
 			BL32_LIMIT - BL32_BASE,
@@ -201,7 +350,7 @@ void bl2_el3_plat_arch_setup(void)
 
 	configure_mmu();
 
-	if (dt_open_and_check() < 0) {
+	if (dt_open_and_check(STM32MP_DTB_BASE) < 0) {
 		panic();
 	}
 
@@ -219,10 +368,6 @@ void bl2_el3_plat_arch_setup(void)
 		;
 	}
 
-	if (bsec_probe() != 0) {
-		panic();
-	}
-
 	/* Reset backup domain on cold boot cases */
 	if ((mmio_read_32(rcc_base + RCC_BDCR) & RCC_BDCR_RTCSRC_MASK) == 0U) {
 		mmio_setbits_32(rcc_base + RCC_BDCR, RCC_BDCR_VSWRST);
@@ -235,25 +380,45 @@ void bl2_el3_plat_arch_setup(void)
 		mmio_clrbits_32(rcc_base + RCC_BDCR, RCC_BDCR_VSWRST);
 	}
 
-	/* Disable MCKPROT */
-	mmio_clrbits_32(rcc_base + RCC_TZCR, RCC_TZCR_MCKPROT);
+	/* Enable BKP Register protection */
+	mmio_write_32(TAMP_SMCR,
+		      TAMP_BKP_SEC_NUMBER << TAMP_BKP_SEC_WDPROT_SHIFT |
+		      TAMP_BKP_SEC_NUMBER << TAMP_BKP_SEC_RWDPROT_SHIFT);
 
 	generic_delay_timer_init();
 
+#if STM32MP_UART_PROGRAMMER
+	uart_prog_addr = get_uart_address(boot_context->boot_interface_instance);
+
+	/* Disable programmer UART before changing clock tree */
+	if (serial_uart_interface) {
+		stm32_uart_stop(uart_prog_addr);
+	}
+#endif
+#if STM32MP_USB_PROGRAMMER
+	if (boot_context->boot_interface_selected ==
+	    BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB) {
+		boot_device = BOOT_DEVICE_USB;
+	}
+#endif
 	if (stm32mp1_clk_probe() < 0) {
 		panic();
 	}
 
-	if (stm32mp1_clk_init() < 0) {
-		panic();
+	if (dt_pmic_status() > 0) {
+		initialize_pmic();
 	}
 
-	stm32mp1_syscfg_init();
+	initialize_clock();
 
 	result = dt_get_stdout_uart_info(&dt_uart_info);
 
 	if ((result <= 0) ||
-	    (dt_uart_info.status == 0U) ||
+	    (dt_uart_info.status == DT_DISABLED) ||
+#if STM32MP_UART_PROGRAMMER
+	    (serial_uart_interface &&
+	     (uart_prog_addr == dt_uart_info.base)) ||
+#endif
 	    (dt_uart_info.clock < 0) ||
 	    (dt_uart_info.reset < 0)) {
 		goto skip_console_init;
@@ -263,23 +428,15 @@ void bl2_el3_plat_arch_setup(void)
 		goto skip_console_init;
 	}
 
-	stm32mp_clk_enable((unsigned long)dt_uart_info.clock);
-
-	if (stm32mp_reset_assert((uint32_t)dt_uart_info.reset,
-				 RESET_TIMEOUT_US_1MS) != 0) {
+	if (dt_uart_info.status == DT_DISABLED) {
 		panic();
 	}
 
-	udelay(2);
-
-	if (stm32mp_reset_deassert((uint32_t)dt_uart_info.reset,
-				   RESET_TIMEOUT_US_1MS) != 0) {
-		panic();
-	}
+	clk_enable((unsigned long)dt_uart_info.clock);
 
-	mdelay(1);
+	reset_uart((uint32_t)dt_uart_info.reset);
 
-	clk_rate = stm32mp_clk_get_rate((unsigned long)dt_uart_info.clock);
+	clk_rate = clk_get_rate((unsigned long)dt_uart_info.clock);
 
 	if (console_stm32_register(dt_uart_info.base, clk_rate,
 				   STM32MP_UART_BAUDRATE, &console) == 0) {
@@ -298,22 +455,40 @@ void bl2_el3_plat_arch_setup(void)
 
 	stm32mp_print_boardinfo();
 
+#if TRUSTED_BOARD_BOOT
 	if (boot_context->auth_status != BOOT_API_CTX_AUTH_NO) {
 		NOTICE("Bootrom authentication %s\n",
 		       (boot_context->auth_status == BOOT_API_CTX_AUTH_FAILED) ?
 		       "failed" : "succeeded");
 	}
+#endif
 
 skip_console_init:
+#if !TRUSTED_BOARD_BOOT
+	if (stm32mp_is_closed_device()) {
+		/* Closed chip required authentication */
+		ERROR("Secured chip must enabled TRUSTED_BOARD_BOOT\n");
+		panic();
+	}
+#endif
+
+	stm32mp1_syscfg_init();
+
 	if (stm32_iwdg_init() < 0) {
 		panic();
 	}
 
 	stm32_iwdg_refresh();
 
-	result = stm32mp1_dbgmcu_freeze_iwdg2();
-	if (result != 0) {
-		INFO("IWDG2 freeze error : %i\n", result);
+	if (bsec_read_debug_conf() != 0U) {
+		result = stm32mp1_dbgmcu_freeze_iwdg2();
+		if (result != 0) {
+			INFO("IWDG2 freeze error : %i\n", result);
+		}
+
+		if (stm32mp_is_closed_device()) {
+			NOTICE("\n%s", debug_msg);
+		}
 	}
 
 	if (stm32_save_boot_interface(boot_context->boot_interface_selected,
@@ -322,20 +497,57 @@ skip_console_init:
 		ERROR("Cannot save boot interface\n");
 	}
 
-	stm32mp1_auth_ops.check_key = boot_context->bootrom_ecdsa_check_key;
-	stm32mp1_auth_ops.verify_signature =
-		boot_context->bootrom_ecdsa_verify_signature;
-
-	stm32mp_init_auth(&stm32mp1_auth_ops);
-
 	stm32mp1_arch_security_setup();
 
 	print_reset_reason();
 
+	update_monotonic_counter();
+
+	stm32mp1_syscfg_enable_io_compensation_finish();
+
+	if (dt_pmic_status() > 0) {
+		initialize_pmic();
+		print_pmic_info_and_debug();
+	}
+
+#if STM32MP_USE_STM32IMAGE
+	if (!stm32mp1_ddr_is_restored()) {
+		stm32mp_io_setup();
+	}
+#else
+	fconf_populate("TB_FW", STM32MP_DTB_BASE);
+
 	stm32mp_io_setup();
+#endif
 }
 
-#if defined(AARCH32_SP_OPTEE)
+#if defined(AARCH32_SP_OPTEE) && STM32MP_USE_STM32IMAGE
+static void set_mem_params_info(entry_point_info_t *ep_info,
+				image_info_t *unpaged, image_info_t *paged)
+{
+	uintptr_t bl32_ep = 0;
+
+	/* Use the default dram setup if no valid ep found */
+	if (get_optee_header_ep(ep_info, &bl32_ep) &&
+	    (bl32_ep >= STM32MP_OPTEE_BASE) &&
+	    (bl32_ep < (STM32MP_OPTEE_BASE + STM32MP_OPTEE_SIZE))) {
+		assert((STM32MP_OPTEE_BASE >= BL2_LIMIT) ||
+		       ((STM32MP_OPTEE_BASE + STM32MP_OPTEE_SIZE) <= BL2_BASE));
+
+		unpaged->image_base = STM32MP_OPTEE_BASE;
+		unpaged->image_max_size = STM32MP_OPTEE_SIZE;
+	} else {
+		unpaged->image_base = STM32MP_DDR_BASE + dt_get_ddr_size() -
+				      STM32MP_DDR_S_SIZE -
+				      STM32MP_DDR_SHMEM_SIZE;
+		unpaged->image_max_size = STM32MP_DDR_S_SIZE;
+	}
+	paged->image_base = STM32MP_DDR_BASE + dt_get_ddr_size() -
+			    STM32MP_DDR_S_SIZE - STM32MP_DDR_SHMEM_SIZE;
+	paged->image_max_size = STM32MP_DDR_S_SIZE;
+}
+#endif
+
 /*******************************************************************************
  * This function can be used by the platforms to update/use image
  * information for given `image_id`.
@@ -345,50 +557,160 @@ int bl2_plat_handle_post_image_load(unsigned int image_id)
 	int err = 0;
 	bl_mem_params_node_t *bl_mem_params = get_bl_mem_params_node(image_id);
 	bl_mem_params_node_t *bl32_mem_params;
-	bl_mem_params_node_t *pager_mem_params;
-	bl_mem_params_node_t *paged_mem_params;
+	bl_mem_params_node_t *pager_mem_params __unused;
+	bl_mem_params_node_t *paged_mem_params __unused;
+#if !STM32MP_USE_STM32IMAGE
+	const struct dyn_cfg_dtb_info_t *config_info;
+	bl_mem_params_node_t *tos_fw_mem_params;
+	unsigned int i;
+	unsigned long long ddr_top __unused;
+	bool wakeup_ddr_sr = stm32mp1_ddr_is_restored();
+	const unsigned int image_ids[] = {
+		BL32_IMAGE_ID,
+		BL33_IMAGE_ID,
+		HW_CONFIG_ID,
+		TOS_FW_CONFIG_ID,
+	};
+#endif
 
 	assert(bl_mem_params != NULL);
 
+#if TRUSTED_BOARD_BOOT && STM32MP_USE_STM32IMAGE
+	/* Clean header to avoid loaded header reused */
+	stm32mp_delete_loaded_header();
+#endif
+
 	switch (image_id) {
+#if !STM32MP_USE_STM32IMAGE
+	case FW_CONFIG_ID:
+		/* Set global DTB info for fixed fw_config information */
+		set_config_info(STM32MP_FW_CONFIG_BASE, STM32MP_FW_CONFIG_MAX_SIZE, FW_CONFIG_ID);
+		fconf_populate("FW_CONFIG", STM32MP_FW_CONFIG_BASE);
+
+		/* Iterate through all the fw config IDs */
+		for (i = 0; i < ARRAY_SIZE(image_ids); i++) {
+			bl_mem_params = get_bl_mem_params_node(image_ids[i]);
+			assert(bl_mem_params != NULL);
+
+			config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, image_ids[i]);
+			if (config_info == NULL) {
+				continue;
+			}
+
+			bl_mem_params->image_info.image_base = config_info->config_addr;
+			bl_mem_params->image_info.image_max_size = config_info->config_max_size;
+
+			/*
+			 * If going back from CSTANDBY / STANDBY and DDR was in Self-Refresh,
+			 * DDR partitions must not be reloaded.
+			 */
+			if (!(wakeup_ddr_sr && (config_info->config_addr >= STM32MP_DDR_BASE))) {
+				bl_mem_params->image_info.h.attr &= ~IMAGE_ATTRIB_SKIP_LOADING;
+			}
+
+			switch (image_ids[i]) {
+			case BL32_IMAGE_ID:
+				bl_mem_params->ep_info.pc = config_info->config_addr;
+
+				/* In case of OPTEE, initialize address space with tos_fw addr */
+				pager_mem_params = get_bl_mem_params_node(BL32_EXTRA1_IMAGE_ID);
+				pager_mem_params->image_info.image_base = config_info->config_addr;
+				pager_mem_params->image_info.image_max_size =
+					config_info->config_max_size;
+
+				/* Init base and size for pager if exist */
+				paged_mem_params = get_bl_mem_params_node(BL32_EXTRA2_IMAGE_ID);
+				paged_mem_params->image_info.image_base = STM32MP_DDR_BASE +
+					(dt_get_ddr_size() - STM32MP_DDR_S_SIZE);
+				paged_mem_params->image_info.image_max_size = STM32MP_DDR_S_SIZE;
+				break;
+
+			case BL33_IMAGE_ID:
+				if (wakeup_ddr_sr) {
+					/*
+					 * Set ep_info PC to 0, to inform BL32 it is a reset
+					 * after STANDBY
+					 */
+					bl_mem_params->ep_info.pc = 0U;
+				} else {
+					bl_mem_params->ep_info.pc = config_info->config_addr;
+				}
+				break;
+
+			case HW_CONFIG_ID:
+			case TOS_FW_CONFIG_ID:
+				break;
+
+			default:
+				return -EINVAL;
+			}
+		}
+
+		break;
+#endif /* !STM32MP_USE_STM32IMAGE */
+
 	case BL32_IMAGE_ID:
-		bl_mem_params->ep_info.pc =
-					bl_mem_params->image_info.image_base;
-
-		pager_mem_params = get_bl_mem_params_node(BL32_EXTRA1_IMAGE_ID);
-		assert(pager_mem_params != NULL);
-		pager_mem_params->image_info.image_base = STM32MP_OPTEE_BASE;
-		pager_mem_params->image_info.image_max_size =
-			STM32MP_OPTEE_SIZE;
-
-		paged_mem_params = get_bl_mem_params_node(BL32_EXTRA2_IMAGE_ID);
-		assert(paged_mem_params != NULL);
-		paged_mem_params->image_info.image_base = STM32MP_DDR_BASE +
-			stm32mp_get_ddr_ns_size();
-		paged_mem_params->image_info.image_max_size =
-			STM32MP_DDR_S_SIZE;
-
-		err = parse_optee_header(&bl_mem_params->ep_info,
-					 &pager_mem_params->image_info,
-					 &paged_mem_params->image_info);
-		if (err) {
-			ERROR("OPTEE header parse error.\n");
-			panic();
+#if defined(AARCH32_SP_OPTEE) || !STM32MP_USE_STM32IMAGE
+		bl_mem_params->ep_info.pc = bl_mem_params->image_info.image_base;
+		if (get_optee_header_ep(&bl_mem_params->ep_info, &bl_mem_params->ep_info.pc) == 1) {
+			/* BL32 is OP-TEE header */
+#if !STM32MP_USE_STM32IMAGE
+			if (wakeup_ddr_sr) {
+				bl_mem_params->ep_info.pc = stm32_pm_get_optee_ep();
+				if (stm32mp1_addr_inside_backupsram(bl_mem_params->ep_info.pc)) {
+					clk_enable(BKPSRAM);
+				}
+
+				break;
+			}
+#endif
+			pager_mem_params = get_bl_mem_params_node(BL32_EXTRA1_IMAGE_ID);
+			paged_mem_params = get_bl_mem_params_node(BL32_EXTRA2_IMAGE_ID);
+			assert((pager_mem_params != NULL) && (paged_mem_params != NULL));
+
+#if STM32MP_USE_STM32IMAGE
+			set_mem_params_info(&bl_mem_params->ep_info, &pager_mem_params->image_info,
+					    &paged_mem_params->image_info);
+#endif
+
+			err = parse_optee_header(&bl_mem_params->ep_info,
+						 &pager_mem_params->image_info,
+						 &paged_mem_params->image_info);
+			if (err) {
+				ERROR("OPTEE header parse error.\n");
+				panic();
+			}
+
+			/* Set optee boot info from parsed header data */
+			bl_mem_params->ep_info.args.arg0 = paged_mem_params->image_info.image_base;
+			bl_mem_params->ep_info.args.arg1 = 0; /* Unused */
+			bl_mem_params->ep_info.args.arg2 = 0; /* No DT supported */
+		} else {
+#if STM32MP_USE_STM32IMAGE
+			bl_mem_params->ep_info.pc = STM32MP_BL32_BASE;
+#else
+			tos_fw_mem_params = get_bl_mem_params_node(TOS_FW_CONFIG_ID);
+			bl_mem_params->image_info.image_max_size +=
+				tos_fw_mem_params->image_info.image_max_size;
+#endif
+			bl_mem_params->ep_info.args.arg0 = 0;
 		}
 
-		/* Set optee boot info from parsed header data */
-		bl_mem_params->ep_info.pc =
-				pager_mem_params->image_info.image_base;
-		bl_mem_params->ep_info.args.arg0 =
-				paged_mem_params->image_info.image_base;
-		bl_mem_params->ep_info.args.arg1 = 0; /* Unused */
-		bl_mem_params->ep_info.args.arg2 = 0; /* No DT supported */
+		if (bl_mem_params->ep_info.pc >= STM32MP_DDR_BASE) {
+			stm32_context_save_bl2_param();
+		}
+#endif
 		break;
 
 	case BL33_IMAGE_ID:
 		bl32_mem_params = get_bl_mem_params_node(BL32_IMAGE_ID);
 		assert(bl32_mem_params != NULL);
 		bl32_mem_params->ep_info.lr_svc = bl_mem_params->ep_info.pc;
+
+#if STM32MP_USB_PROGRAMMER || STM32MP_UART_PROGRAMMER
+		/* Invalidate downloaded package from cache */
+		inv_dcache_range(DWL_BUFFER_BASE, DWL_BUFFER_SIZE);
+#endif
 		break;
 
 	default:
@@ -398,4 +720,8 @@ int bl2_plat_handle_post_image_load(unsigned int image_id)
 
 	return err;
 }
-#endif
+
+void bl2_el3_plat_prepare_exit(void)
+{
+	stm32mp1_security_setup();
+}
diff --git a/plat/st/stm32mp1/include/boot_api.h b/plat/st/stm32mp1/include/boot_api.h
index c16639ac40..8c9d3afb49 100644
--- a/plat/st/stm32mp1/include/boot_api.h
+++ b/plat/st/stm32mp1/include/boot_api.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,6 +10,90 @@
 #include <stdint.h>
 #include <stdio.h>
 
+/*
+ * Exported constants
+ */
+
+/*
+ * Boot Context related definitions
+ */
+
+/*
+ * Possible value of boot context field 'boot_action'
+ */
+/* Boot action is Process Cold Boot */
+#define BOOT_API_CTX_BOOT_ACTION_COLD_BOOT_PROCESS		0x09U
+/* Boot action is Process Wakeup from CSTANDBY */
+#define BOOT_API_CTX_BOOT_ACTION_WAKEUP_CSTANDBY		0x0AU
+/* Boot action is Process Wakeup from STANDBY  */
+#define BOOT_API_CTX_BOOT_ACTION_WAKEUP_STANDBY			0x0BU
+/* Boot action is Process Engineering Boot */
+#define BOOT_API_CTX_BOOT_ACTION_ENGI_BOOT			0x0CU
+
+#define BOOT_API_CTX_BOOT_ACTION_MPU_CORE0_RESET_PROCESS	0x0F
+
+/*
+ * Possible value of boot context field 'stby_exit_status'
+ */
+
+/* The boot reason is not a STANDBY Exit reason */
+#define BOOT_API_CTX_STBY_EXIT_STATUS_NO_STANDBY                0x00
+
+/* STANDBY Exit with MPU_BEN=1, MCU_BEN=0 */
+#define BOOT_API_CTX_STBY_EXIT_STATUS_WKUP_MPU_ONLY             0x01
+
+/*
+ * STANDBY Exit with MPU_BEN=1, MCU_BEN=1, MPU will go for cold boot
+ * MCU restarted by bootROM
+ */
+#define BOOT_API_CTX_STBY_EXIT_STATUS_WKUP_ALL_CORES            0x02
+
+/*
+ * STANDBY Exit with MPU_BEN=1, MCU_BEN=1, MPU will go for cold boot
+ * but MCU restart aborted (code integrity check) : have not been restarted
+ * by bootROM
+ */
+#define BOOT_API_CTX_STBY_EXIT_STATUS_WKUP_ALL_CORES_MCU_ABT    0x03
+
+/*
+ * STANDBY Exit with MPU_BEN=0, MCU_BEN=1, MPU gone to CSTANDBY,
+ * MCU restarted correctly by bootROM
+ * This value should never be read by FSBL, because not executed in that case
+ */
+#define BOOT_API_CTX_STBY_EXIT_STATUS_WKUP_MCU_ONLY             0x04
+
+/*
+ * STANDBY Exit with MPU_BEN=0, MCU_BEN=1, MCU restart aborted
+ * due code integrity check, then MPU will go for cold boot despite
+ * was not planned initially
+ */
+#define BOOT_API_CTX_STBY_EXIT_STATUS_WKUP_MCU_ONLY_MCU_ABT     0x05
+
+/*
+ * STANDBY Exit with MPU_BEN=1, MCU_BEN=1, MCU restart aborted
+ * due to MCU security perimeter issue
+ */
+#define \
+BOOT_API_CTX_STBY_EXIT_STATUS_WKUP_ALL_CORES_MCU_ABT_SEC_PERIMETER_ISSUE 0x06
+
+/*
+ * STANDBY Exit with MPU_BEN=0, MCU_BEN=1, MCU restart aborted
+ * due to MCU security perimeter issue, then MPU will go for cold boot
+ * despite was not planned initially
+ */
+#define \
+BOOT_API_CTX_STBY_EXIT_STATUS_WKUP_MCU_ONLY_MCU_ABT_SEC_PERIMETER_ISSUE	0x07
+
+/*
+ * Possible value of boot context field 'cstby_exit_status'
+ */
+/* The boot reason is not a CSTANDBY Exit reason */
+#define BOOT_API_CTX_CSTBY_EXIT_STATUS_NO_CSTBY			0x00
+/* CSTANDBY Exit with MCU detected as Not running */
+#define BOOT_API_CTX_CSTBY_EXIT_STATUS_MCU_NOT_RUNNING		0x01
+/* CSTANDBY Exit with MCU detected as Running  */
+#define BOOT_API_CTX_CSTBY_EXIT_STATUS_MCU_RUNNING		0x02
+
 /*
  * Possible value of boot context field 'auth_status'
  */
@@ -37,10 +121,16 @@
 #define BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_FMC		0x3U
 
 /* Boot occurred on QSPI NOR */
-#define BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NOR_QSPI		0x4U
+#define BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NOR_SPI		0x4U
+
+/* Boot occurred on UART  */
+#define BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART		0x5U
+
+/* Boot occurred on USB */
+#define BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB		0x6U
 
 /* Boot occurred on QSPI NAND */
-#define BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_QSPI		0x7U
+#define BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_SPI		0x7U
 
 /**
  * @brief  Possible value of boot context field 'EmmcXferStatus'
@@ -65,6 +155,10 @@
 #define BOOT_API_CTX_EMMC_ERROR_STATUS_HEADER_SIZE_ZERO         0x6U
 #define BOOT_API_CTX_EMMC_ERROR_STATUS_IMAGE_NOT_COMPLETE       0x7U
 
+/* Definitions relative to 'p_rom_version_info->platform_type_ver' field */
+#define BOOT_API_CTX_ROM_VERSION_PLAT_VER_IC_EMU_FPGA           0xAA
+#define BOOT_API_CTX_ROM_VERSION_PLAT_VER_FPGA_ONLY             0xBB
+
 /* Image Header related definitions */
 
 /* Definition of header version */
@@ -95,6 +189,64 @@
 #define BOOT_API_A7_CORE0_MAGIC_NUMBER				0xCA7FACE0U
 #define BOOT_API_A7_CORE1_MAGIC_NUMBER				0xCA7FACE1U
 
+/*
+ * MCU Code Integrity Check related definitions
+ */
+
+/*
+ * Defines to identify RTC backup registers to be used for MCU code integrity
+ * check
+ */
+
+/*
+ * TAMP_BCK0R contains two bits
+ * bit 0 : wanted value of 'RCC_TZCR.TZEN'
+ * bit 1 : wanted value of 'RCC_TZCR.MCKPROT'
+ */
+
+/*
+ * TAMP_BCK0R bit position coding wanted value of 'RCC_TZCR.TZEN'
+ * trustZone aware domain enabling/disabling
+ */
+#define BOOT_API_MCIC_MCU_SECURITY_PERIMETER_TZEN_BIT			0
+
+/*
+ * TAMP_BCK0R bit position coding wanted value of 'RCC_TZCR.MCKPROT'
+ * ability of MCU to modify some clock settings in RCC
+ */
+#define BOOT_API_MCIC_MCU_SECURITY_PERIMETER_MCKPROT_BIT		1
+
+/* TAMP_BCK0R register index */
+#define \
+BOOT_API_MCIC_MCU_SECURITY_PERIMETER_TZEN_MCKPROT_TAMP_BCK_REG_IDX	0
+
+/*
+ * TAMP_BCK1R register index
+ * This register is coding the wanted value of register 'EXTI_TZENR1'
+ * to be programmed by bootROM on wakeup from STANDBY when MCUBEN=1
+ * that is MCU quick restart requested
+ */
+#define \
+BOOT_API_MCIC_MCU_SECURITY_PERIMETER_EXTI_TZENR1_TAMP_BCK_REG_IDX	1
+
+/*
+ * TAMP_BCK2R register index
+ * This register is coding the wanted value of register 'EXTI_TZENR2'
+ * to be programmed by bootROM on wakeup from STANDBY when MCUBEN=1
+ * that is MCU quick restart requested
+ */
+#define \
+BOOT_API_MCIC_MCU_SECURITY_PERIMETER_EXTI_TZENR2_TAMP_BCK_REG_IDX	2
+
+/*
+ * TAMP_BCK3R register index
+ * This register is coding the wanted value of register 'EXTI_TZENR3'
+ * to be programmed by bootROM on wakeup from STANDBY when MCUBEN=1
+ * that is MCU quick restart requested
+ */
+#define \
+BOOT_API_MCIC_MCU_SECURITY_PERIMETER_EXTI_TZENR3_TAMP_BCK_REG_IDX	3
+
 /*
  * TAMP_BCK4R register index
  * This register is used to write a Magic Number in order to restart
@@ -109,6 +261,39 @@
  */
 #define BOOT_API_CORE1_BRANCH_ADDRESS_TAMP_BCK_REG_IDX		5U
 
+/*
+ * TAMP_BCK22R register index
+ * This register contains offset in bytes of code to Hash in RETRAM region
+ * Note : offset is intended as relative value from start of RETRAM
+ */
+#define \
+BOOT_API_MCIC_OFFSET_IN_BYTES_CODE_TO_HASH_RETRAM_TAMP_BCK_REG_IDX	22
+
+/*
+ * TAMP_BCK23R register index
+ * This register contains the size in bytes of the single consecutive region
+ * of MCU Firmware in RETRAM (Retention RAM) to hash (by SHA-256)
+ * Note : This is required as a MCU firmware Code Integrity Check (aka : MCIC)
+ * to avoid bootROM restarting MCU on a corrupted firmware
+ */
+#define \
+BOOT_API_MCIC_RETRAM_REGION_TO_HASH_IN_BYTES_TAMP_BCK_REG_IDX		23
+
+/*
+ * TAMP_BCK24R to TAMP_BCK31R register indexes
+ * Those registers contains SHA-256 digest of RETRAM MCU Firmware code between
+ * [(RETRAM_start + offset) -- (RETRAM_start + offset + size_to_hash)]
+ * in this order
+ * This is the MCU Code Integrity Check MCU Firmware signature
+ * value on 256 bits
+ */
+
+/* First TAMP_BKP index of MCU Firmware signature : ie TAMP_BCK24R */
+#define BOOT_API_MCIC_SHA_DIGEST_FIRST_TAMP_BCK_REG_IDX			24
+
+/* Last TAMP_BKP index of MCU Firmware signature : ie TAMP_BCK31R */
+#define BOOT_API_MCIC_SHA_DIGEST_LAST_TAMP_BCK_REG_IDX			31
+
 /*
  * Possible value of boot context field 'hse_clock_value_in_hz'
  */
@@ -135,6 +320,320 @@
 
 #define BOOT_API_RETURN_OK					0x77U
 
+/* Mapping of OTP Word and OTP bits managing SSP and useful to FSBL-SSP */
+/* OTP_CFG8 */
+#define BOOT_API_OTP_SSP_WORD_NB				8U
+/* SSP_REQ = OTP_CFG8[8] */
+#define BOOT_API_OTP_SSP_REQ_BIT_POS				8
+/* SSP_SUCCESS = OTP_CFG8[9] */
+#define BOOT_API_OTP_SSP_SUCCESS_BIT_POS			9
+
+/*
+ * Possible values of boot context field
+ * 'ssp_config_ptr_in->ssp_cmd'
+ */
+/* 'K' 'B' 'U' 'P' -.> 'PUBK' */
+#define BOOT_API_CTX_SSP_CMD_CALC_CHIP_PUBK             0x4B425550
+
+#if STM32MP_SSP
+/* 'V' 'O' 'R' 'P' -.> 'PROV' */
+#define BOOT_API_CTX_SSP_CMD_PROV_SECRET		0x564F5250
+/*
+ * Possible values of boot context field
+ * 'ssp_config_ptr_in->ssp_cmd' written by bootROM as Acknowledge
+ * of a request of SSP by FSBL.
+ */
+
+/* Written by bootROM on SSP error */
+#define BOOT_API_CTX_SSP_CMD_INVALID			0x00000000
+/*
+ * 'A' 'B' 'U' 'P' -.> 'PUBA' : ACK of ECIES_CHIP_PUBK calculation
+ * request by bootROM.
+ */
+#define BOOT_API_CTX_SSP_CMD_CALC_CHIP_PUBK_ACK		0x41425550
+/*
+ * 'A' 'O' 'R' 'P' -.> 'PROA' : ACK of OEM Secret Provisioning request
+ * by bootROM.
+ */
+#define BOOT_API_CTX_SSP_CMD_PROV_SECRET_ACK		0x414F5250
+
+/*
+ * Constants required for SSP
+ */
+/* '.' 'P' 'S' 'S' -.> 'SSP.' */
+#define BOOT_API_SSP_BLOB_LICENSE_TYPE_SSP_NORMAL	0x2E505353
+/* 'L' 'P' 'S' 'S' -.> 'SSPL' */
+#define BOOT_API_SSP_BLOB_LICENSE_TYPE_SSP_LIVE		0x4C505353
+/* version 1 */
+#define BOOT_API_SSP_LICENSE_LAYOUT_VERSION_TO_MATCH	0x00000001
+/* 'P' 'P' 'S' 'S' -.> 'SSPP' */
+#define BOOT_API_SSP_BLOB_PAYLOAD_MAGIC_SSP		0x50505353
+/* IV AES on 128 bits = 16 bytes and KEY AES on 128 bits = 16 bytes */
+#define BOOT_API_SSP_ENCRYPTED_IV_AND_KEY_SIZE_BYTES	32
+/* version 1 */
+#define BOOT_API_SSP_PAYLOAD_PROTOCOL_VERSION_TO_MATCH	0x00000001
+/*
+ * Scalar in Elliptic curve cryptography is an integer (often a Prime)
+ * the number of bytes of this scalar is defined below.
+ */
+#define BOOT_API_SSP_SCALAR_SIZE_BYTES				32
+
+/*
+ * In Elliptic curve cryptography coordinates of points are 2D P
+ * (Px, Py) as concatenation of two scalars.
+ */
+#define BOOT_API_SSP_EC_COORDINATE_SIZE_BYTES \
+	(2 * BOOT_API_SSP_SCALAR_SIZE_BYTES)
+
+/* In Elliptic curve cryptography Private Keys are scalars */
+#define BOOT_API_SSP_PRIVK_KEY_SIZE_BYTES \
+	BOOT_API_SSP_SCALAR_SIZE_BYTES
+
+/*
+ * In ECIES algorithm the Shared Secret (SS) is
+ * the x coordinate (Px) of a point P(Px,Py) obtained on reference
+ * chosen NIST-P256 Elliptic curve.
+ */
+#define BOOT_API_SSP_ECDH_SHARED_SECRET_SIZE_BYTES \
+	BOOT_API_SSP_SCALAR_SIZE_BYTES
+
+/*
+ * In Elliptic curve cryptography Public Keys are Points on chosen
+ * Elliptic curve P(x,y).
+ * Public Key is the x, y coordinates concatenated
+ * Ecies_eph_pubk and OEM_ECDSA_PUBK are each 64 bytes = 512 bits key
+ * sizes.
+ */
+#define BOOT_API_SSP_PUBK_KEY_SIZE_BYTES \
+	BOOT_API_SSP_EC_COORDINATE_SIZE_BYTES
+
+/*
+ * Size in bytes of ECIES_Chip_pubk obtained from bootROM at end of SSP
+ * phase 1 : Chip public key calculation.
+ */
+#define BOOT_API_SSP_ECIES_CHIP_PUBK_SIZE_BYTES \
+	BOOT_API_SSP_PUBK_KEY_SIZE_BYTES
+
+/* AES-GCM authentication tag size is 16 bytes = 128 bits */
+#define BOOT_API_SSP_AES_GCM_AUTH_TAG_SIZE_BYTES		16
+
+/* AES-GCM Symmetric Key size is 16 bytes = 128 bits */
+#define BOOT_API_SSP_AES_GCM_KEY_SIZE_BYTES			16
+
+/* AES-GCM Initialization Vector (IV) size is of 16 bytes = 128 bits */
+#define BOOT_API_SSP_AES_GCM_IV_SIZE_BYTES			16
+
+/*
+ * 88 bytes (license_type, live_session_id, license_version,
+ * fsbl_min_version, rfu[8], eph_ecies_pubk[])
+ */
+#define BOOT_API_SSP_AES_GCM_LICENSE_AAD_NB_BYTES_FROM_LICENSE	88
+
+/*
+ * 32 bytes AAD License Secret from 2nd round KDF-SHA-256
+ * from ECDH Shared Secret hence KDF[32..63] aka "Authorization Token"
+ */
+#define BOOT_API_SSP_AES_GCM_LICENSE_AAD_NB_BYTES_FROM_KDF	32
+
+/*
+ * Total License AAD size = 88 + 32 = 120 bytes
+ */
+#define BOOT_API_SSP_AES_GCM_LICENSE_AAD_SIZE_BYTES \
+	(BOOT_API_SSP_AES_GCM_LICENSE_AAD_NB_BYTES_FROM_LICENSE + \
+	 BOOT_API_SSP_AES_GCM_LICENSE_AAD_NB_BYTES_FROM_KDF)
+
+/*
+ * AAD for Payload size : composed of :
+ * payload_magic, payload_protocol_version, oem_ecdsa_pubk[], oem_secret_size
+ * = 4 + 4 + 64 + 4 = 76 bytes AAD for Payload
+ */
+#define BOOT_API_SSP_AES_GCM_PAYLOAD_AAD_SIZE_BYTES		76
+
+/*
+ * OEM Secrets max size in bytes :
+ * [OTP[95:59] + OTP_CFG56 (RMA Unlock and Relock passwords)] x 4 bytes
+ * by OTP word = 152 bytes
+ */
+#define BOOT_API_SSP_OEM_SECRETS_MAX_SIZE_BYTES			152
+
+/*
+ * Possible values of boot context field 'ssp_status'
+ * as can be read by FSBL-SSP
+ */
+#define BOOT_API_CTX_SSP_STATUS_NO_SSP				0
+#define BOOT_API_CTX_SSP_STATUS_CHIP_PUBK_CALC_FINISHED		1
+#define BOOT_API_CTX_SSP_STATUS_OEM_SEC_PROV_FINISHED		2
+#define BOOT_API_CTX_SSP_STATUS_OEM_SEC_PROV_FORBIDDEN		3
+
+/*
+ * Reserved size for future use
+ */
+#define BOOT_API_SSP_HSM_OEM_RFU_SIZE				8
+
+/*
+ * Exported types
+ */
+
+/*
+ * SSP related definitions
+ */
+/*
+ * SSP BLOB License structure : Binary Large OBject License structure
+ * Should be written by FSBL-SSP to provide bootROM with SSP OEM Secret
+ * provisioning.
+ * License information data, the structure is read by bootROM.
+ */
+typedef struct {
+	/*
+	 * License Type provided by HSM-OEM tool
+	 * should match Normal SSP License of Live SSP License.
+	 */
+	uint32_t license_type;
+
+	/* Live Session ID provided by HSM-OEM tool */
+	uint32_t live_session_id;
+
+	/*
+	 * Version of the License Protocol (Structure)
+	 * should be incremented each time a new.
+	 */
+	uint32_t license_version;
+
+	/*
+	 * Minimum FSBL version to be compared
+	 * with FSBL Header field 'imageVersion'.
+	 */
+	uint32_t fsbl_min_version;
+
+	/* RFU provided by HSM-OEM tool */
+	uint8_t  rfu[BOOT_API_SSP_HSM_OEM_RFU_SIZE];
+
+	/*
+	 * Ephemeral ECIES Public Key from HSM-OEM
+	 * 64 bytes = 512 bits.
+	 */
+	uint8_t  eph_ecies_pubk[BOOT_API_SSP_PUBK_KEY_SIZE_BYTES];
+
+	/*
+	 * Encrypted (IV,Key) : with Shared Secret based on
+	 * 'Ephemeral ECIES Key pair' and 'ECIES Chip Key pair'.
+	 */
+	uint8_t encrypted_iv_and_key
+		[BOOT_API_SSP_ENCRYPTED_IV_AND_KEY_SIZE_BYTES];
+
+	/*
+	 * AUTH_TAG AES-GCM from encryption of (IV, Key)
+	 * in HSM-OEM with License AAD scheme
+	 * License Tag is 16 bytes = 128 bits.
+	 */
+	uint8_t  license_tag[BOOT_API_SSP_AES_GCM_AUTH_TAG_SIZE_BYTES];
+
+} boot_api_ssp_blob_license_t;
+
+/*
+ * SSP BLOB Payload structure : Binary Large OBject Payload Structure
+ * Should be written by FSBL-SSP to provide bootROM with SSP OEM Secret
+ * provisioning input data, the structure is read by bootROM
+ * The BLOB Payload size is fixed to a max size of 244 bytes based
+ * on a max number of bytes of OEM secret derived from OTP upper free
+ * area in STM32MP15xx cut 2.0.In this table oem_encrypted_secrets[]
+ * of max size only the first 'p_blob_payload->oem_secret_size_bytes'
+ * bytes will be considered and used by bootROM.
+ */
+typedef struct {
+	/*
+	 * BLOB Payload Magic : for memory validity check of BLOB Payload
+	 * to match against BOOT_API_SSP_BLOB_PAYLOAD_MAGIC_SSP by bootROM.
+	 */
+	uint32_t payload_magic;
+
+	/*
+	 * SSP Payload protocol version : on 32 bits
+	 * to be checked by bootROM for equality with
+	 * BOOT_API_SSP_PAYLOAD_PROTOCOL_VERSION_TO_MATCH
+	 * ie : 0x00000001 : version 1 of SSP Payload
+	 */
+	uint32_t payload_protocol_version;
+
+	/*
+	 * OEM_ECDSA_PUBK Public Key defined by OEM
+	 * 64 bytes = 512 bits
+	 */
+	uint8_t  oem_ecdsa_pubk[BOOT_API_SSP_PUBK_KEY_SIZE_BYTES];
+
+	/*
+	 * Size of Table of OEM Secrets encrypted with AES-GCM (Key,IV) from
+	 * License field 'encrypted_iv_and_key[]'
+	 * should be <= BOOT_API_SSP_OEM_SECRETS_MAX_SIZE_BYTES:
+	 * is verified by bootROM.
+	 */
+	uint32_t oem_secret_size_bytes;
+
+	/*
+	 * AUTH_TAG AES-GCM computed by HSM-OEM when encrypting OEM Secrets with
+	 * (Key,IV) using special AAD scheme for Payload.
+	 * 16 bytes = 128 bits
+	 */
+	uint8_t  payload_tag[BOOT_API_SSP_AES_GCM_AUTH_TAG_SIZE_BYTES];
+
+	/*
+	 * OEM Secrets encrypted with AES-GCM (IV, Key) from
+	 * License field 'encrypted_iv_and_key[]'.
+	 * The payload size is 'oem_secret_size_bytes'
+	 * should be <= BOOT_API_SSP_OEM_SECRETS_MAX_SIZE_BYTES =
+	 *  152 bytes : OEM Secrets max size in bytes :
+	 * [OTP_CFG56, OTP_CFG59, OTP_CFG60..95] x 4 bytes by OTP word.
+	 */
+	uint8_t oem_encrypted_secrets[BOOT_API_SSP_OEM_SECRETS_MAX_SIZE_BYTES];
+
+} boot_api_ssp_blob_payload_t;
+#endif
+
+/* SSP Configuration structure */
+typedef struct {
+	/* SSP Command*/
+	uint32_t ssp_cmd;
+#if STM32MP_SSP
+	/* ECIES chip public key */
+	uint8_t *p_chip_pubk;
+	/* Blob License Address */
+	boot_api_ssp_blob_license_t *p_blob_license;
+	/* Blob Payload Address */
+	boot_api_ssp_blob_payload_t *p_blob_payload;
+	/* Secrets Decrypted Address */
+	uint8_t *p_ssp_oem_secrets_decrypted;
+	/* Reserved for Future Use (RFU) */
+	uint32_t padding_rfu;
+#else
+	uint8_t	reserved[20];
+#endif
+} boot_api_ssp_config_t;
+
+/*
+ * bootROM version information structure definition
+ * Total size = 24 bytes = 6 uint32_t
+ */
+typedef struct {
+	/* Chip Version */
+	uint32_t chip_ver;
+
+	/* Cut version within a fixed chip version */
+	uint32_t cut_ver;
+
+	/* Version of ROM Mask within a fixed cut version */
+	uint32_t rom_mask_ver;
+
+	/* Internal Version of bootROM code */
+	uint32_t bootrom_ver;
+
+	/* Version of bootROM adapted */
+	uint32_t for_chip_design_rtl_ver;
+
+	/* Restriction on compiled platform when it applies */
+	uint32_t platform_type_ver;
+
+} boot_api_rom_version_info_t;
+
 /*
  * Boot Context related definitions
  */
@@ -153,26 +652,38 @@ typedef struct {
 	uint16_t boot_interface_instance;
 	uint32_t reserved1[13];
 	uint32_t otp_afmux_values[3];
-	uint32_t reserved[5];
+	uint32_t reserved[2];
+	/*
+	 * Log to boot context, what was the kind of boot action
+	 * takes values from defines BOOT_API_BOOT_ACTION_XXX above
+	 */
+	uint32_t boot_action;
+	/*
+	 * STANDBY Exit status to be checked by FSBL in case
+	 * field 'boot_action' == BOOT_API_CTX_BOOT_ACTION_WAKEUP_STANDBY
+	 * take values from defines above 'BOOT_API_CTX_STBY_EXIT_STATUS_XXX'
+	 * depending on encountered situation
+	 */
+	uint32_t stby_exit_status;
+	/*
+	 * CSTANDBY Exit status to be checked by FSBL in case
+	 * boot_action == BOOT_API_CTX_BOOT_ACTION_WAKEUP_CSTANDBY
+	 * take values from defines above 'BOOT_API_CTX_CSTBY_EXIT_STATUS_XXX'
+	 * depending on encountered situation
+	 */
+	uint32_t cstby_exit_status;
 	uint32_t auth_status;
 
 	/*
 	 * Pointers to bootROM External Secure Services
-	 * - ECDSA check key
 	 * - ECDSA verify signature
-	 * - ECDSA verify signature and go
 	 */
-	uint32_t (*bootrom_ecdsa_check_key)(uint8_t *pubkey_in,
-					    uint8_t *pubkey_out);
+	uint32_t reserved3;
 	uint32_t (*bootrom_ecdsa_verify_signature)(uint8_t *hash_in,
 						   uint8_t *pubkey_in,
 						   uint8_t *signature,
 						   uint32_t ecc_algo);
-	uint32_t (*bootrom_ecdsa_verify_and_go)(uint8_t *hash_in,
-						uint8_t *pub_key_in,
-						uint8_t *signature,
-						uint32_t ecc_algo,
-						uint32_t *entry_in);
+	uint32_t reserved4;
 
 	/*
 	 * Information specific to an SD boot
@@ -203,6 +714,21 @@ typedef struct {
 	 * ie FSBL partition on which the boot was successful
 	 */
 	uint32_t boot_partition_used_toboot;
+	/*
+	 * Address of SSP configuration structure :
+	 * given and defined by bootROM
+	 * and used by FSBL. The structure is of type
+	 * 'boot_api_ssp_config_t'
+	 */
+	boot_api_ssp_config_t *p_ssp_config;
+	/*
+	 * boot context field containing bootROM updated SSP Status
+	 * Values can be of type BOOT_API_CTX_SSP_STATUS_XXX
+	 */
+	uint32_t	ssp_status;
+
+	/* Pointer on ROM constant containing ROM information */
+	const boot_api_rom_version_info_t *p_rom_version_info;
 
 } __packed boot_api_context_t;
 
diff --git a/plat/st/stm32mp1/include/platform_def.h b/plat/st/stm32mp1/include/platform_def.h
index 7076a7105f..4434afa172 100644
--- a/plat/st/stm32mp1/include/platform_def.h
+++ b/plat/st/stm32mp1/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -25,18 +25,22 @@
 #define PLATFORM_STACK_SIZE		0xC00
 #endif
 
+#if STM32MP_USE_STM32IMAGE
 #ifdef AARCH32_SP_OPTEE
 #define OPTEE_HEADER_IMAGE_NAME		"teeh"
+#define OPTEE_CORE_IMAGE_NAME		"teex"
 #define OPTEE_PAGED_IMAGE_NAME		"teed"
-#define OPTEE_PAGER_IMAGE_NAME		"teex"
 #define OPTEE_HEADER_BINARY_TYPE	U(0x20)
-#define OPTEE_PAGER_BINARY_TYPE		U(0x21)
+#define OPTEE_CORE_BINARY_TYPE		U(0x21)
 #define OPTEE_PAGED_BINARY_TYPE		U(0x22)
 #endif
 
 /* SSBL = second stage boot loader */
 #define BL33_IMAGE_NAME			"ssbl"
 #define BL33_BINARY_TYPE		U(0x0)
+#else /* STM32MP_USE_STM32IMAGE */
+#define FIP_IMAGE_NAME			"fip"
+#endif /* STM32MP_USE_STM32IMAGE */
 
 #define STM32MP_PRIMARY_CPU		U(0x0)
 #define STM32MP_SECONDARY_CPU		U(0x1)
@@ -64,14 +68,26 @@
 #define BL2_LIMIT			(STM32MP_BL2_BASE + \
 					 STM32MP_BL2_SIZE)
 
+#define BL2_RO_BASE			STM32MP_BL2_RO_BASE
+#define BL2_RO_LIMIT			(STM32MP_BL2_RO_BASE + \
+					 STM32MP_BL2_RO_SIZE)
+
+#define BL2_RW_BASE			STM32MP_BL2_RW_BASE
+#define BL2_RW_LIMIT			(STM32MP_BL2_RW_BASE + \
+					 STM32MP_BL2_RW_SIZE)
 /*******************************************************************************
  * BL32 specific defines.
  ******************************************************************************/
-#ifndef AARCH32_SP_OPTEE
+#if STM32MP_USE_STM32IMAGE || defined(IMAGE_BL32)
+#if ENABLE_PIE
+#define BL32_BASE			0
+#define BL32_LIMIT			STM32MP_BL32_SIZE
+#else
 #define BL32_BASE			STM32MP_BL32_BASE
 #define BL32_LIMIT			(STM32MP_BL32_BASE + \
 					 STM32MP_BL32_SIZE)
 #endif
+#endif
 
 /*******************************************************************************
  * BL33 specific defines.
@@ -83,6 +99,12 @@
  */
 #define PLAT_STM32MP_NS_IMAGE_OFFSET	BL33_BASE
 
+/* Needed by STM32CubeProgrammer support */
+#define FLASHLAYOUT_BASE		STM32MP_DDR_BASE
+#define FLASHLAYOUT_SIZE		0x00100000
+#define DWL_BUFFER_BASE			(STM32MP_DDR_BASE + 0x08000000)
+#define DWL_BUFFER_SIZE			0x08000000
+
 /*******************************************************************************
  * DTB specific defines.
  ******************************************************************************/
@@ -113,6 +135,8 @@
  */
 #define ARM_IRQ_SEC_PHY_TIMER		U(29)
 
+#define ARM_IRQ_NON_SEC_SGI_0		U(0)
+
 #define ARM_IRQ_SEC_SGI_0		U(8)
 #define ARM_IRQ_SEC_SGI_1		U(9)
 #define ARM_IRQ_SEC_SGI_2		U(10)
@@ -122,7 +146,15 @@
 #define ARM_IRQ_SEC_SGI_6		U(14)
 #define ARM_IRQ_SEC_SGI_7		U(15)
 
+/* Platform IRQ Priority */
+#define STM32MP1_IRQ_RCC_SEC_PRIO	U(0x6)
+#define STM32MP_IRQ_SEC_SPI_PRIO	U(0x10)
+
 #define STM32MP1_IRQ_TZC400		U(36)
+#define STM32MP1_IRQ_MCU_SEV		U(176)
+#define STM32MP1_IRQ_RCC_WAKEUP		U(177)
+#define STM32MP1_IRQ_IWDG1		U(182)
+#define STM32MP1_IRQ_IWDG2		U(183)
 #define STM32MP1_IRQ_TAMPSERRS		U(229)
 #define STM32MP1_IRQ_AXIERRIRQ		U(244)
 
diff --git a/plat/st/stm32mp1/include/stm32mp15_mbedtls_config.h b/plat/st/stm32mp1/include/stm32mp15_mbedtls_config.h
new file mode 100644
index 0000000000..0023fecd94
--- /dev/null
+++ b/plat/st/stm32mp1/include/stm32mp15_mbedtls_config.h
@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) 2015-2020, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef MBEDTLS_CONFIG_H
+#define MBEDTLS_CONFIG_H
+
+/*
+ * Key algorithms currently supported on mbed TLS libraries
+ */
+#define TF_MBEDTLS_USE_RSA	0
+#define TF_MBEDTLS_USE_ECDSA	1
+
+/*
+ * Hash algorithms currently supported on mbed TLS libraries
+ */
+#define TF_MBEDTLS_SHA256		1
+#define TF_MBEDTLS_SHA384		2
+#define TF_MBEDTLS_SHA512		3
+
+/*
+ * Configuration file to build mbed TLS with the required features for
+ * Trusted Boot
+ */
+
+#define MBEDTLS_PLATFORM_MEMORY
+#define MBEDTLS_PLATFORM_NO_STD_FUNCTIONS
+/* Prevent mbed TLS from using snprintf so that it can use tf_snprintf. */
+#define MBEDTLS_PLATFORM_SNPRINTF_ALT
+
+#define MBEDTLS_PKCS1_V21
+
+#define MBEDTLS_X509_ALLOW_UNSUPPORTED_CRITICAL_EXTENSION
+#define MBEDTLS_X509_CHECK_KEY_USAGE
+#define MBEDTLS_X509_CHECK_EXTENDED_KEY_USAGE
+
+#define MBEDTLS_ASN1_PARSE_C
+#define MBEDTLS_ASN1_WRITE_C
+
+#define MBEDTLS_BASE64_C
+#define MBEDTLS_BIGNUM_C
+
+#define MBEDTLS_ERROR_C
+#define MBEDTLS_MD_C
+
+#define MBEDTLS_MEMORY_BUFFER_ALLOC_C
+#define MBEDTLS_OID_C
+
+#define MBEDTLS_PK_C
+#define MBEDTLS_PK_PARSE_C
+#define MBEDTLS_PK_WRITE_C
+
+#define MBEDTLS_PLATFORM_C
+
+#if TF_MBEDTLS_USE_ECDSA
+#define MBEDTLS_ECDSA_C
+#define MBEDTLS_ECP_C
+#define MBEDTLS_ECP_DP_SECP256R1_ENABLED
+#define MBEDTLS_ECP_NO_INTERNAL_RNG
+#endif
+#if TF_MBEDTLS_USE_RSA
+#define MBEDTLS_RSA_C
+#define MBEDTLS_X509_RSASSA_PSS_SUPPORT
+#endif
+
+#define MBEDTLS_SHA256_C
+#if (TF_MBEDTLS_HASH_ALG_ID != TF_MBEDTLS_SHA256)
+#define MBEDTLS_SHA512_C
+#endif
+
+#define MBEDTLS_VERSION_C
+
+#define MBEDTLS_X509_USE_C
+#define MBEDTLS_X509_CRT_PARSE_C
+
+#if TF_MBEDTLS_USE_AES_GCM
+#define MBEDTLS_AES_C
+#define MBEDTLS_CIPHER_C
+#define MBEDTLS_GCM_C
+#endif
+
+/* MPI / BIGNUM options */
+#define MBEDTLS_MPI_WINDOW_SIZE			2
+
+#if TF_MBEDTLS_USE_RSA
+#if TF_MBEDTLS_KEY_SIZE <= 2048
+#define MBEDTLS_MPI_MAX_SIZE			256
+#else
+#define MBEDTLS_MPI_MAX_SIZE			512
+#endif
+#else
+#define MBEDTLS_MPI_MAX_SIZE			256
+#endif
+
+/* Memory buffer allocator options */
+#define MBEDTLS_MEMORY_ALIGN_MULTIPLE		8
+
+/*
+ * Prevent the use of 128-bit division which
+ * creates dependency on external libraries.
+ */
+#define MBEDTLS_NO_UDBL_DIVISION
+
+#ifndef __ASSEMBLER__
+/* System headers required to build mbed TLS with the current configuration */
+#include <stdlib.h>
+#include <mbedtls/check_config.h>
+#endif
+
+/*
+ * Mbed TLS heap size is smal as we only use the asn1
+ * parsing functions
+ * digest, signature and crypto algorithm are done by
+ * other library.
+ */
+
+#define TF_MBEDTLS_HEAP_SIZE           U(5120)
+#endif /* MBEDTLS_CONFIG_H */
diff --git a/plat/st/stm32mp1/include/stm32mp1_context.h b/plat/st/stm32mp1/include/stm32mp1_context.h
index 698415af2c..2c035cdca4 100644
--- a/plat/st/stm32mp1/include/stm32mp1_context.h
+++ b/plat/st/stm32mp1/include/stm32mp1_context.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,8 +7,33 @@
 #ifndef STM32MP1_CONTEXT_H
 #define STM32MP1_CONTEXT_H
 
+#include <stdbool.h>
 #include <stdint.h>
 
-int stm32_save_boot_interface(uint32_t interface, uint32_t instance);
+#include <drivers/st/stm32_rtc.h>
+
+#define DDR_CRC_GRANULE		32
+
+void stm32_clean_context(void);
+int stm32_save_context(uint32_t zq0cr0_zdata,
+		       struct stm32_rtc_calendar *rtc_time,
+		       unsigned long long stgen_cnt);
+int stm32_restore_context(void);
+unsigned long long stm32_get_stgen_from_context(void);
+int stm32_restore_backup_reg(void);
+void stm32_context_get_bl2_low_power_params(uintptr_t *bl2_code_base,
+					    uintptr_t *bl2_code_end,
+					    uintptr_t *bl2_end);
+void stm32_context_save_bl2_param(void);
+uint32_t stm32_get_zdata_from_context(void);
+int stm32_get_pll1_settings_from_context(void);
+bool stm32_are_pll1_settings_valid_in_context(void);
+bool stm32_pm_context_is_valid(void);
+void stm32_save_ddr_training_area(void);
+void stm32_restore_ddr_training_area(void);
+uint32_t stm32_pm_get_optee_ep(void);
+
+void stm32mp1_pm_save_clock_cfg(size_t offset, uint8_t *data, size_t size);
+void stm32mp1_pm_restore_clock_cfg(size_t offset, uint8_t *data, size_t size);
 
 #endif /* STM32MP1_CONTEXT_H */
diff --git a/plat/st/stm32mp1/include/stm32mp1_critic_power.h b/plat/st/stm32mp1/include/stm32mp1_critic_power.h
new file mode 100644
index 0000000000..cd7099c444
--- /dev/null
+++ b/plat/st/stm32mp1/include/stm32mp1_critic_power.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) 2019-2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef STM32MP1_CRITIC_POWER_H
+#define STM32MP1_CRITIC_POWER_H
+
+ /* Only BL32 compilation unit need stm32_pwr_down_wfi
+  * function/variable symbol
+  */
+#if defined(IMAGE_BL32)
+ #if STM32MP_SP_MIN_IN_DDR
+extern void (*stm32_pwr_down_wfi)(bool is_cstop);
+ #else
+extern void stm32_pwr_down_wfi(bool is_cstop);
+ #endif
+#endif
+extern void stm32_pwr_down_wfi_wrapper(bool is_cstop);
+
+#endif /* STM32MP1_CRITIC_POWER_H */
diff --git a/plat/st/stm32mp1/include/stm32mp1_low_power.h b/plat/st/stm32mp1/include/stm32mp1_low_power.h
new file mode 100644
index 0000000000..79ae3bfc92
--- /dev/null
+++ b/plat/st/stm32mp1/include/stm32mp1_low_power.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef STM32MP1_LOW_POWER_H
+#define STM32MP1_LOW_POWER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <stm32mp1_critic_power.h>
+
+void stm32_rcc_wakeup_update(bool state);
+void stm32_apply_pmic_suspend_config(uint32_t mode);
+bool stm32_is_cstop_done(void);
+void stm32_exit_cstop(void);
+void stm32_enter_low_power(uint32_t mode, uint32_t nsec_addr);
+void stm32_auto_stop(void);
+void stm32_init_low_power(void);
+
+#endif /* STM32MP1_LOW_POWER_H */
diff --git a/plat/st/stm32mp1/include/stm32mp1_power_config.h b/plat/st/stm32mp1/include/stm32mp1_power_config.h
new file mode 100644
index 0000000000..37312c8de6
--- /dev/null
+++ b/plat/st/stm32mp1/include/stm32mp1_power_config.h
@@ -0,0 +1,29 @@
+/*
+ * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef STM32MP1_POWER_CONFIG_H
+#define STM32MP1_POWER_CONFIG_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define PSCI_MODE_SYSTEM_SUSPEND	0
+#define PSCI_MODE_SYSTEM_OFF		1
+
+enum stm32mp1_pm_domain {
+	STM32MP1_PD_VSW,
+	STM32MP1_PD_CORE_RET,
+	STM32MP1_PD_CORE,
+	STM32MP1_PD_MAX_PM_DOMAIN
+};
+
+void stm32mp1_init_lp_states(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);
+bool stm32mp1_get_retram_enabled(void);
+
+#endif /* STM32MP1_POWER_CONFIG_H */
diff --git a/plat/st/stm32mp1/include/stm32mp1_private.h b/plat/st/stm32mp1/include/stm32mp1_private.h
index b6cb91efa0..ebf9e52bc9 100644
--- a/plat/st/stm32mp1/include/stm32mp1_private.h
+++ b/plat/st/stm32mp1/include/stm32mp1_private.h
@@ -9,19 +9,46 @@
 
 #include <stdint.h>
 
+#include <drivers/st/etzpc.h>
+
+enum boot_device_e {
+	BOOT_DEVICE_USB,
+	BOOT_DEVICE_BOARD
+};
+
 void configure_mmu(void);
 
+void stm32mp_mask_timer(void);
+void __dead2 stm32mp_wait_cpu_reset(void);
+
 void stm32mp1_arch_security_setup(void);
 void stm32mp1_security_setup(void);
 
-void stm32mp1_gic_pcpu_init(void);
-void stm32mp1_gic_init(void);
+enum boot_device_e get_boot_device(void);
+
+bool stm32mp1_addr_inside_backupsram(uintptr_t addr);
+bool stm32mp1_is_wakeup_from_standby(void);
+
+int stm32_save_boot_interface(uint32_t interface, uint32_t instance);
+int stm32_get_boot_interface(uint32_t *interface, uint32_t *instance);
+bool stm32_boot_is_serial(void);
+
+enum etzpc_decprot_attributes stm32mp_etzpc_binding2decprot(uint32_t mode);
 
 void stm32mp1_syscfg_init(void);
-void stm32mp1_syscfg_enable_io_compensation(void);
+void stm32mp1_syscfg_enable_io_compensation_start(void);
+void stm32mp1_syscfg_enable_io_compensation_finish(void);
 void stm32mp1_syscfg_disable_io_compensation(void);
 
+#if STM32MP_USE_STM32IMAGE
 uint32_t stm32mp_get_ddr_ns_size(void);
+#endif
 
 void stm32mp1_init_scmi_server(void);
+void stm32mp1_pm_save_scmi_state(uint8_t *state, size_t size);
+void stm32mp1_pm_restore_scmi_state(uint8_t *state, size_t size);
+
+#if defined(IMAGE_BL32) && DEBUG
+void stm32mp_dump_core_registers(bool fcore);
+#endif
 #endif /* STM32MP1_PRIVATE_H */
diff --git a/plat/st/stm32mp1/include/stm32mp1_shared_resources.h b/plat/st/stm32mp1/include/stm32mp1_shared_resources.h
index 3f6367ebef..519a67bad8 100644
--- a/plat/st/stm32mp1/include/stm32mp1_shared_resources.h
+++ b/plat/st/stm32mp1/include/stm32mp1_shared_resources.h
@@ -7,6 +7,8 @@
 #ifndef STM32MP1_SHARED_RESOURCES_H
 #define STM32MP1_SHARED_RESOURCES_H
 
+#include <drivers/st/etzpc.h>
+
 #include <stm32mp_shared_resources.h>
 
 #define STM32MP1_SHRES_GPIOZ(i)		(STM32MP1_SHRES_GPIOZ_0 + (i))
@@ -35,4 +37,19 @@ enum stm32mp_shres {
 
 	STM32MP1_SHRES_COUNT
 };
+
+#ifdef STM32MP_SHARED_RESOURCES
+/*
+ * Register a (non-)secure peripheral based on the ETZPC DECPROT configuration
+ */
+void stm32mp1_register_etzpc_decprot(unsigned int id,
+				     enum etzpc_decprot_attributes attr);
+#else
+static inline
+void stm32mp1_register_etzpc_decprot(unsigned int id,
+				     enum etzpc_decprot_attributes attr)
+{
+}
+#endif
+
 #endif /* STM32MP1_SHARED_RESOURCES_H */
diff --git a/plat/st/stm32mp1/include/stm32mp1_smc.h b/plat/st/stm32mp1/include/stm32mp1_smc.h
index 57240bcaf3..62210a4dd9 100644
--- a/plat/st/stm32mp1/include/stm32mp1_smc.h
+++ b/plat/st/stm32mp1/include/stm32mp1_smc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2016-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -17,6 +17,39 @@
 
 /* Secure Service access from Non-secure */
 
+/*
+ * SMC function STM32_SMC_RCC.
+ *
+ * Argument a0: (input) SMCC ID.
+ *		(output) Status return code.
+ * Argument a1: (input) Service ID (STM32_SMC_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_SMC_RCC			0x82001000
+
+/*
+ * SMC function STM32_SMC_PWR.
+ *
+ * Argument a0: (input) SMCC ID.
+ *		(output) Status return code.
+ * Argument a1: (input) Service ID (STM32_SMC_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_SMC_PWR			0x82001001
+
+/*
+ * SMC functions STM32_SMC_RCC_CAL.
+ *
+ * Argument a0: (input) SMCC ID.
+ *		(output) Status return code.
+ * Argument a1: (input) Clock ID (from DT clock bindings).
+ */
+#define STM32_SMC_RCC_CAL		0x82001002
+
 /*
  * STM32_SMC_BSEC call API
  *
@@ -29,6 +62,37 @@
  */
 #define STM32_SMC_BSEC			0x82001003
 
+/* Low Power services */
+
+/*
+ * SIP function STM32_SMC_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 enable target domain.
+ */
+#define STM32_SMC_PD_DOMAIN		0x82001008
+
+/*
+ * SIP function STM32_SMC_RCC_OPP.
+ *
+ * Argument a0: (input) SMCC ID.
+ *		(output) Status return code.
+ * Argument a1: (input) Service ID (STM32_SMC_RCC_OPP_xxx).
+ *		(output) Rounded frequency, if applicable.
+ * Argument a2: (input) Requested frequency.
+ */
+#define STM32_SMC_RCC_OPP		0x82001009
+
+/*
+ * SIP function STM32_SMC_AUTO_STOP - CPU auto stop for OS driver suspend
+ *
+ * Argument a0: (input) This SMCC ID: STM32_SMC_AUTO_STOP
+ *		(output) Status return code.
+ */
+#define STM32_SMC_AUTO_STOP		0x8200100a
+
 /*
  * STM32_SIP_SMC_SCMI_AGENT0
  * STM32_SIP_SMC_SCMI_AGENT1
@@ -50,12 +114,31 @@
 #define STM32_SIP_SVC_VERSION_MINOR	0x1
 
 /* Number of STM32 SiP Calls implemented */
-#define STM32_COMMON_SIP_NUM_CALLS	3
+#define STM32_COMMON_SIP_NUM_CALLS	9
+
+/* Service ID for STM32_SMC_RCC/_PWR */
+#define STM32_SMC_REG_READ		0x0
+#define STM32_SMC_REG_WRITE		0x1
+#define STM32_SMC_REG_SET		0x2
+#define STM32_SMC_REG_CLEAR		0x3
 
 /* Service for BSEC */
 #define STM32_SMC_READ_SHADOW		0x01
 #define STM32_SMC_PROG_OTP		0x02
 #define STM32_SMC_WRITE_SHADOW		0x03
 #define STM32_SMC_READ_OTP		0x04
+#define STM32_SMC_READ_ALL		0x05
+#define STM32_SMC_WRITE_ALL		0x06
+#define STM32_SMC_WRLOCK_OTP		0x07
+
+/* SMC error codes */
+#define STM32_SMC_OK			0x00000000U
+#define STM32_SMC_NOT_SUPPORTED		0xFFFFFFFFU
+#define STM32_SMC_FAILED		0xFFFFFFFEU
+#define STM32_SMC_INVALID_PARAMS	0xFFFFFFFDU
+
+/* Service ID for STM32_SMC_RCC_OPP */
+#define STM32_SMC_RCC_OPP_SET		0x0
+#define STM32_SMC_RCC_OPP_ROUND		0x1
 
 #endif /* STM32MP1_SMC_H */
diff --git a/plat/st/stm32mp1/plat_bl2_mem_params_desc.c b/plat/st/stm32mp1/plat_bl2_mem_params_desc.c
index 1d407bb72b..d322da0090 100644
--- a/plat/st/stm32mp1/plat_bl2_mem_params_desc.c
+++ b/plat/st/stm32mp1/plat_bl2_mem_params_desc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,6 +19,22 @@
  * the next executable image id.
  ******************************************************************************/
 static bl_mem_params_node_t bl2_mem_params_descs[] = {
+	/* Fill FW_CONFIG related information if it exists */
+	{
+	    .image_id = FW_CONFIG_ID,
+	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+				  VERSION_2, entry_point_info_t,
+				  SECURE | NON_EXECUTABLE),
+	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+				  VERSION_2, image_info_t,
+				  IMAGE_ATTRIB_PLAT_SETUP),
+
+	    .image_info.image_base = STM32MP_FW_CONFIG_BASE,
+	    .image_info.image_max_size = STM32MP_FW_CONFIG_MAX_SIZE,
+
+	    .next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+
 	/* Fill BL32 related information */
 	{
 		.image_id = BL32_IMAGE_ID,
@@ -27,28 +43,17 @@ static bl_mem_params_node_t bl2_mem_params_descs[] = {
 				      VERSION_2, entry_point_info_t,
 				      SECURE | EXECUTABLE | EP_FIRST_EXE),
 
-#if !defined(AARCH32_SP_OPTEE)
-		.ep_info.pc = BL32_BASE,
-#endif
 		.ep_info.spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM,
 					    SPSR_E_LITTLE,
 					    DISABLE_ALL_EXCEPTIONS),
 
 		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
 				      VERSION_2, image_info_t,
-				      IMAGE_ATTRIB_PLAT_SETUP),
-#if defined(AARCH32_SP_OPTEE)
-		/* optee header is loaded in SYSRAM above BL2 */
-		.image_info.image_base = STM32MP_OPTEE_BASE,
-		.image_info.image_max_size = STM32MP_OPTEE_SIZE,
-#else
-		.image_info.image_base = BL32_BASE,
-		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
-#endif
+				      IMAGE_ATTRIB_SKIP_LOADING),
+
 		.next_handoff_image_id = BL33_IMAGE_ID,
 	},
 
-#if defined(AARCH32_SP_OPTEE)
 	/* Fill BL32 external 1 image related information */
 	{
 		.image_id = BL32_EXTRA1_IMAGE_ID,
@@ -77,7 +82,31 @@ static bl_mem_params_node_t bl2_mem_params_descs[] = {
 
 		.next_handoff_image_id = INVALID_IMAGE_ID,
 	},
-#endif /* AARCH32_SP_OPTEE */
+
+	/* Fill HW_CONFIG related information if it exists */
+	{
+	    .image_id = HW_CONFIG_ID,
+	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+				  VERSION_2, entry_point_info_t,
+				  NON_SECURE | NON_EXECUTABLE),
+	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+				  VERSION_2, image_info_t,
+				  IMAGE_ATTRIB_SKIP_LOADING),
+
+	    .next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+
+	{
+	    .image_id = TOS_FW_CONFIG_ID,
+	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+				  VERSION_2, entry_point_info_t,
+				  SECURE | NON_EXECUTABLE),
+	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+				  VERSION_2, image_info_t,
+				  IMAGE_ATTRIB_SKIP_LOADING),
+
+	    .next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 
 	/* Fill BL33 related information */
 	{
@@ -87,17 +116,17 @@ static bl_mem_params_node_t bl2_mem_params_descs[] = {
 				      VERSION_2, entry_point_info_t,
 				      NON_SECURE | EXECUTABLE),
 
-		.ep_info.pc = PLAT_STM32MP_NS_IMAGE_OFFSET,
+#if BL33_HYP
+		.ep_info.spsr = SPSR_MODE32(MODE32_hyp, SPSR_T_ARM,
+#else
 		.ep_info.spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM,
+#endif
 					    SPSR_E_LITTLE,
 					    DISABLE_ALL_EXCEPTIONS),
 
 		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-				      VERSION_2, image_info_t, 0),
-
-		.image_info.image_base = PLAT_STM32MP_NS_IMAGE_OFFSET,
-		.image_info.image_max_size = STM32MP_DDR_MAX_SIZE -
-			(PLAT_STM32MP_NS_IMAGE_OFFSET - STM32MP_DDR_BASE),
+				      VERSION_2, image_info_t,
+				      IMAGE_ATTRIB_SKIP_LOADING),
 
 		.next_handoff_image_id = INVALID_IMAGE_ID,
 	}
diff --git a/plat/st/stm32mp1/plat_bl2_stm32_mem_params_desc.c b/plat/st/stm32mp1/plat_bl2_stm32_mem_params_desc.c
new file mode 100644
index 0000000000..2d1332ee89
--- /dev/null
+++ b/plat/st/stm32mp1/plat_bl2_stm32_mem_params_desc.c
@@ -0,0 +1,111 @@
+/*
+ * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <platform_def.h>
+
+#include <common/bl_common.h>
+#include <common/desc_image_load.h>
+#include <plat/common/platform.h>
+
+/*******************************************************************************
+ * Following descriptor provides BL image/ep information that gets used
+ * by BL2 to load the images and also subset of this information is
+ * passed to next BL image. The image loading sequence is managed by
+ * populating the images in required loading order. The image execution
+ * sequence is managed by populating the `next_handoff_image_id` with
+ * the next executable image id.
+ ******************************************************************************/
+static bl_mem_params_node_t bl2_mem_params_descs[] = {
+	/* Fill BL32 related information */
+	{
+		.image_id = BL32_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+				      VERSION_2, entry_point_info_t,
+				      SECURE | EXECUTABLE | EP_FIRST_EXE),
+
+#if !defined(AARCH32_SP_OPTEE)
+		.ep_info.pc = STM32MP_BL32_BASE,
+#endif
+		.ep_info.spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM,
+					    SPSR_E_LITTLE,
+					    DISABLE_ALL_EXCEPTIONS),
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+				      VERSION_2, image_info_t,
+				      IMAGE_ATTRIB_PLAT_SETUP),
+#if defined(AARCH32_SP_OPTEE)
+		/* optee header is loaded in SYSRAM above BL2 */
+		.image_info.image_base = STM32MP_OPTEE_BASE,
+		.image_info.image_max_size = STM32MP_OPTEE_SIZE,
+#else
+		.image_info.image_base = STM32MP_BL32_BASE,
+		.image_info.image_max_size = STM32MP_BL32_BIN_SIZE,
+#endif
+		.next_handoff_image_id = BL33_IMAGE_ID,
+	},
+
+	/* Fill BL32 external 1 image related information */
+	{
+		.image_id = BL32_EXTRA1_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+				      VERSION_2, entry_point_info_t,
+				      SECURE | NON_EXECUTABLE),
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+				      VERSION_2, image_info_t,
+				      IMAGE_ATTRIB_SKIP_LOADING),
+
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+
+	/* Fill BL32 external 2 image related information */
+	{
+		.image_id = BL32_EXTRA2_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+				      VERSION_2, entry_point_info_t,
+				      SECURE | NON_EXECUTABLE),
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+				      VERSION_2, image_info_t,
+				      IMAGE_ATTRIB_SKIP_LOADING),
+
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+
+	/* Fill BL33 related information */
+	{
+		.image_id = BL33_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+				      VERSION_2, entry_point_info_t,
+				      NON_SECURE | EXECUTABLE),
+
+		.ep_info.pc = PLAT_STM32MP_NS_IMAGE_OFFSET,
+
+#if BL33_HYP
+		.ep_info.spsr = SPSR_MODE32(MODE32_hyp, SPSR_T_ARM,
+#else
+		.ep_info.spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM,
+#endif
+					    SPSR_E_LITTLE,
+					    DISABLE_ALL_EXCEPTIONS),
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+				      VERSION_2, image_info_t,
+				      0),
+
+		.image_info.image_base = PLAT_STM32MP_NS_IMAGE_OFFSET,
+		.image_info.image_max_size = STM32MP_DDR_MAX_SIZE -
+			(PLAT_STM32MP_NS_IMAGE_OFFSET - STM32MP_DDR_BASE),
+
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	}
+};
+
+REGISTER_BL_IMAGE_DESCS(bl2_mem_params_descs)
diff --git a/plat/st/stm32mp1/plat_image_load.c b/plat/st/stm32mp1/plat_image_load.c
index 6d7af741a0..c6a30de5bb 100644
--- a/plat/st/stm32mp1/plat_image_load.c
+++ b/plat/st/stm32mp1/plat_image_load.c
@@ -6,7 +6,10 @@
 
 #include <platform_def.h>
 
+#include <common/bl_common.h>
 #include <common/desc_image_load.h>
+#include <drivers/clk.h>
+#include <lib/mmio.h>
 #include <plat/common/platform.h>
 
 /*******************************************************************************
@@ -23,12 +26,38 @@ void plat_flush_next_bl_params(void)
  ******************************************************************************/
 bl_load_info_t *plat_get_bl_image_load_info(void)
 {
+#if STM32MP_USE_STM32IMAGE
 	bl_mem_params_node_t *bl33 = get_bl_mem_params_node(BL33_IMAGE_ID);
 	uint32_t ddr_ns_size = stm32mp_get_ddr_ns_size();
 
+	/*
+	 * If going back from CSTANDBY / STANDBY and DDR was in Self-Refresh,
+	 * BL33 must not be loaded as it would overwrite the code already
+	 * in DDR. For this, the BL33 part of the bl_mem_params_desc_ptr
+	 * struct should be modified to skip its loading
+	 */
+	if (stm32mp1_is_wakeup_from_standby()) {
+		bl_mem_params_node_t *bl32;
+
+		bl33->image_info.h.attr |= IMAGE_ATTRIB_SKIP_LOADING;
+		bl32 = get_bl_mem_params_node(BL32_IMAGE_ID);
+		bl32->image_info.h.attr |= IMAGE_ATTRIB_SKIP_LOADING;
+#if defined(AARCH32_SP_OPTEE)
+		bl32->ep_info.pc = stm32_pm_get_optee_ep();
+
+		if (stm32mp1_addr_inside_backupsram(bl32->ep_info.pc)) {
+			clk_enable(BKPSRAM);
+		}
+#else
+		/* Set ep_info PC to 0, to inform BL32 it is a reset after STANDBY */
+		bl33->ep_info.pc = 0;
+#endif
+	}
+
 	/* Max size is non-secure DDR end address minus image_base */
 	bl33->image_info.image_max_size = STM32MP_DDR_BASE + ddr_ns_size -
 					  bl33->image_info.image_base;
+#endif /* STM32MP_USE_STM32IMAGE */
 
 	return get_bl_load_info_from_mem_params_desc();
 }
@@ -38,5 +67,9 @@ bl_load_info_t *plat_get_bl_image_load_info(void)
  ******************************************************************************/
 bl_params_t *plat_get_next_bl_params(void)
 {
-	return get_next_bl_params_from_mem_params_desc();
+	bl_params_t *bl_params = get_next_bl_params_from_mem_params_desc();
+
+	populate_next_bl_params_config(bl_params);
+
+	return bl_params;
 }
diff --git a/plat/st/stm32mp1/platform.mk b/plat/st/stm32mp1/platform.mk
index 359581925a..067e1fff9f 100644
--- a/plat/st/stm32mp1/platform.mk
+++ b/plat/st/stm32mp1/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -9,26 +9,68 @@ ARM_WITH_NEON		:=	yes
 BL2_AT_EL3		:=	1
 USE_COHERENT_MEM	:=	0
 
+# Allow TF-A to concatenate BL2 & BL32 binaries in a single file,
+# share DTB file between BL2 and BL32
+# If it is set to 0, then FIP and FCONF are used
+STM32MP_USE_STM32IMAGE	?=	0
+
+# Add specific ST version
+ST_VERSION 		:=	r1.0
+ifeq ($(STM32MP_USE_STM32IMAGE),1)
+ST_VERSION 		:=	${ST_VERSION}-nofip
+endif
+VERSION_STRING		:=	v${VERSION_MAJOR}.${VERSION_MINOR}-${ST_VERSION}(${BUILD_TYPE}):${BUILD_STRING}
+
+ifneq ($(STM32MP_USE_STM32IMAGE),1)
+ENABLE_PIE		:=	1
+endif
+TRUSTED_BOARD_BOOT	?=	0
+STM32MP_USE_EXTERNAL_HEAP ?=	0
+
+# Please don't increment this value without good understanding of
+# the monotonic counter
 STM32_TF_VERSION	?=	0
 
 # Enable dynamic memory mapping
 PLAT_XLAT_TABLES_DYNAMIC :=	1
 
+ifeq ($(STM32MP_USE_STM32IMAGE),1)
+BL2_IN_XIP_MEM 		:= 	0
+else
+BL2_IN_XIP_MEM 		:= 	1
+endif
+
+# DDR controller with dual AXI port
+STM32MP_DDR_DUAL_AXI_PORT:= 	1
+
+# STM32 image header version v1.0
+STM32_HEADER_VERSION_MAJOR:=	1
+STM32_HEADER_VERSION_MINOR:=	0
+
+# STM32 Secure Secret Provisioning mode (SSP)
+STM32MP_SSP		?=	0
+
 ifeq ($(AARCH32_SP),sp_min)
 # Disable Neon support: sp_min runtime may conflict with non-secure world
 TF_CFLAGS		+=	-mfloat-abi=soft
 endif
 
+TF_CFLAGS		+=	-Wsign-compare
+
 # Not needed for Cortex-A7
 WORKAROUND_CVE_2017_5715:=	0
 
+AARCH32_EXCEPTION_DEBUG	:=	1
+
 # Number of TF-A copies in the device
 STM32_TF_A_COPIES		:=	2
 STM32_BL33_PARTS_NUM		:=	1
 ifeq ($(AARCH32_SP),optee)
 STM32_RUNTIME_PARTS_NUM		:=	3
-else
+else ifeq ($(STM32MP_USE_STM32IMAGE),1)
 STM32_RUNTIME_PARTS_NUM		:=	0
+else
+STM32_RUNTIME_PARTS_NUM		:=	1
 endif
 PLAT_PARTITION_MAX_ENTRIES	:=	$(shell echo $$(($(STM32_TF_A_COPIES) + \
 							 $(STM32_BL33_PARTS_NUM) + \
@@ -41,14 +83,31 @@ STM32MP_RAW_NAND	?=	0
 STM32MP_SPI_NAND	?=	0
 STM32MP_SPI_NOR		?=	0
 
-ifeq ($(filter 1,${STM32MP_EMMC} ${STM32MP_SDMMC} ${STM32MP_RAW_NAND} \
-	${STM32MP_SPI_NAND} ${STM32MP_SPI_NOR}),)
-$(error "No boot device driver is enabled")
-endif
+# Serial boot devices
+STM32MP_USB_PROGRAMMER	?=	0
+STM32MP_UART_PROGRAMMER	?=	0
+
+# Hypervisor mode
+BL33_HYP			?= 0
 
 # Device tree
 DTB_FILE_NAME		?=	stm32mp157c-ev1.dtb
+ifeq ($(STM32MP_USE_STM32IMAGE),1)
+ifeq ($(AARCH32_SP),optee)
+BL2_DTSI		:=	stm32mp15-bl2.dtsi
+FDT_SOURCES		:=	$(addprefix ${BUILD_PLAT}/fdts/, $(patsubst %.dtb,%-bl2.dts,$(DTB_FILE_NAME)))
+else
 FDT_SOURCES		:=	$(addprefix fdts/, $(patsubst %.dtb,%.dts,$(DTB_FILE_NAME)))
+endif
+else
+BL2_DTSI		:=	stm32mp15-bl2.dtsi
+FDT_SOURCES		:=	$(addprefix ${BUILD_PLAT}/fdts/, $(patsubst %.dtb,%-bl2.dts,$(DTB_FILE_NAME)))
+ifeq ($(AARCH32_SP),sp_min)
+BL32_DTSI		:=	stm32mp15-bl32.dtsi
+FDT_SOURCES		+=	$(addprefix ${BUILD_PLAT}/fdts/, $(patsubst %.dtb,%-bl32.dts,$(DTB_FILE_NAME)))
+endif
+endif
+DTC_CPPFLAGS		+=	${INCLUDES}
 DTC_FLAGS		+=	-Wno-unit_address_vs_reg
 
 # Macros and rules to build TF binary
@@ -66,6 +125,39 @@ endif
 # Variables for use with stm32image
 STM32IMAGEPATH		?= tools/stm32image
 STM32IMAGE		?= ${STM32IMAGEPATH}/stm32image${BIN_EXT}
+STM32IMAGE_SRC		:= ${STM32IMAGEPATH}/stm32image.c
+
+ifneq (${STM32MP_USE_STM32IMAGE},1)
+FIP_DEPS		+=	dtbs
+STM32MP_NT_FW_CONFIG	:=	${BL33_CFG}
+STM32MP_FW_CONFIG_NAME	:=	$(patsubst %.dtb,%-fw-config.dtb,$(DTB_FILE_NAME))
+STM32MP_FW_CONFIG	:=	${BUILD_PLAT}/fdts/$(STM32MP_FW_CONFIG_NAME)
+ifneq (${AARCH32_SP},none)
+FDT_SOURCES		+=	$(addprefix fdts/, $(patsubst %.dtb,%.dts,$(STM32MP_FW_CONFIG_NAME)))
+endif
+# Add the FW_CONFIG to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${STM32MP_FW_CONFIG},--fw-config))
+# Add the NT_FW_CONFIG to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${STM32MP_NT_FW_CONFIG},--hw-config))
+ifeq (${GENERATE_COT},1)
+$(eval $(call TOOL_ADD_PAYLOAD,${BUILD_PLAT}/tb_fw.crt,--tb-fw-cert))
+endif
+$(eval $(call CERT_ADD_CMD_OPT,${BUILD_PLAT}/bl2.bin,--tb-fw))
+CRT_DEPS+=${BUILD_PLAT}/bl2.bin
+ifeq ($(AARCH32_SP),sp_min)
+STM32MP_TOS_FW_CONFIG	:= $(addprefix ${BUILD_PLAT}/fdts/, $(patsubst %.dtb,%-bl32.dtb,$(DTB_FILE_NAME)))
+$(eval $(call TOOL_ADD_PAYLOAD,${STM32MP_TOS_FW_CONFIG},--tos-fw-config))
+else
+# Add the build options to pack Trusted OS Extra1 and Trusted OS Extra2 images
+# in the FIP if the platform requires.
+ifneq ($(BL32_EXTRA1),)
+$(eval $(call TOOL_ADD_IMG,BL32_EXTRA1,--tos-fw-extra1))
+endif
+ifneq ($(BL32_EXTRA2),)
+$(eval $(call TOOL_ADD_IMG,BL32_EXTRA2,--tos-fw-extra2))
+endif
+endif
+endif
 
 # Enable flags for C files
 $(eval $(call assert_booleans,\
@@ -76,12 +168,19 @@ $(eval $(call assert_booleans,\
 		STM32MP_SPI_NAND \
 		STM32MP_SPI_NOR \
 		PLAT_XLAT_TABLES_DYNAMIC \
+		STM32MP_UART_PROGRAMMER \
+		STM32MP_USB_PROGRAMMER \
+		STM32MP_USE_STM32IMAGE \
+		STM32MP_DDR_DUAL_AXI_PORT \
+		STM32MP_SSP \
+		BL33_HYP \
 )))
 
 $(eval $(call assert_numerics,\
 	$(sort \
 		STM32_TF_A_COPIES \
 		PLAT_PARTITION_MAX_ENTRIES \
+		STM32_TF_VERSION \
 )))
 
 $(eval $(call add_defines,\
@@ -94,6 +193,13 @@ $(eval $(call add_defines,\
 		PLAT_XLAT_TABLES_DYNAMIC \
 		STM32_TF_A_COPIES \
 		PLAT_PARTITION_MAX_ENTRIES \
+		STM32MP_UART_PROGRAMMER \
+		STM32MP_USB_PROGRAMMER \
+		STM32_TF_VERSION \
+		STM32MP_USE_STM32IMAGE \
+		STM32MP_DDR_DUAL_AXI_PORT \
+		STM32MP_SSP \
+		BL33_HYP \
 )))
 
 # Include paths and source files
@@ -118,9 +224,10 @@ PLAT_BL_COMMON_SOURCES	+=	${XLAT_TABLES_LIB_SRCS}
 PLAT_BL_COMMON_SOURCES	+=	lib/cpus/aarch32/cortex_a7.S
 
 PLAT_BL_COMMON_SOURCES	+=	drivers/arm/tzc/tzc400.c				\
+				drivers/clk/clk.c					\
 				drivers/delay_timer/delay_timer.c			\
 				drivers/delay_timer/generic_delay_timer.c		\
-				drivers/st/bsec/bsec.c					\
+				drivers/st/bsec/bsec2.c					\
 				drivers/st/clk/stm32mp_clkfunc.c			\
 				drivers/st/clk/stm32mp1_clk.c				\
 				drivers/st/ddr/stm32mp1_ddr_helpers.c			\
@@ -129,24 +236,73 @@ PLAT_BL_COMMON_SOURCES	+=	drivers/arm/tzc/tzc400.c				\
 				drivers/st/iwdg/stm32_iwdg.c				\
 				drivers/st/pmic/stm32mp_pmic.c				\
 				drivers/st/pmic/stpmic1.c				\
+				drivers/st/regulator/stm32mp_dummy_regulator.c		\
+				drivers/st/regulator/stm32mp_regulator.c		\
 				drivers/st/reset/stm32mp1_reset.c			\
 				plat/st/common/stm32mp_dt.c				\
+				plat/st/common/stm32mp_shres_helpers.c			\
 				plat/st/stm32mp1/stm32mp1_context.c			\
 				plat/st/stm32mp1/stm32mp1_dbgmcu.c			\
 				plat/st/stm32mp1/stm32mp1_helper.S			\
-				plat/st/stm32mp1/stm32mp1_security.c			\
 				plat/st/stm32mp1/stm32mp1_syscfg.c
 
+ifneq (${STM32MP_USE_STM32IMAGE},1)
+BL2_SOURCES		+=	drivers/io/io_fip.c					\
+				plat/st/common/bl2_io_storage.c				\
+				plat/st/stm32mp1/plat_bl2_mem_params_desc.c
+
+BL2_SOURCES		+=	lib/fconf/fconf.c					\
+				lib/fconf/fconf_dyn_cfg_getter.c			\
+				plat/st/common/stm32mp_fconf_io.c			\
+				plat/st/stm32mp1/stm32mp1_fconf_firewall.c
+else
+BL2_SOURCES		+=	drivers/io/io_dummy.c					\
+				drivers/st/io/io_stm32image.c				\
+				plat/st/common/bl2_stm32_io_storage.c			\
+				plat/st/stm32mp1/plat_bl2_stm32_mem_params_desc.c	\
+				plat/st/stm32mp1/stm32mp1_security.c
+endif
+
 BL2_SOURCES		+=	drivers/io/io_block.c					\
-				drivers/io/io_dummy.c					\
 				drivers/io/io_mtd.c					\
 				drivers/io/io_storage.c					\
 				drivers/st/crypto/stm32_hash.c				\
-				drivers/st/io/io_stm32image.c				\
-				plat/st/common/stm32mp_auth.c				\
-				plat/st/common/bl2_io_storage.c				\
 				plat/st/stm32mp1/bl2_plat_setup.c
 
+ifeq (${TRUSTED_BOARD_BOOT},1)
+AUTH_SOURCES		:=	drivers/auth/auth_mod.c					\
+				drivers/auth/crypto_mod.c				\
+				drivers/auth/img_parser_mod.c
+
+ifneq (${STM32MP_USE_STM32IMAGE},1)
+ifeq (${GENERATE_COT},1)
+TFW_NVCTR_VAL		:=	0
+NTFW_NVCTR_VAL		:=	0
+KEY_SIZE		:=
+KEY_ALG			:=	ecdsa
+HASH_ALG		:=	sha256
+endif
+TF_MBEDTLS_KEY_ALG 	:=	ecdsa
+MBEDTLS_CONFIG_FILE	?=	"<stm32mp15_mbedtls_config.h>"
+
+include drivers/auth/mbedtls/mbedtls_x509.mk
+
+
+AUTH_SOURCES		+=	drivers/auth/tbbr/tbbr_cot_common.c			\
+				lib/fconf/fconf_tbbr_getter.c				\
+				plat/st/common/stm32mp_crypto_lib.c
+
+BL2_SOURCES		+=	drivers/auth/tbbr/tbbr_cot_bl2.c
+else
+AUTH_SOURCES		+=	plat/st/common/stm32mp_cot.c				\
+				plat/st/common/stm32mp_crypto_lib.c			\
+				plat/st/common/stm32mp_img_parser_lib.c
+endif
+
+BL2_SOURCES		+=	$(AUTH_SOURCES)						\
+				plat/st/common/stm32mp_trusted_boot.c
+endif
+
 ifneq ($(filter 1,${STM32MP_EMMC} ${STM32MP_SDMMC}),)
 BL2_SOURCES		+=	drivers/mmc/mmc.c					\
 				drivers/partition/gpt.c					\
@@ -166,6 +322,10 @@ BL2_SOURCES		+=	drivers/mtd/nand/spi_nand.c
 endif
 
 ifeq (${STM32MP_SPI_NOR},1)
+ifneq (${STM32MP_FORCE_MTD_START_OFFSET},)
+$(eval $(call add_define_val,STM32MP_NOR_FIP_OFFSET,${STM32MP_FORCE_MTD_START_OFFSET}))
+$(eval $(call add_define_val,STM32MP_NOR_BASE_OFFSET,${STM32MP_FORCE_MTD_START_OFFSET}))
+endif
 BL2_SOURCES		+=	drivers/mtd/nor/spi_nor.c
 endif
 
@@ -175,6 +335,10 @@ BL2_SOURCES		+=	drivers/mtd/spi-mem/spi_mem.c				\
 endif
 
 ifneq ($(filter 1,${STM32MP_RAW_NAND} ${STM32MP_SPI_NAND}),)
+ifneq (${STM32MP_FORCE_MTD_START_OFFSET},)
+$(eval $(call add_define_val,STM32MP_NAND_FIP_OFFSET,${STM32MP_FORCE_MTD_START_OFFSET}))
+$(eval $(call add_define_val,STM32MP_NAND_BASE_OFFSET,${STM32MP_FORCE_MTD_START_OFFSET}))
+endif
 BL2_SOURCES		+=	drivers/mtd/nand/core.c
 endif
 
@@ -182,25 +346,60 @@ ifneq ($(filter 1,${STM32MP_RAW_NAND} ${STM32MP_SPI_NAND} ${STM32MP_SPI_NOR}),)
 BL2_SOURCES		+=	plat/st/stm32mp1/stm32mp1_boot_device.c
 endif
 
+ifneq ($(filter 1,${STM32MP_UART_PROGRAMMER} ${STM32MP_USB_PROGRAMMER}),)
+BL2_SOURCES		+=	drivers/io/io_memmap.c
+endif
+
+ifeq (${STM32MP_UART_PROGRAMMER},1)
+BL2_SOURCES		+=	drivers/st/uart/stm32_uart.c				\
+				plat/st/common/stm32cubeprogrammer_uart.c
+endif
+
+ifeq (${STM32MP_USB_PROGRAMMER},1)
+BL2_SOURCES		+=	drivers/st/usb_dwc2/usb_dwc2.c				\
+				lib/usb/usb_core.c					\
+				lib/usb/usb_st_dfu.c					\
+				plat/st/common/stm32cubeprogrammer_usb.c		\
+				plat/st/stm32mp1/stm32mp1_usb.c
+endif
+
 BL2_SOURCES		+=	drivers/st/ddr/stm32mp1_ddr.c				\
 				drivers/st/ddr/stm32mp1_ram.c
 
 BL2_SOURCES		+=	common/desc_image_load.c				\
-				plat/st/stm32mp1/plat_bl2_mem_params_desc.c		\
 				plat/st/stm32mp1/plat_image_load.c
 
-ifeq ($(AARCH32_SP),optee)
 BL2_SOURCES		+=	lib/optee/optee_utils.c
+
+BL2_SOURCES		+=	plat/st/stm32mp1/stm32mp1_critic_power.c
+BL2_SOURCES		+=	plat/st/stm32mp1/stm32mp1_critic_power_wrapper.S
+
+ifeq ($(STM32MP_SSP),1)
+include plat/st/stm32mp1/stm32mp1_ssp.mk
 endif
 
 # Compilation rules
-.PHONY: check_dtc_version stm32image clean_stm32image
+.PHONY: check_dtc_version stm32image clean_stm32image check_boot_device
 .SUFFIXES:
 
 all: check_dtc_version stm32image ${STM32_TF_STM32}
 
 distclean realclean clean: clean_stm32image
 
+bl2: check_boot_device
+
+check_boot_device:
+	@if [ ${STM32MP_EMMC} != 1 ] && \
+	    [ ${STM32MP_SDMMC} != 1 ] && \
+	    [ ${STM32MP_RAW_NAND} != 1 ] && \
+	    [ ${STM32MP_SPI_NAND} != 1 ] && \
+	    [ ${STM32MP_SPI_NOR} != 1 ] && \
+	    [ ${STM32MP_UART_PROGRAMMER} != 1 ] && \
+	    [ ${STM32MP_USB_PROGRAMMER} != 1 ]; then \
+		echo "No boot device driver is enabled"; \
+		false; \
+	fi
+
 stm32image: ${STM32IMAGE}
 
 ${STM32IMAGE}: ${STM32IMAGE_SRC}
@@ -217,12 +416,35 @@ check_dtc_version:
 		false; \
 	fi
 
-
+ifeq ($(STM32MP_USE_STM32IMAGE)$(AARCH32_SP),1sp_min)
 ${BUILD_PLAT}/stm32mp1-%.o: ${BUILD_PLAT}/fdts/%.dtb plat/st/stm32mp1/stm32mp1.S bl2 ${BL32_DEP}
+	@echo "  AS      stm32mp1.S"
+	${Q}${AS} ${ASFLAGS} ${TF_CFLAGS} \
+		-DDTB_BIN_PATH=\"$<\" \
+		-c $(word 2,$^) -o $@
+else
+# Create DTB file for BL2
+${BUILD_PLAT}/fdts/%-bl2.dts: fdts/%.dts fdts/${BL2_DTSI} | ${BUILD_PLAT} fdt_dirs
+	@echo '#include "$(patsubst fdts/%,%,$<)"' > $@
+	@echo '#include "${BL2_DTSI}"' >> $@
+
+${BUILD_PLAT}/fdts/%-bl2.dtb: ${BUILD_PLAT}/fdts/%-bl2.dts
+
+ifeq ($(AARCH32_SP),sp_min)
+# Create DTB file for BL32
+${BUILD_PLAT}/fdts/%-bl32.dts: fdts/%.dts fdts/${BL32_DTSI} | ${BUILD_PLAT} fdt_dirs
+	@echo '#include "$(patsubst fdts/%,%,$<)"' > $@
+	@echo '#include "${BL32_DTSI}"' >> $@
+
+${BUILD_PLAT}/fdts/%-bl32.dtb: ${BUILD_PLAT}/fdts/%-bl32.dts
+endif
+
+${BUILD_PLAT}/stm32mp1-%.o: ${BUILD_PLAT}/fdts/%-bl2.dtb plat/st/stm32mp1/stm32mp1.S bl2
 	@echo "  AS      stm32mp1.S"
 	${Q}${AS} ${ASFLAGS} ${TF_CFLAGS} \
 		-DDTB_BIN_PATH=\"$<\" \
 		-c plat/st/stm32mp1/stm32mp1.S -o $@
+endif
 
 $(eval $(call MAKE_LD,${STM32_TF_LINKERFILE},plat/st/stm32mp1/stm32mp1.ld.S,2))
 
@@ -243,5 +465,7 @@ tf-a-%.stm32: ${STM32IMAGE} tf-a-%.bin
 	$(eval ENTRY = $(shell cat $(@:.stm32=.map) | grep "__BL2_IMAGE_START" | awk '{print $$1}'))
 	${Q}${STM32IMAGE} -s $(word 2,$^) -d $@ \
 		-l $(LOADADDR) -e ${ENTRY} \
-		-v ${STM32_TF_VERSION}
+		-v ${STM32_TF_VERSION} \
+		-m ${STM32_HEADER_VERSION_MAJOR} \
+		-n ${STM32_HEADER_VERSION_MINOR}
 	@echo
diff --git a/plat/st/stm32mp1/services/bsec_svc.c b/plat/st/stm32mp1/services/bsec_svc.c
index 2a60e43393..e3d845d2ec 100644
--- a/plat/st/stm32mp1/services/bsec_svc.c
+++ b/plat/st/stm32mp1/services/bsec_svc.c
@@ -1,23 +1,444 @@
 /*
- * Copyright (c) 2016-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2016-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <assert.h>
+
 #include <platform_def.h>
 
+#include <arch.h>
+#include <arch_helpers.h>
 #include <common/debug.h>
+#include <common/runtime_svc.h>
 #include <drivers/st/bsec.h>
+#include <drivers/st/bsec2_reg.h>
+#include <drivers/st/stm32mp_pmic.h>
+#include <drivers/st/stm32mp1_ddr_helpers.h>
+#include <drivers/st/stpmic1.h>
+#include <lib/mmio.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <services/std_svc.h>
 
+#include <boot_api.h>
+#include <stm32mp1_dbgmcu.h>
 #include <stm32mp1_smc.h>
 
 #include "bsec_svc.h"
 
+enum bsec_ssp_status {
+	BSEC_NO_SSP = 0,
+	BSEC_SSP_SET,
+	BSEC_SSP_ERROR
+};
+
+struct otp_exchange {
+	uint32_t version;
+	uint32_t configuration;
+	uint32_t reserved;
+	uint32_t status;
+	uint32_t general_lock;
+	uint32_t debug_conf;
+	uint32_t reserved1[2];
+	uint32_t otp_disturb[3];
+	uint32_t reserved2[3];
+	uint32_t error_status[3];
+	uint32_t reserved3[3];
+	uint32_t permanent_lock[3];
+	uint32_t reserved4[3];
+	uint32_t programming_lock[3];
+	uint32_t reserved5[3];
+	uint32_t shadow_write_lock[3];
+	uint32_t reserved6[3];
+	uint32_t shadow_read_lock[3];
+	uint32_t reserved7[3];
+	uint32_t otp_value[STM32MP1_OTP_MAX_ID + 1];
+	uint32_t reserved8[112];
+	uint32_t bsec_hw_conf;
+	uint32_t ip_version;
+	uint32_t ip_id;
+	uint32_t ip_magic_id;
+};
+
+static enum bsec_ssp_status bsec_check_ssp(uint32_t otp, uint32_t update)
+{
+	boot_api_context_t *boot_context =
+		(boot_api_context_t *)BOOT_PARAM_ADDR;
+
+	/* No SSP update or SSP already done*/
+	if ((((otp & SSP_OTP_MASK) == 0U) && ((update & SSP_OTP_MASK) == 0U)) ||
+	    (((otp & SSP_OTP_MASK) == SSP_OTP_MASK) &&
+	     ((update & SSP_OTP_MASK) == SSP_OTP_MASK))) {
+		return BSEC_NO_SSP;
+	}
+
+	/* SSP update */
+	if ((update & SSP_OTP_MASK) != 0U) {
+		if ((update & SSP_OTP_SUCCESS) != 0U) {
+			return BSEC_SSP_ERROR;
+		}
+
+		/* SSP boot process */
+		boot_context->p_ssp_config->ssp_cmd =
+			BOOT_API_CTX_SSP_CMD_CALC_CHIP_PUBK;
+#ifndef DCACHE_OFF
+		flush_dcache_range((uintptr_t)boot_context->p_ssp_config,
+				   sizeof(boot_api_ssp_config_t));
+#endif
+		if (dt_pmic_status() > 0) {
+			const char *name;
+
+			initialize_pmic();
+
+			name = stm32mp_get_cpu_supply_name();
+			if (name == NULL) {
+				return BSEC_SSP_ERROR;
+			}
+
+			stpmic1_regulator_mask_reset_set(name);
+		}
+
+		return BSEC_SSP_SET;
+	}
+	return BSEC_NO_SSP;
+}
+
+static uint32_t bsec_read_all_bsec(struct otp_exchange *exchange)
+{
+	uint32_t i;
+	uint32_t result;
+
+	if (exchange == NULL) {
+		return BSEC_ERROR;
+	}
+
+	exchange->version = BSEC_SERVICE_VERSION;
+
+	for (i = 0U; i <= STM32MP1_OTP_MAX_ID; i++) {
+		if (bsec_check_nsec_access_rights(i) == BSEC_OK) {
+			result = bsec_shadow_register(i);
+			if (result != BSEC_OK) {
+				return result;
+			}
+
+			result = bsec_read_otp(&exchange->otp_value[i], i);
+			if (result != BSEC_OK) {
+				return result;
+			}
+		}
+	}
+
+	exchange->configuration = mmio_read_32(bsec_get_base() +
+					       BSEC_OTP_CONF_OFF);
+
+	exchange->status = mmio_read_32(bsec_get_base() + BSEC_OTP_STATUS_OFF);
+
+	exchange->general_lock = mmio_read_32(bsec_get_base() +
+					      BSEC_OTP_LOCK_OFF);
+
+	exchange->debug_conf = mmio_read_32(bsec_get_base() + BSEC_DEN_OFF);
+
+	exchange->otp_disturb[0] = mmio_read_32(bsec_get_base() +
+						BSEC_DISTURBED_OFF);
+
+	exchange->otp_disturb[1] = mmio_read_32(bsec_get_base() +
+						BSEC_DISTURBED1_OFF);
+
+	exchange->otp_disturb[2] = mmio_read_32(bsec_get_base() +
+						BSEC_DISTURBED2_OFF);
+
+	exchange->error_status[0] = mmio_read_32(bsec_get_base() +
+						 BSEC_ERROR_OFF);
+
+	exchange->error_status[1] = mmio_read_32(bsec_get_base() +
+						 BSEC_ERROR1_OFF);
+
+	exchange->error_status[2] = mmio_read_32(bsec_get_base() +
+						 BSEC_ERROR2_OFF);
+
+	exchange->permanent_lock[0] = mmio_read_32(bsec_get_base() +
+						   BSEC_WRLOCK_OFF);
+
+	exchange->permanent_lock[1] = mmio_read_32(bsec_get_base() +
+						   BSEC_WRLOCK1_OFF);
+
+	exchange->permanent_lock[2] = mmio_read_32(bsec_get_base() +
+						   BSEC_WRLOCK2_OFF);
+
+	exchange->programming_lock[0] = mmio_read_32(bsec_get_base() +
+						     BSEC_SPLOCK_OFF);
+
+	exchange->programming_lock[1] = mmio_read_32(bsec_get_base() +
+						     BSEC_SPLOCK1_OFF);
+
+	exchange->programming_lock[2] = mmio_read_32(bsec_get_base() +
+						     BSEC_SPLOCK2_OFF);
+
+	exchange->shadow_write_lock[0] = mmio_read_32(bsec_get_base() +
+						      BSEC_SWLOCK_OFF);
+
+	exchange->shadow_write_lock[1] = mmio_read_32(bsec_get_base() +
+						      BSEC_SWLOCK1_OFF);
+
+	exchange->shadow_write_lock[2] = mmio_read_32(bsec_get_base() +
+						      BSEC_SWLOCK2_OFF);
+
+	exchange->shadow_read_lock[0] = mmio_read_32(bsec_get_base() +
+						     BSEC_SRLOCK_OFF);
+
+	exchange->shadow_read_lock[1] = mmio_read_32(bsec_get_base() +
+						     BSEC_SRLOCK1_OFF);
+
+	exchange->shadow_read_lock[2] = mmio_read_32(bsec_get_base() +
+						     BSEC_SRLOCK2_OFF);
+
+	exchange->bsec_hw_conf = mmio_read_32(bsec_get_base() +
+					      BSEC_IPHW_CFG_OFF);
+
+	exchange->ip_version = mmio_read_32(bsec_get_base() + BSEC_IPVR_OFF);
+
+	exchange->ip_id = mmio_read_32(bsec_get_base() + BSEC_IP_ID_OFF);
+
+	exchange->ip_magic_id = mmio_read_32(bsec_get_base() +
+					     BSEC_IP_MAGIC_ID_OFF);
+
+	return BSEC_OK;
+}
+
+static uint32_t bsec_write_all_bsec(struct otp_exchange *exchange,
+				    uint32_t *ret_otp_value)
+{
+	uint32_t i;
+	uint32_t j;
+	uint32_t start_otp = 0U;
+	uint32_t value = 0U;
+	uint32_t ret;
+	struct bsec_config config_param;
+
+	*ret_otp_value = 0U;
+
+	if (exchange == NULL) {
+		return BSEC_ERROR;
+	}
+
+	if (exchange->version != BSEC_SERVICE_VERSION) {
+		return BSEC_ERROR;
+	}
+
+	for (i = start_otp; i <= STM32MP1_OTP_MAX_ID; i++) {
+		if (bsec_check_nsec_access_rights(i) != BSEC_OK) {
+			continue;
+		}
+
+		ret = bsec_shadow_register(i);
+		if (ret != BSEC_OK) {
+			return ret;
+		}
+
+		ret = bsec_read_otp(&value, i);
+		if (ret != BSEC_OK) {
+			return ret;
+		}
+
+		if ((value ==  exchange->otp_value[i]) &&
+		    (i != BOOT_API_OTP_SSP_WORD_NB)) {
+			continue;
+		}
+
+		if (i == BOOT_API_OTP_SSP_WORD_NB) {
+			*ret_otp_value = (uint32_t)bsec_check_ssp(value,
+							exchange->otp_value[i]);
+			VERBOSE("Result OTP SSP %d\n", *ret_otp_value);
+			if (*ret_otp_value == (uint32_t)BSEC_SSP_ERROR) {
+				continue;
+			}
+		}
+
+		ret = bsec_program_otp(exchange->otp_value[i], i);
+		if (ret != BSEC_OK) {
+			return ret;
+		}
+
+		ret = bsec_write_otp(exchange->otp_value[i], i);
+		if (ret != BSEC_OK) {
+			return ret;
+		}
+	}
+
+	bsec_write_debug_conf(exchange->debug_conf);
+
+	for (j = 0U; j < 3U; j++) {
+		if (exchange->permanent_lock[j] == 0U) {
+			continue;
+		}
+
+		for (i = 0U; i < 32U; i++) {
+			if (bsec_check_nsec_access_rights((32U * j) + i) !=
+			    BSEC_OK) {
+				continue;
+			}
+
+			value = (exchange->permanent_lock[j] >> i) & 1U;
+			if (value != 0U) {
+				ret = bsec_permanent_lock_otp((32U * j) + i);
+				if (ret != BSEC_OK) {
+					return ret;
+				}
+			}
+		}
+	}
+
+	for (j = 0U; j < 3U; j++) {
+		if (exchange->programming_lock[j] == 0U) {
+			continue;
+		}
+
+		for (i = 0U; i < 32U; i++) {
+			if (bsec_check_nsec_access_rights((32U * j) + i) !=
+			    BSEC_OK) {
+				continue;
+			}
+
+			value = (exchange->programming_lock[j] >> i) & 1U;
+			if (value != 0U) {
+				if (bsec_set_sp_lock((32U * j) + i) !=
+				    BSEC_OK) {
+					return BSEC_ERROR;
+				}
+			}
+		}
+	}
+
+	for (j = 0U; j < 3U; j++) {
+		if (exchange->shadow_write_lock[j] == 0U) {
+			continue;
+		}
+
+		for (i = 0U; i < 32U; i++) {
+			if (bsec_check_nsec_access_rights((32U * j) + i) !=
+			    BSEC_OK) {
+				continue;
+			}
+
+			value = (exchange->shadow_write_lock[j] >> i) & 1U;
+			if (value != 0U) {
+				if (bsec_set_sw_lock((32U * j) + i) !=
+				    BSEC_OK) {
+					return BSEC_ERROR;
+				}
+			}
+		}
+	}
+
+	for (j = 0U; j < 3U; j++) {
+		if (exchange->shadow_read_lock[j] == 0U) {
+			continue;
+		}
+
+		for (i = 0U; i < 32U; i++) {
+			if (bsec_check_nsec_access_rights((32U * j) + i) !=
+			    BSEC_OK) {
+				continue;
+			}
+
+			value = (exchange->shadow_read_lock[j] >> i) & 1U;
+			if (value != 0U) {
+				if (bsec_set_sr_lock((32U * j) + i) !=
+				    BSEC_OK) {
+					return BSEC_ERROR;
+				}
+			}
+		}
+	}
+
+	ret = bsec_get_config(&config_param);
+	if (ret != BSEC_OK) {
+		return ret;
+	}
+
+	config_param.power =
+		(uint8_t)(exchange->configuration & BSEC_CONF_POWER_UP_MASK) >>
+		BSEC_CONF_POWER_UP_SHIFT;
+	config_param.freq =
+		(uint8_t)(exchange->configuration & BSEC_CONF_FRQ_MASK) >>
+		BSEC_CONF_FRQ_SHIFT;
+	config_param.pulse_width =
+		(uint8_t)(exchange->configuration & BSEC_CONF_PRG_WIDTH_MASK) >>
+		BSEC_CONF_PRG_WIDTH_SHIFT;
+	config_param.tread =
+		(uint8_t)((exchange->configuration & BSEC_CONF_TREAD_MASK) >>
+		BSEC_CONF_TREAD_SHIFT);
+	config_param.den_lock =
+		(uint8_t)(exchange->general_lock & DENREG_LOCK_MASK) >>
+		DENREG_LOCK_SHIFT;
+	config_param.prog_lock =
+		(uint8_t)(exchange->general_lock & GPLOCK_LOCK_MASK) >>
+		GPLOCK_LOCK_SHIFT;
+
+	config_param.upper_otp_lock =
+		(uint8_t)(exchange->general_lock & UPPER_OTP_LOCK_MASK) >>
+		 UPPER_OTP_LOCK_SHIFT;
+
+	ret = bsec_set_config(&config_param);
+	if (ret != BSEC_OK) {
+		return ret;
+	}
+
+	INFO("write all otp succeed\n");
+
+	return BSEC_OK;
+}
+
 uint32_t bsec_main(uint32_t x1, uint32_t x2, uint32_t x3,
 		   uint32_t *ret_otp_value)
 {
 	uint32_t result;
 	uint32_t tmp_data = 0U;
+	struct otp_exchange *otp_exch __unused;
+	uintptr_t map_begin __unused;
+	size_t map_size __unused = PAGE_SIZE;
+	int ret __unused;
+
+	if ((x1 != STM32_SMC_READ_ALL) && (x1 != STM32_SMC_WRITE_ALL) &&
+	    (bsec_check_nsec_access_rights(x2) != BSEC_OK)) {
+		return STM32_SMC_INVALID_PARAMS;
+	}
+
+	otp_exch = NULL;
+	map_begin = 0U;
+
+	if ((x1 == STM32_SMC_READ_ALL) || (x1 == STM32_SMC_WRITE_ALL)) {
+		if (!stm32_boot_is_serial()) {
+			return STM32_SMC_FAILED;
+		}
+
+		map_begin = round_down(x2, PAGE_SIZE);
+
+		if (round_down(x2 + sizeof(struct otp_exchange), PAGE_SIZE) !=
+		    map_begin) {
+			/*
+			 * Buffer end is in the next page, 2 pages need to be
+			 * mapped.
+			 */
+			map_size += PAGE_SIZE;
+		}
+
+		ret = mmap_add_dynamic_region(map_begin,
+					      map_begin,
+					      map_size,
+					      MT_MEMORY | MT_RW | MT_NS);
+		assert(ret == 0);
+
+		if (!ddr_is_nonsecured_area(map_begin, map_size)) {
+			ret = mmap_remove_dynamic_region(map_begin, map_size);
+			assert(ret == 0);
+
+			return STM32_SMC_INVALID_PARAMS;
+		}
+
+		otp_exch = (struct otp_exchange *)(uintptr_t)x2;
+	}
 
 	switch (x1) {
 	case STM32_SMC_READ_SHADOW:
@@ -25,6 +446,18 @@ uint32_t bsec_main(uint32_t x1, uint32_t x2, uint32_t x3,
 		break;
 	case STM32_SMC_PROG_OTP:
 		*ret_otp_value = 0U;
+		if (x2 == BOOT_API_OTP_SSP_WORD_NB) {
+			result = bsec_read_otp(&tmp_data, x2);
+			if (result != BSEC_OK) {
+				break;
+			}
+
+			*ret_otp_value = (uint32_t)bsec_check_ssp(tmp_data, x3);
+			if (*ret_otp_value == (uint32_t)BSEC_SSP_ERROR) {
+				result = BSEC_OK;
+				break;
+			}
+		}
 		result = bsec_program_otp(x3, x2);
 		break;
 	case STM32_SMC_WRITE_SHADOW:
@@ -50,11 +483,23 @@ uint32_t bsec_main(uint32_t x1, uint32_t x2, uint32_t x3,
 
 		result = bsec_write_otp(tmp_data, x2);
 		break;
-
-	default:
-		result = BSEC_ERROR;
+	case STM32_SMC_READ_ALL:
+		result = bsec_read_all_bsec(otp_exch);
+		break;
+	case STM32_SMC_WRITE_ALL:
+		result = bsec_write_all_bsec(otp_exch, ret_otp_value);
 		break;
+	case STM32_SMC_WRLOCK_OTP:
+		result = bsec_permanent_lock_otp(x2);
+		break;
+	default:
+		return STM32_SMC_INVALID_PARAMS;
+	}
+
+	if ((x1 == STM32_SMC_READ_ALL) || (x1 == STM32_SMC_WRITE_ALL)) {
+		ret = mmap_remove_dynamic_region(map_begin, map_size);
+		assert(ret == 0);
 	}
 
-	return result;
+	return (result == BSEC_OK) ? STM32_SMC_OK : STM32_SMC_FAILED;
 }
diff --git a/plat/st/stm32mp1/services/low_power_svc.c b/plat/st/stm32mp1/services/low_power_svc.c
new file mode 100644
index 0000000000..567a3c70f5
--- /dev/null
+++ b/plat/st/stm32mp1/services/low_power_svc.c
@@ -0,0 +1,22 @@
+/*
+ * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <smccc_helpers.h>
+
+#include <stm32mp1_power_config.h>
+#include <stm32mp1_smc.h>
+
+#include "low_power_svc.h"
+
+uint32_t pm_domain_scv_handler(uint32_t x1, uint32_t x2)
+{
+	if (stm32mp1_set_pm_domain_state((enum stm32mp1_pm_domain)x1,
+					 (bool)x2) < 0) {
+		return STM32_SMC_FAILED;
+	}
+
+	return STM32_SMC_OK;
+}
diff --git a/plat/st/stm32mp1/services/low_power_svc.h b/plat/st/stm32mp1/services/low_power_svc.h
new file mode 100644
index 0000000000..eb98e92252
--- /dev/null
+++ b/plat/st/stm32mp1/services/low_power_svc.h
@@ -0,0 +1,14 @@
+/*
+ * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef LOW_POWER_SVC_H
+#define LOW_POWER_SVC_H
+
+#include <stdint.h>
+
+uint32_t pm_domain_scv_handler(uint32_t x1, uint32_t x2);
+
+#endif /* LOW_POWER_SVC_H */
diff --git a/plat/st/stm32mp1/services/pwr_svc.c b/plat/st/stm32mp1/services/pwr_svc.c
new file mode 100644
index 0000000000..1213d7ef64
--- /dev/null
+++ b/plat/st/stm32mp1/services/pwr_svc.c
@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <platform_def.h>
+
+#include <common/debug.h>
+#include <drivers/st/stm32mp1_pwr.h>
+#include <lib/mmio.h>
+
+#include <stm32mp_common.h>
+#include <stm32mp1_smc.h>
+
+#include "pwr_svc.h"
+
+static void access_allowed_mask(uint32_t request, uint32_t offset,
+				uint32_t value, uint32_t allowed_mask)
+{
+	uint32_t addr = stm32mp_pwr_base() + offset;
+	uint32_t masked_value = value & allowed_mask;
+
+	stm32mp_pwr_regs_lock();
+
+	switch (request) {
+	case STM32_SMC_REG_WRITE:
+		mmio_clrsetbits_32(addr, allowed_mask, masked_value);
+		VERBOSE("wrt 0x%x = 0x%x => 0x%x\n", offset, value,
+			mmio_read_32(addr));
+		break;
+
+	case STM32_SMC_REG_SET:
+		mmio_setbits_32(addr, masked_value);
+		VERBOSE("set 0x%x = 0x%x => 0x%x\n", offset, value,
+			mmio_read_32(addr));
+		break;
+
+	case STM32_SMC_REG_CLEAR:
+		mmio_clrbits_32(addr, masked_value);
+		VERBOSE("clear 0x%x = 0x%x => 0x%x\n", offset, value,
+			mmio_read_32(addr));
+		break;
+
+	default:
+		break;
+	}
+
+	stm32mp_pwr_regs_unlock();
+}
+
+static void raw_allowed_access_request(uint32_t request,
+				       uint32_t offset, uint32_t value)
+{
+	uint32_t allowed_mask = 0;
+
+	switch (offset) {
+	case PWR_CR3:
+		allowed_mask |= PWR_CR3_VBE | PWR_CR3_VBRS | PWR_CR3_USB33DEN |
+				PWR_CR3_REG18EN | PWR_CR3_REG11EN;
+		break;
+
+	case PWR_WKUPCR:
+		allowed_mask |= PWR_WKUPCR_MASK;
+		break;
+
+	case PWR_MPUWKUPENR:
+		allowed_mask |= PWR_MPUWKUPENR_MASK;
+		break;
+
+	default:
+		return;
+	}
+
+	if (allowed_mask != 0U) {
+		access_allowed_mask(request, offset, value, allowed_mask);
+	}
+}
+
+uint32_t pwr_scv_handler(uint32_t x1, uint32_t x2, uint32_t x3)
+{
+	uint32_t request = x1;
+	uint32_t offset = x2;
+	uint32_t value = x3;
+
+	/*
+	 * x2 may be either the PWR register offset or the register
+	 * full physical address.
+	 */
+	if ((offset & ~PWR_OFFSET_MASK) != 0) {
+		if ((offset & ~PWR_OFFSET_MASK) != stm32mp_pwr_base()) {
+			return STM32_SMC_INVALID_PARAMS;
+		}
+
+		offset &= PWR_OFFSET_MASK;
+	}
+
+	/* PWR controls for non secure resource may be accessed straight */
+	raw_allowed_access_request(request, offset, value);
+
+	return STM32_SMC_OK;
+}
diff --git a/plat/st/stm32mp1/services/pwr_svc.h b/plat/st/stm32mp1/services/pwr_svc.h
new file mode 100644
index 0000000000..6dacdf80d7
--- /dev/null
+++ b/plat/st/stm32mp1/services/pwr_svc.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PWR_SVC_H
+#define PWR_SVC_H
+
+uint32_t pwr_scv_handler(uint32_t x1, uint32_t x2, uint32_t x3);
+
+#endif /* PWR_SVC_H */
diff --git a/plat/st/stm32mp1/services/rcc_svc.c b/plat/st/stm32mp1/services/rcc_svc.c
new file mode 100644
index 0000000000..0be76bbda4
--- /dev/null
+++ b/plat/st/stm32mp1/services/rcc_svc.c
@@ -0,0 +1,169 @@
+/*
+ * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <limits.h>
+
+#include <platform_def.h>
+
+#include <common/debug.h>
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32mp1_clk.h>
+#include <drivers/st/stm32mp1_rcc.h>
+#include <lib/mmio.h>
+
+#include <stm32mp_common.h>
+#include <stm32mp_shres_helpers.h>
+#include <stm32mp1_shared_resources.h>
+#include <stm32mp1_smc.h>
+
+#include "rcc_svc.h"
+
+static bool offset_is_clear_register(uint32_t __unused offset)
+{
+	/* All currently 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)
+{
+	uint32_t addr = stm32mp_rcc_base() + offset;
+	uint32_t masked_value = value & allowed_mask;
+
+	switch (request) {
+	case STM32_SMC_REG_WRITE:
+		if (offset_is_clear_register(offset)) {
+			mmio_write_32(addr, masked_value);
+		} else {
+			stm32mp_mmio_clrsetbits_32_shregs(addr, allowed_mask,
+							  masked_value);
+		}
+		VERBOSE("wrt 0x%x = 0x%x => 0x%x\n", offset, value,
+			mmio_read_32(addr));
+		break;
+
+	case STM32_SMC_REG_SET:
+		if (offset_is_clear_register(offset)) {
+			mmio_write_32(addr, masked_value);
+		} else {
+			stm32mp_mmio_setbits_32_shregs(addr, masked_value);
+		}
+		VERBOSE("set 0x%x = 0x%x => 0x%x\n", offset, value,
+			mmio_read_32(addr));
+		break;
+
+	case STM32_SMC_REG_CLEAR:
+		if (offset_is_clear_register(offset)) {
+			/* Nothing to do on CLR registers */
+		} else {
+			stm32mp_mmio_clrbits_32_shregs(addr, masked_value);
+		}
+		VERBOSE("clear 0x%x = 0x%x => 0x%x\n", offset, value,
+			mmio_read_32(addr));
+		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;
+	default:
+		return STM32_SMC_INVALID_PARAMS;
+	}
+
+	if (allowed_mask != 0U) {
+		access_allowed_mask(request, offset, value, allowed_mask);
+	}
+
+	return STM32_SMC_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;
+
+	/*
+	 * x2 may be either the RCC register offset or the register
+	 * full physical address.
+	 */
+	if ((offset & ~RCC_OFFSET_MASK) != 0) {
+		if ((offset & ~RCC_OFFSET_MASK) != stm32mp_rcc_base()) {
+			return STM32_SMC_INVALID_PARAMS;
+		}
+
+		offset &= RCC_OFFSET_MASK;
+	}
+
+	return raw_allowed_access_request(request, offset, value);
+}
+
+uint32_t rcc_cal_scv_handler(uint32_t x1)
+{
+	uint32_t ret = STM32_SMC_FAILED;
+
+	switch (x1) {
+	case CK_CSI:
+		if (stm32mp1_calib_start_csi_cal() ==  0) {
+			ret = STM32_SMC_OK;
+		}
+		break;
+
+	case CK_HSI:
+		if (stm32mp1_calib_start_hsi_cal() == 0) {
+			ret = STM32_SMC_OK;
+		}
+		break;
+
+	default:
+		ret = STM32_SMC_INVALID_PARAMS;
+		break;
+	}
+
+	return ret;
+}
+
+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_SMC_RCC_OPP_SET:
+		if (stm32mp1_set_opp_khz(opp) != 0) {
+			return STM32_SMC_FAILED;
+		}
+		break;
+
+	case STM32_SMC_RCC_OPP_ROUND:
+		if (stm32mp1_round_opp_khz(&opp) != 0) {
+			return STM32_SMC_FAILED;
+		}
+
+		if (opp > (UINT32_MAX / 1000U)) {
+			return STM32_SMC_FAILED;
+		}
+
+		*res = opp * 1000U;
+		break;
+
+	default:
+		return STM32_SMC_INVALID_PARAMS;
+	}
+
+	return STM32_SMC_OK;
+}
diff --git a/plat/st/stm32mp1/services/rcc_svc.h b/plat/st/stm32mp1/services/rcc_svc.h
new file mode 100644
index 0000000000..23c75824f0
--- /dev/null
+++ b/plat/st/stm32mp1/services/rcc_svc.h
@@ -0,0 +1,14 @@
+/*
+ * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef RCC_SVC_H
+#define RCC_SVC_H
+
+uint32_t rcc_scv_handler(uint32_t x1, uint32_t x2, uint32_t x3);
+uint32_t rcc_cal_scv_handler(uint32_t x1);
+uint32_t rcc_opp_scv_handler(uint32_t x1, uint32_t x2, uint32_t *res);
+
+#endif /* RCC_SVC_H */
diff --git a/plat/st/stm32mp1/services/stm32mp1_svc_setup.c b/plat/st/stm32mp1/services/stm32mp1_svc_setup.c
index 49375a62d0..037a0b40b8 100644
--- a/plat/st/stm32mp1/services/stm32mp1_svc_setup.c
+++ b/plat/st/stm32mp1/services/stm32mp1_svc_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2014-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -13,9 +13,13 @@
 #include <lib/psci/psci.h>
 #include <tools_share/uuid.h>
 
+#include <stm32mp1_low_power.h>
 #include <stm32mp1_smc.h>
 
 #include "bsec_svc.h"
+#include "low_power_svc.h"
+#include "pwr_svc.h"
+#include "rcc_svc.h"
 
 /* STM32 SiP Service UUID */
 DEFINE_SVC_UUID2(stm32_sip_svc_uid,
@@ -66,6 +70,32 @@ static uintptr_t stm32mp1_svc_smc_handler(uint32_t smc_fid, u_register_t x1,
 		ret2_enabled = true;
 		break;
 
+	case STM32_SMC_RCC:
+		ret1 = rcc_scv_handler(x1, x2, x3);
+		break;
+
+	case STM32_SMC_RCC_CAL:
+		ret1 = rcc_cal_scv_handler(x1);
+		break;
+
+	case STM32_SMC_RCC_OPP:
+		ret1 = rcc_opp_scv_handler(x1, x2, &ret2);
+		ret2_enabled = true;
+		break;
+
+	case STM32_SMC_PWR:
+		ret1 = pwr_scv_handler(x1, x2, x3);
+		break;
+
+	case STM32_SMC_PD_DOMAIN:
+		ret1 = pm_domain_scv_handler(x1, x2);
+		break;
+
+	case STM32_SMC_AUTO_STOP:
+		stm32_auto_stop();
+		ret1 = STM32_SMC_OK;
+		break;
+
 	case STM32_SIP_SMC_SCMI_AGENT0:
 		scmi_smt_fastcall_smc_entry(0);
 		break;
@@ -75,7 +105,7 @@ static uintptr_t stm32mp1_svc_smc_handler(uint32_t smc_fid, u_register_t x1,
 
 	default:
 		WARN("Unimplemented STM32MP1 Service Call: 0x%x\n", smc_fid);
-		ret1 = SMC_UNK;
+		ret1 = STM32_SMC_NOT_SUPPORTED;
 		break;
 	}
 
diff --git a/plat/st/stm32mp1/sp_min/sp_min-stm32mp1.mk b/plat/st/stm32mp1/sp_min/sp_min-stm32mp1.mk
index 8866fb556c..dd81d13a08 100644
--- a/plat/st/stm32mp1/sp_min/sp_min-stm32mp1.mk
+++ b/plat/st/stm32mp1/sp_min/sp_min-stm32mp1.mk
@@ -1,17 +1,31 @@
 #
-# Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
 SP_MIN_WITH_SECURE_FIQ	:=	1
 
+# Allow SP_min to be placed in DDR
+STM32MP_SP_MIN_IN_DDR	?=	0
+
+$(eval $(call assert_booleans, STM32MP_SP_MIN_IN_DDR))
+
+$(eval $(call add_defines, STM32MP_SP_MIN_IN_DDR))
+
 BL32_CFLAGS		+=	-DSTM32MP_SHARED_RESOURCES
 
-BL32_SOURCES		+=	drivers/st/etzpc/etzpc.c			\
+BL32_SOURCES		+=	drivers/st/clk/stm32mp1_calib.c			\
+				drivers/st/etzpc/etzpc.c			\
+				drivers/st/rng/stm32_rng.c			\
+				drivers/st/rtc/stm32_rtc.c			\
+				drivers/st/tamper/stm32_tamp.c			\
+				drivers/st/timer/stm32_timer.c 			\
 				plat/common/aarch32/platform_mp_stack.S		\
 				plat/st/stm32mp1/sp_min/sp_min_setup.c		\
+				plat/st/stm32mp1/stm32mp1_low_power.c		\
 				plat/st/stm32mp1/stm32mp1_pm.c			\
+				plat/st/stm32mp1/stm32mp1_power_config.c	\
 				plat/st/stm32mp1/stm32mp1_shared_resources.c	\
 				plat/st/stm32mp1/stm32mp1_topology.c
 
@@ -20,11 +34,14 @@ include drivers/arm/gic/v2/gicv2.mk
 
 BL32_SOURCES		+=	${GICV2_SOURCES}			\
 				plat/common/plat_gicv2.c		\
-				plat/st/stm32mp1/stm32mp1_gic.c
+				plat/st/common/stm32_gic.c
 
 # Generic PSCI
 BL32_SOURCES		+=	plat/common/plat_psci_common.c
 
+# Generic FDT
+BL32_SOURCES		+=	common/fdt_fixup.c
+
 # SCMI server drivers
 BL32_SOURCES		+=	drivers/st/scmi-msg/base.c		\
 				drivers/st/scmi-msg/clock.c		\
@@ -34,8 +51,15 @@ BL32_SOURCES		+=	drivers/st/scmi-msg/base.c		\
 
 # stm32mp1 specific services
 BL32_SOURCES		+=	plat/st/stm32mp1/services/bsec_svc.c		\
+				plat/st/stm32mp1/services/low_power_svc.c	\
+				plat/st/stm32mp1/services/pwr_svc.c		\
+				plat/st/stm32mp1/services/rcc_svc.c		\
 				plat/st/stm32mp1/services/stm32mp1_svc_setup.c	\
 				plat/st/stm32mp1/stm32mp1_scmi.c
 
 # Arm Archtecture services
 BL32_SOURCES		+=	services/arm_arch_svc/arm_arch_svc_setup.c
+
+ifneq ($(STM32MP_SP_MIN_IN_DDR),1)
+BL32_SOURCES		+=	plat/st/stm32mp1/stm32mp1_critic_power.c
+endif
diff --git a/plat/st/stm32mp1/sp_min/sp_min_setup.c b/plat/st/stm32mp1/sp_min/sp_min_setup.c
index b639fcb358..0065be44fd 100644
--- a/plat/st/stm32mp1/sp_min/sp_min_setup.c
+++ b/plat/st/stm32mp1/sp_min/sp_min_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -12,16 +12,27 @@
 #include <arch_helpers.h>
 #include <common/bl_common.h>
 #include <common/debug.h>
+#include <common/fdt_fixup.h>
 #include <context.h>
 #include <drivers/arm/gicv2.h>
 #include <drivers/arm/tzc400.h>
+#include <drivers/clk.h>
 #include <drivers/generic_delay_timer.h>
 #include <drivers/st/bsec.h>
 #include <drivers/st/etzpc.h>
 #include <drivers/st/stm32_console.h>
 #include <drivers/st/stm32_gpio.h>
 #include <drivers/st/stm32_iwdg.h>
+#include <drivers/st/stm32_rng.h>
+#include <drivers/st/stm32_rtc.h>
+#include <drivers/st/stm32_tamp.h>
+#include <drivers/st/stm32_timer.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <drivers/st/stm32mp_pmic.h>
+#include <drivers/st/stm32mp_reset.h>
 #include <drivers/st/stm32mp1_clk.h>
+#include <drivers/st/stm32mp1_ddr_helpers.h>
+#include <drivers/st/stpmic1.h>
 #include <dt-bindings/clock/stm32mp1-clks.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/mmio.h>
@@ -29,6 +40,9 @@
 #include <plat/common/platform.h>
 
 #include <platform_sp_min.h>
+#include <stm32mp1_context.h>
+#include <stm32mp1_low_power.h>
+#include <stm32mp1_power_config.h>
 
 /******************************************************************************
  * Placeholder variables for copying the arguments that have been passed to
@@ -37,6 +51,132 @@
 static entry_point_info_t bl33_image_ep_info;
 
 static console_t console;
+static void stm32mp1_tamper_action(int id);
+
+static const char *tamper_name[PLAT_MAX_TAMP_INT] = {
+	"RTC power domain",
+	"Temperature monitoring",
+	"LSE monitoring",
+	"HSE monitoring",
+	"RTC calendar overflow",
+	"Monotonic counter"
+};
+
+static struct stm32_tamp_int int_tamp[PLAT_MAX_TAMP_INT] = {
+	{
+		.id = ITAMP1,
+		.func = stm32mp1_tamper_action,
+	},
+	{
+		.id = ITAMP2,
+		.func = stm32mp1_tamper_action,
+	},
+	{
+		.id = ITAMP3,
+		.func = stm32mp1_tamper_action,
+	},
+	{
+		.id = ITAMP4,
+		.func = stm32mp1_tamper_action,
+	},
+	TAMP_UNUSED,
+	TAMP_UNUSED,
+};
+
+static struct stm32_tamp_ext ext_tamp[PLAT_MAX_TAMP_EXT] = {
+	TAMP_UNUSED,
+	TAMP_UNUSED,
+	TAMP_UNUSED,
+};
+
+static void stm32_sgi1_it_handler(void)
+{
+	uint32_t id;
+
+	stm32mp_mask_timer();
+
+#if DEBUG
+	stm32mp_dump_core_registers(false);
+#endif
+
+	gicv2_end_of_interrupt(ARM_IRQ_SEC_SGI_1);
+
+	do {
+		id = plat_ic_get_pending_interrupt_id();
+
+		if (id <= MAX_SPI_ID) {
+			gicv2_end_of_interrupt(id);
+
+			plat_ic_disable_interrupt(id);
+		}
+	} while (id <= MAX_SPI_ID);
+
+	stm32mp_wait_cpu_reset();
+}
+
+static void stm32mp1_tamper_action(int id)
+{
+	ERROR("Tamper %s occurs\n", tamper_name[id]);
+	stm32mp_system_reset();
+}
+
+static void configure_wakeup_interrupt(void)
+{
+	int irq_num = fdt_rcc_enable_it("wakeup");
+
+	if (irq_num < 0) {
+		ERROR("irq_num = %d\n", irq_num);
+		panic();
+	}
+
+	plat_ic_set_interrupt_priority(irq_num, STM32MP1_IRQ_RCC_SEC_PRIO);
+}
+
+static void initialize_pll1_settings(void)
+{
+	uint32_t cpu_voltage = 0U;
+
+	if (stm32_are_pll1_settings_valid_in_context()) {
+		return;
+	}
+
+	if (dt_pmic_status() > 0) {
+		const char *name = stm32mp_get_cpu_supply_name();
+		int ret;
+
+		if (name == NULL) {
+			panic();
+		}
+
+		ret = stpmic1_regulator_voltage_get(name);
+		if (ret < 0) {
+			panic();
+		}
+
+		cpu_voltage = (uint32_t)ret;
+	}
+
+	if (stm32mp1_clk_compute_all_pll1_settings(cpu_voltage) != 0) {
+		panic();
+	}
+}
+
+static void disable_usb_phy_regulator(void)
+{
+	if (dt_pmic_status() > 0) {
+		const char *name = stm32mp_get_usb_phy_supply_name();
+		int ret;
+
+		if (name == NULL) {
+			return;
+		}
+
+		ret = stpmic1_regulator_disable(name);
+		if (ret < 0) {
+			WARN("USBPHYC phy-supply (%s) disable failed\n", name);
+		}
+	}
+}
 
 /*******************************************************************************
  * Interrupt handler for FIQ (secure IRQ)
@@ -44,20 +184,75 @@ static console_t console;
 void sp_min_plat_fiq_handler(uint32_t id)
 {
 	switch (id & INT_ID_MASK) {
+	case ARM_IRQ_SEC_PHY_TIMER:
+	case STM32MP1_IRQ_MCU_SEV:
+	case STM32MP1_IRQ_RCC_WAKEUP:
+		stm32mp1_calib_it_handler(id);
+		break;
 	case STM32MP1_IRQ_TZC400:
-		ERROR("STM32MP1_IRQ_TZC400 generated\n");
+		tzc400_init(STM32MP1_TZC_BASE);
+		tzc400_it_handler();
 		panic();
 		break;
+	case STM32MP1_IRQ_TAMPSERRS:
+		stm32_tamp_it_handler();
+		break;
+	case ARM_IRQ_SEC_SGI_1:
+		stm32_sgi1_it_handler();
+		break;
+	case ARM_IRQ_SEC_SGI_6:
+		/* tell the primary cpu to exit from stm32_pwr_down_wfi() */
+		if (plat_my_core_pos() == STM32MP_PRIMARY_CPU) {
+			stm32mp1_calib_set_wakeup(true);
+		}
+		gicv2_end_of_interrupt(ARM_IRQ_SEC_SGI_6);
+		break;
+	case STM32MP1_IRQ_IWDG1:
+	case STM32MP1_IRQ_IWDG2:
+		stm32_iwdg_it_handler(id);
+		break;
 	case STM32MP1_IRQ_AXIERRIRQ:
 		ERROR("STM32MP1_IRQ_AXIERRIRQ generated\n");
 		panic();
 		break;
 	default:
-		ERROR("SECURE IT handler not define for it : %u", id);
+		ERROR("SECURE IT handler not define for it : %u\n", id);
 		break;
 	}
 }
 
+/*******************************************************************************
+ * Return the value of the saved PC from the backup register if present
+ ******************************************************************************/
+static uintptr_t get_saved_pc(void)
+{
+	uint32_t bkpr_core1_addr =
+		tamp_bkpr(BOOT_API_CORE1_BRANCH_ADDRESS_TAMP_BCK_REG_IDX);
+	uint32_t saved_pc;
+	uint32_t bkpr_core1_magic =
+		tamp_bkpr(BOOT_API_CORE1_MAGIC_NUMBER_TAMP_BCK_REG_IDX);
+	uint32_t magic_nb;
+
+	clk_enable(RTCAPB);
+
+	magic_nb = mmio_read_32(bkpr_core1_magic);
+	saved_pc = mmio_read_32(bkpr_core1_addr);
+
+	clk_disable(RTCAPB);
+
+	if (magic_nb != BOOT_API_A7_CORE0_MAGIC_NUMBER) {
+		return 0U;
+	}
+
+	/* BL33 return address should be in DDR */
+	if ((saved_pc < STM32MP_DDR_BASE) ||
+	    (saved_pc > (STM32MP_DDR_BASE + (dt_get_ddr_size() - 1U)))) {
+		panic();
+	}
+
+	return saved_pc;
+}
+
 /*******************************************************************************
  * Return a pointer to the 'entry_point_info' structure of the next image for
  * the security state specified. BL33 corresponds to the non-secure image type
@@ -66,12 +261,36 @@ void sp_min_plat_fiq_handler(uint32_t id)
  ******************************************************************************/
 entry_point_info_t *sp_min_plat_get_bl33_ep_info(void)
 {
-	entry_point_info_t *next_image_info;
-
-	next_image_info = &bl33_image_ep_info;
+	entry_point_info_t *next_image_info = &bl33_image_ep_info;
 
+	/*
+	 * PC is set to 0 when resetting after STANDBY
+	 * The context should be restored, and the image information
+	 * should be filled with what was saved
+	 */
 	if (next_image_info->pc == 0U) {
-		return NULL;
+		void *cpu_context;
+		uintptr_t saved_pc;
+
+		if (stm32_restore_context() != 0) {
+			panic();
+		}
+
+		cpu_context = cm_get_context(NON_SECURE);
+
+		next_image_info->spsr = read_ctx_reg(get_regs_ctx(cpu_context),
+						     CTX_SPSR);
+
+		/* PC should be retrieved in backup register if OK, else it can
+		 * be retrieved from non-secure context
+		 */
+		saved_pc = get_saved_pc();
+		if (saved_pc != 0U) {
+			next_image_info->pc = saved_pc;
+		} else {
+			next_image_info->pc =
+				read_ctx_reg(get_regs_ctx(cpu_context), CTX_LR);
+		}
 	}
 
 	return next_image_info;
@@ -108,15 +327,107 @@ static void stm32mp1_etzpc_early_setup(void)
 	etzpc_configure_tzma(STM32MP1_ETZPC_TZMA_SYSRAM, TZMA1_SECURE_RANGE);
 }
 
+#if !STM32MP_USE_STM32IMAGE
+static void populate_ns_dt(u_register_t ns_dt_addr, uintptr_t sec_base, size_t sec_size)
+{
+	void *external_fdt = (void *)ns_dt_addr;
+	int ret;
+
+	if (sec_base < STM32MP_DDR_BASE) {
+		/* No need to reserve memory if secure monitor is not in DDR */
+		return;
+	}
+
+	/* Map Base Non Secure DDR for Non secure DT update */
+	ret = mmap_add_dynamic_region(ns_dt_addr, ns_dt_addr, STM32MP_HW_CONFIG_MAX_SIZE,
+				      MT_NON_CACHEABLE | MT_EXECUTE_NEVER | MT_RW | MT_NS);
+	assert(ret == 0);
+
+	if (fdt_check_header(external_fdt) != 0) {
+		INFO("Non-secure device tree not found\n");
+
+		goto out;
+	}
+
+	ret = fdt_open_into(external_fdt, external_fdt, STM32MP_HW_CONFIG_MAX_SIZE);
+	if (ret < 0) {
+		WARN("Error opening DT %i\n", ret);
+		goto out;
+	}
+
+	ret = fdt_add_reserved_memory(external_fdt, "tf-a", sec_base, sec_size);
+	if (ret < 0) {
+		WARN("Error updating DT %i\n", ret);
+		goto out;
+	}
+
+	ret = fdt_pack(external_fdt);
+	if (ret < 0) {
+		WARN("Error packing DT %i\n", ret);
+	}
+
+out:
+	ret = mmap_remove_dynamic_region(ns_dt_addr, STM32MP_HW_CONFIG_MAX_SIZE);
+	assert(ret == 0);
+}
+#endif
+
+/*******************************************************************************
+ * Setup UART console using device tree information.
+ ******************************************************************************/
+static void setup_uart_console(void)
+{
+	struct dt_node_info dt_uart_info;
+	unsigned int console_flags;
+	int result;
+	uint32_t boot_itf;
+	uint32_t boot_instance;
+
+	result = dt_get_stdout_uart_info(&dt_uart_info);
+	if ((result <= 0) || (dt_uart_info.status == DT_DISABLED)) {
+		return;
+	}
+
+	stm32_get_boot_interface(&boot_itf, &boot_instance);
+
+	if ((boot_itf == BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART) &&
+	    (get_uart_address(boot_instance) == dt_uart_info.base)) {
+		return;
+	}
+
+	if (console_stm32_register(dt_uart_info.base, 0,
+				   STM32MP_UART_BAUDRATE, &console) == 0U) {
+		panic();
+	}
+
+	console_flags = CONSOLE_FLAG_BOOT | CONSOLE_FLAG_CRASH |
+			CONSOLE_FLAG_TRANSLATE_CRLF;
+#ifdef DEBUG
+	console_flags |= CONSOLE_FLAG_RUNTIME;
+#endif
+	console_set_scope(&console, console_flags);
+}
+
 /*******************************************************************************
  * Perform any BL32 specific platform actions.
  ******************************************************************************/
 void sp_min_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 				  u_register_t arg2, u_register_t arg3)
 {
-	struct dt_node_info dt_uart_info;
-	int result;
 	bl_params_t *params_from_bl2 = (bl_params_t *)arg0;
+#if STM32MP_USE_STM32IMAGE
+	uintptr_t dt_addr = STM32MP_DTB_BASE;
+#else
+	uintptr_t dt_addr = arg1;
+	uintptr_t sec_base = 0U;
+	size_t sec_size = 0U;
+#endif
+#if STM32MP_SP_MIN_IN_DDR
+	uintptr_t bl2_code_base = 0U;
+	uintptr_t bl2_code_end = 0U;
+	uintptr_t bl2_end = 0U;
+	int result __unused;
+#endif
 
 	/* Imprecise aborts can be masked in NonSecure */
 	write_scr(read_scr() | SCR_AW_BIT);
@@ -125,59 +436,152 @@ void sp_min_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 			BL_CODE_END - BL_CODE_BASE,
 			MT_CODE | MT_SECURE);
 
+#if STM32MP_SP_MIN_IN_DDR
+	/* BL32 data*/
+	mmap_add_region(BL_CODE_END, BL_CODE_END,
+			BL_END - BL_CODE_END,
+			MT_RW_DATA | MT_SECURE);
+
+	/* BL32 Device Tree Blob */
+	mmap_add_region(dt_addr, dt_addr,
+			STM32MP_BL32_DTB_SIZE,
+			MT_RO_DATA | MT_SECURE);
+
+	/* Map SCMI shared buffers */
+	mmap_add_region(STM32MP_SCMI_NS_SHM_BASE, STM32MP_SCMI_NS_SHM_BASE,
+			STM32MP_SCMI_NS_SHM_SIZE,
+			MT_DEVICE | MT_RW | MT_NS | MT_EXECUTE_NEVER);
+#endif
+
 	configure_mmu();
 
+	if (dt_open_and_check(dt_addr) < 0) {
+		panic();
+	}
+
+	if (bsec_probe() != 0) {
+		panic();
+	}
+
+	if (stm32mp1_clk_probe() < 0) {
+		panic();
+	}
+
+	setup_uart_console();
+
+	stm32mp1_etzpc_early_setup();
+
+#if STM32MP_SP_MIN_IN_DDR
+	stm32_context_get_bl2_low_power_params(&bl2_code_base, &bl2_code_end, &bl2_end);
+
+	/* BL2 Code */
+	result = mmap_add_dynamic_region(bl2_code_base, bl2_code_base,
+					 bl2_code_end - bl2_code_base,
+					 MT_CODE | MT_SECURE);
+	assert(result == 0);
+
+	/* BL2 RW memory */
+	result = mmap_add_dynamic_region(bl2_code_end, bl2_code_end,
+					 bl2_end - bl2_code_end,
+					 MT_RW_DATA | MT_SECURE);
+	assert(result == 0);
+#endif
+
 	assert(params_from_bl2 != NULL);
 	assert(params_from_bl2->h.type == PARAM_BL_PARAMS);
 	assert(params_from_bl2->h.version >= VERSION_2);
 
 	bl_params_node_t *bl_params = params_from_bl2->head;
 
-	/*
-	 * Copy BL33 entry point information.
-	 * They are stored in Secure RAM, in BL2's address space.
-	 */
 	while (bl_params != NULL) {
+		/*
+		 * Copy BL33 entry point information.
+		 * They are stored in Secure RAM, in BL2's address space.
+		 */
 		if (bl_params->image_id == BL33_IMAGE_ID) {
 			bl33_image_ep_info = *bl_params->ep_info;
-			break;
+			/*
+			 *  Check if hw_configuration is given to BL32 and
+			 *  share it to BL33
+			 */
+			if (arg2 != 0U) {
+				bl33_image_ep_info.args.arg0 = 0U;
+				bl33_image_ep_info.args.arg1 = 0U;
+				bl33_image_ep_info.args.arg2 = arg2;
+			}
 		}
 
+#if !STM32MP_USE_STM32IMAGE
+		if (bl_params->image_id == BL32_IMAGE_ID) {
+			sec_base = bl_params->image_info->image_base;
+			sec_size = bl_params->image_info->image_max_size;
+		}
+#endif
+
 		bl_params = bl_params->next_params_info;
 	}
 
-	if (dt_open_and_check() < 0) {
-		panic();
-	}
+#if !STM32MP_USE_STM32IMAGE
+	if (arg2 != 0U) {
+		/* This will expect the BL32 DT and BL32 are grouped */
+		if (dt_addr < sec_base) {
+			sec_size = sec_size + sec_base - dt_addr;
+			sec_base = dt_addr;
+		} else {
+			sec_size = dt_addr - sec_base + STM32MP_BL32_DTB_SIZE;
+		}
 
-	if (bsec_probe() != 0) {
-		panic();
+		populate_ns_dt(arg2, sec_base, sec_size);
+	} else {
+		INFO("Non-secure device tree not found\n");
 	}
+#endif
 
-	if (stm32mp1_clk_probe() < 0) {
-		panic();
+	if (dt_pmic_status() > 0) {
+		initialize_pmic();
 	}
 
-	result = dt_get_stdout_uart_info(&dt_uart_info);
+	disable_usb_phy_regulator();
 
-	if ((result > 0) && (dt_uart_info.status != 0U)) {
-		unsigned int console_flags;
+	initialize_pll1_settings();
 
-		if (console_stm32_register(dt_uart_info.base, 0,
-					   STM32MP_UART_BAUDRATE, &console) ==
-		    0) {
-			panic();
-		}
+	stm32mp1_init_lp_states();
+}
 
-		console_flags = CONSOLE_FLAG_BOOT | CONSOLE_FLAG_CRASH |
-			CONSOLE_FLAG_TRANSLATE_CRLF;
-#ifdef DEBUG
-		console_flags |= CONSOLE_FLAG_RUNTIME;
-#endif
-		console_set_scope(&console, console_flags);
+static void init_sec_peripherals(void)
+{
+	uint32_t filter_conf = 0;
+	uint32_t active_conf = 0;
+	int ret;
+
+	/* Disable MCU subsystem protection */
+	stm32mp1_clk_mcuss_protect(false);
+
+	/* Init rtc driver */
+	ret = stm32_rtc_init();
+	if (ret < 0) {
+		WARN("RTC driver init error %i\n", ret);
 	}
 
-	stm32mp1_etzpc_early_setup();
+	/*  Init rng driver */
+	ret = stm32_rng_init();
+	if (ret < 0) {
+		WARN("RNG driver init error %i\n", ret);
+	}
+
+	/* Init tamper */
+	if (stm32_tamp_init() > 0) {
+		stm32_tamp_configure_internal(int_tamp, PLAT_MAX_TAMP_INT);
+		stm32_tamp_configure_external(ext_tamp, PLAT_MAX_TAMP_EXT,
+					      filter_conf, active_conf);
+
+		/* Enable timestamp for tamper */
+		stm32_rtc_set_tamper_timestamp();
+	}
+
+	if (stm32_timer_init() == 0) {
+		stm32mp1_calib_init();
+	}
 }
 
 /*******************************************************************************
@@ -185,17 +589,22 @@ void sp_min_early_platform_setup2(u_register_t arg0, u_register_t arg1,
  ******************************************************************************/
 void sp_min_platform_setup(void)
 {
-	/* Initialize tzc400 after DDR initialization */
-	stm32mp1_security_setup();
+	stm32_init_low_power();
+
+	ddr_save_sr_mode();
 
 	generic_delay_timer_init();
 
-	stm32mp1_gic_init();
+	stm32_gic_init();
+
+	init_sec_peripherals();
 
 	if (stm32_iwdg_init() < 0) {
 		panic();
 	}
 
+	configure_wakeup_interrupt();
+
 	stm32mp_lock_periph_registering();
 
 	stm32mp1_init_scmi_server();
diff --git a/plat/st/stm32mp1/stm32mp1.S b/plat/st/stm32mp1/stm32mp1.S
index 7255fe5aa9..1b7e9e7eff 100644
--- a/plat/st/stm32mp1/stm32mp1.S
+++ b/plat/st/stm32mp1/stm32mp1.S
@@ -1,13 +1,15 @@
 /*
- * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#if STM32MP_USE_STM32IMAGE
 #ifdef BL32_BIN_PATH
 .section .bl32_image
 .incbin BL32_BIN_PATH
 #endif
+#endif
 
 .section .bl2_image
 .incbin BL2_BIN_PATH
diff --git a/plat/st/stm32mp1/stm32mp1.ld.S b/plat/st/stm32mp1/stm32mp1.ld.S
index b347baddf7..43e2733397 100644
--- a/plat/st/stm32mp1/stm32mp1.ld.S
+++ b/plat/st/stm32mp1/stm32mp1.ld.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -16,7 +16,7 @@ OUTPUT_ARCH(PLATFORM_LINKER_ARCH)
 ENTRY(__BL2_IMAGE_START__)
 
 MEMORY {
-	HEADER (rw) : ORIGIN = 0x00000000, LENGTH = 0x3000
+	HEADER (rw) : ORIGIN = 0x00000000, LENGTH = STM32MP_HEADER_RESERVED_SIZE
 	RAM (rwx) : ORIGIN = STM32MP_BINARY_BASE, LENGTH = STM32MP_BINARY_SIZE
 }
 
@@ -43,7 +43,11 @@ SECTIONS
          * The strongest and only alignment contraint is MMU 4K page.
          * Indeed as images below will be removed, 4K pages will be re-used.
          */
+#if STM32MP_USE_STM32IMAGE
         . = ( STM32MP_DTB_BASE - STM32MP_BINARY_BASE );
+#else
+        . = ( STM32MP_BL2_DTB_BASE - STM32MP_BINARY_BASE );
+#endif
         __DTB_IMAGE_START__ = .;
         *(.dtb_image*)
         __DTB_IMAGE_END__ = .;
@@ -53,12 +57,16 @@ SECTIONS
          * The strongest and only alignment contraint is MMU 4K page.
          * Indeed as images below will be removed, 4K pages will be re-used.
          */
+#if SEPARATE_CODE_AND_RODATA
+        . = ( STM32MP_BL2_RO_BASE - STM32MP_BINARY_BASE );
+#else
         . = ( STM32MP_BL2_BASE - STM32MP_BINARY_BASE );
+#endif
         __BL2_IMAGE_START__ = .;
         *(.bl2_image*)
         __BL2_IMAGE_END__ = .;
 
-#ifndef AARCH32_SP_OPTEE
+#if STM32MP_USE_STM32IMAGE && !defined(AARCH32_SP_OPTEE)
         /*
          * bl32 will be settled by bl2.
          * The strongest and only alignment constraint is 8 words to simplify
diff --git a/plat/st/stm32mp1/stm32mp1_boot_device.c b/plat/st/stm32mp1/stm32mp1_boot_device.c
index 997335d0da..690d8f04c9 100644
--- a/plat/st/stm32mp1/stm32mp1_boot_device.c
+++ b/plat/st/stm32mp1/stm32mp1_boot_device.c
@@ -19,13 +19,11 @@
 #if STM32MP_RAW_NAND || STM32MP_SPI_NAND
 static int get_data_from_otp(struct nand_device *nand_dev, bool is_slc)
 {
-	int result;
 	uint32_t nand_param;
 
 	/* Check if NAND parameters are stored in OTP */
-	result = bsec_shadow_read_otp(&nand_param, NAND_OTP);
-	if (result != BSEC_OK) {
-		ERROR("BSEC: NAND_OTP Error %i\n", result);
+	if (stm32_get_otp_value(NAND_OTP, &nand_param) != 0) {
+		ERROR("BSEC: NAND_OTP Error\n");
 		return -EACCES;
 	}
 
diff --git a/plat/st/stm32mp1/stm32mp1_context.c b/plat/st/stm32mp1/stm32mp1_context.c
index cf8a91eb41..027475ed40 100644
--- a/plat/st/stm32mp1/stm32mp1_context.c
+++ b/plat/st/stm32mp1/stm32mp1_context.c
@@ -1,35 +1,492 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #include <errno.h>
+#include <string.h>
 
 #include <platform_def.h>
 
-#include <drivers/st/stm32mp1_clk.h>
+#include <arch_helpers.h>
+#include <common/bl_common.h>
+#include <context.h>
+#include <drivers/clk.h>
+#include <drivers/st/stm32_rtc.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <drivers/st/stm32mp1_ddr_regs.h>
 #include <dt-bindings/clock/stm32mp1-clks.h>
+#include <lib/el3_runtime/context_mgmt.h>
 #include <lib/mmio.h>
+#include <lib/utils.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <smccc_helpers.h>
 
 #include <stm32mp1_context.h>
 
-#define TAMP_BOOT_ITF_BACKUP_REG_ID	U(20)
-#define TAMP_BOOT_ITF_MASK		U(0x0000FF00)
-#define TAMP_BOOT_ITF_SHIFT		8
+#include <boot_api.h>
+#include <stm32mp1_critic_power.h>
 
-int stm32_save_boot_interface(uint32_t interface, uint32_t instance)
+#define TRAINING_AREA_SIZE		64
+
+#define BL32_CANARY_ID			U(0x424c3332)
+
+/*
+ * MAILBOX_MAGIC relates to struct backup_data_s as defined
+ *
+ * MAILBOX_MAGIC_V1:
+ * Context provides magic, resume entry, zq0cr0 zdata and DDR training buffer.
+ *
+ * MAILBOX_MAGIC_V2:
+ * Context provides magic, resume entry, zq0cr0 zdata, DDR training buffer
+ * and PLL1 dual OPP settings structure (86 bytes).
+ *
+ * MAILBOX_MAGIC_V3:
+ * Context provides magic, resume entry, zq0cr0 zdata, DDR training buffer
+ * and PLL1 dual OPP settings structure, low power entry point, BL2 code start, end and BL2_END
+ * (102 bytes).
+ */
+#define MAILBOX_MAGIC_V1		(0x0001 << 16)
+#define MAILBOX_MAGIC_V2		(0x0002 << 16)
+#define MAILBOX_MAGIC_V3		(0x0003 << 16)
+#define MAILBOX_MAGIC			(MAILBOX_MAGIC_V3 | \
+					 TRAINING_AREA_SIZE)
+
+#define MAGIC_ID(magic)			((magic) & GENMASK_32(31, 16))
+#define MAGIC_AREA_SIZE(magic)		((magic) & GENMASK_32(15, 0))
+
+#if (PLAT_MAX_OPP_NB != 2) || (PLAT_MAX_PLLCFG_NB != 6)
+#error MAILBOX_MAGIC_V2/_V3 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))
+
+/* Set to 600 bytes to be a bit flexible but could be optimized if needed */
+#define CLOCK_CONTEXT_SIZE		600
+
+/* SCMI needs only 24 bits to save the state of the 24 exposed clocks */
+#define SCMI_CONTEXT_SIZE		(sizeof(uint8_t) * 4)
+
+struct backup_data_s {
+	uint32_t magic;
+	uint32_t core0_resume_hint;
+	uint32_t zq0cr0_zdata;
+	uint8_t ddr_training_backup[TRAINING_AREA_SIZE];
+	uint8_t pll1_settings[PLL1_SETTINGS_SIZE];
+	uint32_t low_power_ep;
+	uint32_t bl2_code_base;
+	uint32_t bl2_code_end;
+	uint32_t bl2_end;
+};
+
+#if defined(IMAGE_BL32)
+struct backup_bl32_data_s {
+	uint32_t canary_id;
+	smc_ctx_t saved_smc_context[PLATFORM_CORE_COUNT];
+	cpu_context_t saved_cpu_context[PLATFORM_CORE_COUNT];
+	struct stm32_rtc_calendar rtc;
+	unsigned long long stgen;
+	uint8_t clock_cfg[CLOCK_CONTEXT_SIZE];
+	uint8_t scmi_context[SCMI_CONTEXT_SIZE];
+};
+
+static struct backup_bl32_data_s *get_bl32_backup_data(void)
+{
+	return (struct backup_bl32_data_s *)(STM32MP_BACKUP_RAM_BASE +
+					     sizeof(struct backup_data_s));
+}
+
+#if STM32MP_SP_MIN_IN_DDR
+void (*stm32_pwr_down_wfi)(bool is_cstop);
+#endif
+#endif
+
+uint32_t stm32_pm_get_optee_ep(void)
+{
+	struct backup_data_s *backup_data;
+	uint32_t ep;
+
+	clk_enable(BKPSRAM);
+
+	/* Context & Data to be saved at the beginning of Backup SRAM */
+	backup_data = (struct backup_data_s *)STM32MP_BACKUP_RAM_BASE;
+
+	switch (MAGIC_ID(backup_data->magic)) {
+	case MAILBOX_MAGIC_V1:
+	case MAILBOX_MAGIC_V2:
+	case MAILBOX_MAGIC_V3:
+		if (MAGIC_AREA_SIZE(backup_data->magic) != TRAINING_AREA_SIZE) {
+			panic();
+		}
+		break;
+	default:
+		ERROR("PM context: bad magic\n");
+		panic();
+	}
+
+	ep = backup_data->core0_resume_hint;
+
+	clk_disable(BKPSRAM);
+
+	return ep;
+}
+
+void stm32_clean_context(void)
+{
+	clk_enable(BKPSRAM);
+
+#if defined(IMAGE_BL2)
+	zeromem((void *)STM32MP_BACKUP_RAM_BASE, sizeof(struct backup_data_s));
+#elif defined(IMAGE_BL32)
+	zeromem((void *)get_bl32_backup_data(), sizeof(struct backup_bl32_data_s));
+#endif
+
+	clk_disable(BKPSRAM);
+}
+
+#if defined(IMAGE_BL32)
+void stm32mp1_pm_save_clock_cfg(size_t offset, uint8_t *data, size_t size)
+{
+	struct backup_bl32_data_s *backup_data = get_bl32_backup_data();
+
+	if (offset + size > sizeof(backup_data->clock_cfg)) {
+		panic();
+	}
+
+	clk_enable(BKPSRAM);
+
+	memcpy(backup_data->clock_cfg + offset, data, size);
+
+	clk_disable(BKPSRAM);
+}
+
+void stm32mp1_pm_restore_clock_cfg(size_t offset, uint8_t *data, size_t size)
+{
+	struct backup_bl32_data_s *backup_data = get_bl32_backup_data();
+
+	if (offset + size > sizeof(backup_data->clock_cfg))
+		panic();
+
+	clk_enable(BKPSRAM);
+
+	memcpy(data, backup_data->clock_cfg + offset, size);
+
+	clk_disable(BKPSRAM);
+}
+
+int stm32_save_context(uint32_t zq0cr0_zdata,
+		       struct stm32_rtc_calendar *rtc_time,
+		       unsigned long long stgen_cnt)
 {
-	uint32_t bkpr_itf_idx = tamp_bkpr(TAMP_BOOT_ITF_BACKUP_REG_ID);
+	void *smc_context;
+	void *cpu_context;
+	struct backup_data_s *backup_data;
+	struct backup_bl32_data_s *backup_bl32_data;
+
+	stm32mp1_clock_suspend();
+
+	clk_enable(BKPSRAM);
+
+	/* Context & Data to be saved at the beginning of Backup SRAM */
+	backup_data = (struct backup_data_s *)STM32MP_BACKUP_RAM_BASE;
+
+	/* Save BL32 context data provided to BL2 */
+	backup_data->magic = MAILBOX_MAGIC;
+
+	backup_data->zq0cr0_zdata = zq0cr0_zdata;
+
+	stm32mp1_clk_lp_save_opp_pll1_settings(backup_data->pll1_settings,
+					       sizeof(backup_data->pll1_settings));
+
+	/* Save the BL32 specific data */
+	backup_bl32_data = get_bl32_backup_data();
+
+	backup_bl32_data->canary_id = BL32_CANARY_ID;
+
+	/* Retrieve smc context struct address */
+	smc_context = smc_get_ctx(NON_SECURE);
+
+	/* Retrieve smc context struct address */
+	cpu_context = cm_get_context(NON_SECURE);
+
+	/* Save context in Backup SRAM */
+	memcpy(&backup_bl32_data->saved_smc_context[0], smc_context,
+	       sizeof(smc_ctx_t) * PLATFORM_CORE_COUNT);
+	memcpy(&backup_bl32_data->saved_cpu_context[0], cpu_context,
+	       sizeof(cpu_context_t) * PLATFORM_CORE_COUNT);
+
+	memcpy(&backup_bl32_data->rtc, rtc_time, sizeof(struct stm32_rtc_calendar));
+	backup_bl32_data->stgen = stgen_cnt;
 
-	stm32mp_clk_enable(RTCAPB);
+	stm32mp1_pm_save_scmi_state(backup_bl32_data->scmi_context,
+				    sizeof(backup_bl32_data->scmi_context));
 
-	mmio_clrsetbits_32(bkpr_itf_idx,
-			   TAMP_BOOT_ITF_MASK,
-			   ((interface << 4) | (instance & 0xFU)) <<
-			   TAMP_BOOT_ITF_SHIFT);
+	save_clock_pm_context();
 
-	stm32mp_clk_disable(RTCAPB);
+	clk_disable(BKPSRAM);
 
 	return 0;
 }
+
+int stm32_restore_context(void)
+{
+	void *smc_context;
+	void *cpu_context;
+	struct backup_data_s *backup_data;
+	struct backup_bl32_data_s *backup_bl32_data;
+	struct stm32_rtc_calendar current_calendar;
+	unsigned long long stdby_time_in_ms;
+
+	/* Context & Data to be saved at the beginning of Backup SRAM */
+	backup_data = (struct backup_data_s *)STM32MP_BACKUP_RAM_BASE;
+
+	clk_enable(BKPSRAM);
+
+	stm32mp1_clk_lp_load_opp_pll1_settings(backup_data->pll1_settings,
+					       sizeof(backup_data->pll1_settings));
+
+	backup_bl32_data = get_bl32_backup_data();
+
+	if (backup_bl32_data->canary_id != BL32_CANARY_ID) {
+		ERROR("Incorrect BL32 backup data\n");
+		return -EINVAL;
+	}
+
+	/* Retrieve smc context struct address */
+	smc_context = smc_get_ctx(NON_SECURE);
+
+	/* Retrieve smc context struct address */
+	cpu_context = cm_get_context(NON_SECURE);
+
+	restore_clock_pm_context();
+
+	stm32mp1_pm_restore_scmi_state(backup_bl32_data->scmi_context,
+				       sizeof(backup_bl32_data->scmi_context));
+
+	/* Restore data from Backup SRAM */
+	memcpy(smc_context, backup_bl32_data->saved_smc_context,
+	       sizeof(smc_ctx_t) * PLATFORM_CORE_COUNT);
+	memcpy(cpu_context, backup_bl32_data->saved_cpu_context,
+	       sizeof(cpu_context_t) * PLATFORM_CORE_COUNT);
+
+	/* Restore STGEN counter with standby mode length */
+	stm32_rtc_get_calendar(&current_calendar);
+	stdby_time_in_ms = stm32_rtc_diff_calendar(&current_calendar,
+						   &backup_bl32_data->rtc);
+	stm32mp_stgen_restore_counter(backup_bl32_data->stgen, stdby_time_in_ms);
+
+	clk_disable(BKPSRAM);
+
+	stm32mp1_clock_resume();
+
+	return 0;
+}
+
+unsigned long long stm32_get_stgen_from_context(void)
+{
+	struct backup_bl32_data_s *backup_data;
+	unsigned long long stgen_cnt;
+
+	clk_enable(BKPSRAM);
+
+	backup_data = get_bl32_backup_data();
+
+	stgen_cnt = backup_data->stgen;
+
+	clk_disable(BKPSRAM);
+
+	return stgen_cnt;
+}
+
+void stm32_context_get_bl2_low_power_params(uintptr_t *bl2_code_base,
+					    uintptr_t *bl2_code_end,
+					    uintptr_t *bl2_end)
+{
+	struct backup_data_s *backup_data;
+
+	clk_enable(BKPSRAM);
+
+	backup_data = (struct backup_data_s *)STM32MP_BACKUP_RAM_BASE;
+
+	if (MAGIC_ID(backup_data->magic) != MAILBOX_MAGIC_V3) {
+		panic();
+	}
+
+#if STM32MP_SP_MIN_IN_DDR
+	stm32_pwr_down_wfi = (void (*)(bool))backup_data->low_power_ep;
+#endif
+	*bl2_code_base = (uintptr_t)backup_data->bl2_code_base;
+	*bl2_code_end = (uintptr_t)backup_data->bl2_code_end;
+	*bl2_end = (uintptr_t)backup_data->bl2_end;
+
+	clk_disable(BKPSRAM);
+}
+
+#endif /* IMAGE_BL32 */
+
+#if defined(IMAGE_BL2)
+void stm32_context_save_bl2_param(void)
+{
+	struct backup_data_s *backup_data;
+
+	clk_enable(BKPSRAM);
+
+	backup_data = (struct backup_data_s *)STM32MP_BACKUP_RAM_BASE;
+
+	backup_data->low_power_ep = (uint32_t)&stm32_pwr_down_wfi_wrapper;
+	backup_data->bl2_code_base = BL_CODE_BASE;
+	backup_data->bl2_code_end = BL_CODE_END;
+	backup_data->bl2_end = BL2_END;
+	backup_data->magic = MAILBOX_MAGIC_V3;
+
+	clk_disable(BKPSRAM);
+}
+#endif
+
+uint32_t stm32_get_zdata_from_context(void)
+{
+	struct backup_data_s *backup_data;
+	uint32_t zdata;
+
+	clk_enable(BKPSRAM);
+
+	backup_data = (struct backup_data_s *)STM32MP_BACKUP_RAM_BASE;
+
+	zdata = (backup_data->zq0cr0_zdata >> DDRPHYC_ZQ0CRN_ZDATA_SHIFT) &
+		DDRPHYC_ZQ0CRN_ZDATA_MASK;
+
+	clk_disable(BKPSRAM);
+
+	return zdata;
+}
+
+static int pll1_settings_in_context(struct backup_data_s *backup_data)
+{
+	switch (MAGIC_ID(backup_data->magic)) {
+	case MAILBOX_MAGIC_V1:
+		return -ENOENT;
+	case MAILBOX_MAGIC_V2:
+	case MAILBOX_MAGIC_V3:
+		assert(MAGIC_AREA_SIZE(backup_data->magic) ==
+		       TRAINING_AREA_SIZE);
+		return 0;
+	default:
+		panic();
+	}
+}
+
+int stm32_get_pll1_settings_from_context(void)
+{
+	struct backup_data_s *backup_data;
+	int ret;
+
+	backup_data = (struct backup_data_s *)STM32MP_BACKUP_RAM_BASE;
+
+	clk_enable(BKPSRAM);
+
+	ret = pll1_settings_in_context(backup_data);
+	if (ret == 0) {
+		uint8_t *data = (uint8_t *)backup_data->pll1_settings;
+		size_t size = sizeof(backup_data->pll1_settings);
+
+		stm32mp1_clk_lp_load_opp_pll1_settings(data, size);
+	}
+
+	clk_disable(BKPSRAM);
+
+	return ret;
+}
+
+bool stm32_are_pll1_settings_valid_in_context(void)
+{
+	struct backup_data_s *backup_data;
+	uint32_t *data;
+	bool is_valid;
+
+	clk_enable(BKPSRAM);
+
+	backup_data = (struct backup_data_s *)STM32MP_BACKUP_RAM_BASE;
+	data = (uint32_t *)backup_data->pll1_settings;
+
+	is_valid = (data[0] == PLL1_SETTINGS_VALID_ID);
+
+	clk_disable(BKPSRAM);
+
+	return is_valid;
+}
+
+bool stm32_pm_context_is_valid(void)
+{
+	struct backup_data_s *backup_data;
+	bool ret;
+
+	clk_enable(BKPSRAM);
+
+	backup_data = (struct backup_data_s *)STM32MP_BACKUP_RAM_BASE;
+
+	switch (MAGIC_ID(backup_data->magic)) {
+	case MAILBOX_MAGIC_V1:
+	case MAILBOX_MAGIC_V2:
+	case MAILBOX_MAGIC_V3:
+		ret = true;
+		break;
+	default:
+		ret = false;
+		break;
+	}
+
+	clk_disable(BKPSRAM);
+
+	return ret;
+}
+
+#if defined(IMAGE_BL32)
+/*
+ * 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, and will be restored after
+ */
+void stm32_save_ddr_training_area(void)
+{
+	struct backup_data_s *backup_data;
+	int ret __unused;
+
+	clk_enable(BKPSRAM);
+
+	backup_data = (struct backup_data_s *)STM32MP_BACKUP_RAM_BASE;
+
+	ret = mmap_add_dynamic_region(STM32MP_DDR_BASE, STM32MP_DDR_BASE,
+				      PAGE_SIZE, MT_MEMORY | MT_RW | MT_NS);
+	assert(ret == 0);
+
+	memcpy(&backup_data->ddr_training_backup,
+	       (const uint32_t *)STM32MP_DDR_BASE,
+	       TRAINING_AREA_SIZE);
+	dsb();
+
+	ret = mmap_remove_dynamic_region(STM32MP_DDR_BASE, PAGE_SIZE);
+	assert(ret == 0);
+
+	clk_disable(BKPSRAM);
+}
+#endif
+
+void stm32_restore_ddr_training_area(void)
+{
+	struct backup_data_s *backup_data;
+
+	clk_enable(BKPSRAM);
+
+	backup_data = (struct backup_data_s *)STM32MP_BACKUP_RAM_BASE;
+
+	memcpy((uint32_t *)STM32MP_DDR_BASE,
+	       &backup_data->ddr_training_backup,
+	       TRAINING_AREA_SIZE);
+	dsb();
+
+	clk_disable(BKPSRAM);
+}
diff --git a/plat/st/stm32mp1/stm32mp1_critic_power.c b/plat/st/stm32mp1/stm32mp1_critic_power.c
new file mode 100644
index 0000000000..583cf93fad
--- /dev/null
+++ b/plat/st/stm32mp1/stm32mp1_critic_power.c
@@ -0,0 +1,91 @@
+/*
+ * Copyright (C) 2019-2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <platform_def.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/gicv2.h>
+#include <drivers/st/stm32_iwdg.h>
+#include <drivers/st/stm32mp1_ddr_helpers.h>
+
+#include <stm32mp1_critic_power.h>
+
+static void cstop_critic_enter(void)
+{
+	/*
+	 * Set DDR in Self-refresh,.
+	 * This is also the procedure awaited when switching off power supply.
+	 */
+	if (ddr_standby_sr_entry() != 0) {
+		ERROR("Unable to put DDR in SR\n");
+		panic();
+	}
+}
+
+/*
+ * stm32_exit_cstop_critic - Exit from CSTOP mode reenable DDR
+ */
+static void cstop_critic_exit(void)
+{
+	if (ddr_sw_self_refresh_exit() != 0) {
+		panic();
+	}
+}
+
+void stm32_pwr_down_wfi_load(bool is_cstop)
+{
+	if (is_cstop) {
+		cstop_critic_enter();
+	}
+
+	/*
+	 * Synchronize on memory accesses and instruction flow before the WFI
+	 * instruction.
+	 */
+	dsb();
+	isb();
+	wfi();
+
+	stm32_iwdg_refresh();
+
+	if (is_cstop) {
+		cstop_critic_exit();
+	}
+}
+
+#if defined(IMAGE_BL32) && !STM32MP_SP_MIN_IN_DDR
+extern void wfi_svc_int_enable(uintptr_t stack_addr);
+static uint32_t int_stack[STM32MP_INT_STACK_SIZE];
+
+void stm32_pwr_down_wfi(bool is_cstop)
+{
+	uint32_t interrupt = GIC_SPURIOUS_INTERRUPT;
+
+	if (is_cstop) {
+		cstop_critic_enter();
+	}
+
+	stm32mp1_calib_set_wakeup(false);
+
+	while (interrupt == GIC_SPURIOUS_INTERRUPT &&
+	       !stm32mp1_calib_get_wakeup()) {
+		wfi_svc_int_enable((uintptr_t)&int_stack[0]);
+
+		interrupt = gicv2_acknowledge_interrupt();
+
+		if (interrupt != GIC_SPURIOUS_INTERRUPT) {
+			gicv2_end_of_interrupt(interrupt);
+		}
+
+		stm32_iwdg_refresh();
+	}
+
+	if (is_cstop) {
+		cstop_critic_exit();
+	}
+}
+#endif
diff --git a/plat/st/stm32mp1/stm32mp1_critic_power_wrapper.S b/plat/st/stm32mp1/stm32mp1_critic_power_wrapper.S
new file mode 100644
index 0000000000..2c4dd844d2
--- /dev/null
+++ b/plat/st/stm32mp1/stm32mp1_critic_power_wrapper.S
@@ -0,0 +1,62 @@
+/*
+ * Copyright (C) 2019-2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <platform_def.h>
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <common/bl_common.h>
+#include <drivers/st/stm32_gpio.h>
+#include <smccc_helpers.h>
+
+	.global	stm32_pwr_down_wfi_load
+	.global	stm32_pwr_down_wfi_wrapper
+	.global	bl2_vector_table
+	.global	disable_mmu_secure
+	.global	__STACKS_END__
+
+func stm32_pwr_down_wfi_wrapper
+	push	{r4,r5,r6,lr}
+
+	# Save current sp in r4
+	mov	r4, sp
+	# Save current VBAR in r5
+	ldcopr	r5, VBAR
+	# Save current MVBAR in r6
+	ldcopr	r6, MVBAR
+
+	# Reuse BL2 vector table for VBAR and MVBAR
+	ldr	r1, =bl2_vector_table
+	stcopr	r1, VBAR
+	stcopr	r1, MVBAR
+
+	# Set sp to BL2 STACK (as BL2 is not using it anymore)
+	ldr sp, =__STACKS_END__
+
+	# Disable MMU as TLB are still stored in DDR,
+	# and in few instructions DDR won't be readable
+	bl disable_mmu_secure
+
+	# dsb is done in disable mmu
+	# isb is done in disable mmu
+
+	# We didn't change R0 to keep it as first parameter
+	bl	 stm32_pwr_down_wfi_load
+
+	# Restore stack
+	mov	 sp, r4
+	# Restore VBAR
+	stcopr	r5, VBAR
+	# Restore MVBAR
+	stcopr	r6, MVBAR
+
+	# Synchronize on memory access and instruction
+	# after resetting stack/IT handler
+	dsb
+	isb
+
+	pop	{r4,r5,r6,pc}
+endfunc stm32_pwr_down_wfi_wrapper
diff --git a/plat/st/stm32mp1/stm32mp1_dbgmcu.c b/plat/st/stm32mp1/stm32mp1_dbgmcu.c
index d0264968c6..eac2cf68c5 100644
--- a/plat/st/stm32mp1/stm32mp1_dbgmcu.c
+++ b/plat/st/stm32mp1/stm32mp1_dbgmcu.c
@@ -1,15 +1,17 @@
 /*
- * Copyright (c) 2016-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2016-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <assert.h>
 #include <errno.h>
 
 #include <platform_def.h>
 
 #include <common/debug.h>
 #include <drivers/st/bsec.h>
+#include <drivers/st/bsec2_reg.h>
 #include <drivers/st/stm32mp1_rcc.h>
 #include <lib/mmio.h>
 #include <lib/utils_def.h>
@@ -25,36 +27,27 @@
 
 #define DBGMCU_APB4FZ1_IWDG2	BIT(2)
 
-static uintptr_t get_rcc_base(void)
-{
-	/* This is called before stm32mp_rcc_base() is available */
-	return RCC_BASE;
-}
-
 static int stm32mp1_dbgmcu_init(void)
 {
-	uint32_t dbg_conf;
-	uintptr_t rcc_base = get_rcc_base();
-
-	dbg_conf = bsec_read_debug_conf();
-
-	if ((dbg_conf & BSEC_DBGSWGEN) == 0U) {
-		uint32_t result = bsec_write_debug_conf(dbg_conf |
-							BSEC_DBGSWGEN);
-
-		if (result != BSEC_OK) {
-			ERROR("Error enabling DBGSWGEN\n");
-			return -1;
-		}
+	if ((bsec_read_debug_conf() & BSEC_DBGSWGEN) == 0U) {
+		INFO("Software access to all debug components is disabled\n");
+		return -1;
 	}
 
-	mmio_setbits_32(rcc_base + RCC_DBGCFGR, RCC_DBGCFGR_DBGCKEN);
+	mmio_setbits_32(RCC_BASE + RCC_DBGCFGR, RCC_DBGCFGR_DBGCKEN);
 
 	return 0;
 }
 
+/*
+ * @brief  Get silicon revision from DBGMCU registers.
+ * @param  chip_version: pointer to the read value.
+ * @retval 0 on success, negative value on failure.
+ */
 int stm32mp1_dbgmcu_get_chip_version(uint32_t *chip_version)
 {
+	assert(chip_version != NULL);
+
 	if (stm32mp1_dbgmcu_init() != 0) {
 		return -EPERM;
 	}
@@ -65,18 +58,29 @@ int stm32mp1_dbgmcu_get_chip_version(uint32_t *chip_version)
 	return 0;
 }
 
+/*
+ * @brief  Get device ID from DBGMCU registers.
+ * @param  chip_dev_id: pointer to the read value.
+ * @retval 0 on success, negative value on failure.
+ */
 int stm32mp1_dbgmcu_get_chip_dev_id(uint32_t *chip_dev_id)
 {
+	assert(chip_dev_id != NULL);
+
 	if (stm32mp1_dbgmcu_init() != 0) {
 		return -EPERM;
 	}
 
 	*chip_dev_id = mmio_read_32(DBGMCU_BASE + DBGMCU_IDC) &
-		DBGMCU_IDC_DEV_ID_MASK;
+		       DBGMCU_IDC_DEV_ID_MASK;
 
 	return 0;
 }
 
+/*
+ * @brief  Freeze IWDG2 in debug mode.
+ * @retval None.
+ */
 int stm32mp1_dbgmcu_freeze_iwdg2(void)
 {
 	uint32_t dbg_conf;
diff --git a/plat/st/stm32mp1/stm32mp1_def.h b/plat/st/stm32mp1/stm32mp1_def.h
index ee04a23fd9..027dba1bde 100644
--- a/plat/st/stm32mp1/stm32mp1_def.h
+++ b/plat/st/stm32mp1/stm32mp1_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -11,18 +11,22 @@
 #include <drivers/st/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 <lib/utils_def.h>
 #include <lib/xlat_tables/xlat_tables_defs.h>
 
 #ifndef __ASSEMBLER__
 #include <drivers/st/bsec.h>
+#include <drivers/st/stm32mp1_calib.h>
 #include <drivers/st/stm32mp1_clk.h>
+#include <drivers/st/stm32mp1_ddr_regs.h>
+#include <drivers/st/stm32mp1_pwr.h>
 
 #include <boot_api.h>
-#include <stm32mp_auth.h>
 #include <stm32mp_common.h>
 #include <stm32mp_dt.h>
 #include <stm32mp_shres_helpers.h>
+#include <stm32mp1_context.h>
 #include <stm32mp1_dbgmcu.h>
 #include <stm32mp1_private.h>
 #include <stm32mp1_shared_resources.h>
@@ -31,6 +35,8 @@
 /*******************************************************************************
  * CHIP ID
  ******************************************************************************/
+#define STM32MP1_CHIP_ID	U(0x500)
+
 #define STM32MP157C_PART_NB	U(0x05000000)
 #define STM32MP157A_PART_NB	U(0x05000001)
 #define STM32MP153C_PART_NB	U(0x05000024)
@@ -55,15 +61,27 @@
 #define PKG_AC_TFBGA361		U(2)
 #define PKG_AD_TFBGA257		U(1)
 
+/*******************************************************************************
+ * BOOT PARAM
+ ******************************************************************************/
+#define BOOT_PARAM_ADDR			U(0x2FFC0078)
+
 /*******************************************************************************
  * STM32MP1 memory map related constants
  ******************************************************************************/
 #define STM32MP_ROM_BASE		U(0x00000000)
 #define STM32MP_ROM_SIZE		U(0x00020000)
+#define STM32MP_ROM_SIZE_2MB_ALIGNED	U(0x00200000)
 
 #define STM32MP_SYSRAM_BASE		U(0x2FFC0000)
 #define STM32MP_SYSRAM_SIZE		U(0x00040000)
 
+#define STM32MP_RETRAM_BASE		U(0x38000000)
+#define STM32MP_RETRAM_SIZE		U(0x00010000)
+
+#define STM32MP_BACKUP_RAM_BASE		U(0x54000000)
+#define STM32MP_BACKUP_RAM_SIZE		U(0x00001000)
+
 #define STM32MP_NS_SYSRAM_SIZE		PAGE_SIZE
 #define STM32MP_NS_SYSRAM_BASE		(STM32MP_SYSRAM_BASE + \
 					 STM32MP_SYSRAM_SIZE - \
@@ -79,12 +97,13 @@
 /* DDR configuration */
 #define STM32MP_DDR_BASE		U(0xC0000000)
 #define STM32MP_DDR_MAX_SIZE		U(0x40000000)	/* Max 1GB */
-#ifdef AARCH32_SP_OPTEE
+#if STM32MP_USE_STM32IMAGE
 #define STM32MP_DDR_S_SIZE		U(0x01E00000)	/* 30 MB */
+#ifdef AARCH32_SP_OPTEE
 #define STM32MP_DDR_SHMEM_SIZE		U(0x00200000)	/* 2 MB */
+#endif
 #else
-#define STM32MP_DDR_S_SIZE		U(0)
-#define STM32MP_DDR_SHMEM_SIZE		U(0)
+#define STM32MP_DDR_S_SIZE		U(0x02000000)	/* 32 MB */
 #endif
 
 /* DDR power initializations */
@@ -100,6 +119,8 @@ enum ddr_type {
 #define STM32MP_PARAM_LOAD_SIZE		U(0x00002400)	/* 9 KB for param */
 /* 256 Octets reserved for header */
 #define STM32MP_HEADER_SIZE		U(0x00000100)
+/* round_up(STM32MP_PARAM_LOAD_SIZE + STM32MP_HEADER_SIZE, PAGE_SIZE) */
+#define STM32MP_HEADER_RESERVED_SIZE	U(0x3000)
 
 #define STM32MP_BINARY_BASE		(STM32MP_SEC_SYSRAM_BASE +	\
 					 STM32MP_PARAM_LOAD_SIZE +	\
@@ -109,83 +130,214 @@ enum ddr_type {
 					 (STM32MP_PARAM_LOAD_SIZE +	\
 					  STM32MP_HEADER_SIZE))
 
-#ifdef AARCH32_SP_OPTEE
-#define STM32MP_BL32_SIZE		U(0)
+#if !STM32MP_SSP
+#if STM32MP_USE_STM32IMAGE
+#define STM32MP_BL2_SIZE		U(0x0001C000)	/* 112 KB for BL2 */
 
+#ifdef AARCH32_SP_OPTEE
 #define STM32MP_OPTEE_BASE		STM32MP_SEC_SYSRAM_BASE
 
-#define STM32MP_OPTEE_SIZE		(STM32MP_DTB_BASE -  \
+#define STM32MP_OPTEE_SIZE		(TF_A_MAPPING_START -  \
 					 STM32MP_OPTEE_BASE)
-#else
-#if STACK_PROTECTOR_ENABLED
-#define STM32MP_BL32_SIZE		U(0x00012000)	/* 72 KB for BL32 */
-#else
-#define STM32MP_BL32_SIZE		U(0x00011000)	/* 68 KB for BL32 */
-#endif
-#endif
+
+#define STM32MP_BL2_BASE		(STM32MP_SEC_SYSRAM_BASE + \
+					 STM32MP_SEC_SYSRAM_SIZE - \
+					 STM32MP_BL2_SIZE)
+
+#define STM32MP_BL32_BASE		STM32MP_SEC_SYSRAM_BASE
+
+#else /* AARCH32_SP_OPTEE */
+#define STM32MP_BL32_SIZE		U(0x0001A000)	/* 104 KB for BL32 */
+#define STM32MP_BL32_BIN_SIZE		STM32MP_BL32_SIZE
 
 #define STM32MP_BL32_BASE		(STM32MP_SEC_SYSRAM_BASE + \
 					 STM32MP_SEC_SYSRAM_SIZE - \
 					 STM32MP_BL32_SIZE)
 
-#ifdef AARCH32_SP_OPTEE
-#if STACK_PROTECTOR_ENABLED
-#define STM32MP_BL2_SIZE		U(0x0001A000)	/* 100 KB for BL2 */
+#define STM32MP_BL2_BASE		(STM32MP_BL32_BASE - \
+					 STM32MP_BL2_SIZE)
+#endif /* AARCH32_SP_OPTEE */
+#else /* STM32MP_USE_STM32IMAGE */
+#if TRUSTED_BOARD_BOOT && !STM32MP_USE_EXTERNAL_HEAP
+#define STM32MP_BL2_RO_SIZE		U(0x00014000)	/* 80 KB */
+#define STM32MP_BL2_SIZE		U(0x0001B000)	/* 108 KB for BL2 */
 #else
-#define STM32MP_BL2_SIZE		U(0x00018000)	/* 92 KB for BL2 */
+#define STM32MP_BL2_RO_SIZE		U(0x00010000)	/* 64 KB */
+#define STM32MP_BL2_SIZE		U(0x00015000)	/* 84 KB for BL2 */
 #endif
+
+#define STM32MP_BL2_BASE		(STM32MP_SEC_SYSRAM_BASE + \
+					 STM32MP_SEC_SYSRAM_SIZE - \
+					 STM32MP_BL2_SIZE)
+
+#define STM32MP_BL2_RO_BASE		STM32MP_BL2_BASE
+
+#define STM32MP_BL2_RW_BASE		(STM32MP_BL2_RO_BASE + \
+					 STM32MP_BL2_RO_SIZE)
+
+#define STM32MP_BL2_RW_SIZE		STM32MP_SEC_SYSRAM_BASE + \
+					STM32MP_SEC_SYSRAM_SIZE - \
+					STM32MP_BL2_RW_BASE
+
+#if STM32MP_SP_MIN_IN_DDR
+#define STM32MP_BL32_SIZE		U(0x00025000)	/* 148 KB for BL32 */
 #else
-#if STACK_PROTECTOR_ENABLED
-#define STM32MP_BL2_SIZE		U(0x00019000)	/* 96 KB for BL2 */
-#else
-#define STM32MP_BL2_SIZE		U(0x00017000)	/* 88 KB for BL2 */
+#define STM32MP_BL32_SIZE		U(0x0001A000)	/* 100 KB for BL32 */
 #endif
+#endif /* STM32MP_USE_STM32IMAGE */
+#endif /* STM32MP_SSP */
+
+#if defined(IMAGE_BL2)
+ #define STM32MP_DEFAULT_XLAT		U(2) /* 8 KB for mapping */
 #endif
 
-#define STM32MP_BL2_BASE		(STM32MP_BL32_BASE - \
-					 STM32MP_BL2_SIZE)
+/* BL32/sp_min require finer granularity tables */
+#if defined(IMAGE_BL32)
+ #define STM32MP_DEFAULT_XLAT		U(4) /* 16 KB for mapping */
+#endif
 
-/* BL2 and BL32/sp_min require 4 tables */
-#define MAX_XLAT_TABLES			U(4)		/* 16 KB for mapping */
+#if STM32MP_SP_MIN_IN_DDR && defined(IMAGE_BL32)
+ #define STM32MP_SP_MIN_IN_DDR_XLAT	U(3) /* 12KB for mapping
+					      * (BL32 data, BL32 DT, SCMI buffers)
+					      */
+#else
+ #define STM32MP_SP_MIN_IN_DDR_XLAT	U(0)
+#endif
 
+#define MAX_XLAT_TABLES			(STM32MP_DEFAULT_XLAT + \
+					 STM32MP_SP_MIN_IN_DDR_XLAT)
 /*
  * MAX_MMAP_REGIONS is usually:
  * BL stm32mp1_mmap size + mmap regions in *_plat_arch_setup
  */
 #if defined(IMAGE_BL2)
-  #define MAX_MMAP_REGIONS		11
+ #if STM32MP_USB_PROGRAMMER
+  #define MAX_MMAP_REGIONS		8
+ #else
+  #if STM32MP_USE_STM32IMAGE
+    #define MAX_MMAP_REGIONS		8
+  #else
+    #define MAX_MMAP_REGIONS		7
+  #endif
+ #endif
 #endif
 #if defined(IMAGE_BL32)
-  #define MAX_MMAP_REGIONS		6
+  #if STM32MP_USE_STM32IMAGE
+    #define MAX_MMAP_REGIONS		6
+  #else
+    #define MAX_MMAP_REGIONS		10
+  #endif
 #endif
 
+#if STM32MP_SSP
+#define STM32MP_BL2_DTB_SIZE		U(0x00004000)	/* 16 KB for DTB */
+
+#define STM32MP_BL2_DTB_BASE		(STM32MP_SYSRAM_BASE + \
+					 STM32MP_HEADER_RESERVED_SIZE)
+
+#define STM32MP_BL2_RO_SIZE		U(0x0000C000)	/* 48 Ko for BL2 */
+
+#define STM32MP_BL2_RO_BASE		STM32MP_BL2_DTB_BASE + \
+					STM32MP_BL2_DTB_SIZE
+
+#define STM32MP_BL2_RW_BASE		(STM32MP_BL2_RO_BASE + \
+					 STM32MP_BL2_RO_SIZE)
+
+#define STM32MP_BL2_RW_SIZE		(STM32MP_SYSRAM_BASE + \
+					 STM32MP_SYSRAM_SIZE - \
+					 STM32MP_BL2_RW_BASE)
+
+#define STM32MP_DTB_SIZE		STM32MP_BL2_DTB_SIZE
+#define STM32MP_DTB_BASE		STM32MP_BL2_DTB_BASE
+
+#define TF_A_MAPPING_START		STM32MP_BL2_DTB_BASE
+#else
+#if STM32MP_USE_STM32IMAGE
 /* DTB initialization value */
-#define STM32MP_DTB_SIZE		U(0x00005000)	/* 20 KB for DTB */
+#define STM32MP_DTB_SIZE		U(0x00006000)	/* 24 KB for DTB */
 
-#define STM32MP_DTB_BASE		(STM32MP_BL2_BASE - \
+#define STM32MP_DTB_BASE		(STM32MP_BL2_BASE -	\
 					 STM32MP_DTB_SIZE)
+#define TF_A_MAPPING_START		STM32MP_DTB_BASE
+#else /* STM32MP_USE_STM32IMAGE */
+#define STM32MP_BL2_DTB_SIZE		U(0x00007000)	/* 28 KB for DTB */
+#define STM32MP_BL2_DTB_BASE		(STM32MP_BL2_BASE - \
+					 STM32MP_BL2_DTB_SIZE)
 
+#define STM32MP_BL32_DTB_SIZE		U(0x00005000)	/* 20 KB for DTB */
+#define STM32MP_BL32_DTB_BASE		STM32MP_SYSRAM_BASE
+
+#define STM32MP_BL32_BASE		(STM32MP_BL32_DTB_BASE + \
+					 STM32MP_BL32_DTB_SIZE)
+
+#if defined(IMAGE_BL2)
+#define STM32MP_DTB_SIZE		STM32MP_BL2_DTB_SIZE
+#define STM32MP_DTB_BASE		STM32MP_BL2_DTB_BASE
+#endif
+#if defined(IMAGE_BL32)
+#define STM32MP_DTB_SIZE		STM32MP_BL32_DTB_SIZE
+#define STM32MP_DTB_BASE		STM32MP_BL32_DTB_BASE
+#endif
+#define TF_A_MAPPING_START		STM32MP_BL2_DTB_BASE
+#endif /* STM32MP_USE_STM32IMAGE */
+#endif /* STM32MP_SSP */
+
+#define STM32MP_FW_CONFIG_BASE		(STM32MP_SYSRAM_BASE + \
+					 STM32MP_SYSRAM_SIZE - \
+					 PAGE_SIZE)
+#define STM32MP_FW_CONFIG_MAX_SIZE	PAGE_SIZE
 #define STM32MP_BL33_BASE		(STM32MP_DDR_BASE + U(0x100000))
+#define STM32MP_BL33_MAX_SIZE		U(0x400000)
+#define STM32MP_HW_CONFIG_BASE		(STM32MP_BL33_BASE + \
+					STM32MP_BL33_MAX_SIZE)
+#define STM32MP_HW_CONFIG_MAX_SIZE	U(0x20000)
+
+/* Define Temporary Stack size use during low power mode */
+#define STM32MP_INT_STACK_SIZE		0x200
 
 /* Define maximum page size for NAND devices */
 #define PLATFORM_MTD_MAX_PAGE_SIZE	U(0x1000)
 
+/*
+ * Only used for MTD devices that need some backup blocks.
+ * Must define a number of reserved blocks (depends on devices).
+ */
+#define PLATFORM_MTD_BACKUP_BLOCKS	U(20)	/* (20 * MTD block size) */
+
 /*******************************************************************************
  * STM32MP1 RAW partition offset for MTD devices
  ******************************************************************************/
-#define STM32MP_NOR_BL33_OFFSET		U(0x00080000)
+#if STM32MP_USE_STM32IMAGE
+#ifndef STM32MP_NOR_BASE_OFFSET
+#define STM32MP_NOR_BASE_OFFSET		U(0x00080000)
+#endif
+#define STM32MP_NOR_BL33_OFFSET		STM32MP_NOR_BASE_OFFSET
 #ifdef AARCH32_SP_OPTEE
-#define STM32MP_NOR_TEEH_OFFSET		U(0x00280000)
-#define STM32MP_NOR_TEED_OFFSET		U(0x002C0000)
-#define STM32MP_NOR_TEEX_OFFSET		U(0x00300000)
+#define STM32MP_NOR_TEEH_OFFSET		U(0x00300000)
+#define STM32MP_NOR_TEED_OFFSET		U(0x00340000)
+#define STM32MP_NOR_TEEX_OFFSET		U(0x003C0000)
 #endif
 
-#define STM32MP_NAND_BL33_OFFSET	U(0x00200000)
+#ifndef STM32MP_NAND_BASE_OFFSET
+#define STM32MP_NAND_BASE_OFFSET	U(0x00200000)
+#endif
+#define STM32MP_NAND_BL33_OFFSET	STM32MP_NAND_BASE_OFFSET
 #ifdef AARCH32_SP_OPTEE
-#define STM32MP_NAND_TEEH_OFFSET	U(0x00600000)
-#define STM32MP_NAND_TEED_OFFSET	U(0x00680000)
-#define STM32MP_NAND_TEEX_OFFSET	U(0x00700000)
+#define STM32MP_NAND_TEEH_OFFSET	(STM32MP_NAND_BASE_OFFSET + \
+					 U(0x00400000))
+#define STM32MP_NAND_TEED_OFFSET	(STM32MP_NAND_BASE_OFFSET + \
+					 U(0x00480000))
+#define STM32MP_NAND_TEEX_OFFSET	(STM32MP_NAND_BASE_OFFSET + \
+					 U(0x00500000))
 #endif
+#else /* STM32MP_USE_STM32IMAGE */
+#ifndef STM32MP_NOR_FIP_OFFSET
+#define STM32MP_NOR_FIP_OFFSET		U(0x00080000)
+#endif
+#ifndef STM32MP_NAND_FIP_OFFSET
+#define STM32MP_NAND_FIP_OFFSET		U(0x00200000)
+#endif
+#endif /* STM32MP_USE_STM32IMAGE */
 
 /*******************************************************************************
  * STM32MP1 device/io map related constants (used for MMU)
@@ -206,6 +358,22 @@ enum ddr_type {
  ******************************************************************************/
 #define PWR_BASE			U(0x50001000)
 
+/*******************************************************************************
+ * STM32MP1 EXTI
+ ******************************************************************************/
+#define EXTI_BASE			U(0x5000D000)
+#define EXTI_TZENR1			U(0x14)
+#define EXTI_RPR3			U(0x4C)
+#define EXTI_FPR3			U(0x50)
+#define EXTI_C1IMR1			U(0x80)
+#define EXTI_C2IMR1			U(0xC0)
+#define EXTI_C2IMR2			U(0xD0)
+#define EXTI_C2IMR3			U(0xE0)
+#define EXTI_TZENR1_TZEN18		BIT(18)
+#define EXTI_IMR1_IM18			BIT(18)
+#define EXTI_RPR3_RPIF65		BIT(1)
+#define EXTI_FPR3_FPIF65		BIT(1)
+
 /*******************************************************************************
  * STM32MP1 GPIO
  ******************************************************************************/
@@ -277,111 +445,12 @@ enum ddr_type {
 
 #define STM32MP1_ETZPC_TZMA_ALL_SECURE	GENMASK_32(9, 0)
 
-/* ETZPC DECPROT IDs */
-#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_RNG1_ID		7
-#define STM32MP1_ETZPC_HASH1_ID		8
-#define STM32MP1_ETZPC_CRYP1_ID		9
-#define STM32MP1_ETZPC_DDRCTRL_ID	10
-#define STM32MP1_ETZPC_DDRPHYC_ID	11
-#define STM32MP1_ETZPC_I2C6_ID		12
-#define STM32MP1_ETZPC_SEC_ID_LIMIT	13
-
-#define STM32MP1_ETZPC_TIM2_ID		16
-#define STM32MP1_ETZPC_TIM3_ID		17
-#define STM32MP1_ETZPC_TIM4_ID		18
-#define STM32MP1_ETZPC_TIM5_ID		19
-#define STM32MP1_ETZPC_TIM6_ID		20
-#define STM32MP1_ETZPC_TIM7_ID		21
-#define STM32MP1_ETZPC_TIM12_ID		22
-#define STM32MP1_ETZPC_TIM13_ID		23
-#define STM32MP1_ETZPC_TIM14_ID		24
-#define STM32MP1_ETZPC_LPTIM1_ID	25
-#define STM32MP1_ETZPC_WWDG1_ID		26
-#define STM32MP1_ETZPC_SPI2_ID		27
-#define STM32MP1_ETZPC_SPI3_ID		28
-#define STM32MP1_ETZPC_SPDIFRX_ID	29
-#define STM32MP1_ETZPC_USART2_ID	30
-#define STM32MP1_ETZPC_USART3_ID	31
-#define STM32MP1_ETZPC_UART4_ID		32
-#define STM32MP1_ETZPC_UART5_ID		33
-#define STM32MP1_ETZPC_I2C1_ID		34
-#define STM32MP1_ETZPC_I2C2_ID		35
-#define STM32MP1_ETZPC_I2C3_ID		36
-#define STM32MP1_ETZPC_I2C5_ID		37
-#define STM32MP1_ETZPC_CEC_ID		38
-#define STM32MP1_ETZPC_DAC_ID		39
-#define STM32MP1_ETZPC_UART7_ID		40
-#define STM32MP1_ETZPC_UART8_ID		41
-#define STM32MP1_ETZPC_MDIOS_ID		44
-#define STM32MP1_ETZPC_TIM1_ID		48
-#define STM32MP1_ETZPC_TIM8_ID		49
-#define STM32MP1_ETZPC_USART6_ID	51
-#define STM32MP1_ETZPC_SPI1_ID		52
-#define STM32MP1_ETZPC_SPI4_ID		53
-#define STM32MP1_ETZPC_TIM15_ID		54
-#define STM32MP1_ETZPC_TIM16_ID		55
-#define STM32MP1_ETZPC_TIM17_ID		56
-#define STM32MP1_ETZPC_SPI5_ID		57
-#define STM32MP1_ETZPC_SAI1_ID		58
-#define STM32MP1_ETZPC_SAI2_ID		59
-#define STM32MP1_ETZPC_SAI3_ID		60
-#define STM32MP1_ETZPC_DFSDM_ID		61
-#define STM32MP1_ETZPC_TT_FDCAN_ID	62
-#define STM32MP1_ETZPC_LPTIM2_ID	64
-#define STM32MP1_ETZPC_LPTIM3_ID	65
-#define STM32MP1_ETZPC_LPTIM4_ID	66
-#define STM32MP1_ETZPC_LPTIM5_ID	67
-#define STM32MP1_ETZPC_SAI4_ID		68
-#define STM32MP1_ETZPC_VREFBUF_ID	69
-#define STM32MP1_ETZPC_DCMI_ID		70
-#define STM32MP1_ETZPC_CRC2_ID		71
-#define STM32MP1_ETZPC_ADC_ID		72
-#define STM32MP1_ETZPC_HASH2_ID		73
-#define STM32MP1_ETZPC_RNG2_ID		74
-#define STM32MP1_ETZPC_CRYP2_ID		75
-#define STM32MP1_ETZPC_SRAM1_ID		80
-#define STM32MP1_ETZPC_SRAM2_ID		81
-#define STM32MP1_ETZPC_SRAM3_ID		82
-#define STM32MP1_ETZPC_SRAM4_ID		83
-#define STM32MP1_ETZPC_RETRAM_ID	84
-#define STM32MP1_ETZPC_OTG_ID		85
-#define STM32MP1_ETZPC_SDMMC3_ID	86
-#define STM32MP1_ETZPC_DLYBSD3_ID	87
-#define STM32MP1_ETZPC_DMA1_ID		88
-#define STM32MP1_ETZPC_DMA2_ID		89
-#define STM32MP1_ETZPC_DMAMUX_ID	90
-#define STM32MP1_ETZPC_FMC_ID		91
-#define STM32MP1_ETZPC_QSPI_ID		92
-#define STM32MP1_ETZPC_DLYBQ_ID		93
-#define STM32MP1_ETZPC_ETH_ID		94
-#define STM32MP1_ETZPC_RSV_ID		95
-
-#define STM32MP_ETZPC_MAX_ID		96
-
 /*******************************************************************************
  * STM32MP1 TZC (TZ400)
  ******************************************************************************/
 #define STM32MP1_TZC_BASE		U(0x5C006000)
 
-#define STM32MP1_TZC_A7_ID		U(0)
-#define STM32MP1_TZC_M4_ID		U(1)
-#define STM32MP1_TZC_LCD_ID		U(3)
-#define STM32MP1_TZC_GPU_ID		U(4)
-#define STM32MP1_TZC_MDMA_ID		U(5)
-#define STM32MP1_TZC_DMA_ID		U(6)
-#define STM32MP1_TZC_USB_HOST_ID	U(7)
-#define STM32MP1_TZC_USB_OTG_ID		U(8)
-#define STM32MP1_TZC_SDMMC_ID		U(9)
-#define STM32MP1_TZC_ETH_ID		U(10)
-#define STM32MP1_TZC_DAP_ID		U(15)
-
-#define STM32MP1_FILTER_BIT_ALL		U(3)
+#define STM32MP1_FILTER_BIT_ALL		(BIT(1) | BIT(0))
 
 /*******************************************************************************
  * STM32MP1 SDMMC
@@ -404,16 +473,44 @@ enum ddr_type {
 
 #define OTP_MAX_SIZE			(STM32MP1_OTP_MAX_ID + 1U)
 
-/* OTP offsets */
-#define DATA0_OTP			U(0)
-#define PART_NUMBER_OTP			U(1)
-#define NAND_OTP			U(9)
-#define PACKAGE_OTP			U(16)
-#define HW2_OTP				U(18)
+/* OTP labels */
+#define CFG0_OTP			"cfg0_otp"
+#define PART_NUMBER_OTP			"part_number_otp"
+#define PACKAGE_OTP			"package_otp"
+#define HW2_OTP				"hw2_otp"
+#define NAND_OTP			"nand_otp"
+#define MONOTONIC_OTP			"monotonic_otp"
+#define UID_OTP				"uid_otp"
+#define PKH_OTP				"pkh_otp"
+#define BOARD_ID_OTP			"board_id"
+#define CFG2_OTP			"cfg2_otp"
+#define SSP_OTP				"ssp_otp"
+#define CHIP_CERTIFICATE_OTP		"chip_otp"
+#define RMA_OTP				"rma_otp"
 
 /* OTP mask */
-/* DATA0 */
-#define DATA0_OTP_SECURED		BIT(6)
+/* CFG0 */
+#define CFG0_CLOSED_DEVICE		BIT(6)
+
+/* CFG2 */
+#define OTP_CFG2_SEC_COUNTER_MASK	GENMASK_32(27, 20)
+#define OTP_CFG2_SEC_COUNTER_SHIFT	U(20)
+#define OTP_CFG2_ST_KEY_MASK		GENMASK_32(31, 28)
+#define OTP_CFG2_ST_KEY_SHIFT		U(28)
+
+/* SSP */
+#define SSP_OTP_REQ			BIT(BOOT_API_OTP_SSP_REQ_BIT_POS)
+#define SSP_OTP_SUCCESS			BIT(BOOT_API_OTP_SSP_SUCCESS_BIT_POS)
+#define SSP_OTP_MASK			GENMASK_32(BOOT_API_OTP_SSP_SUCCESS_BIT_POS, \
+						   BOOT_API_OTP_SSP_REQ_BIT_POS)
+#define SSP_OTP_SECRET_BASE		U(59)
+#define SSP_OTP_SECRET_END		U(95)
+
+/* CHIP_CERT */
+#define CHIP_CERTIFICATE_MAX_SIZE	U(0x40)
+
+/* RMA */
+#define RMA_OTP_MASK			GENMASK_32(29, 0)
 
 /* PART NUMBER */
 #define PART_NUMBER_OTP_PART_MASK	GENMASK_32(7, 0)
@@ -470,11 +567,24 @@ enum ddr_type {
 /* NAND number of planes */
 #define NAND_PLANE_BIT_NB_MASK		BIT(14)
 
+/* MONOTONIC OTP */
+#define MAX_MONOTONIC_VALUE		32
+
+/* UID OTP */
+#define UID_WORD_NB			3
+
 /*******************************************************************************
  * STM32MP1 TAMP
  ******************************************************************************/
+#define PLAT_MAX_TAMP_INT		U(6)
+#define PLAT_MAX_TAMP_EXT		U(3)
 #define TAMP_BASE			U(0x5C00A000)
+#define TAMP_SMCR			(TAMP_BASE + U(0x20))
 #define TAMP_BKP_REGISTER_BASE		(TAMP_BASE + U(0x100))
+#define TAMP_BKP_SEC_NUMBER		U(10)
+#define TAMP_BKP_SEC_WDPROT_SHIFT	U(16)
+#define TAMP_BKP_SEC_RWDPROT_SHIFT	U(0)
+
 
 #if !(defined(__LINKER__) || defined(__ASSEMBLER__))
 static inline uint32_t tamp_bkpr(uint32_t idx)
@@ -483,6 +593,11 @@ static inline uint32_t tamp_bkpr(uint32_t idx)
 }
 #endif
 
+/*******************************************************************************
+ * STM32MP1 USB
+ ******************************************************************************/
+#define USB_OTG_BASE			U(0x49000000)
+
 /*******************************************************************************
  * STM32MP1 DDRCTRL
  ******************************************************************************/
@@ -518,12 +633,41 @@ static inline uint32_t tamp_bkpr(uint32_t idx)
 #define STGEN_BASE			U(0x5c008000)
 #define SYSCFG_BASE			U(0x50020000)
 
+/*******************************************************************************
+ * STM32MP1 TIMERS
+ ******************************************************************************/
+#define TIM12_BASE			U(0x40006000)
+#define TIM15_BASE			U(0x44006000)
+#define TIM_MAX_INSTANCE		U(2)
+
+/*******************************************************************************
+ * STM32MP1 OPP
+ ******************************************************************************/
+#define PLAT_OPP_ID1			U(1)
+#define PLAT_OPP_ID2			U(2)
+#define PLAT_MAX_OPP_NB			U(2)
+#define PLAT_MAX_PLLCFG_NB		U(6)
+
+/*******************************************************************************
+ * DEBUG
+ ******************************************************************************/
+/*#define  ICACHE_OFF*/
+/*#define  DCACHE_OFF*/
+/*#define  MMU_OFF*/
+
 /*******************************************************************************
  * Device Tree defines
  ******************************************************************************/
 #define DT_BSEC_COMPAT			"st,stm32mp15-bsec"
+#define DT_DDR_COMPAT			"st,stm32mp1-ddr"
 #define DT_IWDG_COMPAT			"st,stm32mp1-iwdg"
+#define DT_NVMEM_LAYOUT_COMPAT		"st,stm32-nvmem-layout"
+#define DT_OPP_COMPAT			"operating-points-v2"
 #define DT_PWR_COMPAT			"st,stm32mp1,pwr-reg"
 #define DT_RCC_CLK_COMPAT		"st,stm32mp1-rcc"
+#define DT_RCC_SEC_CLK_COMPAT		"st,stm32mp1-rcc-secure"
+#define DT_USBPHYC_COMPAT		"st,stm32mp1-usbphyc"
+
+#define DT_PLL1_NODE_NAME		"st,pll@0"
 
 #endif /* STM32MP1_DEF_H */
diff --git a/plat/st/stm32mp1/stm32mp1_fconf_firewall.c b/plat/st/stm32mp1/stm32mp1_fconf_firewall.c
new file mode 100644
index 0000000000..16aee904e7
--- /dev/null
+++ b/plat/st/stm32mp1/stm32mp1_fconf_firewall.c
@@ -0,0 +1,124 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <platform_def.h>
+
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/arm/tzc400.h>
+#include <drivers/clk.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <lib/fconf/fconf.h>
+#include <lib/object_pool.h>
+#include <libfdt.h>
+#include <tools_share/firmware_image_package.h>
+
+#include <stm32mp_fconf_getter.h>
+
+#define STM32MP_REGION_PARAMS	4
+#define STM32MP_MAX_REGIONS	8
+#define FORCE_SEC_REGION	BIT(31)
+
+static uint32_t nb_regions;
+
+struct dt_id_attr {
+	fdt32_t id_attr[STM32MP_MAX_REGIONS];
+};
+
+void stm32mp1_arch_security_setup(void)
+{
+	clk_enable(TZC1);
+	clk_enable(TZC2);
+
+	tzc400_init(STM32MP1_TZC_BASE);
+	tzc400_disable_filters();
+
+	/*
+	 * Region 0 set to cover all DRAM at 0xC000_0000
+	 * Only secure access is granted in read/write.
+	 */
+	tzc400_configure_region0(TZC_REGION_S_RDWR, 0);
+
+	tzc400_set_action(TZC_ACTION_ERR);
+	tzc400_enable_filters();
+}
+
+void stm32mp1_security_setup(void)
+{
+	uint8_t i;
+
+	assert(nb_regions > 0U);
+
+	tzc400_init(STM32MP1_TZC_BASE);
+	tzc400_disable_filters();
+
+	/*
+	 * Region 0 set to cover all DRAM at 0xC000_0000
+	 * No access is allowed.
+	 */
+	tzc400_configure_region0(TZC_REGION_S_NONE, 0);
+
+	for (i = 1U; i <= nb_regions; i++) {
+		tzc400_update_filters(i, STM32MP1_FILTER_BIT_ALL);
+	}
+
+	tzc400_set_action(TZC_ACTION_INT);
+	tzc400_enable_filters();
+}
+
+static int fconf_populate_stm32mp1_firewall(uintptr_t config)
+{
+	int node, len;
+	unsigned int i;
+	const struct dt_id_attr *conf_list;
+	const void *dtb = (const void *)config;
+
+	/* Assert the node offset point to "st,mem-firewall" compatible property */
+	const char *compatible_str = "st,mem-firewall";
+
+	node = fdt_node_offset_by_compatible(dtb, -1, compatible_str);
+	if (node < 0) {
+		ERROR("FCONF: Can't find %s compatible in dtb\n", compatible_str);
+		return node;
+	}
+
+	conf_list = (const struct dt_id_attr *)fdt_getprop(dtb, node, "memory-ranges", &len);
+	if (conf_list == NULL) {
+		WARN("FCONF: Read cell failed for %s\n", "memory-ranges");
+		return -1;
+	}
+
+	/* Locate the memory cells and read all values */
+	for (i = 0U; i < (unsigned int)(len / (sizeof(uint32_t) * STM32MP_REGION_PARAMS)); i++) {
+		uint32_t base;
+		uint32_t size;
+		uint32_t sec_attr;
+		uint32_t nsaid;
+
+		base = fdt32_to_cpu(conf_list->id_attr[i * STM32MP_REGION_PARAMS]);
+		size = fdt32_to_cpu(conf_list->id_attr[i * STM32MP_REGION_PARAMS + 1]);
+		sec_attr = fdt32_to_cpu(conf_list->id_attr[i * STM32MP_REGION_PARAMS + 2]);
+		nsaid = fdt32_to_cpu(conf_list->id_attr[i * STM32MP_REGION_PARAMS + 3]);
+
+		VERBOSE("FCONF: stm32mp1-firewall cell found with value = 0x%x 0x%x 0x%x 0x%x\n",
+			base, size, sec_attr, nsaid);
+
+		nb_regions++;
+
+		/* Configure region but keep disabled for secure access for BL2 load */
+		tzc400_configure_region(0, nb_regions, (unsigned long long)base,
+					(unsigned long long)base + size - 1ULL, sec_attr, nsaid);
+	}
+
+	/* Force flush as the value will be used cache off */
+	flush_dcache_range((uintptr_t)&nb_regions, sizeof(uint32_t));
+
+	return 0;
+}
+
+FCONF_REGISTER_POPULATOR(FW_CONFIG, stm32mp1_firewall, fconf_populate_stm32mp1_firewall);
diff --git a/plat/st/stm32mp1/stm32mp1_gic.c b/plat/st/stm32mp1/stm32mp1_gic.c
deleted file mode 100644
index 851a9cf0c4..0000000000
--- a/plat/st/stm32mp1/stm32mp1_gic.c
+++ /dev/null
@@ -1,92 +0,0 @@
-/*
- * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <libfdt.h>
-
-#include <platform_def.h>
-
-#include <common/bl_common.h>
-#include <common/debug.h>
-#include <drivers/arm/gicv2.h>
-#include <dt-bindings/interrupt-controller/arm-gic.h>
-#include <lib/utils.h>
-#include <plat/common/platform.h>
-
-struct stm32_gic_instance {
-	uint32_t cells;
-	uint32_t phandle_node;
-};
-
-/******************************************************************************
- * On a GICv2 system, the Group 1 secure interrupts are treated as Group 0
- * interrupts.
- *****************************************************************************/
-static const interrupt_prop_t stm32mp1_interrupt_props[] = {
-	PLATFORM_G1S_PROPS(GICV2_INTR_GROUP0),
-	PLATFORM_G0_PROPS(GICV2_INTR_GROUP0)
-};
-
-/* Fix target_mask_array as secondary core is not able to initialize it */
-static unsigned int target_mask_array[PLATFORM_CORE_COUNT] = {1, 2};
-
-static gicv2_driver_data_t platform_gic_data = {
-	.interrupt_props = stm32mp1_interrupt_props,
-	.interrupt_props_num = ARRAY_SIZE(stm32mp1_interrupt_props),
-	.target_masks = target_mask_array,
-	.target_masks_num = ARRAY_SIZE(target_mask_array),
-};
-
-static struct stm32_gic_instance stm32_gic;
-
-void stm32mp1_gic_init(void)
-{
-	int node;
-	void *fdt;
-	const fdt32_t *cuint;
-	struct dt_node_info dt_gic;
-
-	if (fdt_get_address(&fdt) == 0) {
-		panic();
-	}
-
-	node = dt_get_node(&dt_gic, -1, "arm,cortex-a7-gic");
-	if (node < 0) {
-		panic();
-	}
-
-	platform_gic_data.gicd_base = dt_gic.base;
-
-	cuint = fdt_getprop(fdt, node, "reg", NULL);
-	if (cuint == NULL) {
-		panic();
-	}
-
-	platform_gic_data.gicc_base = fdt32_to_cpu(*(cuint + 2));
-
-	cuint = fdt_getprop(fdt, node, "#interrupt-cells", NULL);
-	if (cuint == NULL) {
-		panic();
-	}
-
-	stm32_gic.cells = fdt32_to_cpu(*cuint);
-
-	stm32_gic.phandle_node = fdt_get_phandle(fdt, node);
-	if (stm32_gic.phandle_node == 0U) {
-		panic();
-	}
-
-	gicv2_driver_init(&platform_gic_data);
-	gicv2_distif_init();
-
-	stm32mp1_gic_pcpu_init();
-}
-
-void stm32mp1_gic_pcpu_init(void)
-{
-	gicv2_pcpu_distif_init();
-	gicv2_set_pe_target_mask(plat_my_core_pos());
-	gicv2_cpuif_enable();
-}
diff --git a/plat/st/stm32mp1/stm32mp1_helper.S b/plat/st/stm32mp1/stm32mp1_helper.S
index 3021362365..bc2d73cebb 100644
--- a/plat/st/stm32mp1/stm32mp1_helper.S
+++ b/plat/st/stm32mp1/stm32mp1_helper.S
@@ -10,11 +10,17 @@
 #include <asm_macros.S>
 #include <common/bl_common.h>
 #include <drivers/st/stm32_gpio.h>
+#include <smccc_helpers.h>
 
 #define GPIO_TX_SHIFT		(DEBUG_UART_TX_GPIO_PORT << 1)
 
 	.globl	platform_mem_init
 	.globl	plat_report_exception
+#if AARCH32_EXCEPTION_DEBUG
+	.globl	plat_report_undef_inst
+	.globl	plat_report_prefetch_abort
+	.globl	plat_report_data_abort
+#endif
 	.globl	plat_get_my_entrypoint
 	.globl	plat_secondary_cold_boot_setup
 	.globl	plat_reset_handler
@@ -24,6 +30,7 @@
 	.globl	plat_crash_console_flush
 	.globl	plat_crash_console_putc
 	.globl	plat_panic_handler
+	.globl	wfi_svc_int_enable
 
 func platform_mem_init
 	/* Nothing to do, don't need to init SYSRAM */
@@ -75,6 +82,96 @@ print_exception_info:
 #endif
 endfunc plat_report_exception
 
+#if AARCH32_EXCEPTION_DEBUG
+func plat_report_undef_inst
+#if DEBUG
+	mov	r8, lr
+
+	mov	r9, r0
+
+	ldr	r4, =undefined_str
+	bl	asm_print_str
+
+	mov	r4, r9
+	sub	r4, r4, #4
+	bl	asm_print_hex
+
+	ldr	r4, =end_error_str
+	bl	asm_print_str
+
+	bx	r8
+#else
+	bx	lr
+#endif
+endfunc plat_report_undef_inst
+
+func plat_report_prefetch_abort
+#if DEBUG
+	mov	r8, lr
+	mov	r9, r0
+
+	ldr	r4, =prefetch_abort_str
+	bl	asm_print_str
+
+	mov	r4, r9
+	sub	r4, r4, #4
+	bl	asm_print_hex
+
+	ldr	r4, =ifsr_str
+	bl	asm_print_str
+
+	ldcopr	r4, IFSR
+	bl	asm_print_hex
+
+	ldr	r4, =ifar_str
+	bl	asm_print_str
+
+	ldcopr	r4, IFAR
+	bl	asm_print_hex
+
+	ldr	r4, =end_error_str
+	bl	asm_print_str
+
+	bx	r8
+#else
+	bx	lr
+#endif
+endfunc plat_report_prefetch_abort
+
+func plat_report_data_abort
+#if DEBUG
+	mov	r8, lr
+	mov	r9, r0
+
+	ldr	r4, =data_abort_str
+	bl	asm_print_str
+
+	mov	r4, r9
+	sub	r4, r4, #8
+	bl	asm_print_hex
+
+	ldr	r4, =dfsr_str
+	bl	asm_print_str
+
+	ldcopr	r4, DFSR
+	bl	asm_print_hex
+
+	ldr	r4, =dfar_str
+	bl	asm_print_str
+
+	ldcopr	r4, DFAR
+	bl	asm_print_hex
+
+	ldr	r4, =end_error_str
+	bl	asm_print_str
+
+	bx	r8
+#else
+	bx	lr
+#endif
+endfunc plat_report_data_abort
+#endif
+
 func plat_reset_handler
 	bx	lr
 endfunc plat_reset_handler
@@ -204,7 +301,7 @@ endfunc plat_crash_console_init
 	 * ---------------------------------------------
 	 */
 func plat_crash_console_flush
-	ldr	r1, =STM32MP_DEBUG_USART_BASE
+	ldr	r0, =STM32MP_DEBUG_USART_BASE
 	b	console_stm32_core_flush
 endfunc plat_crash_console_flush
 
@@ -245,12 +342,48 @@ endfunc plat_panic_handler
 .section .rodata.rev_err_str, "aS"
 abort_str:
 	.asciz "\nAbort at: 0x"
+#if AARCH32_EXCEPTION_DEBUG
+prefetch_abort_str:
+	.asciz "\nPrefetch Abort at: 0x"
+data_abort_str:
+	.asciz "\nData Abort at: 0x"
+#endif
 undefined_str:
 	.asciz "\nUndefined instruction at: 0x"
 exception_start_str:
 	.asciz "\nException mode=0x"
 exception_end_str:
 	.asciz " at: 0x"
+#if AARCH32_EXCEPTION_DEBUG
+dfsr_str:
+	.asciz " DFSR = 0x"
+dfar_str:
+	.asciz " DFAR = 0x"
+ifsr_str:
+	.asciz " IFSR = 0x"
+ifar_str:
+	.asciz " IFAR = 0x"
+#endif
 end_error_str:
 	.asciz "\n\r"
 #endif
+
+func wfi_svc_int_enable
+	push	{r4,r8,lr}
+	ldcopr	r4, SCR
+	mov	r8, sp
+	mov	sp, r0
+	add	r0, r0, #STM32MP_INT_STACK_SIZE
+	str	r0, [sp, #SMC_CTX_SP_MON]
+	str	r4, [sp, #SMC_CTX_SCR]
+	cps	#MODE32_svc
+	cpsie	af
+	dsb
+	isb
+	wfi
+	cpsid	af
+	cps	#MODE32_mon
+	mov	sp, r8
+	pop	{r4,r8,lr}
+	bx	lr
+endfunc wfi_svc_int_enable
diff --git a/plat/st/stm32mp1/stm32mp1_low_power.c b/plat/st/stm32mp1/stm32mp1_low_power.c
new file mode 100644
index 0000000000..ae6fd88c88
--- /dev/null
+++ b/plat/st/stm32mp1/stm32mp1_low_power.c
@@ -0,0 +1,455 @@
+/*
+ * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <libfdt.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/gicv2.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_iwdg.h>
+#include <drivers/st/stm32_rtc.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <drivers/st/stm32mp_pmic.h>
+#include <drivers/st/stm32mp1_ddr_helpers.h>
+#include <drivers/st/stm32mp1_pwr.h>
+#include <drivers/st/stm32mp1_rcc.h>
+#include <drivers/st/stpmic1.h>
+#include <dt-bindings/clock/stm32mp1-clks.h>
+#include <dt-bindings/power/stm32mp1-power.h>
+#include <lib/mmio.h>
+#include <lib/psci/psci.h>
+#include <lib/spinlock.h>
+#include <plat/common/platform.h>
+
+#include <boot_api.h>
+#include <stm32mp_common.h>
+#include <stm32mp_dt.h>
+#include <stm32mp1_context.h>
+#include <stm32mp1_low_power.h>
+#include <stm32mp1_power_config.h>
+#include <stm32mp1_private.h>
+
+static unsigned int gicc_pmr;
+static struct stm32_rtc_calendar sleep_time;
+static bool enter_cstop_done;
+static unsigned long long stgen_cnt;
+
+struct pwr_lp_config {
+	uint32_t pwr_cr1;
+	uint32_t pwr_mpucr;
+	const char *regul_suspend_node_name;
+};
+
+#define PWRLP_TEMPO_5_HSI	5
+
+#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 = PWR_MPUCR_CSSF,
+		.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",
+	},
+};
+
+#define GICC_PMR_PRIORITY_8	U(0x8)
+
+enum {
+	STATE_NONE = 0,
+	STATE_AUTOSTOP_ENTRY,
+	STATE_AUTOSTOP_EXIT,
+};
+
+static struct spinlock lp_lock;
+static volatile int cpu0_state = STATE_NONE;
+static volatile int cpu1_state = STATE_NONE;
+
+void stm32_apply_pmic_suspend_config(uint32_t mode)
+{
+	const char *node_name;
+
+	assert(mode < ARRAY_SIZE(config_pwr));
+
+	node_name = config_pwr[mode].regul_suspend_node_name;
+
+	if (node_name != NULL) {
+		if (!initialize_pmic_i2c()) {
+			panic();
+		}
+
+		if (pmic_set_lp_config(node_name) < 0) {
+			panic();
+		}
+
+		if (pmic_configure_boot_on_regulators() < 0) {
+			panic();
+		}
+	}
+}
+
+/*
+ * stm32_enter_cstop - Prepare CSTOP mode
+ *
+ * @mode - Target low power mode
+ * @nsec_addr - Non secure resume entry point
+ * Return 0 if succeed to suspend, non 0 else.
+ */
+static void enter_cstop(uint32_t mode, uint32_t nsec_addr)
+{
+	uint32_t zq0cr0_zdata;
+	uint32_t bkpr_core1_addr =
+		tamp_bkpr(BOOT_API_CORE1_BRANCH_ADDRESS_TAMP_BCK_REG_IDX);
+	uint32_t bkpr_core1_magic =
+		tamp_bkpr(BOOT_API_CORE1_MAGIC_NUMBER_TAMP_BCK_REG_IDX);
+	uint32_t pwr_cr1 = config_pwr[mode].pwr_cr1;
+	uintptr_t pwr_base = stm32mp_pwr_base();
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	stm32mp1_syscfg_disable_io_compensation();
+
+	/* Switch to Software Self-Refresh mode */
+	ddr_set_sr_mode(DDR_SSR_MODE);
+
+	dcsw_op_all(DC_OP_CISW);
+
+	stm32_clean_context();
+
+	if (mode == STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR) {
+		/*
+		 * The first 64 bytes of DDR need to be saved for DDR DQS
+		 * training
+		 */
+		stm32_save_ddr_training_area();
+	}
+
+	if (dt_pmic_status() > 0) {
+		stm32_apply_pmic_suspend_config(mode);
+
+		if (mode == STM32_PM_CSTOP_ALLOW_LP_STOP) {
+			pwr_cr1 |= PWR_CR1_LPCFG;
+		}
+	}
+
+	/* Clear RCC interrupt before enabling it */
+	mmio_setbits_32(rcc_base + RCC_MP_CIFR, RCC_MP_CIFR_WKUPF);
+
+	/* Enable RCC Wake-up */
+	mmio_setbits_32(rcc_base + RCC_MP_CIER, RCC_MP_CIFR_WKUPF);
+
+	/* Configure low power mode */
+	mmio_clrsetbits_32(pwr_base + PWR_MPUCR, PWR_MPUCR_MASK,
+			   config_pwr[mode].pwr_mpucr);
+	mmio_clrsetbits_32(pwr_base + PWR_CR1, PWR_CR1_MASK,
+			   pwr_cr1);
+
+	/* Clear RCC pending interrupt flags */
+	mmio_write_32(rcc_base + RCC_MP_CIFR, RCC_MP_CIFR_MASK);
+
+	/* Request CSTOP mode to RCC */
+	mmio_setbits_32(rcc_base + RCC_MP_SREQSETR,
+			RCC_MP_SREQSETR_STPREQ_P0 | RCC_MP_SREQSETR_STPREQ_P1);
+
+	stm32_iwdg_refresh();
+
+	gicc_pmr = plat_ic_set_priority_mask(GICC_PMR_PRIORITY_8);
+
+	zq0cr0_zdata = ddr_get_io_calibration_val();
+
+	clk_enable(RTCAPB);
+
+	mmio_write_32(bkpr_core1_addr, 0);
+	mmio_write_32(bkpr_core1_magic, 0);
+
+	stm32mp1_clock_stopmode_save();
+
+	stm32_rtc_get_calendar(&sleep_time);
+	stgen_cnt = stm32mp_stgen_get_counter();
+
+	if (mode == STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR) {
+		/*
+		 * Save non-secure world entrypoint after standby in Backup
+		 * register
+		 */
+		mmio_write_32(bkpr_core1_addr, nsec_addr);
+		mmio_write_32(bkpr_core1_magic,
+			      BOOT_API_A7_CORE0_MAGIC_NUMBER);
+
+		if (stm32_save_context(zq0cr0_zdata, &sleep_time,
+				       stgen_cnt) != 0) {
+			panic();
+		}
+
+		if (stm32mp1_get_retram_enabled()) {
+			mmio_setbits_32(pwr_base + PWR_CR2, PWR_CR2_RREN);
+			while ((mmio_read_32(pwr_base + PWR_CR2) &
+				PWR_CR2_RRRDY) == 0U) {
+				;
+			}
+		}
+
+		/* Keep backup RAM content in standby */
+		mmio_setbits_32(pwr_base + PWR_CR2, PWR_CR2_BREN);
+		while ((mmio_read_32(pwr_base + PWR_CR2) &
+			PWR_CR2_BRRDY) == 0U) {
+			;
+		}
+	}
+
+	clk_disable(RTCAPB);
+
+	enter_cstop_done = true;
+}
+
+bool stm32_is_cstop_done(void)
+{
+	return enter_cstop_done;
+}
+
+/*
+ * stm32_exit_cstop - Exit from CSTOP mode
+ */
+void stm32_exit_cstop(void)
+{
+	uintptr_t pwr_base = stm32mp_pwr_base();
+	uintptr_t rcc_base = stm32mp_rcc_base();
+	unsigned long long stdby_time_in_ms;
+	struct stm32_rtc_calendar current_calendar;
+
+	if (!enter_cstop_done) {
+		return;
+	}
+
+	enter_cstop_done = false;
+
+	stm32mp1_syscfg_enable_io_compensation_start();
+
+	plat_ic_set_priority_mask(gicc_pmr);
+
+	/* Disable RCC Wake-up */
+	mmio_clrbits_32(rcc_base + RCC_MP_CIER, RCC_MP_CIFR_WKUPF);
+
+	/* Disable STOP request */
+	mmio_setbits_32(rcc_base + RCC_MP_SREQCLRR,
+			RCC_MP_SREQSETR_STPREQ_P0 | RCC_MP_SREQSETR_STPREQ_P1);
+
+	dsb();
+	isb();
+
+	/* Disable retention and backup RAM content after stop */
+	mmio_clrbits_32(pwr_base + PWR_CR2, PWR_CR2_BREN | PWR_CR2_RREN);
+
+	/* Update STGEN counter with low power mode duration */
+	stm32_rtc_get_calendar(&current_calendar);
+
+	stdby_time_in_ms = stm32_rtc_diff_calendar(&current_calendar,
+						   &sleep_time);
+	stm32mp_stgen_restore_counter(stgen_cnt, stdby_time_in_ms);
+
+	if (stm32mp1_clock_stopmode_resume() != 0) {
+		panic();
+	}
+
+	stm32mp1_syscfg_enable_io_compensation_finish();
+}
+
+static int get_locked(volatile int *state)
+{
+	volatile int val;
+
+	spin_lock(&lp_lock);
+	val = *state;
+	spin_unlock(&lp_lock);
+
+	return val;
+}
+
+static void set_locked(volatile int *state, int val)
+{
+	spin_lock(&lp_lock);
+	*state = val;
+	spin_unlock(&lp_lock);
+}
+
+static void smp_synchro(int state, bool wake_up)
+{
+	/* if the other CPU is stopped, no need to synchronize */
+	if (psci_is_last_on_cpu() == 1U) {
+		return;
+	}
+
+	if (plat_my_core_pos() == STM32MP_PRIMARY_CPU) {
+		set_locked(&cpu0_state, state);
+
+		while (get_locked(&cpu1_state) != state) {
+			if (wake_up) {
+				/* wakeup secondary CPU */
+				gicv2_raise_sgi(ARM_IRQ_SEC_SGI_6,
+						STM32MP_SECONDARY_CPU);
+				udelay(10);
+			}
+		};
+	} else {
+		while (get_locked(&cpu0_state) != state) {
+			if (wake_up) {
+				/* wakeup primary CPU */
+				gicv2_raise_sgi(ARM_IRQ_SEC_SGI_6,
+						STM32MP_PRIMARY_CPU);
+				udelay(10);
+			}
+		};
+
+		set_locked(&cpu1_state, state);
+	}
+}
+
+static void stm32_auto_stop_cpu0(void)
+{
+	smp_synchro(STATE_AUTOSTOP_ENTRY, false);
+
+	enter_cstop(STM32_PM_CSTOP_ALLOW_LP_STOP, 0);
+
+	stm32_pwr_down_wfi(true);
+
+	stm32_exit_cstop();
+
+	smp_synchro(STATE_AUTOSTOP_EXIT, true);
+}
+
+static void stm32_auto_stop_cpu1(void)
+{
+	unsigned int gicc_pmr_cpu1;
+
+	/* clear cache before the DDR is being disabled by cpu0 */
+	dcsw_op_all(DC_OP_CISW);
+
+	smp_synchro(STATE_AUTOSTOP_ENTRY, false);
+
+	gicc_pmr_cpu1 = plat_ic_set_priority_mask(GICC_PMR_PRIORITY_8);
+	wfi();
+	plat_ic_set_priority_mask(gicc_pmr_cpu1);
+
+	smp_synchro(STATE_AUTOSTOP_EXIT, true);
+}
+
+void stm32_auto_stop(void)
+{
+	if (plat_my_core_pos() == STM32MP_PRIMARY_CPU) {
+		stm32_auto_stop_cpu0();
+	} else {
+		stm32_auto_stop_cpu1();
+	}
+}
+
+static void enter_shutdown(void)
+{
+	/* Set DDR in Self-refresh before shutting down the platform */
+	if (ddr_standby_sr_entry() != 0) {
+		WARN("DDR can't be set in Self-refresh mode\n");
+	}
+
+	if (dt_pmic_status() > 0) {
+		if (!initialize_pmic_i2c()) {
+			panic();
+		}
+
+		stpmic1_switch_off();
+
+		udelay(100);
+
+		/* Shouldn't be reached */
+		panic();
+	}
+}
+
+static void enter_csleep(void)
+{
+	uintptr_t pwr_base = stm32mp_pwr_base();
+
+	mmio_clrsetbits_32(pwr_base + PWR_MPUCR, PWR_MPUCR_MASK,
+			   config_pwr[STM32_PM_CSLEEP_RUN].pwr_mpucr);
+	mmio_clrsetbits_32(pwr_base + PWR_CR1, PWR_CR1_MASK,
+			   config_pwr[STM32_PM_CSLEEP_RUN].pwr_cr1);
+
+	stm32_pwr_down_wfi(false);
+}
+
+void stm32_enter_low_power(uint32_t mode, uint32_t nsec_addr)
+{
+	switch (mode) {
+	case STM32_PM_SHUTDOWN:
+		enter_shutdown();
+		break;
+
+	case STM32_PM_CSLEEP_RUN:
+		enter_csleep();
+		break;
+
+	default:
+		enter_cstop(mode, nsec_addr);
+		break;
+	}
+}
+
+void stm32_init_low_power(void)
+{
+	uintptr_t pwr_base = stm32mp_pwr_base();
+	uintptr_t rcc_base = stm32mp_rcc_base();
+
+	/*
+	 * Configure Standby mode available for MCU by default
+	 * and allow to switch in standby SoC in all case
+	 */
+	mmio_setbits_32(pwr_base + PWR_MCUCR, PWR_MCUCR_PDDS);
+
+	/* Disable STOP request */
+	mmio_setbits_32(rcc_base + RCC_MP_SREQCLRR,
+			RCC_MP_SREQSETR_STPREQ_P0 | RCC_MP_SREQSETR_STPREQ_P1);
+
+	/* Disable retention and backup RAM content after standby */
+	mmio_clrbits_32(pwr_base + PWR_CR2, PWR_CR2_BREN | PWR_CR2_RREN);
+
+	/* Wait 5 HSI periods before re-enabling PLLs after STOP modes */
+	mmio_clrsetbits_32(rcc_base + RCC_PWRLPDLYCR,
+			   RCC_PWRLPDLYCR_PWRLP_DLY_MASK,
+			   PWRLP_TEMPO_5_HSI);
+}
diff --git a/plat/st/stm32mp1/stm32mp1_pm.c b/plat/st/stm32mp1/stm32mp1_pm.c
index 31a9ae7f13..962a992243 100644
--- a/plat/st/stm32mp1/stm32mp1_pm.c
+++ b/plat/st/stm32mp1/stm32mp1_pm.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,14 +14,21 @@
 #include <common/debug.h>
 #include <drivers/arm/gic_common.h>
 #include <drivers/arm/gicv2.h>
-#include <drivers/st/stm32mp1_clk.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <drivers/st/stm32mp1_rcc.h>
 #include <dt-bindings/clock/stm32mp1-clks.h>
 #include <lib/mmio.h>
 #include <lib/psci/psci.h>
 #include <plat/common/platform.h>
 
+#include <stm32mp1_low_power.h>
+#include <stm32mp1_power_config.h>
+
 static uintptr_t stm32_sec_entrypoint;
 static uint32_t cntfrq_core0;
+static uintptr_t saved_entrypoint;
 
 /*******************************************************************************
  * STM32MP1 handler called when a CPU is about to enter standby.
@@ -34,11 +41,12 @@ static void stm32_cpu_standby(plat_local_state_t cpu_state)
 	assert(cpu_state == ARM_LOCAL_STATE_RET);
 
 	/*
-	 * Enter standby state
-	 * dsb is good practice before using wfi to enter low power states
+	 * Enter standby state.
+	 * Synchronize on memory accesses and instruction flow before the WFI
+	 * instruction.
 	 */
-	isb();
 	dsb();
+	isb();
 	while (interrupt == GIC_SPURIOUS_INTERRUPT) {
 		wfi();
 
@@ -65,16 +73,29 @@ static int stm32_pwr_domain_on(u_register_t mpidr)
 	uint32_t bkpr_core1_magic =
 		tamp_bkpr(BOOT_API_CORE1_MAGIC_NUMBER_TAMP_BCK_REG_IDX);
 
+	if (stm32mp_is_single_core()) {
+		return PSCI_E_INTERN_FAIL;
+	}
+
 	if (mpidr == current_cpu_mpidr) {
 		return PSCI_E_INVALID_PARAMS;
 	}
 
+	/* Reset backup register content */
+	mmio_write_32(bkpr_core1_magic, 0);
+
+	/* Need to send additional IT 0 after individual core 1 reset */
+	gicv2_raise_sgi(ARM_IRQ_NON_SEC_SGI_0, STM32MP_SECONDARY_CPU);
+
+	/* Wait for this IT to be acknowledged by ROM code. */
+	udelay(10);
+
 	/* Only one valid entry point */
 	if (stm32_sec_entrypoint != (uintptr_t)&sp_min_warm_entrypoint) {
 		return PSCI_E_INVALID_ADDRESS;
 	}
 
-	stm32mp_clk_enable(RTCAPB);
+	clk_enable(RTCAPB);
 
 	cntfrq_core0 = read_cntfrq_el0();
 
@@ -84,7 +105,7 @@ static int stm32_pwr_domain_on(u_register_t mpidr)
 	/* Write magic number in backup register */
 	mmio_write_32(bkpr_core1_magic, BOOT_API_A7_CORE1_MAGIC_NUMBER);
 
-	stm32mp_clk_disable(RTCAPB);
+	clk_disable(RTCAPB);
 
 	/* Generate an IT to core 1 */
 	gicv2_raise_sgi(ARM_IRQ_SEC_SGI_0, STM32MP_SECONDARY_CPU);
@@ -107,7 +128,9 @@ static void stm32_pwr_domain_off(const psci_power_state_t *target_state)
  ******************************************************************************/
 static void stm32_pwr_domain_suspend(const psci_power_state_t *target_state)
 {
-	/* Nothing to do, power domain is not disabled */
+	uint32_t soc_mode = stm32mp1_get_lp_soc_mode(PSCI_MODE_SYSTEM_SUSPEND);
+
+	stm32_enter_low_power(soc_mode, saved_entrypoint);
 }
 
 /*******************************************************************************
@@ -118,7 +141,7 @@ static void stm32_pwr_domain_suspend(const psci_power_state_t *target_state)
  ******************************************************************************/
 static void stm32_pwr_domain_on_finish(const psci_power_state_t *target_state)
 {
-	stm32mp1_gic_pcpu_init();
+	stm32_gic_pcpu_init();
 
 	write_cntfrq_el0(cntfrq_core0);
 }
@@ -134,28 +157,65 @@ static void stm32_pwr_domain_suspend_finish(const psci_power_state_t
 	/* Nothing to do, power domain is not disabled */
 }
 
+/*******************************************************************************
+ * STM32MP1 handler called when a core tries to power itself down. If this
+ * call is made by core 0, it is a return from stop mode. In this case, we
+ * should restore previous context and jump to secure entrypoint.
+ ******************************************************************************/
 static void __dead2 stm32_pwr_domain_pwr_down_wfi(const psci_power_state_t
 						  *target_state)
 {
-	ERROR("stm32mpu1 Power Down WFI: operation not handled.\n");
+	if (MPIDR_AFFLVL0_VAL(read_mpidr_el1()) == STM32MP_PRIMARY_CPU) {
+		void (*warm_entrypoint)(void) =
+			(void (*)(void))stm32_sec_entrypoint;
+
+		stm32_pwr_down_wfi(stm32_is_cstop_done());
+
+		stm32_exit_cstop();
+
+		disable_mmu_icache_secure();
+
+		warm_entrypoint();
+	}
+
+	mmio_write_32(stm32mp_rcc_base() + RCC_MP_GRSTCSETR,
+		      RCC_MP_GRSTCSETR_MPUP1RST);
+
+	/*
+	 * Synchronize on memory accesses and instruction flow before
+	 * auto-reset from the WFI instruction.
+	 */
+	dsb();
+	isb();
+	wfi();
+
+	/* This shouldn't be reached */
 	panic();
 }
 
 static void __dead2 stm32_system_off(void)
 {
-	ERROR("stm32mpu1 System Off: operation not handled.\n");
+	uint32_t soc_mode = stm32mp1_get_lp_soc_mode(PSCI_MODE_SYSTEM_OFF);
+
+	if (!stm32mp_is_single_core()) {
+		/* Prepare Core 1 reset */
+		mmio_setbits_32(stm32mp_rcc_base() + RCC_MP_GRSTCSETR,
+				RCC_MP_GRSTCSETR_MPUP1RST);
+		/* Send IT to core 1 to put itself in WFI */
+		gicv2_raise_sgi(ARM_IRQ_SEC_SGI_1, STM32MP_SECONDARY_CPU);
+	}
+
+	stm32_enter_low_power(soc_mode, 0);
+
+	stm32_pwr_down_wfi(false);
+
+	/* This shouldn't be reached */
 	panic();
 }
 
 static void __dead2 stm32_system_reset(void)
 {
-	mmio_setbits_32(stm32mp_rcc_base() + RCC_MP_GRSTCSETR,
-			RCC_MP_GRSTCSETR_MPSYSRST);
-
-	/* Loop in case system reset is not immediately caught */
-	for ( ; ; ) {
-		;
-	}
+	stm32mp_system_reset();
 }
 
 static int stm32_validate_power_state(unsigned int power_state,
@@ -188,6 +248,8 @@ static int stm32_validate_ns_entrypoint(uintptr_t entrypoint)
 		return PSCI_E_INVALID_ADDRESS;
 	}
 
+	saved_entrypoint = entrypoint;
+
 	return PSCI_E_SUCCESS;
 }
 
@@ -211,6 +273,12 @@ static int stm32_node_hw_state(u_register_t target_cpu,
 	return (int)HW_ON;
 }
 
+static void stm32_get_sys_suspend_power_state(psci_power_state_t *req_state)
+{
+	req_state->pwr_domain_state[0] = ARM_LOCAL_STATE_OFF;
+	req_state->pwr_domain_state[1] = ARM_LOCAL_STATE_OFF;
+}
+
 /*******************************************************************************
  * Export the platform handlers. The ARM Standard platform layer will take care
  * of registering the handlers with PSCI.
@@ -227,7 +295,8 @@ static const plat_psci_ops_t stm32_psci_ops = {
 	.system_reset = stm32_system_reset,
 	.validate_power_state = stm32_validate_power_state,
 	.validate_ns_entrypoint = stm32_validate_ns_entrypoint,
-	.get_node_hw_state = stm32_node_hw_state
+	.get_node_hw_state = stm32_node_hw_state,
+	.get_sys_suspend_power_state = stm32_get_sys_suspend_power_state,
 };
 
 /*******************************************************************************
diff --git a/plat/st/stm32mp1/stm32mp1_power_config.c b/plat/st/stm32mp1/stm32mp1_power_config.c
new file mode 100644
index 0000000000..1b22b3122f
--- /dev/null
+++ b/plat/st/stm32mp1/stm32mp1_power_config.c
@@ -0,0 +1,223 @@
+/*
+ * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <limits.h>
+
+#include <libfdt.h>
+
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <dt-bindings/power/stm32mp1-power.h>
+
+#include <stm32mp_dt.h>
+#include <stm32mp1_power_config.h>
+
+#define SYSTEM_SUSPEND_SUPPORTED_MODES	"system_suspend_supported_soc_modes"
+#define SYSTEM_OFF_MODE			"system_off_soc_mode"
+#define RETRAM_ENABLED			"st,retram-enabled-in-standby-ddr-sr"
+
+static uint32_t deepest_system_suspend_mode;
+static uint32_t system_off_mode;
+static bool retram_enabled;
+static uint8_t stm32mp1_supported_soc_modes[STM32_PM_MAX_SOC_MODE];
+
+static int dt_get_pwr_node(void *fdt)
+{
+	return fdt_node_offset_by_compatible(fdt, -1, DT_PWR_COMPAT);
+}
+
+static void save_supported_mode(void *fdt, int pwr_node)
+{
+	int len;
+	uint32_t count;
+	unsigned int i;
+	uint32_t supported[ARRAY_SIZE(stm32mp1_supported_soc_modes)];
+	const void *prop;
+
+	prop = fdt_getprop(fdt, pwr_node, SYSTEM_SUSPEND_SUPPORTED_MODES, &len);
+	if (prop == NULL) {
+		panic();
+	}
+
+	count = (uint32_t)len / sizeof(uint32_t);
+	if (count > STM32_PM_MAX_SOC_MODE) {
+		panic();
+	}
+
+	if (fdt_read_uint32_array(fdt, pwr_node, SYSTEM_SUSPEND_SUPPORTED_MODES,
+				  count, &supported[0]) < 0) {
+		ERROR("PWR DT\n");
+		panic();
+	}
+
+	for (i = 0; i < count; i++) {
+		if (supported[i] >= STM32_PM_MAX_SOC_MODE) {
+			ERROR("Invalid mode\n");
+			panic();
+		}
+		stm32mp1_supported_soc_modes[supported[i]] = 1U;
+	}
+
+	/* Initialize to deepest possible mode */
+	for (i = STM32_PM_MAX_SOC_MODE - 1U; i != STM32_PM_CSLEEP_RUN; i--) {
+		if (stm32mp1_supported_soc_modes[i] == 1U) {
+			deepest_system_suspend_mode = i;
+			break;
+		}
+	}
+}
+
+static int dt_fill_lp_state(uint32_t *lp_state_config, const char *lp_state)
+{
+	int pwr_node;
+	void *fdt;
+	const fdt32_t *cuint;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	pwr_node = dt_get_pwr_node(fdt);
+	if (pwr_node < 0) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	cuint = fdt_getprop(fdt, pwr_node, lp_state, NULL);
+	if (cuint == NULL) {
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	*lp_state_config = fdt32_to_cpu(*cuint);
+
+	save_supported_mode(fdt, pwr_node);
+
+	return 0;
+}
+
+static int dt_fill_retram_enabled(void)
+{
+	int pwr_node;
+	void *fdt;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return -ENOENT;
+	}
+
+	pwr_node = dt_get_pwr_node(fdt);
+	if (pwr_node < 0) {
+		return -ENOENT;
+	}
+
+	if (fdt_getprop(fdt, pwr_node, RETRAM_ENABLED, NULL) == NULL) {
+		retram_enabled = false;
+	} else {
+		retram_enabled = true;
+	}
+
+	return 0;
+}
+
+void stm32mp1_init_lp_states(void)
+{
+	if (dt_fill_lp_state(&system_off_mode, SYSTEM_OFF_MODE) < 0) {
+		ERROR("Node %s not found\n", SYSTEM_OFF_MODE);
+		panic();
+	}
+
+	if (dt_fill_retram_enabled() < 0) {
+		ERROR("could not configure retram state\n");
+		panic();
+	}
+}
+
+/* Init with all domains ON */
+static bool pm_dom[STM32MP1_PD_MAX_PM_DOMAIN] = {
+	[STM32MP1_PD_VSW] = false,
+	[STM32MP1_PD_CORE_RET] = false,
+	[STM32MP1_PD_CORE] = false
+};
+
+static bool stm32mp1_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 &= 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 -EINVAL;
+	}
+
+	pm_dom[domain] = status;
+
+	return 0;
+}
+
+static bool is_allowed_mode(uint32_t soc_mode)
+{
+	assert(soc_mode < ARRAY_SIZE(stm32mp1_supported_soc_modes));
+
+	if ((soc_mode == STM32_PM_CSTOP_ALLOW_STANDBY_DDR_SR) &&
+	    !stm32mp1_get_pm_domain_state(STM32MP1_PD_CORE_RET)) {
+		return false;
+	}
+
+	if ((soc_mode == STM32_PM_CSTOP_ALLOW_LPLV_STOP) &&
+	    !stm32mp1_get_pm_domain_state(STM32MP1_PD_CORE)) {
+		return false;
+	}
+
+	return stm32mp1_supported_soc_modes[soc_mode] == 1U;
+}
+
+uint32_t stm32mp1_get_lp_soc_mode(uint32_t psci_mode)
+{
+	uint32_t mode;
+
+	if (psci_mode == PSCI_MODE_SYSTEM_OFF) {
+		return system_off_mode;
+	}
+
+	mode = deepest_system_suspend_mode;
+
+	while ((mode > STM32_PM_CSLEEP_RUN) && !is_allowed_mode(mode)) {
+		mode--;
+	}
+
+	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 -EINVAL;
+	}
+
+	if (psci_mode == PSCI_MODE_SYSTEM_SUSPEND) {
+		deepest_system_suspend_mode = soc_mode;
+	}
+
+	if (psci_mode == PSCI_MODE_SYSTEM_OFF) {
+		system_off_mode = soc_mode;
+	}
+
+	return 0;
+}
+
+bool stm32mp1_get_retram_enabled(void)
+{
+	return retram_enabled;
+}
diff --git a/plat/st/stm32mp1/stm32mp1_private.c b/plat/st/stm32mp1/stm32mp1_private.c
index bc77ee3342..09afe1a6ea 100644
--- a/plat/st/stm32mp1/stm32mp1_private.c
+++ b/plat/st/stm32mp1/stm32mp1_private.c
@@ -10,8 +10,18 @@
 
 #include <platform_def.h>
 
+#include <arch_helpers.h>
+#include <drivers/arm/gicv2.h>
+#include <drivers/clk.h>
 #include <drivers/st/stm32_iwdg.h>
+#include <drivers/st/stm32mp_dummy_regulator.h>
+#include <drivers/st/stm32mp_pmic.h>
+#include <drivers/st/stm32mp_regulator.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <lib/mmio.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
+#include <plat/common/platform.h>
+#include <smccc_helpers.h>
 
 /* Internal layout of the 32bit OTP word board_id */
 #define BOARD_ID_BOARD_NB_MASK		GENMASK(31, 16)
@@ -34,6 +44,10 @@
 					 BOARD_ID_VARFG_SHIFT)
 #define BOARD_ID2BOM(_id)		((_id) & BOARD_ID_BOM_MASK)
 
+#define TAMP_BOOT_ITF_BACKUP_REG_ID	U(20)
+#define TAMP_BOOT_ITF_MASK		U(0x0000FF00)
+#define TAMP_BOOT_ITF_SHIFT		8
+
 #if defined(IMAGE_BL2)
 #define MAP_SEC_SYSRAM	MAP_REGION_FLAT(STM32MP_SYSRAM_BASE, \
 					STM32MP_SYSRAM_SIZE, \
@@ -58,6 +72,13 @@
 					MT_EXECUTE_NEVER)
 #endif
 
+#define MAP_RETRAM	MAP_REGION_FLAT(STM32MP_RETRAM_BASE, \
+					STM32MP_RETRAM_SIZE, \
+					MT_MEMORY | \
+					MT_RW | \
+					MT_NS | \
+					MT_EXECUTE_NEVER)
+
 #define MAP_DEVICE1	MAP_REGION_FLAT(STM32MP1_DEVICE1_BASE, \
 					STM32MP1_DEVICE1_SIZE, \
 					MT_DEVICE | \
@@ -76,14 +97,18 @@
 static const mmap_region_t stm32mp1_mmap[] = {
 	MAP_SEC_SYSRAM,
 	MAP_DEVICE1,
+#if STM32MP_RAW_NAND
 	MAP_DEVICE2,
+#endif
 	{0}
 };
 #endif
 #if defined(IMAGE_BL32)
 static const mmap_region_t stm32mp1_mmap[] = {
+#if !STM32MP_SP_MIN_IN_DDR
 	MAP_SEC_SYSRAM,
 	MAP_NS_SYSRAM,
+#endif
 	MAP_DEVICE1,
 	MAP_DEVICE2,
 	{0}
@@ -92,10 +117,156 @@ static const mmap_region_t stm32mp1_mmap[] = {
 
 void configure_mmu(void)
 {
+#ifndef MMU_OFF
+	unsigned int flags = 0;
+
 	mmap_add(stm32mp1_mmap);
 	init_xlat_tables();
+#ifdef DCACHE_OFF
+	flags |= DISABLE_DCACHE;
+#endif
+	enable_mmu_svc_mon(flags);
+#endif
+}
+
+#define ARM_CNTXCTL_IMASK	BIT(1)
+
+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);
+}
+
+void __dead2 stm32mp_wait_cpu_reset(void)
+{
+	uint32_t id;
+
+	dcsw_op_all(DC_OP_CISW);
+	write_sctlr(read_sctlr() & ~SCTLR_C_BIT);
+	dcsw_op_all(DC_OP_CISW);
+	__asm__("clrex");
+
+	dsb();
+	isb();
+
+	for ( ; ; ) {
+		do {
+			id = plat_ic_get_pending_interrupt_id();
+
+			if (id <= MAX_SPI_ID) {
+				gicv2_end_of_interrupt(id);
+
+				plat_ic_disable_interrupt(id);
+			}
+		} while (id <= MAX_SPI_ID);
+
+		wfi();
+	}
+}
+
+/*
+ * 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 {
+	uint32_t reset_id;
+	uint32_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 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),
+};
 
-	enable_mmu_svc_mon(0);
+#define TIMEOUT_US_1MS		U(1000)
+
+void __dead2 stm32mp_plat_reset(int cpu)
+{
+	uint32_t reg = RCC_MP_GRSTCSETR_MPUP0RST;
+	uint32_t id;
+
+	/* Mask timer interrupts */
+	stm32mp_mask_timer();
+
+	for (id = 0U; id < ARRAY_SIZE(tzc_source_ip); id++) {
+		if ((!clk_is_enabled(tzc_source_ip[id].clock_id)) ||
+		    ((tzc_source_ip[id].decprot_id != STM32MP1_ETZPC_MAX_ID) &&
+		     (etzpc_get_decprot(tzc_source_ip[id].decprot_id) ==
+		      ETZPC_DECPROT_MCU_ISOLATION))) {
+			continue;
+		}
+
+		if (tzc_source_ip[id].reset_id != GPU_R) {
+			uint32_t reset = tzc_source_ip[id].reset_id;
+			int ret;
+
+			ret = stm32mp_reset_assert(reset, TIMEOUT_US_1MS);
+			if (ret != 0) {
+				panic();
+			}
+			ret = stm32mp_reset_deassert(reset, TIMEOUT_US_1MS);
+			if (ret != 0) {
+				panic();
+			}
+		} else {
+			/* GPU reset automatically cleared by hardware */
+			mmio_setbits_32(stm32mp_rcc_base() + RCC_AHB6RSTSETR,
+					RCC_AHB6RSTSETR_GPURST);
+		}
+	}
+
+	if (!stm32mp_is_single_core()) {
+		unsigned int sec_cpu = (cpu == STM32MP_PRIMARY_CPU) ?
+			STM32MP_SECONDARY_CPU : STM32MP_PRIMARY_CPU;
+
+		gicv2_raise_sgi(ARM_IRQ_SEC_SGI_1, sec_cpu);
+		reg |= RCC_MP_GRSTCSETR_MPUP1RST;
+	}
+
+	do {
+		id = plat_ic_get_pending_interrupt_id();
+
+		if (id <= MAX_SPI_ID) {
+			gicv2_end_of_interrupt(id);
+
+			plat_ic_disable_interrupt(id);
+		}
+	} while (id <= MAX_SPI_ID);
+
+	mmio_write_32(stm32mp_rcc_base() + RCC_MP_GRSTCSETR, reg);
+
+	stm32mp_wait_cpu_reset();
 }
 
 uintptr_t stm32_get_gpio_bank_base(unsigned int bank)
@@ -120,6 +291,32 @@ uint32_t stm32_get_gpio_bank_offset(unsigned int bank)
 	return bank * GPIO_BANK_OFFSET;
 }
 
+#if STM32MP_UART_PROGRAMMER || defined(IMAGE_BL32)
+/*
+ * UART Management
+ */
+static const uintptr_t stm32mp1_uart_addresses[8] = {
+	USART1_BASE,
+	USART2_BASE,
+	USART3_BASE,
+	UART4_BASE,
+	UART5_BASE,
+	USART6_BASE,
+	UART7_BASE,
+	UART8_BASE,
+};
+
+uintptr_t get_uart_address(uint32_t instance_nb)
+{
+	if ((instance_nb == 0U) ||
+	    (instance_nb > ARRAY_SIZE(stm32mp1_uart_addresses))) {
+		return 0U;
+	}
+
+	return stm32mp1_uart_addresses[instance_nb - 1U];
+}
+#endif
+
 unsigned long stm32_get_gpio_bank_clock(unsigned int bank)
 {
 	if (bank == GPIO_BANK_Z) {
@@ -153,63 +350,138 @@ int stm32_get_gpio_bank_pinctrl_node(void *fdt, unsigned int bank)
 	}
 }
 
-static int get_part_number(uint32_t *part_nb)
+int stm32_get_otp_index(const char *otp_name, uint32_t *otp_idx,
+			uint32_t *otp_len)
 {
-	uint32_t part_number;
-	uint32_t dev_id;
+	assert(otp_name != NULL);
+	assert(otp_idx != NULL);
+
+	if (bsec_find_otp_name_in_dt(otp_name, otp_idx, otp_len) != BSEC_OK) {
+		return -1;
+	}
 
-	assert(part_nb != NULL);
+	return 0;
+}
 
-	if (stm32mp1_dbgmcu_get_chip_dev_id(&dev_id) < 0) {
+int stm32_get_otp_value(const char *otp_name, uint32_t *otp_val)
+{
+	uint32_t otp_idx;
+
+	assert(otp_name != NULL);
+	assert(otp_val != NULL);
+
+	if (stm32_get_otp_index(otp_name, &otp_idx, NULL) != 0) {
+		return -1;
+	}
+
+	if (stm32_get_otp_value_from_idx(otp_idx, otp_val) != 0) {
+		ERROR("BSEC: %s Read Error\n", otp_name);
 		return -1;
 	}
 
-	if (bsec_shadow_read_otp(&part_number, PART_NUMBER_OTP) != BSEC_OK) {
-		ERROR("BSEC: PART_NUMBER_OTP Error\n");
+	return 0;
+}
+
+int stm32_get_otp_value_from_idx(const uint32_t otp_idx, uint32_t *otp_val)
+{
+	int ret = BSEC_NOT_SUPPORTED;
+
+	assert(otp_val != NULL);
+
+#if defined(IMAGE_BL2)
+	ret = bsec_shadow_read_otp(otp_val, otp_idx);
+#elif defined(IMAGE_BL32)
+	ret = bsec_read_otp(otp_val, otp_idx);
+#else
+#error "Not supported"
+#endif
+	if (ret != BSEC_OK) {
+		ERROR("BSEC: idx=%d Read Error\n", otp_idx);
 		return -1;
 	}
 
+	return 0;
+}
+
+int stm32mp_get_chip_version(uint32_t *chip_version)
+{
+	return stm32mp1_dbgmcu_get_chip_version(chip_version);
+}
+
+static uint32_t get_part_number(void)
+{
+	static uint32_t part_number;
+	uint32_t dev_id;
+
+	if (part_number != 0U) {
+		return part_number;
+	}
+
+	if (stm32mp1_dbgmcu_get_chip_dev_id(&dev_id) < 0) {
+		INFO("Use default chip ID, debug disabled\n");
+		dev_id = STM32MP1_CHIP_ID;
+	}
+
+	if (stm32_get_otp_value(PART_NUMBER_OTP, &part_number) != 0) {
+		panic();
+	}
+
 	part_number = (part_number & PART_NUMBER_OTP_PART_MASK) >>
 		PART_NUMBER_OTP_PART_SHIFT;
 
-	*part_nb = part_number | (dev_id << 16);
+	part_number |= dev_id << 16;
 
-	return 0;
+	return part_number;
 }
 
-static int get_cpu_package(uint32_t *cpu_package)
+static uint32_t get_cpu_package(void)
 {
 	uint32_t package;
 
-	assert(cpu_package != NULL);
-
-	if (bsec_shadow_read_otp(&package, PACKAGE_OTP) != BSEC_OK) {
-		ERROR("BSEC: PACKAGE_OTP Error\n");
-		return -1;
+	if (stm32_get_otp_value(PACKAGE_OTP, &package) != 0) {
+		panic();
 	}
 
-	*cpu_package = (package & PACKAGE_OTP_PKG_MASK) >>
+	package = (package & PACKAGE_OTP_PKG_MASK) >>
 		PACKAGE_OTP_PKG_SHIFT;
 
-	return 0;
+	return package;
 }
 
-void stm32mp_print_cpuinfo(void)
+bool stm32mp_supports_cpu_opp(uint32_t opp_id)
+{
+	uint32_t id;
+
+	switch (opp_id) {
+	case PLAT_OPP_ID1:
+	case PLAT_OPP_ID2:
+		id = opp_id;
+		break;
+	default:
+		return false;
+	}
+
+	switch (get_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;
+	}
+}
+
+void stm32mp_get_soc_name(char name[STM32_SOC_NAME_SIZE])
 {
-	const char *cpu_s, *cpu_r, *pkg;
-	uint32_t part_number;
-	uint32_t cpu_package;
+	char *cpu_s, *cpu_r, *pkg;
 	uint32_t chip_dev_id;
 	int ret;
 
 	/* MPUs Part Numbers */
-	ret = get_part_number(&part_number);
-	if (ret < 0) {
-		WARN("Cannot get part number\n");
-		return;
-	}
-
-	switch (part_number) {
+	switch (get_part_number()) {
 	case STM32MP157C_PART_NB:
 		cpu_s = "157C";
 		break;
@@ -252,13 +524,7 @@ void stm32mp_print_cpuinfo(void)
 	}
 
 	/* Package */
-	ret = get_cpu_package(&cpu_package);
-	if (ret < 0) {
-		WARN("Cannot get CPU package\n");
-		return;
-	}
-
-	switch (cpu_package) {
+	switch (get_cpu_package()) {
 	case PKG_AA_LFBGA448:
 		pkg = "AA";
 		break;
@@ -277,10 +543,9 @@ void stm32mp_print_cpuinfo(void)
 	}
 
 	/* REVISION */
-	ret = stm32mp1_dbgmcu_get_chip_version(&chip_dev_id);
+	ret = stm32mp_get_chip_version(&chip_dev_id);
 	if (ret < 0) {
-		WARN("Cannot get CPU version\n");
-		return;
+		INFO("Cannot get CPU version, debug disabled\n");
 	}
 
 	switch (chip_dev_id) {
@@ -295,40 +560,23 @@ void stm32mp_print_cpuinfo(void)
 		break;
 	}
 
-	NOTICE("CPU: STM32MP%s%s Rev.%s\n", cpu_s, pkg, cpu_r);
+	snprintf(name, STM32_SOC_NAME_SIZE,
+		 "STM32MP%s%s Rev.%s", cpu_s, pkg, cpu_r);
 }
 
-void stm32mp_print_boardinfo(void)
+void stm32mp_print_cpuinfo(void)
 {
-	uint32_t board_id;
-	uint32_t board_otp;
-	int bsec_node, bsec_board_id_node;
-	void *fdt;
-	const fdt32_t *cuint;
-
-	if (fdt_get_address(&fdt) == 0) {
-		panic();
-	}
+	char name[STM32_SOC_NAME_SIZE];
 
-	bsec_node = fdt_node_offset_by_compatible(fdt, -1, DT_BSEC_COMPAT);
-	if (bsec_node < 0) {
-		return;
-	}
-
-	bsec_board_id_node = fdt_subnode_offset(fdt, bsec_node, "board_id");
-	if (bsec_board_id_node <= 0) {
-		return;
-	}
-
-	cuint = fdt_getprop(fdt, bsec_board_id_node, "reg", NULL);
-	if (cuint == NULL) {
-		panic();
-	}
+	stm32mp_get_soc_name(name);
+	NOTICE("CPU: %s\n", name);
+}
 
-	board_otp = fdt32_to_cpu(*cuint) / sizeof(uint32_t);
+void stm32mp_print_boardinfo(void)
+{
+	uint32_t board_id = 0;
 
-	if (bsec_shadow_read_otp(&board_id, board_otp) != BSEC_OK) {
-		ERROR("BSEC: PART_NUMBER_OTP Error\n");
+	if (stm32_get_otp_value(BOARD_ID_OTP, &board_id) != 0) {
 		return;
 	}
 
@@ -349,20 +597,12 @@ void stm32mp_print_boardinfo(void)
 /* Return true when SoC provides a single Cortex-A7 core, and false otherwise */
 bool stm32mp_is_single_core(void)
 {
-	uint32_t part_number;
-
-	if (get_part_number(&part_number) < 0) {
-		ERROR("Invalid part number, assume single core chip");
-		return true;
-	}
-
-	switch (part_number) {
+	switch (get_part_number()) {
 	case STM32MP151A_PART_NB:
 	case STM32MP151C_PART_NB:
 	case STM32MP151D_PART_NB:
 	case STM32MP151F_PART_NB:
 		return true;
-
 	default:
 		return false;
 	}
@@ -373,12 +613,27 @@ bool stm32mp_is_closed_device(void)
 {
 	uint32_t value;
 
-	if ((bsec_shadow_register(DATA0_OTP) != BSEC_OK) ||
-	    (bsec_read_otp(&value, DATA0_OTP) != BSEC_OK)) {
+	if (stm32_get_otp_value(CFG0_OTP, &value) != 0) {
 		return true;
 	}
 
-	return (value & DATA0_OTP_SECURED) == DATA0_OTP_SECURED;
+	return (value & CFG0_CLOSED_DEVICE) == CFG0_CLOSED_DEVICE;
+}
+
+/* Return true when device supports secure boot */
+bool stm32mp_is_auth_supported(void)
+{
+	switch (get_part_number()) {
+	case STM32MP151C_PART_NB:
+	case STM32MP151F_PART_NB:
+	case STM32MP153C_PART_NB:
+	case STM32MP153F_PART_NB:
+	case STM32MP157C_PART_NB:
+	case STM32MP157F_PART_NB:
+		return true;
+	default:
+		return false;
+	}
 }
 
 uint32_t stm32_iwdg_get_instance(uintptr_t base)
@@ -398,13 +653,7 @@ uint32_t stm32_iwdg_get_otp_config(uint32_t iwdg_inst)
 	uint32_t iwdg_cfg = 0U;
 	uint32_t otp_value;
 
-#if defined(IMAGE_BL2)
-	if (bsec_shadow_register(HW2_OTP) != BSEC_OK) {
-		panic();
-	}
-#endif
-
-	if (bsec_read_otp(&otp_value, HW2_OTP) != BSEC_OK) {
+	if (stm32_get_otp_value(HW2_OTP, &otp_value) != 0) {
 		panic();
 	}
 
@@ -426,29 +675,34 @@ uint32_t stm32_iwdg_get_otp_config(uint32_t iwdg_inst)
 #if defined(IMAGE_BL2)
 uint32_t stm32_iwdg_shadow_update(uint32_t iwdg_inst, uint32_t flags)
 {
+	uint32_t otp_value;
 	uint32_t otp;
 	uint32_t result;
 
-	if (bsec_shadow_read_otp(&otp, HW2_OTP) != BSEC_OK) {
+	if (stm32_get_otp_index(HW2_OTP, &otp, NULL) != 0) {
+		panic();
+	}
+
+	if (stm32_get_otp_value(HW2_OTP, &otp_value) != 0) {
 		panic();
 	}
 
-	if ((flags & IWDG_DISABLE_ON_STOP) != 0U) {
-		otp |= BIT(iwdg_inst + HW2_OTP_IWDG_FZ_STOP_POS);
+	if ((flags & IWDG_DISABLE_ON_STOP) != 0) {
+		otp_value |= BIT(iwdg_inst + HW2_OTP_IWDG_FZ_STOP_POS);
 	}
 
-	if ((flags & IWDG_DISABLE_ON_STANDBY) != 0U) {
-		otp |= BIT(iwdg_inst + HW2_OTP_IWDG_FZ_STANDBY_POS);
+	if ((flags & IWDG_DISABLE_ON_STANDBY) != 0) {
+		otp_value |= BIT(iwdg_inst + HW2_OTP_IWDG_FZ_STANDBY_POS);
 	}
 
-	result = bsec_write_otp(otp, HW2_OTP);
+	result = bsec_write_otp(otp_value, otp);
 	if (result != BSEC_OK) {
 		return result;
 	}
 
 	/* Sticky lock OTP_IWDG (read and write) */
-	if (!bsec_write_sr_lock(HW2_OTP, 1U) ||
-	    !bsec_write_sw_lock(HW2_OTP, 1U)) {
+	if ((bsec_set_sr_lock(otp) != BSEC_OK) ||
+	    (bsec_set_sw_lock(otp) != BSEC_OK)) {
 		return BSEC_LOCK_FAIL;
 	}
 
@@ -456,6 +710,52 @@ uint32_t stm32_iwdg_shadow_update(uint32_t iwdg_inst, uint32_t flags)
 }
 #endif
 
+/*
+ * This function allows to split bindings between platform and ETZPC
+ * HW mapping. If this conversion was done at driver level, the driver
+ * should include all supported platform bindings. ETZPC may be used on
+ * other platforms.
+ */
+enum etzpc_decprot_attributes stm32mp_etzpc_binding2decprot(uint32_t mode)
+{
+	switch (mode) {
+	case DECPROT_S_RW:
+		return 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();
+	}
+}
+
+int plat_bind_regulator(struct stm32mp_regulator *regu)
+{
+	void *fdt;
+	int regu_node;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return false;
+	}
+
+	if ((dt_pmic_status() > 0) && is_pmic_regulator(regu)) {
+		bind_pmic_regulator(regu);
+	} else {
+		bind_dummy_regulator(regu);
+	}
+
+	regu_node = fdt_node_offset_by_phandle(fdt, regu->id);
+	if (fdt_getprop(fdt, regu_node, "regulator-always-on", NULL) != NULL) {
+		regu->always_on = true;
+	}
+
+	return 0;
+}
+
+#if STM32MP_USE_STM32IMAGE
 /* Get the non-secure DDR size */
 uint32_t stm32mp_get_ddr_ns_size(void)
 {
@@ -467,12 +767,145 @@ uint32_t stm32mp_get_ddr_ns_size(void)
 	}
 
 	ddr_size = dt_get_ddr_size();
-	if ((ddr_size <= (STM32MP_DDR_S_SIZE + STM32MP_DDR_SHMEM_SIZE)) ||
-	    (ddr_size > STM32MP_DDR_MAX_SIZE)) {
+	if ((ddr_size <= STM32MP_DDR_S_SIZE) || (ddr_size > STM32MP_DDR_MAX_SIZE)) {
 		panic();
 	}
 
+#if defined(AARCH32_SP_OPTEE)
 	ddr_ns_size = ddr_size - (STM32MP_DDR_S_SIZE + STM32MP_DDR_SHMEM_SIZE);
+#else
+	ddr_ns_size = ddr_size;
+#endif
 
 	return ddr_ns_size;
 }
+#endif
+
+bool stm32mp1_addr_inside_backupsram(uintptr_t addr)
+{
+	return (addr >= STM32MP_BACKUP_RAM_BASE) &&
+		(addr < (STM32MP_BACKUP_RAM_BASE + STM32MP_BACKUP_RAM_SIZE));
+}
+
+bool stm32mp1_is_wakeup_from_standby(void)
+{
+	uint32_t bkpr_core1_addr = tamp_bkpr(BOOT_API_CORE1_BRANCH_ADDRESS_TAMP_BCK_REG_IDX);
+	uint32_t nsec_address;
+
+	if (stm32mp_get_boot_action() != BOOT_API_CTX_BOOT_ACTION_WAKEUP_STANDBY) {
+		return false;
+	}
+
+	clk_enable(RTCAPB);
+	nsec_address = mmio_read_32(bkpr_core1_addr);
+	clk_disable(RTCAPB);
+
+	if (nsec_address == 0U) {
+		return false;
+	}
+
+	return stm32_pm_context_is_valid();
+}
+
+int stm32_save_boot_interface(uint32_t interface, uint32_t instance)
+{
+	uint32_t bkpr_itf_idx = tamp_bkpr(TAMP_BOOT_ITF_BACKUP_REG_ID);
+
+	clk_enable(RTCAPB);
+
+	mmio_clrsetbits_32(bkpr_itf_idx,
+			   TAMP_BOOT_ITF_MASK,
+			   ((interface << 4) | (instance & 0xFU)) <<
+			   TAMP_BOOT_ITF_SHIFT);
+
+	clk_disable(RTCAPB);
+
+	return 0;
+}
+
+int stm32_get_boot_interface(uint32_t *interface, uint32_t *instance)
+{
+	static uint32_t itf;
+
+	if (itf == 0U) {
+		uint32_t bkpr = tamp_bkpr(TAMP_BOOT_ITF_BACKUP_REG_ID);
+
+		clk_enable(RTCAPB);
+
+		itf = (mmio_read_32(bkpr) & TAMP_BOOT_ITF_MASK) >> TAMP_BOOT_ITF_SHIFT;
+
+		clk_disable(RTCAPB);
+	}
+
+	*interface = itf >> 4;
+	*instance = itf & 0xFU;
+
+	return 0;
+}
+
+bool stm32_boot_is_serial(void)
+{
+	uint32_t boot_itf;
+	uint32_t boot_instance;
+
+	stm32_get_boot_interface(&boot_itf, &boot_instance);
+
+	if ((boot_itf == BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART) ||
+	    (boot_itf == BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB)) {
+		return true;
+	}
+
+	return false;
+}
+
+#if defined(IMAGE_BL32) && DEBUG
+static const char *const dump_table[] = {
+	"sp_usr  ",
+	"lr_usr  ",
+	"spsr_irq",
+	"sp_irq  ",
+	"lr_irq  ",
+	"spsr_fiq",
+	"sp_fiq  ",
+	"lr_fiq  ",
+	"spsr_svc",
+	"sp_svc  ",
+	"lr_svc  ",
+	"spsr_abt",
+	"sp_abt  ",
+	"lr_abt  ",
+	"spsr_und",
+	"sp_und  ",
+	"lr_und  ",
+	"spsr_mon",
+	"sp_mon",
+	"lr_mon",
+	"scr",
+	"pmcr",
+};
+
+/*
+ * Dump CPU registers when entering in monitor.
+ */
+void stm32mp_dump_core_registers(bool fcore)
+{
+	static bool firstcore;
+	unsigned int i;
+	smc_ctx_t *ctx = smc_get_ctx(NON_SECURE);
+	uint32_t *reg = (uint32_t *)&ctx->sp_usr;
+
+	if (fcore) {
+		firstcore = true;
+	}
+
+	if (!firstcore) {
+		return;
+	}
+
+	INFO("CPU : %i\n", plat_my_core_pos());
+
+	for (i = 0U; i < ARRAY_SIZE(dump_table); i++) {
+		INFO("%s : 0x%x\n", dump_table[i], reg[i]);
+	}
+}
+#endif
diff --git a/plat/st/stm32mp1/stm32mp1_scmi.c b/plat/st/stm32mp1/stm32mp1_scmi.c
index 80faf0c6e0..a6086f93fd 100644
--- a/plat/st/stm32mp1/stm32mp1_scmi.c
+++ b/plat/st/stm32mp1/stm32mp1_scmi.c
@@ -8,12 +8,14 @@
 
 #include <platform_def.h>
 
+#include <drivers/clk.h>
 #include <drivers/st/scmi-msg.h>
 #include <drivers/st/scmi.h>
 #include <drivers/st/stm32mp1_clk.h>
 #include <drivers/st/stm32mp_reset.h>
 #include <dt-bindings/clock/stm32mp1-clks.h>
 #include <dt-bindings/reset/stm32mp1-resets.h>
+#include <lib/utils.h>
 
 #define TIMEOUT_US_1MS		1000U
 
@@ -124,6 +126,7 @@ static struct stm32_scmi_rstd stm32_scmi0_reset_domain[] = {
 	RESET_CELL(RST_SCMI0_RNG1, RNG1_R, "rng1"),
 	RESET_CELL(RST_SCMI0_MDMA, MDMA_R, "mdma"),
 	RESET_CELL(RST_SCMI0_MCU, MCU_R, "mcu"),
+	RESET_CELL(RST_SCMI0_MCU_HOLD_BOOT, MCU_HOLD_BOOT_R, "mcu_hold_boot"),
 };
 
 struct scmi_agent_resources {
@@ -260,6 +263,17 @@ const char *plat_scmi_clock_get_name(unsigned int agent_id,
 
 int32_t plat_scmi_clock_rates_array(unsigned int agent_id, unsigned int scmi_id,
 				    unsigned long *array, size_t *nb_elts)
+{
+	/*
+	 * Do not expose clock rates by array since not supported by
+	 * Linux kernel
+	 */
+	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);
 
@@ -271,12 +285,50 @@ int32_t plat_scmi_clock_rates_array(unsigned int agent_id, unsigned int scmi_id,
 		return SCMI_DENIED;
 	}
 
-	if (array == NULL) {
-		*nb_elts = 1U;
-	} else if (*nb_elts == 1U) {
-		*array = stm32mp_clk_get_rate(clock->clock_id);
-	} else {
-		return SCMI_GENERIC_ERROR;
+	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;
+	}
+	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);
+
+	if (clock == NULL) {
+		return SCMI_NOT_FOUND;
+	}
+
+	if (!stm32mp_nsec_can_access_clock(clock->clock_id)) {
+		return SCMI_DENIED;
+	}
+
+	switch (scmi_id) {
+	case CK_SCMI0_MPU:
+		if (stm32mp1_set_opp_khz(rate / 1000UL) != 0) {
+			return SCMI_INVALID_PARAMETERS;
+		}
+		break;
+	default:
+		if (rate != clk_get_rate(clock->clock_id)) {
+			return SCMI_INVALID_PARAMETERS;
+		}
+		break;
 	}
 
 	return SCMI_SUCCESS;
@@ -292,7 +344,7 @@ unsigned long plat_scmi_clock_get_rate(unsigned int agent_id,
 		return 0U;
 	}
 
-	return stm32mp_clk_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)
@@ -323,13 +375,13 @@ int32_t plat_scmi_clock_set_state(unsigned int agent_id, unsigned int scmi_id,
 	if (enable_not_disable) {
 		if (!clock->enabled) {
 			VERBOSE("SCMI clock %u enable\n", scmi_id);
-			stm32mp_clk_enable(clock->clock_id);
+			clk_enable(clock->clock_id);
 			clock->enabled = true;
 		}
 	} else {
 		if (clock->enabled) {
 			VERBOSE("SCMI clock %u disable\n", scmi_id);
-			stm32mp_clk_disable(clock->clock_id);
+			clk_disable(clock->clock_id);
 			clock->enabled = false;
 		}
 	}
@@ -388,6 +440,10 @@ int32_t plat_scmi_rstd_autonomous(unsigned int agent_id, unsigned int scmi_id,
 		return SCMI_NOT_FOUND;
 	}
 
+	if (rstd->reset_id == MCU_HOLD_BOOT_R) {
+		return SCMI_NOT_SUPPORTED;
+	}
+
 	if (!stm32mp_nsec_can_access_reset(rstd->reset_id)) {
 		return SCMI_DENIED;
 	}
@@ -423,6 +479,13 @@ int32_t plat_scmi_rstd_set_state(unsigned int agent_id, unsigned int scmi_id,
 		return SCMI_DENIED;
 	}
 
+	if (rstd->reset_id == MCU_HOLD_BOOT_R) {
+		VERBOSE("SCMI MCU reset %s\n",
+			assert_not_deassert ? "set" : "release");
+		stm32mp_reset_assert_deassert_to_mcu(assert_not_deassert);
+		return SCMI_SUCCESS;
+	}
+
 	if (assert_not_deassert) {
 		VERBOSE("SCMI reset %lu set\n", rstd->reset_id);
 		stm32mp_reset_set(rstd->reset_id);
@@ -461,7 +524,7 @@ void stm32mp1_init_scmi_server(void)
 			/* Sync SCMI clocks with their targeted initial state */
 			if (clk->enabled &&
 			    stm32mp_nsec_can_access_clock(clk->clock_id)) {
-				stm32mp_clk_enable(clk->clock_id);
+				clk_enable(clk->clock_id);
 			}
 		}
 
@@ -476,3 +539,51 @@ void stm32mp1_init_scmi_server(void)
 		}
 	}
 }
+
+/*
+ * Save and restore SCMI state since lost during suspend.
+ * Only clock enabled field needs to be updated.
+ */
+void stm32mp1_pm_save_scmi_state(uint8_t *state, size_t size)
+{
+	size_t i;
+	size_t j;
+	size_t cnt = 0U;
+
+	zeromem(state, size);
+
+	for (i = 0U; i < ARRAY_SIZE(agent_resources); i++) {
+		for (j = 0U; j < agent_resources[i].clock_count; j++) {
+			if ((cnt / 8) > size) {
+				VERBOSE("state table too small\n");
+				panic();
+			}
+
+			if (agent_resources[i].clock[j].enabled) {
+				*(state + (cnt / 8)) |= (uint8_t)BIT(cnt % 8);
+			}
+
+			cnt++;
+		}
+	}
+}
+
+void stm32mp1_pm_restore_scmi_state(uint8_t *state, size_t size)
+{
+	size_t i;
+	size_t j;
+	size_t cnt = 0U;
+
+	for (i = 0U; i < ARRAY_SIZE(agent_resources); i++) {
+		for (j = 0U; j < agent_resources[i].clock_count; j++) {
+			if ((*(state + (cnt / 8)) & BIT(cnt % 8)) == 0U) {
+				agent_resources[i].clock[j].enabled = 0;
+			} else {
+				agent_resources[i].clock[j].enabled = 1;
+			}
+
+			assert((cnt / 8) <= size);
+			cnt++;
+		}
+	}
+}
diff --git a/plat/st/stm32mp1/stm32mp1_security.c b/plat/st/stm32mp1/stm32mp1_security.c
index 3a29ba9661..b1838db26e 100644
--- a/plat/st/stm32mp1/stm32mp1_security.c
+++ b/plat/st/stm32mp1/stm32mp1_security.c
@@ -10,22 +10,50 @@
 
 #include <common/debug.h>
 #include <drivers/arm/tzc400.h>
+#include <drivers/clk.h>
 #include <drivers/st/stm32mp1_clk.h>
 #include <dt-bindings/clock/stm32mp1-clks.h>
+#include <dt-bindings/soc/stm32mp1-tzc400.h>
 #include <lib/mmio.h>
 
-#define TZC_REGION_NSEC_ALL_ACCESS_RDWR \
-	TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_A7_ID) | \
-	TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_GPU_ID) | \
-	TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_LCD_ID) | \
-	TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_MDMA_ID) | \
-	TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_M4_ID) | \
-	TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_DMA_ID) | \
-	TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_USB_HOST_ID) | \
-	TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_USB_OTG_ID) | \
-	TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_SDMMC_ID) | \
-	TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_ETH_ID) | \
-	TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_DAP_ID)
+static unsigned int region_nb;
+
+static void init_tzc400_begin(unsigned int region0_attr)
+{
+	tzc400_init(STM32MP1_TZC_BASE);
+	tzc400_disable_filters();
+
+	/* Region 0 set to cover all DRAM at 0xC000_0000 */
+	tzc400_configure_region0(region0_attr, 0);
+
+	region_nb = 1U;
+}
+
+static void init_tzc400_end(unsigned int action)
+{
+	tzc400_set_action(action);
+	tzc400_enable_filters();
+}
+
+static void tzc400_add_region(unsigned long long region_base,
+			      unsigned long long region_top, bool sec)
+{
+	unsigned int sec_attr;
+	unsigned int nsaid_permissions;
+
+	if (sec) {
+		sec_attr = TZC_REGION_S_RDWR;
+		nsaid_permissions = 0;
+	} else {
+		sec_attr = TZC_REGION_S_NONE;
+		nsaid_permissions = TZC_REGION_NSEC_ALL_ACCESS_RDWR;
+	}
+
+	tzc400_configure_region(STM32MP1_FILTER_BIT_ALL, region_nb, region_base,
+				region_top, sec_attr, nsaid_permissions);
+
+	region_nb++;
+}
 
 /*******************************************************************************
  * Initialize the TrustZone Controller. Configure Region 0 with Secure RW access
@@ -38,10 +66,9 @@ static void init_tzc400(void)
 	unsigned long long ddr_ns_size =
 		(unsigned long long)stm32mp_get_ddr_ns_size();
 	unsigned long long ddr_ns_top = ddr_base + (ddr_ns_size - 1U);
+	unsigned long long ddr_top __unused;
 
-	tzc400_init(STM32MP1_TZC_BASE);
-
-	tzc400_disable_filters();
+	init_tzc400_begin(TZC_REGION_S_NONE);
 
 	/*
 	 * Region 1 set to cover all non-secure DRAM at 0xC000_0000. Apply the
@@ -49,36 +76,28 @@ static void init_tzc400(void)
 	 */
 	region_base = ddr_base;
 	region_top = ddr_ns_top;
-	tzc400_configure_region(STM32MP1_FILTER_BIT_ALL, 1,
-				region_base,
-				region_top,
-				TZC_REGION_S_NONE,
-				TZC_REGION_NSEC_ALL_ACCESS_RDWR);
+	tzc400_add_region(region_base, region_top, false);
 
-#ifdef AARCH32_SP_OPTEE
+#if defined(AARCH32_SP_OPTEE)
 	/* Region 2 set to cover all secure DRAM. */
 	region_base = region_top + 1U;
 	region_top += STM32MP_DDR_S_SIZE;
-	tzc400_configure_region(STM32MP1_FILTER_BIT_ALL, 2,
-				region_base,
-				region_top,
-				TZC_REGION_S_RDWR,
-				0);
-
-	/* Region 3 set to cover non-secure shared memory DRAM. */
-	region_base = region_top + 1U;
-	region_top += STM32MP_DDR_SHMEM_SIZE;
-	tzc400_configure_region(STM32MP1_FILTER_BIT_ALL, 3,
-				region_base,
-				region_top,
-				TZC_REGION_S_NONE,
-				TZC_REGION_NSEC_ALL_ACCESS_RDWR);
+	tzc400_add_region(region_base, region_top, true);
+
+	ddr_top = STM32MP_DDR_BASE + dt_get_ddr_size() - 1U;
+	if (region_top < ddr_top) {
+		/* Region 3 set to cover non-secure memory DRAM after BL32. */
+		region_base = region_top + 1U;
+		region_top = ddr_top;
+		tzc400_add_region(region_base, region_top, false);
+	}
 #endif
 
-	/* Raise an exception if a NS device tries to access secure memory */
-	tzc400_set_action(TZC_ACTION_ERR);
-
-	tzc400_enable_filters();
+	/*
+	 * Raise an interrupt (secure FIQ) if a NS device tries to access
+	 * secure memory
+	 */
+	init_tzc400_end(TZC_ACTION_INT);
 }
 
 /*******************************************************************************
@@ -88,26 +107,14 @@ static void init_tzc400(void)
  ******************************************************************************/
 static void early_init_tzc400(void)
 {
-	stm32mp_clk_enable(TZC1);
-	stm32mp_clk_enable(TZC2);
-
-	tzc400_init(STM32MP1_TZC_BASE);
+	clk_enable(TZC1);
+	clk_enable(TZC2);
 
-	tzc400_disable_filters();
-
-	/* Region 1 set to cover Non-Secure DRAM at 0xC000_0000 */
-	tzc400_configure_region(STM32MP1_FILTER_BIT_ALL, 1,
-				STM32MP_DDR_BASE,
-				STM32MP_DDR_BASE +
-				(STM32MP_DDR_MAX_SIZE - 1U),
-				TZC_REGION_S_NONE,
-				TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_A7_ID) |
-				TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_SDMMC_ID));
+	/* Region 0 set to cover all DRAM secure at 0xC000_0000 */
+	init_tzc400_begin(TZC_REGION_S_RDWR);
 
 	/* Raise an exception if a NS device tries to access secure memory */
-	tzc400_set_action(TZC_ACTION_ERR);
-
-	tzc400_enable_filters();
+	init_tzc400_end(TZC_ACTION_ERR);
 }
 
 /*******************************************************************************
diff --git a/plat/st/stm32mp1/stm32mp1_shared_resources.c b/plat/st/stm32mp1/stm32mp1_shared_resources.c
index 208e34a8b0..01fb499c1a 100644
--- a/plat/st/stm32mp1/stm32mp1_shared_resources.c
+++ b/plat/st/stm32mp1/stm32mp1_shared_resources.c
@@ -87,6 +87,64 @@ static const char __unused *shres2str_state(unsigned int state)
 	return shres2str_state_tbl[state];
 }
 
+struct shres2decprot {
+	unsigned int shres_id;
+	unsigned int decprot_id;
+	const char *decprot_str;
+};
+
+#define SHRES2DECPROT(shres, decprot, str) {	\
+		.shres_id = shres,		\
+		.decprot_id = decprot,		\
+		.decprot_str = str,		\
+	}
+
+#define SHRES_INVALID		~0U
+
+static const struct shres2decprot shres2decprot_tbl[] = {
+	SHRES2DECPROT(STM32MP1_SHRES_IWDG1, STM32MP1_ETZPC_IWDG1_ID, "IWDG1"),
+	SHRES2DECPROT(STM32MP1_SHRES_USART1, STM32MP1_ETZPC_USART1_ID, "UART1"),
+	SHRES2DECPROT(STM32MP1_SHRES_SPI6, STM32MP1_ETZPC_SPI6_ID, "SPI6"),
+	SHRES2DECPROT(STM32MP1_SHRES_I2C4, STM32MP1_ETZPC_I2C4_ID, "I2C4"),
+	SHRES2DECPROT(STM32MP1_SHRES_RNG1, STM32MP1_ETZPC_RNG1_ID, "RNG1"),
+	SHRES2DECPROT(STM32MP1_SHRES_HASH1, STM32MP1_ETZPC_HASH1_ID, "HASH1"),
+	SHRES2DECPROT(STM32MP1_SHRES_CRYP1, STM32MP1_ETZPC_CRYP1_ID, "CRYP1"),
+	SHRES2DECPROT(STM32MP1_SHRES_I2C6, STM32MP1_ETZPC_I2C6_ID, "I2C6"),
+	/* Below are specific IDs without a 1-to-1 mapping to SHRES IDs */
+	SHRES2DECPROT(SHRES_INVALID, STM32MP1_ETZPC_STGENC_ID, "STGEN"),
+	SHRES2DECPROT(SHRES_INVALID, STM32MP1_ETZPC_BKPSRAM_ID, "BKPSRAM"),
+	SHRES2DECPROT(SHRES_INVALID, STM32MP1_ETZPC_DDRCTRL_ID, "DDRCTRL"),
+	SHRES2DECPROT(SHRES_INVALID, STM32MP1_ETZPC_DDRPHYC_ID, "DDRPHY"),
+};
+
+static unsigned int decprot2shres(unsigned int decprot_id)
+{
+	uint32_t i;
+
+	for (i = 0; i < ARRAY_SIZE(shres2decprot_tbl); i++) {
+		if (shres2decprot_tbl[i].decprot_id == decprot_id) {
+			return shres2decprot_tbl[i].shres_id;
+		}
+	}
+
+	VERBOSE("No shared resource %u", decprot_id);
+	return SHRES_INVALID;
+}
+
+static const char *decprot2str(unsigned int decprot_id)
+{
+	size_t i;
+
+	for (i = 0; i < ARRAY_SIZE(shres2decprot_tbl); i++) {
+		if (shres2decprot_tbl[i].decprot_id == decprot_id) {
+			return shres2decprot_tbl[i].decprot_str;
+		}
+	}
+
+	ERROR("Invalid ID %u", decprot_id);
+	panic();
+}
+
 /* Get resource state: these accesses lock the registering support */
 static void lock_registering(void)
 {
@@ -114,7 +172,7 @@ static unsigned int get_gpio_nbpin(unsigned int bank)
 	if (bank != GPIO_BANK_Z) {
 		int count = fdt_get_gpio_bank_pin_count(bank);
 
-		assert((count >= 0) || (count <= (GPIO_PIN_MAX + 1)));
+		assert((count >= 0) || ((unsigned int)count <= (GPIO_PIN_MAX + 1)));
 
 		return (unsigned int)count;
 	}
@@ -163,7 +221,7 @@ static void register_periph(enum stm32mp_shres id, unsigned int state)
 
 	if ((id >= STM32MP1_SHRES_GPIOZ(0)) &&
 	    (id <= STM32MP1_SHRES_GPIOZ(7)) &&
-	    ((id - STM32MP1_SHRES_GPIOZ(0)) >= get_gpioz_nbpin())) {
+	    ((unsigned int)(id - STM32MP1_SHRES_GPIOZ(0)) >= get_gpioz_nbpin())) {
 		ERROR("Invalid GPIO pin %u, %u pin(s) available\n",
 		      id - STM32MP1_SHRES_GPIOZ(0), get_gpioz_nbpin());
 		panic();
@@ -334,6 +392,53 @@ void stm32mp_register_non_secure_gpio(unsigned int bank, unsigned int pin)
 	}
 }
 
+void stm32mp1_register_etzpc_decprot(unsigned int id,
+				     enum etzpc_decprot_attributes attr)
+{
+	unsigned int state = SHRES_SECURE;
+	unsigned int id_shres;
+
+	switch (attr) {
+	case ETZPC_DECPROT_S_RW:
+		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();
+	}
+
+	switch (id) {
+	case STM32MP1_ETZPC_STGENC_ID:
+	case STM32MP1_ETZPC_BKPSRAM_ID:
+		/* We assume these must always be assigned to secure world */
+		if (state != SHRES_SECURE) {
+			panic();
+		}
+		break;
+	case STM32MP1_ETZPC_DDRCTRL_ID:
+	case STM32MP1_ETZPC_DDRPHYC_ID:
+		/* allow write only for secure world */
+		if ((attr != ETZPC_DECPROT_S_RW) &&
+		    (attr != ETZPC_DECPROT_NS_R_S_W)) {
+			panic();
+		}
+		break;
+	default:
+		id_shres = decprot2shres(id);
+		if (id_shres == SHRES_INVALID) {
+			if (state == SHRES_SECURE) {
+				panic();
+			}
+		} else {
+			register_periph(id_shres, state);
+		}
+		break;
+	}
+}
+
 static bool stm32mp_gpio_bank_is_non_secure(unsigned int bank)
 {
 	unsigned int non_secure = 0U;
@@ -379,12 +484,15 @@ bool stm32mp_nsec_can_access_clock(unsigned long clock_id)
 	enum stm32mp_shres shres_id = STM32MP1_SHRES_COUNT;
 
 	switch (clock_id) {
+	case BSEC:
+	case CK_AXI:
 	case CK_CSI:
 	case CK_HSE:
 	case CK_HSE_DIV2:
 	case CK_HSI:
 	case CK_LSE:
 	case CK_LSI:
+	case CK_MPU:
 	case PLL1_P:
 	case PLL1_Q:
 	case PLL1_R:
@@ -454,6 +562,7 @@ bool stm32mp_nsec_can_access_reset(unsigned int reset_id)
 		shres_id = STM32MP1_SHRES_I2C6;
 		break;
 	case MCU_R:
+	case MCU_HOLD_BOOT_R:
 		shres_id = STM32MP1_SHRES_MCU;
 		break;
 	case MDMA_R:
@@ -499,33 +608,81 @@ static enum etzpc_decprot_attributes shres2decprot_attr(enum stm32mp_shres id)
 	return ETZPC_DECPROT_S_RW;
 }
 
-static void set_etzpc_secure_configuration(void)
+static bool check_decprot(unsigned int id, enum etzpc_decprot_attributes exp)
 {
-	/* Some system peripherals shall be secure */
-	etzpc_configure_decprot(STM32MP1_ETZPC_STGENC_ID, ETZPC_DECPROT_S_RW);
-	etzpc_configure_decprot(STM32MP1_ETZPC_BKPSRAM_ID, ETZPC_DECPROT_S_RW);
-	etzpc_configure_decprot(STM32MP1_ETZPC_DDRCTRL_ID,
-				ETZPC_DECPROT_NS_R_S_W);
-	etzpc_configure_decprot(STM32MP1_ETZPC_DDRPHYC_ID,
-				ETZPC_DECPROT_NS_R_S_W);
-
-	/* Configure ETZPC with peripheral registering */
-	etzpc_configure_decprot(STM32MP1_ETZPC_CRYP1_ID,
-				shres2decprot_attr(STM32MP1_SHRES_CRYP1));
-	etzpc_configure_decprot(STM32MP1_ETZPC_HASH1_ID,
-				shres2decprot_attr(STM32MP1_SHRES_HASH1));
-	etzpc_configure_decprot(STM32MP1_ETZPC_I2C4_ID,
-				shres2decprot_attr(STM32MP1_SHRES_I2C4));
-	etzpc_configure_decprot(STM32MP1_ETZPC_I2C6_ID,
-				shres2decprot_attr(STM32MP1_SHRES_I2C6));
-	etzpc_configure_decprot(STM32MP1_ETZPC_IWDG1_ID,
-				shres2decprot_attr(STM32MP1_SHRES_IWDG1));
-	etzpc_configure_decprot(STM32MP1_ETZPC_RNG1_ID,
-				shres2decprot_attr(STM32MP1_SHRES_RNG1));
-	etzpc_configure_decprot(STM32MP1_ETZPC_USART1_ID,
+	enum etzpc_decprot_attributes cur = etzpc_get_decprot(id);
+
+	if (cur == exp) {
+		return true;
+	}
+
+	switch (exp) {
+	case ETZPC_DECPROT_NS_RW:
+		if (cur == ETZPC_DECPROT_S_RW) {
+			INFO("ETZPC: %s (%d) could be non secure\n",
+			     decprot2str(id), id);
+		}
+		return true;
+
+	case ETZPC_DECPROT_S_RW:
+		ERROR("ETZPC: %s (%d) expected secure but DECPROT = %d\n",
+		      decprot2str(id), id, cur);
+		break;
+
+	case ETZPC_DECPROT_NS_R_S_W:
+	case ETZPC_DECPROT_MCU_ISOLATION:
+		break;
+	default:
+		panic();
+	}
+
+	return false;
+}
+
+static void check_etzpc_secure_configuration(void)
+{
+	bool error = false;
+
+	assert(registering_locked);
+
+	error |= !check_decprot(STM32MP1_ETZPC_STGENC_ID, ETZPC_DECPROT_S_RW);
+
+	error |= !check_decprot(STM32MP1_ETZPC_BKPSRAM_ID, ETZPC_DECPROT_S_RW);
+
+	error |= !check_decprot(STM32MP1_ETZPC_USART1_ID,
 				shres2decprot_attr(STM32MP1_SHRES_USART1));
-	etzpc_configure_decprot(STM32MP1_ETZPC_SPI6_ID,
+
+	error |= !check_decprot(STM32MP1_ETZPC_I2C4_ID,
+				shres2decprot_attr(STM32MP1_SHRES_I2C4));
+
+	error |= !check_decprot(STM32MP1_ETZPC_SPI6_ID,
 				shres2decprot_attr(STM32MP1_SHRES_SPI6));
+
+	error |= !check_decprot(STM32MP1_ETZPC_RNG1_ID,
+				shres2decprot_attr(STM32MP1_SHRES_RNG1));
+
+	error |= !check_decprot(STM32MP1_ETZPC_HASH1_ID,
+				shres2decprot_attr(STM32MP1_SHRES_HASH1));
+
+	error |= !check_decprot(STM32MP1_ETZPC_CRYP1_ID,
+				shres2decprot_attr(STM32MP1_SHRES_CRYP1));
+
+	error |= !((check_decprot(STM32MP1_ETZPC_DDRCTRL_ID,
+				  ETZPC_DECPROT_NS_R_S_W)) ||
+		   (check_decprot(STM32MP1_ETZPC_DDRCTRL_ID,
+				  ETZPC_DECPROT_S_RW)));
+
+	error |= !((check_decprot(STM32MP1_ETZPC_DDRPHYC_ID,
+				  ETZPC_DECPROT_NS_R_S_W)) ||
+		   (check_decprot(STM32MP1_ETZPC_DDRPHYC_ID,
+				  ETZPC_DECPROT_S_RW)));
+
+	error |= !check_decprot(STM32MP1_ETZPC_I2C6_ID,
+				shres2decprot_attr(STM32MP1_SHRES_I2C6));
+
+	if (error) {
+		panic();
+	}
 }
 
 static void check_rcc_secure_configuration(void)
@@ -592,6 +749,6 @@ void stm32mp_lock_periph_registering(void)
 	print_shared_resources_state();
 
 	check_rcc_secure_configuration();
-	set_etzpc_secure_configuration();
+	check_etzpc_secure_configuration();
 	set_gpio_secure_configuration();
 }
diff --git a/plat/st/stm32mp1/stm32mp1_ssp.c b/plat/st/stm32mp1/stm32mp1_ssp.c
new file mode 100644
index 0000000000..bcbf374782
--- /dev/null
+++ b/plat/st/stm32mp1/stm32mp1_ssp.c
@@ -0,0 +1,1039 @@
+/*
+ * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <endian.h>
+#include <errno.h>
+#include <limits.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <arch_helpers.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/generic_delay_timer.h>
+#include <drivers/st/bsec.h>
+#include <drivers/st/stm32_console.h>
+#include <drivers/st/stm32_hash.h>
+#include <drivers/st/stm32_iwdg.h>
+#include <drivers/st/stm32_uart.h>
+#include <drivers/st/stm32mp_pmic.h>
+#include <drivers/st/stm32mp_reset.h>
+#include <drivers/st/stpmic1.h>
+#include <lib/mmio.h>
+#include <lib/usb/usb_core.h>
+#include <lib/usb/usb_st_dfu.h>
+#include <lib/utils.h>
+#include <lib/utils_def.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <plat/common/platform.h>
+
+#include <stm32cubeprogrammer.h>
+
+#define CERT_CHIP_ID_LEN		U(3)
+#define CERT_SECURITY_COUNTER_LEN	U(2)
+#define CERT_SECURITY_COUNTER_SHIFT	CERT_CHIP_ID_LEN
+#define CERT_RFU_LEN			U(1)
+#define CERT_RFU_SHIFT			(CERT_SECURITY_COUNTER_LEN + \
+					 CERT_SECURITY_COUNTER_SHIFT)
+#define CERT_PRODUCT_KEY_LEN		U(2)
+#define CERT_PRODUCT_KEY_SHIFT		(CERT_RFU_LEN + CERT_RFU_SHIFT)
+#define CERT_PRODUCT_ID_SIZE		(CERT_PRODUCT_KEY_LEN + \
+					 CERT_PRODUCT_KEY_SHIFT)
+#define CERT_SIGNATURE_LEN		CHIP_CERTIFICATE_MAX_SIZE
+#define CERT_SIGNATURE_SHIFT		(CERT_PRODUCT_ID_SIZE + \
+					 BOOT_API_SSP_PUBK_KEY_SIZE_BYTES)
+#define CERTIFICATE_SIZE		(CERT_PRODUCT_ID_SIZE + \
+					 BOOT_API_SSP_PUBK_KEY_SIZE_BYTES + \
+					 CERT_SIGNATURE_LEN) /* 136 bytes */
+#define RESET_TIMEOUT_US_1MS		U(1000)
+#define BLOB_FILE_MAX_ADDR		BL2_RW_LIMIT
+
+/* Local status for SSP processing sequences */
+typedef enum {
+	SSP_NONE,
+	SSP_GET_CERT,
+	SSP_FLASH_OEM,
+	SSP_DONE,
+	SSP_ERROR
+} ssp_result_e;
+
+struct otp_val {
+	uint32_t idx;
+	uint32_t nb;
+};
+
+static struct otp_val otp_ssp;
+static struct otp_val otp_rma;
+static struct otp_val otp_pubkey;
+
+#if DEBUG
+static console_t console;
+#endif
+
+/* Platform empty definition required */
+void bl2_platform_setup(void) {}
+struct bl_params *plat_get_next_bl_params(void) { return NULL; }
+void plat_flush_next_bl_params(void) {}
+struct bl_load_info *plat_get_bl_image_load_info(void) { return NULL; }
+int plat_get_image_source(unsigned int image_id, uintptr_t *dev_handle,
+			  uintptr_t *image_spec)
+{
+	return 0;
+}
+
+/*
+ * Initialized OTP index from device tree.
+ */
+static int initialize_otp(void)
+{
+	uint32_t len;
+
+	/* OTP SSP */
+	if (stm32_get_otp_index(SSP_OTP, &otp_ssp.idx, NULL) != 0) {
+		VERBOSE("%s: get index error\n", __func__);
+		return -EINVAL;
+	}
+
+	/* OTP public key */
+	if (stm32_get_otp_index(PKH_OTP, &otp_pubkey.idx, &len) != 0) {
+		VERBOSE("%s: get index error\n", __func__);
+		return -EINVAL;
+	}
+
+	if (len != (CHAR_BIT * BOOT_API_SHA256_DIGEST_SIZE_IN_BYTES)) {
+		VERBOSE("%s: length Error\n", __func__);
+		return -EINVAL;
+	}
+
+	otp_pubkey.nb = len / __WORD_BIT;
+
+	/* OTP RMA */
+	if (stm32_get_otp_index(RMA_OTP, &otp_rma.idx, NULL) != 0) {
+		VERBOSE("%s: get index error\n", __func__);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * Compute HASH from public key and burn it in OTP.
+ */
+static int ssp_pub_key_prog(boot_api_context_t *boot_context)
+{
+	uint8_t key_hash[BOOT_API_SHA256_DIGEST_SIZE_IN_BYTES] __aligned(4);
+	uint8_t *pubk = (uint8_t *)
+		boot_context->p_ssp_config->p_blob_payload->oem_ecdsa_pubk;
+	uint32_t *value = (uint32_t *)key_hash;
+	uint32_t i;
+
+	if (stm32_hash_register() != 0) {
+		return -EINVAL;
+	}
+
+	stm32_hash_init(HASH_SHA256);
+
+	if (stm32_hash_final_update(pubk, BOOT_API_SSP_PUBK_KEY_SIZE_BYTES,
+				    key_hash) != 0) {
+		ERROR("Hash of payload failed\n");
+		return -EINVAL;
+	}
+
+	for (i = otp_pubkey.idx; i < (otp_pubkey.idx + otp_pubkey.nb); i++) {
+		if (bsec_program_otp(bswap32(*value), i) != BSEC_OK) {
+			return -EINVAL;
+		}
+
+		value++;
+		if (bsec_permanent_lock_otp(i) != BSEC_OK) {
+			ERROR("Error locking OTP %i\n", i);
+			panic();
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * Burn OTP to close device.
+ */
+static int ssp_close_device(void)
+{
+	uint32_t otp;
+	uint32_t value;
+
+	if (stm32_get_otp_index(CFG0_OTP, &otp, NULL) != 0) {
+		return -EINVAL;
+	}
+
+	if (bsec_read_otp(&value, otp) != BSEC_OK) {
+		return -EINVAL;
+	}
+
+	if ((value & CFG0_CLOSED_DEVICE) != 0U) {
+		ERROR("Device already closed\n");
+		return -EINVAL;
+	}
+
+	value |= CFG0_CLOSED_DEVICE;
+	if (bsec_program_otp(value, otp) != BSEC_OK) {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * OTP initial check to detect previous values.
+ */
+static int ssp_secrets_check(boot_api_context_t *boot_ctx)
+{
+	uint32_t i;
+	uint32_t check_val;
+	uint32_t otp_bytes = boot_ctx->p_ssp_config->p_blob_payload->oem_secret_size_bytes;
+	uint32_t otp_decrypted;
+
+	if (otp_bytes == 0U) {
+		return -EINVAL;
+	}
+
+	for (i = otp_pubkey.idx; i < (otp_pubkey.idx + otp_pubkey.nb); i++) {
+		if (stm32_get_otp_value_from_idx(i, &check_val) != 0) {
+			return -EINVAL;
+		}
+
+		if (check_val != 0U) {
+			ERROR("OTP %u value already programmed\n", i);
+			return -EINVAL;
+		}
+	}
+
+	otp_decrypted = round_up(otp_bytes, sizeof(uint32_t)) / sizeof(uint32_t);
+
+	/* OTP decrypted include RMA password */
+	if (otp_decrypted > (2U + SSP_OTP_SECRET_END - SSP_OTP_SECRET_BASE)) {
+		return -EINVAL;
+	}
+
+	/* Check RMA password */
+	if (stm32_get_otp_value_from_idx(otp_rma.idx, &check_val) != 0) {
+		return -EINVAL;
+	}
+
+	if (check_val != 0U) {
+		ERROR("OTP %s value already programmed\n", RMA_OTP);
+		return -EINVAL;
+	}
+
+	/* Check all OTP available */
+	for (i = SSP_OTP_SECRET_BASE; i < SSP_OTP_SECRET_BASE + otp_decrypted - 1U; i++) {
+		if (stm32_get_otp_value_from_idx(i, &check_val) != 0) {
+			return -EINVAL;
+		}
+
+		if (check_val != 0U) {
+			ERROR("OTP %u value already programmed\n", i);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * Burn OTP with the decrypted secret received.
+ */
+static int ssp_secrets_flash(boot_api_context_t *boot_ctx)
+{
+	uint32_t i;
+	uint32_t *val;
+	uint32_t otp_bytes =
+		boot_ctx->p_ssp_config->p_blob_payload->oem_secret_size_bytes;
+	uint32_t otp_decrypted;
+	uint32_t otp_mask = 0U;
+
+	if (otp_bytes == 0U) {
+		return -EINVAL;
+	}
+
+	if (otp_bytes % sizeof(uint32_t) != 0U) {
+		otp_mask = GENMASK_32(((otp_bytes % sizeof(uint32_t)) *
+				       sizeof(uint32_t)) - 1, 0);
+	}
+
+	val = (uint32_t *)boot_ctx->p_ssp_config->p_ssp_oem_secrets_decrypted;
+
+	otp_decrypted = round_up(otp_bytes, sizeof(uint32_t)) / sizeof(uint32_t);
+
+	/* Burn RMA password */
+	if (bsec_program_otp((*val & RMA_OTP_MASK), otp_rma.idx) !=  BSEC_OK) {
+		WARN("RMA programing failed\n");
+		return -EINVAL;
+	}
+
+	val++;
+	otp_decrypted--;
+	for (i = SSP_OTP_SECRET_BASE; i < (SSP_OTP_SECRET_BASE + otp_decrypted - 1U); i++) {
+		if (*val == 0U) {
+			val++;
+			continue;
+		}
+
+		if (bsec_program_otp(*val, i) != BSEC_OK) {
+			WARN("Error writing OTP %i\n", i);
+			return -EINVAL;
+		}
+
+		if (bsec_permanent_lock_otp(i) != BSEC_OK) {
+			WARN("Error locking OTP %i\n", i);
+			return -EINVAL;
+		}
+
+		val++;
+	}
+
+	if (*val == 0U) {
+		return 0;
+	}
+
+	/* Mask the last OTP value if needed */
+	if (otp_mask != 0U) {
+		*val &= otp_mask;
+	}
+
+	if (bsec_program_otp(*val, i) != BSEC_OK) {
+		WARN("Error writing OTP %i\n", i);
+		return -EINVAL;
+	}
+
+	if (bsec_permanent_lock_otp(i) != BSEC_OK) {
+		WARN("Error locking OTP %i\n", i);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * Finish SSP processing by fusing OTP SSP success.
+ */
+static int ssp_finish_process(void)
+{
+	uint32_t val;
+
+	if (stm32_get_otp_value_from_idx(otp_ssp.idx, &val) != 0) {
+		return -EINVAL;
+	}
+
+	if ((val & SSP_OTP_SUCCESS) != 0U) {
+		WARN("Error while configuring OTP\n");
+		return -EINVAL;
+	}
+
+	val |= SSP_OTP_SUCCESS;
+	if (bsec_program_otp(val, otp_ssp.idx) != BSEC_OK) {
+		return -EINVAL;
+	}
+
+	VERBOSE("Write OTP Success\n");
+
+	return 0;
+}
+
+/*
+ * Transform integer to string.
+ */
+static void itoa(uint32_t num, char *str, int nb)
+{
+	if (num == 0U) {
+		while (nb-- != 0U) {
+			str[nb] = '0';
+		}
+
+		return;
+	}
+
+	while (num != 0U) {
+		int rem = num % 16;
+
+		str[--nb] = (rem > 9) ? (rem - 10) + 'A' : rem + '0';
+		num /= 16;
+	}
+
+	while (nb != 0) {
+		str[--nb] = '0';
+	}
+}
+
+/*
+ * Return chip product ID.
+ */
+static int ssp_get_product_id(char *msg)
+{
+	uint32_t otp;
+	uint32_t otp_idx;
+	uint32_t chip_id;
+
+	if (stm32_get_otp_index(CFG2_OTP, &otp_idx, NULL) != 0) {
+		VERBOSE("Get index error\n");
+		return -EINVAL;
+	}
+
+	if (stm32_get_otp_value_from_idx(otp_idx, &otp) != 0) {
+		return -EINVAL;
+	}
+
+	if (stm32mp1_dbgmcu_get_chip_dev_id(&chip_id) < 0) {
+		return -EINVAL;
+	}
+
+	itoa(chip_id, msg, CERT_CHIP_ID_LEN);
+	itoa((otp & OTP_CFG2_SEC_COUNTER_MASK) >> OTP_CFG2_SEC_COUNTER_SHIFT,
+	     msg + CERT_SECURITY_COUNTER_SHIFT,
+	     CERT_SECURITY_COUNTER_LEN);
+
+	itoa(0, msg + CERT_RFU_SHIFT, CERT_RFU_LEN);
+	itoa((otp & OTP_CFG2_ST_KEY_MASK) >> OTP_CFG2_ST_KEY_SHIFT,
+	     msg + CERT_PRODUCT_KEY_SHIFT,
+	     CERT_PRODUCT_KEY_LEN);
+
+	return 0;
+}
+
+/*
+ * Construct SSP certificate.
+ */
+static int prepare_certificate(uint8_t *cert, const uint8_t *pubkey)
+{
+	uint32_t i;
+	uint32_t j;
+	uint32_t otp;
+	uint32_t otp_idx;
+	uint32_t otp_len;
+
+	/* Prepare the ROM Security constant */
+	if (ssp_get_product_id((char *)cert) != 0) {
+		return -EINVAL;
+	}
+
+	/* Prepare public key and certificate for flashloader */
+	/* Read Public Key from boot_context */
+	memcpy(cert + CERT_PRODUCT_ID_SIZE, pubkey, BOOT_API_SSP_PUBK_KEY_SIZE_BYTES);
+
+	if (stm32_get_otp_index(CHIP_CERTIFICATE_OTP,
+				&otp_idx, &otp_len) != 0) {
+		VERBOSE("Get index error\n");
+		return -EINVAL;
+	}
+
+	if (otp_len != (CHAR_BIT * CHIP_CERTIFICATE_MAX_SIZE)) {
+		VERBOSE("Length error\n");
+		return -EINVAL;
+	}
+
+	otp_len /= __WORD_BIT;
+
+	/* Read Certificat from OTP */
+	for (i = otp_idx, j = 0U; i < (otp_idx + otp_len); i++, j++) {
+		uint32_t otp_s;
+
+		if (stm32_get_otp_value_from_idx(i, &otp) != 0) {
+			return -EINVAL;
+		}
+
+		otp_s = bswap32(otp);
+		memcpy(&cert[CERT_SIGNATURE_SHIFT + (sizeof(uint32_t) * j)],
+		       &otp_s, sizeof(uint32_t));
+	}
+
+	return 0;
+}
+
+/*
+ * Clean external data and bootrom context secret values.
+ */
+static void ssp_cleanup(boot_api_context_t *boot_context)
+{
+	boot_api_ssp_config_t *ssp_config = boot_context->p_ssp_config;
+
+	/* Cleanup boot_context */
+	if (ssp_config->p_ssp_oem_secrets_decrypted != NULL) {
+		zeromem(ssp_config->p_ssp_oem_secrets_decrypted,
+			BOOT_API_SSP_OEM_SECRETS_MAX_SIZE_BYTES);
+#ifndef DCACHE_OFF
+		flush_dcache_range((uintptr_t)ssp_config->p_ssp_oem_secrets_decrypted,
+				   BOOT_API_SSP_OEM_SECRETS_MAX_SIZE_BYTES);
+#endif
+		ssp_config->p_ssp_oem_secrets_decrypted = NULL;
+	}
+
+	if (ssp_config->p_chip_pubk != NULL) {
+		zeromem(ssp_config->p_chip_pubk,
+			BOOT_API_SSP_PUBK_KEY_SIZE_BYTES);
+#ifndef DCACHE_OFF
+		flush_dcache_range((uintptr_t)ssp_config->p_chip_pubk,
+				   BOOT_API_SSP_PUBK_KEY_SIZE_BYTES);
+#endif
+		ssp_config->p_chip_pubk = NULL;
+	}
+
+	if (ssp_config->p_blob_license != NULL) {
+		zeromem(ssp_config->p_blob_license,
+			sizeof(boot_api_ssp_blob_license_t));
+#ifndef DCACHE_OFF
+		flush_dcache_range((uintptr_t)ssp_config->p_blob_license,
+				   sizeof(boot_api_ssp_blob_license_t));
+#endif
+		ssp_config->p_blob_license = NULL;
+	}
+
+	if (ssp_config->p_blob_payload != NULL) {
+		zeromem(ssp_config->p_blob_payload,
+			sizeof(boot_api_ssp_blob_payload_t));
+#ifndef DCACHE_OFF
+		flush_dcache_range((uintptr_t)ssp_config->p_blob_payload,
+			   sizeof(boot_api_ssp_blob_payload_t));
+#endif
+		ssp_config->p_blob_payload = NULL;
+	}
+
+	ssp_config->ssp_cmd = 0U;
+
+#ifndef DCACHE_OFF
+	flush_dcache_range((uintptr_t)boot_context->p_ssp_config,
+			   sizeof(boot_api_ssp_config_t));
+#endif
+}
+
+/*
+ * Send certificate to the programmer and retrieve the associated
+ * encrypted file.
+ */
+static int ssp_download_phase(boot_api_context_t *boot_ctx)
+{
+	uint8_t *blob_file;
+#if STM32MP_USB_PROGRAMMER
+	usb_handle_t *pdev;
+#endif
+#if STM32MP_UART_PROGRAMMER
+	uintptr_t uart_base;
+#endif
+	int result = 0;
+	uint8_t cert[CERTIFICATE_SIZE];
+
+	blob_file = (uint8_t *)page_align(BLOB_FILE_MAX_ADDR -
+					  sizeof(boot_api_ssp_blob_license_t) -
+					  sizeof(boot_api_ssp_blob_payload_t),
+					  DOWN);
+
+	if (prepare_certificate(cert, boot_ctx->p_ssp_config->p_chip_pubk) != 0) {
+		return -EINVAL;
+	}
+
+	switch (boot_ctx->boot_interface_selected) {
+#if STM32MP_USB_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB:
+
+		/* init USB on platform */
+		pdev = usb_dfu_plat_init();
+
+		result = stm32cubeprog_usb_ssp(pdev, (uintptr_t)cert,
+					       sizeof(cert), (uintptr_t)blob_file,
+					       sizeof(boot_api_ssp_blob_license_t) +
+					       sizeof(boot_api_ssp_blob_payload_t));
+		if (result != 0) {
+			return -EINVAL;
+		}
+
+		break;
+#endif
+
+#if STM32MP_UART_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART:
+		uart_base = get_uart_address(boot_ctx->boot_interface_instance);
+
+		if (uart_base == 0U) {
+			return -EINVAL;
+		}
+
+		result = stm32cubeprog_uart_ssp(uart_base, (uintptr_t)cert, sizeof(cert),
+						(uintptr_t)blob_file,
+						sizeof(boot_api_ssp_blob_license_t) +
+						sizeof(boot_api_ssp_blob_payload_t));
+		if (result != 0) {
+			return -EINVAL;
+		}
+		break;
+#endif
+	default:
+		return -EINVAL;
+	}
+
+	boot_ctx->p_ssp_config->p_blob_license =
+		(boot_api_ssp_blob_license_t *)blob_file;
+
+	/* Payload is concatened with license file */
+	boot_ctx->p_ssp_config->p_blob_payload =
+		(boot_api_ssp_blob_payload_t *)(blob_file +
+		sizeof(boot_api_ssp_blob_license_t));
+
+#ifndef DCACHE_OFF
+	flush_dcache_range((uintptr_t)blob_file,
+			   sizeof(boot_api_ssp_blob_license_t) +
+			   sizeof(boot_api_ssp_blob_payload_t));
+#endif
+
+	/* Set return address for decrypted_secrets */
+	boot_ctx->p_ssp_config->p_ssp_oem_secrets_decrypted =
+		boot_ctx->p_ssp_config->p_blob_payload->oem_encrypted_secrets;
+
+	return result;
+}
+
+/*
+ * Burn decrypted secrets into OTP, clean memory and close the device.
+ */
+static int ssp_secret_programming(boot_api_context_t *boot_context)
+{
+	int result;
+
+	result = ssp_secrets_check(boot_context);
+	if (result != 0) {
+		ERROR("SSP ERROR checking OTP\n");
+		goto clean;
+	}
+
+	result = ssp_pub_key_prog(boot_context);
+	if (result != 0) {
+		ERROR("SSP ERROR writing HASH key\n");
+		goto clean;
+	}
+
+	result = ssp_close_device();
+	if (result != 0) {
+		ERROR("SSP close device failed\n");
+		goto clean;
+	}
+
+	result = ssp_secrets_flash(boot_context);
+	if (result != 0) {
+		ERROR("SSP Secret flash failed\n");
+	}
+
+clean:
+	ssp_cleanup(boot_context);
+
+	if (result != 0) {
+		return result;
+	}
+
+	return ssp_finish_process();
+}
+
+/*
+ * Enable the SSP processing.
+ */
+static int ssp_enable_processing(boot_api_context_t *boot_context)
+{
+	uint32_t val;
+	int result;
+#if STM32MP_USB_PROGRAMMER
+	usb_handle_t *pdev;
+#endif
+#if STM32MP_UART_PROGRAMMER
+	uintptr_t uart_base;
+#endif
+
+	if (stm32_get_otp_value_from_idx(otp_ssp.idx, &val) != 0) {
+		return -EINVAL;
+	}
+
+	if (((val & SSP_OTP_MASK) == SSP_OTP_MASK) ||
+	    ((val & SSP_OTP_MASK) == SSP_OTP_SUCCESS)) {
+		return -EINVAL;
+	}
+
+	if ((val & SSP_OTP_MASK) == 0U) {
+		if (bsec_program_otp(SSP_OTP_REQ, otp_ssp.idx) != BSEC_OK) {
+			return -EINVAL;
+		}
+	}
+
+	switch (boot_context->boot_interface_selected) {
+#if STM32MP_USB_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB:
+		/* init USB on platform */
+		pdev = usb_dfu_plat_init();
+
+		result = stm32cubeprog_usb_ssp(pdev, (uintptr_t)-1, 0,
+					       (uintptr_t)NULL, 0);
+		if (result != 0) {
+			return -EINVAL;
+		}
+
+		break;
+#endif
+
+#if STM32MP_UART_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART:
+		uart_base = get_uart_address(boot_context->boot_interface_instance);
+		if (uart_base == 0U) {
+			return -EINVAL;
+		}
+
+		result = stm32cubeprog_uart_ssp(uart_base, (uintptr_t)-1, 0,
+						(uintptr_t)NULL, 0);
+		if (result != 0) {
+			return -EINVAL;
+		}
+		break;
+#endif
+	default:
+		return -EINVAL;
+	}
+
+	boot_context->p_ssp_config->ssp_cmd =
+		BOOT_API_CTX_SSP_CMD_CALC_CHIP_PUBK;
+
+	return 0;
+}
+
+/*
+ * Retrieve the current status of the SSP from bootrom context and OTP value.
+ */
+static ssp_result_e ssp_check_status(boot_api_context_t *boot_context)
+{
+	uint32_t otp;
+
+	if (initialize_otp() < 0) {
+		return SSP_ERROR;
+	}
+
+	if (stm32_get_otp_value_from_idx(otp_ssp.idx, &otp) != 0) {
+		return SSP_ERROR;
+	}
+
+	if ((otp & SSP_OTP_REQ) == 0U) {
+		return SSP_NONE;
+	}
+
+	if ((otp & SSP_OTP_SUCCESS) != 0U) {
+		return SSP_DONE;
+	}
+
+	VERBOSE("Start Get ssp_cmd : %x\n",
+		boot_context->p_ssp_config->ssp_cmd);
+
+	switch (boot_context->p_ssp_config->ssp_cmd) {
+	case BOOT_API_CTX_SSP_CMD_CALC_CHIP_PUBK_ACK:
+		INFO("Detected start SSP Phase 2\n");
+		return SSP_GET_CERT;
+	case BOOT_API_CTX_SSP_CMD_PROV_SECRET_ACK:
+		INFO("Detected start SSP Phase 3\n");
+		return SSP_FLASH_OEM;
+	default:
+		return SSP_NONE;
+	}
+}
+
+/*
+ * Start the SSP processing.
+ */
+static void ssp_start(boot_api_context_t *boot_context)
+{
+	int result;
+	uint8_t ssp_phase = ssp_check_status(boot_context);
+
+	switch (ssp_phase) {
+	case SSP_GET_CERT:
+		result = ssp_download_phase(boot_context);
+		if (result != 0) {
+			/*
+			 * Download Phase failed, clean, reset
+			 */
+			ssp_cleanup(boot_context);
+
+			ERROR("SSP_Error: Resetting target\n");
+		} else {
+			/* Process completed, go to Phase 3 */
+			boot_context->p_ssp_config->ssp_cmd =
+				BOOT_API_CTX_SSP_CMD_PROV_SECRET;
+		}
+
+		break;
+
+	case SSP_FLASH_OEM:
+		result = ssp_secret_programming(boot_context);
+		if (result != 0) {
+			ERROR("Error during provisionning\n");
+		} else {
+			NOTICE("Provisioning completed\n");
+		}
+
+		break;
+
+	case SSP_ERROR:
+		/*
+		 * Error during bootrom SSP processing
+		 */
+		result = -EINVAL;
+		ERROR("SSP_Error: Resetting target\n");
+		break;
+
+	case SSP_NONE:
+	default:
+		result = ssp_enable_processing(boot_context);
+		if (result != 0) {
+			ERROR("Start SSP Failed (%i)\n", result);
+		}
+	}
+
+	if ((result != 0) || (ssp_phase == SSP_FLASH_OEM)) {
+		goto out;
+	}
+
+	/*
+	 * Keep VDDCORE && VDD enabled if pmic used to generate
+	 * the required MPSYSRST.
+	 */
+	if (dt_pmic_status() > 0) {
+		const char *name;
+
+		name = stm32mp_get_cpu_supply_name();
+		if (name == NULL) {
+			goto out;
+		}
+
+		if (stpmic1_regulator_mask_reset_set(name) != 0) {
+			WARN("Failed to write %s reset mask\n", name);
+		}
+
+		name = stm32mp_get_vdd_supply_name();
+		if (name == NULL) {
+			goto out;
+		}
+
+		if (stpmic1_regulator_mask_reset_set(name) != 0) {
+			WARN("Failed to write %s reset mask\n", name);
+		}
+	} else {
+		static const char debug_msg[] = {
+			"SSP next step will be only guarantee if the VDD\n"
+			"domain is maintained during system reset\n"
+		};
+
+		NOTICE("%s", debug_msg);
+	}
+
+out:
+#ifndef DCACHE_OFF
+	if (boot_context->p_ssp_config != NULL) {
+		flush_dcache_range((uintptr_t)boot_context->p_ssp_config,
+				   sizeof(boot_api_ssp_config_t));
+	}
+#endif
+
+	stm32mp_system_reset();
+}
+
+#if DEBUG
+static void reset_uart(uint32_t reset)
+{
+	int ret;
+
+	ret = stm32mp_reset_assert(reset, RESET_TIMEOUT_US_1MS);
+	if (ret != 0) {
+		panic();
+	}
+
+	udelay(2);
+
+	ret = stm32mp_reset_deassert(reset, RESET_TIMEOUT_US_1MS);
+	if (ret != 0) {
+		panic();
+	}
+
+	mdelay(1);
+}
+#endif
+
+void bl2_el3_early_platform_setup(u_register_t arg0,
+				  u_register_t arg1 __unused,
+				  u_register_t arg2 __unused,
+				  u_register_t arg3 __unused)
+{
+	stm32mp_save_boot_ctx_address(arg0);
+}
+
+void bl2_el3_plat_arch_setup(void)
+{
+#if DEBUG
+	int32_t result;
+	struct dt_node_info dt_uart_info;
+	const char *board_model;
+	uint32_t clk_rate;
+#endif
+	uintptr_t pwr_base;
+	uintptr_t rcc_base;
+
+	boot_api_context_t *boot_context =
+		(boot_api_context_t *)stm32mp_get_boot_ctx_address();
+	bool serial_uart_interface __unused =
+				(boot_context->boot_interface_selected ==
+				 BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART);
+	uintptr_t uart_prog_addr __unused;
+
+	if (bsec_probe() != 0) {
+		panic();
+	}
+
+	mmap_add_region(BL_CODE_BASE, BL_CODE_BASE,
+			BL_CODE_END - BL_CODE_BASE,
+			MT_CODE | MT_SECURE);
+
+#if SEPARATE_CODE_AND_RODATA
+	mmap_add_region(BL_RO_DATA_BASE, BL_RO_DATA_BASE,
+			BL_RO_DATA_END - BL_RO_DATA_BASE,
+			MT_RO_DATA | MT_SECURE);
+#endif
+
+	/* Prevent corruption of preloaded Device Tree */
+	mmap_add_region(DTB_BASE, DTB_BASE,
+			DTB_LIMIT - DTB_BASE,
+			MT_RO_DATA | MT_SECURE);
+
+	configure_mmu();
+
+	if (dt_open_and_check(STM32MP_DTB_BASE) < 0) {
+		panic();
+	}
+
+	pwr_base = stm32mp_pwr_base();
+	rcc_base = stm32mp_rcc_base();
+
+	/*
+	 * Disable the backup domain write protection.
+	 * The protection is enable at each reset by hardware
+	 * and must be disabled by software.
+	 */
+	mmio_setbits_32(pwr_base + PWR_CR1, PWR_CR1_DBP);
+
+	while ((mmio_read_32(pwr_base + PWR_CR1) & PWR_CR1_DBP) == 0U) {
+		;
+	}
+
+	/* Reset backup domain on cold boot cases */
+	if ((mmio_read_32(rcc_base + RCC_BDCR) & RCC_BDCR_RTCSRC_MASK) == 0U) {
+		mmio_setbits_32(rcc_base + RCC_BDCR, RCC_BDCR_VSWRST);
+
+		while ((mmio_read_32(rcc_base + RCC_BDCR) & RCC_BDCR_VSWRST) ==
+		       0U) {
+			;
+		}
+
+		mmio_clrbits_32(rcc_base + RCC_BDCR, RCC_BDCR_VSWRST);
+	}
+
+
+	generic_delay_timer_init();
+
+#if STM32MP_UART_PROGRAMMER
+	uart_prog_addr = get_uart_address(boot_context->boot_interface_instance);
+
+	/* Disable programmer UART before changing clock tree */
+	if (serial_uart_interface) {
+		stm32_uart_stop(uart_prog_addr);
+	}
+#endif
+
+	if (stm32mp1_clk_probe() < 0) {
+		panic();
+	}
+
+	if (dt_pmic_status() > 0) {
+		initialize_pmic();
+	}
+
+#if DEBUG
+	result = dt_get_stdout_uart_info(&dt_uart_info);
+
+	if ((result <= 0) ||
+	    (dt_uart_info.status == DT_DISABLED) ||
+#if STM32MP_UART_PROGRAMMER
+	    (serial_uart_interface &&
+	     (uart_prog_addr == dt_uart_info.base)) ||
+#endif
+	    (dt_uart_info.clock < 0) ||
+	    (dt_uart_info.reset < 0)) {
+		goto skip_console_init;
+	}
+
+	if (dt_set_stdout_pinctrl() != 0) {
+		goto skip_console_init;
+	}
+
+	if (dt_uart_info.status == DT_DISABLED) {
+		panic();
+	}
+
+	clk_enable((unsigned long)dt_uart_info.clock);
+
+	reset_uart((uint32_t)dt_uart_info.reset);
+
+	clk_rate = clk_get_rate((unsigned long)dt_uart_info.clock);
+
+	if (console_stm32_register(dt_uart_info.base, clk_rate,
+				   STM32MP_UART_BAUDRATE, &console) == 0) {
+		panic();
+	}
+
+	console_set_scope(&console, CONSOLE_FLAG_BOOT |
+			  CONSOLE_FLAG_CRASH | CONSOLE_FLAG_TRANSLATE_CRLF);
+
+	stm32mp_print_cpuinfo();
+
+	board_model = dt_get_board_model();
+	if (board_model != NULL) {
+		NOTICE("Model: %s\n", board_model);
+	}
+
+	if ((boot_context->p_ssp_config == NULL) ||
+	    (boot_context->p_ssp_config->ssp_cmd !=
+	     BOOT_API_CTX_SSP_CMD_PROV_SECRET_ACK)) {
+		stm32mp_print_cpuinfo();
+		if (!stm32mp_is_auth_supported()) {
+			ERROR("Chip doesn't support SSP\n");
+			panic();
+		}
+	}
+
+skip_console_init:
+#endif
+	if (stm32mp_is_closed_device()) {
+		/* Closed chip required authentication */
+		ERROR("SSP not supported on closed chip\n");
+		panic();
+	}
+
+	if (stm32_iwdg_init() < 0) {
+		panic();
+	}
+
+	stm32_iwdg_refresh();
+
+	if (dt_pmic_status() > 0) {
+		initialize_pmic();
+		print_pmic_info_and_debug();
+	}
+
+	ssp_start(boot_context);
+
+	/* This must not be reached */
+	panic();
+}
diff --git a/plat/st/stm32mp1/stm32mp1_ssp.mk b/plat/st/stm32mp1/stm32mp1_ssp.mk
new file mode 100644
index 0000000000..9041e6a032
--- /dev/null
+++ b/plat/st/stm32mp1/stm32mp1_ssp.mk
@@ -0,0 +1,84 @@
+#
+# Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ST_VERSION 		:=	r1.0-ssp
+VERSION_STRING		:=	v${VERSION_MAJOR}.${VERSION_MINOR}-${ST_VERSION}(${BUILD_TYPE}):${BUILD_STRING}
+
+# Required to use BL2_IN_XIP_MEM
+BL2_IN_XIP_MEM 		:= 	1
+
+SEPARATE_CODE_AND_RODATA :=	1
+
+TRUSTED_BOARD_BOOT	:=	0
+
+# Macros and rules to build TF-A binary
+STM32_TF_STM32		:=	$(addprefix ${BUILD_PLAT}/tf-a-ssp-, $(patsubst %.dtb,%.stm32,$(DTB_FILE_NAME)))
+
+PLAT_BL_COMMON_SOURCES	:=	common/fdt_wrappers.c					\
+				plat/st/common/stm32mp_common.c				\
+				plat/st/stm32mp1/stm32mp1_private.c
+
+PLAT_BL_COMMON_SOURCES	+=	${XLAT_TABLES_LIB_SRCS}
+
+PLAT_BL_COMMON_SOURCES	+=	lib/cpus/aarch32/cortex_a7.S
+
+PLAT_BL_COMMON_SOURCES	+=	drivers/st/uart/aarch32/stm32_console.S
+
+PLAT_BL_COMMON_SOURCES	+=	drivers/arm/tzc/tzc400.c				\
+				drivers/clk/clk.c					\
+				drivers/delay_timer/delay_timer.c			\
+				drivers/delay_timer/generic_delay_timer.c		\
+				drivers/st/bsec/bsec2.c					\
+				drivers/st/clk/stm32mp_clkfunc.c			\
+				drivers/st/gpio/stm32_gpio.c				\
+				drivers/st/i2c/stm32_i2c.c				\
+				drivers/st/iwdg/stm32_iwdg.c				\
+				drivers/st/pmic/stm32mp_pmic.c				\
+				drivers/st/pmic/stpmic1.c				\
+				drivers/st/regulator/stm32mp_dummy_regulator.c		\
+				drivers/st/regulator/stm32mp_regulator.c		\
+				drivers/st/reset/stm32mp1_reset.c			\
+				plat/st/common/stm32mp_dt.c				\
+				plat/st/common/stm32mp_shres_helpers.c			\
+				plat/st/stm32mp1/stm32mp1_dbgmcu.c			\
+				plat/st/stm32mp1/stm32mp1_helper.S			\
+				plat/st/stm32mp1/stm32mp1_syscfg.c
+
+PLAT_BL_COMMON_SOURCES	+=	drivers/st/clk/stm32mp1_clk.c
+
+BL2_SOURCES		:=	drivers/io/io_storage.c					\
+				drivers/st/crypto/stm32_hash.c				\
+				plat/st/stm32mp1/stm32mp1_ssp.c
+
+ifeq (${STM32MP_UART_PROGRAMMER},1)
+BL2_SOURCES		+=	drivers/st/uart/stm32_uart.c				\
+				plat/st/common/stm32cubeprogrammer_uart.c
+endif
+
+ifeq (${STM32MP_USB_PROGRAMMER},1)
+BL2_SOURCES		+=	drivers/st/usb_dwc2/usb_dwc2.c				\
+				lib/usb/usb_core.c					\
+				lib/usb/usb_st_dfu.c					\
+				plat/st/common/stm32cubeprogrammer_usb.c		\
+				plat/st/stm32mp1/stm32mp1_usb.c
+endif
+
+BL2_DTSI		:=	stm32mp15-ssp-bl2.dtsi
+
+check_boot_ssp:
+	@if ([ ${STM32MP_UART_PROGRAMMER} = 1 ] && [ ${STM32MP_USB_PROGRAMMER} = 1 ]) || \
+	([ ${STM32MP_UART_PROGRAMMER} = 0 ] && [ ${STM32MP_USB_PROGRAMMER} = 0 ]); then \
+		echo "Error selecting serial boot device"; \
+		false; \
+	fi
+
+bl2: check_boot_ssp
+
+${BUILD_PLAT}/stm32mp1-ssp-%.o: ${BUILD_PLAT}/fdts/%-bl2.dtb plat/st/stm32mp1/stm32mp1.S bl2
+	@echo "  SSP AS      stm32mp1.S"
+	${Q}${AS} ${ASFLAGS} ${TF_CFLAGS} \
+		-DDTB_BIN_PATH=\"$<\" \
+		-c plat/st/stm32mp1/stm32mp1.S -o $@
diff --git a/plat/st/stm32mp1/stm32mp1_syscfg.c b/plat/st/stm32mp1/stm32mp1_syscfg.c
index 109725c8ac..2c5fa082fc 100644
--- a/plat/st/stm32mp1/stm32mp1_syscfg.c
+++ b/plat/st/stm32mp1/stm32mp1_syscfg.c
@@ -7,21 +7,24 @@
 #include <platform_def.h>
 
 #include <common/debug.h>
-#include <drivers/st/bsec.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32mp1_clk.h>
 #include <drivers/st/stpmic1.h>
 #include <lib/mmio.h>
 
+#include <stm32mp_common.h>
 #include <stm32mp_dt.h>
 #include <stm32mp1_private.h>
 
 /*
- * SYSCFG REGISTER OFFSET (base relative)
+ * SYSCFG register offsets (base relative)
  */
 #define SYSCFG_BOOTR				0x00U
 #define SYSCFG_IOCTRLSETR			0x18U
 #define SYSCFG_ICNR				0x1CU
 #define SYSCFG_CMPCR				0x20U
 #define SYSCFG_CMPENSETR			0x24U
+#define SYSCFG_CMPENCLRR			0x28U
 
 /*
  * SYSCFG_BOOTR Register
@@ -53,6 +56,8 @@
 #define SYSCFG_CMPCR_RAPSRC			GENMASK(23, 20)
 #define SYSCFG_CMPCR_ANSRC_SHIFT		24
 
+#define SYSCFG_CMPCR_READY_TIMEOUT_US		10000U
+
 /*
  * SYSCFG_CMPENSETR Register
  */
@@ -61,8 +66,9 @@
 void stm32mp1_syscfg_init(void)
 {
 	uint32_t bootr;
-	uint32_t otp = 0;
+	uint32_t otp_value;
 	uint32_t vdd_voltage;
+	bool product_below_2v5;
 
 	/*
 	 * Interconnect update : select master using the port 1.
@@ -91,18 +97,18 @@ void stm32mp1_syscfg_init(void)
 	 *   => TF-A enables the low power mode only if VDD < 2.7V (in DT)
 	 *      but this value needs to be consistent with board design.
 	 */
-	if (bsec_read_otp(&otp, HW2_OTP) != BSEC_OK) {
+	if (stm32_get_otp_value(HW2_OTP, &otp_value) != 0) {
 		panic();
 	}
 
-	otp = otp & HW2_OTP_PRODUCT_BELOW_2V5;
+	product_below_2v5 = (otp_value & HW2_OTP_PRODUCT_BELOW_2V5) != 0U;
 
 	/* Get VDD supply */
 	vdd_voltage = dt_get_pwr_vdd_voltage();
 
 	/* Check if VDD is Low Voltage */
 	if (vdd_voltage == 0U) {
-		WARN("VDD unknown");
+		WARN("VDD unknown\n");
 	} else if (vdd_voltage < 2700000U) {
 		mmio_write_32(SYSCFG_BASE + SYSCFG_IOCTRLSETR,
 			      SYSCFG_IOCTRLSETR_HSLVEN_TRACE |
@@ -111,11 +117,11 @@ void stm32mp1_syscfg_init(void)
 			      SYSCFG_IOCTRLSETR_HSLVEN_SDMMC |
 			      SYSCFG_IOCTRLSETR_HSLVEN_SPI);
 
-		if (otp == 0U) {
+		if (!product_below_2v5) {
 			INFO("Product_below_2v5=0: HSLVEN protected by HW\n");
 		}
 	} else {
-		if (otp != 0U) {
+		if (product_below_2v5) {
 			ERROR("Product_below_2v5=1:\n");
 			ERROR("\tHSLVEN update is destructive,\n");
 			ERROR("\tno update as VDD > 2.7V\n");
@@ -123,24 +129,38 @@ void stm32mp1_syscfg_init(void)
 		}
 	}
 
-	stm32mp1_syscfg_enable_io_compensation();
+	stm32mp1_syscfg_enable_io_compensation_start();
 }
 
-void stm32mp1_syscfg_enable_io_compensation(void)
+void stm32mp1_syscfg_enable_io_compensation_start(void)
 {
 	/*
 	 * Activate automatic I/O compensation.
 	 * Warning: need to ensure CSI enabled and ready in clock driver.
 	 * Enable non-secure clock, we assume non-secure is suspended.
 	 */
-	stm32mp1_clk_enable_non_secure(SYSCFG);
+	stm32mp1_clk_force_enable(SYSCFG);
 
 	mmio_setbits_32(SYSCFG_BASE + SYSCFG_CMPENSETR,
 			SYSCFG_CMPENSETR_MPU_EN);
+}
+
+void stm32mp1_syscfg_enable_io_compensation_finish(void)
+{
+	uint64_t start;
+
+	start = timeout_init_us(SYSCFG_CMPCR_READY_TIMEOUT_US);
 
 	while ((mmio_read_32(SYSCFG_BASE + SYSCFG_CMPCR) &
 		SYSCFG_CMPCR_READY) == 0U) {
-		;
+		if (timeout_elapsed(start)) {
+			/*
+			 * Failure on IO compensation enable is not a issue:
+			 * warn only.
+			 */
+			WARN("IO compensation cell not ready\n");
+			break;
+		}
 	}
 
 	mmio_clrbits_32(SYSCFG_BASE + SYSCFG_CMPCR, SYSCFG_CMPCR_SW_CTRL);
@@ -150,6 +170,8 @@ void stm32mp1_syscfg_disable_io_compensation(void)
 {
 	uint32_t value;
 
+	stm32mp1_clk_force_enable(SYSCFG);
+
 	/*
 	 * Deactivate automatic I/O compensation.
 	 * Warning: CSI is disabled automatically in STOP if not
@@ -167,8 +189,7 @@ void stm32mp1_syscfg_disable_io_compensation(void)
 
 	mmio_write_32(SYSCFG_BASE + SYSCFG_CMPCR, value | SYSCFG_CMPCR_SW_CTRL);
 
-	mmio_clrbits_32(SYSCFG_BASE + SYSCFG_CMPENSETR,
-			SYSCFG_CMPENSETR_MPU_EN);
+	mmio_setbits_32(SYSCFG_BASE + SYSCFG_CMPENCLRR, SYSCFG_CMPENSETR_MPU_EN);
 
-	stm32mp1_clk_disable_non_secure(SYSCFG);
+	stm32mp1_clk_force_disable(SYSCFG);
 }
diff --git a/plat/st/stm32mp1/stm32mp1_usb.c b/plat/st/stm32mp1/stm32mp1_usb.c
new file mode 100644
index 0000000000..c63db4a2ff
--- /dev/null
+++ b/plat/st/stm32mp1/stm32mp1_usb.c
@@ -0,0 +1,491 @@
+/*
+ * Copyright (c) 2020, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <limits.h>
+#include <string.h>
+
+#include <platform_def.h>
+
+#include <common/debug.h>
+#include <drivers/st/bsec.h>
+#include <drivers/st/usb_dwc2.h>
+#include <lib/usb/usb_core.h>
+#include <lib/usb/usb_st_dfu.h>
+
+#include <stm32cubeprogrammer.h>
+#include <stm32mp_common.h>
+
+/*  String size (1 byte) + type (1 byte) + 24 UTF16 characters */
+/*  (2 bytes per character) */
+#define USB_SIZ_STRING_SERIAL		(1 + 1 + (24 * 2))
+#define USBD_MAX_STR_DESC_SIZ		0x100
+#define USBD_VID			0x0483
+#define USBD_PID			0xDF11
+#define USBD_LANGID_STRING		0x409
+#define USBD_MANUFACTURER_STRING	"STMicroelectronics"
+#define USBD_CONFIGURATION_STRING	"DFU Config"
+#define USBD_INTERFACE_STRING		"DFU Interface"
+
+#define USB_DFU_ITF_NUM			6
+
+#define USB_DFU_CONFIG_DESC_SIZ		USB_DFU_DESC_SIZ(USB_DFU_ITF_NUM)
+
+/* DFU devices */
+static usb_dfu_handle_t usb_dfu_handle;
+
+/* USB Standard Device Descriptor */
+static const uint8_t usb_stm32mp1_desc[USB_LEN_DEV_DESC] = {
+	USB_LEN_DEV_DESC,           /* bLength */
+	USB_DESC_TYPE_DEVICE,       /* bDescriptorType */
+	0x00,                       /* bcdUSB */
+	0x02,                       /* version */
+	0x00,                       /* bDeviceClass */
+	0x00,                       /* bDeviceSubClass */
+	0x00,                       /* bDeviceProtocol */
+	USB_MAX_EP0_SIZE,           /* bMaxPacketSize */
+	LOBYTE(USBD_VID),           /* idVendor */
+	HIBYTE(USBD_VID),           /* idVendor */
+	LOBYTE(USBD_PID),           /* idVendor */
+	HIBYTE(USBD_PID),           /* idVendor */
+	0x00,                       /* bcdDevice rel. 2.00 */
+	0x02,
+	USBD_IDX_MFC_STR,           /* Index of manufacturer string */
+	USBD_IDX_PRODUCT_STR,       /* Index of product string */
+	USBD_IDX_SERIAL_STR,        /* Index of serial number string */
+	USBD_MAX_NUM_CONFIGURATION  /* bNumConfigurations */
+}; /* USB_DeviceDescriptor */
+
+/* USB Standard String Descriptor */
+static const uint8_t usb_stm32mp1_lang_id_desc[USB_LEN_LANGID_STR_DESC] = {
+	USB_LEN_LANGID_STR_DESC,
+	USB_DESC_TYPE_STRING,
+	LOBYTE(USBD_LANGID_STRING),
+	HIBYTE(USBD_LANGID_STRING),
+};
+
+/* USB Standard Device Descriptor */
+static const uint8_t
+usbd_stm32mp1_qualifier_desc[USB_LEN_DEV_QUALIFIER_DESC] = {
+	USB_LEN_DEV_QUALIFIER_DESC,
+	USB_DESC_TYPE_DEVICE_QUALIFIER,
+	0x00,
+	0x02,
+	0x00,
+	0x00,
+	0x00,
+	0x40,
+	0x01,
+	0x00,
+};
+
+static uint8_t usb_stm32mp1_serial[USB_SIZ_STRING_SERIAL + 1] = {
+	USB_SIZ_STRING_SERIAL,
+	USB_DESC_TYPE_STRING,
+};
+
+/* USB DFU device Configuration Descriptor */
+static uint8_t usb_stm32mp1_config_desc[USB_DFU_CONFIG_DESC_SIZ] = {
+	0x09, /* bLength: Configuration Descriptor size */
+	USB_DESC_TYPE_CONFIGURATION, /* bDescriptorType: Configuration */
+	USB_DFU_CONFIG_DESC_SIZ, /* wTotalLength: Bytes returned */
+	0x00,
+	0x01, /* bNumInterfaces: 1 interface */
+	0x01, /* bConfigurationValue: Configuration value */
+	0x02, /* iConfiguration: Index of string descriptor
+	       * describing the configuration
+	       */
+	0xC0, /* bmAttributes: bus powered and Supprts Remote Wakeup */
+	0x32, /* MaxPower 100 mA: this current is used for detecting Vbus */
+	/* 09 */
+
+	/* Descriptor of DFU interface 0 Alternate setting 0..N */
+	USBD_DFU_IF_DESC(0),
+	USBD_DFU_IF_DESC(1),
+	USBD_DFU_IF_DESC(2),
+#if USB_DFU_ITF_NUM > 3
+	USBD_DFU_IF_DESC(3),
+	USBD_DFU_IF_DESC(4),
+	USBD_DFU_IF_DESC(5),
+#endif
+	/* DFU Functional Descriptor */
+	0x09, /* blength = 9 Bytes */
+	DFU_DESCRIPTOR_TYPE, /* DFU Functional Descriptor */
+	DFU_BM_ATTRIBUTE, /* bmAttribute
+			   * bitCanDnload		= 1 (bit 0)
+			   * bitCanUpload		= 1 (bit 1)
+			   * bitManifestationTolerant	= 1 (bit 2)
+			   * bitWillDetach		= 1 (bit 3)
+			   * Reserved			(bit4-6)
+			   * bitAcceleratedST		= 0 (bit 7)
+			   */
+	0xFF, /* DetachTimeOut = 255 ms */
+	0x00,
+	/* WARNING: In DMA mode the multiple MPS packets feature
+	 *  is still not supported ==> In this case,
+	 *  when using DMA USBD_DFU_XFER_SIZE should be set
+	 *  to 64 in usbd_conf.h
+	 */
+	TRANSFER_SIZE_BYTES(USBD_DFU_XFER_SIZE), /* TransferSize = 1024 Byte */
+	((USB_DFU_VERSION >> 0) & 0xFF), /* bcdDFUVersion */
+	((USB_DFU_VERSION >> 8) & 0xFF)
+};
+
+static uint8_t usb_local_string_dec[USBD_MAX_STR_DESC_SIZ];
+
+/*
+ * Convert Hex 32Bits value into char
+ * value: value to convert
+ * pbuf: pointer to the buffer
+ * len: buffer length
+ */
+static void int_to_unicode(uint32_t value, uint8_t *pbuf, uint8_t len)
+{
+	uint8_t idx;
+
+	for (idx = 0U; idx < len; idx++) {
+		if (((value >> 28)) < 0xA) {
+			pbuf[2U * idx] = (value >> 28) + '0';
+		} else {
+			pbuf[2U * idx] = (value >> 28) + 'A' - 10U;
+		}
+		value = value << 4;
+		pbuf[(2U * idx) + 1U] = 0U;
+	}
+}
+
+/*
+ * Convert Hex 32Bits value into string with a fixed length (as sprintf %0<n>X)
+ * value: value to convert
+ * pstr: pointer to the string
+ * len: buffer length
+ */
+static void int_to_str(uint32_t value, uint8_t *pstr, uint8_t len)
+{
+	uint8_t idx, v;
+
+	if (len > 9U) {
+		len = 9U;
+	}
+
+	for (idx = 0U; idx < len - 1U; idx++) {
+		v = (value >> (4U * (len - 2U - idx))) & 0xFU;
+		if (v < 0xAU) {
+			pstr[idx] = '0' + v;
+		} else {
+			pstr[idx] = 'A' + v - 0xAU;
+		}
+	}
+	pstr[len - 1] = 0U;
+}
+
+/*
+ * Create the serial number string descriptor
+ */
+static void update_serial_num_string(void)
+{
+	uint8_t i;
+	/* serial number is set to 0 */
+	uint32_t deviceserial[UID_WORD_NB] = {0U, 0U, 0U};
+	uint32_t otp;
+	uint32_t len;
+
+	if (stm32_get_otp_index(UID_OTP, &otp, &len) != 0) {
+		ERROR("BSEC: Get UID_OTP number Error\n");
+		return;
+	}
+
+	if ((len / __WORD_BIT) != UID_WORD_NB) {
+		ERROR("BSEC: Get UID_OTP length Error\n");
+		return;
+	}
+
+	for (i = 0; i < UID_WORD_NB; i++) {
+		if (bsec_shadow_read_otp(&deviceserial[i], i + otp) !=
+		    BSEC_OK) {
+			ERROR("BSEC: UID%d Error\n", i);
+			return;
+		}
+	}
+
+	int_to_unicode(deviceserial[0], (uint8_t *)&usb_stm32mp1_serial[2], 8);
+	int_to_unicode(deviceserial[1], (uint8_t *)&usb_stm32mp1_serial[18], 8);
+	int_to_unicode(deviceserial[2], (uint8_t *)&usb_stm32mp1_serial[34], 8);
+}
+
+/*
+ * usb_get_qualifier_desc
+ *         return Device Qualifier descriptor
+ * param :  length : pointer data length
+ * return : pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_get_qualifier_desc(uint16_t *length)
+{
+	*length = sizeof(usbd_stm32mp1_qualifier_desc);
+
+	return (uint8_t *)usbd_stm32mp1_qualifier_desc;
+}
+
+/*
+ * stm32mp1_get_config_desc
+ *         return configuration descriptor
+ * param : speed : current device speed
+ * param : length : pointer data length
+ * return : pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_get_config_desc(uint16_t *length)
+{
+	*length = sizeof(usb_stm32mp1_config_desc);
+
+	return (uint8_t *)usb_stm32mp1_config_desc;
+}
+
+/*
+ * stm32mp1_get_string
+ *         Convert Ascii string into unicode one
+ * param : desc : descriptor buffer
+ * param : unicode : Formatted string buffer (unicode)
+ * param : len : descriptor length
+ * return : None
+ */
+static void stm32mp1_get_string(uint8_t *desc, uint8_t *unicode, uint16_t *len)
+{
+	uint8_t idx = 0;
+
+	if (desc == NULL) {
+		return;
+	}
+
+	*len =  strlen((char *)desc) * 2 + 2;
+	unicode[idx++] = *len;
+	unicode[idx++] =  USB_DESC_TYPE_STRING;
+
+	while (*desc != '\0') {
+		unicode[idx++] = *desc++;
+		unicode[idx++] =  0x00;
+	}
+}
+
+/*
+ * stm32mp1_device_desc
+ * Returns the device descriptor.
+ * length: Pointer to data length variable
+ * return : Pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_device_desc(uint16_t *length)
+{
+	*length = sizeof(usb_stm32mp1_desc);
+
+	return (uint8_t *)usb_stm32mp1_desc;
+}
+
+/*
+ * stm32mp1_lang_id_desc
+ * Returns the LangID string descriptor.
+ * speed: Current device speed
+ * length: Pointer to data length variable
+ * return : Pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_lang_id_desc(uint16_t *length)
+{
+	*length = sizeof(usb_stm32mp1_lang_id_desc);
+
+	return (uint8_t *)usb_stm32mp1_lang_id_desc;
+}
+
+/*
+ * stm32mp1_product_desc
+ * Returns the product string descriptor.
+ * length: Pointer to data length variable
+ * return : Pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_product_desc(uint16_t *length)
+{
+	char name[STM32_SOC_NAME_SIZE];
+	char product[128];
+	uint32_t chip_version;
+	char str_chip_id[4];
+	char str_chip_version[5];
+
+	stm32mp_get_soc_name(name);
+	stm32mp_get_chip_version(&chip_version);
+
+	int_to_str(STM32MP1_CHIP_ID, (uint8_t *)str_chip_id, 4);
+	int_to_str(chip_version, (uint8_t *)str_chip_version, 5);
+	snprintf(product, sizeof(product),
+		 "DFU @Device ID /0x%s, @Revision ID /0x%s, @Name /%s,",
+		 str_chip_id, str_chip_version, name);
+
+	stm32mp1_get_string((uint8_t *)product, usb_local_string_dec, length);
+
+	return usb_local_string_dec;
+}
+
+/*
+ * stm32mp1_manufacturer_desc
+ * Returns the manufacturer string descriptor.
+ * length: Pointer to data length variable
+ * return : Pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_manufacturer_desc(uint16_t *length)
+{
+	stm32mp1_get_string((uint8_t *)USBD_MANUFACTURER_STRING,
+			    usb_local_string_dec, length);
+
+	return usb_local_string_dec;
+}
+
+/*
+ * stm32mp1_serial_desc
+ * Returns the serial number string descriptor.
+ * length: Pointer to data length variable
+ * return : Pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_serial_desc(uint16_t *length)
+{
+	*length = USB_SIZ_STRING_SERIAL;
+
+	/* Update the serial number string descriptor
+	 * with the data from the unique ID
+	 */
+	update_serial_num_string();
+
+	return (uint8_t *)usb_stm32mp1_serial;
+}
+
+/*
+ * stm32mp1_Config_desc
+ * Returns the configuration string descriptor.
+ * length: Pointer to data length variable
+ * return : Pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_config_desc(uint16_t *length)
+{
+	stm32mp1_get_string((uint8_t *)USBD_CONFIGURATION_STRING,
+			    usb_local_string_dec, length);
+
+	return usb_local_string_dec;
+}
+
+/*
+ * stm32mp1_interface_desc
+ * Returns the interface string descriptor.
+ * length : Pointer to data length variable
+ * return : Pointer to descriptor buffer
+ */
+static uint8_t *stm32mp1_interface_desc(uint16_t *length)
+{
+	stm32mp1_get_string((uint8_t *)USBD_INTERFACE_STRING,
+			    usb_local_string_dec, length);
+
+	return usb_local_string_dec;
+}
+
+/*
+ * stm32mp1_get_usr_desc
+ *         Manages the transfer of memory interfaces string descriptors.
+ * param : index: descriptor index
+ * param : length : pointer data length
+ * return : pointer to the descriptor table or NULL if the descriptor
+ *          is not supported.
+ */
+static uint8_t *stm32mp1_get_usr_desc(uint8_t index, uint16_t *length)
+{
+	uint8_t *ret;
+
+	switch (index) {
+	case 0:
+		stm32mp1_get_string((uint8_t *)"@Partition0 /0x00/1*256Ke",
+				    usb_local_string_dec, length);
+		ret = usb_local_string_dec;
+		break;
+	case 1:
+		stm32mp1_get_string((uint8_t *)"@FSBL /0x01/1*1Me",
+				    usb_local_string_dec, length);
+		ret = usb_local_string_dec;
+		break;
+	case 2:
+		stm32mp1_get_string((uint8_t *)"@Partition2 /0x02/1*1Me",
+				    usb_local_string_dec, length);
+		ret = usb_local_string_dec;
+		break;
+	case 3:
+		stm32mp1_get_string((uint8_t *)"@Partition3 /0x03/1*16Me",
+				    usb_local_string_dec, length);
+		ret = usb_local_string_dec;
+		break;
+	case 4:
+		stm32mp1_get_string((uint8_t *)"@Partition4 /0x04/1*16Me",
+				    usb_local_string_dec, length);
+		ret = usb_local_string_dec;
+		break;
+	case 5:
+		stm32mp1_get_string((uint8_t *)"@virtual /0xF1/1*512Ba",
+				    usb_local_string_dec, length);
+		ret = usb_local_string_dec;
+		break;
+	default:
+		ret = NULL;
+		break;
+	}
+
+	return ret;
+}
+
+static const usb_desc_t dfu_desc = {
+	.get_device_desc = stm32mp1_device_desc,
+	.get_lang_id_desc = stm32mp1_lang_id_desc,
+	.get_manufacturer_desc = stm32mp1_manufacturer_desc,
+	.get_product_desc = stm32mp1_product_desc,
+	.get_configuration_desc = stm32mp1_config_desc,
+	.get_serial_desc = stm32mp1_serial_desc,
+	.get_interface_desc = stm32mp1_interface_desc,
+	.get_usr_desc = stm32mp1_get_usr_desc,
+	.get_config_desc = stm32mp1_get_config_desc,
+	.get_device_qualifier_desc = stm32mp1_get_qualifier_desc,
+};
+
+static usb_handle_t usb_core_handle;
+static pcd_handle_t pcd_handle;
+
+usb_handle_t *usb_dfu_plat_init(void)
+{
+	/* prepare USB Driver */
+	pcd_handle.in_ep[0].maxpacket = USB_MAX_EP0_SIZE;
+	pcd_handle.out_ep[0].maxpacket = USB_MAX_EP0_SIZE;
+	usb_dwc2_init_driver(&usb_core_handle, &pcd_handle,
+			     (uint32_t *)USB_OTG_BASE);
+
+	/* STM32MP15 = keep the configuration from ROM code */
+	usb_core_handle.ep0_state = USBD_EP0_DATA_IN;
+	usb_core_handle.dev_state = USBD_STATE_CONFIGURED;
+
+	/* prepare USB DFU stack */
+	usb_dfu_register(&usb_core_handle, &usb_dfu_handle);
+
+	/* register descriptor in USB stack */
+	register_platform(&usb_core_handle, &dfu_desc);
+
+	return &usb_core_handle;
+}
+
+/* Link between USB alternate and STM32CubeProgramer phase */
+uint8_t usb_dfu_get_phase(uint8_t alt)
+{
+	switch (alt) {
+	case 0:
+#if STM32MP_SSP
+	return PHASE_SSP;
+#else
+	return PHASE_FLASHLAYOUT;
+#endif
+	case 3:
+		return PHASE_SSBL;
+	case 5:
+		return PHASE_CMD;
+	default:
+		return PHASE_RESET;
+	}
+}
diff --git a/tools/cert_create/Makefile b/tools/cert_create/Makefile
index 0ec08b0540..5ef8faf522 100644
--- a/tools/cert_create/Makefile
+++ b/tools/cert_create/Makefile
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -9,7 +9,7 @@ V		?= 0
 DEBUG		:= 0
 CRTTOOL		?= cert_create${BIN_EXT}
 BINARY		:= $(notdir ${CRTTOOL})
-OPENSSL_DIR	:= /usr
+OPENSSL_DIR	?= /usr
 COT		:= tbbr
 
 MAKE_HELPERS_DIRECTORY := ../../make_helpers/
@@ -53,13 +53,13 @@ HOSTCCFLAGS += ${DEFINES}
 # could get pulled in from firmware tree.
 INC_DIR := -I ./include -I ${PLAT_INCLUDE} -I ${OPENSSL_DIR}/include
 LIB_DIR := -L ${OPENSSL_DIR}/lib
-LIB := -lssl -lcrypto
+LIB := -lssl -lcrypto -lpthread
 
 HOSTCC ?= gcc
 
 .PHONY: all clean realclean
 
-all: clean ${BINARY}
+all: ${BINARY}
 
 ${BINARY}: ${OBJECTS} Makefile
 	@echo "  HOSTLD  $@"
diff --git a/tools/cert_create/include/key.h b/tools/cert_create/include/key.h
index d96d9839a2..3409502d82 100644
--- a/tools/cert_create/include/key.h
+++ b/tools/cert_create/include/key.h
@@ -22,7 +22,8 @@ enum {
 enum {
 	KEY_ALG_RSA,		/* RSA PSS as defined by PKCS#1 v2.1 (default) */
 #ifndef OPENSSL_NO_EC
-	KEY_ALG_ECDSA,
+	KEY_ALG_ECDSA_NIST,
+	KEY_ALG_ECDSA_BRAINPOOL,
 #endif /* OPENSSL_NO_EC */
 	KEY_ALG_MAX_NUM
 };
@@ -42,7 +43,8 @@ enum{
 static const unsigned int KEY_SIZES[KEY_ALG_MAX_NUM][KEY_SIZE_MAX_NUM] = {
 	{ 2048, 1024, 3072, 4096 },	/* KEY_ALG_RSA */
 #ifndef OPENSSL_NO_EC
-	{}				/* KEY_ALG_ECDSA */
+	{},				/* KEY_ALG_ECDSA_NIST */
+	{}				/* KEY_ALG_ECDSA_BRAINPOOL */
 #endif /* OPENSSL_NO_EC */
 };
 
diff --git a/tools/cert_create/src/key.c b/tools/cert_create/src/key.c
index fcc9d53162..25d7d4bd9b 100644
--- a/tools/cert_create/src/key.c
+++ b/tools/cert_create/src/key.c
@@ -76,11 +76,11 @@ err:
 }
 
 #ifndef OPENSSL_NO_EC
-static int key_create_ecdsa(key_t *key, int key_bits)
+static int key_create_ecdsa(key_t *key, int key_bits, int curve_id)
 {
 	EC_KEY *ec;
 
-	ec = EC_KEY_new_by_curve_name(NID_X9_62_prime256v1);
+	ec = EC_KEY_new_by_curve_name(curve_id);
 	if (ec == NULL) {
 		printf("Cannot create EC key\n");
 		goto err;
@@ -101,13 +101,25 @@ err:
 	EC_KEY_free(ec);
 	return 0;
 }
+
+static int key_create_ecdsa_nist(key_t *key, int key_bits)
+{
+	return key_create_ecdsa(key, key_bits, NID_X9_62_prime256v1);
+}
+
+static int key_create_ecdsa_brainpool(key_t *key, int key_bits)
+{
+	return key_create_ecdsa(key, key_bits, NID_brainpoolP256t1);
+}
+
 #endif /* OPENSSL_NO_EC */
 
 typedef int (*key_create_fn_t)(key_t *key, int key_bits);
 static const key_create_fn_t key_create_fn[KEY_ALG_MAX_NUM] = {
-	key_create_rsa, 	/* KEY_ALG_RSA */
+	[KEY_ALG_RSA] = key_create_rsa,
 #ifndef OPENSSL_NO_EC
-	key_create_ecdsa, 	/* KEY_ALG_ECDSA */
+	[KEY_ALG_ECDSA_NIST] = key_create_ecdsa_nist,
+	[KEY_ALG_ECDSA_BRAINPOOL] = key_create_ecdsa_brainpool,
 #endif /* OPENSSL_NO_EC */
 };
 
diff --git a/tools/cert_create/src/main.c b/tools/cert_create/src/main.c
index 2ba110132a..8a1e02e62f 100644
--- a/tools/cert_create/src/main.c
+++ b/tools/cert_create/src/main.c
@@ -84,7 +84,8 @@ static char *strdup(const char *str)
 static const char *key_algs_str[] = {
 	[KEY_ALG_RSA] = "rsa",
 #ifndef OPENSSL_NO_EC
-	[KEY_ALG_ECDSA] = "ecdsa"
+	[KEY_ALG_ECDSA_NIST] = "ecdsa",
+	[KEY_ALG_ECDSA_BRAINPOOL] = "ecdsa-brainpool"
 #endif /* OPENSSL_NO_EC */
 };
 
diff --git a/tools/encrypt_fw/Makefile b/tools/encrypt_fw/Makefile
index 6eb6fae7a8..7f959abeb3 100644
--- a/tools/encrypt_fw/Makefile
+++ b/tools/encrypt_fw/Makefile
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2019-2020, Linaro Limited. All rights reserved.
+# Copyright (c) 2019-2021, Linaro Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -9,7 +9,7 @@ BUILD_INFO	?= 1
 DEBUG		:= 0
 ENCTOOL		?= encrypt_fw${BIN_EXT}
 BINARY		:= $(notdir ${ENCTOOL})
-OPENSSL_DIR	:= /usr
+OPENSSL_DIR	?= /usr
 
 OBJECTS := src/encrypt.o \
            src/cmd_opt.o \
@@ -40,13 +40,13 @@ endif
 # could get pulled in from firmware tree.
 INC_DIR := -I ./include -I ../../include/tools_share -I ${OPENSSL_DIR}/include
 LIB_DIR := -L ${OPENSSL_DIR}/lib
-LIB := -lssl -lcrypto
+LIB := -lssl -lcrypto -lpthread
 
 HOSTCC ?= gcc
 
 .PHONY: all clean realclean
 
-all: clean ${BINARY}
+all: ${BINARY}
 
 ${BINARY}: ${OBJECTS} Makefile
 	@echo "  HOSTLD  $@"
diff --git a/tools/fiptool/Makefile b/tools/fiptool/Makefile
index df8ab5c7be..0ef5c42707 100644
--- a/tools/fiptool/Makefile
+++ b/tools/fiptool/Makefile
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2014-2021, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -8,6 +8,7 @@ MAKE_HELPERS_DIRECTORY := ../../make_helpers/
 include ${MAKE_HELPERS_DIRECTORY}build_macros.mk
 include ${MAKE_HELPERS_DIRECTORY}build_env.mk
 
+OPENSSL_DIR ?= /usr
 FIPTOOL ?= fiptool${BIN_EXT}
 PROJECT := $(notdir ${FIPTOOL})
 OBJECTS := fiptool.o tbbr_config.o
@@ -20,7 +21,8 @@ ifeq (${DEBUG},1)
 else
   HOSTCCFLAGS += -O2
 endif
-LDLIBS := -lcrypto
+LIB_DIR := -L ${OPENSSL_DIR}/lib
+LDLIBS := -lcrypto -lpthread
 
 ifeq (${V},0)
   Q := @
@@ -38,7 +40,7 @@ all: ${PROJECT}
 
 ${PROJECT}: ${OBJECTS} Makefile
 	@echo "  HOSTLD  $@"
-	${Q}${HOSTCC} ${OBJECTS} -o $@ ${LDLIBS}
+	${Q}${HOSTCC} ${OBJECTS} -o $@ ${LIB_DIR} ${LDLIBS}
 	@${ECHO_BLANK_LINE}
 	@echo "Built $@ successfully"
 	@${ECHO_BLANK_LINE}
diff --git a/tools/stm32image/stm32image.c b/tools/stm32image/stm32image.c
index 41024e2866..209e0c9d80 100644
--- a/tools/stm32image/stm32image.c
+++ b/tools/stm32image/stm32image.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2018, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -45,8 +45,6 @@ struct stm32_header {
 	uint8_t binary_type;
 };
 
-static struct stm32_header stm32image_header;
-
 static void stm32image_default_header(struct stm32_header *ptr)
 {
 	if (!ptr) {
@@ -54,10 +52,9 @@ static void stm32image_default_header(struct stm32_header *ptr)
 	}
 
 	ptr->magic_number = HEADER_MAGIC;
-	ptr->header_version[VER_MAJOR] = HEADER_VERSION_V1;
 	ptr->option_flags = HEADER_DEFAULT_OPTION;
-	ptr->ecdsa_algorithm = 1;
-	ptr->version_number = 0;
+	ptr->ecdsa_algorithm = __cpu_to_le32(1);
+	ptr->version_number = __cpu_to_le32(0);
 	ptr->binary_type = TF_BINARY_TYPE;
 }
 
@@ -105,27 +102,33 @@ static void stm32image_print_header(const void *ptr)
 }
 
 static void stm32image_set_header(void *ptr, struct stat *sbuf, int ifd,
-				  uint32_t loadaddr, uint32_t ep, uint32_t ver)
+				  uint32_t loadaddr, uint32_t ep, uint32_t ver,
+				  uint32_t major, uint32_t minor)
 {
 	struct stm32_header *stm32hdr = (struct stm32_header *)ptr;
 
 	stm32image_default_header(stm32hdr);
 
+	stm32hdr->header_version[VER_MAJOR] = major;
+	stm32hdr->header_version[VER_MINOR] = minor;
 	stm32hdr->load_address = __cpu_to_le32(loadaddr);
 	stm32hdr->image_entry_point = __cpu_to_le32(ep);
 	stm32hdr->image_length = __cpu_to_le32((uint32_t)sbuf->st_size -
 					     sizeof(struct stm32_header));
-	stm32hdr->image_checksum = stm32image_checksum(ptr, sbuf->st_size);
+	stm32hdr->image_checksum =
+		__cpu_to_le32(stm32image_checksum(ptr, sbuf->st_size));
 	stm32hdr->version_number = __cpu_to_le32(ver);
 }
 
 static int stm32image_create_header_file(char *srcname, char *destname,
 					 uint32_t loadaddr, uint32_t entry,
-					 uint32_t version)
+					 uint32_t version, uint32_t major,
+					 uint32_t minor)
 {
 	int src_fd, dest_fd;
 	struct stat sbuf;
 	unsigned char *ptr;
+	struct stm32_header stm32image_header;
 
 	dest_fd = open(destname, O_RDWR | O_CREAT | O_TRUNC | O_APPEND, 0666);
 	if (dest_fd == -1) {
@@ -177,11 +180,12 @@ static int stm32image_create_header_file(char *srcname, char *destname,
 		   dest_fd, 0);
 
 	if (ptr == MAP_FAILED) {
-		fprintf(stderr, "Can't read %s\n", srcname);
+		fprintf(stderr, "Can't write %s\n", destname);
 		return -1;
 	}
 
-	stm32image_set_header(ptr, &sbuf, dest_fd, loadaddr, entry, version);
+	stm32image_set_header(ptr, &sbuf, dest_fd, loadaddr, entry, version,
+			      major, minor);
 
 	stm32image_print_header(ptr);
 
@@ -193,9 +197,11 @@ static int stm32image_create_header_file(char *srcname, char *destname,
 int main(int argc, char *argv[])
 {
 	int opt, loadaddr = -1, entry = -1, err = 0, version = 0;
+	int major = HEADER_VERSION_V1;
+	int minor = 0;
 	char *dest = NULL, *src = NULL;
 
-	while ((opt = getopt(argc, argv, ":s:d:l:e:v:")) != -1) {
+	while ((opt = getopt(argc, argv, ":s:d:l:e:v:m:n:")) != -1) {
 		switch (opt) {
 		case 's':
 			src = optarg;
@@ -204,17 +210,23 @@ int main(int argc, char *argv[])
 			dest = optarg;
 			break;
 		case 'l':
-			loadaddr = strtol(optarg, NULL, 16);
+			loadaddr = strtol(optarg, NULL, 0);
 			break;
 		case 'e':
-			entry = strtol(optarg, NULL, 16);
+			entry = strtol(optarg, NULL, 0);
 			break;
 		case 'v':
-			version = strtol(optarg, NULL, 10);
+			version = strtol(optarg, NULL, 0);
+			break;
+		case 'm':
+			major = strtol(optarg, NULL, 0);
+			break;
+		case 'n':
+			minor = strtol(optarg, NULL, 0);
 			break;
 		default:
 			fprintf(stderr,
-				"Usage : %s [-s srcfile] [-d destfile] [-l loadaddr] [-e entry_point]\n",
+				"Usage : %s [-s srcfile] [-d destfile] [-l loadaddr] [-e entry_point] [-m major] [-n minor]\n",
 					argv[0]);
 			return -1;
 		}
@@ -241,7 +253,7 @@ int main(int argc, char *argv[])
 	}
 
 	err = stm32image_create_header_file(src, dest, loadaddr,
-					    entry, version);
+					    entry, version, major, minor);
 
 	return err;
 }
-- 
2.17.1

