diff --git a/.cz.json b/.cz.json
index 5ed24f4..3c28d3c 100644
--- a/.cz.json
+++ b/.cz.json
@@ -93,12 +93,16 @@
                     "scopes": ["sve"]
                 },
                 {
+                    "title": "System Register Trace Extensions (FEAT_ETMv4, FEAT_ETE and FEAT_ETEv1.1)",
+                    "scopes": ["sys-reg-trace", "sys_reg_trace"]
+                },
+                {
                     "title": "Trace Buffer Extension (FEAT_TRBE)",
                     "scopes": ["trbe"]
                 },
                 {
-                    "title": "Self-hosted Trace Extensions (FEAT_TRF)",
-                    "scopes": ["trf", "sys_reg_trace"]
+                    "title": "Self-hosted Trace Extension (FEAT_TRF)",
+                    "scopes": ["trf"]
                 }
             ]
         },
@@ -805,6 +809,10 @@
                 {
                     "title": "STM32 Image",
                     "scopes": ["stm32image", "tools/stm32image"]
+                },
+                {
+                    "title": "fiptool",
+                    "scopes": ["fiptool"]
                 }
             ]
         },
diff --git a/Makefile b/Makefile
index fd60b5f..419e1b9 100644
--- a/Makefile
+++ b/Makefile
@@ -267,6 +267,16 @@
 ENABLE_FEAT_SB		= 	1
 endif
 
+# Determine and enable FEAT_FGT to access HDFGRTR_EL2 register for v8.6 and higher versions.
+ifeq "8.6" "$(word 1, $(sort 8.6 $(ARM_ARCH_MAJOR).$(ARM_ARCH_MINOR)))"
+ENABLE_FEAT_FGT		=	1
+endif
+
+# Determine and enable FEAT_ECV to access CNTPOFF_EL2 register for v8.6 and higher versions.
+ifeq "8.6" "$(word 1, $(sort 8.6 $(ARM_ARCH_MAJOR).$(ARM_ARCH_MINOR)))"
+ENABLE_FEAT_ECV		=	1
+endif
+
 ifneq ($(findstring armclang,$(notdir $(CC))),)
 TF_CFLAGS_aarch32	=	-target arm-arm-none-eabi $(march32-directive)
 TF_CFLAGS_aarch64	=	-target aarch64-arm-none-eabi $(march64-directive)
@@ -1041,6 +1051,9 @@
         ENABLE_FEAT_HCX \
         ENABLE_MPMM \
         ENABLE_MPMM_FCONF \
+        ENABLE_FEAT_FGT \
+        ENABLE_FEAT_AMUv1 \
+        ENABLE_FEAT_ECV \
 )))
 
 $(eval $(call assert_numerics,\
@@ -1153,6 +1166,9 @@
         ENABLE_FEAT_HCX \
         ENABLE_MPMM \
         ENABLE_MPMM_FCONF \
+        ENABLE_FEAT_FGT \
+        ENABLE_FEAT_AMUv1 \
+        ENABLE_FEAT_ECV \
 )))
 
 ifeq (${SANITIZE_UB},trap)
diff --git a/docs/about/release-information.rst b/docs/about/release-information.rst
index b65571d..b3553ae 100644
--- a/docs/about/release-information.rst
+++ b/docs/about/release-information.rst
@@ -48,6 +48,8 @@
 +-----------------+---------------------------+------------------------------+
 | v2.6            | 4th week of Nov '21       | 2nd week of Nov '21          |
 +-----------------+---------------------------+------------------------------+
+| v2.7            | 2nd week of May '22       | 4th week of Apr '22          |
++-----------------+---------------------------+------------------------------+
 
 Removal of Deprecated Interfaces
 --------------------------------
@@ -61,7 +63,7 @@
 |                                | Date        | after   |                                                         |
 |                                |             | Release |                                                         |
 +================================+=============+=========+=========================================================+
-|                                |             |         |                                                         |
+| STM32MP_USE_STM32IMAGE macro   |   Dec '21   |   2.7   | FIP is the recommended boot method for STM32MP          |
 +--------------------------------+-------------+---------+---------------------------------------------------------+
 
 --------------
diff --git a/docs/change-log.md b/docs/change-log.md
index f0cb352..7f36d01 100644
--- a/docs/change-log.md
+++ b/docs/change-log.md
@@ -63,15 +63,18 @@
 
     - enable SVE for the secure world ([0c5e7d1](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0c5e7d1ce376cabcebebc43dbf238fe4482ab2dc))
 
+  - **System Register Trace Extensions (FEAT_ETMv4, FEAT_ETE and FEAT_ETEv1.1)**
+
+    - enable trace system registers access from lower NS ELs ([d4582d3](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d4582d30885673987240cf01fd4f5d2e6780e84c))
+    - initialize trap settings of trace system registers access ([2031d61](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2031d6166a58623ae59034bc2353fcd2fabe9c30))
+
   - **Trace Buffer Extension (FEAT_TRBE)**
 
     - enable access to trace buffer control registers from lower NS EL ([813524e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/813524ea9d2e4138246b8f77a772299e52fb33bc))
     - initialize trap settings of trace buffer control registers access ([40ff907](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/40ff90747098ed9d2a09894d1a886c10ca76cee6))
 
-  - **Self-hosted Trace Extensions (FEAT_TRF)**
+  - **Self-hosted Trace Extension (FEAT_TRF)**
 
-    - enable trace system registers access from lower NS ELs ([d4582d3](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d4582d30885673987240cf01fd4f5d2e6780e84c))
-    - initialize trap settings of trace system registers access ([2031d61](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2031d6166a58623ae59034bc2353fcd2fabe9c30))
     - enable trace filter control register access from lower NS EL ([8fcd3d9](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/8fcd3d9600bb2cb6809c6fc68f945ce3ad89633d))
     - initialize trap settings of trace filter control registers access ([5de20ec](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5de20ece38f782c8459f546a08c6a97b9e0f5bc5))
 
@@ -324,6 +327,7 @@
 
     - add support for Hayes CPU ([7bd8dfb](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7bd8dfb85a8bf5c22d6a39f4538b89cc748090d1))
     - add support for Hunter CPU ([fb9e5f7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/fb9e5f7bb76e9764b3ecd7973668c851015fa1b4))
+    - add support for Demeter CPU ([f4616ef](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f4616efafbc1004f1330f515b898e7617e338875))
     - workaround for Cortex A78 AE erratum 1941500 ([47d6f5f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/47d6f5ff16d1f2ad009d630a381054b10fa0a06f))
     - workaround for Cortex A78 AE erratum 1951502 ([8913047](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/8913047a52e646877812617a2d98cff99494487b))
 
diff --git a/docs/components/secure-partition-manager.rst b/docs/components/secure-partition-manager.rst
index 4faabf9..a598e5f 100644
--- a/docs/components/secure-partition-manager.rst
+++ b/docs/components/secure-partition-manager.rst
@@ -533,7 +533,10 @@
 
 SP manifests provide an optional boot order attribute meant to resolve
 dependencies such as an SP providing a service required to properly boot
-another SP.
+another SP. SPMC boots the SPs in accordance to the boot order attribute,
+lowest to the highest value. If the boot order attribute is absent from the FF-A
+manifest, the SP is treated as if it had the highest boot order value
+(i.e. lowest booting priority).
 
 It is possible for an SP to call into another SP through a direct request
 provided the latter SP has already been booted.
diff --git a/docs/design/cpu-specific-build-macros.rst b/docs/design/cpu-specific-build-macros.rst
index 9d0dd5e..7075ca6 100644
--- a/docs/design/cpu-specific-build-macros.rst
+++ b/docs/design/cpu-specific-build-macros.rst
@@ -441,6 +441,19 @@
 -  ``ERRATA_N2_2280757``: This applies errata 2280757 workaround to Neoverse-N2
    CPU. This needs to be enabled for revision r0p0 of the CPU and is still open.
 
+For Cortex-X2, the following errata build flags are defined :
+
+-  ``ERRATA_X2_2002765``: This applies errata 2002765 workaround to Cortex-X2
+   CPU. This needs to be enabled for revisions r0p0, r1p0, and r2p0 of the CPU,
+   it is still open.
+
+-  ``ERRATA_X2_2058056``: This applies errata 2058056 workaround to Cortex-X2
+   CPU. This needs to be enabled for revisions r0p0, r1p0, and r2p0 of the CPU,
+   it is still open.
+
+-  ``ERRATA_X2_2083908``: This applies errata 2083908 workaround to Cortex-X2
+   CPU. This needs to be enabled for revision r2p0 of the CPU, it is still open.
+
 DSU Errata Workarounds
 ----------------------
 
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index 7662a14..d77875e 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -246,6 +246,24 @@
    builds, but this behaviour can be overridden in each platform's Makefile or
    in the build command line.
 
+-  ``ENABLE_FEAT_AMUv1``: Boolean option to enable access to the HAFGRTR_EL2
+   (Hypervisor Activity Monitors Fine-Grained Read Trap Register) during EL2
+   to EL3 context save/restore operations. It is an optional feature available
+   on v8.4 and onwards and must be set to 1 alongside ``ENABLE_FEAT_FGT``, to
+   access the HAFGRTR_EL2 register. Defaults to ``0``.
+
+-  ``ENABLE_FEAT_ECV``: Boolean option to enable support for the Enhanced Counter
+   Virtualization feature, allowing for access to the CNTPOFF_EL2 (Counter-timer
+   Physical Offset register) during EL2 to EL3 context save/restore operations.
+   Its a mandatory architectural feature in Armv8.6 and defaults to ``1`` for
+   v8.6 or later CPUs.
+
+-  ``ENABLE_FEAT_FGT``: Boolean option to enable support for FGT (Fine Grain Traps)
+   feature allowing for access to the HDFGRTR_EL2 (Hypervisor Debug Fine-Grained
+   Read Trap Register)  during EL2 to EL3 context save/restore operations.
+   Its a mandatory architectural feature in Armv8.6 and defaults to ``1`` for
+   v8.6 or later CPUs.
+
 -  ``ENABLE_FEAT_HCX``: This option sets the bit SCR_EL3.HXEn in EL3 to allow
    access to HCRX_EL2 (extended hypervisor control register) from EL2 as well as
    adding HCRX_EL2 to the EL2 context save/restore operations.
diff --git a/docs/getting_started/porting-guide.rst b/docs/getting_started/porting-guide.rst
index 92ff39f..24af13e 100644
--- a/docs/getting_started/porting-guide.rst
+++ b/docs/getting_started/porting-guide.rst
@@ -1195,7 +1195,7 @@
 ::
 
     Argument : unsigned int, image_info_t *
-    Return   : void
+    Return   : int
 
 When the MEASURED_BOOT flag is enabled:
 
@@ -1204,7 +1204,25 @@
 -  On the Arm FVP port, this function measures the given image using its
    passed id and information and then records that measurement in the
    Event Log buffer.
--  This function must return 0 on success, a negative error code otherwise.
+-  This function must return 0 on success, a signed integer error code
+   otherwise.
+
+When the MEASURED_BOOT flag is disabled, this function doesn't do anything.
+
+Function : plat_mboot_measure_critical_data()
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+::
+
+    Argument : unsigned int, const void *, size_t
+    Return   : int
+
+When the MEASURED_BOOT flag is enabled:
+
+-  This function measures the given critical data structure and records its
+   measurement using the measured boot backend driver.
+-  This function must return 0 on success, a signed integer error code
+   otherwise.
 
 When the MEASURED_BOOT flag is disabled, this function doesn't do anything.
 
diff --git a/docs/threat_model/index.rst b/docs/threat_model/index.rst
index b5ede69..335937e 100644
--- a/docs/threat_model/index.rst
+++ b/docs/threat_model/index.rst
@@ -15,6 +15,7 @@
 
    threat_model
    threat_model_spm
+   threat_model_fvp_r
 
 --------------
 
diff --git a/docs/threat_model/threat_model.rst b/docs/threat_model/threat_model.rst
index 4a31e79..072babc 100644
--- a/docs/threat_model/threat_model.rst
+++ b/docs/threat_model/threat_model.rst
@@ -1,4 +1,4 @@
-Generic threat model
+Generic Threat Model
 ********************
 
 ************************
diff --git a/docs/threat_model/threat_model_fvp_r.rst b/docs/threat_model/threat_model_fvp_r.rst
new file mode 100644
index 0000000..c1462bb
--- /dev/null
+++ b/docs/threat_model/threat_model_fvp_r.rst
@@ -0,0 +1,97 @@
+fvp_r-Platform Threat Model
+***************************
+
+************************
+Introduction
+************************
+This document provides a threat model for TF-A fvp_r platform.
+
+************************
+Target of Evaluation
+************************
+In this threat model, the target of evaluation is the fvp_r platform of Trusted
+Firmware for A-class Processors (TF-A).  The fvp_r platform provides limited
+support of AArch64 R-class Processors (v8-R64).
+
+This is a delta document, only pointing out differences from the general TF-A
+threat-model document, :ref:`Generic Threat Model`
+
+BL1 Only
+========
+The most fundamental difference between the threat model for the current fvp_r
+implementation compared to the general TF-A threat model, is that fvp_r is
+currently limited to BL1 only.  Any threats from the general TF-A threat model
+unrelated to BL1 are therefore not relevant to the fvp_r implementation.
+
+The fvp_r BL1 implementation directly loads a customer/partner-defined runtime
+system.  The threat model for that runtime system, being partner-defined, is
+out-of-scope for this threat-model.
+
+Relatedly, all exceptions, synchronous and asynchronous, are disabled during BL1
+execution.  So, any references to exceptions are not relevant.
+
+EL3 is Unsupported and All Secure
+=================================
+v8-R64 cores do not support EL3, and (essentially) all operation is defined as
+Secure-mode.  Therefore:
+
+    - Any threats regarding NS operation are not relevant.
+
+    - Any mentions of SMCs are also not relevant.
+
+    - Anything otherwise-relevant code running in EL3 is instead run in EL2.
+
+MPU instead of MMU
+==================
+v8-R64 cores, running in EL2, use an MPU for memory management, rather than an
+MMU.  The MPU in the fvp_r implementation is configured to function effectively
+identically with the MMU for the usual BL1 implementation.  There are
+memory-map differences, but the MPU configuration is functionally equivalent.
+
+No AArch32 Support
+==================
+Another substantial difference between v8-A and v8-R64 cores is that v8-R64 does
+not support AArch32.  However, this is not believed to have any threat-modeling
+ramifications.
+
+
+Threat Assessment
+=================
+For this section, please reference the Threat Assessment under the general TF-A
+threat-model document, :ref:`Generic Threat Model`
+
+The following threats from that document are still relevant to the fvp_r
+implementation:
+
+    - ID 01:  An attacker can mangle firmware images to execute arbitrary code.
+
+    - ID 03:  An attacker can use Time-of-Check-Time-of-Use (TOCTOU) attack to
+      bypass image authentication during the boot process.
+
+    - ID 04:  An attacker with physical access can execute arbitrary image by
+      bypassing the signature verification stage using clock- or power-glitching
+      techniques.
+
+    - ID 05:  Information leak via UART logs such as crashes
+
+    - ID 06:  An attacker can read sensitive data and execute arbitrary code
+      through the external debug and trace interface.
+
+    - ID 08:  Memory corruption due to memory overflows and lack of boundary
+      checking when accessing resources could allow an attacker to execute 
+      arbitrary code, modify some state variable to change the normal flow of
+      the program, or leak sensitive.
+
+    - ID 11:  Misconfiguration of the Memory Protection Unit (MPU) may allow
+      normal world software to access sensitive data or execute arbitrary code.
+      Arguably, MPUs having fewer memory regions, there may be a temptation to
+      share memory regions, making this a greater threat.  However, since the
+      fvp_r implementation is limited to BL1, since BL1's regions are fixed,
+      and since the MPU configuration is equivalent with that for the fvp
+      platform and others, this is not expected to be a concern.
+
+
+
+--------------
+
+*Copyright (c) 2021, Arm Limited. All rights reserved.*
diff --git a/docs/threat_model/threat_model_spm.rst b/docs/threat_model/threat_model_spm.rst
index 4db4c66..a7bc2a9 100644
--- a/docs/threat_model/threat_model_spm.rst
+++ b/docs/threat_model/threat_model_spm.rst
@@ -1,4 +1,4 @@
-SPMC threat model
+SPMC Threat Model
 *****************
 
 ************************
diff --git a/drivers/arm/css/scmi/scmi_private.h b/drivers/arm/css/scmi/scmi_private.h
index 61437f6..a684ca5 100644
--- a/drivers/arm/css/scmi/scmi_private.h
+++ b/drivers/arm/css/scmi/scmi_private.h
@@ -136,7 +136,7 @@
 	uint64_t res_b; /* Reserved */
 	uint32_t flags;
 	volatile uint32_t len;
-	uint32_t msg_header;
+	volatile uint32_t msg_header;
 	uint32_t payload[];
 } mailbox_mem_t;
 
diff --git a/drivers/arm/gic/v3/gicv3_private.h b/drivers/arm/gic/v3/gicv3_private.h
index 93ee1a1..3af0500 100644
--- a/drivers/arm/gic/v3/gicv3_private.h
+++ b/drivers/arm/gic/v3/gicv3_private.h
@@ -171,7 +171,7 @@
 static inline u_register_t gicd_irouter_val_from_mpidr(u_register_t mpidr,
 						       unsigned int irm)
 {
-	return (mpidr & ~(U(0xff) << 24)) |
+	return (mpidr & MPIDR_AFFINITY_MASK) |
 		((irm & IROUTER_IRM_MASK) << IROUTER_IRM_SHIFT);
 }
 
diff --git a/drivers/auth/crypto_mod.c b/drivers/auth/crypto_mod.c
index c63ff08..127eb0d 100644
--- a/drivers/auth/crypto_mod.c
+++ b/drivers/auth/crypto_mod.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -114,8 +114,9 @@
  *   data_ptr, data_len: data to be hashed
  *   output: resulting hash
  */
-int crypto_mod_calc_hash(unsigned int alg, void *data_ptr,
-			 unsigned int data_len, unsigned char *output)
+int crypto_mod_calc_hash(enum crypto_md_algo alg, void *data_ptr,
+			 unsigned int data_len,
+			 unsigned char output[CRYPTO_MD_MAX_SIZE])
 {
 	assert(data_ptr != NULL);
 	assert(data_len != 0);
diff --git a/drivers/auth/mbedtls/mbedtls_common.mk b/drivers/auth/mbedtls/mbedtls_common.mk
index 53ebe30..54c819c 100644
--- a/drivers/auth/mbedtls/mbedtls_common.mk
+++ b/drivers/auth/mbedtls/mbedtls_common.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015-2020, Arm Limited. All rights reserved.
+# Copyright (c) 2015-2021, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -96,6 +96,18 @@
     TF_MBEDTLS_USE_AES_GCM	:=	0
 endif
 
+ifeq ($(MEASURED_BOOT),1)
+    ifeq (${TPM_HASH_ALG}, sha256)
+        TF_MBEDTLS_TPM_HASH_ALG_ID	:=	TF_MBEDTLS_SHA256
+    else ifeq (${TPM_HASH_ALG}, sha384)
+        TF_MBEDTLS_TPM_HASH_ALG_ID	:=	TF_MBEDTLS_SHA384
+    else ifeq (${TPM_HASH_ALG}, sha512)
+        TF_MBEDTLS_TPM_HASH_ALG_ID	:=	TF_MBEDTLS_SHA512
+    else
+        $(error "TPM_HASH_ALG not defined.")
+    endif
+endif
+
 # Needs to be set to drive mbed TLS configuration correctly
 $(eval $(call add_defines,\
     $(sort \
@@ -105,6 +117,10 @@
         TF_MBEDTLS_USE_AES_GCM \
 )))
 
+ifeq ($(MEASURED_BOOT),1)
+  $(eval $(call add_define,TF_MBEDTLS_TPM_HASH_ALG_ID))
+endif
+
 $(eval $(call MAKE_LIB,mbedtls))
 
 endif
diff --git a/drivers/auth/mbedtls/mbedtls_crypto.c b/drivers/auth/mbedtls/mbedtls_crypto.c
index 6d6efb5..114e6ad 100644
--- a/drivers/auth/mbedtls/mbedtls_crypto.c
+++ b/drivers/auth/mbedtls/mbedtls_crypto.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -24,6 +24,16 @@
 
 #define LIB_NAME		"mbed TLS"
 
+#if MEASURED_BOOT
+/*
+ * CRYPTO_MD_MAX_SIZE value is as per current stronger algorithm available
+ * so make sure that mbed TLS MD maximum size must be lesser than this.
+ */
+CASSERT(CRYPTO_MD_MAX_SIZE >= MBEDTLS_MD_MAX_SIZE,
+	assert_mbedtls_md_size_overflow);
+
+#endif /* MEASURED_BOOT */
+
 /*
  * AlgorithmIdentifier  ::=  SEQUENCE  {
  *     algorithm               OBJECT IDENTIFIER,
@@ -211,21 +221,45 @@
 
 #if MEASURED_BOOT
 /*
+ * Map a generic crypto message digest algorithm to the corresponding macro used
+ * by Mbed TLS.
+ */
+static inline mbedtls_md_type_t md_type(enum crypto_md_algo algo)
+{
+	switch (algo) {
+	case CRYPTO_MD_SHA512:
+		return MBEDTLS_MD_SHA512;
+	case CRYPTO_MD_SHA384:
+		return MBEDTLS_MD_SHA384;
+	case CRYPTO_MD_SHA256:
+		return MBEDTLS_MD_SHA256;
+	default:
+		/* Invalid hash algorithm. */
+		return MBEDTLS_MD_NONE;
+	}
+}
+
+/*
  * Calculate a hash
  *
  * output points to the computed hash
  */
-int calc_hash(unsigned int alg, void *data_ptr,
-	      unsigned int data_len, unsigned char *output)
+static int calc_hash(enum crypto_md_algo md_algo, void *data_ptr,
+		     unsigned int data_len,
+		     unsigned char output[CRYPTO_MD_MAX_SIZE])
 {
 	const mbedtls_md_info_t *md_info;
 
-	md_info = mbedtls_md_info_from_type((mbedtls_md_type_t)alg);
+	md_info = mbedtls_md_info_from_type(md_type(md_algo));
 	if (md_info == NULL) {
 		return CRYPTO_ERR_HASH;
 	}
 
-	/* Calculate the hash of the data */
+	/*
+	 * Calculate the hash of the data, it is safe to pass the
+	 * 'output' hash buffer pointer considering its size is always
+	 * bigger than or equal to MBEDTLS_MD_MAX_SIZE.
+	 */
 	return mbedtls_md(md_info, data_ptr, data_len, output);
 }
 #endif /* MEASURED_BOOT */
diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
new file mode 100644
index 0000000..4cbc0f7
--- /dev/null
+++ b/drivers/clk/clk.c
@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ * Author(s): Ludovic Barre, <ludovic.barre@st.com> for STMicroelectronics.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdbool.h>
+
+#include <drivers/clk.h>
+
+static const struct clk_ops *ops;
+
+int clk_enable(unsigned long id)
+{
+	assert((ops != NULL) && (ops->enable != NULL));
+
+	return ops->enable(id);
+}
+
+void clk_disable(unsigned long id)
+{
+	assert((ops != NULL) && (ops->disable != NULL));
+
+	ops->disable(id);
+}
+
+unsigned long clk_get_rate(unsigned long id)
+{
+	assert((ops != NULL) && (ops->get_rate != NULL));
+
+	return ops->get_rate(id);
+}
+
+int clk_get_parent(unsigned long id)
+{
+	assert((ops != NULL) && (ops->get_parent != NULL));
+
+	return ops->get_parent(id);
+}
+
+bool clk_is_enabled(unsigned long id)
+{
+	assert((ops != NULL) && (ops->is_enabled != NULL));
+
+	return ops->is_enabled(id);
+}
+
+/*
+ * Initialize the clk. The fields in the provided clk
+ * ops pointer must be valid.
+ */
+void clk_register(const struct clk_ops *ops_ptr)
+{
+	assert((ops_ptr != NULL) &&
+	       (ops_ptr->enable != NULL) &&
+	       (ops_ptr->disable != NULL) &&
+	       (ops_ptr->get_rate != NULL) &&
+	       (ops_ptr->get_parent != NULL) &&
+	       (ops_ptr->is_enabled != NULL));
+
+	ops = ops_ptr;
+}
diff --git a/drivers/marvell/comphy/phy-comphy-3700.c b/drivers/marvell/comphy/phy-comphy-3700.c
index a3e414c..1a97753 100644
--- a/drivers/marvell/comphy/phy-comphy-3700.c
+++ b/drivers/marvell/comphy/phy-comphy-3700.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2018 Marvell International Ltd.
+ * Copyright (C) 2018-2021 Marvell International Ltd.
  *
  * SPDX-License-Identifier:     BSD-3-Clause
  * https://spdx.org/licenses
@@ -301,7 +301,7 @@
 	}
 
 	/* Clear phy isolation mode to make it work in normal mode */
-	offset =  COMPHY_ISOLATION_CTRL_REG + SATAPHY_LANE2_REG_BASE_OFFSET;
+	offset =  COMPHY_ISOLATION_CTRL + SATAPHY_LANE2_REG_BASE_OFFSET;
 	comphy_sata_set_indirect(comphy_indir_regs, offset, 0, PHY_ISOLATE_MODE);
 
 	/* 0. Check the Polarity invert bits */
@@ -310,21 +310,21 @@
 	if (invert & COMPHY_POLARITY_RXD_INVERT)
 		data |= RXD_INVERT_BIT;
 
-	offset = COMPHY_SYNC_PATTERN_REG + SATAPHY_LANE2_REG_BASE_OFFSET;
+	offset = COMPHY_SYNC_PATTERN + SATAPHY_LANE2_REG_BASE_OFFSET;
 	comphy_sata_set_indirect(comphy_indir_regs, offset, data, TXD_INVERT_BIT |
 				 RXD_INVERT_BIT);
 
 	/* 1. Select 40-bit data width width */
-	offset = COMPHY_LOOPBACK_REG0 + SATAPHY_LANE2_REG_BASE_OFFSET;
+	offset = COMPHY_DIG_LOOPBACK_EN + SATAPHY_LANE2_REG_BASE_OFFSET;
 	comphy_sata_set_indirect(comphy_indir_regs, offset, DATA_WIDTH_40BIT,
 				 SEL_DATA_WIDTH_MASK);
 
 	/* 2. Select reference clock(25M) and PHY mode (SATA) */
 	offset = COMPHY_POWER_PLL_CTRL + SATAPHY_LANE2_REG_BASE_OFFSET;
 	if (get_ref_clk() == 40)
-		ref_clk = REF_CLOCK_SPEED_40M;
+		ref_clk = REF_FREF_SEL_SERDES_40MHZ;
 	else
-		ref_clk = REF_CLOCK_SPEED_25M;
+		ref_clk = REF_FREF_SEL_SERDES_25MHZ;
 
 	comphy_sata_set_indirect(comphy_indir_regs, offset, ref_clk | PHY_MODE_SATA,
 				 REF_FREF_SEL_MASK | PHY_MODE_MASK);
@@ -352,7 +352,7 @@
 
 	/* Polling status */
 	mmio_write_32(comphy_indir_regs + COMPHY_LANE2_INDIR_ADDR_OFFSET,
-		      COMPHY_LOOPBACK_REG0 + SATAPHY_LANE2_REG_BASE_OFFSET);
+		      COMPHY_DIG_LOOPBACK_EN + SATAPHY_LANE2_REG_BASE_OFFSET);
 
 	ret = polling_with_timeout(comphy_indir_regs +
 				   COMPHY_LANE2_INDIR_DATA_OFFSET,
@@ -401,8 +401,8 @@
 	 *    PHY TXP/TXN output to idle state during PHY initialization
 	 * 3. Set PHY input port PIN_PU_PLL=0, PIN_PU_RX=0, PIN_PU_TX=0.
 	 */
-	data = PIN_PU_IVEREF_BIT | PIN_TX_IDLE_BIT | PIN_RESET_COMPHY_BIT;
-	mask = PIN_RESET_CORE_BIT | PIN_PU_PLL_BIT | PIN_PU_RX_BIT |
+	data = PIN_PU_IVREF_BIT | PIN_TX_IDLE_BIT | PIN_RESET_COMPHY_BIT;
+	mask = data | PIN_RESET_CORE_BIT | PIN_PU_PLL_BIT | PIN_PU_RX_BIT |
 		PIN_PU_TX_BIT;
 	offset = MVEBU_COMPHY_REG_BASE + COMPHY_PHY_CFG1_OFFSET(comphy_index);
 	reg_set(offset, data, mask);
@@ -422,8 +422,8 @@
 		data |= SD_SPEED_1_25_G << GEN_TX_SEL_OFFSET;
 	} else if (mode == COMPHY_2500BASEX_MODE) {
 		/* 2500Base-X, SerDes speed 3.125G */
-		data |= SD_SPEED_2_5_G << GEN_RX_SEL_OFFSET;
-		data |= SD_SPEED_2_5_G << GEN_TX_SEL_OFFSET;
+		data |= SD_SPEED_3_125_G << GEN_RX_SEL_OFFSET;
+		data |= SD_SPEED_3_125_G << GEN_TX_SEL_OFFSET;
 	} else {
 		/* Other rates are not supported */
 		ERROR("unsupported SGMII speed on comphy lane%d\n",
@@ -450,16 +450,16 @@
 	 */
 	data = 0;
 	mask = PHY_REF_CLK_SEL;
-	reg_set16(SGMIIPHY_ADDR(COMPHY_MISC_REG0_ADDR, sd_ip_addr), data, mask);
+	reg_set16(SGMIIPHY_ADDR(COMPHY_MISC_CTRL0, sd_ip_addr), data, mask);
 
 	/*
 	 * 9. Set correct reference clock frequency in COMPHY register
 	 * REF_FREF_SEL.
 	 */
 	if (get_ref_clk() == 40)
-		data = REF_CLOCK_SPEED_50M;
+		data = REF_FREF_SEL_SERDES_50MHZ;
 	else
-		data = REF_CLOCK_SPEED_25M;
+		data = REF_FREF_SEL_SERDES_25MHZ;
 
 	mask = REF_FREF_SEL_MASK;
 	reg_set16(SGMIIPHY_ADDR(COMPHY_POWER_PLL_CTRL, sd_ip_addr), data, mask);
@@ -477,7 +477,8 @@
 	 */
 	data = DATA_WIDTH_10BIT;
 	mask = SEL_DATA_WIDTH_MASK;
-	reg_set16(SGMIIPHY_ADDR(COMPHY_LOOPBACK_REG0, sd_ip_addr), data, mask);
+	reg_set16(SGMIIPHY_ADDR(COMPHY_DIG_LOOPBACK_EN, sd_ip_addr),
+		  data, mask);
 
 	/*
 	 * 12. As long as DFE function needs to be enabled in any mode,
@@ -523,7 +524,7 @@
 	if (invert & COMPHY_POLARITY_RXD_INVERT)
 		data |= RXD_INVERT_BIT;
 	mask = TXD_INVERT_BIT | RXD_INVERT_BIT;
-	reg_set16(SGMIIPHY_ADDR(COMPHY_SYNC_PATTERN_REG, sd_ip_addr), data, mask);
+	reg_set16(SGMIIPHY_ADDR(COMPHY_SYNC_PATTERN, sd_ip_addr), data, mask);
 
 	/*
 	 * 17. Set PHY input ports PIN_PU_PLL, PIN_PU_TX and PIN_PU_RX to 1 to
@@ -645,68 +646,68 @@
 	 */
 	mask = PRD_TXDEEMPH0_MASK | PRD_TXMARGIN_MASK | PRD_TXSWING_MASK |
 		CFG_TX_ALIGN_POS_MASK;
-	usb3_reg_set(reg_base, COMPHY_REG_LANE_CFG0_ADDR, PRD_TXDEEMPH0_MASK,
-		     mask);
+	usb3_reg_set(reg_base, COMPHY_LANE_CFG0, PRD_TXDEEMPH0_MASK, mask);
 
 	/*
 	 * 2. Set BIT0: enable transmitter in high impedance mode
 	 *    Set BIT[3:4]: delay 2 clock cycles for HiZ off latency
 	 *    Set BIT6: Tx detect Rx at HiZ mode
 	 *    Unset BIT15: set to 0 to set USB3 De-emphasize level to -3.5db
-	 *            together with bit 0 of COMPHY_REG_LANE_CFG0_ADDR register
+	 *            together with bit 0 of COMPHY_LANE_CFG0 register
 	 */
 	mask = PRD_TXDEEMPH1_MASK | TX_DET_RX_MODE | GEN2_TX_DATA_DLY_MASK |
 		TX_ELEC_IDLE_MODE_EN;
 	data = TX_DET_RX_MODE | GEN2_TX_DATA_DLY_DEFT | TX_ELEC_IDLE_MODE_EN;
-	usb3_reg_set(reg_base, COMPHY_REG_LANE_CFG1_ADDR, data, mask);
+	usb3_reg_set(reg_base, COMPHY_LANE_CFG1, data, mask);
 
 	/*
 	 * 3. Set Spread Spectrum Clock Enabled
 	 */
-	usb3_reg_set(reg_base, COMPHY_REG_LANE_CFG4_ADDR,
+	usb3_reg_set(reg_base, COMPHY_LANE_CFG4,
 		     SPREAD_SPECTRUM_CLK_EN, SPREAD_SPECTRUM_CLK_EN);
 
 	/*
 	 * 4. Set Override Margining Controls From the MAC:
 	 *    Use margining signals from lane configuration
 	 */
-	usb3_reg_set(reg_base, COMPHY_REG_TEST_MODE_CTRL_ADDR,
+	usb3_reg_set(reg_base, COMPHY_TEST_MODE_CTRL,
 		     MODE_MARGIN_OVERRIDE, REG_16_BIT_MASK);
 
 	/*
 	 * 5. Set Lane-to-Lane Bundle Clock Sampling Period = per PCLK cycles
 	 *    set Mode Clock Source = PCLK is generated from REFCLK
 	 */
-	usb3_reg_set(reg_base, COMPHY_REG_GLOB_CLK_SRC_LO_ADDR, 0x0,
-		     (MODE_CLK_SRC | BUNDLE_PERIOD_SEL | BUNDLE_PERIOD_SCALE |
-		      BUNDLE_SAMPLE_CTRL | PLL_READY_DLY));
+	usb3_reg_set(reg_base, COMPHY_CLK_SRC_LO, 0x0,
+		     (MODE_CLK_SRC | BUNDLE_PERIOD_SEL |
+		      BUNDLE_PERIOD_SCALE_MASK | BUNDLE_SAMPLE_CTRL |
+		      PLL_READY_DLY_MASK));
 
 	/*
 	 * 6. Set G2 Spread Spectrum Clock Amplitude at 4K
 	 */
-	usb3_reg_set(reg_base, COMPHY_REG_GEN2_SET_2,
-		     G2_TX_SSC_AMP_VALUE_20, G2_TX_SSC_AMP_MASK);
+	usb3_reg_set(reg_base, COMPHY_GEN2_SET2,
+		     GS2_TX_SSC_AMP_VALUE_20, GS2_TX_SSC_AMP_MASK);
 
 	/*
 	 * 7. Unset G3 Spread Spectrum Clock Amplitude
 	 *    set G3 TX and RX Register Master Current Select
 	 */
-	mask = G3_TX_SSC_AMP_MASK | G3_VREG_RXTX_MAS_ISET_MASK |
-		RSVD_PH03FH_6_0_MASK;
-	usb3_reg_set(reg_base, COMPHY_REG_GEN2_SET_3,
-		     G3_VREG_RXTX_MAS_ISET_60U, mask);
+	mask = GS2_TX_SSC_AMP_MASK | GS2_VREG_RXTX_MAS_ISET_MASK |
+		GS2_RSVD_6_0_MASK;
+	usb3_reg_set(reg_base, COMPHY_GEN3_SET2,
+		     GS2_VREG_RXTX_MAS_ISET_60U, mask);
 
 	/*
 	 * 8. Check crystal jumper setting and program the Power and PLL Control
 	 * accordingly Change RX wait
 	 */
 	if (get_ref_clk() == 40) {
-		ref_clk = REF_CLOCK_SPEED_40M;
+		ref_clk = REF_FREF_SEL_PCIE_USB3_40MHZ;
 		cfg = CFG_PM_RXDLOZ_WAIT_12_UNIT;
 
 	} else {
 		/* 25 MHz */
-		ref_clk = USB3_REF_CLOCK_SPEED_25M;
+		ref_clk = REF_FREF_SEL_PCIE_USB3_25MHZ;
 		cfg = CFG_PM_RXDLOZ_WAIT_7_UNIT;
 	}
 
@@ -720,24 +721,24 @@
 	mask = CFG_PM_OSCCLK_WAIT_MASK | CFG_PM_RXDEN_WAIT_MASK |
 		CFG_PM_RXDLOZ_WAIT_MASK;
 	data = CFG_PM_RXDEN_WAIT_1_UNIT  | cfg;
-	usb3_reg_set(reg_base, COMPHY_REG_PWR_MGM_TIM1_ADDR, data, mask);
+	usb3_reg_set(reg_base, COMPHY_PWR_MGM_TIM1, data, mask);
 
 	/*
 	 * 9. Enable idle sync
 	 */
-	data = UNIT_CTRL_DEFAULT_VALUE | IDLE_SYNC_EN;
-	usb3_reg_set(reg_base, COMPHY_REG_UNIT_CTRL_ADDR, data, REG_16_BIT_MASK);
+	data = IDLE_SYNC_EN_DEFAULT_VALUE | IDLE_SYNC_EN;
+	usb3_reg_set(reg_base, COMPHY_IDLE_SYNC_EN, data, REG_16_BIT_MASK);
 
 	/*
 	 * 10. Enable the output of 500M clock
 	 */
-	data = MISC_REG0_DEFAULT_VALUE | CLK500M_EN;
-	usb3_reg_set(reg_base, COMPHY_MISC_REG0_ADDR, data, REG_16_BIT_MASK);
+	data = MISC_CTRL0_DEFAULT_VALUE | CLK500M_EN;
+	usb3_reg_set(reg_base, COMPHY_MISC_CTRL0, data, REG_16_BIT_MASK);
 
 	/*
 	 * 11. Set 20-bit data width
 	 */
-	usb3_reg_set(reg_base, COMPHY_LOOPBACK_REG0, DATA_WIDTH_20BIT,
+	usb3_reg_set(reg_base, COMPHY_DIG_LOOPBACK_EN, DATA_WIDTH_20BIT,
 		     REG_16_BIT_MASK);
 
 	/*
@@ -758,32 +759,31 @@
 		data |= RXD_INVERT_BIT;
 	}
 	mask = TXD_INVERT_BIT | RXD_INVERT_BIT;
-	usb3_reg_set(reg_base, COMPHY_SYNC_PATTERN_REG, data, mask);
+	usb3_reg_set(reg_base, COMPHY_SYNC_PATTERN, data, mask);
 
 	/*
 	 * 14. Set max speed generation to USB3.0 5Gbps
 	 */
-	usb3_reg_set(reg_base, COMPHY_SYNC_MASK_GEN_REG, PHY_GEN_USB3_5G,
+	usb3_reg_set(reg_base, COMPHY_SYNC_MASK_GEN, PHY_GEN_MAX_USB3_5G,
 		     PHY_GEN_MAX_MASK);
 
 	/*
 	 * 15. Set capacitor value for FFE gain peaking to 0xF
 	 */
-	usb3_reg_set(reg_base, COMPHY_REG_GEN3_SETTINGS_3,
-		     COMPHY_GEN_FFE_CAP_SEL_VALUE, COMPHY_GEN_FFE_CAP_SEL_MASK);
+	usb3_reg_set(reg_base, COMPHY_GEN2_SET3,
+		     GS3_FFE_CAP_SEL_VALUE, GS3_FFE_CAP_SEL_MASK);
 
 	/*
 	 * 16. Release SW reset
 	 */
 	data = MODE_CORE_CLK_FREQ_SEL | MODE_PIPE_WIDTH_32 | MODE_REFDIV_BY_4;
-	usb3_reg_set(reg_base, COMPHY_REG_GLOB_PHY_CTRL0_ADDR, data,
-		     REG_16_BIT_MASK);
+	usb3_reg_set(reg_base, COMPHY_RST_CLK_CTRL, data, REG_16_BIT_MASK);
 
 	/* Wait for > 55 us to allow PCLK be enabled */
 	udelay(PLL_SET_DELAY_US);
 
 	if (comphy_index == COMPHY_LANE2) {
-		data = COMPHY_REG_LANE_STATUS1_ADDR + USB3PHY_LANE2_REG_BASE_OFFSET;
+		data = COMPHY_LANE_STAT1 + USB3PHY_LANE2_REG_BASE_OFFSET;
 		mmio_write_32(reg_base + COMPHY_LANE2_INDIR_ADDR_OFFSET,
 			      data);
 
@@ -791,7 +791,7 @@
 		ret = polling_with_timeout(addr, TXDCLK_PCLK_EN, TXDCLK_PCLK_EN,
 					   COMPHY_PLL_TIMEOUT, REG_32BIT);
 	} else {
-		ret = polling_with_timeout(LANE_STATUS1_ADDR(USB3) + reg_base,
+		ret = polling_with_timeout(LANE_STAT1_ADDR(USB3) + reg_base,
 					   TXDCLK_PCLK_EN, TXDCLK_PCLK_EN,
 					   COMPHY_PLL_TIMEOUT, REG_16BIT);
 	}
@@ -826,11 +826,11 @@
 		  USE_MAX_PLL_RATE_EN, USE_MAX_PLL_RATE_EN);
 
 	/* 2. Select 20 bit SERDES interface. */
-	reg_set16(GLOB_CLK_SRC_LO_ADDR(PCIE) + COMPHY_SD_ADDR,
+	reg_set16(CLK_SRC_LO_ADDR(PCIE) + COMPHY_SD_ADDR,
 		  CFG_SEL_20B, CFG_SEL_20B);
 
 	/* 3. Force to use reg setting for PCIe mode */
-	reg_set16(MISC_REG1_ADDR(PCIE) + COMPHY_SD_ADDR,
+	reg_set16(MISC_CTRL1_ADDR(PCIE) + COMPHY_SD_ADDR,
 		  SEL_BITS_PCIE_FORCE, SEL_BITS_PCIE_FORCE);
 
 	/* 4. Change RX wait */
@@ -840,12 +840,12 @@
 		   CFG_PM_RXDLOZ_WAIT_MASK));
 
 	/* 5. Enable idle sync */
-	reg_set16(UNIT_CTRL_ADDR(PCIE) + COMPHY_SD_ADDR,
-		  UNIT_CTRL_DEFAULT_VALUE | IDLE_SYNC_EN, REG_16_BIT_MASK);
+	reg_set16(IDLE_SYNC_EN_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  IDLE_SYNC_EN_DEFAULT_VALUE | IDLE_SYNC_EN, REG_16_BIT_MASK);
 
 	/* 6. Enable the output of 100M/125M/500M clock */
-	reg_set16(MISC_REG0_ADDR(PCIE) + COMPHY_SD_ADDR,
-		  MISC_REG0_DEFAULT_VALUE | CLK500M_EN | TXDCLK_2X_SEL | CLK100M_125M_EN,
+	reg_set16(MISC_CTRL0_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  MISC_CTRL0_DEFAULT_VALUE | CLK500M_EN | TXDCLK_2X_SEL | CLK100M_125M_EN,
 		  REG_16_BIT_MASK);
 
 	/*
@@ -859,9 +859,9 @@
 	 */
 
 	if (get_ref_clk() == 40)
-		ref_clk = REF_CLOCK_SPEED_40M;
+		ref_clk = REF_FREF_SEL_PCIE_USB3_40MHZ;
 	else
-		ref_clk = PCIE_REF_CLOCK_SPEED_25M;
+		ref_clk = REF_FREF_SEL_PCIE_USB3_25MHZ;
 
 	reg_set16(PWR_PLL_CTRL_ADDR(PCIE) + COMPHY_SD_ADDR,
 		  (PU_IVREF_BIT | PU_PLL_BIT | PU_RX_BIT | PU_TX_BIT |
@@ -881,17 +881,17 @@
 		data |= RXD_INVERT_BIT;
 	}
 	mask = TXD_INVERT_BIT | RXD_INVERT_BIT;
-	reg_set16(SYNC_PATTERN_REG_ADDR(PCIE) + COMPHY_SD_ADDR, data, mask);
+	reg_set16(SYNC_PATTERN_ADDR(PCIE) + COMPHY_SD_ADDR, data, mask);
 
 	/* 11. Release SW reset */
-	reg_set16(GLOB_PHY_CTRL0_ADDR(PCIE) + COMPHY_SD_ADDR,
-		  MODE_CORE_CLK_FREQ_SEL | MODE_PIPE_WIDTH_32,
-		  SOFT_RESET | MODE_REFDIV);
+	data = MODE_CORE_CLK_FREQ_SEL | MODE_PIPE_WIDTH_32;
+	mask = data | SOFT_RESET | MODE_REFDIV_MASK;
+	reg_set16(RST_CLK_CTRL_ADDR(PCIE) + COMPHY_SD_ADDR, data, mask);
 
 	/* Wait for > 55 us to allow PCLK be enabled */
 	udelay(PLL_SET_DELAY_US);
 
-	ret = polling_with_timeout(LANE_STATUS1_ADDR(PCIE) + COMPHY_SD_ADDR,
+	ret = polling_with_timeout(LANE_STAT1_ADDR(PCIE) + COMPHY_SD_ADDR,
 				   TXDCLK_PCLK_EN, TXDCLK_PCLK_EN,
 				   COMPHY_PLL_TIMEOUT, REG_16BIT);
 	if (ret) {
@@ -961,7 +961,7 @@
 	debug_enter();
 
 	/* Set phy isolation mode */
-	offset = COMPHY_ISOLATION_CTRL_REG + SATAPHY_LANE2_REG_BASE_OFFSET;
+	offset = COMPHY_ISOLATION_CTRL + SATAPHY_LANE2_REG_BASE_OFFSET;
 	comphy_sata_set_indirect(comphy_indir_regs, offset, PHY_ISOLATE_MODE,
 				 PHY_ISOLATE_MODE);
 
@@ -1025,7 +1025,7 @@
 
 	/* Polling status */
 	mmio_write_32(comphy_indir_regs + COMPHY_LANE2_INDIR_ADDR_OFFSET,
-	       COMPHY_LOOPBACK_REG0 + SATAPHY_LANE2_REG_BASE_OFFSET);
+	       COMPHY_DIG_LOOPBACK_EN + SATAPHY_LANE2_REG_BASE_OFFSET);
 	addr = comphy_indir_regs + COMPHY_LANE2_INDIR_DATA_OFFSET;
 	data = polling_with_timeout(addr, PLL_READY_TX_BIT, PLL_READY_TX_BIT,
 				    COMPHY_PLL_TIMEOUT, REG_32BIT);
diff --git a/drivers/marvell/comphy/phy-comphy-3700.h b/drivers/marvell/comphy/phy-comphy-3700.h
index 94056f1..ed07624 100644
--- a/drivers/marvell/comphy/phy-comphy-3700.h
+++ b/drivers/marvell/comphy/phy-comphy-3700.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2018 Marvell International Ltd.
+ * Copyright (C) 2018-2021 Marvell International Ltd.
  *
  * SPDX-License-Identifier:     BSD-3-Clause
  * https://spdx.org/licenses
@@ -13,11 +13,11 @@
 #define REG_16_BIT_MASK				0xFFFF
 
 #define COMPHY_SELECTOR_PHY_REG			0xFC
-/* bit0: 0: Lane0 is GBE0; 1: Lane1 is PCIE */
+/* bit0: 0: Lane1 is GbE0; 1: Lane1 is PCIE */
 #define COMPHY_SELECTOR_PCIE_GBE0_SEL_BIT	BIT(0)
-/* bit4: 0: Lane1 is GBE1; 1: Lane1 is USB3 */
+/* bit4: 0: Lane0 is GbE1; 1: Lane0 is USB3 */
 #define COMPHY_SELECTOR_USB3_GBE1_SEL_BIT	BIT(4)
-/* bit8: 0: Lane1 is USB, Lane2 is SATA; 1: Lane2 is USB3 */
+/* bit8: 0: Lane0 is USB3 instead of GbE1, Lane2 is SATA; 1: Lane2 is USB3 */
 #define COMPHY_SELECTOR_USB3_PHY_SEL_BIT	BIT(8)
 
 /* SATA PHY register offset */
@@ -53,12 +53,11 @@
 #define PLL_LOCK_BIT			BIT(8)
 #define REF_FREF_SEL_OFFSET		0
 #define REF_FREF_SEL_MASK		(0x1F << REF_FREF_SEL_OFFSET)
-#define REF_CLOCK_SPEED_25M		(0x1 << REF_FREF_SEL_OFFSET)
-#define REF_CLOCK_SPEED_30M		(0x2 << REF_FREF_SEL_OFFSET)
-#define PCIE_REF_CLOCK_SPEED_25M	REF_CLOCK_SPEED_30M
-#define USB3_REF_CLOCK_SPEED_25M	REF_CLOCK_SPEED_30M
-#define REF_CLOCK_SPEED_40M		(0x3 << REF_FREF_SEL_OFFSET)
-#define REF_CLOCK_SPEED_50M		(0x4 << REF_FREF_SEL_OFFSET)
+#define REF_FREF_SEL_SERDES_25MHZ	(0x1 << REF_FREF_SEL_OFFSET)
+#define REF_FREF_SEL_SERDES_40MHZ	(0x3 << REF_FREF_SEL_OFFSET)
+#define REF_FREF_SEL_SERDES_50MHZ	(0x4 << REF_FREF_SEL_OFFSET)
+#define REF_FREF_SEL_PCIE_USB3_25MHZ	(0x2 << REF_FREF_SEL_OFFSET)
+#define REF_FREF_SEL_PCIE_USB3_40MHZ	(0x3 << REF_FREF_SEL_OFFSET)
 #define PHY_MODE_OFFSET			5
 #define PHY_MODE_MASK			(7 << PHY_MODE_OFFSET)
 #define PHY_MODE_SATA			(0x0 << PHY_MODE_OFFSET)
@@ -73,11 +72,9 @@
 #define SPEED_PLL_MASK			(0x3F << SPEED_PLL_OFFSET)
 #define SPEED_PLL_VALUE_16		(0x10 << SPEED_PLL_OFFSET)
 
-#define COMPHY_RESERVED_REG		0x0E
-#define PHYCTRL_FRM_PIN_BIT		BIT(13)
-
-#define COMPHY_LOOPBACK_REG0		0x23
-#define DIG_LB_EN_ADDR(unit)		(COMPHY_LOOPBACK_REG0 * PHY_SHFT(unit))
+#define COMPHY_DIG_LOOPBACK_EN		0x23
+#define DIG_LOOPBACK_EN_ADDR(unit)	(COMPHY_DIG_LOOPBACK_EN * \
+					 PHY_SHFT(unit))
 #define SEL_DATA_WIDTH_OFFSET		10
 #define SEL_DATA_WIDTH_MASK		(0x3 << SEL_DATA_WIDTH_OFFSET)
 #define DATA_WIDTH_10BIT		(0x0 << SEL_DATA_WIDTH_OFFSET)
@@ -85,80 +82,71 @@
 #define DATA_WIDTH_40BIT		(0x2 << SEL_DATA_WIDTH_OFFSET)
 #define PLL_READY_TX_BIT		BIT(4)
 
-#define COMPHY_SYNC_PATTERN_REG		0x24
-#define SYNC_PATTERN_REG_ADDR(unit)	(COMPHY_SYNC_PATTERN_REG * \
-					 PHY_SHFT(unit))
+#define COMPHY_SYNC_PATTERN		0x24
+#define SYNC_PATTERN_ADDR(unit)		(COMPHY_SYNC_PATTERN * PHY_SHFT(unit))
 #define TXD_INVERT_BIT			BIT(10)
 #define RXD_INVERT_BIT			BIT(11)
 
-#define COMPHY_SYNC_MASK_GEN_REG	0x25
+#define COMPHY_SYNC_MASK_GEN		0x25
 #define PHY_GEN_MAX_OFFSET		10
 #define PHY_GEN_MAX_MASK		(3 << PHY_GEN_MAX_OFFSET)
-#define PHY_GEN_USB3_5G			(1 << PHY_GEN_MAX_OFFSET)
+#define PHY_GEN_MAX_USB3_5G		(1 << PHY_GEN_MAX_OFFSET)
 
-#define COMPHY_ISOLATION_CTRL_REG	0x26
-#define ISOLATION_CTRL_REG_ADDR(unit)	(COMPHY_ISOLATION_CTRL_REG * \
-					 PHY_SHFT(unit))
+#define COMPHY_ISOLATION_CTRL		0x26
+#define ISOLATION_CTRL_ADDR(unit)	(COMPHY_ISOLATION_REG * PHY_SHFT(unit))
 #define PHY_ISOLATE_MODE		BIT(15)
 
-#define COMPHY_MISC_REG0_ADDR		0x4F
-#define MISC_REG0_ADDR(unit)		(COMPHY_MISC_REG0_ADDR * PHY_SHFT(unit))
+#define COMPHY_GEN2_SET2		0x3e
+#define GEN2_SET2_ADDR(unit)		(COMPHY_GEN2_SET2 * PHY_SHFT(unit))
+#define GS2_TX_SSC_AMP_VALUE_20		BIT(14)
+#define GS2_TX_SSC_AMP_OFF		9
+#define GS2_TX_SSC_AMP_LEN		7
+#define GS2_TX_SSC_AMP_MASK		(((1 << GS2_TX_SSC_AMP_LEN) - 1) << \
+					 GS2_TX_SSC_AMP_OFF)
+#define GS2_VREG_RXTX_MAS_ISET_OFF	7
+#define GS2_VREG_RXTX_MAS_ISET_60U	(0 << GS2_VREG_RXTX_MAS_ISET_OFF)
+#define GS2_VREG_RXTX_MAS_ISET_80U	(1 << GS2_VREG_RXTX_MAS_ISET_OFF)
+#define GS2_VREG_RXTX_MAS_ISET_100U	(2 << GS2_VREG_RXTX_MAS_ISET_OFF)
+#define GS2_VREG_RXTX_MAS_ISET_120U	(3 << GS2_VREG_RXTX_MAS_ISET_OFF)
+#define GS2_VREG_RXTX_MAS_ISET_MASK	(BIT(7) | BIT(8))
+#define GS2_RSVD_6_0_OFF		0
+#define GS2_RSVD_6_0_LEN		7
+#define GS2_RSVD_6_0_MASK		(((1 << GS2_RSVD_6_0_LEN) - 1) << \
+					 GS2_RSVD_6_0_OFF)
+
+#define COMPHY_GEN3_SET2		0x3f
+#define GEN3_SET2_ADDR(unit)		(COMPHY_GEN3_SET2 * PHY_SHFT(unit))
+
+#define COMPHY_IDLE_SYNC_EN		0x48
+#define IDLE_SYNC_EN_ADDR(unit)		(COMPHY_IDLE_SYNC_EN * PHY_SHFT(unit))
+#define IDLE_SYNC_EN			BIT(12)
+#define IDLE_SYNC_EN_DEFAULT_VALUE	0x60
+
+#define COMPHY_MISC_CTRL0		0x4F
+#define MISC_CTRL0_ADDR(unit)		(COMPHY_MISC_CTRL0 * PHY_SHFT(unit))
 #define CLK100M_125M_EN			BIT(4)
 #define TXDCLK_2X_SEL			BIT(6)
 #define CLK500M_EN			BIT(7)
 #define PHY_REF_CLK_SEL			BIT(10)
-#define MISC_REG0_DEFAULT_VALUE		0xA00D
+#define MISC_CTRL0_DEFAULT_VALUE	0xA00D
 
-#define COMPHY_REG_GEN2_SET_2	0x3e
-#define GEN2_SETTING_2_ADDR(unit)	(COMPHY_REG_GEN2_SET_2 * PHY_SHFT(unit))
-#define G2_TX_SSC_AMP_VALUE_20		BIT(14)
-#define G2_TX_SSC_AMP_OFF		9
-#define G2_TX_SSC_AMP_LEN		7
-#define G2_TX_SSC_AMP_MASK		(((1 << G2_TX_SSC_AMP_LEN) - 1) << \
-					 G2_TX_SSC_AMP_OFF)
-
-#define COMPHY_REG_GEN2_SET_3	0x3f
-#define GEN2_SETTING_3_ADDR(unit)	(COMPHY_REG_GEN2_SET_3 * PHY_SHFT(unit))
-#define G3_TX_SSC_AMP_OFF		9
-#define G3_TX_SSC_AMP_LEN		7
-#define G3_TX_SSC_AMP_MASK		(((1 << G2_TX_SSC_AMP_LEN) - 1) << \
-					 G2_TX_SSC_AMP_OFF)
-#define G3_VREG_RXTX_MAS_ISET_OFF	7
-#define G3_VREG_RXTX_MAS_ISET_60U	(0 << G3_VREG_RXTX_MAS_ISET_OFF)
-#define G3_VREG_RXTX_MAS_ISET_80U	(1 << G3_VREG_RXTX_MAS_ISET_OFF)
-#define G3_VREG_RXTX_MAS_ISET_100U	(2 << G3_VREG_RXTX_MAS_ISET_OFF)
-#define G3_VREG_RXTX_MAS_ISET_120U	(3 << G3_VREG_RXTX_MAS_ISET_OFF)
-#define G3_VREG_RXTX_MAS_ISET_MASK	(BIT(7) | BIT(8))
-#define RSVD_PH03FH_6_0_OFF		0
-#define RSVD_PH03FH_6_0_LEN		7
-#define RSVD_PH03FH_6_0_MASK		(((1 << RSVD_PH03FH_6_0_LEN) - 1) << \
-					 RSVD_PH03FH_6_0_OFF)
-
-#define COMPHY_REG_UNIT_CTRL_ADDR	0x48
-#define UNIT_CTRL_ADDR(unit)		(COMPHY_REG_UNIT_CTRL_ADDR * \
-					 PHY_SHFT(unit))
-#define IDLE_SYNC_EN			BIT(12)
-#define UNIT_CTRL_DEFAULT_VALUE		0x60
-
-#define COMPHY_MISC_REG1_ADDR		0x73
-#define MISC_REG1_ADDR(unit)		(COMPHY_MISC_REG1_ADDR * PHY_SHFT(unit))
+#define COMPHY_MISC_CTRL1		0x73
+#define MISC_CTRL1_ADDR(unit)		(COMPHY_MISC_CTRL1 * PHY_SHFT(unit))
 #define SEL_BITS_PCIE_FORCE		BIT(15)
 
-#define COMPHY_REG_GEN3_SETTINGS_3	0x112
-#define COMPHY_GEN_FFE_CAP_SEL_MASK	0xF
-#define COMPHY_GEN_FFE_CAP_SEL_VALUE	0xF
+#define COMPHY_GEN2_SET3		0x112
+#define GS3_FFE_CAP_SEL_MASK		0xF
+#define GS3_FFE_CAP_SEL_VALUE		0xF
 
-#define COMPHY_REG_LANE_CFG0_ADDR	0x180
-#define LANE_CFG0_ADDR(unit)		(COMPHY_REG_LANE_CFG0_ADDR * \
-					 PHY_SHFT(unit))
+#define COMPHY_LANE_CFG0		0x180
+#define LANE_CFG0_ADDR(unit)		(COMPHY_LANE_CFG0 * PHY_SHFT(unit))
 #define PRD_TXDEEMPH0_MASK		BIT(0)
 #define PRD_TXMARGIN_MASK		(BIT(1) | BIT(2) | BIT(3))
 #define PRD_TXSWING_MASK		BIT(4)
 #define CFG_TX_ALIGN_POS_MASK		(BIT(5) | BIT(6) | BIT(7) | BIT(8))
 
-#define COMPHY_REG_LANE_CFG1_ADDR	0x181
-#define LANE_CFG1_ADDR(unit)		(COMPHY_REG_LANE_CFG1_ADDR * \
-					 PHY_SHFT(unit))
+#define COMPHY_LANE_CFG1		0x181
+#define LANE_CFG1_ADDR(unit)		(COMPHY_LANE_CFG1 * PHY_SHFT(unit))
 #define PRD_TXDEEMPH1_MASK		BIT(15)
 #define USE_MAX_PLL_RATE_EN		BIT(9)
 #define TX_DET_RX_MODE			BIT(6)
@@ -166,21 +154,17 @@
 #define GEN2_TX_DATA_DLY_DEFT		(2 << 3)
 #define TX_ELEC_IDLE_MODE_EN		BIT(0)
 
-#define COMPHY_REG_LANE_STATUS1_ADDR	0x183
-#define LANE_STATUS1_ADDR(unit)		(COMPHY_REG_LANE_STATUS1_ADDR * \
-					 PHY_SHFT(unit))
+#define COMPHY_LANE_STAT1		0x183
+#define LANE_STAT1_ADDR(unit)		(COMPHY_LANE_STAT1 * PHY_SHFT(unit))
 #define TXDCLK_PCLK_EN				BIT(0)
 
-#define COMPHY_REG_LANE_CFG4_ADDR	0x188
-#define LANE_CFG4_ADDR(unit)		(COMPHY_REG_LANE_CFG4_ADDR * \
-					 PHY_SHFT(unit))
+#define COMPHY_LANE_CFG4		0x188
+#define LANE_CFG4_ADDR(unit)		(COMPHY_LANE_CFG4 * PHY_SHFT(unit))
 #define SPREAD_SPECTRUM_CLK_EN		BIT(7)
 
-#define COMPHY_REG_GLOB_PHY_CTRL0_ADDR	0x1C1
-#define GLOB_PHY_CTRL0_ADDR(unit)	(COMPHY_REG_GLOB_PHY_CTRL0_ADDR * \
-					 PHY_SHFT(unit))
+#define COMPHY_RST_CLK_CTRL		0x1C1
+#define RST_CLK_CTRL_ADDR(unit)		(COMPHY_RST_CLK_CTRL * PHY_SHFT(unit))
 #define SOFT_RESET			BIT(0)
-#define MODE_REFDIV			0x30
 #define MODE_CORE_CLK_FREQ_SEL		BIT(9)
 #define MODE_PIPE_WIDTH_32		BIT(3)
 #define MODE_REFDIV_OFFSET		4
@@ -188,24 +172,21 @@
 #define MODE_REFDIV_MASK		(0x3 << MODE_REFDIV_OFFSET)
 #define MODE_REFDIV_BY_4		(0x2 << MODE_REFDIV_OFFSET)
 
-#define COMPHY_REG_TEST_MODE_CTRL_ADDR	0x1C2
-#define TEST_MODE_CTRL_ADDR(unit)	(COMPHY_REG_TEST_MODE_CTRL_ADDR * \
-					 PHY_SHFT(unit))
+#define COMPHY_TEST_MODE_CTRL		0x1C2
+#define TEST_MODE_CTRL_ADDR(unit)	(COMPHY_TEST_MODE_CTRL * PHY_SHFT(unit))
 #define MODE_MARGIN_OVERRIDE		BIT(2)
 
-#define COMPHY_REG_GLOB_CLK_SRC_LO_ADDR	0x1C3
-#define GLOB_CLK_SRC_LO_ADDR(unit)	(COMPHY_REG_GLOB_CLK_SRC_LO_ADDR * \
-					 PHY_SHFT(unit))
+#define COMPHY_CLK_SRC_LO		0x1C3
+#define CLK_SRC_LO_ADDR(unit)		(COMPHY_CLK_SRC_LO * PHY_SHFT(unit))
 #define MODE_CLK_SRC			BIT(0)
 #define BUNDLE_PERIOD_SEL		BIT(1)
-#define BUNDLE_PERIOD_SCALE		(BIT(2) | BIT(3))
+#define BUNDLE_PERIOD_SCALE_MASK	(BIT(2) | BIT(3))
 #define BUNDLE_SAMPLE_CTRL		BIT(4)
-#define PLL_READY_DLY			(BIT(5) | BIT(6) | BIT(7))
+#define PLL_READY_DLY_MASK		(BIT(5) | BIT(6) | BIT(7))
 #define CFG_SEL_20B			BIT(15)
 
-#define COMPHY_REG_PWR_MGM_TIM1_ADDR	0x1D0
-#define PWR_MGM_TIM1_ADDR(unit)		(COMPHY_REG_PWR_MGM_TIM1_ADDR * \
-					 PHY_SHFT(unit))
+#define COMPHY_PWR_MGM_TIM1		0x1D0
+#define PWR_MGM_TIM1_ADDR(unit)		(COMPHY_PWR_MGM_TIM1 * PHY_SHFT(unit))
 #define CFG_PM_OSCCLK_WAIT_OFF		12
 #define CFG_PM_OSCCLK_WAIT_LEN		4
 #define CFG_PM_OSCCLK_WAIT_MASK		(((1 << CFG_PM_OSCCLK_WAIT_LEN) - 1) \
@@ -222,9 +203,18 @@
 #define CFG_PM_RXDLOZ_WAIT_7_UNIT	(7 << CFG_PM_RXDLOZ_WAIT_OFF)
 #define CFG_PM_RXDLOZ_WAIT_12_UNIT	(0xC << CFG_PM_RXDLOZ_WAIT_OFF)
 
+/*
+ * This register is not from PHY lane register space. It only exists in the
+ * indirect register space, before the actual PHY lane 2 registers. So the
+ * offset is absolute, not relative to SATAPHY_LANE2_REG_BASE_OFFSET.
+ * It is used only for SATA PHY initialization.
+ */
+#define COMPHY_RESERVED_REG		0x0E
+#define PHYCTRL_FRM_PIN_BIT		BIT(13)
+
 /* SGMII */
 #define COMPHY_PHY_CFG1_OFFSET(lane)	((1 - (lane)) * 0x28)
-#define PIN_PU_IVEREF_BIT		BIT(1)
+#define PIN_PU_IVREF_BIT		BIT(1)
 #define PIN_RESET_CORE_BIT		BIT(11)
 #define PIN_RESET_COMPHY_BIT		BIT(12)
 #define PIN_PU_PLL_BIT			BIT(16)
@@ -237,11 +227,11 @@
 #define GEN_TX_SEL_MASK			(0xF << GEN_TX_SEL_OFFSET)
 #define PHY_RX_INIT_BIT			BIT(30)
 #define SD_SPEED_1_25_G			0x6
-#define SD_SPEED_2_5_G			0x8
+#define SD_SPEED_3_125_G		0x8
 
 /* COMPHY status reg:
- * lane0: PCIe/GbE0 PHY Status 1
- * lane1: USB3/GbE1 PHY Status 1
+ * lane0: USB3/GbE1 PHY Status 1
+ * lane1: PCIe/GbE0 PHY Status 1
  */
 #define COMPHY_PHY_STATUS_OFFSET(lane)	(0x18 + (1 - (lane)) * 0x28)
 #define PHY_RX_INIT_DONE_BIT		BIT(0)
diff --git a/drivers/marvell/comphy/phy-comphy-common.h b/drivers/marvell/comphy/phy-comphy-common.h
index c599437..ba5d255 100644
--- a/drivers/marvell/comphy/phy-comphy-common.h
+++ b/drivers/marvell/comphy/phy-comphy-common.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2018 Marvell International Ltd.
+ * Copyright (C) 2018-2021 Marvell International Ltd.
  *
  * SPDX-License-Identifier:     BSD-3-Clause
  * https://spdx.org/licenses
@@ -147,7 +147,7 @@
 	debug("<atf>: WR to addr = 0x%lx, data = 0x%x (mask = 0x%x) - ",
 	      addr, data, mask);
 	debug("old value = 0x%x ==> ", mmio_read_32(addr));
-	mmio_clrsetbits_32(addr, mask, data);
+	mmio_clrsetbits_32(addr, mask, data & mask);
 
 	debug("new val 0x%x\n", mmio_read_32(addr));
 }
@@ -159,7 +159,7 @@
 	debug("<atf>: WR to addr = 0x%lx, data = 0x%x (mask = 0x%x) - ",
 	      addr, data, mask);
 	debug("old value = 0x%x ==> ", mmio_read_16(addr));
-	mmio_clrsetbits_16(addr, mask, data);
+	mmio_clrsetbits_16(addr, mask, data & mask);
 
 	debug("new val 0x%x\n", mmio_read_16(addr));
 }
diff --git a/drivers/measured_boot/event_log/event_log.c b/drivers/measured_boot/event_log/event_log.c
index 1755dd9..792f235 100644
--- a/drivers/measured_boot/event_log/event_log.c
+++ b/drivers/measured_boot/event_log/event_log.c
@@ -13,10 +13,19 @@
 #include <common/debug.h>
 #include <drivers/auth/crypto_mod.h>
 #include <drivers/measured_boot/event_log/event_log.h>
-#include <mbedtls/md.h>
 
 #include <plat/common/platform.h>
 
+#if TPM_ALG_ID == TPM_ALG_SHA512
+#define	CRYPTO_MD_ID	CRYPTO_MD_SHA512
+#elif TPM_ALG_ID == TPM_ALG_SHA384
+#define	CRYPTO_MD_ID	CRYPTO_MD_SHA384
+#elif TPM_ALG_ID == TPM_ALG_SHA256
+#define	CRYPTO_MD_ID	CRYPTO_MD_SHA256
+#else
+#  error Invalid TPM algorithm.
+#endif /* TPM_ALG_ID */
+
 /* Running Event Log Pointer */
 static uint8_t *log_ptr;
 
@@ -245,20 +254,20 @@
 int event_log_measure_and_record(uintptr_t data_base, uint32_t data_size,
 				 uint32_t data_id)
 {
-	unsigned char hash_data[MBEDTLS_MD_MAX_SIZE];
+	unsigned char hash_data[CRYPTO_MD_MAX_SIZE];
 	int rc;
 	const event_log_metadata_t *metadata_ptr = plat_metadata_ptr;
 
 	/* Get the metadata associated with this image. */
-	while ((metadata_ptr->id != INVALID_ID) &&
+	while ((metadata_ptr->id != EVLOG_INVALID_ID) &&
 		(metadata_ptr->id != data_id)) {
 		metadata_ptr++;
 	}
-	assert(metadata_ptr->id != INVALID_ID);
+	assert(metadata_ptr->id != EVLOG_INVALID_ID);
 
 	/* Calculate hash */
-	rc = crypto_mod_calc_hash((unsigned int)MBEDTLS_MD_ID,
-				(void *)data_base, data_size, hash_data);
+	rc = crypto_mod_calc_hash(CRYPTO_MD_ID,
+				  (void *)data_base, data_size, hash_data);
 	if (rc != 0) {
 		return rc;
 	}
diff --git a/drivers/measured_boot/event_log/event_log.mk b/drivers/measured_boot/event_log/event_log.mk
index 37e5e29..d3fbbb5 100644
--- a/drivers/measured_boot/event_log/event_log.mk
+++ b/drivers/measured_boot/event_log/event_log.mk
@@ -12,35 +12,24 @@
 TPM_HASH_ALG			:=	sha256
 
 ifeq (${TPM_HASH_ALG}, sha512)
-    MBEDTLS_MD_ID		:=	MBEDTLS_MD_SHA512
     TPM_ALG_ID			:=	TPM_ALG_SHA512
     TCG_DIGEST_SIZE		:=	64U
 else ifeq (${TPM_HASH_ALG}, sha384)
-    MBEDTLS_MD_ID		:=	MBEDTLS_MD_SHA384
     TPM_ALG_ID			:=	TPM_ALG_SHA384
     TCG_DIGEST_SIZE		:=	48U
 else
-    MBEDTLS_MD_ID		:=	MBEDTLS_MD_SHA256
     TPM_ALG_ID			:=	TPM_ALG_SHA256
     TCG_DIGEST_SIZE		:=	32U
-endif
+endif #TPM_HASH_ALG
 
-
-# Set definitions for mbed TLS library and Measured Boot driver
+# Set definitions for Measured Boot driver.
 $(eval $(call add_defines,\
     $(sort \
-        MBEDTLS_MD_ID \
         TPM_ALG_ID \
         TCG_DIGEST_SIZE \
         EVENT_LOG_LEVEL \
 )))
 
-ifeq (${HASH_ALG}, sha256)
-    ifneq (${TPM_HASH_ALG}, sha256)
-        $(eval $(call add_define,MBEDTLS_SHA512_C))
-    endif
-endif
-
 MEASURED_BOOT_SRC_DIR	:= drivers/measured_boot/event_log/
 
 MEASURED_BOOT_SOURCES	:= ${MEASURED_BOOT_SRC_DIR}event_log.c		\
diff --git a/drivers/st/clk/stm32mp1_clk.c b/drivers/st/clk/stm32mp1_clk.c
index 3ebc376..684ab1a 100644
--- a/drivers/st/clk/stm32mp1_clk.c
+++ b/drivers/st/clk/stm32mp1_clk.c
@@ -17,6 +17,7 @@
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/generic_delay_timer.h>
 #include <drivers/st/stm32mp_clkfunc.h>
@@ -1157,17 +1158,19 @@
 	stm32mp1_clk_unlock(&refcount_lock);
 }
 
-void stm32mp_clk_enable(unsigned long id)
+static int stm32mp_clk_enable(unsigned long id)
 {
 	__stm32mp1_clk_enable(id, true);
+
+	return 0;
 }
 
-void stm32mp_clk_disable(unsigned long id)
+static void stm32mp_clk_disable(unsigned long id)
 {
 	__stm32mp1_clk_disable(id, true);
 }
 
-bool stm32mp_clk_is_enabled(unsigned long id)
+static bool stm32mp_clk_is_enabled(unsigned long id)
 {
 	int i;
 
@@ -1183,15 +1186,55 @@
 	return __clk_is_enabled(gate_ref(i));
 }
 
-unsigned long stm32mp_clk_get_rate(unsigned long id)
+static unsigned long stm32mp_clk_get_rate(unsigned long id)
 {
+	uintptr_t rcc_base = stm32mp_rcc_base();
 	int p = stm32mp1_clk_get_parent(id);
+	uint32_t prescaler, timpre;
+	unsigned long parent_rate;
 
 	if (p < 0) {
 		return 0;
 	}
 
-	return get_clock_rate(p);
+	parent_rate = get_clock_rate(p);
+
+	switch (id) {
+	case TIM2_K:
+	case TIM3_K:
+	case TIM4_K:
+	case TIM5_K:
+	case TIM6_K:
+	case TIM7_K:
+	case TIM12_K:
+	case TIM13_K:
+	case TIM14_K:
+		prescaler = mmio_read_32(rcc_base + RCC_APB1DIVR) &
+			    RCC_APBXDIV_MASK;
+		timpre = mmio_read_32(rcc_base + RCC_TIMG1PRER) &
+			 RCC_TIMGXPRER_TIMGXPRE;
+		break;
+
+	case TIM1_K:
+	case TIM8_K:
+	case TIM15_K:
+	case TIM16_K:
+	case TIM17_K:
+		prescaler = mmio_read_32(rcc_base + RCC_APB2DIVR) &
+			    RCC_APBXDIV_MASK;
+		timpre = mmio_read_32(rcc_base + RCC_TIMG2PRER) &
+			 RCC_TIMGXPRER_TIMGXPRE;
+		break;
+
+	default:
+		return parent_rate;
+	}
+
+	if (prescaler == 0U) {
+		return parent_rate;
+	}
+
+	return parent_rate * (timpre + 1U) * 2U;
 }
 
 static void stm32mp1_ls_osc_set(bool enable, uint32_t offset, uint32_t mask_on)
@@ -1744,6 +1787,8 @@
 	bool pll4_bootrom = false;
 	const fdt32_t *pkcs_cell;
 	void *fdt;
+	int stgen_p = stm32mp1_clk_get_parent(STGEN_K);
+	int usbphy_p = stm32mp1_clk_get_parent(USBPHY_K);
 
 	if (fdt_get_address(&fdt) == 0) {
 		return -FDT_ERR_NOTFOUND;
@@ -1843,6 +1888,13 @@
 							pllcfg[_PLL4],
 							plloff[_PLL4]);
 	}
+	/* Don't initialize PLL4, when used by BOOTROM */
+	if ((stm32mp_get_boot_itf_selected() ==
+	     BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB) &&
+	    ((stgen_p == (int)_PLL4_R) || (usbphy_p == (int)_PLL4_R))) {
+		pll4_bootrom = true;
+		pll4_preserve = true;
+	}
 
 	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
 		if (((i == _PLL3) && pll3_preserve) ||
@@ -1994,6 +2046,11 @@
 	if (pkcs_cell != NULL) {
 		bool ckper_disabled = false;
 		uint32_t j;
+		uint32_t usbreg_bootrom = 0U;
+
+		if (pll4_bootrom) {
+			usbreg_bootrom = mmio_read_32(rcc_base + RCC_USBCKSELR);
+		}
 
 		for (j = 0; j < ((uint32_t)len / sizeof(uint32_t)); j++) {
 			uint32_t pkcs = fdt32_to_cpu(pkcs_cell[j]);
@@ -2014,6 +2071,25 @@
 		if (ckper_disabled) {
 			stm32mp1_pkcs_config(CLK_CKPER_DISABLED);
 		}
+
+		if (pll4_bootrom) {
+			uint32_t usbreg_value, usbreg_mask;
+			const struct stm32mp1_clk_sel *sel;
+
+			sel = clk_sel_ref(_USBPHY_SEL);
+			usbreg_mask = (uint32_t)sel->msk << sel->src;
+			sel = clk_sel_ref(_USBO_SEL);
+			usbreg_mask |= (uint32_t)sel->msk << sel->src;
+
+			usbreg_value = mmio_read_32(rcc_base + RCC_USBCKSELR) &
+				       usbreg_mask;
+			usbreg_bootrom &= usbreg_mask;
+			if (usbreg_bootrom != usbreg_value) {
+				VERBOSE("forbidden new USB clk path\n");
+				VERBOSE("vs bootrom on USB boot\n");
+				return -FDT_ERR_BADVALUE;
+			}
+		}
 	}
 
 	/* Switch OFF HSI if not found in device-tree */
@@ -2231,11 +2307,21 @@
 	}
 }
 
+static const struct clk_ops stm32mp_clk_ops = {
+	.enable		= stm32mp_clk_enable,
+	.disable	= stm32mp_clk_disable,
+	.is_enabled	= stm32mp_clk_is_enabled,
+	.get_rate	= stm32mp_clk_get_rate,
+	.get_parent	= stm32mp1_clk_get_parent,
+};
+
 int stm32mp1_clk_probe(void)
 {
 	stm32mp1_osc_init();
 
 	sync_earlyboot_clocks_state();
 
+	clk_register(&stm32mp_clk_ops);
+
 	return 0;
 }
diff --git a/drivers/st/clk/stm32mp_clkfunc.c b/drivers/st/clk/stm32mp_clkfunc.c
index d57f120..c83b8ad 100644
--- a/drivers/st/clk/stm32mp_clkfunc.c
+++ b/drivers/st/clk/stm32mp_clkfunc.c
@@ -11,9 +11,11 @@
 #include <platform_def.h>
 
 #include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
 #include <drivers/st/stm32_gpio.h>
 #include <drivers/st/stm32mp_clkfunc.h>
 
+#define DT_UART_COMPAT		"st,stm32h7-uart"
 /*
  * Get the frequency of an oscillator from its name in device tree.
  * @param name: oscillator name
@@ -297,3 +299,32 @@
 	cuint++;
 	return (int)fdt32_to_cpu(*cuint);
 }
+
+/*
+ * Get the frequency of the specified UART instance.
+ * @param instance: UART interface registers base address.
+ * @return: clock frequency on success, 0 value on failure.
+ */
+unsigned long fdt_get_uart_clock_freq(uintptr_t instance)
+{
+	void *fdt;
+	int node;
+	int clk_id;
+
+	if (fdt_get_address(&fdt) == 0) {
+		return 0UL;
+	}
+
+	/* Check for UART nodes */
+	node = dt_match_instance_by_compatible(DT_UART_COMPAT, instance);
+	if (node < 0) {
+		return 0UL;
+	}
+
+	clk_id = fdt_get_clock_id(node);
+	if (clk_id < 0) {
+		return 0UL;
+	}
+
+	return clk_get_rate((unsigned long)clk_id);
+}
diff --git a/drivers/st/crypto/stm32_hash.c b/drivers/st/crypto/stm32_hash.c
index 317fd9e..6a1d476 100644
--- a/drivers/st/crypto/stm32_hash.c
+++ b/drivers/st/crypto/stm32_hash.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2019-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,6 +14,7 @@
 
 #include <arch_helpers.h>
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/st/stm32_hash.h>
 #include <drivers/st/stm32mp_reset.h>
@@ -189,7 +190,7 @@
 		return 0;
 	}
 
-	stm32mp_clk_enable(stm32_hash.clock);
+	clk_enable(stm32_hash.clock);
 
 	if (stm32_remain.length != 0U) {
 		uint32_t copysize;
@@ -231,7 +232,7 @@
 	}
 
 exit:
-	stm32mp_clk_disable(stm32_hash.clock);
+	clk_disable(stm32_hash.clock);
 
 	return ret;
 }
@@ -240,12 +241,12 @@
 {
 	int ret;
 
-	stm32mp_clk_enable(stm32_hash.clock);
+	clk_enable(stm32_hash.clock);
 
 	if (stm32_remain.length != 0U) {
 		ret = hash_write_data(stm32_remain.buffer);
 		if (ret != 0) {
-			stm32mp_clk_disable(stm32_hash.clock);
+			clk_disable(stm32_hash.clock);
 			return ret;
 		}
 
@@ -260,7 +261,7 @@
 
 	ret = hash_get_digest(digest);
 
-	stm32mp_clk_disable(stm32_hash.clock);
+	clk_disable(stm32_hash.clock);
 
 	return ret;
 }
@@ -280,11 +281,11 @@
 
 void stm32_hash_init(enum stm32_hash_algo_mode mode)
 {
-	stm32mp_clk_enable(stm32_hash.clock);
+	clk_enable(stm32_hash.clock);
 
 	hash_hw_init(mode);
 
-	stm32mp_clk_disable(stm32_hash.clock);
+	clk_disable(stm32_hash.clock);
 
 	zeromem(&stm32_remain, sizeof(stm32_remain));
 }
@@ -321,7 +322,7 @@
 	stm32_hash.base = hash_info.base;
 	stm32_hash.clock = hash_info.clock;
 
-	stm32mp_clk_enable(stm32_hash.clock);
+	clk_enable(stm32_hash.clock);
 
 	if (hash_info.reset >= 0) {
 		uint32_t id = (uint32_t)hash_info.reset;
@@ -335,7 +336,7 @@
 		}
 	}
 
-	stm32mp_clk_disable(stm32_hash.clock);
+	clk_disable(stm32_hash.clock);
 
 	return 0;
 }
diff --git a/drivers/st/ddr/stm32mp1_ddr.c b/drivers/st/ddr/stm32mp1_ddr.c
index 7d89d02..3db47c4 100644
--- a/drivers/st/ddr/stm32mp1_ddr.c
+++ b/drivers/st/ddr/stm32mp1_ddr.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2018-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (C) 2018-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
  */
@@ -12,6 +12,7 @@
 #include <arch.h>
 #include <arch_helpers.h>
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/st/stm32mp_pmic.h>
 #include <drivers/st/stm32mp1_ddr.h>
@@ -627,7 +628,7 @@
 	 */
 
 	/* Change Bypass Mode Frequency Range */
-	if (stm32mp_clk_get_rate(DDRPHYC) < 100000000U) {
+	if (clk_get_rate(DDRPHYC) < 100000000U) {
 		mmio_clrbits_32((uintptr_t)&priv->phy->dllgcr,
 				DDRPHYC_DLLGCR_BPS200);
 	} else {
diff --git a/drivers/st/ddr/stm32mp1_ram.c b/drivers/st/ddr/stm32mp1_ram.c
index b21c894..064e318 100644
--- a/drivers/st/ddr/stm32mp1_ram.c
+++ b/drivers/st/ddr/stm32mp1_ram.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2018-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (C) 2018-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
  */
@@ -13,6 +13,7 @@
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
 #include <drivers/st/stm32mp1_ddr.h>
 #include <drivers/st/stm32mp1_ddr_helpers.h>
 #include <drivers/st/stm32mp1_ram.h>
@@ -29,7 +30,7 @@
 
 	ddr_enable_clock();
 
-	ddrphy_clk = stm32mp_clk_get_rate(DDRPHYC);
+	ddrphy_clk = clk_get_rate(DDRPHYC);
 
 	VERBOSE("DDR: mem_speed (%d kHz), RCC %ld kHz\n",
 		mem_speed, ddrphy_clk / 1000U);
diff --git a/drivers/st/fmc/stm32_fmc2_nand.c b/drivers/st/fmc/stm32_fmc2_nand.c
index 453069b..e9ab6da 100644
--- a/drivers/st/fmc/stm32_fmc2_nand.c
+++ b/drivers/st/fmc/stm32_fmc2_nand.c
@@ -14,6 +14,7 @@
 #include <platform_def.h>
 
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/raw_nand.h>
 #include <drivers/st/stm32_fmc2_nand.h>
@@ -162,7 +163,7 @@
 static void stm32_fmc2_nand_setup_timing(void)
 {
 	struct stm32_fmc2_nand_timings tims;
-	unsigned long hclk = stm32mp_clk_get_rate(stm32_fmc2.clock_id);
+	unsigned long hclk = clk_get_rate(stm32_fmc2.clock_id);
 	unsigned long hclkp = FMC2_PSEC_PER_MSEC / (hclk / 1000U);
 	unsigned long timing, tar, tclr, thiz, twait;
 	unsigned long tset_mem, tset_att, thold_mem, thold_att;
@@ -909,7 +910,7 @@
 	}
 
 	/* Enable Clock */
-	stm32mp_clk_enable(stm32_fmc2.clock_id);
+	clk_enable(stm32_fmc2.clock_id);
 
 	/* Reset IP */
 	ret = stm32mp_reset_assert(stm32_fmc2.reset_id, TIMEOUT_US_1_MS);
diff --git a/drivers/st/gpio/stm32_gpio.c b/drivers/st/gpio/stm32_gpio.c
index 7d63262..5c54762 100644
--- a/drivers/st/gpio/stm32_gpio.c
+++ b/drivers/st/gpio/stm32_gpio.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2016-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,6 +14,7 @@
 
 #include <common/bl_common.h>
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/st/stm32_gpio.h>
 #include <drivers/st/stm32mp_clkfunc.h>
 #include <lib/mmio.h>
@@ -208,7 +209,7 @@
 
 	assert(pin <= GPIO_PIN_MAX);
 
-	stm32mp_clk_enable(clock);
+	clk_enable(clock);
 
 	mmio_clrbits_32(base + GPIO_MODE_OFFSET,
 			((uint32_t)GPIO_MODE_MASK << (pin << 1)));
@@ -254,7 +255,7 @@
 	VERBOSE("GPIO %u mode alternate high to 0x%x\n", bank,
 		mmio_read_32(base + GPIO_AFRH_OFFSET));
 
-	stm32mp_clk_disable(clock);
+	clk_disable(clock);
 
 	if (status == DT_SECURE) {
 		stm32mp_register_secure_gpio(bank, pin);
@@ -273,7 +274,7 @@
 
 	assert(pin <= GPIO_PIN_MAX);
 
-	stm32mp_clk_enable(clock);
+	clk_enable(clock);
 
 	if (secure) {
 		mmio_setbits_32(base + GPIO_SECR_OFFSET, BIT(pin));
@@ -281,5 +282,12 @@
 		mmio_clrbits_32(base + GPIO_SECR_OFFSET, BIT(pin));
 	}
 
-	stm32mp_clk_disable(clock);
+	clk_disable(clock);
+}
+
+void set_gpio_reset_cfg(uint32_t bank, uint32_t pin)
+{
+	set_gpio(bank, pin, GPIO_MODE_ANALOG, GPIO_SPEED_LOW,
+		 GPIO_NO_PULL, GPIO_ALTERNATE_(0), DT_DISABLED);
+	set_gpio_secure_cfg(bank, pin, stm32_gpio_is_secure_at_reset(bank));
 }
diff --git a/drivers/st/i2c/stm32_i2c.c b/drivers/st/i2c/stm32_i2c.c
index ed88052..bf6c3ee 100644
--- a/drivers/st/i2c/stm32_i2c.c
+++ b/drivers/st/i2c/stm32_i2c.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2016-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -13,6 +13,7 @@
 #include <platform_def.h>
 
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/st/stm32_gpio.h>
 #include <drivers/st/stm32_i2c.h>
@@ -158,7 +159,7 @@
 
 	hi2c->i2c_state = I2C_STATE_BUSY;
 
-	stm32mp_clk_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	/* Disable the selected I2C peripheral */
 	mmio_clrbits_32(hi2c->i2c_base_addr + I2C_CR1, I2C_CR1_PE);
@@ -220,11 +221,11 @@
 						I2C_ANALOGFILTER_DISABLE);
 	if (rc != 0) {
 		ERROR("Cannot initialize I2C analog filter (%d)\n", rc);
-		stm32mp_clk_disable(hi2c->clock);
+		clk_disable(hi2c->clock);
 		return rc;
 	}
 
-	stm32mp_clk_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 
 	return rc;
 }
@@ -548,7 +549,7 @@
 		return -EINVAL;
 	}
 
-	stm32mp_clk_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	hi2c->lock = 1;
 
@@ -648,7 +649,7 @@
 
 bail:
 	hi2c->lock = 0;
-	stm32mp_clk_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 
 	return rc;
 }
@@ -729,7 +730,7 @@
 		return  -EINVAL;
 	}
 
-	stm32mp_clk_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	hi2c->lock = 1;
 
@@ -817,7 +818,7 @@
 
 bail:
 	hi2c->lock = 0;
-	stm32mp_clk_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 
 	return rc;
 }
@@ -882,7 +883,7 @@
 		return rc;
 	}
 
-	stm32mp_clk_enable(hi2c->clock);
+	clk_enable(hi2c->clock);
 
 	hi2c->lock = 1;
 	hi2c->i2c_mode = I2C_MODE_NONE;
@@ -974,7 +975,7 @@
 
 bail:
 	hi2c->lock = 0;
-	stm32mp_clk_disable(hi2c->clock);
+	clk_disable(hi2c->clock);
 
 	return rc;
 }
diff --git a/drivers/st/iwdg/stm32_iwdg.c b/drivers/st/iwdg/stm32_iwdg.c
index c052b4d..74451d7 100644
--- a/drivers/st/iwdg/stm32_iwdg.c
+++ b/drivers/st/iwdg/stm32_iwdg.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -15,6 +15,7 @@
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <drivers/arm/gicv2.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/st/stm32_iwdg.h>
 #include <drivers/st/stm32mp_clkfunc.h>
@@ -61,12 +62,12 @@
 
 		/* 0x00000000 is not a valid address for IWDG peripherals */
 		if (iwdg->base != 0U) {
-			stm32mp_clk_enable(iwdg->clock);
+			clk_enable(iwdg->clock);
 
 			mmio_write_32(iwdg->base + IWDG_KR_OFFSET,
 				      IWDG_KR_RELOAD_KEY);
 
-			stm32mp_clk_disable(iwdg->clock);
+			clk_disable(iwdg->clock);
 		}
 	}
 }
diff --git a/drivers/st/mmc/stm32_sdmmc2.c b/drivers/st/mmc/stm32_sdmmc2.c
index d3adeab..9db97d4 100644
--- a/drivers/st/mmc/stm32_sdmmc2.c
+++ b/drivers/st/mmc/stm32_sdmmc2.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2018-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -15,6 +15,7 @@
 #include <arch.h>
 #include <arch_helpers.h>
 #include <common/debug.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/mmc.h>
 #include <drivers/st/stm32_gpio.h>
@@ -714,7 +715,7 @@
 		return -ENOMEM;
 	}
 
-	stm32mp_clk_enable(sdmmc2_params.clock_id);
+	clk_enable(sdmmc2_params.clock_id);
 
 	rc = stm32mp_reset_assert(sdmmc2_params.reset_id, TIMEOUT_US_1_MS);
 	if (rc != 0) {
@@ -727,7 +728,7 @@
 	}
 	mdelay(1);
 
-	sdmmc2_params.clk_rate = stm32mp_clk_get_rate(sdmmc2_params.clock_id);
+	sdmmc2_params.clk_rate = clk_get_rate(sdmmc2_params.clock_id);
 	sdmmc2_params.device_info->ocr_voltage = OCR_3_2_3_3 | OCR_3_3_3_4;
 
 	return mmc_init(&stm32_sdmmc2_ops, sdmmc2_params.clk_rate,
diff --git a/drivers/st/spi/stm32_qspi.c b/drivers/st/spi/stm32_qspi.c
index 4b1a029..d3c26d9 100644
--- a/drivers/st/spi/stm32_qspi.c
+++ b/drivers/st/spi/stm32_qspi.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2019-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
  */
@@ -11,6 +11,7 @@
 
 #include <common/debug.h>
 #include <common/fdt_wrappers.h>
+#include <drivers/clk.h>
 #include <drivers/delay_timer.h>
 #include <drivers/spi_mem.h>
 #include <drivers/st/stm32_gpio.h>
@@ -364,7 +365,7 @@
 
 static int stm32_qspi_set_speed(unsigned int hz)
 {
-	unsigned long qspi_clk = stm32mp_clk_get_rate(stm32_qspi.clock_id);
+	unsigned long qspi_clk = clk_get_rate(stm32_qspi.clock_id);
 	uint32_t prescaler = UINT8_MAX;
 	uint32_t csht;
 	int ret;
@@ -494,7 +495,7 @@
 	stm32_qspi.clock_id = (unsigned long)info.clock;
 	stm32_qspi.reset_id = (unsigned int)info.reset;
 
-	stm32mp_clk_enable(stm32_qspi.clock_id);
+	clk_enable(stm32_qspi.clock_id);
 
 	ret = stm32mp_reset_assert(stm32_qspi.reset_id, TIMEOUT_US_1_MS);
 	if (ret != 0) {
diff --git a/drivers/st/uart/stm32_uart.c b/drivers/st/uart/stm32_uart.c
new file mode 100644
index 0000000..e2e5405
--- /dev/null
+++ b/drivers/st/uart/stm32_uart.c
@@ -0,0 +1,404 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <common/bl_common.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_uart.h>
+#include <drivers/st/stm32_uart_regs.h>
+#include <drivers/st/stm32mp_clkfunc.h>
+#include <lib/mmio.h>
+
+#include <platform_def.h>
+
+/* UART time-out value */
+#define STM32_UART_TIMEOUT_US	20000U
+
+/* Mask to clear ALL the configuration registers */
+
+#define STM32_UART_CR1_FIELDS \
+		(USART_CR1_M | USART_CR1_PCE | USART_CR1_PS | USART_CR1_TE | \
+		 USART_CR1_RE | USART_CR1_OVER8 | USART_CR1_FIFOEN)
+
+#define STM32_UART_CR2_FIELDS \
+		(USART_CR2_SLVEN | USART_CR2_DIS_NSS | USART_CR2_ADDM7 | \
+		 USART_CR2_LBDL | USART_CR2_LBDIE | USART_CR2_LBCL | \
+		 USART_CR2_CPHA | USART_CR2_CPOL | USART_CR2_CLKEN | \
+		 USART_CR2_STOP | USART_CR2_LINEN | USART_CR2_SWAP | \
+		 USART_CR2_RXINV | USART_CR2_TXINV | USART_CR2_DATAINV | \
+		 USART_CR2_MSBFIRST | USART_CR2_ABREN | USART_CR2_ABRMODE | \
+		 USART_CR2_RTOEN | USART_CR2_ADD)
+
+#define STM32_UART_CR3_FIELDS \
+		(USART_CR3_EIE | USART_CR3_IREN | USART_CR3_IRLP | \
+		 USART_CR3_HDSEL | USART_CR3_NACK | USART_CR3_SCEN | \
+		 USART_CR3_DMAR | USART_CR3_DMAT | USART_CR3_RTSE | \
+		 USART_CR3_CTSE | USART_CR3_CTSIE | USART_CR3_ONEBIT | \
+		 USART_CR3_OVRDIS | USART_CR3_DDRE | USART_CR3_DEM | \
+		 USART_CR3_DEP | USART_CR3_SCARCNT | USART_CR3_WUS | \
+		 USART_CR3_WUFIE | USART_CR3_TXFTIE | USART_CR3_TCBGTIE | \
+		 USART_CR3_RXFTCFG | USART_CR3_RXFTIE | USART_CR3_TXFTCFG)
+
+#define STM32_UART_ISR_ERRORS	 \
+		(USART_ISR_ORE | USART_ISR_NE |  USART_ISR_FE | USART_ISR_PE)
+
+static const uint16_t presc_table[STM32_UART_PRESCALER_NB] = {
+	1U, 2U, 4U, 6U, 8U, 10U, 12U, 16U, 32U, 64U, 128U, 256U
+};
+
+/* @brief  BRR division operation to set BRR register in 8-bit oversampling
+ * mode.
+ * @param  clockfreq: UART clock.
+ * @param  baud_rate: Baud rate set by the user.
+ * @param  prescaler: UART prescaler value.
+ * @retval Division result.
+ */
+static uint32_t uart_div_sampling8(unsigned long clockfreq,
+				   uint32_t baud_rate,
+				   uint32_t prescaler)
+{
+	uint32_t scaled_freq = clockfreq / presc_table[prescaler];
+
+	return ((scaled_freq * 2) + (baud_rate / 2)) / baud_rate;
+
+}
+
+/* @brief  BRR division operation to set BRR register in 16-bit oversampling
+ * mode.
+ * @param  clockfreq: UART clock.
+ * @param  baud_rate: Baud rate set by the user.
+ * @param  prescaler: UART prescaler value.
+ * @retval Division result.
+ */
+static uint32_t uart_div_sampling16(unsigned long clockfreq,
+				    uint32_t baud_rate,
+				    uint32_t prescaler)
+{
+	uint32_t scaled_freq = clockfreq / presc_table[prescaler];
+
+	return (scaled_freq + (baud_rate / 2)) / baud_rate;
+
+}
+
+/*
+ * @brief  Return the UART clock frequency.
+ * @param  huart: UART handle.
+ * @retval Frequency value in Hz.
+ */
+static unsigned long uart_get_clock_freq(struct stm32_uart_handle_s *huart)
+{
+	return fdt_get_uart_clock_freq((uintptr_t)huart->base);
+}
+
+/*
+ * @brief  Configure the UART peripheral.
+ * @param  huart: UART handle.
+ * @retval UART status.
+ */
+static int uart_set_config(struct stm32_uart_handle_s *huart,
+			   const struct stm32_uart_init_s *init)
+{
+	uint32_t tmpreg;
+	unsigned long clockfreq;
+	uint32_t brrtemp;
+
+	/*
+	 * ---------------------- USART CR1 Configuration --------------------
+	 * Clear M, PCE, PS, TE, RE and OVER8 bits and configure
+	 * the UART word length, parity, mode and oversampling:
+	 * - set the M bits according to init->word_length value,
+	 * - set PCE and PS bits according to init->parity value,
+	 * - set TE and RE bits according to init->mode value,
+	 * - set OVER8 bit according to init->over_sampling value.
+	 */
+	tmpreg = init->word_length |
+		 init->parity |
+		 init->mode |
+		 init->over_sampling |
+		 init->fifo_mode;
+	mmio_clrsetbits_32(huart->base + USART_CR1, STM32_UART_CR1_FIELDS, tmpreg);
+
+	/*
+	 * --------------------- USART CR2 Configuration ---------------------
+	 * Configure the UART Stop Bits: Set STOP[13:12] bits according
+	 * to init->stop_bits value.
+	 */
+	mmio_clrsetbits_32(huart->base + USART_CR2, STM32_UART_CR2_FIELDS,
+			   init->stop_bits);
+
+	/*
+	 * --------------------- USART CR3 Configuration ---------------------
+	 * Configure:
+	 * - UART HardWare Flow Control: set CTSE and RTSE bits according
+	 *   to init->hw_flow_control value,
+	 * - one-bit sampling method versus three samples' majority rule
+	 *   according to init->one_bit_sampling (not applicable to
+	 *   LPUART),
+	 * - set TXFTCFG bit according to init->tx_fifo_threshold value,
+	 * - set RXFTCFG bit according to init->rx_fifo_threshold value.
+	 */
+	tmpreg = init->hw_flow_control | init->one_bit_sampling;
+
+	if (init->fifo_mode == USART_CR1_FIFOEN) {
+		tmpreg |= init->tx_fifo_threshold |
+			  init->rx_fifo_threshold;
+	}
+
+	mmio_clrsetbits_32(huart->base + USART_CR3, STM32_UART_CR3_FIELDS, tmpreg);
+
+	/*
+	 * --------------------- USART PRESC Configuration -------------------
+	 * Configure UART Clock Prescaler : set PRESCALER according to
+	 * init->prescaler value.
+	 */
+	assert(init->prescaler < STM32_UART_PRESCALER_NB);
+	mmio_clrsetbits_32(huart->base + USART_PRESC, USART_PRESC_PRESCALER,
+			   init->prescaler);
+
+	/*---------------------- USART BRR configuration --------------------*/
+	clockfreq = uart_get_clock_freq(huart);
+	if (clockfreq == 0UL) {
+		return -ENODEV;
+	}
+
+	if (init->over_sampling == STM32_UART_OVERSAMPLING_8) {
+		uint32_t usartdiv = uart_div_sampling8(clockfreq,
+						       init->baud_rate,
+						       init->prescaler);
+
+		brrtemp = (usartdiv & USART_BRR_DIV_MANTISSA) |
+			  ((usartdiv & USART_BRR_DIV_FRACTION) >> 1);
+	} else {
+		brrtemp = uart_div_sampling16(clockfreq,
+					      init->baud_rate,
+					      init->prescaler) &
+			  (USART_BRR_DIV_FRACTION | USART_BRR_DIV_MANTISSA);
+	}
+	mmio_write_32(huart->base + USART_BRR, brrtemp);
+
+	return 0;
+}
+
+/*
+ * @brief  Handle UART communication timeout.
+ * @param  huart: UART handle.
+ * @param  flag: Specifies the UART flag to check.
+ * @retval UART status.
+ */
+static int stm32_uart_wait_flag(struct stm32_uart_handle_s *huart, uint32_t flag)
+{
+	uint64_t timeout_ref = timeout_init_us(STM32_UART_TIMEOUT_US);
+
+	while ((mmio_read_32(huart->base + USART_ISR) & flag) == 0U) {
+		if (timeout_elapsed(timeout_ref)) {
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * @brief  Check the UART idle State.
+ * @param  huart: UART handle.
+ * @retval UART status.
+ */
+static int stm32_uart_check_idle(struct stm32_uart_handle_s *huart)
+{
+	int ret;
+
+	/* Check if the transmitter is enabled */
+	if ((mmio_read_32(huart->base + USART_CR1) & USART_CR1_TE) == USART_CR1_TE) {
+		ret = stm32_uart_wait_flag(huart, USART_ISR_TEACK);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	/* Check if the receiver is enabled */
+	if ((mmio_read_32(huart->base + USART_CR1) & USART_CR1_RE) == USART_CR1_RE) {
+		ret = stm32_uart_wait_flag(huart, USART_ISR_REACK);
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * @brief  Compute RDR register mask depending on word length.
+ * @param  huart: UART handle.
+ * @retval Mask value.
+ */
+static unsigned int stm32_uart_rdr_mask(const struct stm32_uart_init_s *init)
+{
+	unsigned int mask = 0U;
+
+	switch (init->word_length) {
+	case STM32_UART_WORDLENGTH_9B:
+		mask = GENMASK(8, 0);
+		break;
+	case STM32_UART_WORDLENGTH_8B:
+		mask = GENMASK(7, 0);
+		break;
+	case STM32_UART_WORDLENGTH_7B:
+		mask = GENMASK(6, 0);
+		break;
+	default:
+		break; /* not reached */
+	}
+
+	if (init->parity != STM32_UART_PARITY_NONE) {
+		mask >>= 1;
+	}
+
+	return mask;
+}
+
+/*
+ * @brief  Check interrupt and status errors.
+ * @retval True if error detected, false otherwise.
+ */
+static bool stm32_uart_error_detected(struct stm32_uart_handle_s *huart)
+{
+	return (mmio_read_32(huart->base + USART_ISR) & STM32_UART_ISR_ERRORS) != 0U;
+}
+
+/*
+ * @brief  Clear status errors.
+ */
+static void stm32_uart_error_clear(struct stm32_uart_handle_s *huart)
+{
+	mmio_write_32(huart->base + USART_ICR, STM32_UART_ISR_ERRORS);
+}
+
+/*
+ * @brief  Stop the UART.
+ * @param  base: UART base address.
+ */
+void stm32_uart_stop(uintptr_t base)
+{
+	mmio_clrbits_32(base + USART_CR1, USART_CR1_UE);
+}
+
+/*
+ * @brief  Initialize UART.
+ * @param  huart: UART handle.
+ * @param  base_addr: base address of UART.
+ * @param  init: UART initialization parameter.
+ * @retval UART status.
+ */
+
+int stm32_uart_init(struct stm32_uart_handle_s *huart,
+		    uintptr_t base_addr,
+		    const struct stm32_uart_init_s *init)
+{
+	int ret;
+
+	if (huart == NULL || init == NULL || base_addr == 0U) {
+		return -EINVAL;
+	}
+
+	huart->base = base_addr;
+
+	/* Disable the peripheral */
+	stm32_uart_stop(huart->base);
+
+	/* Computation of UART mask to apply to RDR register */
+	huart->rdr_mask = stm32_uart_rdr_mask(init);
+
+	/* Init the peripheral */
+	ret = uart_set_config(huart, init);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* Enable the peripheral */
+	mmio_setbits_32(huart->base + USART_CR1, USART_CR1_UE);
+
+	/* TEACK and/or REACK to check */
+	return stm32_uart_check_idle(huart);
+}
+
+/*
+ * @brief  Transmit one data in no blocking mode.
+ * @param  huart: UART handle.
+ * @param  c: data to sent.
+ * @retval UART status.
+ */
+int stm32_uart_putc(struct stm32_uart_handle_s *huart, int c)
+{
+	int ret;
+
+	if (huart == NULL) {
+		return -EINVAL;
+	}
+
+	ret = stm32_uart_wait_flag(huart, USART_ISR_TXE);
+	if (ret != 0) {
+		return ret;
+	}
+
+	mmio_write_32(huart->base + USART_TDR, c);
+	if (stm32_uart_error_detected(huart)) {
+		stm32_uart_error_clear(huart);
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+/*
+ * @brief  Flush TX Transmit fifo
+ * @param  huart: UART handle.
+ * @retval UART status.
+ */
+int stm32_uart_flush(struct stm32_uart_handle_s *huart)
+{
+	int ret;
+
+	if (huart == NULL) {
+		return -EINVAL;
+	}
+
+	ret = stm32_uart_wait_flag(huart, USART_ISR_TXE);
+	if (ret != 0) {
+		return ret;
+	}
+
+	return stm32_uart_wait_flag(huart, USART_ISR_TC);
+}
+
+/*
+ * @brief  Receive a data in no blocking mode.
+ * @retval value if >0 or UART status.
+ */
+int stm32_uart_getc(struct stm32_uart_handle_s *huart)
+{
+	uint32_t data;
+
+	if (huart == NULL) {
+		return -EINVAL;
+	}
+
+	/* Check if data is available */
+	if ((mmio_read_32(huart->base + USART_ISR) & USART_ISR_RXNE) == 0U) {
+		return -EAGAIN;
+	}
+
+	data = mmio_read_32(huart->base + USART_RDR) & huart->rdr_mask;
+
+	if (stm32_uart_error_detected(huart)) {
+		stm32_uart_error_clear(huart);
+		return -EFAULT;
+	}
+
+	return (int)data;
+}
diff --git a/fdts/morello-soc.dts b/fdts/morello-soc.dts
new file mode 100644
index 0000000..8464634
--- /dev/null
+++ b/fdts/morello-soc.dts
@@ -0,0 +1,271 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/dts-v1/;
+#include "morello.dtsi"
+
+/ {
+
+	chosen {
+		stdout-path = "soc_uart0:115200n8";
+	};
+
+	reserved-memory {
+		#address-cells = <2>;
+		#size-cells = <2>;
+		ranges;
+
+		secure-firmware@ff000000 {
+			reg = <0 0xff000000 0 0x01000000>;
+			no-map;
+		};
+	};
+
+	cpus {
+		#address-cells = <2>;
+		#size-cells = <0>;
+		cpu0@0 {
+			compatible = "arm,armv8";
+			reg = <0x0 0x0>;
+			device_type = "cpu";
+			enable-method = "psci";
+			clocks = <&scmi_dvfs 0>;
+		};
+		cpu1@100 {
+			compatible = "arm,armv8";
+			reg = <0x0 0x100>;
+			device_type = "cpu";
+			enable-method = "psci";
+			clocks = <&scmi_dvfs 0>;
+		};
+		cpu2@10000 {
+			compatible = "arm,armv8";
+			reg = <0x0 0x10000>;
+			device_type = "cpu";
+			enable-method = "psci";
+			clocks = <&scmi_dvfs 1>;
+		};
+		cpu3@10100 {
+			compatible = "arm,armv8";
+			reg = <0x0 0x10100>;
+			device_type = "cpu";
+			enable-method = "psci";
+			clocks = <&scmi_dvfs 1>;
+		};
+	};
+
+	/* The first bank of memory, memory map is actually provided by UEFI. */
+	memory@80000000 {
+		#address-cells = <2>;
+		#size-cells = <2>;
+		device_type = "memory";
+		/* [0x80000000-0xffffffff] */
+		reg = <0x00000000 0x80000000 0x0 0x7F000000>;
+	};
+
+	memory@8080000000 {
+		#address-cells = <2>;
+		#size-cells = <2>;
+		device_type = "memory";
+		/* [0x8080000000-0x83f7ffffff] */
+		reg = <0x00000080 0x80000000 0x3 0x78000000>;
+	};
+
+	smmu_pcie: iommu@4f400000 {
+		compatible = "arm,smmu-v3";
+		reg = <0 0x4f400000 0 0x40000>;
+		interrupts = <GIC_SPI 235 IRQ_TYPE_EDGE_RISING>,
+				<GIC_SPI 40 IRQ_TYPE_EDGE_RISING>,
+				<GIC_SPI 236 IRQ_TYPE_EDGE_RISING>,
+				<GIC_SPI 237 IRQ_TYPE_EDGE_RISING>;
+		interrupt-names = "eventq", "priq", "cmdq-sync", "gerror";
+		msi-parent = <&its2 0>;
+		#iommu-cells = <1>;
+		dma-coherent;
+	};
+
+	pcie_ctlr: pcie@28c0000000 {
+		compatible = "pci-host-ecam-generic";
+		device_type = "pci";
+		reg = <0x28 0xC0000000 0 0x10000000>;
+		bus-range = <0 255>;
+		linux,pci-domain = <0>;
+		#address-cells = <3>;
+		#size-cells = <2>;
+		dma-coherent;
+		ranges = <0x01000000 0x00 0x00000000 0x00 0x6F000000 0x00 0x00800000>,
+		     <0x02000000 0x00 0x60000000 0x00 0x60000000 0x00 0x0F000000>,
+			 <0x42000000 0x09 0x00000000 0x09 0x00000000 0x1F 0xC0000000>;
+		#interrupt-cells = <1>;
+		interrupt-map-mask = <0 0 0 7>;
+		interrupt-map = <0 0 0 1 &gic 0 0 0 169 IRQ_TYPE_LEVEL_HIGH>,
+			<0 0 0 2 &gic 0 0 0 170 IRQ_TYPE_LEVEL_HIGH>,
+			<0 0 0 3 &gic 0 0 0 171 IRQ_TYPE_LEVEL_HIGH>,
+			<0 0 0 4 &gic 0 0 0 172 IRQ_TYPE_LEVEL_HIGH>;
+		msi-map = <0 &its_pcie 0 0x10000>;
+		iommu-map = <0 &smmu_pcie 0 0x10000>;
+		status = "okay";
+	};
+
+	smmu_ccix: iommu@4f000000 {
+		compatible = "arm,smmu-v3";
+		reg = <0 0x4f000000 0 0x40000>;
+		interrupts = <GIC_SPI 228 IRQ_TYPE_EDGE_RISING>,
+				<GIC_SPI 41 IRQ_TYPE_EDGE_RISING>,
+				<GIC_SPI 229 IRQ_TYPE_EDGE_RISING>,
+				<GIC_SPI 230 IRQ_TYPE_EDGE_RISING>;
+		interrupt-names = "eventq", "priq", "cmdq-sync", "gerror";
+		msi-parent = <&its1 0>;
+		#iommu-cells = <1>;
+		dma-coherent;
+	};
+
+	ccix_pcie_ctlr: pcie@4fc0000000 {
+		compatible = "pci-host-ecam-generic";
+		device_type = "pci";
+		reg = <0x4F 0xC0000000 0 0x10000000>;
+		bus-range = <0 255>;
+		linux,pci-domain = <1>;
+		#address-cells = <3>;
+		#size-cells = <2>;
+		dma-coherent;
+		ranges = <0x01000000 0x00 0x00000000 0x00 0x7F000000 0x00 0x00800000>,
+		     <0x02000000 0x00 0x70000000 0x00 0x70000000 0x00 0x0F000000>,
+			 <0x42000000 0x30 0x00000000 0x30 0x00000000 0x1F 0xC0000000>;
+		#interrupt-cells = <1>;
+		interrupt-map-mask = <0 0 0 7>;
+		interrupt-map = <0 0 0 1 &gic 0 0 0 201 IRQ_TYPE_LEVEL_HIGH>,
+			<0 0 0 2 &gic 0 0 0 202 IRQ_TYPE_LEVEL_HIGH>,
+			<0 0 0 3 &gic 0 0 0 203 IRQ_TYPE_LEVEL_HIGH>,
+			<0 0 0 4 &gic 0 0 0 204 IRQ_TYPE_LEVEL_HIGH>;
+		msi-map = <0 &its_ccix 0 0x10000>;
+		iommu-map = <0 &smmu_ccix 0 0x10000>;
+		status = "okay";
+	};
+
+	smmu_dp: iommu@2ce00000 {
+		compatible = "arm,smmu-v3";
+		reg = <0 0x2ce00000 0 0x40000>;
+		interrupts = <GIC_SPI 76 IRQ_TYPE_EDGE_RISING>,
+				<GIC_SPI 78 IRQ_TYPE_EDGE_RISING>,
+				<GIC_SPI 80 IRQ_TYPE_EDGE_RISING>;
+		interrupt-names = "eventq", "cmdq-sync", "gerror";
+		#iommu-cells = <1>;
+	};
+
+	dp0: display@2cc00000 {
+		#address-cells = <1>;
+		#size-cells = <0>;
+		compatible = "arm,mali-d32";
+		reg = <0 0x2cc00000 0 0x20000>;
+		interrupts = <0 69 4>;
+		interrupt-names = "DPU";
+		clocks = <&dpu_aclk>;
+		clock-names = "aclk";
+		iommus = <&smmu_dp 0>, <&smmu_dp 1>, <&smmu_dp 2>, <&smmu_dp 3>,
+			<&smmu_dp 8>;
+
+		pl0: pipeline@0 {
+			reg = <0>;
+			clocks = <&scmi_clk 1>;
+			clock-names = "pxclk";
+			pl_id = <0>;
+			ports {
+				#address-cells = <1>;
+				#size-cells = <0>;
+				port@0 {
+					reg = <0>;
+					dp_pl0_out0: endpoint {
+						remote-endpoint = <&tda998x_0_input>;
+					};
+				};
+			};
+		};
+	};
+
+	i2c@1c0f0000 {
+		compatible = "cdns,i2c-r1p14";
+		reg = <0x0 0x1c0f0000 0x0 0x1000>;
+		#address-cells = <1>;
+		#size-cells = <0>;
+		clock-frequency = <100000>;
+		i2c-sda-hold-time-ns = <500>;
+		interrupts = <GIC_SPI 138 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&dpu_aclk>;
+
+		hdmi-transmitter@70 {
+			compatible = "nxp,tda998x";
+			reg = <0x70>;
+			video-ports = <0x234501>;
+			port {
+				tda998x_0_input: endpoint {
+					remote-endpoint = <&dp_pl0_out0>;
+				};
+			};
+		};
+	};
+
+	dpu_aclk: dpu_aclk {
+		/* 77.1 MHz derived from 24 MHz reference clock */
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <350000000>;
+		clock-output-names = "aclk";
+	};
+
+	firmware {
+		scmi {
+			compatible = "arm,scmi";
+			mbox-names = "tx", "rx";
+			mboxes = <&mailbox 1 0 &mailbox 1 1>;
+			shmem = <&cpu_scp_hpri0 &cpu_scp_hpri1>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+			scmi_dvfs: protocol@13 {
+				reg = <0x13>;
+				#clock-cells = <1>;
+			};
+			scmi_clk: protocol@14 {
+				reg = <0x14>;
+				#clock-cells = <1>;
+			};
+		};
+	};
+};
+
+&gic {
+	reg = <0x0 0x30000000 0 0x10000>,	/* GICD */
+	      <0x0 0x300c0000 0 0x80000>;	/* GICR */
+	interrupts = <GIC_PPI 9 IRQ_TYPE_LEVEL_HIGH>;
+
+	its1: its@30040000 {
+		compatible = "arm,gic-v3-its";
+		msi-controller;
+		#msi-cells = <1>;
+		reg = <0x0 0x30040000 0x0 0x20000>;
+	};
+
+	its2: its@30060000 {
+		compatible = "arm,gic-v3-its";
+		msi-controller;
+		#msi-cells = <1>;
+		reg = <0x0 0x30060000 0x0 0x20000>;
+	};
+
+	its_ccix: its@30080000 {
+		compatible = "arm,gic-v3-its";
+		msi-controller;
+		#msi-cells = <1>;
+		reg = <0x0 0x30080000 0x0 0x20000>;
+	};
+
+	its_pcie: its@300a0000 {
+		compatible = "arm,gic-v3-its";
+		msi-controller;
+		#msi-cells = <1>;
+		reg = <0x0 0x300a0000 0x0 0x20000>;
+	};
+};
diff --git a/fdts/morello.dtsi b/fdts/morello.dtsi
index 52c04cd..f119820 100644
--- a/fdts/morello.dtsi
+++ b/fdts/morello.dtsi
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -58,17 +58,17 @@
 				  "mhu_hpri_rx";
 		#mbox-cells = <2>;
 		mbox-name = "ARM-MHU";
-		clocks = <&soc_refclk100mhz>;
+		clocks = <&soc_refclk50mhz>;
 		clock-names = "apb_pclk";
 	};
 
 	sram: sram@45200000 {
 		compatible = "mmio-sram";
-		reg = <0x0 0x45200000 0x0 0x8000>;
+		reg = <0x0 0x06000000 0x0 0x8000>;
 
 		#address-cells = <1>;
 		#size-cells = <1>;
-		ranges = <0 0x0 0x45200000 0x8000>;
+		ranges = <0 0x0 0x06000000 0x8000>;
 
 		cpu_scp_hpri0: scp-shmem@0 {
 			compatible = "arm,scmi-shmem";
@@ -81,10 +81,10 @@
 		};
 	};
 
-	soc_refclk100mhz: refclk100mhz {
+	soc_refclk50mhz: refclk50mhz {
 		compatible = "fixed-clock";
 		#clock-cells = <0>;
-		clock-frequency = <100000000>;
+		clock-frequency = <50000000>;
 		clock-output-names = "apb_pclk";
 	};
 
@@ -99,7 +99,7 @@
 		compatible = "arm,pl011", "arm,primecell";
 		reg = <0x0 0x2a400000 0x0 0x1000>;
 		interrupts = <GIC_SPI 63 IRQ_TYPE_LEVEL_HIGH>;
-		clocks = <&soc_uartclk>, <&soc_refclk100mhz>;
+		clocks = <&soc_uartclk>, <&soc_refclk50mhz>;
 		clock-names = "uartclk", "apb_pclk";
 		status = "okay";
 	};
diff --git a/include/drivers/auth/crypto_mod.h b/include/drivers/auth/crypto_mod.h
index 71cf673..cdcf504 100644
--- a/include/drivers/auth/crypto_mod.h
+++ b/include/drivers/auth/crypto_mod.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -25,6 +25,16 @@
 	CRYPTO_GCM_DECRYPT = 0
 };
 
+/* Message digest algorithm */
+enum crypto_md_algo {
+	CRYPTO_MD_SHA256,
+	CRYPTO_MD_SHA384,
+	CRYPTO_MD_SHA512,
+};
+
+/* Maximum size as per the known stronger hash algorithm i.e.SHA512 */
+#define CRYPTO_MD_MAX_SIZE		64U
+
 /*
  * Cryptographic library descriptor
  */
@@ -49,8 +59,9 @@
 
 #if MEASURED_BOOT
 	/* Calculate a hash. Return hash value */
-	int (*calc_hash)(unsigned int alg, void *data_ptr,
-			 unsigned int data_len, unsigned char *output);
+	int (*calc_hash)(enum crypto_md_algo md_alg, void *data_ptr,
+			 unsigned int data_len,
+			 unsigned char output[CRYPTO_MD_MAX_SIZE]);
 #endif /* MEASURED_BOOT */
 
 	/*
@@ -79,8 +90,9 @@
 			    unsigned int tag_len);
 
 #if MEASURED_BOOT
-int crypto_mod_calc_hash(unsigned int alg, void *data_ptr,
-			 unsigned int data_len, unsigned char *output);
+int crypto_mod_calc_hash(enum crypto_md_algo alg, void *data_ptr,
+			 unsigned int data_len,
+			 unsigned char output[CRYPTO_MD_MAX_SIZE]);
 
 /* Macro to register a cryptographic library */
 #define REGISTER_CRYPTO_LIB(_name, _init, _verify_signature, _verify_hash, \
diff --git a/include/drivers/auth/mbedtls/mbedtls_config.h b/include/drivers/auth/mbedtls/mbedtls_config.h
index ad39fa9..8ad6d7a 100644
--- a/include/drivers/auth/mbedtls/mbedtls_config.h
+++ b/include/drivers/auth/mbedtls/mbedtls_config.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2015-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -71,9 +71,20 @@
 #endif
 
 #define MBEDTLS_SHA256_C
-#if (TF_MBEDTLS_HASH_ALG_ID != TF_MBEDTLS_SHA256)
+
+/*
+ * If either Trusted Boot or Measured Boot require a stronger algorithm than
+ * SHA-256, pull in SHA-512 support.
+ */
+#if (TF_MBEDTLS_HASH_ALG_ID != TF_MBEDTLS_SHA256) /* TBB hash algo */
+#define	MBEDTLS_SHA512_C
+#else
+   /* TBB uses SHA-256, what about measured boot? */
+#if defined(TF_MBEDTLS_TPM_HASH_ALG_ID) && \
+	(TF_MBEDTLS_TPM_HASH_ALG_ID != TF_MBEDTLS_SHA256)
 #define MBEDTLS_SHA512_C
 #endif
+#endif
 
 #define MBEDTLS_VERSION_C
 
diff --git a/include/drivers/clk.h b/include/drivers/clk.h
new file mode 100644
index 0000000..a18f41f
--- /dev/null
+++ b/include/drivers/clk.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CLK_H
+#define CLK_H
+
+#include <stdbool.h>
+
+struct clk_ops {
+	int (*enable)(unsigned long id);
+	void (*disable)(unsigned long id);
+	unsigned long (*get_rate)(unsigned long id);
+	int (*get_parent)(unsigned long id);
+	bool (*is_enabled)(unsigned long id);
+};
+
+int clk_enable(unsigned long id);
+void clk_disable(unsigned long id);
+unsigned long clk_get_rate(unsigned long id);
+bool clk_is_enabled(unsigned long id);
+int clk_get_parent(unsigned long id);
+
+void clk_register(const struct clk_ops *ops);
+
+#endif /* CLK_H */
diff --git a/include/drivers/measured_boot/event_log/event_log.h b/include/drivers/measured_boot/event_log/event_log.h
index c6eb29c..a687d41 100644
--- a/include/drivers/measured_boot/event_log/event_log.h
+++ b/include/drivers/measured_boot/event_log/event_log.h
@@ -36,9 +36,9 @@
 #endif
 
 /* Number of hashing algorithms supported */
-#define HASH_ALG_COUNT	1U
+#define HASH_ALG_COUNT		1U
 
-#define	INVALID_ID	MAX_NUMBER_IDS
+#define EVLOG_INVALID_ID	UINT32_MAX
 
 #define MEMBER_SIZE(type, member) sizeof(((type *)0)->member)
 
diff --git a/include/drivers/st/stm32_gpio.h b/include/drivers/st/stm32_gpio.h
index e241f58..b072345 100644
--- a/include/drivers/st/stm32_gpio.h
+++ b/include/drivers/st/stm32_gpio.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2015-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -52,6 +52,7 @@
 void set_gpio(uint32_t bank, uint32_t pin, uint32_t mode, uint32_t speed,
 	      uint32_t pull, uint32_t alternate, uint8_t status);
 void set_gpio_secure_cfg(uint32_t bank, uint32_t pin, bool secure);
+void set_gpio_reset_cfg(uint32_t bank, uint32_t pin);
 #endif /*__ASSEMBLER__*/
 
 #endif /* STM32_GPIO_H */
diff --git a/include/drivers/st/stm32_uart.h b/include/drivers/st/stm32_uart.h
new file mode 100644
index 0000000..212968f
--- /dev/null
+++ b/include/drivers/st/stm32_uart.h
@@ -0,0 +1,170 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef STM32_UART_H
+#define STM32_UART_H
+
+/* UART word length */
+#define STM32_UART_WORDLENGTH_7B		USART_CR1_M1
+#define STM32_UART_WORDLENGTH_8B		0x00000000U
+#define STM32_UART_WORDLENGTH_9B		USART_CR1_M0
+
+/* UART number of stop bits */
+#define STM32_UART_STOPBITS_0_5			USART_CR2_STOP_0
+#define STM32_UART_STOPBITS_1			0x00000000U
+#define STM32_UART_STOPBITS_1_5			(USART_CR2_STOP_0 | USART_CR2_STOP_1)
+#define STM32_UART_STOPBITS_2			USART_CR2_STOP_1
+
+/* UART parity */
+#define STM32_UART_PARITY_NONE			0x00000000U
+#define STM32_UART_PARITY_EVEN			USART_CR1_PCE
+#define STM32_UART_PARITY_ODD			(USART_CR1_PCE | USART_CR1_PS)
+
+/* UART transfer mode */
+#define STM32_UART_MODE_RX			USART_CR1_RE
+#define STM32_UART_MODE_TX			USART_CR1_TE
+#define STM32_UART_MODE_TX_RX			(USART_CR1_TE | USART_CR1_RE)
+
+/* UART hardware flow control */
+#define STM32_UART_HWCONTROL_NONE		0x00000000U
+#define STM32_UART_HWCONTROL_RTS		USART_CR3_RTSE
+#define STM32_UART_HWCONTROL_CTS		USART_CR3_CTSE
+#define STM32_UART_HWCONTROL_RTS_CTS		(USART_CR3_RTSE | USART_CR3_CTSE)
+
+/* UART over sampling */
+#define STM32_UART_OVERSAMPLING_16		0x00000000U
+#define STM32_UART_OVERSAMPLING_8		USART_CR1_OVER8
+
+/* UART prescaler */
+#define STM32_UART_PRESCALER_DIV1		0x00000000U
+#define STM32_UART_PRESCALER_DIV2		0x00000001U
+#define STM32_UART_PRESCALER_DIV4		0x00000002U
+#define STM32_UART_PRESCALER_DIV6		0x00000003U
+#define STM32_UART_PRESCALER_DIV8		0x00000004U
+#define STM32_UART_PRESCALER_DIV10		0x00000005U
+#define STM32_UART_PRESCALER_DIV12		0x00000006U
+#define STM32_UART_PRESCALER_DIV16		0x00000007U
+#define STM32_UART_PRESCALER_DIV32		0x00000008U
+#define STM32_UART_PRESCALER_DIV64		0x00000009U
+#define STM32_UART_PRESCALER_DIV128		0x0000000AU
+#define STM32_UART_PRESCALER_DIV256		0x0000000BU
+#define STM32_UART_PRESCALER_NB			0x0000000CU
+
+/* UART fifo mode */
+#define STM32_UART_FIFOMODE_EN			USART_CR1_FIFOEN
+#define STM32_UART_FIFOMODE_DIS			0x00000000U
+
+/* UART TXFIFO threshold level */
+#define STM32_UART_TXFIFO_THRESHOLD_1EIGHTHFULL		0x00000000U
+#define STM32_UART_TXFIFO_THRESHOLD_1QUARTERFUL		USART_CR3_TXFTCFG_0
+#define STM32_UART_TXFIFO_THRESHOLD_HALFFULL		USART_CR3_TXFTCFG_1
+#define STM32_UART_TXFIFO_THRESHOLD_3QUARTERSFULL	(USART_CR3_TXFTCFG_0 | USART_CR3_TXFTCFG_1)
+#define STM32_UART_TXFIFO_THRESHOLD_7EIGHTHFULL		USART_CR3_TXFTCFG_2
+#define STM32_UART_TXFIFO_THRESHOLD_EMPTY		(USART_CR3_TXFTCFG_2 | USART_CR3_TXFTCFG_0)
+
+/* UART RXFIFO threshold level */
+#define STM32_UART_RXFIFO_THRESHOLD_1EIGHTHFULL		0x00000000U
+#define STM32_UART_RXFIFO_THRESHOLD_1QUARTERFULL	USART_CR3_RXFTCFG_0
+#define STM32_UART_RXFIFO_THRESHOLD_HALFFULL		USART_CR3_RXFTCFG_1
+#define STM32_UART_RXFIFO_THRESHOLD_3QUARTERSFULL	(USART_CR3_RXFTCFG_0 | USART_CR3_RXFTCFG_1)
+#define STM32_UART_RXFIFO_THRESHOLD_7EIGHTHFULL		USART_CR3_RXFTCFG_2
+#define STM32_UART_RXFIFO_THRESHOLD_FULL		(USART_CR3_RXFTCFG_2 | USART_CR3_RXFTCFG_0)
+
+struct stm32_uart_init_s {
+	uint32_t baud_rate;		/*
+					 * Configures the UART communication
+					 * baud rate.
+					 */
+
+	uint32_t word_length;		/*
+					 * Specifies the number of data bits
+					 * transmitted or received in a frame.
+					 * This parameter can be a value of
+					 * @ref STM32_UART_WORDLENGTH_*.
+					 */
+
+	uint32_t stop_bits;		/*
+					 * Specifies the number of stop bits
+					 * transmitted. This parameter can be
+					 * a value of @ref STM32_UART_STOPBITS_*.
+					 */
+
+	uint32_t parity;		/*
+					 * Specifies the parity mode.
+					 * This parameter can be a value of
+					 * @ref STM32_UART_PARITY_*.
+					 */
+
+	uint32_t mode;			/*
+					 * Specifies whether the receive or
+					 * transmit mode is enabled or
+					 * disabled. This parameter can be a
+					 * value of @ref @ref STM32_UART_MODE_*.
+					 */
+
+	uint32_t hw_flow_control;	/*
+					 * Specifies whether the hardware flow
+					 * control mode is enabled or
+					 * disabled. This parameter can be a
+					 * value of @ref STM32_UARTHWCONTROL_*.
+					 */
+
+	uint32_t over_sampling;		/*
+					 * Specifies whether the over sampling
+					 * 8 is enabled or disabled.
+					 * This parameter can be a value of
+					 * @ref STM32_UART_OVERSAMPLING_*.
+					 */
+
+	uint32_t one_bit_sampling;	/*
+					 * Specifies whether a single sample
+					 * or three samples' majority vote is
+					 * selected. This parameter can be 0
+					 * or USART_CR3_ONEBIT.
+					 */
+
+	uint32_t prescaler;		/*
+					 * Specifies the prescaler value used
+					 * to divide the UART clock source.
+					 * This parameter can be a value of
+					 * @ref STM32_UART_PRESCALER_*.
+					 */
+
+	uint32_t fifo_mode;		/*
+					 * Specifies if the FIFO mode will be
+					 * used. This parameter can be a value
+					 * of @ref STM32_UART_FIFOMODE_*.
+					 */
+
+	uint32_t tx_fifo_threshold;	/*
+					 * Specifies the TXFIFO threshold
+					 * level. This parameter can be a
+					 * value of @ref
+					 * STM32_UART_TXFIFO_THRESHOLD_*.
+					 */
+
+	uint32_t rx_fifo_threshold;	/*
+					 * Specifies the RXFIFO threshold
+					 * level. This parameter can be a
+					 * value of @ref
+					 * STM32_UART_RXFIFO_THRESHOLD_*.
+					 */
+};
+
+struct stm32_uart_handle_s {
+	uint32_t base;
+	uint32_t rdr_mask;
+};
+
+int stm32_uart_init(struct stm32_uart_handle_s *huart,
+		    uintptr_t base_addr,
+		    const struct stm32_uart_init_s *init);
+void stm32_uart_stop(uintptr_t base_addr);
+int stm32_uart_putc(struct stm32_uart_handle_s *huart, int c);
+int stm32_uart_flush(struct stm32_uart_handle_s *huart);
+int stm32_uart_getc(struct stm32_uart_handle_s *huart);
+
+#endif /* STM32_UART_H */
diff --git a/include/drivers/st/stm32mp_clkfunc.h b/include/drivers/st/stm32mp_clkfunc.h
index c3329cc..a282035 100644
--- a/include/drivers/st/stm32mp_clkfunc.h
+++ b/include/drivers/st/stm32mp_clkfunc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -26,5 +26,6 @@
 bool fdt_get_rcc_secure_status(void);
 
 int fdt_get_clock_id(int node);
+unsigned long fdt_get_uart_clock_freq(uintptr_t instance);
 
 #endif /* STM32MP_CLKFUNC_H */
diff --git a/include/lib/cpus/aarch64/cortex_x2.h b/include/lib/cpus/aarch64/cortex_x2.h
index 9ce1223..bf1b460 100644
--- a/include/lib/cpus/aarch64/cortex_x2.h
+++ b/include/lib/cpus/aarch64/cortex_x2.h
@@ -15,9 +15,23 @@
 #define CORTEX_X2_CPUECTLR_EL1					S3_0_C15_C1_4
 
 /*******************************************************************************
+ * CPU Extended Control register 2 specific definitions
+ ******************************************************************************/
+#define CORTEX_X2_CPUECTLR2_EL1					S3_0_C15_C1_5
+
+#define CORTEX_X2_CPUECTLR2_EL1_PF_MODE_SHIFT			U(11)
+#define CORTEX_X2_CPUECTLR2_EL1_PF_MODE_WIDTH			U(4)
+#define CORTEX_X2_CPUECTLR2_EL1_PF_MODE_CNSRV			ULL(0x9)
+
+/*******************************************************************************
  * CPU Power Control register specific definitions
  ******************************************************************************/
 #define CORTEX_X2_CPUPWRCTLR_EL1				S3_0_C15_C2_7
 #define CORTEX_X2_CPUPWRCTLR_EL1_CORE_PWRDN_BIT			U(1)
 
+/*******************************************************************************
+ * CPU Auxiliary Control Register 5 definitions
+ ******************************************************************************/
+#define CORTEX_X2_CPUACTLR5_EL1					S3_0_C15_C8_0
+
 #endif /* CORTEX_X2_H */
diff --git a/include/lib/el3_runtime/aarch64/context.h b/include/lib/el3_runtime/aarch64/context.h
index 698e208..512d196 100644
--- a/include/lib/el3_runtime/aarch64/context.h
+++ b/include/lib/el3_runtime/aarch64/context.h
@@ -207,8 +207,8 @@
 #define CTX_MPAMVPMV_EL2	U(0x158)
 
 // Starting with Armv8.6
-#define CTX_HAFGRTR_EL2		U(0x160)
-#define CTX_HDFGRTR_EL2		U(0x168)
+#define CTX_HDFGRTR_EL2		U(0x160)
+#define CTX_HAFGRTR_EL2		U(0x168)
 #define CTX_HDFGWTR_EL2		U(0x170)
 #define CTX_HFGITR_EL2		U(0x178)
 #define CTX_HFGRTR_EL2		U(0x180)
diff --git a/include/lib/fconf/fconf_dyn_cfg_getter.h b/include/lib/fconf/fconf_dyn_cfg_getter.h
index 6f8da0d..ff51c6c 100644
--- a/include/lib/fconf/fconf_dyn_cfg_getter.h
+++ b/include/lib/fconf/fconf_dyn_cfg_getter.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2019-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,8 @@
 
 #include <lib/fconf/fconf.h>
 
+#define FCONF_INVALID_IDX	0xFFFFFFFFU
+
 /* Dynamic configuration related getter */
 #define dyn_cfg__dtb_getter(id)	dyn_cfg_dtb_info_getter(id)
 
@@ -18,6 +20,7 @@
 	unsigned int config_id;
 };
 
+unsigned int dyn_cfg_dtb_info_get_index(unsigned int config_id);
 struct dyn_cfg_dtb_info_t *dyn_cfg_dtb_info_getter(unsigned int config_id);
 int fconf_populate_dtb_registry(uintptr_t config);
 
diff --git a/include/plat/common/common_def.h b/include/plat/common/common_def.h
index 14ae603..632f122 100644
--- a/include/plat/common/common_def.h
+++ b/include/plat/common/common_def.h
@@ -85,4 +85,12 @@
 #endif /* BL2_IN_XIP_MEM */
 #endif /* SEPARATE_CODE_AND_RODATA */
 
+#if MEASURED_BOOT
+/*
+ * Start critical data Ids from 2^32/2 reserving Ids from 0 to (2^32/2 - 1)
+ * for Images, It is a critical data Id base for all platforms.
+ */
+#define CRITICAL_DATA_ID_BASE	U(0x80000000)
+#endif /* MEASURED_BOOT */
+
 #endif /* COMMON_DEF_H */
diff --git a/include/plat/common/platform.h b/include/plat/common/platform.h
index 3fa63f5..9a61b50 100644
--- a/include/plat/common/platform.h
+++ b/include/plat/common/platform.h
@@ -124,12 +124,22 @@
 
 #if MEASURED_BOOT
 int plat_mboot_measure_image(unsigned int image_id, image_info_t *image_data);
+int plat_mboot_measure_critical_data(unsigned int critical_data_id,
+				     const void *base,
+				     size_t size);
 #else
 static inline int plat_mboot_measure_image(unsigned int image_id __unused,
 					   image_info_t *image_data __unused)
 {
 	return 0;
 }
+static inline int plat_mboot_measure_critical_data(
+					unsigned int critical_data_id __unused,
+					const void *base __unused,
+					size_t size __unused)
+{
+	return 0;
+}
 #endif /* MEASURED_BOOT */
 
 /*******************************************************************************
diff --git a/include/services/gtsi_svc.h b/include/services/gtsi_svc.h
index cb942ed..ef4289f 100644
--- a/include/services/gtsi_svc.h
+++ b/include/services/gtsi_svc.h
@@ -14,12 +14,16 @@
 #define GTSI_ERROR_INVALID_PAS		-3
 
 /* The macros below are used to identify GTSI calls from the SMC function ID */
-#define GTSI_FNUM_MIN_VALUE	U(0x100)
-#define GTSI_FNUM_MAX_VALUE	U(0x101)
+#define GTSI_FNUM_MIN_VALUE	U(0x1B0)
+#define GTSI_FNUM_MAX_VALUE	U(0x1B1)
 #define is_gtsi_fid(fid) __extension__ ({		\
 	__typeof__(fid) _fid = (fid);			\
 	((GET_SMC_NUM(_fid) >= GTSI_FNUM_MIN_VALUE) &&	\
-	 (GET_SMC_NUM(_fid) <= GTSI_FNUM_MAX_VALUE)); })
+	(GET_SMC_NUM(_fid) <= GTSI_FNUM_MAX_VALUE)  &&	\
+	(GET_SMC_TYPE(_fid) == SMC_TYPE_FAST)	    &&	\
+	(GET_SMC_CC(_fid) == SMC_64)                &&	\
+	(GET_SMC_OEN(_fid) == OEN_STD_START)        &&	\
+	((_fid & 0x00FE0000) == 0U)); })
 
 /* Get GTSI fastcall std FID from function number */
 #define GTSI_FID(smc_cc, func_num)			\
@@ -28,8 +32,8 @@
 	 (OEN_STD_START << FUNCID_OEN_SHIFT)	|	\
 	 ((func_num) << FUNCID_NUM_SHIFT))
 
-#define GRAN_TRANS_TO_REALM_FNUM	U(0x100)
-#define GRAN_TRANS_TO_NS_FNUM		U(0x101)
+#define GRAN_TRANS_TO_REALM_FNUM	U(0x1B0)
+#define GRAN_TRANS_TO_NS_FNUM		U(0x1B1)
 
 #define SMC_ASC_MARK_REALM	GTSI_FID(SMC_64, GRAN_TRANS_TO_REALM_FNUM)
 #define SMC_ASC_MARK_NONSECURE	GTSI_FID(SMC_64, GRAN_TRANS_TO_NS_FNUM)
diff --git a/include/services/rmi_svc.h b/include/services/rmi_svc.h
index 22f635b..46fd510 100644
--- a/include/services/rmi_svc.h
+++ b/include/services/rmi_svc.h
@@ -17,22 +17,22 @@
 #define RMI_ERROR_INVALID_PAS		-3
 
 /* The macros below are used to identify RMI calls from the SMC function ID */
-#define RMI_FNUM_MIN_VALUE	U(0x00)
-#define RMI_FNUM_MAX_VALUE	U(0x20)
+#define RMI_FNUM_MIN_VALUE	U(0x150)
+#define RMI_FNUM_MAX_VALUE	U(0x18F)
 #define is_rmi_fid(fid) __extension__ ({		\
 	__typeof__(fid) _fid = (fid);			\
 	((GET_SMC_NUM(_fid) >= RMI_FNUM_MIN_VALUE) &&	\
 	 (GET_SMC_NUM(_fid) <= RMI_FNUM_MAX_VALUE) &&	\
 	 (GET_SMC_TYPE(_fid) == SMC_TYPE_FAST)	   &&	\
 	 (GET_SMC_CC(_fid) == SMC_64)              &&	\
-	 (GET_SMC_OEN(_fid) == OEN_ARM_START)      &&	\
+	 (GET_SMC_OEN(_fid) == OEN_STD_START)      &&	\
 	 ((_fid & 0x00FE0000) == 0U)); })
 
 /* Get RMI fastcall std FID from function number */
 #define RMI_FID(smc_cc, func_num)			\
 	((SMC_TYPE_FAST << FUNCID_TYPE_SHIFT)	|	\
 	((smc_cc) << FUNCID_CC_SHIFT)		|	\
-	(OEN_ARM_START << FUNCID_OEN_SHIFT)	|	\
+	(OEN_STD_START << FUNCID_OEN_SHIFT)	|	\
 	((func_num) << FUNCID_NUM_SHIFT))
 
 /*
@@ -41,19 +41,20 @@
  * always invoked by the Normal world, forwarded by RMMD and handled by the
  * RMM
  */
-#define RMI_FNUM_REQ_COMPLETE		U(0x10)
-#define RMI_FNUM_VERSION_REQ		U(0x00)
+#define RMI_FNUM_REQ_COMPLETE		U(0x18F)
+#define RMI_FNUM_VERSION_REQ		U(0x150)
 
-#define RMI_FNUM_GRAN_NS_REALM		U(0x01)
-#define RMI_FNUM_GRAN_REALM_NS		U(0x02)
+#define RMI_FNUM_GRANULE_DELEGATE	U(0x151)
+#define RMI_FNUM_GRANULE_UNDELEGATE	U(0x152)
 
 /* RMI SMC64 FIDs handled by the RMMD */
 #define RMI_RMM_REQ_COMPLETE		RMI_FID(SMC_64, RMI_FNUM_REQ_COMPLETE)
 #define RMI_RMM_REQ_VERSION		RMI_FID(SMC_64, RMI_FNUM_VERSION_REQ)
 
-#define RMI_RMM_GRANULE_DELEGATE	RMI_FID(SMC_64, RMI_FNUM_GRAN_NS_REALM)
-#define RMI_RMM_GRANULE_UNDELEGATE	RMI_FID(SMC_64, RMI_FNUM_GRAN_REALM_NS)
-
+#define RMI_RMM_GRANULE_DELEGATE	RMI_FID(SMC_64, \
+						RMI_FNUM_GRANULE_DELEGATE)
+#define RMI_RMM_GRANULE_UNDELEGATE	RMI_FID(SMC_64, \
+						RMI_FNUM_GRANULE_UNDELEGATE)
 
 #define RMI_ABI_VERSION_GET_MAJOR(_version) ((_version) >> 16)
 #define RMI_ABI_VERSION_GET_MINOR(_version) ((_version) & 0xFFFF)
diff --git a/lib/cpus/aarch64/cortex_x2.S b/lib/cpus/aarch64/cortex_x2.S
index 87a9bdf..88116c3 100644
--- a/lib/cpus/aarch64/cortex_x2.S
+++ b/lib/cpus/aarch64/cortex_x2.S
@@ -21,6 +21,98 @@
 #error "Cortex X2 supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
 #endif
 
+	/* --------------------------------------------------
+	 * Errata Workaround for Cortex X2 Errata #2002765.
+	 * This applies to revisions r0p0, r1p0, and r2p0 and
+	 * is open.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0, x1, x17
+	 * --------------------------------------------------
+	 */
+func errata_cortex_x2_2002765_wa
+	/* Check workaround compatibility. */
+	mov	x17, x30
+	bl	check_errata_2002765
+	cbz	x0, 1f
+
+	ldr	x0, =0x6
+	msr	S3_6_C15_C8_0, x0 /* CPUPSELR_EL3 */
+	ldr	x0, =0xF3A08002
+	msr	S3_6_C15_C8_2, x0 /* CPUPOR_EL3 */
+	ldr	x0, =0xFFF0F7FE
+	msr	S3_6_C15_C8_3, x0 /* CPUPMR_EL3 */
+	ldr	x0, =0x40000001003ff
+	msr	S3_6_C15_C8_1, x0 /* CPUPCR_EL3 */
+	isb
+
+1:
+	ret	x17
+endfunc errata_cortex_x2_2002765_wa
+
+func check_errata_2002765
+	/* Applies to r0p0 - r2p0 */
+	mov	x1, #0x20
+	b	cpu_rev_var_ls
+endfunc check_errata_2002765
+
+	/* --------------------------------------------------
+	 * Errata Workaround for Cortex X2 Errata #2058056.
+	 * This applies to revisions r0p0, r1p0, and r2p0 and
+	 * is open.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0, x1, x17
+	 * --------------------------------------------------
+	 */
+func errata_cortex_x2_2058056_wa
+	/* Check workaround compatibility. */
+	mov	x17, x30
+	bl	check_errata_2058056
+	cbz	x0, 1f
+
+	mrs	x1, CORTEX_X2_CPUECTLR2_EL1
+	mov	x0, #CORTEX_X2_CPUECTLR2_EL1_PF_MODE_CNSRV
+	bfi	x1, x0, #CORTEX_X2_CPUECTLR2_EL1_PF_MODE_SHIFT, #CORTEX_X2_CPUECTLR2_EL1_PF_MODE_WIDTH
+	msr	CORTEX_X2_CPUECTLR2_EL1, x1
+
+1:
+	ret	x17
+endfunc errata_cortex_x2_2058056_wa
+
+func check_errata_2058056
+	/* Applies to r0p0 - r2p0 */
+	mov	x1, #0x20
+	b	cpu_rev_var_ls
+endfunc check_errata_2058056
+
+	/* --------------------------------------------------
+	 * Errata Workaround for Cortex X2 Errata #2083908.
+	 * This applies to revision r2p0 and is open.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0-x2, x17
+	 * --------------------------------------------------
+	 */
+func errata_cortex_x2_2083908_wa
+	/* Check workaround compatibility. */
+	mov	x17, x30
+	bl	check_errata_2083908
+	cbz	x0, 1f
+
+	/* Apply the workaround by setting bit 13 in CPUACTLR5_EL1. */
+	mrs	x1, CORTEX_X2_CPUACTLR5_EL1
+	orr	x1, x1, #BIT(13)
+	msr	CORTEX_X2_CPUACTLR5_EL1, x1
+
+1:
+	ret	x17
+endfunc errata_cortex_x2_2083908_wa
+
+func check_errata_2083908
+	/* Applies to r2p0 */
+	mov	x1, #0x20
+	mov	x2, #0x20
+	b	cpu_rev_var_range
+endfunc check_errata_2083908
+
 	/* ----------------------------------------------------
 	 * HW will do the cache maintenance while powering down
 	 * ----------------------------------------------------
@@ -42,15 +134,51 @@
 	 */
 #if REPORT_ERRATA
 func cortex_x2_errata_report
+	stp	x8, x30, [sp, #-16]!
+
+	bl	cpu_get_rev_var
+	mov	x8, x0
+
+	/*
+	 * Report all errata. The revision-variant information is passed to
+	 * checking functions of each errata.
+	 */
+	report_errata ERRATA_X2_2002765, cortex_x2, 2002765
+	report_errata ERRATA_X2_2058056, cortex_x2, 2058056
+	report_errata ERRATA_X2_2083908, cortex_x2, 2083908
+
+	ldp	x8, x30, [sp], #16
 	ret
 endfunc cortex_x2_errata_report
 #endif
 
 func cortex_x2_reset_func
+	mov	x19, x30
+
 	/* Disable speculative loads */
 	msr	SSBS, xzr
 	isb
-	ret
+
+	/* Get the CPU revision and stash it in x18. */
+	bl	cpu_get_rev_var
+	mov	x18, x0
+
+#if ERRATA_X2_2002765
+	mov	x0, x18
+	bl	errata_cortex_x2_2002765_wa
+#endif
+
+#if ERRATA_X2_2058056
+	mov	x0, x18
+	bl	errata_cortex_x2_2058056_wa
+#endif
+
+#if ERRATA_X2_2083908
+	mov	x0, x18
+	bl	errata_cortex_x2_2083908_wa
+#endif
+
+	ret x19
 endfunc cortex_x2_reset_func
 
 	/* ---------------------------------------------
diff --git a/lib/cpus/cpu-ops.mk b/lib/cpus/cpu-ops.mk
index a5b8aae..dd06cb8 100644
--- a/lib/cpus/cpu-ops.mk
+++ b/lib/cpus/cpu-ops.mk
@@ -495,6 +495,18 @@
 # to revision r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is still open.
 ERRATA_A710_2017096	?=0
 
+# Flag to apply erratum 2002765 workaround during reset. This erratum applies
+# to revisions r0p0, r1p0, and r2p0 of the Cortex-X2 cpu and is still open.
+ERRATA_X2_2002765   ?=0
+
+# Flag to apply erratum 2058056 workaround during reset. This erratum applies
+# to revisions r0p0, r1p0, and r2p0 of the Cortex-X2 cpu and is still open.
+ERRATA_X2_2058056   ?=0
+
+# Flag to apply erratum 2083908 workaround during reset. This erratum applies
+# to revision r2p0 of the Cortex-X2 cpu and is still open.
+ERRATA_X2_2083908   ?=0
+
 # Flag to apply DSU erratum 798953. This erratum applies to DSUs revision r0p0.
 # Applying the workaround results in higher DSU power consumption on idle.
 ERRATA_DSU_798953	?=0
@@ -920,6 +932,18 @@
 $(eval $(call assert_boolean,ERRATA_A710_2017096))
 $(eval $(call add_define,ERRATA_A710_2017096))
 
+# Process ERRATA_X2_2002765 flag
+$(eval $(call assert_boolean,ERRATA_X2_2002765))
+$(eval $(call add_define,ERRATA_X2_2002765))
+
+# Process ERRATA_X2_2058056 flag
+$(eval $(call assert_boolean,ERRATA_X2_2058056))
+$(eval $(call add_define,ERRATA_X2_2058056))
+
+# Process ERRATA_X2_2083908 flag
+$(eval $(call assert_boolean,ERRATA_X2_2083908))
+$(eval $(call add_define,ERRATA_X2_2083908))
+
 # Process ERRATA_DSU_798953 flag
 $(eval $(call assert_boolean,ERRATA_DSU_798953))
 $(eval $(call add_define,ERRATA_DSU_798953))
diff --git a/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index e270ad0..c9035e8 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S
@@ -145,11 +145,14 @@
 	stp	x11, x12, [x0, #CTX_MPAMVPM7_EL2]
 #endif
 
-#if ARM_ARCH_AT_LEAST(8, 6)
-	mrs	x13, HAFGRTR_EL2
-	mrs	x14, HDFGRTR_EL2
-	stp	x13, x14, [x0, #CTX_HAFGRTR_EL2]
-
+#if ENABLE_FEAT_FGT
+	mrs	x13, HDFGRTR_EL2
+#if ENABLE_FEAT_AMUv1
+   	mrs	x14, HAFGRTR_EL2
+   	stp	x13, x14, [x0, #CTX_HDFGRTR_EL2]
+#else
+   	str	x13, [x0, #CTX_HDFGRTR_EL2]
+#endif
 	mrs	x15, HDFGWTR_EL2
 	mrs	x16, HFGITR_EL2
 	stp	x15, x16, [x0, #CTX_HDFGWTR_EL2]
@@ -157,7 +160,9 @@
 	mrs	x9, HFGRTR_EL2
 	mrs	x10, HFGWTR_EL2
 	stp	x9, x10, [x0, #CTX_HFGRTR_EL2]
+#endif
 
+#if ENABLE_FEAT_ECV
 	mrs	x11, CNTPOFF_EL2
 	str	x11, [x0, #CTX_CNTPOFF_EL2]
 #endif
@@ -319,10 +324,14 @@
 	msr	MPAMVPMV_EL2, x12
 #endif
 
-#if ARM_ARCH_AT_LEAST(8, 6)
-	ldp	x13, x14, [x0, #CTX_HAFGRTR_EL2]
-	msr	HAFGRTR_EL2, x13
-	msr	HDFGRTR_EL2, x14
+#if ENABLE_FEAT_FGT
+#if ENABLE_FEAT_AMUv1
+	ldp	x13, x14, [x0, #CTX_HDFGRTR_EL2]
+	msr	HAFGRTR_EL2, x14
+#else
+	ldr	x13, [x0, #CTX_HDFGRTR_EL2]
+#endif
+	msr	HDFGRTR_EL2, x13
 
 	ldp	x15, x16, [x0, #CTX_HDFGWTR_EL2]
 	msr	HDFGWTR_EL2, x15
@@ -331,7 +340,9 @@
 	ldp	x9, x10, [x0, #CTX_HFGRTR_EL2]
 	msr	HFGRTR_EL2, x9
 	msr	HFGWTR_EL2, x10
+#endif
 
+#if ENABLE_FEAT_ECV
 	ldr	x11, [x0, #CTX_CNTPOFF_EL2]
 	msr	CNTPOFF_EL2, x11
 #endif
diff --git a/lib/fconf/fconf_dyn_cfg_getter.c b/lib/fconf/fconf_dyn_cfg_getter.c
index 25dd7f9..34623fb 100644
--- a/lib/fconf/fconf_dyn_cfg_getter.c
+++ b/lib/fconf/fconf_dyn_cfg_getter.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2019-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -40,17 +40,30 @@
 	dtb_info->config_id = config_id;
 }
 
-struct dyn_cfg_dtb_info_t *dyn_cfg_dtb_info_getter(unsigned int config_id)
+/* Get index of the config_id image */
+unsigned int dyn_cfg_dtb_info_get_index(unsigned int config_id)
 {
 	unsigned int index;
 
 	/* Positions index to the proper config-id */
 	for (index = 0U; index < MAX_DTB_INFO; index++) {
 		if (dtb_infos[index].config_id == config_id) {
-			return &dtb_infos[index];
+			return index;
 		}
 	}
 
+	return FCONF_INVALID_IDX;
+}
+
+struct dyn_cfg_dtb_info_t *dyn_cfg_dtb_info_getter(unsigned int config_id)
+{
+	/* Positions index to the proper config-id */
+	unsigned int index = dyn_cfg_dtb_info_get_index(config_id);
+
+	if (index < MAX_DTB_INFO) {
+		return &dtb_infos[index];
+	}
+
 	WARN("FCONF: Invalid config id %u\n", config_id);
 
 	return NULL;
diff --git a/lib/xlat_tables_v2/xlat_tables_utils.c b/lib/xlat_tables_v2/xlat_tables_utils.c
index df17386..38a375e 100644
--- a/lib/xlat_tables_v2/xlat_tables_utils.c
+++ b/lib/xlat_tables_v2/xlat_tables_utils.c
@@ -6,6 +6,7 @@
 
 #include <assert.h>
 #include <errno.h>
+#include <inttypes.h>
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdio.h>
@@ -199,7 +200,7 @@
 					(uint64_t *)addr_inner,
 					XLAT_TABLE_ENTRIES, level + 1U);
 			} else {
-				printf("%sVA:0x%lx PA:0x%llx size:0x%zx ",
+				printf("%sVA:0x%lx PA:0x%" PRIx64 " size:0x%zx ",
 				       level_spacers[level], table_idx_va,
 				       (uint64_t)(desc & TABLE_ADDR_MASK),
 				       level_size);
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index e88148f..72f5e33 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -136,6 +136,15 @@
 # Flag to enable access to the HCRX_EL2 register by setting SCR_EL3.HXEn.
 ENABLE_FEAT_HCX			:= 0
 
+# Flag to enable access to the HAFGRTR_EL2 register
+ENABLE_FEAT_AMUv1		:= 0
+
+# Flag to enable access to the HDFGRTR_EL2 register
+ENABLE_FEAT_FGT			:= 0
+
+# Flag to enable access to the CNTPOFF_EL2 register
+ENABLE_FEAT_ECV			:= 0
+
 # By default BL31 encryption disabled
 ENCRYPT_BL31			:= 0
 
diff --git a/plat/arm/board/fvp/fvp_bl1_measured_boot.c b/plat/arm/board/fvp/fvp_bl1_measured_boot.c
index 47af1f5..5468555 100644
--- a/plat/arm/board/fvp/fvp_bl1_measured_boot.c
+++ b/plat/arm/board/fvp/fvp_bl1_measured_boot.c
@@ -17,7 +17,8 @@
 	{ FW_CONFIG_ID, EVLOG_FW_CONFIG_STRING, PCR_0 },
 	{ TB_FW_CONFIG_ID, EVLOG_TB_FW_CONFIG_STRING, PCR_0 },
 	{ BL2_IMAGE_ID, EVLOG_BL2_STRING, PCR_0 },
-	{ INVALID_ID, NULL, (unsigned int)(-1) }	/* Terminator */
+
+	{ EVLOG_INVALID_ID, NULL, (unsigned int)(-1) }	/* Terminator */
 };
 
 void bl1_plat_mboot_init(void)
diff --git a/plat/arm/board/fvp/fvp_bl2_measured_boot.c b/plat/arm/board/fvp/fvp_bl2_measured_boot.c
index 5ebfede..4943e58 100644
--- a/plat/arm/board/fvp/fvp_bl2_measured_boot.c
+++ b/plat/arm/board/fvp/fvp_bl2_measured_boot.c
@@ -7,7 +7,11 @@
 #include <stdint.h>
 
 #include <drivers/measured_boot/event_log/event_log.h>
+#include <tools_share/tbbr_oid.h>
+#include <fvp_critical_data.h>
+
 #include <plat/arm/common/plat_arm.h>
+#include <plat/common/common_def.h>
 
 /* Event Log data */
 static uint64_t event_log_base;
@@ -24,7 +28,10 @@
 	{ SCP_BL2_IMAGE_ID, EVLOG_SCP_BL2_STRING, PCR_0 },
 	{ SOC_FW_CONFIG_ID, EVLOG_SOC_FW_CONFIG_STRING, PCR_0 },
 	{ TOS_FW_CONFIG_ID, EVLOG_TOS_FW_CONFIG_STRING, PCR_0 },
-	{ INVALID_ID, NULL, (unsigned int)(-1) }	/* Terminator */
+
+	{ CRITICAL_DATA_ID, EVLOG_CRITICAL_DATA_STRING, PCR_1 },
+
+	{ EVLOG_INVALID_ID, NULL, (unsigned int)(-1) }	/* Terminator */
 };
 
 void bl2_plat_mboot_init(void)
@@ -58,6 +65,60 @@
 	event_log_init((uint8_t *)event_log_start, event_log_finish);
 }
 
+int plat_mboot_measure_critical_data(unsigned int critical_data_id,
+				     const void *base, size_t size)
+{
+	/*
+	 * It is very unlikely that the critical data size would be
+	 * bigger than 2^32 bytes
+	 */
+	assert(size < UINT32_MAX);
+	assert(base != NULL);
+
+	/* Calculate image hash and record data in Event Log */
+	int err = event_log_measure_and_record((uintptr_t)base, (uint32_t)size,
+					       critical_data_id);
+	if (err != 0) {
+		ERROR("%s%s critical data (%i)\n",
+		      "Failed to ", "record",  err);
+		return err;
+	}
+
+	return 0;
+}
+
+static int fvp_populate_critical_data(struct fvp_critical_data *critical_data)
+{
+	char *nv_ctr_oids[MAX_NV_CTR_IDS] = {
+		[TRUSTED_NV_CTR_ID] = TRUSTED_FW_NVCOUNTER_OID,
+		[NON_TRUSTED_NV_CTR_ID] = NON_TRUSTED_FW_NVCOUNTER_OID,
+	};
+
+	for (int i = 0; i < MAX_NV_CTR_IDS; i++) {
+		int rc = plat_get_nv_ctr(nv_ctr_oids[i],
+					 &critical_data->nv_ctr[i]);
+		if (rc != 0) {
+			return rc;
+		}
+	}
+
+	return 0;
+}
+
+static int fvp_populate_and_measure_critical_data(void)
+{
+	struct fvp_critical_data populate_critical_data;
+
+	int rc = fvp_populate_critical_data(&populate_critical_data);
+	if (rc == 0) {
+		rc = plat_mboot_measure_critical_data(CRITICAL_DATA_ID,
+						&populate_critical_data,
+						sizeof(populate_critical_data));
+	}
+
+	return rc;
+}
+
 void bl2_plat_mboot_finish(void)
 {
 	int rc;
@@ -68,6 +129,11 @@
 	/* Event Log filled size */
 	size_t event_log_cur_size;
 
+	rc = fvp_populate_and_measure_critical_data();
+	if (rc != 0) {
+		panic();
+	}
+
 	event_log_cur_size = event_log_get_cur_size((uint8_t *)event_log_base);
 
 	rc = arm_set_nt_fw_info(
diff --git a/plat/arm/board/fvp/include/fvp_critical_data.h b/plat/arm/board/fvp/include/fvp_critical_data.h
new file mode 100644
index 0000000..3010d21
--- /dev/null
+++ b/plat/arm/board/fvp/include/fvp_critical_data.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/nv_cntr_ids.h>
+#include <lib/utils_def.h>
+#include <plat/common/platform.h>
+
+#define EVLOG_CRITICAL_DATA_STRING	"CRITICAL DATA"
+
+#define CRITICAL_DATA_ID		CRITICAL_DATA_ID_BASE
+
+struct fvp_critical_data {
+
+	/* platform NV counters */
+	unsigned int nv_ctr[MAX_NV_CTR_IDS];
+};
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index 0d2c319..b7f9c61 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -142,7 +142,8 @@
 					lib/cpus/aarch64/cortex_a65ae.S		\
 					lib/cpus/aarch64/cortex_a78c.S		\
 					lib/cpus/aarch64/cortex_hayes.S		\
-					lib/cpus/aarch64/cortex_hunter.S
+					lib/cpus/aarch64/cortex_hunter.S	\
+					lib/cpus/aarch64/cortex_x2.S
 	endif
 	# AArch64/AArch32 cores
 	FVP_CPU_LIBS	+=	lib/cpus/aarch64/cortex_a55.S		\
diff --git a/plat/arm/board/morello/fdts/morello_fw_config.dts b/plat/arm/board/morello/fdts/morello_fw_config.dts
new file mode 100644
index 0000000..c47bae5
--- /dev/null
+++ b/plat/arm/board/morello/fdts/morello_fw_config.dts
@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/tbbr/tbbr_img_def.h>
+
+/dts-v1/;
+/ {
+	dtb-registry {
+		compatible = "fconf,dyn_cfg-dtb_registry";
+
+		tb_fw-config {
+			load-address = <0x0 0x4001300>;
+			max-size = <0x200>;
+			id = <TB_FW_CONFIG_ID>;
+		};
+
+		nt_fw-config {
+			load-address = <0x0 0xFEF00000>;
+			max-size = <0x0100000>;
+			id = <NT_FW_CONFIG_ID>;
+		};
+	};
+};
diff --git a/plat/arm/board/morello/fdts/morello_nt_fw_config.dts b/plat/arm/board/morello/fdts/morello_nt_fw_config.dts
new file mode 100644
index 0000000..e730d34
--- /dev/null
+++ b/plat/arm/board/morello/fdts/morello_nt_fw_config.dts
@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/dts-v1/;
+/ {
+	/* compatible string */
+	compatible = "arm,morello";
+
+	/*
+	 * Place holder for platform-info node with default values.
+	 * The values will be set to the correct values during
+	 * the BL2 stage of boot.
+	 */
+	platform-info {
+		local-ddr-size = <0x0 0x0>;
+#ifdef TARGET_PLATFORM_SOC
+		remote-ddr-size = <0x0 0x0>;
+		remote-chip-count = <0x0>;
+		multichip-mode = <0x0>;
+		scc-config = <0x0>;
+#endif
+	};
+};
diff --git a/plat/arm/board/morello/fdts/morello_tb_fw_config.dts b/plat/arm/board/morello/fdts/morello_tb_fw_config.dts
new file mode 100644
index 0000000..305a818
--- /dev/null
+++ b/plat/arm/board/morello/fdts/morello_tb_fw_config.dts
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/dts-v1/;
+/ {
+	tb_fw-config {
+		compatible = "arm,tb_fw";
+
+		/* Disable authentication for development */
+		disable_auth = <0x0>;
+
+		/*
+		 * The following two entries are placeholders for Mbed TLS
+		 * heap information. The default values don't matter since
+		 * they will be overwritten by BL1.
+		 * In case of having shared Mbed TLS heap between BL1 and BL2,
+		 * BL1 will populate these two properties with the respective
+		 * info about the shared heap. This info will be available for
+		 * BL2 in order to locate and re-use the heap.
+		 */
+		mbedtls_heap_addr = <0x0 0x0>;
+		mbedtls_heap_size = <0x0>;
+	};
+};
diff --git a/plat/arm/board/morello/include/platform_def.h b/plat/arm/board/morello/include/platform_def.h
index 9ca75ff..08e2d60 100644
--- a/plat/arm/board/morello/include/platform_def.h
+++ b/plat/arm/board/morello/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -15,8 +15,9 @@
 #define PLAT_ARM_BOOT_UART_BASE 		ULL(0x2A400000)
 #define PLAT_ARM_BOOT_UART_CLK_IN_HZ		U(50000000)
 
-#define PLAT_ARM_RUN_UART_BASE			ULL(0x2A410000)
-#define PLAT_ARM_RUN_UART_CLK_IN_HZ		U(50000000)
+/* IOFPGA UART0 */
+#define PLAT_ARM_RUN_UART_BASE			ULL(0x1C090000)
+#define PLAT_ARM_RUN_UART_CLK_IN_HZ		U(24000000)
 
 #define PLAT_ARM_CRASH_UART_BASE		PLAT_ARM_RUN_UART_BASE
 #define PLAT_ARM_CRASH_UART_CLK_IN_HZ		PLAT_ARM_RUN_UART_CLK_IN_HZ
@@ -24,6 +25,30 @@
 #define PLAT_ARM_DRAM2_BASE			ULL(0x8080000000)
 #define PLAT_ARM_DRAM2_SIZE			ULL(0xF80000000)
 
+#define MAX_IO_DEVICES				U(3)
+#define MAX_IO_HANDLES				U(4)
+
+#define PLAT_ARM_FLASH_IMAGE_BASE		ULL(0x1A000000)
+#define PLAT_ARM_FLASH_IMAGE_MAX_SIZE		ULL(0x01000000)
+
+#define PLAT_ARM_NVM_BASE			ULL(0x1A000000)
+#define PLAT_ARM_NVM_SIZE			ULL(0x01000000)
+
+#if defined NS_BL1U_BASE
+#undef NS_BL1U_BASE
+#define NS_BL1U_BASE			(PLAT_ARM_NVM_BASE + UL(0x00800000))
+#endif
+
+/*
+ * There are no non-volatile counters in morello, these macros points
+ * to unused addresses.
+ */
+#define SOC_TRUSTED_NVCTR_BASE		ULL(0x7FE70000)
+#define TFW_NVCTR_BASE			(SOC_TRUSTED_NVCTR_BASE + U(0x0000))
+#define TFW_NVCTR_SIZE			U(4)
+#define NTFW_CTR_BASE			(SOC_TRUSTED_NVCTR_BASE + U(0x0004))
+#define NTFW_CTR_SIZE			U(4)
+
 /*
  * To access the complete DDR memory along with remote chip's DDR memory,
  * which is at 4 TB offset, physical and virtual address space limits are
@@ -39,7 +64,36 @@
 #endif
 
 #define PLAT_ARM_TRUSTED_SRAM_SIZE		UL(0x00080000)
-#define PLAT_ARM_MAX_BL31_SIZE			UL(0x20000)
+
+/*
+ * PLAT_ARM_MAX_BL1_RW_SIZE is calculated using the current BL1 RW debug size
+ * plus a little space for growth.
+ */
+#define PLAT_ARM_MAX_BL1_RW_SIZE		UL(0xC000)
+
+/*
+ * PLAT_ARM_MAX_ROMLIB_RW_SIZE is define to use a full page
+ */
+
+#if USE_ROMLIB
+#define PLAT_ARM_MAX_ROMLIB_RW_SIZE		UL(0x1000)
+#define PLAT_ARM_MAX_ROMLIB_RO_SIZE		UL(0xE000)
+#else
+#define PLAT_ARM_MAX_ROMLIB_RW_SIZE		U(0)
+#define PLAT_ARM_MAX_ROMLIB_RO_SIZE		U(0)
+#endif
+
+/*
+ * PLAT_ARM_MAX_BL2_SIZE is calculated using the current BL2 debug size plus a
+ * little space for growth.
+ */
+#if TRUSTED_BOARD_BOOT
+# define PLAT_ARM_MAX_BL2_SIZE			UL(0x1D000)
+#else
+# define PLAT_ARM_MAX_BL2_SIZE			UL(0x14000)
+#endif
+
+#define PLAT_ARM_MAX_BL31_SIZE			UL(0x3B000)
 
 /*******************************************************************************
  * MORELLO topology related constants
@@ -62,12 +116,49 @@
  * PLAT_ARM_MMAP_ENTRIES depends on the number of entries in the
  * plat_arm_mmap array defined for each BL stage.
  */
-#define PLAT_ARM_MMAP_ENTRIES			U(9)
-#define MAX_XLAT_TABLES				U(10)
+#if IMAGE_BL1 || IMAGE_BL31
+# define PLAT_ARM_MMAP_ENTRIES			U(6)
+# define MAX_XLAT_TABLES			U(7)
+#else
+# define PLAT_ARM_MMAP_ENTRIES			U(5)
+# define MAX_XLAT_TABLES			U(6)
+#endif
 
-#define PLATFORM_STACK_SIZE			U(0x400)
+/*
+ * Size of cacheable stacks
+ */
+#if defined(IMAGE_BL1)
+# if TRUSTED_BOARD_BOOT
+#  define PLATFORM_STACK_SIZE			UL(0x1000)
+# else
+#  define PLATFORM_STACK_SIZE			UL(0x440)
+# endif
+#elif defined(IMAGE_BL2)
+# if TRUSTED_BOARD_BOOT
+#  define PLATFORM_STACK_SIZE			UL(0x1000)
+# else
+#  define PLATFORM_STACK_SIZE			UL(0x400)
+# endif
+#elif defined(IMAGE_BL2U)
+# define PLATFORM_STACK_SIZE			UL(0x400)
+#elif defined(IMAGE_BL31)
+# if SPM_MM
+#  define PLATFORM_STACK_SIZE			UL(0x500)
+# else
+#  define PLATFORM_STACK_SIZE			UL(0x400)
+# endif
+#elif defined(IMAGE_BL32)
+# define PLATFORM_STACK_SIZE			UL(0x440)
+#endif
 
 #define PLAT_ARM_NSTIMER_FRAME_ID		U(0)
+
+#define PLAT_ARM_TRUSTED_ROM_BASE		U(0x0)
+#define PLAT_ARM_TRUSTED_ROM_SIZE		UL(0x00020000)	/* 128KB */
+
+#define PLAT_ARM_NSRAM_BASE			ULL(0x06000000)
+#define PLAT_ARM_NSRAM_SIZE			UL(0x00010000)	/* 64KB */
+
 #define PLAT_CSS_MHU_BASE			UL(0x45000000)
 #define PLAT_MHUV2_BASE				PLAT_CSS_MHU_BASE
 #define PLAT_MAX_PWR_LVL			U(2)
@@ -78,6 +169,10 @@
 #define MORELLO_DEVICE_BASE			ULL(0x08000000)
 #define MORELLO_DEVICE_SIZE			ULL(0x48000000)
 
+/*Secure Watchdog Constants */
+#define SBSA_SECURE_WDOG_BASE			UL(0x2A480000)
+#define SBSA_SECURE_WDOG_TIMEOUT		UL(1000)
+
 #define MORELLO_MAP_DEVICE			MAP_REGION_FLAT(	\
 						MORELLO_DEVICE_BASE,	\
 						MORELLO_DEVICE_SIZE,	\
diff --git a/plat/arm/board/morello/morello_bl1_setup.c b/plat/arm/board/morello/morello_bl1_setup.c
new file mode 100644
index 0000000..9155542
--- /dev/null
+++ b/plat/arm/board/morello/morello_bl1_setup.c
@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <plat/arm/common/plat_arm.h>
+
+/*******************************************************************************
+ * Perform any BL1 specific platform actions.
+ ******************************************************************************/
+
+void soc_css_init_nic400(void)
+{
+}
+
+void soc_css_init_pcie(void)
+{
+}
diff --git a/plat/arm/board/morello/morello_bl2_setup.c b/plat/arm/board/morello/morello_bl2_setup.c
new file mode 100644
index 0000000..0d4b6d0
--- /dev/null
+++ b/plat/arm/board/morello/morello_bl2_setup.c
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <lib/utils.h>
+#include <plat/arm/common/plat_arm.h>
+
+void bl2_platform_setup(void)
+{
+#ifdef TARGET_PLATFORM_SOC
+	/*
+	 * Morello platform supports RDIMMs with ECC capability. To use the ECC
+	 * capability, the entire DDR memory space has to be zeroed out before
+	 * enabling the ECC bits in DMC-Bing.
+	 * Zeroing DDR memory range 0x80000000 - 0xFFFFFFFF during BL2 stage,
+	 * as BL33 binary cannot be copied to DDR memory before enabling ECC.
+	 * Rest of the DDR memory space is zeroed out during BL31 stage.
+	 */
+	INFO("Zeroing DDR memory range 0x80000000 - 0xFFFFFFFF\n");
+	zero_normalmem((void *)ARM_DRAM1_BASE, ARM_DRAM1_SIZE);
+	flush_dcache_range(ARM_DRAM1_BASE, ARM_DRAM1_SIZE);
+#endif
+	arm_bl2_platform_setup();
+}
diff --git a/plat/arm/board/morello/morello_bl31_setup.c b/plat/arm/board/morello/morello_bl31_setup.c
index 59dd37b..e418518 100644
--- a/plat/arm/board/morello/morello_bl31_setup.c
+++ b/plat/arm/board/morello/morello_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,40 +9,45 @@
 #include <drivers/arm/css/scmi.h>
 #include <drivers/arm/css/sds.h>
 #include <lib/cassert.h>
+#include <lib/utils.h>
 #include <plat/arm/common/plat_arm.h>
 
 #include "morello_def.h"
 #include <platform_def.h>
 
+#ifdef TARGET_PLATFORM_FVP
+/*
+ * Platform information structure stored in SDS.
+ * This structure holds information about platform's DDR
+ * size
+ *	- Local DDR size in bytes, DDR memory in main board
+ */
+struct morello_plat_info {
+	uint64_t local_ddr_size;
+} __packed;
+#else
 /*
  * Platform information structure stored in SDS.
  * This structure holds information about platform's DDR
  * size which is an information about multichip setup
- *	- Local DDR size in bytes, DDR memory in master board
- *	- Remote DDR size in bytes, DDR memory in slave board
- *	- slave_count
+ *	- Local DDR size in bytes, DDR memory in main board
+ *	- Remote DDR size in bytes, DDR memory in remote board
+ *	- remote_chip_count
  *	- multichip mode
+ *	- scc configuration
  */
 struct morello_plat_info {
 	uint64_t local_ddr_size;
 	uint64_t remote_ddr_size;
-	uint8_t slave_count;
+	uint8_t remote_chip_count;
 	bool multichip_mode;
+	uint32_t scc_config;
 } __packed;
+#endif
 
 /* Compile time assertion to ensure the size of structure is 18 bytes */
 CASSERT(sizeof(struct morello_plat_info) == MORELLO_SDS_PLATFORM_INFO_SIZE,
 		assert_invalid_plat_info_size);
-/*
- * BL33 image information structure stored in SDS.
- * This structure holds the source & destination addresses and
- * the size of the BL33 image which will be loaded by BL31.
- */
-struct morello_bl33_info {
-	uint32_t bl33_src_addr;
-	uint32_t bl33_dst_addr;
-	uint32_t bl33_size;
-};
 
 static scmi_channel_plat_info_t morello_scmi_plat_info = {
 	.scmi_mbx_mem = MORELLO_SCMI_PAYLOAD_BASE,
@@ -62,29 +67,143 @@
 	return css_scmi_override_pm_ops(ops);
 }
 
-static void copy_bl33(uint32_t src, uint32_t dst, uint32_t size)
+#ifdef TARGET_PLATFORM_SOC
+/*
+ * Morello platform supports RDIMMs with ECC capability. To use the ECC
+ * capability, the entire DDR memory space has to be zeroed out before
+ * enabling the ECC bits in DMC-Bing. Zeroing out several gigabytes of
+ * memory from SCP is quite time consuming so the following function
+ * is added to zero out the DDR memory from application processor which is
+ * much faster compared to SCP.
+ */
+
+static void dmc_ecc_setup(struct morello_plat_info *plat_info)
 {
-	unsigned int i;
+	uint64_t dram2_size;
+	uint32_t val;
+	uint64_t tag_mem_base;
+	uint64_t usable_mem_size;
 
-	INFO("Copying BL33 to DDR memory...\n");
-	for (i = 0U; i < size; (i = i + 8U))
-		mmio_write_64((dst + i), mmio_read_64(src + i));
+	INFO("Total DIMM size: %uGB\n",
+			(uint32_t)(plat_info->local_ddr_size / 0x40000000));
 
-	for (i = 0U; i < size; (i = i + 8U)) {
-		if (mmio_read_64(src + i) != mmio_read_64(dst + i)) {
-			ERROR("Copy failed!\n");
-			panic();
-		}
+	assert(plat_info->local_ddr_size > ARM_DRAM1_SIZE);
+	dram2_size = plat_info->local_ddr_size - ARM_DRAM1_SIZE;
+
+	INFO("Zeroing DDR memory range 0x%llx - 0x%llx\n",
+		ARM_DRAM2_BASE, ARM_DRAM2_BASE + dram2_size);
+	zero_normalmem((void *)ARM_DRAM2_BASE, dram2_size);
+	flush_dcache_range(ARM_DRAM2_BASE, dram2_size);
+
+	/* Clear previous ECC errors while zeroing out the memory */
+	val = mmio_read_32(MORELLO_DMC0_ERR2STATUS_REG);
+	mmio_write_32(MORELLO_DMC0_ERR2STATUS_REG, val);
+
+	val = mmio_read_32(MORELLO_DMC1_ERR2STATUS_REG);
+	mmio_write_32(MORELLO_DMC1_ERR2STATUS_REG, val);
+
+	/* Set DMCs to CONFIG state before writing ERR0CTLR0 register */
+	mmio_write_32(MORELLO_DMC0_MEMC_CMD_REG, MORELLO_DMC_MEMC_CMD_CONFIG);
+	mmio_write_32(MORELLO_DMC1_MEMC_CMD_REG, MORELLO_DMC_MEMC_CMD_CONFIG);
+
+	while ((mmio_read_32(MORELLO_DMC0_MEMC_STATUS_REG) &
+			MORELLO_DMC_MEMC_STATUS_MASK) !=
+			MORELLO_DMC_MEMC_CMD_CONFIG) {
+		continue;
 	}
-	INFO("done\n");
+
+	while ((mmio_read_32(MORELLO_DMC1_MEMC_STATUS_REG) &
+			MORELLO_DMC_MEMC_STATUS_MASK) !=
+			MORELLO_DMC_MEMC_CMD_CONFIG) {
+		continue;
+	}
+
+	/* Configure Bing client/server mode based on SCC configuration */
+	if (plat_info->scc_config & MORELLO_SCC_CLIENT_MODE_MASK) {
+		INFO("Configuring DMC Bing in client mode\n");
+		usable_mem_size = plat_info->local_ddr_size -
+			(plat_info->local_ddr_size / 128ULL);
+
+		/* Linear DDR address */
+		tag_mem_base = usable_mem_size;
+		tag_mem_base = tag_mem_base / 4;
+
+		/* Reverse translation */
+		if (tag_mem_base < ARM_DRAM1_BASE) {
+			tag_mem_base += ARM_DRAM1_BASE;
+		} else {
+			tag_mem_base = tag_mem_base - ARM_DRAM1_BASE +
+				ARM_DRAM2_BASE;
+		}
+
+		mmio_write_32(MORELLO_DMC0_CAP_CTRL_REG, 0x1);
+		mmio_write_32(MORELLO_DMC1_CAP_CTRL_REG, 0x1);
+		mmio_write_32(MORELLO_DMC0_TAG_CACHE_CFG, 0x1);
+		mmio_write_32(MORELLO_DMC1_TAG_CACHE_CFG, 0x1);
+
+		if (plat_info->scc_config & MORELLO_SCC_C1_TAG_CACHE_EN_MASK) {
+			mmio_setbits_32(MORELLO_DMC0_TAG_CACHE_CFG, 0x2);
+			mmio_setbits_32(MORELLO_DMC1_TAG_CACHE_CFG, 0x2);
+			INFO("C1 Tag Cache Enabled\n");
+		}
+
+		if (plat_info->scc_config & MORELLO_SCC_C2_TAG_CACHE_EN_MASK) {
+			mmio_setbits_32(MORELLO_DMC0_TAG_CACHE_CFG, 0x4);
+			mmio_setbits_32(MORELLO_DMC1_TAG_CACHE_CFG, 0x4);
+			INFO("C2 Tag Cache Enabled\n");
+		}
+
+		mmio_write_32(MORELLO_DMC0_MEM_ADDR_CTL,
+				(uint32_t)tag_mem_base);
+		mmio_write_32(MORELLO_DMC1_MEM_ADDR_CTL,
+				(uint32_t)tag_mem_base);
+		mmio_write_32(MORELLO_DMC0_MEM_ADDR_CTL2,
+				(uint32_t)(tag_mem_base >> 32));
+		mmio_write_32(MORELLO_DMC1_MEM_ADDR_CTL2,
+				(uint32_t)(tag_mem_base >> 32));
+
+		mmio_setbits_32(MORELLO_DMC0_MEM_ACCESS_CTL,
+				MORELLO_DMC_MEM_ACCESS_DIS);
+		mmio_setbits_32(MORELLO_DMC1_MEM_ACCESS_CTL,
+				MORELLO_DMC_MEM_ACCESS_DIS);
+
+		INFO("Tag base set to 0x%lx\n", tag_mem_base);
+		plat_info->local_ddr_size = usable_mem_size;
+	} else {
+		INFO("Configuring DMC Bing in server mode\n");
+		mmio_write_32(MORELLO_DMC0_CAP_CTRL_REG, 0x0);
+		mmio_write_32(MORELLO_DMC1_CAP_CTRL_REG, 0x0);
+	}
+
+	INFO("Enabling ECC on DMCs\n");
+	/* Enable ECC in DMCs */
+	mmio_setbits_32(MORELLO_DMC0_ERR0CTLR0_REG,
+		MORELLO_DMC_ERR0CTLR0_ECC_EN);
+	mmio_setbits_32(MORELLO_DMC1_ERR0CTLR0_REG,
+		MORELLO_DMC_ERR0CTLR0_ECC_EN);
+
+	/* Set DMCs to READY state */
+	mmio_write_32(MORELLO_DMC0_MEMC_CMD_REG, MORELLO_DMC_MEMC_CMD_READY);
+	mmio_write_32(MORELLO_DMC1_MEMC_CMD_REG, MORELLO_DMC_MEMC_CMD_READY);
+
+	while ((mmio_read_32(MORELLO_DMC0_MEMC_STATUS_REG) &
+			MORELLO_DMC_MEMC_STATUS_MASK) !=
+			MORELLO_DMC_MEMC_CMD_READY) {
+		continue;
+	}
+
+	while ((mmio_read_32(MORELLO_DMC1_MEMC_STATUS_REG) &
+			MORELLO_DMC_MEMC_STATUS_MASK) !=
+			MORELLO_DMC_MEMC_CMD_READY) {
+		continue;
+	}
 }
+#endif
 
 void bl31_platform_setup(void)
 {
 	int ret;
 	struct morello_plat_info plat_info;
-	struct morello_bl33_info bl33_info;
-	struct morello_plat_info *copy_dest;
 
 	ret = sds_init();
 	if (ret != SDS_OK) {
@@ -103,35 +222,22 @@
 	}
 
 	/* Validate plat_info SDS */
+#ifdef TARGET_PLATFORM_FVP
+	if (plat_info.local_ddr_size == 0U) {
+#else
 	if ((plat_info.local_ddr_size == 0U)
 		|| (plat_info.local_ddr_size > MORELLO_MAX_DDR_CAPACITY)
 		|| (plat_info.remote_ddr_size > MORELLO_MAX_DDR_CAPACITY)
-		|| (plat_info.slave_count > MORELLO_MAX_SLAVE_COUNT)) {
+		|| (plat_info.remote_chip_count > MORELLO_MAX_REMOTE_CHIP_COUNT)
+		) {
+#endif
 		ERROR("platform info SDS is corrupted\n");
 		panic();
 	}
 
 	arm_bl31_platform_setup();
 
-	ret = sds_struct_read(MORELLO_SDS_BL33_INFO_STRUCT_ID,
-				MORELLO_SDS_BL33_INFO_OFFSET,
-				&bl33_info,
-				MORELLO_SDS_BL33_INFO_SIZE,
-				SDS_ACCESS_MODE_NON_CACHED);
-	if (ret != SDS_OK) {
-		ERROR("Error getting BL33 info from SDS. ret:%d\n", ret);
-		panic();
-	}
-	copy_bl33(bl33_info.bl33_src_addr,
-			bl33_info.bl33_dst_addr,
-			bl33_info.bl33_size);
-	/*
-	 * Pass platform information to BL33. This method is followed as
-	 * currently there is no BL1/BL2 involved in boot flow of MORELLO.
-	 * When TBBR is implemented for MORELLO, this method should be removed
-	 * and platform information should be passed to BL33 using NT_FW_CONFIG
-	 * passing mechanism.
-	 */
-	copy_dest = (struct morello_plat_info *)MORELLO_PLATFORM_INFO_BASE;
-	*copy_dest = plat_info;
+#ifdef TARGET_PLATFORM_SOC
+	dmc_ecc_setup(&plat_info);
+#endif
 }
diff --git a/plat/arm/board/morello/morello_def.h b/plat/arm/board/morello/morello_def.h
index 793729b..f154924 100644
--- a/plat/arm/board/morello/morello_def.h
+++ b/plat/arm/board/morello/morello_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,16 +18,75 @@
 /* SDS Platform information defines */
 #define MORELLO_SDS_PLATFORM_INFO_STRUCT_ID	U(8)
 #define MORELLO_SDS_PLATFORM_INFO_OFFSET	U(0)
-#define MORELLO_SDS_PLATFORM_INFO_SIZE		U(18)
+#ifdef TARGET_PLATFORM_FVP
+# define MORELLO_SDS_PLATFORM_INFO_SIZE		U(8)
+#else
+# define MORELLO_SDS_PLATFORM_INFO_SIZE		U(22)
+#endif
 #define MORELLO_MAX_DDR_CAPACITY		U(0x1000000000)
-#define MORELLO_MAX_SLAVE_COUNT			U(16)
+#define MORELLO_MAX_REMOTE_CHIP_COUNT		U(16)
 
-/* SDS BL33 image information defines */
-#define MORELLO_SDS_BL33_INFO_STRUCT_ID		U(9)
-#define MORELLO_SDS_BL33_INFO_OFFSET		U(0)
-#define MORELLO_SDS_BL33_INFO_SIZE		U(12)
+#define MORELLO_SCC_SERVER_MODE			U(0)
+#define MORELLO_SCC_CLIENT_MODE_MASK		U(1)
+#define MORELLO_SCC_C1_TAG_CACHE_EN_MASK	U(4)
+#define MORELLO_SCC_C2_TAG_CACHE_EN_MASK	U(8)
 
 /* Base address of non-secure SRAM where Platform information will be filled */
 #define MORELLO_PLATFORM_INFO_BASE		UL(0x06000000)
 
+/* DMC memory status registers */
+#define MORELLO_DMC0_MEMC_STATUS_REG		UL(0x4E000000)
+#define MORELLO_DMC1_MEMC_STATUS_REG		UL(0x4E100000)
+
+#define MORELLO_DMC_MEMC_STATUS_MASK		U(7)
+
+/* DMC memory command registers */
+#define MORELLO_DMC0_MEMC_CMD_REG		UL(0x4E000008)
+#define MORELLO_DMC1_MEMC_CMD_REG		UL(0x4E100008)
+
+/* DMC capability control register */
+#define MORELLO_DMC0_CAP_CTRL_REG		UL(0x4E000D00)
+#define MORELLO_DMC1_CAP_CTRL_REG		UL(0x4E100D00)
+
+/* DMC tag cache control register */
+#define MORELLO_DMC0_TAG_CACHE_CTL		UL(0x4E000D04)
+#define MORELLO_DMC1_TAG_CACHE_CTL		UL(0x4E100D04)
+
+/* DMC tag cache config register */
+#define MORELLO_DMC0_TAG_CACHE_CFG		UL(0x4E000D08)
+#define MORELLO_DMC1_TAG_CACHE_CFG		UL(0x4E100D08)
+
+/* DMC memory access control register */
+#define MORELLO_DMC0_MEM_ACCESS_CTL		UL(0x4E000D0C)
+#define MORELLO_DMC1_MEM_ACCESS_CTL		UL(0x4E100D0C)
+
+#define MORELLO_DMC_MEM_ACCESS_DIS		(1UL << 16)
+
+/* DMC memory address control register */
+#define MORELLO_DMC0_MEM_ADDR_CTL		UL(0x4E000D10)
+#define MORELLO_DMC1_MEM_ADDR_CTL		UL(0x4E100D10)
+
+/* DMC memory address control 2 register */
+#define MORELLO_DMC0_MEM_ADDR_CTL2		UL(0x4E000D14)
+#define MORELLO_DMC1_MEM_ADDR_CTL2		UL(0x4E100D14)
+
+/* DMC special control register */
+#define MORELLO_DMC0_SPL_CTL_REG		UL(0x4E000D18)
+#define MORELLO_DMC1_SPL_CTL_REG		UL(0x4E100D18)
+
+/* DMC ERR0CTLR0 registers */
+#define MORELLO_DMC0_ERR0CTLR0_REG		UL(0x4E000708)
+#define MORELLO_DMC1_ERR0CTLR0_REG		UL(0x4E100708)
+
+/* DMC ECC in ERR0CTLR0 register */
+#define MORELLO_DMC_ERR0CTLR0_ECC_EN		U(9)
+
+/* DMC ERR2STATUS register */
+#define MORELLO_DMC0_ERR2STATUS_REG		UL(0x4E000790)
+#define MORELLO_DMC1_ERR2STATUS_REG		UL(0x4E100790)
+
+/* DMC memory commands */
+#define MORELLO_DMC_MEMC_CMD_CONFIG		U(0)
+#define MORELLO_DMC_MEMC_CMD_READY		U(3)
+
 #endif /* MORELLO_DEF_H */
diff --git a/plat/arm/board/morello/morello_err.c b/plat/arm/board/morello/morello_err.c
new file mode 100644
index 0000000..4d20a09
--- /dev/null
+++ b/plat/arm/board/morello/morello_err.c
@@ -0,0 +1,17 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <plat/arm/common/plat_arm.h>
+
+/*
+ * morello error handler
+ */
+void __dead2 plat_arm_error_handler(int err)
+{
+	while (true) {
+		wfi();
+	}
+}
diff --git a/plat/arm/board/morello/morello_image_load.c b/plat/arm/board/morello/morello_image_load.c
new file mode 100644
index 0000000..52d46f3
--- /dev/null
+++ b/plat/arm/board/morello/morello_image_load.c
@@ -0,0 +1,185 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <common/desc_image_load.h>
+#include <drivers/arm/css/sds.h>
+#include <libfdt.h>
+
+#include "morello_def.h"
+#include <plat/arm/common/plat_arm.h>
+#include <plat/common/platform.h>
+
+#ifdef TARGET_PLATFORM_FVP
+/*
+ * Platform information structure stored in SDS.
+ * This structure holds information about platform's DDR
+ * size which is an information about multichip setup
+ *	- Local DDR size in bytes, DDR memory in main board
+ */
+struct morello_plat_info {
+	uint64_t local_ddr_size;
+} __packed;
+#else
+/*
+ * Platform information structure stored in SDS.
+ * This structure holds information about platform's DDR
+ * size which is an information about multichip setup
+ *	- Local DDR size in bytes, DDR memory in main board
+ *	- Remote DDR size in bytes, DDR memory in remote board
+ *	- remote_chip_count
+ *	- multichip mode
+ *	- scc configuration
+ */
+struct morello_plat_info {
+	uint64_t local_ddr_size;
+	uint64_t remote_ddr_size;
+	uint8_t remote_chip_count;
+	bool multichip_mode;
+	uint32_t scc_config;
+} __packed;
+#endif
+
+/* In client mode, a part of the DDR memory is reserved for Tag bits.
+ * Calculate the usable memory size after subtracting the Tag memory.
+ */
+static inline uint64_t get_mem_client_mode(uint64_t size)
+{
+	return (size - (size / 128ULL));
+}
+
+/*******************************************************************************
+ * This function inserts Platform information via device tree nodes as,
+ *	platform-info {
+ *		local-ddr-size = <0x0 0x0>;
+ *#ifdef TARGET_PLATFORM_SOC
+ *		remote-ddr-size = <0x0 0x0>;
+ *		remote-chip-count = <0x0>;
+ *		multichip-mode = <0x0>;
+ *		scc-config = <0x0>;
+ *#endif
+ *	};
+ ******************************************************************************/
+static int plat_morello_append_config_node(struct morello_plat_info *plat_info)
+{
+	bl_mem_params_node_t *mem_params;
+	void *fdt;
+	int nodeoffset, err;
+	uint64_t usable_mem_size;
+
+	usable_mem_size = plat_info->local_ddr_size;
+
+	mem_params = get_bl_mem_params_node(NT_FW_CONFIG_ID);
+	if (mem_params == NULL) {
+		ERROR("NT_FW CONFIG base address is NULL\n");
+		return -1;
+	}
+
+	fdt = (void *)(mem_params->image_info.image_base);
+
+	/* Check the validity of the fdt */
+	if (fdt_check_header(fdt) != 0) {
+		ERROR("Invalid NT_FW_CONFIG DTB passed\n");
+		return -1;
+	}
+
+	nodeoffset = fdt_subnode_offset(fdt, 0, "platform-info");
+	if (nodeoffset < 0) {
+		ERROR("NT_FW_CONFIG: Failed to get platform-info node offset\n");
+		return -1;
+	}
+
+#ifdef TARGET_PLATFORM_SOC
+	err = fdt_setprop_u64(fdt, nodeoffset, "remote-ddr-size",
+			plat_info->remote_ddr_size);
+	if (err < 0) {
+		ERROR("NT_FW_CONFIG: Failed to set remote-ddr-size\n");
+		return -1;
+	}
+
+	err = fdt_setprop_u32(fdt, nodeoffset, "remote-chip-count",
+			plat_info->remote_chip_count);
+	if (err < 0) {
+		ERROR("NT_FW_CONFIG: Failed to set remote-chip-count\n");
+		return -1;
+	}
+
+	err = fdt_setprop_u32(fdt, nodeoffset, "multichip-mode",
+			plat_info->multichip_mode);
+	if (err < 0) {
+		ERROR("NT_FW_CONFIG: Failed to set multichip-mode\n");
+		return -1;
+	}
+
+	err = fdt_setprop_u32(fdt, nodeoffset, "scc-config",
+			plat_info->scc_config);
+	if (err < 0) {
+		ERROR("NT_FW_CONFIG: Failed to set scc-config\n");
+		return -1;
+	}
+
+	if (plat_info->scc_config & MORELLO_SCC_CLIENT_MODE_MASK) {
+		usable_mem_size = get_mem_client_mode(plat_info->local_ddr_size);
+	}
+#endif
+	err = fdt_setprop_u64(fdt, nodeoffset, "local-ddr-size",
+			usable_mem_size);
+	if (err < 0) {
+		ERROR("NT_FW_CONFIG: Failed to set local-ddr-size\n");
+		return -1;
+	}
+
+	flush_dcache_range((uintptr_t)fdt, mem_params->image_info.image_size);
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function returns the list of executable images.
+ ******************************************************************************/
+bl_params_t *plat_get_next_bl_params(void)
+{
+	int ret;
+	struct morello_plat_info plat_info;
+
+	ret = sds_init();
+	if (ret != SDS_OK) {
+		ERROR("SDS initialization failed. ret:%d\n", ret);
+		panic();
+	}
+
+	ret = sds_struct_read(MORELLO_SDS_PLATFORM_INFO_STRUCT_ID,
+				MORELLO_SDS_PLATFORM_INFO_OFFSET,
+				&plat_info,
+				MORELLO_SDS_PLATFORM_INFO_SIZE,
+				SDS_ACCESS_MODE_NON_CACHED);
+	if (ret != SDS_OK) {
+		ERROR("Error getting platform info from SDS. ret:%d\n", ret);
+		panic();
+	}
+
+	/* Validate plat_info SDS */
+#ifdef TARGET_PLATFORM_FVP
+	if (plat_info.local_ddr_size == 0U) {
+#else
+	if ((plat_info.local_ddr_size == 0U)
+		|| (plat_info.local_ddr_size > MORELLO_MAX_DDR_CAPACITY)
+		|| (plat_info.remote_ddr_size > MORELLO_MAX_DDR_CAPACITY)
+		|| (plat_info.remote_chip_count > MORELLO_MAX_REMOTE_CHIP_COUNT)
+		){
+#endif
+		ERROR("platform info SDS is corrupted\n");
+		panic();
+	}
+
+	ret = plat_morello_append_config_node(&plat_info);
+	if (ret != 0) {
+		panic();
+	}
+
+	return arm_get_next_bl_params();
+}
diff --git a/plat/arm/board/morello/morello_plat.c b/plat/arm/board/morello/morello_plat.c
index 3830687..42e5171 100644
--- a/plat/arm/board/morello/morello_plat.c
+++ b/plat/arm/board/morello/morello_plat.c
@@ -1,9 +1,12 @@
 /*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <assert.h>
+
+#include <drivers/arm/sbsa.h>
 #include <plat/arm/common/plat_arm.h>
 
 #include "morello_def.h"
@@ -12,11 +15,45 @@
  * Table of regions to map using the MMU.
  * Replace or extend the below regions as required
  */
-
+#if IMAGE_BL1 || IMAGE_BL31
 const mmap_region_t plat_arm_mmap[] = {
 	ARM_MAP_SHARED_RAM,
 	MORELLO_MAP_DEVICE,
 	MORELLO_MAP_NS_SRAM,
 	ARM_MAP_DRAM1,
+	ARM_MAP_DRAM2,
 	{0}
 };
+#endif
+#if IMAGE_BL2
+const mmap_region_t plat_arm_mmap[] = {
+	ARM_MAP_SHARED_RAM,
+	MORELLO_MAP_DEVICE,
+	MORELLO_MAP_NS_SRAM,
+	ARM_MAP_DRAM1,
+#if TRUSTED_BOARD_BOOT && !BL2_AT_EL3
+	ARM_MAP_BL1_RW,
+#endif
+	{0}
+};
+#endif
+
+#if TRUSTED_BOARD_BOOT
+int plat_get_mbedtls_heap(void **heap_addr, size_t *heap_size)
+{
+	assert(heap_addr != NULL);
+	assert(heap_size != NULL);
+
+	return arm_get_mbedtls_heap(heap_addr, heap_size);
+}
+#endif
+
+void plat_arm_secure_wdt_start(void)
+{
+	sbsa_wdog_start(SBSA_SECURE_WDOG_BASE, SBSA_SECURE_WDOG_TIMEOUT);
+}
+
+void plat_arm_secure_wdt_stop(void)
+{
+	sbsa_wdog_stop(SBSA_SECURE_WDOG_BASE);
+}
diff --git a/plat/arm/board/morello/morello_trusted_boot.c b/plat/arm/board/morello/morello_trusted_boot.c
new file mode 100644
index 0000000..f9bc009
--- /dev/null
+++ b/plat/arm/board/morello/morello_trusted_boot.c
@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <plat/arm/common/plat_arm.h>
+
+/*
+ * Return the non-volatile counter value stored in the platform. The cookie
+ * will contain the OID of the counter in the certificate.
+ *
+ * Return: 0 = success, Otherwise = error
+ */
+int plat_get_nv_ctr(void *cookie, unsigned int *nv_ctr)
+{
+	*nv_ctr = MORELLO_FW_NVCTR_VAL;
+
+	return 0;
+}
+
+/*
+ * Store a new non-volatile counter value. By default on ARM development
+ * platforms, the non-volatile counters are RO and cannot be modified. We expect
+ * the values in the certificates to always match the RO values so that this
+ * function is never called.
+ *
+ * Return: 0 = success, Otherwise = error
+ */
+int plat_set_nv_ctr(void *cookie, unsigned int nv_ctr)
+{
+	return 1;
+}
+
+/*
+ * Return the ROTPK hash in the following ASN.1 structure in DER format:
+ *
+ * AlgorithmIdentifier  ::=  SEQUENCE  {
+ *     algorithm         OBJECT IDENTIFIER,
+ *     parameters        ANY DEFINED BY algorithm OPTIONAL
+ * }
+ *
+ * DigestInfo ::= SEQUENCE {
+ *     digestAlgorithm   AlgorithmIdentifier,
+ *     digest            OCTET STRING
+ * }
+ */
+int plat_get_rotpk_info(void *cookie, void **key_ptr, unsigned int *key_len,
+			unsigned int *flags)
+{
+	return arm_get_rotpk_info(cookie, key_ptr, key_len, flags);
+}
diff --git a/plat/arm/board/morello/platform.mk b/plat/arm/board/morello/platform.mk
index fc7d4d3..26a1911 100644
--- a/plat/arm/board/morello/platform.mk
+++ b/plat/arm/board/morello/platform.mk
@@ -1,9 +1,14 @@
 #
-# Copyright (c) 2020, Arm Limited. All rights reserved.
+# Copyright (c) 2020-2021, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+# Making sure the Morello platform type is specified
+ifeq ($(filter ${TARGET_PLATFORM}, fvp soc),)
+	$(error TARGET_PLATFORM must be fvp or soc)
+endif
+
 MORELLO_BASE		:=	plat/arm/board/morello
 
 INTERCONNECT_SOURCES	:=	${MORELLO_BASE}/morello_interconnect.c
@@ -25,6 +30,21 @@
 PLAT_BL_COMMON_SOURCES	:=	${MORELLO_BASE}/morello_plat.c		\
 				${MORELLO_BASE}/aarch64/morello_helper.S
 
+BL1_SOURCES		:=	${MORELLO_CPU_SOURCES}			\
+				${INTERCONNECT_SOURCES}			\
+				${MORELLO_BASE}/morello_err.c		\
+				${MORELLO_BASE}/morello_trusted_boot.c	\
+				${MORELLO_BASE}/morello_bl1_setup.c	\
+				drivers/arm/sbsa/sbsa.c
+
+BL2_SOURCES		:=	${MORELLO_BASE}/morello_security.c	\
+				${MORELLO_BASE}/morello_err.c		\
+				${MORELLO_BASE}/morello_trusted_boot.c	\
+				${MORELLO_BASE}/morello_bl2_setup.c	\
+				${MORELLO_BASE}/morello_image_load.c	\
+				lib/utils/mem_region.c			\
+				drivers/arm/css/sds/sds.c
+
 BL31_SOURCES		:=	${MORELLO_CPU_SOURCES}			\
 				${INTERCONNECT_SOURCES}			\
 				${MORELLO_GIC_SOURCES}			\
@@ -33,19 +53,31 @@
 				${MORELLO_BASE}/morello_security.c	\
 				drivers/arm/css/sds/sds.c
 
-FDT_SOURCES		+=	fdts/morello-fvp.dts
+FDT_SOURCES		+=	fdts/morello-${TARGET_PLATFORM}.dts		\
+				${MORELLO_BASE}/fdts/morello_fw_config.dts	\
+				${MORELLO_BASE}/fdts/morello_tb_fw_config.dts	\
+				${MORELLO_BASE}/fdts/morello_nt_fw_config.dts
+
+FW_CONFIG		:=	${BUILD_PLAT}/fdts/morello_fw_config.dtb
+TB_FW_CONFIG		:=	${BUILD_PLAT}/fdts/morello_tb_fw_config.dtb
+NT_FW_CONFIG		:=	${BUILD_PLAT}/fdts/morello_nt_fw_config.dtb
+
+# Add the FW_CONFIG to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${FW_CONFIG},--fw-config,${FW_CONFIG}))
+# Add the TB_FW_CONFIG to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${TB_FW_CONFIG},--tb-fw-config,${TB_FW_CONFIG}))
+# Add the NT_FW_CONFIG to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${NT_FW_CONFIG},--nt-fw-config,${NT_FW_CONFIG}))
+
+MORELLO_FW_NVCTR_VAL	:=	0
+TFW_NVCTR_VAL		:=	${MORELLO_FW_NVCTR_VAL}
+NTFW_NVCTR_VAL		:=	${MORELLO_FW_NVCTR_VAL}
 
 # TF-A not required to load the SCP Images
 override CSS_LOAD_SCP_IMAGES		:=	0
 
-# BL1/BL2 Image not a part of the capsule Image for morello
-override NEED_BL1			:=	no
-override NEED_BL2			:=	no
 override NEED_BL2U			:=	no
 
-#TF-A for morello starts from BL31
-override RESET_TO_BL31			:=	1
-
 # 32 bit mode not supported
 override CTX_INCLUDE_AARCH32_REGS	:=	0
 
@@ -62,6 +94,12 @@
 # use USE_COHERENT_MEM. Require that USE_COHERENT_MEM must be set to 0 too.
 USE_COHERENT_MEM			:=	0
 
+# Add TARGET_PLATFORM to differentiate between Morello FVP and Morello SoC platform
+$(eval $(call add_define,TARGET_PLATFORM_$(call uppercase,${TARGET_PLATFORM})))
+
+# Add MORELLO_FW_NVCTR_VAL
+$(eval $(call add_define,MORELLO_FW_NVCTR_VAL))
+
 include plat/arm/common/arm_common.mk
 include plat/arm/css/common/css_common.mk
 include plat/arm/board/common/board_common.mk
diff --git a/plat/arm/common/arm_common.mk b/plat/arm/common/arm_common.mk
index 78efb0f..064ed57 100644
--- a/plat/arm/common/arm_common.mk
+++ b/plat/arm/common/arm_common.mk
@@ -282,7 +282,7 @@
 ifeq (${JUNO_AARCH32_EL3_RUNTIME},1)
 BL2_SOURCES		+=	plat/arm/common/aarch32/arm_bl2_mem_params_desc.c
 else
-ifeq ($(filter ${TARGET_PLATFORM}, fpga fvp),)
+ifneq (${PLAT}, diphda)
 BL2_SOURCES		+=	plat/arm/common/${ARCH}/arm_bl2_mem_params_desc.c
 endif
 endif
@@ -389,6 +389,15 @@
 
     $(eval $(call TOOL_ADD_IMG,ns_bl2u,--fwu,FWU_))
 
+# Include Measured Boot makefile before any Crypto library makefile.
+# Crypto library makefile may need default definitions of Measured Boot build
+# flags present in Measured Boot makefile.
+ifeq (${MEASURED_BOOT},1)
+    MEASURED_BOOT_MK := drivers/measured_boot/event_log/event_log.mk
+    $(info Including ${MEASURED_BOOT_MK})
+    include ${MEASURED_BOOT_MK}
+endif
+
     # We expect to locate the *.mk files under the directories specified below
 ifeq (${ARM_CRYPTOCELL_INTEG},0)
     CRYPTO_LIB_MK := drivers/auth/mbedtls/mbedtls_crypto.mk
@@ -411,8 +420,3 @@
     endif
 endif
 
-ifeq (${MEASURED_BOOT},1)
-    MEASURED_BOOT_MK := drivers/measured_boot/event_log/event_log.mk
-    $(info Including ${MEASURED_BOOT_MK})
-    include ${MEASURED_BOOT_MK}
-endif
diff --git a/plat/mediatek/mt8186/aarch64/platform_common.c b/plat/mediatek/mt8186/aarch64/platform_common.c
index 745e547..9b9697f 100644
--- a/plat/mediatek/mt8186/aarch64/platform_common.c
+++ b/plat/mediatek/mt8186/aarch64/platform_common.c
@@ -13,8 +13,6 @@
 	/* for TF text, RO, RW */
 	MAP_REGION_FLAT(MTK_DEV_RNG0_BASE, MTK_DEV_RNG0_SIZE,
 			MT_DEVICE | MT_RW | MT_SECURE),
-	MAP_REGION_FLAT(MTK_DEV_RNG1_BASE, MTK_DEV_RNG1_SIZE,
-			MT_DEVICE | MT_RW | MT_SECURE),
 	MAP_REGION_FLAT(MTK_DEV_RNG2_BASE, MTK_DEV_RNG2_SIZE,
 			MT_DEVICE | MT_RW | MT_SECURE),
 	{ 0 }
diff --git a/plat/mediatek/mt8186/bl31_plat_setup.c b/plat/mediatek/mt8186/bl31_plat_setup.c
index 3fd1c71..b70245f 100644
--- a/plat/mediatek/mt8186/bl31_plat_setup.c
+++ b/plat/mediatek/mt8186/bl31_plat_setup.c
@@ -11,10 +11,16 @@
 #include <common/bl_common.h>
 #include <common/debug.h>
 #include <common/desc_image_load.h>
+#include <drivers/generic_delay_timer.h>
 #include <drivers/ti/uart/uart_16550.h>
 #include <lib/coreboot.h>
 
 /* Platform Includes */
+#include <emi_mpu.h>
+#include <mt_gic_v3.h>
+#include <mt_timer.h>
+#include <mtgpio.h>
+#include <mtk_dcm.h>
 #include <plat_params.h>
 #include <plat_private.h>
 
@@ -79,6 +85,17 @@
  ******************************************************************************/
 void bl31_platform_setup(void)
 {
+	dcm_set_default();
+
+	/* Initialize the GIC driver, CPU and distributor interfaces */
+	mt_gic_driver_init();
+	mt_gic_init();
+
+	mt_gpio_init();
+	mt_systimer_init();
+	generic_delay_timer_init();
+
+	emi_mpu_init();
 }
 
 /*******************************************************************************
diff --git a/plat/mediatek/mt8186/drivers/dcm/mtk_dcm.c b/plat/mediatek/mt8186/drivers/dcm/mtk_dcm.c
new file mode 100644
index 0000000..5dde5c5
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/dcm/mtk_dcm.c
@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <mtk_dcm.h>
+#include <mtk_dcm_utils.h>
+
+static void dcm_armcore(bool mode)
+{
+	dcm_mp_cpusys_top_bus_pll_div_dcm(mode);
+	dcm_mp_cpusys_top_cpu_pll_div_0_dcm(mode);
+	dcm_mp_cpusys_top_cpu_pll_div_1_dcm(mode);
+}
+
+static void dcm_mcusys(bool on)
+{
+	dcm_mp_cpusys_top_adb_dcm(on);
+	dcm_mp_cpusys_top_apb_dcm(on);
+	dcm_mp_cpusys_top_cpubiu_dcm(on);
+	dcm_mp_cpusys_top_cpubiu_dbg_cg(on);
+	dcm_mp_cpusys_top_misc_dcm(on);
+	dcm_mp_cpusys_top_mp0_qdcm(on);
+	dcm_cpccfg_reg_emi_wfifo(on);
+	dcm_mp_cpusys_top_last_cor_idle_dcm(on);
+}
+
+static void dcm_stall(bool on)
+{
+	dcm_mp_cpusys_top_core_stall_dcm(on);
+	dcm_mp_cpusys_top_fcm_stall_dcm(on);
+}
+
+static bool check_dcm_state(void)
+{
+	bool ret = true;
+
+	ret &= dcm_mp_cpusys_top_bus_pll_div_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_cpu_pll_div_0_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_cpu_pll_div_1_dcm_is_on();
+
+	ret &= dcm_mp_cpusys_top_adb_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_apb_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_cpubiu_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_cpubiu_dbg_cg_is_on();
+	ret &= dcm_mp_cpusys_top_misc_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_mp0_qdcm_is_on();
+	ret &= dcm_cpccfg_reg_emi_wfifo_is_on();
+	ret &= dcm_mp_cpusys_top_last_cor_idle_dcm_is_on();
+
+	ret &= dcm_mp_cpusys_top_core_stall_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_fcm_stall_dcm_is_on();
+
+	return ret;
+}
+
+void dcm_set_default(void)
+{
+	dcm_armcore(true);
+	dcm_mcusys(true);
+	dcm_stall(true);
+
+	INFO("%s: %d", __func__, check_dcm_state());
+}
diff --git a/plat/mediatek/mt8186/drivers/dcm/mtk_dcm.h b/plat/mediatek/mt8186/drivers/dcm/mtk_dcm.h
new file mode 100644
index 0000000..6abcff4
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/dcm/mtk_dcm.h
@@ -0,0 +1,14 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTK_DCM_H
+#define MTK_DCM_H
+
+#include <stdbool.h>
+
+void dcm_set_default(void);
+
+#endif /* #ifndef MTK_DCM_H */
diff --git a/plat/mediatek/mt8186/drivers/dcm/mtk_dcm_utils.c b/plat/mediatek/mt8186/drivers/dcm/mtk_dcm_utils.c
new file mode 100644
index 0000000..ae0e964
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/dcm/mtk_dcm_utils.c
@@ -0,0 +1,490 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <mtk_dcm_utils.h>
+
+#define MP_CPUSYS_TOP_ADB_DCM_REG0_MASK (BIT(16) |	\
+					 BIT(17) |	\
+					 BIT(18) |	\
+					 BIT(21))
+#define MP_CPUSYS_TOP_ADB_DCM_REG1_MASK (BIT(16) |	\
+					 BIT(17) |	\
+					 BIT(18))
+#define MP_CPUSYS_TOP_ADB_DCM_REG0_ON (BIT(16) |	\
+				       BIT(17) |	\
+				       BIT(18) |	\
+				       BIT(21))
+#define MP_CPUSYS_TOP_ADB_DCM_REG1_ON (BIT(16) |	\
+				       BIT(17) |	\
+				       BIT(18))
+#define MP_CPUSYS_TOP_ADB_DCM_REG0_OFF ((0x0 << 16) |	\
+					(0x0 << 17) |	\
+					(0x0 << 18) |	\
+					(0x0 << 21))
+#define MP_CPUSYS_TOP_ADB_DCM_REG1_OFF ((0x0 << 16) |	\
+					(0x0 << 17) |	\
+					(0x0 << 18))
+
+bool dcm_mp_cpusys_top_adb_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP_ADB_DCM_CFG4) &
+		MP_CPUSYS_TOP_ADB_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_ADB_DCM_REG0_ON);
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0) &
+		MP_CPUSYS_TOP_ADB_DCM_REG1_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_ADB_DCM_REG1_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_adb_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_adb_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_ADB_DCM_CFG4,
+			MP_CPUSYS_TOP_ADB_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_ADB_DCM_REG0_ON);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+			MP_CPUSYS_TOP_ADB_DCM_REG1_MASK,
+			MP_CPUSYS_TOP_ADB_DCM_REG1_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_adb_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_ADB_DCM_CFG4,
+			MP_CPUSYS_TOP_ADB_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_ADB_DCM_REG0_OFF);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+			MP_CPUSYS_TOP_ADB_DCM_REG1_MASK,
+			MP_CPUSYS_TOP_ADB_DCM_REG1_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_APB_DCM_REG0_MASK (BIT(5))
+#define MP_CPUSYS_TOP_APB_DCM_REG1_MASK (BIT(8))
+#define MP_CPUSYS_TOP_APB_DCM_REG2_MASK (BIT(16))
+#define MP_CPUSYS_TOP_APB_DCM_REG0_ON (BIT(5))
+#define MP_CPUSYS_TOP_APB_DCM_REG1_ON (BIT(8))
+#define MP_CPUSYS_TOP_APB_DCM_REG2_ON (BIT(16))
+#define MP_CPUSYS_TOP_APB_DCM_REG0_OFF ((0x0 << 5))
+#define MP_CPUSYS_TOP_APB_DCM_REG1_OFF ((0x0 << 8))
+#define MP_CPUSYS_TOP_APB_DCM_REG2_OFF ((0x0 << 16))
+
+bool dcm_mp_cpusys_top_apb_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0) &
+		MP_CPUSYS_TOP_APB_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_APB_DCM_REG0_ON);
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0) &
+		MP_CPUSYS_TOP_APB_DCM_REG1_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_APB_DCM_REG1_ON);
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP0_DCM_CFG0) &
+		MP_CPUSYS_TOP_APB_DCM_REG2_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_APB_DCM_REG2_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_apb_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_apb_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG0_ON);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG1_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG1_ON);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG2_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG2_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_apb_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG0_OFF);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG1_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG1_OFF);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+			MP_CPUSYS_TOP_APB_DCM_REG2_MASK,
+			MP_CPUSYS_TOP_APB_DCM_REG2_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_MASK (BIT(11) |	\
+						 BIT(24) |	\
+						 BIT(25))
+#define MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_ON (BIT(11) |	\
+					       BIT(24) |	\
+					       BIT(25))
+#define MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_OFF ((0x0 << 11) |	\
+						(0x0 << 24) |	\
+						(0x0 << 25))
+
+bool dcm_mp_cpusys_top_bus_pll_div_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG) &
+		MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_bus_pll_div_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_bus_pll_div_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+			MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_bus_pll_div_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+			MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_MASK (BIT(0))
+#define MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_ON (BIT(0))
+#define MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_OFF ((0x0 << 0))
+
+bool dcm_mp_cpusys_top_core_stall_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP0_DCM_CFG7) &
+		MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_core_stall_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_core_stall_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+			MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_core_stall_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+			MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_MASK (BIT(0))
+#define MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_ON ((0x0 << 0))
+#define MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_OFF (BIT(0))
+
+bool dcm_mp_cpusys_top_cpubiu_dbg_cg_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MCSI_CFG2) &
+		MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_cpubiu_dbg_cg(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_cpubiu_dbg_cg'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCSI_CFG2,
+			MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_MASK,
+			MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_cpubiu_dbg_cg'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCSI_CFG2,
+			MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_MASK,
+			MP_CPUSYS_TOP_CPUBIU_DBG_CG_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CPUBIU_DCM_REG0_MASK ((0xffff << 0))
+#define MP_CPUSYS_TOP_CPUBIU_DCM_REG0_ON ((0xffff << 0))
+#define MP_CPUSYS_TOP_CPUBIU_DCM_REG0_OFF ((0x0 << 0))
+
+bool dcm_mp_cpusys_top_cpubiu_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MCSIC_DCM0) &
+		MP_CPUSYS_TOP_CPUBIU_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_CPUBIU_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_cpubiu_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_cpubiu_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCSIC_DCM0,
+			MP_CPUSYS_TOP_CPUBIU_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPUBIU_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_cpubiu_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCSIC_DCM0,
+			MP_CPUSYS_TOP_CPUBIU_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPUBIU_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_MASK (BIT(11) |	\
+						   BIT(24) |	\
+						   BIT(25))
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_ON (BIT(11) |	\
+						 BIT(24) |	\
+						 BIT(25))
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_OFF ((0x0 << 11) |	\
+						  (0x0 << 24) |	\
+						  (0x0 << 25))
+
+bool dcm_mp_cpusys_top_cpu_pll_div_0_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG0) &
+		MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_cpu_pll_div_0_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_cpu_pll_div_0_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG0,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_cpu_pll_div_0_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG0,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_MASK (BIT(11) |	\
+						   BIT(24) |	\
+						   BIT(25))
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_ON (BIT(11) |	\
+						 BIT(24) |	\
+						 BIT(25))
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_OFF ((0x0 << 11) |	\
+						  (0x0 << 24) |	\
+						  (0x0 << 25))
+
+bool dcm_mp_cpusys_top_cpu_pll_div_1_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG1) &
+		MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_cpu_pll_div_1_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_cpu_pll_div_1_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG1,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_cpu_pll_div_1_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG1,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_MASK (BIT(4))
+#define MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_ON (BIT(4))
+#define MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_OFF ((0x0 << 4))
+
+bool dcm_mp_cpusys_top_fcm_stall_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP0_DCM_CFG7) &
+		MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_fcm_stall_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_fcm_stall_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+			MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_fcm_stall_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+			MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_MASK (BIT(31))
+#define MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_ON (BIT(31))
+#define MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_OFF ((0x0 << 31))
+
+bool dcm_mp_cpusys_top_last_cor_idle_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG) &
+		MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_last_cor_idle_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_last_cor_idle_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+			MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_last_cor_idle_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+			MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_MISC_DCM_REG0_MASK (BIT(0) |	\
+					  BIT(1) |	\
+					  BIT(2) |	\
+					  BIT(3) |	\
+					  BIT(4))
+#define MP_CPUSYS_TOP_MISC_DCM_REG0_ON (BIT(0) |	\
+					BIT(1) |	\
+					BIT(2) |	\
+					BIT(3) |	\
+					BIT(4))
+#define MP_CPUSYS_TOP_MISC_DCM_REG0_OFF ((0x0 << 0) |	\
+					 (0x0 << 1) |	\
+					 (0x0 << 2) |	\
+					 (0x0 << 3) |	\
+					 (0x0 << 4))
+
+bool dcm_mp_cpusys_top_misc_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0) &
+		MP_CPUSYS_TOP_MISC_DCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_MISC_DCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_misc_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_misc_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+			MP_CPUSYS_TOP_MISC_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_MISC_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_misc_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+			MP_CPUSYS_TOP_MISC_DCM_REG0_MASK,
+			MP_CPUSYS_TOP_MISC_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_MP0_QDCM_REG0_MASK (BIT(0) |	\
+			BIT(1) |			\
+			BIT(2) |			\
+			BIT(3))
+#define MP_CPUSYS_TOP_MP0_QDCM_REG0_ON (BIT(0) |	\
+			BIT(1) |			\
+			BIT(2) |			\
+			BIT(3))
+#define MP_CPUSYS_TOP_MP0_QDCM_REG0_OFF ((0x0 << 0) |	\
+			(0x0 << 1) |			\
+			(0x0 << 2) |			\
+			(0x0 << 3))
+
+bool dcm_mp_cpusys_top_mp0_qdcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(MP_CPUSYS_TOP_MP0_DCM_CFG0) &
+		MP_CPUSYS_TOP_MP0_QDCM_REG0_MASK) ==
+		(unsigned int) MP_CPUSYS_TOP_MP0_QDCM_REG0_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_mp0_qdcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_mp0_qdcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+			MP_CPUSYS_TOP_MP0_QDCM_REG0_MASK,
+			MP_CPUSYS_TOP_MP0_QDCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_mp0_qdcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+			MP_CPUSYS_TOP_MP0_QDCM_REG0_MASK,
+			MP_CPUSYS_TOP_MP0_QDCM_REG0_OFF);
+	}
+}
+
+#define CPCCFG_REG_EMI_WFIFO_REG0_MASK (BIT(0) | BIT(2))
+#define CPCCFG_REG_EMI_WFIFO_REG0_ON (BIT(0) | BIT(2))
+#define CPCCFG_REG_EMI_WFIFO_REG0_OFF ((0x0 << 0) | (0x0 << 2))
+
+bool dcm_cpccfg_reg_emi_wfifo_is_on(void)
+{
+	bool ret = true;
+
+	ret &= ((mmio_read_32(CPCCFG_REG_EMI_WFIFO) &
+		CPCCFG_REG_EMI_WFIFO_REG0_MASK) ==
+		(unsigned int) CPCCFG_REG_EMI_WFIFO_REG0_ON);
+
+	return ret;
+}
+
+void dcm_cpccfg_reg_emi_wfifo(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'cpccfg_reg_emi_wfifo'" */
+		mmio_clrsetbits_32(CPCCFG_REG_EMI_WFIFO,
+			CPCCFG_REG_EMI_WFIFO_REG0_MASK,
+			CPCCFG_REG_EMI_WFIFO_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'cpccfg_reg_emi_wfifo'" */
+		mmio_clrsetbits_32(CPCCFG_REG_EMI_WFIFO,
+			CPCCFG_REG_EMI_WFIFO_REG0_MASK,
+			CPCCFG_REG_EMI_WFIFO_REG0_OFF);
+	}
+}
+
diff --git a/plat/mediatek/mt8186/drivers/dcm/mtk_dcm_utils.h b/plat/mediatek/mt8186/drivers/dcm/mtk_dcm_utils.h
new file mode 100644
index 0000000..ba76594
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/dcm/mtk_dcm_utils.h
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTK_DCM_UTILS_H
+#define MTK_DCM_UTILS_H
+
+#include <stdbool.h>
+
+#include <mtk_dcm.h>
+#include <platform_def.h>
+
+/* Base */
+#define MP_CPUSYS_TOP_BASE	0xc538000
+#define CPCCFG_REG_BASE		0xc53a800
+
+/* Register Definition */
+#define CPCCFG_REG_EMI_WFIFO		(CPCCFG_REG_BASE + 0x100)
+#define MP_CPUSYS_TOP_BUS_PLLDIV_CFG	(MP_CPUSYS_TOP_BASE + 0x22e0)
+#define MP_CPUSYS_TOP_CPU_PLLDIV_CFG0	(MP_CPUSYS_TOP_BASE + 0x22a0)
+#define MP_CPUSYS_TOP_CPU_PLLDIV_CFG1	(MP_CPUSYS_TOP_BASE + 0x22a4)
+#define MP_CPUSYS_TOP_MCSIC_DCM0	(MP_CPUSYS_TOP_BASE + 0x2440)
+#define MP_CPUSYS_TOP_MCSI_CFG2		(MP_CPUSYS_TOP_BASE + 0x2418)
+#define MP_CPUSYS_TOP_MCUSYS_DCM_CFG0	(MP_CPUSYS_TOP_BASE + 0x25c0)
+#define MP_CPUSYS_TOP_MP0_DCM_CFG0	(MP_CPUSYS_TOP_BASE + 0x4880)
+#define MP_CPUSYS_TOP_MP0_DCM_CFG7	(MP_CPUSYS_TOP_BASE + 0x489c)
+#define MP_CPUSYS_TOP_MP_ADB_DCM_CFG4	(MP_CPUSYS_TOP_BASE + 0x2510)
+#define MP_CPUSYS_TOP_MP_MISC_DCM_CFG0	(MP_CPUSYS_TOP_BASE + 0x2518)
+
+bool dcm_mp_cpusys_top_adb_dcm_is_on(void);
+void dcm_mp_cpusys_top_adb_dcm(bool on);
+bool dcm_mp_cpusys_top_apb_dcm_is_on(void);
+void dcm_mp_cpusys_top_apb_dcm(bool on);
+bool dcm_mp_cpusys_top_bus_pll_div_dcm_is_on(void);
+void dcm_mp_cpusys_top_bus_pll_div_dcm(bool on);
+bool dcm_mp_cpusys_top_core_stall_dcm_is_on(void);
+void dcm_mp_cpusys_top_core_stall_dcm(bool on);
+bool dcm_mp_cpusys_top_cpubiu_dbg_cg_is_on(void);
+void dcm_mp_cpusys_top_cpubiu_dbg_cg(bool on);
+bool dcm_mp_cpusys_top_cpubiu_dcm_is_on(void);
+void dcm_mp_cpusys_top_cpubiu_dcm(bool on);
+bool dcm_mp_cpusys_top_cpu_pll_div_0_dcm_is_on(void);
+void dcm_mp_cpusys_top_cpu_pll_div_0_dcm(bool on);
+bool dcm_mp_cpusys_top_cpu_pll_div_1_dcm_is_on(void);
+void dcm_mp_cpusys_top_cpu_pll_div_1_dcm(bool on);
+bool dcm_mp_cpusys_top_fcm_stall_dcm_is_on(void);
+void dcm_mp_cpusys_top_fcm_stall_dcm(bool on);
+bool dcm_mp_cpusys_top_last_cor_idle_dcm_is_on(void);
+void dcm_mp_cpusys_top_last_cor_idle_dcm(bool on);
+bool dcm_mp_cpusys_top_misc_dcm_is_on(void);
+void dcm_mp_cpusys_top_misc_dcm(bool on);
+bool dcm_mp_cpusys_top_mp0_qdcm_is_on(void);
+void dcm_mp_cpusys_top_mp0_qdcm(bool on);
+bool dcm_cpccfg_reg_emi_wfifo_is_on(void);
+void dcm_cpccfg_reg_emi_wfifo(bool on);
+#endif
diff --git a/plat/mediatek/mt8186/drivers/emi_mpu/emi_mpu.c b/plat/mediatek/mt8186/drivers/emi_mpu/emi_mpu.c
new file mode 100644
index 0000000..989ecf1
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/emi_mpu/emi_mpu.c
@@ -0,0 +1,100 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <emi_mpu.h>
+
+#if ENABLE_EMI_MPU_SW_LOCK
+static unsigned char region_lock_state[EMI_MPU_REGION_NUM];
+#endif
+
+#define EMI_MPU_START_MASK		(0x00FFFFFF)
+#define EMI_MPU_END_MASK		(0x00FFFFFF)
+#define EMI_MPU_APC_SW_LOCK_MASK	(0x00FFFFFF)
+#define EMI_MPU_APC_HW_LOCK_MASK	(0x80FFFFFF)
+
+static int _emi_mpu_set_protection(unsigned int start, unsigned int end,
+				   unsigned int apc)
+{
+	unsigned int dgroup;
+	unsigned int region;
+
+	region = (start >> 24) & 0xFF;
+	start &= EMI_MPU_START_MASK;
+	dgroup = (end >> 24) & 0xFF;
+	end &= EMI_MPU_END_MASK;
+
+	if  ((region >= EMI_MPU_REGION_NUM) || (dgroup > EMI_MPU_DGROUP_NUM)) {
+		WARN("invalid region, domain\n");
+		return -1;
+	}
+
+#if ENABLE_EMI_MPU_SW_LOCK
+	if (region_lock_state[region] == 1) {
+		WARN("invalid region\n");
+		return -1;
+	}
+
+	if ((dgroup == 0U) && ((apc >> 31) & 0x1)) {
+		region_lock_state[region] = 1;
+	}
+
+	apc &= EMI_MPU_APC_SW_LOCK_MASK;
+#else
+	apc &= EMI_MPU_APC_HW_LOCK_MASK;
+#endif
+
+	if ((start >= DRAM_OFFSET) && (end >= start)) {
+		start -= DRAM_OFFSET;
+		end -= DRAM_OFFSET;
+	} else {
+		WARN("invalid range\n");
+		return -1;
+	}
+
+	mmio_write_32(EMI_MPU_SA(region), start);
+	mmio_write_32(EMI_MPU_EA(region), end);
+	mmio_write_32(EMI_MPU_APC(region, dgroup), apc);
+
+#if defined(SUB_EMI_MPU_BASE)
+	mmio_write_32(SUB_EMI_MPU_SA(region), start);
+	mmio_write_32(SUB_EMI_MPU_EA(region), end);
+	mmio_write_32(SUB_EMI_MPU_APC(region, dgroup), apc);
+#endif
+	return 0;
+}
+
+int emi_mpu_set_protection(struct emi_region_info_t *region_info)
+{
+	unsigned int start, end;
+	int i;
+
+	if (region_info->region >= EMI_MPU_REGION_NUM) {
+		WARN("invalid region\n");
+		return -1;
+	}
+
+	start = (unsigned int)(region_info->start >> EMI_MPU_ALIGN_BITS) |
+		(region_info->region << 24);
+
+	for (i = EMI_MPU_DGROUP_NUM - 1; i >= 0; i--) {
+		end = (unsigned int)(region_info->end >> EMI_MPU_ALIGN_BITS) | (i << 24);
+
+		if (_emi_mpu_set_protection(start, end, region_info->apc[i]) < 0) {
+			WARN("failed to set emi mpu protection(%d, %d, %d)\n",
+			     start, end, region_info->apc[i]);
+		}
+	}
+
+	return 0;
+}
+
+void emi_mpu_init(void)
+{
+	/* TODO: more setting for EMI MPU. */
+}
diff --git a/plat/mediatek/mt8186/drivers/emi_mpu/emi_mpu.h b/plat/mediatek/mt8186/drivers/emi_mpu/emi_mpu.h
new file mode 100644
index 0000000..415146e
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/emi_mpu/emi_mpu.h
@@ -0,0 +1,98 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef EMI_MPU_H
+#define EMI_MPU_H
+
+#include <platform_def.h>
+
+#define ENABLE_EMI_MPU_SW_LOCK		1
+
+#define EMI_MPU_CTRL			(EMI_MPU_BASE + 0x000)
+#define EMI_MPU_DBG			(EMI_MPU_BASE + 0x004)
+#define EMI_MPU_SA0			(EMI_MPU_BASE + 0x100)
+#define EMI_MPU_EA0			(EMI_MPU_BASE + 0x200)
+#define EMI_MPU_SA(region)		(EMI_MPU_SA0 + (region * 4))
+#define EMI_MPU_EA(region)		(EMI_MPU_EA0 + (region * 4))
+#define EMI_MPU_APC0			(EMI_MPU_BASE + 0x300)
+#define EMI_MPU_APC(region, dgroup)	(EMI_MPU_APC0 + (region * 4) + (dgroup * 0x100))
+#define EMI_MPU_CTRL_D0			(EMI_MPU_BASE + 0x800)
+#define EMI_MPU_CTRL_D(domain)		(EMI_MPU_CTRL_D0 + (domain * 4))
+#define EMI_RG_MASK_D0			(EMI_MPU_BASE + 0x900)
+#define EMI_RG_MASK_D(domain)		(EMI_RG_MASK_D0 + (domain * 4))
+#define EMI_MPU_START			(0x000)
+#define EMI_MPU_END			(0x93C)
+
+#define SUB_EMI_MPU_CTRL		(SUB_EMI_MPU_BASE + 0x000)
+#define SUB_EMI_MPU_DBG			(SUB_EMI_MPU_BASE + 0x004)
+#define SUB_EMI_MPU_SA0			(SUB_EMI_MPU_BASE + 0x100)
+#define SUB_EMI_MPU_EA0			(SUB_EMI_MPU_BASE + 0x200)
+#define SUB_EMI_MPU_SA(region)		(SUB_EMI_MPU_SA0 + (region * 4))
+#define SUB_EMI_MPU_EA(region)		(SUB_EMI_MPU_EA0 + (region * 4))
+#define SUB_EMI_MPU_APC0		(SUB_EMI_MPU_BASE + 0x300)
+#define SUB_EMI_MPU_APC(region, dgroup)	(SUB_EMI_MPU_APC0 + (region * 4) + (dgroup * 0x100))
+#define SUB_EMI_MPU_CTRL_D0		(SUB_EMI_MPU_BASE + 0x800)
+#define SUB_EMI_MPU_CTRL_D(domain)	(SUB_EMI_MPU_CTRL_D0 + (domain * 4))
+#define SUB_EMI_RG_MASK_D0		(SUB_EMI_MPU_BASE + 0x900)
+#define SUB_EMI_RG_MASK_D(domain)	(SUB_EMI_RG_MASK_D0 + (domain * 4))
+
+#define EMI_MPU_DOMAIN_NUM		(16)
+#define EMI_MPU_REGION_NUM		(32)
+#define EMI_MPU_ALIGN_BITS		(16)
+#define DRAM_OFFSET			(0x40000000 >> EMI_MPU_ALIGN_BITS)
+
+#define NO_PROTECTION			0
+#define SEC_RW				1
+#define SEC_RW_NSEC_R			2
+#define SEC_RW_NSEC_W			3
+#define SEC_R_NSEC_R			4
+#define FORBIDDEN			5
+#define SEC_R_NSEC_RW			6
+
+#define LOCK				1
+#define UNLOCK				0
+
+#define EMI_MPU_DGROUP_NUM		(EMI_MPU_DOMAIN_NUM / 8)
+
+#if (EMI_MPU_DGROUP_NUM == 1)
+#define SET_ACCESS_PERMISSION(apc_ary, lock, d7, d6, d5, d4, d3, d2, d1, d0) \
+do { \
+	apc_ary[1] = 0; \
+	apc_ary[0] = \
+		(((unsigned int)  d7) << 21) | (((unsigned int)  d6) << 18) | \
+		(((unsigned int)  d5) << 15) | (((unsigned int)  d4) << 12) | \
+		(((unsigned int)  d3) <<  9) | (((unsigned int)  d2) <<  6) | \
+		(((unsigned int)  d1) <<  3) |  ((unsigned int)  d0) | \
+		((unsigned int) lock << 31); \
+} while (0)
+#elif (EMI_MPU_DGROUP_NUM == 2)
+#define SET_ACCESS_PERMISSION(apc_ary, lock, d15, d14, d13, d12, d11, d10, \
+				d9, d8, d7, d6, d5, d4, d3, d2, d1, d0) \
+do { \
+	apc_ary[1] = \
+		(((unsigned int) d15) << 21) | (((unsigned int) d14) << 18) | \
+		(((unsigned int) d13) << 15) | (((unsigned int) d12) << 12) | \
+		(((unsigned int) d11) <<  9) | (((unsigned int) d10) <<  6) | \
+		(((unsigned int)  d9) <<  3) |  ((unsigned int)  d8); \
+	apc_ary[0] = \
+		(((unsigned int)  d7) << 21) | (((unsigned int)  d6) << 18) | \
+		(((unsigned int)  d5) << 15) | (((unsigned int)  d4) << 12) | \
+		(((unsigned int)  d3) <<  9) | (((unsigned int)  d2) <<  6) | \
+		(((unsigned int)  d1) <<  3) |  ((unsigned int)  d0) | \
+		((unsigned int) lock << 31); \
+} while (0)
+#endif
+
+struct emi_region_info_t {
+	unsigned long long start;
+	unsigned long long end;
+	unsigned int region;
+	unsigned int apc[EMI_MPU_DGROUP_NUM];
+};
+
+void emi_mpu_init(void);
+
+#endif
diff --git a/plat/mediatek/mt8186/drivers/gpio/mtgpio.c b/plat/mediatek/mt8186/drivers/gpio/mtgpio.c
new file mode 100644
index 0000000..134476a
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/gpio/mtgpio.c
@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <mtgpio.h>
+#include <platform_def.h>
+
+uintptr_t mt_gpio_find_reg_addr(uint32_t pin)
+{
+	uintptr_t reg_addr = 0U;
+	struct mt_pin_info gpio_info;
+
+	assert(pin < MAX_GPIO_PIN);
+
+	gpio_info = mt_pin_infos[pin];
+
+	switch (gpio_info.base & 0x0f) {
+	case 0:
+		reg_addr = IOCFG_LT_BASE;
+		break;
+	case 1:
+		reg_addr = IOCFG_LM_BASE;
+		break;
+	case 2:
+		reg_addr = IOCFG_LB_BASE;
+		break;
+	case 3:
+		reg_addr = IOCFG_BL_BASE;
+		break;
+	case 4:
+		reg_addr = IOCFG_RB_BASE;
+		break;
+	case 5:
+		reg_addr = IOCFG_RT_BASE;
+		break;
+	default:
+		break;
+	}
+
+	return reg_addr;
+}
diff --git a/plat/mediatek/mt8186/drivers/gpio/mtgpio.h b/plat/mediatek/mt8186/drivers/gpio/mtgpio.h
new file mode 100644
index 0000000..4430a58
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/gpio/mtgpio.h
@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MT_GPIO_H
+#define MT_GPIO_H
+
+#include <mtgpio_common.h>
+
+/* Enumeration for GPIO pin */
+typedef enum GPIO_PIN {
+	GPIO_UNSUPPORTED = -1,
+	GPIO0, GPIO1, GPIO2, GPIO3, GPIO4, GPIO5, GPIO6,
+	GPIO7, GPIO8, GPIO9, GPIO10, GPIO11, GPIO12, GPIO13, GPIO14,
+	GPIO15, GPIO16, GPIO17, GPIO18, GPIO19, GPIO20, GPIO21, GPIO22,
+	GPIO23, GPIO24, GPIO25, GPIO26, GPIO27, GPIO28, GPIO29, GPIO30,
+	GPIO31, GPIO32, GPIO33, GPIO34, GPIO35, GPIO36, GPIO37, GPIO38,
+	GPIO39, GPIO40, GPIO41, GPIO42, GPIO43, GPIO44, GPIO45, GPIO46,
+	GPIO47, GPIO48, GPIO49, GPIO50, GPIO51, GPIO52, GPIO53, GPIO54,
+	GPIO55, GPIO56, GPIO57, GPIO58, GPIO59, GPIO60, GPIO61, GPIO62,
+	GPIO63, GPIO64, GPIO65, GPIO66, GPIO67, GPIO68, GPIO69, GPIO70,
+	GPIO71, GPIO72, GPIO73, GPIO74, GPIO75, GPIO76, GPIO77, GPIO78,
+	GPIO79, GPIO80, GPIO81, GPIO82, GPIO83, GPIO84, GPIO85, GPIO86,
+	GPIO87, GPIO88, GPIO89, GPIO90, GPIO91, GPIO92, GPIO93, GPIO94,
+	GPIO95, GPIO96, GPIO97, GPIO98, GPIO99, GPIO100, GPIO101, GPIO102,
+	GPIO103, GPIO104, GPIO105, GPIO106, GPIO107, GPIO108, GPIO109, GPIO110,
+	GPIO111, GPIO112, GPIO113, GPIO114, GPIO115, GPIO116, GPIO117, GPIO118,
+	GPIO119, GPIO120, GPIO121, GPIO122, GPIO123, GPIO124, GPIO125, GPIO126,
+	GPIO127, GPIO128, GPIO129, GPIO130, GPIO131, GPIO132, GPIO133, GPIO134,
+	GPIO135, GPIO136, GPIO137, GPIO138, GPIO139, GPIO140, GPIO141, GPIO142,
+	GPIO143, GPIO144, GPIO145, GPIO146, GPIO147, GPIO148, GPIO149, GPIO150,
+	GPIO151, GPIO152, GPIO153, GPIO154, GPIO155, GPIO156, GPIO157, GPIO158,
+	GPIO159, GPIO160, GPIO161, GPIO162, GPIO163, GPIO164, GPIO165, GPIO166,
+	GPIO167, GPIO168, GPIO169, GPIO170, GPIO171, GPIO172, GPIO173, GPIO174,
+	GPIO175, GPIO176, GPIO177, GPIO178, GPIO179, GPIO180, GPIO181, GPIO182,
+	GPIO183, GPIO184,
+	MT_GPIO_BASE_MAX
+} GPIO_PIN;
+
+static const struct mt_pin_info mt_pin_infos[] = {
+	PIN(0, 0, 13, 0x16, 0x40),
+	PIN(1, 0, 14, 0x16, 0x40),
+	PIN(2, 0, 17, 0x16, 0x40),
+	PIN(3, 0, 18, 0x16, 0x40),
+	PIN(4, 0, 19, 0x16, 0x40),
+	PIN(5, 0, 20, 0x16, 0x40),
+	PIN(6, 0, 19, 0x24, 0x40),
+	PIN(7, 0, 20, 0x24, 0x40),
+	PIN(8, 0, 21, 0x24, 0x40),
+	PIN(9, 0, 22, 0x24, 0x40),
+	PIN(10, 0, 16, 0x24, 0x40),
+	PIN(11, 0, 17, 0x24, 0x40),
+	PIN(12, 0, 18, 0x24, 0x40),
+	PIN(13, 0, 0, 0x23, 0x60),
+	PIN(14, 0, 1, 0x23, 0x60),
+	PIN(15, 0, 15, 0x16, 0x40),
+	PIN(16, 0, 16, 0x16, 0x40),
+	PIN(17, 0, 9, 0x25, 0x70),
+	PIN(18, 0, 10, 0x25, 0x70),
+	PIN(19, 0, 3, 0x25, 0x70),
+	PIN(20, 0, 6, 0x25, 0x70),
+	PIN(21, 0, 4, 0x25, 0x70),
+	PIN(22, 0, 7, 0x25, 0x70),
+	PIN(23, 0, 5, 0x25, 0x70),
+	PIN(24, 0, 8, 0x25, 0x70),
+	PIN(25, 0, 18, 0x25, 0x70),
+	PIN(26, 0, 15, 0x25, 0x70),
+	PIN(27, 0, 17, 0x25, 0x70),
+	PIN(28, 0, 16, 0x25, 0x70),
+	PIN(29, 0, 0, 0x16, 0x40),
+	PIN(30, 0, 1, 0x16, 0x40),
+	PIN(31, 0, 2, 0x16, 0x40),
+	PIN(32, 0, 25, 0x12, 0x50),
+	PIN(33, 0, 27, 0x12, 0x50),
+	PIN(34, 0, 26, 0x12, 0x50),
+	PIN(35, 0, 28, 0x12, 0x50),
+	PIN(36, 0, 9, 0x12, 0x50),
+	PIN(37, 0, 10, 0x12, 0x50),
+	PIN(38, 0, 12, 0x12, 0x50),
+	PIN(39, 0, 11, 0x12, 0x50),
+	PIN(40, 0, 13, 0x12, 0x50),
+	PIN(41, 0, 14, 0x12, 0x50),
+	PIN(42, 0, 16, 0x12, 0x50),
+	PIN(43, 0, 15, 0x12, 0x50),
+	PIN(44, 0, 28, 0x25, 0x70),
+	PIN(45, 0, 29, 0x25, 0x70),
+	PIN(46, 0, 31, 0x25, 0x70),
+	PIN(47, 0, 30, 0x25, 0x70),
+	PIN(48, 0, 17, 0x12, 0x50),
+	PIN(49, 0, 18, 0x12, 0x50),
+	PIN(50, 0, 20, 0x12, 0x50),
+	PIN(51, 0, 19, 0x12, 0x50),
+	PIN(52, 0, 12, 0x23, 0x60),
+	PIN(53, 0, 13, 0x23, 0x60),
+	PIN(54, 0, 15, 0x23, 0x60),
+	PIN(55, 0, 14, 0x23, 0x60),
+	PIN(56, 0, 12, 0x25, 0x70),
+	PIN(57, 0, 11, 0x25, 0x70),
+	PIN(58, 0, 13, 0x25, 0x70),
+	PIN(59, 0, 14, 0x25, 0x70),
+	PIN(60, 0, 21, 0x23, 0x60),
+	PIN(61, 0, 16, 0x23, 0x60),
+	PIN(62, 0, 22, 0x23, 0x60),
+	PIN(63, 0, 17, 0x23, 0x60),
+	PIN(64, 0, 18, 0x23, 0x60),
+	PIN(65, 0, 19, 0x23, 0x60),
+	PIN(66, 0, 20, 0x23, 0x60),
+	PIN(67, 1, 10, 0x21, 0x70),
+	PIN(68, 1, 0, 0x21, 0x70),
+	PIN(69, 1, 1, 0x21, 0x70),
+	PIN(70, 1, 11, 0x21, 0x70),
+	PIN(71, 1, 2, 0x21, 0x70),
+	PIN(72, 1, 3, 0x21, 0x70),
+	PIN(73, 1, 4, 0x21, 0x70),
+	PIN(74, 1, 5, 0x21, 0x70),
+	PIN(75, 1, 6, 0x21, 0x70),
+	PIN(76, 1, 7, 0x21, 0x70),
+	PIN(77, 1, 8, 0x21, 0x70),
+	PIN(78, 1, 9, 0x21, 0x70),
+	PIN(79, 1, 0, 0x25, 0x80),
+	PIN(80, 1, 1, 0x25, 0x80),
+	PIN(81, 1, 2, 0x25, 0x80),
+	PIN(82, 1, 3, 0x25, 0x80),
+	PIN(83, 0, 3, 0x16, 0x40),
+	PIN(84, 1, 0, 0x23, 0x70),
+	PIN(85, 1, 1, 0x23, 0x70),
+	PIN(86, 1, 2, 0x23, 0x70),
+	PIN(87, 1, 3, 0x23, 0x70),
+	PIN(88, 1, 4, 0x23, 0x70),
+	PIN(89, 1, 5, 0x23, 0x70),
+	PIN(90, 0, 2, 0x23, 0x60),
+	PIN(91, 0, 23, 0x23, 0x60),
+	PIN(92, 0, 25, 0x23, 0x60),
+	PIN(93, 0, 3, 0x23, 0x60),
+	PIN(94, 0, 24, 0x23, 0x60),
+	PIN(95, 0, 26, 0x23, 0x60),
+	PIN(96, 0, 1, 0x12, 0x50),
+	PIN(97, 0, 0, 0x12, 0x50),
+	PIN(98, 0, 2, 0x12, 0x50),
+	PIN(99, 0, 14, 0x24, 0x40),
+	PIN(100, 0, 15, 0x24, 0x40),
+	PIN(101, 0, 13, 0x24, 0x40),
+	PIN(102, 0, 12, 0x24, 0x40),
+	PIN(103, 0, 0, 0x24, 0x40),
+	PIN(104, 0, 1, 0x24, 0x40),
+	PIN(105, 0, 4, 0x24, 0x40),
+	PIN(106, 0, 5, 0x24, 0x40),
+	PIN(107, 0, 6, 0x24, 0x40),
+	PIN(108, 0, 7, 0x24, 0x40),
+	PIN(109, 0, 8, 0x24, 0x40),
+	PIN(110, 0, 9, 0x24, 0x40),
+	PIN(111, 0, 10, 0x24, 0x40),
+	PIN(112, 0, 11, 0x24, 0x40),
+	PIN(113, 0, 2, 0x24, 0x40),
+	PIN(114, 0, 3, 0x24, 0x40),
+	PIN(115, 0, 4, 0x23, 0x60),
+	PIN(116, 0, 7, 0x23, 0x60),
+	PIN(117, 0, 5, 0x23, 0x60),
+	PIN(118, 0, 6, 0x23, 0x60),
+	PIN(119, 0, 22, 0x25, 0x70),
+	PIN(120, 0, 19, 0x25, 0x70),
+	PIN(121, 0, 20, 0x25, 0x70),
+	PIN(122, 0, 21, 0x25, 0x70),
+	PIN(123, 0, 23, 0x25, 0x70),
+	PIN(124, 0, 0, 0x25, 0x70),
+	PIN(125, 0, 1, 0x25, 0x70),
+	PIN(126, 0, 2, 0x25, 0x70),
+	PIN(127, 0, 8, 0x23, 0x60),
+	PIN(128, 0, 10, 0x23, 0x60),
+	PIN(129, 0, 24, 0x25, 0x70),
+	PIN(130, 0, 26, 0x25, 0x70),
+	PIN(131, 0, 25, 0x25, 0x70),
+	PIN(132, 0, 27, 0x25, 0x70),
+	PIN(133, 0, 9, 0x21, 0x60),
+	PIN(134, 0, 12, 0x21, 0x60),
+	PIN(135, 0, 21, 0x16, 0x40),
+	PIN(136, 0, 24, 0x16, 0x40),
+	PIN(137, 0, 10, 0x21, 0x60),
+	PIN(138, 0, 13, 0x21, 0x60),
+	PIN(139, 0, 7, 0x12, 0x50),
+	PIN(140, 0, 8, 0x12, 0x50),
+	PIN(141, 0, 9, 0x23, 0x60),
+	PIN(142, 0, 11, 0x23, 0x60),
+	PIN(143, 0, 22, 0x16, 0x40),
+	PIN(144, 0, 25, 0x16, 0x40),
+	PIN(145, 0, 23, 0x16, 0x40),
+	PIN(146, 0, 26, 0x16, 0x40),
+	PIN(147, 0, 23, 0x24, 0x40),
+	PIN(148, 0, 24, 0x24, 0x40),
+	PIN(149, 0, 25, 0x24, 0x40),
+	PIN(150, 0, 26, 0x24, 0x40),
+	PIN(151, 0, 27, 0x24, 0x40),
+	PIN(152, 0, 28, 0x24, 0x40),
+	PIN(153, 0, 29, 0x24, 0x40),
+	PIN(154, 0, 30, 0x24, 0x40),
+	PIN(155, 0, 31, 0x24, 0x40),
+	PIN(156, 0, 0, 0x24, 0x50),
+	PIN(157, 0, 4, 0x12, 0x50),
+	PIN(158, 0, 3, 0x12, 0x50),
+	PIN(159, 0, 6, 0x12, 0x50),
+	PIN(160, 0, 5, 0x12, 0x50),
+	PIN(161, 0, 23, 0x12, 0x50),
+	PIN(162, 0, 24, 0x12, 0x50),
+	PIN(163, 0, 11, 0x21, 0x60),
+	PIN(164, 0, 8, 0x21, 0x60),
+	PIN(165, 0, 16, 0x21, 0x60),
+	PIN(166, 0, 1, 0x21, 0x60),
+	PIN(167, 0, 7, 0x21, 0x60),
+	PIN(168, 0, 4, 0x21, 0x60),
+	PIN(169, 0, 5, 0x21, 0x60),
+	PIN(170, 0, 0, 0x21, 0x60),
+	PIN(171, 0, 6, 0x21, 0x60),
+	PIN(172, 0, 2, 0x21, 0x60),
+	PIN(173, 0, 3, 0x21, 0x60),
+	PIN(174, 0, 7, 0x16, 0x40),
+	PIN(175, 0, 8, 0x16, 0x40),
+	PIN(176, 0, 4, 0x16, 0x40),
+	PIN(177, 0, 5, 0x16, 0x40),
+	PIN(178, 0, 6, 0x16, 0x40),
+	PIN(179, 0, 9, 0x16, 0x40),
+	PIN(180, 0, 10, 0x16, 0x40),
+	PIN(181, 0, 11, 0x16, 0x40),
+	PIN(182, 0, 12, 0x16, 0x40),
+	PIN(183, 0, 21, 0x12, 0x50),
+	PIN(184, 0, 22, 0x12, 0x50),
+};
+
+#endif /* MT_GPIO_H */
diff --git a/plat/mediatek/mt8186/drivers/mcdi/build.mk b/plat/mediatek/mt8186/drivers/mcdi/build.mk
new file mode 100644
index 0000000..45acb5d
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/build.mk
@@ -0,0 +1,31 @@
+#
+# Copyright (c) 2021, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+MCDI_TINYSYS_TYPE = sspm
+MCDI_TINYSYS_MBOX_TYPE = share_sram
+
+CUR_MCDI_FOLDER = ${MTK_PLAT_SOC}/drivers/mcdi
+
+BL31_MT_LPM_PLAT_CFLAGS += -I${CUR_MCDI_FOLDER}/
+
+BL31_MT_LPM_PLAT_SOURCE += \
+	${CUR_MCDI_FOLDER}/mt_cpu_pm.c	\
+	${CUR_MCDI_FOLDER}/mt_cpu_pm_cpc.c \
+	${CUR_MCDI_FOLDER}/mt_mcdi.c	\
+	${CUR_MCDI_FOLDER}/mt_lp_irqremain.c
+
+
+ifeq ($(MCDI_TINYSYS_TYPE), sspm)
+BL31_MT_LPM_PLAT_CFLAGS += -DMCDI_TINYSYS_SSPM
+BL31_MT_LPM_PLAT_SOURCE += ${CUR_MCDI_FOLDER}/mt_cpu_pm_mbox_sspm.c
+else
+BL31_MT_LPM_PLAT_CFLAGS += -DMCDI_TINYSYS_MCUPM
+BL31_MT_LPM_PLAT_SOURCE += ${CUR_MCDI_FOLDER}/mt_cpu_pm_mbox.c
+endif
+
+ifeq ($(MCDI_TINYSYS_MBOX_TYPE), share_sram)
+BL31_MT_LPM_PLAT_CFLAGS += -DMCDI_TINYSYS_MBOX_SHARE_SRAM
+endif
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm.c b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm.c
new file mode 100644
index 0000000..c6c2e38
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm.c
@@ -0,0 +1,123 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdint.h>
+
+#include <arch_helpers.h>
+#include <lib/psci/psci.h>
+#include <lib/spinlock.h>
+
+#include <mt_cpu_pm_cpc.h>
+#include <mt_mcdi.h>
+#include <plat_mtk_lpm.h>
+#include <plat_pm.h>
+
+DEFINE_SYSREG_RW_FUNCS(dbgprcr_el1);
+
+static int plat_mt_lp_cpu_rc;
+
+static int pwr_state_prompt(unsigned int cpu, const psci_power_state_t *state)
+{
+	return 0;
+}
+
+static int pwr_state_reflect(unsigned int cpu, const psci_power_state_t *state)
+{
+	mtk_cpc_core_on_hint_clr(cpu);
+
+	if (IS_SYSTEM_SUSPEND_STATE(state)) {
+		mtk_cpc_time_sync();
+	}
+
+	return 0;
+}
+
+static int pwr_cpu_pwron(unsigned int cpu, const psci_power_state_t *state)
+{
+	return 0;
+}
+
+static int pwr_cpu_pwrdwn(unsigned int cpu, const psci_power_state_t *state)
+{
+	/* clear DBGPRCR.CORENPDRQ to allow CPU power down  */
+	write_dbgprcr_el1(0ULL);
+
+	return 0;
+}
+
+static int pwr_cluster_pwron(unsigned int cpu, const psci_power_state_t *state)
+{
+	return 0;
+}
+
+static int pwr_cluster_pwrdwn(unsigned int cpu, const psci_power_state_t *state)
+{
+	return 0;
+}
+
+static int pwr_mcusys_pwron(unsigned int cpu, const psci_power_state_t *state)
+{
+	if (!IS_MCUSYS_OFF_STATE(state) || (plat_mt_lp_cpu_rc < 0)) {
+		return -1;
+	}
+
+	mtk_cpc_mcusys_off_reflect();
+
+	return 0;
+}
+
+static int pwr_mcusys_pwron_finished(unsigned int cpu,
+					const psci_power_state_t *state)
+{
+	if (!IS_MCUSYS_OFF_STATE(state) || (plat_mt_lp_cpu_rc < 0)) {
+		return -1;
+	}
+
+	return 0;
+}
+
+static int pwr_mcusys_pwrdwn(unsigned int cpu, const psci_power_state_t *state)
+{
+	if (!IS_MCUSYS_OFF_STATE(state)) {
+		goto mt_pwr_mcusysoff_break;
+	}
+
+	if (mcdi_try_init() != 0) { /* not ready to process mcusys-off */
+		goto mt_pwr_mcusysoff_break;
+	}
+
+	return 0;
+
+mt_pwr_mcusysoff_break:
+
+	plat_mt_lp_cpu_rc = -1;
+
+	return -1;
+}
+
+static const struct mt_lpm_tz plat_pm = {
+	.pwr_prompt			= pwr_state_prompt,
+	.pwr_reflect			= pwr_state_reflect,
+	.pwr_cpu_on			= pwr_cpu_pwron,
+	.pwr_cpu_dwn			= pwr_cpu_pwrdwn,
+	.pwr_cluster_on			= pwr_cluster_pwron,
+	.pwr_cluster_dwn		= pwr_cluster_pwrdwn,
+	.pwr_mcusys_dwn			= pwr_mcusys_pwrdwn,
+	.pwr_mcusys_on			= pwr_mcusys_pwron,
+	.pwr_mcusys_on_finished		= pwr_mcusys_pwron_finished
+};
+
+const struct mt_lpm_tz *mt_plat_cpu_pm_init(void)
+{
+	mtk_cpc_init();
+
+	if (mcdi_try_init() == 0) {
+		INFO("MCDI init done.\n");
+	}
+
+	return &plat_pm;
+}
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm.h b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm.h
new file mode 100644
index 0000000..83a7a53
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef __MT_CPU_PM_H__
+#define __MT_CPU_PM_H__
+
+#define MCUSYS_STATUS_PDN		(1 << 0UL)
+#define MCUSYS_STATUS_CPUSYS_PROTECT	(1 << 8UL)
+#define MCUSYS_STATUS_MCUSYS_PROTECT	(1 << 9UL)
+
+/* cpu_pm function ID*/
+enum mt_cpu_pm_user_id {
+	MCUSYS_STATUS,
+	CPC_COMMAND,
+	IRQ_REMAIN_LIST_ALLOC,
+	IRQ_REMAIN_IRQ_ADD,
+	IRQ_REMAIN_IRQ_SUBMIT,
+	MBOX_INFO,
+};
+
+/* cpu_pm lp function ID */
+enum mt_cpu_pm_lp_smc_id {
+	LP_CPC_COMMAND,
+	IRQS_REMAIN_ALLOC,
+	IRQS_REMAIN_CTRL,
+	IRQS_REMAIN_IRQ,
+	IRQS_REMAIN_WAKEUP_CAT,
+	IRQS_REMAIN_WAKEUP_SRC,
+};
+
+#endif
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_cpc.c b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_cpc.c
new file mode 100644
index 0000000..2b0f071
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_cpc.c
@@ -0,0 +1,269 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+
+#include <drivers/delay_timer.h>
+
+#include <mt_cpu_pm_cpc.h>
+#include <mt_timer.h>
+
+struct mtk_cpc_dev {
+	int auto_off;
+	unsigned int auto_thres_tick;
+};
+
+static struct mtk_cpc_dev cpc;
+
+static int mtk_cpc_last_core_prot(uint32_t prot_req,
+				uint32_t resp_reg, uint32_t resp_ofs)
+{
+	uint32_t sta, retry;
+
+	retry = 0U;
+
+	while (retry++ < RETRY_CNT_MAX) {
+
+		mmio_write_32(CPC_MCUSYS_LAST_CORE_REQ, prot_req);
+
+		udelay(1U);
+
+		sta = (mmio_read_32(resp_reg) >> resp_ofs) & CPC_PROT_RESP_MASK;
+
+		if (sta == PROT_SUCCESS) {
+			return CPC_SUCCESS;
+		} else if (sta == PROT_GIVEUP) {
+			return CPC_ERR_FAIL;
+		}
+	}
+
+	return CPC_ERR_TIMEOUT;
+}
+
+int mtk_cpu_pm_mcusys_prot_aquire(void)
+{
+	return mtk_cpc_last_core_prot(
+			MCUSYS_PROT_SET,
+			CPC_MCUSYS_LAST_CORE_RESP,
+			MCUSYS_RESP_OFS);
+}
+
+void mtk_cpu_pm_mcusys_prot_release(void)
+{
+	mmio_write_32(CPC_MCUSYS_PWR_ON_MASK, MCUSYS_PROT_CLR);
+}
+
+int mtk_cpu_pm_cluster_prot_aquire(unsigned int cluster)
+{
+	return mtk_cpc_last_core_prot(
+			CPUSYS_PROT_SET,
+			CPC_MCUSYS_MP_LAST_CORE_RESP,
+			CPUSYS_RESP_OFS);
+}
+
+void mtk_cpu_pm_cluster_prot_release(unsigned int cluster)
+{
+	mmio_write_32(CPC_MCUSYS_PWR_ON_MASK, CPUSYS_PROT_CLR);
+}
+
+static void mtk_cpc_cluster_cnt_backup(void)
+{
+	uint32_t backup_cnt;
+	uint32_t curr_cnt;
+	uint32_t cnt_mask = GENMASK(14, 0);
+	uint32_t clr_mask = GENMASK(1, 0);
+
+	/* Single Cluster */
+	backup_cnt = mmio_read_32(CPC_CLUSTER_CNT_BACKUP);
+	curr_cnt = mmio_read_32(CPC_MCUSYS_CLUSTER_COUNTER);
+
+	/* Get off count if dormant count is 0 */
+	if ((curr_cnt & cnt_mask) == 0U) {
+		curr_cnt = (curr_cnt >> 16) & cnt_mask;
+	} else {
+		curr_cnt = curr_cnt & cnt_mask;
+	}
+
+	mmio_write_32(CPC_CLUSTER_CNT_BACKUP, backup_cnt + curr_cnt);
+	mmio_write_32(CPC_MCUSYS_CLUSTER_COUNTER_CLR, clr_mask);
+}
+
+static inline void mtk_cpc_mcusys_off_en(void)
+{
+	mmio_write_32(CPC_MCUSYS_PWR_CTRL, 1U);
+}
+
+static inline void mtk_cpc_mcusys_off_dis(void)
+{
+	mmio_write_32(CPC_MCUSYS_PWR_CTRL, 0U);
+}
+
+void mtk_cpc_mcusys_off_reflect(void)
+{
+	mtk_cpc_mcusys_off_dis();
+	mtk_cpu_pm_mcusys_prot_release();
+}
+
+int mtk_cpc_mcusys_off_prepare(void)
+{
+	if (mtk_cpu_pm_mcusys_prot_aquire() != CPC_SUCCESS) {
+		return CPC_ERR_FAIL;
+	}
+
+	mtk_cpc_cluster_cnt_backup();
+	mtk_cpc_mcusys_off_en();
+
+	return CPC_SUCCESS;
+}
+
+void mtk_cpc_core_on_hint_set(unsigned int cpu)
+{
+	mmio_write_32(CPC_MCUSYS_CPU_ON_SW_HINT_SET, BIT(cpu));
+}
+
+void mtk_cpc_core_on_hint_clr(unsigned int cpu)
+{
+	mmio_write_32(CPC_MCUSYS_CPU_ON_SW_HINT_CLR, BIT(cpu));
+}
+
+static void mtk_cpc_dump_timestamp(void)
+{
+	uint32_t id;
+
+	for (id = 0U; id < CPC_TRACE_ID_NUM; id++) {
+		mmio_write_32(CPC_MCUSYS_TRACE_SEL, id);
+
+		memcpy((void *)(uintptr_t)CPC_TRACE_SRAM(id),
+				(const void *)(uintptr_t)CPC_MCUSYS_TRACE_DATA,
+				CPC_TRACE_SIZE);
+	}
+}
+
+void mtk_cpc_time_sync(void)
+{
+	uint64_t kt;
+	uint32_t systime_l, systime_h;
+
+	kt = sched_clock();
+	systime_l = mmio_read_32(CNTSYS_L_REG);
+	systime_h = mmio_read_32(CNTSYS_H_REG);
+
+	/* sync kernel timer to cpc */
+	mmio_write_32(CPC_MCUSYS_CPC_KERNEL_TIME_L_BASE, (uint32_t)kt);
+	mmio_write_32(CPC_MCUSYS_CPC_KERNEL_TIME_H_BASE, (uint32_t)(kt >> 32));
+	/* sync system timer to cpc */
+	mmio_write_32(CPC_MCUSYS_CPC_SYSTEM_TIME_L_BASE, systime_l);
+	mmio_write_32(CPC_MCUSYS_CPC_SYSTEM_TIME_H_BASE, systime_h);
+}
+
+static void mtk_cpc_config(uint32_t cfg, uint32_t data)
+{
+	uint32_t val;
+	uint32_t reg = 0U;
+
+	switch (cfg) {
+	case CPC_SMC_CONFIG_PROF:
+		reg = CPC_MCUSYS_CPC_DBG_SETTING;
+		val = mmio_read_32(reg);
+		val = (data != 0U) ? (val | CPC_PROF_EN) : (val & ~CPC_PROF_EN);
+		break;
+	case CPC_SMC_CONFIG_AUTO_OFF:
+		reg = CPC_MCUSYS_CPC_FLOW_CTRL_CFG;
+		val = mmio_read_32(reg);
+		if (data != 0U) {
+			val |= CPC_AUTO_OFF_EN;
+			cpc.auto_off = 1;
+		} else {
+			val &= ~CPC_AUTO_OFF_EN;
+			cpc.auto_off = 0;
+		}
+		break;
+	case CPC_SMC_CONFIG_AUTO_OFF_THRES:
+		reg = CPC_MCUSYS_CPC_OFF_THRES;
+		cpc.auto_thres_tick = us_to_ticks(data);
+		val = cpc.auto_thres_tick;
+		break;
+	case CPC_SMC_CONFIG_CNT_CLR:
+		reg = CPC_MCUSYS_CLUSTER_COUNTER_CLR;
+		val = GENMASK(1, 0);	/* clr_mask */
+		break;
+	case CPC_SMC_CONFIG_TIME_SYNC:
+		mtk_cpc_time_sync();
+		break;
+	default:
+		break;
+	}
+
+	if (reg != 0U) {
+		mmio_write_32(reg, val);
+	}
+}
+
+static uint32_t mtk_cpc_read_config(uint32_t cfg)
+{
+	uint32_t res = 0U;
+
+	switch (cfg) {
+	case CPC_SMC_CONFIG_PROF:
+		res = (mmio_read_32(CPC_MCUSYS_CPC_DBG_SETTING) & CPC_PROF_EN) ?
+			1U : 0U;
+		break;
+	case CPC_SMC_CONFIG_AUTO_OFF:
+		res = cpc.auto_off;
+		break;
+	case CPC_SMC_CONFIG_AUTO_OFF_THRES:
+		res = ticks_to_us(cpc.auto_thres_tick);
+		break;
+	case CPC_SMC_CONFIG_CNT_CLR:
+		break;
+	default:
+		break;
+	}
+
+	return res;
+}
+
+uint64_t mtk_cpc_handler(uint64_t act, uint64_t arg1, uint64_t arg2)
+{
+	uint64_t res = 0ULL;
+
+	switch (act) {
+	case CPC_SMC_EVENT_DUMP_TRACE_DATA:
+		mtk_cpc_dump_timestamp();
+		break;
+	case CPC_SMC_EVENT_GIC_DPG_SET:
+		/* isolated_status = x2; */
+		break;
+	case CPC_SMC_EVENT_CPC_CONFIG:
+		mtk_cpc_config((uint32_t)arg1, (uint32_t)arg2);
+		break;
+	case CPC_SMC_EVENT_READ_CONFIG:
+		res = mtk_cpc_read_config((uint32_t)arg1);
+		break;
+	default:
+		break;
+	}
+
+	return res;
+}
+
+void mtk_cpc_init(void)
+{
+	mmio_write_32(CPC_MCUSYS_CPC_DBG_SETTING,
+			mmio_read_32(CPC_MCUSYS_CPC_DBG_SETTING)
+			| CPC_DBG_EN
+			| CPC_CALC_EN);
+
+	cpc.auto_off = 1;
+	cpc.auto_thres_tick = us_to_ticks(8000);
+
+	mmio_write_32(CPC_MCUSYS_CPC_FLOW_CTRL_CFG,
+			mmio_read_32(CPC_MCUSYS_CPC_FLOW_CTRL_CFG)
+			| CPC_OFF_PRE_EN
+			| (cpc.auto_off ? CPC_AUTO_OFF_EN : 0U));
+
+	mmio_write_32(CPC_MCUSYS_CPC_OFF_THRES, cpc.auto_thres_tick);
+}
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_cpc.h b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_cpc.h
new file mode 100644
index 0000000..488b1d1
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_cpc.h
@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MT_CPU_PM_CPC_H
+#define MT_CPU_PM_CPC_H
+
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <mcucfg.h>
+#include <platform_def.h>
+
+#define NEED_CPUSYS_PROT_WORKAROUND	1
+
+/* system sram registers */
+#define CPUIDLE_SRAM_REG(r)	(0x11B000 + (r))
+
+/* db dump */
+#define CPC_TRACE_SIZE		U(0x20)
+#define CPC_TRACE_ID_NUM	U(10)
+#define CPC_TRACE_SRAM(id)	(CPUIDLE_SRAM_REG(0x10) + (id) * CPC_TRACE_SIZE)
+
+/* buckup off count */
+#define CPC_CLUSTER_CNT_BACKUP	CPUIDLE_SRAM_REG(0x1F0)
+#define CPC_MCUSYS_CNT		CPUIDLE_SRAM_REG(0x1F4)
+
+/* CPC_MCUSYS_CPC_FLOW_CTRL_CFG(0xA814): debug setting */
+#define CPC_PWR_ON_SEQ_DIS	BIT(1)
+#define CPC_PWR_ON_PRIORITY	BIT(2)
+#define CPC_AUTO_OFF_EN		BIT(5)
+#define CPC_DORMANT_WAIT_EN	BIT(14)
+#define CPC_CTRL_EN		BIT(16)
+#define CPC_OFF_PRE_EN		BIT(29)
+
+/* CPC_MCUSYS_LAST_CORE_REQ(0xA818) : last core protection */
+#define CPUSYS_PROT_SET		BIT(0)
+#define MCUSYS_PROT_SET		BIT(8)
+#define CPUSYS_PROT_CLR		BIT(8)
+#define MCUSYS_PROT_CLR		BIT(9)
+
+#define CPC_PROT_RESP_MASK	U(0x3)
+#define CPUSYS_RESP_OFS		U(16)
+#define MCUSYS_RESP_OFS		U(30)
+
+#define cpusys_resp(r)		(((r) >> CPUSYS_RESP_OFS) & CPC_PROT_RESP_MASK)
+#define mcusys_resp(r)		(((r) >> MCUSYS_RESP_OFS) & CPC_PROT_RESP_MASK)
+
+#define RETRY_CNT_MAX		U(1000)
+
+#define PROT_RETRY		U(0)
+#define PROT_SUCCESS		U(1)
+#define PROT_GIVEUP		U(2)
+
+/* CPC_MCUSYS_CPC_DBG_SETTING(0xAB00): debug setting */
+#define CPC_PROF_EN		BIT(0)
+#define CPC_DBG_EN		BIT(1)
+#define CPC_FREEZE		BIT(2)
+#define CPC_CALC_EN		BIT(3)
+
+enum {
+	CPC_SUCCESS = 0U,
+	CPC_ERR_FAIL = 1U,
+	CPC_ERR_TIMEOUT = 2U,
+	NF_CPC_ERR = 3U,
+};
+
+enum {
+	CPC_SMC_EVENT_DUMP_TRACE_DATA = 0U,
+	CPC_SMC_EVENT_GIC_DPG_SET = 1U,
+	CPC_SMC_EVENT_CPC_CONFIG = 2U,
+	CPC_SMC_EVENT_READ_CONFIG = 3U,
+	NF_CPC_SMC_EVENT = 4U,
+};
+
+enum {
+	CPC_SMC_CONFIG_PROF = 0U,
+	CPC_SMC_CONFIG_AUTO_OFF = 1U,
+	CPC_SMC_CONFIG_AUTO_OFF_THRES = 2U,
+	CPC_SMC_CONFIG_CNT_CLR = 3U,
+	CPC_SMC_CONFIG_TIME_SYNC = 4U,
+	NF_CPC_SMC_CONFIG = 5U,
+};
+
+#define us_to_ticks(us)		((us) * 13)
+#define ticks_to_us(tick)	((tick) / 13)
+
+int mtk_cpu_pm_cluster_prot_aquire(unsigned int cluster);
+void mtk_cpu_pm_cluster_prot_release(unsigned int cluster);
+
+void mtk_cpc_mcusys_off_reflect(void);
+int mtk_cpc_mcusys_off_prepare(void);
+
+void mtk_cpc_core_on_hint_set(unsigned int cpu);
+void mtk_cpc_core_on_hint_clr(unsigned int cpu);
+void mtk_cpc_time_sync(void);
+
+uint64_t mtk_cpc_handler(uint64_t act, uint64_t arg1, uint64_t arg2);
+void mtk_cpc_init(void);
+
+#endif /* MT_CPU_PM_CPC_H */
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_mbox.c b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_mbox.c
new file mode 100644
index 0000000..789fafb
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_mbox.c
@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <mmio.h>
+#include <mt_cpu_pm_mbox.h>
+#include <platform_def.h>
+#include <sspm_reg.h>
+
+#define MCUPM_MBOX_3_BASE	(MTK_MCUPM_SRAM_BASE + 0xFCE0)
+
+#define _sspm_mbox_write(id, val) \
+		mmio_write_32(SSPM_MBOX_3_BASE + 4 * (id), val)
+#define _sspm_mbox_read(id) \
+		mmio_read_32(SSPM_MBOX_3_BASE + 4 * (id))
+
+#define _mcupm_mbox_write(id, val) \
+		mmio_write_32(MCUPM_MBOX_3_BASE + 4 * (id), val)
+#define _mcupm_mbox_read(id) \
+		mmio_read_32(MCUPM_MBOX_3_BASE + 4 * (id))
+
+
+#define MCUPM_MBOX_OFFSET_PDN		(0x0C55FDA8)
+#define MCUPM_POWER_DOWN		(0x4D50444E)
+
+void mtk_set_sspm_lp_cmd(void *buf, unsigned int size)
+{
+	unsigned int *p = (unsigned int *)buf;
+	int i;
+
+	for (i = 0; i < size; i++) {
+		_sspm_mbox_write(SSPM_MBOX_SPM_CMD + i, p[i]);
+	}
+}
+
+void mtk_clr_sspm_lp_cmd(unsigned int size)
+{
+	int i;
+
+	for (i = 0; i < size; i++) {
+		_sspm_mbox_write(SSPM_MBOX_SPM_CMD + i, 0);
+	}
+}
+
+void mtk_set_cpu_pm_pll_mode(unsigned int mode)
+{
+	if (mode < NF_MCUPM_ARMPLL_MODE) {
+		_mcupm_mbox_write(MCUPM_MBOX_ARMPLL_MODE, mode);
+	}
+}
+
+int mtk_get_cpu_pm_pll_mode(void)
+{
+	return _mcupm_mbox_read(MCUPM_MBOX_ARMPLL_MODE);
+}
+
+void mtk_set_cpu_pm_buck_mode(unsigned int mode)
+{
+	if (mode < NF_MCUPM_BUCK_MODE) {
+		_mcupm_mbox_write(MCUPM_MBOX_BUCK_MODE, mode);
+	}
+}
+
+int mtk_get_cpu_pm_buck_mode(void)
+{
+	return _mcupm_mbox_read(MCUPM_MBOX_BUCK_MODE);
+}
+
+void mtk_set_cpu_pm_preffered_cpu(unsigned int cpuid)
+{
+	return _mcupm_mbox_read(MCUPM_MBOX_WAKEUP_CPU);
+}
+
+int mtk_set_cpu_pm_mbox_addr(uint64_t phy_addr)
+{
+	return 0;
+}
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_mbox.h b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_mbox.h
new file mode 100644
index 0000000..63a43af
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_mbox.h
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef __MT_CPU_PM_MBOX_H__
+#define __MT_CPU_PM_MBOX_H__
+
+/* SSPM Mbox */
+/* AP Write */
+#define SSPM_MBOX_SPM_CMD	(0U)
+#define SSPM_MBOX_SPM_ARGS1	(1U)
+#define SSPM_MBOX_SPM_ARGS2	(2U)
+#define SSPM_MBOX_SPM_ARGS3	(3U)
+#define SSPM_MBOX_SPM_ARGS4	(4U)
+#define SSPM_MBOX_SPM_ARGS5	(5U)
+#define SSPM_MBOX_SPM_ARGS6	(6U)
+#define SSPM_MBOX_SPM_ARGS7	(7U)
+#define SSPM_MBOX_AP_READY	(17U)
+
+#define SSPM_MBOX_SPM_CMD_SIZE	(8U)
+
+void mtk_set_sspm_lp_cmd(void *buf, unsigned int size);
+void mtk_clr_sspm_lp_cmd(unsigned int size);
+
+/* MCUPM Mbox */
+/* AP Write */
+#define MCUPM_MBOX_AP_READY		(0U)
+#define MCUPM_MBOX_RESERVED_1		(1U)
+#define MCUPM_MBOX_RESERVED_2		(2U)
+#define MCUPM_MBOX_RESERVED_3		(3U)
+#define MCUPM_MBOX_PWR_CTRL_EN		(4U)
+#define MCUPM_MBOX_L3_CACHE_MODE	(5U)
+#define MCUPM_MBOX_BUCK_MODE		(6U)
+#define MCUPM_MBOX_ARMPLL_MODE		(7U)
+/* AP Read */
+#define MCUPM_MBOX_TASK_STA		(8U)
+#define MCUPM_MBOX_RESERVED_9		(9U)
+#define MCUPM_MBOX_RESERVED_10		(10U)
+#define MCUPM_MBOX_RESERVED_11		(11U)
+/* CPC mode - Read/Write */
+#define MCUPM_MBOX_WAKEUP_CPU		(12U)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_PWR_CTRL_EN (4) */
+#define MCUPM_MCUSYS_CTRL		(1U << 0)
+#define MCUPM_BUCK_CTRL			(1U << 1)
+#define MCUPM_ARMPLL_CTRL		(1U << 2)
+#define MCUPM_PWR_CTRL_MASK		((1U << 3) - 1U)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_L3_CACHE_MODE (5) */
+#define MCUPM_L3_OFF_MODE		(0U) /* default */
+#define MCUPM_L3_DORMANT_MODE		(1U)
+#define NF_MCUPM_L3_MODE		(2U)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_BUCK_MODE (6) */
+#define MCUPM_BUCK_NORMAL_MODE		(0U) /* default */
+#define MCUPM_BUCK_LP_MODE		(1U)
+#define MCUPM_BUCK_OFF_MODE		(2U)
+#define NF_MCUPM_BUCK_MODE		(3U)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_ARMPLL_MODE (7) */
+#define MCUPM_ARMPLL_ON			(0U) /* default */
+#define MCUPM_ARMPLL_GATING		(1U)
+#define MCUPM_ARMPLL_OFF		(2U)
+#define NF_MCUPM_ARMPLL_MODE		(3U)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_TASK_STA (9) */
+#define MCUPM_TASK_UNINIT		(0U)
+#define MCUPM_TASK_INIT			(1U)
+#define MCUPM_TASK_INIT_FINISH		(2U)
+#define MCUPM_TASK_WAIT			(3U)
+#define MCUPM_TASK_RUN			(4U)
+#define MCUPM_TASK_PAUSE		(5U)
+
+void mtk_set_cpu_pm_pll_mode(unsigned int mode);
+int mtk_get_cpu_pm_pll_mode(void);
+void mtk_set_cpu_pm_buck_mode(unsigned int mode);
+int mtk_get_cpu_pm_buck_mode(void);
+void mtk_set_cpu_pm_preffered_cpu(unsigned int cpuid);
+int mtk_set_cpu_pm_mbox_addr(uint64_t phy_addr);
+
+#endif
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_mbox_sspm.c b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_mbox_sspm.c
new file mode 100644
index 0000000..34c281d
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_cpu_pm_mbox_sspm.c
@@ -0,0 +1,90 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <mmio.h>
+#include <mt_cpu_pm.h>
+#include <mt_cpu_pm_mbox.h>
+#include <platform_def.h>
+#include <sspm_reg.h>
+
+#ifdef MCDI_TINYSYS_MBOX_SHARE_SRAM
+struct cpu_pm_mbox {
+	unsigned int ap_ready;
+	unsigned int reserved1;
+	unsigned int reserved2;
+	unsigned int reserved3;
+	unsigned int pwr_ctrl_en;
+	unsigned int l3_cache_mode;
+	unsigned int buck_mode;
+	unsigned int armpll_mode;
+	unsigned int task_sta;
+	unsigned int reserved9;
+	unsigned int reserved10;
+	unsigned int reserved11;
+	unsigned int wakeup_cpu;
+};
+
+struct cpu_pm_mbox *_cpu_pm_box = (struct cpu_pm_mbox *)SSPM_MBOX_3_BASE;
+#endif
+
+void mtk_set_cpu_pm_pll_mode(unsigned int mode)
+{
+#ifdef MCDI_TINYSYS_MBOX_SHARE_SRAM
+	if (_cpu_pm_box) {
+		_cpu_pm_box->armpll_mode = mode;
+	}
+#endif
+}
+
+int mtk_get_cpu_pm_pll_mode(void)
+{
+#ifdef MCDI_TINYSYS_MBOX_SHARE_SRAM
+	if (!_cpu_pm_box) {
+		return 0;
+	}
+	return _cpu_pm_box->armpll_mode;
+#endif
+}
+
+void mtk_set_cpu_pm_buck_mode(unsigned int mode)
+{
+#ifdef MCDI_TINYSYS_MBOX_SHARE_SRAM
+	if (_cpu_pm_box) {
+		_cpu_pm_box->buck_mode = mode;
+	}
+#endif
+}
+
+int mtk_get_cpu_pm_buck_mode(void)
+{
+#ifdef MCDI_TINYSYS_MBOX_SHARE_SRAM
+	if (!_cpu_pm_box) {
+		return 0;
+	}
+	return _cpu_pm_box->buck_mode;
+#endif
+}
+
+void mtk_set_cpu_pm_preffered_cpu(unsigned int cpuid)
+{
+#ifdef MCDI_TINYSYS_MBOX_SHARE_SRAM
+	if (_cpu_pm_box) {
+		_cpu_pm_box->wakeup_cpu = cpuid;
+	}
+#endif
+}
+
+int mtk_set_cpu_pm_mbox_addr(uint64_t phy_addr)
+{
+#ifdef MCDI_TINYSYS_MBOX_SHARE_SRAM
+	if (_cpu_pm_box || (phy_addr == 0)) {
+		return -1;
+	}
+
+	_cpu_pm_box = (struct cpu_pm_mbox *)(MTK_SSPM_BASE + phy_addr);
+#endif
+	return 0;
+}
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_mcdi.c b/plat/mediatek/mt8186/drivers/mcdi/mt_mcdi.c
new file mode 100644
index 0000000..0103612
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_mcdi.c
@@ -0,0 +1,150 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <cdefs.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <mt_mcdi.h>
+
+/* Read/Write */
+#define APMCU_MCUPM_MBOX_AP_READY	U(0)
+#define APMCU_MCUPM_MBOX_RESERVED_1	U(1)
+#define APMCU_MCUPM_MBOX_RESERVED_2	U(2)
+#define APMCU_MCUPM_MBOX_RESERVED_3	U(3)
+#define APMCU_MCUPM_MBOX_PWR_CTRL_EN	U(4)
+#define APMCU_MCUPM_MBOX_L3_CACHE_MODE	U(5)
+#define APMCU_MCUPM_MBOX_BUCK_MODE	U(6)
+#define APMCU_MCUPM_MBOX_ARMPLL_MODE	U(7)
+/* Read only */
+#define APMCU_MCUPM_MBOX_TASK_STA	U(8)
+#define APMCU_MCUPM_MBOX_RESERVED_9	U(9)
+#define APMCU_MCUPM_MBOX_RESERVED_10	U(10)
+#define APMCU_MCUPM_MBOX_RESERVED_11	U(11)
+
+/* CPC mode - Read/Write */
+#define APMCU_MCUPM_MBOX_WAKEUP_CPU	U(12)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_PWR_CTRL_EN */
+#define MCUPM_MCUSYS_CTRL		BIT(0)
+#define MCUPM_BUCK_CTRL			BIT(1)
+#define MCUPM_ARMPLL_CTRL		BIT(2)
+#define MCUPM_CM_CTRL			BIT(3)
+#define MCUPM_PWR_CTRL_MASK		GENMASK(3, 0)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_BUCK_MODE */
+#define MCUPM_BUCK_NORMAL_MODE		U(0) /* default */
+#define MCUPM_BUCK_LP_MODE		U(1)
+#define MCUPM_BUCK_OFF_MODE		U(2)
+#define NF_MCUPM_BUCK_MODE		U(3)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_ARMPLL_MODE */
+#define MCUPM_ARMPLL_ON			U(0) /* default */
+#define MCUPM_ARMPLL_GATING		U(1)
+#define MCUPM_ARMPLL_OFF		U(2)
+#define NF_MCUPM_ARMPLL_MODE		U(3)
+
+/* Mbox Slot: APMCU_MCUPM_MBOX_TASK_STA */
+#define MCUPM_TASK_UNINIT		U(0)
+#define MCUPM_TASK_INIT			U(1)
+#define MCUPM_TASK_INIT_FINISH		U(2)
+#define MCUPM_TASK_WAIT			U(3)
+#define MCUPM_TASK_RUN			U(4)
+#define MCUPM_TASK_PAUSE		U(5)
+
+#define SSPM_MBOX_3_BASE		U(0x10420000)
+
+#define MCDI_NOT_INIT			U(0)
+#define MCDI_INIT_1			U(1)
+#define MCDI_INIT_2			U(2)
+#define MCDI_INIT_DONE			U(3)
+
+static int mcdi_init_status __section("tzfw_coherent_mem");
+
+static inline uint32_t mcdi_mbox_read(uint32_t id)
+{
+	return mmio_read_32(SSPM_MBOX_3_BASE + (id << 2));
+}
+
+static inline void mcdi_mbox_write(uint32_t id, uint32_t val)
+{
+	mmio_write_32(SSPM_MBOX_3_BASE + (id << 2), val);
+}
+
+static void mtk_mcupm_pwr_ctrl_setting(uint32_t dev)
+{
+	mcdi_mbox_write(APMCU_MCUPM_MBOX_PWR_CTRL_EN, dev);
+}
+
+static void mtk_set_mcupm_pll_mode(uint32_t mode)
+{
+	if (mode < NF_MCUPM_ARMPLL_MODE) {
+		mcdi_mbox_write(APMCU_MCUPM_MBOX_ARMPLL_MODE, mode);
+	}
+}
+
+static void mtk_set_mcupm_buck_mode(uint32_t mode)
+{
+	if (mode < NF_MCUPM_BUCK_MODE) {
+		mcdi_mbox_write(APMCU_MCUPM_MBOX_BUCK_MODE, mode);
+	}
+}
+
+static int mtk_mcupm_is_ready(void)
+{
+	unsigned int sta = mcdi_mbox_read(APMCU_MCUPM_MBOX_TASK_STA);
+
+	return ((sta == MCUPM_TASK_WAIT) || (sta == MCUPM_TASK_INIT_FINISH));
+}
+
+static int mcdi_init_1(void)
+{
+	unsigned int sta = mcdi_mbox_read(APMCU_MCUPM_MBOX_TASK_STA);
+
+	if (sta != MCUPM_TASK_INIT) {
+		return -1;
+	}
+
+	mtk_set_mcupm_pll_mode(MCUPM_ARMPLL_OFF);
+	mtk_set_mcupm_buck_mode(MCUPM_BUCK_OFF_MODE);
+
+	mtk_mcupm_pwr_ctrl_setting(
+			 MCUPM_MCUSYS_CTRL |
+			 MCUPM_BUCK_CTRL |
+			 MCUPM_ARMPLL_CTRL);
+
+	mcdi_mbox_write(APMCU_MCUPM_MBOX_AP_READY, 1);
+
+	return 0;
+}
+
+static int mcdi_init_2(void)
+{
+	return mtk_mcupm_is_ready() ? 0 : -1;
+}
+
+int mcdi_try_init(void)
+{
+	if (mcdi_init_status == MCDI_INIT_DONE) {
+		return 0;
+	}
+
+	if (mcdi_init_status == MCDI_NOT_INIT) {
+		mcdi_init_status = MCDI_INIT_1;
+	}
+
+	if (mcdi_init_status == MCDI_INIT_1 && mcdi_init_1() == 0) {
+		mcdi_init_status = MCDI_INIT_2;
+	}
+
+	if (mcdi_init_status == MCDI_INIT_2 && mcdi_init_2() == 0) {
+		mcdi_init_status = MCDI_INIT_DONE;
+	}
+
+	INFO("mcdi ready for mcusys-off-idle and system suspend\n");
+
+	return (mcdi_init_status == MCDI_INIT_DONE) ? 0 : mcdi_init_status;
+}
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_mcdi.h b/plat/mediatek/mt8186/drivers/mcdi/mt_mcdi.h
new file mode 100644
index 0000000..0e6444a
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_mcdi.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MT_MCDI_H
+#define MT_MCDI_H
+
+int mcdi_try_init(void);
+
+#endif /* MT_MCDI_H */
diff --git a/plat/mediatek/mt8186/drivers/pmic/pmic.c b/plat/mediatek/mt8186/drivers/pmic/pmic.c
new file mode 100644
index 0000000..4f7ab13
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/pmic/pmic.c
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <pmic.h>
+#include <pmic_wrap_init.h>
+
+uint32_t pmic_get_hwcid(void)
+{
+	uint32_t val = 0;
+
+	pwrap_read(PMIC_RG_HWCID_ADDR, &val);
+
+	return val;
+}
+
+void pmic_power_off(void)
+{
+	pwrap_write(PMIC_PWRHOLD, 0x0);
+}
diff --git a/plat/mediatek/mt8186/drivers/pmic/pmic.h b/plat/mediatek/mt8186/drivers/pmic/pmic.h
new file mode 100644
index 0000000..91ccb19
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/pmic/pmic.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PMIC_H
+#define PMIC_H
+
+#include <stdint.h>
+
+#define PMIC_RG_HWCID_ADDR 0x8
+#define PMIC_PWRHOLD 0xa08
+
+/* external API */
+uint32_t pmic_get_hwcid(void);
+void pmic_power_off(void);
+
+#endif /* PMIC_H */
diff --git a/plat/mediatek/mt8186/drivers/pmic/pmic_wrap_init.h b/plat/mediatek/mt8186/drivers/pmic/pmic_wrap_init.h
new file mode 100644
index 0000000..e837456
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/pmic/pmic_wrap_init.h
@@ -0,0 +1,94 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PMIC_WRAP_INIT_H
+#define PMIC_WRAP_INIT_H
+
+#include <stdint.h>
+
+#include "platform_def.h"
+
+/* external API */
+int32_t pwrap_read(uint32_t adr, uint32_t *rdata);
+int32_t pwrap_write(uint32_t adr, uint32_t wdata);
+
+static struct mt8186_pmic_wrap_regs *const mtk_pwrap = (void *)PMIC_WRAP_BASE;
+
+/* timeout setting */
+enum {
+	TIMEOUT_RESET       = 50,	/* us */
+	TIMEOUT_READ        = 50,	/* us */
+	TIMEOUT_WAIT_IDLE   = 50	/* us */
+};
+
+/* PMIC_WRAP registers */
+struct mt8186_pmic_wrap_regs {
+	uint32_t unused[776];
+	uint32_t wacs2_cmd;
+	uint32_t wacs2_rdata;
+	uint32_t wacs2_vldclr;
+};
+
+enum {
+	RDATA_WACS_RDATA_SHIFT = 0,
+	RDATA_WACS_FSM_SHIFT = 16,
+	RDATA_WACS_REQ_SHIFT = 19,
+	RDATA_SYNC_IDLE_SHIFT = 20,
+	RDATA_INIT_DONE_SHIFT = 22,
+	RDATA_SYS_IDLE_SHIFT = 23,
+};
+
+enum {
+	RDATA_WACS_RDATA_MASK = 0xffff,
+	RDATA_WACS_FSM_MASK = 0x7,
+	RDATA_WACS_REQ_MASK = 0x1,
+	RDATA_SYNC_IDLE_MASK = 0x1,
+	RDATA_INIT_DONE_MASK = 0x1,
+	RDATA_SYS_IDLE_MASK = 0x1,
+};
+
+/* WACS_FSM */
+enum {
+	WACS_FSM_IDLE            = 0x00,
+	WACS_FSM_REQ             = 0x02,
+	WACS_FSM_WFDLE           = 0x04,
+	WACS_FSM_WFVLDCLR        = 0x06,
+	WACS_INIT_DONE           = 0x01,
+	WACS_SYNC_IDLE           = 0x01,
+	WACS_SYNC_BUSY           = 0x00
+};
+
+/* error information flag */
+enum {
+	E_PWR_INVALID_ARG             = 1,
+	E_PWR_INVALID_RW              = 2,
+	E_PWR_INVALID_ADDR            = 3,
+	E_PWR_INVALID_WDAT            = 4,
+	E_PWR_INVALID_OP_MANUAL       = 5,
+	E_PWR_NOT_IDLE_STATE          = 6,
+	E_PWR_NOT_INIT_DONE           = 7,
+	E_PWR_NOT_INIT_DONE_READ      = 8,
+	E_PWR_WAIT_IDLE_TIMEOUT       = 9,
+	E_PWR_WAIT_IDLE_TIMEOUT_READ  = 10,
+	E_PWR_INIT_SIDLY_FAIL         = 11,
+	E_PWR_RESET_TIMEOUT           = 12,
+	E_PWR_TIMEOUT                 = 13,
+	E_PWR_INIT_RESET_SPI          = 20,
+	E_PWR_INIT_SIDLY              = 21,
+	E_PWR_INIT_REG_CLOCK          = 22,
+	E_PWR_INIT_ENABLE_PMIC        = 23,
+	E_PWR_INIT_DIO                = 24,
+	E_PWR_INIT_CIPHER             = 25,
+	E_PWR_INIT_WRITE_TEST         = 26,
+	E_PWR_INIT_ENABLE_CRC         = 27,
+	E_PWR_INIT_ENABLE_DEWRAP      = 28,
+	E_PWR_INIT_ENABLE_EVENT       = 29,
+	E_PWR_READ_TEST_FAIL          = 30,
+	E_PWR_WRITE_TEST_FAIL         = 31,
+	E_PWR_SWITCH_DIO              = 32
+};
+
+#endif /* PMIC_WRAP_INIT_H */
diff --git a/plat/mediatek/mt8186/drivers/rtc/rtc.c b/plat/mediatek/mt8186/drivers/rtc/rtc.c
new file mode 100644
index 0000000..4fcf58e
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/rtc/rtc.c
@@ -0,0 +1,133 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <rtc.h>
+
+static void RTC_Config_Interface(uint32_t addr, uint16_t data,
+			    uint16_t MASK, uint16_t SHIFT)
+{
+	uint16_t pmic_reg = 0;
+
+	pmic_reg = RTC_Read(addr);
+
+	pmic_reg &= ~(MASK << SHIFT);
+	pmic_reg |= (data << SHIFT);
+
+	RTC_Write(addr, pmic_reg);
+}
+
+static void rtc_disable_2sec_reboot(void)
+{
+	uint16_t reboot;
+
+	reboot = (RTC_Read(RTC_AL_SEC) & ~RTC_BBPU_2SEC_EN) &
+		 ~RTC_BBPU_AUTO_PDN_SEL;
+	RTC_Write(RTC_AL_SEC, reboot);
+	RTC_Write_Trigger();
+}
+
+static void rtc_xosc_write(uint16_t val, bool reload)
+{
+	uint16_t bbpu;
+
+	RTC_Write(RTC_OSC32CON, RTC_OSC32CON_UNLOCK1);
+	rtc_busy_wait();
+	RTC_Write(RTC_OSC32CON, RTC_OSC32CON_UNLOCK2);
+	rtc_busy_wait();
+
+	RTC_Write(RTC_OSC32CON, val);
+	rtc_busy_wait();
+
+	if (reload) {
+		bbpu = RTC_Read(RTC_BBPU) | RTC_BBPU_KEY | RTC_BBPU_RELOAD;
+		RTC_Write(RTC_BBPU, bbpu);
+		RTC_Write_Trigger();
+	}
+}
+
+static void rtc_enable_k_eosc(void)
+{
+	uint16_t osc32;
+	uint16_t rtc_eosc_cali_td = 8; /* eosc cali period time */
+
+	/* Truning on eosc cali mode clock */
+	RTC_Config_Interface(PMIC_RG_TOP_CON, 1,
+			PMIC_RG_SRCLKEN_IN0_HW_MODE_MASK,
+			PMIC_RG_SRCLKEN_IN0_HW_MODE_SHIFT);
+	RTC_Config_Interface(PMIC_RG_TOP_CON, 1,
+			PMIC_RG_SRCLKEN_IN1_HW_MODE_MASK,
+			PMIC_RG_SRCLKEN_IN1_HW_MODE_SHIFT);
+	RTC_Config_Interface(PMIC_RG_SCK_TOP_CKPDN_CON0, 0,
+			PMIC_RG_RTC_EOSC32_CK_PDN_MASK,
+			PMIC_RG_RTC_EOSC32_CK_PDN_SHIFT);
+
+	switch (rtc_eosc_cali_td) {
+	case 1:
+		RTC_Config_Interface(PMIC_RG_EOSC_CALI_CON0, 0x3,
+			PMIC_RG_EOSC_CALI_TD_MASK, PMIC_RG_EOSC_CALI_TD_SHIFT);
+		break;
+	case 2:
+		RTC_Config_Interface(PMIC_RG_EOSC_CALI_CON0, 0x4,
+			PMIC_RG_EOSC_CALI_TD_MASK, PMIC_RG_EOSC_CALI_TD_SHIFT);
+		break;
+	case 4:
+		RTC_Config_Interface(PMIC_RG_EOSC_CALI_CON0, 0x5,
+			PMIC_RG_EOSC_CALI_TD_MASK, PMIC_RG_EOSC_CALI_TD_SHIFT);
+		break;
+	case 16:
+		RTC_Config_Interface(PMIC_RG_EOSC_CALI_CON0, 0x7,
+			PMIC_RG_EOSC_CALI_TD_MASK, PMIC_RG_EOSC_CALI_TD_SHIFT);
+		break;
+	default:
+		RTC_Config_Interface(PMIC_RG_EOSC_CALI_CON0, 0x6,
+			PMIC_RG_EOSC_CALI_TD_MASK, PMIC_RG_EOSC_CALI_TD_SHIFT);
+		break;
+	}
+	/* Switch the DCXO from 32k-less mode to RTC mode,
+	 * otherwise, EOSC cali will fail
+	 */
+	/* RTC mode will have only OFF mode and FPM */
+	RTC_Config_Interface(PMIC_RG_DCXO_CW02, 0, PMIC_RG_XO_EN32K_MAN_MASK,
+		PMIC_RG_XO_EN32K_MAN_SHIFT);
+	RTC_Write(RTC_BBPU,
+		  RTC_Read(RTC_BBPU) | RTC_BBPU_KEY | RTC_BBPU_RELOAD);
+	RTC_Write_Trigger();
+	/* Enable K EOSC mode for normal power off and then plug out battery */
+	RTC_Write(RTC_AL_YEA, ((RTC_Read(RTC_AL_YEA) | RTC_K_EOSC_RSV_0)
+				& (~RTC_K_EOSC_RSV_1)) | RTC_K_EOSC_RSV_2);
+	RTC_Write_Trigger();
+
+	osc32 = RTC_Read(RTC_OSC32CON);
+	rtc_xosc_write(osc32 | RTC_EMBCK_SRC_SEL, true);
+	INFO("[RTC] RTC_enable_k_eosc\n");
+}
+
+void rtc_power_off_sequence(void)
+{
+	uint16_t bbpu;
+
+	rtc_disable_2sec_reboot();
+	rtc_enable_k_eosc();
+
+	/* clear alarm */
+	bbpu = RTC_BBPU_KEY | RTC_BBPU_CLR | RTC_BBPU_PWREN;
+	if (Writeif_unlock()) {
+		RTC_Write(RTC_BBPU, bbpu);
+
+		RTC_Write(RTC_AL_MASK, RTC_AL_MASK_DOW);
+		RTC_Write_Trigger();
+		mdelay(1);
+
+		bbpu = RTC_Read(RTC_BBPU) | RTC_BBPU_KEY | RTC_BBPU_RELOAD;
+		RTC_Write(RTC_BBPU, bbpu);
+		RTC_Write_Trigger();
+		INFO("[RTC] BBPU=0x%x, IRQ_EN=0x%x, AL_MSK=0x%x, AL_SEC=0x%x\n",
+		     RTC_Read(RTC_BBPU), RTC_Read(RTC_IRQ_EN),
+		     RTC_Read(RTC_AL_MASK), RTC_Read(RTC_AL_SEC));
+	}
+}
diff --git a/plat/mediatek/mt8186/drivers/rtc/rtc.h b/plat/mediatek/mt8186/drivers/rtc/rtc.h
new file mode 100644
index 0000000..b48dbb9
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/rtc/rtc.h
@@ -0,0 +1,145 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef RTC_H
+#define RTC_H
+
+#define PMIC_RG_SRCLKEN_IN0_HW_MODE_MASK	(1U)
+#define PMIC_RG_SRCLKEN_IN0_HW_MODE_SHIFT	(1U)
+#define PMIC_RG_SRCLKEN_IN1_HW_MODE_MASK	(1U)
+#define PMIC_RG_SRCLKEN_IN1_HW_MODE_SHIFT	(3U)
+#define PMIC_RG_RTC_EOSC32_CK_PDN_MASK		(1U)
+#define PMIC_RG_RTC_EOSC32_CK_PDN_SHIFT		(2U)
+#define PMIC_RG_EOSC_CALI_TD_MASK		(7U)
+#define PMIC_RG_EOSC_CALI_TD_SHIFT		(5U)
+#define PMIC_RG_XO_EN32K_MAN_MASK		(1U)
+#define PMIC_RG_XO_EN32K_MAN_SHIFT		(0U)
+
+/* RTC registers */
+enum {
+	RTC_BBPU = 0x0588,
+	RTC_IRQ_STA = 0x058A,
+	RTC_IRQ_EN = 0x058C,
+	RTC_CII_EN = 0x058E
+};
+
+enum {
+	RTC_AL_SEC = 0x05A0,
+	RTC_AL_MIN = 0x05A2,
+	RTC_AL_HOU = 0x05A4,
+	RTC_AL_DOM = 0x05A6,
+	RTC_AL_DOW = 0x05A8,
+	RTC_AL_MTH = 0x05AA,
+	RTC_AL_YEA = 0x05AC,
+	RTC_AL_MASK = 0x0590
+};
+
+enum {
+	RTC_OSC32CON = 0x05AE,
+	RTC_CON = 0x05C4,
+	RTC_WRTGR = 0x05C2
+};
+
+enum {
+	RTC_PDN1 = 0x05B4,
+	RTC_PDN2 = 0x05B6,
+	RTC_SPAR0 = 0x05B8,
+	RTC_SPAR1 = 0x05BA,
+	RTC_PROT = 0x05BC,
+	RTC_DIFF = 0x05BE,
+	RTC_CALI = 0x05C0
+};
+
+enum {
+	RTC_OSC32CON_UNLOCK1 = 0x1A57,
+	RTC_OSC32CON_UNLOCK2 = 0x2B68
+};
+
+enum {
+	RTC_PROT_UNLOCK1 = 0x586A,
+	RTC_PROT_UNLOCK2 = 0x9136
+};
+
+enum {
+	RTC_BBPU_PWREN	= 1U << 0,
+	RTC_BBPU_CLR	= 1U << 1,
+	RTC_BBPU_INIT	= 1U << 2,
+	RTC_BBPU_AUTO	= 1U << 3,
+	RTC_BBPU_CLRPKY	= 1U << 4,
+	RTC_BBPU_RELOAD	= 1U << 5,
+	RTC_BBPU_CBUSY	= 1U << 6
+};
+
+enum {
+	RTC_AL_MASK_SEC = 1U << 0,
+	RTC_AL_MASK_MIN = 1U << 1,
+	RTC_AL_MASK_HOU = 1U << 2,
+	RTC_AL_MASK_DOM = 1U << 3,
+	RTC_AL_MASK_DOW = 1U << 4,
+	RTC_AL_MASK_MTH = 1U << 5,
+	RTC_AL_MASK_YEA = 1U << 6
+};
+
+enum {
+	RTC_BBPU_AUTO_PDN_SEL = 1U << 6,
+	RTC_BBPU_2SEC_CK_SEL = 1U << 7,
+	RTC_BBPU_2SEC_EN = 1U << 8,
+	RTC_BBPU_2SEC_MODE = 0x3 << 9,
+	RTC_BBPU_2SEC_STAT_CLEAR = 1U << 11,
+	RTC_BBPU_2SEC_STAT_STA = 1U << 12
+};
+
+enum {
+	RTC_BBPU_KEY	= 0x43 << 8
+};
+
+enum {
+	RTC_EMBCK_SRC_SEL	= 1 << 8,
+	RTC_EMBCK_SEL_MODE	= 3 << 6,
+	RTC_XOSC32_ENB		= 1 << 5,
+	RTC_REG_XOSC32_ENB	= 1 << 15
+};
+
+enum {
+	RTC_K_EOSC_RSV_0	= 1 << 8,
+	RTC_K_EOSC_RSV_1	= 1 << 9,
+	RTC_K_EOSC_RSV_2	= 1 << 10
+};
+
+/* PMIC TOP Register Definition */
+enum {
+	PMIC_RG_TOP_CON = 0x001E,
+	PMIC_RG_TOP_CKPDN_CON1 = 0x0112,
+	PMIC_RG_TOP_CKPDN_CON1_SET = 0x0114,
+	PMIC_RG_TOP_CKPDN_CON1_CLR = 0x0116,
+	PMIC_RG_TOP_CKSEL_CON0 = 0x0118,
+	PMIC_RG_TOP_CKSEL_CON0_SET = 0x011A,
+	PMIC_RG_TOP_CKSEL_CON0_CLR = 0x011C
+};
+
+/* PMIC SCK Register Definition */
+enum {
+	PMIC_RG_SCK_TOP_CKPDN_CON0 = 0x051A,
+	PMIC_RG_SCK_TOP_CKPDN_CON0_SET = 0x051C,
+	PMIC_RG_SCK_TOP_CKPDN_CON0_CLR = 0x051E,
+	PMIC_RG_EOSC_CALI_CON0 = 0x540
+};
+
+/* PMIC DCXO Register Definition */
+enum {
+	PMIC_RG_DCXO_CW00 = 0x0788,
+	PMIC_RG_DCXO_CW02 = 0x0790
+};
+
+/* external API */
+uint16_t RTC_Read(uint32_t addr);
+void RTC_Write(uint32_t addr, uint16_t data);
+int32_t rtc_busy_wait(void);
+int32_t RTC_Write_Trigger(void);
+int32_t Writeif_unlock(void);
+void rtc_power_off_sequence(void);
+
+#endif /* RTC_H */
diff --git a/plat/mediatek/mt8186/drivers/spmc/mtspmc.c b/plat/mediatek/mt8186/drivers/spmc/mtspmc.c
new file mode 100644
index 0000000..91ef096
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/spmc/mtspmc.c
@@ -0,0 +1,172 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+
+#include <mcucfg.h>
+#include <mtspmc.h>
+#include <mtspmc_private.h>
+#include <plat/common/platform.h>
+
+void mcucfg_disable_gic_wakeup(unsigned int cluster, unsigned int cpu)
+{
+	mmio_setbits_32(MCUCFG_CPC_FLOW_CTRL_CFG, GIC_WAKEUP_IGNORE(cpu));
+}
+
+void mcucfg_enable_gic_wakeup(unsigned int cluster, unsigned int cpu)
+{
+	mmio_clrbits_32(MCUCFG_CPC_FLOW_CTRL_CFG, GIC_WAKEUP_IGNORE(cpu));
+	/* Clear cpu's cpc sw hint */
+	mmio_write_32(CPC_MCUSYS_CPU_ON_SW_HINT_CLR, BIT(cpu));
+}
+
+void mcucfg_set_bootaddr(unsigned int cluster, unsigned int cpu, uintptr_t bootaddr)
+{
+	assert(cluster == 0U);
+
+	mmio_write_32(per_cpu(cluster, cpu, MCUCFG_BOOTADDR), bootaddr);
+}
+
+uintptr_t mcucfg_get_bootaddr(unsigned int cluster, unsigned int cpu)
+{
+	assert(cluster == 0U);
+
+	return (uintptr_t)mmio_read_32(per_cpu(cluster, cpu, MCUCFG_BOOTADDR));
+}
+
+void mcucfg_init_archstate(unsigned int cluster, unsigned int cpu, bool arm64)
+{
+	uint32_t reg;
+
+	assert(cluster == 0U);
+
+	reg = per_cluster(cluster, MCUCFG_INITARCH);
+
+	if (arm64) {
+		mmio_setbits_32(reg, MCUCFG_INITARCH_CPU_BIT(cpu));
+	} else {
+		mmio_clrbits_32(reg, MCUCFG_INITARCH_CPU_BIT(cpu));
+	}
+}
+
+/*
+ * Return subsystem's power state.
+ *
+ * @mask: mask to SPM_CPU_PWR_STATUS to query the power state
+ *        of one subsystem.
+ * RETURNS:
+ * 0 (the subsys was powered off)
+ * 1 (the subsys was powered on)
+ */
+bool spm_get_powerstate(uint32_t mask)
+{
+	return (mmio_read_32(SPM_CPU_PWR_STATUS) & mask);
+}
+
+bool spm_get_cluster_powerstate(unsigned int cluster)
+{
+	assert(cluster == 0U);
+
+	return spm_get_powerstate(MP0_CPUTOP);
+}
+
+bool spm_get_cpu_powerstate(unsigned int cluster, unsigned int cpu)
+{
+	uint32_t mask = BIT(cpu);
+
+	assert(cluster == 0U);
+
+	return spm_get_powerstate(mask);
+}
+
+int spmc_init(void)
+{
+	unsigned int cpu = plat_my_core_pos();
+
+
+	INFO("SPM: enable CPC mode\n");
+
+	mmio_write_32(SPM_POWERON_CONFIG_EN, PROJECT_CODE | BCLK_CG_EN);
+
+	mmio_setbits_32(per_cpu(0, 1, SPM_CPU_PWR), PWR_RST_B);
+	mmio_setbits_32(per_cpu(0, 2, SPM_CPU_PWR), PWR_RST_B);
+	mmio_setbits_32(per_cpu(0, 3, SPM_CPU_PWR), PWR_RST_B);
+	mmio_setbits_32(per_cpu(0, 4, SPM_CPU_PWR), PWR_RST_B);
+	mmio_setbits_32(per_cpu(0, 5, SPM_CPU_PWR), PWR_RST_B);
+	mmio_setbits_32(per_cpu(0, 6, SPM_CPU_PWR), PWR_RST_B);
+	mmio_setbits_32(per_cpu(0, 7, SPM_CPU_PWR), PWR_RST_B);
+
+	mmio_clrbits_32(SPM_MCUSYS_PWR_CON, RESETPWRON_CONFIG);
+	mmio_clrbits_32(SPM_MP0_CPUTOP_PWR_CON, RESETPWRON_CONFIG);
+	mmio_clrbits_32(per_cpu(0, 0, SPM_CPU_PWR), RESETPWRON_CONFIG);
+
+	mmio_setbits_32(MCUCFG_CPC_FLOW_CTRL_CFG, CPC_CTRL_ENABLE);
+
+	/* Clear bootup cpu's cpc sw hint */
+	mmio_write_32(CPC_MCUSYS_CPU_ON_SW_HINT_CLR, BIT(cpu));
+
+	return 0;
+}
+
+/*
+ * Power on a core with specified cluster and core index
+ *
+ * @cluster: the cluster ID of the CPU which to be powered on
+ * @cpu: the CPU ID of the CPU which to be powered on
+ */
+void spm_poweron_cpu(unsigned int cluster, unsigned int cpu)
+{
+	/* info CPC that CPU hotplug on */
+	mmio_setbits_32(MCUCFG_CPC_FLOW_CTRL_CFG, SSPM_ALL_PWR_CTRL_EN);
+
+	/* Set mp0_spmc_pwr_on_cpuX = 1 */
+	mmio_setbits_32(per_cpu(cluster, cpu, SPM_CPU_PWR), PWR_ON);
+
+	/* wait for power on ack */
+	while (!spm_get_cpu_powerstate(cluster, cpu))
+		;
+
+	/* info CPC that CPU hotplug off */
+	mmio_clrbits_32(MCUCFG_CPC_FLOW_CTRL_CFG, SSPM_ALL_PWR_CTRL_EN);
+}
+
+/*
+ * Power off a core with specified cluster and core index
+ *
+ * @cluster: the cluster ID of the CPU which to be powered off
+ * @cpu: the CPU ID of the CPU which to be powered off
+ */
+void spm_poweroff_cpu(unsigned int cluster, unsigned int cpu)
+{
+	/* Set mp0_spmc_pwr_on_cpuX = 0 */
+	mmio_clrbits_32(per_cpu(cluster, cpu, SPM_CPU_PWR), PWR_ON);
+}
+
+/*
+ * Power off a cluster with specified index
+ *
+ * @cluster: the cluster index which to be powered off
+ */
+void spm_poweroff_cluster(unsigned int cluster)
+{
+	/* No need to power on/off cluster on single cluster platform */
+	assert(false);
+}
+
+/*
+ * Power on a cluster with specified index
+ *
+ * @cluster: the cluster index which to be powered on
+ */
+void spm_poweron_cluster(unsigned int cluster)
+{
+	/* No need to power on/off cluster on single cluster platform */
+	assert(false);
+}
diff --git a/plat/mediatek/mt8186/drivers/spmc/mtspmc.h b/plat/mediatek/mt8186/drivers/spmc/mtspmc.h
new file mode 100644
index 0000000..768599b
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/spmc/mtspmc.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTSPMC_H
+#define MTSPMC_H
+
+#include <stdint.h>
+
+int spmc_init(void);
+
+void spm_poweron_cpu(unsigned int cluster, unsigned int cpu);
+void spm_poweroff_cpu(unsigned int cluster, unsigned int cpu);
+
+void spm_poweroff_cluster(unsigned int cluster);
+void spm_poweron_cluster(unsigned int cluster);
+
+bool spm_get_cpu_powerstate(unsigned int cluster, unsigned int cpu);
+bool spm_get_cluster_powerstate(unsigned int cluster);
+bool spm_get_powerstate(uint32_t mask);
+
+void mcucfg_init_archstate(unsigned int cluster, unsigned int cpu, bool arm64);
+void mcucfg_set_bootaddr(unsigned int cluster, unsigned int cpu, uintptr_t bootaddr);
+uintptr_t mcucfg_get_bootaddr(unsigned int cluster, unsigned int cpu);
+
+void mcucfg_disable_gic_wakeup(unsigned int cluster, unsigned int cpu);
+void mcucfg_enable_gic_wakeup(unsigned int cluster, unsigned int cpu);
+
+#endif /* MTSPMC_H */
diff --git a/plat/mediatek/mt8186/drivers/spmc/mtspmc_private.h b/plat/mediatek/mt8186/drivers/spmc/mtspmc_private.h
new file mode 100644
index 0000000..472b54c
--- /dev/null
+++ b/plat/mediatek/mt8186/drivers/spmc/mtspmc_private.h
@@ -0,0 +1,176 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTSPMC_PRIVATE_H
+#define MTSPMC_PRIVATE_H
+
+#include <lib/utils_def.h>
+#include <platform_def.h>
+
+unsigned long read_cpuectlr(void);
+void write_cpuectlr(unsigned long cpuectlr);
+
+unsigned long read_cpupwrctlr_el1(void);
+void write_cpupwrctlr_el1(unsigned long cpuectlr);
+
+/* per_cpu/cluster helper */
+struct per_cpu_reg {
+	unsigned int cluster_addr;
+	unsigned int cpu_stride;
+};
+
+#define per_cpu(cluster, cpu, reg)	\
+	(reg[cluster].cluster_addr + (cpu << reg[cluster].cpu_stride))
+
+#define per_cluster(cluster, reg)	(reg[cluster].cluster_addr)
+
+#define SPM_REG(ofs)			(uint32_t)(SPM_BASE + (ofs))
+#define MCUCFG_REG(ofs)			(uint32_t)(MCUCFG_BASE + (ofs))
+#define INFRACFG_AO_REG(ofs)		(uint32_t)(INFRACFG_AO_BASE + (ofs))
+
+/* SPMC related registers */
+#define SPM_POWERON_CONFIG_EN		SPM_REG(0x000)
+/* bit-fields of SPM_POWERON_CONFIG_EN */
+#define PROJECT_CODE			(U(0xb16) << 16)
+#define BCLK_CG_EN			BIT(0)
+
+#define SPM_PWR_STATUS			SPM_REG(0x16c)
+#define SPM_PWR_STATUS_2ND		SPM_REG(0x170)
+#define SPM_CPU_PWR_STATUS		SPM_REG(0x174)
+
+/* bit-fields of SPM_PWR_STATUS */
+#define MD				BIT(0)
+#define CONN				BIT(1)
+#define DDRPHY				BIT(2)
+#define DISP				BIT(3)
+#define MFG				BIT(4)
+#define ISP				BIT(5)
+#define INFRA				BIT(6)
+#define VDEC				BIT(7)
+#define MP0_CPUTOP			BIT(8)
+#define MP0_CPU0			BIT(9)
+#define MP0_CPU1			BIT(10)
+#define MP0_CPU2			BIT(11)
+#define MP0_CPU3			BIT(12)
+#define MCUSYS				BIT(14)
+#define MP0_CPU4			BIT(15)
+#define MP0_CPU5			BIT(16)
+#define MP0_CPU6			BIT(17)
+#define MP0_CPU7			BIT(18)
+#define VEN				BIT(21)
+
+/* SPMC related registers */
+#define SPM_MCUSYS_PWR_CON		SPM_REG(0x200)
+#define SPM_MP0_CPUTOP_PWR_CON		SPM_REG(0x204)
+#define SPM_MP0_CPU0_PWR_CON		SPM_REG(0x208)
+#define SPM_MP0_CPU1_PWR_CON		SPM_REG(0x20c)
+#define SPM_MP0_CPU2_PWR_CON		SPM_REG(0x210)
+#define SPM_MP0_CPU3_PWR_CON		SPM_REG(0x214)
+#define SPM_MP0_CPU4_PWR_CON		SPM_REG(0x218)
+#define SPM_MP0_CPU5_PWR_CON		SPM_REG(0x21c)
+#define SPM_MP0_CPU6_PWR_CON		SPM_REG(0x220)
+#define SPM_MP0_CPU7_PWR_CON		SPM_REG(0x224)
+
+/* bit-fields of SPM_*_PWR_CON */
+#define PWR_ON_ACK			BIT(31)
+#define VPROC_EXT_OFF			BIT(7)
+#define DORMANT_EN			BIT(6)
+#define RESETPWRON_CONFIG		BIT(5)
+#define PWR_CLK_DIS			BIT(4)
+#define PWR_ON				BIT(2)
+#define PWR_RST_B			BIT(0)
+
+/* per_cpu registers for SPM_MP0_CPU_PWR_CON */
+static const struct per_cpu_reg SPM_CPU_PWR[] = {
+	{ .cluster_addr = SPM_MP0_CPU0_PWR_CON, .cpu_stride = 2U }
+};
+
+/* per_cluster registers for SPM_MP0_CPUTOP_PWR_CON */
+static const struct per_cpu_reg SPM_CLUSTER_PWR[] = {
+	{ .cluster_addr = SPM_MP0_CPUTOP_PWR_CON, .cpu_stride = 0U }
+};
+
+/* MCUCFG related registers */
+#define MCUCFG_MP0_CLUSTER_CFG5		MCUCFG_REG(0xc8e4)
+/* reset vectors */
+#define MCUCFG_MP0_CLUSTER_CFG8		MCUCFG_REG(0xc900)
+#define MCUCFG_MP0_CLUSTER_CFG10	MCUCFG_REG(0xc908)
+#define MCUCFG_MP0_CLUSTER_CFG12	MCUCFG_REG(0xc910)
+#define MCUCFG_MP0_CLUSTER_CFG14	MCUCFG_REG(0xc918)
+#define MCUCFG_MP0_CLUSTER_CFG16	MCUCFG_REG(0xc920)
+#define MCUCFG_MP0_CLUSTER_CFG18	MCUCFG_REG(0xc928)
+#define MCUCFG_MP0_CLUSTER_CFG20	MCUCFG_REG(0xc930)
+#define MCUCFG_MP0_CLUSTER_CFG22	MCUCFG_REG(0xc938)
+
+/* per_cpu registers for MCUCFG_MP0_CLUSTER_CFG */
+static const struct per_cpu_reg MCUCFG_BOOTADDR[] = {
+	{ .cluster_addr = MCUCFG_MP0_CLUSTER_CFG8, .cpu_stride = 3U }
+};
+
+/* per_cpu registers for MCUCFG_MP0_CLUSTER_CFG5 */
+static const struct per_cpu_reg MCUCFG_INITARCH[] = {
+	{ .cluster_addr = MCUCFG_MP0_CLUSTER_CFG5, .cpu_stride = 0U }
+};
+
+#define MCUCFG_INITARCH_CPU_BIT(cpu)	BIT(16U + cpu)
+/* CPC control */
+#define MCUCFG_CPC_FLOW_CTRL_CFG	MCUCFG_REG(0xa814)
+#define MCUCFG_CPC_SPMC_PWR_STATUS	MCUCFG_REG(0xa840)
+
+/* bit-fields of CPC_FLOW_CTRL_CFG */
+#define CPC_CTRL_ENABLE			BIT(16)
+#define SSPM_ALL_PWR_CTRL_EN		BIT(13) /* for cpu-hotplug */
+#define GIC_WAKEUP_IGNORE(cpu)		BIT(21 + cpu)
+
+/* bit-fields of CPC_SPMC_PWR_STATUS */
+#define CORE_SPMC_PWR_ON_ACK		GENMASK(11, 0)
+
+/* APB module infracfg_ao */
+#define INFRA_TOPAXI_PROTECTEN		INFRACFG_AO_REG(0x0220)
+#define INFRA_TOPAXI_PROTECTEN_STA0	INFRACFG_AO_REG(0x0224)
+#define INFRA_TOPAXI_PROTECTEN_STA1	INFRACFG_AO_REG(0x0228)
+#define INFRA_TOPAXI_PROTECTEN_SET	INFRACFG_AO_REG(0x02a0)
+#define INFRA_TOPAXI_PROTECTEN_CLR	INFRACFG_AO_REG(0x02a4)
+#define INFRA_TOPAXI_PROTECTEN_1	INFRACFG_AO_REG(0x0250)
+#define INFRA_TOPAXI_PROTECTEN_STA0_1	INFRACFG_AO_REG(0x0254)
+#define INFRA_TOPAXI_PROTECTEN_STA1_1	INFRACFG_AO_REG(0x0258)
+#define INFRA_TOPAXI_PROTECTEN_1_SET	INFRACFG_AO_REG(0x02a8)
+#define INFRA_TOPAXI_PROTECTEN_1_CLR	INFRACFG_AO_REG(0x02ac)
+
+/* bit-fields of INFRA_TOPAXI_PROTECTEN */
+#define MP0_SPMC_PROT_STEP1_0_MASK	BIT(12)
+#define MP0_SPMC_PROT_STEP1_1_MASK	(BIT(26) | BIT(12))
+
+/* SPARK */
+#define VOLTAGE_04			U(0x40)
+#define VOLTAGE_05			U(0x60)
+
+#define PTP3_CPU0_SPMC_SW_CFG		MCUCFG_REG(0x200)
+#define CPU0_ILDO_CONTROL5		MCUCFG_REG(0x334)
+#define CPU0_ILDO_CONTROL8		MCUCFG_REG(0x340)
+
+/* bit-fields of CPU0_ILDO_CONTROL5 */
+#define ILDO_RET_VOSEL			GENMASK(7, 0)
+
+/* bit-fields of PTP3_CPU_SPMC_SW_CFG */
+#define SW_SPARK_EN			BIT(0)
+
+/* bit-fields of CPU0_ILDO_CONTROL8 */
+#define ILDO_BYPASS_B			BIT(0)
+
+static const struct per_cpu_reg MCUCFG_SPARK[] = {
+	{ .cluster_addr = PTP3_CPU0_SPMC_SW_CFG, .cpu_stride = 11U }
+};
+
+static const struct per_cpu_reg ILDO_CONTROL5[] = {
+	{ .cluster_addr = CPU0_ILDO_CONTROL5, .cpu_stride = 11U }
+};
+
+static const struct per_cpu_reg ILDO_CONTROL8[] = {
+	{ .cluster_addr = CPU0_ILDO_CONTROL8, .cpu_stride = 11U }
+};
+
+#endif /* MTSPMC_PRIVATE_H */
diff --git a/plat/mediatek/mt8186/include/mcucfg.h b/plat/mediatek/mt8186/include/mcucfg.h
new file mode 100644
index 0000000..78a01a8
--- /dev/null
+++ b/plat/mediatek/mt8186/include/mcucfg.h
@@ -0,0 +1,255 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MCUCFG_H
+#define MCUCFG_H
+
+#ifndef __ASSEMBLER__
+#include <stdint.h>
+#endif /* __ASSEMBLER__ */
+
+#include <platform_def.h>
+
+#define MCUCFG_REG(ofs)			(uint32_t)(MCUCFG_BASE + (ofs))
+
+#define MP2_MISC_CONFIG_BOOT_ADDR_L(cpu) (MCUCFG_REG(0x2290) + ((cpu) * 8))
+#define MP2_MISC_CONFIG_BOOT_ADDR_H(cpu) (MCUCFG_REG(0x2294) + ((cpu) * 8))
+
+#define MP2_CPUCFG			MCUCFG_REG(0x2208)
+
+#define MP2_CPU0_STANDBYWFE		BIT(4)
+#define MP2_CPU1_STANDBYWFE		BIT(5)
+
+#define MP0_CPUTOP_SPMC_CTL		MCUCFG_REG(0x788)
+#define MP1_CPUTOP_SPMC_CTL		MCUCFG_REG(0x78C)
+#define MP1_CPUTOP_SPMC_SRAM_CTL	MCUCFG_REG(0x790)
+
+#define sw_spark_en			BIT(0)
+#define sw_no_wait_for_q_channel	BIT(1)
+#define sw_fsm_override			BIT(2)
+#define sw_logic_pre1_pdb		BIT(3)
+#define sw_logic_pre2_pdb		BIT(4)
+#define sw_logic_pdb			BIT(5)
+#define sw_iso				BIT(6)
+#define sw_sram_sleepb			(U(0x3F) << 7)
+#define sw_sram_isointb			BIT(13)
+#define sw_clk_dis			BIT(14)
+#define sw_ckiso			BIT(15)
+#define sw_pd				(U(0x3F) << 16)
+#define sw_hot_plug_reset		BIT(22)
+#define sw_pwr_on_override_en		BIT(23)
+#define sw_pwr_on			BIT(24)
+#define sw_coq_dis			BIT(25)
+#define logic_pdbo_all_off_ack		BIT(26)
+#define logic_pdbo_all_on_ack		BIT(27)
+#define logic_pre2_pdbo_all_on_ack	BIT(28)
+#define logic_pre1_pdbo_all_on_ack	BIT(29)
+
+
+#define CPUSYSx_CPUx_SPMC_CTL(cluster, cpu) \
+	(MCUCFG_REG(0x1c30) + cluster * 0x2000 + cpu * 4)
+
+#define CPUSYS0_CPU0_SPMC_CTL		MCUCFG_REG(0x1c30)
+#define CPUSYS0_CPU1_SPMC_CTL		MCUCFG_REG(0x1c34)
+#define CPUSYS0_CPU2_SPMC_CTL		MCUCFG_REG(0x1c38)
+#define CPUSYS0_CPU3_SPMC_CTL		MCUCFG_REG(0x1c3C)
+
+#define CPUSYS1_CPU0_SPMC_CTL		MCUCFG_REG(0x3c30)
+#define CPUSYS1_CPU1_SPMC_CTL		MCUCFG_REG(0x3c34)
+#define CPUSYS1_CPU2_SPMC_CTL		MCUCFG_REG(0x3c38)
+#define CPUSYS1_CPU3_SPMC_CTL		MCUCFG_REG(0x3c3C)
+
+#define cpu_sw_spark_en			BIT(0)
+#define cpu_sw_no_wait_for_q_channel	BIT(1)
+#define cpu_sw_fsm_override		BIT(2)
+#define cpu_sw_logic_pre1_pdb		BIT(3)
+#define cpu_sw_logic_pre2_pdb		BIT(4)
+#define cpu_sw_logic_pdb		BIT(5)
+#define cpu_sw_iso			BIT(6)
+#define cpu_sw_sram_sleepb		BIT(7)
+#define cpu_sw_sram_isointb		BIT(8)
+#define cpu_sw_clk_dis			BIT(9)
+#define cpu_sw_ckiso			BIT(10)
+#define cpu_sw_pd			(U(0x1F) << 11)
+#define cpu_sw_hot_plug_reset		BIT(16)
+#define cpu_sw_powr_on_override_en	BIT(17)
+#define cpu_sw_pwr_on			BIT(18)
+#define cpu_spark2ldo_allswoff		BIT(19)
+#define cpu_pdbo_all_on_ack		BIT(20)
+#define cpu_pre2_pdbo_allon_ack		BIT(21)
+#define cpu_pre1_pdbo_allon_ack		BIT(22)
+
+/* CPC related registers */
+#define CPC_MCUSYS_CPC_OFF_THRES		MCUCFG_REG(0xa714)
+#define CPC_MCUSYS_PWR_CTRL			MCUCFG_REG(0xa804)
+#define CPC_MCUSYS_CPC_FLOW_CTRL_CFG		MCUCFG_REG(0xa814)
+#define CPC_MCUSYS_LAST_CORE_REQ		MCUCFG_REG(0xa818)
+#define CPC_MCUSYS_MP_LAST_CORE_RESP		MCUCFG_REG(0xa81c)
+#define CPC_MCUSYS_LAST_CORE_RESP		MCUCFG_REG(0xa824)
+#define CPC_MCUSYS_PWR_ON_MASK			MCUCFG_REG(0xa828)
+#define CPC_MCUSYS_CPU_ON_SW_HINT_SET		MCUCFG_REG(0xa8a8)
+#define CPC_MCUSYS_CPU_ON_SW_HINT_CLR		MCUCFG_REG(0xa8ac)
+#define CPC_MCUSYS_CPC_DBG_SETTING		MCUCFG_REG(0xab00)
+#define CPC_MCUSYS_CPC_KERNEL_TIME_L_BASE	MCUCFG_REG(0xab04)
+#define CPC_MCUSYS_CPC_KERNEL_TIME_H_BASE	MCUCFG_REG(0xab08)
+#define CPC_MCUSYS_CPC_SYSTEM_TIME_L_BASE	MCUCFG_REG(0xab0c)
+#define CPC_MCUSYS_CPC_SYSTEM_TIME_H_BASE	MCUCFG_REG(0xab10)
+#define CPC_MCUSYS_TRACE_SEL			MCUCFG_REG(0xab14)
+#define CPC_MCUSYS_TRACE_DATA			MCUCFG_REG(0xab20)
+#define CPC_MCUSYS_CLUSTER_COUNTER		MCUCFG_REG(0xab70)
+#define CPC_MCUSYS_CLUSTER_COUNTER_CLR		MCUCFG_REG(0xab74)
+#define SPARK2LDO				MCUCFG_REG(0x2700)
+/* APB module mcucfg */
+#define MP0_CA7_CACHE_CONFIG		MCUCFG_REG(0x000)
+#define MP0_AXI_CONFIG			MCUCFG_REG(0x02C)
+#define MP0_MISC_CONFIG0		MCUCFG_REG(0x030)
+#define MP0_MISC_CONFIG1		MCUCFG_REG(0x034)
+#define MP0_MISC_CONFIG2		MCUCFG_REG(0x038)
+#define MP0_MISC_CONFIG_BOOT_ADDR(cpu)	(MP0_MISC_CONFIG2 + ((cpu) * 8))
+#define MP0_MISC_CONFIG3		MCUCFG_REG(0x03C)
+#define MP0_MISC_CONFIG9		MCUCFG_REG(0x054)
+#define MP0_CA7_MISC_CONFIG		MCUCFG_REG(0x064)
+
+#define MP0_RW_RSVD0			MCUCFG_REG(0x06C)
+
+
+#define MP1_CA7_CACHE_CONFIG		MCUCFG_REG(0x200)
+#define MP1_AXI_CONFIG			MCUCFG_REG(0x22C)
+#define MP1_MISC_CONFIG0		MCUCFG_REG(0x230)
+#define MP1_MISC_CONFIG1		MCUCFG_REG(0x234)
+#define MP1_MISC_CONFIG2		MCUCFG_REG(0x238)
+#define MP1_MISC_CONFIG_BOOT_ADDR(cpu)	(MP1_MISC_CONFIG2 + ((cpu) * 8))
+#define MP1_MISC_CONFIG3		MCUCFG_REG(0x23C)
+#define MP1_MISC_CONFIG9		MCUCFG_REG(0x254)
+#define MP1_CA7_MISC_CONFIG		MCUCFG_REG(0x264)
+
+#define CCI_ADB400_DCM_CONFIG		MCUCFG_REG(0x740)
+#define SYNC_DCM_CONFIG			MCUCFG_REG(0x744)
+
+#define MP0_CLUSTER_CFG0		MCUCFG_REG(0xC8D0)
+
+#define MP0_SPMC			MCUCFG_REG(0x788)
+#define MP1_SPMC			MCUCFG_REG(0x78C)
+#define MP2_AXI_CONFIG			MCUCFG_REG(0x220C)
+#define MP2_AXI_CONFIG_ACINACTM		BIT(0)
+#define MP2_AXI_CONFIG_AINACTS		BIT(4)
+
+#define MPx_AXI_CONFIG_ACINACTM			BIT(4)
+#define MPx_AXI_CONFIG_AINACTS			BIT(5)
+#define MPx_CA7_MISC_CONFIG_standbywfil2	BIT(28)
+
+#define MP0_CPU0_STANDBYWFE		BIT(20)
+#define MP0_CPU1_STANDBYWFE		BIT(21)
+#define MP0_CPU2_STANDBYWFE		BIT(22)
+#define MP0_CPU3_STANDBYWFE		BIT(23)
+
+#define MP1_CPU0_STANDBYWFE		BIT(20)
+#define MP1_CPU1_STANDBYWFE		BIT(21)
+#define MP1_CPU2_STANDBYWFE		BIT(22)
+#define MP1_CPU3_STANDBYWFE		BIT(23)
+
+#define CPUSYS0_SPARKVRETCNTRL		MCUCFG_REG(0x1c00)
+#define CPUSYS0_SPARKEN			MCUCFG_REG(0x1c04)
+#define CPUSYS0_AMUXSEL			MCUCFG_REG(0x1c08)
+#define CPUSYS1_SPARKVRETCNTRL		MCUCFG_REG(0x3c00)
+#define CPUSYS1_SPARKEN			MCUCFG_REG(0x3c04)
+#define CPUSYS1_AMUXSEL			MCUCFG_REG(0x3c08)
+
+#define MP2_PWR_RST_CTL			MCUCFG_REG(0x2008)
+#define MP2_PTP3_CPUTOP_SPMC0		MCUCFG_REG(0x22A0)
+#define MP2_PTP3_CPUTOP_SPMC1		MCUCFG_REG(0x22A4)
+
+#define MP2_COQ				MCUCFG_REG(0x22BC)
+#define MP2_COQ_SW_DIS			BIT(0)
+
+#define MP2_CA15M_MON_SEL		MCUCFG_REG(0x2400)
+#define MP2_CA15M_MON_L			MCUCFG_REG(0x2404)
+
+#define CPUSYS2_CPU0_SPMC_CTL		MCUCFG_REG(0x2430)
+#define CPUSYS2_CPU1_SPMC_CTL		MCUCFG_REG(0x2438)
+#define CPUSYS2_CPU0_SPMC_STA		MCUCFG_REG(0x2434)
+#define CPUSYS2_CPU1_SPMC_STA		MCUCFG_REG(0x243C)
+
+#define MP0_CA7L_DBG_PWR_CTRL		MCUCFG_REG(0x068)
+#define MP1_CA7L_DBG_PWR_CTRL		MCUCFG_REG(0x268)
+#define BIG_DBG_PWR_CTRL		MCUCFG_REG(0x75C)
+
+#define MP2_SW_RST_B			BIT(0)
+#define MP2_TOPAON_APB_MASK		BIT(1)
+
+#define B_SW_HOT_PLUG_RESET		BIT(30)
+
+#define B_SW_PD_OFFSET			(18U)
+#define B_SW_PD				(U(0x3f) << B_SW_PD_OFFSET)
+
+#define B_SW_SRAM_SLEEPB_OFFSET		(12U)
+#define B_SW_SRAM_SLEEPB		(U(0x3f) << B_SW_SRAM_SLEEPB_OFFSET)
+
+#define B_SW_SRAM_ISOINTB		BIT(9)
+#define B_SW_ISO			BIT(8)
+#define B_SW_LOGIC_PDB			BIT(7)
+#define B_SW_LOGIC_PRE2_PDB		BIT(6)
+#define B_SW_LOGIC_PRE1_PDB		BIT(5)
+#define B_SW_FSM_OVERRIDE		BIT(4)
+#define B_SW_PWR_ON			BIT(3)
+#define B_SW_PWR_ON_OVERRIDE_EN		BIT(2)
+
+#define B_FSM_STATE_OUT_OFFSET		(6U)
+#define B_FSM_STATE_OUT_MASK		(U(0x1f) << B_FSM_STATE_OUT_OFFSET)
+#define B_SW_LOGIC_PDBO_ALL_OFF_ACK	BIT(5)
+#define B_SW_LOGIC_PDBO_ALL_ON_ACK	BIT(4)
+#define B_SW_LOGIC_PRE2_PDBO_ALL_ON_ACK	BIT(3)
+#define B_SW_LOGIC_PRE1_PDBO_ALL_ON_ACK	BIT(2)
+
+#define B_FSM_OFF			(0U << B_FSM_STATE_OUT_OFFSET)
+#define B_FSM_ON			(1U << B_FSM_STATE_OUT_OFFSET)
+#define B_FSM_RET			(2U << B_FSM_STATE_OUT_OFFSET)
+
+#ifndef __ASSEMBLER__
+/* cpu boot mode */
+enum {
+	MP0_CPUCFG_64BIT_SHIFT = 12U,
+	MP1_CPUCFG_64BIT_SHIFT = 28U,
+	MP0_CPUCFG_64BIT = U(0xf) << MP0_CPUCFG_64BIT_SHIFT,
+	MP1_CPUCFG_64BIT = U(0xf) << MP1_CPUCFG_64BIT_SHIFT
+};
+
+enum {
+	MP1_DIS_RGU0_WAIT_PD_CPUS_L1_ACK_SHIFT = 0U,
+	MP1_DIS_RGU1_WAIT_PD_CPUS_L1_ACK_SHIFT = 4U,
+	MP1_DIS_RGU2_WAIT_PD_CPUS_L1_ACK_SHIFT = 8U,
+	MP1_DIS_RGU3_WAIT_PD_CPUS_L1_ACK_SHIFT = 12U,
+	MP1_DIS_RGU_NOCPU_WAIT_PD_CPUS_L1_ACK_SHIFT = 16U,
+
+	MP1_DIS_RGU0_WAIT_PD_CPUS_L1_ACK =
+		U(0xf) << MP1_DIS_RGU0_WAIT_PD_CPUS_L1_ACK_SHIFT,
+	MP1_DIS_RGU1_WAIT_PD_CPUS_L1_ACK =
+		U(0xf) << MP1_DIS_RGU1_WAIT_PD_CPUS_L1_ACK_SHIFT,
+	MP1_DIS_RGU2_WAIT_PD_CPUS_L1_ACK =
+		U(0xf) << MP1_DIS_RGU2_WAIT_PD_CPUS_L1_ACK_SHIFT,
+	MP1_DIS_RGU3_WAIT_PD_CPUS_L1_ACK =
+		U(0xf) << MP1_DIS_RGU3_WAIT_PD_CPUS_L1_ACK_SHIFT,
+	MP1_DIS_RGU_NOCPU_WAIT_PD_CPUS_L1_ACK =
+		U(0xf) << MP1_DIS_RGU_NOCPU_WAIT_PD_CPUS_L1_ACK_SHIFT
+};
+
+enum {
+	MP1_AINACTS_SHIFT = 4U,
+	MP1_AINACTS = 1U << MP1_AINACTS_SHIFT
+};
+
+enum {
+	MP1_SW_CG_GEN_SHIFT = 12U,
+	MP1_SW_CG_GEN = 1U << MP1_SW_CG_GEN_SHIFT
+};
+
+enum {
+	MP1_L2RSTDISABLE_SHIFT = 14U,
+	MP1_L2RSTDISABLE = 1U << MP1_L2RSTDISABLE_SHIFT
+};
+#endif /* __ASSEMBLER__ */
+
+#endif  /* MCUCFG_H */
diff --git a/plat/mediatek/mt8186/include/plat_mtk_lpm.h b/plat/mediatek/mt8186/include/plat_mtk_lpm.h
new file mode 100644
index 0000000..347f358
--- /dev/null
+++ b/plat/mediatek/mt8186/include/plat_mtk_lpm.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_MTK_LPM_H
+#define PLAT_MTK_LPM_H
+
+#include <lib/psci/psci.h>
+#include <lib/utils_def.h>
+
+#define MT_IRQ_REMAIN_MAX	U(32)
+#define MT_IRQ_REMAIN_CAT_LOG	BIT(31)
+
+struct mt_irqremain {
+	unsigned int count;
+	unsigned int irqs[MT_IRQ_REMAIN_MAX];
+	unsigned int wakeupsrc_cat[MT_IRQ_REMAIN_MAX];
+	unsigned int wakeupsrc[MT_IRQ_REMAIN_MAX];
+};
+
+#define PLAT_RC_STATUS_READY		BIT(0)
+#define PLAT_RC_STATUS_FEATURE_EN	BIT(1)
+#define PLAT_RC_STATUS_UART_NONSLEEP	BIT(31)
+
+struct mt_lpm_tz {
+	int (*pwr_prompt)(unsigned int cpu, const psci_power_state_t *state);
+	int (*pwr_reflect)(unsigned int cpu, const psci_power_state_t *state);
+
+	int (*pwr_cpu_on)(unsigned int cpu, const psci_power_state_t *state);
+	int (*pwr_cpu_dwn)(unsigned int cpu, const psci_power_state_t *state);
+
+	int (*pwr_cluster_on)(unsigned int cpu,
+					const psci_power_state_t *state);
+	int (*pwr_cluster_dwn)(unsigned int cpu,
+					const psci_power_state_t *state);
+
+	int (*pwr_mcusys_on)(unsigned int cpu, const psci_power_state_t *state);
+	int (*pwr_mcusys_on_finished)(unsigned int cpu,
+					const psci_power_state_t *state);
+	int (*pwr_mcusys_dwn)(unsigned int cpu,
+					const psci_power_state_t *state);
+};
+
+const struct mt_lpm_tz *mt_plat_cpu_pm_init(void);
+
+#endif /* PLAT_MTK_LPM_H */
diff --git a/plat/mediatek/mt8186/include/plat_pm.h b/plat/mediatek/mt8186/include/plat_pm.h
new file mode 100644
index 0000000..436db34
--- /dev/null
+++ b/plat/mediatek/mt8186/include/plat_pm.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_PM_H
+#define PLAT_PM_H
+
+#include <lib/utils_def.h>
+
+#define MT_PLAT_PWR_STATE_CPU			U(1)
+#define MT_PLAT_PWR_STATE_CLUSTER		U(2)
+#define MT_PLAT_PWR_STATE_MCUSYS		U(3)
+#define MT_PLAT_PWR_STATE_SUSPEND2IDLE		U(8)
+#define MT_PLAT_PWR_STATE_SYSTEM_SUSPEND	U(9)
+
+#define MTK_LOCAL_STATE_RUN			U(0)
+#define MTK_LOCAL_STATE_RET			U(1)
+#define MTK_LOCAL_STATE_OFF			U(2)
+
+#define MTK_AFFLVL_CPU				U(0)
+#define MTK_AFFLVL_CLUSTER			U(1)
+#define MTK_AFFLVL_MCUSYS			U(2)
+#define MTK_AFFLVL_SYSTEM			U(3)
+
+#define IS_CLUSTER_OFF_STATE(s)					\
+		is_local_state_off(s->pwr_domain_state[MTK_AFFLVL_CLUSTER])
+#define IS_MCUSYS_OFF_STATE(s)					\
+		is_local_state_off(s->pwr_domain_state[MTK_AFFLVL_MCUSYS])
+#define IS_SYSTEM_SUSPEND_STATE(s)				\
+		is_local_state_off(s->pwr_domain_state[MTK_AFFLVL_SYSTEM])
+
+#define IS_PLAT_SUSPEND_ID(stateid)				\
+		((stateid == MT_PLAT_PWR_STATE_SUSPEND2IDLE)	\
+		|| (stateid == MT_PLAT_PWR_STATE_SYSTEM_SUSPEND))
+
+#endif /* PLAT_PM_H */
diff --git a/plat/mediatek/mt8186/include/plat_sip_calls.h b/plat/mediatek/mt8186/include/plat_sip_calls.h
new file mode 100644
index 0000000..598a5b8
--- /dev/null
+++ b/plat/mediatek/mt8186/include/plat_sip_calls.h
@@ -0,0 +1,15 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_SIP_CALLS_H
+#define PLAT_SIP_CALLS_H
+
+/*******************************************************************************
+ * Plat SiP function constants
+ ******************************************************************************/
+#define MTK_PLAT_SIP_NUM_CALLS    0
+
+#endif /* PLAT_SIP_CALLS_H */
diff --git a/plat/mediatek/mt8186/include/platform_def.h b/plat/mediatek/mt8186/include/platform_def.h
index bf1bbef..479a8d4 100644
--- a/plat/mediatek/mt8186/include/platform_def.h
+++ b/plat/mediatek/mt8186/include/platform_def.h
@@ -15,12 +15,22 @@
 
 /* Aggregate of all devices for MMU mapping */
 #define MTK_DEV_RNG0_BASE	IO_PHYS
-#define MTK_DEV_RNG0_SIZE	0x400000
-#define MTK_DEV_RNG1_BASE	(IO_PHYS + 0x1000000)
-#define MTK_DEV_RNG1_SIZE	0xa110000
+#define MTK_DEV_RNG0_SIZE	0x10000000
 #define MTK_DEV_RNG2_BASE	MT_GIC_BASE
 #define MTK_DEV_RNG2_SIZE	0x600000
 
+#define SPM_BASE		(IO_PHYS + 0x00006000)
+
+/*******************************************************************************
+ * GPIO related constants
+ ******************************************************************************/
+#define GPIO_BASE		(IO_PHYS + 0x00005000)
+#define IOCFG_LT_BASE		(IO_PHYS + 0x00002000)
+#define IOCFG_LM_BASE		(IO_PHYS + 0x00002200)
+#define IOCFG_LB_BASE		(IO_PHYS + 0x00002400)
+#define IOCFG_BL_BASE		(IO_PHYS + 0x00002600)
+#define IOCFG_RB_BASE		(IO_PHYS + 0x00002A00)
+#define IOCFG_RT_BASE		(IO_PHYS + 0x00002C00)
 
 /*******************************************************************************
  * UART related constants
@@ -30,6 +40,28 @@
 #define UART_BAUDRATE			115200
 
 /*******************************************************************************
+ * PWRAP related constants
+ ******************************************************************************/
+#define PMIC_WRAP_BASE			(IO_PHYS + 0x0000D000)
+
+/*******************************************************************************
+ * EMI MPU related constants
+ ******************************************************************************/
+#define EMI_MPU_BASE		(IO_PHYS + 0x0021B000)
+
+/*******************************************************************************
+ * GIC-600 & interrupt handling related constants
+ ******************************************************************************/
+/* Base MTK_platform compatible GIC memory map */
+#define BASE_GICD_BASE			MT_GIC_BASE
+#define MT_GIC_RDIST_BASE		(MT_GIC_BASE + 0x40000)
+
+#define SYS_CIRQ_BASE			(IO_PHYS + 0x204000)
+#define CIRQ_REG_NUM			11
+#define CIRQ_IRQ_NUM			326
+#define CIRQ_SPI_START			64
+#define MD_WDT_IRQ_BIT_ID		107
+/*******************************************************************************
  * System counter frequency related constants
  ******************************************************************************/
 #define SYS_COUNTER_FREQ_IN_TICKS	13000000
diff --git a/plat/mediatek/mt8186/plat_pm.c b/plat/mediatek/mt8186/plat_pm.c
index 61d2cc9..e8d834d 100644
--- a/plat/mediatek/mt8186/plat_pm.c
+++ b/plat/mediatek/mt8186/plat_pm.c
@@ -4,15 +4,397 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+/* common headers */
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/gpio.h>
 #include <lib/psci/psci.h>
 
+/* platform specific headers */
+#include <mt_gic_v3.h>
+#include <mtspmc.h>
+#include <plat/common/platform.h>
+#include <plat_mtk_lpm.h>
+#include <plat_params.h>
+#include <plat_pm.h>
+#include <pmic.h>
+#include <rtc.h>
+
+/*
+ * Cluster state request:
+ * [0] : The CPU requires cluster power down
+ * [1] : The CPU requires cluster power on
+ */
+#define coordinate_cluster(onoff)	write_clusterpwrdn_el1(onoff)
+#define coordinate_cluster_pwron()	coordinate_cluster(1)
+#define coordinate_cluster_pwroff()	coordinate_cluster(0)
+
+/* platform secure entry point */
+static uintptr_t secure_entrypoint;
+/* per-CPU power state */
+static unsigned int plat_power_state[PLATFORM_CORE_COUNT];
+
+/* platform CPU power domain - ops */
+static const struct mt_lpm_tz *plat_mt_pm;
+
+static inline int plat_mt_pm_invoke(int (*func)(unsigned int cpu,
+						const psci_power_state_t *state),
+				    int cpu, const psci_power_state_t *state)
+{
+	int ret = -1;
+
+	if (func != NULL) {
+		ret = func(cpu, state);
+	}
+	return ret;
+}
+
+/*
+ * Common MTK_platform operations to power on/off a
+ * CPU in response to a CPU_ON, CPU_OFF or CPU_SUSPEND request.
+ */
+static void plat_cpu_pwrdwn_common(unsigned int cpu,
+		const psci_power_state_t *state, unsigned int req_pstate)
+{
+	assert(cpu == plat_my_core_pos());
+	assert(plat_mt_pm != NULL);
+
+	(void)plat_mt_pm_invoke(plat_mt_pm->pwr_cpu_dwn, cpu, state);
+
+	if ((psci_get_pstate_pwrlvl(req_pstate) >= MTK_AFFLVL_CLUSTER) ||
+			(req_pstate == 0U)) { /* hotplug off */
+		coordinate_cluster_pwroff();
+	}
+
+	/* Prevent interrupts from spuriously waking up this CPU */
+	mt_gic_rdistif_save();
+	gicv3_cpuif_disable(cpu);
+	gicv3_rdistif_off(cpu);
+}
+
+static void plat_cpu_pwron_common(unsigned int cpu,
+		const psci_power_state_t *state, unsigned int req_pstate)
+{
+	assert(cpu == plat_my_core_pos());
+	assert(plat_mt_pm != NULL);
+
+	(void)plat_mt_pm_invoke(plat_mt_pm->pwr_cpu_on, cpu, state);
+
+	coordinate_cluster_pwron();
+
+	/*
+	 * If mcusys does power down before then restore
+	 * all CPUs' GIC Redistributors
+	 */
+	if (IS_MCUSYS_OFF_STATE(state)) {
+		mt_gic_rdistif_restore_all();
+	} else {
+		gicv3_rdistif_on(cpu);
+		gicv3_cpuif_enable(cpu);
+		mt_gic_rdistif_init();
+		mt_gic_rdistif_restore();
+	}
+}
+
+/*
+ * Common MTK_platform operations to power on/off a
+ * cluster in response to a CPU_ON, CPU_OFF or CPU_SUSPEND request.
+ */
+static void plat_cluster_pwrdwn_common(unsigned int cpu,
+		const psci_power_state_t *state, unsigned int req_pstate)
+{
+	assert(cpu == plat_my_core_pos());
+	assert(plat_mt_pm != NULL);
+
+	if (plat_mt_pm_invoke(plat_mt_pm->pwr_cluster_dwn, cpu, state) != 0) {
+		coordinate_cluster_pwron();
+
+		/*
+		 * TODO:
+		 * Return on fail and add a 'return' here before
+		 * adding any code following the if-block.
+		 */
+	}
+}
+
+static void plat_cluster_pwron_common(unsigned int cpu,
+		const psci_power_state_t *state, unsigned int req_pstate)
+{
+	assert(cpu == plat_my_core_pos());
+	assert(plat_mt_pm != NULL);
+
+	if (plat_mt_pm_invoke(plat_mt_pm->pwr_cluster_on, cpu, state) != 0) {
+		/*
+		 * TODO:
+		 * return on fail and add a 'return' here before
+		 * adding any code following the if-block.
+		 */
+	}
+}
+
+/*
+ * Common MTK_platform operations to power on/off a
+ * mcusys in response to a CPU_ON, CPU_OFF or CPU_SUSPEND request.
+ */
+static void plat_mcusys_pwrdwn_common(unsigned int cpu,
+		const psci_power_state_t *state, unsigned int req_pstate)
+{
+	assert(cpu == plat_my_core_pos());
+	assert(plat_mt_pm != NULL);
+
+	if (plat_mt_pm_invoke(plat_mt_pm->pwr_mcusys_dwn, cpu, state) != 0) {
+		return;		/* return on fail */
+	}
+
+	mt_gic_distif_save();
+	gic_sgi_save_all();
+}
+
+static void plat_mcusys_pwron_common(unsigned int cpu,
+		const psci_power_state_t *state, unsigned int req_pstate)
+{
+	assert(cpu == plat_my_core_pos());
+	assert(plat_mt_pm != NULL);
+
+	if (plat_mt_pm_invoke(plat_mt_pm->pwr_mcusys_on, cpu, state) != 0) {
+		/* return on fail */
+		return;
+	}
+
+	mt_gic_init();
+	mt_gic_distif_restore();
+	gic_sgi_restore_all();
+
+	(void)plat_mt_pm_invoke(plat_mt_pm->pwr_mcusys_on_finished, cpu, state);
+}
+
+/* plat_psci_ops implementation */
+static void plat_cpu_standby(plat_local_state_t cpu_state)
+{
+	uint64_t scr;
+
+	scr = read_scr_el3();
+	write_scr_el3(scr | SCR_IRQ_BIT | SCR_FIQ_BIT);
+
+	isb();
+	dsb();
+	wfi();
+
+	write_scr_el3(scr);
+}
+
+static int plat_power_domain_on(u_register_t mpidr)
+{
+	unsigned int cpu = (unsigned int)plat_core_pos_by_mpidr(mpidr);
+	unsigned int cluster = 0U;
+
+	if (cpu >= PLATFORM_CORE_COUNT) {
+		return PSCI_E_INVALID_PARAMS;
+	}
+
+	if (!spm_get_cluster_powerstate(cluster)) {
+		spm_poweron_cluster(cluster);
+	}
+
+	/* init CPU reset arch as AARCH64 */
+	mcucfg_init_archstate(cluster, cpu, true);
+	mcucfg_set_bootaddr(cluster, cpu, secure_entrypoint);
+	spm_poweron_cpu(cluster, cpu);
+
+	return PSCI_E_SUCCESS;
+}
+
+static void plat_power_domain_on_finish(const psci_power_state_t *state)
+{
+	unsigned long mpidr = read_mpidr_el1();
+	unsigned int cpu = (unsigned int)plat_core_pos_by_mpidr(mpidr);
+
+	assert(cpu < PLATFORM_CORE_COUNT);
+
+	/* Allow IRQs to wakeup this core in IDLE flow */
+	mcucfg_enable_gic_wakeup(0U, cpu);
+
+	if (IS_CLUSTER_OFF_STATE(state)) {
+		plat_cluster_pwron_common(cpu, state, 0U);
+	}
+
+	plat_cpu_pwron_common(cpu, state, 0U);
+}
+
+static void plat_power_domain_off(const psci_power_state_t *state)
+{
+	unsigned long mpidr = read_mpidr_el1();
+	unsigned int cpu = (unsigned int)plat_core_pos_by_mpidr(mpidr);
+
+	assert(cpu < PLATFORM_CORE_COUNT);
+
+	plat_cpu_pwrdwn_common(cpu, state, 0U);
+	spm_poweroff_cpu(0U, cpu);
+
+	/* prevent unintended IRQs from waking up the hot-unplugged core */
+	mcucfg_disable_gic_wakeup(0U, cpu);
+
+	if (IS_CLUSTER_OFF_STATE(state)) {
+		plat_cluster_pwrdwn_common(cpu, state, 0U);
+	}
+}
+
+static void plat_power_domain_suspend(const psci_power_state_t *state)
+{
+	unsigned int cpu = plat_my_core_pos();
+
+	assert(cpu < PLATFORM_CORE_COUNT);
+	assert(plat_mt_pm != NULL);
+
+	(void)plat_mt_pm_invoke(plat_mt_pm->pwr_prompt, cpu, state);
+
+	/* Perform the common CPU specific operations */
+	plat_cpu_pwrdwn_common(cpu, state, plat_power_state[cpu]);
+
+	if (IS_CLUSTER_OFF_STATE(state)) {
+		/* Perform the common cluster specific operations */
+		plat_cluster_pwrdwn_common(cpu, state, plat_power_state[cpu]);
+	}
+
+	if (IS_MCUSYS_OFF_STATE(state)) {
+		/* Perform the common mcusys specific operations */
+		plat_mcusys_pwrdwn_common(cpu, state, plat_power_state[cpu]);
+	}
+}
+
+static void plat_power_domain_suspend_finish(const psci_power_state_t *state)
+{
+	unsigned int cpu = plat_my_core_pos();
+
+	assert(cpu < PLATFORM_CORE_COUNT);
+	assert(plat_mt_pm != NULL);
+
+	if (IS_MCUSYS_OFF_STATE(state)) {
+		/* Perform the common mcusys specific operations */
+		plat_mcusys_pwron_common(cpu, state, plat_power_state[cpu]);
+	}
+
+	if (IS_CLUSTER_OFF_STATE(state)) {
+		/* Perform the common cluster specific operations */
+		plat_cluster_pwron_common(cpu, state, plat_power_state[cpu]);
+	}
+
+	/* Perform the common CPU specific operations */
+	plat_cpu_pwron_common(cpu, state, plat_power_state[cpu]);
+
+	(void)plat_mt_pm_invoke(plat_mt_pm->pwr_reflect, cpu, state);
+}
+
+static int plat_validate_power_state(unsigned int power_state,
+					psci_power_state_t *req_state)
+{
+	unsigned int pstate = psci_get_pstate_type(power_state);
+	unsigned int aff_lvl = psci_get_pstate_pwrlvl(power_state);
+	unsigned int cpu = plat_my_core_pos();
+
+	if (aff_lvl > PLAT_MAX_PWR_LVL) {
+		return PSCI_E_INVALID_PARAMS;
+	}
+
+	if (pstate == PSTATE_TYPE_STANDBY) {
+		req_state->pwr_domain_state[0] = PLAT_MAX_RET_STATE;
+	} else {
+		unsigned int i;
+		unsigned int pstate_id = psci_get_pstate_id(power_state);
+		plat_local_state_t s = MTK_LOCAL_STATE_OFF;
+
+		/* Use pstate_id to be power domain state */
+		if (pstate_id > s) {
+			s = (plat_local_state_t)pstate_id;
+		}
+
+		for (i = 0U; i <= aff_lvl; i++) {
+			req_state->pwr_domain_state[i] = s;
+		}
+	}
+
+	plat_power_state[cpu] = power_state;
+	return PSCI_E_SUCCESS;
+}
+
+static void plat_get_sys_suspend_power_state(psci_power_state_t *req_state)
+{
+	unsigned int lv;
+	unsigned int cpu = plat_my_core_pos();
+
+	for (lv = PSCI_CPU_PWR_LVL; lv <= PLAT_MAX_PWR_LVL; lv++) {
+		req_state->pwr_domain_state[lv] = PLAT_MAX_OFF_STATE;
+	}
+
+	plat_power_state[cpu] =
+			psci_make_powerstate(
+				MT_PLAT_PWR_STATE_SYSTEM_SUSPEND,
+				PSTATE_TYPE_POWERDOWN, PLAT_MAX_PWR_LVL);
+
+	flush_dcache_range((uintptr_t)
+			&plat_power_state[cpu],
+			sizeof(plat_power_state[cpu]));
+}
+
+/*******************************************************************************
+ * MTK handlers to shutdown/reboot the system
+ ******************************************************************************/
+static void __dead2 plat_mtk_system_reset(void)
+{
+	struct bl_aux_gpio_info *gpio_reset = plat_get_mtk_gpio_reset();
+
+	INFO("MTK System Reset\n");
+
+	gpio_set_value(gpio_reset->index, gpio_reset->polarity);
+
+	wfi();
+	ERROR("MTK System Reset: operation not handled.\n");
+	panic();
+}
+
+static void __dead2 plat_mtk_system_off(void)
+{
+	INFO("MTK System Off\n");
+
+	rtc_power_off_sequence();
+	pmic_power_off();
+
+	wfi();
+	ERROR("MTK System Off: operation not handled.\n");
+	panic();
+}
+
 static const plat_psci_ops_t plat_psci_ops = {
+	.cpu_standby			= plat_cpu_standby,
+	.pwr_domain_on			= plat_power_domain_on,
+	.pwr_domain_on_finish		= plat_power_domain_on_finish,
+	.pwr_domain_off			= plat_power_domain_off,
+	.pwr_domain_suspend		= plat_power_domain_suspend,
+	.pwr_domain_suspend_finish	= plat_power_domain_suspend_finish,
+	.validate_power_state		= plat_validate_power_state,
+	.get_sys_suspend_power_state	= plat_get_sys_suspend_power_state,
+	.system_off			= plat_mtk_system_off,
+	.system_reset			= plat_mtk_system_reset,
 };
 
 int plat_setup_psci_ops(uintptr_t sec_entrypoint,
 			const plat_psci_ops_t **psci_ops)
 {
 	*psci_ops = &plat_psci_ops;
+	secure_entrypoint = sec_entrypoint;
+
+	/*
+	 * init the warm reset config for boot CPU
+	 * reset arch as AARCH64
+	 * reset addr as function bl31_warm_entrypoint()
+	 */
+	mcucfg_init_archstate(0U, 0U, true);
+	mcucfg_set_bootaddr(0U, 0U, secure_entrypoint);
+
+	spmc_init();
+	plat_mt_pm = mt_plat_cpu_pm_init();
 
 	return 0;
 }
diff --git a/plat/mediatek/mt8186/plat_sip_calls.c b/plat/mediatek/mt8186/plat_sip_calls.c
new file mode 100644
index 0000000..716f3d9
--- /dev/null
+++ b/plat/mediatek/mt8186/plat_sip_calls.c
@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+
+uintptr_t mediatek_plat_sip_handler(uint32_t smc_fid,
+				u_register_t x1,
+				u_register_t x2,
+				u_register_t x3,
+				u_register_t x4,
+				void *cookie,
+				void *handle,
+				u_register_t flags)
+{
+	switch (smc_fid) {
+	default:
+		ERROR("%s: unhandled SMC (0x%x)\n", __func__, smc_fid);
+		break;
+	}
+
+	SMC_RET1(handle, SMC_UNK);
+}
diff --git a/plat/mediatek/mt8186/platform.mk b/plat/mediatek/mt8186/platform.mk
index 2d20213..79c6834 100644
--- a/plat/mediatek/mt8186/platform.mk
+++ b/plat/mediatek/mt8186/platform.mk
@@ -8,8 +8,19 @@
 MTK_PLAT_SOC := ${MTK_PLAT}/${PLAT}
 
 PLAT_INCLUDES := -I${MTK_PLAT}/common/                            \
+                 -I${MTK_PLAT}/common/drivers/gic600/             \
+                 -I${MTK_PLAT}/common/drivers/gpio/             \
+                 -I${MTK_PLAT}/common/drivers/timer/              \
+                 -I${MTK_PLAT_SOC}/drivers/dcm/                   \
+                 -I${MTK_PLAT_SOC}/drivers/emi_mpu/               \
+                 -I${MTK_PLAT_SOC}/drivers/gpio/               \
+                 -I${MTK_PLAT_SOC}/drivers/mcdi/                  \
+                 -I${MTK_PLAT_SOC}/drivers/pmic/                  \
+                 -I${MTK_PLAT_SOC}/drivers/rtc/                   \
+                 -I${MTK_PLAT_SOC}/drivers/spmc/                  \
                  -I${MTK_PLAT_SOC}/include/
 
+GICV3_SUPPORT_GIC600        :=      1
 include drivers/arm/gic/v3/gicv3.mk
 include lib/xlat_tables_v2/xlat_tables.mk
 
@@ -20,17 +31,38 @@
 
 
 BL31_SOURCES += common/desc_image_load.c                              \
+                drivers/delay_timer/delay_timer.c                     \
+                drivers/gpio/gpio.c                                   \
+                drivers/delay_timer/generic_delay_timer.c             \
                 drivers/ti/uart/aarch64/16550_console.S               \
                 lib/bl_aux_params/bl_aux_params.c                     \
                 lib/cpus/aarch64/cortex_a55.S                         \
                 lib/cpus/aarch64/cortex_a76.S                         \
                 plat/common/plat_gicv3.c                              \
+                ${MTK_PLAT}/common/drivers/gic600/mt_gic_v3.c         \
+                ${MTK_PLAT}/common/drivers/gpio/mtgpio_common.c       \
+                ${MTK_PLAT}/common/drivers/pmic_wrap/pmic_wrap_init.c \
+                ${MTK_PLAT}/common/drivers/rtc/rtc_common.c           \
                 ${MTK_PLAT}/common/mtk_plat_common.c                  \
+                ${MTK_PLAT}/common/mtk_sip_svc.c                      \
                 ${MTK_PLAT}/common/params_setup.c                     \
+                ${MTK_PLAT}/common/drivers/timer/mt_timer.c           \
+                ${MTK_PLAT}/common/mtk_cirq.c           	      \
                 ${MTK_PLAT_SOC}/aarch64/platform_common.c             \
                 ${MTK_PLAT_SOC}/aarch64/plat_helpers.S                \
                 ${MTK_PLAT_SOC}/bl31_plat_setup.c                     \
+                ${MTK_PLAT_SOC}/drivers/dcm/mtk_dcm.c                 \
+                ${MTK_PLAT_SOC}/drivers/dcm/mtk_dcm_utils.c           \
+                ${MTK_PLAT_SOC}/drivers/emi_mpu/emi_mpu.c             \
+                ${MTK_PLAT_SOC}/drivers/gpio/mtgpio.c                 \
+                ${MTK_PLAT_SOC}/drivers/mcdi/mt_cpu_pm.c              \
+                ${MTK_PLAT_SOC}/drivers/mcdi/mt_cpu_pm_cpc.c          \
+                ${MTK_PLAT_SOC}/drivers/mcdi/mt_mcdi.c                \
+                ${MTK_PLAT_SOC}/drivers/pmic/pmic.c                   \
+                ${MTK_PLAT_SOC}/drivers/rtc/rtc.c                     \
+                ${MTK_PLAT_SOC}/drivers/spmc/mtspmc.c                 \
                 ${MTK_PLAT_SOC}/plat_pm.c                             \
+                ${MTK_PLAT_SOC}/plat_sip_calls.c                      \
                 ${MTK_PLAT_SOC}/plat_topology.c
 
 # Configs for A76 and A55
@@ -40,6 +72,16 @@
 ERRATA_A55_1530923 := 1
 ERRATA_A55_1221012 := 1
 
+ERRATA_A76_1257314 := 1
+ERRATA_A76_1262606 := 1
+ERRATA_A76_1262888 := 1
+ERRATA_A76_1275112 := 1
+ERRATA_A76_1286807 := 1
+ERRATA_A76_1791580 := 1
+ERRATA_A76_1165522 := 1
+ERRATA_A76_1868343 := 1
+ERRATA_A76_1946160 := 1
+
 # indicate the reset vector address can be programmed
 PROGRAMMABLE_RESET_ADDRESS := 1
 
diff --git a/plat/socionext/synquacer/drivers/scp/sq_scmi.c b/plat/socionext/synquacer/drivers/scp/sq_scmi.c
index e2013cc..0e99256 100644
--- a/plat/socionext/synquacer/drivers/scp/sq_scmi.c
+++ b/plat/socionext/synquacer/drivers/scp/sq_scmi.c
@@ -189,6 +189,11 @@
 /*
  * Helper function to reset the system via SCMI.
  */
+void __dead2 sq_scmi_sys_shutdown(void)
+{
+	sq_scmi_system_off(SCMI_SYS_PWR_SHUTDOWN);
+}
+
 void __dead2 sq_scmi_sys_reboot(void)
 {
 	sq_scmi_system_off(SCMI_SYS_PWR_COLD_RESET);
diff --git a/plat/socionext/synquacer/include/platform_def.h b/plat/socionext/synquacer/include/platform_def.h
index 2f8613a..49ffbf9 100644
--- a/plat/socionext/synquacer/include/platform_def.h
+++ b/plat/socionext/synquacer/include/platform_def.h
@@ -76,6 +76,7 @@
 
 #define SQ_SYS_TIMCTL_BASE		0x2a810000
 #define PLAT_SQ_NSTIMER_FRAME_ID	0
+#define SQ_SYS_CNT_BASE_NS		0x2a830000
 
 #define DRAMINFO_BASE			0x2E00FFC0
 
diff --git a/plat/socionext/synquacer/include/sq_common.h b/plat/socionext/synquacer/include/sq_common.h
index a985822..b09d22a 100644
--- a/plat/socionext/synquacer/include/sq_common.h
+++ b/plat/socionext/synquacer/include/sq_common.h
@@ -45,6 +45,7 @@
 /* SCMI API for power management by SCP */
 void sq_scmi_off(const struct psci_power_state *target_state);
 void sq_scmi_on(u_register_t mpidr);
+void __dead2 sq_scmi_sys_shutdown(void);
 void __dead2 sq_scmi_sys_reboot(void);
 void __dead2 sq_scmi_system_off(int state);
 /* SCMI API for vendor specific protocol */
diff --git a/plat/socionext/synquacer/sq_bl31_setup.c b/plat/socionext/synquacer/sq_bl31_setup.c
index 9723ef9..a7a0ce0 100644
--- a/plat/socionext/synquacer/sq_bl31_setup.c
+++ b/plat/socionext/synquacer/sq_bl31_setup.c
@@ -24,6 +24,20 @@
 IMPORT_SYM(uintptr_t, __SPM_SHIM_EXCEPTIONS_END__,   SPM_SHIM_EXCEPTIONS_END);
 IMPORT_SYM(uintptr_t, __SPM_SHIM_EXCEPTIONS_LMA__,   SPM_SHIM_EXCEPTIONS_LMA);
 
+unsigned int plat_get_syscnt_freq2(void)
+{
+	unsigned int counter_base_frequency;
+
+	/* Read the frequency from Frequency modes table */
+	counter_base_frequency = mmio_read_32(SQ_SYS_CNTCTL_BASE + CNTFID_OFF);
+
+	/* The first entry of the frequency modes table must not be 0 */
+	if (counter_base_frequency == 0)
+		panic();
+
+	return counter_base_frequency;
+}
+
 entry_point_info_t *bl31_plat_get_next_image_ep_info(uint32_t type)
 {
 	assert(sec_state_is_valid(type));
@@ -119,6 +133,7 @@
 static void sq_configure_sys_timer(void)
 {
 	unsigned int reg_val;
+	unsigned int freq_val = plat_get_syscnt_freq2();
 
 	reg_val = (1 << CNTACR_RPCT_SHIFT) | (1 << CNTACR_RVCT_SHIFT);
 	reg_val |= (1 << CNTACR_RFRQ_SHIFT) | (1 << CNTACR_RVOFF_SHIFT);
@@ -128,6 +143,17 @@
 
 	reg_val = (1 << CNTNSAR_NS_SHIFT(PLAT_SQ_NSTIMER_FRAME_ID));
 	mmio_write_32(SQ_SYS_TIMCTL_BASE + CNTNSAR, reg_val);
+
+	/* Initialize CNTFRQ register in CNTCTLBase frame */
+	mmio_write_32(SQ_SYS_TIMCTL_BASE + CNTCTLBASE_CNTFRQ, freq_val);
+
+	/*
+	 * Initialize CNTFRQ register in Non-secure CNTBase frame.
+	 * This is required for SynQuacer, because it does not
+	 * follow ARM ARM in that the value updated in CNTFRQ is not
+	 * reflected in CNTBASEN_CNTFRQ. Hence update the value manually.
+	 */
+	mmio_write_32(SQ_SYS_CNT_BASE_NS + CNTBASEN_CNTFRQ, freq_val);
 }
 
 void bl31_platform_setup(void)
@@ -184,17 +210,3 @@
 {
 	enable_mmu_el3(flags | XLAT_TABLE_NC);
 }
-
-unsigned int plat_get_syscnt_freq2(void)
-{
-	unsigned int counter_base_frequency;
-
-	/* Read the frequency from Frequency modes table */
-	counter_base_frequency = mmio_read_32(SQ_SYS_CNTCTL_BASE + CNTFID_OFF);
-
-	/* The first entry of the frequency modes table must not be 0 */
-	if (counter_base_frequency == 0)
-		panic();
-
-	return counter_base_frequency;
-}
diff --git a/plat/socionext/synquacer/sq_psci.c b/plat/socionext/synquacer/sq_psci.c
index 4168df9..3062f63 100644
--- a/plat/socionext/synquacer/sq_psci.c
+++ b/plat/socionext/synquacer/sq_psci.c
@@ -113,6 +113,9 @@
 
 void __dead2 sq_system_off(void)
 {
+#if SQ_USE_SCMI_DRIVER
+	sq_scmi_sys_shutdown();
+#else
 	volatile uint32_t *gpio = (uint32_t *)PLAT_SQ_GPIO_BASE;
 
 	/* set PD[9] high to power off the system */
@@ -139,6 +142,7 @@
 	wfi();
 	ERROR("SQ System Off: operation not handled.\n");
 	panic();
+#endif
 }
 
 void __dead2 sq_system_reset(void)
diff --git a/plat/st/common/bl2_io_storage.c b/plat/st/common/bl2_io_storage.c
index b0314d2..6069e5f 100644
--- a/plat/st/common/bl2_io_storage.c
+++ b/plat/st/common/bl2_io_storage.c
@@ -99,7 +99,7 @@
 static const io_dev_connector_t *spi_dev_con;
 #endif
 
-#if STM32MP_USB_PROGRAMMER
+#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
 static const io_dev_connector_t *memmap_dev_con;
 #endif
 
@@ -136,6 +136,9 @@
 	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NAND_QSPI:
 		INFO("Using SPI NAND\n");
 		break;
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART:
+		INFO("Using UART\n");
+		break;
 	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB:
 		INFO("Using USB\n");
 		break;
@@ -257,7 +260,7 @@
 }
 #endif /* STM32MP_SPI_NAND */
 
-#if STM32MP_USB_PROGRAMMER
+#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
 static void mmap_io_setup(void)
 {
 	int io_result __unused;
@@ -270,6 +273,21 @@
 	assert(io_result == 0);
 }
 
+#if STM32MP_UART_PROGRAMMER
+static void stm32cubeprogrammer_uart(void)
+{
+	int ret __unused;
+	boot_api_context_t *boot_context =
+		(boot_api_context_t *)stm32mp_get_boot_ctx_address();
+	uintptr_t uart_base;
+
+	uart_base = get_uart_address(boot_context->boot_interface_instance);
+	ret = stm32cubeprog_uart_load(uart_base, DWL_BUFFER_BASE, DWL_BUFFER_SIZE);
+	assert(ret == 0);
+}
+#endif
+
+#if STM32MP_USB_PROGRAMMER
 static void stm32cubeprogrammer_usb(void)
 {
 	int ret __unused;
@@ -282,6 +300,8 @@
 	assert(ret == 0);
 }
 #endif
+#endif /* STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER */
+
 
 void stm32mp_io_setup(void)
 {
@@ -334,8 +354,13 @@
 		boot_spi_nand(boot_context);
 		break;
 #endif
+#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
+#if STM32MP_UART_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART:
+#endif
 #if STM32MP_USB_PROGRAMMER
 	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB:
+#endif
 		dmbsy();
 		mmap_io_setup();
 		break;
@@ -400,6 +425,16 @@
 		break;
 #endif
 
+#if STM32MP_UART_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART:
+		if (image_id == FW_CONFIG_ID) {
+			stm32cubeprogrammer_uart();
+			/* FIP loaded at DWL address */
+			image_block_spec.offset = DWL_BUFFER_BASE;
+			image_block_spec.length = DWL_BUFFER_SIZE;
+		}
+		break;
+#endif
 #if STM32MP_USB_PROGRAMMER
 	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB:
 		if (image_id == FW_CONFIG_ID) {
diff --git a/plat/st/common/include/stm32cubeprogrammer.h b/plat/st/common/include/stm32cubeprogrammer.h
index 503d919..0f5a64d 100644
--- a/plat/st/common/include/stm32cubeprogrammer.h
+++ b/plat/st/common/include/stm32cubeprogrammer.h
@@ -24,4 +24,6 @@
 			   uintptr_t ssbl_base,
 			   size_t ssbl_len);
 
+int stm32cubeprog_uart_load(uintptr_t instance, uintptr_t base, size_t len);
+
 #endif /* STM32CUBEPROGRAMMER_H */
diff --git a/plat/st/common/include/stm32mp_common.h b/plat/st/common/include/stm32mp_common.h
index 8a5fe48..9ca5d16 100644
--- a/plat/st/common/include/stm32mp_common.h
+++ b/plat/st/common/include/stm32mp_common.h
@@ -48,6 +48,14 @@
 uint32_t stm32_iwdg_shadow_update(uint32_t iwdg_inst, uint32_t flags);
 #endif
 
+#if STM32MP_UART_PROGRAMMER || !defined(IMAGE_BL2)
+/* Get the UART address from its instance number */
+uintptr_t get_uart_address(uint32_t instance_nb);
+#endif
+
+/* Setup the UART console */
+int stm32mp_uart_console_setup(void);
+
 /*
  * Platform util functions for the GPIO driver
  * @bank: Target GPIO bank ID as per DT bindings
@@ -64,6 +72,7 @@
 uintptr_t stm32_get_gpio_bank_base(unsigned int bank);
 unsigned long stm32_get_gpio_bank_clock(unsigned int bank);
 uint32_t stm32_get_gpio_bank_offset(unsigned int bank);
+bool stm32_gpio_is_secure_at_reset(unsigned int bank);
 
 /* Return node offset for target GPIO bank ID @bank or a FDT error code */
 int stm32_get_gpio_bank_pinctrl_node(void *fdt, unsigned int bank);
@@ -83,15 +92,6 @@
 /* Print board information */
 void stm32mp_print_boardinfo(void);
 
-/*
- * Util for clock gating and to get clock rate for stm32 and platform drivers
- * @id: Target clock ID, ID used in clock DT bindings
- */
-bool stm32mp_clk_is_enabled(unsigned long id);
-void stm32mp_clk_enable(unsigned long id);
-void stm32mp_clk_disable(unsigned long id);
-unsigned long stm32mp_clk_get_rate(unsigned long id);
-
 /* Initialise the IO layer and register platform IO devices */
 void stm32mp_io_setup(void);
 
@@ -109,4 +109,8 @@
 int stm32mp_map_ddr_non_cacheable(void);
 int stm32mp_unmap_ddr(void);
 
+/* Functions to save and get boot peripheral info */
+void stm32_save_boot_interface(uint32_t interface, uint32_t instance);
+void stm32_get_boot_interface(uint32_t *interface, uint32_t *instance);
+
 #endif /* STM32MP_COMMON_H */
diff --git a/plat/st/common/stm32cubeprogrammer_uart.c b/plat/st/common/stm32cubeprogrammer_uart.c
new file mode 100644
index 0000000..46ac9cf
--- /dev/null
+++ b/plat/st/common/stm32cubeprogrammer_uart.c
@@ -0,0 +1,521 @@
+/*
+ * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <endian.h>
+#include <errno.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_iwdg.h>
+#include <drivers/st/stm32_uart.h>
+#include <drivers/st/stm32_uart_regs.h>
+#include <lib/mmio.h>
+#include <tools_share/firmware_image_package.h>
+
+#include <platform_def.h>
+#include <stm32cubeprogrammer.h>
+
+/* USART bootloader protocol version V4.0 */
+#define USART_BL_VERSION	0x40U
+
+/* Command definition */
+#define GET_CMD_COMMAND		0x00U
+#define GET_VER_COMMAND		0x01U
+#define GET_ID_COMMAND		0x02U
+#define PHASE_COMMAND		0x03U
+#define READ_PART_COMMAND	0x12U
+#define START_COMMAND		0x21U
+#define DOWNLOAD_COMMAND	0x31U
+
+/* Answer defines */
+#define INIT_BYTE		0x7FU
+#define ACK_BYTE		0x79U
+#define NACK_BYTE		0x1FU
+#define ABORT			0x5FU
+
+#define UNDEFINED_DOWN_ADDR	U(0xFFFFFFFF)
+#define PROGRAMMER_TIMEOUT_US	20000U
+
+static const uint8_t command_tab[] = {
+	GET_CMD_COMMAND,
+	GET_VER_COMMAND,
+	GET_ID_COMMAND,
+	PHASE_COMMAND,
+	START_COMMAND,
+	DOWNLOAD_COMMAND
+};
+
+/* STM32CubeProgrammer over UART handle */
+struct stm32prog_uart_handle_s {
+	struct stm32_uart_handle_s uart;
+	uint32_t packet;
+	uint8_t *addr;
+	uint32_t len;
+	uint8_t phase;
+	/* Error msg buffer: max 255 in UART protocol, reduced in TF-A */
+	uint8_t error[64];
+} handle;
+
+/* Trace and handle unrecoverable UART protocol error */
+#define STM32PROG_ERROR(...) \
+	{ \
+		ERROR(__VA_ARGS__); \
+		if (handle.phase != PHASE_RESET) { \
+			snprintf((char *)&handle.error, sizeof(handle.error), __VA_ARGS__); \
+			handle.phase = PHASE_RESET; \
+			handle.addr = (uint8_t *)UNDEFINED_DOWN_ADDR; \
+			handle.len = 0U; \
+			handle.packet = 0U; \
+		} \
+	}
+
+static int uart_write(const uint8_t *addr, uint16_t size)
+{
+	while (size != 0U) {
+		if (stm32_uart_putc(&handle.uart, *addr) != 0) {
+			return -EIO;
+		}
+		size--;
+		addr++;
+	}
+
+	return 0;
+}
+
+static int uart_write_8(uint8_t byte)
+{
+	return stm32_uart_putc(&handle.uart, byte);
+}
+
+static int uart_write_32(uint32_t value)
+{
+	return uart_write((uint8_t *)&value, 4U);
+}
+
+static int uart_read_8(uint8_t *byte)
+{
+	int ret;
+	uint64_t timeout_ref = timeout_init_us(PROGRAMMER_TIMEOUT_US);
+
+	do {
+		ret = stm32_uart_getc(&handle.uart);
+		if (ret == -EAGAIN) {
+			if (timeout_elapsed(timeout_ref)) {
+				return -ETIMEDOUT;
+			}
+		} else if (ret < 0) {
+			return ret;
+		}
+	} while (ret == -EAGAIN);
+
+	*byte = (uint8_t)ret;
+
+	return 0;
+}
+
+static int uart_send_result(uint8_t byte)
+{
+	int ret;
+
+	/* Always flush fifo before to send result = read all pending data */
+	do {
+		ret = stm32_uart_getc(&handle.uart);
+	} while (ret >= 0);
+
+	return uart_write_8(byte);
+}
+
+static bool is_valid_header(fip_toc_header_t *header)
+{
+	return (header->name == TOC_HEADER_NAME) &&
+	       (header->serial_number != 0U);
+}
+
+static int uart_receive_command(uint8_t *command)
+{
+	uint8_t byte = 0U;
+	uint8_t xor = 0U;
+	unsigned int count;
+	bool found = false;
+	int ret;
+
+	/* Repeat read until something is received */
+	do {
+		stm32_iwdg_refresh();
+		ret = uart_read_8(&byte);
+	} while (ret == -ETIMEDOUT);
+
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* Handle reconnection request */
+	if (byte == INIT_BYTE) {
+		*command = byte;
+		return 0;
+	}
+
+	for (count = 0U; count < ARRAY_SIZE(command_tab); count++) {
+		if (command_tab[count] == byte) {
+			found = true;
+			break;
+		}
+	}
+	if (!found) {
+		VERBOSE("UART: Command unknown (byte=0x%x)\n", byte);
+		return -EPROTO;
+	}
+
+	ret = uart_read_8(&xor);
+	if (ret != 0) {
+		return ret;
+	}
+	if ((byte ^ xor) != 0xFF) {
+		VERBOSE("UART: Command XOR check fail (byte=0x%x, xor=0x%x)\n",
+			byte, xor);
+		return -EPROTO;
+	}
+
+	*command = byte;
+
+	return 0;
+}
+
+static int get_cmd_command(void)
+{
+	const uint8_t msg[2] = {
+		sizeof(command_tab), /* Length of data - 1 */
+		USART_BL_VERSION
+	};
+	int ret;
+
+	ret = uart_write(msg, sizeof(msg));
+	if (ret != 0) {
+		return ret;
+	}
+
+	return uart_write(command_tab, sizeof(command_tab));
+}
+
+static int get_version_command(void)
+{
+	return uart_write_8(STM32_TF_VERSION);
+}
+
+static int get_id_command(void)
+{
+	uint8_t msg[3] = {
+		sizeof(msg) - 1 /* Length of data - 1 */
+	};
+	uint32_t chip_id = stm32mp_get_chip_dev_id();
+
+	be16enc(&msg[1], chip_id);
+
+	return uart_write(msg, sizeof(msg));
+}
+
+static int uart_send_phase(uint32_t address)
+{
+	int ret;
+	uint8_t msg_size = 5U; /* Length of data - 1 */
+	uint8_t error_size = 0U;
+
+	/* Additional information only for RESET phase */
+	if (handle.phase == PHASE_RESET) {
+		error_size = strnlen((char *)&handle.error, sizeof(handle.error));
+	}
+	ret = uart_write_8(msg_size + error_size);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* Send the ID of next partition */
+	ret = uart_write_8(handle.phase);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* Destination address */
+	ret = uart_write_32(address);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = uart_write_8(error_size);
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* Additional information: message error */
+	if (error_size > 0U) {
+		ret = uart_write(handle.error, error_size);
+	}
+
+	return ret;
+}
+
+static int uart_download_part(void)
+{
+	uint8_t operation = 0U;
+	uint8_t xor;
+	uint8_t byte = 0U;
+	uint32_t packet_number = 0U;
+	uint32_t packet_size = 0U;
+	uint32_t i = 0U;
+	int ret;
+
+	/* Get operation number */
+	ret = uart_read_8(&operation);
+	if (ret != 0) {
+		return ret;
+	}
+
+	xor = operation;
+
+	/* Get packet number */
+	for (i = 3U; i != 0U; i--) {
+		ret = uart_read_8(&byte);
+		if (ret != 0) {
+			return ret;
+		}
+
+		xor ^= byte;
+		packet_number = (packet_number << 8) | byte;
+	}
+
+	if (packet_number != handle.packet) {
+		WARN("UART: Bad packet number receive: %u, expected %u\n",
+		     packet_number, handle.packet);
+		return -EPROTO;
+	}
+
+	/* Checksum */
+	ret = uart_read_8(&byte);
+	if (ret != 0) {
+		return ret;
+	}
+	if (xor != byte) {
+		VERBOSE("UART: Download Command checksum xor: %x, received %x\n",
+			xor, byte);
+		return -EPROTO;
+	}
+
+	ret = uart_send_result(ACK_BYTE);
+	if (ret != 0) {
+		return ret;
+	}
+
+	ret = uart_read_8(&byte);
+	if (ret != 0) {
+		return ret;
+	}
+	xor = byte;
+	packet_size = byte + 1U;
+	if (handle.len < packet_size) {
+		STM32PROG_ERROR("Download overflow at %p\n", handle.addr + packet_size);
+		return 0;
+	}
+
+	for (i = 0U; i < packet_size; i++) {
+		ret = uart_read_8(&byte);
+		if (ret != 0) {
+			return ret;
+		}
+
+		*(handle.addr + i) = byte;
+		xor ^= byte;
+	}
+
+	/* Checksum */
+	ret = uart_read_8(&byte) != 0;
+	if (ret != 0) {
+		return ret;
+	}
+	if (xor != byte) {
+		VERBOSE("UART: Download Data checksum xor: %x, received %x\n",
+			xor, byte);
+		return -EPROTO;
+	}
+
+	/* Packet treated */
+	handle.packet++;
+	handle.addr += packet_size;
+	handle.len -= packet_size;
+
+	return 0;
+}
+
+static int uart_start_cmd(uintptr_t buffer)
+{
+	uint8_t byte = 0U;
+	uint8_t xor = 0U;
+	uint32_t i;
+	uint32_t start_address = 0U;
+	int ret;
+
+	/* Get address */
+	for (i = 4U; i != 0U; i--) {
+		ret = uart_read_8(&byte);
+		if (ret != 0U) {
+			return ret;
+		}
+
+		xor ^= byte;
+		start_address = (start_address << 8) | byte;
+	}
+
+	/* Checksum */
+	ret = uart_read_8(&byte);
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (xor != byte) {
+		VERBOSE("UART: Start Command checksum xor: %x, received %x\n",
+			xor, byte);
+		return -EPROTO;
+	}
+
+	if (start_address != UNDEFINED_DOWN_ADDR) {
+		STM32PROG_ERROR("Invalid start at %x, for phase %u\n",
+				start_address, handle.phase);
+		return 0;
+	}
+
+	if (!is_valid_header((fip_toc_header_t *)buffer)) {
+		STM32PROG_ERROR("FIP Header check failed %lx, for phase %u\n",
+				buffer, handle.phase);
+		return -EIO;
+	}
+	VERBOSE("FIP header looks OK.\n");
+
+	return 0;
+}
+
+static int uart_read(uint8_t id, uintptr_t buffer, size_t length)
+{
+	bool start_done = false;
+	int ret;
+	uint8_t command = 0U;
+
+	handle.phase = id;
+	handle.packet = 0U;
+	handle.addr = (uint8_t *)buffer;
+	handle.len = length;
+
+	INFO("UART: read phase %u at 0x%lx size 0x%x\n",
+	     id, buffer, length);
+	while (!start_done) {
+		ret = uart_receive_command(&command);
+		if (ret != 0) {
+			/* Delay to wait STM32CubeProgrammer end of transmission */
+			mdelay(3);
+
+			ret = uart_send_result(NACK_BYTE);
+			if (ret != 0U) {
+				return ret;
+			}
+
+			continue;
+		}
+
+		uart_send_result(ACK_BYTE);
+
+		switch (command) {
+		case INIT_BYTE:
+			INFO("UART: Connected\n");
+			/* Nothing to do */
+			continue;
+
+		case GET_CMD_COMMAND:
+			ret = get_cmd_command();
+			break;
+
+		case GET_VER_COMMAND:
+			ret = get_version_command();
+			break;
+
+		case GET_ID_COMMAND:
+			ret = get_id_command();
+			break;
+
+		case PHASE_COMMAND:
+			ret = uart_send_phase((uint32_t)buffer);
+			if ((ret == 0) && (handle.phase == PHASE_RESET)) {
+				start_done = true;
+				INFO("UART: Reset\n");
+			}
+			break;
+
+		case DOWNLOAD_COMMAND:
+			ret = uart_download_part();
+			break;
+
+		case START_COMMAND:
+			ret = uart_start_cmd(buffer);
+			if ((ret == 0) && (handle.phase == id)) {
+				INFO("UART: Start phase %u\n", handle.phase);
+				start_done = true;
+			}
+			break;
+
+		default:
+			WARN("UART: Unknown command\n");
+			ret = -EINVAL;
+			break;
+		}
+
+		if (ret == 0) {
+			ret = uart_send_result(ACK_BYTE);
+		} else {
+			ret = uart_send_result(NACK_BYTE);
+		}
+		if (ret != 0) {
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/* Init UART: 115200, 8bit 1stop parity even and enable FIFO mode */
+const struct stm32_uart_init_s init = {
+	.baud_rate = U(115200),
+	.word_length = STM32_UART_WORDLENGTH_9B,
+	.stop_bits = STM32_UART_STOPBITS_1,
+	.parity = STM32_UART_PARITY_EVEN,
+	.hw_flow_control = STM32_UART_HWCONTROL_NONE,
+	.mode = STM32_UART_MODE_TX_RX,
+	.over_sampling = STM32_UART_OVERSAMPLING_16,
+	.fifo_mode = STM32_UART_FIFOMODE_EN,
+};
+
+int stm32cubeprog_uart_load(uintptr_t instance, uintptr_t base, size_t len)
+{
+	int ret;
+
+	if (stm32_uart_init(&handle.uart, instance, &init) != 0) {
+		return -EIO;
+	}
+
+	/*
+	 * The following NACK_BYTE is written because STM32CubeProgrammer has
+	 * already sent its command before TF-A has reached this point, and
+	 * because FIFO was not configured by BootROM.
+	 * The byte in the UART_RX register is then the checksum and not the
+	 * command. NACK_BYTE has to be written, so that the programmer will
+	 * re-send the good command.
+	 */
+	ret = uart_send_result(NACK_BYTE);
+	if (ret != 0) {
+		return ret;
+	}
+
+	return uart_read(PHASE_SSBL, base, len);
+}
diff --git a/plat/st/common/stm32mp_common.c b/plat/st/common/stm32mp_common.c
index 9120408..918c289 100644
--- a/plat/st/common/stm32mp_common.c
+++ b/plat/st/common/stm32mp_common.c
@@ -7,17 +7,24 @@
 #include <assert.h>
 #include <errno.h>
 
-#include <platform_def.h>
-
 #include <arch_helpers.h>
 #include <common/debug.h>
+#include <drivers/clk.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_console.h>
 #include <drivers/st/stm32mp_clkfunc.h>
+#include <drivers/st/stm32mp_reset.h>
 #include <lib/smccc.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 #include <services/arm_arch_svc.h>
 
+#include <platform_def.h>
+
 #define HEADER_VERSION_MAJOR_MASK	GENMASK(23, 16)
+#define RESET_TIMEOUT_US_1MS		1000U
+
+static console_t console;
 
 uintptr_t plat_get_ns_image_entrypoint(void)
 {
@@ -128,6 +135,83 @@
 					   STM32MP_DDR_MAX_SIZE);
 }
 
+#if  defined(IMAGE_BL2)
+static void reset_uart(uint32_t reset)
+{
+	int ret;
+
+	ret = stm32mp_reset_assert(reset, RESET_TIMEOUT_US_1MS);
+	if (ret != 0) {
+		panic();
+	}
+
+	udelay(2);
+
+	ret = stm32mp_reset_deassert(reset, RESET_TIMEOUT_US_1MS);
+	if (ret != 0) {
+		panic();
+	}
+
+	mdelay(1);
+}
+#endif
+
+int stm32mp_uart_console_setup(void)
+{
+	struct dt_node_info dt_uart_info;
+	unsigned int console_flags;
+	uint32_t clk_rate;
+	int result;
+	uint32_t boot_itf __unused;
+	uint32_t boot_instance __unused;
+
+	result = dt_get_stdout_uart_info(&dt_uart_info);
+
+	if ((result <= 0) ||
+	    (dt_uart_info.status == DT_DISABLED) ||
+	    (dt_uart_info.clock < 0) ||
+	    (dt_uart_info.reset < 0)) {
+		return -ENODEV;
+	}
+
+#if STM32MP_UART_PROGRAMMER || !defined(IMAGE_BL2)
+	stm32_get_boot_interface(&boot_itf, &boot_instance);
+
+	if ((boot_itf == BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART) &&
+	    (get_uart_address(boot_instance) == dt_uart_info.base)) {
+		return -EACCES;
+	}
+#endif
+
+#if defined(IMAGE_BL2)
+	if (dt_set_stdout_pinctrl() != 0) {
+		return -ENODEV;
+	}
+#endif
+
+	clk_enable((unsigned long)dt_uart_info.clock);
+
+#if defined(IMAGE_BL2)
+	reset_uart((uint32_t)dt_uart_info.reset);
+#endif
+
+	clk_rate = clk_get_rate((unsigned long)dt_uart_info.clock);
+
+	if (console_stm32_register(dt_uart_info.base, clk_rate,
+				   STM32MP_UART_BAUDRATE, &console) == 0) {
+		panic();
+	}
+
+	console_flags = CONSOLE_FLAG_BOOT | CONSOLE_FLAG_CRASH |
+			CONSOLE_FLAG_TRANSLATE_CRLF;
+#if !defined(IMAGE_BL2) && defined(DEBUG)
+	console_flags |= CONSOLE_FLAG_RUNTIME;
+#endif
+	console_set_scope(&console, console_flags);
+
+	return 0;
+}
+
 /*****************************************************************************
  * plat_is_smccc_feature_available() - This function checks whether SMCCC
  *                                     feature is availabile for platform.
diff --git a/plat/st/stm32mp1/bl2_plat_setup.c b/plat/st/stm32mp1/bl2_plat_setup.c
index 7eaf0ed..512afa8 100644
--- a/plat/st/stm32mp1/bl2_plat_setup.c
+++ b/plat/st/stm32mp1/bl2_plat_setup.c
@@ -8,23 +8,19 @@
 #include <errno.h>
 #include <string.h>
 
-#include <platform_def.h>
-
 #include <arch_helpers.h>
 #include <common/bl_common.h>
 #include <common/debug.h>
 #include <common/desc_image_load.h>
-#include <drivers/delay_timer.h>
 #include <drivers/generic_delay_timer.h>
 #include <drivers/mmc.h>
 #include <drivers/st/bsec.h>
-#include <drivers/st/stm32_console.h>
 #include <drivers/st/stm32_iwdg.h>
-#include <drivers/st/stm32mp_pmic.h>
-#include <drivers/st/stm32mp_reset.h>
+#include <drivers/st/stm32_uart.h>
 #include <drivers/st/stm32mp1_clk.h>
 #include <drivers/st/stm32mp1_pwr.h>
 #include <drivers/st/stm32mp1_ram.h>
+#include <drivers/st/stm32mp_pmic.h>
 #include <lib/fconf/fconf.h>
 #include <lib/fconf/fconf_dyn_cfg_getter.h>
 #include <lib/mmio.h>
@@ -32,12 +28,9 @@
 #include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
-#include <stm32mp1_context.h>
+#include <platform_def.h>
 #include <stm32mp1_dbgmcu.h>
 
-#define RESET_TIMEOUT_US_1MS		1000U
-
-static console_t console;
 static struct stm32mp_auth_ops stm32mp1_auth_ops;
 
 static void print_reset_reason(void)
@@ -167,11 +160,9 @@
 void bl2_el3_plat_arch_setup(void)
 {
 	int32_t result;
-	struct dt_node_info dt_uart_info;
 	const char *board_model;
 	boot_api_context_t *boot_context =
 		(boot_api_context_t *)stm32mp_get_boot_ctx_address();
-	uint32_t clk_rate;
 	uintptr_t pwr_base;
 	uintptr_t rcc_base;
 
@@ -238,6 +229,16 @@
 
 	generic_delay_timer_init();
 
+#if STM32MP_UART_PROGRAMMER
+	/* Disable programmer UART before changing clock tree */
+	if (boot_context->boot_interface_selected ==
+	    BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART) {
+		uintptr_t uart_prog_addr =
+			get_uart_address(boot_context->boot_interface_instance);
+
+		stm32_uart_stop(uart_prog_addr);
+	}
+#endif
 	if (stm32mp1_clk_probe() < 0) {
 		panic();
 	}
@@ -248,45 +249,18 @@
 
 	stm32mp1_syscfg_init();
 
-	result = dt_get_stdout_uart_info(&dt_uart_info);
+	stm32_save_boot_interface(boot_context->boot_interface_selected,
+				  boot_context->boot_interface_instance);
 
-	if ((result <= 0) ||
-	    (dt_uart_info.status == 0U) ||
-	    (dt_uart_info.clock < 0) ||
-	    (dt_uart_info.reset < 0)) {
+#if STM32MP_USB_PROGRAMMER
+	/* Deconfigure all UART RX pins configured by ROM code */
+	stm32mp1_deconfigure_uart_pins();
+#endif
+
+	if (stm32mp_uart_console_setup() != 0) {
 		goto skip_console_init;
 	}
 
-	if (dt_set_stdout_pinctrl() != 0) {
-		goto skip_console_init;
-	}
-
-	stm32mp_clk_enable((unsigned long)dt_uart_info.clock);
-
-	if (stm32mp_reset_assert((uint32_t)dt_uart_info.reset,
-				 RESET_TIMEOUT_US_1MS) != 0) {
-		panic();
-	}
-
-	udelay(2);
-
-	if (stm32mp_reset_deassert((uint32_t)dt_uart_info.reset,
-				   RESET_TIMEOUT_US_1MS) != 0) {
-		panic();
-	}
-
-	mdelay(1);
-
-	clk_rate = stm32mp_clk_get_rate((unsigned long)dt_uart_info.clock);
-
-	if (console_stm32_register(dt_uart_info.base, clk_rate,
-				   STM32MP_UART_BAUDRATE, &console) == 0) {
-		panic();
-	}
-
-	console_set_scope(&console, CONSOLE_FLAG_BOOT |
-			  CONSOLE_FLAG_CRASH | CONSOLE_FLAG_TRANSLATE_CRLF);
-
 	stm32mp_print_cpuinfo();
 
 	board_model = dt_get_board_model();
@@ -314,12 +288,6 @@
 		INFO("IWDG2 freeze error : %i\n", result);
 	}
 
-	if (stm32_save_boot_interface(boot_context->boot_interface_selected,
-				      boot_context->boot_interface_instance) !=
-	    0) {
-		ERROR("Cannot save boot interface\n");
-	}
-
 	stm32mp1_auth_ops.check_key = boot_context->bootrom_ecdsa_check_key;
 	stm32mp1_auth_ops.verify_signature =
 		boot_context->bootrom_ecdsa_verify_signature;
@@ -352,6 +320,7 @@
 	const struct dyn_cfg_dtb_info_t *config_info;
 	bl_mem_params_node_t *tos_fw_mem_params;
 	unsigned int i;
+	unsigned int idx;
 	unsigned long long ddr_top __unused;
 	const unsigned int image_ids[] = {
 		BL32_IMAGE_ID,
@@ -370,8 +339,14 @@
 		set_config_info(STM32MP_FW_CONFIG_BASE, STM32MP_FW_CONFIG_MAX_SIZE, FW_CONFIG_ID);
 		fconf_populate("FW_CONFIG", STM32MP_FW_CONFIG_BASE);
 
+		idx = dyn_cfg_dtb_info_get_index(TOS_FW_CONFIG_ID);
+
 		/* Iterate through all the fw config IDs */
 		for (i = 0U; i < ARRAY_SIZE(image_ids); i++) {
+			if ((image_ids[i] == TOS_FW_CONFIG_ID) && (idx == FCONF_INVALID_IDX)) {
+				continue;
+			}
+
 			bl_mem_params = get_bl_mem_params_node(image_ids[i]);
 			assert(bl_mem_params != NULL);
 
@@ -493,12 +468,13 @@
 	uint16_t boot_itf = stm32mp_get_boot_itf_selected();
 
 	switch (boot_itf) {
-#if STM32MP_USB_PROGRAMMER
+#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART:
 	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB:
 		/* Invalidate the downloaded buffer used with io_memmap */
 		inv_dcache_range(DWL_BUFFER_BASE, DWL_BUFFER_SIZE);
 		break;
-#endif
+#endif /* STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER */
 	default:
 		/* Do nothing in default case */
 		break;
diff --git a/plat/st/stm32mp1/include/boot_api.h b/plat/st/stm32mp1/include/boot_api.h
index 52b1d1a..198ffa9 100644
--- a/plat/st/stm32mp1/include/boot_api.h
+++ b/plat/st/stm32mp1/include/boot_api.h
@@ -39,6 +39,9 @@
 /* Boot occurred on QSPI NOR */
 #define BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_NOR_QSPI		0x4U
 
+/* Boot occurred on UART */
+#define BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART		0x5U
+
 /* Boot occurred on USB */
 #define BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB		0x6U
 
diff --git a/plat/st/stm32mp1/include/stm32mp1_context.h b/plat/st/stm32mp1/include/stm32mp1_context.h
deleted file mode 100644
index 698415a..0000000
--- a/plat/st/stm32mp1/include/stm32mp1_context.h
+++ /dev/null
@@ -1,14 +0,0 @@
-/*
- * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef STM32MP1_CONTEXT_H
-#define STM32MP1_CONTEXT_H
-
-#include <stdint.h>
-
-int stm32_save_boot_interface(uint32_t interface, uint32_t instance);
-
-#endif /* STM32MP1_CONTEXT_H */
diff --git a/plat/st/stm32mp1/include/stm32mp1_private.h b/plat/st/stm32mp1/include/stm32mp1_private.h
index 729d233..2eec16f 100644
--- a/plat/st/stm32mp1/include/stm32mp1_private.h
+++ b/plat/st/stm32mp1/include/stm32mp1_private.h
@@ -21,6 +21,8 @@
 void stm32mp1_syscfg_enable_io_compensation(void);
 void stm32mp1_syscfg_disable_io_compensation(void);
 
+void stm32mp1_deconfigure_uart_pins(void);
+
 #if STM32MP_USE_STM32IMAGE
 uint32_t stm32mp_get_ddr_ns_size(void);
 #endif /* STM32MP_USE_STM32IMAGE */
diff --git a/plat/st/stm32mp1/platform.mk b/plat/st/stm32mp1/platform.mk
index badc926..aa91646 100644
--- a/plat/st/stm32mp1/platform.mk
+++ b/plat/st/stm32mp1/platform.mk
@@ -28,6 +28,8 @@
 TF_CFLAGS		+=	-mfloat-abi=soft
 endif
 
+TF_CFLAGS		+=	-Wsign-compare
+
 # Not needed for Cortex-A7
 WORKAROUND_CVE_2017_5715:=	0
 
@@ -55,6 +57,7 @@
 
 # Serial boot devices
 STM32MP_USB_PROGRAMMER	?=	0
+STM32MP_UART_PROGRAMMER	?=	0
 
 # Device tree
 DTB_FILE_NAME		?=	stm32mp157c-ev1.dtb
@@ -123,34 +126,38 @@
 # Enable flags for C files
 $(eval $(call assert_booleans,\
 	$(sort \
+		PLAT_XLAT_TABLES_DYNAMIC \
 		STM32MP_EMMC \
-		STM32MP_SDMMC \
+		STM32MP_EMMC_BOOT \
 		STM32MP_RAW_NAND \
+		STM32MP_SDMMC \
 		STM32MP_SPI_NAND \
 		STM32MP_SPI_NOR \
-		STM32MP_EMMC_BOOT \
-		PLAT_XLAT_TABLES_DYNAMIC \
+		STM32MP_UART_PROGRAMMER \
 		STM32MP_USB_PROGRAMMER \
 		STM32MP_USE_STM32IMAGE \
 )))
 
 $(eval $(call assert_numerics,\
 	$(sort \
-		STM32_TF_A_COPIES \
 		PLAT_PARTITION_MAX_ENTRIES \
+		STM32_TF_A_COPIES \
+		STM32_TF_VERSION \
 )))
 
 $(eval $(call add_defines,\
 	$(sort \
-		STM32MP_EMMC \
-		STM32MP_SDMMC \
-		STM32MP_RAW_NAND \
-		STM32MP_SPI_NAND \
-		STM32MP_SPI_NOR \
-		STM32MP_EMMC_BOOT \
+		PLAT_PARTITION_MAX_ENTRIES \
 		PLAT_XLAT_TABLES_DYNAMIC \
 		STM32_TF_A_COPIES \
-		PLAT_PARTITION_MAX_ENTRIES \
+		STM32_TF_VERSION \
+		STM32MP_EMMC \
+		STM32MP_EMMC_BOOT \
+		STM32MP_RAW_NAND \
+		STM32MP_SDMMC \
+		STM32MP_SPI_NAND \
+		STM32MP_SPI_NOR \
+		STM32MP_UART_PROGRAMMER \
 		STM32MP_USB_PROGRAMMER \
 		STM32MP_USE_STM32IMAGE \
 )))
@@ -182,6 +189,7 @@
 PLAT_BL_COMMON_SOURCES	+=	lib/cpus/aarch32/cortex_a7.S
 
 PLAT_BL_COMMON_SOURCES	+=	drivers/arm/tzc/tzc400.c				\
+				drivers/clk/clk.c					\
 				drivers/delay_timer/delay_timer.c			\
 				drivers/delay_timer/generic_delay_timer.c		\
 				drivers/st/bsec/bsec.c					\
@@ -195,7 +203,6 @@
 				drivers/st/pmic/stpmic1.c				\
 				drivers/st/reset/stm32mp1_reset.c			\
 				plat/st/common/stm32mp_dt.c				\
-				plat/st/stm32mp1/stm32mp1_context.c			\
 				plat/st/stm32mp1/stm32mp1_dbgmcu.c			\
 				plat/st/stm32mp1/stm32mp1_helper.S			\
 				plat/st/stm32mp1/stm32mp1_syscfg.c
@@ -260,11 +267,19 @@
 BL2_SOURCES		+=	plat/st/stm32mp1/stm32mp1_boot_device.c
 endif
 
+ifneq ($(filter 1,${STM32MP_UART_PROGRAMMER} ${STM32MP_USB_PROGRAMMER}),)
+BL2_SOURCES		+=	drivers/io/io_memmap.c
+endif
+
+ifeq (${STM32MP_UART_PROGRAMMER},1)
+BL2_SOURCES		+=	drivers/st/uart/stm32_uart.c				\
+				plat/st/common/stm32cubeprogrammer_uart.c
+endif
+
 ifeq (${STM32MP_USB_PROGRAMMER},1)
 #The DFU stack uses only one end point, reduce the USB stack footprint
 $(eval $(call add_define_val,CONFIG_USBD_EP_NB,1U))
-BL2_SOURCES		+=	drivers/io/io_memmap.c					\
-				drivers/st/usb/stm32mp1_usb.c				\
+BL2_SOURCES		+=	drivers/st/usb/stm32mp1_usb.c				\
 				drivers/usb/usb_device.c				\
 				plat/st/common/stm32cubeprogrammer_usb.c		\
 				plat/st/common/usb_dfu.c					\
@@ -295,6 +310,7 @@
 	    [ ${STM32MP_RAW_NAND} != 1 ] && \
 	    [ ${STM32MP_SPI_NAND} != 1 ] && \
 	    [ ${STM32MP_SPI_NOR} != 1 ] && \
+	    [ ${STM32MP_UART_PROGRAMMER} != 1 ] && \
 	    [ ${STM32MP_USB_PROGRAMMER} != 1 ]; then \
 		echo "No boot device driver is enabled"; \
 		false; \
diff --git a/plat/st/stm32mp1/sp_min/sp_min_setup.c b/plat/st/stm32mp1/sp_min/sp_min_setup.c
index 1495e02..8106795 100644
--- a/plat/st/stm32mp1/sp_min/sp_min_setup.c
+++ b/plat/st/stm32mp1/sp_min/sp_min_setup.c
@@ -18,7 +18,6 @@
 #include <drivers/generic_delay_timer.h>
 #include <drivers/st/bsec.h>
 #include <drivers/st/etzpc.h>
-#include <drivers/st/stm32_console.h>
 #include <drivers/st/stm32_gpio.h>
 #include <drivers/st/stm32_iwdg.h>
 #include <drivers/st/stm32mp1_clk.h>
@@ -36,8 +35,6 @@
  ******************************************************************************/
 static entry_point_info_t bl33_image_ep_info;
 
-static console_t console;
-
 /*******************************************************************************
  * Interrupt handler for FIQ (secure IRQ)
  ******************************************************************************/
@@ -115,8 +112,6 @@
 void sp_min_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 				  u_register_t arg2, u_register_t arg3)
 {
-	struct dt_node_info dt_uart_info;
-	int result;
 	bl_params_t *params_from_bl2 = (bl_params_t *)arg0;
 #if STM32MP_USE_STM32IMAGE
 	uintptr_t dt_addr = STM32MP_DTB_BASE;
@@ -174,24 +169,7 @@
 		panic();
 	}
 
-	result = dt_get_stdout_uart_info(&dt_uart_info);
-
-	if ((result > 0) && (dt_uart_info.status != 0U)) {
-		unsigned int console_flags;
-
-		if (console_stm32_register(dt_uart_info.base, 0,
-					   STM32MP_UART_BAUDRATE, &console) ==
-		    0) {
-			panic();
-		}
-
-		console_flags = CONSOLE_FLAG_BOOT | CONSOLE_FLAG_CRASH |
-			CONSOLE_FLAG_TRANSLATE_CRLF;
-#ifdef DEBUG
-		console_flags |= CONSOLE_FLAG_RUNTIME;
-#endif
-		console_set_scope(&console, console_flags);
-	}
+	(void)stm32mp_uart_console_setup();
 
 	stm32mp1_etzpc_early_setup();
 }
diff --git a/plat/st/stm32mp1/stm32mp1_context.c b/plat/st/stm32mp1/stm32mp1_context.c
deleted file mode 100644
index cf8a91e..0000000
--- a/plat/st/stm32mp1/stm32mp1_context.c
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <errno.h>
-
-#include <platform_def.h>
-
-#include <drivers/st/stm32mp1_clk.h>
-#include <dt-bindings/clock/stm32mp1-clks.h>
-#include <lib/mmio.h>
-
-#include <stm32mp1_context.h>
-
-#define TAMP_BOOT_ITF_BACKUP_REG_ID	U(20)
-#define TAMP_BOOT_ITF_MASK		U(0x0000FF00)
-#define TAMP_BOOT_ITF_SHIFT		8
-
-int stm32_save_boot_interface(uint32_t interface, uint32_t instance)
-{
-	uint32_t bkpr_itf_idx = tamp_bkpr(TAMP_BOOT_ITF_BACKUP_REG_ID);
-
-	stm32mp_clk_enable(RTCAPB);
-
-	mmio_clrsetbits_32(bkpr_itf_idx,
-			   TAMP_BOOT_ITF_MASK,
-			   ((interface << 4) | (instance & 0xFU)) <<
-			   TAMP_BOOT_ITF_SHIFT);
-
-	stm32mp_clk_disable(RTCAPB);
-
-	return 0;
-}
diff --git a/plat/st/stm32mp1/stm32mp1_fconf_firewall.c b/plat/st/stm32mp1/stm32mp1_fconf_firewall.c
index caf9ff1..a1969eb 100644
--- a/plat/st/stm32mp1/stm32mp1_fconf_firewall.c
+++ b/plat/st/stm32mp1/stm32mp1_fconf_firewall.c
@@ -9,7 +9,7 @@
 #include <common/debug.h>
 #include <common/fdt_wrappers.h>
 #include <drivers/arm/tzc400.h>
-#include <drivers/st/stm32mp1_clk.h>
+#include <drivers/clk.h>
 #include <dt-bindings/clock/stm32mp1-clks.h>
 #include <lib/fconf/fconf.h>
 #include <lib/object_pool.h>
@@ -31,8 +31,8 @@
 
 void stm32mp1_arch_security_setup(void)
 {
-	stm32mp_clk_enable(TZC1);
-	stm32mp_clk_enable(TZC2);
+	clk_enable(TZC1);
+	clk_enable(TZC2);
 
 	tzc400_init(STM32MP1_TZC_BASE);
 	tzc400_disable_filters();
diff --git a/plat/st/stm32mp1/stm32mp1_pm.c b/plat/st/stm32mp1/stm32mp1_pm.c
index 31a9ae7..ed4d5e5 100644
--- a/plat/st/stm32mp1/stm32mp1_pm.c
+++ b/plat/st/stm32mp1/stm32mp1_pm.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,7 +14,7 @@
 #include <common/debug.h>
 #include <drivers/arm/gic_common.h>
 #include <drivers/arm/gicv2.h>
-#include <drivers/st/stm32mp1_clk.h>
+#include <drivers/clk.h>
 #include <dt-bindings/clock/stm32mp1-clks.h>
 #include <lib/mmio.h>
 #include <lib/psci/psci.h>
@@ -74,7 +74,7 @@
 		return PSCI_E_INVALID_ADDRESS;
 	}
 
-	stm32mp_clk_enable(RTCAPB);
+	clk_enable(RTCAPB);
 
 	cntfrq_core0 = read_cntfrq_el0();
 
@@ -84,7 +84,7 @@
 	/* Write magic number in backup register */
 	mmio_write_32(bkpr_core1_magic, BOOT_API_A7_CORE1_MAGIC_NUMBER);
 
-	stm32mp_clk_disable(RTCAPB);
+	clk_disable(RTCAPB);
 
 	/* Generate an IT to core 1 */
 	gicv2_raise_sgi(ARM_IRQ_SEC_SGI_0, STM32MP_SECONDARY_CPU);
diff --git a/plat/st/stm32mp1/stm32mp1_private.c b/plat/st/stm32mp1/stm32mp1_private.c
index e4065c1..e58528e 100644
--- a/plat/st/stm32mp1/stm32mp1_private.c
+++ b/plat/st/stm32mp1/stm32mp1_private.c
@@ -6,13 +6,15 @@
 
 #include <assert.h>
 
+#include <drivers/clk.h>
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32_iwdg.h>
+#include <lib/mmio.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
 #include <libfdt.h>
 
 #include <platform_def.h>
 
-#include <drivers/st/stm32_iwdg.h>
-#include <lib/xlat_tables/xlat_tables_v2.h>
-
 /* Internal layout of the 32bit OTP word board_id */
 #define BOARD_ID_BOARD_NB_MASK		GENMASK(31, 16)
 #define BOARD_ID_BOARD_NB_SHIFT		16
@@ -34,6 +36,10 @@
 					 BOARD_ID_VARFG_SHIFT)
 #define BOARD_ID2BOM(_id)		((_id) & BOARD_ID_BOM_MASK)
 
+#define TAMP_BOOT_MODE_BACKUP_REG_ID	U(20)
+#define TAMP_BOOT_MODE_ITF_MASK		U(0x0000FF00)
+#define TAMP_BOOT_MODE_ITF_SHIFT	8
+
 #if defined(IMAGE_BL2)
 #define MAP_SEC_SYSRAM	MAP_REGION_FLAT(STM32MP_SYSRAM_BASE, \
 					STM32MP_SYSRAM_SIZE, \
@@ -120,6 +126,15 @@
 	return bank * GPIO_BANK_OFFSET;
 }
 
+bool stm32_gpio_is_secure_at_reset(unsigned int bank)
+{
+	if (bank == GPIO_BANK_Z) {
+		return true;
+	}
+
+	return false;
+}
+
 unsigned long stm32_get_gpio_bank_clock(unsigned int bank)
 {
 	if (bank == GPIO_BANK_Z) {
@@ -153,6 +168,79 @@
 	}
 }
 
+#if STM32MP_UART_PROGRAMMER || !defined(IMAGE_BL2)
+/*
+ * UART Management
+ */
+static const uintptr_t stm32mp1_uart_addresses[8] = {
+	USART1_BASE,
+	USART2_BASE,
+	USART3_BASE,
+	UART4_BASE,
+	UART5_BASE,
+	USART6_BASE,
+	UART7_BASE,
+	UART8_BASE,
+};
+
+uintptr_t get_uart_address(uint32_t instance_nb)
+{
+	if ((instance_nb == 0U) ||
+	    (instance_nb > ARRAY_SIZE(stm32mp1_uart_addresses))) {
+		return 0U;
+	}
+
+	return stm32mp1_uart_addresses[instance_nb - 1U];
+}
+#endif
+
+#if STM32MP_USB_PROGRAMMER
+struct gpio_bank_pin_list {
+	uint32_t bank;
+	uint32_t pin;
+};
+
+static const struct gpio_bank_pin_list gpio_list[] = {
+	{	/* USART2_RX: GPIOA3 */
+		.bank = 0U,
+		.pin = 3U,
+	},
+	{	/* USART3_RX: GPIOB12 */
+		.bank = 1U,
+		.pin = 12U,
+	},
+	{	/* UART4_RX: GPIOB2 */
+		.bank = 1U,
+		.pin = 2U,
+	},
+	{	/* UART5_RX: GPIOB4 */
+		.bank = 1U,
+		.pin = 5U,
+	},
+	{	/* USART6_RX: GPIOC7 */
+		.bank = 2U,
+		.pin = 7U,
+	},
+	{	/* UART7_RX: GPIOF6 */
+		.bank = 5U,
+		.pin = 6U,
+	},
+	{	/* UART8_RX: GPIOE0 */
+		.bank = 4U,
+		.pin = 0U,
+	},
+};
+
+void stm32mp1_deconfigure_uart_pins(void)
+{
+	size_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(gpio_list); i++) {
+		set_gpio_reset_cfg(gpio_list[i].bank, gpio_list[i].pin);
+	}
+}
+#endif
+
 uint32_t stm32mp_get_chip_version(void)
 {
 	uint32_t version = 0U;
@@ -474,3 +562,36 @@
 	return ddr_ns_size;
 }
 #endif /* STM32MP_USE_STM32IMAGE */
+
+void stm32_save_boot_interface(uint32_t interface, uint32_t instance)
+{
+	uint32_t bkpr_itf_idx = tamp_bkpr(TAMP_BOOT_MODE_BACKUP_REG_ID);
+
+	clk_enable(RTCAPB);
+
+	mmio_clrsetbits_32(bkpr_itf_idx,
+			   TAMP_BOOT_MODE_ITF_MASK,
+			   ((interface << 4) | (instance & 0xFU)) <<
+			   TAMP_BOOT_MODE_ITF_SHIFT);
+
+	clk_disable(RTCAPB);
+}
+
+void stm32_get_boot_interface(uint32_t *interface, uint32_t *instance)
+{
+	static uint32_t itf;
+
+	if (itf == 0U) {
+		uint32_t bkpr = tamp_bkpr(TAMP_BOOT_MODE_BACKUP_REG_ID);
+
+		clk_enable(RTCAPB);
+
+		itf = (mmio_read_32(bkpr) & TAMP_BOOT_MODE_ITF_MASK) >>
+			TAMP_BOOT_MODE_ITF_SHIFT;
+
+		clk_disable(RTCAPB);
+	}
+
+	*interface = itf >> 4;
+	*instance = itf & 0xFU;
+}
diff --git a/plat/st/stm32mp1/stm32mp1_scmi.c b/plat/st/stm32mp1/stm32mp1_scmi.c
index 6d60bd4..98585dc 100644
--- a/plat/st/stm32mp1/stm32mp1_scmi.c
+++ b/plat/st/stm32mp1/stm32mp1_scmi.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2019-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -8,6 +8,7 @@
 
 #include <platform_def.h>
 
+#include <drivers/clk.h>
 #include <drivers/scmi-msg.h>
 #include <drivers/scmi.h>
 #include <drivers/st/stm32mp1_clk.h>
@@ -274,7 +275,7 @@
 	if (array == NULL) {
 		*nb_elts = 1U;
 	} else if (*nb_elts == 1U) {
-		*array = stm32mp_clk_get_rate(clock->clock_id);
+		*array = clk_get_rate(clock->clock_id);
 	} else {
 		return SCMI_GENERIC_ERROR;
 	}
@@ -292,7 +293,7 @@
 		return 0U;
 	}
 
-	return stm32mp_clk_get_rate(clock->clock_id);
+	return clk_get_rate(clock->clock_id);
 }
 
 int32_t plat_scmi_clock_get_state(unsigned int agent_id, unsigned int scmi_id)
@@ -323,13 +324,13 @@
 	if (enable_not_disable) {
 		if (!clock->enabled) {
 			VERBOSE("SCMI clock %u enable\n", scmi_id);
-			stm32mp_clk_enable(clock->clock_id);
+			clk_enable(clock->clock_id);
 			clock->enabled = true;
 		}
 	} else {
 		if (clock->enabled) {
 			VERBOSE("SCMI clock %u disable\n", scmi_id);
-			stm32mp_clk_disable(clock->clock_id);
+			clk_disable(clock->clock_id);
 			clock->enabled = false;
 		}
 	}
@@ -461,7 +462,7 @@
 			/* Sync SCMI clocks with their targeted initial state */
 			if (clk->enabled &&
 			    stm32mp_nsec_can_access_clock(clk->clock_id)) {
-				stm32mp_clk_enable(clk->clock_id);
+				clk_enable(clk->clock_id);
 			}
 		}
 
diff --git a/plat/st/stm32mp1/stm32mp1_security.c b/plat/st/stm32mp1/stm32mp1_security.c
index 19ef4f0..c84bffc 100644
--- a/plat/st/stm32mp1/stm32mp1_security.c
+++ b/plat/st/stm32mp1/stm32mp1_security.c
@@ -10,6 +10,7 @@
 
 #include <common/debug.h>
 #include <drivers/arm/tzc400.h>
+#include <drivers/clk.h>
 #include <drivers/st/stm32mp1_clk.h>
 #include <dt-bindings/clock/stm32mp1-clks.h>
 #include <dt-bindings/soc/stm32mp15-tzc400.h>
@@ -106,8 +107,8 @@
  ******************************************************************************/
 static void early_init_tzc400(void)
 {
-	stm32mp_clk_enable(TZC1);
-	stm32mp_clk_enable(TZC2);
+	clk_enable(TZC1);
+	clk_enable(TZC2);
 
 	/* Region 0 set to cover all DRAM secure at 0xC000_0000 */
 	init_tzc400_begin(TZC_REGION_S_RDWR);
diff --git a/plat/xilinx/versal/bl31_versal_setup.c b/plat/xilinx/versal/bl31_versal_setup.c
index f59859e..60f7449 100644
--- a/plat/xilinx/versal/bl31_versal_setup.c
+++ b/plat/xilinx/versal/bl31_versal_setup.c
@@ -64,7 +64,7 @@
 {
 	uint64_t atf_handoff_addr;
 
-	if (VERSAL_CONSOLE_IS(pl011)) {
+	if (VERSAL_CONSOLE_IS(pl011) || (VERSAL_CONSOLE_IS(pl011_1))) {
 		static console_t versal_runtime_console;
 		/* Initialize the console to provide early debug support */
 		int rc = console_pl011_register((unsigned long)VERSAL_UART_BASE,
diff --git a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
index 47be4e1..58eee3a 100644
--- a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
+++ b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
@@ -68,7 +68,7 @@
 {
 	uint64_t atf_handoff_addr;
 
-	if (ZYNQMP_CONSOLE_IS(cadence)) {
+	if (ZYNQMP_CONSOLE_IS(cadence) || (ZYNQMP_CONSOLE_IS(cadence1))) {
 		/* Register the console to provide early debug support */
 		static console_t bl31_boot_console;
 		(void)console_cdns_register(ZYNQMP_UART_BASE,
diff --git a/plat/xilinx/zynqmp/platform.mk b/plat/xilinx/zynqmp/platform.mk
index d075a56..3d98584 100644
--- a/plat/xilinx/zynqmp/platform.mk
+++ b/plat/xilinx/zynqmp/platform.mk
@@ -118,6 +118,7 @@
 endif
 
 BL31_CPPFLAGS		+=	-fno-jump-tables
+TF_CFLAGS_aarch64	+=	-mbranch-protection=none
 
 ifneq (${RESET_TO_BL31},1)
   $(error "Using BL31 as the reset vector is only one option supported on ZynqMP. Please set RESET_TO_BL31 to 1.")
diff --git a/services/arm_arch_svc/arm_arch_svc_setup.c b/services/arm_arch_svc/arm_arch_svc_setup.c
index 1d4423c..5523a1c 100644
--- a/services/arm_arch_svc/arm_arch_svc_setup.c
+++ b/services/arm_arch_svc/arm_arch_svc_setup.c
@@ -11,19 +11,9 @@
 #include <lib/cpus/wa_cve_2018_3639.h>
 #include <lib/smccc.h>
 #include <services/arm_arch_svc.h>
-#include <services/rmi_svc.h>
-#include <services/rmmd_svc.h>
 #include <smccc_helpers.h>
 #include <plat/common/platform.h>
 
-#if ENABLE_RME
-/* Setup Arm architecture Services */
-static int32_t arm_arch_svc_setup(void)
-{
-	return rmmd_setup();
-}
-#endif
-
 static int32_t smccc_version(void)
 {
 	return MAKE_SMCCC_VERSION(SMCCC_MAJOR_VERSION, SMCCC_MINOR_VERSION);
@@ -143,16 +133,6 @@
 		SMC_RET0(handle);
 #endif
 	default:
-#if ENABLE_RME
-		/*
-		 * RMI functions are allocated from the Arch service range. Call
-		 * the RMM dispatcher to handle RMI calls.
-		 */
-		if (is_rmi_fid(smc_fid)) {
-			return rmmd_rmi_handler(smc_fid, x1, x2, x3, x4, cookie,
-						handle, flags);
-		}
-#endif
 		WARN("Unimplemented Arm Architecture Service Call: 0x%x \n",
 			smc_fid);
 		SMC_RET1(handle, SMC_UNK);
@@ -165,10 +145,6 @@
 		OEN_ARM_START,
 		OEN_ARM_END,
 		SMC_TYPE_FAST,
-#if ENABLE_RME
-		arm_arch_svc_setup,
-#else
 		NULL,
-#endif
 		arm_arch_svc_smc_handler
 );
diff --git a/services/std_svc/rmmd/rmmd_main.c b/services/std_svc/rmmd/rmmd_main.c
index e9004c7..c4ea706 100644
--- a/services/std_svc/rmmd/rmmd_main.c
+++ b/services/std_svc/rmmd/rmmd_main.c
@@ -30,6 +30,7 @@
 #include <services/rmi_svc.h>
 #include <services/rmmd_svc.h>
 #include <smccc_helpers.h>
+#include <lib/extensions/sve.h>
 #include "rmmd_initial_context.h"
 #include "rmmd_private.h"
 
@@ -112,6 +113,26 @@
 }
 
 /*******************************************************************************
+ * Enable architecture extensions on first entry to Realm world.
+ ******************************************************************************/
+static void manage_extensions_realm(cpu_context_t *ctx)
+{
+#if ENABLE_SVE_FOR_NS
+	/*
+	 * Enable SVE and FPU in realm context when it is enabled for NS.
+	 * Realm manager must ensure that the SVE and FPU register
+	 * contexts are properly managed.
+	 */
+	sve_enable(ctx);
+#else
+	/*
+	 * Disable SVE and FPU in realm context when it is disabled for NS.
+	 */
+	sve_disable(ctx);
+#endif /* ENABLE_SVE_FOR_NS */
+}
+
+/*******************************************************************************
  * Jump to the RMM for the first time.
  ******************************************************************************/
 static int32_t rmm_init(void)
@@ -124,6 +145,9 @@
 	INFO("RMM init start.\n");
 	ctx->state = RMM_STATE_RESET;
 
+	/* Enable architecture extensions */
+	manage_extensions_realm(&ctx->cpu_ctx);
+
 	/* Initialize RMM EL2 context. */
 	rmm_el2_context_init(&ctx->cpu_ctx.el2_sysregs_ctx);
 
@@ -281,6 +305,9 @@
 	/* Initialise RMM context with this entry point information */
 	cm_setup_context(&ctx->cpu_ctx, rmm_ep_info);
 
+	/* Enable architecture extensions */
+	manage_extensions_realm(&ctx->cpu_ctx);
+
 	/* Initialize RMM EL2 context. */
 	rmm_el2_context_init(&ctx->cpu_ctx.el2_sysregs_ctx);
 
diff --git a/services/std_svc/rmmd/trp/trp_entry.S b/services/std_svc/rmmd/trp/trp_entry.S
index 23b48fb..5826d75 100644
--- a/services/std_svc/rmmd/trp/trp_entry.S
+++ b/services/std_svc/rmmd/trp/trp_entry.S
@@ -33,8 +33,19 @@
 	 */
 trp_head:
 	bl	plat_set_my_stack
-	bl	plat_is_my_cpu_primary
-	cbz	x0, trp_secondary_cpu_entry
+
+	/*
+	 * Find out whether this is a cold or warm boot
+	 */
+	ldr	x1, cold_boot_flag
+	cbz	x1, warm_boot
+
+	/*
+	 * Update cold boot flag to indicate cold boot is done
+	 */
+	adr	x2, cold_boot_flag
+	str	xzr, [x2]
+
 
 	/* ---------------------------------------------
 	 * Zero out BSS section
@@ -47,12 +58,20 @@
 	bl	trp_setup
 
 	bl	trp_main
-trp_secondary_cpu_entry:
+warm_boot:
 	mov_imm	x0, RMI_RMM_REQ_COMPLETE
 	mov	x1, xzr
 	smc	#0
 	b	trp_handler
 
+	/*
+	 * Flag to mark if it is a cold boot.
+	 * 1: cold boot, 0: warmboot.
+	 */
+.align 3
+cold_boot_flag:
+	.dword		1
+
 	/* ---------------------------------------------
 	 *   Direct SMC call to BL31 service provided by
 	 *   RMM Dispatcher
diff --git a/services/std_svc/std_svc_setup.c b/services/std_svc/std_svc_setup.c
index 39db429..eea7e14 100644
--- a/services/std_svc/std_svc_setup.c
+++ b/services/std_svc/std_svc_setup.c
@@ -15,6 +15,7 @@
 #include <lib/runtime_instr.h>
 #include <services/gtsi_svc.h>
 #include <services/pci_svc.h>
+#include <services/rmi_svc.h>
 #include <services/rmmd_svc.h>
 #include <services/sdei.h>
 #include <services/spm_mm_svc.h>
@@ -62,6 +63,12 @@
 	}
 #endif
 
+#if ENABLE_RME
+	if (rmmd_setup() != 0) {
+		ret = 1;
+	}
+#endif
+
 #if SDEI_SUPPORT
 	/* SDEI initialisation */
 	sdei_init();
@@ -169,6 +176,11 @@
 		return rmmd_gtsi_handler(smc_fid, x1, x2, x3, x4, cookie,
 						handle, flags);
 	}
+
+	if (is_rmi_fid(smc_fid)) {
+		return rmmd_rmi_handler(smc_fid, x1, x2, x3, x4, cookie,
+					handle, flags);
+	}
 #endif
 
 #if SMC_PCI_SUPPORT
diff --git a/tools/fiptool/fiptool.c b/tools/fiptool/fiptool.c
index d92c31d..5c240b5 100644
--- a/tools/fiptool/fiptool.c
+++ b/tools/fiptool/fiptool.c
@@ -537,7 +537,7 @@
 	for (desc = image_desc_head; desc != NULL; desc = desc->next) {
 		image_t *image = desc->image;
 
-		if (image == NULL)
+		if (image == NULL || (image->toc_e.size == 0ULL))
 			continue;
 		payload_size += image->toc_e.size;
 		entry_offset = (entry_offset + align - 1) & ~(align - 1);