diff --git a/docs/about/features.rst b/docs/about/features.rst
index 4b7fbe5..cb8b552 100644
--- a/docs/about/features.rst
+++ b/docs/about/features.rst
@@ -46,8 +46,8 @@
 -  A Test SP and SPD to demonstrate AArch64 Secure Monitor functionality and SP
    interaction with PSCI.
 
--  SPDs for the `OP-TEE Secure OS`_, `NVIDIA Trusted Little Kernel`_
-   and `Trusty Secure OS`_.
+-  SPDs for the `OP-TEE Secure OS`_, `NVIDIA Trusted Little Kernel`_,
+   `Trusty Secure OS`_ and `ProvenCore Secure OS`_.
 
 -  A Trusted Board Boot implementation, conforming to all mandatory TBBR
    requirements. This includes image authentication, Firmware Update (or
@@ -121,6 +121,7 @@
 .. _OP-TEE Secure OS: https://github.com/OP-TEE/optee_os
 .. _NVIDIA Trusted Little Kernel: http://nv-tegra.nvidia.com/gitweb/?p=3rdparty/ote_partner/tlk.git;a=summary
 .. _Trusty Secure OS: https://source.android.com/security/trusty
+.. _ProvenCore Secure OS: https://provenrun.com/products/provencore/
 
 --------------
 
diff --git a/docs/about/maintainers.rst b/docs/about/maintainers.rst
index 96a0523..1f2c310 100644
--- a/docs/about/maintainers.rst
+++ b/docs/about/maintainers.rst
@@ -731,7 +731,7 @@
 :|G|: `michalsimek`_
 :|M|: Venkatesh Yadav Abbarapu <venkatesh.abbarapu@amd.com>
 :|G|: `venkatesh`_
-:|F|: docs/plat/xilinx-zynqmp.rst
+:|F|: docs/plat/xilinx\*
 :|F|: plat/xilinx/
 
 
@@ -772,6 +772,13 @@
 :|F|: bl32/tsp/
 :|F|: services/spd/tspd/
 
+ProvenCore Secure Payload Dispatcher
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+:|M|: Jérémie Corbier <jeremie.corbier@provenrun.com>
+:|G|: `jcorbier`_
+:|F|: docs/components/spd/pnc-dispatcher.rst
+:|F|: services/spd/pncd/
+
 Tools
 ~~~~~
 
@@ -842,6 +849,7 @@
 .. _grandpaul: https://github.com/grandpaul
 .. _hzhuang1: https://github.com/hzhuang1
 .. _JackyBai: https://github.com/JackyBai
+.. _jcorbier: https://github.com/jcorbier
 .. _jenswi-linaro: https://github.com/jenswi-linaro
 .. _jwerner-chromium: https://github.com/jwerner-chromium
 .. _kostapr: https://github.com/kostapr
diff --git a/docs/components/spd/index.rst b/docs/components/spd/index.rst
index 25d0124..6857806 100644
--- a/docs/components/spd/index.rst
+++ b/docs/components/spd/index.rst
@@ -8,3 +8,4 @@
    optee-dispatcher
    tlk-dispatcher
    trusty-dispatcher
+   pnc-dispatcher
diff --git a/docs/components/spd/pnc-dispatcher.rst b/docs/components/spd/pnc-dispatcher.rst
new file mode 100644
index 0000000..5be2fc7
--- /dev/null
+++ b/docs/components/spd/pnc-dispatcher.rst
@@ -0,0 +1,10 @@
+ProvenCore Dispatcher
+=====================
+
+ProvenCore dispatcher (PnC-D) adds support for ProvenRun's ProvenCore micro-kernel
+to work with Trusted Firmware-A (TF-A).
+
+ProvenCore is a secure OS developed by ProvenRun S.A.S. using deductive formal methods.
+
+Once a BL32 is ready, PnC-D can be included in the image by adding "SPD=pncd"
+to the build command.
diff --git a/docs/getting_started/prerequisites.rst b/docs/getting_started/prerequisites.rst
index 81c55a5..0ed8517 100644
--- a/docs/getting_started/prerequisites.rst
+++ b/docs/getting_started/prerequisites.rst
@@ -26,7 +26,7 @@
 |TF-A| can be built with any of the following *cross-compiler* toolchains that
 target the Armv7-A or Armv8-A architectures:
 
-- GCC >= 11.2-2022.02 (from the `Arm Developer website`_)
+- GCC >= 11.3.Rel1 (from the `Arm Developer website`_)
 - Clang >= 14.0.0
 - Arm Compiler >= 6.18
 
diff --git a/docs/plat/arm/arm-build-options.rst b/docs/plat/arm/arm-build-options.rst
index 339ebbe..afe89b9 100644
--- a/docs/plat/arm/arm-build-options.rst
+++ b/docs/plat/arm/arm-build-options.rst
@@ -152,6 +152,11 @@
     to select the appropriate platform variant for the build. The range of
     valid values is platform specific.
 
+- ``CSS_SYSTEM_GRACEFUL_RESET``: Build option to enable graceful powerdown of
+   CPU core on reset. This build option can be used on CSS platforms that
+   require all the CPUs to execute the CPU specific power down sequence to
+   complete a warm reboot sequence in which only the CPUs are power cycled.
+
 --------------
 
 .. |FIP in a GPT image| image:: ../../resources/diagrams/FIP_in_a_GPT_image.png
diff --git a/docs/plat/index.rst b/docs/plat/index.rst
index 1f497e0..28b1787 100644
--- a/docs/plat/index.rst
+++ b/docs/plat/index.rst
@@ -43,6 +43,7 @@
    synquacer
    stm32mp1
    ti-k3
+   xilinx-versal-net
    xilinx-versal
    xilinx-zynqmp
    brcm-stingray
diff --git a/docs/plat/xilinx-versal-net.rst b/docs/plat/xilinx-versal-net.rst
new file mode 100644
index 0000000..5d2e663
--- /dev/null
+++ b/docs/plat/xilinx-versal-net.rst
@@ -0,0 +1,31 @@
+Xilinx Versal NET
+=================
+
+Trusted Firmware-A implements the EL3 firmware layer for Xilinx Versal NET.
+The platform only uses the runtime part of TF-A as Xilinx Versal NET already
+has a BootROM (BL1) and PMC FW (BL2).
+
+BL31 is TF-A.
+BL32 is an optional Secure Payload.
+BL33 is the non-secure world software (U-Boot, Linux etc).
+
+To build:
+```bash
+make RESET_TO_BL31=1 CROSS_COMPILE=aarch64-none-elf- PLAT=versal_net bl31
+```
+
+Xilinx Versal NET platform specific build options
+-------------------------------------------------
+
+*   `VERSAL_NET_ATF_MEM_BASE`: Specifies the base address of the bl31 binary.
+*   `VERSAL_NET_ATF_MEM_SIZE`: Specifies the size of the memory region of the bl31 binary.
+*   `VERSAL_NET_BL32_MEM_BASE`: Specifies the base address of the bl32 binary.
+*   `VERSAL_NET_BL32_MEM_SIZE`: Specifies the size of the memory region of the bl32 binary.
+
+*   `VERSAL_NET_CONSOLE`: Select the console driver. Options:
+    -   `pl011`, `pl011_0`: ARM pl011 UART 0
+    -   `pl011_1`         : ARM pl011 UART 1
+
+*   `TFA_NO_PM` : Platform Management support.
+    -    0 : Enable Platform Management (Default)
+    -    1 : Disable Platform Management
diff --git a/drivers/amlogic/crypto/sha_dma.c b/drivers/amlogic/crypto/sha_dma.c
index fceb1c0..5c16d49 100644
--- a/drivers/amlogic/crypto/sha_dma.c
+++ b/drivers/amlogic/crypto/sha_dma.c
@@ -8,6 +8,7 @@
 #include <assert.h>
 #include <crypto/sha_dma.h>
 #include <lib/mmio.h>
+#include <platform_def.h>
 
 #include "aml_private.h"
 
diff --git a/drivers/arm/css/scp/css_pm_scmi.c b/drivers/arm/css/scp/css_pm_scmi.c
index 5de2604..9fe8b37 100644
--- a/drivers/arm/css/scp/css_pm_scmi.c
+++ b/drivers/arm/css/scp/css_pm_scmi.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -11,6 +11,7 @@
 #include <common/debug.h>
 #include <drivers/arm/css/css_scp.h>
 #include <drivers/arm/css/scmi.h>
+#include <lib/mmio.h>
 #include <plat/arm/common/plat_arm.h>
 #include <plat/arm/css/common/css_pm.h>
 #include <plat/common/platform.h>
@@ -286,15 +287,42 @@
 	return HW_OFF;
 }
 
+/*
+ * Callback function to raise a SGI designated to trigger the CPU power down
+ * sequence on all the online secondary cores.
+ */
+static void css_raise_pwr_down_interrupt(u_register_t mpidr)
+{
+#if CSS_SYSTEM_GRACEFUL_RESET
+	plat_ic_raise_el3_sgi(CSS_CPU_PWR_DOWN_REQ_INTR, mpidr);
+#endif
+}
+
 void __dead2 css_scp_system_off(int state)
 {
 	int ret;
 
 	/*
+	 * Before issuing the system power down command, set the trusted mailbox
+	 * to 0. This will ensure that in the case of a warm/cold reset, the
+	 * primary CPU executes from the cold boot sequence.
+	 */
+	mmio_write_64(PLAT_ARM_TRUSTED_MAILBOX_BASE, 0U);
+
+	/*
+	 * Send powerdown request to online secondary core(s)
+	 */
+	ret = psci_stop_other_cores(0, css_raise_pwr_down_interrupt);
+	if (ret != PSCI_E_SUCCESS) {
+		ERROR("Failed to powerdown secondary core(s)\n");
+	}
+
+	/*
 	 * Disable GIC CPU interface to prevent pending interrupt from waking
 	 * up the AP from WFI.
 	 */
 	plat_arm_gic_cpuif_disable();
+	plat_arm_gic_redistif_off();
 
 	/*
 	 * Issue SCMI command. First issue a graceful
@@ -309,6 +337,9 @@
 			state, ret);
 		panic();
 	}
+
+	/* Powerdown of primary core */
+	psci_pwrdown_cpu(PLAT_MAX_PWR_LVL);
 	wfi();
 	ERROR("CSS set power state: operation not handled.\n");
 	panic();
diff --git a/drivers/arm/gic/v2/gicv2_main.c b/drivers/arm/gic/v2/gicv2_main.c
index 939d097..1925a13 100644
--- a/drivers/arm/gic/v2/gicv2_main.c
+++ b/drivers/arm/gic/v2/gicv2_main.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Portions copyright (c) 2021-2022, ProvenRun S.A.S. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -417,7 +418,7 @@
  * The proc_num parameter must be the linear index of the target PE in the
  * system.
  ******************************************************************************/
-void gicv2_raise_sgi(int sgi_num, int proc_num)
+void gicv2_raise_sgi(int sgi_num, bool ns, int proc_num)
 {
 	unsigned int sgir_val, target;
 
@@ -437,7 +438,7 @@
 	target = driver_data->target_masks[proc_num];
 	assert(target != 0U);
 
-	sgir_val = GICV2_SGIR_VALUE(SGIR_TGT_SPECIFIC, target, sgi_num);
+	sgir_val = GICV2_SGIR_VALUE(SGIR_TGT_SPECIFIC, target, ns, sgi_num);
 
 	/*
 	 * Ensure that any shared variable updates depending on out of band
diff --git a/drivers/arm/gic/v3/gic-x00.c b/drivers/arm/gic/v3/gic-x00.c
index 75eb69a..83ef32f 100644
--- a/drivers/arm/gic/v3/gic-x00.c
+++ b/drivers/arm/gic/v3/gic-x00.c
@@ -16,6 +16,7 @@
 #include <assert.h>
 
 #include <arch_helpers.h>
+#include <common/debug.h>
 #include <drivers/arm/arm_gicv3_common.h>
 #include <drivers/arm/gicv3.h>
 
diff --git a/drivers/arm/gic/v3/gicv3_helpers.c b/drivers/arm/gic/v3/gicv3_helpers.c
index f3852d2..446d0ad 100644
--- a/drivers/arm/gic/v3/gicv3_helpers.c
+++ b/drivers/arm/gic/v3/gicv3_helpers.c
@@ -12,6 +12,8 @@
 #include <common/interrupt_props.h>
 #include <drivers/arm/gic_common.h>
 
+#include <platform_def.h>
+
 #include "../common/gic_common_private.h"
 #include "gicv3_private.h"
 
diff --git a/drivers/arm/gic/v3/gicv3_main.c b/drivers/arm/gic/v3/gicv3_main.c
index 8ead43b..bc93f93 100644
--- a/drivers/arm/gic/v3/gicv3_main.c
+++ b/drivers/arm/gic/v3/gicv3_main.c
@@ -1095,11 +1095,12 @@
 }
 
 /*******************************************************************************
- * This function raises the specified Secure Group 0 SGI.
+ * This function raises the specified SGI of the specified group.
  *
  * The target parameter must be a valid MPIDR in the system.
  ******************************************************************************/
-void gicv3_raise_secure_g0_sgi(unsigned int sgi_num, u_register_t target)
+void gicv3_raise_sgi(unsigned int sgi_num, gicv3_irq_group_t group,
+		u_register_t target)
 {
 	unsigned int tgt, aff3, aff2, aff1, aff0;
 	uint64_t sgi_val;
@@ -1129,7 +1130,22 @@
 	 * interrupt trigger are observed before raising SGI.
 	 */
 	dsbishst();
-	write_icc_sgi0r_el1(sgi_val);
+
+	switch (group) {
+	case GICV3_G0:
+		write_icc_sgi0r_el1(sgi_val);
+		break;
+	case GICV3_G1NS:
+		write_icc_asgi1r(sgi_val);
+		break;
+	case GICV3_G1S:
+		write_icc_sgi1r(sgi_val);
+		break;
+	default:
+		assert(false);
+		break;
+	}
+
 	isb();
 }
 
diff --git a/drivers/auth/cryptocell/713/cryptocell_crypto.c b/drivers/auth/cryptocell/713/cryptocell_crypto.c
index 077317e..3ac16af 100644
--- a/drivers/auth/cryptocell/713/cryptocell_crypto.c
+++ b/drivers/auth/cryptocell/713/cryptocell_crypto.c
@@ -8,6 +8,8 @@
 #include <stddef.h>
 #include <string.h>
 
+#include <platform_def.h>
+
 #include <drivers/arm/cryptocell/713/bsv_api.h>
 #include <drivers/arm/cryptocell/713/bsv_crypto_asym_api.h>
 #include <drivers/auth/crypto_mod.h>
diff --git a/drivers/console/multi_console.c b/drivers/console/multi_console.c
index 08b8e9f..a68a018 100644
--- a/drivers/console/multi_console.c
+++ b/drivers/console/multi_console.c
@@ -5,6 +5,7 @@
  */
 
 #include <assert.h>
+#include <stddef.h>
 
 #include <drivers/console.h>
 
diff --git a/drivers/measured_boot/rss/rss_measured_boot.c b/drivers/measured_boot/rss/rss_measured_boot.c
index fe2baf0..6cbb4dc 100644
--- a/drivers/measured_boot/rss/rss_measured_boot.c
+++ b/drivers/measured_boot/rss/rss_measured_boot.c
@@ -5,6 +5,7 @@
  */
 #include <assert.h>
 #include <stdint.h>
+#include <string.h>
 
 #include <common/debug.h>
 #include <drivers/auth/crypto_mod.h>
diff --git a/drivers/rpi3/gpio/rpi3_gpio.c b/drivers/rpi3/gpio/rpi3_gpio.c
index f938f56..55a8832 100644
--- a/drivers/rpi3/gpio/rpi3_gpio.c
+++ b/drivers/rpi3/gpio/rpi3_gpio.c
@@ -10,6 +10,7 @@
 #include <lib/mmio.h>
 #include <drivers/delay_timer.h>
 #include <drivers/rpi3/gpio/rpi3_gpio.h>
+#include <platform_def.h>
 
 static uintptr_t reg_base;
 
diff --git a/include/arch/aarch32/arch_helpers.h b/include/arch/aarch32/arch_helpers.h
index 0330989..95d056f 100644
--- a/include/arch/aarch32/arch_helpers.h
+++ b/include/arch/aarch32/arch_helpers.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
+ * Portions copyright (c) 2021-2022, ProvenRun S.A.S. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -282,6 +283,7 @@
 DEFINE_COPROCR_RW_FUNCS(icc_eoir1_el1, ICC_EOIR1)
 DEFINE_COPROCR_RW_FUNCS_64(icc_sgi0r_el1, ICC_SGI0R_EL1_64)
 DEFINE_COPROCR_WRITE_FUNC_64(icc_sgi1r, ICC_SGI1R_EL1_64)
+DEFINE_COPROCR_WRITE_FUNC_64(icc_asgi1r, ICC_ASGI1R_EL1_64)
 
 DEFINE_COPROCR_RW_FUNCS(sdcr, SDCR)
 DEFINE_COPROCR_RW_FUNCS(hdcr, HDCR)
@@ -402,6 +404,8 @@
 #define read_ctr_el0()		read_ctr()
 
 #define write_icc_sgi0r_el1(_v)	write64_icc_sgi0r_el1(_v)
+#define write_icc_sgi1r(_v)	write64_icc_sgi1r(_v)
+#define write_icc_asgi1r(_v)	write64_icc_asgi1r(_v)
 
 #define read_daif()		read_cpsr()
 #define write_daif(flags)	write_cpsr(flags)
diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index 3a2a032..f63e923 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -79,6 +79,7 @@
  ******************************************************************************/
 #define ICC_IGRPEN1_EL1		S3_0_C12_C12_7
 #define ICC_SGI1R		S3_0_C12_C11_5
+#define ICC_ASGI1R		S3_0_C12_C11_6
 #define ICC_SRE_EL1		S3_0_C12_C12_5
 #define ICC_SRE_EL2		S3_4_C12_C9_5
 #define ICC_SRE_EL3		S3_6_C12_C12_5
@@ -261,6 +262,15 @@
 #define ID_AA64ISAR1_SB_SUPPORTED	ULL(0x1)
 #define ID_AA64ISAR1_SB_NOT_SUPPORTED	ULL(0x0)
 
+/* ID_AA64ISAR2_EL1 definitions */
+#define ID_AA64ISAR2_EL1		S3_0_C0_C6_2
+
+#define ID_AA64ISAR2_GPA3_SHIFT		U(8)
+#define ID_AA64ISAR2_GPA3_MASK		ULL(0xf)
+
+#define ID_AA64ISAR2_APA3_SHIFT		U(12)
+#define ID_AA64ISAR2_APA3_MASK		ULL(0xf)
+
 /* ID_AA64MMFR0_EL1 definitions */
 #define ID_AA64MMFR0_EL1_PARANGE_SHIFT	U(0)
 #define ID_AA64MMFR0_EL1_PARANGE_MASK	ULL(0xf)
diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index 0af5b74..9ec114c 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h
@@ -35,15 +35,30 @@
 		ID_AA64MMFR2_EL1_CNP_MASK) != 0U;
 }
 
+static inline bool is_feat_pacqarma3_present(void)
+{
+	uint64_t mask_id_aa64isar2 =
+			(ID_AA64ISAR2_GPA3_MASK << ID_AA64ISAR2_GPA3_SHIFT) |
+			(ID_AA64ISAR2_APA3_MASK << ID_AA64ISAR2_APA3_SHIFT);
+
+	/* If any of the fields is not zero, QARMA3 algorithm is present */
+	return (read_id_aa64isar2_el1() & mask_id_aa64isar2) != 0U;
+}
+
 static inline bool is_armv8_3_pauth_present(void)
 {
-	uint64_t mask = (ID_AA64ISAR1_GPI_MASK << ID_AA64ISAR1_GPI_SHIFT) |
-			(ID_AA64ISAR1_GPA_MASK << ID_AA64ISAR1_GPA_SHIFT) |
-			(ID_AA64ISAR1_API_MASK << ID_AA64ISAR1_API_SHIFT) |
-			(ID_AA64ISAR1_APA_MASK << ID_AA64ISAR1_APA_SHIFT);
+	uint64_t mask_id_aa64isar1 =
+		(ID_AA64ISAR1_GPI_MASK << ID_AA64ISAR1_GPI_SHIFT) |
+		(ID_AA64ISAR1_GPA_MASK << ID_AA64ISAR1_GPA_SHIFT) |
+		(ID_AA64ISAR1_API_MASK << ID_AA64ISAR1_API_SHIFT) |
+		(ID_AA64ISAR1_APA_MASK << ID_AA64ISAR1_APA_SHIFT);
 
-	/* If any of the fields is not zero, PAuth is present */
-	return (read_id_aa64isar1_el1() & mask) != 0U;
+	/*
+	 * If any of the fields is not zero or QARMA3 is present,
+	 * PAuth is present
+	 */
+	return ((read_id_aa64isar1_el1() & mask_id_aa64isar1) != 0U ||
+		is_feat_pacqarma3_present());
 }
 
 static inline bool is_armv8_4_dit_present(void)
diff --git a/include/arch/aarch64/arch_helpers.h b/include/arch/aarch64/arch_helpers.h
index 10b0a0b..2a3eb72 100644
--- a/include/arch/aarch64/arch_helpers.h
+++ b/include/arch/aarch64/arch_helpers.h
@@ -250,6 +250,7 @@
 DEFINE_SYSREG_READ_FUNC(id_pfr1_el1)
 DEFINE_SYSREG_READ_FUNC(id_aa64isar0_el1)
 DEFINE_SYSREG_READ_FUNC(id_aa64isar1_el1)
+DEFINE_RENAME_SYSREG_READ_FUNC(id_aa64isar2_el1, ID_AA64ISAR2_EL1)
 DEFINE_SYSREG_READ_FUNC(id_aa64pfr0_el1)
 DEFINE_SYSREG_READ_FUNC(id_aa64pfr1_el1)
 DEFINE_SYSREG_READ_FUNC(id_aa64dfr0_el1)
@@ -492,6 +493,7 @@
 DEFINE_RENAME_SYSREG_WRITE_FUNC(icc_eoir1_el1, ICC_EOIR1_EL1)
 DEFINE_RENAME_SYSREG_WRITE_FUNC(icc_sgi0r_el1, ICC_SGI0R_EL1)
 DEFINE_RENAME_SYSREG_RW_FUNCS(icc_sgi1r, ICC_SGI1R)
+DEFINE_RENAME_SYSREG_RW_FUNCS(icc_asgi1r, ICC_ASGI1R)
 
 DEFINE_RENAME_SYSREG_READ_FUNC(amcfgr_el0, AMCFGR_EL0)
 DEFINE_RENAME_SYSREG_READ_FUNC(amcgcr_el0, AMCGCR_EL0)
diff --git a/include/bl31/interrupt_mgmt.h b/include/bl31/interrupt_mgmt.h
index 694f1f0..21af112 100644
--- a/include/bl31/interrupt_mgmt.h
+++ b/include/bl31/interrupt_mgmt.h
@@ -107,7 +107,7 @@
 
 static inline int32_t validate_el3_interrupt_rm(uint32_t x)
 {
-#if defined (EL3_EXCEPTION_HANDLING) && !(defined(SPD_spmd) && (SPMD_SPM_AT_SEL2 == 1))
+#if EL3_EXCEPTION_HANDLING && !(defined(SPD_spmd) && (SPMD_SPM_AT_SEL2 == 1))
 	/*
 	 * With EL3 exception handling, EL3 interrupts are always routed to EL3
 	 * from both Secure and Non-secure, when the SPMC does not live in S-EL2.
diff --git a/include/bl32/pnc/pnc.h b/include/bl32/pnc/pnc.h
new file mode 100644
index 0000000..03a3214
--- /dev/null
+++ b/include/bl32/pnc/pnc.h
@@ -0,0 +1,29 @@
+/*
+ * Copyright (c) 2021-2022, ProvenRun S.A.S. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef __PNC_H__
+#define __PNC_H__
+
+#define SMC_YIELD		0xbf000000
+#define SMC_ACTION_FROM_S	0xbf000001
+#define SMC_GET_SHAREDMEM	0xbf000002
+#define SMC_CONFIG_SHAREDMEM	0xbf000003
+#define SMC_ACTION_FROM_NS	0xbf000004
+
+#ifndef __ASSEMBLER__
+
+#include <stdint.h>
+
+void *pncd_context_switch_to(unsigned long security_state);
+int plat_pncd_setup(void);
+uintptr_t plat_pncd_smc_handler(uint32_t smc_fid, u_register_t x1,
+				u_register_t x2, u_register_t x3,
+				u_register_t x4, void *cookie, void *handle,
+				u_register_t flags);
+
+#endif /* __ASSEMBLER__ */
+
+#endif /* __PNC_H__ */
diff --git a/include/bl32/sp_min/platform_sp_min.h b/include/bl32/sp_min/platform_sp_min.h
index 971f661..a7dffff 100644
--- a/include/bl32/sp_min/platform_sp_min.h
+++ b/include/bl32/sp_min/platform_sp_min.h
@@ -9,6 +9,8 @@
 
 #include <stdint.h>
 
+#include <common/bl_common.h>
+
 /*******************************************************************************
  * Mandatory SP_MIN functions
  ******************************************************************************/
diff --git a/include/drivers/arm/gicv2.h b/include/drivers/arm/gicv2.h
index b960194..cfc168d 100644
--- a/include/drivers/arm/gicv2.h
+++ b/include/drivers/arm/gicv2.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2015-2018, ARM Limited and Contributors. All rights reserved.
+ * Portions copyright (c) 2021-2022, ProvenRun S.A.S. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -50,13 +51,15 @@
 #define SGIR_TGTLSTFLT_MASK	U(0x3)
 #define SGIR_TGTLST_SHIFT	16
 #define SGIR_TGTLST_MASK	U(0xff)
+#define SGIR_NSATT		(U(0x1) << 16)
 #define SGIR_INTID_MASK		ULL(0xf)
 
 #define SGIR_TGT_SPECIFIC	U(0)
 
-#define GICV2_SGIR_VALUE(tgt_lst_flt, tgt, intid) \
+#define GICV2_SGIR_VALUE(tgt_lst_flt, tgt, nsatt, intid) \
 	((((tgt_lst_flt) & SGIR_TGTLSTFLT_MASK) << SGIR_TGTLSTFLT_SHIFT) | \
 	 (((tgt) & SGIR_TGTLST_MASK) << SGIR_TGTLST_SHIFT) | \
+	 ((nsatt) ? SGIR_NSATT : U(0)) | \
 	 ((intid) & SGIR_INTID_MASK))
 
 /*******************************************************************************
@@ -127,6 +130,7 @@
 #ifndef __ASSEMBLER__
 
 #include <cdefs.h>
+#include <stdbool.h>
 #include <stdint.h>
 
 #include <common/interrupt_props.h>
@@ -185,7 +189,7 @@
 void gicv2_disable_interrupt(unsigned int id);
 void gicv2_set_interrupt_priority(unsigned int id, unsigned int priority);
 void gicv2_set_interrupt_type(unsigned int id, unsigned int type);
-void gicv2_raise_sgi(int sgi_num, int proc_num);
+void gicv2_raise_sgi(int sgi_num, bool ns, int proc_num);
 void gicv2_set_spi_routing(unsigned int id, int proc_num);
 void gicv2_set_interrupt_pending(unsigned int id);
 void gicv2_clear_interrupt_pending(unsigned int id);
diff --git a/include/drivers/arm/gicv3.h b/include/drivers/arm/gicv3.h
index 8371dd5..5bb22fd 100644
--- a/include/drivers/arm/gicv3.h
+++ b/include/drivers/arm/gicv3.h
@@ -315,7 +315,7 @@
 #define SGIR_IRM_SHIFT			40
 #define SGIR_IRM_MASK			ULL(0x1)
 #define SGIR_AFF3_SHIFT			48
-#define SGIR_AFF_MASK			ULL(0xf)
+#define SGIR_AFF_MASK			ULL(0xff)
 
 #define SGIR_IRM_TO_AFF			U(0)
 
@@ -354,6 +354,12 @@
 #include <drivers/arm/gic_common.h>
 #include <lib/utils_def.h>
 
+typedef enum {
+	GICV3_G1S,
+	GICV3_G1NS,
+	GICV3_G0
+} gicv3_irq_group_t;
+
 static inline uintptr_t gicv3_redist_size(uint64_t typer_val)
 {
 #if GIC_ENABLE_V4_EXTN
@@ -575,7 +581,8 @@
 		unsigned int priority);
 void gicv3_set_interrupt_type(unsigned int id, unsigned int proc_num,
 		unsigned int type);
-void gicv3_raise_secure_g0_sgi(unsigned int sgi_num, u_register_t target);
+void gicv3_raise_sgi(unsigned int sgi_num, gicv3_irq_group_t group,
+					 u_register_t target);
 void gicv3_set_spi_routing(unsigned int id, unsigned int irm,
 		u_register_t mpidr);
 void gicv3_set_interrupt_pending(unsigned int id, unsigned int proc_num);
diff --git a/include/lib/fconf/fconf.h b/include/lib/fconf/fconf.h
index 917e053..131c542 100644
--- a/include/lib/fconf/fconf.h
+++ b/include/lib/fconf/fconf.h
@@ -7,6 +7,7 @@
 #ifndef FCONF_H
 #define FCONF_H
 
+#include <stddef.h>
 #include <stdint.h>
 
 /* Public API */
diff --git a/include/lib/libc/assert.h b/include/lib/libc/assert.h
index 486bbc2..462bb43 100644
--- a/include/lib/libc/assert.h
+++ b/include/lib/libc/assert.h
@@ -9,8 +9,6 @@
 
 #include <cdefs.h>
 
-#include <platform_def.h>
-
 #include <common/debug.h>
 
 #ifndef PLAT_LOG_LEVEL_ASSERT
@@ -18,9 +16,7 @@
 #endif
 
 #if ENABLE_ASSERTIONS
-# if PLAT_LOG_LEVEL_ASSERT >= LOG_LEVEL_VERBOSE
-#  define assert(e)	((e) ? (void)0 : __assert(__FILE__, __LINE__, #e))
-# elif PLAT_LOG_LEVEL_ASSERT >= LOG_LEVEL_INFO
+# if PLAT_LOG_LEVEL_ASSERT >= LOG_LEVEL_INFO
 #  define assert(e)	((e) ? (void)0 : __assert(__FILE__, __LINE__))
 # else
 #  define assert(e)	((e) ? (void)0 : __assert())
@@ -29,10 +25,7 @@
 #define assert(e)	((void)0)
 #endif /* ENABLE_ASSERTIONS */
 
-#if PLAT_LOG_LEVEL_ASSERT >= LOG_LEVEL_VERBOSE
-void __dead2 __assert(const char *file, unsigned int line,
-		      const char *assertion);
-#elif PLAT_LOG_LEVEL_ASSERT >= LOG_LEVEL_INFO
+#if PLAT_LOG_LEVEL_ASSERT >= LOG_LEVEL_INFO
 void __dead2 __assert(const char *file, unsigned int line);
 #else
 void __dead2 __assert(void);
diff --git a/include/lib/psci/psci_lib.h b/include/lib/psci/psci_lib.h
index 43e2f96..3edc50b 100644
--- a/include/lib/psci/psci_lib.h
+++ b/include/lib/psci/psci_lib.h
@@ -92,6 +92,7 @@
 int psci_stop_other_cores(unsigned int wait_ms,
 			  void (*stop_func)(u_register_t mpidr));
 bool psci_is_last_on_cpu_safe(void);
+void psci_pwrdown_cpu(unsigned int power_level);
 
 #endif /* __ASSEMBLER__ */
 
diff --git a/include/plat/arm/common/fconf_arm_sp_getter.h b/include/plat/arm/common/fconf_arm_sp_getter.h
index aa628df..96ed963 100644
--- a/include/plat/arm/common/fconf_arm_sp_getter.h
+++ b/include/plat/arm/common/fconf_arm_sp_getter.h
@@ -7,6 +7,7 @@
 #ifndef FCONF_ARM_SP_GETTER_H
 #define FCONF_ARM_SP_GETTER_H
 
+#include <common/tbbr/tbbr_img_def.h>
 #include <lib/fconf/fconf.h>
 #include <tools_share/uuid.h>
 
diff --git a/include/plat/arm/css/common/css_pm.h b/include/plat/arm/css/common/css_pm.h
index e5357f5..84e6b38 100644
--- a/include/plat/arm/css/common/css_pm.h
+++ b/include/plat/arm/css/common/css_pm.h
@@ -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
  */
@@ -12,6 +12,9 @@
 
 #include <lib/psci/psci.h>
 
+/* SGI used to trigger per-core power down request */
+#define CSS_CPU_PWR_DOWN_REQ_INTR	ARM_IRQ_SEC_SGI_7
+
 /* Macros to read the CSS power domain state */
 #define CSS_CORE_PWR_STATE(state)	(state)->pwr_domain_state[ARM_PWR_LVL0]
 #define CSS_CLUSTER_PWR_STATE(state)	(state)->pwr_domain_state[ARM_PWR_LVL1]
@@ -37,6 +40,9 @@
 void css_cpu_standby(plat_local_state_t cpu_state);
 void css_get_sys_suspend_power_state(psci_power_state_t *req_state);
 int css_node_hw_state(u_register_t mpidr, unsigned int power_level);
+void css_setup_cpu_pwr_down_intr(void);
+int css_reboot_interrupt_handler(uint32_t intr_raw, uint32_t flags,
+		void *handle, void *cookie);
 
 /*
  * This mapping array has to be exported by the platform. Each element at
diff --git a/include/plat/common/platform.h b/include/plat/common/platform.h
index 184606a..31607c2 100644
--- a/include/plat/common/platform.h
+++ b/include/plat/common/platform.h
@@ -104,6 +104,8 @@
 void plat_ic_set_interrupt_type(unsigned int id, unsigned int type);
 void plat_ic_set_interrupt_priority(unsigned int id, unsigned int priority);
 void plat_ic_raise_el3_sgi(int sgi_num, u_register_t target);
+void plat_ic_raise_ns_sgi(int sgi_num, u_register_t target);
+void plat_ic_raise_s_el1_sgi(int sgi_num, u_register_t target);
 void plat_ic_set_spi_routing(unsigned int id, unsigned int routing_mode,
 		u_register_t mpidr);
 void plat_ic_set_interrupt_pending(unsigned int id);
diff --git a/include/plat/marvell/armada/a8k/common/plat_marvell.h b/include/plat/marvell/armada/a8k/common/plat_marvell.h
index 5d805a7..bec21a0 100644
--- a/include/plat/marvell/armada/a8k/common/plat_marvell.h
+++ b/include/plat/marvell/armada/a8k/common/plat_marvell.h
@@ -10,6 +10,7 @@
 
 #include <stdint.h>
 
+#include <common/bl_common.h>
 #include <lib/cassert.h>
 #include <lib/el3_runtime/cpu_data.h>
 #include <lib/utils.h>
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 4355b12..d88bea7 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -16,6 +16,7 @@
 #include <arch_features.h>
 #include <bl31/interrupt_mgmt.h>
 #include <common/bl_common.h>
+#include <common/debug.h>
 #include <context.h>
 #include <drivers/arm/gicv3.h>
 #include <lib/el3_runtime/context_mgmt.h>
diff --git a/lib/fconf/fconf_dyn_cfg_getter.c b/lib/fconf/fconf_dyn_cfg_getter.c
index 3038c09..351772e 100644
--- a/lib/fconf/fconf_dyn_cfg_getter.c
+++ b/lib/fconf/fconf_dyn_cfg_getter.c
@@ -12,6 +12,8 @@
 #include <lib/object_pool.h>
 #include <libfdt.h>
 
+#include <platform_def.h>
+
 /* We currently use FW, TB_FW, SOC_FW, TOS_FW, NT_FW and HW configs  */
 #define MAX_DTB_INFO	U(6)
 /*
diff --git a/lib/libc/assert.c b/lib/libc/assert.c
index c199de6..8973ed5 100644
--- a/lib/libc/assert.c
+++ b/lib/libc/assert.c
@@ -17,16 +17,7 @@
  * LOG_LEVEL_INFO, which is the default value for builds with DEBUG=1.
  */
 
-#if PLAT_LOG_LEVEL_ASSERT >= LOG_LEVEL_VERBOSE
-void __dead2 __assert(const char *file, unsigned int line,
-		      const char *assertion)
-{
-	printf("ASSERT: %s:%u:%s\n", file, line, assertion);
-	backtrace("assert");
-	console_flush();
-	plat_panic_handler();
-}
-#elif PLAT_LOG_LEVEL_ASSERT >= LOG_LEVEL_INFO
+#if PLAT_LOG_LEVEL_ASSERT >= LOG_LEVEL_INFO
 void __dead2 __assert(const char *file, unsigned int line)
 {
 	printf("ASSERT: %s:%u\n", file, line);
diff --git a/lib/libc/printf.c b/lib/libc/printf.c
index 45e153e..0def148 100644
--- a/lib/libc/printf.c
+++ b/lib/libc/printf.c
@@ -9,8 +9,6 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include <common/debug.h>
-
 #define get_num_va_args(_args, _lcount)				\
 	(((_lcount) > 1)  ? va_arg(_args, long long int) :	\
 	(((_lcount) == 1) ? va_arg(_args, long int) :		\
@@ -43,6 +41,12 @@
 	int i = 0, count = 0;
 	unsigned int rem;
 
+	/* num_buf is only large enough for radix >= 10 */
+	if (radix < 10) {
+		assert(0);
+		return 0;
+	}
+
 	do {
 		rem = unum % radix;
 		if (rem < 0xa)
diff --git a/lib/libc/snprintf.c b/lib/libc/snprintf.c
index 12f51c0..6a2f0ba 100644
--- a/lib/libc/snprintf.c
+++ b/lib/libc/snprintf.c
@@ -6,11 +6,10 @@
 
 #include <assert.h>
 #include <stdarg.h>
+#include <stdbool.h>
+#include <stddef.h>
 #include <stdint.h>
 
-#include <common/debug.h>
-#include <plat/common/platform.h>
-
 #define get_num_va_args(_args, _lcount)				\
 	(((_lcount) > 1)  ? va_arg(_args, long long int) :	\
 	(((_lcount) == 1) ? va_arg(_args, long int) :		\
@@ -51,10 +50,10 @@
 	unsigned int rem;
 	char ascii_a = capitalise ? 'A' : 'a';
 
+	/* num_buf is only large enough for radix >= 10 */
 	if (radix < 10) {
-		ERROR("snprintf: unsupported radix '%u'.", radix);
-		plat_panic_handler();
-		assert(0); /* Unreachable */
+		assert(0);
+		return;
 	}
 
 	do {
@@ -218,11 +217,8 @@
 				break;
 
 			default:
-				/* Panic on any other format specifier. */
-				ERROR("snprintf: specifier with ASCII code '%d' not supported.",
-				      *fmt);
-				plat_panic_handler();
-				assert(0); /* Unreachable */
+				CHECK_AND_PUT_CHAR(s, n, chars_printed, '%');
+				CHECK_AND_PUT_CHAR(s, n, chars_printed, *fmt);
 			}
 			fmt++;
 			continue;
diff --git a/lib/optee/optee_utils.c b/lib/optee/optee_utils.c
index d30248f..6c87b0d 100644
--- a/lib/optee/optee_utils.c
+++ b/lib/optee/optee_utils.c
@@ -9,6 +9,8 @@
 #include <common/debug.h>
 #include <lib/optee_utils.h>
 
+#include <platform_def.h>
+
 /*
  * load_addr_hi and load_addr_lo: image load address.
  * image_id: 0 - pager, 1 - paged
diff --git a/lib/psci/psci_common.c b/lib/psci/psci_common.c
index b60ddbb..efcfed8 100644
--- a/lib/psci/psci_common.c
+++ b/lib/psci/psci_common.c
@@ -954,7 +954,7 @@
  * Initiate power down sequence, by calling power down operations registered for
  * this CPU.
  ******************************************************************************/
-void psci_do_pwrdown_sequence(unsigned int power_level)
+void psci_pwrdown_cpu(unsigned int power_level)
 {
 #if HW_ASSISTED_COHERENCY
 	/*
diff --git a/lib/psci/psci_off.c b/lib/psci/psci_off.c
index 5447045..637adb9 100644
--- a/lib/psci/psci_off.c
+++ b/lib/psci/psci_off.c
@@ -109,7 +109,7 @@
 	/*
 	 * Arch. management. Initiate power down sequence.
 	 */
-	psci_do_pwrdown_sequence(psci_find_max_off_lvl(&state_info));
+	psci_pwrdown_cpu(psci_find_max_off_lvl(&state_info));
 
 #if ENABLE_RUNTIME_INSTRUMENTATION
 	PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
diff --git a/lib/psci/psci_private.h b/lib/psci/psci_private.h
index 61bd966..caade9c 100644
--- a/lib/psci/psci_private.h
+++ b/lib/psci/psci_private.h
@@ -296,7 +296,6 @@
 void psci_print_power_domain_map(void);
 unsigned int psci_is_last_on_cpu(void);
 int psci_spd_migrate_info(u_register_t *mpidr);
-void psci_do_pwrdown_sequence(unsigned int power_level);
 
 /*
  * CPU power down is directly called only when HW_ASSISTED_COHERENCY is
diff --git a/lib/psci/psci_suspend.c b/lib/psci/psci_suspend.c
index ffe3a91..f71994d 100644
--- a/lib/psci/psci_suspend.c
+++ b/lib/psci/psci_suspend.c
@@ -124,7 +124,7 @@
 	 * TODO : Introduce a mechanism to query the cache level to flush
 	 * and the cpu-ops power down to perform from the platform.
 	 */
-	psci_do_pwrdown_sequence(max_off_lvl);
+	psci_pwrdown_cpu(max_off_lvl);
 
 #if ENABLE_RUNTIME_INSTRUMENTATION
 	PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
diff --git a/plat/arm/board/fvp/fvp_gicv3.c b/plat/arm/board/fvp/fvp_gicv3.c
index 8f3e7b7..e780f21 100644
--- a/plat/arm/board/fvp/fvp_gicv3.c
+++ b/plat/arm/board/fvp/fvp_gicv3.c
@@ -7,6 +7,7 @@
 #include <assert.h>
 #include <platform_def.h>
 
+#include <common/debug.h>
 #include <common/interrupt_props.h>
 #include <drivers/arm/gicv3.h>
 #include <fconf_hw_config_getter.h>
diff --git a/plat/arm/board/juno/include/platform_def.h b/plat/arm/board/juno/include/platform_def.h
index 3265b0b..409d7a6 100644
--- a/plat/arm/board/juno/include/platform_def.h
+++ b/plat/arm/board/juno/include/platform_def.h
@@ -196,12 +196,6 @@
 # define PLATFORM_STACK_SIZE		UL(0x440)
 #endif
 
-/*
- * Since free SRAM space is scant, enable the ASSERTION message size
- * optimization by fixing the PLAT_LOG_LEVEL_ASSERT to LOG_LEVEL_INFO (40).
- */
-#define PLAT_LOG_LEVEL_ASSERT		40
-
 /* CCI related constants */
 #define PLAT_ARM_CCI_BASE		UL(0x2c090000)
 #define PLAT_ARM_CCI_CLUSTER0_SL_IFACE_IX	4
diff --git a/plat/arm/board/rdn2/include/platform_def.h b/plat/arm/board/rdn2/include/platform_def.h
index 464a157..3474016 100644
--- a/plat/arm/board/rdn2/include/platform_def.h
+++ b/plat/arm/board/rdn2/include/platform_def.h
@@ -96,4 +96,8 @@
 #define PLAT_ARM_GICR_BASE		UL(0x301C0000)
 #endif
 
+/* Interrupt priority level for shutdown/reboot */
+#define PLAT_REBOOT_PRI		GIC_HIGHEST_SEC_PRIORITY
+#define PLAT_EHF_DESC		EHF_PRI_DESC(PLAT_PRI_BITS, PLAT_REBOOT_PRI)
+
 #endif /* PLATFORM_DEF_H */
diff --git a/plat/arm/board/rdn2/platform.mk b/plat/arm/board/rdn2/platform.mk
index b882dc8..cfe4e28 100644
--- a/plat/arm/board/rdn2/platform.mk
+++ b/plat/arm/board/rdn2/platform.mk
@@ -1,4 +1,4 @@
-# Copyright (c) 2020-2021, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2020-2022, Arm Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -24,6 +24,9 @@
 GICV3_IMPL_GIC600_MULTICHIP	:=	1
 endif
 
+override CSS_SYSTEM_GRACEFUL_RESET	:= 1
+override EL3_EXCEPTION_HANDLING		:= 1
+
 include plat/arm/css/sgi/sgi-common.mk
 
 RDN2_BASE		=	plat/arm/board/rdn2
diff --git a/plat/arm/common/arm_bl1_setup.c b/plat/arm/common/arm_bl1_setup.c
index 7a9e04d..7000236 100644
--- a/plat/arm/common/arm_bl1_setup.c
+++ b/plat/arm/common/arm_bl1_setup.c
@@ -11,6 +11,7 @@
 #include <arch.h>
 #include <bl1/bl1.h>
 #include <common/bl_common.h>
+#include <common/debug.h>
 #include <lib/fconf/fconf.h>
 #include <lib/fconf/fconf_dyn_cfg_getter.h>
 #include <lib/utils.h>
diff --git a/plat/arm/common/arm_dyn_cfg_helpers.c b/plat/arm/common/arm_dyn_cfg_helpers.c
index 6a2a6f8..e88ea65 100644
--- a/plat/arm/common/arm_dyn_cfg_helpers.c
+++ b/plat/arm/common/arm_dyn_cfg_helpers.c
@@ -6,6 +6,7 @@
 
 #include <assert.h>
 
+#include <common/debug.h>
 #if MEASURED_BOOT
 #include <common/desc_image_load.h>
 #endif
diff --git a/plat/arm/common/arm_gicv3.c b/plat/arm/common/arm_gicv3.c
index 4a3a22e..469e22a 100644
--- a/plat/arm/common/arm_gicv3.c
+++ b/plat/arm/common/arm_gicv3.c
@@ -7,6 +7,7 @@
 #include <assert.h>
 #include <platform_def.h>
 
+#include <common/debug.h>
 #include <common/interrupt_props.h>
 #include <drivers/arm/gicv3.h>
 #include <lib/utils.h>
diff --git a/plat/arm/css/common/css_common.mk b/plat/arm/css/common/css_common.mk
index 2fbbe45..1e4851c 100644
--- a/plat/arm/css/common/css_common.mk
+++ b/plat/arm/css/common/css_common.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015-2018, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -35,6 +35,7 @@
 				drivers/arm/css/scmi/scmi_common.c		\
 				drivers/arm/css/scmi/scmi_pwr_dmn_proto.c	\
 				drivers/arm/css/scmi/scmi_sys_pwr_proto.c	\
+				drivers/delay_timer/delay_timer.c		\
 				drivers/arm/css/scp/css_pm_scmi.c
 endif
 
@@ -88,3 +89,9 @@
 $(eval $(call assert_boolean,CSS_NON_SECURE_UART))
 $(eval $(call add_define,CSS_NON_SECURE_UART))
 
+# Process CSS_SYSTEM_GRACEFUL_RESET flag
+# This build option can be used on CSS platforms that require all the CPUs
+# to execute the CPU specific power down sequence to complete a warm reboot
+# sequence in which only the CPUs are power cycled.
+CSS_SYSTEM_GRACEFUL_RESET	:= 0
+$(eval $(call add_define,CSS_SYSTEM_GRACEFUL_RESET))
diff --git a/plat/arm/css/common/css_pm.c b/plat/arm/css/common/css_pm.c
index 926b8ec..9b2639c 100644
--- a/plat/arm/css/common/css_pm.c
+++ b/plat/arm/css/common/css_pm.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
  */
@@ -9,10 +9,14 @@
 #include <platform_def.h>
 
 #include <arch_helpers.h>
+#include <bl31/interrupt_mgmt.h>
 #include <common/debug.h>
 #include <drivers/arm/css/css_scp.h>
 #include <lib/cassert.h>
 #include <plat/arm/common/plat_arm.h>
+
+#include <plat/common/platform.h>
+
 #include <plat/arm/css/common/css_pm.h>
 
 /* Allow CSS platforms to override `plat_arm_psci_pm_ops` */
@@ -110,6 +114,9 @@
 
 	/* Enable the gic cpu interface */
 	plat_arm_gic_cpuif_enable();
+
+	/* Setup the CPU power down request interrupt for secondary core(s) */
+	css_setup_cpu_pwr_down_intr();
 }
 
 /*******************************************************************************
@@ -331,6 +338,52 @@
 	return arm_validate_power_state(power_state, output_state);
 }
 
+/*
+ * Setup the SGI interrupt that will be used trigger the execution of power
+ * down sequence for all the secondary cores. This interrupt is setup to be
+ * handled in EL3 context at a priority defined by the platform.
+ */
+void css_setup_cpu_pwr_down_intr(void)
+{
+#if CSS_SYSTEM_GRACEFUL_RESET
+	plat_ic_set_interrupt_type(CSS_CPU_PWR_DOWN_REQ_INTR, INTR_TYPE_EL3);
+	plat_ic_set_interrupt_priority(CSS_CPU_PWR_DOWN_REQ_INTR,
+			PLAT_REBOOT_PRI);
+	plat_ic_enable_interrupt(CSS_CPU_PWR_DOWN_REQ_INTR);
+#endif
+}
+
+/*
+ * For a graceful shutdown/reboot, each CPU in the system should do their power
+ * down sequence. On a PSCI shutdown/reboot request, only one CPU gets an
+ * opportunity to do the powerdown sequence. To achieve graceful reset, of all
+ * cores in the system, the CPU gets the opportunity raise warm reboot SGI to
+ * rest of the CPUs which are online. Add handler for the reboot SGI where the
+ * rest of the CPU execute the powerdown sequence.
+ */
+int css_reboot_interrupt_handler(uint32_t intr_raw, uint32_t flags,
+		void *handle, void *cookie)
+{
+	assert(intr_raw == CSS_CPU_PWR_DOWN_REQ_INTR);
+
+	/* Deactivate warm reboot SGI */
+	plat_ic_end_of_interrupt(CSS_CPU_PWR_DOWN_REQ_INTR);
+
+	/*
+	 * Disable GIC CPU interface to prevent pending interrupt from waking
+	 * up the AP from WFI.
+	 */
+	plat_arm_gic_cpuif_disable();
+	plat_arm_gic_redistif_off();
+
+	psci_pwrdown_cpu(PLAT_MAX_PWR_LVL);
+
+	dmbsy();
+
+	wfi();
+	return 0;
+}
+
 /*******************************************************************************
  * Export the platform handlers via plat_arm_psci_pm_ops. The ARM Standard
  * platform will take care of registering the handlers with PSCI.
diff --git a/plat/arm/css/common/sp_min/css_sp_min.mk b/plat/arm/css/common/sp_min/css_sp_min.mk
index 6523a16..ae489fd 100644
--- a/plat/arm/css/common/sp_min/css_sp_min.mk
+++ b/plat/arm/css/common/sp_min/css_sp_min.mk
@@ -15,6 +15,7 @@
 else
 BL32_SOURCES		+=	drivers/arm/css/mhu/css_mhu_doorbell.c		\
 				drivers/arm/css/scp/css_pm_scmi.c		\
+				drivers/delay_timer/delay_timer.c		\
 				drivers/arm/css/scmi/scmi_common.c		\
 				drivers/arm/css/scmi/scmi_pwr_dmn_proto.c	\
 				drivers/arm/css/scmi/scmi_sys_pwr_proto.c
diff --git a/plat/arm/css/sgi/include/sgi_base_platform_def.h b/plat/arm/css/sgi/include/sgi_base_platform_def.h
index 22870c4..c1fadc6 100644
--- a/plat/arm/css/sgi/include/sgi_base_platform_def.h
+++ b/plat/arm/css/sgi/include/sgi_base_platform_def.h
@@ -19,7 +19,7 @@
 					CSS_SGI_MAX_CPUS_PER_CLUSTER *	\
 					CSS_SGI_MAX_PE_PER_CPU)
 
-#define PLAT_ARM_TRUSTED_SRAM_SIZE	0x00040000	/* 256 KB */
+#define PLAT_ARM_TRUSTED_SRAM_SIZE	0x00080000	/* 512 KB */
 
 /* Remote chip address offset */
 #define CSS_SGI_REMOTE_CHIP_MEM_OFFSET(n)	\
@@ -99,10 +99,16 @@
 
 /*
  * 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
+ * calculated using the current BL31 PROGBITS debug size plus the sizes of BL2
+ * and BL1-RW. CSS_SGI_BL31_SIZE - is tuned with respect to the actual BL31
+ * PROGBITS size which is around 64-68KB at the time this change is being made.
+ * A buffer of ~35KB is added to account for future expansion of the image,
+ * making it a total of 100KB.
  */
-#define PLAT_ARM_MAX_BL31_SIZE		0x48000
+#define CSS_SGI_BL31_SIZE		(100 * 1024)	/* 100 KB */
+#define PLAT_ARM_MAX_BL31_SIZE		(CSS_SGI_BL31_SIZE +		\
+						PLAT_ARM_MAX_BL2_SIZE +	\
+						PLAT_ARM_MAX_BL1_RW_SIZE)
 
 /*
  * Size of cacheable stacks
diff --git a/plat/arm/css/sgi/sgi-common.mk b/plat/arm/css/sgi/sgi-common.mk
index 4af579e..6c1a2dd 100644
--- a/plat/arm/css/sgi/sgi-common.mk
+++ b/plat/arm/css/sgi/sgi-common.mk
@@ -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
 #
@@ -69,7 +69,6 @@
 
 override CSS_LOAD_SCP_IMAGES	:=	0
 override NEED_BL2U		:=	no
-override ARM_BL31_IN_DRAM	:=	1
 override ARM_PLAT_MT		:=	1
 override PSCI_EXTENDED_STATE_ID	:=	1
 override ARM_RECOM_STATE_ID_ENC	:=	1
diff --git a/plat/arm/css/sgi/sgi_bl31_setup.c b/plat/arm/css/sgi/sgi_bl31_setup.c
index 99f2f20..7ef7e6f 100644
--- a/plat/arm/css/sgi/sgi_bl31_setup.c
+++ b/plat/arm/css/sgi/sgi_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * 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
  */
@@ -13,8 +13,11 @@
 #include <drivers/arm/css/css_mhu_doorbell.h>
 #include <drivers/arm/css/scmi.h>
 #include <plat/arm/common/plat_arm.h>
+
 #include <plat/common/platform.h>
 
+#include <plat/arm/css/common/css_pm.h>
+
 #include <sgi_ras.h>
 #include <sgi_variant.h>
 
@@ -105,6 +108,15 @@
 #if RAS_EXTENSION
 	sgi_ras_intr_handler_setup();
 #endif
+
+	/* Configure the warm reboot SGI for primary core */
+	css_setup_cpu_pwr_down_intr();
+
+#if CSS_SYSTEM_GRACEFUL_RESET
+	/* Register priority level handlers for reboot */
+	ehf_register_priority_handler(PLAT_REBOOT_PRI,
+			css_reboot_interrupt_handler);
+#endif
 }
 
 const plat_psci_ops_t *plat_arm_psci_override_pm_ops(plat_psci_ops_t *ops)
diff --git a/plat/brcm/board/common/cmn_plat_def.h b/plat/brcm/board/common/cmn_plat_def.h
index 8aa7fd4..79d9a29 100644
--- a/plat/brcm/board/common/cmn_plat_def.h
+++ b/plat/brcm/board/common/cmn_plat_def.h
@@ -8,6 +8,7 @@
 #define CMN_PLAT_DEF_H
 
 #include <bcm_elog.h>
+#include <platform_def.h>
 
 #ifndef GET_LOG_LEVEL
 #define GET_LOG_LEVEL() LOG_LEVEL
@@ -57,9 +58,6 @@
 			}						 \
 		} while (0)
 
-/* Print file and line number on assert */
-#define PLAT_LOG_LEVEL_ASSERT LOG_LEVEL_INFO
-
 /*
  * The number of regions like RO(code), coherent and data required by
  * different BL stages which need to be mapped in the MMU.
diff --git a/plat/common/aarch64/plat_common.c b/plat/common/aarch64/plat_common.c
index e807660..8f998af 100644
--- a/plat/common/aarch64/plat_common.c
+++ b/plat/common/aarch64/plat_common.c
@@ -9,6 +9,7 @@
 #include <stdint.h>
 
 #include <arch_helpers.h>
+#include <common/debug.h>
 #include <drivers/console.h>
 #if RAS_EXTENSION
 #include <lib/extensions/ras.h>
diff --git a/plat/common/plat_gicv2.c b/plat/common/plat_gicv2.c
index 4c76f1b..0f988dc 100644
--- a/plat/common/plat_gicv2.c
+++ b/plat/common/plat_gicv2.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2015-2018, ARM Limited and Contributors. All rights reserved.
+ * Portions copyright (c) 2021-2022, ProvenRun S.A.S. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -34,6 +35,8 @@
 #pragma weak plat_ic_set_interrupt_priority
 #pragma weak plat_ic_set_interrupt_type
 #pragma weak plat_ic_raise_el3_sgi
+#pragma weak plat_ic_raise_ns_sgi
+#pragma weak plat_ic_raise_s_el1_sgi
 #pragma weak plat_ic_set_spi_routing
 
 /*
@@ -247,12 +250,44 @@
 	/* Verify that this is a secure SGI */
 	assert(plat_ic_get_interrupt_type(sgi_num) == INTR_TYPE_EL3);
 
-	gicv2_raise_sgi(sgi_num, id);
+	gicv2_raise_sgi(sgi_num, false, id);
 #else
 	assert(false);
 #endif
 }
 
+void plat_ic_raise_ns_sgi(int sgi_num, u_register_t target)
+{
+	int id;
+
+	/* Target must be a valid MPIDR in the system */
+	id = plat_core_pos_by_mpidr(target);
+	assert(id >= 0);
+
+	/* Verify that this is a non-secure SGI */
+	assert(plat_ic_get_interrupt_type(sgi_num) == INTR_TYPE_NS);
+
+	gicv2_raise_sgi(sgi_num, true, id);
+}
+
+void plat_ic_raise_s_el1_sgi(int sgi_num, u_register_t target)
+{
+#if GICV2_G0_FOR_EL3
+	assert(false);
+#else
+	int id;
+
+	/* Target must be a valid MPIDR in the system */
+	id = plat_core_pos_by_mpidr(target);
+	assert(id >= 0);
+
+	/* Verify that this is a secure EL1 SGI */
+	assert(plat_ic_get_interrupt_type(sgi_num) == INTR_TYPE_S_EL1);
+
+	gicv2_raise_sgi(sgi_num, false, id);
+#endif
+}
+
 void plat_ic_set_spi_routing(unsigned int id, unsigned int routing_mode,
 		u_register_t mpidr)
 {
diff --git a/plat/common/plat_gicv3.c b/plat/common/plat_gicv3.c
index 4a8a7ee..e1420bb 100644
--- a/plat/common/plat_gicv3.c
+++ b/plat/common/plat_gicv3.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2015-2018, ARM Limited and Contributors. All rights reserved.
+ * Portions copyright (c) 2021-2022, ProvenRun S.A.S. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +10,7 @@
 
 #include <arch_helpers.h>
 #include <common/bl_common.h>
+#include <common/debug.h>
 #include <bl31/interrupt_mgmt.h>
 #include <drivers/arm/gic_common.h>
 #include <drivers/arm/gicv3.h>
@@ -39,6 +41,8 @@
 #pragma weak plat_ic_set_interrupt_priority
 #pragma weak plat_ic_set_interrupt_type
 #pragma weak plat_ic_raise_el3_sgi
+#pragma weak plat_ic_raise_ns_sgi
+#pragma weak plat_ic_raise_s_el1_sgi
 #pragma weak plat_ic_set_spi_routing
 #pragma weak plat_ic_set_interrupt_pending
 #pragma weak plat_ic_clear_interrupt_pending
@@ -242,7 +246,31 @@
 	assert(plat_ic_get_interrupt_type((unsigned int)sgi_num) ==
 					  INTR_TYPE_EL3);
 
-	gicv3_raise_secure_g0_sgi((unsigned int)sgi_num, target);
+	gicv3_raise_sgi((unsigned int)sgi_num, GICV3_G0, target);
+}
+
+void plat_ic_raise_ns_sgi(int sgi_num, u_register_t target)
+{
+	/* Target must be a valid MPIDR in the system */
+	assert(plat_core_pos_by_mpidr(target) >= 0);
+
+	/* Verify that this is a non-secure SGI */
+	assert(plat_ic_get_interrupt_type((unsigned int)sgi_num) ==
+					  INTR_TYPE_NS);
+
+	gicv3_raise_sgi((unsigned int)sgi_num, GICV3_G1NS, target);
+}
+
+void plat_ic_raise_s_el1_sgi(int sgi_num, u_register_t target)
+{
+	/* Target must be a valid MPIDR in the system */
+	assert(plat_core_pos_by_mpidr(target) >= 0);
+
+	/* Verify that this is a secure EL1 SGI */
+	assert(plat_ic_get_interrupt_type((unsigned int)sgi_num) ==
+					  INTR_TYPE_S_EL1);
+
+	gicv3_raise_sgi((unsigned int)sgi_num, GICV3_G1S, target);
 }
 
 void plat_ic_set_spi_routing(unsigned int id, unsigned int routing_mode,
diff --git a/plat/imx/imx8m/imx8m_caam.c b/plat/imx/imx8m/imx8m_caam.c
index 644572c..a491550 100644
--- a/plat/imx/imx8m/imx8m_caam.c
+++ b/plat/imx/imx8m/imx8m_caam.c
@@ -24,7 +24,7 @@
 
 	/* config CAAM JRaMID set MID to Cortex A */
 	if (mmio_read_32(CAAM_JR0MID) == HAB_JR0_DID) {
-		NOTICE("Do not release JR0 to NS as it can be used by HAB");
+		NOTICE("Do not release JR0 to NS as it can be used by HAB\n");
 	} else {
 		mmio_write_32(CAAM_JR0MID, CAAM_NS_MID);
 	}
diff --git a/plat/imx/imx8m/imx8m_dyn_cfg_helpers.c b/plat/imx/imx8m/imx8m_dyn_cfg_helpers.c
index 8b2fdd6..5d65ef2 100644
--- a/plat/imx/imx8m/imx8m_dyn_cfg_helpers.c
+++ b/plat/imx/imx8m/imx8m_dyn_cfg_helpers.c
@@ -13,6 +13,7 @@
 #endif
 #include <common/fdt_wrappers.h>
 #include <libfdt.h>
+#include <platform_def.h>
 
 #define DTB_PROP_HW_LOG_ADDR	"tpm_event_log_addr"
 #define DTB_PROP_HW_LOG_SIZE	"tpm_event_log_size"
diff --git a/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c b/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c
index ba0db0c..1667baf 100644
--- a/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c
@@ -134,13 +134,13 @@
 
 	imx_csu_init(csu_cfg);
 
-	imx8m_caam_init();
-
 	console_imx_uart_register(IMX_BOOT_UART_BASE, IMX_BOOT_UART_CLK_IN_HZ,
 		IMX_CONSOLE_BAUDRATE, &console);
 	/* This console is only used for boot stage */
 	console_set_scope(&console, CONSOLE_FLAG_BOOT);
 
+	imx8m_caam_init();
+
 	/*
 	 * tell BL3-1 where the non-secure software image is located
 	 * and the entry state information.
diff --git a/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c b/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c
index 5d2a64e..464c87d 100644
--- a/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c
@@ -139,13 +139,13 @@
 	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,
 		IMX_CONSOLE_BAUDRATE, &console);
 	/* This console is only used for boot stage */
 	console_set_scope(&console, CONSOLE_FLAG_BOOT);
 
+	imx8m_caam_init();
+
 	/*
 	 * tell BL3-1 where the non-secure software image is located
 	 * and the entry state information.
diff --git a/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c b/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c
index d443c3d..34631b8 100644
--- a/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c
@@ -135,13 +135,13 @@
 	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,
 		IMX_CONSOLE_BAUDRATE, &console);
 	/* This console is only used for boot stage */
 	console_set_scope(&console, CONSOLE_FLAG_BOOT);
 
+	imx8m_caam_init();
+
 	/*
 	 * tell BL3-1 where the non-secure software image is located
 	 * and the entry state information.
diff --git a/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c b/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c
index e998a16..59c3779 100644
--- a/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c
@@ -132,14 +132,15 @@
 
 	imx_aipstz_init(aipstz);
 
-	imx8m_caam_init();
-
 #if DEBUG_CONSOLE
 	static console_t console;
 
 	console_imx_uart_register(IMX_BOOT_UART_BASE, IMX_BOOT_UART_CLK_IN_HZ,
 		IMX_CONSOLE_BAUDRATE, &console);
 #endif
+
+	imx8m_caam_init();
+
 	/*
 	 * tell BL3-1 where the non-secure software image is located
 	 * and the entry state information.
diff --git a/plat/mediatek/build_helpers/options.mk b/plat/mediatek/build_helpers/options.mk
index eb579e5..7b63a3e 100644
--- a/plat/mediatek/build_helpers/options.mk
+++ b/plat/mediatek/build_helpers/options.mk
@@ -15,3 +15,10 @@
 $(eval $(call add_defined_option,MTK_SOC))
 $(eval $(call add_defined_option,UART_CLOCK))
 $(eval $(call add_defined_option,UART_BAUDRATE))
+$(eval $(call add_defined_option,CONFIG_MTK_MCUSYS))
+$(eval $(call add_defined_option,CONFIG_MTK_PM_SUPPORT))
+$(eval $(call add_defined_option,CONFIG_MTK_CPU_PM_SUPPORT))
+$(eval $(call add_defined_option,CONFIG_MTK_SMP_EN))
+$(eval $(call add_defined_option,CONFIG_MTK_CPU_SUSPEND_EN))
+$(eval $(call add_defined_option,CONFIG_MTK_PM_ARCH))
+$(eval $(call add_defined_option,CONFIG_MTK_CPU_PM_ARCH))
diff --git a/plat/mediatek/common/cold_boot.c b/plat/mediatek/common/cold_boot.c
deleted file mode 100644
index ff585fe..0000000
--- a/plat/mediatek/common/cold_boot.c
+++ /dev/null
@@ -1,336 +0,0 @@
-/*
- * Copyright (c) 2022, Mediatek Inc. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <assert.h>
-#include <errno.h>
-#include <inttypes.h>
-#include <stdint.h>
-#include <common/debug.h>
-#include <common/runtime_svc.h>
-#include <lib/el3_runtime/context_mgmt.h>
-
-/* Vendors headers */
-#include <cold_boot.h>
-#include <lib/mtk_init/mtk_init.h>
-#include <mtk_sip_svc.h>
-
-static struct kernel_info k_info;
-static entry_point_info_t bl32_image_ep_info;
-static entry_point_info_t bl33_image_ep_info;
-static bool el1_is_2nd_bootloader = true;
-static struct atf_arg_t atfarg;
-
-static int init_mtk_bl32_arg(void)
-{
-	struct mtk_bl_param_t *p_mtk_bl_param;
-	struct atf_arg_t *p_atfarg;
-
-	p_mtk_bl_param = (struct mtk_bl_param_t *) get_mtk_bl31_fw_config(BOOT_ARG_FROM_BL2);
-	if (p_mtk_bl_param == NULL) {
-		ERROR("p_mtk_bl_param is NULL!\n");
-		return -1;
-	}
-	p_atfarg = (struct atf_arg_t *)p_mtk_bl_param->atf_arg_addr;
-	if (p_atfarg == NULL) {
-		ERROR("bl32 argument is NULL!\n");
-		return -1;
-	}
-	memcpy((void *)&atfarg, (void *)p_atfarg, sizeof(struct atf_arg_t));
-	return 0;
-}
-MTK_EARLY_PLAT_INIT(init_mtk_bl32_arg);
-
-static void save_kernel_info(uint64_t pc, uint64_t r0, uint64_t r1, uint64_t k32_64)
-{
-	k_info.k32_64 = k32_64;
-	k_info.pc = pc;
-
-	if (k32_64 == LINUX_KERNEL_32) {
-		/* for 32 bits kernel */
-		k_info.r0 = 0;
-		/* machtype */
-		k_info.r1 = r0;
-		/* tags */
-		k_info.r2 = r1;
-	} else {
-		/* for 64 bits kernel */
-		k_info.r0 = r0;
-		k_info.r1 = r1;
-	}
-}
-
-static uint32_t plat_get_spsr_for_bl32_64_entry(void)
-{
-	return SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
-}
-
-#if MTK_BL33_IS_64BIT
-static uint32_t plat_get_spsr_for_bl33_entry(void)
-{
-	uint32_t spsr;
-	uint32_t mode;
-
-	mode = MODE_EL1;
-	spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
-	return spsr;
-}
-#else
-static uint32_t plat_get_spsr_for_bl33_entry(void)
-{
-	unsigned int mode;
-	uint32_t spsr;
-	unsigned int ee;
-	unsigned long daif;
-
-	INFO("Secondary bootloader is AArch32\n");
-	mode = MODE32_svc;
-	ee = 0;
-	/*
-	 * TODO: Choose async. exception bits if HYP mode is not
-	 * implemented according to the values of SCR.{AW, FW} bits
-	 */
-	daif = DAIF_ABT_BIT | DAIF_IRQ_BIT | DAIF_FIQ_BIT;
-
-	spsr = SPSR_MODE32(mode, 0, ee, daif);
-	return spsr;
-}
-#endif
-
-static void populate_bl32_image_ep(entry_point_info_t *bl32_ep_instance,
-		struct mtk_bl_param_t *p_mtk_bl_param)
-{
-	entry_point_info_t *populated_ep_bl32 = bl32_ep_instance;
-
-	if (p_mtk_bl_param == NULL) {
-		ERROR("p_mtk_bl_param is NULL!\n");
-		panic();
-	}
-	SET_SECURITY_STATE(bl32_ep_instance->h.attr, SECURE);
-	SET_PARAM_HEAD(populated_ep_bl32,
-		       PARAM_EP,
-		       VERSION_1,
-		       populated_ep_bl32->h.attr);
-	populated_ep_bl32->pc = atfarg.tee_entry;
-	populated_ep_bl32->spsr = plat_get_spsr_for_bl32_64_entry();
-}
-
-static void populate_bl33_image_ep(entry_point_info_t *bl33_ep_instance,
-		struct mtk_bl_param_t *p_mtk_bl_param)
-{
-	entry_point_info_t *populated_ep_bl33 = bl33_ep_instance;
-
-	if (p_mtk_bl_param == NULL) {
-		ERROR("p_mtk_bl_param is NULL!\n");
-		panic();
-	}
-	SET_SECURITY_STATE(bl33_ep_instance->h.attr, NON_SECURE);
-	SET_PARAM_HEAD(populated_ep_bl33,
-		       PARAM_EP,
-		       VERSION_1,
-		       populated_ep_bl33->h.attr);
-	populated_ep_bl33->pc = p_mtk_bl_param->bl33_start_addr;
-	/* standardize 2nd bootloader input argument */
-	populated_ep_bl33->args.arg0 = p_mtk_bl_param->bootarg_loc;
-	/* compatible to old GZ version */
-	populated_ep_bl33->args.arg4 = p_mtk_bl_param->bootarg_loc;
-	populated_ep_bl33->args.arg5 = p_mtk_bl_param->bootarg_size;
-	populated_ep_bl33->spsr = plat_get_spsr_for_bl33_entry();
-}
-
-static int populate_bl_images_ep(struct mtk_bl_param_t *p_mtk_bl_param)
-{
-	/*
-	 * Tell BL31 where the non-trusted software image
-	 * is located and the entry state information
-	 */
-	populate_bl33_image_ep(&bl33_image_ep_info, p_mtk_bl_param);
-	populate_bl32_image_ep(&bl32_image_ep_info, p_mtk_bl_param);
-	return 0;
-}
-
-static int populate_bl_images_ep_init(void)
-{
-	return populate_bl_images_ep(get_mtk_bl31_fw_config(BOOT_ARG_FROM_BL2));
-}
-MTK_PLAT_SETUP_0_INIT(populate_bl_images_ep_init);
-
-static entry_point_info_t *bl31_plat_get_next_kernel64_ep_info(void)
-{
-	entry_point_info_t *next_image_info;
-	unsigned long el_status;
-	unsigned int mode;
-
-	el_status = 0;
-	mode = 0;
-
-	/* Kernel image is always non-secured */
-	next_image_info = &bl33_image_ep_info;
-
-	/* Figure out what mode we enter the non-secure world in */
-	el_status = read_id_aa64pfr0_el1() >> ID_AA64PFR0_EL2_SHIFT;
-	el_status &= ID_AA64PFR0_ELX_MASK;
-
-	INFO("Kernel_EL %d\n", el_status?2:1);
-	if (el_status) {
-		mode = MODE_EL2;
-	} else {
-		mode = MODE_EL1;
-	}
-	INFO("Kernel is 64Bit\n");
-	next_image_info->spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
-	next_image_info->pc = k_info.pc;
-	next_image_info->args.arg0 = k_info.r0;
-	next_image_info->args.arg1 = k_info.r1;
-
-	INFO("pc=0x%lx, r0=0x%" PRIx64 ", r1=0x%" PRIx64 "\n",
-	     next_image_info->pc,
-	     next_image_info->args.arg0,
-	     next_image_info->args.arg1);
-
-	SET_SECURITY_STATE(next_image_info->h.attr, NON_SECURE);
-
-	/* None of the images on this platform can have 0x0 as the entrypoint */
-	if (next_image_info->pc) {
-		return next_image_info;
-	}
-
-	return NULL;
-}
-
-static entry_point_info_t *bl31_plat_get_next_kernel32_ep_info(void)
-{
-	entry_point_info_t *next_image_info;
-	unsigned int mode;
-
-	mode = 0;
-
-	/* Kernel image is always non-secured */
-	next_image_info = &bl33_image_ep_info;
-
-	/* Figure out what mode we enter the non-secure world in */
-	mode = MODE32_hyp;
-	/*
-	 * TODO: Consider the possibility of specifying the SPSR in
-	 * the FIP ToC and allowing the platform to have a say as
-	 * well.
-	 */
-
-	INFO("Kernel is 32Bit\n");
-	next_image_info->spsr = SPSR_MODE32(mode, SPSR_T_ARM, SPSR_E_LITTLE,
-					    (DAIF_FIQ_BIT | DAIF_IRQ_BIT | DAIF_ABT_BIT));
-	next_image_info->pc = k_info.pc;
-	next_image_info->args.arg0 = k_info.r0;
-	next_image_info->args.arg1 = k_info.r1;
-	next_image_info->args.arg2 = k_info.r2;
-
-	INFO("pc=0x%lx, r0=0x%" PRIx64 ", r1=0x%" PRIx64 ", r2=0x%" PRIx64 "\n",
-	     next_image_info->pc,
-	     next_image_info->args.arg0,
-	     next_image_info->args.arg1,
-	     next_image_info->args.arg2);
-
-	SET_SECURITY_STATE(next_image_info->h.attr, NON_SECURE);
-
-	/* None of the images on this platform can have 0x0 as the entrypoint */
-	if (next_image_info->pc) {
-		return next_image_info;
-	}
-
-	return NULL;
-}
-
-static void bl31_prepare_kernel_entry(uint64_t k32_64)
-{
-	entry_point_info_t *next_image_info = NULL;
-	uint32_t image_type;
-
-	/* Determine which image to execute next */
-	image_type = NON_SECURE; /* bl31_get_next_image_type(); */
-
-	/* Leave 2nd bootloader then jump to kernel */
-	el1_is_2nd_bootloader = false;
-
-	/* Program EL3 registers to enable entry into the next EL */
-	if (k32_64 == LINUX_KERNEL_32) {
-		next_image_info = bl31_plat_get_next_kernel32_ep_info();
-	} else {
-		next_image_info = bl31_plat_get_next_kernel64_ep_info();
-	}
-
-	assert(next_image_info);
-	assert(image_type == GET_SECURITY_STATE(next_image_info->h.attr));
-
-	INFO("BL31: Preparing for EL3 exit to %s world, Kernel\n",
-	     (image_type == SECURE) ? "secure" : "normal");
-	INFO("BL31: Next image address = 0x%" PRIx64 "\n",
-	     next_image_info->pc);
-	INFO("BL31: Next image spsr = 0x%x\n", next_image_info->spsr);
-	cm_init_my_context(next_image_info);
-	cm_prepare_el3_exit(image_type);
-}
-
-bool is_el1_2nd_bootloader(void)
-{
-	return el1_is_2nd_bootloader;
-}
-
-/*******************************************************************************
- * Return a pointer to the 'entry_point_info' structure of the next image for
- * the security state specified. BL33 corresponds to the non-secure image type
- * while BL32 corresponds to the secure image type. A NULL pointer is returned
- * if the image does not exist.
- ******************************************************************************/
-entry_point_info_t *bl31_plat_get_next_image_ep_info(uint32_t type)
-{
-	entry_point_info_t *next_image_info;
-
-	next_image_info = (type == NON_SECURE) ? &bl33_image_ep_info : &bl32_image_ep_info;
-
-	/* None of the images on this platform can have 0x0 as the entrypoint */
-	if (next_image_info->pc) {
-		return next_image_info;
-	}
-	return NULL;
-}
-
-u_register_t boot_to_kernel(u_register_t x1,
-			    u_register_t x2,
-			    u_register_t x3,
-			    u_register_t x4,
-			    void *handle,
-			    struct smccc_res *smccc_ret)
-{
-	static uint8_t kernel_boot_once_flag;
-
-	/* only support in booting flow */
-	if (kernel_boot_once_flag == 0) {
-		kernel_boot_once_flag = 1;
-
-		INFO("save kernel info\n");
-		save_kernel_info(x1, x2, x3, x4);
-		bl31_prepare_kernel_entry(x4);
-		INFO("el3_exit\n");
-		/*
-		 * FIXME: no better way so far to prevent from
-		 * SiP root handler wipe x0~x3 if not assign smccc_ret
-		 * return register
-		 */
-		smccc_ret->a1 = x3;
-
-		mtk_init_one_level(MTK_INIT_LVL_BL33_DEFER);
-
-#if MTK_CONSOLE_RUNTIME_DISABLE
-		INFO("Turn off BL31 console\n");
-		mtk_console_core_end();
-#endif
-
-		/* Re-assign as x0 register entering Linux kernel */
-		return x2;
-	}
-	return 0;
-}
-/* Register SiP SMC service */
-DECLARE_SMC_HANDLER(MTK_SIP_KERNEL_BOOT, boot_to_kernel);
diff --git a/plat/mediatek/common/mtk_smc_handlers.c b/plat/mediatek/common/mtk_smc_handlers.c
index 24b978c..51a960f 100644
--- a/plat/mediatek/common/mtk_smc_handlers.c
+++ b/plat/mediatek/common/mtk_smc_handlers.c
@@ -6,9 +6,11 @@
 
 #include <assert.h>
 #include <errno.h>
+#if MTK_SIP_KERNEL_BOOT_ENABLE
+#include <cold_boot.h>
+#endif
 #include <common/debug.h>
 #include <common/runtime_svc.h>
-#include <cold_boot.h>
 #include <lib/mtk_init/mtk_init.h>
 #include <mtk_sip_svc.h>
 
diff --git a/plat/mediatek/drivers/dcm/mt8188/mtk_dcm_utils.c b/plat/mediatek/drivers/dcm/mt8188/mtk_dcm_utils.c
new file mode 100644
index 0000000..c054de9
--- /dev/null
+++ b/plat/mediatek/drivers/dcm/mt8188/mtk_dcm_utils.c
@@ -0,0 +1,402 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <mtk_dcm_utils.h>
+
+#define MP_CPUSYS_TOP_ADB_DCM_REG0_MASK		BIT(17)
+#define MP_CPUSYS_TOP_ADB_DCM_REG1_MASK		(BIT(15) | BIT(16) | BIT(17) | \
+						 BIT(18) | BIT(21))
+#define MP_CPUSYS_TOP_ADB_DCM_REG2_MASK		(BIT(15) | BIT(16) | BIT(17) | BIT(18))
+#define MP_CPUSYS_TOP_ADB_DCM_REG0_ON		BIT(17)
+#define MP_CPUSYS_TOP_ADB_DCM_REG1_ON		(BIT(15) | BIT(16) | BIT(17) | \
+						 BIT(18) | BIT(21))
+#define MP_CPUSYS_TOP_ADB_DCM_REG2_ON		(BIT(15) | BIT(16) | BIT(17) | BIT(18))
+#define MP_CPUSYS_TOP_ADB_DCM_REG0_OFF		(0x0 << 17)
+#define MP_CPUSYS_TOP_ADB_DCM_REG1_OFF		((0x0 << 15) | (0x0 << 16) | \
+						 (0x0 << 17) | (0x0 << 18) | \
+						 (0x0 << 21))
+#define MP_CPUSYS_TOP_ADB_DCM_REG2_OFF		((0x0 << 15) | (0x0 << 16) | \
+						 (0x0 << 17) | (0x0 << 18))
+
+bool dcm_mp_cpusys_top_adb_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= dcm_check_state(MP_CPUSYS_TOP_MP_ADB_DCM_CFG0,
+			       MP_CPUSYS_TOP_ADB_DCM_REG0_MASK,
+			       MP_CPUSYS_TOP_ADB_DCM_REG0_ON);
+	ret &= dcm_check_state(MP_CPUSYS_TOP_MP_ADB_DCM_CFG4,
+			       MP_CPUSYS_TOP_ADB_DCM_REG1_MASK,
+			       MP_CPUSYS_TOP_ADB_DCM_REG1_ON);
+	ret &= dcm_check_state(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+			       MP_CPUSYS_TOP_ADB_DCM_REG2_MASK,
+			       MP_CPUSYS_TOP_ADB_DCM_REG2_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_adb_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_adb_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_ADB_DCM_CFG0,
+				   MP_CPUSYS_TOP_ADB_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_ADB_DCM_REG0_ON);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_ADB_DCM_CFG4,
+				   MP_CPUSYS_TOP_ADB_DCM_REG1_MASK,
+				   MP_CPUSYS_TOP_ADB_DCM_REG1_ON);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+				   MP_CPUSYS_TOP_ADB_DCM_REG2_MASK,
+				   MP_CPUSYS_TOP_ADB_DCM_REG2_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_adb_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_ADB_DCM_CFG0,
+				   MP_CPUSYS_TOP_ADB_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_ADB_DCM_REG0_OFF);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_ADB_DCM_CFG4,
+				   MP_CPUSYS_TOP_ADB_DCM_REG1_MASK,
+				   MP_CPUSYS_TOP_ADB_DCM_REG1_OFF);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+				   MP_CPUSYS_TOP_ADB_DCM_REG2_MASK,
+				   MP_CPUSYS_TOP_ADB_DCM_REG2_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_APB_DCM_REG0_MASK	BIT(5)
+#define MP_CPUSYS_TOP_APB_DCM_REG1_MASK	BIT(8)
+#define MP_CPUSYS_TOP_APB_DCM_REG2_MASK	BIT(16)
+#define MP_CPUSYS_TOP_APB_DCM_REG0_ON	BIT(5)
+#define MP_CPUSYS_TOP_APB_DCM_REG1_ON	BIT(8)
+#define MP_CPUSYS_TOP_APB_DCM_REG2_ON	BIT(16)
+#define MP_CPUSYS_TOP_APB_DCM_REG0_OFF	(0x0 << 5)
+#define MP_CPUSYS_TOP_APB_DCM_REG1_OFF	(0x0 << 8)
+#define MP_CPUSYS_TOP_APB_DCM_REG2_OFF	(0x0 << 16)
+
+bool dcm_mp_cpusys_top_apb_dcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= dcm_check_state(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+			       MP_CPUSYS_TOP_APB_DCM_REG0_MASK,
+			       MP_CPUSYS_TOP_APB_DCM_REG0_ON);
+	ret &= dcm_check_state(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+			       MP_CPUSYS_TOP_APB_DCM_REG1_MASK,
+			       MP_CPUSYS_TOP_APB_DCM_REG1_ON);
+	ret &= dcm_check_state(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+			       MP_CPUSYS_TOP_APB_DCM_REG2_MASK,
+			       MP_CPUSYS_TOP_APB_DCM_REG2_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_apb_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_apb_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+				   MP_CPUSYS_TOP_APB_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_APB_DCM_REG0_ON);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+				   MP_CPUSYS_TOP_APB_DCM_REG1_MASK,
+				   MP_CPUSYS_TOP_APB_DCM_REG1_ON);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+				   MP_CPUSYS_TOP_APB_DCM_REG2_MASK,
+				   MP_CPUSYS_TOP_APB_DCM_REG2_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_apb_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+				   MP_CPUSYS_TOP_APB_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_APB_DCM_REG0_OFF);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCUSYS_DCM_CFG0,
+				   MP_CPUSYS_TOP_APB_DCM_REG1_MASK,
+				   MP_CPUSYS_TOP_APB_DCM_REG1_OFF);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+				   MP_CPUSYS_TOP_APB_DCM_REG2_MASK,
+				   MP_CPUSYS_TOP_APB_DCM_REG2_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_MASK (BIT(11) | BIT(24) | BIT(25))
+#define MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_ON (BIT(11) | BIT(24) | BIT(25))
+#define MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_OFF ((0x0 << 11) | \
+						(0x0 << 24) | \
+						(0x0 << 25))
+
+bool dcm_mp_cpusys_top_bus_pll_div_dcm_is_on(void)
+{
+	return dcm_check_state(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+			       MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_MASK,
+			       MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_ON);
+}
+
+void dcm_mp_cpusys_top_bus_pll_div_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_bus_pll_div_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+				   MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_bus_pll_div_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+				   MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_BUS_PLL_DIV_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_MASK	BIT(0)
+#define MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_ON	BIT(0)
+#define MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_OFF	(0x0 << 0)
+
+bool dcm_mp_cpusys_top_core_stall_dcm_is_on(void)
+{
+	return dcm_check_state(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+			       MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_MASK,
+			       MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_ON);
+}
+
+void dcm_mp_cpusys_top_core_stall_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_core_stall_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+				   MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_core_stall_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+				   MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_CORE_STALL_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CPUBIU_DCM_REG0_MASK	(0xffff << 0)
+#define MP_CPUSYS_TOP_CPUBIU_DCM_REG0_ON	(0xffff << 0)
+#define MP_CPUSYS_TOP_CPUBIU_DCM_REG0_OFF	(0x0 << 0)
+
+bool dcm_mp_cpusys_top_cpubiu_dcm_is_on(void)
+{
+	return dcm_check_state(MP_CPUSYS_TOP_MCSIC_DCM0,
+			       MP_CPUSYS_TOP_CPUBIU_DCM_REG0_MASK,
+			       MP_CPUSYS_TOP_CPUBIU_DCM_REG0_ON);
+}
+
+void dcm_mp_cpusys_top_cpubiu_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_cpubiu_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCSIC_DCM0,
+				   MP_CPUSYS_TOP_CPUBIU_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_CPUBIU_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_cpubiu_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MCSIC_DCM0,
+				   MP_CPUSYS_TOP_CPUBIU_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_CPUBIU_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_MASK (BIT(24) | BIT(25))
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_ON (BIT(24) | BIT(25))
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_OFF ((0x0 << 24) | (0x0 << 25))
+
+bool dcm_mp_cpusys_top_cpu_pll_div_0_dcm_is_on(void)
+{
+	return dcm_check_state(MP_CPUSYS_TOP_CPU_PLLDIV_CFG0,
+			       MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_MASK,
+			       MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_ON);
+}
+
+void dcm_mp_cpusys_top_cpu_pll_div_0_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_cpu_pll_div_0_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG0,
+				   MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_cpu_pll_div_0_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG0,
+				   MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_CPU_PLL_DIV_0_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_MASK (BIT(24) | BIT(25))
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_ON (BIT(24) | BIT(25))
+#define MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_OFF ((0x0 << 24) | (0x0 << 25))
+
+bool dcm_mp_cpusys_top_cpu_pll_div_1_dcm_is_on(void)
+{
+	return dcm_check_state(MP_CPUSYS_TOP_CPU_PLLDIV_CFG1,
+			       MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_MASK,
+			       MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_ON);
+}
+
+void dcm_mp_cpusys_top_cpu_pll_div_1_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_cpu_pll_div_1_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG1,
+				   MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_cpu_pll_div_1_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_CPU_PLLDIV_CFG1,
+				   MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_CPU_PLL_DIV_1_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_MASK	BIT(4)
+#define MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_ON	BIT(4)
+#define MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_OFF	(0x0 << 4)
+
+bool dcm_mp_cpusys_top_fcm_stall_dcm_is_on(void)
+{
+	return dcm_check_state(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+			       MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_MASK,
+			       MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_ON);
+}
+
+void dcm_mp_cpusys_top_fcm_stall_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_fcm_stall_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+				   MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_fcm_stall_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG7,
+				   MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_FCM_STALL_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_MASK	BIT(31)
+#define MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_ON		BIT(31)
+#define MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_OFF	(0x0U << 31)
+
+bool dcm_mp_cpusys_top_last_cor_idle_dcm_is_on(void)
+{
+	return dcm_check_state(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+			       MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_MASK,
+			       MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_ON);
+}
+
+void dcm_mp_cpusys_top_last_cor_idle_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_last_cor_idle_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+				   MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_last_cor_idle_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_BUS_PLLDIV_CFG,
+				   MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_LAST_COR_IDLE_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_MISC_DCM_REG0_MASK (BIT(1) | BIT(4))
+#define MP_CPUSYS_TOP_MISC_DCM_REG0_ON (BIT(1) | BIT(4))
+#define MP_CPUSYS_TOP_MISC_DCM_REG0_OFF ((0x0 << 1) | (0x0 << 4))
+
+bool dcm_mp_cpusys_top_misc_dcm_is_on(void)
+{
+	return dcm_check_state(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+			       MP_CPUSYS_TOP_MISC_DCM_REG0_MASK,
+			       MP_CPUSYS_TOP_MISC_DCM_REG0_ON);
+}
+
+void dcm_mp_cpusys_top_misc_dcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_misc_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+				   MP_CPUSYS_TOP_MISC_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_MISC_DCM_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_misc_dcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+				   MP_CPUSYS_TOP_MISC_DCM_REG0_MASK,
+				   MP_CPUSYS_TOP_MISC_DCM_REG0_OFF);
+	}
+}
+
+#define MP_CPUSYS_TOP_MP0_QDCM_REG0_MASK BIT(3)
+#define MP_CPUSYS_TOP_MP0_QDCM_REG1_MASK (BIT(0) | BIT(1) | BIT(2) | BIT(3))
+#define MP_CPUSYS_TOP_MP0_QDCM_REG0_ON BIT(3)
+#define MP_CPUSYS_TOP_MP0_QDCM_REG1_ON (BIT(0) | BIT(1) | BIT(2) | BIT(3))
+#define MP_CPUSYS_TOP_MP0_QDCM_REG0_OFF ((0x0 << 3))
+#define MP_CPUSYS_TOP_MP0_QDCM_REG1_OFF ((0x0 << 0) | (0x0 << 1) | \
+					 (0x0 << 2) | (0x0 << 3))
+
+bool dcm_mp_cpusys_top_mp0_qdcm_is_on(void)
+{
+	bool ret = true;
+
+	ret &= dcm_check_state(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+			       MP_CPUSYS_TOP_MP0_QDCM_REG0_MASK,
+			       MP_CPUSYS_TOP_MP0_QDCM_REG0_ON);
+	ret &= dcm_check_state(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+			       MP_CPUSYS_TOP_MP0_QDCM_REG1_MASK,
+			       MP_CPUSYS_TOP_MP0_QDCM_REG1_ON);
+
+	return ret;
+}
+
+void dcm_mp_cpusys_top_mp0_qdcm(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'mp_cpusys_top_mp0_qdcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+				   MP_CPUSYS_TOP_MP0_QDCM_REG0_MASK,
+				   MP_CPUSYS_TOP_MP0_QDCM_REG0_ON);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+				   MP_CPUSYS_TOP_MP0_QDCM_REG1_MASK,
+				   MP_CPUSYS_TOP_MP0_QDCM_REG1_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'mp_cpusys_top_mp0_qdcm'" */
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP_MISC_DCM_CFG0,
+				   MP_CPUSYS_TOP_MP0_QDCM_REG0_MASK,
+				   MP_CPUSYS_TOP_MP0_QDCM_REG0_OFF);
+		mmio_clrsetbits_32(MP_CPUSYS_TOP_MP0_DCM_CFG0,
+				   MP_CPUSYS_TOP_MP0_QDCM_REG1_MASK,
+				   MP_CPUSYS_TOP_MP0_QDCM_REG1_OFF);
+	}
+}
+
+#define CPCCFG_REG_EMI_WFIFO_REG0_MASK (BIT(0) | BIT(1) | BIT(2) | BIT(3))
+#define CPCCFG_REG_EMI_WFIFO_REG0_ON (BIT(0) | BIT(1) | BIT(2) | BIT(3))
+#define CPCCFG_REG_EMI_WFIFO_REG0_OFF ((0x0 << 0) | (0x0 << 1) | \
+				       (0x0 << 2) | (0x0 << 3))
+
+bool dcm_cpccfg_reg_emi_wfifo_is_on(void)
+{
+	return dcm_check_state(CPCCFG_REG_EMI_WFIFO,
+			       CPCCFG_REG_EMI_WFIFO_REG0_MASK,
+			       CPCCFG_REG_EMI_WFIFO_REG0_ON);
+}
+
+void dcm_cpccfg_reg_emi_wfifo(bool on)
+{
+	if (on) {
+		/* TINFO = "Turn ON DCM 'cpccfg_reg_emi_wfifo'" */
+		mmio_clrsetbits_32(CPCCFG_REG_EMI_WFIFO,
+				   CPCCFG_REG_EMI_WFIFO_REG0_MASK,
+				   CPCCFG_REG_EMI_WFIFO_REG0_ON);
+	} else {
+		/* TINFO = "Turn OFF DCM 'cpccfg_reg_emi_wfifo'" */
+		mmio_clrsetbits_32(CPCCFG_REG_EMI_WFIFO,
+				   CPCCFG_REG_EMI_WFIFO_REG0_MASK,
+				   CPCCFG_REG_EMI_WFIFO_REG0_OFF);
+	}
+}
diff --git a/plat/mediatek/drivers/dcm/mt8188/mtk_dcm_utils.h b/plat/mediatek/drivers/dcm/mt8188/mtk_dcm_utils.h
new file mode 100644
index 0000000..5d758dd
--- /dev/null
+++ b/plat/mediatek/drivers/dcm/mt8188/mtk_dcm_utils.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTK_DCM_UTILS_H
+#define MTK_DCM_UTILS_H
+
+#include <stdbool.h>
+
+#include <mtk_dcm.h>
+#include <platform_def.h>
+
+/* Base */
+#define MP_CPUSYS_TOP_BASE	(MCUCFG_BASE + 0x8000)
+#define CPCCFG_REG_BASE		(MCUCFG_BASE + 0xA800)
+
+/* Register Definition */
+#define CPCCFG_REG_EMI_WFIFO		(CPCCFG_REG_BASE + 0x100)
+#define MP_CPUSYS_TOP_CPU_PLLDIV_CFG0	(MP_CPUSYS_TOP_BASE + 0x22a0)
+#define MP_CPUSYS_TOP_CPU_PLLDIV_CFG1	(MP_CPUSYS_TOP_BASE + 0x22a4)
+#define MP_CPUSYS_TOP_BUS_PLLDIV_CFG	(MP_CPUSYS_TOP_BASE + 0x22e0)
+#define MP_CPUSYS_TOP_MCSIC_DCM0	(MP_CPUSYS_TOP_BASE + 0x2440)
+#define MP_CPUSYS_TOP_MP_ADB_DCM_CFG0	(MP_CPUSYS_TOP_BASE + 0x2500)
+#define MP_CPUSYS_TOP_MP_ADB_DCM_CFG4	(MP_CPUSYS_TOP_BASE + 0x2510)
+#define MP_CPUSYS_TOP_MP_MISC_DCM_CFG0	(MP_CPUSYS_TOP_BASE + 0x2518)
+#define MP_CPUSYS_TOP_MCUSYS_DCM_CFG0	(MP_CPUSYS_TOP_BASE + 0x25c0)
+#define MP_CPUSYS_TOP_MP0_DCM_CFG0	(MP_CPUSYS_TOP_BASE + 0x4880)
+#define MP_CPUSYS_TOP_MP0_DCM_CFG7	(MP_CPUSYS_TOP_BASE + 0x489c)
+
+/* MP_CPUSYS_TOP */
+bool dcm_mp_cpusys_top_adb_dcm_is_on(void);
+void dcm_mp_cpusys_top_adb_dcm(bool on);
+bool dcm_mp_cpusys_top_apb_dcm_is_on(void);
+void dcm_mp_cpusys_top_apb_dcm(bool on);
+bool dcm_mp_cpusys_top_bus_pll_div_dcm_is_on(void);
+void dcm_mp_cpusys_top_bus_pll_div_dcm(bool on);
+bool dcm_mp_cpusys_top_core_stall_dcm_is_on(void);
+void dcm_mp_cpusys_top_core_stall_dcm(bool on);
+bool dcm_mp_cpusys_top_cpubiu_dcm_is_on(void);
+void dcm_mp_cpusys_top_cpubiu_dcm(bool on);
+bool dcm_mp_cpusys_top_cpu_pll_div_0_dcm_is_on(void);
+void dcm_mp_cpusys_top_cpu_pll_div_0_dcm(bool on);
+bool dcm_mp_cpusys_top_cpu_pll_div_1_dcm_is_on(void);
+void dcm_mp_cpusys_top_cpu_pll_div_1_dcm(bool on);
+bool dcm_mp_cpusys_top_fcm_stall_dcm_is_on(void);
+void dcm_mp_cpusys_top_fcm_stall_dcm(bool on);
+bool dcm_mp_cpusys_top_last_cor_idle_dcm_is_on(void);
+void dcm_mp_cpusys_top_last_cor_idle_dcm(bool on);
+bool dcm_mp_cpusys_top_misc_dcm_is_on(void);
+void dcm_mp_cpusys_top_misc_dcm(bool on);
+bool dcm_mp_cpusys_top_mp0_qdcm_is_on(void);
+void dcm_mp_cpusys_top_mp0_qdcm(bool on);
+/* CPCCFG_REG */
+bool dcm_cpccfg_reg_emi_wfifo_is_on(void);
+void dcm_cpccfg_reg_emi_wfifo(bool on);
+
+#endif
diff --git a/plat/mediatek/drivers/dcm/mtk_dcm.c b/plat/mediatek/drivers/dcm/mtk_dcm.c
new file mode 100644
index 0000000..ca79a20
--- /dev/null
+++ b/plat/mediatek/drivers/dcm/mtk_dcm.c
@@ -0,0 +1,84 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <lib/mtk_init/mtk_init.h>
+#include <mtk_dcm.h>
+#include <mtk_dcm_utils.h>
+
+static void dcm_armcore(bool mode)
+{
+	dcm_mp_cpusys_top_bus_pll_div_dcm(mode);
+	dcm_mp_cpusys_top_cpu_pll_div_0_dcm(mode);
+	dcm_mp_cpusys_top_cpu_pll_div_1_dcm(mode);
+}
+
+static void dcm_mcusys(bool on)
+{
+	dcm_mp_cpusys_top_adb_dcm(on);
+	dcm_mp_cpusys_top_apb_dcm(on);
+	dcm_mp_cpusys_top_cpubiu_dcm(on);
+	dcm_mp_cpusys_top_misc_dcm(on);
+	dcm_mp_cpusys_top_mp0_qdcm(on);
+
+	/* CPCCFG_REG */
+	dcm_cpccfg_reg_emi_wfifo(on);
+	dcm_mp_cpusys_top_last_cor_idle_dcm(on);
+}
+
+static void dcm_stall(bool on)
+{
+	dcm_mp_cpusys_top_core_stall_dcm(on);
+	dcm_mp_cpusys_top_fcm_stall_dcm(on);
+}
+
+static bool check_dcm_state(void)
+{
+	bool ret = true;
+
+	ret &= dcm_mp_cpusys_top_bus_pll_div_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_cpu_pll_div_0_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_cpu_pll_div_1_dcm_is_on();
+
+	ret &= dcm_mp_cpusys_top_adb_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_apb_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_cpubiu_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_misc_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_mp0_qdcm_is_on();
+	ret &= dcm_cpccfg_reg_emi_wfifo_is_on();
+	ret &= dcm_mp_cpusys_top_last_cor_idle_dcm_is_on();
+
+	ret &= dcm_mp_cpusys_top_core_stall_dcm_is_on();
+	ret &= dcm_mp_cpusys_top_fcm_stall_dcm_is_on();
+
+	return ret;
+}
+
+bool dcm_check_state(uintptr_t addr, unsigned int mask, unsigned int compare)
+{
+	return ((mmio_read_32(addr) & mask) == compare);
+}
+
+int dcm_set_init(void)
+{
+	int ret;
+
+	dcm_armcore(true);
+	dcm_mcusys(true);
+	dcm_stall(true);
+
+	if (check_dcm_state() == false) {
+		ERROR("Failed to set default dcm on!!\n");
+		ret = -1;
+	} else {
+		INFO("%s, dcm pass\n", __func__);
+		ret = 0;
+	}
+
+	return ret;
+}
+MTK_PLAT_SETUP_0_INIT(dcm_set_init);
diff --git a/plat/mediatek/drivers/dcm/mtk_dcm.h b/plat/mediatek/drivers/dcm/mtk_dcm.h
new file mode 100644
index 0000000..05f8d45
--- /dev/null
+++ b/plat/mediatek/drivers/dcm/mtk_dcm.h
@@ -0,0 +1,15 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTK_DCM_H
+#define MTK_DCM_H
+
+#include <stdbool.h>
+
+bool dcm_check_state(uintptr_t addr, unsigned int mask, unsigned int compare);
+int dcm_set_init(void);
+
+#endif /* #ifndef MTK_DCM_H */
diff --git a/plat/mediatek/drivers/dcm/rules.mk b/plat/mediatek/drivers/dcm/rules.mk
new file mode 100644
index 0000000..a8ee05e
--- /dev/null
+++ b/plat/mediatek/drivers/dcm/rules.mk
@@ -0,0 +1,17 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := mtk_dcm
+
+LOCAL_SRCS-y := ${LOCAL_DIR}/mtk_dcm.c
+LOCAL_SRCS-y += ${LOCAL_DIR}/${MTK_SOC}/mtk_dcm_utils.c
+
+PLAT_INCLUDES += -I${LOCAL_DIR}
+PLAT_INCLUDES += -I${LOCAL_DIR}/${MTK_SOC}
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/drivers/dfd/dfd.c b/plat/mediatek/drivers/dfd/dfd.c
new file mode 100644
index 0000000..5770d50
--- /dev/null
+++ b/plat/mediatek/drivers/dfd/dfd.c
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <dfd.h>
+#include <mtk_sip_svc.h>
+#include <plat_dfd.h>
+
+static u_register_t dfd_smc_dispatcher(u_register_t arg0, u_register_t arg1,
+				       u_register_t arg2, u_register_t arg3,
+				       void *handle, struct smccc_res *smccc_ret)
+{
+	int ret = MTK_SIP_E_SUCCESS;
+
+	switch (arg0) {
+	case PLAT_MTK_DFD_SETUP_MAGIC:
+		INFO("[%s] DFD setup call from kernel\n", __func__);
+		dfd_setup(arg1, arg2, arg3);
+		break;
+	case PLAT_MTK_DFD_READ_MAGIC:
+		/* only allow to access DFD register base + 0x200 */
+		if (arg1 <= 0x200) {
+			ret = mmio_read_32(MISC1_CFG_BASE + arg1);
+		}
+		break;
+	case PLAT_MTK_DFD_WRITE_MAGIC:
+		/* only allow to access DFD register base + 0x200 */
+		if (arg1 <= 0x200) {
+			sync_writel(MISC1_CFG_BASE + arg1, arg2);
+		}
+		break;
+	default:
+		ret = MTK_SIP_E_INVALID_PARAM;
+		break;
+	}
+
+	return ret;
+}
+DECLARE_SMC_HANDLER(MTK_SIP_KERNEL_DFD, dfd_smc_dispatcher);
diff --git a/plat/mediatek/drivers/dfd/dfd.h b/plat/mediatek/drivers/dfd/dfd.h
new file mode 100644
index 0000000..c088bd0
--- /dev/null
+++ b/plat/mediatek/drivers/dfd/dfd.h
@@ -0,0 +1,16 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef DFD_H
+#define DFD_H
+
+#include <arch_helpers.h>
+#include <lib/mmio.h>
+
+void dfd_resume(void);
+void dfd_setup(uint64_t base_addr, uint64_t chain_length, uint64_t cache_dump);
+
+#endif /* DFD_H */
diff --git a/plat/mediatek/drivers/dfd/mt8188/plat_dfd.c b/plat/mediatek/drivers/dfd/mt8188/plat_dfd.c
new file mode 100644
index 0000000..1aa68f5
--- /dev/null
+++ b/plat/mediatek/drivers/dfd/mt8188/plat_dfd.c
@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <dfd.h>
+#include <plat_dfd.h>
+
+static uint64_t dfd_cache_dump;
+static bool dfd_enabled;
+static uint64_t dfd_base_addr;
+static uint64_t dfd_chain_length;
+
+void dfd_setup(uint64_t base_addr, uint64_t chain_length, uint64_t cache_dump)
+{
+	mmio_write_32(MTK_DRM_LATCH_CTL1, MTK_DRM_LATCH_CTL1_VAL);
+	mmio_write_32(MTK_DRM_LATCH_CTL2, MTK_DRM_LATCH_CTL2_VAL);
+	mmio_write_32(MTK_WDT_LATCH_CTL2, MTK_WDT_LATCH_CTL2_VAL);
+
+	mmio_clrbits_32(DFD_O_INTRF_MCU_PWR_CTL_MASK, BIT(2));
+	mmio_setbits_32(DFD_V50_GROUP_0_63_DIFF, 0x1);
+	sync_writel(DFD_INTERNAL_CTL, 0x5);
+	mmio_setbits_32(DFD_INTERNAL_CTL, BIT(13));
+	mmio_setbits_32(DFD_INTERNAL_CTL, 0x1F << 3);
+	mmio_setbits_32(DFD_INTERNAL_CTL, 0x3 << 9);
+	mmio_setbits_32(DFD_INTERNAL_CTL, 0x3 << 19);
+
+	mmio_write_32(DFD_INTERNAL_PWR_ON, 0xB);
+	mmio_write_32(DFD_CHAIN_LENGTH0, chain_length);
+	mmio_write_32(DFD_INTERNAL_SHIFT_CLK_RATIO, 0x0);
+	mmio_write_32(DFD_INTERNAL_TEST_SO_OVER_64, 0x1);
+
+	mmio_write_32(DFD_TEST_SI_0, 0x0);
+	mmio_write_32(DFD_TEST_SI_1, 0x0);
+	mmio_write_32(DFD_TEST_SI_2, 0x0);
+	mmio_write_32(DFD_TEST_SI_3, 0x0);
+
+	sync_writel(DFD_POWER_CTL, 0xF9);
+	sync_writel(DFD_READ_ADDR, DFD_READ_ADDR_VAL);
+	sync_writel(DFD_V30_CTL, 0xD);
+
+	mmio_write_32(DFD_O_SET_BASEADDR_REG, base_addr >> 24);
+	mmio_write_32(DFD_O_REG_0, 0);
+
+	/* setup global variables for suspend and resume */
+	dfd_enabled = true;
+	dfd_base_addr = base_addr;
+	dfd_chain_length = chain_length;
+	dfd_cache_dump = cache_dump;
+
+	if ((cache_dump & DFD_CACHE_DUMP_ENABLE) != 0UL) {
+		mmio_write_32(MTK_DRM_LATCH_CTL2, MTK_DRM_LATCH_CTL2_CACHE_VAL);
+		sync_writel(DFD_V35_ENABLE, 0x1);
+		sync_writel(DFD_V35_TAP_NUMBER, 0xB);
+		sync_writel(DFD_V35_TAP_EN, DFD_V35_TAP_EN_VAL);
+		sync_writel(DFD_V35_SEQ0_0, DFD_V35_SEQ0_0_VAL);
+
+		/* Cache dump only mode */
+		sync_writel(DFD_V35_CTL, 0x1);
+		mmio_write_32(DFD_INTERNAL_NUM_OF_TEST_SO_GROUP, 0xF);
+		mmio_write_32(DFD_CHAIN_LENGTH0, DFD_CHAIN_LENGTH_VAL);
+		mmio_write_32(DFD_CHAIN_LENGTH1, DFD_CHAIN_LENGTH_VAL);
+		mmio_write_32(DFD_CHAIN_LENGTH2, DFD_CHAIN_LENGTH_VAL);
+		mmio_write_32(DFD_CHAIN_LENGTH3, DFD_CHAIN_LENGTH_VAL);
+
+		if ((cache_dump & DFD_PARITY_ERR_TRIGGER) != 0UL) {
+			sync_writel(DFD_HW_TRIGGER_MASK, 0xC);
+			mmio_setbits_32(DFD_INTERNAL_CTL, 0x1 << 4);
+		}
+	}
+	dsbsy();
+}
+
+void dfd_resume(void)
+{
+	if (dfd_enabled == true) {
+		dfd_setup(dfd_base_addr, dfd_chain_length, dfd_cache_dump);
+	}
+}
diff --git a/plat/mediatek/drivers/dfd/mt8188/plat_dfd.h b/plat/mediatek/drivers/dfd/mt8188/plat_dfd.h
new file mode 100644
index 0000000..5b98024
--- /dev/null
+++ b/plat/mediatek/drivers/dfd/mt8188/plat_dfd.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_DFD_H
+#define PLAT_DFD_H
+
+#include <lib/mmio.h>
+#include <platform_def.h>
+
+#define sync_writel(addr, val) do { mmio_write_32((addr), (val)); dsbsy(); } while (0)
+
+#define PLAT_MTK_DFD_SETUP_MAGIC		(0x99716150)
+#define PLAT_MTK_DFD_READ_MAGIC			(0x99716151)
+#define PLAT_MTK_DFD_WRITE_MAGIC		(0x99716152)
+
+#define MTK_DRM_LATCH_CTL1			(DRM_BASE + 0x40)
+#define MTK_DRM_LATCH_CTL2			(DRM_BASE + 0x44)
+
+#define MTK_WDT_BASE				(RGU_BASE)
+#define MTK_WDT_INTERVAL			(MTK_WDT_BASE + 0x10)
+#define MTK_WDT_LATCH_CTL2			(MTK_WDT_BASE + 0x48)
+
+#define MCU_BIU_BASE				(MCUCFG_BASE)
+#define MISC1_CFG_BASE				(MCU_BIU_BASE + 0xE040)
+#define DFD_INTERNAL_CTL			(MISC1_CFG_BASE + 0x00)
+#define DFD_INTERNAL_PWR_ON			(MISC1_CFG_BASE + 0x08)
+#define DFD_CHAIN_LENGTH0			(MISC1_CFG_BASE + 0x0C)
+#define DFD_INTERNAL_SHIFT_CLK_RATIO		(MISC1_CFG_BASE + 0x10)
+#define DFD_CHAIN_LENGTH1			(MISC1_CFG_BASE + 0x1C)
+#define DFD_CHAIN_LENGTH2			(MISC1_CFG_BASE + 0x20)
+#define DFD_CHAIN_LENGTH3			(MISC1_CFG_BASE + 0x24)
+#define DFD_INTERNAL_TEST_SO_0			(MISC1_CFG_BASE + 0x28)
+#define DFD_INTERNAL_NUM_OF_TEST_SO_GROUP	(MISC1_CFG_BASE + 0x30)
+#define DFD_INTERNAL_TEST_SO_OVER_64		(MISC1_CFG_BASE + 0x34)
+#define DFD_INTERNAL_SW_NS_TRIGGER		(MISC1_CFG_BASE + 0x3c)
+#define DFD_V30_CTL				(MISC1_CFG_BASE + 0x48)
+#define DFD_V30_BASE_ADDR			(MISC1_CFG_BASE + 0x4C)
+#define DFD_POWER_CTL				(MISC1_CFG_BASE + 0x50)
+#define DFD_TEST_SI_0				(MISC1_CFG_BASE + 0x58)
+#define DFD_TEST_SI_1				(MISC1_CFG_BASE + 0x5C)
+#define DFD_CLEAN_STATUS			(MISC1_CFG_BASE + 0x60)
+#define DFD_TEST_SI_2				(MISC1_CFG_BASE + 0x1D8)
+#define DFD_TEST_SI_3				(MISC1_CFG_BASE + 0x1DC)
+#define DFD_READ_ADDR				(MISC1_CFG_BASE + 0x1E8)
+#define DFD_HW_TRIGGER_MASK			(MISC1_CFG_BASE + 0xBC)
+
+#define DFD_V35_ENABLE				(MCU_BIU_BASE + 0xE0A8)
+#define DFD_V35_TAP_NUMBER			(MCU_BIU_BASE + 0xE0AC)
+#define DFD_V35_TAP_EN				(MCU_BIU_BASE + 0xE0B0)
+#define DFD_V35_CTL				(MCU_BIU_BASE + 0xE0B4)
+#define DFD_V35_SEQ0_0				(MCU_BIU_BASE + 0xE0C0)
+#define DFD_V35_SEQ0_1				(MCU_BIU_BASE + 0xE0C4)
+#define DFD_V50_GROUP_0_63_DIFF			(MCU_BIU_BASE + 0xE2AC)
+
+#define DFD_O_PROTECT_EN_REG			(0x10001220)
+#define DFD_O_INTRF_MCU_PWR_CTL_MASK		(0x10001A3C)
+#define DFD_O_SET_BASEADDR_REG			(0x10043000)
+#define DFD_O_REG_0				(0x10001390)
+
+#define DFD_CACHE_DUMP_ENABLE			(1U)
+#define DFD_PARITY_ERR_TRIGGER			(2U)
+
+#define DFD_V35_TAP_EN_VAL			(0x43FF)
+#define DFD_V35_SEQ0_0_VAL			(0x63668820)
+#define DFD_READ_ADDR_VAL			(0x40000008)
+#define DFD_CHAIN_LENGTH_VAL			(0xFFFFFFFF)
+
+#define MTK_WDT_LATCH_CTL2_VAL			(0x9507FFFF)
+#define MTK_WDT_INTERVAL_VAL			(0x6600000A)
+#define MTK_DRM_LATCH_CTL2_VAL			(0x950607D0)
+#define MTK_DRM_LATCH_CTL2_CACHE_VAL		(0x95065DC0)
+
+#define MTK_DRM_LATCH_CTL1_VAL			(0x95000013)
+
+#endif /* PLAT_DFD_H */
diff --git a/plat/mediatek/drivers/dfd/rules.mk b/plat/mediatek/drivers/dfd/rules.mk
new file mode 100644
index 0000000..60fbc88
--- /dev/null
+++ b/plat/mediatek/drivers/dfd/rules.mk
@@ -0,0 +1,17 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := mtk_dfd
+
+LOCAL_SRCS-y := ${LOCAL_DIR}/dfd.c
+LOCAL_SRCS-y += ${LOCAL_DIR}/$(MTK_SOC)/plat_dfd.c
+
+PLAT_INCLUDES += -I${LOCAL_DIR}
+PLAT_INCLUDES += -I${LOCAL_DIR}/$(MTK_SOC)
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/drivers/emi_mpu/emi_mpu.h b/plat/mediatek/drivers/emi_mpu/emi_mpu.h
new file mode 100644
index 0000000..66a369e
--- /dev/null
+++ b/plat/mediatek/drivers/emi_mpu/emi_mpu.h
@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef EMI_MPU_H
+#define EMI_MPU_H
+
+#include <emi_mpu_priv.h>
+#include <platform_def.h>
+
+#define NO_PROTECTION			(0)
+#define SEC_RW				(1)
+#define SEC_RW_NSEC_R			(2)
+#define SEC_RW_NSEC_W			(3)
+#define SEC_R_NSEC_R			(4)
+#define FORBIDDEN			(5)
+#define SEC_R_NSEC_RW			(6)
+
+#define LOCK				(1)
+#define UNLOCK				(0)
+
+#if (EMI_MPU_DGROUP_NUM == 1)
+#define SET_ACCESS_PERMISSION(apc_ary, lock, d7, d6, d5, d4, d3, d2, d1, d0) \
+do { \
+	apc_ary[1] = 0; \
+	apc_ary[0] = \
+		(((unsigned int)  d7) << 21) | (((unsigned int)  d6) << 18) | \
+		(((unsigned int)  d5) << 15) | (((unsigned int)  d4) << 12) | \
+		(((unsigned int)  d3) <<  9) | (((unsigned int)  d2) <<  6) | \
+		(((unsigned int)  d1) <<  3) |  ((unsigned int)  d0) | \
+		((unsigned int) lock << 31); \
+} while (0)
+#elif (EMI_MPU_DGROUP_NUM == 2)
+#define SET_ACCESS_PERMISSION(apc_ary, lock, d15, d14, d13, d12, d11, d10, \
+				d9, d8, d7, d6, d5, d4, d3, d2, d1, d0) \
+do { \
+	apc_ary[1] = \
+		(((unsigned int) d15) << 21) | (((unsigned int) d14) << 18) | \
+		(((unsigned int) d13) << 15) | (((unsigned int) d12) << 12) | \
+		(((unsigned int) d11) <<  9) | (((unsigned int) d10) <<  6) | \
+		(((unsigned int)  d9) <<  3) |  ((unsigned int)  d8); \
+	apc_ary[0] = \
+		(((unsigned int)  d7) << 21) | (((unsigned int)  d6) << 18) | \
+		(((unsigned int)  d5) << 15) | (((unsigned int)  d4) << 12) | \
+		(((unsigned int)  d3) <<  9) | (((unsigned int)  d2) <<  6) | \
+		(((unsigned int)  d1) <<  3) |  ((unsigned int)  d0) | \
+		((unsigned int) lock << 31); \
+} while (0)
+#endif
+
+struct emi_region_info_t {
+	unsigned long long start;
+	unsigned long long end;
+	unsigned int region;
+	unsigned int apc[EMI_MPU_DGROUP_NUM];
+};
+
+int emi_mpu_init(void);
+int emi_mpu_set_protection(struct emi_region_info_t *region_info);
+void set_emi_mpu_regions(void);
+
+#endif
diff --git a/plat/mediatek/drivers/emi_mpu/emi_mpu_common.c b/plat/mediatek/drivers/emi_mpu/emi_mpu_common.c
new file mode 100644
index 0000000..27b2b07
--- /dev/null
+++ b/plat/mediatek/drivers/emi_mpu/emi_mpu_common.c
@@ -0,0 +1,123 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <string.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <emi_mpu.h>
+#include <lib/mtk_init/mtk_init.h>
+
+#if ENABLE_EMI_MPU_SW_LOCK
+static unsigned char region_lock_state[EMI_MPU_REGION_NUM];
+#endif
+
+#define EMI_MPU_START_MASK		(0x00FFFFFF)
+#define EMI_MPU_END_MASK		(0x00FFFFFF)
+#define EMI_MPU_APC_SW_LOCK_MASK	(0x00FFFFFF)
+#define EMI_MPU_APC_HW_LOCK_MASK	(0x80FFFFFF)
+
+static int _emi_mpu_set_protection(unsigned int start, unsigned int end,
+					unsigned int apc)
+{
+	unsigned int dgroup;
+	unsigned int region;
+
+	region = (start >> 24) & 0xFF;
+	start &= EMI_MPU_START_MASK;
+	dgroup = (end >> 24) & 0xFF;
+	end &= EMI_MPU_END_MASK;
+
+	if  ((region >= EMI_MPU_REGION_NUM) || (dgroup > EMI_MPU_DGROUP_NUM)) {
+		WARN("invalid region, domain\n");
+		return -1;
+	}
+
+#if ENABLE_EMI_MPU_SW_LOCK
+	if (region_lock_state[region] == 1) {
+		WARN("invalid region\n");
+		return -1;
+	}
+
+	if ((dgroup == 0) && ((apc >> 31) & 0x1)) {
+		region_lock_state[region] = 1;
+	}
+
+	apc &= EMI_MPU_APC_SW_LOCK_MASK;
+#else
+	apc &= EMI_MPU_APC_HW_LOCK_MASK;
+#endif
+
+	if ((start >= DRAM_OFFSET) && (end >= start)) {
+		start -= DRAM_OFFSET;
+		end -= DRAM_OFFSET;
+	} else {
+		WARN("invalid range\n");
+		return -1;
+	}
+
+	mmio_write_32(EMI_MPU_SA(region), start);
+	mmio_write_32(EMI_MPU_EA(region), end);
+	mmio_write_32(EMI_MPU_APC(region, dgroup), apc);
+
+#if defined(SUB_EMI_MPU_BASE)
+	mmio_write_32(SUB_EMI_MPU_SA(region), start);
+	mmio_write_32(SUB_EMI_MPU_EA(region), end);
+	mmio_write_32(SUB_EMI_MPU_APC(region, dgroup), apc);
+#endif
+	return 0;
+}
+
+static void dump_emi_mpu_regions(void)
+{
+	int region, i;
+
+	/* Only dump 8 regions(max: EMI_MPU_REGION_NUM --> 32) */
+	for (region = 0; region < 8; ++region) {
+		INFO("region %d:\n", region);
+		INFO("\tsa: 0x%x, ea: 0x%x\n",
+		     mmio_read_32(EMI_MPU_SA(region)), mmio_read_32(EMI_MPU_EA(region)));
+
+		for (i = 0; i < EMI_MPU_DGROUP_NUM; ++i) {
+			INFO("\tapc%d: 0x%x\n", i, mmio_read_32(EMI_MPU_APC(region, i)));
+		}
+	}
+}
+
+int emi_mpu_set_protection(struct emi_region_info_t *region_info)
+{
+	unsigned int start, end;
+	int i;
+
+	if (region_info->region >= EMI_MPU_REGION_NUM) {
+		WARN("invalid region\n");
+		return -1;
+	}
+
+	start = (unsigned int)(region_info->start >> EMI_MPU_ALIGN_BITS) |
+		(region_info->region << 24);
+
+	for (i = EMI_MPU_DGROUP_NUM - 1; i >= 0; i--) {
+		end = (unsigned int)(region_info->end >> EMI_MPU_ALIGN_BITS) | (i << 24);
+
+		if (_emi_mpu_set_protection(start, end, region_info->apc[i]) < 0) {
+			WARN("Failed to set emi mpu protection(%d, %d, %d)\n",
+			     start, end, region_info->apc[i]);
+		}
+	}
+
+	return 0;
+}
+
+int emi_mpu_init(void)
+{
+	INFO("[%s] emi mpu initialization\n", __func__);
+
+	set_emi_mpu_regions();
+	dump_emi_mpu_regions();
+
+	return 0;
+}
+MTK_PLAT_SETUP_0_INIT(emi_mpu_init);
diff --git a/plat/mediatek/drivers/emi_mpu/mt8188/emi_mpu.c b/plat/mediatek/drivers/emi_mpu/mt8188/emi_mpu.c
new file mode 100644
index 0000000..558533d
--- /dev/null
+++ b/plat/mediatek/drivers/emi_mpu/mt8188/emi_mpu.c
@@ -0,0 +1,14 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <emi_mpu.h>
+
+void set_emi_mpu_regions(void)
+{
+	/* TODO: set emi mpu region */
+	INFO("%s, emi mpu is not setting currently\n", __func__);
+}
diff --git a/plat/mediatek/drivers/emi_mpu/mt8188/emi_mpu_priv.h b/plat/mediatek/drivers/emi_mpu/mt8188/emi_mpu_priv.h
new file mode 100644
index 0000000..1ee7397
--- /dev/null
+++ b/plat/mediatek/drivers/emi_mpu/mt8188/emi_mpu_priv.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef EMI_MPU_PRIV_H
+#define EMI_MPU_PRIV_H
+
+#define ENABLE_EMI_MPU_SW_LOCK		(1)
+
+#define EMI_MPU_CTRL			(EMI_MPU_BASE + 0x000)
+#define EMI_MPU_DBG			(EMI_MPU_BASE + 0x004)
+#define EMI_MPU_SA0			(EMI_MPU_BASE + 0x100)
+#define EMI_MPU_EA0			(EMI_MPU_BASE + 0x200)
+#define EMI_MPU_SA(region)		(EMI_MPU_SA0 + (region * 4))
+#define EMI_MPU_EA(region)		(EMI_MPU_EA0 + (region * 4))
+#define EMI_MPU_APC0			(EMI_MPU_BASE + 0x300)
+#define EMI_MPU_APC(region, dgroup)	(EMI_MPU_APC0 + (region * 4) + (dgroup * 0x100))
+#define EMI_MPU_CTRL_D0			(EMI_MPU_BASE + 0x800)
+#define EMI_MPU_CTRL_D(domain)		(EMI_MPU_CTRL_D0 + (domain * 4))
+#define EMI_RG_MASK_D0			(EMI_MPU_BASE + 0x900)
+#define EMI_RG_MASK_D(domain)		(EMI_RG_MASK_D0 + (domain * 4))
+
+#define SUB_EMI_MPU_CTRL		(SUB_EMI_MPU_BASE + 0x000)
+#define SUB_EMI_MPU_DBG			(SUB_EMI_MPU_BASE + 0x004)
+#define SUB_EMI_MPU_SA0			(SUB_EMI_MPU_BASE + 0x100)
+#define SUB_EMI_MPU_EA0			(SUB_EMI_MPU_BASE + 0x200)
+#define SUB_EMI_MPU_SA(region)		(SUB_EMI_MPU_SA0 + (region * 4))
+#define SUB_EMI_MPU_EA(region)		(SUB_EMI_MPU_EA0 + (region * 4))
+#define SUB_EMI_MPU_APC0		(SUB_EMI_MPU_BASE + 0x300)
+#define SUB_EMI_MPU_APC(region, dgroup)	(SUB_EMI_MPU_APC0 + (region * 4) + (dgroup * 0x100))
+#define SUB_EMI_MPU_CTRL_D0		(SUB_EMI_MPU_BASE + 0x800)
+#define SUB_EMI_MPU_CTRL_D(domain)	(SUB_EMI_MPU_CTRL_D0 + (domain * 4))
+#define SUB_EMI_RG_MASK_D0		(SUB_EMI_MPU_BASE + 0x900)
+#define SUB_EMI_RG_MASK_D(domain)	(SUB_EMI_RG_MASK_D0 + (domain * 4))
+
+#define EMI_MPU_DOMAIN_NUM		(16)
+#define EMI_MPU_REGION_NUM		(32)
+#define EMI_MPU_ALIGN_BITS		(16)
+#define DRAM_OFFSET			(0x40000000 >> EMI_MPU_ALIGN_BITS)
+
+#define EMI_MPU_DGROUP_NUM		(EMI_MPU_DOMAIN_NUM / 8)
+
+#endif
diff --git a/plat/mediatek/drivers/emi_mpu/rules.mk b/plat/mediatek/drivers/emi_mpu/rules.mk
new file mode 100644
index 0000000..ed3d777
--- /dev/null
+++ b/plat/mediatek/drivers/emi_mpu/rules.mk
@@ -0,0 +1,16 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := emi_mpu
+LOCAL_SRCS-y := $(LOCAL_DIR)/emi_mpu_common.c
+LOCAL_SRCS-y += ${LOCAL_DIR}/${MTK_SOC}/emi_mpu.c
+
+PLAT_INCLUDES += -I${LOCAL_DIR}
+PLAT_INCLUDES += -I${LOCAL_DIR}/${MTK_SOC}
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/drivers/lpm/rules.mk b/plat/mediatek/drivers/lpm/rules.mk
new file mode 100644
index 0000000..87a212a
--- /dev/null
+++ b/plat/mediatek/drivers/lpm/rules.mk
@@ -0,0 +1,14 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := lpm
+LOCAL_SRCS-y := $(LOCAL_DIR)/mt_lp_rm.c
+
+PLAT_INCLUDES += -I${LOCAL_DIR}
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/drivers/mcusys/mcusys.c b/plat/mediatek/drivers/mcusys/mcusys.c
new file mode 100644
index 0000000..63edb23
--- /dev/null
+++ b/plat/mediatek/drivers/mcusys/mcusys.c
@@ -0,0 +1,15 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include <mtk_mmap_pool.h>
+#include <platform_def.h>
+
+static const mmap_region_t mcusys_mmap[] MTK_MMAP_SECTION = {
+	MAP_REGION_FLAT(MCUCFG_BASE, MCUCFG_REG_SIZE, MT_DEVICE | MT_RW | MT_SECURE),
+	{0}
+};
+DECLARE_MTK_MMAP_REGIONS(mcusys_mmap);
diff --git a/plat/mediatek/drivers/mcusys/rules.mk b/plat/mediatek/drivers/mcusys/rules.mk
new file mode 100644
index 0000000..5438998
--- /dev/null
+++ b/plat/mediatek/drivers/mcusys/rules.mk
@@ -0,0 +1,15 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := mcusys
+
+PLAT_INCLUDES += -I$(LOCAL_DIR)/$(MCUSYS_VERSION)
+
+LOCAL_SRCS-y := $(LOCAL_DIR)/mcusys.c
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/drivers/mcusys/v1/mcucfg.h b/plat/mediatek/drivers/mcusys/v1/mcucfg.h
new file mode 100644
index 0000000..7aced5a
--- /dev/null
+++ b/plat/mediatek/drivers/mcusys/v1/mcucfg.h
@@ -0,0 +1,232 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MCUCFG_V1_H
+#define MCUCFG_V1_H
+
+#ifndef __ASSEMBLER__
+#include <stdint.h>
+#endif /*__ASSEMBLER__*/
+
+#include <platform_def.h>
+
+#define MP2_MISC_CONFIG_BOOT_ADDR_L(cpu)	(MCUCFG_BASE + 0x2290 + ((cpu) * 8))
+#define MP2_MISC_CONFIG_BOOT_ADDR_H(cpu)	(MCUCFG_BASE + 0x2294 + ((cpu) * 8))
+
+#define MP2_CPUCFG				(MCUCFG_BASE + 0x2208)
+
+#define MP0_CPUTOP_SPMC_CTL			(MCUCFG_BASE + 0x788)
+#define MP1_CPUTOP_SPMC_CTL			(MCUCFG_BASE + 0x78C)
+#define MP1_CPUTOP_SPMC_SRAM_CTL		(MCUCFG_BASE + 0x790)
+
+#define CPUSYSx_CPUx_SPMC_CTL(cluster, cpu)	(MCUCFG_BASE + 0x1C30 + \
+						 (cluster) * 0x2000 + (cpu) * 4)
+
+#define CPUSYS0_CPU0_SPMC_CTL			(MCUCFG_BASE + 0x1C30)
+#define CPUSYS0_CPU1_SPMC_CTL			(MCUCFG_BASE + 0x1C34)
+#define CPUSYS0_CPU2_SPMC_CTL			(MCUCFG_BASE + 0x1C38)
+#define CPUSYS0_CPU3_SPMC_CTL			(MCUCFG_BASE + 0x1C3C)
+
+#define CPUSYS1_CPU0_SPMC_CTL			(MCUCFG_BASE + 0x3C30)
+#define CPUSYS1_CPU1_SPMC_CTL			(MCUCFG_BASE + 0x3C34)
+#define CPUSYS1_CPU2_SPMC_CTL			(MCUCFG_BASE + 0x3C38)
+#define CPUSYS1_CPU3_SPMC_CTL			(MCUCFG_BASE + 0x3C3C)
+
+/* CPC related registers */
+#define CPC_MCUSYS_CPC_OFF_THRES		(MCUCFG_BASE + 0xA714)
+#define CPC_MCUSYS_PWR_CTRL			(MCUCFG_BASE + 0xA804)
+#define CPC_MCUSYS_CPC_FLOW_CTRL_CFG		(MCUCFG_BASE + 0xA814)
+#define CPC_MCUSYS_LAST_CORE_REQ		(MCUCFG_BASE + 0xA818)
+#define CPC_MCUSYS_MP_LAST_CORE_RESP		(MCUCFG_BASE + 0xA81C)
+#define CPC_MCUSYS_LAST_CORE_RESP		(MCUCFG_BASE + 0xA824)
+#define CPC_MCUSYS_PWR_ON_MASK			(MCUCFG_BASE + 0xA828)
+#define CPC_SPMC_PWR_STATUS			(MCUCFG_BASE + 0xA840)
+#define CPC_MCUSYS_CPU_ON_SW_HINT_SET		(MCUCFG_BASE + 0xA8A8)
+#define CPC_MCUSYS_CPU_ON_SW_HINT_CLR		(MCUCFG_BASE + 0xA8AC)
+#define CPC_MCUSYS_CPC_DBG_SETTING		(MCUCFG_BASE + 0xAB00)
+#define CPC_MCUSYS_CPC_KERNEL_TIME_L_BASE	(MCUCFG_BASE + 0xAB04)
+#define CPC_MCUSYS_CPC_KERNEL_TIME_H_BASE	(MCUCFG_BASE + 0xAB08)
+#define CPC_MCUSYS_CPC_SYSTEM_TIME_L_BASE	(MCUCFG_BASE + 0xAB0C)
+#define CPC_MCUSYS_CPC_SYSTEM_TIME_H_BASE	(MCUCFG_BASE + 0xAB10)
+#define CPC_MCUSYS_TRACE_SEL			(MCUCFG_BASE + 0xAB14)
+#define CPC_MCUSYS_TRACE_DATA			(MCUCFG_BASE + 0xAB20)
+#define CPC_MCUSYS_CLUSTER_COUNTER		(MCUCFG_BASE + 0xAB70)
+#define CPC_MCUSYS_CLUSTER_COUNTER_CLR		(MCUCFG_BASE + 0xAB74)
+
+/* CPC_MCUSYS_CPC_FLOW_CTRL_CFG bit control */
+#define CPC_CTRL_ENABLE				BIT(16)
+#define SSPM_CORE_PWR_ON_EN			BIT(7) /* for cpu-hotplug */
+#define SSPM_ALL_PWR_CTRL_EN			BIT(13) /* for cpu-hotplug */
+#define GIC_WAKEUP_IGNORE(cpu)			BIT(21 + cpu)
+
+#define CPC_MCUSYS_CPC_RESET_ON_KEEP_ON		BIT(17)
+#define CPC_MCUSYS_CPC_RESET_PWR_ON_EN		BIT(20)
+
+/* SPMC related registers */
+#define SPM_MCUSYS_PWR_CON			(MCUCFG_BASE + 0xD200)
+#define SPM_MP0_CPUTOP_PWR_CON			(MCUCFG_BASE + 0xD204)
+#define SPM_MP0_CPU0_PWR_CON			(MCUCFG_BASE + 0xD208)
+#define SPM_MP0_CPU1_PWR_CON			(MCUCFG_BASE + 0xD20C)
+#define SPM_MP0_CPU2_PWR_CON			(MCUCFG_BASE + 0xD210)
+#define SPM_MP0_CPU3_PWR_CON			(MCUCFG_BASE + 0xD214)
+#define SPM_MP0_CPU4_PWR_CON			(MCUCFG_BASE + 0xD218)
+#define SPM_MP0_CPU5_PWR_CON			(MCUCFG_BASE + 0xD21C)
+#define SPM_MP0_CPU6_PWR_CON			(MCUCFG_BASE + 0xD220)
+#define SPM_MP0_CPU7_PWR_CON			(MCUCFG_BASE + 0xD224)
+
+/* bit fields of SPM_*_PWR_CON */
+#define PWR_ON_ACK				BIT(31)
+#define VPROC_EXT_OFF				BIT(7)
+#define DORMANT_EN				BIT(6)
+#define RESETPWRON_CONFIG			BIT(5)
+#define PWR_CLK_DIS				BIT(4)
+#define PWR_ON					BIT(2)
+#define PWR_RST_B				BIT(0)
+
+#define SPARK2LDO				(MCUCFG_BASE + 0x2700)
+/* APB Module mcucfg */
+#define MP0_CA7_CACHE_CONFIG			(MCUCFG_BASE + 0x000)
+#define MP0_AXI_CONFIG				(MCUCFG_BASE + 0x02C)
+#define MP0_MISC_CONFIG0			(MCUCFG_BASE + 0x030)
+#define MP0_MISC_CONFIG1			(MCUCFG_BASE + 0x034)
+#define MP0_MISC_CONFIG2			(MCUCFG_BASE + 0x038)
+#define MP0_MISC_CONFIG_BOOT_ADDR(cpu)		(MCUCFG_BASE + 0x038 + ((cpu) * 8))
+#define MP0_MISC_CONFIG3			(MCUCFG_BASE + 0x03C)
+#define MP0_MISC_CONFIG9			(MCUCFG_BASE + 0x054)
+#define MP0_CA7_MISC_CONFIG			(MCUCFG_BASE + 0x064)
+
+#define MP0_RW_RSVD0				(MCUCFG_BASE + 0x06C)
+#define MP1_CA7_CACHE_CONFIG			(MCUCFG_BASE + 0x200)
+#define MP1_AXI_CONFIG				(MCUCFG_BASE + 0x22C)
+#define MP1_MISC_CONFIG0			(MCUCFG_BASE + 0x230)
+#define MP1_MISC_CONFIG1			(MCUCFG_BASE + 0x234)
+#define MP1_MISC_CONFIG2			(MCUCFG_BASE + 0x238)
+#define MP1_MISC_CONFIG_BOOT_ADDR(cpu)		(MCUCFG_BASE + 0x238 + ((cpu) * 8))
+#define MP1_MISC_CONFIG3			(MCUCFG_BASE + 0x23C)
+#define MP1_MISC_CONFIG9			(MCUCFG_BASE + 0x254)
+#define MP1_CA7_MISC_CONFIG			(MCUCFG_BASE + 0x264)
+
+#define CCI_ADB400_DCM_CONFIG			(MCUCFG_BASE + 0x740)
+#define SYNC_DCM_CONFIG				(MCUCFG_BASE + 0x744)
+
+#define MP0_CLUSTER_CFG0			(MCUCFG_BASE + 0xC8D0)
+
+#define MP0_SPMC				(MCUCFG_BASE + 0x788)
+#define MP1_SPMC				(MCUCFG_BASE + 0x78C)
+#define MP2_AXI_CONFIG				(MCUCFG_BASE + 0x220C)
+#define MP2_AXI_CONFIG_ACINACTM			BIT(0)
+#define MP2_AXI_CONFIG_AINACTS			BIT(4)
+
+#define MPx_AXI_CONFIG_ACINACTM			BIT(4)
+#define MPx_AXI_CONFIG_AINACTS			BIT(5)
+
+#define MPx_CA7_MISC_CONFIG_standbywfil2	BIT(28)
+
+#define MP0_CPU0_STANDBYWFE			BIT(20)
+#define MP0_CPU1_STANDBYWFE			BIT(21)
+#define MP0_CPU2_STANDBYWFE			BIT(22)
+#define MP0_CPU3_STANDBYWFE			BIT(23)
+
+#define MP1_CPU0_STANDBYWFE			BIT(20)
+#define MP1_CPU1_STANDBYWFE			BIT(21)
+#define MP1_CPU2_STANDBYWFE			BIT(22)
+#define MP1_CPU3_STANDBYWFE			BIT(23)
+
+#define CPUSYS0_SPARKVRETCNTRL			(MCUCFG_BASE+0x1c00)
+#define CPUSYS0_SPARKEN				(MCUCFG_BASE+0x1c04)
+#define CPUSYS0_AMUXSEL				(MCUCFG_BASE+0x1c08)
+#define CPUSYS1_SPARKVRETCNTRL			(MCUCFG_BASE+0x3c00)
+#define CPUSYS1_SPARKEN				(MCUCFG_BASE+0x3c04)
+#define CPUSYS1_AMUXSEL				(MCUCFG_BASE+0x3c08)
+
+#define MP2_PWR_RST_CTL				(MCUCFG_BASE + 0x2008)
+#define MP2_PTP3_CPUTOP_SPMC0			(MCUCFG_BASE + 0x22A0)
+#define MP2_PTP3_CPUTOP_SPMC1			(MCUCFG_BASE + 0x22A4)
+
+#define MP2_COQ					(MCUCFG_BASE + 0x22BC)
+#define MP2_COQ_SW_DIS				BIT(0)
+
+#define MP2_CA15M_MON_SEL			(MCUCFG_BASE + 0x2400)
+#define MP2_CA15M_MON_L				(MCUCFG_BASE + 0x2404)
+
+#define CPUSYS2_CPU0_SPMC_CTL			(MCUCFG_BASE + 0x2430)
+#define CPUSYS2_CPU1_SPMC_CTL			(MCUCFG_BASE + 0x2438)
+#define CPUSYS2_CPU0_SPMC_STA			(MCUCFG_BASE + 0x2434)
+#define CPUSYS2_CPU1_SPMC_STA			(MCUCFG_BASE + 0x243C)
+
+#define MP0_CA7L_DBG_PWR_CTRL			(MCUCFG_BASE + 0x068)
+#define MP1_CA7L_DBG_PWR_CTRL			(MCUCFG_BASE + 0x268)
+#define BIG_DBG_PWR_CTRL			(MCUCFG_BASE + 0x75C)
+
+#define MP2_SW_RST_B				BIT(0)
+#define MP2_TOPAON_APB_MASK			BIT(1)
+#define B_SW_HOT_PLUG_RESET			BIT(30)
+#define B_SW_PD_OFFSET				(18)
+#define B_SW_PD					(0x3F << B_SW_PD_OFFSET)
+
+#define B_SW_SRAM_SLEEPB_OFFSET			(12)
+#define B_SW_SRAM_SLEEPB			(0x3F << B_SW_SRAM_SLEEPB_OFFSET)
+
+#define B_SW_SRAM_ISOINTB			BIT(9)
+#define B_SW_ISO				BIT(8)
+#define B_SW_LOGIC_PDB				BIT(7)
+#define B_SW_LOGIC_PRE2_PDB			BIT(6)
+#define B_SW_LOGIC_PRE1_PDB			BIT(5)
+#define B_SW_FSM_OVERRIDE			BIT(4)
+#define B_SW_PWR_ON				BIT(3)
+#define B_SW_PWR_ON_OVERRIDE_EN			BIT(2)
+
+#define B_FSM_STATE_OUT_OFFSET			(6)
+#define B_FSM_STATE_OUT_MASK			(0x1F << B_FSM_STATE_OUT_OFFSET)
+#define B_SW_LOGIC_PDBO_ALL_OFF_ACK		BIT(5)
+#define B_SW_LOGIC_PDBO_ALL_ON_ACK		BIT(4)
+#define B_SW_LOGIC_PRE2_PDBO_ALL_ON_ACK		BIT(3)
+#define B_SW_LOGIC_PRE1_PDBO_ALL_ON_ACK		BIT(2)
+
+
+#define B_FSM_OFF				(0U << B_FSM_STATE_OUT_OFFSET)
+#define B_FSM_ON				(1U << B_FSM_STATE_OUT_OFFSET)
+#define B_FSM_RET				(2U << B_FSM_STATE_OUT_OFFSET)
+
+#ifndef __ASSEMBLER__
+/* cpu boot mode */
+enum mp0_coucfg_64bit_ctrl {
+	MP0_CPUCFG_64BIT_SHIFT = 12,
+	MP1_CPUCFG_64BIT_SHIFT = 28,
+	MP0_CPUCFG_64BIT = 0xfu << MP0_CPUCFG_64BIT_SHIFT,
+	MP1_CPUCFG_64BIT = 0xfu << MP1_CPUCFG_64BIT_SHIFT,
+};
+
+enum mp1_dis_rgu0_ctrl {
+	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_ctrl {
+	MP1_AINACTS_SHIFT = 4,
+	MP1_AINACTS = 1U << MP1_AINACTS_SHIFT,
+};
+
+enum mp1_sw_cg_gen {
+	MP1_SW_CG_GEN_SHIFT = 12,
+	MP1_SW_CG_GEN = 1U << MP1_SW_CG_GEN_SHIFT,
+};
+
+enum mp1_l2rstdisable {
+	MP1_L2RSTDISABLE_SHIFT = 14,
+	MP1_L2RSTDISABLE = 1U << MP1_L2RSTDISABLE_SHIFT,
+};
+#endif /*__ASSEMBLER__*/
+
+#endif  /* MCUCFG_V1_H */
diff --git a/plat/mediatek/helpers/armv8_2/arch_helpers.S b/plat/mediatek/helpers/armv8_2/arch_helpers.S
new file mode 100644
index 0000000..02d8d53
--- /dev/null
+++ b/plat/mediatek/helpers/armv8_2/arch_helpers.S
@@ -0,0 +1,118 @@
+/*
+ * Copyright (c) 2022, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <assert_macros.S>
+#include <cpu_macros.S>
+#if CONFIG_MTK_MCUSYS
+#include <mcucfg.h>
+#endif
+#include <platform_def.h>
+	/*
+	 * Declare as weak function so that can be
+	 * overwritten by platform helpers
+	 */
+	.weak platform_mem_init
+	.weak plat_core_pos_by_mpidr
+	.weak plat_my_core_pos
+	.weak plat_mediatek_calc_core_pos
+	.global plat_mpidr_by_core_pos
+	.global plat_reset_handler
+
+	/* -----------------------------------------------------
+	 * unsigned long plat_mpidr_by_core_pos(uint32_t cpuid)
+	 * This function calcuate mpidr by cpu pos if cpu
+	 * topology is linear.
+	 *
+	 * Clobbers: x0-x1
+	 * -----------------------------------------------------
+	 */
+func plat_mpidr_by_core_pos
+	lsl x0, x0, #MPIDR_AFF1_SHIFT
+	mrs x1, mpidr_el1
+	and x1, x1, #MPIDR_MT_MASK
+	orr x0, x0, x1
+	ret
+endfunc plat_mpidr_by_core_pos
+
+	/* -----------------------------------------------------
+	 *  unsigned int plat_my_core_pos(void)
+	 *  This function uses the plat_arm_calc_core_pos()
+	 *  definition to get the index of the calling CPU.
+	 * -----------------------------------------------------
+	 */
+func plat_my_core_pos
+	mrs	x0, mpidr_el1
+	b plat_mediatek_calc_core_pos
+endfunc plat_my_core_pos
+
+	/* -----------------------------------------------------
+	 * int plat_mediatek_calc_core_pos(u_register_t mpidr);
+	 *
+	 * In ARMv8.2, AFF2 is cluster id, AFF1 is core id and
+	 * AFF0 is thread id. There is only one cluster in ARMv8.2
+	 * and one thread in current implementation.
+	 *
+	 * With this function: CorePos = CoreID (AFF1)
+	 * we do it with x0 = (x0 >> 8) & 0xff
+	 * -----------------------------------------------------
+	 */
+func plat_mediatek_calc_core_pos
+	b plat_core_pos_by_mpidr
+endfunc plat_mediatek_calc_core_pos
+
+	/* ------------------------------------------------------
+	 * int32_t plat_core_pos_by_mpidr(u_register_t mpidr)
+	 *
+	 * This function implements a part of the critical
+	 * interface between the psci generic layer and the
+	 * platform that allows the former to query the platform
+	 * to convert an MPIDR to a unique linear index.
+	 *
+	 * Clobbers: x0-x1
+	 * ------------------------------------------------------
+	 */
+func plat_core_pos_by_mpidr
+	mov	x1, #MPIDR_AFFLVL_MASK
+	and	x0, x1, x0, lsr #MPIDR_AFF1_SHIFT
+	ret
+endfunc plat_core_pos_by_mpidr
+
+	/* --------------------------------------------------------
+	 * 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
+
+func plat_reset_handler
+#if CONFIG_MTK_MCUSYS
+	mov	x10, x30
+	bl	plat_my_core_pos
+	mov	x30, x10
+	mov     w1, #0x1
+	lsl     w1, w1, w0
+	ldr	x0, =CPC_MCUSYS_CPU_ON_SW_HINT_SET
+	str     w1, [x0]
+	dsb     sy
+#endif
+
+#if CONFIG_MTK_ECC
+	mov x10, x30
+	/* enable sequence of ecc for cpus */
+	bl disable_core_ecc
+	bl ft_ecc_clear_per_core
+	bl enable_core_ecc
+	mov x30, x10
+#endif
+
+	ret
+endfunc plat_reset_handler
diff --git a/plat/mediatek/helpers/rules.mk b/plat/mediatek/helpers/rules.mk
new file mode 100644
index 0000000..ae8068e
--- /dev/null
+++ b/plat/mediatek/helpers/rules.mk
@@ -0,0 +1,12 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := helpers
+LOCAL_SRCS-y += $(LOCAL_DIR)/$(ARCH_VERSION)/arch_helpers.S
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/include/armv8_2/arch_def.h b/plat/mediatek/include/armv8_2/arch_def.h
new file mode 100644
index 0000000..61f818f
--- /dev/null
+++ b/plat/mediatek/include/armv8_2/arch_def.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2022, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef ARCH_DEF_H
+#define ARCH_DEF_H
+
+/* Topology constants */
+#define PLAT_MAX_PWR_LVL		(2)
+#define PLAT_MAX_RET_STATE		(1)
+#define PLAT_MAX_OFF_STATE		(2)
+
+#define PLATFORM_SYSTEM_COUNT		(1)
+#define PLATFORM_CLUSTER_COUNT		(1)
+#define PLATFORM_CLUSTER0_CORE_COUNT	(8)
+#define PLATFORM_CORE_COUNT		(PLATFORM_CLUSTER0_CORE_COUNT)
+#define PLATFORM_MAX_CPUS_PER_CLUSTER	(8)
+#define PLATFORM_NUM_AFFS		(PLATFORM_SYSTEM_COUNT + \
+					 PLATFORM_CLUSTER_COUNT + \
+					 PLATFORM_CORE_COUNT)
+
+/*******************************************************************************
+ * 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.
+ ******************************************************************************/
+/* Cachline size */
+#define CACHE_WRITEBACK_SHIFT		(6)
+#define CACHE_WRITEBACK_GRANULE		(1 << CACHE_WRITEBACK_SHIFT)
+
+#endif /* ARCH_DEF_H */
+
diff --git a/plat/mediatek/include/cold_boot.h b/plat/mediatek/include/cold_boot.h
deleted file mode 100644
index 6b94b57..0000000
--- a/plat/mediatek/include/cold_boot.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
- * Copyright (c) 2022, Mediatek Inc. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef COLD_BOOT_H
-#define COLD_BOOT_H
-
-#include <stdint.h>
-
-/*******************************************************************************
- * Function and variable prototypes
- ******************************************************************************/
-#define LINUX_KERNEL_32	(0)
-#define LINUX_KERNEL_64	(1)
-#define DEVINFO_SIZE	(4)
-
-struct atf_arg_t {
-	uint32_t atf_magic;
-	uint32_t tee_support;
-	uint64_t tee_entry;
-	uint64_t tee_boot_arg_addr;
-	uint32_t hwuid[4]; /* HW Unique id for t-base used */
-	uint32_t HRID[8]; /* HW random id for t-base used */
-	uint32_t devinfo[DEVINFO_SIZE];
-};
-
-struct mtk_bl31_fw_config {
-	void *from_bl2; /* MTK boot tag */
-	void *soc_fw_config;
-	void *hw_config;
-	void *reserved;
-};
-
-enum {
-	BOOT_ARG_FROM_BL2,
-	BOOT_ARG_SOC_FW_CONFIG,
-	BOOT_ARG_HW_CONFIG,
-	BOOT_ARG_RESERVED
-};
-
-struct kernel_info {
-	uint64_t pc;
-	uint64_t r0;
-	uint64_t r1;
-	uint64_t r2;
-	uint64_t k32_64;
-};
-
-struct mtk_bl_param_t {
-	uint64_t bootarg_loc;
-	uint64_t bootarg_size;
-	uint64_t bl33_start_addr;
-	uint64_t atf_arg_addr;
-};
-
-void *get_mtk_bl31_fw_config(int index);
-bool is_el1_2nd_bootloader(void);
-
-#endif /* COLD_BOOT_H */
diff --git a/plat/mediatek/lib/pm/armv8_2/pwr_ctrl.c b/plat/mediatek/lib/pm/armv8_2/pwr_ctrl.c
new file mode 100644
index 0000000..7ffc4ed
--- /dev/null
+++ b/plat/mediatek/lib/pm/armv8_2/pwr_ctrl.c
@@ -0,0 +1,544 @@
+/*
+ * Copyright (c) 2022, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <common/debug.h>
+#include <drivers/arm/gicv3.h>
+#include <lib/psci/psci.h>
+#include <lib/utils.h>
+#ifdef MTK_PUBEVENT_ENABLE
+#include <mtk_event/mtk_pubsub_events.h>
+#endif
+#include <plat/arm/common/plat_arm.h>
+#include <plat/common/platform.h>
+
+#include <dfd.h>
+#include <lib/mtk_init/mtk_init.h>
+#include <lib/pm/mtk_pm.h>
+#include <mt_gic_v3.h>
+#include <platform_def.h>
+
+#define IS_AFFLV_PUBEVENT(_pstate) \
+	((_pstate & (MT_CPUPM_PWR_DOMAIN_MCUSYS | MT_CPUPM_PWR_DOMAIN_CLUSTER)) != 0)
+
+#ifdef MTK_PUBEVENT_ENABLE
+#define MT_CPUPM_EVENT_PWR_ON(x) ({ \
+	PUBLISH_EVENT_ARG(mt_cpupm_publish_pwr_on, (const void *)(x)); })
+
+#define MT_CPUPM_EVENT_PWR_OFF(x) ({ \
+	PUBLISH_EVENT_ARG(mt_cpupm_publish_pwr_off, (const void *)(x)); })
+
+#define MT_CPUPM_EVENT_AFFLV_PWR_ON(x) ({ \
+	PUBLISH_EVENT_ARG(mt_cpupm_publish_afflv_pwr_on, (const void *)(x)); })
+
+#define MT_CPUPM_EVENT_AFFLV_PWR_OFF(x) ({ \
+	PUBLISH_EVENT_ARG(mt_cpupm_publish_afflv_pwr_off, (const void *)(x)); })
+
+#else
+#define MT_CPUPM_EVENT_PWR_ON(x) ({ (void)x; })
+#define MT_CPUPM_EVENT_PWR_OFF(x) ({ (void)x; })
+#define MT_CPUPM_EVENT_AFFLV_PWR_ON(x) ({ (void)x; })
+#define MT_CPUPM_EVENT_AFFLV_PWR_OFF(x) ({ (void)x; })
+#endif
+
+/*
+ * The cpu require to cluster power stattus
+ * [0] : The cpu require cluster power down
+ * [1] : The cpu require cluster power on
+ */
+#define coordinate_cluster(onoff) write_clusterpwrdn_el1(onoff)
+#define coordinate_cluster_pwron() coordinate_cluster(1)
+#define coordinate_cluster_pwroff() coordinate_cluster(0)
+
+/* defaultly disable all functions */
+#define MTK_CPUPM_FN_MASK_DEFAULT	(0)
+
+struct mtk_cpu_pwr_ctrl {
+	unsigned int fn_mask;
+	struct mtk_cpu_pm_ops *ops;
+	struct mtk_cpu_smp_ops *smp;
+};
+
+static struct mtk_cpu_pwr_ctrl mtk_cpu_pwr = {
+	.fn_mask = MTK_CPUPM_FN_MASK_DEFAULT,
+	.ops = NULL,
+};
+
+#define IS_CPUIDLE_FN_ENABLE(x)	((mtk_cpu_pwr.ops != NULL) && ((mtk_cpu_pwr.fn_mask & x) != 0))
+#define IS_CPUSMP_FN_ENABLE(x)	((mtk_cpu_pwr.smp != NULL) && ((mtk_cpu_pwr.fn_mask & x) != 0))
+
+/* per-cpu power state */
+static unsigned int armv8_2_power_state[PLATFORM_CORE_COUNT];
+
+#define armv8_2_get_pwr_stateid(cpu) psci_get_pstate_id(armv8_2_power_state[cpu])
+
+static unsigned int get_mediatek_pstate(unsigned int domain, unsigned int psci_state,
+					struct mtk_cpupm_pwrstate *state)
+{
+	if (IS_CPUIDLE_FN_ENABLE(MTK_CPUPM_FN_CPUPM_GET_PWR_STATE)) {
+		return mtk_cpu_pwr.ops->get_pstate(domain, psci_state, state);
+	}
+
+	return 0;
+}
+
+unsigned int armv8_2_get_pwr_afflv(const psci_power_state_t *state_info)
+{
+	int i;
+
+	for (i = (int)PLAT_MAX_PWR_LVL; i >= (int)PSCI_CPU_PWR_LVL; i--) {
+		if (is_local_state_run(state_info->pwr_domain_state[i]) == 0) {
+			return (unsigned int) i;
+		}
+	}
+
+	return PSCI_INVALID_PWR_LVL;
+}
+
+/* MediaTek mcusys power on control interface */
+static void armv8_2_mcusys_pwr_on_common(const struct mtk_cpupm_pwrstate *state)
+{
+	mt_gic_init();
+	mt_gic_distif_restore();
+	gic_sgi_restore_all();
+
+	dfd_resume();
+
+	/* Add code here that behavior before system enter mcusys'on */
+	if (IS_CPUIDLE_FN_ENABLE(MTK_CPUPM_FN_RESUME_MCUSYS)) {
+		mtk_cpu_pwr.ops->mcusys_resume(state);
+	}
+}
+
+/* MediaTek mcusys power down control interface */
+static void armv8_2_mcusys_pwr_dwn_common(const struct mtk_cpupm_pwrstate *state)
+{
+	mt_gic_distif_save();
+	gic_sgi_save_all();
+
+	/* Add code here that behaves before entering mcusys off */
+	if (IS_CPUIDLE_FN_ENABLE(MTK_CPUPM_FN_SUSPEND_MCUSYS)) {
+		mtk_cpu_pwr.ops->mcusys_suspend(state);
+	}
+}
+
+/* MediaTek Cluster power on control interface */
+static void armv8_2_cluster_pwr_on_common(const struct mtk_cpupm_pwrstate *state)
+{
+	/* Add code here that behavior before system enter cluster'on */
+#if defined(MTK_CM_MGR) && !defined(MTK_FPGA_EARLY_PORTING)
+	/* init cpu stall counter */
+	init_cpu_stall_counter_all();
+#endif
+
+	if (IS_CPUIDLE_FN_ENABLE(MTK_CPUPM_FN_RESUME_CLUSTER)) {
+		mtk_cpu_pwr.ops->cluster_resume(state);
+	}
+}
+
+/* MediaTek Cluster power down control interface */
+static void armv8_2_cluster_pwr_dwn_common(const struct mtk_cpupm_pwrstate *state)
+{
+	if (IS_CPUIDLE_FN_ENABLE(MTK_CPUPM_FN_SUSPEND_CLUSTER)) {
+		mtk_cpu_pwr.ops->cluster_suspend(state);
+	}
+}
+
+/* MediaTek CPU power on control interface */
+static void armv8_2_cpu_pwr_on_common(const struct mtk_cpupm_pwrstate *state, unsigned int pstate)
+{
+	coordinate_cluster_pwron();
+
+	gicv3_rdistif_on(plat_my_core_pos());
+	gicv3_cpuif_enable(plat_my_core_pos());
+	mt_gic_rdistif_init();
+
+	/* If MCUSYS has been powered down then restore GIC redistributor for all CPUs. */
+	if (IS_PLAT_SYSTEM_RETENTION(state->pwr.afflv)) {
+		mt_gic_rdistif_restore_all();
+	} else {
+		mt_gic_rdistif_restore();
+	}
+}
+
+/* MediaTek CPU power down control interface */
+static void armv8_2_cpu_pwr_dwn_common(const struct mtk_cpupm_pwrstate *state, unsigned int pstate)
+{
+	if ((pstate & MT_CPUPM_PWR_DOMAIN_PERCORE_DSU) != 0) {
+		coordinate_cluster_pwroff();
+	}
+
+	mt_gic_rdistif_save();
+	gicv3_cpuif_disable(plat_my_core_pos());
+	gicv3_rdistif_off(plat_my_core_pos());
+}
+
+static void armv8_2_cpu_pwr_resume(const struct mtk_cpupm_pwrstate *state, unsigned int pstate)
+{
+	armv8_2_cpu_pwr_on_common(state, pstate);
+	if (IS_CPUIDLE_FN_ENABLE(MTK_CPUPM_FN_RESUME_CORE)) {
+		mtk_cpu_pwr.ops->cpu_resume(state);
+	}
+}
+
+static void armv8_2_cpu_pwr_suspend(const struct mtk_cpupm_pwrstate *state, unsigned int pstate)
+{
+	if (IS_CPUIDLE_FN_ENABLE(MTK_CPUPM_FN_SUSPEND_CORE)) {
+		mtk_cpu_pwr.ops->cpu_suspend(state);
+	}
+	armv8_2_cpu_pwr_dwn_common(state, pstate);
+}
+
+static void armv8_2_cpu_pwr_on(const struct mtk_cpupm_pwrstate *state, unsigned int pstate)
+{
+	armv8_2_cpu_pwr_on_common(state, pstate);
+
+	if (IS_CPUSMP_FN_ENABLE(MTK_CPUPM_FN_SMP_CORE_ON)) {
+		mtk_cpu_pwr.smp->cpu_on(state);
+	}
+}
+
+static void armv8_2_cpu_pwr_off(const struct mtk_cpupm_pwrstate *state, unsigned int pstate)
+{
+	if (IS_CPUSMP_FN_ENABLE(MTK_CPUPM_FN_SMP_CORE_OFF)) {
+		mtk_cpu_pwr.smp->cpu_off(state);
+	}
+	armv8_2_cpu_pwr_dwn_common(state, pstate);
+}
+
+/* MediaTek PSCI power domain */
+static int armv8_2_power_domain_on(u_register_t mpidr)
+{
+	int ret = PSCI_E_SUCCESS;
+	int cpu = plat_core_pos_by_mpidr(mpidr);
+	uintptr_t entry = plat_pm_get_warm_entry();
+
+	if (IS_CPUSMP_FN_ENABLE(MTK_CPUPM_FN_PWR_ON_CORE_PREPARE)) {
+		if (mtk_cpu_pwr.smp->cpu_pwr_on_prepare(cpu, entry) != 0) {
+			ret = PSCI_E_DENIED;
+		}
+	}
+	INFO("CPU %u power domain prepare on\n", cpu);
+	return ret;
+}
+
+/* MediaTek PSCI power domain */
+static void armv8_2_power_domain_on_finish(const psci_power_state_t *state)
+{
+	struct mt_cpupm_event_data nb;
+	unsigned int pstate = (MT_CPUPM_PWR_DOMAIN_CORE | MT_CPUPM_PWR_DOMAIN_PERCORE_DSU);
+	struct mtk_cpupm_pwrstate pm_state = {
+		.info = {
+			.cpuid = plat_my_core_pos(),
+			.mode = MTK_CPU_PM_SMP,
+		},
+		.pwr = {
+			.afflv = armv8_2_get_pwr_afflv(state),
+			.state_id = 0x0,
+		},
+	};
+
+	armv8_2_cpu_pwr_on(&pm_state, pstate);
+
+	nb.cpuid = pm_state.info.cpuid;
+	nb.pwr_domain = pstate;
+	MT_CPUPM_EVENT_PWR_ON(&nb);
+
+	INFO("CPU %u power domain on finished\n", pm_state.info.cpuid);
+}
+
+/* MediaTek PSCI power domain */
+static void armv8_2_power_domain_off(const psci_power_state_t *state)
+{
+	struct mt_cpupm_event_data nb;
+	unsigned int pstate = (MT_CPUPM_PWR_DOMAIN_CORE | MT_CPUPM_PWR_DOMAIN_PERCORE_DSU);
+	struct mtk_cpupm_pwrstate pm_state = {
+		.info = {
+			.cpuid = plat_my_core_pos(),
+			.mode = MTK_CPU_PM_SMP,
+		},
+		.pwr = {
+			.afflv = armv8_2_get_pwr_afflv(state),
+			.state_id = 0x0,
+		},
+	};
+	armv8_2_cpu_pwr_off(&pm_state, pstate);
+
+	nb.cpuid = pm_state.info.cpuid;
+	nb.pwr_domain = pstate;
+	MT_CPUPM_EVENT_PWR_OFF(&nb);
+
+	INFO("CPU %u power domain off\n", pm_state.info.cpuid);
+}
+
+/* MediaTek PSCI power domain */
+static void armv8_2_power_domain_suspend(const psci_power_state_t *state)
+{
+	unsigned int pstate = 0;
+	struct mt_cpupm_event_data nb;
+	struct mtk_cpupm_pwrstate pm_state = {
+		.info = {
+			.cpuid = plat_my_core_pos(),
+			.mode = MTK_CPU_PM_CPUIDLE,
+		},
+	};
+
+	pm_state.pwr.state_id = armv8_2_get_pwr_stateid(pm_state.info.cpuid);
+	pm_state.pwr.afflv = armv8_2_get_pwr_afflv(state);
+	pm_state.pwr.raw = state;
+
+	pstate = get_mediatek_pstate(CPUPM_PWR_OFF,
+				     armv8_2_power_state[pm_state.info.cpuid], &pm_state);
+
+	armv8_2_cpu_pwr_suspend(&pm_state, pstate);
+
+	if ((pstate & MT_CPUPM_PWR_DOMAIN_CLUSTER) != 0) {
+		armv8_2_cluster_pwr_dwn_common(&pm_state);
+	}
+
+	if ((pstate & MT_CPUPM_PWR_DOMAIN_MCUSYS) != 0) {
+		armv8_2_mcusys_pwr_dwn_common(&pm_state);
+	}
+
+	nb.cpuid = pm_state.info.cpuid;
+	nb.pwr_domain = pstate;
+	MT_CPUPM_EVENT_PWR_OFF(&nb);
+
+	if (IS_AFFLV_PUBEVENT(pstate)) {
+		MT_CPUPM_EVENT_AFFLV_PWR_OFF(&nb);
+	}
+}
+
+/* MediaTek PSCI power domain */
+static void armv8_2_power_domain_suspend_finish(const psci_power_state_t *state)
+{
+	unsigned int pstate = 0;
+	struct mt_cpupm_event_data nb;
+	struct mtk_cpupm_pwrstate pm_state = {
+		.info = {
+			.cpuid = plat_my_core_pos(),
+			.mode = MTK_CPU_PM_CPUIDLE,
+		},
+	};
+
+	pm_state.pwr.state_id = armv8_2_get_pwr_stateid(pm_state.info.cpuid);
+	pm_state.pwr.afflv = armv8_2_get_pwr_afflv(state);
+	pm_state.pwr.raw = state;
+
+	pstate = get_mediatek_pstate(CPUPM_PWR_ON,
+				     armv8_2_power_state[pm_state.info.cpuid], &pm_state);
+
+	if ((pstate & MT_CPUPM_PWR_DOMAIN_MCUSYS) != 0) {
+		armv8_2_mcusys_pwr_on_common(&pm_state);
+	}
+
+	if ((pstate & MT_CPUPM_PWR_DOMAIN_CLUSTER) != 0) {
+		armv8_2_cluster_pwr_on_common(&pm_state);
+	}
+
+	armv8_2_cpu_pwr_resume(&pm_state, pstate);
+
+	nb.cpuid = pm_state.info.cpuid;
+	nb.pwr_domain = pstate;
+	MT_CPUPM_EVENT_PWR_ON(&nb);
+
+	if (IS_AFFLV_PUBEVENT(pstate)) {
+		MT_CPUPM_EVENT_AFFLV_PWR_ON(&nb);
+	}
+}
+
+/* MediaTek PSCI power domain */
+static int armv8_2_validate_power_state(unsigned int power_state, psci_power_state_t *req_state)
+{
+	unsigned int i;
+	unsigned int pstate = psci_get_pstate_type(power_state);
+	unsigned int aff_lvl = psci_get_pstate_pwrlvl(power_state);
+	unsigned int my_core_pos = plat_my_core_pos();
+
+	if (mtk_cpu_pwr.ops == NULL) {
+		return PSCI_E_INVALID_PARAMS;
+	}
+
+	if (IS_CPUIDLE_FN_ENABLE(MTK_CPUPM_FN_PWR_STATE_VALID)) {
+		if (mtk_cpu_pwr.ops->pwr_state_valid(aff_lvl, pstate) != 0) {
+			return PSCI_E_INVALID_PARAMS;
+		}
+	}
+
+	if (pstate == PSTATE_TYPE_STANDBY) {
+		req_state->pwr_domain_state[0] = PLAT_MAX_RET_STATE;
+	} else {
+		for (i = PSCI_CPU_PWR_LVL; i <= aff_lvl; i++) {
+			req_state->pwr_domain_state[i] = PLAT_MAX_OFF_STATE;
+		}
+	}
+	armv8_2_power_state[my_core_pos] = power_state;
+
+	return PSCI_E_SUCCESS;
+}
+
+/* MediaTek PSCI power domain */
+#if CONFIG_MTK_SUPPORT_SYSTEM_SUSPEND
+static void armv8_2_get_sys_suspend_power_state(psci_power_state_t *req_state)
+{
+	unsigned int i;
+	int ret;
+	unsigned int power_state;
+	unsigned int my_core_pos = plat_my_core_pos();
+
+	ret = mtk_cpu_pwr.ops->pwr_state_valid(PLAT_MAX_PWR_LVL,
+						PSTATE_TYPE_POWERDOWN);
+
+	if (ret != MTK_CPUPM_E_OK) {
+		/* Avoid suspend due to platform is not ready. */
+		req_state->pwr_domain_state[PSCI_CPU_PWR_LVL] =
+						PLAT_MAX_RET_STATE;
+		for (i = PSCI_CPU_PWR_LVL + 1; i <= PLAT_MAX_PWR_LVL; i++) {
+			req_state->pwr_domain_state[i] = PSCI_LOCAL_STATE_RUN;
+		}
+
+		power_state = psci_make_powerstate(0, PSTATE_TYPE_STANDBY, PSCI_CPU_PWR_LVL);
+	} else {
+		for (i = PSCI_CPU_PWR_LVL; i <= PLAT_MAX_PWR_LVL; i++) {
+			req_state->pwr_domain_state[i] = PLAT_MAX_OFF_STATE;
+		}
+
+		power_state = psci_make_powerstate(MT_PLAT_PWR_STATE_SYSTEM_SUSPEND,
+						   PSTATE_TYPE_POWERDOWN, PLAT_MAX_PWR_LVL);
+	}
+
+	armv8_2_power_state[my_core_pos] = power_state;
+	flush_dcache_range((uintptr_t)&armv8_2_power_state[my_core_pos],
+			   sizeof(armv8_2_power_state[my_core_pos]));
+}
+#endif
+static void armv8_2_pm_smp_init(unsigned int cpu_id, uintptr_t entry_point)
+{
+	if (entry_point == 0) {
+		ERROR("%s, warm_entry_point is null\n", __func__);
+		panic();
+	}
+	if (IS_CPUSMP_FN_ENABLE(MTK_CPUPM_FN_SMP_INIT)) {
+		mtk_cpu_pwr.smp->init(cpu_id, entry_point);
+	}
+	INFO("[%s:%d] - Initialize finished\n", __func__, __LINE__);
+}
+
+static struct plat_pm_pwr_ctrl armv8_2_pwr_ops = {
+	.pwr_domain_suspend = armv8_2_power_domain_suspend,
+	.pwr_domain_suspend_finish = armv8_2_power_domain_suspend_finish,
+	.validate_power_state = armv8_2_validate_power_state,
+#if CONFIG_MTK_SUPPORT_SYSTEM_SUSPEND
+	.get_sys_suspend_power_state = armv8_2_get_sys_suspend_power_state,
+#endif
+};
+
+struct plat_pm_smp_ctrl armv8_2_smp_ops = {
+	.init = armv8_2_pm_smp_init,
+	.pwr_domain_on = armv8_2_power_domain_on,
+	.pwr_domain_off = armv8_2_power_domain_off,
+	.pwr_domain_on_finish = armv8_2_power_domain_on_finish,
+};
+
+#define ISSUE_CPU_PM_REG_FAIL(_success) ({ _success = false; assert(0); })
+
+#define CPM_PM_FN_CHECK(_fns, _ops, _id, _func, _result, _flag) ({ \
+	if ((_fns & _id)) { \
+		if (_ops->_func) \
+			_flag |= _id; \
+		else { \
+			ISSUE_CPU_PM_REG_FAIL(_result); \
+		} \
+	} })
+
+int register_cpu_pm_ops(unsigned int fn_flags, struct mtk_cpu_pm_ops *ops)
+{
+	bool success = true;
+	unsigned int fns = 0;
+
+	if ((ops == NULL) || (mtk_cpu_pwr.ops != NULL)) {
+		ERROR("[%s:%d] register cpu_pm fail !!\n", __FILE__, __LINE__);
+		return MTK_CPUPM_E_ERR;
+	}
+
+	CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_RESUME_CORE,
+			cpu_resume, success, fns);
+
+	CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_SUSPEND_CORE,
+			cpu_suspend, success, fns);
+
+	CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_RESUME_CLUSTER,
+			cluster_resume, success, fns);
+
+	CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_SUSPEND_CLUSTER,
+			cluster_suspend, success, fns);
+
+	CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_RESUME_MCUSYS,
+			mcusys_resume, success, fns);
+
+	CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_SUSPEND_MCUSYS,
+			mcusys_suspend, success, fns);
+
+	CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_CPUPM_GET_PWR_STATE,
+			get_pstate, success, fns);
+
+	CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_PWR_STATE_VALID,
+			pwr_state_valid, success, fns);
+
+	CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_INIT,
+			init, success, fns);
+
+	if (success) {
+		mtk_cpu_pwr.ops = ops;
+		mtk_cpu_pwr.fn_mask |= fns;
+		plat_pm_ops_setup_pwr(&armv8_2_pwr_ops);
+		INFO("[%s:%d] CPU pwr ops register success, support:0x%x\n",
+		     __func__, __LINE__, fns);
+	} else {
+		ERROR("[%s:%d] register cpu_pm ops fail !, fn:0x%x\n",
+		      __func__, __LINE__, fn_flags);
+		assert(0);
+	}
+	return MTK_CPUPM_E_OK;
+}
+
+int register_cpu_smp_ops(unsigned int fn_flags, struct mtk_cpu_smp_ops *ops)
+{
+	bool success = true;
+	unsigned int fns = 0;
+
+	if ((ops == NULL) || (mtk_cpu_pwr.smp != NULL)) {
+		ERROR("[%s:%d] register cpu_smp fail !!\n", __FILE__, __LINE__);
+		return MTK_CPUPM_E_ERR;
+	}
+
+	CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_SMP_INIT,
+			init, success, fns);
+
+	CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_PWR_ON_CORE_PREPARE,
+			cpu_pwr_on_prepare, success, fns);
+
+	CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_SMP_CORE_ON,
+			cpu_on, success, fns);
+
+	CPM_PM_FN_CHECK(fn_flags, ops, MTK_CPUPM_FN_SMP_CORE_OFF,
+			cpu_off, success, fns);
+
+	if (success == true) {
+		mtk_cpu_pwr.smp = ops;
+		mtk_cpu_pwr.fn_mask |= fns;
+		plat_pm_ops_setup_smp(&armv8_2_smp_ops);
+		INFO("[%s:%d] CPU smp ops register success, support:0x%x\n",
+		     __func__, __LINE__, fns);
+	} else {
+		ERROR("[%s:%d] register cpu_smp ops fail !, fn:0x%x\n",
+		      __func__, __LINE__, fn_flags);
+		assert(0);
+	}
+	return MTK_CPUPM_E_OK;
+}
diff --git a/plat/mediatek/lib/pm/armv8_2/rules.mk b/plat/mediatek/lib/pm/armv8_2/rules.mk
new file mode 100644
index 0000000..0e065c5
--- /dev/null
+++ b/plat/mediatek/lib/pm/armv8_2/rules.mk
@@ -0,0 +1,12 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := armv${CONFIG_MTK_PM_ARCH}
+LOCAL_SRCS-y := ${LOCAL_DIR}/pwr_ctrl.c
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/lib/pm/mtk_pm.c b/plat/mediatek/lib/pm/mtk_pm.c
index 632a1e7..3dbeb51 100644
--- a/plat/mediatek/lib/pm/mtk_pm.c
+++ b/plat/mediatek/lib/pm/mtk_pm.c
@@ -4,15 +4,117 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <lib/psci/psci.h>
+#include <assert.h>
+#include <plat/common/platform.h>
+#include <lib/pm/mtk_pm.h>
 
-static const plat_psci_ops_t plat_psci_ops = {
-};
+#define MTK_PM_ST_SMP_READY	BIT(0)
+#define MTK_PM_ST_PWR_READY	BIT(1)
+#define MTK_PM_ST_RESET_READY	BIT(2)
+
+static uintptr_t mtk_secure_entrypoint;
+static plat_init_func mtk_plat_smp_init;
+static plat_psci_ops_t mtk_pm_ops;
+static unsigned int mtk_pm_status;
+
+uintptr_t plat_pm_get_warm_entry(void)
+{
+	return mtk_secure_entrypoint;
+}
+
+int plat_pm_ops_setup_pwr(struct plat_pm_pwr_ctrl *ops)
+{
+	if (!ops) {
+		return MTK_CPUPM_E_FAIL;
+	}
+
+#if CONFIG_MTK_CPU_SUSPEND_EN
+	if (!mtk_pm_ops.pwr_domain_suspend) {
+		mtk_pm_ops.pwr_domain_suspend = ops->pwr_domain_suspend;
+	}
+
+	if (!mtk_pm_ops.pwr_domain_suspend_finish) {
+		mtk_pm_ops.pwr_domain_suspend_finish = ops->pwr_domain_suspend_finish;
+	}
+
+	if (!mtk_pm_ops.validate_power_state) {
+		mtk_pm_ops.validate_power_state = ops->validate_power_state;
+	}
+
+	if (!mtk_pm_ops.get_sys_suspend_power_state) {
+		mtk_pm_ops.get_sys_suspend_power_state = ops->get_sys_suspend_power_state;
+	}
+
+	mtk_pm_status |= MTK_PM_ST_PWR_READY;
+#endif
+	return MTK_CPUPM_E_OK;
+}
+
+int plat_pm_ops_setup_smp(struct plat_pm_smp_ctrl *ops)
+{
+	if (!ops) {
+		return MTK_CPUPM_E_FAIL;
+	}
+
+#if CONFIG_MTK_SMP_EN
+	if (!mtk_pm_ops.pwr_domain_on) {
+		mtk_pm_ops.pwr_domain_on = ops->pwr_domain_on;
+	}
+
+	if (!mtk_pm_ops.pwr_domain_on_finish) {
+		mtk_pm_ops.pwr_domain_on_finish = ops->pwr_domain_on_finish;
+	}
+
+	if (!mtk_pm_ops.pwr_domain_off) {
+		mtk_pm_ops.pwr_domain_off = ops->pwr_domain_off;
+	}
+
+	if (!mtk_plat_smp_init) {
+		mtk_plat_smp_init = ops->init;
+	}
+
+	mtk_pm_status |= MTK_PM_ST_SMP_READY;
+#endif
+	return MTK_CPUPM_E_OK;
+}
+
+int plat_pm_ops_setup_reset(struct plat_pm_reset_ctrl *ops)
+{
+	if (!ops) {
+		return MTK_CPUPM_E_FAIL;
+	}
+
+	if (!mtk_pm_ops.system_off) {
+		mtk_pm_ops.system_off = ops->system_off;
+	}
+
+	if (!mtk_pm_ops.system_reset) {
+		mtk_pm_ops.system_reset = ops->system_reset;
+	}
+
+	if (!mtk_pm_ops.system_reset2) {
+		mtk_pm_ops.system_reset2 = ops->system_reset2;
+	}
+
+	mtk_pm_status |= MTK_PM_ST_RESET_READY;
+
+	return MTK_CPUPM_E_OK;
+}
 
 int plat_setup_psci_ops(uintptr_t sec_entrypoint,
 			const plat_psci_ops_t **psci_ops)
 {
-	*psci_ops = &plat_psci_ops;
+	*psci_ops = &mtk_pm_ops;
+	mtk_secure_entrypoint = sec_entrypoint;
+
+	if (mtk_plat_smp_init) {
+		unsigned int cpu_id = plat_my_core_pos();
 
+		mtk_plat_smp_init(cpu_id, mtk_secure_entrypoint);
+	}
+	INFO("%s, smp:(%d), pwr_ctrl:(%d), system_reset:(%d)\n", __func__,
+	     !!(mtk_pm_status & MTK_PM_ST_SMP_READY),
+	     !!(mtk_pm_status & MTK_PM_ST_PWR_READY),
+	     !!(mtk_pm_status & MTK_PM_ST_RESET_READY));
 	return 0;
 }
diff --git a/plat/mediatek/lib/pm/mtk_pm.h b/plat/mediatek/lib/pm/mtk_pm.h
new file mode 100644
index 0000000..0bfb493
--- /dev/null
+++ b/plat/mediatek/lib/pm/mtk_pm.h
@@ -0,0 +1,214 @@
+/*
+ * Copyright (c) 2022, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MTK_PM_H
+#define MTK_PM_H
+#include <lib/psci/psci.h>
+
+#if MTK_PUBEVENT_ENABLE
+#include <mtk_event/mtk_pubsub_events.h>
+#endif
+
+#define MTK_CPUPM_E_OK				(0)
+#define MTK_CPUPM_E_UNKNOWN			(-1)
+#define MTK_CPUPM_E_ERR				(-2)
+#define MTK_CPUPM_E_FAIL			(-3)
+#define MTK_CPUPM_E_NOT_SUPPORT			(-4)
+
+
+#define MTK_CPUPM_FN_PWR_LOCK_AQUIRE		BIT(0)
+#define MTK_CPUPM_FN_INIT			BIT(1)
+#define MTK_CPUPM_FN_PWR_STATE_VALID		BIT(2)
+#define MTK_CPUPM_FN_PWR_ON_CORE_PREPARE	BIT(3)
+#define MTK_CPUPM_FN_SUSPEND_CORE		BIT(4)
+#define MTK_CPUPM_FN_RESUME_CORE		BIT(5)
+#define MTK_CPUPM_FN_SUSPEND_CLUSTER		BIT(6)
+#define MTK_CPUPM_FN_RESUME_CLUSTER		BIT(7)
+#define MTK_CPUPM_FN_SUSPEND_MCUSYS		BIT(8)
+#define MTK_CPUPM_FN_RESUME_MCUSYS		BIT(9)
+#define MTK_CPUPM_FN_CPUPM_GET_PWR_STATE	BIT(10)
+#define MTK_CPUPM_FN_SMP_INIT			BIT(11)
+#define MTK_CPUPM_FN_SMP_CORE_ON		BIT(12)
+#define MTK_CPUPM_FN_SMP_CORE_OFF		BIT(13)
+
+enum mtk_cpupm_pstate {
+	MTK_CPUPM_CORE_ON,
+	MTK_CPUPM_CORE_OFF,
+	MTK_CPUPM_CORE_SUSPEND,
+	MTK_CPUPM_CORE_RESUME,
+	MTK_CPUPM_CLUSTER_SUSPEND,
+	MTK_CPUPM_CLUSTER_RESUME,
+	MTK_CPUPM_MCUSYS_SUSPEND,
+	MTK_CPUPM_MCUSYS_RESUME,
+};
+
+enum mtk_cpu_pm_mode {
+	MTK_CPU_PM_CPUIDLE,
+	MTK_CPU_PM_SMP,
+};
+
+#define MT_IRQ_REMAIN_MAX	(32)
+#define MT_IRQ_REMAIN_CAT_LOG	BIT(31)
+
+struct mt_irqremain {
+	unsigned int count;
+	unsigned int irqs[MT_IRQ_REMAIN_MAX];
+	unsigned int wakeupsrc_cat[MT_IRQ_REMAIN_MAX];
+	unsigned int wakeupsrc[MT_IRQ_REMAIN_MAX];
+};
+
+typedef void (*plat_init_func)(unsigned int, uintptr_t);
+
+struct plat_pm_smp_ctrl {
+	plat_init_func init;
+	int (*pwr_domain_on)(u_register_t mpidr);
+	void (*pwr_domain_off)(const psci_power_state_t *target_state);
+	void (*pwr_domain_on_finish)(const psci_power_state_t *target_state);
+};
+
+struct plat_pm_pwr_ctrl {
+	void (*pwr_domain_suspend)(const psci_power_state_t *target_state);
+	void (*pwr_domain_on_finish_late)(const psci_power_state_t *target_state);
+	void (*pwr_domain_suspend_finish)(const psci_power_state_t *target_state);
+	int (*validate_power_state)(unsigned int power_state, psci_power_state_t *req_state);
+	void (*get_sys_suspend_power_state)(psci_power_state_t *req_state);
+};
+
+struct plat_pm_reset_ctrl {
+	__dead2 void (*system_off)();
+	__dead2 void (*system_reset)();
+	int (*system_reset2)(int is_vendor, int reset_type, u_register_t cookie);
+};
+
+struct mtk_cpu_pm_info {
+	unsigned int cpuid;
+	unsigned int mode;
+};
+
+struct mtk_cpu_pm_state {
+	unsigned int afflv;
+	unsigned int state_id;
+	const psci_power_state_t *raw;
+};
+
+struct mtk_cpupm_pwrstate {
+	struct mtk_cpu_pm_info info;
+	struct mtk_cpu_pm_state pwr;
+};
+
+struct mtk_cpu_smp_ops {
+	void (*init)(unsigned int cpu, uintptr_t sec_entrypoint);
+	int (*cpu_pwr_on_prepare)(unsigned int cpu, uintptr_t entry);
+	void (*cpu_on)(const struct mtk_cpupm_pwrstate *state);
+	void (*cpu_off)(const struct mtk_cpupm_pwrstate *state);
+	int (*invoke)(unsigned int funcID, void *priv);
+};
+
+#define MT_CPUPM_PWR_DOMAIN_CORE		BIT(0)
+#define MT_CPUPM_PWR_DOMAIN_PERCORE_DSU		BIT(1)
+#define MT_CPUPM_PWR_DOMAIN_PERCORE_DSU_MEM	BIT(2)
+#define MT_CPUPM_PWR_DOMAIN_CLUSTER		BIT(3)
+#define MT_CPUPM_PWR_DOMAIN_MCUSYS		BIT(4)
+#define MT_CPUPM_PWR_DOMAIN_SUSPEND		BIT(5)
+
+enum mt_cpupm_pwr_domain {
+	CPUPM_PWR_ON,
+	CPUPM_PWR_OFF,
+};
+
+typedef	unsigned int mtk_pstate_type;
+
+struct mtk_cpu_pm_ops {
+	void (*init)(unsigned int cpu, uintptr_t sec_entrypoint);
+	unsigned int (*get_pstate)(enum mt_cpupm_pwr_domain domain,
+				   const mtk_pstate_type psci_state,
+				   const struct mtk_cpupm_pwrstate *state);
+	int (*pwr_state_valid)(unsigned int afflv, unsigned int state);
+	void (*cpu_suspend)(const struct mtk_cpupm_pwrstate *state);
+	void (*cpu_resume)(const struct mtk_cpupm_pwrstate *state);
+	void (*cluster_suspend)(const struct mtk_cpupm_pwrstate *state);
+	void (*cluster_resume)(const struct mtk_cpupm_pwrstate *state);
+	void (*mcusys_suspend)(const struct mtk_cpupm_pwrstate *state);
+	void (*mcusys_resume)(const struct mtk_cpupm_pwrstate *state);
+	int (*invoke)(unsigned int funcID, void *priv);
+};
+
+int register_cpu_pm_ops(unsigned int fn_flags, struct mtk_cpu_pm_ops *ops);
+int register_cpu_smp_ops(unsigned int fn_flags, struct mtk_cpu_smp_ops *ops);
+
+struct mt_cpupm_event_data {
+	unsigned int cpuid;
+	unsigned int pwr_domain;
+};
+
+/* Extension event for platform driver */
+#if MTK_PUBEVENT_ENABLE
+/* [PUB_EVENT] Core power on */
+#define MT_CPUPM_SUBCRIBE_EVENT_PWR_ON(_fn) \
+	SUBSCRIBE_TO_EVENT(mt_cpupm_publish_pwr_on, _fn)
+
+/* [PUB_EVENT] Core power off */
+#define MT_CPUPM_SUBCRIBE_EVENT_PWR_OFF(_fn) \
+	SUBSCRIBE_TO_EVENT(mt_cpupm_publish_pwr_off, _fn)
+
+/* [PUB_EVENT] Cluster power on */
+#define MT_CPUPM_SUBCRIBE_CLUSTER_PWR_ON(_fn) \
+	SUBSCRIBE_TO_EVENT(mt_cpupm_publish_afflv_pwr_on, _fn)
+
+/* [PUB_EVENT] Cluster power off */
+#define MT_CPUPM_SUBCRIBE_CLUSTER_PWR_OFF(_fn) \
+	SUBSCRIBE_TO_EVENT(mt_cpupm_publish_afflv_pwr_off, _fn)
+
+/* [PUB_EVENT] Mcusys power on */
+#define MT_CPUPM_SUBCRIBE_MCUSYS_PWR_ON(_fn) \
+	SUBSCRIBE_TO_EVENT(mt_cpupm_publish_afflv_pwr_on, _fn)
+
+/* [PUB_EVENT] Mcusys power off */
+#define MT_CPUPM_SUBCRIBE_MCUSYS_PWR_OFF(_fn) \
+	SUBSCRIBE_TO_EVENT(mt_cpupm_publish_afflv_pwr_off, _fn)
+
+#else
+#define MT_CPUPM_SUBCRIBE_EVENT_PWR_ON(_fn)
+#define MT_CPUPM_SUBCRIBE_EVENT_PWR_OFF(_fn)
+#define MT_CPUPM_SUBCRIBE_CLUSTER_PWR_ON(_fn)
+#define MT_CPUPM_SUBCRIBE_CLUSTER_PWR_OFF(_fn)
+#define MT_CPUPM_SUBCRIBE_MCUSYS_PWR_ON(_fn)
+#define MT_CPUPM_SUBCRIBE_MCUSYS_PWR_OFF(_fn)
+#endif
+
+#define MT_PLAT_PWR_STATE_L_CPU				(0x0001)
+#define MT_PLAT_PWR_STATE_B_CPU				(0x0002)
+#define MT_PLAT_PWR_STATE_L_CLUSTER			(0x0101)
+#define MT_PLAT_PWR_STATE_B_CLUSTER			(0x0102)
+#define MT_PLAT_PWR_STATE_MCUSYS			(0x0701)
+#define MT_PLAT_PWR_STATE_SYSTEM_MEM			(0x0f01)
+#define MT_PLAT_PWR_STATE_SYSTEM_PLL			(0x0f02)
+#define MT_PLAT_PWR_STATE_SYSTEM_BUS			(0x0f03)
+#define MT_PLAT_PWR_STATE_SUSPEND2IDLE			(0x1f01)
+#define MT_PLAT_PWR_STATE_SYSTEM_SUSPEND		(0x1f02)
+
+#define IS_MT_PLAT_PWR_STATE_MCUSYS(state)		(state & 0x400)
+#define IS_MT_PLAT_PWR_STATE_SYSTEM(state)		(state & 0x800)
+#define IS_MT_PLAT_PWR_STATE_PLATFORM(state)		(state & 0x1800)
+
+#define PLAT_MT_SYSTEM_SUSPEND		PLAT_MAX_OFF_STATE
+#define PLAT_MT_CPU_SUSPEND_CLUSTER	PLAT_MAX_RET_STATE
+
+#define IS_PLAT_SYSTEM_SUSPEND(aff)	(aff == PLAT_MT_SYSTEM_SUSPEND)
+#define IS_PLAT_SYSTEM_RETENTION(aff)	(aff >= PLAT_MAX_RET_STATE)
+
+#define IS_PLAT_SUSPEND2IDLE_ID(stateid) (stateid == MT_PLAT_PWR_STATE_SUSPEND2IDLE)
+
+#define IS_PLAT_SUSPEND_ID(stateid) ((stateid == MT_PLAT_PWR_STATE_SUSPEND2IDLE) || \
+				     (stateid == MT_PLAT_PWR_STATE_SYSTEM_SUSPEND))
+
+
+int plat_pm_ops_setup_pwr(struct plat_pm_pwr_ctrl *ops);
+int plat_pm_ops_setup_reset(struct plat_pm_reset_ctrl *ops);
+int plat_pm_ops_setup_smp(struct plat_pm_smp_ctrl *ops);
+uintptr_t plat_pm_get_warm_entry(void);
+
+#endif
diff --git a/plat/mediatek/lib/pm/rules.mk b/plat/mediatek/lib/pm/rules.mk
index 77d0408..29265c4 100644
--- a/plat/mediatek/lib/pm/rules.mk
+++ b/plat/mediatek/lib/pm/rules.mk
@@ -12,3 +12,6 @@
 LOCAL_SRCS-y := ${LOCAL_DIR}/mtk_pm.c
 
 $(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
+
+SUB_RULES-$(CONFIG_MTK_PM_SUPPORT) := $(LOCAL_DIR)/armv${CONFIG_MTK_PM_ARCH}
+$(eval $(call INCLUDE_MAKEFILE,$(SUB_RULES-y)))
diff --git a/plat/mediatek/lib/system_reset/reset_cros.c b/plat/mediatek/lib/system_reset/reset_cros.c
new file mode 100644
index 0000000..40e68ba
--- /dev/null
+++ b/plat/mediatek/lib/system_reset/reset_cros.c
@@ -0,0 +1,56 @@
+/*
+ * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/gpio.h>
+#include <lib/mtk_init/mtk_init.h>
+#include <lib/pm/mtk_pm.h>
+#include <plat_params.h>
+#include <pmic.h>
+#include <rtc.h>
+
+static void __dead2 mtk_system_reset_cros(void)
+{
+	struct bl_aux_gpio_info *gpio_reset = plat_get_mtk_gpio_reset();
+
+	INFO("MTK System Reset\n");
+
+	gpio_set_value(gpio_reset->index, gpio_reset->polarity);
+
+	wfi();
+	ERROR("MTK System Reset: operation not handled.\n");
+	panic();
+}
+
+static void __dead2 mtk_system_off_cros(void)
+{
+	INFO("MTK System Off\n");
+
+	rtc_power_off_sequence();
+	pmic_power_off();
+
+	wfi();
+	ERROR("MTK System Off: operation not handled.\n");
+	panic();
+}
+
+static struct plat_pm_reset_ctrl lib_reset_ctrl = {
+	.system_off = mtk_system_off_cros,
+	.system_reset = mtk_system_reset_cros,
+	.system_reset2 = NULL,
+};
+
+static int lib_reset_ctrl_init(void)
+{
+	INFO("Reset init\n");
+
+	plat_pm_ops_setup_reset(&lib_reset_ctrl);
+
+	return 0;
+}
+MTK_ARCH_INIT(lib_reset_ctrl_init);
diff --git a/plat/mediatek/lib/system_reset/rules.mk b/plat/mediatek/lib/system_reset/rules.mk
new file mode 100644
index 0000000..4f20663
--- /dev/null
+++ b/plat/mediatek/lib/system_reset/rules.mk
@@ -0,0 +1,14 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := system_reset
+
+LOCAL_SRCS-y := ${LOCAL_DIR}/reset_cros.c
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/mt8186/drivers/emi_mpu/emi_mpu.c b/plat/mediatek/mt8186/drivers/emi_mpu/emi_mpu.c
index 989ecf1..a183483 100644
--- a/plat/mediatek/mt8186/drivers/emi_mpu/emi_mpu.c
+++ b/plat/mediatek/mt8186/drivers/emi_mpu/emi_mpu.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -96,5 +96,38 @@
 
 void emi_mpu_init(void)
 {
-	/* TODO: more setting for EMI MPU. */
+	struct emi_region_info_t region_info;
+
+	/* SCP DRAM */
+	region_info.start = 0x50000000ULL;
+	region_info.end = 0x5109FFFFULL;
+	region_info.region = 2;
+	SET_ACCESS_PERMISSION(region_info.apc, 1,
+			      FORBIDDEN, FORBIDDEN, FORBIDDEN, FORBIDDEN,
+			      FORBIDDEN, FORBIDDEN, FORBIDDEN, FORBIDDEN,
+			      FORBIDDEN, FORBIDDEN, FORBIDDEN, FORBIDDEN,
+			      NO_PROTECTION, FORBIDDEN, FORBIDDEN, NO_PROTECTION);
+	emi_mpu_set_protection(&region_info);
+
+	/* DSP protect address */
+	region_info.start = 0x60000000ULL;	/* dram base addr */
+	region_info.end = 0x610FFFFFULL;
+	region_info.region = 3;
+	SET_ACCESS_PERMISSION(region_info.apc, 1,
+			      FORBIDDEN, FORBIDDEN, FORBIDDEN, FORBIDDEN,
+			      FORBIDDEN, FORBIDDEN, FORBIDDEN, FORBIDDEN,
+			      FORBIDDEN, FORBIDDEN, FORBIDDEN, NO_PROTECTION,
+			      FORBIDDEN, FORBIDDEN, FORBIDDEN, NO_PROTECTION);
+	emi_mpu_set_protection(&region_info);
+
+	/* Forbidden All */
+	region_info.start = 0x40000000ULL;	/* dram base addr */
+	region_info.end = 0x1FFFF0000ULL;
+	region_info.region = 4;
+	SET_ACCESS_PERMISSION(region_info.apc, 1,
+			      FORBIDDEN, FORBIDDEN, FORBIDDEN, FORBIDDEN,
+			      FORBIDDEN, FORBIDDEN, FORBIDDEN, FORBIDDEN,
+			      FORBIDDEN, FORBIDDEN, FORBIDDEN, FORBIDDEN,
+			      FORBIDDEN, FORBIDDEN, FORBIDDEN, NO_PROTECTION);
+	emi_mpu_set_protection(&region_info);
 }
diff --git a/plat/mediatek/mt8188/aarch64/plat_helpers.S b/plat/mediatek/mt8188/aarch64/plat_helpers.S
deleted file mode 100644
index 7073ab1..0000000
--- a/plat/mediatek/mt8188/aarch64/plat_helpers.S
+++ /dev/null
@@ -1,45 +0,0 @@
-/*
- * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <arch.h>
-#include <asm_macros.S>
-#include <platform_def.h>
-
-	.globl plat_is_my_cpu_primary
-	.globl plat_my_core_pos
-	.globl plat_mediatek_calc_core_pos
-
-func plat_is_my_cpu_primary
-	mrs	x0, mpidr_el1
-	and	x0, x0, #(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK)
-	cmp	x0, #PLAT_PRIMARY_CPU
-	cset	x0, eq
-	ret
-endfunc plat_is_my_cpu_primary
-
-	/* -----------------------------------------------------
-	 *  unsigned int plat_my_core_pos(void)
-	 *  This function uses the plat_mediatek_calc_core_pos()
-	 *  definition to get the index of the calling CPU.
-	 * -----------------------------------------------------
-	 */
-func plat_my_core_pos
-	mrs	x0, mpidr_el1
-	b	plat_mediatek_calc_core_pos
-endfunc plat_my_core_pos
-
-	/* -----------------------------------------------------
-	 * unsigned int plat_mediatek_calc_core_pos(u_register_t mpidr);
-	 *
-	 * With this function: CorePos = CoreID (AFF1)
-	 * we do it with x0 = (x0 >> 8) & 0xff
-	 * -----------------------------------------------------
-	 */
-func plat_mediatek_calc_core_pos
-	mov	x1, #MPIDR_AFFLVL_MASK
-	and	x0, x1, x0, lsr #MPIDR_AFF1_SHIFT
-	ret
-endfunc plat_mediatek_calc_core_pos
diff --git a/plat/mediatek/include/mt8188/platform_def.h b/plat/mediatek/mt8188/include/platform_def.h
similarity index 86%
rename from plat/mediatek/include/mt8188/platform_def.h
rename to plat/mediatek/mt8188/include/platform_def.h
index 962d952..499c4f4 100644
--- a/plat/mediatek/include/mt8188/platform_def.h
+++ b/plat/mediatek/mt8188/include/platform_def.h
@@ -7,10 +7,13 @@
 #ifndef PLATFORM_DEF_H
 #define PLATFORM_DEF_H
 
+#include <arch_def.h>
+
 #define PLAT_PRIMARY_CPU	(0x0)
 
 #define MT_GIC_BASE		(0x0C000000)
 #define MCUCFG_BASE		(0x0C530000)
+#define MCUCFG_REG_SIZE		(0x10000)
 #define IO_PHYS			(0x10000000)
 
 /* Aggregate of all devices for MMU mapping */
@@ -23,6 +26,8 @@
  * GPIO related constants
  ******************************************************************************/
 #define GPIO_BASE		(IO_PHYS + 0x00005000)
+#define RGU_BASE		(IO_PHYS + 0x00007000)
+#define DRM_BASE		(IO_PHYS + 0x0000D000)
 #define IOCFG_RM_BASE		(IO_PHYS + 0x01C00000)
 #define IOCFG_LT_BASE		(IO_PHYS + 0x01E10000)
 #define IOCFG_LM_BASE		(IO_PHYS + 0x01E20000)
@@ -101,6 +106,12 @@
 #define DP_SEC_SIZE		(0x1000)
 
 /*******************************************************************************
+ * EMI MPU related constants
+ *******************************************************************************/
+#define EMI_MPU_BASE		(IO_PHYS + 0x00226000)
+#define SUB_EMI_MPU_BASE	(IO_PHYS + 0x00225000)
+
+/*******************************************************************************
  * System counter frequency related constants
  ******************************************************************************/
 #define SYS_COUNTER_FREQ_IN_HZ	(13000000)
@@ -116,22 +127,7 @@
  * Generic platform constants
  ******************************************************************************/
 #define PLATFORM_STACK_SIZE		(0x800)
-
 #define FIRMWARE_WELCOME_STR		"Booting Trusted Firmware\n"
-
-#define PLAT_MAX_PWR_LVL		U(3)
-#define PLAT_MAX_RET_STATE		U(1)
-#define PLAT_MAX_OFF_STATE		U(9)
-
-#define PLATFORM_SYSTEM_COUNT		U(1)
-#define PLATFORM_MCUSYS_COUNT		U(1)
-#define PLATFORM_CLUSTER_COUNT		U(1)
-#define PLATFORM_CLUSTER0_CORE_COUNT	U(8)
-#define PLATFORM_CLUSTER1_CORE_COUNT	U(0)
-
-#define PLATFORM_CORE_COUNT		(PLATFORM_CLUSTER0_CORE_COUNT)
-#define PLATFORM_MAX_CPUS_PER_CLUSTER	U(8)
-
 #define SOC_CHIP_ID			U(0x8188)
 
 /*******************************************************************************
@@ -159,16 +155,4 @@
 #define MAX_XLAT_TABLES			(16)
 #define MAX_MMAP_REGIONS		(16)
 
-/*******************************************************************************
- * Declarations and constants to access the mailboxes safely. Each mailbox is
- * aligned on the biggest cache line size in the platform. This is known only
- * to the platform as it might have a combination of integrated and external
- * caches. Such alignment ensures that two maiboxes do not sit on the same cache
- * line at any cache level. They could belong to different cpus/clusters &
- * get written while being protected by different locks causing corruption of
- * a valid mailbox address.
- ******************************************************************************/
-#define CACHE_WRITEBACK_SHIFT		(6)
-#define CACHE_WRITEBACK_GRANULE		(1 << CACHE_WRITEBACK_SHIFT)
-
 #endif /* PLATFORM_DEF_H */
diff --git a/plat/mediatek/mt8188/plat_config.mk b/plat/mediatek/mt8188/plat_config.mk
index 0c1e7cd..ff4fe1d 100644
--- a/plat/mediatek/mt8188/plat_config.mk
+++ b/plat/mediatek/mt8188/plat_config.mk
@@ -29,6 +29,12 @@
 ERRATA_A78_1821534 := 1
 ERRATA_A78_2132060 := 1
 ERRATA_A78_2242635 := 1
+ERRATA_A78_2376745 := 1
+ERRATA_A78_2395406 := 1
+
+CONFIG_ARCH_ARM_V8_2 := y
+CONFIG_MTK_MCUSYS := y
+MCUSYS_VERSION := v1
 
 MACH_MT8188 := 1
 $(eval $(call add_define,MACH_MT8188))
diff --git a/plat/mediatek/mt8188/plat_topology.c b/plat/mediatek/mt8188/plat_topology.c
deleted file mode 100644
index 9fa2855..0000000
--- a/plat/mediatek/mt8188/plat_topology.c
+++ /dev/null
@@ -1,70 +0,0 @@
-/*
- * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <arch.h>
-#include <arch_helpers.h>
-#include <lib/psci/psci.h>
-
-#include <plat_helpers.h>
-#include <platform_def.h>
-
-const unsigned char mtk_power_domain_tree_desc[] = {
-	/* Number of root nodes */
-	PLATFORM_SYSTEM_COUNT,
-	/* Number of children for the root node */
-	PLATFORM_MCUSYS_COUNT,
-	/* Number of children for the mcusys node */
-	PLATFORM_CLUSTER_COUNT,
-	/* Number of children for the first cluster node */
-	PLATFORM_CLUSTER0_CORE_COUNT,
-};
-
-const unsigned char *plat_get_power_domain_tree_desc(void)
-{
-	return mtk_power_domain_tree_desc;
-}
-
-/*******************************************************************************
- * This function implements a part of the critical interface between the psci
- * generic layer and the platform that allows the former to query the platform
- * to convert an MPIDR to a unique linear index. An error code (-1) is returned
- * in case the MPIDR is invalid.
- ******************************************************************************/
-int plat_core_pos_by_mpidr(u_register_t mpidr)
-{
-	unsigned int cluster_id, cpu_id;
-
-	if ((read_mpidr() & MPIDR_MT_MASK) != 0) {
-		/* ARMv8.2 arch */
-		if ((mpidr & (MPIDR_AFFLVL_MASK << MPIDR_AFF0_SHIFT)) != 0) {
-			return -1;
-		}
-		return plat_mediatek_calc_core_pos(mpidr);
-	}
-
-	mpidr &= MPIDR_AFFINITY_MASK;
-
-	if ((mpidr & ~(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK)) != 0) {
-		return -1;
-	}
-
-	cluster_id = (mpidr >> MPIDR_AFF1_SHIFT) & MPIDR_AFFLVL_MASK;
-	cpu_id = (mpidr >> MPIDR_AFF0_SHIFT) & MPIDR_AFFLVL_MASK;
-
-	if (cluster_id >= PLATFORM_CLUSTER_COUNT) {
-		return -1;
-	}
-
-	/*
-	 * Validate cpu_id by checking whether it represents a CPU in
-	 * one of the two clusters present on the platform.
-	 */
-	if (cpu_id >= PLATFORM_MAX_CPUS_PER_CLUSTER) {
-		return -1;
-	}
-
-	return (cpu_id + (cluster_id * 8));
-}
diff --git a/plat/mediatek/mt8188/platform.mk b/plat/mediatek/mt8188/platform.mk
index 84bcfac..99ba43f 100644
--- a/plat/mediatek/mt8188/platform.mk
+++ b/plat/mediatek/mt8188/platform.mk
@@ -14,7 +14,7 @@
 
 PLAT_INCLUDES := -I${MTK_PLAT}/common \
 		 -I${MTK_PLAT}/include \
-		 -I${MTK_PLAT}/include/${MTK_SOC} \
+		 -I${MTK_PLAT}/include/${ARCH_VERSION} \
 		 -I${MTK_PLAT} \
 		 -I${MTK_PLAT_SOC}/include \
 		 -Idrivers/arm/gic \
@@ -22,15 +22,23 @@
 MODULES-y += $(MTK_PLAT)/common
 MODULES-y += $(MTK_PLAT)/lib/mtk_init
 MODULES-y += $(MTK_PLAT)/lib/pm
+MODULES-y += $(MTK_PLAT)/lib/system_reset
 MODULES-y += $(MTK_PLAT)/drivers/cirq
+MODULES-y += $(MTK_PLAT)/drivers/dcm
+MODULES-y += $(MTK_PLAT)/drivers/dfd
 MODULES-y += $(MTK_PLAT)/drivers/dp
+MODULES-y += $(MTK_PLAT)/drivers/emi_mpu
 MODULES-y += $(MTK_PLAT)/drivers/gic600
 MODULES-y += $(MTK_PLAT)/drivers/gpio
 MODULES-y += $(MTK_PLAT)/drivers/iommu
+MODULES-y += $(MTK_PLAT)/drivers/lpm
+MODULES-y += $(MTK_PLAT)/drivers/mcusys
 MODULES-y += $(MTK_PLAT)/drivers/pmic
 MODULES-y += $(MTK_PLAT)/drivers/pmic_wrap
 MODULES-y += $(MTK_PLAT)/drivers/rtc
 MODULES-y += $(MTK_PLAT)/drivers/timer
+MODULES-y += $(MTK_PLAT)/helpers
+MODULES-y += $(MTK_PLAT)/topology
 
 PLAT_BL_COMMON_SOURCES := common/desc_image_load.c \
 			  drivers/ti/uart/aarch64/16550_console.S \
@@ -47,9 +55,7 @@
 		plat/common/aarch64/crash_console_helpers.S \
 		${MTK_PLAT}/common/mtk_plat_common.c \
 		${MTK_PLAT}/common/params_setup.c \
-		${MTK_PLAT_SOC}/aarch64/plat_helpers.S \
-		$(MTK_PLAT)/$(MTK_SOC)/plat_mmap.c \
-		$(MTK_PLAT)/$(MTK_SOC)/plat_topology.c
+		$(MTK_PLAT)/$(MTK_SOC)/plat_mmap.c
 
 include plat/mediatek/build_helpers/mtk_build_helpers_epilogue.mk
 
diff --git a/plat/mediatek/topology/armv8_2/topology.c b/plat/mediatek/topology/armv8_2/topology.c
new file mode 100644
index 0000000..1627bbd
--- /dev/null
+++ b/plat/mediatek/topology/armv8_2/topology.c
@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2022, Mediatek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <lib/psci/psci.h>
+#include <platform_def.h>
+
+#pragma weak plat_get_power_domain_tree_desc
+
+static const unsigned char mtk_power_domain_tree_desc[] = {
+	/* Number of root nodes */
+	PLATFORM_SYSTEM_COUNT,
+	/* Number of children for the root node */
+	PLATFORM_CLUSTER_COUNT,
+	/* Number of children for the first cluster node */
+	PLATFORM_CLUSTER0_CORE_COUNT,
+};
+
+/*******************************************************************************
+ * This function returns the default topology tree information.
+ ******************************************************************************/
+const unsigned char *plat_get_power_domain_tree_desc(void)
+{
+	return mtk_power_domain_tree_desc;
+}
diff --git a/plat/mediatek/topology/rules.mk b/plat/mediatek/topology/rules.mk
new file mode 100644
index 0000000..29f15bb
--- /dev/null
+++ b/plat/mediatek/topology/rules.mk
@@ -0,0 +1,12 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := topology
+LOCAL_SRCS-y := $(LOCAL_DIR)/$(ARCH_VERSION)/topology.c
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/nxp/common/nv_storage/plat_nv_storage.c b/plat/nxp/common/nv_storage/plat_nv_storage.c
index 7ec4fdb..af3b966 100644
--- a/plat/nxp/common/nv_storage/plat_nv_storage.c
+++ b/plat/nxp/common/nv_storage/plat_nv_storage.c
@@ -13,6 +13,7 @@
 #include <stdlib.h>
 #include <string.h>
 
+#include <platform_def.h>
 #include <common/debug.h>
 #ifndef NXP_COINED_BB
 #include <flash_info.h>
diff --git a/plat/nxp/soc-ls1046a/ls1046aqds/ddr_init.c b/plat/nxp/soc-ls1046a/ls1046aqds/ddr_init.c
index 6d1707c..89c9c0a 100644
--- a/plat/nxp/soc-ls1046a/ls1046aqds/ddr_init.c
+++ b/plat/nxp/soc-ls1046a/ls1046aqds/ddr_init.c
@@ -12,6 +12,7 @@
 #include <lib/utils.h>
 
 #include <errata.h>
+#include "platform_def.h"
 
 static const struct rc_timing rce[] = {
 	{U(1600), U(8), U(7)},
diff --git a/plat/qemu/qemu/qemu_helpers.c b/plat/qemu/qemu/qemu_helpers.c
index 01b8249..1b31ab5 100644
--- a/plat/qemu/qemu/qemu_helpers.c
+++ b/plat/qemu/qemu/qemu_helpers.c
@@ -6,10 +6,12 @@
 
 #include <assert.h>
 
+#include <common/bl_common.h>
 #if MEASURED_BOOT
 #include <common/desc_image_load.h>
 #endif
 #include <common/fdt_wrappers.h>
+#include <platform_def.h>
 
 #include <libfdt.h>
 
diff --git a/plat/socionext/synquacer/include/platform_def.h b/plat/socionext/synquacer/include/platform_def.h
index a84660b..d6bfe42 100644
--- a/plat/socionext/synquacer/include/platform_def.h
+++ b/plat/socionext/synquacer/include/platform_def.h
@@ -82,7 +82,7 @@
 
 /* Alternative BL33 */
 #define PLAT_SQ_BL33_BASE		0xe0000000
-#define PLAT_SQ_BL33_SIZE		0x00100000
+#define PLAT_SQ_BL33_SIZE		0x00200000
 
 /* FWU FIP IO base */
 #define PLAT_SQ_FIP_IOBASE		0x08600000
diff --git a/plat/st/stm32mp1/stm32mp1_pm.c b/plat/st/stm32mp1/stm32mp1_pm.c
index 3892151..6e438c4 100644
--- a/plat/st/stm32mp1/stm32mp1_pm.c
+++ b/plat/st/stm32mp1/stm32mp1_pm.c
@@ -87,7 +87,7 @@
 	clk_disable(RTCAPB);
 
 	/* Generate an IT to core 1 */
-	gicv2_raise_sgi(ARM_IRQ_SEC_SGI_0, STM32MP_SECONDARY_CPU);
+	gicv2_raise_sgi(ARM_IRQ_SEC_SGI_0, false, STM32MP_SECONDARY_CPU);
 
 	return PSCI_E_SUCCESS;
 }
diff --git a/plat/xilinx/common/include/ipi.h b/plat/xilinx/common/include/ipi.h
index ac76bf0..1d62f3e 100644
--- a/plat/xilinx/common/include/ipi.h
+++ b/plat/xilinx/common/include/ipi.h
@@ -14,20 +14,20 @@
 /*********************************************************************
  * IPI mailbox status macros
  ********************************************************************/
-#define IPI_MB_STATUS_IDLE		0
-#define IPI_MB_STATUS_SEND_PENDING	1
-#define IPI_MB_STATUS_RECV_PENDING	2
+#define IPI_MB_STATUS_IDLE		(0U)
+#define IPI_MB_STATUS_SEND_PENDING	(1U)
+#define IPI_MB_STATUS_RECV_PENDING	(2U)
 
 /*********************************************************************
  * IPI mailbox call is secure or not macros
  ********************************************************************/
-#define IPI_MB_CALL_NOTSECURE	0
-#define IPI_MB_CALL_SECURE	1
+#define IPI_MB_CALL_NOTSECURE	(0U)
+#define IPI_MB_CALL_SECURE	(1U)
 
 /*********************************************************************
  * IPI secure check
  ********************************************************************/
-#define IPI_SECURE_MASK  0x1U
+#define IPI_SECURE_MASK  (0x1U)
 #define IPI_IS_SECURE(I) ((ipi_table[(I)].secure_only & \
 			   IPI_SECURE_MASK) ? 1 : 0)
 
diff --git a/plat/xilinx/common/include/plat_startup.h b/plat/xilinx/common/include/plat_startup.h
index 5ccb774..1733930 100644
--- a/plat/xilinx/common/include/plat_startup.h
+++ b/plat/xilinx/common/include/plat_startup.h
@@ -7,6 +7,8 @@
 #ifndef PLAT_STARTUP_H
 #define PLAT_STARTUP_H
 
+#include <common/bl_common.h>
+
 /* For FSBL handover */
 enum fsbl_handoff {
 	FSBL_HANDOFF_SUCCESS = 0,
diff --git a/plat/xilinx/common/include/pm_common.h b/plat/xilinx/common/include/pm_common.h
index 06efa4b..89626e5 100644
--- a/plat/xilinx/common/include/pm_common.h
+++ b/plat/xilinx/common/include/pm_common.h
@@ -27,6 +27,11 @@
 #endif
 #define PAYLOAD_ARG_SIZE	4U	/* size in bytes */
 
+#define TZ_VERSION_MAJOR	1
+#define TZ_VERSION_MINOR	0
+#define TZ_VERSION		((TZ_VERSION_MAJOR << 16) | \
+				 TZ_VERSION_MINOR)
+
 /**
  * pm_ipi - struct for capturing IPI-channel specific info
  * @local_ipi_id	Local IPI agent ID
diff --git a/plat/xilinx/common/ipi.c b/plat/xilinx/common/ipi.c
index 318079b..6438896 100644
--- a/plat/xilinx/common/ipi.c
+++ b/plat/xilinx/common/ipi.c
@@ -141,7 +141,7 @@
  */
 int ipi_mb_enquire_status(uint32_t local, uint32_t remote)
 {
-	int ret = 0;
+	int ret = 0U;
 	uint32_t status;
 
 	status = mmio_read_32(IPI_REG_BASE(local) + IPI_OBR_OFFSET);
diff --git a/plat/xilinx/common/pm_service/pm_ipi.c b/plat/xilinx/common/pm_service/pm_ipi.c
index 9c9fd62..a0403cf 100644
--- a/plat/xilinx/common/pm_service/pm_ipi.c
+++ b/plat/xilinx/common/pm_service/pm_ipi.c
@@ -18,7 +18,7 @@
 #include "pm_defs.h"
 #include "pm_ipi.h"
 
-#define ERROR_CODE_MASK		0xFFFFU
+#define ERROR_CODE_MASK		(0xFFFFU)
 
 DEFINE_BAKERY_LOCK(pm_secure_lock);
 
diff --git a/plat/xilinx/versal/include/plat_pm_common.h b/plat/xilinx/versal/include/plat_pm_common.h
index fb4812d..4c057b8 100644
--- a/plat/xilinx/versal/include/plat_pm_common.h
+++ b/plat/xilinx/versal/include/plat_pm_common.h
@@ -19,8 +19,4 @@
 #define NON_SECURE_FLAG		1U
 #define SECURE_FLAG		0U
 
-#define VERSAL_TZ_VERSION_MAJOR		1U
-#define VERSAL_TZ_VERSION_MINOR		0U
-#define VERSAL_TZ_VERSION		((VERSAL_TZ_VERSION_MAJOR << 16U) | \
-					VERSAL_TZ_VERSION_MINOR)
 #endif /* PLAT_PM_COMMON_H */
diff --git a/plat/xilinx/versal/pm_service/pm_svc_main.c b/plat/xilinx/versal/pm_service/pm_svc_main.c
index 929008a..48888e4 100644
--- a/plat/xilinx/versal/pm_service/pm_svc_main.c
+++ b/plat/xilinx/versal/pm_service/pm_svc_main.c
@@ -289,7 +289,7 @@
 
 	case PM_GET_TRUSTZONE_VERSION:
 		SMC_RET1(handle, (uint64_t)PM_RET_SUCCESS |
-			 ((uint64_t)VERSAL_TZ_VERSION << 32U));
+			 ((uint64_t)TZ_VERSION << 32U));
 
 	default:
 		return (uintptr_t)0;
diff --git a/plat/xilinx/versal_net/aarch64/versal_net_common.c b/plat/xilinx/versal_net/aarch64/versal_net_common.c
new file mode 100644
index 0000000..c78b5d0
--- /dev/null
+++ b/plat/xilinx/versal_net/aarch64/versal_net_common.c
@@ -0,0 +1,127 @@
+/*
+ * Copyright (c) 2021-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, Xilinx, Inc. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <drivers/generic_delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <plat/common/platform.h>
+#include <plat_ipi.h>
+
+#include <plat_private.h>
+#include <versal_net_def.h>
+
+uint32_t platform_id, platform_version;
+
+/*
+ * Table of regions to map using the MMU.
+ * This doesn't include TZRAM as the 'mem_layout' argument passed to
+ * configure_mmu_elx() will give the available subset of that,
+ */
+const mmap_region_t plat_versal_net_mmap[] = {
+	MAP_REGION_FLAT(DEVICE0_BASE, DEVICE0_SIZE, MT_DEVICE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(DEVICE1_BASE, DEVICE1_SIZE, MT_DEVICE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(DEVICE2_BASE, DEVICE2_SIZE, MT_DEVICE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(CRF_BASE, CRF_SIZE, MT_DEVICE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(IPI_BASE, IPI_SIZE, MT_DEVICE | MT_RW | MT_SECURE),
+	{ 0 }
+};
+
+const mmap_region_t *plat_versal_net_get_mmap(void)
+{
+	return plat_versal_net_mmap;
+}
+
+/* For saving cpu clock for certain platform */
+uint32_t cpu_clock;
+
+char *board_name_decode(void)
+{
+	switch (platform_id) {
+	case VERSAL_NET_SPP:
+		return "IPP";
+	case VERSAL_NET_EMU:
+		return "EMU";
+	case VERSAL_NET_SILICON:
+		return "Silicon";
+	case VERSAL_NET_QEMU:
+		return "QEMU";
+	default:
+		return "Unknown";
+	}
+}
+
+void board_detection(void)
+{
+	uint32_t version;
+
+	version = mmio_read_32(PMC_TAP_VERSION);
+	platform_id = FIELD_GET(PLATFORM_MASK, version);
+	platform_version = FIELD_GET(PLATFORM_VERSION_MASK, version);
+
+	if (platform_id == VERSAL_NET_QEMU_COSIM) {
+		platform_id = VERSAL_NET_QEMU;
+	}
+
+	if ((platform_id == VERSAL_NET_SPP) ||
+	    (platform_id == VERSAL_NET_EMU) ||
+	    (platform_id == VERSAL_NET_QEMU)) {
+		/*
+		 * 9 is diff for
+		 * 0 means 0.9 version
+		 * 1 means 1.0 version
+		 * 2 means 1.1 version
+		 * etc,
+		 */
+		platform_version += 9U;
+	}
+
+	/* Make sure that console is setup to see this message */
+	VERBOSE("Platform id: %d version: %d.%d\n", platform_id,
+	      platform_version / 10U, platform_version % 10U);
+}
+
+void versal_net_config_setup(void)
+{
+	uint32_t val;
+	uintptr_t crl_base, iou_scntrs_base, psx_base;
+
+	crl_base = VERSAL_NET_CRL;
+	iou_scntrs_base = VERSAL_NET_IOU_SCNTRS;
+	psx_base = PSX_CRF;
+
+	/* Reset for system timestamp generator in FPX */
+	mmio_write_32(psx_base + PSX_CRF_RST_TIMESTAMP_OFFSET, 0);
+
+	/* Global timer init - Program time stamp reference clk */
+	val = mmio_read_32(crl_base + VERSAL_NET_CRL_TIMESTAMP_REF_CTRL_OFFSET);
+	val |= VERSAL_NET_CRL_APB_TIMESTAMP_REF_CTRL_CLKACT_BIT;
+	mmio_write_32(crl_base + VERSAL_NET_CRL_TIMESTAMP_REF_CTRL_OFFSET, val);
+
+	/* Clear reset of timestamp reg */
+	mmio_write_32(crl_base + VERSAL_NET_CRL_RST_TIMESTAMP_OFFSET, 0);
+
+	/* Program freq register in System counter and enable system counter. */
+	mmio_write_32(iou_scntrs_base + VERSAL_NET_IOU_SCNTRS_BASE_FREQ_OFFSET,
+		      cpu_clock);
+	mmio_write_32(iou_scntrs_base + VERSAL_NET_IOU_SCNTRS_COUNTER_CONTROL_REG_OFFSET,
+		      VERSAL_NET_IOU_SCNTRS_CONTROL_EN);
+
+	generic_delay_timer_init();
+
+#if (TFA_NO_PM == 0)
+	/* Configure IPI data for versal_net */
+	versal_net_ipi_config_table_init();
+#endif
+}
+
+uint32_t plat_get_syscnt_freq2(void)
+{
+	return cpu_clock;
+}
diff --git a/plat/xilinx/versal_net/aarch64/versal_net_helpers.S b/plat/xilinx/versal_net/aarch64/versal_net_helpers.S
new file mode 100644
index 0000000..48082a6
--- /dev/null
+++ b/plat/xilinx/versal_net/aarch64/versal_net_helpers.S
@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, Xilinx, Inc. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <drivers/arm/gicv3.h>
+#include <platform_def.h>
+
+	.globl	plat_secondary_cold_boot_setup
+	.globl	plat_is_my_cpu_primary
+	.globl	platform_mem_init
+	.globl	plat_my_core_pos
+	.globl	plat_crash_console_init
+	.globl	plat_crash_console_putc
+	.globl	plat_crash_console_flush
+
+	/* -----------------------------------------------------
+	 * 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.
+	 * TODO: Should we read the PSYS register to make sure
+	 * that the request has gone through.
+	 * -----------------------------------------------------
+	 */
+func plat_secondary_cold_boot_setup
+	mrs	x0, mpidr_el1
+
+	/*
+	 * There is no sane reason to come out of this wfi. This
+	 * cpu will be powered on and reset by the cpu_on pm api
+	 */
+	dsb	sy
+	bl	plat_panic_handler
+endfunc plat_secondary_cold_boot_setup
+
+func plat_is_my_cpu_primary
+	mov	x9, x30
+	bl	plat_my_core_pos
+	cmp	x0, #VERSAL_NET_PRIMARY_CPU
+	cset	x0, eq
+	ret	x9
+endfunc plat_is_my_cpu_primary
+
+	/* -----------------------------------------------------
+	 *  unsigned int plat_my_core_pos(void)
+	 *  This function uses the plat_core_pos_by_mpidr()
+	 *  definition to get the index of the calling CPU.
+	 * -----------------------------------------------------
+	 */
+func plat_my_core_pos
+	mrs	x0, mpidr_el1
+	b	plat_core_pos_by_mpidr
+endfunc plat_my_core_pos
+
+	/* ---------------------------------------------------------------------
+	 * We don't need to carry out any memory initialization on Versal NET
+	 * platform. The Secure RAM is accessible straight away.
+	 * ---------------------------------------------------------------------
+	 */
+func platform_mem_init
+	ret
+endfunc platform_mem_init
+
+
+	/* ---------------------------------------------
+	 * 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, PLAT_VERSAL_NET_CRASH_UART_BASE
+	mov_imm	x1, PLAT_VERSAL_NET_CRASH_UART_CLK_IN_HZ
+	mov_imm	x2, VERSAL_NET_CONSOLE_BAUDRATE
+	b	console_pl011_core_init */
+endfunc plat_crash_console_init
+
+	/* ---------------------------------------------
+	 * int plat_crash_console_putc(int c)
+	 * 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, PLAT_VERSAL_NET_CRASH_UART_BASE
+	b	console_pl011_core_putc
+endfunc plat_crash_console_putc
+
+	/* ---------------------------------------------
+	 * void plat_crash_console_flush()
+	 * 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, PLAT_VERSAL_NET_CRASH_UART_BASE
+	b	console_pl011_core_flush
+endfunc plat_crash_console_flush
diff --git a/plat/xilinx/versal_net/bl31_versal_net_setup.c b/plat/xilinx/versal_net/bl31_versal_net_setup.c
new file mode 100644
index 0000000..97080e9
--- /dev/null
+++ b/plat/xilinx/versal_net/bl31_versal_net_setup.c
@@ -0,0 +1,220 @@
+/*
+ * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, Xilinx, Inc. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <bl31/bl31.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <common/fdt_fixup.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/arm/pl011.h>
+#include <drivers/console.h>
+#include <lib/mmio.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <libfdt.h>
+#include <plat/common/platform.h>
+#include <plat_arm.h>
+
+#include <plat_private.h>
+#include <plat_startup.h>
+#include <versal_net_def.h>
+
+static entry_point_info_t bl32_image_ep_info;
+static entry_point_info_t bl33_image_ep_info;
+static console_t versal_net_runtime_console;
+
+/*
+ * 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)
+{
+	assert(sec_state_is_valid(type));
+
+	if (type == NON_SECURE) {
+		return &bl33_image_ep_info;
+	}
+
+	return &bl32_image_ep_info;
+}
+
+/*
+ * Set the build time defaults,if we can't find any config data.
+ */
+static inline void bl31_set_default_config(void)
+{
+	bl32_image_ep_info.pc = BL32_BASE;
+	bl32_image_ep_info.spsr = arm_get_spsr_for_bl32_entry();
+	bl33_image_ep_info.pc = plat_get_ns_image_entrypoint();
+	bl33_image_ep_info.spsr = SPSR_64(MODE_EL2, MODE_SP_ELX,
+					DISABLE_ALL_EXCEPTIONS);
+}
+
+/*
+ * Perform any BL31 specific platform actions. Here is an opportunity to copy
+ * parameters passed by the calling EL (S-EL1 in BL2 & S-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.
+ */
+void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
+				u_register_t arg2, u_register_t arg3)
+{
+	uint32_t uart_clock;
+	int32_t rc;
+
+	board_detection();
+
+	switch (platform_id) {
+	case VERSAL_NET_SPP:
+		cpu_clock = 1000000;
+		uart_clock = 1000000;
+		break;
+	case VERSAL_NET_EMU:
+		cpu_clock = 3660000;
+		uart_clock = 25000000;
+		break;
+	case VERSAL_NET_QEMU:
+		/* Random values now */
+		cpu_clock = 100000000;
+		uart_clock = 25000000;
+		break;
+	case VERSAL_NET_SILICON:
+	default:
+		panic();
+	}
+
+	/* Initialize the console to provide early debug support */
+	rc = console_pl011_register(VERSAL_NET_UART_BASE, uart_clock,
+				    VERSAL_NET_UART_BAUDRATE,
+				    &versal_net_runtime_console);
+	if (rc == 0) {
+		panic();
+	}
+
+	console_set_scope(&versal_net_runtime_console, CONSOLE_FLAG_BOOT |
+			  CONSOLE_FLAG_RUNTIME);
+
+	NOTICE("TF-A running on Xilinx %s %d.%d\n", board_name_decode(),
+	       platform_version / 10U, platform_version % 10U);
+
+	/* Initialize the platform config for future decision making */
+	versal_net_config_setup();
+	/* There are no parameters from BL2 if BL31 is a reset vector */
+	assert(arg0 == 0U);
+	assert(arg1 == 0U);
+
+	/*
+	 * Do initial security configuration to allow DRAM/device access. On
+	 * Base VERSAL_NET only DRAM security is programmable (via TrustZone), but
+	 * other platforms might have more programmable security devices
+	 * present.
+	 */
+
+	/* Populate common information for BL32 and BL33 */
+	SET_PARAM_HEAD(&bl32_image_ep_info, PARAM_EP, VERSION_1, 0);
+	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
+	SET_PARAM_HEAD(&bl33_image_ep_info, PARAM_EP, VERSION_1, 0);
+	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
+
+	bl31_set_default_config();
+
+	NOTICE("BL31: Secure code at 0x%lx\n", bl32_image_ep_info.pc);
+	NOTICE("BL31: Non secure code at 0x%lx\n", bl33_image_ep_info.pc);
+}
+
+static versal_intr_info_type_el3_t type_el3_interrupt_table[MAX_INTR_EL3];
+
+int request_intr_type_el3(uint32_t id, interrupt_type_handler_t handler)
+{
+	static uint32_t index;
+	uint32_t i;
+
+	/* Validate 'handler' and 'id' parameters */
+	if (handler == NULL || index >= MAX_INTR_EL3) {
+		return -EINVAL;
+	}
+
+	/* Check if a handler has already been registered */
+	for (i = 0; i < index; i++) {
+		if (id == type_el3_interrupt_table[i].id) {
+			return -EALREADY;
+		}
+	}
+
+	type_el3_interrupt_table[index].id = id;
+	type_el3_interrupt_table[index].handler = handler;
+
+	index++;
+
+	return 0;
+}
+
+static uint64_t rdo_el3_interrupt_handler(uint32_t id, uint32_t flags,
+					  void *handle, void *cookie)
+{
+	uint32_t intr_id;
+	uint32_t i;
+	interrupt_type_handler_t handler = NULL;
+
+	intr_id = plat_ic_get_pending_interrupt_id();
+
+	for (i = 0; i < MAX_INTR_EL3; i++) {
+		if (intr_id == type_el3_interrupt_table[i].id) {
+			handler = type_el3_interrupt_table[i].handler;
+		}
+	}
+
+	if (handler != NULL) {
+		handler(intr_id, flags, handle, cookie);
+	}
+
+	return 0;
+}
+
+void bl31_platform_setup(void)
+{
+	/* Initialize the gic cpu and distributor interfaces */
+	plat_versal_net_gic_driver_init();
+	plat_versal_net_gic_init();
+}
+
+void bl31_plat_runtime_setup(void)
+{
+	uint64_t flags = 0;
+	int32_t rc;
+
+	set_interrupt_rm_flag(flags, NON_SECURE);
+	rc = register_interrupt_type_handler(INTR_TYPE_EL3,
+					     rdo_el3_interrupt_handler, flags);
+	if (rc != 0) {
+		panic();
+	}
+}
+
+/*
+ * Perform the very early platform specific architectural setup here.
+ */
+void bl31_plat_arch_setup(void)
+{
+	const mmap_region_t bl_regions[] = {
+		MAP_REGION_FLAT(BL31_BASE, BL31_END - BL31_BASE,
+			MT_MEMORY | MT_RW | MT_SECURE),
+		MAP_REGION_FLAT(BL_CODE_BASE, BL_CODE_END - BL_CODE_BASE,
+				MT_CODE | MT_SECURE),
+		MAP_REGION_FLAT(BL_RO_DATA_BASE, BL_RO_DATA_END - BL_RO_DATA_BASE,
+				MT_RO_DATA | MT_SECURE),
+		{0}
+	};
+
+	setup_page_tables(bl_regions, plat_versal_net_get_mmap());
+	enable_mmu(0);
+}
diff --git a/plat/xilinx/versal_net/include/plat_ipi.h b/plat/xilinx/versal_net/include/plat_ipi.h
new file mode 100644
index 0000000..5255f8f
--- /dev/null
+++ b/plat/xilinx/versal_net/include/plat_ipi.h
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2022, Xilinx, Inc. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/* Versal IPI management enums and defines */
+
+#ifndef PLAT_IPI_H
+#define PLAT_IPI_H
+
+#include <stdint.h>
+
+#include <ipi.h>
+
+/*********************************************************************
+ * IPI agent IDs macros
+ ********************************************************************/
+#define IPI_ID_PMC	1U
+#define IPI_ID_APU	2U
+#define IPI_ID_RPU0	3U
+#define IPI_ID_RPU1	4U
+#define IPI_ID_3	5U
+#define IPI_ID_4	6U
+#define IPI_ID_5	7U
+#define IPI_ID_MAX	8U
+
+/*********************************************************************
+ * IPI message buffers
+ ********************************************************************/
+#define IPI_BUFFER_BASEADDR	(0xEB3F0000U)
+
+#define IPI_BUFFER_APU_BASE	(IPI_BUFFER_BASEADDR + 0x400U)
+#define IPI_BUFFER_PMC_BASE	(IPI_BUFFER_BASEADDR + 0x200U)
+
+#define IPI_BUFFER_TARGET_APU_OFFSET	0x80U
+#define IPI_BUFFER_TARGET_PMC_OFFSET	0x40U
+
+#define IPI_BUFFER_LOCAL_BASE	IPI_BUFFER_APU_BASE
+#define IPI_BUFFER_REMOTE_BASE	IPI_BUFFER_PMC_BASE
+
+#define IPI_BUFFER_TARGET_LOCAL_OFFSET	IPI_BUFFER_TARGET_APU_OFFSET
+#define IPI_BUFFER_TARGET_REMOTE_OFFSET	IPI_BUFFER_TARGET_PMC_OFFSET
+
+#define IPI_BUFFER_MAX_WORDS	8
+
+#define IPI_BUFFER_REQ_OFFSET	0x0U
+#define IPI_BUFFER_RESP_OFFSET	0x20U
+
+/*********************************************************************
+ * Platform specific IPI API declarations
+ ********************************************************************/
+
+/* Configure IPI table for versal_net */
+void versal_net_ipi_config_table_init(void);
+
+#endif /* PLAT_IPI_H */
diff --git a/plat/xilinx/versal_net/include/plat_macros.S b/plat/xilinx/versal_net/include/plat_macros.S
new file mode 100644
index 0000000..fb108b6
--- /dev/null
+++ b/plat/xilinx/versal_net/include/plat_macros.S
@@ -0,0 +1,118 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, Xilinx, Inc. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_MACROS_S
+#define PLAT_MACROS_S
+
+#include <drivers/arm/gic_common.h>
+#include <drivers/arm/gicv2.h>
+#include <drivers/arm/gicv3.h>
+
+#include "../include/platform_def.h"
+
+.section .rodata.gic_reg_name, "aS"
+/* Applicable only to GICv2 and GICv3 with SRE disabled (legacy mode) */
+gicc_regs:
+	.asciz "gicc_hppir", "gicc_ahppir", "gicc_ctlr", ""
+
+/* Applicable only to GICv3 with SRE enabled */
+icc_regs:
+	.asciz "icc_hppir0_el1", "icc_hppir1_el1", "icc_ctlr_el3", ""
+
+/* Registers common to both GICv2 and GICv3 */
+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 utility macro prints out relevant GIC
+	 * registers whenever an unhandled exception is
+	 * taken in BL31 on Versal NET platform.
+	 * Expects: GICD base in x16, GICC base in x17
+	 * Clobbers: x0 - x10, sp
+	 * ---------------------------------------------
+	 */
+	.macro versal_net_print_gic_regs
+	/* Check for GICv3 system register access */
+	mrs	x7, id_aa64pfr0_el1
+	ubfx	x7, x7, #ID_AA64PFR0_GIC_SHIFT, #ID_AA64PFR0_GIC_WIDTH
+	cmp	x7, #1
+	b.ne	print_gicv2
+
+	/* Check for SRE enable */
+	mrs	x8, ICC_SRE_EL3
+	tst	x8, #ICC_SRE_SRE_BIT
+	b.eq	print_gicv2
+
+	/* Load the icc reg list to x6 */
+	adr	x6, icc_regs
+	/* Load the icc regs to gp regs used by str_in_crash_buf_print */
+	mrs	x8, ICC_HPPIR0_EL1
+	mrs	x9, ICC_HPPIR1_EL1
+	mrs	x10, ICC_CTLR_EL3
+	/* Store to the crash buf and print to console */
+	bl	str_in_crash_buf_print
+	b	print_gic_common
+
+print_gicv2:
+	/* 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_gic_common:
+	/* 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
+
+	/* ---------------------------------------------
+	 * The below required platform porting macro
+	 * prints out relevant GIC and CCI registers
+	 * whenever an unhandled exception is taken in
+	 * BL31.
+	 * Clobbers: x0 - x10, x16, x17, sp
+	 * ---------------------------------------------
+	 */
+	.macro plat_crash_print_regs
+	/*
+	 * Empty for now to handle more platforms variant.
+	 * Uncomment it when versions are stable
+	 */
+	/*
+	mov_imm	x17, PLAT_VERSAL_NET_GICD_BASE
+	mov_imm	x16, PLAT_VERSAL_NET_GICR_BASE
+	versal_net_print_gic_regs
+	*/
+	.endm
+
+#endif /* PLAT_MACROS_S */
diff --git a/plat/xilinx/versal_net/include/plat_pm_common.h b/plat/xilinx/versal_net/include/plat_pm_common.h
new file mode 100644
index 0000000..ad7b40f
--- /dev/null
+++ b/plat/xilinx/versal_net/include/plat_pm_common.h
@@ -0,0 +1,25 @@
+/*
+ * Copyright (c) 2022, Xilinx, Inc. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*
+ * Contains platform specific definitions of commonly used macros data types
+ * for PU Power Management. This file should be common for all PU's.
+ */
+
+#ifndef PLAT_PM_COMMON_H
+#define PLAT_PM_COMMON_H
+
+#include <stdint.h>
+
+#include <common/debug.h>
+
+#include "pm_defs.h"
+
+#define NON_SECURE_FLAG		1U
+#define SECURE_FLAG		0U
+
+#endif /* PLAT_PM_COMMON_H */
diff --git a/plat/xilinx/versal_net/include/plat_private.h b/plat/xilinx/versal_net/include/plat_private.h
new file mode 100644
index 0000000..6a3bc19
--- /dev/null
+++ b/plat/xilinx/versal_net/include/plat_private.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, Xilinx, Inc. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_PRIVATE_H
+#define PLAT_PRIVATE_H
+
+#include <bl31/interrupt_mgmt.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+
+typedef struct versal_intr_info_type_el3 {
+	uint32_t id;
+	interrupt_type_handler_t handler;
+} versal_intr_info_type_el3_t;
+
+void versal_net_config_setup(void);
+
+const mmap_region_t *plat_versal_net_get_mmap(void);
+
+void plat_versal_net_gic_driver_init(void);
+void plat_versal_net_gic_init(void);
+void plat_versal_net_gic_cpuif_enable(void);
+void plat_versal_net_gic_cpuif_disable(void);
+void plat_versal_net_gic_pcpu_init(void);
+void plat_versal_net_gic_save(void);
+void plat_versal_net_gic_resume(void);
+void plat_versal_net_gic_redistif_on(void);
+void plat_versal_net_gic_redistif_off(void);
+
+extern uint32_t cpu_clock, platform_id, platform_version;
+void board_detection(void);
+char *board_name_decode(void);
+uint64_t smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3,
+		       uint64_t x4, void *cookie, void *handle, uint64_t flags);
+int32_t sip_svc_setup_init(void);
+/*
+ * Register handler to specific GIC entrance
+ * for INTR_TYPE_EL3 type of interrupt
+ */
+int request_intr_type_el3(uint32_t irq, interrupt_type_handler_t fiq_handler);
+
+#define PM_GET_CHIPID			(24U)
+#define IOCTL_OSPI_MUX_SELECT		(21U)
+
+#endif /* PLAT_PRIVATE_H */
diff --git a/plat/xilinx/versal_net/include/platform_def.h b/plat/xilinx/versal_net/include/platform_def.h
new file mode 100644
index 0000000..696771f
--- /dev/null
+++ b/plat/xilinx/versal_net/include/platform_def.h
@@ -0,0 +1,114 @@
+/*
+ * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, Xilinx, Inc. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLATFORM_DEF_H
+#define PLATFORM_DEF_H
+
+#include <arch.h>
+#include "versal_net_def.h"
+
+/*******************************************************************************
+ * Generic platform constants
+ ******************************************************************************/
+
+/* Size of cacheable stacks */
+#define PLATFORM_STACK_SIZE		U(0x440)
+
+#define PLATFORM_CLUSTER_COUNT		U(4)
+#define PLATFORM_CORE_COUNT_PER_CLUSTER	U(4) /* 4 CPUs per cluster */
+
+#define PLATFORM_CORE_COUNT		(PLATFORM_CLUSTER_COUNT * PLATFORM_CORE_COUNT_PER_CLUSTER)
+
+#define PLAT_MAX_PWR_LVL		U(2)
+#define PLAT_MAX_RET_STATE		U(1)
+#define PLAT_MAX_OFF_STATE		U(2)
+
+/*******************************************************************************
+ * BL31 specific defines.
+ ******************************************************************************/
+/*
+ * Put BL31 at the top of the Trusted SRAM (just below the shared memory, if
+ * present). BL31_BASE is calculated using the current BL31 debug size plus a
+ * little space for growth.
+ */
+#ifndef VERSAL_NET_ATF_MEM_BASE
+# define BL31_BASE			U(0xBBF00000)
+# define BL31_LIMIT			U(0xBBFFFFFF)
+#else
+# define BL31_BASE			U(VERSAL_NET_ATF_MEM_BASE)
+# define BL31_LIMIT			U(VERSAL_NET_ATF_MEM_BASE + VERSAL_NET_ATF_MEM_SIZE - 1)
+# ifdef VERSAL_NET_ATF_MEM_PROGBITS_SIZE
+#  define BL31_PROGBITS_LIMIT		U(VERSAL_NET_ATF_MEM_BASE + \
+					  VERSAL_NET_ATF_MEM_PROGBITS_SIZE - 1)
+# endif
+#endif
+
+/*******************************************************************************
+ * BL32 specific defines.
+ ******************************************************************************/
+#ifndef VERSAL_NET_BL32_MEM_BASE
+# define BL32_BASE			U(0x60000000)
+# define BL32_LIMIT			U(0x7FFFFFFF)
+#else
+# define BL32_BASE			U(VERSAL_NET_BL32_MEM_BASE)
+# define BL32_LIMIT			U(VERSAL_NET_BL32_MEM_BASE + VERSAL_NET_BL32_MEM_SIZE - 1)
+#endif
+
+/*******************************************************************************
+ * BL33 specific defines.
+ ******************************************************************************/
+#ifndef PRELOADED_BL33_BASE
+# define PLAT_ARM_NS_IMAGE_BASE		U(0x8000000)
+#else
+# define PLAT_ARM_NS_IMAGE_BASE		U(PRELOADED_BL33_BASE)
+#endif
+
+/*******************************************************************************
+ * TSP  specific defines.
+ ******************************************************************************/
+#define TSP_SEC_MEM_BASE		BL32_BASE
+#define TSP_SEC_MEM_SIZE		(BL32_LIMIT - BL32_BASE + 1U)
+
+/* ID of the secure physical generic timer interrupt used by the TSP */
+#define TSP_IRQ_SEC_PHY_TIMER		ARM_IRQ_SEC_PHY_TIMER
+
+/*******************************************************************************
+ * Platform specific page table and MMU setup constants
+ ******************************************************************************/
+#define PLAT_DDR_LOWMEM_MAX		U(0x80000000)
+
+#define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 32U)
+#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 32U)
+#if (BL31_LIMIT < PLAT_DDR_LOWMEM_MAX)
+#define MAX_MMAP_REGIONS		U(10)
+#else
+#define MAX_MMAP_REGIONS		U(9)
+#endif
+
+#define MAX_XLAT_TABLES			U(8)
+
+#define CACHE_WRITEBACK_SHIFT	U(6)
+#define CACHE_WRITEBACK_GRANULE	(1 << CACHE_WRITEBACK_SHIFT)
+
+#define PLAT_VERSAL_NET_GICD_BASE	U(0xE2000000)
+#define PLAT_VERSAL_NET_GICR_BASE	U(0xE2060000)
+
+/*
+ * 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_VERSAL_IPI_IRQ	62
+
+#define PLAT_VERSAL_NET_G1S_IRQ_PROPS(grp) \
+	INTR_PROP_DESC(VERSAL_NET_IRQ_SEC_PHY_TIMER, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_LEVEL)
+
+#define PLAT_VERSAL_NET_G0_IRQ_PROPS(grp)
+
+#endif /* PLATFORM_DEF_H */
diff --git a/plat/xilinx/versal_net/include/versal_net_def.h b/plat/xilinx/versal_net/include/versal_net_def.h
new file mode 100644
index 0000000..8cb5bf3
--- /dev/null
+++ b/plat/xilinx/versal_net/include/versal_net_def.h
@@ -0,0 +1,171 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, Xilinx, Inc. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef VERSAL_NET_DEF_H
+#define VERSAL_NET_DEF_H
+
+#include <plat/arm/common/smccc_def.h>
+#include <plat/common/common_def.h>
+
+#define MAX_INTR_EL3			2
+/* This part is taken from U-Boot project under GPL that's why dual license above */
+#define __bf_shf(x) (__builtin_ffsll(x) - 1U)
+#define FIELD_GET(_mask, _reg)						\
+	({								\
+		(typeof(_mask))(((_reg) & (_mask)) >> __bf_shf(_mask));	\
+	})
+
+/* List all consoles */
+#define VERSAL_NET_CONSOLE_ID_pl011	U(1)
+#define VERSAL_NET_CONSOLE_ID_pl011_0	U(1)
+#define VERSAL_NET_CONSOLE_ID_pl011_1	U(2)
+
+#define VERSAL_NET_CONSOLE_IS(con)	(VERSAL_NET_CONSOLE_ID_ ## con == VERSAL_NET_CONSOLE)
+
+/* List all platforms */
+#define VERSAL_NET_SILICON		U(0)
+#define VERSAL_NET_SPP			U(1)
+#define VERSAL_NET_EMU			U(2)
+#define VERSAL_NET_QEMU			U(3)
+#define VERSAL_NET_QEMU_COSIM		U(7)
+
+/* For platform detection */
+#define PMC_TAP				U(0xF11A0000)
+#define PMC_TAP_VERSION			(PMC_TAP + 0x4U)
+# define PLATFORM_MASK			GENMASK(27U, 24U)
+# define PLATFORM_VERSION_MASK		GENMASK(31U, 28U)
+
+/* Global timer reset */
+#define PSX_CRF			U(0xEC200000)
+#define ACPU0_CLK_CTRL		U(0x10C)
+#define ACPU_CLK_CTRL_CLKACT	BIT(25)
+
+#define RST_APU0_OFFSET		U(0x300)
+#define RST_APU_COLD_RESET	BIT(0)
+#define RST_APU_WARN_RESET	BIT(4)
+#define RST_APU_CLUSTER_COLD_RESET	BIT(8)
+#define RST_APU_CLUSTER_WARM_RESET	BIT(9)
+
+#define PSX_CRF_RST_TIMESTAMP_OFFSET	U(0x33C)
+
+#define APU_PCLI			U(0xECB10000)
+#define APU_PCLI_CPU_STEP		U(0x30)
+#define APU_PCLI_CLUSTER_CPU_STEP	(4U * APU_PCLI_CPU_STEP)
+#define APU_PCLI_CLUSTER_OFFSET		U(0x8000)
+#define APU_PCLI_CLUSTER_STEP		U(0x1000)
+#define PCLI_PREQ_OFFSET		U(0x4)
+#define PREQ_CHANGE_REQUEST		BIT(0)
+#define PCLI_PSTATE_OFFSET		U(0x8)
+#define PCLI_PSTATE_VAL_SET		U(0x48)
+#define PCLI_PSTATE_VAL_CLEAR		U(0x38)
+
+/* Firmware Image Package */
+#define VERSAL_NET_PRIMARY_CPU		U(0)
+
+#define CORE_0_IEN_POWER_OFFSET			(0x00000018U)
+#define APU_PCIL_CORE_X_IEN_POWER_REG(cpu_id)	(APU_PCLI + (CORE_0_IEN_POWER_OFFSET + \
+						 (0x30 * cpu_id)))
+#define APU_PCIL_CORE_X_IEN_POWER_MASK		(0x00000001U)
+#define CORE_0_IDS_POWER_OFFSET			(0x0000001CU)
+#define APU_PCIL_CORE_X_IDS_POWER_REG(cpu_id)	(APU_PCLI + (CORE_0_IDS_POWER_OFFSET + \
+						 (0x30 * cpu_id)))
+#define APU_PCIL_CORE_X_IDS_POWER_MASK		(0x00000001U)
+#define CORE_PWRDN_EN_BIT_MASK			(0x1U)
+
+/*******************************************************************************
+ * memory map related constants
+ ******************************************************************************/
+/* IPP 1.2/SPP 0.9 mapping */
+#define DEVICE0_BASE		U(0xE8000000) /* psx, crl, iou */
+#define DEVICE0_SIZE		U(0x08000000)
+#define DEVICE1_BASE		U(0xE2000000) /* gic */
+#define DEVICE1_SIZE		U(0x00800000)
+#define DEVICE2_BASE		U(0xF1000000) /* uart, pmc_tap */
+#define DEVICE2_SIZE		U(0x01000000)
+#define CRF_BASE		U(0xFD1A0000)
+#define CRF_SIZE		U(0x00600000)
+#define IPI_BASE		U(0xEB300000)
+#define IPI_SIZE		U(0x00100000)
+
+/* CRL */
+#define VERSAL_NET_CRL					U(0xEB5E0000)
+#define VERSAL_NET_CRL_TIMESTAMP_REF_CTRL_OFFSET	U(0x14C)
+#define VERSAL_NET_CRL_RST_TIMESTAMP_OFFSET		U(0x348)
+
+#define VERSAL_NET_CRL_APB_TIMESTAMP_REF_CTRL_CLKACT_BIT	(1U << 25U)
+
+/* IOU SCNTRS */
+#define VERSAL_NET_IOU_SCNTRS					U(0xEC920000)
+#define VERSAL_NET_IOU_SCNTRS_COUNTER_CONTROL_REG_OFFSET	U(0)
+#define VERSAL_NET_IOU_SCNTRS_BASE_FREQ_OFFSET			U(0x20)
+
+#define VERSAL_NET_IOU_SCNTRS_CONTROL_EN	U(1)
+
+#define APU_CLUSTER0		U(0xECC00000)
+#define APU_RVBAR_L_0		U(0x40)
+#define APU_RVBAR_H_0		U(0x44)
+#define APU_CLUSTER_STEP	U(0x100000)
+
+#define SLCR_OSPI_QSPI_IOU_AXI_MUX_SEL	U(0xF1060504)
+
+/*******************************************************************************
+ * IRQ constants
+ ******************************************************************************/
+#define VERSAL_NET_IRQ_SEC_PHY_TIMER	U(29)
+
+/*******************************************************************************
+ * UART related constants
+ ******************************************************************************/
+#define VERSAL_NET_UART0_BASE		U(0xF1920000)
+#define VERSAL_NET_UART_BAUDRATE	115200
+
+#define VERSAL_NET_UART_BASE		VERSAL_NET_UART0_BASE
+
+#define PLAT_VERSAL_NET_CRASH_UART_BASE		VERSAL_NET_UART_BASE
+#define PLAT_VERSAL_NET_CRASH_UART_CLK_IN_HZ	VERSAL_NET_UART_CLOCK
+#define VERSAL_NET_CONSOLE_BAUDRATE		VERSAL_NET_UART_BAUDRATE
+
+/*******************************************************************************
+ * IPI registers and bitfields
+ ******************************************************************************/
+#define IPI0_REG_BASE		(0xEB330000U)
+#define IPI0_TRIG_BIT		(1 << 2)
+#define PMC_IPI_TRIG_BIT	(1 << 1)
+#define IPI1_REG_BASE		(0xEB340000U)
+#define IPI1_TRIG_BIT		(1 << 3)
+#define IPI2_REG_BASE		(0xEB350000U)
+#define IPI2_TRIG_BIT		(1 << 4)
+#define IPI3_REG_BASE		(0xEB360000U)
+#define IPI3_TRIG_BIT		(1 << 5)
+#define IPI4_REG_BASE		(0xEB370000U)
+#define IPI4_TRIG_BIT		(1 << 6)
+#define IPI5_REG_BASE		(0xEB380000U)
+#define IPI5_TRIG_BIT		(1 << 7)
+
+/* Processor core device IDs */
+#define PM_DEV_CLUSTER0_ACPU_0	(0x1810C0AFU)
+#define PM_DEV_CLUSTER0_ACPU_1	(0x1810C0B0U)
+#define PM_DEV_CLUSTER0_ACPU_2	(0x1810C0B1U)
+#define PM_DEV_CLUSTER0_ACPU_3	(0x1810C0B2U)
+
+#define PM_DEV_CLUSTER1_ACPU_0	(0x1810C0B3U)
+#define PM_DEV_CLUSTER1_ACPU_1	(0x1810C0B4U)
+#define PM_DEV_CLUSTER1_ACPU_2	(0x1810C0B5U)
+#define PM_DEV_CLUSTER1_ACPU_3	(0x1810C0B6U)
+
+#define PM_DEV_CLUSTER2_ACPU_0	(0x1810C0B7U)
+#define PM_DEV_CLUSTER2_ACPU_1	(0x1810C0B8U)
+#define PM_DEV_CLUSTER2_ACPU_2	(0x1810C0B9U)
+#define PM_DEV_CLUSTER2_ACPU_3	(0x1810C0BAU)
+
+#define PM_DEV_CLUSTER3_ACPU_0	(0x1810C0BBU)
+#define PM_DEV_CLUSTER3_ACPU_1	(0x1810C0BCU)
+#define PM_DEV_CLUSTER3_ACPU_2	(0x1810C0BDU)
+#define PM_DEV_CLUSTER3_ACPU_3	(0x1810C0BEU)
+
+#endif /* VERSAL_NET_DEF_H */
diff --git a/plat/xilinx/versal_net/plat_psci.c b/plat/xilinx/versal_net/plat_psci.c
new file mode 100644
index 0000000..299eca4
--- /dev/null
+++ b/plat/xilinx/versal_net/plat_psci.c
@@ -0,0 +1,221 @@
+/*
+ * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, Xilinx, Inc. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <lib/mmio.h>
+#include <lib/psci/psci.h>
+#include <plat/arm/common/plat_arm.h>
+#include <plat/common/platform.h>
+#include <plat_arm.h>
+
+#include <plat_private.h>
+
+#define FUNCID_MASK	U(0xffff)
+#define PM_RET_ERROR_NOFEATURE U(19)
+
+#define PM_IOCTL	34U
+
+static uintptr_t versal_net_sec_entry;
+
+static void zynqmp_cpu_standby(plat_local_state_t cpu_state)
+{
+	dsb();
+	wfi();
+}
+
+static int32_t zynqmp_nopmu_pwr_domain_on(u_register_t mpidr)
+{
+	uint32_t cpu_id = plat_core_pos_by_mpidr(mpidr);
+	uint32_t cpu = cpu_id % PLATFORM_CORE_COUNT_PER_CLUSTER;
+	uint32_t cluster = cpu_id / PLATFORM_CORE_COUNT_PER_CLUSTER;
+	uintptr_t apu_cluster_base = 0, apu_pcli_base, apu_pcli_cluster = 0;
+	uintptr_t rst_apu_cluster = PSX_CRF + RST_APU0_OFFSET + (cluster * 0x4);
+
+	VERBOSE("%s: mpidr: 0x%lx, cpuid: %x, cpu: %x, cluster: %x\n",
+		__func__, mpidr, cpu_id, cpu, cluster);
+
+	if (cpu_id == -1) {
+		return PSCI_E_INTERN_FAIL;
+	}
+
+	if (platform_id == VERSAL_NET_SPP && cluster > 1) {
+		panic();
+	}
+
+	if (cluster > 3) {
+		panic();
+	}
+
+	apu_pcli_cluster = APU_PCLI + APU_PCLI_CLUSTER_OFFSET + (cluster * APU_PCLI_CLUSTER_STEP);
+	apu_cluster_base = APU_CLUSTER0 + (cluster * APU_CLUSTER_STEP);
+
+	/* Enable clock */
+	mmio_setbits_32(PSX_CRF + ACPU0_CLK_CTRL + (cluster * 0x4), ACPU_CLK_CTRL_CLKACT);
+
+	/* Enable cluster states */
+	mmio_setbits_32(apu_pcli_cluster + PCLI_PSTATE_OFFSET, PCLI_PSTATE_VAL_SET);
+	mmio_setbits_32(apu_pcli_cluster + PCLI_PREQ_OFFSET, PREQ_CHANGE_REQUEST);
+
+	/* assert core reset */
+	mmio_setbits_32(rst_apu_cluster, ((RST_APU_COLD_RESET|RST_APU_WARN_RESET) << cpu));
+
+	/* program RVBAR */
+	mmio_write_32(apu_cluster_base + APU_RVBAR_L_0 + (cpu << 3),
+		      (uint32_t)versal_net_sec_entry);
+	mmio_write_32(apu_cluster_base + APU_RVBAR_H_0 + (cpu << 3),
+		      versal_net_sec_entry >> 32);
+
+	/* de-assert core reset */
+	mmio_clrbits_32(rst_apu_cluster, ((RST_APU_COLD_RESET|RST_APU_WARN_RESET) << cpu));
+
+	/* clear cluster resets */
+	mmio_clrbits_32(rst_apu_cluster, RST_APU_CLUSTER_WARM_RESET);
+	mmio_clrbits_32(rst_apu_cluster, RST_APU_CLUSTER_COLD_RESET);
+
+	apu_pcli_base = APU_PCLI + (APU_PCLI_CPU_STEP * cpu) +
+			(APU_PCLI_CLUSTER_CPU_STEP * cluster);
+
+	mmio_write_32(apu_pcli_base + PCLI_PSTATE_OFFSET, PCLI_PSTATE_VAL_CLEAR);
+	mmio_write_32(apu_pcli_base + PCLI_PREQ_OFFSET, PREQ_CHANGE_REQUEST);
+
+	return PSCI_E_SUCCESS;
+}
+
+static void zynqmp_nopmu_pwr_domain_off(const psci_power_state_t *target_state)
+{
+}
+
+static void __dead2 zynqmp_nopmu_system_reset(void)
+{
+	while (1)
+		wfi();
+}
+
+static int32_t zynqmp_validate_ns_entrypoint(uint64_t ns_entrypoint)
+{
+	return PSCI_E_SUCCESS;
+}
+
+static void zynqmp_pwr_domain_suspend(const psci_power_state_t *target_state)
+{
+}
+
+static void zynqmp_pwr_domain_on_finish(const psci_power_state_t *target_state)
+{
+	plat_versal_net_gic_pcpu_init();
+	plat_versal_net_gic_cpuif_enable();
+}
+
+static void zynqmp_pwr_domain_suspend_finish(const psci_power_state_t *target_state)
+{
+}
+
+static void __dead2 zynqmp_system_off(void)
+{
+	while (1)
+		wfi();
+}
+
+static int32_t zynqmp_validate_power_state(uint32_t power_state, psci_power_state_t *req_state)
+{
+	return PSCI_E_SUCCESS;
+}
+
+static void zynqmp_get_sys_suspend_power_state(psci_power_state_t *req_state)
+{
+	req_state->pwr_domain_state[PSCI_CPU_PWR_LVL] = PLAT_MAX_OFF_STATE;
+	req_state->pwr_domain_state[1] = PLAT_MAX_OFF_STATE;
+}
+
+static const struct plat_psci_ops versal_net_nopmc_psci_ops = {
+	.cpu_standby			= zynqmp_cpu_standby,
+	.pwr_domain_on			= zynqmp_nopmu_pwr_domain_on,
+	.pwr_domain_off			= zynqmp_nopmu_pwr_domain_off,
+	.system_reset			= zynqmp_nopmu_system_reset,
+	.validate_ns_entrypoint		= zynqmp_validate_ns_entrypoint,
+	.pwr_domain_suspend		= zynqmp_pwr_domain_suspend,
+	.pwr_domain_on_finish		= zynqmp_pwr_domain_on_finish,
+	.pwr_domain_suspend_finish	= zynqmp_pwr_domain_suspend_finish,
+	.system_off			= zynqmp_system_off,
+	.validate_power_state		= zynqmp_validate_power_state,
+	.get_sys_suspend_power_state	= zynqmp_get_sys_suspend_power_state,
+};
+
+/*******************************************************************************
+ * Export the platform specific power ops.
+ ******************************************************************************/
+int32_t plat_setup_psci_ops(uintptr_t sec_entrypoint,
+			    const struct plat_psci_ops **psci_ops)
+{
+	versal_net_sec_entry = sec_entrypoint;
+
+	VERBOSE("Setting up entry point %lx\n", versal_net_sec_entry);
+
+	*psci_ops = &versal_net_nopmc_psci_ops;
+
+	return 0;
+}
+
+int sip_svc_setup_init(void)
+{
+	return 0;
+}
+
+static int32_t no_pm_ioctl(uint32_t device_id, uint32_t ioctl_id,
+			   uint32_t arg1, uint32_t arg2)
+{
+	VERBOSE("%s: ioctl_id: %x, arg1: %x\n", __func__, ioctl_id, arg1);
+	if (ioctl_id == IOCTL_OSPI_MUX_SELECT) {
+		mmio_write_32(SLCR_OSPI_QSPI_IOU_AXI_MUX_SEL, arg1);
+		return 0;
+	}
+	return PM_RET_ERROR_NOFEATURE;
+}
+
+static uint64_t no_pm_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3,
+			      uint64_t x4, void *cookie, void *handle, uint64_t flags)
+{
+	int32_t ret;
+	uint32_t arg[4], api_id;
+
+	arg[0] = (uint32_t)x1;
+	arg[1] = (uint32_t)(x1 >> 32);
+	arg[2] = (uint32_t)x2;
+	arg[3] = (uint32_t)(x2 >> 32);
+
+	api_id = smc_fid & FUNCID_NUM_MASK;
+	VERBOSE("%s: smc_fid: %x, api_id=0x%x\n", __func__, smc_fid, api_id);
+
+	switch (smc_fid & FUNCID_MASK) {
+	case PM_IOCTL:
+	{
+		ret = no_pm_ioctl(arg[0], arg[1], arg[2], arg[3]);
+		SMC_RET1(handle, (uint64_t)ret);
+	}
+	case PM_GET_CHIPID:
+	{
+		uint32_t idcode, version;
+
+		idcode  = mmio_read_32(PMC_TAP);
+		version = mmio_read_32(PMC_TAP_VERSION);
+		SMC_RET2(handle, ((uint64_t)idcode << 32), version);
+	}
+	default:
+		WARN("Unimplemented PM Service Call: 0x%x\n", smc_fid);
+		SMC_RET1(handle, SMC_UNK);
+	}
+}
+
+uint64_t smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3, uint64_t x4,
+		     void *cookie, void *handle, uint64_t flags)
+{
+	return no_pm_handler(smc_fid, x1, x2, x3, x4, cookie, handle, flags);
+}
diff --git a/plat/xilinx/versal_net/plat_psci_pm.c b/plat/xilinx/versal_net/plat_psci_pm.c
new file mode 100644
index 0000000..8beaa9a
--- /dev/null
+++ b/plat/xilinx/versal_net/plat_psci_pm.c
@@ -0,0 +1,264 @@
+/*
+ * Copyright (c) 2022, Xilinx, Inc. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <lib/psci/psci.h>
+#include <plat/arm/common/plat_arm.h>
+#include <plat/common/platform.h>
+#include <plat_arm.h>
+
+#include <plat_private.h>
+#include "pm_api_sys.h"
+#include "pm_client.h"
+#include <pm_common.h>
+#include "pm_svc_main.h"
+#include "versal_net_def.h"
+
+static uintptr_t versal_net_sec_entry;
+
+static int32_t versal_net_pwr_domain_on(u_register_t mpidr)
+{
+	uint32_t cpu_id = plat_core_pos_by_mpidr(mpidr);
+	const struct pm_proc *proc;
+
+	VERBOSE("%s: mpidr: 0x%lx, cpuid: %x\n",
+		__func__, mpidr, cpu_id);
+
+	if (cpu_id == -1) {
+		return PSCI_E_INTERN_FAIL;
+	}
+
+	proc = pm_get_proc(cpu_id);
+	if (!proc) {
+		return PSCI_E_INTERN_FAIL;
+	}
+
+	pm_req_wakeup(proc->node_id, (versal_net_sec_entry & 0xFFFFFFFFU) | 0x1U,
+		      versal_net_sec_entry >> 32, 0, 0);
+
+	/* Clear power down request */
+	pm_client_wakeup(proc);
+
+	return PSCI_E_SUCCESS;
+}
+
+/**
+ * versal_net_pwr_domain_off() - This function performs actions to turn off core
+ *
+ * @param target_state	Targeted state
+ */
+static void versal_net_pwr_domain_off(const psci_power_state_t *target_state)
+{
+	uint32_t 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++) {
+		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 */
+	plat_versal_net_gic_cpuif_disable();
+
+	/*
+	 * Send request to PMC to power down the appropriate APU CPU
+	 * core.
+	 * According to PSCI specification, CPU_off function does not
+	 * have resume address and CPU core can only be woken up
+	 * invoking CPU_on function, during which resume address will
+	 * be set.
+	 */
+	pm_self_suspend(proc->node_id, MAX_LATENCY, PM_STATE_CPU_IDLE, 0,
+			SECURE_FLAG);
+}
+
+/**
+ * versal_net_system_reset() - This function sends the reset request
+ * to firmware for the system to reset.  This function does not return.
+ */
+static void __dead2 versal_net_system_reset(void)
+{
+	/* Send the system reset request to the PMC */
+	pm_system_shutdown(XPM_SHUTDOWN_TYPE_RESET,
+			  pm_get_shutdown_scope(), SECURE_FLAG);
+
+	while (1) {
+		wfi();
+	}
+}
+
+/**
+ * versal_net_pwr_domain_suspend() - This function sends request to PMC to suspend
+ * core.
+ *
+ * @param target_state	Targeted state
+ */
+static void versal_net_pwr_domain_suspend(const psci_power_state_t *target_state)
+{
+	uint32_t state;
+	uint32_t 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++) {
+		VERBOSE("%s: target_state->pwr_domain_state[%lu]=%x\n",
+			__func__, i, target_state->pwr_domain_state[i]);
+	}
+
+	plat_versal_net_gic_cpuif_disable();
+
+	if (target_state->pwr_domain_state[1] > PLAT_MAX_RET_STATE) {
+		plat_versal_net_gic_save();
+	}
+
+	state = target_state->pwr_domain_state[1] > PLAT_MAX_RET_STATE ?
+		PM_STATE_SUSPEND_TO_RAM : PM_STATE_CPU_IDLE;
+
+	/* Send request to PMC to suspend this core */
+	pm_self_suspend(proc->node_id, MAX_LATENCY, state, versal_net_sec_entry,
+			SECURE_FLAG);
+
+	/* TODO: disable coherency */
+}
+
+static void versal_net_pwr_domain_on_finish(const psci_power_state_t *target_state)
+{
+	(void)target_state;
+
+	/* Enable the gic cpu interface */
+	plat_versal_net_gic_pcpu_init();
+
+	/* Program the gic per-cpu distributor or re-distributor interface */
+	plat_versal_net_gic_cpuif_enable();
+}
+
+/**
+ * versal_net_pwr_domain_suspend_finish() - This function performs actions to finish
+ * suspend procedure.
+ *
+ * @param target_state	Targeted state
+ */
+static void versal_net_pwr_domain_suspend_finish(const psci_power_state_t *target_state)
+{
+	uint32_t 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++)
+		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);
+
+	/* TODO: enable coherency */
+
+	/* APU was turned off, so restore GIC context */
+	if (target_state->pwr_domain_state[1] > PLAT_MAX_RET_STATE) {
+		plat_versal_net_gic_resume();
+	}
+
+	plat_versal_net_gic_cpuif_enable();
+}
+
+/**
+ * versal_net_system_off() - This function sends the system off request
+ * to firmware.  This function does not return.
+ */
+static void __dead2 versal_net_system_off(void)
+{
+	/* Send the power down request to the PMC */
+	pm_system_shutdown(XPM_SHUTDOWN_TYPE_SHUTDOWN,
+			  pm_get_shutdown_scope(), SECURE_FLAG);
+
+	while (1) {
+		wfi();
+	}
+}
+
+/**
+ * versal_net_validate_power_state() - This function ensures that the power state
+ * parameter in request is valid.
+ *
+ * @param power_state		Power state of core
+ * @param req_state		Requested state
+ *
+ * @return Returns status, either PSCI_E_SUCCESS or reason
+ */
+static int32_t versal_net_validate_power_state(unsigned int power_state,
+					       psci_power_state_t *req_state)
+{
+	VERBOSE("%s: power_state: 0x%x\n", __func__, power_state);
+
+	int32_t pstate = psci_get_pstate_type(power_state);
+
+	assert(req_state);
+
+	/* Sanity check the requested state */
+	if (pstate == PSTATE_TYPE_STANDBY) {
+		req_state->pwr_domain_state[MPIDR_AFFLVL0] = PLAT_MAX_RET_STATE;
+	} 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)) {
+		return PSCI_E_INVALID_PARAMS;
+	}
+
+	return PSCI_E_SUCCESS;
+}
+
+/**
+ * versal_net_get_sys_suspend_power_state() - Get power state for system suspend
+ *
+ * @param req_state	Requested state
+ */
+static void versal_net_get_sys_suspend_power_state(psci_power_state_t *req_state)
+{
+	req_state->pwr_domain_state[PSCI_CPU_PWR_LVL] = PLAT_MAX_OFF_STATE;
+	req_state->pwr_domain_state[1] = PLAT_MAX_OFF_STATE;
+}
+
+static const struct plat_psci_ops versal_net_nopmc_psci_ops = {
+	.pwr_domain_on                  = versal_net_pwr_domain_on,
+	.pwr_domain_off                 = versal_net_pwr_domain_off,
+	.pwr_domain_on_finish           = versal_net_pwr_domain_on_finish,
+	.pwr_domain_suspend             = versal_net_pwr_domain_suspend,
+	.pwr_domain_suspend_finish      = versal_net_pwr_domain_suspend_finish,
+	.system_off                     = versal_net_system_off,
+	.system_reset                   = versal_net_system_reset,
+	.validate_power_state           = versal_net_validate_power_state,
+	.get_sys_suspend_power_state    = versal_net_get_sys_suspend_power_state,
+};
+
+/*******************************************************************************
+ * Export the platform specific power ops.
+ ******************************************************************************/
+int32_t plat_setup_psci_ops(uintptr_t sec_entrypoint,
+			    const struct plat_psci_ops **psci_ops)
+{
+	versal_net_sec_entry = sec_entrypoint;
+
+	VERBOSE("Setting up entry point %lx\n", versal_net_sec_entry);
+
+	*psci_ops = &versal_net_nopmc_psci_ops;
+
+	return 0;
+}
+
+int32_t sip_svc_setup_init(void)
+{
+	return pm_setup();
+}
+
+uint64_t smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3, uint64_t x4,
+		     void *cookie, void *handle, uint64_t flags)
+{
+	return pm_smc_handler(smc_fid, x1, x2, x3, x4, cookie, handle, flags);
+}
diff --git a/plat/xilinx/versal_net/plat_topology.c b/plat/xilinx/versal_net/plat_topology.c
new file mode 100644
index 0000000..7f985b0
--- /dev/null
+++ b/plat/xilinx/versal_net/plat_topology.c
@@ -0,0 +1,63 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, Xilinx, Inc. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <plat/common/platform.h>
+
+#include <plat_private.h>
+#include <platform_def.h>
+
+static const uint8_t plat_power_domain_tree_desc[] = {
+	/* Number of root nodes */
+	1,
+	/* Number of clusters */
+	PLATFORM_CLUSTER_COUNT,
+	/* Number of children for the first cluster node */
+	PLATFORM_CORE_COUNT_PER_CLUSTER,
+	/* Number of children for the second cluster node */
+	PLATFORM_CORE_COUNT_PER_CLUSTER,
+	/* Number of children for the third cluster node */
+	PLATFORM_CORE_COUNT_PER_CLUSTER,
+	/* Number of children for the fourth cluster node */
+	PLATFORM_CORE_COUNT_PER_CLUSTER,
+};
+
+const uint8_t *plat_get_power_domain_tree_desc(void)
+{
+	return plat_power_domain_tree_desc;
+}
+
+/*******************************************************************************
+ * This function implements a part of the critical interface between the psci
+ * generic layer and the platform that allows the former to query the platform
+ * to convert an MPIDR to a unique linear index. An error code (-1) is returned
+ * in case the MPIDR is invalid.
+ ******************************************************************************/
+int32_t plat_core_pos_by_mpidr(u_register_t mpidr)
+{
+	uint32_t cluster_id, cpu_id;
+
+	mpidr &= MPIDR_AFFINITY_MASK;
+
+	cluster_id = MPIDR_AFFLVL2_VAL(mpidr);
+	cpu_id = MPIDR_AFFLVL1_VAL(mpidr);
+
+	if (cluster_id >= PLATFORM_CLUSTER_COUNT) {
+		return -3;
+	}
+
+	/*
+	 * Validate cpu_id by checking whether it represents a CPU in
+	 * one of the two clusters present on the platform.
+	 */
+	if (cpu_id >= PLATFORM_CORE_COUNT_PER_CLUSTER) {
+		return -1;
+	}
+
+	return (cpu_id + (cluster_id * PLATFORM_CORE_COUNT_PER_CLUSTER));
+}
diff --git a/plat/xilinx/versal_net/platform.mk b/plat/xilinx/versal_net/platform.mk
new file mode 100644
index 0000000..08e65ac
--- /dev/null
+++ b/plat/xilinx/versal_net/platform.mk
@@ -0,0 +1,96 @@
+# Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2021-2022, Xilinx, Inc. All rights reserved.
+# Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+PLAT_PATH := plat/xilinx/versal_net
+
+override PROGRAMMABLE_RESET_ADDRESS := 1
+PSCI_EXTENDED_STATE_ID := 1
+SEPARATE_CODE_AND_RODATA := 1
+override RESET_TO_BL31 := 1
+PL011_GENERIC_UART := 1
+GIC_ENABLE_V4_EXTN :=  0
+GICV3_SUPPORT_GIC600 := 1
+TFA_NO_PM := 0
+
+override CTX_INCLUDE_AARCH32_REGS    := 0
+
+ifdef TFA_NO_PM
+   $(eval $(call add_define,TFA_NO_PM))
+endif
+
+ifdef VERSAL_NET_ATF_MEM_BASE
+    $(eval $(call add_define,VERSAL_NET_ATF_MEM_BASE))
+
+    ifndef VERSAL_NET_ATF_MEM_SIZE
+        $(error "VERSAL_NET_ATF_BASE defined without VERSAL_NET_ATF_SIZE")
+    endif
+    $(eval $(call add_define,VERSAL_NET_ATF_MEM_SIZE))
+
+    ifdef VERSAL_NET_ATF_MEM_PROGBITS_SIZE
+        $(eval $(call add_define,VERSAL_NET_ATF_MEM_PROGBITS_SIZE))
+    endif
+endif
+
+ifdef VERSAL_NET_BL32_MEM_BASE
+    $(eval $(call add_define,VERSAL_NET_BL32_MEM_BASE))
+
+    ifndef VERSAL_NET_BL32_MEM_SIZE
+        $(error "VERSAL_NET_BL32_BASE defined without VERSAL_NET_BL32_SIZE")
+    endif
+    $(eval $(call add_define,VERSAL_NET_BL32_MEM_SIZE))
+endif
+
+USE_COHERENT_MEM := 0
+HW_ASSISTED_COHERENCY := 1
+
+VERSAL_NET_CONSOLE	?=	pl011
+$(eval $(call add_define_val,VERSAL_NET_CONSOLE,VERSAL_NET_CONSOLE_ID_${VERSAL_NET_CONSOLE}))
+
+PLAT_INCLUDES		:=	-Iinclude/plat/arm/common/			\
+				-Iplat/xilinx/common/include/			\
+				-Iplat/xilinx/common/ipi_mailbox_service/	\
+				-I${PLAT_PATH}/include/				\
+				-Iplat/xilinx/versal/pm_service/
+
+# Include GICv3 driver files
+include drivers/arm/gic/v3/gicv3.mk
+include lib/xlat_tables_v2/xlat_tables.mk
+include lib/libfdt/libfdt.mk
+
+PLAT_BL_COMMON_SOURCES	:=	\
+				drivers/delay_timer/delay_timer.c		\
+				drivers/delay_timer/generic_delay_timer.c	\
+				${GICV3_SOURCES}				\
+				drivers/arm/pl011/aarch64/pl011_console.S	\
+				plat/arm/common/arm_common.c			\
+				plat/common/plat_gicv3.c			\
+				${PLAT_PATH}/aarch64/versal_net_helpers.S	\
+				${PLAT_PATH}/aarch64/versal_net_common.c
+
+BL31_SOURCES		+=	drivers/arm/cci/cci.c				\
+				lib/cpus/aarch64/cortex_a78_ae.S		\
+				lib/cpus/aarch64/cortex_a78.S			\
+				plat/common/plat_psci_common.c
+ifeq ($(TFA_NO_PM), 0)
+BL31_SOURCES		+=	plat/xilinx/versal/pm_service/pm_api_sys.c	\
+				plat/xilinx/common/pm_service/pm_ipi.c		\
+				${PLAT_PATH}/plat_psci_pm.c			\
+				plat/xilinx/versal/pm_service/pm_svc_main.c	\
+				${PLAT_PATH}/pm_service/pm_client.c		\
+				${PLAT_PATH}/versal_net_ipi.c
+else
+BL31_SOURCES		+=	${PLAT_PATH}/plat_psci.c
+endif
+BL31_SOURCES		+=	plat/xilinx/common/plat_startup.c		\
+				plat/xilinx/common/ipi.c			\
+				plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c \
+				${PLAT_PATH}/bl31_versal_net_setup.c		\
+				${PLAT_PATH}/plat_topology.c			\
+				common/fdt_fixup.c				\
+				${LIBFDT_SRCS}					\
+				${PLAT_PATH}/sip_svc_setup.c			\
+				${PLAT_PATH}/versal_net_gicv3.c			\
+				${XLAT_TABLES_LIB_SRCS}
diff --git a/plat/xilinx/versal_net/pm_service/pm_client.c b/plat/xilinx/versal_net/pm_service/pm_client.c
new file mode 100644
index 0000000..6487324
--- /dev/null
+++ b/plat/xilinx/versal_net/pm_service/pm_client.c
@@ -0,0 +1,240 @@
+/*
+ * Copyright (c) 2022, Xilinx, Inc. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*
+ * APU specific definition of processors in the subsystem as well as functions
+ * for getting information about and changing state of the APU.
+ */
+
+#include <assert.h>
+
+#include <drivers/arm/gic_common.h>
+#include <drivers/arm/gicv3.h>
+#include <lib/bakery_lock.h>
+#include <lib/mmio.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+#include <plat/common/platform.h>
+
+#include <plat_ipi.h>
+#include <platform_def.h>
+#include "pm_api_sys.h"
+#include "pm_client.h"
+#include <versal_net_def.h>
+
+#define UNDEFINED_CPUID		(~0)
+
+DEFINE_RENAME_SYSREG_RW_FUNCS(cpu_pwrctrl_val, S3_0_C15_C2_7)
+DEFINE_BAKERY_LOCK(pm_client_secure_lock);
+
+static const struct pm_ipi apu_ipi = {
+	.local_ipi_id = IPI_ID_APU,
+	.remote_ipi_id = IPI_ID_PMC,
+	.buffer_base = IPI_BUFFER_APU_BASE,
+};
+
+/* Order in pm_procs_all array must match cpu ids */
+static const struct pm_proc pm_procs_all[] = {
+	{
+		.node_id = PM_DEV_CLUSTER0_ACPU_0,
+		.ipi = &apu_ipi,
+		.pwrdn_mask = 0,
+	},
+	{
+		.node_id = PM_DEV_CLUSTER0_ACPU_1,
+		.ipi = &apu_ipi,
+		.pwrdn_mask = 0,
+	},
+	{
+		.node_id = PM_DEV_CLUSTER0_ACPU_2,
+		.ipi = &apu_ipi,
+		.pwrdn_mask = 0,
+	},
+	{
+		.node_id = PM_DEV_CLUSTER0_ACPU_3,
+		.ipi = &apu_ipi,
+		.pwrdn_mask = 0,
+	},
+	{
+		.node_id = PM_DEV_CLUSTER1_ACPU_0,
+		.ipi = &apu_ipi,
+		.pwrdn_mask = 0,
+	},
+	{
+		.node_id = PM_DEV_CLUSTER1_ACPU_1,
+		.ipi = &apu_ipi,
+		.pwrdn_mask = 0,
+	},
+	{
+		.node_id = PM_DEV_CLUSTER1_ACPU_2,
+		.ipi = &apu_ipi,
+		.pwrdn_mask = 0,
+	},
+	{
+		.node_id = PM_DEV_CLUSTER1_ACPU_3,
+		.ipi = &apu_ipi,
+		.pwrdn_mask = 0,
+	},
+	{
+		.node_id = PM_DEV_CLUSTER2_ACPU_0,
+		.ipi = &apu_ipi,
+		.pwrdn_mask = 0,
+	},
+	{
+		.node_id = PM_DEV_CLUSTER2_ACPU_1,
+		.ipi = &apu_ipi,
+		.pwrdn_mask = 0,
+	},
+	{
+		.node_id = PM_DEV_CLUSTER2_ACPU_2,
+		.ipi = &apu_ipi,
+		.pwrdn_mask = 0,
+	},
+	{
+		.node_id = PM_DEV_CLUSTER2_ACPU_3,
+		.ipi = &apu_ipi,
+		.pwrdn_mask = 0,
+	},
+	{
+		.node_id = PM_DEV_CLUSTER3_ACPU_0,
+		.ipi = &apu_ipi,
+		.pwrdn_mask = 0,
+	},
+	{
+		.node_id = PM_DEV_CLUSTER3_ACPU_1,
+		.ipi = &apu_ipi,
+		.pwrdn_mask = 0,
+	},
+	{
+		.node_id = PM_DEV_CLUSTER3_ACPU_2,
+		.ipi = &apu_ipi,
+		.pwrdn_mask = 0,
+	},
+	{
+		.node_id = PM_DEV_CLUSTER3_ACPU_3,
+		.ipi = &apu_ipi,
+		.pwrdn_mask = 0,
+	}
+};
+
+const struct pm_proc *primary_proc = &pm_procs_all[0];
+
+/**
+ * pm_get_proc() - returns pointer to the proc structure
+ * @param cpuid	id of the cpu whose proc struct pointer should be returned
+ *
+ * @return pointer to a proc structure if proc is found, otherwise NULL
+ */
+const struct pm_proc *pm_get_proc(uint32_t cpuid)
+{
+	if (cpuid < ARRAY_SIZE(pm_procs_all)) {
+		return &pm_procs_all[cpuid];
+	}
+
+	NOTICE("ERROR: cpuid: %d proc NULL\n", cpuid);
+	return NULL;
+}
+
+/**
+ * pm_client_suspend() - Client-specific suspend actions
+ *
+ * This function should contain any PU-specific actions
+ * required prior to sending suspend request to PMU
+ * Actions taken depend on the state system is suspending to.
+ *
+ * @param proc	processor which need to suspend
+ * @param state	desired suspend state
+ */
+void pm_client_suspend(const struct pm_proc *proc, uint32_t state)
+{
+	uint32_t cpu_id = plat_my_core_pos();
+	uintptr_t val;
+
+	bakery_lock_get(&pm_client_secure_lock);
+
+	/* TODO: Set wakeup source */
+
+	val = read_cpu_pwrctrl_val();
+	val |= CORE_PWRDN_EN_BIT_MASK;
+	write_cpu_pwrctrl_val(val);
+
+	isb();
+
+	mmio_write_32(APU_PCIL_CORE_X_IEN_POWER_REG(cpu_id),
+		      APU_PCIL_CORE_X_IEN_POWER_MASK);
+
+	bakery_lock_release(&pm_client_secure_lock);
+}
+
+/**
+ * pm_get_cpuid() - get the local cpu ID for a global node ID
+ * @param nid	node id of the processor
+ *
+ * @return the cpu ID (starting from 0) for the subsystem
+ */
+static uint32_t pm_get_cpuid(uint32_t nid)
+{
+	for (size_t i = 0; i < ARRAY_SIZE(pm_procs_all); i++) {
+		if (pm_procs_all[i].node_id == nid) {
+			return i;
+		}
+	}
+	return UNDEFINED_CPUID;
+}
+
+/**
+ * pm_client_wakeup() - Client-specific wakeup actions
+ *
+ * This function should contain any PU-specific actions
+ * required for waking up another APU core
+ *
+ * @param proc	Processor which need to wakeup
+ */
+void pm_client_wakeup(const struct pm_proc *proc)
+{
+	uint32_t cpuid = pm_get_cpuid(proc->node_id);
+
+	if (cpuid == UNDEFINED_CPUID) {
+		return;
+	}
+
+	bakery_lock_get(&pm_client_secure_lock);
+
+	/* TODO: clear powerdown bit for affected cpu */
+
+	bakery_lock_release(&pm_client_secure_lock);
+}
+
+/**
+ * pm_client_abort_suspend() - Client-specific abort-suspend actions
+ *
+ * This function should contain any PU-specific actions
+ * required for aborting a prior suspend request
+ */
+void pm_client_abort_suspend(void)
+{
+	uint32_t cpu_id = plat_my_core_pos();
+	uintptr_t val;
+
+	/* Enable interrupts at processor level (for current cpu) */
+	gicv3_cpuif_enable(plat_my_core_pos());
+
+	bakery_lock_get(&pm_client_secure_lock);
+
+	/* Clear powerdown request */
+	val = read_cpu_pwrctrl_val();
+	val &= ~CORE_PWRDN_EN_BIT_MASK;
+	write_cpu_pwrctrl_val(val);
+
+	isb();
+
+	/* Disabled power down interrupt */
+	mmio_write_32(APU_PCIL_CORE_X_IDS_POWER_REG(cpu_id),
+			APU_PCIL_CORE_X_IDS_POWER_MASK);
+
+	bakery_lock_release(&pm_client_secure_lock);
+}
diff --git a/plat/xilinx/versal_net/sip_svc_setup.c b/plat/xilinx/versal_net/sip_svc_setup.c
new file mode 100644
index 0000000..0e3940f
--- /dev/null
+++ b/plat/xilinx/versal_net/sip_svc_setup.c
@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, Xilinx, Inc. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/* Top level SMC handler for SiP calls. Dispatch PM calls to PM SMC handler. */
+
+#include <errno.h>
+
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <tools_share/uuid.h>
+
+#include "ipi_mailbox_svc.h"
+#include "plat_private.h"
+#include "pm_svc_main.h"
+
+/* SMC function IDs for SiP Service queries */
+#define VERSAL_NET_SIP_SVC_CALL_COUNT	(0x8200ff00U)
+#define VERSAL_NET_SIP_SVC_UID		(0x8200ff01U)
+#define VERSAL_NET_SIP_SVC_VERSION	(0x8200ff03U)
+
+/* SiP Service Calls version numbers */
+#define SIP_SVC_VERSION_MAJOR		(0U)
+#define SIP_SVC_VERSION_MINOR		(1U)
+
+/* These macros are used to identify PM calls from the SMC function ID */
+#define PM_FID_MASK	0xf000u
+#define PM_FID_VALUE	0u
+#define IPI_FID_VALUE	0x1000u
+#define is_pm_fid(_fid)	(((_fid) & PM_FID_MASK) == PM_FID_VALUE)
+#define is_ipi_fid(_fid) (((_fid) & PM_FID_MASK) == IPI_FID_VALUE)
+
+/* SiP Service UUID */
+DEFINE_SVC_UUID2(versal_net_sip_uuid,
+		0x80d4c25a, 0xebaf, 0x11eb, 0x94, 0x68,
+		0x0b, 0x4e, 0x3b, 0x8f, 0xc3, 0x60);
+
+/**
+ * sip_svc_setup() - Setup SiP Service
+ */
+static int32_t sip_svc_setup(void)
+{
+	return sip_svc_setup_init();
+}
+
+/*
+ * sip_svc_smc_handler() - Top-level SiP Service SMC handler
+ *
+ * Handler for all SiP SMC calls. Handles standard SIP requests
+ * and calls PM SMC handler if the call is for a PM-API function.
+ */
+static uintptr_t sip_svc_smc_handler(uint32_t smc_fid,
+			     u_register_t x1,
+			     u_register_t x2,
+			     u_register_t x3,
+			     u_register_t x4,
+			     void *cookie,
+			     void *handle,
+			     u_register_t flags)
+{
+	/* Let PM SMC handler deal with PM-related requests */
+	if (is_pm_fid(smc_fid)) {
+		return smc_handler(smc_fid, x1, x2, x3, x4, cookie, handle,
+				flags);
+	}
+
+	/* Let IPI SMC handler deal with IPI-related requests if platform */
+	if (is_ipi_fid(smc_fid)) {
+		return ipi_smc_handler(smc_fid, x1, x2, x3, x4, cookie, handle, flags);
+	}
+
+	/* Let PM SMC handler deal with PM-related requests */
+	switch (smc_fid) {
+	case VERSAL_NET_SIP_SVC_CALL_COUNT:
+		/* PM functions + default functions */
+		SMC_RET1(handle, 2);
+
+	case VERSAL_NET_SIP_SVC_UID:
+		SMC_UUID_RET(handle, versal_net_sip_uuid);
+
+	case VERSAL_NET_SIP_SVC_VERSION:
+		SMC_RET2(handle, SIP_SVC_VERSION_MAJOR, SIP_SVC_VERSION_MINOR);
+
+	default:
+		WARN("Unimplemented SiP Service Call: 0x%x\n", smc_fid);
+		SMC_RET1(handle, SMC_UNK);
+	}
+}
+
+/* Register PM Service Calls as runtime service */
+DECLARE_RT_SVC(
+		sip_svc,
+		OEN_SIP_START,
+		OEN_SIP_END,
+		SMC_TYPE_FAST,
+		sip_svc_setup,
+		sip_svc_smc_handler);
diff --git a/plat/xilinx/versal_net/versal_net_gicv3.c b/plat/xilinx/versal_net/versal_net_gicv3.c
new file mode 100644
index 0000000..b7ac6ab
--- /dev/null
+++ b/plat/xilinx/versal_net/versal_net_gicv3.c
@@ -0,0 +1,222 @@
+/*
+ * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, Xilinx, Inc. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <common/interrupt_props.h>
+#include <drivers/arm/gicv3.h>
+#include <lib/utils.h>
+#include <plat/common/platform.h>
+
+#include <plat_private.h>
+#include <platform_def.h>
+
+/******************************************************************************
+ * The following functions are defined as weak to allow a platform to override
+ * the way the GICv3 driver is initialised and used.
+ *****************************************************************************/
+#pragma weak plat_versal_net_gic_driver_init
+#pragma weak plat_versal_net_gic_init
+#pragma weak plat_versal_net_gic_cpuif_enable
+#pragma weak plat_versal_net_gic_cpuif_disable
+#pragma weak plat_versal_net_gic_pcpu_init
+#pragma weak plat_versal_net_gic_redistif_on
+#pragma weak plat_versal_net_gic_redistif_off
+
+/* The GICv3 driver only needs to be initialized in EL3 */
+static uintptr_t rdistif_base_addrs[PLATFORM_CORE_COUNT];
+
+static const uintptr_t gicr_base_addrs[2] = {
+	PLAT_VERSAL_NET_GICR_BASE,	/* GICR Base address of the primary CPU */
+	0U				/* Zero Termination */
+};
+
+/* List of zero terminated GICR frame addresses which CPUs will probe */
+static const uintptr_t *gicr_frames;
+
+static const interrupt_prop_t versal_net_interrupt_props[] = {
+	PLAT_VERSAL_NET_G1S_IRQ_PROPS(INTR_GROUP1S),
+	PLAT_VERSAL_NET_G0_IRQ_PROPS(INTR_GROUP0)
+};
+
+/*
+ * We save and restore the GICv3 context on system suspend. Allocate the
+ * data in the designated EL3 Secure carve-out memory.
+ */
+static gicv3_redist_ctx_t rdist_ctx __section("versal_net_el3_tzc_dram");
+static gicv3_dist_ctx_t dist_ctx __section("versal_net_el3_tzc_dram");
+
+/*
+ * MPIDR hashing function for translating MPIDRs read from GICR_TYPER register
+ * to core position.
+ *
+ * Calculating core position is dependent on MPIDR_EL1.MT bit. However, affinity
+ * values read from GICR_TYPER don't have an MT field. To reuse the same
+ * translation used for CPUs, we insert MT bit read from the PE's MPIDR into
+ * that read from GICR_TYPER.
+ *
+ * Assumptions:
+ *
+ *   - All CPUs implemented in the system have MPIDR_EL1.MT bit set;
+ *   - No CPUs implemented in the system use affinity level 3.
+ */
+static uint32_t versal_net_gicv3_mpidr_hash(u_register_t mpidr)
+{
+	mpidr |= (read_mpidr_el1() & MPIDR_MT_MASK);
+	return plat_core_pos_by_mpidr(mpidr);
+}
+
+static const gicv3_driver_data_t versal_net_gic_data __unused = {
+	.gicd_base = PLAT_VERSAL_NET_GICD_BASE,
+	.gicr_base = 0U,
+	.interrupt_props = versal_net_interrupt_props,
+	.interrupt_props_num = ARRAY_SIZE(versal_net_interrupt_props),
+	.rdistif_num = PLATFORM_CORE_COUNT,
+	.rdistif_base_addrs = rdistif_base_addrs,
+	.mpidr_to_core_pos = versal_net_gicv3_mpidr_hash
+};
+
+void __init plat_versal_net_gic_driver_init(void)
+{
+	/*
+	 * The GICv3 driver is initialized in EL3 and does not need
+	 * to be initialized again in SEL1. This is because the S-EL1
+	 * can use GIC system registers to manage interrupts and does
+	 * not need GIC interface base addresses to be configured.
+	 */
+#if IMAGE_BL31
+	gicv3_driver_init(&versal_net_gic_data);
+	gicr_frames = gicr_base_addrs;
+
+	if (gicv3_rdistif_probe(gicr_frames[0]) == -1) {
+		ERROR("No GICR base frame found for Primary CPU\n");
+		panic();
+	}
+#endif
+}
+
+/******************************************************************************
+ * Versal NET common helper to initialize the GIC. Only invoked by BL31
+ *****************************************************************************/
+void __init plat_versal_net_gic_init(void)
+{
+	gicv3_distif_init();
+	gicv3_rdistif_init(plat_my_core_pos());
+	gicv3_cpuif_enable(plat_my_core_pos());
+}
+
+/******************************************************************************
+ * Versal NET common helper to enable the GIC CPU interface
+ *****************************************************************************/
+void plat_versal_net_gic_cpuif_enable(void)
+{
+	gicv3_cpuif_enable(plat_my_core_pos());
+}
+
+/******************************************************************************
+ * Versal NET common helper to disable the GIC CPU interface
+ *****************************************************************************/
+void plat_versal_net_gic_cpuif_disable(void)
+{
+	gicv3_cpuif_disable(plat_my_core_pos());
+}
+
+/******************************************************************************
+ * Versal NET common helper to initialize the per-cpu redistributor interface in
+ * GICv3
+ *****************************************************************************/
+void plat_versal_net_gic_pcpu_init(void)
+{
+	int32_t result;
+	const uintptr_t *plat_gicr_frames = gicr_frames;
+
+	do {
+		result = gicv3_rdistif_probe(*plat_gicr_frames);
+
+		/* If the probe is successful, no need to proceed further */
+		if (result == 0) {
+			break;
+		}
+
+		plat_gicr_frames++;
+	} while (*plat_gicr_frames != 0U);
+
+	if (result == -1) {
+		ERROR("No GICR base frame found for CPU 0x%lx\n", read_mpidr());
+		panic();
+	}
+
+	gicv3_rdistif_init(plat_my_core_pos());
+}
+
+/******************************************************************************
+ * Versal NET common helpers to power GIC redistributor interface
+ *****************************************************************************/
+void plat_versal_net_gic_redistif_on(void)
+{
+	gicv3_rdistif_on(plat_my_core_pos());
+}
+
+void plat_versal_net_gic_redistif_off(void)
+{
+	gicv3_rdistif_off(plat_my_core_pos());
+}
+
+/******************************************************************************
+ * Versal NET common helper to save & restore the GICv3 on resume from system
+ * suspend
+ *****************************************************************************/
+void plat_versal_net_gic_save(void)
+{
+	/*
+	 * If an ITS is available, save its context before
+	 * the Redistributor using:
+	 * gicv3_its_save_disable(gits_base, &its_ctx[i])
+	 * Additionnaly, an implementation-defined sequence may
+	 * be required to save the whole ITS state.
+	 */
+
+	/*
+	 * Save the GIC Redistributors and ITS contexts before the
+	 * Distributor context. As we only handle SYSTEM SUSPEND API,
+	 * we only need to save the context of the CPU that is issuing
+	 * the SYSTEM SUSPEND call, i.e. the current CPU.
+	 */
+	gicv3_rdistif_save(plat_my_core_pos(), &rdist_ctx);
+
+	/* Save the GIC Distributor context */
+	gicv3_distif_save(&dist_ctx);
+
+	/*
+	 * From here, all the components of the GIC can be safely powered down
+	 * as long as there is an alternate way to handle wakeup interrupt
+	 * sources.
+	 */
+}
+
+void plat_versal_net_gic_resume(void)
+{
+	/* Restore the GIC Distributor context */
+	gicv3_distif_init_restore(&dist_ctx);
+
+	/*
+	 * Restore the GIC Redistributor and ITS contexts after the
+	 * Distributor context. As we only handle SYSTEM SUSPEND API,
+	 * we only need to restore the context of the CPU that issued
+	 * the SYSTEM SUSPEND call.
+	 */
+	gicv3_rdistif_init_restore(plat_my_core_pos(), &rdist_ctx);
+
+	/*
+	 * If an ITS is available, restore its context after
+	 * the Redistributor using:
+	 * gicv3_its_restore(gits_base, &its_ctx[i])
+	 * An implementation-defined sequence may be required to
+	 * restore the whole ITS state. The ITS must also be
+	 * re-enabled after this sequence has been executed.
+	 */
+}
diff --git a/plat/xilinx/versal_net/versal_net_ipi.c b/plat/xilinx/versal_net/versal_net_ipi.c
new file mode 100644
index 0000000..26ded89
--- /dev/null
+++ b/plat/xilinx/versal_net/versal_net_ipi.c
@@ -0,0 +1,85 @@
+/*
+ * Copyright (C) 2022, Xilinx, Inc. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*
+ * Versal NET IPI agent registers access management
+ */
+
+#include <errno.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <lib/bakery_lock.h>
+#include <lib/mmio.h>
+
+#include <ipi.h>
+#include <plat_ipi.h>
+#include <plat_private.h>
+
+/* versal_net ipi configuration table */
+static const struct ipi_config versal_net_ipi_table[IPI_ID_MAX] = {
+	/* A72 IPI */
+	[IPI_ID_APU] = {
+		.ipi_bit_mask = IPI0_TRIG_BIT,
+		.ipi_reg_base = IPI0_REG_BASE,
+		.secure_only = 0,
+	},
+
+	/* PMC IPI */
+	[IPI_ID_PMC] = {
+		.ipi_bit_mask = PMC_IPI_TRIG_BIT,
+		.ipi_reg_base = IPI0_REG_BASE,
+		.secure_only = 0,
+	},
+
+	/* RPU0 IPI */
+	[IPI_ID_RPU0] = {
+		.ipi_bit_mask = IPI1_TRIG_BIT,
+		.ipi_reg_base = IPI1_REG_BASE,
+		.secure_only = 0,
+	},
+
+	/* RPU1 IPI */
+	[IPI_ID_RPU1] = {
+		.ipi_bit_mask = IPI2_TRIG_BIT,
+		.ipi_reg_base = IPI2_REG_BASE,
+		.secure_only = 0,
+	},
+
+	/* IPI3 IPI */
+	[IPI_ID_3] = {
+		.ipi_bit_mask = IPI3_TRIG_BIT,
+		.ipi_reg_base = IPI3_REG_BASE,
+		.secure_only = 0,
+	},
+
+	/* IPI4 IPI */
+	[IPI_ID_4] = {
+		.ipi_bit_mask = IPI4_TRIG_BIT,
+		.ipi_reg_base = IPI4_REG_BASE,
+		.secure_only = 0,
+	},
+
+	/* IPI5 IPI */
+	[IPI_ID_5] = {
+		.ipi_bit_mask = IPI5_TRIG_BIT,
+		.ipi_reg_base = IPI5_REG_BASE,
+		.secure_only = 0,
+	},
+};
+
+/* versal_net_ipi_config_table_init() - Initialize versal_net IPI configuration data
+ *
+ * @ipi_config_table  - IPI configuration table
+ * @ipi_total - Total number of IPI available
+ *
+ */
+void versal_net_ipi_config_table_init(void)
+{
+	ipi_config_table_init(versal_net_ipi_table, ARRAY_SIZE(versal_net_ipi_table));
+}
diff --git a/plat/xilinx/zynqmp/platform.mk b/plat/xilinx/zynqmp/platform.mk
index dd82bc0..05adbd0 100644
--- a/plat/xilinx/zynqmp/platform.mk
+++ b/plat/xilinx/zynqmp/platform.mk
@@ -1,5 +1,6 @@
 #
 # Copyright (c) 2013-2021, ARM Limited and Contributors. All rights reserved.
+# Portions copyright (c) 2021-2022, ProvenRun S.A.S. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 
@@ -11,11 +12,20 @@
 ZYNQMP_WDT_RESTART := 0
 IPI_CRC_CHECK := 0
 override RESET_TO_BL31 := 1
-override GICV2_G0_FOR_EL3 := 1
 override WARMBOOT_ENABLE_DCACHE_EARLY := 1
 
 EL3_EXCEPTION_HANDLING := $(SDEI_SUPPORT)
 
+# pncd SPD requires secure SGI to be handled at EL1
+ifeq (${SPD},pncd)
+ifeq (${ZYNQMP_WDT_RESTART},1)
+$(error "Error: ZYNQMP_WDT_RESTART and SPD=pncd are incompatible")
+endif
+override GICV2_G0_FOR_EL3 := 0
+else
+override GICV2_G0_FOR_EL3 := 1
+endif
+
 # Do not enable SVE
 ENABLE_SVE_FOR_NS	:= 0
 
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_clock.c b/plat/xilinx/zynqmp/pm_service/pm_api_clock.c
index c6bed7d..0099070 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_clock.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_clock.c
@@ -2400,7 +2400,7 @@
 {
 	unsigned int i;
 
-	for (i = 0; i < ARRAY_SIZE(pm_clk_invalid_list); i++)
+	for (i = 0U; i < ARRAY_SIZE(pm_clk_invalid_list); i++)
 		if (pm_clk_invalid_list[i] == clock_id)
 			return 0;
 
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_clock.h b/plat/xilinx/zynqmp/pm_service/pm_api_clock.h
index bc15592..db476e8 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_clock.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_clock.h
@@ -15,19 +15,19 @@
 
 #include "pm_common.h"
 
-#define CLK_NAME_LEN		U(15)
-#define MAX_PARENTS		U(100)
+#define CLK_NAME_LEN		(15U)
+#define MAX_PARENTS		(100U)
 #define CLK_NA_PARENT		-1
 #define CLK_DUMMY_PARENT	-2
 
 /* Flags for parent id */
-#define PARENT_CLK_SELF		U(0)
-#define PARENT_CLK_NODE1	U(1)
-#define PARENT_CLK_NODE2	U(2)
-#define PARENT_CLK_NODE3	U(3)
-#define PARENT_CLK_NODE4	U(4)
-#define PARENT_CLK_EXTERNAL	U(5)
-#define PARENT_CLK_MIO0_MIO77	U(6)
+#define PARENT_CLK_SELF		(0U)
+#define PARENT_CLK_NODE1	(1U)
+#define PARENT_CLK_NODE2	(2U)
+#define PARENT_CLK_NODE3	(3U)
+#define PARENT_CLK_NODE4	(4U)
+#define PARENT_CLK_EXTERNAL	(5U)
+#define PARENT_CLK_MIO0_MIO77	(6U)
 
 #define CLK_SET_RATE_GATE	BIT(0) /* must be gated across rate change */
 #define CLK_SET_PARENT_GATE	BIT(1) /* must be gated across re-parent */
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c b/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c
index 0192f81..945d060 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c
@@ -2037,7 +2037,7 @@
 {
 	uint16_t grps;
 	uint16_t end_of_grp_offset;
-	unsigned int i;
+	uint16_t i;
 
 	if (fid >= MAX_FUNCTION) {
 		return PM_RET_ERROR_ARGS;
@@ -2048,7 +2048,7 @@
 	grps = pinctrl_functions[fid].group_base;
 	end_of_grp_offset = grps + pinctrl_functions[fid].group_size;
 
-	for (i = 0; i < NUM_GROUPS_PER_RESP; i++) {
+	for (i = 0U; i < NUM_GROUPS_PER_RESP; i++) {
 		if ((grps + index + i) >= end_of_grp_offset) {
 			break;
 		}
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.h b/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.h
index b3159c2..1b46375 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.h
@@ -13,9 +13,9 @@
 
 #include "pm_common.h"
 
-#define FUNCTION_NAME_LEN		U(16)
-#define GROUPS_PAYLOAD_LEN		U(12)
-#define NUM_GROUPS_PER_RESP		U(6)
+#define FUNCTION_NAME_LEN		(16U)
+#define GROUPS_PAYLOAD_LEN		(12U)
+#define NUM_GROUPS_PER_RESP		(6U)
 #define END_OF_FUNCTION			"END_OF_FUNCTION"
 #define END_OF_GROUPS			-1
 #define PINCTRL_GRP_RESERVED		-2
diff --git a/plat/xilinx/zynqmp/pm_service/pm_defs.h b/plat/xilinx/zynqmp/pm_service/pm_defs.h
index bf5ecfe..e335b94 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_defs.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_defs.h
@@ -256,19 +256,19 @@
  * @PM_RET_ERROR_NODE_USED:	node is already in use
  */
 enum pm_ret_status {
-	PM_RET_SUCCESS,
-	PM_RET_ERROR_ARGS = 1,
-	PM_RET_ERROR_NOTSUPPORTED = 4,
-	PM_RET_ERROR_NOT_ENABLED = 29,
-	PM_RET_ERROR_INTERNAL = 2000,
-	PM_RET_ERROR_CONFLICT = 2001,
-	PM_RET_ERROR_ACCESS = 2002,
-	PM_RET_ERROR_INVALID_NODE = 2003,
-	PM_RET_ERROR_DOUBLE_REQ = 2004,
-	PM_RET_ERROR_ABORT_SUSPEND = 2005,
-	PM_RET_ERROR_TIMEOUT = 2006,
-	PM_RET_ERROR_NODE_USED = 2007,
-	PM_RET_ERROR_NO_FEATURE = 2008
+	PM_RET_SUCCESS = (0U),
+	PM_RET_ERROR_ARGS = (1U),
+	PM_RET_ERROR_NOTSUPPORTED = (4U),
+	PM_RET_ERROR_NOT_ENABLED = (29U),
+	PM_RET_ERROR_INTERNAL = (2000U),
+	PM_RET_ERROR_CONFLICT = (2001U),
+	PM_RET_ERROR_ACCESS = (2002U),
+	PM_RET_ERROR_INVALID_NODE = (2003U),
+	PM_RET_ERROR_DOUBLE_REQ = (2004U),
+	PM_RET_ERROR_ABORT_SUSPEND = (2005U),
+	PM_RET_ERROR_TIMEOUT = (2006U),
+	PM_RET_ERROR_NODE_USED = (2007U),
+	PM_RET_ERROR_NO_FEATURE = (2008U)
 };
 
 /**
diff --git a/services/spd/pncd/pncd.mk b/services/spd/pncd/pncd.mk
new file mode 100644
index 0000000..0f8eb25
--- /dev/null
+++ b/services/spd/pncd/pncd.mk
@@ -0,0 +1,24 @@
+# Copyright (c) 2021-2022, ProvenRun S.A.S. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+PNCD_DIR		:=	services/spd/pncd
+SPD_INCLUDES		:=	-Iinclude/bl32/pnc
+SPD_INCLUDES		+=	-Iinclude/common/
+
+SPD_SOURCES		:=	services/spd/pncd/pncd_common.c		\
+				services/spd/pncd/pncd_helpers.S	\
+				services/spd/pncd/pncd_main.c
+
+NEED_BL32		:=	yes
+
+# The following constants need to be defined:
+#   - SPD_PNCD_NS_IRQ: IRQ number used to notify NS world when SMC_ACTION_FROM_S is received
+#   - SPD_PNCD_S_IRQ: IRQ number used to notify S world when SMC_ACTION_FROM_NS is received
+$(eval $(call assert_numerics, SPD_PNCD_NS_IRQ SPD_PNCD_S_IRQ))
+
+$(eval $(call add_defines,\
+    $(sort \
+        SPD_PNCD_NS_IRQ \
+        SPD_PNCD_S_IRQ \
+)))
diff --git a/services/spd/pncd/pncd_common.c b/services/spd/pncd/pncd_common.c
new file mode 100644
index 0000000..6fdb629
--- /dev/null
+++ b/services/spd/pncd/pncd_common.c
@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2021-2022, ProvenRun S.A.S. 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 <lib/el3_runtime/context_mgmt.h>
+#include <lib/utils.h>
+#include <plat/common/platform.h>
+
+#include "pncd_private.h"
+
+/*******************************************************************************
+ * Given a secure payload entrypoint info pointer, entry point PC & pointer to a
+ * context data structure, this function will initialize pnc context and entry
+ * point info for the secure payload
+ ******************************************************************************/
+void pncd_init_pnc_ep_state(struct entry_point_info *pnc_entry_point,
+				uint64_t pc,
+				pnc_context_t *pnc_ctx)
+{
+	uint32_t ep_attr;
+
+	/* Passing a NULL context is a critical programming error */
+	assert(pnc_ctx);
+	assert(pnc_entry_point);
+	assert(pc);
+
+	/* Associate this context with the current cpu */
+	pnc_ctx->mpidr = read_mpidr();
+
+	cm_set_context(&pnc_ctx->cpu_ctx, SECURE);
+
+	/* initialise an entrypoint to set up the CPU context */
+	ep_attr = SECURE | EP_ST_ENABLE;
+	if (read_sctlr_el3() & SCTLR_EE_BIT) {
+		ep_attr |= EP_EE_BIG;
+	}
+	SET_PARAM_HEAD(pnc_entry_point, PARAM_EP, VERSION_1, ep_attr);
+
+	pnc_entry_point->pc = pc;
+	pnc_entry_point->spsr = SPSR_64(MODE_EL1,
+					MODE_SP_ELX,
+					DISABLE_ALL_EXCEPTIONS);
+	memset(&pnc_entry_point->args, 0, sizeof(pnc_entry_point->args));
+}
+
+/*******************************************************************************
+ * This function takes an SP context pointer and:
+ * 1. Applies the S-EL1 system register context from pnc_ctx->cpu_ctx.
+ * 2. Saves the current C runtime state (callee saved registers) on the stack
+ *    frame and saves a reference to this state.
+ * 3. Calls el3_exit() so that the EL3 system and general purpose registers
+ *    from the pnc_ctx->cpu_ctx are used to enter the secure payload image.
+ ******************************************************************************/
+uint64_t pncd_synchronous_sp_entry(pnc_context_t *pnc_ctx)
+{
+	assert(pnc_ctx != NULL);
+	assert(pnc_ctx->c_rt_ctx == 0U);
+
+	/* Apply the Secure EL1 system register context and switch to it */
+	assert(cm_get_context(SECURE) == &pnc_ctx->cpu_ctx);
+	cm_el1_sysregs_context_restore(SECURE);
+#if CTX_INCLUDE_FPREGS
+	fpregs_context_restore(get_fpregs_ctx(cm_get_context(SECURE)));
+#endif
+	cm_set_next_eret_context(SECURE);
+
+	return pncd_enter_sp(&pnc_ctx->c_rt_ctx);
+}
+
+
+/*******************************************************************************
+ * This function takes an SP context pointer and:
+ * 1. Saves the S-EL1 system register context tp pnc_ctx->cpu_ctx.
+ * 2. Restores the current C runtime state (callee saved registers) from the
+ *    stack frame using the reference to this state saved in pncd_enter_sp().
+ * 3. It does not need to save any general purpose or EL3 system register state
+ *    as the generic smc entry routine should have saved those.
+ ******************************************************************************/
+void pncd_synchronous_sp_exit(pnc_context_t *pnc_ctx, uint64_t ret)
+{
+	assert(pnc_ctx != NULL);
+	/* Save the Secure EL1 system register context */
+	assert(cm_get_context(SECURE) == &pnc_ctx->cpu_ctx);
+	cm_el1_sysregs_context_save(SECURE);
+#if CTX_INCLUDE_FPREGS
+	fpregs_context_save(get_fpregs_ctx(cm_get_context(SECURE)));
+#endif
+
+	assert(pnc_ctx->c_rt_ctx != 0);
+	pncd_exit_sp(pnc_ctx->c_rt_ctx, ret);
+
+	/* Should never reach here */
+	panic();
+}
diff --git a/services/spd/pncd/pncd_helpers.S b/services/spd/pncd/pncd_helpers.S
new file mode 100644
index 0000000..736b30f
--- /dev/null
+++ b/services/spd/pncd/pncd_helpers.S
@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2021-2022, ProvenRun S.A.S. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+#include "pncd_private.h"
+
+	.global	pncd_enter_sp
+	/* ---------------------------------------------
+	 * This function is called with SP_EL0 as stack.
+	 * Here we stash our EL3 callee-saved registers
+	 * on to the stack as a part of saving the C
+	 * runtime and enter the secure payload.
+	 * 'x0' contains a pointer to the memory where
+	 * the address of the C runtime context is to be
+	 * saved.
+	 * ---------------------------------------------
+	 */
+func pncd_enter_sp
+	/* Make space for the registers that we're going to save */
+	mov	x3, sp
+	str	x3, [x0, #0]
+	sub	sp, sp, #PNCD_C_RT_CTX_SIZE
+
+	/* Save callee-saved registers on to the stack */
+	stp	x19, x20, [sp, #PNCD_C_RT_CTX_X19]
+	stp	x21, x22, [sp, #PNCD_C_RT_CTX_X21]
+	stp	x23, x24, [sp, #PNCD_C_RT_CTX_X23]
+	stp	x25, x26, [sp, #PNCD_C_RT_CTX_X25]
+	stp	x27, x28, [sp, #PNCD_C_RT_CTX_X27]
+	stp	x29, x30, [sp, #PNCD_C_RT_CTX_X29]
+
+	/* ---------------------------------------------
+	 * Everything is setup now. el3_exit() will
+	 * use the secure context to restore to the
+	 * general purpose and EL3 system registers to
+	 * ERET into the secure payload.
+	 * ---------------------------------------------
+	 */
+	b	el3_exit
+endfunc pncd_enter_sp
+
+	/* ---------------------------------------------
+	 * This function is called 'x0' pointing to a C
+	 * runtime context saved in pncd_enter_sp(). It
+	 * restores the saved registers and jumps to
+	 * that runtime with 'x0' as the new sp. This
+	 * destroys the C runtime context that had been
+	 * built on the stack below the saved context by
+	 * the caller. Later the second parameter 'x1'
+	 * is passed as return value to the caller
+	 * ---------------------------------------------
+	 */
+	.global pncd_exit_sp
+func pncd_exit_sp
+	/* Restore the previous stack */
+	mov	sp, x0
+
+	/* Restore callee-saved registers on to the stack */
+	ldp	x19, x20, [x0, #(PNCD_C_RT_CTX_X19 - PNCD_C_RT_CTX_SIZE)]
+	ldp	x21, x22, [x0, #(PNCD_C_RT_CTX_X21 - PNCD_C_RT_CTX_SIZE)]
+	ldp	x23, x24, [x0, #(PNCD_C_RT_CTX_X23 - PNCD_C_RT_CTX_SIZE)]
+	ldp	x25, x26, [x0, #(PNCD_C_RT_CTX_X25 - PNCD_C_RT_CTX_SIZE)]
+	ldp	x27, x28, [x0, #(PNCD_C_RT_CTX_X27 - PNCD_C_RT_CTX_SIZE)]
+	ldp	x29, x30, [x0, #(PNCD_C_RT_CTX_X29 - PNCD_C_RT_CTX_SIZE)]
+
+	/* ---------------------------------------------
+	 * This should take us back to the instruction
+	 * after the call to the last pncd_enter_sp().
+	 * Place the second parameter to x0 so that the
+	 * caller will see it as a return value from the
+	 * original entry call
+	 * ---------------------------------------------
+	 */
+	mov	x0, x1
+	ret
+endfunc pncd_exit_sp
diff --git a/services/spd/pncd/pncd_main.c b/services/spd/pncd/pncd_main.c
new file mode 100644
index 0000000..99c4aa1
--- /dev/null
+++ b/services/spd/pncd/pncd_main.c
@@ -0,0 +1,471 @@
+/*
+ * Copyright (c) 2021-2022, ProvenRun S.A.S. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*******************************************************************************
+ * This is the Secure Payload Dispatcher (SPD). The dispatcher is meant to be a
+ * plug-in component to the Secure Monitor, registered as a runtime service. The
+ * SPD is expected to be a functional extension of the Secure Payload (SP) that
+ * executes in Secure EL1. The Secure Monitor will delegate all SMCs targeting
+ * the Trusted OS/Applications range to the dispatcher. The SPD will either
+ * handle the request locally or delegate it to the Secure Payload. It is also
+ * responsible for initialising and maintaining communication with the SP.
+ ******************************************************************************/
+
+#include <assert.h>
+#include <errno.h>
+#include <stddef.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <bl31/bl31.h>
+#include <bl31/interrupt_mgmt.h>
+#include <bl_common.h>
+#include <common/debug.h>
+#include <common/ep_info.h>
+#include <drivers/arm/gic_common.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/spinlock.h>
+#include <plat/common/platform.h>
+#include <pnc.h>
+#include "pncd_private.h"
+#include <runtime_svc.h>
+#include <tools_share/uuid.h>
+
+/*******************************************************************************
+ * Structure to keep track of ProvenCore state
+ ******************************************************************************/
+static pnc_context_t pncd_sp_context;
+
+static bool ree_info;
+static uint64_t ree_base_addr;
+static uint64_t ree_length;
+static uint64_t ree_tag;
+
+static bool pnc_initialized;
+
+static spinlock_t smc_handler_lock;
+
+static int pncd_init(void);
+
+static void context_save(unsigned long security_state)
+{
+	assert(sec_state_is_valid(security_state));
+
+	cm_el1_sysregs_context_save((uint32_t) security_state);
+#if CTX_INCLUDE_FPREGS
+	fpregs_context_save(get_fpregs_ctx(cm_get_context(security_state)));
+#endif
+}
+
+static void *context_restore(unsigned long security_state)
+{
+	void *handle;
+
+	assert(sec_state_is_valid(security_state));
+
+	/* Get a reference to the next context */
+	handle = cm_get_context((uint32_t) security_state);
+	assert(handle);
+
+	/* Restore state */
+	cm_el1_sysregs_context_restore((uint32_t) security_state);
+#if CTX_INCLUDE_FPREGS
+	fpregs_context_restore(get_fpregs_ctx(cm_get_context(security_state)));
+#endif
+
+	cm_set_next_eret_context((uint32_t) security_state);
+
+	return handle;
+}
+
+static uint64_t pncd_sel1_interrupt_handler(uint32_t id,
+		uint32_t flags, void *handle, void *cookie);
+
+/*******************************************************************************
+ * Switch context to the specified security state and return the targeted
+ * handle. Note that the context may remain unchanged if the switch is not
+ * allowed.
+ ******************************************************************************/
+void *pncd_context_switch_to(unsigned long security_state)
+{
+	unsigned long sec_state_from =
+	    security_state == SECURE ? NON_SECURE : SECURE;
+
+	assert(sec_state_is_valid(security_state));
+
+	/* Check if this is the first world switch */
+	if (!pnc_initialized) {
+		int rc;
+		uint32_t flags;
+
+		assert(sec_state_from == SECURE);
+
+		INFO("PnC initialization done\n");
+
+		/*
+		 * Register an interrupt handler for S-EL1 interrupts
+		 * when generated during code executing in the
+		 * non-secure state.
+		 */
+		flags = 0U;
+		set_interrupt_rm_flag(flags, NON_SECURE);
+		rc = register_interrupt_type_handler(INTR_TYPE_S_EL1,
+				pncd_sel1_interrupt_handler,
+				flags);
+		if (rc != 0) {
+			ERROR("Failed to register S-EL1 interrupt handler (%d)\n",
+			      rc);
+			panic();
+		}
+
+		context_save(SECURE);
+
+		pnc_initialized = true;
+
+		/*
+		 * Release the lock before restoring the EL3 context to
+		 * bl31_main.
+		 */
+		spin_unlock(&smc_handler_lock);
+
+		/*
+		 * SP reports completion. The SPD must have initiated
+		 * the original request through a synchronous entry
+		 * into the SP. Jump back to the original C runtime
+		 * context.
+		 */
+		pncd_synchronous_sp_exit(&pncd_sp_context, (uint64_t) 0x0);
+
+		/* Unreachable */
+		ERROR("Returned from pncd_synchronous_sp_exit... Should not happen\n");
+		panic();
+	}
+
+	/* Check that the world switch is allowed */
+	if (read_mpidr() != pncd_sp_context.mpidr) {
+		if (sec_state_from == SECURE) {
+			/*
+			 * Secure -> Non-Secure world switch initiated on a CPU where there
+			 * should be no Trusted OS running
+			 */
+			WARN("Secure to Non-Secure switch requested on CPU where ProvenCore is not supposed to be running...\n");
+		}
+
+		/*
+		 * Secure or Non-Secure world wants to switch world but there is no Secure
+		 * software on this core
+		 */
+		return cm_get_context((uint32_t) sec_state_from);
+	}
+
+	context_save(sec_state_from);
+
+	return context_restore(security_state);
+}
+
+/*******************************************************************************
+ * This function is the handler registered for S-EL1 interrupts by the PNCD. It
+ * validates the interrupt and upon success arranges entry into the PNC at
+ * 'pnc_sel1_intr_entry()' for handling the interrupt.
+ ******************************************************************************/
+static uint64_t pncd_sel1_interrupt_handler(uint32_t id,
+		uint32_t flags,
+		void *handle,
+		void *cookie)
+{
+	/* Check the security state when the exception was generated */
+	assert(get_interrupt_src_ss(flags) == NON_SECURE);
+
+	/* Sanity check the pointer to this cpu's context */
+	assert(handle == cm_get_context(NON_SECURE));
+
+	/* switch to PnC */
+	handle = pncd_context_switch_to(SECURE);
+
+	assert(handle != NULL);
+
+	SMC_RET0(handle);
+}
+
+#pragma weak plat_pncd_setup
+int plat_pncd_setup(void)
+{
+	return 0;
+}
+
+/*******************************************************************************
+ * Secure Payload Dispatcher setup. The SPD finds out the SP entrypoint and type
+ * (aarch32/aarch64) if not already known and initialises the context for entry
+ * into the SP for its initialisation.
+ ******************************************************************************/
+static int pncd_setup(void)
+{
+	entry_point_info_t *pnc_ep_info;
+
+	/*
+	 * Get information about the Secure Payload (BL32) image. Its
+	 * absence is a critical failure.
+	 *
+	 * TODO: Add support to conditionally include the SPD service
+	 */
+	pnc_ep_info = bl31_plat_get_next_image_ep_info(SECURE);
+	if (!pnc_ep_info) {
+		WARN("No PNC provided by BL2 boot loader, Booting device without PNC initialization. SMC`s destined for PNC will return SMC_UNK\n");
+		return 1;
+	}
+
+	/*
+	 * If there's no valid entry point for SP, we return a non-zero value
+	 * signalling failure initializing the service. We bail out without
+	 * registering any handlers
+	 */
+	if (!pnc_ep_info->pc) {
+		return 1;
+	}
+
+	pncd_init_pnc_ep_state(pnc_ep_info,
+			pnc_ep_info->pc,
+			&pncd_sp_context);
+
+	/*
+	 * All PNCD initialization done. Now register our init function with
+	 * BL31 for deferred invocation
+	 */
+	bl31_register_bl32_init(&pncd_init);
+	bl31_set_next_image_type(NON_SECURE);
+
+	return plat_pncd_setup();
+}
+
+/*******************************************************************************
+ * This function passes control to the Secure Payload image (BL32) for the first
+ * time on the primary cpu after a cold boot. It assumes that a valid secure
+ * context has already been created by pncd_setup() which can be directly used.
+ * It also assumes that a valid non-secure context has been initialised by PSCI
+ * so it does not need to save and restore any non-secure state. This function
+ * performs a synchronous entry into the Secure payload. The SP passes control
+ * back to this routine through a SMC.
+ ******************************************************************************/
+static int32_t pncd_init(void)
+{
+	entry_point_info_t *pnc_entry_point;
+	uint64_t rc = 0;
+
+	/*
+	 * Get information about the Secure Payload (BL32) image. Its
+	 * absence is a critical failure.
+	 */
+	pnc_entry_point = bl31_plat_get_next_image_ep_info(SECURE);
+	assert(pnc_entry_point);
+
+	cm_init_my_context(pnc_entry_point);
+
+	/*
+	 * Arrange for an entry into the test secure payload. It will be
+	 * returned via PNC_ENTRY_DONE case
+	 */
+	rc = pncd_synchronous_sp_entry(&pncd_sp_context);
+
+	/*
+	 * If everything went well at this point, the return value should be 0.
+	 */
+	return rc == 0;
+}
+
+#pragma weak plat_pncd_smc_handler
+/*******************************************************************************
+ * This function is responsible for handling the platform-specific SMCs in the
+ * Trusted OS/App range as defined in the SMC Calling Convention Document.
+ ******************************************************************************/
+uintptr_t plat_pncd_smc_handler(uint32_t smc_fid,
+		u_register_t x1,
+		u_register_t x2,
+		u_register_t x3,
+		u_register_t x4,
+		void *cookie,
+		void *handle,
+		u_register_t flags)
+{
+	(void) smc_fid;
+	(void) x1;
+	(void) x2;
+	(void) x3;
+	(void) x4;
+	(void) cookie;
+	(void) flags;
+
+	SMC_RET1(handle, SMC_UNK);
+}
+
+/*******************************************************************************
+ * This function is responsible for handling all SMCs in the Trusted OS/App
+ * range as defined in the SMC Calling Convention Document. It is also
+ * responsible for communicating with the Secure payload to delegate work and
+ * return results back to the non-secure state. Lastly it will also return any
+ * information that the secure payload needs to do the work assigned to it.
+ *
+ * It should only be called with the smc_handler_lock held.
+ ******************************************************************************/
+static uintptr_t pncd_smc_handler_unsafe(uint32_t smc_fid,
+		u_register_t x1,
+		u_register_t x2,
+		u_register_t x3,
+		u_register_t x4,
+		void *cookie,
+		void *handle,
+		u_register_t flags)
+{
+	uint32_t ns;
+
+	/* Determine which security state this SMC originated from */
+	ns = is_caller_non_secure(flags);
+
+	assert(ns != 0 || read_mpidr() == pncd_sp_context.mpidr);
+
+	switch (smc_fid) {
+	case SMC_CONFIG_SHAREDMEM:
+		if (ree_info) {
+			/* Do not Yield */
+			SMC_RET0(handle);
+		}
+
+		/*
+		 * Fetch the physical base address (x1) and size (x2) of the
+		 * shared memory allocated by the Non-Secure world. This memory
+		 * will be used by PNC to communicate with the Non-Secure world.
+		 * Verifying the validity of these values is up to the Trusted
+		 * OS.
+		 */
+		ree_base_addr = x1 | (x2 << 32);
+		ree_length = x3;
+		ree_tag = x4;
+
+		INFO("IN SMC_CONFIG_SHAREDMEM: addr=%lx, length=%lx, tag=%lx\n",
+		     (unsigned long) ree_base_addr,
+		     (unsigned long) ree_length,
+		     (unsigned long) ree_tag);
+
+		if ((ree_base_addr % 0x200000) != 0) {
+			SMC_RET1(handle, SMC_UNK);
+		}
+
+		if ((ree_length % 0x200000) != 0) {
+			SMC_RET1(handle, SMC_UNK);
+		}
+
+		ree_info = true;
+
+		/* Do not Yield */
+		SMC_RET4(handle, 0, 0, 0, 0);
+
+		break;
+
+	case SMC_GET_SHAREDMEM:
+		if (ree_info) {
+			x1 = (1U << 16) | ree_tag;
+			x2 = ree_base_addr & 0xFFFFFFFF;
+			x3 = (ree_base_addr >> 32) & 0xFFFFFFFF;
+			x4 = ree_length & 0xFFFFFFFF;
+			SMC_RET4(handle, x1, x2, x3, x4);
+		} else {
+			SMC_RET4(handle, 0, 0, 0, 0);
+		}
+
+		break;
+
+	case SMC_ACTION_FROM_NS:
+		if (ns == 0) {
+			SMC_RET1(handle, SMC_UNK);
+		}
+
+		if (SPD_PNCD_S_IRQ < MIN_PPI_ID) {
+			plat_ic_raise_s_el1_sgi(SPD_PNCD_S_IRQ,
+						pncd_sp_context.mpidr);
+		} else {
+			plat_ic_set_interrupt_pending(SPD_PNCD_S_IRQ);
+		}
+
+		SMC_RET0(handle);
+
+		break;
+
+	case SMC_ACTION_FROM_S:
+		if (ns != 0) {
+			SMC_RET1(handle, SMC_UNK);
+		}
+
+		if (SPD_PNCD_NS_IRQ < MIN_PPI_ID) {
+			/*
+			 * NS SGI is sent to the same core as the one running
+			 * PNC
+			 */
+			plat_ic_raise_ns_sgi(SPD_PNCD_NS_IRQ, read_mpidr());
+		} else {
+			plat_ic_set_interrupt_pending(SPD_PNCD_NS_IRQ);
+		}
+
+		SMC_RET0(handle);
+
+		break;
+
+	case SMC_YIELD:
+		assert(handle == cm_get_context(ns != 0 ? NON_SECURE : SECURE));
+		handle = pncd_context_switch_to(ns != 0 ? SECURE : NON_SECURE);
+
+		assert(handle != NULL);
+
+		SMC_RET0(handle);
+
+		break;
+
+	default:
+		INFO("Unknown smc: %x\n", smc_fid);
+		break;
+	}
+
+	return plat_pncd_smc_handler(smc_fid, x1, x2, x3, x4,
+				     cookie, handle, flags);
+}
+
+static uintptr_t pncd_smc_handler(uint32_t smc_fid,
+		u_register_t x1,
+		u_register_t x2,
+		u_register_t x3,
+		u_register_t x4,
+		void *cookie,
+		void *handle,
+		u_register_t flags)
+{
+	uintptr_t ret;
+
+	/* SMC handling is serialized */
+	spin_lock(&smc_handler_lock);
+	ret = pncd_smc_handler_unsafe(smc_fid, x1, x2, x3, x4, cookie, handle,
+								  flags);
+	spin_unlock(&smc_handler_lock);
+
+	return ret;
+}
+
+/* Define a SPD runtime service descriptor for fast SMC calls */
+DECLARE_RT_SVC(
+	pncd_fast,
+	OEN_TOS_START,
+	OEN_TOS_END,
+	SMC_TYPE_FAST,
+	pncd_setup,
+	pncd_smc_handler
+);
+
+/* Define a SPD runtime service descriptor for standard SMC calls */
+DECLARE_RT_SVC(
+	pncd_std,
+	OEN_TOS_START,
+	OEN_TOS_END,
+	SMC_TYPE_YIELD,
+	NULL,
+	pncd_smc_handler
+);
diff --git a/services/spd/pncd/pncd_private.h b/services/spd/pncd/pncd_private.h
new file mode 100644
index 0000000..8c9b634
--- /dev/null
+++ b/services/spd/pncd/pncd_private.h
@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2021-2022, ARM Limited and Contributors. All rights reserved.
+ * Portions copyright (c) 2021-2022, ProvenRun S.A.S. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef __PNCD_PRIVATE_H__
+#define __PNCD_PRIVATE_H__
+
+#ifndef __ASSEMBLER__
+#include <stdint.h>
+#endif /* __ASSEMBLER __ */
+
+#include <context.h>
+#ifndef __ASSEMBLER__
+#include <lib/cassert.h>
+#endif /* __ASSEMBLER __ */
+
+#include <platform_def.h>
+
+/*******************************************************************************
+ * Constants that allow assembler code to preserve callee-saved registers of the
+ * C runtime context while performing a security state switch.
+ ******************************************************************************/
+#define PNCD_C_RT_CTX_X19		U(0x0)
+#define PNCD_C_RT_CTX_X20		U(0x8)
+#define PNCD_C_RT_CTX_X21		U(0x10)
+#define PNCD_C_RT_CTX_X22		U(0x18)
+#define PNCD_C_RT_CTX_X23		U(0x20)
+#define PNCD_C_RT_CTX_X24		U(0x28)
+#define PNCD_C_RT_CTX_X25		U(0x30)
+#define PNCD_C_RT_CTX_X26		U(0x38)
+#define PNCD_C_RT_CTX_X27		U(0x40)
+#define PNCD_C_RT_CTX_X28		U(0x48)
+#define PNCD_C_RT_CTX_X29		U(0x50)
+#define PNCD_C_RT_CTX_X30		U(0x58)
+#define PNCD_C_RT_CTX_SIZE		U(0x60)
+#define PNCD_C_RT_CTX_ENTRIES		(PNCD_C_RT_CTX_SIZE >> DWORD_SHIFT)
+
+#ifndef __ASSEMBLER__
+
+/* AArch64 callee saved general purpose register context structure. */
+DEFINE_REG_STRUCT(c_rt_regs, PNCD_C_RT_CTX_ENTRIES);
+
+/*
+ * Compile time assertion to ensure that both the compiler and linker
+ * have the same double word aligned view of the size of the C runtime
+ * register context.
+ */
+CASSERT(sizeof(c_rt_regs_t) == PNCD_C_RT_CTX_SIZE,
+		assert_spd_c_rt_regs_size_mismatch);
+
+/*******************************************************************************
+ * Structure which helps the SPD to maintain the per-cpu state of the SP.
+ * 'mpidr'          - mpidr of the CPU running PNC
+ * 'c_rt_ctx'       - stack address to restore C runtime context from after
+ *                    returning from a synchronous entry into the SP.
+ * 'cpu_ctx'        - space to maintain SP architectural state
+ ******************************************************************************/
+typedef struct pnc_context {
+	uint64_t mpidr;
+	uint64_t c_rt_ctx;
+	cpu_context_t cpu_ctx;
+} pnc_context_t;
+
+/*******************************************************************************
+ * Function & Data prototypes
+ ******************************************************************************/
+uint64_t pncd_enter_sp(uint64_t *c_rt_ctx);
+void __dead2 pncd_exit_sp(uint64_t c_rt_ctx, uint64_t ret);
+uint64_t pncd_synchronous_sp_entry(pnc_context_t *pnc_ctx);
+void __dead2 pncd_synchronous_sp_exit(pnc_context_t *pnc_ctx, uint64_t ret);
+void pncd_init_pnc_ep_state(struct entry_point_info *pnc_ep,
+				uint64_t pc,
+				pnc_context_t *pnc_ctx);
+#endif /* __ASSEMBLER__ */
+
+#endif /* __PNCD_PRIVATE_H__ */
diff --git a/services/std_svc/spm/el3_spmc/spmc.h b/services/std_svc/spm/el3_spmc/spmc.h
index d62be91..5233650 100644
--- a/services/std_svc/spm/el3_spmc/spmc.h
+++ b/services/std_svc/spm/el3_spmc/spmc.h
@@ -9,6 +9,7 @@
 
 #include <stdint.h>
 
+#include <common/bl_common.h>
 #include <lib/psci/psci.h>
 #include <lib/spinlock.h>
 #include <services/el3_spmc_logical_sp.h>
diff --git a/services/std_svc/spmd/spmd_private.h b/services/std_svc/spmd/spmd_private.h
index 07fecb6..d21a622 100644
--- a/services/std_svc/spmd/spmd_private.h
+++ b/services/std_svc/spmd/spmd_private.h
@@ -7,6 +7,7 @@
 #ifndef SPMD_PRIVATE_H
 #define SPMD_PRIVATE_H
 
+#include <common/bl_common.h>
 #include <context.h>
 
 /*******************************************************************************
diff --git a/tools/sptool/sp_mk_generator.py b/tools/sptool/sp_mk_generator.py
index 814d051..c0beb65 100644
--- a/tools/sptool/sp_mk_generator.py
+++ b/tools/sptool/sp_mk_generator.py
@@ -196,7 +196,7 @@
     ''' 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'])
+        uuid_std = uuid.UUID(sp_layout[sp]['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]