diff --git a/.readthedocs.yaml b/.readthedocs.yaml
index 6e6fa84..2b418ae 100644
--- a/.readthedocs.yaml
+++ b/.readthedocs.yaml
@@ -22,8 +22,3 @@
 
 sphinx:
   configuration: docs/conf.py
-
-# Auxiliary formats to export to (in addition to the default HTML output).
-formats:
-  - pdf
-
diff --git a/Makefile b/Makefile
index a7894ec..d242722 100644
--- a/Makefile
+++ b/Makefile
@@ -517,6 +517,13 @@
 				plat/common/${ARCH}/platform_helpers.S	\
 				${COMPILER_RT_SRCS}
 
+# Pointer Authentication sources
+ifeq (${ENABLE_PAUTH}, 1)
+# arm/common/aarch64/arm_pauth.c contains a sample platform hook to complete the
+# Pauth support. As it's not secure, it must be reimplemented for real platforms
+BL_COMMON_SOURCES	+=	lib/extensions/pauth/pauth_helpers.S
+endif
+
 ifeq ($(notdir $(CC)),armclang)
 BL_COMMON_SOURCES	+=	lib/${ARCH}/armclang_printf.S
 endif
@@ -854,6 +861,10 @@
     $(info FEATURE_DETECTION is an experimental feature)
 endif
 
+ifneq ($(ENABLE_SME_FOR_NS), 0)
+    $(info ENABLE_SME_FOR_NS is an experimental feature)
+endif
+
 ifeq (${ARM_XLAT_TABLES_LIB_V1}, 1)
     ifeq (${ALLOW_RO_XLAT_TABLES}, 1)
         $(error "ALLOW_RO_XLAT_TABLES requires translation tables library v2")
@@ -870,7 +881,7 @@
 ifeq (${ARCH},aarch32)
 
     # SME/SVE only supported on AArch64
-    ifeq (${ENABLE_SME_FOR_NS},1)
+    ifneq (${ENABLE_SME_FOR_NS},0)
         $(error "ENABLE_SME_FOR_NS cannot be used with ARCH=aarch32")
     endif
     ifeq (${ENABLE_SVE_FOR_NS},1)
@@ -891,7 +902,7 @@
 
 # Ensure ENABLE_RME is not used with SME
 ifeq (${ENABLE_RME},1)
-    ifeq (${ENABLE_SME_FOR_NS},1)
+    ifneq (${ENABLE_SME_FOR_NS},0)
         $(error "ENABLE_SME_FOR_NS cannot be used with ENABLE_RME")
     endif
 endif
@@ -911,7 +922,7 @@
 # SVE and SME cannot be used with CTX_INCLUDE_FPREGS since secure manager does
 # its own context management including FPU registers.
 ifeq (${CTX_INCLUDE_FPREGS},1)
-    ifeq (${ENABLE_SME_FOR_NS},1)
+    ifneq (${ENABLE_SME_FOR_NS},0)
         $(error "ENABLE_SME_FOR_NS cannot be used with CTX_INCLUDE_FPREGS")
     endif
     ifeq (${ENABLE_SVE_FOR_NS},1)
@@ -1085,19 +1096,16 @@
         DISABLE_MTPMU \
         DYN_DISABLE_AUTH \
         EL3_EXCEPTION_HANDLING \
-        ENABLE_AMU \
         ENABLE_AMU_AUXILIARY_COUNTERS \
         ENABLE_AMU_FCONF \
         AMU_RESTRICT_COUNTERS \
         ENABLE_ASSERTIONS \
+        ENABLE_FEAT_SB \
         ENABLE_PIE \
         ENABLE_PMF \
         ENABLE_PSCI_STAT \
         ENABLE_RUNTIME_INSTRUMENTATION \
-        ENABLE_SME_FOR_NS \
         ENABLE_SME_FOR_SWD \
-        ENABLE_SPE_FOR_LOWER_ELS \
-        ENABLE_SVE_FOR_NS \
         ENABLE_SVE_FOR_SWD \
         ERROR_DEPRECATED \
         FAULT_INJECTION_SUPPORT \
@@ -1114,6 +1122,7 @@
         PLAT_RSS_NOT_SUPPORTED \
         PROGRAMMABLE_RESET_ADDRESS \
         PSCI_EXTENDED_STATE_ID \
+        PSCI_OS_INIT_MODE \
         RESET_TO_BL31 \
         RESET_TO_BL31_WITH_PARAMS \
         SAVE_KEYS \
@@ -1144,7 +1153,6 @@
         COT_DESC_IN_DTB \
         USE_SP804_TIMER \
         PSA_FWU_SUPPORT \
-        ENABLE_SYS_REG_TRACE_FOR_NS \
         ENABLE_MPMM \
         ENABLE_MPMM_FCONF \
         SIMICS_BUILD \
@@ -1166,7 +1174,7 @@
         ENABLE_TRBE_FOR_NS \
         ENABLE_BTI \
         ENABLE_PAUTH \
-        ENABLE_FEAT_AMUv1 \
+        ENABLE_FEAT_AMU \
         ENABLE_FEAT_AMUv1p1 \
         ENABLE_FEAT_CSV2_2 \
         ENABLE_FEAT_DIT \
@@ -1176,11 +1184,15 @@
         ENABLE_FEAT_PAN \
         ENABLE_FEAT_RNG \
         ENABLE_FEAT_RNG_TRAP \
-        ENABLE_FEAT_SB \
         ENABLE_FEAT_SEL2 \
+        ENABLE_FEAT_TCR2 \
         ENABLE_FEAT_VHE \
         ENABLE_MPAM_FOR_LOWER_ELS \
         ENABLE_RME \
+        ENABLE_SPE_FOR_NS \
+        ENABLE_SYS_REG_TRACE_FOR_NS \
+        ENABLE_SME_FOR_NS \
+        ENABLE_SVE_FOR_NS \
         ENABLE_TRF_FOR_NS \
         FW_ENC_STATUS \
         NR_OF_FW_BANKS \
@@ -1221,7 +1233,7 @@
         CTX_INCLUDE_NEVE_REGS \
         DECRYPTION_SUPPORT_${DECRYPTION_SUPPORT} \
         DISABLE_MTPMU \
-        ENABLE_AMU \
+        ENABLE_FEAT_AMU \
         ENABLE_AMU_AUXILIARY_COUNTERS \
         ENABLE_AMU_FCONF \
         AMU_RESTRICT_COUNTERS \
@@ -1236,7 +1248,7 @@
         ENABLE_RUNTIME_INSTRUMENTATION \
         ENABLE_SME_FOR_NS \
         ENABLE_SME_FOR_SWD \
-        ENABLE_SPE_FOR_LOWER_ELS \
+        ENABLE_SPE_FOR_NS \
         ENABLE_SVE_FOR_NS \
         ENABLE_SVE_FOR_SWD \
         ENCRYPT_BL31 \
@@ -1255,6 +1267,7 @@
         PLAT_RSS_NOT_SUPPORTED \
         PROGRAMMABLE_RESET_ADDRESS \
         PSCI_EXTENDED_STATE_ID \
+        PSCI_OS_INIT_MODE \
         RAS_EXTENSION \
         RESET_TO_BL31 \
         RESET_TO_BL31_WITH_PARAMS \
@@ -1302,7 +1315,6 @@
         ENABLE_MPMM \
         ENABLE_MPMM_FCONF \
         ENABLE_FEAT_FGT \
-        ENABLE_FEAT_AMUv1 \
         ENABLE_FEAT_ECV \
         SIMICS_BUILD \
         ENABLE_FEAT_AMUv1p1 \
@@ -1310,6 +1322,7 @@
         ENABLE_FEAT_VHE \
         ENABLE_FEAT_CSV2_2 \
         ENABLE_FEAT_PAN \
+        ENABLE_FEAT_TCR2 \
         FEATURE_DETECTION \
         TWED_DELAY \
         ENABLE_FEAT_TWED \
diff --git a/bl31/aarch64/runtime_exceptions.S b/bl31/aarch64/runtime_exceptions.S
index 500e87b..2fa9f06 100644
--- a/bl31/aarch64/runtime_exceptions.S
+++ b/bl31/aarch64/runtime_exceptions.S
@@ -55,7 +55,7 @@
 	 * delegated to platform EA handler.
 	 *
 	 * Without RAS_EXTENSION, this macro synchronizes pending errors using
-         * a DSB, unmasks Asynchronous External Aborts and saves X30 before
+	 * a DSB, unmasks Asynchronous External Aborts and saves X30 before
 	 * setting the flag CTX_IS_IN_EL3.
 	 */
 	.macro check_and_unmask_ea
@@ -506,6 +506,17 @@
 	/* Copy SCR_EL3.NS bit to the flag to indicate caller's security */
 	bfi	x7, x18, #0, #1
 
+	mov	sp, x12
+
+	/*
+	 * Per SMCCC documentation, bits [23:17] must be zero for Fast
+	 * SMCs. Other values are reserved for future use. Ensure that
+	 * these bits are zeroes, if not report as unknown SMC.
+	 */
+	tbz	x0, #FUNCID_TYPE_SHIFT, 2f  /* Skip check if its a Yield Call*/
+	tst	x0, #(FUNCID_FC_RESERVED_MASK << FUNCID_FC_RESERVED_SHIFT)
+	b.ne	smc_unknown
+
 	/*
 	 * Per SMCCCv1.3 a caller can set the SVE hint bit in the SMC FID
 	 * passed through x0. Copy the SVE hint bit to flags and mask the
@@ -513,11 +524,10 @@
 	 * A service/dispatcher can retrieve the SVE hint bit state from
 	 * flags using the appropriate helper.
 	 */
+2:
 	bfi	x7, x0, #FUNCID_SVE_HINT_SHIFT, #FUNCID_SVE_HINT_MASK
 	bic	x0, x0, #(FUNCID_SVE_HINT_MASK << FUNCID_SVE_HINT_SHIFT)
 
-	mov	sp, x12
-
 	/* Get the unique owning entity number */
 	ubfx	x16, x0, #FUNCID_OEN_SHIFT, #FUNCID_OEN_WIDTH
 	ubfx	x15, x0, #FUNCID_TYPE_SHIFT, #FUNCID_TYPE_WIDTH
@@ -584,7 +594,7 @@
 	 * Unknown SMC call. Populate return value with SMC_UNK and call
 	 * el3_exit() which will restore the remaining architectural state
 	 * i.e., SYS, GP and PAuth registers(if any) prior to issuing the ERET
-         * to the desired lower EL.
+	 * to the desired lower EL.
 	 */
 	mov	x0, #SMC_UNK
 	str	x0, [x6, #CTX_GPREGS_OFFSET + CTX_GPREG_X0]
diff --git a/bl31/bl31.mk b/bl31/bl31.mk
index e9590d5..4d151ab 100644
--- a/bl31/bl31.mk
+++ b/bl31/bl31.mk
@@ -87,11 +87,11 @@
 				services/std_svc/trng/trng_entropy_pool.c
 endif
 
-ifeq (${ENABLE_SPE_FOR_LOWER_ELS},1)
+ifneq (${ENABLE_SPE_FOR_NS},0)
 BL31_SOURCES		+=	lib/extensions/spe/spe.c
 endif
 
-ifeq (${ENABLE_AMU},1)
+ifneq (${ENABLE_FEAT_AMU},0)
 BL31_SOURCES		+=	${AMU_SOURCES}
 endif
 
@@ -99,16 +99,16 @@
 BL31_SOURCES		+=	${MPMM_SOURCES}
 endif
 
-ifeq (${ENABLE_SME_FOR_NS},1)
+ifneq (${ENABLE_SME_FOR_NS},0)
 BL31_SOURCES		+=	lib/extensions/sme/sme.c
 BL31_SOURCES		+=	lib/extensions/sve/sve.c
 else
-ifeq (${ENABLE_SVE_FOR_NS},1)
+ifneq (${ENABLE_SVE_FOR_NS},0)
 BL31_SOURCES		+=	lib/extensions/sve/sve.c
 endif
 endif
 
-ifeq (${ENABLE_MPAM_FOR_LOWER_ELS},1)
+ifneq (${ENABLE_MPAM_FOR_LOWER_ELS},0)
 BL31_SOURCES		+=	lib/extensions/mpam/mpam.c
 endif
 
@@ -120,7 +120,7 @@
 BL31_SOURCES		+=	lib/extensions/brbe/brbe.c
 endif
 
-ifeq (${ENABLE_SYS_REG_TRACE_FOR_NS},1)
+ifneq (${ENABLE_SYS_REG_TRACE_FOR_NS},0)
 BL31_SOURCES		+=      lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
 endif
 
diff --git a/bl32/sp_min/sp_min.mk b/bl32/sp_min/sp_min.mk
index 2a6612a..0e5c142 100644
--- a/bl32/sp_min/sp_min.mk
+++ b/bl32/sp_min/sp_min.mk
@@ -28,7 +28,7 @@
 BL32_SOURCES		+=	lib/pmf/pmf_main.c
 endif
 
-ifeq (${ENABLE_AMU},1)
+ifneq (${ENABLE_FEAT_AMU},0)
 BL32_SOURCES		+=	${AMU_SOURCES}
 endif
 
@@ -46,7 +46,7 @@
 				services/std_svc/trng/trng_entropy_pool.c
 endif
 
-ifeq (${ENABLE_SYS_REG_TRACE_FOR_NS},1)
+ifneq (${ENABLE_SYS_REG_TRACE_FOR_NS},0)
 BL32_SOURCES		+=	lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c
 endif
 
diff --git a/common/feat_detect.c b/common/feat_detect.c
index 5fb56b9..9394304 100644
--- a/common/feat_detect.c
+++ b/common/feat_detect.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -60,46 +60,6 @@
 	}
 }
 
-/******************************************
- * Feature : FEAT_SB (Speculation Barrier)
- *****************************************/
-static void read_feat_sb(void)
-{
-#if (ENABLE_FEAT_SB == FEAT_STATE_ALWAYS)
-	feat_detect_panic(is_armv8_0_feat_sb_present(), "SB");
-#endif
-}
-
-/******************************************************
- * Feature : FEAT_CSV2_2 (Cache Speculation Variant 2)
- *****************************************************/
-static void read_feat_csv2_2(void)
-{
-#if (ENABLE_FEAT_CSV2_2 == FEAT_STATE_ALWAYS)
-	feat_detect_panic(is_armv8_0_feat_csv2_2_present(), "CSV2_2");
-#endif
-}
-
-/***********************************************
- * Feature : FEAT_PAN (Privileged Access Never)
- **********************************************/
-static void read_feat_pan(void)
-{
-#if (ENABLE_FEAT_PAN == FEAT_STATE_ALWAYS)
-	feat_detect_panic(is_armv8_1_pan_present(), "PAN");
-#endif
-}
-
-/******************************************************
- * Feature : FEAT_VHE (Virtualization Host Extensions)
- *****************************************************/
-static void read_feat_vhe(void)
-{
-#if (ENABLE_FEAT_VHE == FEAT_STATE_ALWAYS)
-	feat_detect_panic(is_armv8_1_vhe_present(), "VHE");
-#endif
-}
-
 /*******************************************************************************
  * Feature : FEAT_RAS (Reliability, Availability, and Serviceability Extension)
  ******************************************************************************/
@@ -130,38 +90,6 @@
 #endif
 }
 
-/****************************************************************************
- * Feature : FEAT_MPAM (Memory Partitioning and Monitoring (MPAM) Extension)
- ***************************************************************************/
-static void read_feat_mpam(void)
-{
-#if (ENABLE_MPAM_FOR_LOWER_ELS == FEAT_STATE_ALWAYS)
-	feat_detect_panic(get_mpam_version() != 0U, "MPAM");
-#endif
-}
-
-/**************************************************************
- * Feature : FEAT_NV2 (Enhanced Nested Virtualization Support)
- *************************************************************/
-static void read_feat_nv2(void)
-{
-#if (CTX_INCLUDE_NEVE_REGS == FEAT_STATE_ALWAYS)
-	unsigned int nv = get_armv8_4_feat_nv_support();
-
-	feat_detect_panic((nv == ID_AA64MMFR2_EL1_NV2_SUPPORTED), "NV2");
-#endif
-}
-
-/***********************************
- * Feature : FEAT_SEL2 (Secure EL2)
- **********************************/
-static void read_feat_sel2(void)
-{
-#if (ENABLE_FEAT_SEL2 == FEAT_STATE_ALWAYS)
-	feat_detect_panic(is_armv8_4_sel2_present(), "SEL2");
-#endif
-}
-
 /************************************************
  * Feature : FEAT_MTE (Memory Tagging Extension)
  ***********************************************/
@@ -174,16 +102,6 @@
 #endif
 }
 
-/***********************************************
- * Feature : FEAT_RNG (Random Number Generator)
- **********************************************/
-static void read_feat_rng(void)
-{
-#if (ENABLE_FEAT_RNG == FEAT_STATE_ALWAYS)
-	feat_detect_panic(is_armv8_5_rng_present(), "RNG");
-#endif
-}
-
 /****************************************************
  * Feature : FEAT_BTI (Branch Target Identification)
  ***************************************************/
@@ -194,39 +112,6 @@
 #endif
 }
 
-/***********************************************
- * Feature : FEAT_AMUv1p1 (AMU Extensions v1.1)
- **********************************************/
-static void read_feat_amuv1p1(void)
-{
-#if (ENABLE_FEAT_AMUv1p1 == FEAT_STATE_ALWAYS)
-	feat_detect_panic(is_armv8_6_feat_amuv1p1_present(), "AMUv1p1");
-#endif
-}
-
-/*******************************************************
- * Feature : FEAT_ECV (Enhanced Counter Virtualization)
- ******************************************************/
-static void read_feat_ecv(void)
-{
-#if (ENABLE_FEAT_ECV == FEAT_STATE_ALWAYS)
-	unsigned int ecv = get_armv8_6_ecv_support();
-
-	feat_detect_panic(((ecv == ID_AA64MMFR0_EL1_ECV_SUPPORTED) ||
-			(ecv == ID_AA64MMFR0_EL1_ECV_SELF_SYNCH)), "ECV");
-#endif
-}
-
-/***********************************************************
- * Feature : FEAT_TWED (Delayed Trapping of WFE Instruction)
- **********************************************************/
-static void read_feat_twed(void)
-{
-#if (ENABLE_FEAT_TWED == FEAT_STATE_ALWAYS)
-	feat_detect_panic(is_armv8_6_twed_present(), "TWED");
-#endif
-}
-
 /**************************************************
  * Feature : FEAT_RME (Realm Management Extension)
  *************************************************/
@@ -276,44 +161,56 @@
 	tainted = false;
 
 	/* v8.0 features */
-	read_feat_sb();
-	read_feat_csv2_2();
+	check_feature(ENABLE_FEAT_SB, read_feat_sb_id_field(), "SB", 1, 1);
+	check_feature(ENABLE_FEAT_CSV2_2, read_feat_csv2_id_field(),
+		      "CSV2_2", 2, 3);
 
 	/* v8.1 features */
-	read_feat_pan();
-	read_feat_vhe();
+	check_feature(ENABLE_FEAT_PAN, read_feat_pan_id_field(), "PAN", 1, 3);
+	check_feature(ENABLE_FEAT_VHE, read_feat_vhe_id_field(), "VHE", 1, 1);
 
 	/* v8.2 features */
 	read_feat_ras();
+	check_feature(ENABLE_SVE_FOR_NS, read_feat_sve_id_field(),
+		      "SVE", 1, 1);
 
 	/* v8.3 features */
 	read_feat_pauth();
 
 	/* v8.4 features */
 	read_feat_dit();
-	check_feature(ENABLE_FEAT_AMUv1, read_feat_amu_id_field(),
+	check_feature(ENABLE_FEAT_AMU, read_feat_amu_id_field(),
 		      "AMUv1", 1, 2);
-	read_feat_mpam();
-	read_feat_nv2();
-	read_feat_sel2();
+	check_feature(ENABLE_MPAM_FOR_LOWER_ELS, read_feat_mpam_version(),
+		      "MPAM", 1, 17);
+	check_feature(CTX_INCLUDE_NEVE_REGS, read_feat_nv_id_field(),
+		      "NV2", 2, 2);
+	check_feature(ENABLE_FEAT_SEL2, read_feat_sel2_id_field(),
+		      "SEL2", 1, 1);
 	check_feature(ENABLE_TRF_FOR_NS, read_feat_trf_id_field(),
 		      "TRF", 1, 1);
 
 	/* v8.5 features */
 	read_feat_mte();
-	read_feat_rng();
+	check_feature(ENABLE_FEAT_RNG, read_feat_rng_id_field(), "RNG", 1, 1);
 	read_feat_bti();
 	read_feat_rng_trap();
 
 	/* v8.6 features */
-	read_feat_amuv1p1();
+	check_feature(ENABLE_FEAT_AMUv1p1, read_feat_amu_id_field(),
+		      "AMUv1p1", 2, 2);
 	check_feature(ENABLE_FEAT_FGT, read_feat_fgt_id_field(), "FGT", 1, 1);
-	read_feat_ecv();
-	read_feat_twed();
+	check_feature(ENABLE_FEAT_ECV, read_feat_ecv_id_field(), "ECV", 1, 2);
+	check_feature(ENABLE_FEAT_TWED, read_feat_twed_id_field(),
+		      "TWED", 1, 1);
 
 	/* v8.7 features */
 	check_feature(ENABLE_FEAT_HCX, read_feat_hcx_id_field(), "HCX", 1, 1);
 
+	/* v8.9 features */
+	check_feature(ENABLE_FEAT_TCR2, read_feat_tcrx_id_field(),
+		      "TCR2", 1, 1);
+
 	/* v9.0 features */
 	check_feature(ENABLE_BRBE_FOR_NS, read_feat_brbe_id_field(),
 		      "BRBE", 1, 2);
@@ -321,6 +218,8 @@
 		      "TRBE", 1, 1);
 
 	/* v9.2 features */
+	check_feature(ENABLE_SME_FOR_NS, read_feat_sme_id_field(),
+		      "SME", 1, 2);
 	read_feat_rme();
 
 	if (tainted) {
diff --git a/docs/about/maintainers.rst b/docs/about/maintainers.rst
index 914c959..db412f5 100644
--- a/docs/about/maintainers.rst
+++ b/docs/about/maintainers.rst
@@ -543,6 +543,10 @@
 :|G|: `mtk-rex-bc-chen`_
 :|M|: Leon Chen <leon.chen@mediatek.com>
 :|G|: `leon-chen-mtk`_
+:|M|: Jason-CH Chen <jason-ch.chen@mediatek.com>
+:|G|: `jason-ch-chen`_
+:|M|: Yidi Lin <yidilin@chromium.org>
+:|G|: `linyidi`_
 :|F|: docs/plat/mt\*.rst
 :|F|: plat/mediatek/
 
@@ -909,6 +913,8 @@
 .. _MrVan: https://github.com/MrVan
 .. _mtk-rex-bc-chen: https://github.com/mtk-rex-bc-chen
 .. _leon-chen-mtk: https://github.com/leon-chen-mtk
+.. _jason-ch-chen: https://github.com/jason-ch-chen
+.. _linyidi: https://github.com/linyidi
 .. _niej: https://github.com/niej
 .. _npoushin: https://github.com/npoushin
 .. _prabhakarlad: https://github.com/prabhakarlad
diff --git a/docs/components/activity-monitors.rst b/docs/components/activity-monitors.rst
index dd45c43..5c1c2c2 100644
--- a/docs/components/activity-monitors.rst
+++ b/docs/components/activity-monitors.rst
@@ -6,9 +6,9 @@
 Unit (|AMU|), an optional non-invasive component for monitoring core events
 through a set of 64-bit counters.
 
-When the ``ENABLE_AMU=1`` build option is provided, Trusted Firmware-A sets up
-the |AMU| prior to its exit from EL3, and will save and restore architected
-|AMU| counters as necessary upon suspend and resume.
+When the ``ENABLE_FEAT_AMU=1`` build option is provided, Trusted Firmware-A
+sets up the |AMU| prior to its exit from EL3, and will save and restore
+architected |AMU| counters as necessary upon suspend and resume.
 
 .. _Activity Monitor Auxiliary Counters:
 
diff --git a/docs/design/cpu-specific-build-macros.rst b/docs/design/cpu-specific-build-macros.rst
index 8a20186..0f1f92a 100644
--- a/docs/design/cpu-specific-build-macros.rst
+++ b/docs/design/cpu-specific-build-macros.rst
@@ -349,6 +349,14 @@
 
 For Cortex-A78C, the following errata build flags are defined :
 
+- ``ERRATA_A78C_1827430`` : This applies errata 1827430 workaround to
+  Cortex-A78C CPU. This needs to be enabled for revision r0p0. The erratum is
+  fixed in r0p1.
+
+- ``ERRATA_A78C_1827440`` : This applies errata 1827440 workaround to
+  Cortex-A78C CPU. This needs to be enabled for revision r0p0. The erratum is
+  fixed in r0p1.
+
 - ``ERRATA_A78C_2132064`` : This applies errata 2132064 workaround to
   Cortex-A78C CPU. This needs to be enabled for revisions r0p1, r0p2 and
   it is still open.
diff --git a/docs/design_documents/index.rst b/docs/design_documents/index.rst
index 3d82e69..d20fc58 100644
--- a/docs/design_documents/index.rst
+++ b/docs/design_documents/index.rst
@@ -10,6 +10,7 @@
    measured_boot_poc
    drtm_poc
    rss
+   psci_osi_mode
 
 --------------
 
diff --git a/docs/design_documents/psci_osi_mode.rst b/docs/design_documents/psci_osi_mode.rst
new file mode 100644
index 0000000..3296e27
--- /dev/null
+++ b/docs/design_documents/psci_osi_mode.rst
@@ -0,0 +1,716 @@
+PSCI OS-initiated mode
+======================
+
+:Author: Maulik Shah & Wing Li
+:Organization: Qualcomm Innovation Center, Inc. & Google LLC
+:Contact: Maulik Shah <quic_mkshah@quicinc.com> & Wing Li <wingers@google.com>
+:Status: RFC
+
+.. contents:: Table of Contents
+
+Introduction
+------------
+
+Power state coordination
+^^^^^^^^^^^^^^^^^^^^^^^^
+
+A power domain topology is a logical hierarchy of power domains in a system that
+arises from the physical dependencies between power domains.
+
+Local power states describe power states for an individual node, and composite
+power states describe the combined power states for an individual node and its
+parent node(s).
+
+Entry into low-power states for a topology node above the core level requires
+coordinating its children nodes. For example, in a system with a power domain
+that encompasses a shared cache, and a separate power domain for each core that
+uses the shared cache, the core power domains must be powered down before the
+shared cache power domain can be powered down.
+
+PSCI supports two modes of power state coordination: platform-coordinated and
+OS-initiated.
+
+Platform-coordinated
+~~~~~~~~~~~~~~~~~~~~
+
+Platform-coordinated mode is the default mode of power state coordination, and
+is currently the only supported mode in TF-A.
+
+In platform-coordinated mode, the platform is responsible for coordinating power
+states, and chooses the deepest power state for a topology node that can be
+tolerated by its children.
+
+OS-initiated
+~~~~~~~~~~~~
+
+OS-initiated mode is optional.
+
+In OS-initiated mode, the calling OS is responsible for coordinating power
+states, and may request for a topology node to enter a low-power state when
+its last child enters the low-power state.
+
+Motivation
+----------
+
+There are two reasons why OS-initiated mode might be a more suitable option than
+platform-coordinated mode for a platform.
+
+Scalability
+^^^^^^^^^^^
+
+In platform-coordinated mode, each core independently selects their own local
+power states, and doesn't account for composite power states that are shared
+between cores.
+
+In OS-initiated mode, the OS has knowledge of the next wakeup event for each
+core, and can have more precise control over the entry, exit, and wakeup
+latencies when deciding if a composite power state (e.g. for a cluster) is
+appropriate. This is especially important for multi-cluster SMP systems and
+heterogeneous systems like big.LITTLE, where different processor types can have
+different power efficiencies.
+
+Simplicity
+^^^^^^^^^^
+
+In platform-coordinated mode, the OS doesn't have visibility when the last core
+at a power level enters a low-power state. If the OS wants to perform last man
+activity (e.g. powering off a shared resource when it is no longer needed), it
+would have to communicate with an API side channel to know when it can do so.
+This could result in a design smell where the platform is using
+platform-coordinated mode when it should be using OS-initiated mode instead.
+
+In OS-initiated mode, the OS can perform last man activity if it selects a
+composite power state when the last core enters a low-power state. This
+eliminates the need for a side channel, and uses the well documented API between
+the OS and the platform.
+
+Current vendor implementations and workarounds
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+* STMicroelectronics
+
+  * For their ARM32 platforms, they're using OS-initiated mode implemented in
+    OP-TEE.
+  * For their future ARM64 platforms, they are interested in using OS-initiated
+    mode in TF-A.
+
+* Qualcomm
+
+  * For their mobile platforms, they're using OS-initiated mode implemented in
+    their own custom secure monitor firmware.
+  * For their Chrome OS platforms, they're using platform-coordinated mode in
+    TF-A with custom driver logic to perform last man activity.
+
+* Google
+
+  * They're using platform-coordinated mode in TF-A with custom driver logic to
+    perform last man activity.
+
+Both Qualcomm and Google would like to be able to use OS-initiated mode in TF-A
+in order to simplify custom driver logic.
+
+Requirements
+------------
+
+PSCI_FEATURES
+^^^^^^^^^^^^^
+
+PSCI_FEATURES is for checking whether or not a PSCI function is implemented and
+what its properties are.
+
+.. c:macro:: PSCI_FEATURES
+
+   :param func_id: 0x8400_000A.
+   :param psci_func_id: the function ID of a PSCI function.
+   :retval NOT_SUPPORTED: if the function is not implemented.
+   :retval feature flags associated with the function: if the function is
+       implemented.
+
+CPU_SUSPEND feature flags
+~~~~~~~~~~~~~~~~~~~~~~~~~
+
+* Reserved, bits[31:2]
+* Power state parameter format, bit[1]
+
+  * A value of 0 indicates the original format is used.
+  * A value of 1 indicates the extended format is used.
+
+* OS-initiated mode, bit[0]
+
+  * A value of 0 indicates OS-initiated mode is not supported.
+  * A value of 1 indicates OS-initiated mode is supported.
+
+See sections 5.1.14 and 5.15 of the PSCI spec (DEN0022D.b) for more details.
+
+PSCI_SET_SUSPEND_MODE
+^^^^^^^^^^^^^^^^^^^^^
+
+PSCI_SET_SUSPEND_MODE is for switching between the two different modes of power
+state coordination.
+
+.. c:macro:: PSCI_SET_SUSPEND_MODE
+
+   :param func_id: 0x8400_000F.
+   :param mode: 0 indicates platform-coordinated mode, 1 indicates OS-initiated
+       mode.
+   :retval SUCCESS: if the request is successful.
+   :retval NOT_SUPPORTED: if OS-initiated mode is not supported.
+   :retval INVALID_PARAMETERS: if the requested mode is not a valid value (0 or
+       1).
+   :retval DENIED: if the cores are not in the correct state.
+
+Switching from platform-coordinated to OS-initiated is only allowed if the
+following conditions are met:
+
+* All cores are in one of the following states:
+
+  * Running.
+  * Off, through a call to CPU_OFF or not yet booted.
+  * Suspended, through a call to CPU_DEFAULT_SUSPEND.
+
+* None of the cores has called CPU_SUSPEND since the last change of mode or
+  boot.
+
+Switching from OS-initiated to platform-coordinated is only allowed if all cores
+other than the calling core are off, either through a call to CPU_OFF or not yet
+booted.
+
+If these conditions are not met, the PSCI implementation must return DENIED.
+
+See sections 5.1.19 and 5.20 of the PSCI spec (DEN0022D.b) for more details.
+
+CPU_SUSPEND
+^^^^^^^^^^^
+
+CPU_SUSPEND is for moving a topology node into a low-power state.
+
+.. c:macro:: CPU_SUSPEND
+
+   :param func_id: 0xC400_0001.
+   :param power_state: the requested low-power state to enter.
+   :param entry_point_address: the address at which the core must resume
+       execution following wakeup from a powerdown state.
+   :param context_id: this field specifies a pointer to the saved context that
+       must be restored on a core following wakeup from a powerdown state.
+   :retval SUCCESS: if the request is successful.
+   :retval INVALID_PARAMETERS: in OS-initiated mode, this error is returned when
+       a low-power state is requested for a topology node above the core level,
+       and at least one of the node's children is in a local low-power state
+       that is incompatible with the request.
+   :retval INVALID_ADDRESS: if the entry_point_address argument is invalid.
+   :retval DENIED: only in OS-initiated mode; this error is returned when a
+       low-power state is requested for a topology node above the core level,
+       and at least one of the node's children is running, i.e. not in a
+       low-power state.
+
+In platform-coordinated mode, the PSCI implementation coordinates requests from
+all cores to determine the deepest power state to enter.
+
+In OS-initiated mode, the calling OS is making an explicit request for a
+specific power state, as opposed to expressing a vote. The PSCI implementation
+must comply with the request, unless the request is not consistent with the
+implementation's view of the system's state, in which case, the implementation
+must return INVALID_PARAMETERS or DENIED.
+
+See sections 5.1.2 and 5.4 of the PSCI spec (DEN0022D.b) for more details.
+
+Power state formats
+~~~~~~~~~~~~~~~~~~~
+
+Original format
+
+* Power Level, bits[25:24]
+
+  * The requested level in the power domain topology to enter a low-power
+    state.
+
+* State Type, bit[16]
+
+  * A value of 0 indicates a standby or retention state.
+  * A value of 1 indicates a powerdown state.
+
+* State ID, bits[15:0]
+
+  * Field to specify the requested composite power state.
+  * The state ID encodings must uniquely describe every possible composite
+    power state.
+  * In OS-initiated mode, the state ID encoding must allow expressing the
+    power level at which the calling core is the last to enter a powerdown
+    state.
+
+Extended format
+
+* State Type, bit[30]
+* State ID, bits[27:0]
+
+Races in OS-initiated mode
+~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+In OS-initiated mode, there are race windows where the OS's view and
+implementation's view of the system's state differ. It is possible for the OS to
+make requests that are invalid given the implementation's view of the system's
+state. For example, the OS might request a powerdown state for a node from one
+core, while at the same time, the implementation observes that another core in
+that node is powering up.
+
+To address potential race conditions in power state requests:
+
+* The calling OS must specify in each CPU_SUSPEND request the deepest power
+  level for which it sees the calling core as the last running core (last man).
+  This is required even if the OS doesn't want the node at that power level to
+  enter a low-power state.
+* The implementation must validate that the requested power states in the
+  CPU_SUSPEND request are consistent with the system's state, and that the
+  calling core is the last core running at the requested power level, or deny
+  the request otherwise.
+
+See sections 4.2.3.2, 6.2, and 6.3 of the PSCI spec (DEN0022D.b) for more
+details.
+
+Caveats
+-------
+
+CPU_OFF
+^^^^^^^
+
+CPU_OFF is always platform-coordinated, regardless of whether the power state
+coordination mode for suspend is platform-coordinated or OS-initiated. If all
+cores in a topology node call CPU_OFF, the last core will power down the node.
+
+In OS-initiated mode, if a subset of the cores in a topology node has called
+CPU_OFF, the last running core may call CPU_SUSPEND to request a powerdown state
+at or above that node's power level.
+
+See section 5.5.2 of the PSCI spec (DEN0022D.b) for more details.
+
+Implementation
+--------------
+
+Current implementation of platform-coordinated mode
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Platform-coordinated is currently the only supported power state coordination
+mode in TF-A.
+
+The functions of interest in the ``psci_cpu_suspend`` call stack are as follows:
+
+* ``psci_validate_power_state``
+
+  * This function calls a platform specific ``validate_power_state`` handler,
+    which takes the ``power_state`` parameter, and updates the ``state_info``
+    object with the requested states for each power level.
+
+* ``psci_find_target_suspend_lvl``
+
+  * This function takes the ``state_info`` object containing the requested power
+    states for each power level, and returns the deepest power level that was
+    requested to enter a low power state, i.e. the target power level.
+
+* ``psci_do_state_coordination``
+
+  * This function takes the target power level and the ``state_info`` object
+    containing the requested power states for each power level, and updates the
+    ``state_info`` object with the coordinated target power state for each
+    level.
+
+* ``pwr_domain_suspend``
+
+  * This is a platform specific handler that takes the ``state_info`` object
+    containing the target power states for each power level, and transitions
+    each power level to the specified power state.
+
+Proposed implementation of OS-initiated mode
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+To add support for OS-initiated mode, the following changes are proposed:
+
+* Add a boolean build option ``PSCI_OS_INIT_MODE`` for a platform to enable
+  optional support for PSCI OS-initiated mode. This build option defaults to 0.
+
+.. note::
+
+   If ``PSCI_OS_INIT_MODE=0``, the following changes will not be compiled into
+   the build.
+
+* Update ``psci_features`` to return 1 in bit[0] to indicate support for
+  OS-initiated mode for CPU_SUSPEND.
+* Define a ``suspend_mode`` enum: ``PLAT_COORD`` and ``OS_INIT``.
+* Define a ``psci_suspend_mode`` global variable with a default value of
+  ``PLAT_COORD``.
+* Implement a new function handler ``psci_set_suspend_mode`` for
+  PSCI_SET_SUSPEND_MODE.
+* Since ``psci_validate_power_state`` calls a platform specific
+  ``validate_power_state`` handler, the platform implementation should populate
+  the ``state_info`` object based on the state ID from the given ``power_state``
+  parameter.
+* ``psci_find_target_suspend_lvl`` remains unchanged.
+* Implement a new function ``psci_validate_state_coordination`` that ensures the
+  request satisfies the following conditions, and denies any requests
+  that don't:
+
+  * The requested power states for each power level are consistent with the
+    system's state
+  * The calling core is the last core running at the requested power level
+
+  This function differs from ``psci_do_state_coordination`` in that:
+
+  * The ``psci_req_local_pwr_states`` map is not modified if the request were to
+    be denied
+  * The ``state_info`` argument is never modified since it contains the power
+    states requested by the calling OS
+
+* Update ``psci_cpu_suspend_start`` to do the following:
+
+  * If ``PSCI_SUSPEND_MODE`` is ``PLAT_COORD``, call
+    ``psci_do_state_coordination``.
+  * If ``PSCI_SUSPEND_MODE`` is ``OS_INIT``, call
+    ``psci_validate_state_coordination``. If validation fails, propagate the
+    error up the call stack.
+
+* Update the return type of the platform specific ``pwr_domain_suspend``
+  handler from ``void`` to ``int``, to allow the platform to optionally perform
+  validations based on hardware states.
+
+.. image:: ../resources/diagrams/psci-osi-mode.png
+
+Testing
+-------
+
+The proposed patches can be found at
+https://review.trustedfirmware.org/q/topic:psci-osi.
+
+Testing on FVP and Google platforms
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+The proposed patches add a new CPU Suspend in OSI mode test suite to TF-A Tests.
+This has been enabled and verified on the FVP_Base_RevC-2xAEMvA platform and
+Google platforms, and excluded from all other platforms via the build option
+``PLAT_TESTS_SKIP_LIST``.
+
+Testing on STM32MP15
+^^^^^^^^^^^^^^^^^^^^
+
+The proposed patches have been tested and verified on the STM32MP15 platform,
+which has a single cluster with 2 CPUs, by Gabriel Fernandez
+<gabriel.fernandez@st.com> from STMicroelectronics with this device tree
+configuration:
+
+.. code-block:: devicetree
+
+   cpus {
+           #address-cells = <1>;
+           #size-cells = <0>;
+
+           cpu0: cpu@0 {
+                   device_type = "cpu";
+                   compatible = "arm,cortex-a7";
+                   reg = <0>;
+                   enable-method = "psci";
+                   power-domains = <&CPU_PD0>;
+                   power-domain-names = "psci";
+           };
+           cpu1: cpu@1 {
+                   device_type = "cpu";
+                   compatible = "arm,cortex-a7";
+                   reg = <1>;
+                   enable-method = "psci";
+                   power-domains = <&CPU_PD1>;
+                   power-domain-names = "psci";
+           };
+
+           idle-states {
+                   cpu_retention: cpu-retention {
+                           compatible = "arm,idle-state";
+                           arm,psci-suspend-param = <0x00000001>;
+                           entry-latency-us = <130>;
+                           exit-latency-us = <620>;
+                           min-residency-us = <700>;
+                           local-timer-stop;
+                   };
+           };
+
+           domain-idle-states {
+                   CLUSTER_STOP: core-power-domain {
+                           compatible = "domain-idle-state";
+                           arm,psci-suspend-param = <0x01000001>;
+                           entry-latency-us = <230>;
+                           exit-latency-us = <720>;
+                           min-residency-us = <2000>;
+                           local-timer-stop;
+                   };
+           };
+   };
+
+   psci {
+           compatible = "arm,psci-1.0";
+           method = "smc";
+
+           CPU_PD0: power-domain-cpu0 {
+                   #power-domain-cells = <0>;
+                   power-domains = <&pd_core>;
+                   domain-idle-states = <&cpu_retention>;
+           };
+
+           CPU_PD1: power-domain-cpu1 {
+                   #power-domain-cells = <0>;
+                   power-domains = <&pd_core>;
+                   domain-idle-states = <&cpu_retention>;
+           };
+
+           pd_core: power-domain-cluster {
+                   #power-domain-cells = <0>;
+                   domain-idle-states = <&CLUSTER_STOP>;
+           };
+   };
+
+Testing on Qualcomm SC7280
+^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+The proposed patches have been tested and verified on the SC7280 platform by
+Maulik Shah <quic_mkshah@quicinc.com> from Qualcomm with this device tree
+configuration:
+
+.. code-block:: devicetree
+
+   cpus {
+           #address-cells = <2>;
+           #size-cells = <0>;
+
+           CPU0: cpu@0 {
+                   device_type = "cpu";
+                   compatible = "arm,kryo";
+                   reg = <0x0 0x0>;
+                   enable-method = "psci";
+                   power-domains = <&CPU_PD0>;
+                   power-domain-names = "psci";
+           };
+
+           CPU1: cpu@100 {
+                   device_type = "cpu";
+                   compatible = "arm,kryo";
+                   reg = <0x0 0x100>;
+                   enable-method = "psci";
+                   power-domains = <&CPU_PD1>;
+                   power-domain-names = "psci";
+           };
+
+           CPU2: cpu@200 {
+                   device_type = "cpu";
+                   compatible = "arm,kryo";
+                   reg = <0x0 0x200>;
+                   enable-method = "psci";
+                   power-domains = <&CPU_PD2>;
+                   power-domain-names = "psci";
+           };
+
+           CPU3: cpu@300 {
+                   device_type = "cpu";
+                   compatible = "arm,kryo";
+                   reg = <0x0 0x300>;
+                   enable-method = "psci";
+                   power-domains = <&CPU_PD3>;
+                   power-domain-names = "psci";
+           }
+
+           CPU4: cpu@400 {
+                   device_type = "cpu";
+                   compatible = "arm,kryo";
+                   reg = <0x0 0x400>;
+                   enable-method = "psci";
+                   power-domains = <&CPU_PD4>;
+                   power-domain-names = "psci";
+           };
+
+           CPU5: cpu@500 {
+                   device_type = "cpu";
+                   compatible = "arm,kryo";
+                   reg = <0x0 0x500>;
+                   enable-method = "psci";
+                   power-domains = <&CPU_PD5>;
+                   power-domain-names = "psci";
+           };
+
+           CPU6: cpu@600 {
+                   device_type = "cpu";
+                   compatible = "arm,kryo";
+                   reg = <0x0 0x600>;
+                   enable-method = "psci";
+                   power-domains = <&CPU_PD6>;
+                   power-domain-names = "psci";
+           };
+
+           CPU7: cpu@700 {
+                   device_type = "cpu";
+                   compatible = "arm,kryo";
+                   reg = <0x0 0x700>;
+                   enable-method = "psci";
+                   power-domains = <&CPU_PD7>;
+                   power-domain-names = "psci";
+           };
+
+           idle-states {
+                   entry-method = "psci";
+
+                   LITTLE_CPU_SLEEP_0: cpu-sleep-0-0 {
+                           compatible = "arm,idle-state";
+                           idle-state-name = "little-power-down";
+                           arm,psci-suspend-param = <0x40000003>;
+                           entry-latency-us = <549>;
+                           exit-latency-us = <901>;
+                           min-residency-us = <1774>;
+                           local-timer-stop;
+                   };
+
+                   LITTLE_CPU_SLEEP_1: cpu-sleep-0-1 {
+                           compatible = "arm,idle-state";
+                           idle-state-name = "little-rail-power-down";
+                           arm,psci-suspend-param = <0x40000004>;
+                           entry-latency-us = <702>;
+                           exit-latency-us = <915>;
+                           min-residency-us = <4001>;
+                           local-timer-stop;
+                   };
+
+                   BIG_CPU_SLEEP_0: cpu-sleep-1-0 {
+                           compatible = "arm,idle-state";
+                           idle-state-name = "big-power-down";
+                           arm,psci-suspend-param = <0x40000003>;
+                           entry-latency-us = <523>;
+                           exit-latency-us = <1244>;
+                           min-residency-us = <2207>;
+                           local-timer-stop;
+                   };
+
+                   BIG_CPU_SLEEP_1: cpu-sleep-1-1 {
+                           compatible = "arm,idle-state";
+                           idle-state-name = "big-rail-power-down";
+                           arm,psci-suspend-param = <0x40000004>;
+                           entry-latency-us = <526>;
+                           exit-latency-us = <1854>;
+                           min-residency-us = <5555>;
+                           local-timer-stop;
+                   };
+           };
+
+           domain-idle-states {
+                   CLUSTER_SLEEP_0: cluster-sleep-0 {
+                           compatible = "arm,idle-state";
+                           idle-state-name = "cluster-power-down";
+                           arm,psci-suspend-param = <0x40003444>;
+                           entry-latency-us = <3263>;
+                           exit-latency-us = <6562>;
+                           min-residency-us = <9926>;
+                           local-timer-stop;
+                   };
+           };
+   };
+
+   psci {
+           compatible = "arm,psci-1.0";
+           method = "smc";
+
+           CPU_PD0: cpu0 {
+                   #power-domain-cells = <0>;
+                   power-domains = <&CLUSTER_PD>;
+                   domain-idle-states = <&LITTLE_CPU_SLEEP_0 &LITTLE_CPU_SLEEP_1>;
+           };
+
+           CPU_PD1: cpu1 {
+                   #power-domain-cells = <0>;
+                   power-domains = <&CLUSTER_PD>;
+                   domain-idle-states = <&LITTLE_CPU_SLEEP_0 &LITTLE_CPU_SLEEP_1>;
+           };
+
+           CPU_PD2: cpu2 {
+                   #power-domain-cells = <0>;
+                   power-domains = <&CLUSTER_PD>;
+                   domain-idle-states = <&LITTLE_CPU_SLEEP_0 &LITTLE_CPU_SLEEP_1>;
+           };
+
+           CPU_PD3: cpu3 {
+                   #power-domain-cells = <0>;
+                   power-domains = <&CLUSTER_PD>;
+                   domain-idle-states = <&LITTLE_CPU_SLEEP_0 &LITTLE_CPU_SLEEP_1>;
+           };
+
+           CPU_PD4: cpu4 {
+                   #power-domain-cells = <0>;
+                   power-domains = <&CLUSTER_PD>;
+                   domain-idle-states = <&BIG_CPU_SLEEP_0 &BIG_CPU_SLEEP_1>;
+           };
+
+           CPU_PD5: cpu5 {
+                   #power-domain-cells = <0>;
+                   power-domains = <&CLUSTER_PD>;
+                   domain-idle-states = <&BIG_CPU_SLEEP_0 &BIG_CPU_SLEEP_1>;
+           };
+
+           CPU_PD6: cpu6 {
+                   #power-domain-cells = <0>;
+                   power-domains = <&CLUSTER_PD>;
+                   domain-idle-states = <&BIG_CPU_SLEEP_0 &BIG_CPU_SLEEP_1>;
+           };
+
+           CPU_PD7: cpu7 {
+                   #power-domain-cells = <0>;
+                   power-domains = <&CLUSTER_PD>;
+                   domain-idle-states = <&BIG_CPU_SLEEP_0 &BIG_CPU_SLEEP_1>;
+           };
+
+           CLUSTER_PD: cpu-cluster0 {
+                   #power-domain-cells = <0>;
+                   domain-idle-states = <&CLUSTER_SLEEP_0>;
+           };
+   };
+
+Comparisons on Qualcomm SC7280
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+CPUIdle states
+~~~~~~~~~~~~~~
+
+* 8 CPUs, 1 L3 cache
+* Platform-coordinated mode
+
+  * CPUIdle states
+
+    * State0 - WFI
+    * State1 - Core collapse
+    * State2 - Rail collapse
+    * State3 - L3 cache off and system resources voted off
+
+* OS-initiated mode
+
+  * CPUIdle states
+
+    * State0 - WFI
+    * State1 - Core collapse
+    * State2 - Rail collapse
+
+  * Cluster domain idle state
+
+    * State3 - L3 cache off and system resources voted off
+
+.. image:: ../resources/diagrams/psci-flattened-vs-hierarchical-idle-states.png
+
+Results
+~~~~~~~
+
+* The following stats have been captured with fixed CPU frequencies from the use
+  case of 10 seconds of device idle with the display turned on and Wi-Fi and
+  modem turned off.
+* Count refers to the number of times a CPU or cluster entered power collapse.
+* Residency refers to the time in seconds a CPU or cluster stayed in power
+  collapse.
+* The results are an average of 3 iterations of actual counts and residencies.
+
+.. image:: ../resources/diagrams/psci-pc-mode-vs-osi-mode.png
+
+OS-initiated mode was able to scale better than platform-coordinated mode for
+multiple CPUs. The count and residency results for state3 (i.e. a cluster domain
+idle state) in OS-initiated mode for multiple CPUs were much closer to the
+results for a single CPU than in platform-coordinated mode.
+
+--------------
+
+*Copyright (c) 2023, Arm Limited and Contributors. All rights reserved.*
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index 0dc8b71..1ee07d9 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -230,11 +230,6 @@
    payload. Please refer to the "Booting an EL3 payload" section for more
    details.
 
--  ``ENABLE_AMU``: Boolean option to enable Activity Monitor Unit extensions.
-   This is an optional architectural feature available on v8.4 onwards. Some
-   v8.2 implementations also implement an AMU and this option can be used to
-   enable this feature on those systems as well. Default is 0.
-
 -  ``ENABLE_AMU_AUXILIARY_COUNTERS``: Enables support for AMU auxiliary counters
    (also known as group 1 counters). These are implementation-defined counters,
    and as such require additional platform configuration. Default is 0.
@@ -261,13 +256,12 @@
    builds, but this behaviour can be overridden in each platform's Makefile or
    in the build command line.
 
--  ``ENABLE_FEAT_AMUv1``: Numeric value to enable access to the HAFGRTR_EL2
-   (Hypervisor Activity Monitors Fine-Grained Read Trap Register) during EL2
-   to EL3 context save/restore operations. This flag can take the values 0 to 2,
-   to align with the ``FEATURE_DETECTION`` mechanism. It is an optional feature
-   available on v8.4 and onwards and must be set to either 1 or 2 alongside
-   ``ENABLE_FEAT_FGT``, to access the HAFGRTR_EL2 register.
-   Default value is ``0``.
+-  ``ENABLE_FEAT_AMU``: Numeric value to enable Activity Monitor Unit
+   extensions. This flag can take the values 0 to 2, to align with the
+   ``FEATURE_DETECTION`` mechanism. This is an optional architectural feature
+   available on v8.4 onwards. Some v8.2 implementations also implement an AMU
+   and this option can be used to enable this feature on those systems as well.
+   This flag can take the values 0 to 2, the default is 0.
 
 -  ``ENABLE_FEAT_AMUv1p1``: Numeric value to enable the ``FEAT_AMUv1p1``
    extension. ``FEAT_AMUv1p1`` is an optional feature available on Arm v8.6
@@ -327,12 +321,11 @@
    Default value is ``0``. ``FEAT_RNG_TRAP`` is an optional feature from
    Armv8.5 onwards.
 
--  ``ENABLE_FEAT_SB``: Numeric value to enable the ``FEAT_SB`` (Speculation
-   Barrier) extension allowing access to ``sb`` instruction. ``FEAT_SB`` is an
-   optional feature and defaults to ``0`` for pre-Armv8.5 CPUs but are mandatory
-   for Armv8.5 or later CPUs. This flag can take values 0 to 2, to align with
-   ``FEATURE_DETECTION`` mechanism. It is enabled from v8.5 and upwards and if
-   needed could be overidden from platforms explicitly. Default value is ``0``.
+-  ``ENABLE_FEAT_SB``: Boolean option to let the TF-A code use the ``FEAT_SB``
+   (Speculation Barrier) instruction ``FEAT_SB`` is an optional feature and
+   defaults to ``0`` for pre-Armv8.5 CPUs, but is mandatory for Armv8.5 or
+   later CPUs. It is enabled from v8.5 and upwards and if needed can be
+   overidden from platforms explicitly.
 
 -  ``ENABLE_FEAT_SEL2``: Numeric value to enable the ``FEAT_SEL2`` (Secure EL2)
    extension. ``FEAT_SEL2`` is a mandatory feature available on Arm v8.4.
@@ -354,6 +347,13 @@
    values 0 to 2, to align  with the ``FEATURE_DETECTION`` mechanism.
    Default value is ``0``.
 
+-  ``ENABLE_FEAT_TCR2``: Numeric value to set the bit SCR_EL3.ENTCR2 in EL3 to
+   allow access to TCR2_EL2 (extended translation control) from EL2 as
+   well as adding TCR2_EL2 to the EL2 context save/restore operations. Its a
+   mandatory architectural feature and is enabled from v8.9 and upwards. This
+   flag can take the values 0 to 2, to align  with the ``FEATURE_DETECTION``
+   mechanism. Default value is ``0``.
+
 -  ``ENABLE_LTO``: Boolean option to enable Link Time Optimization (LTO)
    support in GCC for TF-A. This option is currently only supported for
    AArch64. Default is 0.
@@ -406,14 +406,15 @@
    instrumented. Enabling this option enables the ``ENABLE_PMF`` build option
    as well. Default is 0.
 
--  ``ENABLE_SME_FOR_NS``: Boolean option to enable Scalable Matrix Extension
+-  ``ENABLE_SME_FOR_NS``: Numeric value to enable Scalable Matrix Extension
    (SME), SVE, and FPU/SIMD for the non-secure world only. These features share
    registers so are enabled together. Using this option without
    ENABLE_SME_FOR_SWD=1 will cause SME, SVE, and FPU/SIMD instructions in secure
    world to trap to EL3. SME is an optional architectural feature for AArch64
    and TF-A support is experimental. At this time, this build option cannot be
    used on systems that have SPD=spmd/SPM_MM or ENABLE_RME, and attempting to
-   build with these options will fail. Default is 0.
+   build with these options will fail. This flag can take the values 0 to 2, to
+   align with the ``FEATURE_DETECTION`` mechanism. Default is 0.
 
 -  ``ENABLE_SME_FOR_SWD``: Boolean option to enable the Scalable Matrix
    Extension for secure world use along with SVE and FPU/SIMD, ENABLE_SME_FOR_NS
@@ -421,12 +422,13 @@
    handle context switching for SME, SVE, and FPU/SIMD registers to ensure that
    no data is leaked to non-secure world. This is experimental. Default is 0.
 
--  ``ENABLE_SPE_FOR_LOWER_ELS`` : Boolean option to enable Statistical Profiling
+-  ``ENABLE_SPE_FOR_NS`` : Numeric value to enable Statistical Profiling
    extensions. This is an optional architectural feature for AArch64.
-   The default is 1 but is automatically disabled when the target architecture
-   is AArch32.
+   This flag can take the values 0 to 2, to align with the ``FEATURE_DETECTION``
+   mechanism. The default is 2 but is automatically disabled when the target
+   architecture is AArch32.
 
--  ``ENABLE_SVE_FOR_NS``: Boolean option to enable Scalable Vector Extension
+-  ``ENABLE_SVE_FOR_NS``: Numeric value to enable Scalable Vector Extension
    (SVE) for the Non-secure world only. SVE is an optional architectural feature
    for AArch64. Note that when SVE is enabled for the Non-secure world, access
    to SIMD and floating-point functionality from the Secure world is disabled by
@@ -435,9 +437,10 @@
    which are aliased by the SIMD and FP registers. The build option is not
    compatible with the ``CTX_INCLUDE_FPREGS`` build option, and will raise an
    assert on platforms where SVE is implemented and ``ENABLE_SVE_FOR_NS`` set to
-   1. The default is 1 but is automatically disabled when ENABLE_SME_FOR_NS=1
-   since SME encompasses SVE. At this time, this build option cannot be used on
-   systems that have SPM_MM enabled.
+   1. This flag can take the values 0 to 2, to align with the ``FEATURE_DETECTION``
+   mechanism. The default is 1 but is automatically disabled when
+   ENABLE_SME_FOR_NS=1 since SME encompasses SVE. At this time, this build
+   option cannot be used on systems that have SPM_MM enabled.
 
 -  ``ENABLE_SVE_FOR_SWD``: Boolean option to enable SVE for the Secure world.
    SVE is an optional architectural feature for AArch64. Note that this option
@@ -733,6 +736,9 @@
    enabled on Arm platforms, the option ``ARM_RECOM_STATE_ID_ENC`` needs to be
    set to 1 as well.
 
+-  ``PSCI_OS_INIT_MODE``: Boolean flag to enable support for optional PSCI
+   OS-initiated mode. This option defaults to 0.
+
 -  ``RAS_EXTENSION``: Numeric value to enable Armv8.2 RAS features. RAS features
    are an optional extension for pre-Armv8.2 CPUs, but are mandatory for Armv8.2
    or later CPUs. This flag can take the values 0 to 2, to align with the
@@ -1071,10 +1077,11 @@
   ``FEATURE_DETECTION`` mechanism. The default is 0 and it is automatically
   disabled when the target architecture is AArch32.
 
-- ``ENABLE_SYS_REG_TRACE_FOR_NS``: Boolean option to enable trace system
+- ``ENABLE_SYS_REG_TRACE_FOR_NS``: Numeric value to enable trace system
   registers access from NS ELs, NS-EL2 or NS-EL1 (when NS-EL2 is implemented
   but unused). This feature is available if trace unit such as ETMv4.x, and
-  ETE(extending ETM feature) is implemented. This flag is disabled by default.
+  ETE(extending ETM feature) is implemented. This flag can take the values
+  0 to 2, to align with the ``FEATURE_DETECTION`` mechanism. The default is 0.
 
 - ``ENABLE_TRF_FOR_NS``: Numeric value to enable trace filter control registers
   access from NS ELs, NS-EL2 or NS-EL1 (when NS-EL2 is implemented but unused),
diff --git a/docs/getting_started/porting-guide.rst b/docs/getting_started/porting-guide.rst
index 6735cb1..8d6a2bf 100644
--- a/docs/getting_started/porting-guide.rst
+++ b/docs/getting_started/porting-guide.rst
@@ -538,6 +538,15 @@
 
    Defines the maximum address that the TSP's progbits sections can occupy.
 
+If the platform supports OS-initiated mode, i.e. the build option
+``PSCI_OS_INIT_MODE`` is enabled, and if the platform's maximum power domain
+level for PSCI_CPU_SUSPEND differs from ``PLAT_MAX_PWR_LVL``, the following
+constant must be defined.
+
+-  **#define : PLAT_MAX_CPU_SUSPEND_PWR_LVL**
+
+   Defines the maximum power domain level that PSCI_CPU_SUSPEND should apply to.
+
 If the platform port uses the PL061 GPIO driver, the following constant may
 optionally be defined:
 
@@ -2810,6 +2819,10 @@
 data, for example in DRAM. The Distributor can then be powered down using an
 implementation-defined sequence.
 
+If the build option ``PSCI_OS_INIT_MODE`` is enabled, the generic code expects
+the platform to return PSCI_E_SUCCESS on success, or either PSCI_E_DENIED or
+PSCI_E_INVALID_PARAMS as appropriate for any invalid requests.
+
 plat_psci_ops.pwr_domain_pwr_down_wfi()
 .......................................
 
diff --git a/docs/resources/diagrams/psci-flattened-vs-hierarchical-idle-states.png b/docs/resources/diagrams/psci-flattened-vs-hierarchical-idle-states.png
new file mode 100644
index 0000000..7c41f75
--- /dev/null
+++ b/docs/resources/diagrams/psci-flattened-vs-hierarchical-idle-states.png
Binary files differ
diff --git a/docs/resources/diagrams/psci-osi-mode.png b/docs/resources/diagrams/psci-osi-mode.png
new file mode 100644
index 0000000..d322953
--- /dev/null
+++ b/docs/resources/diagrams/psci-osi-mode.png
Binary files differ
diff --git a/docs/resources/diagrams/psci-pc-mode-vs-osi-mode.png b/docs/resources/diagrams/psci-pc-mode-vs-osi-mode.png
new file mode 100644
index 0000000..7270a3d
--- /dev/null
+++ b/docs/resources/diagrams/psci-pc-mode-vs-osi-mode.png
Binary files differ
diff --git a/drivers/arm/gic/v3/gic600_multichip.c b/drivers/arm/gic/v3/gic600_multichip.c
index e85dbc1..f26e056 100644
--- a/drivers/arm/gic/v3/gic600_multichip.c
+++ b/drivers/arm/gic/v3/gic600_multichip.c
@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2019, Arm Limited. All rights reserved.
- * Copyright (c) 2022, NVIDIA Corporation. All rights reserved.
+ * Copyright (c) 2022-2023, NVIDIA Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,6 +19,29 @@
 #include "../common/gic_common_private.h"
 #include "gic600_multichip_private.h"
 
+static struct gic600_multichip_data *plat_gic_multichip_data;
+
+/*******************************************************************************
+ * Retrieve the address of the chip owner for a given SPI ID
+ ******************************************************************************/
+uintptr_t gic600_multichip_gicd_base_for_spi(uint32_t spi_id)
+{
+	unsigned int i;
+
+	/* Find the multichip instance */
+	for (i = 0U; i < GIC600_MAX_MULTICHIP; i++) {
+		if ((spi_id <= plat_gic_multichip_data->spi_ids[i].spi_id_max) &&
+		     (spi_id >= plat_gic_multichip_data->spi_ids[i].spi_id_min)) {
+			break;
+		}
+	}
+
+	/* Ensure that plat_gic_multichip_data contains valid values */
+	assert(i < GIC600_MAX_MULTICHIP);
+
+	return plat_gic_multichip_data->spi_ids[i].gicd_base;
+}
+
 /*******************************************************************************
  * GIC-600 multichip operation related helper functions
  ******************************************************************************/
@@ -27,7 +50,7 @@
 	unsigned int retry = GICD_PUP_UPDATE_RETRIES;
 
 	while ((read_gicd_dchipr(base) & GICD_DCHIPR_PUP_BIT) != 0U) {
-		if (retry-- == 0) {
+		if (retry-- == 0U) {
 			ERROR("GIC-600 connection to Routing Table Owner timed "
 					 "out\n");
 			panic();
@@ -186,11 +209,11 @@
 		panic();
 	}
 
-	for (i = 0; i < multichip_data->chip_count; i++) {
-		spi_id_min = multichip_data->spi_ids[i][SPI_MIN_INDEX];
-		spi_id_max = multichip_data->spi_ids[i][SPI_MAX_INDEX];
+	for (i = 0U; i < multichip_data->chip_count; i++) {
+		spi_id_min = multichip_data->spi_ids[i].spi_id_min;
+		spi_id_max = multichip_data->spi_ids[i].spi_id_max;
 
-		if ((spi_id_min != 0) || (spi_id_max != 0)) {
+		if ((spi_id_min != 0U) || (spi_id_max != 0U)) {
 
 			/* SPI IDs range check */
 			if (!(spi_id_min >= GIC600_SPI_ID_MIN) ||
@@ -232,8 +255,8 @@
 	}
 
 	for (i = 0U; i < multichip_data->chip_count; i++) {
-		spi_id_min = multichip_data->spi_ids[i][SPI_MIN_INDEX];
-		spi_id_max = multichip_data->spi_ids[i][SPI_MAX_INDEX];
+		spi_id_min = multichip_data->spi_ids[i].spi_id_min;
+		spi_id_max = multichip_data->spi_ids[i].spi_id_max;
 
 		if ((spi_id_min == 0U) || (spi_id_max == 0U)) {
 			continue;
@@ -342,9 +365,9 @@
 	set_gicd_chipr_n(multichip_data->rt_owner_base, multichip_data->rt_owner,
 			multichip_data->chip_addrs[multichip_data->rt_owner],
 			multichip_data->
-			spi_ids[multichip_data->rt_owner][SPI_MIN_INDEX],
+			spi_ids[multichip_data->rt_owner].spi_id_min,
 			multichip_data->
-			spi_ids[multichip_data->rt_owner][SPI_MAX_INDEX]);
+			spi_ids[multichip_data->rt_owner].spi_id_max);
 
 	for (i = 0; i < multichip_data->chip_count; i++) {
 		if (i == multichip_data->rt_owner)
@@ -352,7 +375,17 @@
 
 		set_gicd_chipr_n(multichip_data->rt_owner_base, i,
 				multichip_data->chip_addrs[i],
-				multichip_data->spi_ids[i][SPI_MIN_INDEX],
-				multichip_data->spi_ids[i][SPI_MAX_INDEX]);
+				multichip_data->spi_ids[i].spi_id_min,
+				multichip_data->spi_ids[i].spi_id_max);
 	}
+
+	plat_gic_multichip_data = multichip_data;
+}
+
+/*******************************************************************************
+ * Allow a way to query the status of the GIC600 multichip driver
+ ******************************************************************************/
+bool gic600_multichip_is_initialized(void)
+{
+	return (plat_gic_multichip_data != NULL);
 }
diff --git a/drivers/arm/gic/v3/gic600_multichip_private.h b/drivers/arm/gic/v3/gic600_multichip_private.h
index 414bd5b..f6028ad 100644
--- a/drivers/arm/gic/v3/gic600_multichip_private.h
+++ b/drivers/arm/gic/v3/gic600_multichip_private.h
@@ -49,9 +49,6 @@
 /* Number of retries for PUP update */
 #define GICD_PUP_UPDATE_RETRIES		10000
 
-#define SPI_MIN_INDEX			0
-#define SPI_MAX_INDEX			1
-
 #define SPI_BLOCK_MIN_VALUE(spi_id_min) \
 			(((spi_id_min) - GIC600_SPI_ID_MIN) / \
 			GIC600_SPI_ID_MIN)
diff --git a/drivers/arm/gic/v3/gicv3.mk b/drivers/arm/gic/v3/gicv3.mk
index 1d20ff3..89bce95 100644
--- a/drivers/arm/gic/v3/gicv3.mk
+++ b/drivers/arm/gic/v3/gicv3.mk
@@ -41,6 +41,10 @@
 $(eval $(call assert_boolean,GICV3_SUPPORT_GIC600AE_FMU))
 $(eval $(call add_define,GICV3_SUPPORT_GIC600AE_FMU))
 
+# Set GIC-600 multichip support
+$(eval $(call assert_boolean,GICV3_IMPL_GIC600_MULTICHIP))
+$(eval $(call add_define,GICV3_IMPL_GIC600_MULTICHIP))
+
 # Set GICv4 extension
 $(eval $(call assert_boolean,GIC_ENABLE_V4_EXTN))
 $(eval $(call add_define,GIC_ENABLE_V4_EXTN))
diff --git a/drivers/arm/gic/v3/gicv3_helpers.c b/drivers/arm/gic/v3/gicv3_helpers.c
index 940c939..00bd7a1 100644
--- a/drivers/arm/gic/v3/gicv3_helpers.c
+++ b/drivers/arm/gic/v3/gicv3_helpers.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2023, NVIDIA Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,6 +11,7 @@
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <common/interrupt_props.h>
+#include <drivers/arm/gic600_multichip.h>
 #include <drivers/arm/gic_common.h>
 
 #include <platform_def.h>
@@ -17,6 +19,16 @@
 #include "../common/gic_common_private.h"
 #include "gicv3_private.h"
 
+uintptr_t gicv3_get_multichip_base(uint32_t spi_id, uintptr_t gicd_base)
+{
+#if GICV3_IMPL_GIC600_MULTICHIP
+	if (gic600_multichip_is_initialized()) {
+		return gic600_multichip_gicd_base_for_spi(spi_id);
+	}
+#endif
+	return gicd_base;
+}
+
 /******************************************************************************
  * This function marks the core as awake in the re-distributor and
  * ensures that the interface is active.
@@ -148,7 +160,7 @@
 
 	/* Treat all (E)SPIs as G1NS by default. We do 32 at a time. */
 	for (i = MIN_SPI_ID; i < num_ints; i += (1U << IGROUPR_SHIFT)) {
-		gicd_write_igroupr(gicd_base, i, ~0U);
+		gicd_write_igroupr(gicv3_get_multichip_base(i, gicd_base), i, ~0U);
 	}
 
 #if GIC_EXT_INTID
@@ -158,7 +170,7 @@
 
 		for (i = MIN_ESPI_ID; i < num_eints;
 					i += (1U << IGROUPR_SHIFT)) {
-			gicd_write_igroupr(gicd_base, i, ~0U);
+			gicd_write_igroupr(gicv3_get_multichip_base(i, gicd_base), i, ~0U);
 		}
 	} else {
 		INFO("ESPI range is not implemented.\n");
@@ -167,25 +179,25 @@
 
 	/* Setup the default (E)SPI priorities doing four at a time */
 	for (i = MIN_SPI_ID; i < num_ints; i += (1U << IPRIORITYR_SHIFT)) {
-		gicd_write_ipriorityr(gicd_base, i, GICD_IPRIORITYR_DEF_VAL);
+		gicd_write_ipriorityr(gicv3_get_multichip_base(i, gicd_base), i, GICD_IPRIORITYR_DEF_VAL);
 	}
 
 #if GIC_EXT_INTID
 	for (i = MIN_ESPI_ID; i < num_eints;
 					i += (1U << IPRIORITYR_SHIFT)) {
-		gicd_write_ipriorityr(gicd_base, i, GICD_IPRIORITYR_DEF_VAL);
+		gicd_write_ipriorityr(gicv3_get_multichip_base(i, gicd_base), i, GICD_IPRIORITYR_DEF_VAL);
 	}
 #endif
 	/*
 	 * Treat all (E)SPIs as level triggered by default, write 16 at a time
 	 */
 	for (i = MIN_SPI_ID; i < num_ints; i += (1U << ICFGR_SHIFT)) {
-		gicd_write_icfgr(gicd_base, i, 0U);
+		gicd_write_icfgr(gicv3_get_multichip_base(i, gicd_base), i, 0U);
 	}
 
 #if GIC_EXT_INTID
 	for (i = MIN_ESPI_ID; i < num_eints; i += (1U << ICFGR_SHIFT)) {
-		gicd_write_icfgr(gicd_base, i, 0U);
+		gicd_write_icfgr(gicv3_get_multichip_base(i, gicd_base), i, 0U);
 	}
 #endif
 }
@@ -211,6 +223,7 @@
 		current_prop = &interrupt_props[i];
 
 		unsigned int intr_num = current_prop->intr_num;
+		uintptr_t multichip_gicd_base = gicv3_get_multichip_base(intr_num, gicd_base);
 
 		/* Skip SGI, (E)PPI and LPI interrupts */
 		if (!IS_SPI(intr_num)) {
@@ -218,35 +231,36 @@
 		}
 
 		/* Configure this interrupt as a secure interrupt */
-		gicd_clr_igroupr(gicd_base, intr_num);
+		gicd_clr_igroupr(multichip_gicd_base, intr_num);
 
 		/* Configure this interrupt as G0 or a G1S interrupt */
 		assert((current_prop->intr_grp == INTR_GROUP0) ||
 				(current_prop->intr_grp == INTR_GROUP1S));
 
 		if (current_prop->intr_grp == INTR_GROUP1S) {
-			gicd_set_igrpmodr(gicd_base, intr_num);
+			gicd_set_igrpmodr(multichip_gicd_base, intr_num);
 			ctlr_enable |= CTLR_ENABLE_G1S_BIT;
 		} else {
-			gicd_clr_igrpmodr(gicd_base, intr_num);
+			gicd_clr_igrpmodr(multichip_gicd_base, intr_num);
 			ctlr_enable |= CTLR_ENABLE_G0_BIT;
 		}
 
 		/* Set interrupt configuration */
-		gicd_set_icfgr(gicd_base, intr_num, current_prop->intr_cfg);
+		gicd_set_icfgr(multichip_gicd_base, intr_num,
+				current_prop->intr_cfg);
 
 		/* Set the priority of this interrupt */
-		gicd_set_ipriorityr(gicd_base, intr_num,
-					current_prop->intr_pri);
+		gicd_set_ipriorityr(multichip_gicd_base, intr_num,
+				current_prop->intr_pri);
 
 		/* Target (E)SPIs to the primary CPU */
 		gic_affinity_val =
 			gicd_irouter_val_from_mpidr(read_mpidr(), 0U);
-		gicd_write_irouter(gicd_base, intr_num,
-					gic_affinity_val);
+		gicd_write_irouter(multichip_gicd_base, intr_num,
+			gic_affinity_val);
 
 		/* Enable this interrupt */
-		gicd_set_isenabler(gicd_base, intr_num);
+		gicd_set_isenabler(multichip_gicd_base, intr_num);
 	}
 
 	return ctlr_enable;
diff --git a/drivers/arm/gic/v3/gicv3_main.c b/drivers/arm/gic/v3/gicv3_main.c
index f6c251d..168d0eb 100644
--- a/drivers/arm/gic/v3/gicv3_main.c
+++ b/drivers/arm/gic/v3/gicv3_main.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2023, NVIDIA Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,6 +11,7 @@
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <common/interrupt_props.h>
+#include <drivers/arm/gic600_multichip.h>
 #include <drivers/arm/gicv3.h>
 #include <lib/spinlock.h>
 #include <plat/common/platform.h>
@@ -430,6 +432,7 @@
 {
 	unsigned int igroup, grpmodr;
 	uintptr_t gicr_base;
+	uintptr_t gicd_base;
 
 	assert(IS_IN_EL3());
 	assert(gicv3_driver_data != NULL);
@@ -453,8 +456,9 @@
 	} else {
 		/* SPIs: 32-1019, ESPIs: 4096-5119 */
 		assert(gicv3_driver_data->gicd_base != 0U);
-		igroup = gicd_get_igroupr(gicv3_driver_data->gicd_base, id);
-		grpmodr = gicd_get_igrpmodr(gicv3_driver_data->gicd_base, id);
+		gicd_base = gicv3_get_multichip_base(id, gicv3_driver_data->gicd_base);
+		igroup = gicd_get_igroupr(gicd_base, id);
+		grpmodr = gicd_get_igrpmodr(gicd_base, id);
 	}
 
 	/*
@@ -930,6 +934,8 @@
  ******************************************************************************/
 unsigned int gicv3_get_interrupt_active(unsigned int id, unsigned int proc_num)
 {
+	uintptr_t gicd_base;
+
 	assert(gicv3_driver_data != NULL);
 	assert(gicv3_driver_data->gicd_base != 0U);
 	assert(proc_num < gicv3_driver_data->rdistif_num);
@@ -943,7 +949,8 @@
 	}
 
 	/* For SPIs: 32-1019 and ESPIs: 4096-5119 */
-	return gicd_get_isactiver(gicv3_driver_data->gicd_base, id);
+	gicd_base = gicv3_get_multichip_base(id, gicv3_driver_data->gicd_base);
+	return gicd_get_isactiver(gicd_base, id);
 }
 
 /*******************************************************************************
@@ -953,6 +960,8 @@
  ******************************************************************************/
 void gicv3_enable_interrupt(unsigned int id, unsigned int proc_num)
 {
+	uintptr_t gicd_base;
+
 	assert(gicv3_driver_data != NULL);
 	assert(gicv3_driver_data->gicd_base != 0U);
 	assert(proc_num < gicv3_driver_data->rdistif_num);
@@ -971,7 +980,8 @@
 			gicv3_driver_data->rdistif_base_addrs[proc_num], id);
 	} else {
 		/* For SPIs: 32-1019 and ESPIs: 4096-5119 */
-		gicd_set_isenabler(gicv3_driver_data->gicd_base, id);
+		gicd_base = gicv3_get_multichip_base(id, gicv3_driver_data->gicd_base);
+		gicd_set_isenabler(gicd_base, id);
 	}
 }
 
@@ -982,6 +992,8 @@
  ******************************************************************************/
 void gicv3_disable_interrupt(unsigned int id, unsigned int proc_num)
 {
+	uintptr_t gicd_base;
+
 	assert(gicv3_driver_data != NULL);
 	assert(gicv3_driver_data->gicd_base != 0U);
 	assert(proc_num < gicv3_driver_data->rdistif_num);
@@ -1003,10 +1015,11 @@
 			gicv3_driver_data->rdistif_base_addrs[proc_num]);
 	} else {
 		/* For SPIs: 32-1019 and ESPIs: 4096-5119 */
-		gicd_set_icenabler(gicv3_driver_data->gicd_base, id);
+		gicd_base = gicv3_get_multichip_base(id, gicv3_driver_data->gicd_base);
+		gicd_set_icenabler(gicd_base, id);
 
 		/* Write to clear enable requires waiting for pending writes */
-		gicd_wait_for_pending_write(gicv3_driver_data->gicd_base);
+		gicd_wait_for_pending_write(gicd_base);
 	}
 
 	dsbishst();
@@ -1020,6 +1033,7 @@
 		unsigned int priority)
 {
 	uintptr_t gicr_base;
+	uintptr_t gicd_base;
 
 	assert(gicv3_driver_data != NULL);
 	assert(gicv3_driver_data->gicd_base != 0U);
@@ -1033,7 +1047,8 @@
 		gicr_set_ipriorityr(gicr_base, id, priority);
 	} else {
 		/* For SPIs: 32-1019 and ESPIs: 4096-5119 */
-		gicd_set_ipriorityr(gicv3_driver_data->gicd_base, id, priority);
+		gicd_base = gicv3_get_multichip_base(id, gicv3_driver_data->gicd_base);
+		gicd_set_ipriorityr(gicd_base, id, priority);
 	}
 }
 
@@ -1047,6 +1062,7 @@
 {
 	bool igroup = false, grpmod = false;
 	uintptr_t gicr_base;
+	uintptr_t gicd_base;
 
 	assert(gicv3_driver_data != NULL);
 	assert(gicv3_driver_data->gicd_base != 0U);
@@ -1086,10 +1102,12 @@
 		/* Serialize read-modify-write to Distributor registers */
 		spin_lock(&gic_lock);
 
-		igroup ? gicd_set_igroupr(gicv3_driver_data->gicd_base, id) :
-			 gicd_clr_igroupr(gicv3_driver_data->gicd_base, id);
-		grpmod ? gicd_set_igrpmodr(gicv3_driver_data->gicd_base, id) :
-			 gicd_clr_igrpmodr(gicv3_driver_data->gicd_base, id);
+		gicd_base = gicv3_get_multichip_base(id, gicv3_driver_data->gicd_base);
+
+		igroup ? gicd_set_igroupr(gicd_base, id) :
+			 gicd_clr_igroupr(gicd_base, id);
+		grpmod ? gicd_set_igrpmodr(gicd_base, id) :
+			 gicd_clr_igrpmodr(gicd_base, id);
 
 		spin_unlock(&gic_lock);
 	}
@@ -1165,6 +1183,7 @@
 {
 	unsigned long long aff;
 	uint64_t router;
+	uintptr_t gicd_base;
 
 	assert(gicv3_driver_data != NULL);
 	assert(gicv3_driver_data->gicd_base != 0U);
@@ -1174,14 +1193,15 @@
 	assert(IS_SPI(id));
 
 	aff = gicd_irouter_val_from_mpidr(mpidr, irm);
-	gicd_write_irouter(gicv3_driver_data->gicd_base, id, aff);
+	gicd_base = gicv3_get_multichip_base(id, gicv3_driver_data->gicd_base);
+	gicd_write_irouter(gicd_base, id, aff);
 
 	/*
 	 * In implementations that do not require 1 of N distribution of SPIs,
 	 * IRM might be RAZ/WI. Read back and verify IRM bit.
 	 */
 	if (irm == GICV3_IRM_ANY) {
-		router = gicd_read_irouter(gicv3_driver_data->gicd_base, id);
+		router = gicd_read_irouter(gicd_base, id);
 		if (((router >> IROUTER_IRM_SHIFT) & IROUTER_IRM_MASK) == 0U) {
 			ERROR("GICv3 implementation doesn't support routing ANY\n");
 			panic();
@@ -1196,6 +1216,8 @@
  ******************************************************************************/
 void gicv3_clear_interrupt_pending(unsigned int id, unsigned int proc_num)
 {
+	uintptr_t gicd_base;
+
 	assert(gicv3_driver_data != NULL);
 	assert(gicv3_driver_data->gicd_base != 0U);
 	assert(proc_num < gicv3_driver_data->rdistif_num);
@@ -1213,7 +1235,8 @@
 			gicv3_driver_data->rdistif_base_addrs[proc_num], id);
 	} else {
 		/* For SPIs: 32-1019 and ESPIs: 4096-5119 */
-		gicd_set_icpendr(gicv3_driver_data->gicd_base, id);
+		gicd_base = gicv3_get_multichip_base(id, gicv3_driver_data->gicd_base);
+		gicd_set_icpendr(gicd_base, id);
 	}
 
 	dsbishst();
@@ -1226,6 +1249,8 @@
  ******************************************************************************/
 void gicv3_set_interrupt_pending(unsigned int id, unsigned int proc_num)
 {
+	uintptr_t gicd_base;
+
 	assert(gicv3_driver_data != NULL);
 	assert(gicv3_driver_data->gicd_base != 0U);
 	assert(proc_num < gicv3_driver_data->rdistif_num);
@@ -1244,7 +1269,8 @@
 			gicv3_driver_data->rdistif_base_addrs[proc_num], id);
 	} else {
 		/* For SPIs: 32-1019 and ESPIs: 4096-5119 */
-		gicd_set_ispendr(gicv3_driver_data->gicd_base, id);
+		gicd_base = gicv3_get_multichip_base(id, gicv3_driver_data->gicd_base);
+		gicd_set_ispendr(gicd_base, id);
 	}
 }
 
diff --git a/drivers/arm/gic/v3/gicv3_private.h b/drivers/arm/gic/v3/gicv3_private.h
index 3af0500..8ad251b 100644
--- a/drivers/arm/gic/v3/gicv3_private.h
+++ b/drivers/arm/gic/v3/gicv3_private.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2023, NVIDIA Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -233,6 +234,7 @@
 /*******************************************************************************
  * Private GICv3 helper function prototypes
  ******************************************************************************/
+uintptr_t gicv3_get_multichip_base(uint32_t spi_id, uintptr_t gicd_base);
 unsigned int gicv3_get_spi_limit(uintptr_t gicd_base);
 unsigned int gicv3_get_espi_limit(uintptr_t gicd_base);
 void gicv3_spis_config_defaults(uintptr_t gicd_base);
diff --git a/drivers/mmc/mmc.c b/drivers/mmc/mmc.c
index 2b727d4..57f4748 100644
--- a/drivers/mmc/mmc.c
+++ b/drivers/mmc/mmc.c
@@ -452,11 +452,6 @@
 	int ret, n;
 	unsigned int resp_data[4];
 
-	ret = mmc_reset_to_idle();
-	if (ret != 0) {
-		return ret;
-	}
-
 	for (n = 0; n < SEND_OP_COND_MAX_RETRIES; n++) {
 		ret = mmc_send_cmd(MMC_CMD(1), OCR_SECTOR_MODE |
 				   OCR_VDD_MIN_2V7 | OCR_VDD_MIN_1V7,
diff --git a/drivers/nxp/timer/nxp_timer.c b/drivers/nxp/timer/nxp_timer.c
index 8eecd2e..448c0ba 100644
--- a/drivers/nxp/timer/nxp_timer.c
+++ b/drivers/nxp/timer/nxp_timer.c
@@ -59,7 +59,7 @@
 
 static void delay_timer_init_args(uint32_t mult, uint32_t div)
 {
-	ops.get_timer_value	= timer_get_value,
+	ops.get_timer_value	= timer_get_value;
 	ops.clk_mult		= mult;
 	ops.clk_div		= div;
 
diff --git a/drivers/ufs/ufs.c b/drivers/ufs/ufs.c
index 6074374..ea82d02 100644
--- a/drivers/ufs/ufs.c
+++ b/drivers/ufs/ufs.c
@@ -30,9 +30,138 @@
 static ufs_params_t ufs_params;
 static int nutrs;	/* Number of UTP Transfer Request Slots */
 
+/*
+ * ufs_uic_error_handler - UIC error interrupts handler
+ * @ignore_linereset: set to ignore PA_LAYER_GEN_ERR (UIC error)
+ *
+ * Returns
+ * 0 - ignore error
+ * -EIO - fatal error, needs re-init
+ * -EAGAIN - non-fatal error, retries are sufficient
+ */
+static int ufs_uic_error_handler(bool ignore_linereset)
+{
+	uint32_t data;
+	int result = 0;
+
+	data = mmio_read_32(ufs_params.reg_base + UECPA);
+	if (data & UFS_UIC_PA_ERROR_MASK) {
+		if (data & PA_LAYER_GEN_ERR) {
+			if (!ignore_linereset) {
+				return -EIO;
+			}
+		} else {
+			result = -EAGAIN;
+		}
+	}
+
+	data = mmio_read_32(ufs_params.reg_base + UECDL);
+	if (data & UFS_UIC_DL_ERROR_MASK) {
+		if (data & PA_INIT_ERR) {
+			return -EIO;
+		}
+		result = -EAGAIN;
+	}
+
+	/* NL/TL/DME error requires retries */
+	data = mmio_read_32(ufs_params.reg_base + UECN);
+	if (data & UFS_UIC_NL_ERROR_MASK) {
+		result = -EAGAIN;
+	}
+
+	data = mmio_read_32(ufs_params.reg_base + UECT);
+	if (data & UFS_UIC_TL_ERROR_MASK) {
+		result = -EAGAIN;
+	}
+
+	data = mmio_read_32(ufs_params.reg_base + UECDME);
+	if (data & UFS_UIC_DME_ERROR_MASK) {
+		result = -EAGAIN;
+	}
+
+	return result;
+}
+
+/*
+ * ufs_error_handler - error interrupts handler
+ * @status: interrupt status
+ * @ignore_linereset: set to ignore PA_LAYER_GEN_ERR (UIC error)
+ *
+ * Returns
+ * 0 - ignore error
+ * -EIO - fatal error, needs re-init
+ * -EAGAIN - non-fatal error, retries are sufficient
+ */
+static int ufs_error_handler(uint32_t status, bool ignore_linereset)
+{
+	int result;
+
+	if (status & UFS_INT_UE) {
+		result = ufs_uic_error_handler(ignore_linereset);
+		if (result != 0) {
+			return result;
+		}
+	}
+
+	/* Return I/O error on fatal error, it is upto the caller to re-init UFS */
+	if (status & UFS_INT_FATAL) {
+		return -EIO;
+	}
+
+	/* retry for non-fatal errors */
+	return -EAGAIN;
+}
+
+/*
+ * ufs_wait_for_int_status - wait for expected interrupt status
+ * @expected: expected interrupt status bit
+ * @timeout_ms: timeout in milliseconds to poll for
+ * @ignore_linereset: set to ignore PA_LAYER_GEN_ERR (UIC error)
+ *
+ * Returns
+ * 0 - received expected interrupt and cleared it
+ * -EIO - fatal error, needs re-init
+ * -EAGAIN - non-fatal error, caller can retry
+ * -ETIMEDOUT - timed out waiting for interrupt status
+ */
+static int ufs_wait_for_int_status(const uint32_t expected_status,
+				   unsigned int timeout_ms,
+				   bool ignore_linereset)
+{
+	uint32_t interrupt_status, interrupts_enabled;
+	int result = 0;
+
+	interrupts_enabled = mmio_read_32(ufs_params.reg_base + IE);
+	do {
+		interrupt_status = mmio_read_32(ufs_params.reg_base + IS) & interrupts_enabled;
+		if (interrupt_status & UFS_INT_ERR) {
+			mmio_write_32(ufs_params.reg_base + IS, interrupt_status & UFS_INT_ERR);
+			result = ufs_error_handler(interrupt_status, ignore_linereset);
+			if (result != 0) {
+				return result;
+			}
+		}
+
+		if (interrupt_status & expected_status) {
+			break;
+		}
+		mdelay(1);
+	} while (timeout_ms-- > 0);
+
+	if (!(interrupt_status & expected_status)) {
+		return -ETIMEDOUT;
+	}
+
+	mmio_write_32(ufs_params.reg_base + IS, expected_status);
+
+	return result;
+}
+
+
 int ufshc_send_uic_cmd(uintptr_t base, uic_cmd_t *cmd)
 {
 	unsigned int data;
+	int result;
 
 	if (base == 0 || cmd == NULL)
 		return -EINVAL;
@@ -46,10 +175,12 @@
 	mmio_write_32(base + UCMDARG3, cmd->arg3);
 	mmio_write_32(base + UICCMD, cmd->op);
 
-	do {
-		data = mmio_read_32(base + IS);
-	} while ((data & UFS_INT_UCCS) == 0);
-	mmio_write_32(base + IS, UFS_INT_UCCS);
+	result = ufs_wait_for_int_status(UFS_INT_UCCS, UIC_CMD_TIMEOUT_MS,
+					 cmd->op == DME_SET);
+	if (result != 0) {
+		return result;
+	}
+
 	return mmio_read_32(base + UCMDARG2) & CONFIG_RESULT_CODE_MASK;
 }
 
@@ -83,9 +214,10 @@
 		result = ufshc_send_uic_cmd(base, &cmd);
 		if (result == 0)
 			break;
-		data = mmio_read_32(base + IS);
-		if (data & UFS_INT_UE)
-			return -EINVAL;
+		/* -EIO requires UFS re-init */
+		if (result == -EIO) {
+			return result;
+		}
 	}
 	if (retries >= UFS_UIC_COMMAND_RETRIES)
 		return -EIO;
@@ -97,7 +229,6 @@
 int ufshc_dme_set(unsigned int attr, unsigned int idx, unsigned int val)
 {
 	uintptr_t base;
-	unsigned int data;
 	int result, retries;
 	uic_cmd_t cmd;
 
@@ -113,9 +244,10 @@
 		result = ufshc_send_uic_cmd(base, &cmd);
 		if (result == 0)
 			break;
-		data = mmio_read_32(base + IS);
-		if (data & UFS_INT_UE)
-			return -EINVAL;
+		/* -EIO requires UFS re-init */
+		if (result == -EIO) {
+			return result;
+		}
 	}
 	if (retries >= UFS_UIC_COMMAND_RETRIES)
 		return -EIO;
@@ -193,9 +325,10 @@
 		return -EIO;
 	}
 
-	/* Enable Interrupts */
-	data = UFS_INT_UCCS | UFS_INT_ULSS | UFS_INT_UE | UFS_INT_UTPES |
-	       UFS_INT_DFES | UFS_INT_HCFES | UFS_INT_SBFES;
+	/* Enable UIC Interrupts alone. We can ignore other interrupts until
+	 * link is up as there might be spurious error interrupts during link-up
+	 */
+	data = UFS_INT_UCCS | UFS_INT_UHES | UFS_INT_UHXS | UFS_INT_UPMS;
 	mmio_write_32(base + IE, data);
 
 	return 0;
@@ -229,6 +362,13 @@
 		data = mmio_read_32(base + IS);
 		if (data & UFS_INT_ULSS)
 			mmio_write_32(base + IS, UFS_INT_ULSS);
+
+		/* clear UE set due to line-reset */
+		if (data & UFS_INT_UE) {
+			mmio_write_32(base + IS, UFS_INT_UE);
+		}
+		/* clearing line-reset, UECPA is cleared on read */
+		mmio_read_32(base + UECPA);
 		return 0;
 	}
 	return -EIO;
@@ -472,21 +612,22 @@
 	mmio_setbits_32(ufs_params.reg_base + UTRLDBR, 1 << slot);
 }
 
-static int ufs_check_resp(utp_utrd_t *utrd, int trans_type)
+static int ufs_check_resp(utp_utrd_t *utrd, int trans_type, unsigned int timeout_ms)
 {
 	utrd_header_t *hd;
 	resp_upiu_t *resp;
 	sense_data_t *sense;
 	unsigned int data;
-	int slot;
+	int slot, result;
 
 	hd = (utrd_header_t *)utrd->header;
 	resp = (resp_upiu_t *)utrd->resp_upiu;
-	do {
-		data = mmio_read_32(ufs_params.reg_base + IS);
-		if ((data & ~(UFS_INT_UCCS | UFS_INT_UTRCS)) != 0)
-			return -EIO;
-	} while ((data & UFS_INT_UTRCS) == 0);
+
+	result = ufs_wait_for_int_status(UFS_INT_UTRCS, timeout_ms, false);
+	if (result != 0) {
+		return result;
+	}
+
 	slot = utrd->task_tag - 1;
 
 	data = mmio_read_32(ufs_params.reg_base + UTRLDBR);
@@ -510,6 +651,7 @@
 
 	(void)resp;
 	(void)slot;
+	(void)data;
 	return 0;
 }
 
@@ -523,7 +665,7 @@
 		result = ufs_prepare_cmd(utrd, cmd_op, lun, lba, buf, length);
 		assert(result == 0);
 		ufs_send_request(utrd->task_tag);
-		result = ufs_check_resp(utrd, RESPONSE_UPIU);
+		result = ufs_check_resp(utrd, RESPONSE_UPIU, CMD_TIMEOUT_MS);
 		if (result == 0 || result == -EIO) {
 			break;
 		}
@@ -574,7 +716,7 @@
 	get_utrd(&utrd);
 	ufs_prepare_nop_out(&utrd);
 	ufs_send_request(utrd.task_tag);
-	result = ufs_check_resp(&utrd, NOP_IN_UPIU);
+	result = ufs_check_resp(&utrd, NOP_IN_UPIU, NOP_OUT_TIMEOUT_MS);
 	assert(result == 0);
 	(void)result;
 }
@@ -607,7 +749,7 @@
 	get_utrd(&utrd);
 	ufs_prepare_query(&utrd, op, idn, index, sel, buf, size);
 	ufs_send_request(utrd.task_tag);
-	result = ufs_check_resp(&utrd, QUERY_RESPONSE_UPIU);
+	result = ufs_check_resp(&utrd, QUERY_RESPONSE_UPIU, QUERY_REQ_TIMEOUT_MS);
 	assert(result == 0);
 	resp = (query_resp_upiu_t *)utrd.resp_upiu;
 #ifdef UFS_RESP_DEBUG
@@ -895,6 +1037,11 @@
 		result = ufshc_link_startup(ufs_params.reg_base);
 		assert(result == 0);
 
+		/* enable all interrupts */
+		data = UFS_INT_UCCS | UFS_INT_UHES | UFS_INT_UHXS | UFS_INT_UPMS;
+		data |= UFS_INT_UTRCS | UFS_INT_ERR;
+		mmio_write_32(ufs_params.reg_base + IE, data);
+
 		ufs_enum();
 
 		ufs_get_device_info(&card);
diff --git a/fdts/stm32mp15-ddr3-1x2Gb-1066-binG.dtsi b/fdts/stm32mp15-ddr3-1x2Gb-1066-binG.dtsi
new file mode 100644
index 0000000..332d1bf
--- /dev/null
+++ b/fdts/stm32mp15-ddr3-1x2Gb-1066-binG.dtsi
@@ -0,0 +1,119 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) 2023, STMicroelectronics - All Rights Reserved
+ */
+
+/*
+ * File generated by STMicroelectronics STM32CubeMX DDR Tool for MPUs
+ * DDR type: DDR3 / DDR3L
+ * DDR width: 16bits
+ * DDR density: 4Gb
+ * System frequency: 533000Khz
+ * Relaxed Timing Mode: false
+ * Address mapping type: RBC
+ *
+ * Save Date: 2020.02.20, save Time: 18:45:20
+ */
+
+#define DDR_MEM_NAME	"DDR3-DDR3L 16bits 533000kHz"
+#define DDR_MEM_SPEED	533000
+#define DDR_MEM_SIZE	0x10000000
+
+#define DDR_MSTR 0x00041401
+#define DDR_MRCTRL0 0x00000010
+#define DDR_MRCTRL1 0x00000000
+#define DDR_DERATEEN 0x00000000
+#define DDR_DERATEINT 0x00800000
+#define DDR_PWRCTL 0x00000000
+#define DDR_PWRTMG 0x00400010
+#define DDR_HWLPCTL 0x00000000
+#define DDR_RFSHCTL0 0x00210000
+#define DDR_RFSHCTL3 0x00000000
+#define DDR_RFSHTMG 0x0040008B
+#define DDR_CRCPARCTL0 0x00000000
+#define DDR_DRAMTMG0 0x121B1214
+#define DDR_DRAMTMG1 0x000A041C
+#define DDR_DRAMTMG2 0x0608090F
+#define DDR_DRAMTMG3 0x0050400C
+#define DDR_DRAMTMG4 0x08040608
+#define DDR_DRAMTMG5 0x06060403
+#define DDR_DRAMTMG6 0x02020002
+#define DDR_DRAMTMG7 0x00000202
+#define DDR_DRAMTMG8 0x00001005
+#define DDR_DRAMTMG14 0x000000A0
+#define DDR_ZQCTL0 0xC2000040
+#define DDR_DFITMG0 0x02060105
+#define DDR_DFITMG1 0x00000202
+#define DDR_DFILPCFG0 0x07000000
+#define DDR_DFIUPD0 0xC0400003
+#define DDR_DFIUPD1 0x00000000
+#define DDR_DFIUPD2 0x00000000
+#define DDR_DFIPHYMSTR 0x00000000
+#define DDR_ODTCFG 0x06000600
+#define DDR_ODTMAP 0x00000001
+#define DDR_SCHED 0x00000C01
+#define DDR_SCHED1 0x00000000
+#define DDR_PERFHPR1 0x01000001
+#define DDR_PERFLPR1 0x08000200
+#define DDR_PERFWR1 0x08000400
+#define DDR_DBG0 0x00000000
+#define DDR_DBG1 0x00000000
+#define DDR_DBGCMD 0x00000000
+#define DDR_POISONCFG 0x00000000
+#define DDR_PCCFG 0x00000010
+#define DDR_PCFGR_0 0x00010000
+#define DDR_PCFGW_0 0x00000000
+#define DDR_PCFGQOS0_0 0x02100C03
+#define DDR_PCFGQOS1_0 0x00800100
+#define DDR_PCFGWQOS0_0 0x01100C03
+#define DDR_PCFGWQOS1_0 0x01000200
+#define DDR_PCFGR_1 0x00010000
+#define DDR_PCFGW_1 0x00000000
+#define DDR_PCFGQOS0_1 0x02100C03
+#define DDR_PCFGQOS1_1 0x00800040
+#define DDR_PCFGWQOS0_1 0x01100C03
+#define DDR_PCFGWQOS1_1 0x01000200
+#define DDR_ADDRMAP1 0x00151515
+#define DDR_ADDRMAP2 0x00000000
+#define DDR_ADDRMAP3 0x1F000000
+#define DDR_ADDRMAP4 0x00001F1F
+#define DDR_ADDRMAP5 0x03030303
+#define DDR_ADDRMAP6 0x0F0F0303
+#define DDR_ADDRMAP9 0x00000000
+#define DDR_ADDRMAP10 0x00000000
+#define DDR_ADDRMAP11 0x00000000
+#define DDR_PGCR 0x01442E02
+#define DDR_PTR0 0x0022AA5B
+#define DDR_PTR1 0x04841104
+#define DDR_PTR2 0x042DA068
+#define DDR_ACIOCR 0x10400812
+#define DDR_DXCCR 0x00000C40
+#define DDR_DSGCR 0xF200011F
+#define DDR_DCR 0x0000000B
+#define DDR_DTPR0 0x38D488D0
+#define DDR_DTPR1 0x098B00D8
+#define DDR_DTPR2 0x10023600
+#define DDR_MR0 0x00000840
+#define DDR_MR1 0x00000000
+#define DDR_MR2 0x00000248
+#define DDR_MR3 0x00000000
+#define DDR_ODTCR 0x00010000
+#define DDR_ZQ0CR1 0x00000038
+#define DDR_DX0GCR 0x0000CE81
+#define DDR_DX0DLLCR 0x40000000
+#define DDR_DX0DQTR 0xFFFFFFFF
+#define DDR_DX0DQSTR 0x3DB02000
+#define DDR_DX1GCR 0x0000CE81
+#define DDR_DX1DLLCR 0x40000000
+#define DDR_DX1DQTR 0xFFFFFFFF
+#define DDR_DX1DQSTR 0x3DB02000
+#define DDR_DX2GCR 0x0000CE80
+#define DDR_DX2DLLCR 0x40000000
+#define DDR_DX2DQTR 0xFFFFFFFF
+#define DDR_DX2DQSTR 0x3DB02000
+#define DDR_DX3GCR 0x0000CE80
+#define DDR_DX3DLLCR 0x40000000
+#define DDR_DX3DQTR 0xFFFFFFFF
+#define DDR_DX3DQSTR 0x3DB02000
+
+#include "stm32mp15-ddr.dtsi"
diff --git a/fdts/stm32mp151a-prtt1a-fw-config.dts b/fdts/stm32mp151a-prtt1a-fw-config.dts
new file mode 100644
index 0000000..1bbae4e
--- /dev/null
+++ b/fdts/stm32mp151a-prtt1a-fw-config.dts
@@ -0,0 +1,7 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (c) 2023, Oleksij Rempel <kernel@pengutronix.de>, Pengutronix
+ */
+
+#define DDR_SIZE	0x10000000 /* 256 MB */
+#include "stm32mp15-fw-config.dtsi"
diff --git a/fdts/stm32mp151a-prtt1a.dts b/fdts/stm32mp151a-prtt1a.dts
new file mode 100644
index 0000000..be9bdae
--- /dev/null
+++ b/fdts/stm32mp151a-prtt1a.dts
@@ -0,0 +1,235 @@
+// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
+/*
+ * Copyright (C) 2023, Protonic Holland - All Rights Reserved
+ * Author: David Jander <david@protonic.nl>
+ */
+/dts-v1/;
+
+#include "stm32mp151.dtsi"
+#include "stm32mp15-pinctrl.dtsi"
+#include "stm32mp15xxad-pinctrl.dtsi"
+#include <dt-bindings/clock/stm32mp1-clksrc.h>
+#include "stm32mp15-ddr3-1x2Gb-1066-binG.dtsi"
+
+/ {
+	model = "Protonic PRTT1A";
+	compatible = "prt,prtt1a", "st,stm32mp151";
+
+	chosen {
+		stdout-path = "serial0:115200n8";
+	};
+
+	aliases {
+		mmc0 = &sdmmc1;
+		mmc1 = &sdmmc2;
+		serial0 = &uart4;
+	};
+
+	memory@c0000000 {
+		device_type = "memory";
+		reg = <0xC0000000 0x10000000>;
+	};
+};
+
+&iwdg2 {
+	timeout-sec = <32>;
+	status = "okay";
+	secure-status = "okay";
+};
+
+&qspi {
+	pinctrl-names = "default", "sleep";
+	pinctrl-0 = <&qspi_clk_pins_a &qspi_bk1_pins_a>;
+	reg = <0x58003000 0x1000>, <0x70000000 0x4000000>;
+	#address-cells = <1>;
+	#size-cells = <0>;
+	status = "okay";
+
+	flash@0 {
+		compatible = "spi-nand";
+		reg = <0>;
+		spi-rx-bus-width = <4>;
+		spi-max-frequency = <104000000>;
+		#address-cells = <1>;
+		#size-cells = <1>;
+	};
+};
+
+&qspi_bk1_pins_a {
+	pins1 {
+		bias-pull-up;
+		drive-push-pull;
+		slew-rate = <1>;
+	};
+};
+
+&rcc {
+	st,clksrc = <
+		CLK_MPU_PLL1P
+		CLK_AXI_PLL2P
+		CLK_MCU_PLL3P
+		CLK_PLL12_HSE
+		CLK_PLL3_HSE
+		CLK_PLL4_HSE
+		CLK_RTC_LSI
+		CLK_MCO1_DISABLED
+		CLK_MCO2_DISABLED
+	>;
+
+	st,clkdiv = <
+		1 /*MPU*/
+		0 /*AXI*/
+		0 /*MCU*/
+		1 /*APB1*/
+		1 /*APB2*/
+		1 /*APB3*/
+		1 /*APB4*/
+		2 /*APB5*/
+		23 /*RTC*/
+		0 /*MCO1*/
+		0 /*MCO2*/
+	>;
+
+	st,pkcs = <
+		CLK_CKPER_HSE
+		CLK_FMC_ACLK
+		CLK_QSPI_ACLK
+		CLK_ETH_DISABLED
+		CLK_SDMMC12_PLL4P
+		CLK_DSI_DSIPLL
+		CLK_STGEN_HSE
+		CLK_USBPHY_HSE
+		CLK_SPI2S1_PLL3Q
+		CLK_SPI2S23_PLL3Q
+		CLK_SPI45_HSI
+		CLK_SPI6_HSI
+		CLK_I2C46_HSI
+		CLK_SDMMC3_PLL4P
+		CLK_USBO_USBPHY
+		CLK_ADC_CKPER
+		CLK_CEC_LSI
+		CLK_I2C12_HSI
+		CLK_I2C35_HSI
+		CLK_UART1_HSI
+		CLK_UART24_HSI
+		CLK_UART35_HSI
+		CLK_UART6_HSI
+		CLK_UART78_HSI
+		CLK_SPDIF_PLL4P
+		CLK_FDCAN_PLL4R
+		CLK_SAI1_PLL3Q
+		CLK_SAI2_PLL3Q
+		CLK_SAI3_PLL3Q
+		CLK_SAI4_PLL3Q
+		CLK_RNG1_LSI
+		CLK_RNG2_LSI
+		CLK_LPTIM1_PCLK1
+		CLK_LPTIM23_PCLK3
+		CLK_LPTIM45_LSI
+	>;
+
+	/* VCO = 1300.0 MHz => P = 650 (CPU) */
+	pll1: st,pll@0 {
+		compatible = "st,stm32mp1-pll";
+		reg = <0>;
+		cfg = <2 80 0 0 0 PQR(1,0,0)>;
+		frac = <0x800>;
+	};
+
+	/* VCO = 1066.0 MHz => P = 266 (AXI), Q = 533 (GPU), R = 533 (DDR) */
+	pll2: st,pll@1 {
+		compatible = "st,stm32mp1-pll";
+		reg = <1>;
+		cfg = <2 65 1 0 0 PQR(1,1,1)>;
+		frac = <0x1400>;
+	};
+
+	/* VCO = 417.8 MHz => P = 209, Q = 24, R = 11 */
+	pll3: st,pll@2 {
+		compatible = "st,stm32mp1-pll";
+		reg = <2>;
+		cfg = <1 33 1 16 36 PQR(1,1,1)>;
+		frac = <0x1a04>;
+	};
+
+	/* VCO = 480.0 MHz => P = 120, Q = 40, R = 96 */
+	pll4: st,pll@3 {
+		compatible = "st,stm32mp1-pll";
+		reg = <3>;
+		cfg = <1 39 3 11 4 PQR(1,1,1)>;
+	};
+};
+
+&rng1 {
+	status = "okay";
+};
+
+&rtc {
+	status = "okay";
+};
+
+&sdmmc1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&sdmmc1_b4_pins_a>;
+	bus-width = <4>;
+	status = "okay";
+};
+
+&sdmmc1_b4_pins_a {
+	pins1 {
+		bias-pull-up;
+	};
+	pins2 {
+		bias-pull-up;
+	};
+};
+
+/* NOTE: Although the PRTT1A does not have an eMMC, we declare it
+ * anyway, in order to be able to use the same binary for the
+ * PRTT1C also. All involved pins are N.C. on PRTT1A/S for that
+ * reason, so it should do no harm. All inputs configured with
+ * pull-ups to avoid floating inputs. */
+&sdmmc2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&sdmmc2_b4_pins_a &sdmmc2_d47_pins_a>;
+	bus-width = <8>;
+	status = "okay";
+};
+
+&sdmmc2_b4_pins_a {
+	pins1 {
+		pinmux = <STM32_PINMUX('B', 14, AF9)>, /* SDMMC2_D0 */
+			 <STM32_PINMUX('B', 7, AF10)>, /* SDMMC2_D1 */
+			 <STM32_PINMUX('B', 3, AF9)>, /* SDMMC2_D2 */
+			 <STM32_PINMUX('B', 4, AF9)>, /* SDMMC2_D3 */
+			 <STM32_PINMUX('G', 6, AF10)>; /* SDMMC2_CMD */
+	};
+};
+
+&sdmmc2_d47_pins_a {
+	pins {
+		pinmux = <STM32_PINMUX('A', 8, AF9)>, /* SDMMC2_D4 */
+			 <STM32_PINMUX('A', 9, AF10)>, /* SDMMC2_D5 */
+			 <STM32_PINMUX('C', 6, AF10)>, /* SDMMC2_D6 */
+			 <STM32_PINMUX('C', 7, AF10)>; /* SDMMC2_D7 */
+	};
+};
+
+&uart4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&uart4_pins_a>;
+	status = "okay";
+};
+
+&uart4_pins_a {
+	pins1 {
+		pinmux = <STM32_PINMUX('B', 9, AF8)>; /* UART4_TX */
+		bias-disable;
+		drive-push-pull;
+		slew-rate = <0>;
+	};
+	pins2 {
+		pinmux = <STM32_PINMUX('B', 2, AF8)>; /* UART4_RX */
+		bias-pull-up;
+	};
+};
diff --git a/include/arch/aarch32/arch_features.h b/include/arch/aarch32/arch_features.h
index a5a5e27..7c25b99 100644
--- a/include/arch/aarch32/arch_features.h
+++ b/include/arch/aarch32/arch_features.h
@@ -25,6 +25,37 @@
 	return ISOLATE_FIELD(read_id_mmfr4(), ID_MMFR4_CNP) != 0U;
 }
 
+static unsigned int read_feat_amu_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_pfr0(), ID_PFR0_AMU);
+}
+
+static inline bool is_feat_amu_supported(void)
+{
+	if (ENABLE_FEAT_AMU == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_AMU == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_amu_id_field() >= ID_PFR0_AMU_V1;
+}
+
+static inline bool is_feat_amuv1p1_supported(void)
+{
+	if (ENABLE_FEAT_AMUv1p1 == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_AMUv1p1 == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_amu_id_field() >= ID_PFR0_AMU_V1P1;
+}
+
 static inline unsigned int read_feat_trf_id_field(void)
 {
 	return ISOLATE_FIELD(read_id_dfr0(), ID_DFR0_TRACEFILT);
@@ -43,4 +74,28 @@
 	return read_feat_trf_id_field() != 0U;
 }
 
+static inline unsigned int read_feat_coptrc_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_dfr0(), ID_DFR0_COPTRC);
+}
+
+static inline bool is_feat_sys_reg_trace_supported(void)
+{
+	if (ENABLE_SYS_REG_TRACE_FOR_NS == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_SYS_REG_TRACE_FOR_NS == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_coptrc_id_field() != 0U;
+}
+
+static inline bool is_feat_spe_supported(void)
+{
+	/* FEAT_SPE is AArch64 only */
+	return false;
+}
+
 #endif /* ARCH_FEATURES_H */
diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index 9e4a3b7..dee7fdc 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -119,8 +119,11 @@
 #define MPAMVPM7_EL2		S3_4_C10_C6_7
 #define MPAMVPMV_EL2		S3_4_C10_C4_1
 #define TRFCR_EL2		S3_4_C1_C2_1
+#define VNCR_EL2		S3_4_C2_C2_0
 #define PMSCR_EL2		S3_4_C9_C9_0
 #define TFSR_EL2		S3_4_C5_C6_0
+#define CONTEXTIDR_EL2		S3_4_C13_C0_1
+#define TTBR1_EL2		S3_4_C2_C0_1
 
 /*******************************************************************************
  * Generic timer memory mapped registers & offsets
@@ -349,6 +352,12 @@
 #define ID_AA64MMFR2_EL1_NV_SUPPORTED		ULL(0x1)
 #define ID_AA64MMFR2_EL1_NV2_SUPPORTED		ULL(0x2)
 
+/* ID_AA64MMFR3_EL1 definitions */
+#define ID_AA64MMFR3_EL1			S3_0_C0_C7_3
+
+#define ID_AA64MMFR3_EL1_TCRX_SHIFT		U(0)
+#define ID_AA64MMFR3_EL1_TCRX_MASK		ULL(0xf)
+
 /* ID_AA64PFR1_EL1 definitions */
 #define ID_AA64PFR1_EL1_SSBS_SHIFT	U(4)
 #define ID_AA64PFR1_EL1_SSBS_MASK	ULL(0xf)
@@ -384,8 +393,10 @@
 #define ID_AA64PFR1_MPAM_FRAC_SHIFT	ULL(16)
 #define ID_AA64PFR1_MPAM_FRAC_MASK	ULL(0xf)
 
-#define ID_AA64PFR1_EL1_SME_SHIFT	U(24)
-#define ID_AA64PFR1_EL1_SME_MASK	ULL(0xf)
+#define ID_AA64PFR1_EL1_SME_SHIFT		U(24)
+#define ID_AA64PFR1_EL1_SME_MASK		ULL(0xf)
+#define ID_AA64PFR1_EL1_SME_NOT_SUPPORTED	ULL(0x0)
+#define ID_AA64PFR1_EL1_SME_SUPPORTED		ULL(0x1)
 
 /* ID_PFR1_EL1 definitions */
 #define ID_PFR1_VIRTEXT_SHIFT	U(12)
@@ -501,6 +512,7 @@
 #define SCR_GPF_BIT		(UL(1) << 48)
 #define SCR_TWEDEL_SHIFT	U(30)
 #define SCR_TWEDEL_MASK		ULL(0xf)
+#define SCR_TCR2EN_BIT		(UL(1) << 43)
 #define SCR_TRNDR_BIT		(UL(1) << 40)
 #define SCR_HXEn_BIT		(UL(1) << 38)
 #define SCR_ENTP2_SHIFT		U(41)
@@ -994,7 +1006,9 @@
 #define SMCR_EL3			S3_6_C1_C2_6
 
 /* ID_AA64SMFR0_EL1 definitions */
-#define ID_AA64SMFR0_EL1_FA64_BIT	(UL(1) << 63)
+#define ID_AA64SMFR0_EL1_SME_FA64_SHIFT		U(63)
+#define ID_AA64SMFR0_EL1_SME_FA64_MASK		U(0x1)
+#define ID_AA64SMFR0_EL1_SME_FA64_SUPPORTED	U(0x1)
 
 /* SMCR_ELx definitions */
 #define SMCR_ELX_LEN_SHIFT		U(0)
@@ -1070,10 +1084,8 @@
 #define MPAMHCR_EL2		S3_4_C10_C4_0
 #define MPAM3_EL3		S3_6_C10_C5_0
 
-#define MPAMIDR_EL1_HAS_HCR_SHIFT	ULL(0x11)
-#define MPAMIDR_EL1_VPMR_MAX_SHIFT	ULL(0x12)
-#define MPAMIDR_EL1_VPMR_MAX_WIDTH	ULL(0x3)
-#define MPAMIDR_EL1_VPMR_MAX_POSSIBLE	ULL(0x7)
+#define MPAMIDR_EL1_VPMR_MAX_SHIFT	ULL(18)
+#define MPAMIDR_EL1_VPMR_MAX_MASK	ULL(0x7)
 /*******************************************************************************
  * Definitions for system register interface to AMU for FEAT_AMUv1
  ******************************************************************************/
@@ -1222,6 +1234,8 @@
 #define GPCCR_EL3			S3_6_C2_C1_6
 #define GPTBR_EL3			S3_6_C2_C1_4
 
+#define SCXTNUM_EL2			S3_4_C13_C0_7
+
 /*******************************************************************************
  * RAS system registers
  ******************************************************************************/
@@ -1302,6 +1316,11 @@
 #define HCRX_EL2_EnAS0_BIT	(UL(1) << 0)
 
 /*******************************************************************************
+ * FEAT_TCR2 - Extended Translation Control Register
+ ******************************************************************************/
+#define TCR2_EL2		S3_4_C2_C0_3
+
+/*******************************************************************************
  * Definitions for DynamicIQ Shared Unit registers
  ******************************************************************************/
 #define CLUSTERPWRDN_EL1	S3_0_c15_c3_6
@@ -1323,4 +1342,7 @@
 #define CPUMPMMCR_EL3_MPMM_EN_SHIFT	UINT64_C(0)
 #define CPUMPMMCR_EL3_MPMM_EN_MASK	UINT64_C(0x1)
 
+/* alternative system register encoding for the "sb" speculation barrier */
+#define SYSREG_SB			S0_3_C3_C0_7
+
 #endif /* ARCH_H */
diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index 9ff81aa..3ea08a6 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h
@@ -21,16 +21,40 @@
 	return true;
 }
 
-static inline bool is_armv8_1_pan_present(void)
+static inline unsigned int read_feat_pan_id_field(void)
 {
-	return ((read_id_aa64mmfr1_el1() >> ID_AA64MMFR1_EL1_PAN_SHIFT) &
-		ID_AA64MMFR1_EL1_PAN_MASK) != 0U;
+	return ISOLATE_FIELD(read_id_aa64mmfr1_el1(), ID_AA64MMFR1_EL1_PAN);
 }
 
-static inline bool is_armv8_1_vhe_present(void)
+static inline bool is_feat_pan_supported(void)
 {
-	return ((read_id_aa64mmfr1_el1() >> ID_AA64MMFR1_EL1_VHE_SHIFT) &
-		ID_AA64MMFR1_EL1_VHE_MASK) != 0U;
+	if (ENABLE_FEAT_PAN == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_PAN == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_pan_id_field() != 0U;
+}
+
+static inline unsigned int read_feat_vhe_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_aa64mmfr1_el1(), ID_AA64MMFR1_EL1_VHE);
+}
+
+static inline bool is_feat_vhe_supported(void)
+{
+	if (ENABLE_FEAT_VHE == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_VHE == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_vhe_id_field() != 0U;
 }
 
 static inline bool is_armv8_2_ttcnp_present(void)
@@ -89,18 +113,42 @@
 		ID_AA64PFR1_EL1_MTE_MASK);
 }
 
-static inline bool is_armv8_4_sel2_present(void)
+static inline unsigned int read_feat_sel2_id_field(void)
 {
-	return ((read_id_aa64pfr0_el1() >> ID_AA64PFR0_SEL2_SHIFT) &
-		ID_AA64PFR0_SEL2_MASK) == 1ULL;
+	return ISOLATE_FIELD(read_id_aa64pfr0_el1(), ID_AA64PFR0_SEL2);
 }
 
-static inline bool is_armv8_6_twed_present(void)
+static inline bool is_feat_sel2_supported(void)
 {
-	return (((read_id_aa64mmfr1_el1() >> ID_AA64MMFR1_EL1_TWED_SHIFT) &
-		ID_AA64MMFR1_EL1_TWED_MASK) == ID_AA64MMFR1_EL1_TWED_SUPPORTED);
+	if (ENABLE_FEAT_SEL2 == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_SEL2 == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_sel2_id_field() != 0U;
+}
+
+static inline unsigned int read_feat_twed_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_aa64mmfr1_el1(), ID_AA64MMFR1_EL1_TWED);
 }
 
+static inline bool is_feat_twed_supported(void)
+{
+	if (ENABLE_FEAT_TWED == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_TWED == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_twed_id_field() != 0U;
+}
+
 static unsigned int read_feat_fgt_id_field(void)
 {
 	return ISOLATE_FIELD(read_id_aa64mmfr0_el1(), ID_AA64MMFR0_EL1_FGT);
@@ -119,18 +167,73 @@
 	return read_feat_fgt_id_field() != 0U;
 }
 
+static unsigned int read_feat_ecv_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_aa64mmfr0_el1(), ID_AA64MMFR0_EL1_ECV);
+}
+
-static inline unsigned long int get_armv8_6_ecv_support(void)
+static inline bool is_feat_ecv_supported(void)
 {
-	return ((read_id_aa64mmfr0_el1() >> ID_AA64MMFR0_EL1_ECV_SHIFT) &
-		ID_AA64MMFR0_EL1_ECV_MASK);
+	if (ENABLE_FEAT_ECV == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_ECV == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_ecv_id_field() != 0U;
+}
+
+static inline bool is_feat_ecv_v2_supported(void)
+{
+	if (ENABLE_FEAT_ECV == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_ECV == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_ecv_id_field() >= ID_AA64MMFR0_EL1_ECV_SELF_SYNCH;
 }
 
-static inline bool is_armv8_5_rng_present(void)
+static unsigned int read_feat_rng_id_field(void)
 {
-	return ((read_id_aa64isar0_el1() >> ID_AA64ISAR0_RNDR_SHIFT) &
-		ID_AA64ISAR0_RNDR_MASK);
+	return ISOLATE_FIELD(read_id_aa64isar0_el1(), ID_AA64ISAR0_RNDR);
 }
 
+static inline bool is_feat_rng_supported(void)
+{
+	if (ENABLE_FEAT_RNG == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_RNG == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_rng_id_field() != 0U;
+}
+
+static unsigned int read_feat_tcrx_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_aa64mmfr3_el1(), ID_AA64MMFR3_EL1_TCRX);
+}
+
+static inline bool is_feat_tcr2_supported(void)
+{
+	if (ENABLE_FEAT_TCR2 == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_TCR2 == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_tcrx_id_field() != 0U;
+}
+
 /*******************************************************************************
  * Functions to identify the presence of the Activity Monitors Extension
  ******************************************************************************/
@@ -141,19 +244,27 @@
 
 static inline bool is_feat_amu_supported(void)
 {
-	if (ENABLE_FEAT_AMUv1 == FEAT_STATE_DISABLED) {
+	if (ENABLE_FEAT_AMU == FEAT_STATE_DISABLED) {
 		return false;
 	}
 
-	if (ENABLE_FEAT_AMUv1 == FEAT_STATE_ALWAYS) {
+	if (ENABLE_FEAT_AMU == FEAT_STATE_ALWAYS) {
 		return true;
 	}
 
 	return read_feat_amu_id_field() >= ID_AA64PFR0_AMU_V1;
 }
 
-static inline bool is_armv8_6_feat_amuv1p1_present(void)
+static inline bool is_feat_amuv1p1_supported(void)
 {
+	if (ENABLE_FEAT_AMUv1p1 == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_AMUv1p1 == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
 	return read_feat_amu_id_field() >= ID_AA64PFR0_AMU_V1P1;
 }
 
@@ -166,7 +277,7 @@
  * 0x11: v1.1 Armv8.4 or later
  *
  */
-static inline unsigned int get_mpam_version(void)
+static inline unsigned int read_feat_mpam_version(void)
 {
 	return (unsigned int)((((read_id_aa64pfr0_el1() >>
 		ID_AA64PFR0_MPAM_SHIFT) & ID_AA64PFR0_MPAM_MASK) << 4) |
@@ -174,6 +285,19 @@
 		ID_AA64PFR1_MPAM_FRAC_SHIFT) & ID_AA64PFR1_MPAM_FRAC_MASK));
 }
 
+static inline bool is_feat_mpam_supported(void)
+{
+	if (ENABLE_MPAM_FOR_LOWER_ELS == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_MPAM_FOR_LOWER_ELS == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_mpam_version() != 0U;
+}
+
 static inline unsigned int read_feat_hcx_id_field(void)
 {
 	return ISOLATE_FIELD(read_id_aa64mmfr1_el1(), ID_AA64MMFR1_EL1_HCX);
@@ -213,39 +337,74 @@
 /*********************************************************************************
  * Function to identify the presence of FEAT_SB (Speculation Barrier Instruction)
  ********************************************************************************/
-static inline bool is_armv8_0_feat_sb_present(void)
+static inline unsigned int read_feat_sb_id_field(void)
 {
-	return (((read_id_aa64isar1_el1() >> ID_AA64ISAR1_SB_SHIFT) &
-		ID_AA64ISAR1_SB_MASK) == ID_AA64ISAR1_SB_SUPPORTED);
+	return ISOLATE_FIELD(read_id_aa64isar1_el1(), ID_AA64ISAR1_SB);
 }
 
 /*********************************************************************************
  * Function to identify the presence of FEAT_CSV2_2 (Cache Speculation Variant 2)
  ********************************************************************************/
-static inline bool is_armv8_0_feat_csv2_2_present(void)
+static inline unsigned int read_feat_csv2_id_field(void)
 {
-	return (((read_id_aa64pfr0_el1() >> ID_AA64PFR0_CSV2_SHIFT) &
-		ID_AA64PFR0_CSV2_MASK) == ID_AA64PFR0_CSV2_2_SUPPORTED);
+	return ISOLATE_FIELD(read_id_aa64pfr0_el1(), ID_AA64PFR0_CSV2);
 }
 
+static inline bool is_feat_csv2_2_supported(void)
+{
+	if (ENABLE_FEAT_CSV2_2 == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_FEAT_CSV2_2 == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_csv2_id_field() >= ID_AA64PFR0_CSV2_2_SUPPORTED;
+}
+
 /**********************************************************************************
  * Function to identify the presence of FEAT_SPE (Statistical Profiling Extension)
  *********************************************************************************/
-static inline bool is_armv8_2_feat_spe_present(void)
+static inline unsigned int read_feat_spe_id_field(void)
 {
-	return (((read_id_aa64dfr0_el1() >> ID_AA64DFR0_PMS_SHIFT) &
-		ID_AA64DFR0_PMS_MASK) != ID_AA64DFR0_SPE_NOT_SUPPORTED);
+	return ISOLATE_FIELD(read_id_aa64dfr0_el1(), ID_AA64DFR0_PMS);
+}
+
+static inline bool is_feat_spe_supported(void)
+{
+	if (ENABLE_SPE_FOR_NS == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_SPE_FOR_NS == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_spe_id_field() != 0U;
 }
 
 /*******************************************************************************
  * Function to identify the presence of FEAT_SVE (Scalable Vector Extension)
  ******************************************************************************/
-static inline bool is_armv8_2_feat_sve_present(void)
+static inline unsigned int read_feat_sve_id_field(void)
 {
-	return (((read_id_aa64pfr0_el1() >> ID_AA64PFR0_SVE_SHIFT) &
-		ID_AA64PFR0_SVE_MASK) == ID_AA64PFR0_SVE_SUPPORTED);
+	return ISOLATE_FIELD(read_id_aa64pfr0_el1(), ID_AA64PFR0_SVE);
 }
 
+static inline bool is_feat_sve_supported(void)
+{
+	if (ENABLE_SVE_FOR_NS == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_SVE_FOR_NS == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_sve_id_field() >= ID_AA64PFR0_SVE_SUPPORTED;
+}
+
 /*******************************************************************************
  * Function to identify the presence of FEAT_RAS (Reliability,Availability,
  * and Serviceability Extension)
@@ -265,6 +424,24 @@
 		ID_AA64PFR0_DIT_MASK) == ID_AA64PFR0_DIT_SUPPORTED);
 }
 
+static inline unsigned int read_feat_tracever_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_aa64dfr0_el1(), ID_AA64DFR0_TRACEVER);
+}
+
+static inline bool is_feat_sys_reg_trace_supported(void)
+{
+	if (ENABLE_SYS_REG_TRACE_FOR_NS == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_SYS_REG_TRACE_FOR_NS == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_tracever_id_field() != 0U;
+}
+
 /*************************************************************************
  * Function to identify the presence of FEAT_TRF (TraceLift)
  ************************************************************************/
@@ -290,10 +467,22 @@
  * Function to identify the presence of FEAT_NV2 (Enhanced Nested Virtualization
  * Support)
  *******************************************************************************/
-static inline unsigned int get_armv8_4_feat_nv_support(void)
+static inline unsigned int read_feat_nv_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_aa64mmfr2_el1(), ID_AA64MMFR2_EL1_NV);
+}
+
+static inline bool is_feat_nv2_supported(void)
 {
-	return (((read_id_aa64mmfr2_el1() >> ID_AA64MMFR2_EL1_NV_SHIFT) &
-		ID_AA64MMFR2_EL1_NV_MASK));
+	if (CTX_INCLUDE_NEVE_REGS == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (CTX_INCLUDE_NEVE_REGS == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_nv_id_field() >= ID_AA64MMFR2_EL1_NV2_SUPPORTED;
 }
 
 /*******************************************************************************
@@ -338,5 +527,31 @@
 
 	return read_feat_trbe_id_field() != 0U;
 
+}
+/*******************************************************************************
+ * Function to identify the presence of FEAT_SMEx (Scalar Matrix Extension)
+ ******************************************************************************/
+static inline unsigned int read_feat_sme_fa64_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_aa64smfr0_el1(), ID_AA64SMFR0_EL1_SME_FA64);
+}
+
+static inline unsigned int read_feat_sme_id_field(void)
+{
+	return ISOLATE_FIELD(read_id_aa64pfr1_el1(), ID_AA64PFR1_EL1_SME);
 }
+
+static inline bool is_feat_sme_supported(void)
+{
+	if (ENABLE_SME_FOR_NS == FEAT_STATE_DISABLED) {
+		return false;
+	}
+
+	if (ENABLE_SME_FOR_NS == FEAT_STATE_ALWAYS) {
+		return true;
+	}
+
+	return read_feat_sme_id_field() >= ID_AA64PFR1_EL1_SME_SUPPORTED;
+}
+
 #endif /* ARCH_FEATURES_H */
diff --git a/include/arch/aarch64/arch_helpers.h b/include/arch/aarch64/arch_helpers.h
index 3350c8f..04b64be 100644
--- a/include/arch/aarch64/arch_helpers.h
+++ b/include/arch/aarch64/arch_helpers.h
@@ -522,11 +522,6 @@
 DEFINE_RENAME_SYSREG_RW_FUNCS(amcntenclr1_el0, AMCNTENCLR1_EL0)
 DEFINE_RENAME_SYSREG_RW_FUNCS(amcntenset1_el0, AMCNTENSET1_EL0)
 
-DEFINE_RENAME_SYSREG_READ_FUNC(mpamidr_el1, MPAMIDR_EL1)
-DEFINE_RENAME_SYSREG_RW_FUNCS(mpam3_el3, MPAM3_EL3)
-DEFINE_RENAME_SYSREG_RW_FUNCS(mpam2_el2, MPAM2_EL2)
-DEFINE_RENAME_SYSREG_RW_FUNCS(mpamhcr_el2, MPAMHCR_EL2)
-
 DEFINE_RENAME_SYSREG_RW_FUNCS(pmblimitr_el1, PMBLIMITR_EL1)
 
 DEFINE_RENAME_SYSREG_WRITE_FUNC(zcr_el3, ZCR_EL3)
@@ -545,9 +540,30 @@
 DEFINE_RENAME_SYSREG_READ_FUNC(erxmisc0_el1, ERXMISC0_EL1)
 DEFINE_RENAME_SYSREG_READ_FUNC(erxmisc1_el1, ERXMISC1_EL1)
 
+DEFINE_RENAME_SYSREG_RW_FUNCS(scxtnum_el2, SCXTNUM_EL2)
+
-/* Armv8.2 Registers */
+/* Armv8.1 VHE Registers */
+DEFINE_RENAME_SYSREG_RW_FUNCS(contextidr_el2, CONTEXTIDR_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(ttbr1_el2, TTBR1_EL2)
+
+/* Armv8.2 ID Registers */
 DEFINE_RENAME_IDREG_READ_FUNC(id_aa64mmfr2_el1, ID_AA64MMFR2_EL1)
 
+/* Armv8.2 MPAM Registers */
+DEFINE_RENAME_SYSREG_READ_FUNC(mpamidr_el1, MPAMIDR_EL1)
+DEFINE_RENAME_SYSREG_RW_FUNCS(mpam3_el3, MPAM3_EL3)
+DEFINE_RENAME_SYSREG_RW_FUNCS(mpam2_el2, MPAM2_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(mpamhcr_el2, MPAMHCR_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(mpamvpm0_el2, MPAMVPM0_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(mpamvpm1_el2, MPAMVPM1_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(mpamvpm2_el2, MPAMVPM2_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(mpamvpm3_el2, MPAMVPM3_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(mpamvpm4_el2, MPAMVPM4_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(mpamvpm5_el2, MPAMVPM5_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(mpamvpm6_el2, MPAMVPM6_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(mpamvpm7_el2, MPAMVPM7_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(mpamvpmv_el2, MPAMVPMV_EL2)
+
 /* Armv8.3 Pointer Authentication Registers */
 DEFINE_RENAME_SYSREG_RW_FUNCS(apiakeyhi_el1, APIAKeyHi_EL1)
 DEFINE_RENAME_SYSREG_RW_FUNCS(apiakeylo_el1, APIAKeyLo_EL1)
@@ -557,6 +573,7 @@
 
 /* Armv8.4 FEAT_TRF Register */
 DEFINE_RENAME_SYSREG_RW_FUNCS(trfcr_el2, TRFCR_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(vncr_el2, VNCR_EL2)
 
 /* Armv8.5 MTE Registers */
 DEFINE_RENAME_SYSREG_RW_FUNCS(tfsre0_el1, TFSRE0_EL1)
@@ -576,9 +593,18 @@
 DEFINE_RENAME_SYSREG_RW_FUNCS(hfgrtr_el2, HFGRTR_EL2)
 DEFINE_RENAME_SYSREG_RW_FUNCS(hfgwtr_el2, HFGWTR_EL2)
 
+/* ARMv8.6 FEAT_ECV Register */
+DEFINE_RENAME_SYSREG_RW_FUNCS(cntpoff_el2, CNTPOFF_EL2)
+
 /* FEAT_HCX Register */
 DEFINE_RENAME_SYSREG_RW_FUNCS(hcrx_el2, HCRX_EL2)
 
+/* Armv8.9 system registers */
+DEFINE_RENAME_IDREG_READ_FUNC(id_aa64mmfr3_el1, ID_AA64MMFR3_EL1)
+
+/* FEAT_TCR2 Register */
+DEFINE_RENAME_SYSREG_RW_FUNCS(tcr2_el2, TCR2_EL2)
+
 /* DynamIQ Shared Unit power management */
 DEFINE_RENAME_SYSREG_RW_FUNCS(clusterpwrdn_el1, CLUSTERPWRDN_EL1)
 
diff --git a/include/arch/aarch64/asm_macros.S b/include/arch/aarch64/asm_macros.S
index 66c39e5..b4dab08 100644
--- a/include/arch/aarch64/asm_macros.S
+++ b/include/arch/aarch64/asm_macros.S
@@ -215,12 +215,24 @@
 	.endm
 
 	/*
+	 * The "sb" instruction was introduced later into the architecture,
+	 * so not all toolchains understand it. Some deny its usage unless
+	 * a supported processor is specified on the build command line.
+	 * Use sb's system register encoding to work around this, we already
+	 * guard the sb execution with a feature flag.
+	 */
+
+	.macro sb_barrier_insn
+	msr	SYSREG_SB, xzr
+	.endm
+
+	/*
 	 * Macro for using speculation barrier instruction introduced by
 	 * FEAT_SB, if it's enabled.
 	 */
 	.macro speculation_barrier
 #if ENABLE_FEAT_SB
-	sb
+	sb_barrier_insn
 #else
 	dsb	sy
 	isb
@@ -234,7 +246,7 @@
 	.macro exception_return
 	eret
 #if ENABLE_FEAT_SB
-	sb
+	sb_barrier_insn
 #else
 	dsb	nsh
 	isb
diff --git a/include/drivers/arm/gic600_multichip.h b/include/drivers/arm/gic600_multichip.h
index bda406b..978d735 100644
--- a/include/drivers/arm/gic600_multichip.h
+++ b/include/drivers/arm/gic600_multichip.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2019, ARM Limited. All rights reserved.
+ * Copyright (c) 2023, NVIDIA Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -15,8 +16,11 @@
  */
 #define GIC600_MAX_MULTICHIP	16
 
-/* SPI IDs array consist of min and max ids */
-#define GIC600_SPI_IDS_SIZE	2
+typedef struct multichip_spi_ids_desc {
+	uintptr_t gicd_base;
+	uint32_t spi_id_min;
+	uint32_t spi_id_max;
+} multichip_spi_ids_desc_t;
 
 /*******************************************************************************
  * GIC-600 multichip data structure describes platform specific attributes
@@ -37,19 +41,23 @@
  * The 'chip_addrs' field contains array of chip addresses. These addresses are
  * implementation specific values.
  *
- * The 'spi_ids' field contains array of minimum and maximum SPI interrupt ids
- * that each chip owns. Note that SPI interrupt ids can range from 32 to 960 and
- * it should be group of 32 (i.e., SPI minimum and (SPI maximum + 1) should be
- * a multiple of 32). If a chip doesn't own any SPI interrupts a value of {0, 0}
- * should be passed.
+ * The 'multichip_spi_ids_desc_t' field contains array of descriptors used to
+ * provide minimum and maximum SPI interrupt ids that each chip owns and the
+ * corresponding chip base address. Note that SPI interrupt ids can range from
+ * 32 to 960 and it should be group of 32 (i.e., SPI minimum and (SPI maximum +
+ * 1) should be a multiple of 32). If a chip doesn't own any SPI interrupts a
+ * value of {0, 0, 0} should be passed.
  ******************************************************************************/
 struct gic600_multichip_data {
 	uintptr_t rt_owner_base;
 	unsigned int rt_owner;
 	unsigned int chip_count;
 	uint64_t chip_addrs[GIC600_MAX_MULTICHIP];
-	unsigned int spi_ids[GIC600_MAX_MULTICHIP][GIC600_SPI_IDS_SIZE];
+	multichip_spi_ids_desc_t spi_ids[GIC600_MAX_MULTICHIP];
 };
 
+uintptr_t gic600_multichip_gicd_base_for_spi(uint32_t spi_id);
 void gic600_multichip_init(struct gic600_multichip_data *multichip_data);
+bool gic600_multichip_is_initialized(void);
+
 #endif /* GIC600_MULTICHIP_H */
diff --git a/include/drivers/ufs.h b/include/drivers/ufs.h
index e4ec00d..2a63fd4 100644
--- a/include/drivers/ufs.h
+++ b/include/drivers/ufs.h
@@ -57,6 +57,21 @@
 /* UTP Transfer Request Completion Status */
 #define UFS_INT_UTRCS			(1 << 0)
 
+#define UFS_INT_FATAL			(UFS_INT_DFES |\
+					 UFS_INT_HCFES |\
+					 UFS_INT_SBFES)
+#define UFS_INT_ERR			(UFS_INT_FATAL |\
+					 UFS_INT_UE)
+
+#define UFS_UIC_PA_ERROR_MASK		0x8000001F
+#define UFS_UIC_DL_ERROR_MASK		0x8000FFFF
+#define UFS_UIC_NL_ERROR_MASK		0x80000007
+#define UFS_UIC_TL_ERROR_MASK		0x8000007F
+#define UFS_UIC_DME_ERROR_MASK		0x80000001
+
+#define PA_INIT_ERR			(1 << 13)
+#define PA_LAYER_GEN_ERR		(1 << 4)
+
 /* Host Controller Status */
 #define HCS				0x30
 #define HCS_UPMCRS_MASK			(7 << 8)
@@ -275,6 +290,11 @@
 
 #define FDEVICEINIT_TIMEOUT_MS	        1500
 
+#define UIC_CMD_TIMEOUT_MS		500
+#define QUERY_REQ_TIMEOUT_MS		1500
+#define NOP_OUT_TIMEOUT_MS		50
+#define CMD_TIMEOUT_MS		        5000
+
 /**
  * ufs_dev_desc - ufs device details from the device descriptor
  * @wmanufacturerid: card details
diff --git a/include/lib/cpus/aarch64/cortex_a78c.h b/include/lib/cpus/aarch64/cortex_a78c.h
index 18cba2c..301be69 100644
--- a/include/lib/cpus/aarch64/cortex_a78c.h
+++ b/include/lib/cpus/aarch64/cortex_a78c.h
@@ -26,6 +26,7 @@
 #define CORTEX_A78C_CPUECTLR_EL1		        S3_0_C15_C1_4
 #define CORTEX_A78C_CPUECTLR_EL1_BIT_6		        (ULL(1) << 6)
 #define CORTEX_A78C_CPUECTLR_EL1_BIT_7		        (ULL(1) << 7)
+#define CORTEX_A78C_CPUECTLR_EL1_MM_ASP_EN		(ULL(1) << 53)
 
 /*******************************************************************************
  * CPU Power Control register specific definitions
diff --git a/include/lib/el3_runtime/aarch64/context.h b/include/lib/el3_runtime/aarch64/context.h
index 40bfa39..e5e7e74 100644
--- a/include/lib/el3_runtime/aarch64/context.h
+++ b/include/lib/el3_runtime/aarch64/context.h
@@ -193,7 +193,6 @@
 // Only if MTE registers in use
 #define CTX_TFSR_EL2		U(0x100)
 
-// Only if ENABLE_MPAM_FOR_LOWER_ELS==1
 #define CTX_MPAM2_EL2		U(0x108)
 #define CTX_MPAMHCR_EL2		U(0x110)
 #define CTX_MPAMVPM0_EL2	U(0x118)
@@ -229,6 +228,9 @@
 // Register for FEAT_HCX
 #define CTX_HCRX_EL2            U(0x1d0)
 
+// Starting with Armv8.9
+#define CTX_TCR2_EL2            U(0x1d8)
+
 /* Align to the next 16 byte boundary */
 #define CTX_EL2_SYSREGS_END	U(0x1e0)
 
@@ -511,38 +513,14 @@
 #if CTX_INCLUDE_EL2_REGS
 void el2_sysregs_context_save_common(el2_sysregs_t *regs);
 void el2_sysregs_context_restore_common(el2_sysregs_t *regs);
-#if ENABLE_SPE_FOR_LOWER_ELS
-void el2_sysregs_context_save_spe(el2_sysregs_t *regs);
-void el2_sysregs_context_restore_spe(el2_sysregs_t *regs);
-#endif /* ENABLE_SPE_FOR_LOWER_ELS */
 #if CTX_INCLUDE_MTE_REGS
 void el2_sysregs_context_save_mte(el2_sysregs_t *regs);
 void el2_sysregs_context_restore_mte(el2_sysregs_t *regs);
 #endif /* CTX_INCLUDE_MTE_REGS */
-#if ENABLE_MPAM_FOR_LOWER_ELS
-void el2_sysregs_context_save_mpam(el2_sysregs_t *regs);
-void el2_sysregs_context_restore_mpam(el2_sysregs_t *regs);
-#endif /* ENABLE_MPAM_FOR_LOWER_ELS */
-#if ENABLE_FEAT_ECV
-void el2_sysregs_context_save_ecv(el2_sysregs_t *regs);
-void el2_sysregs_context_restore_ecv(el2_sysregs_t *regs);
-#endif /* ENABLE_FEAT_ECV */
-#if ENABLE_FEAT_VHE
-void el2_sysregs_context_save_vhe(el2_sysregs_t *regs);
-void el2_sysregs_context_restore_vhe(el2_sysregs_t *regs);
-#endif /* ENABLE_FEAT_VHE */
 #if RAS_EXTENSION
 void el2_sysregs_context_save_ras(el2_sysregs_t *regs);
 void el2_sysregs_context_restore_ras(el2_sysregs_t *regs);
 #endif /* RAS_EXTENSION */
-#if CTX_INCLUDE_NEVE_REGS
-void el2_sysregs_context_save_nv2(el2_sysregs_t *regs);
-void el2_sysregs_context_restore_nv2(el2_sysregs_t *regs);
-#endif /* CTX_INCLUDE_NEVE_REGS */
-#if ENABLE_FEAT_CSV2_2
-void el2_sysregs_context_save_csv2(el2_sysregs_t *regs);
-void el2_sysregs_context_restore_csv2(el2_sysregs_t *regs);
-#endif /* ENABLE_FEAT_CSV2_2 */
 #endif /* CTX_INCLUDE_EL2_REGS */
 
 #if CTX_INCLUDE_FPREGS
diff --git a/include/lib/extensions/amu.h b/include/lib/extensions/amu.h
index 6452f7e..de476e4 100644
--- a/include/lib/extensions/amu.h
+++ b/include/lib/extensions/amu.h
@@ -14,11 +14,23 @@
 
 #include <platform_def.h>
 
+#if ENABLE_FEAT_AMU
 #if __aarch64__
 void amu_enable(bool el2_unused, cpu_context_t *ctx);
 #else
 void amu_enable(bool el2_unused);
 #endif
+#else
+#if __aarch64__
+static inline void amu_enable(bool el2_unused, cpu_context_t *ctx)
+{
+}
+#else
+static inline void amu_enable(bool el2_unused)
+{
+}
+#endif
+#endif
 
 #if ENABLE_AMU_AUXILIARY_COUNTERS
 /*
diff --git a/include/lib/extensions/brbe.h b/include/lib/extensions/brbe.h
index aee208d..9ee2444 100644
--- a/include/lib/extensions/brbe.h
+++ b/include/lib/extensions/brbe.h
@@ -10,7 +10,7 @@
 #if ENABLE_BRBE_FOR_NS
 void brbe_enable(void);
 #else
-void brbe_enable(void)
+static inline void brbe_enable(void)
 {
 }
 #endif /* ENABLE_BRBE_FOR_NS */
diff --git a/include/lib/extensions/mpam.h b/include/lib/extensions/mpam.h
index 414adcb..4327278 100644
--- a/include/lib/extensions/mpam.h
+++ b/include/lib/extensions/mpam.h
@@ -9,6 +9,12 @@
 
 #include <stdbool.h>
 
+#if ENABLE_MPAM_FOR_LOWER_ELS
 void mpam_enable(bool el2_unused);
+#else
+static inline void mpam_enable(bool el2_unused)
+{
+}
+#endif
 
 #endif /* MPAM_H */
diff --git a/include/lib/extensions/sme.h b/include/lib/extensions/sme.h
index 893f9f2..0e9c4b9 100644
--- a/include/lib/extensions/sme.h
+++ b/include/lib/extensions/sme.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -8,7 +8,6 @@
 #define SME_H
 
 #include <stdbool.h>
-
 #include <context.h>
 
 /*
@@ -21,7 +20,16 @@
  */
 #define SME_SMCR_LEN_MAX	U(0x1FF)
 
+#if ENABLE_SME_FOR_NS
 void sme_enable(cpu_context_t *context);
 void sme_disable(cpu_context_t *context);
+#else
+static inline void sme_enable(cpu_context_t *context)
+{
+}
+static inline void sme_disable(cpu_context_t *context)
+{
+}
+#endif /* ENABLE_SME_FOR_NS */
 
 #endif /* SME_H */
diff --git a/include/lib/extensions/spe.h b/include/lib/extensions/spe.h
index d4b925f..02fccae 100644
--- a/include/lib/extensions/spe.h
+++ b/include/lib/extensions/spe.h
@@ -9,8 +9,16 @@
 
 #include <stdbool.h>
 
-bool spe_supported(void);
+#if ENABLE_SPE_FOR_NS
 void spe_enable(bool el2_unused);
 void spe_disable(void);
+#else
+static inline void spe_enable(bool el2_unused)
+{
+}
+static inline void spe_disable(void)
+{
+}
+#endif
 
 #endif /* SPE_H */
diff --git a/include/lib/extensions/sve.h b/include/lib/extensions/sve.h
index 4b66cdb..1faed2d 100644
--- a/include/lib/extensions/sve.h
+++ b/include/lib/extensions/sve.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,7 +9,16 @@
 
 #include <context.h>
 
+#if (ENABLE_SME_FOR_NS || ENABLE_SVE_FOR_NS)
 void sve_enable(cpu_context_t *context);
 void sve_disable(cpu_context_t *context);
+#else
+static inline void sve_enable(cpu_context_t *context)
+{
+}
+static inline void sve_disable(cpu_context_t *context)
+{
+}
+#endif /* ( ENABLE_SME_FOR_NS | ENABLE_SVE_FOR_NS ) */
 
 #endif /* SVE_H */
diff --git a/include/lib/extensions/sys_reg_trace.h b/include/lib/extensions/sys_reg_trace.h
index 74470fe..5915c55 100644
--- a/include/lib/extensions/sys_reg_trace.h
+++ b/include/lib/extensions/sys_reg_trace.h
@@ -9,10 +9,24 @@
 
 #include <context.h>
 
+#if ENABLE_SYS_REG_TRACE_FOR_NS
 #if __aarch64__
 void sys_reg_trace_enable(cpu_context_t *context);
 #else
 void sys_reg_trace_enable(void);
 #endif /* __aarch64__ */
 
+#else /* !ENABLE_SYS_REG_TRACE_FOR_NS */
+
+#if __aarch64__
+static inline void sys_reg_trace_enable(cpu_context_t *context)
+{
+}
+#else
+static inline void sys_reg_trace_enable(void)
+{
+}
+#endif /* __aarch64__ */
+#endif /* ENABLE_SYS_REG_TRACE_FOR_NS */
+
 #endif /* SYS_REG_TRACE_H */
diff --git a/include/lib/extensions/trbe.h b/include/lib/extensions/trbe.h
index 0eff7c4..861a4ad 100644
--- a/include/lib/extensions/trbe.h
+++ b/include/lib/extensions/trbe.h
@@ -10,7 +10,7 @@
 #if ENABLE_TRBE_FOR_NS
 void trbe_enable(void);
 #else
-void trbe_enable(void)
+static inline void trbe_enable(void)
 {
 }
 #endif /* ENABLE_TRBE_FOR_NS */
diff --git a/include/lib/extensions/trf.h b/include/lib/extensions/trf.h
index a440abd..91a9615 100644
--- a/include/lib/extensions/trf.h
+++ b/include/lib/extensions/trf.h
@@ -10,7 +10,7 @@
 #if ENABLE_TRF_FOR_NS
 void trf_enable(void);
 #else
-void trf_enable(void)
+static inline void trf_enable(void)
 {
 }
 #endif /* ENABLE_TRF_FOR_NS */
diff --git a/include/lib/libc/cdefs.h b/include/lib/libc/cdefs.h
index 423f0db..2423f38 100644
--- a/include/lib/libc/cdefs.h
+++ b/include/lib/libc/cdefs.h
@@ -15,6 +15,7 @@
 #define __maybe_unused	__attribute__((__unused__))
 #define __aligned(x)	__attribute__((__aligned__(x)))
 #define __section(x)	__attribute__((__section__(x)))
+#define __fallthrough	__attribute__((__fallthrough__))
 #if RECLAIM_INIT_CODE
 /*
  * Add each function to a section that is unique so the functions can still
diff --git a/include/lib/psci/psci.h b/include/lib/psci/psci.h
index b56e98b..6d27b7b 100644
--- a/include/lib/psci/psci.h
+++ b/include/lib/psci/psci.h
@@ -59,6 +59,7 @@
 #define PSCI_NODE_HW_STATE_AARCH64	U(0xc400000d)
 #define PSCI_SYSTEM_SUSPEND_AARCH32	U(0x8400000E)
 #define PSCI_SYSTEM_SUSPEND_AARCH64	U(0xc400000E)
+#define PSCI_SET_SUSPEND_MODE		U(0x8400000F)
 #define PSCI_STAT_RESIDENCY_AARCH32	U(0x84000010)
 #define PSCI_STAT_RESIDENCY_AARCH64	U(0xc4000010)
 #define PSCI_STAT_COUNT_AARCH32		U(0x84000011)
@@ -73,9 +74,17 @@
  * Number of PSCI calls (above) implemented
  */
 #if ENABLE_PSCI_STAT
-#define PSCI_NUM_CALLS			U(22)
+#if PSCI_OS_INIT_MODE
+#define PSCI_NUM_CALLS			U(30)
 #else
-#define PSCI_NUM_CALLS			U(18)
+#define PSCI_NUM_CALLS			U(29)
+#endif
+#else
+#if PSCI_OS_INIT_MODE
+#define PSCI_NUM_CALLS			U(26)
+#else
+#define PSCI_NUM_CALLS			U(25)
+#endif
 #endif
 
 /* The macros below are used to identify PSCI calls from the SMC function ID */
@@ -134,7 +143,11 @@
 
 /* Features flags for CPU SUSPEND OS Initiated mode support. Bits [0:0] */
 #define FF_MODE_SUPPORT_SHIFT		U(0)
+#if PSCI_OS_INIT_MODE
 #define FF_SUPPORTS_OS_INIT_MODE	U(1)
+#else
+#define FF_SUPPORTS_OS_INIT_MODE	U(0)
+#endif
 
 /*******************************************************************************
  * PSCI version
@@ -268,6 +281,13 @@
 	 * for the CPU.
 	 */
 	plat_local_state_t pwr_domain_state[PLAT_MAX_PWR_LVL + U(1)];
+#if PSCI_OS_INIT_MODE
+	/*
+	 * The highest power level at which the current CPU is the last running
+	 * CPU.
+	 */
+	unsigned int last_at_pwrlvl;
+#endif
 } psci_power_state_t;
 
 /*******************************************************************************
@@ -299,7 +319,11 @@
 	void (*pwr_domain_off)(const psci_power_state_t *target_state);
 	void (*pwr_domain_suspend_pwrdown_early)(
 				const psci_power_state_t *target_state);
+#if PSCI_OS_INIT_MODE
+	int (*pwr_domain_suspend)(const psci_power_state_t *target_state);
+#else
 	void (*pwr_domain_suspend)(const psci_power_state_t *target_state);
+#endif
 	void (*pwr_domain_on_finish)(const psci_power_state_t *target_state);
 	void (*pwr_domain_on_finish_late)(
 				const psci_power_state_t *target_state);
@@ -347,6 +371,9 @@
 int psci_node_hw_state(u_register_t target_cpu,
 		       unsigned int power_level);
 int psci_features(unsigned int psci_fid);
+#if PSCI_OS_INIT_MODE
+int psci_set_suspend_mode(unsigned int mode);
+#endif
 void __dead2 psci_power_down_wfi(void);
 void psci_arch_setup(void);
 
diff --git a/include/lib/psci/psci_lib.h b/include/lib/psci/psci_lib.h
index 3edc50b..4b244ec 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);
+bool psci_are_all_cpus_on_safe(void);
 void psci_pwrdown_cpu(unsigned int power_level);
 
 #endif /* __ASSEMBLER__ */
diff --git a/include/lib/smccc.h b/include/lib/smccc.h
index cce91af..63637d5 100644
--- a/include/lib/smccc.h
+++ b/include/lib/smccc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2023, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -37,9 +37,13 @@
 #define FUNCID_OEN_MASK			U(0x3f)
 #define FUNCID_OEN_WIDTH		U(6)
 
-#define FUNCID_SVE_HINT_SHIFT          U(16)
-#define FUNCID_SVE_HINT_MASK           U(1)
-#define FUNCID_SVE_HINT_WIDTH          U(1)
+#define FUNCID_FC_RESERVED_SHIFT	U(17)
+#define FUNCID_FC_RESERVED_MASK		U(0x7f)
+#define FUNCID_FC_RESERVED_WIDTH	U(7)
+
+#define FUNCID_SVE_HINT_SHIFT		U(16)
+#define FUNCID_SVE_HINT_MASK		U(1)
+#define FUNCID_SVE_HINT_WIDTH		U(1)
 
 #define FUNCID_NUM_SHIFT		U(0)
 #define FUNCID_NUM_MASK			U(0xffff)
diff --git a/include/plat/arm/common/arm_def.h b/include/plat/arm/common/arm_def.h
index 1bb7db7..925ff21 100644
--- a/include/plat/arm/common/arm_def.h
+++ b/include/plat/arm/common/arm_def.h
@@ -82,6 +82,7 @@
  *   - L1 GPT DRAM: Reserved for L1 GPT if RME is enabled
  *   - REALM DRAM: Reserved for Realm world if RME is enabled
  *   - TF-A <-> RMM SHARED: Area shared for communication between TF-A and RMM
+ *   - Event Log: Area for Event Log if MEASURED_BOOT feature is enabled
  *   - AP TZC DRAM: The remaining TZC secured DRAM reserved for AP use
  *
  *              RME enabled(64MB)                RME not enabled(16MB)
@@ -89,13 +90,15 @@
  *              |                  |             |                 |
  *              |  AP TZC (~28MB)  |             |  AP TZC (~14MB) |
  *              --------------------             -------------------
+ *              |     Event Log    |             |     Event Log   |
+ *              |      (4KB)       |             |      (4KB)      |
+ *              --------------------             -------------------
+ *              |   REALM (RMM)    |             |                 |
+ *              |   (32MB - 4KB)   |             |  EL3 TZC (2MB)  |
+ *              --------------------             -------------------
  *              |                  |             |                 |
- *              |   REALM (RMM)    |             |  EL3 TZC (2MB)  |
- *              |   (32MB - 4KB)   |             -------------------
- *              --------------------             |                 |
- *              |                  |             |    SCP TZC      |
- *              |   TF-A <-> RMM   |  0xFFFF_FFFF-------------------
- *              |   SHARED (4KB)   |
+ *              |   TF-A <-> RMM   |             |    SCP TZC      |
+ *              |   SHARED (4KB)   |  0xFFFF_FFFF-------------------
  *              --------------------
  *              |                  |
  *              |  EL3 TZC (3MB)   |
@@ -114,7 +117,6 @@
  */
 #define ARM_EL3_TZC_DRAM1_SIZE		UL(0x00300000) /* 3MB */
 #define ARM_L1_GPT_SIZE			UL(0x00100000) /* 1MB */
-
 /* 32MB - ARM_EL3_RMM_SHARED_SIZE */
 #define ARM_REALM_SIZE			(UL(0x02000000) -		\
 						ARM_EL3_RMM_SHARED_SIZE)
@@ -134,6 +136,25 @@
 #define ARM_SCP_TZC_DRAM1_SIZE		PLAT_ARM_SCP_TZC_DRAM1_SIZE
 #define ARM_SCP_TZC_DRAM1_END		(ARM_SCP_TZC_DRAM1_BASE +	\
 					ARM_SCP_TZC_DRAM1_SIZE - 1U)
+
+# if (defined(SPD_tspd) || defined(SPD_opteed) || defined(SPD_spmd)) && \
+MEASURED_BOOT
+#define ARM_EVENT_LOG_DRAM1_SIZE	UL(0x00001000)	/* 4KB */
+
+#if ENABLE_RME
+#define ARM_EVENT_LOG_DRAM1_BASE	(ARM_REALM_BASE -		\
+					 ARM_EVENT_LOG_DRAM1_SIZE)
+#else
+#define ARM_EVENT_LOG_DRAM1_BASE	(ARM_EL3_TZC_DRAM1_BASE -	\
+					 ARM_EVENT_LOG_DRAM1_SIZE)
+#endif /* ENABLE_RME */
+#define ARM_EVENT_LOG_DRAM1_END		(ARM_EVENT_LOG_DRAM1_BASE +	\
+					 ARM_EVENT_LOG_DRAM1_SIZE -	\
+					 1U)
+#else
+#define ARM_EVENT_LOG_DRAM1_SIZE	UL(0)
+#endif /* (SPD_tspd || SPD_opteed || SPD_spmd) && MEASURED_BOOT */
+
 #if ENABLE_RME
 #define ARM_L1_GPT_ADDR_BASE		(ARM_DRAM1_BASE +		\
 					ARM_DRAM1_SIZE -		\
@@ -170,7 +191,9 @@
 					ARM_EL3_TZC_DRAM1_SIZE +	\
 					ARM_EL3_RMM_SHARED_SIZE +	\
 					ARM_REALM_SIZE +		\
-					ARM_L1_GPT_SIZE))
+					ARM_L1_GPT_SIZE +		\
+					ARM_EVENT_LOG_DRAM1_SIZE))
+
 #define ARM_AP_TZC_DRAM1_END		(ARM_AP_TZC_DRAM1_BASE +	\
 					ARM_AP_TZC_DRAM1_SIZE - 1U)
 
@@ -312,6 +335,15 @@
 					PLAT_ARM_TRUSTED_DRAM_BASE,	\
 					PLAT_ARM_TRUSTED_DRAM_SIZE,	\
 					MT_MEMORY | MT_RW | MT_SECURE)
+
+# if (defined(SPD_tspd) || defined(SPD_opteed) || defined(SPD_spmd)) && \
+MEASURED_BOOT
+#define ARM_MAP_EVENT_LOG_DRAM1						\
+				MAP_REGION_FLAT(			\
+					ARM_EVENT_LOG_DRAM1_BASE,	\
+					ARM_EVENT_LOG_DRAM1_SIZE,	\
+					MT_MEMORY | MT_RW | MT_SECURE)
+#endif /* (SPD_tspd || SPD_opteed || SPD_spmd) && MEASURED_BOOT */
 
 #if ENABLE_RME
 /*
diff --git a/include/plat/arm/common/plat_arm.h b/include/plat/arm/common/plat_arm.h
index 494e470..34f913b 100644
--- a/include/plat/arm/common/plat_arm.h
+++ b/include/plat/arm/common/plat_arm.h
@@ -125,6 +125,12 @@
 #define ARM_LOCAL_PSTATE_WIDTH		4
 #define ARM_LOCAL_PSTATE_MASK		((1 << ARM_LOCAL_PSTATE_WIDTH) - 1)
 
+#if PSCI_OS_INIT_MODE
+#define ARM_LAST_AT_PLVL_MASK		(ARM_LOCAL_PSTATE_MASK <<	\
+					 (ARM_LOCAL_PSTATE_WIDTH *	\
+					  (PLAT_MAX_PWR_LVL + 1)))
+#endif /* __PSCI_OS_INIT_MODE__ */
+
 /* Macros to construct the composite power state */
 
 /* Make composite power state parameter till power level 0 */
diff --git a/lib/cpus/aarch64/cortex_a75.S b/lib/cpus/aarch64/cortex_a75.S
index d561be4..e22c828 100644
--- a/lib/cpus/aarch64/cortex_a75.S
+++ b/lib/cpus/aarch64/cortex_a75.S
@@ -121,7 +121,7 @@
 	bl	errata_dsu_936184_wa
 #endif
 
-#if ENABLE_AMU
+#if ENABLE_FEAT_AMU
 	/* Make sure accesses from EL0/EL1 and EL2 are not trapped to EL3 */
 	mrs	x0, actlr_el3
 	orr	x0, x0, #CORTEX_A75_ACTLR_AMEN_BIT
diff --git a/lib/cpus/aarch64/cortex_a78.S b/lib/cpus/aarch64/cortex_a78.S
index 421509d..69d7ab0 100644
--- a/lib/cpus/aarch64/cortex_a78.S
+++ b/lib/cpus/aarch64/cortex_a78.S
@@ -483,7 +483,7 @@
 	bl	errata_a78_2779479_wa
 #endif
 
-#if ENABLE_AMU
+#if ENABLE_FEAT_AMU
 	/* Make sure accesses from EL0/EL1 and EL2 are not trapped to EL3 */
 	mrs	x0, actlr_el3
 	bic	x0, x0, #CORTEX_A78_ACTLR_TAM_BIT
diff --git a/lib/cpus/aarch64/cortex_a78_ae.S b/lib/cpus/aarch64/cortex_a78_ae.S
index 27adc38..d56f835 100644
--- a/lib/cpus/aarch64/cortex_a78_ae.S
+++ b/lib/cpus/aarch64/cortex_a78_ae.S
@@ -214,7 +214,7 @@
 	bl	errata_a78_ae_2395408_wa
 #endif
 
-#if ENABLE_AMU
+#if ENABLE_FEAT_AMU
 	/* Make sure accesses from EL0/EL1 and EL2 are not trapped to EL3 */
 	mrs	x0, actlr_el3
 	bic	x0, x0, #CORTEX_A78_ACTLR_TAM_BIT
diff --git a/lib/cpus/aarch64/cortex_a78c.S b/lib/cpus/aarch64/cortex_a78c.S
index 71f0725..fddd24f 100644
--- a/lib/cpus/aarch64/cortex_a78c.S
+++ b/lib/cpus/aarch64/cortex_a78c.S
@@ -18,6 +18,60 @@
 #endif
 
 /* --------------------------------------------------
+ * Errata Workaround for A78C Erratum 1827430.
+ * This applies to revision r0p0 of the Cortex A78C
+ * processor and is fixed in r0p1.
+ * x0: variant[4:7] and revision[0:3] of current cpu.
+ * Shall clobber: x0-x17
+ * --------------------------------------------------
+ */
+func errata_a78c_1827430_wa
+	mov	x17, x30
+	bl	check_errata_1827430
+	cbz	x0, 1f
+
+	/* Disable allocation of splintered pages in the L2 TLB */
+	mrs	x1, CORTEX_A78C_CPUECTLR_EL1
+	orr	x1, x1, CORTEX_A78C_CPUECTLR_EL1_MM_ASP_EN
+	msr	CORTEX_A78C_CPUECTLR_EL1, x1
+1:
+	ret	x17
+endfunc errata_a78c_1827430_wa
+
+func check_errata_1827430
+	/* Applies to revision r0p0 only */
+	mov	x1, #0x00
+	b	cpu_rev_var_ls
+endfunc check_errata_1827430
+
+/* --------------------------------------------------
+ * Errata Workaround for A78C Erratum 1827440.
+ * This applies to revision r0p0 of the Cortex A78C
+ * processor and is fixed in r0p1.
+ * x0: variant[4:7] and revision[0:3] of current cpu.
+ * Shall clobber: x0-x17
+ * --------------------------------------------------
+ */
+func errata_a78c_1827440_wa
+	mov	x17, x30
+	bl	check_errata_1827440
+	cbz	x0, 1f
+
+	/* Force Atomic Store to WB memory be done in L1 data cache */
+	mrs	x1, CORTEX_A78C_CPUACTLR2_EL1
+	orr	x1, x1, #BIT(2)
+	msr	CORTEX_A78C_CPUACTLR2_EL1, x1
+1:
+	ret	x17
+endfunc errata_a78c_1827440_wa
+
+func check_errata_1827440
+	/* Applies to revision r0p0 only */
+	mov	x1, #0x00
+	b	cpu_rev_var_ls
+endfunc check_errata_1827440
+
+/* --------------------------------------------------
  * Errata Workaround for Cortex A78C Erratum 2376749.
  * This applies to revision r0p1 and r0p2 of the A78C
  * and is currently open. It is a Cat B erratum.
@@ -225,6 +279,16 @@
 	bl	cpu_get_rev_var
 	mov	x18, x0
 
+#if ERRATA_A78C_1827430
+	mov	x0, x18
+	bl	errata_a78c_1827430_wa
+#endif
+
+#if ERRATA_A78C_1827440
+	mov	x0, x18
+	bl	errata_a78c_1827440_wa
+#endif
+
 #if ERRATA_A78C_2132064
 	mov	x0, x18
 	bl	errata_a78c_2132064_wa
@@ -299,6 +363,8 @@
 	 * Report all errata. The revision-variant information is passed to
 	 * checking functions of each errata.
 	 */
+	report_errata ERRATA_A78C_1827430, cortex_a78c, 1827430
+	report_errata ERRATA_A78C_1827440, cortex_a78c, 1827440
 	report_errata ERRATA_A78C_2132064, cortex_a78c, 2132064
 	report_errata ERRATA_A78C_2242638, cortex_a78c, 2242638
 	report_errata ERRATA_A78C_2376749, cortex_a78c, 2376749
diff --git a/lib/cpus/aarch64/cpu_helpers.S b/lib/cpus/aarch64/cpu_helpers.S
index ee880f7..0a03e38 100644
--- a/lib/cpus/aarch64/cpu_helpers.S
+++ b/lib/cpus/aarch64/cpu_helpers.S
@@ -31,10 +31,6 @@
 
 	/* Get the matching cpu_ops pointer */
 	bl	get_cpu_ops_ptr
-#if ENABLE_ASSERTIONS
-	cmp	x0, #0
-	ASM_ASSERT(ne)
-#endif
 
 	/* Get the cpu_ops reset handler */
 	ldr	x2, [x0, #CPU_RESET_FUNC]
@@ -100,10 +96,6 @@
 	cbnz	x0, 1f
 	mov	x10, x30
 	bl	get_cpu_ops_ptr
-#if ENABLE_ASSERTIONS
-	cmp	x0, #0
-	ASM_ASSERT(ne)
-#endif
 	str	x0, [x6, #CPU_DATA_CPU_OPS_PTR]!
 	mov x30, x10
 1:
diff --git a/lib/cpus/aarch64/neoverse_n1.S b/lib/cpus/aarch64/neoverse_n1.S
index ec62519..827c0b0 100644
--- a/lib/cpus/aarch64/neoverse_n1.S
+++ b/lib/cpus/aarch64/neoverse_n1.S
@@ -585,7 +585,7 @@
 	bl	errata_n1_1946160_wa
 #endif
 
-#if ENABLE_AMU
+#if ENABLE_FEAT_AMU
 	/* Make sure accesses from EL0/EL1 and EL2 are not trapped to EL3 */
 	mrs	x0, actlr_el3
 	orr	x0, x0, #NEOVERSE_N1_ACTLR_AMEN_BIT
diff --git a/lib/cpus/aarch64/neoverse_n2.S b/lib/cpus/aarch64/neoverse_n2.S
index dbf5941..60d322f 100644
--- a/lib/cpus/aarch64/neoverse_n2.S
+++ b/lib/cpus/aarch64/neoverse_n2.S
@@ -545,7 +545,7 @@
 	bl	errata_n2_2388450_wa
 #endif
 
-#if ENABLE_AMU
+#if ENABLE_FEAT_AMU
 	/* Make sure accesses from EL0/EL1 and EL2 are not trapped to EL3 */
 	mrs	x0, cptr_el3
 	orr	x0, x0, #TAM_BIT
diff --git a/lib/cpus/aarch64/rainier.S b/lib/cpus/aarch64/rainier.S
index 584ab97..3b7b8b2 100644
--- a/lib/cpus/aarch64/rainier.S
+++ b/lib/cpus/aarch64/rainier.S
@@ -94,7 +94,7 @@
 	bl	errata_n1_1868343_wa
 #endif
 
-#if ENABLE_AMU
+#if ENABLE_FEAT_AMU
 	/* Make sure accesses from EL0/EL1 and EL2 are not trapped to EL3 */
 	mrs	x0, actlr_el3
 	orr	x0, x0, #RAINIER_ACTLR_AMEN_BIT
diff --git a/lib/cpus/cpu-ops.mk b/lib/cpus/cpu-ops.mk
index 51b2175..82a4890 100644
--- a/lib/cpus/cpu-ops.mk
+++ b/lib/cpus/cpu-ops.mk
@@ -5,1462 +5,768 @@
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+include ${MAKE_HELPERS_DIRECTORY}$/build_macros.mk
+
 # Cortex A57 specific optimisation to skip L1 cache flush when
 # cluster is powered down.
-SKIP_A57_L1_FLUSH_PWR_DWN		?=0
+CPU_FLAG_LIST += SKIP_A57_L1_FLUSH_PWR_DWN
 
 # Flag to disable the cache non-temporal hint.
 # It is enabled by default.
 A53_DISABLE_NON_TEMPORAL_HINT		?=1
+CPU_FLAG_LIST += A53_DISABLE_NON_TEMPORAL_HINT
 
 # Flag to disable the cache non-temporal hint.
 # It is enabled by default.
 A57_DISABLE_NON_TEMPORAL_HINT		?=1
+CPU_FLAG_LIST += A57_DISABLE_NON_TEMPORAL_HINT
 
 # Flag to enable higher performance non-cacheable load forwarding.
 # It is disabled by default.
-A57_ENABLE_NONCACHEABLE_LOAD_FWD	?= 0
+CPU_FLAG_LIST += A57_ENABLE_NONCACHEABLE_LOAD_FWD
 
 WORKAROUND_CVE_2017_5715		?=1
+CPU_FLAG_LIST += WORKAROUND_CVE_2017_5715
 WORKAROUND_CVE_2018_3639		?=1
-DYNAMIC_WORKAROUND_CVE_2018_3639	?=0
+CPU_FLAG_LIST += WORKAROUND_CVE_2018_3639
+CPU_FLAG_LIST += DYNAMIC_WORKAROUND_CVE_2018_3639
 WORKAROUND_CVE_2022_23960		?=1
+CPU_FLAG_LIST += WORKAROUND_CVE_2022_23960
 
 # Flags to indicate internal or external Last level cache
 # By default internal
-NEOVERSE_Nx_EXTERNAL_LLC		?=0
-
-# Process A57_ENABLE_NONCACHEABLE_LOAD_FWD flag
-$(eval $(call assert_boolean,A57_ENABLE_NONCACHEABLE_LOAD_FWD))
-$(eval $(call add_define,A57_ENABLE_NONCACHEABLE_LOAD_FWD))
-
-# Process SKIP_A57_L1_FLUSH_PWR_DWN flag
-$(eval $(call assert_boolean,SKIP_A57_L1_FLUSH_PWR_DWN))
-$(eval $(call add_define,SKIP_A57_L1_FLUSH_PWR_DWN))
-
-# Process A53_DISABLE_NON_TEMPORAL_HINT flag
-$(eval $(call assert_boolean,A53_DISABLE_NON_TEMPORAL_HINT))
-$(eval $(call add_define,A53_DISABLE_NON_TEMPORAL_HINT))
-
-# Process A57_DISABLE_NON_TEMPORAL_HINT flag
-$(eval $(call assert_boolean,A57_DISABLE_NON_TEMPORAL_HINT))
-$(eval $(call add_define,A57_DISABLE_NON_TEMPORAL_HINT))
-
-# Process WORKAROUND_CVE_2017_5715 flag
-$(eval $(call assert_boolean,WORKAROUND_CVE_2017_5715))
-$(eval $(call add_define,WORKAROUND_CVE_2017_5715))
-
-# Process WORKAROUND_CVE_2018_3639 flag
-$(eval $(call assert_boolean,WORKAROUND_CVE_2018_3639))
-$(eval $(call add_define,WORKAROUND_CVE_2018_3639))
-
-$(eval $(call assert_boolean,DYNAMIC_WORKAROUND_CVE_2018_3639))
-$(eval $(call add_define,DYNAMIC_WORKAROUND_CVE_2018_3639))
-
-# Process WORKAROUND_CVE_2022_23960 flag
-$(eval $(call assert_boolean,WORKAROUND_CVE_2022_23960))
-$(eval $(call add_define,WORKAROUND_CVE_2022_23960))
-
-$(eval $(call assert_boolean,NEOVERSE_Nx_EXTERNAL_LLC))
-$(eval $(call add_define,NEOVERSE_Nx_EXTERNAL_LLC))
-
-ifneq (${DYNAMIC_WORKAROUND_CVE_2018_3639},0)
-	ifeq (${WORKAROUND_CVE_2018_3639},0)
-		$(error "Error: WORKAROUND_CVE_2018_3639 must be 1 if DYNAMIC_WORKAROUND_CVE_2018_3639 is 1")
-	endif
-endif
+CPU_FLAG_LIST += NEOVERSE_Nx_EXTERNAL_LLC
 
 # CPU Errata Build flags.
 # These should be enabled by the platform if the erratum workaround needs to be
 # applied.
 
 # Flag to apply erratum 794073 workaround when disabling mmu.
-ERRATA_A9_794073	?=0
+CPU_FLAG_LIST += ERRATA_A9_794073
 
 # Flag to apply erratum 816470 workaround during power down. This erratum
 # applies only to revision >= r3p0 of the Cortex A15 cpu.
-ERRATA_A15_816470	?=0
+CPU_FLAG_LIST += ERRATA_A15_816470
 
 # Flag to apply erratum 827671 workaround during reset. This erratum applies
 # only to revision >= r3p0 of the Cortex A15 cpu.
-ERRATA_A15_827671	?=0
+CPU_FLAG_LIST += ERRATA_A15_827671
 
 # Flag to apply erratum 852421 workaround during reset. This erratum applies
 # only to revision <= r1p2 of the Cortex A17 cpu.
-ERRATA_A17_852421	?=0
+CPU_FLAG_LIST += ERRATA_A17_852421
 
 # Flag to apply erratum 852423 workaround during reset. This erratum applies
 # only to revision <= r1p2 of the Cortex A17 cpu.
-ERRATA_A17_852423	?=0
+CPU_FLAG_LIST += ERRATA_A17_852423
 
 # Flag to apply erratum 855472 workaround during reset. This erratum applies
 # only to revision r0p0 of the Cortex A35 cpu.
-ERRATA_A35_855472	?=0
+CPU_FLAG_LIST += ERRATA_A35_855472
 
 # Flag to apply erratum 819472 workaround during reset. This erratum applies
 # only to revision <= r0p1 of the Cortex A53 cpu.
-ERRATA_A53_819472	?=0
+CPU_FLAG_LIST += ERRATA_A53_819472
 
 # Flag to apply erratum 824069 workaround during reset. This erratum applies
 # only to revision <= r0p2 of the Cortex A53 cpu.
-ERRATA_A53_824069	?=0
+CPU_FLAG_LIST += ERRATA_A53_824069
 
 # Flag to apply erratum 826319 workaround during reset. This erratum applies
 # only to revision <= r0p2 of the Cortex A53 cpu.
-ERRATA_A53_826319	?=0
+CPU_FLAG_LIST += ERRATA_A53_826319
 
 # Flag to apply erratum 827319 workaround during reset. This erratum applies
 # only to revision <= r0p2 of the Cortex A53 cpu.
-ERRATA_A53_827319	?=0
+CPU_FLAG_LIST += ERRATA_A53_827319
 
 # Flag to apply erratum 835769 workaround at compile and link time. This
 # erratum applies to revision <= r0p4 of the Cortex A53 cpu. Enabling this
 # workaround can lead the linker to create "*.stub" sections.
-ERRATA_A53_835769	?=0
+CPU_FLAG_LIST += ERRATA_A53_835769
 
 # Flag to apply erratum 836870 workaround during reset. This erratum applies
 # only to revision <= r0p3 of the Cortex A53 cpu. From r0p4 and onwards, this
 # erratum workaround is enabled by default in hardware.
-ERRATA_A53_836870	?=0
+CPU_FLAG_LIST += ERRATA_A53_836870
 
 # Flag to apply erratum 843419 workaround at link time.
 # This erratum applies to revision <= r0p4 of the Cortex A53 cpu. Enabling this
 # workaround could lead the linker to emit "*.stub" sections which are 4kB
 # aligned.
-ERRATA_A53_843419	?=0
+CPU_FLAG_LIST += ERRATA_A53_843419
 
 # Flag to apply errata 855873 during reset. This errata applies to all
 # revisions of the Cortex A53 CPU, but this firmware workaround only works
 # for revisions r0p3 and higher. Earlier revisions are taken care
 # of by the rich OS.
-ERRATA_A53_855873	?=0
+CPU_FLAG_LIST += ERRATA_A53_855873
 
 # Flag to apply erratum 1530924 workaround during reset. This erratum applies
 # to all revisions of Cortex A53 cpu.
-ERRATA_A53_1530924	?=0
+CPU_FLAG_LIST += ERRATA_A53_1530924
 
 # Flag to apply erratum 768277 workaround during reset. This erratum applies
 # only to revision r0p0 of the Cortex A55 cpu.
-ERRATA_A55_768277	?=0
+CPU_FLAG_LIST += ERRATA_A55_768277
 
 # Flag to apply erratum 778703 workaround during reset. This erratum applies
 # only to revision r0p0 of the Cortex A55 cpu.
-ERRATA_A55_778703	?=0
+CPU_FLAG_LIST += ERRATA_A55_778703
 
 # Flag to apply erratum 798797 workaround during reset. This erratum applies
 # only to revision r0p0 of the Cortex A55 cpu.
-ERRATA_A55_798797	?=0
+CPU_FLAG_LIST += ERRATA_A55_798797
 
 # Flag to apply erratum 846532 workaround during reset. This erratum applies
 # only to revision <= r0p1 of the Cortex A55 cpu.
-ERRATA_A55_846532	?=0
+CPU_FLAG_LIST += ERRATA_A55_846532
 
 # Flag to apply erratum 903758 workaround during reset. This erratum applies
 # only to revision <= r0p1 of the Cortex A55 cpu.
-ERRATA_A55_903758	?=0
+CPU_FLAG_LIST += ERRATA_A55_903758
 
 # Flag to apply erratum 1221012 workaround during reset. This erratum applies
 # only to revision <= r1p0 of the Cortex A55 cpu.
-ERRATA_A55_1221012	?=0
+CPU_FLAG_LIST += ERRATA_A55_1221012
 
 # Flag to apply erratum 1530923 workaround during reset. This erratum applies
 # to all revisions of Cortex A55 cpu.
-ERRATA_A55_1530923	?=0
+CPU_FLAG_LIST += ERRATA_A55_1530923
 
 # Flag to apply erratum 806969 workaround during reset. This erratum applies
 # only to revision r0p0 of the Cortex A57 cpu.
-ERRATA_A57_806969	?=0
+CPU_FLAG_LIST += ERRATA_A57_806969
 
 # Flag to apply erratum 813419 workaround during reset. This erratum applies
 # only to revision r0p0 of the Cortex A57 cpu.
-ERRATA_A57_813419	?=0
+CPU_FLAG_LIST += ERRATA_A57_813419
 
 # Flag to apply erratum 813420 workaround during reset. This erratum applies
 # only to revision r0p0 of the Cortex A57 cpu.
-ERRATA_A57_813420	?=0
+CPU_FLAG_LIST += ERRATA_A57_813420
 
 # Flag to apply erratum 814670 workaround during reset. This erratum applies
 # only to revision r0p0 of the Cortex A57 cpu.
-ERRATA_A57_814670	?=0
+CPU_FLAG_LIST += ERRATA_A57_814670
 
 # Flag to apply erratum 817169 workaround during power down. This erratum
 # applies only to revision <= r0p1 of the Cortex A57 cpu.
-ERRATA_A57_817169	?=0
+CPU_FLAG_LIST += ERRATA_A57_817169
 
 # Flag to apply erratum 826974 workaround during reset. This erratum applies
 # only to revision <= r1p1 of the Cortex A57 cpu.
-ERRATA_A57_826974	?=0
+CPU_FLAG_LIST += ERRATA_A57_826974
 
 # Flag to apply erratum 826977 workaround during reset. This erratum applies
 # only to revision <= r1p1 of the Cortex A57 cpu.
-ERRATA_A57_826977	?=0
+CPU_FLAG_LIST += ERRATA_A57_826977
 
 # Flag to apply erratum 828024 workaround during reset. This erratum applies
 # only to revision <= r1p1 of the Cortex A57 cpu.
-ERRATA_A57_828024	?=0
+CPU_FLAG_LIST += ERRATA_A57_828024
 
 # Flag to apply erratum 829520 workaround during reset. This erratum applies
 # only to revision <= r1p2 of the Cortex A57 cpu.
-ERRATA_A57_829520	?=0
+CPU_FLAG_LIST += ERRATA_A57_829520
 
 # Flag to apply erratum 833471 workaround during reset. This erratum applies
 # only to revision <= r1p2 of the Cortex A57 cpu.
-ERRATA_A57_833471	?=0
+CPU_FLAG_LIST += ERRATA_A57_833471
 
 # Flag to apply erratum 855972 workaround during reset. This erratum applies
 # only to revision <= r1p3 of the Cortex A57 cpu.
-ERRATA_A57_859972	?=0
+CPU_FLAG_LIST += ERRATA_A57_859972
 
 # Flag to apply erratum 1319537 workaround during reset. This erratum applies
 # to all revisions of Cortex A57 cpu.
-ERRATA_A57_1319537	?=0
+CPU_FLAG_LIST += ERRATA_A57_1319537
 
 # Flag to apply erratum 855971 workaround during reset. This erratum applies
 # only to revision <= r0p3 of the Cortex A72 cpu.
-ERRATA_A72_859971	?=0
+CPU_FLAG_LIST += ERRATA_A72_859971
 
 # Flag to apply erratum 1319367 workaround during reset. This erratum applies
 # to all revisions of Cortex A72 cpu.
-ERRATA_A72_1319367	?=0
+CPU_FLAG_LIST += ERRATA_A72_1319367
 
 # Flag to apply erratum 852427 workaround during reset. This erratum applies
 # only to revision r0p0 of the Cortex A73 cpu.
-ERRATA_A73_852427	?=0
+CPU_FLAG_LIST += ERRATA_A73_852427
 
 # Flag to apply erratum 855423 workaround during reset. This erratum applies
 # only to revision <= r0p1 of the Cortex A73 cpu.
-ERRATA_A73_855423	?=0
+CPU_FLAG_LIST += ERRATA_A73_855423
 
 # Flag to apply erratum 764081 workaround during reset. This erratum applies
 # only to revision <= r0p0 of the Cortex A75 cpu.
-ERRATA_A75_764081	?=0
+CPU_FLAG_LIST += ERRATA_A75_764081
 
 # Flag to apply erratum 790748 workaround during reset. This erratum applies
 # only to revision <= r0p0 of the Cortex A75 cpu.
-ERRATA_A75_790748	?=0
+CPU_FLAG_LIST += ERRATA_A75_790748
 
 # Flag to apply erratum 1073348 workaround during reset. This erratum applies
 # only to revision <= r1p0 of the Cortex A76 cpu.
-ERRATA_A76_1073348	?=0
+CPU_FLAG_LIST += ERRATA_A76_1073348
 
 # Flag to apply erratum 1130799 workaround during reset. This erratum applies
 # only to revision <= r2p0 of the Cortex A76 cpu.
-ERRATA_A76_1130799	?=0
+CPU_FLAG_LIST += ERRATA_A76_1130799
 
 # Flag to apply erratum 1220197 workaround during reset. This erratum applies
 # only to revision <= r2p0 of the Cortex A76 cpu.
-ERRATA_A76_1220197	?=0
+CPU_FLAG_LIST += ERRATA_A76_1220197
 
 # Flag to apply erratum 1257314 workaround during reset. This erratum applies
 # only to revision <= r3p0 of the Cortex A76 cpu.
-ERRATA_A76_1257314	?=0
+CPU_FLAG_LIST += ERRATA_A76_1257314
 
 # Flag to apply erratum 1262606 workaround during reset. This erratum applies
 # only to revision <= r3p0 of the Cortex A76 cpu.
-ERRATA_A76_1262606	?=0
+CPU_FLAG_LIST += ERRATA_A76_1262606
 
 # Flag to apply erratum 1262888 workaround during reset. This erratum applies
 # only to revision <= r3p0 of the Cortex A76 cpu.
-ERRATA_A76_1262888	?=0
+CPU_FLAG_LIST += ERRATA_A76_1262888
 
 # Flag to apply erratum 1275112 workaround during reset. This erratum applies
 # only to revision <= r3p0 of the Cortex A76 cpu.
-ERRATA_A76_1275112	?=0
+CPU_FLAG_LIST += ERRATA_A76_1275112
 
 # Flag to apply erratum 1286807 workaround during reset. This erratum applies
 # only to revision <= r3p0 of the Cortex A76 cpu.
-ERRATA_A76_1286807	?=0
+CPU_FLAG_LIST += ERRATA_A76_1286807
 
 # Flag to apply erratum 1791580 workaround during reset. This erratum applies
 # only to revision <= r4p0 of the Cortex A76 cpu.
-ERRATA_A76_1791580	?=0
+CPU_FLAG_LIST += ERRATA_A76_1791580
 
 # Flag to apply erratum 1165522 workaround during reset. This erratum applies
 # to all revisions of Cortex A76 cpu.
-ERRATA_A76_1165522	?=0
+CPU_FLAG_LIST += ERRATA_A76_1165522
 
 # Flag to apply erratum 1868343 workaround during reset. This erratum applies
 # only to revision <= r4p0 of the Cortex A76 cpu.
-ERRATA_A76_1868343	?=0
+CPU_FLAG_LIST += ERRATA_A76_1868343
 
 # Flag to apply erratum 1946160 workaround during reset. This erratum applies
 # only to revisions r3p0 - r4p1 of the Cortex A76 cpu.
-ERRATA_A76_1946160	?=0
+CPU_FLAG_LIST += ERRATA_A76_1946160
 
 # Flag to apply erratum 2743102 workaround during powerdown. This erratum
 # applies to all revisions <= r4p1 of the Cortex A76 cpu and is still open.
-ERRATA_A76_2743102	?=0
+CPU_FLAG_LIST += ERRATA_A76_2743102
 
 # Flag to apply erratum 1508412 workaround during reset. This erratum applies
 # only to revision <= r1p0 of the Cortex A77 cpu.
-ERRATA_A77_1508412	?=0
+CPU_FLAG_LIST += ERRATA_A77_1508412
 
 # Flag to apply erratum 1925769 workaround during reset. This erratum applies
 # only to revision <= r1p1 of the Cortex A77 cpu.
-ERRATA_A77_1925769	?=0
+CPU_FLAG_LIST += ERRATA_A77_1925769
 
 # Flag to apply erratum 1946167 workaround during reset. This erratum applies
 # only to revision <= r1p1 of the Cortex A77 cpu.
-ERRATA_A77_1946167	?=0
+CPU_FLAG_LIST += ERRATA_A77_1946167
 
 # Flag to apply erratum 1791578 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0, and r1p1, it is still open.
-ERRATA_A77_1791578	?=0
+CPU_FLAG_LIST += ERRATA_A77_1791578
 
 # Flag to apply erratum 2356587 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0, and r1p1, it is still open.
-ERRATA_A77_2356587	?=0
+CPU_FLAG_LIST += ERRATA_A77_2356587
 
 # Flag to apply erratum 1800714 workaround during reset. This erratum applies
 # to revisions <= r1p1 of the Cortex A77 cpu.
-ERRATA_A77_1800714	?=0
+CPU_FLAG_LIST += ERRATA_A77_1800714
 
 # Flag to apply erratum 2743100 workaround during power down. This erratum
 # applies to revisions r0p0, r1p0, and r1p1, it is still open.
-ERRATA_A77_2743100	?=0
+CPU_FLAG_LIST += ERRATA_A77_2743100
 
 # Flag to apply erratum 1688305 workaround during reset. This erratum applies
 # to revisions r0p0 - r1p0 of the A78 cpu.
-ERRATA_A78_1688305	?=0
+CPU_FLAG_LIST += ERRATA_A78_1688305
 
 # Flag to apply erratum 1941498 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0, and r1p1 of the A78 cpu.
-ERRATA_A78_1941498	?=0
+CPU_FLAG_LIST += ERRATA_A78_1941498
 
 # Flag to apply erratum 1951500 workaround during reset. This erratum applies
 # to revisions r1p0 and r1p1 of the A78 cpu. The issue is present in r0p0 as
 # well but there is no workaround for that revision.
-ERRATA_A78_1951500	?=0
+CPU_FLAG_LIST += ERRATA_A78_1951500
 
 # Flag to apply erratum 1821534 workaround during reset. This erratum applies
 # to revisions r0p0 and r1p0 of the A78 cpu.
-ERRATA_A78_1821534	?=0
+CPU_FLAG_LIST += ERRATA_A78_1821534
 
 # Flag to apply erratum 1952683 workaround during reset. This erratum applies
 # to revision r0p0 of the A78 cpu and was fixed in the revision r1p0.
-ERRATA_A78_1952683	?=0
+CPU_FLAG_LIST += ERRATA_A78_1952683
 
 # Flag to apply erratum 2132060 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0, r1p1, and r1p2 of the A78 cpu. It is still open.
-ERRATA_A78_2132060	?=0
+CPU_FLAG_LIST += ERRATA_A78_2132060
 
 # Flag to apply erratum 2242635 workaround during reset. This erratum applies
 # to revisions r1p0, r1p1, and r1p2 of the A78 cpu and is open. The issue is
 # present in r0p0 as well but there is no workaround for that revision.
-ERRATA_A78_2242635	?=0
+CPU_FLAG_LIST += ERRATA_A78_2242635
 
 # Flag to apply erratum 2376745 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0, r1p1, and r1p2 of the A78 cpu. It is still open.
-ERRATA_A78_2376745	?=0
+CPU_FLAG_LIST += ERRATA_A78_2376745
 
 # Flag to apply erratum 2395406 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0, r1p1, and r1p2 of the A78 cpu. It is still open.
-ERRATA_A78_2395406	?=0
+CPU_FLAG_LIST += ERRATA_A78_2395406
 
 # Flag to apply erratum 2742426 workaround during reset. This erratum
 # applies to revisions r0p0, r1p0, r1p1 and r1p2 of the A78 cpu. It is still
 # open.
-ERRATA_A78_2742426	?=0
+CPU_FLAG_LIST += ERRATA_A78_2742426
 
 # Flag to apply erratum 2772019 workaround during powerdown. This erratum
 # applies to revisions r0p0, r1p0, r1p1 and r1p2 of the A78 cpu. It is still
 # open.
-ERRATA_A78_2772019	?=0
+CPU_FLAG_LIST += ERRATA_A78_2772019
 
 # Flag to apply erratum 2779479 workaround during reset. This erratum applies
 # to revision r0p0, r1p0, r1p1 and r1p2 of the A78 cpu. It is still open.
-ERRATA_A78_2779479	?=0
+CPU_FLAG_LIST += ERRATA_A78_2779479
 
 # Flag to apply erratum 1941500 workaround during reset. This erratum applies
 # to revisions r0p0 and r0p1 of the A78 AE cpu. It is still open.
-ERRATA_A78_AE_1941500	?=0
+CPU_FLAG_LIST += ERRATA_A78_AE_1941500
 
 # Flag to apply erratum 1951502 workaround during reset. This erratum applies
 # to revisions r0p0 and r0p1 of the A78 AE cpu. It is still open.
-ERRATA_A78_AE_1951502	?=0
+CPU_FLAG_LIST += ERRATA_A78_AE_1951502
 
 # Flag to apply erratum 2376748 workaround during reset. This erratum applies
 # to revisions r0p0 and r0p1 of the A78 AE cpu. It is still open.
-ERRATA_A78_AE_2376748	?=0
+CPU_FLAG_LIST += ERRATA_A78_AE_2376748
 
 # Flag to apply erratum 2395408 workaround during reset. This erratum applies
 # to revisions r0p0 and r0p1 of the A78 AE cpu. It is still open.
-ERRATA_A78_AE_2395408	?=0
+CPU_FLAG_LIST += ERRATA_A78_AE_2395408
+
+# Flag to apply erratum 1827430 workaround during reset. This erratum applies
+# to revision r0p0 of the A78C cpu. It is fixed in r0p1.
+CPU_FLAG_LIST += ERRATA_A78C_1827430
+
+# Flag to apply erratum 1827440 workaround during reset. This erratum applies
+# to revision r0p0 of the A78C cpu. It is fixed in r0p1.
+CPU_FLAG_LIST += ERRATA_A78C_1827440
 
 # Flag to apply erratum 2132064 workaround during reset. This erratum applies
 # to revisions r0p1 and r0p2 of the A78C cpu. It is still open.
-ERRATA_A78C_2132064	?=0
+CPU_FLAG_LIST += ERRATA_A78C_2132064
 
 # Flag to apply erratum 2242638 workaround during reset. This erratum applies
 # to revisions r0p1 and r0p2 of the A78C cpu. It is still open.
-ERRATA_A78C_2242638	?=0
+CPU_FLAG_LIST += ERRATA_A78C_2242638
 
 # Flag to apply erratum 2376749 workaround during reset. This erratum applies
 # to revisions r0p1 and r0p2 of the A78C cpu. It is still open.
-ERRATA_A78C_2376749 	?=0
+CPU_FLAG_LIST += ERRATA_A78C_2376749
 
 # Flag to apply erratum 2395411 workaround during reset. This erratum applies
 # to revisions r0p1 and r0p2 of the A78C cpu. It is still open.
-ERRATA_A78C_2395411 	?=0
+CPU_FLAG_LIST += ERRATA_A78C_2395411
 
 # Flag to apply erratum 2772121 workaround during powerdown. This erratum
 # applies to revisions r0p0, r0p1 and r0p2 of the A78C cpu. It is still open.
-ERRATA_A78C_2772121 	?=0
+CPU_FLAG_LIST += ERRATA_A78C_2772121
 
 # Flag to apply erratum 2779484 workaround during reset. This erratum
 # applies to revisions r0p1 and r0p2 of the A78C cpu. It is still open.
-ERRATA_A78C_2779484 	?=0
+CPU_FLAG_LIST += ERRATA_A78C_2779484
 
 # Flag to apply erratum 1821534 workaround during reset. This erratum applies
 # to revisions r0p0 - r1p0 of the X1 cpu and fixed in r1p1.
-ERRATA_X1_1821534	?=0
+CPU_FLAG_LIST += ERRATA_X1_1821534
 
 # Flag to apply erratum 1688305 workaround during reset. This erratum applies
 # to revisions r0p0 - r1p0 of the X1 cpu and fixed in r1p1.
-ERRATA_X1_1688305	?=0
+CPU_FLAG_LIST += ERRATA_X1_1688305
 
 # Flag to apply erratum 1827429 workaround during reset. This erratum applies
 # to revisions r0p0 - r1p0 of the X1 cpu and fixed in r1p1.
-ERRATA_X1_1827429	?=0
+CPU_FLAG_LIST += ERRATA_X1_1827429
 
 # Flag to apply T32 CLREX workaround during reset. This erratum applies
 # only to r0p0 and r1p0 of the Neoverse N1 cpu.
-ERRATA_N1_1043202	?=0
+CPU_FLAG_LIST += ERRATA_N1_1043202
 
 # Flag to apply erratum 1073348 workaround during reset. This erratum applies
 # only to revision r0p0 and r1p0 of the Neoverse N1 cpu.
-ERRATA_N1_1073348	?=0
+CPU_FLAG_LIST += ERRATA_N1_1073348
 
 # Flag to apply erratum 1130799 workaround during reset. This erratum applies
 # only to revision <= r2p0 of the Neoverse N1 cpu.
-ERRATA_N1_1130799	?=0
+CPU_FLAG_LIST += ERRATA_N1_1130799
 
 # Flag to apply erratum 1165347 workaround during reset. This erratum applies
 # only to revision <= r2p0 of the Neoverse N1 cpu.
-ERRATA_N1_1165347	?=0
+CPU_FLAG_LIST += ERRATA_N1_1165347
 
 # Flag to apply erratum 1207823 workaround during reset. This erratum applies
 # only to revision <= r2p0 of the Neoverse N1 cpu.
-ERRATA_N1_1207823	?=0
+CPU_FLAG_LIST += ERRATA_N1_1207823
 
 # Flag to apply erratum 1220197 workaround during reset. This erratum applies
 # only to revision <= r2p0 of the Neoverse N1 cpu.
-ERRATA_N1_1220197	?=0
+CPU_FLAG_LIST += ERRATA_N1_1220197
 
 # Flag to apply erratum 1257314 workaround during reset. This erratum applies
 # only to revision <= r3p0 of the Neoverse N1 cpu.
-ERRATA_N1_1257314	?=0
+CPU_FLAG_LIST += ERRATA_N1_1257314
 
 # Flag to apply erratum 1262606 workaround during reset. This erratum applies
 # only to revision <= r3p0 of the Neoverse N1 cpu.
-ERRATA_N1_1262606	?=0
+CPU_FLAG_LIST += ERRATA_N1_1262606
 
 # Flag to apply erratum 1262888 workaround during reset. This erratum applies
 # only to revision <= r3p0 of the Neoverse N1 cpu.
-ERRATA_N1_1262888	?=0
+CPU_FLAG_LIST += ERRATA_N1_1262888
 
 # Flag to apply erratum 1275112 workaround during reset. This erratum applies
 # only to revision <= r3p0 of the Neoverse N1 cpu.
-ERRATA_N1_1275112	?=0
+CPU_FLAG_LIST += ERRATA_N1_1275112
 
 # Flag to apply erratum 1315703 workaround during reset. This erratum applies
 # to revisions before r3p1 of the Neoverse N1 cpu.
-ERRATA_N1_1315703	?=0
+CPU_FLAG_LIST += ERRATA_N1_1315703
 
 # Flag to apply erratum 1542419 workaround during reset. This erratum applies
 # to revisions r3p0 - r4p0 of the Neoverse N1 cpu.
-ERRATA_N1_1542419	?=0
+CPU_FLAG_LIST += ERRATA_N1_1542419
 
 # Flag to apply erratum 1868343 workaround during reset. This erratum applies
 # to revision <= r4p0 of the Neoverse N1 cpu.
-ERRATA_N1_1868343	?=0
+CPU_FLAG_LIST += ERRATA_N1_1868343
 
 # Flag to apply erratum 1946160 workaround during reset. This erratum applies
 # to revisions r3p0, r3p1, r4p0, and r4p1 of the Neoverse N1 cpu. The issue
 # exists in revisions r0p0, r1p0, and r2p0 as well but there is no workaround.
-ERRATA_N1_1946160	?=0
+CPU_FLAG_LIST += ERRATA_N1_1946160
 
 # Flag to apply erratum 2743102 workaround during powerdown. This erratum
 # applies to all revisions <= r4p1 of the Neoverse N1 cpu and is still open.
-ERRATA_N1_2743102	?=0
+CPU_FLAG_LIST += ERRATA_N1_2743102
 
 # Flag to apply erratum 1618635 workaround during reset. This erratum applies
 # to revision r0p0 of the Neoverse V1 cpu and was fixed in the revision r1p0.
-ERRATA_V1_1618635	?=0
+CPU_FLAG_LIST += ERRATA_V1_1618635
 
 # Flag to apply erratum 1774420 workaround during reset. This erratum applies
 # to revisions r0p0 and r1p0 of the Neoverse V1 core, and was fixed in r1p1.
-ERRATA_V1_1774420	?=0
+CPU_FLAG_LIST += ERRATA_V1_1774420
 
 # Flag to apply erratum 1791573 workaround during reset. This erratum applies
 # to revisions r0p0 and r1p0 of the Neoverse V1 core, and was fixed in r1p1.
-ERRATA_V1_1791573	?=0
+CPU_FLAG_LIST += ERRATA_V1_1791573
 
 # Flag to apply erratum 1852267 workaround during reset. This erratum applies
 # to revisions r0p0 and r1p0 of the Neoverse V1 core, and was fixed in r1p1.
-ERRATA_V1_1852267	?=0
+CPU_FLAG_LIST += ERRATA_V1_1852267
 
 # Flag to apply erratum 1925756 workaround during reset. This needs to be
 # enabled for r0p0, r1p0, and r1p1 of the Neoverse V1 core, it is still open.
-ERRATA_V1_1925756	?=0
+CPU_FLAG_LIST += ERRATA_V1_1925756
 
 # Flag to apply erratum 1940577 workaround during reset. This erratum applies
 # to revisions r1p0 and r1p1 of the Neoverse V1 cpu.
-ERRATA_V1_1940577	?=0
+CPU_FLAG_LIST += ERRATA_V1_1940577
 
 # Flag to apply erratum 1966096 workaround during reset. This erratum applies
 # to revisions r1p0 and r1p1 of the Neoverse V1 CPU and is open. This issue
 # exists in r0p0 as well but there is no workaround for that revision.
-ERRATA_V1_1966096	?=0
+CPU_FLAG_LIST += ERRATA_V1_1966096
 
 # Flag to apply erratum 2139242 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0, and r1p1 of the Neoverse V1 cpu and is still open.
-ERRATA_V1_2139242	?=0
+CPU_FLAG_LIST += ERRATA_V1_2139242
 
 # Flag to apply erratum 2108267 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0, and r1p1 of the Neoverse V1 cpu and is still open.
-ERRATA_V1_2108267	?=0
+CPU_FLAG_LIST += ERRATA_V1_2108267
 
 # Flag to apply erratum 2216392 workaround during reset. This erratum applies
 # to revisions r1p0 and r1p1 of the Neoverse V1 cpu and is still open. This
 # issue exists in r0p0 as well but there is no workaround for that revision.
-ERRATA_V1_2216392	?=0
+CPU_FLAG_LIST += ERRATA_V1_2216392
 
 # Flag to apply erratum 2294912 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0, and r1p1 of the Neoverse V1 cpu and is still open.
-ERRATA_V1_2294912	?=0
+CPU_FLAG_LIST += ERRATA_V1_2294912
 
 # Flag to apply erratum 2372203 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0 and r1p1 of the Neoverse V1 cpu and is still open.
-ERRATA_V1_2372203	?=0
+CPU_FLAG_LIST += ERRATA_V1_2372203
 
 # Flag to apply erratum 2743093 workaround during powerdown. This erratum
 # applies to revisions r0p0, r1p0, r1p1 and r1p2  of the Neoverse V1 cpu and is
 # still open.
-ERRATA_V1_2743093	?=0
+CPU_FLAG_LIST += ERRATA_V1_2743093
 
 # Flag to apply erratum 2743233 workaround during powerdown. This erratum
 # applies to revisions r0p0, r1p0, r1p1 and r1p2 of the Neoverse V1 cpu and is
 # still open.
-ERRATA_V1_2743233	?=0
+CPU_FLAG_LIST += ERRATA_V1_2743233
 
 # Flag to apply erratum 2779461 workaround during powerdown. This erratum
 # applies to revisions r0p0, r1p0, r1p1 and r1p2  of the Neoverse V1 cpu and is
 # still open.
-ERRATA_V1_2779461       ?=0
+CPU_FLAG_LIST += ERRATA_V1_2779461
 
 # Flag to apply erratum 1987031 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is still open.
-ERRATA_A710_1987031	?=0
+CPU_FLAG_LIST += ERRATA_A710_1987031
 
 # Flag to apply erratum 2081180 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is still open.
-ERRATA_A710_2081180	?=0
+CPU_FLAG_LIST += ERRATA_A710_2081180
 
 # Flag to apply erratum 2083908 workaround during reset. This erratum applies
 # to revision r2p0 of the Cortex-A710 cpu and is still open.
-ERRATA_A710_2083908	?=0
+CPU_FLAG_LIST += ERRATA_A710_2083908
 
 # Flag to apply erratum 2058056 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is still open.
-ERRATA_A710_2058056	?=0
+CPU_FLAG_LIST += ERRATA_A710_2058056
 
 # Flag to apply erratum 2055002 workaround during reset. This erratum applies
 # to revision r1p0, r2p0 of the Cortex-A710 cpu and is still open.
-ERRATA_A710_2055002	?=0
+CPU_FLAG_LIST += ERRATA_A710_2055002
 
 # Flag to apply erratum 2017096 workaround during reset. This erratum applies
 # to revision r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is still open.
-ERRATA_A710_2017096	?=0
+CPU_FLAG_LIST += ERRATA_A710_2017096
 
 # Flag to apply erratum 2267065 workaround during reset. This erratum applies
 # to revision r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is fixed in r2p1.
-ERRATA_A710_2267065	?=0
+CPU_FLAG_LIST += ERRATA_A710_2267065
 
 # Flag to apply erratum 2136059 workaround during reset. This erratum applies
 # to revision r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is fixed in r2p1.
-ERRATA_A710_2136059	?=0
+CPU_FLAG_LIST += ERRATA_A710_2136059
 
 # Flag to apply erratum 2147715 workaround during reset. This erratum applies
 # to revision r2p0 of the Cortex-A710 CPU and is fixed in revision r2p1.
-ERRATA_A710_2147715	?=0
+CPU_FLAG_LIST += ERRATA_A710_2147715
 
 # Flag to apply erratum 2216384 workaround during reset. This erratum applies
 # to revision r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is fixed in r2p1.
-ERRATA_A710_2216384	?=0
+CPU_FLAG_LIST += ERRATA_A710_2216384
 
 # Flag to apply erratum 2282622 workaround during reset. This erratum applies
 # to revision r0p0, r1p0, r2p0 and r2p1 of the Cortex-A710 cpu and is still
 # open.
-ERRATA_A710_2282622	?=0
+CPU_FLAG_LIST += ERRATA_A710_2282622
 
 # Flag to apply erratum 2291219 workaround during reset. This erratum applies
 # to revision r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is fixed in r2p1.
-ERRATA_A710_2291219	?=0
+CPU_FLAG_LIST += ERRATA_A710_2291219
 
 # Flag to apply erratum 2008768 workaround during reset. This erratum applies
 # to revision r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is fixed in r2p1.
-ERRATA_A710_2008768	?=0
+CPU_FLAG_LIST += ERRATA_A710_2008768
 
 # Flag to apply erratum 2371105 workaround during reset. This erratum applies
 # to revision r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is fixed in r2p1.
-ERRATA_A710_2371105	?=0
+CPU_FLAG_LIST += ERRATA_A710_2371105
 
 # Flag to apply erratum 2768515 workaround during power down. This erratum
 # applies to revision r0p0, r1p0, r2p0 and r2p1 of the Cortex-A710 cpu and is
 # still open.
-ERRATA_A710_2768515	?=0
+CPU_FLAG_LIST += ERRATA_A710_2768515
 
 # Flag to apply erratum 2002655 workaround during reset. This erratum applies
 # to revisions r0p0 of the Neoverse-N2 cpu, it is still open.
-ERRATA_N2_2002655	?=0
+CPU_FLAG_LIST += ERRATA_N2_2002655
 
 # Flag to apply erratum 2067956 workaround during reset. This erratum applies
 # to revision r0p0 of the Neoverse N2 cpu and is still open.
-ERRATA_N2_2067956	?=0
+CPU_FLAG_LIST += ERRATA_N2_2067956
 
 # Flag to apply erratum 2025414 workaround during reset. This erratum applies
 # to revision r0p0 of the Neoverse N2 cpu and is still open.
-ERRATA_N2_2025414	?=0
+CPU_FLAG_LIST += ERRATA_N2_2025414
 
 # Flag to apply erratum 2189731 workaround during reset. This erratum applies
 # to revision r0p0 of the Neoverse N2 cpu and is still open.
-ERRATA_N2_2189731	?=0
+CPU_FLAG_LIST += ERRATA_N2_2189731
 
 # Flag to apply erratum 2138956 workaround during reset. This erratum applies
 # to revision r0p0 of the Neoverse N2 cpu and is still open.
-ERRATA_N2_2138956	?=0
+CPU_FLAG_LIST += ERRATA_N2_2138956
 
 # Flag to apply erratum 2138953 workaround during reset. This erratum applies
 # to revision r0p0 of the Neoverse N2 cpu and is still open.
-ERRATA_N2_2138953	?=0
+CPU_FLAG_LIST += ERRATA_N2_2138953
 
 # Flag to apply erratum 2242415 workaround during reset. This erratum applies
 # to revision r0p0 of the Neoverse N2 cpu and is still open.
-ERRATA_N2_2242415	?=0
+CPU_FLAG_LIST += ERRATA_N2_2242415
 
 # Flag to apply erratum 2138958 workaround during reset. This erratum applies
 # to revision r0p0 of the Neoverse N2 cpu and is still open.
-ERRATA_N2_2138958	?=0
+CPU_FLAG_LIST += ERRATA_N2_2138958
 
 # Flag to apply erratum 2242400 workaround during reset. This erratum applies
 # to revision r0p0 of the Neoverse N2 cpu and is still open.
-ERRATA_N2_2242400	?=0
+CPU_FLAG_LIST += ERRATA_N2_2242400
 
 # Flag to apply erratum 2280757 workaround during reset. This erratum applies
 # to revision r0p0 of the Neoverse N2 cpu and is still open.
-ERRATA_N2_2280757	?=0
+CPU_FLAG_LIST += ERRATA_N2_2280757
 
 # Flag to apply erraturm 2326639 workaroud during powerdown. This erratum
 # applies to revision r0p0 of the Neoverse N2 cpu and is fixed in r0p1.
-ERRATA_N2_2326639	?=0
+CPU_FLAG_LIST += ERRATA_N2_2326639
 
 # Flag to apply erratum 2376738 workaround during reset. This erratum applies
 # to revision r0p0 of the Neoverse N2 cpu, it is fixed in r0p1.
-ERRATA_N2_2376738	?=0
+CPU_FLAG_LIST += ERRATA_N2_2376738
 
 # Flag to apply erratum 2388450 workaround during reset. This erratum applies
 # to revision r0p0 of the Neoverse N2 cpu, it is fixed in r0p1.
-ERRATA_N2_2388450	?=0
+CPU_FLAG_LIST += ERRATA_N2_2388450
 
 # Flag to apply erratum 2743089 workaround during during powerdown. This erratum
 # applies to all revisions <= r0p2 of the Neoverse N2 cpu, it is fixed in r0p3.
-ERRATA_N2_2743089	?=0
+CPU_FLAG_LIST += ERRATA_N2_2743089
 
 # Flag to apply erratum 2002765 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0, and r2p0 of the Cortex-X2 cpu and is still open.
-ERRATA_X2_2002765	?=0
+CPU_FLAG_LIST += ERRATA_X2_2002765
 
 # Flag to apply erratum 2058056 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0, and r2p0 of the Cortex-X2 cpu and is still open.
-ERRATA_X2_2058056	?=0
+CPU_FLAG_LIST += ERRATA_X2_2058056
 
 # Flag to apply erratum 2083908 workaround during reset. This erratum applies
 # to revision r2p0 of the Cortex-X2 cpu and is still open.
-ERRATA_X2_2083908	?=0
+CPU_FLAG_LIST += ERRATA_X2_2083908
 
 # Flag to apply erratum 2017096 workaround during reset. This erratum applies
 # only to revisions r0p0, r1p0 and r2p0 of the Cortex-X2 cpu, it is fixed in
 # r2p1.
-ERRATA_X2_2017096	?=0
+CPU_FLAG_LIST += ERRATA_X2_2017096
 
 # Flag to apply erratum 2081180 workaround during reset. This erratum applies
 # only to revisions r0p0, r1p0 and r2p0 of the Cortex-X2 cpu, it is fixed in
 # r2p1.
-ERRATA_X2_2081180	?=0
+CPU_FLAG_LIST += ERRATA_X2_2081180
 
 # Flag to apply erratum 2216384 workaround during reset. This erratum applies
 # only to revisions r0p0, r1p0 and r2p0 of the Cortex-X2 cpu, it is fixed in
 # r2p1.
-ERRATA_X2_2216384	?=0
+CPU_FLAG_LIST += ERRATA_X2_2216384
 
 # Flag to apply erratum 2147715 workaround during reset. This erratum applies
 # only to revision r2p0 of the Cortex-X2 cpu, it is fixed in r2p1.
-ERRATA_X2_2147715	?=0
+CPU_FLAG_LIST += ERRATA_X2_2147715
 
 # Flag to apply erratum 2282622 workaround during reset. This erratum applies
 # to revision r0p0, r1p0, r2p0 and r2p1 of the Cortex-X2 cpu and is still
 # open.
-ERRATA_X2_2282622	?=0
+CPU_FLAG_LIST += ERRATA_X2_2282622
 
 # Flag to apply erratum 2371105 workaround during reset. This erratum applies
 # to revision r0p0, r1p0 and r2p0 of the Cortex-X2 cpu and is fixed in r2p1.
-ERRATA_X2_2371105	?=0
+CPU_FLAG_LIST += ERRATA_X2_2371105
 
 # Flag to apply erratum 2768515 workaround during power down. This erratum
 # applies to revision r0p0, r1p0, r2p0 and r2p1 of the Cortex-X2 cpu and is
 # still open.
-ERRATA_X2_2768515	?=0
+CPU_FLAG_LIST += ERRATA_X2_2768515
 
 # Flag to apply erratum 2313909 workaround on powerdown. This erratum applies
 # to revisions r0p0 and r1p0 of the Cortex-X3 cpu, it is fixed in r1p1.
-ERRATA_X3_2313909	?=0
+CPU_FLAG_LIST += ERRATA_X3_2313909
 
 # Flag to apply erratum 2615812 workaround on powerdown. This erratum applies
 # to revisions r0p0, r1p0, r1p1 of the Cortex-X3 cpu, it is still open.
-ERRATA_X3_2615812	?=0
+CPU_FLAG_LIST += ERRATA_X3_2615812
 
 # Flag to apply erratum 1922240 workaround during reset. This erratum applies
 # to revision r0p0 of the Cortex-A510 cpu and is fixed in r0p1.
-ERRATA_A510_1922240	?=0
+CPU_FLAG_LIST += ERRATA_A510_1922240
 
 # Flag to apply erratum 2288014 workaround during reset. This erratum applies
 # to revisions r0p0, r0p1, r0p2, r0p3 and r1p0 of the Cortex-A510 cpu and is
 # fixed in r1p1.
-ERRATA_A510_2288014	?=0
+CPU_FLAG_LIST += ERRATA_A510_2288014
 
 # Flag to apply erratum 2042739 workaround during reset. This erratum applies
 # to revisions r0p0, r0p1 and r0p2 of the Cortex-A510 cpu and is fixed in r0p3.
-ERRATA_A510_2042739	?=0
+CPU_FLAG_LIST += ERRATA_A510_2042739
 
 # Flag to apply erratum 2041909 workaround during reset. This erratum applies
 # to revision r0p2 of the Cortex-A510 cpu and is fixed in r0p3. The issue is
 # present in r0p0 and r0p1 but there is no workaround for those revisions.
-ERRATA_A510_2041909	?=0
+CPU_FLAG_LIST += ERRATA_A510_2041909
 
 # Flag to apply erratum 2250311 workaround during reset. This erratum applies
 # to revisions r0p0, r0p1, r0p2, r0p3 and r1p0, and is fixed in r1p1.
-ERRATA_A510_2250311	?=0
+CPU_FLAG_LIST += ERRATA_A510_2250311
 
 # Flag to apply erratum 2218950 workaround during reset. This erratum applies
 # to revisions r0p0, r0p1, r0p2, r0p3 and r1p0, and is fixed in r1p1.
-ERRATA_A510_2218950	?=0
+CPU_FLAG_LIST += ERRATA_A510_2218950
 
 # Flag to apply erratum 2172148 workaround during reset. This erratum applies
 # to revisions r0p0, r0p1, r0p2, r0p3 and r1p0, and is fixed in r1p1.
-ERRATA_A510_2172148	?=0
+CPU_FLAG_LIST += ERRATA_A510_2172148
 
 # Flag to apply erratum 2347730 workaround during reset. This erratum applies
 # to revisions r0p0, r0p1, r0p2, r0p3, r1p0 and r1p1 of the Cortex-A510 CPU,
 # and is fixed in r1p2.
-ERRATA_A510_2347730	?=0
+CPU_FLAG_LIST += ERRATA_A510_2347730
 
 # Flag to apply erratum 2371937 workaround during reset. This erratum applies
 # to revisions r0p0, r0p1, r0p2, r0p3, r1p0, and r1p1. It is fixed in r1p2.
-ERRATA_A510_2371937	?=0
+CPU_FLAG_LIST += ERRATA_A510_2371937
 
 # Flag to apply erratum 2666669 workaround during reset. This erratum applies
 # to revisions r0p0, r0p1, r0p2, r0p3, r1p0, and r1p1. It is fixed in r1p2.
-ERRATA_A510_2666669	?=0
+CPU_FLAG_LIST += ERRATA_A510_2666669
 
 # Flag to apply erratum 2684597 workaround during powerdown. This erratum
 # applies to revision r0p0, r0p1, r0p2, r0p3, r1p0, r1p1 and r1p2 of the
 # Cortex-A510 cpu and is fixed in r1p3.
-ERRATA_A510_2684597	?=0
+CPU_FLAG_LIST += ERRATA_A510_2684597
 
 # Flag to apply DSU erratum 798953. This erratum applies to DSUs revision r0p0.
 # Applying the workaround results in higher DSU power consumption on idle.
-ERRATA_DSU_798953	?=0
+CPU_FLAG_LIST += ERRATA_DSU_798953
 
 # Flag to apply DSU erratum 936184. This erratum applies to DSUs containing
 # the ACP interface and revision < r2p0. Applying the workaround results in
 # higher DSU power consumption on idle.
-ERRATA_DSU_936184	?=0
+CPU_FLAG_LIST += ERRATA_DSU_936184
 
 # Flag to apply DSU erratum 2313941. This erratum applies to DSUs revisions
 # r0p0, r1p0, r2p0, r2p1, r3p0, r3p1 and is still open. Applying the workaround
 # results in higher DSU power consumption on idle.
-ERRATA_DSU_2313941	?=0
-
-# Process ERRATA_A9_794073 flag
-$(eval $(call assert_boolean,ERRATA_A9_794073))
-$(eval $(call add_define,ERRATA_A9_794073))
-
-# Process ERRATA_A15_816470 flag
-$(eval $(call assert_boolean,ERRATA_A15_816470))
-$(eval $(call add_define,ERRATA_A15_816470))
-
-# Process ERRATA_A15_827671 flag
-$(eval $(call assert_boolean,ERRATA_A15_827671))
-$(eval $(call add_define,ERRATA_A15_827671))
-
-# Process ERRATA_A17_852421 flag
-$(eval $(call assert_boolean,ERRATA_A17_852421))
-$(eval $(call add_define,ERRATA_A17_852421))
-
-# Process ERRATA_A17_852423 flag
-$(eval $(call assert_boolean,ERRATA_A17_852423))
-$(eval $(call add_define,ERRATA_A17_852423))
-
-# Process ERRATA_A35_855472 flag
-$(eval $(call assert_boolean,ERRATA_A35_855472))
-$(eval $(call add_define,ERRATA_A35_855472))
-
-# Process ERRATA_A53_819472 flag
-$(eval $(call assert_boolean,ERRATA_A53_819472))
-$(eval $(call add_define,ERRATA_A53_819472))
-
-# Process ERRATA_A53_824069 flag
-$(eval $(call assert_boolean,ERRATA_A53_824069))
-$(eval $(call add_define,ERRATA_A53_824069))
-
-# Process ERRATA_A53_826319 flag
-$(eval $(call assert_boolean,ERRATA_A53_826319))
-$(eval $(call add_define,ERRATA_A53_826319))
-
-# Process ERRATA_A53_827319 flag
-$(eval $(call assert_boolean,ERRATA_A53_827319))
-$(eval $(call add_define,ERRATA_A53_827319))
-
-# Process ERRATA_A53_835769 flag
-$(eval $(call assert_boolean,ERRATA_A53_835769))
-$(eval $(call add_define,ERRATA_A53_835769))
-
-# Process ERRATA_A53_836870 flag
-$(eval $(call assert_boolean,ERRATA_A53_836870))
-$(eval $(call add_define,ERRATA_A53_836870))
-
-# Process ERRATA_A53_843419 flag
-$(eval $(call assert_boolean,ERRATA_A53_843419))
-$(eval $(call add_define,ERRATA_A53_843419))
-
-# Process ERRATA_A53_855873 flag
-$(eval $(call assert_boolean,ERRATA_A53_855873))
-$(eval $(call add_define,ERRATA_A53_855873))
-
-# Process ERRATA_A53_1530924 flag
-$(eval $(call assert_boolean,ERRATA_A53_1530924))
-$(eval $(call add_define,ERRATA_A53_1530924))
-
-# Process ERRATA_A55_768277 flag
-$(eval $(call assert_boolean,ERRATA_A55_768277))
-$(eval $(call add_define,ERRATA_A55_768277))
-
-# Process ERRATA_A55_778703 flag
-$(eval $(call assert_boolean,ERRATA_A55_778703))
-$(eval $(call add_define,ERRATA_A55_778703))
-
-# Process ERRATA_A55_798797 flag
-$(eval $(call assert_boolean,ERRATA_A55_798797))
-$(eval $(call add_define,ERRATA_A55_798797))
-
-# Process ERRATA_A55_846532 flag
-$(eval $(call assert_boolean,ERRATA_A55_846532))
-$(eval $(call add_define,ERRATA_A55_846532))
-
-# Process ERRATA_A55_903758 flag
-$(eval $(call assert_boolean,ERRATA_A55_903758))
-$(eval $(call add_define,ERRATA_A55_903758))
-
-# Process ERRATA_A55_1221012 flag
-$(eval $(call assert_boolean,ERRATA_A55_1221012))
-$(eval $(call add_define,ERRATA_A55_1221012))
-
-# Process ERRATA_A55_1530923 flag
-$(eval $(call assert_boolean,ERRATA_A55_1530923))
-$(eval $(call add_define,ERRATA_A55_1530923))
-
-# Process ERRATA_A57_806969 flag
-$(eval $(call assert_boolean,ERRATA_A57_806969))
-$(eval $(call add_define,ERRATA_A57_806969))
-
-# Process ERRATA_A57_813419 flag
-$(eval $(call assert_boolean,ERRATA_A57_813419))
-$(eval $(call add_define,ERRATA_A57_813419))
-
-# Process ERRATA_A57_813420 flag
-$(eval $(call assert_boolean,ERRATA_A57_813420))
-$(eval $(call add_define,ERRATA_A57_813420))
-
-# Process ERRATA_A57_814670 flag
-$(eval $(call assert_boolean,ERRATA_A57_814670))
-$(eval $(call add_define,ERRATA_A57_814670))
-
-# Process ERRATA_A57_817169 flag
-$(eval $(call assert_boolean,ERRATA_A57_817169))
-$(eval $(call add_define,ERRATA_A57_817169))
-
-# Process ERRATA_A57_826974 flag
-$(eval $(call assert_boolean,ERRATA_A57_826974))
-$(eval $(call add_define,ERRATA_A57_826974))
-
-# Process ERRATA_A57_826977 flag
-$(eval $(call assert_boolean,ERRATA_A57_826977))
-$(eval $(call add_define,ERRATA_A57_826977))
-
-# Process ERRATA_A57_828024 flag
-$(eval $(call assert_boolean,ERRATA_A57_828024))
-$(eval $(call add_define,ERRATA_A57_828024))
+CPU_FLAG_LIST += ERRATA_DSU_2313941
 
-# Process ERRATA_A57_829520 flag
-$(eval $(call assert_boolean,ERRATA_A57_829520))
-$(eval $(call add_define,ERRATA_A57_829520))
-
-# Process ERRATA_A57_833471 flag
-$(eval $(call assert_boolean,ERRATA_A57_833471))
-$(eval $(call add_define,ERRATA_A57_833471))
-
-# Process ERRATA_A57_859972 flag
-$(eval $(call assert_boolean,ERRATA_A57_859972))
-$(eval $(call add_define,ERRATA_A57_859972))
-
-# Process ERRATA_A57_1319537 flag
-$(eval $(call assert_boolean,ERRATA_A57_1319537))
-$(eval $(call add_define,ERRATA_A57_1319537))
-
-# Process ERRATA_A72_859971 flag
-$(eval $(call assert_boolean,ERRATA_A72_859971))
-$(eval $(call add_define,ERRATA_A72_859971))
-
-# Process ERRATA_A72_1319367 flag
-$(eval $(call assert_boolean,ERRATA_A72_1319367))
-$(eval $(call add_define,ERRATA_A72_1319367))
-
-# Process ERRATA_A73_852427 flag
-$(eval $(call assert_boolean,ERRATA_A73_852427))
-$(eval $(call add_define,ERRATA_A73_852427))
-
-# Process ERRATA_A73_855423 flag
-$(eval $(call assert_boolean,ERRATA_A73_855423))
-$(eval $(call add_define,ERRATA_A73_855423))
-
-# Process ERRATA_A75_764081 flag
-$(eval $(call assert_boolean,ERRATA_A75_764081))
-$(eval $(call add_define,ERRATA_A75_764081))
-
-# Process ERRATA_A75_790748 flag
-$(eval $(call assert_boolean,ERRATA_A75_790748))
-$(eval $(call add_define,ERRATA_A75_790748))
-
-# Process ERRATA_A76_1073348 flag
-$(eval $(call assert_boolean,ERRATA_A76_1073348))
-$(eval $(call add_define,ERRATA_A76_1073348))
-
-# Process ERRATA_A76_1130799 flag
-$(eval $(call assert_boolean,ERRATA_A76_1130799))
-$(eval $(call add_define,ERRATA_A76_1130799))
-
-# Process ERRATA_A76_1220197 flag
-$(eval $(call assert_boolean,ERRATA_A76_1220197))
-$(eval $(call add_define,ERRATA_A76_1220197))
-
-# Process ERRATA_A76_1257314 flag
-$(eval $(call assert_boolean,ERRATA_A76_1257314))
-$(eval $(call add_define,ERRATA_A76_1257314))
-
-# Process ERRATA_A76_1262606 flag
-$(eval $(call assert_boolean,ERRATA_A76_1262606))
-$(eval $(call add_define,ERRATA_A76_1262606))
-
-# Process ERRATA_A76_1262888 flag
-$(eval $(call assert_boolean,ERRATA_A76_1262888))
-$(eval $(call add_define,ERRATA_A76_1262888))
-
-# Process ERRATA_A76_1275112 flag
-$(eval $(call assert_boolean,ERRATA_A76_1275112))
-$(eval $(call add_define,ERRATA_A76_1275112))
-
-# Process ERRATA_A76_1286807 flag
-$(eval $(call assert_boolean,ERRATA_A76_1286807))
-$(eval $(call add_define,ERRATA_A76_1286807))
-
-# Process ERRATA_A76_1791580 flag
-$(eval $(call assert_boolean,ERRATA_A76_1791580))
-$(eval $(call add_define,ERRATA_A76_1791580))
-
-# Process ERRATA_A76_1165522 flag
-$(eval $(call assert_boolean,ERRATA_A76_1165522))
-$(eval $(call add_define,ERRATA_A76_1165522))
-
-# Process ERRATA_A76_1868343 flag
-$(eval $(call assert_boolean,ERRATA_A76_1868343))
-$(eval $(call add_define,ERRATA_A76_1868343))
-
-# Process ERRATA_A76_1946160 flag
-$(eval $(call assert_boolean,ERRATA_A76_1946160))
-$(eval $(call add_define,ERRATA_A76_1946160))
-
-# Process ERRATA_A76_2743102 flag
-$(eval $(call assert_boolean,ERRATA_A76_2743102))
-$(eval $(call add_define,ERRATA_A76_2743102))
-
-# Process ERRATA_A77_1508412 flag
-$(eval $(call assert_boolean,ERRATA_A77_1508412))
-$(eval $(call add_define,ERRATA_A77_1508412))
-
-# Process ERRATA_A77_1925769 flag
-$(eval $(call assert_boolean,ERRATA_A77_1925769))
-$(eval $(call add_define,ERRATA_A77_1925769))
-
-# Process ERRATA_A77_1946167 flag
-$(eval $(call assert_boolean,ERRATA_A77_1946167))
-$(eval $(call add_define,ERRATA_A77_1946167))
-
-# Process ERRATA_A77_1791578 flag
-$(eval $(call assert_boolean,ERRATA_A77_1791578))
-$(eval $(call add_define,ERRATA_A77_1791578))
-
-# Process ERRATA_A77_2356587 flag
-$(eval $(call assert_boolean,ERRATA_A77_2356587))
-$(eval $(call add_define,ERRATA_A77_2356587))
-
-# Process ERRATA_A77_1800714 flag
-$(eval $(call assert_boolean,ERRATA_A77_1800714))
-$(eval $(call add_define,ERRATA_A77_1800714))
-
-# Process ERRATA_A77_2743100 flag
-$(eval $(call assert_boolean,ERRATA_A77_2743100))
-$(eval $(call add_define,ERRATA_A77_2743100))
-
-# Process ERRATA_A78_1688305 flag
-$(eval $(call assert_boolean,ERRATA_A78_1688305))
-$(eval $(call add_define,ERRATA_A78_1688305))
-
-# Process ERRATA_A78_1941498 flag
-$(eval $(call assert_boolean,ERRATA_A78_1941498))
-$(eval $(call add_define,ERRATA_A78_1941498))
-
-# Process ERRATA_A78_1951500 flag
-$(eval $(call assert_boolean,ERRATA_A78_1951500))
-$(eval $(call add_define,ERRATA_A78_1951500))
-
-# Process ERRATA_A78_1821534 flag
-$(eval $(call assert_boolean,ERRATA_A78_1821534))
-$(eval $(call add_define,ERRATA_A78_1821534))
-
-# Process ERRATA_A78_1952683 flag
-$(eval $(call assert_boolean,ERRATA_A78_1952683))
-$(eval $(call add_define,ERRATA_A78_1952683))
-
-# Process ERRATA_A78_2132060 flag
-$(eval $(call assert_boolean,ERRATA_A78_2132060))
-$(eval $(call add_define,ERRATA_A78_2132060))
-
-# Process ERRATA_A78_2242635 flag
-$(eval $(call assert_boolean,ERRATA_A78_2242635))
-$(eval $(call add_define,ERRATA_A78_2242635))
-
-# Process ERRATA_A78_2376745 flag
-$(eval $(call assert_boolean,ERRATA_A78_2376745))
-$(eval $(call add_define,ERRATA_A78_2376745))
-
-# Process ERRATA_A78_2395406 flag
-$(eval $(call assert_boolean,ERRATA_A78_2395406))
-$(eval $(call add_define,ERRATA_A78_2395406))
-
-# Process ERRATA_A78_2742426 flag
-$(eval $(call assert_boolean,ERRATA_A78_2742426))
-$(eval $(call add_define,ERRATA_A78_2742426))
-
-# Process ERRATA_A78_2772019 flag
-$(eval $(call assert_boolean,ERRATA_A78_2772019))
-$(eval $(call add_define,ERRATA_A78_2772019))
-
-# Process ERRATA_A78_2779479 flag
-$(eval $(call assert_boolean,ERRATA_A78_2779479))
-$(eval $(call add_define,ERRATA_A78_2779479))
-
-# Process ERRATA_A78_AE_1941500 flag
-$(eval $(call assert_boolean,ERRATA_A78_AE_1941500))
-$(eval $(call add_define,ERRATA_A78_AE_1941500))
-
-# Process ERRATA_A78_AE_1951502 flag
-$(eval $(call assert_boolean,ERRATA_A78_AE_1951502))
-$(eval $(call add_define,ERRATA_A78_AE_1951502))
-
-# Process ERRATA_A78_AE_2376748 flag
-$(eval $(call assert_boolean,ERRATA_A78_AE_2376748))
-$(eval $(call add_define,ERRATA_A78_AE_2376748))
-
-# Process ERRATA_A78_AE_2395408 flag
-$(eval $(call assert_boolean,ERRATA_A78_AE_2395408))
-$(eval $(call add_define,ERRATA_A78_AE_2395408))
-
-# Process ERRATA_A78C_2132064 flag
-$(eval $(call assert_boolean,ERRATA_A78C_2132064))
-$(eval $(call add_define,ERRATA_A78C_2132064))
-
-# Process ERRATA_A78C_2242638 flag
-$(eval $(call assert_boolean,ERRATA_A78C_2242638))
-$(eval $(call add_define,ERRATA_A78C_2242638))
-
-# Process ERRATA_A78C_2376749 flag
-$(eval $(call assert_boolean,ERRATA_A78C_2376749))
-$(eval $(call add_define,ERRATA_A78C_2376749))
-
-# Process ERRATA_A78C_2395411 flag
-$(eval $(call assert_boolean,ERRATA_A78C_2395411))
-$(eval $(call add_define,ERRATA_A78C_2395411))
-
-# Process ERRATA_A78C_2772121 flag
-$(eval $(call assert_boolean,ERRATA_A78C_2772121))
-$(eval $(call add_define,ERRATA_A78C_2772121))
-
-# Process ERRATA_A78C_2779484 flag
-$(eval $(call assert_boolean,ERRATA_A78C_2779484))
-$(eval $(call add_define,ERRATA_A78C_2779484))
-
-# Process ERRATA_X1_1821534 flag
-$(eval $(call assert_boolean,ERRATA_X1_1821534))
-$(eval $(call add_define,ERRATA_X1_1821534))
-
-# Process ERRATA_X1_1688305 flag
-$(eval $(call assert_boolean,ERRATA_X1_1688305))
-$(eval $(call add_define,ERRATA_X1_1688305))
-
-# Process ERRATA_X1_1827429 flag
-$(eval $(call assert_boolean,ERRATA_X1_1827429))
-$(eval $(call add_define,ERRATA_X1_1827429))
-
-# Process ERRATA_N1_1043202 flag
-$(eval $(call assert_boolean,ERRATA_N1_1043202))
-$(eval $(call add_define,ERRATA_N1_1043202))
-
-# Process ERRATA_N1_1073348 flag
-$(eval $(call assert_boolean,ERRATA_N1_1073348))
-$(eval $(call add_define,ERRATA_N1_1073348))
-
-# Process ERRATA_N1_1130799 flag
-$(eval $(call assert_boolean,ERRATA_N1_1130799))
-$(eval $(call add_define,ERRATA_N1_1130799))
-
-# Process ERRATA_N1_1165347 flag
-$(eval $(call assert_boolean,ERRATA_N1_1165347))
-$(eval $(call add_define,ERRATA_N1_1165347))
-
-# Process ERRATA_N1_1207823 flag
-$(eval $(call assert_boolean,ERRATA_N1_1207823))
-$(eval $(call add_define,ERRATA_N1_1207823))
-
-# Process ERRATA_N1_1220197 flag
-$(eval $(call assert_boolean,ERRATA_N1_1220197))
-$(eval $(call add_define,ERRATA_N1_1220197))
-
-# Process ERRATA_N1_1257314 flag
-$(eval $(call assert_boolean,ERRATA_N1_1257314))
-$(eval $(call add_define,ERRATA_N1_1257314))
-
-# Process ERRATA_N1_1262606 flag
-$(eval $(call assert_boolean,ERRATA_N1_1262606))
-$(eval $(call add_define,ERRATA_N1_1262606))
-
-# Process ERRATA_N1_1262888 flag
-$(eval $(call assert_boolean,ERRATA_N1_1262888))
-$(eval $(call add_define,ERRATA_N1_1262888))
-
-# Process ERRATA_N1_1275112 flag
-$(eval $(call assert_boolean,ERRATA_N1_1275112))
-$(eval $(call add_define,ERRATA_N1_1275112))
-
-# Process ERRATA_N1_1315703 flag
-$(eval $(call assert_boolean,ERRATA_N1_1315703))
-$(eval $(call add_define,ERRATA_N1_1315703))
-
-# Process ERRATA_N1_1542419 flag
-$(eval $(call assert_boolean,ERRATA_N1_1542419))
-$(eval $(call add_define,ERRATA_N1_1542419))
-
-# Process ERRATA_N1_1868343 flag
-$(eval $(call assert_boolean,ERRATA_N1_1868343))
-$(eval $(call add_define,ERRATA_N1_1868343))
-
-# Process ERRATA_N1_1946160 flag
-$(eval $(call assert_boolean,ERRATA_N1_1946160))
-$(eval $(call add_define,ERRATA_N1_1946160))
-
-# Process ERRATA_N1_2743102 flag
-$(eval $(call assert_boolean,ERRATA_N1_2743102))
-$(eval $(call add_define,ERRATA_N1_2743102))
-
-# Process ERRATA_V1_1618635 flag
-$(eval $(call assert_boolean,ERRATA_V1_1618635))
-$(eval $(call add_define,ERRATA_V1_1618635))
-
-# Process ERRATA_V1_1774420 flag
-$(eval $(call assert_boolean,ERRATA_V1_1774420))
-$(eval $(call add_define,ERRATA_V1_1774420))
-
-# Process ERRATA_V1_1791573 flag
-$(eval $(call assert_boolean,ERRATA_V1_1791573))
-$(eval $(call add_define,ERRATA_V1_1791573))
-
-# Process ERRATA_V1_1852267 flag
-$(eval $(call assert_boolean,ERRATA_V1_1852267))
-$(eval $(call add_define,ERRATA_V1_1852267))
-
-# Process ERRATA_V1_1925756 flag
-$(eval $(call assert_boolean,ERRATA_V1_1925756))
-$(eval $(call add_define,ERRATA_V1_1925756))
-
-# Process ERRATA_V1_1940577 flag
-$(eval $(call assert_boolean,ERRATA_V1_1940577))
-$(eval $(call add_define,ERRATA_V1_1940577))
-
-# Process ERRATA_V1_1966096 flag
-$(eval $(call assert_boolean,ERRATA_V1_1966096))
-$(eval $(call add_define,ERRATA_V1_1966096))
-
-# Process ERRATA_V1_2139242 flag
-$(eval $(call assert_boolean,ERRATA_V1_2139242))
-$(eval $(call add_define,ERRATA_V1_2139242))
-
-# Process ERRATA_V1_2108267 flag
-$(eval $(call assert_boolean,ERRATA_V1_2108267))
-$(eval $(call add_define,ERRATA_V1_2108267))
-
-# Process ERRATA_V1_2216392 flag
-$(eval $(call assert_boolean,ERRATA_V1_2216392))
-$(eval $(call add_define,ERRATA_V1_2216392))
-
-# Process ERRATA_V1_2294912 flag
-$(eval $(call assert_boolean,ERRATA_V1_2294912))
-$(eval $(call add_define,ERRATA_V1_2294912))
-
-# Process ERRATA_V1_2372203 flag
-$(eval $(call assert_boolean,ERRATA_V1_2372203))
-$(eval $(call add_define,ERRATA_V1_2372203))
-
-# Process ERRATA_V1_2743093 flag
-$(eval $(call assert_boolean,ERRATA_V1_2743093))
-$(eval $(call add_define,ERRATA_V1_2743093))
-
-# Process ERRATA_V1_2743233 flag
-$(eval $(call assert_boolean,ERRATA_V1_2743233))
-$(eval $(call add_define,ERRATA_V1_2743233))
-
-# Process ERRATA_V1_2779461 flag
-$(eval $(call assert_boolean,ERRATA_V1_2779461))
-$(eval $(call add_define,ERRATA_V1_2779461))
-
-# Process ERRATA_A710_1987031 flag
-$(eval $(call assert_boolean,ERRATA_A710_1987031))
-$(eval $(call add_define,ERRATA_A710_1987031))
-
-# Process ERRATA_A710_2081180 flag
-$(eval $(call assert_boolean,ERRATA_A710_2081180))
-$(eval $(call add_define,ERRATA_A710_2081180))
-
-# Process ERRATA_A710_2083908 flag
-$(eval $(call assert_boolean,ERRATA_A710_2083908))
-$(eval $(call add_define,ERRATA_A710_2083908))
-
-# Process ERRATA_A710_2058056 flag
-$(eval $(call assert_boolean,ERRATA_A710_2058056))
-$(eval $(call add_define,ERRATA_A710_2058056))
-
-# Process ERRATA_A710_2055002 flag
-$(eval $(call assert_boolean,ERRATA_A710_2055002))
-$(eval $(call add_define,ERRATA_A710_2055002))
-
-# Process ERRATA_A710_2017096 flag
-$(eval $(call assert_boolean,ERRATA_A710_2017096))
-$(eval $(call add_define,ERRATA_A710_2017096))
-
-# Process ERRATA_A710_2267065 flag
-$(eval $(call assert_boolean,ERRATA_A710_2267065))
-$(eval $(call add_define,ERRATA_A710_2267065))
-
-# Process ERRATA_A710_2136059 flag
-$(eval $(call assert_boolean,ERRATA_A710_2136059))
-$(eval $(call add_define,ERRATA_A710_2136059))
-
-# Process ERRATA_A710_2147715 flag
-$(eval $(call assert_boolean,ERRATA_A710_2147715))
-$(eval $(call add_define,ERRATA_A710_2147715))
-
-# Process ERRATA_A710_2216384 flag
-$(eval $(call assert_boolean,ERRATA_A710_2216384))
-$(eval $(call add_define,ERRATA_A710_2216384))
-
-# Process ERRATA_A710_2282622 flag
-$(eval $(call assert_boolean,ERRATA_A710_2282622))
-$(eval $(call add_define,ERRATA_A710_2282622))
-
-# Process ERRATA_A710_2291219 flag
-$(eval $(call assert_boolean,ERRATA_A710_2291219))
-$(eval $(call add_define,ERRATA_A710_2291219))
-
-# Process ERRATA_A710_2008768 flag
-$(eval $(call assert_boolean,ERRATA_A710_2008768))
-$(eval $(call add_define,ERRATA_A710_2008768))
-
-# Process ERRATA_A710_2371105 flag
-$(eval $(call assert_boolean,ERRATA_A710_2371105))
-$(eval $(call add_define,ERRATA_A710_2371105))
-
-# Process ERRATA_A710_2768515 flag
-$(eval $(call assert_boolean,ERRATA_A710_2768515))
-$(eval $(call add_define,ERRATA_A710_2768515))
-
-# Process ERRATA_N2_2002655 flag
-$(eval $(call assert_boolean,ERRATA_N2_2002655))
-$(eval $(call add_define,ERRATA_N2_2002655))
-
-# Process ERRATA_N2_2067956 flag
-$(eval $(call assert_boolean,ERRATA_N2_2067956))
-$(eval $(call add_define,ERRATA_N2_2067956))
-
-# Process ERRATA_N2_2025414 flag
-$(eval $(call assert_boolean,ERRATA_N2_2025414))
-$(eval $(call add_define,ERRATA_N2_2025414))
-
-# Process ERRATA_N2_2189731 flag
-$(eval $(call assert_boolean,ERRATA_N2_2189731))
-$(eval $(call add_define,ERRATA_N2_2189731))
-
-# Process ERRATA_N2_2138956 flag
-$(eval $(call assert_boolean,ERRATA_N2_2138956))
-$(eval $(call add_define,ERRATA_N2_2138956))
-
-# Process ERRATA_N2_2138953 flag
-$(eval $(call assert_boolean,ERRATA_N2_2138953))
-$(eval $(call add_define,ERRATA_N2_2138953))
-
-# Process ERRATA_N2_2242415 flag
-$(eval $(call assert_boolean,ERRATA_N2_2242415))
-$(eval $(call add_define,ERRATA_N2_2242415))
-
-# Process ERRATA_N2_2138958 flag
-$(eval $(call assert_boolean,ERRATA_N2_2138958))
-$(eval $(call add_define,ERRATA_N2_2138958))
-
-# Process ERRATA_N2_2242400 flag
-$(eval $(call assert_boolean,ERRATA_N2_2242400))
-$(eval $(call add_define,ERRATA_N2_2242400))
-
-# Process ERRATA_N2_2280757 flag
-$(eval $(call assert_boolean,ERRATA_N2_2280757))
-$(eval $(call add_define,ERRATA_N2_2280757))
-
-# Process ERRATA_N2_2326639 flag
-$(eval $(call assert_boolean,ERRATA_N2_2326639))
-$(eval $(call add_define,ERRATA_N2_2326639))
-
-# Process ERRATA_N2_2376738 flag
-$(eval $(call assert_boolean,ERRATA_N2_2376738))
-$(eval $(call add_define,ERRATA_N2_2376738))
-
-# Process ERRATA_N2_2388450 flag
-$(eval $(call assert_boolean,ERRATA_N2_2388450))
-$(eval $(call add_define,ERRATA_N2_2388450))
-
-# Process ERRATA_N2_2743089 flag
-$(eval $(call assert_boolean,ERRATA_N2_2743089))
-$(eval $(call add_define,ERRATA_N2_2743089))
-
-# Process ERRATA_X2_2002765 flag
-$(eval $(call assert_boolean,ERRATA_X2_2002765))
-$(eval $(call add_define,ERRATA_X2_2002765))
-
-# Process ERRATA_X2_2058056 flag
-$(eval $(call assert_boolean,ERRATA_X2_2058056))
-$(eval $(call add_define,ERRATA_X2_2058056))
-
-# Process ERRATA_X2_2083908 flag
-$(eval $(call assert_boolean,ERRATA_X2_2083908))
-$(eval $(call add_define,ERRATA_X2_2083908))
-
-# Process ERRATA_X2_2017096 flag
-$(eval $(call assert_boolean,ERRATA_X2_2017096))
-$(eval $(call add_define,ERRATA_X2_2017096))
-
-# Process ERRATA_X2_2081180 flag
-$(eval $(call assert_boolean,ERRATA_X2_2081180))
-$(eval $(call add_define,ERRATA_X2_2081180))
-
-# Process ERRATA_X2_2216384 flag
-$(eval $(call assert_boolean,ERRATA_X2_2216384))
-$(eval $(call add_define,ERRATA_X2_2216384))
-
-# Process ERRATA_X2_2147715 flag
-$(eval $(call assert_boolean,ERRATA_X2_2147715))
-$(eval $(call add_define,ERRATA_X2_2147715))
-
-# Process ERRATA_X2_2282622 flag
-$(eval $(call assert_boolean,ERRATA_X2_2282622))
-$(eval $(call add_define,ERRATA_X2_2282622))
-
-# Process ERRATA_X2_2371105 flag
-$(eval $(call assert_boolean,ERRATA_X2_2371105))
-$(eval $(call add_define,ERRATA_X2_2371105))
-
-# Process ERRATA_X2_2768515 flag
-$(eval $(call assert_boolean,ERRATA_X2_2768515))
-$(eval $(call add_define,ERRATA_X2_2768515))
-
-# Process ERRATA_X3_2313909 flag
-$(eval $(call assert_boolean,ERRATA_X3_2313909))
-$(eval $(call add_define,ERRATA_X3_2313909))
-
-# Process ERRATA_X3_2615812 flag
-$(eval $(call assert_boolean,ERRATA_X3_2615812))
-$(eval $(call add_define,ERRATA_X3_2615812))
-
-# Process ERRATA_A510_1922240 flag
-$(eval $(call assert_boolean,ERRATA_A510_1922240))
-$(eval $(call add_define,ERRATA_A510_1922240))
-
-# Process ERRATA_A510_2288014 flag
-$(eval $(call assert_boolean,ERRATA_A510_2288014))
-$(eval $(call add_define,ERRATA_A510_2288014))
-
-# Process ERRATA_A510_2042739 flag
-$(eval $(call assert_boolean,ERRATA_A510_2042739))
-$(eval $(call add_define,ERRATA_A510_2042739))
-
-# Process ERRATA_A510_2041909 flag
-$(eval $(call assert_boolean,ERRATA_A510_2041909))
-$(eval $(call add_define,ERRATA_A510_2041909))
-
-# Process ERRATA_A510_2250311 flag
-$(eval $(call assert_boolean,ERRATA_A510_2250311))
-$(eval $(call add_define,ERRATA_A510_2250311))
-
-# Process ERRATA_A510_2218950 flag
-$(eval $(call assert_boolean,ERRATA_A510_2218950))
-$(eval $(call add_define,ERRATA_A510_2218950))
-
-# Process ERRATA_A510_2172148 flag
-$(eval $(call assert_boolean,ERRATA_A510_2172148))
-$(eval $(call add_define,ERRATA_A510_2172148))
-
-# Process ERRATA_A510_2347730 flag
-$(eval $(call assert_boolean,ERRATA_A510_2347730))
-$(eval $(call add_define,ERRATA_A510_2347730))
-
-# Process ERRATA_A510_2371937 flag
-$(eval $(call assert_boolean,ERRATA_A510_2371937))
-$(eval $(call add_define,ERRATA_A510_2371937))
-
-# Process ERRATA_A510_2666669 flag
-$(eval $(call assert_boolean,ERRATA_A510_2666669))
-$(eval $(call add_define,ERRATA_A510_2666669))
-
-# Process ERRATA_A510_2684597 flag
-$(eval $(call assert_boolean,ERRATA_A510_2684597))
-$(eval $(call add_define,ERRATA_A510_2684597))
-
-#Process ERRATA_DSU_798953 flag
-$(eval $(call assert_boolean,ERRATA_DSU_798953))
-$(eval $(call add_define,ERRATA_DSU_798953))
-
-# Process ERRATA_DSU_936184 flag
-$(eval $(call assert_boolean,ERRATA_DSU_936184))
-$(eval $(call add_define,ERRATA_DSU_936184))
+ifneq (${DYNAMIC_WORKAROUND_CVE_2018_3639},0)
+	ifeq (${WORKAROUND_CVE_2018_3639},0)
+		$(error "Error: WORKAROUND_CVE_2018_3639 must be 1 if DYNAMIC_WORKAROUND_CVE_2018_3639 is 1")
+	endif
+endif
 
-# Process ERRATA_DSU_2313941 flag
-$(eval $(call assert_boolean,ERRATA_DSU_2313941))
-$(eval $(call add_define,ERRATA_DSU_2313941))
+# process all flags
+$(eval $(call default_zeros, $(CPU_FLAG_LIST)))
+$(eval $(call add_defines, $(CPU_FLAG_LIST)))
+$(eval $(call assert_booleans, $(CPU_FLAG_LIST)))
 
 # Errata build flags
 ifneq (${ERRATA_A53_843419},0)
diff --git a/lib/el3_runtime/aarch32/context_mgmt.c b/lib/el3_runtime/aarch32/context_mgmt.c
index e494a86..62e30fc 100644
--- a/lib/el3_runtime/aarch32/context_mgmt.c
+++ b/lib/el3_runtime/aarch32/context_mgmt.c
@@ -136,13 +136,13 @@
 static void enable_extensions_nonsecure(bool el2_unused)
 {
 #if IMAGE_BL32
-#if ENABLE_AMU
-	amu_enable(el2_unused);
-#endif
+	if (is_feat_amu_supported()) {
+		amu_enable(el2_unused);
+	}
 
-#if ENABLE_SYS_REG_TRACE_FOR_NS
-	sys_reg_trace_enable();
-#endif /* ENABLE_SYS_REG_TRACE_FOR_NS */
+	if (is_feat_sys_reg_trace_supported()) {
+		sys_reg_trace_enable();
+	}
 
 	if (is_feat_trf_supported()) {
 		trf_enable();
diff --git a/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index d439148..013a505 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S
@@ -13,38 +13,14 @@
 #if CTX_INCLUDE_EL2_REGS
 	.global	el2_sysregs_context_save_common
 	.global	el2_sysregs_context_restore_common
-#if ENABLE_SPE_FOR_LOWER_ELS
-	.global	el2_sysregs_context_save_spe
-	.global	el2_sysregs_context_restore_spe
-#endif /* ENABLE_SPE_FOR_LOWER_ELS */
 #if CTX_INCLUDE_MTE_REGS
 	.global	el2_sysregs_context_save_mte
 	.global	el2_sysregs_context_restore_mte
 #endif /* CTX_INCLUDE_MTE_REGS */
-#if ENABLE_MPAM_FOR_LOWER_ELS
-	.global	el2_sysregs_context_save_mpam
-	.global	el2_sysregs_context_restore_mpam
-#endif /* ENABLE_MPAM_FOR_LOWER_ELS */
-#if ENABLE_FEAT_ECV
-	.global	el2_sysregs_context_save_ecv
-	.global	el2_sysregs_context_restore_ecv
-#endif /* ENABLE_FEAT_ECV */
-#if ENABLE_FEAT_VHE
-	.global	el2_sysregs_context_save_vhe
-	.global	el2_sysregs_context_restore_vhe
-#endif /* ENABLE_FEAT_VHE */
 #if RAS_EXTENSION
 	.global	el2_sysregs_context_save_ras
 	.global	el2_sysregs_context_restore_ras
 #endif /* RAS_EXTENSION */
-#if CTX_INCLUDE_NEVE_REGS
-	.global	el2_sysregs_context_save_nv2
-	.global	el2_sysregs_context_restore_nv2
-#endif /* CTX_INCLUDE_NEVE_REGS */
-#if ENABLE_FEAT_CSV2_2
-	.global	el2_sysregs_context_save_csv2
-	.global	el2_sysregs_context_restore_csv2
-#endif /* ENABLE_FEAT_CSV2_2 */
 #endif /* CTX_INCLUDE_EL2_REGS */
 
 	.global	el1_sysregs_context_save
@@ -220,20 +196,6 @@
 	ret
 endfunc el2_sysregs_context_restore_common
 
-#if ENABLE_SPE_FOR_LOWER_ELS
-func el2_sysregs_context_save_spe
-	mrs	x13, PMSCR_EL2
-	str	x13, [x0, #CTX_PMSCR_EL2]
-	ret
-endfunc el2_sysregs_context_save_spe
-
-func el2_sysregs_context_restore_spe
-	ldr	x13, [x0, #CTX_PMSCR_EL2]
-	msr	PMSCR_EL2, x13
-	ret
-endfunc el2_sysregs_context_restore_spe
-#endif /* ENABLE_SPE_FOR_LOWER_ELS */
-
 #if CTX_INCLUDE_MTE_REGS
 func el2_sysregs_context_save_mte
 	mrs	x9, TFSR_EL2
@@ -248,246 +210,6 @@
 endfunc el2_sysregs_context_restore_mte
 #endif /* CTX_INCLUDE_MTE_REGS */
 
-#if ENABLE_MPAM_FOR_LOWER_ELS
-func el2_sysregs_context_save_mpam
-	mrs	x10, MPAM2_EL2
-	str	x10, [x0, #CTX_MPAM2_EL2]
-
-	mrs	x10, MPAMIDR_EL1
-
-	/*
-	 * The context registers that we intend to save would be part of the
-	 * PE's system register frame only if MPAMIDR_EL1.HAS_HCR == 1.
-	 */
-	tbz	w10, #MPAMIDR_EL1_HAS_HCR_SHIFT, 3f
-
-	/*
-	 * MPAMHCR_EL2, MPAMVPMV_EL2 and MPAMVPM0_EL2 would be present in the
-	 * system register frame if MPAMIDR_EL1.HAS_HCR == 1. Proceed to save
-	 * the context of these registers.
-	 */
-	mrs	x11, MPAMHCR_EL2
-	mrs	x12, MPAMVPM0_EL2
-	stp	x11, x12, [x0, #CTX_MPAMHCR_EL2]
-
-	mrs	x13, MPAMVPMV_EL2
-	str	x13, [x0, #CTX_MPAMVPMV_EL2]
-
-	/*
-	 * MPAMIDR_EL1.VPMR_MAX has to be probed to obtain the maximum supported
-	 * VPMR value. Proceed to save the context of registers from
-	 * MPAMVPM1_EL2 to MPAMVPM<x>_EL2 where x is VPMR_MAX. From MPAM spec,
-	 * VPMR_MAX should not be zero if HAS_HCR == 1.
-	 */
-	ubfx	x10, x10, #MPAMIDR_EL1_VPMR_MAX_SHIFT, \
-		#MPAMIDR_EL1_VPMR_MAX_WIDTH
-
-	/*
-	 * Once VPMR_MAX has been identified, calculate the offset relative to
-	 * PC to jump to so that relevant context can be saved. The offset is
-	 * calculated as (VPMR_POSSIBLE_MAX - VPMR_MAX) * (instruction size for
-	 * saving one VPM register) + (absolute address of label "1").
-	 */
-	mov	w11, #MPAMIDR_EL1_VPMR_MAX_POSSIBLE
-	sub	w10, w11, w10
-
-	/* Calculate the size of one block of MPAMVPM*_EL2 save */
-	adr	x11, 1f
-	adr	x12, 2f
-	sub	x12, x12, x11
-
-	madd	x10, x10, x12, x11
-	br	x10
-
-	/*
-	 * The branch above would land properly on one of the blocks following
-	 * label "1". Make sure that the order of save is retained.
-	 */
-1:
-#if ENABLE_BTI
-	bti	j
-#endif
-	mrs	x10, MPAMVPM7_EL2
-	str	x10, [x0, #CTX_MPAMVPM7_EL2]
-2:
-#if ENABLE_BTI
-	bti	j
-#endif
-	mrs	x11, MPAMVPM6_EL2
-	str	x11, [x0, #CTX_MPAMVPM6_EL2]
-
-#if ENABLE_BTI
-	bti	j
-#endif
-	mrs	x12, MPAMVPM5_EL2
-	str	x12, [x0, #CTX_MPAMVPM5_EL2]
-
-#if ENABLE_BTI
-	bti	j
-#endif
-	mrs	x13, MPAMVPM4_EL2
-	str	x13, [x0, #CTX_MPAMVPM4_EL2]
-
-#if ENABLE_BTI
-	bti	j
-#endif
-	mrs	x14, MPAMVPM3_EL2
-	str	x14, [x0, #CTX_MPAMVPM3_EL2]
-
-#if ENABLE_BTI
-	bti	j
-#endif
-	mrs	x15, MPAMVPM2_EL2
-	str	x15, [x0, #CTX_MPAMVPM2_EL2]
-
-#if ENABLE_BTI
-	bti	j
-#endif
-	mrs	x16, MPAMVPM1_EL2
-	str	x16, [x0, #CTX_MPAMVPM1_EL2]
-
-3:	ret
-endfunc el2_sysregs_context_save_mpam
-
-func el2_sysregs_context_restore_mpam
-	ldr	x10, [x0, #CTX_MPAM2_EL2]
-	msr	MPAM2_EL2, x10
-
-	mrs	x10, MPAMIDR_EL1
-	/*
-	 * The context registers that we intend to restore would be part of the
-	 * PE's system register frame only if MPAMIDR_EL1.HAS_HCR == 1.
-	 */
-	tbz	w10, #MPAMIDR_EL1_HAS_HCR_SHIFT, 3f
-
-	/*
-	 * MPAMHCR_EL2, MPAMVPMV_EL2 and MPAMVPM0_EL2 would be present in the
-	 * system register frame if MPAMIDR_EL1.HAS_HCR == 1. Proceed to restore
-	 * the context of these registers
-	 */
-	ldp	x11, x12, [x0, #CTX_MPAMHCR_EL2]
-	msr	MPAMHCR_EL2, x11
-	msr	MPAMVPM0_EL2, x12
-
-	ldr	x13, [x0, #CTX_MPAMVPMV_EL2]
-	msr	MPAMVPMV_EL2, x13
-
-	/*
-	 * MPAMIDR_EL1.VPMR_MAX has to be probed to obtain the maximum supported
-	 * VPMR value. Proceed to restore the context of registers from
-	 * MPAMVPM1_EL2 to MPAMVPM<x>_EL2 where x is VPMR_MAX. from MPAM spec,
-	 * VPMR_MAX should not be zero if HAS_HCR == 1.
-	 */
-	ubfx	x10, x10, #MPAMIDR_EL1_VPMR_MAX_SHIFT,	\
-		#MPAMIDR_EL1_VPMR_MAX_WIDTH
-
-	/*
-	 * Once VPMR_MAX has been identified, calculate the offset relative to
-	 * PC to jump to so that relevant context can be restored. The offset is
-	 * calculated as (VPMR_POSSIBLE_MAX - VPMR_MAX) * (instruction size for
-	 * restoring one VPM register) + (absolute address of label "1").
-	 */
-	mov	w11, #MPAMIDR_EL1_VPMR_MAX_POSSIBLE
-	sub	w10, w11, w10
-
-	/* Calculate the size of one block of MPAMVPM*_EL2 restore */
-	adr	x11, 1f
-	adr	x12, 2f
-	sub	x12, x12, x11
-
-	madd	x10, x10, x12, x11
-	br	x10
-
-	/*
-	 * The branch above would land properly on one of the blocks following
-	 * label "1". Make sure that the order of restore is retained.
-	 */
-1:
-
-#if ENABLE_BTI
-	bti	j
-#endif
-	ldr	x10, [x0, #CTX_MPAMVPM7_EL2]
-	msr	MPAMVPM7_EL2, x10
-2:
-#if ENABLE_BTI
-	bti	j
-#endif
-	ldr	x11, [x0, #CTX_MPAMVPM6_EL2]
-	msr	MPAMVPM6_EL2, x11
-
-#if ENABLE_BTI
-	bti	j
-#endif
-	ldr	x12, [x0, #CTX_MPAMVPM5_EL2]
-	msr	MPAMVPM5_EL2, x12
-
-#if ENABLE_BTI
-	bti	j
-#endif
-	ldr	x13, [x0, #CTX_MPAMVPM4_EL2]
-	msr	MPAMVPM4_EL2, x13
-
-#if ENABLE_BTI
-	bti	j
-#endif
-	ldr	x14, [x0, #CTX_MPAMVPM3_EL2]
-	msr	MPAMVPM3_EL2, x14
-
-#if ENABLE_BTI
-	bti	j
-#endif
-	ldr	x15, [x0, #CTX_MPAMVPM2_EL2]
-	msr	MPAMVPM2_EL2, x15
-
-#if ENABLE_BTI
-	bti	j
-#endif
-	ldr	x16, [x0, #CTX_MPAMVPM1_EL2]
-	msr	MPAMVPM1_EL2, x16
-
-3:	ret
-endfunc el2_sysregs_context_restore_mpam
-#endif /* ENABLE_MPAM_FOR_LOWER_ELS */
-
-#if ENABLE_FEAT_ECV
-func el2_sysregs_context_save_ecv
-	mrs	x11, CNTPOFF_EL2
-	str	x11, [x0, #CTX_CNTPOFF_EL2]
-	ret
-endfunc el2_sysregs_context_save_ecv
-
-func el2_sysregs_context_restore_ecv
-	ldr	x11, [x0, #CTX_CNTPOFF_EL2]
-	msr	CNTPOFF_EL2, x11
-	ret
-endfunc el2_sysregs_context_restore_ecv
-#endif /* ENABLE_FEAT_ECV */
-
-#if ENABLE_FEAT_VHE
-func el2_sysregs_context_save_vhe
-	/*
-	 * CONTEXTIDR_EL2 register is saved only when FEAT_VHE or
-	 * FEAT_Debugv8p2 (currently not in TF-A) is supported.
-	 */
-	mrs	x9, contextidr_el2
-	mrs	x10, ttbr1_el2
-	stp	x9, x10, [x0, #CTX_CONTEXTIDR_EL2]
-	ret
-endfunc el2_sysregs_context_save_vhe
-
-func el2_sysregs_context_restore_vhe
-	/*
-	 * CONTEXTIDR_EL2 register is restored only when FEAT_VHE or
-	 * FEAT_Debugv8p2 (currently not in TF-A) is supported.
-	 */
-	ldp	x9, x10, [x0, #CTX_CONTEXTIDR_EL2]
-	msr	contextidr_el2, x9
-	msr	ttbr1_el2, x10
-	ret
-endfunc el2_sysregs_context_restore_vhe
-#endif /* ENABLE_FEAT_VHE */
-
 #if RAS_EXTENSION
 func el2_sysregs_context_save_ras
 	/*
@@ -512,46 +234,6 @@
 endfunc el2_sysregs_context_restore_ras
 #endif /* RAS_EXTENSION */
 
-#if CTX_INCLUDE_NEVE_REGS
-func el2_sysregs_context_save_nv2
-	/*
-	 * VNCR_EL2 register is saved only when FEAT_NV2 is supported.
-	 */
-	mrs	x16, vncr_el2
-	str	x16, [x0, #CTX_VNCR_EL2]
-	ret
-endfunc el2_sysregs_context_save_nv2
-
-func el2_sysregs_context_restore_nv2
-	/*
-	 * VNCR_EL2 register is restored only when FEAT_NV2 is supported.
-	 */
-	ldr	x16, [x0, #CTX_VNCR_EL2]
-	msr	vncr_el2, x16
-	ret
-endfunc el2_sysregs_context_restore_nv2
-#endif /* CTX_INCLUDE_NEVE_REGS */
-
-#if ENABLE_FEAT_CSV2_2
-func el2_sysregs_context_save_csv2
-	/*
-	 * SCXTNUM_EL2 register is saved only when FEAT_CSV2_2 is supported.
-	 */
-	mrs	x13, scxtnum_el2
-	str	x13, [x0, #CTX_SCXTNUM_EL2]
-	ret
-endfunc el2_sysregs_context_save_csv2
-
-func el2_sysregs_context_restore_csv2
-	/*
-	 * SCXTNUM_EL2 register is restored only when FEAT_CSV2_2 is supported.
-	 */
-	ldr	x13, [x0, #CTX_SCXTNUM_EL2]
-	msr	scxtnum_el2, x13
-	ret
-endfunc el2_sysregs_context_restore_csv2
-#endif /* ENABLE_FEAT_CSV2_2 */
-
 #endif /* CTX_INCLUDE_EL2_REGS */
 
 /* ------------------------------------------------------------------
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 4d2079d..42166eb 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -134,7 +134,7 @@
 #endif /* CTX_INCLUDE_MTE_REGS */
 
 	/* Enable S-EL2 if the next EL is EL2 and S-EL2 is present */
-	if ((GET_EL(ep->spsr) == MODE_EL2) && is_armv8_4_sel2_present()) {
+	if ((GET_EL(ep->spsr) == MODE_EL2) && is_feat_sel2_supported()) {
 		if (GET_RW(ep->spsr) != MODE_RW_64) {
 			ERROR("S-EL2 can not be used in AArch32\n.");
 			panic();
@@ -171,10 +171,10 @@
 
 	scr_el3 |= SCR_NS_BIT | SCR_NSE_BIT;
 
-#if ENABLE_FEAT_CSV2_2
-	/* Enable access to the SCXTNUM_ELx registers. */
-	scr_el3 |= SCR_EnSCXT_BIT;
-#endif
+	if (is_feat_csv2_2_supported()) {
+		/* Enable access to the SCXTNUM_ELx registers. */
+		scr_el3 |= SCR_EnSCXT_BIT;
+	}
 
 	write_ctx_reg(state, CTX_SCR_EL3, scr_el3);
 }
@@ -227,10 +227,10 @@
 	scr_el3 |= SCR_TERR_BIT;
 #endif
 
-#if ENABLE_FEAT_CSV2_2
-	/* Enable access to the SCXTNUM_ELx registers. */
-	scr_el3 |= SCR_EnSCXT_BIT;
-#endif
+	if (is_feat_csv2_2_supported()) {
+		/* Enable access to the SCXTNUM_ELx registers. */
+		scr_el3 |= SCR_EnSCXT_BIT;
+	}
 
 #ifdef IMAGE_BL31
 	/*
@@ -348,6 +348,13 @@
 #endif
 
 	/*
+	 * SCR_EL3.TCR2EN: Enable access to TCR2_ELx for AArch64 if present.
+	 */
+	if (is_feat_tcr2_supported() && (GET_RW(ep->spsr) == MODE_RW_64)) {
+		scr_el3 |= SCR_TCR2EN_BIT;
+	}
+
+	/*
 	 * CPTR_EL3 was initialized out of reset, copy that value to the
 	 * context register.
 	 */
@@ -373,22 +380,21 @@
 			scr_el3 |= SCR_FGTEN_BIT;
 		}
 
-		if (get_armv8_6_ecv_support()
-		    == ID_AA64MMFR0_EL1_ECV_SELF_SYNCH) {
+		if (is_feat_ecv_supported()) {
 			scr_el3 |= SCR_ECVEN_BIT;
 		}
 	}
 
-#if ENABLE_FEAT_TWED
 	/* Enable WFE trap delay in SCR_EL3 if supported and configured */
-	/* Set delay in SCR_EL3 */
-	scr_el3 &= ~(SCR_TWEDEL_MASK << SCR_TWEDEL_SHIFT);
-	scr_el3 |= ((TWED_DELAY & SCR_TWEDEL_MASK)
-			<< SCR_TWEDEL_SHIFT);
+	if (is_feat_twed_supported()) {
+		/* Set delay in SCR_EL3 */
+		scr_el3 &= ~(SCR_TWEDEL_MASK << SCR_TWEDEL_SHIFT);
+		scr_el3 |= ((TWED_DELAY & SCR_TWEDEL_MASK)
+				<< SCR_TWEDEL_SHIFT);
 
-	/* Enable WFE delay */
-	scr_el3 |= SCR_TWEDEn_BIT;
-#endif /* ENABLE_FEAT_TWED */
+		/* Enable WFE delay */
+		scr_el3 |= SCR_TWEDEn_BIT;
+	}
 
 	/*
 	 * Populate EL3 state so that we've the right context
@@ -475,25 +481,25 @@
 static void manage_extensions_nonsecure(bool el2_unused, cpu_context_t *ctx)
 {
 #if IMAGE_BL31
-#if ENABLE_SPE_FOR_LOWER_ELS
-	spe_enable(el2_unused);
-#endif
+	if (is_feat_spe_supported()) {
+		spe_enable(el2_unused);
+	}
 
-#if ENABLE_AMU
-	amu_enable(el2_unused, ctx);
-#endif
+	if (is_feat_amu_supported()) {
+		amu_enable(el2_unused, ctx);
+	}
 
-#if ENABLE_SME_FOR_NS
 	/* Enable SME, SVE, and FPU/SIMD for non-secure world. */
-	sme_enable(ctx);
-#elif ENABLE_SVE_FOR_NS
-	/* Enable SVE and FPU/SIMD for non-secure world. */
-	sve_enable(ctx);
-#endif
+	if (is_feat_sme_supported()) {
+		sme_enable(ctx);
+	} else if (is_feat_sve_supported()) {
+		/* Enable SVE and FPU/SIMD for non-secure world. */
+		sve_enable(ctx);
+	}
 
-#if ENABLE_MPAM_FOR_LOWER_ELS
-	mpam_enable(el2_unused);
-#endif
+	if (is_feat_mpam_supported()) {
+		mpam_enable(el2_unused);
+	}
 
 	if (is_feat_trbe_supported()) {
 		trbe_enable();
@@ -503,9 +509,9 @@
 		brbe_enable();
 	}
 
-#if ENABLE_SYS_REG_TRACE_FOR_NS
-	sys_reg_trace_enable(ctx);
-#endif /* ENABLE_SYS_REG_TRACE_FOR_NS */
+	if (is_feat_sys_reg_trace_supported()) {
+		sys_reg_trace_enable(ctx);
+	}
 
 	if (is_feat_trf_supported()) {
 		trf_enable();
@@ -519,35 +525,38 @@
 static void manage_extensions_secure(cpu_context_t *ctx)
 {
 #if IMAGE_BL31
- #if ENABLE_SME_FOR_NS
-  #if ENABLE_SME_FOR_SWD
-	/*
-	 * Enable SME, SVE, FPU/SIMD in secure context, secure manager must
-	 * ensure SME, SVE, and FPU/SIMD context properly managed.
-	 */
-	sme_enable(ctx);
-  #else /* ENABLE_SME_FOR_SWD */
-	/*
-	 * Disable SME, SVE, FPU/SIMD in secure context so non-secure world can
-	 * safely use the associated registers.
-	 */
-	sme_disable(ctx);
-  #endif /* ENABLE_SME_FOR_SWD */
- #elif ENABLE_SVE_FOR_NS
-  #if ENABLE_SVE_FOR_SWD
-	/*
-	 * Enable SVE and FPU in secure context, secure manager must ensure that
-	 * the SVE and FPU register contexts are properly managed.
-	 */
-	sve_enable(ctx);
- #else /* ENABLE_SVE_FOR_SWD */
-	/*
-	 * Disable SVE and FPU in secure context so non-secure world can safely
-	 * use them.
-	 */
-	sve_disable(ctx);
-  #endif /* ENABLE_SVE_FOR_SWD */
- #endif /* ENABLE_SVE_FOR_NS */
+
+	if (is_feat_sme_supported()) {
+		if (ENABLE_SME_FOR_SWD) {
+		/*
+		 * Enable SME, SVE, FPU/SIMD in secure context, secure manager
+		 * must ensure SME, SVE, and FPU/SIMD context properly managed.
+		 */
+			sme_enable(ctx);
+		} else {
+		/*
+		 * Disable SME, SVE, FPU/SIMD in secure context so non-secure
+		 * world can safely use the associated registers.
+		 */
+			sme_disable(ctx);
+		}
+	} else if (is_feat_sve_supported()) {
+		if (ENABLE_SVE_FOR_SWD) {
+		/*
+		 * Enable SVE and FPU in secure context, secure manager must
+		 * ensure that the SVE and FPU register contexts are properly
+		 * managed.
+		 */
+			sve_enable(ctx);
+		} else {
+		/*
+		 * Disable SVE and FPU in secure context so non-secure world
+		 * can safely use them.
+		 */
+			sve_disable(ctx);
+		}
+	}
+
 #endif /* IMAGE_BL31 */
 }
 
@@ -827,6 +836,96 @@
 	write_hfgwtr_el2(read_ctx_reg(ctx, CTX_HFGWTR_EL2));
 }
 
+static void el2_sysregs_context_save_mpam(el2_sysregs_t *ctx)
+{
+	u_register_t mpam_idr = read_mpamidr_el1();
+
+	write_ctx_reg(ctx, CTX_MPAM2_EL2, read_mpam2_el2());
+
+	/*
+	 * The context registers that we intend to save would be part of the
+	 * PE's system register frame only if MPAMIDR_EL1.HAS_HCR == 1.
+	 */
+	if ((mpam_idr & MPAMIDR_HAS_HCR_BIT) == 0U) {
+		return;
+	}
+
+	/*
+	 * MPAMHCR_EL2, MPAMVPMV_EL2 and MPAMVPM0_EL2 are always present if
+	 * MPAMIDR_HAS_HCR_BIT == 1.
+	 */
+	write_ctx_reg(ctx, CTX_MPAMHCR_EL2, read_mpamhcr_el2());
+	write_ctx_reg(ctx, CTX_MPAMVPM0_EL2, read_mpamvpm0_el2());
+	write_ctx_reg(ctx, CTX_MPAMVPMV_EL2, read_mpamvpmv_el2());
+
+	/*
+	 * The number of MPAMVPM registers is implementation defined, their
+	 * number is stored in the MPAMIDR_EL1 register.
+	 */
+	switch ((mpam_idr >> MPAMIDR_EL1_VPMR_MAX_SHIFT) & MPAMIDR_EL1_VPMR_MAX_MASK) {
+	case 7:
+		write_ctx_reg(ctx, CTX_MPAMVPM7_EL2, read_mpamvpm7_el2());
+		__fallthrough;
+	case 6:
+		write_ctx_reg(ctx, CTX_MPAMVPM6_EL2, read_mpamvpm6_el2());
+		__fallthrough;
+	case 5:
+		write_ctx_reg(ctx, CTX_MPAMVPM5_EL2, read_mpamvpm5_el2());
+		__fallthrough;
+	case 4:
+		write_ctx_reg(ctx, CTX_MPAMVPM4_EL2, read_mpamvpm4_el2());
+		__fallthrough;
+	case 3:
+		write_ctx_reg(ctx, CTX_MPAMVPM3_EL2, read_mpamvpm3_el2());
+		__fallthrough;
+	case 2:
+		write_ctx_reg(ctx, CTX_MPAMVPM2_EL2, read_mpamvpm2_el2());
+		__fallthrough;
+	case 1:
+		write_ctx_reg(ctx, CTX_MPAMVPM1_EL2, read_mpamvpm1_el2());
+		break;
+	}
+}
+
+static void el2_sysregs_context_restore_mpam(el2_sysregs_t *ctx)
+{
+	u_register_t mpam_idr = read_mpamidr_el1();
+
+	write_mpam2_el2(read_ctx_reg(ctx, CTX_MPAM2_EL2));
+
+	if ((mpam_idr & MPAMIDR_HAS_HCR_BIT) == 0U) {
+		return;
+	}
+
+	write_mpamhcr_el2(read_ctx_reg(ctx, CTX_MPAMHCR_EL2));
+	write_mpamvpm0_el2(read_ctx_reg(ctx, CTX_MPAMVPM0_EL2));
+	write_mpamvpmv_el2(read_ctx_reg(ctx, CTX_MPAMVPMV_EL2));
+
+	switch ((mpam_idr >> MPAMIDR_EL1_VPMR_MAX_SHIFT) & MPAMIDR_EL1_VPMR_MAX_MASK) {
+	case 7:
+		write_mpamvpm7_el2(read_ctx_reg(ctx, CTX_MPAMVPM7_EL2));
+		__fallthrough;
+	case 6:
+		write_mpamvpm6_el2(read_ctx_reg(ctx, CTX_MPAMVPM6_EL2));
+		__fallthrough;
+	case 5:
+		write_mpamvpm5_el2(read_ctx_reg(ctx, CTX_MPAMVPM5_EL2));
+		__fallthrough;
+	case 4:
+		write_mpamvpm4_el2(read_ctx_reg(ctx, CTX_MPAMVPM4_EL2));
+		__fallthrough;
+	case 3:
+		write_mpamvpm3_el2(read_ctx_reg(ctx, CTX_MPAMVPM3_EL2));
+		__fallthrough;
+	case 2:
+		write_mpamvpm2_el2(read_ctx_reg(ctx, CTX_MPAMVPM2_EL2));
+		__fallthrough;
+	case 1:
+		write_mpamvpm1_el2(read_ctx_reg(ctx, CTX_MPAMVPM1_EL2));
+		break;
+	}
+}
+
 /*******************************************************************************
  * Save EL2 sysreg context
  ******************************************************************************/
@@ -849,41 +948,52 @@
 		el2_sysregs_ctx = get_el2_sysregs_ctx(ctx);
 
 		el2_sysregs_context_save_common(el2_sysregs_ctx);
-#if ENABLE_SPE_FOR_LOWER_ELS
-		el2_sysregs_context_save_spe(el2_sysregs_ctx);
-#endif
 #if CTX_INCLUDE_MTE_REGS
 		el2_sysregs_context_save_mte(el2_sysregs_ctx);
 #endif
-#if ENABLE_MPAM_FOR_LOWER_ELS
-		el2_sysregs_context_save_mpam(el2_sysregs_ctx);
-#endif
+		if (is_feat_mpam_supported()) {
+			el2_sysregs_context_save_mpam(el2_sysregs_ctx);
+		}
 
 		if (is_feat_fgt_supported()) {
 			el2_sysregs_context_save_fgt(el2_sysregs_ctx);
 		}
 
-#if ENABLE_FEAT_ECV
-		el2_sysregs_context_save_ecv(el2_sysregs_ctx);
-#endif
-#if ENABLE_FEAT_VHE
-		el2_sysregs_context_save_vhe(el2_sysregs_ctx);
-#endif
+		if (is_feat_ecv_v2_supported()) {
+			write_ctx_reg(el2_sysregs_ctx, CTX_CNTPOFF_EL2,
+				      read_cntpoff_el2());
+		}
+
+		if (is_feat_vhe_supported()) {
+			write_ctx_reg(el2_sysregs_ctx, CTX_CONTEXTIDR_EL2,
+				      read_contextidr_el2());
+			write_ctx_reg(el2_sysregs_ctx, CTX_TTBR1_EL2,
+				      read_ttbr1_el2());
+		}
 #if RAS_EXTENSION
 		el2_sysregs_context_save_ras(el2_sysregs_ctx);
 #endif
-#if CTX_INCLUDE_NEVE_REGS
-		el2_sysregs_context_save_nv2(el2_sysregs_ctx);
-#endif
+
+		if (is_feat_nv2_supported()) {
+			write_ctx_reg(el2_sysregs_ctx, CTX_VNCR_EL2,
+				      read_vncr_el2());
+		}
+
 		if (is_feat_trf_supported()) {
 			write_ctx_reg(el2_sysregs_ctx, CTX_TRFCR_EL2, read_trfcr_el2());
 		}
-#if ENABLE_FEAT_CSV2_2
-		el2_sysregs_context_save_csv2(el2_sysregs_ctx);
-#endif
+
+		if (is_feat_csv2_2_supported()) {
+			write_ctx_reg(el2_sysregs_ctx, CTX_SCXTNUM_EL2,
+				      read_scxtnum_el2());
+		}
+
 		if (is_feat_hcx_supported()) {
 			write_ctx_reg(el2_sysregs_ctx, CTX_HCRX_EL2, read_hcrx_el2());
 		}
+		if (is_feat_tcr2_supported()) {
+			write_ctx_reg(el2_sysregs_ctx, CTX_TCR2_EL2, read_tcr2_el2());
+		}
 	}
 }
 
@@ -909,41 +1019,48 @@
 		el2_sysregs_ctx = get_el2_sysregs_ctx(ctx);
 
 		el2_sysregs_context_restore_common(el2_sysregs_ctx);
-#if ENABLE_SPE_FOR_LOWER_ELS
-		el2_sysregs_context_restore_spe(el2_sysregs_ctx);
-#endif
 #if CTX_INCLUDE_MTE_REGS
 		el2_sysregs_context_restore_mte(el2_sysregs_ctx);
 #endif
-#if ENABLE_MPAM_FOR_LOWER_ELS
-		el2_sysregs_context_restore_mpam(el2_sysregs_ctx);
-#endif
+		if (is_feat_mpam_supported()) {
+			el2_sysregs_context_restore_mpam(el2_sysregs_ctx);
+		}
 
 		if (is_feat_fgt_supported()) {
 			el2_sysregs_context_restore_fgt(el2_sysregs_ctx);
 		}
 
-#if ENABLE_FEAT_ECV
-		el2_sysregs_context_restore_ecv(el2_sysregs_ctx);
-#endif
-#if ENABLE_FEAT_VHE
-		el2_sysregs_context_restore_vhe(el2_sysregs_ctx);
-#endif
+		if (is_feat_ecv_v2_supported()) {
+			write_cntpoff_el2(read_ctx_reg(el2_sysregs_ctx,
+						       CTX_CNTPOFF_EL2));
+		}
+
+		if (is_feat_vhe_supported()) {
+			write_contextidr_el2(read_ctx_reg(el2_sysregs_ctx, CTX_CONTEXTIDR_EL2));
+			write_ttbr1_el2(read_ctx_reg(el2_sysregs_ctx, CTX_TTBR1_EL2));
+		}
 #if RAS_EXTENSION
 		el2_sysregs_context_restore_ras(el2_sysregs_ctx);
 #endif
-#if CTX_INCLUDE_NEVE_REGS
-		el2_sysregs_context_restore_nv2(el2_sysregs_ctx);
-#endif
+
+		if (is_feat_nv2_supported()) {
+			write_vncr_el2(read_ctx_reg(el2_sysregs_ctx, CTX_VNCR_EL2));
+		}
 		if (is_feat_trf_supported()) {
 			write_trfcr_el2(read_ctx_reg(el2_sysregs_ctx, CTX_TRFCR_EL2));
 		}
-#if ENABLE_FEAT_CSV2_2
-		el2_sysregs_context_restore_csv2(el2_sysregs_ctx);
-#endif
+
+		if (is_feat_csv2_2_supported()) {
+			write_scxtnum_el2(read_ctx_reg(el2_sysregs_ctx,
+						       CTX_SCXTNUM_EL2));
+		}
+
 		if (is_feat_hcx_supported()) {
 			write_hcrx_el2(read_ctx_reg(el2_sysregs_ctx, CTX_HCRX_EL2));
 		}
+		if (is_feat_tcr2_supported()) {
+			write_tcr2_el2(read_ctx_reg(el2_sysregs_ctx, CTX_TCR2_EL2));
+		}
 	}
 }
 #endif /* CTX_INCLUDE_EL2_REGS */
diff --git a/lib/extensions/amu/aarch32/amu.c b/lib/extensions/amu/aarch32/amu.c
index 57b1158..03186d6 100644
--- a/lib/extensions/amu/aarch32/amu.c
+++ b/lib/extensions/amu/aarch32/amu.c
@@ -10,6 +10,7 @@
 
 #include "../amu_private.h"
 #include <arch.h>
+#include <arch_features.h>
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <lib/el3_runtime/pubsub_events.h>
@@ -39,12 +40,6 @@
 	amu_ctx_group1_enable_cannot_represent_all_group1_counters);
 #endif
 
-static inline __unused uint32_t read_id_pfr0_amu(void)
-{
-	return (read_id_pfr0() >> ID_PFR0_AMU_SHIFT) &
-		ID_PFR0_AMU_MASK;
-}
-
 static inline __unused void write_hcptr_tam(uint32_t value)
 {
 	write_hcptr((read_hcptr() & ~TAM_BIT) |
@@ -129,11 +124,6 @@
 	write_amcntenclr1(value);
 }
 
-static __unused bool amu_supported(void)
-{
-	return read_id_pfr0_amu() >= ID_PFR0_AMU_V1;
-}
-
 #if ENABLE_AMU_AUXILIARY_COUNTERS
 static __unused bool amu_group1_supported(void)
 {
@@ -147,23 +137,12 @@
  */
 void amu_enable(bool el2_unused)
 {
-	uint32_t id_pfr0_amu;		/* AMU version */
-
 	uint32_t amcfgr_ncg;		/* Number of counter groups */
 	uint32_t amcgcr_cg0nc;		/* Number of group 0 counters */
 
 	uint32_t amcntenset0_px = 0x0;	/* Group 0 enable mask */
 	uint32_t amcntenset1_px = 0x0;	/* Group 1 enable mask */
 
-	id_pfr0_amu = read_id_pfr0_amu();
-	if (id_pfr0_amu == ID_PFR0_AMU_NOT_SUPPORTED) {
-		/*
-		 * If the AMU is unsupported, nothing needs to be done.
-		 */
-
-		return;
-	}
-
 	if (el2_unused) {
 		/*
 		 * HCPTR.TAM: Set to zero so any accesses to the Activity
@@ -221,8 +200,8 @@
 #endif
 	}
 
-	/* Initialize FEAT_AMUv1p1 features if present. */
-	if (id_pfr0_amu < ID_PFR0_AMU_V1P1) {
+	/* Bail out if FEAT_AMUv1p1 features are not present. */
+	if (!is_feat_amuv1p1_supported()) {
 		return;
 	}
 
@@ -244,7 +223,7 @@
 /* Read the group 0 counter identified by the given `idx`. */
 static uint64_t amu_group0_cnt_read(unsigned int idx)
 {
-	assert(amu_supported());
+	assert(is_feat_amu_supported());
 	assert(idx < read_amcgcr_cg0nc());
 
 	return amu_group0_cnt_read_internal(idx);
@@ -253,7 +232,7 @@
 /* Write the group 0 counter identified by the given `idx` with `val` */
 static void amu_group0_cnt_write(unsigned  int idx, uint64_t val)
 {
-	assert(amu_supported());
+	assert(is_feat_amu_supported());
 	assert(idx < read_amcgcr_cg0nc());
 
 	amu_group0_cnt_write_internal(idx, val);
@@ -264,7 +243,7 @@
 /* Read the group 1 counter identified by the given `idx` */
 static uint64_t amu_group1_cnt_read(unsigned  int idx)
 {
-	assert(amu_supported());
+	assert(is_feat_amu_supported());
 	assert(amu_group1_supported());
 	assert(idx < read_amcgcr_cg1nc());
 
@@ -274,7 +253,7 @@
 /* Write the group 1 counter identified by the given `idx` with `val` */
 static void amu_group1_cnt_write(unsigned  int idx, uint64_t val)
 {
-	assert(amu_supported());
+	assert(is_feat_amu_supported());
 	assert(amu_group1_supported());
 	assert(idx < read_amcgcr_cg1nc());
 
@@ -290,7 +269,6 @@
 	unsigned int core_pos;
 	struct amu_ctx *ctx;
 
-	uint32_t id_pfr0_amu;	/* AMU version */
 	uint32_t amcgcr_cg0nc;	/* Number of group 0 counters */
 
 #if ENABLE_AMU_AUXILIARY_COUNTERS
@@ -298,8 +276,7 @@
 	uint32_t amcgcr_cg1nc;	/* Number of group 1 counters */
 #endif
 
-	id_pfr0_amu = read_id_pfr0_amu();
-	if (id_pfr0_amu == ID_PFR0_AMU_NOT_SUPPORTED) {
+	if (!is_feat_amu_supported()) {
 		return (void *)0;
 	}
 
@@ -353,8 +330,6 @@
 	unsigned int core_pos;
 	struct amu_ctx *ctx;
 
-	uint32_t id_pfr0_amu;	/* AMU version */
-
 	uint32_t amcfgr_ncg;	/* Number of counter groups */
 	uint32_t amcgcr_cg0nc;	/* Number of group 0 counters */
 
@@ -362,8 +337,7 @@
 	uint32_t amcgcr_cg1nc;	/* Number of group 1 counters */
 #endif
 
-	id_pfr0_amu = read_id_pfr0_amu();
-	if (id_pfr0_amu == ID_PFR0_AMU_NOT_SUPPORTED) {
+	if (!is_feat_amu_supported()) {
 		return (void *)0;
 	}
 
diff --git a/lib/extensions/amu/aarch64/amu.c b/lib/extensions/amu/aarch64/amu.c
index 72566fd..c650629 100644
--- a/lib/extensions/amu/aarch64/amu.c
+++ b/lib/extensions/amu/aarch64/amu.c
@@ -57,12 +57,6 @@
 	amu_ctx_group1_enable_cannot_represent_all_group1_counters);
 #endif
 
-static inline __unused uint64_t read_id_aa64pfr0_el1_amu(void)
-{
-	return (read_id_aa64pfr0_el1() >> ID_AA64PFR0_AMU_SHIFT) &
-		ID_AA64PFR0_AMU_MASK;
-}
-
 static inline __unused uint64_t read_hcr_el2_amvoffen(void)
 {
 	return (read_hcr_el2() & HCR_AMVOFFEN_BIT) >>
@@ -183,16 +177,6 @@
 	write_amcntenclr1_el0(value);
 }
 
-static __unused bool amu_supported(void)
-{
-	return read_id_aa64pfr0_el1_amu() >= ID_AA64PFR0_AMU_V1;
-}
-
-static __unused bool amu_v1p1_supported(void)
-{
-	return read_id_aa64pfr0_el1_amu() >= ID_AA64PFR0_AMU_V1P1;
-}
-
 #if ENABLE_AMU_AUXILIARY_COUNTERS
 static __unused bool amu_group1_supported(void)
 {
@@ -206,23 +190,12 @@
  */
 void amu_enable(bool el2_unused, cpu_context_t *ctx)
 {
-	uint64_t id_aa64pfr0_el1_amu;		/* AMU version */
-
 	uint64_t amcfgr_el0_ncg;		/* Number of counter groups */
 	uint64_t amcgcr_el0_cg0nc;		/* Number of group 0 counters */
 
 	uint64_t amcntenset0_el0_px = 0x0;	/* Group 0 enable mask */
 	uint64_t amcntenset1_el0_px = 0x0;	/* Group 1 enable mask */
 
-	id_aa64pfr0_el1_amu = read_id_aa64pfr0_el1_amu();
-	if (id_aa64pfr0_el1_amu == ID_AA64PFR0_AMU_NOT_SUPPORTED) {
-		/*
-		 * If the AMU is unsupported, nothing needs to be done.
-		 */
-
-		return;
-	}
-
 	if (el2_unused) {
 		/*
 		 * CPTR_EL2.TAM: Set to zero so any accesses to the Activity
@@ -288,7 +261,7 @@
 	}
 
 	/* Initialize FEAT_AMUv1p1 features if present. */
-	if (id_aa64pfr0_el1_amu >= ID_AA64PFR0_AMU_V1P1) {
+	if (is_feat_amuv1p1_supported()) {
 		if (el2_unused) {
 			/*
 			 * Make sure virtual offsets are disabled if EL2 not
@@ -327,7 +300,7 @@
 /* Read the group 0 counter identified by the given `idx`. */
 static uint64_t amu_group0_cnt_read(unsigned int idx)
 {
-	assert(amu_supported());
+	assert(is_feat_amu_supported());
 	assert(idx < read_amcgcr_el0_cg0nc());
 
 	return amu_group0_cnt_read_internal(idx);
@@ -336,7 +309,7 @@
 /* Write the group 0 counter identified by the given `idx` with `val` */
 static void amu_group0_cnt_write(unsigned  int idx, uint64_t val)
 {
-	assert(amu_supported());
+	assert(is_feat_amu_supported());
 	assert(idx < read_amcgcr_el0_cg0nc());
 
 	amu_group0_cnt_write_internal(idx, val);
@@ -376,7 +349,7 @@
  */
 static uint64_t amu_group0_voffset_read(unsigned int idx)
 {
-	assert(amu_v1p1_supported());
+	assert(is_feat_amuv1p1_supported());
 	assert(idx < read_amcgcr_el0_cg0nc());
 	assert(idx != 1U);
 
@@ -391,7 +364,7 @@
  */
 static void amu_group0_voffset_write(unsigned int idx, uint64_t val)
 {
-	assert(amu_v1p1_supported());
+	assert(is_feat_amuv1p1_supported());
 	assert(idx < read_amcgcr_el0_cg0nc());
 	assert(idx != 1U);
 
@@ -403,7 +376,7 @@
 /* Read the group 1 counter identified by the given `idx` */
 static uint64_t amu_group1_cnt_read(unsigned int idx)
 {
-	assert(amu_supported());
+	assert(is_feat_amu_supported());
 	assert(amu_group1_supported());
 	assert(idx < read_amcgcr_el0_cg1nc());
 
@@ -413,7 +386,7 @@
 /* Write the group 1 counter identified by the given `idx` with `val` */
 static void amu_group1_cnt_write(unsigned int idx, uint64_t val)
 {
-	assert(amu_supported());
+	assert(is_feat_amu_supported());
 	assert(amu_group1_supported());
 	assert(idx < read_amcgcr_el0_cg1nc());
 
@@ -428,7 +401,7 @@
  */
 static uint64_t amu_group1_voffset_read(unsigned int idx)
 {
-	assert(amu_v1p1_supported());
+	assert(is_feat_amuv1p1_supported());
 	assert(amu_group1_supported());
 	assert(idx < read_amcgcr_el0_cg1nc());
 	assert((read_amcg1idr_el0_voff() & (UINT64_C(1) << idx)) != 0U);
@@ -443,7 +416,7 @@
  */
 static void amu_group1_voffset_write(unsigned int idx, uint64_t val)
 {
-	assert(amu_v1p1_supported());
+	assert(is_feat_amuv1p1_supported());
 	assert(amu_group1_supported());
 	assert(idx < read_amcgcr_el0_cg1nc());
 	assert((read_amcg1idr_el0_voff() & (UINT64_C(1) << idx)) != 0U);
@@ -460,8 +433,7 @@
 	unsigned int core_pos;
 	struct amu_ctx *ctx;
 
-	uint64_t id_aa64pfr0_el1_amu;	/* AMU version */
-	uint64_t hcr_el2_amvoffen;	/* AMU virtual offsets enabled */
+	uint64_t hcr_el2_amvoffen = 0;	/* AMU virtual offsets enabled */
 	uint64_t amcgcr_el0_cg0nc;	/* Number of group 0 counters */
 
 #if ENABLE_AMU_AUXILIARY_COUNTERS
@@ -470,8 +442,7 @@
 	uint64_t amcgcr_el0_cg1nc;	/* Number of group 1 counters */
 #endif
 
-	id_aa64pfr0_el1_amu = read_id_aa64pfr0_el1_amu();
-	if (id_aa64pfr0_el1_amu == ID_AA64PFR0_AMU_NOT_SUPPORTED) {
+	if (!is_feat_amu_supported()) {
 		return (void *)0;
 	}
 
@@ -479,8 +450,9 @@
 	ctx = &amu_ctxs_[core_pos];
 
 	amcgcr_el0_cg0nc = read_amcgcr_el0_cg0nc();
-	hcr_el2_amvoffen = (id_aa64pfr0_el1_amu >= ID_AA64PFR0_AMU_V1P1) ?
-		read_hcr_el2_amvoffen() : 0U;
+	if (is_feat_amuv1p1_supported()) {
+		hcr_el2_amvoffen = read_hcr_el2_amvoffen();
+	}
 
 #if ENABLE_AMU_AUXILIARY_COUNTERS
 	amcfgr_el0_ncg = read_amcfgr_el0_ncg();
@@ -552,9 +524,7 @@
 	unsigned int core_pos;
 	struct amu_ctx *ctx;
 
-	uint64_t id_aa64pfr0_el1_amu;	/* AMU version */
-
-	uint64_t hcr_el2_amvoffen;	/* AMU virtual offsets enabled */
+	uint64_t hcr_el2_amvoffen = 0;	/* AMU virtual offsets enabled */
 
 	uint64_t amcfgr_el0_ncg;	/* Number of counter groups */
 	uint64_t amcgcr_el0_cg0nc;	/* Number of group 0 counters */
@@ -564,8 +534,7 @@
 	uint64_t amcg1idr_el0_voff;	/* Auxiliary counters with virtual offsets */
 #endif
 
-	id_aa64pfr0_el1_amu = read_id_aa64pfr0_el1_amu();
-	if (id_aa64pfr0_el1_amu == ID_AA64PFR0_AMU_NOT_SUPPORTED) {
+	if (!is_feat_amu_supported()) {
 		return (void *)0;
 	}
 
@@ -575,8 +544,9 @@
 	amcfgr_el0_ncg = read_amcfgr_el0_ncg();
 	amcgcr_el0_cg0nc = read_amcgcr_el0_cg0nc();
 
-	hcr_el2_amvoffen = (id_aa64pfr0_el1_amu >= ID_AA64PFR0_AMU_V1P1) ?
-		read_hcr_el2_amvoffen() : 0U;
+	if (is_feat_amuv1p1_supported()) {
+		hcr_el2_amvoffen = read_hcr_el2_amvoffen();
+	}
 
 #if ENABLE_AMU_AUXILIARY_COUNTERS
 	amcgcr_el0_cg1nc = (amcfgr_el0_ncg > 0U) ? read_amcgcr_el0_cg1nc() : 0U;
diff --git a/lib/extensions/amu/amu.mk b/lib/extensions/amu/amu.mk
index 0d203cb..868ab12 100644
--- a/lib/extensions/amu/amu.mk
+++ b/lib/extensions/amu/amu.mk
@@ -10,8 +10,8 @@
 			lib/extensions/amu/${ARCH}/amu_helpers.S
 
 ifneq (${ENABLE_AMU_AUXILIARY_COUNTERS},0)
-        ifeq (${ENABLE_AMU},0)
-                $(error AMU auxiliary counter support (`ENABLE_AMU_AUXILIARY_COUNTERS`) requires AMU support (`ENABLE_AMU`))
+        ifeq (${ENABLE_FEAT_AMU},0)
+                $(error AMU auxiliary counter support (`ENABLE_AMU_AUXILIARY_COUNTERS`) requires AMU support (`ENABLE_FEAT_AMU`))
         endif
 endif
 
diff --git a/lib/extensions/mpam/mpam.c b/lib/extensions/mpam/mpam.c
index 884d480..62533fc 100644
--- a/lib/extensions/mpam/mpam.c
+++ b/lib/extensions/mpam/mpam.c
@@ -13,11 +13,6 @@
 
 void mpam_enable(bool el2_unused)
 {
-	/* Check if MPAM is implemented */
-	if (get_mpam_version() == 0U) {
-		return;
-	}
-
 	/*
 	 * Enable MPAM, and disable trapping to EL3 when lower ELs access their
 	 * own MPAM registers.
diff --git a/lib/extensions/sme/sme.c b/lib/extensions/sme/sme.c
index ec8cca8..29034fd 100644
--- a/lib/extensions/sme/sme.c
+++ b/lib/extensions/sme/sme.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,41 +7,19 @@
 #include <stdbool.h>
 
 #include <arch.h>
+#include <arch_features.h>
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/extensions/sme.h>
 #include <lib/extensions/sve.h>
 
-static bool feat_sme_supported(void)
-{
-	uint64_t features;
-
-	features = read_id_aa64pfr1_el1() >> ID_AA64PFR1_EL1_SME_SHIFT;
-	return (features & ID_AA64PFR1_EL1_SME_MASK) != 0U;
-}
-
-static bool feat_sme_fa64_supported(void)
-{
-	uint64_t features;
-
-	features = read_id_aa64smfr0_el1();
-	return (features & ID_AA64SMFR0_EL1_FA64_BIT) != 0U;
-}
-
 void sme_enable(cpu_context_t *context)
 {
 	u_register_t reg;
 	u_register_t cptr_el3;
 	el3_state_t *state;
 
-	/* Make sure SME is implemented in hardware before continuing. */
-	if (!feat_sme_supported()) {
-		/* Perhaps the hardware supports SVE only */
-		sve_enable(context);
-		return;
-	}
-
 	/* Get the context state. */
 	state = get_el3state_ctx(context);
 
@@ -66,7 +44,7 @@
 	 * using SMCR_EL2 and SMCR_EL1.
 	 */
 	reg = SMCR_ELX_LEN_MASK;
-	if (feat_sme_fa64_supported()) {
+	if (read_feat_sme_fa64_id_field() != 0U) {
 		VERBOSE("[SME] FA64 enabled\n");
 		reg |= SMCR_ELX_FA64_BIT;
 	}
@@ -85,13 +63,6 @@
 	u_register_t reg;
 	el3_state_t *state;
 
-	/* Make sure SME is implemented in hardware before continuing. */
-	if (!feat_sme_supported()) {
-		/* Perhaps the hardware supports SVE only */
-		sve_disable(context);
-		return;
-	}
-
 	/* Get the context state. */
 	state = get_el3state_ctx(context);
 
diff --git a/lib/extensions/spe/spe.c b/lib/extensions/spe/spe.c
index d747efc..b1fe39f 100644
--- a/lib/extensions/spe/spe.c
+++ b/lib/extensions/spe/spe.c
@@ -7,6 +7,7 @@
 #include <stdbool.h>
 
 #include <arch.h>
+#include <arch_features.h>
 #include <arch_helpers.h>
 #include <lib/el3_runtime/pubsub.h>
 #include <lib/extensions/spe.h>
@@ -20,21 +21,10 @@
 	__asm__ volatile("hint #17");
 }
 
-bool spe_supported(void)
-{
-	uint64_t features;
-
-	features = read_id_aa64dfr0_el1() >> ID_AA64DFR0_PMS_SHIFT;
-	return (features & ID_AA64DFR0_PMS_MASK) > 0ULL;
-}
-
 void spe_enable(bool el2_unused)
 {
 	uint64_t v;
 
-	if (!spe_supported())
-		return;
-
 	if (el2_unused) {
 		/*
 		 * MDCR_EL2.TPMS (ARM v8.2): Do not trap statistical
@@ -69,9 +59,6 @@
 {
 	uint64_t v;
 
-	if (!spe_supported())
-		return;
-
 	/* Drain buffered data */
 	psb_csync();
 	dsbnsh();
@@ -85,7 +72,7 @@
 
 static void *spe_drain_buffers_hook(const void *arg)
 {
-	if (!spe_supported())
+	if (!is_feat_spe_supported())
 		return (void *)-1;
 
 	/* Drain buffered data */
diff --git a/lib/extensions/sve/sve.c b/lib/extensions/sve/sve.c
index f7dcc76..f551ca7 100644
--- a/lib/extensions/sve/sve.c
+++ b/lib/extensions/sve/sve.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2023, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -22,22 +22,10 @@
  */
 #define CONVERT_SVE_LENGTH(x)	(((x / 128) - 1))
 
-static bool sve_supported(void)
-{
-	uint64_t features;
-
-	features = read_id_aa64pfr0_el1() >> ID_AA64PFR0_SVE_SHIFT;
-	return (features & ID_AA64PFR0_SVE_MASK) == 1U;
-}
-
 void sve_enable(cpu_context_t *context)
 {
 	u_register_t cptr_el3;
 
-	if (!sve_supported()) {
-		return;
-	}
-
 	cptr_el3 = read_ctx_reg(get_el3state_ctx(context), CTX_CPTR_EL3);
 
 	/* Enable access to SVE functionality for all ELs. */
@@ -54,11 +42,6 @@
 	u_register_t reg;
 	el3_state_t *state;
 
-	/* Make sure SME is implemented in hardware before continuing. */
-	if (!sve_supported()) {
-		return;
-	}
-
 	/* Get the context state. */
 	state = get_el3state_ctx(context);
 
diff --git a/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c b/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c
index 89b8029..b3f44b7 100644
--- a/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c
+++ b/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c
@@ -10,27 +10,16 @@
 #include <arch_helpers.h>
 #include <lib/extensions/sys_reg_trace.h>
 
-static bool sys_reg_trace_supported(void)
-{
-	uint32_t features;
-
-	features = read_id_dfr0() >> ID_DFR0_COPTRC_SHIFT;
-	return ((features & ID_DFR0_COPTRC_MASK) ==
-		ID_DFR0_COPTRC_SUPPORTED);
-}
-
 void sys_reg_trace_enable(void)
 {
 	uint32_t val;
 
-	if (sys_reg_trace_supported()) {
-		/*
-		 * NSACR.NSTRCDIS = b0
-		 * enable NS system register access to implemented trace
-		 * registers.
-		 */
-		val = read_nsacr();
-		val &= ~NSTRCDIS_BIT;
-		write_nsacr(val);
-	}
+	/*
+	 * NSACR.NSTRCDIS = b0
+	 * enable NS system register access to implemented trace
+	 * registers.
+	 */
+	val = read_nsacr();
+	val &= ~NSTRCDIS_BIT;
+	write_nsacr(val);
 }
diff --git a/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c b/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
index 960d698..e61cb90 100644
--- a/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
+++ b/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
@@ -10,28 +10,17 @@
 #include <arch_helpers.h>
 #include <lib/extensions/sys_reg_trace.h>
 
-static bool sys_reg_trace_supported(void)
-{
-	uint64_t features;
-
-	features = read_id_aa64dfr0_el1() >> ID_AA64DFR0_TRACEVER_SHIFT;
-	return ((features & ID_AA64DFR0_TRACEVER_MASK) ==
-		ID_AA64DFR0_TRACEVER_SUPPORTED);
-}
-
 void sys_reg_trace_enable(cpu_context_t *ctx)
 {
 	uint64_t val;
 
-	if (sys_reg_trace_supported()) {
-		/* Retrieve CPTR_EL3 value from the given context 'ctx',
-		 * and update CPTR_EL3.TTA bit to 0.
-		 * This function is called while switching context to NS to
-		 * allow system trace register access to NS-EL2 and NS-EL1
-		 * when NS-EL2 is implemented but not used.
-		 */
-		val = read_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3);
-		val &= ~TTA_BIT;
-		write_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3, val);
-	}
+	/* Retrieve CPTR_EL3 value from the given context 'ctx',
+	 * and update CPTR_EL3.TTA bit to 0.
+	 * This function is called while switching context to NS to
+	 * allow system trace register access to NS-EL2 and NS-EL1
+	 * when NS-EL2 is implemented but not used.
+	 */
+	val = read_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3);
+	val &= ~TTA_BIT;
+	write_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3, val);
 }
diff --git a/lib/psa/delegated_attestation.c b/lib/psa/delegated_attestation.c
index 399a3f1..a813e84 100644
--- a/lib/psa/delegated_attestation.c
+++ b/lib/psa/delegated_attestation.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2022-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
@@ -87,104 +87,90 @@
 };
 
 static const uint8_t platform_token[] = {
-	0xD2, 0x84, 0x43, 0xA1, 0x01, 0x26, 0xA0, 0x59,
-	0x02, 0xBE, 0xAA, 0x3A, 0x00, 0x01, 0x24, 0xFF,
-	0x58, 0x20, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB,
-	0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB,
-	0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB,
-	0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB,
-	0xAB, 0xAB, 0x3A, 0x00, 0x01, 0x24, 0xFB, 0x58,
-	0x20, 0xA0, 0xA1, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6,
-	0xA7, 0xA8, 0xA9, 0xAA, 0xAB, 0xAC, 0xAD, 0xAE,
-	0xAF, 0xB0, 0xB1, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6,
-	0xB7, 0xB8, 0xB9, 0xBA, 0xBB, 0xBC, 0xBD, 0xBE,
-	0xBF, 0x3A, 0x00, 0x01, 0x25, 0x00, 0x58, 0x21,
-	0x01, 0xFA, 0x58, 0x75, 0x5F, 0x65, 0x86, 0x27,
-	0xCE, 0x54, 0x60, 0xF2, 0x9B, 0x75, 0x29, 0x67,
-	0x13, 0x24, 0x8C, 0xAE, 0x7A, 0xD9, 0xE2, 0x98,
-	0x4B, 0x90, 0x28, 0x0E, 0xFC, 0xBC, 0xB5, 0x02,
-	0x48, 0x3A, 0x00, 0x01, 0x24, 0xFA, 0x58, 0x20,
-	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA,
-	0xBB, 0xBB, 0xBB, 0xBB, 0xBB, 0xBB, 0xBB, 0xBB,
-	0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC,
-	0xDD, 0xDD, 0xDD, 0xDD, 0xDD, 0xDD, 0xDD, 0xDD,
-	0x3A, 0x00, 0x01, 0x24, 0xF8, 0x20, 0x3A, 0x00,
-	0x01, 0x24, 0xF9, 0x00, 0x3A, 0x00, 0x01, 0x24,
-	0xFD, 0x85, 0xA5, 0x05, 0x58, 0x20, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x60,
-	0x01, 0x65, 0x42, 0x4C, 0x31, 0x5F, 0x32, 0x06,
-	0x66, 0x53, 0x48, 0x41, 0x32, 0x35, 0x36, 0x02,
-	0x58, 0x20, 0xF8, 0xB7, 0xCE, 0xAD, 0x9B, 0xE4,
-	0x5A, 0x8F, 0x5C, 0x52, 0x6F, 0x0C, 0x05, 0x25,
-	0x8F, 0xF3, 0xE9, 0x81, 0xDC, 0xBC, 0xF2, 0x05,
-	0x7F, 0x33, 0xF6, 0xBB, 0xDC, 0xD9, 0x4D, 0xA2,
-	0x34, 0x3A, 0xA5, 0x05, 0x58, 0x20, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x67,
-	0x31, 0x2E, 0x37, 0x2E, 0x32, 0x2B, 0x30, 0x01,
-	0x63, 0x42, 0x4C, 0x32, 0x06, 0x66, 0x53, 0x48,
-	0x41, 0x32, 0x35, 0x36, 0x02, 0x58, 0x20, 0x3A,
-	0xE5, 0x9E, 0x40, 0xA9, 0x6B, 0xD5, 0x29, 0x1C,
-	0xAB, 0x7A, 0x5F, 0xBD, 0x1F, 0x9A, 0xA6, 0x52,
-	0xFB, 0x77, 0x7D, 0xA3, 0xEC, 0x9C, 0x29, 0xBC,
-	0xE6, 0x5B, 0x3B, 0x43, 0xFC, 0x9D, 0x26, 0xA5,
-	0x05, 0x58, 0x20, 0xBF, 0xE6, 0xD8, 0x6F, 0x88,
-	0x26, 0xF4, 0xFF, 0x97, 0xFB, 0x96, 0xC4, 0xE6,
-	0xFB, 0xC4, 0x99, 0x3E, 0x46, 0x19, 0xFC, 0x56,
-	0x5D, 0xA2, 0x6A, 0xDF, 0x34, 0xC3, 0x29, 0x48,
-	0x9A, 0xDC, 0x38, 0x04, 0x67, 0x31, 0x2E, 0x35,
-	0x2E, 0x30, 0x2B, 0x30, 0x01, 0x64, 0x52, 0x54,
-	0x5F, 0x30, 0x06, 0x66, 0x53, 0x48, 0x41, 0x32,
-	0x35, 0x36, 0x02, 0x58, 0x20, 0x47, 0x94, 0x9D,
-	0x27, 0x33, 0x82, 0x45, 0x1A, 0xDD, 0x25, 0xF4,
-	0x9A, 0x89, 0x6F, 0x5F, 0xD9, 0xB0, 0xE8, 0x14,
-	0xD3, 0xA4, 0x9B, 0x53, 0xB0, 0x44, 0x0B, 0xCF,
-	0x32, 0x1A, 0xC4, 0xD2, 0x65, 0xA5, 0x05, 0x58,
-	0x20, 0xB3, 0x60, 0xCA, 0xF5, 0xC9, 0x8C, 0x6B,
-	0x94, 0x2A, 0x48, 0x82, 0xFA, 0x9D, 0x48, 0x23,
-	0xEF, 0xB1, 0x66, 0xA9, 0xEF, 0x6A, 0x6E, 0x4A,
-	0xA3, 0x7C, 0x19, 0x19, 0xED, 0x1F, 0xCC, 0xC0,
-	0x49, 0x04, 0x67, 0x30, 0x2E, 0x30, 0x2E, 0x37,
-	0x2B, 0x30, 0x01, 0x64, 0x52, 0x54, 0x5F, 0x31,
-	0x06, 0x66, 0x53, 0x48, 0x41, 0x32, 0x35, 0x36,
-	0x02, 0x58, 0x20, 0xCD, 0x38, 0xBE, 0xC8, 0xB7,
-	0xC0, 0x9E, 0xD5, 0x24, 0x30, 0xFE, 0xC8, 0xD0,
-	0x19, 0x12, 0x56, 0xB2, 0x7A, 0xA5, 0x53, 0x6F,
-	0xBC, 0x7D, 0x09, 0xCA, 0x11, 0xDD, 0x90, 0xD7,
-	0xD6, 0x70, 0xFD, 0xA5, 0x05, 0x58, 0x20, 0xAA,
-	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA,
-	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA,
-	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA,
-	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0x04,
-	0x60, 0x01, 0x60, 0x06, 0x66, 0x53, 0x48, 0x41,
-	0x32, 0x35, 0x36, 0x02, 0x58, 0x20, 0x28, 0x3D,
-	0x0C, 0x25, 0x22, 0x0C, 0x87, 0x46, 0xA0, 0x58,
-	0x64, 0x6C, 0x0B, 0x14, 0x37, 0x39, 0x40, 0x9D,
-	0x2D, 0x11, 0xD1, 0xCC, 0x54, 0x51, 0xB4, 0x29,
-	0x22, 0xCD, 0x70, 0x92, 0x71, 0xC3, 0x3A, 0x00,
-	0x01, 0x25, 0x01, 0x77, 0x77, 0x77, 0x77, 0x2E,
-	0x74, 0x72, 0x75, 0x73, 0x74, 0x65, 0x64, 0x66,
-	0x69, 0x72, 0x6D, 0x77, 0x61, 0x72, 0x65, 0x2E,
-	0x6F, 0x72, 0x67, 0x3A, 0x00, 0x01, 0x24, 0xF7,
-	0x71, 0x50, 0x53, 0x41, 0x5F, 0x49, 0x4F, 0x54,
-	0x5F, 0x50, 0x52, 0x4F, 0x46, 0x49, 0x4C, 0x45,
-	0x5F, 0x31, 0x3A, 0x00, 0x01, 0x24, 0xFC, 0x70,
-	0x30, 0x36, 0x30, 0x34, 0x35, 0x36, 0x35, 0x32,
-	0x37, 0x32, 0x38, 0x32, 0x39, 0x31, 0x30, 0x30,
-	0x58, 0x40, 0x1E, 0x0D, 0x2B, 0xD8, 0x7A, 0xC9,
-	0x2D, 0xCB, 0x73, 0xD1, 0x42, 0x2F, 0xBF, 0xDA,
-	0x24, 0x71, 0xE2, 0xAF, 0xEA, 0x48, 0x60, 0x17,
-	0x23, 0x75, 0x64, 0xAC, 0xCC, 0x23, 0xA2, 0x67,
-	0xC4, 0xE7, 0x8F, 0x1C, 0x7C, 0x68, 0x49, 0x42,
-	0x4D, 0xDA, 0xC6, 0xD6, 0x21, 0x1C, 0xAA, 0x00,
-	0xDA, 0x1E, 0x68, 0x56, 0xA3, 0x48, 0xEE, 0xA7,
-	0x92, 0xA9, 0x09, 0x83, 0x42, 0x04, 0x06, 0x9E,
-	0x62, 0xBB
+	0xD2, 0x84, 0x44, 0xA1, 0x01, 0x38, 0x22, 0xA0,
+	0x59, 0x02, 0x33, 0xA9, 0x19, 0x01, 0x09, 0x78,
+	0x1C, 0x68, 0x74, 0x74, 0x70, 0x3A, 0x2F, 0x2F,
+	0x61, 0x72, 0x6D, 0x2E, 0x63, 0x6F, 0x6D, 0x2F,
+	0x43, 0x43, 0x41, 0x2D, 0x53, 0x53, 0x44, 0x2F,
+	0x31, 0x2E, 0x30, 0x2E, 0x30, 0x0A, 0x58, 0x20,
+	0xB5, 0x97, 0x3C, 0xB6, 0x8B, 0xAA, 0x9F, 0xC5,
+	0x55, 0x58, 0x78, 0x6B, 0x7E, 0xC6, 0x7F, 0x69,
+	0xE4, 0x0D, 0xF5, 0xBA, 0x5A, 0xA9, 0x21, 0xCD,
+	0x0C, 0x27, 0xF4, 0x05, 0x87, 0xA0, 0x11, 0xEA,
+	0x19, 0x09, 0x5C, 0x58, 0x20, 0x7F, 0x45, 0x4C,
+	0x46, 0x02, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0x3E,
+	0x00, 0x01, 0x00, 0x00, 0x00, 0x50, 0x58, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x01, 0x00,
+	0x58, 0x21, 0x01, 0x07, 0x06, 0x05, 0x04, 0x03,
+	0x02, 0x01, 0x00, 0x0F, 0x0E, 0x0D, 0x0C, 0x0B,
+	0x0A, 0x09, 0x08, 0x17, 0x16, 0x15, 0x14, 0x13,
+	0x12, 0x11, 0x10, 0x1F, 0x1E, 0x1D, 0x1C, 0x1B,
+	0x1A, 0x19, 0x18, 0x19, 0x09, 0x61, 0x58, 0x21,
+	0x01, 0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x01,
+	0x00, 0x0F, 0x0E, 0x0D, 0x0C, 0x0B, 0x0A, 0x09,
+	0x08, 0x17, 0x16, 0x15, 0x14, 0x13, 0x12, 0x11,
+	0x10, 0x1F, 0x1E, 0x1D, 0x1C, 0x1B, 0x1A, 0x19,
+	0x18, 0x19, 0x09, 0x5B, 0x19, 0x30, 0x03, 0x19,
+	0x09, 0x62, 0x67, 0x73, 0x68, 0x61, 0x2D, 0x32,
+	0x35, 0x36, 0x19, 0x09, 0x5F, 0x84, 0xA5, 0x01,
+	0x62, 0x42, 0x4C, 0x05, 0x58, 0x20, 0x07, 0x06,
+	0x05, 0x04, 0x03, 0x02, 0x01, 0x00, 0x0F, 0x0E,
+	0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08, 0x17, 0x16,
+	0x15, 0x14, 0x13, 0x12, 0x11, 0x10, 0x1F, 0x1E,
+	0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18, 0x04, 0x65,
+	0x33, 0x2E, 0x34, 0x2E, 0x32, 0x02, 0x58, 0x20,
+	0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, 0x00,
+	0x0F, 0x0E, 0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08,
+	0x17, 0x16, 0x15, 0x14, 0x13, 0x12, 0x11, 0x10,
+	0x1F, 0x1E, 0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18,
+	0x06, 0x74, 0x54, 0x46, 0x2D, 0x4D, 0x5F, 0x53,
+	0x48, 0x41, 0x32, 0x35, 0x36, 0x4D, 0x65, 0x6D,
+	0x50, 0x72, 0x65, 0x58, 0x49, 0x50, 0xA4, 0x01,
+	0x62, 0x4D, 0x31, 0x05, 0x58, 0x20, 0x07, 0x06,
+	0x05, 0x04, 0x03, 0x02, 0x01, 0x00, 0x0F, 0x0E,
+	0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08, 0x17, 0x16,
+	0x15, 0x14, 0x13, 0x12, 0x11, 0x10, 0x1F, 0x1E,
+	0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18, 0x04, 0x63,
+	0x31, 0x2E, 0x32, 0x02, 0x58, 0x20, 0x07, 0x06,
+	0x05, 0x04, 0x03, 0x02, 0x01, 0x00, 0x0F, 0x0E,
+	0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08, 0x17, 0x16,
+	0x15, 0x14, 0x13, 0x12, 0x11, 0x10, 0x1F, 0x1E,
+	0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18, 0xA4, 0x01,
+	0x62, 0x4D, 0x32, 0x05, 0x58, 0x20, 0x07, 0x06,
+	0x05, 0x04, 0x03, 0x02, 0x01, 0x00, 0x0F, 0x0E,
+	0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08, 0x17, 0x16,
+	0x15, 0x14, 0x13, 0x12, 0x11, 0x10, 0x1F, 0x1E,
+	0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18, 0x04, 0x65,
+	0x31, 0x2E, 0x32, 0x2E, 0x33, 0x02, 0x58, 0x20,
+	0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, 0x00,
+	0x0F, 0x0E, 0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08,
+	0x17, 0x16, 0x15, 0x14, 0x13, 0x12, 0x11, 0x10,
+	0x1F, 0x1E, 0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18,
+	0xA4, 0x01, 0x62, 0x4D, 0x33, 0x05, 0x58, 0x20,
+	0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, 0x00,
+	0x0F, 0x0E, 0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08,
+	0x17, 0x16, 0x15, 0x14, 0x13, 0x12, 0x11, 0x10,
+	0x1F, 0x1E, 0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18,
+	0x04, 0x61, 0x31, 0x02, 0x58, 0x20, 0x07, 0x06,
+	0x05, 0x04, 0x03, 0x02, 0x01, 0x00, 0x0F, 0x0E,
+	0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08, 0x17, 0x16,
+	0x15, 0x14, 0x13, 0x12, 0x11, 0x10, 0x1F, 0x1E,
+	0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18, 0x19, 0x09,
+	0x60, 0x6C, 0x77, 0x68, 0x61, 0x74, 0x65, 0x76,
+	0x65, 0x72, 0x2E, 0x63, 0x6F, 0x6D, 0x58, 0x60,
+	0xE6, 0xB6, 0x38, 0x4F, 0xAE, 0x3F, 0x6E, 0x67,
+	0xF5, 0xD4, 0x97, 0x4B, 0x3F, 0xFD, 0x0A, 0xFA,
+	0x1D, 0xF0, 0x2F, 0x73, 0xB8, 0xFF, 0x5F, 0x02,
+	0xC0, 0x0F, 0x40, 0xAC, 0xF3, 0xA2, 0x9D, 0xB5,
+	0x31, 0x50, 0x16, 0x4F, 0xFA, 0x34, 0x3D, 0x0E,
+	0xAF, 0xE0, 0xD0, 0xD1, 0x6C, 0xF0, 0x9D, 0xC1,
+	0x01, 0x42, 0xA2, 0x3C, 0xCE, 0xD4, 0x4A, 0x59,
+	0xDC, 0x29, 0x0A, 0x30, 0x93, 0x5F, 0xB4, 0x98,
+	0x61, 0xBA, 0xE3, 0x91, 0x22, 0x95, 0x24, 0xF4,
+	0xAE, 0x47, 0x93, 0xD3, 0x84, 0xA3, 0x76, 0xD0,
+	0xC1, 0x26, 0x96, 0x53, 0xA3, 0x60, 0x3F, 0x6C,
+	0x75, 0x96, 0x90, 0x6A, 0xF9, 0x4E, 0xDA, 0x30
 };
 
 psa_status_t
diff --git a/lib/psci/psci_common.c b/lib/psci/psci_common.c
index f233be1..ebeb10b 100644
--- a/lib/psci/psci_common.c
+++ b/lib/psci/psci_common.c
@@ -76,6 +76,14 @@
 	(PLAT_MAX_PWR_LVL >= PSCI_CPU_PWR_LVL),
 	assert_platform_max_pwrlvl_check);
 
+#if PSCI_OS_INIT_MODE
+/*******************************************************************************
+ * The power state coordination mode used in CPU_SUSPEND.
+ * Defaults to platform-coordinated mode.
+ ******************************************************************************/
+suspend_mode_t psci_suspend_mode = PLAT_COORD;
+#endif
+
 /*
  * The plat_local_state used by the platform is one of these types: RUN,
  * RETENTION and OFF. The platform can define further sub-states for each type
@@ -153,8 +161,51 @@
 	psci_plat_pm_ops->get_sys_suspend_power_state(state_info);
 }
 
+#if PSCI_OS_INIT_MODE
 /*******************************************************************************
- * This function verifies that the all the other cores in the system have been
+ * This function verifies that all the other cores at the 'end_pwrlvl' have been
+ * idled and the current CPU is the last running CPU at the 'end_pwrlvl'.
+ * Returns 1 (true) if the current CPU is the last ON CPU or 0 (false)
+ * otherwise.
+ ******************************************************************************/
+static bool psci_is_last_cpu_to_idle_at_pwrlvl(unsigned int end_pwrlvl)
+{
+	unsigned int my_idx, lvl, parent_idx;
+	unsigned int cpu_start_idx, ncpus, cpu_idx;
+	plat_local_state_t local_state;
+
+	if (end_pwrlvl == PSCI_CPU_PWR_LVL) {
+		return true;
+	}
+
+	my_idx = plat_my_core_pos();
+
+	for (lvl = PSCI_CPU_PWR_LVL; lvl <= end_pwrlvl; lvl++) {
+		parent_idx = psci_cpu_pd_nodes[my_idx].parent_node;
+	}
+
+	cpu_start_idx = psci_non_cpu_pd_nodes[parent_idx].cpu_start_idx;
+	ncpus = psci_non_cpu_pd_nodes[parent_idx].ncpus;
+
+	for (cpu_idx = cpu_start_idx; cpu_idx < cpu_start_idx + ncpus;
+			cpu_idx++) {
+		local_state = psci_get_cpu_local_state_by_idx(cpu_idx);
+		if (cpu_idx == my_idx) {
+			assert(is_local_state_run(local_state) != 0);
+			continue;
+		}
+
+		if (is_local_state_run(local_state) != 0) {
+			return false;
+		}
+	}
+
+	return true;
+}
+#endif
+
+/*******************************************************************************
+ * This function verifies that all the other cores in the system have been
  * turned OFF and the current CPU is the last running CPU in the system.
  * Returns true, if the current CPU is the last ON CPU or false otherwise.
  ******************************************************************************/
@@ -179,6 +230,23 @@
 }
 
 /*******************************************************************************
+ * This function verifies that all cores in the system have been turned ON.
+ * Returns true, if all CPUs are ON or false otherwise.
+ ******************************************************************************/
+static bool psci_are_all_cpus_on(void)
+{
+	unsigned int cpu_idx;
+
+	for (cpu_idx = 0; cpu_idx < psci_plat_core_count; cpu_idx++) {
+		if (psci_get_aff_info_state_by_idx(cpu_idx) == AFF_STATE_OFF) {
+			return false;
+		}
+	}
+
+	return true;
+}
+
+/*******************************************************************************
  * Routine to return the maximum power level to traverse to after a cpu has
  * been physically powered up. It is expected to be called immediately after
  * reset from assembler code.
@@ -252,6 +320,60 @@
 	} else
 		return NULL;
 }
+
+#if PSCI_OS_INIT_MODE
+/******************************************************************************
+ * Helper function to save a copy of the psci_req_local_pwr_states (prev) for a
+ * CPU (cpu_idx), and update psci_req_local_pwr_states with the new requested
+ * local power states (state_info).
+ *****************************************************************************/
+void psci_update_req_local_pwr_states(unsigned int end_pwrlvl,
+				      unsigned int cpu_idx,
+				      psci_power_state_t *state_info,
+				      plat_local_state_t *prev)
+{
+	unsigned int lvl;
+#ifdef PLAT_MAX_CPU_SUSPEND_PWR_LVL
+	unsigned int max_pwrlvl = PLAT_MAX_CPU_SUSPEND_PWR_LVL;
+#else
+	unsigned int max_pwrlvl = PLAT_MAX_PWR_LVL;
+#endif
+	plat_local_state_t req_state;
+
+	for (lvl = PSCI_CPU_PWR_LVL + 1U; lvl <= max_pwrlvl; lvl++) {
+		/* Save the previous requested local power state */
+		prev[lvl - 1U] = *psci_get_req_local_pwr_states(lvl, cpu_idx);
+
+		/* Update the new requested local power state */
+		if (lvl <= end_pwrlvl) {
+			req_state = state_info->pwr_domain_state[lvl];
+		} else {
+			req_state = state_info->pwr_domain_state[end_pwrlvl];
+		}
+		psci_set_req_local_pwr_state(lvl, cpu_idx, req_state);
+	}
+}
+
+/******************************************************************************
+ * Helper function to restore the previously saved requested local power states
+ * (prev) for a CPU (cpu_idx) to psci_req_local_pwr_states.
+ *****************************************************************************/
+void psci_restore_req_local_pwr_states(unsigned int cpu_idx,
+				       plat_local_state_t *prev)
+{
+	unsigned int lvl;
+#ifdef PLAT_MAX_CPU_SUSPEND_PWR_LVL
+	unsigned int max_pwrlvl = PLAT_MAX_CPU_SUSPEND_PWR_LVL;
+#else
+	unsigned int max_pwrlvl = PLAT_MAX_PWR_LVL;
+#endif
+
+	for (lvl = PSCI_CPU_PWR_LVL + 1U; lvl <= max_pwrlvl; lvl++) {
+		/* Restore the previous requested local power state */
+		psci_set_req_local_pwr_state(lvl, cpu_idx, prev[lvl - 1U]);
+	}
+}
+#endif
 
 /*
  * psci_non_cpu_pd_nodes can be placed either in normal memory or coherent
@@ -399,6 +521,8 @@
 }
 
 /******************************************************************************
+ * This function is used in platform-coordinated mode.
+ *
  * This function is passed the local power states requested for each power
  * domain (state_info) between the current CPU domain and its ancestors until
  * the target power level (end_pwrlvl). It updates the array of requested power
@@ -476,6 +600,97 @@
 	psci_set_target_local_pwr_states(end_pwrlvl, state_info);
 }
 
+#if PSCI_OS_INIT_MODE
+/******************************************************************************
+ * This function is used in OS-initiated mode.
+ *
+ * This function is passed the local power states requested for each power
+ * domain (state_info) between the current CPU domain and its ancestors until
+ * the target power level (end_pwrlvl), and ensures the requested power states
+ * are valid. It updates the array of requested power states with this
+ * information.
+ *
+ * Then, for each level (apart from the CPU level) until the 'end_pwrlvl', it
+ * retrieves the states requested by all the cpus of which the power domain at
+ * that level is an ancestor. It passes this information to the platform to
+ * coordinate and return the target power state. If the requested state does
+ * not match the target state, the request is denied.
+ *
+ * The 'state_info' is not modified.
+ *
+ * This function will only be invoked with data cache enabled and while
+ * powering down a core.
+ *****************************************************************************/
+int psci_validate_state_coordination(unsigned int end_pwrlvl,
+				     psci_power_state_t *state_info)
+{
+	int rc = PSCI_E_SUCCESS;
+	unsigned int lvl, parent_idx, cpu_idx = plat_my_core_pos();
+	unsigned int start_idx;
+	unsigned int ncpus;
+	plat_local_state_t target_state, *req_states;
+	plat_local_state_t prev[PLAT_MAX_PWR_LVL];
+
+	assert(end_pwrlvl <= PLAT_MAX_PWR_LVL);
+	parent_idx = psci_cpu_pd_nodes[cpu_idx].parent_node;
+
+	/*
+	 * Save a copy of the previous requested local power states and update
+	 * the new requested local power states.
+	 */
+	psci_update_req_local_pwr_states(end_pwrlvl, cpu_idx, state_info, prev);
+
+	for (lvl = PSCI_CPU_PWR_LVL + 1U; lvl <= end_pwrlvl; lvl++) {
+		/* Get the requested power states for this power level */
+		start_idx = psci_non_cpu_pd_nodes[parent_idx].cpu_start_idx;
+		req_states = psci_get_req_local_pwr_states(lvl, start_idx);
+
+		/*
+		 * Let the platform coordinate amongst the requested states at
+		 * this power level and return the target local power state.
+		 */
+		ncpus = psci_non_cpu_pd_nodes[parent_idx].ncpus;
+		target_state = plat_get_target_pwr_state(lvl,
+							 req_states,
+							 ncpus);
+
+		/*
+		 * Verify that the requested power state matches the target
+		 * local power state.
+		 */
+		if (state_info->pwr_domain_state[lvl] != target_state) {
+			if (target_state == PSCI_LOCAL_STATE_RUN) {
+				rc = PSCI_E_DENIED;
+			} else {
+				rc = PSCI_E_INVALID_PARAMS;
+			}
+			goto exit;
+		}
+	}
+
+	/*
+	 * Verify that the current core is the last running core at the
+	 * specified power level.
+	 */
+	lvl = state_info->last_at_pwrlvl;
+	if (!psci_is_last_cpu_to_idle_at_pwrlvl(lvl)) {
+		rc = PSCI_E_DENIED;
+	}
+
+exit:
+	if (rc != PSCI_E_SUCCESS) {
+		/* Restore the previous requested local power states. */
+		psci_restore_req_local_pwr_states(cpu_idx, prev);
+		return rc;
+	}
+
+	/* Update the target state in the power domain nodes */
+	psci_set_target_local_pwr_states(end_pwrlvl, state_info);
+
+	return rc;
+}
+#endif
+
 /******************************************************************************
  * This function validates a suspend request by making sure that if a standby
  * state is requested then no power level is turned off and the highest power
@@ -1050,3 +1265,29 @@
 
 	return true;
 }
+
+/*******************************************************************************
+ * This function verifies that all cores in the system have been turned ON.
+ * Returns true, if all CPUs are ON or false otherwise.
+ *
+ * This API has following differences with psci_are_all_cpus_on
+ *  1. PSCI states are locked
+ ******************************************************************************/
+bool psci_are_all_cpus_on_safe(void)
+{
+	unsigned int this_core = plat_my_core_pos();
+	unsigned int parent_nodes[PLAT_MAX_PWR_LVL] = {0};
+
+	psci_get_parent_pwr_domain_nodes(this_core, PLAT_MAX_PWR_LVL, parent_nodes);
+
+	psci_acquire_pwr_domain_locks(PLAT_MAX_PWR_LVL, parent_nodes);
+
+	if (!psci_are_all_cpus_on()) {
+		psci_release_pwr_domain_locks(PLAT_MAX_PWR_LVL, parent_nodes);
+		return false;
+	}
+
+	psci_release_pwr_domain_locks(PLAT_MAX_PWR_LVL, parent_nodes);
+
+	return true;
+}
diff --git a/lib/psci/psci_main.c b/lib/psci/psci_main.c
index a631f3f..276c3a5 100644
--- a/lib/psci/psci_main.c
+++ b/lib/psci/psci_main.c
@@ -60,6 +60,10 @@
 	entry_point_info_t ep;
 	psci_power_state_t state_info = { {PSCI_LOCAL_STATE_RUN} };
 	plat_local_state_t cpu_pd_state;
+#if PSCI_OS_INIT_MODE
+	unsigned int cpu_idx = plat_my_core_pos();
+	plat_local_state_t prev[PLAT_MAX_PWR_LVL];
+#endif
 
 	/* Validate the power_state parameter */
 	rc = psci_validate_power_state(power_state, &state_info);
@@ -95,6 +99,18 @@
 		cpu_pd_state = state_info.pwr_domain_state[PSCI_CPU_PWR_LVL];
 		psci_set_cpu_local_state(cpu_pd_state);
 
+#if PSCI_OS_INIT_MODE
+		/*
+		 * If in OS-initiated mode, save a copy of the previous
+		 * requested local power states and update the new requested
+		 * local power states for this CPU.
+		 */
+		if (psci_suspend_mode == OS_INIT) {
+			psci_update_req_local_pwr_states(target_pwrlvl, cpu_idx,
+							 &state_info, prev);
+		}
+#endif
+
 #if ENABLE_PSCI_STAT
 		plat_psci_stat_accounting_start(&state_info);
 #endif
@@ -110,6 +126,16 @@
 		/* Upon exit from standby, set the state back to RUN. */
 		psci_set_cpu_local_state(PSCI_LOCAL_STATE_RUN);
 
+#if PSCI_OS_INIT_MODE
+		/*
+		 * If in OS-initiated mode, restore the previous requested
+		 * local power states for this CPU.
+		 */
+		if (psci_suspend_mode == OS_INIT) {
+			psci_restore_req_local_pwr_states(cpu_idx, prev);
+		}
+#endif
+
 #if ENABLE_RUNTIME_INSTRUMENTATION
 		PMF_CAPTURE_TIMESTAMP(rt_instr_svc,
 		    RT_INSTR_EXIT_HW_LOW_PWR,
@@ -142,12 +168,12 @@
 	 * might return if the power down was abandoned for any reason, e.g.
 	 * arrival of an interrupt
 	 */
-	psci_cpu_suspend_start(&ep,
-			    target_pwrlvl,
-			    &state_info,
-			    is_power_down_state);
+	rc = psci_cpu_suspend_start(&ep,
+				    target_pwrlvl,
+				    &state_info,
+				    is_power_down_state);
 
-	return PSCI_E_SUCCESS;
+	return rc;
 }
 
 
@@ -187,12 +213,12 @@
 	 * might return if the power down was abandoned for any reason, e.g.
 	 * arrival of an interrupt
 	 */
-	psci_cpu_suspend_start(&ep,
-			    PLAT_MAX_PWR_LVL,
-			    &state_info,
-			    PSTATE_TYPE_POWERDOWN);
+	rc = psci_cpu_suspend_start(&ep,
+				    PLAT_MAX_PWR_LVL,
+				    &state_info,
+				    PSTATE_TYPE_POWERDOWN);
 
-	return PSCI_E_SUCCESS;
+	return rc;
 }
 
 int psci_cpu_off(void)
@@ -357,19 +383,48 @@
 	/* Format the feature flags */
 	if ((psci_fid == PSCI_CPU_SUSPEND_AARCH32) ||
 	    (psci_fid == PSCI_CPU_SUSPEND_AARCH64)) {
-		/*
-		 * The trusted firmware does not support OS Initiated Mode.
-		 */
 		unsigned int ret = ((FF_PSTATE << FF_PSTATE_SHIFT) |
-			(((FF_SUPPORTS_OS_INIT_MODE == 1U) ? 0U : 1U)
-				<< FF_MODE_SUPPORT_SHIFT));
-		return (int) ret;
+			(FF_SUPPORTS_OS_INIT_MODE << FF_MODE_SUPPORT_SHIFT));
+		return (int)ret;
 	}
 
 	/* Return 0 for all other fid's */
 	return PSCI_E_SUCCESS;
 }
 
+#if PSCI_OS_INIT_MODE
+int psci_set_suspend_mode(unsigned int mode)
+{
+	if (psci_suspend_mode == mode) {
+		return PSCI_E_SUCCESS;
+	}
+
+	if (mode == PLAT_COORD) {
+		/* Check if the current CPU is the last ON CPU in the system */
+		if (!psci_is_last_on_cpu_safe()) {
+			return PSCI_E_DENIED;
+		}
+	}
+
+	if (mode == OS_INIT) {
+		/*
+		 * Check if all CPUs in the system are ON or if the current
+		 * CPU is the last ON CPU in the system.
+		 */
+		if (!(psci_are_all_cpus_on_safe() ||
+		      psci_is_last_on_cpu_safe())) {
+			return PSCI_E_DENIED;
+		}
+	}
+
+	psci_suspend_mode = mode;
+	psci_flush_dcache_range((uintptr_t)&psci_suspend_mode,
+				sizeof(psci_suspend_mode));
+
+	return PSCI_E_SUCCESS;
+}
+#endif
+
 /*******************************************************************************
  * PSCI top level handler for servicing SMCs.
  ******************************************************************************/
@@ -453,6 +508,12 @@
 			ret = (u_register_t)psci_features(r1);
 			break;
 
+#if PSCI_OS_INIT_MODE
+		case PSCI_SET_SUSPEND_MODE:
+			ret = (u_register_t)psci_set_suspend_mode(r1);
+			break;
+#endif
+
 #if ENABLE_PSCI_STAT
 		case PSCI_STAT_RESIDENCY_AARCH32:
 			ret = psci_stat_residency(r1, r2);
@@ -506,6 +567,10 @@
 			ret = psci_migrate_info_up_cpu();
 			break;
 
+		case PSCI_FEATURES:
+			ret = (u_register_t)psci_features(x1);
+			break;
+
 		case PSCI_NODE_HW_STATE_AARCH64:
 			ret = (u_register_t)psci_node_hw_state(
 					x1, (unsigned int) x2);
@@ -515,6 +580,12 @@
 			ret = (u_register_t)psci_system_suspend(x1, x2);
 			break;
 
+#if PSCI_OS_INIT_MODE
+		case PSCI_SET_SUSPEND_MODE:
+			ret = (u_register_t)psci_set_suspend_mode(x1);
+			break;
+#endif
+
 #if ENABLE_PSCI_STAT
 		case PSCI_STAT_RESIDENCY_AARCH64:
 			ret = psci_stat_residency(x1, (unsigned int) x2);
diff --git a/lib/psci/psci_private.h b/lib/psci/psci_private.h
index 6ca9ef6..b9987fe 100644
--- a/lib/psci/psci_private.h
+++ b/lib/psci/psci_private.h
@@ -163,6 +163,16 @@
 	spinlock_t cpu_lock;
 } cpu_pd_node_t;
 
+#if PSCI_OS_INIT_MODE
+/*******************************************************************************
+ * The supported power state coordination modes that can be used in CPU_SUSPEND.
+ ******************************************************************************/
+typedef enum suspend_mode {
+	PLAT_COORD = 0,
+	OS_INIT = 1
+} suspend_mode_t;
+#endif
+
 /*******************************************************************************
  * The following are helpers and declarations of locks.
  ******************************************************************************/
@@ -260,6 +270,9 @@
 extern cpu_pd_node_t psci_cpu_pd_nodes[PLATFORM_CORE_COUNT];
 extern unsigned int psci_caps;
 extern unsigned int psci_plat_core_count;
+#if PSCI_OS_INIT_MODE
+extern suspend_mode_t psci_suspend_mode;
+#endif
 
 /*******************************************************************************
  * SPD's power management hooks registered with PSCI
@@ -275,6 +288,14 @@
 void psci_query_sys_suspend_pwrstate(psci_power_state_t *state_info);
 int psci_validate_mpidr(u_register_t mpidr);
 void psci_init_req_local_pwr_states(void);
+#if PSCI_OS_INIT_MODE
+void psci_update_req_local_pwr_states(unsigned int end_pwrlvl,
+				      unsigned int cpu_idx,
+				      psci_power_state_t *state_info,
+				      plat_local_state_t *prev);
+void psci_restore_req_local_pwr_states(unsigned int cpu_idx,
+				       plat_local_state_t *prev);
+#endif
 void psci_get_target_local_pwr_states(unsigned int end_pwrlvl,
 				      psci_power_state_t *target_state);
 int psci_validate_entry_point(entry_point_info_t *ep,
@@ -284,6 +305,10 @@
 				      unsigned int *node_index);
 void psci_do_state_coordination(unsigned int end_pwrlvl,
 				psci_power_state_t *state_info);
+#if PSCI_OS_INIT_MODE
+int psci_validate_state_coordination(unsigned int end_pwrlvl,
+				     psci_power_state_t *state_info);
+#endif
 void psci_acquire_pwr_domain_locks(unsigned int end_pwrlvl,
 				   const unsigned int *parent_nodes);
 void psci_release_pwr_domain_locks(unsigned int end_pwrlvl,
@@ -317,10 +342,10 @@
 int psci_do_cpu_off(unsigned int end_pwrlvl);
 
 /* Private exported functions from psci_suspend.c */
-void psci_cpu_suspend_start(const entry_point_info_t *ep,
-			unsigned int end_pwrlvl,
-			psci_power_state_t *state_info,
-			unsigned int is_power_down_state);
+int psci_cpu_suspend_start(const entry_point_info_t *ep,
+			   unsigned int end_pwrlvl,
+			   psci_power_state_t *state_info,
+			   unsigned int is_power_down_state);
 
 void psci_cpu_suspend_finish(unsigned int cpu_idx, const psci_power_state_t *state_info);
 
diff --git a/lib/psci/psci_setup.c b/lib/psci/psci_setup.c
index 3cb4f7e..16d6e45 100644
--- a/lib/psci/psci_setup.c
+++ b/lib/psci/psci_setup.c
@@ -254,6 +254,9 @@
 			psci_caps |=  define_psci_cap(PSCI_CPU_SUSPEND_AARCH64);
 		if (psci_plat_pm_ops->get_sys_suspend_power_state != NULL)
 			psci_caps |=  define_psci_cap(PSCI_SYSTEM_SUSPEND_AARCH64);
+#if PSCI_OS_INIT_MODE
+		psci_caps |= define_psci_cap(PSCI_SET_SUSPEND_MODE);
+#endif
 	}
 	if (psci_plat_pm_ops->system_off != NULL)
 		psci_caps |=  define_psci_cap(PSCI_SYSTEM_OFF);
diff --git a/lib/psci/psci_suspend.c b/lib/psci/psci_suspend.c
index f71994d..861b875 100644
--- a/lib/psci/psci_suspend.c
+++ b/lib/psci/psci_suspend.c
@@ -75,6 +75,14 @@
 
 	PUBLISH_EVENT(psci_suspend_pwrdown_start);
 
+#if PSCI_OS_INIT_MODE
+#ifdef PLAT_MAX_CPU_SUSPEND_PWR_LVL
+	end_pwrlvl = PLAT_MAX_CPU_SUSPEND_PWR_LVL;
+#else
+	end_pwrlvl = PLAT_MAX_PWR_LVL;
+#endif
+#endif
+
 	/* Save PSCI target power level for the suspend finisher handler */
 	psci_set_suspend_pwrlvl(end_pwrlvl);
 
@@ -151,12 +159,13 @@
  * the state transition has been done, no further error is expected and it is
  * not possible to undo any of the actions taken beyond that point.
  ******************************************************************************/
-void psci_cpu_suspend_start(const entry_point_info_t *ep,
-			    unsigned int end_pwrlvl,
-			    psci_power_state_t *state_info,
-			    unsigned int is_power_down_state)
+int psci_cpu_suspend_start(const entry_point_info_t *ep,
+			   unsigned int end_pwrlvl,
+			   psci_power_state_t *state_info,
+			   unsigned int is_power_down_state)
 {
-	int skip_wfi = 0;
+	int rc = PSCI_E_SUCCESS;
+	bool skip_wfi = false;
 	unsigned int idx = plat_my_core_pos();
 	unsigned int parent_nodes[PLAT_MAX_PWR_LVL] = {0};
 
@@ -183,16 +192,32 @@
 	 * detection that a wake-up interrupt has fired.
 	 */
 	if (read_isr_el1() != 0U) {
-		skip_wfi = 1;
+		skip_wfi = true;
 		goto exit;
 	}
 
-	/*
-	 * This function is passed the requested state info and
-	 * it returns the negotiated state info for each power level upto
-	 * the end level specified.
-	 */
-	psci_do_state_coordination(end_pwrlvl, state_info);
+#if PSCI_OS_INIT_MODE
+	if (psci_suspend_mode == OS_INIT) {
+		/*
+		 * This function validates the requested state info for
+		 * OS-initiated mode.
+		 */
+		rc = psci_validate_state_coordination(end_pwrlvl, state_info);
+		if (rc != PSCI_E_SUCCESS) {
+			skip_wfi = true;
+			goto exit;
+		}
+	} else {
+#endif
+		/*
+		 * This function is passed the requested state info and
+		 * it returns the negotiated state info for each power level upto
+		 * the end level specified.
+		 */
+		psci_do_state_coordination(end_pwrlvl, state_info);
+#if PSCI_OS_INIT_MODE
+	}
+#endif
 
 #if ENABLE_PSCI_STAT
 	/* Update the last cpu for each level till end_pwrlvl */
@@ -208,7 +233,16 @@
 	 * platform defined mailbox with the psci entrypoint,
 	 * program the power controller etc.
 	 */
+
+#if PSCI_OS_INIT_MODE
+	rc = psci_plat_pm_ops->pwr_domain_suspend(state_info);
+	if (rc != PSCI_E_SUCCESS) {
+		skip_wfi = true;
+		goto exit;
+	}
+#else
 	psci_plat_pm_ops->pwr_domain_suspend(state_info);
+#endif
 
 #if ENABLE_PSCI_STAT
 	plat_psci_stat_accounting_start(state_info);
@@ -221,8 +255,9 @@
 	 */
 	psci_release_pwr_domain_locks(end_pwrlvl, parent_nodes);
 
-	if (skip_wfi == 1)
-		return;
+	if (skip_wfi) {
+		return rc;
+	}
 
 	if (is_power_down_state != 0U) {
 #if ENABLE_RUNTIME_INSTRUMENTATION
@@ -269,6 +304,8 @@
 	 * context retaining suspend finisher.
 	 */
 	psci_suspend_to_standby_finisher(idx, end_pwrlvl);
+
+	return rc;
 }
 
 /*******************************************************************************
diff --git a/make_helpers/build_macros.mk b/make_helpers/build_macros.mk
index 89f43f2..9ca6bdf 100644
--- a/make_helpers/build_macros.mk
+++ b/make_helpers/build_macros.mk
@@ -38,6 +38,18 @@
 $(eval uppercase_result:=$(call uppercase_internal,$(uppercase_table),$(1)))$(uppercase_result)
 endef
 
+# Convenience function for setting a variable to 0 if not previously set
+# $(eval $(call default_zero,FOO))
+define default_zero
+	$(eval $(1) ?= 0)
+endef
+
+# Convenience function for setting a list of variables to 0 if not previously set
+# $(eval $(call default_zeros,FOO BAR))
+define default_zeros
+	$(foreach var,$1,$(eval $(call default_zero,$(var))))
+endef
+
 # Convenience function for adding build definitions
 # $(eval $(call add_define,FOO)) will have:
 # -DFOO if $(FOO) is empty; -DFOO=$(FOO) otherwise
@@ -45,7 +57,6 @@
     DEFINES			+=	-D$(1)$(if $(value $(1)),=$(value $(1)),)
 endef
 
-
 # Convenience function for addding multiple build definitions
 # $(eval $(call add_defines,FOO BOO))
 define add_defines
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index 4f94cd0..63617b2 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -133,9 +133,6 @@
 # Use BRANCH_PROTECTION to enable PAUTH.
 ENABLE_PAUTH			:= 0
 
-# Flag to enable access to the HAFGRTR_EL2 register
-ENABLE_FEAT_AMUv1		:= 0
-
 # Flag to enable AMUv1p1 extension.
 ENABLE_FEAT_AMUv1p1		:= 0
 
@@ -176,6 +173,9 @@
 # Flag to enable delayed trapping of WFE instruction (FEAT_TWED)
 ENABLE_FEAT_TWED		:= 0
 
+# Flag to enable access to TCR2 (FEAT_TCR2)
+ENABLE_FEAT_TCR2		:= 0
+
 # By default BL31 encryption disabled
 ENCRYPT_BL31			:= 0
 
@@ -255,6 +255,9 @@
 # Flag used to choose the power state format: Extended State-ID or Original
 PSCI_EXTENDED_STATE_ID		:= 0
 
+# Enable PSCI OS-initiated mode support
+PSCI_OS_INIT_MODE		:= 0
+
 # Enable RAS support
 RAS_EXTENSION			:= 0
 
@@ -352,11 +355,11 @@
 WARMBOOT_ENABLE_DCACHE_EARLY	:= 0
 
 # Build option to enable/disable the Statistical Profiling Extensions
-ENABLE_SPE_FOR_LOWER_ELS	:= 1
+ENABLE_SPE_FOR_NS		:= 2
 
 # SPE is only supported on AArch64 so disable it on AArch32.
 ifeq (${ARCH},aarch32)
-	override ENABLE_SPE_FOR_LOWER_ELS := 0
+	override ENABLE_SPE_FOR_NS := 0
 endif
 
 # Include Memory Tagging Extension registers in cpu context. This must be set
@@ -364,13 +367,13 @@
 # enabled at ELX.
 CTX_INCLUDE_MTE_REGS		:= 0
 
-ENABLE_AMU			:= 0
+ENABLE_FEAT_AMU			:= 0
 ENABLE_AMU_AUXILIARY_COUNTERS	:= 0
 ENABLE_AMU_FCONF		:= 0
 AMU_RESTRICT_COUNTERS		:= 0
 
 # Enable SVE for non-secure world by default
-ENABLE_SVE_FOR_NS		:= 1
+ENABLE_SVE_FOR_NS		:= 2
 # SVE is only supported on AArch64 so disable it on AArch32.
 ifeq (${ARCH},aarch32)
 	override ENABLE_SVE_FOR_NS	:= 0
@@ -385,7 +388,7 @@
 ENABLE_SME_FOR_SWD		:= 0
 
 # If SME is enabled then force SVE off
-ifeq (${ENABLE_SME_FOR_NS},1)
+ifneq (${ENABLE_SME_FOR_NS},0)
 	override ENABLE_SVE_FOR_NS	:= 0
 	override ENABLE_SVE_FOR_SWD	:= 0
 endif
diff --git a/plat/allwinner/common/allwinner-common.mk b/plat/allwinner/common/allwinner-common.mk
index 61c1dbe..3164a25 100644
--- a/plat/allwinner/common/allwinner-common.mk
+++ b/plat/allwinner/common/allwinner-common.mk
@@ -87,7 +87,7 @@
 COLD_BOOT_SINGLE_CPU		:=	1
 
 # Do not enable SPE (not supported on ARM v8.0).
-ENABLE_SPE_FOR_LOWER_ELS	:=	0
+ENABLE_SPE_FOR_NS		:=	0
 
 # Do not enable SVE (not supported on ARM v8.0).
 ENABLE_SVE_FOR_NS		:=	0
diff --git a/plat/arm/board/arm_fpga/fpga_bl31_setup.c b/plat/arm/board/arm_fpga/fpga_bl31_setup.c
index e1b3abb..1a38f25 100644
--- a/plat/arm/board/arm_fpga/fpga_bl31_setup.c
+++ b/plat/arm/board/arm_fpga/fpga_bl31_setup.c
@@ -7,6 +7,7 @@
 #include <assert.h>
 #include <errno.h>
 
+#include <arch_features.h>
 #include <common/fdt_fixup.h>
 #include <common/fdt_wrappers.h>
 #include <drivers/arm/gicv3.h>
@@ -364,7 +365,7 @@
 	fpga_dtb_update_clock(fdt, system_freq);
 
 	/* Check whether we support the SPE PMU. Remove the DT node if not. */
-	if (!spe_supported()) {
+	if (!is_feat_spe_supported()) {
 		int node = fdt_node_offset_by_compatible(fdt, 0,
 				     "arm,statistical-profiling-extension-v1");
 
diff --git a/plat/arm/board/arm_fpga/platform.mk b/plat/arm/board/arm_fpga/platform.mk
index a14a0d8..109bfbe 100644
--- a/plat/arm/board/arm_fpga/platform.mk
+++ b/plat/arm/board/arm_fpga/platform.mk
@@ -33,7 +33,7 @@
 FPGA_PRELOADED_CMD_LINE := 0x1000
 $(eval $(call add_define,FPGA_PRELOADED_CMD_LINE))
 
-ENABLE_AMU		:=	1
+ENABLE_FEAT_AMU		:=	2
 
 # Treating this as a memory-constrained port for now
 USE_COHERENT_MEM	:=	0
diff --git a/plat/arm/board/fvp/fvp_bl2_measured_boot.c b/plat/arm/board/fvp/fvp_bl2_measured_boot.c
index 29b6619..7b0673a 100644
--- a/plat/arm/board/fvp/fvp_bl2_measured_boot.c
+++ b/plat/arm/board/fvp/fvp_bl2_measured_boot.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -15,6 +15,11 @@
 #include <plat/arm/common/plat_arm.h>
 #include <plat/common/common_def.h>
 
+#if defined(SPD_tspd) || defined(SPD_opteed) || defined(SPD_spmd)
+CASSERT(ARM_EVENT_LOG_DRAM1_SIZE >= PLAT_ARM_EVENT_LOG_MAX_SIZE, \
+	assert_res_eventlog_mem_insufficient);
+#endif /* defined(SPD_tspd) || defined(SPD_opteed) || defined(SPD_spmd) */
+
 /* Event Log data */
 static uint64_t event_log_base;
 
@@ -195,9 +200,19 @@
 
 	event_log_cur_size = event_log_get_cur_size((uint8_t *)event_log_base);
 
+#if defined(SPD_tspd) || defined(SPD_opteed) || defined(SPD_spmd)
+	/* Copy Event Log to TZC secured DRAM memory */
+	(void)memcpy((void *)ARM_EVENT_LOG_DRAM1_BASE,
+		     (const void *)event_log_base,
+		     event_log_cur_size);
+
+	/* Ensure that the Event Log is visible in TZC secured DRAM memory */
+	flush_dcache_range(ARM_EVENT_LOG_DRAM1_BASE, event_log_cur_size);
+#endif /* defined(SPD_tspd) || defined(SPD_opteed) || defined(SPD_spmd) */
+
 	rc = arm_set_nt_fw_info(
 #ifdef SPD_opteed
-			    (uintptr_t)event_log_base,
+			    (uintptr_t)ARM_EVENT_LOG_DRAM1_BASE,
 #endif
 			    event_log_cur_size, &ns_log_addr);
 	if (rc != 0) {
@@ -222,7 +237,7 @@
 
 #if defined(SPD_tspd) || defined(SPD_spmd)
 	/* Set Event Log data in TOS_FW_CONFIG */
-	rc = arm_set_tos_fw_info((uintptr_t)event_log_base,
+	rc = arm_set_tos_fw_info((uintptr_t)ARM_EVENT_LOG_DRAM1_BASE,
 				 event_log_cur_size);
 	if (rc != 0) {
 		ERROR("%s(): Unable to update %s_FW_CONFIG\n",
diff --git a/plat/arm/board/fvp/fvp_common.c b/plat/arm/board/fvp/fvp_common.c
index f500f3a..283918b 100644
--- a/plat/arm/board/fvp/fvp_common.c
+++ b/plat/arm/board/fvp/fvp_common.c
@@ -111,6 +111,15 @@
 	 * Required to load HW_CONFIG, SPMC and SPs to trusted DRAM.
 	 */
 	ARM_MAP_TRUSTED_DRAM,
+
+	/*
+	 * Required to load Event Log in TZC secured memory
+	 */
+#if MEASURED_BOOT && (defined(SPD_tspd) || defined(SPD_opteed) || \
+defined(SPD_spmd))
+	ARM_MAP_EVENT_LOG_DRAM1,
+#endif /* MEASURED_BOOT && (SPD_tspd || SPD_opteed || SPD_spmd) */
+
 #if ENABLE_RME
 	ARM_MAP_RMM_DRAM,
 	ARM_MAP_GPT_L1_DRAM,
diff --git a/plat/arm/board/fvp/fvp_plat_attest_token.c b/plat/arm/board/fvp/fvp_plat_attest_token.c
index dda2156..5af2405 100644
--- a/plat/arm/board/fvp/fvp_plat_attest_token.c
+++ b/plat/arm/board/fvp/fvp_plat_attest_token.c
@@ -1,113 +1,26 @@
 /*
- * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <errno.h>
-#include <stdint.h>
-#include <string.h>
+#include <delegated_attestation.h>
+#include <psa/error.h>
 
-/* Using hardcoded token values for AEM FVP */
-static uint8_t platform_token[] = {
-	0xD2, 0x84, 0x44, 0xA1, 0x01, 0x38, 0x22, 0xA0,
-	0x59, 0x02, 0x33, 0xA9, 0x19, 0x01, 0x09, 0x78,
-	0x1C, 0x68, 0x74, 0x74, 0x70, 0x3A, 0x2F, 0x2F,
-	0x61, 0x72, 0x6D, 0x2E, 0x63, 0x6F, 0x6D, 0x2F,
-	0x43, 0x43, 0x41, 0x2D, 0x53, 0x53, 0x44, 0x2F,
-	0x31, 0x2E, 0x30, 0x2E, 0x30, 0x0A, 0x58, 0x20,
-	0xB5, 0x97, 0x3C, 0xB6, 0x8B, 0xAA, 0x9F, 0xC5,
-	0x55, 0x58, 0x78, 0x6B, 0x7E, 0xC6, 0x7F, 0x69,
-	0xE4, 0x0D, 0xF5, 0xBA, 0x5A, 0xA9, 0x21, 0xCD,
-	0x0C, 0x27, 0xF4, 0x05, 0x87, 0xA0, 0x11, 0xEA,
-	0x19, 0x09, 0x5C, 0x58, 0x20, 0x7F, 0x45, 0x4C,
-	0x46, 0x02, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0x3E,
-	0x00, 0x01, 0x00, 0x00, 0x00, 0x50, 0x58, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x01, 0x00,
-	0x58, 0x21, 0x01, 0x07, 0x06, 0x05, 0x04, 0x03,
-	0x02, 0x01, 0x00, 0x0F, 0x0E, 0x0D, 0x0C, 0x0B,
-	0x0A, 0x09, 0x08, 0x17, 0x16, 0x15, 0x14, 0x13,
-	0x12, 0x11, 0x10, 0x1F, 0x1E, 0x1D, 0x1C, 0x1B,
-	0x1A, 0x19, 0x18, 0x19, 0x09, 0x61, 0x58, 0x21,
-	0x01, 0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x01,
-	0x00, 0x0F, 0x0E, 0x0D, 0x0C, 0x0B, 0x0A, 0x09,
-	0x08, 0x17, 0x16, 0x15, 0x14, 0x13, 0x12, 0x11,
-	0x10, 0x1F, 0x1E, 0x1D, 0x1C, 0x1B, 0x1A, 0x19,
-	0x18, 0x19, 0x09, 0x5B, 0x19, 0x30, 0x03, 0x19,
-	0x09, 0x62, 0x67, 0x73, 0x68, 0x61, 0x2D, 0x32,
-	0x35, 0x36, 0x19, 0x09, 0x5F, 0x84, 0xA5, 0x01,
-	0x62, 0x42, 0x4C, 0x05, 0x58, 0x20, 0x07, 0x06,
-	0x05, 0x04, 0x03, 0x02, 0x01, 0x00, 0x0F, 0x0E,
-	0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08, 0x17, 0x16,
-	0x15, 0x14, 0x13, 0x12, 0x11, 0x10, 0x1F, 0x1E,
-	0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18, 0x04, 0x65,
-	0x33, 0x2E, 0x34, 0x2E, 0x32, 0x02, 0x58, 0x20,
-	0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, 0x00,
-	0x0F, 0x0E, 0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08,
-	0x17, 0x16, 0x15, 0x14, 0x13, 0x12, 0x11, 0x10,
-	0x1F, 0x1E, 0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18,
-	0x06, 0x74, 0x54, 0x46, 0x2D, 0x4D, 0x5F, 0x53,
-	0x48, 0x41, 0x32, 0x35, 0x36, 0x4D, 0x65, 0x6D,
-	0x50, 0x72, 0x65, 0x58, 0x49, 0x50, 0xA4, 0x01,
-	0x62, 0x4D, 0x31, 0x05, 0x58, 0x20, 0x07, 0x06,
-	0x05, 0x04, 0x03, 0x02, 0x01, 0x00, 0x0F, 0x0E,
-	0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08, 0x17, 0x16,
-	0x15, 0x14, 0x13, 0x12, 0x11, 0x10, 0x1F, 0x1E,
-	0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18, 0x04, 0x63,
-	0x31, 0x2E, 0x32, 0x02, 0x58, 0x20, 0x07, 0x06,
-	0x05, 0x04, 0x03, 0x02, 0x01, 0x00, 0x0F, 0x0E,
-	0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08, 0x17, 0x16,
-	0x15, 0x14, 0x13, 0x12, 0x11, 0x10, 0x1F, 0x1E,
-	0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18, 0xA4, 0x01,
-	0x62, 0x4D, 0x32, 0x05, 0x58, 0x20, 0x07, 0x06,
-	0x05, 0x04, 0x03, 0x02, 0x01, 0x00, 0x0F, 0x0E,
-	0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08, 0x17, 0x16,
-	0x15, 0x14, 0x13, 0x12, 0x11, 0x10, 0x1F, 0x1E,
-	0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18, 0x04, 0x65,
-	0x31, 0x2E, 0x32, 0x2E, 0x33, 0x02, 0x58, 0x20,
-	0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, 0x00,
-	0x0F, 0x0E, 0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08,
-	0x17, 0x16, 0x15, 0x14, 0x13, 0x12, 0x11, 0x10,
-	0x1F, 0x1E, 0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18,
-	0xA4, 0x01, 0x62, 0x4D, 0x33, 0x05, 0x58, 0x20,
-	0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, 0x00,
-	0x0F, 0x0E, 0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08,
-	0x17, 0x16, 0x15, 0x14, 0x13, 0x12, 0x11, 0x10,
-	0x1F, 0x1E, 0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18,
-	0x04, 0x61, 0x31, 0x02, 0x58, 0x20, 0x07, 0x06,
-	0x05, 0x04, 0x03, 0x02, 0x01, 0x00, 0x0F, 0x0E,
-	0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08, 0x17, 0x16,
-	0x15, 0x14, 0x13, 0x12, 0x11, 0x10, 0x1F, 0x1E,
-	0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18, 0x19, 0x09,
-	0x60, 0x6C, 0x77, 0x68, 0x61, 0x74, 0x65, 0x76,
-	0x65, 0x72, 0x2E, 0x63, 0x6F, 0x6D, 0x58, 0x60,
-	0xE6, 0xB6, 0x38, 0x4F, 0xAE, 0x3F, 0x6E, 0x67,
-	0xF5, 0xD4, 0x97, 0x4B, 0x3F, 0xFD, 0x0A, 0xFA,
-	0x1D, 0xF0, 0x2F, 0x73, 0xB8, 0xFF, 0x5F, 0x02,
-	0xC0, 0x0F, 0x40, 0xAC, 0xF3, 0xA2, 0x9D, 0xB5,
-	0x31, 0x50, 0x16, 0x4F, 0xFA, 0x34, 0x3D, 0x0E,
-	0xAF, 0xE0, 0xD0, 0xD1, 0x6C, 0xF0, 0x9D, 0xC1,
-	0x01, 0x42, 0xA2, 0x3C, 0xCE, 0xD4, 0x4A, 0x59,
-	0xDC, 0x29, 0x0A, 0x30, 0x93, 0x5F, 0xB4, 0x98,
-	0x61, 0xBA, 0xE3, 0x91, 0x22, 0x95, 0x24, 0xF4,
-	0xAE, 0x47, 0x93, 0xD3, 0x84, 0xA3, 0x76, 0xD0,
-	0xC1, 0x26, 0x96, 0x53, 0xA3, 0x60, 0x3F, 0x6C,
-	0x75, 0x96, 0x90, 0x6A, 0xF9, 0x4E, 0xDA, 0x30
-};
-
+/*
+ * Get the platform attestation token through the PSA delegated attestation
+ * layer.
+ *
+ * FVP cannot support RSS hardware at the moment, but it can still mock the
+ * RSS implementation of the PSA interface (see PLAT_RSS_NOT_SUPPORTED).
+ */
 int plat_rmmd_get_cca_attest_token(uintptr_t buf, size_t *len,
 				   uintptr_t hash, size_t hash_size)
 {
-	(void)hash;
-	(void)hash_size;
+	psa_status_t ret;
 
-	if (*len < sizeof(platform_token)) {
-		return -EINVAL;
-	}
-
-	(void)memcpy((void *)buf, platform_token, sizeof(platform_token));
-	*len = sizeof(platform_token);
+	ret = rss_delegated_attest_get_token((const uint8_t *)hash, hash_size,
+					     (uint8_t *)buf, *len, len);
 
-	return 0;
+	return ret;
 }
diff --git a/plat/arm/board/fvp/fvp_pm.c b/plat/arm/board/fvp/fvp_pm.c
index 6b9d618..a3289b6 100644
--- a/plat/arm/board/fvp/fvp_pm.c
+++ b/plat/arm/board/fvp/fvp_pm.c
@@ -6,6 +6,7 @@
 
 #include <assert.h>
 
+#include <arch_features.h>
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <drivers/arm/gicv3.h>
@@ -53,13 +54,13 @@
 {
 	uint64_t mpidr = read_mpidr_el1();
 
-#if ENABLE_SPE_FOR_LOWER_ELS
 	/*
 	 * On power down we need to disable statistical profiling extensions
 	 * before exiting coherency.
 	 */
-	spe_disable();
-#endif
+	if (is_feat_spe_supported()) {
+		spe_disable();
+	}
 
 	/* Disable coherency if this cluster is to be turned off */
 	fvp_interconnect_disable();
@@ -227,7 +228,11 @@
  * FVP handler called when a power domain is about to be suspended. The
  * target_state encodes the power state that each level should transition to.
  ******************************************************************************/
+#if PSCI_OS_INIT_MODE
+static int fvp_pwr_domain_suspend(const psci_power_state_t *target_state)
+#else
 static void fvp_pwr_domain_suspend(const psci_power_state_t *target_state)
+#endif
 {
 	unsigned long mpidr;
 
@@ -237,7 +242,11 @@
 	 */
 	if (target_state->pwr_domain_state[ARM_PWR_LVL0] ==
 					ARM_LOCAL_STATE_RET)
+#if PSCI_OS_INIT_MODE
+		return PSCI_E_SUCCESS;
+#else
 		return;
+#endif
 
 	assert(target_state->pwr_domain_state[ARM_PWR_LVL0] ==
 					ARM_LOCAL_STATE_OFF);
@@ -269,6 +278,12 @@
 
 	/* Program the power controller to power off this cpu. */
 	fvp_pwrc_write_ppoffr(read_mpidr_el1());
+
+#if PSCI_OS_INIT_MODE
+	return PSCI_E_SUCCESS;
+#else
+	return;
+#endif
 }
 
 /*******************************************************************************
diff --git a/plat/arm/board/fvp/fvp_realm_attest_key.c b/plat/arm/board/fvp/fvp_realm_attest_key.c
index 1af1f0d..26354f4 100644
--- a/plat/arm/board/fvp/fvp_realm_attest_key.c
+++ b/plat/arm/board/fvp/fvp_realm_attest_key.c
@@ -1,36 +1,30 @@
 /*
- * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #include <assert.h>
-#include <errno.h>
-#include <stddef.h>
-#include <stdint.h>
-#include <string.h>
+#include <delegated_attestation.h>
+#include <psa/error.h>
 #include <services/rmmd_svc.h>
 
-static uint8_t sample_attest_priv_key[] = {
-	0x20, 0x11, 0xC7, 0xF0, 0x3C, 0xEE, 0x43, 0x25, 0x17, 0x6E,
-	0x52, 0x4F, 0x03, 0x3C, 0x0C, 0xE1, 0xE2, 0x1A, 0x76, 0xE6,
-	0xC1, 0xA4, 0xF0, 0xB8, 0x39, 0xAA, 0x1D, 0xF6, 0x1E, 0x0E,
-	0x8A, 0x5C, 0x8A, 0x05, 0x74, 0x0F, 0x9B, 0x69, 0xEF, 0xA7,
-	0xEB, 0x1A, 0x41, 0x85, 0xBD, 0x11, 0x7F, 0x68
-};
-
+/*
+ * Get the delegated realm attestation key through the PSA delegated
+ * attestation layer.
+ *
+ * FVP cannot support RSS hardware at the moment, but it can still mock
+ * the RSS implementation of the PSA interface (see PLAT_RSS_NOT_SUPPORTED).
+ */
 int plat_rmmd_get_cca_realm_attest_key(uintptr_t buf, size_t *len,
 				       unsigned int type)
 {
-	assert(type == ATTEST_KEY_CURVE_ECC_SECP384R1);
+	psa_status_t ret;
 
-	if (*len < sizeof(sample_attest_priv_key)) {
-		return -EINVAL;
-	}
+	assert(type == ATTEST_KEY_CURVE_ECC_SECP384R1);
 
-	(void)memcpy((void *)buf, sample_attest_priv_key,
-		     sizeof(sample_attest_priv_key));
-	*len = sizeof(sample_attest_priv_key);
+	ret = rss_delegated_attest_get_delegated_key(0U, 0U, (uint8_t *)buf,
+						     *len, len, 0U);
 
-	return 0;
+	return ret;
 }
diff --git a/plat/arm/board/fvp/include/platform_def.h b/plat/arm/board/fvp/include/platform_def.h
index 52146a3..84e2e82 100644
--- a/plat/arm/board/fvp/include/platform_def.h
+++ b/plat/arm/board/fvp/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2022, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -30,6 +30,10 @@
 
 #define PLAT_MAX_PWR_LVL		ARM_PWR_LVL2
 
+#if PSCI_OS_INIT_MODE
+#define PLAT_MAX_CPU_SUSPEND_PWR_LVL	ARM_PWR_LVL1
+#endif
+
 /*
  * Other platform porting definitions are provided by included headers
  */
@@ -168,7 +172,12 @@
 # define MAX_XLAT_TABLES		5
 #else
 # define PLAT_ARM_MMAP_ENTRIES		12
-# define MAX_XLAT_TABLES		6
+# if (defined(SPD_tspd) || defined(SPD_opteed) || defined(SPD_spmd)) && \
+defined(IMAGE_BL2) && MEASURED_BOOT
+#  define MAX_XLAT_TABLES		7
+# else
+#  define MAX_XLAT_TABLES		6
+# endif /* (SPD_tspd || SPD_opteed || SPD_spmd) && IMAGE_BL2 && MEASURED_BOOT */
 #endif
 
 /*
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index 6e95fdc..caac8d4 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -102,7 +102,8 @@
 				plat/arm/common/arm_tzc400.c
 
 
-PLAT_INCLUDES		:=	-Iplat/arm/board/fvp/include
+PLAT_INCLUDES		:=	-Iplat/arm/board/fvp/include		\
+				-Iinclude/lib/psa
 
 
 PLAT_BL_COMMON_SOURCES	:=	plat/arm/board/fvp/fvp_common.c
@@ -154,7 +155,8 @@
 endif
 
 else
-FVP_CPU_LIBS		+=	lib/cpus/aarch32/cortex_a32.S
+FVP_CPU_LIBS		+=	lib/cpus/aarch32/cortex_a32.S			\
+				lib/cpus/aarch32/cortex_a57.S
 endif
 
 BL1_SOURCES		+=	drivers/arm/smmu/smmu_v3.c			\
@@ -195,8 +197,14 @@
 
 ifeq (${ENABLE_RME},1)
 BL2_SOURCES		+=	plat/arm/board/fvp/aarch64/fvp_helpers.S
+
 BL31_SOURCES		+=	plat/arm/board/fvp/fvp_plat_attest_token.c	\
 				plat/arm/board/fvp/fvp_realm_attest_key.c
+
+# FVP platform does not support RSS, but it can leverage RSS APIs to
+# provide hardcoded token/key on request.
+BL31_SOURCES		+=	lib/psa/delegated_attestation.c
+
 endif
 
 ifeq (${ENABLE_FEAT_RNG_TRAP},1)
@@ -311,12 +319,13 @@
 endif
 
 # Enable Activity Monitor Unit extensions by default
-ENABLE_AMU			:=	1
+ENABLE_FEAT_AMU			:=	2
+ENABLE_FEAT_AMUv1p1		:=	2
 
 # Enable dynamic mitigation support by default
 DYNAMIC_WORKAROUND_CVE_2018_3639	:=	1
 
-ifeq (${ENABLE_AMU},1)
+ifneq (${ENABLE_FEAT_AMU},0)
 BL31_SOURCES		+=	lib/cpus/aarch64/cpuamu.c		\
 				lib/cpus/aarch64/cpuamu_helpers.S
 
@@ -369,6 +378,10 @@
     override BL1_SOURCES =
 endif
 
+# RSS is not supported on FVP right now. Thus, we use the mocked version
+# of the provided PSA APIs. They return with success and hard-coded token/key.
+PLAT_RSS_NOT_SUPPORTED	:= 1
+
 # Include Measured Boot makefile before any Crypto library makefile.
 # Crypto library makefile may need default definitions of Measured Boot build
 # flags present in Measured Boot makefile.
@@ -397,17 +410,6 @@
 				plat/arm/board/fvp/fvp_bl2_measured_boot.c	\
 				lib/psa/measured_boot.c
 
-# Note that attestation code does not depend on measured boot interfaces per se,
-# but the two features go together - attestation without boot measurements is
-# pretty much pointless...
-BL31_SOURCES		+=	lib/psa/delegated_attestation.c
-
-PLAT_INCLUDES		+=	-Iinclude/lib/psa
-
-# RSS is not supported on FVP right now. Thus, we use the mocked version
-# of the provided PSA APIs. They return with success and hard-coded data.
-PLAT_RSS_NOT_SUPPORTED	:= 1
-
 # Even though RSS is not supported on FVP (see above), we support overriding
 # PLAT_RSS_NOT_SUPPORTED from the command line, just for the purpose of building
 # the code to detect any build regressions. The resulting firmware will not be
@@ -417,8 +419,7 @@
     include drivers/arm/rss/rss_comms.mk
     BL1_SOURCES		+=	${RSS_COMMS_SOURCES}
     BL2_SOURCES		+=	${RSS_COMMS_SOURCES}
-    BL31_SOURCES	+=	${RSS_COMMS_SOURCES}		\
-				lib/psa/delegated_attestation.c
+    BL31_SOURCES	+=	${RSS_COMMS_SOURCES}
 
     BL1_CFLAGS		+=	-DPLAT_RSS_COMMS_PAYLOAD_MAX_SIZE=0
     BL2_CFLAGS		+=	-DPLAT_RSS_COMMS_PAYLOAD_MAX_SIZE=0
@@ -459,7 +460,7 @@
 endif
 
 # enable trace system registers access to NS by default
-ENABLE_SYS_REG_TRACE_FOR_NS	:= 1
+ENABLE_SYS_REG_TRACE_FOR_NS	:= 2
 
 # enable trace filter control registers access to NS by default
 ENABLE_TRF_FOR_NS		:= 2
@@ -467,7 +468,30 @@
 # Linux relies on EL3 enablement if those features are present
 ENABLE_FEAT_FGT			:= 2
 ENABLE_FEAT_HCX			:= 2
+ENABLE_FEAT_TCR2		:= 2
+
+CTX_INCLUDE_NEVE_REGS		:= 2
+ENABLE_FEAT_CSV2_2		:= 2
+ENABLE_FEAT_ECV			:= 2
+ENABLE_FEAT_PAN			:= 2
+ENABLE_FEAT_SEL2		:= 2
+ENABLE_FEAT_TWED		:= 2
+ENABLE_FEAT_VHE			:= 2
+ENABLE_MPAM_FOR_LOWER_ELS	:= 2
+
+# Enable SME access to NS by default
+ifeq (${ARCH},aarch64)
+ifeq (${SPM_MM}, 0)
+ifeq (${ENABLE_RME}, 0)
+ifeq (${CTX_INCLUDE_FPREGS}, 0)
+	ENABLE_SME_FOR_NS		:= 2
+endif
+endif
+endif
+endif
 
 ifeq (${SPMC_AT_EL3}, 1)
 PLAT_BL_COMMON_SOURCES	+=	plat/arm/board/fvp/fvp_el3_spmc.c
 endif
+
+PSCI_OS_INIT_MODE	:=	1
diff --git a/plat/arm/board/fvp_r/platform.mk b/plat/arm/board/fvp_r/platform.mk
index 93b5cf2..5dd28b9 100644
--- a/plat/arm/board/fvp_r/platform.mk
+++ b/plat/arm/board/fvp_r/platform.mk
@@ -69,7 +69,7 @@
 endif
 
 # Enable Activity Monitor Unit extensions by default
-ENABLE_AMU			:=	1
+ENABLE_FEAT_AMU			:=	2
 
 ifneq (${ENABLE_STACK_PROTECTOR},0)
 FVP_R_BL_COMMON_SOURCES	+=	plat/arm/board/fvp_r/fvp_r_stack_protector.c
diff --git a/plat/arm/board/n1sdp/n1sdp_bl31_setup.c b/plat/arm/board/n1sdp/n1sdp_bl31_setup.c
index ad6c1f8..4941a4b 100644
--- a/plat/arm/board/n1sdp/n1sdp_bl31_setup.c
+++ b/plat/arm/board/n1sdp/n1sdp_bl31_setup.c
@@ -51,8 +51,8 @@
 		PLAT_ARM_GICD_BASE >> 16
 	},
 	.spi_ids = {
-		{32, 479},
-		{512, 959}
+		{PLAT_ARM_GICD_BASE, 32, 479},
+		{PLAT_ARM_GICD_BASE, 512, 959}
 	}
 };
 
diff --git a/plat/arm/board/rdn1edge/rdn1edge_plat.c b/plat/arm/board/rdn1edge/rdn1edge_plat.c
index 045c316..6da8bcd 100644
--- a/plat/arm/board/rdn1edge/rdn1edge_plat.c
+++ b/plat/arm/board/rdn1edge/rdn1edge_plat.c
@@ -27,8 +27,8 @@
 		(PLAT_ARM_GICD_BASE + CSS_SGI_REMOTE_CHIP_MEM_OFFSET(1)) >> 16
 	},
 	.spi_ids = {
-		{32, 255},
-		{0, 0}
+		{PLAT_ARM_GICD_BASE, 32, 255},
+		{0, 0, 0}
 	}
 };
 
diff --git a/plat/arm/board/rdn2/platform.mk b/plat/arm/board/rdn2/platform.mk
index b30e3fc..ca55036 100644
--- a/plat/arm/board/rdn2/platform.mk
+++ b/plat/arm/board/rdn2/platform.mk
@@ -87,4 +87,4 @@
 $(eval $(call TOOL_ADD_PAYLOAD,${NT_FW_CONFIG},--nt-fw-config))
 
 override CTX_INCLUDE_AARCH32_REGS	:= 0
-override ENABLE_AMU			:= 1
+override ENABLE_FEAT_AMU		:= 1
diff --git a/plat/arm/board/rdn2/rdn2_plat.c b/plat/arm/board/rdn2/rdn2_plat.c
index 2506f9d..a5564ce 100644
--- a/plat/arm/board/rdn2/rdn2_plat.c
+++ b/plat/arm/board/rdn2/rdn2_plat.c
@@ -47,15 +47,15 @@
 #endif
 	},
 	.spi_ids = {
-		{32, 511},
+		{PLAT_ARM_GICD_BASE, 32, 511},
 	#if CSS_SGI_CHIP_COUNT > 1
-		{512, 991},
+		{PLAT_ARM_GICD_BASE, 512, 991},
 	#endif
 	#if CSS_SGI_CHIP_COUNT > 2
-		{4096, 4575},
+		{PLAT_ARM_GICD_BASE, 4096, 4575},
 	#endif
 	#if CSS_SGI_CHIP_COUNT > 3
-		{4576, 5055},
+		{PLAT_ARM_GICD_BASE, 4576, 5055},
 	#endif
 	}
 };
diff --git a/plat/arm/board/rdv1/platform.mk b/plat/arm/board/rdv1/platform.mk
index 11f5212..a5fba67 100644
--- a/plat/arm/board/rdv1/platform.mk
+++ b/plat/arm/board/rdv1/platform.mk
@@ -57,7 +57,7 @@
 $(eval $(call TOOL_ADD_PAYLOAD,${NT_FW_CONFIG},--nt-fw-config,${NT_FW_CONFIG}))
 
 override CTX_INCLUDE_AARCH32_REGS	:= 0
-override ENABLE_AMU			:= 1
+override ENABLE_FEAT_AMU		:= 1
 
 ifneq ($(CSS_SGI_PLATFORM_VARIANT),0)
  $(error "CSS_SGI_PLATFORM_VARIANT for RD-V1 should always be 0, \
diff --git a/plat/arm/board/rdv1mc/platform.mk b/plat/arm/board/rdv1mc/platform.mk
index df0b09a..92f7c10 100644
--- a/plat/arm/board/rdv1mc/platform.mk
+++ b/plat/arm/board/rdv1mc/platform.mk
@@ -68,7 +68,7 @@
 $(eval $(call TOOL_ADD_PAYLOAD,${NT_FW_CONFIG},--nt-fw-config,${NT_FW_CONFIG}))
 
 override CTX_INCLUDE_AARCH32_REGS	:= 0
-override ENABLE_AMU			:= 1
+override ENABLE_FEAT_AMU		:= 1
 
 ifneq ($(CSS_SGI_PLATFORM_VARIANT),0)
  $(error "CSS_SGI_PLATFORM_VARIANT for RD-V1-MC should always be 0, \
diff --git a/plat/arm/board/rdv1mc/rdv1mc_plat.c b/plat/arm/board/rdv1mc/rdv1mc_plat.c
index d859400..e4469dc 100644
--- a/plat/arm/board/rdv1mc/rdv1mc_plat.c
+++ b/plat/arm/board/rdv1mc/rdv1mc_plat.c
@@ -43,13 +43,13 @@
 #endif
 	},
 	.spi_ids = {
-		{32, 255},
-		{0, 0},
+		{PLAT_ARM_GICD_BASE, 32, 255},
+		{0, 0, 0},
 #if (CSS_SGI_CHIP_COUNT > 2)
-		{0, 0},
+		{0, 0, 0},
 #endif
 #if (CSS_SGI_CHIP_COUNT > 3)
-		{0, 0},
+		{0, 0, 0},
 #endif
 	}
 };
diff --git a/plat/arm/board/tc/include/platform_def.h b/plat/arm/board/tc/include/platform_def.h
index a884c7a..eea1be6 100644
--- a/plat/arm/board/tc/include/platform_def.h
+++ b/plat/arm/board/tc/include/platform_def.h
@@ -200,11 +200,7 @@
 
 #define PLAT_ARM_NSTIMER_FRAME_ID	0
 
-#if (TARGET_PLATFORM >= 2)
-#define PLAT_ARM_TRUSTED_ROM_BASE	0x1000
-#else
 #define PLAT_ARM_TRUSTED_ROM_BASE	0x0
-#endif
 
 /* PLAT_ARM_TRUSTED_ROM_SIZE 512KB minus ROM base. */
 #define PLAT_ARM_TRUSTED_ROM_SIZE	(0x00080000 - PLAT_ARM_TRUSTED_ROM_BASE)
diff --git a/plat/arm/board/tc/include/tc_plat.h b/plat/arm/board/tc/include/tc_plat.h
index f7ce2fe..ba1831f 100644
--- a/plat/arm/board/tc/include/tc_plat.h
+++ b/plat/arm/board/tc/include/tc_plat.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2023, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -11,6 +11,7 @@
 
 #ifdef PLATFORM_TEST
 void run_platform_tests(void);
+void nv_counter_test(void);
 #endif
 
 #endif /* TC_PLAT_H */
diff --git a/plat/arm/board/tc/nv_counter_test.c b/plat/arm/board/tc/nv_counter_test.c
new file mode 100644
index 0000000..76c9915
--- /dev/null
+++ b/plat/arm/board/tc/nv_counter_test.c
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2023, ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+
+#include <drivers/arm/rss_comms.h>
+#include <plat/common/platform.h>
+#include "rss_platform_api.h"
+
+#include <platform_def.h>
+
+void nv_counter_test(void)
+{
+	psa_status_t status;
+	uint32_t old_val;
+	uint32_t new_val;
+	uint32_t id;
+
+	status = rss_comms_init(PLAT_RSS_AP_SND_MHU_BASE, PLAT_RSS_AP_RCV_MHU_BASE);
+	if (status != PSA_SUCCESS) {
+		printf("Failed to initialize RSS communication channel\n");
+		plat_error_handler(-1);
+	}
+
+	for (id = 0; id < 3; id++) {
+		status = rss_platform_nv_counter_read(id, sizeof(old_val), (uint8_t *)&old_val);
+		if (status != PSA_SUCCESS) {
+			printf("Failed during first id=(%d) rss_platform_nv_counter_read\n",
+				       id);
+			plat_error_handler(-1);
+		}
+
+		status = rss_platform_nv_counter_increment(id);
+		if (status != PSA_SUCCESS) {
+			printf("Failed during id=(%d) rss_platform_nv_counter_increment\n",
+					id);
+			plat_error_handler(-1);
+		}
+
+		status = rss_platform_nv_counter_read(id, sizeof(new_val), (uint8_t *)&new_val);
+		if (status != PSA_SUCCESS) {
+			printf("Failed during second id=(%d) rss_platform_nv_counter_read\n",
+					id);
+			plat_error_handler(-1);
+		}
+
+		if (old_val + 1 != new_val) {
+			printf("Failed nv_counter_test: old_val (%d) + 1 != new_val (%d)\n",
+					old_val, new_val);
+			plat_error_handler(-1);
+		}
+	}
+	printf("Passed nv_counter_test\n");
+}
diff --git a/plat/arm/board/tc/platform.mk b/plat/arm/board/tc/platform.mk
index 2182477..c6a82de 100644
--- a/plat/arm/board/tc/platform.mk
+++ b/plat/arm/board/tc/platform.mk
@@ -1,4 +1,4 @@
-# Copyright (c) 2021-2022, Arm Limited. All rights reserved.
+# Copyright (c) 2021-2023, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -161,9 +161,9 @@
 
 override CTX_INCLUDE_PAUTH_REGS	:= 1
 
-override ENABLE_SPE_FOR_LOWER_ELS	:= 0
+override ENABLE_SPE_FOR_NS	:= 0
 
-override ENABLE_AMU := 1
+override ENABLE_FEAT_AMU := 1
 override ENABLE_AMU_AUXILIARY_COUNTERS := 1
 override ENABLE_AMU_FCONF := 1
 
@@ -196,10 +196,30 @@
 
 endif
 
-# Add this include as first, before arm_common.mk. This is necessary because
-# arm_common.mk builds Mbed TLS, and platform_test.mk can change the list of
-# Mbed TLS files that are to be compiled (LIBMBEDTLS_SRCS).
-include plat/arm/board/tc/platform_test.mk
+ifeq (${PLATFORM_TEST},rss-nv-counters)
+    include drivers/arm/rss/rss_comms.mk
+
+    # Test code.
+    BL31_SOURCES	+=	plat/arm/board/tc/nv_counter_test.c
+
+    # Code under testing.
+    BL31_SOURCES	+=	lib/psa/rss_platform.c \
+				drivers/arm/rss/rss_comms.c \
+				${RSS_COMMS_SOURCES}
+
+    PLAT_INCLUDES	+=	-Iinclude/lib/psa
+
+    $(eval $(call add_define,PLATFORM_TEST))
+else ifeq (${PLATFORM_TEST},tfm-testsuite)
+    # Add this include as first, before arm_common.mk. This is necessary
+    # because arm_common.mk builds Mbed TLS, and platform_test.mk can
+    # change the list of Mbed TLS files that are to be compiled
+    # (LIBMBEDTLS_SRCS).
+    include plat/arm/board/tc/platform_test.mk
+else ifneq (${PLATFORM_TEST},)
+    $(error "Unsupported PLATFORM_TEST value")
+endif
+
 
 include plat/arm/common/arm_common.mk
 include plat/arm/css/common/css_common.mk
diff --git a/plat/arm/board/tc/platform_test.mk b/plat/arm/board/tc/platform_test.mk
index c2ee69e..5cdbb86 100644
--- a/plat/arm/board/tc/platform_test.mk
+++ b/plat/arm/board/tc/platform_test.mk
@@ -1,9 +1,9 @@
-# Copyright (c) 2022, Arm Limited. All rights reserved.
+# Copyright (c) 2022-2023, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
-ifeq (${PLATFORM_TEST},1)
+ifeq (${PLATFORM_TEST},tfm-testsuite)
 
     # The variables need to be set to compile the platform test:
     ifeq (${TF_M_TESTS_PATH},)
diff --git a/plat/arm/board/tc/tc_bl31_setup.c b/plat/arm/board/tc/tc_bl31_setup.c
index c79558d..1c1e2fb 100644
--- a/plat/arm/board/tc/tc_bl31_setup.c
+++ b/plat/arm/board/tc/tc_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020-2023, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -52,9 +52,13 @@
 void tc_bl31_common_platform_setup(void)
 {
 	arm_bl31_platform_setup();
-#ifdef PLATFORM_TEST
-	run_platform_tests();
 
+#ifdef PLATFORM_TEST
+#if PLATFORM_TEST == rss-nv-counters
+	nv_counter_test();
+#elif PLATFORM_TEST == tfm-testsuite
+	run_platform_tests()
+#endif
 	/* Suspend booting */
 	plat_error_handler(-1);
 #endif
diff --git a/plat/arm/common/arm_common.mk b/plat/arm/common/arm_common.mk
index 4d5a56a..de2c4f8 100644
--- a/plat/arm/common/arm_common.mk
+++ b/plat/arm/common/arm_common.mk
@@ -353,8 +353,7 @@
 
 # Pointer Authentication sources
 ifeq (${ENABLE_PAUTH}, 1)
-PLAT_BL_COMMON_SOURCES	+=	plat/arm/common/aarch64/arm_pauth.c	\
-				lib/extensions/pauth/pauth_helpers.S
+PLAT_BL_COMMON_SOURCES	+=	plat/arm/common/aarch64/arm_pauth.c
 endif
 
 ifeq (${SPD},spmd)
diff --git a/plat/arm/common/arm_pm.c b/plat/arm/common/arm_pm.c
index 62cc8bb..055ab36 100644
--- a/plat/arm/common/arm_pm.c
+++ b/plat/arm/common/arm_pm.c
@@ -79,7 +79,12 @@
 	 *  search if the number of entries justify the additional complexity.
 	 */
 	for (i = 0; !!arm_pm_idle_states[i]; i++) {
+#if PSCI_OS_INIT_MODE
+		if ((power_state & ~ARM_LAST_AT_PLVL_MASK) ==
+					arm_pm_idle_states[i])
+#else
 		if (power_state == arm_pm_idle_states[i])
+#endif /* __PSCI_OS_INIT_MODE__ */
 			break;
 	}
 
@@ -91,11 +96,14 @@
 	state_id = psci_get_pstate_id(power_state);
 
 	/* Parse the State ID and populate the state info parameter */
-	while (state_id) {
-		req_state->pwr_domain_state[i++] = state_id &
+	for (i = ARM_PWR_LVL0; i <= PLAT_MAX_PWR_LVL; i++) {
+		req_state->pwr_domain_state[i] = state_id &
 						ARM_LOCAL_PSTATE_MASK;
 		state_id >>= ARM_LOCAL_PSTATE_WIDTH;
 	}
+#if PSCI_OS_INIT_MODE
+	req_state->last_at_pwrlvl = state_id & ARM_LOCAL_PSTATE_MASK;
+#endif /* __PSCI_OS_INIT_MODE__ */
 
 	return PSCI_E_SUCCESS;
 }
diff --git a/plat/mediatek/common/common_config.mk b/plat/mediatek/common/common_config.mk
index 851eb2c..31a61e0 100644
--- a/plat/mediatek/common/common_config.mk
+++ b/plat/mediatek/common/common_config.mk
@@ -19,7 +19,7 @@
 ENABLE_STACK_PROTECTOR := strong
 # AMU, Kernel will access amuserenr_el0 if PE supported
 # Firmware _must_ implement AMU support
-ENABLE_AMU := 1
+ENABLE_FEAT_AMU := 2
 VENDOR_EXTEND_PUBEVENT_ENABLE := 1
 
 # MTK define options
diff --git a/plat/mediatek/common/mtk_smc_handlers.c b/plat/mediatek/common/mtk_smc_handlers.c
index a8a5a48..5a3ad1f 100644
--- a/plat/mediatek/common/mtk_smc_handlers.c
+++ b/plat/mediatek/common/mtk_smc_handlers.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2022-2023, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -76,6 +76,7 @@
 
 MTK_SIP_SMC_FROM_BL33_TABLE(SMC_ID_EXPAND_AS_DESCRIPTOR_INDEX);
 MTK_SIP_SMC_FROM_NS_EL1_TABLE(SMC_ID_EXPAND_AS_DESCRIPTOR_INDEX);
+MTK_SIP_SMC_FROM_S_EL1_TABLE(SMC_ID_EXPAND_AS_DESCRIPTOR_INDEX);
 
 IMPORT_SYM(uintptr_t, __MTK_SMC_POOL_START__, MTK_SMC_POOL_START);
 IMPORT_SYM(uintptr_t, __MTK_SMC_POOL_END_UNALIGNED__, MTK_SMC_POOL_END_UNALIGNED);
@@ -134,6 +135,28 @@
 }
 MTK_EARLY_PLAT_INIT(mtk_smc_handler_init);
 
+/* This function handles Mediatek defined SiP Calls from Secure world */
+static u_register_t mtk_smc_handler_sel1(uint32_t smc_id,
+					 u_register_t x1,
+					 u_register_t x2,
+					 u_register_t x3,
+					 u_register_t x4,
+					 void *cookie,
+					 void *handle,
+					 u_register_t flags)
+{
+	u_register_t ret = MTK_SIP_E_SUCCESS;
+	struct smccc_res smc_ret = {0};
+
+	switch (smc_id) {
+		MTK_SIP_SMC_FROM_S_EL1_TABLE(SMC_ID_EXPAND_AS_SMC_OPERATION);
+	default:
+		INFO("SEL1 SMC ID:0x%x not support\n", smc_id);
+		ret = SMC_UNK;
+	}
+	SMC_RET4(handle, ret, smc_ret.a1, smc_ret.a2, smc_ret.a3);
+}
+
 /* This function handles Mediatek defined SiP Calls from Bootloader */
 static uintptr_t mtk_smc_handler_bl33(uint32_t smc_id,
 				      u_register_t x1,
@@ -209,8 +232,8 @@
 
 	if (!ns) {
 		/* SiP SMC service secure world's call */
-		INFO("Secure SMC ID:0x%x not supported\n", smc_id);
-		SMC_RET1(handle, ret);
+		return mtk_smc_handler_sel1(smc_num, x1, x2, x3, x4,
+					    cookie, handle, flags);
 	}
 	if (is_from_bl33(smc_ori)) {
 		/* SiP SMC service secure bootloader's call */
diff --git a/plat/mediatek/drivers/apusys/apusys.c b/plat/mediatek/drivers/apusys/apusys.c
new file mode 100644
index 0000000..1d34627
--- /dev/null
+++ b/plat/mediatek/drivers/apusys/apusys.c
@@ -0,0 +1,20 @@
+/*
+ * Copyright (c) 2023, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/* TF-A system header */
+#include <common/debug.h>
+
+/* Vendor header */
+#include "apusys.h"
+#include "apusys_power.h"
+#include <lib/mtk_init/mtk_init.h>
+
+int apusys_init(void)
+{
+	apusys_power_init();
+	return 0;
+}
+MTK_PLAT_SETUP_1_INIT(apusys_init);
diff --git a/plat/mediatek/drivers/apusys/apusys.h b/plat/mediatek/drivers/apusys/apusys.h
new file mode 100644
index 0000000..5fdd2ec
--- /dev/null
+++ b/plat/mediatek/drivers/apusys/apusys.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2023, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef APUSYS_H
+#define APUSYS_H
+
+#define MODULE_TAG "[APUSYS]"
+
+#endif
diff --git a/plat/mediatek/drivers/apusys/mt8188/apusys_power.c b/plat/mediatek/drivers/apusys/mt8188/apusys_power.c
new file mode 100644
index 0000000..d7b0d24
--- /dev/null
+++ b/plat/mediatek/drivers/apusys/mt8188/apusys_power.c
@@ -0,0 +1,298 @@
+/*
+ * Copyright (c) 2023, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <inttypes.h>
+
+/* TF-A system header */
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+
+/* Vendor header */
+#include "apusys.h"
+#include "apusys_power.h"
+#include <mtk_mmap_pool.h>
+
+static int apu_poll(uintptr_t reg, uint32_t mask, uint32_t value, uint32_t timeout_us)
+{
+	uint32_t reg_val, count;
+
+	count = timeout_us / APU_POLL_STEP_US;
+	if (count == 0) {
+		count = 1;
+	}
+
+	do {
+		reg_val = mmio_read_32(reg);
+		if ((reg_val & mask) == value) {
+			return 0;
+		}
+
+		udelay(APU_POLL_STEP_US);
+	} while (--count);
+
+	ERROR(MODULE_TAG "Timeout polling APU register %#" PRIxPTR "\n", reg);
+	ERROR(MODULE_TAG "Read value 0x%x, expected 0x%x\n", reg_val,
+	      (value == 0U) ? (reg_val & ~mask) : (reg_val | mask));
+
+	return -1;
+}
+
+static void get_pll_pcw(const uint32_t clk_rate, uint32_t *r1, uint32_t *r2)
+{
+	unsigned int fvco = clk_rate;
+	unsigned int pcw_val;
+	unsigned int postdiv_val = 1;
+	unsigned int postdiv_reg = 0;
+
+	while (fvco <= OUT_CLK_FREQ_MIN) {
+		postdiv_val = postdiv_val << 1;
+		postdiv_reg = postdiv_reg + 1;
+		fvco = fvco << 1;
+	}
+
+	pcw_val = (fvco * (1 << DDS_SHIFT)) / BASIC_CLK_FREQ;
+
+	if (postdiv_reg == 0) {
+		pcw_val = pcw_val * 2;
+		postdiv_val = postdiv_val << 1;
+		postdiv_reg = postdiv_reg + 1;
+	}
+
+	*r1 = postdiv_reg;
+	*r2 = pcw_val;
+}
+
+static void apu_pll_init(void)
+{
+	const uint32_t pll_hfctl_cfg[PLL_NUM] = {
+		PLL4HPLL_FHCTL0_CFG,
+		PLL4HPLL_FHCTL1_CFG,
+		PLL4HPLL_FHCTL2_CFG,
+		PLL4HPLL_FHCTL3_CFG
+	};
+	const uint32_t pll_con1[PLL_NUM] = {
+		PLL4H_PLL1_CON1,
+		PLL4H_PLL2_CON1,
+		PLL4H_PLL3_CON1,
+		PLL4H_PLL4_CON1
+	};
+	const uint32_t pll_fhctl_dds[PLL_NUM] = {
+		PLL4HPLL_FHCTL0_DDS,
+		PLL4HPLL_FHCTL1_DDS,
+		PLL4HPLL_FHCTL2_DDS,
+		PLL4HPLL_FHCTL3_DDS
+	};
+	const uint32_t pll_freq_out[PLL_NUM] = {
+		APUPLL0_DEFAULT_FREQ,
+		APUPLL1_DEFAULT_FREQ,
+		APUPLL2_DEFAULT_FREQ,
+		APUPLL3_DEFAULT_FREQ
+	};
+	uint32_t pcw_val, posdiv_val;
+	int pll_idx;
+
+	mmio_setbits_32(APU_PLL_BASE + PLL4HPLL_FHCTL_RST_CON, PLL4H_PLL_HP_SWRSTB);
+	mmio_setbits_32(APU_PLL_BASE + PLL4HPLL_FHCTL_HP_EN, PLL4H_PLL_HP_EN);
+	mmio_setbits_32(APU_PLL_BASE + PLL4HPLL_FHCTL_CLK_CON, PLL4H_PLL_HP_CLKEN);
+
+	for (pll_idx = 0; pll_idx < PLL_NUM; pll_idx++) {
+		mmio_setbits_32(APU_PLL_BASE + pll_hfctl_cfg[pll_idx], (FHCTL0_EN | SFSTR0_EN));
+
+		posdiv_val = 0;
+		pcw_val = 0;
+		get_pll_pcw(pll_freq_out[pll_idx], &posdiv_val, &pcw_val);
+
+		mmio_clrsetbits_32(APU_PLL_BASE + pll_con1[pll_idx],
+				   (RG_PLL_POSDIV_MASK << RG_PLL_POSDIV_SFT),
+				   (posdiv_val << RG_PLL_POSDIV_SFT));
+		mmio_write_32(APU_PLL_BASE + pll_fhctl_dds[pll_idx],
+			      (FHCTL_PLL_TGL_ORG | pcw_val));
+	}
+}
+
+static void apu_acc_init(void)
+{
+	mmio_write_32(APU_ACC_BASE + APU_ACC_CONFG_CLR0, CGEN_SOC);
+	mmio_write_32(APU_ACC_BASE + APU_ACC_CONFG_SET0, HW_CTRL_EN);
+
+	mmio_write_32(APU_ACC_BASE + APU_ACC_CONFG_CLR1, CGEN_SOC);
+	mmio_write_32(APU_ACC_BASE + APU_ACC_CONFG_SET1, HW_CTRL_EN);
+
+	mmio_write_32(APU_ACC_BASE + APU_ACC_CONFG_CLR2, CGEN_SOC);
+	mmio_write_32(APU_ACC_BASE + APU_ACC_CONFG_SET2, HW_CTRL_EN);
+	mmio_write_32(APU_ACC_BASE + APU_ACC_AUTO_CTRL_SET2, CLK_REQ_SW_EN);
+
+	mmio_write_32(APU_ACC_BASE + APU_ACC_CONFG_CLR3, CGEN_SOC);
+	mmio_write_32(APU_ACC_BASE + APU_ACC_CONFG_SET3, HW_CTRL_EN);
+	mmio_write_32(APU_ACC_BASE + APU_ACC_AUTO_CTRL_SET3, CLK_REQ_SW_EN);
+
+	mmio_write_32(APU_ACC_BASE + APU_ACC_CLK_INV_EN_SET, CLK_INV_EN);
+}
+
+static void apu_buck_off_cfg(void)
+{
+	mmio_write_32(APU_RPC_BASE + APU_RPC_HW_CON, BUCK_PROT_REQ_SET);
+	udelay(10);
+
+	mmio_write_32(APU_RPC_BASE + APU_RPC_HW_CON, BUCK_ELS_EN_SET);
+	udelay(10);
+
+	mmio_write_32(APU_RPC_BASE + APU_RPC_HW_CON, BUCK_AO_RST_B_CLR);
+	udelay(10);
+}
+
+static void apu_pcu_init(void)
+{
+	uint32_t vapu_en_offset = BUCK_VAPU_PMIC_REG_EN_ADDR;
+	uint32_t vapu_sram_en_offset = BUCK_VAPU_SRAM_PMIC_REG_EN_ADDR;
+
+	mmio_write_32(APU_PCU_BASE + APU_PCU_CTRL_SET, AUTO_BUCK_EN);
+
+	mmio_write_32(APU_PCU_BASE + APU_PCU_BUCK_STEP_SEL, BUCK_ON_OFF_CMD_EN);
+
+	mmio_write_32(APU_PCU_BASE + APU_PCU_BUCK_ON_DAT0_L,
+		      ((vapu_sram_en_offset << BUCK_OFFSET_SFT) + BUCK_ON_CMD));
+	mmio_write_32(APU_PCU_BASE + APU_PCU_BUCK_ON_DAT0_H, CMD_OP);
+
+	mmio_write_32(APU_PCU_BASE + APU_PCU_BUCK_ON_DAT1_L,
+		      ((vapu_en_offset << BUCK_OFFSET_SFT) + BUCK_ON_CMD));
+	mmio_write_32(APU_PCU_BASE + APU_PCU_BUCK_ON_DAT1_H, CMD_OP);
+
+	mmio_write_32(APU_PCU_BASE + APU_PCU_BUCK_OFF_DAT0_L,
+		      ((vapu_en_offset << BUCK_OFFSET_SFT) + BUCK_OFF_CMD));
+	mmio_write_32(APU_PCU_BASE + APU_PCU_BUCK_OFF_DAT0_H, CMD_OP);
+
+	mmio_write_32(APU_PCU_BASE + APU_PCU_BUCK_OFF_DAT1_L,
+		      ((vapu_sram_en_offset << BUCK_OFFSET_SFT) + BUCK_OFF_CMD));
+	mmio_write_32(APU_PCU_BASE + APU_PCU_BUCK_OFF_DAT1_H, CMD_OP);
+
+	mmio_write_32(APU_PCU_BASE + APU_PCU_BUCK_ON_SLE0, APU_PCU_BUCK_ON_SETTLE_TIME);
+	mmio_write_32(APU_PCU_BASE + APU_PCU_BUCK_ON_SLE1, APU_PCU_BUCK_ON_SETTLE_TIME);
+}
+
+static void apu_rpclite_init(void)
+{
+	const uint32_t sleep_type_offset[] = {
+		APU_RPC_SW_TYPE2,
+		APU_RPC_SW_TYPE3,
+		APU_RPC_SW_TYPE4,
+		APU_RPC_SW_TYPE5,
+		APU_RPC_SW_TYPE6,
+		APU_RPC_SW_TYPE7,
+		APU_RPC_SW_TYPE8,
+		APU_RPC_SW_TYPE9
+	};
+	int ofs_arr_size = ARRAY_SIZE(sleep_type_offset);
+	int ofs_idx;
+
+	for (ofs_idx = 0 ; ofs_idx < ofs_arr_size ; ofs_idx++) {
+		mmio_clrbits_32(APU_ACX0_RPC_LITE_BASE + sleep_type_offset[ofs_idx],
+				SW_TYPE);
+	}
+
+	mmio_setbits_32(APU_ACX0_RPC_LITE_BASE + APU_RPC_TOP_SEL, RPC_CTRL);
+}
+
+static void apu_rpc_init(void)
+{
+	mmio_clrbits_32(APU_RPC_BASE + APU_RPC_SW_TYPE0, SW_TYPE);
+	mmio_setbits_32(APU_RPC_BASE + APU_RPC_TOP_SEL, RPC_TOP_CTRL);
+	mmio_setbits_32(APU_RPC_BASE + APU_RPC_TOP_SEL_1, RPC_TOP_CTRL1);
+}
+
+static int apu_are_init(void)
+{
+	int ret;
+	int are_id = 0;
+	const uint32_t are_base[APU_ARE_NUM] = { APU_ARE0_BASE, APU_ARE1_BASE, APU_ARE2_BASE };
+	const uint32_t are_entry2_cfg_l[APU_ARE_NUM] = {
+		ARE0_ENTRY2_CFG_L,
+		ARE1_ENTRY2_CFG_L,
+		ARE2_ENTRY2_CFG_L
+	};
+
+	mmio_setbits_32(APU_AO_CTL_BASE + CSR_DUMMY_0_ADDR, VCORE_ARE_REQ);
+
+	ret = apu_poll(APU_ARE2_BASE + APU_ARE_GLO_FSM, ARE_GLO_FSM_IDLE, ARE_GLO_FSM_IDLE,
+		       APU_ARE_POLLING_TIMEOUT_US);
+	if (ret != 0) {
+		ERROR(MODULE_TAG "[%s][%d] ARE init timeout\n",
+		      __func__, __LINE__);
+		return ret;
+	}
+
+	for (are_id = APU_ARE0; are_id < APU_ARE_NUM; are_id++) {
+		mmio_write_32(are_base[are_id] + APU_ARE_ENTRY0_SRAM_H, ARE_ENTRY0_SRAM_H_INIT);
+		mmio_write_32(are_base[are_id] + APU_ARE_ENTRY0_SRAM_L, ARE_ENTRY0_SRAM_L_INIT);
+
+		mmio_write_32(are_base[are_id] + APU_ARE_ENTRY1_SRAM_H, ARE_ENTRY1_SRAM_H_INIT);
+		mmio_write_32(are_base[are_id] + APU_ARE_ENTRY1_SRAM_L, ARE_ENTRY1_SRAM_L_INIT);
+
+		mmio_write_32(are_base[are_id] + APU_ARE_ENTRY2_SRAM_H, ARE_ENTRY_CFG_H);
+		mmio_write_32(are_base[are_id] + APU_ARE_ENTRY2_SRAM_L, are_entry2_cfg_l[are_id]);
+
+		mmio_read_32(are_base[are_id] + APU_ARE_ENTRY2_SRAM_H);
+		mmio_read_32(are_base[are_id] + APU_ARE_ENTRY2_SRAM_L);
+
+		mmio_write_32(are_base[are_id] + APU_ARE_INI_CTRL, ARE_CONFG_INI);
+	}
+
+	return ret;
+}
+
+static void apu_aoc_init(void)
+{
+	mmio_clrbits_32(SPM_BASE + APUSYS_BUCK_ISOLATION, IPU_EXT_BUCK_ISO);
+	mmio_write_32(APU_RPC_BASE + APU_RPC_HW_CON, BUCK_ELS_EN_CLR);
+	udelay(10);
+
+	mmio_write_32(APU_RPC_BASE + APU_RPC_HW_CON, BUCK_AO_RST_B_SET);
+	udelay(10);
+
+	mmio_write_32(APU_RPC_BASE + APU_RPC_HW_CON, BUCK_PROT_REQ_CLR);
+	udelay(10);
+
+	mmio_write_32(APU_RPC_BASE + APU_RPC_HW_CON, SRAM_AOC_ISO_CLR);
+	udelay(10);
+}
+
+static int init_hw_setting(void)
+{
+	int ret;
+
+	apu_aoc_init();
+	apu_pcu_init();
+	apu_rpc_init();
+	apu_rpclite_init();
+
+	ret = apu_are_init();
+	if (ret != 0) {
+		return ret;
+	}
+
+	apu_pll_init();
+	apu_acc_init();
+	apu_buck_off_cfg();
+
+	return ret;
+}
+
+int apusys_power_init(void)
+{
+	int ret;
+
+	ret = init_hw_setting();
+	if (ret != 0) {
+		ERROR(MODULE_TAG "%s initial fail\n", __func__);
+	} else {
+		INFO(MODULE_TAG "%s initial done\n", __func__);
+	}
+
+	return ret;
+}
diff --git a/plat/mediatek/drivers/apusys/mt8188/apusys_power.h b/plat/mediatek/drivers/apusys/mt8188/apusys_power.h
new file mode 100644
index 0000000..1f68bd2
--- /dev/null
+++ b/plat/mediatek/drivers/apusys/mt8188/apusys_power.h
@@ -0,0 +1,193 @@
+/*
+ * Copyright (c) 2023, MediaTek Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef APUSYS_POWER_H
+#define APUSYS_POWER_H
+
+#include <platform_def.h>
+
+enum APU_CLKSRC_ID {
+	PLL_CONN = 0, /* MNOC */
+	PLL_UP,
+	PLL_VPU,
+	PLL_DLA,
+	PLL_NUM,
+};
+
+enum APU_ARE_ID {
+	APU_ARE0 = 0,
+	APU_ARE1,
+	APU_ARE2,
+	APU_ARE_NUM,
+};
+
+#define APU_POLL_STEP_US			(5)
+
+#define OUT_CLK_FREQ_MIN			(1500)
+#define BASIC_CLK_FREQ				(26)
+#define DDS_SHIFT				(14)
+
+#define APUPLL0_DEFAULT_FREQ			(900)
+#define APUPLL1_DEFAULT_FREQ			(832)
+#define APUPLL2_DEFAULT_FREQ			(700)
+#define APUPLL3_DEFAULT_FREQ			(700)
+
+#define APU_TOP_ON_POLLING_TIMEOUT_US		(10000)
+#define APU_TOP_OFF_POLLING_TIMEOUT_US		(5 * APU_TOP_ON_POLLING_TIMEOUT_US)
+#define APU_ARE_POLLING_TIMEOUT_US		(10000)
+
+/* APU related reg */
+#define APU_RPC_BASE				(APU_RPCTOP)
+#define APU_PCU_BASE				(APU_PCUTOP)
+#define APU_ARE0_BASE				(APU_ARETOP_ARE0)
+#define APU_ARE1_BASE				(APU_ARETOP_ARE1)
+#define APU_ARE2_BASE				(APU_ARETOP_ARE2)
+#define APU_AO_CTL_BASE				(APU_AO_CTRL)
+#define APU_PLL_BASE				(APU_PLL)
+#define APU_ACC_BASE				(APU_ACC)
+#define APU_ACX0_RPC_LITE_BASE			(APU_ACX0_RPC_LITE)
+
+/* RPC offset define */
+#define APU_RPC_TOP_SEL				(0x0004)
+#define APU_RPC_TOP_SEL_1			(0x0018)
+#define APU_RPC_HW_CON				(0x001c)
+#define APU_RPC_SW_TYPE0			(0x0200)
+
+/* RPC control */
+#define SRAM_AOC_ISO_CLR			BIT(7)
+#define BUCK_ELS_EN_SET				BIT(10)
+#define BUCK_ELS_EN_CLR				BIT(11)
+#define BUCK_AO_RST_B_SET			BIT(12)
+#define BUCK_AO_RST_B_CLR			BIT(13)
+#define BUCK_PROT_REQ_SET			BIT(14)
+#define BUCK_PROT_REQ_CLR			BIT(15)
+#define SW_TYPE					BIT(1)
+#define RPC_CTRL				(0x0000009e)
+#define RPC_TOP_CTRL				(0x0800501e)
+#define RPC_TOP_CTRL1				BIT(20)
+
+/* PLL offset define */
+#define PLL4H_PLL1_CON1				(0x000c)
+#define PLL4H_PLL2_CON1				(0x001c)
+#define PLL4H_PLL3_CON1				(0x002c)
+#define PLL4H_PLL4_CON1				(0x003c)
+#define PLL4HPLL_FHCTL_HP_EN			(0x0e00)
+#define PLL4HPLL_FHCTL_CLK_CON			(0x0e08)
+#define PLL4HPLL_FHCTL_RST_CON			(0x0e0c)
+#define PLL4HPLL_FHCTL0_CFG			(0x0e3c)
+#define PLL4HPLL_FHCTL0_DDS			(0x0e44)
+#define PLL4HPLL_FHCTL1_CFG			(0x0e50)
+#define PLL4HPLL_FHCTL1_DDS			(0x0e58)
+#define PLL4HPLL_FHCTL2_CFG			(0x0e64)
+#define PLL4HPLL_FHCTL2_DDS			(0x0e6c)
+#define PLL4HPLL_FHCTL3_CFG			(0x0e78)
+#define PLL4HPLL_FHCTL3_DDS			(0x0e80)
+
+/* PLL control */
+#define PLL4H_PLL_HP_EN				(0xf)
+#define PLL4H_PLL_HP_CLKEN			(0xf)
+#define PLL4H_PLL_HP_SWRSTB			(0xf)
+#define FHCTL0_EN				BIT(0)
+#define	SFSTR0_EN				BIT(2)
+#define RG_PLL_POSDIV_MASK			(0x7)
+#define RG_PLL_POSDIV_SFT			(24)
+#define FHCTL_PLL_TGL_ORG			BIT(31)
+
+/* ACC offset define */
+#define APU_ACC_CONFG_SET0			(0x0000)
+#define APU_ACC_CONFG_SET1			(0x0004)
+#define APU_ACC_CONFG_SET2			(0x0008)
+#define APU_ACC_CONFG_SET3			(0x000c)
+#define APU_ACC_CONFG_CLR0			(0x0040)
+#define APU_ACC_CONFG_CLR1			(0x0044)
+#define APU_ACC_CONFG_CLR2			(0x0048)
+#define APU_ACC_CONFG_CLR3			(0x004c)
+#define APU_ACC_CLK_INV_EN_SET			(0x00e8)
+#define APU_ACC_AUTO_CTRL_SET2			(0x0128)
+#define APU_ACC_AUTO_CTRL_SET3			(0x012c)
+
+/* ACC control */
+#define CGEN_SOC				BIT(2)
+#define HW_CTRL_EN				BIT(15)
+#define CLK_REQ_SW_EN				BIT(8)
+#define CLK_INV_EN				(0xaaa8)
+
+/* ARE offset define */
+#define APU_ARE_INI_CTRL			(0x0000)
+#define APU_ARE_GLO_FSM				(0x0048)
+#define APU_ARE_ENTRY0_SRAM_H			(0x0c00)
+#define APU_ARE_ENTRY0_SRAM_L			(0x0800)
+#define APU_ARE_ENTRY1_SRAM_H			(0x0c04)
+#define APU_ARE_ENTRY1_SRAM_L			(0x0804)
+#define APU_ARE_ENTRY2_SRAM_H			(0x0c08)
+#define APU_ARE_ENTRY2_SRAM_L			(0x0808)
+
+/* ARE control */
+#define ARE_ENTRY_CFG_H				(0x00140000)
+#define ARE0_ENTRY2_CFG_L			(0x004e0804)
+#define ARE1_ENTRY2_CFG_L			(0x004e0806)
+#define ARE2_ENTRY2_CFG_L			(0x004e0807)
+#define ARE_GLO_FSM_IDLE			BIT(0)
+#define ARE_ENTRY0_SRAM_H_INIT			(0x12345678)
+#define ARE_ENTRY0_SRAM_L_INIT			(0x89abcdef)
+#define ARE_ENTRY1_SRAM_H_INIT			(0xfedcba98)
+#define ARE_ENTRY1_SRAM_L_INIT			(0x76543210)
+#define ARE_CONFG_INI				BIT(2)
+
+/* SPM offset define */
+#define APUSYS_BUCK_ISOLATION			(0x03ec)
+
+/* SPM control*/
+#define IPU_EXT_BUCK_ISO			(0x21)
+
+/* apu_rcx_ao_ctrl  */
+#define CSR_DUMMY_0_ADDR			(0x0024)
+
+/* apu_rcx_ao_ctrl control */
+#define VCORE_ARE_REQ				BIT(2)
+
+/* PCU offset define */
+#define APU_PCU_CTRL_SET			(0x0000)
+#define APU_PCU_BUCK_STEP_SEL			(0x0030)
+#define APU_PCU_BUCK_ON_DAT0_L			(0x0080)
+#define APU_PCU_BUCK_ON_DAT0_H			(0x0084)
+#define APU_PCU_BUCK_ON_DAT1_L			(0x0088)
+#define APU_PCU_BUCK_ON_DAT1_H			(0x008c)
+#define APU_PCU_BUCK_OFF_DAT0_L			(0x00a0)
+#define APU_PCU_BUCK_OFF_DAT0_H			(0x00a4)
+#define APU_PCU_BUCK_OFF_DAT1_L			(0x00a8)
+#define APU_PCU_BUCK_OFF_DAT1_H			(0x00ac)
+#define APU_PCU_BUCK_ON_SLE0			(0x00c0)
+#define APU_PCU_BUCK_ON_SLE1			(0x00c4)
+#define APU_PCU_BUCK_ON_SETTLE_TIME		(0x012c)
+
+/* PCU initial data */
+#define MT6359P_RG_BUCK_VMODEM_EN_ADDR		(0x1688)
+#define MT6359P_RG_LDO_VSRAM_MD_EN_ADDR		(0x1f2e)
+#define BUCK_VAPU_PMIC_REG_EN_ADDR		MT6359P_RG_BUCK_VMODEM_EN_ADDR
+#define BUCK_VAPU_SRAM_PMIC_REG_EN_ADDR		MT6359P_RG_LDO_VSRAM_MD_EN_ADDR
+
+/* PCU control */
+#define AUTO_BUCK_EN				BIT(16)
+#define BUCK_ON_OFF_CMD_EN			(0x33)
+#define BUCK_OFFSET_SFT				(16)
+#define BUCK_ON_CMD				(0x1)
+#define BUCK_OFF_CMD				(0x0)
+#define CMD_OP					(0x4)
+
+/* RPC lite offset define */
+#define APU_RPC_SW_TYPE2			(0x0208)
+#define APU_RPC_SW_TYPE3			(0x020c)
+#define APU_RPC_SW_TYPE4			(0x0210)
+#define APU_RPC_SW_TYPE5			(0x0214)
+#define APU_RPC_SW_TYPE6			(0x0218)
+#define APU_RPC_SW_TYPE7			(0x021c)
+#define APU_RPC_SW_TYPE8			(0x0220)
+#define APU_RPC_SW_TYPE9			(0x0224)
+
+int apusys_power_init(void);
+
+#endif /* APUSYS_POWER_H */
diff --git a/plat/mediatek/drivers/apusys/mt8188/rules.mk b/plat/mediatek/drivers/apusys/mt8188/rules.mk
new file mode 100644
index 0000000..f676b6e
--- /dev/null
+++ b/plat/mediatek/drivers/apusys/mt8188/rules.mk
@@ -0,0 +1,13 @@
+#
+# Copyright (c) 2023, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := apusys_${MTK_SOC}
+
+LOCAL_SRCS-y := ${LOCAL_DIR}/apusys_power.c
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
diff --git a/plat/mediatek/drivers/apusys/rules.mk b/plat/mediatek/drivers/apusys/rules.mk
new file mode 100644
index 0000000..1aa67bc
--- /dev/null
+++ b/plat/mediatek/drivers/apusys/rules.mk
@@ -0,0 +1,19 @@
+#
+# Copyright (c) 2023, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LOCAL_DIR := $(call GET_LOCAL_DIR)
+
+MODULE := apusys
+
+LOCAL_SRCS-y:= ${LOCAL_DIR}/apusys.c
+
+PLAT_INCLUDES += -I${LOCAL_DIR} -I${LOCAL_DIR}/${MTK_SOC}
+
+$(eval $(call MAKE_MODULE,$(MODULE),$(LOCAL_SRCS-y),$(MTK_BL)))
+
+SUB_RULES-y := ${LOCAL_DIR}/${MTK_SOC}
+
+$(eval $(call INCLUDE_MAKEFILE,$(SUB_RULES-y)))
diff --git a/plat/mediatek/drivers/emi_mpu/emi_mpu_common.c b/plat/mediatek/drivers/emi_mpu/emi_mpu_common.c
index 27b2b07..bf77791 100644
--- a/plat/mediatek/drivers/emi_mpu/emi_mpu_common.c
+++ b/plat/mediatek/drivers/emi_mpu/emi_mpu_common.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,7 @@
 #include <lib/mmio.h>
 #include <emi_mpu.h>
 #include <lib/mtk_init/mtk_init.h>
+#include <mtk_sip_svc.h>
 
 #if ENABLE_EMI_MPU_SW_LOCK
 static unsigned char region_lock_state[EMI_MPU_REGION_NUM];
@@ -111,6 +112,16 @@
 	return 0;
 }
 
+u_register_t mtk_emi_mpu_sip_handler(u_register_t x1, u_register_t x2,
+				     u_register_t x3, u_register_t x4,
+				     void *handle, struct smccc_res *smccc_ret)
+{
+	/* TODO: implement emi mpu handler */
+
+	return 0;
+}
+DECLARE_SMC_HANDLER(MTK_SIP_TEE_MPU_PERM_SET, mtk_emi_mpu_sip_handler);
+
 int emi_mpu_init(void)
 {
 	INFO("[%s] emi mpu initialization\n", __func__);
diff --git a/plat/mediatek/include/mtk_sip_def.h b/plat/mediatek/include/mtk_sip_def.h
index 2039017..a86a46c 100644
--- a/plat/mediatek/include/mtk_sip_def.h
+++ b/plat/mediatek/include/mtk_sip_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2022-2023, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,6 +19,9 @@
 	_func(MTK_SIP_DP_CONTROL, 0x523) \
 	_func(MTK_SIP_KERNEL_GIC_OP, 0x526)
 
+#define MTK_SIP_SMC_FROM_S_EL1_TABLE(_func) \
+	_func(MTK_SIP_TEE_MPU_PERM_SET, 0x031)
+
 #define MTK_SIP_SMC_FROM_BL33_TABLE(_func) \
 	_func(MTK_SIP_KERNEL_BOOT, 0x115)
 
diff --git a/plat/mediatek/include/mtk_sip_svc.h b/plat/mediatek/include/mtk_sip_svc.h
index ce51048..f677915 100644
--- a/plat/mediatek/include/mtk_sip_svc.h
+++ b/plat/mediatek/include/mtk_sip_svc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -55,11 +55,13 @@
 
 MTK_SIP_SMC_FROM_BL33_TABLE(SMC_ID_EXPAND_AS_EXTERN_SMC_INDEX);
 MTK_SIP_SMC_FROM_NS_EL1_TABLE(SMC_ID_EXPAND_AS_EXTERN_SMC_INDEX);
+MTK_SIP_SMC_FROM_S_EL1_TABLE(SMC_ID_EXPAND_AS_EXTERN_SMC_INDEX);
 
 /* Expand SiP SMC ID table as enum */
 enum {
 	MTK_SIP_SMC_FROM_BL33_TABLE(SMC_ID_EXPAND_AS_ENUM)
 	MTK_SIP_SMC_FROM_NS_EL1_TABLE(SMC_ID_EXPAND_AS_ENUM)
+	MTK_SIP_SMC_FROM_S_EL1_TABLE(SMC_ID_EXPAND_AS_ENUM)
 	MTK_SIP_SMC_MAX_NUMBER
 };
 
diff --git a/plat/mediatek/mt8188/include/platform_def.h b/plat/mediatek/mt8188/include/platform_def.h
index 34d4637..576dc3d 100644
--- a/plat/mediatek/mt8188/include/platform_def.h
+++ b/plat/mediatek/mt8188/include/platform_def.h
@@ -25,6 +25,21 @@
 #define TOPCKGEN_BASE		(IO_PHYS)
 
 /*******************************************************************************
+ * APUSYS related constants
+ ******************************************************************************/
+#define BCRM_FMEM_PDN_BASE	(IO_PHYS + 0x00276000)
+#define APU_RPCTOP		(IO_PHYS + 0x090f0000)
+#define APU_PCUTOP		(IO_PHYS + 0x090f1000)
+#define APU_AO_CTRL		(IO_PHYS + 0x090f2000)
+#define APU_PLL			(IO_PHYS + 0x090f3000)
+#define APU_ACC			(IO_PHYS + 0x090f4000)
+#define APU_ARETOP_ARE0		(IO_PHYS + 0x090f6000)
+#define APU_ARETOP_ARE1		(IO_PHYS + 0x090f7000)
+#define APU_ARETOP_ARE2		(IO_PHYS + 0x090f8000)
+#define APU_ACX0_RPC_LITE	(IO_PHYS + 0x09140000)
+#define BCRM_FMEM_PDN_SIZE	(0x1000)
+
+/*******************************************************************************
  * AUDIO related constants
  ******************************************************************************/
 #define AUDIO_BASE		(IO_PHYS + 0x00b10000)
diff --git a/plat/mediatek/mt8188/platform.mk b/plat/mediatek/mt8188/platform.mk
index 85ceeb9..5096e15 100644
--- a/plat/mediatek/mt8188/platform.mk
+++ b/plat/mediatek/mt8188/platform.mk
@@ -24,6 +24,7 @@
 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/apusys
 MODULES-y += $(MTK_PLAT)/drivers/audio
 MODULES-y += $(MTK_PLAT)/drivers/cirq
 MODULES-y += $(MTK_PLAT)/drivers/cpu_pm
diff --git a/plat/mediatek/mt8195/drivers/emi_mpu/emi_mpu.c b/plat/mediatek/mt8195/drivers/emi_mpu/emi_mpu.c
index 794e21e..b6e5a2d 100644
--- a/plat/mediatek/mt8195/drivers/emi_mpu/emi_mpu.c
+++ b/plat/mediatek/mt8195/drivers/emi_mpu/emi_mpu.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2023, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -8,6 +8,7 @@
 #include <common/debug.h>
 #include <lib/mmio.h>
 #include <emi_mpu.h>
+#include <mtk_sip_svc.h>
 
 #if ENABLE_EMI_MPU_SW_LOCK
 static unsigned char region_lock_state[EMI_MPU_REGION_NUM];
@@ -17,6 +18,7 @@
 #define EMI_MPU_END_MASK		(0x00FFFFFF)
 #define EMI_MPU_APC_SW_LOCK_MASK	(0x00FFFFFF)
 #define EMI_MPU_APC_HW_LOCK_MASK	(0x80FFFFFF)
+#define MPU_PHYSICAL_ADDR_SHIFT_BITS	(16)
 
 static int _emi_mpu_set_protection(unsigned int start, unsigned int end,
 					unsigned int apc)
@@ -139,7 +141,7 @@
 	/* Forbidden All */
 	region_info.start = 0x40000000ULL;	/* dram base addr */
 	region_info.end = 0x1FFFF0000ULL;
-	region_info.region = 4;
+	region_info.region = 5;
 	SET_ACCESS_PERMISSION(region_info.apc, 1,
 			      FORBIDDEN, FORBIDDEN, FORBIDDEN, FORBIDDEN,
 			      FORBIDDEN, FORBIDDEN, FORBIDDEN, FORBIDDEN,
@@ -149,3 +151,42 @@
 
 	dump_emi_mpu_regions();
 }
+
+static inline uint64_t get_decoded_phys_addr(uint64_t addr)
+{
+	return (addr << MPU_PHYSICAL_ADDR_SHIFT_BITS);
+}
+
+static inline uint32_t get_decoded_zone_id(uint32_t info)
+{
+	return ((info & 0xFFFF0000) >> MPU_PHYSICAL_ADDR_SHIFT_BITS);
+}
+
+int32_t emi_mpu_sip_handler(uint64_t encoded_addr, uint64_t zone_size, uint64_t zone_info)
+{
+	uint64_t phys_addr = get_decoded_phys_addr(encoded_addr);
+	struct emi_region_info_t region_info;
+	enum MPU_REQ_ORIGIN_ZONE_ID zone_id = get_decoded_zone_id(zone_info);
+
+	INFO("encoded_addr = 0x%lx, zone_size = 0x%lx, zone_info = 0x%lx\n",
+	     encoded_addr, zone_size, zone_info);
+
+	if (zone_id != MPU_REQ_ORIGIN_TEE_ZONE_SVP) {
+		ERROR("Invalid param %s, %d\n", __func__, __LINE__);
+		return MTK_SIP_E_INVALID_PARAM;
+	}
+
+	/* SVP DRAM */
+	region_info.start = phys_addr;
+	region_info.end = phys_addr + zone_size;
+	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, SEC_RW);
+
+	emi_mpu_set_protection(&region_info);
+
+	return 0;
+}
diff --git a/plat/mediatek/mt8195/drivers/emi_mpu/emi_mpu.h b/plat/mediatek/mt8195/drivers/emi_mpu/emi_mpu.h
index 415146e..83bd6de 100644
--- a/plat/mediatek/mt8195/drivers/emi_mpu/emi_mpu.h
+++ b/plat/mediatek/mt8195/drivers/emi_mpu/emi_mpu.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2023, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -93,6 +93,15 @@
 	unsigned int apc[EMI_MPU_DGROUP_NUM];
 };
 
+enum MPU_REQ_ORIGIN_ZONE_ID {
+	MPU_REQ_ORIGIN_TEE_ZONE_SVP = 0,
+	MPU_REQ_ORIGIN_TEE_ZONE_TUI = 1,
+	MPU_REQ_ORIGIN_TEE_ZONE_WFD = 2,
+	MPU_REQ_ORIGIN_TEE_ZONE_MAX = 3,
+	MPU_REQ_ORIGIN_ZONE_INVALID = 0x7FFFFFFF,
+};
+
 void emi_mpu_init(void);
+int32_t emi_mpu_sip_handler(uint64_t encoded_addr, uint64_t zone_size, uint64_t zone_info);
 
 #endif
diff --git a/plat/mediatek/mt8195/plat_sip_calls.c b/plat/mediatek/mt8195/plat_sip_calls.c
index 1cdd622..2debeff 100644
--- a/plat/mediatek/mt8195/plat_sip_calls.c
+++ b/plat/mediatek/mt8195/plat_sip_calls.c
@@ -1,11 +1,12 @@
 /*
- * Copyright (c) 2020-2022, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2020-2023, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #include <common/debug.h>
 #include <common/runtime_svc.h>
+#include <emi_mpu.h>
 #include <mt_dp.h>
 #include <mt_spm.h>
 #include <mt_spm_vcorefs.h>
@@ -27,6 +28,11 @@
 	uint32_t ret_val;
 
 	switch (smc_fid) {
+	case MTK_SIP_TEE_MPU_PERM_SET_AARCH64:
+	case MTK_SIP_TEE_MPU_PERM_SET_AARCH32:
+		ret = emi_mpu_sip_handler(x1, x2, x3);
+		SMC_RET2(handle, ret, ret_val);
+		break;
 	case MTK_SIP_DP_CONTROL_AARCH32:
 	case MTK_SIP_DP_CONTROL_AARCH64:
 		ret = dp_secure_handler(x1, x2, &ret_val);
diff --git a/plat/qemu/common/qemu_bl1_setup.c b/plat/qemu/common/qemu_bl1_setup.c
index 67f3327..529510c 100644
--- a/plat/qemu/common/qemu_bl1_setup.c
+++ b/plat/qemu/common/qemu_bl1_setup.c
@@ -14,6 +14,29 @@
 
 #include "qemu_private.h"
 
+#define MAP_BL1_TOTAL		MAP_REGION_FLAT(			\
+					bl1_tzram_layout.total_base,	\
+					bl1_tzram_layout.total_size,	\
+					MT_MEMORY | MT_RW | EL3_PAS)
+
+#define MAP_BL1_RO		MAP_REGION_FLAT(			\
+					BL_CODE_BASE,			\
+					BL1_CODE_END - BL_CODE_BASE,	\
+					MT_CODE | EL3_PAS),		\
+				MAP_REGION_FLAT(			\
+					BL1_RO_DATA_BASE,		\
+					BL1_RO_DATA_END			\
+						- BL_RO_DATA_BASE,	\
+					MT_RO_DATA | EL3_PAS)
+
+#if USE_COHERENT_MEM
+#define MAP_BL_COHERENT_RAM	MAP_REGION_FLAT(			\
+					BL_COHERENT_RAM_BASE,		\
+					BL_COHERENT_RAM_END		\
+						- BL_COHERENT_RAM_BASE,	\
+					MT_DEVICE | MT_RW | EL3_PAS)
+#endif
+
 /* Data structure which holds the extents of the trusted SRAM for BL1*/
 static meminfo_t bl1_tzram_layout;
 
@@ -49,11 +72,21 @@
 
 void bl1_plat_arch_setup(void)
 {
-	QEMU_CONFIGURE_BL1_MMU(bl1_tzram_layout.total_base,
-				bl1_tzram_layout.total_size,
-				BL_CODE_BASE, BL1_CODE_END,
-				BL1_RO_DATA_BASE, BL1_RO_DATA_END,
-				BL_COHERENT_RAM_BASE, BL_COHERENT_RAM_END);
+	const mmap_region_t bl_regions[] = {
+		MAP_BL1_TOTAL,
+		MAP_BL1_RO,
+#if USE_COHERENT_MEM
+		MAP_BL_COHERENT_RAM,
+#endif
+		{0}
+	};
+
+	setup_page_tables(bl_regions, plat_qemu_get_mmap());
+#ifdef __aarch64__
+	enable_mmu_el3(0);
+#else
+	enable_mmu_svc_mon(0);
+#endif
 }
 
 void bl1_platform_setup(void)
diff --git a/plat/qemu/common/qemu_bl2_setup.c b/plat/qemu/common/qemu_bl2_setup.c
index be55877..c4d235e 100644
--- a/plat/qemu/common/qemu_bl2_setup.c
+++ b/plat/qemu/common/qemu_bl2_setup.c
@@ -23,6 +23,28 @@
 
 #include "qemu_private.h"
 
+#define MAP_BL2_TOTAL		MAP_REGION_FLAT(			\
+					bl2_tzram_layout.total_base,	\
+					bl2_tzram_layout.total_size,	\
+					MT_MEMORY | MT_RW | MT_SECURE)
+
+#define MAP_BL2_RO		MAP_REGION_FLAT(			\
+					BL_CODE_BASE,			\
+					BL_CODE_END - BL_CODE_BASE,	\
+					MT_CODE | MT_SECURE),		\
+				MAP_REGION_FLAT(			\
+					BL_RO_DATA_BASE,		\
+					BL_RO_DATA_END			\
+						- BL_RO_DATA_BASE,	\
+					MT_RO_DATA | MT_SECURE)
+
+#if USE_COHERENT_MEM
+#define MAP_BL_COHERENT_RAM	MAP_REGION_FLAT(			\
+					BL_COHERENT_RAM_BASE,		\
+					BL_COHERENT_RAM_END		\
+						- BL_COHERENT_RAM_BASE,	\
+					MT_DEVICE | MT_RW | MT_SECURE)
+#endif
 
 /* Data structure which holds the extents of the trusted SRAM for BL2 */
 static meminfo_t bl2_tzram_layout __aligned(CACHE_WRITEBACK_GRANULE);
@@ -83,19 +105,24 @@
 	/* TODO Initialize timer */
 }
 
+void bl2_plat_arch_setup(void)
+{
+	const mmap_region_t bl_regions[] = {
+		MAP_BL2_TOTAL,
+		MAP_BL2_RO,
+#if USE_COHERENT_MEM
+		MAP_BL_COHERENT_RAM,
+#endif
+		{0}
+	};
+
+	setup_page_tables(bl_regions, plat_qemu_get_mmap());
+
 #ifdef __aarch64__
-#define QEMU_CONFIGURE_BL2_MMU(...)	qemu_configure_mmu_el1(__VA_ARGS__)
+	enable_mmu_el1(0);
 #else
-#define QEMU_CONFIGURE_BL2_MMU(...)	qemu_configure_mmu_svc_mon(__VA_ARGS__)
+	enable_mmu_svc_mon(0);
 #endif
-
-void bl2_plat_arch_setup(void)
-{
-	QEMU_CONFIGURE_BL2_MMU(bl2_tzram_layout.total_base,
-			      bl2_tzram_layout.total_size,
-			      BL_CODE_BASE, BL_CODE_END,
-			      BL_RO_DATA_BASE, BL_RO_DATA_END,
-			      BL_COHERENT_RAM_BASE, BL_COHERENT_RAM_END);
 }
 
 /*******************************************************************************
diff --git a/plat/qemu/common/qemu_bl31_setup.c b/plat/qemu/common/qemu_bl31_setup.c
index 4f60eb1..0b84e96 100644
--- a/plat/qemu/common/qemu_bl31_setup.c
+++ b/plat/qemu/common/qemu_bl31_setup.c
@@ -12,6 +12,28 @@
 
 #include "qemu_private.h"
 
+#define MAP_BL31_TOTAL		MAP_REGION_FLAT(			\
+					BL31_BASE,			\
+					BL31_END - BL31_BASE,		\
+					MT_MEMORY | MT_RW | EL3_PAS)
+#define MAP_BL31_RO		MAP_REGION_FLAT(			\
+					BL_CODE_BASE,			\
+					BL_CODE_END - BL_CODE_BASE,	\
+					MT_CODE | EL3_PAS),		\
+				MAP_REGION_FLAT(			\
+					BL_RO_DATA_BASE,		\
+					BL_RO_DATA_END			\
+						- BL_RO_DATA_BASE,	\
+					MT_RO_DATA | EL3_PAS)
+
+#if USE_COHERENT_MEM
+#define MAP_BL_COHERENT_RAM	MAP_REGION_FLAT(			\
+					BL_COHERENT_RAM_BASE,		\
+					BL_COHERENT_RAM_END		\
+						- BL_COHERENT_RAM_BASE,	\
+					MT_DEVICE | MT_RW | EL3_PAS)
+#endif
+
 /*
  * Placeholder variables for copying the arguments that have been passed to
  * BL3-1 from BL2.
@@ -64,10 +86,18 @@
 
 void bl31_plat_arch_setup(void)
 {
-	qemu_configure_mmu_el3(BL31_BASE, (BL31_END - BL31_BASE),
-			      BL_CODE_BASE, BL_CODE_END,
-			      BL_RO_DATA_BASE, BL_RO_DATA_END,
-			      BL_COHERENT_RAM_BASE, BL_COHERENT_RAM_END);
+	const mmap_region_t bl_regions[] = {
+		MAP_BL31_TOTAL,
+		MAP_BL31_RO,
+#if USE_COHERENT_MEM
+		MAP_BL_COHERENT_RAM,
+#endif
+		{0}
+	};
+
+	setup_page_tables(bl_regions, plat_qemu_get_mmap());
+
+	enable_mmu_el3(0);
 }
 
 static void qemu_gpio_init(void)
diff --git a/plat/qemu/common/qemu_common.c b/plat/qemu/common/qemu_common.c
index 23ac581..935ba7a 100644
--- a/plat/qemu/common/qemu_common.c
+++ b/plat/qemu/common/qemu_common.c
@@ -122,45 +122,12 @@
 #endif
 
 /*******************************************************************************
- * Macro generating the code for the function setting up the pagetables as per
- * the platform memory map & initialize the mmu, for the given exception level
+ * Returns QEMU platform specific memory map regions.
  ******************************************************************************/
-
-#define DEFINE_CONFIGURE_MMU_EL(_el)					\
-	void qemu_configure_mmu_##_el(unsigned long total_base,	\
-				   unsigned long total_size,		\
-				   unsigned long code_start,		\
-				   unsigned long code_limit,		\
-				   unsigned long ro_start,		\
-				   unsigned long ro_limit,		\
-				   unsigned long coh_start,		\
-				   unsigned long coh_limit)		\
-	{								\
-		mmap_add_region(total_base, total_base,			\
-				total_size,				\
-				MT_MEMORY | MT_RW | MT_SECURE);		\
-		mmap_add_region(code_start, code_start,			\
-				code_limit - code_start,		\
-				MT_CODE | MT_SECURE);			\
-		mmap_add_region(ro_start, ro_start,			\
-				ro_limit - ro_start,			\
-				MT_RO_DATA | MT_SECURE);		\
-		mmap_add_region(coh_start, coh_start,			\
-				coh_limit - coh_start,			\
-				MT_DEVICE | MT_RW | MT_SECURE);		\
-		mmap_add(plat_qemu_mmap);				\
-		init_xlat_tables();					\
-									\
-		enable_mmu_##_el(0);					\
-	}
-
-/* Define EL1 and EL3 variants of the function initialising the MMU */
-#ifdef __aarch64__
-DEFINE_CONFIGURE_MMU_EL(el1)
-DEFINE_CONFIGURE_MMU_EL(el3)
-#else
-DEFINE_CONFIGURE_MMU_EL(svc_mon)
-#endif
+const mmap_region_t *plat_qemu_get_mmap(void)
+{
+	return plat_qemu_mmap;
+}
 
 #if MEASURED_BOOT || TRUSTED_BOARD_BOOT
 int plat_get_mbedtls_heap(void **heap_addr, size_t *heap_size)
diff --git a/plat/qemu/common/qemu_private.h b/plat/qemu/common/qemu_private.h
index 159c44f..199ca01 100644
--- a/plat/qemu/common/qemu_private.h
+++ b/plat/qemu/common/qemu_private.h
@@ -9,26 +9,13 @@
 
 #include <stdint.h>
 
-void qemu_configure_mmu_svc_mon(unsigned long total_base,
-			unsigned long total_size,
-			unsigned long code_start, unsigned long code_limit,
-			unsigned long ro_start, unsigned long ro_limit,
-			unsigned long coh_start, unsigned long coh_limit);
-
-void qemu_configure_mmu_el1(unsigned long total_base, unsigned long total_size,
-			unsigned long code_start, unsigned long code_limit,
-			unsigned long ro_start, unsigned long ro_limit,
-			unsigned long coh_start, unsigned long coh_limit);
-
-void qemu_configure_mmu_el3(unsigned long total_base, unsigned long total_size,
-			unsigned long code_start, unsigned long code_limit,
-			unsigned long ro_start, unsigned long ro_limit,
-			unsigned long coh_start, unsigned long coh_limit);
+#include <lib/xlat_tables/xlat_tables_compat.h>
 
 void plat_qemu_io_setup(void);
 int qemu_io_register_sp_pkg(const char *name, const char *uuid,
 			    uintptr_t load_addr);
 unsigned int plat_qemu_calc_core_pos(u_register_t mpidr);
+const mmap_region_t *plat_qemu_get_mmap(void);
 
 void qemu_console_init(void);
 
diff --git a/plat/qemu/common/qemu_stack_protector.c b/plat/qemu/common/qemu_stack_protector.c
index 15ce3d6..d0b4a0f 100644
--- a/plat/qemu/common/qemu_stack_protector.c
+++ b/plat/qemu/common/qemu_stack_protector.c
@@ -14,12 +14,10 @@
 
 u_register_t plat_get_stack_protector_canary(void)
 {
-#if ENABLE_FEAT_RNG
 	/* Use the RNDR instruction if the CPU supports it */
-	if (is_armv8_5_rng_present()) {
+	if (is_feat_rng_supported()) {
 		return read_rndr();
 	}
-#endif
 
 	/*
 	 * Ideally, a random number should be returned above. If a random
diff --git a/plat/qemu/qemu/platform.mk b/plat/qemu/qemu/platform.mk
index 4cbce9d..c076aba 100644
--- a/plat/qemu/qemu/platform.mk
+++ b/plat/qemu/qemu/platform.mk
@@ -18,6 +18,17 @@
 $(eval $(call add_define,ARMV7_SUPPORTS_VFP))
 # Qemu expects a BL32 boot stage.
 NEED_BL32		:=	yes
+else
+CTX_INCLUDE_AARCH32_REGS := 0
+ifeq (${CTX_INCLUDE_AARCH32_REGS}, 1)
+$(error "This is an AArch64-only port; CTX_INCLUDE_AARCH32_REGS must be disabled")
+endif
+
+# Treating this as a memory-constrained port for now
+USE_COHERENT_MEM	:=	0
+
+# This can be overridden depending on CPU(s) used in the QEMU image
+HW_ASSISTED_COHERENCY	:=	1
 endif # ARMv7
 
 ifeq (${SPD},opteed)
@@ -46,6 +57,17 @@
 
 ifeq (${ARM_ARCH_MAJOR},8)
 PLAT_INCLUDES		+=	-Iinclude/plat/arm/common/${ARCH}
+
+QEMU_CPU_LIBS		:=	lib/cpus/aarch64/aem_generic.S		\
+				lib/cpus/aarch64/cortex_a53.S		\
+				lib/cpus/aarch64/cortex_a57.S		\
+				lib/cpus/aarch64/cortex_a72.S		\
+				lib/cpus/aarch64/cortex_a76.S		\
+				lib/cpus/aarch64/neoverse_n_common.S	\
+				lib/cpus/aarch64/neoverse_n1.S		\
+				lib/cpus/aarch64/qemu_max.S
+else
+QEMU_CPU_LIBS		:=	lib/cpus/${ARCH}/cortex_a15.S
 endif
 
 PLAT_BL_COMMON_SOURCES	:=	${PLAT_QEMU_COMMON_PATH}/qemu_common.c			\
@@ -135,14 +157,15 @@
 				lib/semihosting/${ARCH}/semihosting_call.S \
 				${PLAT_QEMU_COMMON_PATH}/qemu_io_storage.c		\
 				${PLAT_QEMU_COMMON_PATH}/${ARCH}/plat_helpers.S	\
-				${PLAT_QEMU_COMMON_PATH}/qemu_bl1_setup.c
+				${PLAT_QEMU_COMMON_PATH}/qemu_bl1_setup.c	\
+				${QEMU_CPU_LIBS}
 
 ifeq (${ARM_ARCH_MAJOR},8)
-BL1_SOURCES		+=	lib/cpus/aarch64/aem_generic.S		\
-				lib/cpus/aarch64/cortex_a53.S		\
-				lib/cpus/aarch64/cortex_a57.S		\
-				lib/cpus/aarch64/cortex_a72.S		\
-				lib/cpus/aarch64/qemu_max.S		\
+BL1_SOURCES		+=	lib/cpus/${ARCH}/aem_generic.S		\
+				lib/cpus/${ARCH}/cortex_a53.S		\
+				lib/cpus/${ARCH}/cortex_a57.S		\
+				lib/cpus/${ARCH}/cortex_a72.S		\
+				lib/cpus/${ARCH}/qemu_max.S		\
 
 else
 BL1_SOURCES		+=	lib/cpus/${ARCH}/cortex_a15.S
@@ -195,11 +218,7 @@
 endif
 
 ifeq (${ARM_ARCH_MAJOR},8)
-BL31_SOURCES		+=	lib/cpus/aarch64/aem_generic.S		\
-				lib/cpus/aarch64/cortex_a53.S		\
-				lib/cpus/aarch64/cortex_a57.S		\
-				lib/cpus/aarch64/cortex_a72.S		\
-				lib/cpus/aarch64/qemu_max.S		\
+BL31_SOURCES		+=	${QEMU_CPU_LIBS}			\
 				lib/semihosting/semihosting.c		\
 				lib/semihosting/${ARCH}/semihosting_call.S \
 				plat/common/plat_psci_common.c		\
@@ -282,8 +301,25 @@
 ARM_PRELOADED_DTB_BASE := PLAT_QEMU_DT_BASE
 $(eval $(call add_define,ARM_PRELOADED_DTB_BASE))
 
+# QEMU will use the RNDR instruction for the stack protector canary.
+ENABLE_FEAT_RNG			:= 2
+
 # Later QEMU versions support SME and SVE.
 ifneq (${ARCH},aarch32)
 	ENABLE_SVE_FOR_NS	:= 1
 	ENABLE_SME_FOR_NS	:= 1
 endif
+
+qemu_fw.bios: bl1 fip
+	$(ECHO) "  DD      $@"
+	$(Q)cp ${BUILD_PLAT}/bl1.bin ${BUILD_PLAT}/$@
+	$(Q)dd if=${BUILD_PLAT}/fip.bin of=${BUILD_PLAT}/$@ bs=64k seek=4 status=none
+
+qemu_fw.rom: qemu_fw.bios
+	$(ECHO) "  DD      $@"
+	$(Q)cp ${BUILD_PLAT}/$^ ${BUILD_PLAT}/$@
+	$(Q)dd if=/dev/zero of=${BUILD_PLAT}/$@ bs=1M seek=64 count=0 status=none
+
+ifneq (${BL33},)
+all: qemu_fw.bios qemu_fw.rom
+endif
diff --git a/plat/qemu/qemu_sbsa/platform.mk b/plat/qemu/qemu_sbsa/platform.mk
index 2393b39..fec83db 100644
--- a/plat/qemu/qemu_sbsa/platform.mk
+++ b/plat/qemu/qemu_sbsa/platform.mk
@@ -19,6 +19,11 @@
 # Enable new version of image loading on QEMU platforms
 LOAD_IMAGE_V2		:=	1
 
+CTX_INCLUDE_AARCH32_REGS := 0
+ifeq (${CTX_INCLUDE_AARCH32_REGS}, 1)
+$(error "This is an AArch64-only port; CTX_INCLUDE_AARCH32_REGS must be disabled")
+endif
+
 ifeq ($(NEED_BL32),yes)
 $(eval $(call add_define,QEMU_LOAD_BL32))
 endif
@@ -36,6 +41,18 @@
 				${PLAT_QEMU_COMMON_PATH}/qemu_console.c		\
 				drivers/arm/pl011/${ARCH}/pl011_console.S
 
+# Treating this as a memory-constrained port for now
+USE_COHERENT_MEM	:=	0
+
+# This can be overridden depending on CPU(s) used in the QEMU image
+HW_ASSISTED_COHERENCY	:=	1
+
+QEMU_CPU_LIBS		:=	lib/cpus/aarch64/cortex_a57.S			\
+				lib/cpus/aarch64/cortex_a72.S			\
+				lib/cpus/aarch64/neoverse_n_common.S		\
+				lib/cpus/aarch64/neoverse_n1.S			\
+				lib/cpus/aarch64/qemu_max.S
+
 include lib/xlat_tables_v2/xlat_tables.mk
 PLAT_BL_COMMON_SOURCES	+=	${XLAT_TABLES_LIB_SRCS}
 
@@ -49,9 +66,7 @@
 				${PLAT_QEMU_COMMON_PATH}/${ARCH}/plat_helpers.S	\
 				${PLAT_QEMU_COMMON_PATH}/qemu_bl1_setup.c
 
-BL1_SOURCES		+=	lib/cpus/aarch64/cortex_a57.S			\
-				lib/cpus/aarch64/cortex_a72.S			\
-				lib/cpus/aarch64/qemu_max.S			\
+BL1_SOURCES		+=	${QEMU_CPU_LIBS}
 
 BL2_SOURCES		+=	drivers/io/io_semihosting.c			\
 				drivers/io/io_storage.c				\
@@ -77,9 +92,7 @@
 				plat/common/plat_gicv3.c			\
 				${PLAT_QEMU_COMMON_PATH}/qemu_gicv3.c
 
-BL31_SOURCES		+=	lib/cpus/aarch64/cortex_a57.S			\
-				lib/cpus/aarch64/cortex_a72.S			\
-				lib/cpus/aarch64/qemu_max.S			\
+BL31_SOURCES		+=	${QEMU_CPU_LIBS}				\
 				lib/semihosting/semihosting.c			\
 				lib/semihosting/${ARCH}/semihosting_call.S	\
 				plat/common/plat_psci_common.c			\
diff --git a/plat/qti/common/src/qti_pm.c b/plat/qti/common/src/qti_pm.c
index ae361e9..1405ca6 100644
--- a/plat/qti/common/src/qti_pm.c
+++ b/plat/qti/common/src/qti_pm.c
@@ -24,6 +24,12 @@
 #define QTI_LOCAL_PSTATE_WIDTH		4
 #define QTI_LOCAL_PSTATE_MASK		((1 << QTI_LOCAL_PSTATE_WIDTH) - 1)
 
+#if PSCI_OS_INIT_MODE
+#define QTI_LAST_AT_PLVL_MASK		(QTI_LOCAL_PSTATE_MASK <<	\
+					 (QTI_LOCAL_PSTATE_WIDTH *	\
+					  (PLAT_MAX_PWR_LVL + 1)))
+#endif
+
 /* Make composite power state parameter till level 0 */
 #define qti_make_pwrstate_lvl0(lvl0_state, type) \
 		(((lvl0_state) << PSTATE_ID_SHIFT) | ((type) << PSTATE_TYPE_SHIFT))
@@ -88,7 +94,12 @@
 	 *  search if the number of entries justify the additional complexity.
 	 */
 	for (i = 0; !!qti_pm_idle_states[i]; i++) {
+#if PSCI_OS_INIT_MODE
+		if ((power_state & ~QTI_LAST_AT_PLVL_MASK) ==
+		    qti_pm_idle_states[i])
+#else
 		if (power_state == qti_pm_idle_states[i])
+#endif
 			break;
 	}
 
@@ -100,11 +111,14 @@
 	state_id = psci_get_pstate_id(power_state);
 
 	/* Parse the State ID and populate the state info parameter */
-	while (state_id) {
-		req_state->pwr_domain_state[i++] = state_id &
+	for (i = QTI_PWR_LVL0; i <= PLAT_MAX_PWR_LVL; i++) {
+		req_state->pwr_domain_state[i] = state_id &
 		    QTI_LOCAL_PSTATE_MASK;
 		state_id >>= QTI_LOCAL_PSTATE_WIDTH;
 	}
+#if PSCI_OS_INIT_MODE
+	req_state->last_at_pwrlvl = state_id & QTI_LOCAL_PSTATE_MASK;
+#endif
 
 	return PSCI_E_SUCCESS;
 }
@@ -177,6 +191,18 @@
 	}
 }
 
+#if PSCI_OS_INIT_MODE
+static int qti_node_suspend(const psci_power_state_t *target_state)
+{
+	qtiseclib_psci_node_suspend((const uint8_t *)target_state->
+				    pwr_domain_state);
+	if (is_cpu_off(target_state)) {
+		plat_qti_gic_cpuif_disable();
+		qti_set_cpupwrctlr_val();
+	}
+	return PSCI_E_SUCCESS;
+}
+#else
 static void qti_node_suspend(const psci_power_state_t *target_state)
 {
 	qtiseclib_psci_node_suspend((const uint8_t *)target_state->
@@ -186,6 +212,7 @@
 		qti_set_cpupwrctlr_val();
 	}
 }
+#endif
 
 static void qti_node_suspend_finish(const psci_power_state_t *target_state)
 {
diff --git a/plat/qti/msm8916/platform.mk b/plat/qti/msm8916/platform.mk
index e516cea..2baf203 100644
--- a/plat/qti/msm8916/platform.mk
+++ b/plat/qti/msm8916/platform.mk
@@ -43,8 +43,7 @@
 WARMBOOT_ENABLE_DCACHE_EARLY	:= 1
 
 # Disable features unsupported in ARMv8.0
-ENABLE_AMU			:= 0
-ENABLE_SPE_FOR_LOWER_ELS	:= 0
+ENABLE_SPE_FOR_NS		:= 0
 ENABLE_SVE_FOR_NS		:= 0
 
 # MSM8916 uses ARM Cortex-A53 r0p0 so likely all the errata apply
diff --git a/plat/qti/sc7280/platform.mk b/plat/qti/sc7280/platform.mk
index df07bc4..528a1d4 100644
--- a/plat/qti/sc7280/platform.mk
+++ b/plat/qti/sc7280/platform.mk
@@ -28,6 +28,7 @@
 # Enable PSCI v1.0 extended state ID format
 PSCI_EXTENDED_STATE_ID	:=  1
 ARM_RECOM_STATE_ID_ENC  :=  1
+PSCI_OS_INIT_MODE	:=  1
 
 COLD_BOOT_SINGLE_CPU		:=	1
 PROGRAMMABLE_RESET_ADDRESS	:=	1
diff --git a/plat/xilinx/versal/pm_service/pm_api_sys.h b/plat/xilinx/common/include/pm_api_sys.h
similarity index 96%
rename from plat/xilinx/versal/pm_service/pm_api_sys.h
rename to plat/xilinx/common/include/pm_api_sys.h
index 8625e95..baed43d 100644
--- a/plat/xilinx/versal/pm_service/pm_api_sys.h
+++ b/plat/xilinx/common/include/pm_api_sys.h
@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
- * Copyright (c) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -40,6 +40,7 @@
 					uint8_t enable, uint32_t flag);
 enum pm_ret_status pm_get_callbackdata(uint32_t *data, size_t count, uint32_t flag,
 			 uint32_t ack);
+void pm_client_set_wakeup_sources(uint32_t node_id);
 enum pm_ret_status pm_pll_set_param(uint32_t clk_id, uint32_t param,
 				    uint32_t value, uint32_t flag);
 enum pm_ret_status pm_pll_get_param(uint32_t clk_id, uint32_t param,
diff --git a/plat/xilinx/common/include/pm_client.h b/plat/xilinx/common/include/pm_client.h
index eae1d98..8bf4ae3 100644
--- a/plat/xilinx/common/include/pm_client.h
+++ b/plat/xilinx/common/include/pm_client.h
@@ -1,7 +1,7 @@
 /*
  * Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved.
  * Copyright (c) 2020-2022, Xilinx, Inc. All rights reserved.
- * Copyright (c) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -22,6 +22,10 @@
 void pm_client_abort_suspend(void);
 void pm_client_wakeup(const struct pm_proc *proc);
 
+#if !defined(PLAT_zynqmp)
+enum pm_device_node_idx irq_to_pm_node_idx(uint32_t irq);
+#endif
+
 /* Global variables to be set in pm_client.c */
 extern const struct pm_proc *primary_proc;
 
diff --git a/plat/xilinx/common/include/pm_defs.h b/plat/xilinx/common/include/pm_defs.h
new file mode 100644
index 0000000..0188443
--- /dev/null
+++ b/plat/xilinx/common/include/pm_defs.h
@@ -0,0 +1,279 @@
+/*
+ * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/* Versal power management enums and defines */
+
+#ifndef PM_DEFS_H
+#define PM_DEFS_H
+
+#include "pm_node.h"
+
+/*********************************************************************
+ * Macro definitions
+ ********************************************************************/
+
+/* State arguments of the self suspend */
+#define PM_STATE_CPU_IDLE	0x0U
+#define PM_STATE_SUSPEND_TO_RAM	0xFU
+
+#define MAX_LATENCY		(~0U)
+#define MAX_QOS			100U
+
+/* Processor core device IDs */
+#define APU_DEVID(IDX)	NODEID(XPM_NODECLASS_DEVICE, XPM_NODESUBCL_DEV_CORE, \
+			       XPM_NODETYPE_DEV_CORE_APU, (IDX))
+
+#define XPM_DEVID_ACPU_0	APU_DEVID(XPM_NODEIDX_DEV_ACPU_0)
+#define XPM_DEVID_ACPU_1	APU_DEVID(XPM_NODEIDX_DEV_ACPU_1)
+
+#define PERIPH_DEVID(IDX)	NODEID((uint32_t)XPM_NODECLASS_DEVICE, \
+				       (uint32_t)XPM_NODESUBCL_DEV_PERIPH, \
+				       (uint32_t)XPM_NODETYPE_DEV_PERIPH, (IDX))
+
+#define PM_GET_CALLBACK_DATA		0xa01U
+#define PM_GET_TRUSTZONE_VERSION	0xa03U
+#define TF_A_PM_REGISTER_SGI		0xa04U
+
+/* PM API Versions */
+#define PM_API_BASE_VERSION		1U
+#define PM_API_VERSION_2		2U
+
+/* Loader API ids */
+#define PM_LOAD_PDI			0x701U
+#define PM_LOAD_GET_HANDOFF_PARAMS	0x70BU
+
+/* System shutdown macros */
+#define	XPM_SHUTDOWN_TYPE_SHUTDOWN	0U
+#define	XPM_SHUTDOWN_TYPE_RESET		1U
+#define	XPM_SHUTDOWN_TYPE_SETSCOPE_ONLY	2U
+
+#define	XPM_SHUTDOWN_SUBTYPE_RST_SUBSYSTEM	0U
+#define	XPM_SHUTDOWN_SUBTYPE_RST_PS_ONLY	1U
+#define	XPM_SHUTDOWN_SUBTYPE_RST_SYSTEM		2U
+
+/*********************************************************************
+ * Enum definitions
+ ********************************************************************/
+
+//ioctl id
+enum {
+	IOCTL_GET_RPU_OPER_MODE = 0,
+	IOCTL_SET_RPU_OPER_MODE = 1,
+	IOCTL_RPU_BOOT_ADDR_CONFIG = 2,
+	IOCTL_TCM_COMB_CONFIG = 3,
+	IOCTL_SET_TAPDELAY_BYPASS = 4,
+	IOCTL_SET_SGMII_MODE = 5,
+	IOCTL_SD_DLL_RESET = 6,
+	IOCTL_SET_SD_TAPDELAY = 7,
+	 /* Ioctl for clock driver */
+	IOCTL_SET_PLL_FRAC_MODE = 8,
+	IOCTL_GET_PLL_FRAC_MODE = 9,
+	IOCTL_SET_PLL_FRAC_DATA = 10,
+	IOCTL_GET_PLL_FRAC_DATA = 11,
+	IOCTL_WRITE_GGS = 12,
+	IOCTL_READ_GGS = 13,
+	IOCTL_WRITE_PGGS = 14,
+	IOCTL_READ_PGGS = 15,
+	/* IOCTL for ULPI reset */
+	IOCTL_ULPI_RESET = 16,
+	/* Set healthy bit value */
+	IOCTL_SET_BOOT_HEALTH_STATUS = 17,
+	IOCTL_AFI = 18,
+	/* Probe counter read/write */
+	IOCTL_PROBE_COUNTER_READ = 19,
+	IOCTL_PROBE_COUNTER_WRITE = 20,
+	IOCTL_OSPI_MUX_SELECT = 21,
+	/* IOCTL for USB power request */
+	IOCTL_USB_SET_STATE = 22,
+	/* IOCTL to get last reset reason */
+	IOCTL_GET_LAST_RESET_REASON = 23,
+	/* AI engine NPI ISR clear */
+	IOCTL_AIE_ISR_CLEAR = 24,
+	/* Register SGI to TF-A */
+	IOCTL_SET_SGI = 25,
+};
+
+/**
+ * @PM_PLL_PARAM_DIV2:		Enable for divide by 2 function inside the PLL
+ * @PM_PLL_PARAM_FBDIV:		Feedback divisor integer portion for the PLL
+ * @PM_PLL_PARAM_DATA:		Feedback divisor fractional portion for the PLL
+ * @PM_PLL_PARAM_PRE_SRC:	Clock source for PLL input
+ * @PM_PLL_PARAM_POST_SRC:	Clock source for PLL Bypass mode
+ * @PM_PLL_PARAM_LOCK_DLY:	Lock circuit config settings for lock windowsize
+ * @PM_PLL_PARAM_LOCK_CNT:	Lock circuit counter setting
+ * @PM_PLL_PARAM_LFHF:		PLL loop filter high frequency capacitor control
+ * @PM_PLL_PARAM_CP:		PLL charge pump control
+ * @PM_PLL_PARAM_RES:		PLL loop filter resistor control
+ */
+enum pm_pll_param {
+	PM_PLL_PARAM_DIV2,
+	PM_PLL_PARAM_FBDIV,
+	PM_PLL_PARAM_DATA,
+	PM_PLL_PARAM_PRE_SRC,
+	PM_PLL_PARAM_POST_SRC,
+	PM_PLL_PARAM_LOCK_DLY,
+	PM_PLL_PARAM_LOCK_CNT,
+	PM_PLL_PARAM_LFHF,
+	PM_PLL_PARAM_CP,
+	PM_PLL_PARAM_RES,
+	PM_PLL_PARAM_MAX,
+};
+
+enum pm_api_id {
+	/* Miscellaneous API functions: */
+	PM_GET_API_VERSION = 1, /* Do not change or move */
+	PM_SET_CONFIGURATION,
+	PM_GET_NODE_STATUS,
+	PM_GET_OP_CHARACTERISTIC,
+	PM_REGISTER_NOTIFIER,
+	/* API for suspending of PUs: */
+	PM_REQ_SUSPEND,
+	PM_SELF_SUSPEND,
+	PM_FORCE_POWERDOWN,
+	PM_ABORT_SUSPEND,
+	PM_REQ_WAKEUP,
+	PM_SET_WAKEUP_SOURCE,
+	PM_SYSTEM_SHUTDOWN,
+	/* API for managing PM slaves: */
+	PM_REQ_NODE,
+	PM_RELEASE_NODE,
+	PM_SET_REQUIREMENT,
+	PM_SET_MAX_LATENCY,
+	/* Direct control API functions: */
+	PM_RESET_ASSERT,
+	PM_RESET_GET_STATUS,
+	PM_MMIO_WRITE,
+	PM_MMIO_READ,
+	PM_INIT_FINALIZE,
+	PM_FPGA_LOAD,
+	PM_FPGA_GET_STATUS,
+	PM_GET_CHIPID,
+	PM_SECURE_RSA_AES,
+	PM_SECURE_SHA,
+	PM_SECURE_RSA,
+	PM_PINCTRL_REQUEST,
+	PM_PINCTRL_RELEASE,
+	PM_PINCTRL_GET_FUNCTION,
+	PM_PINCTRL_SET_FUNCTION,
+	PM_PINCTRL_CONFIG_PARAM_GET,
+	PM_PINCTRL_CONFIG_PARAM_SET,
+	PM_IOCTL,
+	/* API to query information from firmware */
+	PM_QUERY_DATA,
+	/* Clock control API functions */
+	PM_CLOCK_ENABLE,
+	PM_CLOCK_DISABLE,
+	PM_CLOCK_GETSTATE,
+	PM_CLOCK_SETDIVIDER,
+	PM_CLOCK_GETDIVIDER,
+	PM_CLOCK_SETRATE,
+	PM_CLOCK_GETRATE,
+	PM_CLOCK_SETPARENT,
+	PM_CLOCK_GETPARENT,
+	PM_SECURE_IMAGE,
+	/* FPGA PL Readback */
+	PM_FPGA_READ,
+	PM_SECURE_AES,
+	/* PLL control API functions */
+	PM_PLL_SET_PARAMETER,
+	PM_PLL_GET_PARAMETER,
+	PM_PLL_SET_MODE,
+	PM_PLL_GET_MODE,
+	/* PM Register Access API */
+	PM_REGISTER_ACCESS,
+	PM_EFUSE_ACCESS,
+	PM_FPGA_GET_VERSION,
+	PM_FPGA_GET_FEATURE_LIST,
+	PM_FEATURE_CHECK = 63,
+	PM_API_MAX = 74
+};
+
+enum pm_abort_reason {
+	ABORT_REASON_WKUP_EVENT = 100,
+	ABORT_REASON_PU_BUSY,
+	ABORT_REASON_NO_PWRDN,
+	ABORT_REASON_UNKNOWN,
+};
+
+enum pm_opchar_type {
+	PM_OPCHAR_TYPE_POWER = 1,
+	PM_OPCHAR_TYPE_TEMP,
+	PM_OPCHAR_TYPE_LATENCY,
+};
+
+/**
+ * Subsystem IDs
+ */
+typedef enum {
+	XPM_SUBSYSID_PMC,
+	XPM_SUBSYSID_PSM,
+	XPM_SUBSYSID_APU,
+	XPM_SUBSYSID_RPU0_LOCK,
+	XPM_SUBSYSID_RPU0_0,
+	XPM_SUBSYSID_RPU0_1,
+	XPM_SUBSYSID_DDR0,
+	XPM_SUBSYSID_ME,
+	XPM_SUBSYSID_PL,
+	XPM_SUBSYSID_MAX,
+} XPm_SubsystemId;
+
+/* TODO: move pm_ret_status from device specific location to common location */
+/**
+ * @PM_RET_SUCCESS:		success
+ * @PM_RET_ERROR_ARGS:		illegal arguments provided (deprecated)
+ * @PM_RET_ERROR_NOTSUPPORTED:	feature not supported  (deprecated)
+ * @PM_RET_ERROR_NOFEATURE:	feature is not available
+ * @PM_RET_ERROR_INVALID_CRC:	invalid crc in IPI communication
+ * @PM_RET_ERROR_NOT_ENABLED:   feature is not enabled
+ * @PM_RET_ERROR_INTERNAL:	internal error
+ * @PM_RET_ERROR_CONFLICT:	conflict
+ * @PM_RET_ERROR_ACCESS:	access rights violation
+ * @PM_RET_ERROR_INVALID_NODE:	invalid node
+ * @PM_RET_ERROR_DOUBLE_REQ:	duplicate request for same node
+ * @PM_RET_ERROR_ABORT_SUSPEND:	suspend procedure has been aborted
+ * @PM_RET_ERROR_TIMEOUT:	timeout in communication with PMU
+ * @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_NOFEATURE = 19,
+	PM_RET_ERROR_INVALID_CRC = 301,
+	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
+};
+
+/**
+ * Qids
+ */
+enum pm_query_id {
+	XPM_QID_INVALID,
+	XPM_QID_CLOCK_GET_NAME,
+	XPM_QID_CLOCK_GET_TOPOLOGY,
+	XPM_QID_CLOCK_GET_FIXEDFACTOR_PARAMS,
+	XPM_QID_CLOCK_GET_MUXSOURCES,
+	XPM_QID_CLOCK_GET_ATTRIBUTES,
+	XPM_QID_PINCTRL_GET_NUM_PINS,
+	XPM_QID_PINCTRL_GET_NUM_FUNCTIONS,
+	XPM_QID_PINCTRL_GET_NUM_FUNCTION_GROUPS,
+	XPM_QID_PINCTRL_GET_FUNCTION_NAME,
+	XPM_QID_PINCTRL_GET_FUNCTION_GROUPS,
+	XPM_QID_PINCTRL_GET_PIN_GROUPS,
+	XPM_QID_CLOCK_GET_NUM_CLOCKS,
+	XPM_QID_CLOCK_GET_MAX_DIVISOR,
+	XPM_QID_PLD_GET_PARENT,
+};
+#endif /* PM_DEFS_H */
diff --git a/plat/xilinx/common/include/pm_node.h b/plat/xilinx/common/include/pm_node.h
new file mode 100644
index 0000000..b6c2d81
--- /dev/null
+++ b/plat/xilinx/common/include/pm_node.h
@@ -0,0 +1,243 @@
+/*
+ * Copyright (c) 2019, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/* Versal PM nodes enums and defines */
+
+#ifndef PM_NODE_H
+#define PM_NODE_H
+
+/*********************************************************************
+ * Macro definitions
+ ********************************************************************/
+
+#define NODE_CLASS_SHIFT	26U
+#define NODE_SUBCLASS_SHIFT	20U
+#define NODE_TYPE_SHIFT		14U
+#define NODE_INDEX_SHIFT	0U
+#define NODE_CLASS_MASK_BITS    GENMASK_32(5, 0)
+#define NODE_SUBCLASS_MASK_BITS GENMASK_32(5, 0)
+#define NODE_TYPE_MASK_BITS     GENMASK_32(5, 0)
+#define NODE_INDEX_MASK_BITS    GENMASK_32(13, 0)
+#define NODE_CLASS_MASK         (NODE_CLASS_MASK_BITS << NODE_CLASS_SHIFT)
+#define NODE_SUBCLASS_MASK      (NODE_SUBCLASS_MASK_BITS << NODE_SUBCLASS_SHIFT)
+#define NODE_TYPE_MASK          (NODE_TYPE_MASK_BITS << NODE_TYPE_SHIFT)
+#define NODE_INDEX_MASK         (NODE_INDEX_MASK_BITS << NODE_INDEX_SHIFT)
+
+#define NODEID(CLASS, SUBCLASS, TYPE, INDEX)	\
+	     ((((CLASS) & NODE_CLASS_MASK_BITS) << NODE_CLASS_SHIFT) | \
+	     (((SUBCLASS) & NODE_SUBCLASS_MASK_BITS) << NODE_SUBCLASS_SHIFT) | \
+	     (((TYPE) & NODE_TYPE_MASK_BITS) << NODE_TYPE_SHIFT) | \
+	     (((INDEX) & NODE_INDEX_MASK_BITS) << NODE_INDEX_SHIFT))
+
+#define NODECLASS(ID)		(((ID) & NODE_CLASS_MASK) >> NODE_CLASS_SHIFT)
+#define NODESUBCLASS(ID)	(((ID) & NODE_SUBCLASS_MASK) >> \
+				NODE_SUBCLASS_SHIFT)
+#define NODETYPE(ID)		(((ID) & NODE_TYPE_MASK) >> NODE_TYPE_SHIFT)
+#define NODEINDEX(ID)		(((ID) & NODE_INDEX_MASK) >> NODE_INDEX_SHIFT)
+
+/*********************************************************************
+ * Enum definitions
+ ********************************************************************/
+
+/* Node class types */
+enum pm_node_class {
+	XPM_NODECLASS_MIN,
+
+	XPM_NODECLASS_POWER,
+	XPM_NODECLASS_CLOCK,
+	XPM_NODECLASS_RESET,
+	XPM_NODECLASS_MEMIC,
+	XPM_NODECLASS_STMIC,
+	XPM_NODECLASS_DEVICE,
+
+	XPM_NODECLASS_MAX
+};
+
+enum pm_device_node_subclass {
+	/* Device types */
+	XPM_NODESUBCL_DEV_CORE = 1,
+	XPM_NODESUBCL_DEV_PERIPH,
+	XPM_NODESUBCL_DEV_MEM,
+	XPM_NODESUBCL_DEV_SOC,
+	XPM_NODESUBCL_DEV_MEM_CTRLR,
+	XPM_NODESUBCL_DEV_PHY,
+};
+
+enum pm_device_node_type {
+	/* Device types */
+	XPM_NODETYPE_DEV_CORE_PMC = 1,
+	XPM_NODETYPE_DEV_CORE_PSM,
+	XPM_NODETYPE_DEV_CORE_APU,
+	XPM_NODETYPE_DEV_CORE_RPU,
+	XPM_NODETYPE_DEV_OCM,
+	XPM_NODETYPE_DEV_TCM,
+	XPM_NODETYPE_DEV_L2CACHE,
+	XPM_NODETYPE_DEV_DDR,
+	XPM_NODETYPE_DEV_PERIPH,
+	XPM_NODETYPE_DEV_SOC,
+	XPM_NODETYPE_DEV_GT,
+};
+
+/* Device node Indexes */
+enum pm_device_node_idx {
+	/* Device nodes */
+	XPM_NODEIDX_DEV_MIN = 0x0,
+
+	/* Processor devices */
+	XPM_NODEIDX_DEV_PMC_PROC = 0x1,
+	XPM_NODEIDX_DEV_PSM_PROC = 0x2,
+	XPM_NODEIDX_DEV_ACPU_0 = 0x3,
+	XPM_NODEIDX_DEV_ACPU_1 = 0x4,
+	XPM_NODEIDX_DEV_RPU0_0 = 0x5,
+	XPM_NODEIDX_DEV_RPU0_1 = 0x6,
+
+	/* Memory devices */
+	XPM_NODEIDX_DEV_OCM_0 = 0x7,
+	XPM_NODEIDX_DEV_OCM_1 = 0x8,
+	XPM_NODEIDX_DEV_OCM_2 = 0x9,
+	XPM_NODEIDX_DEV_OCM_3 = 0xA,
+	XPM_NODEIDX_DEV_TCM_0_A = 0xB,
+	XPM_NODEIDX_DEV_TCM_0_B = 0xC,
+	XPM_NODEIDX_DEV_TCM_1_A = 0xD,
+	XPM_NODEIDX_DEV_TCM_1_B = 0xE,
+	XPM_NODEIDX_DEV_L2_BANK_0 = 0xF,
+	XPM_NODEIDX_DEV_DDR_0 = 0x10,
+	XPM_NODEIDX_DEV_DDR_1 = 0x11,
+	XPM_NODEIDX_DEV_DDR_2 = 0x12,
+	XPM_NODEIDX_DEV_DDR_3 = 0x13,
+	XPM_NODEIDX_DEV_DDR_4 = 0x14,
+	XPM_NODEIDX_DEV_DDR_5 = 0x15,
+	XPM_NODEIDX_DEV_DDR_6 = 0x16,
+	XPM_NODEIDX_DEV_DDR_7 = 0x17,
+
+	/* LPD Peripheral devices */
+	XPM_NODEIDX_DEV_USB_0 = 0x18,
+	XPM_NODEIDX_DEV_GEM_0 = 0x19,
+	XPM_NODEIDX_DEV_GEM_1 = 0x1A,
+	XPM_NODEIDX_DEV_SPI_0 = 0x1B,
+	XPM_NODEIDX_DEV_SPI_1 = 0x1C,
+	XPM_NODEIDX_DEV_I2C_0 = 0x1D,
+	XPM_NODEIDX_DEV_I2C_1 = 0x1E,
+	XPM_NODEIDX_DEV_CAN_FD_0 = 0x1F,
+	XPM_NODEIDX_DEV_CAN_FD_1 = 0x20,
+	XPM_NODEIDX_DEV_UART_0 = 0x21,
+	XPM_NODEIDX_DEV_UART_1 = 0x22,
+	XPM_NODEIDX_DEV_GPIO = 0x23,
+	XPM_NODEIDX_DEV_TTC_0 = 0x24,
+	XPM_NODEIDX_DEV_TTC_1 = 0x25,
+	XPM_NODEIDX_DEV_TTC_2 = 0x26,
+	XPM_NODEIDX_DEV_TTC_3 = 0x27,
+	XPM_NODEIDX_DEV_SWDT_LPD = 0x28,
+
+	/* FPD Peripheral devices */
+	XPM_NODEIDX_DEV_SWDT_FPD = 0x29,
+
+	/* PMC Peripheral devices */
+	XPM_NODEIDX_DEV_OSPI = 0x2A,
+	XPM_NODEIDX_DEV_QSPI = 0x2B,
+	XPM_NODEIDX_DEV_GPIO_PMC = 0x2C,
+	XPM_NODEIDX_DEV_I2C_PMC = 0x2D,
+	XPM_NODEIDX_DEV_SDIO_0 = 0x2E,
+	XPM_NODEIDX_DEV_SDIO_1 = 0x2F,
+
+	XPM_NODEIDX_DEV_PL_0 = 0x30,
+	XPM_NODEIDX_DEV_PL_1 = 0x31,
+	XPM_NODEIDX_DEV_PL_2 = 0x32,
+	XPM_NODEIDX_DEV_PL_3 = 0x33,
+	XPM_NODEIDX_DEV_RTC = 0x34,
+	XPM_NODEIDX_DEV_ADMA_0 = 0x35,
+	XPM_NODEIDX_DEV_ADMA_1 = 0x36,
+	XPM_NODEIDX_DEV_ADMA_2 = 0x37,
+	XPM_NODEIDX_DEV_ADMA_3 = 0x38,
+	XPM_NODEIDX_DEV_ADMA_4 = 0x39,
+	XPM_NODEIDX_DEV_ADMA_5 = 0x3A,
+	XPM_NODEIDX_DEV_ADMA_6 = 0x3B,
+	XPM_NODEIDX_DEV_ADMA_7 = 0x3C,
+	XPM_NODEIDX_DEV_IPI_0 = 0x3D,
+	XPM_NODEIDX_DEV_IPI_1 = 0x3E,
+	XPM_NODEIDX_DEV_IPI_2 = 0x3F,
+	XPM_NODEIDX_DEV_IPI_3 = 0x40,
+	XPM_NODEIDX_DEV_IPI_4 = 0x41,
+	XPM_NODEIDX_DEV_IPI_5 = 0x42,
+	XPM_NODEIDX_DEV_IPI_6 = 0x43,
+
+	/* Entire SoC */
+	XPM_NODEIDX_DEV_SOC = 0x44,
+
+	/* DDR memory controllers */
+	XPM_NODEIDX_DEV_DDRMC_0 = 0x45,
+	XPM_NODEIDX_DEV_DDRMC_1 = 0x46,
+	XPM_NODEIDX_DEV_DDRMC_2 = 0x47,
+	XPM_NODEIDX_DEV_DDRMC_3 = 0x48,
+
+	/* GT devices */
+	XPM_NODEIDX_DEV_GT_0 = 0x49,
+	XPM_NODEIDX_DEV_GT_1 = 0x4A,
+	XPM_NODEIDX_DEV_GT_2 = 0x4B,
+	XPM_NODEIDX_DEV_GT_3 = 0x4C,
+	XPM_NODEIDX_DEV_GT_4 = 0x4D,
+	XPM_NODEIDX_DEV_GT_5 = 0x4E,
+	XPM_NODEIDX_DEV_GT_6 = 0x4F,
+	XPM_NODEIDX_DEV_GT_7 = 0x50,
+	XPM_NODEIDX_DEV_GT_8 = 0x51,
+	XPM_NODEIDX_DEV_GT_9 = 0x52,
+	XPM_NODEIDX_DEV_GT_10 = 0x53,
+
+#if defined(PLAT_versal_net)
+	XPM_NODEIDX_DEV_ACPU_0_0 = 0x54,
+	XPM_NODEIDX_DEV_ACPU_0_1 = 0x55,
+	XPM_NODEIDX_DEV_ACPU_0_2 = 0x56,
+	XPM_NODEIDX_DEV_ACPU_0_3 = 0x57,
+	XPM_NODEIDX_DEV_ACPU_1_0 = 0x58,
+	XPM_NODEIDX_DEV_ACPU_1_1 = 0x59,
+	XPM_NODEIDX_DEV_ACPU_1_2 = 0x5A,
+	XPM_NODEIDX_DEV_ACPU_1_3 = 0x5B,
+	XPM_NODEIDX_DEV_ACPU_2_0 = 0x5C,
+	XPM_NODEIDX_DEV_ACPU_2_1 = 0x5D,
+	XPM_NODEIDX_DEV_ACPU_2_2 = 0x5E,
+	XPM_NODEIDX_DEV_ACPU_2_3 = 0x5F,
+	XPM_NODEIDX_DEV_ACPU_3_0 = 0x60,
+	XPM_NODEIDX_DEV_ACPU_3_1 = 0x61,
+	XPM_NODEIDX_DEV_ACPU_3_2 = 0x62,
+	XPM_NODEIDX_DEV_ACPU_3_3 = 0x63,
+	XPM_NODEIDX_DEV_RPU_A_0 = 0x64,
+	XPM_NODEIDX_DEV_RPU_A_1 = 0x65,
+	XPM_NODEIDX_DEV_RPU_B_0 = 0x66,
+	XPM_NODEIDX_DEV_RPU_B_1 = 0x67,
+	XPM_NODEIDX_DEV_OCM_0_0 = 0x68,
+	XPM_NODEIDX_DEV_OCM_0_1 = 0x69,
+	XPM_NODEIDX_DEV_OCM_0_2 = 0x6A,
+	XPM_NODEIDX_DEV_OCM_0_3 = 0x6B,
+	XPM_NODEIDX_DEV_OCM_1_0 = 0x6C,
+	XPM_NODEIDX_DEV_OCM_1_1 = 0x6D,
+	XPM_NODEIDX_DEV_OCM_1_2 = 0x6E,
+	XPM_NODEIDX_DEV_OCM_1_3 = 0x6F,
+	XPM_NODEIDX_DEV_TCM_A_0A = 0x70,
+	XPM_NODEIDX_DEV_TCM_A_0B = 0x71,
+	XPM_NODEIDX_DEV_TCM_A_0C = 0x72,
+	XPM_NODEIDX_DEV_TCM_A_1A = 0x73,
+	XPM_NODEIDX_DEV_TCM_A_1B = 0x74,
+	XPM_NODEIDX_DEV_TCM_A_1C = 0x75,
+	XPM_NODEIDX_DEV_TCM_B_0A = 0x76,
+	XPM_NODEIDX_DEV_TCM_B_0B = 0x77,
+	XPM_NODEIDX_DEV_TCM_B_0C = 0x78,
+	XPM_NODEIDX_DEV_TCM_B_1A = 0x79,
+	XPM_NODEIDX_DEV_TCM_B_1B = 0x7A,
+	XPM_NODEIDX_DEV_TCM_B_1C = 0x7B,
+	XPM_NODEIDX_DEV_USB_1 = 0x7C,
+	XPM_NODEIDX_DEV_PMC_WWDT = 0x7D,
+	XPM_NODEIDX_DEV_LPD_SWDT_0 = 0x7E,
+	XPM_NODEIDX_DEV_LPD_SWDT_1 = 0x7F,
+	XPM_NODEIDX_DEV_FPD_SWDT_0 = 0x80,
+	XPM_NODEIDX_DEV_FPD_SWDT_1 = 0x81,
+	XPM_NODEIDX_DEV_FPD_SWDT_2 = 0x82,
+	XPM_NODEIDX_DEV_FPD_SWDT_3 = 0x83,
+#endif
+	XPM_NODEIDX_DEV_MAX,
+};
+
+#endif /* PM_NODE_H */
diff --git a/plat/xilinx/versal/pm_service/pm_svc_main.h b/plat/xilinx/common/include/pm_svc_main.h
similarity index 84%
rename from plat/xilinx/versal/pm_service/pm_svc_main.h
rename to plat/xilinx/common/include/pm_svc_main.h
index b6e764f..1a27bdf 100644
--- a/plat/xilinx/versal/pm_service/pm_svc_main.h
+++ b/plat/xilinx/common/include/pm_svc_main.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/xilinx/versal/pm_service/pm_api_sys.c b/plat/xilinx/common/pm_service/pm_api_sys.c
similarity index 90%
rename from plat/xilinx/versal/pm_service/pm_api_sys.c
rename to plat/xilinx/common/pm_service/pm_api_sys.c
index cc99f11..c36a0ec 100644
--- a/plat/xilinx/versal/pm_service/pm_api_sys.c
+++ b/plat/xilinx/common/pm_service/pm_api_sys.c
@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
- * Copyright (c) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,14 +10,20 @@
  * IPI interrupts
  */
 
+#include <drivers/arm/gic_common.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+#include <plat/common/platform.h>
+#include <platform_def.h>
+#include <pm_api_sys.h>
+#include <pm_client.h>
 #include <pm_common.h>
+#include <pm_defs.h>
 #include <pm_ipi.h>
-#include <plat/common/platform.h>
-#include "pm_api_sys.h"
-#include "pm_client.h"
-#include "pm_defs.h"
 #include "pm_svc_main.h"
 
+#define NUM_GICD_ISENABLER	((IRQ_MAX >> 5U) + 1U)
+
 /* default shutdown/reboot scope is system(2) */
 static uint32_t pm_shutdown_scope = XPM_SHUTDOWN_SUBTYPE_RST_SYSTEM;
 
@@ -34,6 +40,57 @@
 /* PM API functions */
 
 /**
+ * pm_client_set_wakeup_sources - Set all devices with enabled interrupts as
+ *                                wake sources in the XilPM.
+ * @node_id:    Node id of processor
+ */
+void pm_client_set_wakeup_sources(uint32_t node_id)
+{
+	uint32_t reg_num, device_id;
+	uint8_t pm_wakeup_nodes_set[XPM_NODEIDX_DEV_MAX] = {0U};
+	uint32_t isenabler1 = PLAT_GICD_BASE_VALUE + GICD_ISENABLER + 4U;
+
+	zeromem(&pm_wakeup_nodes_set, (u_register_t)sizeof(pm_wakeup_nodes_set));
+
+	for (reg_num = 0U; reg_num < NUM_GICD_ISENABLER; reg_num++) {
+		uint32_t base_irq = reg_num << ISENABLER_SHIFT;
+		uint32_t reg = mmio_read_32(isenabler1 + (reg_num << 2));
+
+		if (reg == 0U) {
+			continue;
+		}
+
+		while (reg != 0U) {
+			enum pm_device_node_idx node_idx;
+			uint32_t idx, irq, lowest_set = reg & (-reg);
+			enum pm_ret_status ret;
+
+			idx = (uint32_t)__builtin_ctz(lowest_set);
+			irq = base_irq + idx;
+
+			if (irq > IRQ_MAX) {
+				break;
+			}
+
+			node_idx = irq_to_pm_node_idx(irq);
+			reg &= ~lowest_set;
+
+			if (node_idx > XPM_NODEIDX_DEV_MIN) {
+				if (pm_wakeup_nodes_set[node_idx] == 0U) {
+					/* Get device ID from node index */
+					device_id = PERIPH_DEVID((uint32_t)node_idx);
+					ret = pm_set_wakeup_source(node_id,
+								   device_id, 1U,
+								   SECURE_FLAG);
+					pm_wakeup_nodes_set[node_idx] = (ret == PM_RET_SUCCESS) ?
+										 1U : 0U;
+				}
+			}
+		}
+	}
+}
+
+/**
  * pm_handle_eemi_call() - PM call for processor to send eemi payload
  * @flag	0 - Call from secure source
  *		1 - Call from non-secure source
@@ -404,7 +461,7 @@
 	PM_PACK_PAYLOAD5(payload, LIBPM_MODULE_ID, flag, PM_QUERY_DATA, qid,
 			 arg1, arg2, arg3);
 
-	ret = pm_feature_check(PM_QUERY_DATA, &version[0], flag);
+	ret = pm_feature_check((uint32_t)PM_QUERY_DATA, &version[0], flag);
 	if (ret == PM_RET_SUCCESS) {
 		fw_api_version = version[0] & 0xFFFFU;
 		if ((fw_api_version == 2U) &&
@@ -458,10 +515,10 @@
 		ret =  pm_pll_get_mode(arg1, value, flag);
 		break;
 	case IOCTL_SET_PLL_FRAC_DATA:
-		ret =  pm_pll_set_param(arg1, PM_PLL_PARAM_DATA, arg2, flag);
+		ret =  pm_pll_set_param(arg1, (uint32_t)PM_PLL_PARAM_DATA, arg2, flag);
 		break;
 	case IOCTL_GET_PLL_FRAC_DATA:
-		ret =  pm_pll_get_param(arg1, PM_PLL_PARAM_DATA, value, flag);
+		ret =  pm_pll_get_param(arg1, (uint32_t)PM_PLL_PARAM_DATA, value, flag);
 		break;
 	case IOCTL_SET_SGI:
 		/* Get the sgi number */
diff --git a/plat/xilinx/common/pm_service/pm_ipi.c b/plat/xilinx/common/pm_service/pm_ipi.c
index 37d0384..b19fc10 100644
--- a/plat/xilinx/common/pm_service/pm_ipi.c
+++ b/plat/xilinx/common/pm_service/pm_ipi.c
@@ -10,6 +10,7 @@
 #include <arch_helpers.h>
 #include <lib/bakery_lock.h>
 #include <lib/mmio.h>
+#include <lib/spinlock.h>
 #include <ipi.h>
 #include <plat_ipi.h>
 #include <plat_private.h>
@@ -21,7 +22,33 @@
 #define ERROR_CODE_MASK		(0xFFFFU)
 #define PM_OFFSET		(0U)
 
+/*
+ * ARM v8.2, the cache will turn off automatically when cpu
+ * power down. Therefore, there is no doubt to use the spin_lock here.
+ */
+#if !HW_ASSISTED_COHERENCY
 DEFINE_BAKERY_LOCK(pm_secure_lock);
+static inline void pm_ipi_lock_get(void)
+{
+	bakery_lock_get(&pm_secure_lock);
+}
+
+static inline void pm_ipi_lock_release(void)
+{
+	bakery_lock_release(&pm_secure_lock);
+}
+#else
+spinlock_t pm_secure_lock;
+static inline void pm_ipi_lock_get(void)
+{
+	spin_lock(&pm_secure_lock);
+}
+
+static inline void pm_ipi_lock_release(void)
+{
+	spin_unlock(&pm_secure_lock);
+}
+#endif
 
 /**
  * pm_ipi_init() - Initialize IPI peripheral for communication with
@@ -36,7 +63,6 @@
  */
 void pm_ipi_init(const struct pm_proc *proc)
 {
-	bakery_lock_init(&pm_secure_lock);
 	ipi_mb_open(proc->ipi->local_ipi_id, proc->ipi->remote_ipi_id);
 }
 
@@ -90,11 +116,11 @@
 {
 	enum pm_ret_status ret;
 
-	bakery_lock_get(&pm_secure_lock);
+	pm_ipi_lock_get();
 
 	ret = pm_ipi_send_common(proc, payload, IPI_NON_BLOCKING);
 
-	bakery_lock_release(&pm_secure_lock);
+	pm_ipi_lock_release();
 
 	return ret;
 }
@@ -113,11 +139,11 @@
 {
 	enum pm_ret_status ret;
 
-	bakery_lock_get(&pm_secure_lock);
+	pm_ipi_lock_get();
 
 	ret = pm_ipi_send_common(proc, payload, IPI_BLOCKING);
 
-	bakery_lock_release(&pm_secure_lock);
+	pm_ipi_lock_release();
 
 	return ret;
 }
@@ -249,7 +275,7 @@
 {
 	enum pm_ret_status ret;
 
-	bakery_lock_get(&pm_secure_lock);
+	pm_ipi_lock_get();
 
 	ret = pm_ipi_send_common(proc, payload, IPI_BLOCKING);
 	if (ret != PM_RET_SUCCESS) {
@@ -259,7 +285,7 @@
 	ret = ERROR_CODE_MASK & (pm_ipi_buff_read(proc, value, count));
 
 unlock:
-	bakery_lock_release(&pm_secure_lock);
+	pm_ipi_lock_release();
 
 	return ret;
 }
diff --git a/plat/xilinx/versal/pm_service/pm_svc_main.c b/plat/xilinx/common/pm_service/pm_svc_main.c
similarity index 96%
rename from plat/xilinx/versal/pm_service/pm_svc_main.c
rename to plat/xilinx/common/pm_service/pm_svc_main.c
index 185bfdb..1bd2192 100644
--- a/plat/xilinx/versal/pm_service/pm_svc_main.c
+++ b/plat/xilinx/common/pm_service/pm_svc_main.c
@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
- * Copyright (c) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -168,7 +168,7 @@
 
 	switch (api_id) {
 
-	case PM_IOCTL:
+	case (uint32_t)PM_IOCTL:
 	{
 		uint32_t value = 0U;
 
@@ -181,7 +181,7 @@
 		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32U);
 	}
 
-	case PM_QUERY_DATA:
+	case (uint32_t)PM_QUERY_DATA:
 	{
 		uint32_t data[PAYLOAD_ARG_CNT] = { 0 };
 
@@ -192,7 +192,7 @@
 			 (uint64_t)data[1] | ((uint64_t)data[2] << 32U));
 	}
 
-	case PM_FEATURE_CHECK:
+	case (uint32_t)PM_FEATURE_CHECK:
 	{
 		uint32_t result[PAYLOAD_ARG_CNT] = {0U};
 
@@ -230,25 +230,25 @@
 
 	switch (api_id) {
 
-	case PM_SELF_SUSPEND:
+	case (uint32_t)PM_SELF_SUSPEND:
 		ret = pm_self_suspend(pm_arg[0], pm_arg[1], pm_arg[2],
 				      pm_arg[3], security_flag);
 		SMC_RET1(handle, (u_register_t)ret);
 
-	case PM_FORCE_POWERDOWN:
+	case (uint32_t)PM_FORCE_POWERDOWN:
 		ret = pm_force_powerdown(pm_arg[0], pm_arg[1], security_flag);
 		SMC_RET1(handle, (u_register_t)ret);
 
-	case PM_REQ_SUSPEND:
+	case (uint32_t)PM_REQ_SUSPEND:
 		ret = pm_req_suspend(pm_arg[0], pm_arg[1], pm_arg[2],
 				     pm_arg[3], security_flag);
 		SMC_RET1(handle, (u_register_t)ret);
 
-	case PM_ABORT_SUSPEND:
+	case (uint32_t)PM_ABORT_SUSPEND:
 		ret = pm_abort_suspend(pm_arg[0], security_flag);
 		SMC_RET1(handle, (u_register_t)ret);
 
-	case PM_SYSTEM_SHUTDOWN:
+	case (uint32_t)PM_SYSTEM_SHUTDOWN:
 		ret = pm_system_shutdown(pm_arg[0], pm_arg[1], security_flag);
 		SMC_RET1(handle, (u_register_t)ret);
 
@@ -330,7 +330,7 @@
 	 * receive only 4 words from TF-A. So, this needs to be handled separately
 	 * than other eemi calls.
 	 */
-	if (api_id == PM_QUERY_DATA) {
+	if (api_id == (uint32_t)PM_QUERY_DATA) {
 		if ((pm_arg[0] == XPM_QID_CLOCK_GET_NAME ||
 		    pm_arg[0] == XPM_QID_PINCTRL_GET_FUNCTION_NAME) &&
 		    ret == PM_RET_SUCCESS) {
diff --git a/plat/xilinx/versal/include/plat_macros.S b/plat/xilinx/versal/include/plat_macros.S
index 3a52212..f1f9bb7 100644
--- a/plat/xilinx/versal/include/plat_macros.S
+++ b/plat/xilinx/versal/include/plat_macros.S
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -102,8 +103,8 @@
 	 * ---------------------------------------------
 	 */
 	.macro plat_crash_print_regs
-	mov_imm	x17, PLAT_VERSAL_GICD_BASE
-	mov_imm	x16, PLAT_VERSAL_GICR_BASE
+	mov_imm	x17, PLAT_GICD_BASE_VALUE
+	mov_imm	x16, PLAT_GICR_BASE_VALUE
 	versal_print_gic_regs
 	.endm
 
diff --git a/plat/xilinx/versal/include/platform_def.h b/plat/xilinx/versal/include/platform_def.h
index 6d95fdc..bd23bfb 100644
--- a/plat/xilinx/versal/include/platform_def.h
+++ b/plat/xilinx/versal/include/platform_def.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -81,8 +82,8 @@
 #define CACHE_WRITEBACK_SHIFT	6
 #define CACHE_WRITEBACK_GRANULE	(1 << CACHE_WRITEBACK_SHIFT)
 
-#define PLAT_VERSAL_GICD_BASE	U(0xF9000000)
-#define PLAT_VERSAL_GICR_BASE	U(0xF9080000)
+#define PLAT_GICD_BASE_VALUE	U(0xF9000000)
+#define PLAT_GICR_BASE_VALUE	U(0xF9080000)
 
 /*
  * Define a list of Group 1 Secure and Group 0 interrupts as per GICv3
@@ -101,4 +102,6 @@
 	INTR_PROP_DESC(PLAT_VERSAL_IPI_IRQ, GIC_HIGHEST_SEC_PRIORITY, grp, \
 			GIC_INTR_CFG_EDGE), \
 
+#define IRQ_MAX		142U
+
 #endif /* PLATFORM_DEF_H */
diff --git a/plat/xilinx/versal/platform.mk b/plat/xilinx/versal/platform.mk
index 8087297..71f6802 100644
--- a/plat/xilinx/versal/platform.mk
+++ b/plat/xilinx/versal/platform.mk
@@ -1,4 +1,5 @@
 # Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 
@@ -82,6 +83,8 @@
 				plat/xilinx/common/plat_startup.c		\
 				plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c \
 				plat/xilinx/common/pm_service/pm_ipi.c		\
+				plat/xilinx/common/pm_service/pm_api_sys.c	\
+				plat/xilinx/common/pm_service/pm_svc_main.c	\
 				plat/xilinx/versal/bl31_versal_setup.c		\
 				plat/xilinx/versal/plat_psci.c			\
 				plat/xilinx/versal/plat_versal.c		\
@@ -89,8 +92,6 @@
 				plat/xilinx/versal/sip_svc_setup.c		\
 				plat/xilinx/versal/versal_gicv3.c		\
 				plat/xilinx/versal/versal_ipi.c			\
-				plat/xilinx/versal/pm_service/pm_svc_main.c	\
-				plat/xilinx/versal/pm_service/pm_api_sys.c	\
 				plat/xilinx/versal/pm_service/pm_client.c
 
 ifeq ($(HARDEN_SLS_ALL), 1)
diff --git a/plat/xilinx/versal/pm_service/pm_client.c b/plat/xilinx/versal/pm_service/pm_client.c
index 54f4eb2..ecec405 100644
--- a/plat/xilinx/versal/pm_service/pm_client.c
+++ b/plat/xilinx/versal/pm_service/pm_client.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -24,8 +25,6 @@
 #include "pm_defs.h"
 
 #define UNDEFINED_CPUID		(~0)
-#define IRQ_MAX		142U
-#define NUM_GICD_ISENABLER	((IRQ_MAX >> 5U) + 1U)
 
 DEFINE_BAKERY_LOCK(pm_client_secure_lock);
 
@@ -51,114 +50,83 @@
 
 const struct pm_proc *primary_proc = &pm_procs_all[0];
 
-/* Interrupt to PM node index map */
-static enum pm_device_node_idx irq_node_map[IRQ_MAX + 1] = {
-	[13] = XPM_NODEIDX_DEV_GPIO,
-	[14] = XPM_NODEIDX_DEV_I2C_0,
-	[15] = XPM_NODEIDX_DEV_I2C_1,
-	[16] = XPM_NODEIDX_DEV_SPI_0,
-	[17] = XPM_NODEIDX_DEV_SPI_1,
-	[18] = XPM_NODEIDX_DEV_UART_0,
-	[19] = XPM_NODEIDX_DEV_UART_1,
-	[20] = XPM_NODEIDX_DEV_CAN_FD_0,
-	[21] = XPM_NODEIDX_DEV_CAN_FD_1,
-	[22] = XPM_NODEIDX_DEV_USB_0,
-	[23] = XPM_NODEIDX_DEV_USB_0,
-	[24] = XPM_NODEIDX_DEV_USB_0,
-	[25] = XPM_NODEIDX_DEV_USB_0,
-	[26] = XPM_NODEIDX_DEV_USB_0,
-	[37] = XPM_NODEIDX_DEV_TTC_0,
-	[38] = XPM_NODEIDX_DEV_TTC_0,
-	[39] = XPM_NODEIDX_DEV_TTC_0,
-	[40] = XPM_NODEIDX_DEV_TTC_1,
-	[41] = XPM_NODEIDX_DEV_TTC_1,
-	[42] = XPM_NODEIDX_DEV_TTC_1,
-	[43] = XPM_NODEIDX_DEV_TTC_2,
-	[44] = XPM_NODEIDX_DEV_TTC_2,
-	[45] = XPM_NODEIDX_DEV_TTC_2,
-	[46] = XPM_NODEIDX_DEV_TTC_3,
-	[47] = XPM_NODEIDX_DEV_TTC_3,
-	[48] = XPM_NODEIDX_DEV_TTC_3,
-	[56] = XPM_NODEIDX_DEV_GEM_0,
-	[57] = XPM_NODEIDX_DEV_GEM_0,
-	[58] = XPM_NODEIDX_DEV_GEM_1,
-	[59] = XPM_NODEIDX_DEV_GEM_1,
-	[60] = XPM_NODEIDX_DEV_ADMA_0,
-	[61] = XPM_NODEIDX_DEV_ADMA_1,
-	[62] = XPM_NODEIDX_DEV_ADMA_2,
-	[63] = XPM_NODEIDX_DEV_ADMA_3,
-	[64] = XPM_NODEIDX_DEV_ADMA_4,
-	[65] = XPM_NODEIDX_DEV_ADMA_5,
-	[66] = XPM_NODEIDX_DEV_ADMA_6,
-	[67] = XPM_NODEIDX_DEV_ADMA_7,
-	[74] = XPM_NODEIDX_DEV_USB_0,
-	[126] = XPM_NODEIDX_DEV_SDIO_0,
-	[127] = XPM_NODEIDX_DEV_SDIO_0,
-	[128] = XPM_NODEIDX_DEV_SDIO_1,
-	[129] = XPM_NODEIDX_DEV_SDIO_1,
-	[142] = XPM_NODEIDX_DEV_RTC,
-};
-
 /**
  * irq_to_pm_node_idx - Get PM node index corresponding to the interrupt number
  * @irq:	Interrupt number
  *
  * Return:	PM node index corresponding to the specified interrupt
  */
-static enum pm_device_node_idx irq_to_pm_node_idx(uint32_t irq)
-{
-	assert(irq <= IRQ_MAX);
-	return irq_node_map[irq];
-}
-
-/**
- * pm_client_set_wakeup_sources - Set all devices with enabled interrupts as
- *				  wake sources in the LibPM.
- * @node_id:	Node id of processor
- */
-static void pm_client_set_wakeup_sources(uint32_t node_id)
+enum pm_device_node_idx irq_to_pm_node_idx(uint32_t irq)
 {
-	uint32_t reg_num;
-	uint32_t device_id;
-	uint8_t pm_wakeup_nodes_set[XPM_NODEIDX_DEV_MAX] = { 0U };
-	uintptr_t isenabler1 = PLAT_VERSAL_GICD_BASE + GICD_ISENABLER + 4;
+	enum pm_device_node_idx dev_idx = XPM_NODEIDX_DEV_MIN;
 
-	for (reg_num = 0U; reg_num < NUM_GICD_ISENABLER; reg_num++) {
-		uint32_t base_irq = reg_num << ISENABLER_SHIFT;
-		uint32_t reg = mmio_read_32(isenabler1 + (reg_num << 2));
-
-		if (reg == 0U) {
-			continue;
-		}
-
-		while (reg != 0U) {
-			enum pm_device_node_idx node_idx;
-			uint32_t idx, irq, lowest_set = reg & (-reg);
-			enum pm_ret_status ret;
-
-			idx = __builtin_ctz(lowest_set);
-			irq = base_irq + idx;
-
-			if (irq > IRQ_MAX) {
-				break;
-			}
-
-			node_idx = irq_to_pm_node_idx(irq);
-			reg &= ~lowest_set;
+	assert(irq <= IRQ_MAX);
 
-			if (node_idx > XPM_NODEIDX_DEV_MIN && node_idx < XPM_NODEIDX_DEV_MAX) {
-				if (pm_wakeup_nodes_set[node_idx] == 0U) {
-					/* Get device ID from node index */
-					device_id = PERIPH_DEVID(node_idx);
-					ret = pm_set_wakeup_source(node_id,
-								   device_id, 1,
-								   SECURE_FLAG);
-					pm_wakeup_nodes_set[node_idx] = (ret == PM_RET_SUCCESS) ?
-											 1 : 0;
-				}
-			}
-		}
+	switch (irq) {
+	case 13:
+		dev_idx = XPM_NODEIDX_DEV_GPIO;
+		break;
+	case 14:
+		dev_idx = XPM_NODEIDX_DEV_I2C_0;
+		break;
+	case 15:
+		dev_idx = XPM_NODEIDX_DEV_I2C_1;
+		break;
+	case 16:
+		dev_idx = XPM_NODEIDX_DEV_SPI_0;
+		break;
+	case 17:
+		dev_idx = XPM_NODEIDX_DEV_SPI_1;
+		break;
+	case 18:
+		dev_idx = XPM_NODEIDX_DEV_UART_0;
+		break;
+	case 19:
+		dev_idx = XPM_NODEIDX_DEV_UART_1;
+		break;
+	case 20:
+		dev_idx = XPM_NODEIDX_DEV_CAN_FD_0;
+		break;
+	case 21:
+		dev_idx = XPM_NODEIDX_DEV_CAN_FD_1;
+		break;
+	case 22:
+	case 23:
+	case 24:
+	case 25:
+	case 26:
+		dev_idx = XPM_NODEIDX_DEV_USB_0;
+		break;
+	case 37:
+	case 38:
+	case 39:
+		dev_idx = XPM_NODEIDX_DEV_TTC_0;
+		break;
+	case 40:
+	case 41:
+	case 42:
+		dev_idx = XPM_NODEIDX_DEV_TTC_1;
+		break;
+	case 43:
+	case 44:
+	case 45:
+		dev_idx = XPM_NODEIDX_DEV_TTC_2;
+		break;
+	case 46:
+	case 47:
+	case 48:
+		dev_idx = XPM_NODEIDX_DEV_TTC_3;
+		break;
+	case 56:
+	case 57:
+		dev_idx = XPM_NODEIDX_DEV_GEM_0;
+		break;
+	default:
+		dev_idx = XPM_NODEIDX_DEV_MIN;
+		break;
 	}
+
+	return dev_idx;
 }
 
 /**
diff --git a/plat/xilinx/versal/pm_service/pm_defs.h b/plat/xilinx/versal/pm_service/pm_defs.h
deleted file mode 100644
index dbc801c..0000000
--- a/plat/xilinx/versal/pm_service/pm_defs.h
+++ /dev/null
@@ -1,171 +0,0 @@
-/*
- * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
- * Copyright (c) 2022-2023, Advanced Micro Devices Inc. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-/* Versal power management enums and defines */
-
-#ifndef PM_DEFS_H
-#define PM_DEFS_H
-
-#include "pm_node.h"
-
-/*********************************************************************
- * Macro definitions
- ********************************************************************/
-
-/* State arguments of the self suspend */
-#define PM_STATE_CPU_IDLE	0x0U
-#define PM_STATE_SUSPEND_TO_RAM	0xFU
-
-#define MAX_LATENCY		(~0U)
-#define MAX_QOS			100U
-
-/* Processor core device IDs */
-#define APU_DEVID(IDX)	NODEID(XPM_NODECLASS_DEVICE, XPM_NODESUBCL_DEV_CORE, \
-			       XPM_NODETYPE_DEV_CORE_APU, (IDX))
-
-#define XPM_DEVID_ACPU_0	APU_DEVID(XPM_NODEIDX_DEV_ACPU_0)
-#define XPM_DEVID_ACPU_1	APU_DEVID(XPM_NODEIDX_DEV_ACPU_1)
-
-#define PERIPH_DEVID(IDX)	NODEID(XPM_NODECLASS_DEVICE, \
-				       XPM_NODESUBCL_DEV_PERIPH, \
-				       XPM_NODETYPE_DEV_PERIPH, (IDX))
-
-#define PM_GET_CALLBACK_DATA		0xa01U
-#define PM_GET_TRUSTZONE_VERSION	0xa03U
-#define TF_A_PM_REGISTER_SGI		0xa04U
-
-/* PM API Versions */
-#define PM_API_BASE_VERSION		1U
-#define PM_API_VERSION_2		2U
-
-/* PM API ids */
-#define PM_REGISTER_NOTIFIER		5U
-#define PM_REQ_SUSPEND			6U
-#define PM_SELF_SUSPEND			7U
-#define PM_FORCE_POWERDOWN		8U
-#define PM_ABORT_SUSPEND		9U
-#define PM_REQ_WAKEUP			10U
-#define PM_SET_WAKEUP_SOURCE		11U
-#define PM_SYSTEM_SHUTDOWN		12U
-#define PM_IOCTL			34U
-#define PM_QUERY_DATA			35U
-#define PM_PLL_SET_PARAMETER		48U
-#define PM_PLL_GET_PARAMETER		49U
-#define PM_PLL_SET_MODE			50U
-#define PM_PLL_GET_MODE			51U
-#define PM_FEATURE_CHECK		63U
-
-/* Loader API ids */
-#define PM_LOAD_PDI			0x701U
-#define PM_LOAD_GET_HANDOFF_PARAMS	0x70BU
-
-/* IOCTL IDs for clock driver */
-#define IOCTL_SET_PLL_FRAC_MODE		8U
-#define	IOCTL_GET_PLL_FRAC_MODE		9U
-#define	IOCTL_SET_PLL_FRAC_DATA		10U
-#define	IOCTL_GET_PLL_FRAC_DATA		11U
-#define	IOCTL_SET_SGI			25U
-
-/* Parameter ID for PLL IOCTLs */
-/* Fractional data portion for PLL */
-#define PM_PLL_PARAM_DATA	2
-
-/* System shutdown macros */
-#define	XPM_SHUTDOWN_TYPE_SHUTDOWN	0U
-#define	XPM_SHUTDOWN_TYPE_RESET		1U
-#define	XPM_SHUTDOWN_TYPE_SETSCOPE_ONLY	2U
-
-#define	XPM_SHUTDOWN_SUBTYPE_RST_SUBSYSTEM	0U
-#define	XPM_SHUTDOWN_SUBTYPE_RST_PS_ONLY	1U
-#define	XPM_SHUTDOWN_SUBTYPE_RST_SYSTEM		2U
-
-/*********************************************************************
- * Enum definitions
- ********************************************************************/
-
-enum pm_abort_reason {
-	ABORT_REASON_WKUP_EVENT = 100,
-	ABORT_REASON_PU_BUSY,
-	ABORT_REASON_NO_PWRDN,
-	ABORT_REASON_UNKNOWN,
-};
-
-enum pm_opchar_type {
-	PM_OPCHAR_TYPE_POWER = 1,
-	PM_OPCHAR_TYPE_TEMP,
-	PM_OPCHAR_TYPE_LATENCY,
-};
-
-/**
- * Subsystem IDs
- */
-typedef enum {
-	XPM_SUBSYSID_PMC,
-	XPM_SUBSYSID_PSM,
-	XPM_SUBSYSID_APU,
-	XPM_SUBSYSID_RPU0_LOCK,
-	XPM_SUBSYSID_RPU0_0,
-	XPM_SUBSYSID_RPU0_1,
-	XPM_SUBSYSID_DDR0,
-	XPM_SUBSYSID_ME,
-	XPM_SUBSYSID_PL,
-	XPM_SUBSYSID_MAX,
-} XPm_SubsystemId;
-
-/* TODO: move pm_ret_status from device specific location to common location */
-/**
- * @PM_RET_SUCCESS:		success
- * @PM_RET_ERROR_ARGS:		illegal arguments provided (deprecated)
- * @PM_RET_ERROR_NOTSUPPORTED:	feature not supported  (deprecated)
- * @PM_RET_ERROR_NOFEATURE:	feature is not available
- * @PM_RET_ERROR_INVALID_CRC:	invalid crc in IPI communication
- * @PM_RET_ERROR_INTERNAL:	internal error
- * @PM_RET_ERROR_CONFLICT:	conflict
- * @PM_RET_ERROR_ACCESS:	access rights violation
- * @PM_RET_ERROR_INVALID_NODE:	invalid node
- * @PM_RET_ERROR_DOUBLE_REQ:	duplicate request for same node
- * @PM_RET_ERROR_ABORT_SUSPEND:	suspend procedure has been aborted
- * @PM_RET_ERROR_TIMEOUT:	timeout in communication with PMU
- * @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_NOFEATURE = 19,
-	PM_RET_ERROR_INVALID_CRC = 301,
-	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
-};
-
-/**
- * Qids
- */
-enum pm_query_id {
-	XPM_QID_INVALID,
-	XPM_QID_CLOCK_GET_NAME,
-	XPM_QID_CLOCK_GET_TOPOLOGY,
-	XPM_QID_CLOCK_GET_FIXEDFACTOR_PARAMS,
-	XPM_QID_CLOCK_GET_MUXSOURCES,
-	XPM_QID_CLOCK_GET_ATTRIBUTES,
-	XPM_QID_PINCTRL_GET_NUM_PINS,
-	XPM_QID_PINCTRL_GET_NUM_FUNCTIONS,
-	XPM_QID_PINCTRL_GET_NUM_FUNCTION_GROUPS,
-	XPM_QID_PINCTRL_GET_FUNCTION_NAME,
-	XPM_QID_PINCTRL_GET_FUNCTION_GROUPS,
-	XPM_QID_PINCTRL_GET_PIN_GROUPS,
-	XPM_QID_CLOCK_GET_NUM_CLOCKS,
-	XPM_QID_CLOCK_GET_MAX_DIVISOR,
-	XPM_QID_PLD_GET_PARENT,
-};
-#endif /* PM_DEFS_H */
diff --git a/plat/xilinx/versal/pm_service/pm_node.h b/plat/xilinx/versal/pm_service/pm_node.h
deleted file mode 100644
index 1b82ec7..0000000
--- a/plat/xilinx/versal/pm_service/pm_node.h
+++ /dev/null
@@ -1,192 +0,0 @@
-/*
- * Copyright (c) 2019, Xilinx, Inc. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-/* Versal PM nodes enums and defines */
-
-#ifndef PM_NODE_H
-#define PM_NODE_H
-
-/*********************************************************************
- * Macro definitions
- ********************************************************************/
-
-#define NODE_CLASS_SHIFT	26U
-#define NODE_SUBCLASS_SHIFT	20U
-#define NODE_TYPE_SHIFT		14U
-#define NODE_INDEX_SHIFT	0U
-#define NODE_CLASS_MASK_BITS    0x3F
-#define NODE_SUBCLASS_MASK_BITS 0x3F
-#define NODE_TYPE_MASK_BITS     0x3F
-#define NODE_INDEX_MASK_BITS    0x3FFF
-#define NODE_CLASS_MASK         (NODE_CLASS_MASK_BITS << NODE_CLASS_SHIFT)
-#define NODE_SUBCLASS_MASK      (NODE_SUBCLASS_MASK_BITS << NODE_SUBCLASS_SHIFT)
-#define NODE_TYPE_MASK          (NODE_TYPE_MASK_BITS << NODE_TYPE_SHIFT)
-#define NODE_INDEX_MASK         (NODE_INDEX_MASK_BITS << NODE_INDEX_SHIFT)
-
-#define NODEID(CLASS, SUBCLASS, TYPE, INDEX)	\
-	     ((((CLASS) & NODE_CLASS_MASK_BITS) << NODE_CLASS_SHIFT) | \
-	     (((SUBCLASS) & NODE_SUBCLASS_MASK_BITS) << NODE_SUBCLASS_SHIFT) | \
-	     (((TYPE) & NODE_TYPE_MASK_BITS) << NODE_TYPE_SHIFT) | \
-	     (((INDEX) & NODE_INDEX_MASK_BITS) << NODE_INDEX_SHIFT))
-
-#define NODECLASS(ID)		(((ID) & NODE_CLASS_MASK) >> NODE_CLASS_SHIFT)
-#define NODESUBCLASS(ID)	(((ID) & NODE_SUBCLASS_MASK) >> \
-				NODE_SUBCLASS_SHIFT)
-#define NODETYPE(ID)		(((ID) & NODE_TYPE_MASK) >> NODE_TYPE_SHIFT)
-#define NODEINDEX(ID)		(((ID) & NODE_INDEX_MASK) >> NODE_INDEX_SHIFT)
-
-/*********************************************************************
- * Enum definitions
- ********************************************************************/
-
-/* Node class types */
-enum pm_node_class {
-	XPM_NODECLASS_MIN,
-
-	XPM_NODECLASS_POWER,
-	XPM_NODECLASS_CLOCK,
-	XPM_NODECLASS_RESET,
-	XPM_NODECLASS_MEMIC,
-	XPM_NODECLASS_STMIC,
-	XPM_NODECLASS_DEVICE,
-
-	XPM_NODECLASS_MAX
-};
-
-enum pm_device_node_subclass {
-	/* Device types */
-	XPM_NODESUBCL_DEV_CORE = 1,
-	XPM_NODESUBCL_DEV_PERIPH,
-	XPM_NODESUBCL_DEV_MEM,
-	XPM_NODESUBCL_DEV_SOC,
-	XPM_NODESUBCL_DEV_MEM_CTRLR,
-	XPM_NODESUBCL_DEV_PHY,
-};
-
-enum pm_device_node_type {
-	/* Device types */
-	XPM_NODETYPE_DEV_CORE_PMC = 1,
-	XPM_NODETYPE_DEV_CORE_PSM,
-	XPM_NODETYPE_DEV_CORE_APU,
-	XPM_NODETYPE_DEV_CORE_RPU,
-	XPM_NODETYPE_DEV_OCM,
-	XPM_NODETYPE_DEV_TCM,
-	XPM_NODETYPE_DEV_L2CACHE,
-	XPM_NODETYPE_DEV_DDR,
-	XPM_NODETYPE_DEV_PERIPH,
-	XPM_NODETYPE_DEV_SOC,
-	XPM_NODETYPE_DEV_GT,
-};
-
-/* Device node Indexes */
-enum pm_device_node_idx {
-	/* Device nodes */
-	XPM_NODEIDX_DEV_MIN,
-
-	/* Processor devices */
-	XPM_NODEIDX_DEV_PMC_PROC,
-	XPM_NODEIDX_DEV_PSM_PROC,
-	XPM_NODEIDX_DEV_ACPU_0,
-	XPM_NODEIDX_DEV_ACPU_1,
-	XPM_NODEIDX_DEV_RPU0_0,
-	XPM_NODEIDX_DEV_RPU0_1,
-
-	/* Memory devices */
-	XPM_NODEIDX_DEV_OCM_0,
-	XPM_NODEIDX_DEV_OCM_1,
-	XPM_NODEIDX_DEV_OCM_2,
-	XPM_NODEIDX_DEV_OCM_3,
-	XPM_NODEIDX_DEV_TCM_0_A,
-	XPM_NODEIDX_DEV_TCM_0_B,
-	XPM_NODEIDX_DEV_TCM_1_A,
-	XPM_NODEIDX_DEV_TCM_1_B,
-	XPM_NODEIDX_DEV_L2_BANK_0,
-	XPM_NODEIDX_DEV_DDR_0,
-	XPM_NODEIDX_DEV_DDR_1,
-	XPM_NODEIDX_DEV_DDR_2,
-	XPM_NODEIDX_DEV_DDR_3,
-	XPM_NODEIDX_DEV_DDR_4,
-	XPM_NODEIDX_DEV_DDR_5,
-	XPM_NODEIDX_DEV_DDR_6,
-	XPM_NODEIDX_DEV_DDR_7,
-
-	/* LPD Peripheral devices */
-	XPM_NODEIDX_DEV_USB_0,
-	XPM_NODEIDX_DEV_GEM_0,
-	XPM_NODEIDX_DEV_GEM_1,
-	XPM_NODEIDX_DEV_SPI_0,
-	XPM_NODEIDX_DEV_SPI_1,
-	XPM_NODEIDX_DEV_I2C_0,
-	XPM_NODEIDX_DEV_I2C_1,
-	XPM_NODEIDX_DEV_CAN_FD_0,
-	XPM_NODEIDX_DEV_CAN_FD_1,
-	XPM_NODEIDX_DEV_UART_0,
-	XPM_NODEIDX_DEV_UART_1,
-	XPM_NODEIDX_DEV_GPIO,
-	XPM_NODEIDX_DEV_TTC_0,
-	XPM_NODEIDX_DEV_TTC_1,
-	XPM_NODEIDX_DEV_TTC_2,
-	XPM_NODEIDX_DEV_TTC_3,
-	XPM_NODEIDX_DEV_SWDT_LPD,
-
-	/* FPD Peripheral devices */
-	XPM_NODEIDX_DEV_SWDT_FPD,
-
-	/* PMC Peripheral devices */
-	XPM_NODEIDX_DEV_OSPI,
-	XPM_NODEIDX_DEV_QSPI,
-	XPM_NODEIDX_DEV_GPIO_PMC,
-	XPM_NODEIDX_DEV_I2C_PMC,
-	XPM_NODEIDX_DEV_SDIO_0,
-	XPM_NODEIDX_DEV_SDIO_1,
-
-	XPM_NODEIDX_DEV_PL_0,
-	XPM_NODEIDX_DEV_PL_1,
-	XPM_NODEIDX_DEV_PL_2,
-	XPM_NODEIDX_DEV_PL_3,
-	XPM_NODEIDX_DEV_RTC,
-	XPM_NODEIDX_DEV_ADMA_0,
-	XPM_NODEIDX_DEV_ADMA_1,
-	XPM_NODEIDX_DEV_ADMA_2,
-	XPM_NODEIDX_DEV_ADMA_3,
-	XPM_NODEIDX_DEV_ADMA_4,
-	XPM_NODEIDX_DEV_ADMA_5,
-	XPM_NODEIDX_DEV_ADMA_6,
-	XPM_NODEIDX_DEV_ADMA_7,
-	XPM_NODEIDX_DEV_IPI_0,
-	XPM_NODEIDX_DEV_IPI_1,
-	XPM_NODEIDX_DEV_IPI_2,
-	XPM_NODEIDX_DEV_IPI_3,
-	XPM_NODEIDX_DEV_IPI_4,
-	XPM_NODEIDX_DEV_IPI_5,
-	XPM_NODEIDX_DEV_IPI_6,
-
-	/* Entire SoC */
-	XPM_NODEIDX_DEV_SOC,
-
-	/* DDR memory controllers */
-	XPM_NODEIDX_DEV_DDRMC_0,
-	XPM_NODEIDX_DEV_DDRMC_1,
-	XPM_NODEIDX_DEV_DDRMC_2,
-	XPM_NODEIDX_DEV_DDRMC_3,
-
-	/* GT devices */
-	XPM_NODEIDX_DEV_GT_0,
-	XPM_NODEIDX_DEV_GT_1,
-	XPM_NODEIDX_DEV_GT_2,
-	XPM_NODEIDX_DEV_GT_3,
-	XPM_NODEIDX_DEV_GT_4,
-	XPM_NODEIDX_DEV_GT_5,
-	XPM_NODEIDX_DEV_GT_6,
-	XPM_NODEIDX_DEV_GT_7,
-	XPM_NODEIDX_DEV_GT_8,
-	XPM_NODEIDX_DEV_GT_9,
-	XPM_NODEIDX_DEV_GT_10,
-
-	XPM_NODEIDX_DEV_MAX
-};
-
-#endif /* PM_NODE_H */
diff --git a/plat/xilinx/versal/versal_gicv3.c b/plat/xilinx/versal/versal_gicv3.c
index 0959c8e..4f4e0d9 100644
--- a/plat/xilinx/versal/versal_gicv3.c
+++ b/plat/xilinx/versal/versal_gicv3.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -60,8 +61,8 @@
 }
 
 static const gicv3_driver_data_t versal_gic_data __unused = {
-	.gicd_base = PLAT_VERSAL_GICD_BASE,
-	.gicr_base = PLAT_VERSAL_GICR_BASE,
+	.gicd_base = PLAT_GICD_BASE_VALUE,
+	.gicr_base = PLAT_GICR_BASE_VALUE,
 	.interrupt_props = versal_interrupt_props,
 	.interrupt_props_num = ARRAY_SIZE(versal_interrupt_props),
 	.rdistif_num = PLATFORM_CORE_COUNT,
diff --git a/plat/xilinx/versal_net/include/plat_macros.S b/plat/xilinx/versal_net/include/plat_macros.S
index fb108b6..a0c6604 100644
--- a/plat/xilinx/versal_net/include/plat_macros.S
+++ b/plat/xilinx/versal_net/include/plat_macros.S
@@ -1,7 +1,7 @@
 /*
  * 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.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -109,8 +109,8 @@
 	 * Uncomment it when versions are stable
 	 */
 	/*
-	mov_imm	x17, PLAT_VERSAL_NET_GICD_BASE
-	mov_imm	x16, PLAT_VERSAL_NET_GICR_BASE
+	mov_imm	x17, PLAT_GICD_BASE_VALUE
+	mov_imm	x16, PLAT_GICR_BASE_VALUE
 	versal_net_print_gic_regs
 	*/
 	.endm
diff --git a/plat/xilinx/versal_net/include/plat_private.h b/plat/xilinx/versal_net/include/plat_private.h
index 6a3bc19..ca4ed1d 100644
--- a/plat/xilinx/versal_net/include/plat_private.h
+++ b/plat/xilinx/versal_net/include/plat_private.h
@@ -1,7 +1,7 @@
 /*
  * 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.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -43,7 +43,4 @@
  */
 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
index 9aa1441..f74cb1e 100644
--- a/plat/xilinx/versal_net/include/platform_def.h
+++ b/plat/xilinx/versal_net/include/platform_def.h
@@ -1,7 +1,7 @@
 /*
  * 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.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -95,8 +95,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 PLAT_GICD_BASE_VALUE	U(0xE2000000)
+#define PLAT_GICR_BASE_VALUE	U(0xE2060000)
 
 /*
  * Define a list of Group 1 Secure and Group 0 interrupts as per GICv3
@@ -114,4 +114,6 @@
 	INTR_PROP_DESC(PLAT_VERSAL_IPI_IRQ, GIC_HIGHEST_SEC_PRIORITY, grp, \
 			GIC_INTR_CFG_EDGE), \
 
+#define IRQ_MAX		200U
+
 #endif /* PLATFORM_DEF_H */
diff --git a/plat/xilinx/versal_net/plat_psci.c b/plat/xilinx/versal_net/plat_psci.c
index c5833a9..8bb9bda 100644
--- a/plat/xilinx/versal_net/plat_psci.c
+++ b/plat/xilinx/versal_net/plat_psci.c
@@ -1,7 +1,7 @@
 /*
  * 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.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -17,11 +17,10 @@
 #include <plat_arm.h>
 
 #include <plat_private.h>
+#include <pm_defs.h>
 
 #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)
diff --git a/plat/xilinx/versal_net/plat_psci_pm.c b/plat/xilinx/versal_net/plat_psci_pm.c
index c713061..9d401a5 100644
--- a/plat/xilinx/versal_net/plat_psci_pm.c
+++ b/plat/xilinx/versal_net/plat_psci_pm.c
@@ -196,7 +196,6 @@
 	VERBOSE("%s: power_state: 0x%x\n", __func__, power_state);
 
 	int32_t pstate = psci_get_pstate_type(power_state);
-	uint64_t i;
 
 	assert(req_state);
 
@@ -204,8 +203,7 @@
 	if (pstate == PSTATE_TYPE_STANDBY) {
 		req_state->pwr_domain_state[MPIDR_AFFLVL0] = PLAT_MAX_RET_STATE;
 	} else {
-		for (i = MPIDR_AFFLVL0; i <= PLAT_MAX_PWR_LVL; i++)
-			req_state->pwr_domain_state[i] = PLAT_MAX_OFF_STATE;
+		req_state->pwr_domain_state[MPIDR_AFFLVL0] = PLAT_MAX_OFF_STATE;
 	}
 
 	/* We expect the 'state id' to be zero */
diff --git a/plat/xilinx/versal_net/platform.mk b/plat/xilinx/versal_net/platform.mk
index 28e3295..0bc5925 100644
--- a/plat/xilinx/versal_net/platform.mk
+++ b/plat/xilinx/versal_net/platform.mk
@@ -1,6 +1,6 @@
 # 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.
+# Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 
@@ -87,10 +87,10 @@
 				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	\
+BL31_SOURCES		+=	plat/xilinx/common/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/xilinx/common/pm_service/pm_svc_main.c	\
 				${PLAT_PATH}/pm_service/pm_client.c		\
 				${PLAT_PATH}/versal_net_ipi.c
 else
diff --git a/plat/xilinx/versal_net/pm_service/pm_client.c b/plat/xilinx/versal_net/pm_service/pm_client.c
index f543193..2741d47 100644
--- a/plat/xilinx/versal_net/pm_service/pm_client.c
+++ b/plat/xilinx/versal_net/pm_service/pm_client.c
@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2022, Xilinx, Inc. All rights reserved.
- * Copyright (C) 2022, Advanced Micro Devices, Inc. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,6 +18,7 @@
 #include <lib/mmio.h>
 #include <lib/mmio.h>
 #include <lib/utils.h>
+#include <lib/spinlock.h>
 #include <plat/common/platform.h>
 
 #include <plat_ipi.h>
@@ -29,7 +30,34 @@
 #define UNDEFINED_CPUID		(~0)
 
 DEFINE_RENAME_SYSREG_RW_FUNCS(cpu_pwrctrl_val, S3_0_C15_C2_7)
+
+/*
+ * ARM v8.2, the cache will turn off automatically when cpu
+ * power down. Therefore, there is no doubt to use the spin_lock here.
+ */
+#if !HW_ASSISTED_COHERENCY
 DEFINE_BAKERY_LOCK(pm_client_secure_lock);
+static inline void pm_client_lock_get(void)
+{
+	bakery_lock_get(&pm_client_secure_lock);
+}
+
+static inline void pm_client_lock_release(void)
+{
+	bakery_lock_release(&pm_client_secure_lock);
+}
+#else
+spinlock_t pm_client_secure_lock;
+static inline void pm_client_lock_get(void)
+{
+	spin_lock(&pm_client_secure_lock);
+}
+
+static inline void pm_client_lock_release(void)
+{
+	spin_unlock(&pm_client_secure_lock);
+}
+#endif
 
 static const struct pm_ipi apu_ipi = {
 	.local_ipi_id = IPI_ID_APU,
@@ -140,6 +168,133 @@
 }
 
 /**
+ * irq_to_pm_node_idx - Get PM node index corresponding to the interrupt number
+ * @irq:        Interrupt number
+ *
+ * Return:      PM node index corresponding to the specified interrupt
+ */
+enum pm_device_node_idx irq_to_pm_node_idx(uint32_t irq)
+{
+	enum pm_device_node_idx dev_idx = XPM_NODEIDX_DEV_MIN;
+
+	assert(irq <= IRQ_MAX);
+
+	switch (irq) {
+	case 20:
+		dev_idx = XPM_NODEIDX_DEV_GPIO;
+		break;
+	case 21:
+		dev_idx = XPM_NODEIDX_DEV_I2C_0;
+		break;
+	case 22:
+		dev_idx = XPM_NODEIDX_DEV_I2C_1;
+		break;
+	case 23:
+		dev_idx = XPM_NODEIDX_DEV_SPI_0;
+		break;
+	case 24:
+		dev_idx = XPM_NODEIDX_DEV_SPI_1;
+		break;
+	case 25:
+		dev_idx = XPM_NODEIDX_DEV_UART_0;
+		break;
+	case 26:
+		dev_idx = XPM_NODEIDX_DEV_UART_1;
+		break;
+	case 27:
+		dev_idx = XPM_NODEIDX_DEV_CAN_FD_0;
+		break;
+	case 28:
+		dev_idx = XPM_NODEIDX_DEV_CAN_FD_1;
+		break;
+	case 29:
+	case 30:
+	case 31:
+	case 32:
+	case 33:
+	case 98:
+		dev_idx = XPM_NODEIDX_DEV_USB_0;
+		break;
+	case 34:
+	case 35:
+	case 36:
+	case 37:
+	case 38:
+	case 99:
+		dev_idx = XPM_NODEIDX_DEV_USB_1;
+		break;
+	case 39:
+	case 40:
+		dev_idx = XPM_NODEIDX_DEV_GEM_0;
+		break;
+	case 41:
+	case 42:
+		dev_idx = XPM_NODEIDX_DEV_GEM_1;
+		break;
+	case 43:
+	case 44:
+	case 45:
+		dev_idx = XPM_NODEIDX_DEV_TTC_0;
+		break;
+	case 46:
+	case 47:
+	case 48:
+		dev_idx = XPM_NODEIDX_DEV_TTC_1;
+		break;
+	case 49:
+	case 50:
+	case 51:
+		dev_idx = XPM_NODEIDX_DEV_TTC_2;
+		break;
+	case 52:
+	case 53:
+	case 54:
+		dev_idx = XPM_NODEIDX_DEV_TTC_3;
+		break;
+	case 72:
+		dev_idx = XPM_NODEIDX_DEV_ADMA_0;
+		break;
+	case 73:
+		dev_idx = XPM_NODEIDX_DEV_ADMA_1;
+		break;
+	case 74:
+		dev_idx = XPM_NODEIDX_DEV_ADMA_2;
+		break;
+	case 75:
+		dev_idx = XPM_NODEIDX_DEV_ADMA_3;
+		break;
+	case 76:
+		dev_idx = XPM_NODEIDX_DEV_ADMA_4;
+		break;
+	case 77:
+		dev_idx = XPM_NODEIDX_DEV_ADMA_5;
+		break;
+	case 78:
+		dev_idx = XPM_NODEIDX_DEV_ADMA_6;
+		break;
+	case 79:
+		dev_idx = XPM_NODEIDX_DEV_ADMA_7;
+		break;
+	case 184:
+	case 185:
+		dev_idx = XPM_NODEIDX_DEV_SDIO_0;
+		break;
+	case 186:
+	case 187:
+		dev_idx = XPM_NODEIDX_DEV_SDIO_1;
+		break;
+	case 200:
+		dev_idx = XPM_NODEIDX_DEV_RTC;
+		break;
+	default:
+		dev_idx = XPM_NODEIDX_DEV_MIN;
+		break;
+	}
+
+	return dev_idx;
+}
+
+/**
  * pm_client_suspend() - Client-specific suspend actions
  *
  * This function should contain any PU-specific actions
@@ -154,9 +309,11 @@
 	uint32_t cpu_id = plat_my_core_pos();
 	uintptr_t val;
 
-	bakery_lock_get(&pm_client_secure_lock);
+	pm_client_lock_get();
 
-	/* TODO: Set wakeup source */
+	if (state == PM_STATE_SUSPEND_TO_RAM) {
+		pm_client_set_wakeup_sources((uint32_t)proc->node_id);
+	}
 
 	val = read_cpu_pwrctrl_val();
 	val |= CORE_PWRDN_EN_BIT_MASK;
@@ -177,7 +334,7 @@
 	mmio_write_32(APU_PCIL_CORE_X_IEN_WAKE_REG(cpu_id),
 		      APU_PCIL_CORE_X_IEN_WAKE_MASK);
 
-	bakery_lock_release(&pm_client_secure_lock);
+	pm_client_lock_release();
 }
 
 /**
@@ -213,7 +370,7 @@
 		return;
 	}
 
-	bakery_lock_get(&pm_client_secure_lock);
+	pm_client_lock_get();
 
 	/* Clear powerdown request */
 	val = read_cpu_pwrctrl_val();
@@ -232,7 +389,7 @@
 	mmio_write_32(APU_PCIL_CORE_X_IDS_WAKE_REG(cpuid),
 		      APU_PCIL_CORE_X_IDS_WAKE_MASK);
 
-	bakery_lock_release(&pm_client_secure_lock);
+	pm_client_lock_release();
 }
 
 /**
@@ -249,7 +406,7 @@
 	/* Enable interrupts at processor level (for current cpu) */
 	gicv3_cpuif_enable(plat_my_core_pos());
 
-	bakery_lock_get(&pm_client_secure_lock);
+	pm_client_lock_get();
 
 	/* Clear powerdown request */
 	val = read_cpu_pwrctrl_val();
@@ -262,5 +419,5 @@
 	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);
+	pm_client_lock_release();
 }
diff --git a/plat/xilinx/versal_net/versal_net_gicv3.c b/plat/xilinx/versal_net/versal_net_gicv3.c
index 138d2c2..cee8092 100644
--- a/plat/xilinx/versal_net/versal_net_gicv3.c
+++ b/plat/xilinx/versal_net/versal_net_gicv3.c
@@ -63,8 +63,8 @@
 }
 
 static const gicv3_driver_data_t versal_net_gic_data __unused = {
-	.gicd_base = PLAT_VERSAL_NET_GICD_BASE,
-	.gicr_base = PLAT_VERSAL_NET_GICR_BASE,
+	.gicd_base = PLAT_GICD_BASE_VALUE,
+	.gicr_base = PLAT_GICR_BASE_VALUE,
 	.interrupt_props = versal_net_interrupt_props,
 	.interrupt_props_num = ARRAY_SIZE(versal_net_interrupt_props),
 	.rdistif_num = PLATFORM_CORE_COUNT,
diff --git a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
index 95a266e..8f28636 100644
--- a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
+++ b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
- * Copyright (C) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,7 +19,7 @@
 #include <plat/common/platform.h>
 #include <services/arm_arch_svc.h>
 
-#include "pm_api_sys.h"
+#include "zynqmp_pm_api_sys.h"
 
 /*
  * Table of regions to map using the MMU.
diff --git a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
index 0ebd088..50fd67b 100644
--- a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
+++ b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
@@ -125,10 +125,10 @@
 		}
 	}
 	if (bl32_image_ep_info.pc != 0) {
-		VERBOSE("BL31: Secure code at 0x%lx\n", bl32_image_ep_info.pc);
+		NOTICE("BL31: Secure code at 0x%lx\n", bl32_image_ep_info.pc);
 	}
 	if (bl33_image_ep_info.pc != 0) {
-		VERBOSE("BL31: Non secure code at 0x%lx\n", bl33_image_ep_info.pc);
+		NOTICE("BL31: Non secure code at 0x%lx\n", bl33_image_ep_info.pc);
 	}
 
 	custom_early_setup();
diff --git a/plat/xilinx/zynqmp/include/plat_pm_common.h b/plat/xilinx/zynqmp/include/plat_pm_common.h
index a57aebe..8167eb9 100644
--- a/plat/xilinx/zynqmp/include/plat_pm_common.h
+++ b/plat/xilinx/zynqmp/include/plat_pm_common.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2013-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,7 +15,7 @@
 
 #include <stdint.h>
 #include <common/debug.h>
-#include "pm_defs.h"
+#include "zynqmp_pm_defs.h"
 
 
 #define ZYNQMP_TZ_VERSION_MAJOR		1
diff --git a/plat/xilinx/zynqmp/libpm.mk b/plat/xilinx/zynqmp/libpm.mk
new file mode 100644
index 0000000..db3c742
--- /dev/null
+++ b/plat/xilinx/zynqmp/libpm.mk
@@ -0,0 +1,18 @@
+#
+# Copyright (c) 2023, Advanced Micro Devices, Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+LIBPM_SRCS	:=	$(addprefix plat/xilinx/common/pm_service/,	\
+			pm_ipi.c)
+
+LIBPM_SRCS      +=      $(addprefix plat/xilinx/zynqmp/pm_service/,  \
+                        zynqmp_pm_svc_main.c 				\
+			zynqmp_pm_api_sys.c				\
+			pm_api_pinctrl.c				\
+			pm_api_ioctl.c					\
+			pm_api_clock.c					\
+			pm_client.c)
+
+$(eval $(call MAKE_LIB,pm))
diff --git a/plat/xilinx/zynqmp/plat_psci.c b/plat/xilinx/zynqmp/plat_psci.c
index b7408b1..5211ace 100644
--- a/plat/xilinx/zynqmp/plat_psci.c
+++ b/plat/xilinx/zynqmp/plat_psci.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -16,8 +17,8 @@
 #include <plat/common/platform.h>
 
 #include <plat_private.h>
-#include "pm_api_sys.h"
 #include "pm_client.h"
+#include "zynqmp_pm_api_sys.h"
 
 static uintptr_t zynqmp_sec_entry;
 
diff --git a/plat/xilinx/zynqmp/platform.mk b/plat/xilinx/zynqmp/platform.mk
index 4671f5f..86b7839 100644
--- a/plat/xilinx/zynqmp/platform.mk
+++ b/plat/xilinx/zynqmp/platform.mk
@@ -116,6 +116,9 @@
 endif
 $(eval $(call add_define_val,ZYNQMP_CONSOLE,ZYNQMP_CONSOLE_ID_${ZYNQMP_CONSOLE}))
 
+# Build PM code as a Library
+include plat/xilinx/zynqmp/libpm.mk
+
 BL31_SOURCES		+=	drivers/arm/cci/cci.c				\
 				lib/cpus/aarch64/aem_generic.S			\
 				lib/cpus/aarch64/cortex_a53.S			\
@@ -123,19 +126,12 @@
 				common/fdt_fixup.c				\
 				${LIBFDT_SRCS}					\
 				plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c \
-				plat/xilinx/common/pm_service/pm_ipi.c		\
 				plat/xilinx/common/plat_startup.c		\
 				plat/xilinx/zynqmp/bl31_zynqmp_setup.c		\
 				plat/xilinx/zynqmp/plat_psci.c			\
 				plat/xilinx/zynqmp/plat_zynqmp.c		\
 				plat/xilinx/zynqmp/plat_topology.c		\
-				plat/xilinx/zynqmp/sip_svc_setup.c		\
-				plat/xilinx/zynqmp/pm_service/pm_svc_main.c	\
-				plat/xilinx/zynqmp/pm_service/pm_api_sys.c	\
-				plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c	\
-				plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c	\
-				plat/xilinx/zynqmp/pm_service/pm_api_clock.c	\
-				plat/xilinx/zynqmp/pm_service/pm_client.c
+				plat/xilinx/zynqmp/sip_svc_setup.c
 
 ifeq (${SDEI_SUPPORT},1)
 BL31_SOURCES		+=	plat/xilinx/zynqmp/zynqmp_ehf.c			\
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_clock.c b/plat/xilinx/zynqmp/pm_service/pm_api_clock.c
index e61310a..9f4278d 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_clock.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_clock.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -16,10 +17,10 @@
 #include <plat/common/platform.h>
 
 #include "pm_api_clock.h"
-#include "pm_api_sys.h"
 #include "pm_client.h"
 #include "pm_common.h"
 #include "pm_ipi.h"
+#include "zynqmp_pm_api_sys.h"
 
 #define CLK_NODE_MAX			(6U)
 
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c b/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c
index c0bfd51..45038b0 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -16,10 +17,10 @@
 
 #include "pm_api_clock.h"
 #include "pm_api_ioctl.h"
-#include "pm_api_sys.h"
 #include "pm_client.h"
 #include "pm_common.h"
 #include "pm_ipi.h"
+#include "zynqmp_pm_api_sys.h"
 
 /**
  * pm_ioctl_get_rpu_oper_mode () - Get current RPU operation mode
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.h b/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.h
index 3b0d6ee..de93b2d 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -13,44 +14,6 @@
 
 #include "pm_common.h"
 
-//ioctl id
-enum {
-	IOCTL_GET_RPU_OPER_MODE = 0,
-	IOCTL_SET_RPU_OPER_MODE = 1,
-	IOCTL_RPU_BOOT_ADDR_CONFIG = 2,
-	IOCTL_TCM_COMB_CONFIG = 3,
-	IOCTL_SET_TAPDELAY_BYPASS = 4,
-	IOCTL_SET_SGMII_MODE = 5,
-	IOCTL_SD_DLL_RESET = 6,
-	IOCTL_SET_SD_TAPDELAY = 7,
-	 /* Ioctl for clock driver */
-	IOCTL_SET_PLL_FRAC_MODE = 8,
-	IOCTL_GET_PLL_FRAC_MODE = 9,
-	IOCTL_SET_PLL_FRAC_DATA = 10,
-	IOCTL_GET_PLL_FRAC_DATA = 11,
-	IOCTL_WRITE_GGS = 12,
-	IOCTL_READ_GGS = 13,
-	IOCTL_WRITE_PGGS = 14,
-	IOCTL_READ_PGGS = 15,
-	/* IOCTL for ULPI reset */
-	IOCTL_ULPI_RESET = 16,
-	/* Set healthy bit value */
-	IOCTL_SET_BOOT_HEALTH_STATUS = 17,
-	IOCTL_AFI = 18,
-	/* Probe counter read/write */
-	IOCTL_PROBE_COUNTER_READ = 19,
-	IOCTL_PROBE_COUNTER_WRITE = 20,
-	IOCTL_OSPI_MUX_SELECT = 21,
-	/* IOCTL for USB power request */
-	IOCTL_USB_SET_STATE = 22,
-	/* IOCTL to get last reset reason */
-	IOCTL_GET_LAST_RESET_REASON = 23,
-	/* AI engine NPI ISR clear */
-	IOCTL_AIE_ISR_CLEAR = 24,
-	/* Register SGI to ATF */
-	IOCTL_REGISTER_SGI = 25,
-};
-
 //RPU operation mode
 #define	PM_RPU_MODE_LOCKSTEP 0U
 #define	PM_RPU_MODE_SPLIT 1U
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c b/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c
index 8f37341..847ec2c 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,10 +15,10 @@
 #include <plat/common/platform.h>
 
 #include "pm_api_pinctrl.h"
-#include "pm_api_sys.h"
 #include "pm_client.h"
 #include "pm_common.h"
 #include "pm_ipi.h"
+#include "zynqmp_pm_api_sys.h"
 
 struct pinctrl_function {
 	char name[FUNCTION_NAME_LEN];
diff --git a/plat/xilinx/zynqmp/pm_service/pm_client.c b/plat/xilinx/zynqmp/pm_service/pm_client.c
index 7217fa1..f752525 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_client.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_client.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2013-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -21,9 +22,9 @@
 
 #include <plat_ipi.h>
 #include <zynqmp_def.h>
-#include "pm_api_sys.h"
 #include "pm_client.h"
 #include "pm_ipi.h"
+#include "zynqmp_pm_api_sys.h"
 
 #define IRQ_MAX		84U
 #define NUM_GICD_ISENABLER	((IRQ_MAX >> 5U) + 1U)
diff --git a/plat/xilinx/zynqmp/pm_service/pm_defs.h b/plat/xilinx/zynqmp/pm_service/pm_defs.h
deleted file mode 100644
index f00ab4b..0000000
--- a/plat/xilinx/zynqmp/pm_service/pm_defs.h
+++ /dev/null
@@ -1,359 +0,0 @@
-/*
- * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
- * Copyright (c) 2022-2023, Advanced Micro Devices Inc. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-/* ZynqMP power management enums and defines */
-
-#ifndef PM_DEFS_H
-#define PM_DEFS_H
-
-/*********************************************************************
- * Macro definitions
- ********************************************************************/
-
-/*
- * Version number is a 32bit value, like:
- * (PM_VERSION_MAJOR << 16) | PM_VERSION_MINOR
- */
-#define PM_VERSION_MAJOR	1U
-#define PM_VERSION_MINOR	1U
-
-#define PM_VERSION	((PM_VERSION_MAJOR << 16U) | PM_VERSION_MINOR)
-
-/**
- * PM API versions
- */
-/* Expected version of firmware APIs */
-#define FW_API_BASE_VERSION		(1U)
-/* Expected version of firmware API for feature check */
-#define FW_API_VERSION_2		(2U)
-/* Version of APIs implemented in ATF */
-#define ATF_API_BASE_VERSION		(1U)
-/* Updating the QUERY_DATA API versioning as the bitmask functionality
- * support is added in the v2.*/
-#define TFA_API_QUERY_DATA_VERSION	(2U)
-
-/* Capabilities for RAM */
-#define PM_CAP_ACCESS	0x1U
-#define PM_CAP_CONTEXT	0x2U
-
-#define MAX_LATENCY	(~0U)
-#define MAX_QOS		100U
-
-/* State arguments of the self suspend */
-#define PM_STATE_CPU_IDLE		0x0U
-#define PM_STATE_SUSPEND_TO_RAM		0xFU
-
-/* APU processor states */
-#define PM_PROC_STATE_FORCEDOFF		0U
-#define PM_PROC_STATE_ACTIVE		1U
-#define PM_PROC_STATE_SLEEP		2U
-#define PM_PROC_STATE_SUSPENDING	3U
-
-#define PM_GET_CALLBACK_DATA		0xa01
-#define PM_SET_SUSPEND_MODE		0xa02
-#define PM_GET_TRUSTZONE_VERSION	0xa03
-
-/*********************************************************************
- * Enum definitions
- ********************************************************************/
-
-enum pm_api_id {
-	/* Miscellaneous API functions: */
-	PM_GET_API_VERSION = 1, /* Do not change or move */
-	PM_SET_CONFIGURATION,
-	PM_GET_NODE_STATUS,
-	PM_GET_OP_CHARACTERISTIC,
-	PM_REGISTER_NOTIFIER,
-	/* API for suspending of PUs: */
-	PM_REQ_SUSPEND,
-	PM_SELF_SUSPEND,
-	PM_FORCE_POWERDOWN,
-	PM_ABORT_SUSPEND,
-	PM_REQ_WAKEUP,
-	PM_SET_WAKEUP_SOURCE,
-	PM_SYSTEM_SHUTDOWN,
-	/* API for managing PM slaves: */
-	PM_REQ_NODE,
-	PM_RELEASE_NODE,
-	PM_SET_REQUIREMENT,
-	PM_SET_MAX_LATENCY,
-	/* Direct control API functions: */
-	PM_RESET_ASSERT,
-	PM_RESET_GET_STATUS,
-	PM_MMIO_WRITE,
-	PM_MMIO_READ,
-	PM_INIT_FINALIZE,
-	PM_FPGA_LOAD,
-	PM_FPGA_GET_STATUS,
-	PM_GET_CHIPID,
-	PM_SECURE_RSA_AES,
-	PM_SECURE_SHA,
-	PM_SECURE_RSA,
-	PM_PINCTRL_REQUEST,
-	PM_PINCTRL_RELEASE,
-	PM_PINCTRL_GET_FUNCTION,
-	PM_PINCTRL_SET_FUNCTION,
-	PM_PINCTRL_CONFIG_PARAM_GET,
-	PM_PINCTRL_CONFIG_PARAM_SET,
-	PM_IOCTL,
-	/* API to query information from firmware */
-	PM_QUERY_DATA,
-	/* Clock control API functions */
-	PM_CLOCK_ENABLE,
-	PM_CLOCK_DISABLE,
-	PM_CLOCK_GETSTATE,
-	PM_CLOCK_SETDIVIDER,
-	PM_CLOCK_GETDIVIDER,
-	PM_CLOCK_SETRATE,
-	PM_CLOCK_GETRATE,
-	PM_CLOCK_SETPARENT,
-	PM_CLOCK_GETPARENT,
-	PM_SECURE_IMAGE,
-	/* FPGA PL Readback */
-	PM_FPGA_READ,
-	PM_SECURE_AES,
-	/* PLL control API functions */
-	PM_PLL_SET_PARAMETER,
-	PM_PLL_GET_PARAMETER,
-	PM_PLL_SET_MODE,
-	PM_PLL_GET_MODE,
-	/* PM Register Access API */
-	PM_REGISTER_ACCESS,
-	PM_EFUSE_ACCESS,
-	PM_FEATURE_CHECK = 63,
-	PM_FPGA_GET_VERSION = 72,
-	PM_FPGA_GET_FEATURE_LIST,
-	PM_API_MAX
-};
-
-enum pm_node_id {
-	NODE_UNKNOWN = 0,
-	NODE_APU,
-	NODE_APU_0,
-	NODE_APU_1,
-	NODE_APU_2,
-	NODE_APU_3,
-	NODE_RPU,
-	NODE_RPU_0,
-	NODE_RPU_1,
-	NODE_PLD,
-	NODE_FPD,
-	NODE_OCM_BANK_0,
-	NODE_OCM_BANK_1,
-	NODE_OCM_BANK_2,
-	NODE_OCM_BANK_3,
-	NODE_TCM_0_A,
-	NODE_TCM_0_B,
-	NODE_TCM_1_A,
-	NODE_TCM_1_B,
-	NODE_L2,
-	NODE_GPU_PP_0,
-	NODE_GPU_PP_1,
-	NODE_USB_0,
-	NODE_USB_1,
-	NODE_TTC_0,
-	NODE_TTC_1,
-	NODE_TTC_2,
-	NODE_TTC_3,
-	NODE_SATA,
-	NODE_ETH_0,
-	NODE_ETH_1,
-	NODE_ETH_2,
-	NODE_ETH_3,
-	NODE_UART_0,
-	NODE_UART_1,
-	NODE_SPI_0,
-	NODE_SPI_1,
-	NODE_I2C_0,
-	NODE_I2C_1,
-	NODE_SD_0,
-	NODE_SD_1,
-	NODE_DP,
-	NODE_GDMA,
-	NODE_ADMA,
-	NODE_NAND,
-	NODE_QSPI,
-	NODE_GPIO,
-	NODE_CAN_0,
-	NODE_CAN_1,
-	NODE_EXTERN,
-	NODE_APLL,
-	NODE_VPLL,
-	NODE_DPLL,
-	NODE_RPLL,
-	NODE_IOPLL,
-	NODE_DDR,
-	NODE_IPI_APU,
-	NODE_IPI_RPU_0,
-	NODE_GPU,
-	NODE_PCIE,
-	NODE_PCAP,
-	NODE_RTC,
-	NODE_LPD,
-	NODE_VCU,
-	NODE_IPI_RPU_1,
-	NODE_IPI_PL_0,
-	NODE_IPI_PL_1,
-	NODE_IPI_PL_2,
-	NODE_IPI_PL_3,
-	NODE_PL,
-	NODE_GEM_TSU,
-	NODE_SWDT_0,
-	NODE_SWDT_1,
-	NODE_CSU,
-	NODE_PJTAG,
-	NODE_TRACE,
-	NODE_TESTSCAN,
-	NODE_PMU,
-	NODE_MAX,
-};
-
-enum pm_request_ack {
-	REQ_ACK_NO = 1,
-	REQ_ACK_BLOCKING,
-	REQ_ACK_NON_BLOCKING,
-};
-
-enum pm_abort_reason {
-	ABORT_REASON_WKUP_EVENT = 100,
-	ABORT_REASON_PU_BUSY,
-	ABORT_REASON_NO_PWRDN,
-	ABORT_REASON_UNKNOWN,
-};
-
-enum pm_suspend_reason {
-	SUSPEND_REASON_PU_REQ = 201,
-	SUSPEND_REASON_ALERT,
-	SUSPEND_REASON_SYS_SHUTDOWN,
-};
-
-enum pm_ram_state {
-	PM_RAM_STATE_OFF = 1,
-	PM_RAM_STATE_RETENTION,
-	PM_RAM_STATE_ON,
-};
-
-enum pm_opchar_type {
-	PM_OPCHAR_TYPE_POWER = 1,
-	PM_OPCHAR_TYPE_TEMP,
-	PM_OPCHAR_TYPE_LATENCY,
-};
-
-/* TODO: move pm_ret_status from device specific location to common location */
-/**
- * @PM_RET_SUCCESS:		success
- * @PM_RET_ERROR_ARGS:		illegal arguments provided (deprecated)
- * @PM_RET_ERROR_NOTSUPPORTED:	feature not supported  (deprecated)
- * @PM_RET_ERROR_NOT_ENABLED:	feature is not enabled
- * @PM_RET_ERROR_INVALID_CRC:	invalid crc in IPI communication
- * @PM_RET_ERROR_INTERNAL:	internal error
- * @PM_RET_ERROR_CONFLICT:	conflict
- * @PM_RET_ERROR_ACCESS:	access rights violation
- * @PM_RET_ERROR_INVALID_NODE:	invalid node
- * @PM_RET_ERROR_DOUBLE_REQ:	duplicate request for same node
- * @PM_RET_ERROR_ABORT_SUSPEND:	suspend procedure has been aborted
- * @PM_RET_ERROR_TIMEOUT:	timeout in communication with PMU
- * @PM_RET_ERROR_NODE_USED:	node is already in use
- */
-enum pm_ret_status {
-	PM_RET_SUCCESS = (0U),
-	PM_RET_ERROR_ARGS = (1U),
-	PM_RET_ERROR_NOTSUPPORTED = (4U),
-	PM_RET_ERROR_NOT_ENABLED = (29U),
-	PM_RET_ERROR_INVALID_CRC = (301U),
-	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)
-};
-
-/**
- * @PM_INITIAL_BOOT:	boot is a fresh system startup
- * @PM_RESUME:		boot is a resume
- * @PM_BOOT_ERROR:	error, boot cause cannot be identified
- */
-enum pm_boot_status {
-	PM_INITIAL_BOOT,
-	PM_RESUME,
-	PM_BOOT_ERROR,
-};
-
-/**
- * @PMF_SHUTDOWN_TYPE_SHUTDOWN:		shutdown
- * @PMF_SHUTDOWN_TYPE_RESET:		reset/reboot
- * @PMF_SHUTDOWN_TYPE_SETSCOPE_ONLY:	set the shutdown/reboot scope
- */
-enum pm_shutdown_type {
-	PMF_SHUTDOWN_TYPE_SHUTDOWN,
-	PMF_SHUTDOWN_TYPE_RESET,
-	PMF_SHUTDOWN_TYPE_SETSCOPE_ONLY,
-};
-
-/**
- * @PMF_SHUTDOWN_SUBTYPE_SUBSYSTEM:	shutdown/reboot APU subsystem only
- * @PMF_SHUTDOWN_SUBTYPE_PS_ONLY:	shutdown/reboot entire PS (but not PL)
- * @PMF_SHUTDOWN_SUBTYPE_SYSTEM:	shutdown/reboot entire system
- */
-enum pm_shutdown_subtype {
-	PMF_SHUTDOWN_SUBTYPE_SUBSYSTEM,
-	PMF_SHUTDOWN_SUBTYPE_PS_ONLY,
-	PMF_SHUTDOWN_SUBTYPE_SYSTEM,
-};
-
-/**
- * @PM_PLL_PARAM_DIV2:		Enable for divide by 2 function inside the PLL
- * @PM_PLL_PARAM_FBDIV:		Feedback divisor integer portion for the PLL
- * @PM_PLL_PARAM_DATA:		Feedback divisor fractional portion for the PLL
- * @PM_PLL_PARAM_PRE_SRC:	Clock source for PLL input
- * @PM_PLL_PARAM_POST_SRC:	Clock source for PLL Bypass mode
- * @PM_PLL_PARAM_LOCK_DLY:	Lock circuit config settings for lock windowsize
- * @PM_PLL_PARAM_LOCK_CNT:	Lock circuit counter setting
- * @PM_PLL_PARAM_LFHF:		PLL loop filter high frequency capacitor control
- * @PM_PLL_PARAM_CP:		PLL charge pump control
- * @PM_PLL_PARAM_RES:		PLL loop filter resistor control
- */
-enum pm_pll_param {
-	PM_PLL_PARAM_DIV2,
-	PM_PLL_PARAM_FBDIV,
-	PM_PLL_PARAM_DATA,
-	PM_PLL_PARAM_PRE_SRC,
-	PM_PLL_PARAM_POST_SRC,
-	PM_PLL_PARAM_LOCK_DLY,
-	PM_PLL_PARAM_LOCK_CNT,
-	PM_PLL_PARAM_LFHF,
-	PM_PLL_PARAM_CP,
-	PM_PLL_PARAM_RES,
-	PM_PLL_PARAM_MAX,
-};
-
-/**
- * @PM_PLL_MODE_RESET:		PLL is in reset (not locked)
- * @PM_PLL_MODE_INTEGER:	PLL is locked in integer mode
- * @PM_PLL_MODE_FRACTIONAL:	PLL is locked in fractional mode
- */
-enum pm_pll_mode {
-	PM_PLL_MODE_RESET,
-	PM_PLL_MODE_INTEGER,
-	PM_PLL_MODE_FRACTIONAL,
-	PM_PLL_MODE_MAX,
-};
-
-/**
- * @PM_CLOCK_DIV0_ID:		Clock divider 0
- * @PM_CLOCK_DIV1_ID:		Clock divider 1
- */
-enum pm_clock_div_id {
-	PM_CLOCK_DIV0_ID,
-	PM_CLOCK_DIV1_ID,
-};
-
-#endif /* PM_DEFS_H */
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_sys.c b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.c
similarity index 99%
rename from plat/xilinx/zynqmp/pm_service/pm_api_sys.c
rename to plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.c
index 58491a0..9133121 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
+++ b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.c
@@ -16,10 +16,10 @@
 #include "pm_api_clock.h"
 #include "pm_api_ioctl.h"
 #include "pm_api_pinctrl.h"
-#include "pm_api_sys.h"
 #include "pm_client.h"
 #include "pm_common.h"
 #include "pm_ipi.h"
+#include "zynqmp_pm_api_sys.h"
 
 #define PM_QUERY_FEATURE_BITMASK ( \
 	(1ULL << (uint64_t)PM_QID_CLOCK_GET_NAME) | \
@@ -1496,7 +1496,7 @@
  *
  * This function returns requested data.
  */
-void pm_query_data(enum pm_query_id qid, uint32_t arg1, uint32_t arg2,
+void pm_query_data(enum pm_query_ids qid, uint32_t arg1, uint32_t arg2,
 		   uint32_t arg3, uint32_t *data)
 {
 	switch (qid) {
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_sys.h b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.h
similarity index 96%
rename from plat/xilinx/zynqmp/pm_service/pm_api_sys.h
rename to plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.h
index 1341e7b..71a0bd5 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_sys.h
+++ b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_api_sys.h
@@ -5,14 +5,15 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#ifndef PM_API_SYS_H
-#define PM_API_SYS_H
+#ifndef ZYNQMP_PM_API_SYS_H
+#define ZYNQMP_PM_API_SYS_H
 
 #include <stdint.h>
 
 #include "pm_defs.h"
+#include "zynqmp_pm_defs.h"
 
-enum pm_query_id {
+enum pm_query_ids {
 	PM_QID_INVALID,
 	PM_QID_CLOCK_GET_NAME,
 	PM_QID_CLOCK_GET_TOPOLOGY,
@@ -150,7 +151,7 @@
 				      uint32_t parent_index);
 enum pm_ret_status pm_clock_getparent(uint32_t clock_id,
 				      uint32_t *parent_index);
-void pm_query_data(enum pm_query_id qid, uint32_t arg1, uint32_t arg2,
+void pm_query_data(enum pm_query_ids qid, uint32_t arg1, uint32_t arg2,
 		   uint32_t arg3, uint32_t *data);
 enum pm_ret_status pm_sha_hash(uint32_t address_high,
 				    uint32_t address_low,
@@ -192,4 +193,4 @@
 				    uint32_t *bit_mask, uint8_t len);
 enum pm_ret_status check_api_dependency(uint8_t id);
 
-#endif /* PM_API_SYS_H */
+#endif /* ZYNQMP_PM_API_SYS_H */
diff --git a/plat/xilinx/zynqmp/pm_service/zynqmp_pm_defs.h b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_defs.h
new file mode 100644
index 0000000..c82a3ef
--- /dev/null
+++ b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_defs.h
@@ -0,0 +1,209 @@
+/*
+ * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Advanced Micro Devices, Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/* ZynqMP power management enums and defines */
+
+#ifndef ZYNQMP_PM_DEFS_H
+#define ZYNQMP_PM_DEFS_H
+
+/*********************************************************************
+ * Macro definitions
+ ********************************************************************/
+
+/*
+ * Version number is a 32bit value, like:
+ * (PM_VERSION_MAJOR << 16) | PM_VERSION_MINOR
+ */
+#define PM_VERSION_MAJOR	1U
+#define PM_VERSION_MINOR	1U
+
+#define PM_VERSION	((PM_VERSION_MAJOR << 16U) | PM_VERSION_MINOR)
+
+/**
+ * PM API versions
+ */
+/* Expected version of firmware APIs */
+#define FW_API_BASE_VERSION		(1U)
+/* Expected version of firmware API for feature check */
+#define FW_API_VERSION_2		(2U)
+/* Version of APIs implemented in ATF */
+#define ATF_API_BASE_VERSION		(1U)
+/* Updating the QUERY_DATA API versioning as the bitmask functionality
+ * support is added in the v2.*/
+#define TFA_API_QUERY_DATA_VERSION	(2U)
+
+/* Capabilities for RAM */
+#define PM_CAP_ACCESS	0x1U
+#define PM_CAP_CONTEXT	0x2U
+
+/* APU processor states */
+#define PM_PROC_STATE_FORCEDOFF		0U
+#define PM_PROC_STATE_ACTIVE		1U
+#define PM_PROC_STATE_SLEEP		2U
+#define PM_PROC_STATE_SUSPENDING	3U
+
+#define PM_SET_SUSPEND_MODE		0xa02
+
+/*********************************************************************
+ * Enum definitions
+ ********************************************************************/
+
+enum pm_node_id {
+	NODE_UNKNOWN = 0,
+	NODE_APU,
+	NODE_APU_0,
+	NODE_APU_1,
+	NODE_APU_2,
+	NODE_APU_3,
+	NODE_RPU,
+	NODE_RPU_0,
+	NODE_RPU_1,
+	NODE_PLD,
+	NODE_FPD,
+	NODE_OCM_BANK_0,
+	NODE_OCM_BANK_1,
+	NODE_OCM_BANK_2,
+	NODE_OCM_BANK_3,
+	NODE_TCM_0_A,
+	NODE_TCM_0_B,
+	NODE_TCM_1_A,
+	NODE_TCM_1_B,
+	NODE_L2,
+	NODE_GPU_PP_0,
+	NODE_GPU_PP_1,
+	NODE_USB_0,
+	NODE_USB_1,
+	NODE_TTC_0,
+	NODE_TTC_1,
+	NODE_TTC_2,
+	NODE_TTC_3,
+	NODE_SATA,
+	NODE_ETH_0,
+	NODE_ETH_1,
+	NODE_ETH_2,
+	NODE_ETH_3,
+	NODE_UART_0,
+	NODE_UART_1,
+	NODE_SPI_0,
+	NODE_SPI_1,
+	NODE_I2C_0,
+	NODE_I2C_1,
+	NODE_SD_0,
+	NODE_SD_1,
+	NODE_DP,
+	NODE_GDMA,
+	NODE_ADMA,
+	NODE_NAND,
+	NODE_QSPI,
+	NODE_GPIO,
+	NODE_CAN_0,
+	NODE_CAN_1,
+	NODE_EXTERN,
+	NODE_APLL,
+	NODE_VPLL,
+	NODE_DPLL,
+	NODE_RPLL,
+	NODE_IOPLL,
+	NODE_DDR,
+	NODE_IPI_APU,
+	NODE_IPI_RPU_0,
+	NODE_GPU,
+	NODE_PCIE,
+	NODE_PCAP,
+	NODE_RTC,
+	NODE_LPD,
+	NODE_VCU,
+	NODE_IPI_RPU_1,
+	NODE_IPI_PL_0,
+	NODE_IPI_PL_1,
+	NODE_IPI_PL_2,
+	NODE_IPI_PL_3,
+	NODE_PL,
+	NODE_GEM_TSU,
+	NODE_SWDT_0,
+	NODE_SWDT_1,
+	NODE_CSU,
+	NODE_PJTAG,
+	NODE_TRACE,
+	NODE_TESTSCAN,
+	NODE_PMU,
+	NODE_MAX,
+};
+
+enum pm_request_ack {
+	REQ_ACK_NO = 1,
+	REQ_ACK_BLOCKING,
+	REQ_ACK_NON_BLOCKING,
+};
+
+enum pm_suspend_reason {
+	SUSPEND_REASON_PU_REQ = 201,
+	SUSPEND_REASON_ALERT,
+	SUSPEND_REASON_SYS_SHUTDOWN,
+};
+
+enum pm_ram_state {
+	PM_RAM_STATE_OFF = 1,
+	PM_RAM_STATE_RETENTION,
+	PM_RAM_STATE_ON,
+};
+
+/**
+ * @PM_INITIAL_BOOT:	boot is a fresh system startup
+ * @PM_RESUME:		boot is a resume
+ * @PM_BOOT_ERROR:	error, boot cause cannot be identified
+ */
+enum pm_boot_status {
+	PM_INITIAL_BOOT,
+	PM_RESUME,
+	PM_BOOT_ERROR,
+};
+
+/**
+ * @PMF_SHUTDOWN_TYPE_SHUTDOWN:		shutdown
+ * @PMF_SHUTDOWN_TYPE_RESET:		reset/reboot
+ * @PMF_SHUTDOWN_TYPE_SETSCOPE_ONLY:	set the shutdown/reboot scope
+ */
+enum pm_shutdown_type {
+	PMF_SHUTDOWN_TYPE_SHUTDOWN,
+	PMF_SHUTDOWN_TYPE_RESET,
+	PMF_SHUTDOWN_TYPE_SETSCOPE_ONLY,
+};
+
+/**
+ * @PMF_SHUTDOWN_SUBTYPE_SUBSYSTEM:	shutdown/reboot APU subsystem only
+ * @PMF_SHUTDOWN_SUBTYPE_PS_ONLY:	shutdown/reboot entire PS (but not PL)
+ * @PMF_SHUTDOWN_SUBTYPE_SYSTEM:	shutdown/reboot entire system
+ */
+enum pm_shutdown_subtype {
+	PMF_SHUTDOWN_SUBTYPE_SUBSYSTEM,
+	PMF_SHUTDOWN_SUBTYPE_PS_ONLY,
+	PMF_SHUTDOWN_SUBTYPE_SYSTEM,
+};
+
+/**
+ * @PM_PLL_MODE_RESET:		PLL is in reset (not locked)
+ * @PM_PLL_MODE_INTEGER:	PLL is locked in integer mode
+ * @PM_PLL_MODE_FRACTIONAL:	PLL is locked in fractional mode
+ */
+enum pm_pll_mode {
+	PM_PLL_MODE_RESET,
+	PM_PLL_MODE_INTEGER,
+	PM_PLL_MODE_FRACTIONAL,
+	PM_PLL_MODE_MAX,
+};
+
+/**
+ * @PM_CLOCK_DIV0_ID:		Clock divider 0
+ * @PM_CLOCK_DIV1_ID:		Clock divider 1
+ */
+enum pm_clock_div_id {
+	PM_CLOCK_DIV0_ID,
+	PM_CLOCK_DIV1_ID,
+};
+
+#endif /* ZYNQMP_PM_DEFS_H */
diff --git a/plat/xilinx/zynqmp/pm_service/pm_svc_main.c b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_svc_main.c
similarity index 99%
rename from plat/xilinx/zynqmp/pm_service/pm_svc_main.c
rename to plat/xilinx/zynqmp/pm_service/zynqmp_pm_svc_main.c
index b35859d..1ccf258 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
+++ b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_svc_main.c
@@ -22,10 +22,10 @@
 #endif
 
 #include <plat_private.h>
-#include "pm_api_sys.h"
 #include "pm_client.h"
-#include "pm_defs.h"
 #include "pm_ipi.h"
+#include "zynqmp_pm_api_sys.h"
+#include "zynqmp_pm_defs.h"
 
 /* pm_up = !0 - UP, pm_up = 0 - DOWN */
 static int32_t pm_up, ipi_irq_flag;
diff --git a/plat/xilinx/zynqmp/pm_service/pm_svc_main.h b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_svc_main.h
similarity index 81%
rename from plat/xilinx/zynqmp/pm_service/pm_svc_main.h
rename to plat/xilinx/zynqmp/pm_service/zynqmp_pm_svc_main.h
index 3c3082f..03ff6d3 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_svc_main.h
+++ b/plat/xilinx/zynqmp/pm_service/zynqmp_pm_svc_main.h
@@ -5,8 +5,8 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#ifndef PM_SVC_MAIN_H
-#define PM_SVC_MAIN_H
+#ifndef ZYNQMP_PM_SVC_MAIN_H
+#define ZYNQMP_PM_SVC_MAIN_H
 
 #include "pm_common.h"
 
@@ -14,4 +14,4 @@
 uint64_t pm_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3,
 			uint64_t x4, const void *cookie, void *handle,
 			uint64_t flags);
-#endif /* PM_SVC_MAIN_H */
+#endif /* ZYNQMP_PM_SVC_MAIN_H */
diff --git a/plat/xilinx/zynqmp/sip_svc_setup.c b/plat/xilinx/zynqmp/sip_svc_setup.c
index c55784e..3844b16 100644
--- a/plat/xilinx/zynqmp/sip_svc_setup.c
+++ b/plat/xilinx/zynqmp/sip_svc_setup.c
@@ -14,7 +14,8 @@
 
 #include <custom_svc.h>
 #include "ipi_mailbox_svc.h"
-#include "pm_svc_main.h"
+#include "pm_defs.h"
+#include "zynqmp_pm_svc_main.h"
 
 /* SMC function IDs for SiP Service queries */
 #define ZYNQMP_SIP_SVC_CALL_COUNT	U(0x8200ff00)
diff --git a/services/spd/opteed/opteed_main.c b/services/spd/opteed/opteed_main.c
index ff09e7e..f069775 100644
--- a/services/spd/opteed/opteed_main.c
+++ b/services/spd/opteed/opteed_main.c
@@ -47,6 +47,10 @@
 
 #if OPTEE_ALLOW_SMC_LOAD
 static bool opteed_allow_load;
+/* OP-TEE image loading service UUID */
+DEFINE_SVC_UUID2(optee_image_load_uuid,
+	0xb1eafba3, 0x5d31, 0x4612, 0xb9, 0x06,
+	0xc4, 0xc7, 0xa4, 0xbe, 0x3c, 0xc0);
 #else
 static int32_t opteed_init(void);
 #endif
@@ -335,6 +339,10 @@
 
 	if (is_caller_non_secure(flags)) {
 #if OPTEE_ALLOW_SMC_LOAD
+		if (opteed_allow_load && smc_fid == NSSMC_OPTEED_CALL_UID) {
+			/* Provide the UUID of the image loading service. */
+			SMC_UUID_RET(handle, optee_image_load_uuid);
+		}
 		if (smc_fid == NSSMC_OPTEED_CALL_LOAD_IMAGE) {
 			/*
 			 * TODO: Consider wiping the code for SMC loading from
diff --git a/services/spd/opteed/teesmc_opteed.h b/services/spd/opteed/teesmc_opteed.h
index eae3ed2..4026fa4 100644
--- a/services/spd/opteed/teesmc_opteed.h
+++ b/services/spd/opteed/teesmc_opteed.h
@@ -157,4 +157,13 @@
 #define NSSMC_OPTEED_CALL_LOAD_IMAGE \
 	NSSMC_OPTEED_CALL(NSSMC_OPTEED_FUNCID_LOAD_IMAGE)
 
+/*
+ * Returns the UID of the OP-TEE image loading service if image loading is
+ * enabled and the image had not been loaded yet. Otherwise this call will be
+ * passed through to OP-TEE where it will return the OP-TEE UID.
+ */
+#define NSSMC_OPTEED_FUNCID_CALLS_UID 0xFF01
+#define NSSMC_OPTEED_CALL_UID \
+	NSSMC_OPTEED_CALL(NSSMC_OPTEED_FUNCID_CALLS_UID)
+
 #endif /*TEESMC_OPTEED_H*/
diff --git a/services/spd/opteed/teesmc_opteed_macros.h b/services/spd/opteed/teesmc_opteed_macros.h
index ad3ed75..7219140 100644
--- a/services/spd/opteed/teesmc_opteed_macros.h
+++ b/services/spd/opteed/teesmc_opteed_macros.h
@@ -17,7 +17,7 @@
 #define NSSMC_OPTEED_CALL(func_num) \
 		((SMC_TYPE_FAST << FUNCID_TYPE_SHIFT) | \
 		((SMC_32) << FUNCID_CC_SHIFT) | \
-		(50 << FUNCID_OEN_SHIFT) | \
+		(63 << FUNCID_OEN_SHIFT) | \
 		((func_num) & FUNCID_NUM_MASK))
 
 #endif /* TEESMC_OPTEED_MACROS_H */
diff --git a/services/std_svc/rmmd/rmmd_main.c b/services/std_svc/rmmd/rmmd_main.c
index e12eae7..24f6c41 100644
--- a/services/std_svc/rmmd/rmmd_main.c
+++ b/services/std_svc/rmmd/rmmd_main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2023, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -117,19 +117,14 @@
  ******************************************************************************/
 static void manage_extensions_realm(cpu_context_t *ctx)
 {
-#if ENABLE_SVE_FOR_NS
+	if (is_feat_sve_supported()) {
 	/*
 	 * Enable SVE and FPU in realm context when it is enabled for NS.
 	 * Realm manager must ensure that the SVE and FPU register
 	 * contexts are properly managed.
 	 */
-	sve_enable(ctx);
-#else
-	/*
-	 * Disable SVE and FPU in realm context when it is disabled for NS.
-	 */
-	sve_disable(ctx);
-#endif /* ENABLE_SVE_FOR_NS */
+		sve_enable(ctx);
+	}
 }
 
 /*******************************************************************************
diff --git a/services/std_svc/sdei/sdei_intr_mgmt.c b/services/std_svc/sdei/sdei_intr_mgmt.c
index 87a1fb7..3bdf4a2 100644
--- a/services/std_svc/sdei/sdei_intr_mgmt.c
+++ b/services/std_svc/sdei/sdei_intr_mgmt.c
@@ -270,11 +270,11 @@
 	 * HCR_EL2.E2H = 1 and HCR_EL2.TGE = 1
 	 */
 	u_register_t hcr_el2 = read_hcr();
-	bool el_is_in_host = is_armv8_1_vhe_present() &&
+	bool el_is_in_host = (read_feat_vhe_id_field() != 0U) &&
 			     (hcr_el2 & HCR_TGE_BIT) &&
 			     (hcr_el2 & HCR_E2H_BIT);
 
-	if (is_armv8_1_pan_present() &&
+	if (is_feat_pan_supported() &&
 	    ((client_el == MODE_EL1) ||
 		(client_el == MODE_EL2 && el_is_in_host)) &&
 	    ((client_el_sctlr & SCTLR_SPAN_BIT) == 0U)) {
diff --git a/services/std_svc/spm/spm_mm/spm_mm.mk b/services/std_svc/spm/spm_mm/spm_mm.mk
index f6691c3..513e8ef 100644
--- a/services/std_svc/spm/spm_mm/spm_mm.mk
+++ b/services/std_svc/spm/spm_mm/spm_mm.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2017-2023, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -10,10 +10,10 @@
 ifneq (${ARCH},aarch64)
         $(error "Error: SPM_MM is only supported on aarch64.")
 endif
-ifeq (${ENABLE_SVE_FOR_NS},1)
+ifneq (${ENABLE_SVE_FOR_NS},0)
         $(error "Error: SPM_MM is not compatible with ENABLE_SVE_FOR_NS")
 endif
-ifeq (${ENABLE_SME_FOR_NS},1)
+ifneq (${ENABLE_SME_FOR_NS},0)
         $(error "Error: SPM_MM is not compatible with ENABLE_SME_FOR_NS")
 endif
 ifeq (${CTX_INCLUDE_FPREGS},0)
diff --git a/services/std_svc/spmd/spmd.mk b/services/std_svc/spmd/spmd.mk
index 8efbdc8..6f451c8 100644
--- a/services/std_svc/spmd/spmd.mk
+++ b/services/std_svc/spmd/spmd.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2021-2023, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -8,7 +8,7 @@
 	$(error "Error: SPMD is only supported on aarch64.")
 endif
 
-ifeq (${ENABLE_SME_FOR_NS},1)
+ifneq (${ENABLE_SME_FOR_NS},0)
 	$(error "Error: SPMD is not compatible with ENABLE_SME_FOR_NS")
 endif
 
diff --git a/services/std_svc/spmd/spmd_main.c b/services/std_svc/spmd/spmd_main.c
index e85109d..0e1899e 100644
--- a/services/std_svc/spmd/spmd_main.c
+++ b/services/std_svc/spmd/spmd_main.c
@@ -399,7 +399,7 @@
 	 * Check if S-EL2 is supported on this system if S-EL2
 	 * is required for SPM
 	 */
-	if (!is_armv8_4_sel2_present()) {
+	if (!is_feat_sel2_supported()) {
 		WARN("SPM Core run time S-EL2 is not supported.\n");
 		return -EINVAL;
 	}