diff --git a/.commitlintrc.js b/.commitlintrc.js
index ed971a3..cfafbed 100644
--- a/.commitlintrc.js
+++ b/.commitlintrc.js
@@ -68,7 +68,6 @@
         "type-enum": [2, "always", types], /* Error */
 
         "scope-case": [2, "always", "lower-case"], /* Error */
-        "scope-empty": [2, "never"], /* Error */
         "scope-enum": [1, "always", scopes] /* Warning */
     },
 };
diff --git a/.gitignore b/.gitignore
index f524658..b005fab 100644
--- a/.gitignore
+++ b/.gitignore
@@ -30,8 +30,7 @@
 tools/stm32image/*.o
 tools/stm32image/stm32image
 tools/stm32image/stm32image.exe
-tools/sptool/sptool
-tools/sptool/sptool.exe
+tools/sptool/__pycache__/
 
 # GNU GLOBAL files
 GPATH
diff --git a/Makefile b/Makefile
index 851c944..16c85bc 100644
--- a/Makefile
+++ b/Makefile
@@ -135,6 +135,10 @@
 ifneq (${ENABLE_PIE},0)
         $(error ENABLE_RME does not support PIE)
 endif
+# RME doesn't support BRBE
+ifneq (${ENABLE_BRBE_FOR_NS},0)
+        $(error ENABLE_RME does not support BRBE.)
+endif
 # RME requires AARCH64
 ifneq (${ARCH},aarch64)
         $(error ENABLE_RME requires AArch64)
@@ -263,24 +267,24 @@
 # Determine if FEAT_SB is supported
 ENABLE_FEAT_SB		=	$(if $(findstring sb,${arch-features}),1,0)
 
-ifneq ($(findstring armclang,$(notdir $(CC))),)
-TF_CFLAGS_aarch32	=	-target arm-arm-none-eabi $(march32-directive)
-TF_CFLAGS_aarch64	=	-target aarch64-arm-none-eabi $(march64-directive)
-LD			=	$(LINKER)
-AS			=	$(CC) -c -x assembler-with-cpp $(TF_CFLAGS_$(ARCH))
-CPP			=	$(CC) -E $(TF_CFLAGS_$(ARCH))
-PP			=	$(CC) -E $(TF_CFLAGS_$(ARCH))
-else ifneq ($(findstring clang,$(notdir $(CC))),)
-CLANG_CCDIR		=	$(if $(filter-out ./,$(dir $(CC))),$(dir $(CC)),)
-TF_CFLAGS_aarch32	=	$(target32-directive) $(march32-directive)
-TF_CFLAGS_aarch64	=	-target aarch64-elf $(march64-directive)
-LD			=	$(CLANG_CCDIR)ld.lld
-ifeq (, $(shell which $(LD)))
-$(error "No $(LD) in PATH, make sure it is installed or set LD to a different linker")
-endif
-AS			=	$(CC) -c -x assembler-with-cpp $(TF_CFLAGS_$(ARCH))
-CPP			=	$(CC) -E
-PP			=	$(CC) -E
+ifneq ($(findstring clang,$(notdir $(CC))),)
+	ifneq ($(findstring armclang,$(notdir $(CC))),)
+		TF_CFLAGS_aarch32	:=	-target arm-arm-none-eabi $(march32-directive)
+		TF_CFLAGS_aarch64	:=	-target aarch64-arm-none-eabi $(march64-directive)
+		LD			:=	$(LINKER)
+	else
+		TF_CFLAGS_aarch32	:=	$(target32-directive) $(march32-directive)
+		TF_CFLAGS_aarch64	:=	-target aarch64-elf $(march64-directive)
+		LD			:=	$(shell $(CC) --print-prog-name ld.lld)
+
+		AR			:=	$(shell $(CC) --print-prog-name llvm-ar)
+		OD			:=	$(shell $(CC) --print-prog-name llvm-objdump)
+		OC			:=	$(shell $(CC) --print-prog-name llvm-objcopy)
+	endif
+
+	CPP		:=	$(CC) -E $(TF_CFLAGS_$(ARCH))
+	PP		:=	$(CC) -E $(TF_CFLAGS_$(ARCH))
+	AS		:=	$(CC) -c -x assembler-with-cpp $(TF_CFLAGS_$(ARCH))
 else ifneq ($(findstring gcc,$(notdir $(CC))),)
 TF_CFLAGS_aarch32	=	$(march32-directive)
 TF_CFLAGS_aarch64	=	$(march64-directive)
@@ -777,8 +781,10 @@
     endif
 endif
 
-# SME/SVE only supported on AArch64
+# Ensure that no Aarch64-only features are enabled in Aarch32 build
 ifeq (${ARCH},aarch32)
+
+    # SME/SVE only supported on AArch64
     ifeq (${ENABLE_SME_FOR_NS},1)
         $(error "ENABLE_SME_FOR_NS cannot be used with ARCH=aarch32")
     endif
@@ -786,6 +792,12 @@
         # Warning instead of error due to CI dependency on this
         $(error "ENABLE_SVE_FOR_NS cannot be used with ARCH=aarch32")
     endif
+
+    # BRBE is not supported in Aarch32
+    ifeq (${ENABLE_BRBE_FOR_NS},1)
+        $(error "ENABLE_BRBE_FOR_NS cannot be used with ARCH=aarch32")
+    endif
+
 endif
 
 # Ensure ENABLE_RME is not used with SME
@@ -918,7 +930,7 @@
 
 # Variables for use with sptool
 SPTOOLPATH		?=	tools/sptool
-SPTOOL			?=	${SPTOOLPATH}/sptool${BIN_EXT}
+SPTOOL			?=	${SPTOOLPATH}/sptool.py
 SP_MK_GEN		?=	${SPTOOLPATH}/sp_mk_generator.py
 
 # Variables for use with ROMLIB
@@ -1002,6 +1014,7 @@
         PROGRAMMABLE_RESET_ADDRESS \
         PSCI_EXTENDED_STATE_ID \
         RESET_TO_BL31 \
+        RESET_TO_BL31_WITH_PARAMS \
         SAVE_KEYS \
         SEPARATE_CODE_AND_RODATA \
         SEPARATE_BL2_NOLOAD_REGION \
@@ -1031,6 +1044,7 @@
         COT_DESC_IN_DTB \
         USE_SP804_TIMER \
         PSA_FWU_SUPPORT \
+        ENABLE_BRBE_FOR_NS \
         ENABLE_TRBE_FOR_NS \
         ENABLE_SYS_REG_TRACE_FOR_NS \
         ENABLE_MPMM \
@@ -1136,6 +1150,7 @@
         PSCI_EXTENDED_STATE_ID \
         RAS_EXTENSION \
         RESET_TO_BL31 \
+        RESET_TO_BL31_WITH_PARAMS \
         SEPARATE_CODE_AND_RODATA \
         SEPARATE_BL2_NOLOAD_REGION \
         SEPARATE_NOBITS_REGION \
@@ -1170,6 +1185,7 @@
         NR_OF_FW_BANKS \
         NR_OF_IMAGES_IN_FW_BANK \
         PSA_FWU_SUPPORT \
+        ENABLE_BRBE_FOR_NS \
         ENABLE_TRBE_FOR_NS \
         ENABLE_SYS_REG_TRACE_FOR_NS \
         ENABLE_TRF_FOR_NS \
@@ -1334,8 +1350,7 @@
 ifeq (${NEED_SP_PKG},yes)
 $(BUILD_PLAT)/sp_gen.mk: ${SP_MK_GEN} ${SP_LAYOUT_FILE} | ${BUILD_PLAT}
 	${Q}${PYTHON} "$<" "$@" $(filter-out $<,$^) $(BUILD_PLAT) ${COT}
-sp: $(SPTOOL) $(DTBS) $(BUILD_PLAT)/sp_gen.mk
-	${Q}$(SPTOOL) $(SPTOOL_ARGS)
+sp: $(DTBS) $(BUILD_PLAT)/sp_gen.mk $(SP_PKGS)
 	@${ECHO_BLANK_LINE}
 	@echo "Built SP Images successfully"
 	@${ECHO_BLANK_LINE}
@@ -1375,7 +1390,6 @@
 # to pass the gnumake flags to nmake.
 	${Q}set MAKEFLAGS= && ${MSVC_NMAKE} /nologo /f ${FIPTOOLPATH}/Makefile.msvc FIPTOOLPATH=$(subst /,\,$(FIPTOOLPATH)) FIPTOOL=$(subst /,\,$(FIPTOOL)) realclean
 endif
-	${Q}${MAKE} --no-print-directory -C ${SPTOOLPATH} clean
 	${Q}${MAKE} PLAT=${PLAT} --no-print-directory -C ${CRTTOOLPATH} realclean
 	${Q}${MAKE} PLAT=${PLAT} --no-print-directory -C ${ENCTOOLPATH} realclean
 	${Q}${MAKE} --no-print-directory -C ${ROMLIBPATH} clean
@@ -1469,10 +1483,6 @@
 	${Q}set MAKEFLAGS= && ${MSVC_NMAKE} /nologo /f ${FIPTOOLPATH}/Makefile.msvc FIPTOOLPATH=$(subst /,\,$(FIPTOOLPATH)) FIPTOOL=$(subst /,\,$(FIPTOOL))
 endif
 
-sptool: ${SPTOOL}
-${SPTOOL}: FORCE
-	${Q}${MAKE} CPPFLAGS="-DVERSION='\"${VERSION_STRING}\"'" SPTOOL=${SPTOOL} --no-print-directory -C ${SPTOOLPATH}
-
 romlib.bin: libraries FORCE
 	${Q}${MAKE} PLAT_DIR=${PLAT_DIR} BUILD_PLAT=${BUILD_PLAT} ENABLE_BTI=${ENABLE_BTI} ARM_ARCH_MINOR=${ARM_ARCH_MINOR} INCLUDES='${INCLUDES}' DEFINES='${DEFINES}' --no-print-directory -C ${ROMLIBPATH} all
 
diff --git a/bl31/aarch64/bl31_entrypoint.S b/bl31/aarch64/bl31_entrypoint.S
index ed05864..b0c46dc 100644
--- a/bl31/aarch64/bl31_entrypoint.S
+++ b/bl31/aarch64/bl31_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
  */
@@ -67,6 +67,7 @@
 		_exception_vectors=runtime_exceptions		\
 		_pie_fixup_size=BL31_LIMIT - BL31_BASE
 
+#if !RESET_TO_BL31_WITH_PARAMS
 	/* ---------------------------------------------------------------------
 	 * For RESET_TO_BL31 systems, BL31 is the first bootloader to run so
 	 * there's no argument to relay from a previous bootloader. Zero the
@@ -77,6 +78,7 @@
 	mov	x21, 0
 	mov	x22, 0
 	mov	x23, 0
+#endif /* RESET_TO_BL31_WITH_PARAMS */
 #endif /* RESET_TO_BL31 */
 
 	/* --------------------------------------------------------------------
diff --git a/bl31/bl31.mk b/bl31/bl31.mk
index 214cf2f..3964469 100644
--- a/bl31/bl31.mk
+++ b/bl31/bl31.mk
@@ -115,6 +115,10 @@
 BL31_SOURCES		+=	lib/extensions/trbe/trbe.c
 endif
 
+ifeq (${ENABLE_BRBE_FOR_NS},1)
+BL31_SOURCES		+=	lib/extensions/brbe/brbe.c
+endif
+
 ifeq (${ENABLE_SYS_REG_TRACE_FOR_NS},1)
 BL31_SOURCES		+=      lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
 endif
diff --git a/bl32/sp_min/sp_min.mk b/bl32/sp_min/sp_min.mk
index 590b032..ab1287d 100644
--- a/bl32/sp_min/sp_min.mk
+++ b/bl32/sp_min/sp_min.mk
@@ -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
 #
@@ -35,6 +35,10 @@
 ifeq (${WORKAROUND_CVE_2017_5715},1)
 BL32_SOURCES		+=	bl32/sp_min/wa_cve_2017_5715_bpiall.S	\
 				bl32/sp_min/wa_cve_2017_5715_icache_inv.S
+else
+ifeq (${WORKAROUND_CVE_2022_23960},1)
+BL32_SOURCES		+=	bl32/sp_min/wa_cve_2017_5715_icache_inv.S
+endif
 endif
 
 ifeq (${TRNG_SUPPORT},1)
diff --git a/common/fdt_fixup.c b/common/fdt_fixup.c
index de02b46..b1d628c 100644
--- a/common/fdt_fixup.c
+++ b/common/fdt_fixup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -394,6 +394,110 @@
 	return offs;
 }
 
+/*******************************************************************************
+ * fdt_add_cpu_idle_states() - add PSCI CPU idle states to cpu nodes in the DT
+ * @dtb:	pointer to the device tree blob in memory
+ * @states:	array of idle state descriptions, ending with empty element
+ *
+ * Add information about CPU idle states to the devicetree. This function
+ * assumes that CPU idle states are not already present in the devicetree, and
+ * that all CPU states are equally applicable to all CPUs.
+ *
+ * See arm/idle-states.yaml and arm/psci.yaml in the (Linux kernel) DT binding
+ * documentation for more details.
+ *
+ * Return: 0 on success, a negative error value otherwise.
+ ******************************************************************************/
+int fdt_add_cpu_idle_states(void *dtb, const struct psci_cpu_idle_state *state)
+{
+	int cpu_node, cpus_node, idle_states_node, ret;
+	uint32_t count, phandle;
+
+	ret = fdt_find_max_phandle(dtb, &phandle);
+	phandle++;
+	if (ret < 0) {
+		return ret;
+	}
+
+	cpus_node = fdt_path_offset(dtb, "/cpus");
+	if (cpus_node < 0) {
+		return cpus_node;
+	}
+
+	/* Create the idle-states node and its child nodes. */
+	idle_states_node = fdt_add_subnode(dtb, cpus_node, "idle-states");
+	if (idle_states_node < 0) {
+		return idle_states_node;
+	}
+
+	ret = fdt_setprop_string(dtb, idle_states_node, "entry-method", "psci");
+	if (ret < 0) {
+		return ret;
+	}
+
+	for (count = 0U; state->name != NULL; count++, phandle++, state++) {
+		int idle_state_node;
+
+		idle_state_node = fdt_add_subnode(dtb, idle_states_node,
+						  state->name);
+		if (idle_state_node < 0) {
+			return idle_state_node;
+		}
+
+		fdt_setprop_string(dtb, idle_state_node, "compatible",
+				   "arm,idle-state");
+		fdt_setprop_u32(dtb, idle_state_node, "arm,psci-suspend-param",
+				state->power_state);
+		if (state->local_timer_stop) {
+			fdt_setprop_empty(dtb, idle_state_node,
+					  "local-timer-stop");
+		}
+		fdt_setprop_u32(dtb, idle_state_node, "entry-latency-us",
+				state->entry_latency_us);
+		fdt_setprop_u32(dtb, idle_state_node, "exit-latency-us",
+				state->exit_latency_us);
+		fdt_setprop_u32(dtb, idle_state_node, "min-residency-us",
+				state->min_residency_us);
+		if (state->wakeup_latency_us) {
+			fdt_setprop_u32(dtb, idle_state_node,
+					"wakeup-latency-us",
+					state->wakeup_latency_us);
+		}
+		fdt_setprop_u32(dtb, idle_state_node, "phandle", phandle);
+	}
+
+	if (count == 0U) {
+		return 0;
+	}
+
+	/* Link each cpu node to the idle state nodes. */
+	fdt_for_each_subnode(cpu_node, dtb, cpus_node) {
+		const char *device_type;
+		fdt32_t *value;
+
+		/* Only process child nodes with device_type = "cpu". */
+		device_type = fdt_getprop(dtb, cpu_node, "device_type", NULL);
+		if (device_type == NULL || strcmp(device_type, "cpu") != 0) {
+			continue;
+		}
+
+		/* Allocate space for the list of phandles. */
+		ret = fdt_setprop_placeholder(dtb, cpu_node, "cpu-idle-states",
+					      count * sizeof(phandle),
+					      (void **)&value);
+		if (ret < 0) {
+			return ret;
+		}
+
+		/* Fill in the phandles of the idle state nodes. */
+		for (uint32_t i = 0U; i < count; ++i) {
+			value[i] = cpu_to_fdt32(phandle - count + i);
+		}
+	}
+
+	return 0;
+}
+
 /**
  * fdt_adjust_gic_redist() - Adjust GICv3 redistributor size
  * @dtb: Pointer to the DT blob in memory
diff --git a/docs/about/maintainers.rst b/docs/about/maintainers.rst
index c6c3e22..b9b5878 100644
--- a/docs/about/maintainers.rst
+++ b/docs/about/maintainers.rst
@@ -315,9 +315,21 @@
 :|G|: `AlexeiFedorov`_
 :|M|: Javier Almansa Sobrino <Javier.AlmansaSobrino@arm.com>
 :|G|: `javieralso-arm`_
+:|M|: Sandrine Bailleux <sandrine.bailleux@arm.com>
+:|G|: `sandrine-bailleux-arm`_
 :|F|: drivers/measured_boot
 :|F|: include/drivers/measured_boot
-:|F|: plat/arm/board/fvp/fvp_measured_boot.c
+:|F|: docs/components/measured_boot
+:|F|: plat/arm/board/fvp/fvp\*_measured_boot.c
+
+PSA Firmware Update
+^^^^^^^^^^^^^^^^^^^
+:|M|: Manish Badarkhe <manish.badarkhe@arm.com>
+:|G|: `ManishVB-Arm`_
+:|M|: Sandrine Bailleux <sandrine.bailleux@arm.com>
+:|G|: `sandrine-bailleux-arm`_
+:|F|: drivers/fwu
+:|F|: include/drivers/fwu
 
 System Control and Management Interface (SCMI) Server
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -720,16 +732,26 @@
 :|F|: docs/components/spd/optee-dispatcher.rst
 :|F|: services/spd/opteed/
 
-TLK/Trusty secure payloads
+TLK
 ^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Varun Wadekar <vwadekar@nvidia.com>
 :|G|: `vwadekar`_
 :|F|: docs/components/spd/tlk-dispatcher.rst
-:|F|: docs/components/spd/trusty-dispatcher.rst
 :|F|: include/bl32/payloads/tlk.h
 :|F|: services/spd/tlkd/
+
+Trusty secure payloads
+^^^^^^^^^^^^^^^^^^^^^^^^^^
+:|M|: Arve Hjønnevåg <arve@android.com>
+:|G|: `arve-android`_
+:|M|: Marco Nelissen <marcone@google.com>
+:|G|: `marcone`_
+:|M|: Varun Wadekar <vwadekar@nvidia.com>
+:|G|: `vwadekar`_
+:|F|: docs/components/spd/trusty-dispatcher.rst
 :|F|: services/spd/trusty/
 
+
 Test Secure Payload (TSP)
 ^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Manish Badarkhe <manish.badarkhe@arm.com>
@@ -862,5 +884,7 @@
 .. _uarif1: https://github.com/uarif1
 .. _pangupta: https://github.com/pangupta
 .. _JiafeiPan: https://github.com/JiafeiPan
+.. _arve-android: https://github.com/arve-android
+.. _marcone: https://github.com/marcone
 
 .. _Project Maintenance Process: https://developer.trustedfirmware.org/w/collaboration/project-maintenance-process/
diff --git a/docs/components/fconf/fconf_properties.rst b/docs/components/fconf/fconf_properties.rst
index 5c28a7a..20cc758 100644
--- a/docs/components/fconf/fconf_properties.rst
+++ b/docs/components/fconf/fconf_properties.rst
@@ -30,3 +30,10 @@
     - value type: <u32>
     - Image ID of the configuration.
 
+- ns-load-address [optional]
+    - value type: <u64>
+    - Physical loading base address of the configuration in the non-secure
+      memory.
+      Only needed by those configuration files which require being loaded
+      in secure memory (at load-address) as well as in non-secure memory
+      e.g. HW_CONFIG
diff --git a/docs/design/firmware-design.rst b/docs/design/firmware-design.rst
index 0831dc0..71fdfcb 100644
--- a/docs/design/firmware-design.rst
+++ b/docs/design/firmware-design.rst
@@ -131,6 +131,9 @@
    -  For other BL3x images, if the firmware configuration file is loaded by
       BL2, then its address is passed in ``arg0`` and if HW_CONFIG is loaded
       then its address is passed in ``arg1``.
+   -  In case of the Arm FVP platform, FW_CONFIG address passed in ``arg1`` to
+      BL31/SP_MIN, and the SOC_FW_CONFIG and HW_CONFIG details are retrieved
+      from FW_CONFIG device tree.
 
 BL1
 ~~~
@@ -1757,12 +1760,20 @@
                    DRAM
     0xffffffff +----------+
                :          :
-               |----------|
+    0x82100000 |----------|
                |HW_CONFIG |
-    0x83000000 |----------|  (non-secure)
+    0x82000000 |----------|  (non-secure)
                |          |
     0x80000000 +----------+
 
+               Trusted DRAM
+    0x08000000 +----------+
+               |HW_CONFIG |
+    0x07f00000 |----------|
+               :          :
+               |          |
+    0x06000000 +----------+
+
                Trusted SRAM
     0x04040000 +----------+  loaded by BL2  +----------------+
                | BL1 (rw) |  <<<<<<<<<<<<<  |                |
@@ -1790,15 +1801,18 @@
                      DRAM
     0xffffffff +--------------+
                :              :
-               |--------------|
+    0x82100000 |--------------|
                |  HW_CONFIG   |
-    0x83000000 |--------------|  (non-secure)
+    0x82000000 |--------------|  (non-secure)
                |              |
     0x80000000 +--------------+
 
-                Trusted DRAM
+                 Trusted DRAM
     0x08000000 +--------------+
-               |     BL32     |
+               |  HW_CONFIG   |
+    0x07f00000 |--------------|
+               :              :
+               |    BL32      |
     0x06000000 +--------------+
 
                  Trusted SRAM
@@ -1829,12 +1843,20 @@
                |  BL32    |  (secure)
     0xff000000 +----------+
                |          |
-               |----------|
+    0x82100000 |----------|
                |HW_CONFIG |
-    0x83000000 |----------|  (non-secure)
+    0x82000000 |----------|  (non-secure)
                |          |
     0x80000000 +----------+
 
+               Trusted DRAM
+    0x08000000 +----------+
+               |HW_CONFIG |
+    0x7f000000 |----------|
+               :          :
+               |          |
+    0x06000000 +----------+
+
                Trusted SRAM
     0x04040000 +----------+  loaded by BL2  +----------------+
                | BL1 (rw) |  <<<<<<<<<<<<<  |                |
@@ -2729,7 +2751,7 @@
 
 --------------
 
-*Copyright (c) 2013-2021, Arm Limited and Contributors. All rights reserved.*
+*Copyright (c) 2013-2022, Arm Limited and Contributors. All rights reserved.*
 
 .. _Power State Coordination Interface PDD: http://infocenter.arm.com/help/topic/com.arm.doc.den0022d/Power_State_Coordination_Interface_PDD_v1_1_DEN0022D.pdf
 .. _SMCCC: https://developer.arm.com/docs/den0028/latest
diff --git a/docs/design/reset-design.rst b/docs/design/reset-design.rst
index 7b10c95..666ee4f 100644
--- a/docs/design/reset-design.rst
+++ b/docs/design/reset-design.rst
@@ -141,19 +141,26 @@
 Platform initialization
 ~~~~~~~~~~~~~~~~~~~~~~~
 
-In this configuration, when the CPU resets to BL31 there are no parameters that
-can be passed in registers by previous boot stages. Instead, the platform code
-in BL31 needs to know, or be able to determine, the location of the BL32 (if
-required) and BL33 images and provide this information in response to the
+In this configuration, when the CPU resets to BL31 there should be no parameters
+that can be passed in registers by previous boot stages. Instead, the platform
+code in BL31 needs to know, or be able to determine, the location of the BL32
+(if required) and BL33 images and provide this information in response to the
 ``bl31_plat_get_next_image_ep_info()`` function.
 
+.. note::
+   Some platforms that configure ``RESET_TO_BL31`` might still be able to
+   receive parameters in registers depending on their actual boot sequence. On
+   those occasions, and in addition to ``RESET_TO_BL31``, these platforms should
+   set ``RESET_TO_BL31_WITH_PARAMS`` to avoid the input registers from being
+   zeroed before entering BL31.
+
 Additionally, platform software is responsible for carrying out any security
 initialisation, for example programming a TrustZone address space controller.
 This might be done by the Trusted Boot Firmware or by platform code in BL31.
 
 --------------
 
-*Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.*
+*Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.*
 
 .. |Default reset code flow| image:: ../resources/diagrams/default_reset_code.png
 .. |Reset code flow with programmable reset address| image:: ../resources/diagrams/reset_code_no_boot_type_check.png
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index 3a67b0f..d2cda4d 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -717,6 +717,11 @@
    entrypoint) or 1 (CPU reset to BL31 entrypoint).
    The default value is 0.
 
+-  ``RESET_TO_BL31_WITH_PARAMS``: If ``RESET_TO_BL31`` has been enabled, setting
+   this additional option guarantees that the input registers are not cleared
+   therefore allowing parameters to be passed to the BL31 entrypoint.
+   The default value is 0.
+
 -  ``RESET_TO_SP_MIN``: SP_MIN is the minimal AArch32 Secure Payload provided
    in TF-A. This flag configures SP_MIN entrypoint as the CPU reset vector
    instead of the BL1 entrypoint. It can take the value 0 (CPU reset to BL1
@@ -968,6 +973,11 @@
   functions that wait for an arbitrary time length (udelay and mdelay). The
   default value is 0.
 
+- ``ENABLE_BRBE_FOR_NS``: This flag enables access to the branch record buffer
+  registers from NS ELs when FEAT_BRBE is implemented. BRBE is an optional
+  architectural feature for AArch64. The default is 0 and it is automatically
+  disabled when the target architecture is AArch32.
+
 - ``ENABLE_TRBE_FOR_NS``: This flag is used to enable access of trace buffer
   control registers from NS ELs, NS-EL2 or NS-EL1(when NS-EL2 is implemented
   but unused) when FEAT_TRBE is implemented. TRBE is an optional architectural
diff --git a/docs/getting_started/initial-build.rst b/docs/getting_started/initial-build.rst
index d4a8f01..62f1941 100644
--- a/docs/getting_started/initial-build.rst
+++ b/docs/getting_started/initial-build.rst
@@ -18,16 +18,12 @@
 
    It is possible to build TF-A using Clang or Arm Compiler 6. To do so
    ``CC`` needs to point to the clang or armclang binary, which will
-   also select the clang or armclang assembler. Be aware that for Arm Compiler,
-   the GNU linker is used by default. However for Clang LLVM linker (LLD)
-   is used by default. In case of being needed the linker can be overridden
-   using the ``LD`` variable. LLVM linker (LLD) version 9 is
-   known to work with TF-A.
-
-   In both cases ``CROSS_COMPILE`` should be set as described above.
-
-   Arm Compiler 6 will be selected when the base name of the path assigned
-   to ``CC`` matches the string 'armclang'.
+   also select the clang or armclang assembler. Arm Compiler 6 will be selected
+   when the base name of the path assigned to ``CC`` matches the string
+   'armclang'. GNU binutils are required since the TF-A build system doesn't
+   currently support Arm Scatter files. Meaning the GNU linker is used by
+   default for Arm Compiler 6. Because of this dependency, ``CROSS_COMPILE``
+   should be set as described above.
 
    For AArch64 using Arm Compiler 6:
 
@@ -36,6 +32,11 @@
        export CROSS_COMPILE=<path-to-aarch64-gcc>/bin/aarch64-none-elf-
        make CC=<path-to-armclang>/bin/armclang PLAT=<platform> all
 
+   On the other hand, Clang uses LLVM linker (LLD) and other LLVM binutils by
+   default instead of GNU utilities (LLVM linker (LLD) 14.0.0 is known to
+   work with TF-A). ``CROSS_COMPILE`` need not be set for Clang. Please note,
+   that the default linker may be manually overridden using the ``LD`` variable.
+
    Clang will be selected when the base name of the path assigned to ``CC``
    contains the string 'clang'. This is to allow both clang and clang-X.Y
    to work.
@@ -44,7 +45,6 @@
 
    .. code:: shell
 
-       export CROSS_COMPILE=<path-to-aarch64-gcc>/bin/aarch64-none-elf-
        make CC=<path-to-clang>/bin/clang PLAT=<platform> all
 
 -  Change to the root directory of the TF-A source tree and build.
@@ -115,4 +115,4 @@
 
 --------------
 
-*Copyright (c) 2020, Arm Limited. All rights reserved.*
+*Copyright (c) 2020-2022, Arm Limited. All rights reserved.*
diff --git a/docs/getting_started/prerequisites.rst b/docs/getting_started/prerequisites.rst
index 92a2c83..3a54e69 100644
--- a/docs/getting_started/prerequisites.rst
+++ b/docs/getting_started/prerequisites.rst
@@ -27,8 +27,8 @@
 target the Armv7-A or Armv8-A architectures:
 
 - GCC >= 11.2-2022.02 (from the `Arm Developer website`_)
-- Clang >= 4.0
-- Arm Compiler >= 6.0
+- Clang >= 14.0.0
+- Arm Compiler >= 6.18
 
 In addition, a native compiler is required to build the supporting tools.
 
@@ -160,7 +160,7 @@
 
 --------------
 
-*Copyright (c) 2021, Arm Limited. All rights reserved.*
+*Copyright (c) 2021-2022, Arm Limited. All rights reserved.*
 
 .. _Arm Developer website: https://developer.arm.com/tools-and-software/open-source-software/developer-tools/gnu-toolchain/downloads
 .. _Gerrit Code Review: https://www.gerritcodereview.com/
diff --git a/docs/plat/arm/fvp/index.rst b/docs/plat/arm/fvp/index.rst
index 2aaf195..9280f7b 100644
--- a/docs/plat/arm/fvp/index.rst
+++ b/docs/plat/arm/fvp/index.rst
@@ -392,7 +392,8 @@
 -  BL1 is loaded at the start of the Trusted ROM.
 -  The Firmware Image Package is loaded at the start of NOR FLASH0.
 -  The firmware loads the FDT packaged in FIP to the DRAM. The FDT load address
-   is specified via the ``hw_config_addr`` property in `TB_FW_CONFIG for FVP`_.
+   is specified via the ``load-address`` property in the ``hw-config`` node of
+   `FW_CONFIG for FVP`_.
 -  The default use-case for the Foundation FVP is to use the ``--gicv3`` option
    and enable the GICv3 device in the model. Note that without this option,
    the Foundation FVP defaults to legacy (Versatile Express) memory map which
@@ -643,9 +644,9 @@
 
 --------------
 
-*Copyright (c) 2019-2021, Arm Limited. All rights reserved.*
+*Copyright (c) 2019-2022, Arm Limited. All rights reserved.*
 
-.. _TB_FW_CONFIG for FVP: https://git.trustedfirmware.org/TF-A/trusted-firmware-a.git/tree/plat/arm/board/fvp/fdts/fvp_tb_fw_config.dts
+.. _FW_CONFIG for FVP: https://git.trustedfirmware.org/TF-A/trusted-firmware-a.git/tree/plat/arm/board/fvp/fdts/fvp_fw_config.dts
 .. _Arm's website: `FVP models`_
 .. _FVP models: https://developer.arm.com/products/system-design/fixed-virtual-platforms
 .. _Linaro Release 20.01: http://releases.linaro.org/members/arm/platforms/20.01
diff --git a/docs/plat/xilinx-versal.rst b/docs/plat/xilinx-versal.rst
index d65b048..91ad6f1 100644
--- a/docs/plat/xilinx-versal.rst
+++ b/docs/plat/xilinx-versal.rst
@@ -43,6 +43,8 @@
 
 *   `VERSAL_PLATFORM`: Select the platform. Options:
     -   `versal_virt`	: Versal Virtual platform
+    -   `spp_itr6`	: SPP ITR6
+    -   `emu_it6`	: EMU ITR6
 
 # PLM->TF-A Parameter Passing
 ------------------------------
diff --git a/drivers/arm/smmu/smmu_v3.c b/drivers/arm/smmu/smmu_v3.c
index a082a81..45f6df9 100644
--- a/drivers/arm/smmu/smmu_v3.c
+++ b/drivers/arm/smmu/smmu_v3.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,7 @@
 #include <drivers/arm/smmu_v3.h>
 #include <drivers/delay_timer.h>
 #include <lib/mmio.h>
+#include <arch_features.h>
 
 /* SMMU poll number of retries */
 #define SMMU_POLL_TIMEOUT_US	U(1000)
@@ -79,14 +80,74 @@
 	if (smmuv3_security_init(smmu_base) != 0)
 		return -1;
 
-	/* Check if the SMMU supports secure state */
-	if ((mmio_read_32(smmu_base + SMMU_S_IDR1) &
-				SMMU_S_IDR1_SECURE_IMPL) == 0U)
-		return 0;
+#if ENABLE_RME
+
+	if (get_armv9_2_feat_rme_support() != 0U) {
+		if ((mmio_read_32(smmu_base + SMMU_ROOT_IDR0) &
+				  SMMU_ROOT_IDR0_ROOT_IMPL) == 0U) {
+			WARN("Skip SMMU GPC configuration.\n");
+		} else {
+			uint64_t gpccr_el3 = read_gpccr_el3();
+			uint64_t gptbr_el3 = read_gptbr_el3();
+
+			/* SMMU_ROOT_GPT_BASE_CFG[16] is RES0. */
+			gpccr_el3 &= ~(1UL << 16);
+
+			/*
+			 * TODO: SMMU_ROOT_GPT_BASE_CFG is 64b in the spec,
+			 * but SMMU model only accepts 32b access.
+			 */
+			mmio_write_32(smmu_base + SMMU_ROOT_GPT_BASE_CFG,
+				      gpccr_el3);
+
+			/*
+			 * pa_gpt_table_base[51:12] maps to GPTBR_EL3[39:0]
+			 * whereas it maps to SMMU_ROOT_GPT_BASE[51:12]
+			 * hence needs a 12 bit left shit.
+			 */
+			mmio_write_64(smmu_base + SMMU_ROOT_GPT_BASE,
+				      gptbr_el3 << 12);
+
+			/*
+			 * ACCESSEN=1: SMMU- and client-originated accesses are
+			 *             not terminated by this mechanism.
+			 * GPCEN=1: All clients and SMMU-originated accesses,
+			 *          except GPT-walks, are subject to GPC.
+			 */
+			mmio_setbits_32(smmu_base + SMMU_ROOT_CR0,
+					SMMU_ROOT_CR0_GPCEN |
+					SMMU_ROOT_CR0_ACCESSEN);
+
+			/* Poll for ACCESSEN and GPCEN ack bits. */
+			if (smmuv3_poll(smmu_base + SMMU_ROOT_CR0ACK,
+					SMMU_ROOT_CR0_GPCEN |
+					SMMU_ROOT_CR0_ACCESSEN,
+					SMMU_ROOT_CR0_GPCEN |
+					SMMU_ROOT_CR0_ACCESSEN) != 0) {
+				WARN("Failed enabling SMMU GPC.\n");
+
+				/*
+				 * Do not return in error, but fall back to
+				 * invalidating all entries through the secure
+				 * register file.
+				 */
+			}
+		}
+	}
+
+#endif /* ENABLE_RME */
+
 	/*
 	 * Initiate invalidation of secure caches and TLBs if the SMMU
 	 * supports secure state. If not, it's implementation defined
 	 * as to how SMMU_S_INIT register is accessed.
+	 * Arm SMMU Arch RME supplement, section 3.4: all SMMU registers
+	 * specified to be accessible only in secure physical address space are
+	 * additionally accessible in root physical address space in an SMMU
+	 * with RME.
+	 * Section 3.3: as GPT information is permitted to be cached in a TLB,
+	 * the SMMU_S_INIT.INV_ALL mechanism also invalidates GPT information
+	 * cached in TLBs.
 	 */
 	mmio_write_32(smmu_base + SMMU_S_INIT, SMMU_S_INIT_INV_ALL);
 
diff --git a/drivers/ufs/ufs.c b/drivers/ufs/ufs.c
index 15d80ae..7db6c0b 100644
--- a/drivers/ufs/ufs.c
+++ b/drivers/ufs/ufs.c
@@ -146,11 +146,43 @@
 	return 0;
 }
 
+static int ufshc_hce_disable(uintptr_t base)
+{
+	unsigned int data;
+	int timeout;
+
+	/* Disable Host Controller */
+	mmio_write_32(base + HCE, HCE_DISABLE);
+	timeout = HCE_DISABLE_TIMEOUT_US;
+	do {
+		data = mmio_read_32(base + HCE);
+		if ((data & HCE_ENABLE) == HCE_DISABLE) {
+			break;
+		}
+		udelay(1);
+	} while (--timeout > 0);
+
+	if (timeout <= 0) {
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+
 static int ufshc_reset(uintptr_t base)
 {
 	unsigned int data;
 	int retries, result;
 
+	/* disable controller if enabled */
+	if (mmio_read_32(base + HCE) & HCE_ENABLE) {
+		result = ufshc_hce_disable(base);
+		if (result != 0) {
+			return -EIO;
+		}
+	}
+
 	for (retries = 0; retries < HCE_ENABLE_OUTER_RETRIES; ++retries) {
 		result = ufshc_hce_enable(base);
 		if (result == 0) {
@@ -408,7 +440,7 @@
 		break;
 	case QUERY_WRITE_ATTR:
 		query_upiu->query_func = QUERY_FUNC_STD_WRITE;
-		memcpy((void *)&query_upiu->ts.attr.value, (void *)buf, length);
+		query_upiu->ts.attr.value = htobe32(*((uint32_t *)buf));
 		break;
 	default:
 		assert(0);
@@ -594,12 +626,14 @@
 	case QUERY_READ_FLAG:
 		*(uint32_t *)buf = (uint32_t)resp->ts.flag.value;
 		break;
-	case QUERY_READ_ATTR:
 	case QUERY_READ_DESC:
 		memcpy((void *)buf,
 		       (void *)(utrd.resp_upiu + sizeof(query_resp_upiu_t)),
 		       size);
 		break;
+	case QUERY_READ_ATTR:
+		*(uint32_t *)buf = htobe32(resp->ts.attr.value);
+		break;
 	default:
 		/* Do nothing in default case */
 		break;
@@ -733,16 +767,41 @@
 	return size - resp->res_trans_cnt;
 }
 
+static int ufs_set_fdevice_init(void)
+{
+	unsigned int result;
+	int timeout;
+
+	ufs_set_flag(FLAG_DEVICE_INIT);
+
+	timeout = FDEVICEINIT_TIMEOUT_MS;
+	do {
+		result = ufs_read_flag(FLAG_DEVICE_INIT);
+		if (!result) {
+			break;
+		}
+		mdelay(5);
+		timeout -= 5;
+	} while (timeout > 0);
+
+	if (result != 0U) {
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
 static void ufs_enum(void)
 {
 	unsigned int blk_num, blk_size;
-	int i;
+	int i, result;
 
 	ufs_verify_init();
 	ufs_verify_ready();
 
-	ufs_set_flag(FLAG_DEVICE_INIT);
-	mdelay(200);
+	result = ufs_set_fdevice_init();
+	assert(result == 0);
+
 	/* dump available LUNs */
 	for (i = 0; i < UFS_MAX_LUNS; i++) {
 		ufs_read_capacity(i, &blk_num, &blk_size);
@@ -751,6 +810,8 @@
 			     i, blk_num, blk_size);
 		}
 	}
+
+	(void)result;
 }
 
 static void ufs_get_device_info(struct ufs_dev_desc *card_data)
diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index 83c469b..5866af8 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -234,6 +234,11 @@
 #define ID_AA64DFR0_MTPMU_MASK		ULL(0xf)
 #define ID_AA64DFR0_MTPMU_SUPPORTED	ULL(1)
 
+/* ID_AA64DFR0_EL1.BRBE definitions */
+#define ID_AA64DFR0_BRBE_SHIFT		U(52)
+#define ID_AA64DFR0_BRBE_MASK		ULL(0xf)
+#define ID_AA64DFR0_BRBE_SUPPORTED	ULL(1)
+
 /* ID_AA64ISAR0_EL1 definitions */
 #define ID_AA64ISAR0_RNDR_SHIFT	U(60)
 #define ID_AA64ISAR0_RNDR_MASK	ULL(0xf)
@@ -513,6 +518,8 @@
 #define MDCR_EnPMSN_BIT		(ULL(1) << 36)
 #define MDCR_MPMX_BIT		(ULL(1) << 35)
 #define MDCR_MCCD_BIT		(ULL(1) << 34)
+#define MDCR_SBRBE_SHIFT	U(32)
+#define MDCR_SBRBE_MASK		ULL(0x3)
 #define MDCR_NSTB(x)		((x) << 24)
 #define MDCR_NSTB_EL1		ULL(0x3)
 #define MDCR_NSTBE		(ULL(1) << 26)
diff --git a/include/common/bl_common.h b/include/common/bl_common.h
index 8cb4990..3a06cfb 100644
--- a/include/common/bl_common.h
+++ b/include/common/bl_common.h
@@ -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
  */
@@ -81,6 +81,10 @@
 #define __RODATA_END__			Load$$__RODATA_EPILOGUE__$$Base
 #define __RT_SVC_DESCS_START__		Load$$__RT_SVC_DESCS__$$Base
 #define __RT_SVC_DESCS_END__		Load$$__RT_SVC_DESCS__$$Limit
+#if SPMC_AT_EL3
+#define __EL3_LP_DESCS_START__		Load$$__EL3_LP_DESCS__$$Base
+#define __EL3_LP_DESCS_END__		Load$$__EL3_LP_DESCS__$$Limit
+#endif
 #define __RW_START__			Load$$LR$$LR_RW_DATA$$Base
 #define __RW_END__			Load$$LR$$LR_END$$Base
 #define __SPM_SHIM_EXCEPTIONS_START__	Load$$__SPM_SHIM_EXCEPTIONS__$$Base
diff --git a/include/common/bl_common.ld.h b/include/common/bl_common.ld.h
index 9888a3c..080e331 100644
--- a/include/common/bl_common.ld.h
+++ b/include/common/bl_common.ld.h
@@ -39,6 +39,16 @@
 	KEEP(*(rt_svc_descs))				\
 	__RT_SVC_DESCS_END__ = .;
 
+#if SPMC_AT_EL3
+#define EL3_LP_DESCS					\
+	. = ALIGN(STRUCT_ALIGN);			\
+	__EL3_LP_DESCS_START__ = .;			\
+	KEEP(*(el3_lp_descs))				\
+	__EL3_LP_DESCS_END__ = .;
+#else
+#define EL3_LP_DESCS
+#endif
+
 #define PMF_SVC_DESCS					\
 	. = ALIGN(STRUCT_ALIGN);			\
 	__PMF_SVC_DESCS_START__ = .;			\
@@ -89,7 +99,8 @@
 	PARSER_LIB_DESCS				\
 	CPU_OPS						\
 	GOT						\
-	BASE_XLAT_TABLE_RO
+	BASE_XLAT_TABLE_RO				\
+	EL3_LP_DESCS
 
 /*
  * .data must be placed at a lower address than the stacks if the stack
diff --git a/include/common/fdt_fixup.h b/include/common/fdt_fixup.h
index 7a590b2..c35e9be 100644
--- a/include/common/fdt_fixup.h
+++ b/include/common/fdt_fixup.h
@@ -7,14 +7,29 @@
 #ifndef FDT_FIXUP_H
 #define FDT_FIXUP_H
 
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
 #define INVALID_BASE_ADDR	((uintptr_t)~0UL)
 
+struct psci_cpu_idle_state {
+	const char *name;
+	uint32_t power_state;
+	bool local_timer_stop;
+	uint32_t entry_latency_us;
+	uint32_t exit_latency_us;
+	uint32_t min_residency_us;
+	uint32_t wakeup_latency_us;
+};
+
 int dt_add_psci_node(void *fdt);
 int dt_add_psci_cpu_enable_methods(void *fdt);
 int fdt_add_reserved_memory(void *dtb, const char *node_name,
 			    uintptr_t base, size_t size);
 int fdt_add_cpus_node(void *dtb, unsigned int afflv0,
 		      unsigned int afflv1, unsigned int afflv2);
+int fdt_add_cpu_idle_states(void *dtb, const struct psci_cpu_idle_state *state);
 int fdt_adjust_gic_redist(void *dtb, unsigned int nr_cores, uintptr_t gicr_base,
 			  unsigned int gicr_frame_size);
 
diff --git a/include/drivers/arm/smmu_v3.h b/include/drivers/arm/smmu_v3.h
index a820a44..e60c754 100644
--- a/include/drivers/arm/smmu_v3.h
+++ b/include/drivers/arm/smmu_v3.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,7 @@
 
 #include <stdint.h>
 #include <lib/utils_def.h>
+#include <platform_def.h>
 
 /* SMMUv3 register offsets from device base */
 #define SMMU_GBPA	U(0x0044)
@@ -16,6 +17,26 @@
 #define SMMU_S_INIT	U(0x803c)
 #define SMMU_S_GBPA	U(0x8044)
 
+/*
+ * TODO: SMMU_ROOT_PAGE_OFFSET is platform specific.
+ * Currently defined as a command line model parameter.
+ */
+#if ENABLE_RME
+
+#define SMMU_ROOT_PAGE_OFFSET	(PLAT_ARM_SMMUV3_ROOT_REG_OFFSET)
+#define SMMU_ROOT_IDR0		U(SMMU_ROOT_PAGE_OFFSET + 0x0000)
+#define SMMU_ROOT_IIDR		U(SMMU_ROOT_PAGE_OFFSET + 0x0008)
+#define SMMU_ROOT_CR0		U(SMMU_ROOT_PAGE_OFFSET + 0x0020)
+#define SMMU_ROOT_CR0ACK	U(SMMU_ROOT_PAGE_OFFSET + 0x0024)
+#define SMMU_ROOT_GPT_BASE	U(SMMU_ROOT_PAGE_OFFSET + 0x0028)
+#define SMMU_ROOT_GPT_BASE_CFG	U(SMMU_ROOT_PAGE_OFFSET + 0x0030)
+#define SMMU_ROOT_GPF_FAR	U(SMMU_ROOT_PAGE_OFFSET + 0x0038)
+#define SMMU_ROOT_GPT_CFG_FAR	U(SMMU_ROOT_PAGE_OFFSET + 0x0040)
+#define SMMU_ROOT_TLBI		U(SMMU_ROOT_PAGE_OFFSET + 0x0050)
+#define SMMU_ROOT_TLBI_CTRL	U(SMMU_ROOT_PAGE_OFFSET + 0x0058)
+
+#endif /* ENABLE_RME */
+
 /* SMMU_GBPA register fields */
 #define SMMU_GBPA_UPDATE		(1UL << 31)
 #define SMMU_GBPA_ABORT			(1UL << 20)
@@ -30,6 +51,13 @@
 #define SMMU_S_GBPA_UPDATE		(1UL << 31)
 #define SMMU_S_GBPA_ABORT		(1UL << 20)
 
+/* SMMU_ROOT_IDR0 register fields */
+#define SMMU_ROOT_IDR0_ROOT_IMPL	(1UL << 0)
+
+/* SMMU_ROOT_CR0 register fields */
+#define SMMU_ROOT_CR0_GPCEN		(1UL << 1)
+#define SMMU_ROOT_CR0_ACCESSEN		(1UL << 0)
+
 int smmuv3_init(uintptr_t smmu_base);
 int smmuv3_security_init(uintptr_t smmu_base);
 
diff --git a/include/drivers/ufs.h b/include/drivers/ufs.h
index c074e85..4a5e464 100644
--- a/include/drivers/ufs.h
+++ b/include/drivers/ufs.h
@@ -69,6 +69,7 @@
 /* Host Controller Enable */
 #define HCE				0x34
 #define HCE_ENABLE			1
+#define HCE_DISABLE			0
 
 /* Host UIC Error Code PHY Adapter Layer */
 #define UECPA				0x38
@@ -264,6 +265,9 @@
 #define HCE_ENABLE_OUTER_RETRIES	3
 #define HCE_ENABLE_INNER_RETRIES	50
 #define HCE_ENABLE_TIMEOUT_US		100
+#define HCE_DISABLE_TIMEOUT_US		1000
+
+#define FDEVICEINIT_TIMEOUT_MS	        1500
 
 /**
  * ufs_dev_desc - ufs device details from the device descriptor
diff --git a/include/lib/extensions/brbe.h b/include/lib/extensions/brbe.h
new file mode 100644
index 0000000..aac1ace
--- /dev/null
+++ b/include/lib/extensions/brbe.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef BRBE_H
+#define BRBE_H
+
+void brbe_enable(void);
+
+#endif /* BRBE_H */
diff --git a/include/lib/fconf/fconf_dyn_cfg_getter.h b/include/lib/fconf/fconf_dyn_cfg_getter.h
index ff51c6c..43f298e 100644
--- a/include/lib/fconf/fconf_dyn_cfg_getter.h
+++ b/include/lib/fconf/fconf_dyn_cfg_getter.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2019-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,6 +18,13 @@
 	uintptr_t config_addr;
 	uint32_t config_max_size;
 	unsigned int config_id;
+	/*
+	 * Load address in non-secure memory. Only needed by those
+	 * configuration files which require being loaded in secure
+	 * memory (at config_addr) as well as in non-secure memory
+	 * - e.g. HW_CONFIG
+	 */
+	uintptr_t ns_config_addr;
 };
 
 unsigned int dyn_cfg_dtb_info_get_index(unsigned int config_id);
@@ -25,7 +32,8 @@
 int fconf_populate_dtb_registry(uintptr_t config);
 
 /* Set config information in global DTB array */
-void set_config_info(uintptr_t config_addr, uint32_t config_max_size,
-			unsigned int config_id);
+void set_config_info(uintptr_t config_addr, uintptr_t ns_config_addr,
+		     uint32_t config_max_size,
+		     unsigned int config_id);
 
 #endif /* FCONF_DYN_CFG_GETTER_H */
diff --git a/include/plat/arm/common/arm_def.h b/include/plat/arm/common/arm_def.h
index 2af8c11..a8211bd 100644
--- a/include/plat/arm/common/arm_def.h
+++ b/include/plat/arm/common/arm_def.h
@@ -284,12 +284,10 @@
 					ARM_EL3_TZC_DRAM1_SIZE,		\
 					MT_MEMORY | MT_RW | EL3_PAS)
 
-#if defined(SPD_spmd)
 #define ARM_MAP_TRUSTED_DRAM	MAP_REGION_FLAT(			\
 					PLAT_ARM_TRUSTED_DRAM_BASE,	\
 					PLAT_ARM_TRUSTED_DRAM_SIZE,	\
 					MT_MEMORY | MT_RW | MT_SECURE)
-#endif
 
 #if ENABLE_RME
 #define ARM_MAP_RMM_DRAM	MAP_REGION_FLAT(			\
@@ -620,7 +618,7 @@
  * Trusted DRAM (if available) or the DRAM region secured by the TrustZone
  * controller.
  */
-# if SPM_MM
+# if SPM_MM || SPMC_AT_EL3
 #  define TSP_SEC_MEM_BASE		(ARM_AP_TZC_DRAM1_BASE + ULL(0x200000))
 #  define TSP_SEC_MEM_SIZE		(ARM_AP_TZC_DRAM1_SIZE - ULL(0x200000))
 #  define BL32_BASE			(ARM_AP_TZC_DRAM1_BASE + ULL(0x200000))
@@ -666,12 +664,13 @@
 
 /*
  * BL32 is mandatory in AArch32. In AArch64, undefine BL32_BASE if there is no
- * SPD and no SPM-MM, as they are the only ones that can be used as BL32.
+ * SPD and no SPM-MM and no SPMC-AT-EL3, as they are the only ones that can be
+ * used as BL32.
  */
 #if defined(__aarch64__) && !JUNO_AARCH32_EL3_RUNTIME
-# if defined(SPD_none) && !SPM_MM
+# if defined(SPD_none) && !SPM_MM && !SPMC_AT_EL3
 #  undef BL32_BASE
-# endif /* defined(SPD_none) && !SPM_MM */
+# endif /* defined(SPD_none) && !SPM_MM || !SPMC_AT_EL3 */
 #endif /* defined(__aarch64__) && !JUNO_AARCH32_EL3_RUNTIME */
 
 /*******************************************************************************
diff --git a/include/services/el3_spmc_logical_sp.h b/include/services/el3_spmc_logical_sp.h
new file mode 100644
index 0000000..7ec9958
--- /dev/null
+++ b/include/services/el3_spmc_logical_sp.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef EL3_SP_H
+#define EL3_SP_H
+
+#include <common/bl_common.h>
+#include <lib/cassert.h>
+
+/*******************************************************************************
+ * Structure definition, typedefs & constants for the Logical SPs.
+ ******************************************************************************/
+
+typedef uint64_t (*direct_msg_handler)(uint32_t smc_fid, bool secure_origin,
+				       uint64_t x1, uint64_t x2, uint64_t x3,
+				       uint64_t x4, void *cookie, void *handle,
+				       uint64_t flags);
+
+/* Prototype for logical partition initializing function. */
+typedef int32_t (*ffa_partition_init_t)(void);
+
+/* Logical Partition Descriptor. */
+struct el3_lp_desc {
+	ffa_partition_init_t init;
+	uint16_t sp_id;
+	uint32_t properties;
+	uint32_t uuid[4];  /* Little Endian. */
+	direct_msg_handler direct_req;
+	const char *debug_name;
+};
+
+/* Convenience macro to declare a logical partition descriptor. */
+#define DECLARE_LOGICAL_PARTITION(_name, _init, _sp_id, _uuid, _properties, \
+				  _direct_req)				    \
+	static const struct el3_lp_desc __partition_desc_ ## _name	    \
+		__section("el3_lp_descs") __used = {			    \
+			.debug_name = #_name,				    \
+			.init = (_init),				    \
+			.sp_id = (_sp_id),				    \
+			.uuid = _uuid,					    \
+			.properties = (_properties),			    \
+			.direct_req = (_direct_req),			    \
+		}
+
+
+/*******************************************************************************
+ * Function & variable prototypes.
+ ******************************************************************************/
+int el3_sp_desc_validate(void);
+uintptr_t handle_el3_sp(uint32_t smc_fid, void *cookie, void *handle,
+						unsigned int flags);
+IMPORT_SYM(uintptr_t, __EL3_LP_DESCS_START__,	EL3_LP_DESCS_START);
+IMPORT_SYM(uintptr_t, __EL3_LP_DESCS_END__,	EL3_LP_DESCS_END);
+
+#define EL3_LP_DESCS_COUNT ((EL3_LP_DESCS_END - EL3_LP_DESCS_START) \
+			  / sizeof(struct el3_lp_desc))
+
+#endif /* EL3_SP_H */
diff --git a/include/services/ffa_svc.h b/include/services/ffa_svc.h
index d3fb012..0836579 100644
--- a/include/services/ffa_svc.h
+++ b/include/services/ffa_svc.h
@@ -38,6 +38,7 @@
 #define FFA_VERSION_MINOR_SHIFT		0
 #define FFA_VERSION_MINOR_MASK		U(0xFFFF)
 #define FFA_VERSION_BIT31_MASK 		U(0x1u << 31)
+#define FFA_VERSION_MASK		U(0xFFFFFFFF)
 
 
 #define MAKE_FFA_VERSION(major, minor) 	\
@@ -164,6 +165,13 @@
 	FFA_FID(SMC_64, FFA_FNUM_NOTIFICATION_INFO_GET)
 
 /*
+ * FF-A partition properties values.
+ */
+#define FFA_PARTITION_DIRECT_REQ_RECV	U(1 << 0)
+#define FFA_PARTITION_DIRECT_REQ_SEND	U(1 << 1)
+#define FFA_PARTITION_INDIRECT_MSG	U(1 << 2)
+
+/*
  * Reserve a special value for traffic targeted to the Hypervisor or SPM.
  */
 #define FFA_TARGET_INFO_MBZ		U(0x0)
diff --git a/include/tools_share/sptool.h b/include/tools_share/sptool.h
deleted file mode 100644
index 53668e0..0000000
--- a/include/tools_share/sptool.h
+++ /dev/null
@@ -1,25 +0,0 @@
-/*
- * Copyright (c) 2018-2020, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef SPTOOL_H
-#define SPTOOL_H
-
-#include <stdint.h>
-
-/* 4 Byte magic name "SPKG" */
-#define SECURE_PARTITION_MAGIC		0x474B5053
-
-/* Header for a secure partition package. */
-struct sp_pkg_header {
-	uint32_t magic;
-	uint32_t version;
-	uint32_t pm_offset;
-	uint32_t pm_size;
-	uint32_t img_offset;
-	uint32_t img_size;
-};
-
-#endif /* SPTOOL_H */
diff --git a/lib/cpus/aarch32/cortex_a15.S b/lib/cpus/aarch32/cortex_a15.S
index ab136ad..1143e9b 100644
--- a/lib/cpus/aarch32/cortex_a15.S
+++ b/lib/cpus/aarch32/cortex_a15.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -100,6 +100,15 @@
 	bx	lr
 endfunc check_errata_cve_2017_5715
 
+func check_errata_cve_2022_23960
+#if WORKAROUND_CVE_2022_23960
+	mov	r0, #ERRATA_APPLIES
+#else
+	mov	r0, #ERRATA_MISSING
+#endif
+	bx	lr
+endfunc check_errata_cve_2022_23960
+
 #if REPORT_ERRATA
 /*
  * Errata printing function for Cortex A15. Must follow AAPCS.
@@ -117,6 +126,7 @@
 	report_errata ERRATA_A15_816470, cortex_a15, 816470
 	report_errata ERRATA_A15_827671, cortex_a15, 827671
 	report_errata WORKAROUND_CVE_2017_5715, cortex_a15, cve_2017_5715
+	report_errata WORKAROUND_CVE_2022_23960, cortex_a15, cve_2022_23960
 
 	pop	{r12, lr}
 	bx	lr
@@ -131,11 +141,11 @@
 	bl	errata_a15_827671_wa
 #endif
 
-#if IMAGE_BL32 && WORKAROUND_CVE_2017_5715
+#if IMAGE_BL32 && (WORKAROUND_CVE_2017_5715 || WORKAROUND_CVE_2022_23960)
 	ldcopr	r0, ACTLR
 	orr	r0, #CORTEX_A15_ACTLR_INV_BTB_BIT
 	stcopr	r0, ACTLR
-	ldr	r0, =workaround_icache_inv_runtime_exceptions
+	ldr	r0, =wa_cve_2017_5715_icache_inv_vbar
 	stcopr	r0, VBAR
 	stcopr	r0, MVBAR
 	/* isb will be applied in the course of the reset func */
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 7a0073f..0f09ebe 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -20,6 +20,7 @@
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/el3_runtime/pubsub_events.h>
 #include <lib/extensions/amu.h>
+#include <lib/extensions/brbe.h>
 #include <lib/extensions/mpam.h>
 #include <lib/extensions/sme.h>
 #include <lib/extensions/spe.h>
@@ -459,6 +460,10 @@
 	trbe_enable();
 #endif /* ENABLE_TRBE_FOR_NS */
 
+#if ENABLE_BRBE_FOR_NS
+	brbe_enable();
+#endif /* ENABLE_BRBE_FOR_NS */
+
 #if ENABLE_SYS_REG_TRACE_FOR_NS
 	sys_reg_trace_enable(ctx);
 #endif /* ENABLE_SYS_REG_TRACE_FOR_NS */
@@ -828,6 +833,12 @@
 	 */
 	write_scr_el3(read_scr_el3() | SCR_NS_BIT);
 
+	/*
+	 * Ensure the NS bit change is committed before the EL2/EL1
+	 * state restoration.
+	 */
+	isb();
+
 	/* Restore EL2 and EL1 sysreg contexts */
 	cm_el2_sysregs_context_restore(NON_SECURE);
 	cm_el1_sysregs_context_restore(NON_SECURE);
diff --git a/lib/extensions/brbe/brbe.c b/lib/extensions/brbe/brbe.c
new file mode 100644
index 0000000..6975b04
--- /dev/null
+++ b/lib/extensions/brbe/brbe.c
@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+
+static bool brbe_supported(void)
+{
+	uint64_t features;
+
+	features = read_id_aa64dfr0_el1() >> ID_AA64DFR0_BRBE_SHIFT;
+	return ((features & ID_AA64DFR0_BRBE_MASK) ==
+		ID_AA64DFR0_BRBE_SUPPORTED);
+}
+
+void brbe_enable(void)
+{
+	uint64_t val;
+
+	if (brbe_supported()) {
+		/*
+		 * MDCR_EL3.SBRBE = 0b01
+		 *
+		 * Allows BRBE usage in non-secure world and prohibited in
+		 * secure world.
+		 */
+		val = read_mdcr_el3();
+		val &= ~(MDCR_SBRBE_MASK << MDCR_SBRBE_SHIFT);
+		val |= (0x1UL << MDCR_SBRBE_SHIFT);
+		write_mdcr_el3(val);
+	}
+}
diff --git a/lib/fconf/fconf_dyn_cfg_getter.c b/lib/fconf/fconf_dyn_cfg_getter.c
index 34623fb..3038c09 100644
--- a/lib/fconf/fconf_dyn_cfg_getter.c
+++ b/lib/fconf/fconf_dyn_cfg_getter.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2019-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -29,13 +29,15 @@
  * This function is used to alloc memory for config information from
  * global pool and set the configuration information.
  */
-void set_config_info(uintptr_t config_addr, uint32_t config_max_size,
-			unsigned int config_id)
+void set_config_info(uintptr_t config_addr, uintptr_t ns_config_addr,
+		     uint32_t config_max_size,
+		     unsigned int config_id)
 {
 	struct dyn_cfg_dtb_info_t *dtb_info;
 
 	dtb_info = pool_alloc(&dtb_info_pool);
 	dtb_info->config_addr = config_addr;
+	dtb_info->ns_config_addr = ns_config_addr;
 	dtb_info->config_max_size = config_max_size;
 	dtb_info->config_id = config_id;
 }
@@ -88,7 +90,7 @@
 	 */
 	if (dtb_infos[0].config_id == 0U) {
 		uint32_t config_max_size = fdt_totalsize(dtb);
-		set_config_info(config, config_max_size, FW_CONFIG_ID);
+		set_config_info(config, ~0UL, config_max_size, FW_CONFIG_ID);
 	}
 
 	/* Find the node offset point to "fconf,dyn_cfg-dtb_registry" compatible property */
@@ -102,6 +104,7 @@
 	fdt_for_each_subnode(child, dtb, node) {
 		uint32_t config_max_size, config_id;
 		uintptr_t config_addr;
+		uintptr_t ns_config_addr = ~0UL;
 		uint64_t val64;
 
 		/* Read configuration dtb information */
@@ -129,7 +132,14 @@
 		VERBOSE("\tmax-size = 0x%x\n", config_max_size);
 		VERBOSE("\tconfig-id = %u\n", config_id);
 
-		set_config_info(config_addr, config_max_size, config_id);
+		rc = fdt_read_uint64(dtb, child, "ns-load-address", &val64);
+		if (rc == 0) {
+			ns_config_addr = (uintptr_t)val64;
+			VERBOSE("\tns-load-address = %lx\n", ns_config_addr);
+		}
+
+		set_config_info(config_addr, ns_config_addr, config_max_size,
+				config_id);
 	}
 
 	if ((child < 0) && (child != -FDT_ERR_NOTFOUND)) {
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index bf8771d..6e57237 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -257,6 +257,9 @@
 # By default, BL1 acts as the reset handler, not BL31
 RESET_TO_BL31			:= 0
 
+# By default, clear the input registers when RESET_TO_BL31 is enabled
+RESET_TO_BL31_WITH_PARAMS	:= 0
+
 # For Chain of Trust
 SAVE_KEYS			:= 0
 
@@ -437,6 +440,11 @@
 	override ENABLE_TRBE_FOR_NS	:= 0
 endif
 
+# By default, disable access to branch record buffer control registers from NS
+# lower ELs i.e. NS-EL2, or NS-EL1 if NS-EL2 implemented but unused
+# if FEAT_BRBE is implemented.
+ENABLE_BRBE_FOR_NS		:= 0
+
 # By default, disable access of trace system registers from NS lower
 # ELs  i.e. NS-EL2, or NS-EL1 if NS-EL2 implemented but unused if
 # system register trace is implemented.
diff --git a/plat/allwinner/common/allwinner-common.mk b/plat/allwinner/common/allwinner-common.mk
index 34fdaf6..61c1dbe 100644
--- a/plat/allwinner/common/allwinner-common.mk
+++ b/plat/allwinner/common/allwinner-common.mk
@@ -27,6 +27,7 @@
 				plat/common/plat_gicv2.c		\
 				plat/common/plat_psci_common.c		\
 				${AW_PLAT}/common/sunxi_bl31_setup.c	\
+				${AW_PLAT}/${PLAT}/sunxi_idle_states.c	\
 				${AW_PLAT}/common/sunxi_pm.c		\
 				${AW_PLAT}/${PLAT}/sunxi_power.c	\
 				${AW_PLAT}/common/sunxi_security.c	\
@@ -65,6 +66,23 @@
 $(eval $(call assert_boolean,SUNXI_SETUP_REGULATORS))
 $(eval $(call add_define,SUNXI_SETUP_REGULATORS))
 
+SUNXI_BL31_IN_DRAM	?=	0
+$(eval $(call assert_boolean,SUNXI_BL31_IN_DRAM))
+
+ifeq (${SUNXI_BL31_IN_DRAM},1)
+SUNXI_AMEND_DTB		:=	1
+$(eval $(call add_define,SUNXI_BL31_IN_DRAM))
+endif
+
+SUNXI_AMEND_DTB		?=	0
+$(eval $(call assert_boolean,SUNXI_AMEND_DTB))
+$(eval $(call add_define,SUNXI_AMEND_DTB))
+
+ifeq (${SUNXI_AMEND_DTB},1)
+BL31_SOURCES		+=	common/fdt_fixup.c			\
+				${AW_PLAT}/common/sunxi_prepare_dtb.c
+endif
+
 # The bootloader is guaranteed to only run on CPU 0 by the boot ROM.
 COLD_BOOT_SINGLE_CPU		:=	1
 
diff --git a/plat/allwinner/common/include/platform_def.h b/plat/allwinner/common/include/platform_def.h
index 49951e0..c9d075a 100644
--- a/plat/allwinner/common/include/platform_def.h
+++ b/plat/allwinner/common/include/platform_def.h
@@ -57,9 +57,10 @@
 #define PLAT_CSS_SCP_COM_SHARED_MEM_BASE \
 	(SUNXI_SRAM_A2_BASE + SUNXI_SRAM_A2_SIZE - 0x200)
 
-#define PLAT_MAX_PWR_LVL_STATES		U(2)
+/* These states are used directly for SCPI communication. */
+#define PLAT_MAX_PWR_LVL_STATES		U(3)
 #define PLAT_MAX_RET_STATE		U(1)
-#define PLAT_MAX_OFF_STATE		U(2)
+#define PLAT_MAX_OFF_STATE		U(3)
 
 #define PLAT_MAX_PWR_LVL		U(2)
 #define PLAT_NUM_PWR_DOMAINS		(U(1) + \
diff --git a/plat/allwinner/common/include/sunxi_def.h b/plat/allwinner/common/include/sunxi_def.h
index ec50887..c17ef95 100644
--- a/plat/allwinner/common/include/sunxi_def.h
+++ b/plat/allwinner/common/include/sunxi_def.h
@@ -20,4 +20,7 @@
 #define SUNXI_SOC_H616			0x1823
 #define SUNXI_SOC_R329			0x1851
 
+#define JEDEC_ALLWINNER_BKID		9U
+#define JEDEC_ALLWINNER_MFID		0x9eU
+
 #endif /* SUNXI_DEF_H */
diff --git a/plat/allwinner/common/include/sunxi_private.h b/plat/allwinner/common/include/sunxi_private.h
index 6cf4670..6a38657 100644
--- a/plat/allwinner/common/include/sunxi_private.h
+++ b/plat/allwinner/common/include/sunxi_private.h
@@ -7,8 +7,12 @@
 #ifndef SUNXI_PRIVATE_H
 #define SUNXI_PRIVATE_H
 
+#include <common/fdt_fixup.h>
+
 #include <lib/psci/psci.h>
 
+extern const struct psci_cpu_idle_state sunxi_idle_states[];
+
 void sunxi_configure_mmu_el3(int flags);
 
 void sunxi_cpu_on(u_register_t mpidr);
@@ -24,8 +28,13 @@
 }
 #endif
 #if SUNXI_PSCI_USE_SCPI
+bool sunxi_psci_is_scpi(void);
 int sunxi_set_scpi_psci_ops(const plat_psci_ops_t **psci_ops);
 #else
+static inline bool sunxi_psci_is_scpi(void)
+{
+	return false;
+}
 static inline int sunxi_set_scpi_psci_ops(const plat_psci_ops_t **psci_ops)
 {
 	return -1;
@@ -41,7 +50,7 @@
 int sunxi_init_platform_r_twi(uint16_t socid, bool use_rsb);
 void sunxi_execute_arisc_code(uint32_t *code, size_t size, uint16_t param);
 
-#ifdef SUNXI_BL31_IN_DRAM
+#if SUNXI_AMEND_DTB
 void sunxi_prepare_dtb(void *fdt);
 #else
 static inline void sunxi_prepare_dtb(void *fdt)
diff --git a/plat/allwinner/common/sunxi_bl31_setup.c b/plat/allwinner/common/sunxi_bl31_setup.c
index 14049e8..a32124a 100644
--- a/plat/allwinner/common/sunxi_bl31_setup.c
+++ b/plat/allwinner/common/sunxi_bl31_setup.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
  */
@@ -32,6 +32,8 @@
 
 static console_t console;
 
+static void *fdt;
+
 static const gicv2_driver_data_t sunxi_gic_data = {
 	.gicd_base = SUNXI_GICD_BASE,
 	.gicc_base = SUNXI_GICC_BASE,
@@ -49,7 +51,7 @@
  * entry point to find the load address, which should be followed by the
  * size. Adding those together gives us the address of the DTB.
  */
-static void *sunxi_find_dtb(void)
+static void sunxi_find_dtb(void)
 {
 	uint64_t *u_boot_base;
 	int i;
@@ -57,7 +59,7 @@
 	u_boot_base = (void *)SUNXI_BL33_VIRT_BASE;
 
 	for (i = 0; i < 2048 / sizeof(uint64_t); i++) {
-		uint32_t *dtb_base;
+		void *dtb_base;
 
 		if (u_boot_base[i] != PRELOADED_BL33_BASE)
 			continue;
@@ -67,15 +69,13 @@
 			continue;
 
 		/* end of the image: base address + size */
-		dtb_base = (void *)((char *)u_boot_base + u_boot_base[i + 1]);
-
-		if (fdt_check_header(dtb_base) != 0)
-			continue;
+		dtb_base = (char *)u_boot_base + u_boot_base[i + 1];
 
-		return dtb_base;
+		if (fdt_check_header(dtb_base) == 0) {
+			fdt = dtb_base;
+			return;
+		}
 	}
-
-	return NULL;
 }
 
 void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
@@ -113,7 +113,6 @@
 {
 	const char *soc_name;
 	uint16_t soc_id = sunxi_read_soc_id();
-	void *fdt;
 
 	switch (soc_id) {
 	case SUNXI_SOC_A64:
@@ -139,7 +138,7 @@
 
 	generic_delay_timer_init();
 
-	fdt = sunxi_find_dtb();
+	sunxi_find_dtb();
 	if (fdt) {
 		const char *model;
 		int length;
@@ -180,9 +179,15 @@
 
 	sunxi_pmic_setup(soc_id, fdt);
 
+	INFO("BL31: Platform setup done\n");
+}
+
+void bl31_plat_runtime_setup(void)
+{
+	/* Change the DTB if the configuration requires so. */
 	sunxi_prepare_dtb(fdt);
 
-	INFO("BL31: Platform setup done\n");
+	console_switch_state(CONSOLE_FLAG_RUNTIME);
 }
 
 entry_point_info_t *bl31_plat_get_next_image_ep_info(uint32_t type)
diff --git a/plat/allwinner/common/sunxi_common.c b/plat/allwinner/common/sunxi_common.c
index 82410b1..092659c 100644
--- a/plat/allwinner/common/sunxi_common.c
+++ b/plat/allwinner/common/sunxi_common.c
@@ -8,7 +8,9 @@
 
 #include <common/debug.h>
 #include <lib/mmio.h>
+#include <lib/smccc.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
+#include <services/arm_arch_svc.h>
 
 #include <sunxi_def.h>
 #include <sunxi_mmap.h>
@@ -157,3 +159,29 @@
 
 	return 0;
 }
+
+int32_t plat_is_smccc_feature_available(u_register_t fid)
+{
+	switch (fid) {
+	case SMCCC_ARCH_SOC_ID:
+		return SMC_ARCH_CALL_SUCCESS;
+	default:
+		return SMC_ARCH_CALL_NOT_SUPPORTED;
+	}
+}
+
+int32_t plat_get_soc_version(void)
+{
+	int32_t ret;
+
+	ret = SOC_ID_SET_JEP_106(JEDEC_ALLWINNER_BKID, JEDEC_ALLWINNER_MFID);
+
+	return ret | (sunxi_read_soc_id() & SOC_ID_IMPL_DEF_MASK);
+}
+
+int32_t plat_get_soc_revision(void)
+{
+	uint32_t reg = mmio_read_32(SRAM_VER_REG);
+
+	return reg & GENMASK_32(7, 0);
+}
diff --git a/plat/allwinner/common/sunxi_pm.c b/plat/allwinner/common/sunxi_pm.c
index eb1b7e7..3772b4a 100644
--- a/plat/allwinner/common/sunxi_pm.c
+++ b/plat/allwinner/common/sunxi_pm.c
@@ -9,12 +9,22 @@
 #include <platform_def.h>
 
 #include <common/debug.h>
+#include <common/fdt_fixup.h>
 #include <lib/mmio.h>
 #include <lib/psci/psci.h>
 
 #include <sunxi_cpucfg.h>
 #include <sunxi_private.h>
 
+static bool psci_is_scpi;
+
+#if SUNXI_PSCI_USE_SCPI
+bool sunxi_psci_is_scpi(void)
+{
+	return psci_is_scpi;
+}
+#endif
+
 int sunxi_validate_ns_entrypoint(uintptr_t ns_entrypoint)
 {
 	/* The non-secure entry point must be in DRAM */
@@ -40,6 +50,7 @@
 
 	if (sunxi_set_scpi_psci_ops(psci_ops) == 0) {
 		INFO("PSCI: Suspend is available via SCPI\n");
+		psci_is_scpi = true;
 	} else {
 		INFO("PSCI: Suspend is unavailable\n");
 		sunxi_set_native_psci_ops(psci_ops);
diff --git a/plat/allwinner/sun50i_h616/prepare_dtb.c b/plat/allwinner/common/sunxi_prepare_dtb.c
similarity index 69%
rename from plat/allwinner/sun50i_h616/prepare_dtb.c
rename to plat/allwinner/common/sunxi_prepare_dtb.c
index e94b0b4..66af35a 100644
--- a/plat/allwinner/sun50i_h616/prepare_dtb.c
+++ b/plat/allwinner/common/sunxi_prepare_dtb.c
@@ -19,25 +19,34 @@
 	if (fdt == NULL || fdt_check_header(fdt) != 0) {
 		return;
 	}
-	ret = fdt_open_into(fdt, fdt, 0x100000);
+
+	ret = fdt_open_into(fdt, fdt, 0x10000);
 	if (ret < 0) {
 		ERROR("Preparing devicetree at %p: error %d\n", fdt, ret);
 		return;
 	}
 
+#ifdef SUNXI_BL31_IN_DRAM
 	/* Reserve memory used by Trusted Firmware. */
 	if (fdt_add_reserved_memory(fdt, "tf-a@40000000", BL31_BASE,
 				    BL31_LIMIT - BL31_BASE)) {
 		WARN("Failed to add reserved memory nodes to DT.\n");
-		return;
 	}
+#endif
+
+	if (sunxi_psci_is_scpi()) {
+		ret = fdt_add_cpu_idle_states(fdt, sunxi_idle_states);
+		if (ret < 0) {
+			WARN("Failed to add idle states to DT: %d\n", ret);
+		}
+	}
 
 	ret = fdt_pack(fdt);
 	if (ret < 0) {
 		ERROR("Failed to pack devicetree at %p: error %d\n",
 		      fdt, ret);
-	} else {
-		clean_dcache_range((uintptr_t)fdt, fdt_blob_size(fdt));
-		INFO("Changed devicetree to reserve BL31 memory.\n");
 	}
+
+	clean_dcache_range((uintptr_t)fdt, fdt_blob_size(fdt));
+	INFO("Changed devicetree.\n");
 }
diff --git a/plat/allwinner/common/sunxi_scpi_pm.c b/plat/allwinner/common/sunxi_scpi_pm.c
index eb37daa..41dc563 100644
--- a/plat/allwinner/common/sunxi_scpi_pm.c
+++ b/plat/allwinner/common/sunxi_scpi_pm.c
@@ -33,6 +33,9 @@
  */
 #define SCP_FIRMWARE_MAGIC		0xb4400012
 
+#define PLAT_LOCAL_PSTATE_WIDTH		U(4)
+#define PLAT_LOCAL_PSTATE_MASK		((U(1) << PLAT_LOCAL_PSTATE_WIDTH) - 1)
+
 #define CPU_PWR_LVL			MPIDR_AFFLVL0
 #define CLUSTER_PWR_LVL			MPIDR_AFFLVL1
 #define SYSTEM_PWR_LVL			MPIDR_AFFLVL2
@@ -44,17 +47,6 @@
 #define SYSTEM_PWR_STATE(state) \
 	((state)->pwr_domain_state[SYSTEM_PWR_LVL])
 
-static inline scpi_power_state_t scpi_map_state(plat_local_state_t psci_state)
-{
-	if (is_local_state_run(psci_state)) {
-		return scpi_power_on;
-	}
-	if (is_local_state_retn(psci_state)) {
-		return scpi_power_retention;
-	}
-	return scpi_power_off;
-}
-
 static void sunxi_cpu_standby(plat_local_state_t cpu_state)
 {
 	u_register_t scr = read_scr_el3();
@@ -87,9 +79,9 @@
 	}
 
 	scpi_set_css_power_state(read_mpidr(),
-				 scpi_map_state(cpu_pwr_state),
-				 scpi_map_state(cluster_pwr_state),
-				 scpi_map_state(system_pwr_state));
+				 cpu_pwr_state,
+				 cluster_pwr_state,
+				 system_pwr_state);
 }
 
 static void sunxi_pwr_domain_on_finish(const psci_power_state_t *target_state)
@@ -137,7 +129,9 @@
 				      psci_power_state_t *req_state)
 {
 	unsigned int power_level = psci_get_pstate_pwrlvl(power_state);
+	unsigned int state_id = psci_get_pstate_id(power_state);
 	unsigned int type = psci_get_pstate_type(power_state);
+	unsigned int i;
 
 	assert(req_state != NULL);
 
@@ -146,28 +140,19 @@
 	}
 
 	if (type == PSTATE_TYPE_STANDBY) {
-		/* Only one retention power state is supported. */
-		if (psci_get_pstate_id(power_state) > 0) {
-			return PSCI_E_INVALID_PARAMS;
-		}
-		/* The SoC cannot be suspended without losing state */
-		if (power_level == SYSTEM_PWR_LVL) {
-			return PSCI_E_INVALID_PARAMS;
-		}
-		for (unsigned int i = 0; i <= power_level; ++i) {
-			req_state->pwr_domain_state[i] = PLAT_MAX_RET_STATE;
-		}
-	} else {
-		/* Only one off power state is supported. */
-		if (psci_get_pstate_id(power_state) > 0) {
-			return PSCI_E_INVALID_PARAMS;
-		}
-		for (unsigned int i = 0; i <= power_level; ++i) {
-			req_state->pwr_domain_state[i] = PLAT_MAX_OFF_STATE;
-		}
+		return PSCI_E_INVALID_PARAMS;
 	}
+
+	/* Pass through the requested PSCI state as-is. */
+	for (i = 0; i <= power_level; ++i) {
+		unsigned int local_pstate = state_id & PLAT_LOCAL_PSTATE_MASK;
+
+		req_state->pwr_domain_state[i] = local_pstate;
+		state_id >>= PLAT_LOCAL_PSTATE_WIDTH;
+	}
+
 	/* Higher power domain levels should all remain running */
-	for (unsigned int i = power_level + 1; i <= PLAT_MAX_PWR_LVL; ++i) {
+	for (; i <= PLAT_MAX_PWR_LVL; ++i) {
 		req_state->pwr_domain_state[i] = PSCI_LOCAL_STATE_RUN;
 	}
 
diff --git a/plat/allwinner/sun50i_a64/sunxi_idle_states.c b/plat/allwinner/sun50i_a64/sunxi_idle_states.c
new file mode 100644
index 0000000..2918bb7
--- /dev/null
+++ b/plat/allwinner/sun50i_a64/sunxi_idle_states.c
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <sunxi_private.h>
+
+const struct psci_cpu_idle_state sunxi_idle_states[] = {
+	{
+		.name			= "cpu-sleep",
+		.power_state		= 0x00010003,
+		.local_timer_stop	= true,
+		.entry_latency_us	= 800,
+		.exit_latency_us	= 1500,
+		.min_residency_us	= 25000
+	},
+	{
+		.name			= "cluster-sleep",
+		.power_state		= 0x01010013,
+		.local_timer_stop	= true,
+		.entry_latency_us	= 850,
+		.exit_latency_us	= 1500,
+		.min_residency_us	= 50000
+	},
+	{}
+};
diff --git a/plat/allwinner/sun50i_h6/sunxi_idle_states.c b/plat/allwinner/sun50i_h6/sunxi_idle_states.c
new file mode 100644
index 0000000..4339bcd
--- /dev/null
+++ b/plat/allwinner/sun50i_h6/sunxi_idle_states.c
@@ -0,0 +1,11 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <sunxi_private.h>
+
+const struct psci_cpu_idle_state sunxi_idle_states[] = {
+	{}
+};
diff --git a/plat/allwinner/sun50i_h616/platform.mk b/plat/allwinner/sun50i_h616/platform.mk
index fc09af7..de494a2 100644
--- a/plat/allwinner/sun50i_h616/platform.mk
+++ b/plat/allwinner/sun50i_h616/platform.mk
@@ -4,6 +4,8 @@
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+SUNXI_BL31_IN_DRAM	:=	1
+
 # Without a management processor there is no SCPI support.
 SUNXI_PSCI_USE_SCPI	:=	0
 SUNXI_PSCI_USE_NATIVE	:=	1
@@ -18,7 +20,3 @@
 
 BL31_SOURCES		+=	drivers/allwinner/axp/axp805.c		\
 				drivers/allwinner/sunxi_rsb.c		\
-				common/fdt_fixup.c			\
-				${AW_PLAT}/${PLAT}/prepare_dtb.c
-
-$(eval $(call add_define,SUNXI_BL31_IN_DRAM))
diff --git a/plat/allwinner/sun50i_h616/sunxi_idle_states.c b/plat/allwinner/sun50i_h616/sunxi_idle_states.c
new file mode 100644
index 0000000..4339bcd
--- /dev/null
+++ b/plat/allwinner/sun50i_h616/sunxi_idle_states.c
@@ -0,0 +1,11 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <sunxi_private.h>
+
+const struct psci_cpu_idle_state sunxi_idle_states[] = {
+	{}
+};
diff --git a/plat/allwinner/sun50i_r329/sunxi_idle_states.c b/plat/allwinner/sun50i_r329/sunxi_idle_states.c
new file mode 100644
index 0000000..4339bcd
--- /dev/null
+++ b/plat/allwinner/sun50i_r329/sunxi_idle_states.c
@@ -0,0 +1,11 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <sunxi_private.h>
+
+const struct psci_cpu_idle_state sunxi_idle_states[] = {
+	{}
+};
diff --git a/plat/arm/board/fvp/fdts/fvp_fw_config.dts b/plat/arm/board/fvp/fdts/fvp_fw_config.dts
index c26b519..577ac74 100644
--- a/plat/arm/board/fvp/fdts/fvp_fw_config.dts
+++ b/plat/arm/board/fvp/fdts/fvp_fw_config.dts
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, ARM Limited. All rights reserved.
+ * Copyright (c) 2019-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,9 +19,10 @@
 		};
 
 		hw-config {
-			load-address = <0x0 0x82000000>;
-			max-size = <0x01000000>;
+			load-address = <0x0 0x07f00000>;
+			max-size = <0x00100000>;
 			id = <HW_CONFIG_ID>;
+			ns-load-address = <0x0 0x82000000>;
 		};
 
 		/*
diff --git a/plat/arm/board/fvp/fdts/optee_sp_manifest.dts b/plat/arm/board/fvp/fdts/optee_sp_manifest.dts
index b803340..27f4724 100644
--- a/plat/arm/board/fvp/fdts/optee_sp_manifest.dts
+++ b/plat/arm/board/fvp/fdts/optee_sp_manifest.dts
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
@@ -22,7 +22,7 @@
 	exception-level = <2>; /* S-EL1 */
 	execution-state = <0>; /* AARCH64 */
 	load-address = <0x6280000>;
-	entrypoint-offset = <0x1000>;
+	entrypoint-offset = <0x4000>;
 	xlat-granule = <0>; /* 4KiB */
 	boot-order = <0>;
 	messaging-method = <0x3>; /* Direct request/response supported. */
diff --git a/plat/arm/board/fvp/fvp_bl2_setup.c b/plat/arm/board/fvp/fvp_bl2_setup.c
index 5a17a0d..74e5d72 100644
--- a/plat/arm/board/fvp/fvp_bl2_setup.c
+++ b/plat/arm/board/fvp/fvp_bl2_setup.c
@@ -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
  */
@@ -40,17 +40,23 @@
 struct bl_params *plat_get_next_bl_params(void)
 {
 	struct bl_params *arm_bl_params;
+	const struct dyn_cfg_dtb_info_t *hw_config_info __unused;
+	bl_mem_params_node_t *param_node __unused;
 
 	arm_bl_params = arm_get_next_bl_params();
 
-#if __aarch64__ && !BL2_AT_EL3
+#if !BL2_AT_EL3 && !EL3_PAYLOAD_BASE
 	const struct dyn_cfg_dtb_info_t *fw_config_info;
-	bl_mem_params_node_t *param_node;
-	uintptr_t fw_config_base = 0U;
+	uintptr_t fw_config_base = 0UL;
 	entry_point_info_t *ep_info;
 
+#if __aarch64__
 	/* Get BL31 image node */
 	param_node = get_bl_mem_params_node(BL31_IMAGE_ID);
+#else /* aarch32 */
+	/* Get SP_MIN image node */
+	param_node = get_bl_mem_params_node(BL32_IMAGE_ID);
+#endif /* __aarch64__ */
 	assert(param_node != NULL);
 
 	/* get fw_config load address */
@@ -58,15 +64,42 @@
 	assert(fw_config_info != NULL);
 
 	fw_config_base = fw_config_info->config_addr;
-	assert(fw_config_base != 0U);
+	assert(fw_config_base != 0UL);
 
 	/*
-	 * Get the entry point info of BL31 image and override
+	 * Get the entry point info of next executable image and override
 	 * arg1 of entry point info with fw_config base address
 	 */
 	ep_info = &param_node->ep_info;
 	ep_info->args.arg1 = (uint32_t)fw_config_base;
-#endif /* __aarch64__ && !BL2_AT_EL3 */
+
+	/* grab NS HW config address */
+	hw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, HW_CONFIG_ID);
+	assert(hw_config_info != NULL);
+
+	/* To retrieve actual size of the HW_CONFIG */
+	param_node = get_bl_mem_params_node(HW_CONFIG_ID);
+	assert(param_node != NULL);
+
+	/* Copy HW config from Secure address to NS address */
+	memcpy((void *)hw_config_info->ns_config_addr,
+	       (void *)hw_config_info->config_addr,
+	       (size_t)param_node->image_info.image_size);
+
+	/*
+	 * Ensure HW-config device tree committed to memory, as there is
+	 * a possibility to use HW-config without cache and MMU enabled
+	 * at BL33
+	 */
+	flush_dcache_range(hw_config_info->ns_config_addr,
+			   param_node->image_info.image_size);
+
+	param_node = get_bl_mem_params_node(BL33_IMAGE_ID);
+	assert(param_node != NULL);
+
+	/* Update BL33's ep info with NS HW config address  */
+	param_node->ep_info.args.arg1 = hw_config_info->ns_config_addr;
+#endif /* !BL2_AT_EL3 && !EL3_PAYLOAD_BASE */
 
 	return arm_bl_params;
 }
diff --git a/plat/arm/board/fvp/fvp_bl31_setup.c b/plat/arm/board/fvp/fvp_bl31_setup.c
index a94a4f4..dd90965 100644
--- a/plat/arm/board/fvp/fvp_bl31_setup.c
+++ b/plat/arm/board/fvp/fvp_bl31_setup.c
@@ -17,6 +17,8 @@
 
 #include "fvp_private.h"
 
+static const struct dyn_cfg_dtb_info_t *hw_config_info __unused;
+
 void __init bl31_early_platform_setup2(u_register_t arg0,
 		u_register_t arg1, u_register_t arg2, u_register_t arg3)
 {
@@ -34,6 +36,17 @@
 	if (soc_fw_config_info != NULL) {
 		arg1 = soc_fw_config_info->config_addr;
 	}
+
+	/*
+	 * arg2 is currently holding the 'secure' address of HW_CONFIG.
+	 * But arm_bl31_early_platform_setup() below expects the 'non-secure'
+	 * address of HW_CONFIG (which it will pass to BL33).
+	 * This why we need to override arg2 here.
+	 */
+	hw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, HW_CONFIG_ID);
+	assert(hw_config_info != NULL);
+	assert(hw_config_info->ns_config_addr != 0UL);
+	arg2 = hw_config_info->ns_config_addr;
 #endif /* !RESET_TO_BL31 && !BL2_AT_EL3 */
 
 	arm_bl31_early_platform_setup((void *)arg0, arg1, arg2, (void *)arg3);
@@ -66,22 +79,57 @@
 
 void __init bl31_plat_arch_setup(void)
 {
+	int rc __unused;
+	uintptr_t hw_config_base_align __unused;
+	size_t mapped_size_align __unused;
+
 	arm_bl31_plat_arch_setup();
 
 	/*
 	 * For RESET_TO_BL31 systems, BL31 is the first bootloader to run.
 	 * So there is no BL2 to load the HW_CONFIG dtb into memory before
-	 * control is passed to BL31.
+	 * control is passed to BL31. The code below relies on dynamic mapping
+	 * capability, which is not supported by xlat tables lib V1.
+	 * TODO: remove the ARM_XLAT_TABLES_LIB_V1 check when its support
+	 * gets deprecated.
 	 */
-#if !RESET_TO_BL31 && !BL2_AT_EL3
-	/* HW_CONFIG was also loaded by BL2 */
-	const struct dyn_cfg_dtb_info_t *hw_config_info;
-
-	hw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, HW_CONFIG_ID);
+#if !RESET_TO_BL31 && !BL2_AT_EL3 && !ARM_XLAT_TABLES_LIB_V1
 	assert(hw_config_info != NULL);
+	assert(hw_config_info->config_addr != 0UL);
 
+	/* Page aligned address and size if necessary */
+	hw_config_base_align = page_align(hw_config_info->config_addr, DOWN);
+	mapped_size_align = page_align(hw_config_info->config_max_size, UP);
+
+	if ((hw_config_info->config_addr != hw_config_base_align) &&
+	    (hw_config_info->config_max_size == mapped_size_align)) {
+		mapped_size_align += PAGE_SIZE;
+	}
+
+	/*
+	 * map dynamically HW config region with its aligned base address and
+	 * size
+	 */
+	rc = mmap_add_dynamic_region((unsigned long long)hw_config_base_align,
+				     hw_config_base_align,
+				     mapped_size_align,
+				     MT_RO_DATA);
+	if (rc != 0) {
+		ERROR("Error while mapping HW_CONFIG device tree (%d).\n", rc);
+		panic();
+	}
+
+	/* Populate HW_CONFIG device tree with the mapped address */
 	fconf_populate("HW_CONFIG", hw_config_info->config_addr);
-#endif
+
+	/* unmap the HW_CONFIG memory region */
+	rc = mmap_remove_dynamic_region(hw_config_base_align, mapped_size_align);
+	if (rc != 0) {
+		ERROR("Error while unmapping HW_CONFIG device tree (%d).\n",
+		      rc);
+		panic();
+	}
+#endif /* !RESET_TO_BL31 && !BL2_AT_EL3 && !ARM_XLAT_TABLES_LIB_V1 */
 }
 
 unsigned int plat_get_syscnt_freq2(void)
diff --git a/plat/arm/board/fvp/fvp_common.c b/plat/arm/board/fvp/fvp_common.c
index d8d19de..a7028f6 100644
--- a/plat/arm/board/fvp/fvp_common.c
+++ b/plat/arm/board/fvp/fvp_common.c
@@ -104,9 +104,10 @@
 #ifdef __aarch64__
 	ARM_MAP_DRAM2,
 #endif
-#if defined(SPD_spmd)
+	/*
+	 * Required to load HW_CONFIG, SPMC and SPs to trusted DRAM.
+	 */
 	ARM_MAP_TRUSTED_DRAM,
-#endif
 #if ENABLE_RME
 	ARM_MAP_RMM_DRAM,
 	ARM_MAP_GPT_L1_DRAM,
@@ -126,7 +127,7 @@
 	 */
 	ARM_MAP_BL1_RW,
 #endif /* CRYPTO_SUPPORT && !BL2_AT_EL3 */
-#if SPM_MM
+#if SPM_MM || SPMC_AT_EL3
 	ARM_SP_IMAGE_MMAP,
 #endif
 #if ARM_BL31_IN_DRAM
@@ -166,8 +167,6 @@
 #if SPM_MM
 	ARM_SPM_BUF_EL3_MMAP,
 #endif
-	/* Required by fconf APIs to read HW_CONFIG dtb loaded into DRAM */
-	ARM_DTB_DRAM_NS,
 #if ENABLE_RME
 	ARM_MAP_GPT_L1_DRAM,
 #endif
@@ -197,8 +196,6 @@
 	V2M_MAP_IOFPGA,
 	MAP_DEVICE0,
 	MAP_DEVICE1,
-	/* Required by fconf APIs to read HW_CONFIG dtb loaded into DRAM */
-	ARM_DTB_DRAM_NS,
 	{0}
 };
 #endif
diff --git a/plat/arm/board/fvp/fvp_el3_spmc_logical_sp.c b/plat/arm/board/fvp/fvp_el3_spmc_logical_sp.c
new file mode 100644
index 0000000..b9e4f86
--- /dev/null
+++ b/plat/arm/board/fvp/fvp_el3_spmc_logical_sp.c
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <services/el3_spmc_logical_sp.h>
+#include <services/ffa_svc.h>
+#include <smccc_helpers.h>
+
+#define LP_PARTITION_ID 0xC001
+#define LP_UUID {0x47a3bf57, 0xe98e43ad, 0xb7db524f, 0x1588f4e3}
+
+/* Our Logical SP currently only supports receipt of direct messaging. */
+#define PARTITION_PROPERTIES FFA_PARTITION_DIRECT_REQ_RECV
+
+static int32_t sp_init(void)
+{
+	INFO("LSP: Init function called.\n");
+	return 0;
+}
+
+static uint64_t handle_ffa_direct_request(uint32_t smc_fid,  bool secure_origin,
+					  uint64_t x1, uint64_t x2, uint64_t x3,
+					  uint64_t x4, void *cookie,
+					  void *handle, uint64_t flags)
+{
+	uint64_t ret;
+
+	/* Determine if we have a 64 or 32 direct request. */
+	if (smc_fid == FFA_MSG_SEND_DIRECT_REQ_SMC32) {
+		ret = FFA_MSG_SEND_DIRECT_RESP_SMC32;
+	} else if (smc_fid == FFA_MSG_SEND_DIRECT_REQ_SMC64) {
+		ret = FFA_MSG_SEND_DIRECT_RESP_SMC64;
+	} else {
+		panic(); /* Unknown SMC. */
+	}
+	/*
+	 * Handle the incoming request. For testing purposes we echo the
+	 * incoming message.
+	 */
+	INFO("Logical Partition: Received Direct Request from %s world!\n",
+	     secure_origin ? "Secure" : "Normal");
+
+	/*
+	 * Logical SP's must always send a direct response so we can populate
+	 * our response directly.
+	 */
+	SMC_RET8(handle, ret, 0, 0, x4, 0, 0, 0, 0);
+}
+
+/* Register logical partition  */
+DECLARE_LOGICAL_PARTITION(
+	my_logical_partition,
+	sp_init,			/* Init Function */
+	LP_PARTITION_ID,		/* FF-A Partition ID */
+	LP_UUID,			/* UUID */
+	PARTITION_PROPERTIES,		/* Partition Properties. */
+	handle_ffa_direct_request	/* Callback for direct requests. */
+);
diff --git a/plat/arm/board/fvp/include/platform_def.h b/plat/arm/board/fvp/include/platform_def.h
index 5e5ddce..82bd7c8 100644
--- a/plat/arm/board/fvp/include/platform_def.h
+++ b/plat/arm/board/fvp/include/platform_def.h
@@ -90,6 +90,31 @@
 					FVP_DTB_DRAM_MAP_START,		\
 					FVP_DTB_DRAM_MAP_SIZE,		\
 					MT_MEMORY | MT_RO | MT_NS)
+
+#if SPMC_AT_EL3
+/*
+ * Number of Secure Partitions supported.
+ * SPMC at EL3, uses this count to configure the maximum number of supported
+ * secure partitions.
+ */
+#define SECURE_PARTITION_COUNT		1
+
+/*
+ * Number of Normal World Partitions supported.
+ * SPMC at EL3, uses this count to configure the maximum number of supported
+ * NWd partitions.
+ */
+#define NS_PARTITION_COUNT		1
+
+/*
+ * Number of Logical Partitions supported.
+ * SPMC at EL3, uses this count to configure the maximum number of supported
+ * logical partitions.
+ */
+#define MAX_EL3_LP_DESCS_COUNT		1
+
+#endif /* SPMC_AT_EL3 */
+
 /*
  * Load address of BL33 for this platform port
  */
@@ -106,9 +131,12 @@
 #   define MAX_XLAT_TABLES		11
 #  else
 #   define MAX_XLAT_TABLES		9
-# endif
+#  endif
 #  define PLAT_SP_IMAGE_MMAP_REGIONS	30
 #  define PLAT_SP_IMAGE_MAX_XLAT_TABLES	10
+# elif SPMC_AT_EL3
+#  define PLAT_ARM_MMAP_ENTRIES		13
+#  define MAX_XLAT_TABLES		11
 # else
 #  define PLAT_ARM_MMAP_ENTRIES		9
 #  if USE_DEBUGFS
@@ -126,8 +154,13 @@
 #  endif
 # endif
 #elif defined(IMAGE_BL32)
-# define PLAT_ARM_MMAP_ENTRIES		9
-# define MAX_XLAT_TABLES		6
+# if SPMC_AT_EL3
+#  define PLAT_ARM_MMAP_ENTRIES		270
+#  define MAX_XLAT_TABLES		10
+# else
+#  define PLAT_ARM_MMAP_ENTRIES		9
+#  define MAX_XLAT_TABLES		6
+# endif
 #elif !USE_ROMLIB
 # define PLAT_ARM_MMAP_ENTRIES		11
 # define MAX_XLAT_TABLES		5
@@ -261,6 +294,7 @@
 #define PLAT_ARM_TRP_UART_CLK_IN_HZ	V2M_IOFPGA_UART3_CLK_IN_HZ
 
 #define PLAT_FVP_SMMUV3_BASE		UL(0x2b400000)
+#define PLAT_ARM_SMMUV3_ROOT_REG_OFFSET UL(0x20000)
 
 /* CCI related constants */
 #define PLAT_FVP_CCI400_BASE		UL(0x2c090000)
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index c9f5551..d89e91f 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -333,15 +333,10 @@
 endif
 
 # Enable the dynamic translation tables library.
-ifeq (${ARCH},aarch32)
-    ifeq (${RESET_TO_SP_MIN},1)
+ifeq ($(filter 1,${BL2_AT_EL3} ${ARM_XLAT_TABLES_LIB_V1}),)
+    ifeq (${ARCH},aarch32)
         BL32_CPPFLAGS	+=	-DPLAT_XLAT_TABLES_DYNAMIC
-    endif
-else # AArch64
-    ifeq (${RESET_TO_BL31},1)
-        BL31_CPPFLAGS	+=	-DPLAT_XLAT_TABLES_DYNAMIC
-    endif
-    ifeq (${SPD},trusty)
+    else # AArch64
         BL31_CPPFLAGS	+=	-DPLAT_XLAT_TABLES_DYNAMIC
     endif
 endif
@@ -394,6 +389,15 @@
 # enable trace buffer control registers access to NS by default
 ENABLE_TRBE_FOR_NS		:= 1
 
+# enable branch record buffer control registers access in NS by default
+# only enable for aarch64
+# do not enable when ENABLE_RME=1
+ifeq (${ARCH}, aarch64)
+ifeq (${ENABLE_RME},0)
+	ENABLE_BRBE_FOR_NS		:= 1
+endif
+endif
+
 # enable trace system registers access to NS by default
 ENABLE_SYS_REG_TRACE_FOR_NS	:= 1
 
diff --git a/plat/arm/board/fvp/sp_min/fvp_sp_min_setup.c b/plat/arm/board/fvp/sp_min/fvp_sp_min_setup.c
index 763b42a..9ab36a6 100644
--- a/plat/arm/board/fvp/sp_min/fvp_sp_min_setup.c
+++ b/plat/arm/board/fvp/sp_min/fvp_sp_min_setup.c
@@ -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
  */
@@ -9,15 +9,31 @@
 #include <bl32/sp_min/platform_sp_min.h>
 #include <common/debug.h>
 #include <lib/fconf/fconf.h>
+#include <lib/fconf/fconf_dyn_cfg_getter.h>
 #include <plat/arm/common/plat_arm.h>
 
 #include "../fvp_private.h"
 
-uintptr_t hw_config_dtb;
-
 void plat_arm_sp_min_early_platform_setup(u_register_t arg0, u_register_t arg1,
 			u_register_t arg2, u_register_t arg3)
 {
+	const struct dyn_cfg_dtb_info_t *tos_fw_config_info __unused;
+
+	/* Initialize the console to provide early debug support */
+	arm_console_boot_init();
+
+#if !RESET_TO_SP_MIN && !BL2_AT_EL3
+
+	INFO("SP_MIN FCONF: FW_CONFIG address = %lx\n", (uintptr_t)arg1);
+	/* Fill the properties struct with the info from the config dtb */
+	fconf_populate("FW_CONFIG", arg1);
+
+	tos_fw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, TOS_FW_CONFIG_ID);
+	if (tos_fw_config_info != NULL) {
+		arg1 = tos_fw_config_info->config_addr;
+	}
+#endif /* !RESET_TO_SP_MIN && !BL2_AT_EL3 */
+
 	arm_sp_min_early_platform_setup((void *)arg0, arg1, arg2, (void *)arg3);
 
 	/* Initialize the platform config for future decision making */
@@ -37,12 +53,15 @@
 	 * FVP PSCI code will enable coherency for other clusters.
 	 */
 	fvp_interconnect_enable();
-
-	hw_config_dtb = arg2;
 }
 
 void sp_min_plat_arch_setup(void)
 {
+	int rc __unused;
+	const struct dyn_cfg_dtb_info_t *hw_config_info __unused;
+	uintptr_t hw_config_base_align __unused;
+	size_t mapped_size_align __unused;
+
 	arm_sp_min_plat_arch_setup();
 
 	/*
@@ -50,11 +69,53 @@
 	 * to run. So there is no BL2 to load the HW_CONFIG dtb into memory
 	 * before control is passed to SP_MIN.
 	 * Also, BL2 skips loading HW_CONFIG dtb for BL2_AT_EL3 builds.
+	 * The code below relies on dynamic mapping capability, which is not
+	 * supported by xlat tables lib V1.
+	 * TODO: remove the ARM_XLAT_TABLES_LIB_V1 check when its support
+	 * gets deprecated.
 	 */
-#if !RESET_TO_SP_MIN && !BL2_AT_EL3
-	assert(hw_config_dtb != 0U);
+#if !RESET_TO_SP_MIN && !BL2_AT_EL3 && !ARM_XLAT_TABLES_LIB_V1
+	hw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, HW_CONFIG_ID);
+	assert(hw_config_info != NULL);
+	assert(hw_config_info->config_addr != 0UL);
+
+	INFO("SP_MIN FCONF: HW_CONFIG address = %p\n",
+	     (void *)hw_config_info->config_addr);
+
+	/*
+	 * Preferrably we expect this address and size are page aligned,
+	 * but if they are not then align it.
+	 */
+	hw_config_base_align = page_align(hw_config_info->config_addr, DOWN);
+	mapped_size_align = page_align(hw_config_info->config_max_size, UP);
+
+	if ((hw_config_info->config_addr != hw_config_base_align) &&
+	    (hw_config_info->config_max_size == mapped_size_align)) {
+		mapped_size_align += PAGE_SIZE;
+	}
+
+	/*
+	 * map dynamically HW config region with its aligned base address and
+	 * size
+	 */
+	rc = mmap_add_dynamic_region((unsigned long long)hw_config_base_align,
+				     hw_config_base_align,
+				     mapped_size_align,
+				     MT_RO_DATA);
+	if (rc != 0) {
+		ERROR("Error while mapping HW_CONFIG device tree (%d).\n", rc);
+		panic();
+	}
+
+	/* Populate HW_CONFIG device tree with the mapped address */
+	fconf_populate("HW_CONFIG", hw_config_info->config_addr);
 
-	INFO("SP_MIN FCONF: HW_CONFIG address = %p\n", (void *)hw_config_dtb);
-	fconf_populate("HW_CONFIG", hw_config_dtb);
-#endif
+	/* unmap the HW_CONFIG memory region */
+	rc = mmap_remove_dynamic_region(hw_config_base_align, mapped_size_align);
+	if (rc != 0) {
+		ERROR("Error while unmapping HW_CONFIG device tree (%d).\n",
+		      rc);
+		panic();
+	}
+#endif /* !RESET_TO_SP_MIN && !BL2_AT_EL3 && !ARM_XLAT_TABLES_LIB_V1 */
 }
diff --git a/plat/arm/board/fvp/sp_min/sp_min-fvp.mk b/plat/arm/board/fvp/sp_min/sp_min-fvp.mk
index 0d8cca5..183d802 100644
--- a/plat/arm/board/fvp/sp_min/sp_min-fvp.mk
+++ b/plat/arm/board/fvp/sp_min/sp_min-fvp.mk
@@ -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
 #
@@ -25,7 +25,8 @@
 # Added separately from the above list for better readability
 ifeq ($(filter 1,${BL2_AT_EL3} ${RESET_TO_SP_MIN}),)
 BL32_SOURCES		+=	lib/fconf/fconf.c				\
-				plat/arm/board/fvp/fconf/fconf_hw_config_getter.c
+				lib/fconf/fconf_dyn_cfg_getter.c		\
+				plat/arm/board/fvp/fconf/fconf_hw_config_getter.c \
 
 BL32_SOURCES		+=	${FDT_WRAPPERS_SOURCES}
 
diff --git a/plat/arm/board/fvp_r/fvp_r_bl1_setup.c b/plat/arm/board/fvp_r/fvp_r_bl1_setup.c
index 68872c1..1ac0a9c 100644
--- a/plat/arm/board/fvp_r/fvp_r_bl1_setup.c
+++ b/plat/arm/board/fvp_r/fvp_r_bl1_setup.c
@@ -154,7 +154,8 @@
 
 	/* Set global DTB info for fixed fw_config information */
 	fw_config_max_size = ARM_FW_CONFIG_LIMIT - ARM_FW_CONFIG_BASE;
-	set_config_info(ARM_FW_CONFIG_BASE, fw_config_max_size, FW_CONFIG_ID);
+	set_config_info(ARM_FW_CONFIG_BASE, ~0UL, fw_config_max_size,
+			FW_CONFIG_ID);
 
 	assert(bl1_plat_get_image_desc(BL33_IMAGE_ID) != NULL);
 
diff --git a/plat/arm/board/sgm775/fdts/sgm775_fw_config.dts b/plat/arm/board/sgm775/fdts/sgm775_fw_config.dts
deleted file mode 100644
index 5d478e9..0000000
--- a/plat/arm/board/sgm775/fdts/sgm775_fw_config.dts
+++ /dev/null
@@ -1,21 +0,0 @@
-/*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <common/tbbr/tbbr_img_def.h>
-
-/dts-v1/;
-
-/ {
-	dtb-registry {
-		compatible = "fconf,dyn_cfg-dtb_registry";
-
-		tb_fw-config {
-			load-address = <0x0 0x4001300>;
-			max-size = <0x200>;
-			id = <TB_FW_CONFIG_ID>;
-		};
-	};
-};
diff --git a/plat/arm/board/sgm775/fdts/sgm775_tb_fw_config.dts b/plat/arm/board/sgm775/fdts/sgm775_tb_fw_config.dts
deleted file mode 100644
index 49eda27..0000000
--- a/plat/arm/board/sgm775/fdts/sgm775_tb_fw_config.dts
+++ /dev/null
@@ -1,28 +0,0 @@
-/*
- * Copyright (c) 2020, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-/dts-v1/;
-
-/ {
-	tb_fw-config {
-		compatible = "arm,tb_fw";
-
-		/* Disable authentication for development */
-		disable_auth = <0x0>;
-
-		/*
-		 * The following two entries are placeholders for Mbed TLS
-		 * heap information. The default values don't matter since
-		 * they will be overwritten by BL1.
-		 * In case of having shared Mbed TLS heap between BL1 and BL2,
-		 * BL1 will populate these two properties with the respective
-		 * info about the shared heap. This info will be available for
-		 * BL2 in order to locate and re-use the heap.
-		 */
-		mbedtls_heap_addr = <0x0 0x0>;
-		mbedtls_heap_size = <0x0>;
-	};
-};
diff --git a/plat/arm/board/sgm775/include/platform_def.h b/plat/arm/board/sgm775/include/platform_def.h
deleted file mode 100644
index e83cd57..0000000
--- a/plat/arm/board/sgm775/include/platform_def.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef PLATFORM_DEF_H
-#define PLATFORM_DEF_H
-
-#include <sgm_base_platform_def.h>
-
-#define PLAT_MAX_CPUS_PER_CLUSTER	U(8)
-#define PLAT_MAX_PE_PER_CPU		U(1)
-
-#define PLAT_ARM_DRAM2_BASE		ULL(0x880000000)
-#define PLAT_ARM_DRAM2_SIZE		ULL(0x180000000)
-
-/*
- * Physical and virtual address space limits for MMU in AARCH64 & AARCH32 modes
- */
-#ifdef __aarch64__
-#define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 36)
-#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 36)
-#else
-#define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 32)
-#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 32)
-#endif
-
-#endif /* PLATFORM_DEF_H */
diff --git a/plat/arm/board/sgm775/platform.mk b/plat/arm/board/sgm775/platform.mk
deleted file mode 100644
index f8df1a7..0000000
--- a/plat/arm/board/sgm775/platform.mk
+++ /dev/null
@@ -1,39 +0,0 @@
-#
-# Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
-#
-# SPDX-License-Identifier: BSD-3-Clause
-#
-
-$(warning Platform ${PLAT} is deprecated. Some of the features might not work as expected)
-
-include plat/arm/css/sgm/sgm-common.mk
-
-SGM775_BASE= plat/arm/board/sgm775
-
-# Add the FDT_SOURCES and options for Dynamic Config
-FDT_SOURCES		+=      ${SGM775_BASE}/fdts/${PLAT}_fw_config.dts	\
-				${SGM775_BASE}/fdts/${PLAT}_tb_fw_config.dts
-FW_CONFIG		:=      ${BUILD_PLAT}/fdts/${PLAT}_fw_config.dtb
-TB_FW_CONFIG		:=      ${BUILD_PLAT}/fdts/${PLAT}_tb_fw_config.dtb
-
-# Add the FW_CONFIG to FIP and specify the same to certtool
-$(eval $(call TOOL_ADD_PAYLOAD,${FW_CONFIG},--fw-config,${FW_CONFIG}))
-# Add the TB_FW_CONFIG to FIP and specify the same to certtool
-$(eval $(call TOOL_ADD_PAYLOAD,${TB_FW_CONFIG},--tb-fw-config,${TB_FW_CONFIG}))
-
-PLAT_INCLUDES +=-I${SGM775_BASE}/include/
-
-BL1_SOURCES		+=	${SGM775_BASE}/sgm775_err.c
-
-BL2_SOURCES		+=	lib/utils/mem_region.c                  \
-				${SGM775_BASE}/sgm775_err.c		\
-				plat/arm/common/arm_nor_psci_mem_protect.c
-
-BL31_SOURCES		+=	drivers/cfi/v2m/v2m_flash.c		\
-				lib/utils/mem_region.c			\
-				plat/arm/common/arm_nor_psci_mem_protect.c
-
-ifeq (${TRUSTED_BOARD_BOOT}, 1)
-BL1_SOURCES		+=	${SGM775_BASE}/sgm775_trusted_boot.c
-BL2_SOURCES		+=	${SGM775_BASE}/sgm775_trusted_boot.c
-endif
diff --git a/plat/arm/board/sgm775/sgm775_err.c b/plat/arm/board/sgm775/sgm775_err.c
deleted file mode 100644
index dc114f0..0000000
--- a/plat/arm/board/sgm775/sgm775_err.c
+++ /dev/null
@@ -1,17 +0,0 @@
-/*
- * Copyright (c) 2019-2020, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <plat/arm/common/plat_arm.h>
-
-/*
- * sgm775 error handler
- */
-void __dead2 plat_arm_error_handler(int err)
-{
-	while (true) {
-		wfi();
-	}
-}
diff --git a/plat/arm/board/sgm775/sgm775_trusted_boot.c b/plat/arm/board/sgm775/sgm775_trusted_boot.c
deleted file mode 100644
index 4592b8f..0000000
--- a/plat/arm/board/sgm775/sgm775_trusted_boot.c
+++ /dev/null
@@ -1,26 +0,0 @@
-/*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <plat/arm/common/plat_arm.h>
-
-/*
- * Return the ROTPK hash in the following ASN.1 structure in DER format:
- *
- * AlgorithmIdentifier  ::=  SEQUENCE  {
- *     algorithm         OBJECT IDENTIFIER,
- *     parameters        ANY DEFINED BY algorithm OPTIONAL
- * }
- *
- * DigestInfo ::= SEQUENCE {
- *     digestAlgorithm   AlgorithmIdentifier,
- *     digest            OCTET STRING
- * }
- */
-int plat_get_rotpk_info(void *cookie, void **key_ptr, unsigned int *key_len,
-			unsigned int *flags)
-{
-	return arm_get_rotpk_info(cookie, key_ptr, key_len, flags);
-}
diff --git a/plat/arm/board/sgm775/tsp/tsp-sgm775.mk b/plat/arm/board/sgm775/tsp/tsp-sgm775.mk
deleted file mode 100644
index 129b586..0000000
--- a/plat/arm/board/sgm775/tsp/tsp-sgm775.mk
+++ /dev/null
@@ -1,7 +0,0 @@
-#
-# Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
-#
-# SPDX-License-Identifier: BSD-3-Clause
-#
-
-include plat/arm/css/sgm/tsp/tsp-sgm.mk
\ No newline at end of file
diff --git a/plat/arm/common/arm_bl1_setup.c b/plat/arm/common/arm_bl1_setup.c
index 73338cb..7a9e04d 100644
--- a/plat/arm/common/arm_bl1_setup.c
+++ b/plat/arm/common/arm_bl1_setup.c
@@ -166,7 +166,7 @@
 
 	/* Set global DTB info for fixed fw_config information */
 	fw_config_max_size = ARM_FW_CONFIG_LIMIT - ARM_FW_CONFIG_BASE;
-	set_config_info(ARM_FW_CONFIG_BASE, fw_config_max_size, FW_CONFIG_ID);
+	set_config_info(ARM_FW_CONFIG_BASE, ~0UL, fw_config_max_size, FW_CONFIG_ID);
 
 	/* Fill the device tree information struct with the info from the config dtb */
 	err = fconf_load_config(FW_CONFIG_ID);
diff --git a/plat/arm/css/sgm/aarch64/css_sgm_helpers.S b/plat/arm/css/sgm/aarch64/css_sgm_helpers.S
deleted file mode 100644
index 32ca1bb..0000000
--- a/plat/arm/css/sgm/aarch64/css_sgm_helpers.S
+++ /dev/null
@@ -1,82 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <arch.h>
-#include <asm_macros.S>
-#include <platform_def.h>
-#include <cortex_a75.h>
-#include <cortex_a55.h>
-#include <cpu_macros.S>
-
-	.globl	plat_arm_calc_core_pos
-	.globl	plat_reset_handler
-
-	/* ---------------------------------------------------------------------
-	 * unsigned int plat_arm_calc_core_pos(u_register_t mpidr)
-	 *
-	 * Function to calculate the core position on FVP.
-	 *
-	 * (ClusterId * MAX_CPUS_PER_CLUSTER * MAX_PE_PER_CPU) +
-	 * (CPUId * MAX_PE_PER_CPU) +
-	 * ThreadId
-	 *
-	 * which can be simplified as:
-	 *
-	 * ((ClusterId * MAX_CPUS_PER_CLUSTER + CPUId) * MAX_PE_PER_CPU)
-	 * + ThreadId
-	 * ---------------------------------------------------------------------
-	 */
-func plat_arm_calc_core_pos
-	/*
-	 * Check for MT bit in MPIDR. If not set, shift MPIDR to left to make it
-	 * look as if in a multi-threaded implementation.
-	 */
-	tst	x0, #MPIDR_MT_MASK
-	lsr	x3, x0, #MPIDR_AFFINITY_BITS
-	csel	x3, x3, x0, eq
-
-	/* Extract individual affinity fields from MPIDR */
-	ubfx	x0, x3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
-	ubfx	x1, x3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
-	ubfx	x2, x3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
-
-	/* Compute linear position */
-	mov	x4, #PLAT_MAX_CPUS_PER_CLUSTER
-	madd	x1, x2, x4, x1
-	mov	x5, #PLAT_MAX_PE_PER_CPU
-	madd	x0, x1, x5, x0
-	ret
-endfunc plat_arm_calc_core_pos
-
-	/* -----------------------------------------------------
-	 * void plat_reset_handler(void);
-	 *
-	 * Determine the CPU MIDR and disable power down bit for
-	 * that CPU.
-	 * -----------------------------------------------------
-	 */
-func plat_reset_handler
-	jump_if_cpu_midr CORTEX_A75_MIDR, A75
-	jump_if_cpu_midr CORTEX_A55_MIDR, A55
-	ret
-
-	/* -----------------------------------------------------
-	 * Disable CPU power down bit in power control register
-	 * -----------------------------------------------------
-	 */
-A75:
-	mrs	x0, CORTEX_A75_CPUPWRCTLR_EL1
-	bic	x0, x0, #CORTEX_A75_CORE_PWRDN_EN_MASK
-	msr	CORTEX_A75_CPUPWRCTLR_EL1, x0
-	isb
-	ret
-A55:
-	mrs	x0, CORTEX_A55_CPUPWRCTLR_EL1
-	bic	x0, x0, #CORTEX_A55_CORE_PWRDN_EN_MASK
-	msr	CORTEX_A55_CPUPWRCTLR_EL1, x0
-	isb
-	ret
-endfunc plat_reset_handler
diff --git a/plat/arm/css/sgm/fdts/sgm_tb_fw_config.dts b/plat/arm/css/sgm/fdts/sgm_tb_fw_config.dts
deleted file mode 100644
index 54b2423..0000000
--- a/plat/arm/css/sgm/fdts/sgm_tb_fw_config.dts
+++ /dev/null
@@ -1,27 +0,0 @@
-/*
- * Copyright (c) 2020, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-/dts-v1/;
-
-/ {
-	/* Platform Config */
-	tb_fw-config {
-		compatible = "arm,tb_fw";
-		hw_config_addr = <0x0 0x83000000>;
-		hw_config_max_size = <0x01000000>;
-		/*
-		 * The following two entries are placeholders for Mbed TLS
-		 * heap information. The default values don't matter since
-		 * they will be overwritten by BL1.
-		 * In case of having shared Mbed TLS heap between BL1 and BL2,
-		 * BL1 will populate these two properties with the respective
-		 * info about the shared heap. This info will be available for
-		 * BL2 in order to locate and re-use the heap.
-		 */
-		mbedtls_heap_addr = <0x0 0x0>;
-		mbedtls_heap_size = <0x0>;
-	};
-};
diff --git a/plat/arm/css/sgm/include/plat_macros.S b/plat/arm/css/sgm/include/plat_macros.S
deleted file mode 100644
index 715ded2..0000000
--- a/plat/arm/css/sgm/include/plat_macros.S
+++ /dev/null
@@ -1,24 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-#ifndef PLAT_MACROS_S
-#define PLAT_MACROS_S
-
-#include <cci_macros.S>
-#include <css_macros.S>
-
-/* ---------------------------------------------
- * The below required platform porting macro
- * prints out relevant platform registers
- * whenever an unhandled exception is taken in
- * BL31.
- * ---------------------------------------------
- */
-.macro plat_crash_print_regs
-css_print_gic_regs
-print_cci_regs
-.endm
-
-#endif /* PLAT_MACROS_S */
diff --git a/plat/arm/css/sgm/include/sgm_base_platform_def.h b/plat/arm/css/sgm/include/sgm_base_platform_def.h
deleted file mode 100644
index 2d8e677..0000000
--- a/plat/arm/css/sgm/include/sgm_base_platform_def.h
+++ /dev/null
@@ -1,242 +0,0 @@
-/*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef SGM_BASE_PLATFORM_DEF_H
-#define SGM_BASE_PLATFORM_DEF_H
-
-#include <drivers/arm/tzc400.h>
-#include <drivers/arm/tzc_common.h>
-#include <plat/arm/board/common/board_css_def.h>
-#include <plat/arm/board/common/v2m_def.h>
-#include <plat/arm/common/arm_def.h>
-#include <plat/arm/css/common/css_def.h>
-#include <plat/arm/soc/common/soc_css_def.h>
-#include <plat/common/common_def.h>
-
-/* CPU topology */
-#define PLAT_ARM_CLUSTER_COUNT		U(1)
-#define PLAT_ARM_CLUSTER_CORE_COUNT	U(8)
-#define PLATFORM_CORE_COUNT		PLAT_ARM_CLUSTER_CORE_COUNT
-
-#define PLAT_MAX_PWR_LVL		ARM_PWR_LVL2
-#define PLAT_NUM_PWR_DOMAINS		(ARM_SYSTEM_COUNT + \
-					PLAT_ARM_CLUSTER_COUNT + \
-					PLATFORM_CORE_COUNT)
-
-/*
- * Define a list of Group 1 Secure and Group 0 interrupts as per GICv3
- * terminology. On a GICv2 system or mode, the lists will be merged and treated
- * as Group 0 interrupts.
- */
-#define PLAT_ARM_G1S_IRQ_PROPS(grp) \
-			CSS_G1S_IRQ_PROPS(grp), \
-			ARM_G1S_IRQ_PROPS(grp)
-
-#define PLAT_ARM_G0_IRQ_PROPS(grp)	ARM_G0_IRQ_PROPS(grp)
-
-/* GIC related constants */
-#define PLAT_ARM_GICD_BASE		0x30000000
-#define PLAT_ARM_GICR_BASE		0x300C0000
-#define PLAT_ARM_GICC_BASE		0x2c000000
-
-#define CSS_GIC_SIZE			0x00200000
-
-#define CSS_MAP_GIC_DEVICE		MAP_REGION_FLAT(	\
-					PLAT_ARM_GICD_BASE,	\
-					CSS_GIC_SIZE,		\
-					MT_DEVICE | MT_RW | MT_SECURE)
-
-/* Platform ID address */
-#define SSC_VERSION                     (SSC_REG_BASE + SSC_VERSION_OFFSET)
-#ifndef __ASSEMBLER__
-/* SSC_VERSION related accessors */
-/* Returns the part number of the platform */
-#define GET_PLAT_PART_NUM                                       \
-		GET_SSC_VERSION_PART_NUM(mmio_read_32(SSC_VERSION))
-/* Returns the configuration number of the platform */
-#define GET_PLAT_CONFIG_NUM                                     \
-		GET_SSC_VERSION_CONFIG(mmio_read_32(SSC_VERSION))
-#endif /* __ASSEMBLER__ */
-
-
-/*************************************************************************
- * Definitions common to all SGM CSS based platforms
- *************************************************************************/
-
-/* TZC-400 related constants */
-#define PLAT_ARM_TZC_BASE		0x2a500000
-#define TZC_NSAID_ALL_AP		0  /* Note: Same as default NSAID!! */
-#define TZC_NSAID_HDLCD0		2
-#define TZC_NSAID_HDLCD1		3
-#define TZC_NSAID_GPU			9
-#define TZC_NSAID_VIDEO			10
-#define TZC_NSAID_DISP0			11
-#define TZC_NSAID_DISP1			12
-
-
-/*************************************************************************
- * Required platform porting definitions common to all SGM CSS based
- * platforms
- *************************************************************************/
-
-#define PLAT_ARM_TRUSTED_SRAM_SIZE	0x00040000	/* 256 KB */
-
-/* MHU related constants */
-#define PLAT_CSS_MHU_BASE		0x2b1f0000
-
-#define PLAT_ARM_TRUSTED_ROM_BASE	0x00000000
-#define PLAT_ARM_TRUSTED_ROM_SIZE	0x00080000
-
-#define PLAT_ARM_CCI_BASE		0x2a000000
-
-/* Cluster to CCI slave mapping */
-#define PLAT_ARM_CCI_CLUSTER0_SL_IFACE_IX	6
-#define PLAT_ARM_CCI_CLUSTER1_SL_IFACE_IX	5
-
-/* System timer related constants */
-#define PLAT_ARM_NSTIMER_FRAME_ID	0
-
-/* TZC related constants */
-#define PLAT_ARM_TZC_NS_DEV_ACCESS	(				\
-		TZC_REGION_ACCESS_RDWR(TZC_NSAID_ALL_AP)	|	\
-		TZC_REGION_ACCESS_RDWR(TZC_NSAID_HDLCD0)	|	\
-		TZC_REGION_ACCESS_RDWR(TZC_NSAID_HDLCD1)	|	\
-		TZC_REGION_ACCESS_RDWR(TZC_NSAID_GPU)	|	\
-		TZC_REGION_ACCESS_RDWR(TZC_NSAID_VIDEO)	|	\
-		TZC_REGION_ACCESS_RDWR(TZC_NSAID_DISP0)	|	\
-		TZC_REGION_ACCESS_RDWR(TZC_NSAID_DISP1))
-
-/* Display Processor register definitions to setup the NSAIDs */
-#define MALI_DP_BASE           0x2cc00000
-#define DP_NPROT_NSAID_OFFSET     0x1000c
-#define W_NPROT_NSAID_SHIFT            24
-#define LS_NPORT_NSAID_SHIFT           12
-
-/*
- * Base address of the first memory region used for communication between AP
- * and SCP. Used by the BootOverMHU and SCPI protocols.
- */
-#if !CSS_USE_SCMI_SDS_DRIVER
-/*
- * Note that this is located at the same address as SCP_BOOT_CFG_ADDR, which
- * means the SCP/AP configuration data gets overwritten when the AP initiates
- * communication with the SCP. The configuration data is expected to be a
- * 32-bit word on all CSS platforms. Part of this configuration is
- * which CPU is the primary, according to the shift and mask definitions below.
- */
-#define PLAT_CSS_SCP_COM_SHARED_MEM_BASE	(ARM_TRUSTED_SRAM_BASE + 0x80)
-#define PLAT_CSS_PRIMARY_CPU_SHIFT		8
-#define PLAT_CSS_PRIMARY_CPU_BIT_WIDTH		4
-#endif
-
-/*
- * SCP_BL2 uses up whatever remaining space is available as it is loaded before
- * anything else in this memory region and is handed over to the SCP before
- * BL31 is loaded over the top.
- */
-#define PLAT_CSS_MAX_SCP_BL2_SIZE \
-	((SCP_BL2_LIMIT - ARM_FW_CONFIG_LIMIT) & ~PAGE_SIZE_MASK)
-
-#define PLAT_CSS_MAX_SCP_BL2U_SIZE	PLAT_CSS_MAX_SCP_BL2_SIZE
-
-/*
- * Most platform porting definitions provided by included headers
- */
-
-/*
- * PLAT_ARM_MMAP_ENTRIES depends on the number of entries in the
- * plat_arm_mmap array defined for each BL stage.
- */
-#if defined(IMAGE_BL31)
-# define PLAT_ARM_MMAP_ENTRIES		8
-# define MAX_XLAT_TABLES		5
-#elif defined(IMAGE_BL32)
-# define PLAT_ARM_MMAP_ENTRIES		8
-# define MAX_XLAT_TABLES		5
-#elif !USE_ROMLIB
-# define PLAT_ARM_MMAP_ENTRIES		11
-# define MAX_XLAT_TABLES		5
-#else
-# define PLAT_ARM_MMAP_ENTRIES		12
-# define MAX_XLAT_TABLES		6
-#endif
-
-/*
- * PLAT_ARM_MAX_BL1_RW_SIZE is calculated using the current BL1 RW debug size
- * plus a little space for growth.
- */
-#define PLAT_ARM_MAX_BL1_RW_SIZE	0xB000
-
-/*
- * PLAT_ARM_MAX_ROMLIB_RW_SIZE is define to use a full page
- */
-
-#if USE_ROMLIB
-#define PLAT_ARM_MAX_ROMLIB_RW_SIZE	0x1000
-#define PLAT_ARM_MAX_ROMLIB_RO_SIZE	0xe000
-#else
-#define PLAT_ARM_MAX_ROMLIB_RW_SIZE	0
-#define PLAT_ARM_MAX_ROMLIB_RO_SIZE	0
-#endif
-
-/*
- * PLAT_ARM_MAX_BL2_SIZE is calculated using the current BL2 debug size plus a
- * little space for growth.
- */
-#if TRUSTED_BOARD_BOOT
-# define PLAT_ARM_MAX_BL2_SIZE		0x1D000
-#else
-# define PLAT_ARM_MAX_BL2_SIZE		0x12000
-#endif
-
-/*
- * Since BL31 NOBITS overlays BL2 and BL1-RW, PLAT_ARM_MAX_BL31_SIZE is
- * calculated using the current BL31 PROGBITS debug size plus the sizes of
- * BL2 and BL1-RW
- */
-#define PLAT_ARM_MAX_BL31_SIZE		0x3B000
-
-/*
- * Size of cacheable stacks
- */
-#if defined(IMAGE_BL1)
-# if TRUSTED_BOARD_BOOT
-#  define PLATFORM_STACK_SIZE 0x1000
-# else
-#  define PLATFORM_STACK_SIZE 0x440
-# endif
-#elif defined(IMAGE_BL2)
-# if TRUSTED_BOARD_BOOT
-#  define PLATFORM_STACK_SIZE 0x1000
-# else
-#  define PLATFORM_STACK_SIZE 0x400
-# endif
-#elif defined(IMAGE_BL2U)
-# define PLATFORM_STACK_SIZE 0x400
-#elif defined(IMAGE_BL31)
-# define PLATFORM_STACK_SIZE 0x400
-#elif defined(IMAGE_BL32)
-# define PLATFORM_STACK_SIZE 0x440
-#endif
-
-/*******************************************************************************
- * Memprotect definitions
- ******************************************************************************/
-/* PSCI memory protect definitions:
- * This variable is stored in a non-secure flash because some ARM reference
- * platforms do not have secure NVRAM. Real systems that provided MEM_PROTECT
- * support must use a secure NVRAM to store the PSCI MEM_PROTECT definitions.
- */
-#define PLAT_ARM_MEM_PROT_ADDR		(V2M_FLASH0_BASE + \
-					 V2M_FLASH0_SIZE - V2M_FLASH_BLOCK_SIZE)
-
-/* System power domain level */
-#define CSS_SYSTEM_PWR_DMN_LVL		ARM_PWR_LVL2
-
-/* Number of SCMI channels on the platform */
-#define PLAT_ARM_SCMI_CHANNEL_COUNT	U(1)
-
-#endif /* SGM_BASE_PLATFORM_DEF_H */
diff --git a/plat/arm/css/sgm/include/sgm_plat_config.h b/plat/arm/css/sgm/include/sgm_plat_config.h
deleted file mode 100644
index 29b98d4..0000000
--- a/plat/arm/css/sgm/include/sgm_plat_config.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef SGM_PLAT_CONFIG_H
-#define SGM_PLAT_CONFIG_H
-
-#include <drivers/arm/ccn.h>
-#include <drivers/arm/gicv3.h>
-
-/* The type of interconnect */
-typedef enum {
-	ARM_CCI = 0,
-	ARM_CCN,
-	ARM_CMN
-} css_inteconn_type_t;
-
-typedef ccn_desc_t inteconn_desc_t;
-
-/* Interconnect configurations */
-typedef struct css_inteconn_config {
-	css_inteconn_type_t ip_type;
-	const inteconn_desc_t *plat_inteconn_desc;
-} css_inteconn_config_t;
-
-/* Topology configurations */
-typedef struct css_topology {
-	const unsigned char *power_tree;
-	unsigned int plat_cluster_core_count;
-} css_topology_t;
-
-typedef struct css_plat_config {
-	const gicv3_driver_data_t *gic_data;
-	const css_inteconn_config_t *inteconn;
-	const css_topology_t *topology;
-} css_plat_config_t;
-
-void plat_config_init(void);
-css_plat_config_t *get_plat_config(void);
-
-#endif /* SGM_PLAT_CONFIG_H */
diff --git a/plat/arm/css/sgm/include/sgm_variant.h b/plat/arm/css/sgm/include/sgm_variant.h
deleted file mode 100644
index 859ddb5..0000000
--- a/plat/arm/css/sgm/include/sgm_variant.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef SGM_VARIANT_H
-#define SGM_VARIANT_H
-
-/* SSC_VERSION values for sgm */
-#define SGM775_SSC_VER_PART_NUM		0x0790
-
-/* DMC configuration for sgm */
-#define SGM_DMC_SIZE			0x40000
-#define SGM775_DMC_COUNT		4
-
-#endif /* SGM_VARIANT_H */
diff --git a/plat/arm/css/sgm/sgm-common.mk b/plat/arm/css/sgm/sgm-common.mk
deleted file mode 100644
index 5b954f8..0000000
--- a/plat/arm/css/sgm/sgm-common.mk
+++ /dev/null
@@ -1,76 +0,0 @@
-#
-# Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
-#
-# SPDX-License-Identifier: BSD-3-Clause
-#
-
-CSS_USE_SCMI_SDS_DRIVER	:=	1
-
-CSS_SGM_BASE		:=	plat/arm/css/sgm
-
-PLAT_INCLUDES		:=	-I${CSS_SGM_BASE}/include
-
-PLAT_BL_COMMON_SOURCES	:=	${CSS_SGM_BASE}/sgm_mmap_config.c	\
-				${CSS_SGM_BASE}/aarch64/css_sgm_helpers.S
-
-SECURITY_SOURCES	:=	drivers/arm/tzc/tzc_dmc500.c		\
-				plat/arm/common/arm_tzc_dmc500.c	\
-				${CSS_SGM_BASE}/sgm_security.c
-
-SGM_CPU_SOURCES		:=	lib/cpus/aarch64/cortex_a55.S		\
-				lib/cpus/aarch64/cortex_a75.S
-
-INTERCONNECT_SOURCES	:=	${CSS_SGM_BASE}/sgm_interconnect.c
-
-# GIC-600 configuration
-GICV3_SUPPORT_GIC600	:=	1
-
-# Include GICv3 driver files
-include drivers/arm/gic/v3/gicv3.mk
-
-SGM_GIC_SOURCES		:=	${GICV3_SOURCES}			\
-				plat/common/plat_gicv3.c		\
-				plat/arm/common/arm_gicv3.c
-
-BL1_SOURCES		+=	$(SGM_CPU_SOURCES)			\
-				${INTERCONNECT_SOURCES}			\
-				${CSS_SGM_BASE}/sgm_bl1_setup.c		\
-				${CSS_SGM_BASE}/sgm_plat_config.c	\
-				drivers/arm/sp805/sp805.c
-
-BL2_SOURCES		+=	${SECURITY_SOURCES}			\
-				${CSS_SGM_BASE}/sgm_plat_config.c
-
-BL2U_SOURCES		+=	${SECURITY_SOURCES}
-
-BL31_SOURCES		+=	$(SGM_CPU_SOURCES)			\
-				${INTERCONNECT_SOURCES}			\
-				${SECURITY_SOURCES}			\
-				${SGM_GIC_SOURCES}			\
-				${CSS_SGM_BASE}/sgm_topology.c		\
-				${CSS_SGM_BASE}/sgm_bl31_setup.c	\
-				${CSS_SGM_BASE}/sgm_plat_config.c
-
-ifneq (${RESET_TO_BL31},0)
-  $(error "Using BL31 as the reset vector is not supported on ${PLAT} platform. \
-  Please set RESET_TO_BL31 to 0.")
-endif
-
-# sgm uses CCI-500 as Cache Coherent Interconnect
-ARM_CCI_PRODUCT_ID	:=	500
-
-# System coherency is managed in hardware
-HW_ASSISTED_COHERENCY	:=	1
-
-# When building for systems with hardware-assisted coherency, there's no need to
-# use USE_COHERENT_MEM. Require that USE_COHERENT_MEM must be set to 0 too.
-USE_COHERENT_MEM	:=	0
-
-override ARM_PLAT_MT	:=	1
-
-$(eval $(call add_define,SGM_PLAT))
-
-include plat/arm/common/arm_common.mk
-include plat/arm/board/common/board_common.mk
-include plat/arm/css/common/css_common.mk
-include plat/arm/soc/common/soc_css.mk
diff --git a/plat/arm/css/sgm/sgm_bl1_setup.c b/plat/arm/css/sgm/sgm_bl1_setup.c
deleted file mode 100644
index 5fd9655..0000000
--- a/plat/arm/css/sgm/sgm_bl1_setup.c
+++ /dev/null
@@ -1,45 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <common/bl_common.h>
-#include <common/debug.h>
-#include <plat/arm/common/plat_arm.h>
-#include <plat/arm/soc/common/soc_css.h>
-#include <plat/arm/common/arm_def.h>
-#include <drivers/arm/sp805.h>
-#include <sgm_plat_config.h>
-
-void bl1_early_platform_setup(void)
-{
-
-	/* Initialize the console before anything else */
-	arm_bl1_early_platform_setup();
-
-	/* Initialize the platform configuration structure */
-	plat_config_init();
-
-#if !HW_ASSISTED_COHERENCY
-	/*
-	 * Initialize Interconnect for this cluster during cold boot.
-	 * No need for locks as no other CPU is active.
-	 */
-	plat_arm_interconnect_init();
-	/*
-	 * Enable Interconnect coherency for the primary CPU's cluster.
-	 */
-	plat_arm_interconnect_enter_coherency();
-#endif
-}
-
-void plat_arm_secure_wdt_start(void)
-{
-	sp805_start(ARM_SP805_TWDG_BASE, ARM_TWDG_LOAD_VAL);
-}
-
-void plat_arm_secure_wdt_stop(void)
-{
-	sp805_stop(ARM_SP805_TWDG_BASE);
-}
diff --git a/plat/arm/css/sgm/sgm_bl31_setup.c b/plat/arm/css/sgm/sgm_bl31_setup.c
deleted file mode 100644
index 907e9fd..0000000
--- a/plat/arm/css/sgm/sgm_bl31_setup.c
+++ /dev/null
@@ -1,55 +0,0 @@
-/*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <common/bl_common.h>
-#include <common/debug.h>
-#include <drivers/arm/css/css_mhu_doorbell.h>
-#include <drivers/arm/css/scmi.h>
-#include <plat/arm/common/plat_arm.h>
-
-#include <sgm_plat_config.h>
-
-static scmi_channel_plat_info_t sgm775_scmi_plat_info = {
-		.scmi_mbx_mem = CSS_SCMI_PAYLOAD_BASE,
-		.db_reg_addr = PLAT_CSS_MHU_BASE + CSS_SCMI_MHU_DB_REG_OFF,
-		.db_preserve_mask = 0xfffffffe,
-		.db_modify_mask = 0x1,
-		.ring_doorbell = &mhu_ring_doorbell,
-};
-
-scmi_channel_plat_info_t *plat_css_get_scmi_info(int channel_id)
-{
-	return &sgm775_scmi_plat_info;
-}
-
-void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
-				u_register_t arg2, u_register_t arg3)
-{
-	uint32_t plat_version;
-	bl_params_node_t *bl_params;
-
-	bl_params = ((bl_params_t *)arg0)->head;
-
-	/* Initialize the platform configuration structure */
-	plat_config_init();
-
-	while (bl_params) {
-		if (bl_params->image_id == BL33_IMAGE_ID) {
-			plat_version = mmio_read_32(SSC_VERSION);
-			bl_params->ep_info->args.arg2 = plat_version;
-			break;
-		}
-
-		bl_params = bl_params->next_params_info;
-	}
-
-	arm_bl31_early_platform_setup((void *)arg0, arg1, arg2, (void *)arg3);
-}
-
-const plat_psci_ops_t *plat_arm_psci_override_pm_ops(plat_psci_ops_t *ops)
-{
-	return css_scmi_override_pm_ops(ops);
-}
diff --git a/plat/arm/css/sgm/sgm_interconnect.c b/plat/arm/css/sgm/sgm_interconnect.c
deleted file mode 100644
index 5b45341..0000000
--- a/plat/arm/css/sgm/sgm_interconnect.c
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <cdefs.h>
-
-/*
- * As the SGM platform supports FCM (with automatic interconnect
- * enter/exit), we should not do anything in these interface functions.
- * They are used to override the weak functions in cci drivers.
- */
-
-/******************************************************************************
- * Helper function to initialize ARM interconnect driver.
- *****************************************************************************/
-void __init plat_arm_interconnect_init(void)
-{
-}
-
-/******************************************************************************
- * Helper function to place current master into coherency
- *****************************************************************************/
-void plat_arm_interconnect_enter_coherency(void)
-{
-}
-
-/******************************************************************************
- * Helper function to remove current master from coherency
- *****************************************************************************/
-void plat_arm_interconnect_exit_coherency(void)
-{
-}
diff --git a/plat/arm/css/sgm/sgm_mmap_config.c b/plat/arm/css/sgm/sgm_mmap_config.c
deleted file mode 100644
index e5b4b03..0000000
--- a/plat/arm/css/sgm/sgm_mmap_config.c
+++ /dev/null
@@ -1,89 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <platform_def.h>
-
-#include <common/bl_common.h>
-#include <common/debug.h>
-#include <plat/arm/common/plat_arm.h>
-
-#include <sgm_variant.h>
-
-/*
- * Table of regions for different BL stages to map using the MMU.
- * This doesn't include Trusted RAM as the 'mem_layout' argument passed to
- * arm_configure_mmu_elx() will give the available subset of that.
- */
-#if IMAGE_BL1
-const mmap_region_t plat_arm_mmap[] = {
-	ARM_MAP_SHARED_RAM,
-	V2M_MAP_FLASH0_RO,
-	V2M_MAP_IOFPGA,
-	CSS_MAP_DEVICE,
-	CSS_MAP_GIC_DEVICE,
-	SOC_CSS_MAP_DEVICE,
-#if TRUSTED_BOARD_BOOT
-	ARM_MAP_NS_DRAM1,
-#endif
-	{0}
-};
-#endif
-#if IMAGE_BL2
-const mmap_region_t plat_arm_mmap[] = {
-	ARM_MAP_SHARED_RAM,
-	V2M_MAP_FLASH0_RO,
-	V2M_MAP_IOFPGA,
-	CSS_MAP_DEVICE,
-	CSS_MAP_GIC_DEVICE,
-	SOC_CSS_MAP_DEVICE,
-	ARM_MAP_NS_DRAM1,
-#ifdef SPD_tspd
-	ARM_MAP_TSP_SEC_MEM,
-#endif
-#ifdef SPD_opteed
-	ARM_OPTEE_PAGEABLE_LOAD_MEM,
-#endif
-#if TRUSTED_BOARD_BOOT && !BL2_AT_EL3
-	ARM_MAP_BL1_RW,
-#endif
-	{0}
-};
-#endif
-#if IMAGE_BL2U
-const mmap_region_t plat_arm_mmap[] = {
-	ARM_MAP_SHARED_RAM,
-	CSS_MAP_DEVICE,
-	CSS_MAP_GIC_DEVICE,
-	SOC_CSS_MAP_DEVICE,
-	{0}
-};
-#endif
-#if IMAGE_BL31
-const mmap_region_t plat_arm_mmap[] = {
-	ARM_MAP_SHARED_RAM,
-	V2M_MAP_IOFPGA,
-	CSS_MAP_DEVICE,
-	CSS_MAP_GIC_DEVICE,
-	SOC_CSS_MAP_DEVICE,
-	{0}
-};
-#endif
-#if IMAGE_BL32
-const mmap_region_t plat_arm_mmap[] = {
-	V2M_MAP_IOFPGA,
-	CSS_MAP_DEVICE,
-	CSS_MAP_GIC_DEVICE,
-	SOC_CSS_MAP_DEVICE,
-	{0}
-};
-#endif
-
-ARM_CASSERT_MMAP
-
-const mmap_region_t *plat_arm_get_mmap(void)
-{
-	return plat_arm_mmap;
-}
diff --git a/plat/arm/css/sgm/sgm_plat_config.c b/plat/arm/css/sgm/sgm_plat_config.c
deleted file mode 100644
index eed3631..0000000
--- a/plat/arm/css/sgm/sgm_plat_config.c
+++ /dev/null
@@ -1,80 +0,0 @@
-/*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <assert.h>
-#include <string.h>
-
-#include <platform_def.h>
-
-#include <common/debug.h>
-#include <plat/common/platform.h>
-#include <plat/arm/common/plat_arm.h>
-
-#include <sgm_plat_config.h>
-#include <sgm_variant.h>
-
-static css_plat_config_t *css_plat_info;
-
-/* Interconnect */
-const css_inteconn_config_t sgm_inteconn = {
-	.ip_type = ARM_CCI,
-	.plat_inteconn_desc = NULL
-};
-
-/* Special definition for SGM775 */
-/* Topology configuration for SGM775 */
-const unsigned char sgm775_power_domain_tree_desc[] = {
-	/* No of root nodes */
-	ARM_SYSTEM_COUNT,
-	/* No of children for the root node */
-	PLAT_ARM_CLUSTER_COUNT,
-	/* No of children for the first cluster node */
-	PLAT_ARM_CLUSTER_CORE_COUNT,
-};
-
-const css_topology_t sgm775_topology = {
-	.power_tree = sgm775_power_domain_tree_desc,
-	.plat_cluster_core_count = PLAT_ARM_CLUSTER_CORE_COUNT
-};
-
-/* Configuration structure for SGM775 */
-css_plat_config_t sgm775_config = {
-	.inteconn = &sgm_inteconn,
-	.topology = &sgm775_topology
-};
-
-/*******************************************************************************
- * This function initializes the platform structure.
- ******************************************************************************/
-void plat_config_init(void)
-{
-	/* Get the platform configurations */
-	switch (GET_PLAT_PART_NUM) {
-	case SGM775_SSC_VER_PART_NUM:
-		css_plat_info = &sgm775_config;
-
-		break;
-	default:
-		ERROR("Not a valid sgm variant!\n");
-		panic();
-	}
-}
-
-/*******************************************************************************
- * This function returns the platform structure pointer.
- ******************************************************************************/
-css_plat_config_t *get_plat_config(void)
-{
-	assert(css_plat_info != NULL);
-	return css_plat_info;
-}
-
-#if TRUSTED_BOARD_BOOT
-int plat_get_mbedtls_heap(void **heap_addr, size_t *heap_size)
-{
-	return get_mbedtls_heap_helper(heap_addr, heap_size);
-}
-#endif
diff --git a/plat/arm/css/sgm/sgm_security.c b/plat/arm/css/sgm/sgm_security.c
deleted file mode 100644
index 21d5306..0000000
--- a/plat/arm/css/sgm/sgm_security.c
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <common/debug.h>
-#include <drivers/arm/tzc_dmc500.h>
-#include <plat/arm/common/plat_arm.h>
-#include <plat/arm/soc/common/soc_css.h>
-
-#include <sgm_variant.h>
-
-/* Is populated with the DMC-500 controllers base addresses */
-static tzc_dmc500_driver_data_t plat_driver_data;
-
-void plat_sgm_dp_security_setup(void)
-{
-	unsigned int nprot_nsaid;
-
-	/*
-	 * At reset the Mali display processors start with NSAIDs set to zero
-	 * so the firmware must set them up to the expected values for ARM sgm
-	 * platforms.
-	 */
-
-	nprot_nsaid = mmio_read_32(MALI_DP_BASE + DP_NPROT_NSAID_OFFSET);
-	nprot_nsaid &= ~((0xF << W_NPROT_NSAID_SHIFT) |
-			(0xF << LS_NPORT_NSAID_SHIFT));
-	nprot_nsaid |= ((TZC_NSAID_DISP1 << W_NPROT_NSAID_SHIFT) |
-			(TZC_NSAID_DISP0 << LS_NPORT_NSAID_SHIFT));
-	mmio_write_32(MALI_DP_BASE + DP_NPROT_NSAID_OFFSET, nprot_nsaid);
-}
-
-void plat_arm_security_setup(void)
-{
-	unsigned int i;
-	unsigned int part_num = GET_PLAT_PART_NUM;
-
-	INFO("part_num: 0x%x\n", part_num);
-
-	/*
-	 * Initialise plat_driver_data with platform specific DMC_BASE
-	 * addresses
-	 */
-	switch (part_num) {
-	case SGM775_SSC_VER_PART_NUM:
-		for (i = 0; i < SGM775_DMC_COUNT; i++)
-			plat_driver_data.dmc_base[i] = PLAT_ARM_TZC_BASE
-					+ SGM_DMC_SIZE * i;
-		plat_driver_data.dmc_count = SGM775_DMC_COUNT;
-		break;
-	default:
-		/* Unexpected platform */
-		ERROR("Unexpected platform\n");
-		panic();
-	}
-	/* Initialize the TrustZone Controller in DMC-500 */
-	arm_tzc_dmc500_setup(&plat_driver_data, NULL);
-
-	/* Do DP NSAID setup */
-	plat_sgm_dp_security_setup();
-	/* Do ARM CSS SoC security setup */
-	soc_css_security_setup();
-}
diff --git a/plat/arm/css/sgm/sgm_topology.c b/plat/arm/css/sgm/sgm_topology.c
deleted file mode 100644
index 2d9552d..0000000
--- a/plat/arm/css/sgm/sgm_topology.c
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <plat/arm/common/plat_arm.h>
-
-#include <sgm_plat_config.h>
-
-/*******************************************************************************
- * This function returns the topology tree information.
- ******************************************************************************/
-const unsigned char *plat_get_power_domain_tree_desc(void)
-{
-	return get_plat_config()->topology->power_tree;
-}
-
-/*******************************************************************************
- * This function returns the core count within the cluster corresponding to
- * `mpidr`.
- ******************************************************************************/
-unsigned int plat_arm_get_cluster_core_count(u_register_t mpidr)
-{
-	return get_plat_config()->topology->plat_cluster_core_count;
-}
-
-/*
- * The array mapping platform core position (implemented by plat_my_core_pos())
- * to the SCMI power domain ID implemented by SCP.
- */
-const uint32_t plat_css_core_pos_to_scmi_dmn_id_map[PLATFORM_CORE_COUNT] = {
-			0, 1, 2, 3, 4, 5, 6, 7 };
diff --git a/plat/arm/css/sgm/tsp/sgm_tsp_setup.c b/plat/arm/css/sgm/tsp/sgm_tsp_setup.c
deleted file mode 100644
index 5f40c4c..0000000
--- a/plat/arm/css/sgm/tsp/sgm_tsp_setup.c
+++ /dev/null
@@ -1,17 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <plat/arm/common/plat_arm.h>
-
-#include <sgm_plat_config.h>
-
-void tsp_early_platform_setup(void)
-{
-	/* Initialize the platform configuration structure */
-	plat_config_init();
-
-	arm_tsp_early_platform_setup();
-}
diff --git a/plat/arm/css/sgm/tsp/tsp-sgm.mk b/plat/arm/css/sgm/tsp/tsp-sgm.mk
deleted file mode 100644
index de5e5b7..0000000
--- a/plat/arm/css/sgm/tsp/tsp-sgm.mk
+++ /dev/null
@@ -1,11 +0,0 @@
-#
-# Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
-#
-# SPDX-License-Identifier: BSD-3-Clause
-#
-
-BL32_SOURCES		+= 	${SGM_GIC_SOURCES}			\
-				${CSS_SGM_BASE}/sgm_plat_config.c	\
-				plat/arm/css/sgm/tsp/sgm_tsp_setup.c
-
-include plat/arm/common/tsp/arm_tsp.mk
diff --git a/plat/imx/imx8m/imx8m_caam.c b/plat/imx/imx8m/imx8m_caam.c
index 478005e..644572c 100644
--- a/plat/imx/imx8m/imx8m_caam.c
+++ b/plat/imx/imx8m/imx8m_caam.c
@@ -1,13 +1,16 @@
 /*
- * Copyright (c) 2019, NXP. All rights reserved.
+ * Copyright (c) 2019-2022 NXP. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <common/debug.h>
 #include <lib/mmio.h>
 
 #include <imx8m_caam.h>
 
+#define HAB_JR0_DID	U(0x8011)
+
 void imx8m_caam_init(void)
 {
 	uint32_t sm_cmd;
@@ -20,7 +23,12 @@
 	mmio_write_32(SM_CMD, sm_cmd);
 
 	/* config CAAM JRaMID set MID to Cortex A */
-	mmio_write_32(CAAM_JR0MID, CAAM_NS_MID);
+	if (mmio_read_32(CAAM_JR0MID) == HAB_JR0_DID) {
+		NOTICE("Do not release JR0 to NS as it can be used by HAB");
+	} else {
+		mmio_write_32(CAAM_JR0MID, CAAM_NS_MID);
+	}
+
 	mmio_write_32(CAAM_JR1MID, CAAM_NS_MID);
 	mmio_write_32(CAAM_JR2MID, CAAM_NS_MID);
 
diff --git a/plat/imx/imx8m/imx8m_csu.c b/plat/imx/imx8m/imx8m_csu.c
new file mode 100644
index 0000000..2b3a7d9
--- /dev/null
+++ b/plat/imx/imx8m/imx8m_csu.c
@@ -0,0 +1,56 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/mmio.h>
+
+#include <imx8m_csu.h>
+
+void imx_csu_init(const struct imx_csu_cfg *csu_cfg)
+{
+	const struct imx_csu_cfg *csu = csu_cfg;
+	uint32_t val;
+
+	while (csu->type != CSU_INVALID) {
+		switch (csu->type) {
+		case CSU_CSL:
+			val = mmio_read_32(CSLx_REG(csu->idx));
+			if (val & CSLx_LOCK(csu->idx)) {
+				break;
+			}
+			mmio_clrsetbits_32(CSLx_REG(csu->idx), CSLx_CFG(0xff, csu->idx),
+				CSLx_CFG(csu->csl_level | (csu->lock << 8), csu->idx));
+			break;
+		case CSU_HP:
+			val = mmio_read_32(CSU_HP_REG(csu->idx));
+			if (val & CSU_HP_LOCK(csu->idx)) {
+				break;
+			}
+			mmio_clrsetbits_32(CSU_HP_REG(csu->idx), CSU_HP_CFG(0x1, csu->idx),
+				CSU_HP_CFG(csu->hp | (csu->lock << 0x1), csu->idx));
+			break;
+		case CSU_SA:
+			val = mmio_read_32(CSU_SA_REG(csu->idx));
+			if (val & CSU_SA_LOCK(csu->idx)) {
+				break;
+			}
+			mmio_clrsetbits_32(CSU_SA_REG(csu->idx), CSU_SA_CFG(0x1, csu->idx),
+				CSU_SA_CFG(csu->sa | (csu->lock << 0x1), csu->idx));
+			break;
+		case CSU_HPCONTROL:
+			val = mmio_read_32(CSU_HPCONTROL_REG(csu->idx));
+			if (val & CSU_HPCONTROL_LOCK(csu->idx)) {
+				break;
+			}
+			mmio_clrsetbits_32(CSU_HPCONTROL_REG(csu->idx), CSU_HPCONTROL_CFG(0x1, csu->idx),
+				CSU_HPCONTROL_CFG(csu->hpctrl | (csu->lock << 0x1), csu->idx));
+			break;
+		default:
+			break;
+		}
+
+		csu++;
+	}
+}
diff --git a/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c b/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c
index 40110d7..debede1 100644
--- a/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2022 ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,7 +18,7 @@
 #include <drivers/generic_delay_timer.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/mmio.h>
-#include <lib/xlat_tables/xlat_tables.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
 #include <gpc.h>
@@ -26,8 +26,11 @@
 #include <imx_uart.h>
 #include <imx_rdc.h>
 #include <imx8m_caam.h>
+#include <imx8m_csu.h>
 #include <plat_imx8.h>
 
+#define TRUSTY_PARAMS_LEN_BYTES      (4096*2)
+
 static const mmap_region_t imx_mmap[] = {
 	MAP_REGION_FLAT(IMX_GIC_BASE, IMX_GIC_SIZE, MT_DEVICE | MT_RW),
 	MAP_REGION_FLAT(IMX_AIPS_BASE, IMX_AIPS_SIZE, MT_DEVICE | MT_RW), /* AIPS map */
@@ -44,9 +47,11 @@
 
 static const struct imx_rdc_cfg rdc[] = {
 	/* Master domain assignment */
-	RDC_MDAn(0x1, DID1),
+	RDC_MDAn(RDC_MDA_M4, DID1),
 
 	/* peripherals domain permission */
+	RDC_PDAPn(RDC_PDAP_UART4, D1R | D1W),
+	RDC_PDAPn(RDC_PDAP_UART2, D0R | D0W),
 
 	/* memory region */
 
@@ -54,6 +59,20 @@
 	{0},
 };
 
+static const struct imx_csu_cfg csu_cfg[] = {
+	/* peripherals csl setting */
+	CSU_CSLx(0x1, CSU_SEC_LEVEL_0, UNLOCKED),
+
+	/* master HP0~1 */
+
+	/* SA setting */
+
+	/* HP control setting */
+
+	/* Sentinel */
+	{0}
+};
+
 static entry_point_info_t bl32_image_ep_info;
 static entry_point_info_t bl33_image_ep_info;
 
@@ -109,6 +128,8 @@
 
 	imx_rdc_init(rdc);
 
+	imx_csu_init(csu_cfg);
+
 	imx8m_caam_init();
 
 	console_imx_uart_register(IMX_BOOT_UART_BASE, IMX_BOOT_UART_CLK_IN_HZ,
@@ -124,7 +145,7 @@
 	bl33_image_ep_info.spsr = get_spsr_for_bl33_entry();
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
 
-#ifdef SPD_opteed
+#if defined(SPD_opteed) || defined(SPD_trusty)
 	/* Populate entry point information for BL32 */
 	SET_PARAM_HEAD(&bl32_image_ep_info, PARAM_EP, VERSION_1, 0);
 	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
@@ -134,6 +155,16 @@
 	/* Pass TEE base and size to bl33 */
 	bl33_image_ep_info.args.arg1 = BL32_BASE;
 	bl33_image_ep_info.args.arg2 = BL32_SIZE;
+
+#ifdef SPD_trusty
+	bl32_image_ep_info.args.arg0 = BL32_SIZE;
+	bl32_image_ep_info.args.arg1 = BL32_BASE;
+#else
+	/* Make sure memory is clean */
+	mmio_write_32(BL32_FDT_OVERLAY_ADDR, 0);
+	bl33_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+	bl32_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+#endif
 #endif
 
 	bl31_tzc380_setup();
@@ -150,6 +181,9 @@
 		(BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE),
 		MT_DEVICE | MT_RW | MT_SECURE);
 #endif
+	/* Map TEE memory */
+	mmap_add_region(BL32_BASE, BL32_BASE, BL32_SIZE, MT_MEMORY | MT_RW);
+
 	mmap_add(imx_mmap);
 
 	init_xlat_tables();
@@ -184,3 +218,12 @@
 {
 	return COUNTER_FREQUENCY;
 }
+
+#ifdef SPD_trusty
+void plat_trusty_set_boot_args(aapcs64_params_t *args)
+{
+	args->arg0 = BL32_SIZE;
+	args->arg1 = BL32_BASE;
+	args->arg2 = TRUSTY_PARAMS_LEN_BYTES;
+}
+#endif
diff --git a/plat/imx/imx8m/imx8mm/include/imx_sec_def.h b/plat/imx/imx8m/imx8mm/include/imx_sec_def.h
new file mode 100644
index 0000000..6215983
--- /dev/null
+++ b/plat/imx/imx8m/imx8mm/include/imx_sec_def.h
@@ -0,0 +1,216 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_SEC_DEF_H
+#define IMX_SEC_DEF_H
+
+/* RDC MDA index */
+enum rdc_mda_idx {
+	RDC_MDA_A53 = 0,
+	RDC_MDA_M4 = 1,
+	RDC_MDA_PCIE_CTRL1 = 2,
+	RDC_MDA_SDMA3p = 3,
+	RDC_MDA_VPU_Decoders = 4,
+	RDC_MDA_LCDIF = 5,
+	RDC_MDA_CSI1 = 6,
+	RDC_MDA_SDMA3b = 7,
+	RDC_MDA_Coresight = 8,
+	RDC_MDA_DAP = 9,
+	RDC_MDA_CAAM = 10,
+	RDC_MDA_SDMA1p = 11,
+	RDC_MDA_SDMA1b = 12,
+	RDC_MDA_APBHDMA = 13,
+	RDC_MDA_NAND = 14,
+	RDC_MDA_uSDHC1 = 15,
+	RDC_MDA_uSDHC2 = 16,
+	RDC_MDA_uSDHC3 = 17,
+	RDC_MDA_GPU = 18,
+	RDC_MDA_USB1 = 19,
+	RDC_MDA_USB2 = 20,
+	RDC_MDA_TESTPORT = 21,
+	RDC_MDA_ENET1_TX = 22,
+	RDC_MDA_ENET1_RX = 23,
+	RDC_MDA_SDMA2p = 24,
+	RDC_MDA_SDMA2b = 24,
+	RDC_MDA_SDMA2_to_SPBA2 = 24,
+	RDC_MDA_SDMA3_to_SPBA2 = 25,
+	RDC_MDA_SDMA1_to_SPBA1 = 26,
+};
+
+/* RDC Peripherals index */
+enum rdc_pdap_idx {
+	RDC_PDAP_GPIO2 = 1,
+	RDC_PDAP_GPIO3 = 2,
+	RDC_PDAP_GPIO4 = 3,
+	RDC_PDAP_GPIO5 = 4,
+	RDC_PDAP_ANA_TSENSOR = 6,
+	RDC_PDAP_ANA_OSC = 7,
+	RDC_PDAP_WDOG1 = 8,
+	RDC_PDAP_WDOG2 = 9,
+	RDC_PDAP_WDOG3 = 10,
+	RDC_PDAP_SDMA3 = 11,
+	RDC_PDAP_SDMA2 = 12,
+	RDC_PDAP_GPT1 = 13,
+	RDC_PDAP_GPT2 = 14,
+	RDC_PDAP_GPT3 = 15,
+	RDC_PDAP_ROMCP = 17,
+	RDC_PDAP_IOMUXC = 19,
+	RDC_PDAP_IOMUXC_GPR = 20,
+	RDC_PDAP_OCOTP_CTRL = 21,
+	RDC_PDAP_ANA_PLL = 22,
+	RDC_PDAP_SNVS_HP = 23,
+	RDC_PDAP_CCM = 24,
+	RDC_PDAP_SRC = 25,
+	RDC_PDAP_GPC = 26,
+	RDC_PDAP_SEMAPHORE1 = 27,
+	RDC_PDAP_SEMAPHORE2 = 28,
+	RDC_PDAP_RDC = 29,
+	RDC_PDAP_CSU = 30,
+	RDC_PDAP_LCDIF = 32,
+	RDC_PDAP_MIPI_DSI = 33,
+	RDC_PDAP_CSI = 34,
+	RDC_PDAP_MIPI_CSI = 35,
+	RDC_PDAP_USB1 = 36,
+	RDC_PDAP_PWM1 = 38,
+	RDC_PDAP_PWM2 = 39,
+	RDC_PDAP_PWM3 = 40,
+	RDC_PDAP_PWM4 = 41,
+	RDC_PDAP_System_Counter_RD = 42,
+	RDC_PDAP_System_Counter_CMP = 43,
+	RDC_PDAP_System_Counter_CTRL = 44,
+	RDC_PDAP_GPT6 = 46,
+	RDC_PDAP_GPT5 = 47,
+	RDC_PDAP_GPT4 = 48,
+	RDC_PDAP_TZASC = 56,
+	RDC_PDAP_USB2 = 59,
+	RDC_PDAP_PERFMON1 = 60,
+	RDC_PDAP_PERFMON2 = 61,
+	RDC_PDAP_PLATFORM_CTRL = 62,
+	RDC_PDAP_QoSC = 63,
+	RDC_PDAP_I2C1 = 66,
+	RDC_PDAP_I2C2 = 67,
+	RDC_PDAP_I2C3 = 68,
+	RDC_PDAP_I2C4 = 69,
+	RDC_PDAP_UART4 = 70,
+	RDC_PDAP_MU_A = 74,
+	RDC_PDAP_MU_B = 75,
+	RDC_PDAP_SEMAPHORE_HS = 76,
+	RDC_PDAP_SAI1 = 78,
+	RDC_PDAP_SAI2 = 79,
+	RDC_PDAP_SAI3 = 80,
+	RDC_PDAP_SAI5 = 82,
+	RDC_PDAP_SAI6 = 83,
+	RDC_PDAP_uSDHC1 = 84,
+	RDC_PDAP_uSDHC2 = 85,
+	RDC_PDAP_uSDHC3 = 86,
+	RDC_PDAP_PCIE_PHY1 = 88,
+	RDC_PDAP_SPBA2 = 90,
+	RDC_PDAP_QSPI = 91,
+	RDC_PDAP_SDMA1 = 93,
+	RDC_PDAP_ENET1 = 94,
+	RDC_PDAP_SPDIF1 = 97,
+	RDC_PDAP_eCSPI1 = 98,
+	RDC_PDAP_eCSPI2 = 99,
+	RDC_PDAP_eCSPI3 = 100,
+	RDC_PDAP_MICFIL = 101,
+	RDC_PDAP_UART1 = 102,
+	RDC_PDAP_UART3 = 104,
+	RDC_PDAP_UART2 = 105,
+	RDC_PDAP_SPDIF2 = 106,
+	RDC_PDAP_SPBA1 = 111,
+	RDC_PDAP_CAAM = 114,
+};
+
+enum csu_csl_idx {
+	CSU_CSL_GPIO1 = 0,
+	CSU_CSL_GPIO2 = 1,
+	CSU_CSL_GPIO3 = 2,
+	CSU_CSL_GPIO4 = 3,
+	CSU_CSL_GPIO5 = 4,
+	CSU_CSL_ANA_TSENSOR = 6,
+	CSU_CSL_ANA_OSC = 7,
+	CSU_CSL_WDOG1 = 8,
+	CSU_CSL_WDOG2 = 9,
+	CSU_CSL_WDOG3 = 10,
+	CSU_CSL_SDMA2 = 12,
+	CSU_CSL_GPT1 = 13,
+	CSU_CSL_GPT2 = 14,
+	CSU_CSL_GPT3 = 15,
+	CSU_CSL_ROMCP = 17,
+	CSU_CSL_LCDIF = 18,
+	CSU_CSL_IOMUXC = 19,
+	CSU_CSL_IOMUXC_GPR = 20,
+	CSU_CSL_OCOTP_CTRL = 21,
+	CSU_CSL_ANA_PLL = 22,
+	CSU_CSL_SNVS_HP = 23,
+	CSU_CSL_CCM = 24,
+	CSU_CSL_SRC = 25,
+	CSU_CSL_GPC = 26,
+	CSU_CSL_SEMAPHORE1 = 27,
+	CSU_CSL_SEMAPHORE2 = 28,
+	CSU_CSL_RDC = 29,
+	CSU_CSL_CSU = 30,
+	CSU_CSL_DC_MST0 = 32,
+	CSU_CSL_DC_MST1 = 33,
+	CSU_CSL_DC_MST2 = 34,
+	CSU_CSL_DC_MST3 = 35,
+	CSU_CSL_PWM1 = 38,
+	CSU_CSL_PWM2 = 39,
+	CSU_CSL_PWM3 = 40,
+	CSU_CSL_PWM4 = 41,
+	CSU_CSL_System_Counter_RD = 42,
+	CSU_CSL_System_Counter_CMP = 43,
+	CSU_CSL_System_Counter_CTRL = 44,
+	CSU_CSL_GPT6 = 46,
+	CSU_CSL_GPT5 = 47,
+	CSU_CSL_GPT4 = 48,
+	CSU_CSL_TZASC = 56,
+	CSU_CSL_MTR = 59,
+	CSU_CSL_PERFMON1 = 60,
+	CSU_CSL_PERFMON2 = 61,
+	CSU_CSL_PLATFORM_CTRL = 62,
+	CSU_CSL_QoSC = 63,
+	CSU_CSL_MIPI_PHY = 64,
+	CSU_CSL_MIPI_DSI = 65,
+	CSU_CSL_I2C1 = 66,
+	CSU_CSL_I2C2 = 67,
+	CSU_CSL_I2C3 = 68,
+	CSU_CSL_I2C4 = 69,
+	CSU_CSL_UART4 = 70,
+	CSU_CSL_MIPI_CSI1 = 71,
+	CSU_CSL_MIPI_CSI_PHY1 = 72,
+	CSU_CSL_CSI1 = 73,
+	CSU_CSL_MU_A = 74,
+	CSU_CSL_MU_B = 75,
+	CSU_CSL_SEMAPHORE_HS = 76,
+	CSU_CSL_SAI1 = 78,
+	CSU_CSL_SAI6 = 80,
+	CSU_CSL_SAI5 = 81,
+	CSU_CSL_SAI4 = 82,
+	CSU_CSL_uSDHC1 = 84,
+	CSU_CSL_uSDHC2 = 85,
+	CSU_CSL_MIPI_CSI2 = 86,
+	CSU_CSL_MIPI_CSI_PHY2 = 87,
+	CSU_CSL_CSI2 = 88,
+	CSU_CSL_SPBA2 = 90,
+	CSU_CSL_QSPI = 91,
+	CSU_CSL_SDMA1 = 93,
+	CSU_CSL_ENET1 = 94,
+	CSU_CSL_SPDIF1 = 97,
+	CSU_CSL_eCSPI1 = 98,
+	CSU_CSL_eCSPI2 = 99,
+	CSU_CSL_eCSPI3 = 100,
+	CSU_CSL_UART1 = 102,
+	CSU_CSL_UART3 = 104,
+	CSU_CSL_UART2 = 105,
+	CSU_CSL_SPDIF2 = 106,
+	CSU_CSL_SAI2 = 107,
+	CSU_CSL_SAI3 = 108,
+	CSU_CSL_SPBA1 = 111,
+	CSU_CSL_CAAM = 114,
+};
+
+#endif /* IMX_SEC_DEF_H */
diff --git a/plat/imx/imx8m/imx8mm/include/platform_def.h b/plat/imx/imx8m/imx8mm/include/platform_def.h
index 300ef9e..ed693b9 100644
--- a/plat/imx/imx8m/imx8mm/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mm/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
  */
@@ -59,6 +59,8 @@
 #define PLAT_NS_IMAGE_OFFSET		U(0x40200000)
 #define PLAT_NS_IMAGE_SIZE		U(0x00200000)
 
+#define BL32_FDT_OVERLAY_ADDR		(PLAT_NS_IMAGE_OFFSET + 0x3000000)
+
 /* GICv3 base address */
 #define PLAT_GICD_BASE			U(0x38800000)
 #define PLAT_GICR_BASE			U(0x38880000)
diff --git a/plat/imx/imx8m/imx8mm/platform.mk b/plat/imx/imx8m/imx8mm/platform.mk
index cd8de89..0cce7ca 100644
--- a/plat/imx/imx8m/imx8mm/platform.mk
+++ b/plat/imx/imx8m/imx8mm/platform.mk
@@ -1,8 +1,11 @@
 #
-# Copyright (c) 2019-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2019-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
+#
+# Translation tables library
+include lib/xlat_tables_v2/xlat_tables.mk
 
 PLAT_INCLUDES		:=	-Iplat/imx/common/include		\
 				-Iplat/imx/imx8m/include		\
@@ -25,6 +28,7 @@
 				plat/imx/imx8m/gpc_common.c			\
 				plat/imx/imx8m/imx_aipstz.c			\
 				plat/imx/imx8m/imx_rdc.c			\
+				plat/imx/imx8m/imx8m_csu.c			\
 				plat/imx/imx8m/imx8m_caam.c			\
 				plat/imx/imx8m/imx8m_psci_common.c		\
 				plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c	\
@@ -34,14 +38,11 @@
 				plat/imx/common/imx_sip_handler.c		\
 				plat/imx/common/imx_sip_svc.c			\
 				plat/imx/common/imx_uart_console.S		\
-				plat/imx/common/imx_ehf.c                       \
-				plat/imx/common/imx_sdei.c                      \
-				lib/xlat_tables/aarch64/xlat_tables.c		\
-				lib/xlat_tables/xlat_tables_common.c		\
 				lib/cpus/aarch64/cortex_a53.S			\
 				drivers/arm/tzc/tzc380.c			\
 				drivers/delay_timer/delay_timer.c		\
 				drivers/delay_timer/generic_delay_timer.c	\
+				${XLAT_TABLES_LIB_SRCS}				\
 				${IMX_GIC_SOURCES}
 
 ifeq (${NEED_BL2},yes)
@@ -150,8 +151,11 @@
 IMX_BOOT_UART_BASE	?=	0x30890000
 $(eval $(call add_define,IMX_BOOT_UART_BASE))
 
-EL3_EXCEPTION_HANDLING := 1
-SDEI_SUPPORT := 1
+EL3_EXCEPTION_HANDLING := $(SDEI_SUPPORT)
+ifeq (${SDEI_SUPPORT}, 1)
+BL31_SOURCES 		+= 	plat/imx/common/imx_ehf.c	\
+				plat/imx/common/imx_sdei.c
+endif
 
 ifeq (${MEASURED_BOOT},1)
     MEASURED_BOOT_MK := drivers/measured_boot/event_log/event_log.mk
@@ -161,5 +165,8 @@
 BL2_SOURCES		+=	plat/imx/imx8m/imx8m_measured_boot.c	\
 				plat/imx/imx8m/imx8m_dyn_cfg_helpers.c	\
 				${EVENT_LOG_SOURCES}
+endif
 
+ifeq (${SPD},trusty)
+	BL31_CFLAGS    +=      -DPLAT_XLAT_TABLES_DYNAMIC=1
 endif
diff --git a/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c b/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c
index d4705ee..8147792 100644
--- a/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2019-2020 NXP
+ * Copyright 2019-2022 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -24,9 +24,12 @@
 #include <imx_uart.h>
 #include <imx_rdc.h>
 #include <imx8m_caam.h>
+#include <imx8m_csu.h>
 #include <platform_def.h>
 #include <plat_imx8.h>
 
+#define TRUSTY_PARAMS_LEN_BYTES      (4096*2)
+
 static const mmap_region_t imx_mmap[] = {
 	GIC_MAP, AIPS_MAP, OCRAM_S_MAP, DDRC_MAP, {0},
 };
@@ -41,9 +44,11 @@
 
 static const struct imx_rdc_cfg rdc[] = {
 	/* Master domain assignment */
-	RDC_MDAn(0x1, DID1),
+	RDC_MDAn(RDC_MDA_M7, DID1),
 
 	/* peripherals domain permission */
+	RDC_PDAPn(RDC_PDAP_UART4, D1R | D1W),
+	RDC_PDAPn(RDC_PDAP_UART2, D0R | D0W),
 
 	/* memory region */
 	RDC_MEM_REGIONn(16, 0x0, 0x0, 0xff),
@@ -54,6 +59,22 @@
 	{0},
 };
 
+static const struct imx_csu_cfg csu_cfg[] = {
+	/* peripherals csl setting */
+	CSU_CSLx(CSU_CSL_OCRAM, CSU_SEC_LEVEL_2, UNLOCKED),
+	CSU_CSLx(CSU_CSL_OCRAM_S, CSU_SEC_LEVEL_2, UNLOCKED),
+
+	/* master HP0~1 */
+
+	/* SA setting */
+
+	/* HP control setting */
+
+	/* Sentinel */
+	{0}
+};
+
+
 static entry_point_info_t bl32_image_ep_info;
 static entry_point_info_t bl33_image_ep_info;
 
@@ -98,6 +119,7 @@
 		u_register_t arg2, u_register_t arg3)
 {
 	static console_t console;
+	unsigned int val;
 	int i;
 
 	/* Enable CSU NS access permission */
@@ -109,6 +131,13 @@
 
 	imx_rdc_init(rdc);
 
+	imx_csu_init(csu_cfg);
+
+	/* config the ocram memory range for secure access */
+	mmio_write_32(IMX_IOMUX_GPR_BASE + 0x2c, 0x4c1);
+	val = mmio_read_32(IMX_IOMUX_GPR_BASE + 0x2c);
+	mmio_write_32(IMX_IOMUX_GPR_BASE + 0x2c, val | 0x3DFF0000);
+
 	imx8m_caam_init();
 
 	console_imx_uart_register(IMX_BOOT_UART_BASE, IMX_BOOT_UART_CLK_IN_HZ,
@@ -124,7 +153,7 @@
 	bl33_image_ep_info.spsr = get_spsr_for_bl33_entry();
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
 
-#ifdef SPD_opteed
+#if defined(SPD_opteed) || defined(SPD_trusty)
 	/* Populate entry point information for BL32 */
 	SET_PARAM_HEAD(&bl32_image_ep_info, PARAM_EP, VERSION_1, 0);
 	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
@@ -134,6 +163,16 @@
 	/* Pass TEE base and size to bl33 */
 	bl33_image_ep_info.args.arg1 = BL32_BASE;
 	bl33_image_ep_info.args.arg2 = BL32_SIZE;
+
+#ifdef SPD_trusty
+	bl32_image_ep_info.args.arg0 = BL32_SIZE;
+	bl32_image_ep_info.args.arg1 = BL32_BASE;
+#else
+	/* Make sure memory is clean */
+	mmio_write_32(BL32_FDT_OVERLAY_ADDR, 0);
+	bl33_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+	bl32_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+#endif
 #endif
 
 	bl31_tzc380_setup();
@@ -150,6 +189,10 @@
 		(BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE),
 		MT_DEVICE | MT_RW | MT_SECURE);
 #endif
+
+	/* Map TEE memory */
+	mmap_add_region(BL32_BASE, BL32_BASE, BL32_SIZE, MT_MEMORY | MT_RW);
+
 	mmap_add(imx_mmap);
 
 	init_xlat_tables();
@@ -184,3 +227,12 @@
 {
 	return COUNTER_FREQUENCY;
 }
+
+#ifdef SPD_trusty
+void plat_trusty_set_boot_args(aapcs64_params_t *args)
+{
+	args->arg0 = BL32_SIZE;
+	args->arg1 = BL32_BASE;
+	args->arg2 = TRUSTY_PARAMS_LEN_BYTES;
+}
+#endif
diff --git a/plat/imx/imx8m/imx8mn/include/imx_sec_def.h b/plat/imx/imx8m/imx8mn/include/imx_sec_def.h
new file mode 100644
index 0000000..0ef14a9
--- /dev/null
+++ b/plat/imx/imx8m/imx8mn/include/imx_sec_def.h
@@ -0,0 +1,210 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_SEC_DEF_H
+#define IMX_SEC_DEF_H
+
+/* RDC MDA index */
+enum rdc_mda_idx {
+	RDC_MDA_A53 = 0,
+	RDC_MDA_M7 = 1,
+	RDC_MDA_SDMA3p = 3,
+	RDC_MDA_LCDIF = 5,
+	RDC_MDA_ISI = 6,
+	RDC_MDA_SDMA3b = 7,
+	RDC_MDA_Coresight = 8,
+	RDC_MDA_DAP = 9,
+	RDC_MDA_CAAM = 10,
+	RDC_MDA_SDMA1p = 11,
+	RDC_MDA_SDMA1b = 12,
+	RDC_MDA_APBHDMA = 13,
+	RDC_MDA_RAWNAND = 14,
+	RDC_MDA_uSDHC1 = 15,
+	RDC_MDA_uSDHC2 = 16,
+	RDC_MDA_uSDHC3 = 17,
+	RDC_MDA_GPU = 18,
+	RDC_MDA_USB1 = 19,
+	RDC_MDA_TESTPORT = 21,
+	RDC_MDA_ENET1_TX = 22,
+	RDC_MDA_ENET1_RX = 23,
+	RDC_MDA_SDMA2 = 24,
+};
+
+/* RDC Peripherals index */
+enum rdc_pdap_idx {
+	RDC_PDAP_GPIO1 = 0,
+	RDC_PDAP_GPIO2 = 1,
+	RDC_PDAP_GPIO3 = 2,
+	RDC_PDAP_GPIO4 = 3,
+	RDC_PDAP_GPIO5 = 4,
+	RDC_PDAP_ANA_TSENSOR = 6,
+	RDC_PDAP_ANA_OSC = 7,
+	RDC_PDAP_WDOG1 = 8,
+	RDC_PDAP_WDOG2 = 9,
+	RDC_PDAP_WDOG3 = 10,
+	RDC_PDAP_SDMA3 = 11,
+	RDC_PDAP_SDMA2 = 12,
+	RDC_PDAP_GPT1 = 13,
+	RDC_PDAP_GPT2 = 14,
+	RDC_PDAP_GPT3 = 15,
+	RDC_PDAP_ROMCP = 17,
+	RDC_PDAP_IOMUXC = 19,
+	RDC_PDAP_IOMUXC_GPR = 20,
+	RDC_PDAP_OCOTP_CTRL = 21,
+	RDC_PDAP_ANA_PLL = 22,
+	RDC_PDAP_SNVS_HP = 23,
+	RDC_PDAP_CCM = 24,
+	RDC_PDAP_SRC = 25,
+	RDC_PDAP_GPC = 26,
+	RDC_PDAP_SEMAPHORE1 = 27,
+	RDC_PDAP_SEMAPHORE2 = 28,
+	RDC_PDAP_RDC = 29,
+	RDC_PDAP_CSU = 30,
+	RDC_PDAP_LCDIF = 32,
+	RDC_PDAP_MIPI_DSI = 33,
+	RDC_PDAP_ISI = 34,
+	RDC_PDAP_MIPI_CSI = 35,
+	RDC_PDAP_USB1 = 36,
+	RDC_PDAP_PWM1 = 38,
+	RDC_PDAP_PWM2 = 39,
+	RDC_PDAP_PWM3 = 40,
+	RDC_PDAP_PWM4 = 41,
+	RDC_PDAP_System_Counter_RD = 42,
+	RDC_PDAP_System_Counter_CMP = 43,
+	RDC_PDAP_System_Counter_CTRL = 44,
+	RDC_PDAP_GPT6 = 46,
+	RDC_PDAP_GPT5 = 47,
+	RDC_PDAP_GPT4 = 48,
+	RDC_PDAP_TZASC = 56,
+	RDC_PDAP_PERFMON1 = 60,
+	RDC_PDAP_PERFMON2 = 61,
+	RDC_PDAP_PLATFORM_CTRL = 62,
+	RDC_PDAP_QoSC = 63,
+	RDC_PDAP_I2C1 = 66,
+	RDC_PDAP_I2C2 = 67,
+	RDC_PDAP_I2C3 = 68,
+	RDC_PDAP_I2C4 = 69,
+	RDC_PDAP_UART4 = 70,
+	RDC_PDAP_MU_A = 74,
+	RDC_PDAP_MU_B = 75,
+	RDC_PDAP_SEMAPHORE_HS = 76,
+	RDC_PDAP_SAI2 = 79,
+	RDC_PDAP_SAI3 = 80,
+	RDC_PDAP_SAI5 = 82,
+	RDC_PDAP_SAI6 = 83,
+	RDC_PDAP_uSDHC1 = 84,
+	RDC_PDAP_uSDHC2 = 85,
+	RDC_PDAP_uSDHC3 = 86,
+	RDC_PDAP_SAI7 = 87,
+	RDC_PDAP_SPBA2 = 90,
+	RDC_PDAP_QSPI = 91,
+	RDC_PDAP_SDMA1 = 93,
+	RDC_PDAP_ENET1 = 94,
+	RDC_PDAP_SPDIF1 = 97,
+	RDC_PDAP_eCSPI1 = 98,
+	RDC_PDAP_eCSPI2 = 99,
+	RDC_PDAP_eCSPI3 = 100,
+	RDC_PDAP_MICFIL = 101,
+	RDC_PDAP_UART1 = 102,
+	RDC_PDAP_UART3 = 104,
+	RDC_PDAP_UART2 = 105,
+	RDC_PDAP_ASRC = 107,
+	RDC_PDAP_SPBA1 = 111,
+	RDC_PDAP_CAAM = 114,
+};
+
+enum csu_csl_idx {
+	CSU_CSL_GPIO1 = 0,
+	CSU_CSL_GPIO2 = 1,
+	CSU_CSL_GPIO3 = 2,
+	CSU_CSL_GPIO4 = 3,
+	CSU_CSL_GPIO5 = 4,
+	CSU_CSL_ANA_TSENSOR = 6,
+	CSU_CSL_ANA_OSC = 7,
+	CSU_CSL_WDOG1 = 8,
+	CSU_CSL_WDOG2 = 9,
+	CSU_CSL_WDOG3 = 10,
+	CSU_CSL_SDMA2 = 12,
+	CSU_CSL_GPT1 = 13,
+	CSU_CSL_GPT2 = 14,
+	CSU_CSL_GPT3 = 15,
+	CSU_CSL_ROMCP = 17,
+	CSU_CSL_LCDIF = 18,
+	CSU_CSL_IOMUXC = 19,
+	CSU_CSL_IOMUXC_GPR = 20,
+	CSU_CSL_OCOTP_CTRL = 21,
+	CSU_CSL_ANA_PLL = 22,
+	CSU_CSL_SNVS_HP = 23,
+	CSU_CSL_CCM = 24,
+	CSU_CSL_SRC = 25,
+	CSU_CSL_GPC = 26,
+	CSU_CSL_SEMAPHORE1 = 27,
+	CSU_CSL_SEMAPHORE2 = 28,
+	CSU_CSL_RDC = 29,
+	CSU_CSL_CSU = 30,
+	CSU_CSL_DC_MST0 = 32,
+	CSU_CSL_DC_MST1 = 33,
+	CSU_CSL_DC_MST2 = 34,
+	CSU_CSL_DC_MST3 = 35,
+	CSU_CSL_PWM1 = 38,
+	CSU_CSL_PWM2 = 39,
+	CSU_CSL_PWM3 = 40,
+	CSU_CSL_PWM4 = 41,
+	CSU_CSL_System_Counter_RD = 42,
+	CSU_CSL_System_Counter_CMP = 43,
+	CSU_CSL_System_Counter_CTRL = 44,
+	CSU_CSL_GPT6 = 46,
+	CSU_CSL_GPT5 = 47,
+	CSU_CSL_GPT4 = 48,
+	CSU_CSL_TZASC = 56,
+	CSU_CSL_MTR = 59,
+	CSU_CSL_PERFMON1 = 60,
+	CSU_CSL_PERFMON2 = 61,
+	CSU_CSL_PLATFORM_CTRL = 62,
+	CSU_CSL_QoSC = 63,
+	CSU_CSL_MIPI_PHY = 64,
+	CSU_CSL_MIPI_DSI = 65,
+	CSU_CSL_I2C1 = 66,
+	CSU_CSL_I2C2 = 67,
+	CSU_CSL_I2C3 = 68,
+	CSU_CSL_I2C4 = 69,
+	CSU_CSL_UART4 = 70,
+	CSU_CSL_MIPI_CSI1 = 71,
+	CSU_CSL_MIPI_CSI_PHY1 = 72,
+	CSU_CSL_CSI1 = 73,
+	CSU_CSL_MU_A = 74,
+	CSU_CSL_MU_B = 75,
+	CSU_CSL_SEMAPHORE_HS = 76,
+	CSU_CSL_SAI1 = 78,
+	CSU_CSL_SAI6 = 80,
+	CSU_CSL_SAI5 = 81,
+	CSU_CSL_SAI4 = 82,
+	CSU_CSL_uSDHC1 = 84,
+	CSU_CSL_uSDHC2 = 85,
+	CSU_CSL_MIPI_CSI2 = 86,
+	CSU_CSL_MIPI_CSI_PHY2 = 87,
+	CSU_CSL_CSI2 = 88,
+	CSU_CSL_SPBA2 = 90,
+	CSU_CSL_QSPI = 91,
+	CSU_CSL_SDMA1 = 93,
+	CSU_CSL_ENET1 = 94,
+	CSU_CSL_SPDIF1 = 97,
+	CSU_CSL_eCSPI1 = 98,
+	CSU_CSL_eCSPI2 = 99,
+	CSU_CSL_eCSPI3 = 100,
+	CSU_CSL_UART1 = 102,
+	CSU_CSL_UART3 = 104,
+	CSU_CSL_UART2 = 105,
+	CSU_CSL_SPDIF2 = 106,
+	CSU_CSL_SAI2 = 107,
+	CSU_CSL_SAI3 = 108,
+	CSU_CSL_SPBA1 = 111,
+	CSU_CSL_CAAM = 114,
+	CSU_CSL_OCRAM = 118,
+	CSU_CSL_OCRAM_S = 119,
+};
+
+#endif /* IMX_SEC_DEF_H */
diff --git a/plat/imx/imx8m/imx8mn/include/platform_def.h b/plat/imx/imx8m/imx8mn/include/platform_def.h
index 9c46d8d..8d39ea6 100644
--- a/plat/imx/imx8m/imx8mn/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mn/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2020 NXP
+ * Copyright 2020-2022 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -45,6 +45,8 @@
 /* non-secure uboot base */
 #define PLAT_NS_IMAGE_OFFSET		U(0x40200000)
 
+#define BL32_FDT_OVERLAY_ADDR		(PLAT_NS_IMAGE_OFFSET + 0x3000000)
+
 /* GICv3 base address */
 #define PLAT_GICD_BASE			U(0x38800000)
 #define PLAT_GICR_BASE			U(0x38880000)
diff --git a/plat/imx/imx8m/imx8mn/platform.mk b/plat/imx/imx8m/imx8mn/platform.mk
index 2087089..54be41b 100644
--- a/plat/imx/imx8m/imx8mn/platform.mk
+++ b/plat/imx/imx8m/imx8mn/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright 2019-2020 NXP
+# Copyright 2019-2022 NXP
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -23,6 +23,7 @@
 				plat/imx/imx8m/imx_aipstz.c			\
 				plat/imx/imx8m/imx_rdc.c			\
 				plat/imx/imx8m/imx8m_caam.c			\
+				plat/imx/imx8m/imx8m_csu.c			\
 				plat/imx/imx8m/imx8m_psci_common.c		\
 				plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c	\
 				plat/imx/imx8m/imx8mn/imx8mn_psci.c		\
@@ -31,8 +32,6 @@
 				plat/imx/common/imx_sip_handler.c		\
 				plat/imx/common/imx_sip_svc.c			\
 				plat/imx/common/imx_uart_console.S		\
-				plat/imx/common/imx_ehf.c                       \
-				plat/imx/common/imx_sdei.c                      \
 				lib/cpus/aarch64/cortex_a53.S			\
 				drivers/arm/tzc/tzc380.c			\
 				drivers/delay_timer/delay_timer.c		\
@@ -57,5 +56,12 @@
 IMX_BOOT_UART_BASE	?=	0x30890000
 $(eval $(call add_define,IMX_BOOT_UART_BASE))
 
-EL3_EXCEPTION_HANDLING := 1
-SDEI_SUPPORT := 1
+EL3_EXCEPTION_HANDLING := $(SDEI_SUPPORT)
+ifeq (${SDEI_SUPPORT}, 1)
+BL31_SOURCES 		+= 	plat/imx/common/imx_ehf.c	\
+				plat/imx/common/imx_sdei.c
+endif
+
+ifeq (${SPD},trusty)
+	BL31_CFLAGS    +=      -DPLAT_XLAT_TABLES_DYNAMIC=1
+endif
diff --git a/plat/imx/imx8m/imx8mp/gpc.c b/plat/imx/imx8m/imx8mp/gpc.c
index d660e3d..3d68b94 100644
--- a/plat/imx/imx8m/imx8mp/gpc.c
+++ b/plat/imx/imx8m/imx8mp/gpc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2019-2020 NXP
+ * Copyright 2019-2022 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -69,10 +69,11 @@
 	HDMIMIX,
 	HDMI_PHY,
 	DDRMIX,
+	MAX_DOMAINS,
 };
 
 /* PU domain, add some hole to minimize the uboot change */
-static struct imx_pwr_domain pu_domains[20] = {
+static struct imx_pwr_domain pu_domains[MAX_DOMAINS] = {
 	[MIPI_PHY1] = IMX_PD_DOMAIN(MIPI_PHY1, false),
 	[PCIE_PHY] = IMX_PD_DOMAIN(PCIE_PHY, false),
 	[USB1_PHY] = IMX_PD_DOMAIN(USB1_PHY, true),
@@ -174,6 +175,11 @@
 	struct imx_pwr_domain *pwr_domain = &pu_domains[domain_id];
 	unsigned int i;
 
+	/* validate the domain id */
+	if (domain_id >= MAX_DOMAINS) {
+		return;
+	}
+
 	if (domain_id == HSIOMIX) {
 		for (i = 0; i < ARRAY_SIZE(hsiomix_clk); i++) {
 			hsiomix_clk[i].val = mmio_read_32(IMX_CCM_BASE + hsiomix_clk[i].offset);
diff --git a/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c b/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c
index 22fbd5e..57e5c51 100644
--- a/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2020 NXP
+ * Copyright 2020-2022 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -24,9 +24,12 @@
 #include <imx_uart.h>
 #include <imx_rdc.h>
 #include <imx8m_caam.h>
+#include <imx8m_csu.h>
 #include <platform_def.h>
 #include <plat_imx8.h>
 
+#define TRUSTY_PARAMS_LEN_BYTES      (4096*2)
+
 static const mmap_region_t imx_mmap[] = {
 	GIC_MAP, AIPS_MAP, OCRAM_S_MAP, DDRC_MAP,
 	NOC_MAP, {0},
@@ -42,9 +45,10 @@
 
 static const struct imx_rdc_cfg rdc[] = {
 	/* Master domain assignment */
-	RDC_MDAn(0x1, DID1),
+	RDC_MDAn(RDC_MDA_M7, DID1),
 
 	/* peripherals domain permission */
+	RDC_PDAPn(RDC_PDAP_UART2, D0R | D0W),
 
 	/* memory region */
 
@@ -52,6 +56,21 @@
 	{0},
 };
 
+static const struct imx_csu_cfg csu_cfg[] = {
+	/* peripherals csl setting */
+	CSU_CSLx(CSU_CSL_OCRAM, CSU_SEC_LEVEL_2, UNLOCKED),
+	CSU_CSLx(CSU_CSL_OCRAM_S, CSU_SEC_LEVEL_2, UNLOCKED),
+
+	/* master HP0~1 */
+
+	/* SA setting */
+
+	/* HP control setting */
+
+	/* Sentinel */
+	{0}
+};
+
 static entry_point_info_t bl32_image_ep_info;
 static entry_point_info_t bl33_image_ep_info;
 
@@ -96,6 +115,7 @@
 		u_register_t arg2, u_register_t arg3)
 {
 	static console_t console;
+	unsigned int val;
 	unsigned int i;
 
 	/* Enable CSU NS access permission */
@@ -107,6 +127,13 @@
 
 	imx_rdc_init(rdc);
 
+	imx_csu_init(csu_cfg);
+
+	/* config the ocram memory range for secure access */
+	mmio_write_32(IMX_IOMUX_GPR_BASE + 0x2c, 0x4E1);
+	val = mmio_read_32(IMX_IOMUX_GPR_BASE + 0x2c);
+	mmio_write_32(IMX_IOMUX_GPR_BASE + 0x2c, val | 0x3DFF0000);
+
 	imx8m_caam_init();
 
 	console_imx_uart_register(IMX_BOOT_UART_BASE, IMX_BOOT_UART_CLK_IN_HZ,
@@ -122,7 +149,7 @@
 	bl33_image_ep_info.spsr = get_spsr_for_bl33_entry();
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
 
-#ifdef SPD_opteed
+#if defined(SPD_opteed) || defined(SPD_trusty)
 	/* Populate entry point information for BL32 */
 	SET_PARAM_HEAD(&bl32_image_ep_info, PARAM_EP, VERSION_1, 0);
 	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
@@ -132,6 +159,16 @@
 	/* Pass TEE base and size to bl33 */
 	bl33_image_ep_info.args.arg1 = BL32_BASE;
 	bl33_image_ep_info.args.arg2 = BL32_SIZE;
+
+#ifdef SPD_trusty
+	bl32_image_ep_info.args.arg0 = BL32_SIZE;
+	bl32_image_ep_info.args.arg1 = BL32_BASE;
+#else
+	/* Make sure memory is clean */
+	mmio_write_32(BL32_FDT_OVERLAY_ADDR, 0);
+	bl33_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+	bl32_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+#endif
 #endif
 
 	bl31_tzc380_setup();
@@ -148,6 +185,10 @@
 		(BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE),
 		MT_DEVICE | MT_RW | MT_SECURE);
 #endif
+
+	/* Map TEE memory */
+	mmap_add_region(BL32_BASE, BL32_BASE, BL32_SIZE, MT_MEMORY | MT_RW);
+
 	mmap_add(imx_mmap);
 
 	init_xlat_tables();
@@ -185,3 +226,12 @@
 {
 	return COUNTER_FREQUENCY;
 }
+
+#ifdef SPD_trusty
+void plat_trusty_set_boot_args(aapcs64_params_t *args)
+{
+	args->arg0 = BL32_SIZE;
+	args->arg1 = BL32_BASE;
+	args->arg2 = TRUSTY_PARAMS_LEN_BYTES;
+}
+#endif
diff --git a/plat/imx/imx8m/imx8mp/include/imx_sec_def.h b/plat/imx/imx8m/imx8mp/include/imx_sec_def.h
new file mode 100644
index 0000000..ba248b5
--- /dev/null
+++ b/plat/imx/imx8m/imx8mp/include/imx_sec_def.h
@@ -0,0 +1,274 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_SEC_DEF_H
+#define IMX_SEC_DEF_H
+
+/* RDC MDA index */
+enum rdc_mda_idx {
+	RDC_MDA_A53 = 0,
+	RDC_MDA_M7 = 1,
+	RDC_MDA_PCIE_CTRL1 = 2,
+	RDC_MDA_SDMA3p = 3,
+	RDC_MDA_SDMA3b = 4,
+	RDC_MDA_LCDIF = 5,
+	RDC_MDA_ISI = 6,
+	RDC_MDA_NPU = 7,
+	RDC_MDA_Coresight = 8,
+	RDC_MDA_DAP = 9,
+	RDC_MDA_CAAM = 10,
+	RDC_MDA_SDMA1p = 11,
+	RDC_MDA_SDMA1b = 12,
+	RDC_MDA_APBHDMA = 13,
+	RDC_MDA_RAWNAND = 14,
+	RDC_MDA_uSDHC1 = 15,
+	RDC_MDA_uSDHC2 = 16,
+	RDC_MDA_uSDHC3 = 17,
+	RDC_MDA_AUDIO_PROCESSOR = 18,
+	RDC_MDA_USB1 = 19,
+	RDC_MDA_USB2 = 20,
+	RDC_MDA_TESTPORT = 21,
+	RDC_MDA_ENET1_TX = 22,
+	RDC_MDA_ENET1_RX = 23,
+	RDC_MDA_SDMA2 = 24,
+	RDC_MDA_SDMA3_to_SPBA2 = 25,
+	RDC_MDA_SDMA1_to_SPBA1 = 26,
+	RDC_MDA_LCDIF2 = 27,
+	RDC_MDA_HDMI_TX = 28,
+	RDC_MDA_ENET2 = 29,
+	RDC_MDA_GPU3D = 30,
+	RDC_MDA_GPU2D = 31,
+	RDC_MDA_VPU_G1 = 32,
+	RDC_MDA_VPU_G2 = 33,
+	RDC_MDA_VPU_VC8000E = 34,
+	RDC_MDA_AUDIO_EDMA = 35,
+	RDC_MDA_ISP1 = 36,
+	RDC_MDA_ISP2 = 37,
+	RDC_MDA_DEWARP = 38,
+	RDC_MDA_GIC500 = 39,
+};
+
+/* RDC Peripherals index */
+enum rdc_pdap_idx {
+	RDC_PDAP_GPIO1 = 0,
+	RDC_PDAP_GPIO2 = 1,
+	RDC_PDAP_GPIO3 = 2,
+	RDC_PDAP_GPIO4 = 3,
+	RDC_PDAP_GPIO5 = 4,
+	RDC_PDAP_MU_2_A = 5,
+	RDC_PDAP_ANA_TSENSOR = 6,
+	RDC_PDAP_ANA_OSC = 7,
+	RDC_PDAP_WDOG1 = 8,
+	RDC_PDAP_WDOG2 = 9,
+	RDC_PDAP_WDOG3 = 10,
+	RDC_PDAP_GPT1 = 13,
+	RDC_PDAP_GPT2 = 14,
+	RDC_PDAP_GPT3 = 15,
+	RDC_PDAP_MU_2_B = 16,
+	RDC_PDAP_ROMCP = 17,
+	RDC_PDAP_MU_3_A = 18,
+	RDC_PDAP_IOMUXC = 19,
+	RDC_PDAP_IOMUXC_GPR = 20,
+	RDC_PDAP_OCOTP_CTRL = 21,
+	RDC_PDAP_ANA_PLL = 22,
+	RDC_PDAP_SNVS_HP = 23,
+	RDC_PDAP_CCM = 24,
+	RDC_PDAP_SRC = 25,
+	RDC_PDAP_GPC = 26,
+	RDC_PDAP_SEMAPHORE1 = 27,
+	RDC_PDAP_SEMAPHORE2 = 28,
+	RDC_PDAP_RDC = 29,
+	RDC_PDAP_CSU = 30,
+	RDC_PDAP_MU_3_B = 31,
+	RDC_PDAP_ISI = 32,
+	RDC_PDAP_ISP0 = 33,
+	RDC_PDAP_ISP1 = 34,
+	RDC_PDAP_IPS_Dewarp = 35,
+	RDC_PDAP_MIPI_CSI0 = 36,
+	RDC_PDAP_HSIOMIX_BLK_CTL = 37,
+	RDC_PDAP_PWM1 = 38,
+	RDC_PDAP_PWM2 = 39,
+	RDC_PDAP_PWM3 = 40,
+	RDC_PDAP_PWM4 = 41,
+	RDC_PDAP_System_Counter_RD = 42,
+	RDC_PDAP_System_Counter_CMP = 43,
+	RDC_PDAP_System_Counter_CTRL = 44,
+	RDC_PDAP_I2C5 = 45,
+	RDC_PDAP_GPT6 = 46,
+	RDC_PDAP_GPT5 = 47,
+	RDC_PDAP_GPT4 = 48,
+	RDC_PDAP_MIPI_CSI1 = 49,
+	RDC_PDAP_MIPI_DSI0 = 50,
+	RDC_PDAP_MEDIAMIX_BLK_CTL = 51,
+	RDC_PDAP_LCDIF1 = 52,
+	RDC_PDAP_eDMA_Management_Page = 53,
+	RDC_PDAP_eDMA_Channels_15_0 = 54,
+	RDC_PDAP_eDMA_Channels_31_16 = 55,
+	RDC_PDAP_TZASC = 56,
+	RDC_PDAP_I2C6 = 57,
+	RDC_PDAP_CAAM = 58,
+	RDC_PDAP_LCDIF2 = 59,
+	RDC_PDAP_PERFMON1 = 60,
+	RDC_PDAP_PERFMON2 = 61,
+	RDC_PDAP_NOC_BLK_CTL = 62,
+	RDC_PDAP_QoSC = 63,
+	RDC_PDAP_LVDS0 = 64,
+	RDC_PDAP_LVDS1 = 65,
+	RDC_PDAP_I2C1 = 66,
+	RDC_PDAP_I2C2 = 67,
+	RDC_PDAP_I2C3 = 68,
+	RDC_PDAP_I2C4 = 69,
+	RDC_PDAP_UART4 = 70,
+	RDC_PDAP_HDMI_TX = 71,
+	RDC_PDAP_IRQ_STEER_Audio_Processor = 72,
+	RDC_PDAP_SDMA2 = 73,
+	RDC_PDAP_MU_1_A = 74,
+	RDC_PDAP_MU_1_B = 75,
+	RDC_PDAP_SEMAPHORE_HS = 76,
+	RDC_PDAP_SAI1 = 78,
+	RDC_PDAP_SAI2 = 79,
+	RDC_PDAP_SAI3 = 80,
+	RDC_PDAP_CAN_FD1 = 81,
+	RDC_PDAP_SAI5 = 82,
+	RDC_PDAP_SAI6 = 83,
+	RDC_PDAP_uSDHC1 = 84,
+	RDC_PDAP_uSDHC2 = 85,
+	RDC_PDAP_uSDHC3 = 86,
+	RDC_PDAP_PCIE_PHY1 = 87,
+	RDC_PDAP_HDMI_TX_AUDLNK_MSTR = 88,
+	RDC_PDAP_CAN_FD2 = 89,
+	RDC_PDAP_SPBA2 = 90,
+	RDC_PDAP_QSPI = 91,
+	RDC_PDAP_AUDIO_BLK_CTRL = 92,
+	RDC_PDAP_SDMA1 = 93,
+	RDC_PDAP_ENET1 = 94,
+	RDC_PDAP_ENET2_TSN = 95,
+	RDC_PDAP_ASRC = 97,
+	RDC_PDAP_eCSPI1 = 98,
+	RDC_PDAP_eCSPI2 = 99,
+	RDC_PDAP_eCSPI3 = 100,
+	RDC_PDAP_SAI7 = 101,
+	RDC_PDAP_UART1 = 102,
+	RDC_PDAP_UART3 = 104,
+	RDC_PDAP_UART2 = 105,
+	RDC_PDAP_PDM_MICFIL = 106,
+	RDC_PDAP_AUDIO_XCVR_RX_eARC = 107,
+	RDC_PDAP_SDMA3 = 109,
+	RDC_PDAP_SPBA1 = 111,
+};
+
+enum csu_csl_idx {
+	CSU_CSL_GPIO1 = 0,
+	CSU_CSL_GPIO2 = 1,
+	CSU_CSL_GPIO3 = 2,
+	CSU_CSL_GPIO4 = 3,
+	CSU_CSL_GPIO5 = 4,
+	CSU_CSL_MU_2_A = 5,
+	CSU_CSL_ANA_TSENSOR = 6,
+	CSU_CSL_ANA_OSC = 7,
+	CSU_CSL_WDOG1 = 8,
+	CSU_CSL_WDOG2 = 9,
+	CSU_CSL_WDOG3 = 10,
+	CSU_CSL_GPT1 = 13,
+	CSU_CSL_GPT2 = 14,
+	CSU_CSL_GPT3 = 15,
+	CSU_CSL_MU_2_B = 16,
+	CSU_CSL_ROMCP = 17,
+	CSU_CSL_MU_3_A = 18,
+	CSU_CSL_IOMUXC = 19,
+	CSU_CSL_IOMUXC_GPR = 20,
+	CSU_CSL_OCOTP_CTRL = 21,
+	CSU_CSL_ANA_PLL = 22,
+	CSU_CSL_SNVS_HP = 23,
+	CSU_CSL_CCM = 24,
+	CSU_CSL_SRC = 25,
+	CSU_CSL_GPC = 26,
+	CSU_CSL_SEMAPHORE1 = 27,
+	CSU_CSL_SEMAPHORE2 = 28,
+	CSU_CSL_RDC = 29,
+	CSU_CSL_CSU = 30,
+	CSU_CSL_MU_3_B = 31,
+	CSU_CSL_ISI = 32,
+	CSU_CSL_ISP0 = 33,
+	CSU_CSL_ISP1 = 34,
+	CSU_CSL_IPS_Dewarp = 35,
+	CSU_CSL_MIPI_CSI0 = 36,
+	CSU_CSL_HSIOMIX_BLK_CTL	= 37,
+	CSU_CSL_PWM1 = 38,
+	CSU_CSL_PWM2 = 39,
+	CSU_CSL_PWM3 = 40,
+	CSU_CSL_PWM4 = 41,
+	CSU_CSL_System_Counter_RD = 42,
+	CSU_CSL_System_Counter_CMP = 43,
+	CSU_CSL_System_Counter_CTRL = 44,
+	CSU_CSL_I2C5 = 45,
+	CSU_CSL_GPT6 = 46,
+	CSU_CSL_GPT5 = 47,
+	CSU_CSL_GPT4 = 48,
+	CSU_CSL_MIPI_CSI1 = 49,
+	CSU_CSL_MIPI_DSI0 = 50,
+	CSU_CSL_MEDIAMIX_BLK_CTL = 51,
+	CSU_CSL_LCDIF1 = 52,
+	CSU_CSL_eDMA_Management_Page = 53,
+	CSU_CSL_eDMA_Channels_15_0 = 54,
+	CSU_CSL_eDMA_Channels_31_16 = 55,
+	CSU_CSL_TZASC = 56,
+	CSU_CSL_I2C6 = 57,
+	CSU_CSL_CAAM = 58,
+	CSU_CSL_LCDIF2 = 59,
+	CSU_CSL_PERFMON1 = 60,
+	CSU_CSL_PERFMON2 = 61,
+	CSU_CSL_NOC_BLK_CTL = 62,
+	CSU_CSL_QoSC = 63,
+	CSU_CSL_LVDS0 = 64,
+	CSU_CSL_LVDS1 = 65,
+	CSU_CSL_I2C1 = 66,
+	CSU_CSL_I2C2 = 67,
+	CSU_CSL_I2C3 = 68,
+	CSU_CSL_I2C4 = 69,
+	CSU_CSL_UART4 = 70,
+	CSU_CSL_HDMI_TX = 71,
+	CSU_CSL_IRQ_STEER_Audio_Processor = 72,
+	CSU_CSL_SDMA2 = 73,
+	CSU_CSL_MU_1_A = 74,
+	CSU_CSL_MU_1_B = 75,
+	CSU_CSL_SEMAPHORE_HS = 76,
+	CSU_CSL_SAI1 = 78,
+	CSU_CSL_SAI2 = 79,
+	CSU_CSL_SAI3 = 80,
+	CSU_CSL_CAN_FD1 = 81,
+	CSU_CSL_SAI5 = 82,
+	CSU_CSL_SAI6 = 83,
+	CSU_CSL_uSDHC1 = 84,
+	CSU_CSL_uSDHC2 = 85,
+	CSU_CSL_uSDHC3 = 86,
+	CSU_CSL_PCIE_PHY1 = 87,
+	CSU_CSL_HDMI_TX_AUDLNK_MSTR = 88,
+	CSU_CSL_CAN_FD2 = 89,
+	CSU_CSL_SPBA2 = 90,
+	CSU_CSL_QSPI = 91,
+	CSU_CSL_AUDIO_BLK_CTRL = 92,
+	CSU_CSL_SDMA1 = 93,
+	CSU_CSL_ENET1 = 94,
+	CSU_CSL_ENET2_TSN = 95,
+	CSU_CSL_ASRC = 97,
+	CSU_CSL_eCSPI1 = 98,
+	CSU_CSL_eCSPI2 = 99,
+	CSU_CSL_eCSPI3 = 100,
+	CSU_CSL_SAI7 = 101,
+	CSU_CSL_UART1 = 102,
+	CSU_CSL_UART3 = 104,
+	CSU_CSL_UART2 = 105,
+	CSU_CSL_PDM_MICFIL = 106,
+	CSU_CSL_AUDIO_XCVR_RX_eARC = 107,
+	CSU_CSL_SDMA3 = 109,
+	CSU_CSL_SPBA1 = 111,
+	CSU_CSL_OCRAM_A = 113,
+	CSU_CSL_OCRAM = 118,
+	CSU_CSL_OCRAM_S = 119,
+};
+
+#endif /* IMX_SEC_DEF_H */
diff --git a/plat/imx/imx8m/imx8mp/include/platform_def.h b/plat/imx/imx8m/imx8mp/include/platform_def.h
index 486c1ee..8807f5d 100644
--- a/plat/imx/imx8m/imx8mp/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mp/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2020 NXP
+ * Copyright 2020-2022 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -62,6 +62,8 @@
 #define PLAT_NS_IMAGE_OFFSET		U(0x40200000)
 #define PLAT_NS_IMAGE_SIZE		U(0x00200000)
 
+#define BL32_FDT_OVERLAY_ADDR		(PLAT_NS_IMAGE_OFFSET + 0x3000000)
+
 /* GICv3 base address */
 #define PLAT_GICD_BASE			U(0x38800000)
 #define PLAT_GICR_BASE			U(0x38880000)
diff --git a/plat/imx/imx8m/imx8mp/platform.mk b/plat/imx/imx8m/imx8mp/platform.mk
index 823b5d6..73fbd87 100644
--- a/plat/imx/imx8m/imx8mp/platform.mk
+++ b/plat/imx/imx8m/imx8mp/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright 2019-2020 NXP
+# Copyright 2019-2022 NXP
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -25,13 +25,12 @@
 				plat/imx/imx8m/imx_aipstz.c			\
 				plat/imx/imx8m/imx_rdc.c			\
 				plat/imx/imx8m/imx8m_caam.c			\
+				plat/imx/imx8m/imx8m_csu.c			\
 				plat/imx/imx8m/imx8m_psci_common.c		\
 				plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c	\
 				plat/imx/imx8m/imx8mp/imx8mp_psci.c		\
 				plat/imx/imx8m/imx8mp/gpc.c			\
 				plat/imx/common/imx8_topology.c			\
-				plat/imx/common/imx_ehf.c                       \
-				plat/imx/common/imx_sdei.c                      \
 				plat/imx/common/imx_sip_handler.c		\
 				plat/imx/common/imx_sip_svc.c			\
 				plat/imx/common/imx_uart_console.S		\
@@ -149,5 +148,12 @@
 IMX_BOOT_UART_BASE	?=	0x30890000
 $(eval $(call add_define,IMX_BOOT_UART_BASE))
 
-EL3_EXCEPTION_HANDLING := 1
-SDEI_SUPPORT := 1
+EL3_EXCEPTION_HANDLING := $(SDEI_SUPPORT)
+ifeq (${SDEI_SUPPORT}, 1)
+BL31_SOURCES 		+= 	plat/imx/common/imx_ehf.c	\
+				plat/imx/common/imx_sdei.c
+endif
+
+ifeq (${SPD},trusty)
+	BL31_CFLAGS    +=      -DPLAT_XLAT_TABLES_DYNAMIC=1
+endif
diff --git a/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c b/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c
index 05b5970..e998a16 100644
--- a/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,7 +18,7 @@
 #include <drivers/generic_delay_timer.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/mmio.h>
-#include <lib/xlat_tables/xlat_tables.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
 #include <gpc.h>
@@ -27,6 +27,8 @@
 #include <imx8m_caam.h>
 #include <plat_imx8.h>
 
+#define TRUSTY_PARAMS_LEN_BYTES      (4096*2)
+
 static const mmap_region_t imx_mmap[] = {
 	MAP_REGION_FLAT(GPV_BASE, GPV_SIZE, MT_DEVICE | MT_RW), /* GPV map */
 	MAP_REGION_FLAT(IMX_ROM_BASE, IMX_ROM_SIZE, MT_MEMORY | MT_RO), /* ROM map */
@@ -146,7 +148,7 @@
 	bl33_image_ep_info.spsr = get_spsr_for_bl33_entry();
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
 
-#ifdef SPD_opteed
+#if defined(SPD_opteed) || defined(SPD_trusty)
 	/* Populate entry point information for BL32 */
 	SET_PARAM_HEAD(&bl32_image_ep_info, PARAM_EP, VERSION_1, 0);
 	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
@@ -156,6 +158,16 @@
 	/* Pass TEE base and size to bl33 */
 	bl33_image_ep_info.args.arg1 = BL32_BASE;
 	bl33_image_ep_info.args.arg2 = BL32_SIZE;
+
+#ifdef SPD_trusty
+	bl32_image_ep_info.args.arg0 = BL32_SIZE;
+	bl32_image_ep_info.args.arg1 = BL32_BASE;
+#else
+	/* Make sure memory is clean */
+	mmio_write_32(BL32_FDT_OVERLAY_ADDR, 0);
+	bl33_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+	bl32_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+#endif
 #endif
 
 	bl31_tz380_setup();
@@ -168,6 +180,9 @@
 	mmap_add_region(BL_CODE_BASE, BL_CODE_BASE, (BL_CODE_END - BL_CODE_BASE),
 		MT_MEMORY | MT_RO | MT_SECURE);
 
+	/* Map TEE memory */
+	mmap_add_region(BL32_BASE, BL32_BASE, BL32_SIZE, MT_MEMORY | MT_RW);
+
 	mmap_add(imx_mmap);
 
 #if USE_COHERENT_MEM
@@ -215,3 +230,12 @@
 {
 	return;
 }
+
+#ifdef SPD_trusty
+void plat_trusty_set_boot_args(aapcs64_params_t *args)
+{
+	args->arg0 = BL32_SIZE;
+	args->arg1 = BL32_BASE;
+	args->arg2 = TRUSTY_PARAMS_LEN_BYTES;
+}
+#endif
diff --git a/plat/imx/imx8m/imx8mq/include/imx_sec_def.h b/plat/imx/imx8m/imx8mq/include/imx_sec_def.h
new file mode 100644
index 0000000..0f77141
--- /dev/null
+++ b/plat/imx/imx8m/imx8mq/include/imx_sec_def.h
@@ -0,0 +1,249 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_SEC_DEF_H
+#define IMX_SEC_DEF_H
+
+/* RDC MDA index */
+enum rdc_mda_idx {
+	RDC_MDA_A53 = 0,
+	RDC_MDA_M4 = 1,
+	RDC_MDA_PCIE_CTRL1 = 2,
+	RDC_MDA_PCIE_CTRL2 = 3,
+	RDC_MDA_VPU_DEC = 4,
+	RDC_MDA_LCDIF = 5,
+	RDC_MDA_CSI1 = 6,
+	RDC_MDA_CSI2 = 7,
+	RDC_MDA_Coresight = 8,
+	RDC_MDA_DAP = 9,
+	RDC_MDA_CAAM = 10,
+	RDC_MDA_SDMAp = 11,
+	RDC_MDA_SDMAb = 12,
+	RDC_MDA_APBHDMA = 13,
+	RDC_MDA_RAWNAND = 14,
+	RDC_MDA_uSDHC1 = 15,
+	RDC_MDA_uSDHC2 = 16,
+	RDC_MDA_DCSS = 17,
+	RDC_MDA_GPU = 18,
+	RDC_MDA_USB1 = 19,
+	RDC_MDA_USB2 = 20,
+	RDC_MDA_TESTPORT = 21,
+	RDC_MDA_ENET1_TX = 22,
+	RDC_MDA_ENET1_RX = 23,
+	RDC_MDA_SDMA2 = 24,
+	RDC_MDA_SDMA1 = 26,
+};
+
+/* RDC Peripherals index */
+enum rdc_pdap_idx {
+	RDC_PDAP_GPIO1 = 0,
+	RDC_PDAP_GPIO2 = 1,
+	RDC_PDAP_GPIO3 = 2,
+	RDC_PDAP_GPIO4 = 3,
+	RDC_PDAP_GPIO5 = 4,
+	RDC_PDAP_ANA_TSENSOR = 6,
+	RDC_PDAP_ANA_OSC = 7,
+	RDC_PDAP_WDOG1 = 8,
+	RDC_PDAP_WDOG2 = 9,
+	RDC_PDAP_WDOG3 = 10,
+	RDC_PDAP_SDMA2 = 12,
+	RDC_PDAP_GPT1 = 13,
+	RDC_PDAP_GPT2 = 14,
+	RDC_PDAP_GPT3 = 15,
+	RDC_PDAP_ROMCP = 17,
+	RDC_PDAP_LCDIF = 18,
+	RDC_PDAP_IOMUXC = 19,
+	RDC_PDAP_IOMUXC_GPR = 20,
+	RDC_PDAP_OCOTP_CTRL = 21,
+	RDC_PDAP_ANATOP_PLL = 22,
+	RDC_PDAP_SNVS_HP = 23,
+	RDC_PDAP_CCM = 24,
+	RDC_PDAP_SRC = 25,
+	RDC_PDAP_GPC = 26,
+	RDC_PDAP_SEMAPHORE1 = 27,
+	RDC_PDAP_SEMAPHORE2 = 28,
+	RDC_PDAP_RDC = 29,
+	RDC_PDAP_CSU = 30,
+	RDC_PDAP_MST0 = 32,
+	RDC_PDAP_MST1 = 33,
+	RDC_PDAP_MST2 = 34,
+	RDC_PDAP_MST3 = 35,
+	RDC_PDAP_HDMI_SEC = 36,
+	RDC_PDAP_PWM1 = 38,
+	RDC_PDAP_PWM2 = 39,
+	RDC_PDAP_PWM3 = 40,
+	RDC_PDAP_PWM4 = 41,
+	RDC_PDAP_SysCounter_RD = 42,
+	RDC_PDAP_SysCounter_CMP = 43,
+	RDC_PDAP_SysCounter_CTRL = 44,
+	RDC_PDAP_HDMI_CTRL = 45,
+	RDC_PDAP_GPT6 = 46,
+	RDC_PDAP_GPT5 = 47,
+	RDC_PDAP_GPT4 = 48,
+	RDC_PDAP_TZASC = 56,
+	RDC_PDAP_MTR = 59,
+	RDC_PDAP_PERFMON1 = 60,
+	RDC_PDAP_PERFMON2 = 61,
+	RDC_PDAP_PLATFORM_CTRL = 62,
+	RDC_PDAP_QoSC = 63,
+	RDC_PDAP_MIPI_PHY = 64,
+	RDC_PDAP_MIPI_DSI = 65,
+	RDC_PDAP_I2C1 = 66,
+	RDC_PDAP_I2C2 = 67,
+	RDC_PDAP_I2C3 = 68,
+	RDC_PDAP_I2C4 = 69,
+	RDC_PDAP_UART4 = 70,
+	RDC_PDAP_MIPI_CSI1 = 71,
+	RDC_PDAP_MIPI_CSI_PHY1 = 72,
+	RDC_PDAP_CSI1 = 73,
+	RDC_PDAP_MU_A = 74,
+	RDC_PDAP_MU_B = 75,
+	RDC_PDAP_SEMAPHORE_HS = 76,
+	RDC_PDAP_SAI1 = 78,
+	RDC_PDAP_SAI6 = 80,
+	RDC_PDAP_SAI5 = 81,
+	RDC_PDAP_SAI4 = 82,
+	RDC_PDAP_USDHC1 = 84,
+	RDC_PDAP_USDHC2 = 85,
+	RDC_PDAP_MIPI_CSI2 = 86,
+	RDC_PDAP_MIPI_CSI_PHY2 = 87,
+	RDC_PDAP_CSI2 = 88,
+	RDC_PDAP_QSPI = 91,
+	RDC_PDAP_SDMA1 = 93,
+	RDC_PDAP_ENET1 = 94,
+	RDC_PDAP_SPDIF1 = 97,
+	RDC_PDAP_ECSPI1 = 98,
+	RDC_PDAP_ECSPI2 = 99,
+	RDC_PDAP_ECSPI3 = 100,
+	RDC_PDAP_UART1 = 102,
+	RDC_PDAP_UART3 = 104,
+	RDC_PDAP_UART2 = 105,
+	RDC_PDAP_SPDIF2 = 106,
+	RDC_PDAP_SAI2 = 107,
+	RDC_PDAP_SAI3 = 108,
+	RDC_PDAP_SPBA1 = 111,
+	RDC_PDAP_CAAM = 114,
+	RDC_PDAP_DDRC_SEC = 115,
+	RDC_PDAP_GIC_EXSC = 116,
+	RDC_PDAP_USB_EXSC = 117,
+	RDC_PDAP_OCRAM_TZ = 118,
+	RDC_PDAP_OCRAM_S_TZ = 119,
+	RDC_PDAP_VPU_SEC = 120,
+	RDC_PDAP_DAP_EXSC = 121,
+	RDC_PDAP_ROMCP_SEC = 122,
+	RDC_PDAP_APBHDMA_SEC = 123,
+	RDC_PDAP_M4_SEC = 124,
+	RDC_PDAP_QSPI_SEC = 125,
+	RDC_PDAP_GPU_EXSC = 126,
+	RDC_PDAP_PCIE = 127,
+};
+
+enum csu_csl_idx {
+	CSU_CSL_GPIO1 = 0,
+	CSU_CSL_GPIO2 = 1,
+	CSU_CSL_GPIO3 = 2,
+	CSU_CSL_GPIO4 = 3,
+	CSU_CSL_GPIO5 = 4,
+	CSU_CSL_ANA_TSENSOR = 6,
+	CSU_CSL_ANA_OSC = 7,
+	CSU_CSL_WDOG1 = 8,
+	CSU_CSL_WDOG2 = 9,
+	CSU_CSL_WDOG3 = 10,
+	CSU_CSL_SDMA2 = 12,
+	CSU_CSL_GPT1 = 13,
+	CSU_CSL_GPT2 = 14,
+	CSU_CSL_GPT3 = 15,
+	CSU_CSL_ROMCP = 17,
+	CSU_CSL_LCDIF = 18,
+	CSU_CSL_IOMUXC = 19,
+	CSU_CSL_IOMUXC_GPR = 20,
+	CSU_CSL_OCOTP_CTRL = 21,
+	CSU_CSL_ANATOP_PLL = 22,
+	CSU_CSL_SNVS_HP = 23,
+	CSU_CSL_CCM = 24,
+	CSU_CSL_SRC = 25,
+	CSU_CSL_GPC = 26,
+	CSU_CSL_SEMAPHORE1 = 27,
+	CSU_CSL_SEMAPHORE2 = 28,
+	CSU_CSL_RDC = 29,
+	CSU_CSL_CSU = 30,
+	CSU_CSL_MST0 = 32,
+	CSU_CSL_MST1 = 33,
+	CSU_CSL_MST2 = 34,
+	CSU_CSL_MST3 = 35,
+	CSU_CSL_HDMI_SEC = 36,
+	CSU_CSL_PWM1 = 38,
+	CSU_CSL_PWM2 = 39,
+	CSU_CSL_PWM3 = 40,
+	CSU_CSL_PWM4 = 41,
+	CSU_CSL_SysCounter_RD = 42,
+	CSU_CSL_SysCounter_CMP = 43,
+	CSU_CSL_SysCounter_CTRL = 44,
+	CSU_CSL_HDMI_CTRL = 45,
+	CSU_CSL_GPT6 = 46,
+	CSU_CSL_GPT5 = 47,
+	CSU_CSL_GPT4 = 48,
+	CSU_CSL_TZASC = 56,
+	CSU_CSL_MTR = 59,
+	CSU_CSL_PERFMON1 = 60,
+	CSU_CSL_PERFMON2 = 61,
+	CSU_CSL_PLATFORM_CTRL = 62,
+	CSU_CSL_QoSC = 63,
+	CSU_CSL_MIPI_PHY = 64,
+	CSU_CSL_MIPI_DSI = 65,
+	CSU_CSL_I2C1 = 66,
+	CSU_CSL_I2C2 = 67,
+	CSU_CSL_I2C3 = 68,
+	CSU_CSL_I2C4 = 69,
+	CSU_CSL_UART4 = 70,
+	CSU_CSL_MIPI_CSI1 = 71,
+	CSU_CSL_MIPI_CSI_PHY1 = 72,
+	CSU_CSL_CSI1 = 73,
+	CSU_CSL_MU_A = 74,
+	CSU_CSL_MU_B = 75,
+	CSU_CSL_SEMAPHORE_HS = 76,
+	CSU_CSL_SAI1 = 78,
+	CSU_CSL_SAI6 = 80,
+	CSU_CSL_SAI5 = 81,
+	CSU_CSL_SAI4 = 82,
+	CSU_CSL_USDHC1 = 84,
+	CSU_CSL_USDHC2 = 85,
+	CSU_CSL_MIPI_CSI2 = 86,
+	CSU_CSL_MIPI_CSI_PHY2 = 87,
+	CSU_CSL_CSI2 = 88,
+	CSU_CSL_QSPI = 91,
+	CSU_CSL_SDMA1 = 93,
+	CSU_CSL_ENET1 = 94,
+	CSU_CSL_SPDIF1 = 97,
+	CSU_CSL_ECSPI1 = 98,
+	CSU_CSL_ECSPI2 = 99,
+	CSU_CSL_ECSPI3 = 100,
+	CSU_CSL_UART1 = 102,
+	CSU_CSL_UART3 = 104,
+	CSU_CSL_UART2 = 105,
+	CSU_CSL_SPDIF2 = 106,
+	CSU_CSL_SAI2 = 107,
+	CSU_CSL_SAI3 = 108,
+	CSU_CSL_SPBA1 = 111,
+	CSU_CSL_MOD_EN3 = 112,
+	CSU_CSL_MOD_EN0 = 113,
+	CSU_CSL_CAAM = 114,
+	CSU_CSL_DDRC_SEC = 115,
+	CSU_CSL_GIC_EXSC = 116,
+	CSU_CSL_USB_EXSC = 117,
+	CSU_CSL_OCRAM_TZ = 118,
+	CSU_CSL_OCRAM_S_TZ = 119,
+	CSU_CSL_VPU_SEC = 120,
+	CSU_CSL_DAP_EXSC = 121,
+	CSU_CSL_ROMCP_SEC = 122,
+	CSU_CSL_APBHDMA_SEC = 123,
+	CSU_CSL_M4_SEC = 124,
+	CSU_CSL_QSPI_SEC = 125,
+	CSU_CSL_GPU_EXSC = 126,
+	CSU_CSL_PCIE = 127,
+};
+
+#endif /* IMX_SEC_DEF_H */
diff --git a/plat/imx/imx8m/imx8mq/include/platform_def.h b/plat/imx/imx8m/imx8mq/include/platform_def.h
index 6d6a865..a76e895 100644
--- a/plat/imx/imx8m/imx8mq/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mq/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -35,6 +35,7 @@
 
 /* non-secure uboot base */
 #define PLAT_NS_IMAGE_OFFSET		U(0x40200000)
+#define BL32_FDT_OVERLAY_ADDR		(PLAT_NS_IMAGE_OFFSET + 0x3000000)
 
 /* GICv3 base address */
 #define PLAT_GICD_BASE			U(0x38800000)
@@ -43,8 +44,13 @@
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(1ull << 32)
 #define PLAT_PHY_ADDR_SPACE_SIZE	(1ull << 32)
 
+#ifdef SPD_trusty
+#define MAX_XLAT_TABLES			5
+#define MAX_MMAP_REGIONS		15
+#else
 #define MAX_XLAT_TABLES			4
 #define MAX_MMAP_REGIONS		14
+#endif
 
 #define HAB_RVT_BASE			U(0x00000880) /* HAB_RVT for i.MX8MQ */
 
diff --git a/plat/imx/imx8m/imx8mq/platform.mk b/plat/imx/imx8m/imx8mq/platform.mk
index 5461010..7b6df92 100644
--- a/plat/imx/imx8m/imx8mq/platform.mk
+++ b/plat/imx/imx8m/imx8mq/platform.mk
@@ -1,9 +1,12 @@
 #
-# Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+# Translation tables library
+include lib/xlat_tables_v2/xlat_tables.mk
+
 PLAT_INCLUDES		:=	-Iplat/imx/common/include		\
 				-Iplat/imx/imx8m/include		\
 				-Iplat/imx/imx8m/imx8mq/include
@@ -28,12 +31,11 @@
 				plat/imx/common/imx_sip_handler.c		\
 				plat/imx/common/imx_sip_svc.c			\
 				plat/imx/common/imx_uart_console.S		\
-				lib/xlat_tables/aarch64/xlat_tables.c		\
-				lib/xlat_tables/xlat_tables_common.c		\
 				lib/cpus/aarch64/cortex_a53.S			\
 				drivers/arm/tzc/tzc380.c			\
 				drivers/delay_timer/delay_timer.c		\
 				drivers/delay_timer/generic_delay_timer.c	\
+				${XLAT_TABLES_LIB_SRCS}				\
 				${IMX_GIC_SOURCES}
 
 USE_COHERENT_MEM	:=	1
@@ -49,3 +51,7 @@
 
 BL32_SIZE		?=	0x2000000
 $(eval $(call add_define,BL32_SIZE))
+
+ifeq (${SPD},trusty)
+	BL31_CFLAGS    +=      -DPLAT_XLAT_TABLES_DYNAMIC=1
+endif
diff --git a/plat/imx/imx8m/include/imx8m_csu.h b/plat/imx/imx8m/include/imx8m_csu.h
new file mode 100644
index 0000000..dc634ed
--- /dev/null
+++ b/plat/imx/imx8m/include/imx8m_csu.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_CSU_H
+#define IMX_CSU_H
+
+#include <lib/utils_def.h>
+
+#include <platform_def.h>
+
+#define CSU_SEC_LEVEL_0		0xff
+#define CSU_SEC_LEVEL_1		0xbb
+#define CSU_SEC_LEVEL_2		0x3f
+#define CSU_SEC_LEVEL_3		0x3b
+#define CSU_SEC_LEVEL_4		0x33
+#define CSU_SEC_LEVEL_5		0x22
+#define CSU_SEC_LEVEL_6		0x03
+#define CSU_SEC_LEVEL_7		0x0
+
+#define LOCKED			0x1
+#define UNLOCKED		0x0
+
+#define CSLx_REG(x)		(IMX_CSU_BASE + ((x) / 2) * 4)
+#define CSLx_LOCK(x)		((0x1 << (((x) % 2) * 16 + 8)))
+#define CSLx_CFG(x, n)		((x) << (((n) % 2) * 16))
+
+#define CSU_HP_REG(x)		(IMX_CSU_BASE + ((x) / 16) * 4 + 0x200)
+#define CSU_HP_LOCK(x)		((0x1 << (((x) % 16) * 2 + 1)))
+#define CSU_HP_CFG(x, n)	((x) << (((n) % 16) * 2))
+
+#define CSU_SA_REG(x)		(IMX_CSU_BASE + 0x218)
+#define CSU_SA_LOCK(x)		((0x1 << (((x) % 16) * 2 + 1)))
+#define CSU_SA_CFG(x, n)	((x) << (((n) % 16) * 2))
+
+#define CSU_HPCONTROL_REG(x)		(IMX_CSU_BASE + (((x) / 16) * 4) + 0x358)
+#define CSU_HPCONTROL_LOCK(x)		((0x1 << (((x) % 16) * 2 + 1)))
+#define CSU_HPCONTROL_CFG(x, n)		((x) << (((n) % 16) * 2))
+
+enum csu_cfg_type {
+	CSU_INVALID,
+	CSU_CSL,
+	CSU_HP,
+	CSU_SA,
+	CSU_HPCONTROL,
+};
+
+struct imx_csu_cfg {
+	enum csu_cfg_type type;
+	uint16_t idx;
+	uint16_t lock : 1;
+	uint16_t csl_level : 8;
+	uint16_t hp : 1;
+	uint16_t sa : 1;
+	uint16_t hpctrl : 1;
+};
+
+#define CSU_CSLx(i, level, lk)	\
+	{CSU_CSL, .idx = (i), .csl_level = (level), .lock = (lk),}
+
+#define CSU_HPx(i, val, lk)	\
+	{CSU_HP, .idx = (i), .hp = (val), .lock = (lk), }
+
+#define CSU_SA(i, val, lk)	\
+	{CSU_SA, .idx = (i), .sa = (val), .lock = (lk), }
+
+#define CSU_HPCTRL(i, val, lk)	\
+	{CSU_HPCONTROL, .idx = (i), .hpctrl = (val), .lock = (lk), }
+
+void imx_csu_init(const struct imx_csu_cfg *csu_cfg);
+
+#endif /* IMX_CSU_H */
diff --git a/plat/imx/imx8m/include/imx_rdc.h b/plat/imx/imx8m/include/imx_rdc.h
index e25b0e6..a6e10a7 100644
--- a/plat/imx/imx8m/include/imx_rdc.h
+++ b/plat/imx/imx8m/include/imx_rdc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, NXP. All rights reserved.
+ * Copyright (c) 2019-2022 NXP. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,7 @@
 
 #include <lib/utils_def.h>
 
+#include <imx_sec_def.h>
 #include <platform_def.h>
 
 #define MDAn(x)		(IMX_RDC_BASE + 0x200 + (x) * 4)
diff --git a/plat/imx/imx8qm/imx8qm_bl31_setup.c b/plat/imx/imx8qm/imx8qm_bl31_setup.c
index d9c9110..68eb534 100644
--- a/plat/imx/imx8qm/imx8qm_bl31_setup.c
+++ b/plat/imx/imx8qm/imx8qm_bl31_setup.c
@@ -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
  */
@@ -19,7 +19,7 @@
 #include <drivers/console.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/mmio.h>
-#include <lib/xlat_tables/xlat_tables.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
 #include <imx8qm_pads.h>
diff --git a/plat/imx/imx8qm/platform.mk b/plat/imx/imx8qm/platform.mk
index f35fa00..c57edbe 100644
--- a/plat/imx/imx8qm/platform.mk
+++ b/plat/imx/imx8qm/platform.mk
@@ -1,9 +1,12 @@
 #
-# 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
 #
 
+# Translation tables library
+include lib/xlat_tables_v2/xlat_tables.mk
+
 PLAT_INCLUDES		:=	-Iplat/imx/imx8qm/include		\
 				-Iplat/imx/common/include		\
 
@@ -23,11 +26,10 @@
 				plat/imx/common/imx8_psci.c		\
 				plat/imx/common/imx_sip_svc.c		\
 				plat/imx/common/imx_sip_handler.c	\
-				lib/xlat_tables/aarch64/xlat_tables.c		\
-				lib/xlat_tables/xlat_tables_common.c		\
 				lib/cpus/aarch64/cortex_a53.S			\
 				lib/cpus/aarch64/cortex_a72.S			\
 				drivers/arm/cci/cci.c				\
+				${XLAT_TABLES_LIB_SRCS}				\
 				${IMX_GIC_SOURCES}				\
 
 include plat/imx/common/sci/sci_api.mk
diff --git a/plat/imx/imx8qx/imx8qx_bl31_setup.c b/plat/imx/imx8qx/imx8qx_bl31_setup.c
index 3739cd6..1da8d29 100644
--- a/plat/imx/imx8qx/imx8qx_bl31_setup.c
+++ b/plat/imx/imx8qx/imx8qx_bl31_setup.c
@@ -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
  */
@@ -19,7 +19,7 @@
 #include <drivers/console.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/mmio.h>
-#include <lib/xlat_tables/xlat_tables.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
 #include <imx8qx_pads.h>
diff --git a/plat/imx/imx8qx/platform.mk b/plat/imx/imx8qx/platform.mk
index b25be07..85b5f3d 100644
--- a/plat/imx/imx8qx/platform.mk
+++ b/plat/imx/imx8qx/platform.mk
@@ -1,9 +1,12 @@
 #
-# 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
 #
 
+# Translation tables library
+include lib/xlat_tables_v2/xlat_tables.mk
+
 PLAT_INCLUDES		:=	-Iplat/imx/imx8qx/include		\
 				-Iplat/imx/common/include		\
 
@@ -23,9 +26,8 @@
 				plat/imx/common/imx_sip_svc.c		\
 				plat/imx/common/imx_sip_handler.c	\
 				plat/common/plat_psci_common.c		\
-				lib/xlat_tables/xlat_tables_common.c	\
-				lib/xlat_tables/aarch64/xlat_tables.c	\
 				lib/cpus/aarch64/cortex_a35.S		\
+				${XLAT_TABLES_LIB_SRCS}			\
 				${IMX_GIC_SOURCES}			\
 
 include plat/imx/common/sci/sci_api.mk
diff --git a/plat/intel/soc/agilex/include/agilex_clock_manager.h b/plat/intel/soc/agilex/include/agilex_clock_manager.h
index 20667f0..f39d475 100644
--- a/plat/intel/soc/agilex/include/agilex_clock_manager.h
+++ b/plat/intel/soc/agilex/include/agilex_clock_manager.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/intel/soc/agilex/include/socfpga_plat_def.h b/plat/intel/soc/agilex/include/socfpga_plat_def.h
index 6a5cf9b..499684d 100644
--- a/plat/intel/soc/agilex/include/socfpga_plat_def.h
+++ b/plat/intel/soc/agilex/include/socfpga_plat_def.h
@@ -31,5 +31,9 @@
 #define SOCFPGA_SOC2FPGA_SCR_REG_BASE           0xffd21200
 #define SOCFPGA_LWSOC2FPGA_SCR_REG_BASE         0xffd21300
 
-#endif /* PLAT_SOCFPGA_DEF_H */
+/* Platform specific system counter */
+#define PLAT_SYS_COUNTER_FREQ_IN_MHZ	get_cpu_clk()
 
+uint32_t get_cpu_clk(void);
+
+#endif /* PLAT_SOCFPGA_DEF_H */
diff --git a/plat/intel/soc/agilex/platform.mk b/plat/intel/soc/agilex/platform.mk
index 89df46a..0e5f911 100644
--- a/plat/intel/soc/agilex/platform.mk
+++ b/plat/intel/soc/agilex/platform.mk
@@ -45,7 +45,7 @@
 		plat/intel/soc/agilex/soc/agilex_memory_controller.c	\
 		plat/intel/soc/agilex/soc/agilex_mmc.c			\
 		plat/intel/soc/agilex/soc/agilex_pinmux.c		\
-                plat/intel/soc/common/bl2_plat_mem_params_desc.c	\
+		plat/intel/soc/common/bl2_plat_mem_params_desc.c	\
 		plat/intel/soc/common/socfpga_image_load.c		\
 		plat/intel/soc/common/socfpga_storage.c			\
 		plat/intel/soc/common/soc/socfpga_emac.c		\
@@ -62,6 +62,7 @@
 		lib/cpus/aarch64/cortex_a53.S				\
 		plat/common/plat_psci_common.c				\
 		plat/intel/soc/agilex/bl31_plat_setup.c 		\
+		plat/intel/soc/agilex/soc/agilex_clock_manager.c	\
 		plat/intel/soc/common/socfpga_psci.c			\
 		plat/intel/soc/common/socfpga_sip_svc.c			\
 		plat/intel/soc/common/socfpga_topology.c		\
diff --git a/plat/intel/soc/agilex/soc/agilex_clock_manager.c b/plat/intel/soc/agilex/soc/agilex_clock_manager.c
index 4efd713..76b9937 100644
--- a/plat/intel/soc/agilex/soc/agilex_clock_manager.c
+++ b/plat/intel/soc/agilex/soc/agilex_clock_manager.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -387,3 +387,13 @@
 
 	return mmc_clk;
 }
+
+/* Get cpu freq clock */
+uint32_t get_cpu_clk(void)
+{
+	uint32_t cpu_clk;
+
+	cpu_clk = get_l3_clk()/PLAT_SYS_COUNTER_CONVERT_TO_MHZ;
+
+	return cpu_clk;
+}
diff --git a/plat/intel/soc/common/include/platform_def.h b/plat/intel/soc/common/include/platform_def.h
index d37904b..a31adf7 100644
--- a/plat/intel/soc/common/include/platform_def.h
+++ b/plat/intel/soc/common/include/platform_def.h
@@ -192,7 +192,7 @@
  * System counter frequency related constants
  ******************************************************************************/
 #define PLAT_SYS_COUNTER_FREQ_IN_TICKS	(400000000)
-#define PLAT_SYS_COUNTER_FREQ_IN_MHZ	(400)
+#define PLAT_SYS_COUNTER_CONVERT_TO_MHZ	(1000000)
 
 #define PLAT_INTEL_SOCFPGA_GICD_BASE	PLAT_GICD_BASE
 #define PLAT_INTEL_SOCFPGA_GICC_BASE	PLAT_GICC_BASE
diff --git a/plat/intel/soc/common/include/socfpga_private.h b/plat/intel/soc/common/include/socfpga_private.h
index ca38f62..9d389e3 100644
--- a/plat/intel/soc/common/include/socfpga_private.h
+++ b/plat/intel/soc/common/include/socfpga_private.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -55,6 +55,8 @@
 
 void socfpga_gic_driver_init(void);
 
+void socfpga_delay_timer_init_args(void);
+
 uint32_t socfpga_get_spsr_for_bl32_entry(void);
 
 uint32_t socfpga_get_spsr_for_bl33_entry(void);
diff --git a/plat/intel/soc/common/sip/socfpga_sip_fcs.c b/plat/intel/soc/common/sip/socfpga_sip_fcs.c
index 3a7d693..85551a4 100644
--- a/plat/intel/soc/common/sip/socfpga_sip_fcs.c
+++ b/plat/intel/soc/common/sip/socfpga_sip_fcs.c
@@ -11,6 +11,15 @@
 #include "socfpga_mailbox.h"
 #include "socfpga_sip_svc.h"
 
+static bool is_size_4_bytes_aligned(uint32_t size)
+{
+	if ((size % MBOX_WORD_BYTE) != 0U) {
+		return false;
+	} else {
+		return true;
+	}
+}
+
 uint32_t intel_fcs_random_number_gen(uint64_t addr, uint64_t *ret_size,
 					uint32_t *mbox_error)
 {
@@ -57,6 +66,10 @@
 		return INTEL_SIP_SMC_STATUS_REJECTED;
 	}
 
+	if (!is_size_4_bytes_aligned(size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
 	status = mailbox_send_cmd_async(send_id, MBOX_CMD_VAB_SRC_CERT,
 				(uint32_t *)addr, size / MBOX_WORD_BYTE,
 				CMD_DIRECT);
@@ -89,11 +102,6 @@
 	int status;
 	uint32_t cmd;
 
-	if (!is_address_in_ddr_range(src_addr, src_size) ||
-		!is_address_in_ddr_range(dst_addr, dst_size)) {
-		return INTEL_SIP_SMC_STATUS_REJECTED;
-	}
-
 	fcs_crypt_payload payload = {
 		FCS_CRYPTION_DATA_0,
 		src_addr,
@@ -101,6 +109,15 @@
 		dst_addr,
 		dst_size };
 
+	if (!is_address_in_ddr_range(src_addr, src_size) ||
+		!is_address_in_ddr_range(dst_addr, dst_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_size_4_bytes_aligned(sizeof(fcs_crypt_payload))) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
 	if (mode != 0U) {
 		cmd = MBOX_FCS_ENCRYPT_REQ;
 	} else {
diff --git a/plat/intel/soc/common/socfpga_delay_timer.c b/plat/intel/soc/common/socfpga_delay_timer.c
index c55cc9d..957738c 100644
--- a/plat/intel/soc/common/socfpga_delay_timer.c
+++ b/plat/intel/soc/common/socfpga_delay_timer.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -8,10 +8,12 @@
 #include <arch_helpers.h>
 #include <drivers/delay_timer.h>
 #include <lib/mmio.h>
+#include "socfpga_plat_def.h"
 
 #define SOCFPGA_GLOBAL_TIMER		0xffd01000
 #define SOCFPGA_GLOBAL_TIMER_EN		0x3
 
+static timer_ops_t plat_timer_ops;
 /********************************************************************
  * The timer delay function
  ********************************************************************/
@@ -26,15 +28,20 @@
 	return (uint32_t)(~read_cntpct_el0());
 }
 
-static const timer_ops_t plat_timer_ops = {
-	.get_timer_value    = socfpga_get_timer_value,
-	.clk_mult           = 1,
-	.clk_div	    = PLAT_SYS_COUNTER_FREQ_IN_MHZ,
-};
+void socfpga_delay_timer_init_args(void)
+{
+	plat_timer_ops.get_timer_value	= socfpga_get_timer_value;
+	plat_timer_ops.clk_mult		= 1;
+	plat_timer_ops.clk_div		= PLAT_SYS_COUNTER_FREQ_IN_MHZ;
+
+	timer_init(&plat_timer_ops);
+
+	NOTICE("BL31: MPU clock frequency: %d MHz\n", plat_timer_ops.clk_div);
+}
 
 void socfpga_delay_timer_init(void)
 {
-	timer_init(&plat_timer_ops);
+	socfpga_delay_timer_init_args();
 	mmio_write_32(SOCFPGA_GLOBAL_TIMER, SOCFPGA_GLOBAL_TIMER_EN);
 
 	asm volatile("msr cntp_ctl_el0, %0" : : "r" (SOCFPGA_GLOBAL_TIMER_EN));
diff --git a/plat/intel/soc/n5x/include/socfpga_plat_def.h b/plat/intel/soc/n5x/include/socfpga_plat_def.h
index 9186852..3ce03dc 100644
--- a/plat/intel/soc/n5x/include/socfpga_plat_def.h
+++ b/plat/intel/soc/n5x/include/socfpga_plat_def.h
@@ -29,4 +29,11 @@
 #define SOCFPGA_SOC2FPGA_SCR_REG_BASE			U(0xffd21200)
 #define SOCFPGA_LWSOC2FPGA_SCR_REG_BASE			U(0xffd21300)
 
+/* Platform specific system counter */
+/*
+ * In N5X the clk init is done in Uboot SPL.
+ * BL31 shall bypass the clk init and only provides other APIs.
+ */
+#define PLAT_SYS_COUNTER_FREQ_IN_MHZ	(400)
+
 #endif /* PLAT_SOCFPGA_DEF_H */
diff --git a/plat/intel/soc/stratix10/bl2_plat_setup.c b/plat/intel/soc/stratix10/bl2_plat_setup.c
index faff898..cca564a 100644
--- a/plat/intel/soc/stratix10/bl2_plat_setup.c
+++ b/plat/intel/soc/stratix10/bl2_plat_setup.c
@@ -25,6 +25,7 @@
 #include "socfpga_system_manager.h"
 #include "s10_clock_manager.h"
 #include "s10_memory_controller.h"
+#include "s10_mmc.h"
 #include "s10_pinmux.h"
 #include "wdt/watchdog.h"
 
@@ -76,6 +77,7 @@
 	socfpga_delay_timer_init();
 	init_hard_memory_controller();
 	mailbox_init();
+	s10_mmc_init();
 
 	if (!intel_mailbox_is_fpga_not_ready())
 		socfpga_bridges_enable();
diff --git a/plat/intel/soc/stratix10/include/s10_clock_manager.h b/plat/intel/soc/stratix10/include/s10_clock_manager.h
index acc700a..cf57df3 100644
--- a/plat/intel/soc/stratix10/include/s10_clock_manager.h
+++ b/plat/intel/soc/stratix10/include/s10_clock_manager.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -60,6 +60,7 @@
 
 #define ALT_CLKMGR_PERPLL			0xffd100a4
 #define ALT_CLKMGR_PERPLL_EN			0x0
+#define ALT_CLKMGR_PERPLL_EN_SDMMCCLK		BIT(5)
 #define ALT_CLKMGR_PERPLL_BYPASS		0xc
 #define ALT_CLKMGR_PERPLL_CNTR2CLK		0x18
 #define ALT_CLKMGR_PERPLL_CNTR3CLK		0x1c
@@ -92,5 +93,7 @@
 uint32_t get_wdt_clk(void);
 uint32_t get_uart_clk(void);
 uint32_t get_mmc_clk(void);
+uint32_t get_l3_clk(uint32_t ref_clk);
+uint32_t get_ref_clk(uint32_t pllglob);
 
 #endif
diff --git a/plat/intel/soc/stratix10/include/s10_mmc.h b/plat/intel/soc/stratix10/include/s10_mmc.h
new file mode 100644
index 0000000..99f86f5
--- /dev/null
+++ b/plat/intel/soc/stratix10/include/s10_mmc.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2022, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef __S10_MMC_H__
+#define __S10_MMC_H__
+
+void s10_mmc_init(void);
+
+#endif /* S10_MMC_H */
diff --git a/plat/intel/soc/stratix10/include/socfpga_plat_def.h b/plat/intel/soc/stratix10/include/socfpga_plat_def.h
index 2defeb9..ae4b674 100644
--- a/plat/intel/soc/stratix10/include/socfpga_plat_def.h
+++ b/plat/intel/soc/stratix10/include/socfpga_plat_def.h
@@ -30,6 +30,10 @@
 #define SOCFPGA_SOC2FPGA_SCR_REG_BASE		0xffd21200
 #define SOCFPGA_LWSOC2FPGA_SCR_REG_BASE		0xffd21300
 
+/* Platform specific system counter */
+#define PLAT_SYS_COUNTER_FREQ_IN_MHZ	get_cpu_clk()
+
+uint32_t get_cpu_clk(void);
 
 #endif /* PLATSOCFPGA_DEF_H */
 
diff --git a/plat/intel/soc/stratix10/platform.mk b/plat/intel/soc/stratix10/platform.mk
index b7808ae..273b975 100644
--- a/plat/intel/soc/stratix10/platform.mk
+++ b/plat/intel/soc/stratix10/platform.mk
@@ -43,8 +43,9 @@
 		plat/intel/soc/stratix10/bl2_plat_setup.c		\
 		plat/intel/soc/stratix10/soc/s10_clock_manager.c	\
 		plat/intel/soc/stratix10/soc/s10_memory_controller.c	\
+		plat/intel/soc/stratix10/soc/s10_mmc.c			\
 		plat/intel/soc/stratix10/soc/s10_pinmux.c		\
-                plat/intel/soc/common/bl2_plat_mem_params_desc.c	\
+		plat/intel/soc/common/bl2_plat_mem_params_desc.c	\
 		plat/intel/soc/common/socfpga_image_load.c		\
 		plat/intel/soc/common/socfpga_storage.c			\
 		plat/intel/soc/common/soc/socfpga_emac.c		\
@@ -59,6 +60,7 @@
 		lib/cpus/aarch64/aem_generic.S				\
 		lib/cpus/aarch64/cortex_a53.S				\
 		plat/common/plat_psci_common.c				\
+		plat/intel/soc/stratix10/soc/s10_clock_manager.c	\
 		plat/intel/soc/stratix10/bl31_plat_setup.c	 	\
 		plat/intel/soc/common/socfpga_psci.c			\
 		plat/intel/soc/common/socfpga_sip_svc.c			\
diff --git a/plat/intel/soc/stratix10/soc/s10_clock_manager.c b/plat/intel/soc/stratix10/soc/s10_clock_manager.c
index 1e092de..30009f7 100644
--- a/plat/intel/soc/stratix10/soc/s10_clock_manager.c
+++ b/plat/intel/soc/stratix10/soc/s10_clock_manager.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -307,3 +307,16 @@
 
 	return mmc_clk;
 }
+
+/* Get cpu freq clock */
+uint32_t get_cpu_clk(void)
+{
+	uint32_t data32, ref_clk, cpu_clk;
+
+	data32 = mmio_read_32(ALT_CLKMGR_MAINPLL + ALT_CLKMGR_MAINPLL_PLLGLOB);
+	ref_clk = get_ref_clk(data32);
+
+	cpu_clk = get_l3_clk(ref_clk)/PLAT_SYS_COUNTER_CONVERT_TO_MHZ;
+
+	return cpu_clk;
+}
diff --git a/plat/intel/soc/stratix10/soc/s10_mmc.c b/plat/intel/soc/stratix10/soc/s10_mmc.c
new file mode 100644
index 0000000..333bdd6
--- /dev/null
+++ b/plat/intel/soc/stratix10/soc/s10_mmc.c
@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2022, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <lib/mmio.h>
+
+#include "s10_clock_manager.h"
+#include "socfpga_system_manager.h"
+
+void s10_mmc_init(void)
+{
+	mmio_clrbits_32(ALT_CLKMGR_PERPLL + ALT_CLKMGR_PERPLL_EN,
+		ALT_CLKMGR_PERPLL_EN_SDMMCCLK);
+	mmio_write_32(SOCFPGA_SYSMGR(SDMMC),
+		SYSMGR_SDMMC_SMPLSEL(2) | SYSMGR_SDMMC_DRVSEL(3));
+	mmio_setbits_32(ALT_CLKMGR_PERPLL + ALT_CLKMGR_PERPLL_EN,
+		ALT_CLKMGR_PERPLL_EN_SDMMCCLK);
+}
diff --git a/plat/mediatek/mt6795/aarch64/plat_helpers.S b/plat/mediatek/mt6795/aarch64/plat_helpers.S
deleted file mode 100644
index aaddb2b..0000000
--- a/plat/mediatek/mt6795/aarch64/plat_helpers.S
+++ /dev/null
@@ -1,148 +0,0 @@
-/*
- * Copyright (c) 2016-2020, 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_secondary_cold_boot_setup
-	.globl	plat_report_exception
-	.globl	platform_is_primary_cpu
-	.globl	plat_crash_console_init
-	.globl	plat_crash_console_putc
-	.globl	plat_crash_console_flush
-	.globl	platform_mem_init
-
-
-	.macro crash_ram_log
-	 /*
-	 * Check teearg->atf_log_buf_size.
-	 * Exit if atf_log_buf_size equals 0
-	 */
-	adr	x2, ptr_atf_crash_flag
-	ldr	x2, [x2]
-	/* exit if ptr_atf_crash_flag equals NULL */
-	cbz x2, exit_putc
-
-	/*
-	 * set atf crash magic number
-	 */
-1:
-	adr	x2, ptr_atf_crash_flag
-	ldr	x2, [x2]
-	mov_imm x1, 0xdead1abf
-	/* p_atf_log_ctrl->atf_crash_flag = 0xdead1abf */
-	str	w1, [x2]
-	/* can't use w3 return addr, w4, start of buffer addr */
-	ldr	w2, [x2]
-	cmp	w2, w1
-	b.ne	1b
-
-	/*
-	 * get cpu id
-	 */
-	mrs	x1, mpidr_el1
-	/* refer to platform_get_core_pos */
-	and	x2, x1, #MPIDR_CPU_MASK
-	and	x1, x1, #MPIDR_CLUSTER_MASK
-	/* x1 = cpu id (cpu id = aff0 + aff1*4 ) */
-	add	x1, x2, x1, LSR #6
-
-	adr	x2, ptr_atf_except_write_pos_per_cpu
-	ldr	x2, [x2]
-	/*
-	 * plus (cpu_id * 8)-->
-	 * &p_atf_log_ctrl->atf_except_write_pos_per_cpu[cpu_id]
-	 * x2 = &p_atf_log_ctrl->atf_except_write_pos_per_cpu[cpu_id];
-	 */
-	add x2, x2, x1, LSL # 3
-	/* log write */
-	/* w1 = p_atf_log_ctrl->atf_except_write_pos_per_cpu[cpu_id] */
-	ldr	x1, [x2]
-	/* *x1 = w0-->
-	 *  *(p_atf_log_ctrl->atf_except_write_pos_per_cpu[cpu_id]) = c)
-	 */
-	strb	w0, [x1]
-	/* w1++ */
-	add	x1, x1, #1
-	/* p_atf_log_ctrl->atf_except_write_pos_per_cpu[cpu_id] = w1 */
-	str	x1, [x2]
-exit_putc:
-	.endm
-
-	/* -----------------------------------------------------
-	 * void plat_secondary_cold_boot_setup (void);
-	 *
-	 * This function performs any platform specific actions
-	 * needed for a secondary cpu after a cold reset e.g
-	 * mark the cpu's presence, mechanism to place it in a
-	 * holding pen etc.
-	 * -----------------------------------------------------
-	 */
-func plat_secondary_cold_boot_setup
-	/* Do not do cold boot for secondary CPU */
-cb_panic:
-	b	cb_panic
-endfunc plat_secondary_cold_boot_setup
-
-func platform_is_primary_cpu
-	and	x0, x0, #(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK)
-	cmp	x0, #PLAT_PRIMARY_CPU
-	cset	x0, eq
-	ret
-endfunc platform_is_primary_cpu
-
-	/* ---------------------------------------------
-	 * int plat_crash_console_init(void)
-	 * Function to initialize the crash console
-	 * without a C Runtime to print crash report.
-	 * Clobber list : x0, x1, x2
-	 * ---------------------------------------------
-	 */
-func plat_crash_console_init
-	mov_imm	x0, UART0_BASE
-	mov_imm	x1, UART_CLOCK
-	mov_imm	x2, UART_BAUDRATE
-	b	console_init
-	ret
-endfunc plat_crash_console_init
-
-	/* ---------------------------------------------
-	 * int plat_crash_console_putc(void)
-	 * Function to print a character on the crash
-	 * console without a C Runtime.
-	 * Clobber list : x1, x2
-	 * ---------------------------------------------
-	 */
-func plat_crash_console_putc
-	mov_imm x1, UART0_BASE
-	b	console_core_putc
-	ret
-endfunc plat_crash_console_putc
-
-	/* ---------------------------------------------
-	 * void plat_crash_console_flush(int c)
-	 * Function to force a write of all buffered
-	 * data that hasn't been output.
-	 * Out : void.
-	 * Clobber list : x0, x1
-	 * ---------------------------------------------
-	 */
-func plat_crash_console_flush
-	mov_imm	x0, UART0_BASE
-	b	console_core_flush
-endfunc plat_crash_console_flush
-
-	/* --------------------------------------------------------
-	 * void platform_mem_init (void);
-	 *
-	 * Any memory init, relocation to be done before the
-	 * platform boots. Called very early in the boot process.
-	 * --------------------------------------------------------
-	 */
-func platform_mem_init
-	ret
-endfunc platform_mem_init
-
diff --git a/plat/mediatek/mt6795/bl31.ld.S b/plat/mediatek/mt6795/bl31.ld.S
deleted file mode 100644
index 3d881fc..0000000
--- a/plat/mediatek/mt6795/bl31.ld.S
+++ /dev/null
@@ -1,116 +0,0 @@
-/*
- * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <common/bl_common.ld.h>
-#include <lib/xlat_tables/xlat_tables_defs.h>
-
-OUTPUT_FORMAT(PLATFORM_LINKER_FORMAT)
-OUTPUT_ARCH(PLATFORM_LINKER_ARCH)
-ENTRY(bl31_entrypoint)
-
-
-MEMORY {
-	RAM (rwx): ORIGIN = BL31_BASE, LENGTH = BL31_TZRAM_SIZE
-	RAM2 (rwx): ORIGIN = TZRAM2_BASE, LENGTH = TZRAM2_SIZE
-}
-
-
-SECTIONS
-{
-    . = BL31_BASE;
-
-    ASSERT(. == ALIGN(2048),
-           "vector base is not aligned on a 2K boundary.")
-
-    __RO_START__ = .;
-    vector . : {
-        *(.vectors)
-    } >RAM
-
-    ASSERT(. == ALIGN(PAGE_SIZE),
-           "BL31_BASE address is not aligned on a page boundary.")
-
-    ro . : {
-        *bl31_entrypoint.o(.text*)
-        *(.text*)
-        *(.rodata*)
-
-	RODATA_COMMON
-
-        __RO_END_UNALIGNED__ = .;
-        /*
-         * Memory page(s) mapped to this section will be marked as read-only,
-         * executable.  No RW data from the next section must creep in.
-         * Ensure the rest of the current memory page is unused.
-         */
-        . = ALIGN(PAGE_SIZE);
-        __RO_END__ = .;
-    } >RAM
-
-    ASSERT(__CPU_OPS_END__ > __CPU_OPS_START__,
-           "cpu_ops not defined for this platform.")
-
-    /*
-     * Define a linker symbol to mark start of the RW memory area for this
-     * image.
-     */
-    __RW_START__ = . ;
-
-    DATA_SECTION >RAM
-
-#ifdef BL31_PROGBITS_LIMIT
-    ASSERT(. <= BL31_PROGBITS_LIMIT, "BL3-1 progbits has exceeded its limit.")
-#endif
-
-    STACK_SECTION >RAM
-    BSS_SECTION >RAM
-    __RW_END__ = __BSS_END__;
-
-    ASSERT(. <= BL31_LIMIT, "BL3-1 image has exceeded its limit.")
-
-    XLAT_TABLE_SECTION >RAM2
-
-#if USE_COHERENT_MEM
-    /*
-     * The base address of the coherent memory section must be page-aligned (4K)
-     * to guarantee that the coherent data are stored on their own pages and
-     * are not mixed with normal data.  This is required to set up the correct
-     * memory attributes for the coherent data page tables.
-     */
-    coherent_ram (NOLOAD) : ALIGN(PAGE_SIZE) {
-        __COHERENT_RAM_START__ = .;
-        /*
-         * Bakery locks are stored in coherent memory
-         *
-         * Each lock's data is contiguous and fully allocated by the compiler
-         */
-        *(bakery_lock)
-        *(tzfw_coherent_mem)
-        __COHERENT_RAM_END_UNALIGNED__ = .;
-        /*
-         * Memory page(s) mapped to this section will be marked
-         * as device memory.  No other unexpected data must creep in.
-         * Ensure the rest of the current memory page is unused.
-         */
-        . = ALIGN(PAGE_SIZE);
-        __COHERENT_RAM_END__ = .;
-    } >RAM2
-#endif
-
-    /*
-     * Define a linker symbol to mark end of the RW memory area for this
-     * image.
-     */
-    __BL31_END__ = .;
-
-    __BSS_SIZE__ = SIZEOF(.bss);
-#if USE_COHERENT_MEM
-    __COHERENT_RAM_UNALIGNED_SIZE__ =
-        __COHERENT_RAM_END_UNALIGNED__ - __COHERENT_RAM_START__;
-#endif
-
-    ASSERT(. <= TZRAM2_LIMIT, "TZRAM2 image has exceeded its limit.")
-}
diff --git a/plat/mediatek/mt6795/bl31_plat_setup.c b/plat/mediatek/mt6795/bl31_plat_setup.c
deleted file mode 100644
index 2051fe7..0000000
--- a/plat/mediatek/mt6795/bl31_plat_setup.c
+++ /dev/null
@@ -1,449 +0,0 @@
-/*
- * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <assert.h>
-#include <string.h>
-
-#include <arch_helpers.h>
-#include <common/bl_common.h>
-#include <common/debug.h>
-#include <drivers/arm/cci.h>
-#include <drivers/console.h>
-#include <drivers/generic_delay_timer.h>
-#include <lib/el3_runtime/context_mgmt.h>
-#include <lib/mmio.h>
-#include <lib/utils_def.h>
-#include <lib/xlat_tables/xlat_tables.h>
-#include <plat/common/common_def.h>
-#include <plat/common/platform.h>
-
-#include <mcucfg.h>
-#include <mt_cpuxgpt.h>
-#include <mtk_plat_common.h>
-#include <mtk_sip_svc.h>
-#include <plat_private.h>
-
-/*******************************************************************************
- * Declarations of linker defined symbols which will help us find the layout
- * of trusted SRAM
- ******************************************************************************/
-/*
- * The next 2 constants identify the extents of the code & RO data region.
- * These addresses are used by the MMU setup code and therefore they must be
- * page-aligned.  It is the responsibility of the linker script to ensure that
- * __RO_START__ and __RO_END__ linker symbols refer to page-aligned addresses.
- */
-IMPORT_SYM(unsigned long, __RO_START__,	BL31_RO_BASE);
-IMPORT_SYM(unsigned long, __RO_END__,	BL31_RO_LIMIT);
-
-/*
- * Placeholder variables for copying the arguments that have been passed to
- * BL3-1 from BL2.
- */
-static entry_point_info_t bl32_image_ep_info;
-static entry_point_info_t bl33_image_ep_info;
-
-static const int cci_map[] = {
-	PLAT_MT_CCI_CLUSTER0_SL_IFACE_IX,
-	PLAT_MT_CCI_CLUSTER1_SL_IFACE_IX
-};
-
-static uint32_t cci_map_length = ARRAY_SIZE(cci_map);
-
-/* Table of regions to map using the MMU.  */
-static const mmap_region_t plat_mmap[] = {
-	/* for TF text, RO, RW */
-	MAP_REGION_FLAT(MTK_DEV_RNG0_BASE, MTK_DEV_RNG0_SIZE,
-			MT_DEVICE | MT_RW | MT_SECURE),
-	MAP_REGION_FLAT(MTK_DEV_RNG1_BASE, MTK_DEV_RNG1_SIZE,
-			MT_DEVICE | MT_RW | MT_SECURE),
-	MAP_REGION_FLAT(RAM_CONSOLE_BASE & ~(PAGE_SIZE_MASK), RAM_CONSOLE_SIZE,
-						MT_DEVICE | MT_RW | MT_NS),
-	{ 0 }
-
-};
-
-/*******************************************************************************
- * Macro generating the code for the function setting up the pagetables as per
- * the platform memory map & initialize the mmu, for the given exception level
- ******************************************************************************/
-#define DEFINE_CONFIGURE_MMU_EL(_el)					\
-	void plat_configure_mmu_el ## _el(unsigned long total_base,	\
-				unsigned long total_size,	\
-				unsigned long ro_start,	\
-				unsigned long ro_limit,	\
-				unsigned long coh_start,	\
-				unsigned long coh_limit)	\
-	{								\
-		mmap_add_region(total_base, total_base,			\
-				total_size,				\
-				MT_MEMORY | MT_RW | MT_SECURE);		\
-		mmap_add_region(ro_start, ro_start,			\
-				ro_limit - ro_start,			\
-				MT_MEMORY | MT_RO | MT_SECURE);		\
-		mmap_add_region(coh_start, coh_start,			\
-				coh_limit - coh_start,			\
-				MT_DEVICE | MT_RW | MT_SECURE);		\
-		mmap_add(plat_mmap);					\
-		init_xlat_tables();					\
-									\
-		enable_mmu_el ## _el(0);				\
-	}
-
-/* Define EL3 variants of the function initialising the MMU */
-DEFINE_CONFIGURE_MMU_EL(3)
-
-unsigned int plat_get_syscnt_freq2(void)
-{
-	return SYS_COUNTER_FREQ_IN_TICKS;
-}
-
-void plat_cci_init(void)
-{
-	/* Initialize CCI driver */
-	cci_init(PLAT_MT_CCI_BASE, cci_map, cci_map_length);
-}
-
-void plat_cci_enable(void)
-{
-	/*
-	 * Enable CCI coherency for this cluster.
-	 * No need for locks as no other cpu is active at the moment.
-	 */
-	cci_enable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr()));
-}
-
-void plat_cci_disable(void)
-{
-	cci_disable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr()));
-}
-
-
-static void platform_setup_cpu(void)
-{
-	/* setup big cores */
-	mmio_write_32((uintptr_t)&mt6795_mcucfg->mp1_config_res,
-		MP1_DIS_RGU0_WAIT_PD_CPUS_L1_ACK |
-		MP1_DIS_RGU1_WAIT_PD_CPUS_L1_ACK |
-		MP1_DIS_RGU2_WAIT_PD_CPUS_L1_ACK |
-		MP1_DIS_RGU3_WAIT_PD_CPUS_L1_ACK |
-		MP1_DIS_RGU_NOCPU_WAIT_PD_CPUS_L1_ACK);
-	mmio_setbits_32((uintptr_t)&mt6795_mcucfg->mp1_miscdbg, MP1_AINACTS);
-	mmio_setbits_32((uintptr_t)&mt6795_mcucfg->mp1_clkenm_div,
-		MP1_SW_CG_GEN);
-	mmio_clrbits_32((uintptr_t)&mt6795_mcucfg->mp1_rst_ctl,
-		MP1_L2RSTDISABLE);
-
-	/* set big cores arm64 boot mode */
-	mmio_setbits_32((uintptr_t)&mt6795_mcucfg->mp1_cpucfg,
-		MP1_CPUCFG_64BIT);
-
-	/* set LITTLE cores arm64 boot mode */
-	mmio_setbits_32((uintptr_t)&mt6795_mcucfg->mp0_rv_addr[0].rv_addr_hw,
-		MP0_CPUCFG_64BIT);
-}
-
-/*******************************************************************************
- * 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;
-	else
-		return NULL;
-}
-
-/*******************************************************************************
- * Perform any BL3-1 early platform setup. Here is an opportunity to copy
- * parameters passed by the calling EL (S-EL1 in BL2 & EL3 in BL1) before they
- * are lost (potentially). This needs to be done before the MMU is initialized
- * so that the memory layout can be used while creating page tables.
- * BL2 has flushed this information to memory, so we are guaranteed to pick up
- * good data.
- ******************************************************************************/
-void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
-				u_register_t arg2, u_register_t arg3)
-{
-	struct mtk_bl_param_t *pmtk_bl_param = (struct mtk_bl_param_t *)arg0;
-	struct atf_arg_t *teearg;
-	unsigned long long normal_base;
-	unsigned long long atf_base;
-
-	assert(pmtk_bl_param != NULL);
-	/*
-	 * Mediatek preloader(i.e, BL2) is in 32 bit state, high 32bits
-	 * of 64 bit GP registers are UNKNOWN if CPU warm reset from 32 bit
-	 * to 64 bit state. So we need to clear high 32bit,
-	 * which may be random value.
-	 */
-	pmtk_bl_param =
-	(struct mtk_bl_param_t *)((uint64_t)pmtk_bl_param & 0x00000000ffffffff);
-
-	teearg  = (struct atf_arg_t *)pmtk_bl_param->tee_info_addr;
-
-	console_init(teearg->atf_log_port, UART_CLOCK, UART_BAUDRATE);
-	memcpy((void *)&gteearg, (void *)teearg, sizeof(struct atf_arg_t));
-
-	normal_base = 0;
-    /* in ATF boot time, timer for cntpct_el0 is not initialized
-     * so it will not count now.
-     */
-	atf_base = read_cntpct_el0();
-	sched_clock_init(normal_base, atf_base);
-
-	VERBOSE("bl31_setup\n");
-
-	/* Populate entry point information for BL3-2 and BL3-3 */
-	SET_PARAM_HEAD(&bl32_image_ep_info,
-				PARAM_EP,
-				VERSION_1,
-				0);
-	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
-	bl32_image_ep_info.pc = BL32_BASE;
-
-	SET_PARAM_HEAD(&bl33_image_ep_info,
-				PARAM_EP,
-				VERSION_1,
-				0);
-	/*
-	 * Tell BL3-1 where the non-trusted software image
-	 * is located and the entry state information
-	 */
-	/* BL33_START_ADDRESS */
-	bl33_image_ep_info.pc = pmtk_bl_param->bl33_start_addr;
-	bl33_image_ep_info.spsr = plat_get_spsr_for_bl33_entry();
-	bl33_image_ep_info.args.arg4 =  pmtk_bl_param->bootarg_loc;
-	bl33_image_ep_info.args.arg5 =  pmtk_bl_param->bootarg_size;
-	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
-}
-/*******************************************************************************
- * Perform any BL3-1 platform setup code
- ******************************************************************************/
-
-void bl31_platform_setup(void)
-{
-	platform_setup_cpu();
-
-	generic_delay_timer_init();
-
-	plat_mt_gic_driver_init();
-	/* Initialize the gic cpu and distributor interfaces */
-	plat_mt_gic_init();
-
-	/* Topologies are best known to the platform. */
-	mt_setup_topology();
-}
-/*******************************************************************************
- * Perform the very early platform specific architectural setup here. At the
- * moment this is only intializes the mmu in a quick and dirty way.
- * Init MTK propiartary log buffer control field.
- ******************************************************************************/
-void bl31_plat_arch_setup(void)
-{
-	/* Enable non-secure access to CCI-400 registers */
-	mmio_write_32(CCI400_BASE + CCI_SEC_ACCESS_OFFSET, 0x1);
-
-	plat_cci_init();
-	plat_cci_enable();
-
-	if (gteearg.atf_log_buf_size != 0) {
-		INFO("mmap atf buffer : 0x%x, 0x%x\n\r",
-			gteearg.atf_log_buf_start,
-			gteearg.atf_log_buf_size);
-
-		mmap_add_region(
-			gteearg.atf_log_buf_start &
-			~(PAGE_SIZE_2MB_MASK),
-			gteearg.atf_log_buf_start &
-			~(PAGE_SIZE_2MB_MASK),
-			PAGE_SIZE_2MB,
-			MT_DEVICE | MT_RW | MT_NS);
-
-		INFO("mmap atf buffer (force 2MB aligned):0x%x, 0x%x\n",
-			(gteearg.atf_log_buf_start & ~(PAGE_SIZE_2MB_MASK)),
-		PAGE_SIZE_2MB);
-	}
-	/*
-	 * add TZRAM_BASE to memory map
-	 * then set RO and COHERENT to different attribute
-	 */
-	plat_configure_mmu_el3(
-		(TZRAM_BASE & ~(PAGE_SIZE_MASK)),
-		(TZRAM_SIZE & ~(PAGE_SIZE_MASK)),
-		(BL31_RO_BASE & ~(PAGE_SIZE_MASK)),
-		BL31_RO_LIMIT,
-		BL_COHERENT_RAM_BASE,
-		BL_COHERENT_RAM_END);
-	/* Initialize for ATF log buffer */
-	if (gteearg.atf_log_buf_size != 0) {
-		gteearg.atf_aee_debug_buf_size = ATF_AEE_BUFFER_SIZE;
-		gteearg.atf_aee_debug_buf_start =
-			gteearg.atf_log_buf_start +
-			gteearg.atf_log_buf_size - ATF_AEE_BUFFER_SIZE;
-		INFO("ATF log service is registered (0x%x, aee:0x%x)\n",
-			gteearg.atf_log_buf_start,
-			gteearg.atf_aee_debug_buf_start);
-	} else{
-		gteearg.atf_aee_debug_buf_size = 0;
-		gteearg.atf_aee_debug_buf_start = 0;
-	}
-
-	/* Platform code before bl31_main */
-	/* compatible to the earlier chipset */
-
-	/* Show to ATF log buffer & UART */
-	INFO("BL3-1: %s\n", version_string);
-	INFO("BL3-1: %s\n", build_message);
-
-}
-#if 0
-/* MTK Define */
-#define ACTLR_CPUECTLR_BIT    (1 << 1)
-
-void enable_ns_access_to_cpuectlr(void)
-{
-	unsigned int next_actlr;
-
-
-	/* ACTLR_EL1 do not implement CUPECTLR  */
-	next_actlr = read_actlr_el2();
-	next_actlr |= ACTLR_CPUECTLR_BIT;
-	write_actlr_el2(next_actlr);
-
-	next_actlr = read_actlr_el3();
-	next_actlr |= ACTLR_CPUECTLR_BIT;
-	write_actlr_el3(next_actlr);
-}
-#endif
-/*******************************************************************************
- * This function prepare boot argument for 64 bit kernel entry
- ******************************************************************************/
-static entry_point_info_t *bl31_plat_get_next_kernel64_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 */
-	if (el_implemented(2) != EL_IMPL_NONE) {
-		INFO("Kernel_EL2\n");
-		mode = MODE_EL2;
-	} else{
-		INFO("Kernel_EL1\n");
-		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 = get_kernel_info_pc();
-	next_image_info->args.arg0 = get_kernel_info_r0();
-	next_image_info->args.arg1 = get_kernel_info_r1();
-
-	INFO("pc=0x%lx, r0=0x%lx, r1=0x%lx\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;
-	else
-		return NULL;
-}
-
-/*******************************************************************************
- * This function prepare boot argument for 32 bit kernel entry
- ******************************************************************************/
-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 = get_kernel_info_pc();
-	next_image_info->args.arg0 = get_kernel_info_r0();
-	next_image_info->args.arg1 = get_kernel_info_r1();
-	next_image_info->args.arg2 = get_kernel_info_r2();
-
-	INFO("pc=0x%lx, r0=0x%lx, r1=0x%lx, r2=0x%lx\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;
-	else
-		return NULL;
-}
-
-/*******************************************************************************
- * This function prepare boot argument for kernel entrypoint
- ******************************************************************************/
-void bl31_prepare_kernel_entry(uint64_t k32_64)
-{
-	entry_point_info_t *next_image_info;
-	uint32_t image_type;
-
-	/* Determine which image to execute next */
-	/* image_type = bl31_get_next_image_type(); */
-	image_type = NON_SECURE;
-
-	/* Program EL3 registers to enable entry into the next EL */
-	if (k32_64 == 0)
-		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("BL3-1: Preparing for EL3 exit to %s world, Kernel\n",
-		(image_type == SECURE) ? "secure" : "normal");
-	INFO("BL3-1: Next image address = 0x%llx\n",
-		(unsigned long long) next_image_info->pc);
-	INFO("BL3-1: Next image spsr = 0x%x\n", next_image_info->spsr);
-	cm_init_my_context(next_image_info);
-	cm_prepare_el3_exit(image_type);
-}
diff --git a/plat/mediatek/mt6795/drivers/timer/mt_cpuxgpt.c b/plat/mediatek/mt6795/drivers/timer/mt_cpuxgpt.c
deleted file mode 100644
index 3696f8e..0000000
--- a/plat/mediatek/mt6795/drivers/timer/mt_cpuxgpt.c
+++ /dev/null
@@ -1,67 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <stdint.h>
-
-#include <arch_helpers.h>
-#include <common/debug.h>
-#include <lib/mmio.h>
-#include <plat/common/platform.h>
-
-#include <mt_cpuxgpt.h>
-
-#define CPUXGPT_BASE	0x10200000
-#define INDEX_BASE		(CPUXGPT_BASE+0x0674)
-#define CTL_BASE		(CPUXGPT_BASE+0x0670)
-
-uint64_t normal_time_base;
-uint64_t atf_time_base;
-
-void sched_clock_init(uint64_t normal_base, uint64_t atf_base)
-{
-	normal_time_base = normal_base;
-	atf_time_base = atf_base;
-}
-
-uint64_t sched_clock(void)
-{
-	uint64_t cval;
-
-	cval = (((read_cntpct_el0() - atf_time_base)*1000)/
-		SYS_COUNTER_FREQ_IN_MHZ) + normal_time_base;
-	return cval;
-}
-
-/*
-  * Return: 0 - Trying to disable the CPUXGPT control bit,
-  * and not allowed to disable it.
-  * Return: 1 - reg_addr is not realted to disable the control bit.
-  */
-unsigned char check_cpuxgpt_write_permission(unsigned int reg_addr,
-	unsigned int reg_value)
-{
-	unsigned int idx;
-	unsigned int ctl_val;
-
-	if (reg_addr == CTL_BASE) {
-		idx = mmio_read_32(INDEX_BASE);
-
-		/* idx 0: CPUXGPT system control */
-		if (idx == 0) {
-			ctl_val = mmio_read_32(CTL_BASE);
-			if (ctl_val & 1) {
-				/*
-				 * if enable bit already set,
-				 * then bit 0 is not allow to set as 0
-				 */
-				if (!(reg_value & 1))
-					return 0;
-			}
-		}
-	}
-	return 1;
-}
-
diff --git a/plat/mediatek/mt6795/drivers/timer/mt_cpuxgpt.h b/plat/mediatek/mt6795/drivers/timer/mt_cpuxgpt.h
deleted file mode 100644
index 84df081..0000000
--- a/plat/mediatek/mt6795/drivers/timer/mt_cpuxgpt.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef MT_CPUXGPT_H
-#define MT_CPUXGPT_H
-
-/* REG */
-#define INDEX_CTL_REG       0x000
-#define INDEX_STA_REG       0x004
-#define INDEX_CNT_L_INIT    0x008
-#define INDEX_CNT_H_INIT    0x00C
-
-/* CTL_REG SET */
-#define EN_CPUXGPT          0x01
-#define EN_AHLT_DEBUG       0x02
-#define CLK_DIV1            (0x1 << 8)
-#define CLK_DIV2            (0x2 << 8)
-#define CLK_DIV4            (0x4 << 8)
-#define CLK_DIV_MASK        (~(0x7<<8))
-
-void generic_timer_backup(void);
-void sched_clock_init(uint64_t normal_base, uint64_t atf_base);
-uint64_t sched_clock(void);
-
-#endif /* MT_CPUXGPT_H */
diff --git a/plat/mediatek/mt6795/include/mcucfg.h b/plat/mediatek/mt6795/include/mcucfg.h
deleted file mode 100644
index 21c5394..0000000
--- a/plat/mediatek/mt6795/include/mcucfg.h
+++ /dev/null
@@ -1,155 +0,0 @@
-/*
- * Copyright (c) 2016-2017, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef MCUCFG_H
-#define MCUCFG_H
-
-#include <stdint.h>
-
-#include <platform_def.h>
-
-struct mt6795_mcucfg_regs {
-	uint32_t mp0_ca7l_cache_config;
-	struct {
-		uint32_t mem_delsel0;
-		uint32_t mem_delsel1;
-	} mp0_cpu[4];
-	uint32_t mp0_cache_mem_delsel0;
-	uint32_t mp0_cache_mem_delsel1;
-	uint32_t mp0_axi_config;
-	uint32_t mp0_misc_config[2];
-	struct {
-		uint32_t rv_addr_lw;
-		uint32_t rv_addr_hw;
-	} mp0_rv_addr[4];
-	uint32_t mp0_ca7l_cfg_dis;
-	uint32_t mp0_ca7l_clken_ctrl;
-	uint32_t mp0_ca7l_rst_ctrl;
-	uint32_t mp0_ca7l_misc_config;
-	uint32_t mp0_ca7l_dbg_pwr_ctrl;
-	uint32_t mp0_rw_rsvd0;
-	uint32_t mp0_rw_rsvd1;
-	uint32_t mp0_ro_rsvd;
-	uint32_t reserved0_0[100];
-	uint32_t mp1_cpucfg;
-	uint32_t mp1_miscdbg;
-	uint32_t reserved0_1[13];
-	uint32_t mp1_rst_ctl;
-	uint32_t mp1_clkenm_div;
-	uint32_t reserved0_2[7];
-	uint32_t mp1_config_res;
-	uint32_t reserved0_3[13];
-	struct {
-		uint32_t rv_addr_lw;
-		uint32_t rv_addr_hw;
-	} mp1_rv_addr[2];
-	uint32_t reserved0_4[84];
-	uint32_t mp0_rst_status;		/* 0x400 */
-	uint32_t mp0_dbg_ctrl;
-	uint32_t mp0_dbg_flag;
-	uint32_t mp0_ca7l_ir_mon;
-	struct {
-		uint32_t pc_lw;
-		uint32_t pc_hw;
-		uint32_t fp_arch32;
-		uint32_t sp_arch32;
-		uint32_t fp_arch64_lw;
-		uint32_t fp_arch64_hw;
-		uint32_t sp_arch64_lw;
-		uint32_t sp_arch64_hw;
-	} mp0_dbg_core[4];
-	uint32_t dfd_ctrl;
-	uint32_t dfd_cnt_l;
-	uint32_t dfd_cnt_h;
-	uint32_t misccfg_mp0_rw_rsvd;
-	uint32_t misccfg_sec_vio_status0;
-	uint32_t misccfg_sec_vio_status1;
-	uint32_t reserved1[22];
-	uint32_t misccfg_rw_rsvd;		/* 0x500 */
-	uint32_t mcusys_dbg_mon_sel_a;
-	uint32_t mcusys_dbg_mon;
-	uint32_t reserved2[61];
-	uint32_t mcusys_config_a;		/* 0x600 */
-	uint32_t mcusys_config1_a;
-	uint32_t mcusys_gic_peribase_a;
-	uint32_t reserved3;
-	uint32_t sec_range0_start;		/* 0x610 */
-	uint32_t sec_range0_end;
-	uint32_t sec_range_enable;
-	uint32_t reserved4;
-	uint32_t int_pol_ctl[8];		/* 0x620 */
-	uint32_t aclken_div;			/* 0x640 */
-	uint32_t pclken_div;
-	uint32_t l2c_sram_ctrl;
-	uint32_t armpll_jit_ctrl;
-	uint32_t cci_addrmap;			/* 0x650 */
-	uint32_t cci_config;
-	uint32_t cci_periphbase;
-	uint32_t cci_nevntcntovfl;
-	uint32_t cci_clk_ctrl;			/* 0x660 */
-	uint32_t cci_acel_s1_ctrl;
-	uint32_t bus_fabric_dcm_ctrl;
-	uint32_t reserved5;
-	uint32_t xgpt_ctl;			/* 0x670 */
-	uint32_t xgpt_idx;
-	uint32_t ptpod2_ctl0;
-	uint32_t ptpod2_ctl1;
-	uint32_t mcusys_revid;
-	uint32_t mcusys_rw_rsvd0;
-	uint32_t mcusys_rw_rsvd1;
-};
-
-static struct mt6795_mcucfg_regs *const mt6795_mcucfg = (void *)MCUCFG_BASE;
-
-/* cpu boot mode */
-#define	MP0_CPUCFG_64BIT_SHIFT	12
-#define	MP1_CPUCFG_64BIT_SHIFT	28
-#define	MP0_CPUCFG_64BIT	(U(0xf) << MP0_CPUCFG_64BIT_SHIFT)
-#define	MP1_CPUCFG_64BIT	(U(0xf) << MP1_CPUCFG_64BIT_SHIFT)
-
-/* scu related */
-enum {
-	MP0_ACINACTM_SHIFT = 4,
-	MP1_ACINACTM_SHIFT = 0,
-	MP0_ACINACTM = 1 << MP0_ACINACTM_SHIFT,
-	MP1_ACINACTM = 1 << MP1_ACINACTM_SHIFT
-};
-
-enum {
-	MP1_DIS_RGU0_WAIT_PD_CPUS_L1_ACK_SHIFT = 0,
-	MP1_DIS_RGU1_WAIT_PD_CPUS_L1_ACK_SHIFT = 4,
-	MP1_DIS_RGU2_WAIT_PD_CPUS_L1_ACK_SHIFT = 8,
-	MP1_DIS_RGU3_WAIT_PD_CPUS_L1_ACK_SHIFT = 12,
-	MP1_DIS_RGU_NOCPU_WAIT_PD_CPUS_L1_ACK_SHIFT = 16,
-
-	MP1_DIS_RGU0_WAIT_PD_CPUS_L1_ACK =
-		0xf << MP1_DIS_RGU0_WAIT_PD_CPUS_L1_ACK_SHIFT,
-	MP1_DIS_RGU1_WAIT_PD_CPUS_L1_ACK =
-		0xf << MP1_DIS_RGU1_WAIT_PD_CPUS_L1_ACK_SHIFT,
-	MP1_DIS_RGU2_WAIT_PD_CPUS_L1_ACK =
-		0xf << MP1_DIS_RGU2_WAIT_PD_CPUS_L1_ACK_SHIFT,
-	MP1_DIS_RGU3_WAIT_PD_CPUS_L1_ACK =
-		0xf << MP1_DIS_RGU3_WAIT_PD_CPUS_L1_ACK_SHIFT,
-	MP1_DIS_RGU_NOCPU_WAIT_PD_CPUS_L1_ACK =
-		0xf << MP1_DIS_RGU_NOCPU_WAIT_PD_CPUS_L1_ACK_SHIFT
-};
-
-enum {
-	MP1_AINACTS_SHIFT = 4,
-	MP1_AINACTS = 1 << MP1_AINACTS_SHIFT
-};
-
-enum {
-	MP1_SW_CG_GEN_SHIFT = 12,
-	MP1_SW_CG_GEN = 1 << MP1_SW_CG_GEN_SHIFT
-};
-
-enum {
-	MP1_L2RSTDISABLE_SHIFT = 14,
-	MP1_L2RSTDISABLE = 1 << MP1_L2RSTDISABLE_SHIFT
-};
-
-#endif  /* MCUCFG_H */
diff --git a/plat/mediatek/mt6795/include/plat_macros.S b/plat/mediatek/mt6795/include/plat_macros.S
deleted file mode 100644
index d198fdc..0000000
--- a/plat/mediatek/mt6795/include/plat_macros.S
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <drivers/arm/cci.h>
-#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"
-
-	/* ---------------------------------------------
-	 * The below macro prints out relevant GIC
-	 * registers whenever an unhandled exception is
-	 * taken in BL3-1.
-	 * Clobbers: x0 - x10, x16, x17, sp
-	 * ---------------------------------------------
-	 */
-	.macro plat_crash_print_regs
-	mov_imm x16, BASE_GICD_BASE
-	mov_imm x17, BASE_GICC_BASE
-	/* Load the gicc reg list to x6 */
-	adr	x6, gicc_regs
-	/* Load the gicc regs to gp regs used by str_in_crash_buf_print */
-	ldr	w8, [x17, #GICC_HPPIR]
-	ldr	w9, [x17, #GICC_AHPPIR]
-	ldr	w10, [x17, #GICC_CTLR]
-	/* Store to the crash buf and print to console */
-	bl	str_in_crash_buf_print
-
-	/* Print the GICD_ISPENDR regs */
-	add	x7, x16, #GICD_ISPENDR
-	adr	x4, gicd_pend_reg
-	bl	asm_print_str
-gicd_ispendr_loop:
-	sub	x4, x7, x16
-	cmp	x4, #0x280
-	b.eq	exit_print_gic_regs
-	bl	asm_print_hex
-
-	adr	x4, spacer
-	bl	asm_print_str
-
-	ldr	x4, [x7], #8
-	bl	asm_print_hex
-
-	adr	x4, newline
-	bl	asm_print_str
-	b	gicd_ispendr_loop
-exit_print_gic_regs:
-	.endm
-
-.section .rodata.cci_reg_name, "aS"
-cci_iface_regs:
-	.asciz "cci_snoop_ctrl_cluster0", "cci_snoop_ctrl_cluster1" , ""
-
-	/* ------------------------------------------------
-	 * The below macro prints out relevant interconnect
-	 * registers whenever an unhandled exception is
-	 * taken in BL3-1.
-	 * Clobbers: x0 - x9, sp
-	 * ------------------------------------------------
-	 */
-	.macro plat_print_interconnect_regs
-	adr	x6, cci_iface_regs
-	/* Store in x7 the base address of the first interface */
-	mov_imm	x7, (PLAT_MT_CCI_BASE + SLAVE_IFACE_OFFSET(	\
-			PLAT_MT_CCI_CLUSTER0_SL_IFACE_IX))
-	ldr	w8, [x7, #SNOOP_CTRL_REG]
-	/* Store in x7 the base address of the second interface */
-	mov_imm	x7, (PLAT_MT_CCI_BASE + SLAVE_IFACE_OFFSET(	\
-			PLAT_MT_CCI_CLUSTER1_SL_IFACE_IX))
-	ldr	w9, [x7, #SNOOP_CTRL_REG]
-	/* Store to the crash buf and print to console */
-	bl	str_in_crash_buf_print
-	.endm
diff --git a/plat/mediatek/mt6795/include/plat_private.h b/plat/mediatek/mt6795/include/plat_private.h
deleted file mode 100644
index f7450ca..0000000
--- a/plat/mediatek/mt6795/include/plat_private.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef PLAT_PRIVATE_H
-#define PLAT_PRIVATE_H
-
-#include <stdint.h>
-
-#include <lib/xlat_tables/xlat_tables.h>
-
-void plat_configure_mmu_el3(unsigned long total_base,
-					unsigned long total_size,
-					unsigned long,
-					unsigned long,
-					unsigned long,
-					unsigned long);
-
-void plat_cci_init(void);
-void plat_cci_enable(void);
-void plat_cci_disable(void);
-
-/* Declarations for plat_mt_gic.c */
-void plat_mt_gic_init(void);
-
-/* Declarations for plat_topology.c */
-int mt_setup_topology(void);
-void plat_delay_timer_init(void);
-
-void plat_mt_gic_driver_init(void);
-void plat_mt_gic_init(void);
-void plat_mt_gic_cpuif_enable(void);
-void plat_mt_gic_cpuif_disable(void);
-void plat_mt_gic_pcpu_init(void);
-
-#endif /* PLAT_PRIVATE_H */
diff --git a/plat/mediatek/mt6795/include/plat_sip_calls.h b/plat/mediatek/mt6795/include/plat_sip_calls.h
deleted file mode 100644
index b9a1363..0000000
--- a/plat/mediatek/mt6795/include/plat_sip_calls.h
+++ /dev/null
@@ -1,15 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef PLAT_SIP_CALLS_H
-#define PLAT_SIP_CALLS_H
-
-/*******************************************************************************
- * Plat SiP function constants
- ******************************************************************************/
-#define MTK_PLAT_SIP_NUM_CALLS	0
-
-#endif /* PLAT_SIP_CALLS_H */
diff --git a/plat/mediatek/mt6795/include/platform_def.h b/plat/mediatek/mt6795/include/platform_def.h
deleted file mode 100644
index b353a3d..0000000
--- a/plat/mediatek/mt6795/include/platform_def.h
+++ /dev/null
@@ -1,238 +0,0 @@
-/*
- * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef PLATFORM_DEF_H
-#define PLATFORM_DEF_H
-
-#include <lib/utils_def.h>
-
-#define PLAT_PRIMARY_CPU  0x0
-
-/* Special value used to verify platform parameters from BL2 to BL3-1 */
-#define MT_BL31_PLAT_PARAM_VAL  0x0f1e2d3c4b5a6978ULL
-
-#define IO_PHYS             (0x10000000)
-#define INFRACFG_AO_BASE    (IO_PHYS + 0x1000)
-#define MCUCFG_BASE         (IO_PHYS + 0x200000)
-#define PERI_BASE           (IO_PHYS + 0x1000000)
-
-
-#define GPIO_BASE           (IO_PHYS + 0x370000)
-#define SPM_BASE            (IO_PHYS + 0x6000)
-#define RGU_BASE            (MCUCFG_BASE + 0x11000)
-#define PMIC_WRAP_BASE      (IO_PHYS + 0x10000)
-
-#define TRNG_base           (MCUCFG_BASE + 0x230000)
-#define MT_GIC_BASE         (0x10220000)
-#define MCU_SYS_SIZE        (0x700000)
-#define PLAT_MT_CCI_BASE    (IO_PHYS + 0x390000)
-
-/* Aggregate of all devices in the first GB */
-#define MTK_DEV_RNG0_BASE   IO_PHYS
-#define MTK_DEV_RNG0_SIZE   0x400000
-#define MTK_DEV_RNG1_BASE   (PERI_BASE)
-#define MTK_DEV_RNG1_SIZE   0x4000000
-
-/*******************************************************************************
- * UART related constants
- ******************************************************************************/
-#define UART0_BASE (PERI_BASE + 0x2000)
-
-#define UART_BAUDRATE   (921600)
-#define UART_CLOCK (26000000)
-
-/*******************************************************************************
- * System counter frequency related constants
- ******************************************************************************/
-#define SYS_COUNTER_FREQ_IN_TICKS	13000000
-#define SYS_COUNTER_FREQ_IN_MHZ		(SYS_COUNTER_FREQ_IN_TICKS/1000000)
-
-/*******************************************************************************
- * GIC-400 & interrupt handling related constants
- ******************************************************************************/
-
-/* Base MTK_platform compatible GIC memory map */
-#define BASE_GICD_BASE      (MT_GIC_BASE+0x1000)
-#define BASE_GICC_BASE      (MT_GIC_BASE + 0x2000)
-#define BASE_GICR_BASE      (MT_GIC_BASE + 0x200000)
-#define BASE_GICH_BASE      (MT_GIC_BASE + 0x4000)
-#define BASE_GICV_BASE      (MT_GIC_BASE + 0x6000)
-
-#define INT_POL_CTL0        0x10200620
-#define GIC_PRIVATE_SIGNALS (32)
-
-/*******************************************************************************
- * CCI-400 related constants
- ******************************************************************************/
-#define PLAT_MT_CCI_CLUSTER0_SL_IFACE_IX  4
-#define PLAT_MT_CCI_CLUSTER1_SL_IFACE_IX  3
-
-/*******************************************************************************
- * WDT Registers
- ******************************************************************************/
-#define MTK_WDT_BASE                        (RGU_BASE)
-#define MTK_WDT_SIZE                        (0x1000)
-#define MTK_WDT_MODE                        (MTK_WDT_BASE+0x0000)
-#define MTK_WDT_LENGTH                      (MTK_WDT_BASE+0x0004)
-#define MTK_WDT_RESTART                     (MTK_WDT_BASE+0x0008)
-#define MTK_WDT_STATUS                      (MTK_WDT_BASE+0x000C)
-#define MTK_WDT_INTERVAL                    (MTK_WDT_BASE+0x0010)
-#define MTK_WDT_SWRST                       (MTK_WDT_BASE+0x0014)
-#define MTK_WDT_SWSYSRST                    (MTK_WDT_BASE+0x0018)
-#define MTK_WDT_NONRST_REG                  (MTK_WDT_BASE+0x0020)
-#define MTK_WDT_NONRST_REG2                 (MTK_WDT_BASE+0x0024)
-#define MTK_WDT_REQ_MODE                    (MTK_WDT_BASE+0x0030)
-#define MTK_WDT_REQ_IRQ_EN                  (MTK_WDT_BASE+0x0034)
-#define MTK_WDT_DEBUG_CTL                   (MTK_WDT_BASE+0x0040)
-
-/*WDT_STATUS*/
-#define MTK_WDT_STATUS_HWWDT_RST            (0x80000000)
-#define MTK_WDT_STATUS_SWWDT_RST            (0x40000000)
-#define MTK_WDT_STATUS_IRQWDT_RST           (0x20000000)
-#define MTK_WDT_STATUS_DEBUGWDT_RST         (0x00080000)
-#define MTK_WDT_STATUS_SPMWDT_RST           (0x0002)
-#define MTK_WDT_STATUS_SPM_THERMAL_RST      (0x0001)
-#define MTK_WDT_STATUS_THERMAL_DIRECT_RST   (1<<18)
-#define MTK_WDT_STATUS_SECURITY_RST         (1<<28)
-
-#define MTK_WDT_MODE_DUAL_MODE              0x0040
-#define MTK_WDT_MODE_IRQ                    0x0008
-#define MTK_WDT_MODE_KEY                    0x22000000
-#define MTK_WDT_MODE_EXTEN                  0x0004
-#define MTK_WDT_SWRST_KEY                   0x1209
-#define MTK_WDT_RESTART_KEY                 (0x1971)
-
-/* FIQ platform related define */
-#define MT_IRQ_SEC_SGI_0  8
-#define MT_IRQ_SEC_SGI_1  9
-#define MT_IRQ_SEC_SGI_2  10
-#define MT_IRQ_SEC_SGI_3  11
-#define MT_IRQ_SEC_SGI_4  12
-#define MT_IRQ_SEC_SGI_5  13
-#define MT_IRQ_SEC_SGI_6  14
-#define MT_IRQ_SEC_SGI_7  15
-
-#define FIQ_SMP_CALL_SGI  MT_IRQ_SEC_SGI_5
-
-/*******************************************************************************
- * Platform binary types for linking
- ******************************************************************************/
-#define PLATFORM_LINKER_FORMAT    "elf64-littleaarch64"
-#define PLATFORM_LINKER_ARCH      aarch64
-
-/*******************************************************************************
- * Generic platform constants
- ******************************************************************************/
-
-/* Size of cacheable stacks */
-#if defined(IMAGE_BL1)
-#define PLATFORM_STACK_SIZE 0x440
-#elif defined(IMAGE_BL2)
-#define PLATFORM_STACK_SIZE 0x400
-#elif defined(IMAGE_BL31)
-#define PLATFORM_STACK_SIZE 0x800
-#elif defined(IMAGE_BL32)
-#define PLATFORM_STACK_SIZE 0x440
-#endif
-
-#define FIRMWARE_WELCOME_STR    "Booting Trusted Firmware\n"
-#define PLAT_MAX_PWR_LVL        U(2) /* MPIDR_AFFLVL2 */
-
-#define PLAT_MAX_RET_STATE	U(1)
-#define PLAT_MAX_OFF_STATE	U(2)
-
-#define PLATFORM_CACHE_LINE_SIZE      64
-#define PLATFORM_SYSTEM_COUNT         U(1)
-#define PLATFORM_CLUSTER_COUNT        U(2)
-#define PLATFORM_CLUSTER0_CORE_COUNT  U(4)
-#define PLATFORM_CLUSTER1_CORE_COUNT  U(4)
-#define PLATFORM_CORE_COUNT   (PLATFORM_CLUSTER1_CORE_COUNT + \
-					PLATFORM_CLUSTER0_CORE_COUNT)
-#define PLATFORM_MAX_CPUS_PER_CLUSTER U(4)
-#define PLATFORM_NUM_AFFS   (PLATFORM_SYSTEM_COUNT +  \
-					PLATFORM_CLUSTER_COUNT + \
-					PLATFORM_CORE_COUNT)
-
-/*******************************************************************************
- * Platform memory map related constants
- ******************************************************************************/
-/* ATF Argument */
-#define ATF_ARG_SIZE      (0x800)
-
-/* TF txet, ro, rw, internal SRAM, Size: release: 80KB, debug: 92KB */
-#define TZRAM_BASE        (0x110000)
-#if DEBUG
-#define TZRAM_SIZE        (0x1C400)
-#else
-#define TZRAM_SIZE        (0x1C400)
-#endif
-#define TZRAM2_BASE	   0x00100000
-#define TZRAM2_SIZE	   0xDC00
-#define TZRAM2_LIMIT		(TZRAM2_BASE + TZRAM2_SIZE)
-
-#define RAM_CONSOLE_BASE  0x0012D000
-#define RAM_CONSOLE_SIZE  0x00001000
-/*******************************************************************************
- * 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)
-#define BSS1_STACK_LIMIT    (TZRAM_BASE + TZRAM_SIZE)
-#define BL31_TZRAM_SIZE     (TZRAM_SIZE - ATF_ARG_SIZE)
-
-/*******************************************************************************
- * Platform specific page table and MMU setup constants
- ******************************************************************************/
-#define PLAT_VIRT_ADDR_SPACE_SIZE   (1ULL << 32)
-#define PLAT_PHY_ADDR_SPACE_SIZE    (1ULL << 32)
-#define MAX_XLAT_TABLES   7
-#define MAX_MMAP_REGIONS  16
-
-
-/*******************************************************************************
- * CCI-400 related constants
- ******************************************************************************/
-#define CCI400_BASE                     0x10390000
-#define CCI400_SL_IFACE_CLUSTER0        4
-#define CCI400_SL_IFACE_CLUSTER1        3
-#define CCI400_SL_IFACE_INDEX(mpidr)  (mpidr & MPIDR_CLUSTER_MASK ? \
-					CCI400_SL_IFACE_CLUSTER1 :   \
-					CCI400_SL_IFACE_CLUSTER0)
-#define CCI_SEC_ACCESS_OFFSET           (0x8)
-
-
-/*******************************************************************************
- * 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)
-
-#define BL32_BASE                 (0x0)
-
-/*
- * Load address of BL3-3 for this platform port
- */
-#define LK_SIZE_LIMIT				(0x100000)
-#define PLAT_MTK_NS_IMAGE_OFFSET	(0x41E00000)
-/* 16KB */
-#define ATF_AEE_BUFFER_SIZE         (0x4000)
-#define PAGE_SIZE_2MB_MASK          (PAGE_SIZE_2MB - 1)
-#define IS_PAGE_2MB_ALIGNED(addr)   (((addr) & PAGE_SIZE_2MB_MASK) == 0)
-#define PAGE_SIZE_2MB               (1 << PAGE_SIZE_2MB_SHIFT)
-#define PAGE_SIZE_2MB_SHIFT         TWO_MB_SHIFT
-
-#endif /* PLATFORM_DEF_H */
diff --git a/plat/mediatek/mt6795/include/power_tracer.h b/plat/mediatek/mt6795/include/power_tracer.h
deleted file mode 100644
index 8c98dbd..0000000
--- a/plat/mediatek/mt6795/include/power_tracer.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef POWER_TRACER_H
-#define POWER_TRACER_H
-
-#define CPU_UP		0
-#define CPU_DOWN	1
-#define CPU_SUSPEND	2
-#define CLUSTER_UP	3
-#define CLUSTER_DOWN	4
-#define CLUSTER_SUSPEND	5
-
-void trace_power_flow(unsigned long mpidr, unsigned char mode);
-
-#endif /* POWER_TRACER_H */
diff --git a/plat/mediatek/mt6795/include/scu.h b/plat/mediatek/mt6795/include/scu.h
deleted file mode 100644
index 625418a..0000000
--- a/plat/mediatek/mt6795/include/scu.h
+++ /dev/null
@@ -1,13 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef SCU_H
-#define SCU_H
-
-void disable_scu(unsigned long mpidr);
-void enable_scu(unsigned long mpidr);
-
-#endif /* SCU_H */
diff --git a/plat/mediatek/mt6795/include/spm.h b/plat/mediatek/mt6795/include/spm.h
deleted file mode 100644
index 5227e1d..0000000
--- a/plat/mediatek/mt6795/include/spm.h
+++ /dev/null
@@ -1,198 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef SPM_H
-#define SPM_H
-
-#define SPM_POWERON_CONFIG_SET			(SPM_BASE + 0x000)
-#define SPM_POWER_ON_VAL0			(SPM_BASE + 0x010)
-#define SPM_POWER_ON_VAL1			(SPM_BASE + 0x014)
-#define SPM_CLK_SETTLE				(SPM_BASE + 0x100)
-#define SPM_CA7_CPU1_PWR_CON			(SPM_BASE + 0x218)
-#define SPM_CA7_CPU2_PWR_CON			(SPM_BASE + 0x21c)
-#define SPM_CA7_CPU3_PWR_CON			(SPM_BASE + 0x220)
-#define SPM_CA7_CPU1_L1_PDN			(SPM_BASE + 0x264)
-#define SPM_CA7_CPU2_L1_PDN			(SPM_BASE + 0x26c)
-#define SPM_CA7_CPU3_L1_PDN			(SPM_BASE + 0x274)
-#define SPM_MD32_SRAM_CON			(SPM_BASE + 0x2c8)
-#define SPM_PCM_CON0				(SPM_BASE + 0x310)
-#define SPM_PCM_CON1				(SPM_BASE + 0x314)
-#define SPM_PCM_IM_PTR				(SPM_BASE + 0x318)
-#define SPM_PCM_IM_LEN				(SPM_BASE + 0x31c)
-#define SPM_PCM_REG_DATA_INI			(SPM_BASE + 0x320)
-#define SPM_PCM_EVENT_VECTOR0			(SPM_BASE + 0x340)
-#define SPM_PCM_EVENT_VECTOR1			(SPM_BASE + 0x344)
-#define SPM_PCM_EVENT_VECTOR2			(SPM_BASE + 0x348)
-#define SPM_PCM_EVENT_VECTOR3			(SPM_BASE + 0x34c)
-#define SPM_PCM_MAS_PAUSE_MASK			(SPM_BASE + 0x354)
-#define SPM_PCM_PWR_IO_EN			(SPM_BASE + 0x358)
-#define SPM_PCM_TIMER_VAL			(SPM_BASE + 0x35c)
-#define SPM_PCM_TIMER_OUT			(SPM_BASE + 0x360)
-#define SPM_PCM_REG0_DATA			(SPM_BASE + 0x380)
-#define SPM_PCM_REG1_DATA			(SPM_BASE + 0x384)
-#define SPM_PCM_REG2_DATA			(SPM_BASE + 0x388)
-#define SPM_PCM_REG3_DATA			(SPM_BASE + 0x38c)
-#define SPM_PCM_REG4_DATA			(SPM_BASE + 0x390)
-#define SPM_PCM_REG5_DATA			(SPM_BASE + 0x394)
-#define SPM_PCM_REG6_DATA			(SPM_BASE + 0x398)
-#define SPM_PCM_REG7_DATA			(SPM_BASE + 0x39c)
-#define SPM_PCM_REG8_DATA			(SPM_BASE + 0x3a0)
-#define SPM_PCM_REG9_DATA			(SPM_BASE + 0x3a4)
-#define SPM_PCM_REG10_DATA			(SPM_BASE + 0x3a8)
-#define SPM_PCM_REG11_DATA			(SPM_BASE + 0x3ac)
-#define SPM_PCM_REG12_DATA			(SPM_BASE + 0x3b0)
-#define SPM_PCM_REG13_DATA			(SPM_BASE + 0x3b4)
-#define SPM_PCM_REG14_DATA			(SPM_BASE + 0x3b8)
-#define SPM_PCM_REG15_DATA			(SPM_BASE + 0x3bc)
-#define SPM_PCM_EVENT_REG_STA			(SPM_BASE + 0x3c0)
-#define SPM_PCM_FSM_STA				(SPM_BASE + 0x3c4)
-#define SPM_PCM_IM_HOST_RW_PTR			(SPM_BASE + 0x3c8)
-#define SPM_PCM_IM_HOST_RW_DAT			(SPM_BASE + 0x3cc)
-#define SPM_PCM_EVENT_VECTOR4			(SPM_BASE + 0x3d0)
-#define SPM_PCM_EVENT_VECTOR5			(SPM_BASE + 0x3d4)
-#define SPM_PCM_EVENT_VECTOR6			(SPM_BASE + 0x3d8)
-#define SPM_PCM_EVENT_VECTOR7			(SPM_BASE + 0x3dc)
-#define SPM_PCM_SW_INT_SET			(SPM_BASE + 0x3e0)
-#define SPM_PCM_SW_INT_CLEAR			(SPM_BASE + 0x3e4)
-#define SPM_CLK_CON				(SPM_BASE + 0x400)
-#define SPM_SLEEP_PTPOD2_CON			(SPM_BASE + 0x408)
-#define SPM_APMCU_PWRCTL			(SPM_BASE + 0x600)
-#define SPM_AP_DVFS_CON_SET			(SPM_BASE + 0x604)
-#define SPM_AP_STANBY_CON			(SPM_BASE + 0x608)
-#define SPM_PWR_STATUS				(SPM_BASE + 0x60c)
-#define SPM_PWR_STATUS_2ND			(SPM_BASE + 0x610)
-#define SPM_AP_BSI_REQ				(SPM_BASE + 0x614)
-#define SPM_SLEEP_TIMER_STA			(SPM_BASE + 0x720)
-#define SPM_SLEEP_WAKEUP_EVENT_MASK		(SPM_BASE + 0x810)
-#define SPM_SLEEP_CPU_WAKEUP_EVENT		(SPM_BASE + 0x814)
-#define SPM_SLEEP_MD32_WAKEUP_EVENT_MASK	(SPM_BASE + 0x818)
-#define SPM_PCM_WDT_TIMER_VAL			(SPM_BASE + 0x824)
-#define SPM_PCM_WDT_TIMER_OUT			(SPM_BASE + 0x828)
-#define SPM_PCM_MD32_MAILBOX			(SPM_BASE + 0x830)
-#define SPM_PCM_MD32_IRQ			(SPM_BASE + 0x834)
-#define SPM_SLEEP_ISR_MASK			(SPM_BASE + 0x900)
-#define SPM_SLEEP_ISR_STATUS			(SPM_BASE + 0x904)
-#define SPM_SLEEP_ISR_RAW_STA			(SPM_BASE + 0x910)
-#define SPM_SLEEP_MD32_ISR_RAW_STA		(SPM_BASE + 0x914)
-#define SPM_SLEEP_WAKEUP_MISC			(SPM_BASE + 0x918)
-#define SPM_SLEEP_BUS_PROTECT_RDY		(SPM_BASE + 0x91c)
-#define SPM_SLEEP_SUBSYS_IDLE_STA		(SPM_BASE + 0x920)
-#define SPM_PCM_RESERVE				(SPM_BASE + 0xb00)
-#define SPM_PCM_RESERVE2			(SPM_BASE + 0xb04)
-#define SPM_PCM_FLAGS				(SPM_BASE + 0xb08)
-#define SPM_PCM_SRC_REQ				(SPM_BASE + 0xb0c)
-#define SPM_PCM_DEBUG_CON			(SPM_BASE + 0xb20)
-#define SPM_CA7_CPU0_IRQ_MASK			(SPM_BASE + 0xb30)
-#define SPM_CA7_CPU1_IRQ_MASK			(SPM_BASE + 0xb34)
-#define SPM_CA7_CPU2_IRQ_MASK			(SPM_BASE + 0xb38)
-#define SPM_CA7_CPU3_IRQ_MASK			(SPM_BASE + 0xb3c)
-#define SPM_CA15_CPU0_IRQ_MASK			(SPM_BASE + 0xb40)
-#define SPM_CA15_CPU1_IRQ_MASK			(SPM_BASE + 0xb44)
-#define SPM_CA15_CPU2_IRQ_MASK			(SPM_BASE + 0xb48)
-#define SPM_CA15_CPU3_IRQ_MASK			(SPM_BASE + 0xb4c)
-#define SPM_PCM_PASR_DPD_0			(SPM_BASE + 0xb60)
-#define SPM_PCM_PASR_DPD_1			(SPM_BASE + 0xb64)
-#define SPM_PCM_PASR_DPD_2			(SPM_BASE + 0xb68)
-#define SPM_PCM_PASR_DPD_3			(SPM_BASE + 0xb6c)
-#define SPM_SLEEP_CA7_WFI0_EN			(SPM_BASE + 0xf00)
-#define SPM_SLEEP_CA7_WFI1_EN			(SPM_BASE + 0xf04)
-#define SPM_SLEEP_CA7_WFI2_EN			(SPM_BASE + 0xf08)
-#define SPM_SLEEP_CA7_WFI3_EN			(SPM_BASE + 0xf0c)
-#define SPM_SLEEP_CA15_WFI0_EN			(SPM_BASE + 0xf10)
-#define SPM_SLEEP_CA15_WFI1_EN			(SPM_BASE + 0xf14)
-#define SPM_SLEEP_CA15_WFI2_EN			(SPM_BASE + 0xf18)
-#define SPM_SLEEP_CA15_WFI3_EN			(SPM_BASE + 0xf1c)
-
-#define SPM_PROJECT_CODE	0xb16
-
-#define SPM_REGWR_EN		(1U << 0)
-#define SPM_REGWR_CFG_KEY	(SPM_PROJECT_CODE << 16)
-
-#define SPM_CPU_PDN_DIS		(1U << 0)
-#define SPM_INFRA_PDN_DIS	(1U << 1)
-#define SPM_DDRPHY_PDN_DIS	(1U << 2)
-#define SPM_DUALVCORE_PDN_DIS	(1U << 3)
-#define SPM_PASR_DIS		(1U << 4)
-#define SPM_DPD_DIS		(1U << 5)
-#define SPM_SODI_DIS		(1U << 6)
-#define SPM_MEMPLL_RESET	(1U << 7)
-#define SPM_MAINPLL_PDN_DIS	(1U << 8)
-#define SPM_CPU_DVS_DIS		(1U << 9)
-#define SPM_CPU_DORMANT		(1U << 10)
-#define SPM_EXT_VSEL_GPIO103	(1U << 11)
-#define SPM_DDR_HIGH_SPEED	(1U << 12)
-#define SPM_OPT			(1U << 13)
-
-#define POWER_ON_VAL1_DEF	0x01011820
-#define PCM_FSM_STA_DEF		0x48490
-#define PCM_END_FSM_STA_DEF	0x08490
-#define PCM_END_FSM_STA_MASK	0x3fff0
-#define PCM_HANDSHAKE_SEND1	0xbeefbeef
-
-#define PCM_WDT_TIMEOUT		(30 * 32768)
-#define PCM_TIMER_MAX		(0xffffffff - PCM_WDT_TIMEOUT)
-
-#define CON0_PCM_KICK		(1U << 0)
-#define CON0_IM_KICK		(1U << 1)
-#define CON0_IM_SLEEP_DVS	(1U << 3)
-#define CON0_PCM_SW_RESET	(1U << 15)
-#define CON0_CFG_KEY		(SPM_PROJECT_CODE << 16)
-
-#define CON1_IM_SLAVE		(1U << 0)
-#define CON1_MIF_APBEN		(1U << 3)
-#define CON1_PCM_TIMER_EN	(1U << 5)
-#define CON1_IM_NONRP_EN	(1U << 6)
-#define CON1_PCM_WDT_EN		(1U << 8)
-#define CON1_PCM_WDT_WAKE_MODE	(1U << 9)
-#define CON1_SPM_SRAM_SLP_B	(1U << 10)
-#define CON1_SPM_SRAM_ISO_B	(1U << 11)
-#define CON1_EVENT_LOCK_EN	(1U << 12)
-#define CON1_CFG_KEY		(SPM_PROJECT_CODE << 16)
-
-#define PCM_PWRIO_EN_R0		(1U << 0)
-#define PCM_PWRIO_EN_R7		(1U << 7)
-#define PCM_RF_SYNC_R0		(1U << 16)
-#define PCM_RF_SYNC_R2		(1U << 18)
-#define PCM_RF_SYNC_R6		(1U << 22)
-#define PCM_RF_SYNC_R7		(1U << 23)
-
-#define CC_SYSCLK0_EN_0		(1U << 0)
-#define CC_SYSCLK0_EN_1		(1U << 1)
-#define CC_SYSCLK1_EN_0		(1U << 2)
-#define CC_SYSCLK1_EN_1		(1U << 3)
-#define CC_SYSSETTLE_SEL	(1U << 4)
-#define CC_LOCK_INFRA_DCM	(1U << 5)
-#define CC_SRCLKENA_MASK_0	(1U << 6)
-#define CC_CXO32K_RM_EN_MD1	(1U << 9)
-#define CC_CXO32K_RM_EN_MD2	(1U << 10)
-#define CC_CLKSQ1_SEL		(1U << 12)
-#define CC_DISABLE_DORM_PWR	(1U << 14)
-#define CC_MD32_DCM_EN		(1U << 18)
-
-#define WFI_OP_AND		1
-#define WFI_OP_OR		0
-
-#define WAKE_MISC_PCM_TIMER	(1U << 19)
-#define WAKE_MISC_CPU_WAKE	(1U << 20)
-
-/* define WAKE_SRC_XXX */
-#define WAKE_SRC_SPM_MERGE	(1 << 0)
-#define WAKE_SRC_KP		(1 << 2)
-#define WAKE_SRC_WDT		(1 << 3)
-#define WAKE_SRC_GPT		(1 << 4)
-#define WAKE_SRC_EINT		(1 << 6)
-#define WAKE_SRC_LOW_BAT	(1 << 9)
-#define WAKE_SRC_MD32		(1 << 10)
-#define WAKE_SRC_USB_CD		(1 << 14)
-#define WAKE_SRC_USB_PDN	(1 << 15)
-#define WAKE_SRC_AFE		(1 << 20)
-#define WAKE_SRC_THERM		(1 << 21)
-#define WAKE_SRC_SYSPWREQ	(1 << 24)
-#define WAKE_SRC_SEJ		(1 << 27)
-#define WAKE_SRC_ALL_MD32	(1 << 28)
-#define WAKE_SRC_CPU_IRQ	(1 << 29)
-
-#endif /* SPM_H */
diff --git a/plat/mediatek/mt6795/plat_delay_timer.c b/plat/mediatek/mt6795/plat_delay_timer.c
deleted file mode 100644
index 965b653..0000000
--- a/plat/mediatek/mt6795/plat_delay_timer.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <platform_def.h>
-
-#include <arch_helpers.h>
-#include <drivers/delay_timer.h>
-
-static uint32_t plat_get_timer_value(void)
-{
-	/*
-	 * Generic delay timer implementation expects the timer to be a down
-	 * counter. We apply bitwise NOT operator to the tick values returned
-	 * by read_cntpct_el0() to simulate the down counter.
-	 */
-	return (uint32_t)(~read_cntpct_el0());
-}
-
-static const timer_ops_t plat_timer_ops = {
-	.get_timer_value	= plat_get_timer_value,
-	.clk_mult		= 1,
-	.clk_div		= SYS_COUNTER_FREQ_IN_MHZ,
-};
-
-void plat_delay_timer_init(void)
-{
-	timer_init(&plat_timer_ops);
-}
diff --git a/plat/mediatek/mt6795/plat_mt_gic.c b/plat/mediatek/mt6795/plat_mt_gic.c
deleted file mode 100644
index 20cb26d..0000000
--- a/plat/mediatek/mt6795/plat_mt_gic.c
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <platform_def.h>
-
-#include <common/interrupt_props.h>
-#include <drivers/arm/gicv2.h>
-#include <plat/common/platform.h>
-
-static const interrupt_prop_t g0_interrupt_props[] = {
-	INTR_PROP_DESC(FIQ_SMP_CALL_SGI, GIC_HIGHEST_SEC_PRIORITY,
-		       GICV2_INTR_GROUP0, GIC_INTR_CFG_LEVEL),
-};
-
-gicv2_driver_data_t arm_gic_data = {
-	.gicd_base = BASE_GICD_BASE,
-	.gicc_base = BASE_GICC_BASE,
-	.interrupt_props = g0_interrupt_props,
-	.interrupt_props_num = ARRAY_SIZE(g0_interrupt_props),
-};
-
-void plat_mt_gic_driver_init(void)
-{
-	gicv2_driver_init(&arm_gic_data);
-}
-
-void plat_mt_gic_init(void)
-{
-	gicv2_distif_init();
-	gicv2_pcpu_distif_init();
-	gicv2_cpuif_enable();
-}
-
-void plat_mt_gic_cpuif_enable(void)
-{
-	gicv2_cpuif_enable();
-}
-
-void plat_mt_gic_cpuif_disable(void)
-{
-	gicv2_cpuif_disable();
-}
-
-void plat_mt_gic_pcpu_init(void)
-{
-	gicv2_pcpu_distif_init();
-}
diff --git a/plat/mediatek/mt6795/plat_pm.c b/plat/mediatek/mt6795/plat_pm.c
deleted file mode 100644
index 0dfbd18..0000000
--- a/plat/mediatek/mt6795/plat_pm.c
+++ /dev/null
@@ -1,473 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <assert.h>
-#include <errno.h>
-
-#include <platform_def.h>
-
-#include <arch_helpers.h>
-#include <common/debug.h>
-#include <drivers/arm/cci.h>
-#include <drivers/console.h>
-#include <lib/bakery_lock.h>
-#include <lib/mmio.h>
-#include <lib/psci/psci.h>
-
-#include <mcucfg.h>
-#include <plat_private.h>
-#include <power_tracer.h>
-#include <scu.h>
-
-struct core_context {
-	unsigned long timer_data[8];
-	unsigned int count;
-	unsigned int rst;
-	unsigned int abt;
-	unsigned int brk;
-};
-
-struct cluster_context {
-	struct core_context core[PLATFORM_MAX_CPUS_PER_CLUSTER];
-};
-
-/*
- * Top level structure to hold the complete context of a multi cluster system
- */
-struct system_context {
-	struct cluster_context cluster[PLATFORM_CLUSTER_COUNT];
-};
-
-/*
- * Top level structure which encapsulates the context of the entire system
- */
-static struct system_context dormant_data[1];
-
-static inline struct cluster_context *system_cluster(
-						struct system_context *system,
-						uint32_t clusterid)
-{
-	return &system->cluster[clusterid];
-}
-
-static inline struct core_context *cluster_core(struct cluster_context *cluster,
-						uint32_t cpuid)
-{
-	return &cluster->core[cpuid];
-}
-
-static struct cluster_context *get_cluster_data(unsigned long mpidr)
-{
-	uint32_t clusterid;
-
-	clusterid = (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS;
-
-	return system_cluster(dormant_data, clusterid);
-}
-
-static struct core_context *get_core_data(unsigned long mpidr)
-{
-	struct cluster_context *cluster;
-	uint32_t cpuid;
-
-	cluster = get_cluster_data(mpidr);
-	cpuid = mpidr & MPIDR_CPU_MASK;
-
-	return cluster_core(cluster, cpuid);
-}
-
-static void mt_save_generic_timer(unsigned long *container)
-{
-	uint64_t ctl;
-	uint64_t val;
-
-	__asm__ volatile("mrs	%x0, cntkctl_el1\n\t"
-			 "mrs	%x1, cntp_cval_el0\n\t"
-			 "stp	%x0, %x1, [%2, #0]"
-			 : "=&r" (ctl), "=&r" (val)
-			 : "r" (container)
-			 : "memory");
-
-	__asm__ volatile("mrs	%x0, cntp_tval_el0\n\t"
-			 "mrs	%x1, cntp_ctl_el0\n\t"
-			 "stp	%x0, %x1, [%2, #16]"
-			 : "=&r" (val), "=&r" (ctl)
-			 : "r" (container)
-			 : "memory");
-
-	__asm__ volatile("mrs	%x0, cntv_tval_el0\n\t"
-			 "mrs	%x1, cntv_ctl_el0\n\t"
-			 "stp	%x0, %x1, [%2, #32]"
-			 : "=&r" (val), "=&r" (ctl)
-			 : "r" (container)
-			 : "memory");
-}
-
-static void mt_restore_generic_timer(unsigned long *container)
-{
-	uint64_t ctl;
-	uint64_t val;
-
-	__asm__ volatile("ldp	%x0, %x1, [%2, #0]\n\t"
-			 "msr	cntkctl_el1, %x0\n\t"
-			 "msr	cntp_cval_el0, %x1"
-			 : "=&r" (ctl), "=&r" (val)
-			 : "r" (container)
-			 : "memory");
-
-	__asm__ volatile("ldp	%x0, %x1, [%2, #16]\n\t"
-			 "msr	cntp_tval_el0, %x0\n\t"
-			 "msr	cntp_ctl_el0, %x1"
-			 : "=&r" (val), "=&r" (ctl)
-			 : "r" (container)
-			 : "memory");
-
-	__asm__ volatile("ldp	%x0, %x1, [%2, #32]\n\t"
-			 "msr	cntv_tval_el0, %x0\n\t"
-			 "msr	cntv_ctl_el0, %x1"
-			 : "=&r" (val), "=&r" (ctl)
-			 : "r" (container)
-			 : "memory");
-}
-
-static void stop_generic_timer(void)
-{
-	/*
-	 * Disable the timer and mask the irq to prevent
-	 * suprious interrupts on this cpu interface. It
-	 * will bite us when we come back if we don't. It
-	 * will be replayed on the inbound cluster.
-	 */
-	uint64_t cntpctl = read_cntp_ctl_el0();
-
-	write_cntp_ctl_el0(clr_cntp_ctl_enable(cntpctl));
-}
-
-static void mt_cpu_save(unsigned long mpidr)
-{
-	struct core_context *core;
-
-	core = get_core_data(mpidr);
-	mt_save_generic_timer(core->timer_data);
-
-	/* disable timer irq, and upper layer should enable it again. */
-	stop_generic_timer();
-}
-
-static void mt_cpu_restore(unsigned long mpidr)
-{
-	struct core_context *core;
-
-	core = get_core_data(mpidr);
-	mt_restore_generic_timer(core->timer_data);
-}
-
-static void mt_platform_save_context(unsigned long mpidr)
-{
-	/* mcusys_save_context: */
-	mt_cpu_save(mpidr);
-}
-
-static void mt_platform_restore_context(unsigned long mpidr)
-{
-	/* mcusys_restore_context: */
-	mt_cpu_restore(mpidr);
-}
-
-/*******************************************************************************
-* Private function which is used to determine if any platform actions
-* should be performed for the specified affinity instance given its
-* state. Nothing needs to be done if the 'state' is not off or if this is not
-* the highest affinity level which will enter the 'state'.
-*******************************************************************************/
-static int32_t plat_do_plat_actions(unsigned int afflvl, unsigned int state)
-{
-	unsigned int max_phys_off_afflvl;
-
-	assert(afflvl <= MPIDR_AFFLVL2);
-
-	if (state != PSCI_STATE_OFF)
-		return -EAGAIN;
-
-	/*
-	 * Find the highest affinity level which will be suspended and postpone
-	 * all the platform specific actions until that level is hit.
-	 */
-	max_phys_off_afflvl = psci_get_max_phys_off_afflvl();
-	assert(max_phys_off_afflvl != PSCI_INVALID_DATA);
-	if (afflvl != max_phys_off_afflvl)
-		return -EAGAIN;
-
-	return 0;
-}
-
-/*******************************************************************************
- * MTK_platform handler called when an affinity instance is about to enter
- * standby.
- ******************************************************************************/
-static void plat_affinst_standby(unsigned int power_state)
-{
-	unsigned int target_afflvl;
-
-	/* Sanity check the requested state */
-	target_afflvl = psci_get_pstate_afflvl(power_state);
-
-	/*
-	 * It's possible to enter standby only on affinity level 0 i.e. a cpu
-	 * on the MTK_platform. Ignore any other affinity level.
-	 */
-	if (target_afflvl == MPIDR_AFFLVL0) {
-		/*
-		 * Enter standby state. dsb is good practice before using wfi
-		 * to enter low power states.
-		 */
-		dsb();
-		wfi();
-	}
-}
-
-/*******************************************************************************
- * MTK_platform handler called when an affinity instance is about to be turned
- * on. The level and mpidr determine the affinity instance.
- ******************************************************************************/
-static int plat_affinst_on(unsigned long mpidr,
-		    unsigned long sec_entrypoint,
-		    unsigned int afflvl,
-		    unsigned int state)
-{
-	int rc = PSCI_E_SUCCESS;
-	unsigned long cpu_id;
-	unsigned long cluster_id;
-	uintptr_t rv;
-
-	/*
-	 * It's possible to turn on only affinity level 0 i.e. a cpu
-	 * on the MTK_platform. Ignore any other affinity level.
-	 */
-	if (afflvl != MPIDR_AFFLVL0)
-		return rc;
-
-	cpu_id = mpidr & MPIDR_CPU_MASK;
-	cluster_id = mpidr & MPIDR_CLUSTER_MASK;
-
-	if (cluster_id)
-		rv = (uintptr_t)&mt6795_mcucfg->mp1_rv_addr[cpu_id].rv_addr_lw;
-	else
-		rv = (uintptr_t)&mt6795_mcucfg->mp0_rv_addr[cpu_id].rv_addr_lw;
-
-	mmio_write_32(rv, sec_entrypoint);
-	INFO("mt_on[%ld:%ld], entry %x\n",
-		cluster_id, cpu_id, mmio_read_32(rv));
-
-	return rc;
-}
-
-/*******************************************************************************
- * MTK_platform handler called when an affinity instance is about to be turned
- * off. The level and mpidr determine the affinity instance. The 'state' arg.
- * allows the platform to decide whether the cluster is being turned off and
- * take apt actions.
- *
- * CAUTION: This function is called with coherent stacks so that caches can be
- * turned off, flushed and coherency disabled. There is no guarantee that caches
- * will remain turned on across calls to this function as each affinity level is
- * dealt with. So do not write & read global variables across calls. It will be
- * wise to do flush a write to the global to prevent unpredictable results.
- ******************************************************************************/
-static void plat_affinst_off(unsigned int afflvl, unsigned int state)
-{
-	unsigned long mpidr = read_mpidr_el1();
-
-	/* Determine if any platform actions need to be executed. */
-	if (plat_do_plat_actions(afflvl, state) == -EAGAIN)
-		return;
-
-	/* Prevent interrupts from spuriously waking up this cpu */
-	plat_mt_gic_cpuif_disable();
-
-	trace_power_flow(mpidr, CPU_DOWN);
-
-	if (afflvl != MPIDR_AFFLVL0) {
-		/* Disable coherency if this cluster is to be turned off */
-		plat_cci_disable();
-
-		trace_power_flow(mpidr, CLUSTER_DOWN);
-	}
-}
-
-/*******************************************************************************
- * MTK_platform handler called when an affinity instance is about to be
- * suspended. The level and mpidr determine the affinity instance. The 'state'
- * arg. allows the platform to decide whether the cluster is being turned off
- * and take apt actions.
- *
- * CAUTION: This function is called with coherent stacks so that caches can be
- * turned off, flushed and coherency disabled. There is no guarantee that caches
- * will remain turned on across calls to this function as each affinity level is
- * dealt with. So do not write & read global variables across calls. It will be
- * wise to do flush a write to the global to prevent unpredictable results.
- ******************************************************************************/
-static void plat_affinst_suspend(unsigned long sec_entrypoint,
-			  unsigned int afflvl,
-			  unsigned int state)
-{
-	unsigned long mpidr = read_mpidr_el1();
-	unsigned long cluster_id;
-	unsigned long cpu_id;
-	uintptr_t rv;
-
-	/* Determine if any platform actions need to be executed. */
-	if (plat_do_plat_actions(afflvl, state) == -EAGAIN)
-		return;
-
-	cpu_id = mpidr & MPIDR_CPU_MASK;
-	cluster_id = mpidr & MPIDR_CLUSTER_MASK;
-
-	if (cluster_id)
-		rv = (uintptr_t)&mt6795_mcucfg->mp1_rv_addr[cpu_id].rv_addr_lw;
-	else
-		rv = (uintptr_t)&mt6795_mcucfg->mp0_rv_addr[cpu_id].rv_addr_lw;
-
-	mmio_write_32(rv, sec_entrypoint);
-
-	if (afflvl >= MPIDR_AFFLVL0)
-		mt_platform_save_context(mpidr);
-
-	/* Perform the common cluster specific operations */
-	if (afflvl >= MPIDR_AFFLVL1) {
-		/* Disable coherency if this cluster is to be turned off */
-		plat_cci_disable();
-		disable_scu(mpidr);
-
-		trace_power_flow(mpidr, CLUSTER_SUSPEND);
-	}
-
-	if (afflvl >= MPIDR_AFFLVL2) {
-		/* Prevent interrupts from spuriously waking up this cpu */
-		plat_mt_gic_cpuif_disable();
-	}
-}
-
-/*******************************************************************************
- * MTK_platform handler called when an affinity instance has just been powered
- * on after being turned off earlier. The level and mpidr determine the affinity
- * instance. The 'state' arg. allows the platform to decide whether the cluster
- * was turned off prior to wakeup and do what's necessary to setup it up
- * correctly.
- ******************************************************************************/
-static void plat_affinst_on_finish(unsigned int afflvl, unsigned int state)
-{
-	unsigned long mpidr = read_mpidr_el1();
-
-	/* Determine if any platform actions need to be executed. */
-	if (plat_do_plat_actions(afflvl, state) == -EAGAIN)
-		return;
-
-	/* Perform the common cluster specific operations */
-	if (afflvl >= MPIDR_AFFLVL1) {
-		enable_scu(mpidr);
-
-		/* Enable coherency if this cluster was off */
-		plat_cci_enable();
-		trace_power_flow(mpidr, CLUSTER_UP);
-	}
-
-	/* Enable the gic cpu interface */
-	plat_mt_gic_cpuif_enable();
-	plat_mt_gic_pcpu_init();
-	trace_power_flow(mpidr, CPU_UP);
-}
-
-/*******************************************************************************
- * MTK_platform handler called when an affinity instance has just been powered
- * on after having been suspended earlier. The level and mpidr determine the
- * affinity instance.
- ******************************************************************************/
-static void plat_affinst_suspend_finish(unsigned int afflvl, unsigned int state)
-{
-	unsigned long mpidr = read_mpidr_el1();
-
-	/* Determine if any platform actions need to be executed. */
-	if (plat_do_plat_actions(afflvl, state) == -EAGAIN)
-		return;
-
-	if (afflvl >= MPIDR_AFFLVL2) {
-		/* Enable the gic cpu interface */
-		plat_mt_gic_init();
-		plat_mt_gic_cpuif_enable();
-	}
-
-	/* Perform the common cluster specific operations */
-	if (afflvl >= MPIDR_AFFLVL1) {
-		enable_scu(mpidr);
-
-		/* Enable coherency if this cluster was off */
-		plat_cci_enable();
-		trace_power_flow(mpidr, CLUSTER_UP);
-	}
-
-	if (afflvl >= MPIDR_AFFLVL0)
-		mt_platform_restore_context(mpidr);
-
-	plat_mt_gic_pcpu_init();
-}
-
-static unsigned int plat_get_sys_suspend_power_state(void)
-{
-	/* StateID: 0, StateType: 1(power down), PowerLevel: 2(system) */
-	return psci_make_powerstate(0, 1, 2);
-}
-
-/*******************************************************************************
- * MTK handlers to shutdown/reboot the system
- ******************************************************************************/
-static void __dead2 plat_system_off(void)
-{
-	INFO("MTK System Off\n");
-	wfi();
-	ERROR("MTK System Off: operation not handled.\n");
-	panic();
-}
-
-static void __dead2 plat_system_reset(void)
-{
-	/* Write the System Configuration Control Register */
-	INFO("MTK System Reset\n");
-
-	mmio_clrbits_32(MTK_WDT_BASE,
-		(MTK_WDT_MODE_DUAL_MODE | MTK_WDT_MODE_IRQ));
-	mmio_setbits_32(MTK_WDT_BASE, (MTK_WDT_MODE_KEY | MTK_WDT_MODE_EXTEN));
-	mmio_setbits_32(MTK_WDT_SWRST, MTK_WDT_SWRST_KEY);
-
-	wfi();
-	ERROR("MTK System Reset: operation not handled.\n");
-	panic();
-}
-
-/*******************************************************************************
- * Export the platform handlers to enable psci to invoke them
- ******************************************************************************/
-static const plat_pm_ops_t plat_plat_pm_ops = {
-	.affinst_standby		= plat_affinst_standby,
-	.affinst_on			= plat_affinst_on,
-	.affinst_off			= plat_affinst_off,
-	.affinst_suspend		= plat_affinst_suspend,
-	.affinst_on_finish		= plat_affinst_on_finish,
-	.affinst_suspend_finish		= plat_affinst_suspend_finish,
-	.system_off			= plat_system_off,
-	.system_reset			= plat_system_reset,
-	.get_sys_suspend_power_state	= plat_get_sys_suspend_power_state,
-};
-
-/*******************************************************************************
- * Export the platform specific power ops & initialize the mtk_platform power
- * controller
- ******************************************************************************/
-int platform_setup_pm(const plat_pm_ops_t **plat_ops)
-{
-	*plat_ops = &plat_plat_pm_ops;
-	return 0;
-}
diff --git a/plat/mediatek/mt6795/plat_topology.c b/plat/mediatek/mt6795/plat_topology.c
deleted file mode 100644
index 7425d26..0000000
--- a/plat/mediatek/mt6795/plat_topology.c
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <platform_def.h>
-
-#include <arch.h>
-#include <lib/psci/psci.h>
-
-unsigned int plat_get_aff_count(unsigned int aff_lvl, unsigned long mpidr)
-{
-	/* Report 1 (absent) instance at levels higher that the cluster level */
-	if (aff_lvl > MPIDR_AFFLVL1)
-		return PLATFORM_SYSTEM_COUNT;
-
-	if (aff_lvl == MPIDR_AFFLVL1)
-		return PLATFORM_CLUSTER_COUNT;
-
-	return mpidr & 0x100 ? PLATFORM_CLUSTER1_CORE_COUNT :
-			       PLATFORM_CLUSTER0_CORE_COUNT;
-}
-
-unsigned int plat_get_aff_state(unsigned int aff_lvl, unsigned long mpidr)
-{
-	return aff_lvl <= MPIDR_AFFLVL2 ? PSCI_AFF_PRESENT : PSCI_AFF_ABSENT;
-}
-
-int mt_setup_topology(void)
-{
-	/* [TODO] Make topology configurable via SCC */
-	return 0;
-}
diff --git a/plat/mediatek/mt6795/platform.mk b/plat/mediatek/mt6795/platform.mk
deleted file mode 100644
index 4ab692d..0000000
--- a/plat/mediatek/mt6795/platform.mk
+++ /dev/null
@@ -1,67 +0,0 @@
-#
-# Copyright (c) 2016-2017, ARM Limited and Contributors. All rights reserved.
-#
-# SPDX-License-Identifier: BSD-3-Clause
-#
-
-MTK_PLAT		:=	plat/mediatek
-MTK_PLAT_SOC		:=	${MTK_PLAT}/${PLAT}
-
-# Add OEM customized codes
-OEMS				:= true
-MTK_SIP_KERNEL_BOOT_ENABLE := 1
-
-
-ifneq (${OEMS},none)
-  OEMS_INCLUDES		:= -I${MTK_PLAT}/common/custom/
-  OEMS_SOURCES		:=	${MTK_PLAT}/common/custom/oem_svc.c
-endif
-
-PLAT_INCLUDES		:=	-I${MTK_PLAT}/common/				\
-				-I${MTK_PLAT}/common/drivers/uart			\
-				-I${MTK_PLAT_SOC}/				\
-				-I${MTK_PLAT_SOC}/drivers/timer/			\
-				-I${MTK_PLAT_SOC}/include/					\
-				-Iinclude/plat/arm/common/					\
-				${OEMS_INCLUDES}
-
-PLAT_BL_COMMON_SOURCES	:=	lib/xlat_tables/aarch64/xlat_tables.c			\
-				lib/xlat_tables/xlat_tables_common.c			\
-				plat/common/plat_gic.c
-
-BL31_SOURCES		+=	drivers/arm/cci/cci.c				\
-				drivers/delay_timer/generic_delay_timer.c	\
-				drivers/arm/gic/common/gic_common.c		\
-				drivers/arm/gic/v2/gicv2_main.c			\
-				drivers/arm/gic/v2/gicv2_helpers.c		\
-				plat/common/plat_gicv2.c			\
-				drivers/console/aarch64/console.S		\
-				drivers/delay_timer/delay_timer.c		\
-				lib/cpus/aarch64/cortex_a53.S			\
-				${MTK_PLAT_SOC}/bl31_plat_setup.c		\
-				${MTK_PLAT_SOC}/plat_mt_gic.c			\
-				${MTK_PLAT}/common/mtk_sip_svc.c		\
-				${MTK_PLAT}/common/mtk_plat_common.c		\
-				${MTK_PLAT}/common/drivers/uart/8250_console.S		\
-				${MTK_PLAT_SOC}/aarch64/plat_helpers.S		\
-				${MTK_PLAT_SOC}/drivers/timer/mt_cpuxgpt.c	\
-				${MTK_PLAT_SOC}/plat_delay_timer.c		\
-				${MTK_PLAT_SOC}/plat_pm.c			\
-				${MTK_PLAT_SOC}/plat_topology.c			\
-				${MTK_PLAT_SOC}/power_tracer.c			\
-				${MTK_PLAT_SOC}/scu.c		\
-				${OEMS_SOURCES}
-
-# Enable workarounds for selected Cortex-A53 erratas.
-ERRATA_A53_826319	:=	1
-ERRATA_A53_836870	:=	1
-
-WORKAROUND_CVE_2017_5715	:=	0
-
-# indicate the reset vector address can be programmed
-PROGRAMMABLE_RESET_ADDRESS	:=	1
-
-$(eval $(call add_define,MTK_SIP_KERNEL_BOOT_ENABLE))
-
-# Do not enable SVE
-ENABLE_SVE_FOR_NS	:=	0
diff --git a/plat/mediatek/mt6795/power_tracer.c b/plat/mediatek/mt6795/power_tracer.c
deleted file mode 100644
index 64d086d..0000000
--- a/plat/mediatek/mt6795/power_tracer.c
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <arch.h>
-#include <common/debug.h>
-
-#include <power_tracer.h>
-
-#define trace_log(...)  INFO("psci: " __VA_ARGS__)
-
-void trace_power_flow(unsigned long mpidr, unsigned char mode)
-{
-	switch (mode) {
-	case CPU_UP:
-		trace_log("core %lld:%lld ON\n",
-			  (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS,
-			  (mpidr & MPIDR_CPU_MASK));
-		break;
-	case CPU_DOWN:
-		trace_log("core %lld:%lld OFF\n",
-			  (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS,
-			  (mpidr & MPIDR_CPU_MASK));
-		break;
-	case CPU_SUSPEND:
-		trace_log("core %lld:%lld SUSPEND\n",
-			  (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS,
-			  (mpidr & MPIDR_CPU_MASK));
-		break;
-	case CLUSTER_UP:
-		trace_log("cluster %lld ON\n",
-			  (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS);
-		break;
-	case CLUSTER_DOWN:
-		trace_log("cluster %lld OFF\n",
-			  (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS);
-		break;
-	case CLUSTER_SUSPEND:
-		trace_log("cluster %lld SUSPEND\n",
-			  (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS);
-		break;
-	default:
-		trace_log("unknown power mode\n");
-		break;
-	}
-}
diff --git a/plat/mediatek/mt6795/scu.c b/plat/mediatek/mt6795/scu.c
deleted file mode 100644
index 3b74527..0000000
--- a/plat/mediatek/mt6795/scu.c
+++ /dev/null
@@ -1,30 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <arch.h>
-#include <lib/mmio.h>
-
-#include <mcucfg.h>
-
-void disable_scu(unsigned long mpidr)
-{
-	if (mpidr & MPIDR_CLUSTER_MASK)
-		mmio_setbits_32((uintptr_t)&mt6795_mcucfg->mp1_miscdbg,
-			MP1_ACINACTM);
-	else
-		mmio_setbits_32((uintptr_t)&mt6795_mcucfg->mp0_axi_config,
-			MP0_ACINACTM);
-}
-
-void enable_scu(unsigned long mpidr)
-{
-	if (mpidr & MPIDR_CLUSTER_MASK)
-		mmio_clrbits_32((uintptr_t)&mt6795_mcucfg->mp1_miscdbg,
-			MP1_ACINACTM);
-	else
-		mmio_clrbits_32((uintptr_t)&mt6795_mcucfg->mp0_axi_config,
-			MP0_ACINACTM);
-}
diff --git a/plat/st/common/bl2_io_storage.c b/plat/st/common/bl2_io_storage.c
index 5cc3390..b2038bc 100644
--- a/plat/st/common/bl2_io_storage.c
+++ b/plat/st/common/bl2_io_storage.c
@@ -38,6 +38,7 @@
 #include <platform_def.h>
 #include <stm32cubeprogrammer.h>
 #include <stm32mp_fconf_getter.h>
+#include <stm32mp_io_storage.h>
 #include <usb_dfu.h>
 
 /* IO devices */
@@ -409,6 +410,7 @@
 			gpt_init_done = true;
 		} else {
 			bl_mem_params_node_t *bl_mem_params = get_bl_mem_params_node(image_id);
+			assert(bl_mem_params != NULL);
 
 			mmc_block_dev_spec.buffer.offset = bl_mem_params->image_info.image_base;
 			mmc_block_dev_spec.buffer.length = bl_mem_params->image_info.image_max_size;
diff --git a/plat/st/stm32mp1/bl2_plat_setup.c b/plat/st/stm32mp1/bl2_plat_setup.c
index 88d0f8a..6f5fcc7 100644
--- a/plat/st/stm32mp1/bl2_plat_setup.c
+++ b/plat/st/stm32mp1/bl2_plat_setup.c
@@ -432,7 +432,8 @@
 #if !STM32MP_USE_STM32IMAGE
 	case FW_CONFIG_ID:
 		/* Set global DTB info for fixed fw_config information */
-		set_config_info(STM32MP_FW_CONFIG_BASE, STM32MP_FW_CONFIG_MAX_SIZE, FW_CONFIG_ID);
+		set_config_info(STM32MP_FW_CONFIG_BASE, ~0UL, STM32MP_FW_CONFIG_MAX_SIZE,
+				FW_CONFIG_ID);
 		fconf_populate("FW_CONFIG", STM32MP_FW_CONFIG_BASE);
 
 		idx = dyn_cfg_dtb_info_get_index(TOS_FW_CONFIG_ID);
@@ -462,12 +463,14 @@
 
 				/* In case of OPTEE, initialize address space with tos_fw addr */
 				pager_mem_params = get_bl_mem_params_node(BL32_EXTRA1_IMAGE_ID);
+				assert(pager_mem_params != NULL);
 				pager_mem_params->image_info.image_base = config_info->config_addr;
 				pager_mem_params->image_info.image_max_size =
 					config_info->config_max_size;
 
 				/* Init base and size for pager if exist */
 				paged_mem_params = get_bl_mem_params_node(BL32_EXTRA2_IMAGE_ID);
+				assert(paged_mem_params != NULL);
 				paged_mem_params->image_info.image_base = STM32MP_DDR_BASE +
 					(dt_get_ddr_size() - STM32MP_DDR_S_SIZE -
 					 STM32MP_DDR_SHMEM_SIZE);
@@ -525,6 +528,7 @@
 #if !STM32MP_USE_STM32IMAGE
 			bl_mem_params->ep_info.pc = bl_mem_params->image_info.image_base;
 			tos_fw_mem_params = get_bl_mem_params_node(TOS_FW_CONFIG_ID);
+			assert(tos_fw_mem_params != NULL);
 			bl_mem_params->image_info.image_max_size +=
 				tos_fw_mem_params->image_info.image_max_size;
 #endif /* !STM32MP_USE_STM32IMAGE */
diff --git a/plat/st/stm32mp1/plat_image_load.c b/plat/st/stm32mp1/plat_image_load.c
index 36a3a1c..76af0fc 100644
--- a/plat/st/stm32mp1/plat_image_load.c
+++ b/plat/st/stm32mp1/plat_image_load.c
@@ -1,14 +1,14 @@
 /*
- * 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
  */
 
-#include <platform_def.h>
-
 #include <common/desc_image_load.h>
 #include <plat/common/platform.h>
 
+#include <platform_def.h>
+
 /*******************************************************************************
  * This function flushes the data structures so that they are visible
  * in memory for the next BL image.
@@ -27,6 +27,8 @@
 	bl_mem_params_node_t *bl33 = get_bl_mem_params_node(BL33_IMAGE_ID);
 	uint32_t ddr_ns_size = stm32mp_get_ddr_ns_size();
 
+	assert(bl33 != NULL);
+
 	/* Max size is non-secure DDR end address minus image_base */
 	bl33->image_info.image_max_size = STM32MP_DDR_BASE + ddr_ns_size -
 					  bl33->image_info.image_base;
diff --git a/plat/xilinx/common/ipi.c b/plat/xilinx/common/ipi.c
index 0b8020b..2f52f38 100644
--- a/plat/xilinx/common/ipi.c
+++ b/plat/xilinx/common/ipi.c
@@ -69,8 +69,9 @@
 {
 	int ret = 1;
 
-	if (remote >= ipi_total || local >= ipi_total)
+	if (remote >= ipi_total || local >= ipi_total) {
 		ret = 0;
+	}
 
 	return ret;
 }
@@ -88,12 +89,15 @@
 {
 	int ret = 0;
 
-	if (!is_ipi_mb_within_range(local, remote))
+	if (!is_ipi_mb_within_range(local, remote)) {
 		ret = -EINVAL;
-	else if (IPI_IS_SECURE(local) && !is_secure)
+	} else if (IPI_IS_SECURE(local) && !is_secure) {
 		ret = -EPERM;
-	else if (IPI_IS_SECURE(remote) && !is_secure)
+	} else if (IPI_IS_SECURE(remote) && !is_secure) {
 		ret = -EPERM;
+	} else {
+		/* To fix the misra 15.7 warning */
+	}
 
 	return ret;
 }
@@ -141,11 +145,13 @@
 	uint32_t status;
 
 	status = mmio_read_32(IPI_REG_BASE(local) + IPI_OBR_OFFSET);
-	if (status & IPI_BIT_MASK(remote))
+	if (status & IPI_BIT_MASK(remote)) {
 		ret |= IPI_MB_STATUS_SEND_PENDING;
+	}
 	status = mmio_read_32(IPI_REG_BASE(local) + IPI_ISR_OFFSET);
-	if (status & IPI_BIT_MASK(remote))
+	if (status & IPI_BIT_MASK(remote)) {
 		ret |= IPI_MB_STATUS_RECV_PENDING;
+	}
 
 	return ret;
 }
diff --git a/plat/xilinx/common/plat_startup.c b/plat/xilinx/common/plat_startup.c
index f02f41e..b8f88c4 100644
--- a/plat/xilinx/common/plat_startup.c
+++ b/plat/xilinx/common/plat_startup.c
@@ -123,10 +123,11 @@
 
 	flags >>= FSBL_FLAGS_ENDIAN_SHIFT;
 
-	if (flags == FSBL_FLAGS_ENDIAN_BE)
+	if (flags == FSBL_FLAGS_ENDIAN_BE) {
 		return SPSR_E_BIG;
-	else
+	} else {
 		return SPSR_E_LITTLE;
+	}
 }
 
 /**
@@ -226,30 +227,33 @@
 		if (target_secure == FSBL_FLAGS_SECURE) {
 			image = bl32;
 
-			if (target_estate == FSBL_FLAGS_ESTATE_A32)
+			if (target_estate == FSBL_FLAGS_ESTATE_A32) {
 				bl32->spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM,
 							 target_endianness,
 							 DISABLE_ALL_EXCEPTIONS);
-			else
+			} else {
 				bl32->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX,
 						     DISABLE_ALL_EXCEPTIONS);
+			}
 		} else {
 			image = bl33;
 
 			if (target_estate == FSBL_FLAGS_ESTATE_A32) {
-				if (target_el == FSBL_FLAGS_EL2)
+				if (target_el == FSBL_FLAGS_EL2) {
 					target_el = MODE32_hyp;
-				else
+				} else {
 					target_el = MODE32_sys;
+				}
 
 				bl33->spsr = SPSR_MODE32(target_el, SPSR_T_ARM,
 							 target_endianness,
 							 DISABLE_ALL_EXCEPTIONS);
 			} else {
-				if (target_el == FSBL_FLAGS_EL2)
+				if (target_el == FSBL_FLAGS_EL2) {
 					target_el = MODE_EL2;
-				else
+				} else {
 					target_el = MODE_EL1;
+				}
 
 				bl33->spsr = SPSR_64(target_el, MODE_SP_ELX,
 						     DISABLE_ALL_EXCEPTIONS);
@@ -262,10 +266,11 @@
 			target_el);
 		image->pc = ATFHandoffParams->partition[i].entry_point;
 
-		if (target_endianness == SPSR_E_BIG)
+		if (target_endianness == SPSR_E_BIG) {
 			EP_SET_EE(image->h.attr, EP_EE_BIG);
-		else
+		} else {
 			EP_SET_EE(image->h.attr, EP_EE_LITTLE);
+		}
 	}
 
 	return FSBL_HANDOFF_SUCCESS;
diff --git a/plat/xilinx/common/pm_service/pm_ipi.c b/plat/xilinx/common/pm_service/pm_ipi.c
index e362347..1d1ba85 100644
--- a/plat/xilinx/common/pm_service/pm_ipi.c
+++ b/plat/xilinx/common/pm_service/pm_ipi.c
@@ -154,14 +154,16 @@
 		value++;
 	}
 #if IPI_CRC_CHECK
-	for (j = 0; j < PAYLOAD_ARG_CNT; j++)
+	for (j = 0; j < PAYLOAD_ARG_CNT; j++) {
 		response_payload[j] = mmio_read_32(buffer_base +
 						(j * PAYLOAD_ARG_SIZE));
+	}
 
 	if (response_payload[PAYLOAD_CRC_POS] !=
-			calculate_crc(response_payload, IPI_W0_TO_W6_SIZE))
+			calculate_crc(response_payload, IPI_W0_TO_W6_SIZE)) {
 		NOTICE("ERROR in CRC response payload value:0x%x\n",
 					response_payload[PAYLOAD_CRC_POS]);
+	}
 #endif
 
 	return mmio_read_32(buffer_base);
@@ -186,22 +188,25 @@
 				IPI_BUFFER_TARGET_LOCAL_OFFSET +
 				IPI_BUFFER_REQ_OFFSET;
 
-	if (count > IPI_BUFFER_MAX_WORDS)
+	if (count > IPI_BUFFER_MAX_WORDS) {
 		count = IPI_BUFFER_MAX_WORDS;
+	}
 
 	for (i = 0; i <= count; i++) {
 		*value = mmio_read_32(buffer_base + (i * PAYLOAD_ARG_SIZE));
 		value++;
 	}
 #if IPI_CRC_CHECK
-	for (j = 0; j < PAYLOAD_ARG_CNT; j++)
+	for (j = 0; j < PAYLOAD_ARG_CNT; j++) {
 		response_payload[j] = mmio_read_32(buffer_base +
 						(j * PAYLOAD_ARG_SIZE));
+	}
 
 	if (response_payload[PAYLOAD_CRC_POS] !=
-			calculate_crc(response_payload, IPI_W0_TO_W6_SIZE))
+			calculate_crc(response_payload, IPI_W0_TO_W6_SIZE)) {
 		NOTICE("ERROR in CRC response payload value:0x%x\n",
 					response_payload[PAYLOAD_CRC_POS]);
+	}
 #endif
 }
 
@@ -226,8 +231,9 @@
 	bakery_lock_get(&pm_secure_lock);
 
 	ret = pm_ipi_send_common(proc, payload, IPI_BLOCKING);
-	if (ret != PM_RET_SUCCESS)
+	if (ret != PM_RET_SUCCESS) {
 		goto unlock;
+	}
 
 	ret = ERROR_CODE_MASK & (pm_ipi_buff_read(proc, value, count));
 
@@ -253,10 +259,11 @@
 
 	ret = ipi_mb_enquire_status(proc->ipi->local_ipi_id,
 				    proc->ipi->remote_ipi_id);
-	if (ret & IPI_MB_STATUS_RECV_PENDING)
+	if (ret & IPI_MB_STATUS_RECV_PENDING) {
 		return 1;
-	else
+	} else {
 		return 0;
+	}
 }
 
 #if IPI_CRC_CHECK
diff --git a/plat/xilinx/versal/include/versal_def.h b/plat/xilinx/versal/include/versal_def.h
index 9372954..731742d 100644
--- a/plat/xilinx/versal/include/versal_def.h
+++ b/plat/xilinx/versal/include/versal_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -20,6 +20,8 @@
 
 /* List all supported platforms */
 #define VERSAL_PLATFORM_ID_versal_virt	1
+#define VERSAL_PLATFORM_ID_spp_itr6	2
+#define VERSAL_PLATFORM_ID_emu_itr6	3
 #define VERSAL_PLATFORM_ID_silicon	4
 
 #define VERSAL_PLATFORM_IS(con)	(VERSAL_PLATFORM_ID_ ## con == VERSAL_PLATFORM)
@@ -92,6 +94,16 @@
 # define VERSAL_UART_CLOCK	100000000
 # define VERSAL_UART_BAUDRATE	115200
 # define VERSAL_CPU_CLOCK	100000000
+#elif VERSAL_PLATFORM_IS(spp_itr6)
+# define PLATFORM_NAME          "SPP ITR6"
+# define VERSAL_UART_CLOCK      25000000
+# define VERSAL_UART_BAUDRATE   115200
+# define VERSAL_CPU_CLOCK       2720000
+#elif VERSAL_PLATFORM_IS(emu_itr6)
+# define PLATFORM_NAME          "EMU ITR6"
+# define VERSAL_UART_CLOCK      212000
+# define VERSAL_UART_BAUDRATE   9600
+# define VERSAL_CPU_CLOCK       212000
 #endif
 
 /* Access control register defines */
diff --git a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
index fae73cf..32b0123 100644
--- a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
+++ b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
@@ -1,5 +1,5 @@
 /*
- * 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
  */
@@ -47,10 +47,11 @@
 {
 	unsigned int ver = zynqmp_get_silicon_ver();
 
-	if (ver == ZYNQMP_CSU_VERSION_QEMU)
+	if (ver == ZYNQMP_CSU_VERSION_QEMU) {
 		return 133000000;
-	else
+	} else {
 		return 100000000;
+	}
 }
 
 #if LOG_LEVEL >= LOG_LEVEL_NOTICE
@@ -232,8 +233,9 @@
 	chipid[0] = mmio_read_32(ZYNQMP_CSU_BASEADDR + ZYNQMP_CSU_IDCODE_OFFSET);
 	chipid[1] = mmio_read_32(EFUSE_BASEADDR + EFUSE_IPDISABLE_OFFSET);
 #else
-	if (pm_get_chipid(chipid) != PM_RET_SUCCESS)
+	if (pm_get_chipid(chipid) != PM_RET_SUCCESS) {
 		return "XCZUUNKN";
+	}
 #endif
 
 	id = chipid[0] & (ZYNQMP_CSU_IDCODE_DEVICE_CODE_MASK |
@@ -243,8 +245,9 @@
 
 	for (i = 0; i < ARRAY_SIZE(zynqmp_devices); i++) {
 		if (zynqmp_devices[i].id == id &&
-		    zynqmp_devices[i].ver == (ver & ZYNQMP_CSU_VERSION_MASK))
+		    zynqmp_devices[i].ver == (ver & ZYNQMP_CSU_VERSION_MASK)) {
 			break;
+		}
 	}
 
 	if (i >= ARRAY_SIZE(zynqmp_devices)) {
@@ -255,11 +258,13 @@
 		}
 	}
 
-	if (!zynqmp_devices[i].evexists)
+	if (!zynqmp_devices[i].evexists) {
 		return zynqmp_devices[i].name;
+	}
 
-	if (ver & ZYNQMP_PL_STATUS_MASK)
+	if ((ver & ZYNQMP_PL_STATUS_MASK) != 0U) {
 		return zynqmp_devices[i].name;
+	}
 
 	len = strlen(zynqmp_devices[i].name) - 2;
 	for (j = 0; j < strlen(name); j++) {
@@ -329,8 +334,8 @@
 		break;
 	}
 
-	NOTICE("TF-A running on %s/%s at 0x%x\n",
-	       zynqmp_print_silicon_idcode(), label, BL31_BASE);
+	VERBOSE("TF-A running on %s/%s at 0x%x\n",
+		zynqmp_print_silicon_idcode(), label, BL31_BASE);
 	VERBOSE("TF-A running on v%d/RTL%d.%d\n",
 	       zynqmp_get_ps_ver(), (rtl & 0xf0) >> 4, rtl & 0xf);
 }
@@ -345,8 +350,9 @@
 
 	ret = pm_mmio_read(CRL_APB_BOOT_MODE_USER, &r);
 
-	if (ret != PM_RET_SUCCESS)
+	if (ret != PM_RET_SUCCESS) {
 		r = mmio_read_32(CRL_APB_BOOT_MODE_USER);
+	}
 
 	return r & CRL_APB_BOOT_MODE_MASK;
 }
@@ -373,8 +379,9 @@
 {
 	unsigned int ver = zynqmp_get_silicon_ver();
 
-	if (ver == ZYNQMP_CSU_VERSION_QEMU)
+	if (ver == ZYNQMP_CSU_VERSION_QEMU) {
 		return 65000000;
-	else
+	} else {
 		return mmio_read_32(IOU_SCNTRS_BASEFREQ);
+	}
 }
diff --git a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
index 58eee3a..5ad33cc 100644
--- a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
+++ b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
@@ -83,6 +83,8 @@
 		if (rc == 0) {
 			panic();
 		}
+	} else {
+		ERROR("BL31: No console device found.\n");
 	}
 	/* Initialize the platform config for future decision making */
 	zynqmp_config_setup();
@@ -119,10 +121,10 @@
 			panic();
 		}
 	}
-	if (bl32_image_ep_info.pc) {
+	if (bl32_image_ep_info.pc != 0) {
 		VERBOSE("BL31: Secure code at 0x%lx\n", bl32_image_ep_info.pc);
 	}
-	if (bl33_image_ep_info.pc) {
+	if (bl33_image_ep_info.pc != 0) {
 		VERBOSE("BL31: Non secure code at 0x%lx\n", bl33_image_ep_info.pc);
 	}
 }
diff --git a/plat/xilinx/zynqmp/include/platform_def.h b/plat/xilinx/zynqmp/include/platform_def.h
index 1c4daa1..9c1600a 100644
--- a/plat/xilinx/zynqmp/include/platform_def.h
+++ b/plat/xilinx/zynqmp/include/platform_def.h
@@ -37,11 +37,11 @@
  */
 #ifndef ZYNQMP_ATF_MEM_BASE
 #if !DEBUG && defined(SPD_none) && !SDEI_SUPPORT
-# define BL31_BASE			0xfffea000
-# define BL31_LIMIT			0x100000000
+# define BL31_BASE			U(0xfffea000)
+# define BL31_LIMIT			U(0x100000000)
 #else
-# define BL31_BASE			0x1000
-# define BL31_LIMIT			0x7ffff
+# define BL31_BASE			U(0x1000)
+# define BL31_LIMIT			U(0x7ffff)
 #endif
 #else
 # define BL31_BASE			(ZYNQMP_ATF_MEM_BASE)
@@ -55,8 +55,8 @@
  * BL32 specific defines.
  ******************************************************************************/
 #ifndef ZYNQMP_BL32_MEM_BASE
-# define BL32_BASE			0x60000000
-# define BL32_LIMIT			0x7fffffff
+# define BL32_BASE			U(0x60000000)
+# define BL32_LIMIT			U(0x7fffffff)
 #else
 # define BL32_BASE			(ZYNQMP_BL32_MEM_BASE)
 # define BL32_LIMIT			(ZYNQMP_BL32_MEM_BASE + ZYNQMP_BL32_MEM_SIZE - 1)
@@ -66,7 +66,7 @@
  * BL33 specific defines.
  ******************************************************************************/
 #ifndef PRELOADED_BL33_BASE
-# define PLAT_ARM_NS_IMAGE_BASE	0x8000000
+# define PLAT_ARM_NS_IMAGE_BASE	U(0x8000000)
 #else
 # define PLAT_ARM_NS_IMAGE_BASE	PRELOADED_BL33_BASE
 #endif
@@ -83,9 +83,9 @@
 /*******************************************************************************
  * Platform specific page table and MMU setup constants
  ******************************************************************************/
-#define XILINX_OF_BOARD_DTB_ADDR	0x100000
-#define XILINX_OF_BOARD_DTB_MAX_SIZE	0x200000
-#define PLAT_DDR_LOWMEM_MAX		0x80000000
+#define XILINX_OF_BOARD_DTB_ADDR	U(0x100000)
+#define XILINX_OF_BOARD_DTB_MAX_SIZE	U(0x200000)
+#define PLAT_DDR_LOWMEM_MAX		U(0x80000000)
 
 #define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 32)
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 32)
diff --git a/plat/xilinx/zynqmp/include/zynqmp_def.h b/plat/xilinx/zynqmp/include/zynqmp_def.h
index 7e58391..19b6937 100644
--- a/plat/xilinx/zynqmp/include/zynqmp_def.h
+++ b/plat/xilinx/zynqmp/include/zynqmp_def.h
@@ -74,11 +74,11 @@
 #define ZYNQMP_ULPI_RESET_VAL_LOW	CRL_APB_BOOT_ENABLE_PIN_1
 
 /* system counter registers and bitfields */
-#define IOU_SCNTRS_BASE			0xFF260000
+#define IOU_SCNTRS_BASE			U(0xFF260000)
 #define IOU_SCNTRS_BASEFREQ		(IOU_SCNTRS_BASE + 0x20)
 
 /* APU registers and bitfields */
-#define APU_BASE		0xFD5C0000
+#define APU_BASE		U(0xFD5C0000)
 #define APU_CONFIG_0		(APU_BASE + 0x20)
 #define APU_RVBAR_L_0		(APU_BASE + 0x40)
 #define APU_RVBAR_H_0		(APU_BASE + 0x44)
@@ -91,7 +91,7 @@
 #define APU_3_PWRCTL_CPUPWRDWNREQ_MASK		8
 
 /* PMU registers and bitfields */
-#define PMU_GLOBAL_BASE			0xFFD80000
+#define PMU_GLOBAL_BASE			U(0xFFD80000)
 #define PMU_GLOBAL_CNTRL		(PMU_GLOBAL_BASE + 0)
 #define PMU_GLOBAL_GEN_STORAGE6		(PMU_GLOBAL_BASE + 0x48)
 #define PMU_GLOBAL_REQ_PWRUP_STATUS	(PMU_GLOBAL_BASE + 0x110)
@@ -104,22 +104,22 @@
 /*******************************************************************************
  * CCI-400 related constants
  ******************************************************************************/
-#define PLAT_ARM_CCI_BASE		0xFD6E0000
+#define PLAT_ARM_CCI_BASE		U(0xFD6E0000)
 #define PLAT_ARM_CCI_CLUSTER0_SL_IFACE_IX	3
 #define PLAT_ARM_CCI_CLUSTER1_SL_IFACE_IX	4
 
 /*******************************************************************************
  * GIC-400 & interrupt handling related constants
  ******************************************************************************/
-#define BASE_GICD_BASE		0xF9010000
-#define BASE_GICC_BASE		0xF9020000
-#define BASE_GICH_BASE		0xF9040000
-#define BASE_GICV_BASE		0xF9060000
+#define BASE_GICD_BASE		U(0xF9010000)
+#define BASE_GICC_BASE		U(0xF9020000)
+#define BASE_GICH_BASE		U(0xF9040000)
+#define BASE_GICV_BASE		U(0xF9060000)
 
 #if ZYNQMP_WDT_RESTART
 #define IRQ_SEC_IPI_APU		67
 #define IRQ_TTC3_1		77
-#define TTC3_BASE_ADDR		0xFF140000
+#define TTC3_BASE_ADDR		U(0xFF140000)
 #define TTC3_INTR_REGISTER_1	(TTC3_BASE_ADDR + 0x54)
 #define TTC3_INTR_ENABLE_1	(TTC3_BASE_ADDR + 0x60)
 #endif
@@ -140,8 +140,8 @@
 /*******************************************************************************
  * UART related constants
  ******************************************************************************/
-#define ZYNQMP_UART0_BASE		0xFF000000
-#define ZYNQMP_UART1_BASE		0xFF010000
+#define ZYNQMP_UART0_BASE		U(0xFF000000)
+#define ZYNQMP_UART1_BASE		U(0xFF010000)
 
 #if ZYNQMP_CONSOLE_IS(cadence) || ZYNQMP_CONSOLE_IS(dcc)
 # define ZYNQMP_UART_BASE	ZYNQMP_UART0_BASE
@@ -169,7 +169,7 @@
 #define ZYNQMP_PS_VER_MASK		0xF
 #define ZYNQMP_PS_VER_SHIFT		0
 
-#define ZYNQMP_CSU_BASEADDR		0xFFCA0000
+#define ZYNQMP_CSU_BASEADDR		U(0xFFCA0000)
 #define ZYNQMP_CSU_IDCODE_OFFSET	0x40
 
 #define ZYNQMP_CSU_IDCODE_XILINX_ID_SHIFT	0
@@ -199,7 +199,7 @@
 #define ZYNQMP_CSU_VERSION_OFFSET	0x44
 
 /* Efuse */
-#define EFUSE_BASEADDR		0xFFCC0000
+#define EFUSE_BASEADDR		U(0xFFCC0000)
 #define EFUSE_IPDISABLE_OFFSET	0x1018
 #define EFUSE_IPDISABLE_VERSION	0x1FFU
 #define ZYNQMP_EFUSE_IPDISABLE_SHIFT	20
@@ -357,9 +357,9 @@
 #define  FABRIC_WIDTH		U(3)
 
 /* CSUDMA Module Base Address*/
-#define CSUDMA_BASE		0xFFC80000
+#define CSUDMA_BASE		U(0xFFC80000)
 
 /* RSA-CORE Module Base Address*/
-#define RSA_CORE_BASE		0xFFCE0000
+#define RSA_CORE_BASE		U(0xFFCE0000)
 
 #endif /* ZYNQMP_DEF_H */
diff --git a/plat/xilinx/zynqmp/plat_psci.c b/plat/xilinx/zynqmp/plat_psci.c
index b2b473a..881dfe6 100644
--- a/plat/xilinx/zynqmp/plat_psci.c
+++ b/plat/xilinx/zynqmp/plat_psci.c
@@ -38,9 +38,9 @@
 
 	VERBOSE("%s: mpidr: 0x%lx\n", __func__, mpidr);
 
-	if (cpu_id == -1)
+	if (cpu_id == -1) {
 		return PSCI_E_INTERN_FAIL;
-
+	}
 	proc = pm_get_proc(cpu_id);
 
 	/* Check the APU proc status before wakeup */
@@ -63,9 +63,10 @@
 	unsigned int cpu_id = plat_my_core_pos();
 	const struct pm_proc *proc = pm_get_proc(cpu_id);
 
-	for (size_t i = 0; i <= PLAT_MAX_PWR_LVL; i++)
+	for (size_t i = 0; i <= PLAT_MAX_PWR_LVL; i++) {
 		VERBOSE("%s: target_state->pwr_domain_state[%lu]=%x\n",
 			__func__, i, target_state->pwr_domain_state[i]);
+	}
 
 	/* Prevent interrupts from spuriously waking up this cpu */
 	gicv2_cpuif_disable();
@@ -106,9 +107,10 @@
 
 static void zynqmp_pwr_domain_on_finish(const psci_power_state_t *target_state)
 {
-	for (size_t i = 0; i <= PLAT_MAX_PWR_LVL; i++)
+	for (size_t i = 0; i <= PLAT_MAX_PWR_LVL; i++) {
 		VERBOSE("%s: target_state->pwr_domain_state[%lu]=%x\n",
 			__func__, i, target_state->pwr_domain_state[i]);
+	}
 	plat_arm_gic_pcpu_init();
 	gicv2_cpuif_enable();
 }
@@ -118,9 +120,10 @@
 	unsigned int cpu_id = plat_my_core_pos();
 	const struct pm_proc *proc = pm_get_proc(cpu_id);
 
-	for (size_t i = 0; i <= PLAT_MAX_PWR_LVL; i++)
+	for (size_t i = 0; i <= PLAT_MAX_PWR_LVL; i++) {
 		VERBOSE("%s: target_state->pwr_domain_state[%lu]=%x\n",
 			__func__, i, target_state->pwr_domain_state[i]);
+	}
 
 	/* Clear the APU power control register for this cpu */
 	pm_client_wakeup(proc);
@@ -149,8 +152,9 @@
 	pm_system_shutdown(PMF_SHUTDOWN_TYPE_SHUTDOWN,
 			   pm_get_shutdown_scope());
 
-	while (1)
+	while (1) {
 		wfi();
+	}
 }
 
 static void __dead2 zynqmp_system_reset(void)
@@ -162,8 +166,9 @@
 	pm_system_shutdown(PMF_SHUTDOWN_TYPE_RESET,
 			   pm_get_shutdown_scope());
 
-	while (1)
+	while (1) {
 		wfi();
+	}
 }
 
 int zynqmp_validate_power_state(unsigned int power_state,
@@ -176,14 +181,15 @@
 	assert(req_state);
 
 	/* Sanity check the requested state */
-	if (pstate == PSTATE_TYPE_STANDBY)
+	if (pstate == PSTATE_TYPE_STANDBY) {
 		req_state->pwr_domain_state[MPIDR_AFFLVL0] = PLAT_MAX_RET_STATE;
-	else
+	} else {
 		req_state->pwr_domain_state[MPIDR_AFFLVL0] = PLAT_MAX_OFF_STATE;
-
+	}
 	/* We expect the 'state id' to be zero */
-	if (psci_get_pstate_id(power_state))
+	if (psci_get_pstate_id(power_state)) {
 		return PSCI_E_INVALID_PARAMS;
+	}
 
 	return PSCI_E_SUCCESS;
 }
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_clock.c b/plat/xilinx/zynqmp/pm_service/pm_api_clock.c
index 4109830..1ea741c 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_clock.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_clock.c
@@ -2446,16 +2446,17 @@
  */
 void pm_api_clock_get_name(unsigned int clock_id, char *name)
 {
-	if (clock_id == CLK_MAX)
+	if (clock_id == CLK_MAX) {
 		memcpy(name, END_OF_CLK, sizeof(END_OF_CLK) > CLK_NAME_LEN ?
 					 CLK_NAME_LEN : sizeof(END_OF_CLK));
-	else if (!pm_clock_valid(clock_id))
+	} else if (!pm_clock_valid(clock_id)) {
 		memset(name, 0, CLK_NAME_LEN);
-	else if (clock_id < CLK_MAX_OUTPUT_CLK)
+	} else if (clock_id < CLK_MAX_OUTPUT_CLK) {
 		memcpy(name, clocks[clock_id].name, CLK_NAME_LEN);
-	else
+	} else {
 		memcpy(name, ext_clocks[clock_id - CLK_MAX_OUTPUT_CLK].name,
 		       CLK_NAME_LEN);
+	}
 }
 
 /**
@@ -2480,24 +2481,28 @@
 	unsigned int i;
 	uint16_t typeflags;
 
-	if (!pm_clock_valid(clock_id))
+	if (!pm_clock_valid(clock_id)) {
 		return PM_RET_ERROR_ARGS;
+	}
 
-	if (pm_clock_type(clock_id) != CLK_TYPE_OUTPUT)
+	if (pm_clock_type(clock_id) != CLK_TYPE_OUTPUT) {
 		return PM_RET_ERROR_NOTSUPPORTED;
-
+	}
 
 	memset(topology, 0, CLK_TOPOLOGY_PAYLOAD_LEN);
 	clock_nodes = *clocks[clock_id].nodes;
 	num_nodes = clocks[clock_id].num_nodes;
 
 	/* Skip parent till index */
-	if (index >= num_nodes)
+	if (index >= num_nodes) {
 		return PM_RET_SUCCESS;
+	}
 
 	for (i = 0; i < 3U; i++) {
-		if ((index + i) == num_nodes)
+		if ((index + i) == num_nodes) {
 			break;
+		}
+
 		topology[i] = clock_nodes[index + i].type;
 		topology[i] |= clock_nodes[index + i].clkflags <<
 					CLK_CLKFLAGS_SHIFT;
@@ -2531,11 +2536,13 @@
 	uint8_t num_nodes;
 	unsigned int type, i;
 
-	if (!pm_clock_valid(clock_id))
+	if (!pm_clock_valid(clock_id)) {
 		return PM_RET_ERROR_ARGS;
+	}
 
-	if (pm_clock_type(clock_id) != CLK_TYPE_OUTPUT)
+	if (pm_clock_type(clock_id) != CLK_TYPE_OUTPUT) {
 		return PM_RET_ERROR_NOTSUPPORTED;
+	}
 
 	clock_nodes = *clocks[clock_id].nodes;
 	num_nodes = clocks[clock_id].num_nodes;
@@ -2550,8 +2557,9 @@
 	}
 
 	/* Clock is not fixed clock */
-	if (i == num_nodes)
+	if (i == num_nodes) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	return PM_RET_SUCCESS;
 }
@@ -2580,27 +2588,33 @@
 	unsigned int i;
 	int32_t *clk_parents;
 
-	if (!pm_clock_valid(clock_id))
+	if (!pm_clock_valid(clock_id)) {
 		return PM_RET_ERROR_ARGS;
+	}
 
-	if (pm_clock_type(clock_id) != CLK_TYPE_OUTPUT)
+	if (pm_clock_type(clock_id) != CLK_TYPE_OUTPUT) {
 		return PM_RET_ERROR_NOTSUPPORTED;
+	}
 
 	clk_parents = *clocks[clock_id].parents;
-	if (clk_parents == NULL)
+	if (clk_parents == NULL) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	memset(parents, 0, CLK_PARENTS_PAYLOAD_LEN);
 
 	/* Skip parent till index */
-	for (i = 0; i < index; i++)
-		if (clk_parents[i] == CLK_NA_PARENT)
+	for (i = 0; i < index; i++) {
+		if (clk_parents[i] == CLK_NA_PARENT) {
 			return PM_RET_SUCCESS;
+		}
+	}
 
 	for (i = 0; i < 3; i++) {
 		parents[i] = clk_parents[index + i];
-		if (clk_parents[index + i] == CLK_NA_PARENT)
+		if (clk_parents[index + i] == CLK_NA_PARENT) {
 			break;
+		}
 	}
 
 	return PM_RET_SUCCESS;
@@ -2619,8 +2633,9 @@
 enum pm_ret_status pm_api_clock_get_attributes(unsigned int clock_id,
 					       uint32_t *attr)
 {
-	if (clock_id >= CLK_MAX)
+	if (clock_id >= CLK_MAX) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Clock valid bit */
 	*attr = pm_clock_valid(clock_id);
@@ -2648,8 +2663,9 @@
 	uint32_t i;
 	struct pm_clock_node *nodes;
 
-	if (clock_id >= CLK_MAX_OUTPUT_CLK)
+	if (clock_id >= CLK_MAX_OUTPUT_CLK) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	nodes = *clocks[clock_id].nodes;
 	for (i = 0; i < clocks[clock_id].num_nodes; i++) {
@@ -2738,8 +2754,9 @@
 	uint32_t i;
 
 	for (i = 0; i < ARRAY_SIZE(pm_plls); i++) {
-		if (pm_plls[i].cid == clock_id)
+		if (pm_plls[i].cid == clock_id) {
 			return &pm_plls[i];
+		}
 	}
 
 	return NULL;
@@ -2798,12 +2815,14 @@
  */
 enum pm_ret_status pm_clock_pll_enable(struct pm_pll *pll)
 {
-	if (!pll)
+	if (!pll) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Set the PLL mode according to the buffered mode value */
-	if (pll->mode == PLL_FRAC_MODE)
+	if (pll->mode == PLL_FRAC_MODE) {
 		return pm_pll_set_mode(pll->nid, PM_PLL_MODE_FRACTIONAL);
+	}
 
 	return pm_pll_set_mode(pll->nid, PM_PLL_MODE_INTEGER);
 }
@@ -2819,8 +2838,9 @@
  */
 enum pm_ret_status pm_clock_pll_disable(struct pm_pll *pll)
 {
-	if (!pll)
+	if (!pll) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	return pm_pll_set_mode(pll->nid, PM_PLL_MODE_RESET);
 }
@@ -2842,17 +2862,20 @@
 	enum pm_ret_status status;
 	enum pm_pll_mode mode;
 
-	if (!pll || !state)
+	if (!pll || !state) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	status = pm_pll_get_mode(pll->nid, &mode);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
-	if (mode == PM_PLL_MODE_RESET)
+	if (mode == PM_PLL_MODE_RESET) {
 		*state = 0;
-	else
+	} else {
 		*state = 1;
+	}
 
 	return PM_RET_SUCCESS;
 }
@@ -2873,17 +2896,21 @@
 					   enum clock_id clock_id,
 					   unsigned int parent_index)
 {
-	if (!pll)
+	if (!pll) {
 		return PM_RET_ERROR_ARGS;
-	if (pll->pre_src == clock_id)
+	}
+	if (pll->pre_src == clock_id) {
 		return pm_pll_set_parameter(pll->nid, PM_PLL_PARAM_PRE_SRC,
 					    parent_index);
-	if (pll->post_src == clock_id)
+	}
+	if (pll->post_src == clock_id) {
 		return pm_pll_set_parameter(pll->nid, PM_PLL_PARAM_POST_SRC,
 					    parent_index);
-	if (pll->div2 == clock_id)
+	}
+	if (pll->div2 == clock_id) {
 		return pm_pll_set_parameter(pll->nid, PM_PLL_PARAM_DIV2,
 					    parent_index);
+	}
 
 	return PM_RET_ERROR_ARGS;
 }
@@ -2902,17 +2929,21 @@
 					   enum clock_id clock_id,
 					   unsigned int *parent_index)
 {
-	if (!pll)
+	if (!pll) {
 		return PM_RET_ERROR_ARGS;
-	if (pll->pre_src == clock_id)
+	}
+	if (pll->pre_src == clock_id) {
 		return pm_pll_get_parameter(pll->nid, PM_PLL_PARAM_PRE_SRC,
 					    parent_index);
-	if (pll->post_src == clock_id)
+	}
+	if (pll->post_src == clock_id) {
 		return pm_pll_get_parameter(pll->nid, PM_PLL_PARAM_POST_SRC,
 					    parent_index);
-	if (pll->div2 == clock_id)
+	}
+	if (pll->div2 == clock_id) {
 		return pm_pll_get_parameter(pll->nid, PM_PLL_PARAM_DIV2,
 					    parent_index);
+	}
 	if (pll->bypass == clock_id) {
 		*parent_index = 0;
 		return PM_RET_SUCCESS;
@@ -2935,8 +2966,9 @@
 {
 	struct pm_pll *pll = pm_clock_get_pll(clock_id);
 
-	if (!pll || (mode != PLL_FRAC_MODE && mode != PLL_INT_MODE))
+	if (!pll || (mode != PLL_FRAC_MODE && mode != PLL_INT_MODE)) {
 		return PM_RET_ERROR_ARGS;
+	}
 	pll->mode = mode;
 
 	return PM_RET_SUCCESS;
@@ -2956,8 +2988,9 @@
 {
 	struct pm_pll *pll = pm_clock_get_pll(clock_id);
 
-	if (!pll || !mode)
+	if (!pll || !mode) {
 		return PM_RET_ERROR_ARGS;
+	}
 	*mode = pll->mode;
 
 	return PM_RET_SUCCESS;
@@ -2971,11 +3004,13 @@
  */
 enum pm_ret_status pm_clock_id_is_valid(unsigned int clock_id)
 {
-	if (!pm_clock_valid(clock_id))
+	if (!pm_clock_valid(clock_id)) {
 		return PM_RET_ERROR_ARGS;
+	}
 
-	if (pm_clock_type(clock_id) != CLK_TYPE_OUTPUT)
+	if (pm_clock_type(clock_id) != CLK_TYPE_OUTPUT) {
 		return PM_RET_ERROR_NOTSUPPORTED;
+	}
 
 	return PM_RET_SUCCESS;
 }
@@ -2992,8 +3027,9 @@
 	uint32_t i;
 	struct pm_clock_node *nodes;
 
-	if (clock_id >= CLK_MAX_OUTPUT_CLK)
+	if (clock_id >= CLK_MAX_OUTPUT_CLK) {
 		return 0;
+	}
 
 	nodes = *clocks[clock_id].nodes;
 	for (i = 0; i < clocks[clock_id].num_nodes; i++) {
@@ -3003,6 +3039,8 @@
 		} else if (nodes[i].type == TYPE_DIV2) {
 			if (div_id == PM_CLOCK_DIV1_ID)
 				return 1;
+		} else {
+			/* To fix the misra 15.7 warning */
 		}
 	}
 
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c b/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c
index a87681b..f12143a 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c
@@ -35,10 +35,11 @@
 
 	val = mmio_read_32(ZYNQMP_RPU_GLBL_CNTL);
 	val &= ZYNQMP_SLSPLIT_MASK;
-	if (val == 0)
+	if (val == 0) {
 		*mode = PM_RPU_MODE_LOCKSTEP;
-	else
+	} else {
 		*mode = PM_RPU_MODE_SPLIT;
+	}
 
 	return PM_RET_SUCCESS;
 }
@@ -58,8 +59,9 @@
 {
 	unsigned int val;
 
-	if (mmio_read_32(CRL_APB_RST_LPD_TOP) & CRL_APB_RPU_AMBA_RESET)
+	if (mmio_read_32(CRL_APB_RST_LPD_TOP) & CRL_APB_RPU_AMBA_RESET) {
 		return PM_RET_ERROR_ACCESS;
+	}
 
 	val = mmio_read_32(ZYNQMP_RPU_GLBL_CNTL);
 
@@ -94,21 +96,23 @@
 {
 	unsigned int rpu_cfg_addr, val;
 
-	if (nid == NODE_RPU_0)
+	if (nid == NODE_RPU_0) {
 		rpu_cfg_addr = ZYNQMP_RPU0_CFG;
-	else if (nid == NODE_RPU_1)
+	} else if (nid == NODE_RPU_1) {
 		rpu_cfg_addr = ZYNQMP_RPU1_CFG;
-	else
+	} else {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	val = mmio_read_32(rpu_cfg_addr);
 
-	if (value == PM_RPU_BOOTMEM_LOVEC)
+	if (value == PM_RPU_BOOTMEM_LOVEC) {
 		val &= ~ZYNQMP_VINITHI_MASK;
-	else if (value == PM_RPU_BOOTMEM_HIVEC)
+	} else if (value == PM_RPU_BOOTMEM_HIVEC) {
 		val |= ZYNQMP_VINITHI_MASK;
-	else
+	} else {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	mmio_write_32(rpu_cfg_addr, val);
 
@@ -130,12 +134,13 @@
 
 	val = mmio_read_32(ZYNQMP_RPU_GLBL_CNTL);
 
-	if (value == PM_RPU_TCM_SPLIT)
+	if (value == PM_RPU_TCM_SPLIT) {
 		val &= ~ZYNQMP_TCM_COMB_MASK;
-	else if (value == PM_RPU_TCM_COMB)
+	} else if (value == PM_RPU_TCM_COMB) {
 		val |= ZYNQMP_TCM_COMB_MASK;
-	else
+	} else {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	mmio_write_32(ZYNQMP_RPU_GLBL_CNTL, val);
 
@@ -155,8 +160,9 @@
 						       unsigned int value)
 {
 	if ((value != PM_TAPDELAY_BYPASS_ENABLE &&
-	     value != PM_TAPDELAY_BYPASS_DISABLE) || type >= PM_TAPDELAY_MAX)
+	     value != PM_TAPDELAY_BYPASS_DISABLE) || type >= PM_TAPDELAY_MAX) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	return pm_mmio_write(IOU_TAPDLY_BYPASS, TAP_DELAY_MASK, value << type);
 }
@@ -178,8 +184,9 @@
 	unsigned int val, mask, shift;
 	enum pm_ret_status ret;
 
-	if (value != PM_SGMII_DISABLE && value != PM_SGMII_ENABLE)
+	if (value != PM_SGMII_DISABLE && value != PM_SGMII_ENABLE) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	switch (nid) {
 	case NODE_ETH_0:
@@ -206,8 +213,9 @@
 		mask = SGMII_SD_MASK << SGMII_SD_OFFSET * shift;
 		val = SGMII_PCS_SD_1 << SGMII_SD_OFFSET * shift;
 		ret = pm_mmio_write(IOU_GEM_CTRL, mask, val);
-		if (ret != PM_RET_SUCCESS)
+		if (ret != PM_RET_SUCCESS) {
 			return ret;
+		}
 
 		/* Set the GEM to SGMII mode */
 		mask = GEM_CLK_CTRL_MASK << GEM_CLK_CTRL_OFFSET * shift;
@@ -248,11 +256,13 @@
 	case PM_DLL_RESET_ASSERT:
 	case PM_DLL_RESET_PULSE:
 		ret = pm_mmio_write(ZYNQMP_SD_DLL_CTRL, mask, val);
-		if (ret != PM_RET_SUCCESS)
+		if (ret != PM_RET_SUCCESS) {
 			return ret;
+		}
 
-		if (type == PM_DLL_RESET_ASSERT)
+		if (type == PM_DLL_RESET_ASSERT) {
 			break;
+		}
 		mdelay(1);
 		/* Fallthrough */
 	case PM_DLL_RESET_RELEASE:
@@ -310,31 +320,44 @@
 		ret = pm_mmio_write(ZYNQMP_SD_ITAP_DLY,
 				    (ZYNQMP_SD_ITAPCHGWIN_MASK << shift),
 				    (ZYNQMP_SD_ITAPCHGWIN << shift));
-		if (ret != PM_RET_SUCCESS)
+
+		if (ret != PM_RET_SUCCESS) {
 			goto reset_release;
-		if (value == 0)
+		}
+
+		if (value == 0) {
 			ret = pm_mmio_write(ZYNQMP_SD_ITAP_DLY,
 					    (ZYNQMP_SD_ITAPDLYENA_MASK <<
 					     shift), 0);
-		else
+		} else {
 			ret = pm_mmio_write(ZYNQMP_SD_ITAP_DLY,
 					    (ZYNQMP_SD_ITAPDLYENA_MASK <<
 					    shift), (ZYNQMP_SD_ITAPDLYENA <<
 					    shift));
-		if (ret != PM_RET_SUCCESS)
+		}
+
+		if (ret != PM_RET_SUCCESS) {
 			goto reset_release;
+		}
+
 		ret = pm_mmio_write(ZYNQMP_SD_ITAP_DLY,
 				    (ZYNQMP_SD_ITAPDLYSEL_MASK << shift),
 				    (value << shift));
-		if (ret != PM_RET_SUCCESS)
+
+		if (ret != PM_RET_SUCCESS) {
 			goto reset_release;
+		}
+
 		ret = pm_mmio_write(ZYNQMP_SD_ITAP_DLY,
 				    (ZYNQMP_SD_ITAPCHGWIN_MASK << shift), 0);
 	} else if (type == PM_TAPDELAY_OUTPUT) {
 		ret = pm_mmio_write(ZYNQMP_SD_OTAP_DLY,
 				    (ZYNQMP_SD_OTAPDLYENA_MASK << shift), 0);
-		if (ret != PM_RET_SUCCESS)
+
+		if (ret != PM_RET_SUCCESS) {
 			goto reset_release;
+		}
+
 		ret = pm_mmio_write(ZYNQMP_SD_OTAP_DLY,
 				    (ZYNQMP_SD_OTAPDLYSEL_MASK << shift),
 				    (value << shift));
@@ -401,8 +424,9 @@
 
 	/* Get PLL node ID using PLL clock ID */
 	status = pm_clock_get_pll_node_id(pll, &pll_nid);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
 	return pm_pll_set_parameter(pll_nid, PM_PLL_PARAM_DATA, data);
 }
@@ -425,8 +449,9 @@
 
 	/* Get PLL node ID using PLL clock ID */
 	status = pm_clock_get_pll_node_id(pll, &pll_nid);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
 	return pm_pll_get_parameter(pll_nid, PM_PLL_PARAM_DATA, data);
 }
@@ -444,8 +469,9 @@
 static enum pm_ret_status pm_ioctl_write_ggs(unsigned int index,
 					     unsigned int value)
 {
-	if (index >= GGS_NUM_REGS)
+	if (index >= GGS_NUM_REGS) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	return pm_mmio_write(GGS_BASEADDR + (index << 2),
 			     0xFFFFFFFFU, value);
@@ -464,8 +490,9 @@
 static enum pm_ret_status pm_ioctl_read_ggs(unsigned int index,
 					    unsigned int *value)
 {
-	if (index >= GGS_NUM_REGS)
+	if (index >= GGS_NUM_REGS) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	return pm_mmio_read(GGS_BASEADDR + (index << 2), value);
 }
@@ -483,8 +510,9 @@
 static enum pm_ret_status pm_ioctl_write_pggs(unsigned int index,
 					      unsigned int value)
 {
-	if (index >= PGGS_NUM_REGS)
+	if (index >= PGGS_NUM_REGS) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	return pm_mmio_write(PGGS_BASEADDR + (index << 2),
 			     0xFFFFFFFFU, value);
@@ -503,31 +531,33 @@
 					      unsigned int value)
 {
 	unsigned int mask;
-	unsigned int regarr[] = {0xFD360000,
-				0xFD360014,
-				0xFD370000,
-				0xFD370014,
-				0xFD380000,
-				0xFD380014,
-				0xFD390000,
-				0xFD390014,
-				0xFD3a0000,
-				0xFD3a0014,
-				0xFD3b0000,
-				0xFD3b0014,
-				0xFF9b0000,
-				0xFF9b0014,
-				0xFD615000,
-				0xFF419000,
+	unsigned int regarr[] = {0xFD360000U,
+				0xFD360014U,
+				0xFD370000U,
+				0xFD370014U,
+				0xFD380000U,
+				0xFD380014U,
+				0xFD390000U,
+				0xFD390014U,
+				0xFD3a0000U,
+				0xFD3a0014U,
+				0xFD3b0000U,
+				0xFD3b0014U,
+				0xFF9b0000U,
+				0xFF9b0014U,
+				0xFD615000U,
+				0xFF419000U,
 				};
 
-	if (index >= ARRAY_SIZE(regarr))
+	if (index >= ARRAY_SIZE(regarr)) {
 		return PM_RET_ERROR_ARGS;
+	}
 
-	if (index < AFIFM6_WRCTRL)
+	if (index < AFIFM6_WRCTRL) {
 		mask = FABRIC_WIDTH;
-	else
+	} else {
 		mask = 0xf00;
+	}
 
 	return pm_mmio_write(regarr[index], mask, value);
 }
@@ -545,8 +575,9 @@
 static enum pm_ret_status pm_ioctl_read_pggs(unsigned int index,
 					     unsigned int *value)
 {
-	if (index >= PGGS_NUM_REGS)
+	if (index >= PGGS_NUM_REGS) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	return pm_mmio_read(PGGS_BASEADDR + (index << 2), value);
 }
@@ -565,16 +596,18 @@
 
 	ret = pm_mmio_write(CRL_APB_BOOT_PIN_CTRL, CRL_APB_BOOT_PIN_MASK,
 			    ZYNQMP_ULPI_RESET_VAL_HIGH);
-	if (ret != PM_RET_SUCCESS)
+	if (ret != PM_RET_SUCCESS) {
 		return ret;
+	}
 
 	/* Drive ULPI assert for atleast 1ms */
 	mdelay(1);
 
 	ret = pm_mmio_write(CRL_APB_BOOT_PIN_CTRL, CRL_APB_BOOT_PIN_MASK,
 			    ZYNQMP_ULPI_RESET_VAL_LOW);
-	if (ret != PM_RET_SUCCESS)
+	if (ret != PM_RET_SUCCESS) {
 		return ret;
+	}
 
 	/* Drive ULPI de-assert for atleast 1ms */
 	mdelay(1);
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c b/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c
index 9a6b497..75e0499 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c
@@ -2549,17 +2549,20 @@
 	int i = 0;
 	uint16_t *grps;
 
-	if (fid >= MAX_FUNCTION)
+	if (fid >= MAX_FUNCTION) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	*ngroups = 0;
 
 	grps = *pinctrl_functions[fid].groups;
-	if (grps == NULL)
+	if (grps == NULL) {
 		return PM_RET_SUCCESS;
+	}
 
-	while (grps[i++] != (uint16_t)END_OF_GROUPS)
+	while (grps[i++] != (uint16_t)END_OF_GROUPS) {
 		(*ngroups)++;
+	}
 
 	return PM_RET_SUCCESS;
 }
@@ -2574,10 +2577,11 @@
  */
 void pm_api_pinctrl_get_function_name(unsigned int fid, char *name)
 {
-	if (fid >= MAX_FUNCTION)
+	if (fid >= MAX_FUNCTION) {
 		memcpy(name, END_OF_FUNCTION, FUNCTION_NAME_LEN);
-	else
+	} else {
 		memcpy(name, pinctrl_functions[fid].name, FUNCTION_NAME_LEN);
+	}
 }
 
 /**
@@ -2605,24 +2609,29 @@
 	unsigned int i;
 	uint16_t *grps;
 
-	if (fid >= MAX_FUNCTION)
+	if (fid >= MAX_FUNCTION) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	memset(groups, END_OF_GROUPS, GROUPS_PAYLOAD_LEN);
 
 	grps = *pinctrl_functions[fid].groups;
-	if (grps == NULL)
+	if (grps == NULL) {
 		return PM_RET_SUCCESS;
+	}
 
 	/* Skip groups till index */
-	for (i = 0; i < index; i++)
-		if (grps[i] == (uint16_t)END_OF_GROUPS)
+	for (i = 0; i < index; i++) {
+		if (grps[i] == (uint16_t)END_OF_GROUPS) {
 			return PM_RET_SUCCESS;
+		}
+	}
 
 	for (i = 0; i < NUM_GROUPS_PER_RESP; i++) {
 		groups[i] = grps[index + i];
-		if (groups[i] == (uint16_t)END_OF_GROUPS)
+		if (groups[i] == (uint16_t)END_OF_GROUPS) {
 			break;
+		}
 	}
 
 	return PM_RET_SUCCESS;
@@ -2653,24 +2662,29 @@
 	unsigned int i;
 	uint16_t *grps;
 
-	if (pin >= MAX_PIN)
+	if (pin >= MAX_PIN) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	memset(groups, END_OF_GROUPS, GROUPS_PAYLOAD_LEN);
 
 	grps = *zynqmp_pin_groups[pin].groups;
-	if (!grps)
+	if (grps == NULL) {
 		return PM_RET_SUCCESS;
+	}
 
 	/* Skip groups till index */
-	for (i = 0; i < index; i++)
-		if (grps[i] == (uint16_t)END_OF_GROUPS)
+	for (i = 0; i < index; i++) {
+		if (grps[i] == (uint16_t)END_OF_GROUPS) {
 			return PM_RET_SUCCESS;
+		}
+	}
 
 	for (i = 0; i < NUM_GROUPS_PER_RESP; i++) {
 		groups[i] = grps[index + i];
-		if (groups[i] == (uint16_t)END_OF_GROUPS)
+		if (groups[i] == (uint16_t)END_OF_GROUPS) {
 			break;
+		}
 	}
 
 	return PM_RET_SUCCESS;
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_sys.c b/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
index f9af451..e524ba5 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
@@ -306,10 +306,11 @@
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD5(payload, PM_REQ_SUSPEND, target, ack, latency, state);
-	if (ack == REQ_ACK_BLOCKING)
+	if (ack == REQ_ACK_BLOCKING) {
 		return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-	else
+	} else {
 		return pm_ipi_send(primary_proc, payload);
+	}
 }
 
 /**
@@ -345,10 +346,11 @@
 	PM_PACK_PAYLOAD5(payload, PM_REQ_WAKEUP, target, encoded_address,
 			 encoded_address >> 32, ack);
 
-	if (ack == REQ_ACK_BLOCKING)
+	if (ack == REQ_ACK_BLOCKING) {
 		return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-	else
+	} else {
 		return pm_ipi_send(primary_proc, payload);
+	}
 }
 
 /**
@@ -367,10 +369,11 @@
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD3(payload, PM_FORCE_POWERDOWN, target, ack);
 
-	if (ack == REQ_ACK_BLOCKING)
+	if (ack == REQ_ACK_BLOCKING) {
 		return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-	else
+	} else {
 		return pm_ipi_send(primary_proc, payload);
+	}
 }
 
 /**
@@ -459,10 +462,11 @@
 
 	PM_PACK_PAYLOAD5(payload, PM_REQ_NODE, nid, capabilities, qos, ack);
 
-	if (ack == REQ_ACK_BLOCKING)
+	if (ack == REQ_ACK_BLOCKING) {
 		return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-	else
+	} else {
 		return pm_ipi_send(primary_proc, payload);
+	}
 }
 
 /**
@@ -486,10 +490,11 @@
 	PM_PACK_PAYLOAD5(payload, PM_SET_REQUIREMENT, nid, capabilities, qos,
 			 ack);
 
-	if (ack == REQ_ACK_BLOCKING)
+	if (ack == REQ_ACK_BLOCKING) {
 		return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-	else
+	} else {
 		return pm_ipi_send(primary_proc, payload);
+	}
 }
 
 /* Miscellaneous API functions */
@@ -689,8 +694,9 @@
 void pm_get_callbackdata(uint32_t *data, size_t count)
 {
 	/* Return if interrupt is not from PMU */
-	if (!pm_ipi_irq_status(primary_proc))
+	if (!pm_ipi_irq_status(primary_proc)) {
 		return;
+	}
 
 	pm_ipi_buff_read_callb(data, count);
 	pm_ipi_irq_clear(primary_proc);
@@ -1070,21 +1076,24 @@
 
 	/* Check if clock ID is valid and return an error if it is not */
 	status = pm_clock_id_is_valid(clock_id);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
-	if (enable)
+	if (enable) {
 		api_id = PM_CLOCK_ENABLE;
-	else
+	} else {
 		api_id = PM_CLOCK_DISABLE;
+	}
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD2(payload, api_id, clock_id);
 	status = pm_ipi_send_sync(primary_proc, payload, NULL, 0);
 
 	/* If action fails due to the lack of permissions filter the error */
-	if (status == PM_RET_ERROR_ACCESS)
+	if (status == PM_RET_ERROR_ACCESS) {
 		status = PM_RET_SUCCESS;
+	}
 
 	return status;
 }
@@ -1105,8 +1114,9 @@
 
 	/* First try to handle it as a PLL */
 	pll = pm_clock_get_pll(clock_id);
-	if (pll)
+	if (pll) {
 		return pm_clock_pll_enable(pll);
+	}
 
 	/* It's an on-chip clock, PMU should configure clock's gate */
 	return pm_clock_gate(clock_id, 1);
@@ -1128,8 +1138,9 @@
 
 	/* First try to handle it as a PLL */
 	pll = pm_clock_get_pll(clock_id);
-	if (pll)
+	if (pll) {
 		return pm_clock_pll_disable(pll);
+	}
 
 	/* It's an on-chip clock, PMU should configure clock's gate */
 	return pm_clock_gate(clock_id, 0);
@@ -1159,8 +1170,9 @@
 
 	/* Check if clock ID is a valid on-chip clock */
 	status = pm_clock_id_is_valid(clock_id);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD2(payload, PM_CLOCK_GETSTATE, clock_id);
@@ -1190,13 +1202,15 @@
 
 	/* Get PLL node ID using PLL clock ID */
 	status = pm_clock_get_pll_node_id(clock_id, &nid);
-	if (status == PM_RET_SUCCESS)
+	if (status == PM_RET_SUCCESS) {
 		return pm_pll_set_parameter(nid, PM_PLL_PARAM_FBDIV, divider);
+	}
 
 	/* Check if clock ID is a valid on-chip clock */
 	status = pm_clock_id_is_valid(clock_id);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
 	if (div0 == (divider & div0)) {
 		div_id = PM_CLOCK_DIV0_ID;
@@ -1233,21 +1247,24 @@
 
 	/* Get PLL node ID using PLL clock ID */
 	status = pm_clock_get_pll_node_id(clock_id, &nid);
-	if (status == PM_RET_SUCCESS)
+	if (status == PM_RET_SUCCESS) {
 		return pm_pll_get_parameter(nid, PM_PLL_PARAM_FBDIV, divider);
+	}
 
 	/* Check if clock ID is a valid on-chip clock */
 	status = pm_clock_id_is_valid(clock_id);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
 	if (pm_clock_has_div(clock_id, PM_CLOCK_DIV0_ID)) {
 		/* Send request to the PMU to get div0 */
 		PM_PACK_PAYLOAD3(payload, PM_CLOCK_GETDIVIDER, clock_id,
 				 PM_CLOCK_DIV0_ID);
 		status = pm_ipi_send_sync(primary_proc, payload, &val, 1);
-		if (status != PM_RET_SUCCESS)
+		if (status != PM_RET_SUCCESS) {
 			return status;
+		}
 		*divider = val;
 	}
 
@@ -1256,8 +1273,9 @@
 		PM_PACK_PAYLOAD3(payload, PM_CLOCK_GETDIVIDER, clock_id,
 				 PM_CLOCK_DIV1_ID);
 		status = pm_ipi_send_sync(primary_proc, payload, &val, 1);
-		if (status != PM_RET_SUCCESS)
+		if (status != PM_RET_SUCCESS) {
 			return status;
+		}
 		*divider |= val << 16;
 	}
 
@@ -1313,13 +1331,15 @@
 
 	/* First try to handle it as a PLL */
 	pll = pm_clock_get_pll_by_related_clk(clock_id);
-	if (pll)
+	if (pll) {
 		return pm_clock_pll_set_parent(pll, clock_id, parent_index);
+	}
 
 	/* Check if clock ID is a valid on-chip clock */
 	status = pm_clock_id_is_valid(clock_id);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD3(payload, PM_CLOCK_SETPARENT, clock_id, parent_index);
@@ -1345,13 +1365,15 @@
 
 	/* First try to handle it as a PLL */
 	pll = pm_clock_get_pll_by_related_clk(clock_id);
-	if (pll)
+	if (pll) {
 		return pm_clock_pll_get_parent(pll, clock_id, parent_index);
+	}
 
 	/* Check if clock ID is a valid on-chip clock */
 	status = pm_clock_id_is_valid(clock_id);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD2(payload, PM_CLOCK_GETPARENT, clock_id);
@@ -1522,6 +1544,7 @@
 	default:
 		data[0] = PM_RET_ERROR_ARGS;
 		WARN("Unimplemented query service call: 0x%x\n", qid);
+		break;
 	}
 }
 
@@ -1614,12 +1637,14 @@
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
 	/* Check if given node ID is a PLL node */
-	if (nid < NODE_APLL || nid > NODE_IOPLL)
+	if (nid < NODE_APLL || nid > NODE_IOPLL) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Check if parameter ID is valid and return an error if it's not */
-	if (param_id >= PM_PLL_PARAM_MAX)
+	if (param_id >= PM_PLL_PARAM_MAX) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD4(payload, PM_PLL_SET_PARAMETER, nid, param_id, value);
@@ -1642,12 +1667,14 @@
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
 	/* Check if given node ID is a PLL node */
-	if (nid < NODE_APLL || nid > NODE_IOPLL)
+	if (nid < NODE_APLL || nid > NODE_IOPLL) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Check if parameter ID is valid and return an error if it's not */
-	if (param_id >= PM_PLL_PARAM_MAX)
+	if (param_id >= PM_PLL_PARAM_MAX) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD3(payload, PM_PLL_GET_PARAMETER, nid, param_id);
@@ -1672,12 +1699,14 @@
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
 	/* Check if given node ID is a PLL node */
-	if (nid < NODE_APLL || nid > NODE_IOPLL)
+	if (nid < NODE_APLL || nid > NODE_IOPLL) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Check if PLL mode is valid */
-	if (mode >= PM_PLL_MODE_MAX)
+	if (mode >= PM_PLL_MODE_MAX) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD3(payload, PM_PLL_SET_MODE, nid, mode);
@@ -1697,8 +1726,9 @@
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
 	/* Check if given node ID is a PLL node */
-	if (nid < NODE_APLL || nid > NODE_IOPLL)
+	if (nid < NODE_APLL || nid > NODE_IOPLL) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD2(payload, PM_PLL_GET_MODE, nid);
@@ -1733,8 +1763,9 @@
 	if (((ZYNQMP_CSU_BASEADDR & address) != ZYNQMP_CSU_BASEADDR) &&
 			((CSUDMA_BASE & address) != CSUDMA_BASE) &&
 			((RSA_CORE_BASE & address) != RSA_CORE_BASE) &&
-			((PMU_GLOBAL_BASE & address) != PMU_GLOBAL_BASE))
+			((PMU_GLOBAL_BASE & address) != PMU_GLOBAL_BASE)) {
 		return PM_RET_ERROR_ACCESS;
+	}
 
 	switch (register_access_id) {
 	case CONFIG_REG_WRITE:
@@ -1746,6 +1777,7 @@
 	default:
 		ret = PM_RET_ERROR_ARGS;
 		WARN("Unimplemented register_access call\n\r");
+		break;
 	}
 	return ret;
 }
diff --git a/plat/xilinx/zynqmp/pm_service/pm_client.c b/plat/xilinx/zynqmp/pm_service/pm_client.c
index 163e891..1f54430 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_client.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_client.c
@@ -188,8 +188,9 @@
 		 * If NODE_EXTERN could not be set as wake source, proceed with
 		 * standard suspend (no one will wake the system otherwise)
 		 */
-		if (ret == PM_RET_SUCCESS)
+		if (ret == PM_RET_SUCCESS) {
 			return;
+		}
 	}
 
 	zeromem(&pm_wakeup_nodes_set, sizeof(pm_wakeup_nodes_set));
@@ -198,8 +199,9 @@
 		uint32_t base_irq = reg_num << ISENABLER_SHIFT;
 		uint32_t reg = mmio_read_32(isenabler1 + (reg_num << 2));
 
-		if (!reg)
+		if (!reg) {
 			continue;
+		}
 
 		while (reg) {
 			enum pm_node_id node;
@@ -208,8 +210,9 @@
 			idx = __builtin_ctz(lowest_set);
 			irq = base_irq + idx;
 
-			if (irq > IRQ_MAX)
+			if (irq > IRQ_MAX) {
 				break;
+			}
 
 			node = irq_to_pm_node(irq);
 			reg &= ~lowest_set;
@@ -231,8 +234,9 @@
  */
 const struct pm_proc *pm_get_proc(unsigned int cpuid)
 {
-	if (cpuid < ARRAY_SIZE(pm_procs_all))
+	if (cpuid < ARRAY_SIZE(pm_procs_all)) {
 		return &pm_procs_all[cpuid];
+	}
 
 	return NULL;
 }
@@ -246,8 +250,9 @@
 const struct pm_proc *pm_get_proc_by_node(enum pm_node_id nid)
 {
 	for (size_t i = 0; i < ARRAY_SIZE(pm_procs_all); i++) {
-		if (nid == pm_procs_all[i].node_id)
+		if (nid == pm_procs_all[i].node_id) {
 			return &pm_procs_all[i];
+		}
 	}
 	return NULL;
 }
@@ -261,8 +266,9 @@
 static unsigned int pm_get_cpuid(enum pm_node_id nid)
 {
 	for (size_t i = 0; i < ARRAY_SIZE(pm_procs_all); i++) {
-		if (pm_procs_all[i].node_id == nid)
+		if (pm_procs_all[i].node_id == nid) {
 			return i;
+		}
 	}
 	return UNDEFINED_CPUID;
 }
@@ -280,8 +286,9 @@
 {
 	bakery_lock_get(&pm_client_secure_lock);
 
-	if (state == PM_STATE_SUSPEND_TO_RAM)
+	if (state == PM_STATE_SUSPEND_TO_RAM) {
 		pm_client_set_wakeup_sources();
+	}
 
 	/* Set powerdown request */
 	mmio_write_32(APU_PWRCTL, mmio_read_32(APU_PWRCTL) | proc->pwrdn_mask);
@@ -320,8 +327,9 @@
 {
 	unsigned int cpuid = pm_get_cpuid(proc->node_id);
 
-	if (cpuid == UNDEFINED_CPUID)
+	if (cpuid == UNDEFINED_CPUID) {
 		return;
+	}
 
 	bakery_lock_get(&pm_client_secure_lock);
 
@@ -336,8 +344,9 @@
 enum pm_ret_status pm_set_suspend_mode(uint32_t mode)
 {
 	if ((mode != PM_SUSPEND_MODE_STD) &&
-	    (mode != PM_SUSPEND_MODE_POWER_OFF))
+	    (mode != PM_SUSPEND_MODE_POWER_OFF)) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	suspend_mode = mode;
 	return PM_RET_SUCCESS;
diff --git a/plat/xilinx/zynqmp/sip_svc_setup.c b/plat/xilinx/zynqmp/sip_svc_setup.c
index 114da33..4a6095c 100644
--- a/plat/xilinx/zynqmp/sip_svc_setup.c
+++ b/plat/xilinx/zynqmp/sip_svc_setup.c
@@ -13,9 +13,9 @@
 #include "pm_svc_main.h"
 
 /* SMC function IDs for SiP Service queries */
-#define ZYNQMP_SIP_SVC_CALL_COUNT	0x8200ff00
-#define ZYNQMP_SIP_SVC_UID		0x8200ff01
-#define ZYNQMP_SIP_SVC_VERSION		0x8200ff03
+#define ZYNQMP_SIP_SVC_CALL_COUNT	U(0x8200ff00)
+#define ZYNQMP_SIP_SVC_UID		U(0x8200ff01)
+#define ZYNQMP_SIP_SVC_VERSION		U(0x8200ff03)
 
 /* SiP Service Calls version numbers */
 #define SIP_SVC_VERSION_MAJOR	0
@@ -100,6 +100,6 @@
 		sip_svc,
 		OEN_SIP_START,
 		OEN_SIP_END,
-		SMC_TYPE_FAST,
+		(uint8_t)SMC_TYPE_FAST,
 		sip_svc_setup,
 		sip_svc_smc_handler);
diff --git a/plat/xilinx/zynqmp/zynqmp_ipi.c b/plat/xilinx/zynqmp/zynqmp_ipi.c
index f57369f..4ea3c6a 100644
--- a/plat/xilinx/zynqmp/zynqmp_ipi.c
+++ b/plat/xilinx/zynqmp/zynqmp_ipi.c
@@ -25,67 +25,67 @@
 	/* APU IPI */
 	{
 		.ipi_bit_mask = 0x1,
-		.ipi_reg_base = 0xFF300000,
+		.ipi_reg_base = 0xFF300000U,
 		.secure_only = 0,
 	},
 	/* RPU0 IPI */
 	{
 		.ipi_bit_mask = 0x100,
-		.ipi_reg_base = 0xFF310000,
+		.ipi_reg_base = 0xFF310000U,
 		.secure_only = 0,
 	},
 	/* RPU1 IPI */
 	{
 		.ipi_bit_mask = 0x200,
-		.ipi_reg_base = 0xFF320000,
+		.ipi_reg_base = 0xFF320000U,
 		.secure_only = 0,
 	},
 	/* PMU0 IPI */
 	{
 		.ipi_bit_mask = 0x10000,
-		.ipi_reg_base = 0xFF330000,
+		.ipi_reg_base = 0xFF330000U,
 		.secure_only = IPI_SECURE_MASK,
 	},
 	/* PMU1 IPI */
 	{
 		.ipi_bit_mask = 0x20000,
-		.ipi_reg_base = 0xFF331000,
+		.ipi_reg_base = 0xFF331000U,
 		.secure_only = 0,
 	},
 	/* PMU2 IPI */
 	{
 		.ipi_bit_mask = 0x40000,
-		.ipi_reg_base = 0xFF332000,
+		.ipi_reg_base = 0xFF332000U,
 		.secure_only = IPI_SECURE_MASK,
 	},
 	/* PMU3 IPI */
 	{
 		.ipi_bit_mask = 0x80000,
-		.ipi_reg_base = 0xFF333000,
+		.ipi_reg_base = 0xFF333000U,
 		.secure_only = IPI_SECURE_MASK,
 	},
 	/* PL0 IPI */
 	{
 		.ipi_bit_mask = 0x1000000,
-		.ipi_reg_base = 0xFF340000,
+		.ipi_reg_base = 0xFF340000U,
 		.secure_only = 0,
 	},
 	/* PL1 IPI */
 	{
 		.ipi_bit_mask = 0x2000000,
-		.ipi_reg_base = 0xFF350000,
+		.ipi_reg_base = 0xFF350000U,
 		.secure_only = 0,
 	},
 	/* PL2 IPI */
 	{
 		.ipi_bit_mask = 0x4000000,
-		.ipi_reg_base = 0xFF360000,
+		.ipi_reg_base = 0xFF360000U,
 		.secure_only = 0,
 	},
 	/* PL3 IPI */
 	{
 		.ipi_bit_mask = 0x8000000,
-		.ipi_reg_base = 0xFF370000,
+		.ipi_reg_base = 0xFF370000U,
 		.secure_only = 0,
 	},
 };
diff --git a/services/std_svc/spm/el3_spmc/logical_sp.c b/services/std_svc/spm/el3_spmc/logical_sp.c
new file mode 100644
index 0000000..e080832
--- /dev/null
+++ b/services/std_svc/spm/el3_spmc/logical_sp.c
@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <services/el3_spmc_logical_sp.h>
+#include <services/ffa_svc.h>
+#include "spmc.h"
+
+/*******************************************************************************
+ * Validate any logical partition descriptors before we initialise.
+ * Initialization of said partitions will be taken care of during SPMC boot.
+ ******************************************************************************/
+int el3_sp_desc_validate(void)
+{
+	struct el3_lp_desc *lp_array;
+
+	/*
+	 * Assert the number of descriptors is less than maximum allowed.
+	 * This constant should be define on a per platform basis.
+	 */
+	assert(EL3_LP_DESCS_COUNT <= MAX_EL3_LP_DESCS_COUNT);
+
+	/* Check the array bounds are valid. */
+	assert(EL3_LP_DESCS_END >= EL3_LP_DESCS_START);
+
+	/* If no logical partitions are implemented then simply bail out. */
+	if (EL3_LP_DESCS_COUNT == 0U) {
+		return 0;
+	}
+
+	lp_array = get_el3_lp_array();
+
+	for (unsigned int index = 0; index < EL3_LP_DESCS_COUNT; index++) {
+		struct el3_lp_desc *lp_desc = &lp_array[index];
+
+		/* Validate our logical partition descriptors. */
+		if (lp_desc == NULL) {
+			ERROR("Invalid Logical SP Descriptor\n");
+			return -EINVAL;
+		}
+
+		/*
+		 * Ensure the ID follows the convention to indidate it resides
+		 * in the secure world.
+		 */
+		if (!ffa_is_secure_world_id(lp_desc->sp_id)) {
+			ERROR("Invalid Logical SP ID (0x%x)\n",
+			      lp_desc->sp_id);
+			return -EINVAL;
+		}
+
+		/* Ensure we don't conflict with the SPMC partition ID. */
+		if (lp_desc->sp_id == FFA_SPMC_ID) {
+			ERROR("Logical SP ID clashes with SPMC ID(0x%x)\n",
+			      lp_desc->sp_id);
+			return -EINVAL;
+		}
+
+		/* Ensure the UUID is not the NULL UUID. */
+		if (lp_desc->uuid[0] == 0 && lp_desc->uuid[1] == 0 &&
+		    lp_desc->uuid[2] == 0 && lp_desc->uuid[3] == 0) {
+			ERROR("Invalid UUID for Logical SP (0x%x)\n",
+			      lp_desc->sp_id);
+			return -EINVAL;
+		}
+
+		/* Ensure init function callback is registered. */
+		if (lp_desc->init == NULL) {
+			ERROR("Missing init function for Logical SP(0x%x)\n",
+			      lp_desc->sp_id);
+			return -EINVAL;
+		}
+
+		/* Ensure that LP only supports receiving direct requests. */
+		if (lp_desc->properties &
+		    ~(FFA_PARTITION_DIRECT_REQ_RECV)) {
+			ERROR("Invalid partition properties (0x%x)\n",
+			      lp_desc->properties);
+			return -EINVAL;
+		}
+
+		/* Ensure direct request function callback is registered. */
+		if (lp_desc->direct_req == NULL) {
+			ERROR("No Direct Req handler for Logical SP (0x%x)\n",
+			      lp_desc->sp_id);
+			return -EINVAL;
+		}
+
+		/* Ensure that all partition IDs are unique. */
+		for (unsigned int inner_idx = index + 1;
+		     inner_idx < EL3_LP_DESCS_COUNT; inner_idx++) {
+			if (lp_desc->sp_id == lp_array[inner_idx].sp_id) {
+				ERROR("Duplicate SP ID Detected (0x%x)\n",
+				      lp_desc->sp_id);
+				return -EINVAL;
+			}
+		}
+	}
+	return 0;
+}
diff --git a/services/std_svc/spm/el3_spmc/spmc.h b/services/std_svc/spm/el3_spmc/spmc.h
index df0aa61..faa604f 100644
--- a/services/std_svc/spm/el3_spmc/spmc.h
+++ b/services/std_svc/spm/el3_spmc/spmc.h
@@ -11,6 +11,7 @@
 
 #include <lib/psci/psci.h>
 #include <lib/spinlock.h>
+#include <services/el3_spmc_logical_sp.h>
 #include "spm_common.h"
 
 /*
@@ -68,6 +69,28 @@
 	SP_STATE_AARCH32
 };
 
+enum mailbox_state {
+	/* There is no message in the mailbox. */
+	MAILBOX_STATE_EMPTY,
+
+	/* There is a message that has been populated in the mailbox. */
+	MAILBOX_STATE_FULL,
+};
+
+struct mailbox {
+	enum mailbox_state state;
+
+	/* RX/TX Buffers. */
+	void *rx_buffer;
+	const void *tx_buffer;
+
+	/* Size of RX/TX Buffer. */
+	uint32_t rxtx_page_count;
+
+	/* Lock access to mailbox. */
+	spinlock_t lock;
+};
+
 /*
  * Execution context members for an SP. This is a bit like struct
  * vcpu in a hypervisor.
@@ -118,6 +141,9 @@
 	/* Execution State. */
 	enum sp_execution_state execution_state;
 
+	/* Mailbox tracking. */
+	struct mailbox mailbox;
+
 	/* Secondary entrypoint. Only valid for a S-EL1 SP. */
 	uintptr_t secondary_ep;
 };
@@ -142,7 +168,12 @@
 	uint16_t ns_ep_id;
 
 	/*
-	 * Supported FF-A Version.
+	 * Mailbox tracking.
+	 */
+	struct mailbox mailbox;
+
+	/*
+	 * Supported FF-A Version
 	 */
 	uint32_t ffa_version;
 };
@@ -184,4 +215,16 @@
  */
 bool is_ffa_secure_id_valid(uint16_t partition_id);
 
+/*
+ * Helper function to obtain the array storing the EL3
+ * Logical Partition descriptors.
+ */
+struct el3_lp_desc *get_el3_lp_array(void);
+
+/*
+ * Helper function to obtain the RX/TX buffer pair descriptor of the Hypervisor
+ * or OS kernel in the normal world or the last SP that was run.
+ */
+struct mailbox *spmc_get_mbox_desc(bool secure_origin);
+
 #endif /* SPMC_H */
diff --git a/services/std_svc/spm/el3_spmc/spmc.mk b/services/std_svc/spm/el3_spmc/spmc.mk
index 2b154dd..8067c74 100644
--- a/services/std_svc/spm/el3_spmc/spmc.mk
+++ b/services/std_svc/spm/el3_spmc/spmc.mk
@@ -10,8 +10,15 @@
 
 SPMC_SOURCES	:=	$(addprefix services/std_svc/spm/el3_spmc/,	\
 			spmc_main.c				\
-			spmc_setup.c)
+			spmc_setup.c				\
+			logical_sp.c)
+
+# Specify platform specific logical partition implementation.
+SPMC_LP_SOURCES  := $(addprefix ${PLAT_DIR}/, \
+                    ${PLAT}_el3_spmc_logical_sp.c)
+
 
+SPMC_SOURCES += $(SPMC_LP_SOURCES)
 
 # Let the top-level Makefile know that we intend to include a BL32 image
 NEED_BL32		:=	yes
diff --git a/services/std_svc/spm/el3_spmc/spmc_main.c b/services/std_svc/spm/el3_spmc/spmc_main.c
index 3fd8c78..33a25a2 100644
--- a/services/std_svc/spm/el3_spmc/spmc_main.c
+++ b/services/std_svc/spm/el3_spmc/spmc_main.c
@@ -19,6 +19,7 @@
 #include <lib/xlat_tables/xlat_tables_v2.h>
 #include <libfdt.h>
 #include <plat/common/platform.h>
+#include <services/el3_spmc_logical_sp.h>
 #include <services/ffa_svc.h>
 #include <services/spmc_svc.h>
 #include <services/spmd_svc.h>
@@ -41,6 +42,15 @@
 static struct ns_endpoint_desc ns_ep_desc[NS_PARTITION_COUNT];
 
 /*
+ * Helper function to obtain the array storing the EL3
+ * Logical Partition descriptors.
+ */
+struct el3_lp_desc *get_el3_lp_array(void)
+{
+	return (struct el3_lp_desc *) EL3_LP_DESCS_START;
+}
+
+/*
  * Helper function to obtain the descriptor of the last SP to whom control was
  * handed to on this physical cpu. Currently, we assume there is only one SP.
  * TODO: Expand to track multiple partitions when required.
@@ -62,7 +72,7 @@
 /* Helper function to get pointer to SP context from its ID. */
 struct secure_partition_desc *spmc_get_sp_ctx(uint16_t id)
 {
-	/* Check for SWd Partitions. */
+	/* Check for Secure World Partitions. */
 	for (unsigned int i = 0U; i < SECURE_PARTITION_COUNT; i++) {
 		if (sp_desc[i].sp_id == id) {
 			return &(sp_desc[i]);
@@ -71,6 +81,29 @@
 	return NULL;
 }
 
+/*
+ * Helper function to obtain the descriptor of the Hypervisor or OS kernel.
+ * We assume that the first descriptor is reserved for this entity.
+ */
+struct ns_endpoint_desc *spmc_get_hyp_ctx(void)
+{
+	return &(ns_ep_desc[0]);
+}
+
+/*
+ * Helper function to obtain the RX/TX buffer pair descriptor of the Hypervisor
+ * or OS kernel in the normal world or the last SP that was run.
+ */
+struct mailbox *spmc_get_mbox_desc(bool secure_origin)
+{
+	/* Obtain the RX/TX buffer pair descriptor. */
+	if (secure_origin) {
+		return &(spmc_get_current_sp_ctx()->mailbox);
+	} else {
+		return &(spmc_get_hyp_ctx()->mailbox);
+	}
+}
+
 /******************************************************************************
  * This function returns to the place where spmc_sp_synchronous_entry() was
  * called originally.
@@ -105,6 +138,8 @@
  ******************************************************************************/
 bool is_ffa_secure_id_valid(uint16_t partition_id)
 {
+	struct el3_lp_desc *el3_lp_descs = get_el3_lp_array();
+
 	/* Ensure the ID is not the invalid partition ID. */
 	if (partition_id == INV_SP_ID) {
 		return false;
@@ -133,12 +168,22 @@
 		return false;
 	}
 
+	/* Ensure we don't clash with any Logical SP's. */
+	for (unsigned int i = 0U; i < EL3_LP_DESCS_COUNT; i++) {
+		if (el3_lp_descs[i].sp_id == partition_id) {
+			return false;
+		}
+	}
+
 	return true;
 }
 
 /*******************************************************************************
  * This function either forwards the request to the other world or returns
  * with an ERET depending on the source of the call.
+ * We can assume that the destination is for an entity at a lower exception
+ * level as any messages destined for a logical SP resident in EL3 will have
+ * already been taken care of by the SPMC before entering this function.
  ******************************************************************************/
 static uint64_t spmc_smc_return(uint32_t smc_fid,
 				bool secure_origin,
@@ -210,6 +255,7 @@
 				       uint64_t flags)
 {
 	uint16_t dst_id = ffa_endpoint_destination(x1);
+	struct el3_lp_desc *el3_lp_descs;
 	struct secure_partition_desc *sp;
 	unsigned int idx;
 
@@ -219,11 +265,22 @@
 					     FFA_ERROR_INVALID_PARAMETER);
 	}
 
+	el3_lp_descs = get_el3_lp_array();
+
+	/* Check if the request is destined for a Logical Partition. */
+	for (unsigned int i = 0U; i < MAX_EL3_LP_DESCS_COUNT; i++) {
+		if (el3_lp_descs[i].sp_id == dst_id) {
+			return el3_lp_descs[i].direct_req(
+					smc_fid, secure_origin, x1, x2, x3, x4,
+					cookie, handle, flags);
+		}
+	}
+
 	/*
-	 * If called by the secure world it is an invalid call since a
-	 * SP cannot call into the Normal world and there is no other SP to call
-	 * into. If there are other SPs in future then the partition runtime
-	 * model would need to be validated as well.
+	 * If the request was not targeted to a LSP and from the secure world
+	 * then it is invalid since a SP cannot call into the Normal world and
+	 * there is no other SP to call into. If there are other SPs in future
+	 * then the partition runtime model would need to be validated as well.
 	 */
 	if (secure_origin) {
 		VERBOSE("Direct request not supported to the Normal World.\n");
@@ -457,7 +514,60 @@
 	return spmc_ffa_error_return(handle, FFA_ERROR_NOT_SUPPORTED);
 }
 
+static uint64_t ffa_version_handler(uint32_t smc_fid,
+				    bool secure_origin,
+				    uint64_t x1,
+				    uint64_t x2,
+				    uint64_t x3,
+				    uint64_t x4,
+				    void *cookie,
+				    void *handle,
+				    uint64_t flags)
+{
+	uint32_t requested_version = x1 & FFA_VERSION_MASK;
+
+	if (requested_version & FFA_VERSION_BIT31_MASK) {
+		/* Invalid encoding, return an error. */
+		SMC_RET1(handle, FFA_ERROR_NOT_SUPPORTED);
+		/* Execution stops here. */
+	}
+
+	/* Determine the caller to store the requested version. */
+	if (secure_origin) {
+		/*
+		 * Ensure that the SP is reporting the same version as
+		 * specified in its manifest. If these do not match there is
+		 * something wrong with the SP.
+		 * TODO: Should we abort the SP? For now assert this is not
+		 *       case.
+		 */
+		assert(requested_version ==
+		       spmc_get_current_sp_ctx()->ffa_version);
+	} else {
+		/*
+		 * If this is called by the normal world, record this
+		 * information in its descriptor.
+		 */
+		spmc_get_hyp_ctx()->ffa_version = requested_version;
+	}
+
+	SMC_RET1(handle, MAKE_FFA_VERSION(FFA_VERSION_MAJOR,
+					  FFA_VERSION_MINOR));
+}
+
 /*******************************************************************************
+ * Helper function to obtain the FF-A version of the calling partition.
+ ******************************************************************************/
+uint32_t get_partition_ffa_version(bool secure_origin)
+{
+	if (secure_origin) {
+		return spmc_get_current_sp_ctx()->ffa_version;
+	} else {
+		return spmc_get_hyp_ctx()->ffa_version;
+	}
+}
+
+/*******************************************************************************
  * This function will parse the Secure Partition Manifest. From manifest, it
  * will fetch details for preparing Secure partition image context and secure
  * partition image boot arguments if any.
@@ -479,6 +589,13 @@
 		return node;
 	}
 
+	ret = fdt_read_uint32_array(sp_manifest, node, "uuid",
+				    ARRAY_SIZE(sp->uuid), sp->uuid);
+	if (ret != 0) {
+		ERROR("Missing Secure Partition UUID.\n");
+		return ret;
+	}
+
 	ret = fdt_read_uint32(sp_manifest, node, "exception-level", &config_32);
 	if (ret != 0) {
 		ERROR("Missing SP Exception Level information.\n");
@@ -503,6 +620,42 @@
 
 	sp->execution_state = config_32;
 
+	ret = fdt_read_uint32(sp_manifest, node,
+			      "messaging-method", &config_32);
+	if (ret != 0) {
+		ERROR("Missing Secure Partition messaging method.\n");
+		return ret;
+	}
+
+	/* Validate this entry, we currently only support direct messaging. */
+	if ((config_32 & ~(FFA_PARTITION_DIRECT_REQ_RECV |
+			  FFA_PARTITION_DIRECT_REQ_SEND)) != 0U) {
+		WARN("Invalid Secure Partition messaging method (0x%x)\n",
+		     config_32);
+		return -EINVAL;
+	}
+
+	sp->properties = config_32;
+
+	ret = fdt_read_uint32(sp_manifest, node,
+			      "execution-ctx-count", &config_32);
+
+	if (ret != 0) {
+		ERROR("Missing SP Execution Context Count.\n");
+		return ret;
+	}
+
+	/*
+	 * Ensure this field is set correctly in the manifest however
+	 * since this is currently a hardcoded value for S-EL1 partitions
+	 * we don't need to save it here, just validate.
+	 */
+	if (config_32 != PLATFORM_CORE_COUNT) {
+		ERROR("SP Execution Context Count (%u) must be %u.\n",
+			config_32, PLATFORM_CORE_COUNT);
+		return -EINVAL;
+	}
+
 	/*
 	 * Look for the optional fields that are expected to be present in
 	 * an SP manifest.
@@ -621,6 +774,37 @@
  * This function takes an SP context pointer and performs a synchronous entry
  * into it.
  ******************************************************************************/
+static int32_t logical_sp_init(void)
+{
+	int32_t rc = 0;
+	struct el3_lp_desc *el3_lp_descs;
+
+	/* Perform initial validation of the Logical Partitions. */
+	rc = el3_sp_desc_validate();
+	if (rc != 0) {
+		ERROR("Logical Partition validation failed!\n");
+		return rc;
+	}
+
+	el3_lp_descs = get_el3_lp_array();
+
+	INFO("Logical Secure Partition init start.\n");
+	for (unsigned int i = 0U; i < EL3_LP_DESCS_COUNT; i++) {
+		rc = el3_lp_descs[i].init();
+		if (rc != 0) {
+			ERROR("Logical SP (0x%x) Failed to Initialize\n",
+			      el3_lp_descs[i].sp_id);
+			return rc;
+		}
+		VERBOSE("Logical SP (0x%x) Initialized\n",
+			      el3_lp_descs[i].sp_id);
+	}
+
+	INFO("Logical Secure Partition init completed.\n");
+
+	return rc;
+}
+
 uint64_t spmc_sp_synchronous_entry(struct sp_exec_ctx *ec)
 {
 	uint64_t rc;
@@ -684,6 +868,9 @@
 	for (unsigned int i = 0U; i < SECURE_PARTITION_COUNT; i++) {
 		sp = &sp_desc[i];
 		sp->sp_id = INV_SP_ID;
+		sp->mailbox.rx_buffer = NULL;
+		sp->mailbox.tx_buffer = NULL;
+		sp->mailbox.state = MAILBOX_STATE_EMPTY;
 		sp->secondary_ep = 0;
 	}
 }
@@ -700,6 +887,9 @@
 		 */
 		ns_ep->ns_ep_id = 0;
 		ns_ep->ffa_version = 0;
+		ns_ep->mailbox.rx_buffer = NULL;
+		ns_ep->mailbox.tx_buffer = NULL;
+		ns_ep->mailbox.state = MAILBOX_STATE_EMPTY;
 	}
 }
 
@@ -725,6 +915,13 @@
 	initalize_sp_descs();
 	initalize_ns_ep_descs();
 
+	/* Setup logical SPs. */
+	ret = logical_sp_init();
+	if (ret != 0) {
+		ERROR("Failed to initialize Logical Partitions.\n");
+		return ret;
+	}
+
 	/* Perform physical SP setup. */
 
 	/* Disable MMU at EL1 (initialized by BL2) */
@@ -762,6 +959,10 @@
 {
 	switch (smc_fid) {
 
+	case FFA_VERSION:
+		return ffa_version_handler(smc_fid, secure_origin, x1, x2, x3,
+					   x4, cookie, handle, flags);
+
 	case FFA_MSG_SEND_DIRECT_REQ_SMC32:
 	case FFA_MSG_SEND_DIRECT_REQ_SMC64:
 		return direct_req_smc_handler(smc_fid, secure_origin, x1, x2,
diff --git a/services/std_svc/spmd/spmd_main.c b/services/std_svc/spmd/spmd_main.c
index 5b131cd..777a962 100644
--- a/services/std_svc/spmd/spmd_main.c
+++ b/services/std_svc/spmd/spmd_main.c
@@ -626,7 +626,8 @@
 		 * If caller is secure and SPMC was initialized,
 		 * return FFA_VERSION of SPMD.
 		 * If caller is non secure and SPMC was initialized,
-		 * return SPMC's version.
+		 * forward to the EL3 SPMC if enabled, otherwise return
+		 * the SPMC version if implemented at a lower EL.
 		 * Sanity check to "input_version".
 		 * If the EL3 SPMC is enabled, ignore the SPMC state as
 		 * this is not used.
@@ -635,6 +636,17 @@
 		    (!is_spmc_at_el3() && (ctx->state == SPMC_STATE_RESET))) {
 			ret = FFA_ERROR_NOT_SUPPORTED;
 		} else if (!secure_origin) {
+			if (is_spmc_at_el3()) {
+				/*
+				 * Forward the call directly to the EL3 SPMC, if
+				 * enabled, as we don't need to wrap the call in
+				 * a direct request.
+				 */
+				return spmd_smc_forward(smc_fid, secure_origin,
+							x1, x2, x3, x4, cookie,
+							handle, flags);
+			}
+
 			gp_regs_t *gpregs = get_gpregs_ctx(&ctx->cpu_ctx);
 			uint64_t rc;
 
diff --git a/tools/sptool/sp_mk_generator.py b/tools/sptool/sp_mk_generator.py
old mode 100755
new mode 100644
index 82d5c1b..f4045d3
--- a/tools/sptool/sp_mk_generator.py
+++ b/tools/sptool/sp_mk_generator.py
@@ -46,112 +46,187 @@
 }
 
 """
-
-import getopt
 import json
 import os
 import re
 import sys
 import uuid
+from spactions import SpSetupActions
 
-with open(sys.argv[2],'r') as in_file:
-    data = json.load(in_file)
-json_file = os.path.abspath(sys.argv[2])
-json_dir = os.path.dirname(json_file)
-gen_file = os.path.abspath(sys.argv[1])
-out_dir = os.path.abspath(sys.argv[3])
-dtb_dir = out_dir + "/fdts/"
 MAX_SP = 8
-dualroot = sys.argv[4].lower() == "dualroot"
-split = int(MAX_SP / 2)
-print(dtb_dir)
-platform_count = 1
-sip_count = 1
+UUID_LEN = 4
 
-with open(gen_file, 'w') as out_file:
-    for idx, key in enumerate(data.keys()):
+# Some helper functions to access args propagated to the action functions in
+# SpSetupActions framework.
+def check_sp_mk_gen(args :dict):
+    if "sp_gen_mk" not in args.keys():
+        raise Exception(f"Path to file sp_gen.mk needs to be in 'args'.")
 
-        pkg_num = idx + 1
+def check_out_dir(args :dict):
+    if "out_dir" not in args.keys() or not os.path.isdir(args["out_dir"]):
+        raise Exception("Define output folder with \'out_dir\' key.")
 
-        if (pkg_num > MAX_SP):
-            print("WARNING: Too many secure partitions\n")
-            exit(-1)
+def check_sp_layout_dir(args :dict):
+    if "sp_layout_dir" not in args.keys() or not os.path.isdir(args["sp_layout_dir"]):
+        raise Exception("Define output folder with \'sp_layout_dir\' key.")
 
-        if dualroot:
-            owner = data[key].get('owner')
-            if owner == "Plat":
-                if (platform_count > split):
-                    print("WARNING: Maximum Secure partitions by Plat " +
-                    "have been exceeded (" + str(split) + ")\n")
-                    exit(-1)
-                pkg_num = split + platform_count
-                platform_count += 1
-            elif (sip_count > split):
-                print("WARNING: Maximum Secure partitions by SiP " +
-                "have been exceeded (" + str(split) + ")\n")
-                exit(-1)
-            else:
-                pkg_num = sip_count
-                sip_count += 1
+def write_to_sp_mk_gen(content, args :dict):
+    check_sp_mk_gen(args)
+    with open(args["sp_gen_mk"], "a") as f:
+        f.write(f"{content}\n")
 
-        """
-        Append FDT_SOURCES
-        """
-        dts = os.path.join(json_dir, data[key]['pm'])
-        dtb = dtb_dir + os.path.basename(data[key]['pm'][:-1] + "b")
-        out_file.write("FDT_SOURCES += " + dts + "\n")
+def get_sp_manifest_full_path(sp_node, args :dict):
+    check_sp_layout_dir(args)
+    return os.path.join(args["sp_layout_dir"], get_file_from_layout(sp_node["pm"]))
 
-        """
-        Update SPTOOL_ARGS
-        """
-        dst = out_dir + "/" + key + ".pkg"
-        src = [ json_dir + "/" + data[key]['image'] , dtb  ]
-        out_file.write("SPTOOL_ARGS += -i " + ":".join(src) + " -o " + dst + "\n")
+def get_sp_img_full_path(sp_node, args :dict):
+    check_sp_layout_dir(args)
+    return os.path.join(args["sp_layout_dir"], get_file_from_layout(sp_node["image"]))
 
-        if "uuid" in data[key]:
-            """
-            Extract the UUID from the JSON file if the SP entry has a 'uuid' field
-            """
-            uuid_std = uuid.UUID(data[key]['uuid'])
-        else:
-            """
-            Extract uuid from partition manifest
-            """
-            pm_file = open(dts)
-            for line in pm_file:
-                if "uuid" in line:
-                    # re.findall returns a list of string tuples.
-                    # uuid_hex is the first item in this list representing the four
-                    # uuid hex integers from the manifest uuid field. The heading
-                    # '0x' of the hexadecimal representation is stripped out.
-                    # e.g. uuid = <0x1e67b5b4 0xe14f904a 0x13fb1fb8 0xcbdae1da>;
-                    # uuid_hex = ('1e67b5b4', 'e14f904a', '13fb1fb8', 'cbdae1da')
-                    uuid_hex = re.findall(r'0x([0-9a-f]+) 0x([0-9a-f]+) 0x([0-9a-f]+) 0x([0-9a-f]+)', line)[0];
+def get_sp_pkg(sp, args :dict):
+    check_out_dir(args)
+    return os.path.join(args["out_dir"], f"{sp}.pkg")
+
+def is_line_in_sp_gen(line, args :dict):
+    with open(args["sp_gen_mk"], "r") as f:
+        sppkg_rule = [l for l in f if line in l]
+    return len(sppkg_rule) is not 0
+
+def get_file_from_layout(node):
+    ''' Helper to fetch a file path from sp_layout.json. '''
+    if type(node) is dict and "file" in node.keys():
+        return node["file"]
+    return node
 
-            # uuid_hex is a list of four hex string values
-            if len(uuid_hex) != 4:
-                print("ERROR: malformed UUID")
-                exit(-1)
+def get_offset_from_layout(node):
+    ''' Helper to fetch an offset from sp_layout.json. '''
+    if type(node) is dict and "offset" in node.keys():
+        return int(node["offset"], 0)
+    return None
 
-            # The uuid field in SP manifest is the little endian representation
-            # mapped to arguments as described in SMCCC section 5.3.
-            # Convert each unsigned integer value to a big endian representation
-            # required by fiptool.
-            y=list(map(bytearray.fromhex, uuid_hex))
-            z=(int.from_bytes(y[0], byteorder='little', signed=False),
-            int.from_bytes(y[1], byteorder='little', signed=False),
-            int.from_bytes(y[2], byteorder='little', signed=False),
-            int.from_bytes(y[3], byteorder='little', signed=False))
-            uuid_std = uuid.UUID(f'{z[0]:08x}{z[1]:08x}{z[2]:08x}{z[3]:08x}')
+def get_image_offset(node):
+    ''' Helper to fetch image offset from sp_layout.json '''
+    return get_offset_from_layout(node["image"])
+
+def get_pm_offset(node):
+    ''' Helper to fetch pm offset from sp_layout.json '''
+    return get_offset_from_layout(node["pm"])
+
+@SpSetupActions.sp_action(global_action=True)
+def check_max_sps(sp_layout, _, args :dict):
+    ''' Check validate the maximum number of SPs is respected. '''
+    if len(sp_layout.keys()) > MAX_SP:
+        raise Exception(f"Too many SPs in SP layout file. Max: {MAX_SP}")
+    return args
+
+@SpSetupActions.sp_action
+def gen_fdt_sources(sp_layout, sp, args :dict):
+    ''' Generate FDT_SOURCES values for a given SP. '''
+    manifest_path = get_sp_manifest_full_path(sp_layout[sp], args)
+    write_to_sp_mk_gen(f"FDT_SOURCES += {manifest_path}", args)
+    return args
 
-        """
-        Append FIP_ARGS
-        """
-        out_file.write("FIP_ARGS += --blob uuid=" + str(uuid_std) + ",file=" + dst + "\n")
+@SpSetupActions.sp_action
+def gen_sptool_args(sp_layout, sp, args :dict):
+    ''' Generate Sp Pkgs rules. '''
+    sp_pkg = get_sp_pkg(sp, args)
+    sp_dtb_name = os.path.basename(get_file_from_layout(sp_layout[sp]["pm"]))[:-1] + "b"
+    sp_dtb = os.path.join(args["out_dir"], f"fdts/{sp_dtb_name}")
 
-        """
-        Append CRT_ARGS
-        """
+    # Do not generate rule if already there.
+    if is_line_in_sp_gen(f'{sp_pkg}:', args):
+        return args
+    write_to_sp_mk_gen(f"SP_PKGS += {sp_pkg}\n", args)
+
+    sptool_args = f" -i {get_sp_img_full_path(sp_layout[sp], args)}:{sp_dtb}"
+    pm_offset = get_pm_offset(sp_layout[sp])
+    sptool_args += f" --pm-offset {pm_offset}" if pm_offset is not None else ""
+    image_offset = get_image_offset(sp_layout[sp])
+    sptool_args += f" --img-offset {image_offset}" if image_offset is not None else ""
+    sptool_args += f" -o {sp_pkg}"
+    sppkg_rule = f'''
+{sp_pkg}:
+\t$(Q)echo Generating {sp_pkg}
+\t$(Q)$(PYTHON) $(SPTOOL) {sptool_args}
+'''
+    write_to_sp_mk_gen(sppkg_rule, args)
+    return args
+
+@SpSetupActions.sp_action(global_action=True, exec_order=1)
+def check_dualroot(sp_layout, _, args :dict):
+    ''' Validate the amount of SPs from SiP and Platform owners. '''
+    if not args.get("dualroot"):
+        return args
+    args["split"] =  int(MAX_SP / 2)
+    owners = [sp_layout[sp].get("owner") for sp in sp_layout]
+    args["plat_max_count"] = owners.count("Plat")
+    # If it is owned by the platform owner, it is assigned to the SiP.
+    args["sip_max_count"] = len(sp_layout.keys()) - args["plat_max_count"]
+    if  args["sip_max_count"] > args["split"] or args["sip_max_count"] > args["split"]:
+        print(f"WARN: SiP Secure Partitions should not be more than {args['split']}")
+    # Counters for gen_crt_args.
+    args["sip_count"] = 1
+    args["plat_count"] = 1
+    return args
+
+@SpSetupActions.sp_action
+def gen_crt_args(sp_layout, sp, args :dict):
+    ''' Append CRT_ARGS. '''
+    # If "dualroot" is configured, 'sp_pkg_idx' depends on whether the SP is owned
+    # by the "SiP" or the "Plat".
+    if args.get("dualroot"):
+        # If the owner is not specified as "Plat", default to "SiP".
+        if sp_layout[sp].get("owner") == "Plat":
+            if args["plat_count"] > args["plat_max_count"]:
+                raise ValueError("plat_count can't surpass plat_max_count in args.")
+            sp_pkg_idx = args["plat_count"] + args["split"]
+            args["plat_count"] += 1
+        else:
+            if args["sip_count"] > args["sip_max_count"]:
+                raise ValueError("sip_count can't surpass sip_max_count in args.")
+            sp_pkg_idx = args["sip_count"]
+            args["sip_count"] += 1
+    else:
+        sp_pkg_idx = [k for k in sp_layout.keys()].index(sp) + 1
+    write_to_sp_mk_gen(f"CRT_ARGS += --sp-pkg{sp_pkg_idx} {get_sp_pkg(sp, args)}\n", args)
+    return args
+
+@SpSetupActions.sp_action
+def gen_fiptool_args(sp_layout, sp, args :dict):
+    ''' Generate arguments for the FIP Tool. '''
+    if "uuid" in sp_layout[sp]:
+        # Extract the UUID from the JSON file if the SP entry has a 'uuid' field
+        uuid_std = uuid.UUID(data[key]['uuid'])
+    else:
+        with open(get_sp_manifest_full_path(sp_layout[sp], args), "r") as pm_f:
+            uuid_lines = [l for l in pm_f if 'uuid' in l]
+        assert(len(uuid_lines) is 1)
+        # The uuid field in SP manifest is the little endian representation
+        # mapped to arguments as described in SMCCC section 5.3.
+        # Convert each unsigned integer value to a big endian representation
+        # required by fiptool.
+        uuid_parsed = re.findall("0x([0-9a-f]+)", uuid_lines[0])
+        y = list(map(bytearray.fromhex, uuid_parsed))
+        z = [int.from_bytes(i, byteorder='little', signed=False) for i in y]
+        uuid_std = uuid.UUID(f'{z[0]:08x}{z[1]:08x}{z[2]:08x}{z[3]:08x}')
+    write_to_sp_mk_gen(f"FIP_ARGS += --blob uuid={str(uuid_std)},file={get_sp_pkg(sp, args)}\n", args)
+    return args
+
+def init_sp_actions(sys):
+    sp_layout_file = os.path.abspath(sys.argv[2])
+    with open(sp_layout_file) as json_file:
+        sp_layout = json.load(json_file)
+    # Initialize arguments for the SP actions framework
+    args = {}
+    args["sp_gen_mk"] = os.path.abspath(sys.argv[1])
+    args["sp_layout_dir"] = os.path.dirname(sp_layout_file)
+    args["out_dir"] = os.path.abspath(sys.argv[3])
+    args["dualroot"] = sys.argv[4] == "dualroot"
+    #Clear content of file "sp_gen.mk".
+    with open(args["sp_gen_mk"], "w"):
+        None
+    return args, sp_layout
 
-        out_file.write("CRT_ARGS += --sp-pkg" + str(pkg_num) + " " + dst + "\n")
-        out_file.write("\n")
+if __name__ == "__main__":
+    args, sp_layout = init_sp_actions(sys)
+    SpSetupActions.run_actions(sp_layout, args)
diff --git a/tools/sptool/spactions.py b/tools/sptool/spactions.py
new file mode 100644
index 0000000..ff28ebb
--- /dev/null
+++ b/tools/sptool/spactions.py
@@ -0,0 +1,155 @@
+#!/usr/bin/python3
+# Copyright (c) 2022, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+'''
+This is a python module for defining and executing SP setup actions, targeting
+a system deploying an SPM implementation.
+Each action consists of a function, that processes the SP layout json file and
+other provided arguments.
+At the core of this is the SpSetupActions which provides a means to register
+the functions into a table of actions, and execute them all when invoking
+SpSetupActions.run_actions.
+Registering the function is done by using the decorator '@SpSetupActions.sp_action'
+at function definition.
+
+Functions can be called:
+- once only, or per SP defined in the SP layout file;
+- following an order, from lowest to highest of their execution order.
+More information in the doc comments below.
+'''
+import bisect
+
+DEFAULT_ACTION_ORDER = 100
+
+class _ConfiguredAction:
+    """
+    Wraps action function with its configuration.
+    """
+    def __init__(self, action, exec_order=DEFAULT_ACTION_ORDER, global_action=True, log_calls = False):
+        self.exec_order = exec_order
+        self.__name__ = action.__name__
+        def logged_action(action):
+            def inner_logged_action(sp_layout, sp, args :dict):
+                print(f"Calling {action.__name__} -> {sp}")
+                return action(sp_layout, sp, args)
+            return inner_logged_action
+        self.action = logged_action(action) if log_calls is True else action
+        self.global_action = global_action
+
+    def __lt__(self, other):
+        """
+        To allow for ordered inserts in a list of actions.
+        """
+        return self.exec_order < other.exec_order
+
+    def __call__(self, sp_layout, sp, args :dict):
+        """
+        Calls action function.
+        """
+        return self.action(sp_layout, sp, args)
+
+    def __repr__(self) -> str:
+        """
+        Pretty format to show debug information about the action.
+        """
+        return f"func: {self.__name__}; global:{self.global_action}; exec_order: {self.exec_order}"
+
+class SpSetupActions:
+    actions = []
+
+    def sp_action(in_action = None, global_action = False, log_calls=False, exec_order=DEFAULT_ACTION_ORDER):
+        """
+        Function decorator that registers and configures action.
+
+        :param in_action - function to register
+        :param global_action - make the function global, i.e. make it be
+        only called once.
+        :param log_calls - at every call to action, a useful log will be printed.
+        :param exec_order - action's calling order.
+        """
+        def append_action(action):
+            action = _ConfiguredAction(action, exec_order, global_action, log_calls)
+            bisect.insort(SpSetupActions.actions, action)
+            return action
+        if in_action is not None:
+            return append_action(in_action)
+        return append_action
+
+    def run_actions(sp_layout: dict, args: dict, verbose=False):
+        """
+        Executes all actions in accordance to their registering configuration:
+        - If set as "global" it will be called once.
+        - Actions are called respecting the order established by their "exec_order" field.
+
+        :param sp_layout - dictionary containing the SP layout information.
+        :param args - arguments to be propagated through the call of actions.
+        :param verbose - prints actions information in order of execution.
+        """
+        args["called"] = [] # for debug purposes
+        def append_called(action, sp, args :dict):
+            args["called"].append(f"{action.__name__} -> {sp}")
+            return args
+
+        for action in SpSetupActions.actions:
+            if verbose:
+                print(f"Calling {action}")
+            if action.global_action:
+                scope = "global"
+                args = action(sp_layout, scope, args)
+                args = append_called(action, scope, args)
+            else:
+                # Functions that are not global called for each SP defined in
+                # the SP layout.
+                for sp in sp_layout.keys():
+                    args = action(sp_layout, sp, args)
+                    args = append_called(action, sp, args)
+
+if __name__ == "__main__":
+    # Executing this module will have the following test code/playground executed
+    sp_layout = {
+        "partition1" : {
+            "boot-info": True,
+            "image": {
+                "file": "partition.bin",
+                "offset":"0x2000"
+            },
+            "pm": {
+                "file": "cactus.dts",
+                "offset":"0x1000"
+            },
+            "owner": "SiP"
+        },
+        "partition2" : {
+            "image": "partition.bin",
+            "pm": "cactus-secondary.dts",
+            "owner": "Plat"
+        },
+        "partition3" : {
+            "image": "partition.bin",
+            "pm": "cactus-tertiary.dts",
+            "owner": "Plat"
+        },
+        "partition4" : {
+            "image": "ivy.bin",
+            "pm": "ivy.dts",
+            "owner": "Plat"
+        }
+    }
+
+    #Example of how to use this module
+    @SpSetupActions.sp_action(global_action=True)
+    def my_action1(sp_layout, _, args :dict):
+        print(f"inside function my_action1{sp_layout}\n\n args:{args})")
+        return args # Always return args in action function.
+    @SpSetupActions.sp_action(exec_order=1)
+    def my_action2(sp_layout, sp_name, args :dict):
+        print(f"inside function my_action2; SP: {sp_name} {sp_layout} args:{args}")
+        return args
+
+    # Example arguments to be propagated through the functions.
+    # 'args' can be extended in the action functions.
+    args = dict()
+    args["arg1"] = 0xEEE
+    args["arg2"] = 0xFF
+    SpSetupActions.run_actions(sp_layout, args)
diff --git a/tools/sptool/sptool.c b/tools/sptool/sptool.c
deleted file mode 100644
index 38baa2c..0000000
--- a/tools/sptool/sptool.c
+++ /dev/null
@@ -1,360 +0,0 @@
-/*
- * Copyright (c) 2018-2020, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <stdarg.h>
-#include <stdbool.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-
-#include "sptool.h"
-
-#define PAGE_SIZE		4096
-
-/*
- * Entry describing Secure Partition package.
- */
-struct sp_pkg_info {
-	/* Location of the files in the host's RAM. */
-	void *img_data, *pm_data;
-
-	/* Size of the files. */
-	uint32_t img_size, pm_size;
-
-	/* Location of the binary files inside the package output file */
-	uint32_t img_offset, pm_offset;
-};
-
-/*
- * List of input provided by user
- */
-struct arg_list {
-	char *usr_input;
-	struct arg_list *next;
-};
-
-/* Align an address to a power-of-two boundary. */
-static unsigned int align_to(unsigned int address, unsigned int boundary)
-{
-	unsigned int mask = boundary - 1U;
-
-	if ((address & mask) != 0U)
-		return (address + boundary) & ~mask;
-	else
-		return address;
-}
-
-/* Allocate a memory area of 'size' bytes and zero it. */
-static void *xzalloc(size_t size, const char *msg)
-{
-	void *d;
-
-	d = malloc(size);
-	if (d == NULL) {
-		fprintf(stderr, "error: malloc: %s\n", msg);
-		exit(1);
-	}
-
-	memset(d, 0, size);
-
-	return d;
-}
-
-/*
- * Write 'size' bytes from 'buf' into the specified file stream.
- * Exit the program on error.
- */
-static void xfwrite(void *buf, size_t size, FILE *fp)
-{
-	if (fwrite(buf, 1, size, fp) != size) {
-		fprintf(stderr, "error: Failed to write to output file.\n");
-		exit(1);
-	}
-}
-
-/*
- * Set the file position indicator for the specified file stream.
- * Exit the program on error.
- */
-static void xfseek(FILE *fp, long offset, int whence)
-{
-	if (fseek(fp, offset, whence) != 0) {
-		fprintf(stderr, "error: Failed to set file to offset 0x%lx (%d).\n",
-		       offset, whence);
-		perror(NULL);
-		exit(1);
-	}
-}
-
-/*
- * Free SP package structure
- */
-static void cleanup(struct sp_pkg_info *sp)
-{
-
-	if (sp != NULL) {
-		if (sp->img_data != NULL) {
-			free(sp->img_data);
-		}
-
-		if (sp->pm_data != NULL) {
-			free(sp->pm_data);
-		}
-
-		free(sp);
-
-	}
-}
-
-/*
- * Free argument list structure
- */
-static void freelist(struct arg_list *head)
-{
-	struct arg_list *tmp;
-
-	while (head != NULL) {
-		tmp = head;
-		head = head->next;
-		free(tmp);
-	}
-}
-
-/*
- * Append user inputs in argument list structure
- */
-static void append_user_input(struct arg_list **head, char *args)
-{
-	struct arg_list *tmp = *head;
-
-	if (tmp == NULL) {
-		tmp = xzalloc(sizeof(struct arg_list),
-				"Failed to allocate arg_list struct");
-		tmp->usr_input = args;
-		*head = tmp;
-	} else {
-		while (tmp->next != NULL) {
-			tmp = tmp->next;
-		}
-		tmp->next = xzalloc(sizeof(struct arg_list),
-				"Failed to allocate arg_list struct");
-		tmp = tmp->next;
-		tmp->usr_input = args;
-	}
-}
-
-/*
- * Allocate a buffer big enough to store the content of the specified file and
- * load the file into it. Fill 'size' with the file size. Exit the program on
- * error.
- */
-static void load_file(const char *path, void **ptr, uint32_t *size)
-{
-	FILE *f = fopen(path, "rb");
-	if (f == NULL) {
-		fprintf(stderr, "error: %s couldn't be opened.\n", path);
-		exit(1);
-	}
-
-	xfseek(f, 0, SEEK_END);
-	*size = ftell(f);
-	if (*size == 0) {
-		fprintf(stderr, "error: Size of %s is 0\n", path);
-		exit(1);
-	}
-
-	rewind(f);
-
-	*ptr = malloc(*size);
-	if (*ptr == NULL) {
-		fprintf(stderr, "error: Not enough memory to load %s\n", path);
-		exit(1);
-	}
-
-	if (fread(*ptr, *size, 1, f) != 1) {
-		fprintf(stderr, "error: Couldn't read %s\n", path);
-		exit(1);
-	}
-
-	fclose(f);
-}
-
-/*
- * Parse the string containing input payloads and fill in the
- * SP Package data structure.
- */
-static void load_sp_pm(char *path, struct sp_pkg_info **sp_out)
-{
-	struct sp_pkg_info *sp_pkg;
-
-	char *split_mark = strstr(path, ":");
-
-	*split_mark = '\0';
-
-	char *sp_path = path;
-	char *pm_path = split_mark + 1;
-
-	sp_pkg = xzalloc(sizeof(struct sp_pkg_info),
-		"Failed to allocate sp_pkg_info struct");
-
-	load_file(pm_path, &sp_pkg->pm_data, &sp_pkg->pm_size);
-	printf("\nLoaded SP Manifest file %s (%u bytes)\n", pm_path, sp_pkg->pm_size);
-
-	load_file(sp_path, &sp_pkg->img_data, &sp_pkg->img_size);
-	printf("Loaded SP Image file %s (%u bytes)\n", sp_path, sp_pkg->img_size);
-
-	*sp_out = sp_pkg;
-}
-
-/*
- * Write SP package data structure into output file.
- */
-static void output_write(const char *path, struct sp_pkg_info *sp, bool header)
-{
-	struct sp_pkg_header sp_header_info;
-	unsigned int file_ptr = 0;
-
-	FILE *f = fopen(path, "wb");
-	if (f == NULL) {
-		fprintf(stderr, "error: Failed to open %s\n", path);
-		exit(1);
-	}
-
-	/* Reserve Header size */
-	if (header) {
-		file_ptr = sizeof(struct sp_pkg_header);
-	}
-
-	/* Save partition manifest */
-	xfseek(f, file_ptr, SEEK_SET);
-	printf("Writing SP Manifest at offset 0x%x (%u bytes)\n",
-	       file_ptr, sp->pm_size);
-
-	sp->pm_offset = file_ptr;
-	xfwrite(sp->pm_data, sp->pm_size, f);
-
-	/* Save partition image aligned to Page size */
-	file_ptr = align_to((sp->pm_offset + sp->pm_size), PAGE_SIZE);
-	xfseek(f, file_ptr, SEEK_SET);
-	printf("Writing SP Image at offset 0x%x (%u bytes)\n",
-	       file_ptr, sp->img_size);
-
-	sp->img_offset = file_ptr;
-	xfwrite(sp->img_data, sp->img_size, f);
-
-	/* Finally, write header, if needed */
-	if (header) {
-		sp_header_info.magic = SECURE_PARTITION_MAGIC;
-		sp_header_info.version = 0x1;
-		sp_header_info.img_offset = sp->img_offset;
-		sp_header_info.img_size = sp->img_size;
-		sp_header_info.pm_offset = sp->pm_offset;
-		sp_header_info.pm_size = sp->pm_size;
-
-		xfseek(f, 0, SEEK_SET);
-
-		printf("Writing package header\n");
-
-		xfwrite(&sp_header_info, sizeof(struct sp_pkg_header), f);
-	}
-
-	/* All information has been written now */
-	printf("\nsptool: Built Secure Partition blob %s\n", path);
-
-	fclose(f);
-}
-
-static void usage(void)
-{
-	printf("usage: sptool ");
-#ifdef VERSION
-	printf(VERSION);
-#else
-	/* If built from sptool directory, VERSION is not set. */
-	printf("version unknown");
-#endif
-	printf(" [<args>]\n\n");
-
-	printf("This tool takes as input set of image binary files and the\n"
-	       "partition manifest blobs as input and generates set of\n"
-	       "output package files\n"
-	       "Usage example: sptool -i sp1.bin:sp1.dtb -o sp1.pkg\n"
-	       "                      -i sp2.bin:sp2.dtb -o sp2.pkg ...\n\n");
-	printf("Commands supported:\n");
-	printf("  -o <path>            Set output file path.\n");
-	printf("  -i <sp_path:pm_path> Add Secure Partition image and\n"
-	       "                       Manifest blob (specified in two paths\n"
-	       "                       separated by a colon).\n");
-	printf("  -n                   Generate package without header\n");
-	printf("  -h                   Show this message.\n");
-	exit(1);
-}
-
-int main(int argc, char *argv[])
-{
-	struct sp_pkg_info *sp_pkg = NULL;
-	struct arg_list *in_head = NULL;
-	struct arg_list *out_head = NULL;
-	struct arg_list *in_list = NULL;
-	struct arg_list *out_list = NULL;
-	unsigned int match_counter = 0;
-	bool need_header = true;
-
-	int ch;
-
-	if (argc <= 1) {
-		fprintf(stderr, "error: File paths must be provided.\n\n");
-		usage();
-		return 1;
-	}
-
-	while ((ch = getopt(argc, argv, "hni:o:")) != -1) {
-		switch (ch) {
-		case 'i':
-			append_user_input(&in_head, optarg);
-			match_counter++;
-			break;
-		case 'o':
-			append_user_input(&out_head, optarg);
-			match_counter--;
-			break;
-		case 'n':
-			need_header = false;
-			break;
-		case 'h':
-		default:
-			usage();
-		}
-	}
-
-	if (match_counter) {
-		fprintf(stderr, "error: Input/Output count mismatch.\n\n");
-		freelist(in_head);
-		freelist(out_head);
-		usage();
-		return 1;
-	}
-
-	in_list = in_head;
-	out_list = out_head;
-	while (in_list != NULL) {
-		load_sp_pm(in_list->usr_input, &sp_pkg);
-		output_write(out_list->usr_input, sp_pkg, need_header);
-		in_list = in_list->next;
-		out_list = out_list->next;
-	}
-
-	argc -= optind;
-	argv += optind;
-
-	cleanup(sp_pkg);
-	freelist(in_head);
-	freelist(out_head);
-
-	return 0;
-}
diff --git a/tools/sptool/sptool.py b/tools/sptool/sptool.py
new file mode 100755
index 0000000..ae7df92
--- /dev/null
+++ b/tools/sptool/sptool.py
@@ -0,0 +1,145 @@
+#!/usr/bin/python3
+# Copyright (c) 2022, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+#
+# Copyright 2022 The Hafnium Authors.
+#
+# Use of this source code is governed by a BSD-style
+# license that can be found in the LICENSE file or at
+# https://opensource.org/licenses/BSD-3-Clause.
+
+"""
+Script which generates a Secure Partition package.
+https://trustedfirmware-a.readthedocs.io/en/latest/components/secure-partition-manager.html#secure-partition-packages
+"""
+
+import argparse
+from collections import namedtuple
+import sys
+from shutil import copyfileobj
+import os
+
+HF_PAGE_SIZE = 0x1000 # bytes
+HEADER_ELEMENT_BYTES = 4 # bytes
+MANIFEST_IMAGE_SPLITTER=':'
+PM_OFFSET_DEFAULT = "0x1000"
+IMG_OFFSET_DEFAULT = "0x4000"
+
+def split_dtb_bin(i : str):
+    return i.split(MANIFEST_IMAGE_SPLITTER)
+
+def align_to_page(n):
+    return HF_PAGE_SIZE * \
+          (round(n / HF_PAGE_SIZE) + \
+           (1 if n % HF_PAGE_SIZE else 0))
+
+def to_bytes(value):
+    return int(value).to_bytes(HEADER_ELEMENT_BYTES, 'little')
+
+class SpPkg:
+    def __init__(self, pm_path : str, img_path : str, pm_offset: int,
+                 img_offset: int):
+        if not os.path.isfile(pm_path) or not os.path.isfile(img_path):
+            raise Exception(f"Parameters should be path.  \
+                              manifest: {pm_path}; img: {img_path}")
+        self.pm_path = pm_path
+        self.img_path = img_path
+        self._SpPkgHeader = namedtuple("SpPkgHeader",
+                             ("magic", "version",
+                              "pm_offset", "pm_size",
+                              "img_offset", "img_size"))
+
+        if pm_offset >= img_offset:
+            raise ValueError("pm_offset must be smaller than img_offset")
+
+        is_hfpage_aligned = lambda val : val % HF_PAGE_SIZE == 0
+        if not is_hfpage_aligned(pm_offset) or not is_hfpage_aligned(img_offset):
+           raise ValueError(f"Offsets provided need to be page aligned: pm-{pm_offset}, img-{img_offset}")
+
+        if img_offset - pm_offset < self.pm_size:
+            raise ValueError(f"pm_offset and img_offset do not fit the specified file:{pm_path})")
+
+        self.pm_offset = pm_offset
+        self.img_offset = img_offset
+
+    def __str__(self):
+        return \
+        f'''--SP package Info--
+        header:{self.header}
+        pm: {self.pm_path}
+        img: {self.img_path}
+        '''
+
+    @property
+    def magic(self):
+        return "SPKG".encode()
+
+    @property
+    def version(self):
+        return 0x2
+
+    @property
+    def pm_size(self):
+        return os.path.getsize(self.pm_path)
+
+    @property
+    def img_size(self):
+        return os.path.getsize(self.img_path)
+
+    @property
+    def header(self):
+        return self._SpPkgHeader(
+                self.magic,
+                self.version,
+                self.pm_offset,
+                self.pm_size,
+                self.img_offset,
+                self.img_size)
+
+    @property
+    def header_size(self):
+        return len(self._SpPkgHeader._fields)
+
+    def generate(self, f_out : str):
+        with open(f_out, "wb+") as output:
+            for h in self.header:
+                to_write = h if type(h) is bytes else to_bytes(h)
+                output.write(to_write)
+            output.seek(self.pm_offset)
+            with open(self.pm_path, "rb") as pm:
+                copyfileobj(pm, output)
+            output.seek(self.img_offset)
+            with open(self.img_path, "rb") as img:
+                copyfileobj(img, output)
+
+def Main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("-i", required=True,
+                        help="path to partition's image and manifest separated by a colon.")
+    parser.add_argument("--pm-offset", required=False, default=PM_OFFSET_DEFAULT,
+                        help="set partitition manifest offset.")
+    parser.add_argument("--img-offset", required=False, default=IMG_OFFSET_DEFAULT,
+                        help="set partition image offset.")
+    parser.add_argument("-o", required=True, help="set output file path.")
+    parser.add_argument("-v", required=False, action="store_true",
+                        help="print package information.")
+    args = parser.parse_args()
+
+    if not os.path.exists(os.path.dirname(args.o)):
+        raise Exception("Provide a valid output file path!\n")
+
+    image_path, manifest_path = split_dtb_bin(args.i)
+    pm_offset = int(args.pm_offset, 0)
+    img_offset = int(args.img_offset, 0)
+    pkg = SpPkg(manifest_path, image_path, pm_offset, img_offset)
+    pkg.generate(args.o)
+
+    if args.v is True:
+        print(pkg)
+
+    return 0
+
+if __name__ == "__main__":
+    sys.exit(Main())