From 9e9811305959e8d853a54714eb43ae21fe5d3e1d Mon Sep 17 00:00:00 2001
From: Romuald JEANNE <romuald.jeanne@st.com>
Date: Tue, 16 Mar 2021 08:17:59 +0100
Subject: [PATCH 3/5] ARM v2020.10-stm32mp-r1 MISC-DRIVERS

Signed-off-by: Romuald JEANNE <romuald.jeanne@st.com>
---
 common/Kconfig                                |   2 +-
 common/cli_readline.c                         |   6 +
 common/image-fdt.c                            |  23 +-
 doc/device-tree-bindings/arm/arm,scmi.txt     | 197 +++++++++
 .../phy/phy-stm32-usbphyc.txt                 |   2 +
 drivers/clk/Kconfig                           |   8 +
 drivers/clk/Makefile                          |   1 +
 drivers/clk/clk_scmi.c                        |  99 +++++
 drivers/clk/clk_stm32mp1.c                    |  63 +--
 drivers/core/ofnode.c                         |  10 +
 drivers/dfu/dfu_mtd.c                         |   9 +-
 drivers/firmware/Kconfig                      |   2 +
 drivers/firmware/Makefile                     |   1 +
 drivers/firmware/scmi/Kconfig                 |  19 +
 drivers/firmware/scmi/Makefile                |   5 +
 drivers/firmware/scmi/mailbox_agent.c         | 102 +++++
 drivers/firmware/scmi/sandbox-scmi_agent.c    | 410 ++++++++++++++++++
 drivers/firmware/scmi/sandbox-scmi_devices.c  | 113 +++++
 drivers/firmware/scmi/scmi_agent-uclass.c     | 119 +++++
 drivers/firmware/scmi/smccc_agent.c           |  89 ++++
 drivers/firmware/scmi/smt.c                   | 139 ++++++
 drivers/firmware/scmi/smt.h                   |  86 ++++
 drivers/gpio/stm32_gpio.c                     |  28 +-
 drivers/mtd/nand/core.c                       |   2 +
 drivers/mtd/nand/spi/core.c                   |   3 +
 drivers/mtd/spi/spi-nor-core.c                |   3 +
 drivers/phy/phy-stm32-usbphyc.c               |  33 +-
 drivers/pinctrl/pinctrl-stmfx.c               |  12 +-
 drivers/pinctrl/pinctrl_stm32.c               |  37 +-
 drivers/remoteproc/Kconfig                    |   8 +
 drivers/remoteproc/Makefile                   |   1 +
 drivers/remoteproc/rproc-optee.c              | 219 ++++++++++
 drivers/remoteproc/stm32_copro.c              | 207 ++++++---
 drivers/reset/Kconfig                         |   8 +
 drivers/reset/Makefile                        |   1 +
 drivers/reset/reset-scmi.c                    |  81 ++++
 drivers/reset/stm32-reset.c                   |  17 +-
 drivers/spi/stm32_qspi.c                      |   2 +
 drivers/usb/gadget/dwc2_udc_otg.c             |  61 ++-
 drivers/usb/gadget/dwc2_udc_otg_regs.h        |   2 +
 drivers/video/dw_mipi_dsi.c                   |  62 ++-
 drivers/video/stm32/stm32_dsi.c               |   3 +
 env/ext4.c                                    |   3 +-
 env/mmc.c                                     |   6 +-
 env/sf.c                                      |  32 +-
 include/configs/10m50_devboard.h              |   5 -
 include/configs/3c120_devboard.h              |   5 -
 include/configs/sandbox.h                     |   2 -
 include/configs/stm32mp1.h                    |   1 -
 include/configs/x86-common.h                  |   2 -
 include/dm/ofnode.h                           |  11 +
 include/dm/uclass-id.h                        |   1 +
 include/dt-bindings/clock/stm32mp1-clks.h     |  33 ++
 include/dt-bindings/pinctrl/stm32-pinfunc.h   |   1 +
 include/dt-bindings/reset/stm32mp1-resets.h   |  15 +
 include/dt-bindings/rtc/rtc-stm32.h           |  13 +
 include/dt-bindings/soc/stm32-hdp.h           | 108 +++++
 include/env_internal.h                        |   1 +
 include/fdtdec.h                              |   5 +-
 include/image.h                               |   2 +-
 include/lmb.h                                 |  29 +-
 include/mipi_dsi.h                            |  17 +
 include/rproc_optee.h                         | 127 ++++++
 include/scmi_agent-uclass.h                   |  24 +
 include/scmi_agent.h                          |  68 +++
 include/scmi_protocols.h                      | 179 ++++++++
 include/usb/dwc2_udc.h                        |   1 +
 lib/Kconfig                                   |  23 +
 lib/fdtdec.c                                  |  10 +-
 lib/lmb.c                                     | 108 +++--
 lib/optee/optee.c                             |   2 +-
 scripts/config_whitelist.txt                  |   1 -
 test/dm/Makefile                              |   1 +
 test/dm/fdtdec.c                              |   9 +-
 test/dm/ofnode.c                              |  12 +
 test/dm/scmi.c                                | 203 +++++++++
 test/lib/lmb.c                                |  89 ++++
 77 files changed, 3162 insertions(+), 282 deletions(-)
 create mode 100644 doc/device-tree-bindings/arm/arm,scmi.txt
 create mode 100644 drivers/clk/clk_scmi.c
 create mode 100644 drivers/firmware/scmi/Kconfig
 create mode 100644 drivers/firmware/scmi/Makefile
 create mode 100644 drivers/firmware/scmi/mailbox_agent.c
 create mode 100644 drivers/firmware/scmi/sandbox-scmi_agent.c
 create mode 100644 drivers/firmware/scmi/sandbox-scmi_devices.c
 create mode 100644 drivers/firmware/scmi/scmi_agent-uclass.c
 create mode 100644 drivers/firmware/scmi/smccc_agent.c
 create mode 100644 drivers/firmware/scmi/smt.c
 create mode 100644 drivers/firmware/scmi/smt.h
 create mode 100644 drivers/remoteproc/rproc-optee.c
 create mode 100644 drivers/reset/reset-scmi.c
 create mode 100644 include/dt-bindings/rtc/rtc-stm32.h
 create mode 100644 include/dt-bindings/soc/stm32-hdp.h
 create mode 100644 include/rproc_optee.h
 create mode 100644 include/scmi_agent-uclass.h
 create mode 100644 include/scmi_agent.h
 create mode 100644 include/scmi_protocols.h
 create mode 100644 test/dm/scmi.c

diff --git a/common/Kconfig b/common/Kconfig
index 9c20a9738e..7099bbf902 100644
--- a/common/Kconfig
+++ b/common/Kconfig
@@ -416,7 +416,7 @@ config USE_PREBOOT
 config PREBOOT
 	string "preboot default value"
 	depends on USE_PREBOOT && !USE_DEFAULT_ENV_FILE
-	default "usb start" if USB_KEYBOARD || USB_STORAGE
+	default "usb start" if USB_KEYBOARD
 	default ""
 	help
 	  This is the default of "preboot" environment variable.
diff --git a/common/cli_readline.c b/common/cli_readline.c
index 1f1e28c6d8..491863303a 100644
--- a/common/cli_readline.c
+++ b/common/cli_readline.c
@@ -15,6 +15,12 @@
 #include <time.h>
 #include <watchdog.h>
 
+#ifdef CONFIG_SPL_BUILD
+#undef CONFIG_CMDLINE_EDITING
+#undef CONFIG_AUTO_COMPLETE
+#undef CONFIG_SHOW_ACTIVITY
+#endif
+
 DECLARE_GLOBAL_DATA_PTR;
 
 static const char erase_seq[] = "\b \b";	/* erase sequence */
diff --git a/common/image-fdt.c b/common/image-fdt.c
index 3d6935ad40..55b3593762 100644
--- a/common/image-fdt.c
+++ b/common/image-fdt.c
@@ -74,18 +74,20 @@ static const image_header_t *image_get_fdt(ulong fdt_addr)
 #endif
 
 static void boot_fdt_reserve_region(struct lmb *lmb, uint64_t addr,
-				    uint64_t size)
+				    uint64_t size, enum lmb_flags flags)
 {
 	long ret;
 
-	ret = lmb_reserve(lmb, addr, size);
+	ret = lmb_reserve_flags(lmb, addr, size, flags);
 	if (ret >= 0) {
-		debug("   reserving fdt memory region: addr=%llx size=%llx\n",
-		      (unsigned long long)addr, (unsigned long long)size);
+		debug("   reserving fdt memory region: addr=%llx size=%llx flags=%x\n",
+		      (unsigned long long)addr,
+		      (unsigned long long)size, flags);
 	} else {
 		puts("ERROR: reserving fdt memory region failed ");
-		printf("(addr=%llx size=%llx)\n",
-		       (unsigned long long)addr, (unsigned long long)size);
+		printf("(addr=%llx size=%llx flags=%x)\n",
+		       (unsigned long long)addr,
+		       (unsigned long long)size, flags);
 	}
 }
 
@@ -105,6 +107,7 @@ void boot_fdt_add_mem_rsv_regions(struct lmb *lmb, void *fdt_blob)
 	int i, total, ret;
 	int nodeoffset, subnode;
 	struct fdt_resource res;
+	enum lmb_flags flags;
 
 	if (fdt_check_header(fdt_blob) != 0)
 		return;
@@ -114,7 +117,7 @@ void boot_fdt_add_mem_rsv_regions(struct lmb *lmb, void *fdt_blob)
 	for (i = 0; i < total; i++) {
 		if (fdt_get_mem_rsv(fdt_blob, i, &addr, &size) != 0)
 			continue;
-		boot_fdt_reserve_region(lmb, addr, size);
+		boot_fdt_reserve_region(lmb, addr, size, LMB_NONE);
 	}
 
 	/* process reserved-memory */
@@ -126,9 +129,13 @@ void boot_fdt_add_mem_rsv_regions(struct lmb *lmb, void *fdt_blob)
 			ret = fdt_get_resource(fdt_blob, subnode, "reg", 0,
 					       &res);
 			if (!ret && fdtdec_get_is_enabled(fdt_blob, subnode)) {
+				flags = LMB_NONE;
+				if (fdtdec_get_bool(fdt_blob, subnode,
+						    "no-map"))
+					flags = LMB_NOMAP;
 				addr = res.start;
 				size = res.end - res.start + 1;
-				boot_fdt_reserve_region(lmb, addr, size);
+				boot_fdt_reserve_region(lmb, addr, size, flags);
 			}
 
 			subnode = fdt_next_subnode(fdt_blob, subnode);
diff --git a/doc/device-tree-bindings/arm/arm,scmi.txt b/doc/device-tree-bindings/arm/arm,scmi.txt
new file mode 100644
index 0000000000..1f293ea24c
--- /dev/null
+++ b/doc/device-tree-bindings/arm/arm,scmi.txt
@@ -0,0 +1,197 @@
+System Control and Management Interface (SCMI) Message Protocol
+----------------------------------------------------------
+
+The SCMI is intended to allow agents such as OSPM to manage various functions
+that are provided by the hardware platform it is running on, including power
+and performance functions.
+
+This binding is intended to define the interface the firmware implementing
+the SCMI as described in ARM document number ARM DEN 0056A ("ARM System Control
+and Management Interface Platform Design Document")[0] provide for OSPM in
+the device tree.
+
+Required properties:
+
+The scmi node with the following properties shall be under the /firmware/ node.
+
+- compatible : shall be "arm,scmi" or "arm,scmi-smc" for smc/hvc transports
+- mboxes: List of phandle and mailbox channel specifiers. It should contain
+	  exactly one or two mailboxes, one for transmitting messages("tx")
+	  and another optional for receiving the notifications("rx") if
+	  supported.
+- shmem : List of phandle pointing to the shared memory(SHM) area as per
+	  generic mailbox client binding.
+- #address-cells : should be '1' if the device has sub-nodes, maps to
+	  protocol identifier for a given sub-node.
+- #size-cells : should be '0' as 'reg' property doesn't have any size
+	  associated with it.
+- arm,smc-id : SMC id required when using smc or hvc transports
+
+Optional properties:
+
+- mbox-names: shall be "tx" or "rx" depending on mboxes entries.
+
+See Documentation/devicetree/bindings/mailbox/mailbox.txt for more details
+about the generic mailbox controller and client driver bindings.
+
+The mailbox is the only permitted method of calling the SCMI firmware.
+Mailbox doorbell is used as a mechanism to alert the presence of a
+messages and/or notification.
+
+Each protocol supported shall have a sub-node with corresponding compatible
+as described in the following sections. If the platform supports dedicated
+communication channel for a particular protocol, the 3 properties namely:
+mboxes, mbox-names and shmem shall be present in the sub-node corresponding
+to that protocol.
+
+Clock/Performance bindings for the clocks/OPPs based on SCMI Message Protocol
+------------------------------------------------------------
+
+This binding uses the common clock binding[1].
+
+Required properties:
+- #clock-cells : Should be 1. Contains the Clock ID value used by SCMI commands.
+
+Power domain bindings for the power domains based on SCMI Message Protocol
+------------------------------------------------------------
+
+This binding for the SCMI power domain providers uses the generic power
+domain binding[2].
+
+Required properties:
+ - #power-domain-cells : Should be 1. Contains the device or the power
+			 domain ID value used by SCMI commands.
+
+Sensor bindings for the sensors based on SCMI Message Protocol
+--------------------------------------------------------------
+SCMI provides an API to access the various sensors on the SoC.
+
+Required properties:
+- #thermal-sensor-cells: should be set to 1. This property follows the
+			 thermal device tree bindings[3].
+
+			 Valid cell values are raw identifiers (Sensor ID)
+			 as used by the firmware. Refer to  platform details
+			 for your implementation for the IDs to use.
+
+Reset signal bindings for the reset domains based on SCMI Message Protocol
+------------------------------------------------------------
+
+This binding for the SCMI reset domain providers uses the generic reset
+signal binding[5].
+
+Required properties:
+ - #reset-cells : Should be 1. Contains the reset domain ID value used
+		  by SCMI commands.
+
+SRAM and Shared Memory for SCMI
+-------------------------------
+
+A small area of SRAM is reserved for SCMI communication between application
+processors and SCP.
+
+The properties should follow the generic mmio-sram description found in [4]
+
+Each sub-node represents the reserved area for SCMI.
+
+Required sub-node properties:
+- reg : The base offset and size of the reserved area with the SRAM
+- compatible : should be "arm,scmi-shmem" for Non-secure SRAM based
+	       shared memory
+
+[0] http://infocenter.arm.com/help/topic/com.arm.doc.den0056a/index.html
+[1] Documentation/devicetree/bindings/clock/clock-bindings.txt
+[2] Documentation/devicetree/bindings/power/power-domain.yaml
+[3] Documentation/devicetree/bindings/thermal/thermal.txt
+[4] Documentation/devicetree/bindings/sram/sram.yaml
+[5] Documentation/devicetree/bindings/reset/reset.txt
+
+Example:
+
+sram@50000000 {
+	compatible = "mmio-sram";
+	reg = <0x0 0x50000000 0x0 0x10000>;
+
+	#address-cells = <1>;
+	#size-cells = <1>;
+	ranges = <0 0x0 0x50000000 0x10000>;
+
+	cpu_scp_lpri: scp-shmem@0 {
+		compatible = "arm,scmi-shmem";
+		reg = <0x0 0x200>;
+	};
+
+	cpu_scp_hpri: scp-shmem@200 {
+		compatible = "arm,scmi-shmem";
+		reg = <0x200 0x200>;
+	};
+};
+
+mailbox@40000000 {
+	....
+	#mbox-cells = <1>;
+	reg = <0x0 0x40000000 0x0 0x10000>;
+};
+
+firmware {
+
+	...
+
+	scmi {
+		compatible = "arm,scmi";
+		mboxes = <&mailbox 0 &mailbox 1>;
+		mbox-names = "tx", "rx";
+		shmem = <&cpu_scp_lpri &cpu_scp_hpri>;
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		scmi_devpd: protocol@11 {
+			reg = <0x11>;
+			#power-domain-cells = <1>;
+		};
+
+		scmi_dvfs: protocol@13 {
+			reg = <0x13>;
+			#clock-cells = <1>;
+		};
+
+		scmi_clk: protocol@14 {
+			reg = <0x14>;
+			#clock-cells = <1>;
+		};
+
+		scmi_sensors0: protocol@15 {
+			reg = <0x15>;
+			#thermal-sensor-cells = <1>;
+		};
+
+		scmi_reset: protocol@16 {
+			reg = <0x16>;
+			#reset-cells = <1>;
+		};
+	};
+};
+
+cpu@0 {
+	...
+	reg = <0 0>;
+	clocks = <&scmi_dvfs 0>;
+};
+
+hdlcd@7ff60000 {
+	...
+	reg = <0 0x7ff60000 0 0x1000>;
+	clocks = <&scmi_clk 4>;
+	power-domains = <&scmi_devpd 1>;
+	resets = <&scmi_reset 10>;
+};
+
+thermal-zones {
+	soc_thermal {
+		polling-delay-passive = <100>;
+		polling-delay = <1000>;
+					/* sensor ID */
+		thermal-sensors = <&scmi_sensors0 3>;
+		...
+	};
+};
diff --git a/doc/device-tree-bindings/phy/phy-stm32-usbphyc.txt b/doc/device-tree-bindings/phy/phy-stm32-usbphyc.txt
index da98407403..edfe4b426c 100644
--- a/doc/device-tree-bindings/phy/phy-stm32-usbphyc.txt
+++ b/doc/device-tree-bindings/phy/phy-stm32-usbphyc.txt
@@ -45,6 +45,8 @@ Required properties:
 - #phy-cells: see phy-bindings.txt in the same directory, must be <0> for PHY
   port#1 and must be <1> for PHY port#2, to select USB controller
 
+Optional properties:
+- vbus-supply: phandle to the regulator providing 5V vbus to the USB connector
 
 Example:
 		usbphyc: usb-phy@5a006000 {
diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index 6003e140b5..4dfbad7986 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -159,6 +159,14 @@ config CLK_CDCE9XX
 	   Enable the clock synthesizer driver for CDCE913/925/937/949
 	   series of chips.
 
+config CLK_SCMI
+	bool "Enable SCMI clock driver"
+	depends on SCMI_FIRMWARE
+	help
+	  Enable this option if you want to support clock devices exposed
+	  by a SCMI agent based on SCMI clock protocol communication
+	  with a SCMI server.
+
 source "drivers/clk/analogbits/Kconfig"
 source "drivers/clk/at91/Kconfig"
 source "drivers/clk/exynos/Kconfig"
diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index cda4b4b605..d1e295ac7c 100644
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -32,6 +32,7 @@ obj-$(CONFIG_CLK_MPC83XX) += mpc83xx_clk.o
 obj-$(CONFIG_CLK_OCTEON) += clk_octeon.o
 obj-$(CONFIG_CLK_OWL) += owl/
 obj-$(CONFIG_CLK_RENESAS) += renesas/
+obj-$(CONFIG_CLK_SCMI) += clk_scmi.o
 obj-$(CONFIG_CLK_SIFIVE) += sifive/
 obj-$(CONFIG_ARCH_SUNXI) += sunxi/
 obj-$(CONFIG_CLK_STM32F) += clk_stm32f.o
diff --git a/drivers/clk/clk_scmi.c b/drivers/clk/clk_scmi.c
new file mode 100644
index 0000000000..93a4819501
--- /dev/null
+++ b/drivers/clk/clk_scmi.c
@@ -0,0 +1,99 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2019-2020 Linaro Limited
+ */
+#include <common.h>
+#include <clk-uclass.h>
+#include <dm.h>
+#include <scmi_agent.h>
+#include <scmi_protocols.h>
+#include <asm/types.h>
+
+static int scmi_clk_gate(struct clk *clk, int enable)
+{
+	struct scmi_clk_state_in in = {
+		.clock_id = clk->id,
+		.attributes = enable,
+	};
+	struct scmi_clk_state_out out;
+	struct scmi_msg msg = SCMI_MSG_IN(SCMI_PROTOCOL_ID_CLOCK,
+					  SCMI_CLOCK_CONFIG_SET,
+					  in, out);
+	int ret;
+
+	ret = devm_scmi_process_msg(clk->dev->parent, &msg);
+	if (ret)
+		return ret;
+
+	return scmi_to_linux_errno(out.status);
+}
+
+static int scmi_clk_enable(struct clk *clk)
+{
+	return scmi_clk_gate(clk, 1);
+}
+
+static int scmi_clk_disable(struct clk *clk)
+{
+	return scmi_clk_gate(clk, 0);
+}
+
+static ulong scmi_clk_get_rate(struct clk *clk)
+{
+	struct scmi_clk_rate_get_in in = {
+		.clock_id = clk->id,
+	};
+	struct scmi_clk_rate_get_out out;
+	struct scmi_msg msg = SCMI_MSG_IN(SCMI_PROTOCOL_ID_CLOCK,
+					  SCMI_CLOCK_RATE_GET,
+					  in, out);
+	int ret;
+
+	ret = devm_scmi_process_msg(clk->dev->parent, &msg);
+	if (ret < 0)
+		return ret;
+
+	ret = scmi_to_linux_errno(out.status);
+	if (ret < 0)
+		return ret;
+
+	return (ulong)(((u64)out.rate_msb << 32) | out.rate_lsb);
+}
+
+static ulong scmi_clk_set_rate(struct clk *clk, ulong rate)
+{
+	struct scmi_clk_rate_set_in in = {
+		.clock_id = clk->id,
+		.flags = SCMI_CLK_RATE_ROUND_CLOSEST,
+		.rate_lsb = (u32)rate,
+		.rate_msb = (u32)((u64)rate >> 32),
+	};
+	struct scmi_clk_rate_set_out out;
+	struct scmi_msg msg = SCMI_MSG_IN(SCMI_PROTOCOL_ID_CLOCK,
+					  SCMI_CLOCK_RATE_SET,
+					  in, out);
+	int ret;
+
+	ret = devm_scmi_process_msg(clk->dev->parent, &msg);
+	if (ret < 0)
+		return ret;
+
+	ret = scmi_to_linux_errno(out.status);
+	if (ret < 0)
+		return ret;
+
+	return scmi_clk_get_rate(clk);
+}
+
+static const struct clk_ops scmi_clk_ops = {
+	.enable = scmi_clk_enable,
+	.disable = scmi_clk_disable,
+	.get_rate = scmi_clk_get_rate,
+	.set_rate = scmi_clk_set_rate,
+};
+
+U_BOOT_DRIVER(scmi_clock) = {
+	.name = "scmi_clk",
+	.id = UCLASS_CLK,
+	.ops = &scmi_clk_ops,
+};
diff --git a/drivers/clk/clk_stm32mp1.c b/drivers/clk/clk_stm32mp1.c
index c8840b9e5f..efa402da52 100644
--- a/drivers/clk/clk_stm32mp1.c
+++ b/drivers/clk/clk_stm32mp1.c
@@ -246,7 +246,7 @@ DECLARE_GLOBAL_DATA_PTR;
 enum stm32mp1_parent_id {
 /*
  * _HSI, _HSE, _CSI, _LSI, _LSE should not be moved
- * they are used as index in osc[] as entry point
+ * they are used as index in osc_clk[] as clock reference
  */
 	_HSI,
 	_HSE,
@@ -426,8 +426,7 @@ struct stm32mp1_clk_data {
 struct stm32mp1_clk_priv {
 	fdt_addr_t base;
 	const struct stm32mp1_clk_data *data;
-	ulong osc[NB_OSC];
-	struct udevice *osc_dev[NB_OSC];
+	struct clk osc_clk[NB_OSC];
 };
 
 #define STM32MP1_CLK(off, b, idx, s)		\
@@ -546,6 +545,7 @@ static const struct stm32mp1_clk_gate stm32mp1_clk_gate[] = {
 	STM32MP1_CLK_SET_CLR(RCC_MP_APB4ENSETR, 16, USBPHY_K, _USBPHY_SEL),
 
 	STM32MP1_CLK_SET_CLR(RCC_MP_APB5ENSETR, 2, I2C4_K, _I2C46_SEL),
+	STM32MP1_CLK_SET_CLR(RCC_MP_APB5ENSETR, 3, I2C6_K, _I2C46_SEL),
 	STM32MP1_CLK_SET_CLR(RCC_MP_APB5ENSETR, 8, RTCAPB, _PCLK5),
 	STM32MP1_CLK_SET_CLR(RCC_MP_APB5ENSETR, 20, STGEN_K, _STGEN_SEL),
 
@@ -785,7 +785,7 @@ static ulong stm32mp1_clk_get_fixed(struct stm32mp1_clk_priv *priv, int idx)
 		return 0;
 	}
 
-	return priv->osc[idx];
+	return clk_get_rate(&priv->osc_clk[idx]);
 }
 
 static int stm32mp1_clk_get_id(struct stm32mp1_clk_priv *priv, unsigned long id)
@@ -1546,7 +1546,7 @@ static int stm32mp1_hsidiv(fdt_addr_t rcc, ulong hsifreq)
 			break;
 
 	if (hsidiv == 4) {
-		pr_err("clk-hsi frequency invalid");
+		pr_err("hsi frequency invalid");
 		return -1;
 	}
 
@@ -1953,13 +1953,13 @@ static int stm32mp1_clktree(struct udevice *dev)
 	 * switch ON oscillator found in device-tree,
 	 * HSI already ON after bootrom
 	 */
-	if (priv->osc[_LSI])
+	if (clk_valid(&priv->osc_clk[_LSI]))
 		stm32mp1_lsi_set(rcc, 1);
 
-	if (priv->osc[_LSE]) {
+	if (clk_valid(&priv->osc_clk[_LSE])) {
 		int bypass, digbyp;
 		u32 lsedrv;
-		struct udevice *dev = priv->osc_dev[_LSE];
+		struct udevice *dev = priv->osc_clk[_LSE].dev;
 
 		bypass = dev_read_bool(dev, "st,bypass");
 		digbyp = dev_read_bool(dev, "st,digbypass");
@@ -1970,9 +1970,9 @@ static int stm32mp1_clktree(struct udevice *dev)
 		stm32mp1_lse_enable(rcc, bypass, digbyp, lsedrv);
 	}
 
-	if (priv->osc[_HSE]) {
+	if (clk_valid(&priv->osc_clk[_HSE])) {
 		int bypass, digbyp, css;
-		struct udevice *dev = priv->osc_dev[_HSE];
+		struct udevice *dev = priv->osc_clk[_HSE].dev;
 
 		bypass = dev_read_bool(dev, "st,bypass");
 		digbyp = dev_read_bool(dev, "st,digbypass");
@@ -1997,8 +1997,8 @@ static int stm32mp1_clktree(struct udevice *dev)
 
 	/* configure HSIDIV */
 	debug("configure HSIDIV\n");
-	if (priv->osc[_HSI]) {
-		stm32mp1_hsidiv(rcc, priv->osc[_HSI]);
+	if (clk_valid(&priv->osc_clk[_HSI])) {
+		stm32mp1_hsidiv(rcc, clk_get_rate(&priv->osc_clk[_HSI]));
 		stgen_config(priv);
 	}
 
@@ -2044,7 +2044,7 @@ static int stm32mp1_clktree(struct udevice *dev)
 	}
 
 	/* wait LSE ready before to use it */
-	if (priv->osc[_LSE])
+	if (clk_valid(&priv->osc_clk[_LSE]))
 		stm32mp1_lse_wait(rcc);
 
 	/* configure with expected clock source */
@@ -2083,7 +2083,7 @@ static int stm32mp1_clktree(struct udevice *dev)
 
 	debug("oscillator off\n");
 	/* switch OFF HSI if not found in device-tree */
-	if (!priv->osc[_HSI])
+	if (!clk_valid(&priv->osc_clk[_HSI]))
 		stm32mp1_hsi_set(rcc, 0);
 
 	/* Software Self-Refresh mode (SSR) during DDR initilialization */
@@ -2181,40 +2181,25 @@ static ulong stm32mp1_clk_set_rate(struct clk *clk, unsigned long clk_rate)
 	return -EINVAL;
 }
 
-static void stm32mp1_osc_clk_init(const char *name,
-				  struct stm32mp1_clk_priv *priv,
-				  int index)
-{
-	struct clk clk;
-	struct udevice *dev = NULL;
-
-	priv->osc[index] = 0;
-	clk.id = 0;
-	if (!uclass_get_device_by_name(UCLASS_CLK, name, &dev)) {
-		if (clk_request(dev, &clk))
-			pr_err("%s request", name);
-		else
-			priv->osc[index] = clk_get_rate(&clk);
-	}
-	priv->osc_dev[index] = dev;
-}
-
 static void stm32mp1_osc_init(struct udevice *dev)
 {
 	struct stm32mp1_clk_priv *priv = dev_get_priv(dev);
 	int i;
 	const char *name[NB_OSC] = {
-		[_LSI] = "clk-lsi",
-		[_LSE] = "clk-lse",
-		[_HSI] = "clk-hsi",
-		[_HSE] = "clk-hse",
-		[_CSI] = "clk-csi",
+		[_LSI] = "lsi",
+		[_LSE] = "lse",
+		[_HSI] = "hsi",
+		[_HSE] = "hse",
+		[_CSI] = "csi",
 		[_I2S_CKIN] = "i2s_ckin",
 	};
 
 	for (i = 0; i < NB_OSC; i++) {
-		stm32mp1_osc_clk_init(name[i], priv, i);
-		debug("%d: %s => %x\n", i, name[i], (u32)priv->osc[i]);
+		if (clk_get_by_name(dev, name[i], &priv->osc_clk[i]))
+			dev_dbg(dev, "No source clock \"%s\"", name[i]);
+		else
+			dev_dbg(dev, "%s clock rate: %luHz\n",
+				name[i], clk_get_rate(&priv->osc_clk[i]));
 	}
 }
 
diff --git a/drivers/core/ofnode.c b/drivers/core/ofnode.c
index d02d8d33fe..2b848cc870 100644
--- a/drivers/core/ofnode.c
+++ b/drivers/core/ofnode.c
@@ -226,6 +226,16 @@ int ofnode_read_u32_array(ofnode node, const char *propname,
 	}
 }
 
+bool ofnode_is_enabled(ofnode node)
+{
+	if (ofnode_is_np(node)) {
+		return of_device_is_available(ofnode_to_np(node));
+	} else {
+		return fdtdec_get_is_enabled(gd->fdt_blob,
+					     ofnode_to_offset(node));
+	}
+}
+
 ofnode ofnode_first_subnode(ofnode node)
 {
 	assert(ofnode_valid(node));
diff --git a/drivers/dfu/dfu_mtd.c b/drivers/dfu/dfu_mtd.c
index 36cd4e945b..8c6c342a75 100644
--- a/drivers/dfu/dfu_mtd.c
+++ b/drivers/dfu/dfu_mtd.c
@@ -190,7 +190,7 @@ static int dfu_flush_medium_mtd(struct dfu_entity *dfu)
 	int ret;
 
 	/* in case of ubi partition, erase rest of the partition */
-	if (dfu->data.nand.ubi) {
+	if (dfu->data.mtd.ubi) {
 		struct erase_info erase_op = {};
 
 		erase_op.mtd = dfu->data.mtd.info;
@@ -228,7 +228,7 @@ static unsigned int dfu_polltimeout_mtd(struct dfu_entity *dfu)
 	 * ubi partition, as sectors which are not used need
 	 * to be erased
 	 */
-	if (dfu->data.nand.ubi)
+	if (dfu->data.mtd.ubi)
 		return DFU_MANIFEST_POLL_TIMEOUT;
 
 	return DFU_DEFAULT_POLL_TIMEOUT;
@@ -238,7 +238,6 @@ int dfu_fill_entity_mtd(struct dfu_entity *dfu, char *devstr, char *s)
 {
 	char *st;
 	struct mtd_info *mtd;
-	bool has_pages;
 	int ret, part;
 
 	mtd = get_mtd_device_nm(devstr);
@@ -248,9 +247,7 @@ int dfu_fill_entity_mtd(struct dfu_entity *dfu, char *devstr, char *s)
 
 	dfu->dev_type = DFU_DEV_MTD;
 	dfu->data.mtd.info = mtd;
-
-	has_pages = mtd->type == MTD_NANDFLASH || mtd->type == MTD_MLCNANDFLASH;
-	dfu->max_buf_size = has_pages ? mtd->erasesize : 0;
+	dfu->max_buf_size = mtd->erasesize;
 
 	st = strsep(&s, " ");
 	if (!strcmp(st, "raw")) {
diff --git a/drivers/firmware/Kconfig b/drivers/firmware/Kconfig
index b70a206355..ef958b3a7a 100644
--- a/drivers/firmware/Kconfig
+++ b/drivers/firmware/Kconfig
@@ -36,3 +36,5 @@ config ZYNQMP_FIRMWARE
 	  various platform management services.
 	  Say yes to enable ZynqMP firmware interface driver.
 	  If in doubt, say N.
+
+source "drivers/firmware/scmi/Kconfig"
diff --git a/drivers/firmware/Makefile b/drivers/firmware/Makefile
index a0c250a473..7ce83d72bd 100644
--- a/drivers/firmware/Makefile
+++ b/drivers/firmware/Makefile
@@ -3,3 +3,4 @@ obj-$(CONFIG_$(SPL_)ARM_PSCI_FW)	+= psci.o
 obj-$(CONFIG_TI_SCI_PROTOCOL)	+= ti_sci.o
 obj-$(CONFIG_SANDBOX)		+= firmware-sandbox.o
 obj-$(CONFIG_ZYNQMP_FIRMWARE)	+= firmware-zynqmp.o
+obj-$(CONFIG_SCMI_FIRMWARE)	+= scmi/
diff --git a/drivers/firmware/scmi/Kconfig b/drivers/firmware/scmi/Kconfig
new file mode 100644
index 0000000000..c3a109beac
--- /dev/null
+++ b/drivers/firmware/scmi/Kconfig
@@ -0,0 +1,19 @@
+config SCMI_FIRMWARE
+	bool "Enable SCMI support"
+	select FIRMWARE
+	select OF_TRANSLATE
+	depends on SANDBOX || DM_MAILBOX || ARM_SMCCC
+	help
+	  System Control and Management Interface (SCMI) is a communication
+	  protocol that defines standard interfaces for power, performance
+	  and system management. The SCMI specification is available at
+	  https://developer.arm.com/architectures/system-architectures/software-standards/scmi
+
+	  An SCMI agent communicates with a related SCMI server firmware
+	  located in another sub-system, as a companion micro controller
+	  or a companion host in the CPU system.
+
+	  Communications between agent (client) and the SCMI server are
+	  based on message exchange. Messages can be exchange over tranport
+	  channels as a mailbox device or an Arm SMCCC service with some
+	  piece of identified shared memory.
diff --git a/drivers/firmware/scmi/Makefile b/drivers/firmware/scmi/Makefile
new file mode 100644
index 0000000000..e1e0224066
--- /dev/null
+++ b/drivers/firmware/scmi/Makefile
@@ -0,0 +1,5 @@
+obj-y	+= scmi_agent-uclass.o
+obj-y	+= smt.o
+obj-$(CONFIG_ARM_SMCCC) 	+= smccc_agent.o
+obj-$(CONFIG_DM_MAILBOX)	+= mailbox_agent.o
+obj-$(CONFIG_SANDBOX)		+= sandbox-scmi_agent.o sandbox-scmi_devices.o
diff --git a/drivers/firmware/scmi/mailbox_agent.c b/drivers/firmware/scmi/mailbox_agent.c
new file mode 100644
index 0000000000..7d9fb3622e
--- /dev/null
+++ b/drivers/firmware/scmi/mailbox_agent.c
@@ -0,0 +1,102 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2020 Linaro Limited.
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <mailbox.h>
+#include <scmi_agent.h>
+#include <scmi_agent-uclass.h>
+#include <dm/devres.h>
+#include <linux/compat.h>
+
+#include "smt.h"
+
+#define TIMEOUT_US_10MS			10000
+
+/**
+ * struct scmi_mbox_channel - Description of an SCMI mailbox transport
+ * @smt:	Shared memory buffer
+ * @mbox:	Mailbox channel description
+ * @timeout_us:	Timeout in microseconds for the mailbox transfer
+ */
+struct scmi_mbox_channel {
+	struct scmi_smt smt;
+	struct mbox_chan mbox;
+	ulong timeout_us;
+};
+
+static int scmi_mbox_process_msg(struct udevice *dev, struct scmi_msg *msg)
+{
+	struct scmi_mbox_channel *chan = dev_get_priv(dev);
+	int ret;
+
+	ret = scmi_write_msg_to_smt(dev, &chan->smt, msg);
+	if (ret)
+		return ret;
+
+	/* Give shm addr to mbox in case it is meaningful */
+	ret = mbox_send(&chan->mbox, chan->smt.buf);
+	if (ret) {
+		dev_err(dev, "Message send failed: %d\n", ret);
+		goto out;
+	}
+
+	/* Receive the response */
+	ret = mbox_recv(&chan->mbox, chan->smt.buf, chan->timeout_us);
+	if (ret) {
+		dev_err(dev, "Response failed: %d, abort\n", ret);
+		goto out;
+	}
+
+	ret = scmi_read_resp_from_smt(dev, &chan->smt, msg);
+
+out:
+	scmi_clear_smt_channel(&chan->smt);
+
+	return ret;
+}
+
+int scmi_mbox_probe(struct udevice *dev)
+{
+	struct scmi_mbox_channel *chan = dev_get_priv(dev);
+	int ret;
+
+	chan->timeout_us = TIMEOUT_US_10MS;
+
+	ret = mbox_get_by_index(dev, 0, &chan->mbox);
+	if (ret) {
+		dev_err(dev, "Failed to find mailbox: %d\n", ret);
+		goto out;
+	}
+
+	ret = scmi_dt_get_smt_buffer(dev, &chan->smt);
+	if (ret)
+		dev_err(dev, "Failed to get shm resources: %d\n", ret);
+
+out:
+	if (ret)
+		devm_kfree(dev, chan);
+
+	return ret;
+}
+
+static const struct udevice_id scmi_mbox_ids[] = {
+	{ .compatible = "arm,scmi" },
+	{ }
+};
+
+static const struct scmi_agent_ops scmi_mbox_ops = {
+	.process_msg = scmi_mbox_process_msg,
+};
+
+U_BOOT_DRIVER(scmi_mbox) = {
+	.name		= "scmi-over-mailbox",
+	.id		= UCLASS_SCMI_AGENT,
+	.of_match	= scmi_mbox_ids,
+	.priv_auto_alloc_size = sizeof(struct scmi_mbox_channel),
+	.probe		= scmi_mbox_probe,
+	.ops		= &scmi_mbox_ops,
+};
diff --git a/drivers/firmware/scmi/sandbox-scmi_agent.c b/drivers/firmware/scmi/sandbox-scmi_agent.c
new file mode 100644
index 0000000000..5b6a4232af
--- /dev/null
+++ b/drivers/firmware/scmi/sandbox-scmi_agent.c
@@ -0,0 +1,410 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2020, Linaro Limited
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <malloc.h>
+#include <scmi_agent.h>
+#include <scmi_agent-uclass.h>
+#include <scmi_protocols.h>
+#include <asm/io.h>
+#include <asm/scmi_test.h>
+#include <dm/device_compat.h>
+
+/*
+ * The sandbox SCMI agent driver simulates to some extend a SCMI message
+ * processing. It simulates few of the SCMI services for some of the
+ * SCMI protocols embedded in U-Boot. Currently:
+ * - SCMI clock protocol: emulate 2 agents each exposing few clocks
+ * - SCMI reset protocol: emulate 1 agents each exposing a reset
+ *
+ * Agent #0 simulates 2 clocks and 1 reset domain.
+ * See IDs in scmi0_clk[]/scmi0_reset[] and "sandbox-scmi-agent@0" in test.dts.
+ *
+ * Agent #1 simulates 1 clock.
+ * See IDs in scmi1_clk[] and "sandbox-scmi-agent@1" in test.dts.
+ *
+ * All clocks are default disabled and reset levels down.
+ *
+ * This Driver exports sandbox_scmi_service_ct() for the test sequence to
+ * get the state of the simulated services (clock state, rate, ...) and
+ * check back-end device state reflects the request send through the
+ * various uclass devices, as clocks and reset controllers.
+ */
+
+#define SANDBOX_SCMI_AGENT_COUNT	2
+
+static struct sandbox_scmi_clk scmi0_clk[] = {
+	{ .id = 7, .rate = 1000 },
+	{ .id = 3, .rate = 333 },
+};
+
+static struct sandbox_scmi_reset scmi0_reset[] = {
+	{ .id = 3 },
+};
+
+static struct sandbox_scmi_clk scmi1_clk[] = {
+	{ .id = 1, .rate = 44 },
+};
+
+/* The list saves to simulted end devices references for test purpose */
+struct sandbox_scmi_agent *sandbox_scmi_agent_list[SANDBOX_SCMI_AGENT_COUNT];
+
+static struct sandbox_scmi_service sandbox_scmi_service_state = {
+	.agent = sandbox_scmi_agent_list,
+	.agent_count = SANDBOX_SCMI_AGENT_COUNT,
+};
+
+struct sandbox_scmi_service *sandbox_scmi_service_ctx(void)
+{
+	return &sandbox_scmi_service_state;
+}
+
+static void debug_print_agent_state(struct udevice *dev, char *str)
+{
+	struct sandbox_scmi_agent *agent = dev_get_priv(dev);
+
+	dev_dbg(dev, "Dump sandbox_scmi_agent %u: %s\n", agent->idx, str);
+	dev_dbg(dev, " scmi%u_clk   (%zu): %d/%ld, %d/%ld, %d/%ld, ...\n",
+		agent->idx,
+		agent->clk_count,
+		agent->clk_count ? agent->clk[0].enabled : -1,
+		agent->clk_count ? agent->clk[0].rate : -1,
+		agent->clk_count > 1 ? agent->clk[1].enabled : -1,
+		agent->clk_count > 1 ? agent->clk[1].rate : -1,
+		agent->clk_count > 2 ? agent->clk[2].enabled : -1,
+		agent->clk_count > 2 ? agent->clk[2].rate : -1);
+	dev_dbg(dev, " scmi%u_reset (%zu): %d, %d, ...\n",
+		agent->idx,
+		agent->reset_count,
+		agent->reset_count ? agent->reset[0].asserted : -1,
+		agent->reset_count > 1 ? agent->reset[1].asserted : -1);
+};
+
+static struct sandbox_scmi_clk *get_scmi_clk_state(uint agent_id, uint clock_id)
+{
+	struct sandbox_scmi_clk *target = NULL;
+	size_t target_count = 0;
+	size_t n;
+
+	switch (agent_id) {
+	case 0:
+		target = scmi0_clk;
+		target_count = ARRAY_SIZE(scmi0_clk);
+		break;
+	case 1:
+		target = scmi1_clk;
+		target_count = ARRAY_SIZE(scmi1_clk);
+		break;
+	default:
+		return NULL;
+	}
+
+	for (n = 0; n < target_count; n++)
+		if (target[n].id == clock_id)
+			return target + n;
+
+	return NULL;
+}
+
+static struct sandbox_scmi_reset *get_scmi_reset_state(uint agent_id,
+						       uint reset_id)
+{
+	size_t n;
+
+	if (agent_id == 0) {
+		for (n = 0; n < ARRAY_SIZE(scmi0_reset); n++)
+			if (scmi0_reset[n].id == reset_id)
+				return scmi0_reset + n;
+	}
+
+	return NULL;
+}
+
+/*
+ * Sandbox SCMI agent ops
+ */
+
+static int sandbox_scmi_clock_rate_set(struct udevice *dev,
+				       struct scmi_msg *msg)
+{
+	struct sandbox_scmi_agent *agent = dev_get_priv(dev);
+	struct scmi_clk_rate_set_in *in = NULL;
+	struct scmi_clk_rate_set_out *out = NULL;
+	struct sandbox_scmi_clk *clk_state = NULL;
+
+	if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
+	    !msg->out_msg || msg->out_msg_sz < sizeof(*out))
+		return -EINVAL;
+
+	in = (struct scmi_clk_rate_set_in *)msg->in_msg;
+	out = (struct scmi_clk_rate_set_out *)msg->out_msg;
+
+	clk_state = get_scmi_clk_state(agent->idx, in->clock_id);
+	if (!clk_state) {
+		dev_err(dev, "Unexpected clock ID %u\n", in->clock_id);
+
+		out->status = SCMI_NOT_FOUND;
+	} else {
+		u64 rate = ((u64)in->rate_msb << 32) + in->rate_lsb;
+
+		clk_state->rate = (ulong)rate;
+
+		out->status = SCMI_SUCCESS;
+	}
+
+	return 0;
+}
+
+static int sandbox_scmi_clock_rate_get(struct udevice *dev,
+				       struct scmi_msg *msg)
+{
+	struct sandbox_scmi_agent *agent = dev_get_priv(dev);
+	struct scmi_clk_rate_get_in *in = NULL;
+	struct scmi_clk_rate_get_out *out = NULL;
+	struct sandbox_scmi_clk *clk_state = NULL;
+
+	if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
+	    !msg->out_msg || msg->out_msg_sz < sizeof(*out))
+		return -EINVAL;
+
+	in = (struct scmi_clk_rate_get_in *)msg->in_msg;
+	out = (struct scmi_clk_rate_get_out *)msg->out_msg;
+
+	clk_state = get_scmi_clk_state(agent->idx, in->clock_id);
+	if (!clk_state) {
+		dev_err(dev, "Unexpected clock ID %u\n", in->clock_id);
+
+		out->status = SCMI_NOT_FOUND;
+	} else {
+		out->rate_msb = (u32)((u64)clk_state->rate >> 32);
+		out->rate_lsb = (u32)clk_state->rate;
+
+		out->status = SCMI_SUCCESS;
+	}
+
+	return 0;
+}
+
+static int sandbox_scmi_clock_gate(struct udevice *dev, struct scmi_msg *msg)
+{
+	struct sandbox_scmi_agent *agent = dev_get_priv(dev);
+	struct scmi_clk_state_in *in = NULL;
+	struct scmi_clk_state_out *out = NULL;
+	struct sandbox_scmi_clk *clk_state = NULL;
+
+	if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
+	    !msg->out_msg || msg->out_msg_sz < sizeof(*out))
+		return -EINVAL;
+
+	in = (struct scmi_clk_state_in *)msg->in_msg;
+	out = (struct scmi_clk_state_out *)msg->out_msg;
+
+	clk_state = get_scmi_clk_state(agent->idx, in->clock_id);
+	if (!clk_state) {
+		dev_err(dev, "Unexpected clock ID %u\n", in->clock_id);
+
+		out->status = SCMI_NOT_FOUND;
+	} else if (in->attributes > 1) {
+		out->status = SCMI_PROTOCOL_ERROR;
+	} else {
+		clk_state->enabled = in->attributes;
+
+		out->status = SCMI_SUCCESS;
+	}
+
+	return 0;
+}
+
+static int sandbox_scmi_rd_attribs(struct udevice *dev, struct scmi_msg *msg)
+{
+	struct sandbox_scmi_agent *agent = dev_get_priv(dev);
+	struct scmi_rd_attr_in *in = NULL;
+	struct scmi_rd_attr_out *out = NULL;
+	struct sandbox_scmi_reset *reset_state = NULL;
+
+	if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
+	    !msg->out_msg || msg->out_msg_sz < sizeof(*out))
+		return -EINVAL;
+
+	in = (struct scmi_rd_attr_in *)msg->in_msg;
+	out = (struct scmi_rd_attr_out *)msg->out_msg;
+
+	reset_state = get_scmi_reset_state(agent->idx, in->domain_id);
+	if (!reset_state) {
+		dev_err(dev, "Unexpected reset domain ID %u\n", in->domain_id);
+
+		out->status = SCMI_NOT_FOUND;
+	} else {
+		memset(out, 0, sizeof(*out));
+		snprintf(out->name, sizeof(out->name), "rd%u", in->domain_id);
+
+		out->status = SCMI_SUCCESS;
+	}
+
+	return 0;
+}
+
+static int sandbox_scmi_rd_reset(struct udevice *dev, struct scmi_msg *msg)
+{
+	struct sandbox_scmi_agent *agent = dev_get_priv(dev);
+	struct scmi_rd_reset_in *in = NULL;
+	struct scmi_rd_reset_out *out = NULL;
+	struct sandbox_scmi_reset *reset_state = NULL;
+
+	if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
+	    !msg->out_msg || msg->out_msg_sz < sizeof(*out))
+		return -EINVAL;
+
+	in = (struct scmi_rd_reset_in *)msg->in_msg;
+	out = (struct scmi_rd_reset_out *)msg->out_msg;
+
+	reset_state = get_scmi_reset_state(agent->idx, in->domain_id);
+	if (!reset_state) {
+		dev_err(dev, "Unexpected reset domain ID %u\n", in->domain_id);
+
+		out->status = SCMI_NOT_FOUND;
+	} else if (in->reset_state > 1) {
+		dev_err(dev, "Invalid reset domain input attribute value\n");
+
+		out->status = SCMI_INVALID_PARAMETERS;
+	} else {
+		if (in->flags & SCMI_RD_RESET_FLAG_CYCLE) {
+			if (in->flags & SCMI_RD_RESET_FLAG_ASYNC) {
+				out->status = SCMI_NOT_SUPPORTED;
+			} else {
+				/* Ends deasserted whatever current state */
+				reset_state->asserted = false;
+				out->status = SCMI_SUCCESS;
+			}
+		} else {
+			reset_state->asserted = in->flags &
+						SCMI_RD_RESET_FLAG_ASSERT;
+
+			out->status = SCMI_SUCCESS;
+		}
+	}
+
+	return 0;
+}
+
+static int sandbox_scmi_test_process_msg(struct udevice *dev,
+					 struct scmi_msg *msg)
+{
+	switch (msg->protocol_id) {
+	case SCMI_PROTOCOL_ID_CLOCK:
+		switch (msg->message_id) {
+		case SCMI_CLOCK_RATE_SET:
+			return sandbox_scmi_clock_rate_set(dev, msg);
+		case SCMI_CLOCK_RATE_GET:
+			return sandbox_scmi_clock_rate_get(dev, msg);
+		case SCMI_CLOCK_CONFIG_SET:
+			return sandbox_scmi_clock_gate(dev, msg);
+		default:
+			break;
+		}
+		break;
+	case SCMI_PROTOCOL_ID_RESET_DOMAIN:
+		switch (msg->message_id) {
+		case SCMI_RESET_DOMAIN_ATTRIBUTES:
+			return sandbox_scmi_rd_attribs(dev, msg);
+		case SCMI_RESET_DOMAIN_RESET:
+			return sandbox_scmi_rd_reset(dev, msg);
+		default:
+			break;
+		}
+		break;
+	case SCMI_PROTOCOL_ID_BASE:
+	case SCMI_PROTOCOL_ID_POWER_DOMAIN:
+	case SCMI_PROTOCOL_ID_SYSTEM:
+	case SCMI_PROTOCOL_ID_PERF:
+	case SCMI_PROTOCOL_ID_SENSOR:
+		*(u32 *)msg->out_msg = SCMI_NOT_SUPPORTED;
+		return 0;
+	default:
+		break;
+	}
+
+	dev_err(dev, "%s(%s): Unhandled protocol_id %#x/message_id %#x\n",
+		__func__, dev->name, msg->protocol_id, msg->message_id);
+
+	if (msg->out_msg_sz < sizeof(u32))
+		return -EINVAL;
+
+	/* Intentionnaly report unhandled IDs through the SCMI return code */
+	*(u32 *)msg->out_msg = SCMI_PROTOCOL_ERROR;
+	return 0;
+}
+
+static int sandbox_scmi_test_remove(struct udevice *dev)
+{
+	struct sandbox_scmi_agent *agent = dev_get_priv(dev);
+
+	debug_print_agent_state(dev, "removed");
+
+	/* We only need to dereference the agent in the context */
+	sandbox_scmi_service_ctx()->agent[agent->idx] = NULL;
+
+	return 0;
+}
+
+static int sandbox_scmi_test_probe(struct udevice *dev)
+{
+	static const char basename[] = "sandbox-scmi-agent@";
+	struct sandbox_scmi_agent *agent = dev_get_priv(dev);
+	const size_t basename_size = sizeof(basename) - 1;
+
+	if (strncmp(basename, dev->name, basename_size))
+		return -ENOENT;
+
+	switch (dev->name[basename_size]) {
+	case '0':
+		*agent = (struct sandbox_scmi_agent){
+			.idx = 0,
+			.clk = scmi0_clk,
+			.clk_count = ARRAY_SIZE(scmi0_clk),
+			.reset = scmi0_reset,
+			.reset_count = ARRAY_SIZE(scmi0_reset),
+		};
+		break;
+	case '1':
+		*agent = (struct sandbox_scmi_agent){
+			.idx = 1,
+			.clk = scmi1_clk,
+			.clk_count = ARRAY_SIZE(scmi1_clk),
+		};
+		break;
+	default:
+		dev_err(dev, "%s(): Unexpected agent ID %s\n",
+			__func__, dev->name + basename_size);
+		return -ENOENT;
+	}
+
+	debug_print_agent_state(dev, "probed");
+
+	/* Save reference for tests purpose */
+	sandbox_scmi_service_ctx()->agent[agent->idx] = agent;
+
+	return 0;
+};
+
+static const struct udevice_id sandbox_scmi_test_ids[] = {
+	{ .compatible = "sandbox,scmi-agent" },
+	{ }
+};
+
+struct scmi_agent_ops sandbox_scmi_test_ops = {
+	.process_msg = sandbox_scmi_test_process_msg,
+};
+
+U_BOOT_DRIVER(sandbox_scmi_agent) = {
+	.name = "sandbox-scmi_agent",
+	.id = UCLASS_SCMI_AGENT,
+	.of_match = sandbox_scmi_test_ids,
+	.priv_auto_alloc_size = sizeof(struct sandbox_scmi_agent),
+	.probe = sandbox_scmi_test_probe,
+	.remove = sandbox_scmi_test_remove,
+	.ops = &sandbox_scmi_test_ops,
+};
diff --git a/drivers/firmware/scmi/sandbox-scmi_devices.c b/drivers/firmware/scmi/sandbox-scmi_devices.c
new file mode 100644
index 0000000000..c69967bf69
--- /dev/null
+++ b/drivers/firmware/scmi/sandbox-scmi_devices.c
@@ -0,0 +1,113 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020, Linaro Limited
+ */
+
+#include <common.h>
+#include <clk.h>
+#include <dm.h>
+#include <malloc.h>
+#include <reset.h>
+#include <asm/io.h>
+#include <asm/scmi_test.h>
+#include <dm/device_compat.h>
+
+/*
+ * Simulate to some extent a SCMI exchange.
+ * This drivers gets SCMI resources and offers API function to the
+ * SCMI test sequence manipulate the resources, currently clock
+ * and reset controllers.
+ */
+
+#define SCMI_TEST_DEVICES_CLK_COUNT		3
+#define SCMI_TEST_DEVICES_RD_COUNT		1
+
+/*
+ * struct sandbox_scmi_device_priv - Storage for device handles used by test
+ * @clk:		Array of clock instances used by tests
+ * @reset_clt:		Array of the reset controller instances used by tests
+ * @devices:		Resources exposed by sandbox_scmi_devices_ctx()
+ */
+struct sandbox_scmi_device_priv {
+	struct clk clk[SCMI_TEST_DEVICES_CLK_COUNT];
+	struct reset_ctl reset_ctl[SCMI_TEST_DEVICES_RD_COUNT];
+	struct sandbox_scmi_devices devices;
+};
+
+struct sandbox_scmi_devices *sandbox_scmi_devices_ctx(struct udevice *dev)
+{
+	struct sandbox_scmi_device_priv *priv = dev_get_priv(dev);
+
+	if (priv)
+		return &priv->devices;
+
+	return NULL;
+}
+
+static int sandbox_scmi_devices_remove(struct udevice *dev)
+{
+	struct sandbox_scmi_devices *devices = sandbox_scmi_devices_ctx(dev);
+	int ret = 0;
+	size_t n;
+
+	for (n = 0; n < SCMI_TEST_DEVICES_RD_COUNT; n++) {
+		int ret2 = reset_free(devices->reset + n);
+
+		if (ret2 && !ret)
+			ret = ret2;
+	}
+
+	return ret;
+}
+
+static int sandbox_scmi_devices_probe(struct udevice *dev)
+{
+	struct sandbox_scmi_device_priv *priv = dev_get_priv(dev);
+	int ret;
+	size_t n;
+
+	priv->devices = (struct sandbox_scmi_devices){
+		.clk = priv->clk,
+		.clk_count = SCMI_TEST_DEVICES_CLK_COUNT,
+		.reset = priv->reset_ctl,
+		.reset_count = SCMI_TEST_DEVICES_RD_COUNT,
+	};
+
+	for (n = 0; n < SCMI_TEST_DEVICES_CLK_COUNT; n++) {
+		ret = clk_get_by_index(dev, n, priv->devices.clk + n);
+		if (ret) {
+			dev_err(dev, "%s: Failed on clk %zu\n", __func__, n);
+			return ret;
+		}
+	}
+
+	for (n = 0; n < SCMI_TEST_DEVICES_RD_COUNT; n++) {
+		ret = reset_get_by_index(dev, n, priv->devices.reset + n);
+		if (ret) {
+			dev_err(dev, "%s: Failed on reset %zu\n", __func__, n);
+			goto err_reset;
+		}
+	}
+
+	return 0;
+
+err_reset:
+	for (; n > 0; n--)
+		reset_free(priv->devices.reset + n - 1);
+
+	return ret;
+}
+
+static const struct udevice_id sandbox_scmi_devices_ids[] = {
+	{ .compatible = "sandbox,scmi-devices" },
+	{ }
+};
+
+U_BOOT_DRIVER(sandbox_scmi_devices) = {
+	.name = "sandbox-scmi_devices",
+	.id = UCLASS_MISC,
+	.of_match = sandbox_scmi_devices_ids,
+	.priv_auto_alloc_size = sizeof(struct sandbox_scmi_device_priv),
+	.remove = sandbox_scmi_devices_remove,
+	.probe = sandbox_scmi_devices_probe,
+};
diff --git a/drivers/firmware/scmi/scmi_agent-uclass.c b/drivers/firmware/scmi/scmi_agent-uclass.c
new file mode 100644
index 0000000000..77160b1999
--- /dev/null
+++ b/drivers/firmware/scmi/scmi_agent-uclass.c
@@ -0,0 +1,119 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2020 Linaro Limited.
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <scmi_agent-uclass.h>
+#include <scmi_protocols.h>
+
+#include <dm/device-internal.h>
+#include <linux/compat.h>
+
+/**
+ * struct error_code - Helper structure for SCMI error code conversion
+ * @scmi:	SCMI error code
+ * @errno:	Related standard error number
+ */
+struct error_code {
+	int scmi;
+	int errno;
+};
+
+static const struct error_code scmi_linux_errmap[] = {
+	{ .scmi = SCMI_NOT_SUPPORTED, .errno = -EOPNOTSUPP, },
+	{ .scmi = SCMI_INVALID_PARAMETERS, .errno = -EINVAL, },
+	{ .scmi = SCMI_DENIED, .errno = -EACCES, },
+	{ .scmi = SCMI_NOT_FOUND, .errno = -ENOENT, },
+	{ .scmi = SCMI_OUT_OF_RANGE, .errno = -ERANGE, },
+	{ .scmi = SCMI_BUSY, .errno = -EBUSY, },
+	{ .scmi = SCMI_COMMS_ERROR, .errno = -ECOMM, },
+	{ .scmi = SCMI_GENERIC_ERROR, .errno = -EIO, },
+	{ .scmi = SCMI_HARDWARE_ERROR, .errno = -EREMOTEIO, },
+	{ .scmi = SCMI_PROTOCOL_ERROR, .errno = -EPROTO, },
+};
+
+int scmi_to_linux_errno(s32 scmi_code)
+{
+	int n;
+
+	if (!scmi_code)
+		return 0;
+
+	for (n = 0; n < ARRAY_SIZE(scmi_linux_errmap); n++)
+		if (scmi_code == scmi_linux_errmap[n].scmi)
+			return scmi_linux_errmap[1].errno;
+
+	return -EPROTO;
+}
+
+/*
+ * SCMI agent devices binds devices of various uclasses depeding on
+ * the FDT description. scmi_bind_protocol() is a generic bind sequence
+ * called by the uclass at bind stage, that is uclass post_bind.
+ */
+static int scmi_bind_protocols(struct udevice *dev)
+{
+	int ret = 0;
+	ofnode node;
+
+	dev_for_each_subnode(node, dev) {
+		struct driver *drv = NULL;
+		u32 protocol_id;
+
+		if (!ofnode_is_available(node))
+			continue;
+
+		if (ofnode_read_u32(node, "reg", &protocol_id))
+			continue;
+
+		switch (protocol_id) {
+		case SCMI_PROTOCOL_ID_CLOCK:
+			if (IS_ENABLED(CONFIG_CLK_SCMI))
+				drv = DM_GET_DRIVER(scmi_clock);
+			break;
+		case SCMI_PROTOCOL_ID_RESET_DOMAIN:
+			if (IS_ENABLED(CONFIG_RESET_SCMI))
+				drv = DM_GET_DRIVER(scmi_reset_domain);
+			break;
+		default:
+			break;
+		}
+
+		if (!drv) {
+			dev_dbg(dev, "Ignore unsupported SCMI protocol %#x\n",
+				protocol_id);
+			continue;
+		}
+
+		ret = device_bind_ofnode(dev, drv, ofnode_get_name(node),
+					 NULL, node, NULL);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+
+static const struct scmi_agent_ops *transport_dev_ops(struct udevice *dev)
+{
+	return (const struct scmi_agent_ops *)dev->driver->ops;
+}
+
+int devm_scmi_process_msg(struct udevice *dev, struct scmi_msg *msg)
+{
+	const struct scmi_agent_ops *ops = transport_dev_ops(dev);
+
+	if (ops->process_msg)
+		return ops->process_msg(dev, msg);
+
+	return -EPROTONOSUPPORT;
+}
+
+UCLASS_DRIVER(scmi_agent) = {
+	.id		= UCLASS_SCMI_AGENT,
+	.name		= "scmi_agent",
+	.post_bind	= scmi_bind_protocols,
+};
diff --git a/drivers/firmware/scmi/smccc_agent.c b/drivers/firmware/scmi/smccc_agent.c
new file mode 100644
index 0000000000..85dbf9195e
--- /dev/null
+++ b/drivers/firmware/scmi/smccc_agent.c
@@ -0,0 +1,89 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2020 Linaro Limited.
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <scmi_agent.h>
+#include <scmi_agent-uclass.h>
+#include <dm/devres.h>
+#include <dm/device-internal.h>
+#include <linux/arm-smccc.h>
+#include <linux/compat.h>
+
+#include "smt.h"
+
+#define SMCCC_RET_NOT_SUPPORTED         ((unsigned long)-1)
+
+/**
+ * struct scmi_smccc_channel - Description of an SCMI SMCCC transport
+ * @func_id:	SMCCC function ID used by the SCMI transport
+ * @smt:	Shared memory buffer
+ */
+struct scmi_smccc_channel {
+	ulong func_id;
+	struct scmi_smt smt;
+};
+
+static int scmi_smccc_process_msg(struct udevice *dev, struct scmi_msg *msg)
+{
+	struct scmi_smccc_channel *chan = dev_get_priv(dev);
+	struct arm_smccc_res res;
+	int ret;
+
+	ret = scmi_write_msg_to_smt(dev, &chan->smt, msg);
+	if (ret)
+		return ret;
+
+	arm_smccc_smc(chan->func_id, 0, 0, 0, 0, 0, 0, 0, &res);
+	if (res.a0 == SMCCC_RET_NOT_SUPPORTED)
+		ret = -ENXIO;
+	else
+		ret = scmi_read_resp_from_smt(dev, &chan->smt, msg);
+
+	scmi_clear_smt_channel(&chan->smt);
+
+	return ret;
+}
+
+static int scmi_smccc_probe(struct udevice *dev)
+{
+	struct scmi_smccc_channel *chan = dev_get_priv(dev);
+	u32 func_id;
+	int ret;
+
+	if (dev_read_u32(dev, "arm,smc-id", &func_id)) {
+		dev_err(dev, "Missing property func-id\n");
+		return -EINVAL;
+	}
+
+	chan->func_id = func_id;
+
+	ret = scmi_dt_get_smt_buffer(dev, &chan->smt);
+	if (ret) {
+		dev_err(dev, "Failed to get smt resources: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct udevice_id scmi_smccc_ids[] = {
+	{ .compatible = "arm,scmi-smc" },
+	{ }
+};
+
+static const struct scmi_agent_ops scmi_smccc_ops = {
+	.process_msg = scmi_smccc_process_msg,
+};
+
+U_BOOT_DRIVER(scmi_smccc) = {
+	.name		= "scmi-over-smccc",
+	.id		= UCLASS_SCMI_AGENT,
+	.of_match	= scmi_smccc_ids,
+	.priv_auto_alloc_size = sizeof(struct scmi_smccc_channel),
+	.probe		= scmi_smccc_probe,
+	.ops		= &scmi_smccc_ops,
+};
diff --git a/drivers/firmware/scmi/smt.c b/drivers/firmware/scmi/smt.c
new file mode 100644
index 0000000000..ce8fe49939
--- /dev/null
+++ b/drivers/firmware/scmi/smt.c
@@ -0,0 +1,139 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (C) 2019-2020 Linaro Limited.
+ */
+
+#include <common.h>
+#include <cpu_func.h>
+#include <dm.h>
+#include <errno.h>
+#include <scmi_agent.h>
+#include <asm/cache.h>
+#include <asm/system.h>
+#include <dm/ofnode.h>
+#include <linux/compat.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+
+#include "smt.h"
+
+/**
+ * Get shared memory configuration defined by the referred DT phandle
+ * Return with a errno compliant value.
+ */
+int scmi_dt_get_smt_buffer(struct udevice *dev, struct scmi_smt *smt)
+{
+	int ret;
+	struct ofnode_phandle_args args;
+	struct resource resource;
+	fdt32_t faddr;
+	phys_addr_t paddr;
+
+	ret = dev_read_phandle_with_args(dev, "shmem", NULL, 0, 0, &args);
+	if (ret)
+		return ret;
+
+	ret = ofnode_read_resource(args.node, 0, &resource);
+	if (ret)
+		return ret;
+
+	faddr = cpu_to_fdt32(resource.start);
+	paddr = ofnode_translate_address(args.node, &faddr);
+
+	smt->size = resource_size(&resource);
+	if (smt->size < sizeof(struct scmi_smt_header)) {
+		dev_err(dev, "Shared memory buffer too small\n");
+		return -EINVAL;
+	}
+
+	smt->buf = devm_ioremap(dev, paddr, smt->size);
+	if (!smt->buf)
+		return -ENOMEM;
+
+#ifdef CONFIG_ARM
+	if (dcache_status())
+		mmu_set_region_dcache_behaviour((uintptr_t)smt->buf,
+						smt->size, DCACHE_OFF);
+#endif
+
+	return 0;
+}
+
+/**
+ * Write SCMI message @msg into a SMT shared buffer @smt.
+ * Return 0 on success and with a negative errno in case of error.
+ */
+int scmi_write_msg_to_smt(struct udevice *dev, struct scmi_smt *smt,
+			  struct scmi_msg *msg)
+{
+	struct scmi_smt_header *hdr = (void *)smt->buf;
+
+	if ((!msg->in_msg && msg->in_msg_sz) ||
+	    (!msg->out_msg && msg->out_msg_sz))
+		return -EINVAL;
+
+	if (!(hdr->channel_status & SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE)) {
+		dev_dbg(dev, "Channel busy\n");
+		return -EBUSY;
+	}
+
+	if (smt->size < (sizeof(*hdr) + msg->in_msg_sz) ||
+	    smt->size < (sizeof(*hdr) + msg->out_msg_sz)) {
+		dev_dbg(dev, "Buffer too small\n");
+		return -ETOOSMALL;
+	}
+
+	/* Load message in shared memory */
+	hdr->channel_status &= ~SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE;
+	hdr->length = msg->in_msg_sz + sizeof(hdr->msg_header);
+	hdr->msg_header = SMT_HEADER_TOKEN(0) |
+			  SMT_HEADER_MESSAGE_TYPE(0) |
+			  SMT_HEADER_PROTOCOL_ID(msg->protocol_id) |
+			  SMT_HEADER_MESSAGE_ID(msg->message_id);
+
+	memcpy_toio(hdr->msg_payload, msg->in_msg, msg->in_msg_sz);
+
+	return 0;
+}
+
+/**
+ * Read SCMI message from a SMT shared buffer @smt and copy it into @msg.
+ * Return 0 on success and with a negative errno in case of error.
+ */
+int scmi_read_resp_from_smt(struct udevice *dev, struct scmi_smt *smt,
+			    struct scmi_msg *msg)
+{
+	struct scmi_smt_header *hdr = (void *)smt->buf;
+
+	if (!(hdr->channel_status & SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE)) {
+		dev_err(dev, "Channel unexpectedly busy\n");
+		return -EBUSY;
+	}
+
+	if (hdr->channel_status & SCMI_SHMEM_CHAN_STAT_CHANNEL_ERROR) {
+		dev_err(dev, "Channel error reported, reset channel\n");
+		return -ECOMM;
+	}
+
+	if (hdr->length > msg->out_msg_sz + sizeof(hdr->msg_header)) {
+		dev_err(dev, "Buffer to small\n");
+		return -ETOOSMALL;
+	}
+
+	/* Get the data */
+	msg->out_msg_sz = hdr->length - sizeof(hdr->msg_header);
+	memcpy_fromio(msg->out_msg, hdr->msg_payload, msg->out_msg_sz);
+
+	return 0;
+}
+
+/**
+ * Clear SMT flags in shared buffer to allow further message exchange
+ */
+void scmi_clear_smt_channel(struct scmi_smt *smt)
+{
+	struct scmi_smt_header *hdr = (void *)smt->buf;
+
+	hdr->channel_status &= ~SCMI_SHMEM_CHAN_STAT_CHANNEL_ERROR;
+}
diff --git a/drivers/firmware/scmi/smt.h b/drivers/firmware/scmi/smt.h
new file mode 100644
index 0000000000..a8c0987bd3
--- /dev/null
+++ b/drivers/firmware/scmi/smt.h
@@ -0,0 +1,86 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (C) 2019-2020 Linaro Limited.
+ */
+#ifndef SCMI_SMT_H
+#define SCMI_SMT_H
+
+#include <asm/types.h>
+
+/**
+ * struct scmi_smt_header - Description of the shared memory message buffer
+ *
+ * SMT stands for Shared Memory based Transport.
+ * SMT uses 28 byte header prior message payload to handle the state of
+ * the communication channel realized by the shared memory area and
+ * to define SCMI protocol information the payload relates to.
+ */
+struct scmi_smt_header {
+	__le32 reserved;
+	__le32 channel_status;
+#define SCMI_SHMEM_CHAN_STAT_CHANNEL_ERROR	BIT(1)
+#define SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE	BIT(0)
+	__le32 reserved1[2];
+	__le32 flags;
+#define SCMI_SHMEM_FLAG_INTR_ENABLED		BIT(0)
+	__le32 length;
+	__le32 msg_header;
+	u8 msg_payload[0];
+};
+
+#define SMT_HEADER_TOKEN(token)		(((token) << 18) & GENMASK(31, 18))
+#define SMT_HEADER_PROTOCOL_ID(proto)	(((proto) << 10) & GENMASK(17, 10))
+#define SMT_HEADER_MESSAGE_TYPE(type)	(((type) << 18) & GENMASK(9, 8))
+#define SMT_HEADER_MESSAGE_ID(id)	((id) & GENMASK(7, 0))
+
+/**
+ * struct scmi_smt - Description of a SMT memory buffer
+ * @buf:	Shared memory base address
+ * @size:	Shared memory byte size
+ */
+struct scmi_smt {
+	u8 *buf;
+	size_t size;
+};
+
+static inline bool scmi_smt_channel_is_free(struct scmi_smt *smt)
+{
+	struct scmi_smt_header *hdr = (void *)smt->buf;
+
+	return hdr->channel_status & SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE;
+}
+
+static inline bool scmi_smt_channel_reports_error(struct scmi_smt *smt)
+{
+	struct scmi_smt_header *hdr = (void *)smt->buf;
+
+	return hdr->channel_status & SCMI_SHMEM_CHAN_STAT_CHANNEL_ERROR;
+}
+
+static inline void scmi_smt_get_channel(struct scmi_smt *smt)
+{
+	struct scmi_smt_header *hdr = (void *)smt->buf;
+
+	hdr->channel_status &= ~SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE;
+}
+
+static inline void scmi_smt_put_channel(struct scmi_smt *smt)
+{
+	struct scmi_smt_header *hdr = (void *)smt->buf;
+
+	hdr->channel_status |= SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE;
+	hdr->channel_status &= ~SCMI_SHMEM_CHAN_STAT_CHANNEL_ERROR;
+}
+
+int scmi_dt_get_smt_buffer(struct udevice *dev, struct scmi_smt *smt);
+
+int scmi_write_msg_to_smt(struct udevice *dev, struct scmi_smt *smt,
+			  struct scmi_msg *msg);
+
+int scmi_read_resp_from_smt(struct udevice *dev, struct scmi_smt *smt,
+			    struct scmi_msg *msg);
+
+void scmi_clear_smt_channel(struct scmi_smt *smt);
+
+#endif /* SCMI_SMT_H */
diff --git a/drivers/gpio/stm32_gpio.c b/drivers/gpio/stm32_gpio.c
index 5bff27f75b..5597247164 100644
--- a/drivers/gpio/stm32_gpio.c
+++ b/drivers/gpio/stm32_gpio.c
@@ -210,11 +210,11 @@ static int stm32_gpio_set_dir_flags(struct udevice *dev, unsigned int offset,
 
 	} else if (flags & GPIOD_IS_IN) {
 		stm32_gpio_set_moder(regs, idx, STM32_GPIO_MODE_IN);
-		if (flags & GPIOD_PULL_UP)
-			stm32_gpio_set_pupd(regs, idx, STM32_GPIO_PUPD_UP);
-		else if (flags & GPIOD_PULL_DOWN)
-			stm32_gpio_set_pupd(regs, idx, STM32_GPIO_PUPD_DOWN);
 	}
+	if (flags & GPIOD_PULL_UP)
+		stm32_gpio_set_pupd(regs, idx, STM32_GPIO_PUPD_UP);
+	else if (flags & GPIOD_PULL_DOWN)
+		stm32_gpio_set_pupd(regs, idx, STM32_GPIO_PUPD_DOWN);
 
 	return 0;
 }
@@ -241,16 +241,16 @@ static int stm32_gpio_get_dir_flags(struct udevice *dev, unsigned int offset,
 		break;
 	case STM32_GPIO_MODE_IN:
 		dir_flags |= GPIOD_IS_IN;
-		switch (stm32_gpio_get_pupd(regs, idx)) {
-		case STM32_GPIO_PUPD_UP:
-			dir_flags |= GPIOD_PULL_UP;
-			break;
-		case STM32_GPIO_PUPD_DOWN:
-			dir_flags |= GPIOD_PULL_DOWN;
-			break;
-		default:
-			break;
-		}
+		break;
+	default:
+		break;
+	}
+	switch (stm32_gpio_get_pupd(regs, idx)) {
+	case STM32_GPIO_PUPD_UP:
+		dir_flags |= GPIOD_PULL_UP;
+		break;
+	case STM32_GPIO_PUPD_DOWN:
+		dir_flags |= GPIOD_PULL_DOWN;
 		break;
 	default:
 		break;
diff --git a/drivers/mtd/nand/core.c b/drivers/mtd/nand/core.c
index 6fbd24ba74..ef7f5f07d5 100644
--- a/drivers/mtd/nand/core.c
+++ b/drivers/mtd/nand/core.c
@@ -10,6 +10,7 @@
 #define pr_fmt(fmt)	"nand: " fmt
 
 #include <common.h>
+#include <watchdog.h>
 #ifndef __UBOOT__
 #include <linux/compat.h>
 #include <linux/module.h>
@@ -164,6 +165,7 @@ int nanddev_mtd_erase(struct mtd_info *mtd, struct erase_info *einfo)
 	nanddev_offs_to_pos(nand, einfo->addr, &pos);
 	nanddev_offs_to_pos(nand, einfo->addr + einfo->len - 1, &last);
 	while (nanddev_pos_cmp(&pos, &last) <= 0) {
+		WATCHDOG_RESET();
 		ret = nanddev_erase(nand, &pos);
 		if (ret) {
 			einfo->fail_addr = nanddev_pos_to_offs(nand, &pos);
diff --git a/drivers/mtd/nand/spi/core.c b/drivers/mtd/nand/spi/core.c
index 93371fdde0..dbf2db6d00 100644
--- a/drivers/mtd/nand/spi/core.c
+++ b/drivers/mtd/nand/spi/core.c
@@ -24,6 +24,7 @@
 #include <errno.h>
 #include <spi.h>
 #include <spi-mem.h>
+#include <watchdog.h>
 #include <dm/device_compat.h>
 #include <dm/devres.h>
 #include <linux/bitops.h>
@@ -578,6 +579,7 @@ static int spinand_mtd_read(struct mtd_info *mtd, loff_t from,
 #endif
 
 	nanddev_io_for_each_page(nand, from, ops, &iter) {
+		WATCHDOG_RESET();
 		ret = spinand_select_target(spinand, iter.req.pos.target);
 		if (ret)
 			break;
@@ -629,6 +631,7 @@ static int spinand_mtd_write(struct mtd_info *mtd, loff_t to,
 #endif
 
 	nanddev_io_for_each_page(nand, to, ops, &iter) {
+		WATCHDOG_RESET();
 		ret = spinand_select_target(spinand, iter.req.pos.target);
 		if (ret)
 			break;
diff --git a/drivers/mtd/spi/spi-nor-core.c b/drivers/mtd/spi/spi-nor-core.c
index 0113e70037..81de0de0c5 100644
--- a/drivers/mtd/spi/spi-nor-core.c
+++ b/drivers/mtd/spi/spi-nor-core.c
@@ -11,6 +11,7 @@
 
 #include <common.h>
 #include <log.h>
+#include <watchdog.h>
 #include <dm/device_compat.h>
 #include <dm/devres.h>
 #include <linux/bitops.h>
@@ -565,6 +566,7 @@ static int spi_nor_erase(struct mtd_info *mtd, struct erase_info *instr)
 	len = instr->len;
 
 	while (len) {
+		WATCHDOG_RESET();
 #ifdef CONFIG_SPI_FLASH_BAR
 		ret = write_bar(nor, addr);
 		if (ret < 0)
@@ -1249,6 +1251,7 @@ static int spi_nor_write(struct mtd_info *mtd, loff_t to, size_t len,
 	for (i = 0; i < len; ) {
 		ssize_t written;
 		loff_t addr = to + i;
+		WATCHDOG_RESET();
 
 		/*
 		 * If page_size is a power of two, the offset can be quickly
diff --git a/drivers/phy/phy-stm32-usbphyc.c b/drivers/phy/phy-stm32-usbphyc.c
index c6d3048602..d4a82f73f4 100644
--- a/drivers/phy/phy-stm32-usbphyc.c
+++ b/drivers/phy/phy-stm32-usbphyc.c
@@ -59,6 +59,7 @@ struct stm32_usbphyc {
 	struct udevice *vdda1v8;
 	struct stm32_usbphyc_phy {
 		struct udevice *vdd;
+		struct udevice *vbus;
 		bool init;
 		bool powered;
 	} phys[MAX_PHYS];
@@ -244,6 +245,11 @@ static int stm32_usbphyc_phy_power_on(struct phy *phy)
 		if (ret)
 			return ret;
 	}
+	if (usbphyc_phy->vbus) {
+		ret = regulator_set_enable(usbphyc_phy->vbus, true);
+		if (ret)
+			return ret;
+	}
 
 	usbphyc_phy->powered = true;
 
@@ -262,6 +268,11 @@ static int stm32_usbphyc_phy_power_off(struct phy *phy)
 	if (stm32_usbphyc_is_powered(usbphyc))
 		return 0;
 
+	if (usbphyc_phy->vbus) {
+		ret = regulator_set_enable(usbphyc_phy->vbus, false);
+		if (ret)
+			return ret;
+	}
 	if (usbphyc_phy->vdd) {
 		ret = regulator_set_enable_if_allowed(usbphyc_phy->vdd, false);
 		if (ret)
@@ -271,7 +282,7 @@ static int stm32_usbphyc_phy_power_off(struct phy *phy)
 	return 0;
 }
 
-static int stm32_usbphyc_get_regulator(struct udevice *dev, ofnode node,
+static int stm32_usbphyc_get_regulator(ofnode node,
 				       char *supply_name,
 				       struct udevice **regulator)
 {
@@ -281,19 +292,14 @@ static int stm32_usbphyc_get_regulator(struct udevice *dev, ofnode node,
 	ret = ofnode_parse_phandle_with_args(node, supply_name,
 					     NULL, 0, 0,
 					     &regulator_phandle);
-	if (ret) {
-		dev_err(dev, "Can't find %s property (%d)\n", supply_name, ret);
+	if (ret)
 		return ret;
-	}
 
 	ret = uclass_get_device_by_ofnode(UCLASS_REGULATOR,
 					  regulator_phandle.node,
 					  regulator);
-
-	if (ret) {
-		dev_err(dev, "Can't get %s regulator (%d)\n", supply_name, ret);
+	if (ret)
 		return ret;
-	}
 
 	return 0;
 }
@@ -380,10 +386,17 @@ static int stm32_usbphyc_probe(struct udevice *dev)
 
 		usbphyc_phy->init = false;
 		usbphyc_phy->powered = false;
-		ret = stm32_usbphyc_get_regulator(dev, node, "phy-supply",
+		ret = stm32_usbphyc_get_regulator(node, "phy-supply",
 						  &usbphyc_phy->vdd);
-		if (ret)
+		if (ret) {
+			dev_err(dev, "Can't get phy-supply regulator\n");
 			return ret;
+		}
+
+		ret = stm32_usbphyc_get_regulator(node, "vbus-supply",
+						  &usbphyc_phy->vbus);
+		if (ret)
+			usbphyc_phy->vbus = NULL;
 
 		node = dev_read_next_subnode(node);
 	}
diff --git a/drivers/pinctrl/pinctrl-stmfx.c b/drivers/pinctrl/pinctrl-stmfx.c
index c2ea82770e..fbbd479575 100644
--- a/drivers/pinctrl/pinctrl-stmfx.c
+++ b/drivers/pinctrl/pinctrl-stmfx.c
@@ -346,15 +346,14 @@ static int stmfx_pinctrl_get_pins_count(struct udevice *dev)
  * STMFX pins[15:0] are called "gpio[15:0]"
  * and STMFX pins[23:16] are called "agpio[7:0]"
  */
-#define MAX_PIN_NAME_LEN 7
-static char pin_name[MAX_PIN_NAME_LEN];
+static char pin_name[PINNAME_SIZE];
 static const char *stmfx_pinctrl_get_pin_name(struct udevice *dev,
 					      unsigned int selector)
 {
 	if (selector < STMFX_MAX_GPIO)
-		snprintf(pin_name, MAX_PIN_NAME_LEN, "gpio%u", selector);
+		snprintf(pin_name, PINNAME_SIZE, "gpio%u", selector);
 	else
-		snprintf(pin_name, MAX_PIN_NAME_LEN, "agpio%u", selector - 16);
+		snprintf(pin_name, PINNAME_SIZE, "agpio%u", selector - 16);
 	return pin_name;
 }
 
@@ -408,8 +407,11 @@ static int stmfx_pinctrl_bind(struct udevice *dev)
 {
 	struct stmfx_pinctrl *plat = dev_get_platdata(dev);
 
+	/* subnode name is not explicit: use father name */
+	device_set_name(dev, dev->parent->name);
+
 	return device_bind_driver_to_node(dev->parent,
-					  "stmfx-gpio", "stmfx-gpio",
+					  "stmfx-gpio", dev->parent->name,
 					  dev_ofnode(dev), &plat->gpio);
 };
 
diff --git a/drivers/pinctrl/pinctrl_stm32.c b/drivers/pinctrl/pinctrl_stm32.c
index 71fa29a389..35a6acbe2c 100644
--- a/drivers/pinctrl/pinctrl_stm32.c
+++ b/drivers/pinctrl/pinctrl_stm32.c
@@ -45,15 +45,15 @@ static const char * const pinmux_mode[PINMUX_MODE_COUNT] = {
 	"alt function",
 };
 
-static const char * const pinmux_output[] = {
-	[STM32_GPIO_PUPD_NO] = "bias-disable",
-	[STM32_GPIO_PUPD_UP] = "bias-pull-up",
-	[STM32_GPIO_PUPD_DOWN] = "bias-pull-down",
+static const char * const pinmux_bias[] = {
+	[STM32_GPIO_PUPD_NO] = "",
+	[STM32_GPIO_PUPD_UP] = "pull-up",
+	[STM32_GPIO_PUPD_DOWN] = "pull-down",
 };
 
-static const char * const pinmux_input[] = {
-	[STM32_GPIO_OTYPE_PP] = "drive-push-pull",
-	[STM32_GPIO_OTYPE_OD] = "drive-open-drain",
+static const char * const pinmux_otype[] = {
+	[STM32_GPIO_OTYPE_PP] = "push-pull",
+	[STM32_GPIO_OTYPE_OD] = "open-drain",
 };
 
 static int stm32_pinctrl_get_af(struct udevice *dev, unsigned int offset)
@@ -210,7 +210,8 @@ static int stm32_pinctrl_get_pin_muxing(struct udevice *dev,
 	dev_dbg(dev, "selector = %d gpio_idx = %d mode = %d\n",
 		selector, gpio_idx, mode);
 	priv = dev_get_priv(gpio_dev);
-
+	pupd = (readl(&priv->regs->pupdr) >> (gpio_idx * 2)) & PUPD_MASK;
+	otype = (readl(&priv->regs->otyper) >> gpio_idx) & OTYPE_MSK;
 
 	switch (mode) {
 	case GPIOF_UNKNOWN:
@@ -221,20 +222,17 @@ static int stm32_pinctrl_get_pin_muxing(struct udevice *dev,
 		break;
 	case GPIOF_FUNC:
 		af_num = stm32_pinctrl_get_af(gpio_dev, gpio_idx);
-		snprintf(buf, size, "%s %d", pinmux_mode[mode], af_num);
+		snprintf(buf, size, "%s %d %s %s", pinmux_mode[mode], af_num,
+			 pinmux_otype[otype], pinmux_bias[pupd]);
 		break;
 	case GPIOF_OUTPUT:
-		pupd = (readl(&priv->regs->pupdr) >> (gpio_idx * 2)) &
-		       PUPD_MASK;
-		snprintf(buf, size, "%s %s %s",
-			 pinmux_mode[mode], pinmux_output[pupd],
-			 label ? label : "");
+		snprintf(buf, size, "%s %s %s %s",
+			 pinmux_mode[mode], pinmux_otype[otype],
+			 pinmux_bias[pupd], label ? label : "");
 		break;
 	case GPIOF_INPUT:
-		otype = (readl(&priv->regs->otyper) >> gpio_idx) & OTYPE_MSK;
-		snprintf(buf, size, "%s %s %s",
-			 pinmux_mode[mode], pinmux_input[otype],
-			 label ? label : "");
+		snprintf(buf, size, "%s %s %s", pinmux_mode[mode],
+			 pinmux_bias[pupd], label ? label : "");
 		break;
 	}
 
@@ -401,6 +399,9 @@ static int stm32_pinctrl_bind(struct udevice *dev)
 	dev_for_each_subnode(node, dev) {
 		debug("%s: bind %s\n", __func__, ofnode_get_name(node));
 
+		if (!ofnode_is_enabled(node))
+			continue;
+
 		ofnode_get_property(node, "gpio-controller", &ret);
 		if (ret < 0)
 			continue;
diff --git a/drivers/remoteproc/Kconfig b/drivers/remoteproc/Kconfig
index 7c2e4804b5..3e1c9ed54f 100644
--- a/drivers/remoteproc/Kconfig
+++ b/drivers/remoteproc/Kconfig
@@ -12,6 +12,14 @@ config REMOTEPROC
 	bool
 	depends on DM
 
+config REMOTEPROC_OPTEE
+	bool "Support for the remoteproc in OPTEE"
+	depends on REMOTEPROC
+	depends on OPTEE
+	help
+	  Say y here to support remote processor firmware management by the
+	  trusted execution environment.
+
 # Please keep the configuration alphabetically sorted.
 config K3_SYSTEM_CONTROLLER
 	bool "Support for TI' K3 System Controller"
diff --git a/drivers/remoteproc/Makefile b/drivers/remoteproc/Makefile
index 69ae7bd1e8..d6c5f54fb7 100644
--- a/drivers/remoteproc/Makefile
+++ b/drivers/remoteproc/Makefile
@@ -5,6 +5,7 @@
 #
 
 obj-$(CONFIG_$(SPL_)REMOTEPROC) += rproc-uclass.o rproc-elf-loader.o
+obj-$(CONFIG_REMOTEPROC_OPTEE) += rproc-optee.o
 
 # Remote proc drivers - Please keep this list alphabetically sorted.
 obj-$(CONFIG_K3_SYSTEM_CONTROLLER) += k3_system_controller.o
diff --git a/drivers/remoteproc/rproc-optee.c b/drivers/remoteproc/rproc-optee.c
new file mode 100644
index 0000000000..07d2811e22
--- /dev/null
+++ b/drivers/remoteproc/rproc-optee.c
@@ -0,0 +1,219 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) STMicroelectronics 2020 - All Rights Reserved
+ * Authors: Arnaud Pouliquen <arnaud.pouliquen@st.com>
+ */
+
+#include <dm.h>
+#include <errno.h>
+#include <remoteproc.h>
+#include <rproc_optee.h>
+#include <tee.h>
+#include <dm/device_compat.h>
+
+#define TA_REMOTEPROC_UUID  { 0x80a4c275, 0x0a47, 0x4905, \
+		   { 0x82, 0x85, 0x14, 0x86, 0xa9, 0x77, 0x1a, 0x08} }
+
+/* The function IDs implemented in the associated TA */
+
+/*
+ * Authentication of the firmware and load in the remote processor memory.
+ *
+ * [in]  params[0].value.a:	unique 32bit identifier of the firmware
+ * [in]  params[1].memref:	buffer containing the image of the firmware
+ */
+#define TA_RPROC_FW_CMD_LOAD_FW		1
+
+/*
+ * Start the remote processor.
+ *
+ * [in]  params[0].value.a:	unique 32bit identifier of the firmware
+ */
+#define TA_RPROC_FW_CMD_START_FW	2
+
+/*
+ * Stop the remote processor.
+ *
+ * [in]  params[0].value.a:	unique 32bit identifier of the firmware
+ */
+#define TA_RPROC_FW_CMD_STOP_FW		3
+
+/*
+ * Return the physical address of the resource table, or 0 if not found
+ * No check is done to verify that the address returned is accessible by the
+ * non secure world. If the resource table is loaded in a protected memory,
+ * then accesses from non-secure world will likely fail.
+ *
+ * [in]  params[0].value.a:	unique 32bit identifier of the firmware
+ * [out] params[1].value.a:	32bit LSB resource table memory address
+ * [out] params[1].value.b:	32bit MSB resource table memory address
+ * [out] params[2].value.a:	32bit LSB resource table memory size
+ * [out] params[2].value.b:	32bit MSB resource table memory size
+ */
+#define TA_RPROC_FW_CMD_GET_RSC_TABLE	4
+
+/*
+ * Get remote processor firmware core dump. If found, return either
+ * TEE_SUCCESS on successful completion or TEE_ERROR_SHORT_BUFFER if output
+ * buffer is too short to store the core dump.
+ *
+ * [in]  params[0].value.a:	unique 32bit identifier of the firmware
+ * [out] params[1].memref:	Core dump, if found
+ */
+#define TA_RPROC_FW_CMD_GET_COREDUMP	5
+
+static void prepare_args(struct rproc_optee *trproc, int cmd,
+			 struct tee_invoke_arg *arg, uint num_param,
+			 struct tee_param *param)
+{
+	memset(arg, 0, sizeof(*arg));
+	memset(param, 0, num_param * sizeof(*param));
+
+	arg->func = cmd;
+	arg->session = trproc->session;
+
+	param[0] = (struct tee_param) {
+		.attr = TEE_PARAM_ATTR_TYPE_VALUE_INPUT,
+		.u.value.a = trproc->fw_id,
+	};
+}
+
+int rproc_optee_load(struct rproc_optee *trproc, ulong addr, ulong size)
+{
+	struct tee_invoke_arg arg;
+	struct tee_param param[2];
+	struct tee_shm *fw_shm;
+	int rc;
+
+	rc = tee_shm_register(trproc->tee, (void *)addr, size, 0, &fw_shm);
+	if (rc)
+		return rc;
+
+	prepare_args(trproc, TA_RPROC_FW_CMD_LOAD_FW, &arg, 2, param);
+
+	/* Provide the address and size of the firmware image */
+	param[1] = (struct tee_param){
+		.attr = TEE_PARAM_ATTR_TYPE_MEMREF_INPUT,
+		.u.memref = {
+			.shm = fw_shm,
+			.size = size,
+			.shm_offs = 0,
+		},
+	};
+
+	rc = tee_invoke_func(trproc->tee, &arg, 2, param);
+	if (rc < 0 || arg.ret != 0) {
+		dev_err(trproc->tee,
+			"TA_RPROC_FW_CMD_LOAD_FW invoke failed TEE err: %x, err:%x\n",
+			arg.ret, rc);
+		if (!rc)
+			rc = -EIO;
+	}
+
+	tee_shm_free(fw_shm);
+
+	return rc;
+}
+
+int rproc_optee_get_rsc_table(struct rproc_optee *trproc, phys_addr_t *rsc_addr,
+			      phys_size_t *rsc_size)
+{
+	struct tee_invoke_arg arg;
+	struct tee_param param[3];
+	int rc;
+
+	prepare_args(trproc, TA_RPROC_FW_CMD_GET_RSC_TABLE, &arg, 3, param);
+
+	param[1].attr = TEE_PARAM_ATTR_TYPE_VALUE_OUTPUT;
+	param[2].attr = TEE_PARAM_ATTR_TYPE_VALUE_OUTPUT;
+
+	rc = tee_invoke_func(trproc->tee, &arg, 3, param);
+	if (rc < 0 || arg.ret != 0) {
+		dev_err(trproc->tee,
+			"TA_RPROC_FW_CMD_GET_RSC_TABLE invoke failed TEE err: %x, err:%x\n",
+			arg.ret, rc);
+		if (!rc)
+			rc = -EIO;
+
+		return rc;
+	}
+
+	*rsc_size = (phys_size_t)
+			(param[2].u.value.b << 32 | param[2].u.value.a);
+	*rsc_addr = (phys_addr_t)
+			(param[1].u.value.b << 32 | param[1].u.value.a);
+
+	return 0;
+}
+
+int rproc_optee_start(struct rproc_optee *trproc)
+{
+	struct tee_invoke_arg arg;
+	struct tee_param param;
+	int rc;
+
+	prepare_args(trproc, TA_RPROC_FW_CMD_START_FW, &arg, 1, &param);
+
+	rc =  tee_invoke_func(trproc->tee, &arg, 1, &param);
+	if (rc < 0 || arg.ret != 0) {
+		dev_err(trproc->tee,
+			"TA_RPROC_FW_CMD_START_FW invoke failed TEE err: %x, err:%x\n",
+			arg.ret, rc);
+		if (!rc)
+			rc = -EIO;
+	}
+
+	return rc;
+}
+
+int rproc_optee_stop(struct rproc_optee *trproc)
+{
+	struct tee_invoke_arg arg;
+	struct tee_param param;
+	int rc;
+
+	prepare_args(trproc, TA_RPROC_FW_CMD_STOP_FW, &arg, 1, &param);
+
+	rc =  tee_invoke_func(trproc->tee, &arg, 1, &param);
+	if (rc < 0 || arg.ret != 0) {
+		dev_err(trproc->tee,
+			"TA_RPROC_FW_CMD_STOP_FW invoke failed TEE err: %x, err:%x\n",
+			arg.ret, rc);
+		if (!rc)
+			rc = -EIO;
+	}
+
+	return rc;
+}
+
+int rproc_optee_open(struct rproc_optee *trproc)
+{
+	struct udevice *tee = NULL;
+	const struct tee_optee_ta_uuid uuid = TA_REMOTEPROC_UUID;
+	struct tee_open_session_arg arg = { };
+	int rc;
+
+	if (!trproc)
+		return -EINVAL;
+
+	tee = tee_find_device(tee, NULL, NULL, NULL);
+	if (!tee)
+		return -ENODEV;
+
+	tee_optee_ta_uuid_to_octets(arg.uuid, &uuid);
+	rc = tee_open_session(tee, &arg, 0, NULL);
+	if (!rc) {
+		trproc->tee = tee;
+		trproc->session = arg.session;
+	}
+
+	return 0;
+}
+
+int rproc_optee_close(struct rproc_optee *trproc)
+{
+	if (!trproc->tee)
+		return -ENODEV;
+
+	return tee_close_session(trproc->tee, trproc->session);
+}
diff --git a/drivers/remoteproc/stm32_copro.c b/drivers/remoteproc/stm32_copro.c
index 33b574b1bd..55d7303c55 100644
--- a/drivers/remoteproc/stm32_copro.c
+++ b/drivers/remoteproc/stm32_copro.c
@@ -8,30 +8,24 @@
 #include <errno.h>
 #include <fdtdec.h>
 #include <log.h>
-#include <regmap.h>
 #include <remoteproc.h>
+#include <rproc_optee.h>
 #include <reset.h>
-#include <syscon.h>
 #include <asm/io.h>
 #include <dm/device_compat.h>
 #include <linux/err.h>
 
-#define RCC_GCR_HOLD_BOOT	0
-#define RCC_GCR_RELEASE_BOOT	1
+#define STM32MP15_M4_FW_ID 0
 
 /**
  * struct stm32_copro_privdata - power processor private data
  * @reset_ctl:		reset controller handle
- * @hold_boot_regmap:	regmap for remote processor reset hold boot
- * @hold_boot_offset:	offset of the register controlling the hold boot setting
- * @hold_boot_mask:	bitmask of the register for the hold boot field
+ * @hold_boot:		hold boot controller handle
  * @rsc_table_addr:	resource table address
  */
 struct stm32_copro_privdata {
 	struct reset_ctl reset_ctl;
-	struct regmap *hold_boot_regmap;
-	uint hold_boot_offset;
-	uint hold_boot_mask;
+	struct reset_ctl hold_boot;
 	ulong rsc_table_addr;
 };
 
@@ -43,32 +37,19 @@ struct stm32_copro_privdata {
 static int stm32_copro_probe(struct udevice *dev)
 {
 	struct stm32_copro_privdata *priv;
-	struct regmap *regmap;
-	const fdt32_t *cell;
-	int len, ret;
+	int ret;
 
 	priv = dev_get_priv(dev);
 
-	regmap = syscon_regmap_lookup_by_phandle(dev, "st,syscfg-holdboot");
-	if (IS_ERR(regmap)) {
-		dev_err(dev, "unable to find holdboot regmap (%ld)\n",
-			PTR_ERR(regmap));
-		return PTR_ERR(regmap);
-	}
-
-	cell = dev_read_prop(dev, "st,syscfg-holdboot", &len);
-	if (len < 3 * sizeof(fdt32_t)) {
-		dev_err(dev, "holdboot offset and mask not available\n");
-		return -EINVAL;
+	ret = reset_get_by_name(dev, "mcu_rst", &priv->reset_ctl);
+	if (ret) {
+		dev_err(dev, "failed to get reset (%d)\n", ret);
+		return ret;
 	}
 
-	priv->hold_boot_regmap = regmap;
-	priv->hold_boot_offset = fdtdec_get_number(cell + 1, 1);
-	priv->hold_boot_mask = fdtdec_get_number(cell + 2, 1);
-
-	ret = reset_get_by_index(dev, 0, &priv->reset_ctl);
+	ret = reset_get_by_name(dev, "hold_boot", &priv->hold_boot);
 	if (ret) {
-		dev_err(dev, "failed to get reset (%d)\n", ret);
+		dev_err(dev, "failed to get hold boot (%d)\n", ret);
 		return ret;
 	}
 
@@ -78,32 +59,29 @@ static int stm32_copro_probe(struct udevice *dev)
 }
 
 /**
- * stm32_copro_set_hold_boot() - Hold boot bit management
+ * stm32_copro_optee_probe() - Open a session toward rproc trusted application
  * @dev:	corresponding STM32 remote processor device
- * @hold:	hold boot value
  * @return 0 if all went ok, else corresponding -ve error
  */
-static int stm32_copro_set_hold_boot(struct udevice *dev, bool hold)
+static int stm32_copro_optee_probe(struct udevice *dev)
 {
-	struct stm32_copro_privdata *priv;
-	uint val;
-	int ret;
+	struct rproc_optee *trproc = dev_get_priv(dev);
 
-	priv = dev_get_priv(dev);
+	trproc->fw_id = (u32)dev_get_driver_data(dev);
 
-	val = hold ? RCC_GCR_HOLD_BOOT : RCC_GCR_RELEASE_BOOT;
+	return rproc_optee_open(trproc);
+}
 
-	/*
-	 * Note: shall run an SMC call (STM32_SMC_RCC) if platform is secured.
-	 * To be updated when the code for this SMC service is available which
-	 * is not the case for the time being.
-	 */
-	ret = regmap_update_bits(priv->hold_boot_regmap, priv->hold_boot_offset,
-				 priv->hold_boot_mask, val);
-	if (ret)
-		dev_err(dev, "failed to set hold boot\n");
+/**
+ * stm32_copro_optee_remove() - Close the rproc trusted application session
+ * @dev:	corresponding STM32 remote processor device
+ * @return 0 if all went ok, else corresponding -ve error
+ */
+static int stm32_copro_optee_remove(struct udevice *dev)
+{
+	struct rproc_optee *trproc = dev_get_priv(dev);
 
-	return ret;
+	return rproc_optee_close(trproc);
 }
 
 /**
@@ -149,9 +127,11 @@ static int stm32_copro_load(struct udevice *dev, ulong addr, ulong size)
 
 	priv = dev_get_priv(dev);
 
-	ret = stm32_copro_set_hold_boot(dev, true);
-	if (ret)
+	ret = reset_assert(&priv->hold_boot);
+	if (ret) {
+		dev_err(dev, "Unable to assert hold boot (ret=%d)\n", ret);
 		return ret;
+	}
 
 	ret = reset_assert(&priv->reset_ctl);
 	if (ret) {
@@ -168,6 +148,18 @@ static int stm32_copro_load(struct udevice *dev, ulong addr, ulong size)
 	return rproc_elf32_load_image(dev, addr, size);
 }
 
+/**
+ * stm32_copro_optee_load() - Request OP−TEE to load the remote processor firmware
+ * @dev:	corresponding OP-TEE remote processor device
+ * @return 0 if all went ok, else corresponding -ve error
+ */
+static int stm32_copro_optee_load(struct udevice *dev, ulong addr, ulong size)
+{
+	struct rproc_optee *trproc = dev_get_priv(dev);
+
+	return rproc_optee_load(trproc, addr, size);
+}
+
 /**
  * stm32_copro_start() - Start the STM32 remote processor
  * @dev:	corresponding STM32 remote processor device
@@ -180,23 +172,54 @@ static int stm32_copro_start(struct udevice *dev)
 
 	priv = dev_get_priv(dev);
 
-	/* move hold boot from true to false start the copro */
-	ret = stm32_copro_set_hold_boot(dev, false);
-	if (ret)
+	ret = reset_deassert(&priv->hold_boot);
+	if (ret) {
+		dev_err(dev, "Unable to deassert hold boot (ret=%d)\n", ret);
 		return ret;
+	}
 
 	/*
 	 * Once copro running, reset hold boot flag to avoid copro
-	 * rebooting autonomously
+	 * rebooting autonomously (error should never occur)
 	 */
-	ret = stm32_copro_set_hold_boot(dev, true);
-	writel(ret ? TAMP_COPRO_STATE_OFF : TAMP_COPRO_STATE_CRUN,
-	       TAMP_COPRO_STATE);
-	if (!ret)
-		/* Store rsc_address in bkp register */
-		writel(priv->rsc_table_addr, TAMP_COPRO_RSC_TBL_ADDRESS);
-
-	return ret;
+	ret = reset_assert(&priv->hold_boot);
+	if (ret)
+		dev_err(dev, "Unable to assert hold boot (ret=%d)\n", ret);
+
+	/* indicates that copro is running */
+	writel(TAMP_COPRO_STATE_CRUN, TAMP_COPRO_STATE);
+	/* Store rsc_address in bkp register */
+	writel(priv->rsc_table_addr, TAMP_COPRO_RSC_TBL_ADDRESS);
+
+	return 0;
+}
+
+/**
+ * stm32_copro_optee_start() - Request OP−TEE to start the STM32 remote processor
+ * @dev:	corresponding OP-TEE remote processor device
+ * @return 0 if all went ok, else corresponding -ve error
+ */
+static int stm32_copro_optee_start(struct udevice *dev)
+{
+	struct rproc_optee *trproc = dev_get_priv(dev);
+	phys_addr_t rsc_addr;
+	phys_size_t rsc_size;
+	int ret;
+
+	ret = rproc_optee_get_rsc_table(trproc, &rsc_addr, &rsc_size);
+	if (ret)
+		return ret;
+
+	ret = rproc_optee_start(trproc);
+	if (ret)
+		return ret;
+
+	/* indicates that copro is running */
+	writel(TAMP_COPRO_STATE_CRUN, TAMP_COPRO_STATE);
+	/* Store rsc_address in bkp register */
+	writel(rsc_addr, TAMP_COPRO_RSC_TBL_ADDRESS);
+
+	return 0;
 }
 
 /**
@@ -211,9 +234,11 @@ static int stm32_copro_reset(struct udevice *dev)
 
 	priv = dev_get_priv(dev);
 
-	ret = stm32_copro_set_hold_boot(dev, true);
-	if (ret)
+	ret = reset_assert(&priv->hold_boot);
+	if (ret) {
+		dev_err(dev, "Unable to assert hold boot (ret=%d)\n", ret);
 		return ret;
+	}
 
 	ret = reset_assert(&priv->reset_ctl);
 	if (ret) {
@@ -236,6 +261,35 @@ static int stm32_copro_stop(struct udevice *dev)
 	return stm32_copro_reset(dev);
 }
 
+/**
+ * stm32_copro_optee_reset() - Request OP−TEE to reset the STM32 remote processor
+ * @dev:	corresponding STM32 remote processor device
+ * @return 0 if all went ok, else corresponding -ve error
+ */
+static int stm32_copro_optee_reset(struct udevice *dev)
+{
+	struct rproc_optee *trproc = dev_get_priv(dev);
+	int ret;
+
+	ret = rproc_optee_stop(trproc);
+	if (ret)
+		return ret;
+
+	writel(TAMP_COPRO_STATE_OFF, TAMP_COPRO_STATE);
+
+	return 0;
+}
+
+/**
+ * stm32_copro_optee_stop() - Request OP−TEE to stop the STM32 remote processor
+ * @dev:	corresponding STM32 remote processor device
+ * @return 0 if all went ok, else corresponding -ve error
+ */
+static int stm32_copro_optee_stop(struct udevice *dev)
+{
+	return stm32_copro_optee_reset(dev);
+}
+
 /**
  * stm32_copro_is_running() - Is the STM32 remote processor running
  * @dev:	corresponding STM32 remote processor device
@@ -268,3 +322,28 @@ U_BOOT_DRIVER(stm32_copro) = {
 	.probe = stm32_copro_probe,
 	.priv_auto_alloc_size = sizeof(struct stm32_copro_privdata),
 };
+
+static const struct dm_rproc_ops stm32_copro_optee_ops = {
+	.load = stm32_copro_optee_load,
+	.start = stm32_copro_optee_start,
+	.stop = stm32_copro_optee_stop,
+	.reset = stm32_copro_optee_reset,
+	.is_running = stm32_copro_is_running,
+	.device_to_virt = stm32_copro_device_to_virt,
+};
+
+static const struct udevice_id stm32_copro_optee_ids[] = {
+	{ .compatible = "st,stm32mp1-m4_optee", .data = STM32MP15_M4_FW_ID },
+	{}
+};
+
+U_BOOT_DRIVER(stm32_copro_optee) = {
+	.name = "stm32_m4_proc_optee",
+	.of_match = stm32_copro_optee_ids,
+	.id = UCLASS_REMOTEPROC,
+	.ops = &stm32_copro_optee_ops,
+	.probe = stm32_copro_optee_probe,
+	.remove = stm32_copro_optee_remove,
+	.priv_auto_alloc_size = sizeof(struct rproc_optee),
+	.flags = DM_FLAG_OS_PREPARE,
+};
diff --git a/drivers/reset/Kconfig b/drivers/reset/Kconfig
index 253902ff57..ee5be0bc2f 100644
--- a/drivers/reset/Kconfig
+++ b/drivers/reset/Kconfig
@@ -173,4 +173,12 @@ config RESET_RASPBERRYPI
 	  relevant. This driver provides a reset controller capable of
 	  interfacing with RPi4's co-processor and model these firmware
 	  initialization routines as reset lines.
+
+config RESET_SCMI
+	bool "Enable SCMI reset domain driver"
+	select SCMI_FIRMWARE
+	help
+	  Enable this option if you want to support reset controller
+	  devices exposed by a SCMI agent based on SCMI reset domain
+	  protocol communication with a SCMI server.
 endmenu
diff --git a/drivers/reset/Makefile b/drivers/reset/Makefile
index 3c7f066ae3..625ec7168e 100644
--- a/drivers/reset/Makefile
+++ b/drivers/reset/Makefile
@@ -26,3 +26,4 @@ obj-$(CONFIG_RESET_IMX7) += reset-imx7.o
 obj-$(CONFIG_RESET_SIFIVE) += reset-sifive.o
 obj-$(CONFIG_RESET_SYSCON) += reset-syscon.o
 obj-$(CONFIG_RESET_RASPBERRYPI) += reset-raspberrypi.o
+obj-$(CONFIG_RESET_SCMI) += reset-scmi.o
diff --git a/drivers/reset/reset-scmi.c b/drivers/reset/reset-scmi.c
new file mode 100644
index 0000000000..1bff8075ee
--- /dev/null
+++ b/drivers/reset/reset-scmi.c
@@ -0,0 +1,81 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2019-2020 Linaro Limited
+ */
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <reset-uclass.h>
+#include <scmi_agent.h>
+#include <scmi_protocols.h>
+#include <asm/types.h>
+
+static int scmi_reset_set_level(struct reset_ctl *rst, bool assert_not_deassert)
+{
+	struct scmi_rd_reset_in in = {
+		.domain_id = rst->id,
+		.flags = assert_not_deassert ? SCMI_RD_RESET_FLAG_ASSERT : 0,
+		.reset_state = 0,
+	};
+	struct scmi_rd_reset_out out;
+	struct scmi_msg msg = SCMI_MSG_IN(SCMI_PROTOCOL_ID_RESET_DOMAIN,
+					  SCMI_RESET_DOMAIN_RESET,
+					  in, out);
+	int ret;
+
+	ret = devm_scmi_process_msg(rst->dev->parent, &msg);
+	if (ret)
+		return ret;
+
+	return scmi_to_linux_errno(out.status);
+}
+
+static int scmi_reset_assert(struct reset_ctl *rst)
+{
+	return scmi_reset_set_level(rst, true);
+}
+
+static int scmi_reset_deassert(struct reset_ctl *rst)
+{
+	return scmi_reset_set_level(rst, false);
+}
+
+static int scmi_reset_request(struct reset_ctl *rst)
+{
+	struct scmi_rd_attr_in in = {
+		.domain_id = rst->id,
+	};
+	struct scmi_rd_attr_out out;
+	struct scmi_msg msg = SCMI_MSG_IN(SCMI_PROTOCOL_ID_RESET_DOMAIN,
+					  SCMI_RESET_DOMAIN_ATTRIBUTES,
+					  in, out);
+	int ret;
+
+	/*
+	 * We don't really care about the attribute, just check
+	 * the reset domain exists.
+	 */
+	ret = devm_scmi_process_msg(rst->dev->parent, &msg);
+	if (ret)
+		return ret;
+
+	return scmi_to_linux_errno(out.status);
+}
+
+static int scmi_reset_rfree(struct reset_ctl *rst)
+{
+	return 0;
+}
+
+static const struct reset_ops scmi_reset_domain_ops = {
+	.request	= scmi_reset_request,
+	.rfree		= scmi_reset_rfree,
+	.rst_assert	= scmi_reset_assert,
+	.rst_deassert	= scmi_reset_deassert,
+};
+
+U_BOOT_DRIVER(scmi_reset_domain) = {
+	.name = "scmi_reset_domain",
+	.id = UCLASS_RESET,
+	.ops = &scmi_reset_domain_ops,
+};
diff --git a/drivers/reset/stm32-reset.c b/drivers/reset/stm32-reset.c
index 64a11cfcfc..20c36a99eb 100644
--- a/drivers/reset/stm32-reset.c
+++ b/drivers/reset/stm32-reset.c
@@ -14,6 +14,9 @@
 #include <asm/io.h>
 #include <linux/bitops.h>
 
+/* offset of register without set/clear management */
+#define RCC_MP_GCR_OFFSET 0x10C
+
 /* reset clear offset for STM32MP RCC */
 #define RCC_CL 0x4
 
@@ -40,8 +43,11 @@ static int stm32_reset_assert(struct reset_ctl *reset_ctl)
 	      reset_ctl->id, bank, offset);
 
 	if (dev_get_driver_data(reset_ctl->dev) == STM32MP1)
-		/* reset assert is done in rcc set register */
-		writel(BIT(offset), priv->base + bank);
+		if (bank != RCC_MP_GCR_OFFSET)
+			/* reset assert is done in rcc set register */
+			writel(BIT(offset), priv->base + bank);
+		else
+			clrbits_le32(priv->base + bank, BIT(offset));
 	else
 		setbits_le32(priv->base + bank, BIT(offset));
 
@@ -57,8 +63,11 @@ static int stm32_reset_deassert(struct reset_ctl *reset_ctl)
 	      reset_ctl->id, bank, offset);
 
 	if (dev_get_driver_data(reset_ctl->dev) == STM32MP1)
-		/* reset deassert is done in rcc clr register */
-		writel(BIT(offset), priv->base + bank + RCC_CL);
+		if (bank != RCC_MP_GCR_OFFSET)
+			/* reset deassert is done in rcc clr register */
+			writel(BIT(offset), priv->base + bank + RCC_CL);
+		else
+			setbits_le32(priv->base + bank, BIT(offset));
 	else
 		clrbits_le32(priv->base + bank, BIT(offset));
 
diff --git a/drivers/spi/stm32_qspi.c b/drivers/spi/stm32_qspi.c
index a53b941410..72a169e196 100644
--- a/drivers/spi/stm32_qspi.c
+++ b/drivers/spi/stm32_qspi.c
@@ -14,6 +14,7 @@
 #include <reset.h>
 #include <spi.h>
 #include <spi-mem.h>
+#include <watchdog.h>
 #include <dm/device_compat.h>
 #include <linux/bitops.h>
 #include <linux/delay.h>
@@ -169,6 +170,7 @@ static int _stm32_qspi_wait_cmd(struct stm32_qspi_priv *priv,
 static void _stm32_qspi_read_fifo(u8 *val, void __iomem *addr)
 {
 	*val = readb(addr);
+	WATCHDOG_RESET();
 }
 
 static void _stm32_qspi_write_fifo(u8 *val, void __iomem *addr)
diff --git a/drivers/usb/gadget/dwc2_udc_otg.c b/drivers/usb/gadget/dwc2_udc_otg.c
index eaa5dcb9b1..a025b36400 100644
--- a/drivers/usb/gadget/dwc2_udc_otg.c
+++ b/drivers/usb/gadget/dwc2_udc_otg.c
@@ -1014,6 +1014,9 @@ static int dwc2_udc_otg_ofdata_to_platdata(struct udevice *dev)
 	platdata->force_b_session_valid =
 		dev_read_bool(dev, "u-boot,force-b-session-valid");
 
+	platdata->force_vbus_detection =
+		dev_read_bool(dev, "u-boot,force-vbus-detection");
+
 	/* force platdata according compatible */
 	drvdata = dev_get_driver_data(dev);
 	if (drvdata) {
@@ -1106,31 +1109,45 @@ static int dwc2_udc_otg_probe(struct udevice *dev)
 	if (ret)
 		return ret;
 
-	if (CONFIG_IS_ENABLED(DM_REGULATOR) &&
-	    platdata->activate_stm_id_vb_detection &&
-	    !platdata->force_b_session_valid) {
-		ret = device_get_supply_regulator(dev, "usb33d-supply",
-						  &priv->usb33d_supply);
-		if (ret) {
-			dev_err(dev, "can't get voltage level detector supply\n");
-			return ret;
+	if (platdata->activate_stm_id_vb_detection) {
+		if (CONFIG_IS_ENABLED(DM_REGULATOR) &&
+		    (!platdata->force_b_session_valid ||
+		     platdata->force_vbus_detection)) {
+			ret = device_get_supply_regulator(dev, "usb33d-supply",
+							  &priv->usb33d_supply);
+			if (ret) {
+				dev_err(dev, "can't get voltage level detector supply\n");
+				return ret;
+			}
+			ret = regulator_set_enable(priv->usb33d_supply, true);
+			if (ret) {
+				dev_err(dev, "can't enable voltage level detector supply\n");
+				return ret;
+			}
 		}
-		ret = regulator_set_enable(priv->usb33d_supply, true);
-		if (ret) {
-			dev_err(dev, "can't enable voltage level detector supply\n");
-			return ret;
+
+		if (platdata->force_b_session_valid &&
+		    !platdata->force_vbus_detection) {
+			/* Override VBUS detection: enable then value*/
+			setbits_le32(&usbotg_reg->gotgctl, VB_VALOEN);
+			setbits_le32(&usbotg_reg->gotgctl, VB_VALOVAL);
+		} else {
+			/* Enable VBUS sensing */
+			setbits_le32(&usbotg_reg->ggpio,
+				     GGPIO_STM32_OTG_GCCFG_VBDEN);
+		}
+		if (platdata->force_b_session_valid) {
+			/* Override B session bits: enable then value */
+			setbits_le32(&usbotg_reg->gotgctl, A_VALOEN | B_VALOEN);
+			setbits_le32(&usbotg_reg->gotgctl,
+				     A_VALOVAL | B_VALOVAL);
+		} else {
+			/* Enable ID detection */
+			setbits_le32(&usbotg_reg->ggpio,
+				     GGPIO_STM32_OTG_GCCFG_IDEN);
 		}
-		/* Enable vbus sensing */
-		setbits_le32(&usbotg_reg->ggpio,
-			     GGPIO_STM32_OTG_GCCFG_VBDEN |
-			     GGPIO_STM32_OTG_GCCFG_IDEN);
 	}
 
-	if (platdata->force_b_session_valid)
-		/* Override B session bits : value and enable */
-		setbits_le32(&usbotg_reg->gotgctl,
-			     A_VALOEN | A_VALOVAL | B_VALOEN | B_VALOVAL);
-
 	ret = dwc2_udc_probe(platdata);
 	if (ret)
 		return ret;
@@ -1160,7 +1177,7 @@ static int dwc2_udc_otg_remove(struct udevice *dev)
 static const struct udevice_id dwc2_udc_otg_ids[] = {
 	{ .compatible = "snps,dwc2" },
 	{ .compatible = "brcm,bcm2835-usb" },
-	{ .compatible = "st,stm32mp1-hsotg",
+	{ .compatible = "st,stm32mp15-hsotg",
 	  .data = (ulong)dwc2_set_stm32mp1_hsotg_params },
 	{},
 };
diff --git a/drivers/usb/gadget/dwc2_udc_otg_regs.h b/drivers/usb/gadget/dwc2_udc_otg_regs.h
index 2eda5c3720..9ca6f42375 100644
--- a/drivers/usb/gadget/dwc2_udc_otg_regs.h
+++ b/drivers/usb/gadget/dwc2_udc_otg_regs.h
@@ -94,6 +94,8 @@ struct dwc2_usbotg_reg {
 #define B_VALOEN			BIT(6)
 #define A_VALOVAL			BIT(5)
 #define A_VALOEN			BIT(4)
+#define VB_VALOVAL			BIT(3)
+#define VB_VALOEN			BIT(2)
 
 /* DWC2_UDC_OTG_GOTINT */
 #define GOTGINT_SES_END_DET		(1<<2)
diff --git a/drivers/video/dw_mipi_dsi.c b/drivers/video/dw_mipi_dsi.c
index b7bfbb5e50..1303d99e56 100644
--- a/drivers/video/dw_mipi_dsi.c
+++ b/drivers/video/dw_mipi_dsi.c
@@ -314,7 +314,8 @@ static int dw_mipi_dsi_gen_pkt_hdr_write(struct dw_mipi_dsi *dsi, u32 hdr_val)
 				 val, !(val & GEN_CMD_FULL),
 				 CMD_PKT_STATUS_TIMEOUT_US);
 	if (ret) {
-		dev_err(dsi->dev, "failed to get available command FIFO\n");
+		dev_err(dsi->dsi_host.dev,
+			"failed to get available command FIFO\n");
 		return ret;
 	}
 
@@ -325,7 +326,7 @@ static int dw_mipi_dsi_gen_pkt_hdr_write(struct dw_mipi_dsi *dsi, u32 hdr_val)
 				 val, (val & mask) == mask,
 				 CMD_PKT_STATUS_TIMEOUT_US);
 	if (ret) {
-		dev_err(dsi->dev, "failed to write command FIFO\n");
+		dev_err(dsi->dsi_host.dev, "failed to write command FIFO\n");
 		return ret;
 	}
 
@@ -357,7 +358,7 @@ static int dw_mipi_dsi_write(struct dw_mipi_dsi *dsi,
 					 val, !(val & GEN_PLD_W_FULL),
 					 CMD_PKT_STATUS_TIMEOUT_US);
 		if (ret) {
-			dev_err(dsi->dev,
+			dev_err(dsi->dsi_host.dev,
 				"failed to get available write payload FIFO\n");
 			return ret;
 		}
@@ -380,7 +381,7 @@ static int dw_mipi_dsi_read(struct dw_mipi_dsi *dsi,
 				 val, !(val & GEN_RD_CMD_BUSY),
 				 CMD_PKT_STATUS_TIMEOUT_US);
 	if (ret) {
-		dev_err(dsi->dev, "Timeout during read operation\n");
+		dev_err(dsi->dsi_host.dev, "Timeout during read operation\n");
 		return ret;
 	}
 
@@ -390,7 +391,8 @@ static int dw_mipi_dsi_read(struct dw_mipi_dsi *dsi,
 					 val, !(val & GEN_PLD_R_EMPTY),
 					 CMD_PKT_STATUS_TIMEOUT_US);
 		if (ret) {
-			dev_err(dsi->dev, "Read payload FIFO is empty\n");
+			dev_err(dsi->dsi_host.dev,
+				"Read payload FIFO is empty\n");
 			return ret;
 		}
 
@@ -411,7 +413,7 @@ static ssize_t dw_mipi_dsi_host_transfer(struct mipi_dsi_host *host,
 
 	ret = mipi_dsi_create_packet(&packet, msg);
 	if (ret) {
-		dev_err(dsi->dev, "failed to create packet: %d\n", ret);
+		dev_err(host->dev, "failed to create packet: %d\n", ret);
 		return ret;
 	}
 
@@ -483,15 +485,27 @@ static void dw_mipi_dsi_set_mode(struct dw_mipi_dsi *dsi,
 
 static void dw_mipi_dsi_init_pll(struct dw_mipi_dsi *dsi)
 {
+	const struct mipi_dsi_phy_ops *phy_ops = dsi->phy_ops;
+	unsigned int esc_rate;
+	u32 esc_clk_division;
+
 	/*
 	 * The maximum permitted escape clock is 20MHz and it is derived from
-	 * lanebyteclk, which is running at "lane_mbps / 8".  Thus we want:
+	 * lanebyteclk, which is running at "lane_mbps / 8".
+	 */
+	if (phy_ops->get_esc_clk_rate)
+		phy_ops->get_esc_clk_rate(dsi->device, &esc_rate);
+	else
+		esc_rate = 20; /* Default to 20MHz */
+
+	/*
+	 * We want:
 	 *
-	 *     (lane_mbps >> 3) / esc_clk_division < 20
+	 *     (lane_mbps >> 3) / esc_clk_division < X
 	 * which is:
-	 *     (lane_mbps >> 3) / 20 > esc_clk_division
+	 *     (lane_mbps >> 3) / X > esc_clk_division
 	 */
-	u32 esc_clk_division = (dsi->lane_mbps >> 3) / 20 + 1;
+	esc_clk_division = (dsi->lane_mbps >> 3) / esc_rate + 1;
 
 	dsi_write(dsi, DSI_PWR_UP, RESET);
 
@@ -643,8 +657,13 @@ static void dw_mipi_dsi_vertical_timing_config(struct dw_mipi_dsi *dsi,
 
 static void dw_mipi_dsi_dphy_timing_config(struct dw_mipi_dsi *dsi)
 {
+	const struct mipi_dsi_phy_ops *phy_ops = dsi->phy_ops;
+	struct mipi_dsi_phy_timing timing = {0x40, 0x40, 0x40, 0x40};
 	u32 hw_version;
 
+	if (phy_ops->get_timing)
+		phy_ops->get_timing(dsi->device, dsi->lane_mbps, &timing);
+
 	/*
 	 * TODO dw drv improvements
 	 * data & clock lane timers should be computed according to panel
@@ -656,16 +675,16 @@ static void dw_mipi_dsi_dphy_timing_config(struct dw_mipi_dsi *dsi)
 	hw_version = dsi_read(dsi, DSI_VERSION) & VERSION;
 
 	if (hw_version >= HWVER_131) {
-		dsi_write(dsi, DSI_PHY_TMR_CFG, PHY_HS2LP_TIME_V131(0x40) |
-			  PHY_LP2HS_TIME_V131(0x40));
+		dsi_write(dsi, DSI_PHY_TMR_CFG, PHY_HS2LP_TIME_V131(timing.data_hs2lp) |
+			  PHY_LP2HS_TIME_V131(timing.data_lp2hs));
 		dsi_write(dsi, DSI_PHY_TMR_RD_CFG, MAX_RD_TIME_V131(10000));
 	} else {
-		dsi_write(dsi, DSI_PHY_TMR_CFG, PHY_HS2LP_TIME(0x40) |
-			  PHY_LP2HS_TIME(0x40) | MAX_RD_TIME(10000));
+		dsi_write(dsi, DSI_PHY_TMR_CFG, PHY_HS2LP_TIME(timing.data_hs2lp) |
+			  PHY_LP2HS_TIME(timing.data_lp2hs) | MAX_RD_TIME(10000));
 	}
 
-	dsi_write(dsi, DSI_PHY_TMR_LPCLK_CFG, PHY_CLKHS2LP_TIME(0x40)
-		  | PHY_CLKLP2HS_TIME(0x40));
+	dsi_write(dsi, DSI_PHY_TMR_LPCLK_CFG, PHY_CLKHS2LP_TIME(timing.clk_hs2lp)
+		  | PHY_CLKLP2HS_TIME(timing.clk_lp2hs));
 }
 
 static void dw_mipi_dsi_dphy_interface_config(struct dw_mipi_dsi *dsi)
@@ -702,13 +721,15 @@ static void dw_mipi_dsi_dphy_enable(struct dw_mipi_dsi *dsi)
 	ret = readl_poll_timeout(dsi->base + DSI_PHY_STATUS, val,
 				 val & PHY_LOCK, PHY_STATUS_TIMEOUT_US);
 	if (ret)
-		dev_warn(dsi->dev, "failed to wait phy lock state\n");
+		dev_dbg(dsi->dsi_host.dev,
+			"failed to wait phy lock state\n");
 
 	ret = readl_poll_timeout(dsi->base + DSI_PHY_STATUS,
 				 val, val & PHY_STOP_STATE_CLK_LANE,
 				 PHY_STATUS_TIMEOUT_US);
 	if (ret)
-		dev_warn(dsi->dev, "failed to wait phy clk lane stop state\n");
+		dev_dbg(dsi->dsi_host.dev,
+			"failed to wait phy clk lane stop state\n");
 }
 
 static void dw_mipi_dsi_clear_err(struct dw_mipi_dsi *dsi)
@@ -729,7 +750,7 @@ static void dw_mipi_dsi_bridge_set(struct dw_mipi_dsi *dsi,
 	ret = phy_ops->get_lane_mbps(dsi->device, timings, device->lanes,
 				     device->format, &dsi->lane_mbps);
 	if (ret)
-		dev_warn(dsi->dev, "Phy get_lane_mbps() failed\n");
+		dev_warn(dsi->dsi_host.dev, "Phy get_lane_mbps() failed\n");
 
 	dw_mipi_dsi_init_pll(dsi);
 	dw_mipi_dsi_dpi_config(dsi, timings);
@@ -748,7 +769,7 @@ static void dw_mipi_dsi_bridge_set(struct dw_mipi_dsi *dsi,
 
 	ret = phy_ops->init(dsi->device);
 	if (ret)
-		dev_warn(dsi->dev, "Phy init() failed\n");
+		dev_warn(dsi->dsi_host.dev, "Phy init() failed\n");
 
 	dw_mipi_dsi_dphy_enable(dsi);
 
@@ -776,6 +797,7 @@ static int dw_mipi_dsi_init(struct udevice *dev,
 	dsi->phy_ops = phy_ops;
 	dsi->max_data_lanes = max_data_lanes;
 	dsi->device = device;
+	dsi->dsi_host.dev = (struct device *)dev;
 	dsi->dsi_host.ops = &dw_mipi_dsi_host_ops;
 	device->host = &dsi->dsi_host;
 
diff --git a/drivers/video/stm32/stm32_dsi.c b/drivers/video/stm32/stm32_dsi.c
index 283151398b..6c027b6388 100644
--- a/drivers/video/stm32/stm32_dsi.c
+++ b/drivers/video/stm32/stm32_dsi.c
@@ -482,6 +482,9 @@ static int stm32_dsi_probe(struct udevice *dev)
 	if (priv->hw_version != HWVER_130 &&
 	    priv->hw_version != HWVER_131) {
 		dev_err(dev, "DSI version 0x%x not supported\n", priv->hw_version);
+		dev_dbg(dev, "remove and unbind all DSI child\n");
+		device_chld_remove(dev, NULL, DM_REMOVE_NORMAL);
+		device_chld_unbind(dev, NULL);
 		ret = -ENODEV;
 		goto err_clk;
 	}
diff --git a/env/ext4.c b/env/ext4.c
index f823b69409..e03aa9d7da 100644
--- a/env/ext4.c
+++ b/env/ext4.c
@@ -173,6 +173,5 @@ U_BOOT_ENV_LOCATION(ext4) = {
 	ENV_NAME("EXT4")
 	.load		= env_ext4_load,
 	.save		= ENV_SAVE_PTR(env_ext4_save),
-	.erase		= CONFIG_IS_ENABLED(CMD_ERASEENV) ? env_ext4_erase :
-							    NULL,
+	.erase		= ENV_ERASE_PTR(env_ext4_erase),
 };
diff --git a/env/mmc.c b/env/mmc.c
index ba872701b0..7838874f4d 100644
--- a/env/mmc.c
+++ b/env/mmc.c
@@ -237,7 +237,6 @@ fini:
 	return ret;
 }
 
-#if defined(CONFIG_CMD_ERASEENV)
 static inline int erase_env(struct mmc *mmc, unsigned long size,
 			    unsigned long offset)
 {
@@ -283,7 +282,6 @@ static int env_mmc_erase(void)
 
 	return ret;
 }
-#endif /* CONFIG_CMD_ERASEENV */
 #endif /* CONFIG_CMD_SAVEENV && !CONFIG_SPL_BUILD */
 
 static inline int read_env(struct mmc *mmc, unsigned long size,
@@ -398,8 +396,6 @@ U_BOOT_ENV_LOCATION(mmc) = {
 	.load		= env_mmc_load,
 #ifndef CONFIG_SPL_BUILD
 	.save		= env_save_ptr(env_mmc_save),
-#if defined(CONFIG_CMD_ERASEENV)
-	.erase		= env_mmc_erase,
-#endif
+	.erase		= ENV_ERASE_PTR(env_mmc_erase)
 #endif
 };
diff --git a/env/sf.c b/env/sf.c
index 937778aa37..5f31b9dd2f 100644
--- a/env/sf.c
+++ b/env/sf.c
@@ -27,9 +27,18 @@
 #define INITENV
 #endif
 
+#define	OFFSET_INVALID		(~(u32)0)
+
 #ifdef CONFIG_ENV_OFFSET_REDUND
+#define ENV_OFFSET_REDUND 	CONFIG_ENV_OFFSET_REDUND
+
 static ulong env_offset		= CONFIG_ENV_OFFSET;
 static ulong env_new_offset	= CONFIG_ENV_OFFSET_REDUND;
+
+#else
+
+#define ENV_OFFSET_REDUND 	OFFSET_INVALID
+
 #endif /* CONFIG_ENV_OFFSET_REDUND */
 
 DECLARE_GLOBAL_DATA_PTR;
@@ -279,6 +288,26 @@ out:
 }
 #endif
 
+static int env_sf_erase(void)
+{
+	int ret;
+	env_t env;
+
+	ret = setup_flash_device();
+	if (ret)
+		return ret;
+
+	memset(&env, 0, sizeof(env_t));
+	ret = spi_flash_write(env_flash, CONFIG_ENV_OFFSET, CONFIG_ENV_SIZE, &env);
+	if (ret)
+		return ret;
+
+	if (ENV_OFFSET_REDUND != OFFSET_INVALID)
+		ret = spi_flash_write(env_flash, ENV_OFFSET_REDUND, CONFIG_ENV_SIZE, &env);
+
+	return ret;
+}
+
 #if CONFIG_ENV_ADDR != 0x0
 __weak void *env_sf_get_env_addr(void)
 {
@@ -307,7 +336,8 @@ U_BOOT_ENV_LOCATION(sf) = {
 	.location	= ENVL_SPI_FLASH,
 	ENV_NAME("SPIFlash")
 	.load		= env_sf_load,
-	.save		= CONFIG_IS_ENABLED(SAVEENV) ? ENV_SAVE_PTR(env_sf_save) : NULL,
+	.save		= ENV_SAVE_PTR(env_sf_save),
+	.erase		= ENV_ERASE_PTR(env_sf_erase),
 #if defined(INITENV) && (CONFIG_ENV_ADDR != 0x0)
 	.init		= env_sf_init,
 #endif
diff --git a/include/configs/10m50_devboard.h b/include/configs/10m50_devboard.h
index 768b4a6dfc..3ffc744928 100644
--- a/include/configs/10m50_devboard.h
+++ b/include/configs/10m50_devboard.h
@@ -34,11 +34,6 @@
  */
 #define CONFIG_BOOTP_BOOTFILESIZE
 
-/*
- * FDT options
- */
-#define CONFIG_LMB
-
 /*
  * MEMORY ORGANIZATION
  * -Monitor at top of sdram.
diff --git a/include/configs/3c120_devboard.h b/include/configs/3c120_devboard.h
index 30bbd716b2..3f065ff315 100644
--- a/include/configs/3c120_devboard.h
+++ b/include/configs/3c120_devboard.h
@@ -34,11 +34,6 @@
  */
 #define CONFIG_BOOTP_BOOTFILESIZE
 
-/*
- * FDT options
- */
-#define CONFIG_LMB
-
 /*
  * MEMORY ORGANIZATION
  * -Monitor at top of sdram.
diff --git a/include/configs/sandbox.h b/include/configs/sandbox.h
index 5554313810..d0a8d9b9d5 100644
--- a/include/configs/sandbox.h
+++ b/include/configs/sandbox.h
@@ -22,8 +22,6 @@
 #define CONFIG_SYS_TIMER_RATE		1000000
 #endif
 
-#define CONFIG_LMB
-
 #define CONFIG_HOST_MAX_DEVICES 4
 
 /*
diff --git a/include/configs/stm32mp1.h b/include/configs/stm32mp1.h
index b937233797..32d0723d62 100644
--- a/include/configs/stm32mp1.h
+++ b/include/configs/stm32mp1.h
@@ -148,7 +148,6 @@
  * and the ramdisk at the end.
  */
 #define CONFIG_EXTRA_ENV_SETTINGS \
-	"bootdelay=1\0" \
 	"kernel_addr_r=0xc2000000\0" \
 	"fdt_addr_r=0xc4000000\0" \
 	"scriptaddr=0xc4100000\0" \
diff --git a/include/configs/x86-common.h b/include/configs/x86-common.h
index 641ed2c5ec..f186dd6b82 100644
--- a/include/configs/x86-common.h
+++ b/include/configs/x86-common.h
@@ -16,8 +16,6 @@
  */
 #define CONFIG_PHYSMEM
 
-#define CONFIG_LMB
-
 #define CONFIG_SYS_BOOTM_LEN		(16 << 20)
 
 /* SATA AHCI storage */
diff --git a/include/dm/ofnode.h b/include/dm/ofnode.h
index 8df2facf99..833b0be839 100644
--- a/include/dm/ofnode.h
+++ b/include/dm/ofnode.h
@@ -344,6 +344,17 @@ const char *ofnode_read_string(ofnode node, const char *propname);
  */
 int ofnode_read_u32_array(ofnode node, const char *propname,
 			  u32 *out_values, size_t sz);
+/**
+ * ofnode_is_enabled() - Checks whether a node is enabled.
+ * This looks for a 'status' property. If this exists, then returns true if
+ * the status is 'okay' and false otherwise. If there is no status property,
+ * it returns true on the assumption that anything mentioned should be enabled
+ * by default.
+ *
+ * @node: node to examine
+ * @return false (not enabled) or true (enabled)
+ */
+bool ofnode_is_enabled(ofnode node);
 
 /**
  * ofnode_read_bool() - read a boolean value from a property
diff --git a/include/dm/uclass-id.h b/include/dm/uclass-id.h
index 4ec5fa6670..88f10c4622 100644
--- a/include/dm/uclass-id.h
+++ b/include/dm/uclass-id.h
@@ -94,6 +94,7 @@ enum uclass_id {
 	UCLASS_RESET,		/* Reset controller device */
 	UCLASS_RNG,		/* Random Number Generator */
 	UCLASS_RTC,		/* Real time clock device */
+	UCLASS_SCMI_AGENT,	/* Interface with an SCMI server */
 	UCLASS_SCSI,		/* SCSI device */
 	UCLASS_SERIAL,		/* Serial UART */
 	UCLASS_SIMPLE_BUS,	/* Bus with child devices */
diff --git a/include/dt-bindings/clock/stm32mp1-clks.h b/include/dt-bindings/clock/stm32mp1-clks.h
index 4cdaf13582..ec7b1a9320 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
@@ -248,4 +254,31 @@
 
 #define STM32MP1_LAST_CLK 232
 
+/* SCMI clock identifiers */
+#define CK_SCMI0_HSE		0
+#define CK_SCMI0_HSI		1
+#define CK_SCMI0_CSI		2
+#define CK_SCMI0_LSE		3
+#define CK_SCMI0_LSI		4
+#define CK_SCMI0_PLL2_Q		5
+#define CK_SCMI0_PLL2_R		6
+#define CK_SCMI0_MPU		7
+#define CK_SCMI0_AXI		8
+#define CK_SCMI0_BSEC		9
+#define CK_SCMI0_CRYP1		10
+#define CK_SCMI0_GPIOZ		11
+#define CK_SCMI0_HASH1		12
+#define CK_SCMI0_I2C4		13
+#define CK_SCMI0_I2C6		14
+#define CK_SCMI0_IWDG1		15
+#define CK_SCMI0_RNG1		16
+#define CK_SCMI0_RTC		17
+#define CK_SCMI0_RTCAPB		18
+#define CK_SCMI0_SPI6		19
+#define CK_SCMI0_USART1		20
+
+#define CK_SCMI1_PLL3_Q		0
+#define CK_SCMI1_PLL3_R		1
+#define CK_SCMI1_MCU		2
+
 #endif /* _DT_BINDINGS_STM32MP1_CLKS_H_ */
diff --git a/include/dt-bindings/pinctrl/stm32-pinfunc.h b/include/dt-bindings/pinctrl/stm32-pinfunc.h
index e6fb8ada3f..370a25a936 100644
--- a/include/dt-bindings/pinctrl/stm32-pinfunc.h
+++ b/include/dt-bindings/pinctrl/stm32-pinfunc.h
@@ -26,6 +26,7 @@
 #define AF14	0xf
 #define AF15	0x10
 #define ANALOG	0x11
+#define RSVD	0x12
 
 /* define Pins number*/
 #define PIN_NO(port, line)	(((port) - 'A') * 0x10 + (line))
diff --git a/include/dt-bindings/reset/stm32mp1-resets.h b/include/dt-bindings/reset/stm32mp1-resets.h
index f0c3aaef67..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
@@ -105,4 +106,18 @@
 #define GPIOJ_R		19785
 #define GPIOK_R		19786
 
+/* SCMI reset domain identifiers */
+#define RST_SCMI0_SPI6		0
+#define RST_SCMI0_I2C4		1
+#define RST_SCMI0_I2C6		2
+#define RST_SCMI0_USART1	3
+#define RST_SCMI0_STGEN		4
+#define RST_SCMI0_GPIOZ		5
+#define RST_SCMI0_CRYP1		6
+#define RST_SCMI0_HASH1		7
+#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/rtc/rtc-stm32.h b/include/dt-bindings/rtc/rtc-stm32.h
new file mode 100644
index 0000000000..4373c4dea5
--- /dev/null
+++ b/include/dt-bindings/rtc/rtc-stm32.h
@@ -0,0 +1,13 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * This header provides constants for STM32_RTC bindings.
+ */
+
+#ifndef _DT_BINDINGS_RTC_RTC_STM32_H
+#define _DT_BINDINGS_RTC_RTC_STM32_H
+
+#define RTC_OUT1	0
+#define RTC_OUT2	1
+#define RTC_OUT2_RMP	2
+
+#endif
diff --git a/include/dt-bindings/soc/stm32-hdp.h b/include/dt-bindings/soc/stm32-hdp.h
new file mode 100644
index 0000000000..d986653272
--- /dev/null
+++ b/include/dt-bindings/soc/stm32-hdp.h
@@ -0,0 +1,108 @@
+/* SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause) */
+/*
+ * Copyright (C) STMicroelectronics 2018 - All Rights Reserved
+ * Author: Roullier Christophe <christophe.roullier@st.com>
+ * for STMicroelectronics.
+ */
+
+#ifndef _DT_BINDINGS_STM32_HDP_H
+#define _DT_BINDINGS_STM32_HDP_H
+
+#define STM32_HDP(port, value) ((value) << ((port) * 4))
+
+/* define HDP Pins number*/
+#define HDP0_PWR_PWRWAKE_SYS			0
+#define HDP0_CM4_SLEEPDEEP			1
+#define HDP0_PWR_STDBY_WKUP			2
+#define HDP0_PWR_ENCOMP_VDDCORE			3
+#define HDP0_BSEC_OUT_SEC_NIDEN			4
+#define HDP0_RCC_CM4_SLEEPDEEP			6
+#define HDP0_GPU_DBG7				7
+#define HDP0_DDRCTRL_LP_REQ			8
+#define HDP0_PWR_DDR_RET_ENABLE_N		9
+#define HDP0_GPOVAL_0				15
+
+#define HDP1_PWR_PWRWAKE_MCU			0
+#define HDP1_CM4_HALTED				1
+#define HDP1_CA7_NAXIERRIRQ			2
+#define HDP1_PWR_OKIN_MR			3
+#define HDP1_BSEC_OUT_SEC_DBGEN			4
+#define HDP1_EXTI_SYS_WAKEUP			5
+#define HDP1_RCC_PWRDS_MPU			6
+#define HDP1_GPU_DBG6				7
+#define HDP1_DDRCTRL_DFI_CTRLUPD_REQ		8
+#define HDP1_DDRCTRL_CACTIVE_DDRC_ASR		9
+#define HDP1_GPOVAL_1				15
+
+#define HDP2_PWR_PWRWAKE_MPU			0
+#define HDP2_CM4_RXEV				1
+#define HDP2_CA7_NPMUIRQ1			2
+#define HDP2_CA7_NFIQOUT1			3
+#define HDP2_BSEC_IN_RSTCORE_N			4
+#define HDP2_EXTI_C2_WAKEUP			5
+#define HDP2_RCC_PWRDS_MCU			6
+#define HDP2_GPU_DBG5				7
+#define HDP2_DDRCTRL_DFI_INIT_COMPLETE		8
+#define HDP2_DDRCTRL_PERF_OP_IS_REFRESH		9
+#define HDP2_DDRCTRL_GSKP_DFI_LP_REQ		10
+#define HDP2_GPOVAL_2				15
+
+#define HDP3_PWR_SEL_VTH_VDD_CORE		0
+#define HDP3_CM4_TXEV				1
+#define HDP3_CA7_NPMUIRQ0			2
+#define HDP3_CA7_NFIQOUT0			3
+#define HDP3_BSEC_OUT_SEC_DFTLOCK		4
+#define HDP3_EXTI_C1_WAKEUP			5
+#define HDP3_RCC_PWRDS_SYS			6
+#define HDP3_GPU_DBG4				7
+#define HDP3_DDRCTRL_STAT_DDRC_REG_SELREF_TYPE0	8
+#define HDP3_DDRCTRL_CACTIVE_1			9
+#define HDP3_GPOVAL_3				15
+
+#define HDP4_PWR_PDDS				0
+#define HDP4_CM4_SLEEPING			1
+#define HDP4_CA7_NRESET1			2
+#define HDP4_CA7_NIRQOUT1			3
+#define HDP4_BSEC_OUT_SEC_DFTEN			4
+#define HDP4_BSEC_OUT_SEC_DBGSWENABLE		5
+#define HDP4_ETH_OUT_PMT_INTR_O			6
+#define HDP4_GPU_DBG3				7
+#define HDP4_DDRCTRL_STAT_DDRC_REG_SELREF_TYPE1	8
+#define HDP4_DDRCTRL_CACTIVE_0			9
+#define HDP4_GPOVAL_4				15
+
+#define HDP5_CA7_STANDBYWFIL2			0
+#define HDP5_PWR_VTH_VDDCORE_ACK		1
+#define HDP5_CA7_NRESET0			2
+#define HDP5_CA7_NIRQOUT0			3
+#define HDP5_BSEC_IN_PWROK			4
+#define HDP5_BSEC_OUT_SEC_DEVICEEN		5
+#define HDP5_ETH_OUT_LPI_INTR_O			6
+#define HDP5_GPU_DBG2				7
+#define HDP5_DDRCTRL_CACTIVE_DDRC		8
+#define HDP5_DDRCTRL_WR_CREDIT_CNT		9
+#define HDP5_GPOVAL_5				15
+
+#define HDP6_CA7_STANDBYWFI1			0
+#define HDP6_CA7_STANDBYWFE1			1
+#define HDP6_CA7_EVENT0				2
+#define HDP6_CA7_DBGACK1			3
+#define HDP6_BSEC_OUT_SEC_SPNIDEN		5
+#define HDP6_ETH_OUT_MAC_SPEED_O1		6
+#define HDP6_GPU_DBG1				7
+#define HDP6_DDRCTRL_CSYSACK_DDRC		8
+#define HDP6_DDRCTRL_LPR_CREDIT_CNT		9
+#define HDP6_GPOVAL_6				15
+
+#define HDP7_CA7_STANDBYWFI0			0
+#define HDP7_CA7_STANDBYWFE0			1
+#define HDP7_CA7_DBGACK0			3
+#define HDP7_BSEC_OUT_FUSE_OK			4
+#define HDP7_BSEC_OUT_SEC_SPIDEN		5
+#define HDP7_ETH_OUT_MAC_SPEED_O0		6
+#define HDP7_GPU_DBG0				7
+#define HDP7_DDRCTRL_CSYSREQ_DDRC		8
+#define HDP7_DDRCTRL_HPR_CREDIT_CNT		9
+#define HDP7_GPOVAL_7				15
+
+#endif /* _DT_BINDINGS_STM32_HDP_H */
diff --git a/include/env_internal.h b/include/env_internal.h
index b26dc6239c..67fd729ba0 100644
--- a/include/env_internal.h
+++ b/include/env_internal.h
@@ -207,6 +207,7 @@ struct env_driver {
 #endif
 
 #define ENV_SAVE_PTR(x) (CONFIG_IS_ENABLED(SAVEENV) ? (x) : NULL)
+#define ENV_ERASE_PTR(x) (CONFIG_IS_ENABLED(CMD_ERASEENV) ? (x) : NULL)
 
 extern struct hsearch_data env_htab;
 
diff --git a/include/fdtdec.h b/include/fdtdec.h
index 152eb07b9e..62d1660973 100644
--- a/include/fdtdec.h
+++ b/include/fdtdec.h
@@ -1029,7 +1029,7 @@ static inline int fdtdec_set_phandle(void *blob, int node, uint32_t phandle)
  *     };
  *     uint32_t phandle;
  *
- *     fdtdec_add_reserved_memory(fdt, "framebuffer", &fb, &phandle);
+ *     fdtdec_add_reserved_memory(fdt, "framebuffer", &fb, &phandle, false);
  *
  * This results in the following subnode being added to the top-level
  * /reserved-memory node:
@@ -1056,11 +1056,12 @@ static inline int fdtdec_set_phandle(void *blob, int node, uint32_t phandle)
  * @param carveout	information about the carveout region
  * @param phandlep	return location for the phandle of the carveout region
  *			can be NULL if no phandle should be added
+ * @param no_map	add "no-map" property if true
  * @return 0 on success or a negative error code on failure
  */
 int fdtdec_add_reserved_memory(void *blob, const char *basename,
 			       const struct fdt_memory *carveout,
-			       uint32_t *phandlep);
+			       uint32_t *phandlep, bool no_map);
 
 /**
  * fdtdec_get_carveout() - reads a carveout from an FDT
diff --git a/include/image.h b/include/image.h
index 9a5a87dbf8..c2df0b3aa5 100644
--- a/include/image.h
+++ b/include/image.h
@@ -430,7 +430,7 @@ typedef struct bootm_headers {
 #define	BOOTM_STATE_OS_GO	(0x00000400)
 	int		state;
 
-#ifdef CONFIG_LMB
+#if defined(CONFIG_LMB) && !defined(USE_HOSTCC)
 	struct lmb	lmb;		/* for memory mgmt */
 #endif
 } bootm_headers_t;
diff --git a/include/lmb.h b/include/lmb.h
index e9f19b16ea..f839359178 100644
--- a/include/lmb.h
+++ b/include/lmb.h
@@ -12,22 +12,33 @@
  * Copyright (C) 2001 Peter Bergner, IBM Corp.
  */
 
-#define MAX_LMB_REGIONS 8
+/**
+ * enum lmb_flags - definition of memory region attributes
+ * @LMB_NONE: no special request
+ * @LMB_NOMAP: don't add to mmu configuration
+ */
+enum lmb_flags {
+	LMB_NONE		= 0x0,	/* No special request */
+	LMB_NOMAP		= 0x4,	/* don't add to mmu config */
+};
 
 struct lmb_property {
 	phys_addr_t base;
 	phys_size_t size;
+	enum lmb_flags flags;
 };
 
 struct lmb_region {
-	unsigned long cnt;
-	phys_size_t size;
-	struct lmb_property region[MAX_LMB_REGIONS+1];
+	unsigned long cnt;	/* number of regions */
+	unsigned long max;	/* size of the allocated array */
+	struct lmb_property *region;
 };
 
 struct lmb {
 	struct lmb_region memory;
 	struct lmb_region reserved;
+	struct lmb_property memory_regions[CONFIG_LMB_MEMORY_REGIONS + 1];
+	struct lmb_property reserved_regions[CONFIG_LMB_RESERVED_REGIONS + 1];
 };
 
 extern void lmb_init(struct lmb *lmb);
@@ -37,6 +48,8 @@ extern void lmb_init_and_reserve_range(struct lmb *lmb, phys_addr_t base,
 				       phys_size_t size, void *fdt_blob);
 extern long lmb_add(struct lmb *lmb, phys_addr_t base, phys_size_t size);
 extern long lmb_reserve(struct lmb *lmb, phys_addr_t base, phys_size_t size);
+extern long lmb_reserve_flags(struct lmb *lmb, phys_addr_t base,
+			      phys_size_t size, enum lmb_flags flags);
 extern phys_addr_t lmb_alloc(struct lmb *lmb, phys_size_t size, ulong align);
 extern phys_addr_t lmb_alloc_base(struct lmb *lmb, phys_size_t size, ulong align,
 			    phys_addr_t max_addr);
@@ -46,6 +59,7 @@ extern phys_addr_t lmb_alloc_addr(struct lmb *lmb, phys_addr_t base,
 				  phys_size_t size);
 extern phys_size_t lmb_get_free_size(struct lmb *lmb, phys_addr_t addr);
 extern int lmb_is_reserved(struct lmb *lmb, phys_addr_t addr);
+extern int lmb_is_reserved_flags(struct lmb *lmb, phys_addr_t addr, int flags);
 extern long lmb_free(struct lmb *lmb, phys_addr_t base, phys_size_t size);
 
 extern void lmb_dump_all(struct lmb *lmb);
@@ -60,6 +74,13 @@ lmb_size_bytes(struct lmb_region *type, unsigned long region_nr)
 void board_lmb_reserve(struct lmb *lmb);
 void arch_lmb_reserve(struct lmb *lmb);
 
+/* Low level functions */
+
+static inline bool lmb_is_nomap(struct lmb_property *m)
+{
+	return !!(m->flags & LMB_NOMAP);
+}
+
 #endif /* __KERNEL__ */
 
 #endif /* _LINUX_LMB_H */
diff --git a/include/mipi_dsi.h b/include/mipi_dsi.h
index c8a7d3daef..4ca05f71e2 100644
--- a/include/mipi_dsi.h
+++ b/include/mipi_dsi.h
@@ -96,6 +96,20 @@ struct mipi_dsi_host_ops {
 			    const struct mipi_dsi_msg *msg);
 };
 
+/**
+ * struct mipi_dsi_phy_timing - DSI host phy timings
+ * @data_hs2lp: High Speed to Low Speed Data Transition Time
+ * @data_lp2hs: Low Speed to High Speed Data Transition Time
+ * @clk_hs2lp: High Speed to Low Speed Clock Transition Time
+ * @clk_lp2hs: Low Speed to High Speed Clock Transition Time
+ */
+struct mipi_dsi_phy_timing {
+	u16 data_hs2lp;
+	u16 data_lp2hs;
+	u16 clk_hs2lp;
+	u16 clk_lp2hs;
+};
+
 /**
  * struct mipi_dsi_phy_ops - DSI host physical operations
  * @init: initialized host physical part
@@ -107,6 +121,9 @@ struct mipi_dsi_phy_ops {
 	int (*get_lane_mbps)(void *priv_data, struct display_timing *timings,
 			     u32 lanes, u32 format, unsigned int *lane_mbps);
 	void (*post_set_mode)(void *priv_data,  unsigned long mode_flags);
+	int (*get_timing)(void *priv_data, unsigned int lane_mbps,
+			  struct mipi_dsi_phy_timing *timing);
+	void (*get_esc_clk_rate)(void *priv_data, unsigned int *esc_clk_rate);
 };
 
 /**
diff --git a/include/rproc_optee.h b/include/rproc_optee.h
new file mode 100644
index 0000000000..13193bbe77
--- /dev/null
+++ b/include/rproc_optee.h
@@ -0,0 +1,127 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) STMicroelectronics 2020 - All Rights Reserved
+ */
+
+#ifndef _RPROC_OPTEE_H_
+#define _RPROC_OPTEE_H_
+
+/**
+ * struct rproc_optee - TEE remoteproc structure
+ * @tee:	TEE device
+ * @fw_id:	Identifier of the target firmware
+ * @session:	TEE session identifier
+ */
+struct rproc_optee {
+	struct udevice *tee;
+	u32 fw_id;
+	u32 session;
+};
+
+#if IS_ENABLED(CONFIG_REMOTEPROC_OPTEE)
+
+/**
+ * rproc_optee_open() - open a rproc tee session
+ *
+ * Open a session towards the trusted application in charge of the remote
+ * processor.
+ *
+ * @trproc: OPTEE remoteproc context structure
+ *
+ * @return 0 if the session is opened, or an appropriate error value.
+ */
+int rproc_optee_open(struct rproc_optee *trproc);
+
+/**
+ * rproc_optee_close() - close a rproc tee session
+ *
+ * Close the trusted application session in charge of the remote processor.
+ *
+ * @trproc: OPTEE remoteproc context structure
+ *
+ * @return 0 on success, or an appropriate error value.
+ */
+int rproc_optee_close(struct rproc_optee *trproc);
+
+/**
+ * rproc_optee_start() - Request OP-TEE to start a remote processor
+ *
+ * @trproc: OPTEE remoteproc context structure
+ *
+ * @return 0 on success, or an appropriate error value.
+ */
+int rproc_optee_start(struct rproc_optee *trproc);
+
+/**
+ * rproc_optee_stop() - Request OP-TEE to stop a remote processor
+ *
+ * @trproc: OPTEE remoteproc context structure
+ *
+ * @return 0 on success, or an appropriate error value.
+ */
+int rproc_optee_stop(struct rproc_optee *trproc);
+
+/**
+ * rproc_optee_get_rsc_table() - Request OP-TEE the resource table
+ *
+ * Get the address and the size of the resource table. If no resource table is
+ * found, the size and address are null.
+ *
+ * @trproc: OPTEE remoteproc context structure
+ * @rsc_addr:  out the physical address of the resource table returned
+ * @rsc_size:  out the size of the resource table
+ *
+ * @return 0 on success, or an appropriate error value.
+ */
+int rproc_optee_get_rsc_table(struct rproc_optee *trproc, phys_addr_t *rsc_addr,
+			      phys_size_t *rsc_size);
+
+/**
+ * rproc_optee_load() - load an signed ELF image
+ *
+ * @trproc: OPTEE remoteproc context structure
+ * @addr:	valid ELF image address
+ * @size:	size of the image
+ *
+ * @return 0 if the image is successfully loaded, else appropriate error value.
+ */
+int rproc_optee_load(struct rproc_optee *trproc, ulong addr, ulong size);
+
+#else
+
+static inline int  rproc_optee_open(struct rproc_optee *trproc)
+{
+	return -ENOSYS;
+}
+
+static inline int  rproc_optee_close(struct rproc_optee *trproc)
+{
+	return -ENOSYS;
+}
+
+static inline int  rproc_optee_start(struct rproc_optee *trproc)
+{
+	return -ENOSYS;
+}
+
+static inline int  rproc_optee_stop(struct rproc_optee *trproc)
+{
+	return -ENOSYS;
+}
+
+static inline int  rproc_optee_get_rsc_table(struct rproc_optee *trproc,
+					     phys_addr_t *rsc_addr,
+					     phys_size_t *rsc_size)
+{
+	return -ENOSYS;
+}
+
+static inline int  rproc_optee_load(struct rproc_optee *trproc, ulong addr,
+				    ulong size)
+{
+	return -ENOSYS;
+}
+
+#endif
+
+#endif	/* _RPROC_OPTEE_H_ */
diff --git a/include/scmi_agent-uclass.h b/include/scmi_agent-uclass.h
new file mode 100644
index 0000000000..a501d1b482
--- /dev/null
+++ b/include/scmi_agent-uclass.h
@@ -0,0 +1,24 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2019-2020 Linaro Limited.
+ */
+#ifndef _SCMI_AGENT_UCLASS_H
+#define _SCMI_AGENT_UCLASS_H
+
+struct udevice;
+struct scmi_msg;
+
+/**
+ * struct scmi_transport_ops - The functions that a SCMI transport layer must implement.
+ */
+struct scmi_agent_ops {
+	/*
+	 * process_msg - Request transport to get the SCMI message processed
+	 *
+	 * @agent:		Agent using the transport
+	 * @msg:		SCMI message to be transmitted
+	 */
+	int (*process_msg)(struct udevice *dev, struct scmi_msg *msg);
+};
+
+#endif /* _SCMI_TRANSPORT_UCLASS_H */
diff --git a/include/scmi_agent.h b/include/scmi_agent.h
new file mode 100644
index 0000000000..f1be9ff209
--- /dev/null
+++ b/include/scmi_agent.h
@@ -0,0 +1,68 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/*
+ * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (C) 2019-2020, Linaro Limited
+ *
+ * An SCMI agent device represent on communication path from a
+ * device driver to the remote SCMI server which driver sends
+ * messages to and receives response messages from.
+ */
+#ifndef SCMI_AGENT_H
+#define SCMI_AGENT_H
+
+#include <asm/types.h>
+
+struct udevice;
+
+/*
+ * struct scmi_msg - Context of a SCMI message sent and the response received
+ *
+ * @protocol_id:	SCMI protocol ID
+ * @message_id:		SCMI message ID for a defined protocol ID
+ * @in_msg:		Pointer to the message payload sent by the driver
+ * @in_msg_sz:		Byte size of the message payload sent
+ * @out_msg:		Pointer to buffer to store response message payload
+ * @out_msg_sz:		Byte size of the response buffer and response payload
+ */
+struct scmi_msg {
+	unsigned int protocol_id;
+	unsigned int message_id;
+	u8 *in_msg;
+	size_t in_msg_sz;
+	u8 *out_msg;
+	size_t out_msg_sz;
+};
+
+/* Helper macro to match a message on input/output array references */
+#define SCMI_MSG_IN(_protocol, _message, _in_array, _out_array) \
+	(struct scmi_msg){			\
+		.protocol_id = (_protocol),	\
+		.message_id = (_message),	\
+		.in_msg = (uint8_t *)&(_in_array),	\
+		.in_msg_sz = sizeof(_in_array),	\
+		.out_msg = (uint8_t *)&(_out_array),	\
+		.out_msg_sz = sizeof(_out_array),	\
+	}
+
+/**
+ * scmi_send_and_process_msg() - send and process a SCMI message
+ *
+ * Send a message to a SCMI server through a target SCMI agent device.
+ * Caller sets scmi_msg::out_msg_sz to the output message buffer size.
+ * On return, scmi_msg::out_msg_sz stores the response payload size.
+ *
+ * @dev:	SCMI agent device
+ * @msg:	Message structure reference
+ * @return 0 on success and a negative errno on failure
+ */
+int devm_scmi_process_msg(struct udevice *dev, struct scmi_msg *msg);
+
+/**
+ * scmi_to_linux_errno() - Convert an SCMI error code into a Linux errno code
+ *
+ * @scmi_errno:	SCMI error code value
+ * @return 0 for successful status and a negative errno otherwise
+ */
+int scmi_to_linux_errno(s32 scmi_errno);
+
+#endif /* SCMI_H */
diff --git a/include/scmi_protocols.h b/include/scmi_protocols.h
new file mode 100644
index 0000000000..ccab97c96c
--- /dev/null
+++ b/include/scmi_protocols.h
@@ -0,0 +1,179 @@
+/* SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause */
+/*
+ * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (C) 2019-2020, Linaro Limited
+ */
+#ifndef _SCMI_PROTOCOLS_H
+#define _SCMI_PROTOCOLS_H
+
+#include <linux/bitops.h>
+#include <asm/types.h>
+
+/*
+ * Subset the SCMI protocols definition
+ * based on SCMI specification v2.0 (DEN0056B)
+ * https://developer.arm.com/docs/den0056/b
+ */
+
+enum scmi_std_protocol {
+	SCMI_PROTOCOL_ID_BASE = 0x10,
+	SCMI_PROTOCOL_ID_POWER_DOMAIN = 0x11,
+	SCMI_PROTOCOL_ID_SYSTEM = 0x12,
+	SCMI_PROTOCOL_ID_PERF = 0x13,
+	SCMI_PROTOCOL_ID_CLOCK = 0x14,
+	SCMI_PROTOCOL_ID_SENSOR = 0x15,
+	SCMI_PROTOCOL_ID_RESET_DOMAIN = 0x16,
+};
+
+enum scmi_status_code {
+	SCMI_SUCCESS =  0,
+	SCMI_NOT_SUPPORTED = -1,
+	SCMI_INVALID_PARAMETERS = -2,
+	SCMI_DENIED = -3,
+	SCMI_NOT_FOUND = -4,
+	SCMI_OUT_OF_RANGE = -5,
+	SCMI_BUSY = -6,
+	SCMI_COMMS_ERROR = -7,
+	SCMI_GENERIC_ERROR = -8,
+	SCMI_HARDWARE_ERROR = -9,
+	SCMI_PROTOCOL_ERROR = -10,
+};
+
+/*
+ * SCMI Clock Protocol
+ */
+
+enum scmi_clock_message_id {
+	SCMI_CLOCK_RATE_SET = 0x5,
+	SCMI_CLOCK_RATE_GET = 0x6,
+	SCMI_CLOCK_CONFIG_SET = 0x7,
+};
+
+#define SCMI_CLK_RATE_ASYNC_NOTIFY	BIT(0)
+#define SCMI_CLK_RATE_ASYNC_NORESP	(BIT(0) | BIT(1))
+#define SCMI_CLK_RATE_ROUND_DOWN	0
+#define SCMI_CLK_RATE_ROUND_UP		BIT(2)
+#define SCMI_CLK_RATE_ROUND_CLOSEST	BIT(3)
+
+/**
+ * struct scmi_clk_state_in - Message payload for CLOCK_CONFIG_SET command
+ * @clock_id:	SCMI clock ID
+ * @attributes:	Attributes of the targets clock state
+ */
+struct scmi_clk_state_in {
+	u32 clock_id;
+	u32 attributes;
+};
+
+/**
+ * struct scmi_clk_state_out - Response payload for CLOCK_CONFIG_SET command
+ * @status:	SCMI command status
+ */
+struct scmi_clk_state_out {
+	s32 status;
+};
+
+/**
+ * struct scmi_clk_state_in - Message payload for CLOCK_RATE_GET command
+ * @clock_id:	SCMI clock ID
+ * @attributes:	Attributes of the targets clock state
+ */
+struct scmi_clk_rate_get_in {
+	u32 clock_id;
+};
+
+/**
+ * struct scmi_clk_rate_get_out - Response payload for CLOCK_RATE_GET command
+ * @status:	SCMI command status
+ * @rate_lsb:	32bit LSB of the clock rate in Hertz
+ * @rate_msb:	32bit MSB of the clock rate in Hertz
+ */
+struct scmi_clk_rate_get_out {
+	s32 status;
+	u32 rate_lsb;
+	u32 rate_msb;
+};
+
+/**
+ * struct scmi_clk_state_in - Message payload for CLOCK_RATE_SET command
+ * @clock_id:	SCMI clock ID
+ * @flags:	Flags for the clock rate set request
+ * @rate_lsb:	32bit LSB of the clock rate in Hertz
+ * @rate_msb:	32bit MSB of the clock rate in Hertz
+ */
+struct scmi_clk_rate_set_in {
+	u32 clock_id;
+	u32 flags;
+	u32 rate_lsb;
+	u32 rate_msb;
+};
+
+/**
+ * struct scmi_clk_rate_set_out - Response payload for CLOCK_RATE_SET command
+ * @status:	SCMI command status
+ */
+struct scmi_clk_rate_set_out {
+	s32 status;
+};
+
+/*
+ * SCMI Reset Domain Protocol
+ */
+
+enum scmi_reset_domain_message_id {
+	SCMI_RESET_DOMAIN_ATTRIBUTES = 0x3,
+	SCMI_RESET_DOMAIN_RESET = 0x4,
+};
+
+#define SCMI_RD_NAME_LEN		16
+
+#define SCMI_RD_ATTRIBUTES_FLAG_ASYNC	BIT(31)
+#define SCMI_RD_ATTRIBUTES_FLAG_NOTIF	BIT(30)
+
+#define SCMI_RD_RESET_FLAG_ASYNC	BIT(2)
+#define SCMI_RD_RESET_FLAG_ASSERT	BIT(1)
+#define SCMI_RD_RESET_FLAG_CYCLE	BIT(0)
+
+/**
+ * struct scmi_rd_attr_in - Payload for RESET_DOMAIN_ATTRIBUTES message
+ * @domain_id:	SCMI reset domain ID
+ */
+struct scmi_rd_attr_in {
+	u32 domain_id;
+};
+
+/**
+ * struct scmi_rd_attr_out - Payload for RESET_DOMAIN_ATTRIBUTES response
+ * @status:	SCMI command status
+ * @attributes:	Retrieved attributes of the reset domain
+ * @latency:	Reset cycle max lantency
+ * @name:	Reset domain name
+ */
+struct scmi_rd_attr_out {
+	s32 status;
+	u32 attributes;
+	u32 latency;
+	char name[SCMI_RD_NAME_LEN];
+};
+
+/**
+ * struct scmi_rd_reset_in - Message payload for RESET command
+ * @domain_id:		SCMI reset domain ID
+ * @flags:		Flags for the reset request
+ * @reset_state:	Reset target state
+ */
+struct scmi_rd_reset_in {
+	u32 domain_id;
+	u32 flags;
+	u32 reset_state;
+};
+
+/**
+ * struct scmi_rd_reset_out - Response payload for RESET command
+ * @status:	SCMI command status
+ */
+struct scmi_rd_reset_out {
+	s32 status;
+};
+
+#endif /* _SCMI_PROTOCOLS_H */
diff --git a/include/usb/dwc2_udc.h b/include/usb/dwc2_udc.h
index a2af381a66..aa37e957b4 100644
--- a/include/usb/dwc2_udc.h
+++ b/include/usb/dwc2_udc.h
@@ -28,6 +28,7 @@ struct dwc2_plat_otg_data {
 	unsigned int	tx_fifo_sz_array[DWC2_MAX_HW_ENDPOINTS];
 	unsigned char   tx_fifo_sz_nb;
 	bool		force_b_session_valid;
+	bool		force_vbus_detection;
 	bool		activate_stm_id_vb_detection;
 };
 
diff --git a/lib/Kconfig b/lib/Kconfig
index 8efb154f73..c27204c520 100644
--- a/lib/Kconfig
+++ b/lib/Kconfig
@@ -684,4 +684,27 @@ config LIB_ELF
 	  Supoort basic elf loading/validating functions.
 	  This supports fir 32 bit and 64 bit versions.
 
+config LMB
+	bool "Enable the logical memory blocks library (lmb)"
+	default y if ARC || ARM || M68K || MICROBLAZE || MIPS || NDS32 || \
+		     NIOS2 || PPC || RISCV || SANDBOX || SH || X86 || XTENSA
+	help
+	  Support the library logical memory blocks.
+
+config LMB_MEMORY_REGIONS
+	int "Number of memory regions in lmb lib"
+	depends on LMB
+	default 8
+	help
+	  Define the number of supported memory regions in the library logical
+	  memory blocks.
+
+config LMB_RESERVED_REGIONS
+	int "Number of reserved regions in lmb lib"
+	depends on LMB
+	default 8
+	help
+	  Define the number of supported reserved regions in the library logical
+	  memory blocks.
+
 endmenu
diff --git a/lib/fdtdec.c b/lib/fdtdec.c
index 5f41f58a63..f16a85c8c5 100644
--- a/lib/fdtdec.c
+++ b/lib/fdtdec.c
@@ -1319,7 +1319,7 @@ static int fdtdec_init_reserved_memory(void *blob)
 
 int fdtdec_add_reserved_memory(void *blob, const char *basename,
 			       const struct fdt_memory *carveout,
-			       uint32_t *phandlep)
+			       uint32_t *phandlep, bool no_map)
 {
 	fdt32_t cells[4] = {}, *ptr = cells;
 	uint32_t upper, lower, phandle;
@@ -1419,6 +1419,12 @@ int fdtdec_add_reserved_memory(void *blob, const char *basename,
 	if (err < 0)
 		return err;
 
+	if (no_map) {
+		err = fdt_setprop(blob, node, "no-map", NULL, 0);
+		if (err < 0)
+			return err;
+	}
+
 	/* return the phandle for the new node for the caller to use */
 	if (phandlep)
 		*phandlep = phandle;
@@ -1484,7 +1490,7 @@ int fdtdec_set_carveout(void *blob, const char *node, const char *prop_name,
 	fdt32_t value;
 	void *prop;
 
-	err = fdtdec_add_reserved_memory(blob, name, carveout, &phandle);
+	err = fdtdec_add_reserved_memory(blob, name, carveout, &phandle, false);
 	if (err < 0) {
 		debug("failed to add reserved memory: %d\n", err);
 		return err;
diff --git a/lib/lmb.c b/lib/lmb.c
index 75082f3559..6729c341b0 100644
--- a/lib/lmb.c
+++ b/lib/lmb.c
@@ -14,32 +14,32 @@
 
 #define LMB_ALLOC_ANYWHERE	0
 
-void lmb_dump_all_force(struct lmb *lmb)
+static void lmb_dump_region(struct lmb_region *rgn, char *name)
 {
-	unsigned long i;
+	unsigned long long base, size, end;
+	enum lmb_flags flags;
+	int i;
 
-	printf("lmb_dump_all:\n");
-	printf("    memory.cnt		   = 0x%lx\n", lmb->memory.cnt);
-	printf("    memory.size		   = 0x%llx\n",
-	       (unsigned long long)lmb->memory.size);
-	for (i = 0; i < lmb->memory.cnt; i++) {
-		printf("    memory.reg[0x%lx].base   = 0x%llx\n", i,
-		       (unsigned long long)lmb->memory.region[i].base);
-		printf("		   .size   = 0x%llx\n",
-		       (unsigned long long)lmb->memory.region[i].size);
-	}
+	printf(" %s.cnt  = 0x%lx\n", name, rgn->cnt);
 
-	printf("\n    reserved.cnt	   = 0x%lx\n", lmb->reserved.cnt);
-	printf("    reserved.size	   = 0x%llx\n",
-	       (unsigned long long)lmb->reserved.size);
-	for (i = 0; i < lmb->reserved.cnt; i++) {
-		printf("    reserved.reg[0x%lx].base = 0x%llx\n", i,
-		       (unsigned long long)lmb->reserved.region[i].base);
-		printf("		     .size = 0x%llx\n",
-		       (unsigned long long)lmb->reserved.region[i].size);
+	for (i = 0; i < rgn->cnt; i++) {
+		base = rgn->region[i].base;
+		size = rgn->region[i].size;
+		end = base + size - 1;
+		flags = rgn->region[i].flags;
+
+		printf(" %s[%d]\t[0x%llx-0x%llx], 0x%08llx bytes flags: %x\n",
+		       name, i, base, end, size, flags);
 	}
 }
 
+void lmb_dump_all_force(struct lmb *lmb)
+{
+	printf("lmb_dump_all:\n");
+	lmb_dump_region(&lmb->memory, "memory");
+	lmb_dump_region(&lmb->reserved, "reserved");
+}
+
 void lmb_dump_all(struct lmb *lmb)
 {
 #ifdef DEBUG
@@ -85,6 +85,7 @@ static void lmb_remove_region(struct lmb_region *rgn, unsigned long r)
 	for (i = r; i < rgn->cnt - 1; i++) {
 		rgn->region[i].base = rgn->region[i + 1].base;
 		rgn->region[i].size = rgn->region[i + 1].size;
+		rgn->region[i].flags = rgn->region[i + 1].flags;
 	}
 	rgn->cnt--;
 }
@@ -99,10 +100,14 @@ static void lmb_coalesce_regions(struct lmb_region *rgn, unsigned long r1,
 
 void lmb_init(struct lmb *lmb)
 {
+	/* Hookup the initial arrays */
+	lmb->memory.region = lmb->memory_regions;
+	lmb->memory.max = ARRAY_SIZE(lmb->memory_regions) - 1;
 	lmb->memory.cnt = 0;
-	lmb->memory.size = 0;
+
+	lmb->reserved.region = lmb->reserved_regions;
+	lmb->reserved.max = ARRAY_SIZE(lmb->reserved_regions) - 1;
 	lmb->reserved.cnt = 0;
-	lmb->reserved.size = 0;
 }
 
 static void lmb_reserve_common(struct lmb *lmb, void *fdt_blob)
@@ -146,7 +151,8 @@ void lmb_init_and_reserve_range(struct lmb *lmb, phys_addr_t base,
 }
 
 /* This routine called with relocation disabled. */
-static long lmb_add_region(struct lmb_region *rgn, phys_addr_t base, phys_size_t size)
+static long lmb_add_region_flags(struct lmb_region *rgn, phys_addr_t base,
+				 phys_size_t size, enum lmb_flags flags)
 {
 	unsigned long coalesced = 0;
 	long adjacent, i;
@@ -154,6 +160,7 @@ static long lmb_add_region(struct lmb_region *rgn, phys_addr_t base, phys_size_t
 	if (rgn->cnt == 0) {
 		rgn->region[0].base = base;
 		rgn->region[0].size = size;
+		rgn->region[0].flags = flags;
 		rgn->cnt = 1;
 		return 0;
 	}
@@ -162,18 +169,27 @@ static long lmb_add_region(struct lmb_region *rgn, phys_addr_t base, phys_size_t
 	for (i = 0; i < rgn->cnt; i++) {
 		phys_addr_t rgnbase = rgn->region[i].base;
 		phys_size_t rgnsize = rgn->region[i].size;
+		phys_size_t rgnflags = rgn->region[i].flags;
 
-		if ((rgnbase == base) && (rgnsize == size))
-			/* Already have this region, so we're done */
-			return 0;
+		if (rgnbase == base && rgnsize == size) {
+			if (flags == rgnflags)
+				/* Already have this region, so we're done */
+				return 0;
+			else
+				return -1; /* regions with new flags */
+		}
 
 		adjacent = lmb_addrs_adjacent(base, size, rgnbase, rgnsize);
 		if (adjacent > 0) {
+			if (flags != rgnflags)
+				break;
 			rgn->region[i].base -= size;
 			rgn->region[i].size += size;
 			coalesced++;
 			break;
 		} else if (adjacent < 0) {
+			if (flags != rgnflags)
+				break;
 			rgn->region[i].size += size;
 			coalesced++;
 			break;
@@ -184,13 +200,15 @@ static long lmb_add_region(struct lmb_region *rgn, phys_addr_t base, phys_size_t
 	}
 
 	if ((i < rgn->cnt - 1) && lmb_regions_adjacent(rgn, i, i + 1)) {
-		lmb_coalesce_regions(rgn, i, i + 1);
-		coalesced++;
+		if (rgn->region[i].flags == rgn->region[i + 1].flags) {
+			lmb_coalesce_regions(rgn, i, i + 1);
+			coalesced++;
+		}
 	}
 
 	if (coalesced)
 		return coalesced;
-	if (rgn->cnt >= MAX_LMB_REGIONS)
+	if (rgn->cnt >= rgn->max)
 		return -1;
 
 	/* Couldn't coalesce the LMB, so add it to the sorted table. */
@@ -198,9 +216,11 @@ static long lmb_add_region(struct lmb_region *rgn, phys_addr_t base, phys_size_t
 		if (base < rgn->region[i].base) {
 			rgn->region[i + 1].base = rgn->region[i].base;
 			rgn->region[i + 1].size = rgn->region[i].size;
+			rgn->region[i + 1].flags = rgn->region[i].flags;
 		} else {
 			rgn->region[i + 1].base = base;
 			rgn->region[i + 1].size = size;
+			rgn->region[i + 1].flags = flags;
 			break;
 		}
 	}
@@ -208,6 +228,7 @@ static long lmb_add_region(struct lmb_region *rgn, phys_addr_t base, phys_size_t
 	if (base < rgn->region[0].base) {
 		rgn->region[0].base = base;
 		rgn->region[0].size = size;
+		rgn->region[0].flags = flags;
 	}
 
 	rgn->cnt++;
@@ -215,6 +236,12 @@ static long lmb_add_region(struct lmb_region *rgn, phys_addr_t base, phys_size_t
 	return 0;
 }
 
+static long lmb_add_region(struct lmb_region *rgn, phys_addr_t base,
+			   phys_size_t size)
+{
+	return lmb_add_region_flags(rgn, base, size, LMB_NONE);
+}
+
 /* This routine may be called with relocation disabled. */
 long lmb_add(struct lmb *lmb, phys_addr_t base, phys_size_t size)
 {
@@ -269,14 +296,21 @@ long lmb_free(struct lmb *lmb, phys_addr_t base, phys_size_t size)
 	 * beginging of the hole and add the region after hole.
 	 */
 	rgn->region[i].size = base - rgn->region[i].base;
-	return lmb_add_region(rgn, end + 1, rgnend - end);
+	return lmb_add_region_flags(rgn, end + 1, rgnend - end,
+				    rgn->region[i].flags);
 }
 
-long lmb_reserve(struct lmb *lmb, phys_addr_t base, phys_size_t size)
+long lmb_reserve_flags(struct lmb *lmb, phys_addr_t base, phys_size_t size,
+		       enum lmb_flags flags)
 {
 	struct lmb_region *_rgn = &(lmb->reserved);
 
-	return lmb_add_region(_rgn, base, size);
+	return lmb_add_region_flags(_rgn, base, size, flags);
+}
+
+long lmb_reserve(struct lmb *lmb, phys_addr_t base, phys_size_t size)
+{
+	return lmb_reserve_flags(lmb, base, size, LMB_NONE);
 }
 
 static long lmb_overlaps_region(struct lmb_region *rgn, phys_addr_t base,
@@ -411,7 +445,7 @@ phys_size_t lmb_get_free_size(struct lmb *lmb, phys_addr_t addr)
 	return 0;
 }
 
-int lmb_is_reserved(struct lmb *lmb, phys_addr_t addr)
+int lmb_is_reserved_flags(struct lmb *lmb, phys_addr_t addr, int flags)
 {
 	int i;
 
@@ -419,11 +453,17 @@ int lmb_is_reserved(struct lmb *lmb, phys_addr_t addr)
 		phys_addr_t upper = lmb->reserved.region[i].base +
 			lmb->reserved.region[i].size - 1;
 		if ((addr >= lmb->reserved.region[i].base) && (addr <= upper))
-			return 1;
+			return !!((lmb->reserved.region[i].flags & flags)
+				   == flags);
 	}
 	return 0;
 }
 
+int lmb_is_reserved(struct lmb *lmb, phys_addr_t addr)
+{
+	return lmb_is_reserved_flags(lmb, addr, LMB_NONE);
+}
+
 __weak void board_lmb_reserve(struct lmb *lmb)
 {
 	/* please define platform specific board_lmb_reserve() */
diff --git a/lib/optee/optee.c b/lib/optee/optee.c
index 457d4cca8a..9e6606568f 100644
--- a/lib/optee/optee.c
+++ b/lib/optee/optee.c
@@ -192,7 +192,7 @@ int optee_copy_fdt_nodes(const void *old_blob, void *new_blob)
 				ret = fdtdec_add_reserved_memory(new_blob,
 								 nodename,
 								 &carveout,
-								 NULL);
+								 NULL, true);
 				free(oldname);
 
 				if (ret < 0)
diff --git a/scripts/config_whitelist.txt b/scripts/config_whitelist.txt
index 093e432efc..bb6aef5ce4 100644
--- a/scripts/config_whitelist.txt
+++ b/scripts/config_whitelist.txt
@@ -970,7 +970,6 @@ CONFIG_LG4573
 CONFIG_LINUX
 CONFIG_LINUX_RESET_VEC
 CONFIG_LITTLETON_LCD
-CONFIG_LMB
 CONFIG_LMS283GF05
 CONFIG_LOADADDR
 CONFIG_LOADCMD
diff --git a/test/dm/Makefile b/test/dm/Makefile
index 864c8d0b4c..70ba1b6695 100644
--- a/test/dm/Makefile
+++ b/test/dm/Makefile
@@ -80,4 +80,5 @@ obj-$(CONFIG_DM_RNG) += rng.o
 obj-$(CONFIG_CLK_K210_SET_RATE) += k210_pll.o
 obj-$(CONFIG_SIMPLE_PM_BUS) += simple-pm-bus.o
 obj-$(CONFIG_RESET_SYSCON) += syscon-reset.o
+obj-$(CONFIG_SCMI_FIRMWARE) += scmi.o
 endif
diff --git a/test/dm/fdtdec.c b/test/dm/fdtdec.c
index 716993f706..017157a2ec 100644
--- a/test/dm/fdtdec.c
+++ b/test/dm/fdtdec.c
@@ -80,7 +80,7 @@ static int dm_test_fdtdec_add_reserved_memory(struct unit_test_state *uts)
 	resv.start = 0x1000;
 	resv.end = 0x1fff;
 	ut_assertok(fdtdec_add_reserved_memory(blob, "rsvd_region",
-					       &resv, &phandle));
+					       &resv, &phandle, false));
 
 	/* Test /reserve-memory and its subnode should exist */
 	parent = fdt_path_offset(blob, "/reserved-memory");
@@ -101,10 +101,13 @@ static int dm_test_fdtdec_add_reserved_memory(struct unit_test_state *uts)
 	resv.start = 0x2000;
 	resv.end = 0x2fff;
 	ut_assertok(fdtdec_add_reserved_memory(blob, "rsvd_region1",
-					       &resv, &phandle1));
+					       &resv, &phandle1, true));
 	subnode = fdt_path_offset(blob, "/reserved-memory/rsvd_region1");
 	ut_assert(subnode > 0);
 
+	/* check that no-map property is present */
+	ut_assert(fdt_getprop(blob, subnode, "no-map", NULL) > 0);
+
 	/* phandles must be different */
 	ut_assert(phandle != phandle1);
 
@@ -115,7 +118,7 @@ static int dm_test_fdtdec_add_reserved_memory(struct unit_test_state *uts)
 	resv.start = 0x1000;
 	resv.end = 0x1fff;
 	ut_assertok(fdtdec_add_reserved_memory(blob, "rsvd_region2",
-					       &resv, &phandle1));
+					       &resv, &phandle1, false));
 	subnode = fdt_path_offset(blob, "/reserved-memory/rsvd_region2");
 	ut_assert(subnode < 0);
 
diff --git a/test/dm/ofnode.c b/test/dm/ofnode.c
index 8bfb706602..9a34fdd7a2 100644
--- a/test/dm/ofnode.c
+++ b/test/dm/ofnode.c
@@ -136,3 +136,15 @@ static int dm_test_ofnode_get_child_count(struct unit_test_state *uts)
 }
 DM_TEST(dm_test_ofnode_get_child_count,
 	UT_TESTF_SCAN_PDATA | UT_TESTF_SCAN_FDT);
+
+static int dm_test_ofnode_is_enabled(struct unit_test_state *uts)
+{
+	ofnode root_node = ofnode_path("/");
+	ofnode node = ofnode_path("/usb@0");
+
+	ut_assert(ofnode_is_enabled(root_node));
+	ut_assert(!ofnode_is_enabled(node));
+
+	return 0;
+}
+DM_TEST(dm_test_ofnode_is_enabled, UT_TESTF_SCAN_PDATA | UT_TESTF_SCAN_FDT);
diff --git a/test/dm/scmi.c b/test/dm/scmi.c
new file mode 100644
index 0000000000..be60b44b3b
--- /dev/null
+++ b/test/dm/scmi.c
@@ -0,0 +1,203 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020, Linaro Limited
+ *
+ * Tests scmi_agent uclass and the SCMI drivers implemented in other
+ * uclass devices probe when a SCMI server exposes resources.
+ *
+ * Note in test.dts the protocol@10 node in agent 1. Protocol 0x10 is not
+ * implemented in U-Boot SCMI components but the implementation is exepected
+ * to not complain on unknown protocol IDs, as long as it is not used. Note
+ * in test.dts tests that SCMI drivers probing does not fail for such an
+ * unknown SCMI protocol ID.
+ */
+
+#include <common.h>
+#include <clk.h>
+#include <dm.h>
+#include <reset.h>
+#include <asm/scmi_test.h>
+#include <dm/device-internal.h>
+#include <dm/test.h>
+#include <linux/kconfig.h>
+#include <test/ut.h>
+
+static int ut_assert_scmi_state_preprobe(struct unit_test_state *uts)
+{
+	struct sandbox_scmi_service *scmi_ctx = sandbox_scmi_service_ctx();
+
+	ut_assertnonnull(scmi_ctx);
+	if (scmi_ctx->agent_count)
+		ut_asserteq(2, scmi_ctx->agent_count);
+
+	return 0;
+}
+
+static int ut_assert_scmi_state_postprobe(struct unit_test_state *uts,
+					  struct udevice *dev)
+{
+	struct sandbox_scmi_devices *scmi_devices;
+	struct sandbox_scmi_service *scmi_ctx;
+
+	/* Device references to check context against test sequence */
+	scmi_devices = sandbox_scmi_devices_ctx(dev);
+
+	ut_assertnonnull(scmi_devices);
+	if (IS_ENABLED(CONFIG_CLK_SCMI))
+		ut_asserteq(3, scmi_devices->clk_count);
+	if (IS_ENABLED(CONFIG_RESET_SCMI))
+		ut_asserteq(1, scmi_devices->reset_count);
+
+	/* State of the simulated SCMI server exposed */
+	scmi_ctx = sandbox_scmi_service_ctx();
+
+	ut_asserteq(2, scmi_ctx->agent_count);
+
+	ut_assertnonnull(scmi_ctx->agent[0]);
+	ut_asserteq(2, scmi_ctx->agent[0]->clk_count);
+	ut_assertnonnull(scmi_ctx->agent[0]->clk);
+	ut_asserteq(1, scmi_ctx->agent[0]->reset_count);
+	ut_assertnonnull(scmi_ctx->agent[0]->reset);
+
+	ut_assertnonnull(scmi_ctx->agent[1]);
+	ut_assertnonnull(scmi_ctx->agent[1]->clk);
+	ut_asserteq(1, scmi_ctx->agent[1]->clk_count);
+
+	return 0;
+}
+
+static int load_sandbox_scmi_test_devices(struct unit_test_state *uts,
+					  struct udevice **dev)
+{
+	int ret;
+
+	ret = ut_assert_scmi_state_preprobe(uts);
+	if (ret)
+		return ret;
+
+	ut_assertok(uclass_get_device_by_name(UCLASS_MISC, "sandbox_scmi",
+					      dev));
+	ut_assertnonnull(*dev);
+
+	return ut_assert_scmi_state_postprobe(uts, *dev);
+}
+
+static int release_sandbox_scmi_test_devices(struct unit_test_state *uts,
+					     struct udevice *dev)
+{
+	ut_assertok(device_remove(dev, DM_REMOVE_NORMAL));
+
+	/* Not sure test devices are fully removed, agent may not be visible */
+	return 0;
+}
+
+/*
+ * Test SCMI states when loading and releasing resources
+ * related to SCMI drivers.
+ */
+static int dm_test_scmi_sandbox_agent(struct unit_test_state *uts)
+{
+	struct udevice *dev = NULL;
+	int ret;
+
+	ret = load_sandbox_scmi_test_devices(uts, &dev);
+	if (!ret)
+		ret = release_sandbox_scmi_test_devices(uts, dev);
+
+	return ret;
+}
+
+DM_TEST(dm_test_scmi_sandbox_agent, UT_TESTF_SCAN_FDT);
+
+static int dm_test_scmi_clocks(struct unit_test_state *uts)
+{
+	struct sandbox_scmi_devices *scmi_devices;
+	struct sandbox_scmi_service *scmi_ctx;
+	struct udevice *dev = NULL;
+	int ret_dev;
+	int ret;
+
+	if (!IS_ENABLED(CONFIG_CLK_SCMI))
+		return 0;
+
+	ret = load_sandbox_scmi_test_devices(uts, &dev);
+	if (ret)
+		return ret;
+
+	scmi_devices = sandbox_scmi_devices_ctx(dev);
+	scmi_ctx = sandbox_scmi_service_ctx();
+
+	/* Test SCMI clocks rate manipulation */
+	ut_asserteq(1000, clk_get_rate(&scmi_devices->clk[0]));
+	ut_asserteq(333, clk_get_rate(&scmi_devices->clk[1]));
+	ut_asserteq(44, clk_get_rate(&scmi_devices->clk[2]));
+
+	ret_dev = clk_set_rate(&scmi_devices->clk[1], 1088);
+	ut_assert(!ret_dev || ret_dev == 1088);
+
+	ut_asserteq(1000, scmi_ctx->agent[0]->clk[0].rate);
+	ut_asserteq(1088, scmi_ctx->agent[0]->clk[1].rate);
+	ut_asserteq(44, scmi_ctx->agent[1]->clk[0].rate);
+
+	ut_asserteq(1000, clk_get_rate(&scmi_devices->clk[0]));
+	ut_asserteq(1088, clk_get_rate(&scmi_devices->clk[1]));
+	ut_asserteq(44, clk_get_rate(&scmi_devices->clk[2]));
+
+	/* restore original rate for further tests */
+	ret_dev = clk_set_rate(&scmi_devices->clk[1], 333);
+	ut_assert(!ret_dev || ret_dev == 333);
+
+	/* Test SCMI clocks gating manipulation */
+	ut_assert(!scmi_ctx->agent[0]->clk[0].enabled);
+	ut_assert(!scmi_ctx->agent[0]->clk[1].enabled);
+	ut_assert(!scmi_ctx->agent[1]->clk[0].enabled);
+
+	ut_asserteq(0, clk_enable(&scmi_devices->clk[1]));
+	ut_asserteq(0, clk_enable(&scmi_devices->clk[2]));
+
+	ut_assert(!scmi_ctx->agent[0]->clk[0].enabled);
+	ut_assert(scmi_ctx->agent[0]->clk[1].enabled);
+	ut_assert(scmi_ctx->agent[1]->clk[0].enabled);
+
+	ut_assertok(clk_disable(&scmi_devices->clk[1]));
+	ut_assertok(clk_disable(&scmi_devices->clk[2]));
+
+	ut_assert(!scmi_ctx->agent[0]->clk[0].enabled);
+	ut_assert(!scmi_ctx->agent[0]->clk[1].enabled);
+	ut_assert(!scmi_ctx->agent[1]->clk[0].enabled);
+
+	return release_sandbox_scmi_test_devices(uts, dev);
+}
+
+DM_TEST(dm_test_scmi_clocks, UT_TESTF_SCAN_FDT);
+
+static int dm_test_scmi_resets(struct unit_test_state *uts)
+{
+	struct sandbox_scmi_devices *scmi_devices;
+	struct sandbox_scmi_service *scmi_ctx;
+	struct udevice *dev = NULL;
+	int ret;
+
+	if (!IS_ENABLED(CONFIG_RESET_SCMI))
+		return 0;
+
+	ret = load_sandbox_scmi_test_devices(uts, &dev);
+	if (ret)
+		return ret;
+
+	scmi_devices = sandbox_scmi_devices_ctx(dev);
+	scmi_ctx = sandbox_scmi_service_ctx();
+
+	/* Test SCMI resect controller manipulation */
+	ut_assert(!scmi_ctx->agent[0]->reset[0].asserted)
+
+	ut_assertok(reset_assert(&scmi_devices->reset[0]));
+	ut_assert(scmi_ctx->agent[0]->reset[0].asserted)
+
+	ut_assertok(reset_deassert(&scmi_devices->reset[0]));
+	ut_assert(!scmi_ctx->agent[0]->reset[0].asserted);
+
+	return release_sandbox_scmi_test_devices(uts, dev);
+}
+
+DM_TEST(dm_test_scmi_resets, UT_TESTF_SCAN_FDT);
diff --git a/test/lib/lmb.c b/test/lib/lmb.c
index 644ee78758..d7bd826190 100644
--- a/test/lib/lmb.c
+++ b/test/lib/lmb.c
@@ -659,3 +659,92 @@ static int lib_test_lmb_get_free_size(struct unit_test_state *uts)
 
 DM_TEST(lib_test_lmb_get_free_size,
 	UT_TESTF_SCAN_PDATA | UT_TESTF_SCAN_FDT);
+
+static int lib_test_lmb_flags(struct unit_test_state *uts)
+{
+	const phys_addr_t ram = 0x40000000;
+	const phys_size_t ram_size = 0x20000000;
+	struct lmb lmb;
+	long ret;
+
+	lmb_init(&lmb);
+
+	ret = lmb_add(&lmb, ram, ram_size);
+	ut_asserteq(ret, 0);
+
+	/* reserve, same flag */
+	ret = lmb_reserve_flags(&lmb, 0x40010000, 0x10000, LMB_NOMAP);
+	ut_asserteq(ret, 0);
+	ASSERT_LMB(&lmb, ram, ram_size, 1, 0x40010000, 0x10000,
+		   0, 0, 0, 0);
+
+	/* reserve again, same flag */
+	ret = lmb_reserve_flags(&lmb, 0x40010000, 0x10000, LMB_NOMAP);
+	ut_asserteq(ret, 0);
+	ASSERT_LMB(&lmb, ram, ram_size, 1, 0x40010000, 0x10000,
+		   0, 0, 0, 0);
+
+	/* reserve again, new flag */
+	ret = lmb_reserve_flags(&lmb, 0x40010000, 0x10000, LMB_NONE);
+	ut_asserteq(ret, -1);
+	ASSERT_LMB(&lmb, ram, ram_size, 1, 0x40010000, 0x10000,
+		   0, 0, 0, 0);
+
+	ut_asserteq(lmb_is_nomap(&lmb.reserved.region[0]), 1);
+
+	/* merge after */
+	ret = lmb_reserve_flags(&lmb, 0x40020000, 0x10000, LMB_NOMAP);
+	ut_asserteq(ret, 1);
+	ASSERT_LMB(&lmb, ram, ram_size, 1, 0x40010000, 0x20000,
+		   0, 0, 0, 0);
+
+	/* merge before */
+	ret = lmb_reserve_flags(&lmb, 0x40000000, 0x10000, LMB_NOMAP);
+	ut_asserteq(ret, 1);
+	ASSERT_LMB(&lmb, ram, ram_size, 1, 0x40000000, 0x30000,
+		   0, 0, 0, 0);
+
+	ut_asserteq(lmb_is_nomap(&lmb.reserved.region[0]), 1);
+
+	ret = lmb_reserve_flags(&lmb, 0x40030000, 0x10000, LMB_NONE);
+	ut_asserteq(ret, 0);
+	ASSERT_LMB(&lmb, ram, ram_size, 2, 0x40000000, 0x30000,
+		   0x40030000, 0x10000, 0, 0);
+
+	ut_asserteq(lmb_is_nomap(&lmb.reserved.region[0]), 1);
+	ut_asserteq(lmb_is_nomap(&lmb.reserved.region[1]), 0);
+
+	/* test that old API use LMB_NONE */
+	ret = lmb_reserve(&lmb, 0x40040000, 0x10000);
+	ut_asserteq(ret, 1);
+	ASSERT_LMB(&lmb, ram, ram_size, 2, 0x40000000, 0x30000,
+		   0x40030000, 0x20000, 0, 0);
+
+	ut_asserteq(lmb_is_nomap(&lmb.reserved.region[0]), 1);
+	ut_asserteq(lmb_is_nomap(&lmb.reserved.region[1]), 0);
+
+	ret = lmb_reserve_flags(&lmb, 0x40070000, 0x10000, LMB_NOMAP);
+	ut_asserteq(ret, 0);
+	ASSERT_LMB(&lmb, ram, ram_size, 3, 0x40000000, 0x30000,
+		   0x40030000, 0x20000, 0x40070000, 0x10000);
+
+	ret = lmb_reserve_flags(&lmb, 0x40050000, 0x10000, LMB_NOMAP);
+	ut_asserteq(ret, 0);
+	ASSERT_LMB(&lmb, ram, ram_size, 4, 0x40000000, 0x30000,
+		   0x40030000, 0x20000, 0x40050000, 0x10000);
+
+	/* merge with 2 adjacent regions */
+	ret = lmb_reserve_flags(&lmb, 0x40060000, 0x10000, LMB_NOMAP);
+	ut_asserteq(ret, 2);
+	ASSERT_LMB(&lmb, ram, ram_size, 3, 0x40000000, 0x30000,
+		   0x40030000, 0x20000, 0x40050000, 0x30000);
+
+	ut_asserteq(lmb_is_nomap(&lmb.reserved.region[0]), 1);
+	ut_asserteq(lmb_is_nomap(&lmb.reserved.region[1]), 0);
+	ut_asserteq(lmb_is_nomap(&lmb.reserved.region[2]), 1);
+
+	return 0;
+}
+
+DM_TEST(lib_test_lmb_flags,
+	UT_TESTF_SCAN_PDATA | UT_TESTF_SCAN_FDT);
-- 
2.17.1

