diff --git a/Makefile b/Makefile
index 5916963..afa417b 100644
--- a/Makefile
+++ b/Makefile
@@ -9,6 +9,7 @@
 #
 VERSION_MAJOR			:= 2
 VERSION_MINOR			:= 7
+VERSION				:= ${VERSION_MAJOR}.${VERSION_MINOR}
 
 # Default goal is build all images
 .DEFAULT_GOAL			:= all
@@ -322,7 +323,7 @@
 ifeq (${BUILD_STRING},)
         BUILD_STRING  :=  $(shell git describe --always --dirty --tags 2> /dev/null)
 endif
-VERSION_STRING    :=  v${VERSION_MAJOR}.${VERSION_MINOR}(${BUILD_TYPE}):${BUILD_STRING}
+VERSION_STRING    :=  v${VERSION}(${BUILD_TYPE}):${BUILD_STRING}
 
 ifeq (${AARCH32_INSTRUCTION_SET},A32)
 TF_CFLAGS_aarch32	+=	-marm
@@ -600,6 +601,9 @@
 PIE_FOUND		:=	$(findstring --enable-default-pie,${GCC_V_OUTPUT})
 ifneq ($(PIE_FOUND),)
 	TF_CFLAGS	+=	-fno-PIE
+ifneq ($(findstring gcc,$(notdir $(LD))),)
+	TF_LDFLAGS	+=	-no-pie
+endif
 endif
 
 ifneq ($(findstring gcc,$(notdir $(LD))),)
@@ -790,11 +794,15 @@
         $(error "ENABLE_SVE_FOR_NS cannot be used with ARCH=aarch32")
     endif
 
-    # BRBE is not supported in Aarch32
+    # BRBE is not supported in AArch32
     ifeq (${ENABLE_BRBE_FOR_NS},1)
         $(error "ENABLE_BRBE_FOR_NS cannot be used with ARCH=aarch32")
     endif
 
+    # FEAT_RNG_TRAP is not supported in AArch32
+    ifeq (${ENABLE_FEAT_RNG_TRAP},1)
+        $(error "ENABLE_FEAT_RNG_TRAP cannot be used with ARCH=aarch32")
+    endif
 endif
 
 # Ensure ENABLE_RME is not used with SME
@@ -1075,6 +1083,7 @@
         ENABLE_FEAT_HCX \
         ENABLE_FEAT_PAN \
         ENABLE_FEAT_RNG \
+        ENABLE_FEAT_RNG_TRAP \
         ENABLE_FEAT_SB \
         ENABLE_FEAT_SEL2 \
         ENABLE_FEAT_VHE \
@@ -1186,6 +1195,7 @@
         COT_DESC_IN_DTB \
         USE_SP804_TIMER \
         ENABLE_FEAT_RNG \
+        ENABLE_FEAT_RNG_TRAP \
         ENABLE_FEAT_SB \
         ENABLE_FEAT_DIT \
         NR_OF_FW_BANKS \
diff --git a/bl31/aarch64/ea_delegate.S b/bl31/aarch64/ea_delegate.S
index fa6ede8..5e53ab4 100644
--- a/bl31/aarch64/ea_delegate.S
+++ b/bl31/aarch64/ea_delegate.S
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022, NVIDIA Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -84,10 +85,6 @@
 	b	2f
 
 1:
-	/* Test for EA bit in the instruction syndrome */
-	mrs	x30, esr_el3
-	tbz	x30, #ESR_ISS_EABORT_EA_BIT, 3f
-
 	/*
 	 * Save general purpose and ARMv8.3-PAuth registers (if enabled).
 	 * If Secure Cycle Counter is not disabled in MDCR_EL3 when
@@ -114,7 +111,6 @@
 	ldp	x2, x3, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_X2]
 	ldp	x4, x5, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_X4]
 
-3:
 	/* Synchronous exceptions other than the above are assumed to be EA */
 	ldr	x30, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_LR]
 	no_ret	report_unhandled_exception
diff --git a/bl31/bl31.ld.S b/bl31/bl31.ld.S
index 8a1573a..309e752 100644
--- a/bl31/bl31.ld.S
+++ b/bl31/bl31.ld.S
@@ -47,6 +47,10 @@
         __RODATA_START__ = .;
         *(SORT_BY_ALIGNMENT(.rodata*))
 
+#if PLAT_EXTRA_RODATA_INCLUDES
+#include <plat.ld.rodata.inc>
+#endif
+
 	RODATA_COMMON
 
         /* Place pubsub sections for events */
@@ -186,10 +190,10 @@
     __RW_END__ = .;
     __BL31_END__ = .;
 
+    ASSERT(. <= BL31_LIMIT, "BL31 image has exceeded its limit.")
+#endif
+
     /DISCARD/ : {
         *(.dynsym .dynstr .hash .gnu.hash)
     }
-
-    ASSERT(. <= BL31_LIMIT, "BL31 image has exceeded its limit.")
-#endif
 }
diff --git a/bl31/ehf.c b/bl31/ehf.c
index 745f165..b328380 100644
--- a/bl31/ehf.c
+++ b/bl31/ehf.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -475,9 +475,16 @@
 	assert((exception_data.pri_bits >= 1U) ||
 			(exception_data.pri_bits < 8U));
 
-	/* Route EL3 interrupts when in Secure and Non-secure. */
+	/* Route EL3 interrupts when in Non-secure. */
 	set_interrupt_rm_flag(flags, NON_SECURE);
+
+	/*
+	 * Route EL3 interrupts when in secure, only when SPMC is not present
+	 * in S-EL2.
+	 */
+#if !(defined(SPD_spmd) && (SPMD_SPM_AT_SEL2 == 1))
 	set_interrupt_rm_flag(flags, SECURE);
+#endif /* !(defined(SPD_spmd) && (SPMD_SPM_AT_SEL2 == 1)) */
 
 	/* Register handler for EL3 interrupts */
 	ret = register_interrupt_type_handler(INTR_TYPE_EL3,
diff --git a/bl32/tsp/aarch64/tsp_entrypoint.S b/bl32/tsp/aarch64/tsp_entrypoint.S
index 7d77f47..e5ea88c 100644
--- a/bl32/tsp/aarch64/tsp_entrypoint.S
+++ b/bl32/tsp/aarch64/tsp_entrypoint.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,12 +10,16 @@
 #include <asm_macros.S>
 #include <bl32/tsp/tsp.h>
 #include <lib/xlat_tables/xlat_tables_defs.h>
+#include <smccc_helpers.h>
 
 #include "../tsp_private.h"
 
 
 	.globl	tsp_entrypoint
 	.globl  tsp_vector_table
+#if SPMC_AT_EL3
+	.globl tsp_cpu_on_entry
+#endif
 
 
 
@@ -25,10 +29,10 @@
 	 * ---------------------------------------------
 	 */
 	.macro restore_args_call_smc
-	ldp	x6, x7, [x0, #TSP_ARG6]
-	ldp	x4, x5, [x0, #TSP_ARG4]
-	ldp	x2, x3, [x0, #TSP_ARG2]
-	ldp	x0, x1, [x0, #TSP_ARG0]
+	ldp	x6, x7, [x0, #SMC_ARG6]
+	ldp	x4, x5, [x0, #SMC_ARG4]
+	ldp	x2, x3, [x0, #SMC_ARG2]
+	ldp	x0, x1, [x0, #SMC_ARG0]
 	smc	#0
 	.endm
 
diff --git a/bl32/tsp/ffa_helpers.c b/bl32/tsp/ffa_helpers.c
new file mode 100644
index 0000000..3639c22
--- /dev/null
+++ b/bl32/tsp/ffa_helpers.c
@@ -0,0 +1,250 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include "ffa_helpers.h"
+#include <services/ffa_svc.h>
+#include "tsp_private.h"
+
+/*******************************************************************************
+ * Wrapper function to send a direct request.
+ ******************************************************************************/
+smc_args_t ffa_msg_send_direct_req(ffa_endpoint_id16_t sender,
+				   ffa_endpoint_id16_t receiver,
+				   uint32_t arg3,
+				   uint32_t arg4,
+				   uint32_t arg5,
+				   uint32_t arg6,
+				   uint32_t arg7)
+{
+	uint32_t src_dst_ids = (sender << FFA_DIRECT_MSG_SOURCE_SHIFT) |
+			       (receiver << FFA_DIRECT_MSG_DESTINATION_SHIFT);
+
+
+	/* Send Direct Request. */
+	return smc_helper(FFA_MSG_SEND_DIRECT_REQ_SMC64, src_dst_ids,
+			0, arg3, arg4, arg5, arg6, arg7);
+}
+
+/*******************************************************************************
+ * Wrapper function to send a direct response.
+ ******************************************************************************/
+smc_args_t *ffa_msg_send_direct_resp(ffa_endpoint_id16_t sender,
+				     ffa_endpoint_id16_t receiver,
+				     uint32_t arg3,
+				     uint32_t arg4,
+				     uint32_t arg5,
+				     uint32_t arg6,
+				     uint32_t arg7)
+{
+	uint32_t src_dst_ids = (sender << FFA_DIRECT_MSG_SOURCE_SHIFT) |
+			       (receiver << FFA_DIRECT_MSG_DESTINATION_SHIFT);
+
+	return set_smc_args(FFA_MSG_SEND_DIRECT_RESP_SMC64, src_dst_ids,
+			    0, arg3, arg4, arg5, arg6, arg7);
+}
+
+/*******************************************************************************
+ * Memory Management Helpers.
+ ******************************************************************************/
+
+/**
+ * Initialises the header of the given `ffa_mtd`, not including the
+ * composite memory region offset.
+ */
+static void ffa_memory_region_init_header(
+	struct ffa_mtd *memory_region, ffa_endpoint_id16_t sender,
+	ffa_mem_attr16_t attributes, ffa_mtd_flag32_t flags,
+	uint64_t handle, uint64_t tag, ffa_endpoint_id16_t *receivers,
+	uint32_t receiver_count, ffa_mem_perm8_t permissions)
+{
+	struct ffa_emad_v1_0 *emad;
+
+	memory_region->emad_offset = sizeof(struct ffa_mtd);
+	memory_region->emad_size = sizeof(struct ffa_emad_v1_0);
+	emad = (struct ffa_emad_v1_0 *)
+		    ((uint8_t *) memory_region +
+		     memory_region->emad_offset);
+	memory_region->sender_id = sender;
+	memory_region->memory_region_attributes = attributes;
+	memory_region->reserved_36_39 = 0;
+	memory_region->flags = flags;
+	memory_region->handle = handle;
+	memory_region->tag = tag;
+	memory_region->reserved_40_47 = 0;
+	memory_region->emad_count = receiver_count;
+	for (uint32_t i = 0U; i < receiver_count; i++) {
+		emad[i].mapd.endpoint_id = receivers[i];
+		emad[i].mapd.memory_access_permissions = permissions;
+		emad[i].mapd.flags = 0;
+		emad[i].comp_mrd_offset = 0;
+		emad[i].reserved_8_15 = 0;
+	}
+}
+/**
+ * Initialises the given `ffa_mtd` to be used for an
+ * `FFA_MEM_RETRIEVE_REQ` by the receiver of a memory transaction.
+ * TODO: Support differing attributes per receiver.
+ *
+ * Returns the size of the descriptor written.
+ */
+static uint32_t ffa_memory_retrieve_request_init(
+	struct ffa_mtd *memory_region, uint64_t handle,
+	ffa_endpoint_id16_t sender, ffa_endpoint_id16_t *receivers, uint32_t receiver_count,
+	uint64_t tag, ffa_mtd_flag32_t flags,
+	ffa_mem_perm8_t permissions,
+	ffa_mem_attr16_t attributes)
+{
+	ffa_memory_region_init_header(memory_region, sender, attributes, flags,
+				      handle, tag, receivers,
+				      receiver_count, permissions);
+
+	return sizeof(struct ffa_mtd) +
+	       memory_region->emad_count * sizeof(struct ffa_emad_v1_0);
+}
+
+/* Relinquish access to memory region. */
+bool ffa_mem_relinquish(void)
+{
+	smc_args_t ret;
+
+	ret = smc_helper(FFA_MEM_RELINQUISH, 0, 0, 0, 0, 0, 0, 0);
+	if (ffa_func_id(ret) != FFA_SUCCESS_SMC32) {
+		ERROR("%s failed to relinquish memory! error: (%x) %x\n",
+		      __func__, ffa_func_id(ret), ffa_error_code(ret));
+		return false;
+	}
+	return true;
+}
+
+/* Retrieve memory shared by another partition. */
+smc_args_t ffa_mem_retrieve_req(uint32_t descriptor_length,
+				uint32_t fragment_length)
+{
+	return smc_helper(FFA_MEM_RETRIEVE_REQ_SMC32,
+		      descriptor_length,
+		      fragment_length,
+		      0, 0, 0, 0, 0);
+}
+
+/* Retrieve the next memory descriptor fragment. */
+smc_args_t ffa_mem_frag_rx(uint64_t handle, uint32_t recv_length)
+{
+	return smc_helper(FFA_MEM_FRAG_RX,
+		       FFA_MEM_HANDLE_LOW(handle),
+		       FFA_MEM_HANDLE_HIGH(handle),
+		       recv_length,
+		       0, 0, 0, 0);
+}
+
+bool memory_retrieve(struct mailbox *mb,
+			    struct ffa_mtd **retrieved,
+			    uint64_t handle, ffa_endpoint_id16_t sender,
+			    ffa_endpoint_id16_t *receivers, uint32_t receiver_count,
+			    ffa_mtd_flag32_t flags, uint32_t *frag_length,
+			    uint32_t *total_length)
+{
+	smc_args_t ret;
+	uint32_t descriptor_size;
+	struct ffa_mtd *memory_region = (struct ffa_mtd *)mb->tx_buffer;
+
+	if (retrieved == NULL || mb == NULL) {
+		ERROR("Invalid parameters!\n");
+		return false;
+	}
+
+	/* Clear TX buffer. */
+	memset(memory_region, 0, PAGE_SIZE);
+
+	/* Clear local buffer. */
+	memset(mem_region_buffer, 0, REGION_BUF_SIZE);
+
+	descriptor_size = ffa_memory_retrieve_request_init(
+	    memory_region, handle, sender, receivers, receiver_count, 0, flags,
+	    FFA_MEM_PERM_RW | FFA_MEM_PERM_NX,
+	    FFA_MEM_ATTR_NORMAL_MEMORY_CACHED_WB |
+	    FFA_MEM_ATTR_INNER_SHAREABLE);
+
+	ret = ffa_mem_retrieve_req(descriptor_size, descriptor_size);
+
+	if (ffa_func_id(ret) == FFA_ERROR) {
+		ERROR("Couldn't retrieve the memory page. Error: %x\n",
+		      ffa_error_code(ret));
+		return false;
+	}
+
+	/*
+	 * Following total_size and fragment_size are useful to keep track
+	 * of the state of transaction. When the sum of all fragment_size of all
+	 * fragments is equal to total_size, the memory transaction has been
+	 * completed.
+	 */
+	*total_length = ret._regs[1];
+	*frag_length = ret._regs[2];
+
+	/* Validate frag_length is less than total_length and mailbox size. */
+	if (*frag_length == 0U || *total_length == 0U ||
+	    *frag_length > *total_length || *frag_length > (mb->rxtx_page_count * PAGE_SIZE)) {
+		ERROR("Invalid parameters!\n");
+		return false;
+	}
+
+	/* Copy response to local buffer. */
+	memcpy(mem_region_buffer, mb->rx_buffer, *frag_length);
+
+	if (ffa_rx_release()) {
+		ERROR("Failed to release buffer!\n");
+		return false;
+	}
+
+	*retrieved = (struct ffa_mtd *) mem_region_buffer;
+
+	if ((*retrieved)->emad_count > MAX_MEM_SHARE_RECIPIENTS) {
+		VERBOSE("SPMC memory sharing supports max of %u receivers!\n",
+			MAX_MEM_SHARE_RECIPIENTS);
+		return false;
+	}
+
+	/*
+	 * We are sharing memory from the normal world therefore validate the NS
+	 * bit was set by the SPMC.
+	 */
+	if (((*retrieved)->memory_region_attributes & FFA_MEM_ATTR_NS_BIT) == 0U) {
+		ERROR("SPMC has not set the NS bit! 0x%x\n",
+		      (*retrieved)->memory_region_attributes);
+		return false;
+	}
+
+	VERBOSE("Memory Descriptor Retrieved!\n");
+
+	return true;
+}
+
+/* Relinquish the memory region. */
+bool memory_relinquish(struct ffa_mem_relinquish_descriptor *m, uint64_t handle,
+		       ffa_endpoint_id16_t id)
+{
+	ffa_mem_relinquish_init(m, handle, 0, id);
+	return ffa_mem_relinquish();
+}
+
+/* Query SPMC that the rx buffer of the partition can be released. */
+bool ffa_rx_release(void)
+{
+	smc_args_t ret;
+
+	ret = smc_helper(FFA_RX_RELEASE, 0, 0, 0, 0, 0, 0, 0);
+	return ret._regs[SMC_ARG0] != FFA_SUCCESS_SMC32;
+}
+
+/* Map the provided buffers with the SPMC. */
+bool ffa_rxtx_map(uintptr_t send, uintptr_t recv, uint32_t pages)
+{
+	smc_args_t ret;
+
+	ret = smc_helper(FFA_RXTX_MAP_SMC64, send, recv, pages, 0, 0, 0, 0);
+	return ret._regs[0] != FFA_SUCCESS_SMC32;
+}
diff --git a/bl32/tsp/ffa_helpers.h b/bl32/tsp/ffa_helpers.h
new file mode 100644
index 0000000..e650a07
--- /dev/null
+++ b/bl32/tsp/ffa_helpers.h
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef FFA_HELPERS_H
+#define FFA_HELPERS_H
+
+#include <stdint.h>
+
+#include "../../services/std_svc/spm/el3_spmc/spmc.h"
+#include "../../services/std_svc/spm/el3_spmc/spmc_shared_mem.h"
+#include <services/el3_spmc_ffa_memory.h>
+#include <services/ffa_svc.h>
+#include "tsp_private.h"
+
+static inline uint32_t ffa_func_id(smc_args_t val)
+{
+	return (uint32_t) val._regs[0];
+}
+
+static inline int32_t ffa_error_code(smc_args_t val)
+{
+	return (uint32_t) val._regs[2];
+}
+
+extern uint8_t mem_region_buffer[4096 * 2]  __aligned(PAGE_SIZE);
+#define REGION_BUF_SIZE sizeof(mem_region_buffer)
+
+/** The maximum number of recipients a memory region may be sent to. */
+#define MAX_MEM_SHARE_RECIPIENTS	2U
+
+/* FFA Memory Management mode flags. */
+#define FFA_FLAG_SHARE_MEMORY (1U << 3)
+#define FFA_FLAG_LEND_MEMORY (1U << 4)
+
+#define FFA_FLAG_MEMORY_MASK (3U << 3)
+
+#define FFA_MEM_HANDLE_LOW(x) (x & 0xFFFFFFFF)
+#define FFA_MEM_HANDLE_HIGH(x) (x >> 32)
+
+#define FFA_MEM_PERM_DATA_OFFSET 0
+#define FFA_MEM_PERM_DATA_MASK 0x3
+
+static inline uint32_t ffa_mem_relinquish_init(
+	struct ffa_mem_relinquish_descriptor *relinquish_request,
+	uint64_t handle, ffa_mtd_flag32_t flags,
+	ffa_endpoint_id16_t sender)
+{
+	relinquish_request->handle = handle;
+	relinquish_request->flags = flags;
+	relinquish_request->endpoint_count = 1;
+	relinquish_request->endpoint_array[0] = sender;
+
+	return sizeof(struct ffa_mem_relinquish_descriptor) + sizeof(ffa_endpoint_id16_t);
+}
+
+/**
+ * Gets the `ffa_comp_mrd` for the given receiver from an
+ * `ffa_mtd`, or NULL if it is not valid.
+ */
+static inline struct ffa_comp_mrd *
+ffa_memory_region_get_composite(struct ffa_mtd *memory_region,
+				uint32_t receiver_index)
+{
+	struct ffa_emad_v1_0 *receivers;
+	uint32_t offset;
+
+	receivers = (struct ffa_emad_v1_0 *)
+		    ((uint8_t *) memory_region +
+		    memory_region->emad_offset +
+		    (memory_region->emad_size * receiver_index));
+	offset = receivers->comp_mrd_offset;
+
+	if (offset == 0U) {
+		return NULL;
+	}
+
+	return (struct ffa_comp_mrd *)
+	       ((uint8_t *) memory_region + offset);
+}
+
+static inline uint32_t ffa_get_data_access_attr(ffa_mem_perm8_t perm)
+{
+	return ((perm >> FFA_MEM_PERM_DATA_OFFSET) & FFA_MEM_PERM_DATA_MASK);
+}
+
+smc_args_t ffa_mem_frag_rx(uint64_t handle, uint32_t recv_length);
+bool ffa_mem_relinquish(void);
+bool ffa_rx_release(void);
+bool memory_relinquish(struct ffa_mem_relinquish_descriptor *m, uint64_t handle,
+		       ffa_endpoint_id16_t id);
+bool ffa_rxtx_map(uintptr_t send, uintptr_t recv, uint32_t pages);
+bool memory_retrieve(struct mailbox *mb,
+		     struct ffa_mtd **retrieved,
+		     uint64_t handle, ffa_endpoint_id16_t sender,
+		     ffa_endpoint_id16_t *receivers, uint32_t receiver_count,
+		     ffa_mtd_flag32_t flags, uint32_t *frag_length,
+		     uint32_t *total_length);
+
+smc_args_t ffa_msg_send_direct_req(ffa_endpoint_id16_t sender,
+				   ffa_endpoint_id16_t receiver,
+				   uint32_t arg3,
+				   uint32_t arg4,
+				   uint32_t arg5,
+				   uint32_t arg6,
+				   uint32_t arg7);
+smc_args_t *ffa_msg_send_direct_resp(ffa_endpoint_id16_t sender,
+				     ffa_endpoint_id16_t receiver,
+				     uint32_t arg3,
+				     uint32_t arg4,
+				     uint32_t arg5,
+				     uint32_t arg6,
+				     uint32_t arg7);
+#endif /* FFA_HELPERS_H */
diff --git a/bl32/tsp/tsp.mk b/bl32/tsp/tsp.mk
index 3fd6d99..c31b9b5 100644
--- a/bl32/tsp/tsp.mk
+++ b/bl32/tsp/tsp.mk
@@ -1,17 +1,24 @@
 #
-# Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
 INCLUDES		+=	-Iinclude/bl32/tsp
 
-BL32_SOURCES		+=	bl32/tsp/tsp_main.c			\
-				bl32/tsp/aarch64/tsp_entrypoint.S	\
+ifeq (${SPMC_AT_EL3},1)
+   BL32_SOURCES            +=      bl32/tsp/tsp_ffa_main.c                    \
+				   bl32/tsp/ffa_helpers.c
+else
+   BL32_SOURCES            +=      bl32/tsp/tsp_main.c
+endif
+
+BL32_SOURCES		+=	bl32/tsp/aarch64/tsp_entrypoint.S	\
 				bl32/tsp/aarch64/tsp_exceptions.S	\
 				bl32/tsp/aarch64/tsp_request.S		\
 				bl32/tsp/tsp_interrupt.c		\
 				bl32/tsp/tsp_timer.c			\
+				bl32/tsp/tsp_common.c			\
 				common/aarch64/early_exceptions.S	\
 				lib/locks/exclusive/aarch64/spinlock.S
 
diff --git a/bl32/tsp/tsp_common.c b/bl32/tsp/tsp_common.c
new file mode 100644
index 0000000..908b4ff
--- /dev/null
+++ b/bl32/tsp/tsp_common.c
@@ -0,0 +1,156 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <inttypes.h>
+#include <stdint.h>
+
+#include <arch_features.h>
+#include <arch_helpers.h>
+#include <bl32/tsp/tsp.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <lib/spinlock.h>
+#include <plat/common/platform.h>
+#include <platform_tsp.h>
+#include "tsp_private.h"
+
+#include <platform_def.h>
+
+/*******************************************************************************
+ * Per cpu data structure to populate parameters for an SMC in C code and use
+ * a pointer to this structure in assembler code to populate x0-x7.
+ ******************************************************************************/
+static smc_args_t tsp_smc_args[PLATFORM_CORE_COUNT];
+
+/*******************************************************************************
+ * Per cpu data structure to keep track of TSP activity
+ ******************************************************************************/
+work_statistics_t tsp_stats[PLATFORM_CORE_COUNT];
+
+smc_args_t *set_smc_args(uint64_t arg0,
+			 uint64_t arg1,
+			 uint64_t arg2,
+			 uint64_t arg3,
+			 uint64_t arg4,
+			 uint64_t arg5,
+			 uint64_t arg6,
+			 uint64_t arg7)
+{
+	uint32_t linear_id;
+	smc_args_t *pcpu_smc_args;
+
+	/*
+	 * Return to Secure Monitor by raising an SMC. The results of the
+	 * service are passed as an arguments to the SMC.
+	 */
+	linear_id = plat_my_core_pos();
+	pcpu_smc_args = &tsp_smc_args[linear_id];
+	write_sp_arg(pcpu_smc_args, SMC_ARG0, arg0);
+	write_sp_arg(pcpu_smc_args, SMC_ARG1, arg1);
+	write_sp_arg(pcpu_smc_args, SMC_ARG2, arg2);
+	write_sp_arg(pcpu_smc_args, SMC_ARG3, arg3);
+	write_sp_arg(pcpu_smc_args, SMC_ARG4, arg4);
+	write_sp_arg(pcpu_smc_args, SMC_ARG5, arg5);
+	write_sp_arg(pcpu_smc_args, SMC_ARG6, arg6);
+	write_sp_arg(pcpu_smc_args, SMC_ARG7, arg7);
+
+	return pcpu_smc_args;
+}
+
+/*******************************************************************************
+ * Setup function for TSP.
+ ******************************************************************************/
+void tsp_setup(void)
+{
+	/* Perform early platform-specific setup. */
+	tsp_early_platform_setup();
+
+	/* Perform late platform-specific setup. */
+	tsp_plat_arch_setup();
+
+#if ENABLE_PAUTH
+	/*
+	 * Assert that the ARMv8.3-PAuth registers are present or an access
+	 * fault will be triggered when they are being saved or restored.
+	 */
+	assert(is_armv8_3_pauth_present());
+#endif /* ENABLE_PAUTH */
+}
+
+/*******************************************************************************
+ * This function performs any remaining bookkeeping in the test secure payload
+ * before the system is switched off (in response to a psci SYSTEM_OFF request).
+ ******************************************************************************/
+smc_args_t *tsp_system_off_main(uint64_t arg0,
+				uint64_t arg1,
+				uint64_t arg2,
+				uint64_t arg3,
+				uint64_t arg4,
+				uint64_t arg5,
+				uint64_t arg6,
+				uint64_t arg7)
+{
+	uint32_t linear_id = plat_my_core_pos();
+
+	/* Update this cpu's statistics. */
+	tsp_stats[linear_id].smc_count++;
+	tsp_stats[linear_id].eret_count++;
+
+	INFO("TSP: cpu 0x%lx SYSTEM_OFF request\n", read_mpidr());
+	INFO("TSP: cpu 0x%lx: %d smcs, %d erets requests\n", read_mpidr(),
+	     tsp_stats[linear_id].smc_count,
+	     tsp_stats[linear_id].eret_count);
+
+	/* Indicate to the SPD that we have completed this request. */
+	return set_smc_args(TSP_SYSTEM_OFF_DONE, 0, 0, 0, 0, 0, 0, 0);
+}
+
+/*******************************************************************************
+ * This function performs any remaining bookkeeping in the test secure payload
+ * before the system is reset (in response to a psci SYSTEM_RESET request).
+ ******************************************************************************/
+smc_args_t *tsp_system_reset_main(uint64_t arg0,
+				  uint64_t arg1,
+				  uint64_t arg2,
+				  uint64_t arg3,
+				  uint64_t arg4,
+				  uint64_t arg5,
+				  uint64_t arg6,
+				  uint64_t arg7)
+{
+	uint32_t linear_id = plat_my_core_pos();
+
+	/* Update this cpu's statistics. */
+	tsp_stats[linear_id].smc_count++;
+	tsp_stats[linear_id].eret_count++;
+
+	INFO("TSP: cpu 0x%lx SYSTEM_RESET request\n", read_mpidr());
+	INFO("TSP: cpu 0x%lx: %d smcs, %d erets requests\n", read_mpidr(),
+	     tsp_stats[linear_id].smc_count,
+	     tsp_stats[linear_id].eret_count);
+
+	/* Indicate to the SPD that we have completed this request. */
+	return set_smc_args(TSP_SYSTEM_RESET_DONE, 0, 0, 0, 0, 0, 0, 0);
+}
+
+/*******************************************************************************
+ * TSP smc abort handler. This function is called when aborting a preempted
+ * yielding SMC request. It should cleanup all resources owned by the SMC
+ * handler such as locks or dynamically allocated memory so following SMC
+ * request are executed in a clean environment.
+ ******************************************************************************/
+smc_args_t *tsp_abort_smc_handler(uint64_t func,
+				  uint64_t arg1,
+				  uint64_t arg2,
+				  uint64_t arg3,
+				  uint64_t arg4,
+				  uint64_t arg5,
+				  uint64_t arg6,
+				  uint64_t arg7)
+{
+	return set_smc_args(TSP_ABORT_DONE, 0, 0, 0, 0, 0, 0, 0);
+}
diff --git a/bl32/tsp/tsp_ffa_main.c b/bl32/tsp/tsp_ffa_main.c
new file mode 100644
index 0000000..53dbd03
--- /dev/null
+++ b/bl32/tsp/tsp_ffa_main.c
@@ -0,0 +1,655 @@
+/*
+ * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <inttypes.h>
+#include <stdint.h>
+
+#include "../../services/std_svc/spm/el3_spmc/spmc.h"
+#include "../../services/std_svc/spm/el3_spmc/spmc_shared_mem.h"
+#include <arch_features.h>
+#include <arch_helpers.h>
+#include <bl32/tsp/tsp.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include "ffa_helpers.h"
+#include <lib/psci/psci.h>
+#include <lib/spinlock.h>
+#include <lib/xlat_tables/xlat_tables_defs.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <plat/common/platform.h>
+#include <platform_tsp.h>
+#include <services/ffa_svc.h>
+#include "tsp_private.h"
+
+#include <platform_def.h>
+
+static ffa_endpoint_id16_t tsp_id, spmc_id;
+uint8_t mem_region_buffer[4096 * 2]  __aligned(PAGE_SIZE);
+
+/* Partition Mailbox. */
+static uint8_t send_page[PAGE_SIZE] __aligned(PAGE_SIZE);
+static uint8_t recv_page[PAGE_SIZE] __aligned(PAGE_SIZE);
+
+/*
+ * Declare a global mailbox for use within the TSP.
+ * This will be initialized appropriately when the buffers
+ * are mapped with the SPMC.
+ */
+static struct mailbox mailbox;
+
+/*******************************************************************************
+ * This enum is used to handle test cases driven from the FF-A Test Driver.
+ ******************************************************************************/
+/* Keep in Sync with FF-A Test Driver. */
+enum message_t {
+	/* Partition Only Messages. */
+	FF_A_RELAY_MESSAGE = 0,
+
+	/* Basic Functionality. */
+	FF_A_ECHO_MESSAGE,
+	FF_A_RELAY_MESSAGE_EL3,
+
+	/* Memory Sharing. */
+	FF_A_MEMORY_SHARE,
+	FF_A_MEMORY_SHARE_FRAGMENTED,
+	FF_A_MEMORY_LEND,
+	FF_A_MEMORY_LEND_FRAGMENTED,
+
+	FF_A_MEMORY_SHARE_MULTI_ENDPOINT,
+	FF_A_MEMORY_LEND_MULTI_ENDPOINT,
+
+	LAST,
+	FF_A_RUN_ALL = 255,
+	FF_A_OP_MAX = 256
+};
+
+#if SPMC_AT_EL3
+extern void tsp_cpu_on_entry(void);
+#endif
+
+/*******************************************************************************
+ * Test Functions.
+ ******************************************************************************/
+
+/*******************************************************************************
+ * Enable the TSP to forward the received message to another partition and ask
+ * it to echo the value back in order to validate direct messages functionality.
+ ******************************************************************************/
+static int ffa_test_relay(uint64_t arg0,
+			  uint64_t arg1,
+			  uint64_t arg2,
+			  uint64_t arg3,
+			  uint64_t arg4,
+			  uint64_t arg5,
+			  uint64_t arg6,
+			  uint64_t arg7)
+{
+	smc_args_t ffa_forward_result;
+	ffa_endpoint_id16_t receiver = arg5;
+
+	ffa_forward_result = ffa_msg_send_direct_req(ffa_endpoint_source(arg1),
+						     receiver,
+						     FF_A_ECHO_MESSAGE, arg4,
+						     0, 0, 0);
+	return ffa_forward_result._regs[3];
+}
+
+/*******************************************************************************
+ * This function handles memory management tests, currently share and lend.
+ * This test supports the use of FRAG_RX to use memory descriptors that do not
+ * fit in a single 4KB buffer.
+ ******************************************************************************/
+static int test_memory_send(ffa_endpoint_id16_t sender, uint64_t handle,
+			    ffa_mtd_flag32_t flags, bool multi_endpoint)
+{
+	struct ffa_mtd *m;
+	struct ffa_emad_v1_0 *receivers;
+	struct ffa_comp_mrd *composite;
+	int ret, status = 0;
+	unsigned int mem_attrs;
+	char *ptr;
+	ffa_endpoint_id16_t source = sender;
+	uint32_t total_length, recv_length = 0;
+
+	/*
+	 * In the case that we're testing multiple endpoints choose a partition
+	 * ID that resides in the normal world so the SPMC won't detect it as
+	 * invalid.
+	 * TODO: Should get endpoint receiver id and flag as input from NWd.
+	 */
+	uint32_t receiver_count = multi_endpoint ? 2 : 1;
+	ffa_endpoint_id16_t test_receivers[2] = { tsp_id, 0x10 };
+
+	/* Ensure that the sender ID resides in the normal world. */
+	if (ffa_is_secure_world_id(sender)) {
+		ERROR("Invalid sender ID 0x%x.\n", sender);
+		return FFA_ERROR_DENIED;
+	}
+
+	if (!memory_retrieve(&mailbox, &m, handle, source, test_receivers,
+			     receiver_count, flags, &recv_length,
+			     &total_length)) {
+		return FFA_ERROR_INVALID_PARAMETER;
+	}
+
+	receivers = (struct ffa_emad_v1_0 *)
+		    ((uint8_t *) m + m->emad_offset);
+	while (total_length != recv_length) {
+		smc_args_t ffa_return;
+		uint32_t frag_length;
+
+		ffa_return = ffa_mem_frag_rx(handle, recv_length);
+
+		if (ffa_return._regs[0] == FFA_ERROR) {
+			WARN("TSP: failed to resume mem with handle %lx\n",
+			     handle);
+			return ffa_return._regs[2];
+		}
+		frag_length = ffa_return._regs[3];
+
+		/* Validate frag_length is less than total_length and mailbox size. */
+		if (frag_length > total_length ||
+				frag_length > (mailbox.rxtx_page_count * PAGE_SIZE)) {
+			ERROR("Invalid parameters!\n");
+			return FFA_ERROR_INVALID_PARAMETER;
+		}
+
+		/* Validate frag_length is less than remaining mem_region_buffer size. */
+		if (frag_length + recv_length >= REGION_BUF_SIZE) {
+			ERROR("Out of memory!\n");
+			return FFA_ERROR_INVALID_PARAMETER;
+		}
+
+		memcpy(&mem_region_buffer[recv_length], mailbox.rx_buffer,
+		       frag_length);
+
+		if (ffa_rx_release()) {
+			ERROR("Failed to release buffer!\n");
+			return FFA_ERROR_DENIED;
+		}
+
+		recv_length += frag_length;
+
+		assert(recv_length <= total_length);
+	}
+
+	composite = ffa_memory_region_get_composite(m, 0);
+	if (composite == NULL) {
+		WARN("Failed to get composite descriptor!\n");
+		return FFA_ERROR_INVALID_PARAMETER;
+	}
+
+	VERBOSE("Address: %p; page_count: %x %lx\n",
+		(void *)composite->address_range_array[0].address,
+		composite->address_range_array[0].page_count, PAGE_SIZE);
+
+	/* This test is only concerned with RW permissions. */
+	if (ffa_get_data_access_attr(
+	    receivers[0].mapd.memory_access_permissions) != FFA_MEM_PERM_RW) {
+		ERROR("Data permission in retrieve response %x does not match share/lend %x!\n",
+		      ffa_get_data_access_attr(receivers[0].mapd.memory_access_permissions),
+		      FFA_MEM_PERM_RW);
+		return FFA_ERROR_INVALID_PARAMETER;
+	}
+
+	mem_attrs = MT_RW_DATA | MT_EXECUTE_NEVER;
+
+	/* Only expecting to be sent memory from NWd so map accordingly. */
+	mem_attrs |= MT_NS;
+
+	for (uint32_t i = 0U; i < composite->address_range_count; i++) {
+		size_t size = composite->address_range_array[i].page_count * PAGE_SIZE;
+
+		ptr = (char *) composite->address_range_array[i].address;
+		ret = mmap_add_dynamic_region(
+				(uint64_t)ptr,
+				(uint64_t)ptr,
+				size, mem_attrs);
+
+		if (ret != 0) {
+			ERROR("Failed [%u] mmap_add_dynamic_region %u (%lx) (%lx) (%x)!\n",
+				i, ret,
+				(uint64_t)composite->address_range_array[i].address,
+				size, mem_attrs);
+
+			/* Remove mappings created in this transaction. */
+			for (i--; i >= 0U; i--) {
+				ret = mmap_remove_dynamic_region(
+					(uint64_t)ptr,
+					composite->address_range_array[i].page_count * PAGE_SIZE);
+
+				if (ret != 0) {
+					ERROR("Failed [%d] mmap_remove_dynamic_region!\n", i);
+					panic();
+				}
+			}
+			return FFA_ERROR_NO_MEMORY;
+		}
+
+		/* Increment memory region for validation purposes. */
+		++(*ptr);
+
+		/*
+		 * Read initial magic number from memory region for
+		 * validation purposes.
+		 */
+		if (!i) {
+			status = *ptr;
+		}
+	}
+
+	for (uint32_t i = 0U; i < composite->address_range_count; i++) {
+		ret = mmap_remove_dynamic_region(
+			(uint64_t)composite->address_range_array[i].address,
+			composite->address_range_array[i].page_count * PAGE_SIZE);
+
+		if (ret != 0) {
+			ERROR("Failed [%d] mmap_remove_dynamic_region!\n", i);
+			return FFA_ERROR_NO_MEMORY;
+		}
+	}
+
+	if (!memory_relinquish((struct ffa_mem_relinquish_descriptor *)mailbox.tx_buffer,
+				m->handle, tsp_id)) {
+		ERROR("Failed to relinquish memory region!\n");
+		return FFA_ERROR_INVALID_PARAMETER;
+	}
+	return status;
+}
+
+static smc_args_t *send_ffa_pm_success(void)
+{
+	return set_smc_args(FFA_MSG_SEND_DIRECT_RESP_SMC32,
+			    ((tsp_id & FFA_DIRECT_MSG_ENDPOINT_ID_MASK)
+			    << FFA_DIRECT_MSG_SOURCE_SHIFT) | spmc_id,
+			    FFA_FWK_MSG_BIT |
+			    (FFA_PM_MSG_PM_RESP & FFA_FWK_MSG_MASK),
+			    0, 0, 0, 0, 0);
+}
+
+/*******************************************************************************
+ * This function performs any remaining book keeping in the test secure payload
+ * before this cpu is turned off in response to a psci cpu_off request.
+ ******************************************************************************/
+smc_args_t *tsp_cpu_off_main(uint64_t arg0,
+			     uint64_t arg1,
+			     uint64_t arg2,
+			     uint64_t arg3,
+			     uint64_t arg4,
+			     uint64_t arg5,
+			     uint64_t arg6,
+			     uint64_t arg7)
+{
+	uint32_t linear_id = plat_my_core_pos();
+
+	/*
+	 * This cpu is being turned off, so disable the timer to prevent the
+	 * secure timer interrupt from interfering with power down. A pending
+	 * interrupt will be lost but we do not care as we are turning off.
+	 */
+	tsp_generic_timer_stop();
+
+	/* Update this cpu's statistics. */
+	tsp_stats[linear_id].smc_count++;
+	tsp_stats[linear_id].eret_count++;
+	tsp_stats[linear_id].cpu_off_count++;
+
+	INFO("TSP: cpu 0x%lx off request\n", read_mpidr());
+	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu off requests\n",
+		read_mpidr(),
+		tsp_stats[linear_id].smc_count,
+		tsp_stats[linear_id].eret_count,
+		tsp_stats[linear_id].cpu_off_count);
+
+	return send_ffa_pm_success();
+}
+
+/*******************************************************************************
+ * This function performs any book keeping in the test secure payload before
+ * this cpu's architectural state is saved in response to an earlier psci
+ * cpu_suspend request.
+ ******************************************************************************/
+smc_args_t *tsp_cpu_suspend_main(uint64_t arg0,
+				 uint64_t arg1,
+				 uint64_t arg2,
+				 uint64_t arg3,
+				 uint64_t arg4,
+				 uint64_t arg5,
+				 uint64_t arg6,
+				 uint64_t arg7)
+{
+	uint32_t linear_id = plat_my_core_pos();
+
+	/*
+	 * Save the time context and disable it to prevent the secure timer
+	 * interrupt from interfering with wakeup from the suspend state.
+	 */
+	tsp_generic_timer_save();
+	tsp_generic_timer_stop();
+
+	/* Update this cpu's statistics. */
+	tsp_stats[linear_id].smc_count++;
+	tsp_stats[linear_id].eret_count++;
+	tsp_stats[linear_id].cpu_suspend_count++;
+
+	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu suspend requests\n",
+		read_mpidr(),
+		tsp_stats[linear_id].smc_count,
+		tsp_stats[linear_id].eret_count,
+		tsp_stats[linear_id].cpu_suspend_count);
+
+	return send_ffa_pm_success();
+}
+
+/*******************************************************************************
+ * This function performs any bookkeeping in the test secure payload after this
+ * cpu's architectural state has been restored after wakeup from an earlier psci
+ * cpu_suspend request.
+ ******************************************************************************/
+smc_args_t *tsp_cpu_resume_main(uint64_t max_off_pwrlvl,
+				uint64_t arg1,
+				uint64_t arg2,
+				uint64_t arg3,
+				uint64_t arg4,
+				uint64_t arg5,
+				uint64_t arg6,
+				uint64_t arg7)
+{
+	uint32_t linear_id = plat_my_core_pos();
+
+	/* Restore the generic timer context. */
+	tsp_generic_timer_restore();
+
+	/* Update this cpu's statistics. */
+	tsp_stats[linear_id].smc_count++;
+	tsp_stats[linear_id].eret_count++;
+	tsp_stats[linear_id].cpu_resume_count++;
+
+	INFO("TSP: cpu 0x%lx resumed. maximum off power level %" PRId64 "\n",
+	     read_mpidr(), max_off_pwrlvl);
+	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu resume requests\n",
+		read_mpidr(),
+		tsp_stats[linear_id].smc_count,
+		tsp_stats[linear_id].eret_count,
+		tsp_stats[linear_id].cpu_resume_count);
+
+	return send_ffa_pm_success();
+}
+
+/*******************************************************************************
+ * This function handles framework messages. Currently only PM.
+ ******************************************************************************/
+static smc_args_t *handle_framework_message(uint64_t arg0,
+					    uint64_t arg1,
+					    uint64_t arg2,
+					    uint64_t arg3,
+					    uint64_t arg4,
+					    uint64_t arg5,
+					    uint64_t arg6,
+					    uint64_t arg7)
+{
+	/* Check if it is a power management message from the SPMC. */
+	if (ffa_endpoint_source(arg1) != spmc_id) {
+		goto err;
+	}
+
+	/* Check if it is a PM request message. */
+	if ((arg2 & FFA_FWK_MSG_MASK) == FFA_FWK_MSG_PSCI) {
+		/* Check if it is a PSCI CPU_OFF request. */
+		if (arg3 == PSCI_CPU_OFF) {
+			return tsp_cpu_off_main(arg0, arg1, arg2, arg3,
+						arg4, arg5, arg6, arg7);
+		} else if (arg3 == PSCI_CPU_SUSPEND_AARCH64) {
+			return tsp_cpu_suspend_main(arg0, arg1, arg2, arg3,
+						arg4, arg5, arg6, arg7);
+		}
+	} else if ((arg2 & FFA_FWK_MSG_MASK) == FFA_PM_MSG_WB_REQ) {
+		/* Check it is a PSCI Warm Boot request. */
+		if (arg3 == FFA_WB_TYPE_NOTS2RAM) {
+			return tsp_cpu_resume_main(arg0, arg1, arg2, arg3,
+						arg4, arg5, arg6, arg7);
+		}
+	}
+
+err:
+	ERROR("%s: Unknown framework message!\n", __func__);
+	panic();
+}
+
+/*******************************************************************************
+ * Handles partition messages. Exercised from the FF-A Test Driver.
+ ******************************************************************************/
+static smc_args_t *handle_partition_message(uint64_t arg0,
+					    uint64_t arg1,
+					    uint64_t arg2,
+					    uint64_t arg3,
+					    uint64_t arg4,
+					    uint64_t arg5,
+					    uint64_t arg6,
+					    uint64_t arg7)
+{
+	uint16_t sender = ffa_endpoint_source(arg1);
+	uint16_t receiver = ffa_endpoint_destination(arg1);
+	int status = -1;
+	const bool multi_endpoint = true;
+
+	switch (arg3) {
+	case FF_A_MEMORY_SHARE:
+		INFO("TSP Tests: Memory Share Request--\n");
+		status = test_memory_send(sender, arg4, FFA_FLAG_SHARE_MEMORY, !multi_endpoint);
+		break;
+
+	case FF_A_MEMORY_LEND:
+		INFO("TSP Tests: Memory Lend Request--\n");
+		status = test_memory_send(sender, arg4, FFA_FLAG_LEND_MEMORY, !multi_endpoint);
+		break;
+
+	case FF_A_MEMORY_SHARE_MULTI_ENDPOINT:
+		INFO("TSP Tests: Multi Endpoint Memory Share Request--\n");
+		status = test_memory_send(sender, arg4, FFA_FLAG_SHARE_MEMORY, multi_endpoint);
+		break;
+
+	case FF_A_MEMORY_LEND_MULTI_ENDPOINT:
+		INFO("TSP Tests: Multi Endpoint Memory Lend Request--\n");
+		status = test_memory_send(sender, arg4, FFA_FLAG_LEND_MEMORY, multi_endpoint);
+		break;
+	case FF_A_RELAY_MESSAGE:
+		INFO("TSP Tests: Relaying message--\n");
+		status = ffa_test_relay(arg0, arg1, arg2, arg3, arg4,
+					arg5, arg6, arg7);
+		break;
+
+	case FF_A_ECHO_MESSAGE:
+		INFO("TSP Tests: echo message--\n");
+		status = arg4;
+		break;
+
+	default:
+		INFO("TSP Tests: Unknown request ID %d--\n", (int) arg3);
+	}
+
+	/* Swap the sender and receiver in the response. */
+	return ffa_msg_send_direct_resp(receiver, sender, status, 0, 0, 0, 0);
+}
+
+/*******************************************************************************
+ * This function implements the event loop for handling FF-A ABI invocations.
+ ******************************************************************************/
+static smc_args_t *tsp_event_loop(uint64_t smc_fid,
+				  uint64_t arg1,
+				  uint64_t arg2,
+				  uint64_t arg3,
+				  uint64_t arg4,
+				  uint64_t arg5,
+				  uint64_t arg6,
+				  uint64_t arg7)
+{
+	/* Panic if the SPMC did not forward an FF-A call. */
+	if (!is_ffa_fid(smc_fid)) {
+		ERROR("%s: Unknown SMC FID (0x%lx)\n", __func__, smc_fid);
+		panic();
+	}
+
+	switch (smc_fid) {
+	case FFA_INTERRUPT:
+		/*
+		 * IRQs were enabled upon re-entry into the TSP. The interrupt
+		 * must have been handled by now. Return to the SPMC indicating
+		 * the same.
+		 */
+		return set_smc_args(FFA_MSG_WAIT, 0, 0, 0, 0, 0, 0, 0);
+
+	case FFA_MSG_SEND_DIRECT_REQ_SMC64:
+	case FFA_MSG_SEND_DIRECT_REQ_SMC32:
+		/* Check if a framework message, handle accordingly. */
+		if ((arg2 & FFA_FWK_MSG_BIT)) {
+			return handle_framework_message(smc_fid, arg1, arg2, arg3,
+							arg4, arg5, arg6, arg7);
+		}
+		return handle_partition_message(smc_fid, arg1, arg2, arg3,
+							arg4, arg5, arg6, arg7);
+	}
+
+	ERROR("%s: Unsupported FF-A FID (0x%lx)\n", __func__, smc_fid);
+	panic();
+}
+
+static smc_args_t *tsp_loop(smc_args_t *args)
+{
+	smc_args_t ret;
+
+	do {
+		/* --------------------------------------------
+		 * Mask FIQ interrupts to avoid preemption
+		 * in case EL3 SPMC delegates an IRQ next or a
+		 * managed exit. Lastly, unmask IRQs so that
+		 * they can be handled immediately upon re-entry.
+		 *  ---------------------------------------------
+		 */
+		write_daifset(DAIF_FIQ_BIT);
+		write_daifclr(DAIF_IRQ_BIT);
+		ret = smc_helper(args->_regs[0], args->_regs[1], args->_regs[2],
+			       args->_regs[3], args->_regs[4], args->_regs[5],
+			       args->_regs[6], args->_regs[7]);
+		args = tsp_event_loop(ret._regs[0], ret._regs[1], ret._regs[2],
+				ret._regs[3], ret._regs[4], ret._regs[5],
+				ret._regs[6], ret._regs[7]);
+	} while (1);
+
+	/* Not Reached. */
+	return NULL;
+}
+
+/*******************************************************************************
+ * TSP main entry point where it gets the opportunity to initialize its secure
+ * state/applications. Once the state is initialized, it must return to the
+ * SPD with a pointer to the 'tsp_vector_table' jump table.
+ ******************************************************************************/
+uint64_t tsp_main(void)
+{
+	smc_args_t smc_args = {0};
+
+	NOTICE("TSP: %s\n", version_string);
+	NOTICE("TSP: %s\n", build_message);
+	INFO("TSP: Total memory base : 0x%lx\n", (unsigned long) BL32_BASE);
+	INFO("TSP: Total memory size : 0x%lx bytes\n", BL32_TOTAL_SIZE);
+	uint32_t linear_id = plat_my_core_pos();
+
+	/* Initialize the platform. */
+	tsp_platform_setup();
+
+	/* Initialize secure/applications state here. */
+	tsp_generic_timer_start();
+
+	/* Register secondary entrypoint with the SPMC. */
+	smc_args = smc_helper(FFA_SECONDARY_EP_REGISTER_SMC64,
+			(uint64_t) tsp_cpu_on_entry,
+			0, 0, 0, 0, 0, 0);
+	if (smc_args._regs[SMC_ARG0] != FFA_SUCCESS_SMC32) {
+		ERROR("TSP could not register secondary ep (0x%lx)\n",
+				smc_args._regs[2]);
+		panic();
+	}
+	/* Get TSP's endpoint id. */
+	smc_args = smc_helper(FFA_ID_GET, 0, 0, 0, 0, 0, 0, 0);
+	if (smc_args._regs[SMC_ARG0] != FFA_SUCCESS_SMC32) {
+		ERROR("TSP could not get own ID (0x%lx) on core%d\n",
+				smc_args._regs[2], linear_id);
+		panic();
+	}
+
+	tsp_id = smc_args._regs[2];
+	INFO("TSP FF-A endpoint id = 0x%x\n", tsp_id);
+
+	/* Get the SPMC ID. */
+	smc_args = smc_helper(FFA_SPM_ID_GET, 0, 0, 0, 0, 0, 0, 0);
+	if (smc_args._regs[SMC_ARG0] != FFA_SUCCESS_SMC32) {
+		ERROR("TSP could not get SPMC ID (0x%lx) on core%d\n",
+				smc_args._regs[2], linear_id);
+		panic();
+	}
+
+	spmc_id = smc_args._regs[2];
+
+	/* Call RXTX_MAP to map a 4k RX and TX buffer. */
+	if (ffa_rxtx_map((uintptr_t) send_page,
+			 (uintptr_t) recv_page, 1)) {
+		ERROR("TSP could not map it's RX/TX Buffers\n");
+		panic();
+	}
+
+	mailbox.tx_buffer = send_page;
+	mailbox.rx_buffer = recv_page;
+	mailbox.rxtx_page_count = 1;
+
+	/* Update this cpu's statistics. */
+	tsp_stats[linear_id].smc_count++;
+	tsp_stats[linear_id].eret_count++;
+	tsp_stats[linear_id].cpu_on_count++;
+
+	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu on requests\n",
+			read_mpidr(),
+			tsp_stats[linear_id].smc_count,
+			tsp_stats[linear_id].eret_count,
+			tsp_stats[linear_id].cpu_on_count);
+
+	/* Tell SPMD that we are done initialising. */
+	tsp_loop(set_smc_args(FFA_MSG_WAIT, 0, 0, 0, 0, 0, 0, 0));
+
+	/* Not reached. */
+	return 0;
+}
+
+/*******************************************************************************
+ * This function performs any remaining book keeping in the test secure payload
+ * after this cpu's architectural state has been setup in response to an earlier
+ * psci cpu_on request.
+ ******************************************************************************/
+smc_args_t *tsp_cpu_on_main(void)
+{
+	uint32_t linear_id = plat_my_core_pos();
+
+	/* Initialize secure/applications state here. */
+	tsp_generic_timer_start();
+
+	/* Update this cpu's statistics. */
+	tsp_stats[linear_id].smc_count++;
+	tsp_stats[linear_id].eret_count++;
+	tsp_stats[linear_id].cpu_on_count++;
+	INFO("TSP: cpu 0x%lx turned on\n", read_mpidr());
+	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu on requests\n",
+			read_mpidr(),
+			tsp_stats[linear_id].smc_count,
+			tsp_stats[linear_id].eret_count,
+			tsp_stats[linear_id].cpu_on_count);
+	/* ---------------------------------------------
+	 * Jump to the main event loop to return to EL3
+	 * and be ready for the next request on this cpu.
+	 * ---------------------------------------------
+	 */
+	return tsp_loop(set_smc_args(FFA_MSG_WAIT, 0, 0, 0, 0, 0, 0, 0));
+}
diff --git a/bl32/tsp/tsp_interrupt.c b/bl32/tsp/tsp_interrupt.c
index 430b5dd..a847b6c 100644
--- a/bl32/tsp/tsp_interrupt.c
+++ b/bl32/tsp/tsp_interrupt.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -35,8 +35,6 @@
 	if (type == TSP_HANDLE_SEL1_INTR_AND_RETURN)
 		tsp_stats[linear_id].sync_sel1_intr_ret_count++;
 
-#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
-	spin_lock(&console_lock);
 	VERBOSE("TSP: cpu 0x%lx sync s-el1 interrupt request from 0x%" PRIx64 "\n",
 		read_mpidr(), elr_el3);
 	VERBOSE("TSP: cpu 0x%lx: %d sync s-el1 interrupt requests,"
@@ -44,8 +42,6 @@
 		read_mpidr(),
 		tsp_stats[linear_id].sync_sel1_intr_count,
 		tsp_stats[linear_id].sync_sel1_intr_ret_count);
-	spin_unlock(&console_lock);
-#endif
 }
 
 /******************************************************************************
@@ -58,12 +54,8 @@
 	uint32_t linear_id = plat_my_core_pos();
 
 	tsp_stats[linear_id].preempt_intr_count++;
-#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
-	spin_lock(&console_lock);
 	VERBOSE("TSP: cpu 0x%lx: %d preempt interrupt requests\n",
 		read_mpidr(), tsp_stats[linear_id].preempt_intr_count);
-	spin_unlock(&console_lock);
-#endif
 	return TSP_PREEMPTED;
 }
 
@@ -91,8 +83,18 @@
 	id = plat_ic_get_pending_interrupt_id();
 
 	/* TSP can only handle the secure physical timer interrupt */
-	if (id != TSP_IRQ_SEC_PHY_TIMER)
+	if (id != TSP_IRQ_SEC_PHY_TIMER) {
+#if SPMC_AT_EL3
+		/*
+		 * With the EL3 FF-A SPMC we expect only Timer secure interrupt to fire in
+		 * the TSP, so panic if any other interrupt does.
+		 */
+		ERROR("Unexpected interrupt id %u\n", id);
+		panic();
+#else
 		return tsp_handle_preemption();
+#endif
+	}
 
 	/*
 	 * Acknowledge and handle the secure timer interrupt. Also sanity check
@@ -105,13 +107,9 @@
 
 	/* Update the statistics and print some messages */
 	tsp_stats[linear_id].sel1_intr_count++;
-#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
-	spin_lock(&console_lock);
 	VERBOSE("TSP: cpu 0x%lx handled S-EL1 interrupt %d\n",
 	       read_mpidr(), id);
 	VERBOSE("TSP: cpu 0x%lx: %d S-EL1 requests\n",
 	     read_mpidr(), tsp_stats[linear_id].sel1_intr_count);
-	spin_unlock(&console_lock);
-#endif
 	return 0;
 }
diff --git a/bl32/tsp/tsp_main.c b/bl32/tsp/tsp_main.c
index 522c1b4..df9903b 100644
--- a/bl32/tsp/tsp_main.c
+++ b/bl32/tsp/tsp_main.c
@@ -15,85 +15,10 @@
 #include <common/debug.h>
 #include <lib/spinlock.h>
 #include <plat/common/platform.h>
-#include <platform_def.h>
 #include <platform_tsp.h>
-
 #include "tsp_private.h"
 
-
-/*******************************************************************************
- * Lock to control access to the console
- ******************************************************************************/
-spinlock_t console_lock;
-
-/*******************************************************************************
- * Per cpu data structure to populate parameters for an SMC in C code and use
- * a pointer to this structure in assembler code to populate x0-x7
- ******************************************************************************/
-static tsp_args_t tsp_smc_args[PLATFORM_CORE_COUNT];
-
-/*******************************************************************************
- * Per cpu data structure to keep track of TSP activity
- ******************************************************************************/
-work_statistics_t tsp_stats[PLATFORM_CORE_COUNT];
-
-/*******************************************************************************
- * The TSP memory footprint starts at address BL32_BASE and ends with the
- * linker symbol __BL32_END__. Use these addresses to compute the TSP image
- * size.
- ******************************************************************************/
-#define BL32_TOTAL_LIMIT BL32_END
-#define BL32_TOTAL_SIZE (BL32_TOTAL_LIMIT - (unsigned long) BL32_BASE)
-
-static tsp_args_t *set_smc_args(uint64_t arg0,
-			     uint64_t arg1,
-			     uint64_t arg2,
-			     uint64_t arg3,
-			     uint64_t arg4,
-			     uint64_t arg5,
-			     uint64_t arg6,
-			     uint64_t arg7)
-{
-	uint32_t linear_id;
-	tsp_args_t *pcpu_smc_args;
-
-	/*
-	 * Return to Secure Monitor by raising an SMC. The results of the
-	 * service are passed as an arguments to the SMC
-	 */
-	linear_id = plat_my_core_pos();
-	pcpu_smc_args = &tsp_smc_args[linear_id];
-	write_sp_arg(pcpu_smc_args, TSP_ARG0, arg0);
-	write_sp_arg(pcpu_smc_args, TSP_ARG1, arg1);
-	write_sp_arg(pcpu_smc_args, TSP_ARG2, arg2);
-	write_sp_arg(pcpu_smc_args, TSP_ARG3, arg3);
-	write_sp_arg(pcpu_smc_args, TSP_ARG4, arg4);
-	write_sp_arg(pcpu_smc_args, TSP_ARG5, arg5);
-	write_sp_arg(pcpu_smc_args, TSP_ARG6, arg6);
-	write_sp_arg(pcpu_smc_args, TSP_ARG7, arg7);
-
-	return pcpu_smc_args;
-}
-
-/*******************************************************************************
- * Setup function for TSP.
- ******************************************************************************/
-void tsp_setup(void)
-{
-	/* Perform early platform-specific setup */
-	tsp_early_platform_setup();
-
-	/* Perform late platform-specific setup */
-	tsp_plat_arch_setup();
-
-#if ENABLE_PAUTH
-	/*
-	 * Assert that the ARMv8.3-PAuth registers are present or an access
-	 * fault will be triggered when they are being saved or restored.
-	 */
-	assert(is_armv8_3_pauth_present());
-#endif /* ENABLE_PAUTH */
-}
+#include <platform_def.h>
 
 /*******************************************************************************
  * TSP main entry point where it gets the opportunity to initialize its secure
@@ -120,15 +45,11 @@
 	tsp_stats[linear_id].eret_count++;
 	tsp_stats[linear_id].cpu_on_count++;
 
-#if LOG_LEVEL >= LOG_LEVEL_INFO
-	spin_lock(&console_lock);
 	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu on requests\n",
 	     read_mpidr(),
 	     tsp_stats[linear_id].smc_count,
 	     tsp_stats[linear_id].eret_count,
 	     tsp_stats[linear_id].cpu_on_count);
-	spin_unlock(&console_lock);
-#endif
 	return (uint64_t) &tsp_vector_table;
 }
 
@@ -137,7 +58,7 @@
  * after this cpu's architectural state has been setup in response to an earlier
  * psci cpu_on request.
  ******************************************************************************/
-tsp_args_t *tsp_cpu_on_main(void)
+smc_args_t *tsp_cpu_on_main(void)
 {
 	uint32_t linear_id = plat_my_core_pos();
 
@@ -149,16 +70,12 @@
 	tsp_stats[linear_id].eret_count++;
 	tsp_stats[linear_id].cpu_on_count++;
 
-#if LOG_LEVEL >= LOG_LEVEL_INFO
-	spin_lock(&console_lock);
 	INFO("TSP: cpu 0x%lx turned on\n", read_mpidr());
 	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu on requests\n",
 		read_mpidr(),
 		tsp_stats[linear_id].smc_count,
 		tsp_stats[linear_id].eret_count,
 		tsp_stats[linear_id].cpu_on_count);
-	spin_unlock(&console_lock);
-#endif
 	/* Indicate to the SPD that we have completed turned ourselves on */
 	return set_smc_args(TSP_ON_DONE, 0, 0, 0, 0, 0, 0, 0);
 }
@@ -167,7 +84,7 @@
  * This function performs any remaining book keeping in the test secure payload
  * before this cpu is turned off in response to a psci cpu_off request.
  ******************************************************************************/
-tsp_args_t *tsp_cpu_off_main(uint64_t arg0,
+smc_args_t *tsp_cpu_off_main(uint64_t arg0,
 			   uint64_t arg1,
 			   uint64_t arg2,
 			   uint64_t arg3,
@@ -190,16 +107,12 @@
 	tsp_stats[linear_id].eret_count++;
 	tsp_stats[linear_id].cpu_off_count++;
 
-#if LOG_LEVEL >= LOG_LEVEL_INFO
-	spin_lock(&console_lock);
 	INFO("TSP: cpu 0x%lx off request\n", read_mpidr());
 	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu off requests\n",
 		read_mpidr(),
 		tsp_stats[linear_id].smc_count,
 		tsp_stats[linear_id].eret_count,
 		tsp_stats[linear_id].cpu_off_count);
-	spin_unlock(&console_lock);
-#endif
 
 	/* Indicate to the SPD that we have completed this request */
 	return set_smc_args(TSP_OFF_DONE, 0, 0, 0, 0, 0, 0, 0);
@@ -210,7 +123,7 @@
  * this cpu's architectural state is saved in response to an earlier psci
  * cpu_suspend request.
  ******************************************************************************/
-tsp_args_t *tsp_cpu_suspend_main(uint64_t arg0,
+smc_args_t *tsp_cpu_suspend_main(uint64_t arg0,
 			       uint64_t arg1,
 			       uint64_t arg2,
 			       uint64_t arg3,
@@ -233,15 +146,11 @@
 	tsp_stats[linear_id].eret_count++;
 	tsp_stats[linear_id].cpu_suspend_count++;
 
-#if LOG_LEVEL >= LOG_LEVEL_INFO
-	spin_lock(&console_lock);
 	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu suspend requests\n",
 		read_mpidr(),
 		tsp_stats[linear_id].smc_count,
 		tsp_stats[linear_id].eret_count,
 		tsp_stats[linear_id].cpu_suspend_count);
-	spin_unlock(&console_lock);
-#endif
 
 	/* Indicate to the SPD that we have completed this request */
 	return set_smc_args(TSP_SUSPEND_DONE, 0, 0, 0, 0, 0, 0, 0);
@@ -252,7 +161,7 @@
  * cpu's architectural state has been restored after wakeup from an earlier psci
  * cpu_suspend request.
  ******************************************************************************/
-tsp_args_t *tsp_cpu_resume_main(uint64_t max_off_pwrlvl,
+smc_args_t *tsp_cpu_resume_main(uint64_t max_off_pwrlvl,
 			      uint64_t arg1,
 			      uint64_t arg2,
 			      uint64_t arg3,
@@ -271,8 +180,6 @@
 	tsp_stats[linear_id].eret_count++;
 	tsp_stats[linear_id].cpu_resume_count++;
 
-#if LOG_LEVEL >= LOG_LEVEL_INFO
-	spin_lock(&console_lock);
 	INFO("TSP: cpu 0x%lx resumed. maximum off power level %" PRId64 "\n",
 	     read_mpidr(), max_off_pwrlvl);
 	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu resume requests\n",
@@ -280,83 +187,17 @@
 		tsp_stats[linear_id].smc_count,
 		tsp_stats[linear_id].eret_count,
 		tsp_stats[linear_id].cpu_resume_count);
-	spin_unlock(&console_lock);
-#endif
 	/* Indicate to the SPD that we have completed this request */
 	return set_smc_args(TSP_RESUME_DONE, 0, 0, 0, 0, 0, 0, 0);
 }
 
 /*******************************************************************************
- * This function performs any remaining bookkeeping in the test secure payload
- * before the system is switched off (in response to a psci SYSTEM_OFF request)
- ******************************************************************************/
-tsp_args_t *tsp_system_off_main(uint64_t arg0,
-				uint64_t arg1,
-				uint64_t arg2,
-				uint64_t arg3,
-				uint64_t arg4,
-				uint64_t arg5,
-				uint64_t arg6,
-				uint64_t arg7)
-{
-	uint32_t linear_id = plat_my_core_pos();
-
-	/* Update this cpu's statistics */
-	tsp_stats[linear_id].smc_count++;
-	tsp_stats[linear_id].eret_count++;
-
-#if LOG_LEVEL >= LOG_LEVEL_INFO
-	spin_lock(&console_lock);
-	INFO("TSP: cpu 0x%lx SYSTEM_OFF request\n", read_mpidr());
-	INFO("TSP: cpu 0x%lx: %d smcs, %d erets requests\n", read_mpidr(),
-	     tsp_stats[linear_id].smc_count,
-	     tsp_stats[linear_id].eret_count);
-	spin_unlock(&console_lock);
-#endif
-
-	/* Indicate to the SPD that we have completed this request */
-	return set_smc_args(TSP_SYSTEM_OFF_DONE, 0, 0, 0, 0, 0, 0, 0);
-}
-
-/*******************************************************************************
- * This function performs any remaining bookkeeping in the test secure payload
- * before the system is reset (in response to a psci SYSTEM_RESET request)
- ******************************************************************************/
-tsp_args_t *tsp_system_reset_main(uint64_t arg0,
-				uint64_t arg1,
-				uint64_t arg2,
-				uint64_t arg3,
-				uint64_t arg4,
-				uint64_t arg5,
-				uint64_t arg6,
-				uint64_t arg7)
-{
-	uint32_t linear_id = plat_my_core_pos();
-
-	/* Update this cpu's statistics */
-	tsp_stats[linear_id].smc_count++;
-	tsp_stats[linear_id].eret_count++;
-
-#if LOG_LEVEL >= LOG_LEVEL_INFO
-	spin_lock(&console_lock);
-	INFO("TSP: cpu 0x%lx SYSTEM_RESET request\n", read_mpidr());
-	INFO("TSP: cpu 0x%lx: %d smcs, %d erets requests\n", read_mpidr(),
-	     tsp_stats[linear_id].smc_count,
-	     tsp_stats[linear_id].eret_count);
-	spin_unlock(&console_lock);
-#endif
-
-	/* Indicate to the SPD that we have completed this request */
-	return set_smc_args(TSP_SYSTEM_RESET_DONE, 0, 0, 0, 0, 0, 0, 0);
-}
-
-/*******************************************************************************
  * TSP fast smc handler. The secure monitor jumps to this function by
  * doing the ERET after populating X0-X7 registers. The arguments are received
  * in the function arguments in order. Once the service is rendered, this
  * function returns to Secure Monitor by raising SMC.
  ******************************************************************************/
-tsp_args_t *tsp_smc_handler(uint64_t func,
+smc_args_t *tsp_smc_handler(uint64_t func,
 			       uint64_t arg1,
 			       uint64_t arg2,
 			       uint64_t arg3,
@@ -376,16 +217,12 @@
 	tsp_stats[linear_id].smc_count++;
 	tsp_stats[linear_id].eret_count++;
 
-#if LOG_LEVEL >= LOG_LEVEL_INFO
-	spin_lock(&console_lock);
 	INFO("TSP: cpu 0x%lx received %s smc 0x%" PRIx64 "\n", read_mpidr(),
 		((func >> 31) & 1) == 1 ? "fast" : "yielding",
 		func);
 	INFO("TSP: cpu 0x%lx: %d smcs, %d erets\n", read_mpidr(),
 		tsp_stats[linear_id].smc_count,
 		tsp_stats[linear_id].eret_count);
-	spin_unlock(&console_lock);
-#endif
 
 	/* Render secure services and obtain results here */
 	results[0] = arg1;
@@ -427,11 +264,7 @@
 		break;
 	case TSP_CHECK_DIT:
 		if (!is_armv8_4_dit_present()) {
-#if LOG_LEVEL >= LOG_LEVEL_ERROR
-			spin_lock(&console_lock);
 			ERROR("DIT not supported\n");
-			spin_unlock(&console_lock);
-#endif
 			results[0] = 0;
 			results[1] = 0xffff;
 			break;
@@ -451,21 +284,3 @@
 			    results[1],
 			    0, 0, 0, 0);
 }
-
-/*******************************************************************************
- * TSP smc abort handler. This function is called when aborting a preempted
- * yielding SMC request. It should cleanup all resources owned by the SMC
- * handler such as locks or dynamically allocated memory so following SMC
- * request are executed in a clean environment.
- ******************************************************************************/
-tsp_args_t *tsp_abort_smc_handler(uint64_t func,
-				  uint64_t arg1,
-				  uint64_t arg2,
-				  uint64_t arg3,
-				  uint64_t arg4,
-				  uint64_t arg5,
-				  uint64_t arg6,
-				  uint64_t arg7)
-{
-	return set_smc_args(TSP_ABORT_DONE, 0, 0, 0, 0, 0, 0, 0);
-}
diff --git a/bl32/tsp/tsp_private.h b/bl32/tsp/tsp_private.h
index 38d9732..66873e2 100644
--- a/bl32/tsp/tsp_private.h
+++ b/bl32/tsp/tsp_private.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,28 +7,22 @@
 #ifndef TSP_PRIVATE_H
 #define TSP_PRIVATE_H
 
-/* Definitions to help the assembler access the SMC/ERET args structure */
-#define TSP_ARGS_SIZE		0x40
-#define TSP_ARG0		0x0
-#define TSP_ARG1		0x8
-#define TSP_ARG2		0x10
-#define TSP_ARG3		0x18
-#define TSP_ARG4		0x20
-#define TSP_ARG5		0x28
-#define TSP_ARG6		0x30
-#define TSP_ARG7		0x38
-#define TSP_ARGS_END		0x40
-
+/*******************************************************************************
+ * The TSP memory footprint starts at address BL32_BASE and ends with the
+ * linker symbol __BL32_END__. Use these addresses to compute the TSP image
+ * size.
+ ******************************************************************************/
+#define BL32_TOTAL_LIMIT BL32_END
+#define BL32_TOTAL_SIZE (BL32_TOTAL_LIMIT - (unsigned long) BL32_BASE)
 
 #ifndef __ASSEMBLER__
 
 #include <stdint.h>
 
-#include <platform_def.h> /* For CACHE_WRITEBACK_GRANULE */
-
 #include <bl32/tsp/tsp.h>
 #include <lib/cassert.h>
 #include <lib/spinlock.h>
+#include <smccc_helpers.h>
 
 typedef struct work_statistics {
 	/* Number of s-el1 interrupts on this cpu */
@@ -47,23 +41,22 @@
 	uint32_t cpu_resume_count;	/* Number of cpu resume requests */
 } __aligned(CACHE_WRITEBACK_GRANULE) work_statistics_t;
 
-typedef struct tsp_args {
-	uint64_t _regs[TSP_ARGS_END >> 3];
-} __aligned(CACHE_WRITEBACK_GRANULE) tsp_args_t;
-
 /* Macros to access members of the above structure using their offsets */
 #define read_sp_arg(args, offset)	((args)->_regs[offset >> 3])
 #define write_sp_arg(args, offset, val) (((args)->_regs[offset >> 3])	\
 					 = val)
-/*
- * Ensure that the assembler's view of the size of the tsp_args is the
- * same as the compilers
- */
-CASSERT(TSP_ARGS_SIZE == sizeof(tsp_args_t), assert_sp_args_size_mismatch);
 
 uint128_t tsp_get_magic(void);
 
-tsp_args_t *tsp_cpu_resume_main(uint64_t max_off_pwrlvl,
+smc_args_t *set_smc_args(uint64_t arg0,
+			 uint64_t arg1,
+			 uint64_t arg2,
+			 uint64_t arg3,
+			 uint64_t arg4,
+			 uint64_t arg5,
+			 uint64_t arg6,
+			 uint64_t arg7);
+smc_args_t *tsp_cpu_resume_main(uint64_t max_off_pwrlvl,
 				uint64_t arg1,
 				uint64_t arg2,
 				uint64_t arg3,
@@ -71,7 +64,7 @@
 				uint64_t arg5,
 				uint64_t arg6,
 				uint64_t arg7);
-tsp_args_t *tsp_cpu_suspend_main(uint64_t arg0,
+smc_args_t *tsp_cpu_suspend_main(uint64_t arg0,
 				 uint64_t arg1,
 				 uint64_t arg2,
 				 uint64_t arg3,
@@ -79,8 +72,8 @@
 				 uint64_t arg5,
 				 uint64_t arg6,
 				 uint64_t arg7);
-tsp_args_t *tsp_cpu_on_main(void);
-tsp_args_t *tsp_cpu_off_main(uint64_t arg0,
+smc_args_t *tsp_cpu_on_main(void);
+smc_args_t *tsp_cpu_off_main(uint64_t arg0,
 			     uint64_t arg1,
 			     uint64_t arg2,
 			     uint64_t arg3,
@@ -101,7 +94,6 @@
 
 
 /* Data structure to keep track of TSP statistics */
-extern spinlock_t console_lock;
 extern work_statistics_t tsp_stats[PLATFORM_CORE_COUNT];
 
 /* Vector table of jumps */
@@ -111,7 +103,7 @@
 int32_t tsp_common_int_handler(void);
 int32_t tsp_handle_preemption(void);
 
-tsp_args_t *tsp_abort_smc_handler(uint64_t func,
+smc_args_t *tsp_abort_smc_handler(uint64_t func,
 				  uint64_t arg1,
 				  uint64_t arg2,
 				  uint64_t arg3,
@@ -120,25 +112,25 @@
 				  uint64_t arg6,
 				  uint64_t arg7);
 
-tsp_args_t *tsp_smc_handler(uint64_t func,
-			       uint64_t arg1,
-			       uint64_t arg2,
-			       uint64_t arg3,
-			       uint64_t arg4,
-			       uint64_t arg5,
-			       uint64_t arg6,
-			       uint64_t arg7);
+smc_args_t *tsp_smc_handler(uint64_t func,
+			    uint64_t arg1,
+			    uint64_t arg2,
+			    uint64_t arg3,
+			    uint64_t arg4,
+			    uint64_t arg5,
+			    uint64_t arg6,
+			    uint64_t arg7);
 
-tsp_args_t *tsp_system_reset_main(uint64_t arg0,
-				uint64_t arg1,
-				uint64_t arg2,
-				uint64_t arg3,
-				uint64_t arg4,
-				uint64_t arg5,
-				uint64_t arg6,
-				uint64_t arg7);
+smc_args_t *tsp_system_reset_main(uint64_t arg0,
+				  uint64_t arg1,
+				  uint64_t arg2,
+				  uint64_t arg3,
+				  uint64_t arg4,
+				  uint64_t arg5,
+				  uint64_t arg6,
+				  uint64_t arg7);
 
-tsp_args_t *tsp_system_off_main(uint64_t arg0,
+smc_args_t *tsp_system_off_main(uint64_t arg0,
 				uint64_t arg1,
 				uint64_t arg2,
 				uint64_t arg3,
diff --git a/changelog.yaml b/changelog.yaml
index c4028c4..d3e235d 100644
--- a/changelog.yaml
+++ b/changelog.yaml
@@ -125,6 +125,9 @@
       - title: Extended Cache Index (FEAT_CCIDX)
         scope: ccidx
 
+      - title: Trapping support for RNDR/RNDRRS (FEAT_RNG_TRAP)
+        scope: rng-trap
+
   - title: Platforms
 
     subsections:
@@ -546,6 +549,13 @@
       - title: BL31
         scope: bl31
 
+      - title: BL32
+        scope: bl32
+
+        subsections:
+          - title: TSP
+            scope: tsp
+
   - title: Services
     scope: services
 
diff --git a/common/bl_common.c b/common/bl_common.c
index 9bfaafd..8fce02f 100644
--- a/common/bl_common.c
+++ b/common/bl_common.c
@@ -269,3 +269,12 @@
 #endif
 #undef PRINT_IMAGE_ARG
 }
+
+/*
+ * This function is for returning the TF-A version
+ */
+const char *get_version(void)
+{
+	extern const char version[];
+	return version;
+}
diff --git a/common/feat_detect.c b/common/feat_detect.c
index be3e20e..ee34588 100644
--- a/common/feat_detect.c
+++ b/common/feat_detect.c
@@ -254,6 +254,16 @@
 #endif
 }
 
+/******************************************************************
+ * Feature : FEAT_RNG_TRAP (Trapping support for RNDR/RNDRRS)
+ *****************************************************************/
+static void read_feat_rng_trap(void)
+{
+#if (ENABLE_FEAT_RNG_TRAP == FEAT_STATE_1)
+	feat_detect_panic(is_feat_rng_trap_present(), "RNG_TRAP");
+#endif
+}
+
 /***********************************************************************************
  * TF-A supports many Arm architectural features starting from arch version
  * (8.0 till 8.7+). These features are mostly enabled through build flags. This
@@ -304,6 +314,7 @@
 	read_feat_mte();
 	read_feat_rng();
 	read_feat_bti();
+	read_feat_rng_trap();
 
 	/* v8.6 features */
 	read_feat_amuv1p1();
diff --git a/docs/design/cpu-specific-build-macros.rst b/docs/design/cpu-specific-build-macros.rst
index 2ddccac..52e1c7d 100644
--- a/docs/design/cpu-specific-build-macros.rst
+++ b/docs/design/cpu-specific-build-macros.rst
@@ -335,6 +335,10 @@
   Cortex-A78C CPU. This needs to be enabled for revisions r0p1, r0p2 and
   it is still open.
 
+- ``ERRATA_A78C_2395411`` : This applies errata 2395411 workaround to
+  Cortex-A78C CPU. This needs to be enabled for revisions r0p1 and r0p2. This
+  erratum is still open.
+
 For Cortex-X1 CPU, the following errata build flags are defined:
 
 - ``ERRATA_X1_1821534`` : This applies errata 1821534 workaround to Cortex-X1
@@ -390,6 +394,10 @@
 
 For Neoverse V1, the following errata build flags are defined :
 
+-  ``ERRATA_V1_1618635``: This applies errata 1618635 workaround to Neoverse-V1
+   CPU. This needs to be enabled for revision r0p0 of the CPU, it is fixed in
+   r1p0.
+
 -  ``ERRATA_V1_1774420``: This applies errata 1774420 workaround to Neoverse-V1
    CPU. This needs to be enabled only for revisions r0p0 and r1p0, it is fixed
    in r1p1.
@@ -468,6 +476,10 @@
    Cortex-A710 CPU. This needs to be enabled for revisions r0p0, r1p0 and r2p0
    of the CPU and is fixed in r2p1.
 
+-  ``ERRATA_A710_2147715``: This applies errata 2147715 workaround to
+   Cortex-A710 CPU. This needs to be enabled for revision r2p0 of the CPU
+   and is fixed in r2p1.
+
 -  ``ERRATA_A710_2282622``: This applies errata 2282622 workaround to
    Cortex-A710 CPU. This needs to be enabled for revisions r0p0, r1p0 and r2p0
    of the CPU and is fixed in r2p1.
@@ -512,6 +524,10 @@
 -  ``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.
 
+-  ``ERRATA_N2_2376738``: This applies errata 2376738 workaround to Neoverse-N2
+   CPU. This needs to be enabled for revision r0p0 of the CPU, it is fixed in
+   r0p1.
+
 -  ``ERRATA_N2_2388450``: This applies errata 2388450 workaround to Neoverse-N2
    CPU. This needs to be enabled for revision r0p0 of the CPU, it is fixed in
    r0p1.
@@ -581,6 +597,14 @@
    Cortex-A510 CPU. This needs to be enabled for revisions r0p0, r0p1, r0p2,
    r0p3 and r1p0, it is fixed in r1p1.
 
+- ``ERRATA_A510_2347730``: This applies errata 2347730 workaround to
+   Cortex-A510 CPU. This needs to be enabled for revisions r0p0, r0p1, r0p2,
+   r0p3, r1p0 and r1p1. It is fixed in r1p2.
+
+-  ``ERRATA_A510_2371937``: This applies errata 2371937 workaround to
+   Cortex-A510 CPU. This needs to applied for revisions r0p0, r0p1, r0p2,
+   r0p3, r1p0, r1p1, and is fixed in r1p2.
+
 DSU Errata Workarounds
 ----------------------
 
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index dc18941..cca76c6 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -313,7 +313,13 @@
 -  ``ENABLE_FEAT_RNG``: Numeric value to enable the ``FEAT_RNG`` extension.
    ``FEAT_RNG`` is an optional feature available on Arm v8.5 onwards. This
    flag can take the values 0 to 2, to align with the ``FEATURE_DETECTION``
-   mechanism. Default is ``0``.
+   mechanism. Default value is ``0``.
+
+-  ``ENABLE_FEAT_RNG_TRAP``: Numeric value to enable the ``FEAT_RNG_TRAP``
+   extension. This feature is only supported in AArch64 state. This flag can
+   take values 0 to 2, to align with the ``FEATURE_DETECTION`` mechanism.
+   Default value is ``0``. ``FEAT_RNG_TRAP`` is an optional feature from
+   Armv8.5 onwards.
 
 -  ``ENABLE_FEAT_SB``: Numeric value to enable the ``FEAT_SB`` (Speculation
    Barrier) extension allowing access to ``sb`` instruction. ``FEAT_SB`` is an
@@ -461,8 +467,11 @@
 
 -  ``EL3_EXCEPTION_HANDLING``: When set to ``1``, enable handling of exceptions
    targeted at EL3. When set ``0`` (default), no exceptions are expected or
-   handled at EL3, and a panic will result. This is supported only for AArch64
-   builds.
+   handled at EL3, and a panic will result. The exception to this rule is when
+   ``SPMD_SPM_AT_SEL2`` is set to ``1``, in which case, only exceptions
+   occuring during normal world execution, are trapped to EL3. Any exception
+   trapped during secure world execution are trapped to the SPMC. This is
+   supported only for AArch64 builds.
 
 -  ``EVENT_LOG_LEVEL``: Chooses the log level to use for Measured Boot when
    ``MEASURED_BOOT`` is enabled. For a list of valid values, see ``LOG_LEVEL``.
diff --git a/docs/plat/index.rst b/docs/plat/index.rst
index 0cef16a..1f497e0 100644
--- a/docs/plat/index.rst
+++ b/docs/plat/index.rst
@@ -21,6 +21,7 @@
    marvell/index
    mt8183
    mt8186
+   mt8188
    mt8192
    mt8195
    nvidia-tegra
diff --git a/docs/plat/marvell/armada/uart-booting.rst b/docs/plat/marvell/armada/uart-booting.rst
new file mode 100644
index 0000000..06601d1
--- /dev/null
+++ b/docs/plat/marvell/armada/uart-booting.rst
@@ -0,0 +1,103 @@
+TF-A UART Booting Instructions for Marvell Platforms
+====================================================
+
+This section describes how to temporary boot the Trusted Firmware-A (TF-A) project over UART
+without flashing it to non-volatile storage for Marvell's platforms.
+
+See :ref:`TF-A Build Instructions for Marvell Platforms` how to build ``mrvl_uart`` and
+``mrvl_flash`` targets used in this section.
+
+Armada37x0 UART image downloading
+---------------------------------
+
+There are two options how to download UART image into any Armada37x0 board.
+
+Marvell Wtpdownloader
+~~~~~~~~~~~~~~~~~~~~~
+
+Marvell Wtpdownloader works only with UART images stored in separate files and supports only upload
+speed with 115200 bauds. Target ``mrvl_uart`` produces GZIPed TAR archive ``uart-images.tgz.bin``
+with either three files ``TIM_ATF.bin``, ``wtmi_h.bin`` and ``boot-image_h.bin`` for non-secure
+boot or with four files ``TIM_ATF_TRUSTED.bin``, ``TIMN_ATF_TRUSTED.bin``, ``wtmi_h.bin`` and
+``boot-image_h.bin`` when secure boot is enabled.
+
+Compilation:
+
+.. code:: shell
+
+    > git clone https://github.com/MarvellEmbeddedProcessors/A3700-utils-marvell.git
+    > make -C A3700-utils-marvell/wtptp/src/Wtpdownloader_Linux -f makefile.mk
+
+It produces executable binary ``A3700-utils-marvell/wtptp/src/Wtpdownloader_Linux/WtpDownload_linux``
+
+To download images from ``uart-images.tgz.bin`` archive unpack it and for non-secure boot variant run:
+
+.. code:: shell
+
+    > stty -F /dev/ttyUSB<port#> clocal
+    > WtpDownload_linux -P UART -C <port#> -E -B TIM_ATF.bin -I wtmi_h.bin -I boot-image_h.bin
+
+After that immediately start terminal on ``/dev/ttyUSB<port#>`` to see boot output.
+
+CZ.NIC mox-imager
+~~~~~~~~~~~~~~~~~
+
+CZ.NIC mox-imager supports all Armada37x0 boards (not only Turris MOX as name suggests). It works
+with either with separate files from ``uart-images.tgz.bin`` archive (like Marvell Wtpdownloader)
+produced by ``mrvl_uart`` target or also with ``flash-image.bin`` file produced by ``mrvl_flash``
+target, which is the exactly same file as used for flashing. So when using CZ.NIC mox-imager there
+is no need to build separate files for UART flashing like in case with Marvell Wtpdownloader.
+
+CZ.NIC mox-imager moreover supports higher upload speeds up to the 6000000 bauds (which seems to
+be limit of Armada37x0 SoC) which is much higher and faster than Marvell Wtpdownloader.
+
+Compilation:
+
+.. code:: shell
+
+    > git clone https://gitlab.nic.cz/turris/mox-imager.git
+    > make -C mox-imager
+
+It produces executable binary ``mox-imager/mox-imager``
+
+To download single file image built by ``mrvl_flash`` target at the highest speed, run:
+
+.. code:: shell
+
+    > mox-imager -D /dev/ttyUSB<port#> -E -b 6000000 -t flash-image.bin
+
+To download images from ``uart-images.tgz.bin`` archive built by ``mrvl_uart`` target for
+non-secure boot variant (like Wtpdownloader) but at the highest speed, first unpack
+``uart-images.tgz.bin`` archive and then run:
+
+.. code:: shell
+
+    > mox-imager -D /dev/ttyUSB<port#> -E -b 6000000 -t TIM_ATF.bin wtmi_h.bin boot-image_h.bin
+
+CZ.NIC mox-imager after successful download will start its own mini terminal (option ``-t``) to
+not loose any boot output. It also prints boot output which is sent either by image files or by
+bootrom during transferring of image files. This mini terminal can be quit by CTRL-\\ + C keypress.
+
+
+A7K/8K/CN913x UART image downloading
+------------------------------------
+
+A7K/8K/CN913x uses same image ``flash-image.bin`` for both flashing and booting over UART.
+For downloading image over UART it is possible to use mvebu64boot tool.
+
+Compilation:
+
+.. code:: shell
+
+    > git clone https://github.com/pali/mvebu64boot.git
+    > make -C mvebu64boot
+
+It produces executable binary ``mvebu64boot/mvebu64boot``
+
+To download ``flash-image.bin`` image run:
+
+.. code:: shell
+
+    > mvebu64boot -t -b flash-image.bin /dev/ttyUSB0
+
+After successful download it will start own mini terminal (option ``-t``) like CZ.NIC mox-imager.
diff --git a/docs/plat/marvell/index.rst b/docs/plat/marvell/index.rst
index 0d33432..2d5cdeb 100644
--- a/docs/plat/marvell/index.rst
+++ b/docs/plat/marvell/index.rst
@@ -6,6 +6,7 @@
    :caption: Contents
 
    armada/build
+   armada/uart-booting
    armada/porting
    armada/misc/mvebu-a8k-addr-map
    armada/misc/mvebu-amb
diff --git a/docs/plat/mt8188.rst b/docs/plat/mt8188.rst
new file mode 100644
index 0000000..93abaa5
--- /dev/null
+++ b/docs/plat/mt8188.rst
@@ -0,0 +1,21 @@
+MediaTek 8188
+=============
+
+MediaTek 8188 (MT8188) is a 64-bit ARM SoC introduced by MediaTek in 2022.
+The chip incorporates eight cores - six Cortex-A55 little cores and two Cortex-A78.
+Cortex-A78 can operate at up to 2.6 GHz.
+Cortex-A55 can operate at up to 2.0 GHz.
+
+Boot Sequence
+-------------
+
+::
+
+    Boot Rom --> Coreboot --> TF-A BL31 --> Depthcharge --> Linux Kernel
+
+    How to Build
+    ------------
+
+    .. code:: shell
+
+           make CROSS_COMPILE=aarch64-linux-gnu- LD=aarch64-linux-gnu-gcc PLAT=mt8188 DEBUG=1 COREBOOT=1
diff --git a/drivers/mmc/mmc.c b/drivers/mmc/mmc.c
index 116afda..1b6e96b 100644
--- a/drivers/mmc/mmc.c
+++ b/drivers/mmc/mmc.c
@@ -25,6 +25,7 @@
 static const struct mmc_ops *ops;
 static unsigned int mmc_ocr_value;
 static struct mmc_csd_emmc mmc_csd;
+static struct sd_switch_status sd_switch_func_status;
 static unsigned char mmc_ext_csd[512] __aligned(16);
 static unsigned int mmc_flags;
 static struct mmc_device_info *mmc_dev_info;
@@ -44,6 +45,11 @@
 	return ((mmc_flags & MMC_FLAG_CMD23) != 0U);
 }
 
+static bool is_sd_cmd6_enabled(void)
+{
+	return ((mmc_flags & MMC_FLAG_SD_CMD6) != 0U);
+}
+
 static int mmc_send_cmd(unsigned int idx, unsigned int arg,
 			unsigned int r_type, unsigned int *r_data)
 {
@@ -357,6 +363,33 @@
 	return 0;
 }
 
+static int sd_switch(unsigned int mode, unsigned char group,
+		     unsigned char func)
+{
+	unsigned int group_shift = (group - 1U) * 4U;
+	unsigned int group_mask = GENMASK(group_shift + 3U,  group_shift);
+	unsigned int arg;
+	int ret;
+
+	ret = ops->prepare(0, (uintptr_t)&sd_switch_func_status,
+			   sizeof(sd_switch_func_status));
+	if (ret != 0) {
+		return ret;
+	}
+
+	/* MMC CMD6: SWITCH_FUNC */
+	arg = mode | SD_SWITCH_ALL_GROUPS_MASK;
+	arg &= ~group_mask;
+	arg |= func << group_shift;
+	ret = mmc_send_cmd(MMC_CMD(6), arg, MMC_RESPONSE_R1, NULL);
+	if (ret != 0) {
+		return ret;
+	}
+
+	return ops->read(0, (uintptr_t)&sd_switch_func_status,
+			 sizeof(sd_switch_func_status));
+}
+
 static int sd_send_op_cond(void)
 {
 	int n;
@@ -524,7 +557,39 @@
 		return ret;
 	}
 
-	return mmc_fill_device_info();
+	ret = mmc_fill_device_info();
+	if (ret != 0) {
+		return ret;
+	}
+
+	if (is_sd_cmd6_enabled() &&
+	    (mmc_dev_info->mmc_dev_type == MMC_IS_SD_HC)) {
+		/* Try to switch to High Speed Mode */
+		ret = sd_switch(SD_SWITCH_FUNC_CHECK, 1U, 1U);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((sd_switch_func_status.support_g1 & BIT(9)) == 0U) {
+			/* High speed not supported, keep default speed */
+			return 0;
+		}
+
+		ret = sd_switch(SD_SWITCH_FUNC_SWITCH, 1U, 1U);
+		if (ret != 0) {
+			return ret;
+		}
+
+		if ((sd_switch_func_status.sel_g2_g1 & 0x1U) == 0U) {
+			/* Cannot switch to high speed, keep default speed */
+			return 0;
+		}
+
+		mmc_dev_info->max_bus_freq = 50000000U;
+		ret = ops->set_ios(clk, bus_width);
+	}
+
+	return ret;
 }
 
 size_t mmc_read_blocks(int lba, uintptr_t buf, size_t size)
diff --git a/drivers/mtd/nand/core.c b/drivers/mtd/nand/core.c
index 9f0331a..6ef2256 100644
--- a/drivers/mtd/nand/core.c
+++ b/drivers/mtd/nand/core.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2019-2022, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -8,18 +8,29 @@
 #include <errno.h>
 #include <stddef.h>
 
-#include <platform_def.h>
-
 #include <common/debug.h>
 #include <drivers/delay_timer.h>
 #include <drivers/nand.h>
 #include <lib/utils.h>
 
+#include <platform_def.h>
+
 /*
  * Define a single nand_device used by specific NAND frameworks.
  */
 static struct nand_device nand_dev;
-static uint8_t scratch_buff[PLATFORM_MTD_MAX_PAGE_SIZE];
+
+#pragma weak plat_get_scratch_buffer
+void plat_get_scratch_buffer(void **buffer_addr, size_t *buf_size)
+{
+	static uint8_t scratch_buff[PLATFORM_MTD_MAX_PAGE_SIZE];
+
+	assert(buffer_addr != NULL);
+	assert(buf_size != NULL);
+
+	*buffer_addr = (void *)scratch_buff;
+	*buf_size = sizeof(scratch_buff);
+}
 
 int nand_read(unsigned int offset, uintptr_t buffer, size_t length,
 	      size_t *length_read)
@@ -34,6 +45,12 @@
 	unsigned int bytes_read;
 	int is_bad;
 	int ret;
+	uint8_t *scratch_buff;
+	size_t scratch_buff_size;
+
+	plat_get_scratch_buffer((void **)&scratch_buff, &scratch_buff_size);
+
+	assert(scratch_buff != NULL);
 
 	VERBOSE("Block %u - %u, page_start %u, nb %u, length %zu, offset %u\n",
 		block, end_block, page_start, nb_pages, length, offset);
@@ -41,7 +58,7 @@
 	*length_read = 0UL;
 
 	if (((start_offset != 0U) || (length % nand_dev.page_size) != 0U) &&
-	    (sizeof(scratch_buff) < nand_dev.page_size)) {
+	    (scratch_buff_size < nand_dev.page_size)) {
 		return -EINVAL;
 	}
 
diff --git a/drivers/st/clk/clk-stm32-core.c b/drivers/st/clk/clk-stm32-core.c
index 8584a52..bb03125 100644
--- a/drivers/st/clk/clk-stm32-core.c
+++ b/drivers/st/clk/clk-stm32-core.c
@@ -215,24 +215,6 @@
 	return 0;
 }
 
-const char *_clk_stm32_get_name(struct stm32_clk_priv *priv, int id)
-{
-	return priv->clks[id].name;
-}
-
-const char *clk_stm32_get_name(struct stm32_clk_priv *priv,
-			       unsigned long binding_id)
-{
-	int id;
-
-	id = clk_get_index(priv, binding_id);
-	if (id == -EINVAL) {
-		return NULL;
-	}
-
-	return _clk_stm32_get_name(priv, id);
-}
-
 const struct clk_stm32 *_clk_get(struct stm32_clk_priv *priv, int id)
 {
 	if ((unsigned int)id < priv->num) {
diff --git a/drivers/st/clk/clk-stm32-core.h b/drivers/st/clk/clk-stm32-core.h
index 809d05f..8bfb513 100644
--- a/drivers/st/clk/clk-stm32-core.h
+++ b/drivers/st/clk/clk-stm32-core.h
@@ -54,7 +54,6 @@
 };
 
 struct clk_stm32 {
-	const char *name;
 	uint16_t binding;
 	uint16_t parent;
 	uint8_t flags;
@@ -163,8 +162,6 @@
 int clk_oscillator_wait_ready_on(struct stm32_clk_priv *priv, int id);
 int clk_oscillator_wait_ready_off(struct stm32_clk_priv *priv, int id);
 
-const char *_clk_stm32_get_name(struct stm32_clk_priv *priv, int id);
-const char *clk_stm32_get_name(struct stm32_clk_priv *priv, unsigned long binding_id);
 int clk_stm32_get_counter(unsigned long binding_id);
 
 void _clk_stm32_gate_disable(struct stm32_clk_priv *priv, uint16_t gate_id);
@@ -226,7 +223,6 @@
 
 #define STM32_DIV(idx, _binding, _parent, _flags, _div_id) \
 	[(idx)] = (struct clk_stm32){ \
-		.name		= #idx,\
 		.binding	= (_binding),\
 		.parent		=  (_parent),\
 		.flags		= (_flags),\
@@ -242,7 +238,6 @@
 
 #define STM32_GATE(idx, _binding, _parent, _flags, _gate_id) \
 	[(idx)] = (struct clk_stm32){ \
-		.name		= #idx,\
 		.binding	= (_binding),\
 		.parent		=  (_parent),\
 		.flags		= (_flags),\
@@ -262,7 +257,6 @@
 
 #define FIXED_FACTOR(idx, _idx, _parent, _mult, _div) \
 	[(idx)] = (struct clk_stm32){ \
-		.name		= #idx,\
 		.binding	= (_idx),\
 		.parent		= (_parent),\
 		.clock_cfg	= &(struct fixed_factor_cfg){\
@@ -274,7 +268,6 @@
 
 #define GATE(idx, _binding, _parent, _flags, _offset, _bit_idx) \
 	[(idx)] = (struct clk_stm32){ \
-		.name		= #idx,\
 		.binding	= (_binding),\
 		.parent		=  (_parent),\
 		.flags		= (_flags),\
@@ -287,7 +280,6 @@
 
 #define STM32_MUX(idx, _binding, _mux_id, _flags) \
 	[(idx)] = (struct clk_stm32){ \
-		.name		= #idx,\
 		.binding	= (_binding),\
 		.parent		= (MUX(_mux_id)),\
 		.flags		= (_flags),\
@@ -302,7 +294,6 @@
 
 #define CK_TIMER(idx, _idx, _parent, _flags, _apbdiv, _timpre) \
 	[(idx)] = (struct clk_stm32){ \
-		.name		= #idx,\
 		.binding	= (_idx),\
 		.parent		= (_parent),\
 		.flags		= (CLK_SET_RATE_PARENT | (_flags)),\
@@ -319,7 +310,6 @@
 
 #define CLK_FIXED_RATE(idx, _binding, _rate) \
 	[(idx)] = (struct clk_stm32){ \
-		.name		= #idx,\
 		.binding	= (_binding),\
 		.parent		= (CLK_IS_ROOT),\
 		.clock_cfg	= &(struct clk_stm32_fixed_rate_cfg){\
@@ -370,7 +360,6 @@
 
 #define CLK_OSC(idx, _idx, _parent, _osc_id) \
 	[(idx)] = (struct clk_stm32){ \
-		.name		= #idx,\
 		.binding	= (_idx),\
 		.parent		= (_parent),\
 		.flags		= CLK_IS_CRITICAL,\
@@ -382,7 +371,6 @@
 
 #define CLK_OSC_FIXED(idx, _idx, _parent, _osc_id) \
 	[(idx)] = (struct clk_stm32){ \
-		.name		= #idx,\
 		.binding	= (_idx),\
 		.parent		= (_parent),\
 		.flags		= CLK_IS_CRITICAL,\
diff --git a/drivers/st/clk/clk-stm32mp13.c b/drivers/st/clk/clk-stm32mp13.c
index d360767..c960928 100644
--- a/drivers/st/clk/clk-stm32mp13.c
+++ b/drivers/st/clk/clk-stm32mp13.c
@@ -1705,7 +1705,6 @@
 };
 
 #define CLK_PLL(idx, _idx, _parent, _gate, _pll_id, _flags)[idx] = {\
-	.name = #idx,\
 	.binding = _idx,\
 	.parent = _parent,\
 	.flags = (_flags),\
@@ -1762,7 +1761,6 @@
 
 #define STM32_COMPOSITE(idx, _binding, _parent, _flags, _gate_id,\
 			_div_id)[idx] = {\
-	.name = #idx,\
 	.binding = (_binding),\
 	.parent =  (_parent),\
 	.flags = (_flags),\
diff --git a/drivers/st/clk/stm32mp1_clk.c b/drivers/st/clk/stm32mp1_clk.c
index 534ee3b..aa5db6f 100644
--- a/drivers/st/clk/stm32mp1_clk.c
+++ b/drivers/st/clk/stm32mp1_clk.c
@@ -232,7 +232,6 @@
 struct stm32mp1_pll {
 	uint8_t refclk_min;
 	uint8_t refclk_max;
-	uint8_t divn_max;
 };
 
 struct stm32mp1_clk_gate {
@@ -543,12 +542,10 @@
 	[PLL_800] = {
 		.refclk_min = 4,
 		.refclk_max = 16,
-		.divn_max = 99,
 	},
 	[PLL_1600] = {
 		.refclk_min = 8,
 		.refclk_max = 16,
-		.divn_max = 199,
 	},
 };
 
diff --git a/drivers/st/etzpc/etzpc.c b/drivers/st/etzpc/etzpc.c
index ff52a22..4c3c26d 100644
--- a/drivers/st/etzpc/etzpc.c
+++ b/drivers/st/etzpc/etzpc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2017-2022, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -225,20 +225,8 @@
 int etzpc_init(void)
 {
 	uint32_t hwcfg;
-	int node;
-	struct dt_node_info etzpc_info;
-
-	node = dt_get_node(&etzpc_info, -1, ETZPC_COMPAT);
-	if (node < 0) {
-		return -EIO;
-	}
-
-	/* Check ETZPC is secure only */
-	if (etzpc_info.status != DT_SECURE) {
-		return -EACCES;
-	}
 
-	etzpc_dev.base = etzpc_info.base;
+	etzpc_dev.base = STM32MP1_ETZPC_BASE;
 
 	hwcfg = mmio_read_32(etzpc_dev.base + ETZPC_HWCFGR);
 
diff --git a/drivers/st/mmc/stm32_sdmmc2.c b/drivers/st/mmc/stm32_sdmmc2.c
index 40641b5..6bdd782 100644
--- a/drivers/st/mmc/stm32_sdmmc2.c
+++ b/drivers/st/mmc/stm32_sdmmc2.c
@@ -129,6 +129,8 @@
 #define DT_SDMMC2_COMPAT		"st,stm32-sdmmc2"
 #endif
 
+#define SDMMC_FIFO_SIZE			64U
+
 static void stm32_sdmmc2_init(void);
 static int stm32_sdmmc2_send_cmd_req(struct mmc_cmd *cmd);
 static int stm32_sdmmc2_send_cmd(struct mmc_cmd *cmd);
@@ -148,6 +150,8 @@
 
 static struct stm32_sdmmc2_params sdmmc2_params;
 
+static bool next_cmd_is_acmd;
+
 #pragma weak plat_sdmmc2_use_dma
 bool plat_sdmmc2_use_dma(unsigned int instance, unsigned int memory)
 {
@@ -257,6 +261,20 @@
 	case MMC_CMD(1):
 		arg_reg |= OCR_POWERUP;
 		break;
+	case MMC_CMD(6):
+		if ((sdmmc2_params.device_info->mmc_dev_type == MMC_IS_SD_HC) &&
+		    (!next_cmd_is_acmd)) {
+			cmd_reg |= SDMMC_CMDR_CMDTRANS;
+			if (sdmmc2_params.use_dma) {
+				flags_data |= SDMMC_STAR_DCRCFAIL |
+					SDMMC_STAR_DTIMEOUT |
+					SDMMC_STAR_DATAEND |
+					SDMMC_STAR_RXOVERR |
+					SDMMC_STAR_IDMATE |
+					SDMMC_STAR_DBCKEND;
+			}
+		}
+		break;
 	case MMC_CMD(8):
 		if (sdmmc2_params.device_info->mmc_dev_type == MMC_IS_EMMC) {
 			cmd_reg |= SDMMC_CMDR_CMDTRANS;
@@ -294,6 +312,8 @@
 		break;
 	}
 
+	next_cmd_is_acmd = (cmd->cmd_idx == MMC_CMD(55));
+
 	mmio_write_32(base + SDMMC_ICR, SDMMC_STATIC_FLAGS);
 
 	/*
@@ -301,8 +321,7 @@
 	 * Skip CMD55 as the next command could be data related, and
 	 * the register could have been set in prepare function.
 	 */
-	if (((cmd_reg & SDMMC_CMDR_CMDTRANS) == 0U) &&
-	    (cmd->cmd_idx != MMC_CMD(55))) {
+	if (((cmd_reg & SDMMC_CMDR_CMDTRANS) == 0U) && !next_cmd_is_acmd) {
 		mmio_write_32(base + SDMMC_DCTRLR, 0U);
 	}
 
@@ -627,7 +646,7 @@
 			return -ETIMEDOUT;
 		}
 
-		if (size < (8U * sizeof(uint32_t))) {
+		if (size < (SDMMC_FIFO_SIZE / 2U)) {
 			if ((mmio_read_32(base + SDMMC_DCNTR) > 0U) &&
 			    ((status & SDMMC_STAR_RXFIFOE) == 0U)) {
 				*buffer = mmio_read_32(fifo_reg);
@@ -637,7 +656,8 @@
 			uint32_t count;
 
 			/* Read data from SDMMC Rx FIFO */
-			for (count = 0; count < 8U; count++) {
+			for (count = 0; count < (SDMMC_FIFO_SIZE / 2U);
+			     count += sizeof(uint32_t)) {
 				*buffer = mmio_read_32(fifo_reg);
 				buffer++;
 			}
@@ -737,8 +757,6 @@
 
 int stm32_sdmmc2_mmc_init(struct stm32_sdmmc2_params *params)
 {
-	int rc;
-
 	assert((params != NULL) &&
 	       ((params->reg_base & MMC_BLOCK_MASK) == 0U) &&
 	       ((params->bus_width == MMC_BUS_WIDTH_1) ||
@@ -756,16 +774,20 @@
 
 	clk_enable(sdmmc2_params.clock_id);
 
-	rc = stm32mp_reset_assert(sdmmc2_params.reset_id, TIMEOUT_US_1_MS);
-	if (rc != 0) {
-		panic();
-	}
-	udelay(2);
-	rc = stm32mp_reset_deassert(sdmmc2_params.reset_id, TIMEOUT_US_1_MS);
-	if (rc != 0) {
-		panic();
+	if ((int)sdmmc2_params.reset_id >= 0) {
+		int rc;
+
+		rc = stm32mp_reset_assert(sdmmc2_params.reset_id, TIMEOUT_US_1_MS);
+		if (rc != 0) {
+			panic();
+		}
+		udelay(2);
+		rc = stm32mp_reset_deassert(sdmmc2_params.reset_id, TIMEOUT_US_1_MS);
+		if (rc != 0) {
+			panic();
+		}
+		mdelay(1);
 	}
-	mdelay(1);
 
 	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;
diff --git a/drivers/ufs/ufs.c b/drivers/ufs/ufs.c
index 2c9bab7..754d173 100644
--- a/drivers/ufs/ufs.c
+++ b/drivers/ufs/ufs.c
@@ -264,7 +264,7 @@
 
 	/* clear utrd */
 	memset((void *)utrd, 0, sizeof(utp_utrd_t));
-	base = ufs_params.desc_base + (slot * UFS_DESC_SIZE);
+	base = ufs_params.desc_base + (slot * sizeof(utrd_header_t));
 	/* clear the descriptor */
 	memset((void *)base, 0, UFS_DESC_SIZE);
 
@@ -299,12 +299,6 @@
 	unsigned int lba_cnt;
 	int prdt_size;
 
-
-	mmio_write_32(ufs_params.reg_base + UTRLBA,
-		      utrd->header & UINT32_MAX);
-	mmio_write_32(ufs_params.reg_base + UTRLBAU,
-		      (utrd->header >> 32) & UINT32_MAX);
-
 	hd = (utrd_header_t *)utrd->header;
 	upiu = (cmd_upiu_t *)utrd->upiu;
 
@@ -402,12 +396,6 @@
 	hd = (utrd_header_t *)utrd->header;
 	query_upiu = (query_upiu_t *)utrd->upiu;
 
-	mmio_write_32(ufs_params.reg_base + UTRLBA,
-		      utrd->header & UINT32_MAX);
-	mmio_write_32(ufs_params.reg_base + UTRLBAU,
-		      (utrd->header >> 32) & UINT32_MAX);
-
-
 	hd->i = 1;
 	hd->ct = CT_UFS_STORAGE;
 	hd->ocs = OCS_MASK;
@@ -454,11 +442,6 @@
 	utrd_header_t *hd;
 	nop_out_upiu_t *nop_out;
 
-	mmio_write_32(ufs_params.reg_base + UTRLBA,
-		      utrd->header & UINT32_MAX);
-	mmio_write_32(ufs_params.reg_base + UTRLBAU,
-		      (utrd->header >> 32) & UINT32_MAX);
-
 	hd = (utrd_header_t *)utrd->header;
 	nop_out = (nop_out_upiu_t *)utrd->upiu;
 
@@ -795,6 +778,11 @@
 	unsigned int blk_num, blk_size;
 	int i, result;
 
+	mmio_write_32(ufs_params.reg_base + UTRLBA,
+		      ufs_params.desc_base & UINT32_MAX);
+	mmio_write_32(ufs_params.reg_base + UTRLBAU,
+		      (ufs_params.desc_base >> 32) & UINT32_MAX);
+
 	ufs_verify_init();
 	ufs_verify_ready();
 
@@ -856,6 +844,11 @@
 
 
 	if (ufs_params.flags & UFS_FLAGS_SKIPINIT) {
+		mmio_write_32(ufs_params.reg_base + UTRLBA,
+			      ufs_params.desc_base & UINT32_MAX);
+		mmio_write_32(ufs_params.reg_base + UTRLBAU,
+			      (ufs_params.desc_base >> 32) & UINT32_MAX);
+
 		result = ufshc_dme_get(0x1571, 0, &data);
 		assert(result == 0);
 		result = ufshc_dme_get(0x41, 0, &data);
diff --git a/fdts/stm32mp13-bl2.dtsi b/fdts/stm32mp13-bl2.dtsi
index 00bf1b5..4e3701c 100644
--- a/fdts/stm32mp13-bl2.dtsi
+++ b/fdts/stm32mp13-bl2.dtsi
@@ -9,70 +9,12 @@
 		/delete-property/ mmc0;
 		/delete-property/ mmc1;
 #endif
-		/delete-property/ ethernet0;
-		/delete-property/ ethernet1;
 	};
 
-	cpus {
-		cpu@0 {
-			/delete-property/ operating-points-v2;
-		};
-	};
-
-	/delete-node/ cpu0-opp-table;
-	/delete-node/ psci;
-
 	soc {
-		/delete-node/ sram@30000000;
-		/delete-node/ timer@40000000;
-		/delete-node/ timer@40001000;
-		/delete-node/ timer@40002000;
-		/delete-node/ timer@40003000;
-		/delete-node/ timer@40004000;
-		/delete-node/ timer@40005000;
-		/delete-node/ timer@40009000;
-		/delete-node/ spi@4000b000;
-		/delete-node/ audio-controller@4000b000;
-		/delete-node/ spi@4000c000;
-		/delete-node/ audio-controller@4000c000;
-		/delete-node/ audio-controller@4000d000;
-		/delete-node/ i2c@40012000;
-		/delete-node/ i2c@40013000;
-		/delete-node/ timer@44000000;
-		/delete-node/ timer@44001000;
-		/delete-node/ spi@44004000;
-		/delete-node/ audio-controller@44004000;
-		/delete-node/ sai@4400a000;
-		/delete-node/ sai@4400b000;
-		/delete-node/ dfsdm@4400d000;
-		/delete-node/ can@4400e000;
-		/delete-node/ can@4400f000;
-		/delete-node/ dma-controller@48000000;
-		/delete-node/ dma-controller@48001000;
-		/delete-node/ dma-router@48002000;
-		/delete-node/ adc@48003000;
-		/delete-node/ adc@48004000;
-		/delete-node/ dma@48005000;
-		/delete-node/ dma-router@48006000;
 #if !STM32MP_USB_PROGRAMMER
 		/delete-node/ usb-otg@49000000;
 #endif
-		/delete-node/ spi@4c002000;
-		/delete-node/ spi@4c003000;
-		/delete-node/ timer@4c007000;
-		/delete-node/ timer@4c008000;
-		/delete-node/ timer@4c009000;
-		/delete-node/ timer@4c00a000;
-		/delete-node/ timer@4c00b000;
-		/delete-node/ timer@4c00c000;
-		/delete-node/ timer@50021000;
-		/delete-node/ timer@50022000;
-		/delete-node/ timer@50023000;
-		/delete-node/ timer@50024000;
-		/delete-node/ vrefbuf@50025000;
-		/delete-node/ thermal@50028000;
-		/delete-node/ hdp@5002a000;
-		/delete-node/ dma-controller@58000000;
 #if !STM32MP_RAW_NAND
 		/delete-node/ memory-controller@58002000;
 #endif
@@ -83,23 +25,13 @@
 		/delete-node/ mmc@58005000;
 		/delete-node/ mmc@58007000;
 #endif
-		/delete-node/ crc@58009000;
-		/delete-node/ stmmac-axi-config;
-		/delete-node/ eth1@5800a000;
 #if !STM32MP_USB_PROGRAMMER
 		/delete-node/ usbh-ohci@5800c000;
 		/delete-node/ usbh-ehci@5800d000;
 #endif
-		/delete-node/ eth2@5800e000;
-		/delete-node/ dcmipp@5a000000;
-		/delete-node/ display-controller@5a001000;
 #if !STM32MP_USB_PROGRAMMER
 		/delete-node/ usbphyc@5a006000;
 #endif
-		/delete-node/ perf@5a007000;
-		/delete-node/ rtc@5c004000;
-		/delete-node/ tamp@5c00a000;
-		/delete-node/ stgen@5c008000;
 
 		pinctrl@50002000 {
 #if !STM32MP_EMMC && !STM32MP_SDMMC
diff --git a/fdts/stm32mp13-fw-config.dtsi b/fdts/stm32mp13-fw-config.dtsi
index 28f7086..4f3bb72 100644
--- a/fdts/stm32mp13-fw-config.dtsi
+++ b/fdts/stm32mp13-fw-config.dtsi
@@ -13,7 +13,7 @@
 #endif
 
 #define DDR_NS_BASE	STM32MP_DDR_BASE
-#define DDR_SEC_SIZE	0x02000000
+#define DDR_SEC_SIZE	STM32MP_DDR_S_SIZE
 #define DDR_SEC_BASE	(STM32MP_DDR_BASE + (DDR_SIZE - DDR_SEC_SIZE))
 #define DDR_NS_SIZE	(DDR_SEC_BASE - DDR_NS_BASE)
 
diff --git a/fdts/stm32mp13-pinctrl.dtsi b/fdts/stm32mp13-pinctrl.dtsi
index 0ad06a4..879da9c 100644
--- a/fdts/stm32mp13-pinctrl.dtsi
+++ b/fdts/stm32mp13-pinctrl.dtsi
@@ -17,7 +17,7 @@
 	};
 
 	sdmmc1_b4_pins_a: sdmmc1-b4-0 {
-		pins1 {
+		pins {
 			pinmux = <STM32_PINMUX('C', 8, AF12)>, /* SDMMC1_D0 */
 				 <STM32_PINMUX('C', 9, AF12)>, /* SDMMC1_D1 */
 				 <STM32_PINMUX('C', 10, AF12)>, /* SDMMC1_D2 */
@@ -27,16 +27,19 @@
 			drive-push-pull;
 			bias-disable;
 		};
-		pins2 {
+	};
+
+	sdmmc1_clk_pins_a: sdmmc1-clk-0 {
+		pins {
 			pinmux = <STM32_PINMUX('C', 12, AF12)>; /* SDMMC1_CK */
-			slew-rate = <2>;
+			slew-rate = <1>;
 			drive-push-pull;
 			bias-disable;
 		};
 	};
 
 	sdmmc2_b4_pins_a: sdmmc2-b4-0 {
-		pins1 {
+		pins {
 			pinmux = <STM32_PINMUX('B', 14, AF10)>, /* SDMMC2_D0 */
 				 <STM32_PINMUX('B', 15, AF10)>, /* SDMMC2_D1 */
 				 <STM32_PINMUX('B', 3, AF10)>, /* SDMMC2_D2 */
@@ -46,9 +49,12 @@
 			drive-push-pull;
 			bias-pull-up;
 		};
-		pins2 {
+	};
+
+	sdmmc2_clk_pins_a: sdmmc2-clk-0 {
+		pins {
 			pinmux = <STM32_PINMUX('E', 3, AF10)>; /* SDMMC2_CK */
-			slew-rate = <2>;
+			slew-rate = <1>;
 			drive-push-pull;
 			bias-pull-up;
 		};
diff --git a/fdts/stm32mp131.dtsi b/fdts/stm32mp131.dtsi
index e4d9d3b..2c62408 100644
--- a/fdts/stm32mp131.dtsi
+++ b/fdts/stm32mp131.dtsi
@@ -259,15 +259,6 @@
 			clocks = <&rcc SYSCFG>;
 		};
 
-		vrefbuf: vrefbuf@50025000 {
-			compatible = "st,stm32-vrefbuf";
-			reg = <0x50025000 0x8>;
-			regulator-min-microvolt = <1500000>;
-			regulator-max-microvolt = <2500000>;
-			clocks = <&rcc VREF>;
-			status = "disabled";
-		};
-
 		hash: hash@54003000 {
 			compatible = "st,stm32mp13-hash";
 			reg = <0x54003000 0x400>;
@@ -333,7 +324,7 @@
 			resets = <&rcc SDMMC1_R>;
 			cap-sd-highspeed;
 			cap-mmc-highspeed;
-			max-frequency = <120000000>;
+			max-frequency = <130000000>;
 			status = "disabled";
 		};
 
@@ -346,16 +337,10 @@
 			resets = <&rcc SDMMC2_R>;
 			cap-sd-highspeed;
 			cap-mmc-highspeed;
-			max-frequency = <120000000>;
+			max-frequency = <130000000>;
 			status = "disabled";
 		};
 
-		crc1: crc@58009000 {
-			compatible = "st,stm32f7-crc";
-			reg = <0x58009000 0x400>;
-			clocks = <&rcc CRC1>;
-		};
-
 		usbh_ohci: usbh-ohci@5800c000 {
 			compatible = "generic-ohci";
 			reg = <0x5800c000 0x1000>;
@@ -471,11 +456,6 @@
 				st,non-secure-otp;
 			};
 		};
-
-		tamp: tamp@5c00a000 {
-			reg = <0x5c00a000 0x400>;
-		};
-
 		/*
 		 * Break node order to solve dependency probe issue between
 		 * pinctrl and exti.
diff --git a/fdts/stm32mp133.dtsi b/fdts/stm32mp133.dtsi
index 8bbcc61..bb468c0 100644
--- a/fdts/stm32mp133.dtsi
+++ b/fdts/stm32mp133.dtsi
@@ -5,17 +5,3 @@
  */
 
 #include "stm32mp131.dtsi"
-
-/ {
-	soc {
-		m_can1: can@4400e000 {
-			reg = <0x4400e000 0x400>, <0x44011000 0x1400>;
-			status = "disabled";
-		};
-
-		m_can2: can@4400f000 {
-			reg = <0x4400f000 0x400>, <0x44011000 0x2800>;
-			status = "disabled";
-		};
-	};
-};
diff --git a/fdts/stm32mp135.dtsi b/fdts/stm32mp135.dtsi
index 415bb9b..b5ebdd9 100644
--- a/fdts/stm32mp135.dtsi
+++ b/fdts/stm32mp135.dtsi
@@ -5,8 +5,3 @@
  */
 
 #include "stm32mp133.dtsi"
-
-/ {
-	soc {
-	};
-};
diff --git a/fdts/stm32mp135f-dk.dts b/fdts/stm32mp135f-dk.dts
index 6240381..e58be40 100644
--- a/fdts/stm32mp135f-dk.dts
+++ b/fdts/stm32mp135f-dk.dts
@@ -303,7 +303,7 @@
 
 &sdmmc1 {
 	pinctrl-names = "default";
-	pinctrl-0 = <&sdmmc1_b4_pins_a>;
+	pinctrl-0 = <&sdmmc1_b4_pins_a &sdmmc1_clk_pins_a>;
 	disable-wp;
 	st,neg-edge;
 	bus-width = <4>;
diff --git a/fdts/stm32mp13xc.dtsi b/fdts/stm32mp13xc.dtsi
index c03bd43..4b30c5c 100644
--- a/fdts/stm32mp13xc.dtsi
+++ b/fdts/stm32mp13xc.dtsi
@@ -8,15 +8,6 @@
 
 / {
 	soc {
-		cryp: crypto@54002000 {
-			compatible = "st,stm32mp1-cryp";
-			reg = <0x54002000 0x400>;
-			interrupts = <GIC_SPI 80 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc CRYP1>;
-			resets = <&rcc CRYP1_R>;
-			status = "disabled";
-		};
-
 		saes: saes@54005000 {
 			compatible = "st,stm32-saes";
 			reg = <0x54005000 0x400>;
diff --git a/fdts/stm32mp13xf.dtsi b/fdts/stm32mp13xf.dtsi
index e467d71..887c4e0 100644
--- a/fdts/stm32mp13xf.dtsi
+++ b/fdts/stm32mp13xf.dtsi
@@ -7,15 +7,6 @@
 
 / {
 	soc {
-		cryp: crypto@54002000 {
-			compatible = "st,stm32mp1-cryp";
-			reg = <0x54002000 0x400>;
-			interrupts = <GIC_SPI 80 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&rcc CRYP1>;
-			resets = <&rcc CRYP1_R>;
-			status = "disabled";
-		};
-
 		saes: saes@54005000 {
 			compatible = "st,stm32-saes";
 			reg = <0x54005000 0x400>;
diff --git a/fdts/stm32mp15-pinctrl.dtsi b/fdts/stm32mp15-pinctrl.dtsi
index 1b5fbc6..7d2be0b 100644
--- a/fdts/stm32mp15-pinctrl.dtsi
+++ b/fdts/stm32mp15-pinctrl.dtsi
@@ -120,6 +120,21 @@
 		};
 	};
 
+	sdmmc1_dir_pins_b: sdmmc1-dir-1 {
+		pins1 {
+			pinmux = <STM32_PINMUX('F', 2, AF11)>, /* SDMMC1_D0DIR */
+				 <STM32_PINMUX('E', 14, AF11)>, /* SDMMC1_D123DIR */
+				 <STM32_PINMUX('B', 9, AF11)>; /* SDMMC1_CDIR */
+			slew-rate = <1>;
+			drive-push-pull;
+			bias-pull-up;
+		};
+		pins2{
+			pinmux = <STM32_PINMUX('E', 4, AF8)>; /* SDMMC1_CKIN */
+			bias-pull-up;
+		};
+	};
+
 	sdmmc2_b4_pins_a: sdmmc2-b4-0 {
 		pins1 {
 			pinmux = <STM32_PINMUX('B', 14, AF9)>, /* SDMMC2_D0 */
@@ -182,6 +197,18 @@
 		};
 	};
 
+	sdmmc2_d47_pins_c: sdmmc2-d47-2 {
+		pins {
+			pinmux = <STM32_PINMUX('A', 8, AF9)>, /* SDMMC2_D4 */
+				 <STM32_PINMUX('A', 15, AF9)>, /* SDMMC2_D5 */
+				 <STM32_PINMUX('C', 6, AF10)>, /* SDMMC2_D6 */
+				 <STM32_PINMUX('C', 7, AF10)>; /* SDMMC2_D7 */
+			slew-rate = <1>;
+			drive-push-pull;
+			bias-pull-up;
+		};
+	};
+
 	sdmmc2_d47_pins_d: sdmmc2-d47-3 {
 		pins {
 			pinmux = <STM32_PINMUX('A', 8, AF9)>, /* SDMMC2_D4 */
diff --git a/fdts/stm32mp151.dtsi b/fdts/stm32mp151.dtsi
index 575d61e..bb16fda 100644
--- a/fdts/stm32mp151.dtsi
+++ b/fdts/stm32mp151.dtsi
@@ -497,8 +497,6 @@
 			compatible = "st,stm32-etzpc";
 			reg = <0x5C007000 0x400>;
 			clocks = <&rcc TZPC>;
-			status = "disabled";
-			secure-status = "okay";
 		};
 
 		stgen: stgen@5c008000 {
diff --git a/fdts/stm32mp157a-dhcor-avenger96-fw-config.dts b/fdts/stm32mp157a-dhcor-avenger96-fw-config.dts
new file mode 100644
index 0000000..2abbe50
--- /dev/null
+++ b/fdts/stm32mp157a-dhcor-avenger96-fw-config.dts
@@ -0,0 +1,7 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (c) 2020-2021, STMicroelectronics - All Rights Reserved
+ */
+
+#define DDR_SIZE	0x40000000 /* 1GB */
+#include "stm32mp15-fw-config.dtsi"
diff --git a/fdts/stm32mp157a-dhcor-avenger96.dts b/fdts/stm32mp157a-dhcor-avenger96.dts
new file mode 100644
index 0000000..82d48aa
--- /dev/null
+++ b/fdts/stm32mp157a-dhcor-avenger96.dts
@@ -0,0 +1,24 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
+/*
+ * Copyright (C) Linaro Ltd 2019 - All Rights Reserved
+ * Author: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>
+ * Copyright (C) 2020 Marek Vasut <marex@denx.de>
+ * Copyright (C) 2022 DH electronics GmbH
+ *
+ * DHCOR STM32MP1 variant:
+ * DHCR-STM32MP157A-C065-R102-V18-SPI-C-01LG
+ * DHCOR PCB number: 586-100 or newer
+ * Avenger96 PCB number: 588-200 or newer
+ */
+
+/dts-v1/;
+
+#include "stm32mp157.dtsi"
+#include "stm32mp15xx-dhcor-som.dtsi"
+#include "stm32mp15xx-dhcor-avenger96.dtsi"
+
+/ {
+	model = "Arrow Electronics STM32MP157A Avenger96 board";
+	compatible = "arrow,stm32mp157a-avenger96", "dh,stm32mp157a-dhcor-som",
+		     "st,stm32mp157";
+};
diff --git a/fdts/stm32mp15xx-dhcor-avenger96.dtsi b/fdts/stm32mp15xx-dhcor-avenger96.dtsi
new file mode 100644
index 0000000..576e0f1
--- /dev/null
+++ b/fdts/stm32mp15xx-dhcor-avenger96.dtsi
@@ -0,0 +1,100 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
+/*
+ * Copyright (C) Linaro Ltd 2019 - All Rights Reserved
+ * Author: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>
+ * Copyright (C) 2020 Marek Vasut <marex@denx.de>
+ * Copyright (C) 2022 DH electronics GmbH
+ */
+
+/* Avenger96 uses DHCOR SoM configured for 1V8 IO operation */
+#include "stm32mp15xx-dhcor-io1v8.dtsi"
+
+/ {
+	aliases {
+		serial0 = &uart4;
+		serial1 = &uart7;
+	};
+
+	chosen {
+		stdout-path = "serial0:115200n8";
+	};
+
+	sd_switch: regulator-sd_switch {
+		compatible = "regulator-gpio";
+		regulator-name = "sd_switch";
+		regulator-min-microvolt = <1800000>;
+		regulator-max-microvolt = <2900000>;
+		regulator-type = "voltage";
+		regulator-always-on;
+
+		gpios = <&gpioi 5 0>;
+		gpios-states = <0>;
+		states = <1800000 0x1>,
+			 <2900000 0x0>;
+	};
+};
+
+&sdmmc1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&sdmmc1_b4_pins_a &sdmmc1_dir_pins_b>;
+	disable-wp;
+	st,sig-dir;
+	st,neg-edge;
+	st,use-ckin;
+	bus-width = <4>;
+	vmmc-supply = <&vdd_sd>;
+	vqmmc-supply = <&sd_switch>;
+	status = "okay";
+};
+
+&sdmmc2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&sdmmc2_b4_pins_a &sdmmc2_d47_pins_c>;
+	bus-width = <8>;
+	mmc-ddr-1_8v;
+	no-sd;
+	no-sdio;
+	non-removable;
+	st,neg-edge;
+	vmmc-supply = <&v3v3>;
+	vqmmc-supply = <&vdd_io>;
+	status = "okay";
+};
+
+&uart4 {
+	/* On Low speed expansion header */
+	label = "LS-UART1";
+	pinctrl-names = "default";
+	pinctrl-0 = <&uart4_pins_b>;
+	status = "okay";
+};
+
+&uart7 {
+	/* On Low speed expansion header */
+	label = "LS-UART0";
+	pinctrl-names = "default";
+	pinctrl-0 = <&uart7_pins_a>;
+	uart-has-rtscts;
+	status = "okay";
+};
+
+&usbotg_hs {
+	pinctrl-0 = <&usbotg_hs_pins_a>;
+	pinctrl-names = "default";
+	phy-names = "usb2-phy";
+	phys = <&usbphyc_port1 0>;
+	status = "okay";
+	vbus-supply = <&vbus_otg>;
+};
+
+&usbphyc {
+	status = "okay";
+};
+
+&usbphyc_port0 {
+	phy-supply = <&vdd_usb>;
+};
+
+&usbphyc_port1 {
+	phy-supply = <&vdd_usb>;
+};
diff --git a/fdts/stm32mp15xx-dhcor-io1v8.dtsi b/fdts/stm32mp15xx-dhcor-io1v8.dtsi
new file mode 100644
index 0000000..9937b28
--- /dev/null
+++ b/fdts/stm32mp15xx-dhcor-io1v8.dtsi
@@ -0,0 +1,28 @@
+// SPDX-License-Identifier: (GPL-2.0 OR BSD-3-Clause)
+/*
+ * Copyright (C) Linaro Ltd 2019 - All Rights Reserved
+ * Author: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>
+ * Copyright (C) 2020 Marek Vasut <marex@denx.de>
+ */
+
+/ {
+	/* Enpirion EP3A8LQI U2 on the DHCOR */
+	vdd_io: regulator-buck-io {
+		compatible = "regulator-fixed";
+		regulator-name = "buck-io";
+		regulator-min-microvolt = <1800000>;
+		regulator-max-microvolt = <1800000>;
+		regulator-always-on;
+		regulator-boot-on;
+		vin-supply = <&vdd>;
+	};
+};
+
+&vdd {
+	regulator-min-microvolt = <2900000>;
+	regulator-max-microvolt = <2900000>;
+};
+
+&pwr_regulators {
+	vdd-supply = <&vdd_io>;
+};
diff --git a/fdts/stm32mp15xx-dhcor-som.dtsi b/fdts/stm32mp15xx-dhcor-som.dtsi
new file mode 100644
index 0000000..0774c1d
--- /dev/null
+++ b/fdts/stm32mp15xx-dhcor-som.dtsi
@@ -0,0 +1,288 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
+/*
+ * Copyright (C) Linaro Ltd 2019 - All Rights Reserved
+ * Author: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>
+ * Copyright (C) 2020 Marek Vasut <marex@denx.de>
+ * Copyright (C) 2022 DH electronics GmbH
+ */
+
+#include "stm32mp15-pinctrl.dtsi"
+#include "stm32mp15xxaa-pinctrl.dtsi"
+#include <dt-bindings/clock/stm32mp1-clksrc.h>
+#include "stm32mp15-ddr3-dhsom-2x4Gb-1066-binG.dtsi"
+
+/ {
+	memory@c0000000 {
+		device_type = "memory";
+		reg = <0xc0000000 0x40000000>;
+	};
+};
+
+&cpu0 {
+	cpu-supply = <&vddcore>;
+};
+
+&cpu1 {
+	cpu-supply = <&vddcore>;
+};
+
+&hash1 {
+	status = "okay";
+};
+
+&i2c4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&i2c4_pins_a>;
+	i2c-scl-rising-time-ns = <185>;
+	i2c-scl-falling-time-ns = <20>;
+	status = "okay";
+
+	pmic: stpmic@33 {
+		compatible = "st,stpmic1";
+		reg = <0x33>;
+		interrupts-extended = <&gpioa 0 IRQ_TYPE_EDGE_FALLING>;
+		interrupt-controller;
+		#interrupt-cells = <2>;
+		status = "okay";
+
+		regulators {
+			compatible = "st,stpmic1-regulators";
+			ldo1-supply = <&v3v3>;
+			ldo2-supply = <&v3v3>;
+			ldo3-supply = <&vdd_ddr>;
+			ldo5-supply = <&v3v3>;
+			ldo6-supply = <&v3v3>;
+			pwr_sw1-supply = <&bst_out>;
+			pwr_sw2-supply = <&bst_out>;
+
+			vddcore: buck1 {
+				regulator-name = "vddcore";
+				regulator-min-microvolt = <1200000>;
+				regulator-max-microvolt = <1350000>;
+				regulator-always-on;
+				regulator-initial-mode = <0>;
+				regulator-over-current-protection;
+			};
+
+			vdd_ddr: buck2 {
+				regulator-name = "vdd_ddr";
+				regulator-min-microvolt = <1350000>;
+				regulator-max-microvolt = <1350000>;
+				regulator-always-on;
+				regulator-initial-mode = <0>;
+				regulator-over-current-protection;
+			};
+
+			vdd: buck3 {
+				regulator-name = "vdd";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+				regulator-initial-mode = <0>;
+				regulator-over-current-protection;
+			};
+
+			v3v3: buck4 {
+				regulator-name = "v3v3";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+				regulator-over-current-protection;
+				regulator-initial-mode = <0>;
+			};
+
+			vdda: ldo1 {
+				regulator-name = "vdda";
+				regulator-min-microvolt = <2900000>;
+				regulator-max-microvolt = <2900000>;
+			};
+
+			v2v8: ldo2 {
+				regulator-name = "v2v8";
+				regulator-min-microvolt = <2800000>;
+				regulator-max-microvolt = <2800000>;
+			};
+
+			vtt_ddr: ldo3 {
+				regulator-name = "vtt_ddr";
+				regulator-always-on;
+				regulator-over-current-protection;
+				st,regulator-sink-source;
+			};
+
+			vdd_usb: ldo4 {
+				regulator-name = "vdd_usb";
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+			};
+
+			vdd_sd: ldo5 {
+				regulator-name = "vdd_sd";
+				regulator-min-microvolt = <2900000>;
+				regulator-max-microvolt = <2900000>;
+				regulator-boot-on;
+			};
+
+			v1v8: ldo6 {
+				regulator-name = "v1v8";
+				regulator-min-microvolt = <1800000>;
+				regulator-max-microvolt = <1800000>;
+				regulator-enable-ramp-delay = <300000>;
+			};
+
+			vref_ddr: vref_ddr {
+				regulator-name = "vref_ddr";
+				regulator-always-on;
+			};
+
+			bst_out: boost {
+				regulator-name = "bst_out";
+			};
+
+			vbus_otg: pwr_sw1 {
+				regulator-name = "vbus_otg";
+				regulator-active-discharge = <1>;
+			};
+
+			vbus_sw: pwr_sw2 {
+				regulator-name = "vbus_sw";
+				regulator-active-discharge = <1>;
+			};
+		};
+	};
+};
+
+&iwdg2 {
+	timeout-sec = <32>;
+	status = "okay";
+	secure-status = "okay";
+};
+
+&pwr_regulators {
+	vdd-supply = <&vdd>;
+	vdd_3v3_usbfs-supply = <&vdd_usb>;
+};
+
+&qspi {
+	pinctrl-names = "default";
+	pinctrl-0 = <&qspi_clk_pins_a &qspi_bk1_pins_a>;
+	reg = <0x58003000 0x1000>, <0x70000000 0x200000>;
+	#address-cells = <1>;
+	#size-cells = <0>;
+	status = "okay";
+
+	flash0: flash@0 {
+		compatible = "jedec,spi-nor";
+		reg = <0>;
+		spi-rx-bus-width = <4>;
+		spi-max-frequency = <50000000>;
+		#address-cells = <1>;
+		#size-cells = <1>;
+	};
+};
+
+&rcc {
+	secure-status = "disabled";
+	st,clksrc = <
+		CLK_MPU_PLL1P
+		CLK_AXI_PLL2P
+		CLK_MCU_PLL3P
+		CLK_PLL12_HSE
+		CLK_PLL3_HSE
+		CLK_PLL4_HSE
+		CLK_RTC_LSE
+		CLK_MCO1_DISABLED
+		CLK_MCO2_DISABLED
+	>;
+
+	st,clkdiv = <
+		1 /*MPU*/
+		0 /*AXI*/
+		0 /*MCU*/
+		1 /*APB1*/
+		1 /*APB2*/
+		1 /*APB3*/
+		1 /*APB4*/
+		2 /*APB5*/
+		23 /*RTC*/
+		0 /*MCO1*/
+		0 /*MCO2*/
+	>;
+
+	st,pkcs = <
+		CLK_CKPER_HSE
+		CLK_FMC_ACLK
+		CLK_QSPI_ACLK
+		CLK_ETH_DISABLED
+		CLK_SDMMC12_PLL4P
+		CLK_DSI_DSIPLL
+		CLK_STGEN_HSE
+		CLK_USBPHY_HSE
+		CLK_SPI2S1_PLL3Q
+		CLK_SPI2S23_PLL3Q
+		CLK_SPI45_HSI
+		CLK_SPI6_HSI
+		CLK_I2C46_HSI
+		CLK_SDMMC3_PLL4P
+		CLK_USBO_USBPHY
+		CLK_ADC_CKPER
+		CLK_CEC_LSE
+		CLK_I2C12_HSI
+		CLK_I2C35_HSI
+		CLK_UART1_HSI
+		CLK_UART24_HSI
+		CLK_UART35_HSI
+		CLK_UART6_HSI
+		CLK_UART78_HSI
+		CLK_SPDIF_PLL4P
+		CLK_FDCAN_PLL4R
+		CLK_SAI1_PLL3Q
+		CLK_SAI2_PLL3Q
+		CLK_SAI3_PLL3Q
+		CLK_SAI4_PLL3Q
+		CLK_RNG1_LSI
+		CLK_RNG2_LSI
+		CLK_LPTIM1_PCLK1
+		CLK_LPTIM23_PCLK3
+		CLK_LPTIM45_LSE
+	>;
+
+	/* VCO = 1300.0 MHz => P = 650 (CPU) */
+	pll1: st,pll@0 {
+		compatible = "st,stm32mp1-pll";
+		reg = <0>;
+		cfg = <2 80 0 0 0 PQR(1,0,0)>;
+		frac = <0x800>;
+	};
+
+	/* VCO = 1066.0 MHz => P = 266 (AXI), Q = 533 (GPU), R = 533 (DDR) */
+	pll2: st,pll@1 {
+		compatible = "st,stm32mp1-pll";
+		reg = <1>;
+		cfg = <2 65 1 0 0 PQR(1,1,1)>;
+		frac = <0x1400>;
+	};
+
+	/* VCO = 417.8 MHz => P = 209, Q = 24, R = 11 */
+	pll3: st,pll@2 {
+		compatible = "st,stm32mp1-pll";
+		reg = <2>;
+		cfg = <1 33 1 16 36 PQR(1,1,1)>;
+		frac = <0x1a04>;
+	};
+
+	/* VCO = 600.0 MHz => P = 99, Q = 74, R = 99 */
+	pll4: st,pll@3 {
+		compatible = "st,stm32mp1-pll";
+		reg = <3>;
+		cfg = <3 98 5 7 5 PQR(1,1,1)>;
+	};
+};
+
+&rng1 {
+	status = "okay";
+};
+
+&rtc {
+	status = "okay";
+};
diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index e55d33f..3a2a032 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -353,6 +353,12 @@
 #define ID_AA64PFR1_EL1_MTE_SHIFT	U(8)
 #define ID_AA64PFR1_EL1_MTE_MASK	ULL(0xf)
 
+#define ID_AA64PFR1_EL1_RNDR_TRAP_SHIFT	U(28)
+#define ID_AA64PFR1_EL1_RNDR_TRAP_MASK	U(0xf)
+
+#define ID_AA64PFR1_EL1_RNG_TRAP_SUPPORTED	ULL(0x1)
+#define ID_AA64PFR1_EL1_RNG_TRAP_NOT_SUPPORTED	ULL(0x0)
+
 /* Memory Tagging Extension is not implemented */
 #define MTE_UNIMPLEMENTED	U(0)
 /* FEAT_MTE: MTE instructions accessible at EL0 are implemented */
@@ -485,6 +491,7 @@
 #define SCR_GPF_BIT		(UL(1) << 48)
 #define SCR_TWEDEL_SHIFT	U(30)
 #define SCR_TWEDEL_MASK		ULL(0xf)
+#define SCR_TRNDR_BIT		(UL(1) << 40)
 #define SCR_HXEn_BIT		(UL(1) << 38)
 #define SCR_ENTP2_SHIFT		U(41)
 #define SCR_ENTP2_BIT		(UL(1) << SCR_ENTP2_SHIFT)
diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index 79a61b5..0af5b74 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h
@@ -129,6 +129,13 @@
 		ID_AA64MMFR1_EL1_HCX_MASK) == ID_AA64MMFR1_EL1_HCX_SUPPORTED);
 }
 
+static inline bool is_feat_rng_trap_present(void)
+{
+	return (((read_id_aa64pfr1_el1() >> ID_AA64PFR1_EL1_RNDR_TRAP_SHIFT) &
+			ID_AA64PFR1_EL1_RNDR_TRAP_MASK)
+			== ID_AA64PFR1_EL1_RNG_TRAP_SUPPORTED);
+}
+
 static inline unsigned int get_armv9_2_feat_rme_support(void)
 {
 	/*
diff --git a/include/arch/aarch64/smccc_helpers.h b/include/arch/aarch64/smccc_helpers.h
index fac6fd9..920f294 100644
--- a/include/arch/aarch64/smccc_helpers.h
+++ b/include/arch/aarch64/smccc_helpers.h
@@ -9,12 +9,26 @@
 
 #include <lib/smccc.h>
 
+/* Definitions to help the assembler access the SMC/ERET args structure */
+#define SMC_ARGS_SIZE		0x40
+#define SMC_ARG0		0x0
+#define SMC_ARG1		0x8
+#define SMC_ARG2		0x10
+#define SMC_ARG3		0x18
+#define SMC_ARG4		0x20
+#define SMC_ARG5		0x28
+#define SMC_ARG6		0x30
+#define SMC_ARG7		0x38
+#define SMC_ARGS_END		0x40
+
 #ifndef __ASSEMBLER__
 
 #include <stdbool.h>
 
 #include <context.h>
 
+#include <platform_def.h> /* For CACHE_WRITEBACK_GRANULE */
+
 /* Convenience macros to return from SMC handler */
 #define SMC_RET0(_h)	{					\
 	return (uint64_t) (_h);					\
@@ -82,6 +96,49 @@
 		_x4 = read_ctx_reg(regs, CTX_GPREG_X4);		\
 	} while (false)
 
+typedef struct {
+	uint64_t _regs[SMC_ARGS_END >> 3];
+} __aligned(CACHE_WRITEBACK_GRANULE) smc_args_t;
+
+/*
+ * Ensure that the assembler's view of the size of the tsp_args is the
+ * same as the compilers.
+ */
+CASSERT(sizeof(smc_args_t) == SMC_ARGS_SIZE, assert_sp_args_size_mismatch);
+
+static inline smc_args_t smc_helper(uint32_t func, uint64_t arg0,
+	       uint64_t arg1, uint64_t arg2,
+	       uint64_t arg3, uint64_t arg4,
+	       uint64_t arg5, uint64_t arg6)
+{
+	smc_args_t ret_args = {0};
+
+	register uint64_t r0 __asm__("x0") = func;
+	register uint64_t r1 __asm__("x1") = arg0;
+	register uint64_t r2 __asm__("x2") = arg1;
+	register uint64_t r3 __asm__("x3") = arg2;
+	register uint64_t r4 __asm__("x4") = arg3;
+	register uint64_t r5 __asm__("x5") = arg4;
+	register uint64_t r6 __asm__("x6") = arg5;
+	register uint64_t r7 __asm__("x7") = arg6;
+
+	/* Output registers, also used as inputs ('+' constraint). */
+	__asm__ volatile("smc #0"
+			: "+r"(r0), "+r"(r1), "+r"(r2), "+r"(r3), "+r"(r4),
+			  "+r"(r5), "+r"(r6), "+r"(r7));
+
+	ret_args._regs[0] = r0;
+	ret_args._regs[1] = r1;
+	ret_args._regs[2] = r2;
+	ret_args._regs[3] = r3;
+	ret_args._regs[4] = r4;
+	ret_args._regs[5] = r5;
+	ret_args._regs[6] = r6;
+	ret_args._regs[7] = r7;
+
+	return ret_args;
+}
+
 #endif /*__ASSEMBLER__*/
 
 #endif /* SMCCC_HELPERS_H */
diff --git a/include/bl31/interrupt_mgmt.h b/include/bl31/interrupt_mgmt.h
index 935bf77..694f1f0 100644
--- a/include/bl31/interrupt_mgmt.h
+++ b/include/bl31/interrupt_mgmt.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -107,15 +107,23 @@
 
 static inline int32_t validate_el3_interrupt_rm(uint32_t x)
 {
-#if EL3_EXCEPTION_HANDLING
+#if defined (EL3_EXCEPTION_HANDLING) && !(defined(SPD_spmd) && (SPMD_SPM_AT_SEL2 == 1))
 	/*
 	 * With EL3 exception handling, EL3 interrupts are always routed to EL3
-	 * from both Secure and Non-secure, and therefore INTR_EL3_VALID_RM1 is
-	 * the only valid routing model.
+	 * from both Secure and Non-secure, when the SPMC does not live in S-EL2.
+	 * Therefore INTR_EL3_VALID_RM1 is the only valid routing model.
 	 */
 	if (x == INTR_EL3_VALID_RM1)
 		return 0;
 #else
+	/*
+	 * When EL3_EXCEPTION_HANDLING is not defined both routing modes are
+	 * valid. This is the most common case. The exception to this rule is
+	 * when EL3_EXCEPTION_HANDLING is defined but also when the SPMC lives
+	 * at S-EL2. In this case, Group0 Interrupts are trapped to the SPMC
+	 * when running in S-EL0 and S-EL1. The SPMC may handle the interrupt
+	 * itself, delegate it to an SP or forward to EL3 for handling.
+	 */
 	if ((x == INTR_EL3_VALID_RM0) || (x == INTR_EL3_VALID_RM1))
 		return 0;
 #endif
diff --git a/include/common/bl_common.h b/include/common/bl_common.h
index 3a06cfb..539280e 100644
--- a/include/common/bl_common.h
+++ b/include/common/bl_common.h
@@ -178,6 +178,7 @@
 
 extern const char build_message[];
 extern const char version_string[];
+const char *get_version(void);
 
 void print_entry_point_info(const entry_point_info_t *ep_info);
 uintptr_t page_align(uintptr_t value, unsigned dir);
diff --git a/include/drivers/mmc.h b/include/drivers/mmc.h
index c154ea5..e973248 100644
--- a/include/drivers/mmc.h
+++ b/include/drivers/mmc.h
@@ -111,6 +111,7 @@
 #define MMC_STATE_SLP			10
 
 #define MMC_FLAG_CMD23			(U(1) << 0)
+#define MMC_FLAG_SD_CMD6		(U(1) << 1)
 
 #define CMD8_CHECK_PATTERN		U(0xAA)
 #define VHS_2_7_3_6_V			BIT(8)
@@ -118,6 +119,10 @@
 #define SD_SCR_BUS_WIDTH_1		BIT(8)
 #define SD_SCR_BUS_WIDTH_4		BIT(10)
 
+#define SD_SWITCH_FUNC_CHECK		0U
+#define SD_SWITCH_FUNC_SWITCH		BIT(31)
+#define SD_SWITCH_ALL_GROUPS_MASK	GENMASK(23, 0)
+
 struct mmc_cmd {
 	unsigned int	cmd_idx;
 	unsigned int	cmd_arg;
@@ -217,6 +222,27 @@
 	unsigned int		csd_structure:		2;
 };
 
+struct sd_switch_status {
+	unsigned short		max_current;
+	unsigned short		support_g6;
+	unsigned short		support_g5;
+	unsigned short		support_g4;
+	unsigned short		support_g3;
+	unsigned short		support_g2;
+	unsigned short		support_g1;
+	unsigned char		sel_g6_g5;
+	unsigned char		sel_g4_g3;
+	unsigned char		sel_g2_g1;
+	unsigned char		data_struct_ver;
+	unsigned short		busy_g6;
+	unsigned short		busy_g5;
+	unsigned short		busy_g4;
+	unsigned short		busy_g3;
+	unsigned short		busy_g2;
+	unsigned short		busy_g1;
+	unsigned short		reserved[17];
+};
+
 enum mmc_device_type {
 	MMC_IS_EMMC,
 	MMC_IS_SD,
diff --git a/include/drivers/nand.h b/include/drivers/nand.h
index 1b78ad4..5e5607c 100644
--- a/include/drivers/nand.h
+++ b/include/drivers/nand.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2019-2022, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -33,6 +33,8 @@
 			     uintptr_t buffer);
 };
 
+void plat_get_scratch_buffer(void **buffer_addr, size_t *buf_size);
+
 /*
  * Read bytes from NAND device
  *
diff --git a/include/lib/cpus/aarch64/cortex_a510.h b/include/lib/cpus/aarch64/cortex_a510.h
index 2b8db14..af38734 100644
--- a/include/lib/cpus/aarch64/cortex_a510.h
+++ b/include/lib/cpus/aarch64/cortex_a510.h
@@ -17,6 +17,8 @@
 #define CORTEX_A510_CPUECTLR_EL1_READPREFERUNIQUE_DISABLE	U(1)
 #define CORTEX_A510_CPUECTLR_EL1_RSCTL_SHIFT			U(23)
 #define CORTEX_A510_CPUECTLR_EL1_NTCTL_SHIFT			U(46)
+#define CORTEX_A510_CPUECTLR_EL1_ATOM_EXECALLINSTRNEAR		U(2)
+#define CORTEX_A510_CPUECTLR_EL1_ATOM				U(38)
 
 /*******************************************************************************
  * CPU Power Control register specific definitions
@@ -33,5 +35,6 @@
  * Auxiliary control register specific definitions
  ******************************************************************************/
 #define CORTEX_A510_CPUACTLR_EL1				S3_0_C15_C1_0
+#define CORTEX_A510_CPUACTLR_EL1_BIT_17				(ULL(1) << 17)
 
-#endif /* CORTEX_A510_H */
+#endif /* CORTEX_A510_H */
\ No newline at end of file
diff --git a/include/lib/cpus/aarch64/cortex_a78c.h b/include/lib/cpus/aarch64/cortex_a78c.h
index 54c95ad..7f7b9a5 100644
--- a/include/lib/cpus/aarch64/cortex_a78c.h
+++ b/include/lib/cpus/aarch64/cortex_a78c.h
@@ -14,11 +14,17 @@
 #define CORTEX_A78C_BHB_LOOP_COUNT			U(32)
 
 /*******************************************************************************
+ * CPU Auxiliary Control register 2 specific definitions.
+ * ****************************************************************************/
+#define CORTEX_A78C_CPUACTLR2_EL1			S3_0_C15_C1_1
+#define CORTEX_A78C_CPUACTLR2_EL1_BIT_40 		(ULL(1) << 40)
+
+/*******************************************************************************
  * CPU Extended Control register specific definitions.
  ******************************************************************************/
 #define CORTEX_A78C_CPUECTLR_EL1		        S3_0_C15_C1_4
-#define CORTEX_A78C_CPUECTLR_EL1_BIT6		        (ULL(1) << 6)
-#define CORTEX_A78C_CPUECTLR_EL1_BIT7		        (ULL(1) << 7)
+#define CORTEX_A78C_CPUECTLR_EL1_BIT_6		        (ULL(1) << 6)
+#define CORTEX_A78C_CPUECTLR_EL1_BIT_7		        (ULL(1) << 7)
 
 /*******************************************************************************
  * CPU Power Control register specific definitions
diff --git a/include/lib/cpus/aarch64/neoverse_n2.h b/include/lib/cpus/aarch64/neoverse_n2.h
index 5d41a13..3ff817c 100644
--- a/include/lib/cpus/aarch64/neoverse_n2.h
+++ b/include/lib/cpus/aarch64/neoverse_n2.h
@@ -37,6 +37,7 @@
  * CPU Auxiliary Control register 2 specific definitions.
  ******************************************************************************/
 #define NEOVERSE_N2_CPUACTLR2_EL1			S3_0_C15_C1_1
+#define NEOVERSE_N2_CPUACTLR2_EL1_BIT_0			(ULL(1) << 0)
 #define NEOVERSE_N2_CPUACTLR2_EL1_BIT_2			(ULL(1) << 2)
 #define NEOVERSE_N2_CPUACTLR2_EL1_BIT_40		(ULL(1) << 40)
 
diff --git a/include/lib/cpus/aarch64/neoverse_v1.h b/include/lib/cpus/aarch64/neoverse_v1.h
index 181be1d..9c7e967 100644
--- a/include/lib/cpus/aarch64/neoverse_v1.h
+++ b/include/lib/cpus/aarch64/neoverse_v1.h
@@ -16,6 +16,10 @@
  * CPU Extended Control register specific definitions.
  ******************************************************************************/
 #define NEOVERSE_V1_CPUECTLR_EL1				S3_0_C15_C1_4
+#define NEOVERSE_V1_CPUPSELR_EL3				S3_6_C15_C8_0
+#define NEOVERSE_V1_CPUPOR_EL3					S3_6_C15_C8_2
+#define NEOVERSE_V1_CPUPMR_EL3					S3_6_C15_C8_3
+#define NEOVERSE_V1_CPUPCR_EL3					S3_6_C15_C8_1
 #define NEOVERSE_V1_CPUECTLR_EL1_BIT_8				(ULL(1) << 8)
 #define NEOVERSE_V1_CPUECTLR_EL1_BIT_53				(ULL(1) << 53)
 #define NEOVERSE_V1_CPUECTLR_EL1_PF_MODE_CNSRV			ULL(3)
diff --git a/include/lib/libc/cdefs.h b/include/lib/libc/cdefs.h
index 0d00722..423f0db 100644
--- a/include/lib/libc/cdefs.h
+++ b/include/lib/libc/cdefs.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -12,6 +12,7 @@
 #define __packed	__attribute__((__packed__))
 #define __used		__attribute__((__used__))
 #define __unused	__attribute__((__unused__))
+#define __maybe_unused	__attribute__((__unused__))
 #define __aligned(x)	__attribute__((__aligned__(x)))
 #define __section(x)	__attribute__((__section__(x)))
 #if RECLAIM_INIT_CODE
diff --git a/include/lib/psa/psa_manifest/sid.h b/include/lib/psa/psa_manifest/sid.h
index 947e58f..580a4cf 100644
--- a/include/lib/psa/psa_manifest/sid.h
+++ b/include/lib/psa/psa_manifest/sid.h
@@ -9,13 +9,9 @@
 #define PSA_MANIFEST_SID_H
 
 /******** PSA_SP_INITIAL_ATTESTATION ********/
-#define RSS_ATTESTATION_SERVICE_SID			(0x00000020U)
-#define RSS_ATTESTATION_SERVICE_VERSION			(1U)
 #define RSS_ATTESTATION_SERVICE_HANDLE			(0x40000103U)
 
 /******** PSA_SP_MEASURED_BOOT ********/
-#define RSS_MEASURED_BOOT_SID				(0x000000E0U)
-#define RSS_MEASURED_BOOT_VERSION			(1U)
-#define RSS_MEASURED_BOOT_HANDLE			(0x40000104U)
+#define RSS_MEASURED_BOOT_HANDLE			(0x40000110U)
 
 #endif /* PSA_MANIFEST_SID_H */
diff --git a/lib/cpus/aarch64/cortex_a510.S b/lib/cpus/aarch64/cortex_a510.S
index f444077..81a4a78 100644
--- a/lib/cpus/aarch64/cortex_a510.S
+++ b/lib/cpus/aarch64/cortex_a510.S
@@ -265,6 +265,72 @@
 endfunc check_errata_2172148
 
 	/* ----------------------------------------------------
+	 * Errata Workaround for Cortex-A510 Errata #2347730.
+	 * This applies to revisions r0p0 - r0p3, r1p0, r1p1.
+	 * It is fixed in r1p2.
+	 * Inputs:
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0-x1, x17
+	 * ----------------------------------------------------
+	 */
+func errata_cortex_a510_2347730_wa
+	mov	x17, x30
+	bl	check_errata_2347730
+	cbz	x0, 1f
+
+	/*
+	 * Set CPUACTLR_EL1[17] to 1'b1, which disables
+	 * specific microarchitectural clock gating
+	 * behaviour.
+	 */
+	mrs	x1, CORTEX_A510_CPUACTLR_EL1
+	orr	x1, x1, CORTEX_A510_CPUACTLR_EL1_BIT_17
+	msr	CORTEX_A510_CPUACTLR_EL1, x1
+1:
+	ret x17
+endfunc errata_cortex_a510_2347730_wa
+
+func check_errata_2347730
+	/* Applies to revisions r1p1 and lower. */
+	mov	x1, #0x11
+	b	cpu_rev_var_ls
+endfunc check_errata_2347730
+
+	/*---------------------------------------------------
+	 * Errata Workaround for Cortex-A510 Errata #2371937.
+	 * This applies to revisions r1p1 and lower, and is
+	 * fixed in r1p2.
+	 * Inputs:
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0, x1, x17
+	 *---------------------------------------------------
+	 */
+func errata_cortex_a510_2371937_wa
+	mov	x17, x30
+	bl	check_errata_2371937
+	cbz	x0, 1f
+
+	/*
+	 * Cacheable atomic operations can be forced
+	 * to be executed near by setting
+	 * IMP_CPUECTLR_EL1.ATOM=0b010. ATOM is found
+	 * in [40:38] of CPUECTLR_EL1.
+	 */
+	mrs 	x0, CORTEX_A510_CPUECTLR_EL1
+	mov 	x1, CORTEX_A510_CPUECTLR_EL1_ATOM_EXECALLINSTRNEAR
+	bfi 	x0, x1, CORTEX_A510_CPUECTLR_EL1_ATOM, #3
+	msr 	CORTEX_A510_CPUECTLR_EL1, x0
+1:
+	ret 	x17
+endfunc errata_cortex_a510_2371937_wa
+
+func check_errata_2371937
+	/* Applies to r1p1 and lower */
+	mov 	x1, #0x11
+	b	cpu_rev_var_ls
+endfunc check_errata_2371937
+
+	/* ----------------------------------------------------
 	 * HW will do the cache maintenance while powering down
 	 * ----------------------------------------------------
 	 */
@@ -301,6 +367,8 @@
 	report_errata ERRATA_A510_2250311, cortex_a510, 2250311
 	report_errata ERRATA_A510_2218950, cortex_a510, 2218950
 	report_errata ERRATA_A510_2172148, cortex_a510, 2172148
+	report_errata ERRATA_A510_2347730, cortex_a510, 2347730
+	report_errata ERRATA_A510_2371937, cortex_a510, 2371937
 	report_errata ERRATA_DSU_2313941, cortex_a510, dsu_2313941
 
 	ldp	x8, x30, [sp], #16
@@ -352,11 +420,21 @@
 	bl	errata_cortex_a510_2218950_wa
 #endif
 
+#if ERRATA_A510_2371937
+	mov 	x0, x18
+	bl	errata_cortex_a510_2371937_wa
+#endif
+
 #if ERRATA_A510_2172148
 	mov	x0, x18
 	bl	errata_cortex_a510_2172148_wa
 #endif
 
+#if ERRATA_A510_2347730
+	mov	x0, x18
+	bl	errata_cortex_a510_2347730_wa
+#endif
+
 	isb
 	ret	x19
 endfunc cortex_a510_reset_func
diff --git a/lib/cpus/aarch64/cortex_a710.S b/lib/cpus/aarch64/cortex_a710.S
index 8d02e7b..0b06169 100644
--- a/lib/cpus/aarch64/cortex_a710.S
+++ b/lib/cpus/aarch64/cortex_a710.S
@@ -280,6 +280,37 @@
 	b       cpu_rev_var_ls
 endfunc check_errata_2136059
 
+/* ----------------------------------------------------------------
+ * Errata workaround for Cortex-A710 Erratum 2147715.
+ * This applies to revision r2p0, and is fixed in r2p1.
+ * Inputs:
+ * x0: variant[4:7] and revision[0:3] of current cpu.
+ * Shall clobber: x0, x1, x17
+ * ----------------------------------------------------------------
+ */
+func errata_a710_2147715_wa
+	mov 	x17, x30
+	bl 	check_errata_2147715
+	cbz	x0, 1f
+
+	/* Apply workaround; set CPUACTLR_EL1[22]
+	 * to 1, which will cause the CFP instruction
+	 * to invalidate all branch predictor resources
+	 * regardless of context.
+	 */
+	mrs 	x1, CORTEX_A710_CPUACTLR_EL1
+	orr	x1, x1, CORTEX_A710_CPUACTLR_EL1_BIT_22
+	msr 	CORTEX_A710_CPUACTLR_EL1, x1
+1:
+	ret	x17
+endfunc errata_a710_2147715_wa
+
+func check_errata_2147715
+	mov 	x1, #0x20
+	mov 	x2, #0x20
+	b 	cpu_rev_var_range
+endfunc check_errata_2147715
+
 /* ---------------------------------------------------------------
  * Errata Workaround for Cortex-A710 Erratum 2282622.
  * This applies to revision r0p0, r1p0 and r2p0.
@@ -438,6 +469,7 @@
 	report_errata ERRATA_A710_2136059, cortex_a710, 2136059
 	report_errata ERRATA_A710_2282622, cortex_a710, 2282622
 	report_errata ERRATA_A710_2008768, cortex_a710, 2008768
+	report_errata ERRATA_A710_2147715, cortex_a710, 2147715
 	report_errata ERRATA_A710_2371105, cortex_a710, 2371105
 	report_errata WORKAROUND_CVE_2022_23960, cortex_a710, cve_2022_23960
 	report_errata ERRATA_DSU_2313941, cortex_a710, dsu_2313941
@@ -500,6 +532,11 @@
 	bl	errata_a710_2136059_wa
 #endif
 
+#if ERRATA_A710_2147715
+	mov	x0, x18
+	bl 	errata_a710_2147715_wa
+#endif
+
 #if ERRATA_A710_2282622
 	mov	x0, x18
 	bl	errata_a710_2282622_wa
diff --git a/lib/cpus/aarch64/cortex_makalu.S b/lib/cpus/aarch64/cortex_a715.S
similarity index 100%
rename from lib/cpus/aarch64/cortex_makalu.S
rename to lib/cpus/aarch64/cortex_a715.S
diff --git a/lib/cpus/aarch64/cortex_a78c.S b/lib/cpus/aarch64/cortex_a78c.S
index fc002e9..4f0bb49 100644
--- a/lib/cpus/aarch64/cortex_a78c.S
+++ b/lib/cpus/aarch64/cortex_a78c.S
@@ -17,6 +17,36 @@
 #error "cortex_a78c must be compiled with HW_ASSISTED_COHERENCY enabled"
 #endif
 
+/* --------------------------------------------------
+ * Errata Workaround for Cortex A78C Erratum 2395411.
+ * This applies to revision r0p1 and r0p2 of the A78C
+ * and is currently open. It is a Cat B erratum.
+ * Inputs:
+ * x0: variant[4:7] and revision[0:3] of current cpu.
+ * Shall clobber: x0-x4, x17
+ * --------------------------------------------------
+ */
+func errata_a78c_2395411_wa
+	/* Check revision. */
+	mov 	x17, x30
+	bl 	check_errata_2395411
+	cbz 	x0, 1f
+
+	/* Set CPUACTRL2_EL1[40] to 1. */
+	mrs 	x1, CORTEX_A78C_CPUACTLR2_EL1
+	orr 	x1, x1, #CORTEX_A78C_CPUACTLR2_EL1_BIT_40
+	msr 	CORTEX_A78C_CPUACTLR2_EL1, x1
+1:
+	ret 	x17
+endfunc errata_a78c_2395411_wa
+
+func check_errata_2395411
+	/* Applies to r0p1 and r0p2 */
+	mov 	x1, #0x01
+	mov 	x2, #0x02
+	b 	cpu_rev_var_range
+endfunc check_errata_2395411
+
 #if WORKAROUND_CVE_2022_23960
 	wa_cve_2022_23960_bhb_vector_table CORTEX_A78C_BHB_LOOP_COUNT, cortex_a78c
 #endif /* WORKAROUND_CVE_2022_23960 */
@@ -43,8 +73,8 @@
 	 * --------------------------------------------------------
 	 */
 	mrs	x0, CORTEX_A78C_CPUECTLR_EL1
-	orr	x0, x0, #CORTEX_A78C_CPUECTLR_EL1_BIT6
-	orr	x0, x0, #CORTEX_A78C_CPUECTLR_EL1_BIT7
+	orr	x0, x0, #CORTEX_A78C_CPUECTLR_EL1_BIT_6
+	orr	x0, x0, #CORTEX_A78C_CPUECTLR_EL1_BIT_7
 	msr	CORTEX_A78C_CPUECTLR_EL1, x0
 	isb
 1:
@@ -121,6 +151,11 @@
 	bl	errata_a78c_2242638_wa
 #endif
 
+#if ERRATA_A78C_2395411
+	mov 	x0, x18
+	bl	errata_a78c_2395411_wa
+#endif
+
 #if IMAGE_BL31 && WORKAROUND_CVE_2022_23960
 	/*
 	 * The Cortex-A78c generic vectors are overridden to apply errata
@@ -166,6 +201,7 @@
 	 */
 	report_errata ERRATA_A78C_2132064, cortex_a78c, 2132064
 	report_errata ERRATA_A78C_2242638, cortex_a78c, 2242638
+	report_errata ERRATA_A78C_2395411, cortex_a78c, 2395411
 	report_errata WORKAROUND_CVE_2022_23960, cortex_a78c, cve_2022_23960
 
 	ldp	x8, x30, [sp], #16
diff --git a/lib/cpus/aarch64/cortex_makalu_elp_arm.S b/lib/cpus/aarch64/cortex_x3.S
similarity index 100%
rename from lib/cpus/aarch64/cortex_makalu_elp_arm.S
rename to lib/cpus/aarch64/cortex_x3.S
diff --git a/lib/cpus/aarch64/neoverse_n2.S b/lib/cpus/aarch64/neoverse_n2.S
index fae3be2..a807b63 100644
--- a/lib/cpus/aarch64/neoverse_n2.S
+++ b/lib/cpus/aarch64/neoverse_n2.S
@@ -338,6 +338,38 @@
 	b	cpu_rev_var_ls
 endfunc check_errata_2280757
 
+
+/* --------------------------------------------------
+ * Errata Workaround for Neoverse N2 Erratum 2376738.
+ * This applies to revision r0p0 of Neoverse N2,
+ * fixed in r0p1.
+ * Inputs:
+ * x0: variant[4:7] and revision[0:3] of current CPU.
+ * Shall clobber: x0-x1, x17
+ * --------------------------------------------------
+ */
+func errata_n2_2376738_wa
+	mov	x17, x30
+	bl	check_errata_2376738
+	cbz	x0, 1f
+
+	/* Set CPUACTLR2_EL1[0] to 1 to force PLDW/PFRM
+	 * ST to behave like PLD/PFRM LD and not cause
+	 * invalidations to other PE caches.
+	 */
+	mrs	x1, NEOVERSE_N2_CPUACTLR2_EL1
+	orr	x1, x1,	NEOVERSE_N2_CPUACTLR2_EL1_BIT_0
+	msr	NEOVERSE_N2_CPUACTLR2_EL1, x1
+1:
+	ret x17
+endfunc errata_n2_2376738_wa
+
+func check_errata_2376738
+	/* Applies to r0p0, fixed in r0p1 */
+	mov	x1, 0x00
+	b	cpu_rev_var_ls
+endfunc check_errata_2376738
+
 /* --------------------------------------------------
  * Errata Workaround for Neoverse N2 Erratum 2388450.
  * This applies to revision r0p0 of Neoverse N2,
@@ -447,6 +479,11 @@
 	bl	errata_n2_2280757_wa
 #endif
 
+#if ERRATA_N2_2376738
+	mov	x0, x18
+	bl	errata_n2_2376738_wa
+#endif
+
 #if ERRATA_N2_2388450
 	mov	x0, x18
 	bl	errata_n2_2388450_wa
@@ -531,6 +568,7 @@
 	report_errata ERRATA_N2_2138958, neoverse_n2, 2138958
 	report_errata ERRATA_N2_2242400, neoverse_n2, 2242400
 	report_errata ERRATA_N2_2280757, neoverse_n2, 2280757
+	report_errata ERRATA_N2_2376738, neoverse_n2, 2376738
 	report_errata ERRATA_N2_2388450, neoverse_n2, 2388450
 	report_errata WORKAROUND_CVE_2022_23960, neoverse_n2, cve_2022_23960
 	report_errata ERRATA_DSU_2313941, neoverse_n2, dsu_2313941
diff --git a/lib/cpus/aarch64/neoverse_v1.S b/lib/cpus/aarch64/neoverse_v1.S
index 378cb92..109b725 100644
--- a/lib/cpus/aarch64/neoverse_v1.S
+++ b/lib/cpus/aarch64/neoverse_v1.S
@@ -27,6 +27,82 @@
 #endif /* WORKAROUND_CVE_2022_23960 */
 
 	/* --------------------------------------------------
+	 * Errata Workaround for Neoverse V1 Errata #1618635.
+	 * This applies to revision r0p0 and is fixed in
+	 * r1p0.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0, x17
+	 * --------------------------------------------------
+	 */
+func errata_neoverse_v1_1618635_wa
+	/* Check workaround compatibility. */
+	mov	x17, x30
+	bl	check_errata_1618635
+	cbz	x0, 1f
+
+	/* Inserts a DMB SY before and after MRS PAR_EL1 */
+	ldr	x0, =0x0
+	msr	NEOVERSE_V1_CPUPSELR_EL3, x0
+	ldr	x0, = 0xEE070F14
+	msr	NEOVERSE_V1_CPUPOR_EL3, x0
+	ldr	x0, = 0xFFFF0FFF
+	msr	NEOVERSE_V1_CPUPMR_EL3, x0
+	ldr	x0, =0x4005027FF
+	msr	NEOVERSE_V1_CPUPCR_EL3, x0
+
+	/* Inserts a DMB SY before STREX imm offset */
+	ldr	x0, =0x1
+	msr	NEOVERSE_V1_CPUPSELR_EL3, x0
+	ldr	x0, =0x00e8400000
+	msr	NEOVERSE_V1_CPUPOR_EL3, x0
+	ldr	x0, =0x00fff00000
+	msr	NEOVERSE_V1_CPUPMR_EL3, x0
+	ldr	x0, = 0x4001027FF
+	msr	NEOVERSE_V1_CPUPCR_EL3, x0
+
+	/* Inserts a DMB SY before STREX[BHD}/STLEX* */
+	ldr	x0, =0x2
+	msr	NEOVERSE_V1_CPUPSELR_EL3, x0
+	ldr	x0, =0x00e8c00040
+	msr	NEOVERSE_V1_CPUPOR_EL3, x0
+	ldr	x0, =0x00fff00040
+	msr	NEOVERSE_V1_CPUPMR_EL3, x0
+	ldr	x0, = 0x4001027FF
+	msr	NEOVERSE_V1_CPUPCR_EL3, x0
+
+	/* Inserts a DMB SY after STREX imm offset */
+	ldr	x0, =0x3
+	msr	NEOVERSE_V1_CPUPSELR_EL3, x0
+	ldr	x0, =0x00e8400000
+	msr	NEOVERSE_V1_CPUPOR_EL3, x0
+	ldr	x0, =0x00fff00000
+	msr	NEOVERSE_V1_CPUPMR_EL3, x0
+	ldr	x0, = 0x4004027FF
+	msr	NEOVERSE_V1_CPUPCR_EL3, x0
+
+	/* Inserts a DMB SY after STREX[BHD}/STLEX* */
+	ldr	x0, =0x4
+	msr	NEOVERSE_V1_CPUPSELR_EL3, x0
+	ldr	x0, =0x00e8c00040
+	msr	NEOVERSE_V1_CPUPOR_EL3, x0
+	ldr	x0, =0x00fff00040
+	msr	NEOVERSE_V1_CPUPMR_EL3, x0
+	ldr	x0, = 0x4004027FF
+	msr	NEOVERSE_V1_CPUPCR_EL3, x0
+
+	/* Synchronize to enable patches */
+	isb
+1:
+	ret x17
+endfunc errata_neoverse_v1_1618635_wa
+
+func check_errata_1618635
+	/* Applies to revision r0p0. */
+	mov	x1, #0x00
+	b	cpu_rev_var_ls
+endfunc check_errata_1618635
+
+	/* --------------------------------------------------
 	 * Errata Workaround for Neoverse V1 Errata #1774420.
 	 * This applies to revisions r0p0 and r1p0, fixed in r1p1.
 	 * x0: variant[4:7] and revision[0:3] of current cpu.
@@ -425,6 +501,7 @@
 	 * Report all errata. The revision-variant information is passed to
 	 * checking functions of each errata.
 	 */
+	report_errata ERRATA_V1_1618635, neoverse_v1, 1618635
 	report_errata ERRATA_V1_1774420, neoverse_v1, 1774420
 	report_errata ERRATA_V1_1791573, neoverse_v1, 1791573
 	report_errata ERRATA_V1_1852267, neoverse_v1, 1852267
@@ -450,6 +527,11 @@
 	msr	SSBS, xzr
 	isb
 
+#if ERRATA_V1_1618635
+	mov x0, x18
+	bl errata_neoverse_v1_1618635_wa
+#endif
+
 #if ERRATA_V1_1774420
 	mov	x0, x18
 	bl	errata_neoverse_v1_1774420_wa
diff --git a/lib/cpus/cpu-ops.mk b/lib/cpus/cpu-ops.mk
index b114824..36832cd 100644
--- a/lib/cpus/cpu-ops.mk
+++ b/lib/cpus/cpu-ops.mk
@@ -369,6 +369,10 @@
 # to revisions r0p1 and r0p2 of the A78C cpu. It is still open.
 ERRATA_A78C_2242638	?=0
 
+# Flag to apply erratum 2395411 workaround during reset. This erratum applies
+# to revisions r0p1 and r0p2 of the A78C cpu. It is still open.
+ERRATA_A78C_2395411 	?=0
+
 # Flag to apply erratum 1821534 workaround during reset. This erratum applies
 # to revisions r0p0 - r1p0 of the X1 cpu and fixed in r1p1.
 ERRATA_X1_1821534	?=0
@@ -442,6 +446,10 @@
 # to revisions r0p0 of the Neoverse-N2 cpu, it is still open.
 ERRATA_N2_2002655	?=0
 
+# Flag to apply erratum 1618635 workaround during reset. This erratum applies
+# to revision r0p0 of the Neoverse V1 cpu and was fixed in the revision r1p0.
+ERRATA_V1_1618635	?=0
+
 # Flag to apply erratum 1774420 workaround during reset. This erratum applies
 # to revisions r0p0 and r1p0 of the Neoverse V1 core, and was fixed in r1p1.
 ERRATA_V1_1774420	?=0
@@ -520,6 +528,10 @@
 # to revision r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is fixed in r2p1.
 ERRATA_A710_2136059	?=0
 
+# Flag to apply erratum 2147715 workaround during reset. This erratum applies
+# to revision r2p0 of the Cortex-A710 CPU and is fixed in revision r2p1.
+ERRATA_A710_2147715	?=0
+
 # Flag to apply erratum 2282622 workaround during reset. This erratum applies
 # to revision r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is fixed in r2p1.
 ERRATA_A710_2282622	?=0
@@ -568,6 +580,10 @@
 # to revision r0p0 of the Neoverse N2 cpu and is still open.
 ERRATA_N2_2280757	?=0
 
+# Flag to apply erratum 2376738 workaround during reset. This erratum applies
+# to revision r0p0 of the Neoverse N2 cpu, it is fixed in r0p1.
+ERRATA_N2_2376738	?=0
+
 # Flag to apply erratum 2388450 workaround during reset. This erratum applies
 # to revision r0p0 of the Neoverse N2 cpu, it is fixed in r0p1.
 ERRATA_N2_2388450	?=0
@@ -637,6 +653,15 @@
 # to revisions r0p0, r0p1, r0p2, r0p3 and r1p0, and is fixed in r1p1.
 ERRATA_A510_2172148	?=0
 
+# Flag to apply erratum 2347730 workaround during reset. This erratum applies
+# to revisions r0p0, r0p1, r0p2, r0p3, r1p0 and r1p1 of the Cortex-A510 CPU,
+# and is fixed in r1p2.
+ERRATA_A510_2347730	?=0
+
+# Flag to apply erratum 2371937 workaround during reset. This erratum applies
+# to revisions r0p0, r0p1, r0p2, r0p3, r1p0, and r1p1. It is fixed in r1p2.
+ERRATA_A510_2371937	?=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
@@ -939,6 +964,10 @@
 $(eval $(call assert_boolean,ERRATA_A78C_2242638))
 $(eval $(call add_define,ERRATA_A78C_2242638))
 
+# Process ERRATA_A78C_2395411 flag
+$(eval $(call assert_boolean,ERRATA_A78C_2395411))
+$(eval $(call add_define,ERRATA_A78C_2395411))
+
 # Process ERRATA_X1_1821534 flag
 $(eval $(call assert_boolean,ERRATA_X1_1821534))
 $(eval $(call add_define,ERRATA_X1_1821534))
@@ -1011,6 +1040,10 @@
 $(eval $(call assert_boolean,ERRATA_N2_2002655))
 $(eval $(call add_define,ERRATA_N2_2002655))
 
+# Process ERRATA_V1_1618635 flag
+$(eval $(call assert_boolean,ERRATA_V1_1618635))
+$(eval $(call add_define,ERRATA_V1_1618635))
+
 # Process ERRATA_V1_1774420 flag
 $(eval $(call assert_boolean,ERRATA_V1_1774420))
 $(eval $(call add_define,ERRATA_V1_1774420))
@@ -1087,6 +1120,10 @@
 $(eval $(call assert_boolean,ERRATA_A710_2136059))
 $(eval $(call add_define,ERRATA_A710_2136059))
 
+# Process ERRATA_A710_2147715 flag
+$(eval $(call assert_boolean,ERRATA_A710_2147715))
+$(eval $(call add_define,ERRATA_A710_2147715))
+
 # Process ERRATA_A710_2282622 flag
 $(eval $(call assert_boolean,ERRATA_A710_2282622))
 $(eval $(call add_define,ERRATA_A710_2282622))
@@ -1135,6 +1172,10 @@
 $(eval $(call assert_boolean,ERRATA_N2_2280757))
 $(eval $(call add_define,ERRATA_N2_2280757))
 
+# Process ERRATA_N2_2376738 flag
+$(eval $(call assert_boolean,ERRATA_N2_2376738))
+$(eval $(call add_define,ERRATA_N2_2376738))
+
 # Process ERRATA_N2_2388450 flag
 $(eval $(call assert_boolean,ERRATA_N2_2388450))
 $(eval $(call add_define,ERRATA_N2_2388450))
@@ -1199,6 +1240,14 @@
 $(eval $(call assert_boolean,ERRATA_A510_2172148))
 $(eval $(call add_define,ERRATA_A510_2172148))
 
+# Process ERRATA_A510_2347730 flag
+$(eval $(call assert_boolean,ERRATA_A510_2347730))
+$(eval $(call add_define,ERRATA_A510_2347730))
+
+# Process ERRATA_A510_2371937 flag
+$(eval $(call assert_boolean,ERRATA_A510_2371937))
+$(eval $(call add_define,ERRATA_A510_2371937))
+
 # 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_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index da610d0..68aacc1 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -299,6 +299,14 @@
 	scr_el3 |= SCR_HXEn_BIT;
 #endif
 
+	/*
+	 * If FEAT_RNG_TRAP is enabled, all reads of the RNDR and RNDRRS
+	 * registers are trapped to EL3.
+	 */
+#if ENABLE_FEAT_RNG_TRAP
+	scr_el3 |= SCR_TRNDR_BIT;
+#endif
+
 #if RAS_TRAP_LOWER_EL_ERR_ACCESS
 	/*
 	 * SCR_EL3.TERR: Trap Error record accesses. Accesses to the RAS ERR
diff --git a/make_helpers/build_macros.mk b/make_helpers/build_macros.mk
index a58caf5..abdd4d0 100644
--- a/make_helpers/build_macros.mk
+++ b/make_helpers/build_macros.mk
@@ -499,7 +499,8 @@
 	$(call MAKE_BUILD_STRINGS, $(BUILD_DIR)/build_message.o)
 else
 	@echo 'const char build_message[] = "Built : "$(BUILD_MESSAGE_TIMESTAMP); \
-	       const char version_string[] = "${VERSION_STRING}";' | \
+	       const char version_string[] = "${VERSION_STRING}"; \
+	       const char version[] = "${VERSION}";' | \
 		$$(CC) $$(TF_CFLAGS) $$(CFLAGS) -xc -c - -o $(BUILD_DIR)/build_message.o
 endif
 ifneq ($(findstring armlink,$(notdir $(LD))),)
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index d957a4b..5e73120 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -160,6 +160,10 @@
 # Flag to enable access to the Random Number Generator registers
 ENABLE_FEAT_RNG			:= 0
 
+# Flag to enable support for EL3 trapping of reads of the RNDR and RNDRRS
+# registers, by setting SCR_EL3.TRNDR.
+ENABLE_FEAT_RNG_TRAP		:= 0
+
 # Flag to enable Speculation Barrier Instruction
 ENABLE_FEAT_SB			:= 0
 
diff --git a/make_helpers/windows.mk b/make_helpers/windows.mk
index 26ea88e..b6d6f0b 100644
--- a/make_helpers/windows.mk
+++ b/make_helpers/windows.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2016-2022, Arm Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -81,8 +81,9 @@
 # by defining the MAKE_BUILD_STRINGS macro.
 BUILT_TIME_DATE_STRING = const char build_message[] = "Built : "${BUILD_MESSAGE_TIMESTAMP};
 VERSION_STRING_MESSAGE = const char version_string[] = "${VERSION_STRING}";
+VERSION_MESSAGE = const char version[] = "${VERSION}";
 define MAKE_BUILD_STRINGS
-	@echo $$(BUILT_TIME_DATE_STRING) $$(VERSION_STRING_MESSAGE) | \
+	@echo $$(BUILT_TIME_DATE_STRING) $$(VERSION_STRING_MESSAGE) $$(VERSION_MESSAGE) | \
 		$$(CC) $$(TF_CFLAGS) $$(CFLAGS) -x c -c - -o $1
 endef
 
diff --git a/plat/arm/board/arm_fpga/platform.mk b/plat/arm/board/arm_fpga/platform.mk
index 084532c..a14a0d8 100644
--- a/plat/arm/board/arm_fpga/platform.mk
+++ b/plat/arm/board/arm_fpga/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2021, Arm Limited. All rights reserved.
+# Copyright (c) 2021-2022, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -71,9 +71,9 @@
 				lib/cpus/aarch64/cortex_a65.S		\
 				lib/cpus/aarch64/cortex_a65ae.S		\
 				lib/cpus/aarch64/cortex_a510.S		\
-				lib/cpus/aarch64/cortex_a710.S	\
-				lib/cpus/aarch64/cortex_makalu.S	\
-				lib/cpus/aarch64/cortex_makalu_elp_arm.S \
+				lib/cpus/aarch64/cortex_a710.S		\
+				lib/cpus/aarch64/cortex_a715.S		\
+				lib/cpus/aarch64/cortex_x3.S 		\
 				lib/cpus/aarch64/cortex_a78c.S
 
 # AArch64/AArch32 cores
diff --git a/plat/arm/board/fvp/fdts/fvp_tsp_sp_manifest.dts b/plat/arm/board/fvp/fdts/fvp_tsp_sp_manifest.dts
new file mode 100644
index 0000000..1587c72
--- /dev/null
+++ b/plat/arm/board/fvp/fdts/fvp_tsp_sp_manifest.dts
@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+/dts-v1/;
+
+#define	AFF	00
+
+#include "fvp-defs.dtsi"
+#undef POST
+#define	POST \
+	};
+
+#define S_EL0	(0x1)
+#define S_EL1	(0x2)
+
+/* For consumption by EL3 SPMC. */
+/ {
+	compatible = "arm,ffa-manifest-1.0";
+	#address-cells = <2>;
+	#size-cells = <1>;
+
+	ffa-version = <0x00010001>; /* 31:16 - Major, 15:0 - Minor */
+	id = <0x8001>;
+	uuid = <0x6b43b460 0x74a24b78 0xade24502 0x40682886>;
+	messaging-method = <0x3>; /* Direct Messaging Only */
+	exception-level = <S_EL1>;
+	execution-state = <0>;
+	execution-ctx-count = <8>;
+	gp-register-num = <0>;
+	/* Subscribe to CPU_OFF, CPU_SUSPEND and CPU_SUSPEND_RESUME PM Msgs */
+	power-management-messages = <0x7>;
+};
diff --git a/plat/arm/board/fvp/include/platform_def.h b/plat/arm/board/fvp/include/platform_def.h
index d0f8aa9..e1bf46d 100644
--- a/plat/arm/board/fvp/include/platform_def.h
+++ b/plat/arm/board/fvp/include/platform_def.h
@@ -249,7 +249,11 @@
 #elif defined(IMAGE_BL31)
 #  define PLATFORM_STACK_SIZE		UL(0x800)
 #elif defined(IMAGE_BL32)
-# define PLATFORM_STACK_SIZE		UL(0x440)
+# if SPMC_AT_EL3
+#  define PLATFORM_STACK_SIZE		UL(0x1000)
+# else
+#  define PLATFORM_STACK_SIZE		UL(0x440)
+# endif /* SPMC_AT_EL3 */
 #elif defined(IMAGE_RMM)
 # define PLATFORM_STACK_SIZE		UL(0x440)
 #endif
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index a7e27e8..2539712 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -135,9 +135,9 @@
 					lib/cpus/aarch64/neoverse_demeter.S	\
 					lib/cpus/aarch64/cortex_a78_ae.S	\
 					lib/cpus/aarch64/cortex_a510.S		\
-					lib/cpus/aarch64/cortex_a710.S	\
-					lib/cpus/aarch64/cortex_makalu.S	\
-					lib/cpus/aarch64/cortex_makalu_elp_arm.S \
+					lib/cpus/aarch64/cortex_a710.S		\
+					lib/cpus/aarch64/cortex_a715.S		\
+					lib/cpus/aarch64/cortex_x3.S 		\
 					lib/cpus/aarch64/cortex_a65.S		\
 					lib/cpus/aarch64/cortex_a65ae.S		\
 					lib/cpus/aarch64/cortex_a78c.S		\
diff --git a/plat/arm/board/tc/platform.mk b/plat/arm/board/tc/platform.mk
index e6c1c7c..1a1bc56 100644
--- a/plat/arm/board/tc/platform.mk
+++ b/plat/arm/board/tc/platform.mk
@@ -73,8 +73,8 @@
 # CPU libraries for TARGET_PLATFORM=1
 ifeq (${TARGET_PLATFORM}, 1)
 TC_CPU_SOURCES	+=	lib/cpus/aarch64/cortex_a510.S \
-			lib/cpus/aarch64/cortex_makalu.S \
-			lib/cpus/aarch64/cortex_makalu_elp_arm.S
+			lib/cpus/aarch64/cortex_a715.S \
+			lib/cpus/aarch64/cortex_x3.S
 endif
 
 # CPU libraries for TARGET_PLATFORM=2
diff --git a/plat/arm/common/aarch32/arm_bl2_mem_params_desc.c b/plat/arm/common/aarch32/arm_bl2_mem_params_desc.c
index 78360b0..18f1a37 100644
--- a/plat/arm/common/aarch32/arm_bl2_mem_params_desc.c
+++ b/plat/arm/common/aarch32/arm_bl2_mem_params_desc.c
@@ -20,71 +20,72 @@
 static bl_mem_params_node_t bl2_mem_params_descs[] = {
 #ifdef SCP_BL2_BASE
 	/* Fill SCP_BL2 related information if it exists */
-    {
-	    .image_id = SCP_BL2_IMAGE_ID,
+	{
+		.image_id = SCP_BL2_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = SCP_BL2_BASE,
-	    .image_info.image_max_size = PLAT_CSS_MAX_SCP_BL2_SIZE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = SCP_BL2_BASE,
+		.image_info.image_max_size = PLAT_CSS_MAX_SCP_BL2_SIZE,
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 #endif /* SCP_BL2_BASE */
 
 	/* Fill BL32 related information */
-    {
-	    .image_id = BL32_IMAGE_ID,
+	{
+		.image_id = BL32_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t,
-		    SECURE | EXECUTABLE | EP_FIRST_EXE),
-	    .ep_info.pc = BL32_BASE,
-	    .ep_info.spsr = SPSR_MODE32(MODE32_mon, SPSR_T_ARM,
-		    SPSR_E_LITTLE, DISABLE_ALL_EXCEPTIONS),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = BL32_BASE,
+		.ep_info.spsr = SPSR_MODE32(MODE32_mon, SPSR_T_ARM,
+			SPSR_E_LITTLE, DISABLE_ALL_EXCEPTIONS),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
-	    .image_info.image_base = BL32_BASE,
-	    .image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
+		.image_info.image_base = BL32_BASE,
+		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
 
-	    .next_handoff_image_id = BL33_IMAGE_ID,
-    },
+		.next_handoff_image_id = BL33_IMAGE_ID,
+	},
 	/* Fill HW_CONFIG related information if it exists */
-    {
-	    .image_id = HW_CONFIG_ID,
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, entry_point_info_t, NON_SECURE | NON_EXECUTABLE),
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+	{
+		.image_id = HW_CONFIG_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t,
+			NON_SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 	/* Fill BL33 related information */
-    {
-	    .image_id = BL33_IMAGE_ID,
+	{
+		.image_id = BL33_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, NON_SECURE | EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, NON_SECURE | EXECUTABLE),
 #ifdef PRELOADED_BL33_BASE
-	    .ep_info.pc = PRELOADED_BL33_BASE,
+		.ep_info.pc = PRELOADED_BL33_BASE,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
 #else
-	    .ep_info.pc = PLAT_ARM_NS_IMAGE_BASE,
+		.ep_info.pc = PLAT_ARM_NS_IMAGE_BASE,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = PLAT_ARM_NS_IMAGE_BASE,
-	    .image_info.image_max_size = ARM_DRAM1_BASE + ARM_DRAM1_SIZE
-		    - PLAT_ARM_NS_IMAGE_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = PLAT_ARM_NS_IMAGE_BASE,
+		.image_info.image_max_size = ARM_DRAM1_BASE + ARM_DRAM1_SIZE
+			- PLAT_ARM_NS_IMAGE_BASE,
 #endif /* PRELOADED_BL33_BASE */
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    }
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	}
 };
 
 REGISTER_BL_IMAGE_DESCS(bl2_mem_params_descs)
diff --git a/plat/arm/common/aarch64/arm_bl2_mem_params_desc.c b/plat/arm/common/aarch64/arm_bl2_mem_params_desc.c
index 0666e57..3d7b361 100644
--- a/plat/arm/common/aarch64/arm_bl2_mem_params_desc.c
+++ b/plat/arm/common/aarch64/arm_bl2_mem_params_desc.c
@@ -20,203 +20,207 @@
 static bl_mem_params_node_t bl2_mem_params_descs[] = {
 #ifdef SCP_BL2_BASE
 	/* Fill SCP_BL2 related information if it exists */
-    {
-	    .image_id = SCP_BL2_IMAGE_ID,
+	{
+		.image_id = SCP_BL2_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = SCP_BL2_BASE,
-	    .image_info.image_max_size = PLAT_CSS_MAX_SCP_BL2_SIZE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = SCP_BL2_BASE,
+		.image_info.image_max_size = PLAT_CSS_MAX_SCP_BL2_SIZE,
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 #endif /* SCP_BL2_BASE */
 
 #ifdef EL3_PAYLOAD_BASE
 	/* Fill EL3 payload related information (BL31 is EL3 payload)*/
-    {
-	    .image_id = BL31_IMAGE_ID,
+	{
+		.image_id = BL31_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t,
-		    SECURE | EXECUTABLE | EP_FIRST_EXE),
-	    .ep_info.pc = EL3_PAYLOAD_BASE,
-	    .ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
-		    DISABLE_ALL_EXCEPTIONS),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = EL3_PAYLOAD_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+			DISABLE_ALL_EXCEPTIONS),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t,
-		    IMAGE_ATTRIB_PLAT_SETUP | IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t,
+			IMAGE_ATTRIB_PLAT_SETUP | IMAGE_ATTRIB_SKIP_LOADING),
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 
 #else /* EL3_PAYLOAD_BASE */
 
 	/* Fill BL31 related information */
-    {
-	    .image_id = BL31_IMAGE_ID,
+	{
+		.image_id = BL31_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t,
-		    SECURE | EXECUTABLE | EP_FIRST_EXE),
-	    .ep_info.pc = BL31_BASE,
-	    .ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
-		    DISABLE_ALL_EXCEPTIONS),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = BL31_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+			DISABLE_ALL_EXCEPTIONS),
 #if DEBUG
-	    .ep_info.args.arg3 = ARM_BL31_PLAT_PARAM_VAL,
+		.ep_info.args.arg3 = ARM_BL31_PLAT_PARAM_VAL,
 #endif
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
-	    .image_info.image_base = BL31_BASE,
-	    .image_info.image_max_size = BL31_LIMIT - BL31_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
+		.image_info.image_base = BL31_BASE,
+		.image_info.image_max_size = BL31_LIMIT - BL31_BASE,
 
 # if defined(BL32_BASE)
-	    .next_handoff_image_id = BL32_IMAGE_ID,
+		.next_handoff_image_id = BL32_IMAGE_ID,
 # elif ENABLE_RME
-	    .next_handoff_image_id = RMM_IMAGE_ID,
+		.next_handoff_image_id = RMM_IMAGE_ID,
 # else
-	    .next_handoff_image_id = BL33_IMAGE_ID,
+		.next_handoff_image_id = BL33_IMAGE_ID,
 # endif
-    },
+	},
 	/* Fill HW_CONFIG related information */
-    {
-	    .image_id = HW_CONFIG_ID,
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, entry_point_info_t, NON_SECURE | NON_EXECUTABLE),
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+	{
+		.image_id = HW_CONFIG_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t,
+			NON_SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 	/* Fill SOC_FW_CONFIG related information */
-    {
-	    .image_id = SOC_FW_CONFIG_ID,
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+	{
+		.image_id = SOC_FW_CONFIG_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 
 # if ENABLE_RME
 	/* Fill RMM related information */
-    {
-	    .image_id = RMM_IMAGE_ID,
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, EP_REALM | EXECUTABLE),
-	    .ep_info.pc = RMM_BASE,
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = RMM_BASE,
-	    .image_info.image_max_size = RMM_LIMIT - RMM_BASE,
-	    .next_handoff_image_id = BL33_IMAGE_ID,
-    },
+	{
+		.image_id = RMM_IMAGE_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, EP_REALM | EXECUTABLE),
+		.ep_info.pc = RMM_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = RMM_BASE,
+		.image_info.image_max_size = RMM_LIMIT - RMM_BASE,
+		.next_handoff_image_id = BL33_IMAGE_ID,
+	},
 # endif
 
 # ifdef BL32_BASE
 	/* Fill BL32 related information */
-    {
-	    .image_id = BL32_IMAGE_ID,
+	{
+		.image_id = BL32_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, SECURE | EXECUTABLE),
-	    .ep_info.pc = BL32_BASE,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | EXECUTABLE),
+		.ep_info.pc = BL32_BASE,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = BL32_BASE,
-	    .image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = BL32_BASE,
+		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
 
 # if ENABLE_RME
-	    .next_handoff_image_id = RMM_IMAGE_ID,
+		.next_handoff_image_id = RMM_IMAGE_ID,
 # else
-	    .next_handoff_image_id = BL33_IMAGE_ID,
+		.next_handoff_image_id = BL33_IMAGE_ID,
 # endif
-    },
+	},
 
 	/*
 	 * Fill BL32 external 1 related information.
-	 * A typical use for extra1 image is with OP-TEE where it is the pager image.
+	 * A typical use for extra1 image is with OP-TEE where it is the pager
+	 * image.
 	 */
-    {
-	    .image_id = BL32_EXTRA1_IMAGE_ID,
+	{
+		.image_id = BL32_EXTRA1_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
-	    .image_info.image_base = BL32_BASE,
-	    .image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		.image_info.image_base = BL32_BASE,
+		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 
 	/*
 	 * Fill BL32 external 2 related information.
-	 * A typical use for extra2 image is with OP-TEE where it is the paged image.
+	 * A typical use for extra2 image is with OP-TEE where it is the paged
+	 * image.
 	 */
-    {
-	    .image_id = BL32_EXTRA2_IMAGE_ID,
+	{
+		.image_id = BL32_EXTRA2_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
 #ifdef SPD_opteed
-	    .image_info.image_base = ARM_OPTEE_PAGEABLE_LOAD_BASE,
-	    .image_info.image_max_size = ARM_OPTEE_PAGEABLE_LOAD_SIZE,
+		.image_info.image_base = ARM_OPTEE_PAGEABLE_LOAD_BASE,
+		.image_info.image_max_size = ARM_OPTEE_PAGEABLE_LOAD_SIZE,
 #endif
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 
 	/* Fill TOS_FW_CONFIG related information */
-    {
-	    .image_id = TOS_FW_CONFIG_ID,
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+	{
+		.image_id = TOS_FW_CONFIG_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 # endif /* BL32_BASE */
 
 	/* Fill BL33 related information */
-    {
-	    .image_id = BL33_IMAGE_ID,
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, NON_SECURE | EXECUTABLE),
+	{
+		.image_id = BL33_IMAGE_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, NON_SECURE | EXECUTABLE),
 # ifdef PRELOADED_BL33_BASE
-	    .ep_info.pc = PRELOADED_BL33_BASE,
+		.ep_info.pc = PRELOADED_BL33_BASE,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
 # else
-	    .ep_info.pc = PLAT_ARM_NS_IMAGE_BASE,
+		.ep_info.pc = PLAT_ARM_NS_IMAGE_BASE,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = PLAT_ARM_NS_IMAGE_BASE,
-	    .image_info.image_max_size = ARM_DRAM1_BASE + ARM_DRAM1_SIZE
-		    - PLAT_ARM_NS_IMAGE_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = PLAT_ARM_NS_IMAGE_BASE,
+		.image_info.image_max_size = ARM_DRAM1_BASE + ARM_DRAM1_SIZE
+			- PLAT_ARM_NS_IMAGE_BASE,
 # endif /* PRELOADED_BL33_BASE */
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 	/* Fill NT_FW_CONFIG related information */
-    {
-	    .image_id = NT_FW_CONFIG_ID,
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, entry_point_info_t, NON_SECURE | NON_EXECUTABLE),
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    }
+	{
+		.image_id = NT_FW_CONFIG_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t,
+			NON_SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	}
 #endif /* EL3_PAYLOAD_BASE */
 };
 
diff --git a/plat/brcm/common/brcm_bl2_mem_params_desc.c b/plat/brcm/common/brcm_bl2_mem_params_desc.c
index f711354..aed99d9 100644
--- a/plat/brcm/common/brcm_bl2_mem_params_desc.c
+++ b/plat/brcm/common/brcm_bl2_mem_params_desc.c
@@ -22,84 +22,84 @@
 #ifdef SCP_BL2_BASE
 	/* Fill SCP_BL2 related information if it exists */
 	{
-	    .image_id = SCP_BL2_IMAGE_ID,
+		.image_id = SCP_BL2_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = SCP_BL2_BASE,
-	    .image_info.image_max_size = PLAT_MAX_SCP_BL2_SIZE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = SCP_BL2_BASE,
+		.image_info.image_max_size = PLAT_MAX_SCP_BL2_SIZE,
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
+		.next_handoff_image_id = INVALID_IMAGE_ID,
 	},
 #endif /* SCP_BL2_BASE */
 
 	/* Fill BL31 related information */
 	{
-	    .image_id = BL31_IMAGE_ID,
+		.image_id = BL31_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t,
-		    SECURE | EXECUTABLE | EP_FIRST_EXE),
-	    .ep_info.pc = BL31_BASE,
-	    .ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
-		    DISABLE_ALL_EXCEPTIONS),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = BL31_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+			DISABLE_ALL_EXCEPTIONS),
 #if DEBUG
-	    .ep_info.args.arg3 = BRCM_BL31_PLAT_PARAM_VAL,
+		.ep_info.args.arg3 = BRCM_BL31_PLAT_PARAM_VAL,
 #endif
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
-	    .image_info.image_base = BL31_BASE,
-	    .image_info.image_max_size = BL31_LIMIT - BL31_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
+		.image_info.image_base = BL31_BASE,
+		.image_info.image_max_size = BL31_LIMIT - BL31_BASE,
 
 #ifdef BL32_BASE
-	    .next_handoff_image_id = BL32_IMAGE_ID,
+		.next_handoff_image_id = BL32_IMAGE_ID,
 #else
-	    .next_handoff_image_id = BL33_IMAGE_ID,
+		.next_handoff_image_id = BL33_IMAGE_ID,
 #endif
 	},
 
 #ifdef BL32_BASE
 	/* Fill BL32 related information */
 	{
-	    .image_id = BL32_IMAGE_ID,
+		.image_id = BL32_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, SECURE | EXECUTABLE),
-	    .ep_info.pc = BL32_BASE,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | EXECUTABLE),
+		.ep_info.pc = BL32_BASE,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = BL32_BASE,
-	    .image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = BL32_BASE,
+		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
 
-	    .next_handoff_image_id = BL33_IMAGE_ID,
+		.next_handoff_image_id = BL33_IMAGE_ID,
 	},
 #endif /* BL32_BASE */
 
 	/* Fill BL33 related information */
 	{
-	    .image_id = BL33_IMAGE_ID,
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, NON_SECURE | EXECUTABLE),
+		.image_id = BL33_IMAGE_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, NON_SECURE | EXECUTABLE),
 #ifdef PRELOADED_BL33_BASE
-	    .ep_info.pc = PRELOADED_BL33_BASE,
+		.ep_info.pc = PRELOADED_BL33_BASE,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
 #else
-	    .ep_info.pc = PLAT_BRCM_NS_IMAGE_OFFSET,
+		.ep_info.pc = PLAT_BRCM_NS_IMAGE_OFFSET,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = PLAT_BRCM_NS_IMAGE_OFFSET,
-	    .image_info.image_max_size = BRCM_DRAM1_SIZE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = PLAT_BRCM_NS_IMAGE_OFFSET,
+		.image_info.image_max_size = BRCM_DRAM1_SIZE,
 #endif /* PRELOADED_BL33_BASE */
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
+		.next_handoff_image_id = INVALID_IMAGE_ID,
 	}
 };
 
diff --git a/plat/hisilicon/hikey/hikey_bl2_mem_params_desc.c b/plat/hisilicon/hikey/hikey_bl2_mem_params_desc.c
index 4e013a0..d029703 100644
--- a/plat/hisilicon/hikey/hikey_bl2_mem_params_desc.c
+++ b/plat/hisilicon/hikey/hikey_bl2_mem_params_desc.c
@@ -21,144 +21,146 @@
 static bl_mem_params_node_t bl2_mem_params_descs[] = {
 #ifdef SCP_BL2_BASE
 	/* Fill SCP_BL2 related information if it exists */
-    {
-	    .image_id = SCP_BL2_IMAGE_ID,
+	{
+		.image_id = SCP_BL2_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
-	    .image_info.image_base = SCP_BL2_BASE,
-	    .image_info.image_max_size = SCP_BL2_SIZE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
+		.image_info.image_base = SCP_BL2_BASE,
+		.image_info.image_max_size = SCP_BL2_SIZE,
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 #endif /* SCP_BL2_BASE */
 
 #ifdef EL3_PAYLOAD_BASE
 	/* Fill EL3 payload related information (BL31 is EL3 payload)*/
-    {
-	    .image_id = BL31_IMAGE_ID,
+	{
+		.image_id = BL31_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t,
-		    SECURE | EXECUTABLE | EP_FIRST_EXE),
-	    .ep_info.pc = EL3_PAYLOAD_BASE,
-	    .ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
-		    DISABLE_ALL_EXCEPTIONS),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = EL3_PAYLOAD_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+			DISABLE_ALL_EXCEPTIONS),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t,
-		    IMAGE_ATTRIB_PLAT_SETUP | IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t,
+			IMAGE_ATTRIB_PLAT_SETUP | IMAGE_ATTRIB_SKIP_LOADING),
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 
 #else /* EL3_PAYLOAD_BASE */
 
 	/* Fill BL31 related information */
-    {
-	    .image_id = BL31_IMAGE_ID,
+	{
+		.image_id = BL31_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t,
-		    SECURE | EXECUTABLE | EP_FIRST_EXE),
-	    .ep_info.pc = BL31_BASE,
-	    .ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
-		    DISABLE_ALL_EXCEPTIONS),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = BL31_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+			DISABLE_ALL_EXCEPTIONS),
 #if DEBUG
-	    .ep_info.args.arg1 = HIKEY_BL31_PLAT_PARAM_VAL,
+		.ep_info.args.arg1 = HIKEY_BL31_PLAT_PARAM_VAL,
 #endif
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
-	    .image_info.image_base = BL31_BASE,
-	    .image_info.image_max_size = BL31_LIMIT - BL31_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
+		.image_info.image_base = BL31_BASE,
+		.image_info.image_max_size = BL31_LIMIT - BL31_BASE,
 
 # ifdef BL32_BASE
-	    .next_handoff_image_id = BL32_IMAGE_ID,
+		.next_handoff_image_id = BL32_IMAGE_ID,
 # else
-	    .next_handoff_image_id = BL33_IMAGE_ID,
+		.next_handoff_image_id = BL33_IMAGE_ID,
 # endif
-    },
+	},
 
 # ifdef BL32_BASE
 	/* Fill BL32 related information */
-    {
-	    .image_id = BL32_IMAGE_ID,
+	{
+		.image_id = BL32_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, SECURE | EXECUTABLE),
-	    .ep_info.pc = BL32_BASE,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | EXECUTABLE),
+		.ep_info.pc = BL32_BASE,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = BL32_BASE,
-	    .image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = BL32_BASE,
+		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
 
-	    .next_handoff_image_id = BL33_IMAGE_ID,
-    },
+		.next_handoff_image_id = BL33_IMAGE_ID,
+	},
 
 	/*
 	 * Fill BL32 external 1 related information.
-	 * A typical use for extra1 image is with OP-TEE where it is the pager image.
+	 * A typical use for extra1 image is with OP-TEE where it is the pager
+	 * image.
 	 */
-    {
-	    .image_id = BL32_EXTRA1_IMAGE_ID,
+	{
+		.image_id = BL32_EXTRA1_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
-	    .image_info.image_base = BL32_BASE,
-	    .image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		.image_info.image_base = BL32_BASE,
+		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 
 	/*
 	 * Fill BL32 external 2 related information.
-	 * A typical use for extra2 image is with OP-TEE where it is the paged image.
+	 * A typical use for extra2 image is with OP-TEE where it is the paged
+	 * image.
 	 */
-    {
-	    .image_id = BL32_EXTRA2_IMAGE_ID,
+	{
+		.image_id = BL32_EXTRA2_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
 #ifdef SPD_opteed
-	    .image_info.image_base = HIKEY_OPTEE_PAGEABLE_LOAD_BASE,
-	    .image_info.image_max_size = HIKEY_OPTEE_PAGEABLE_LOAD_SIZE,
+		.image_info.image_base = HIKEY_OPTEE_PAGEABLE_LOAD_BASE,
+		.image_info.image_max_size = HIKEY_OPTEE_PAGEABLE_LOAD_SIZE,
 #endif
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 # endif /* BL32_BASE */
 
 	/* Fill BL33 related information */
-    {
-	    .image_id = BL33_IMAGE_ID,
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, NON_SECURE | EXECUTABLE),
+	{
+		.image_id = BL33_IMAGE_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, NON_SECURE | EXECUTABLE),
 # ifdef PRELOADED_BL33_BASE
-	    .ep_info.pc = PRELOADED_BL33_BASE,
+		.ep_info.pc = PRELOADED_BL33_BASE,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
 # else
-	    .ep_info.pc = HIKEY_NS_IMAGE_OFFSET,
+		.ep_info.pc = HIKEY_NS_IMAGE_OFFSET,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = HIKEY_NS_IMAGE_OFFSET,
-	    .image_info.image_max_size = 0x200000 /* 2MB */,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = HIKEY_NS_IMAGE_OFFSET,
+		.image_info.image_max_size = 0x200000 /* 2MB */,
 # endif /* PRELOADED_BL33_BASE */
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    }
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	}
 #endif /* EL3_PAYLOAD_BASE */
 };
 
diff --git a/plat/hisilicon/hikey960/hikey960_bl2_mem_params_desc.c b/plat/hisilicon/hikey960/hikey960_bl2_mem_params_desc.c
index ba236d2..39a54cb 100644
--- a/plat/hisilicon/hikey960/hikey960_bl2_mem_params_desc.c
+++ b/plat/hisilicon/hikey960/hikey960_bl2_mem_params_desc.c
@@ -21,144 +21,146 @@
 static bl_mem_params_node_t bl2_mem_params_descs[] = {
 #ifdef SCP_BL2_BASE
 	/* Fill SCP_BL2 related information if it exists */
-    {
-	    .image_id = SCP_BL2_IMAGE_ID,
+	{
+		.image_id = SCP_BL2_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
-	    .image_info.image_base = SCP_BL2_BASE,
-	    .image_info.image_max_size = SCP_BL2_SIZE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
+		.image_info.image_base = SCP_BL2_BASE,
+		.image_info.image_max_size = SCP_BL2_SIZE,
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 #endif /* SCP_BL2_BASE */
 
 #ifdef EL3_PAYLOAD_BASE
 	/* Fill EL3 payload related information (BL31 is EL3 payload)*/
-    {
-	    .image_id = BL31_IMAGE_ID,
+	{
+		.image_id = BL31_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t,
-		    SECURE | EXECUTABLE | EP_FIRST_EXE),
-	    .ep_info.pc = EL3_PAYLOAD_BASE,
-	    .ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
-		    DISABLE_ALL_EXCEPTIONS),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = EL3_PAYLOAD_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+			DISABLE_ALL_EXCEPTIONS),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t,
-		    IMAGE_ATTRIB_PLAT_SETUP | IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t,
+			IMAGE_ATTRIB_PLAT_SETUP | IMAGE_ATTRIB_SKIP_LOADING),
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 
 #else /* EL3_PAYLOAD_BASE */
 
 	/* Fill BL31 related information */
-    {
-	    .image_id = BL31_IMAGE_ID,
+	{
+		.image_id = BL31_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t,
-		    SECURE | EXECUTABLE | EP_FIRST_EXE),
-	    .ep_info.pc = BL31_BASE,
-	    .ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
-		    DISABLE_ALL_EXCEPTIONS),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = BL31_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+			DISABLE_ALL_EXCEPTIONS),
 #if DEBUG
-	    .ep_info.args.arg1 = HIKEY960_BL31_PLAT_PARAM_VAL,
+		.ep_info.args.arg1 = HIKEY960_BL31_PLAT_PARAM_VAL,
 #endif
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
-	    .image_info.image_base = BL31_BASE,
-	    .image_info.image_max_size = BL31_LIMIT - BL31_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
+		.image_info.image_base = BL31_BASE,
+		.image_info.image_max_size = BL31_LIMIT - BL31_BASE,
 
 # ifdef BL32_BASE
-	    .next_handoff_image_id = BL32_IMAGE_ID,
+		.next_handoff_image_id = BL32_IMAGE_ID,
 # else
-	    .next_handoff_image_id = BL33_IMAGE_ID,
+		.next_handoff_image_id = BL33_IMAGE_ID,
 # endif
-    },
+	},
 
 # ifdef BL32_BASE
 	/* Fill BL32 related information */
-    {
-	    .image_id = BL32_IMAGE_ID,
+	{
+		.image_id = BL32_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, SECURE | EXECUTABLE),
-	    .ep_info.pc = BL32_BASE,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | EXECUTABLE),
+		.ep_info.pc = BL32_BASE,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = BL32_BASE,
-	    .image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = BL32_BASE,
+		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
 
-	    .next_handoff_image_id = BL33_IMAGE_ID,
-    },
+		.next_handoff_image_id = BL33_IMAGE_ID,
+	},
 
 	/*
 	 * Fill BL32 external 1 related information.
-	 * A typical use for extra1 image is with OP-TEE where it is the pager image.
+	 * A typical use for extra1 image is with OP-TEE where it is the pager
+	 * image.
 	 */
-    {
-	    .image_id = BL32_EXTRA1_IMAGE_ID,
+	{
+		.image_id = BL32_EXTRA1_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
-	    .image_info.image_base = BL32_BASE,
-	    .image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		.image_info.image_base = BL32_BASE,
+		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 
 	/*
 	 * Fill BL32 external 2 related information.
-	 * A typical use for extra2 image is with OP-TEE where it is the paged image.
+	 * A typical use for extra2 image is with OP-TEE where it is the paged
+	 * image.
 	 */
-    {
-	    .image_id = BL32_EXTRA2_IMAGE_ID,
+	{
+		.image_id = BL32_EXTRA2_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
 #ifdef SPD_opteed
-	    .image_info.image_base = HIKEY960_OPTEE_PAGEABLE_LOAD_BASE,
-	    .image_info.image_max_size = HIKEY960_OPTEE_PAGEABLE_LOAD_SIZE,
+		.image_info.image_base = HIKEY960_OPTEE_PAGEABLE_LOAD_BASE,
+		.image_info.image_max_size = HIKEY960_OPTEE_PAGEABLE_LOAD_SIZE,
 #endif
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 # endif /* BL32_BASE */
 
 	/* Fill BL33 related information */
-    {
-	    .image_id = BL33_IMAGE_ID,
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, NON_SECURE | EXECUTABLE),
+	{
+		.image_id = BL33_IMAGE_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, NON_SECURE | EXECUTABLE),
 # ifdef PRELOADED_BL33_BASE
-	    .ep_info.pc = PRELOADED_BL33_BASE,
+		.ep_info.pc = PRELOADED_BL33_BASE,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
 # else
-	    .ep_info.pc = NS_BL1U_BASE,
+		.ep_info.pc = NS_BL1U_BASE,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = NS_BL1U_BASE,
-	    .image_info.image_max_size = 0x200000 /* 2MB */,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = NS_BL1U_BASE,
+		.image_info.image_max_size = 0x200000 /* 2MB */,
 # endif /* PRELOADED_BL33_BASE */
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    }
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	}
 #endif /* EL3_PAYLOAD_BASE */
 };
 
diff --git a/plat/hisilicon/poplar/bl2_plat_mem_params_desc.c b/plat/hisilicon/poplar/bl2_plat_mem_params_desc.c
index f683d75..9bda02e 100644
--- a/plat/hisilicon/poplar/bl2_plat_mem_params_desc.c
+++ b/plat/hisilicon/poplar/bl2_plat_mem_params_desc.c
@@ -22,143 +22,145 @@
 #ifdef SCP_BL2_BASE
 	/* Fill SCP_BL2 related information if it exists */
 	{
-	    .image_id = SCP_BL2_IMAGE_ID,
+		.image_id = SCP_BL2_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = SCP_BL2_BASE,
-	    .image_info.image_max_size = SCP_BL2_SIZE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = SCP_BL2_BASE,
+		.image_info.image_max_size = SCP_BL2_SIZE,
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
+		.next_handoff_image_id = INVALID_IMAGE_ID,
 	},
 #endif /* SCP_BL2_BASE */
 
 #ifdef EL3_PAYLOAD_BASE
 	/* Fill EL3 payload related information (BL31 is EL3 payload)*/
 	{
-	    .image_id = BL31_IMAGE_ID,
+		.image_id = BL31_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t,
-		    SECURE | EXECUTABLE | EP_FIRST_EXE),
-	    .ep_info.pc = EL3_PAYLOAD_BASE,
-	    .ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
-		    DISABLE_ALL_EXCEPTIONS),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = EL3_PAYLOAD_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+			DISABLE_ALL_EXCEPTIONS),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t,
-		    IMAGE_ATTRIB_PLAT_SETUP | IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t,
+			IMAGE_ATTRIB_PLAT_SETUP | IMAGE_ATTRIB_SKIP_LOADING),
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
+		.next_handoff_image_id = INVALID_IMAGE_ID,
 	},
 
 #else /* EL3_PAYLOAD_BASE */
 
 	/* Fill BL31 related information */
 	{
-	    .image_id = BL31_IMAGE_ID,
+		.image_id = BL31_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t,
-		    SECURE | EXECUTABLE | EP_FIRST_EXE),
-	    .ep_info.pc = BL31_BASE,
-	    .ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
-		    DISABLE_ALL_EXCEPTIONS),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = BL31_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+			DISABLE_ALL_EXCEPTIONS),
 #if DEBUG
-	    .ep_info.args.arg1 = POPLAR_BL31_PLAT_PARAM_VAL,
+		.ep_info.args.arg1 = POPLAR_BL31_PLAT_PARAM_VAL,
 #endif
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
-	    .image_info.image_base = BL31_BASE,
-	    .image_info.image_max_size = BL31_LIMIT - BL31_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
+		.image_info.image_base = BL31_BASE,
+		.image_info.image_max_size = BL31_LIMIT - BL31_BASE,
 
 # ifdef BL32_BASE
-	    .next_handoff_image_id = BL32_IMAGE_ID,
+		.next_handoff_image_id = BL32_IMAGE_ID,
 # else
-	    .next_handoff_image_id = BL33_IMAGE_ID,
+		.next_handoff_image_id = BL33_IMAGE_ID,
 # endif
 	},
 
 # ifdef BL32_BASE
 	/* Fill BL32 related information */
 	{
-	    .image_id = BL32_IMAGE_ID,
+		.image_id = BL32_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, SECURE | EXECUTABLE),
-	    .ep_info.pc = BL32_BASE,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | EXECUTABLE),
+		.ep_info.pc = BL32_BASE,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = BL32_BASE,
-	    .image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = BL32_BASE,
+		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
 
-	    .next_handoff_image_id = BL33_IMAGE_ID,
+		.next_handoff_image_id = BL33_IMAGE_ID,
 	},
 
 	/*
 	 * Fill BL32 external 1 related information.
-	 * A typical use for extra1 image is with OP-TEE where it is the pager image.
+	 * A typical use for extra1 image is with OP-TEE where it is the pager
+	 * image.
 	 */
 	{
-	    .image_id = BL32_EXTRA1_IMAGE_ID,
+		.image_id = BL32_EXTRA1_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
-	    .image_info.image_base = BL32_BASE,
-	    .image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		.image_info.image_base = BL32_BASE,
+		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
+		.next_handoff_image_id = INVALID_IMAGE_ID,
 	},
 
 	/*
 	 * Fill BL32 external 2 related information.
-	 * A typical use for extra2 image is with OP-TEE where it is the paged image.
+	 * A typical use for extra2 image is with OP-TEE where it is the paged
+	 * image.
 	 */
 	{
-	    .image_id = BL32_EXTRA2_IMAGE_ID,
+		.image_id = BL32_EXTRA2_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
 #ifdef SPD_opteed
-	    .image_info.image_base = POPLAR_OPTEE_PAGEABLE_LOAD_BASE,
-	    .image_info.image_max_size = POPLAR_OPTEE_PAGEABLE_LOAD_SIZE,
+		.image_info.image_base = POPLAR_OPTEE_PAGEABLE_LOAD_BASE,
+		.image_info.image_max_size = POPLAR_OPTEE_PAGEABLE_LOAD_SIZE,
 #endif
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
+		.next_handoff_image_id = INVALID_IMAGE_ID,
 	},
 # endif /* BL32_BASE */
 
 	/* Fill BL33 related information */
 	{
-	    .image_id = BL33_IMAGE_ID,
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, NON_SECURE | EXECUTABLE),
+		.image_id = BL33_IMAGE_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, NON_SECURE | EXECUTABLE),
 # ifdef PRELOADED_BL33_BASE
-	    .ep_info.pc = PRELOADED_BL33_BASE,
+		.ep_info.pc = PRELOADED_BL33_BASE,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
 # else
-	    .ep_info.pc = PLAT_POPLAR_NS_IMAGE_OFFSET,
+		.ep_info.pc = PLAT_POPLAR_NS_IMAGE_OFFSET,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = PLAT_POPLAR_NS_IMAGE_OFFSET,
-	    .image_info.image_max_size = DDR_BASE + DDR_SIZE -
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = PLAT_POPLAR_NS_IMAGE_OFFSET,
+		.image_info.image_max_size = DDR_BASE + DDR_SIZE -
 					PLAT_POPLAR_NS_IMAGE_OFFSET,
 # endif /* PRELOADED_BL33_BASE */
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
+		.next_handoff_image_id = INVALID_IMAGE_ID,
 	}
 #endif /* EL3_PAYLOAD_BASE */
 };
diff --git a/plat/intel/soc/common/bl2_plat_mem_params_desc.c b/plat/intel/soc/common/bl2_plat_mem_params_desc.c
index 4f75665..187c53a 100644
--- a/plat/intel/soc/common/bl2_plat_mem_params_desc.c
+++ b/plat/intel/soc/common/bl2_plat_mem_params_desc.c
@@ -22,58 +22,58 @@
 #ifdef SCP_BL2_BASE
 	/* Fill SCP_BL2 related information if it exists */
 	{
-	    .image_id = SCP_BL2_IMAGE_ID,
+		.image_id = SCP_BL2_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = SCP_BL2_BASE,
-	    .image_info.image_max_size = SCP_BL2_SIZE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = SCP_BL2_BASE,
+		.image_info.image_max_size = SCP_BL2_SIZE,
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
+		.next_handoff_image_id = INVALID_IMAGE_ID,
 	},
 #endif /* SCP_BL2_BASE */
 
 #ifdef EL3_PAYLOAD_BASE
 	/* Fill EL3 payload related information (BL31 is EL3 payload)*/
 	{
-	    .image_id = BL31_IMAGE_ID,
+		.image_id = BL31_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t,
-		    SECURE | EXECUTABLE | EP_FIRST_EXE),
-	    .ep_info.pc = EL3_PAYLOAD_BASE,
-	    .ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
-		    DISABLE_ALL_EXCEPTIONS),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = EL3_PAYLOAD_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+			DISABLE_ALL_EXCEPTIONS),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t,
-		    IMAGE_ATTRIB_PLAT_SETUP | IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t,
+			IMAGE_ATTRIB_PLAT_SETUP | IMAGE_ATTRIB_SKIP_LOADING),
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
+		.next_handoff_image_id = INVALID_IMAGE_ID,
 	},
 
 #else /* EL3_PAYLOAD_BASE */
 
 	/* Fill BL31 related information */
 	{
-	    .image_id = BL31_IMAGE_ID,
+		.image_id = BL31_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t,
-		    SECURE | EXECUTABLE | EP_FIRST_EXE),
-	    .ep_info.pc = BL31_BASE,
-	    .ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
-		    DISABLE_ALL_EXCEPTIONS),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = BL31_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+			DISABLE_ALL_EXCEPTIONS),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
-	    .image_info.image_base = BL31_BASE,
-	    .image_info.image_max_size = BL31_LIMIT - BL31_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
+		.image_info.image_base = BL31_BASE,
+		.image_info.image_max_size = BL31_LIMIT - BL31_BASE,
 
-	    .next_handoff_image_id = BL33_IMAGE_ID,
+		.next_handoff_image_id = BL33_IMAGE_ID,
 	},
 #endif /* EL3_PAYLOAD_BASE */
 
diff --git a/plat/marvell/armada/common/aarch64/marvell_bl2_mem_params_desc.c b/plat/marvell/armada/common/aarch64/marvell_bl2_mem_params_desc.c
index 8d909dc..6c55858 100644
--- a/plat/marvell/armada/common/aarch64/marvell_bl2_mem_params_desc.c
+++ b/plat/marvell/armada/common/aarch64/marvell_bl2_mem_params_desc.c
@@ -22,84 +22,84 @@
 static bl_mem_params_node_t bl2_mem_params_descs[] = {
 #ifdef SCP_BL2_BASE
 	/* Fill SCP_BL2 related information if it exists */
-    {
-	    .image_id = SCP_BL2_IMAGE_ID,
+	{
+		.image_id = SCP_BL2_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = SCP_BL2_BASE,
-	    .image_info.image_max_size = SCP_BL2_SIZE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = SCP_BL2_BASE,
+		.image_info.image_max_size = SCP_BL2_SIZE,
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 #endif /* SCP_BL2_BASE */
 
 #ifdef EL3_PAYLOAD_BASE
 	/* Fill EL3 payload related information (BL31 is EL3 payload)*/
-    {
-	    .image_id = BL31_IMAGE_ID,
+	{
+		.image_id = BL31_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t,
-		    SECURE | EXECUTABLE | EP_FIRST_EXE),
-	    .ep_info.pc = EL3_PAYLOAD_BASE,
-	    .ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
-		    DISABLE_ALL_EXCEPTIONS),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = EL3_PAYLOAD_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+			DISABLE_ALL_EXCEPTIONS),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t,
-		    IMAGE_ATTRIB_PLAT_SETUP | IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t,
+			IMAGE_ATTRIB_PLAT_SETUP | IMAGE_ATTRIB_SKIP_LOADING),
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    },
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 
 #else /* EL3_PAYLOAD_BASE */
 
 	/* Fill BL31 related information */
-    {
-	    .image_id = BL31_IMAGE_ID,
+	{
+		.image_id = BL31_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t,
-		    SECURE | EXECUTABLE | EP_FIRST_EXE),
-	    .ep_info.pc = BL31_BASE,
-	    .ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
-		    DISABLE_ALL_EXCEPTIONS),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = BL31_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+			DISABLE_ALL_EXCEPTIONS),
 #if DEBUG
-	    .ep_info.args.arg3 = MARVELL_BL31_PLAT_PARAM_VAL,
+		.ep_info.args.arg3 = MARVELL_BL31_PLAT_PARAM_VAL,
 #endif
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
-	    .image_info.image_base = BL31_BASE,
-	    .image_info.image_max_size = BL31_LIMIT - BL31_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
+		.image_info.image_base = BL31_BASE,
+		.image_info.image_max_size = BL31_LIMIT - BL31_BASE,
 
 # ifdef BL32_BASE
-	    .next_handoff_image_id = BL32_IMAGE_ID,
+		.next_handoff_image_id = BL32_IMAGE_ID,
 # else
-	    .next_handoff_image_id = BL33_IMAGE_ID,
+		.next_handoff_image_id = BL33_IMAGE_ID,
 # endif
-    },
+	},
 
 # ifdef BL32_BASE
 	/* Fill BL32 related information */
-    {
-	    .image_id = BL32_IMAGE_ID,
+	{
+		.image_id = BL32_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, SECURE | EXECUTABLE),
-	    .ep_info.pc = BL32_BASE,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | EXECUTABLE),
+		.ep_info.pc = BL32_BASE,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = BL32_BASE,
-	    .image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = BL32_BASE,
+		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
 
-	    .next_handoff_image_id = BL33_IMAGE_ID,
-    },
+		.next_handoff_image_id = BL33_IMAGE_ID,
+	},
 
 	/*
 	 * Fill BL32 external 1 related information.
@@ -107,17 +107,17 @@
 	 * where it is the pager image.
 	 */
 	{
-	    .image_id = BL32_EXTRA1_IMAGE_ID,
+		.image_id = BL32_EXTRA1_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-	    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+		VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
-	    .image_info.image_base = BL32_BASE,
-	    .image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		.image_info.image_base = BL32_BASE,
+		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
+		.next_handoff_image_id = INVALID_IMAGE_ID,
 	},
 
 	/*
@@ -126,42 +126,42 @@
 	 * where it is the paged image.
 	 */
 	{
-	    .image_id = BL32_EXTRA2_IMAGE_ID,
+		.image_id = BL32_EXTRA2_IMAGE_ID,
 
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
 #ifdef SPD_opteed
-	    .image_info.image_base = MARVELL_OPTEE_PAGEABLE_LOAD_BASE,
-	    .image_info.image_max_size = MARVELL_OPTEE_PAGEABLE_LOAD_SIZE,
+		.image_info.image_base = MARVELL_OPTEE_PAGEABLE_LOAD_BASE,
+		.image_info.image_max_size = MARVELL_OPTEE_PAGEABLE_LOAD_SIZE,
 #endif
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
+		.next_handoff_image_id = INVALID_IMAGE_ID,
 	},
 # endif /* BL32_BASE */
 
 	/* Fill BL33 related information */
-    {
-	    .image_id = BL33_IMAGE_ID,
-	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
-		    VERSION_2, entry_point_info_t, NON_SECURE | EXECUTABLE),
+	{
+		.image_id = BL33_IMAGE_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, NON_SECURE | EXECUTABLE),
 # ifdef PRELOADED_BL33_BASE
-	    .ep_info.pc = PRELOADED_BL33_BASE,
+		.ep_info.pc = PRELOADED_BL33_BASE,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
 # else
-	    .ep_info.pc = MARVELL_DRAM_BASE,
+		.ep_info.pc = MARVELL_DRAM_BASE,
 
-	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
-		    VERSION_2, image_info_t, 0),
-	    .image_info.image_base = MARVELL_DRAM_BASE,
-	    .image_info.image_max_size = MARVELL_DRAM_SIZE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = MARVELL_DRAM_BASE,
+		.image_info.image_max_size = MARVELL_DRAM_SIZE,
 # endif /* PRELOADED_BL33_BASE */
 
-	    .next_handoff_image_id = INVALID_IMAGE_ID,
-    }
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	}
 #endif /* EL3_PAYLOAD_BASE */
 };
 
diff --git a/plat/mediatek/build_helpers/mtk_build_helpers.mk b/plat/mediatek/build_helpers/mtk_build_helpers.mk
index 47d96fa..fc3876e 100644
--- a/plat/mediatek/build_helpers/mtk_build_helpers.mk
+++ b/plat/mediatek/build_helpers/mtk_build_helpers.mk
@@ -116,20 +116,23 @@
 # Include MTK configuration files
 
 # MTK makefile variables
+ifeq (${COREBOOT},1)
+MTK_COMMON_CFG := $(MTK_PLAT)/common/coreboot_config.mk
+else
+MTK_COMMON_CFG := $(MTK_PLAT)/common/common_config.mk
+endif
 MTK_PLAT      := plat/mediatek
 MTK_PLAT_SOC  := ${MTK_PLAT}/${MTK_SOC}
-MTK_COMMON_CFG := $(MTK_PLAT)/common/common_config.mk
 MTK_PLAT_CFG := $(MTK_PLAT_SOC)/plat_config.mk
 MTK_PROJECT_CFG := $(MTK_PLAT)/project/$(PLAT)/project_config.mk
 MTK_OPTIONS := $(MTK_PLAT)/build_helpers/options.mk
 MTK_COND_EVAL := $(MTK_PLAT)/build_helpers/conditional_eval_options.mk
 
 # Indicate which BL should be built in command line
-ifeq (${NEED_BL31},yes)
-MTK_BL := bl31
-endif
 ifeq (${NEED_BL32},yes)
 MTK_BL := bl32
+else
+MTK_BL := bl31
 endif
 # Include common, platform, board level config
 include $(MTK_COMMON_CFG)
diff --git a/plat/mediatek/build_helpers/options.mk b/plat/mediatek/build_helpers/options.mk
index 394a605..eb579e5 100644
--- a/plat/mediatek/build_helpers/options.mk
+++ b/plat/mediatek/build_helpers/options.mk
@@ -6,7 +6,7 @@
 
 # call add_defined_option to evaluate MTK defined value
 $(eval $(call add_defined_option,MTK_SIP_KERNEL_BOOT_ENABLE))
-$(eval $(call add_defined_option,PLAT_EXTRA_LD_SCRIPT))
+$(eval $(call add_defined_option,PLAT_EXTRA_RODATA_INCLUDES))
 $(eval $(call add_defined_option,MTK_EXTRA_LINKERFILE))
 $(eval $(call add_defined_option,MTK_BL31_AS_BL2))
 $(eval $(call add_defined_option,MTK_BL33_IS_64BIT))
diff --git a/plat/mediatek/common/cold_boot.c b/plat/mediatek/common/cold_boot.c
new file mode 100644
index 0000000..ff585fe
--- /dev/null
+++ b/plat/mediatek/common/cold_boot.c
@@ -0,0 +1,336 @@
+/*
+ * Copyright (c) 2022, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <inttypes.h>
+#include <stdint.h>
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <lib/el3_runtime/context_mgmt.h>
+
+/* Vendors headers */
+#include <cold_boot.h>
+#include <lib/mtk_init/mtk_init.h>
+#include <mtk_sip_svc.h>
+
+static struct kernel_info k_info;
+static entry_point_info_t bl32_image_ep_info;
+static entry_point_info_t bl33_image_ep_info;
+static bool el1_is_2nd_bootloader = true;
+static struct atf_arg_t atfarg;
+
+static int init_mtk_bl32_arg(void)
+{
+	struct mtk_bl_param_t *p_mtk_bl_param;
+	struct atf_arg_t *p_atfarg;
+
+	p_mtk_bl_param = (struct mtk_bl_param_t *) get_mtk_bl31_fw_config(BOOT_ARG_FROM_BL2);
+	if (p_mtk_bl_param == NULL) {
+		ERROR("p_mtk_bl_param is NULL!\n");
+		return -1;
+	}
+	p_atfarg = (struct atf_arg_t *)p_mtk_bl_param->atf_arg_addr;
+	if (p_atfarg == NULL) {
+		ERROR("bl32 argument is NULL!\n");
+		return -1;
+	}
+	memcpy((void *)&atfarg, (void *)p_atfarg, sizeof(struct atf_arg_t));
+	return 0;
+}
+MTK_EARLY_PLAT_INIT(init_mtk_bl32_arg);
+
+static void save_kernel_info(uint64_t pc, uint64_t r0, uint64_t r1, uint64_t k32_64)
+{
+	k_info.k32_64 = k32_64;
+	k_info.pc = pc;
+
+	if (k32_64 == LINUX_KERNEL_32) {
+		/* for 32 bits kernel */
+		k_info.r0 = 0;
+		/* machtype */
+		k_info.r1 = r0;
+		/* tags */
+		k_info.r2 = r1;
+	} else {
+		/* for 64 bits kernel */
+		k_info.r0 = r0;
+		k_info.r1 = r1;
+	}
+}
+
+static uint32_t plat_get_spsr_for_bl32_64_entry(void)
+{
+	return SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+}
+
+#if MTK_BL33_IS_64BIT
+static uint32_t plat_get_spsr_for_bl33_entry(void)
+{
+	uint32_t spsr;
+	uint32_t mode;
+
+	mode = MODE_EL1;
+	spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+	return spsr;
+}
+#else
+static uint32_t plat_get_spsr_for_bl33_entry(void)
+{
+	unsigned int mode;
+	uint32_t spsr;
+	unsigned int ee;
+	unsigned long daif;
+
+	INFO("Secondary bootloader is AArch32\n");
+	mode = MODE32_svc;
+	ee = 0;
+	/*
+	 * TODO: Choose async. exception bits if HYP mode is not
+	 * implemented according to the values of SCR.{AW, FW} bits
+	 */
+	daif = DAIF_ABT_BIT | DAIF_IRQ_BIT | DAIF_FIQ_BIT;
+
+	spsr = SPSR_MODE32(mode, 0, ee, daif);
+	return spsr;
+}
+#endif
+
+static void populate_bl32_image_ep(entry_point_info_t *bl32_ep_instance,
+		struct mtk_bl_param_t *p_mtk_bl_param)
+{
+	entry_point_info_t *populated_ep_bl32 = bl32_ep_instance;
+
+	if (p_mtk_bl_param == NULL) {
+		ERROR("p_mtk_bl_param is NULL!\n");
+		panic();
+	}
+	SET_SECURITY_STATE(bl32_ep_instance->h.attr, SECURE);
+	SET_PARAM_HEAD(populated_ep_bl32,
+		       PARAM_EP,
+		       VERSION_1,
+		       populated_ep_bl32->h.attr);
+	populated_ep_bl32->pc = atfarg.tee_entry;
+	populated_ep_bl32->spsr = plat_get_spsr_for_bl32_64_entry();
+}
+
+static void populate_bl33_image_ep(entry_point_info_t *bl33_ep_instance,
+		struct mtk_bl_param_t *p_mtk_bl_param)
+{
+	entry_point_info_t *populated_ep_bl33 = bl33_ep_instance;
+
+	if (p_mtk_bl_param == NULL) {
+		ERROR("p_mtk_bl_param is NULL!\n");
+		panic();
+	}
+	SET_SECURITY_STATE(bl33_ep_instance->h.attr, NON_SECURE);
+	SET_PARAM_HEAD(populated_ep_bl33,
+		       PARAM_EP,
+		       VERSION_1,
+		       populated_ep_bl33->h.attr);
+	populated_ep_bl33->pc = p_mtk_bl_param->bl33_start_addr;
+	/* standardize 2nd bootloader input argument */
+	populated_ep_bl33->args.arg0 = p_mtk_bl_param->bootarg_loc;
+	/* compatible to old GZ version */
+	populated_ep_bl33->args.arg4 = p_mtk_bl_param->bootarg_loc;
+	populated_ep_bl33->args.arg5 = p_mtk_bl_param->bootarg_size;
+	populated_ep_bl33->spsr = plat_get_spsr_for_bl33_entry();
+}
+
+static int populate_bl_images_ep(struct mtk_bl_param_t *p_mtk_bl_param)
+{
+	/*
+	 * Tell BL31 where the non-trusted software image
+	 * is located and the entry state information
+	 */
+	populate_bl33_image_ep(&bl33_image_ep_info, p_mtk_bl_param);
+	populate_bl32_image_ep(&bl32_image_ep_info, p_mtk_bl_param);
+	return 0;
+}
+
+static int populate_bl_images_ep_init(void)
+{
+	return populate_bl_images_ep(get_mtk_bl31_fw_config(BOOT_ARG_FROM_BL2));
+}
+MTK_PLAT_SETUP_0_INIT(populate_bl_images_ep_init);
+
+static entry_point_info_t *bl31_plat_get_next_kernel64_ep_info(void)
+{
+	entry_point_info_t *next_image_info;
+	unsigned long el_status;
+	unsigned int mode;
+
+	el_status = 0;
+	mode = 0;
+
+	/* Kernel image is always non-secured */
+	next_image_info = &bl33_image_ep_info;
+
+	/* Figure out what mode we enter the non-secure world in */
+	el_status = read_id_aa64pfr0_el1() >> ID_AA64PFR0_EL2_SHIFT;
+	el_status &= ID_AA64PFR0_ELX_MASK;
+
+	INFO("Kernel_EL %d\n", el_status?2:1);
+	if (el_status) {
+		mode = MODE_EL2;
+	} else {
+		mode = MODE_EL1;
+	}
+	INFO("Kernel is 64Bit\n");
+	next_image_info->spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+	next_image_info->pc = k_info.pc;
+	next_image_info->args.arg0 = k_info.r0;
+	next_image_info->args.arg1 = k_info.r1;
+
+	INFO("pc=0x%lx, r0=0x%" PRIx64 ", r1=0x%" PRIx64 "\n",
+	     next_image_info->pc,
+	     next_image_info->args.arg0,
+	     next_image_info->args.arg1);
+
+	SET_SECURITY_STATE(next_image_info->h.attr, NON_SECURE);
+
+	/* None of the images on this platform can have 0x0 as the entrypoint */
+	if (next_image_info->pc) {
+		return next_image_info;
+	}
+
+	return NULL;
+}
+
+static entry_point_info_t *bl31_plat_get_next_kernel32_ep_info(void)
+{
+	entry_point_info_t *next_image_info;
+	unsigned int mode;
+
+	mode = 0;
+
+	/* Kernel image is always non-secured */
+	next_image_info = &bl33_image_ep_info;
+
+	/* Figure out what mode we enter the non-secure world in */
+	mode = MODE32_hyp;
+	/*
+	 * TODO: Consider the possibility of specifying the SPSR in
+	 * the FIP ToC and allowing the platform to have a say as
+	 * well.
+	 */
+
+	INFO("Kernel is 32Bit\n");
+	next_image_info->spsr = SPSR_MODE32(mode, SPSR_T_ARM, SPSR_E_LITTLE,
+					    (DAIF_FIQ_BIT | DAIF_IRQ_BIT | DAIF_ABT_BIT));
+	next_image_info->pc = k_info.pc;
+	next_image_info->args.arg0 = k_info.r0;
+	next_image_info->args.arg1 = k_info.r1;
+	next_image_info->args.arg2 = k_info.r2;
+
+	INFO("pc=0x%lx, r0=0x%" PRIx64 ", r1=0x%" PRIx64 ", r2=0x%" PRIx64 "\n",
+	     next_image_info->pc,
+	     next_image_info->args.arg0,
+	     next_image_info->args.arg1,
+	     next_image_info->args.arg2);
+
+	SET_SECURITY_STATE(next_image_info->h.attr, NON_SECURE);
+
+	/* None of the images on this platform can have 0x0 as the entrypoint */
+	if (next_image_info->pc) {
+		return next_image_info;
+	}
+
+	return NULL;
+}
+
+static void bl31_prepare_kernel_entry(uint64_t k32_64)
+{
+	entry_point_info_t *next_image_info = NULL;
+	uint32_t image_type;
+
+	/* Determine which image to execute next */
+	image_type = NON_SECURE; /* bl31_get_next_image_type(); */
+
+	/* Leave 2nd bootloader then jump to kernel */
+	el1_is_2nd_bootloader = false;
+
+	/* Program EL3 registers to enable entry into the next EL */
+	if (k32_64 == LINUX_KERNEL_32) {
+		next_image_info = bl31_plat_get_next_kernel32_ep_info();
+	} else {
+		next_image_info = bl31_plat_get_next_kernel64_ep_info();
+	}
+
+	assert(next_image_info);
+	assert(image_type == GET_SECURITY_STATE(next_image_info->h.attr));
+
+	INFO("BL31: Preparing for EL3 exit to %s world, Kernel\n",
+	     (image_type == SECURE) ? "secure" : "normal");
+	INFO("BL31: Next image address = 0x%" PRIx64 "\n",
+	     next_image_info->pc);
+	INFO("BL31: Next image spsr = 0x%x\n", next_image_info->spsr);
+	cm_init_my_context(next_image_info);
+	cm_prepare_el3_exit(image_type);
+}
+
+bool is_el1_2nd_bootloader(void)
+{
+	return el1_is_2nd_bootloader;
+}
+
+/*******************************************************************************
+ * Return a pointer to the 'entry_point_info' structure of the next image for
+ * the security state specified. BL33 corresponds to the non-secure image type
+ * while BL32 corresponds to the secure image type. A NULL pointer is returned
+ * if the image does not exist.
+ ******************************************************************************/
+entry_point_info_t *bl31_plat_get_next_image_ep_info(uint32_t type)
+{
+	entry_point_info_t *next_image_info;
+
+	next_image_info = (type == NON_SECURE) ? &bl33_image_ep_info : &bl32_image_ep_info;
+
+	/* None of the images on this platform can have 0x0 as the entrypoint */
+	if (next_image_info->pc) {
+		return next_image_info;
+	}
+	return NULL;
+}
+
+u_register_t boot_to_kernel(u_register_t x1,
+			    u_register_t x2,
+			    u_register_t x3,
+			    u_register_t x4,
+			    void *handle,
+			    struct smccc_res *smccc_ret)
+{
+	static uint8_t kernel_boot_once_flag;
+
+	/* only support in booting flow */
+	if (kernel_boot_once_flag == 0) {
+		kernel_boot_once_flag = 1;
+
+		INFO("save kernel info\n");
+		save_kernel_info(x1, x2, x3, x4);
+		bl31_prepare_kernel_entry(x4);
+		INFO("el3_exit\n");
+		/*
+		 * FIXME: no better way so far to prevent from
+		 * SiP root handler wipe x0~x3 if not assign smccc_ret
+		 * return register
+		 */
+		smccc_ret->a1 = x3;
+
+		mtk_init_one_level(MTK_INIT_LVL_BL33_DEFER);
+
+#if MTK_CONSOLE_RUNTIME_DISABLE
+		INFO("Turn off BL31 console\n");
+		mtk_console_core_end();
+#endif
+
+		/* Re-assign as x0 register entering Linux kernel */
+		return x2;
+	}
+	return 0;
+}
+/* Register SiP SMC service */
+DECLARE_SMC_HANDLER(MTK_SIP_KERNEL_BOOT, boot_to_kernel);
diff --git a/plat/mediatek/common/common_config.mk b/plat/mediatek/common/common_config.mk
new file mode 100644
index 0000000..851eb2c
--- /dev/null
+++ b/plat/mediatek/common/common_config.mk
@@ -0,0 +1,34 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# indicate the reset vector address can be programmed
+PROGRAMMABLE_RESET_ADDRESS := 1
+MULTI_CONSOLE_API := 1
+COLD_BOOT_SINGLE_CPU := 1
+# Build flag to include AArch32 registers in cpu context save and restore during
+# world switch. This flag must be set to 0 for AArch64-only platforms.
+CTX_INCLUDE_AARCH32_REGS := 0
+PLAT_XLAT_TABLES_DYNAMIC := 1
+# enable this definition to print irq dump status in tf-a
+GIC_DEBUG := 0
+# Enable stack protector.
+# Allowed values are "all", "strong", "default" and "none"
+ENABLE_STACK_PROTECTOR := strong
+# AMU, Kernel will access amuserenr_el0 if PE supported
+# Firmware _must_ implement AMU support
+ENABLE_AMU := 1
+VENDOR_EXTEND_PUBEVENT_ENABLE := 1
+
+# MTK define options
+MTK_BL33_IS_64BIT := 0
+MTK_ADAPTED := 1
+
+# MTK module config
+CONFIG_MTK_INTERRUPT := y
+CONFIG_MTK_UART := y
+
+# UART baudrate
+UART_BAUDRATE := 921600
diff --git a/plat/mediatek/common/coreboot_config.mk b/plat/mediatek/common/coreboot_config.mk
new file mode 100644
index 0000000..59d18e8
--- /dev/null
+++ b/plat/mediatek/common/coreboot_config.mk
@@ -0,0 +1,15 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# indicate the reset vector address can be programmed
+PROGRAMMABLE_RESET_ADDRESS := 1
+COLD_BOOT_SINGLE_CPU := 1
+# Build flag to include AArch32 registers in cpu context save and restore during
+# world switch. This flag must be set to 0 for AArch64-only platforms.
+CTX_INCLUDE_AARCH32_REGS := 0
+PLAT_XLAT_TABLES_DYNAMIC := 1
+
+VENDOR_EXTEND_PUBEVENT_ENABLE := 1
diff --git a/plat/mediatek/common/custom/oem_svc.c b/plat/mediatek/common/custom/oem_svc.c
deleted file mode 100644
index 27ee6aa..0000000
--- a/plat/mediatek/common/custom/oem_svc.c
+++ /dev/null
@@ -1,100 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <assert.h>
-#include <stdint.h>
-
-#include <arch.h>
-#include <arch_helpers.h>
-#include <common/debug.h>
-#include <common/runtime_svc.h>
-#include <plat/common/platform.h>
-#include <tools_share/uuid.h>
-
-#include <oem_svc.h>
-
-/* OEM Service UUID */
-DEFINE_SVC_UUID2(oem_svc_uid,
-	0xd0ad43b9, 0x9b06, 0xe411, 0x91, 0x91,
-	0x08, 0x00, 0x20, 0x0c, 0x9a, 0x66);
-
-/* Setup OEM Services */
-static int32_t oem_svc_setup(void)
-{
-	/*
-	 * Invoke related module setup from here
-	 */
-
-	return 0;
-}
-
-/*******************************************************************************
- * OEM top level handler for servicing SMCs.
- ******************************************************************************/
-uintptr_t oem_smc_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)
-{
-	WARN("Unimplemented OEM Call: 0x%x\n", smc_fid);
-	SMC_RET1(handle, SMC_UNK);
-}
-
-/*
- * Top-level OEM Service SMC handler. This handler will in turn dispatch
- * calls to related SMC handler
- */
-uintptr_t oem_svc_smc_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)
-{
-	/*
-	 * Dispatch OEM calls to OEM Common handler and return its return value
-	 */
-	if (is_oem_fid(smc_fid)) {
-		return oem_smc_handler(smc_fid, x1, x2, x3, x4, cookie,
-					handle, flags);
-	}
-
-	switch (smc_fid) {
-	case OEM_SVC_CALL_COUNT:
-		/*
-		 * Return the number of OEM Service Calls.
-		 */
-		SMC_RET1(handle, OEM_SVC_NUM_CALLS);
-
-	case OEM_SVC_UID:
-		/* Return UID to the caller */
-		SMC_UUID_RET(handle, oem_svc_uid);
-
-	case OEM_SVC_VERSION:
-		/* Return the version of current implementation */
-		SMC_RET2(handle, OEM_VERSION_MAJOR, OEM_VERSION_MINOR);
-
-	default:
-		WARN("Unimplemented OEM Service Call: 0x%x\n", smc_fid);
-		SMC_RET1(handle, SMC_UNK);
-	}
-}
-
-/* Register OEM Service Calls as runtime service */
-DECLARE_RT_SVC(
-		oem_svc,
-		OEN_OEM_START,
-		OEN_OEM_END,
-		SMC_TYPE_FAST,
-		oem_svc_setup,
-		oem_svc_smc_handler
-);
diff --git a/plat/mediatek/common/custom/oem_svc.h b/plat/mediatek/common/custom/oem_svc.h
deleted file mode 100644
index 76f7c24..0000000
--- a/plat/mediatek/common/custom/oem_svc.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef OEM_SVC_H
-#define OEM_SVC_H
-
-/*******************************************************************************
- * Defines for runtime services func ids
- ******************************************************************************/
-/*
- * Number of OEM calls (above) implemented.
- */
-#define OEM_SVC_NUM_CALLS		3
-
-/*******************************************************************************
- * Defines for OEM Service queries
- ******************************************************************************/
-/* 0x83000000 - 0x8300FEFF is OEM service calls */
-#define OEM_SVC_CALL_COUNT		0x8300ff00
-#define OEM_SVC_UID			0x8300ff01
-/* 0x8300ff02 is reserved */
-#define OEM_SVC_VERSION			0x8300ff03
-/* 0x8300ff04 - 0x8300FFFF is reserved for future expansion */
-
-/* OEM Service Calls version numbers */
-#define OEM_VERSION_MAJOR		0x0
-#define OEM_VERSION_MINOR		0x1
-
-/* The macros below are used to identify OEM calls from the SMC function ID */
-/* SMC32 ID range from 0x83000000 to 0x83000FFF */
-/* SMC64 ID range from 0xC3000000 to 0xC3000FFF */
-#define OEM_FID_MASK			0xf000u
-#define OEM_FID_VALUE			0u
-#define is_oem_fid(_fid) \
-	(((_fid) & OEM_FID_MASK) == OEM_FID_VALUE)
-
-#define OEM_SVC_E_SUCCESS		0
-#define OEM_SVC_E_NOT_SUPPORTED		-1
-#define OEM_SVC_E_INVALID_PARAMS	-2
-
-#endif /* OEM_SVC_H */
diff --git a/plat/mediatek/common/mtk_bl31_setup.c b/plat/mediatek/common/mtk_bl31_setup.c
new file mode 100644
index 0000000..79ab29d
--- /dev/null
+++ b/plat/mediatek/common/mtk_bl31_setup.c
@@ -0,0 +1,174 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <arch.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/generic_delay_timer.h>
+#if XLAT_TABLES_LIB_V2 && PLAT_XLAT_TABLES_DYNAMIC
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#endif
+#include <plat/common/platform.h>
+
+#if COREBOOT
+#include <common/desc_image_load.h>
+
+#include <drivers/ti/uart/uart_16550.h>
+#include <lib/coreboot.h>
+#include <plat_params.h>
+#endif
+
+/* MTK headers */
+#if MTK_SIP_KERNEL_BOOT_ENABLE
+#include <cold_boot.h>
+#endif
+#include <lib/mtk_init/mtk_init.h>
+#include <mtk_mmap_pool.h>
+
+IMPORT_SYM(uintptr_t, __RW_START__, RW_START);
+IMPORT_SYM(uintptr_t, __DATA_START__, DATA_START);
+
+#if COREBOOT
+static entry_point_info_t bl32_ep_info;
+static entry_point_info_t bl33_ep_info;
+
+/*******************************************************************************
+ * Return a pointer to the 'entry_point_info' structure of the next image for
+ * the security state specified. BL33 corresponds to the non-secure image type
+ * while BL32 corresponds to the secure image type. A NULL pointer is returned
+ * if the image does not exist.
+ ******************************************************************************/
+entry_point_info_t *bl31_plat_get_next_image_ep_info(uint32_t type)
+{
+	entry_point_info_t *next_image_info;
+
+	next_image_info = (type == NON_SECURE) ? &bl33_ep_info : &bl32_ep_info;
+	assert(next_image_info->h.type == PARAM_EP);
+
+	/* None of the images on this platform can have 0x0 as the entrypoint */
+	if (next_image_info->pc) {
+		return next_image_info;
+	} else {
+		return NULL;
+	}
+}
+#else
+#ifndef MTK_BL31_AS_BL2
+static struct mtk_bl31_fw_config bl31_fw_config;
+#else
+struct mtk_bl31_fw_config bl31_fw_config;
+#endif
+/* In order to be accessed after MMU enable */
+static struct mtk_bl_param_t bl_param_clone;
+
+void *get_mtk_bl31_fw_config(int index)
+{
+	void *arg = NULL;
+
+	switch (index) {
+	case BOOT_ARG_FROM_BL2:
+		arg = bl31_fw_config.from_bl2;
+		break;
+	case BOOT_ARG_SOC_FW_CONFIG:
+		arg = bl31_fw_config.soc_fw_config;
+		break;
+	case BOOT_ARG_HW_CONFIG:
+		arg = bl31_fw_config.hw_config;
+		break;
+	case BOOT_ARG_RESERVED:
+		arg = bl31_fw_config.reserved;
+		break;
+	default:
+		WARN("Fail to get boot arg, index:%d", index);
+		break;
+	}
+	return arg;
+}
+#endif
+/*****************************************************************************
+ * Perform the very early platform specific architectural setup shared between
+ * ARM standard platforms. This only does basic initialization. Later
+ * architectural setup (bl31_arch_setup()) does not do anything platform
+ * specific.
+ ******************************************************************************/
+void bl31_early_platform_setup2(u_register_t from_bl2,
+				u_register_t soc_fw_config,
+				u_register_t hw_config, u_register_t plat_params_from_bl2)
+
+{
+#if COREBOOT
+	static console_t console;
+
+	params_early_setup(soc_fw_config);
+	if (coreboot_serial.type) {
+		console_16550_register(coreboot_serial.baseaddr,
+				       coreboot_serial.input_hertz,
+				       coreboot_serial.baud,
+				       &console);
+	}
+	bl31_params_parse_helper(from_bl2, &bl32_ep_info, &bl33_ep_info);
+#else
+	struct mtk_bl_param_t *p_mtk_bl_param = (struct mtk_bl_param_t *)from_bl2;
+
+	if (p_mtk_bl_param == NULL) {
+		ERROR("from_bl2 should not be NULL\n");
+		panic();
+	}
+	memcpy(&bl_param_clone, p_mtk_bl_param, sizeof(struct mtk_bl_param_t));
+	bl31_fw_config.from_bl2 = (void *)&bl_param_clone;
+	bl31_fw_config.soc_fw_config = (void *)soc_fw_config;
+	bl31_fw_config.hw_config = (void *)hw_config;
+	bl31_fw_config.reserved = (void *)plat_params_from_bl2;
+#endif
+
+	INFO("MTK BL31 start\n");
+	/* Init delay function */
+	generic_delay_timer_init();
+	/* Initialize module initcall */
+	mtk_init_one_level(MTK_INIT_LVL_EARLY_PLAT);
+}
+
+void bl31_plat_arch_setup(void)
+{
+	const mmap_region_t bl_regions[] = {
+		MAP_BL_RO,
+		MAP_BL_RW,
+#if USE_COHERENT_MEM
+		MAP_BL_COHERENT_RAM,
+#endif
+		{0},
+	};
+
+	mtk_xlat_init(bl_regions);
+	/* Initialize module initcall */
+	mtk_init_one_level(MTK_INIT_LVL_ARCH);
+}
+
+/*****************************************************************************
+ * Perform any BL31 platform setup common to ARM standard platforms
+ ******************************************************************************/
+
+void bl31_platform_setup(void)
+{
+	mtk_init_one_level(MTK_INIT_LVL_PLAT_SETUP_0);
+	mtk_init_one_level(MTK_INIT_LVL_PLAT_SETUP_1);
+}
+
+/*******************************************************************************
+ * Operations before cold CPU leave BL31.
+ * Switch console to runtime state.
+ ******************************************************************************/
+void bl31_plat_runtime_setup(void)
+{
+	mtk_init_one_level(MTK_INIT_LVL_PLAT_RUNTIME);
+}
+
+unsigned int plat_get_syscnt_freq2(void)
+{
+	return SYS_COUNTER_FREQ_IN_HZ;
+}
diff --git a/plat/mediatek/common/mtk_plat_common.c b/plat/mediatek/common/mtk_plat_common.c
index 142b5c9..76f74a9 100644
--- a/plat/mediatek/common/mtk_plat_common.c
+++ b/plat/mediatek/common/mtk_plat_common.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,8 +19,6 @@
 #include <mtk_sip_svc.h>
 #include <plat_private.h>
 
-struct atf_arg_t gteearg;
-
 void clean_top_32b_of_param(uint32_t smc_fid,
 				u_register_t *px1,
 				u_register_t *px2,
@@ -28,96 +26,13 @@
 				u_register_t *px4)
 {
 	/* if parameters from SMC32. Clean top 32 bits */
-	if (0 == (smc_fid & SMC_AARCH64_BIT)) {
+	if (GET_SMC_CC(smc_fid) == SMC_64) {
 		*px1 = *px1 & SMC32_PARAM_MASK;
 		*px2 = *px2 & SMC32_PARAM_MASK;
 		*px3 = *px3 & SMC32_PARAM_MASK;
 		*px4 = *px4 & SMC32_PARAM_MASK;
 	}
 }
-
-#if MTK_SIP_KERNEL_BOOT_ENABLE
-static struct kernel_info k_info;
-
-static void save_kernel_info(uint64_t pc,
-			uint64_t r0,
-			uint64_t r1,
-			uint64_t k32_64)
-{
-	k_info.k32_64 = k32_64;
-	k_info.pc = pc;
-
-	if (LINUX_KERNEL_32 ==  k32_64) {
-		/* for 32 bits kernel */
-		k_info.r0 = 0;
-		/* machtype */
-		k_info.r1 = r0;
-		/* tags */
-		k_info.r2 = r1;
-	} else {
-		/* for 64 bits kernel */
-		k_info.r0 = r0;
-		k_info.r1 = r1;
-	}
-}
-
-uint64_t get_kernel_info_pc(void)
-{
-	return k_info.pc;
-}
-
-uint64_t get_kernel_info_r0(void)
-{
-	return k_info.r0;
-}
-
-uint64_t get_kernel_info_r1(void)
-{
-	return k_info.r1;
-}
-
-uint64_t get_kernel_info_r2(void)
-{
-	return k_info.r2;
-}
-
-void boot_to_kernel(uint64_t x1, uint64_t x2, uint64_t x3, uint64_t x4)
-{
-	static uint8_t kernel_boot_once_flag;
-	/* only support in booting flow */
-	if (0 == kernel_boot_once_flag) {
-		kernel_boot_once_flag = 1;
-
-		console_init(gteearg.atf_log_port,
-			UART_CLOCK, UART_BAUDRATE);
-		INFO("save kernel info\n");
-		save_kernel_info(x1, x2, x3, x4);
-		bl31_prepare_kernel_entry(x4);
-		INFO("el3_exit\n");
-		console_uninit();
-	}
-}
-#endif
-
-uint32_t plat_get_spsr_for_bl33_entry(void)
-{
-	unsigned int mode;
-	uint32_t spsr;
-	unsigned int ee;
-	unsigned long daif;
-
-	INFO("Secondary bootloader is AArch32\n");
-	mode = MODE32_svc;
-	ee = 0;
-	/*
-	 * TODO: Choose async. exception bits if HYP mode is not
-	 * implemented according to the values of SCR.{AW, FW} bits
-	 */
-	daif = DAIF_ABT_BIT | DAIF_IRQ_BIT | DAIF_FIQ_BIT;
-
-	spsr = SPSR_MODE32(mode, 0, ee, daif);
-	return spsr;
-}
 
 /*****************************************************************************
  * plat_is_smccc_feature_available() - This function checks whether SMCCC
diff --git a/plat/mediatek/common/mtk_plat_common.h b/plat/mediatek/common/mtk_plat_common.h
index 919c173..4c14b9d 100644
--- a/plat/mediatek/common/mtk_plat_common.h
+++ b/plat/mediatek/common/mtk_plat_common.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,45 +14,11 @@
 /*******************************************************************************
  * Function and variable prototypes
  ******************************************************************************/
-#define DEVINFO_SIZE 4
-#define LINUX_KERNEL_32 0
 #define SMC32_PARAM_MASK		(0xFFFFFFFF)
 
 #define JEDEC_MTK_BKID U(4)
 #define JEDEC_MTK_MFID U(0x26)
 
-struct atf_arg_t {
-	unsigned int atf_magic;
-	unsigned int tee_support;
-	unsigned int tee_entry;
-	unsigned int tee_boot_arg_addr;
-	unsigned int hwuid[4];     /* HW Unique id for t-base used */
-	unsigned int HRID[2];      /* HW random id for t-base used */
-	unsigned int atf_log_port;
-	unsigned int atf_log_baudrate;
-	unsigned int atf_log_buf_start;
-	unsigned int atf_log_buf_size;
-	unsigned int atf_irq_num;
-	unsigned int devinfo[DEVINFO_SIZE];
-	unsigned int atf_aee_debug_buf_start;
-	unsigned int atf_aee_debug_buf_size;
-};
-
-struct kernel_info {
-	uint64_t pc;
-	uint64_t r0;
-	uint64_t r1;
-	uint64_t r2;
-	uint64_t k32_64;
-};
-
-struct mtk_bl_param_t {
-	uint64_t bootarg_loc;
-	uint64_t bootarg_size;
-	uint64_t bl33_start_addr;
-	uint64_t tee_info_addr;
-};
-
 struct mtk_bl31_params {
        param_header_t h;
        image_info_t *bl31_image_info;
diff --git a/plat/mediatek/common/mtk_sip_svc.h b/plat/mediatek/common/mtk_sip_svc.h
deleted file mode 100644
index 74b17b6..0000000
--- a/plat/mediatek/common/mtk_sip_svc.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-#ifndef MTK_SIP_SVC_H
-#define MTK_SIP_SVC_H
-
-#include <stdint.h>
-
-/* SMC function IDs for SiP Service queries */
-#define SIP_SVC_CALL_COUNT		0x8200ff00
-#define SIP_SVC_UID			0x8200ff01
-/*					0x8200ff02 is reserved */
-#define SIP_SVC_VERSION			0x8200ff03
-
-/* Mediatek SiP Service Calls version numbers */
-#define MTK_SIP_SVC_VERSION_MAJOR	0x0
-#define MTK_SIP_SVC_VERSION_MINOR	0x1
-
-#define SMC_AARCH64_BIT		0x40000000
-
-/* Number of Mediatek SiP Calls implemented */
-#define MTK_COMMON_SIP_NUM_CALLS	4
-
-/* Mediatek SiP Service Calls function IDs */
-#define MTK_SIP_SET_AUTHORIZED_SECURE_REG	0x82000001
-
-/* For MTK SMC from Secure OS */
-/* 0x82000000 - 0x820000FF & 0xC2000000 - 0xC20000FF */
-#define MTK_SIP_KERNEL_BOOT_AARCH32		0x82000200
-#define MTK_SIP_KERNEL_BOOT_AARCH64		0xC2000200
-
-/* VCORE */
-#define MTK_SIP_VCORE_CONTROL_ARCH32		0x82000506
-#define MTK_SIP_VCORE_CONTROL_ARCH64		0xC2000506
-
-/* APUSYS SMC call */
-#define MTK_SIP_APUSYS_CONTROL_AARCH32		0x8200051E
-#define MTK_SIP_APUSYS_CONTROL_AARCH64		0xC200051E
-
-/* Mediatek SiP Calls error code */
-enum {
-	MTK_SIP_E_SUCCESS = 0,
-	MTK_SIP_E_INVALID_PARAM = -1,
-	MTK_SIP_E_NOT_SUPPORTED = -2,
-	MTK_SIP_E_INVALID_RANGE = -3,
-	MTK_SIP_E_PERMISSION_DENY = -4,
-	MTK_SIP_E_LOCK_FAIL = -5
-};
-
-/*
- * This function should be implemented in Mediatek SOC directory. It fullfills
- * MTK_SIP_SET_AUTHORIZED_SECURE_REG SiP call by checking the sreg with the
- * predefined secure register list, if a match was found, set val to sreg.
- *
- * Return MTK_SIP_E_SUCCESS on success, and MTK_SIP_E_INVALID_PARAM on failure.
- */
-uint64_t mt_sip_set_authorized_sreg(uint32_t sreg, uint32_t val);
-
-#endif /* MTK_SIP_SVC_H */
diff --git a/plat/mediatek/common/mtk_smc_handlers.c b/plat/mediatek/common/mtk_smc_handlers.c
new file mode 100644
index 0000000..24b978c
--- /dev/null
+++ b/plat/mediatek/common/mtk_smc_handlers.c
@@ -0,0 +1,233 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <cold_boot.h>
+#include <lib/mtk_init/mtk_init.h>
+#include <mtk_sip_svc.h>
+
+#define SMC_HANDLER_DEBUG(...) VERBOSE(__VA_ARGS__)
+#define SMC_HANDLER_DEBUG_NOT_IMP_MSG "%s[0x%x] smc handler not implemented\n"
+#define SMC_HANDLER_DEBUG_START_MSG "%s[0x%x] smc handler start, smc desc. index:%d\n"
+#define SMC_HANDLER_DEBUG_END_MSG "%s[0x%x] smc handler end\n"
+
+/*
+ * These macros below are used to identify SIP calls from Kernel,
+ * Hypervisor, or 2ndBootloader
+ */
+#define SIP_FID_ORI_MASK	(0xc000)
+#define SIP_FID_ORI_SHIFT	(14)
+#define SIP_FID_KERNEL		(0x0)
+#define SIP_FID_KERNEL_VIA_GZ	(0x1)
+#define SIP_FID_GZ		(0x2)
+
+#define GET_SMC_ORI(_fid)	(((_fid) & SIP_FID_ORI_MASK) >> SIP_FID_ORI_SHIFT)
+#define GET_SMC_ORI_NUM(_fid)	((_fid) & ~(SIP_FID_ORI_MASK))
+
+#define is_from_nsel2(_ori)	(_ori == SIP_FID_GZ)
+#define is_from_bl33(_ori) \
+	((_ori != SIP_FID_GZ) && (is_el1_2nd_bootloader() == 1))
+#define is_from_nsel1(_ori) \
+	(((_ori == SIP_FID_KERNEL) || \
+	 (_ori == SIP_FID_KERNEL_VIA_GZ)) && \
+	 (is_el1_2nd_bootloader() == 0))
+
+#define is_smc_forbidden(_ori) (_ori == SIP_FID_KERNEL_VIA_GZ)
+
+#define MASK_32_BIT (0xffffffffU)
+#define SMC_ID_EXPAND_AS_SMC_OPERATION(_smc_id, _smc_num) \
+	case _smc_id##_AARCH32: \
+	{ \
+		x1 = x1 & MASK_32_BIT; \
+		x2 = x2 & MASK_32_BIT; \
+		x3 = x3 & MASK_32_BIT; \
+		x4 = x4 & MASK_32_BIT; \
+	} \
+	case _smc_id##_AARCH64: \
+	{ \
+		if (_smc_id##_descriptor_index < 0) { \
+			SMC_HANDLER_DEBUG(SMC_HANDLER_DEBUG_NOT_IMP_MSG, #_smc_id, smc_id); \
+			break; \
+		} \
+		if (_smc_id##_descriptor_index >= smc_id_descriptor_max) { \
+			SMC_HANDLER_DEBUG("smc descriptor index[%d] exceed max[%d]\n", \
+					  _smc_id##_descriptor_index, smc_id_descriptor_max); \
+			break; \
+		} \
+		SMC_HANDLER_DEBUG(SMC_HANDLER_DEBUG_START_MSG, #_smc_id, smc_id, \
+				  _smc_id##_descriptor_index); \
+		ret  = smc_handler_pool[_smc_id##_descriptor_index].smc_handler(x1,\
+				x2, x3, x4, handle, &smc_ret); \
+		SMC_HANDLER_DEBUG(SMC_HANDLER_DEBUG_END_MSG, #_smc_id, smc_id); \
+		break; \
+	}
+
+#define SMC_ID_EXPAND_AS_DESCRIPTOR_INDEX(_smc_id, _smc_num) \
+	short _smc_id##_descriptor_index __section("mtk_plat_ro") = -1;
+
+MTK_SIP_SMC_FROM_BL33_TABLE(SMC_ID_EXPAND_AS_DESCRIPTOR_INDEX);
+MTK_SIP_SMC_FROM_NS_EL1_TABLE(SMC_ID_EXPAND_AS_DESCRIPTOR_INDEX);
+
+IMPORT_SYM(uintptr_t, __MTK_SMC_POOL_START__, MTK_SMC_POOL_START);
+IMPORT_SYM(uintptr_t, __MTK_SMC_POOL_END_UNALIGNED__, MTK_SMC_POOL_END_UNALIGNED);
+
+static const struct smc_descriptor *smc_handler_pool;
+static short smc_id_descriptor_max;
+
+#if !MTK_SIP_KERNEL_BOOT_ENABLE
+/*
+ * If there is no SMC request needs to be served in 2nd bootloader,
+ * disable the service path inherently.
+ */
+bool is_el1_2nd_bootloader(void)
+{
+	return false;
+}
+#endif
+
+static void print_smc_descriptor(const struct smc_descriptor pool[])
+{
+	const struct smc_descriptor *p_smc_desc;
+
+	INFO("print smc descriptor pool\n");
+	for (p_smc_desc = &pool[0];
+	     (char *)p_smc_desc < (char *)MTK_SMC_POOL_END_UNALIGNED;
+	     p_smc_desc++) {
+		INFO("descriptor name:%s\n", p_smc_desc->smc_name);
+		INFO("descriptor index:%d\n", *p_smc_desc->smc_descriptor_index);
+		INFO("smc id 32:0x%x, smc id 64:0x%x\n",
+		     p_smc_desc->smc_id_aarch32, p_smc_desc->smc_id_aarch64);
+	}
+}
+
+static int mtk_smc_handler_init(void)
+{
+	const struct smc_descriptor *iter;
+	short index_cnt;
+	int ret = 0;
+
+	smc_handler_pool = (const struct smc_descriptor *)MTK_SMC_POOL_START;
+	/* Designate descriptor index point to smc_handler_pool */
+	for (index_cnt = 0, iter = &smc_handler_pool[0];
+	     (char *)iter < (char *)MTK_SMC_POOL_END_UNALIGNED;
+	     iter++, index_cnt++) {
+		if (index_cnt < 0) {
+			SMC_HANDLER_DEBUG("smc handler pool index overflow!\n");
+			ret = -EPERM;
+			assert(0);
+			break;
+		}
+		*(iter->smc_descriptor_index) = index_cnt;
+	}
+	smc_id_descriptor_max = index_cnt;
+	print_smc_descriptor(smc_handler_pool);
+	return ret;
+}
+MTK_EARLY_PLAT_INIT(mtk_smc_handler_init);
+
+/* This function handles Mediatek defined SiP Calls from Bootloader */
+static uintptr_t mtk_smc_handler_bl33(uint32_t smc_id,
+				      u_register_t x1,
+				      u_register_t x2,
+				      u_register_t x3,
+				      u_register_t x4,
+				      void *cookie,
+				      void *handle,
+				      u_register_t flags)
+{
+	uintptr_t ret = MTK_SIP_E_SUCCESS;
+	struct smccc_res smc_ret = {0};
+
+	switch (smc_id) {
+		MTK_SIP_SMC_FROM_BL33_TABLE(SMC_ID_EXPAND_AS_SMC_OPERATION);
+	default:
+		INFO("BL33 SMC ID:0x%x not supported\n", smc_id);
+		ret = SMC_UNK;
+		break;
+	}
+	SMC_RET4(handle, ret, smc_ret.a1, smc_ret.a2, smc_ret.a3);
+}
+
+/* This function handles Mediatek defined SiP Calls from Kernel */
+static uintptr_t mtk_smc_handler_nsel1(uint32_t smc_id,
+				       u_register_t x1,
+				       u_register_t x2,
+				       u_register_t x3,
+				       u_register_t x4,
+				       void *cookie,
+				       void *handle,
+				       u_register_t flags)
+{
+	uintptr_t ret = MTK_SIP_E_SUCCESS;
+	struct smccc_res smc_ret = {0};
+
+	switch (smc_id) {
+		MTK_SIP_SMC_FROM_NS_EL1_TABLE(SMC_ID_EXPAND_AS_SMC_OPERATION);
+	default:
+		INFO("NSEL1 SMC ID:0x%x not supported\n", smc_id);
+		ret = SMC_UNK;
+		break;
+	}
+	SMC_RET4(handle, ret, smc_ret.a1, smc_ret.a2, smc_ret.a3);
+}
+
+static uintptr_t mtk_smc_handler(uint32_t smc_id,
+				 u_register_t x1,
+				 u_register_t x2,
+				 u_register_t x3,
+				 u_register_t x4,
+				 void *cookie,
+				 void *handle,
+				 u_register_t flags)
+{
+	uintptr_t ret = SMC_UNK;
+	uint32_t ns;
+	uint32_t smc_ori;
+	uint32_t smc_num;
+
+	/* Get SMC Originator bit 14.15 */
+	smc_ori = GET_SMC_ORI(smc_id);
+	/* Get SMC Number. Clean bit 14.15 */
+	smc_num = GET_SMC_ORI_NUM(smc_id);
+
+	/* Determine which security state this SMC originated from */
+	ns = is_caller_non_secure(flags);
+
+	if (ns && is_smc_forbidden(smc_ori)) {
+		ERROR("%s: Forbidden SMC call (0x%x)\n", __func__, smc_id);
+		SMC_RET1(handle, ret);
+	}
+
+	if (!ns) {
+		/* SiP SMC service secure world's call */
+		INFO("Secure SMC ID:0x%x not supported\n", smc_id);
+		SMC_RET1(handle, ret);
+	}
+	if (is_from_bl33(smc_ori)) {
+		/* SiP SMC service secure bootloader's call */
+		return mtk_smc_handler_bl33(smc_num, x1, x2, x3, x4,
+					    cookie, handle, flags);
+	} else if (is_from_nsel1(smc_ori)) {
+		/* SiP SMC service kernel's call */
+		return mtk_smc_handler_nsel1(smc_num, x1, x2, x3, x4,
+					     cookie, handle, flags);
+	}
+	INFO("SMC ID:0x%x not supported\n", smc_id);
+	SMC_RET1(handle, ret);
+}
+
+/* Define a runtime service descriptor for fast SMC calls */
+DECLARE_RT_SVC(
+	mtk_smc_handler,
+	OEN_SIP_START,
+	OEN_SIP_END,
+	SMC_TYPE_FAST,
+	NULL,
+	mtk_smc_handler
+);
diff --git a/plat/mediatek/common/rules.mk b/plat/mediatek/common/rules.mk
new file mode 100644
index 0000000..6acc731
--- /dev/null
+++ b/plat/mediatek/common/rules.mk
@@ -0,0 +1,15 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := mtk_common
+
+LOCAL_SRCS-y := ${LOCAL_DIR}/mtk_bl31_setup.c
+LOCAL_SRCS-y += ${LOCAL_DIR}/mtk_smc_handlers.c
+LOCAL_SRCS-$(MTK_SIP_KERNEL_BOOT_ENABLE) += ${LOCAL_DIR}/cold_boot.c
+
+$(eval $(call MAKE_LOCALS,$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/common/mtk_cirq.c b/plat/mediatek/drivers/cirq/mt_cirq.c
similarity index 98%
rename from plat/mediatek/common/mtk_cirq.c
rename to plat/mediatek/drivers/cirq/mt_cirq.c
index 9cf7144..60534a2 100644
--- a/plat/mediatek/common/mtk_cirq.c
+++ b/plat/mediatek/drivers/cirq/mt_cirq.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,8 +9,8 @@
 #include <drivers/arm/gic_common.h>
 #include <lib/mmio.h>
 
+#include <mt_cirq.h>
 #include <mt_gic_v3.h>
-#include <mtk_cirq.h>
 
 static struct cirq_events cirq_all_events = {
 	.spi_start = CIRQ_SPI_START,
diff --git a/plat/mediatek/common/mtk_cirq.h b/plat/mediatek/drivers/cirq/mt_cirq.h
similarity index 97%
rename from plat/mediatek/common/mtk_cirq.h
rename to plat/mediatek/drivers/cirq/mt_cirq.h
index 6e63bb8..cb96295 100644
--- a/plat/mediatek/common/mtk_cirq.h
+++ b/plat/mediatek/drivers/cirq/mt_cirq.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/mediatek/drivers/cirq/rules.mk b/plat/mediatek/drivers/cirq/rules.mk
new file mode 100644
index 0000000..710eae0
--- /dev/null
+++ b/plat/mediatek/drivers/cirq/rules.mk
@@ -0,0 +1,14 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := cirq
+LOCAL_SRCS-y := $(LOCAL_DIR)/mt_cirq.c
+
+PLAT_INCLUDES += -I${LOCAL_DIR}
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/mt8195/drivers/dp/mt_dp.c b/plat/mediatek/drivers/dp/mt_dp.c
similarity index 76%
rename from plat/mediatek/mt8195/drivers/dp/mt_dp.c
rename to plat/mediatek/drivers/dp/mt_dp.c
index 5930cd5..8aa246f 100644
--- a/plat/mediatek/mt8195/drivers/dp/mt_dp.c
+++ b/plat/mediatek/drivers/dp/mt_dp.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -15,7 +15,7 @@
 static uint32_t dp_write_sec_reg(uint32_t is_edp, uint32_t offset,
 				uint32_t value, uint32_t mask)
 {
-	uint32_t reg = (is_edp != 0U) ? eDP_SEC_BASE : DP_SEC_BASE;
+	uint32_t reg = (is_edp != 0U) ? EDP_SEC_BASE : DP_SEC_BASE;
 
 	mmio_clrsetbits_32(reg + offset, mask, value);
 
@@ -53,7 +53,7 @@
 
 	if (ret == MTK_SIP_E_SUCCESS) {
 		regmsk = (VIDEO_MUTE_SEL_SECURE_FLDMASK |
-				VIDEO_MUTE_SW_SECURE_FLDMASK);
+			  VIDEO_MUTE_SW_SECURE_FLDMASK);
 		if (para > 0U) {
 			fldmask = VIDEO_MUTE_SW_SECURE_FLDMASK;
 		} else {
@@ -67,3 +67,13 @@
 
 	return ret;
 }
+
+u_register_t mtk_dp_sip_handler(u_register_t x1, u_register_t x2,
+				u_register_t x3, u_register_t x4,
+				void *handle, struct smccc_res *smccc_ret)
+{
+	uint32_t ret_val;
+
+	return dp_secure_handler(x1, x2, &ret_val);
+}
+DECLARE_SMC_HANDLER(MTK_SIP_DP_CONTROL, mtk_dp_sip_handler);
diff --git a/plat/mediatek/mt8195/drivers/dp/mt_dp.h b/plat/mediatek/drivers/dp/mt_dp.h
similarity index 87%
rename from plat/mediatek/mt8195/drivers/dp/mt_dp.h
rename to plat/mediatek/drivers/dp/mt_dp.h
index 8157598..d5dad29 100644
--- a/plat/mediatek/mt8195/drivers/dp/mt_dp.h
+++ b/plat/mediatek/drivers/dp/mt_dp.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/mediatek/drivers/dp/rules.mk b/plat/mediatek/drivers/dp/rules.mk
new file mode 100644
index 0000000..786d514
--- /dev/null
+++ b/plat/mediatek/drivers/dp/rules.mk
@@ -0,0 +1,14 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := dp
+LOCAL_SRCS-y := $(LOCAL_DIR)/mt_dp.c
+
+PLAT_INCLUDES += -I${LOCAL_DIR}
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/common/drivers/gic600/mt_gic_v3.c b/plat/mediatek/drivers/gic600/mt_gic_v3.c
similarity index 93%
rename from plat/mediatek/common/drivers/gic600/mt_gic_v3.c
rename to plat/mediatek/drivers/gic600/mt_gic_v3.c
index ae8d697..cca5d0a 100644
--- a/plat/mediatek/common/drivers/gic600/mt_gic_v3.c
+++ b/plat/mediatek/drivers/gic600/mt_gic_v3.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -12,7 +12,7 @@
 #include <bl31/interrupt_mgmt.h>
 #include <common/bl_common.h>
 #include <common/debug.h>
-
+#include <lib/mtk_init/mtk_init.h>
 #include <mt_gic_v3.h>
 #include <mtk_plat_common.h>
 #include <plat/common/platform.h>
@@ -194,3 +194,15 @@
 	mmio_write_32(BASE_GICD_BASE + GICD_ISPENDR +
 		irq / 32 * 4, bit);
 }
+
+int mt_gic_one_init(void)
+{
+	INFO("[%s] GIC initialization\n", __func__);
+
+	/* Initialize the GIC driver, CPU and distributor interfaces */
+	mt_gic_driver_init();
+	mt_gic_init();
+
+	return 0;
+}
+MTK_PLAT_SETUP_0_INIT(mt_gic_one_init);
diff --git a/plat/mediatek/common/drivers/gic600/mt_gic_v3.h b/plat/mediatek/drivers/gic600/mt_gic_v3.h
similarity index 87%
rename from plat/mediatek/common/drivers/gic600/mt_gic_v3.h
rename to plat/mediatek/drivers/gic600/mt_gic_v3.h
index c4ab44f..31513ef 100644
--- a/plat/mediatek/common/drivers/gic600/mt_gic_v3.h
+++ b/plat/mediatek/drivers/gic600/mt_gic_v3.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -23,5 +23,6 @@
 void gic_sgi_restore_all(void);
 uint32_t mt_irq_get_pending(uint32_t irq);
 void mt_irq_set_pending(uint32_t irq);
+int mt_gic_one_init(void);
 
 #endif /* MT_GIC_V3_H */
diff --git a/plat/mediatek/drivers/gic600/rules.mk b/plat/mediatek/drivers/gic600/rules.mk
new file mode 100644
index 0000000..3070591
--- /dev/null
+++ b/plat/mediatek/drivers/gic600/rules.mk
@@ -0,0 +1,14 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := gic600
+LOCAL_SRCS-y := $(LOCAL_DIR)/mt_gic_v3.c
+
+PLAT_INCLUDES += -I${LOCAL_DIR}
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/drivers/gpio/mt8188/mtgpio.c b/plat/mediatek/drivers/gpio/mt8188/mtgpio.c
new file mode 100644
index 0000000..9e9fc5d
--- /dev/null
+++ b/plat/mediatek/drivers/gpio/mt8188/mtgpio.c
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2022, 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_RM_BASE;
+		break;
+	case 1:
+		reg_addr = IOCFG_LT_BASE;
+		break;
+	case 2:
+		reg_addr = IOCFG_LM_BASE;
+		break;
+	case 3:
+		reg_addr = IOCFG_RT_BASE;
+		break;
+	default:
+		break;
+	}
+
+	return reg_addr;
+}
diff --git a/plat/mediatek/drivers/gpio/mt8188/mtgpio.h b/plat/mediatek/drivers/gpio/mt8188/mtgpio.h
new file mode 100644
index 0000000..32a4608
--- /dev/null
+++ b/plat/mediatek/drivers/gpio/mt8188/mtgpio.h
@@ -0,0 +1,221 @@
+/*
+ * Copyright (c) 2022, 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,
+	MT_GPIO_BASE_MAX
+} GPIO_PIN;
+
+static const struct mt_pin_info mt_pin_infos[] = {
+	PIN(0, 0, 6, 0x30, 0xb0),
+	PIN(1, 0, 7, 0x30, 0xb0),
+	PIN(2, 0, 8, 0x30, 0xb0),
+	PIN(3, 0, 9, 0x30, 0xb0),
+	PIN(4, 0, 10, 0x30, 0xb0),
+	PIN(5, 0, 11, 0x30, 0xb0),
+	PIN(6, 0, 12, 0x30, 0xb0),
+	PIN(7, 0, 13, 0x30, 0xb0),
+	PIN(8, 0, 14, 0x30, 0xb0),
+	PIN(9, 0, 15, 0x30, 0xb0),
+	PIN(10, 0, 16, 0x30, 0xb0),
+	PIN(11, 0, 17, 0x30, 0xb0),
+	PIN(12, 0, 12, 0x31, 0xa0),
+	PIN(13, 0, 13, 0x31, 0xa0),
+	PIN(14, 0, 14, 0x31, 0xa0),
+	PIN(15, 0, 15, 0x31, 0xa0),
+	PIN(16, 0, 1, 0x22, 0x50),
+	PIN(17, 0, 2, 0x22, 0x50),
+	PIN(18, 0, 3, 0x23, 0x60),
+	PIN(19, 0, 4, 0x23, 0x60),
+	PIN(20, 0, 5, 0x23, 0x60),
+	PIN(21, 0, 6, 0x23, 0x60),
+	PIN(22, 0, 0, 0x23, 0x60),
+	PIN(23, 0, 1, 0x23, 0x60),
+	PIN(24, 0, 2, 0x23, 0x60),
+	PIN(25, 0, 3, 0x30, 0xb0),
+	PIN(26, 0, 2, 0x30, 0xb0),
+	PIN(27, 0, 5, 0x30, 0xb0),
+	PIN(28, 0, 4, 0x30, 0xb0),
+	PIN(29, 0, 0, 0x30, 0xb0),
+	PIN(30, 0, 1, 0x30, 0xb0),
+	PIN(31, 0, 11, 0x30, 0xc0),
+	PIN(32, 0, 10, 0x30, 0xc0),
+	PIN(33, 0, 13, 0x30, 0xc0),
+	PIN(34, 0, 12, 0x30, 0xc0),
+	PIN(35, 0, 15, 0x30, 0xc0),
+	PIN(36, 0, 14, 0x30, 0xc0),
+	PIN(37, 0, 21, 0x30, 0xb0),
+	PIN(38, 0, 18, 0x30, 0xb0),
+	PIN(39, 0, 19, 0x30, 0xb0),
+	PIN(40, 0, 20, 0x30, 0xb0),
+	PIN(41, 0, 22, 0x30, 0xb0),
+	PIN(42, 1, 12, 0x31, 0xc0),
+	PIN(43, 1, 13, 0x31, 0xc0),
+	PIN(44, 1, 14, 0x31, 0xc0),
+	PIN(45, 1, 15, 0x31, 0xc0),
+	PIN(46, 0, 0, 0x22, 0x50),
+	PIN(47, 0, 25, 0x30, 0xb0),
+	PIN(48, 0, 24, 0x30, 0xb0),
+	PIN(49, 0, 23, 0x30, 0xb0),
+	PIN(50, 0, 5, 0x22, 0x50),
+	PIN(51, 0, 4, 0x22, 0x50),
+	PIN(52, 0, 3, 0x22, 0x50),
+	PIN(53, 0, 6, 0x22, 0x50),
+	PIN(54, 0, 7, 0x22, 0x50),
+	PIN(55, 0, 26, 0x30, 0xb0),
+	PIN(56, 0, 29, 0x30, 0xb0),
+	PIN(57, 0, 6, 0x31, 0xb0),
+	PIN(58, 0, 9, 0x31, 0xb0),
+	PIN(59, 0, 27, 0x30, 0xb0),
+	PIN(60, 0, 30, 0x30, 0xb0),
+	PIN(61, 0, 28, 0x30, 0xb0),
+	PIN(62, 0, 31, 0x30, 0xb0),
+	PIN(63, 0, 7, 0x31, 0xb0),
+	PIN(64, 0, 10, 0x31, 0xb0),
+	PIN(65, 0, 7, 0x23, 0x60),
+	PIN(66, 0, 9, 0x23, 0x60),
+	PIN(67, 0, 8, 0x23, 0x60),
+	PIN(68, 0, 10, 0x23, 0x60),
+	PIN(69, 0, 1, 0x30, 0xc0),
+	PIN(70, 0, 0, 0x30, 0xc0),
+	PIN(71, 0, 5, 0x30, 0xc0),
+	PIN(72, 0, 4, 0x30, 0xc0),
+	PIN(73, 0, 2, 0x30, 0xc0),
+	PIN(74, 0, 3, 0x30, 0xc0),
+	PIN(75, 0, 7, 0x30, 0xc0),
+	PIN(76, 0, 6, 0x30, 0xc0),
+	PIN(77, 0, 9, 0x30, 0xc0),
+	PIN(78, 0, 8, 0x30, 0xc0),
+	PIN(79, 0, 12, 0x23, 0x60),
+	PIN(80, 0, 11, 0x23, 0x60),
+	PIN(81, 0, 14, 0x23, 0x60),
+	PIN(82, 0, 13, 0x23, 0x60),
+	PIN(83, 0, 16, 0x31, 0xb0),
+	PIN(84, 0, 15, 0x31, 0xb0),
+	PIN(85, 0, 17, 0x31, 0xb0),
+	PIN(86, 0, 19, 0x31, 0xb0),
+	PIN(87, 0, 18, 0x31, 0xb0),
+	PIN(88, 0, 20, 0x31, 0xb0),
+	PIN(89, 0, 22, 0x31, 0xb0),
+	PIN(90, 0, 21, 0x31, 0xb0),
+	PIN(91, 0, 23, 0x31, 0xb0),
+	PIN(92, 0, 3, 0x31, 0xb0),
+	PIN(93, 0, 2, 0x31, 0xb0),
+	PIN(94, 0, 5, 0x31, 0xb0),
+	PIN(95, 0, 4, 0x31, 0xb0),
+	PIN(96, 0, 31, 0x31, 0xa0),
+	PIN(97, 0, 0, 0x31, 0xb0),
+	PIN(98, 0, 8, 0x31, 0xb0),
+	PIN(99, 0, 30, 0x31, 0xa0),
+	PIN(100, 0, 1, 0x31, 0xb0),
+	PIN(101, 0, 0, 0x31, 0xa0),
+	PIN(102, 0, 5, 0x31, 0xa0),
+	PIN(103, 0, 3, 0x31, 0xa0),
+	PIN(104, 0, 4, 0x31, 0xa0),
+	PIN(105, 0, 1, 0x31, 0xa0),
+	PIN(106, 0, 2, 0x31, 0xa0),
+	PIN(107, 0, 21, 0x31, 0xa0),
+	PIN(108, 0, 16, 0x31, 0xa0),
+	PIN(109, 0, 22, 0x31, 0xa0),
+	PIN(110, 0, 17, 0x31, 0xa0),
+	PIN(111, 0, 18, 0x31, 0xa0),
+	PIN(112, 0, 19, 0x31, 0xa0),
+	PIN(113, 0, 20, 0x31, 0xa0),
+	PIN(114, 0, 28, 0x31, 0xa0),
+	PIN(115, 0, 23, 0x31, 0xa0),
+	PIN(116, 0, 29, 0x31, 0xa0),
+	PIN(117, 0, 24, 0x31, 0xa0),
+	PIN(118, 0, 25, 0x31, 0xa0),
+	PIN(119, 0, 26, 0x31, 0xa0),
+	PIN(120, 0, 27, 0x31, 0xa0),
+	PIN(121, 0, 8, 0x22, 0x50),
+	PIN(122, 0, 11, 0x22, 0x50),
+	PIN(123, 0, 10, 0x22, 0x50),
+	PIN(124, 0, 9, 0x22, 0x50),
+	PIN(125, 0, 6, 0x31, 0xa0),
+	PIN(126, 0, 7, 0x31, 0xa0),
+	PIN(127, 0, 8, 0x31, 0xa0),
+	PIN(128, 0, 9, 0x31, 0xa0),
+	PIN(129, 0, 10, 0x31, 0xa0),
+	PIN(130, 0, 11, 0x31, 0xa0),
+	PIN(131, 1, 1, 0x30, 0xd0),
+	PIN(132, 1, 2, 0x30, 0xd0),
+	PIN(133, 1, 9, 0x30, 0xd0),
+	PIN(134, 1, 10, 0x30, 0xd0),
+	PIN(135, 1, 11, 0x30, 0xd0),
+	PIN(136, 1, 12, 0x30, 0xd0),
+	PIN(137, 1, 13, 0x30, 0xd0),
+	PIN(138, 1, 14, 0x30, 0xd0),
+	PIN(139, 1, 15, 0x30, 0xd0),
+	PIN(140, 1, 16, 0x30, 0xd0),
+	PIN(141, 1, 3, 0x30, 0xd0),
+	PIN(142, 1, 4, 0x30, 0xd0),
+	PIN(143, 1, 5, 0x30, 0xd0),
+	PIN(144, 1, 6, 0x30, 0xd0),
+	PIN(145, 1, 7, 0x30, 0xd0),
+	PIN(146, 1, 8, 0x30, 0xd0),
+	PIN(147, 1, 18, 0x30, 0xd0),
+	PIN(148, 1, 19, 0x30, 0xd0),
+	PIN(149, 1, 17, 0x30, 0xd0),
+	PIN(150, 1, 0, 0x30, 0xd0),
+	PIN(151, 1, 9, 0x31, 0xc0),
+	PIN(152, 1, 8, 0x31, 0xc0),
+	PIN(153, 1, 7, 0x31, 0xc0),
+	PIN(154, 1, 6, 0x31, 0xc0),
+	PIN(155, 1, 11, 0x31, 0xc0),
+	PIN(156, 1, 1, 0x31, 0xc0),
+	PIN(157, 1, 0, 0x31, 0xc0),
+	PIN(158, 1, 5, 0x31, 0xc0),
+	PIN(159, 1, 4, 0x31, 0xc0),
+	PIN(160, 1, 3, 0x31, 0xc0),
+	PIN(161, 1, 2, 0x31, 0xc0),
+	PIN(162, 1, 10, 0x31, 0xc0),
+	PIN(163, 1, 1, 0x23, 0x70),
+	PIN(164, 1, 0, 0x23, 0x70),
+	PIN(165, 1, 2, 0x23, 0x70),
+	PIN(166, 1, 3, 0x23, 0x70),
+	PIN(167, 1, 4, 0x23, 0x70),
+	PIN(168, 1, 5, 0x23, 0x70),
+	PIN(169, 1, 1, 0x22, 0x60),
+	PIN(170, 1, 0, 0x22, 0x60),
+	PIN(171, 1, 2, 0x22, 0x60),
+	PIN(172, 1, 3, 0x22, 0x60),
+	PIN(173, 1, 4, 0x22, 0x60),
+	PIN(174, 1, 5, 0x22, 0x60),
+	PIN(175, 0, 11, 0x31, 0xb0),
+	PIN(176, 0, 12, 0x31, 0xb0),
+};
+
+#endif /* MT_GPIO_H */
diff --git a/plat/mediatek/common/drivers/gpio/mtgpio_common.c b/plat/mediatek/drivers/gpio/mtgpio_common.c
similarity index 97%
rename from plat/mediatek/common/drivers/gpio/mtgpio_common.c
rename to plat/mediatek/drivers/gpio/mtgpio_common.c
index 89977a5..bad0190 100644
--- a/plat/mediatek/common/drivers/gpio/mtgpio_common.c
+++ b/plat/mediatek/drivers/gpio/mtgpio_common.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,7 @@
 #include <drivers/delay_timer.h>
 #include <drivers/gpio.h>
 #include <lib/mmio.h>
+#include <lib/mtk_init/mtk_init.h>
 #include <mtgpio.h>
 #include <platform_def.h>
 
@@ -292,7 +293,10 @@
 	 .get_pull = mt_get_gpio_pull,
 };
 
-void mt_gpio_init(void)
+int mt_gpio_init(void)
 {
 	gpio_init(&mtgpio_ops);
+
+	return 0;
 }
+MTK_PLAT_SETUP_0_INIT(mt_gpio_init);
diff --git a/plat/mediatek/common/drivers/gpio/mtgpio_common.h b/plat/mediatek/drivers/gpio/mtgpio_common.h
similarity index 95%
rename from plat/mediatek/common/drivers/gpio/mtgpio_common.h
rename to plat/mediatek/drivers/gpio/mtgpio_common.h
index bf51055..d6b858c 100644
--- a/plat/mediatek/common/drivers/gpio/mtgpio_common.h
+++ b/plat/mediatek/drivers/gpio/mtgpio_common.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -104,6 +104,6 @@
 	uint16_t offset;
 };
 
-void mt_gpio_init(void);
+int mt_gpio_init(void);
 uintptr_t mt_gpio_find_reg_addr(uint32_t pin);
 #endif /* MT_GPIO_COMMON_H */
diff --git a/plat/mediatek/drivers/gpio/rules.mk b/plat/mediatek/drivers/gpio/rules.mk
new file mode 100644
index 0000000..78061a8
--- /dev/null
+++ b/plat/mediatek/drivers/gpio/rules.mk
@@ -0,0 +1,18 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := gpio
+
+LOCAL_SRCS-y := drivers/gpio/gpio.c
+LOCAL_SRCS-y += ${LOCAL_DIR}/mtgpio_common.c
+LOCAL_SRCS-y += ${LOCAL_DIR}/${MTK_SOC}/mtgpio.c
+
+PLAT_INCLUDES += -I${LOCAL_DIR}
+PLAT_INCLUDES += -I${LOCAL_DIR}/${MTK_SOC}
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/drivers/iommu/mt8188/mtk_iommu_plat.c b/plat/mediatek/drivers/iommu/mt8188/mtk_iommu_plat.c
new file mode 100644
index 0000000..1d6863f
--- /dev/null
+++ b/plat/mediatek/drivers/iommu/mt8188/mtk_iommu_plat.c
@@ -0,0 +1,99 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <mtk_iommu_plat.h>
+#include <mtk_mmap_pool.h>
+#include <platform_def.h>
+
+/* mm iommu */
+#define SMI_L0_ID		(0)
+#define SMI_L1_ID		(1)
+#define SMI_L2_ID		(2)
+#define SMI_L3_ID		(3)
+#define SMI_L4_ID		(4)
+#define SMI_L5_ID		(5)
+#define SMI_L6_ID		(6)
+#define SMI_L7_ID		(7)
+#define SMI_L9_ID		(8)
+#define SMI_L10_ID		(9)
+#define SMI_L11A_ID		(10)
+#define SMI_L11B_ID		(11)
+#define SMI_L11C_ID		(12)
+#define SMI_L12_ID		(13)
+#define SMI_L13_ID		(14)
+#define SMI_L14_ID		(15)
+#define SMI_L15_ID		(16)
+#define SMI_L16A_ID		(17)
+#define SMI_L16B_ID		(18)
+#define SMI_L17A_ID		(19)
+#define SMI_L17B_ID		(20)
+#define SMI_L19_ID		(21)
+#define SMI_L21_ID		(22)
+#define SMI_L23_ID		(23)
+#define SMI_L27_ID		(24)
+#define SMI_L28_ID		(25)
+
+/* infra iommu */
+#define PERI_MST_PROT		(0x710)
+#define PERICFG_AO_IOMMU_1	(0x714)
+#define MMU_DEV_PCIE_0		(0)
+#define IFR_CFG_GROUP_NUM	(1)
+
+static struct mtk_smi_larb_config mt8188_larb_cfg[SMI_LARB_NUM] = {
+	[SMI_L0_ID] = LARB_CFG_ENTRY(SMI_LARB_0_BASE, 7, 0),
+	[SMI_L1_ID] = LARB_CFG_ENTRY(SMI_LARB_1_BASE, 7, 0),
+	[SMI_L2_ID] = LARB_CFG_ENTRY(SMI_LARB_2_BASE, 5, 0),
+	[SMI_L3_ID] = LARB_CFG_ENTRY(SMI_LARB_3_BASE, 7, 0),
+	[SMI_L4_ID] = LARB_CFG_ENTRY(SMI_LARB_4_BASE, 7, 0),
+	[SMI_L5_ID] = LARB_CFG_ENTRY(SMI_LARB_5_BASE, 8, 0),
+	[SMI_L6_ID] = LARB_CFG_ENTRY(SMI_LARB_6_BASE, 4, 0),
+	[SMI_L7_ID] = LARB_CFG_ENTRY(SMI_LARB_7_BASE, 3, 0),
+	[SMI_L9_ID] = LARB_CFG_ENTRY(SMI_LARB_9_BASE, 25, 0),
+	[SMI_L10_ID] = LARB_CFG_ENTRY(SMI_LARB_10_BASE, 20, 0),
+	[SMI_L11A_ID] = LARB_CFG_ENTRY(SMI_LARB_11A_BASE, 30, 0),
+	[SMI_L11B_ID] = LARB_CFG_ENTRY(SMI_LARB_11B_BASE, 30, 0),
+	[SMI_L11C_ID] = LARB_CFG_ENTRY(SMI_LARB_11C_BASE, 30, 0),
+	[SMI_L12_ID] = LARB_CFG_ENTRY(SMI_LARB_12_BASE, 16, 0),
+	[SMI_L13_ID] = LARB_CFG_ENTRY(SMI_LARB_13_BASE, 24, 0),
+	[SMI_L14_ID] = LARB_CFG_ENTRY(SMI_LARB_14_BASE, 23, 0),
+	[SMI_L15_ID] = LARB_CFG_ENTRY(SMI_LARB_15_BASE, 19, 0),
+	[SMI_L16A_ID] = LARB_CFG_ENTRY(SMI_LARB_16A_BASE, 17, 0),
+	[SMI_L16B_ID] = LARB_CFG_ENTRY(SMI_LARB_16B_BASE, 17, 0),
+	[SMI_L17A_ID] = LARB_CFG_ENTRY(SMI_LARB_17A_BASE, 7, 0),
+	[SMI_L17B_ID] = LARB_CFG_ENTRY(SMI_LARB_17B_BASE, 7, 0),
+	/* venc nbm ports (5/6/11/15/16/17) to sram */
+	[SMI_L19_ID] = LARB_CFG_ENTRY_WITH_PATH(SMI_LARB_19_BASE, 27, 0, 0x38860),
+	[SMI_L21_ID] = LARB_CFG_ENTRY(SMI_LARB_21_BASE, 11, 0),
+	[SMI_L23_ID] = LARB_CFG_ENTRY(SMI_LARB_23_BASE, 9, 0),
+	[SMI_L27_ID] = LARB_CFG_ENTRY(SMI_LARB_27_BASE, 4, 0),
+	[SMI_L28_ID] = LARB_CFG_ENTRY(SMI_LARB_28_BASE, 0, 0),
+};
+
+static bool is_protected;
+
+static uint32_t mt8188_ifr_mst_cfg_base[IFR_CFG_GROUP_NUM] = {
+	PERICFG_AO_BASE,
+};
+static uint32_t mt8188_ifr_mst_cfg_offs[IFR_CFG_GROUP_NUM] = {
+	PERICFG_AO_IOMMU_1,
+};
+static struct mtk_ifr_mst_config mt8188_ifr_mst_cfg[MMU_DEV_NUM] = {
+	[MMU_DEV_PCIE_0] = IFR_MST_CFG_ENTRY(0, 18),
+};
+
+struct mtk_smi_larb_config *g_larb_cfg = &mt8188_larb_cfg[0];
+struct mtk_ifr_mst_config *g_ifr_mst_cfg = &mt8188_ifr_mst_cfg[0];
+uint32_t *g_ifr_mst_cfg_base = &mt8188_ifr_mst_cfg_base[0];
+uint32_t *g_ifr_mst_cfg_offs = &mt8188_ifr_mst_cfg_offs[0];
+
+/* Protect infra iommu enable setting registers as secure access. */
+void mtk_infra_iommu_enable_protect(void)
+{
+	if (!is_protected) {
+		mmio_write_32(PERICFG_AO_BASE + PERI_MST_PROT, 0xffffffff);
+		is_protected = true;
+	}
+}
diff --git a/plat/mediatek/drivers/iommu/mt8188/mtk_iommu_plat.h b/plat/mediatek/drivers/iommu/mt8188/mtk_iommu_plat.h
new file mode 100644
index 0000000..a59e0c7
--- /dev/null
+++ b/plat/mediatek/drivers/iommu/mt8188/mtk_iommu_plat.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IOMMU_PLAT_H
+#define IOMMU_PLAT_H
+
+#include <mtk_iommu_priv.h>
+
+/* mm iommu */
+#define SMI_LARB_NUM	(26)
+extern struct mtk_smi_larb_config *g_larb_cfg;
+
+/* infra iommu */
+#define MMU_DEV_NUM	(1)
+extern struct mtk_ifr_mst_config *g_ifr_mst_cfg;
+extern uint32_t *g_ifr_mst_cfg_base;
+extern uint32_t *g_ifr_mst_cfg_offs;
+
+extern void mtk_infra_iommu_enable_protect(void);
+
+#endif /* IOMMU_PLAT_H */
diff --git a/plat/mediatek/drivers/iommu/mtk_iommu_priv.h b/plat/mediatek/drivers/iommu/mtk_iommu_priv.h
new file mode 100644
index 0000000..3404d31
--- /dev/null
+++ b/plat/mediatek/drivers/iommu/mtk_iommu_priv.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IOMMU_PRIV_H
+#define IOMMU_PRIV_H
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <mtk_sip_svc.h>
+
+#define LARB_CFG_ENTRY(bs, p_nr, dom)			\
+	{ .base = (bs), .port_nr = (p_nr),		\
+	  .dom_id = (dom), .to_sram = 0, }
+
+#define LARB_CFG_ENTRY_WITH_PATH(bs, p_nr, dom, sram)	\
+	{ .base = (bs), .port_nr = (p_nr),		\
+	  .dom_id = (dom), .to_sram = (sram), }
+
+#define IFR_MST_CFG_ENTRY(idx, bit)	\
+	{ .cfg_addr_idx = (idx), .r_mmu_en_bit = (bit), }
+
+enum IOMMU_ATF_CMD {
+	IOMMU_ATF_CMD_CONFIG_SMI_LARB,		/* For mm master to enable iommu */
+	IOMMU_ATF_CMD_CONFIG_INFRA_IOMMU,	/* For infra master to enable iommu */
+	IOMMU_ATF_CMD_COUNT,
+};
+
+struct mtk_smi_larb_config {
+	uint32_t base;
+	uint32_t port_nr;
+	uint32_t dom_id;
+	uint32_t to_sram;
+	uint32_t sec_en_msk;
+};
+
+struct mtk_ifr_mst_config {
+	uint8_t cfg_addr_idx;
+	uint8_t r_mmu_en_bit;
+};
+
+#endif	/* IOMMU_PRIV_H */
diff --git a/plat/mediatek/drivers/iommu/mtk_iommu_smc.c b/plat/mediatek/drivers/iommu/mtk_iommu_smc.c
new file mode 100644
index 0000000..9762d0b
--- /dev/null
+++ b/plat/mediatek/drivers/iommu/mtk_iommu_smc.c
@@ -0,0 +1,125 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stddef.h>
+#include <mtk_iommu_plat.h>
+
+/* defination */
+/* smi larb */
+#define SMI_LARB_NON_SEC_CON(port)	(0x380 + ((port) << 2))
+#define PATH_SEL_MASK			(0xf0000) /* to sram (INT) */
+#define SMI_LARB_SEC_CON_INT(port)	(0xf00 + ((port) << 2))
+#define SMI_LARB_SEC_CON(port)		(0xf80 + ((port) << 2))
+#define MMU_MASK			BIT(0)
+#define MMU_EN(en)			((!!(en)) << 0)
+#define SEC_MASK			BIT(1)
+#define SEC_EN(en)			((!!(en)) << 1)
+#define DOMAIN_MASK			(0x1f << 4)
+#define SMI_MMU_EN(port)		(0x1 << (port))
+
+/* infra master */
+#define IFR_CFG_MMU_EN_MSK(r_bit)	(0x3 << (r_bit))
+
+/* smi larb configure */
+/*
+ * If multimedia security config is enabled, the SMI config register must be
+ * configurated in security world.
+ * And the SRAM path is also configurated here to enhance security.
+ */
+static void mtk_smi_larb_port_config_to_sram(
+				const struct mtk_smi_larb_config *larb,
+				uint32_t port_id)
+{
+	mmio_clrbits_32(larb->base + SMI_LARB_SEC_CON_INT(port_id),
+			MMU_MASK | SEC_MASK | DOMAIN_MASK);
+
+	mmio_setbits_32(larb->base + SMI_LARB_NON_SEC_CON(port_id),
+			PATH_SEL_MASK);
+}
+
+static void mtk_smi_port_config(const struct mtk_smi_larb_config *larb,
+				uint32_t port_id, uint8_t mmu_en, uint8_t sec_en)
+{
+	mmio_clrsetbits_32(larb->base + SMI_LARB_SEC_CON(port_id),
+			   MMU_MASK | SEC_MASK | DOMAIN_MASK,
+			   MMU_EN(mmu_en) | SEC_EN(sec_en));
+}
+
+static int mtk_smi_larb_port_config_sec(uint32_t larb_id, uint32_t mmu_en_msk)
+{
+	uint32_t port_id, port_nr;
+	const struct mtk_smi_larb_config *larb;
+	uint32_t to_sram;
+	uint8_t mmu_en;
+
+	if (larb_id >= SMI_LARB_NUM) {
+		return MTK_SIP_E_INVALID_PARAM;
+	}
+
+	larb = &g_larb_cfg[larb_id];
+	port_nr = larb->port_nr;
+	to_sram = larb->to_sram;
+
+	for (port_id = 0; port_id < port_nr; port_id++) {
+		if ((to_sram & BIT(port_id)) > 0U) {
+			mtk_smi_larb_port_config_to_sram(larb, port_id);
+			continue;
+		}
+		mmu_en = !!(mmu_en_msk & SMI_MMU_EN(port_id));
+		mtk_smi_port_config(larb, port_id, mmu_en, 0);
+	}
+
+	return MTK_SIP_E_SUCCESS;
+}
+
+static int mtk_infra_master_config_sec(uint32_t dev_id, uint32_t enable)
+{
+	const struct mtk_ifr_mst_config *ifr_cfg;
+	uint32_t reg_addr;
+
+	mtk_infra_iommu_enable_protect();
+
+	if (dev_id >= MMU_DEV_NUM) {
+		return MTK_SIP_E_NOT_SUPPORTED;
+	}
+
+	ifr_cfg = &g_ifr_mst_cfg[dev_id];
+	reg_addr = g_ifr_mst_cfg_base[(ifr_cfg->cfg_addr_idx)] +
+		   g_ifr_mst_cfg_offs[(ifr_cfg->cfg_addr_idx)];
+
+	if (enable > 0U) {
+		mmio_setbits_32(reg_addr, IFR_CFG_MMU_EN_MSK(ifr_cfg->r_mmu_en_bit));
+	} else {
+		mmio_clrbits_32(reg_addr, IFR_CFG_MMU_EN_MSK(ifr_cfg->r_mmu_en_bit));
+	}
+
+	return MTK_SIP_E_SUCCESS;
+}
+
+static u_register_t mtk_iommu_handler(u_register_t x1, u_register_t x2,
+				      u_register_t x3, u_register_t x4,
+				      void *handle, struct smccc_res *smccc_ret)
+{
+	uint32_t cmd_id = x1, mdl_id = x2, val = x3;
+	int ret = MTK_SIP_E_NOT_SUPPORTED;
+
+	(void)x4;
+	(void)handle;
+
+	switch (cmd_id) {
+	case IOMMU_ATF_CMD_CONFIG_SMI_LARB:
+		ret = mtk_smi_larb_port_config_sec(mdl_id, val);
+		break;
+	case IOMMU_ATF_CMD_CONFIG_INFRA_IOMMU:
+		ret = mtk_infra_master_config_sec(mdl_id, val);
+		break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+DECLARE_SMC_HANDLER(MTK_SIP_IOMMU_CONTROL, mtk_iommu_handler);
diff --git a/plat/mediatek/drivers/iommu/rules.mk b/plat/mediatek/drivers/iommu/rules.mk
new file mode 100644
index 0000000..5490f41
--- /dev/null
+++ b/plat/mediatek/drivers/iommu/rules.mk
@@ -0,0 +1,17 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := mtk_iommu
+
+LOCAL_SRCS-y := ${LOCAL_DIR}/mtk_iommu_smc.c
+LOCAL_SRCS-y += ${LOCAL_DIR}/${MTK_SOC}/mtk_iommu_plat.c
+
+PLAT_INCLUDES += -I${LOCAL_DIR}
+PLAT_INCLUDES += -I${LOCAL_DIR}/${MTK_SOC}
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/common/lpm/mt_lp_rm.c b/plat/mediatek/drivers/lpm/mt_lp_rm.c
similarity index 96%
rename from plat/mediatek/common/lpm/mt_lp_rm.c
rename to plat/mediatek/drivers/lpm/mt_lp_rm.c
index f3148fe..0bafc66 100644
--- a/plat/mediatek/common/lpm/mt_lp_rm.c
+++ b/plat/mediatek/drivers/lpm/mt_lp_rm.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/mediatek/common/lpm/mt_lp_rm.h b/plat/mediatek/drivers/lpm/mt_lp_rm.h
similarity index 94%
rename from plat/mediatek/common/lpm/mt_lp_rm.h
rename to plat/mediatek/drivers/lpm/mt_lp_rm.h
index 39759f1..e93dac3 100644
--- a/plat/mediatek/common/lpm/mt_lp_rm.h
+++ b/plat/mediatek/drivers/lpm/mt_lp_rm.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/mediatek/mt8192/drivers/pmic/pmic.c b/plat/mediatek/drivers/pmic/pmic.c
similarity index 73%
rename from plat/mediatek/mt8192/drivers/pmic/pmic.c
rename to plat/mediatek/drivers/pmic/pmic.c
index cca4413..a11ad9a 100644
--- a/plat/mediatek/mt8192/drivers/pmic/pmic.c
+++ b/plat/mediatek/drivers/pmic/pmic.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/mediatek/mt8192/drivers/pmic/pmic.h b/plat/mediatek/drivers/pmic/pmic.h
similarity index 63%
rename from plat/mediatek/mt8192/drivers/pmic/pmic.h
rename to plat/mediatek/drivers/pmic/pmic.h
index aac22af..6c10f65 100644
--- a/plat/mediatek/mt8192/drivers/pmic/pmic.h
+++ b/plat/mediatek/drivers/pmic/pmic.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,7 +7,7 @@
 #ifndef PMIC_H
 #define PMIC_H
 
-#define PMIC_PWRHOLD 0xa08
+#define PMIC_PWRHOLD (0xa08)
 
 /* external API */
 void pmic_power_off(void);
diff --git a/plat/mediatek/drivers/pmic/rules.mk b/plat/mediatek/drivers/pmic/rules.mk
new file mode 100644
index 0000000..e408b03
--- /dev/null
+++ b/plat/mediatek/drivers/pmic/rules.mk
@@ -0,0 +1,15 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := pmic
+
+LOCAL_SRCS-y += ${LOCAL_DIR}/pmic.c
+
+PLAT_INCLUDES += -I${LOCAL_DIR}/
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/drivers/pmic_wrap/mt8188/pmic_wrap_init.h b/plat/mediatek/drivers/pmic_wrap/mt8188/pmic_wrap_init.h
new file mode 100644
index 0000000..9027daf
--- /dev/null
+++ b/plat/mediatek/drivers/pmic_wrap/mt8188/pmic_wrap_init.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2022, 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"
+#include <pmic_wrap_init_common.h>
+
+static struct mt8188_pmic_wrap_regs *const mtk_pwrap = (void *)PMIC_WRAP_BASE;
+
+/* PMIC_WRAP registers */
+struct mt8188_pmic_wrap_regs {
+	uint32_t init_done;
+	uint32_t reserved[543];
+	uint32_t wacs2_cmd;
+	uint32_t wacs2_wdata;
+	uint32_t reserved1[3];
+	uint32_t wacs2_rdata;
+	uint32_t reserved2[3];
+	uint32_t wacs2_vldclr;
+	uint32_t wacs2_sta;
+};
+
+#endif /* PMIC_WRAP_INIT_H */
diff --git a/plat/mediatek/common/drivers/pmic_wrap/pmic_wrap_init.c b/plat/mediatek/drivers/pmic_wrap/pmic_wrap_init.c
similarity index 98%
rename from plat/mediatek/common/drivers/pmic_wrap/pmic_wrap_init.c
rename to plat/mediatek/drivers/pmic_wrap/pmic_wrap_init.c
index e3cfd46..0ee1c64 100644
--- a/plat/mediatek/common/drivers/pmic_wrap/pmic_wrap_init.c
+++ b/plat/mediatek/drivers/pmic_wrap/pmic_wrap_init.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2019-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/mediatek/drivers/pmic_wrap/pmic_wrap_init_common.h b/plat/mediatek/drivers/pmic_wrap/pmic_wrap_init_common.h
new file mode 100644
index 0000000..4ba1f5c
--- /dev/null
+++ b/plat/mediatek/drivers/pmic_wrap/pmic_wrap_init_common.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PMIC_WRAP_INIT_COMMON_H
+#define PMIC_WRAP_INIT_COMMON_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);
+
+#define GET_WACS_FSM(x)	((x >> 1) & 0x7)
+
+/* macro for SWINF_FSM */
+#define SWINF_FSM_IDLE		(0x00)
+#define SWINF_FSM_REQ		(0x02)
+#define SWINF_FSM_WFDLE		(0x04)
+#define SWINF_FSM_WFVLDCLR	(0x06)
+#define SWINF_INIT_DONE		(0x01)
+
+/* timeout setting */
+#define PWRAP_READ_US		(1000)
+#define PWRAP_WAIT_IDLE_US	(1000)
+
+/* error information flag */
+enum pwrap_errno {
+	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_COMMON_H */
diff --git a/plat/mediatek/common/drivers/pmic_wrap/pmic_wrap_init_v2.c b/plat/mediatek/drivers/pmic_wrap/pmic_wrap_init_v2.c
similarity index 97%
rename from plat/mediatek/common/drivers/pmic_wrap/pmic_wrap_init_v2.c
rename to plat/mediatek/drivers/pmic_wrap/pmic_wrap_init_v2.c
index d9a79c4..80f55de 100644
--- a/plat/mediatek/common/drivers/pmic_wrap/pmic_wrap_init_v2.c
+++ b/plat/mediatek/drivers/pmic_wrap/pmic_wrap_init_v2.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/mediatek/drivers/pmic_wrap/rules.mk b/plat/mediatek/drivers/pmic_wrap/rules.mk
new file mode 100644
index 0000000..9ba44a6
--- /dev/null
+++ b/plat/mediatek/drivers/pmic_wrap/rules.mk
@@ -0,0 +1,20 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := pmic_wrap
+
+ifeq (${USE_PMIC_WRAP_INIT_V2}, 1)
+LOCAL_SRCS-y += ${LOCAL_DIR}/pmic_wrap_init_v2.c
+else
+LOCAL_SRCS-y += ${LOCAL_DIR}/pmic_wrap_init.c
+endif
+
+PLAT_INCLUDES += -I${LOCAL_DIR}/
+PLAT_INCLUDES += -I${LOCAL_DIR}/${MTK_SOC}
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/drivers/rtc/mt8188/rtc.h b/plat/mediatek/drivers/rtc/mt8188/rtc.h
new file mode 100644
index 0000000..734e89f
--- /dev/null
+++ b/plat/mediatek/drivers/rtc/mt8188/rtc.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef RTC_H
+#define RTC_H
+
+#include <rtc_mt6359p.h>
+
+#endif  /* RTC_H */
diff --git a/plat/mediatek/common/drivers/rtc/rtc_common.c b/plat/mediatek/drivers/rtc/rtc_common.c
similarity index 94%
rename from plat/mediatek/common/drivers/rtc/rtc_common.c
rename to plat/mediatek/drivers/rtc/rtc_common.c
index cad12a0..4efddff 100644
--- a/plat/mediatek/common/drivers/rtc/rtc_common.c
+++ b/plat/mediatek/drivers/rtc/rtc_common.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2019-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/mediatek/common/drivers/rtc/rtc_mt6359p.c b/plat/mediatek/drivers/rtc/rtc_mt6359p.c
similarity index 97%
rename from plat/mediatek/common/drivers/rtc/rtc_mt6359p.c
rename to plat/mediatek/drivers/rtc/rtc_mt6359p.c
index 124bc8f..3bf4337 100644
--- a/plat/mediatek/common/drivers/rtc/rtc_mt6359p.c
+++ b/plat/mediatek/drivers/rtc/rtc_mt6359p.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/mediatek/common/drivers/rtc/rtc_mt6359p.h b/plat/mediatek/drivers/rtc/rtc_mt6359p.h
similarity index 98%
rename from plat/mediatek/common/drivers/rtc/rtc_mt6359p.h
rename to plat/mediatek/drivers/rtc/rtc_mt6359p.h
index 04726e3..199f152 100644
--- a/plat/mediatek/common/drivers/rtc/rtc_mt6359p.h
+++ b/plat/mediatek/drivers/rtc/rtc_mt6359p.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/mediatek/drivers/rtc/rules.mk b/plat/mediatek/drivers/rtc/rules.mk
new file mode 100644
index 0000000..2398f8a
--- /dev/null
+++ b/plat/mediatek/drivers/rtc/rules.mk
@@ -0,0 +1,20 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := rtc
+
+LOCAL_SRCS-y := ${LOCAL_DIR}/rtc_common.c
+
+ifeq (${USE_RTC_MT6359P}, 1)
+LOCAL_SRCS-y += ${LOCAL_DIR}/rtc_mt6359p.c
+PLAT_INCLUDES += -I${LOCAL_DIR}
+endif
+
+PLAT_INCLUDES += -I${LOCAL_DIR}/${MTK_SOC}
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/common/drivers/timer/mt_timer.c b/plat/mediatek/drivers/timer/mt_timer.c
similarity index 72%
rename from plat/mediatek/common/drivers/timer/mt_timer.c
rename to plat/mediatek/drivers/timer/mt_timer.c
index 0860885..11e4572 100644
--- a/plat/mediatek/common/drivers/timer/mt_timer.c
+++ b/plat/mediatek/drivers/timer/mt_timer.c
@@ -1,15 +1,16 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #include <arch_helpers.h>
+#include <common/debug.h>
 #include <lib/mmio.h>
+#include <lib/mtk_init/mtk_init.h>
 #include <mt_timer.h>
 #include <platform_def.h>
 
-
 uint64_t normal_time_base;
 uint64_t atf_time_base;
 
@@ -30,9 +31,14 @@
 	return cval;
 }
 
-void mt_systimer_init(void)
+int mt_systimer_init(void)
 {
+	INFO("[%s] systimer initialization\n", __func__);
+
 	/* Enable access in NS mode */
 	mmio_write_32(CNTWACR_REG, CNT_WRITE_ACCESS_CTL_MASK);
 	mmio_write_32(CNTRACR_REG, CNT_READ_ACCESS_CTL_MASK);
+
+	return 0;
 }
+MTK_PLAT_SETUP_0_INIT(mt_systimer_init);
diff --git a/plat/mediatek/common/drivers/timer/mt_timer.h b/plat/mediatek/drivers/timer/mt_timer.h
similarity index 91%
rename from plat/mediatek/common/drivers/timer/mt_timer.h
rename to plat/mediatek/drivers/timer/mt_timer.h
index b353177..1c08f90 100644
--- a/plat/mediatek/common/drivers/timer/mt_timer.h
+++ b/plat/mediatek/drivers/timer/mt_timer.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -30,6 +30,6 @@
 
 void sched_clock_init(uint64_t normal_base, uint64_t atf_base);
 uint64_t sched_clock(void);
-void mt_systimer_init(void);
+int mt_systimer_init(void);
 
 #endif /* MT_TIMER_H */
diff --git a/plat/mediatek/drivers/timer/rules.mk b/plat/mediatek/drivers/timer/rules.mk
new file mode 100644
index 0000000..005cf45
--- /dev/null
+++ b/plat/mediatek/drivers/timer/rules.mk
@@ -0,0 +1,14 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := timer
+LOCAL_SRCS-y := $(LOCAL_DIR)/mt_timer.c
+
+PLAT_INCLUDES += -I${LOCAL_DIR}
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/common/drivers/uart/8250_console.S b/plat/mediatek/drivers/uart/8250_console.S
similarity index 98%
rename from plat/mediatek/common/drivers/uart/8250_console.S
rename to plat/mediatek/drivers/uart/8250_console.S
index 7a946f9..66f998d 100644
--- a/plat/mediatek/common/drivers/uart/8250_console.S
+++ b/plat/mediatek/drivers/uart/8250_console.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/mediatek/common/drivers/uart/uart.c b/plat/mediatek/drivers/uart/uart.c
similarity index 97%
rename from plat/mediatek/common/drivers/uart/uart.c
rename to plat/mediatek/drivers/uart/uart.c
index b940eb3..fdaa793 100644
--- a/plat/mediatek/common/drivers/uart/uart.c
+++ b/plat/mediatek/drivers/uart/uart.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/mediatek/common/drivers/uart/uart.h b/plat/mediatek/drivers/uart/uart.h
similarity index 97%
rename from plat/mediatek/common/drivers/uart/uart.h
rename to plat/mediatek/drivers/uart/uart.h
index ac8b94d..2ca74fa 100644
--- a/plat/mediatek/common/drivers/uart/uart.h
+++ b/plat/mediatek/drivers/uart/uart.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/mediatek/common/drivers/uart/uart8250.h b/plat/mediatek/drivers/uart/uart8250.h
similarity index 93%
rename from plat/mediatek/common/drivers/uart/uart8250.h
rename to plat/mediatek/drivers/uart/uart8250.h
index da7c7a1..f0541d6 100644
--- a/plat/mediatek/common/drivers/uart/uart8250.h
+++ b/plat/mediatek/drivers/uart/uart8250.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/mediatek/include/cold_boot.h b/plat/mediatek/include/cold_boot.h
new file mode 100644
index 0000000..6b94b57
--- /dev/null
+++ b/plat/mediatek/include/cold_boot.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2022, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef COLD_BOOT_H
+#define COLD_BOOT_H
+
+#include <stdint.h>
+
+/*******************************************************************************
+ * Function and variable prototypes
+ ******************************************************************************/
+#define LINUX_KERNEL_32	(0)
+#define LINUX_KERNEL_64	(1)
+#define DEVINFO_SIZE	(4)
+
+struct atf_arg_t {
+	uint32_t atf_magic;
+	uint32_t tee_support;
+	uint64_t tee_entry;
+	uint64_t tee_boot_arg_addr;
+	uint32_t hwuid[4]; /* HW Unique id for t-base used */
+	uint32_t HRID[8]; /* HW random id for t-base used */
+	uint32_t devinfo[DEVINFO_SIZE];
+};
+
+struct mtk_bl31_fw_config {
+	void *from_bl2; /* MTK boot tag */
+	void *soc_fw_config;
+	void *hw_config;
+	void *reserved;
+};
+
+enum {
+	BOOT_ARG_FROM_BL2,
+	BOOT_ARG_SOC_FW_CONFIG,
+	BOOT_ARG_HW_CONFIG,
+	BOOT_ARG_RESERVED
+};
+
+struct kernel_info {
+	uint64_t pc;
+	uint64_t r0;
+	uint64_t r1;
+	uint64_t r2;
+	uint64_t k32_64;
+};
+
+struct mtk_bl_param_t {
+	uint64_t bootarg_loc;
+	uint64_t bootarg_size;
+	uint64_t bl33_start_addr;
+	uint64_t atf_arg_addr;
+};
+
+void *get_mtk_bl31_fw_config(int index);
+bool is_el1_2nd_bootloader(void);
+
+#endif /* COLD_BOOT_H */
diff --git a/plat/mediatek/include/lib/mtk_init/mtk_init.h b/plat/mediatek/include/lib/mtk_init/mtk_init.h
new file mode 100644
index 0000000..6f23a9b
--- /dev/null
+++ b/plat/mediatek/include/lib/mtk_init/mtk_init.h
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTK_INIT_H
+#define MTK_INIT_H
+
+#include <cdefs.h>
+#include <lib/mtk_init/mtk_init_def.h>
+
+#define INIT_CALL_EXPAND_AS_ENUMERATION(_section_enum, _section_name, _level) \
+	_section_enum = _level,
+
+#define EXPAND_AS_LINK_SECTION(_section_enum, _section_name, _level) \
+	__##_section_enum##_START__ = .; \
+	KEEP(*(_section_name##_level));
+
+#define EXPAND_AS_EXTERN(_section_enum, _section_name, _level) \
+	extern struct initcall __##_section_enum##_START__[];
+
+#define EXPAND_AS_SYMBOL_ARR(_section_enum, _section_name, _level) \
+	__##_section_enum##_START__,
+
+#define DECLARE_MTK_INITCALL(_fn, _level) \
+	const struct initcall _mtk_initcall_##_fn \
+	__used \
+	__aligned(sizeof(void *)) \
+	__section(".mtk_plat_initcall_"#_level) \
+	= { \
+		.name = #_fn, \
+		.fn = _fn \
+	}
+
+/* initcall helpers  */
+#define MTK_EARLY_PLAT_INIT(_fn) DECLARE_MTK_INITCALL(_fn, 0)
+#define MTK_ARCH_INIT(_fn) DECLARE_MTK_INITCALL(_fn, 1)
+#define MTK_PLAT_SETUP_0_INIT(_fn) DECLARE_MTK_INITCALL(_fn, 2)
+#define MTK_PLAT_SETUP_1_INIT(_fn) DECLARE_MTK_INITCALL(_fn, 3)
+#define MTK_PLAT_RUNTIME_INIT(_fn) DECLARE_MTK_INITCALL(_fn, 4)
+#define MTK_PLAT_BL33_DEFER_INIT(_fn) DECLARE_MTK_INITCALL(_fn, 5)
+
+#ifndef __ASSEMBLER__
+struct initcall {
+	const char *name;
+	int (*fn)(void);
+};
+
+enum {
+	INIT_CALL_TABLE(INIT_CALL_EXPAND_AS_ENUMERATION)
+	MTK_INIT_LVL_MAX
+};
+
+void mtk_init_one_level(unsigned int level);
+#endif
+
+#endif /* MTK_INIT_H */
diff --git a/plat/mediatek/include/lib/mtk_init/mtk_init_def.h b/plat/mediatek/include/lib/mtk_init/mtk_init_def.h
new file mode 100644
index 0000000..8aae41d
--- /dev/null
+++ b/plat/mediatek/include/lib/mtk_init/mtk_init_def.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTK_INIT_DEF_H
+#define MTK_INIT_DEF_H
+
+/*
+ * Define init call sections here. _func is for 2nd level expansion, init
+ * section enum, and init section name.
+ */
+#define INIT_CALL_TABLE(_func) \
+	_func(MTK_INIT_LVL_EARLY_PLAT, .mtk_plat_initcall_, 0) \
+	_func(MTK_INIT_LVL_ARCH, .mtk_plat_initcall_, 1) \
+	_func(MTK_INIT_LVL_PLAT_SETUP_0, .mtk_plat_initcall_, 2) \
+	_func(MTK_INIT_LVL_PLAT_SETUP_1, .mtk_plat_initcall_, 3) \
+	_func(MTK_INIT_LVL_PLAT_RUNTIME, .mtk_plat_initcall_, 4) \
+	_func(MTK_INIT_LVL_BL33_DEFER, .mtk_plat_initcall_, 5)
+
+#endif /* MTK_INIT_DEF_H */
diff --git a/plat/mediatek/include/mt8188/platform_def.h b/plat/mediatek/include/mt8188/platform_def.h
new file mode 100644
index 0000000..962d952
--- /dev/null
+++ b/plat/mediatek/include/mt8188/platform_def.h
@@ -0,0 +1,174 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLATFORM_DEF_H
+#define PLATFORM_DEF_H
+
+#define PLAT_PRIMARY_CPU	(0x0)
+
+#define MT_GIC_BASE		(0x0C000000)
+#define MCUCFG_BASE		(0x0C530000)
+#define IO_PHYS			(0x10000000)
+
+/* Aggregate of all devices for MMU mapping */
+#define MTK_DEV_RNG0_BASE	(MT_GIC_BASE)
+#define MTK_DEV_RNG0_SIZE	(0x600000)
+#define MTK_DEV_RNG1_BASE	(IO_PHYS)
+#define MTK_DEV_RNG1_SIZE	(0x10000000)
+
+/*******************************************************************************
+ * GPIO related constants
+ ******************************************************************************/
+#define GPIO_BASE		(IO_PHYS + 0x00005000)
+#define IOCFG_RM_BASE		(IO_PHYS + 0x01C00000)
+#define IOCFG_LT_BASE		(IO_PHYS + 0x01E10000)
+#define IOCFG_LM_BASE		(IO_PHYS + 0x01E20000)
+#define IOCFG_RT_BASE		(IO_PHYS + 0x01EA0000)
+
+/*******************************************************************************
+ * UART related constants
+ ******************************************************************************/
+#define UART0_BASE	(IO_PHYS + 0x01002000)
+#define UART_BAUDRATE	(115200)
+
+/*******************************************************************************
+ * PMIC related constants
+ ******************************************************************************/
+#define PMIC_WRAP_BASE		(IO_PHYS + 0x00024000)
+
+/*******************************************************************************
+ * Infra IOMMU related constants
+ ******************************************************************************/
+#define PERICFG_AO_BASE		(IO_PHYS + 0x01003000)
+#define PERICFG_AO_REG_SIZE	(0x1000)
+
+/*******************************************************************************
+ * 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)
+
+/*******************************************************************************
+ * CIRQ related constants
+ ******************************************************************************/
+#define SYS_CIRQ_BASE		(IO_PHYS + 0x204000)
+#define MD_WDT_IRQ_BIT_ID	(141)
+#define CIRQ_IRQ_NUM		(730)
+#define CIRQ_REG_NUM		(23)
+#define CIRQ_SPI_START		(96)
+
+/*******************************************************************************
+ * MM IOMMU & SMI related constants
+ ******************************************************************************/
+#define SMI_LARB_0_BASE		(IO_PHYS + 0x0c022000)
+#define SMI_LARB_1_BASE		(IO_PHYS + 0x0c023000)
+#define SMI_LARB_2_BASE		(IO_PHYS + 0x0c102000)
+#define SMI_LARB_3_BASE		(IO_PHYS + 0x0c103000)
+#define SMI_LARB_4_BASE		(IO_PHYS + 0x04013000)
+#define SMI_LARB_5_BASE		(IO_PHYS + 0x04f02000)
+#define SMI_LARB_6_BASE		(IO_PHYS + 0x04f03000)
+#define SMI_LARB_7_BASE		(IO_PHYS + 0x04e04000)
+#define SMI_LARB_9_BASE		(IO_PHYS + 0x05001000)
+#define SMI_LARB_10_BASE	(IO_PHYS + 0x05120000)
+#define SMI_LARB_11A_BASE	(IO_PHYS + 0x05230000)
+#define SMI_LARB_11B_BASE	(IO_PHYS + 0x05530000)
+#define SMI_LARB_11C_BASE	(IO_PHYS + 0x05630000)
+#define SMI_LARB_12_BASE	(IO_PHYS + 0x05340000)
+#define SMI_LARB_13_BASE	(IO_PHYS + 0x06001000)
+#define SMI_LARB_14_BASE	(IO_PHYS + 0x06002000)
+#define SMI_LARB_15_BASE	(IO_PHYS + 0x05140000)
+#define SMI_LARB_16A_BASE	(IO_PHYS + 0x06008000)
+#define SMI_LARB_16B_BASE	(IO_PHYS + 0x0600a000)
+#define SMI_LARB_17A_BASE	(IO_PHYS + 0x06009000)
+#define SMI_LARB_17B_BASE	(IO_PHYS + 0x0600b000)
+#define SMI_LARB_19_BASE	(IO_PHYS + 0x0a010000)
+#define SMI_LARB_21_BASE	(IO_PHYS + 0x0802e000)
+#define SMI_LARB_23_BASE	(IO_PHYS + 0x0800d000)
+#define SMI_LARB_27_BASE	(IO_PHYS + 0x07201000)
+#define SMI_LARB_28_BASE	(IO_PHYS + 0x00000000)
+#define SMI_LARB_REG_RNG_SIZE	(0x1000)
+
+/*******************************************************************************
+ * DP related constants
+ ******************************************************************************/
+#define EDP_SEC_BASE		(IO_PHYS + 0x0C504000)
+#define DP_SEC_BASE		(IO_PHYS + 0x0C604000)
+#define EDP_SEC_SIZE		(0x1000)
+#define DP_SEC_SIZE		(0x1000)
+
+/*******************************************************************************
+ * System counter frequency related constants
+ ******************************************************************************/
+#define SYS_COUNTER_FREQ_IN_HZ	(13000000)
+#define SYS_COUNTER_FREQ_IN_MHZ	(13)
+
+/*******************************************************************************
+ * Platform binary types for linking
+ ******************************************************************************/
+#define PLATFORM_LINKER_FORMAT		"elf64-littleaarch64"
+#define PLATFORM_LINKER_ARCH		aarch64
+
+/*******************************************************************************
+ * Generic platform constants
+ ******************************************************************************/
+#define PLATFORM_STACK_SIZE		(0x800)
+
+#define FIRMWARE_WELCOME_STR		"Booting Trusted Firmware\n"
+
+#define PLAT_MAX_PWR_LVL		U(3)
+#define PLAT_MAX_RET_STATE		U(1)
+#define PLAT_MAX_OFF_STATE		U(9)
+
+#define PLATFORM_SYSTEM_COUNT		U(1)
+#define PLATFORM_MCUSYS_COUNT		U(1)
+#define PLATFORM_CLUSTER_COUNT		U(1)
+#define PLATFORM_CLUSTER0_CORE_COUNT	U(8)
+#define PLATFORM_CLUSTER1_CORE_COUNT	U(0)
+
+#define PLATFORM_CORE_COUNT		(PLATFORM_CLUSTER0_CORE_COUNT)
+#define PLATFORM_MAX_CPUS_PER_CLUSTER	U(8)
+
+#define SOC_CHIP_ID			U(0x8188)
+
+/*******************************************************************************
+ * Platform memory map related constants
+ ******************************************************************************/
+#define TZRAM_BASE			(0x54600000)
+#define TZRAM_SIZE			(0x00030000)
+
+/*******************************************************************************
+ * BL31 specific defines.
+ ******************************************************************************/
+/*
+ * Put BL3-1 at the top of the Trusted SRAM (just below the shared memory, if
+ * present). BL31_BASE is calculated using the current BL3-1 debug size plus a
+ * little space for growth.
+ */
+#define BL31_BASE			(TZRAM_BASE + 0x1000)
+#define BL31_LIMIT			(TZRAM_BASE + TZRAM_SIZE)
+
+/*******************************************************************************
+ * Platform specific page table and MMU setup constants
+ ******************************************************************************/
+#define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 32)
+#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 32)
+#define MAX_XLAT_TABLES			(16)
+#define MAX_MMAP_REGIONS		(16)
+
+/*******************************************************************************
+ * Declarations and constants to access the mailboxes safely. Each mailbox is
+ * aligned on the biggest cache line size in the platform. This is known only
+ * to the platform as it might have a combination of integrated and external
+ * caches. Such alignment ensures that two maiboxes do not sit on the same cache
+ * line at any cache level. They could belong to different cpus/clusters &
+ * get written while being protected by different locks causing corruption of
+ * a valid mailbox address.
+ ******************************************************************************/
+#define CACHE_WRITEBACK_SHIFT		(6)
+#define CACHE_WRITEBACK_GRANULE		(1 << CACHE_WRITEBACK_SHIFT)
+
+#endif /* PLATFORM_DEF_H */
diff --git a/plat/mediatek/include/mtk_mmap_pool.h b/plat/mediatek/include/mtk_mmap_pool.h
new file mode 100644
index 0000000..99d1bff
--- /dev/null
+++ b/plat/mediatek/include/mtk_mmap_pool.h
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTK_MMAP_POOL_H
+#define MTK_MMAP_POOL_H
+
+#include <cdefs.h>
+#include <lib/xlat_tables/xlat_tables_compat.h>
+
+struct mtk_mmap_descriptor {
+	const char *mmap_name;
+	const mmap_region_t *mmap_ptr;
+	const uint32_t mmap_size;
+};
+
+#define MTK_MMAP_SECTION \
+	__used \
+	__aligned(sizeof(void *)) \
+	__section(".mtk_mmap_lists")
+
+#define DECLARE_MTK_MMAP_REGIONS(_mmap_array) \
+	static const struct mtk_mmap_descriptor _mtk_mmap_descriptor_##_mmap_array \
+	__used \
+	__aligned(sizeof(void *)) \
+	__section(".mtk_mmap_pool") \
+	= { \
+		.mmap_name = #_mmap_array, \
+		.mmap_ptr = _mmap_array, \
+		.mmap_size = ARRAY_SIZE(_mmap_array) \
+	}
+
+#define MAP_BL_RW MAP_REGION_FLAT( \
+		DATA_START, \
+		BL_END - DATA_START, \
+		MT_MEMORY | MT_RW | MT_SECURE)
+
+#if SEPARATE_CODE_AND_RODATA
+#define MAP_BL_RO \
+	MAP_REGION_FLAT( \
+		BL_CODE_BASE, \
+		BL_CODE_END - BL_CODE_BASE, \
+		MT_CODE | MT_SECURE), \
+	MAP_REGION_FLAT( \
+		BL_RO_DATA_BASE, \
+		BL_RO_DATA_END - BL_RO_DATA_BASE, \
+		MT_RO_DATA | MT_SECURE)
+#else
+#define MAP_BL_RO MAP_REGION_FLAT(BL_CODE_BASE, \
+				  BL_CODE_END - BL_CODE_BASE,	\
+				  MT_CODE | MT_SECURE)
+#endif
+
+void mtk_xlat_init(const mmap_region_t *bl_regions);
+
+#endif /* MTK_MMAP_POOL_H */
diff --git a/plat/mediatek/include/mtk_sip_def.h b/plat/mediatek/include/mtk_sip_def.h
new file mode 100644
index 0000000..d760101
--- /dev/null
+++ b/plat/mediatek/include/mtk_sip_def.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTK_SIP_DEF_H
+#define MTK_SIP_DEF_H
+
+/* Define SiP SMC ID here */
+#define MTK_SIP_SMC_FROM_NS_EL1_TABLE(_func) \
+	_func(MTK_SIP_KERNEL_TIME_SYNC, 0x202) \
+	_func(MTK_SIP_VCORE_CONTROL, 0x506) \
+	_func(MTK_SIP_IOMMU_CONTROL, 0x514) \
+	_func(MTK_SIP_APUSYS_CONTROL, 0x51E) \
+	_func(MTK_SIP_DP_CONTROL, 0x523) \
+	_func(MTK_SIP_KERNEL_GIC_OP, 0x526)
+
+#define MTK_SIP_SMC_FROM_BL33_TABLE(_func) \
+	_func(MTK_SIP_KERNEL_BOOT, 0x115)
+
+#endif /* MTK_SIP_DEF_H */
diff --git a/plat/mediatek/include/mtk_sip_svc.h b/plat/mediatek/include/mtk_sip_svc.h
new file mode 100644
index 0000000..ce51048
--- /dev/null
+++ b/plat/mediatek/include/mtk_sip_svc.h
@@ -0,0 +1,106 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTK_SIP_SVC_H
+#define MTK_SIP_SVC_H
+
+#include <stdint.h>
+#include <lib/smccc.h>
+#include <mtk_sip_def.h>
+
+/* SMC function IDs for SiP Service queries */
+#define SIP_SVC_CALL_COUNT		U(0x8200ff00)
+#define SIP_SVC_UID			U(0x8200ff01)
+/* 0x8200ff02 is reserved */
+#define SIP_SVC_VERSION			U(0x8200ff03)
+
+/* MediaTek SiP Service Calls version numbers */
+#define MTK_SIP_SVC_VERSION_MAJOR	U(0x0)
+#define MTK_SIP_SVC_VERSION_MINOR	U(0x1)
+
+/* Number of MediaTek SiP Calls implemented */
+#define MTK_COMMON_SIP_NUM_CALLS	U(4)
+
+/* MediaTek SiP Service Calls function IDs */
+#define MTK_SIP_SET_AUTHORIZED_SECURE_REG	U(0x82000001)
+
+#define SMC_ID_EXPAND_AS_ENUM(_smc_id, _smc_num) \
+	_smc_id##_AARCH32 = ((SMC_TYPE_FAST << FUNCID_TYPE_SHIFT) | \
+		 ((0) << FUNCID_CC_SHIFT) | \
+		 (OEN_SIP_START << FUNCID_OEN_SHIFT) | \
+		 ((_smc_num) << FUNCID_NUM_SHIFT)), \
+	_smc_id##_AARCH64 = ((SMC_TYPE_FAST << FUNCID_TYPE_SHIFT) | \
+		 ((1) << FUNCID_CC_SHIFT) | \
+		 (OEN_SIP_START << FUNCID_OEN_SHIFT) | \
+		 ((_smc_num) << FUNCID_NUM_SHIFT)),
+
+#define SMC_ID_EXPAND_AS_EXTERN_SMC_INDEX(_smc_id, _smc_num) \
+	extern short _smc_id##_descriptor_index;
+
+/* Bind SMC handler with SMC ID */
+#define DECLARE_SMC_HANDLER(_smc_id, _smc_handler) \
+	const struct smc_descriptor _smc_id##_descriptor \
+		__used \
+		__aligned(sizeof(void *)) \
+		__section(".mtk_smc_descriptor_pool") = { \
+			.smc_handler = _smc_handler, \
+			.smc_name = #_smc_id, \
+			.smc_id_aarch32 = _smc_id##_AARCH32, \
+			.smc_id_aarch64 = _smc_id##_AARCH64, \
+			.smc_descriptor_index = &_smc_id##_descriptor_index \
+		}
+
+MTK_SIP_SMC_FROM_BL33_TABLE(SMC_ID_EXPAND_AS_EXTERN_SMC_INDEX);
+MTK_SIP_SMC_FROM_NS_EL1_TABLE(SMC_ID_EXPAND_AS_EXTERN_SMC_INDEX);
+
+/* Expand SiP SMC ID table as enum */
+enum {
+	MTK_SIP_SMC_FROM_BL33_TABLE(SMC_ID_EXPAND_AS_ENUM)
+	MTK_SIP_SMC_FROM_NS_EL1_TABLE(SMC_ID_EXPAND_AS_ENUM)
+	MTK_SIP_SMC_MAX_NUMBER
+};
+
+/* MediaTek SiP Calls error code */
+enum {
+	MTK_SIP_E_SUCCESS = 0,
+	MTK_SIP_E_INVALID_PARAM = -1,
+	MTK_SIP_E_NOT_SUPPORTED = -2,
+	MTK_SIP_E_INVALID_RANGE = -3,
+	MTK_SIP_E_PERMISSION_DENY = -4,
+	MTK_SIP_E_LOCK_FAIL = -5,
+};
+
+struct smccc_res {
+	uint64_t a1;
+	uint64_t a2;
+	uint64_t a3;
+};
+
+typedef uintptr_t (*smc_handler_t)(u_register_t,
+				   u_register_t,
+				   u_register_t,
+				   u_register_t,
+				   void *,
+				   struct smccc_res *);
+
+struct smc_descriptor {
+	smc_handler_t smc_handler;
+	const uint32_t smc_id_aarch32;
+	const uint32_t smc_id_aarch64;
+	const char *smc_name;
+	short *const smc_descriptor_index;
+};
+
+/*
+ * This function should be implemented in MediaTek SOC directory. It fullfills
+ * MTK_SIP_SET_AUTHORIZED_SECURE_REG SiP call by checking the sreg with the
+ * predefined secure register list, if a match was found, set val to sreg.
+ *
+ * Return MTK_SIP_E_SUCCESS on success, and MTK_SIP_E_INVALID_PARAM on failure.
+ */
+uint64_t mt_sip_set_authorized_sreg(uint32_t sreg, uint32_t val);
+
+#endif /* MTK_SIP_SVC_H */
diff --git a/plat/mediatek/include/plat.ld.rodata.inc b/plat/mediatek/include/plat.ld.rodata.inc
new file mode 100644
index 0000000..06ad491
--- /dev/null
+++ b/plat/mediatek/include/plat.ld.rodata.inc
@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_LD_RODATA_INC
+#define PLAT_LD_RODATA_INC
+
+#include <lib/mtk_init/mtk_init.h>
+	. = ALIGN(32);
+	INIT_CALL_TABLE(EXPAND_AS_LINK_SECTION);
+	__MTK_PLAT_INITCALL_END__ = .;
+	. = ALIGN(32);
+	__MTK_MMAP_POINTER_POOL_START__ = .;
+	KEEP(*(.mtk_mmap_pool))
+	__MTK_MMAP_POINTER_POOL_END_UNALIGNED__ = .;
+	. = ALIGN(8);
+	__MTK_MMAP_POOL_START__ = .;
+	KEEP(*(.mtk_mmap_lists))
+	__MTK_MMAP_POOL_END_UNALIGNED__ = .;
+	. = ALIGN(32);
+	__MTK_SMC_POOL_START__ = .;
+	KEEP(*(.mtk_smc_descriptor_pool))
+	__MTK_SMC_POOL_END_UNALIGNED__ = .;
+	. = ALIGN(8);
+#include <vendor_pubsub_events.h>
+	*(mtk_plat_ro)
+
+#endif /* PLAT_LD_RODATA_INC */
diff --git a/plat/mediatek/include/vendor_pubsub_events.h b/plat/mediatek/include/vendor_pubsub_events.h
new file mode 100644
index 0000000..cb8d878
--- /dev/null
+++ b/plat/mediatek/include/vendor_pubsub_events.h
@@ -0,0 +1,21 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef VENDOR_PUBSUB_EVENTS_H
+#define VENDOR_PUBSUB_EVENTS_H
+
+#include <lib/el3_runtime/pubsub.h>
+
+REGISTER_PUBSUB_EVENT(lpm_publish_event);
+REGISTER_PUBSUB_EVENT(suspend_publish_event);
+REGISTER_PUBSUB_EVENT(mt_cpupm_publish_pwr_on);
+REGISTER_PUBSUB_EVENT(mt_cpupm_publish_pwr_off);
+REGISTER_PUBSUB_EVENT(mt_cpupm_publish_afflv_pwr_on);
+REGISTER_PUBSUB_EVENT(mt_cpupm_publish_afflv_pwr_off);
+REGISTER_PUBSUB_EVENT(publish_check_wakeup_irq);
+REGISTER_PUBSUB_EVENT(watchdog_timeout);
+
+#endif /* VENDOR_PUBSUB_EVENTS_H */
diff --git a/plat/mediatek/lib/mtk_init/mtk_init.c b/plat/mediatek/lib/mtk_init/mtk_init.c
new file mode 100644
index 0000000..2289659
--- /dev/null
+++ b/plat/mediatek/lib/mtk_init/mtk_init.c
@@ -0,0 +1,39 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <lib/utils_def.h>
+#include <lib/mtk_init/mtk_init.h>
+
+INIT_CALL_TABLE(EXPAND_AS_EXTERN);
+extern struct initcall __MTK_PLAT_INITCALL_END__[];
+
+struct initcall *initcall_list[] = {
+	INIT_CALL_TABLE(EXPAND_AS_SYMBOL_ARR)
+	__MTK_PLAT_INITCALL_END__
+};
+
+void mtk_init_one_level(uint32_t level)
+{
+	const struct initcall *entry;
+	int error;
+
+	if (level >= MTK_INIT_LVL_MAX) {
+		ERROR("invalid level:%u\n", level);
+		panic();
+	}
+
+	INFO("init calling level:%u\n", level);
+	for (entry = initcall_list[level];
+	     (entry != NULL) && (entry < initcall_list[level + 1]);
+	     entry++) {
+		INFO("calling %s\n", entry->name);
+		error = entry->fn();
+		if (error != 0) {
+			ERROR("init %s fail, errno:%d\n", entry->name, error);
+		}
+	}
+}
diff --git a/plat/mediatek/lib/mtk_init/mtk_mmap_init.c b/plat/mediatek/lib/mtk_init/mtk_mmap_init.c
new file mode 100644
index 0000000..e3dada0
--- /dev/null
+++ b/plat/mediatek/lib/mtk_init/mtk_mmap_init.c
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/console.h>
+#include <lib/xlat_tables/xlat_tables_compat.h>
+#include <mtk_mmap_pool.h>
+
+IMPORT_SYM(uintptr_t, __MTK_MMAP_POINTER_POOL_START__, MTK_MMAP_POINTER_POOL_START);
+IMPORT_SYM(uintptr_t, __MTK_MMAP_POINTER_POOL_END_UNALIGNED__, MTK_MMAP_POINTER_POOL_END_UNALIGNED);
+IMPORT_SYM(uintptr_t, __RW_START__, RW_START);
+IMPORT_SYM(uintptr_t, __DATA_START__, DATA_START);
+
+#define MAP_MTK_SECTIONS MAP_REGION_FLAT(RW_START, \
+					 DATA_START - RW_START, \
+					 MT_MEMORY | MT_RO | MT_SECURE)
+
+
+static void print_mmap(const mmap_region_t *regions)
+{
+	while (regions->size != 0U) {
+		VERBOSE("Region: 0x%lx - 0x%lx has attributes 0x%x\n",
+			regions->base_va,
+			regions->base_va + regions->size,
+			regions->attr);
+		regions++;
+	}
+}
+
+void mtk_xlat_init(const mmap_region_t *bl_regions)
+{
+	struct mtk_mmap_descriptor *iter;
+	const mmap_region_t *regions = bl_regions;
+
+	print_mmap(regions);
+	mmap_add(bl_regions);
+	if (MTK_MMAP_POINTER_POOL_START != MTK_MMAP_POINTER_POOL_END_UNALIGNED) {
+		for (iter = (struct mtk_mmap_descriptor *)MTK_MMAP_POINTER_POOL_START;
+		     (char *)iter < (char *)MTK_MMAP_POINTER_POOL_END_UNALIGNED;
+		     iter++) {
+			regions = iter->mmap_ptr;
+			INFO("mmap_name: %s\n", iter->mmap_name);
+			INFO("mmap_size: 0x%x\n", iter->mmap_size);
+			print_mmap(regions);
+			mmap_add(regions);
+		}
+	}
+	init_xlat_tables();
+	enable_mmu_el3(0);
+}
diff --git a/plat/mediatek/lib/mtk_init/rules.mk b/plat/mediatek/lib/mtk_init/rules.mk
new file mode 100644
index 0000000..cc6ca95
--- /dev/null
+++ b/plat/mediatek/lib/mtk_init/rules.mk
@@ -0,0 +1,14 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := mtk_init
+
+LOCAL_SRCS-y := $(LOCAL_DIR)/mtk_init.c
+LOCAL_SRCS-y += $(LOCAL_DIR)/mtk_mmap_init.c
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/lib/pm/mtk_pm.c b/plat/mediatek/lib/pm/mtk_pm.c
new file mode 100644
index 0000000..632a1e7
--- /dev/null
+++ b/plat/mediatek/lib/pm/mtk_pm.c
@@ -0,0 +1,18 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/psci/psci.h>
+
+static const plat_psci_ops_t plat_psci_ops = {
+};
+
+int plat_setup_psci_ops(uintptr_t sec_entrypoint,
+			const plat_psci_ops_t **psci_ops)
+{
+	*psci_ops = &plat_psci_ops;
+
+	return 0;
+}
diff --git a/plat/mediatek/lib/pm/rules.mk b/plat/mediatek/lib/pm/rules.mk
new file mode 100644
index 0000000..77d0408
--- /dev/null
+++ b/plat/mediatek/lib/pm/rules.mk
@@ -0,0 +1,14 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := pm
+
+LOCAL_SRCS-y := ${LOCAL_DIR}/mtk_pm.c
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/mt8173/platform.mk b/plat/mediatek/mt8173/platform.mk
index f62802c..6bf1aa7 100644
--- a/plat/mediatek/mt8173/platform.mk
+++ b/plat/mediatek/mt8173/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015-2017, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -8,6 +8,7 @@
 MTK_PLAT_SOC		:=	${MTK_PLAT}/${PLAT}
 
 PLAT_INCLUDES		:=	-I${MTK_PLAT}/common/				\
+				-I${MTK_PLAT}/include/				\
 				-Iinclude/plat/arm/common/aarch64		\
 				-I${MTK_PLAT_SOC}/drivers/crypt/		\
 				-I${MTK_PLAT_SOC}/drivers/mtcmos/		\
@@ -36,10 +37,10 @@
 				lib/cpus/aarch64/cortex_a53.S			\
 				lib/cpus/aarch64/cortex_a57.S			\
 				lib/cpus/aarch64/cortex_a72.S			\
-				${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}/drivers/pmic_wrap/pmic_wrap_init.c	\
+				${MTK_PLAT}/drivers/rtc/rtc_common.c		\
 				${MTK_PLAT_SOC}/aarch64/plat_helpers.S		\
 				${MTK_PLAT_SOC}/aarch64/platform_common.c	\
 				${MTK_PLAT_SOC}/bl31_plat_setup.c		\
diff --git a/plat/mediatek/mt8183/platform.mk b/plat/mediatek/mt8183/platform.mk
index 1615cf9..a737d24 100644
--- a/plat/mediatek/mt8183/platform.mk
+++ b/plat/mediatek/mt8183/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2019, MediaTek Inc. All rights reserved.
+# Copyright (c) 2019-2022, MediaTek Inc. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -8,7 +8,8 @@
 MTK_PLAT_SOC  := ${MTK_PLAT}/${PLAT}
 
 PLAT_INCLUDES := -I${MTK_PLAT}/common/                            \
-                 -I${MTK_PLAT}/common/drivers/uart/                \
+                 -I${MTK_PLAT}/drivers/uart/                      \
+                 -I${MTK_PLAT}/include/                           \
                  -I${MTK_PLAT_SOC}/drivers/                       \
                  -I${MTK_PLAT_SOC}/drivers/emi_mpu/               \
                  -I${MTK_PLAT_SOC}/drivers/devapc/                \
@@ -43,10 +44,10 @@
                    lib/cpus/aarch64/cortex_a73.S                         \
                    plat/common/plat_gicv3.c                              \
                    ${MTK_PLAT}/common/mtk_plat_common.c                  \
-                   ${MTK_PLAT}/common/drivers/pmic_wrap/pmic_wrap_init.c \
-                   ${MTK_PLAT}/common/drivers/rtc/rtc_common.c           \
-                   ${MTK_PLAT}/common/drivers/uart/uart.c                \
                    ${MTK_PLAT}/common/params_setup.c                     \
+                   ${MTK_PLAT}/drivers/pmic_wrap/pmic_wrap_init.c        \
+                   ${MTK_PLAT}/drivers/rtc/rtc_common.c                  \
+                   ${MTK_PLAT}/drivers/uart/uart.c                       \
                    ${MTK_PLAT_SOC}/aarch64/plat_helpers.S                \
                    ${MTK_PLAT_SOC}/aarch64/platform_common.c             \
                    ${MTK_PLAT_SOC}/drivers/devapc/devapc.c               \
diff --git a/plat/mediatek/mt8186/drivers/mcdi/mt_lp_irqremain.c b/plat/mediatek/mt8186/drivers/mcdi/mt_lp_irqremain.c
index 42b2808..b5a0284 100644
--- a/plat/mediatek/mt8186/drivers/mcdi/mt_lp_irqremain.c
+++ b/plat/mediatek/mt8186/drivers/mcdi/mt_lp_irqremain.c
@@ -4,9 +4,9 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <mt_cirq.h>
 #include <mt_lp_irqremain.h>
 #include <mt_lp_rm.h>
-#include <mtk_cirq.h>
 #include <plat_mtk_lpm.h>
 
 #define KEYPAD_IRQ_ID		U(138)
diff --git a/plat/mediatek/mt8186/drivers/pmic/pmic_wrap_init.h b/plat/mediatek/mt8186/drivers/pmic/pmic_wrap_init.h
index e837456..e8cbf7e 100644
--- a/plat/mediatek/mt8186/drivers/pmic/pmic_wrap_init.h
+++ b/plat/mediatek/mt8186/drivers/pmic/pmic_wrap_init.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2021-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,10 +10,7 @@
 #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);
+#include <pmic_wrap_init_common.h>
 
 static struct mt8186_pmic_wrap_regs *const mtk_pwrap = (void *)PMIC_WRAP_BASE;
 
@@ -61,34 +58,4 @@
 	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/spm/constraints/mt_spm_rc_bus26m.c b/plat/mediatek/mt8186/drivers/spm/constraints/mt_spm_rc_bus26m.c
index 66fbe91..dd2aee8 100644
--- a/plat/mediatek/mt8186/drivers/spm/constraints/mt_spm_rc_bus26m.c
+++ b/plat/mediatek/mt8186/drivers/spm/constraints/mt_spm_rc_bus26m.c
@@ -26,7 +26,7 @@
 #include <mt_spm_suspend.h>
 
 #ifndef ATF_PLAT_CIRQ_UNSUPPORT
-#include <mtk_cirq.h>
+#include <mt_cirq.h>
 #endif
 
 #include <plat_mtk_lpm.h>
diff --git a/plat/mediatek/mt8186/plat_sip_calls.c b/plat/mediatek/mt8186/plat_sip_calls.c
index cb66218..d9144d8 100644
--- a/plat/mediatek/mt8186/plat_sip_calls.c
+++ b/plat/mediatek/mt8186/plat_sip_calls.c
@@ -23,8 +23,8 @@
 	uint64_t ret;
 
 	switch (smc_fid) {
-	case MTK_SIP_VCORE_CONTROL_ARCH32:
-	case MTK_SIP_VCORE_CONTROL_ARCH64:
+	case MTK_SIP_VCORE_CONTROL_AARCH32:
+	case MTK_SIP_VCORE_CONTROL_AARCH64:
 		ret = spm_vcorefs_args(x1, x2, x3, (uint64_t *)&x4);
 		SMC_RET2(handle, ret, x4);
 		break;
diff --git a/plat/mediatek/mt8186/platform.mk b/plat/mediatek/mt8186/platform.mk
index b6d9ca8..8717d7d 100644
--- a/plat/mediatek/mt8186/platform.mk
+++ b/plat/mediatek/mt8186/platform.mk
@@ -8,16 +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/uart/               \
-                 -I${MTK_PLAT}/common/drivers/timer/              \
-		 -I${MTK_PLAT}/common/lpm/                        \
+                 -I${MTK_PLAT}/drivers/cirq/                      \
+                 -I${MTK_PLAT}/drivers/gic600/                    \
+                 -I${MTK_PLAT}/drivers/gpio/                      \
+                 -I${MTK_PLAT}/drivers/lpm/                       \
+                 -I${MTK_PLAT}/drivers/pmic_wrap/                 \
+                 -I${MTK_PLAT}/drivers/timer/                     \
+                 -I${MTK_PLAT}/drivers/uart/                      \
+                 -I${MTK_PLAT}/include/                           \
                  -I${MTK_PLAT_SOC}/drivers/spm/                   \
                  -I${MTK_PLAT_SOC}/drivers/dcm/                   \
-                 -I${MTK_PLAT_SOC}/drivers/dfd/                    \
+                 -I${MTK_PLAT_SOC}/drivers/dfd/                   \
                  -I${MTK_PLAT_SOC}/drivers/emi_mpu/               \
-                 -I${MTK_PLAT_SOC}/drivers/gpio/               \
+                 -I${MTK_PLAT_SOC}/drivers/gpio/                  \
                  -I${MTK_PLAT_SOC}/drivers/mcdi/                  \
                  -I${MTK_PLAT_SOC}/drivers/pmic/                  \
                  -I${MTK_PLAT_SOC}/drivers/rtc/                   \
@@ -43,17 +46,17 @@
                 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/drivers/uart/uart.c                \
-                ${MTK_PLAT}/common/mtk_cirq.c           	      \
-		${MTK_PLAT}/common/lpm/mt_lp_rm.c                     \
+                ${MTK_PLAT}/drivers/cirq/mt_cirq.c                    \
+                ${MTK_PLAT}/drivers/gic600/mt_gic_v3.c                \
+                ${MTK_PLAT}/drivers/gpio/mtgpio_common.c              \
+                ${MTK_PLAT}/drivers/lpm/mt_lp_rm.c                    \
+                ${MTK_PLAT}/drivers/pmic_wrap/pmic_wrap_init.c        \
+                ${MTK_PLAT}/drivers/rtc/rtc_common.c                  \
+                ${MTK_PLAT}/drivers/timer/mt_timer.c                  \
+                ${MTK_PLAT}/drivers/uart/uart.c                       \
                 ${MTK_PLAT_SOC}/aarch64/platform_common.c             \
                 ${MTK_PLAT_SOC}/aarch64/plat_helpers.S                \
                 ${MTK_PLAT_SOC}/bl31_plat_setup.c                     \
diff --git a/plat/mediatek/mt8188/aarch64/plat_helpers.S b/plat/mediatek/mt8188/aarch64/plat_helpers.S
new file mode 100644
index 0000000..7073ab1
--- /dev/null
+++ b/plat/mediatek/mt8188/aarch64/plat_helpers.S
@@ -0,0 +1,45 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <platform_def.h>
+
+	.globl plat_is_my_cpu_primary
+	.globl plat_my_core_pos
+	.globl plat_mediatek_calc_core_pos
+
+func plat_is_my_cpu_primary
+	mrs	x0, mpidr_el1
+	and	x0, x0, #(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK)
+	cmp	x0, #PLAT_PRIMARY_CPU
+	cset	x0, eq
+	ret
+endfunc plat_is_my_cpu_primary
+
+	/* -----------------------------------------------------
+	 *  unsigned int plat_my_core_pos(void)
+	 *  This function uses the plat_mediatek_calc_core_pos()
+	 *  definition to get the index of the calling CPU.
+	 * -----------------------------------------------------
+	 */
+func plat_my_core_pos
+	mrs	x0, mpidr_el1
+	b	plat_mediatek_calc_core_pos
+endfunc plat_my_core_pos
+
+	/* -----------------------------------------------------
+	 * unsigned int plat_mediatek_calc_core_pos(u_register_t mpidr);
+	 *
+	 * With this function: CorePos = CoreID (AFF1)
+	 * we do it with x0 = (x0 >> 8) & 0xff
+	 * -----------------------------------------------------
+	 */
+func plat_mediatek_calc_core_pos
+	mov	x1, #MPIDR_AFFLVL_MASK
+	and	x0, x1, x0, lsr #MPIDR_AFF1_SHIFT
+	ret
+endfunc plat_mediatek_calc_core_pos
diff --git a/plat/mediatek/mt8188/include/plat_helpers.h b/plat/mediatek/mt8188/include/plat_helpers.h
new file mode 100644
index 0000000..eb78623
--- /dev/null
+++ b/plat/mediatek/mt8188/include/plat_helpers.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_HELPERS_H
+#define PLAT_HELPERS_H
+
+unsigned int plat_mediatek_calc_core_pos(u_register_t mpidr);
+
+#endif /* PLAT_HELPERS_H */
diff --git a/plat/mediatek/mt8188/include/plat_macros.S b/plat/mediatek/mt8188/include/plat_macros.S
new file mode 100644
index 0000000..a6e05a9
--- /dev/null
+++ b/plat/mediatek/mt8188/include/plat_macros.S
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_MACROS_S
+#define PLAT_MACROS_S
+
+#include <platform_def.h>
+
+.section .rodata.gic_reg_name, "aS"
+gicc_regs:
+	.asciz "gicc_hppir", "gicc_ahppir", "gicc_ctlr", ""
+gicd_pend_reg:
+	.asciz "gicd_ispendr regs (Offsets 0x200 - 0x278)\n"	\
+		" Offset:\t\t\tvalue\n"
+newline:
+	.asciz "\n"
+spacer:
+	.asciz ":\t\t0x"
+
+.section .rodata.cci_reg_name, "aS"
+cci_iface_regs:
+	.asciz "cci_snoop_ctrl_cluster0", "cci_snoop_ctrl_cluster1" , ""
+
+	/* ---------------------------------------------
+	 * The below macro prints out relevant GIC
+	 * registers whenever an unhandled exception
+	 * is taken in BL31.
+	 * Clobbers: x0 - x10, x26, x27, sp
+	 * ---------------------------------------------
+	 */
+	.macro plat_crash_print_regs
+	/* TODO: leave implementation to GIC owner */
+	.endm
+
+#endif /* PLAT_MACROS_S */
diff --git a/plat/mediatek/mt8188/include/plat_private.h b/plat/mediatek/mt8188/include/plat_private.h
new file mode 100644
index 0000000..4d4ac85
--- /dev/null
+++ b/plat/mediatek/mt8188/include/plat_private.h
@@ -0,0 +1,18 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_PRIVATE_H
+#define PLAT_PRIVATE_H
+
+/*******************************************************************************
+ * Function and variable prototypes
+ ******************************************************************************/
+void plat_configure_mmu_el3(uintptr_t total_base,
+			    uintptr_t total_size,
+			    uintptr_t ro_start,
+			    uintptr_t ro_limit);
+
+#endif /* PLAT_PRIVATE_H */
diff --git a/plat/mediatek/mt8188/plat_config.mk b/plat/mediatek/mt8188/plat_config.mk
new file mode 100644
index 0000000..0c1e7cd
--- /dev/null
+++ b/plat/mediatek/mt8188/plat_config.mk
@@ -0,0 +1,34 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# Separate text code and read only data
+SEPARATE_CODE_AND_RODATA := 1
+# ARMv8.2 and above need enable HW assist coherence
+HW_ASSISTED_COHERENCY := 1
+# No need coherency memory because of HW assistency
+USE_COHERENT_MEM := 0
+# GIC600
+GICV3_SUPPORT_GIC600 := 1
+#
+# MTK options
+#
+PLAT_EXTRA_RODATA_INCLUDES := 1
+USE_PMIC_WRAP_INIT_V2 := 1
+USE_RTC_MT6359P := 1
+
+# Configs for A78 and A55
+CTX_INCLUDE_AARCH32_REGS := 0
+ERRATA_A55_1530923 := 1
+ERRATA_A55_1221012 := 1
+ERRATA_A78_1688305 := 1
+ERRATA_A78_1941498 := 1
+ERRATA_A78_1951500 := 1
+ERRATA_A78_1821534 := 1
+ERRATA_A78_2132060 := 1
+ERRATA_A78_2242635 := 1
+
+MACH_MT8188 := 1
+$(eval $(call add_define,MACH_MT8188))
diff --git a/plat/mediatek/mt8188/plat_mmap.c b/plat/mediatek/mt8188/plat_mmap.c
new file mode 100644
index 0000000..0d4cbe8
--- /dev/null
+++ b/plat/mediatek/mt8188/plat_mmap.c
@@ -0,0 +1,18 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include <mtk_mmap_pool.h>
+#include <platform_def.h>
+
+static const mmap_region_t plat_mmap[] = {
+	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),
+	{ 0 }
+};
+DECLARE_MTK_MMAP_REGIONS(plat_mmap);
diff --git a/plat/mediatek/mt8188/plat_topology.c b/plat/mediatek/mt8188/plat_topology.c
new file mode 100644
index 0000000..9fa2855
--- /dev/null
+++ b/plat/mediatek/mt8188/plat_topology.c
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <lib/psci/psci.h>
+
+#include <plat_helpers.h>
+#include <platform_def.h>
+
+const unsigned char mtk_power_domain_tree_desc[] = {
+	/* Number of root nodes */
+	PLATFORM_SYSTEM_COUNT,
+	/* Number of children for the root node */
+	PLATFORM_MCUSYS_COUNT,
+	/* Number of children for the mcusys node */
+	PLATFORM_CLUSTER_COUNT,
+	/* Number of children for the first cluster node */
+	PLATFORM_CLUSTER0_CORE_COUNT,
+};
+
+const unsigned char *plat_get_power_domain_tree_desc(void)
+{
+	return mtk_power_domain_tree_desc;
+}
+
+/*******************************************************************************
+ * This function implements a part of the critical interface between the psci
+ * generic layer and the platform that allows the former to query the platform
+ * to convert an MPIDR to a unique linear index. An error code (-1) is returned
+ * in case the MPIDR is invalid.
+ ******************************************************************************/
+int plat_core_pos_by_mpidr(u_register_t mpidr)
+{
+	unsigned int cluster_id, cpu_id;
+
+	if ((read_mpidr() & MPIDR_MT_MASK) != 0) {
+		/* ARMv8.2 arch */
+		if ((mpidr & (MPIDR_AFFLVL_MASK << MPIDR_AFF0_SHIFT)) != 0) {
+			return -1;
+		}
+		return plat_mediatek_calc_core_pos(mpidr);
+	}
+
+	mpidr &= MPIDR_AFFINITY_MASK;
+
+	if ((mpidr & ~(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK)) != 0) {
+		return -1;
+	}
+
+	cluster_id = (mpidr >> MPIDR_AFF1_SHIFT) & MPIDR_AFFLVL_MASK;
+	cpu_id = (mpidr >> MPIDR_AFF0_SHIFT) & MPIDR_AFFLVL_MASK;
+
+	if (cluster_id >= PLATFORM_CLUSTER_COUNT) {
+		return -1;
+	}
+
+	/*
+	 * Validate cpu_id by checking whether it represents a CPU in
+	 * one of the two clusters present on the platform.
+	 */
+	if (cpu_id >= PLATFORM_MAX_CPUS_PER_CLUSTER) {
+		return -1;
+	}
+
+	return (cpu_id + (cluster_id * 8));
+}
diff --git a/plat/mediatek/mt8188/platform.mk b/plat/mediatek/mt8188/platform.mk
new file mode 100644
index 0000000..84bcfac
--- /dev/null
+++ b/plat/mediatek/mt8188/platform.mk
@@ -0,0 +1,56 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+MTK_PLAT := plat/mediatek
+MTK_PLAT_SOC := ${MTK_PLAT}/${PLAT}
+MTK_SOC := ${PLAT}
+
+include plat/mediatek/build_helpers/mtk_build_helpers.mk
+include drivers/arm/gic/v3/gicv3.mk
+include lib/xlat_tables_v2/xlat_tables.mk
+
+PLAT_INCLUDES := -I${MTK_PLAT}/common \
+		 -I${MTK_PLAT}/include \
+		 -I${MTK_PLAT}/include/${MTK_SOC} \
+		 -I${MTK_PLAT} \
+		 -I${MTK_PLAT_SOC}/include \
+		 -Idrivers/arm/gic \
+
+MODULES-y += $(MTK_PLAT)/common
+MODULES-y += $(MTK_PLAT)/lib/mtk_init
+MODULES-y += $(MTK_PLAT)/lib/pm
+MODULES-y += $(MTK_PLAT)/drivers/cirq
+MODULES-y += $(MTK_PLAT)/drivers/dp
+MODULES-y += $(MTK_PLAT)/drivers/gic600
+MODULES-y += $(MTK_PLAT)/drivers/gpio
+MODULES-y += $(MTK_PLAT)/drivers/iommu
+MODULES-y += $(MTK_PLAT)/drivers/pmic
+MODULES-y += $(MTK_PLAT)/drivers/pmic_wrap
+MODULES-y += $(MTK_PLAT)/drivers/rtc
+MODULES-y += $(MTK_PLAT)/drivers/timer
+
+PLAT_BL_COMMON_SOURCES := common/desc_image_load.c \
+			  drivers/ti/uart/aarch64/16550_console.S \
+			  lib/bl_aux_params/bl_aux_params.c
+
+BL31_SOURCES += drivers/delay_timer/delay_timer.c \
+		drivers/delay_timer/generic_delay_timer.c \
+		lib/cpus/aarch64/cortex_a55.S \
+		lib/cpus/aarch64/cortex_a78.S \
+		${GICV3_SOURCES} \
+		${XLAT_TABLES_LIB_SRCS} \
+		plat/common/plat_gicv3.c \
+		plat/common/plat_psci_common.c \
+		plat/common/aarch64/crash_console_helpers.S \
+		${MTK_PLAT}/common/mtk_plat_common.c \
+		${MTK_PLAT}/common/params_setup.c \
+		${MTK_PLAT_SOC}/aarch64/plat_helpers.S \
+		$(MTK_PLAT)/$(MTK_SOC)/plat_mmap.c \
+		$(MTK_PLAT)/$(MTK_SOC)/plat_topology.c
+
+include plat/mediatek/build_helpers/mtk_build_helpers_epilogue.mk
+
+include lib/coreboot/coreboot.mk
diff --git a/plat/mediatek/mt8192/drivers/mcdi/mt_lp_irqremain.c b/plat/mediatek/mt8192/drivers/mcdi/mt_lp_irqremain.c
index e74d3e7..872f4d0 100644
--- a/plat/mediatek/mt8192/drivers/mcdi/mt_lp_irqremain.c
+++ b/plat/mediatek/mt8192/drivers/mcdi/mt_lp_irqremain.c
@@ -1,12 +1,12 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <mt_lp_rm.h>
+#include <mt_cirq.h>
 #include <mt_lp_irqremain.h>
-#include <mtk_cirq.h>
+#include <mt_lp_rm.h>
 #include <plat_mtk_lpm.h>
 
 #define EDMA0_IRQ_ID		U(448)
diff --git a/plat/mediatek/mt8192/drivers/pmic/pmic_wrap_init.h b/plat/mediatek/mt8192/drivers/pmic/pmic_wrap_init.h
index ae892ed..b9ab586 100644
--- a/plat/mediatek/mt8192/drivers/pmic/pmic_wrap_init.h
+++ b/plat/mediatek/mt8192/drivers/pmic/pmic_wrap_init.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,10 +10,7 @@
 #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);
+#include <pmic_wrap_init_common.h>
 
 static struct mt8192_pmic_wrap_regs *const mtk_pwrap = (void *)PMIC_WRAP_BASE;
 
@@ -30,47 +27,4 @@
 	uint32_t wacs2_sta;
 };
 
-#define GET_WACS_FSM(x)	((x >> 1) & 0x7)
-
-/* macro for SWINF_FSM */
-#define SWINF_FSM_IDLE		(0x00)
-#define SWINF_FSM_REQ		(0x02)
-#define SWINF_FSM_WFDLE		(0x04)
-#define SWINF_FSM_WFVLDCLR	(0x06)
-#define SWINF_INIT_DONE		(0x01)
-
-/* timeout setting */
-#define PWRAP_READ_US	1000
-#define PWRAP_WAIT_IDLE_US	1000
-
-/* error information flag */
-enum pwrap_errno {
-	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/mt8192/drivers/spm/constraints/mt_spm_rc_bus26m.c b/plat/mediatek/mt8192/drivers/spm/constraints/mt_spm_rc_bus26m.c
index f66b8ec..18c43b1 100644
--- a/plat/mediatek/mt8192/drivers/spm/constraints/mt_spm_rc_bus26m.c
+++ b/plat/mediatek/mt8192/drivers/spm/constraints/mt_spm_rc_bus26m.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -23,8 +23,8 @@
 #include <plat_mtk_lpm.h>
 
 #ifndef ATF_PLAT_CIRQ_UNSUPPORT
+#include <mt_cirq.h>
 #include <mt_gic_v3.h>
-#include <mtk_cirq.h>
 #endif
 
 #define CONSTRAINT_BUS26M_ALLOW			\
diff --git a/plat/mediatek/mt8192/plat_sip_calls.c b/plat/mediatek/mt8192/plat_sip_calls.c
index 353faf8..0fffed5 100644
--- a/plat/mediatek/mt8192/plat_sip_calls.c
+++ b/plat/mediatek/mt8192/plat_sip_calls.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -25,8 +25,8 @@
 	uint32_t rnd_val0 = 0U;
 
 	switch (smc_fid) {
-	case MTK_SIP_VCORE_CONTROL_ARCH32:
-	case MTK_SIP_VCORE_CONTROL_ARCH64:
+	case MTK_SIP_VCORE_CONTROL_AARCH32:
+	case MTK_SIP_VCORE_CONTROL_AARCH64:
 		ret = spm_vcorefs_args(x1, x2, x3, (uint64_t *)&x4);
 		SMC_RET2(handle, ret, x4);
 		break;
diff --git a/plat/mediatek/mt8192/platform.mk b/plat/mediatek/mt8192/platform.mk
index cbdaadd..efc14ec 100644
--- a/plat/mediatek/mt8192/platform.mk
+++ b/plat/mediatek/mt8192/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2020, MediaTek Inc. All rights reserved.
+# Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -8,12 +8,16 @@
 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/rtc/                \
-                 -I${MTK_PLAT}/common/drivers/timer/              \
-                 -I${MTK_PLAT}/common/drivers/uart/               \
-                 -I${MTK_PLAT}/common/lpm/                        \
+                 -I${MTK_PLAT}/drivers/cirq/                      \
+                 -I${MTK_PLAT}/drivers/gic600/                    \
+                 -I${MTK_PLAT}/drivers/gpio/                      \
+                 -I${MTK_PLAT}/drivers/lpm/                       \
+                 -I${MTK_PLAT}/drivers/pmic/                      \
+                 -I${MTK_PLAT}/drivers/pmic_wrap/                 \
+                 -I${MTK_PLAT}/drivers/rtc/                       \
+                 -I${MTK_PLAT}/drivers/timer/                     \
+                 -I${MTK_PLAT}/drivers/uart/                      \
+                 -I${MTK_PLAT}/include/                           \
                  -I${MTK_PLAT_SOC}/include/                       \
                  -I${MTK_PLAT_SOC}/drivers/                       \
                  -I${MTK_PLAT_SOC}/drivers/apusys/                \
@@ -45,22 +49,22 @@
                    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_v2.c \
-                   ${MTK_PLAT}/common/drivers/rtc/rtc_common.c           \
-                   ${MTK_PLAT}/common/drivers/rtc/rtc_mt6359p.c          \
-                   ${MTK_PLAT}/common/drivers/timer/mt_timer.c           \
-                   ${MTK_PLAT}/common/drivers/uart/uart.c                \
-                   ${MTK_PLAT}/common/lpm/mt_lp_rm.c                     \
-                   ${MTK_PLAT}/common/mtk_cirq.c                         \
                    ${MTK_PLAT}/common/mtk_plat_common.c                  \
                    ${MTK_PLAT}/common/mtk_sip_svc.c                      \
                    ${MTK_PLAT}/common/params_setup.c                     \
+                   ${MTK_PLAT}/drivers/cirq/mt_cirq.c                    \
+                   ${MTK_PLAT}/drivers/gic600/mt_gic_v3.c                \
+                   ${MTK_PLAT}/drivers/gpio/mtgpio_common.c              \
+                   ${MTK_PLAT}/drivers/lpm/mt_lp_rm.c                    \
+                   ${MTK_PLAT}/drivers/pmic/pmic.c                       \
+                   ${MTK_PLAT}/drivers/pmic_wrap/pmic_wrap_init_v2.c     \
+                   ${MTK_PLAT}/drivers/rtc/rtc_common.c                  \
+                   ${MTK_PLAT}/drivers/rtc/rtc_mt6359p.c                 \
+                   ${MTK_PLAT}/drivers/timer/mt_timer.c                  \
+                   ${MTK_PLAT}/drivers/uart/uart.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/pmic/pmic.c                   \
                    ${MTK_PLAT_SOC}/plat_pm.c                             \
                    ${MTK_PLAT_SOC}/plat_topology.c                       \
                    ${MTK_PLAT_SOC}/plat_sip_calls.c                      \
diff --git a/plat/mediatek/mt8195/aarch64/platform_common.c b/plat/mediatek/mt8195/aarch64/platform_common.c
index 2b95171..1f5c5fa 100644
--- a/plat/mediatek/mt8195/aarch64/platform_common.c
+++ b/plat/mediatek/mt8195/aarch64/platform_common.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,7 +19,7 @@
 			MT_DEVICE | MT_RW | MT_SECURE),
 	MAP_REGION_FLAT(DP_SEC_BASE, DP_SEC_SIZE,
 			MT_DEVICE | MT_RW | MT_SECURE),
-	MAP_REGION_FLAT(eDP_SEC_BASE, eDP_SEC_SIZE,
+	MAP_REGION_FLAT(EDP_SEC_BASE, EDP_SEC_SIZE,
 			MT_DEVICE | MT_RW | MT_SECURE),
 	MAP_REGION_FLAT(APUSYS_SCTRL_REVISER_BASE, APUSYS_SCTRL_REVISER_SIZE,
 			MT_DEVICE | MT_RW | MT_SECURE),
diff --git a/plat/mediatek/mt8195/drivers/mcdi/mt_lp_irqremain.c b/plat/mediatek/mt8195/drivers/mcdi/mt_lp_irqremain.c
index 4147184..f415cb8 100644
--- a/plat/mediatek/mt8195/drivers/mcdi/mt_lp_irqremain.c
+++ b/plat/mediatek/mt8195/drivers/mcdi/mt_lp_irqremain.c
@@ -1,15 +1,14 @@
 /*
- * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2021-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <mt_lp_rm.h>
+#include <mt_cirq.h>
 #include <mt_lp_irqremain.h>
-#include <mtk_cirq.h>
+#include <mt_lp_rm.h>
 #include <plat_mtk_lpm.h>
 
-
 #define KEYPAD_IRQ_ID		U(138)
 
 #define KEYPAD_WAKESRC		0x4
diff --git a/plat/mediatek/mt8195/drivers/pmic/pmic.c b/plat/mediatek/mt8195/drivers/pmic/pmic.c
deleted file mode 100644
index cca4413..0000000
--- a/plat/mediatek/mt8195/drivers/pmic/pmic.c
+++ /dev/null
@@ -1,13 +0,0 @@
-/*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <pmic.h>
-#include <pmic_wrap_init.h>
-
-void pmic_power_off(void)
-{
-	pwrap_write(PMIC_PWRHOLD, 0x0);
-}
diff --git a/plat/mediatek/mt8195/drivers/pmic/pmic.h b/plat/mediatek/mt8195/drivers/pmic/pmic.h
deleted file mode 100644
index aac22af..0000000
--- a/plat/mediatek/mt8195/drivers/pmic/pmic.h
+++ /dev/null
@@ -1,15 +0,0 @@
-/*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef PMIC_H
-#define PMIC_H
-
-#define PMIC_PWRHOLD 0xa08
-
-/* external API */
-void pmic_power_off(void);
-
-#endif /* PMIC_H */
diff --git a/plat/mediatek/mt8195/drivers/pmic/pmic_wrap_init.h b/plat/mediatek/mt8195/drivers/pmic/pmic_wrap_init.h
index 39e78f5..9e6e74c 100644
--- a/plat/mediatek/mt8195/drivers/pmic/pmic_wrap_init.h
+++ b/plat/mediatek/mt8195/drivers/pmic/pmic_wrap_init.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,10 +10,7 @@
 #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);
+#include <pmic_wrap_init_common.h>
 
 static struct mt8195_pmic_wrap_regs *const mtk_pwrap = (void *)PMIC_WRAP_BASE;
 
@@ -30,47 +27,4 @@
 	uint32_t wacs2_sta;
 };
 
-#define GET_WACS_FSM(x)	((x >> 1) & 0x7)
-
-/* macro for SWINF_FSM */
-#define SWINF_FSM_IDLE		(0x00)
-#define SWINF_FSM_REQ		(0x02)
-#define SWINF_FSM_WFDLE		(0x04)
-#define SWINF_FSM_WFVLDCLR	(0x06)
-#define SWINF_INIT_DONE		(0x01)
-
-/* timeout setting */
-#define PWRAP_READ_US	1000
-#define PWRAP_WAIT_IDLE_US	1000
-
-/* error information flag */
-enum pwrap_errno {
-	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/mt8195/drivers/spm/constraints/mt_spm_rc_bus26m.c b/plat/mediatek/mt8195/drivers/spm/constraints/mt_spm_rc_bus26m.c
index d2ad282..87278d7 100644
--- a/plat/mediatek/mt8195/drivers/spm/constraints/mt_spm_rc_bus26m.c
+++ b/plat/mediatek/mt8195/drivers/spm/constraints/mt_spm_rc_bus26m.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2021-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -23,8 +23,8 @@
 #include <plat_mtk_lpm.h>
 
 #ifndef ATF_PLAT_CIRQ_UNSUPPORT
+#include <mt_cirq.h>
 #include <mt_gic_v3.h>
-#include <mtk_cirq.h>
 #endif
 
 #define CONSTRAINT_BUS26M_ALLOW			\
diff --git a/plat/mediatek/mt8195/include/platform_def.h b/plat/mediatek/mt8195/include/platform_def.h
index d4f2f83..2a2f559 100644
--- a/plat/mediatek/mt8195/include/platform_def.h
+++ b/plat/mediatek/mt8195/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -48,9 +48,9 @@
 /*******************************************************************************
  * DP/eDP related constants
  ******************************************************************************/
-#define eDP_SEC_BASE		(IO_PHYS + 0x0C504000)
+#define EDP_SEC_BASE		(IO_PHYS + 0x0C504000)
 #define DP_SEC_BASE		(IO_PHYS + 0x0C604000)
-#define eDP_SEC_SIZE		0x1000
+#define EDP_SEC_SIZE		0x1000
 #define DP_SEC_SIZE		0x1000
 
 /*******************************************************************************
diff --git a/plat/mediatek/mt8195/plat_sip_calls.c b/plat/mediatek/mt8195/plat_sip_calls.c
index 7d3c512..1cdd622 100644
--- a/plat/mediatek/mt8195/plat_sip_calls.c
+++ b/plat/mediatek/mt8195/plat_sip_calls.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -32,8 +32,8 @@
 		ret = dp_secure_handler(x1, x2, &ret_val);
 		SMC_RET2(handle, ret, ret_val);
 		break;
-	case MTK_SIP_VCORE_CONTROL_ARCH32:
-	case MTK_SIP_VCORE_CONTROL_ARCH64:
+	case MTK_SIP_VCORE_CONTROL_AARCH32:
+	case MTK_SIP_VCORE_CONTROL_AARCH64:
 		ret = spm_vcorefs_v2_args(x1, x2, x3, &x4);
 		SMC_RET2(handle, ret, x4);
 		break;
diff --git a/plat/mediatek/mt8195/platform.mk b/plat/mediatek/mt8195/platform.mk
index a81c093..414d655 100644
--- a/plat/mediatek/mt8195/platform.mk
+++ b/plat/mediatek/mt8195/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2021, MediaTek Inc. All rights reserved.
+# Copyright (c) 2021-2022, MediaTek Inc. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -8,22 +8,26 @@
 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/rtc/                \
-                 -I${MTK_PLAT}/common/drivers/timer/              \
-                 -I${MTK_PLAT}/common/drivers/uart/               \
-                 -I${MTK_PLAT}/common/lpm/                        \
+                 -I${MTK_PLAT}/drivers/cirq/                      \
+                 -I${MTK_PLAT}/drivers/dp/                        \
+                 -I${MTK_PLAT}/drivers/gic600/                    \
+                 -I${MTK_PLAT}/drivers/gpio/                      \
+                 -I${MTK_PLAT}/drivers/lpm/                       \
+                 -I${MTK_PLAT}/drivers/pmic/                      \
+                 -I${MTK_PLAT}/drivers/pmic_wrap/                 \
+                 -I${MTK_PLAT}/drivers/rtc/                       \
+                 -I${MTK_PLAT}/drivers/timer/                     \
+                 -I${MTK_PLAT}/drivers/uart/                      \
+                 -I${MTK_PLAT}/include/                           \
                  -I${MTK_PLAT_SOC}/drivers/apusys/                \
                  -I${MTK_PLAT_SOC}/drivers/dcm                    \
                  -I${MTK_PLAT_SOC}/drivers/dfd                    \
-                 -I${MTK_PLAT_SOC}/drivers/dp/                    \
                  -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/spmc/                  \
-                 -I${MTK_PLAT_SOC}/drivers/ptp3/                   \
+                 -I${MTK_PLAT_SOC}/drivers/ptp3/                  \
                  -I${MTK_PLAT_SOC}/include/
 
 GICV3_SUPPORT_GIC600        :=      1
@@ -45,36 +49,36 @@
                 lib/cpus/aarch64/cortex_a55.S                         \
                 lib/cpus/aarch64/cortex_a78.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_v2.c \
-                ${MTK_PLAT}/common/drivers/rtc/rtc_common.c           \
-                ${MTK_PLAT}/common/drivers/rtc/rtc_mt6359p.c          \
-                ${MTK_PLAT}/common/drivers/timer/mt_timer.c           \
-		${MTK_PLAT}/common/drivers/uart/uart.c                \
-		${MTK_PLAT}/common/lpm/mt_lp_rm.c                     \
-                ${MTK_PLAT}/common/mtk_cirq.c                         \
                 ${MTK_PLAT}/common/mtk_plat_common.c                  \
                 ${MTK_PLAT}/common/mtk_sip_svc.c                      \
                 ${MTK_PLAT}/common/params_setup.c                     \
+                ${MTK_PLAT}/drivers/cirq/mt_cirq.c                    \
+                ${MTK_PLAT}/drivers/dp/mt_dp.c                        \
+                ${MTK_PLAT}/drivers/gic600/mt_gic_v3.c                \
+                ${MTK_PLAT}/drivers/gpio/mtgpio_common.c              \
+                ${MTK_PLAT}/drivers/lpm/mt_lp_rm.c                    \
+                ${MTK_PLAT}/drivers/pmic/pmic.c                       \
+                ${MTK_PLAT}/drivers/pmic_wrap/pmic_wrap_init_v2.c     \
+                ${MTK_PLAT}/drivers/rtc/rtc_common.c                  \
+                ${MTK_PLAT}/drivers/rtc/rtc_mt6359p.c                 \
+                ${MTK_PLAT}/drivers/timer/mt_timer.c                  \
+                ${MTK_PLAT}/drivers/uart/uart.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/apusys/apupll.c               \
                 ${MTK_PLAT_SOC}/drivers/apusys/apupwr_clkctl.c        \
-		${MTK_PLAT_SOC}/drivers/apusys/mtk_apusys.c           \
+                ${MTK_PLAT_SOC}/drivers/apusys/mtk_apusys.c           \
                 ${MTK_PLAT_SOC}/drivers/dcm/mtk_dcm.c                 \
                 ${MTK_PLAT_SOC}/drivers/dcm/mtk_dcm_utils.c           \
                 ${MTK_PLAT_SOC}/drivers/dfd/plat_dfd.c                \
-                ${MTK_PLAT_SOC}/drivers/dp/mt_dp.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/mcdi/mt_lp_irqremain.c        \
+                ${MTK_PLAT_SOC}/drivers/mcdi/mt_lp_irqremain.c        \
                 ${MTK_PLAT_SOC}/drivers/gpio/mtgpio.c                 \
-                ${MTK_PLAT_SOC}/drivers/pmic/pmic.c                   \
                 ${MTK_PLAT_SOC}/drivers/ptp3/mtk_ptp3_main.c          \
                 ${MTK_PLAT_SOC}/drivers/spmc/mtspmc.c                 \
                 ${MTK_PLAT_SOC}/plat_pm.c                             \
diff --git a/plat/qemu/qemu/include/platform_def.h b/plat/qemu/qemu/include/platform_def.h
index 78467c4..c9ed640 100644
--- a/plat/qemu/qemu/include/platform_def.h
+++ b/plat/qemu/qemu/include/platform_def.h
@@ -84,7 +84,7 @@
 #define NS_DRAM0_SIZE			ULL(0xc0000000)
 
 #define SEC_SRAM_BASE			0x0e000000
-#define SEC_SRAM_SIZE			0x00060000
+#define SEC_SRAM_SIZE			0x00100000
 
 #define SEC_DRAM_BASE			0x0e100000
 #define SEC_DRAM_SIZE			0x00f00000
@@ -146,7 +146,7 @@
  * Put BL3-1 at the top of the Trusted SRAM. BL31_BASE is calculated using the
  * current BL3-1 debug size plus a little space for growth.
  */
-#define BL31_BASE			(BL31_LIMIT - 0x20000)
+#define BL31_BASE			(BL31_LIMIT - 0x60000)
 #define BL31_LIMIT			(BL_RAM_BASE + BL_RAM_SIZE)
 #define BL31_PROGBITS_LIMIT		BL1_RW_BASE
 
diff --git a/plat/st/common/bl2_io_storage.c b/plat/st/common/bl2_io_storage.c
index 49f6465..8d0bdcc 100644
--- a/plat/st/common/bl2_io_storage.c
+++ b/plat/st/common/bl2_io_storage.c
@@ -221,6 +221,10 @@
 		break;
 	}
 
+	if (mmc_dev_type != MMC_IS_EMMC) {
+		params.flags = MMC_FLAG_SD_CMD6;
+	}
+
 	params.device_info = &mmc_info;
 	if (stm32_sdmmc2_mmc_init(&params) != 0) {
 		ERROR("SDMMC%u init failed\n", boot_interface_instance);
diff --git a/plat/st/stm32mp1/platform.mk b/plat/st/stm32mp1/platform.mk
index b425fa5..7203de8 100644
--- a/plat/st/stm32mp1/platform.mk
+++ b/plat/st/stm32mp1/platform.mk
@@ -357,6 +357,9 @@
 endif
 
 ifeq (${STM32MP_SPI_NOR},1)
+ifneq (${STM32MP_FORCE_MTD_START_OFFSET},)
+$(eval $(call add_define_val,STM32MP_NOR_FIP_OFFSET,${STM32MP_FORCE_MTD_START_OFFSET}))
+endif
 BL2_SOURCES		+=	drivers/mtd/nor/spi_nor.c
 endif
 
@@ -366,6 +369,9 @@
 endif
 
 ifneq ($(filter 1,${STM32MP_RAW_NAND} ${STM32MP_SPI_NAND}),)
+ifneq (${STM32MP_FORCE_MTD_START_OFFSET},)
+$(eval $(call add_define_val,STM32MP_NAND_FIP_OFFSET,${STM32MP_FORCE_MTD_START_OFFSET}))
+endif
 BL2_SOURCES		+=	drivers/mtd/nand/core.c
 endif
 
diff --git a/plat/st/stm32mp1/stm32mp1_boot_device.c b/plat/st/stm32mp1/stm32mp1_boot_device.c
index b05de1c..3a8a27a 100644
--- a/plat/st/stm32mp1/stm32mp1_boot_device.c
+++ b/plat/st/stm32mp1/stm32mp1_boot_device.c
@@ -4,6 +4,7 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <assert.h>
 #include <errno.h>
 
 #include <common/debug.h>
@@ -15,9 +16,21 @@
 #include <plat/common/platform.h>
 
 #if STM32MP_RAW_NAND || STM32MP_SPI_NAND
+#if STM32MP13
+void plat_get_scratch_buffer(void **buffer_addr, size_t *buf_size)
+{
+	assert(buffer_addr != NULL);
+	assert(buf_size != NULL);
+
+	*buffer_addr = (void *)STM32MP_MTD_BUFFER;
+	*buf_size = PLATFORM_MTD_MAX_PAGE_SIZE;
+}
+#endif
+
 static int get_data_from_otp(struct nand_device *nand_dev, bool is_slc)
 {
 	uint32_t nand_param;
+	uint32_t nand2_param __maybe_unused;
 
 	/* Check if NAND parameters are stored in OTP */
 	if (stm32_get_otp_value(NAND_OTP, &nand_param) != 0) {
@@ -26,12 +39,39 @@
 	}
 
 	if (nand_param == 0U) {
+#if STM32MP13
+		if (is_slc) {
+			return 0;
+		}
+#endif
+#if STM32MP15
 		return 0;
+#endif
 	}
 
 	if ((nand_param & NAND_PARAM_STORED_IN_OTP) == 0U) {
+#if STM32MP13
+		if (is_slc) {
+			goto ecc;
+		}
+#endif
+#if STM32MP15
 		goto ecc;
+#endif
+	}
+
+#if STM32MP13
+	if (stm32_get_otp_value(NAND2_OTP, &nand2_param) != 0) {
+		ERROR("BSEC: NAND_OTP Error\n");
+		return -EACCES;
+	}
+
+	/* Check OTP configuration for this device */
+	if ((((nand2_param & NAND2_CONFIG_DISTRIB) == NAND2_PNAND_NAND1_SNAND_NAND2) && !is_slc) ||
+	    (((nand2_param & NAND2_CONFIG_DISTRIB) == NAND2_PNAND_NAND2_SNAND_NAND1) && is_slc)) {
+		nand_param = nand2_param << (NAND_PAGE_SIZE_SHIFT - NAND2_PAGE_SIZE_SHIFT);
 	}
+#endif
 
 	/* NAND parameter shall be read from OTP */
 	if ((nand_param & NAND_WIDTH_MASK) != 0U) {
diff --git a/plat/st/stm32mp1/stm32mp1_def.h b/plat/st/stm32mp1/stm32mp1_def.h
index 8ca6123..116bd5d 100644
--- a/plat/st/stm32mp1/stm32mp1_def.h
+++ b/plat/st/stm32mp1/stm32mp1_def.h
@@ -71,6 +71,7 @@
 
 #define STM32MP1_REV_B		U(0x2000)
 #if STM32MP13
+#define STM32MP1_REV_Y		U(0x1003)
 #define STM32MP1_REV_Z		U(0x1001)
 #endif
 #if STM32MP15
@@ -193,6 +194,12 @@
 /* Define maximum page size for NAND devices */
 #define PLATFORM_MTD_MAX_PAGE_SIZE	U(0x1000)
 
+/* Define location for the MTD scratch buffer */
+#if STM32MP13
+#define STM32MP_MTD_BUFFER		(SRAM1_BASE + \
+					 SRAM1_SIZE - \
+					 PLATFORM_MTD_MAX_PAGE_SIZE)
+#endif
 /*******************************************************************************
  * STM32MP1 device/io map related constants (used for MMU)
  ******************************************************************************/
@@ -435,7 +442,13 @@
 #define PACKAGE_OTP			"package_otp"
 #endif
 #define HW2_OTP				"hw2_otp"
+#if STM32MP13
+#define NAND_OTP			"cfg9_otp"
+#define NAND2_OTP			"cfg10_otp"
+#endif
+#if STM32MP15
 #define NAND_OTP			"nand_otp"
+#endif
 #define MONOTONIC_OTP			"monotonic_otp"
 #define UID_OTP				"uid_otp"
 #define BOARD_ID_OTP			"board_id"
@@ -495,7 +508,7 @@
 #define NAND_BLOCK_SIZE_128_PAGES	U(1)
 #define NAND_BLOCK_SIZE_256_PAGES	U(2)
 
-/* NAND number of block (in unit of 256 blocs) */
+/* NAND number of block (in unit of 256 blocks) */
 #define NAND_BLOCK_NB_MASK		GENMASK_32(26, 19)
 #define NAND_BLOCK_NB_SHIFT		19
 #define NAND_BLOCK_NB_UNIT		U(256)
@@ -516,6 +529,14 @@
 /* NAND number of planes */
 #define NAND_PLANE_BIT_NB_MASK		BIT(14)
 
+/* NAND2 OTP */
+#define NAND2_PAGE_SIZE_SHIFT		16
+
+/* NAND2 config distribution */
+#define NAND2_CONFIG_DISTRIB		BIT(0)
+#define NAND2_PNAND_NAND2_SNAND_NAND1	U(0)
+#define NAND2_PNAND_NAND1_SNAND_NAND2	U(1)
+
 /* MONOTONIC OTP */
 #define MAX_MONOTONIC_VALUE		32
 
diff --git a/plat/st/stm32mp1/stm32mp1_fip_def.h b/plat/st/stm32mp1/stm32mp1_fip_def.h
index 82e53db..5652597 100644
--- a/plat/st/stm32mp1/stm32mp1_fip_def.h
+++ b/plat/st/stm32mp1/stm32mp1_fip_def.h
@@ -106,7 +106,11 @@
  * STM32MP1 RAW partition offset for devices without GPT
  ******************************************************************************/
 #define STM32MP_EMMC_BOOT_FIP_OFFSET	U(0x00040000)
+#ifndef STM32MP_NOR_FIP_OFFSET
 #define STM32MP_NOR_FIP_OFFSET		U(0x00080000)
+#endif
+#ifndef STM32MP_NAND_FIP_OFFSET
 #define STM32MP_NAND_FIP_OFFSET		U(0x00200000)
+#endif
 
 #endif /* STM32MP1_FIP_DEF_H */
diff --git a/plat/st/stm32mp1/stm32mp1_private.c b/plat/st/stm32mp1/stm32mp1_private.c
index 86b9f23..c40e045 100644
--- a/plat/st/stm32mp1/stm32mp1_private.c
+++ b/plat/st/stm32mp1/stm32mp1_private.c
@@ -493,6 +493,11 @@
 	case STM32MP1_REV_B:
 		cpu_r = "B";
 		break;
+#if STM32MP13
+	case STM32MP1_REV_Y:
+		cpu_r = "Y";
+		break;
+#endif
 	case STM32MP1_REV_Z:
 		cpu_r = "Z";
 		break;
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c b/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c
index 8a5a25a..48e1b8d 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c
@@ -553,7 +553,7 @@
 		return PM_RET_ERROR_ARGS;
 	}
 
-	if (index < AFIFM6_WRCTRL) {
+	if (index <= AFIFM6_WRCTRL) {
 		mask = FABRIC_WIDTH;
 	} else {
 		mask = 0xf00;
diff --git a/services/std_svc/spm/el3_spmc/spmc.mk b/services/std_svc/spm/el3_spmc/spmc.mk
index aa591d9..c674e71 100644
--- a/services/std_svc/spm/el3_spmc/spmc.mk
+++ b/services/std_svc/spm/el3_spmc/spmc.mk
@@ -24,3 +24,21 @@
 
 # Let the top-level Makefile know that we intend to include a BL32 image
 NEED_BL32		:=	yes
+
+ifndef BL32
+# The SPMC is paired with a Test Secure Payload source and we intend to
+# build the Test Secure Payload if no other image has been provided
+# for BL32.
+#
+# In cases where an associated Secure Payload lies outside this build
+# system/source tree, the dispatcher Makefile can either invoke an external
+# build command or assume it is pre-built.
+
+BL32_ROOT		:=	bl32/tsp
+
+# Conditionally include SP's Makefile. The assumption is that the TSP's build
+# system is compatible with that of Trusted Firmware, and it'll add and populate
+# necessary build targets and variables.
+
+include ${BL32_ROOT}/tsp.mk
+endif
diff --git a/services/std_svc/spm/el3_spmc/spmc_shared_mem.c b/services/std_svc/spm/el3_spmc/spmc_shared_mem.c
index 4a24108..eab2096 100644
--- a/services/std_svc/spm/el3_spmc/spmc_shared_mem.c
+++ b/services/std_svc/spm/el3_spmc/spmc_shared_mem.c
@@ -260,6 +260,34 @@
 	return comp_mrd->address_range_count * sizeof(struct ffa_cons_mrd);
 }
 
+/**
+ * spmc_shmem_obj_validate_id - Validate a partition ID is participating in
+ *				a given memory transaction.
+ * @sp_id:      Partition ID to validate.
+ * @desc:       Descriptor of the memory transaction.
+ *
+ * Return: true if ID is valid, else false.
+ */
+bool spmc_shmem_obj_validate_id(const struct ffa_mtd *desc, uint16_t sp_id)
+{
+	bool found = false;
+
+	/* Validate the partition is a valid participant. */
+	for (unsigned int i = 0U; i < desc->emad_count; i++) {
+		size_t emad_size;
+		struct ffa_emad_v1_0 *emad;
+
+		emad = spmc_shmem_obj_get_emad(desc, i,
+					       MAKE_FFA_VERSION(1, 1),
+					       &emad_size);
+		if (sp_id == emad->mapd.endpoint_id) {
+			found = true;
+			break;
+		}
+	}
+	return found;
+}
+
 /*
  * Compare two memory regions to determine if any range overlaps with another
  * ongoing memory transaction.
@@ -1403,6 +1431,14 @@
 		}
 	}
 
+	/* Validate the caller is a valid participant. */
+	if (!spmc_shmem_obj_validate_id(&obj->desc, sp_ctx->sp_id)) {
+		WARN("%s: Invalid endpoint ID (0x%x).\n",
+			__func__, sp_ctx->sp_id);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
 	/* Validate that the provided emad offset and structure is valid.*/
 	for (size_t i = 0; i < req->emad_count; i++) {
 		size_t emad_size;
@@ -1657,6 +1693,7 @@
 	struct mailbox *mbox = spmc_get_mbox_desc(secure_origin);
 	struct spmc_shmem_obj *obj;
 	const struct ffa_mem_relinquish_descriptor *req;
+	struct secure_partition_desc *sp_ctx = spmc_get_current_sp_ctx();
 
 	if (!secure_origin) {
 		WARN("%s: unsupported relinquish direction.\n", __func__);
@@ -1694,36 +1731,31 @@
 		goto err_unlock_all;
 	}
 
-	if (obj->desc.emad_count != req->endpoint_count) {
-		WARN("%s: mismatch of endpoint count %u != %u\n", __func__,
-		     obj->desc.emad_count, req->endpoint_count);
+	/*
+	 * Validate the endpoint ID was populated correctly. We don't currently
+	 * support proxy endpoints so the endpoint count should always be 1.
+	 */
+	if (req->endpoint_count != 1U) {
+		WARN("%s: unsupported endpoint count %u != 1\n", __func__,
+		     req->endpoint_count);
 		ret = FFA_ERROR_INVALID_PARAMETER;
 		goto err_unlock_all;
 	}
 
-	/* Validate requested endpoint IDs match descriptor. */
-	for (size_t i = 0; i < req->endpoint_count; i++) {
-		bool found = false;
-		size_t emad_size;
-		struct ffa_emad_v1_0 *emad;
-
-		for (unsigned int j = 0; j < obj->desc.emad_count; j++) {
-			emad = spmc_shmem_obj_get_emad(&obj->desc, j,
-							MAKE_FFA_VERSION(1, 1),
-							&emad_size);
-			if (req->endpoint_array[i] ==
-			    emad->mapd.endpoint_id) {
-				found = true;
-				break;
-			}
-		}
+	/* Validate provided endpoint ID matches the partition ID. */
+	if (req->endpoint_array[0] != sp_ctx->sp_id) {
+		WARN("%s: invalid endpoint ID %u != %u\n", __func__,
+		     req->endpoint_array[0], sp_ctx->sp_id);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
 
-		if (!found) {
-			WARN("%s: Invalid endpoint ID (0x%x).\n",
-			     __func__, req->endpoint_array[i]);
-			ret = FFA_ERROR_INVALID_PARAMETER;
-			goto err_unlock_all;
-		}
+	/* Validate the caller is a valid participant. */
+	if (!spmc_shmem_obj_validate_id(&obj->desc, sp_ctx->sp_id)) {
+		WARN("%s: Invalid endpoint ID (0x%x).\n",
+			__func__, req->endpoint_array[0]);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
 	}
 
 	if (obj->in_use == 0U) {