diff --git a/Makefile b/Makefile
index 3a8a522..0f045e5 100644
--- a/Makefile
+++ b/Makefile
@@ -1002,6 +1002,7 @@
         PROGRAMMABLE_RESET_ADDRESS \
         PSCI_EXTENDED_STATE_ID \
         RESET_TO_BL31 \
+        RESET_TO_BL31_WITH_PARAMS \
         SAVE_KEYS \
         SEPARATE_CODE_AND_RODATA \
         SEPARATE_BL2_NOLOAD_REGION \
@@ -1068,6 +1069,8 @@
         NR_OF_FW_BANKS \
         NR_OF_IMAGES_IN_FW_BANK \
         RAS_EXTENSION \
+        TWED_DELAY \
+        ENABLE_FEAT_TWED \
 )))
 
 ifdef KEY_SIZE
@@ -1134,6 +1137,7 @@
         PSCI_EXTENDED_STATE_ID \
         RAS_EXTENSION \
         RESET_TO_BL31 \
+        RESET_TO_BL31_WITH_PARAMS \
         SEPARATE_CODE_AND_RODATA \
         SEPARATE_BL2_NOLOAD_REGION \
         SEPARATE_NOBITS_REGION \
@@ -1184,6 +1188,8 @@
         ENABLE_FEAT_CSV2_2 \
         ENABLE_FEAT_PAN \
         FEATURE_DETECTION \
+        TWED_DELAY \
+        ENABLE_FEAT_TWED \
 )))
 
 ifeq (${SANITIZE_UB},trap)
diff --git a/bl2/bl2_el3.ld.S b/bl2/bl2_el3.ld.S
index 6aa7afd..c95706c 100644
--- a/bl2/bl2_el3.ld.S
+++ b/bl2/bl2_el3.ld.S
@@ -17,12 +17,12 @@
     RAM (rwx): ORIGIN = BL2_RW_BASE, LENGTH = BL2_RW_LIMIT - BL2_RW_BASE
 #else
     RAM (rwx): ORIGIN = BL2_BASE, LENGTH = BL2_LIMIT - BL2_BASE
+#endif
 #if SEPARATE_BL2_NOLOAD_REGION
     RAM_NOLOAD (rw!a): ORIGIN = BL2_NOLOAD_START, LENGTH = BL2_NOLOAD_LIMIT - BL2_NOLOAD_START
 #else
 #define RAM_NOLOAD RAM
 #endif
-#endif
 }
 
 #if !BL2_IN_XIP_MEM
diff --git a/bl31/aarch64/bl31_entrypoint.S b/bl31/aarch64/bl31_entrypoint.S
index ed05864..b0c46dc 100644
--- a/bl31/aarch64/bl31_entrypoint.S
+++ b/bl31/aarch64/bl31_entrypoint.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -67,6 +67,7 @@
 		_exception_vectors=runtime_exceptions		\
 		_pie_fixup_size=BL31_LIMIT - BL31_BASE
 
+#if !RESET_TO_BL31_WITH_PARAMS
 	/* ---------------------------------------------------------------------
 	 * For RESET_TO_BL31 systems, BL31 is the first bootloader to run so
 	 * there's no argument to relay from a previous bootloader. Zero the
@@ -77,6 +78,7 @@
 	mov	x21, 0
 	mov	x22, 0
 	mov	x23, 0
+#endif /* RESET_TO_BL31_WITH_PARAMS */
 #endif /* RESET_TO_BL31 */
 
 	/* --------------------------------------------------------------------
diff --git a/bl32/sp_min/sp_min.mk b/bl32/sp_min/sp_min.mk
index 590b032..ab1287d 100644
--- a/bl32/sp_min/sp_min.mk
+++ b/bl32/sp_min/sp_min.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2016-2022, Arm Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -35,6 +35,10 @@
 ifeq (${WORKAROUND_CVE_2017_5715},1)
 BL32_SOURCES		+=	bl32/sp_min/wa_cve_2017_5715_bpiall.S	\
 				bl32/sp_min/wa_cve_2017_5715_icache_inv.S
+else
+ifeq (${WORKAROUND_CVE_2022_23960},1)
+BL32_SOURCES		+=	bl32/sp_min/wa_cve_2017_5715_icache_inv.S
+endif
 endif
 
 ifeq (${TRNG_SUPPORT},1)
diff --git a/common/fdt_wrappers.c b/common/fdt_wrappers.c
index 2a9673f..1b065b1 100644
--- a/common/fdt_wrappers.c
+++ b/common/fdt_wrappers.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -618,3 +618,24 @@
 
 	return ret;
 }
+
+/*
+ * Find a given node in device tree. If not present, add it.
+ * Returns offset of node found/added on success, and < 0 on error.
+ */
+int fdtw_find_or_add_subnode(void *fdt, int parentoffset, const char *name)
+{
+	int offset;
+
+	offset = fdt_subnode_offset(fdt, parentoffset, name);
+
+	if (offset == -FDT_ERR_NOTFOUND) {
+		offset = fdt_add_subnode(fdt, parentoffset, name);
+	}
+
+	if (offset < 0) {
+		ERROR("%s: %s: %s\n", __func__, name, fdt_strerror(offset));
+	}
+
+	return offset;
+}
diff --git a/common/feat_detect.c b/common/feat_detect.c
index ef09b86..8f98876 100644
--- a/common/feat_detect.c
+++ b/common/feat_detect.c
@@ -203,6 +203,16 @@
 #endif
 }
 
+/***********************************************************
+ * Feature : FEAT_TWED (Delayed Trapping of WFE Instruction)
+ **********************************************************/
+static void read_feat_twed(void)
+{
+#if (ENABLE_FEAT_TWED == FEAT_STATE_1)
+	feat_detect_panic(is_armv8_6_twed_present(), "TWED");
+#endif
+}
+
 /******************************************************************
  * Feature : FEAT_HCX (Extended Hypervisor Configuration Register)
  *****************************************************************/
@@ -279,6 +289,7 @@
 	read_feat_amuv1p1();
 	read_feat_fgt();
 	read_feat_ecv();
+	read_feat_twed();
 
 	/* v8.7 features */
 	read_feat_hcx();
diff --git a/docs/components/fconf/fconf_properties.rst b/docs/components/fconf/fconf_properties.rst
index 5c28a7a..20cc758 100644
--- a/docs/components/fconf/fconf_properties.rst
+++ b/docs/components/fconf/fconf_properties.rst
@@ -30,3 +30,10 @@
     - value type: <u32>
     - Image ID of the configuration.
 
+- ns-load-address [optional]
+    - value type: <u64>
+    - Physical loading base address of the configuration in the non-secure
+      memory.
+      Only needed by those configuration files which require being loaded
+      in secure memory (at load-address) as well as in non-secure memory
+      e.g. HW_CONFIG
diff --git a/docs/design/firmware-design.rst b/docs/design/firmware-design.rst
index 0831dc0..71fdfcb 100644
--- a/docs/design/firmware-design.rst
+++ b/docs/design/firmware-design.rst
@@ -131,6 +131,9 @@
    -  For other BL3x images, if the firmware configuration file is loaded by
       BL2, then its address is passed in ``arg0`` and if HW_CONFIG is loaded
       then its address is passed in ``arg1``.
+   -  In case of the Arm FVP platform, FW_CONFIG address passed in ``arg1`` to
+      BL31/SP_MIN, and the SOC_FW_CONFIG and HW_CONFIG details are retrieved
+      from FW_CONFIG device tree.
 
 BL1
 ~~~
@@ -1757,12 +1760,20 @@
                    DRAM
     0xffffffff +----------+
                :          :
-               |----------|
+    0x82100000 |----------|
                |HW_CONFIG |
-    0x83000000 |----------|  (non-secure)
+    0x82000000 |----------|  (non-secure)
                |          |
     0x80000000 +----------+
 
+               Trusted DRAM
+    0x08000000 +----------+
+               |HW_CONFIG |
+    0x07f00000 |----------|
+               :          :
+               |          |
+    0x06000000 +----------+
+
                Trusted SRAM
     0x04040000 +----------+  loaded by BL2  +----------------+
                | BL1 (rw) |  <<<<<<<<<<<<<  |                |
@@ -1790,15 +1801,18 @@
                      DRAM
     0xffffffff +--------------+
                :              :
-               |--------------|
+    0x82100000 |--------------|
                |  HW_CONFIG   |
-    0x83000000 |--------------|  (non-secure)
+    0x82000000 |--------------|  (non-secure)
                |              |
     0x80000000 +--------------+
 
-                Trusted DRAM
+                 Trusted DRAM
     0x08000000 +--------------+
-               |     BL32     |
+               |  HW_CONFIG   |
+    0x07f00000 |--------------|
+               :              :
+               |    BL32      |
     0x06000000 +--------------+
 
                  Trusted SRAM
@@ -1829,12 +1843,20 @@
                |  BL32    |  (secure)
     0xff000000 +----------+
                |          |
-               |----------|
+    0x82100000 |----------|
                |HW_CONFIG |
-    0x83000000 |----------|  (non-secure)
+    0x82000000 |----------|  (non-secure)
                |          |
     0x80000000 +----------+
 
+               Trusted DRAM
+    0x08000000 +----------+
+               |HW_CONFIG |
+    0x7f000000 |----------|
+               :          :
+               |          |
+    0x06000000 +----------+
+
                Trusted SRAM
     0x04040000 +----------+  loaded by BL2  +----------------+
                | BL1 (rw) |  <<<<<<<<<<<<<  |                |
@@ -2729,7 +2751,7 @@
 
 --------------
 
-*Copyright (c) 2013-2021, Arm Limited and Contributors. All rights reserved.*
+*Copyright (c) 2013-2022, Arm Limited and Contributors. All rights reserved.*
 
 .. _Power State Coordination Interface PDD: http://infocenter.arm.com/help/topic/com.arm.doc.den0022d/Power_State_Coordination_Interface_PDD_v1_1_DEN0022D.pdf
 .. _SMCCC: https://developer.arm.com/docs/den0028/latest
diff --git a/docs/design/reset-design.rst b/docs/design/reset-design.rst
index 7b10c95..666ee4f 100644
--- a/docs/design/reset-design.rst
+++ b/docs/design/reset-design.rst
@@ -141,19 +141,26 @@
 Platform initialization
 ~~~~~~~~~~~~~~~~~~~~~~~
 
-In this configuration, when the CPU resets to BL31 there are no parameters that
-can be passed in registers by previous boot stages. Instead, the platform code
-in BL31 needs to know, or be able to determine, the location of the BL32 (if
-required) and BL33 images and provide this information in response to the
+In this configuration, when the CPU resets to BL31 there should be no parameters
+that can be passed in registers by previous boot stages. Instead, the platform
+code in BL31 needs to know, or be able to determine, the location of the BL32
+(if required) and BL33 images and provide this information in response to the
 ``bl31_plat_get_next_image_ep_info()`` function.
 
+.. note::
+   Some platforms that configure ``RESET_TO_BL31`` might still be able to
+   receive parameters in registers depending on their actual boot sequence. On
+   those occasions, and in addition to ``RESET_TO_BL31``, these platforms should
+   set ``RESET_TO_BL31_WITH_PARAMS`` to avoid the input registers from being
+   zeroed before entering BL31.
+
 Additionally, platform software is responsible for carrying out any security
 initialisation, for example programming a TrustZone address space controller.
 This might be done by the Trusted Boot Firmware or by platform code in BL31.
 
 --------------
 
-*Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.*
+*Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.*
 
 .. |Default reset code flow| image:: ../resources/diagrams/default_reset_code.png
 .. |Reset code flow with programmable reset address| image:: ../resources/diagrams/reset_code_no_boot_type_check.png
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index d30e22f..585b2a1 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -327,6 +327,14 @@
    This flag can take values 0 to 2, to align with the ``FEATURE_DETECTION``
    mechanism. Default is ``0``.
 
+-  ``ENABLE_FEAT_TWED``: Numeric value to enable the ``FEAT_TWED`` (Delayed
+   trapping of WFE Instruction) extension. ``FEAT_TWED`` is a optional feature
+   available on Arm v8.6. This flag can take values 0 to 2, to align with the
+   ``FEATURE_DETECTION`` mechanism. Default is ``0``.
+
+    When ``ENABLE_FEAT_TWED`` is set to ``1``, WFE instruction trapping gets
+    delayed by the amount of value in ``TWED_DELAY``.
+
 -  ``ENABLE_FEAT_VHE``: Numeric value to enable the ``FEAT_VHE`` (Virtualization
    Host Extensions) extension. It allows access to CONTEXTIDR_EL2 register
    during EL2 context save/restore operations.``FEAT_VHE`` is a mandatory
@@ -709,6 +717,11 @@
    entrypoint) or 1 (CPU reset to BL31 entrypoint).
    The default value is 0.
 
+-  ``RESET_TO_BL31_WITH_PARAMS``: If ``RESET_TO_BL31`` has been enabled, setting
+   this additional option guarantees that the input registers are not cleared
+   therefore allowing parameters to be passed to the BL31 entrypoint.
+   The default value is 0.
+
 -  ``RESET_TO_SP_MIN``: SP_MIN is the minimal AArch32 Secure Payload provided
    in TF-A. This flag configures SP_MIN entrypoint as the CPU reset vector
    instead of the BL1 entrypoint. It can take the value 0 (CPU reset to BL1
@@ -845,6 +858,12 @@
       When ``EL3_EXCEPTION_HANDLING`` is ``1``, ``TSP_NS_INTR_ASYNC_PREEMPT``
       must also be set to ``1``.
 
+-  ``TWED_DELAY``: Numeric value to be set in order to delay the trapping of
+   WFE instruction. ``ENABLE_FEAT_TWED`` build option must be enabled to set
+   this delay. It can take values in the range (0-15). Default value is ``0``
+   and based on this value, 2^(TWED_DELAY + 8) cycles will be delayed.
+   Platforms need to explicitly update this value based on their requirements.
+
 -  ``USE_ARM_LINK``: This flag determines whether to enable support for ARM
    linker. When the ``LINKER`` build variable points to the armlink linker,
    this flag is enabled automatically. To enable support for armlink, platforms
diff --git a/docs/getting_started/porting-guide.rst b/docs/getting_started/porting-guide.rst
index 3d3b2e3..2c6a005 100644
--- a/docs/getting_started/porting-guide.rst
+++ b/docs/getting_started/porting-guide.rst
@@ -2118,21 +2118,6 @@
 of the system counter, which is retrieved from the first entry in the frequency
 modes table.
 
-Function : plat_arm_set_twedel_scr_el3() [optional]
-~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-
-::
-
-    Argument : void
-    Return   : uint32_t
-
-This function is used in v8.6+ systems to set the WFE trap delay value in
-SCR_EL3. If this function returns TWED_DISABLED or is left unimplemented, this
-feature is not enabled.  The only hook provided is to set the TWED fields in
-SCR_EL3, there are similar fields in HCR_EL2, SCTLR_EL2, and SCTLR_EL1 to adjust
-the WFE trap delays in lower ELs and these fields should be set by the
-appropriate EL2 or EL1 code depending on the platform configuration.
-
 #define : PLAT_PERCPU_BAKERY_LOCK_SIZE [optional]
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
diff --git a/docs/getting_started/prerequisites.rst b/docs/getting_started/prerequisites.rst
index a9024e2..92a2c83 100644
--- a/docs/getting_started/prerequisites.rst
+++ b/docs/getting_started/prerequisites.rst
@@ -26,7 +26,7 @@
 |TF-A| can be built with any of the following *cross-compiler* toolchains that
 target the Armv7-A or Armv8-A architectures:
 
-- GCC >= 10.3-2021.07 (from the `Arm Developer website`_)
+- GCC >= 11.2-2022.02 (from the `Arm Developer website`_)
 - Clang >= 4.0
 - Arm Compiler >= 6.0
 
diff --git a/docs/plat/arm/fvp/index.rst b/docs/plat/arm/fvp/index.rst
index 2aaf195..9280f7b 100644
--- a/docs/plat/arm/fvp/index.rst
+++ b/docs/plat/arm/fvp/index.rst
@@ -392,7 +392,8 @@
 -  BL1 is loaded at the start of the Trusted ROM.
 -  The Firmware Image Package is loaded at the start of NOR FLASH0.
 -  The firmware loads the FDT packaged in FIP to the DRAM. The FDT load address
-   is specified via the ``hw_config_addr`` property in `TB_FW_CONFIG for FVP`_.
+   is specified via the ``load-address`` property in the ``hw-config`` node of
+   `FW_CONFIG for FVP`_.
 -  The default use-case for the Foundation FVP is to use the ``--gicv3`` option
    and enable the GICv3 device in the model. Note that without this option,
    the Foundation FVP defaults to legacy (Versatile Express) memory map which
@@ -643,9 +644,9 @@
 
 --------------
 
-*Copyright (c) 2019-2021, Arm Limited. All rights reserved.*
+*Copyright (c) 2019-2022, Arm Limited. All rights reserved.*
 
-.. _TB_FW_CONFIG for FVP: https://git.trustedfirmware.org/TF-A/trusted-firmware-a.git/tree/plat/arm/board/fvp/fdts/fvp_tb_fw_config.dts
+.. _FW_CONFIG for FVP: https://git.trustedfirmware.org/TF-A/trusted-firmware-a.git/tree/plat/arm/board/fvp/fdts/fvp_fw_config.dts
 .. _Arm's website: `FVP models`_
 .. _FVP models: https://developer.arm.com/products/system-design/fixed-virtual-platforms
 .. _Linaro Release 20.01: http://releases.linaro.org/members/arm/platforms/20.01
diff --git a/docs/plat/xilinx-versal.rst b/docs/plat/xilinx-versal.rst
index d65b048..91ad6f1 100644
--- a/docs/plat/xilinx-versal.rst
+++ b/docs/plat/xilinx-versal.rst
@@ -43,6 +43,8 @@
 
 *   `VERSAL_PLATFORM`: Select the platform. Options:
     -   `versal_virt`	: Versal Virtual platform
+    -   `spp_itr6`	: SPP ITR6
+    -   `emu_it6`	: EMU ITR6
 
 # PLM->TF-A Parameter Passing
 ------------------------------
diff --git a/docs/plat/xilinx-zynqmp.rst b/docs/plat/xilinx-zynqmp.rst
index 79c2535..af1cb22 100644
--- a/docs/plat/xilinx-zynqmp.rst
+++ b/docs/plat/xilinx-zynqmp.rst
@@ -14,13 +14,13 @@
 
 .. code:: bash
 
-    make CROSS_COMPILE=aarch64-none-elf- PLAT=zynqmp bl31
+    make CROSS_COMPILE=aarch64-none-elf- PLAT=zynqmp RESET_TO_BL31=1 bl31
 
 To build bl32 TSP you have to rebuild bl31 too:
 
 .. code:: bash
 
-    make CROSS_COMPILE=aarch64-none-elf- PLAT=zynqmp SPD=tspd bl31 bl32
+    make CROSS_COMPILE=aarch64-none-elf- PLAT=zynqmp SPD=tspd RESET_TO_BL31=1 bl31 bl32
 
 To build TF-A for JTAG DCC console:
 
diff --git a/drivers/arm/smmu/smmu_v3.c b/drivers/arm/smmu/smmu_v3.c
index a082a81..45f6df9 100644
--- a/drivers/arm/smmu/smmu_v3.c
+++ b/drivers/arm/smmu/smmu_v3.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,7 @@
 #include <drivers/arm/smmu_v3.h>
 #include <drivers/delay_timer.h>
 #include <lib/mmio.h>
+#include <arch_features.h>
 
 /* SMMU poll number of retries */
 #define SMMU_POLL_TIMEOUT_US	U(1000)
@@ -79,14 +80,74 @@
 	if (smmuv3_security_init(smmu_base) != 0)
 		return -1;
 
-	/* Check if the SMMU supports secure state */
-	if ((mmio_read_32(smmu_base + SMMU_S_IDR1) &
-				SMMU_S_IDR1_SECURE_IMPL) == 0U)
-		return 0;
+#if ENABLE_RME
+
+	if (get_armv9_2_feat_rme_support() != 0U) {
+		if ((mmio_read_32(smmu_base + SMMU_ROOT_IDR0) &
+				  SMMU_ROOT_IDR0_ROOT_IMPL) == 0U) {
+			WARN("Skip SMMU GPC configuration.\n");
+		} else {
+			uint64_t gpccr_el3 = read_gpccr_el3();
+			uint64_t gptbr_el3 = read_gptbr_el3();
+
+			/* SMMU_ROOT_GPT_BASE_CFG[16] is RES0. */
+			gpccr_el3 &= ~(1UL << 16);
+
+			/*
+			 * TODO: SMMU_ROOT_GPT_BASE_CFG is 64b in the spec,
+			 * but SMMU model only accepts 32b access.
+			 */
+			mmio_write_32(smmu_base + SMMU_ROOT_GPT_BASE_CFG,
+				      gpccr_el3);
+
+			/*
+			 * pa_gpt_table_base[51:12] maps to GPTBR_EL3[39:0]
+			 * whereas it maps to SMMU_ROOT_GPT_BASE[51:12]
+			 * hence needs a 12 bit left shit.
+			 */
+			mmio_write_64(smmu_base + SMMU_ROOT_GPT_BASE,
+				      gptbr_el3 << 12);
+
+			/*
+			 * ACCESSEN=1: SMMU- and client-originated accesses are
+			 *             not terminated by this mechanism.
+			 * GPCEN=1: All clients and SMMU-originated accesses,
+			 *          except GPT-walks, are subject to GPC.
+			 */
+			mmio_setbits_32(smmu_base + SMMU_ROOT_CR0,
+					SMMU_ROOT_CR0_GPCEN |
+					SMMU_ROOT_CR0_ACCESSEN);
+
+			/* Poll for ACCESSEN and GPCEN ack bits. */
+			if (smmuv3_poll(smmu_base + SMMU_ROOT_CR0ACK,
+					SMMU_ROOT_CR0_GPCEN |
+					SMMU_ROOT_CR0_ACCESSEN,
+					SMMU_ROOT_CR0_GPCEN |
+					SMMU_ROOT_CR0_ACCESSEN) != 0) {
+				WARN("Failed enabling SMMU GPC.\n");
+
+				/*
+				 * Do not return in error, but fall back to
+				 * invalidating all entries through the secure
+				 * register file.
+				 */
+			}
+		}
+	}
+
+#endif /* ENABLE_RME */
+
 	/*
 	 * Initiate invalidation of secure caches and TLBs if the SMMU
 	 * supports secure state. If not, it's implementation defined
 	 * as to how SMMU_S_INIT register is accessed.
+	 * Arm SMMU Arch RME supplement, section 3.4: all SMMU registers
+	 * specified to be accessible only in secure physical address space are
+	 * additionally accessible in root physical address space in an SMMU
+	 * with RME.
+	 * Section 3.3: as GPT information is permitted to be cached in a TLB,
+	 * the SMMU_S_INIT.INV_ALL mechanism also invalidates GPT information
+	 * cached in TLBs.
 	 */
 	mmio_write_32(smmu_base + SMMU_S_INIT, SMMU_S_INIT_INV_ALL);
 
diff --git a/drivers/st/clk/clk-stm32-core.c b/drivers/st/clk/clk-stm32-core.c
index 355c9da..e1b6940 100644
--- a/drivers/st/clk/clk-stm32-core.c
+++ b/drivers/st/clk/clk-stm32-core.c
@@ -327,6 +327,9 @@
 	}
 
 	old_parent = _clk_stm32_get_parent(priv, clk);
+	if (old_parent < 0) {
+		return old_parent;
+	}
 	if (old_parent == clkp) {
 		return 0;
 	}
@@ -415,7 +418,7 @@
 		sel = clk_mux_get_parent(priv, mux_id);
 	}
 
-	if (sel < parent->num_parents) {
+	if ((sel >= 0) && (sel < parent->num_parents)) {
 		return parent->id_parents[sel];
 	}
 
@@ -488,6 +491,9 @@
 	}
 
 	parent = _clk_stm32_get_parent(priv, id);
+	if (parent < 0) {
+		return 0UL;
+	}
 
 	if (clk->ops->recalc_rate != NULL) {
 		unsigned long prate = 0UL;
@@ -517,6 +523,10 @@
 {
 	int parent_id = _clk_stm32_get_parent(priv, id);
 
+	if (parent_id < 0) {
+		return 0UL;
+	}
+
 	return _clk_stm32_get_rate(priv, parent_id);
 }
 
@@ -552,6 +562,9 @@
 
 	if (priv->gate_refcounts[id] == 0U) {
 		parent = _clk_stm32_get_parent(priv, id);
+		if (parent < 0) {
+			return parent;
+		}
 		if (parent != CLK_IS_ROOT) {
 			ret = _clk_stm32_enable_core(priv, parent);
 			if (ret) {
@@ -616,7 +629,7 @@
 	clk_stm32_disable_call_ops(priv, id);
 
 	parent = _clk_stm32_get_parent(priv, id);
-	if (parent != CLK_IS_ROOT) {
+	if ((parent >= 0) && (parent != CLK_IS_ROOT)) {
 		_clk_stm32_disable_core(priv, parent);
 	}
 }
@@ -1060,12 +1073,15 @@
 	uint32_t i;
 
 	cell = fdt_getprop(fdt, node, name, &len);
-	if (cell != NULL) {
-		for (i = 0; i < ((uint32_t)len / sizeof(uint32_t)); i++) {
-			uint32_t val = fdt32_to_cpu(cell[i]);
+	if (cell == NULL) {
+		*nb = 0U;
+		return 0;
+	}
 
-			tab[i] = val;
-		}
+	for (i = 0; i < ((uint32_t)len / sizeof(uint32_t)); i++) {
+		uint32_t val = fdt32_to_cpu(cell[i]);
+
+		tab[i] = val;
 	}
 
 	*nb = (uint32_t)len / sizeof(uint32_t);
diff --git a/fdts/tc.dts b/fdts/tc.dts
index 7c0e842..2099229 100644
--- a/fdts/tc.dts
+++ b/fdts/tc.dts
@@ -261,6 +261,12 @@
 		arm,mhuv2-protocols = <0 1>;
 	};
 
+	cmn-pmu {
+		compatible = "arm,ci-700";
+		reg = <0x0 0x50000000 0x0 0x10000000>;
+		interrupts = <0x0 460 0x4>;
+	};
+
 	scmi {
 		compatible = "arm,scmi";
 		mbox-names = "tx", "rx";
diff --git a/include/common/fdt_wrappers.h b/include/common/fdt_wrappers.h
index 9c7180c..2929fc2 100644
--- a/include/common/fdt_wrappers.h
+++ b/include/common/fdt_wrappers.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -44,6 +44,8 @@
 int fdtw_for_each_cpu(const void *fdt,
 		      int (*callback)(const void *dtb, int node, uintptr_t mpidr));
 
+int fdtw_find_or_add_subnode(void *fdt, int parentoffset, const char *name);
+
 static inline uint32_t fdt_blob_size(const void *dtb)
 {
 	const uint32_t *dtb_header = dtb;
diff --git a/include/drivers/arm/smmu_v3.h b/include/drivers/arm/smmu_v3.h
index a820a44..e60c754 100644
--- a/include/drivers/arm/smmu_v3.h
+++ b/include/drivers/arm/smmu_v3.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,7 @@
 
 #include <stdint.h>
 #include <lib/utils_def.h>
+#include <platform_def.h>
 
 /* SMMUv3 register offsets from device base */
 #define SMMU_GBPA	U(0x0044)
@@ -16,6 +17,26 @@
 #define SMMU_S_INIT	U(0x803c)
 #define SMMU_S_GBPA	U(0x8044)
 
+/*
+ * TODO: SMMU_ROOT_PAGE_OFFSET is platform specific.
+ * Currently defined as a command line model parameter.
+ */
+#if ENABLE_RME
+
+#define SMMU_ROOT_PAGE_OFFSET	(PLAT_ARM_SMMUV3_ROOT_REG_OFFSET)
+#define SMMU_ROOT_IDR0		U(SMMU_ROOT_PAGE_OFFSET + 0x0000)
+#define SMMU_ROOT_IIDR		U(SMMU_ROOT_PAGE_OFFSET + 0x0008)
+#define SMMU_ROOT_CR0		U(SMMU_ROOT_PAGE_OFFSET + 0x0020)
+#define SMMU_ROOT_CR0ACK	U(SMMU_ROOT_PAGE_OFFSET + 0x0024)
+#define SMMU_ROOT_GPT_BASE	U(SMMU_ROOT_PAGE_OFFSET + 0x0028)
+#define SMMU_ROOT_GPT_BASE_CFG	U(SMMU_ROOT_PAGE_OFFSET + 0x0030)
+#define SMMU_ROOT_GPF_FAR	U(SMMU_ROOT_PAGE_OFFSET + 0x0038)
+#define SMMU_ROOT_GPT_CFG_FAR	U(SMMU_ROOT_PAGE_OFFSET + 0x0040)
+#define SMMU_ROOT_TLBI		U(SMMU_ROOT_PAGE_OFFSET + 0x0050)
+#define SMMU_ROOT_TLBI_CTRL	U(SMMU_ROOT_PAGE_OFFSET + 0x0058)
+
+#endif /* ENABLE_RME */
+
 /* SMMU_GBPA register fields */
 #define SMMU_GBPA_UPDATE		(1UL << 31)
 #define SMMU_GBPA_ABORT			(1UL << 20)
@@ -30,6 +51,13 @@
 #define SMMU_S_GBPA_UPDATE		(1UL << 31)
 #define SMMU_S_GBPA_ABORT		(1UL << 20)
 
+/* SMMU_ROOT_IDR0 register fields */
+#define SMMU_ROOT_IDR0_ROOT_IMPL	(1UL << 0)
+
+/* SMMU_ROOT_CR0 register fields */
+#define SMMU_ROOT_CR0_GPCEN		(1UL << 1)
+#define SMMU_ROOT_CR0_ACCESSEN		(1UL << 0)
+
 int smmuv3_init(uintptr_t smmu_base);
 int smmuv3_security_init(uintptr_t smmu_base);
 
diff --git a/include/lib/extensions/twed.h b/include/lib/extensions/twed.h
deleted file mode 100644
index eac4aa3..0000000
--- a/include/lib/extensions/twed.h
+++ /dev/null
@@ -1,16 +0,0 @@
-/*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef TWED_H
-#define TWED_H
-
-#include <stdint.h>
-
-#define TWED_DISABLED U(0xFFFFFFFF)
-
-uint32_t plat_arm_set_twedel_scr_el3(void);
-
-#endif /* TWEDE_H */
diff --git a/include/lib/fconf/fconf_dyn_cfg_getter.h b/include/lib/fconf/fconf_dyn_cfg_getter.h
index ff51c6c..43f298e 100644
--- a/include/lib/fconf/fconf_dyn_cfg_getter.h
+++ b/include/lib/fconf/fconf_dyn_cfg_getter.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2019-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,6 +18,13 @@
 	uintptr_t config_addr;
 	uint32_t config_max_size;
 	unsigned int config_id;
+	/*
+	 * Load address in non-secure memory. Only needed by those
+	 * configuration files which require being loaded in secure
+	 * memory (at config_addr) as well as in non-secure memory
+	 * - e.g. HW_CONFIG
+	 */
+	uintptr_t ns_config_addr;
 };
 
 unsigned int dyn_cfg_dtb_info_get_index(unsigned int config_id);
@@ -25,7 +32,8 @@
 int fconf_populate_dtb_registry(uintptr_t config);
 
 /* Set config information in global DTB array */
-void set_config_info(uintptr_t config_addr, uint32_t config_max_size,
-			unsigned int config_id);
+void set_config_info(uintptr_t config_addr, uintptr_t ns_config_addr,
+		     uint32_t config_max_size,
+		     unsigned int config_id);
 
 #endif /* FCONF_DYN_CFG_GETTER_H */
diff --git a/include/plat/arm/common/arm_def.h b/include/plat/arm/common/arm_def.h
index 2af8c11..29edb4b 100644
--- a/include/plat/arm/common/arm_def.h
+++ b/include/plat/arm/common/arm_def.h
@@ -284,12 +284,10 @@
 					ARM_EL3_TZC_DRAM1_SIZE,		\
 					MT_MEMORY | MT_RW | EL3_PAS)
 
-#if defined(SPD_spmd)
 #define ARM_MAP_TRUSTED_DRAM	MAP_REGION_FLAT(			\
 					PLAT_ARM_TRUSTED_DRAM_BASE,	\
 					PLAT_ARM_TRUSTED_DRAM_SIZE,	\
 					MT_MEMORY | MT_RW | MT_SECURE)
-#endif
 
 #if ENABLE_RME
 #define ARM_MAP_RMM_DRAM	MAP_REGION_FLAT(			\
diff --git a/lib/cpus/aarch32/cortex_a15.S b/lib/cpus/aarch32/cortex_a15.S
index ab136ad..1143e9b 100644
--- a/lib/cpus/aarch32/cortex_a15.S
+++ b/lib/cpus/aarch32/cortex_a15.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -100,6 +100,15 @@
 	bx	lr
 endfunc check_errata_cve_2017_5715
 
+func check_errata_cve_2022_23960
+#if WORKAROUND_CVE_2022_23960
+	mov	r0, #ERRATA_APPLIES
+#else
+	mov	r0, #ERRATA_MISSING
+#endif
+	bx	lr
+endfunc check_errata_cve_2022_23960
+
 #if REPORT_ERRATA
 /*
  * Errata printing function for Cortex A15. Must follow AAPCS.
@@ -117,6 +126,7 @@
 	report_errata ERRATA_A15_816470, cortex_a15, 816470
 	report_errata ERRATA_A15_827671, cortex_a15, 827671
 	report_errata WORKAROUND_CVE_2017_5715, cortex_a15, cve_2017_5715
+	report_errata WORKAROUND_CVE_2022_23960, cortex_a15, cve_2022_23960
 
 	pop	{r12, lr}
 	bx	lr
@@ -131,11 +141,11 @@
 	bl	errata_a15_827671_wa
 #endif
 
-#if IMAGE_BL32 && WORKAROUND_CVE_2017_5715
+#if IMAGE_BL32 && (WORKAROUND_CVE_2017_5715 || WORKAROUND_CVE_2022_23960)
 	ldcopr	r0, ACTLR
 	orr	r0, #CORTEX_A15_ACTLR_INV_BTB_BIT
 	stcopr	r0, ACTLR
-	ldr	r0, =workaround_icache_inv_runtime_exceptions
+	ldr	r0, =wa_cve_2017_5715_icache_inv_vbar
 	stcopr	r0, VBAR
 	stcopr	r0, MVBAR
 	/* isb will be applied in the course of the reset func */
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 459ca2c..47e7d8c 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -27,11 +27,14 @@
 #include <lib/extensions/sys_reg_trace.h>
 #include <lib/extensions/trbe.h>
 #include <lib/extensions/trf.h>
-#include <lib/extensions/twed.h>
 #include <lib/utils.h>
 
-static void manage_extensions_secure(cpu_context_t *ctx);
+#if ENABLE_FEAT_TWED
+/* Make sure delay value fits within the range(0-15) */
+CASSERT(((TWED_DELAY & ~SCR_TWEDEL_MASK) == 0U), assert_twed_delay_value_check);
+#endif /* ENABLE_FEAT_TWED */
 
+static void manage_extensions_secure(cpu_context_t *ctx);
 /******************************************************************************
  * This function performs initializations that are specific to SECURE state
  * and updates the cpu context specified by 'ctx'.
@@ -329,23 +332,16 @@
 	sctlr_elx |= SCTLR_IESB_BIT;
 #endif
 
+#if ENABLE_FEAT_TWED
 	/* Enable WFE trap delay in SCR_EL3 if supported and configured */
-	if (is_armv8_6_twed_present()) {
-		uint32_t delay = plat_arm_set_twedel_scr_el3();
-
-		if (delay != TWED_DISABLED) {
-			/* Make sure delay value fits */
-			assert((delay & ~SCR_TWEDEL_MASK) == 0U);
+	/* Set delay in SCR_EL3 */
+	scr_el3 &= ~(SCR_TWEDEL_MASK << SCR_TWEDEL_SHIFT);
+	scr_el3 |= ((TWED_DELAY & SCR_TWEDEL_MASK)
+			<< SCR_TWEDEL_SHIFT);
 
-			/* Set delay in SCR_EL3 */
-			scr_el3 &= ~(SCR_TWEDEL_MASK << SCR_TWEDEL_SHIFT);
-			scr_el3 |= ((delay & SCR_TWEDEL_MASK)
-					<< SCR_TWEDEL_SHIFT);
-
-			/* Enable WFE delay */
-			scr_el3 |= SCR_TWEDEn_BIT;
-		}
-	}
+	/* Enable WFE delay */
+	scr_el3 |= SCR_TWEDEn_BIT;
+#endif /* ENABLE_FEAT_TWED */
 
 	/*
 	 * Store the initialised SCTLR_EL1 value in the cpu_context - SCTLR_EL2
diff --git a/lib/fconf/fconf_dyn_cfg_getter.c b/lib/fconf/fconf_dyn_cfg_getter.c
index 34623fb..3038c09 100644
--- a/lib/fconf/fconf_dyn_cfg_getter.c
+++ b/lib/fconf/fconf_dyn_cfg_getter.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2019-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -29,13 +29,15 @@
  * This function is used to alloc memory for config information from
  * global pool and set the configuration information.
  */
-void set_config_info(uintptr_t config_addr, uint32_t config_max_size,
-			unsigned int config_id)
+void set_config_info(uintptr_t config_addr, uintptr_t ns_config_addr,
+		     uint32_t config_max_size,
+		     unsigned int config_id)
 {
 	struct dyn_cfg_dtb_info_t *dtb_info;
 
 	dtb_info = pool_alloc(&dtb_info_pool);
 	dtb_info->config_addr = config_addr;
+	dtb_info->ns_config_addr = ns_config_addr;
 	dtb_info->config_max_size = config_max_size;
 	dtb_info->config_id = config_id;
 }
@@ -88,7 +90,7 @@
 	 */
 	if (dtb_infos[0].config_id == 0U) {
 		uint32_t config_max_size = fdt_totalsize(dtb);
-		set_config_info(config, config_max_size, FW_CONFIG_ID);
+		set_config_info(config, ~0UL, config_max_size, FW_CONFIG_ID);
 	}
 
 	/* Find the node offset point to "fconf,dyn_cfg-dtb_registry" compatible property */
@@ -102,6 +104,7 @@
 	fdt_for_each_subnode(child, dtb, node) {
 		uint32_t config_max_size, config_id;
 		uintptr_t config_addr;
+		uintptr_t ns_config_addr = ~0UL;
 		uint64_t val64;
 
 		/* Read configuration dtb information */
@@ -129,7 +132,14 @@
 		VERBOSE("\tmax-size = 0x%x\n", config_max_size);
 		VERBOSE("\tconfig-id = %u\n", config_id);
 
-		set_config_info(config_addr, config_max_size, config_id);
+		rc = fdt_read_uint64(dtb, child, "ns-load-address", &val64);
+		if (rc == 0) {
+			ns_config_addr = (uintptr_t)val64;
+			VERBOSE("\tns-load-address = %lx\n", ns_config_addr);
+		}
+
+		set_config_info(config_addr, ns_config_addr, config_max_size,
+				config_id);
 	}
 
 	if ((child < 0) && (child != -FDT_ERR_NOTFOUND)) {
diff --git a/make_helpers/build_macros.mk b/make_helpers/build_macros.mk
index 12aaee6..a58caf5 100644
--- a/make_helpers/build_macros.mk
+++ b/make_helpers/build_macros.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -489,6 +489,10 @@
 $(ELF): romlib.bin
 endif
 
+# MODULE_OBJS can be assigned by vendors with different compiled
+# object file path, and prebuilt object file path.
+$(eval OBJS += $(MODULE_OBJS))
+
 $(ELF): $(OBJS) $(LINKERFILE) | $(1)_dirs libraries $(BL_LIBS)
 	$$(ECHO) "  LD      $$@"
 ifdef MAKE_BUILD_STRINGS
@@ -507,7 +511,7 @@
 		$(BUILD_DIR)/build_message.o $(OBJS)
 else ifneq ($(findstring gcc,$(notdir $(LD))),)
 	$$(Q)$$(LD) -o $$@ $$(TF_LDFLAGS) $$(LDFLAGS) -Wl,-Map=$(MAPFILE) \
-		-Wl,-T$(LINKERFILE) $(BUILD_DIR)/build_message.o \
+		-Wl,-dT $(LINKERFILE) $(EXTRA_LINKERFILE) $(BUILD_DIR)/build_message.o \
 		$(OBJS) $(LDPATHS) $(LIBWRAPPER) $(LDLIBS) $(BL_LIBS)
 else
 	$$(Q)$$(LD) -o $$@ $$(TF_LDFLAGS) $$(LDFLAGS) $(BL_LDFLAGS) -Map=$(MAPFILE) \
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index 7b66569..7f92640 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -169,6 +169,9 @@
 # Flag to enable Virtualization Host Extensions
 ENABLE_FEAT_VHE 		:= 0
 
+# Flag to enable delayed trapping of WFE instruction (FEAT_TWED)
+ENABLE_FEAT_TWED		:= 0
+
 # By default BL31 encryption disabled
 ENCRYPT_BL31			:= 0
 
@@ -254,6 +257,9 @@
 # By default, BL1 acts as the reset handler, not BL31
 RESET_TO_BL31			:= 0
 
+# By default, clear the input registers when RESET_TO_BL31 is enabled
+RESET_TO_BL31_WITH_PARAMS	:= 0
+
 # For Chain of Trust
 SAVE_KEYS			:= 0
 
@@ -443,3 +449,9 @@
 # lower ELs, i.e. NS-EL2, or NS-EL1 if NS-EL2 implemented but unused
 # if FEAT_TRF is implemented.
 ENABLE_TRF_FOR_NS		:= 0
+
+# In v8.6+ platforms with delayed trapping of WFE being supported
+# via FEAT_TWED, this flag takes the delay value to be set in the
+# SCR_EL3.TWEDEL(4bit) field, when FEAT_TWED is implemented.
+# By default it takes 0, and need to be updated by the platforms.
+TWED_DELAY			:= 0
diff --git a/plat/arm/board/fvp/fdts/fvp_fw_config.dts b/plat/arm/board/fvp/fdts/fvp_fw_config.dts
index c26b519..577ac74 100644
--- a/plat/arm/board/fvp/fdts/fvp_fw_config.dts
+++ b/plat/arm/board/fvp/fdts/fvp_fw_config.dts
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, ARM Limited. All rights reserved.
+ * Copyright (c) 2019-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,9 +19,10 @@
 		};
 
 		hw-config {
-			load-address = <0x0 0x82000000>;
-			max-size = <0x01000000>;
+			load-address = <0x0 0x07f00000>;
+			max-size = <0x00100000>;
 			id = <HW_CONFIG_ID>;
+			ns-load-address = <0x0 0x82000000>;
 		};
 
 		/*
diff --git a/plat/arm/board/fvp/fvp_bl2_setup.c b/plat/arm/board/fvp/fvp_bl2_setup.c
index 5a17a0d..4eee522 100644
--- a/plat/arm/board/fvp/fvp_bl2_setup.c
+++ b/plat/arm/board/fvp/fvp_bl2_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2021, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -40,17 +40,23 @@
 struct bl_params *plat_get_next_bl_params(void)
 {
 	struct bl_params *arm_bl_params;
+	const struct dyn_cfg_dtb_info_t *hw_config_info __unused;
+	bl_mem_params_node_t *param_node __unused;
 
 	arm_bl_params = arm_get_next_bl_params();
 
-#if __aarch64__ && !BL2_AT_EL3
+#if !BL2_AT_EL3 && !EL3_PAYLOAD_BASE
 	const struct dyn_cfg_dtb_info_t *fw_config_info;
-	bl_mem_params_node_t *param_node;
-	uintptr_t fw_config_base = 0U;
+	uintptr_t fw_config_base = 0UL;
 	entry_point_info_t *ep_info;
 
+#if __aarch64__
 	/* Get BL31 image node */
 	param_node = get_bl_mem_params_node(BL31_IMAGE_ID);
+#else /* aarch32 */
+	/* Get SP_MIN image node */
+	param_node = get_bl_mem_params_node(BL32_IMAGE_ID);
+#endif /* __aarch64__ */
 	assert(param_node != NULL);
 
 	/* get fw_config load address */
@@ -58,15 +64,41 @@
 	assert(fw_config_info != NULL);
 
 	fw_config_base = fw_config_info->config_addr;
-	assert(fw_config_base != 0U);
+	assert(fw_config_base != 0UL);
 
 	/*
-	 * Get the entry point info of BL31 image and override
+	 * Get the entry point info of next executable image and override
 	 * arg1 of entry point info with fw_config base address
 	 */
 	ep_info = &param_node->ep_info;
 	ep_info->args.arg1 = (uint32_t)fw_config_base;
-#endif /* __aarch64__ && !BL2_AT_EL3 */
+
+	/* grab NS HW config address */
+	hw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, HW_CONFIG_ID);
+
+	/* To retrieve actual size of the HW_CONFIG */
+	param_node = get_bl_mem_params_node(HW_CONFIG_ID);
+	assert(param_node != NULL);
+
+	/* Copy HW config from Secure address to NS address */
+	memcpy((void *)hw_config_info->ns_config_addr,
+	       (void *)hw_config_info->config_addr,
+	       (size_t)param_node->image_info.image_size);
+
+	/*
+	 * Ensure HW-config device tree committed to memory, as there is
+	 * a possibility to use HW-config without cache and MMU enabled
+	 * at BL33
+	 */
+	flush_dcache_range(hw_config_info->ns_config_addr,
+			   param_node->image_info.image_size);
+
+	param_node = get_bl_mem_params_node(BL33_IMAGE_ID);
+	assert(param_node != NULL);
+
+	/* Update BL33's ep info with NS HW config address  */
+	param_node->ep_info.args.arg1 = hw_config_info->ns_config_addr;
+#endif /* !BL2_AT_EL3 && !EL3_PAYLOAD_BASE */
 
 	return arm_bl_params;
 }
diff --git a/plat/arm/board/fvp/fvp_bl31_setup.c b/plat/arm/board/fvp/fvp_bl31_setup.c
index a94a4f4..dd90965 100644
--- a/plat/arm/board/fvp/fvp_bl31_setup.c
+++ b/plat/arm/board/fvp/fvp_bl31_setup.c
@@ -17,6 +17,8 @@
 
 #include "fvp_private.h"
 
+static const struct dyn_cfg_dtb_info_t *hw_config_info __unused;
+
 void __init bl31_early_platform_setup2(u_register_t arg0,
 		u_register_t arg1, u_register_t arg2, u_register_t arg3)
 {
@@ -34,6 +36,17 @@
 	if (soc_fw_config_info != NULL) {
 		arg1 = soc_fw_config_info->config_addr;
 	}
+
+	/*
+	 * arg2 is currently holding the 'secure' address of HW_CONFIG.
+	 * But arm_bl31_early_platform_setup() below expects the 'non-secure'
+	 * address of HW_CONFIG (which it will pass to BL33).
+	 * This why we need to override arg2 here.
+	 */
+	hw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, HW_CONFIG_ID);
+	assert(hw_config_info != NULL);
+	assert(hw_config_info->ns_config_addr != 0UL);
+	arg2 = hw_config_info->ns_config_addr;
 #endif /* !RESET_TO_BL31 && !BL2_AT_EL3 */
 
 	arm_bl31_early_platform_setup((void *)arg0, arg1, arg2, (void *)arg3);
@@ -66,22 +79,57 @@
 
 void __init bl31_plat_arch_setup(void)
 {
+	int rc __unused;
+	uintptr_t hw_config_base_align __unused;
+	size_t mapped_size_align __unused;
+
 	arm_bl31_plat_arch_setup();
 
 	/*
 	 * For RESET_TO_BL31 systems, BL31 is the first bootloader to run.
 	 * So there is no BL2 to load the HW_CONFIG dtb into memory before
-	 * control is passed to BL31.
+	 * control is passed to BL31. The code below relies on dynamic mapping
+	 * capability, which is not supported by xlat tables lib V1.
+	 * TODO: remove the ARM_XLAT_TABLES_LIB_V1 check when its support
+	 * gets deprecated.
 	 */
-#if !RESET_TO_BL31 && !BL2_AT_EL3
-	/* HW_CONFIG was also loaded by BL2 */
-	const struct dyn_cfg_dtb_info_t *hw_config_info;
-
-	hw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, HW_CONFIG_ID);
+#if !RESET_TO_BL31 && !BL2_AT_EL3 && !ARM_XLAT_TABLES_LIB_V1
 	assert(hw_config_info != NULL);
+	assert(hw_config_info->config_addr != 0UL);
 
+	/* Page aligned address and size if necessary */
+	hw_config_base_align = page_align(hw_config_info->config_addr, DOWN);
+	mapped_size_align = page_align(hw_config_info->config_max_size, UP);
+
+	if ((hw_config_info->config_addr != hw_config_base_align) &&
+	    (hw_config_info->config_max_size == mapped_size_align)) {
+		mapped_size_align += PAGE_SIZE;
+	}
+
+	/*
+	 * map dynamically HW config region with its aligned base address and
+	 * size
+	 */
+	rc = mmap_add_dynamic_region((unsigned long long)hw_config_base_align,
+				     hw_config_base_align,
+				     mapped_size_align,
+				     MT_RO_DATA);
+	if (rc != 0) {
+		ERROR("Error while mapping HW_CONFIG device tree (%d).\n", rc);
+		panic();
+	}
+
+	/* Populate HW_CONFIG device tree with the mapped address */
 	fconf_populate("HW_CONFIG", hw_config_info->config_addr);
-#endif
+
+	/* unmap the HW_CONFIG memory region */
+	rc = mmap_remove_dynamic_region(hw_config_base_align, mapped_size_align);
+	if (rc != 0) {
+		ERROR("Error while unmapping HW_CONFIG device tree (%d).\n",
+		      rc);
+		panic();
+	}
+#endif /* !RESET_TO_BL31 && !BL2_AT_EL3 && !ARM_XLAT_TABLES_LIB_V1 */
 }
 
 unsigned int plat_get_syscnt_freq2(void)
diff --git a/plat/arm/board/fvp/fvp_common.c b/plat/arm/board/fvp/fvp_common.c
index d8d19de..e9f725c 100644
--- a/plat/arm/board/fvp/fvp_common.c
+++ b/plat/arm/board/fvp/fvp_common.c
@@ -104,9 +104,10 @@
 #ifdef __aarch64__
 	ARM_MAP_DRAM2,
 #endif
-#if defined(SPD_spmd)
+	/*
+	 * Required to load HW_CONFIG, SPMC and SPs to trusted DRAM.
+	 */
 	ARM_MAP_TRUSTED_DRAM,
-#endif
 #if ENABLE_RME
 	ARM_MAP_RMM_DRAM,
 	ARM_MAP_GPT_L1_DRAM,
@@ -166,8 +167,6 @@
 #if SPM_MM
 	ARM_SPM_BUF_EL3_MMAP,
 #endif
-	/* Required by fconf APIs to read HW_CONFIG dtb loaded into DRAM */
-	ARM_DTB_DRAM_NS,
 #if ENABLE_RME
 	ARM_MAP_GPT_L1_DRAM,
 #endif
@@ -197,8 +196,6 @@
 	V2M_MAP_IOFPGA,
 	MAP_DEVICE0,
 	MAP_DEVICE1,
-	/* Required by fconf APIs to read HW_CONFIG dtb loaded into DRAM */
-	ARM_DTB_DRAM_NS,
 	{0}
 };
 #endif
diff --git a/plat/arm/board/fvp/include/platform_def.h b/plat/arm/board/fvp/include/platform_def.h
index 5e5ddce..e701144 100644
--- a/plat/arm/board/fvp/include/platform_def.h
+++ b/plat/arm/board/fvp/include/platform_def.h
@@ -86,10 +86,6 @@
 #define FVP_DTB_DRAM_MAP_START		ULL(0x82000000)
 #define FVP_DTB_DRAM_MAP_SIZE		ULL(0x02000000)	/* 32 MB */
 
-#define ARM_DTB_DRAM_NS			MAP_REGION_FLAT(		\
-					FVP_DTB_DRAM_MAP_START,		\
-					FVP_DTB_DRAM_MAP_SIZE,		\
-					MT_MEMORY | MT_RO | MT_NS)
 /*
  * Load address of BL33 for this platform port
  */
@@ -261,6 +257,7 @@
 #define PLAT_ARM_TRP_UART_CLK_IN_HZ	V2M_IOFPGA_UART3_CLK_IN_HZ
 
 #define PLAT_FVP_SMMUV3_BASE		UL(0x2b400000)
+#define PLAT_ARM_SMMUV3_ROOT_REG_OFFSET UL(0x20000)
 
 /* CCI related constants */
 #define PLAT_FVP_CCI400_BASE		UL(0x2c090000)
diff --git a/plat/arm/board/fvp/jmptbl.i b/plat/arm/board/fvp/jmptbl.i
index b72bdab..85e6e3a 100644
--- a/plat/arm/board/fvp/jmptbl.i
+++ b/plat/arm/board/fvp/jmptbl.i
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -35,6 +35,8 @@
 fdt     fdt_get_name
 fdt     fdt_get_alias
 fdt     fdt_node_offset_by_phandle
+fdt     fdt_subnode_offset
+fdt     fdt_add_subnode
 mbedtls mbedtls_asn1_get_alg
 mbedtls mbedtls_asn1_get_alg_null
 mbedtls mbedtls_asn1_get_bitstring_null
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index c9f5551..19f913e 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -333,15 +333,10 @@
 endif
 
 # Enable the dynamic translation tables library.
-ifeq (${ARCH},aarch32)
-    ifeq (${RESET_TO_SP_MIN},1)
+ifeq ($(filter 1,${BL2_AT_EL3} ${ARM_XLAT_TABLES_LIB_V1}),)
+    ifeq (${ARCH},aarch32)
         BL32_CPPFLAGS	+=	-DPLAT_XLAT_TABLES_DYNAMIC
-    endif
-else # AArch64
-    ifeq (${RESET_TO_BL31},1)
-        BL31_CPPFLAGS	+=	-DPLAT_XLAT_TABLES_DYNAMIC
-    endif
-    ifeq (${SPD},trusty)
+    else # AArch64
         BL31_CPPFLAGS	+=	-DPLAT_XLAT_TABLES_DYNAMIC
     endif
 endif
diff --git a/plat/arm/board/fvp/sp_min/fvp_sp_min_setup.c b/plat/arm/board/fvp/sp_min/fvp_sp_min_setup.c
index 763b42a..9ab36a6 100644
--- a/plat/arm/board/fvp/sp_min/fvp_sp_min_setup.c
+++ b/plat/arm/board/fvp/sp_min/fvp_sp_min_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,15 +9,31 @@
 #include <bl32/sp_min/platform_sp_min.h>
 #include <common/debug.h>
 #include <lib/fconf/fconf.h>
+#include <lib/fconf/fconf_dyn_cfg_getter.h>
 #include <plat/arm/common/plat_arm.h>
 
 #include "../fvp_private.h"
 
-uintptr_t hw_config_dtb;
-
 void plat_arm_sp_min_early_platform_setup(u_register_t arg0, u_register_t arg1,
 			u_register_t arg2, u_register_t arg3)
 {
+	const struct dyn_cfg_dtb_info_t *tos_fw_config_info __unused;
+
+	/* Initialize the console to provide early debug support */
+	arm_console_boot_init();
+
+#if !RESET_TO_SP_MIN && !BL2_AT_EL3
+
+	INFO("SP_MIN FCONF: FW_CONFIG address = %lx\n", (uintptr_t)arg1);
+	/* Fill the properties struct with the info from the config dtb */
+	fconf_populate("FW_CONFIG", arg1);
+
+	tos_fw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, TOS_FW_CONFIG_ID);
+	if (tos_fw_config_info != NULL) {
+		arg1 = tos_fw_config_info->config_addr;
+	}
+#endif /* !RESET_TO_SP_MIN && !BL2_AT_EL3 */
+
 	arm_sp_min_early_platform_setup((void *)arg0, arg1, arg2, (void *)arg3);
 
 	/* Initialize the platform config for future decision making */
@@ -37,12 +53,15 @@
 	 * FVP PSCI code will enable coherency for other clusters.
 	 */
 	fvp_interconnect_enable();
-
-	hw_config_dtb = arg2;
 }
 
 void sp_min_plat_arch_setup(void)
 {
+	int rc __unused;
+	const struct dyn_cfg_dtb_info_t *hw_config_info __unused;
+	uintptr_t hw_config_base_align __unused;
+	size_t mapped_size_align __unused;
+
 	arm_sp_min_plat_arch_setup();
 
 	/*
@@ -50,11 +69,53 @@
 	 * to run. So there is no BL2 to load the HW_CONFIG dtb into memory
 	 * before control is passed to SP_MIN.
 	 * Also, BL2 skips loading HW_CONFIG dtb for BL2_AT_EL3 builds.
+	 * The code below relies on dynamic mapping capability, which is not
+	 * supported by xlat tables lib V1.
+	 * TODO: remove the ARM_XLAT_TABLES_LIB_V1 check when its support
+	 * gets deprecated.
 	 */
-#if !RESET_TO_SP_MIN && !BL2_AT_EL3
-	assert(hw_config_dtb != 0U);
+#if !RESET_TO_SP_MIN && !BL2_AT_EL3 && !ARM_XLAT_TABLES_LIB_V1
+	hw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, HW_CONFIG_ID);
+	assert(hw_config_info != NULL);
+	assert(hw_config_info->config_addr != 0UL);
+
+	INFO("SP_MIN FCONF: HW_CONFIG address = %p\n",
+	     (void *)hw_config_info->config_addr);
+
+	/*
+	 * Preferrably we expect this address and size are page aligned,
+	 * but if they are not then align it.
+	 */
+	hw_config_base_align = page_align(hw_config_info->config_addr, DOWN);
+	mapped_size_align = page_align(hw_config_info->config_max_size, UP);
+
+	if ((hw_config_info->config_addr != hw_config_base_align) &&
+	    (hw_config_info->config_max_size == mapped_size_align)) {
+		mapped_size_align += PAGE_SIZE;
+	}
+
+	/*
+	 * map dynamically HW config region with its aligned base address and
+	 * size
+	 */
+	rc = mmap_add_dynamic_region((unsigned long long)hw_config_base_align,
+				     hw_config_base_align,
+				     mapped_size_align,
+				     MT_RO_DATA);
+	if (rc != 0) {
+		ERROR("Error while mapping HW_CONFIG device tree (%d).\n", rc);
+		panic();
+	}
+
+	/* Populate HW_CONFIG device tree with the mapped address */
+	fconf_populate("HW_CONFIG", hw_config_info->config_addr);
 
-	INFO("SP_MIN FCONF: HW_CONFIG address = %p\n", (void *)hw_config_dtb);
-	fconf_populate("HW_CONFIG", hw_config_dtb);
-#endif
+	/* unmap the HW_CONFIG memory region */
+	rc = mmap_remove_dynamic_region(hw_config_base_align, mapped_size_align);
+	if (rc != 0) {
+		ERROR("Error while unmapping HW_CONFIG device tree (%d).\n",
+		      rc);
+		panic();
+	}
+#endif /* !RESET_TO_SP_MIN && !BL2_AT_EL3 && !ARM_XLAT_TABLES_LIB_V1 */
 }
diff --git a/plat/arm/board/fvp/sp_min/sp_min-fvp.mk b/plat/arm/board/fvp/sp_min/sp_min-fvp.mk
index 0d8cca5..183d802 100644
--- a/plat/arm/board/fvp/sp_min/sp_min-fvp.mk
+++ b/plat/arm/board/fvp/sp_min/sp_min-fvp.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2016-2022, Arm Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -25,7 +25,8 @@
 # Added separately from the above list for better readability
 ifeq ($(filter 1,${BL2_AT_EL3} ${RESET_TO_SP_MIN}),)
 BL32_SOURCES		+=	lib/fconf/fconf.c				\
-				plat/arm/board/fvp/fconf/fconf_hw_config_getter.c
+				lib/fconf/fconf_dyn_cfg_getter.c		\
+				plat/arm/board/fvp/fconf/fconf_hw_config_getter.c \
 
 BL32_SOURCES		+=	${FDT_WRAPPERS_SOURCES}
 
diff --git a/plat/arm/board/fvp_r/fvp_r_bl1_setup.c b/plat/arm/board/fvp_r/fvp_r_bl1_setup.c
index 68872c1..1ac0a9c 100644
--- a/plat/arm/board/fvp_r/fvp_r_bl1_setup.c
+++ b/plat/arm/board/fvp_r/fvp_r_bl1_setup.c
@@ -154,7 +154,8 @@
 
 	/* Set global DTB info for fixed fw_config information */
 	fw_config_max_size = ARM_FW_CONFIG_LIMIT - ARM_FW_CONFIG_BASE;
-	set_config_info(ARM_FW_CONFIG_BASE, fw_config_max_size, FW_CONFIG_ID);
+	set_config_info(ARM_FW_CONFIG_BASE, ~0UL, fw_config_max_size,
+			FW_CONFIG_ID);
 
 	assert(bl1_plat_get_image_desc(BL33_IMAGE_ID) != NULL);
 
diff --git a/plat/arm/board/juno/jmptbl.i b/plat/arm/board/juno/jmptbl.i
index 393a648..8932aa0 100644
--- a/plat/arm/board/juno/jmptbl.i
+++ b/plat/arm/board/juno/jmptbl.i
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -34,6 +34,8 @@
 fdt     fdt_get_name
 fdt     fdt_get_alias
 fdt     fdt_node_offset_by_phandle
+fdt     fdt_subnode_offset
+fdt     fdt_add_subnode
 mbedtls mbedtls_asn1_get_alg
 mbedtls mbedtls_asn1_get_alg_null
 mbedtls mbedtls_asn1_get_bitstring_null
diff --git a/plat/arm/common/arm_bl1_setup.c b/plat/arm/common/arm_bl1_setup.c
index 73338cb..7a9e04d 100644
--- a/plat/arm/common/arm_bl1_setup.c
+++ b/plat/arm/common/arm_bl1_setup.c
@@ -166,7 +166,7 @@
 
 	/* Set global DTB info for fixed fw_config information */
 	fw_config_max_size = ARM_FW_CONFIG_LIMIT - ARM_FW_CONFIG_BASE;
-	set_config_info(ARM_FW_CONFIG_BASE, fw_config_max_size, FW_CONFIG_ID);
+	set_config_info(ARM_FW_CONFIG_BASE, ~0UL, fw_config_max_size, FW_CONFIG_ID);
 
 	/* Fill the device tree information struct with the info from the config dtb */
 	err = fconf_load_config(FW_CONFIG_ID);
diff --git a/plat/common/aarch64/plat_common.c b/plat/common/aarch64/plat_common.c
index 38a5786..e807660 100644
--- a/plat/common/aarch64/plat_common.c
+++ b/plat/common/aarch64/plat_common.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -13,7 +13,6 @@
 #if RAS_EXTENSION
 #include <lib/extensions/ras.h>
 #endif
-#include <lib/extensions/twed.h>
 #include <lib/xlat_tables/xlat_mmu_helpers.h>
 #include <plat/common/platform.h>
 
@@ -23,7 +22,6 @@
  * platforms but may also be overridden by a platform if required.
  */
 #pragma weak bl31_plat_runtime_setup
-#pragma weak plat_arm_set_twedel_scr_el3
 
 #if SDEI_SUPPORT
 #pragma weak plat_sdei_handle_masked_trigger
@@ -105,16 +103,3 @@
 #endif
 	panic();
 }
-
-/*******************************************************************************
- * In v8.6+ platforms with delayed trapping of WFE this hook sets the delay. It
- * is a weak function definition so can be overridden depending on the
- * requirements of a platform.  The only hook provided is for the TWED fields
- * in SCR_EL3, the TWED fields in HCR_EL2, SCTLR_EL2, and SCTLR_EL1 should be
- * configured as needed in lower exception levels.
- ******************************************************************************/
-
-uint32_t plat_arm_set_twedel_scr_el3(void)
-{
-	return TWED_DISABLED;
-}
diff --git a/plat/intel/soc/agilex/bl31_plat_setup.c b/plat/intel/soc/agilex/bl31_plat_setup.c
index 66d6b8f..b1b9514 100644
--- a/plat/intel/soc/agilex/bl31_plat_setup.c
+++ b/plat/intel/soc/agilex/bl31_plat_setup.c
@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2019-2020, ARM Limited and Contributors. All rights reserved.
- * Copyright (c) 2019-2020, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,6 +14,7 @@
 #include <lib/mmio.h>
 #include <lib/xlat_tables/xlat_tables.h>
 
+#include "ccu/ncore_ccu.h"
 #include "socfpga_mailbox.h"
 #include "socfpga_private.h"
 
@@ -114,6 +115,8 @@
 		(uint64_t)plat_secondary_cpus_bl31_entry);
 
 	mailbox_hps_stage_notify(HPS_EXECUTION_STATE_SSBL);
+
+	ncore_enable_ocram_firewall();
 }
 
 const mmap_region_t plat_agilex_mmap[] = {
diff --git a/plat/intel/soc/agilex/include/agilex_noc.h b/plat/intel/soc/agilex/include/agilex_noc.h
index 22db3e2..9aba3c3 100644
--- a/plat/intel/soc/agilex/include/agilex_noc.h
+++ b/plat/intel/soc/agilex/include/agilex_noc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/plat/intel/soc/agilex/include/socfpga_plat_def.h b/plat/intel/soc/agilex/include/socfpga_plat_def.h
index 9c87e45..6a5cf9b 100644
--- a/plat/intel/soc/agilex/include/socfpga_plat_def.h
+++ b/plat/intel/soc/agilex/include/socfpga_plat_def.h
@@ -19,6 +19,8 @@
 #define INTEL_SIP_SMC_FPGA_CONFIG_SIZE		0x2000000
 
 /* Register Mapping */
+#define SOCFPGA_CCU_NOC_REG_BASE		0xf7000000
+
 #define SOCFPGA_MMC_REG_BASE			0xff808000
 
 #define SOCFPGA_RSTMGR_REG_BASE			0xffd11000
diff --git a/plat/intel/soc/agilex/platform.mk b/plat/intel/soc/agilex/platform.mk
index 10a3eec..89df46a 100644
--- a/plat/intel/soc/agilex/platform.mk
+++ b/plat/intel/soc/agilex/platform.mk
@@ -26,6 +26,7 @@
 			lib/xlat_tables/xlat_tables_common.c 		\
 			plat/intel/soc/common/aarch64/platform_common.c \
 			plat/intel/soc/common/aarch64/plat_helpers.S	\
+			plat/intel/soc/common/drivers/ccu/ncore_ccu.c	\
 			plat/intel/soc/common/socfpga_delay_timer.c
 
 BL2_SOURCES     +=	\
@@ -48,13 +49,12 @@
 		plat/intel/soc/common/socfpga_image_load.c		\
 		plat/intel/soc/common/socfpga_storage.c			\
 		plat/intel/soc/common/soc/socfpga_emac.c		\
+		plat/intel/soc/common/soc/socfpga_firewall.c	\
 		plat/intel/soc/common/soc/socfpga_handoff.c		\
 		plat/intel/soc/common/soc/socfpga_mailbox.c		\
 		plat/intel/soc/common/soc/socfpga_reset_manager.c	\
-		plat/intel/soc/common/soc/socfpga_system_manager.c	\
 		plat/intel/soc/common/drivers/qspi/cadence_qspi.c	\
-		plat/intel/soc/common/drivers/wdt/watchdog.c		\
-		plat/intel/soc/common/drivers/ccu/ncore_ccu.c
+		plat/intel/soc/common/drivers/wdt/watchdog.c
 
 BL31_SOURCES	+=	\
 		drivers/arm/cci/cci.c					\
diff --git a/plat/intel/soc/common/drivers/ccu/ncore_ccu.c b/plat/intel/soc/common/drivers/ccu/ncore_ccu.c
index d4716cf..d9a238e 100644
--- a/plat/intel/soc/common/drivers/ccu/ncore_ccu.c
+++ b/plat/intel/soc/common/drivers/ccu/ncore_ccu.c
@@ -118,6 +118,7 @@
 	mmio_setbits_32(COH_CPU0_BYPASS_REG(NCORE_FW_OCRAM_BLK_CGF4),
 			OCRAM_PRIVILEGED_MASK | OCRAM_SECURE_MASK);
 }
+
 uint32_t init_ncore_ccu(void)
 {
 	uint32_t status;
diff --git a/plat/intel/soc/common/include/socfpga_mailbox.h b/plat/intel/soc/common/include/socfpga_mailbox.h
index a6a3565..b260a62 100644
--- a/plat/intel/soc/common/include/socfpga_mailbox.h
+++ b/plat/intel/soc/common/include/socfpga_mailbox.h
@@ -42,6 +42,7 @@
 #define MBOX_CMD_CANCEL			0x03
 #define MBOX_CMD_VAB_SRC_CERT		0x0B
 #define MBOX_CMD_GET_IDCODE		0x10
+#define MBOX_CMD_GET_USERCODE		0x13
 #define MBOX_CMD_REBOOT_HPS		0x47
 
 /* Reconfiguration Commands */
@@ -50,6 +51,11 @@
 #define MBOX_RECONFIG_DATA		0x08
 #define MBOX_RECONFIG_STATUS		0x09
 
+/* HWMON Commands */
+#define MBOX_HWMON_READVOLT		0x18
+#define MBOX_HWMON_READTEMP		0x19
+
+
 /* QSPI Commands */
 #define MBOX_CMD_QSPI_OPEN		0x32
 #define MBOX_CMD_QSPI_CLOSE		0x33
@@ -145,6 +151,10 @@
 #define RSU_VERSION_ACMF		BIT(8)
 #define RSU_VERSION_ACMF_MASK		0xff00
 
+/* Config Status Macros */
+#define CONFIG_STATUS_WORD_SIZE		16U
+#define CONFIG_STATUS_FW_VER_OFFSET	1
+#define CONFIG_STATUS_FW_VER_MASK	0x00FFFFFF
 
 /* Mailbox Function Definitions */
 
@@ -173,5 +183,7 @@
 int mailbox_rsu_status(uint32_t *resp_buf, uint32_t resp_buf_len);
 int mailbox_rsu_update(uint32_t *flash_offset);
 int mailbox_hps_stage_notify(uint32_t execution_stage);
+int mailbox_hwmon_readtemp(uint32_t chan, uint32_t *resp_buf);
+int mailbox_hwmon_readvolt(uint32_t chan, uint32_t *resp_buf);
 
 #endif /* SOCFPGA_MBOX_H */
diff --git a/plat/intel/soc/common/include/socfpga_noc.h b/plat/intel/soc/common/include/socfpga_noc.h
new file mode 100644
index 0000000..e3c0f73
--- /dev/null
+++ b/plat/intel/soc/common/include/socfpga_noc.h
@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2020-2022, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SOCFPGA_NOC_H
+#define SOCFPGA_NOC_H
+
+/* Macros */
+#define SCR_AXI_AP_MASK					BIT(24)
+#define SCR_FPGA2SOC_MASK				BIT(16)
+#define SCR_MPU_MASK					BIT(0)
+#define DISABLE_L4_FIREWALL		(SCR_AXI_AP_MASK | SCR_FPGA2SOC_MASK \
+						| SCR_MPU_MASK)
+#define DISABLE_BRIDGE_FIREWALL				0x0ffe0101
+
+#define SOCFPGA_CCU_NOC(_ctrl, _dev)	(SOCFPGA_CCU_NOC_REG_BASE \
+					+ (SOCFPGA_CCU_NOC_##_ctrl##_##_dev))
+
+#define SOCFPGA_L4_PER_SCR(_reg)	(SOCFPGA_L4_PER_SCR_REG_BASE \
+					+ (SOCFPGA_NOC_FW_L4_PER_SCR_##_reg))
+
+#define SOCFPGA_L4_SYS_SCR(_reg)	(SOCFPGA_L4_SYS_SCR_REG_BASE \
+					+ (SOCFPGA_NOC_FW_L4_SYS_SCR_##_reg))
+
+/* L3 Interconnect Register Map */
+#define SOCFPGA_NOC_FW_L4_PER_SCR_NAND_REGISTER			0x0000
+#define SOCFPGA_NOC_FW_L4_PER_SCR_NAND_DATA			0x0004
+#define SOCFPGA_NOC_FW_L4_PER_SCR_USB0_REGISTER			0x000c
+#define SOCFPGA_NOC_FW_L4_PER_SCR_USB1_REGISTER			0x0010
+#define SOCFPGA_NOC_FW_L4_PER_SCR_SPI_MASTER0			0x001c
+#define SOCFPGA_NOC_FW_L4_PER_SCR_SPI_MASTER1			0x0020
+#define SOCFPGA_NOC_FW_L4_PER_SCR_SPI_SLAVE0			0x0024
+#define SOCFPGA_NOC_FW_L4_PER_SCR_SPI_SLAVE1			0x0028
+#define SOCFPGA_NOC_FW_L4_PER_SCR_EMAC0				0x002c
+#define SOCFPGA_NOC_FW_L4_PER_SCR_EMAC1				0x0030
+#define SOCFPGA_NOC_FW_L4_PER_SCR_EMAC2				0x0034
+#define SOCFPGA_NOC_FW_L4_PER_SCR_SDMMC				0x0040
+#define SOCFPGA_NOC_FW_L4_PER_SCR_GPIO0				0x0044
+#define SOCFPGA_NOC_FW_L4_PER_SCR_GPIO1				0x0048
+#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C0				0x0050
+#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C1				0x0054
+#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C2				0x0058
+#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C3				0x005c
+#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C4				0x0060
+#define SOCFPGA_NOC_FW_L4_PER_SCR_SP_TIMER0			0x0064
+#define SOCFPGA_NOC_FW_L4_PER_SCR_SP_TIMER1			0x0068
+#define SOCFPGA_NOC_FW_L4_PER_SCR_UART0				0x006c
+#define SOCFPGA_NOC_FW_L4_PER_SCR_UART1				0x0070
+
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_DMA_ECC			0x0008
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC0RX_ECC			0x000c
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC0TX_ECC			0x0010
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC1RX_ECC			0x0014
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC1TX_ECC			0x0018
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC2RX_ECC			0x001c
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC2TX_ECC			0x0020
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_NAND_ECC			0x002c
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_NAND_READ_ECC			0x0030
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_NAND_WRITE_ECC		0x0034
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_OCRAM_ECC			0x0038
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_SDMMC_ECC			0x0040
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_USB0_ECC			0x0044
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_USB1_ECC			0x0048
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_CLK_MGR			0x004c
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_IO_MGR			0x0054
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_RST_MGR			0x0058
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_SYS_MGR			0x005c
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_OSC0_TIMER			0x0060
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_OSC1_TIMER			0x0064
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_WATCHDOG0			0x0068
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_WATCHDOG1			0x006c
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_WATCHDOG2			0x0070
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_WATCHDOG3			0x0074
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_DAP				0x0078
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_L4_NOC_PROBES			0x0090
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_L4_NOC_QOS			0x0094
+
+/* CCU NOC Register Map */
+
+#define SOCFPGA_CCU_NOC_CPU0_RAM0				0x04688
+#define SOCFPGA_CCU_NOC_IOM_RAM0				0x18628
+
+#define SOCFPGA_CCU_NOC_ADMASK_P_MASK				BIT(0)
+#define SOCFPGA_CCU_NOC_ADMASK_NS_MASK				BIT(1)
+
+/* Function Definitions */
+
+void enable_ns_peripheral_access(void);
+void enable_ns_bridge_access(void);
+void enable_ns_ocram_access(void);
+void enable_ocram_firewall(void);
+
+#endif
diff --git a/plat/intel/soc/common/include/socfpga_sip_svc.h b/plat/intel/soc/common/include/socfpga_sip_svc.h
index 4d31c77..43f3dc4 100644
--- a/plat/intel/soc/common/include/socfpga_sip_svc.h
+++ b/plat/intel/soc/common/include/socfpga_sip_svc.h
@@ -27,6 +27,12 @@
 #define INTEL_SIP_SMC_FPGA_CONFIG_ISDONE		0xC2000004
 #define INTEL_SIP_SMC_FPGA_CONFIG_GET_MEM		0xC2000005
 
+/* FPGA Bitstream Flag */
+#define FLAG_PARTIAL_CONFIG				BIT(0)
+#define FLAG_AUTHENTICATION				BIT(1)
+#define CONFIG_TEST_FLAG(_flag, _type)			(((flag) & FLAG_##_type) \
+							== FLAG_##_type)
+
 /* Secure Register Access */
 #define INTEL_SIP_SMC_REG_READ				0xC2000007
 #define INTEL_SIP_SMC_REG_WRITE				0xC2000008
@@ -39,7 +45,16 @@
 #define INTEL_SIP_SMC_RSU_RETRY_COUNTER			0xC200000F
 #define INTEL_SIP_SMC_RSU_DCMF_VERSION			0xC2000010
 #define INTEL_SIP_SMC_RSU_COPY_DCMF_VERSION		0xC2000011
+#define INTEL_SIP_SMC_RSU_MAX_RETRY			0xC2000012
+#define INTEL_SIP_SMC_RSU_COPY_MAX_RETRY		0xC2000013
+#define INTEL_SIP_SMC_RSU_DCMF_STATUS			0xC2000014
+#define INTEL_SIP_SMC_RSU_COPY_DCMF_STATUS		0xC2000015
 
+/* Hardware monitor */
+#define INTEL_SIP_SMC_HWMON_READTEMP			0xC2000020
+#define INTEL_SIP_SMC_HWMON_READVOLT			0xC2000021
+#define TEMP_CHANNEL_MAX				(1 << 15)
+#define VOLT_CHANNEL_MAX				(1 << 15)
 
 /* ECC */
 #define INTEL_SIP_SMC_ECC_DBE				0xC200000D
@@ -49,24 +64,31 @@
 
 /* Send Mailbox Command */
 #define INTEL_SIP_SMC_MBOX_SEND_CMD			0xC200001E
+#define INTEL_SIP_SMC_FIRMWARE_VERSION			0xC200001F
+#define INTEL_SIP_SMC_HPS_SET_BRIDGES			0xC2000032
 
+/* Mailbox Command */
+#define INTEL_SIP_SMC_GET_USERCODE			0xC200003D
 
 /* SiP Definitions */
 
 /* ECC DBE */
 #define WARM_RESET_WFI_FLAG				BIT(31)
-#define SYSMGR_ECC_DBE_COLD_RST_MASK		(SYSMGR_ECC_OCRAM_MASK |\
+#define SYSMGR_ECC_DBE_COLD_RST_MASK			(SYSMGR_ECC_OCRAM_MASK |\
 							SYSMGR_ECC_DDR0_MASK |\
 							SYSMGR_ECC_DDR1_MASK)
 
+/* Non-mailbox SMC Call */
+#define INTEL_SIP_SMC_SVC_VERSION			0xC2000200
+
 /* SMC function IDs for SiP Service queries */
-#define SIP_SVC_CALL_COUNT	0x8200ff00
-#define SIP_SVC_UID		0x8200ff01
-#define SIP_SVC_VERSION		0x8200ff03
+#define SIP_SVC_CALL_COUNT				0x8200ff00
+#define SIP_SVC_UID					0x8200ff01
+#define SIP_SVC_VERSION					0x8200ff03
 
 /* SiP Service Calls version numbers */
-#define SIP_SVC_VERSION_MAJOR	0
-#define SIP_SVC_VERSION_MINOR	1
+#define SIP_SVC_VERSION_MAJOR				1
+#define SIP_SVC_VERSION_MINOR				0
 
 
 /* Structure Definitions */
diff --git a/plat/intel/soc/common/include/socfpga_system_manager.h b/plat/intel/soc/common/include/socfpga_system_manager.h
index 2b13f1f..a77734d 100644
--- a/plat/intel/soc/common/include/socfpga_system_manager.h
+++ b/plat/intel/soc/common/include/socfpga_system_manager.h
@@ -42,13 +42,6 @@
 #define IDLE_DATA_SOC2FPGA				BIT(4)
 #define IDLE_DATA_MASK		(IDLE_DATA_LWSOC2FPGA | IDLE_DATA_SOC2FPGA)
 
-#define SCR_AXI_AP_MASK					BIT(24)
-#define SCR_FPGA2SOC_MASK				BIT(16)
-#define SCR_MPU_MASK					BIT(0)
-#define DISABLE_L4_FIREWALL	(SCR_AXI_AP_MASK | SCR_FPGA2SOC_MASK \
-					| SCR_MPU_MASK)
-#define DISABLE_BRIDGE_FIREWALL				0x0ffe0101
-
 #define SYSMGR_ECC_OCRAM_MASK				BIT(1)
 #define SYSMGR_ECC_DDR0_MASK				BIT(16)
 #define SYSMGR_ECC_DDR1_MASK				BIT(17)
@@ -58,69 +51,4 @@
 #define SOCFPGA_SYSMGR(_reg)		(SOCFPGA_SYSMGR_REG_BASE \
 						+ (SOCFPGA_SYSMGR_##_reg))
 
-#define SOCFPGA_L4_PER_SCR(_reg)	(SOCFPGA_L4_PER_SCR_REG_BASE \
-					+ (SOCFPGA_NOC_FW_L4_PER_SCR_##_reg))
-
-#define SOCFPGA_L4_SYS_SCR(_reg)	(SOCFPGA_L4_SYS_SCR_REG_BASE \
-					+ (SOCFPGA_NOC_FW_L4_SYS_SCR_##_reg))
-
-/* L3 Interconnect Register Map */
-#define SOCFPGA_NOC_FW_L4_PER_SCR_NAND_REGISTER			0x0000
-#define SOCFPGA_NOC_FW_L4_PER_SCR_NAND_DATA			0x0004
-#define SOCFPGA_NOC_FW_L4_PER_SCR_USB0_REGISTER			0x000c
-#define SOCFPGA_NOC_FW_L4_PER_SCR_USB1_REGISTER			0x0010
-#define SOCFPGA_NOC_FW_L4_PER_SCR_SPI_MASTER0			0x001c
-#define SOCFPGA_NOC_FW_L4_PER_SCR_SPI_MASTER1			0x0020
-#define SOCFPGA_NOC_FW_L4_PER_SCR_SPI_SLAVE0			0x0024
-#define SOCFPGA_NOC_FW_L4_PER_SCR_SPI_SLAVE1			0x0028
-#define SOCFPGA_NOC_FW_L4_PER_SCR_EMAC0				0x002c
-#define SOCFPGA_NOC_FW_L4_PER_SCR_EMAC1				0x0030
-#define SOCFPGA_NOC_FW_L4_PER_SCR_EMAC2				0x0034
-#define SOCFPGA_NOC_FW_L4_PER_SCR_SDMMC				0x0040
-#define SOCFPGA_NOC_FW_L4_PER_SCR_GPIO0				0x0044
-#define SOCFPGA_NOC_FW_L4_PER_SCR_GPIO1				0x0048
-#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C0				0x0050
-#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C1				0x0054
-#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C2				0x0058
-#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C3				0x005c
-#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C4				0x0060
-#define SOCFPGA_NOC_FW_L4_PER_SCR_SP_TIMER0			0x0064
-#define SOCFPGA_NOC_FW_L4_PER_SCR_SP_TIMER1			0x0068
-#define SOCFPGA_NOC_FW_L4_PER_SCR_UART0				0x006c
-#define SOCFPGA_NOC_FW_L4_PER_SCR_UART1				0x0070
-
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_DMA_ECC			0x0008
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC0RX_ECC			0x000c
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC0TX_ECC			0x0010
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC1RX_ECC			0x0014
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC1TX_ECC			0x0018
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC2RX_ECC			0x001c
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC2TX_ECC			0x0020
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_NAND_ECC			0x002c
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_NAND_READ_ECC			0x0030
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_NAND_WRITE_ECC		0x0034
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_OCRAM_ECC			0x0038
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_SDMMC_ECC			0x0040
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_USB0_ECC			0x0044
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_USB1_ECC			0x0048
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_CLK_MGR			0x004c
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_IO_MGR			0x0054
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_RST_MGR			0x0058
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_SYS_MGR			0x005c
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_OSC0_TIMER			0x0060
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_OSC1_TIMER			0x0064
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_WATCHDOG0			0x0068
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_WATCHDOG1			0x006c
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_WATCHDOG2			0x0070
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_WATCHDOG3			0x0074
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_DAP				0x0078
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_L4_NOC_PROBES			0x0090
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_L4_NOC_QOS			0x0094
-
-#define SOCFPGA_CCU_NOC_CPU0_RAMSPACE0_0			0xf7004688
-#define SOCFPGA_CCU_NOC_IOM_RAMSPACE0_0				0xf7018628
-
-void enable_ns_peripheral_access(void);
-void enable_ns_bridge_access(void);
-
 #endif /* SOCFPGA_SYSTEMMANAGER_H */
diff --git a/plat/intel/soc/common/soc/socfpga_system_manager.c b/plat/intel/soc/common/soc/socfpga_firewall.c
similarity index 84%
rename from plat/intel/soc/common/soc/socfpga_system_manager.c
rename to plat/intel/soc/common/soc/socfpga_firewall.c
index a64053c..515784b 100644
--- a/plat/intel/soc/common/soc/socfpga_system_manager.c
+++ b/plat/intel/soc/common/soc/socfpga_firewall.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,6 +7,8 @@
 #include <lib/mmio.h>
 #include <lib/utils_def.h>
 
+#include "socfpga_noc.h"
+#include "socfpga_plat_def.h"
 #include "socfpga_system_manager.h"
 
 void enable_nonsecure_access(void)
@@ -92,12 +94,18 @@
 	mmio_write_32(SOCFPGA_L4_SYS_SCR(L4_NOC_QOS), DISABLE_L4_FIREWALL);
 
 #if PLATFORM_MODEL == PLAT_SOCFPGA_STRATIX10
-	mmio_clrbits_32(SOCFPGA_CCU_NOC_CPU0_RAMSPACE0_0, 0x03);
-	mmio_clrbits_32(SOCFPGA_CCU_NOC_IOM_RAMSPACE0_0, 0x03);
-
+	enable_ns_ocram_access();
 	mmio_write_32(SOCFPGA_SYSMGR(SDMMC), SYSMGR_SDMMC_DRVSEL(3));
 #endif
 
+}
+
+void enable_ns_ocram_access(void)
+{
+	mmio_clrbits_32(SOCFPGA_CCU_NOC(CPU0, RAM0),
+		SOCFPGA_CCU_NOC_ADMASK_P_MASK | SOCFPGA_CCU_NOC_ADMASK_NS_MASK);
+	mmio_clrbits_32(SOCFPGA_CCU_NOC(IOM, RAM0),
+		SOCFPGA_CCU_NOC_ADMASK_P_MASK | SOCFPGA_CCU_NOC_ADMASK_NS_MASK);
 }
 
 void enable_ns_bridge_access(void)
@@ -105,3 +113,11 @@
 	mmio_write_32(SOCFPGA_SOC2FPGA_SCR_REG_BASE, DISABLE_BRIDGE_FIREWALL);
 	mmio_write_32(SOCFPGA_LWSOC2FPGA_SCR_REG_BASE, DISABLE_BRIDGE_FIREWALL);
 }
+
+void enable_ocram_firewall(void)
+{
+	mmio_setbits_32(SOCFPGA_CCU_NOC(CPU0, RAM0),
+		SOCFPGA_CCU_NOC_ADMASK_P_MASK | SOCFPGA_CCU_NOC_ADMASK_NS_MASK);
+	mmio_setbits_32(SOCFPGA_CCU_NOC(IOM, RAM0),
+		SOCFPGA_CCU_NOC_ADMASK_P_MASK | SOCFPGA_CCU_NOC_ADMASK_NS_MASK);
+}
diff --git a/plat/intel/soc/common/soc/socfpga_mailbox.c b/plat/intel/soc/common/soc/socfpga_mailbox.c
index be900c9..8ecd6db 100644
--- a/plat/intel/soc/common/soc/socfpga_mailbox.c
+++ b/plat/intel/soc/common/soc/socfpga_mailbox.c
@@ -507,11 +507,13 @@
 		return MBOX_CFGSTAT_STATE_ERROR_HARDWARE;
 	}
 
-	if ((res & SOFTFUNC_STATUS_CONF_DONE) == 0U)
+	if ((res & SOFTFUNC_STATUS_CONF_DONE) == 0U) {
 		return MBOX_CFGSTAT_STATE_CONFIG;
+	}
 
-	if (init_done && (res & SOFTFUNC_STATUS_INIT_DONE) == 0U)
+	if (init_done && (res & SOFTFUNC_STATUS_INIT_DONE) == 0U) {
 		return MBOX_CFGSTAT_STATE_CONFIG;
+	}
 
 	return MBOX_RET_OK;
 }
@@ -527,3 +529,22 @@
 
 	return ret;
 }
+
+int mailbox_hwmon_readtemp(uint32_t chan, uint32_t *resp_buf)
+{
+	unsigned int resp_len = sizeof(resp_buf);
+
+	return mailbox_send_cmd(MBOX_JOB_ID, MBOX_HWMON_READTEMP, &chan, 1U,
+				CMD_CASUAL, resp_buf,
+				&resp_len);
+
+}
+
+int mailbox_hwmon_readvolt(uint32_t chan, uint32_t *resp_buf)
+{
+	unsigned int resp_len = sizeof(resp_buf);
+
+	return mailbox_send_cmd(MBOX_JOB_ID, MBOX_HWMON_READVOLT, &chan, 1U,
+				CMD_CASUAL, resp_buf,
+				&resp_len);
+}
diff --git a/plat/intel/soc/common/socfpga_sip_svc.c b/plat/intel/soc/common/socfpga_sip_svc.c
index 2335957..f22c2ee 100644
--- a/plat/intel/soc/common/socfpga_sip_svc.c
+++ b/plat/intel/soc/common/socfpga_sip_svc.c
@@ -20,10 +20,17 @@
 #define FPGA_CONFIG_BUFFER_SIZE 4
 
 static int current_block, current_buffer;
-static int read_block, max_blocks, is_partial_reconfig;
+static int read_block, max_blocks;
 static uint32_t send_id, rcv_id;
 static uint32_t bytes_per_block, blocks_submitted;
+static bool bridge_disable;
 
+/* RSU static variables */
+static uint32_t rsu_dcmf_ver[4] = {0};
+
+/* RSU Max Retry */
+static uint32_t rsu_max_retry;
+static uint16_t rsu_dcmf_stat[4] = {0};
 
 /*  SiP Service UUID */
 DEFINE_SVC_UUID2(intl_svc_uid,
@@ -83,22 +90,23 @@
 {
 	uint32_t ret;
 
-	if (query_type == 1)
+	if (query_type == 1U) {
 		ret = intel_mailbox_get_config_status(MBOX_CONFIG_STATUS, false);
-	else
+	} else {
 		ret = intel_mailbox_get_config_status(MBOX_RECONFIG_STATUS, true);
+	}
 
-	if (ret) {
-		if (ret == MBOX_CFGSTAT_STATE_CONFIG)
+	if (ret != 0U) {
+		if (ret == MBOX_CFGSTAT_STATE_CONFIG) {
 			return INTEL_SIP_SMC_STATUS_BUSY;
-		else
+		} else {
 			return INTEL_SIP_SMC_STATUS_ERROR;
+		}
 	}
 
-	if (query_type != 1) {
-		/* full reconfiguration */
-		if (!is_partial_reconfig)
-			socfpga_bridges_enable();	/* Enable bridge */
+	if (bridge_disable) {
+		socfpga_bridges_enable();	/* Enable bridge */
+		bridge_disable = false;
 	}
 
 	return INTEL_SIP_SMC_STATUS_OK;
@@ -184,7 +192,7 @@
 	return status;
 }
 
-static int intel_fpga_config_start(uint32_t config_type)
+static int intel_fpga_config_start(uint32_t flag)
 {
 	uint32_t argument = 0x1;
 	uint32_t response[3];
@@ -192,7 +200,14 @@
 	unsigned int size = 0;
 	unsigned int resp_len = ARRAY_SIZE(response);
 
+	if (!CONFIG_TEST_FLAG(flag, PARTIAL_CONFIG)) {
+		bridge_disable = true;
+	}
+
-	is_partial_reconfig = config_type;
+	if (CONFIG_TEST_FLAG(flag, AUTHENTICATION)) {
+		size = 1;
+		bridge_disable = false;
+	}
 
 	mailbox_clear_response();
 
@@ -202,8 +217,10 @@
 	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_RECONFIG, &argument, size,
 			CMD_CASUAL, response, &resp_len);
 
-	if (status < 0)
-		return status;
+	if (status < 0) {
+		bridge_disable = false;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
 
 	max_blocks = response[0];
 	bytes_per_block = response[1];
@@ -222,13 +239,12 @@
 	read_block = 0;
 	current_buffer = 0;
 
-	/* full reconfiguration */
-	if (!is_partial_reconfig) {
-		/* Disable bridge */
+	/* Disable bridge on full reconfiguration */
+	if (bridge_disable) {
 		socfpga_bridges_disable();
 	}
 
-	return 0;
+	return INTEL_SIP_SMC_STATUS_OK;
 }
 
 static bool is_fpga_config_buffer_full(void)
@@ -261,8 +277,9 @@
 	intel_fpga_sdm_write_all();
 
 	if (!is_address_in_ddr_range(mem, size) ||
-		is_fpga_config_buffer_full())
+		is_fpga_config_buffer_full()) {
 		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
 
 	for (i = 0; i < FPGA_CONFIG_BUFFER_SIZE; i++) {
 		int j = (i + current_buffer) % FPGA_CONFIG_BUFFER_SIZE;
@@ -279,14 +296,19 @@
 		}
 	}
 
-	if (is_fpga_config_buffer_full())
+	if (is_fpga_config_buffer_full()) {
 		return INTEL_SIP_SMC_STATUS_BUSY;
+	}
 
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
 static int is_out_of_sec_range(uint64_t reg_addr)
 {
+#if DEBUG
+	return 0;
+#endif
+
 	switch (reg_addr) {
 	case(0xF8011100):	/* ECCCTRL1 */
 	case(0xF8011104):	/* ECCCTRL2 */
@@ -393,7 +415,77 @@
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
+static uint32_t intel_rsu_copy_dcmf_version(uint64_t dcmf_ver_1_0,
+					    uint64_t dcmf_ver_3_2)
+{
+	rsu_dcmf_ver[0] = dcmf_ver_1_0;
+	rsu_dcmf_ver[1] = dcmf_ver_1_0 >> 32;
+	rsu_dcmf_ver[2] = dcmf_ver_3_2;
+	rsu_dcmf_ver[3] = dcmf_ver_3_2 >> 32;
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+static uint32_t intel_rsu_copy_dcmf_status(uint64_t dcmf_stat)
+{
+	rsu_dcmf_stat[0] = 0xFFFF & (dcmf_stat >> (0 * 16));
+	rsu_dcmf_stat[1] = 0xFFFF & (dcmf_stat >> (1 * 16));
+	rsu_dcmf_stat[2] = 0xFFFF & (dcmf_stat >> (2 * 16));
+	rsu_dcmf_stat[3] = 0xFFFF & (dcmf_stat >> (3 * 16));
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+/* Intel HWMON services */
+static uint32_t intel_hwmon_readtemp(uint32_t chan, uint32_t *retval)
+{
+	if (chan > TEMP_CHANNEL_MAX) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	if (mailbox_hwmon_readtemp(chan, retval) < 0) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+static uint32_t intel_hwmon_readvolt(uint32_t chan, uint32_t *retval)
+{
+	if (chan > VOLT_CHANNEL_MAX) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	if (mailbox_hwmon_readvolt(chan, retval) < 0) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
 /* Mailbox services */
+static uint32_t intel_smc_fw_version(uint32_t *fw_version)
+{
+	int status;
+	unsigned int resp_len = CONFIG_STATUS_WORD_SIZE;
+	uint32_t resp_data[CONFIG_STATUS_WORD_SIZE] = {0U};
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_CONFIG_STATUS, NULL, 0U,
+			CMD_CASUAL, resp_data, &resp_len);
+
+	if (status < 0) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	if (resp_len <= CONFIG_STATUS_FW_VER_OFFSET) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	*fw_version = resp_data[CONFIG_STATUS_FW_VER_OFFSET] & CONFIG_STATUS_FW_VER_MASK;
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
 static uint32_t intel_mbox_send_cmd(uint32_t cmd, uint32_t *args,
 				unsigned int len,
 				uint32_t urgent, uint32_t *response,
@@ -419,6 +511,33 @@
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
+static int intel_smc_get_usercode(uint32_t *user_code)
+{
+	int status;
+	unsigned int resp_len = sizeof(user_code) / MBOX_WORD_BYTE;
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_GET_USERCODE, NULL,
+				0U, CMD_CASUAL, user_code, &resp_len);
+
+	if (status < 0) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+/* Miscellaneous HPS services */
+static uint32_t intel_hps_set_bridges(uint64_t enable)
+{
+	if (enable != 0U) {
+		socfpga_bridges_enable();
+	} else {
+		socfpga_bridges_disable();
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
 /*
  * This function is responsible for handling all SiP calls from the NS world
  */
@@ -531,10 +650,41 @@
 			SMC_RET2(handle, status, retval);
 		}
 
+	case INTEL_SIP_SMC_RSU_DCMF_VERSION:
+		SMC_RET3(handle, INTEL_SIP_SMC_STATUS_OK,
+			 ((uint64_t)rsu_dcmf_ver[1] << 32) | rsu_dcmf_ver[0],
+			 ((uint64_t)rsu_dcmf_ver[3] << 32) | rsu_dcmf_ver[2]);
+
+	case INTEL_SIP_SMC_RSU_COPY_DCMF_VERSION:
+		status = intel_rsu_copy_dcmf_version(x1, x2);
+		SMC_RET1(handle, status);
+
+	case INTEL_SIP_SMC_RSU_DCMF_STATUS:
+		SMC_RET2(handle, INTEL_SIP_SMC_STATUS_OK,
+			 ((uint64_t)rsu_dcmf_stat[3] << 48) |
+			 ((uint64_t)rsu_dcmf_stat[2] << 32) |
+			 ((uint64_t)rsu_dcmf_stat[1] << 16) |
+			 rsu_dcmf_stat[0]);
+
+	case INTEL_SIP_SMC_RSU_COPY_DCMF_STATUS:
+		status = intel_rsu_copy_dcmf_status(x1);
+		SMC_RET1(handle, status);
+
+	case INTEL_SIP_SMC_RSU_MAX_RETRY:
+		SMC_RET2(handle, INTEL_SIP_SMC_STATUS_OK, rsu_max_retry);
+
+	case INTEL_SIP_SMC_RSU_COPY_MAX_RETRY:
+		rsu_max_retry = x1;
+		SMC_RET1(handle, INTEL_SIP_SMC_STATUS_OK);
+
 	case INTEL_SIP_SMC_ECC_DBE:
 		status = intel_ecc_dbe_notification(x1);
 		SMC_RET1(handle, status);
 
+	case INTEL_SIP_SMC_FIRMWARE_VERSION:
+		status = intel_smc_fw_version(&retval);
+		SMC_RET2(handle, status, retval);
+
 	case INTEL_SIP_SMC_MBOX_SEND_CMD:
 		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
 		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
@@ -543,11 +693,32 @@
 					     &len_in_resp);
 		SMC_RET3(handle, status, mbox_status, len_in_resp);
 
+	case INTEL_SIP_SMC_GET_USERCODE:
+		status = intel_smc_get_usercode(&retval);
+		SMC_RET2(handle, status, retval);
+
 	case INTEL_SIP_SMC_GET_ROM_PATCH_SHA384:
 		status = intel_fcs_get_rom_patch_sha384(x1, &retval64,
 							&mbox_error);
 		SMC_RET4(handle, status, mbox_error, x1, retval64);
 
+	case INTEL_SIP_SMC_SVC_VERSION:
+		SMC_RET3(handle, INTEL_SIP_SMC_STATUS_OK,
+					SIP_SVC_VERSION_MAJOR,
+					SIP_SVC_VERSION_MINOR);
+
+	case INTEL_SIP_SMC_HPS_SET_BRIDGES:
+		status = intel_hps_set_bridges(x1);
+		SMC_RET1(handle, status);
+
+	case INTEL_SIP_SMC_HWMON_READTEMP:
+		status = intel_hwmon_readtemp(x1, &retval);
+		SMC_RET2(handle, status, retval);
+
+	case INTEL_SIP_SMC_HWMON_READVOLT:
+		status = intel_hwmon_readvolt(x1, &retval);
+		SMC_RET2(handle, status, retval);
+
 	default:
 		return socfpga_sip_handler(smc_fid, x1, x2, x3, x4,
 			cookie, handle, flags);
diff --git a/plat/intel/soc/stratix10/bl31_plat_setup.c b/plat/intel/soc/stratix10/bl31_plat_setup.c
index f804c8e..be0fae5 100644
--- a/plat/intel/soc/stratix10/bl31_plat_setup.c
+++ b/plat/intel/soc/stratix10/bl31_plat_setup.c
@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2019-2020, ARM Limited and Contributors. All rights reserved.
- * Copyright (c) 2019-2020, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -17,6 +17,7 @@
 #include <platform_def.h>
 
 #include "socfpga_mailbox.h"
+#include "socfpga_noc.h"
 #include "socfpga_private.h"
 #include "socfpga_reset_manager.h"
 #include "socfpga_system_manager.h"
@@ -122,6 +123,8 @@
 		(uint64_t)plat_secondary_cpus_bl31_entry);
 
 	mailbox_hps_stage_notify(HPS_EXECUTION_STATE_SSBL);
+
+	enable_ocram_firewall();
 }
 
 const mmap_region_t plat_stratix10_mmap[] = {
diff --git a/plat/intel/soc/stratix10/include/socfpga_plat_def.h b/plat/intel/soc/stratix10/include/socfpga_plat_def.h
index b84a567..2defeb9 100644
--- a/plat/intel/soc/stratix10/include/socfpga_plat_def.h
+++ b/plat/intel/soc/stratix10/include/socfpga_plat_def.h
@@ -1,6 +1,5 @@
 /*
  * Copyright (c) 2019-2022, ARM Limited and Contributors. All rights reserved.
- * Copyright (c) 2019-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,6 +18,8 @@
 #define INTEL_SIP_SMC_FPGA_CONFIG_SIZE		0x1000000
 
 /* Register Mapping */
+#define SOCFPGA_CCU_NOC_REG_BASE		0xf7000000
+
 #define SOCFPGA_MMC_REG_BASE                    0xff808000
 
 #define SOCFPGA_RSTMGR_REG_BASE			0xffd11000
diff --git a/plat/intel/soc/stratix10/platform.mk b/plat/intel/soc/stratix10/platform.mk
index d9d88d4..b7808ae 100644
--- a/plat/intel/soc/stratix10/platform.mk
+++ b/plat/intel/soc/stratix10/platform.mk
@@ -26,7 +26,8 @@
 			lib/xlat_tables/xlat_tables_common.c 		\
 			plat/intel/soc/common/aarch64/platform_common.c \
 			plat/intel/soc/common/aarch64/plat_helpers.S	\
-			plat/intel/soc/common/socfpga_delay_timer.c
+			plat/intel/soc/common/socfpga_delay_timer.c	\
+			plat/intel/soc/common/soc/socfpga_firewall.c
 
 BL2_SOURCES     +=	\
 		common/desc_image_load.c				\
@@ -50,7 +51,6 @@
 		plat/intel/soc/common/soc/socfpga_handoff.c		\
 		plat/intel/soc/common/soc/socfpga_mailbox.c		\
 		plat/intel/soc/common/soc/socfpga_reset_manager.c	\
-		plat/intel/soc/common/soc/socfpga_system_manager.c	\
 		plat/intel/soc/common/drivers/qspi/cadence_qspi.c	\
 		plat/intel/soc/common/drivers/wdt/watchdog.c
 
diff --git a/plat/mediatek/build_helpers/conditional_eval_options.mk b/plat/mediatek/build_helpers/conditional_eval_options.mk
new file mode 100644
index 0000000..6bb3b4e
--- /dev/null
+++ b/plat/mediatek/build_helpers/conditional_eval_options.mk
@@ -0,0 +1,51 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# Conditional makefile variable assignment
+
+# Options depend on BUILD_TYPE variable
+ifeq ($(BUILD_TYPE),release)
+MTK_DEBUGSYS_LOCK := 1
+MTK_GET_PERM_DIS := 1
+ERRATA_KLEIN_2218950 := 0
+ERRATA_KLEIN_2184257 := 0
+ERRATA_KLEIN_BOOKER := 0
+ERRATA_MTH_BOOKER := 0
+ERRATA_MTHELP_BOOKER := 0
+CRASH_REPORTING := 1
+CONFIG_MTK_BL31_RAMDUMP := 0
+endif
+
+ifeq ($(BUILD_TYPE),debug)
+MTK_PTP3_PROC_DEBUG := 1
+MTK_SRAMRC_DEBUG := 1
+MTK_IOMMU_DEBUG := 1
+MTK_DCM_DEBUG := 1
+MTK_EMI_MPU_DEBUG := 1
+endif
+
+ifeq (${SPD},none)
+SPD_NONE:=1
+$(eval $(call add_define,SPD_NONE))
+endif
+
+# TEE OS config
+ifeq ($(SPD), tbase)
+CONFIG_TBASE := y
+endif
+
+# MICROTRUST OS config
+ifeq ($(SPD), teeid)
+CONFIG_MICROTRUST_TEEI := y
+endif
+
+ifeq (${CONFIG_ARCH_ARM_V8_2},y)
+ARCH_VERSION := armv8_2
+endif
+
+ifeq (${CONFIG_ARCH_ARM_V9},y)
+ARCH_VERSION := armv9
+endif
diff --git a/plat/mediatek/build_helpers/mtk_build_helpers.mk b/plat/mediatek/build_helpers/mtk_build_helpers.mk
new file mode 100644
index 0000000..47d96fa
--- /dev/null
+++ b/plat/mediatek/build_helpers/mtk_build_helpers.mk
@@ -0,0 +1,139 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# Get local directory path
+define GET_LOCAL_DIR
+$(patsubst %/,%,$(dir $(word $(words $(MAKEFILE_LIST)),$(MAKEFILE_LIST))))
+endef
+
+# Clear module source variable
+define CLEAR_LOCAL_SRCS
+$(eval $(1) :=)
+endef
+
+define EXPAND_SUB_MAKEFILE
+include $(S)
+endef
+
+# Expand sub rules.mk
+define INCLUDE_MAKEFILE
+$(eval MODULES_SUB_MAKEFILE := $(patsubst %,%/rules.mk,$(1)))
+$(foreach S,$(MODULES_SUB_MAKEFILE),$(eval $(EXPAND_SUB_MAKEFILE)))
+endef
+
+# Determine option variable is defined or not then define it
+define add_defined_option
+ifdef $(1)
+ifeq ($(findstring $(value $(1)), $(uppercase_table)),)
+DEFINES += -D$(1)$(if $(value $(1)),=$(value $(1)),)
+else
+ifeq ($(strip $(value $(1))),y)
+DEFINES += -D$(1)$(if $(value $(1)),=1,)
+endif
+endif
+endif
+endef
+
+define EXPAND_RULES_MAKEFILE
+LOCAL_SRCS-y :=
+MODULE :=
+SUB_RULES-y :=
+include $(S)
+endef
+
+# INCLUDE_MODULES macro expand included modules rules.mk
+# Arguments:
+#   $(1) = MODULES variables
+define INCLUDE_MODULES
+$(eval MODULES_TEMP := $(1))
+$(eval MODULES_MAKEFILE := $(patsubst %,%/rules.mk,$(MODULES_TEMP)))
+$(foreach S,$(MODULES_MAKEFILE),$(eval $(EXPAND_RULES_MAKEFILE)))
+endef
+
+# MAKE_LOCALS expand module source file variable to BL${BL}_SOURCES
+# Arguments:
+#   $(1) = source file
+#   $(2) = BL stage (1, 2, 2u, 31, 32)
+define MAKE_LOCALS
+$(eval $(call uppercase,$(2))_SOURCES += $(1))
+endef
+
+# MAKE_LINKERFILE change linker script source file name to
+# target linker script
+#   $(1) = linker script source file
+#   $(2) = BL stage
+define MAKE_LINKERFILE
+$(eval EXTRA_GENERATED_LINKER_SCRIPT += $(BUILD_PLAT)/$(2)/$(patsubst %.ld.S,%.ld,$(notdir $(1))))
+endef
+
+# MAKE_LINKERFILE_ITER call MAKE_LINKERFILE iteratively
+#   $(1) = linker script source file
+#   $(2) = BL stage
+define MAKE_LINKERFILE_ITER
+$(eval $(foreach link_src,$(1),$(call MAKE_LINKERFILE,$(link_src),$(2))))
+endef
+
+# MAKE_LD_ITER generate the linker scripts using the C preprocessor iteratively
+#   $(1) = output linker script
+#   $(2) = input template
+#   $(3) = BL stage (1, 2, 2u, 31, 32)
+define MAKE_LD_ITER
+$(eval index_list=$(shell seq $(words $(1))))
+$(eval $(foreach i, $(index_list), \
+$(call MAKE_LD,$(word $(i), $(1)), $(word $(i), $(2)),$(3))))
+endef
+
+# MAKE_MODULE reference MAKE_OBJS.
+# Create module folder under out/bl$(BL)/$(module)
+# Arguments:
+#   $(1) = module name
+#   $(2) = source file
+#   $(3) = BL stage
+define MAKE_MODULE
+        $(eval MODULE := $(strip $(1)))
+        $(eval BUILD_DIR  := ${BUILD_PLAT}/${3})
+        $(eval SOURCES    := $(2))
+        $(eval OBJS_TEMP  := $(addprefix $(BUILD_DIR)/$(MODULE)/,$(call SOURCES_TO_OBJS,$(SOURCES))))
+        $(eval MODULE_OBJS += $(OBJS_TEMP))
+        # We use sort only to get a list of unique object directory names.
+        # ordering is not relevant but sort removes duplicates.
+        $(eval TEMP_OBJ_DIRS := $(sort $(dir ${OBJS_TEMP} ${LINKERFILE})))
+        # The $(dir ) function leaves a trailing / on the directory names
+        # Rip off the / to match directory names with make rule targets.
+        $(eval OBJ_DIRS := $(patsubst %/,%,$(TEMP_OBJ_DIRS)))
+
+$(eval $(foreach objd,${OBJ_DIRS},$(call MAKE_PREREQ_DIR,${objd},${BUILD_DIR})))
+${3}_dirs: | ${OBJ_DIRS}
+
+$(eval $(call MAKE_OBJS,$(BUILD_DIR)/$(MODULE),$(SOURCES),${3}))
+
+libraries: $(OBJS_TEMP)
+endef
+
+# Include MTK configuration files
+
+# MTK makefile variables
+MTK_PLAT      := plat/mediatek
+MTK_PLAT_SOC  := ${MTK_PLAT}/${MTK_SOC}
+MTK_COMMON_CFG := $(MTK_PLAT)/common/common_config.mk
+MTK_PLAT_CFG := $(MTK_PLAT_SOC)/plat_config.mk
+MTK_PROJECT_CFG := $(MTK_PLAT)/project/$(PLAT)/project_config.mk
+MTK_OPTIONS := $(MTK_PLAT)/build_helpers/options.mk
+MTK_COND_EVAL := $(MTK_PLAT)/build_helpers/conditional_eval_options.mk
+
+# Indicate which BL should be built in command line
+ifeq (${NEED_BL31},yes)
+MTK_BL := bl31
+endif
+ifeq (${NEED_BL32},yes)
+MTK_BL := bl32
+endif
+# Include common, platform, board level config
+include $(MTK_COMMON_CFG)
+include $(MTK_PLAT_CFG)
+-include $(MTK_PROJECT_CFG)
+include $(MTK_COND_EVAL)
+include $(MTK_OPTIONS)
diff --git a/plat/mediatek/build_helpers/mtk_build_helpers_epilogue.mk b/plat/mediatek/build_helpers/mtk_build_helpers_epilogue.mk
new file mode 100644
index 0000000..22a546c
--- /dev/null
+++ b/plat/mediatek/build_helpers/mtk_build_helpers_epilogue.mk
@@ -0,0 +1,30 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# Expand include modules
+$(eval $(call INCLUDE_MODULES,$(MODULES-y)))
+
+# Make next section align to page size
+ifneq ($(MTK_EXTRA_LINKERFILE),)
+$(eval $(call MAKE_LINKERFILE_ITER,$(MTK_LINKERFILE_SOURCE),bl31))
+
+# EXTRA_GENERATED_LINKER_SCRIPT is a global variable of derived linker
+# script list(from MTK_LINKERFILE_SOURCE) after MAKE_LINKERFILE_ITER
+# function call
+EXTRA_LINKERFILE += ${EXTRA_GENERATED_LINKER_SCRIPT}
+
+# Expand derived linker script as build target
+$(eval $(call MAKE_LD_ITER, $(EXTRA_GENERATED_LINKER_SCRIPT),$(MTK_LINKERFILE_SOURCE),bl31))
+
+# mtk_align.ld MUST BE THE LAST LINKER SCRIPT!
+EXTRA_LINKERFILE += ${MTK_PLAT}/include/mtk_align.ld
+
+# bl31.ld should depend on EXTRA_LINKERFILE
+$(eval ${BUILD_PLAT}/bl31/bl31.ld: ${EXTRA_LINKERFILE})
+EXTRA_LINKERFILE := $(addprefix -T,$(EXTRA_LINKERFILE))
+else
+EXTRA_LINKERFILE :=
+endif
diff --git a/plat/mediatek/build_helpers/options.mk b/plat/mediatek/build_helpers/options.mk
new file mode 100644
index 0000000..394a605
--- /dev/null
+++ b/plat/mediatek/build_helpers/options.mk
@@ -0,0 +1,17 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# call add_defined_option to evaluate MTK defined value
+$(eval $(call add_defined_option,MTK_SIP_KERNEL_BOOT_ENABLE))
+$(eval $(call add_defined_option,PLAT_EXTRA_LD_SCRIPT))
+$(eval $(call add_defined_option,MTK_EXTRA_LINKERFILE))
+$(eval $(call add_defined_option,MTK_BL31_AS_BL2))
+$(eval $(call add_defined_option,MTK_BL33_IS_64BIT))
+$(eval $(call add_defined_option,PLAT_XLAT_TABLES_DYNAMIC))
+$(eval $(call add_defined_option,MTK_ADAPTED))
+$(eval $(call add_defined_option,MTK_SOC))
+$(eval $(call add_defined_option,UART_CLOCK))
+$(eval $(call add_defined_option,UART_BAUDRATE))
diff --git a/plat/qemu/common/qemu_common.c b/plat/qemu/common/qemu_common.c
index 47ec791..0c184f4 100644
--- a/plat/qemu/common/qemu_common.c
+++ b/plat/qemu/common/qemu_common.c
@@ -1,6 +1,6 @@
 
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -11,6 +11,7 @@
 #include <common/bl_common.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
 
+#include <plat/common/platform.h>
 #include "qemu_private.h"
 
 #define MAP_DEVICE0	MAP_REGION_FLAT(DEVICE0_BASE,			\
@@ -160,4 +161,9 @@
 DEFINE_CONFIGURE_MMU_EL(svc_mon)
 #endif
 
-
+#if MEASURED_BOOT || TRUSTED_BOARD_BOOT
+int plat_get_mbedtls_heap(void **heap_addr, size_t *heap_size)
+{
+	return get_mbedtls_heap_helper(heap_addr, heap_size);
+}
+#endif
diff --git a/plat/qemu/common/qemu_private.h b/plat/qemu/common/qemu_private.h
index 4dc62f5..c313cb6 100644
--- a/plat/qemu/common/qemu_private.h
+++ b/plat/qemu/common/qemu_private.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -34,4 +34,18 @@
 void qemu_pwr_gic_on_finish(void);
 void qemu_pwr_gic_off(void);
 
+int qemu_set_tos_fw_info(uintptr_t config_base, uintptr_t log_addr,
+			size_t log_size);
+
+int qemu_set_nt_fw_info(
+/*
+ * Currently OP-TEE does not support reading DTBs from Secure memory
+ * and this option should be removed when feature is supported.
+ */
+#ifdef SPD_opteed
+			uintptr_t log_addr,
+#endif
+			size_t log_size,
+			uintptr_t *ns_log_addr);
+
 #endif /* QEMU_PRIVATE_H */
diff --git a/plat/qemu/common/qemu_trusted_boot.c b/plat/qemu/common/qemu_trusted_boot.c
index 1ef7e43..6a8edca 100644
--- a/plat/qemu/common/qemu_trusted_boot.c
+++ b/plat/qemu/common/qemu_trusted_boot.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -29,8 +29,3 @@
 {
 	return 1;
 }
-
-int plat_get_mbedtls_heap(void **heap_addr, size_t *heap_size)
-{
-	return get_mbedtls_heap_helper(heap_addr, heap_size);
-}
diff --git a/plat/qemu/qemu/include/platform_def.h b/plat/qemu/qemu/include/platform_def.h
index c02eff9..78467c4 100644
--- a/plat/qemu/qemu/include/platform_def.h
+++ b/plat/qemu/qemu/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -270,4 +270,9 @@
  */
 #define SYS_COUNTER_FREQ_IN_TICKS	((1000 * 1000 * 1000) / 16)
 
+/*
+ * Maximum size of Event Log buffer used in Measured Boot Event Log driver
+ */
+#define	PLAT_EVENT_LOG_MAX_SIZE		UL(0x400)
+
 #endif /* PLATFORM_DEF_H */
diff --git a/plat/qemu/qemu/platform.mk b/plat/qemu/qemu/platform.mk
index a8f978a..6a877c3 100644
--- a/plat/qemu/qemu/platform.mk
+++ b/plat/qemu/qemu/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2013-2021, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -57,11 +57,7 @@
 
 ifneq (${TRUSTED_BOARD_BOOT},0)
 
-    include drivers/auth/mbedtls/mbedtls_crypto.mk
-    include drivers/auth/mbedtls/mbedtls_x509.mk
-
     AUTH_SOURCES	:=	drivers/auth/auth_mod.c			\
-				drivers/auth/crypto_mod.c		\
 				drivers/auth/img_parser_mod.c		\
 				drivers/auth/tbbr/tbbr_cot_common.c
 
@@ -78,6 +74,8 @@
 				$(PLAT_QEMU_COMMON_PATH)/qemu_rotpk.S	\
 				drivers/auth/tbbr/tbbr_cot_bl2.c
 
+    include drivers/auth/mbedtls/mbedtls_x509.mk
+
     ROT_KEY             = $(BUILD_PLAT)/rot_key.pem
     ROTPK_HASH          = $(BUILD_PLAT)/rotpk_sha256.bin
 
@@ -98,6 +96,34 @@
 	openssl dgst -sha256 -binary > $@ 2>/dev/null
 endif
 
+# Include Measured Boot makefile before any Crypto library makefile.
+# Crypto library makefile may need default definitions of Measured Boot build
+# flags present in Measured Boot makefile.
+ifeq (${MEASURED_BOOT},1)
+    MEASURED_BOOT_MK := drivers/measured_boot/event_log/event_log.mk
+    $(info Including ${MEASURED_BOOT_MK})
+    include ${MEASURED_BOOT_MK}
+
+    BL2_SOURCES		+=	plat/qemu/qemu/qemu_measured_boot.c	\
+				plat/qemu/qemu/qemu_common_measured_boot.c	\
+				plat/qemu/qemu/qemu_helpers.c		\
+				${EVENT_LOG_SOURCES}
+
+     BL1_SOURCES	+=      plat/qemu/qemu/qemu_bl1_measured_boot.c
+
+endif
+
+ifneq ($(filter 1,${MEASURED_BOOT} ${TRUSTED_BOARD_BOOT}),)
+    CRYPTO_SOURCES	:=	drivers/auth/crypto_mod.c
+
+    BL1_SOURCES		+=	${CRYPTO_SOURCES}
+    BL2_SOURCES		+=	${CRYPTO_SOURCES}
+
+    # We expect to locate the *.mk files under the directories specified below
+    #
+    include drivers/auth/mbedtls/mbedtls_crypto.mk
+endif
+
 BL1_SOURCES		+=	drivers/io/io_semihosting.c		\
 				drivers/io/io_storage.c			\
 				drivers/io/io_fip.c			\
@@ -131,6 +157,7 @@
 				${PLAT_QEMU_COMMON_PATH}/qemu_bl2_mem_params_desc.c	\
 				${PLAT_QEMU_COMMON_PATH}/qemu_image_load.c		\
 				common/fdt_fixup.c					\
+				common/fdt_wrappers.c					\
 				common/desc_image_load.c
 
 ifeq ($(add-lib-optee),yes)
diff --git a/plat/qemu/qemu/qemu_bl1_measured_boot.c b/plat/qemu/qemu/qemu_bl1_measured_boot.c
new file mode 100644
index 0000000..3d20f97
--- /dev/null
+++ b/plat/qemu/qemu/qemu_bl1_measured_boot.c
@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2022, Linaro.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include <common/desc_image_load.h>
+
+/*
+ * Add dummy functions for measured boot for BL1.
+ * In most of the SoC's, ROM/BL1 code is pre-built. So we are assumimg that
+ * it doesn't have the capability to do measurements and extend eventlog.
+ * hence these are dummy functions.
+ */
+void bl1_plat_mboot_init(void)
+{
+}
+
+void bl1_plat_mboot_finish(void)
+{
+}
+
+int plat_mboot_measure_image(unsigned int image_id, image_info_t *image_data)
+{
+	return 0;
+}
diff --git a/plat/qemu/qemu/qemu_common_measured_boot.c b/plat/qemu/qemu/qemu_common_measured_boot.c
new file mode 100644
index 0000000..41f7f87
--- /dev/null
+++ b/plat/qemu/qemu/qemu_common_measured_boot.c
@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) 2022, Linaro.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdint.h>
+
+#include <common/desc_image_load.h>
+#include <drivers/measured_boot/event_log/event_log.h>
+#include <plat/common/platform.h>
+
+extern event_log_metadata_t qemu_event_log_metadata[];
+
+const event_log_metadata_t *plat_event_log_get_metadata(void)
+{
+	return qemu_event_log_metadata;
+}
+
+int plat_mboot_measure_image(unsigned int image_id, image_info_t *image_data)
+{
+	/* Calculate image hash and record data in Event Log */
+	int err = event_log_measure_and_record(image_data->image_base,
+					       image_data->image_size,
+					       image_id);
+	if (err != 0) {
+		ERROR("%s%s image id %u (%i)\n",
+		      "Failed to ", "record", image_id, err);
+		return err;
+	}
+
+	return 0;
+}
diff --git a/plat/qemu/qemu/qemu_helpers.c b/plat/qemu/qemu/qemu_helpers.c
new file mode 100644
index 0000000..01b8249
--- /dev/null
+++ b/plat/qemu/qemu/qemu_helpers.c
@@ -0,0 +1,214 @@
+/*
+ * Copyright (c) 2022, Linaro.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#if MEASURED_BOOT
+#include <common/desc_image_load.h>
+#endif
+#include <common/fdt_wrappers.h>
+
+#include <libfdt.h>
+
+#ifdef SPD_opteed
+/*
+ * Currently OP-TEE does not support reading DTBs from Secure memory
+ * and this property should be removed when this feature is supported.
+ */
+#define DTB_PROP_HW_SM_LOG_ADDR	"tpm_event_log_sm_addr"
+#endif
+
+#define DTB_PROP_HW_LOG_ADDR	"tpm_event_log_addr"
+#define DTB_PROP_HW_LOG_SIZE    "tpm_event_log_size"
+
+#if MEASURED_BOOT
+
+#ifdef SPD_opteed
+int qemu_set_tee_fw_info(uintptr_t config_base, uintptr_t log_addr,
+			 size_t log_size)
+{
+	int offs, err = 0;
+	void *dtb = (void *)config_base;
+	const char *compatible = "arm,tpm_event_log";
+	uint64_t sec_base = cpu_to_fdt64(log_addr);
+	uint32_t sz = cpu_to_fdt32(log_size);
+
+	offs = fdtw_find_or_add_subnode(dtb, 0, "tpm-event-log");
+	if (offs < 0) {
+		ERROR("Failed to add node tpm-event-log %d\n", offs);
+		return offs;
+	}
+
+	if (fdt_appendprop(dtb, offs, "compatible", compatible,
+			   strlen(compatible) + 1) < 0) {
+		return -1;
+	}
+
+	err = fdt_setprop(dtb, offs, DTB_PROP_HW_SM_LOG_ADDR, &sec_base, 8);
+	if (err < 0) {
+		ERROR("Failed to add log addr err %d\n", err);
+		return err;
+	}
+
+	err = fdt_setprop(dtb, offs, DTB_PROP_HW_LOG_SIZE, &sz, 4);
+	if (err < 0) {
+		ERROR("Failed to add log addr err %d\n", err);
+		return err;
+	}
+
+	return err;
+}
+#endif
+
+/*
+ * Write the Event Log address and its size in the DTB.
+ *
+ * This function is supposed to be called only by BL2.
+ *
+ * Returns:
+ *	0 = success
+ *    < 0 = error
+ */
+static int qemu_set_event_log_info(uintptr_t config_base,
+#ifdef SPD_opteed
+				  uintptr_t sm_log_addr,
+#endif
+				  uintptr_t log_addr, size_t log_size)
+{
+	/* As libfdt uses void *, we can't avoid this cast */
+	void *dtb = (void *)config_base;
+	const char *compatible_tpm = "tcg,tpm-tis-mmio";
+	uint64_t base = cpu_to_fdt64(log_addr);
+	uint32_t sz = cpu_to_fdt32(log_size);
+	int err, node;
+
+	err = fdt_open_into(dtb, dtb, PLAT_QEMU_DT_MAX_SIZE);
+	if (err < 0) {
+		ERROR("Invalid Device Tree at %p: error %d\n", dtb, err);
+		return err;
+	}
+
+	/*
+	 * Verify that the DTB is valid, before attempting to write to it,
+	 * and get the DTB root node.
+	 */
+
+	/* Check if the pointer to DT is correct */
+	err = fdt_check_header(dtb);
+	if (err < 0) {
+		WARN("Invalid DTB file passed\n");
+		return err;
+	}
+
+	/*
+	 * Find the TPM node in device tree. On qemu, we assume it will
+	 * be sw-tpm.
+	 */
+	node = fdt_node_offset_by_compatible(dtb, -1, compatible_tpm);
+	if (node < 0) {
+		ERROR("The compatible property '%s' not%s", compatible_tpm,
+			" found in the config\n");
+		return node;
+	}
+
+	err = fdt_setprop(dtb, node, DTB_PROP_HW_LOG_ADDR, &base, 8);
+	if (err < 0) {
+		ERROR("Failed to add log addr err %d\n", err);
+		return err;
+	}
+
+	err = fdt_setprop(dtb, node, DTB_PROP_HW_LOG_SIZE, &sz, 4);
+	if (err < 0) {
+		ERROR("Failed to add log addr err %d\n", err);
+		return err;
+	}
+
+#ifdef SPD_opteed
+	err = qemu_set_tee_fw_info(config_base, sm_log_addr, log_size);
+	if (err < 0) {
+		ERROR("Failed to add tpm-event-node at %p: err %d\n", dtb, err);
+		return err;
+	}
+#endif
+
+	err = fdt_pack(dtb);
+	if (err < 0) {
+		ERROR("Failed to pack Device Tree at %p: err %d\n", dtb, err);
+		return err;
+	}
+
+	/*
+	 * Ensure that the info written to the DTB is visible
+	 * to other images.
+	 */
+	flush_dcache_range(config_base, fdt_totalsize(dtb));
+
+	return err;
+}
+
+/*
+ * This function writes the Event Log address and its size
+ * in the TOS_FW_CONFIG DTB.
+ *
+ * This function is supposed to be called only by BL2.
+ *
+ * Returns:
+ *	0 = success
+ *    < 0 = error
+ */
+int qemu_set_tos_fw_info(uintptr_t config_base, uintptr_t log_addr,
+			size_t log_size)
+{
+	int err = 0;
+
+	assert(config_base != 0UL);
+	assert(log_addr != 0UL);
+
+	/*
+	 * FIXME - add code to add/update Log address and it's
+	 * size in TOS FW CONFIG.
+	 * For now we don't have support for TOS FW config in OP-TEE.
+	 * So leave this function blank
+	 */
+
+	return err;
+}
+
+/*
+ * This function writes the Event Log address and its size
+ * in the QEMU DTB.
+ *
+ * This function is supposed to be called only by BL2.
+ *
+ * Returns:
+ *	0 = success
+ *    < 0 = error
+ */
+int qemu_set_nt_fw_info(
+#ifdef SPD_opteed
+			uintptr_t log_addr,
+#endif
+			size_t log_size, uintptr_t *ns_log_addr)
+{
+	uintptr_t ns_addr;
+	int err;
+
+	assert(ns_log_addr != NULL);
+
+	ns_addr = PLAT_QEMU_DT_BASE + PLAT_QEMU_DT_MAX_SIZE;
+
+	/* Write the Event Log address and its size in the DTB */
+	err = qemu_set_event_log_info(PLAT_QEMU_DT_BASE,
+#ifdef SPD_opteed
+					log_addr,
+#endif
+					ns_addr, log_size);
+
+	/* Return Event Log address in Non-secure memory */
+	*ns_log_addr = (err < 0) ? 0UL : ns_addr;
+	return err;
+}
+#endif /* MEASURED_BOOT */
diff --git a/plat/qemu/qemu/qemu_measured_boot.c b/plat/qemu/qemu/qemu_measured_boot.c
new file mode 100644
index 0000000..d9e475a
--- /dev/null
+++ b/plat/qemu/qemu/qemu_measured_boot.c
@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2022, Linaro.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <drivers/measured_boot/event_log/event_log.h>
+#include <plat/common/common_def.h>
+#include <tools_share/tbbr_oid.h>
+
+#include "../common/qemu_private.h"
+
+/* Event Log data */
+static uint8_t event_log[PLAT_EVENT_LOG_MAX_SIZE];
+static uint64_t event_log_base;
+
+/* FVP table with platform specific image IDs, names and PCRs */
+const event_log_metadata_t qemu_event_log_metadata[] = {
+	{ BL31_IMAGE_ID, EVLOG_BL31_STRING, PCR_0 },
+	{ BL32_IMAGE_ID, EVLOG_BL32_STRING, PCR_0 },
+	{ BL32_EXTRA1_IMAGE_ID, EVLOG_BL32_EXTRA1_STRING, PCR_0 },
+	{ BL32_EXTRA2_IMAGE_ID, EVLOG_BL32_EXTRA2_STRING, PCR_0 },
+	{ BL33_IMAGE_ID, EVLOG_BL33_STRING, PCR_0 },
+	{ HW_CONFIG_ID, EVLOG_HW_CONFIG_STRING, PCR_0 },
+	{ NT_FW_CONFIG_ID, EVLOG_NT_FW_CONFIG_STRING, PCR_0 },
+	{ SCP_BL2_IMAGE_ID, EVLOG_SCP_BL2_STRING, PCR_0 },
+	{ SOC_FW_CONFIG_ID, EVLOG_SOC_FW_CONFIG_STRING, PCR_0 },
+	{ TOS_FW_CONFIG_ID, EVLOG_TOS_FW_CONFIG_STRING, PCR_0 },
+
+	{ EVLOG_INVALID_ID, NULL, (unsigned int)(-1) }	/* Terminator */
+};
+
+void bl2_plat_mboot_init(void)
+{
+	/*
+	 * Here we assume that BL1/ROM code doesn't have the driver
+	 * to measure the BL2 code which is a common case for
+	 * already existing platforms
+	 */
+	event_log_init(event_log, event_log + sizeof(event_log));
+	event_log_write_header();
+
+	/*
+	 * TBD - Add code to do self measurement of BL2 code and add an
+	 * event for BL2 measurement
+	 */
+
+	event_log_base = (uintptr_t)event_log;
+}
+
+void bl2_plat_mboot_finish(void)
+{
+	int rc;
+
+	/* Event Log address in Non-Secure memory */
+	uintptr_t ns_log_addr;
+
+	/* Event Log filled size */
+	size_t event_log_cur_size;
+
+	event_log_cur_size = event_log_get_cur_size((uint8_t *)event_log_base);
+
+	rc = qemu_set_nt_fw_info(
+#ifdef SPD_opteed
+			    (uintptr_t)event_log_base,
+#endif
+			    event_log_cur_size, &ns_log_addr);
+	if (rc != 0) {
+		ERROR("%s(): Unable to update %s_FW_CONFIG\n",
+		      __func__, "NT");
+		/*
+		 * It is a fatal error because on QEMU secure world software
+		 * assumes that a valid event log exists and will use it to
+		 * record the measurements into the fTPM or sw-tpm.
+		 * Note: In QEMU platform, OP-TEE uses nt_fw_config to get the
+		 * secure Event Log buffer address.
+		 */
+		panic();
+	}
+
+	/* Copy Event Log to Non-secure memory */
+	(void)memcpy((void *)ns_log_addr, (const void *)event_log_base,
+		     event_log_cur_size);
+
+	/* Ensure that the Event Log is visible in Non-secure memory */
+	flush_dcache_range(ns_log_addr, event_log_cur_size);
+
+#if defined(SPD_tspd) || defined(SPD_spmd)
+	/* Set Event Log data in TOS_FW_CONFIG */
+	rc = qemu_set_tos_fw_info((uintptr_t)event_log_base,
+				 event_log_cur_size);
+	if (rc != 0) {
+		ERROR("%s(): Unable to update %s_FW_CONFIG\n",
+		      __func__, "TOS");
+		panic();
+	}
+#endif /* defined(SPD_tspd) || defined(SPD_spmd) */
+
+	dump_event_log((uint8_t *)event_log_base, event_log_cur_size);
+}
diff --git a/plat/st/stm32mp1/bl2_plat_setup.c b/plat/st/stm32mp1/bl2_plat_setup.c
index 88d0f8a..0d554bd 100644
--- a/plat/st/stm32mp1/bl2_plat_setup.c
+++ b/plat/st/stm32mp1/bl2_plat_setup.c
@@ -432,7 +432,8 @@
 #if !STM32MP_USE_STM32IMAGE
 	case FW_CONFIG_ID:
 		/* Set global DTB info for fixed fw_config information */
-		set_config_info(STM32MP_FW_CONFIG_BASE, STM32MP_FW_CONFIG_MAX_SIZE, FW_CONFIG_ID);
+		set_config_info(STM32MP_FW_CONFIG_BASE, ~0UL, STM32MP_FW_CONFIG_MAX_SIZE,
+				FW_CONFIG_ID);
 		fconf_populate("FW_CONFIG", STM32MP_FW_CONFIG_BASE);
 
 		idx = dyn_cfg_dtb_info_get_index(TOS_FW_CONFIG_ID);
diff --git a/plat/ti/k3/board/lite/include/board_def.h b/plat/ti/k3/board/lite/include/board_def.h
index 18b7f42..fd4e5b1 100644
--- a/plat/ti/k3/board/lite/include/board_def.h
+++ b/plat/ti/k3/board/lite/include/board_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -33,7 +33,7 @@
  * defined as default for our platform.
  */
 #define SEC_SRAM_BASE			UL(0x00000000) /* PIE remapped on fly */
-#define SEC_SRAM_SIZE			UL(0x0001c000) /* 112k */
+#define SEC_SRAM_SIZE			UL(0x00020000) /* 128k */
 
 #define PLAT_MAX_OFF_STATE		U(2)
 #define PLAT_MAX_RET_STATE		U(1)
diff --git a/plat/ti/k3/common/drivers/ti_sci/ti_sci.c b/plat/ti/k3/common/drivers/ti_sci/ti_sci.c
index 2c3313c..2cbfa3d 100644
--- a/plat/ti/k3/common/drivers/ti_sci/ti_sci.c
+++ b/plat/ti/k3/common/drivers/ti_sci/ti_sci.c
@@ -2,7 +2,7 @@
  * Texas Instruments System Control Interface Driver
  *   Based on Linux and U-Boot implementation
  *
- * Copyright (C) 2018 Texas Instruments Incorporated - http://www.ti.com/
+ * Copyright (C) 2018-2022 Texas Instruments Incorporated - https://www.ti.com/
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -1664,6 +1664,57 @@
 }
 
 /**
+ * ti_sci_enter_sleep - Command to initiate system transition into suspend.
+ *
+ * @proc_id: Processor ID.
+ * @mode: Low power mode to enter.
+ * @core_resume_addr: Address that core should be
+ *		      resumed from after low power transition.
+ *
+ * Return: 0 if all goes well, else appropriate error message
+ */
+int ti_sci_enter_sleep(uint8_t proc_id,
+		       uint8_t mode,
+		       uint64_t core_resume_addr)
+{
+	struct ti_sci_msg_req_enter_sleep req;
+	struct ti_sci_msg_hdr *hdr;
+	struct k3_sec_proxy_msg tx_message;
+	int ret;
+
+	/* Ensure we have sane transfer size */
+	if (sizeof(req) > TI_SCI_MAX_MESSAGE_SIZE) {
+		return -ERANGE;
+	}
+
+	hdr = (struct ti_sci_msg_hdr *)&req;
+	hdr->seq = ++message_sequence;
+	hdr->type = TI_SCI_MSG_ENTER_SLEEP;
+	hdr->host = TI_SCI_HOST_ID;
+	/* Setup with NORESPONSE flag to keep response queue clean */
+	hdr->flags = TI_SCI_FLAG_REQ_GENERIC_NORESPONSE;
+
+	req.processor_id = proc_id;
+	req.mode = mode;
+	req.core_resume_lo = core_resume_addr & TISCI_ADDR_LOW_MASK;
+	req.core_resume_hi = (core_resume_addr & TISCI_ADDR_HIGH_MASK) >>
+			     TISCI_ADDR_HIGH_SHIFT;
+
+	tx_message.buf = (uint8_t *)&req;
+	tx_message.len = sizeof(req);
+
+	/* Send message */
+	ret = k3_sec_proxy_send(SP_HIGH_PRIORITY, &tx_message);
+	if (ret != 0) {
+		ERROR("Message sending failed (%d)\n", ret);
+		return ret;
+	}
+
+	/* Return without waiting for response */
+	return 0;
+}
+
+/**
  * ti_sci_init() - Basic initialization
  *
  * Return: 0 if all goes well, else appropriate error message
diff --git a/plat/ti/k3/common/drivers/ti_sci/ti_sci.h b/plat/ti/k3/common/drivers/ti_sci/ti_sci.h
index c7b09b3..06944a7 100644
--- a/plat/ti/k3/common/drivers/ti_sci/ti_sci.h
+++ b/plat/ti/k3/common/drivers/ti_sci/ti_sci.h
@@ -2,7 +2,7 @@
  * Texas Instruments System Control Interface API
  *   Based on Linux and U-Boot implementation
  *
- * Copyright (C) 2018 Texas Instruments Incorporated - http://www.ti.com/
+ * Copyright (C) 2018-2022 Texas Instruments Incorporated - https://www.ti.com/
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -207,6 +207,22 @@
 					 uint32_t status_flags_1_clr_any_wait);
 
 /**
+ * System Low Power Operations
+ *
+ * - ti_sci_enter_sleep - Command to initiate system transition into suspend.
+ *		@proc_id: Processor ID.
+ *		@mode: Low power mode to enter.
+ *		@core_resume_addr: Address that core should be resumed from
+ *				   after low power transition.
+ *
+ * NOTE: for all these functions, the following are generic in nature:
+ * Returns 0 for successful request, else returns corresponding error message.
+ */
+int ti_sci_enter_sleep(uint8_t proc_id,
+		       uint8_t mode,
+		       uint64_t core_resume_addr);
+
+/**
  * ti_sci_init() - Basic initialization
  *
  * Return: 0 if all goes good, else appropriate error message.
diff --git a/plat/ti/k3/common/drivers/ti_sci/ti_sci_protocol.h b/plat/ti/k3/common/drivers/ti_sci/ti_sci_protocol.h
index 310bf45..d220612 100644
--- a/plat/ti/k3/common/drivers/ti_sci/ti_sci_protocol.h
+++ b/plat/ti/k3/common/drivers/ti_sci/ti_sci_protocol.h
@@ -5,7 +5,7 @@
  * The system works in a message response protocol
  * See: http://processors.wiki.ti.com/index.php/TISCI for details
  *
- * Copyright (C) 2018 Texas Instruments Incorporated - http://www.ti.com/
+ * Copyright (C) 2018-2022 Texas Instruments Incorporated - https://www.ti.com/
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -28,6 +28,9 @@
 #define TI_SCI_MSG_GET_DEVICE_STATE	0x0201
 #define TI_SCI_MSG_SET_DEVICE_RESETS	0x0202
 
+/* Low Power Mode Requests */
+#define TI_SCI_MSG_ENTER_SLEEP		0x0301
+
 /* Clock requests */
 #define TI_SCI_MSG_SET_CLOCK_STATE	0x0100
 #define TI_SCI_MSG_GET_CLOCK_STATE	0x0101
@@ -706,4 +709,26 @@
 	uint32_t status_flags_1_clr_any_wait;
 } __packed;
 
+/**
+ * struct ti_sci_msg_req_enter_sleep - Request for TI_SCI_MSG_ENTER_SLEEP.
+ *
+ * @hdr		    Generic Header
+ * @mode	    Low power mode to enter.
+ * @proc_id	    Processor id to be restored.
+ * @core_resume_lo  Low 32-bits of physical pointer to address for core
+ *		    to begin execution upon resume.
+ * @core_resume_hi  High 32-bits of physical pointer to address for core
+ *		    to begin execution upon resume.
+ *
+ * This message is to be sent after TI_SCI_MSG_PREPARE_SLEEP is sent from OS
+ * and is what actually triggers entry into the specified low power mode.
+ */
+struct ti_sci_msg_req_enter_sleep {
+	struct ti_sci_msg_hdr hdr;
+	uint8_t mode;
+	uint8_t processor_id;
+	uint32_t core_resume_lo;
+	uint32_t core_resume_hi;
+} __packed;
+
 #endif /* TI_SCI_PROTOCOL_H */
diff --git a/plat/ti/k3/common/k3_gicv3.c b/plat/ti/k3/common/k3_gicv3.c
index 1932eaa..0199822 100644
--- a/plat/ti/k3/common/k3_gicv3.c
+++ b/plat/ti/k3/common/k3_gicv3.c
@@ -19,6 +19,11 @@
 /* The GICv3 driver only needs to be initialized in EL3 */
 uintptr_t rdistif_base_addrs[PLATFORM_CORE_COUNT];
 
+#if K3_PM_SYSTEM_SUSPEND
+static gicv3_redist_ctx_t rdist_ctx[PLATFORM_CORE_COUNT];
+static gicv3_dist_ctx_t dist_ctx;
+#endif
+
 static const interrupt_prop_t k3_interrupt_props[] = {
 	PLAT_ARM_G1S_IRQ_PROPS(INTR_GROUP1S),
 	PLAT_ARM_G0_IRQ_PROPS(INTR_GROUP0)
@@ -88,3 +93,21 @@
 {
 	gicv3_rdistif_init(plat_my_core_pos());
 }
+
+#if K3_PM_SYSTEM_SUSPEND
+void k3_gic_save_context(void)
+{
+	for (unsigned int i = 0U; i < PLATFORM_CORE_COUNT; i++) {
+		gicv3_rdistif_save(i, &rdist_ctx[i]);
+	}
+	gicv3_distif_save(&dist_ctx);
+}
+
+void k3_gic_restore_context(void)
+{
+	gicv3_distif_init_restore(&dist_ctx);
+	for (unsigned int i = 0U; i < PLATFORM_CORE_COUNT; i++) {
+		gicv3_rdistif_init_restore(i, &rdist_ctx[i]);
+	}
+}
+#endif
diff --git a/plat/ti/k3/common/k3_psci.c b/plat/ti/k3/common/k3_psci.c
index 0500740..6febbc6 100644
--- a/plat/ti/k3/common/k3_psci.c
+++ b/plat/ti/k3/common/k3_psci.c
@@ -234,11 +234,50 @@
 	return PSCI_E_SUCCESS;
 }
 
+#if K3_PM_SYSTEM_SUSPEND
+static void k3_pwr_domain_suspend(const psci_power_state_t *target_state)
+{
+	unsigned int core, proc_id;
+
+	core = plat_my_core_pos();
+	proc_id = PLAT_PROC_START_ID + core;
+
+	/* Prevent interrupts from spuriously waking up this cpu */
+	k3_gic_cpuif_disable();
+	k3_gic_save_context();
+
+	k3_pwr_domain_off(target_state);
+
+	ti_sci_enter_sleep(proc_id, 0, k3_sec_entrypoint);
+}
+
+static void k3_pwr_domain_suspend_finish(const psci_power_state_t *target_state)
+{
+	k3_gic_restore_context();
+	k3_gic_cpuif_enable();
+}
+
+static void k3_get_sys_suspend_power_state(psci_power_state_t *req_state)
+{
+	unsigned int i;
+
+	/* CPU & cluster off, system in retention */
+	for (i = MPIDR_AFFLVL0; i <= PLAT_MAX_PWR_LVL; i++) {
+		req_state->pwr_domain_state[i] = PLAT_MAX_OFF_STATE;
+	}
+}
+#endif
+
 static const plat_psci_ops_t k3_plat_psci_ops = {
 	.cpu_standby = k3_cpu_standby,
 	.pwr_domain_on = k3_pwr_domain_on,
 	.pwr_domain_off = k3_pwr_domain_off,
 	.pwr_domain_on_finish = k3_pwr_domain_on_finish,
+#if K3_PM_SYSTEM_SUSPEND
+	.pwr_domain_suspend = k3_pwr_domain_suspend,
+	.pwr_domain_suspend_finish = k3_pwr_domain_suspend_finish,
+	.get_sys_suspend_power_state = k3_get_sys_suspend_power_state,
+#endif
 	.system_off = k3_system_off,
 	.system_reset = k3_system_reset,
 	.validate_power_state = k3_validate_power_state,
diff --git a/plat/ti/k3/common/plat_common.mk b/plat/ti/k3/common/plat_common.mk
index ab7366b..e299c30 100644
--- a/plat/ti/k3/common/plat_common.mk
+++ b/plat/ti/k3/common/plat_common.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -45,6 +45,10 @@
 K3_USART_BAUD		:=	115200
 $(eval $(call add_define,K3_USART_BAUD))
 
+# Enable system suspend modes
+K3_PM_SYSTEM_SUSPEND	:=	0
+$(eval $(call add_define,K3_PM_SYSTEM_SUSPEND))
+
 # Libraries
 include lib/xlat_tables_v2/xlat_tables.mk
 
diff --git a/plat/ti/k3/include/k3_gicv3.h b/plat/ti/k3/include/k3_gicv3.h
index 2329a16..2c68a75 100644
--- a/plat/ti/k3/include/k3_gicv3.h
+++ b/plat/ti/k3/include/k3_gicv3.h
@@ -14,5 +14,7 @@
 void k3_gic_cpuif_enable(void);
 void k3_gic_cpuif_disable(void);
 void k3_gic_pcpu_init(void);
+void k3_gic_save_context(void);
+void k3_gic_restore_context(void);
 
 #endif /* K3_GICV3_H */
diff --git a/plat/xilinx/versal/include/versal_def.h b/plat/xilinx/versal/include/versal_def.h
index 9372954..731742d 100644
--- a/plat/xilinx/versal/include/versal_def.h
+++ b/plat/xilinx/versal/include/versal_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -20,6 +20,8 @@
 
 /* List all supported platforms */
 #define VERSAL_PLATFORM_ID_versal_virt	1
+#define VERSAL_PLATFORM_ID_spp_itr6	2
+#define VERSAL_PLATFORM_ID_emu_itr6	3
 #define VERSAL_PLATFORM_ID_silicon	4
 
 #define VERSAL_PLATFORM_IS(con)	(VERSAL_PLATFORM_ID_ ## con == VERSAL_PLATFORM)
@@ -92,6 +94,16 @@
 # define VERSAL_UART_CLOCK	100000000
 # define VERSAL_UART_BAUDRATE	115200
 # define VERSAL_CPU_CLOCK	100000000
+#elif VERSAL_PLATFORM_IS(spp_itr6)
+# define PLATFORM_NAME          "SPP ITR6"
+# define VERSAL_UART_CLOCK      25000000
+# define VERSAL_UART_BAUDRATE   115200
+# define VERSAL_CPU_CLOCK       2720000
+#elif VERSAL_PLATFORM_IS(emu_itr6)
+# define PLATFORM_NAME          "EMU ITR6"
+# define VERSAL_UART_CLOCK      212000
+# define VERSAL_UART_BAUDRATE   9600
+# define VERSAL_CPU_CLOCK       212000
 #endif
 
 /* Access control register defines */
diff --git a/plat/xilinx/versal/pm_service/pm_api_sys.c b/plat/xilinx/versal/pm_service/pm_api_sys.c
index 534d910..c7b6047 100644
--- a/plat/xilinx/versal/pm_service/pm_api_sys.c
+++ b/plat/xilinx/versal/pm_service/pm_api_sys.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -25,6 +25,8 @@
 #define LOADER_MODULE_ID	0x7U
 
 #define  MODE	0x80000000U
+#define MODULE_ID_MASK		0x0000ff00
+
 /* default shutdown/reboot scope is system(2) */
 static unsigned int pm_shutdown_scope = XPM_SHUTDOWN_SUBTYPE_RST_SYSTEM;
 
@@ -73,38 +75,29 @@
 /* PM API functions */
 
 /**
- * pm_get_api_version() - Get version number of PMC PM firmware
- * @version	Returns 32-bit version number of PMC Power Management Firmware
+ * pm_handle_eemi_call() - PM call for processor to send eemi payload
  * @flag	0 - Call from secure source
  *		1 - Call from non-secure source
+ * @x0 to x5	Arguments received per SMC64 standard
+ * @result	Payload received from firmware
  *
- * @return	Returns status, either success or error+reason
+ * @return	 PM_RET_SUCCESS on success or error code
  */
-enum pm_ret_status pm_get_api_version(unsigned int *version, uint32_t flag)
+enum pm_ret_status pm_handle_eemi_call(uint32_t flag, uint32_t x0, uint32_t x1,
+				       uint32_t x2, uint32_t x3, uint32_t x4,
+				       uint32_t x5, uint64_t *result)
 {
-	uint32_t payload[PAYLOAD_ARG_CNT];
+	uint32_t payload[PAYLOAD_ARG_CNT] = {0};
+	uint32_t module_id;
 
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD1(payload, LIBPM_MODULE_ID, flag, PM_GET_API_VERSION);
-	return pm_ipi_send_sync(primary_proc, payload, version, 1);
-}
+	module_id = (x0 & MODULE_ID_MASK) >> 8;
 
-/**
- * pm_init_finalize() - Call to notify PMC PM firmware that master has power
- *			management enabled and that it has finished its
- *			initialization
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Status returned by the PMU firmware
- */
-enum pm_ret_status pm_init_finalize(uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
+	//default module id is for LIBPM
+	if (module_id == 0)
+		module_id = LIBPM_MODULE_ID;
 
-	/* Send request to the PMU */
-	PM_PACK_PAYLOAD1(payload, LIBPM_MODULE_ID, flag, PM_INIT_FINALIZE);
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
+	PM_PACK_PAYLOAD6(payload, module_id, flag, x0, x1, x2, x3, x4, x5);
+	return pm_ipi_send_sync(primary_proc, payload, (uint32_t *)result, PAYLOAD_ARG_CNT);
 }
 
 /**
@@ -235,134 +228,6 @@
 }
 
 /**
- * pm_request_device() - Request a device
- * @device_id		Device ID
- * @capabilities	Requested capabilities for the device
- * @qos			Required Quality of Service
- * @ack			Flag to specify whether acknowledge requested
- * @flag		0 - Call from secure source
- *			1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_request_device(uint32_t device_id, uint32_t capabilities,
-				     uint32_t qos, uint32_t ack, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD5(payload, LIBPM_MODULE_ID, flag, PM_REQUEST_DEVICE,
-			 device_id, capabilities, qos, ack);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_release_device() - Release a device
- * @device_id		Device ID
- * @flag		0 - Call from secure source
- *			1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_release_device(uint32_t device_id, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_RELEASE_DEVICE,
-			 device_id);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_set_requirement() - Set requirement for the device
- * @device_id		Device ID
- * @capabilities	Requested capabilities for the device
- * @latency		Requested maximum latency
- * @qos			Required Quality of Service
- * @flag		0 - Call from secure source
- *			1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_set_requirement(uint32_t device_id, uint32_t capabilities,
-				      uint32_t latency, uint32_t qos,
-				      uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD5(payload, LIBPM_MODULE_ID, flag, PM_SET_REQUIREMENT,
-			 device_id, capabilities, latency, qos);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_get_device_status() - Get device's status
- * @device_id		Device ID
- * @response		Buffer to store device status response
- * @flag		0 - Call from secure source
- *			1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_get_device_status(uint32_t device_id, uint32_t *response,
-					uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_GET_DEVICE_STATUS,
-			 device_id);
-
-	return pm_ipi_send_sync(primary_proc, payload, response, 3);
-}
-
-/**
- * pm_reset_assert() - Assert/De-assert reset
- * @reset	Reset ID
- * @assert	Assert (1) or de-assert (0)
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_reset_assert(uint32_t reset, bool assert, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD3(payload, LIBPM_MODULE_ID, flag, PM_RESET_ASSERT, reset,
-			 assert);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_reset_get_status() - Get current status of a reset line
- * @reset	Reset ID
- * @status	Returns current status of selected reset ID
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_reset_get_status(uint32_t reset, uint32_t *status,
-				       uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_RESET_ASSERT,
-			 reset);
-
-	return pm_ipi_send_sync(primary_proc, payload, status, 1);
-}
-
-/**
  * pm_get_callbackdata() - Read from IPI response buffer
  * @data - array of PAYLOAD_ARG_CNT elements
  * @flag - 0 - Call from secure source
@@ -382,294 +247,12 @@
 }
 
 /**
- * pm_pinctrl_request() - Request a pin
- * @pin		Pin ID
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_pinctrl_request(uint32_t pin, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_PINCTRL_REQUEST,
-			 pin);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_pinctrl_release() - Release a pin
- * @pin		Pin ID
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_pinctrl_release(uint32_t pin, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_PINCTRL_RELEASE,
-			 pin);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_pinctrl_set_function() - Set pin function
- * @pin		Pin ID
- * @function	Function ID
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_pinctrl_set_function(uint32_t pin, uint32_t function,
-					   uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD3(payload, LIBPM_MODULE_ID, flag,
-			 PM_PINCTRL_SET_FUNCTION, pin, function)
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_pinctrl_get_function() - Get function set on the pin
- * @pin		Pin ID
- * @function	Function set on the pin
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_pinctrl_get_function(uint32_t pin, uint32_t *function,
-					   uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag,
-			 PM_PINCTRL_SET_FUNCTION, pin);
-
-	return pm_ipi_send_sync(primary_proc, payload, function, 1);
-}
-
-/**
- * pm_pinctrl_set_pin_param() - Set configuration parameter for the pin
- * @pin		Pin ID
- * @param	Parameter ID
- * @value	Parameter value
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_pinctrl_set_pin_param(uint32_t pin, uint32_t param,
-					    uint32_t value, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD4(payload, LIBPM_MODULE_ID, flag,
-			 PM_PINCTRL_CONFIG_PARAM_SET, pin, param, value);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_pinctrl_get_pin_param() - Get configuration parameter value for the pin
- * @pin		Pin ID
- * @param	Parameter ID
- * @value	Buffer to store parameter value
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_pinctrl_get_pin_param(uint32_t pin, uint32_t param,
-					    uint32_t *value, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD3(payload, LIBPM_MODULE_ID, flag,
-			 PM_PINCTRL_CONFIG_PARAM_GET, pin, param);
-
-	return pm_ipi_send_sync(primary_proc, payload, value, 1);
-}
-
-/**
- * pm_clock_enable() - Enable the clock
- * @clk_id	Clock ID
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_clock_enable(uint32_t clk_id, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_CLOCK_ENABLE,
-			 clk_id);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_clock_disable() - Disable the clock
- * @clk_id	Clock ID
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_clock_disable(uint32_t clk_id, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_CLOCK_DISABLE,
-			 clk_id);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_clock_get_state() - Get clock status
- * @clk_id	Clock ID
- * @state:	Buffer to store clock status (1: Enabled, 0:Disabled)
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_clock_get_state(uint32_t clk_id, uint32_t *state,
-				      uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_CLOCK_GETSTATE,
-			 clk_id);
-
-	return pm_ipi_send_sync(primary_proc, payload, state, 1);
-}
-
-/**
- * pm_clock_set_divider() - Set divider for the clock
- * @clk_id	Clock ID
- * @divider	Divider value
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_clock_set_divider(uint32_t clk_id, uint32_t divider,
-					uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD3(payload, LIBPM_MODULE_ID, flag, PM_CLOCK_SETDIVIDER,
-			 clk_id, divider);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_clock_get_divider() - Get divider value for the clock
- * @clk_id	Clock ID
- * @divider:	Buffer to store clock divider value
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_clock_get_divider(uint32_t clk_id, uint32_t *divider,
-					uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_CLOCK_GETDIVIDER,
-			 clk_id);
-
-	return pm_ipi_send_sync(primary_proc, payload, divider, 1);
-}
-
-/**
- * pm_clock_set_parent() - Set parent for the clock
- * @clk_id	Clock ID
- * @parent	Parent ID
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_clock_set_parent(uint32_t clk_id, uint32_t parent,
-				       uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD3(payload, LIBPM_MODULE_ID, flag, PM_CLOCK_SETPARENT,
-			 clk_id, parent);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_clock_get_parent() - Get parent value for the clock
- * @clk_id	Clock ID
- * @parent:	Buffer to store clock parent value
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
+ * pm_pll_set_param() - Set PLL parameter
  *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_clock_get_parent(uint32_t clk_id, uint32_t *parent,
-				       uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_CLOCK_GETPARENT,
-			 clk_id);
-
-	return pm_ipi_send_sync(primary_proc, payload, parent, 1);
-}
-/**
- * pm_clock_get_rate() - Get the rate value for the clock
- * @clk_id	Clock ID
- * @rate:	Buffer to store clock rate value
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
+ * This API is deprecated and maintained here for backward compatibility.
+ * New use of this API should be avoided for versal platform.
+ * This API and its use cases will be removed for versal platform.
  *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_clock_get_rate(uint32_t clk_id, uint32_t *clk_rate,
-				     uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_CLOCK_GETRATE,
-			 clk_id);
-
-	return pm_ipi_send_sync(primary_proc, payload, clk_rate, 2);
-}
-
-/**
- * pm_pll_set_param() - Set PLL parameter
  * @clk_id	PLL clock ID
  * @param	PLL parameter ID
  * @value	Value to set for PLL parameter
@@ -692,6 +275,11 @@
 
 /**
  * pm_pll_get_param() - Get PLL parameter value
+ *
+ * This API is deprecated and maintained here for backward compatibility.
+ * New use of this API should be avoided for versal platform.
+ * This API and its use cases will be removed for versal platform.
+ *
  * @clk_id	PLL clock ID
  * @param	PLL parameter ID
  * @value:	Buffer to store PLL parameter value
@@ -714,6 +302,11 @@
 
 /**
  * pm_pll_set_mode() - Set PLL mode
+ *
+ * This API is deprecated and maintained here for backward compatibility.
+ * New use of this API should be avoided for versal platform.
+ * This API and its use cases will be removed for versal platform.
+ *
  * @clk_id	PLL clock ID
  * @mode	PLL mode
  * @flag	0 - Call from secure source
@@ -735,6 +328,11 @@
 
 /**
  * pm_pll_get_mode() - Get PLL mode
+ *
+ * This API is deprecated and maintained here for backward compatibility.
+ * New use of this API should be avoided for versal platform.
+ * This API and its use cases will be removed for versal platform.
+ *
  * @clk_id	PLL clock ID
  * @mode:	Buffer to store PLL mode
  * @flag	0 - Call from secure source
@@ -808,22 +406,28 @@
 }
 
 /**
-* pm_query_data() -  PM API for querying firmware data
-* @qid	The type of data to query
-* @arg1	Argument 1 to requested query data call
-* @arg2	Argument 2 to requested query data call
-* @arg3	Argument 3 to requested query data call
-* @data	Returned output data
-* @flag 0 - Call from secure source
-*	1 - Call from non-secure source
-*
-* This function returns requested data.
-*/
+ * pm_query_data() -  PM API for querying firmware data
+ *
+ * This API is deprecated and maintained here for backward compatibility.
+ * New use of this API should be avoided for versal platform.
+ * This API and its use cases will be removed for versal platform.
+ *
+ * @qid	The type of data to query
+ * @arg1	Argument 1 to requested query data call
+ * @arg2	Argument 2 to requested query data call
+ * @arg3	Argument 3 to requested query data call
+ * @data	Returned output data
+ * @flag 0 - Call from secure source
+ *	1 - Call from non-secure source
+ *
+ * @retur - 0 if success else non-zero error code of type
+ * enum pm_ret_status
+ */
 enum pm_ret_status pm_query_data(uint32_t qid, uint32_t arg1, uint32_t arg2,
 				 uint32_t arg3, uint32_t *data, uint32_t flag)
 {
 	uint32_t ret;
-	uint32_t version;
+	uint32_t version[PAYLOAD_ARG_CNT] = {0};
 	uint32_t payload[PAYLOAD_ARG_CNT];
 	uint32_t fw_api_version;
 
@@ -831,42 +435,50 @@
 	PM_PACK_PAYLOAD5(payload, LIBPM_MODULE_ID, flag, PM_QUERY_DATA, qid,
 			 arg1, arg2, arg3);
 
-	ret = pm_feature_check(PM_QUERY_DATA, &version, flag);
-	if (PM_RET_SUCCESS == ret) {
-		fw_api_version = version & 0xFFFFU;
-		if ((2U == fw_api_version) &&
-		    ((XPM_QID_CLOCK_GET_NAME == qid) ||
-		     (XPM_QID_PINCTRL_GET_FUNCTION_NAME == qid))) {
-			ret = pm_ipi_send_sync(primary_proc, payload, data, 8);
-			ret = data[0];
-			data[0] = data[1];
-			data[1] = data[2];
-			data[2] = data[3];
+	ret = pm_feature_check(PM_QUERY_DATA, &version[0], flag);
+	if (ret == PM_RET_SUCCESS) {
+		fw_api_version = version[0] & 0xFFFF;
+		if ((fw_api_version == 2U) &&
+		    ((qid == XPM_QID_CLOCK_GET_NAME) ||
+		     (qid == XPM_QID_PINCTRL_GET_FUNCTION_NAME))) {
+			ret = pm_ipi_send_sync(primary_proc, payload, data, PAYLOAD_ARG_CNT);
+			if (ret == PM_RET_SUCCESS) {
+				ret = data[0];
+				data[0] = data[1];
+				data[1] = data[2];
+				data[2] = data[3];
+			}
 		} else {
-			ret = pm_ipi_send_sync(primary_proc, payload, data, 4);
+			ret = pm_ipi_send_sync(primary_proc, payload, data, PAYLOAD_ARG_CNT);
 		}
 	}
 	return ret;
 }
 /**
  * pm_api_ioctl() -  PM IOCTL API for device control and configs
+ *
+ * This API is deprecated and maintained here for backward compatibility.
+ * New use of this API should be avoided for versal platform.
+ * This API and its use cases will be removed for versal platform.
+ *
  * @device_id	Device ID
  * @ioctl_id	ID of the requested IOCTL
  * @arg1	Argument 1 to requested IOCTL call
  * @arg2	Argument 2 to requested IOCTL call
+ * @arg3	Argument 3 to requested IOCTL call
  * @value	Returned output value
  * @flag	0 - Call from secure source
  *		1 - Call from non-secure source
  *
  * This function calls IOCTL to firmware for device control and configuration.
  *
- * @return	Returns status, either success or error+reason
+ * @return	Returns status, either 0 on success or non-zero error code
+ * of type enum pm_ret_status
  */
 enum pm_ret_status pm_api_ioctl(uint32_t device_id, uint32_t ioctl_id,
-				uint32_t arg1, uint32_t arg2, uint32_t *value,
-				uint32_t flag)
+				uint32_t arg1, uint32_t arg2, uint32_t arg3,
+				uint32_t *value, uint32_t flag)
 {
-	uint32_t payload[PAYLOAD_ARG_CNT];
 	enum pm_ret_status ret;
 
 	switch (ioctl_id) {
@@ -892,11 +504,7 @@
 		ret =  PM_RET_SUCCESS;
 		break;
 	default:
-		/* Send request to the PMC */
-		PM_PACK_PAYLOAD5(payload, LIBPM_MODULE_ID, flag, PM_IOCTL,
-				 device_id, ioctl_id, arg1, arg2);
-		ret =  pm_ipi_send_sync(primary_proc, payload, value, 1);
-		break;
+		return PM_RET_ERROR_NOTSUPPORTED;
 	}
 
 	return ret;
@@ -923,116 +531,48 @@
 }
 
 /**
- * pm_get_chipid() - Read silicon ID registers
- * @value       Buffer for return values. Must be large enough
- *		to hold 8 bytes.
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return      Returns silicon ID registers
- */
-enum pm_ret_status pm_get_chipid(uint32_t *value, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD1(payload, LIBPM_MODULE_ID, flag, PM_GET_CHIPID);
-
-	return pm_ipi_send_sync(primary_proc, payload, value, 2);
-}
-
-/**
  * pm_feature_check() - Returns the supported API version if supported
  * @api_id	API ID to check
- * @value	Returned supported API version
  * @flag	0 - Call from secure source
  *		1 - Call from non-secure source
+ * @ret_payload pointer to array of PAYLOAD_ARG_CNT number of
+ *		words Returned supported API version and bitmasks
+ *		for IOCTL and QUERY ID
  *
  * @return	Returns status, either success or error+reason
  */
-enum pm_ret_status pm_feature_check(uint32_t api_id, unsigned int *version,
+enum pm_ret_status pm_feature_check(uint32_t api_id, uint32_t *ret_payload,
 				    uint32_t flag)
 {
-	uint32_t payload[PAYLOAD_ARG_CNT], fw_api_version;
-	enum pm_ret_status status = PM_RET_ERROR_NOFEATURE;
+	uint32_t payload[PAYLOAD_ARG_CNT];
+	uint32_t module_id;
 
+	/* Return version of API which are implemented in ATF only */
 	switch (api_id) {
 	case PM_GET_CALLBACK_DATA:
 	case PM_GET_TRUSTZONE_VERSION:
+		ret_payload[0] = PM_API_VERSION_2;
+		return PM_RET_SUCCESS;
 	case PM_LOAD_PDI:
-		*version = (PM_API_BASE_VERSION << 16);
-		status = PM_RET_SUCCESS;
-		break;
-	case PM_GET_API_VERSION:
-	case PM_GET_DEVICE_STATUS:
-	case PM_GET_OP_CHARACTERISTIC:
-	case PM_REQ_SUSPEND:
-	case PM_SELF_SUSPEND:
-	case PM_FORCE_POWERDOWN:
-	case PM_ABORT_SUSPEND:
-	case PM_REQ_WAKEUP:
-	case PM_SET_WAKEUP_SOURCE:
-	case PM_SYSTEM_SHUTDOWN:
-	case PM_REQUEST_DEVICE:
-	case PM_RELEASE_DEVICE:
-	case PM_SET_REQUIREMENT:
-	case PM_RESET_ASSERT:
-	case PM_RESET_GET_STATUS:
-	case PM_GET_CHIPID:
-	case PM_PINCTRL_REQUEST:
-	case PM_PINCTRL_RELEASE:
-	case PM_PINCTRL_GET_FUNCTION:
-	case PM_PINCTRL_SET_FUNCTION:
-	case PM_PINCTRL_CONFIG_PARAM_GET:
-	case PM_PINCTRL_CONFIG_PARAM_SET:
-	case PM_IOCTL:
-	case PM_CLOCK_ENABLE:
-	case PM_CLOCK_DISABLE:
-	case PM_CLOCK_GETSTATE:
-	case PM_CLOCK_SETDIVIDER:
-	case PM_CLOCK_GETDIVIDER:
-	case PM_CLOCK_SETPARENT:
-	case PM_CLOCK_GETPARENT:
-	case PM_CLOCK_GETRATE:
-	case PM_PLL_SET_PARAMETER:
-	case PM_PLL_GET_PARAMETER:
-	case PM_PLL_SET_MODE:
-	case PM_PLL_GET_MODE:
-	case PM_FEATURE_CHECK:
-	case PM_INIT_FINALIZE:
-	case PM_SET_MAX_LATENCY:
-	case PM_REGISTER_NOTIFIER:
-		*version = (PM_API_BASE_VERSION << 16);
-		status = PM_RET_SUCCESS;
-		break;
-	case PM_QUERY_DATA:
-		*version = (PM_API_QUERY_DATA_VERSION << 16);
-		status = PM_RET_SUCCESS;
-		break;
+		ret_payload[0] = PM_API_BASE_VERSION;
+		return PM_RET_SUCCESS;
 	default:
-		*version = 0U;
-		status = PM_RET_ERROR_NOFEATURE;
 		break;
 	}
 
-	if (status != PM_RET_SUCCESS) {
-		goto done;
+	module_id = (api_id & MODULE_ID_MASK) >> 8;
+
+	/*
+	 * feature check should be done only for LIBPM module
+	 * If module_id is 0, then we consider it LIBPM module as default id
+	 */
+	if ((module_id > 0) && (module_id != LIBPM_MODULE_ID)) {
+		return PM_RET_SUCCESS;
 	}
 
 	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag,
 			 PM_FEATURE_CHECK, api_id);
-
-	status = pm_ipi_send_sync(primary_proc, payload, &fw_api_version, 1);
-	if (status != PM_RET_SUCCESS) {
-		goto done;
-	}
-
-	*version |= fw_api_version;
-
-	status = PM_RET_SUCCESS;
-
-done:
-	return status;
+	return pm_ipi_send_sync(primary_proc, payload, ret_payload, PAYLOAD_ARG_CNT);
 }
 
 /**
@@ -1060,54 +600,6 @@
 }
 
 /**
- * pm_get_op_characteristic() - PM call to request operating characteristics
- *                              of a device
- * @device_id   Device id
- * @type        Type of the operating characteristic
- *              (power, temperature and latency)
- * @result      Returns the operating characteristic for the requested device,
- *              specified by the type
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return      Returns status, either success or error+reason
- */
-enum pm_ret_status pm_get_op_characteristic(uint32_t device_id,
-					    enum pm_opchar_type type,
-					    uint32_t *result, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD3(payload, LIBPM_MODULE_ID, flag,
-			 PM_GET_OP_CHARACTERISTIC, device_id, type);
-	return pm_ipi_send_sync(primary_proc, payload, result, 1);
-}
-
-/**
- * pm_set_max_latency() - PM call to change in the maximum wake-up latency
- *			  requirements for a specific device currently
- *			  used by that CPU.
- * @device_id	Device ID
- * @latency	Latency value
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_set_max_latency(uint32_t device_id, uint32_t latency,
-				      uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD3(payload, LIBPM_MODULE_ID, flag, PM_SET_MAX_LATENCY,
-			 device_id, latency);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
  * pm_register_notifier() - PM call to register a subsystem to be notified
  * 			    about the device event
  * @device_id	Device ID for the Node to which the event is related
diff --git a/plat/xilinx/versal/pm_service/pm_api_sys.h b/plat/xilinx/versal/pm_service/pm_api_sys.h
index 5a92704..86a46d0 100644
--- a/plat/xilinx/versal/pm_service/pm_api_sys.h
+++ b/plat/xilinx/versal/pm_service/pm_api_sys.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,8 +14,9 @@
  * PM API function declarations
  **********************************************************/
 
-enum pm_ret_status pm_get_api_version(unsigned int *version, uint32_t flag);
-enum pm_ret_status pm_init_finalize(uint32_t flag);
+enum pm_ret_status pm_handle_eemi_call(uint32_t flag, uint32_t x0, uint32_t x1,
+				       uint32_t x2, uint32_t x3, uint32_t x4,
+				       uint32_t x5, uint64_t *result);
 enum pm_ret_status pm_self_suspend(uint32_t nid,
 				   unsigned int latency,
 				   unsigned int state,
@@ -29,42 +30,7 @@
 				 uintptr_t address, uint8_t ack, uint32_t flag);
 enum pm_ret_status pm_set_wakeup_source(uint32_t target, uint32_t device_id,
 					uint8_t enable, uint32_t flag);
-enum pm_ret_status pm_request_device(uint32_t device_id, uint32_t capabilities,
-				     uint32_t qos, uint32_t ack, uint32_t flag);
-enum pm_ret_status pm_release_device(uint32_t device_id, uint32_t flag);
-enum pm_ret_status pm_set_requirement(uint32_t device_id, uint32_t capabilities,
-				      uint32_t latency, uint32_t qos,
-				      uint32_t flag);
-enum pm_ret_status pm_get_device_status(uint32_t device_id, uint32_t *response,
-					uint32_t flag);
-enum pm_ret_status pm_reset_assert(uint32_t reset, bool assert, uint32_t flag);
-enum pm_ret_status pm_reset_get_status(uint32_t reset, uint32_t *status,
-				       uint32_t flag);
 void pm_get_callbackdata(uint32_t *data, size_t count, uint32_t flag);
-enum pm_ret_status pm_pinctrl_request(uint32_t pin, uint32_t flag);
-enum pm_ret_status pm_pinctrl_release(uint32_t pin, uint32_t flag);
-enum pm_ret_status pm_pinctrl_set_function(uint32_t pin, uint32_t function,
-					   uint32_t flag);
-enum pm_ret_status pm_pinctrl_get_function(uint32_t pin, uint32_t *function,
-					   uint32_t flag);
-enum pm_ret_status pm_pinctrl_set_pin_param(uint32_t pin, uint32_t param,
-					    uint32_t value, uint32_t flag);
-enum pm_ret_status pm_pinctrl_get_pin_param(uint32_t pin, uint32_t param,
-					    uint32_t *value, uint32_t flag);
-enum pm_ret_status pm_clock_enable(uint32_t clk_id, uint32_t flag);
-enum pm_ret_status pm_clock_disable(uint32_t clk_id, uint32_t flag);
-enum pm_ret_status pm_clock_get_state(uint32_t clk_id, uint32_t *state,
-				      uint32_t flag);
-enum pm_ret_status pm_clock_set_divider(uint32_t clk_id, uint32_t divider,
-					uint32_t flag);
-enum pm_ret_status pm_clock_get_divider(uint32_t clk_id, uint32_t *divider,
-					uint32_t flag);
-enum pm_ret_status pm_clock_set_parent(uint32_t clk_id, uint32_t parent,
-				       uint32_t flag);
-enum pm_ret_status pm_clock_get_parent(uint32_t clk_id, uint32_t *parent,
-				       uint32_t flag);
-enum pm_ret_status pm_clock_get_rate(uint32_t clk_id, uint32_t *clk_rate,
-				     uint32_t flag);
 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,
@@ -78,21 +44,15 @@
 enum pm_ret_status pm_system_shutdown(uint32_t type, uint32_t subtype,
 				      uint32_t flag);
 enum pm_ret_status pm_api_ioctl(uint32_t device_id, uint32_t ioctl_id,
-				uint32_t arg1, uint32_t arg2, uint32_t *value,
-				uint32_t flag);
+				uint32_t arg1, uint32_t arg2, uint32_t arg3,
+				uint32_t *value, uint32_t flag);
 enum pm_ret_status pm_query_data(uint32_t qid, uint32_t arg1, uint32_t arg2,
 				 uint32_t arg3, uint32_t *data, uint32_t flag);
 unsigned int pm_get_shutdown_scope(void);
-enum pm_ret_status pm_get_chipid(uint32_t *value, uint32_t flag);
-enum pm_ret_status pm_feature_check(uint32_t api_id, unsigned int *version,
+enum pm_ret_status pm_feature_check(uint32_t api_id, uint32_t *ret_payload,
 				    uint32_t flag);
 enum pm_ret_status pm_load_pdi(uint32_t src, uint32_t address_low,
 			       uint32_t address_high, uint32_t flag);
-enum pm_ret_status pm_get_op_characteristic(uint32_t device_id,
-					    enum pm_opchar_type type,
-					    uint32_t *result, uint32_t flag);
-enum pm_ret_status pm_set_max_latency(uint32_t device_id, uint32_t latency,
-				      uint32_t flag);
 enum pm_ret_status pm_register_notifier(uint32_t device_id, uint32_t event,
 					uint32_t wake, uint32_t enable,
 					uint32_t flag);
diff --git a/plat/xilinx/versal/pm_service/pm_client.c b/plat/xilinx/versal/pm_service/pm_client.c
index 4c1d340..4012f32 100644
--- a/plat/xilinx/versal/pm_service/pm_client.c
+++ b/plat/xilinx/versal/pm_service/pm_client.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -21,6 +21,7 @@
 #include <plat/common/platform.h>
 #include "pm_api_sys.h"
 #include "pm_client.h"
+#include "pm_defs.h"
 
 #define UNDEFINED_CPUID		(~0)
 #define IRQ_MAX		142U
@@ -147,14 +148,16 @@
 			node_idx = irq_to_pm_node_idx(irq);
 			reg &= ~lowest_set;
 
-			if ((node_idx != XPM_NODEIDX_DEV_MIN) &&
-			    (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] = (uint8_t)(!ret);
+			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;
+				}
 			}
 		}
 	}
diff --git a/plat/xilinx/versal/pm_service/pm_defs.h b/plat/xilinx/versal/pm_service/pm_defs.h
index 08b46e2..3785650 100644
--- a/plat/xilinx/versal/pm_service/pm_defs.h
+++ b/plat/xilinx/versal/pm_service/pm_defs.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -38,13 +38,11 @@
 
 /* PM API Versions */
 #define PM_API_BASE_VERSION		1U
+#define PM_API_VERSION_2                2U
 
 #define PM_API_QUERY_DATA_VERSION	2U
 
 /* PM API ids */
-#define PM_GET_API_VERSION		1U
-#define PM_GET_DEVICE_STATUS		3U
-#define PM_GET_OP_CHARACTERISTIC	4U
 #define PM_REGISTER_NOTIFIER		5U
 #define PM_REQ_SUSPEND			6U
 #define PM_SELF_SUSPEND			7U
@@ -53,31 +51,8 @@
 #define PM_REQ_WAKEUP			10U
 #define PM_SET_WAKEUP_SOURCE		11U
 #define PM_SYSTEM_SHUTDOWN		12U
-#define PM_REQUEST_DEVICE		13U
-#define PM_RELEASE_DEVICE		14U
-#define PM_SET_REQUIREMENT		15U
-#define PM_SET_MAX_LATENCY		16U
-#define PM_RESET_ASSERT			17U
-#define PM_RESET_GET_STATUS		18U
-#define PM_INIT_FINALIZE		21U
-#define PM_GET_CHIPID			24U
-#define	PM_PINCTRL_REQUEST		28U
-#define	PM_PINCTRL_RELEASE		29U
-#define	PM_PINCTRL_GET_FUNCTION		30U
-#define	PM_PINCTRL_SET_FUNCTION		31U
-#define	PM_PINCTRL_CONFIG_PARAM_GET	32U
-#define	PM_PINCTRL_CONFIG_PARAM_SET	33U
 #define PM_IOCTL			34U
 #define PM_QUERY_DATA			35U
-#define PM_CLOCK_ENABLE			36U
-#define PM_CLOCK_DISABLE		37U
-#define PM_CLOCK_GETSTATE		38U
-#define PM_CLOCK_SETDIVIDER		39U
-#define PM_CLOCK_GETDIVIDER		40U
-#define PM_CLOCK_SETRATE		41U
-#define PM_CLOCK_GETRATE		42U
-#define PM_CLOCK_SETPARENT		43U
-#define PM_CLOCK_GETPARENT		44U
 #define PM_PLL_SET_PARAMETER		48U
 #define PM_PLL_GET_PARAMETER		49U
 #define PM_PLL_SET_MODE			50U
diff --git a/plat/xilinx/versal/pm_service/pm_svc_main.c b/plat/xilinx/versal/pm_service/pm_svc_main.c
index b082acb..75c1268 100644
--- a/plat/xilinx/versal/pm_service/pm_svc_main.c
+++ b/plat/xilinx/versal/pm_service/pm_svc_main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -114,49 +114,84 @@
 }
 
 /**
- * pm_smc_handler() - SMC handler for PM-API calls coming from EL1/EL2.
- * @smc_fid - Function Identifier
- * @x1 - x4 - Arguments
- * @cookie  - Unused
- * @handler - Pointer to caller's context structure
+ * eemi_for_compatibility() - EEMI calls handler for deprecated calls
  *
- * @return  - Unused
- *
- * Determines that smc_fid is valid and supported PM SMC Function ID from the
- * list of pm_api_ids, otherwise completes the request with
- * the unknown SMC Function ID
+ * @return - If EEMI API found then, uintptr_t type address, else 0
  *
- * The SMC calls for PM service are forwarded from SIP Service SMC handler
- * function with rt_svc_handle signature
+ * Some EEMI API's use case needs to be changed in Linux driver, so they
+ * can take advantage of common EEMI handler in TF-A. As of now the old
+ * implementation of these APIs are required to maintain backward compatibility
+ * until their use case in linux driver changes.
  */
-uint64_t pm_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3,
-			uint64_t x4, void *cookie, void *handle, uint64_t flags)
+static uintptr_t eemi_for_compatibility(uint32_t api_id, uint32_t *pm_arg,
+					void *handle, uint32_t security_flag)
 {
 	enum pm_ret_status ret;
 
-	uint32_t pm_arg[4];
-	uint32_t security_flag = SECURE_FLAG;
+	switch (api_id) {
 
-	/* Handle case where PM wasn't initialized properly */
-	if (pm_up == false) {
-		SMC_RET1(handle, SMC_UNK);
+	case PM_IOCTL:
+	{
+		uint32_t value;
+
+		ret = pm_api_ioctl(pm_arg[0], pm_arg[1], pm_arg[2],
+				   pm_arg[3], pm_arg[4],
+				   &value, security_flag);
+		if (ret == PM_RET_ERROR_NOTSUPPORTED)
+			return (uintptr_t)0;
+
+		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32);
 	}
 
-	pm_arg[0] = (uint32_t)x1;
-	pm_arg[1] = (uint32_t)(x1 >> 32);
-	pm_arg[2] = (uint32_t)x2;
-	pm_arg[3] = (uint32_t)(x2 >> 32);
+	case PM_QUERY_DATA:
+	{
+		uint32_t data[PAYLOAD_ARG_CNT] = { 0 };
 
-	/*
-	 * Mark BIT24 payload (i.e 1st bit of pm_arg[3] ) as non-secure (1)
-	 * if smc called is non secure
-	 */
-	if (is_caller_non_secure(flags) != 0) {
-		security_flag = NON_SECURE_FLAG;
+		ret = pm_query_data(pm_arg[0], pm_arg[1], pm_arg[2],
+				      pm_arg[3], data, security_flag);
+
+		SMC_RET2(handle, (uint64_t)ret  | ((uint64_t)data[0] << 32),
+				 (uint64_t)data[1] | ((uint64_t)data[2] << 32));
+	}
+
+	case PM_FEATURE_CHECK:
+	{
+		uint32_t result[PAYLOAD_ARG_CNT] = {0U};
+
+		ret = pm_feature_check(pm_arg[0], result, security_flag);
+		SMC_RET2(handle, (uint64_t)ret | ((uint64_t)result[0] << 32),
+			 (uint64_t)result[1] | ((uint64_t)result[2] << 32));
+	}
+
+	case PM_LOAD_PDI:
+	{
+		ret = pm_load_pdi(pm_arg[0], pm_arg[1], pm_arg[2],
+				  security_flag);
+		SMC_RET1(handle, (uint64_t)ret);
+	}
+
+	default:
+		return (uintptr_t)0;
 	}
+}
 
-	switch (smc_fid & FUNCID_NUM_MASK) {
-	/* PM API Functions */
+/**
+ * eemi_psci_debugfs_handler() - EEMI API invoked from PSCI
+ *
+ * These EEMI APIs performs CPU specific power management tasks.
+ * These EEMI APIs are invoked either from PSCI or from debugfs in kernel.
+ * These calls require CPU specific processing before sending IPI request to
+ * Platform Management Controller. For example enable/disable CPU specific
+ * interrupts. This requires separate handler for these calls and may not be
+ * handled using common eemi handler
+ */
+static uintptr_t eemi_psci_debugfs_handler(uint32_t api_id, uint32_t *pm_arg,
+					   void *handle, uint32_t security_flag)
+{
+	enum pm_ret_status ret;
+
+	switch (api_id) {
+
 	case PM_SELF_SUSPEND:
 		ret = pm_self_suspend(pm_arg[0], pm_arg[1], pm_arg[2],
 				      pm_arg[3], security_flag);
@@ -179,65 +214,22 @@
 		ret = pm_system_shutdown(pm_arg[0], pm_arg[1], security_flag);
 		SMC_RET1(handle, (uint64_t)ret);
 
-	case PM_REQ_WAKEUP:
-		ret = pm_req_wakeup(pm_arg[0], pm_arg[1], pm_arg[2], pm_arg[3],
-				    security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_SET_WAKEUP_SOURCE:
-		ret = pm_set_wakeup_source(pm_arg[0], pm_arg[1], pm_arg[2],
-					   security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_REQUEST_DEVICE:
-		ret = pm_request_device(pm_arg[0], pm_arg[1], pm_arg[2],
-					pm_arg[3], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_RELEASE_DEVICE:
-		ret = pm_release_device(pm_arg[0], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_SET_REQUIREMENT:
-		ret = pm_set_requirement(pm_arg[0], pm_arg[1], pm_arg[2],
-					 pm_arg[3], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_GET_API_VERSION:
-	{
-		uint32_t api_version;
-
-		ret = pm_get_api_version(&api_version, security_flag);
-		SMC_RET1(handle, (u_register_t)PM_RET_SUCCESS |
-				 ((u_register_t)api_version << 32));
-	}
-
-	case PM_GET_DEVICE_STATUS:
-	{
-		uint32_t buff[3];
-
-		ret = pm_get_device_status(pm_arg[0], buff, security_flag);
-		SMC_RET2(handle, (u_register_t)ret | ((u_register_t)buff[0] << 32),
-			 (u_register_t)buff[1] | ((u_register_t)buff[2] << 32));
-	}
-
-	case PM_RESET_ASSERT:
-		ret = pm_reset_assert(pm_arg[0], pm_arg[1], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_RESET_GET_STATUS:
-	{
-		uint32_t reset_status;
-
-		ret = pm_reset_get_status(pm_arg[0], &reset_status,
-					  security_flag);
-		SMC_RET1(handle, (u_register_t)ret |
-			 ((u_register_t)reset_status << 32));
+	default:
+		return (uintptr_t)0;
 	}
+}
 
-	case PM_INIT_FINALIZE:
-		ret = pm_init_finalize(security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
+/**
+ * TF_A_specific_handler() - SMC handler for TF-A specific functionality
+ *
+ * These EEMI calls performs functionality that does not require
+ * IPI transaction. The handler ends in TF-A and returns requested data to
+ * kernel from TF-A
+ */
+static uintptr_t TF_A_specific_handler(uint32_t api_id, uint32_t *pm_arg,
+				       void *handle, uint32_t security_flag)
+{
+	switch (api_id) {
 
 	case PM_GET_CALLBACK_DATA:
 	{
@@ -245,192 +237,115 @@
 
 		pm_get_callbackdata(result, ARRAY_SIZE(result), security_flag);
 		SMC_RET2(handle,
-			 (u_register_t)result[0] | ((u_register_t)result[1] << 32),
-			 (u_register_t)result[2] | ((u_register_t)result[3] << 32));
-	}
-
-	case PM_PINCTRL_REQUEST:
-		ret = pm_pinctrl_request(pm_arg[0], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_PINCTRL_RELEASE:
-		ret = pm_pinctrl_release(pm_arg[0], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_PINCTRL_GET_FUNCTION:
-	{
-		uint32_t value = 0;
-
-		ret = pm_pinctrl_get_function(pm_arg[0], &value, security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)value) << 32);
+			(uint64_t)result[0] | ((uint64_t)result[1] << 32),
+			(uint64_t)result[2] | ((uint64_t)result[3] << 32));
 	}
 
-	case PM_PINCTRL_SET_FUNCTION:
-		ret = pm_pinctrl_set_function(pm_arg[0], pm_arg[1],
-					      security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_PINCTRL_CONFIG_PARAM_GET:
-	{
-		uint32_t value;
-
-		ret = pm_pinctrl_get_pin_param(pm_arg[0], pm_arg[1], &value,
-					       security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)value) << 32);
-	}
-
-	case PM_PINCTRL_CONFIG_PARAM_SET:
-		ret = pm_pinctrl_set_pin_param(pm_arg[0], pm_arg[1], pm_arg[2],
-					       security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_IOCTL:
-	{
-		uint32_t value;
-
-		ret = pm_api_ioctl(pm_arg[0], pm_arg[1], pm_arg[2],
-				   pm_arg[3], &value, security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)value) << 32);
-	}
-
-	case PM_QUERY_DATA:
-	{
-		uint32_t data[8] = { 0 };
-
-		ret = pm_query_data(pm_arg[0], pm_arg[1], pm_arg[2],
-				      pm_arg[3], data, security_flag);
-
-		SMC_RET2(handle, (u_register_t)ret  | ((u_register_t)data[0] << 32),
-				 (u_register_t)data[1] | ((u_register_t)data[2] << 32));
-
-	}
-	case PM_CLOCK_ENABLE:
-		ret = pm_clock_enable(pm_arg[0], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_CLOCK_DISABLE:
-		ret = pm_clock_disable(pm_arg[0], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_CLOCK_GETSTATE:
-	{
-		uint32_t value;
-
-		ret = pm_clock_get_state(pm_arg[0], &value, security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)value) << 32);
-	}
-
-	case PM_CLOCK_SETDIVIDER:
-		ret = pm_clock_set_divider(pm_arg[0], pm_arg[1], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_CLOCK_GETDIVIDER:
-	{
-		uint32_t value;
-
-		ret = pm_clock_get_divider(pm_arg[0], &value, security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)value) << 32);
-	}
-
-	case PM_CLOCK_SETPARENT:
-		ret = pm_clock_set_parent(pm_arg[0], pm_arg[1], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_CLOCK_GETPARENT:
-	{
-		uint32_t value;
-
-		ret = pm_clock_get_parent(pm_arg[0], &value, security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)value) << 32);
-	}
-
-	case PM_CLOCK_GETRATE:
-	{
-		uint32_t rate[2] = { 0 };
-
-		ret = pm_clock_get_rate(pm_arg[0], rate, security_flag);
-		SMC_RET2(handle, (u_register_t)ret | ((u_register_t)rate[0] << 32),
-			 (u_register_t)rate[1] | ((u_register_t)0U << 32));
-	}
-
-	case PM_PLL_SET_PARAMETER:
-		ret = pm_pll_set_param(pm_arg[0], pm_arg[1], pm_arg[2],
-				       security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_PLL_GET_PARAMETER:
-	{
-		uint32_t value;
+	case PM_GET_TRUSTZONE_VERSION:
+		SMC_RET1(handle, (uint64_t)PM_RET_SUCCESS |
+			 ((uint64_t)VERSAL_TZ_VERSION << 32));
 
-		ret = pm_pll_get_param(pm_arg[0], pm_arg[1], &value,
-				       security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)value << 32));
+	default:
+		return (uintptr_t)0;
 	}
-
-	case PM_PLL_SET_MODE:
-		ret = pm_pll_set_mode(pm_arg[0], pm_arg[1], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
+}
 
-	case PM_PLL_GET_MODE:
-	{
-		uint32_t mode;
+/**
+ * eemi_handler() - Prepare EEMI payload and perform IPI transaction
+ *
+ * EEMI - Embedded Energy Management Interface is Xilinx proprietary protocol
+ * to allow communication between power management controller and different
+ * processing clusters.
+ *
+ * This handler prepares EEMI protocol payload received from kernel and performs
+ * IPI transaction.
+ */
+static uintptr_t eemi_handler(uint32_t api_id, uint32_t *pm_arg,
+			      void *handle, uint32_t security_flag)
+{
+	enum pm_ret_status ret;
+	uint32_t buf[PAYLOAD_ARG_CNT] = {0};
 
-		ret = pm_pll_get_mode(pm_arg[0], &mode, security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)mode << 32));
+	ret = pm_handle_eemi_call(security_flag, api_id, pm_arg[0], pm_arg[1],
+				  pm_arg[2], pm_arg[3], pm_arg[4],
+				  (uint64_t *)buf);
+	/*
+	 * Two IOCTLs, to get clock name and pinctrl name of pm_query_data API
+	 * receives 5 words of respoonse from firmware. Currently linux driver can
+	 * 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 ((pm_arg[0] == XPM_QID_CLOCK_GET_NAME ||
+		    pm_arg[0] == XPM_QID_PINCTRL_GET_FUNCTION_NAME) &&
+		    ret == PM_RET_SUCCESS) {
+			SMC_RET2(handle, (uint64_t)buf[0] | ((uint64_t)buf[1] << 32),
+				(uint64_t)buf[2] | ((uint64_t)buf[3] << 32));
+		}
 	}
 
-	case PM_GET_TRUSTZONE_VERSION:
-		SMC_RET1(handle, (uint64_t)PM_RET_SUCCESS |
-			 ((uint64_t)VERSAL_TZ_VERSION << 32));
-
-	case PM_GET_CHIPID:
-	{
-		uint32_t result[2];
+	SMC_RET2(handle, (uint64_t)ret | ((uint64_t)buf[0] << 32),
+		 (uint64_t)buf[1] | ((uint64_t)buf[2] << 32));
+}
 
-		ret = pm_get_chipid(result, security_flag);
-		SMC_RET2(handle, (u_register_t)ret | ((u_register_t)result[0] << 32),
-			 (u_register_t)result[1] | ((u_register_t)0U << 32));
-	}
+/**
+ * pm_smc_handler() - SMC handler for PM-API calls coming from EL1/EL2.
+ * @smc_fid - Function Identifier
+ * @x1 - x4 - SMC64 Arguments from kernel
+ *	      x3 (upper 32-bits) and x4 are Unused
+ * @cookie  - Unused
+ * @handler - Pointer to caller's context structure
+ *
+ * @return  - Unused
+ *
+ * Determines that smc_fid is valid and supported PM SMC Function ID from the
+ * list of pm_api_ids, otherwise completes the request with
+ * the unknown SMC Function ID
+ *
+ * The SMC calls for PM service are forwarded from SIP Service SMC handler
+ * function with rt_svc_handle signature
+ */
+uint64_t pm_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3,
+			uint64_t x4, void *cookie, void *handle, uint64_t flags)
+{
+	uintptr_t ret;
+	uint32_t pm_arg[PAYLOAD_ARG_CNT] = {0};
+	uint32_t security_flag = SECURE_FLAG;
+	uint32_t api_id;
 
-	case PM_FEATURE_CHECK:
-	{
-		uint32_t version;
+	/* Handle case where PM wasn't initialized properly */
+	if (!pm_up)
+		SMC_RET1(handle, SMC_UNK);
 
-		ret = pm_feature_check(pm_arg[0], &version, security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)version << 32));
+	/*
+	 * Mark BIT24 payload (i.e 1st bit of pm_arg[3] ) as non-secure (1)
+	 * if smc called is non secure
+	 */
+	if (is_caller_non_secure(flags)) {
+		security_flag = NON_SECURE_FLAG;
 	}
 
-	case PM_LOAD_PDI:
-	{
-		ret = pm_load_pdi(pm_arg[0], pm_arg[1], pm_arg[2],
-				  security_flag);
-		SMC_RET1(handle, (u_register_t)ret);
-	}
+	pm_arg[0] = (uint32_t)x1;
+	pm_arg[1] = (uint32_t)(x1 >> 32);
+	pm_arg[2] = (uint32_t)x2;
+	pm_arg[3] = (uint32_t)(x2 >> 32);
+	pm_arg[4] = (uint32_t)x3;
+	(void)(x4);
+	api_id = smc_fid & FUNCID_NUM_MASK;
 
-	case PM_GET_OP_CHARACTERISTIC:
-	{
-		uint32_t result;
+	ret = eemi_for_compatibility(api_id, pm_arg, handle, security_flag);
+	if (ret != (uintptr_t)0)
+		return ret;
 
-		ret = pm_get_op_characteristic(pm_arg[0], pm_arg[1], &result,
-					       security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)result << 32));
-	}
+	ret = eemi_psci_debugfs_handler(api_id, pm_arg, handle, flags);
+	if (ret !=  (uintptr_t)0)
+		return ret;
 
-	case PM_SET_MAX_LATENCY:
-	{
-		ret = pm_set_max_latency(pm_arg[0], pm_arg[1], security_flag);
-		SMC_RET1(handle, (u_register_t)ret);
-	}
+	ret = TF_A_specific_handler(api_id, pm_arg, handle, security_flag);
+	if (ret !=  (uintptr_t)0)
+		return ret;
 
-	case PM_REGISTER_NOTIFIER:
-	{
-		ret = pm_register_notifier(pm_arg[0], pm_arg[1], pm_arg[2],
-					   pm_arg[3], security_flag);
-		SMC_RET1(handle, (u_register_t)ret);
-	}
+	ret = eemi_handler(api_id, pm_arg, handle, security_flag);
 
-	default:
-		WARN("Unimplemented PM Service Call: 0x%x\n", smc_fid);
-		SMC_RET1(handle, SMC_UNK);
-	}
+	return ret;
 }
diff --git a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
index fae73cf..5bfc2eb 100644
--- a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
+++ b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -329,8 +329,8 @@
 		break;
 	}
 
-	NOTICE("TF-A running on %s/%s at 0x%x\n",
-	       zynqmp_print_silicon_idcode(), label, BL31_BASE);
+	VERBOSE("TF-A running on %s/%s at 0x%x\n",
+		zynqmp_print_silicon_idcode(), label, BL31_BASE);
 	VERBOSE("TF-A running on v%d/RTL%d.%d\n",
 	       zynqmp_get_ps_ver(), (rtl & 0xf0) >> 4, rtl & 0xf);
 }