diff --git a/Makefile b/Makefile
index 6d7865a..f4d623e 100644
--- a/Makefile
+++ b/Makefile
@@ -552,9 +552,7 @@
         SPD_DIR := std_svc
 
         ifeq ($(SPMD_SPM_AT_SEL2),1)
-            ifeq ($(CTX_INCLUDE_EL2_REGS),0)
-                $(error SPMD with SPM at S-EL2 requires CTX_INCLUDE_EL2_REGS option)
-            endif
+            CTX_INCLUDE_EL2_REGS := 1
 	    ifeq ($(SPMC_AT_EL3),1)
                 $(error SPM cannot be enabled in both S-EL2 and EL3.)
             endif
@@ -600,6 +598,14 @@
     # over the sources.
 endif
 
+ifeq (${CTX_INCLUDE_EL2_REGS}, 1)
+ifeq (${SPD},none)
+ifeq (${ENABLE_RME},0)
+    $(error CTX_INCLUDE_EL2_REGS is available only when SPD or RME is enabled)
+endif
+endif
+endif
+
 ################################################################################
 # Include rmmd Makefile if RME is enabled
 ################################################################################
diff --git a/bl31/aarch64/runtime_exceptions.S b/bl31/aarch64/runtime_exceptions.S
index 0283553..c9c3da9 100644
--- a/bl31/aarch64/runtime_exceptions.S
+++ b/bl31/aarch64/runtime_exceptions.S
@@ -10,6 +10,7 @@
 #include <asm_macros.S>
 #include <bl31/ea_handle.h>
 #include <bl31/interrupt_mgmt.h>
+#include <bl31/sync_handle.h>
 #include <common/runtime_svc.h>
 #include <context.h>
 #include <el3_common_macros.S>
@@ -191,7 +192,10 @@
 	b.eq	smc_handler32
 
 	cmp	x30, #EC_AARCH64_SMC
-	b.eq	smc_handler64
+	b.eq	sync_handler64
+
+	cmp	x30, #EC_AARCH64_SYS
+	b.eq	sync_handler64
 
 	/* Synchronous exceptions other than the above are assumed to be EA */
 	ldr	x30, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_LR]
@@ -452,12 +456,12 @@
 	 * Note that x30 has been explicitly saved and can be used here
 	 * ---------------------------------------------------------------------
 	 */
-func smc_handler
+func sync_exception_handler
 smc_handler32:
 	/* Check whether aarch32 issued an SMC64 */
 	tbnz	x0, #FUNCID_CC_SHIFT, smc_prohibited
 
-smc_handler64:
+sync_handler64:
 	/* NOTE: The code below must preserve x0-x4 */
 
 	/*
@@ -493,15 +497,22 @@
 	msr	spsel, #MODE_SP_EL0
 
 	/*
-	 * Save the SPSR_EL3, ELR_EL3, & SCR_EL3 in case there is a world
+	 * Save the SPSR_EL3 and ELR_EL3 in case there is a world
 	 * switch during SMC handling.
 	 * TODO: Revisit if all system registers can be saved later.
 	 */
 	mrs	x16, spsr_el3
 	mrs	x17, elr_el3
-	mrs	x18, scr_el3
 	stp	x16, x17, [x6, #CTX_EL3STATE_OFFSET + CTX_SPSR_EL3]
-	str	x18, [x6, #CTX_EL3STATE_OFFSET + CTX_SCR_EL3]
+
+	/* Load SCR_EL3 */
+	mrs	x18, scr_el3
+
+	/* check for system register traps */
+	mrs	x16, esr_el3
+	ubfx	x17, x16, #ESR_EC_SHIFT, #ESR_EC_LENGTH
+	cmp	x17, #EC_AARCH64_SYS
+	b.eq	sysreg_handler64
 
 	/* Clear flag register */
 	mov	x7, xzr
@@ -566,6 +577,32 @@
 #endif
 	blr	x15
 
+	b	el3_exit
+
+sysreg_handler64:
+	mov	x0, x16		/* ESR_EL3, containing syndrome information */
+	mov	x1, x6		/* lower EL's context */
+	mov	x19, x6		/* save context pointer for after the call */
+	mov	sp, x12		/* EL3 runtime stack, as loaded above */
+
+	/* int handle_sysreg_trap(uint64_t esr_el3, cpu_context_t *ctx); */
+	bl	handle_sysreg_trap
+	/*
+	 * returns:
+	 *   -1: unhandled trap, panic
+	 *    0: handled trap, return to the trapping instruction (repeating it)
+	 *    1: handled trap, return to the next instruction
+	 */
+
+	tst	w0, w0
+	b.mi	do_panic	/* negative return value: panic */
+	b.eq	1f		/* zero: do not change ELR_EL3 */
+
+	/* advance the PC to continue after the instruction */
+	ldr	x1, [x19, #CTX_EL3STATE_OFFSET + CTX_ELR_EL3]
+	add	x1, x1, #4
+	str	x1, [x19, #CTX_EL3STATE_OFFSET + CTX_ELR_EL3]
+1:
 	b	el3_exit
 
 smc_unknown:
@@ -592,7 +629,7 @@
 	msr	spsel, #MODE_SP_ELX
 	no_ret	report_unhandled_exception
 #endif
-endfunc smc_handler
+endfunc sync_exception_handler
 
 	/* ---------------------------------------------------------------------
 	 * The following code handles exceptions caused by BRK instructions.
diff --git a/bl31/bl31.mk b/bl31/bl31.mk
index 4c93a55..ac15f9f 100644
--- a/bl31/bl31.mk
+++ b/bl31/bl31.mk
@@ -42,6 +42,7 @@
 				bl31/aarch64/ea_delegate.S			\
 				bl31/aarch64/runtime_exceptions.S		\
 				bl31/bl31_context_mgmt.c			\
+				bl31/bl31_traps.c				\
 				common/runtime_svc.c				\
 				lib/cpus/aarch64/dsu_helpers.S			\
 				plat/common/aarch64/platform_mp_stack.S		\
diff --git a/bl31/bl31_traps.c b/bl31/bl31_traps.c
new file mode 100644
index 0000000..b12185d
--- /dev/null
+++ b/bl31/bl31_traps.c
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2022, ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * Dispatch synchronous system register traps from lower ELs.
+ */
+
+#include <bl31/sync_handle.h>
+#include <context.h>
+
+int handle_sysreg_trap(uint64_t esr_el3, cpu_context_t *ctx)
+{
+	switch (esr_el3 & ISS_SYSREG_OPCODE_MASK) {
+#if ENABLE_FEAT_RNG_TRAP
+	case ISS_SYSREG_OPCODE_RNDR:
+	case ISS_SYSREG_OPCODE_RNDRRS:
+		return plat_handle_rng_trap(esr_el3, ctx);
+#endif
+	default:
+		return TRAP_RET_UNHANDLED;
+	}
+}
diff --git a/bl32/tsp/ffa_helpers.c b/bl32/tsp/ffa_helpers.c
index 3639c22..ad70c2b 100644
--- a/bl32/tsp/ffa_helpers.c
+++ b/bl32/tsp/ffa_helpers.c
@@ -149,13 +149,15 @@
 {
 	smc_args_t ret;
 	uint32_t descriptor_size;
-	struct ffa_mtd *memory_region = (struct ffa_mtd *)mb->tx_buffer;
+	struct ffa_mtd *memory_region;
 
 	if (retrieved == NULL || mb == NULL) {
 		ERROR("Invalid parameters!\n");
 		return false;
 	}
 
+	memory_region = (struct ffa_mtd *)mb->tx_buffer;
+
 	/* Clear TX buffer. */
 	memset(memory_region, 0, PAGE_SIZE);
 
diff --git a/bl32/tsp/tsp_ffa_main.c b/bl32/tsp/tsp_ffa_main.c
index 53dbd03..2c53977 100644
--- a/bl32/tsp/tsp_ffa_main.c
+++ b/bl32/tsp/tsp_ffa_main.c
@@ -216,10 +216,10 @@
 				(uint64_t)composite->address_range_array[i].address,
 				size, mem_attrs);
 
-			/* Remove mappings created in this transaction. */
-			for (i--; i >= 0U; i--) {
+			/* Remove mappings previously created in this transaction. */
+			for (i--; i >= 0; i--) {
 				ret = mmap_remove_dynamic_region(
-					(uint64_t)ptr,
+					(uint64_t)composite->address_range_array[i].address,
 					composite->address_range_array[i].page_count * PAGE_SIZE);
 
 				if (ret != 0) {
@@ -227,6 +227,7 @@
 					panic();
 				}
 			}
+
 			return FFA_ERROR_NO_MEMORY;
 		}
 
@@ -298,8 +299,8 @@
 	tsp_stats[linear_id].eret_count++;
 	tsp_stats[linear_id].cpu_off_count++;
 
-	INFO("TSP: cpu 0x%lx off request\n", read_mpidr());
-	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu off requests\n",
+	VERBOSE("TSP: cpu 0x%lx off request\n", read_mpidr());
+	VERBOSE("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu off requests\n",
 		read_mpidr(),
 		tsp_stats[linear_id].smc_count,
 		tsp_stats[linear_id].eret_count,
@@ -336,7 +337,7 @@
 	tsp_stats[linear_id].eret_count++;
 	tsp_stats[linear_id].cpu_suspend_count++;
 
-	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu suspend requests\n",
+	VERBOSE("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu suspend requests\n",
 		read_mpidr(),
 		tsp_stats[linear_id].smc_count,
 		tsp_stats[linear_id].eret_count,
@@ -369,9 +370,9 @@
 	tsp_stats[linear_id].eret_count++;
 	tsp_stats[linear_id].cpu_resume_count++;
 
-	INFO("TSP: cpu 0x%lx resumed. maximum off power level %" PRId64 "\n",
+	VERBOSE("TSP: cpu 0x%lx resumed. maximum off power level %" PRId64 "\n",
 	     read_mpidr(), max_off_pwrlvl);
-	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu resume requests\n",
+	VERBOSE("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu resume requests\n",
 		read_mpidr(),
 		tsp_stats[linear_id].smc_count,
 		tsp_stats[linear_id].eret_count,
@@ -611,7 +612,7 @@
 	tsp_stats[linear_id].eret_count++;
 	tsp_stats[linear_id].cpu_on_count++;
 
-	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu on requests\n",
+	VERBOSE("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu on requests\n",
 			read_mpidr(),
 			tsp_stats[linear_id].smc_count,
 			tsp_stats[linear_id].eret_count,
@@ -640,8 +641,8 @@
 	tsp_stats[linear_id].smc_count++;
 	tsp_stats[linear_id].eret_count++;
 	tsp_stats[linear_id].cpu_on_count++;
-	INFO("TSP: cpu 0x%lx turned on\n", read_mpidr());
-	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu on requests\n",
+	VERBOSE("TSP: cpu 0x%lx turned on\n", read_mpidr());
+	VERBOSE("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu on requests\n",
 			read_mpidr(),
 			tsp_stats[linear_id].smc_count,
 			tsp_stats[linear_id].eret_count,
diff --git a/changelog.yaml b/changelog.yaml
index cfb2bb5..1e1f0a1 100644
--- a/changelog.yaml
+++ b/changelog.yaml
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2021-2022, Arm Limited. All rights reserved.
+# Copyright (c) 2021-2023, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -744,6 +744,9 @@
           - title: mbedTLS
             scope: mbedtls
 
+      - title: Console
+        scope: console
+
       - title: Generic Clock
         scope: clk
 
diff --git a/docs/about/maintainers.rst b/docs/about/maintainers.rst
index 9a2ae73..914c959 100644
--- a/docs/about/maintainers.rst
+++ b/docs/about/maintainers.rst
@@ -506,8 +506,10 @@
 
 Arm Total Compute platform port
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-:|M|: Anders Dellien <anders.dellien@arm.com>
-:|G|: `andersdellien-arm`_
+:|M|: Vishnu Banavath <vishnu.banavath@arm.com>
+:|G|: `vishnu-banavath`_
+:|M|: Rupinderjit Singh <rupinderjit.singh@arm.com>
+:|G|: `rupsin01`_
 :|F|: plat/arm/board/tc
 
 HiSilicon HiKey and HiKey960 platform ports
@@ -956,5 +958,6 @@
 .. _marcbonnici: https://github.com/marcbonnici
 .. _jayanthchidanand-arm: https://github.com/jayanthchidanand-arm
 .. _bytefire: https://github.com/bytefire
+.. _rupsin01: https://github.com/rupsin01
 
 .. _Project Maintenance Process: https://developer.trustedfirmware.org/w/collaboration/project-maintenance-process/
diff --git a/docs/components/realm-management-extension.rst b/docs/components/realm-management-extension.rst
index 6fc0c2e..2ea8012 100644
--- a/docs/components/realm-management-extension.rst
+++ b/docs/components/realm-management-extension.rst
@@ -196,7 +196,7 @@
  -C bp.refcounter.use_real_time=0                               \
  -C bp.ve_sysregs.exit_on_shutdown=1                            \
  -C cache_state_modelled=1                                      \
- -C bp.dram_size=2                                              \
+ -C bp.dram_size=4                                              \
  -C bp.secure_memory=1                                          \
  -C pci.pci_smmuv3.mmu.SMMU_ROOT_IDR0=3                         \
  -C pci.pci_smmuv3.mmu.SMMU_ROOT_IIDR=0x43B                     \
diff --git a/docs/components/rmm-el3-comms-spec.rst b/docs/components/rmm-el3-comms-spec.rst
index 8070ff4..25c4269 100644
--- a/docs/components/rmm-el3-comms-spec.rst
+++ b/docs/components/rmm-el3-comms-spec.rst
@@ -101,7 +101,7 @@
    x2,Maximum number of CPUs to be supported at runtime. RMM should ensure that it can support this maximum number.
    x3,Base address for the shared buffer used for communication between EL3 firmware and RMM. This buffer must be of 4KB size (1 page). The boot manifest must be present at the base of this shared buffer during cold boot.
 
-During cold boot, EL3 firmware needs to allocate a 4K page that will be
+During cold boot, EL3 firmware needs to allocate a 4KB page that will be
 passed to RMM in x3. This memory will be used as shared buffer for communication
 between EL3 and RMM. It must be assigned to Realm world and must be mapped with
 Normal memory attributes (IWB-OWB-ISH) at EL3. At boot, this memory will be
@@ -522,8 +522,8 @@
 
 .. _rmm_el3_manifest_struct:
 
-RMM-EL3 Boot Manifest Version
-~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+RMM-EL3 Boot Manifest structure
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
 The RMM-EL3 Boot Manifest structure contains platform boot information passed
 from EL3 to RMM. The width of the Boot Manifest is 128 bits
diff --git a/docs/components/secure-partition-manager.rst b/docs/components/secure-partition-manager.rst
index cd439ad..f0caf89 100644
--- a/docs/components/secure-partition-manager.rst
+++ b/docs/components/secure-partition-manager.rst
@@ -150,9 +150,6 @@
   at EL3.
 - If neither ``SPMD_SPM_AT_SEL2`` or ``SPMC_AT_EL3`` are enabled the SPMC
   exception level is set to S-EL1.
-- **CTX_INCLUDE_EL2_REGS**: this option permits saving (resp.
-  restoring) the EL2 system register context before entering (resp.
-  after leaving) the SPMC. It is mandatorily enabled when
   ``SPMD_SPM_AT_SEL2`` is enabled. The context save/restore routine
   and exhaustive list of registers is visible at `[4]`_.
 - **SP_LAYOUT_FILE**: this option specifies a text description file
@@ -161,16 +158,16 @@
   is required when ``SPMD_SPM_AT_SEL2`` is enabled hence when multiple
   secure partitions are to be loaded by BL2 on behalf of the SPMC.
 
-+---------------+----------------------+------------------+-------------+
-|               | CTX_INCLUDE_EL2_REGS | SPMD_SPM_AT_SEL2 | SPMC_AT_EL3 |
-+---------------+----------------------+------------------+-------------+
-| SPMC at S-EL1 |         0            |        0         |      0      |
-+---------------+----------------------+------------------+-------------+
-| SPMC at S-EL2 |         1            | 1 (default when  |      0      |
-|               |                      |    SPD=spmd)     |             |
-+---------------+----------------------+------------------+-------------+
-| SPMC at EL3   |         0            |        0         |      1      |
-+---------------+----------------------+------------------+-------------+
++---------------+------------------+-------------+-------------------------+
+|               | SPMD_SPM_AT_SEL2 | SPMC_AT_EL3 | CTX_INCLUDE_EL2_REGS(*) |
++---------------+------------------+-------------+-------------------------+
+| SPMC at S-EL1 |        0         |      0      |             0           |
++---------------+------------------+-------------+-------------------------+
+| SPMC at S-EL2 | 1 (default when  |      0      |             1           |
+|               |    SPD=spmd)     |             |                         |
++---------------+------------------+-------------+-------------------------+
+| SPMC at EL3   |        0         |      1      |             0           |
++---------------+------------------+-------------+-------------------------+
 
 Other combinations of such build options either break the build or are not
 supported.
@@ -181,9 +178,9 @@
   stack.
 - When ``SPMD_SPM_AT_SEL2=1``, the reference software stack assumes enablement
   of FEAT_PAuth, FEAT_BTI and FEAT_MTE architecture extensions.
-- The ``CTX_INCLUDE_EL2_REGS`` option provides the generic support for
-  barely saving/restoring EL2 registers from an Arm arch perspective. As such
-  it is decoupled from the ``SPD=spmd`` option.
+- ``(*) CTX_INCLUDE_EL2_REGS``, this flag is |TF-A| internal and informational
+  in this table. When set, it provides the generic support for saving/restoring
+  EL2 registers required when S-EL2 firmware is present.
 - BL32 option is re-purposed to specify the SPMC image. It can specify either
   the Hafnium binary path (built for the secure world) or the path to a TEE
   binary implementing FF-A interfaces.
@@ -212,7 +209,6 @@
     CROSS_COMPILE=aarch64-none-elf- \
     PLAT=fvp \
     SPD=spmd \
-    CTX_INCLUDE_EL2_REGS=1 \
     ARM_ARCH_MINOR=5 \
     BRANCH_PROTECTION=1 \
     CTX_INCLUDE_PAUTH_REGS=1 \
@@ -230,7 +226,6 @@
     CROSS_COMPILE=aarch64-none-elf- \
     PLAT=fvp \
     SPD=spmd \
-    CTX_INCLUDE_EL2_REGS=1 \
     ARM_ARCH_MINOR=5 \
     BRANCH_PROTECTION=1 \
     CTX_INCLUDE_PAUTH_REGS=1 \
diff --git a/docs/design/cpu-specific-build-macros.rst b/docs/design/cpu-specific-build-macros.rst
index d48f284..9311420 100644
--- a/docs/design/cpu-specific-build-macros.rst
+++ b/docs/design/cpu-specific-build-macros.rst
@@ -317,6 +317,10 @@
    CPU. This needs to be enabled for revisions r0p0, r1p0, r1p1, and r1p2, and
    it is still open.
 
+-  ``ERRATA_A78_2772019``: This applies errata 2772019 workaround to Cortex-A78
+   CPU. This needs to be enabled for revisions r0p0, r1p0, r1p1, and r1p2, and
+   it is still open.
+
 For Cortex-A78 AE, the following errata build flags are defined :
 
 - ``ERRATA_A78_AE_1941500`` : This applies errata 1941500 workaround to
@@ -460,6 +464,10 @@
    CPU. This needs to be enabled for revisions r0p0, r1p0 and r1p1 of the CPU.
    It is still open.
 
+-  ``ERRATA_V1_2743093``: This applies errata 2743093 workaround to Neoverse-V1
+   CPU. This needs to be enabled for revisions r0p0, r1p0, r1p1 and r1p2 of the
+   CPU. It is still open.
+
 For Cortex-A710, the following errata build flags are defined :
 
 -  ``ERRATA_A710_1987031``: This applies errata 1987031 workaround to
@@ -518,6 +526,10 @@
    Cortex-A710 CPU. This needs to be enabled for revisions r0p0, r1p0 and r2p0
    of the CPU and is fixed in r2p1.
 
+-  ``ERRATA_A710_2768515``: This applies errata 2768515 workaround to
+   Cortex-A710 CPU. This needs to be enabled for revisions r0p0, r1p0, r2p0 and
+   r2p1 of the CPU and is still open.
+
 For Neoverse N2, the following errata build flags are defined :
 
 -  ``ERRATA_N2_2002655``: This applies errata 2002655 workaround to Neoverse-N2
@@ -562,6 +574,10 @@
    CPU. This needs to be enabled for revision r0p0 of the CPU, it is fixed in
    r0p1.
 
+-  ``ERRATA_N2_2743089``: This applies errata 2743089 workaround to Neoverse-N2
+   CPU. This needs to be enabled for revisions r0p0, r0p1 and r0p2. It is fixed
+   in r0p3.
+
 For Cortex-X2, the following errata build flags are defined :
 
 -  ``ERRATA_X2_2002765``: This applies errata 2002765 workaround to Cortex-X2
@@ -595,6 +611,10 @@
    Cortex-X2 CPU. This needs to be enabled for revisions r0p0, r1p0 and r2p0
    of the CPU and is fixed in r2p1.
 
+-  ``ERRATA_X2_2768515``: This applies errata 2768515 workaround to
+   Cortex-X2 CPU. This needs to be enabled for revisions r0p0, r1p0, r2p0
+   and r2p1 of the CPU and is still open.
+
 For Cortex-X3, the following errata build flags are defined :
 
 - ``ERRATA_X3_2313909``: This applies errata 2313909 workaround to
diff --git a/docs/design/trusted-board-boot-build.rst b/docs/design/trusted-board-boot-build.rst
index c3f3a2f..caf367b 100644
--- a/docs/design/trusted-board-boot-build.rst
+++ b/docs/design/trusted-board-boot-build.rst
@@ -42,7 +42,7 @@
    are loaded from that path instead of the default OS path. Export this
    variable if necessary.
 
-   In the case of Arm platforms, the location of the ROTPK hash must also be
+   In the case of Arm platforms, the location of the ROTPK must also be
    specified at build time. The following locations are currently supported (see
    ``ARM_ROTPK_LOCATION`` build option):
 
@@ -62,6 +62,9 @@
       ``plat/arm/board/common/rotpk/arm_rotpk_ecdsa_sha256.bin``. Enforce
       generation of the new hash if ``ROT_KEY`` is specified.
 
+   -  ``ARM_ROTPK_LOCATION=devel_full_dev_rsa_key``: use the key located in
+      ``plat/arm/board/common/rotpk/arm_full_dev_rsa_rotpk.S``.
+
    Example of command line using RSA development keys:
 
    .. code:: shell
diff --git a/docs/getting_started/build-internals.rst b/docs/getting_started/build-internals.rst
new file mode 100644
index 0000000..a015d71
--- /dev/null
+++ b/docs/getting_started/build-internals.rst
@@ -0,0 +1,14 @@
+Internal Build Options
+======================
+
+|TF-A| internally uses certain options that are not exposed directly through
+:ref:`build-options <build options>` but enabled or disabled indirectly and
+depends on certain options to be enabled or disabled.
+
+.. _build_options_internal:
+
+-  ``CTX_INCLUDE_EL2_REGS``: This boolean option provides context save/restore
+   operations when entering/exiting an EL2 execution context. This is of primary
+   interest when Armv8.4-SecEL2 or RME extension is implemented.
+   Default is 0 (disabled). This option will be set to 1 (enabled) when ``SPD=spmd``
+   and ``SPMD_SPM_AT_SEL2`` is set or when ``ENABLE_RME`` is set to 1 (enabled).
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index c4667ca..d5ded5e 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -164,12 +164,6 @@
    is on hardware that does not implement AArch32, or at least not at EL1 and
    higher ELs). Default value is 1.
 
--  ``CTX_INCLUDE_EL2_REGS`` : This boolean option provides context save/restore
-   operations when entering/exiting an EL2 execution context. This is of primary
-   interest when Armv8.4-SecEL2 extension is implemented. Default is 0 (disabled).
-   This option must be equal to 1 (enabled) when ``SPD=spmd`` and
-   ``SPMD_SPM_AT_SEL2`` is set.
-
 -  ``CTX_INCLUDE_FPREGS``: Boolean option that, when set to 1, will cause the FP
    registers to be included when saving and restoring the CPU context. Default
    is 0.
@@ -830,6 +824,11 @@
    disabled). This configuration supports pre-Armv8.4 platforms (aka not
    implementing the ``FEAT_SEL2`` extension). This is an experimental feature.
 
+-  ``SPMC_OPTEE`` : This boolean option is used jointly with the SPM
+   Dispatcher option (``SPD=spmd``) and with ``SPMD_SPM_AT_SEL2=0`` to
+   indicate that the SPMC at S-EL1 is OP-TEE and an OP-TEE specific loading
+   mechanism should be used.
+
 -  ``SPMD_SPM_AT_SEL2`` : This boolean option is used jointly with the SPM
    Dispatcher option (``SPD=spmd``). When enabled (1) it indicates the SPMC
    component runs at the S-EL2 exception level provided by the ``FEAT_SEL2``
diff --git a/docs/getting_started/index.rst b/docs/getting_started/index.rst
index 3fbf48d..5ebabea 100644
--- a/docs/getting_started/index.rst
+++ b/docs/getting_started/index.rst
@@ -10,6 +10,7 @@
    initial-build
    tools-build
    build-options
+   build-internals
    image-terminology
    porting-guide
    psci-lib-integration-guide
diff --git a/docs/getting_started/porting-guide.rst b/docs/getting_started/porting-guide.rst
index 7836914..80b72e5 100644
--- a/docs/getting_started/porting-guide.rst
+++ b/docs/getting_started/porting-guide.rst
@@ -3398,6 +3398,39 @@
 The default implementation of this function calls
 ``report_unhandled_exception``.
 
+Function : plat_handle_rng_trap
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+::
+
+    Argument : uint64_t
+    Argument : cpu_context_t *
+    Return   : int
+
+This function is invoked by BL31's exception handler when there is a synchronous
+system register trap caused by access to the RNDR or RNDRRS registers. It allows
+platforms implementing ``FEAT_RNG_TRAP`` and enabling ``ENABLE_FEAT_RNG_TRAP`` to
+emulate those system registers by returing back some entropy to the lower EL.
+
+The first parameter (``uint64_t esr_el3``) contains the content of the ESR_EL3
+syndrome register, which encodes the instruction that was trapped. The interesting
+information in there is the target register (``get_sysreg_iss_rt()``).
+
+The second parameter (``cpu_context_t *ctx``) represents the CPU state in the
+lower exception level, at the time when the execution of the ``mrs`` instruction
+was trapped. Its content can be changed, to put the entropy into the target
+register.
+
+The return value indicates how to proceed:
+
+-  When returning ``TRAP_RET_UNHANDLED`` (-1), the machine will panic.
+-  When returning ``TRAP_RET_REPEAT`` (0), the exception handler will return
+   to the same instruction, so its execution will be repeated.
+-  When returning ``TRAP_RET_CONTINUE`` (1), the exception handler will return
+   to the next instruction.
+
+This function needs to be implemented by a platform if it enables FEAT_RNG_TRAP.
+
 Build flags
 -----------
 
diff --git a/docs/plat/arm/arm-build-options.rst b/docs/plat/arm/arm-build-options.rst
index 407c04b..68eb3ec 100644
--- a/docs/plat/arm/arm-build-options.rst
+++ b/docs/plat/arm/arm-build-options.rst
@@ -49,7 +49,7 @@
    field of power-state parameter.
 
 -  ``ARM_ROTPK_LOCATION``: used when ``TRUSTED_BOARD_BOOT=1``. It specifies the
-   location of the ROTPK hash returned by the function ``plat_get_rotpk_info()``
+   location of the ROTPK returned by the function ``plat_get_rotpk_info()``
    for Arm platforms. Depending on the selected option, the proper private key
    must be specified using the ``ROT_KEY`` option when building the Trusted
    Firmware. This private key will be used by the certificate generation tool
@@ -68,12 +68,16 @@
       ``arm_rotpk_ecdsa.der``, located in ``plat/arm/board/common/rotpk``. To
       use this option, ``arm_rotprivk_ecdsa.pem`` must be specified as
       ``ROT_KEY`` when creating the certificates.
+   -  ``devel_full_dev_rsa_key`` : returns a development public key embedded in
+      the BL1 and BL2 binaries. This key has been obtained from the RSA public
+      key ``arm_rotpk_rsa.der``, located in ``plat/arm/board/common/rotpk``.
 
--  ``ARM_ROTPK_HASH``: used when ``ARM_ROTPK_LOCATION=devel_*``. Specifies the
-   location of the ROTPK hash. Not expected to be a build option. This defaults to
-   ``plat/arm/board/common/rotpk/*_sha256.bin`` depending on the specified algorithm.
-   Providing ``ROT_KEY`` enforces generation of the hash from the ``ROT_KEY`` and
-   overwrites the default hash file.
+-  ``ARM_ROTPK_HASH``: used when ``ARM_ROTPK_LOCATION=devel_*``, excluding
+   ``devel_full_dev_rsa_key``. Specifies the location of the ROTPK hash. Not
+   expected to be a build option. This defaults to
+   ``plat/arm/board/common/rotpk/*_sha256.bin`` depending on the specified
+   algorithm. Providing ``ROT_KEY`` enforces generation of the hash from the
+   ``ROT_KEY`` and overwrites the default hash file.
 
 -  ``ARM_TSP_RAM_LOCATION``: location of the TSP binary. Options:
 
diff --git a/docs/resources/diagrams/plantuml/tfa_rss_dfd.puml b/docs/resources/diagrams/plantuml/tfa_rss_dfd.puml
new file mode 100644
index 0000000..23f5b17
--- /dev/null
+++ b/docs/resources/diagrams/plantuml/tfa_rss_dfd.puml
@@ -0,0 +1,77 @@
+/'
+ ' Copyright (c) 2021-2022, Arm Limited. All rights reserved.
+ '
+ ' SPDX-License-Identifier: BSD-3-Clause
+ '/
+
+/'
+TF-A Data Flow Diagram including RSS
+'/
+
+@startuml
+digraph tfa_dfd {
+
+    # Arrange nodes from left to right
+    rankdir="LR"
+
+    # Allow arrows to end on cluster boundaries
+    compound=true
+
+    # Default settings for edges and nodes
+    edge [minlen=2 color="#8c1b07"]
+    node [fillcolor="#ffb866" style=filled shape=box fixedsize=true width=1.6 height=0.7]
+
+    # Nodes outside of the trust boundary
+    nsec [label="Non-secure\nClients"]
+    sec [label="Secure\nClients"]
+    dbg [label="Debug & Trace"]
+    logs [label="Logs\n(UART)"]
+    nvm [label="Non-volatile\nMemory"]
+
+
+    # Trust boundary cluster
+    subgraph cluster_trusted{
+        graph [style=dashed color="#f22430"]
+
+        # HW IPs cluster
+        subgraph cluster_ip{
+            label ="Hardware IPs";
+            graph [style=filled color="#000000" fillcolor="#ffd29e"]
+
+            rank="same"
+            gic [label="GIC" width=1.2 height=0.5]
+            tzc [label="TZ\nController" width=1.2 height=0.5]
+            etc [label="..." shape=none style=none height=0.5]
+        }
+
+        # TF-A cluster
+        subgraph cluster_tfa{
+            label ="TF-A";
+            graph [style=filled color="#000000" fillcolor="#faf9cd"]
+
+            bl1 [label="Boot ROM\n(BL1)" fillcolor="#ddffb3"];
+            bl2 [label="Trusted Boot\nFirmware\n(BL2)" fillcolor="#ddffb3" height=1]
+            bl31 [label="TF-A Runtime\n(BL31)" fillcolor="#ddffb3"]
+        }
+
+        # RSS cluster
+        subgraph cluster_rss{
+            label ="RSS";
+            graph [style=filled color="#000000" fillcolor="#faf9cd"]
+
+            rss [label="Runtime Security\n\ Subsystem\n\ (RSS)" fillcolor="#ddffb3"]
+        }
+    }
+
+    # Interactions between nodes
+    nvm -> bl31 [lhead=cluster_tfa label="DF1"]
+    logs -> bl31 [dir="back" lhead=cluster_tfa label="DF2"]
+    dbg -> bl2 [dir="both" lhead=cluster_tfa label="DF3"]
+    sec -> bl2 [dir="both" lhead=cluster_tfa label="DF4"]
+    nsec -> bl1 [dir="both" lhead=cluster_tfa, label="DF5"]
+    bl2 ->  tzc [dir="both" ltail=cluster_tfa lhead=cluster_ip label="DF6" minlen=1]
+    bl31 -> rss [dir="both" ltail=cluster_tfa lhead=cluster_rss label="DF7" minlen=1]
+
+}
+
+@enduml
diff --git a/docs/threat_model/index.rst b/docs/threat_model/index.rst
index ad8b82a..9fd55a9 100644
--- a/docs/threat_model/index.rst
+++ b/docs/threat_model/index.rst
@@ -16,7 +16,8 @@
    threat_model_spm
    threat_model_el3_spm
    threat_model_fvp_r
+   threat_model_rss_interface
 
 --------------
 
-*Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.*
+*Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.*
diff --git a/docs/threat_model/threat_model.rst b/docs/threat_model/threat_model.rst
index 38e5c87..99bbb3a 100644
--- a/docs/threat_model/threat_model.rst
+++ b/docs/threat_model/threat_model.rst
@@ -170,7 +170,7 @@
 
 For each threat identified, a risk rating that ranges
 from *informational* to *critical* is given based on the likelihood of the
-threat occuring if a mitigation is not in place, and the impact of the
+threat occurring if a mitigation is not in place, and the impact of the
 threat (i.e. how severe the consequences could be). Table 4 explains each
 rating in terms of score, impact and likelihood.
 
@@ -778,8 +778,9 @@
 +========================+====================================================+
 | Threat                 | | **Misconfiguration of the Memory Management Unit |
 |                        |   (MMU) may allow a normal world software to       |
-|                        |   access sensitive data or execute arbitrary       |
-|                        |   code**                                           |
+|                        |   access sensitive data, execute arbitrary         |
+|                        |   code or access otherwise restricted HW           |
+|                        |   interface**                                      |
 |                        |                                                    |
 |                        | | A misconfiguration of the MMU could              |
 |                        |   lead to an open door for software running in the |
@@ -853,6 +854,8 @@
 +------------------------+-----------------------------------------------------+
 | Threat Type            | Information Disclosure                              |
 +------------------------+-------------------+----------------+----------------+
+| Application            | Server            | IoT            | Mobile         |
++------------------------+-------------------+----------------+----------------+
 | Impact                 | Medium (3)        | Medium (3)     | Medium (3)     |
 +------------------------+-------------------+----------------+----------------+
 | Likelihood             | Low (2)           | Low (2)        | Low (2)        |
@@ -880,6 +883,41 @@
 |                        |   mitigated.                                        |
 +------------------------+-----------------------------------------------------+
 
++------------------------+-----------------------------------------------------+
+| ID                     | 13                                                  |
++========================+=====================================================+
+| Threat                 | | **Leaving sensitive information in the memory,    |
+|                        |   can allow an attacker to retrieve them.**         |
+|                        |                                                     |
+|                        | | Accidentally leaving not-needed sensitive data in |
+|                        |   internal buffers can leak them if an attacker     |
+|                        |   gains access to memory due to a vulnerability.    |
++------------------------+-----------------------------------------------------+
+| Diagram Elements       | DF4, DF5                                            |
++------------------------+-----------------------------------------------------+
+| Affected TF-A          | BL1, BL2, BL31                                      |
+| Components             |                                                     |
++------------------------+-----------------------------------------------------+
+| Assets                 | Sensitive Data                                      |
++------------------------+-----------------------------------------------------+
+| Threat Agent           | NSCode, SecCode                                     |
++------------------------+-----------------------------------------------------+
+| Threat Type            | Information Disclosure                              |
++------------------------+-------------------+----------------+----------------+
+| Application            | Server            | IoT            | Mobile         |
++------------------------+-------------------+----------------+----------------+
+| Impact                 |  Critical (5)     | Critical (5)   | Critical (5)   |
++------------------------+-------------------+----------------+----------------+
+| Likelihood             |  Medium (3)       | Medium (3)     | Medium (3)     |
++------------------------+-------------------+----------------+----------------+
+| Total Risk Rating      |  High (15)        | High (15)      | High (15)      |
++------------------------+-------------------+----------------+----------------+
+| Mitigations            |   Clear the sensitive data from internal buffers as |
+|                        |   soon as they are not needed anymore.              |
++------------------------+-----------------------------------------------------+
+| Mitigations            | | Yes / Platform specific                           |
++------------------------+-----------------------------------------------------+
+
 --------------
 
 *Copyright (c) 2021-2022, Arm Limited. All rights reserved.*
diff --git a/docs/threat_model/threat_model_rss_interface.rst b/docs/threat_model/threat_model_rss_interface.rst
new file mode 100644
index 0000000..4bceb63
--- /dev/null
+++ b/docs/threat_model/threat_model_rss_interface.rst
@@ -0,0 +1,59 @@
+Threat Model for RSS - AP interface
+***********************************
+
+************
+Introduction
+************
+This document is an extension for the general TF-A threat-model. It considers
+those platforms where a Runtime Security Subsystem (RSS) is included in the SoC
+next to the Application Processor (AP).
+
+********************
+Target of Evaluation
+********************
+The scope of this threat model only includes the interface between the RSS and
+AP. Otherwise, the TF-A :ref:`Generic Threat Model` document is applicable for
+the AP core. The threat model for the RSS firmware will be provided by the RSS
+firmware project in the future.
+
+
+Data Flow Diagram
+=================
+This diagram is different only from the general TF-A data flow diagram in that
+it includes the RSS and highlights the interface between the AP and the RSS
+cores. The interface description only focuses on the AP-RSS interface the rest
+is the same as in the general TF-A threat-model document.
+
+.. uml:: ../resources/diagrams/plantuml/tfa_rss_dfd.puml
+  :caption: Figure 1: TF-A Data Flow Diagram including RSS
+
+.. table:: Table 1: TF-A - RSS data flow diagram
+
+  +-----------------+--------------------------------------------------------+
+  | Diagram Element | Description                                            |
+  +=================+========================================================+
+  |       DF7       | | Boot images interact with RSS over a communication   |
+  |                 |   channel to record boot measurements and get image    |
+  |                 |   verification keys. At runtime, BL31 obtains the      |
+  |                 |   realm world attestation signing key from RSS.        |
+  +-----------------+--------------------------------------------------------+
+
+Threat Assessment
+=================
+For this section, please reference the Threat Assessment under the general TF-A
+threat-model document, :ref:`Generic Threat Model`. All the threats listed there
+are applicable for the AP core, here only the differences are highlighted.
+
+    - ID 11: The access to the communication interface between AP and RSS is
+      allowed only for firmware running at EL3. Accidentally exposing this
+      interface to NSCode can allow malicious code to interact with RSS and
+      gain access to sensitive data.
+    - ID 13: Relevant in the context of the realm attestation key, which can be
+      retrieved by BL31 through DF7. The RSS communication protocol layer
+      mitigates against this by clearing its internal buffer when reply is
+      received. The caller of the API must do the same if data is not needed
+      anymore.
+
+--------------
+
+*Copyright (c) 2022, Arm Limited. All rights reserved.*
\ No newline at end of file
diff --git a/drivers/arm/gic/v3/gicv3_main.c b/drivers/arm/gic/v3/gicv3_main.c
index bc93f93..f6c251d 100644
--- a/drivers/arm/gic/v3/gicv3_main.c
+++ b/drivers/arm/gic/v3/gicv3_main.c
@@ -12,6 +12,7 @@
 #include <common/interrupt_props.h>
 #include <drivers/arm/gicv3.h>
 #include <lib/spinlock.h>
+#include <plat/common/platform.h>
 
 #include "gicv3_private.h"
 
@@ -1287,12 +1288,14 @@
 
 	assert(gicv3_driver_data->gicr_base == 0U);
 
+	if (plat_can_cmo()) {
 	/* Ensure this function is called with Data Cache enabled */
 #ifndef __aarch64__
-	assert((read_sctlr() & SCTLR_C_BIT) != 0U);
+		assert((read_sctlr() & SCTLR_C_BIT) != 0U);
 #else
-	assert((read_sctlr_el3() & SCTLR_C_BIT) != 0U);
+		assert((read_sctlr_el3() & SCTLR_C_BIT) != 0U);
 #endif /* !__aarch64__ */
+	}
 
 	mpidr_self = read_mpidr_el1() & MPIDR_AFFINITY_MASK;
 	rdistif_base = gicr_frame;
diff --git a/drivers/arm/rss/rss_comms.c b/drivers/arm/rss/rss_comms.c
index 5e224e1..4622af9 100644
--- a/drivers/arm/rss/rss_comms.c
+++ b/drivers/arm/rss/rss_comms.c
@@ -156,14 +156,22 @@
 
 	err = mhu_init_sender(mhu_sender_base);
 	if (err != MHU_ERR_NONE) {
-		ERROR("[RSS-COMMS] Host to RSS MHU driver initialization failed: %d\n", err);
-		return -1;
+		if (err == MHU_ERR_ALREADY_INIT) {
+			INFO("[RSS-COMMS] Host to RSS MHU driver already initialized\n");
+		} else {
+			ERROR("[RSS-COMMS] Host to RSS MHU driver initialization failed: %d\n", err);
+			return -1;
+		}
 	}
 
 	err = mhu_init_receiver(mhu_receiver_base);
 	if (err != MHU_ERR_NONE) {
-		ERROR("[RSS-COMMS] RSS to Host MHU driver initialization failed: %d\n", err);
-		return -1;
+		if (err == MHU_ERR_ALREADY_INIT) {
+			INFO("[RSS-COMMS] RSS to Host MHU driver already initialized\n");
+		} else {
+			ERROR("[RSS-COMMS] RSS to Host MHU driver initialization failed: %d\n", err);
+			return -1;
+		}
 	}
 
 	return 0;
diff --git a/drivers/auth/mbedtls/mbedtls_common.mk b/drivers/auth/mbedtls/mbedtls_common.mk
index 16ce65f..ae4b067 100644
--- a/drivers/auth/mbedtls/mbedtls_common.mk
+++ b/drivers/auth/mbedtls/mbedtls_common.mk
@@ -22,7 +22,7 @@
 MBEDTLS_SOURCES	+=		drivers/auth/mbedtls/mbedtls_common.c
 
 
-LIBMBEDTLS_SRCS		:= $(addprefix ${MBEDTLS_DIR}/library/,	\
+LIBMBEDTLS_SRCS		+= $(addprefix ${MBEDTLS_DIR}/library/,	\
 					aes.c 					\
 					asn1parse.c 				\
 					asn1write.c 				\
diff --git a/drivers/auth/mbedtls/mbedtls_crypto.c b/drivers/auth/mbedtls/mbedtls_crypto.c
index d231179..42a0925 100644
--- a/drivers/auth/mbedtls/mbedtls_crypto.c
+++ b/drivers/auth/mbedtls/mbedtls_crypto.c
@@ -115,7 +115,7 @@
 	end = (unsigned char *)(p + sig_len);
 	signature.tag = *p;
 	rc = mbedtls_asn1_get_bitstring_null(&p, end, &signature.len);
-	if (rc != 0) {
+	if ((rc != 0) || ((size_t)(end - p) != signature.len)) {
 		rc = CRYPTO_ERR_SIGNATURE;
 		goto end1;
 	}
@@ -170,12 +170,15 @@
 	size_t len;
 	int rc;
 
-	/* Digest info should be an MBEDTLS_ASN1_SEQUENCE */
+	/*
+	 * Digest info should be an MBEDTLS_ASN1_SEQUENCE
+	 * and consume all bytes.
+	 */
 	p = (unsigned char *)digest_info_ptr;
 	end = p + digest_info_len;
 	rc = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
 				  MBEDTLS_ASN1_SEQUENCE);
-	if (rc != 0) {
+	if (rc != 0 || ((size_t)(end - p) != len)) {
 		return CRYPTO_ERR_HASH;
 	}
 
@@ -195,9 +198,9 @@
 		return CRYPTO_ERR_HASH;
 	}
 
-	/* Hash should be octet string type */
+	/* Hash should be octet string type and consume all bytes */
 	rc = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_OCTET_STRING);
-	if (rc != 0) {
+	if ((rc != 0) || ((size_t)(end - p) != len)) {
 		return CRYPTO_ERR_HASH;
 	}
 
diff --git a/drivers/auth/mbedtls/mbedtls_x509_parser.c b/drivers/auth/mbedtls/mbedtls_x509_parser.c
index 993ef12..44b25ba 100644
--- a/drivers/auth/mbedtls/mbedtls_x509_parser.c
+++ b/drivers/auth/mbedtls/mbedtls_x509_parser.c
@@ -85,9 +85,6 @@
 	p = v3_ext.p;
 	end = v3_ext.p + v3_ext.len;
 
-	mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
-			     MBEDTLS_ASN1_SEQUENCE);
-
 	while (p < end) {
 		zeromem(&extn_oid, sizeof(extn_oid));
 		is_critical = 0; /* DEFAULT FALSE */
@@ -144,8 +141,23 @@
 {
 	int ret, is_critical;
 	size_t len;
-	unsigned char *p, *end, *crt_end;
-	mbedtls_asn1_buf sig_alg1, sig_alg2;
+	unsigned char *p, *end, *crt_end, *pk_end;
+	mbedtls_asn1_buf sig_alg1;
+	/*
+	 * The unique ASN.1 DER encoding of [0] EXPLICIT INTEGER { v3(2} }.
+	 */
+	static const char v3[] = {
+		/* The outer CONTEXT SPECIFIC 0 tag */
+		MBEDTLS_ASN1_CONSTRUCTED | MBEDTLS_ASN1_CONTEXT_SPECIFIC | 0,
+		/* The number bytes used to encode the inner INTEGER */
+		3,
+		/* The tag of the inner INTEGER */
+		MBEDTLS_ASN1_INTEGER,
+		/* The number of bytes needed to represent 2 */
+		1,
+		/* The actual value 2 */
+		2,
+	};
 
 	p = (unsigned char *)img;
 	len = img_len;
@@ -163,7 +175,7 @@
 		return IMG_PARSER_ERR_FORMAT;
 	}
 
-	if (len > (size_t)(end - p)) {
+	if (len != (size_t)(end - p)) {
 		return IMG_PARSER_ERR_FORMAT;
 	}
 	crt_end = p + len;
@@ -181,15 +193,14 @@
 	tbs.len = end - tbs.p;
 
 	/*
-	 * Version  ::=  INTEGER  {  v1(0), v2(1), v3(2)  }
+	 * Version  ::=  [0] EXPLICIT INTEGER {  v1(0), v2(1), v3(2)  }
+	 * -- only v3 accepted
 	 */
-	ret = mbedtls_asn1_get_tag(&p, end, &len,
-				   MBEDTLS_ASN1_CONTEXT_SPECIFIC |
-				   MBEDTLS_ASN1_CONSTRUCTED | 0);
-	if (ret != 0) {
+	if (((end - p) <= (ptrdiff_t)sizeof(v3)) ||
+	    (memcmp(p, v3, sizeof(v3)) != 0)) {
 		return IMG_PARSER_ERR_FORMAT;
 	}
-	p += len;
+	p += sizeof(v3);
 
 	/*
 	 * CertificateSerialNumber  ::=  INTEGER
@@ -257,9 +268,24 @@
 	if (ret != 0) {
 		return IMG_PARSER_ERR_FORMAT;
 	}
-	pk.len = (p + len) - pk.p;
+	pk_end = p + len;
+	pk.len = pk_end - pk.p;
+
+	/* algorithm */
+	ret = mbedtls_asn1_get_tag(&p, pk_end, &len, MBEDTLS_ASN1_CONSTRUCTED |
+				   MBEDTLS_ASN1_SEQUENCE);
+	if (ret != 0) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
 	p += len;
 
+	/* Key is a BIT STRING and must use all bytes in SubjectPublicKeyInfo */
+	ret = mbedtls_asn1_get_bitstring_null(&p, pk_end, &len);
+	if ((ret != 0) || (p + len != pk_end)) {
+		return IMG_PARSER_ERR_FORMAT;
+	}
+	p = pk_end;
+
 	/*
 	 * issuerUniqueID  [1]  IMPLICIT UniqueIdentifier OPTIONAL,
 	 */
@@ -290,29 +316,45 @@
 
 	/*
 	 * extensions      [3]  EXPLICIT Extensions OPTIONAL
+	 * }
+	 *
+	 * X.509 and RFC5280 allow omitting the extensions entirely.
+	 * However, in TF-A, a certificate with no extensions would
+	 * always fail later on, as the extensions contain the
+	 * information needed to authenticate the next stage in the
+	 * boot chain.  Furthermore, get_ext() assumes that the
+	 * extensions have been parsed into v3_ext, and allowing
+	 * there to be no extensions would pointlessly complicate
+	 * the code.  Therefore, just reject certificates without
+	 * extensions.  This is also why version 1 and 2 certificates
+	 * are rejected above.
 	 */
 	ret = mbedtls_asn1_get_tag(&p, end, &len,
 				   MBEDTLS_ASN1_CONTEXT_SPECIFIC |
 				   MBEDTLS_ASN1_CONSTRUCTED | 3);
-	if (ret != 0) {
+	if ((ret != 0) || (len != (size_t)(end - p))) {
 		return IMG_PARSER_ERR_FORMAT;
 	}
 
 	/*
 	 * Extensions  ::=  SEQUENCE SIZE (1..MAX) OF Extension
+	 * -- must use all remaining bytes in TBSCertificate
 	 */
-	v3_ext.p = p;
 	ret = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
 				   MBEDTLS_ASN1_SEQUENCE);
-	if (ret != 0) {
+	if ((ret != 0) || (len != (size_t)(end - p))) {
 		return IMG_PARSER_ERR_FORMAT;
 	}
-	v3_ext.len = (p + len) - v3_ext.p;
+	v3_ext.p = p;
+	v3_ext.len = len;
 
 	/*
-	 * Check extensions integrity
+	 * Check extensions integrity.  At least one extension is
+	 * required: the ASN.1 specifies a minimum size of 1, and at
+	 * least one extension is needed to authenticate the next stage
+	 * in the boot chain.
 	 */
-	while (p < end) {
+	do {
 		ret = mbedtls_asn1_get_tag(&p, end, &len,
 					   MBEDTLS_ASN1_CONSTRUCTED |
 					   MBEDTLS_ASN1_SEQUENCE);
@@ -340,7 +382,7 @@
 			return IMG_PARSER_ERR_FORMAT;
 		}
 		p += len;
-	}
+	} while (p < end);
 
 	if (p != end) {
 		return IMG_PARSER_ERR_FORMAT;
@@ -353,33 +395,22 @@
 	 *  -- end of TBSCertificate
 	 *
 	 *  signatureAlgorithm   AlgorithmIdentifier
+	 *  -- Does not need to be parsed.  Ensuring it is bitwise
+	 *  -- identical (including the tag!) with the first signature
+	 *  -- algorithm is sufficient.
 	 */
-	sig_alg2.p = p;
-	ret = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
-				   MBEDTLS_ASN1_SEQUENCE);
-	if (ret != 0) {
-		return IMG_PARSER_ERR_FORMAT;
-	}
-	if ((end - p) < 1) {
-		return IMG_PARSER_ERR_FORMAT;
-	}
-	sig_alg2.len = (p + len) - sig_alg2.p;
-	p += len;
-
-	/* Compare both signature algorithms */
-	if (sig_alg1.len != sig_alg2.len) {
-		return IMG_PARSER_ERR_FORMAT;
-	}
-	if (0 != memcmp(sig_alg1.p, sig_alg2.p, sig_alg1.len)) {
+	if ((sig_alg1.len >= (size_t)(end - p)) ||
+	    (0 != memcmp(sig_alg1.p, p, sig_alg1.len))) {
 		return IMG_PARSER_ERR_FORMAT;
 	}
+	p += sig_alg1.len;
 	memcpy(&sig_alg, &sig_alg1, sizeof(sig_alg));
 
 	/*
 	 * signatureValue       BIT STRING
 	 */
 	signature.p = p;
-	ret = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_BIT_STRING);
+	ret = mbedtls_asn1_get_bitstring_null(&p, end, &len);
 	if (ret != 0) {
 		return IMG_PARSER_ERR_FORMAT;
 	}
diff --git a/drivers/measured_boot/event_log/event_log.c b/drivers/measured_boot/event_log/event_log.c
index d661c35..6f2898d 100644
--- a/drivers/measured_boot/event_log/event_log.c
+++ b/drivers/measured_boot/event_log/event_log.c
@@ -14,8 +14,6 @@
 #include <drivers/auth/crypto_mod.h>
 #include <drivers/measured_boot/event_log/event_log.h>
 
-#include <plat/common/platform.h>
-
 #if TPM_ALG_ID == TPM_ALG_SHA512
 #define	CRYPTO_MD_ID	CRYPTO_MD_SHA512
 #elif TPM_ALG_ID == TPM_ALG_SHA384
@@ -32,9 +30,6 @@
 /* Pointer to the first byte past end of the Event Log buffer */
 static uintptr_t log_end;
 
-/* Pointer to event_log_metadata_t */
-static const event_log_metadata_t *plat_metadata_ptr;
-
 /* TCG_EfiSpecIdEvent */
 static const id_event_headers_t id_event_header = {
 	.header = {
@@ -173,10 +168,6 @@
 void event_log_init(uint8_t *event_log_start, uint8_t *event_log_finish)
 {
 	event_log_buf_init(event_log_start, event_log_finish);
-
-	/* Get pointer to platform's event_log_metadata_t structure */
-	plat_metadata_ptr = plat_event_log_get_metadata();
-	assert(plat_metadata_ptr != NULL);
 }
 
 void event_log_write_specid_event(void)
@@ -276,16 +267,19 @@
  * @param[in] data_base		Address of data
  * @param[in] data_size		Size of data
  * @param[in] data_id		Data ID
+ * @param[in] metadata_ptr	Event Log metadata
  * @return:
  *	0 = success
  *    < 0 = error
  */
 int event_log_measure_and_record(uintptr_t data_base, uint32_t data_size,
-				 uint32_t data_id)
+				 uint32_t data_id,
+				 const event_log_metadata_t *metadata_ptr)
 {
 	unsigned char hash_data[CRYPTO_MD_MAX_SIZE];
 	int rc;
-	const event_log_metadata_t *metadata_ptr = plat_metadata_ptr;
+
+	assert(metadata_ptr != NULL);
 
 	/* Get the metadata associated with this image. */
 	while ((metadata_ptr->id != EVLOG_INVALID_ID) &&
diff --git a/drivers/nxp/auth/csf_hdr_parser/csf_hdr_parser.c b/drivers/nxp/auth/csf_hdr_parser/csf_hdr_parser.c
index b878082..4f31c6e 100644
--- a/drivers/nxp/auth/csf_hdr_parser/csf_hdr_parser.c
+++ b/drivers/nxp/auth/csf_hdr_parser/csf_hdr_parser.c
@@ -38,7 +38,7 @@
 
 /* Flag to indicate if values are there in rotpk_hash_table */
 bool rotpk_not_dpld =  true;
-uint8_t rotpk_hash_table[MAX_KEY_ENTRIES][SHA256_BYTES];
+uint8_t rotpk_hash_table[MAX_KEY_ENTRIES][SHA256_BYTES] __aligned(CACHE_WRITEBACK_GRANULE);
 uint32_t num_rotpk_hash_entries;
 
 /*
diff --git a/drivers/nxp/crypto/caam/src/jobdesc.c b/drivers/nxp/crypto/caam/src/jobdesc.c
index f559c4b..92fcb74 100644
--- a/drivers/nxp/crypto/caam/src/jobdesc.c
+++ b/drivers/nxp/crypto/caam/src/jobdesc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2017-2020 NXP
+ * Copyright 2017-2022 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
@@ -11,13 +11,13 @@
 #include <stdio.h>
 #include <stdlib.h>
 
+#include <assert.h>
 #include "caam.h"
 #include <common/debug.h>
 #include "jobdesc.h"
 #include "rsa.h"
 #include "sec_hw_specific.h"
 
-
 /* Return Length of desctiptr from first word */
 uint32_t desc_length(uint32_t *desc)
 {
@@ -41,6 +41,8 @@
 {
 	uint32_t len = desc_length(desc);
 
+	assert((len + 1) < MAX_DESC_SIZE_WORDS);
+
 	/* Add Word at Last */
 	uint32_t *last = desc + len;
 	*last = word;
@@ -54,6 +56,9 @@
 {
 	uint32_t len = desc_length(desc);
 
+	assert((len + (uint32_t) (sizeof(phys_addr_t) / sizeof(uint32_t)))
+		< MAX_DESC_SIZE_WORDS);
+
 	/* Add Word at Last */
 	phys_addr_t *last = (phys_addr_t *) (desc + len);
 
diff --git a/drivers/nxp/ddr/nxp-ddr/regs.c b/drivers/nxp/ddr/nxp-ddr/regs.c
index cedd7ca..26155ab 100644
--- a/drivers/nxp/ddr/nxp-ddr/regs.c
+++ b/drivers/nxp/ddr/nxp-ddr/regs.c
@@ -1302,7 +1302,7 @@
 		return 0;
 	}
 
-	if ((bin[i].taamin_ps[j] == 0) ||
+	if (((bin[i].taamin_ps[j] == 0) && j > 0) ||
 	    (bin[i].taamin_ps[j] > taamin_ps && j > 0)) {
 		j--;
 	}
diff --git a/drivers/nxp/ddr/phy-gen2/phy.c b/drivers/nxp/ddr/phy-gen2/phy.c
index 9e52145..2006c04 100644
--- a/drivers/nxp/ddr/phy-gen2/phy.c
+++ b/drivers/nxp/ddr/phy-gen2/phy.c
@@ -241,12 +241,6 @@
 				rwmax = tmp;
 			}
 
-			tmp = wrmax;
-			wrmax = cdd[56];
-			if (tmp > wrmax) {
-				wrmax = tmp;
-			}
-
 			break;
 
 		case 2U:
@@ -276,15 +270,7 @@
 				rwmax = tmp;
 			}
 
-			buf[0] = cdd[56];
-			buf[1] = cdd[55];
-			buf[2] = cdd[52];
-			buf[3] = cdd[51];
-			tmp = wrmax;
-			wrmax = findmax(buf, 4U);
-			if (tmp > wrmax) {
-				wrmax = tmp;
-			}
+			wrmax = wwmax;
 
 			break;
 
@@ -310,12 +296,7 @@
 				rwmax = tmp;
 			}
 
-			tmp = wrmax;
-			c = &cdd[41];
-			wrmax = findmax(c, 16U);
-			if (tmp > wrmax) {
-				wrmax = tmp;
-			}
+			wrmax = wwmax;
 
 			break;
 
@@ -390,7 +371,12 @@
 
 #ifdef NXP_WARM_BOOT
 int save_phy_training_values(uint16_t **phy_ptr, uint32_t address_to_store,
-		uint32_t num_of_phy, int train2d)
+		uint32_t num_of_phy, int train2d
+#ifdef NXP_APPLY_MAX_CDD
+			, struct ddr_ctrl_reg_values *ddrctrl_regs
+#endif
+		)
+
 {
 	uint16_t *phy = NULL, value = 0x0;
 	uint32_t size = 1U, num_of_regs = 1U, phy_store = 0U;
@@ -457,6 +443,15 @@
 			ret = xspi_write(phy_store, training_2D_values,
 					size);
 		}
+
+#ifdef NXP_APPLY_MAX_CDD
+		/* Save DDR control register Timing CFG 0 and 4 */
+		phy_store  += size;
+		size = sizeof(ddrctrl_regs);
+		if (ret != 0) {
+			ret = xspi_write(phy_store, ddrctrl_regs, size);
+		}
+#endif
 		/* Disable clocks in case they were disabled. */
 		phy_io_write16(phy, t_drtub |
 				csr_ucclk_hclk_enables_addr, 0x0);
@@ -472,7 +467,11 @@
 }
 
 int restore_phy_training_values(uint16_t **phy_ptr, uint32_t address_to_restore,
-		uint32_t num_of_phy, int train2d)
+		uint32_t num_of_phy, int train2d
+#ifdef NXP_APPLY_MAX_CDD
+		, struct ddr_ctrl_reg_values *ddrctrl_regs
+#endif
+		)
 {
 	uint16_t *phy = NULL;
 	uint32_t size = 1U, num_of_regs = 1U, phy_store = 0U;
@@ -504,6 +503,14 @@
 		/* Reading 1D training values from flash*/
 		ret = xspi_read(phy_store, (uint32_t *)training_1D_values,
 				size);
+		if (ret != 0) {
+#ifdef DEBUG_WARM_RESET
+			debug("Unable to Read 1D training values %d\n",
+					ret);
+#endif
+			return -EINVAL;
+		}
+
 		debug("Restoring 1D Training reg val at:%08x\n", phy_store);
 		for (i = 0; i < num_of_regs; i++) {
 			phy_io_write16(phy, training_1D_values[i].addr,
@@ -523,6 +530,15 @@
 			/* Reading 2D training values from flash */
 			ret = xspi_read(phy_store,
 					(uint32_t *)training_2D_values,	size);
+
+			if (ret != 0) {
+#ifdef DEBUG_WARM_RESET
+				debug("Unable to Read 2D training values %d\n",
+						ret);
+#endif
+				return -EINVAL;
+			}
+
 			debug("Restoring 2D Training reg val at:%08x\n",
 					phy_store);
 			for (i = 0; i < num_of_regs; i++) {
@@ -537,6 +553,11 @@
 #endif
 			}
 		}
+#ifdef NXP_APPLY_MAX_CDD
+		phy_store = phy_store + size;
+		size = sizeof(ddrctrl_regs);
+		ret = xspi_read(phy_store, (uint32_t *)ddrctrl_regs, size);
+#endif
 		/* Disable clocks in case they were disabled. */
 		phy_io_write16(phy, t_drtub |
 				csr_ucclk_hclk_enables_addr, 0x0);
@@ -2292,6 +2313,7 @@
 
 	if (i < 0 || i > 3) {
 		printf("Error: invalid chip-select value\n");
+		return;
 	}
 	switch (val) {
 	case DDR_ODT_CS:
@@ -2473,6 +2495,9 @@
 	__unused const soc_info_t *soc_info;
 #ifdef NXP_APPLY_MAX_CDD
 	unsigned int tcfg0, tcfg4, rank;
+#ifdef NXP_WARM_BOOT
+	struct ddr_ctrl_reg_values ddrctrl_regs;
+#endif
 #endif
 
 	if (dimm_param == NULL) {
@@ -2577,11 +2602,19 @@
 		ret = restore_phy_training_values(priv->phy,
 						  PHY_TRAINING_REGS_ON_FLASH,
 						  priv->num_ctlrs,
-						  input.basic.train2d);
+						  input.basic.train2d
+#ifdef NXP_APPLY_MAX_CDD
+						, &ddrctrl_regs
+#endif
+						);
 		if (ret != 0) {
 			ERROR("Restoring of training data failed %d\n", ret);
 			return ret;
 		}
+#ifdef NXP_APPLY_MAX_CDD
+		regs->timing_cfg[0] = ddrctrl_regs.timing_cfg0;
+		regs->timing_cfg[4] = ddrctrl_regs.timing_cfg4;
+#endif
 	} else {
 #endif
 		/* Mapping IMG buffer firstly */
@@ -2644,12 +2677,20 @@
 #ifdef NXP_WARM_BOOT
 		if (priv->warm_boot_flag != DDR_WRM_BOOT_NT_SUPPORTED &&
 		    ret == 0) {
+#ifdef NXP_APPLY_MAX_CDD
+			ddrctrl_regs.timing_cfg0 = regs->timing_cfg[0];
+			ddrctrl_regs.timing_cfg4 = regs->timing_cfg[4];
+#endif
 			debug("save the phy training data\n");
 			//Save training data TBD
 			ret = save_phy_training_values(priv->phy,
 						PHY_TRAINING_REGS_ON_FLASH,
 						priv->num_ctlrs,
-						input.basic.train2d);
+						input.basic.train2d
+#ifdef NXP_APPLY_MAX_CDD
+						, &ddrctrl_regs
+#endif
+						);
 			if (ret != 0) {
 				ERROR("Saving training data failed.");
 				ERROR("Warm boot will fail. Error=%d.\n", ret);
diff --git a/drivers/nxp/ddr/phy-gen2/phy.h b/drivers/nxp/ddr/phy-gen2/phy.h
index 15e80d1..5e80f36 100644
--- a/drivers/nxp/ddr/phy-gen2/phy.h
+++ b/drivers/nxp/ddr/phy-gen2/phy.h
@@ -11,11 +11,18 @@
 /* To store sector size to be erase on flash*/
 #define PHY_ERASE_SIZE F_SECTOR_ERASE_SZ
 
+/*Structure to save DDR controller timing register 0 and 4 values*/
+struct ddr_ctrl_reg_values {
+	uint32_t timing_cfg0;
+	uint32_t timing_cfg4;
+};
+
 /*Structure to implement address-data map tuples to store PHY training values*/
 struct phy_training_values {
 	uint32_t addr;
 	uint16_t data;
 };
+
 /* Saves PHY Training Register values after cold reset
  *@param[in] phy_ptr array to store addresses of PHYs
  *@param[in] address_to_store address to save PHY training register values
@@ -24,6 +31,8 @@
  *to be saved
  *@param[in] train2d flag to store whether 2D training registers are to
  *be saved or not
+ *@param[in] ddrctrl_regs to save ddr controller registers in case
+ *NXP_APPLY_MAX_CDD is applied
  *
  *PHY training values will be stored on flash at contigous memory in the order:
  *1D training registers, 2D training registers
@@ -31,9 +40,13 @@
  *
  *if train2d is false saving 2D training registers will be skipped
  */
-int save_phy_training_values(uint16_t **phy_ptr, uint32_t address_to_store,
-		uint32_t num_of_phy, int train2d);
 
+int save_phy_training_values(uint16_t **phy_ptr, uint32_t address_to_store,
+		uint32_t num_of_phy, int train2d
+#ifdef NXP_APPLY_MAX_CDD
+		, struct ddr_ctrl_reg_values *ddrctrl_regs
+#endif
+		);
 /*Restores PHY Training Register values after warm reset
  *@param[in] phy_ptr array to store addresses of PHYs
  *@param[in] address_to_store address to retrieve PHY training register
@@ -42,12 +55,17 @@
  *to be restored
  *@param[in] train2d flag to store whether 2D training registers are
  *to be restored or not
- *
+ *@param[in] ddrctrl_regs to restore  ddr controller registers in case
+ *NXP_APPLY_MAX_CDD is applied
  *if train2d is false saving 2D training registers will be skipped
  */
 
 int restore_phy_training_values(uint16_t **phy_ptr, uint32_t address_to_restore,
-		uint32_t num_of_phy, int train2d);
+		uint32_t num_of_phy, int train2d
+#ifdef NXP_APPLY_MAX_CDD
+		, struct ddr_ctrl_reg_values *ddrctrl_regs
+#endif
+		);
 
 /*
  * Address data tuples to store the PHY 1D
diff --git a/drivers/nxp/flexspi/nor/fspi.c b/drivers/nxp/flexspi/nor/fspi.c
index 7c919b8..1e8c5a2 100644
--- a/drivers/nxp/flexspi/nor/fspi.c
+++ b/drivers/nxp/flexspi/nor/fspi.c
@@ -123,6 +123,9 @@
 		cmd_id1 = FSPI_NOR_CMD_RDSR;
 		cmd_id2 = FSPI_NOR_CMD_RDSR;
 		break;
+	default:
+		ERROR("Unsupported command\n");
+		return;
 	}
 
 	x_addr = FSPI_LUTREG_OFFSET + (uint32_t)(0x10 * fspi_op_seq_id);
diff --git a/drivers/nxp/tzc/plat_tzc380.c b/drivers/nxp/tzc/plat_tzc380.c
index 13cf3b9..5b27563 100644
--- a/drivers/nxp/tzc/plat_tzc380.c
+++ b/drivers/nxp/tzc/plat_tzc380.c
@@ -91,20 +91,37 @@
 	}
 	/* Continue with list entries for index > 0 */
 	if (dram_idx == 0) {
-		/* TZC Region 1 on DRAM0 for Secure Memory*/
+		/*
+		 * Region 1: Secure Region on DRAM 1 for  2MB out of  2MB,
+		 * excluding 0 sub-region(=256KB).
+		 */
 		tzc380_reg_list[list_idx].secure = TZC_ATTR_SP_S_RW;
 		tzc380_reg_list[list_idx].enabled = TZC_ATTR_REGION_ENABLE;
 		tzc380_reg_list[list_idx].addr = dram_start_addr + dram_size;
-		tzc380_reg_list[list_idx].size = secure_dram_sz;
+		tzc380_reg_list[list_idx].size = TZC_REGION_SIZE_2M;
 		tzc380_reg_list[list_idx].sub_mask = 0x0; /* all enabled */
 		list_idx++;
 
-		/* TZC Region 2 on DRAM0 for Shared Memory*/
+		/*
+		 * Region 2: Secure Region on DRAM 1 for 54MB out of 64MB,
+		 * excluding 1 sub-rgion(=8MB) of 8MB.
+		 */
 		tzc380_reg_list[list_idx].secure = TZC_ATTR_SP_S_RW;
 		tzc380_reg_list[list_idx].enabled = TZC_ATTR_REGION_ENABLE;
+		tzc380_reg_list[list_idx].addr = dram_start_addr + dram_size + shrd_dram_sz;
+		tzc380_reg_list[list_idx].size = TZC_REGION_SIZE_64M;
+		tzc380_reg_list[list_idx].sub_mask = 0x80; /* Disable sub-region 7 */
+		list_idx++;
+
+		/*
+		 * Region 3: Secure Region on DRAM 1 for  6MB out of  8MB,
+		 * excluding 2 sub-rgion(=1MB) of 2MB.
+		 */
+		tzc380_reg_list[list_idx].secure = TZC_ATTR_SP_S_RW;
+		tzc380_reg_list[list_idx].enabled = TZC_ATTR_REGION_ENABLE;
 		tzc380_reg_list[list_idx].addr = dram_start_addr + dram_size + secure_dram_sz;
-		tzc380_reg_list[list_idx].size = shrd_dram_sz;
-		tzc380_reg_list[list_idx].sub_mask = 0x0; /* all enabled */
+		tzc380_reg_list[list_idx].size = TZC_REGION_SIZE_8M;
+		tzc380_reg_list[list_idx].sub_mask = 0xC0; /* Disable sub-region 6 & 7 */
 		list_idx++;
 
 	}
diff --git a/drivers/st/clk/clk-stm32-core.c b/drivers/st/clk/clk-stm32-core.c
index bb03125..9fe8c8c 100644
--- a/drivers/st/clk/clk-stm32-core.c
+++ b/drivers/st/clk/clk-stm32-core.c
@@ -466,10 +466,9 @@
 {
 	const struct clk_stm32 *clk = _clk_get(priv, id);
 	int parent;
-	unsigned long rate = 0UL;
 
 	if ((unsigned int)id >= priv->num) {
-		return rate;
+		return 0UL;
 	}
 
 	parent = _clk_stm32_get_parent(priv, id);
@@ -484,21 +483,14 @@
 			prate = _clk_stm32_get_rate(priv, parent);
 		}
 
-		rate = clk->ops->recalc_rate(priv, id, prate);
-
-		return rate;
+		return clk->ops->recalc_rate(priv, id, prate);
 	}
 
-	switch (parent) {
-	case CLK_IS_ROOT:
+	if (parent == CLK_IS_ROOT) {
 		panic();
-
-	default:
-		rate = _clk_stm32_get_rate(priv, parent);
-		break;
 	}
-	return rate;
 
+	return _clk_stm32_get_rate(priv, parent);
 }
 
 unsigned long _clk_stm32_get_parent_rate(struct stm32_clk_priv *priv, int id)
@@ -519,7 +511,7 @@
 
 bool _stm32_clk_is_flags(struct stm32_clk_priv *priv, int id, uint8_t flag)
 {
-	if (_stm32_clk_get_flags(priv, id) & flag) {
+	if ((_stm32_clk_get_flags(priv, id) & flag) != 0U) {
 		return true;
 	}
 
@@ -549,7 +541,7 @@
 		}
 		if (parent != CLK_IS_ROOT) {
 			ret = _clk_stm32_enable_core(priv, parent);
-			if (ret) {
+			if (ret != 0) {
 				return ret;
 			}
 		}
diff --git a/drivers/st/clk/clk-stm32mp13.c b/drivers/st/clk/clk-stm32mp13.c
index c960928..db427ad 100644
--- a/drivers/st/clk/clk-stm32mp13.c
+++ b/drivers/st/clk/clk-stm32mp13.c
@@ -456,7 +456,7 @@
 	},\
 }
 
-static const struct parent_cfg parent_mp13[] = {
+static const struct parent_cfg parent_mp13[MUX_MAX] = {
 	MUX_CFG(MUX_ADC1,	ADC1_src,	RCC_ADC12CKSELR, 0, 2),
 	MUX_CFG(MUX_ADC2,	ADC2_src,	RCC_ADC12CKSELR, 2, 2),
 	MUX_RDY_CFG(MUX_AXI,	AXI_src,	RCC_ASSCKSELR, 0, 3),
@@ -841,7 +841,7 @@
 		.bitrdy	= _bitrdy,\
 }
 
-static const struct div_cfg dividers_mp13[] = {
+static const struct div_cfg dividers_mp13[DIV_MAX] = {
 	DIV_CFG(DIV_PLL1DIVP, RCC_PLL1CFGR2, 0, 7, 0, NULL, DIV_NO_BIT_RDY),
 	DIV_CFG(DIV_PLL2DIVP, RCC_PLL2CFGR2, 0, 7, 0, NULL, DIV_NO_BIT_RDY),
 	DIV_CFG(DIV_PLL2DIVQ, RCC_PLL2CFGR2, 8, 7, 0, NULL, DIV_NO_BIT_RDY),
@@ -1119,7 +1119,7 @@
 		return ret;
 	}
 
-	if (enable) {
+	if (enable != 0) {
 		clk_stm32_enable_call_ops(priv, clk_id);
 	} else {
 		clk_stm32_disable_call_ops(priv, clk_id);
@@ -1450,7 +1450,7 @@
 
 	/* Configure PLLs source */
 	ret = stm32_clk_configure_mux(priv, pll_conf->vco.src);
-	if (ret) {
+	if (ret != 0) {
 		return ret;
 	}
 
@@ -1485,7 +1485,7 @@
 {
 	struct stm32_pll_dt_cfg *pll_conf = clk_stm32_pll_get_pdata(pll_idx);
 
-	if (pll_conf->vco.status) {
+	if (pll_conf->vco.status != 0U) {
 		return _clk_stm32_pll_init(priv, pll_idx, pll_conf);
 	}
 
@@ -1497,22 +1497,22 @@
 	int err = 0;
 
 	err = clk_stm32_pll_init(priv, _PLL1);
-	if (err) {
+	if (err != 0) {
 		return err;
 	}
 
 	err = clk_stm32_pll_init(priv, _PLL2);
-	if (err) {
+	if (err != 0) {
 		return err;
 	}
 
 	err = clk_stm32_pll_init(priv, _PLL3);
-	if (err) {
+	if (err != 0) {
 		return err;
 	}
 
 	err = clk_stm32_pll_init(priv, _PLL4);
-	if (err) {
+	if (err != 0) {
 		return err;
 	}
 
@@ -2242,7 +2242,7 @@
 	size_t i = 0U;
 
 	for (i = _PLL1; i < pdata->npll; i++) {
-		struct stm32_pll_dt_cfg *pll = pdata->pll + i;
+		struct stm32_pll_dt_cfg *pll = &pdata->pll[i];
 		char name[RCC_PLL_NAME_SIZE];
 		int subnode = 0;
 		int err = 0;
diff --git a/drivers/st/crypto/stm32_pka.c b/drivers/st/crypto/stm32_pka.c
index e03cf0f..2bbb31d 100644
--- a/drivers/st/crypto/stm32_pka.c
+++ b/drivers/st/crypto/stm32_pka.c
@@ -254,13 +254,6 @@
 
 static struct stm32_pka_platdata pka_pdata;
 
-#pragma weak stm32_pka_get_platdata
-
-int stm32_pka_get_platdata(struct stm32_pka_platdata *pdata)
-{
-	return -ENODEV;
-}
-
 static int stm32_pka_parse_fdt(void)
 {
 	int node;
@@ -583,10 +576,7 @@
 
 	err = stm32_pka_parse_fdt();
 	if (err != 0) {
-		err = stm32_pka_get_platdata(&pka_pdata);
-		if (err != 0) {
-			return err;
-		}
+		return err;
 	}
 
 	clk_enable(pka_pdata.clock_id);
diff --git a/drivers/st/crypto/stm32_saes.c b/drivers/st/crypto/stm32_saes.c
index 02baf21..f4da571 100644
--- a/drivers/st/crypto/stm32_saes.c
+++ b/drivers/st/crypto/stm32_saes.c
@@ -139,15 +139,8 @@
 #define SET_CHAINING_MODE(mod, cr) \
 	mmio_clrsetbits_32((cr), _SAES_CR_CHMOD_MASK, _SAES_CR_CHMOD_##mod << _SAES_CR_CHMOD_SHIFT)
 
-#define pragma weak stm32_saes_get_platdata
-
 static struct stm32_saes_platdata saes_pdata;
 
-int stm32_saes_get_platdata(struct stm32_saes_platdata *pdata)
-{
-	return -ENODEV;
-}
-
 static int stm32_saes_parse_fdt(struct stm32_saes_platdata *pdata)
 {
 	int node;
@@ -389,10 +382,7 @@
 
 	err = stm32_saes_parse_fdt(&saes_pdata);
 	if (err != 0) {
-		err = stm32_saes_get_platdata(&saes_pdata);
-		if (err != 0) {
-			return err;
-		}
+		return err;
 	}
 
 	clk_enable(saes_pdata.clock_id);
diff --git a/drivers/st/gpio/stm32_gpio.c b/drivers/st/gpio/stm32_gpio.c
index 708989f..a4a64ca 100644
--- a/drivers/st/gpio/stm32_gpio.c
+++ b/drivers/st/gpio/stm32_gpio.c
@@ -234,27 +234,27 @@
 	clk_enable(clock);
 
 	mmio_clrsetbits_32(base + GPIO_MODE_OFFSET,
-			   (uint32_t)GPIO_MODE_MASK << (pin << 1),
-			   mode << (pin << 1));
+			   (uint32_t)GPIO_MODE_MASK << (pin << 1U),
+			   mode << (pin << 1U));
 
 	mmio_clrsetbits_32(base + GPIO_TYPE_OFFSET,
 			   (uint32_t)GPIO_TYPE_MASK << pin,
 			   type << pin);
 
 	mmio_clrsetbits_32(base + GPIO_SPEED_OFFSET,
-			   (uint32_t)GPIO_SPEED_MASK << (pin << 1),
-			   speed << (pin << 1));
+			   (uint32_t)GPIO_SPEED_MASK << (pin << 1U),
+			   speed << (pin << 1U));
 
 	mmio_clrsetbits_32(base + GPIO_PUPD_OFFSET,
-			   (uint32_t)GPIO_PULL_MASK << (pin << 1),
-			   pull << (pin << 1));
+			   (uint32_t)GPIO_PULL_MASK << (pin << 1U),
+			   pull << (pin << 1U));
 
 	if (pin < GPIO_ALT_LOWER_LIMIT) {
 		mmio_clrsetbits_32(base + GPIO_AFRL_OFFSET,
-				   (uint32_t)GPIO_ALTERNATE_MASK << (pin << 2),
-				   alternate << (pin << 2));
+				   (uint32_t)GPIO_ALTERNATE_MASK << (pin << 2U),
+				   alternate << (pin << 2U));
 	} else {
-		size_t shift = (pin - GPIO_ALT_LOWER_LIMIT) << 2;
+		uint32_t shift = (pin - GPIO_ALT_LOWER_LIMIT) << 2U;
 
 		mmio_clrsetbits_32(base + GPIO_AFRH_OFFSET,
 				   (uint32_t)GPIO_ALTERNATE_MASK << shift,
diff --git a/drivers/st/mmc/stm32_sdmmc2.c b/drivers/st/mmc/stm32_sdmmc2.c
index 6bdd782..1ee3580 100644
--- a/drivers/st/mmc/stm32_sdmmc2.c
+++ b/drivers/st/mmc/stm32_sdmmc2.c
@@ -528,12 +528,12 @@
 	uint32_t data_ctrl = SDMMC_DCTRLR_DTDIR;
 	uint32_t arg_size;
 
-	assert(size != 0U);
+	assert((size != 0U) && (size <= UINT32_MAX));
 
 	if (size > MMC_BLOCK_SIZE) {
 		arg_size = MMC_BLOCK_SIZE;
 	} else {
-		arg_size = size;
+		arg_size = (uint32_t)size;
 	}
 
 	sdmmc2_params.use_dma = plat_sdmmc2_use_dma(base, buf);
diff --git a/drivers/st/pmic/stm32mp_pmic.c b/drivers/st/pmic/stm32mp_pmic.c
index 5b43760..1e16287 100644
--- a/drivers/st/pmic/stm32mp_pmic.c
+++ b/drivers/st/pmic/stm32mp_pmic.c
@@ -20,6 +20,7 @@
 #include <platform_def.h>
 
 #define PMIC_NODE_NOT_FOUND	1
+#define NB_REG			14U
 
 static struct i2c_handle_s i2c_handle;
 static uint32_t pmic_i2c_addr;
@@ -454,13 +455,13 @@
 };
 
 #define DEFINE_REGU(name) { \
-	.node_name = name, \
+	.node_name = (name), \
 	.ops = &pmic_ops, \
 	.driver_data = NULL, \
 	.enable_ramp_delay = 1000, \
 }
 
-static const struct regul_description pmic_regs[] = {
+static const struct regul_description pmic_regs[NB_REG] = {
 	[STPMIC1_BUCK1] = DEFINE_REGU("buck1"),
 	[STPMIC1_BUCK2] = DEFINE_REGU("buck2"),
 	[STPMIC1_BUCK3] = DEFINE_REGU("buck3"),
@@ -477,8 +478,6 @@
 	[STPMIC1_SW_OUT] = DEFINE_REGU("pwr_sw2"),
 };
 
-#define NB_REG ARRAY_SIZE(pmic_regs)
-
 static int register_pmic(void)
 {
 	void *fdt;
@@ -506,7 +505,7 @@
 		unsigned int i;
 		int ret;
 
-		for (i = 0; i < NB_REG; i++) {
+		for (i = 0U; i < NB_REG; i++) {
 			desc = &pmic_regs[i];
 			if (strcmp(desc->node_name, reg_name) == 0) {
 				break;
diff --git a/drivers/st/regulator/regulator_core.c b/drivers/st/regulator/regulator_core.c
index 5cc8329..2a5d0f7 100644
--- a/drivers/st/regulator/regulator_core.c
+++ b/drivers/st/regulator/regulator_core.c
@@ -17,14 +17,16 @@
 
 #define MAX_PROPERTY_LEN 64
 
+CASSERT(PLAT_NB_RDEVS >= 1U, plat_nb_rdevs_must_be_higher);
+
 static struct rdev rdev_array[PLAT_NB_RDEVS];
 
 #define for_each_rdev(rdev) \
-	for (rdev = rdev_array; rdev < (rdev_array + PLAT_NB_RDEVS); rdev++)
+	for ((rdev) = rdev_array; (rdev) <= &rdev_array[PLAT_NB_RDEVS - 1U]; (rdev)++)
 
 #define for_each_registered_rdev(rdev) \
-	for (rdev = rdev_array; \
-	     (rdev < (rdev_array + PLAT_NB_RDEVS)) && (rdev->desc != NULL); rdev++)
+	for ((rdev) = rdev_array; \
+	     ((rdev) <= &rdev_array[PLAT_NB_RDEVS - 1U]) && ((rdev)->desc != NULL); (rdev)++)
 
 static void lock_driver(const struct rdev *rdev)
 {
@@ -86,7 +88,7 @@
 	char prop_name[MAX_PROPERTY_LEN];
 
 	len = snprintf(prop_name, MAX_PROPERTY_LEN - 1, "%s-supply", name);
-	assert((len >= 0) && (len < MAX_PROPERTY_LEN - 1));
+	assert((len >= 0) && (len < (MAX_PROPERTY_LEN - 1)));
 
 	cuint = fdt_getprop(fdt, node, prop_name, NULL);
 	if (cuint != NULL) {
@@ -156,7 +158,7 @@
 
 	assert(rdev != NULL);
 
-	if (rdev->flags & REGUL_ALWAYS_ON) {
+	if ((rdev->flags & REGUL_ALWAYS_ON) != 0U) {
 		return 0;
 	}
 
@@ -525,7 +527,7 @@
 		}
 	}
 
-	if (rdev == rdev_array + PLAT_NB_RDEVS) {
+	if (rdev > &rdev_array[PLAT_NB_RDEVS - 1U]) {
 		WARN("Not enough place for regulators, PLAT_NB_RDEVS should be increased.\n");
 		return -ENOMEM;
 	}
diff --git a/fdts/fvp-base-psci-common.dtsi b/fdts/fvp-base-psci-common.dtsi
index 6018f0c..7838fde 100644
--- a/fdts/fvp-base-psci-common.dtsi
+++ b/fdts/fvp-base-psci-common.dtsi
@@ -28,7 +28,7 @@
 	#size-cells = <2>;
 
 #if (ENABLE_RME == 1)
-	chosen { bootargs = "mem=1G console=ttyAMA0 earlycon=pl011,0x1c090000 root=/dev/vda ip=on";};
+	chosen { bootargs = "console=ttyAMA0 earlycon=pl011,0x1c090000 root=/dev/vda ip=on";};
 #else
 	chosen {};
 #endif
diff --git a/fdts/tc.dts b/fdts/tc.dts
index 5a8792e..192f407 100644
--- a/fdts/tc.dts
+++ b/fdts/tc.dts
@@ -213,9 +213,9 @@
 			linux,cma-default;
 		};
 
-		optee@0xfce00000 {
-			reg = <0x00000000 0xfce00000 0 0x00200000>;
-			no-map;
+		optee@0xf8e00000 {
+			compatible = "restricted-dma-pool";
+			reg = <0x00000000 0xf8e00000 0 0x00200000>;
 		};
 	};
 
@@ -463,17 +463,18 @@
 		interrupt-names = "JOB", "MMU", "GPU";
 		clocks = <&soc_refclk100mhz>;
 		clock-names = "clk_mali";
+		iommus = <&smmu_700 0x200>;
 		operating-points = <
 			/* KHz uV */
 			50000 820000
 		>;
 	};
 
-	smmu: smmu@2ce00000 {
+	smmu_700: smmu_700@3f000000 {
 		#iommu-cells = <1>;
 		compatible = "arm,smmu-v3";
-		reg = <0x0 0x2ce00000 0x0 0x20000>;
-		status = "okay";
+		reg = <0x0 0x3f000000 0x0 0x5000000>;
+		dma-coherent;
 	};
 
 	dp0: display@2cc00000 {
@@ -485,9 +486,7 @@
 		interrupt-names = "DPU";
 		clocks = <&scmi_clk 0>;
 		clock-names = "aclk";
-		iommus = <&smmu 0>, <&smmu 1>, <&smmu 2>, <&smmu 3>,
-			<&smmu 4>, <&smmu 5>, <&smmu 6>, <&smmu 7>,
-			<&smmu 8>, <&smmu 9>;
+		iommus = <&smmu_700 0x100>;
 		pl0: pipeline@0 {
 			reg = <0>;
 			clocks = <&scmi_clk 1>;
diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index f63e923..9e13c3d 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -1282,6 +1282,12 @@
 #define GCR_EL1			S3_0_C1_C0_6
 
 /*******************************************************************************
+ * Armv8.5 - Random Number Generator Registers
+ ******************************************************************************/
+#define RNDR			S3_3_C2_C4_0
+#define RNDRRS			S3_3_C2_C4_1
+
+/*******************************************************************************
  * FEAT_HCX - Extended Hypervisor Configuration Register
  ******************************************************************************/
 #define HCRX_EL2		S3_4_C1_C2_2
diff --git a/include/arch/aarch64/arch_helpers.h b/include/arch/aarch64/arch_helpers.h
index 50a5ad4..fe9b5a5 100644
--- a/include/arch/aarch64/arch_helpers.h
+++ b/include/arch/aarch64/arch_helpers.h
@@ -546,8 +546,8 @@
 DEFINE_RENAME_SYSREG_RW_FUNCS(gcr_el1, GCR_EL1)
 
 /* Armv8.5 FEAT_RNG Registers */
-DEFINE_SYSREG_READ_FUNC(rndr)
-DEFINE_SYSREG_READ_FUNC(rndrrs)
+DEFINE_RENAME_SYSREG_READ_FUNC(rndr, RNDR)
+DEFINE_RENAME_SYSREG_READ_FUNC(rndrrs, RNDRRS)
 
 /* FEAT_HCX Register */
 DEFINE_RENAME_SYSREG_RW_FUNCS(hcrx_el2, HCRX_EL2)
diff --git a/include/bl31/sync_handle.h b/include/bl31/sync_handle.h
new file mode 100644
index 0000000..e211575
--- /dev/null
+++ b/include/bl31/sync_handle.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2022, ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef TRAP_HANDLE_H
+#define TRAP_HANDLE_H
+
+#include <stdbool.h>
+#include <context.h>
+
+#define ISS_SYSREG_OPCODE_MASK		0x3ffc1eUL
+#define ISS_SYSREG_REG_MASK		0x0003e0UL
+#define ISS_SYSREG_REG_SHIFT		5U
+#define ISS_SYSREG_DIRECTION_MASK	0x000001UL
+
+#define ISS_SYSREG_OPCODE_RNDR		0x30c808U
+#define ISS_SYSREG_OPCODE_RNDRRS	0x32c808U
+
+#define TRAP_RET_UNHANDLED		-1
+#define TRAP_RET_REPEAT			0
+#define TRAP_RET_CONTINUE		1
+
+#ifndef __ASSEMBLER__
+static inline unsigned int get_sysreg_iss_rt(uint64_t esr)
+{
+	return (esr & ISS_SYSREG_REG_MASK) >> ISS_SYSREG_REG_SHIFT;
+}
+
+static inline bool is_sysreg_iss_write(uint64_t esr)
+{
+	return !(esr & ISS_SYSREG_DIRECTION_MASK);
+}
+
+/**
+ * handle_sysreg_trap() - Handle AArch64 system register traps from lower ELs
+ * @esr_el3: The content of ESR_EL3, containing the trap syndrome information
+ * @ctx: Pointer to the lower EL context, containing saved registers
+ *
+ * Called by the exception handler when a synchronous trap identifies as a
+ * system register trap (EC=0x18). ESR contains the encoding of the op[x] and
+ * CRm/CRn fields, to identify the system register, and the target/source
+ * GPR plus the direction (MRS/MSR). The lower EL's context can be altered
+ * by the function, to inject back the result of the emulation.
+ *
+ * Return: indication how to proceed with the trap:
+ *   TRAP_RET_UNHANDLED(-1): trap is unhandled, trigger panic
+ *   TRAP_RET_REPEAT(0): trap was handled, return to the trapping instruction
+ *			 (repeating it)
+ *   TRAP_RET_CONTINUE(1): trap was handled, return to the next instruction
+ *		           (continuing after it)
+ */
+int handle_sysreg_trap(uint64_t esr_el3, cpu_context_t *ctx);
+
+/* Prototypes for system register emulation handlers provided by platforms. */
+int plat_handle_rng_trap(uint64_t esr_el3, cpu_context_t *ctx);
+
+#endif /* __ASSEMBLER__ */
+
+#endif
diff --git a/include/drivers/arm/css/scmi.h b/include/drivers/arm/css/scmi.h
index 9dd08e5..356012b 100644
--- a/include/drivers/arm/css/scmi.h
+++ b/include/drivers/arm/css/scmi.h
@@ -168,7 +168,7 @@
 int scmi_ap_core_get_reset_addr(void *p, uint64_t *reset_addr, uint32_t *attr);
 
 /* API to get the platform specific SCMI channel information. */
-scmi_channel_plat_info_t *plat_css_get_scmi_info(int channel_id);
+scmi_channel_plat_info_t *plat_css_get_scmi_info(unsigned int channel_id);
 
 /* API to override default PSCI callbacks for platforms that support SCMI. */
 const plat_psci_ops_t *css_scmi_override_pm_ops(plat_psci_ops_t *ops);
diff --git a/include/drivers/measured_boot/event_log/event_log.h b/include/drivers/measured_boot/event_log/event_log.h
index eb0e2b1..794d613 100644
--- a/include/drivers/measured_boot/event_log/event_log.h
+++ b/include/drivers/measured_boot/event_log/event_log.h
@@ -115,13 +115,13 @@
 void event_log_write_specid_event(void);
 void event_log_write_header(void);
 void dump_event_log(uint8_t *log_addr, size_t log_size);
-const event_log_metadata_t *plat_event_log_get_metadata(void);
 int event_log_measure(uintptr_t data_base, uint32_t data_size,
 		      unsigned char hash_data[CRYPTO_MD_MAX_SIZE]);
 void event_log_record(const uint8_t *hash, uint32_t event_type,
 		      const event_log_metadata_t *metadata_ptr);
 int event_log_measure_and_record(uintptr_t data_base, uint32_t data_size,
-				 uint32_t data_id);
+				 uint32_t data_id,
+				 const event_log_metadata_t *metadata_ptr);
 size_t event_log_get_cur_size(uint8_t *event_log_start);
 
 #endif /* EVENT_LOG_H */
diff --git a/include/drivers/nxp/smmu/nxp_smmu.h b/include/drivers/nxp/smmu/nxp_smmu.h
index d64c33b..bc17703 100644
--- a/include/drivers/nxp/smmu/nxp_smmu.h
+++ b/include/drivers/nxp/smmu/nxp_smmu.h
@@ -10,10 +10,13 @@
 
 #define SMMU_SCR0		(0x0)
 #define SMMU_NSCR0		(0x400)
+#define SMMU_SACR		(0x10)
 
 #define SCR0_CLIENTPD_MASK	0x00000001
 #define SCR0_USFCFG_MASK	0x00000400
 
+#define SMMU_SACR_CACHE_LOCK_ENABLE_BIT      (1ULL << 26U)
+
 static inline void bypass_smmu(uintptr_t smmu_base_addr)
 {
 	uint32_t val;
@@ -27,4 +30,13 @@
 	mmio_write_32((smmu_base_addr + SMMU_NSCR0), val);
 }
 
+static inline void smmu_cache_unlock(uintptr_t smmu_base_addr)
+{
+	uint32_t val;
+
+	val = mmio_read_32((smmu_base_addr + SMMU_SACR));
+	val &= (uint32_t)~SMMU_SACR_CACHE_LOCK_ENABLE_BIT;
+	mmio_write_32((smmu_base_addr + SMMU_SACR), val);
+}
+
 #endif
diff --git a/include/lib/fconf/fconf.h b/include/lib/fconf/fconf.h
index 131c542..3762021 100644
--- a/include/lib/fconf/fconf.h
+++ b/include/lib/fconf/fconf.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, ARM Limited. All rights reserved.
+ * Copyright (c) 2019-2022, ARM Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -21,7 +21,7 @@
  */
 #define FCONF_REGISTER_POPULATOR(config, name, callback)			\
 	__attribute__((used, section(".fconf_populator")))			\
-	const struct fconf_populator (name##__populator) = {			\
+	static const struct fconf_populator (name##__populator) = {		\
 		.config_type = (#config),					\
 		.info = (#name),						\
 		.populate = (callback)						\
diff --git a/include/lib/libc/aarch32/limits_.h b/include/lib/libc/aarch32/limits_.h
index 26cec17..a67ec53 100644
--- a/include/lib/libc/aarch32/limits_.h
+++ b/include/lib/libc/aarch32/limits_.h
@@ -1,11 +1,11 @@
 /*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2023, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #define SCHAR_MAX  0x7F
-#define SCHAR_MIN  (-SCHAR_MIN - 1)
+#define SCHAR_MIN  (-SCHAR_MAX - 1)
 #define CHAR_MAX   0x7F
 #define CHAR_MIN   (-CHAR_MAX - 1)
 #define UCHAR_MAX  0xFFU
diff --git a/include/lib/libc/aarch64/limits_.h b/include/lib/libc/aarch64/limits_.h
index e36cfe7..1bb0681 100644
--- a/include/lib/libc/aarch64/limits_.h
+++ b/include/lib/libc/aarch64/limits_.h
@@ -1,11 +1,11 @@
 /*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2023, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #define SCHAR_MAX  0x7F
-#define SCHAR_MIN  (-SCHAR_MIN - 1)
+#define SCHAR_MIN  (-SCHAR_MAX - 1)
 #define CHAR_MAX   0x7F
 #define CHAR_MIN   (-CHAR_MAX - 1)
 #define UCHAR_MAX  0xFFU
diff --git a/include/lib/psa/measured_boot.h b/include/lib/psa/measured_boot.h
index 47aa0b9..231da2c 100644
--- a/include/lib/psa/measured_boot.h
+++ b/include/lib/psa/measured_boot.h
@@ -74,4 +74,53 @@
 				     size_t measurement_value_size,
 				     bool lock_measurement);
 
+/**
+ * Retrieves a measurement from the requested slot.
+ *
+ * index			Slot number from which measurement is to be
+ *				retrieved.
+ * signer_id			Pointer to signer_id buffer.
+ * signer_id_size		Size of the signer_id buffer in bytes.
+ * signer_id_len		On success, number of bytes that make up
+ * 				signer_id.
+ * version			Pointer to version buffer.
+ * version_size			Size of the version buffer in bytes.
+ * version_len			On success, number of bytes that makeup the
+ * 				version.
+ * measurement_algo		Pointer to measurement_algo.
+ * sw_type			Pointer to sw_type buffer.
+ * sw_type_size			Size of the sw_type buffer in bytes.
+ * sw_type_len			On success, number of bytes that makeup the
+ * 				sw_type.
+ * measurement_value		Pointer to measurement_value buffer.
+ * measurement_value_size	Size of the measurement_value buffer in bytes.
+ * measurement_value_len	On success, number of bytes that make up the
+ * 				measurement_value.
+ * is_locked			Pointer to lock status of requested measurement
+ * 				slot.
+ *
+ * PSA_SUCCESS
+ *	- Success.
+ * PSA_ERROR_INVALID_ARGUMENT
+ *	- The size of at least one of the output buffers is incorrect or the
+ *	  requested slot index is invalid.
+ * PSA_ERROR_DOES_NOT_EXIST
+ *	- The requested slot is empty, does not contain a measurement.
+ */
+psa_status_t rss_measured_boot_read_measurement(uint8_t index,
+					uint8_t *signer_id,
+					size_t signer_id_size,
+					size_t *signer_id_len,
+					uint8_t *version,
+					size_t version_size,
+					size_t *version_len,
+					uint32_t *measurement_algo,
+					uint8_t *sw_type,
+					size_t sw_type_size,
+					size_t *sw_type_len,
+					uint8_t *measurement_value,
+					size_t measurement_value_size,
+					size_t *measurement_value_len,
+					bool *is_locked);
+
 #endif /* PSA_MEASURED_BOOT_H */
diff --git a/include/plat/arm/common/arm_def.h b/include/plat/arm/common/arm_def.h
index ab0e4ff..36b1bdb 100644
--- a/include/plat/arm/common/arm_def.h
+++ b/include/plat/arm/common/arm_def.h
@@ -20,10 +20,12 @@
  *****************************************************************************/
 
 /*
- * Root of trust key hash lengths
+ * Root of trust key lengths
  */
 #define ARM_ROTPK_HEADER_LEN		19
 #define ARM_ROTPK_HASH_LEN		32
+/* ARM_ROTPK_KEY_LEN includes DER header + raw key material */
+#define ARM_ROTPK_KEY_LEN		294
 
 /* Special value used to verify platform parameters from BL2 to BL31 */
 #define ARM_BL31_PLAT_PARAM_VAL		ULL(0x0f1e2d3c4b5a6978)
diff --git a/include/plat/arm/common/arm_pas_def.h b/include/plat/arm/common/arm_pas_def.h
index c199302..fba8d2c 100644
--- a/include/plat/arm/common/arm_pas_def.h
+++ b/include/plat/arm/common/arm_pas_def.h
@@ -21,24 +21,30 @@
  * ============================================================================
  * 0GB      | 1GB         |L0 GPT|ANY   |TBROM (EL3 code)        |Fixed mapping
  *          |             |      |      |TSRAM (EL3 data)        |
- *          |             |      |      |IO (incl.UARTs & GIC)   |
+ * 00000000 |             |      |      |IO (incl.UARTs & GIC)   |
  * ----------------------------------------------------------------------------
  * 1GB      | 1GB         |L0 GPT|ANY   |IO                      |Fixed mapping
+ * 40000000 |             |      |      |                        |
  * ----------------------------------------------------------------------------
- * 2GB      | 1GB         |L1 GPT|NS    |DRAM (NS Kernel)        |Use T.Descrip
+ * 2GB      |2GB-64MB     |L1 GPT|NS    |DRAM (NS Kernel)        |Use T.Descrip
+ * 80000000 |             |      |      |                        |
  * ----------------------------------------------------------------------------
- * 3GB      |1GB-64MB     |L1 GPT|NS    |DRAM (NS Kernel)        |Use T.Descrip
- * ----------------------------------------------------------------------------
- * 4GB-64MB |64MB-32MB    |      |      |                        |
- *          | -4MB        |L1 GPT|SECURE|DRAM TZC                |Use T.Descrip
+ * 4GB-64MB |64MB-32MB-4MB|L1 GPT|SECURE|DRAM TZC                |Use T.Descrip
+ * FC000000 |             |      |      |                        |
  * ----------------------------------------------------------------------------
  * 4GB-32MB |             |      |      |                        |
  * -3MB-1MB |32MB         |L1 GPT|REALM |RMM                     |Use T.Descrip
+ * FDC00000 |             |      |      |                        |
  * ----------------------------------------------------------------------------
  * 4GB-3MB  |             |      |      |                        |
  * -1MB     |3MB          |L1 GPT|ROOT  |EL3 DRAM data           |Use T.Descrip
+ * FFC00000 |             |      |      |                        |
  * ----------------------------------------------------------------------------
  * 4GB-1MB  |1MB          |L1 GPT|ROOT  |DRAM (L1 GPTs, SCP TZC) |Fixed mapping
+ * FFF00000 |             |      |      |                        |
+ * ----------------------------------------------------------------------------
+ * 34GB     |2GB          |L1 GPT|NS    |DRAM (NS Kernel)        |Use T.Descrip
+ * 880000000|             |      |      |                        |
  * ============================================================================
  *
  * - 4KB of L0 GPT reside in TSRAM, on top of the CONFIG section.
@@ -55,7 +61,7 @@
 
 /* Device memory 0 to 2GB */
 #define ARM_PAS_1_BASE			(U(0))
-#define ARM_PAS_1_SIZE			((ULL(1)<<31)) /* 2GB */
+#define ARM_PAS_1_SIZE			((ULL(1) << 31)) /* 2GB */
 
 /* NS memory 2GB to (end - 64MB) */
 #define ARM_PAS_2_BASE			(ARM_PAS_1_BASE + ARM_PAS_1_SIZE)
@@ -69,9 +75,14 @@
 #define ARM_PAS_3_BASE			(ARM_AP_TZC_DRAM1_BASE)
 #define ARM_PAS_3_SIZE			(ARM_AP_TZC_DRAM1_SIZE)
 
+/* NS memory 2GB */
+#define	ARM_PAS_4_BASE			ARM_DRAM2_BASE
+#define	ARM_PAS_4_SIZE			((ULL(1) << 31)) /* 2GB */
+
 #define ARM_PAS_GPI_ANY			MAP_GPT_REGION(ARM_PAS_1_BASE, \
 						       ARM_PAS_1_SIZE, \
 						       GPT_GPI_ANY)
+
 #define	ARM_PAS_KERNEL			GPT_MAP_REGION_GRANULE(ARM_PAS_2_BASE, \
 							       ARM_PAS_2_SIZE, \
 							       GPT_GPI_NS)
@@ -80,6 +91,9 @@
 							       ARM_PAS_3_SIZE, \
 							       GPT_GPI_SECURE)
 
+#define	ARM_PAS_KERNEL_1		GPT_MAP_REGION_GRANULE(ARM_PAS_4_BASE, \
+							       ARM_PAS_4_SIZE, \
+							       GPT_GPI_NS)
 /*
  * REALM and Shared area share the same PAS, so consider them a single
  * PAS region to configure in GPT.
diff --git a/include/plat/arm/common/plat_arm.h b/include/plat/arm/common/plat_arm.h
index 6c0d91d..494e470 100644
--- a/include/plat/arm/common/plat_arm.h
+++ b/include/plat/arm/common/plat_arm.h
@@ -159,7 +159,7 @@
 #define ARM_ROTPK_REGS_ID		1
 #define ARM_ROTPK_DEVEL_RSA_ID		2
 #define ARM_ROTPK_DEVEL_ECDSA_ID	3
-
+#define ARM_ROTPK_DEVEL_FULL_DEV_RSA_KEY_ID	4
 
 /* IO storage utility functions */
 int arm_io_setup(void);
diff --git a/include/plat/common/platform.h b/include/plat/common/platform.h
index 8407bbd..3351036 100644
--- a/include/plat/common/platform.h
+++ b/include/plat/common/platform.h
@@ -421,4 +421,17 @@
 void plat_fwu_set_images_source(const struct fwu_metadata *metadata);
 uint32_t plat_fwu_get_boot_idx(void);
 
+/*
+ * Optional function to indicate if cache management operations can be
+ * performed.
+ */
+#if CONDITIONAL_CMO
+uint64_t plat_can_cmo(void);
+#else
+static inline uint64_t plat_can_cmo(void)
+{
+	return 1;
+}
+#endif /* CONDITIONAL_CMO */
+
 #endif /* PLATFORM_H */
diff --git a/include/services/rmm_core_manifest.h b/include/services/rmm_core_manifest.h
index 2f25858..7edef46 100644
--- a/include/services/rmm_core_manifest.h
+++ b/include/services/rmm_core_manifest.h
@@ -38,6 +38,7 @@
 /* Boot manifest core structure as per v0.1 */
 typedef struct rmm_manifest {
 	uint32_t version;	/* Manifest version */
+	uint32_t padding;	/* RES0 */
 	uintptr_t plat_data;	/* Manifest platform data */
 } rmm_manifest_t;
 
diff --git a/include/services/trp/trp_helpers.h b/include/services/trp/trp_helpers.h
index 8e786e2..83ec740 100644
--- a/include/services/trp/trp_helpers.h
+++ b/include/services/trp/trp_helpers.h
@@ -39,5 +39,12 @@
 
 __dead2 void trp_boot_abort(uint64_t err);
 
+/* TRP SMC result registers X0-X4 */
+#define TRP_SMC_RESULT_REGS	5
+
+struct trp_smc_result {
+	unsigned long long x[TRP_SMC_RESULT_REGS];
+};
+
 #endif /* __ASSEMBLER __ */
 #endif /* TRP_HELPERS_H */
diff --git a/lib/cpus/aarch64/cortex_a710.S b/lib/cpus/aarch64/cortex_a710.S
index fed3f33..3ea55df 100644
--- a/lib/cpus/aarch64/cortex_a710.S
+++ b/lib/cpus/aarch64/cortex_a710.S
@@ -482,6 +482,30 @@
 	b	cpu_rev_var_ls
 endfunc check_errata_2371105
 
+/* ----------------------------------------------------
+ * Errata Workaround for Cortex-A710 Errata #2768515
+ * This applies to revisions <= r2p1 and is still open.
+ * x0: variant[4:7] and revision[0:3] of current cpu.
+ * Shall clobber: x0-x17
+ * ----------------------------------------------------
+ */
+func errata_a710_2768515_wa
+	mov	x17, x30
+	bl	check_errata_2768515
+	cbz	x0, 1f
+
+	/* dsb before isb of power down sequence */
+	dsb	sy
+1:
+	ret	x17
+endfunc errata_a710_2768515_wa
+
+func check_errata_2768515
+	/* Applies to all revisions <= r2p1 */
+	mov	x1, #0x21
+	b	cpu_rev_var_ls
+endfunc check_errata_2768515
+
 func check_errata_cve_2022_23960
 #if WORKAROUND_CVE_2022_23960
 	mov	x0, #ERRATA_APPLIES
@@ -518,6 +542,12 @@
 	mrs	x0, CORTEX_A710_CPUPWRCTLR_EL1
 	orr	x0, x0, #CORTEX_A710_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
 	msr	CORTEX_A710_CPUPWRCTLR_EL1, x0
+#if ERRATA_A710_2768515
+	mov	x15, x30
+	bl	cpu_get_rev_var
+	bl	errata_a710_2768515_wa
+	mov	x30, x15
+#endif /* ERRATA_A710_2768515 */
 	isb
 	ret
 endfunc cortex_a710_core_pwr_dwn
@@ -550,6 +580,7 @@
 	report_errata ERRATA_A710_2216384, cortex_a710, 2216384
 	report_errata ERRATA_A710_2291219, cortex_a710, 2291219
 	report_errata ERRATA_A710_2371105, cortex_a710, 2371105
+	report_errata ERRATA_A710_2768515, cortex_a710, 2768515
 	report_errata WORKAROUND_CVE_2022_23960, cortex_a710, cve_2022_23960
 	report_errata ERRATA_DSU_2313941, cortex_a710, dsu_2313941
 
diff --git a/lib/cpus/aarch64/cortex_a78.S b/lib/cpus/aarch64/cortex_a78.S
index dd3487a..38f58bb 100644
--- a/lib/cpus/aarch64/cortex_a78.S
+++ b/lib/cpus/aarch64/cortex_a78.S
@@ -326,6 +326,31 @@
 	b	cpu_rev_var_ls
 endfunc check_errata_2395406
 
+/* ----------------------------------------------------
+ * Errata Workaround for Cortex-A78 Errata 2772019
+ * This applies to revisions <= r1p2 and is still open.
+ * x0: variant[4:7] and revision[0:3] of current cpu.
+ * Shall clobber: x0-x17
+ * ----------------------------------------------------
+ */
+func errata_a78_2772019_wa
+	mov	x17, x30
+	bl	check_errata_2772019
+	cbz	x0, 1f
+
+
+	/* dsb before isb of power down sequence */
+	dsb	sy
+1:
+	ret	x17
+endfunc errata_a78_2772019_wa
+
+func check_errata_2772019
+	/* Applies to all revisions <= r1p2 */
+	mov	x1, #0x12
+	b	cpu_rev_var_ls
+endfunc check_errata_2772019
+
 func check_errata_cve_2022_23960
 #if WORKAROUND_CVE_2022_23960
 	mov	x0, #ERRATA_APPLIES
@@ -434,6 +459,12 @@
 	mrs	x0, CORTEX_A78_CPUPWRCTLR_EL1
 	orr	x0, x0, #CORTEX_A78_CPUPWRCTLR_EL1_CORE_PWRDN_EN_BIT
 	msr	CORTEX_A78_CPUPWRCTLR_EL1, x0
+#if ERRATA_A78_2772019
+	mov	x15, x30
+	bl	cpu_get_rev_var
+	bl	errata_a78_2772019_wa
+	mov	x30, x15
+#endif /* ERRATA_A78_2772019 */
 	isb
 	ret
 endfunc cortex_a78_core_pwr_dwn
@@ -461,6 +492,7 @@
 	report_errata ERRATA_A78_2242635, cortex_a78, 2242635
 	report_errata ERRATA_A78_2376745, cortex_a78, 2376745
 	report_errata ERRATA_A78_2395406, cortex_a78, 2395406
+	report_errata ERRATA_A78_2772019, cortex_a78, 2772019
 	report_errata WORKAROUND_CVE_2022_23960, cortex_a78, cve_2022_23960
 
 	ldp	x8, x30, [sp], #16
diff --git a/lib/cpus/aarch64/cortex_x2.S b/lib/cpus/aarch64/cortex_x2.S
index c810be6..f56d50a 100644
--- a/lib/cpus/aarch64/cortex_x2.S
+++ b/lib/cpus/aarch64/cortex_x2.S
@@ -295,6 +295,30 @@
 	b	cpu_rev_var_ls
 endfunc check_errata_2371105
 
+/* ----------------------------------------------------
+ * Errata Workaround for Cortex-X2 Errata #2768515
+ * This applies to revisions <= r2p1 and is still open.
+ * x0: variant[4:7] and revision[0:3] of current cpu.
+ * Shall clobber: x0-x17
+ * ----------------------------------------------------
+ */
+func errata_x2_2768515_wa
+	mov	x17, x30
+	bl	check_errata_2768515
+	cbz	x0, 1f
+
+	/* dsb before isb of power down sequence */
+	dsb	sy
+1:
+	ret	x17
+endfunc errata_x2_2768515_wa
+
+func check_errata_2768515
+	/* Applies to all revisions <= r2p1 */
+	mov	x1, #0x21
+	b	cpu_rev_var_ls
+endfunc check_errata_2768515
+
 	/* ----------------------------------------------------
 	 * HW will do the cache maintenance while powering down
 	 * ----------------------------------------------------
@@ -307,6 +331,12 @@
 	mrs	x0, CORTEX_X2_CPUPWRCTLR_EL1
 	orr	x0, x0, #CORTEX_X2_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
 	msr	CORTEX_X2_CPUPWRCTLR_EL1, x0
+#if ERRATA_X2_2768515
+	mov	x15, x30
+	bl	cpu_get_rev_var
+	bl	errata_x2_2768515_wa
+	mov	x30, x15
+#endif /* ERRATA_X2_2768515 */
 	isb
 	ret
 endfunc cortex_x2_core_pwr_dwn
@@ -333,6 +363,7 @@
 	report_errata ERRATA_X2_2147715, cortex_x2, 2147715
 	report_errata ERRATA_X2_2216384, cortex_x2, 2216384
 	report_errata ERRATA_X2_2371105, cortex_x2, 2371105
+	report_errata ERRATA_X2_2768515, cortex_x2, 2768515
 	report_errata WORKAROUND_CVE_2022_23960, cortex_x2, cve_2022_23960
 	report_errata ERRATA_DSU_2313941, cortex_x2, dsu_2313941
 
diff --git a/lib/cpus/aarch64/neoverse_n2.S b/lib/cpus/aarch64/neoverse_n2.S
index 5861dec..dbf5941 100644
--- a/lib/cpus/aarch64/neoverse_n2.S
+++ b/lib/cpus/aarch64/neoverse_n2.S
@@ -428,6 +428,30 @@
 	b	cpu_rev_var_ls
 endfunc check_errata_2388450
 
+/* -------------------------------------------------------
+ * Errata Workaround for Neoverse N2 Erratum 2743089.
+ * This applies to revisions <= r0p2 and is fixed in r0p3.
+ * x0: variant[4:7] and revision[0:3] of current cpu.
+ * Shall clobber: x0-x17
+ * -------------------------------------------------------
+ */
+func errata_n2_2743089_wa
+	mov	x17, x30
+	bl	check_errata_2743089
+	cbz	x0, 1f
+
+	/* dsb before isb of power down sequence */
+	dsb	sy
+1:
+	ret	x17
+endfunc errata_n2_2743089_wa
+
+func check_errata_2743089
+	/* Applies to all revisions <= r0p2 */
+	mov	x1, #0x02
+	b	cpu_rev_var_ls
+endfunc check_errata_2743089
+
 func check_errata_cve_2022_23960
 #if WORKAROUND_CVE_2022_23960
 	mov	x0, #ERRATA_APPLIES
@@ -576,6 +600,12 @@
 	mrs	x0, NEOVERSE_N2_CPUPWRCTLR_EL1
 	orr	x0, x0, #NEOVERSE_N2_CORE_PWRDN_EN_BIT
 	msr	NEOVERSE_N2_CPUPWRCTLR_EL1, x0
+#if ERRATA_N2_2743089
+	mov	x15, x30
+	bl	cpu_get_rev_var
+	bl	errata_n2_2743089_wa
+	mov	x30, x15
+#endif /* ERRATA_N2_2743089 */
 	isb
 	ret
 endfunc neoverse_n2_core_pwr_dwn
@@ -607,6 +637,7 @@
 	report_errata ERRATA_N2_2326639, neoverse_n2, 2326639
 	report_errata ERRATA_N2_2376738, neoverse_n2, 2376738
 	report_errata ERRATA_N2_2388450, neoverse_n2, 2388450
+	report_errata ERRATA_N2_2743089, neoverse_n2, 2743089
 	report_errata WORKAROUND_CVE_2022_23960, neoverse_n2, cve_2022_23960
 	report_errata ERRATA_DSU_2313941, neoverse_n2, dsu_2313941
 
diff --git a/lib/cpus/aarch64/neoverse_v1.S b/lib/cpus/aarch64/neoverse_v1.S
index 3282fbc..c3a70ca 100644
--- a/lib/cpus/aarch64/neoverse_v1.S
+++ b/lib/cpus/aarch64/neoverse_v1.S
@@ -462,6 +462,30 @@
 	b	cpu_rev_var_ls
 endfunc check_errata_2372203
 
+	/* ----------------------------------------------------
+	 * Errata Workaround for Neoverse V1 Errata #2743093.
+	 * This applies to revisions <= r1p2 and is still open.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0-x17
+	 * ----------------------------------------------------
+	 */
+func errata_neoverse_v1_2743093_wa
+	mov	x17, x30
+	bl	check_errata_2743093
+	cbz	x0, 1f
+
+	/* dsb before isb of power down sequence */
+	dsb	sy
+1:
+	ret	x17
+endfunc errata_neoverse_v1_2743093_wa
+
+func check_errata_2743093
+	/* Applies to all revisions <= r1p2 */
+	mov	x1, #0x12
+	b	cpu_rev_var_ls
+endfunc check_errata_2743093
+
 func check_errata_cve_2022_23960
 #if WORKAROUND_CVE_2022_23960
 	mov	x0, #ERRATA_APPLIES
@@ -483,6 +507,12 @@
 	mrs	x0, NEOVERSE_V1_CPUPWRCTLR_EL1
 	orr	x0, x0, #NEOVERSE_V1_CPUPWRCTLR_EL1_CORE_PWRDN_BIT
 	msr	NEOVERSE_V1_CPUPWRCTLR_EL1, x0
+#if ERRATA_V1_2743093
+	mov	x15, x30
+	bl	cpu_get_rev_var
+	bl	errata_neoverse_v1_2743093_wa
+	mov	x30, x15
+#endif /* ERRATA_V1_2743093 */
 	isb
 	ret
 endfunc neoverse_v1_core_pwr_dwn
@@ -513,6 +543,7 @@
 	report_errata ERRATA_V1_2216392, neoverse_v1, 2216392
 	report_errata ERRATA_V1_2294912, neoverse_v1, 2294912
 	report_errata ERRATA_V1_2372203, neoverse_v1, 2372203
+	report_errata ERRATA_V1_2743093, neoverse_v1, 2743093
 	report_errata WORKAROUND_CVE_2022_23960, neoverse_v1, cve_2022_23960
 
 	ldp	x8, x30, [sp], #16
diff --git a/lib/cpus/cpu-ops.mk b/lib/cpus/cpu-ops.mk
index 8ef794b..b1f7d27 100644
--- a/lib/cpus/cpu-ops.mk
+++ b/lib/cpus/cpu-ops.mk
@@ -357,6 +357,11 @@
 # to revisions r0p0, r1p0, r1p1, and r1p2 of the A78 cpu. It is still open.
 ERRATA_A78_2395406	?=0
 
+# Flag to apply erratum 2772019 workaround during powerdown. This erratum
+# applies to revisions r0p0, r1p0, r1p1 and r1p2 of the A78 cpu. It is still
+# open.
+ERRATA_A78_2772019	?=0
+
 # Flag to apply erratum 1941500 workaround during reset. This erratum applies
 # to revisions r0p0 and r0p1 of the A78 AE cpu. It is still open.
 ERRATA_A78_AE_1941500	?=0
@@ -462,10 +467,6 @@
 # applies to all revisions <= r4p1 of the Neoverse N1 cpu and is still open.
 ERRATA_N1_2743102	?=0
 
-# Flag to apply erratum 2002655 workaround during reset. This erratum applies
-# to revisions r0p0 of the Neoverse-N2 cpu, it is still open.
-ERRATA_N2_2002655	?=0
-
 # Flag to apply erratum 1618635 workaround during reset. This erratum applies
 # to revision r0p0 of the Neoverse V1 cpu and was fixed in the revision r1p0.
 ERRATA_V1_1618635	?=0
@@ -516,6 +517,11 @@
 # to revisions r0p0, r1p0 and r1p1 of the Neoverse V1 cpu and is still open.
 ERRATA_V1_2372203	?=0
 
+# Flag to apply erratum 2743093 workaround during powerdown. This erratum
+# applies to revisions r0p0, r1p0, r1p1 and r1p2  of the Neoverse V1 cpu and is
+# still open.
+ERRATA_V1_2743093	?=0
+
 # Flag to apply erratum 1987031 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is still open.
 ERRATA_A710_1987031	?=0
@@ -572,6 +578,15 @@
 # to revision r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is fixed in r2p1.
 ERRATA_A710_2371105	?=0
 
+# Flag to apply erratum 2768515 workaround during power down. This erratum
+# applies to revision r0p0, r1p0, r2p0 and r2p1 of the Cortex-A710 cpu and is
+# still open.
+ERRATA_A710_2768515	?=0
+
+# Flag to apply erratum 2002655 workaround during reset. This erratum applies
+# to revisions r0p0 of the Neoverse-N2 cpu, it is still open.
+ERRATA_N2_2002655	?=0
+
 # Flag to apply erratum 2067956 workaround during reset. This erratum applies
 # to revision r0p0 of the Neoverse N2 cpu and is still open.
 ERRATA_N2_2067956	?=0
@@ -620,6 +635,10 @@
 # to revision r0p0 of the Neoverse N2 cpu, it is fixed in r0p1.
 ERRATA_N2_2388450	?=0
 
+# Flag to apply erratum 2743089 workaround during during powerdown. This erratum
+# applies to all revisions <= r0p2 of the Neoverse N2 cpu, it is fixed in r0p3.
+ERRATA_N2_2743089	?=0
+
 # Flag to apply erratum 2002765 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0, and r2p0 of the Cortex-X2 cpu and is still open.
 ERRATA_X2_2002765	?=0
@@ -655,6 +674,11 @@
 # to revision r0p0, r1p0 and r2p0 of the Cortex-X2 cpu and is fixed in r2p1.
 ERRATA_X2_2371105	?=0
 
+# Flag to apply erratum 2768515 workaround during power down. This erratum
+# applies to revision r0p0, r1p0, r2p0 and r2p1 of the Cortex-X2 cpu and is
+# still open.
+ERRATA_X2_2768515	?=0
+
 # Flag to apply erratum 2313909 workaround on powerdown. This erratum applies
 # to revisions r0p0 and r1p0 of the Cortex-X3 cpu, it is fixed in r1p1.
 ERRATA_X3_2313909	?=0
@@ -996,6 +1020,10 @@
 $(eval $(call assert_boolean,ERRATA_A78_2395406))
 $(eval $(call add_define,ERRATA_A78_2395406))
 
+# Process ERRATA_A78_2772019 flag
+$(eval $(call assert_boolean,ERRATA_A78_2772019))
+$(eval $(call add_define,ERRATA_A78_2772019))
+
 # Process ERRATA_A78_AE_1941500 flag
 $(eval $(call assert_boolean,ERRATA_A78_AE_1941500))
 $(eval $(call add_define,ERRATA_A78_AE_1941500))
@@ -1099,10 +1127,6 @@
 # Process ERRATA_N1_2743102 flag
 $(eval $(call assert_boolean,ERRATA_N1_2743102))
 $(eval $(call add_define,ERRATA_N1_2743102))
-#
-# Process ERRATA_N2_2002655 flag
-$(eval $(call assert_boolean,ERRATA_N2_2002655))
-$(eval $(call add_define,ERRATA_N2_2002655))
 
 # Process ERRATA_V1_1618635 flag
 $(eval $(call assert_boolean,ERRATA_V1_1618635))
@@ -1152,6 +1176,10 @@
 $(eval $(call assert_boolean,ERRATA_V1_2372203))
 $(eval $(call add_define,ERRATA_V1_2372203))
 
+# Process ERRATA_V1_2743093 flag
+$(eval $(call assert_boolean,ERRATA_V1_2743093))
+$(eval $(call add_define,ERRATA_V1_2743093))
+
 # Process ERRATA_A710_1987031 flag
 $(eval $(call assert_boolean,ERRATA_A710_1987031))
 $(eval $(call add_define,ERRATA_A710_1987031))
@@ -1208,6 +1236,14 @@
 $(eval $(call assert_boolean,ERRATA_A710_2371105))
 $(eval $(call add_define,ERRATA_A710_2371105))
 
+# Process ERRATA_A710_2768515 flag
+$(eval $(call assert_boolean,ERRATA_A710_2768515))
+$(eval $(call add_define,ERRATA_A710_2768515))
+
+# Process ERRATA_N2_2002655 flag
+$(eval $(call assert_boolean,ERRATA_N2_2002655))
+$(eval $(call add_define,ERRATA_N2_2002655))
+
 # Process ERRATA_N2_2067956 flag
 $(eval $(call assert_boolean,ERRATA_N2_2067956))
 $(eval $(call add_define,ERRATA_N2_2067956))
@@ -1256,6 +1292,10 @@
 $(eval $(call assert_boolean,ERRATA_N2_2388450))
 $(eval $(call add_define,ERRATA_N2_2388450))
 
+# Process ERRATA_N2_2743089 flag
+$(eval $(call assert_boolean,ERRATA_N2_2743089))
+$(eval $(call add_define,ERRATA_N2_2743089))
+
 # Process ERRATA_X2_2002765 flag
 $(eval $(call assert_boolean,ERRATA_X2_2002765))
 $(eval $(call add_define,ERRATA_X2_2002765))
@@ -1288,6 +1328,10 @@
 $(eval $(call assert_boolean,ERRATA_X2_2371105))
 $(eval $(call add_define,ERRATA_X2_2371105))
 
+# Process ERRATA_X2_2768515 flag
+$(eval $(call assert_boolean,ERRATA_X2_2768515))
+$(eval $(call add_define,ERRATA_X2_2768515))
+
 # Process ERRATA_X3_2313909 flag
 $(eval $(call assert_boolean,ERRATA_X3_2313909))
 $(eval $(call add_define,ERRATA_X3_2313909))
diff --git a/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index 6b88a90..60501f6 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S
@@ -788,6 +788,15 @@
 #endif /* CTX_INCLUDE_FPREGS */
 
 	/*
+	 * Set SCR_EL3.EA bit to enable SErrors at EL3
+	 */
+	.macro enable_serror_at_el3
+	mrs     x8, scr_el3
+	orr     x8, x8, #SCR_EA_BIT
+	msr     scr_el3, x8
+	.endm
+
+	/*
 	 * Set the PSTATE bits not set when the exception was taken as
 	 * described in the AArch64.TakeException() pseudocode function
 	 * in ARM DDI 0487F.c page J1-7635 to a default value.
@@ -917,6 +926,7 @@
  */
 func prepare_el3_entry
 	save_gp_pmcr_pauth_regs
+	enable_serror_at_el3
 	/*
 	 * Set the PSTATE bits not described in the Aarch64.TakeException
 	 * pseudocode to their default values.
@@ -1064,16 +1074,6 @@
 	msr	spsel, #MODE_SP_ELX
 	str	x17, [sp, #CTX_EL3STATE_OFFSET + CTX_RUNTIME_SP]
 
-	/* ----------------------------------------------------------
-	 * Restore SPSR_EL3, ELR_EL3 and SCR_EL3 prior to ERET
-	 * ----------------------------------------------------------
-	 */
-	ldr	x18, [sp, #CTX_EL3STATE_OFFSET + CTX_SCR_EL3]
-	ldp	x16, x17, [sp, #CTX_EL3STATE_OFFSET + CTX_SPSR_EL3]
-	msr	scr_el3, x18
-	msr	spsr_el3, x16
-	msr	elr_el3, x17
-
 #if IMAGE_BL31
 	/* ----------------------------------------------------------
 	 * Restore CPTR_EL3.
@@ -1103,17 +1103,6 @@
 1:
 #endif /* IMAGE_BL31 && DYNAMIC_WORKAROUND_CVE_2018_3639 */
 
-	restore_ptw_el1_sys_regs
-
-	/* ----------------------------------------------------------
-	 * Restore general purpose (including x30), PMCR_EL0 and
-	 * ARMv8.3-PAuth registers.
-	 * Exit EL3 via ERET to a lower exception level.
- 	 * ----------------------------------------------------------
- 	 */
-	bl	restore_gp_pmcr_pauth_regs
-	ldr	x30, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_LR]
-
 #if IMAGE_BL31 && RAS_EXTENSION
 	/* ----------------------------------------------------------
 	 * Issue Error Synchronization Barrier to synchronize SErrors
@@ -1127,6 +1116,27 @@
 	dsb	sy
 #endif /* IMAGE_BL31 && RAS_EXTENSION */
 
+	/* ----------------------------------------------------------
+	 * Restore SPSR_EL3, ELR_EL3 and SCR_EL3 prior to ERET
+	 * ----------------------------------------------------------
+	 */
+	ldr	x18, [sp, #CTX_EL3STATE_OFFSET + CTX_SCR_EL3]
+	ldp	x16, x17, [sp, #CTX_EL3STATE_OFFSET + CTX_SPSR_EL3]
+	msr	scr_el3, x18
+	msr	spsr_el3, x16
+	msr	elr_el3, x17
+
+	restore_ptw_el1_sys_regs
+
+	/* ----------------------------------------------------------
+	 * Restore general purpose (including x30), PMCR_EL0 and
+	 * ARMv8.3-PAuth registers.
+	 * Exit EL3 via ERET to a lower exception level.
+ 	 * ----------------------------------------------------------
+ 	 */
+	bl	restore_gp_pmcr_pauth_regs
+	ldr	x30, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_LR]
+
 #ifdef IMAGE_BL31
 	str	xzr, [sp, #CTX_EL3STATE_OFFSET + CTX_IS_IN_EL3]
 #endif /* IMAGE_BL31 */
diff --git a/lib/gpt_rme/gpt_rme.c b/lib/gpt_rme/gpt_rme.c
index a6e17a3..f5353cb 100644
--- a/lib/gpt_rme/gpt_rme.c
+++ b/lib/gpt_rme/gpt_rme.c
@@ -762,7 +762,7 @@
  * Return
  *   Negative Linux error code in the event of a failure, 0 for success.
  */
-int gpt_init_l0_tables(unsigned int pps, uintptr_t l0_mem_base,
+int gpt_init_l0_tables(gpccr_pps_e pps, uintptr_t l0_mem_base,
 		       size_t l0_mem_size)
 {
 	int ret;
diff --git a/lib/psa/measured_boot.c b/lib/psa/measured_boot.c
index 61747f2..10c43f1 100644
--- a/lib/psa/measured_boot.c
+++ b/lib/psa/measured_boot.c
@@ -111,6 +111,61 @@
 			NULL, 0);
 }
 
+psa_status_t rss_measured_boot_read_measurement(uint8_t index,
+					uint8_t *signer_id,
+					size_t signer_id_size,
+					size_t *signer_id_len,
+					uint8_t *version,
+					size_t version_size,
+					size_t *version_len,
+					uint32_t *measurement_algo,
+					uint8_t *sw_type,
+					size_t sw_type_size,
+					size_t *sw_type_len,
+					uint8_t *measurement_value,
+					size_t measurement_value_size,
+					size_t *measurement_value_len,
+					bool *is_locked)
+{
+	psa_status_t status;
+	struct measured_boot_read_iovec_in_t read_iov_in = {
+		.index = index,
+		.sw_type_size = sw_type_size,
+		.version_size = version_size,
+	};
+
+	struct measured_boot_read_iovec_out_t read_iov_out;
+
+	psa_invec in_vec[] = {
+		{.base = &read_iov_in,
+		 .len = sizeof(struct measured_boot_read_iovec_in_t)},
+	};
+
+	psa_outvec out_vec[] = {
+		{.base = &read_iov_out,
+		 .len = sizeof(struct measured_boot_read_iovec_out_t)},
+		{.base = signer_id, .len = signer_id_size},
+		{.base = measurement_value, .len = measurement_value_size}
+	};
+
+	status = psa_call(RSS_MEASURED_BOOT_HANDLE, RSS_MEASURED_BOOT_READ,
+					  in_vec, IOVEC_LEN(in_vec),
+					  out_vec, IOVEC_LEN(out_vec));
+
+	if (status == PSA_SUCCESS) {
+		*is_locked = read_iov_out.is_locked;
+		*measurement_algo = read_iov_out.measurement_algo;
+		*sw_type_len = read_iov_out.sw_type_len;
+		*version_len = read_iov_out.version_len;
+		memcpy(sw_type, read_iov_out.sw_type, read_iov_out.sw_type_len);
+		memcpy(version, read_iov_out.version, read_iov_out.version_len);
+		*signer_id_len = out_vec[1].len;
+		*measurement_value_len = out_vec[2].len;
+	}
+
+	return status;
+}
+
 #else /* !PLAT_RSS_NOT_SUPPORTED */
 
 psa_status_t
@@ -133,4 +188,24 @@
 
 	return PSA_SUCCESS;
 }
+
+psa_status_t rss_measured_boot_read_measurement(uint8_t index,
+					uint8_t *signer_id,
+					size_t signer_id_size,
+					size_t *signer_id_len,
+					uint8_t *version,
+					size_t version_size,
+					size_t *version_len,
+					uint32_t *measurement_algo,
+					uint8_t *sw_type,
+					size_t sw_type_size,
+					size_t *sw_type_len,
+					uint8_t *measurement_value,
+					size_t measurement_value_size,
+					size_t *measurement_value_len,
+					bool *is_locked)
+{
+	return PSA_SUCCESS;
+}
+
 #endif /* !PLAT_RSS_NOT_SUPPORTED */
diff --git a/lib/psa/measured_boot_private.h b/lib/psa/measured_boot_private.h
index 649c3f6..80d2c19 100644
--- a/lib/psa/measured_boot_private.h
+++ b/lib/psa/measured_boot_private.h
@@ -11,8 +11,24 @@
 #include <stdint.h>
 
 /* Measured boot message types that distinguish its services */
+#define RSS_MEASURED_BOOT_READ		1001U
 #define RSS_MEASURED_BOOT_EXTEND	1002U
 
+struct measured_boot_read_iovec_in_t {
+    uint8_t index;
+    uint8_t sw_type_size;
+    uint8_t version_size;
+};
+
+struct measured_boot_read_iovec_out_t {
+    uint8_t  is_locked;
+    uint32_t measurement_algo;
+    uint8_t  sw_type[SW_TYPE_MAX_SIZE];
+    uint8_t  sw_type_len;
+    uint8_t  version[VERSION_MAX_SIZE];
+    uint8_t  version_len;
+};
+
 struct measured_boot_extend_iovec_t {
 	uint8_t  index;
 	uint8_t  lock_measurement;
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index 683d7ac..a66123a 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -400,9 +400,10 @@
 # Enable Link Time Optimization
 ENABLE_LTO			:= 0
 
-# Build flag to include EL2 registers in cpu context save and restore during
-# S-EL2 firmware entry/exit. This flag is to be used with SPD=spmd option.
-# Default is 0.
+# This option will include EL2 registers in cpu context save and restore during
+# EL2 firmware entry/exit. Internal flag not meant for direct setting.
+# Use SPD=spmd and SPMD_SPM_AT_SEL2=1 or ENABLE_RME=1 to enable
+# CTX_INCLUDE_EL2_REGS.
 CTX_INCLUDE_EL2_REGS		:= 0
 
 # Enable Memory tag extension which is supported for architecture greater
diff --git a/plat/arm/board/common/board_arm_trusted_boot.c b/plat/arm/board/common/board_arm_trusted_boot.c
index 714c444..24d88ee 100644
--- a/plat/arm/board/common/board_arm_trusted_boot.c
+++ b/plat/arm/board/common/board_arm_trusted_boot.c
@@ -47,7 +47,8 @@
 #pragma weak plat_get_nv_ctr
 #pragma weak plat_set_nv_ctr
 
-extern unsigned char arm_rotpk_header[], arm_rotpk_hash_end[];
+extern unsigned char arm_rotpk_header[], arm_rotpk_key[], arm_rotpk_hash_end[],
+       arm_rotpk_key_end[];
 
 #if (ARM_ROTPK_LOCATION_ID == ARM_ROTPK_REGS_ID) || ARM_CRYPTOCELL_INTEG
 static unsigned char rotpk_hash_der[ARM_ROTPK_HEADER_LEN + ARM_ROTPK_HASH_LEN];
@@ -93,16 +94,20 @@
 #endif
 
 #if (ARM_ROTPK_LOCATION_ID == ARM_ROTPK_DEVEL_RSA_ID) || \
-    (ARM_ROTPK_LOCATION_ID == ARM_ROTPK_DEVEL_ECDSA_ID)
-/*
- * Return development ROTPK hash generated from ROT_KEY.
- */
+    (ARM_ROTPK_LOCATION_ID == ARM_ROTPK_DEVEL_ECDSA_ID) || \
+    (ARM_ROTPK_LOCATION_ID == ARM_ROTPK_DEVEL_FULL_DEV_RSA_KEY_ID)
 int arm_get_rotpk_info_dev(void **key_ptr, unsigned int *key_len,
 			unsigned int *flags)
 {
-	*key_ptr = arm_rotpk_header;
-	*key_len = arm_rotpk_hash_end - arm_rotpk_header;
-	*flags = ROTPK_IS_HASH;
+	if (ARM_ROTPK_LOCATION_ID == ARM_ROTPK_DEVEL_FULL_DEV_RSA_KEY_ID) {
+		*key_ptr = arm_rotpk_key;
+		*key_len = arm_rotpk_key_end - arm_rotpk_key;
+		*flags = 0;
+	} else {
+		*key_ptr = arm_rotpk_header;
+		*key_len = arm_rotpk_hash_end - arm_rotpk_header;
+		*flags = ROTPK_IS_HASH;
+	}
 	return 0;
 }
 #endif
@@ -130,7 +135,7 @@
 #endif
 
 /*
- * Wrapper function for most Arm platforms to get ROTPK hash.
+ * Wrapper function for most Arm platforms to get ROTPK info.
  */
 static int get_rotpk_info(void **key_ptr, unsigned int *key_len,
 				unsigned int *flags)
@@ -140,7 +145,8 @@
 #else
 
 #if (ARM_ROTPK_LOCATION_ID == ARM_ROTPK_DEVEL_RSA_ID) || \
-    (ARM_ROTPK_LOCATION_ID == ARM_ROTPK_DEVEL_ECDSA_ID)
+    (ARM_ROTPK_LOCATION_ID == ARM_ROTPK_DEVEL_ECDSA_ID) || \
+    (ARM_ROTPK_LOCATION_ID == ARM_ROTPK_DEVEL_FULL_DEV_RSA_KEY_ID)
 	return arm_get_rotpk_info_dev(key_ptr, key_len, flags);
 #elif (ARM_ROTPK_LOCATION_ID == ARM_ROTPK_REGS_ID)
 	return arm_get_rotpk_info_regs(key_ptr, key_len, flags);
diff --git a/plat/arm/board/common/board_common.mk b/plat/arm/board/common/board_common.mk
index 1d0eb13..d73c2e3 100644
--- a/plat/arm/board/common/board_common.mk
+++ b/plat/arm/board/common/board_common.mk
@@ -12,6 +12,7 @@
 BL2_SOURCES		+=	drivers/cfi/v2m/v2m_flash.c
 
 ifneq (${TRUSTED_BOARD_BOOT},0)
+ARM_ROTPK_S = plat/arm/board/common/rotpk/arm_dev_rotpk.S
 ifneq (${ARM_CRYPTOCELL_INTEG}, 1)
 # ROTPK hash location
 ifeq (${ARM_ROTPK_LOCATION}, regs)
@@ -32,6 +33,12 @@
 $(BUILD_PLAT)/bl2/arm_dev_rotpk.o : $(ARM_ROTPK_HASH)
 $(warning Development keys support for FVP is deprecated. Use `regs` \
 option instead)
+else ifeq (${ARM_ROTPK_LOCATION}, devel_full_dev_rsa_key)
+	CRYPTO_ALG=rsa
+	ARM_ROTPK_LOCATION_ID = ARM_ROTPK_DEVEL_FULL_DEV_RSA_KEY_ID
+	ARM_ROTPK_S = plat/arm/board/common/rotpk/arm_full_dev_rsa_rotpk.S
+$(warning Development keys support for FVP is deprecated. Use `regs` \
+option instead)
 else
 $(error "Unsupported ARM_ROTPK_LOCATION value")
 endif
@@ -67,9 +74,9 @@
 NTFW_NVCTR_VAL	?=	0
 endif
 BL1_SOURCES		+=	plat/arm/board/common/board_arm_trusted_boot.c \
-				plat/arm/board/common/rotpk/arm_dev_rotpk.S
+				${ARM_ROTPK_S}
 BL2_SOURCES		+=	plat/arm/board/common/board_arm_trusted_boot.c \
-				plat/arm/board/common/rotpk/arm_dev_rotpk.S
+				${ARM_ROTPK_S}
 
 # Allows platform code to provide implementation variants depending on the
 # selected chain of trust.
diff --git a/plat/arm/board/common/rotpk/arm_dev_rotpk.S b/plat/arm/board/common/rotpk/arm_dev_rotpk.S
index 06e2a06..a7fadf6 100644
--- a/plat/arm/board/common/rotpk/arm_dev_rotpk.S
+++ b/plat/arm/board/common/rotpk/arm_dev_rotpk.S
@@ -14,7 +14,6 @@
 #endif
 
 	.global arm_rotpk_header
-	.global arm_rotpk_header_end
 	.section .rodata.arm_rotpk_hash, "a"
 
 arm_rotpk_header:
diff --git a/plat/arm/board/common/rotpk/arm_full_dev_rsa_rotpk.S b/plat/arm/board/common/rotpk/arm_full_dev_rsa_rotpk.S
new file mode 100644
index 0000000..4bb04dc
--- /dev/null
+++ b/plat/arm/board/common/rotpk/arm_full_dev_rsa_rotpk.S
@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/* corstone1000 platform provides custom values for the macros defined in
+ * arm_def.h , so only platform_def.h needs to be included
+ */
+#if !defined(TARGET_PLATFORM_FVP) && !defined(TARGET_PLATFORM_FPGA)
+#include "plat/arm/common/arm_def.h"
+#else
+#include <platform_def.h>
+#endif
+
+	.global arm_rotpk_key
+	.global arm_rotpk_key_end
+
+	.section .rodata.arm_rotpk_key, "a"
+
+arm_rotpk_key:
+	.byte 0x30, 0x82, 0x01, 0x22, 0x30, 0x0D, 0x06, 0x09, 0x2A, 0x86, 0x48, 0x86, 0xF7, 0x0D, 0x01, 0x01, 0x01
+	.byte 0x05, 0x00, 0x03, 0x82, 0x01, 0x0F, 0x00, 0x30, 0x82, 0x01, 0x0A, 0x02, 0x82, 0x01, 0x01
+	.byte 0x00, 0xCB, 0x2C, 0x60, 0xD5, 0x8D, 0x63, 0xD4, 0x07, 0x79, 0x7E, 0xC7, 0x16, 0x96, 0xBD, 0x4D, 0x24, 0x4E, 0xAC, 0x86, 0xE6, 0xB7, 0x71, 0xE3, 0xC5, 0x54, 0x0B, 0xE7, 0x14, 0x1C, 0xBD, 0x29, 0x1A, 0xC1, 0x3F, 0x7A, 0xB6, 0x02, 0xAA, 0xAB, 0x36, 0xC4, 0xD9, 0x36, 0x69, 0x6C, 0xE2, 0x65, 0xC3, 0x9B, 0xB1, 0xBF, 0x3D, 0xA8, 0x56, 0x26, 0xCB, 0xFD, 0x04, 0x01, 0xBA, 0xAC, 0x3E, 0x54, 0x32, 0xCA, 0x79, 0x5E, 0xBB, 0xB2, 0x05, 0xEA, 0x06, 0x58, 0xF2, 0x74, 0xBA, 0xE1, 0xF4, 0x87, 0xC0, 0x19, 0x0A, 0x1F, 0x66, 0x07, 0x77, 0x84, 0x83, 0xA1, 0x1C, 0xEF, 0xFF, 0x28, 0x59, 0xE7, 0xC3, 0x68, 0x7D, 0x26, 0x20, 0x43, 0xEB, 0x56, 0x63, 0xF3, 0x39, 0x31, 0xD8, 0x2B, 0x51, 0xA9, 0xBC, 0x4F, 0xD0, 0xF6, 0xDE, 0x95, 0xDC, 0x5F, 0x5B, 0xC1, 0xED, 0x90, 0x6F, 0xEC, 0x28, 0x91, 0x7E, 0x17, 0xED, 0x78, 0x90, 0xF4, 0x60, 0xA7, 0xC4, 0xC7, 0x4F, 0x50, 0xED, 0x5D, 0x13, 0x3A, 0x21, 0x2B, 0x70, 0xC5, 0x61, 0x7B, 0x08, 0x21, 0x65, 0x3A, 0xCD, 0x82, 0x56, 0x8C, 0x7A, 0x47, 0xAC, 0x89, 0xE8, 0xA5, 0x48, 0x48
+	.byte 0x31, 0xD9, 0x1D, 0x46, 0xE5, 0x85, 0x86, 0x98, 0xA0, 0xE5, 0xC0, 0xA6, 0x6A, 0xBD, 0x07, 0xE4, 0x92, 0x57, 0x61, 0x07, 0x8F, 0x7D, 0x5A, 0x4D, 0xCA, 0xAE, 0x36, 0xB9, 0x56, 0x04, 0x10, 0xF2, 0x6C, 0xBE, 0xF6, 0x3B, 0x6C, 0x80, 0x3E, 0xBE , 0x0E, 0xA3, 0x4D , 0xC7 , 0xD4, 0x7E , 0xA7  , 0x49, 0xD4, 0xF2, 0xD2, 0xBC, 0xCF, 0x30, 0xA8, 0xE7, 0x74, 0x8F, 0x64, 0xDF, 0xBC, 0x5C, 0x47, 0x68, 0xCC, 0x40, 0x4C, 0xF8, 0x83, 0xCC, 0xCB, 0x40, 0x35, 0x04, 0x60, 0xCA, 0xB3, 0xA4, 0x17, 0x9F, 0x03, 0xCA, 0x1D, 0x5A, 0xFA, 0xD1, 0xAF, 0x21, 0x57, 0x10, 0xD3, 0x02, 0x03, 0x01, 0x00, 0x01
+
+arm_rotpk_key_end:
+
+.if ARM_ROTPK_KEY_LEN != arm_rotpk_key_end - arm_rotpk_key
+.error "Invalid ROTPK length."
+.endif
+
diff --git a/plat/arm/board/fvp/fvp_bl2_measured_boot.c b/plat/arm/board/fvp/fvp_bl2_measured_boot.c
index e938e24..29b6619 100644
--- a/plat/arm/board/fvp/fvp_bl2_measured_boot.c
+++ b/plat/arm/board/fvp/fvp_bl2_measured_boot.c
@@ -125,7 +125,8 @@
 
 	/* Calculate image hash and record data in Event Log */
 	int err = event_log_measure_and_record((uintptr_t)base, (uint32_t)size,
-					       critical_data_id);
+					       critical_data_id,
+					       fvp_event_log_metadata);
 	if (err != 0) {
 		ERROR("%s%s critical data (%i)\n",
 		      "Failed to ", "record",  err);
diff --git a/plat/arm/board/fvp/fvp_common.c b/plat/arm/board/fvp/fvp_common.c
index f8463f1..f5d9940 100644
--- a/plat/arm/board/fvp/fvp_common.c
+++ b/plat/arm/board/fvp/fvp_common.c
@@ -536,6 +536,7 @@
 	assert(manifest != NULL);
 
 	manifest->version = RMMD_MANIFEST_VERSION;
+	manifest->padding = 0U; /* RES0 */
 	manifest->plat_data = (uintptr_t)NULL;
 
 	return 0;
diff --git a/plat/arm/board/fvp/fvp_common_measured_boot.c b/plat/arm/board/fvp/fvp_common_measured_boot.c
index 93aa055..b5b8f10 100644
--- a/plat/arm/board/fvp/fvp_common_measured_boot.c
+++ b/plat/arm/board/fvp/fvp_common_measured_boot.c
@@ -16,11 +16,6 @@
 extern event_log_metadata_t fvp_event_log_metadata[];
 extern struct rss_mboot_metadata fvp_rss_mboot_metadata[];
 
-const event_log_metadata_t *plat_event_log_get_metadata(void)
-{
-	return fvp_event_log_metadata;
-}
-
 struct rss_mboot_metadata *plat_rss_mboot_get_metadata(void)
 {
 	return fvp_rss_mboot_metadata;
@@ -34,7 +29,8 @@
 	/* Calculate image hash and record data in Event Log */
 	err = event_log_measure_and_record(image_data->image_base,
 					   image_data->image_size,
-					   image_id);
+					   image_id,
+					   fvp_event_log_metadata);
 	if (err != 0) {
 		ERROR("%s%s image id %u (%i)\n",
 		      "Failed to ", "record in event log", image_id, err);
diff --git a/plat/arm/board/fvp/fvp_sync_traps.c b/plat/arm/board/fvp/fvp_sync_traps.c
new file mode 100644
index 0000000..91240f7
--- /dev/null
+++ b/plat/arm/board/fvp/fvp_sync_traps.c
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2022, ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * This file just contains demonstration code, to "handle" RNG traps.
+ */
+
+#include <stdbool.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <bl31/sync_handle.h>
+#include <context.h>
+
+/*
+ * SCR_EL3.SCR_TRNDR_BIT also affects execution in EL3, so allow to disable
+ * the trap temporarily.
+ */
+static void enable_rng_trap(bool enable)
+{
+	uint64_t scr_el3 = read_scr_el3();
+
+	if (enable) {
+		scr_el3 |= SCR_TRNDR_BIT;
+	} else {
+		scr_el3 &= ~SCR_TRNDR_BIT;
+	}
+
+	write_scr_el3(scr_el3);
+	isb();
+}
+
+/*
+ * This emulation code here is not very meaningful: enabling the RNG
+ * trap typically happens for a reason, so just calling the actual
+ * hardware instructions might not be useful or even possible.
+ */
+int plat_handle_rng_trap(uint64_t esr_el3, cpu_context_t *ctx)
+{
+	/* extract the target register number from the exception syndrome */
+	unsigned int rt = get_sysreg_iss_rt(esr_el3);
+
+	/* ignore XZR accesses and writes to the register */
+	if (rt == 31 || is_sysreg_iss_write(esr_el3)) {
+		return TRAP_RET_CONTINUE;
+	}
+
+	enable_rng_trap(false);
+	if ((esr_el3 & ISS_SYSREG_OPCODE_MASK) == ISS_SYSREG_OPCODE_RNDR) {
+		ctx->gpregs_ctx.ctx_regs[rt] = read_rndr();
+	} else {
+		ctx->gpregs_ctx.ctx_regs[rt] = read_rndrrs();
+	}
+	enable_rng_trap(true);
+
+	/*
+	 * We successfully handled the trap, continue with the next
+	 * instruction.
+	 */
+	return TRAP_RET_CONTINUE;
+}
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index 51ba035..f28a6ff 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -198,6 +198,10 @@
 				plat/arm/board/fvp/fvp_realm_attest_key.c
 endif
 
+ifeq (${ENABLE_FEAT_RNG_TRAP},1)
+BL31_SOURCES		+=	plat/arm/board/fvp/fvp_sync_traps.c
+endif
+
 ifeq (${BL2_AT_EL3},1)
 BL2_SOURCES		+=	plat/arm/board/fvp/${ARCH}/fvp_helpers.S	\
 				plat/arm/board/fvp/fvp_bl2_el3_setup.c		\
diff --git a/plat/arm/board/juno/juno_topology.c b/plat/arm/board/juno/juno_topology.c
index 075f512..768741e 100644
--- a/plat/arm/board/juno/juno_topology.c
+++ b/plat/arm/board/juno/juno_topology.c
@@ -20,7 +20,7 @@
 		.ring_doorbell = &mhu_ring_doorbell,
 };
 
-scmi_channel_plat_info_t *plat_css_get_scmi_info(int channel_id)
+scmi_channel_plat_info_t *plat_css_get_scmi_info(unsigned int channel_id)
 {
 	return &juno_scmi_plat_info;
 }
diff --git a/plat/arm/board/morello/morello_bl31_setup.c b/plat/arm/board/morello/morello_bl31_setup.c
index a044212..e04587d 100644
--- a/plat/arm/board/morello/morello_bl31_setup.c
+++ b/plat/arm/board/morello/morello_bl31_setup.c
@@ -19,7 +19,7 @@
 	.ring_doorbell = &mhu_ring_doorbell
 };
 
-scmi_channel_plat_info_t *plat_css_get_scmi_info(int channel_id)
+scmi_channel_plat_info_t *plat_css_get_scmi_info(unsigned int channel_id)
 {
 	return &morello_scmi_plat_info;
 }
diff --git a/plat/arm/board/n1sdp/n1sdp_bl31_setup.c b/plat/arm/board/n1sdp/n1sdp_bl31_setup.c
index 5e897fe..ad6c1f8 100644
--- a/plat/arm/board/n1sdp/n1sdp_bl31_setup.c
+++ b/plat/arm/board/n1sdp/n1sdp_bl31_setup.c
@@ -62,7 +62,7 @@
 	0
 };
 
-scmi_channel_plat_info_t *plat_css_get_scmi_info(int channel_id)
+scmi_channel_plat_info_t *plat_css_get_scmi_info(unsigned int channel_id)
 {
 	return &n1sdp_scmi_plat_info;
 }
diff --git a/plat/arm/board/rdn1edge/rdn1edge_plat.c b/plat/arm/board/rdn1edge/rdn1edge_plat.c
index 1dbbf26..045c316 100644
--- a/plat/arm/board/rdn1edge/rdn1edge_plat.c
+++ b/plat/arm/board/rdn1edge/rdn1edge_plat.c
@@ -65,7 +65,8 @@
 #if defined(IMAGE_BL31)
 void bl31_platform_setup(void)
 {
-	int i, ret;
+	unsigned int i;
+	int ret;
 
 	if (plat_arm_sgi_get_multi_chip_mode() == 0 && CSS_SGI_CHIP_COUNT > 1) {
 		ERROR("Chip Count is set to %d but multi-chip mode not enabled\n",
diff --git a/plat/arm/board/tc/include/platform_def.h b/plat/arm/board/tc/include/platform_def.h
index bc4f254..a3b7839 100644
--- a/plat/arm/board/tc/include/platform_def.h
+++ b/plat/arm/board/tc/include/platform_def.h
@@ -25,13 +25,28 @@
  * The top 16MB of ARM_DRAM1 is configured as secure access only using the TZC,
  * its base is ARM_AP_TZC_DRAM1_BASE.
  *
- * Reserve 32MB below ARM_AP_TZC_DRAM1_BASE for:
+ * Reserve 96 MB below ARM_AP_TZC_DRAM1_BASE for:
  *   - BL32_BASE when SPD_spmd is enabled
- *   - Region to load Trusted OS
+ *   - Region to load secure partitions
+ *
+ *
+ *  0xF900_0000  ------------------   TC_TZC_DRAM1_BASE
+ *               |                |
+ *               |      SPMC      |
+ *               |       SP       |
+ *               |     (96MB)     |
+ *  0xFF00_0000  ------------------   ARM_AP_TZC_DRAM1_BASE
+ *               |       AP       |
+ *               |   EL3 Monitor  |
+ *               |       SCP      |
+ *               |     (16MB)     |
+ *  0xFFFF_FFFF  ------------------
+ *
+ *
  */
 #define TC_TZC_DRAM1_BASE		(ARM_AP_TZC_DRAM1_BASE -	\
 					 TC_TZC_DRAM1_SIZE)
-#define TC_TZC_DRAM1_SIZE		UL(0x02000000)	/* 32 MB */
+#define TC_TZC_DRAM1_SIZE		96 * SZ_1M	/* 96 MB */
 #define TC_TZC_DRAM1_END		(TC_TZC_DRAM1_BASE +		\
 					 TC_TZC_DRAM1_SIZE - 1)
 
@@ -68,7 +83,9 @@
  * max size of BL32 image.
  */
 #if defined(SPD_spmd)
-#define PLAT_ARM_SPMC_BASE		TC_TZC_DRAM1_BASE
+#define TC_EL2SPMC_LOAD_ADDR		(TC_TZC_DRAM1_BASE + 0x04000000)
+
+#define PLAT_ARM_SPMC_BASE		TC_EL2SPMC_LOAD_ADDR
 #define PLAT_ARM_SPMC_SIZE		UL(0x200000)  /* 2 MB */
 #endif
 
@@ -101,7 +118,7 @@
  * PLAT_ARM_MAX_BL1_RW_SIZE is calculated using the current BL1 RW debug size
  * plus a little space for growth.
  */
-#define PLAT_ARM_MAX_BL1_RW_SIZE	0xD000
+#define PLAT_ARM_MAX_BL1_RW_SIZE	0x12000
 
 /*
  * PLAT_ARM_MAX_ROMLIB_RW_SIZE is define to use a full page
@@ -129,7 +146,7 @@
  * BL2 and BL1-RW. Current size is considering that TRUSTED_BOARD_BOOT and
  * MEASURED_BOOT is enabled.
  */
-#define PLAT_ARM_MAX_BL31_SIZE		0x47000
+#define PLAT_ARM_MAX_BL31_SIZE		0x60000
 
 /*
  * Size of cacheable stacks
@@ -152,7 +169,7 @@
 # if SPM_MM
 #  define PLATFORM_STACK_SIZE		0x500
 # else
-#  define PLATFORM_STACK_SIZE		0x400
+#  define PLATFORM_STACK_SIZE		0xa00
 # endif
 #elif defined(IMAGE_BL32)
 # define PLATFORM_STACK_SIZE		0x440
@@ -276,8 +293,8 @@
 		(TZC_REGION_ACCESS_RDWR(TZC_NSAID_DEFAULT))
 
 /*
- * The first region below, TC_TZC_DRAM1_BASE (0xfd000000) to
- * ARM_SCP_TZC_DRAM1_END (0xffffffff) will mark the last 48 MB of DRAM as
+ * The first region below, TC_TZC_DRAM1_BASE (0xf9000000) to
+ * ARM_SCP_TZC_DRAM1_END (0xffffffff) will mark the last 112 MB of DRAM as
  * secure. The second and third regions gives non secure access to rest of DRAM.
  */
 #define TC_TZC_REGIONS_DEF	\
diff --git a/plat/arm/board/tc/include/tc_plat.h b/plat/arm/board/tc/include/tc_plat.h
index 28c0308..f7ce2fe 100644
--- a/plat/arm/board/tc/include/tc_plat.h
+++ b/plat/arm/board/tc/include/tc_plat.h
@@ -9,4 +9,8 @@
 
 void tc_bl31_common_platform_setup(void);
 
+#ifdef PLATFORM_TEST
+void run_platform_tests(void);
+#endif
+
 #endif /* TC_PLAT_H */
diff --git a/plat/arm/board/tc/plat_tc_mbedtls_config.h b/plat/arm/board/tc/plat_tc_mbedtls_config.h
new file mode 100644
index 0000000..d776b63
--- /dev/null
+++ b/plat/arm/board/tc/plat_tc_mbedtls_config.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (c) 2022, Arm Ltd. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLAT_TC_MBEDTLS_CONFIG_H
+#define PLAT_TC_MBEDTLS_CONFIG_H
+
+#include <mbedtls_config.h>
+#include <export/lib/utils_def_exp.h>
+
+#ifndef TF_MBEDTLS_HEAP_SIZE
+#error TF_MBEDTLS_HEAP_SIZE is not defined
+#else
+#define PLATFORM_TEST_MIN_MBEDTLS_HEAP_SIZE	(8 * 1024)
+/* Only change heap size if it is less then the minimum required. */
+#if TF_MBEDTLS_HEAP_SIZE < PLATFORM_TEST_MIN_MBEDTLS_HEAP_SIZE
+#undef TF_MBEDTLS_HEAP_SIZE
+#define TF_MBEDTLS_HEAP_SIZE	PLATFORM_TEST_MIN_MBEDTLS_HEAP_SIZE
+#endif
+#endif
+
+#define MBEDTLS_PSA_CRYPTO_C
+#define MBEDTLS_HMAC_DRBG_C
+#define MBEDTLS_ENTROPY_C
+#define MBEDTLS_NO_DEFAULT_ENTROPY_SOURCES
+#define MBEDTLS_NO_PLATFORM_ENTROPY
+#define MBEDTLS_TEST_NULL_ENTROPY
+#define MBEDTLS_ECP_C
+#define MBEDTLS_ECP_DP_SECP384R1_ENABLED
+
+#endif /* PLAT_TC_MBEDTLS_CONFIG_H */
diff --git a/plat/arm/board/tc/platform.mk b/plat/arm/board/tc/platform.mk
index 37ba229..74c0f17 100644
--- a/plat/arm/board/tc/platform.mk
+++ b/plat/arm/board/tc/platform.mk
@@ -193,6 +193,11 @@
 
 endif
 
+# Add this include as first, before arm_common.mk. This is necessary because
+# arm_common.mk builds Mbed TLS, and platform_test.mk can change the list of
+# Mbed TLS files that are to be compiled (LIBMBEDTLS_SRCS).
+include plat/arm/board/tc/platform_test.mk
+
 include plat/arm/common/arm_common.mk
 include plat/arm/css/common/css_common.mk
 include plat/arm/soc/common/soc_css.mk
diff --git a/plat/arm/board/tc/platform_test.mk b/plat/arm/board/tc/platform_test.mk
new file mode 100644
index 0000000..c2ee69e
--- /dev/null
+++ b/plat/arm/board/tc/platform_test.mk
@@ -0,0 +1,80 @@
+# Copyright (c) 2022, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifeq (${PLATFORM_TEST},1)
+
+    # The variables need to be set to compile the platform test:
+    ifeq (${TF_M_TESTS_PATH},)
+        # Example: ../rss/tf-m-tests
+        $(error Error: TF_M_TESTS_PATH not set)
+    endif
+    ifeq (${TF_M_EXTRAS_PATH},)
+        # Example: ../rss/tf-m-extras
+        $(error Error: TF_M_EXTRAS_PATH not set)
+    endif
+    ifeq (${MEASUREMENT_VALUE_SIZE},)
+        MEASUREMENT_VALUE_SIZE	:=	32
+    endif
+    ifeq (${MEASURED_BOOT_HASH_ALG},)
+        MEASURED_BOOT_HASH_ALG	:=	"PSA_ALG_SHA_256"
+    endif
+
+    DELEGATED_ATTEST_TESTS_PATH	=	$(TF_M_EXTRAS_PATH)/partitions/delegated_attestation/test
+    MEASURED_BOOT_TESTS_PATH	=	$(TF_M_EXTRAS_PATH)/partitions/measured_boot/test
+
+    MBEDTLS_CONFIG_FILE		=	"<plat_tc_mbedtls_config.h>"
+
+    LIBMBEDTLS_SRCS		+= 	$(addprefix ${MBEDTLS_DIR}/library/,	\
+					entropy.c				\
+					entropy_poll.c				\
+					hmac_drbg.c				\
+					psa_crypto.c				\
+					psa_crypto_client.c			\
+					psa_crypto_driver_wrappers.c		\
+					psa_crypto_hash.c			\
+					psa_crypto_rsa.c			\
+					psa_crypto_ecp.c			\
+					psa_crypto_slot_management.c		\
+					)
+
+    BL31_SOURCES	+=	${RSS_COMMS_SOURCES} 				\
+				plat/arm/common/arm_dyn_cfg.c 			\
+				${TC_BASE}/rss_ap_tests.c 			\
+				${TC_BASE}/rss_ap_testsuites.c 			\
+				${TC_BASE}/rss_ap_test_stubs.c			\
+				$(TF_M_TESTS_PATH)/test/framework/test_framework.c \
+				$(MEASURED_BOOT_TESTS_PATH)/measured_boot_common.c \
+				$(MEASURED_BOOT_TESTS_PATH)/measured_boot_tests_common.c \
+				$(DELEGATED_ATTEST_TESTS_PATH)/delegated_attest_test.c \
+				drivers/auth/mbedtls/mbedtls_common.c 		\
+				lib/psa/measured_boot.c 			\
+				lib/psa/delegated_attestation.c
+
+    PLAT_INCLUDES	+=	-I$(TF_M_EXTRAS_PATH)/partitions/measured_boot/interface/include \
+				-I$(TF_M_EXTRAS_PATH)/partitions/delegated_attestation/interface/include \
+				-I$(TF_M_TESTS_PATH)/test/framework 		\
+				-I$(TF_M_TESTS_PATH)/log 			\
+				-I$(TF_M_TESTS_PATH)/test/secure_fw/suites/extra \
+				-I$(MEASURED_BOOT_TESTS_PATH)/non_secure 	\
+				-I$(DELEGATED_ATTEST_TESTS_PATH) 		\
+				-I$(DELEGATED_ATTEST_TESTS_PATH)/non_secure \
+				-Iplat/arm/board/tc 				\
+				-Iinclude/drivers/auth/mbedtls 			\
+				-Iinclude/drivers/arm
+
+    # Some of the PSA functions are declared in multiple header files, that
+    # triggers this warning.
+    TF_CFLAGS		+=	-Wno-error=redundant-decls
+
+    # TODO: Created patch for warning in tf-m-tests
+    TF_CFLAGS		+=	-Wno-error=return-type
+
+    # Define macros that are used by the code coming from the tf-m-extras repo.
+    $(eval $(call add_define,MEASUREMENT_VALUE_SIZE))
+    $(eval $(call add_define,MEASURED_BOOT_HASH_ALG))
+    $(eval $(call add_define,DELEG_ATTEST_DUMP_TOKEN_AND_KEY))
+
+    $(eval $(call add_define,PLATFORM_TEST))
+endif
diff --git a/plat/arm/board/tc/region_defs.h b/plat/arm/board/tc/region_defs.h
new file mode 100644
index 0000000..d3dfd13
--- /dev/null
+++ b/plat/arm/board/tc/region_defs.h
@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2022, Arm Ltd. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef REGION_DEFS_H
+#define REGION_DEFS_H
+
+#define PSA_INITIAL_ATTEST_TOKEN_MAX_SIZE   0x800
+
+#endif /* REGION_DEFS_H */
diff --git a/plat/arm/board/tc/rss_ap_test_stubs.c b/plat/arm/board/tc/rss_ap_test_stubs.c
new file mode 100644
index 0000000..aa97476
--- /dev/null
+++ b/plat/arm/board/tc/rss_ap_test_stubs.c
@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) 2022, Arm Ltd. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdarg.h>
+#include <stdbool.h>
+#include <stdio.h>
+
+#include <delegated_attestation.h>
+#include <measured_boot.h>
+#include <psa/error.h>
+
+
+psa_status_t
+tfm_measured_boot_extend_measurement(uint8_t index,
+				     const uint8_t *signer_id,
+				     size_t signer_id_size,
+				     const uint8_t *version,
+				     size_t version_size,
+				     uint32_t measurement_algo,
+				     const uint8_t *sw_type,
+				     size_t sw_type_size,
+				     const uint8_t *measurement_value,
+				     size_t measurement_value_size,
+				     bool lock_measurement)
+{
+	return rss_measured_boot_extend_measurement(index,
+						    signer_id,
+						    signer_id_size,
+						    version,
+						    version_size,
+						    measurement_algo,
+						    sw_type,
+						    sw_type_size,
+						    measurement_value,
+						    measurement_value_size,
+						    lock_measurement);
+}
+
+psa_status_t
+tfm_measured_boot_read_measurement(uint8_t index,
+				   uint8_t *signer_id,
+				   size_t signer_id_size,
+				   size_t *signer_id_len,
+				   uint8_t *version,
+				   size_t version_size,
+				   size_t *version_len,
+				   uint32_t *measurement_algo,
+				   uint8_t *sw_type,
+				   size_t sw_type_size,
+				   size_t *sw_type_len,
+				   uint8_t *measurement_value,
+				   size_t measurement_value_size,
+				   size_t *measurement_value_len,
+				   bool *is_locked)
+{
+	return rss_measured_boot_read_measurement(index,
+						  signer_id,
+						  signer_id_size,
+						  signer_id_len,
+						  version,
+						  version_size,
+						  version_len,
+						  measurement_algo,
+						  sw_type,
+						  sw_type_size,
+						  sw_type_len,
+						  measurement_value,
+						  measurement_value_size,
+						  measurement_value_len,
+						  is_locked);
+}
+
+psa_status_t
+tfm_delegated_attest_get_token(const uint8_t *dak_pub_hash,
+			       size_t         dak_pub_hash_size,
+			       uint8_t       *token_buf,
+			       size_t         token_buf_size,
+			       size_t        *token_size)
+{
+	return rss_delegated_attest_get_token(dak_pub_hash,
+					      dak_pub_hash_size,
+					      token_buf,
+					      token_buf_size,
+					      token_size);
+}
+
+psa_status_t
+tfm_delegated_attest_get_delegated_key(uint8_t   ecc_curve,
+				       uint32_t  key_bits,
+				       uint8_t  *key_buf,
+				       size_t    key_buf_size,
+				       size_t   *key_size,
+				       uint32_t  hash_algo)
+{
+	return rss_delegated_attest_get_delegated_key(ecc_curve,
+						      key_bits,
+						      key_buf,
+						      key_buf_size,
+						      key_size,
+						      hash_algo);
+}
+
+int tfm_log_printf(const char *fmt, ...)
+{
+	int count;
+	va_list ap;
+
+	va_start(ap, fmt);
+	count = vprintf(fmt, ap);
+	va_end(ap);
+
+	return count;
+}
+
+void printf_set_color(int color_id)
+{
+	(void)color_id;
+}
diff --git a/plat/arm/board/tc/rss_ap_tests.c b/plat/arm/board/tc/rss_ap_tests.c
new file mode 100644
index 0000000..b62043e
--- /dev/null
+++ b/plat/arm/board/tc/rss_ap_tests.c
@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) 2022, Arm Ltd. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdio.h>
+
+#include <mbedtls_common.h>
+#include <plat/common/platform.h>
+#include <psa/crypto.h>
+#include <rss_comms.h>
+
+#include "rss_ap_testsuites.h"
+
+static struct test_suite_t test_suites[] = {
+	{.freg = register_testsuite_delegated_attest},
+	{.freg = register_testsuite_measured_boot},
+};
+
+static void run_tests(void)
+{
+	enum test_suite_err_t ret;
+	psa_status_t status;
+	size_t i;
+
+	rss_comms_init(PLAT_RSS_AP_SND_MHU_BASE, PLAT_RSS_AP_RCV_MHU_BASE);
+	mbedtls_init();
+	status = psa_crypto_init();
+	if (status != PSA_SUCCESS) {
+		printf("\n\npsa_crypto_init failed (status = %d)\n", status);
+		assert(false);
+		plat_error_handler(-1);
+	}
+
+	for (i = 0; i < ARRAY_SIZE(test_suites); ++i) {
+		struct test_suite_t *suite = &(test_suites[i]);
+
+		suite->freg(suite);
+		ret = run_testsuite(suite);
+		if (ret != TEST_SUITE_ERR_NO_ERROR) {
+			printf("\n\nError during executing testsuite '%s'.\n", suite->name);
+			assert(false);
+			plat_error_handler(-1);
+		}
+	}
+	printf("\nAll tests are run.\n");
+}
+
+void run_platform_tests(void)
+{
+	size_t i;
+
+	run_tests();
+
+	printf("\n\n");
+
+	/* Print a summary of all the tests that had been run. */
+	printf("SUMMARY:\n");
+	for (i = 0; i < ARRAY_SIZE(test_suites); ++i) {
+
+		struct test_suite_t *suite = &(test_suites[i]);
+
+		switch (suite->val) {
+		case TEST_PASSED:
+			printf("    %s PASSED.\n", suite->name);
+			break;
+		case TEST_FAILED:
+			printf("    %s FAILED.\n", suite->name);
+			break;
+		case TEST_SKIPPED:
+			printf("    %s SKIPPED.\n", suite->name);
+			break;
+		default:
+			assert(false);
+			break;
+		}
+	}
+
+	printf("\n\n");
+}
diff --git a/plat/arm/board/tc/rss_ap_testsuites.c b/plat/arm/board/tc/rss_ap_testsuites.c
new file mode 100644
index 0000000..aa47d4c
--- /dev/null
+++ b/plat/arm/board/tc/rss_ap_testsuites.c
@@ -0,0 +1,25 @@
+/*
+ * Copyright (c) 2022, Arm Ltd. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*
+ * `delegated_attest_ns_interface_testsuite.c` and
+ * `measured_boot_ns_interface_testsuite.c` are not added to the build directly.
+ * but are included in this file, and this file is added to the build. This is
+ * necessary because both files define the function `extra_tests_init`, so a
+ * linker error occurs when both are linked to BL31. This file defines a macro
+ * that renames the colliding function names to something unique.
+ * `plat/arm/board/tc/rss_ap_tests.c` can call the test init functions with
+ * their new name.
+ */
+
+#define register_testsuite_extra_ns_interface \
+	register_testsuite_delegated_attest
+#include <delegated_attest_ns_interface_testsuite.c>
+
+#undef register_testsuite_extra_ns_interface
+#define register_testsuite_extra_ns_interface \
+	register_testsuite_measured_boot
+#include <measured_boot_ns_interface_testsuite.c>
\ No newline at end of file
diff --git a/plat/arm/board/tc/rss_ap_testsuites.h b/plat/arm/board/tc/rss_ap_testsuites.h
new file mode 100644
index 0000000..58502ab
--- /dev/null
+++ b/plat/arm/board/tc/rss_ap_testsuites.h
@@ -0,0 +1,16 @@
+
+/*
+ * Copyright (c) 2022, Arm Ltd. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef RSS_AP_TESTSUITES_H
+#define RSS_AP_TESTSUITES_H
+
+#include <test_framework.h>
+
+void register_testsuite_measured_boot(struct test_suite_t *p_test_suite);
+void register_testsuite_delegated_attest(struct test_suite_t *p_test_suite);
+
+#endif /* RSS_AP_TESTSUITES_H */
diff --git a/plat/arm/board/tc/tc_bl31_setup.c b/plat/arm/board/tc/tc_bl31_setup.c
index 0523ef8..c79558d 100644
--- a/plat/arm/board/tc/tc_bl31_setup.c
+++ b/plat/arm/board/tc/tc_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -33,7 +33,7 @@
 	tc_bl31_common_platform_setup();
 }
 
-scmi_channel_plat_info_t *plat_css_get_scmi_info(int channel_id)
+scmi_channel_plat_info_t *plat_css_get_scmi_info(unsigned int channel_id)
 {
 
 	return &tc_scmi_plat_info[channel_id];
@@ -52,6 +52,12 @@
 void tc_bl31_common_platform_setup(void)
 {
 	arm_bl31_platform_setup();
+#ifdef PLATFORM_TEST
+	run_platform_tests();
+
+	/* Suspend booting */
+	plat_error_handler(-1);
+#endif
 }
 
 const plat_psci_ops_t *plat_arm_psci_override_pm_ops(plat_psci_ops_t *ops)
diff --git a/plat/arm/common/arm_bl2_setup.c b/plat/arm/common/arm_bl2_setup.c
index 08c014d..02e419a 100644
--- a/plat/arm/common/arm_bl2_setup.c
+++ b/plat/arm/common/arm_bl2_setup.c
@@ -143,11 +143,12 @@
 		ARM_PAS_SECURE,
 		ARM_PAS_REALM,
 		ARM_PAS_EL3_DRAM,
-		ARM_PAS_GPTS
+		ARM_PAS_GPTS,
+		ARM_PAS_KERNEL_1
 	};
 
 	/* Initialize entire protected space to GPT_GPI_ANY. */
-	if (gpt_init_l0_tables(GPCCR_PPS_4GB, ARM_L0_GPT_ADDR_BASE,
+	if (gpt_init_l0_tables(GPCCR_PPS_64GB, ARM_L0_GPT_ADDR_BASE,
 		ARM_L0_GPT_SIZE) < 0) {
 		ERROR("gpt_init_l0_tables() failed!\n");
 		panic();
diff --git a/plat/arm/common/trp/arm_trp_setup.c b/plat/arm/common/trp/arm_trp_setup.c
index 59b4c06..aeacd10 100644
--- a/plat/arm/common/trp/arm_trp_setup.c
+++ b/plat/arm/common/trp/arm_trp_setup.c
@@ -28,6 +28,9 @@
 
 static int arm_trp_process_manifest(rmm_manifest_t *manifest)
 {
+	/* padding field on the manifest must be RES0 */
+	assert(manifest->padding == 0U);
+
 	/* Verify the Boot Manifest Version. Only the Major is considered */
 	if (RMMD_MANIFEST_VERSION_MAJOR !=
 		RMMD_GET_MANIFEST_VERSION_MAJOR(manifest->version)) {
diff --git a/plat/arm/css/sgi/sgi_bl31_setup.c b/plat/arm/css/sgi/sgi_bl31_setup.c
index 27cf183..9adcb7c 100644
--- a/plat/arm/css/sgi/sgi_bl31_setup.c
+++ b/plat/arm/css/sgi/sgi_bl31_setup.c
@@ -74,7 +74,7 @@
 	#endif
 };
 
-scmi_channel_plat_info_t *plat_css_get_scmi_info(int channel_id)
+scmi_channel_plat_info_t *plat_css_get_scmi_info(unsigned int channel_id)
 {
 	if (sgi_plat_info.platform_id == RD_N1E1_EDGE_SID_VER_PART_NUM ||
 		sgi_plat_info.platform_id == RD_V1_SID_VER_PART_NUM ||
diff --git a/plat/common/aarch64/crash_console_helpers.S b/plat/common/aarch64/crash_console_helpers.S
index e2950f5..75b4208 100644
--- a/plat/common/aarch64/crash_console_helpers.S
+++ b/plat/common/aarch64/crash_console_helpers.S
@@ -68,12 +68,12 @@
 	mov	x4, x30		/* x3 and x4 are not clobbered by spin_lock() */
 	mov	x3, #0		/* return value */
 
+	adrp	x0, crash_console_spinlock
+	add	x0, x0, :lo12:crash_console_spinlock
+
 	mrs	x1, sctlr_el3
 	tst	x1, #SCTLR_C_BIT
 	beq	skip_spinlock	/* can't synchronize when cache disabled */
-
-	adrp	x0, crash_console_spinlock
-	add	x0, x0, :lo12:crash_console_spinlock
 	bl	spin_lock
 
 skip_spinlock:
diff --git a/plat/imx/imx8m/imx8m_measured_boot.c b/plat/imx/imx8m/imx8m_measured_boot.c
index ec61606..e9ea2d8 100644
--- a/plat/imx/imx8m/imx8m_measured_boot.c
+++ b/plat/imx/imx8m/imx8m_measured_boot.c
@@ -24,17 +24,13 @@
 	{ EVLOG_INVALID_ID, NULL, (unsigned int)(-1) }	/* Terminator */
 };
 
-const event_log_metadata_t *plat_event_log_get_metadata(void)
-{
-	return imx8m_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);
+					       image_id,
+					       imx8m_event_log_metadata);
 	if (err != 0) {
 		ERROR("%s%s image id %u (%i)\n",
 		      "Failed to ", "record", image_id, err);
diff --git a/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c b/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c
index 59c3779..4706c20 100644
--- a/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c
@@ -124,6 +124,7 @@
 void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 			u_register_t arg2, u_register_t arg3)
 {
+	static console_t console;
 	int i;
 	/* enable CSU NS access permission */
 	for (i = 0; i < 64; i++) {
@@ -132,12 +133,10 @@
 
 	imx_aipstz_init(aipstz);
 
-#if DEBUG_CONSOLE
-	static console_t console;
-
 	console_imx_uart_register(IMX_BOOT_UART_BASE, IMX_BOOT_UART_CLK_IN_HZ,
 		IMX_CONSOLE_BAUDRATE, &console);
-#endif
+	/* This console is only used for boot stage */
+	console_set_scope(&console, CONSOLE_FLAG_BOOT);
 
 	imx8m_caam_init();
 
@@ -176,23 +175,22 @@
 
 void bl31_plat_arch_setup(void)
 {
-	mmap_add_region(BL31_BASE, BL31_BASE, (BL31_LIMIT - BL31_BASE),
-		MT_MEMORY | MT_RW | MT_SECURE);
-	mmap_add_region(BL_CODE_BASE, BL_CODE_BASE, (BL_CODE_END - BL_CODE_BASE),
-		MT_MEMORY | MT_RO | MT_SECURE);
-
-	/* Map TEE memory */
-	mmap_add_region(BL32_BASE, BL32_BASE, BL32_SIZE, MT_MEMORY | MT_RW);
-
-	mmap_add(imx_mmap);
-
+	const mmap_region_t bl_regions[] = {
+		MAP_REGION_FLAT(BL31_START, BL31_SIZE,
+				MT_MEMORY | MT_RW | MT_SECURE),
+		MAP_REGION_FLAT(BL_CODE_BASE, BL_CODE_END - BL_CODE_BASE,
+				MT_MEMORY | MT_RO | MT_SECURE),
 #if USE_COHERENT_MEM
-	mmap_add_region(BL_COHERENT_RAM_BASE, BL_COHERENT_RAM_BASE,
-		BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE,
-		MT_DEVICE | MT_RW | MT_SECURE);
+		MAP_REGION_FLAT(BL_COHERENT_RAM_BASE,
+				BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE,
+				MT_DEVICE | MT_RW | MT_SECURE),
 #endif
-	/* setup xlat table */
-	init_xlat_tables();
+		/* Map TEE memory */
+		MAP_REGION_FLAT(BL32_BASE, BL32_SIZE, MT_MEMORY | MT_RW),
+		{0},
+	};
+
+	setup_page_tables(bl_regions, imx_mmap);
 	/* enable the MMU */
 	enable_mmu_el3(0);
 }
@@ -227,11 +225,6 @@
 	return COUNTER_FREQUENCY;
 }
 
-void bl31_plat_runtime_setup(void)
-{
-	return;
-}
-
 #ifdef SPD_trusty
 void plat_trusty_set_boot_args(aapcs64_params_t *args)
 {
diff --git a/plat/imx/imx8m/imx8mq/imx8mq_psci.c b/plat/imx/imx8m/imx8mq/imx8mq_psci.c
index 662017d..01582af 100644
--- a/plat/imx/imx8m/imx8mq/imx8mq_psci.c
+++ b/plat/imx/imx8m/imx8mq/imx8mq_psci.c
@@ -41,7 +41,7 @@
 
 void imx_domain_suspend(const psci_power_state_t *target_state)
 {
-	uint64_t base_addr = BL31_BASE;
+	uint64_t base_addr = BL31_START;
 	uint64_t mpidr = read_mpidr_el1();
 	unsigned int core_id = MPIDR_AFFLVL0_VAL(mpidr);
 
diff --git a/plat/imx/imx8m/imx8mq/include/platform_def.h b/plat/imx/imx8m/imx8mq/include/platform_def.h
index 1dd22d9..bb57074 100644
--- a/plat/imx/imx8m/imx8mq/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mq/include/platform_def.h
@@ -4,6 +4,8 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <plat/common/common_def.h>
+
 #define PLATFORM_LINKER_FORMAT		"elf64-littleaarch64"
 #define PLATFORM_LINKER_ARCH		aarch64
 
@@ -31,7 +33,8 @@
 #define PLAT_STOP_OFF_STATE		U(3)
 
 #define BL31_BASE			U(0x910000)
-#define BL31_LIMIT			U(0x920000)
+#define BL31_SIZE			SZ_64K
+#define BL31_LIMIT			(BL31_BASE + BL31_SIZE)
 
 /* non-secure uboot base */
 #define PLAT_NS_IMAGE_OFFSET		U(0x40200000)
@@ -54,7 +57,6 @@
 
 #define HAB_RVT_BASE			U(0x00000880) /* HAB_RVT for i.MX8MQ */
 
-#define IMX_BOOT_UART_BASE		U(0x30860000)
 #define IMX_BOOT_UART_CLK_IN_HZ		25000000 /* Select 25Mhz oscillator */
 #define PLAT_CRASH_UART_BASE		IMX_BOOT_UART_BASE
 #define PLAT_CRASH_UART_CLK_IN_HZ	25000000
@@ -128,5 +130,4 @@
 
 #define COUNTER_FREQUENCY		8333333 /* 25MHz / 3 */
 
-#define DEBUG_CONSOLE			0
 #define IMX_WDOG_B_RESET
diff --git a/plat/imx/imx8m/imx8mq/platform.mk b/plat/imx/imx8m/imx8mq/platform.mk
index 7b6df92..901a974 100644
--- a/plat/imx/imx8m/imx8mq/platform.mk
+++ b/plat/imx/imx8m/imx8mq/platform.mk
@@ -38,6 +38,7 @@
 				${XLAT_TABLES_LIB_SRCS}				\
 				${IMX_GIC_SOURCES}
 
+ENABLE_PIE		:=	1
 USE_COHERENT_MEM	:=	1
 RESET_TO_BL31		:=	1
 A53_DISABLE_NON_TEMPORAL_HINT := 0
@@ -52,6 +53,9 @@
 BL32_SIZE		?=	0x2000000
 $(eval $(call add_define,BL32_SIZE))
 
+IMX_BOOT_UART_BASE	?=	0x30860000
+$(eval $(call add_define,IMX_BOOT_UART_BASE))
+
 ifeq (${SPD},trusty)
 	BL31_CFLAGS    +=      -DPLAT_XLAT_TABLES_DYNAMIC=1
 endif
diff --git a/plat/intel/soc/agilex/bl31_plat_setup.c b/plat/intel/soc/agilex/bl31_plat_setup.c
index b1b9514..26ed7ef 100644
--- a/plat/intel/soc/agilex/bl31_plat_setup.c
+++ b/plat/intel/soc/agilex/bl31_plat_setup.c
@@ -17,6 +17,7 @@
 #include "ccu/ncore_ccu.h"
 #include "socfpga_mailbox.h"
 #include "socfpga_private.h"
+#include "socfpga_sip_svc.h"
 
 static entry_point_info_t bl32_image_ep_info;
 static entry_point_info_t bl33_image_ep_info;
@@ -35,6 +36,25 @@
 		return NULL;
 }
 
+void setup_smmu_secure_context(void)
+{
+	/*
+	 * Program SCR0 register (0xFA000000)
+	 * to set SMCFCFG bit[21] to 0x1 which raise stream match conflict fault
+	 * to set CLIENTPD bit[0] to 0x0 which enables SMMU for secure context
+	 */
+	mmio_write_32(0xFA000000, 0x00200000);
+
+	/*
+	 * Program SCR1 register (0xFA000004)
+	 * to set NSNUMSMRGO bit[14:8] to 0x4 which stream mapping register
+	 * for non-secure context and the rest will be secure context
+	 * to set NSNUMCBO bit[5:0] to 0x4 which allocate context bank
+	 * for non-secure context and the rest will be secure context
+	 */
+	mmio_write_32(0xFA000004, 0x00000404);
+}
+
 void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 				u_register_t arg2, u_register_t arg3)
 {
@@ -109,6 +129,7 @@
 	gicv2_distif_init();
 	gicv2_pcpu_distif_init();
 	gicv2_cpuif_enable();
+	setup_smmu_secure_context();
 
 	/* Signal secondary CPUs to jump to BL31 (BL2 = U-boot SPL) */
 	mmio_write_64(PLAT_CPU_RELEASE_ADDR,
diff --git a/plat/intel/soc/common/drivers/ccu/ncore_ccu.c b/plat/intel/soc/common/drivers/ccu/ncore_ccu.c
index d9a238e..99d48d2 100644
--- a/plat/intel/soc/common/drivers/ccu/ncore_ccu.c
+++ b/plat/intel/soc/common/drivers/ccu/ncore_ccu.c
@@ -40,7 +40,7 @@
 			dir_sf_en = DIRECTORY_UNIT(dir, NCORE_DIRUSFER);
 
 			/* Initialize All Entries */
-			mmio_write_32(dir_sf_mtn, SNOOP_FILTER_ID(dir));
+			mmio_write_32(dir_sf_mtn, SNOOP_FILTER_ID(sf));
 
 			/* Poll Active Bit */
 			ret = poll_active_bit(dir);
@@ -49,8 +49,9 @@
 				return -ETIMEDOUT;
 			}
 
-			/* Snoope Filter Enable */
-			mmio_setbits_32(dir_sf_en, BIT(sf));
+			/* Disable snoop filter, a bit per snoop filter */
+			mmio_clrbits_32(dir_sf_en, BIT(sf));
+
 		}
 	}
 
diff --git a/plat/intel/soc/common/include/socfpga_fcs.h b/plat/intel/soc/common/include/socfpga_fcs.h
index 893551d..8a8f348 100644
--- a/plat/intel/soc/common/include/socfpga_fcs.h
+++ b/plat/intel/soc/common/include/socfpga_fcs.h
@@ -84,6 +84,14 @@
 #define FCS_ECDSA_HASH_SIGN_CMD_MAX_WORD_SIZE			17U
 #define FCS_ECDSA_HASH_SIG_VERIFY_CMD_MAX_WORD_SIZE		52U
 #define FCS_ECDH_REQUEST_CMD_MAX_WORD_SIZE			29U
+
+#define FCS_CRYPTO_ECB_BUFFER_SIZE			12U
+#define FCS_CRYPTO_CBC_CTR_BUFFER_SIZE			28U
+#define FCS_CRYPTO_BLOCK_MODE_MASK			0x07
+#define FCS_CRYPTO_ECB_MODE			0x00
+#define FCS_CRYPTO_CBC_MODE			0x01
+#define FCS_CRYPTO_CTR_MODE			0x02
+
 /* FCS Payload Structure */
 typedef struct fcs_rng_payload_t {
 	uint32_t session_id;
@@ -235,6 +243,11 @@
 				uint32_t src_addr, uint32_t src_size,
 				uint64_t dst_addr, uint32_t *dst_size,
 				uint8_t is_finalised, uint32_t *mbox_error);
+int intel_fcs_get_digest_smmu_update_finalize(uint32_t session_id, uint32_t context_id,
+				uint32_t src_addr, uint32_t src_size,
+				uint64_t dst_addr, uint32_t *dst_size,
+				uint8_t is_finalised, uint32_t *mbox_error,
+				uint32_t *send_id);
 
 int intel_fcs_mac_verify_init(uint32_t session_id, uint32_t context_id,
 				uint32_t key_id, uint32_t param_size,
@@ -244,6 +257,11 @@
 				uint64_t dst_addr, uint32_t *dst_size,
 				uint32_t data_size, uint8_t is_finalised,
 				uint32_t *mbox_error);
+int intel_fcs_mac_verify_smmu_update_finalize(uint32_t session_id, uint32_t context_id,
+				uint32_t src_addr, uint32_t src_size,
+				uint64_t dst_addr, uint32_t *dst_size,
+				uint32_t data_size, uint8_t is_finalised,
+				uint32_t *mbox_error, uint32_t *send_id);
 
 int intel_fcs_ecdsa_hash_sign_init(uint32_t session_id, uint32_t context_id,
 				uint32_t key_id, uint32_t param_size,
@@ -270,6 +288,11 @@
 				uint32_t src_size, uint64_t dst_addr,
 				uint32_t *dst_size, uint8_t is_finalised,
 				uint32_t *mbox_error);
+int intel_fcs_ecdsa_sha2_data_sign_smmu_update_finalize(uint32_t session_id,
+				uint32_t context_id, uint32_t src_addr,
+				uint32_t src_size, uint64_t dst_addr,
+				uint32_t *dst_size, uint8_t is_finalised,
+				uint32_t *mbox_error, uint32_t *send_id);
 
 int intel_fcs_ecdsa_sha2_data_sig_verify_init(uint32_t session_id,
 				uint32_t context_id, uint32_t key_id,
@@ -280,6 +303,12 @@
 				uint32_t src_size, uint64_t dst_addr,
 				uint32_t *dst_size, uint32_t data_size,
 				uint8_t is_finalised, uint32_t *mbox_error);
+int intel_fcs_ecdsa_sha2_data_sig_verify_smmu_update_finalize(uint32_t session_id,
+				uint32_t context_id, uint32_t src_addr,
+				uint32_t src_size, uint64_t dst_addr,
+				uint32_t *dst_size, uint32_t data_size,
+				uint8_t is_finalised, uint32_t *mbox_error,
+				uint32_t *send_id);
 
 int intel_fcs_ecdsa_get_pubkey_init(uint32_t session_id, uint32_t context_id,
 				uint32_t key_id, uint32_t param_size,
diff --git a/plat/intel/soc/common/include/socfpga_mailbox.h b/plat/intel/soc/common/include/socfpga_mailbox.h
index 1f4b2a4..3abf39d 100644
--- a/plat/intel/soc/common/include/socfpga_mailbox.h
+++ b/plat/intel/soc/common/include/socfpga_mailbox.h
@@ -129,6 +129,10 @@
 #define MBOX_BUSY					-5
 #define MBOX_TIMEOUT					-2047
 
+/* Key Status */
+#define MBOX_RET_SDOS_DECRYPTION_ERROR_102		-258
+#define MBOX_RET_SDOS_DECRYPTION_ERROR_103		-259
+
 /* Reconfig Status Response */
 #define RECONFIG_STATUS_STATE				0
 #define RECONFIG_STATUS_PIN_STATUS			2
@@ -139,6 +143,7 @@
 #define SOFTFUNC_STATUS_CONF_DONE			(1 << 0)
 #define MBOX_CFGSTAT_STATE_IDLE				0x00000000
 #define MBOX_CFGSTAT_STATE_CONFIG			0x10000000
+#define MBOX_CFGSTAT_VAB_BS_PREAUTH			0x20000000
 #define MBOX_CFGSTAT_STATE_FAILACK			0x08000000
 #define MBOX_CFGSTAT_STATE_ERROR_INVALID		0xf0000001
 #define MBOX_CFGSTAT_STATE_ERROR_CORRUPT		0xf0000002
diff --git a/plat/intel/soc/common/include/socfpga_sip_svc.h b/plat/intel/soc/common/include/socfpga_sip_svc.h
index 0803eb5..21169f7 100644
--- a/plat/intel/soc/common/include/socfpga_sip_svc.h
+++ b/plat/intel/soc/common/include/socfpga_sip_svc.h
@@ -86,49 +86,57 @@
 #define INTEL_SIP_SMC_GET_USERCODE				0xC200003D
 
 /* FPGA Crypto Services */
-#define INTEL_SIP_SMC_FCS_RANDOM_NUMBER				0xC200005A
-#define INTEL_SIP_SMC_FCS_RANDOM_NUMBER_EXT			0x4200008F
-#define INTEL_SIP_SMC_FCS_CRYPTION				0x4200005B
-#define INTEL_SIP_SMC_FCS_CRYPTION_EXT				0xC2000090
-#define INTEL_SIP_SMC_FCS_SERVICE_REQUEST			0x4200005C
-#define INTEL_SIP_SMC_FCS_SEND_CERTIFICATE			0x4200005D
-#define INTEL_SIP_SMC_FCS_GET_PROVISION_DATA			0x4200005E
-#define INTEL_SIP_SMC_FCS_CNTR_SET_PREAUTH			0xC200005F
-#define INTEL_SIP_SMC_FCS_PSGSIGMA_TEARDOWN			0xC2000064
-#define INTEL_SIP_SMC_FCS_CHIP_ID				0xC2000065
-#define INTEL_SIP_SMC_FCS_ATTESTATION_SUBKEY			0xC2000066
-#define INTEL_SIP_SMC_FCS_ATTESTATION_MEASUREMENTS		0xC2000067
-#define INTEL_SIP_SMC_FCS_GET_ATTESTATION_CERT			0xC2000068
-#define INTEL_SIP_SMC_FCS_CREATE_CERT_ON_RELOAD			0xC2000069
-#define INTEL_SIP_SMC_FCS_OPEN_CS_SESSION			0xC200006E
-#define INTEL_SIP_SMC_FCS_CLOSE_CS_SESSION			0xC200006F
-#define INTEL_SIP_SMC_FCS_IMPORT_CS_KEY				0x42000070
-#define INTEL_SIP_SMC_FCS_EXPORT_CS_KEY				0xC2000071
-#define INTEL_SIP_SMC_FCS_REMOVE_CS_KEY				0xC2000072
-#define INTEL_SIP_SMC_FCS_GET_CS_KEY_INFO			0xC2000073
-#define INTEL_SIP_SMC_FCS_AES_CRYPT_INIT			0xC2000074
-#define INTEL_SIP_SMC_FCS_AES_CRYPT_UPDATE			0x42000075
-#define INTEL_SIP_SMC_FCS_AES_CRYPT_FINALIZE			0x42000076
-#define INTEL_SIP_SMC_FCS_GET_DIGEST_INIT			0xC2000077
-#define INTEL_SIP_SMC_FCS_GET_DIGEST_UPDATE			0xC2000078
-#define INTEL_SIP_SMC_FCS_GET_DIGEST_FINALIZE			0xC2000079
-#define INTEL_SIP_SMC_FCS_MAC_VERIFY_INIT			0xC200007A
-#define INTEL_SIP_SMC_FCS_MAC_VERIFY_UPDATE			0xC200007B
-#define INTEL_SIP_SMC_FCS_MAC_VERIFY_FINALIZE			0xC200007C
-#define INTEL_SIP_SMC_FCS_ECDSA_HASH_SIGN_INIT			0xC200007D
-#define INTEL_SIP_SMC_FCS_ECDSA_HASH_SIGN_FINALIZE		0xC200007F
-#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_INIT		0xC2000080
-#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_UPDATE		0xC2000081
-#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_FINALIZE		0xC2000082
-#define INTEL_SIP_SMC_FCS_ECDSA_HASH_SIG_VERIFY_INIT		0xC2000083
-#define INTEL_SIP_SMC_FCS_ECDSA_HASH_SIG_VERIFY_FINALIZE	0xC2000085
-#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_INIT	0xC2000086
-#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_UPDATE	0xC2000087
-#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_FINALIZE	0xC2000088
-#define INTEL_SIP_SMC_FCS_ECDSA_GET_PUBKEY_INIT			0xC2000089
-#define INTEL_SIP_SMC_FCS_ECDSA_GET_PUBKEY_FINALIZE		0xC200008B
-#define INTEL_SIP_SMC_FCS_ECDH_REQUEST_INIT			0xC200008C
-#define INTEL_SIP_SMC_FCS_ECDH_REQUEST_FINALIZE			0xC200008E
+#define INTEL_SIP_SMC_FCS_RANDOM_NUMBER					0xC200005A
+#define INTEL_SIP_SMC_FCS_RANDOM_NUMBER_EXT				0x4200008F
+#define INTEL_SIP_SMC_FCS_CRYPTION					0x4200005B
+#define INTEL_SIP_SMC_FCS_CRYPTION_EXT					0xC2000090
+#define INTEL_SIP_SMC_FCS_SERVICE_REQUEST				0x4200005C
+#define INTEL_SIP_SMC_FCS_SEND_CERTIFICATE				0x4200005D
+#define INTEL_SIP_SMC_FCS_GET_PROVISION_DATA				0x4200005E
+#define INTEL_SIP_SMC_FCS_CNTR_SET_PREAUTH				0xC200005F
+#define INTEL_SIP_SMC_FCS_PSGSIGMA_TEARDOWN				0xC2000064
+#define INTEL_SIP_SMC_FCS_CHIP_ID					0xC2000065
+#define INTEL_SIP_SMC_FCS_ATTESTATION_SUBKEY				0xC2000066
+#define INTEL_SIP_SMC_FCS_ATTESTATION_MEASUREMENTS			0xC2000067
+#define INTEL_SIP_SMC_FCS_GET_ATTESTATION_CERT				0xC2000068
+#define INTEL_SIP_SMC_FCS_CREATE_CERT_ON_RELOAD				0xC2000069
+#define INTEL_SIP_SMC_FCS_OPEN_CS_SESSION				0xC200006E
+#define INTEL_SIP_SMC_FCS_CLOSE_CS_SESSION				0xC200006F
+#define INTEL_SIP_SMC_FCS_IMPORT_CS_KEY					0x42000070
+#define INTEL_SIP_SMC_FCS_EXPORT_CS_KEY					0xC2000071
+#define INTEL_SIP_SMC_FCS_REMOVE_CS_KEY					0xC2000072
+#define INTEL_SIP_SMC_FCS_GET_CS_KEY_INFO				0xC2000073
+#define INTEL_SIP_SMC_FCS_AES_CRYPT_INIT				0xC2000074
+#define INTEL_SIP_SMC_FCS_AES_CRYPT_UPDATE				0x42000075
+#define INTEL_SIP_SMC_FCS_AES_CRYPT_FINALIZE				0x42000076
+#define INTEL_SIP_SMC_FCS_GET_DIGEST_INIT				0xC2000077
+#define INTEL_SIP_SMC_FCS_GET_DIGEST_UPDATE				0xC2000078
+#define INTEL_SIP_SMC_FCS_GET_DIGEST_FINALIZE				0xC2000079
+#define INTEL_SIP_SMC_FCS_GET_DIGEST_SMMU_UPDATE			0x42000091
+#define INTEL_SIP_SMC_FCS_GET_DIGEST_SMMU_FINALIZE			0x42000092
+#define INTEL_SIP_SMC_FCS_MAC_VERIFY_INIT				0xC200007A
+#define INTEL_SIP_SMC_FCS_MAC_VERIFY_UPDATE				0xC200007B
+#define INTEL_SIP_SMC_FCS_MAC_VERIFY_FINALIZE				0xC200007C
+#define INTEL_SIP_SMC_FCS_MAC_VERIFY_SMMU_UPDATE			0x42000093
+#define INTEL_SIP_SMC_FCS_MAC_VERIFY_SMMU_FINALIZE			0x42000094
+#define INTEL_SIP_SMC_FCS_ECDSA_HASH_SIGN_INIT				0xC200007D
+#define INTEL_SIP_SMC_FCS_ECDSA_HASH_SIGN_FINALIZE			0xC200007F
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_INIT			0xC2000080
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_UPDATE			0xC2000081
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_FINALIZE			0xC2000082
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_SMMU_UPDATE		0x42000095
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_SMMU_FINALIZE		0x42000096
+#define INTEL_SIP_SMC_FCS_ECDSA_HASH_SIG_VERIFY_INIT			0xC2000083
+#define INTEL_SIP_SMC_FCS_ECDSA_HASH_SIG_VERIFY_FINALIZE		0xC2000085
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_INIT		0xC2000086
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_UPDATE		0xC2000087
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_FINALIZE		0xC2000088
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_SMMU_UPDATE	0x42000097
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_SMMU_FINALIZE	0x42000098
+#define INTEL_SIP_SMC_FCS_ECDSA_GET_PUBKEY_INIT				0xC2000089
+#define INTEL_SIP_SMC_FCS_ECDSA_GET_PUBKEY_FINALIZE			0xC200008B
+#define INTEL_SIP_SMC_FCS_ECDH_REQUEST_INIT				0xC200008C
+#define INTEL_SIP_SMC_FCS_ECDH_REQUEST_FINALIZE				0xC200008E
 
 #define INTEL_SIP_SMC_FCS_SHA_MODE_MASK				0xF
 #define INTEL_SIP_SMC_FCS_DIGEST_SIZE_MASK			0xF
@@ -166,8 +174,14 @@
 #define SIP_SVC_VERSION						0x8200ff03
 
 /* SiP Service Calls version numbers */
-#define SIP_SVC_VERSION_MAJOR					1
-#define SIP_SVC_VERSION_MINOR					0
+/*
+ * Increase if there is any backward compatibility impact
+ */
+#define SIP_SVC_VERSION_MAJOR					2
+/*
+ * Increase if there is new SMC function ID being added
+ */
+#define SIP_SVC_VERSION_MINOR					1
 
 
 /* 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 7f67313..69ee6d3 100644
--- a/plat/intel/soc/common/include/socfpga_system_manager.h
+++ b/plat/intel/soc/common/include/socfpga_system_manager.h
@@ -42,6 +42,8 @@
 #define IDLE_DATA_SOC2FPGA				BIT(0)
 #define IDLE_DATA_MASK		(IDLE_DATA_LWSOC2FPGA | IDLE_DATA_SOC2FPGA)
 
+#define SYSMGR_QSPI_REFCLK_MASK				GENMASK(27, 0)
+
 #define SYSMGR_ECC_OCRAM_MASK				BIT(1)
 #define SYSMGR_ECC_DDR0_MASK				BIT(16)
 #define SYSMGR_ECC_DDR1_MASK				BIT(17)
diff --git a/plat/intel/soc/common/sip/socfpga_sip_fcs.c b/plat/intel/soc/common/sip/socfpga_sip_fcs.c
index facee0f..3b0b370 100644
--- a/plat/intel/soc/common/sip/socfpga_sip_fcs.c
+++ b/plat/intel/soc/common/sip/socfpga_sip_fcs.c
@@ -411,7 +411,10 @@
 				(uint32_t *) &payload, payload_size,
 				CMD_CASUAL, resp_data, &resp_len);
 
-	if (status < 0) {
+	if (status == MBOX_RET_SDOS_DECRYPTION_ERROR_102 ||
+		status == MBOX_RET_SDOS_DECRYPTION_ERROR_103) {
+		*mbox_error = -status;
+	} else if (status < 0) {
 		*mbox_error = -status;
 		return INTEL_SIP_SMC_STATUS_ERROR;
 	}
@@ -946,6 +949,104 @@
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
+int intel_fcs_get_digest_smmu_update_finalize(uint32_t session_id,
+				uint32_t context_id, uint32_t src_addr,
+				uint32_t src_size, uint64_t dst_addr,
+				uint32_t *dst_size, uint8_t is_finalised,
+				uint32_t *mbox_error, uint32_t *send_id)
+{
+	int status;
+	uint32_t i;
+	uint32_t flag;
+	uint32_t crypto_header;
+	uint32_t resp_len;
+	uint32_t payload[FCS_GET_DIGEST_CMD_MAX_WORD_SIZE] = {0U};
+
+	/* Source data must be 8 bytes aligned */
+	if (dst_size == NULL || mbox_error == NULL ||
+		!is_8_bytes_aligned(src_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (fcs_sha_get_digest_param.session_id != session_id ||
+	    fcs_sha_get_digest_param.context_id != context_id) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_address_in_ddr_range(src_addr, src_size) ||
+		 !is_address_in_ddr_range(dst_addr, *dst_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	resp_len = *dst_size / MBOX_WORD_BYTE;
+
+	/* Prepare crypto header */
+	flag = 0;
+
+	if (fcs_sha_get_digest_param.is_updated) {
+		fcs_sha_get_digest_param.crypto_param_size = 0;
+	} else {
+		flag |=  FCS_CS_FIELD_FLAG_INIT;
+	}
+
+	if (is_finalised != 0U) {
+		flag |=  FCS_CS_FIELD_FLAG_FINALIZE;
+	} else {
+		flag |=  FCS_CS_FIELD_FLAG_UPDATE;
+		fcs_sha_get_digest_param.is_updated = 1;
+	}
+
+	crypto_header = ((flag << FCS_CS_FIELD_FLAG_OFFSET) |
+			(fcs_sha_get_digest_param.crypto_param_size &
+			FCS_CS_FIELD_SIZE_MASK));
+
+	/* Prepare command payload */
+	i = 0;
+	payload[i] = fcs_sha_get_digest_param.session_id;
+	i++;
+	payload[i] = fcs_sha_get_digest_param.context_id;
+	i++;
+	payload[i] = crypto_header;
+	i++;
+
+	if ((crypto_header >> FCS_CS_FIELD_FLAG_OFFSET) &
+		FCS_CS_FIELD_FLAG_INIT) {
+		payload[i] = fcs_sha_get_digest_param.key_id;
+		i++;
+		/* Crypto parameters */
+		payload[i] = fcs_sha_get_digest_param.crypto_param
+				& INTEL_SIP_SMC_FCS_SHA_MODE_MASK;
+		payload[i] |= ((fcs_sha_get_digest_param.crypto_param
+				>> INTEL_SIP_SMC_FCS_DIGEST_SIZE_OFFSET)
+				& INTEL_SIP_SMC_FCS_DIGEST_SIZE_MASK)
+				<< FCS_SHA_HMAC_CRYPTO_PARAM_SIZE_OFFSET;
+		i++;
+	}
+	/* Data source address and size */
+	payload[i] = src_addr;
+	i++;
+	payload[i] = src_size;
+	i++;
+
+	status = mailbox_send_cmd_async(send_id, MBOX_FCS_GET_DIGEST_REQ,
+					payload, i, CMD_INDIRECT);
+
+	if (is_finalised != 0U) {
+		memset((void *)&fcs_sha_get_digest_param, 0,
+		sizeof(fcs_crypto_service_data));
+	}
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	*dst_size = resp_len * MBOX_WORD_BYTE;
+	flush_dcache_range(dst_addr, *dst_size);
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
 int intel_fcs_mac_verify_init(uint32_t session_id, uint32_t context_id,
 				uint32_t key_id, uint32_t param_size,
 				uint64_t param_data, uint32_t *mbox_error)
@@ -1071,6 +1172,127 @@
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
+int intel_fcs_mac_verify_smmu_update_finalize(uint32_t session_id,
+				uint32_t context_id, uint32_t src_addr,
+				uint32_t src_size, uint64_t dst_addr,
+				uint32_t *dst_size, uint32_t data_size,
+				uint8_t is_finalised, uint32_t *mbox_error,
+				uint32_t *send_id)
+{
+	int status;
+	uint32_t i;
+	uint32_t flag;
+	uint32_t crypto_header;
+	uint32_t resp_len;
+	uint32_t payload[FCS_MAC_VERIFY_CMD_MAX_WORD_SIZE] = {0U};
+	uintptr_t mac_offset;
+
+	/*
+	 * Source data must be 4 bytes aligned
+	 * User data must be 8 bytes aligned
+	 */
+	if (dst_size == NULL || mbox_error == NULL ||
+		!is_size_4_bytes_aligned(src_size) ||
+		!is_8_bytes_aligned(data_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (data_size > src_size) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (fcs_sha_mac_verify_param.session_id != session_id ||
+		fcs_sha_mac_verify_param.context_id != context_id) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_address_in_ddr_range(src_addr, src_size) ||
+		!is_address_in_ddr_range(dst_addr, *dst_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	resp_len = *dst_size / MBOX_WORD_BYTE;
+
+	/* Prepare crypto header */
+	flag = 0;
+
+	if (fcs_sha_mac_verify_param.is_updated) {
+		fcs_sha_mac_verify_param.crypto_param_size = 0;
+	} else {
+		flag |=  FCS_CS_FIELD_FLAG_INIT;
+	}
+
+	if (is_finalised) {
+		flag |=  FCS_CS_FIELD_FLAG_FINALIZE;
+	} else {
+		flag |=  FCS_CS_FIELD_FLAG_UPDATE;
+		fcs_sha_mac_verify_param.is_updated = 1;
+	}
+
+	crypto_header = ((flag << FCS_CS_FIELD_FLAG_OFFSET) |
+			(fcs_sha_mac_verify_param.crypto_param_size &
+			FCS_CS_FIELD_SIZE_MASK));
+
+	/* Prepare command payload */
+	i = 0;
+	payload[i] = fcs_sha_mac_verify_param.session_id;
+	i++;
+	payload[i] = fcs_sha_mac_verify_param.context_id;
+	i++;
+	payload[i] = crypto_header;
+	i++;
+
+	if ((crypto_header >> FCS_CS_FIELD_FLAG_OFFSET) &
+		FCS_CS_FIELD_FLAG_INIT) {
+		payload[i] = fcs_sha_mac_verify_param.key_id;
+		i++;
+		/* Crypto parameters */
+		payload[i] = ((fcs_sha_mac_verify_param.crypto_param
+				>> INTEL_SIP_SMC_FCS_DIGEST_SIZE_OFFSET)
+				& INTEL_SIP_SMC_FCS_DIGEST_SIZE_MASK)
+				<< FCS_SHA_HMAC_CRYPTO_PARAM_SIZE_OFFSET;
+		i++;
+	}
+	/* Data source address and size */
+	payload[i] = src_addr;
+	i++;
+	payload[i] = data_size;
+	i++;
+
+	if ((crypto_header >> FCS_CS_FIELD_FLAG_OFFSET) &
+		FCS_CS_FIELD_FLAG_FINALIZE) {
+		/* Copy mac data to command
+		 * Using dst_addr (physical address) to store mac_offset
+		 * mac_offset = MAC data
+		 */
+		mac_offset = dst_addr;
+		memcpy((uint8_t *) &payload[i], (uint8_t *) mac_offset,
+		src_size - data_size);
+
+		memset((void *)&dst_addr, 0, sizeof(dst_size));
+
+		i += (src_size - data_size) / MBOX_WORD_BYTE;
+	}
+
+	status = mailbox_send_cmd_async(send_id, MBOX_FCS_MAC_VERIFY_REQ,
+					payload, i, CMD_INDIRECT);
+
+	if (is_finalised) {
+		memset((void *)&fcs_sha_mac_verify_param, 0,
+		sizeof(fcs_crypto_service_data));
+	}
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	*dst_size = resp_len * MBOX_WORD_BYTE;
+	flush_dcache_range(dst_addr, *dst_size);
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
 int intel_fcs_ecdsa_hash_sign_init(uint32_t session_id, uint32_t context_id,
 				uint32_t key_id, uint32_t param_size,
 				uint64_t param_data, uint32_t *mbox_error)
@@ -1348,6 +1570,99 @@
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
+int intel_fcs_ecdsa_sha2_data_sign_smmu_update_finalize(uint32_t session_id,
+				uint32_t context_id, uint32_t src_addr,
+				uint32_t src_size, uint64_t dst_addr,
+				uint32_t *dst_size, uint8_t is_finalised,
+				uint32_t *mbox_error, uint32_t *send_id)
+{
+	int status;
+	int i;
+	uint32_t flag;
+	uint32_t crypto_header;
+	uint32_t payload[FCS_ECDSA_SHA2_DATA_SIGN_CMD_MAX_WORD_SIZE] = {0U};
+	uint32_t resp_len;
+
+	/* Source data must be 8 bytes aligned */
+	if ((dst_size == NULL) || (mbox_error == NULL ||
+		!is_8_bytes_aligned(src_size))) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (fcs_sha2_data_sign_param.session_id != session_id ||
+		fcs_sha2_data_sign_param.context_id != context_id) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_address_in_ddr_range(src_addr, src_size) ||
+		!is_address_in_ddr_range(dst_addr, *dst_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	resp_len = *dst_size / MBOX_WORD_BYTE;
+
+	/* Prepare crypto header */
+	flag = 0;
+	if (fcs_sha2_data_sign_param.is_updated) {
+		fcs_sha2_data_sign_param.crypto_param_size = 0;
+	} else {
+		flag |= FCS_CS_FIELD_FLAG_INIT;
+	}
+
+	if (is_finalised != 0U) {
+		flag |= FCS_CS_FIELD_FLAG_FINALIZE;
+	} else {
+		flag |= FCS_CS_FIELD_FLAG_UPDATE;
+		fcs_sha2_data_sign_param.is_updated = 1;
+	}
+	crypto_header = (flag << FCS_CS_FIELD_FLAG_OFFSET) |
+			fcs_sha2_data_sign_param.crypto_param_size;
+
+	/* Prepare command payload */
+	i = 0;
+	payload[i] = fcs_sha2_data_sign_param.session_id;
+	i++;
+	payload[i] = fcs_sha2_data_sign_param.context_id;
+	i++;
+	payload[i] = crypto_header;
+	i++;
+
+	if ((crypto_header >> FCS_CS_FIELD_FLAG_OFFSET) &
+		FCS_CS_FIELD_FLAG_INIT) {
+		payload[i] = fcs_sha2_data_sign_param.key_id;
+		/* Crypto parameters */
+		i++;
+		payload[i] = fcs_sha2_data_sign_param.crypto_param
+				& INTEL_SIP_SMC_FCS_ECC_ALGO_MASK;
+		i++;
+	}
+
+	/* Data source address and size */
+	payload[i] = src_addr;
+	i++;
+	payload[i] = src_size;
+	i++;
+
+	status = mailbox_send_cmd_async(send_id,
+					MBOX_FCS_ECDSA_SHA2_DATA_SIGN_REQ,
+					payload, i, CMD_INDIRECT);
+
+	if (is_finalised != 0U) {
+		memset((void *)&fcs_sha2_data_sign_param, 0,
+			sizeof(fcs_crypto_service_data));
+	}
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	*dst_size = resp_len * MBOX_WORD_BYTE;
+	flush_dcache_range(dst_addr, *dst_size);
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
 int intel_fcs_ecdsa_sha2_data_sig_verify_init(uint32_t session_id,
 				uint32_t context_id, uint32_t key_id,
 				uint32_t param_size, uint64_t param_data,
@@ -1469,6 +1784,121 @@
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
+int intel_fcs_ecdsa_sha2_data_sig_verify_smmu_update_finalize(uint32_t session_id,
+				uint32_t context_id, uint32_t src_addr,
+				uint32_t src_size, uint64_t dst_addr,
+				uint32_t *dst_size, uint32_t data_size,
+				uint8_t is_finalised, uint32_t *mbox_error,
+				uint32_t *send_id)
+{
+	int status;
+	uint32_t i;
+	uint32_t flag;
+	uint32_t crypto_header;
+	uint32_t payload[FCS_ECDSA_SHA2_DATA_SIG_VERIFY_CMD_MAX_WORD_SIZE] = {0U};
+	uint32_t resp_len;
+	uintptr_t sig_pubkey_offset;
+
+	/*
+	 * Source data must be 4 bytes aligned
+	 * Source addrress must be 8 bytes aligned
+	 * User data must be 8 bytes aligned
+	 */
+	if ((dst_size == NULL) || (mbox_error == NULL) ||
+		!is_size_4_bytes_aligned(src_size) ||
+		!is_8_bytes_aligned(src_addr) ||
+		!is_8_bytes_aligned(data_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (fcs_sha2_data_sig_verify_param.session_id != session_id ||
+		fcs_sha2_data_sig_verify_param.context_id != context_id) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_address_in_ddr_range(src_addr, src_size) ||
+		!is_address_in_ddr_range(dst_addr, *dst_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	resp_len = *dst_size / MBOX_WORD_BYTE;
+
+	/* Prepare crypto header */
+	flag = 0;
+	if (fcs_sha2_data_sig_verify_param.is_updated)
+		fcs_sha2_data_sig_verify_param.crypto_param_size = 0;
+	else
+		flag |= FCS_CS_FIELD_FLAG_INIT;
+
+	if (is_finalised != 0U)
+		flag |= FCS_CS_FIELD_FLAG_FINALIZE;
+	else {
+		flag |= FCS_CS_FIELD_FLAG_UPDATE;
+		fcs_sha2_data_sig_verify_param.is_updated = 1;
+	}
+	crypto_header = (flag << FCS_CS_FIELD_FLAG_OFFSET) |
+			fcs_sha2_data_sig_verify_param.crypto_param_size;
+
+	/* Prepare command payload */
+	i = 0;
+	payload[i] = fcs_sha2_data_sig_verify_param.session_id;
+	i++;
+	payload[i] = fcs_sha2_data_sig_verify_param.context_id;
+	i++;
+	payload[i] = crypto_header;
+	i++;
+
+	if ((crypto_header >> FCS_CS_FIELD_FLAG_OFFSET) &
+		FCS_CS_FIELD_FLAG_INIT) {
+		payload[i] = fcs_sha2_data_sig_verify_param.key_id;
+		i++;
+		/* Crypto parameters */
+		payload[i] = fcs_sha2_data_sig_verify_param.crypto_param
+				& INTEL_SIP_SMC_FCS_ECC_ALGO_MASK;
+		i++;
+	}
+
+	/* Data source address and size */
+	payload[i] = src_addr;
+	i++;
+	payload[i] = data_size;
+	i++;
+
+	if ((crypto_header >> FCS_CS_FIELD_FLAG_OFFSET) &
+		FCS_CS_FIELD_FLAG_FINALIZE) {
+		/* Copy mac data to command
+		 * Using dst_addr (physical address) to store sig_pubkey_offset
+		 * sig_pubkey_offset is Signature + Public Key Data
+		 */
+		sig_pubkey_offset = dst_addr;
+		memcpy((uint8_t *) &payload[i], (uint8_t *) sig_pubkey_offset,
+			src_size - data_size);
+
+		memset((void *)&dst_addr, 0, sizeof(dst_size));
+
+		i += (src_size - data_size) / MBOX_WORD_BYTE;
+	}
+
+	status = mailbox_send_cmd_async(send_id,
+					MBOX_FCS_ECDSA_SHA2_DATA_SIGN_VERIFY,
+					payload, i, CMD_INDIRECT);
+
+	if (is_finalised != 0U) {
+		memset((void *) &fcs_sha2_data_sig_verify_param, 0,
+			sizeof(fcs_crypto_service_data));
+	}
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	*dst_size = resp_len * MBOX_WORD_BYTE;
+	flush_dcache_range(dst_addr, *dst_size);
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
 int intel_fcs_ecdsa_get_pubkey_init(uint32_t session_id, uint32_t context_id,
 				uint32_t key_id, uint32_t param_size,
 				uint64_t param_data, uint32_t *mbox_error)
@@ -1620,6 +2050,29 @@
 				uint32_t key_id, uint64_t param_addr,
 				uint32_t param_size, uint32_t *mbox_error)
 {
+	/* ptr to get param_addr value */
+	uint64_t *param_addr_ptr;
+
+	param_addr_ptr = (uint64_t *) param_addr;
+
+	/*
+	 * Since crypto param size vary between mode.
+	 * Check ECB here and limit to size 12 bytes
+	 */
+	if (((*param_addr_ptr & FCS_CRYPTO_BLOCK_MODE_MASK) == FCS_CRYPTO_ECB_MODE) &&
+		(param_size > FCS_CRYPTO_ECB_BUFFER_SIZE)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+	/*
+	 * Since crypto param size vary between mode.
+	 * Check CBC/CTR here and limit to size 28 bytes
+	 */
+	if ((((*param_addr_ptr & FCS_CRYPTO_BLOCK_MODE_MASK) == FCS_CRYPTO_CBC_MODE) ||
+		((*param_addr_ptr & FCS_CRYPTO_BLOCK_MODE_MASK) == FCS_CRYPTO_CTR_MODE)) &&
+		(param_size > FCS_CRYPTO_CBC_CTR_BUFFER_SIZE)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
 	if (mbox_error == NULL) {
 		return INTEL_SIP_SMC_STATUS_REJECTED;
 	}
diff --git a/plat/intel/soc/common/soc/socfpga_mailbox.c b/plat/intel/soc/common/soc/socfpga_mailbox.c
index d14ac9a..7010d81 100644
--- a/plat/intel/soc/common/soc/socfpga_mailbox.c
+++ b/plat/intel/soc/common/soc/socfpga_mailbox.c
@@ -10,6 +10,7 @@
 
 #include "socfpga_mailbox.h"
 #include "socfpga_sip_svc.h"
+#include "socfpga_system_manager.h"
 
 static mailbox_payload_t mailbox_resp_payload;
 static mailbox_container_t mailbox_resp_ctr = {0, 0, &mailbox_resp_payload};
@@ -464,8 +465,26 @@
 
 void mailbox_set_qspi_direct(void)
 {
+	uint32_t response[1], qspi_clk, reg;
+	unsigned int resp_len = ARRAY_SIZE(response);
+
 	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_QSPI_DIRECT, NULL, 0U,
-				CMD_CASUAL, NULL, NULL);
+			 CMD_CASUAL, response, &resp_len);
+
+	qspi_clk = response[0];
+	INFO("QSPI ref clock: %u\n", qspi_clk);
+
+	/*
+	 * Store QSPI ref clock frequency in BOOT_SCRATCH_COLD_0 register for
+	 * later boot loader (i.e. u-boot) use.
+	 * The frequency is stored in kHz and occupies BOOT_SCRATCH_COLD_0
+	 * register bits[27:0].
+	 */
+	qspi_clk /= 1000;
+	reg = mmio_read_32(SOCFPGA_SYSMGR(BOOT_SCRATCH_COLD_0));
+	reg &= ~SYSMGR_QSPI_REFCLK_MASK;
+	reg |= qspi_clk & SYSMGR_QSPI_REFCLK_MASK;
+	mmio_write_32(SOCFPGA_SYSMGR(BOOT_SCRATCH_COLD_0), reg);
 }
 
 void mailbox_set_qspi_close(void)
@@ -590,6 +609,11 @@
 	}
 
 	res = response[RECONFIG_STATUS_STATE];
+
+	if (res == MBOX_CFGSTAT_VAB_BS_PREAUTH) {
+		return MBOX_CFGSTAT_STATE_CONFIG;
+	}
+
 	if ((res != 0U) && (res != MBOX_CFGSTAT_STATE_CONFIG)) {
 		return res;
 	}
diff --git a/plat/intel/soc/common/socfpga_sip_svc.c b/plat/intel/soc/common/socfpga_sip_svc.c
index b57ab92..79f743f 100644
--- a/plat/intel/soc/common/socfpga_sip_svc.c
+++ b/plat/intel/soc/common/socfpga_sip_svc.c
@@ -344,6 +344,28 @@
 	case(0xF8011120):	/* INTSTAT */
 	case(0xF8011124):	/* DIAGINTTEST */
 	case(0xF801112C):	/* DERRADDRA */
+	case(0xFA000000):	/* SMMU SCR0 */
+	case(0xFA000004):	/* SMMU SCR1 */
+	case(0xFA000400):	/* SMMU NSCR0 */
+	case(0xFA004000):	/* SMMU SSD0_REG */
+	case(0xFA000820):	/* SMMU SMR8 */
+	case(0xFA000c20):	/* SMMU SCR8 */
+	case(0xFA028000):	/* SMMU CB8_SCTRL */
+	case(0xFA001020):	/* SMMU CBAR8 */
+	case(0xFA028030):	/* SMMU TCR_LPAE */
+	case(0xFA028020):	/* SMMU CB8_TTBR0_LOW */
+	case(0xFA028024):	/* SMMU CB8_PRRR_HIGH */
+	case(0xFA028038):	/* SMMU CB8_PRRR_MIR0 */
+	case(0xFA02803C):	/* SMMU CB8_PRRR_MIR1 */
+	case(0xFA028010):	/* SMMU_CB8)TCR2 */
+	case(0xFFD080A4):	/* SDM SMMU STREAM ID REG */
+	case(0xFA001820):	/* SMMU_CBA2R8 */
+	case(0xFA000074):	/* SMMU_STLBGSTATUS */
+	case(0xFA0287F4):	/* SMMU_CB8_TLBSTATUS */
+	case(0xFA000060):	/* SMMU_STLBIALL */
+	case(0xFA000070):	/* SMMU_STLBGSYNC */
+	case(0xFA028618):	/* CB8_TLBALL */
+	case(0xFA0287F0):	/* CB8_TLBSYNC */
 	case(0xFFD12028):	/* SDMMCGRP_CTRL */
 	case(0xFFD12044):	/* EMAC0 */
 	case(0xFFD12048):	/* EMAC1 */
@@ -589,7 +611,10 @@
 	*ret_size = resp_len * MBOX_WORD_BYTE;
 	flush_dcache_range(addr, *ret_size);
 
-	if (status != MBOX_RET_OK) {
+	if (status == MBOX_RET_SDOS_DECRYPTION_ERROR_102 ||
+		status == MBOX_RET_SDOS_DECRYPTION_ERROR_103) {
+		*mbox_error = -status;
+	} else if (status != MBOX_RET_OK) {
 		*mbox_error = -status;
 		return INTEL_SIP_SMC_STATUS_ERROR;
 	}
@@ -927,6 +952,22 @@
 					&mbox_error);
 		SMC_RET4(handle, status, mbox_error, x5, x6);
 
+	case INTEL_SIP_SMC_FCS_GET_DIGEST_SMMU_UPDATE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		status = intel_fcs_get_digest_smmu_update_finalize(x1, x2, x3,
+					x4, x5, (uint32_t *) &x6, false,
+					&mbox_error, &send_id);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
+	case INTEL_SIP_SMC_FCS_GET_DIGEST_SMMU_FINALIZE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		status = intel_fcs_get_digest_smmu_update_finalize(x1, x2, x3,
+					x4, x5, (uint32_t *) &x6, true,
+					&mbox_error, &send_id);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
 	case INTEL_SIP_SMC_FCS_MAC_VERIFY_INIT:
 		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
 		status = intel_fcs_mac_verify_init(x1, x2, x3,
@@ -951,6 +992,24 @@
 					true, &mbox_error);
 		SMC_RET4(handle, status, mbox_error, x5, x6);
 
+	case INTEL_SIP_SMC_FCS_MAC_VERIFY_SMMU_UPDATE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		x7 = SMC_GET_GP(handle, CTX_GPREG_X7);
+		status = intel_fcs_mac_verify_smmu_update_finalize(x1, x2, x3,
+					x4, x5, (uint32_t *) &x6, x7,
+					false, &mbox_error, &send_id);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
+	case INTEL_SIP_SMC_FCS_MAC_VERIFY_SMMU_FINALIZE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		x7 = SMC_GET_GP(handle, CTX_GPREG_X7);
+		status = intel_fcs_mac_verify_smmu_update_finalize(x1, x2, x3,
+					x4, x5, (uint32_t *) &x6, x7,
+					true, &mbox_error, &send_id);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
 	case INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_INIT:
 		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
 		status = intel_fcs_ecdsa_sha2_data_sign_init(x1, x2, x3,
@@ -973,6 +1032,22 @@
 					&mbox_error);
 		SMC_RET4(handle, status, mbox_error, x5, x6);
 
+	case INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_SMMU_UPDATE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		status = intel_fcs_ecdsa_sha2_data_sign_smmu_update_finalize(x1,
+					x2, x3, x4, x5, (uint32_t *) &x6, false,
+					&mbox_error, &send_id);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
+	case INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_SMMU_FINALIZE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		status = intel_fcs_ecdsa_sha2_data_sign_smmu_update_finalize(x1,
+					x2, x3, x4, x5, (uint32_t *) &x6, true,
+					&mbox_error, &send_id);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
 	case INTEL_SIP_SMC_FCS_ECDSA_HASH_SIGN_INIT:
 		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
 		status = intel_fcs_ecdsa_hash_sign_init(x1, x2, x3,
@@ -1014,6 +1089,24 @@
 					x7, false, &mbox_error);
 		SMC_RET4(handle, status, mbox_error, x5, x6);
 
+	case INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_SMMU_UPDATE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		x7 = SMC_GET_GP(handle, CTX_GPREG_X7);
+		status = intel_fcs_ecdsa_sha2_data_sig_verify_smmu_update_finalize(
+					x1, x2, x3, x4, x5, (uint32_t *) &x6,
+					x7, false, &mbox_error, &send_id);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
+	case INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_SMMU_FINALIZE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		x7 = SMC_GET_GP(handle, CTX_GPREG_X7);
+		status = intel_fcs_ecdsa_sha2_data_sig_verify_smmu_update_finalize(
+					x1, x2, x3, x4, x5, (uint32_t *) &x6,
+					x7, true, &mbox_error, &send_id);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
 	case INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_FINALIZE:
 		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
 		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
diff --git a/plat/mediatek/drivers/iommu/mtk_iommu_smc.c b/plat/mediatek/drivers/iommu/mtk_iommu_smc.c
index 9762d0b..e998725 100644
--- a/plat/mediatek/drivers/iommu/mtk_iommu_smc.c
+++ b/plat/mediatek/drivers/iommu/mtk_iommu_smc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, MediaTek Inc. All rights reserved.
+ * Copyright (c) 2022-2023, MediaTek Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -75,25 +75,32 @@
 	return MTK_SIP_E_SUCCESS;
 }
 
-static int mtk_infra_master_config_sec(uint32_t dev_id, uint32_t enable)
+static int mtk_infra_master_config_sec(uint32_t dev_id_msk, uint32_t enable)
 {
 	const struct mtk_ifr_mst_config *ifr_cfg;
-	uint32_t reg_addr;
+	uint32_t dev_id, reg_addr, reg_mask;
 
 	mtk_infra_iommu_enable_protect();
 
-	if (dev_id >= MMU_DEV_NUM) {
-		return MTK_SIP_E_NOT_SUPPORTED;
+	if (dev_id_msk >= BIT(MMU_DEV_NUM)) {
+		return MTK_SIP_E_INVALID_PARAM;
 	}
 
-	ifr_cfg = &g_ifr_mst_cfg[dev_id];
-	reg_addr = g_ifr_mst_cfg_base[(ifr_cfg->cfg_addr_idx)] +
-		   g_ifr_mst_cfg_offs[(ifr_cfg->cfg_addr_idx)];
+	for (dev_id = 0U; dev_id < MMU_DEV_NUM; dev_id++) {
+		if ((dev_id_msk & BIT(dev_id)) == 0U) {
+			continue;
+		}
 
-	if (enable > 0U) {
-		mmio_setbits_32(reg_addr, IFR_CFG_MMU_EN_MSK(ifr_cfg->r_mmu_en_bit));
-	} else {
-		mmio_clrbits_32(reg_addr, IFR_CFG_MMU_EN_MSK(ifr_cfg->r_mmu_en_bit));
+		ifr_cfg = &g_ifr_mst_cfg[dev_id];
+		reg_addr = g_ifr_mst_cfg_base[(ifr_cfg->cfg_addr_idx)] +
+			   g_ifr_mst_cfg_offs[(ifr_cfg->cfg_addr_idx)];
+		reg_mask = IFR_CFG_MMU_EN_MSK(ifr_cfg->r_mmu_en_bit);
+
+		if (enable > 0U) {
+			mmio_setbits_32(reg_addr, reg_mask);
+		} else {
+			mmio_clrbits_32(reg_addr, reg_mask);
+		}
 	}
 
 	return MTK_SIP_E_SUCCESS;
diff --git a/plat/nxp/common/nv_storage/plat_nv_storage.c b/plat/nxp/common/nv_storage/plat_nv_storage.c
index af3b966..97d777e 100644
--- a/plat/nxp/common/nv_storage/plat_nv_storage.c
+++ b/plat/nxp/common/nv_storage/plat_nv_storage.c
@@ -93,7 +93,7 @@
 	uint8_t ready_to_write_val[sizeof(nv_app_data_t)];
 	uintptr_t nv_base_addr = NV_STORAGE_BASE_ADDR;
 
-	assert((nv_base_addr + data_offset + data_size) > (nv_base_addr + F_SECTOR_ERASE_SZ));
+	assert((nv_base_addr + data_offset + data_size) <= (nv_base_addr + F_SECTOR_ERASE_SZ));
 
 	ret = fspi_init(NXP_FLEXSPI_ADDR, NXP_FLEXSPI_FLASH_ADDR);
 
diff --git a/plat/nxp/soc-ls1028a/soc.def b/plat/nxp/soc-ls1028a/soc.def
index c23c1bb..93275b3 100644
--- a/plat/nxp/soc-ls1028a/soc.def
+++ b/plat/nxp/soc-ls1028a/soc.def
@@ -70,8 +70,8 @@
 BL2_HDR_LOC	:=	0x1800A000
 
 # SoC ERRATAS to be enabled
-ERRATA_SOC_A008850	:=	1
 
+# DDR ERRATA
 ERRATA_DDR_A009803	:=	1
 ERRATA_DDR_A009942	:=	1
 ERRATA_DDR_A010165	:=	1
diff --git a/plat/nxp/soc-ls1043a/soc.c b/plat/nxp/soc-ls1043a/soc.c
index 7badf8c..3e821d0 100644
--- a/plat/nxp/soc-ls1043a/soc.c
+++ b/plat/nxp/soc-ls1043a/soc.c
@@ -21,9 +21,7 @@
 #ifdef POLICY_FUSE_PROVISION
 #include <nxp_gpio.h>
 #endif
-#if TRUSTED_BOARD_BOOT
 #include <nxp_smmu.h>
-#endif
 #include <nxp_timer.h>
 #include <plat_console.h>
 #include <plat_gic.h>
@@ -174,6 +172,12 @@
 	get_cluster_info(soc_list, ARRAY_SIZE(soc_list), &num_clusters, &cores_per_cluster);
 	plat_ls_interconnect_enter_coherency(num_clusters);
 
+	/*
+	 * Unlock write access for SMMU SMMU_CBn_ACTLR in all Non-secure contexts.
+	 */
+	smmu_cache_unlock(NXP_SMMU_ADDR);
+	INFO("SMMU Cache Unlocking is Configured.\n");
+
 #if TRUSTED_BOARD_BOOT
 	uint32_t mode;
 
diff --git a/plat/nxp/soc-ls1043a/soc.mk b/plat/nxp/soc-ls1043a/soc.mk
index b6ce14e..0ebb377 100644
--- a/plat/nxp/soc-ls1043a/soc.mk
+++ b/plat/nxp/soc-ls1043a/soc.mk
@@ -19,8 +19,8 @@
 
 # For Security Features
 DISABLE_FUSE_WRITE	:= 1
-ifeq (${TRUSTED_BOARD_BOOT}, 1)
 $(eval $(call SET_NXP_MAKE_FLAG,SMMU_NEEDED,BL2))
+ifeq (${TRUSTED_BOARD_BOOT}, 1)
 $(eval $(call SET_NXP_MAKE_FLAG,SFP_NEEDED,BL2))
 $(eval $(call SET_NXP_MAKE_FLAG,SNVS_NEEDED,BL2))
 SECURE_BOOT	:= yes
diff --git a/plat/nxp/soc-ls1046a/soc.c b/plat/nxp/soc-ls1046a/soc.c
index d17e672..6dfea89 100644
--- a/plat/nxp/soc-ls1046a/soc.c
+++ b/plat/nxp/soc-ls1046a/soc.c
@@ -21,9 +21,7 @@
 #ifdef POLICY_FUSE_PROVISION
 #include <nxp_gpio.h>
 #endif
-#if TRUSTED_BOARD_BOOT
 #include <nxp_smmu.h>
-#endif
 #include <nxp_timer.h>
 #include <plat_console.h>
 #include <plat_gic.h>
@@ -168,6 +166,12 @@
 	get_cluster_info(soc_list, ARRAY_SIZE(soc_list), &num_clusters, &cores_per_cluster);
 	plat_ls_interconnect_enter_coherency(num_clusters);
 
+	/*
+	 * Unlock write access for SMMU SMMU_CBn_ACTLR in all Non-secure contexts.
+	 */
+	smmu_cache_unlock(NXP_SMMU_ADDR);
+	INFO("SMMU Cache Unlocking is Configured.\n");
+
 #if TRUSTED_BOARD_BOOT
 	uint32_t mode;
 
diff --git a/plat/nxp/soc-ls1046a/soc.def b/plat/nxp/soc-ls1046a/soc.def
index e6b37c0..50fc9c9 100644
--- a/plat/nxp/soc-ls1046a/soc.def
+++ b/plat/nxp/soc-ls1046a/soc.def
@@ -41,11 +41,11 @@
 DDRPHY		:=	NXP
 
 # Area of OCRAM reserved by ROM code
-NXP_ROM_RSVD	:= 0x5900
+NXP_ROM_RSVD	:= 0x8000
 
 # Max Size of CSF header. Required to define BL2 TEXT LIMIT in soc.def
 # Input to CST create_hdr_esbc tool
-CSF_HDR_SZ	:= 0x3000
+CSF_HDR_SZ	:= 0x4000
 
 # In IMAGE_BL2, compile time flag for handling Cache coherency
 # with CAAM for BL2 running from OCRAM
diff --git a/plat/nxp/soc-ls1046a/soc.mk b/plat/nxp/soc-ls1046a/soc.mk
index 8207dcd..7644027 100644
--- a/plat/nxp/soc-ls1046a/soc.mk
+++ b/plat/nxp/soc-ls1046a/soc.mk
@@ -19,8 +19,8 @@
 
 # For Security Features
 DISABLE_FUSE_WRITE	:= 1
-ifeq (${TRUSTED_BOARD_BOOT}, 1)
 $(eval $(call SET_NXP_MAKE_FLAG,SMMU_NEEDED,BL2))
+ifeq (${TRUSTED_BOARD_BOOT}, 1)
 $(eval $(call SET_NXP_MAKE_FLAG,SFP_NEEDED,BL2))
 $(eval $(call SET_NXP_MAKE_FLAG,SNVS_NEEDED,BL2))
 SECURE_BOOT	:= yes
diff --git a/plat/nxp/soc-ls1088a/ls1088aqds/ddr_init.c b/plat/nxp/soc-ls1088a/ls1088aqds/ddr_init.c
index b7397ba..705463b 100644
--- a/plat/nxp/soc-ls1088a/ls1088aqds/ddr_init.c
+++ b/plat/nxp/soc-ls1088a/ls1088aqds/ddr_init.c
@@ -78,7 +78,5 @@
 		ERROR("DDR init failed.\n");
 	}
 
-	erratum_a008850_post();
-
 	return dram_size;
 }
diff --git a/plat/nxp/soc-ls1088a/ls1088ardb/ddr_init.c b/plat/nxp/soc-ls1088a/ls1088ardb/ddr_init.c
index c88583f..107871a 100644
--- a/plat/nxp/soc-ls1088a/ls1088ardb/ddr_init.c
+++ b/plat/nxp/soc-ls1088a/ls1088ardb/ddr_init.c
@@ -80,7 +80,5 @@
 		ERROR("DDR init failed.\n");
 	}
 
-	erratum_a008850_post();
-
 	return dram_size;
 }
diff --git a/plat/nxp/soc-ls1088a/soc.c b/plat/nxp/soc-ls1088a/soc.c
index 5f9f313..02d62ea 100644
--- a/plat/nxp/soc-ls1088a/soc.c
+++ b/plat/nxp/soc-ls1088a/soc.c
@@ -17,9 +17,7 @@
 #include <lib/mmio.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
 #include <ls_interconnect.h>
-#if TRUSTED_BOARD_BOOT
 #include <nxp_smmu.h>
-#endif
 #include <nxp_timer.h>
 #include <plat_console.h>
 #include <plat_gic.h>
@@ -254,6 +252,12 @@
 				MT_DEVICE | MT_RW | MT_NS);
 	}
 
+	/*
+	 * Unlock write access for SMMU SMMU_CBn_ACTLR in all Non-secure contexts.
+	 */
+	smmu_cache_unlock(NXP_SMMU_ADDR);
+	INFO("SMMU Cache Unlocking is Configured.\n");
+
 #if TRUSTED_BOARD_BOOT
 	uint32_t mode;
 
diff --git a/plat/nxp/soc-ls1088a/soc.def b/plat/nxp/soc-ls1088a/soc.def
index 25d0847..17c59ff 100644
--- a/plat/nxp/soc-ls1088a/soc.def
+++ b/plat/nxp/soc-ls1088a/soc.def
@@ -62,7 +62,6 @@
 BL2_BASE		:=	0x1800a000
 
 # SoC ERRATUM to be enabled
-ERRATA_SOC_A008850	:=	1
 
 # ARM Erratum
 ERRATA_A53_855873	:=	1
diff --git a/plat/nxp/soc-ls1088a/soc.mk b/plat/nxp/soc-ls1088a/soc.mk
index 83ac9d0..6e39461 100644
--- a/plat/nxp/soc-ls1088a/soc.mk
+++ b/plat/nxp/soc-ls1088a/soc.mk
@@ -23,12 +23,12 @@
 
 # For Security Features
 DISABLE_FUSE_WRITE	:= 1
+$(eval $(call SET_NXP_MAKE_FLAG,SMMU_NEEDED,BL2))
 ifeq (${TRUSTED_BOARD_BOOT}, 1)
 ifeq (${GENERATE_COT},1)
 # Save Keys to be used by DDR FIP image
 SAVE_KEYS=1
 endif
-$(eval $(call SET_NXP_MAKE_FLAG,SMMU_NEEDED,BL2))
 $(eval $(call SET_NXP_MAKE_FLAG,SFP_NEEDED,BL2))
 $(eval $(call SET_NXP_MAKE_FLAG,SNVS_NEEDED,BL2))
 # Used by create_pbl tool to
diff --git a/plat/nxp/soc-lx2160a/include/soc.h b/plat/nxp/soc-lx2160a/include/soc.h
index 7cc4a03..b781ff8 100644
--- a/plat/nxp/soc-lx2160a/include/soc.h
+++ b/plat/nxp/soc-lx2160a/include/soc.h
@@ -54,8 +54,31 @@
 
 /* SVR Definition (not include major and minor rev) */
 #define SVR_LX2160A		0x873601
+#define SVR_LX2160E		0x873610
+#define SVR_LX2160C		0x873600
+#define SVR_LX2160N		0x873611
 #define SVR_LX2120A		0x873621
+#define SVR_LX2120E		0x873630
+#define SVR_LX2120C		0x873620
+#define SVR_LX2120N		0x873631
 #define SVR_LX2080A		0x873603
+#define SVR_LX2080E		0x873612
+#define SVR_LX2080C		0x873602
+#define SVR_LX2080N		0x873613
+
+/* SVR Definition of SoC LX2162A. */
+#define SVR_LX2162A		0x873609
+#define SVR_LX2162E		0x873618
+#define SVR_LX2162C		0x873608
+#define SVR_LX2162N		0x873619
+#define SVR_LX2122A		0x873629
+#define SVR_LX2122E		0x873638
+#define SVR_LX2122C		0x873628
+#define SVR_LX2122N		0x873639
+#define SVR_LX2082A		0x87360b
+#define SVR_LX2082E		0x87361a
+#define SVR_LX2082C		0x87360a
+#define SVR_LX2082N		0x87361b
 
 /* Number of cores in platform */
 /* Used by common code for array initialization */
diff --git a/plat/nxp/soc-lx2160a/soc.c b/plat/nxp/soc-lx2160a/soc.c
index 2209fda..427189d 100644
--- a/plat/nxp/soc-lx2160a/soc.c
+++ b/plat/nxp/soc-lx2160a/soc.c
@@ -23,9 +23,7 @@
 #ifdef POLICY_FUSE_PROVISION
 #include <nxp_gpio.h>
 #endif
-#if TRUSTED_BOARD_BOOT
 #include <nxp_smmu.h>
-#endif
 #include <nxp_timer.h>
 #include <plat_console.h>
 #include <plat_gic.h>
@@ -37,6 +35,9 @@
 
 #include <errata.h>
 #include <ls_interrupt_mgmt.h>
+#ifdef CONFIG_OCRAM_ECC_EN
+#include <ocram.h>
+#endif
 #include "plat_common.h"
 #ifdef NXP_NV_SW_MAINT_LAST_EXEC_DATA
 #include <plat_nv_storage.h>
@@ -48,9 +49,32 @@
 #include "soc.h"
 
 static struct soc_type soc_list[] =  {
+	/* SoC LX2160A */
 	SOC_ENTRY(LX2160A, LX2160A, 8, 2),
+	SOC_ENTRY(LX2160E, LX2160E, 8, 2),
+	SOC_ENTRY(LX2160C, LX2160C, 8, 2),
+	SOC_ENTRY(LX2160N, LX2160N, 8, 2),
 	SOC_ENTRY(LX2080A, LX2080A, 8, 1),
+	SOC_ENTRY(LX2080E, LX2080E, 8, 1),
+	SOC_ENTRY(LX2080C, LX2080C, 8, 1),
+	SOC_ENTRY(LX2080N, LX2080N, 8, 1),
 	SOC_ENTRY(LX2120A, LX2120A, 6, 2),
+	SOC_ENTRY(LX2120E, LX2120E, 6, 2),
+	SOC_ENTRY(LX2120C, LX2120C, 6, 2),
+	SOC_ENTRY(LX2120N, LX2120N, 6, 2),
+	/* SoC LX2162A */
+	SOC_ENTRY(LX2162A, LX2162A, 8, 2),
+	SOC_ENTRY(LX2162E, LX2162E, 8, 2),
+	SOC_ENTRY(LX2162C, LX2162C, 8, 2),
+	SOC_ENTRY(LX2162N, LX2162N, 8, 2),
+	SOC_ENTRY(LX2082A, LX2082A, 8, 1),
+	SOC_ENTRY(LX2082E, LX2082E, 8, 1),
+	SOC_ENTRY(LX2082C, LX2082C, 8, 1),
+	SOC_ENTRY(LX2082N, LX2082N, 8, 1),
+	SOC_ENTRY(LX2122A, LX2122A, 6, 2),
+	SOC_ENTRY(LX2122E, LX2122E, 6, 2),
+	SOC_ENTRY(LX2122C, LX2122C, 6, 2),
+	SOC_ENTRY(LX2122N, LX2122N, 6, 2),
 };
 
 static dcfg_init_info_t dcfg_init_data = {
@@ -215,6 +239,9 @@
  ******************************************************************************/
 void soc_early_init(void)
 {
+#ifdef CONFIG_OCRAM_ECC_EN
+	ocram_init(NXP_OCRAM_ADDR, NXP_OCRAM_SIZE);
+#endif
 	dcfg_init(&dcfg_init_data);
 #ifdef POLICY_FUSE_PROVISION
 	gpio_init(&gpio_init_data);
@@ -257,6 +284,12 @@
 	sfp_init(NXP_SFP_ADDR);
 #endif
 
+	/*
+	 * Unlock write access for SMMU SMMU_CBn_ACTLR in all Non-secure contexts.
+	 */
+	smmu_cache_unlock(NXP_SMMU_ADDR);
+	INFO("SMMU Cache Unlocking is Configured.\n");
+
 #if TRUSTED_BOARD_BOOT
 	uint32_t mode;
 
@@ -451,8 +484,8 @@
 
 	/* low-level init of the soc */
 	soc_init_start();
-	soc_init_percpu();
 	_init_global_data();
+	soc_init_percpu();
 	_initialize_psci();
 
 	if (ccn_get_part0_id(NXP_CCN_ADDR) != CCN_508_PART0_ID) {
diff --git a/plat/nxp/soc-lx2160a/soc.def b/plat/nxp/soc-lx2160a/soc.def
index 81d6744..0442962 100644
--- a/plat/nxp/soc-lx2160a/soc.def
+++ b/plat/nxp/soc-lx2160a/soc.def
@@ -114,3 +114,6 @@
 
 # enable dynamic memory mapping
 PLAT_XLAT_TABLES_DYNAMIC :=	1
+
+# OCRAM ECC Enabled
+OCRAM_ECC_EN            :=      yes
diff --git a/plat/nxp/soc-lx2160a/soc.mk b/plat/nxp/soc-lx2160a/soc.mk
index 75a3af2..239442c 100644
--- a/plat/nxp/soc-lx2160a/soc.mk
+++ b/plat/nxp/soc-lx2160a/soc.mk
@@ -36,12 +36,12 @@
 
  # For Security Features
 DISABLE_FUSE_WRITE	:= 1
+$(eval $(call SET_NXP_MAKE_FLAG,SMMU_NEEDED,BL2))
 ifeq (${TRUSTED_BOARD_BOOT}, 1)
 ifeq (${GENERATE_COT},1)
 # Save Keys to be used by DDR FIP image
 SAVE_KEYS=1
 endif
-$(eval $(call SET_NXP_MAKE_FLAG,SMMU_NEEDED,BL2))
 $(eval $(call SET_NXP_MAKE_FLAG,SFP_NEEDED,BL2))
 $(eval $(call SET_NXP_MAKE_FLAG,SNVS_NEEDED,BL2))
 # Used by create_pbl tool to
diff --git a/plat/qemu/common/qemu_bl2_mem_params_desc.c b/plat/qemu/common/qemu_bl2_mem_params_desc.c
index 5af3a22..bb1797d 100644
--- a/plat/qemu/common/qemu_bl2_mem_params_desc.c
+++ b/plat/qemu/common/qemu_bl2_mem_params_desc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2021, 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,18 @@
 #include <common/desc_image_load.h>
 #include <plat/common/platform.h>
 
+#define SP_PKG_ENTRY(id) \
+	{ \
+		.image_id = (id), \
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY, VERSION_2, \
+				      entry_point_info_t, \
+				      SECURE | NON_EXECUTABLE), \
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY, \
+				      VERSION_2, image_info_t, \
+				      IMAGE_ATTRIB_SKIP_LOADING), \
+		.next_handoff_image_id = INVALID_IMAGE_ID, \
+	}
+
 /*******************************************************************************
  * Following descriptor provides BL image/ep information that gets used
  * by BL2 to load the images and also subset of this information is
@@ -122,6 +134,48 @@
 #endif
 	   .next_handoff_image_id = INVALID_IMAGE_ID,
 	},
+
+#if defined(SPD_spmd)
+	/* Fill TOS_FW_CONFIG related information */
+	{
+	    .image_id = TOS_FW_CONFIG_ID,
+	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+		    VERSION_2, image_info_t, 0),
+	    .image_info.image_base = TOS_FW_CONFIG_BASE,
+	    .image_info.image_max_size = TOS_FW_CONFIG_LIMIT -
+					 TOS_FW_CONFIG_BASE,
+	    .next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+
+#if SPMD_SPM_AT_SEL2
+	/* Fill TB_FW_CONFIG related information */
+	{
+	    .image_id = TB_FW_CONFIG_ID,
+	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+	    SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+		    VERSION_2, image_info_t, 0),
+	    .image_info.image_base = TB_FW_CONFIG_BASE,
+	    .image_info.image_max_size = TB_FW_CONFIG_LIMIT - TB_FW_CONFIG_BASE,
+	    .next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+
+	/*
+	 * Empty entries for SP packages to be filled in according to
+	 * TB_FW_CONFIG.
+	 */
+	SP_PKG_ENTRY(SP_PKG1_ID),
+	SP_PKG_ENTRY(SP_PKG2_ID),
+	SP_PKG_ENTRY(SP_PKG3_ID),
+	SP_PKG_ENTRY(SP_PKG4_ID),
+	SP_PKG_ENTRY(SP_PKG5_ID),
+	SP_PKG_ENTRY(SP_PKG6_ID),
+	SP_PKG_ENTRY(SP_PKG7_ID),
+	SP_PKG_ENTRY(SP_PKG8_ID),
+#endif
+#endif
 # endif /* QEMU_LOAD_BL32 */
 
 	/* Fill BL33 related information */
diff --git a/plat/qemu/common/qemu_bl2_setup.c b/plat/qemu/common/qemu_bl2_setup.c
index 2c0da15..be55877 100644
--- a/plat/qemu/common/qemu_bl2_setup.c
+++ b/plat/qemu/common/qemu_bl2_setup.c
@@ -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
  */
@@ -16,6 +16,7 @@
 #include <common/debug.h>
 #include <common/desc_image_load.h>
 #include <common/fdt_fixup.h>
+#include <common/fdt_wrappers.h>
 #include <lib/optee_utils.h>
 #include <lib/utils.h>
 #include <plat/common/platform.h>
@@ -140,6 +141,48 @@
 	return spsr;
 }
 
+#if defined(SPD_spmd) && SPMD_SPM_AT_SEL2
+static int load_sps_from_tb_fw_config(struct image_info *image_info)
+{
+	void *dtb = (void *)image_info->image_base;
+	const char *compat_str = "arm,sp";
+	const struct fdt_property *uuid;
+	uint32_t load_addr;
+	const char *name;
+	int sp_node;
+	int node;
+
+	node = fdt_node_offset_by_compatible(dtb, -1, compat_str);
+	if (node < 0) {
+		ERROR("Can't find %s in TB_FW_CONFIG", compat_str);
+		return -1;
+	}
+
+	fdt_for_each_subnode(sp_node, dtb, node) {
+		name = fdt_get_name(dtb, sp_node, NULL);
+		if (name == NULL) {
+			ERROR("Can't get name of node in dtb\n");
+			return -1;
+		}
+		uuid = fdt_get_property(dtb, sp_node, "uuid", NULL);
+		if (uuid == NULL) {
+			ERROR("Can't find property uuid in node %s", name);
+			return -1;
+		}
+		if (fdt_read_uint32(dtb, sp_node, "load-address",
+				    &load_addr) < 0) {
+			ERROR("Can't read load-address in node %s", name);
+			return -1;
+		}
+		if (qemu_io_register_sp_pkg(name, uuid->data, load_addr) < 0) {
+			return -1;
+		}
+	}
+
+	return 0;
+}
+#endif /*defined(SPD_spmd) && SPMD_SPM_AT_SEL2*/
+
 static int qemu_bl2_handle_post_image_load(unsigned int image_id)
 {
 	int err = 0;
@@ -149,8 +192,7 @@
 	bl_mem_params_node_t *paged_mem_params = NULL;
 #endif
 #if defined(SPD_spmd)
-	unsigned int mode_rw = MODE_RW_64;
-	uint64_t pagable_part = 0;
+	bl_mem_params_node_t *bl32_mem_params = NULL;
 #endif
 
 	assert(bl_mem_params);
@@ -170,17 +212,18 @@
 		if (err != 0) {
 			WARN("OPTEE header parse error.\n");
 		}
-#if defined(SPD_spmd)
-		mode_rw = bl_mem_params->ep_info.args.arg0;
-		pagable_part = bl_mem_params->ep_info.args.arg1;
-#endif
 #endif
 
-#if defined(SPD_spmd)
-		bl_mem_params->ep_info.args.arg0 = ARM_PRELOADED_DTB_BASE;
-		bl_mem_params->ep_info.args.arg1 = pagable_part;
-		bl_mem_params->ep_info.args.arg2 = mode_rw;
-		bl_mem_params->ep_info.args.arg3 = 0;
+#if defined(SPMC_OPTEE)
+		/*
+		 * Explicit zeroes to unused registers since they may have
+		 * been populated by parse_optee_header() above.
+		 *
+		 * OP-TEE expects system DTB in x2 and TOS_FW_CONFIG in x0,
+		 * the latter is filled in below for TOS_FW_CONFIG_ID and
+		 * applies to any other SPMC too.
+		 */
+		bl_mem_params->ep_info.args.arg2 = ARM_PRELOADED_DTB_BASE;
 #elif defined(SPD_opteed)
 		/*
 		 * OP-TEE expect to receive DTB address in x2.
@@ -224,6 +267,19 @@
 
 		bl_mem_params->ep_info.spsr = qemu_get_spsr_for_bl33_entry();
 		break;
+#ifdef SPD_spmd
+#if SPMD_SPM_AT_SEL2
+	case TB_FW_CONFIG_ID:
+		err = load_sps_from_tb_fw_config(&bl_mem_params->image_info);
+		break;
+#endif
+	case TOS_FW_CONFIG_ID:
+		/* An SPMC expects TOS_FW_CONFIG in x0/r0 */
+		bl32_mem_params = get_bl_mem_params_node(BL32_IMAGE_ID);
+		bl32_mem_params->ep_info.args.arg0 =
+					bl_mem_params->image_info.image_base;
+		break;
+#endif
 	default:
 		/* Do nothing in default case */
 		break;
diff --git a/plat/qemu/common/qemu_common.c b/plat/qemu/common/qemu_common.c
index 0c184f4..23ac581 100644
--- a/plat/qemu/common/qemu_common.c
+++ b/plat/qemu/common/qemu_common.c
@@ -10,6 +10,7 @@
 #include <arch_helpers.h>
 #include <common/bl_common.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
+#include <services/el3_spmc_ffa_memory.h>
 
 #include <plat/common/platform.h>
 #include "qemu_private.h"
@@ -100,7 +101,7 @@
 #if SPM_MM
 	MAP_NS_DRAM0,
 	QEMU_SPM_BUF_EL3_MMAP,
-#else
+#elif !SPMC_AT_EL3
 	MAP_BL32_MEM,
 #endif
 	{0}
@@ -167,3 +168,30 @@
 	return get_mbedtls_heap_helper(heap_addr, heap_size);
 }
 #endif
+
+#if SPMC_AT_EL3
+/*
+ * When using the EL3 SPMC implementation allocate the datastore
+ * for tracking shared memory descriptors in normal memory.
+ */
+#define PLAT_SPMC_SHMEM_DATASTORE_SIZE 64 * 1024
+
+uint8_t plat_spmc_shmem_datastore[PLAT_SPMC_SHMEM_DATASTORE_SIZE];
+
+int plat_spmc_shmem_datastore_get(uint8_t **datastore, size_t *size)
+{
+	*datastore = plat_spmc_shmem_datastore;
+	*size = PLAT_SPMC_SHMEM_DATASTORE_SIZE;
+	return 0;
+}
+
+int plat_spmc_shmem_begin(struct ffa_mtd *desc)
+{
+	return 0;
+}
+
+int plat_spmc_shmem_reclaim(struct ffa_mtd *desc)
+{
+	return 0;
+}
+#endif
diff --git a/plat/qemu/common/qemu_io_storage.c b/plat/qemu/common/qemu_io_storage.c
index 1107e44..4c61b14 100644
--- a/plat/qemu/common/qemu_io_storage.c
+++ b/plat/qemu/common/qemu_io_storage.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2016, 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,8 @@
 
 #include <common/bl_common.h>
 #include <common/debug.h>
+#include <common/desc_image_load.h>
+#include <common/uuid.h>
 #include <drivers/io/io_driver.h>
 #include <drivers/io/io_encrypted.h>
 #include <drivers/io/io_fip.h>
@@ -20,10 +22,14 @@
 #include <lib/semihosting.h>
 #include <tools_share/firmware_image_package.h>
 
+#include "qemu_private.h"
+
 /* Semihosting filenames */
 #define BL2_IMAGE_NAME			"bl2.bin"
 #define BL31_IMAGE_NAME			"bl31.bin"
 #define BL32_IMAGE_NAME			"bl32.bin"
+#define TB_FW_CONFIG_NAME		"tb_fw_config.dtb"
+#define TOS_FW_CONFIG_NAME		"tos_fw_config.dtb"
 #define BL32_EXTRA1_IMAGE_NAME		"bl32_extra1.bin"
 #define BL32_EXTRA2_IMAGE_NAME		"bl32_extra2.bin"
 #define BL33_IMAGE_NAME			"bl33.bin"
@@ -78,6 +84,14 @@
 	.uuid = UUID_SECURE_PAYLOAD_BL32_EXTRA2,
 };
 
+static const io_uuid_spec_t tb_fw_config_uuid_spec = {
+	.uuid = UUID_TB_FW_CONFIG,
+};
+
+static const io_uuid_spec_t tos_fw_config_uuid_spec = {
+	.uuid = UUID_TOS_FW_CONFIG,
+};
+
 static const io_uuid_spec_t bl33_uuid_spec = {
 	.uuid = UUID_NON_TRUSTED_FIRMWARE_BL33,
 };
@@ -137,6 +151,14 @@
 		.path = BL32_EXTRA2_IMAGE_NAME,
 		.mode = FOPEN_MODE_RB
 	},
+	[TB_FW_CONFIG_ID] = {
+		.path = TB_FW_CONFIG_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[TOS_FW_CONFIG_ID] = {
+		.path = TOS_FW_CONFIG_NAME,
+		.mode = FOPEN_MODE_RB
+	},
 	[BL33_IMAGE_ID] = {
 		.path = BL33_IMAGE_NAME,
 		.mode = FOPEN_MODE_RB
@@ -252,6 +274,16 @@
 		open_fip
 	},
 #endif
+	[TB_FW_CONFIG_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&tb_fw_config_uuid_spec,
+		open_fip
+	},
+	[TOS_FW_CONFIG_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&tos_fw_config_uuid_spec,
+		open_fip
+	},
 	[BL33_IMAGE_ID] = {
 		&fip_dev_handle,
 		(uintptr_t)&bl33_uuid_spec,
@@ -301,6 +333,80 @@
 #endif /* TRUSTED_BOARD_BOOT */
 };
 
+#if defined(SPD_spmd)
+static struct sp_pkg {
+	struct plat_io_policy policy;
+	io_file_spec_t sh_file_spec;
+	uint8_t uuid[UUID_BYTES_LENGTH];
+	char path[80];
+} sp_pkgs[MAX_SP_IDS];
+static unsigned int sp_pkg_count;
+
+int qemu_io_register_sp_pkg(const char *name, const char *uuid,
+			    uintptr_t load_addr)
+{
+	struct sp_pkg *pkg;
+	bl_mem_params_node_t *mem_params;
+
+	if (sp_pkg_count == MAX_SP_IDS) {
+		INFO("Reached Max number of SPs\n");
+		return -1;
+	}
+	mem_params = get_bl_mem_params_node(SP_PKG1_ID + sp_pkg_count);
+	if (mem_params == NULL) {
+		ERROR("Can't find SP_PKG ID %u (SP_PKG%u_ID)\n",
+		      SP_PKG1_ID + sp_pkg_count, sp_pkg_count);
+		return -1;
+	}
+	pkg = sp_pkgs + sp_pkg_count;
+
+	if (read_uuid(pkg->uuid, (char *)uuid)) {
+		return -1;
+	}
+
+	strlcpy(pkg->path, name, sizeof(pkg->path));
+	strlcat(pkg->path, ".pkg", sizeof(pkg->path));
+
+	pkg->policy.dev_handle = &fip_dev_handle;
+	pkg->policy.image_spec = (uintptr_t)&pkg->uuid;
+	pkg->policy.check = open_fip;
+	pkg->sh_file_spec.path = pkg->path;
+	pkg->sh_file_spec.mode = FOPEN_MODE_RB;
+
+	mem_params->image_info.image_base = load_addr;
+	mem_params->image_info.image_max_size = SZ_4M;
+	mem_params->image_info.h.attr &= ~IMAGE_ATTRIB_SKIP_LOADING;
+
+	sp_pkg_count++;
+
+	return 0;
+}
+#endif /*SPD_spmd*/
+
+static const io_file_spec_t *get_io_file_spec(unsigned int image_id)
+{
+#if defined(SPD_spmd)
+	if (image_id >= SP_PKG1_ID && image_id <= SP_PKG8_ID) {
+		return &sp_pkgs[image_id - SP_PKG1_ID].sh_file_spec;
+	}
+#endif
+
+	assert(image_id < ARRAY_SIZE(sh_file_spec));
+	return &sh_file_spec[image_id];
+}
+
+static const struct plat_io_policy *get_io_policy(unsigned int image_id)
+{
+#if defined(SPD_spmd)
+	if (image_id >= SP_PKG1_ID && image_id <= SP_PKG8_ID) {
+		return &sp_pkgs[image_id - SP_PKG1_ID].policy;
+	}
+#endif
+
+	assert(image_id < ARRAY_SIZE(policies));
+	return &policies[image_id];
+}
+
 static int open_fip(const uintptr_t spec)
 {
 	int result;
@@ -413,11 +519,13 @@
 static int get_alt_image_source(unsigned int image_id, uintptr_t *dev_handle,
 				  uintptr_t *image_spec)
 {
-	int result = open_semihosting((const uintptr_t)&sh_file_spec[image_id]);
+	const io_file_spec_t *spec = get_io_file_spec(image_id);
+	int result;
 
+	result = open_semihosting((const uintptr_t)spec);
 	if (result == 0) {
 		*dev_handle = sh_dev_handle;
-		*image_spec = (uintptr_t)&sh_file_spec[image_id];
+		*image_spec = (uintptr_t)spec;
 	}
 
 	return result;
@@ -430,12 +538,9 @@
 int plat_get_image_source(unsigned int image_id, uintptr_t *dev_handle,
 			  uintptr_t *image_spec)
 {
+	const struct plat_io_policy *policy = get_io_policy(image_id);
 	int result;
-	const struct plat_io_policy *policy;
-
-	assert(image_id < ARRAY_SIZE(policies));
 
-	policy = &policies[image_id];
 	result = policy->check(policy->image_spec);
 	if (result == 0) {
 		*image_spec = policy->image_spec;
diff --git a/plat/qemu/common/qemu_private.h b/plat/qemu/common/qemu_private.h
index c313cb6..159c44f 100644
--- a/plat/qemu/common/qemu_private.h
+++ b/plat/qemu/common/qemu_private.h
@@ -26,6 +26,8 @@
 			unsigned long coh_start, unsigned long coh_limit);
 
 void plat_qemu_io_setup(void);
+int qemu_io_register_sp_pkg(const char *name, const char *uuid,
+			    uintptr_t load_addr);
 unsigned int plat_qemu_calc_core_pos(u_register_t mpidr);
 
 void qemu_console_init(void);
diff --git a/plat/qemu/common/qemu_spmd_manifest.c b/plat/qemu/common/qemu_spmd_manifest.c
deleted file mode 100644
index fd46e26..0000000
--- a/plat/qemu/common/qemu_spmd_manifest.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <assert.h>
-
-#include <services/spm_core_manifest.h>
-
-#include <plat/common/platform.h>
-#include <platform_def.h>
-
-int plat_spm_core_manifest_load(spmc_manifest_attribute_t *manifest,
-				const void *pm_addr)
-{
-	entry_point_info_t *ep_info = bl31_plat_get_next_image_ep_info(SECURE);
-
-	assert(ep_info != NULL);
-	assert(manifest != NULL);
-
-	manifest->major_version = 1;
-	manifest->minor_version = 0;
-	manifest->exec_state = ep_info->args.arg2;
-	manifest->load_address = BL32_BASE;
-	manifest->entrypoint = BL32_BASE;
-	manifest->binary_size = BL32_LIMIT - BL32_BASE;
-	manifest->spmc_id = 0x8000;
-
-	return 0;
-}
diff --git a/plat/qemu/qemu/include/platform_def.h b/plat/qemu/qemu/include/platform_def.h
index a22fbe5..803f8e2 100644
--- a/plat/qemu/qemu/include/platform_def.h
+++ b/plat/qemu/qemu/include/platform_def.h
@@ -118,6 +118,11 @@
 #define BL_RAM_BASE			(SHARED_RAM_BASE + SHARED_RAM_SIZE)
 #define BL_RAM_SIZE			(SEC_SRAM_SIZE - SHARED_RAM_SIZE)
 
+#define TB_FW_CONFIG_BASE		BL_RAM_BASE
+#define TB_FW_CONFIG_LIMIT		(TB_FW_CONFIG_BASE + PAGE_SIZE)
+#define TOS_FW_CONFIG_BASE		TB_FW_CONFIG_LIMIT
+#define TOS_FW_CONFIG_LIMIT		(TOS_FW_CONFIG_BASE + PAGE_SIZE)
+
 /*
  * BL1 specific defines.
  *
@@ -183,8 +188,8 @@
 
 #define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 32)
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 32)
-#define MAX_MMAP_REGIONS		11
-#define MAX_XLAT_TABLES			6
+#define MAX_MMAP_REGIONS		(11 + MAX_MMAP_REGIONS_SPMC)
+#define MAX_XLAT_TABLES			(6 + MAX_XLAT_TABLES_SPMC)
 #define MAX_IO_DEVICES			4
 #define MAX_IO_HANDLES			4
 
@@ -275,4 +280,32 @@
  */
 #define	PLAT_EVENT_LOG_MAX_SIZE		UL(0x400)
 
+#if SPMC_AT_EL3
+/*
+ * Number of Secure Partitions supported.
+ * SPMC at EL3, uses this count to configure the maximum number of
+ * supported secure partitions.
+ */
+#define SECURE_PARTITION_COUNT		1
+
+/*
+ * Number of Logical Partitions supported.
+ * SPMC at EL3, uses this count to configure the maximum number of
+ * supported logical partitions.
+ */
+#define MAX_EL3_LP_DESCS_COUNT		0
+
+/*
+ * Number of Normal World Partitions supported.
+ * SPMC at EL3, uses this count to configure the maximum number of
+ * supported normal world partitions.
+ */
+#define NS_PARTITION_COUNT		1
+
+#define MAX_MMAP_REGIONS_SPMC		2
+#define MAX_XLAT_TABLES_SPMC		4
+#else
+#define MAX_MMAP_REGIONS_SPMC		0
+#define MAX_XLAT_TABLES_SPMC		0
+#endif
 #endif /* PLATFORM_DEF_H */
diff --git a/plat/qemu/qemu/platform.mk b/plat/qemu/qemu/platform.mk
index 6becc32..4cbce9d 100644
--- a/plat/qemu/qemu/platform.mk
+++ b/plat/qemu/qemu/platform.mk
@@ -109,7 +109,6 @@
     endif
 
     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}
 
@@ -162,7 +161,8 @@
 				${PLAT_QEMU_COMMON_PATH}/qemu_image_load.c		\
 				common/fdt_fixup.c					\
 				common/fdt_wrappers.c					\
-				common/desc_image_load.c
+				common/desc_image_load.c				\
+				common/uuid.c
 
 ifeq ($(add-lib-optee),yes)
 BL2_SOURCES		+=	lib/optee/optee_utils.c
@@ -211,8 +211,17 @@
 				${PLAT_QEMU_COMMON_PATH}/qemu_bl31_setup.c		\
 				${QEMU_GIC_SOURCES}
 
+# Pointer Authentication sources
+ifeq (${ENABLE_PAUTH}, 1)
+PLAT_BL_COMMON_SOURCES	+=	plat/arm/common/aarch64/arm_pauth.c	\
+				lib/extensions/pauth/pauth_helpers.S
+endif
+
 ifeq (${SPD},spmd)
-BL31_SOURCES		+=	plat/qemu/common/qemu_spmd_manifest.c
+BL31_SOURCES		+=	plat/common/plat_spmd_manifest.c	\
+				common/uuid.c				\
+				${LIBFDT_SRCS} 				\
+				${FDT_WRAPPERS_SOURCES}
 endif
 endif
 
@@ -231,6 +240,20 @@
 else
 $(eval $(call TOOL_ADD_IMG,bl32_extra2,--tos-fw-extra2))
 endif
+endif
+
+ifneq ($(QEMU_TB_FW_CONFIG_DTS),)
+FDT_SOURCES		+=	${QEMU_TB_FW_CONFIG_DTS}
+QEMU_TB_FW_CONFIG	:=	${BUILD_PLAT}/fdts/$(notdir $(basename ${QEMU_TB_FW_CONFIG_DTS})).dtb
+# Add the TB_FW_CONFIG to FIP
+$(eval $(call TOOL_ADD_PAYLOAD,${QEMU_TB_FW_CONFIG},--tb-fw-config,${QEMU_TB_FW_CONFIG}))
+endif
+
+ifneq ($(QEMU_TOS_FW_CONFIG_DTS),)
+FDT_SOURCES		+=	${QEMU_TOS_FW_CONFIG_DTS}
+QEMU_TOS_FW_CONFIG	:=	${BUILD_PLAT}/fdts/$(notdir $(basename ${QEMU_TOS_FW_CONFIG_DTS})).dtb
+# Add the TOS_FW_CONFIG to FIP
+$(eval $(call TOOL_ADD_PAYLOAD,${QEMU_TOS_FW_CONFIG},--tos-fw-config,${QEMU_TOS_FW_CONFIG}))
 endif
 
 SEPARATE_CODE_AND_RODATA := 1
diff --git a/plat/qemu/qemu/qemu_common_measured_boot.c b/plat/qemu/qemu/qemu_common_measured_boot.c
deleted file mode 100644
index 41f7f87..0000000
--- a/plat/qemu/qemu/qemu_common_measured_boot.c
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * 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_measured_boot.c b/plat/qemu/qemu/qemu_measured_boot.c
index d9e475a..122bb23 100644
--- a/plat/qemu/qemu/qemu_measured_boot.c
+++ b/plat/qemu/qemu/qemu_measured_boot.c
@@ -9,6 +9,7 @@
 
 #include <drivers/measured_boot/event_log/event_log.h>
 #include <plat/common/common_def.h>
+#include <plat/common/platform.h>
 #include <tools_share/tbbr_oid.h>
 
 #include "../common/qemu_private.h"
@@ -17,8 +18,8 @@
 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[] = {
+/* QEMU table with platform specific image IDs, names and PCRs */
+static 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 },
@@ -101,3 +102,19 @@
 
 	dump_event_log((uint8_t *)event_log_base, event_log_cur_size);
 }
+
+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,
+					       qemu_event_log_metadata);
+	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_sbsa/platform.mk b/plat/qemu/qemu_sbsa/platform.mk
index 5a6b1e1..2393b39 100644
--- a/plat/qemu/qemu_sbsa/platform.mk
+++ b/plat/qemu/qemu_sbsa/platform.mk
@@ -123,5 +123,6 @@
 ARM_PRELOADED_DTB_BASE := PLAT_QEMU_DT_BASE
 $(eval $(call add_define,ARM_PRELOADED_DTB_BASE))
 
-# Do not enable SVE
-ENABLE_SVE_FOR_NS	:= 0
+# Later QEMU versions support SME and SVE.
+ENABLE_SVE_FOR_NS	:= 1
+ENABLE_SME_FOR_NS	:= 1
diff --git a/plat/st/common/bl2_io_storage.c b/plat/st/common/bl2_io_storage.c
index b271ed6..1bbaff6 100644
--- a/plat/st/common/bl2_io_storage.c
+++ b/plat/st/common/bl2_io_storage.c
@@ -607,7 +607,7 @@
  *     - we already boot FWU_MAX_TRIAL_REBOOT times in trial mode.
  * we select the previous_active_index.
  */
-#define INVALID_BOOT_IDX		0xFFFFFFFF
+#define INVALID_BOOT_IDX		0xFFFFFFFFU
 
 uint32_t plat_fwu_get_boot_idx(void)
 {
diff --git a/plat/st/common/include/stm32mp_efi.h b/plat/st/common/include/stm32mp_efi.h
index 490560f..af9165f 100644
--- a/plat/st/common/include/stm32mp_efi.h
+++ b/plat/st/common/include/stm32mp_efi.h
@@ -1,6 +1,7 @@
 /* SPDX-License-Identifier: BSD-3-Clause */
 /*
  * Copyright (c) 2021, Linaro Limited
+ * Copyright (c) 2022, STMicroelectronics - All Rights Reserved
  */
 
 #ifndef STM32MP_EFI_H
@@ -9,7 +10,7 @@
 #include <drivers/partition/efi.h>
 
 #define STM32MP_FIP_GUID \
-	EFI_GUID(0x19d5df83, 0x11b0, 0x457b, \
-		 0xbe, 0x2c, 0x75, 0x59, 0xc1, 0x31, 0x42, 0xa5)
+	EFI_GUID(0x19d5df83U, 0x11b0U, 0x457bU, \
+		 0xbeU, 0x2cU, 0x75U, 0x59U, 0xc1U, 0x31U, 0x42U, 0xa5U)
 
 #endif /* STM32MP_EFI_H */
diff --git a/plat/st/common/stm32mp_crypto_lib.c b/plat/st/common/stm32mp_crypto_lib.c
index d644242..0da0019 100644
--- a/plat/st/common/stm32mp_crypto_lib.c
+++ b/plat/st/common/stm32mp_crypto_lib.c
@@ -16,6 +16,7 @@
 #include <drivers/st/stm32_pka.h>
 #include <drivers/st/stm32_rng.h>
 #include <drivers/st/stm32_saes.h>
+#include <lib/utils.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
 #include <mbedtls/asn1.h>
 #include <mbedtls/md.h>
@@ -78,7 +79,7 @@
 	}
 }
 
-int get_plain_pk_from_asn1(void *pk_ptr, unsigned int pk_len, void **plain_pk,
+static int get_plain_pk_from_asn1(void *pk_ptr, unsigned int pk_len, void **plain_pk,
 			   unsigned int *len, int *pk_alg)
 {
 	int ret;
diff --git a/plat/st/common/stm32mp_dt.c b/plat/st/common/stm32mp_dt.c
index c9efeb5..34d52e1 100644
--- a/plat/st/common/stm32mp_dt.c
+++ b/plat/st/common/stm32mp_dt.c
@@ -79,11 +79,8 @@
 	}
 
 	cchar = fdt_getprop(fdt, node, "secure-status", NULL);
-	if (cchar == NULL) {
-		if (status == DT_NON_SECURE) {
-			status |= DT_SECURE;
-		}
-	} else if (strncmp(cchar, "okay", strlen("okay")) == 0) {
+	if (((cchar == NULL) && (status == DT_NON_SECURE)) ||
+	    ((cchar != NULL) && (strncmp(cchar, "okay", strlen("okay")) == 0))) {
 		status |= DT_SECURE;
 	}
 
@@ -350,7 +347,7 @@
 		return -FDT_ERR_BADVALUE;
 	}
 
-	if (fdt32_to_cpu(*cuint) % sizeof(uint32_t)) {
+	if ((fdt32_to_cpu(*cuint) % sizeof(uint32_t)) != 0U) {
 		ERROR("Misaligned nvmem %s element: ignored\n", name);
 		return -FDT_ERR_BADVALUE;
 	}
@@ -386,7 +383,7 @@
 
 	fdt_for_each_subnode(node, fdt, pinctrl_node) {
 		const fdt32_t *cuint;
-		int pin_count;
+		int pin_count = 0;
 		int len;
 		int i;
 
@@ -415,11 +412,9 @@
 		}
 
 		/* Get the last defined gpio line (offset + nb of pins) */
-		pin_count = fdt32_to_cpu(*(cuint + 1)) + fdt32_to_cpu(*(cuint + 3));
-		for (i = 0; i < len / 4; i++) {
-			pin_count = MAX(pin_count, (int)(fdt32_to_cpu(*(cuint + 1)) +
-							 fdt32_to_cpu(*(cuint + 3))));
-			cuint += 4;
+		for (i = 0; i < len; i += 4) {
+			pin_count = MAX(pin_count, (int)(fdt32_to_cpu(cuint[i + 1]) +
+							 fdt32_to_cpu(cuint[i + 3])));
 		}
 
 		return pin_count;
diff --git a/plat/st/common/stm32mp_fconf_io.c b/plat/st/common/stm32mp_fconf_io.c
index 0b6cc78..1a59f0b 100644
--- a/plat/st/common/stm32mp_fconf_io.c
+++ b/plat/st/common/stm32mp_fconf_io.c
@@ -28,7 +28,7 @@
 #endif
 
 #if (STM32MP_SDMMC || STM32MP_EMMC) && PSA_FWU_SUPPORT
-io_block_spec_t metadata_block_spec = {
+static io_block_spec_t metadata_block_spec = {
 	.offset = 0,    /* To be filled at runtime */
 	.length = 0,    /* To be filled at runtime */
 };
diff --git a/plat/st/stm32mp1/bl2_plat_setup.c b/plat/st/stm32mp1/bl2_plat_setup.c
index 4f04a6f..87d2d39 100644
--- a/plat/st/stm32mp1/bl2_plat_setup.c
+++ b/plat/st/stm32mp1/bl2_plat_setup.c
@@ -543,20 +543,15 @@
 
 void bl2_el3_plat_prepare_exit(void)
 {
+#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
 	uint16_t boot_itf = stm32mp_get_boot_itf_selected();
 
-	switch (boot_itf) {
-#if STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER
-	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART:
-	case BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB:
+	if ((boot_itf == BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART) ||
+	    (boot_itf == BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB)) {
 		/* Invalidate the downloaded buffer used with io_memmap */
 		inv_dcache_range(DWL_BUFFER_BASE, DWL_BUFFER_SIZE);
-		break;
-#endif /* STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER */
-	default:
-		/* Do nothing in default case */
-		break;
 	}
+#endif /* STM32MP_UART_PROGRAMMER || STM32MP_USB_PROGRAMMER */
 
 	stm32mp1_security_setup();
 }
diff --git a/plat/st/stm32mp1/plat_image_load.c b/plat/st/stm32mp1/plat_image_load.c
index c4048fc..c455544 100644
--- a/plat/st/stm32mp1/plat_image_load.c
+++ b/plat/st/stm32mp1/plat_image_load.c
@@ -5,6 +5,7 @@
  */
 
 #include <common/desc_image_load.h>
+#include <plat/common/platform.h>
 
 /*******************************************************************************
  * This function flushes the data structures so that they are visible
diff --git a/plat/st/stm32mp1/stm32mp1_fconf_firewall.c b/plat/st/stm32mp1/stm32mp1_fconf_firewall.c
index f2568ab..7d99564 100644
--- a/plat/st/stm32mp1/stm32mp1_fconf_firewall.c
+++ b/plat/st/stm32mp1/stm32mp1_fconf_firewall.c
@@ -99,15 +99,16 @@
 
 	/* Locate the memory cells and read all values */
 	for (i = 0U; i < (unsigned int)(len / (sizeof(uint32_t) * STM32MP_REGION_PARAMS)); i++) {
+		uint32_t idx = i * STM32MP_REGION_PARAMS;
 		uint32_t base;
 		uint32_t size;
 		uint32_t sec_attr;
 		uint32_t nsaid;
 
-		base = fdt32_to_cpu(conf_list->id_attr[i * STM32MP_REGION_PARAMS]);
-		size = fdt32_to_cpu(conf_list->id_attr[i * STM32MP_REGION_PARAMS + 1]);
-		sec_attr = fdt32_to_cpu(conf_list->id_attr[i * STM32MP_REGION_PARAMS + 2]);
-		nsaid = fdt32_to_cpu(conf_list->id_attr[i * STM32MP_REGION_PARAMS + 3]);
+		base = fdt32_to_cpu(conf_list->id_attr[idx]);
+		size = fdt32_to_cpu(conf_list->id_attr[idx + 1]);
+		sec_attr = fdt32_to_cpu(conf_list->id_attr[idx + 2]);
+		nsaid = fdt32_to_cpu(conf_list->id_attr[idx + 3]);
 
 		VERBOSE("FCONF: stm32mp1-firewall cell found with value = 0x%x 0x%x 0x%x 0x%x\n",
 			base, size, sec_attr, nsaid);
diff --git a/plat/st/stm32mp1/stm32mp1_private.c b/plat/st/stm32mp1/stm32mp1_private.c
index 9bdb075..e6cb071 100644
--- a/plat/st/stm32mp1/stm32mp1_private.c
+++ b/plat/st/stm32mp1/stm32mp1_private.c
@@ -140,14 +140,14 @@
 uintptr_t stm32_get_gpio_bank_base(unsigned int bank)
 {
 #if STM32MP13
-	assert(GPIO_BANK_A == 0 && bank <= GPIO_BANK_I);
+	assert((GPIO_BANK_A == 0) && (bank <= GPIO_BANK_I));
 #endif
 #if STM32MP15
 	if (bank == GPIO_BANK_Z) {
 		return GPIOZ_BASE;
 	}
 
-	assert(GPIO_BANK_A == 0 && bank <= GPIO_BANK_K);
+	assert((GPIO_BANK_A == 0) && (bank <= GPIO_BANK_K));
 #endif
 
 	return GPIOA_BASE + (bank * GPIO_BANK_OFFSET);
@@ -156,14 +156,14 @@
 uint32_t stm32_get_gpio_bank_offset(unsigned int bank)
 {
 #if STM32MP13
-	assert(GPIO_BANK_A == 0 && bank <= GPIO_BANK_I);
+	assert((GPIO_BANK_A == 0) && (bank <= GPIO_BANK_I));
 #endif
 #if STM32MP15
 	if (bank == GPIO_BANK_Z) {
 		return 0;
 	}
 
-	assert(GPIO_BANK_A == 0 && bank <= GPIO_BANK_K);
+	assert((GPIO_BANK_A == 0) && (bank <= GPIO_BANK_K));
 #endif
 
 	return bank * GPIO_BANK_OFFSET;
@@ -186,14 +186,14 @@
 unsigned long stm32_get_gpio_bank_clock(unsigned int bank)
 {
 #if STM32MP13
-	assert(GPIO_BANK_A == 0 && bank <= GPIO_BANK_I);
+	assert((GPIO_BANK_A == 0) && (bank <= GPIO_BANK_I));
 #endif
 #if STM32MP15
 	if (bank == GPIO_BANK_Z) {
 		return GPIOZ;
 	}
 
-	assert(GPIO_BANK_A == 0 && bank <= GPIO_BANK_K);
+	assert((GPIO_BANK_A == 0) && (bank <= GPIO_BANK_K));
 #endif
 
 	return GPIOA + (bank - GPIO_BANK_A);
@@ -378,7 +378,7 @@
 
 void stm32mp_get_soc_name(char name[STM32_SOC_NAME_SIZE])
 {
-	char *cpu_s, *cpu_r, *pkg;
+	const char *cpu_s, *cpu_r, *pkg;
 
 	/* MPUs Part Numbers */
 	switch (get_part_number()) {
diff --git a/plat/st/stm32mp1/stm32mp1_syscfg.c b/plat/st/stm32mp1/stm32mp1_syscfg.c
index ff79428..75dd709 100644
--- a/plat/st/stm32mp1/stm32mp1_syscfg.c
+++ b/plat/st/stm32mp1/stm32mp1_syscfg.c
@@ -235,7 +235,9 @@
 	}
 
 	if (apply_hslv) {
-		mmio_write_32(SYSCFG_BASE + SYSCFG_HSLVEN0R + index * sizeof(uint32_t), HSLV_KEY);
+		uint32_t reg_offset = index * sizeof(uint32_t);
+
+		mmio_write_32(SYSCFG_BASE + SYSCFG_HSLVEN0R + reg_offset, HSLV_KEY);
 	}
 }
 #endif
diff --git a/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c b/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c
index cb6aaa5..8438aba 100644
--- a/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c
+++ b/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c
@@ -21,7 +21,6 @@
 #include <plat_private.h>
 
 #include "ipi_mailbox_svc.h"
-#include "../../../services/spd/trusty/smcall.h"
 
 /*********************************************************************
  * Macros definitions
@@ -75,17 +74,23 @@
 	ipi_local_id = x1 & UNSIGNED32_MASK;
 	ipi_remote_id = x2 & UNSIGNED32_MASK;
 
-	if (SMC_ENTITY(smc_fid) >= SMC_ENTITY_TRUSTED_APP)
+	/* OEN Number 48 to 63 is for Trusted App and OS
+	 * GET_SMC_OEN limits the return value of OEN number to 63 by bitwise
+	 * AND operation with 0x3F.
+	 * Upper limit check for OEN value is not required.
+	 */
+	if (GET_SMC_OEN(smc_fid) >= OEN_TAP_START) {
 		is_secure = 1;
-	else
+	} else {
 		is_secure = 0;
+	}
 
 	/* Validate IPI mailbox access */
 	ret = ipi_mb_validate(ipi_local_id, ipi_remote_id, is_secure);
 	if (ret)
 		SMC_RET1(handle, ret);
 
-	switch (SMC_FUNCTION(smc_fid)) {
+	switch (GET_SMC_NUM(smc_fid)) {
 	case IPI_MAILBOX_OPEN:
 		ipi_mb_open(ipi_local_id, ipi_remote_id);
 		SMC_RET1(handle, 0);
diff --git a/plat/xilinx/zynqmp/pm_service/pm_svc_main.c b/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
index 42eec0d..a3f0278 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
@@ -352,7 +352,7 @@
 
 	case PM_FPGA_GET_STATUS:
 	{
-		uint32_t value = 0;
+		uint32_t value = 0U;
 
 		ret = pm_fpga_get_status(&value);
 		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32);
@@ -370,7 +370,7 @@
 			 (uint64_t)result[2] | ((uint64_t)result[3] << 32));
 	case PM_IOCTL:
 	{
-		uint32_t value = 0;
+		uint32_t value = 0U;
 
 		ret = pm_ioctl(pm_arg[0], pm_arg[1], pm_arg[2],
 			       pm_arg[3], &value);
@@ -397,7 +397,7 @@
 
 	case PM_CLOCK_GETSTATE:
 	{
-		uint32_t value = 0;
+		uint32_t value = 0U;
 
 		ret = pm_clock_getstate(pm_arg[0], &value);
 		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32);
@@ -409,7 +409,7 @@
 
 	case PM_CLOCK_GETDIVIDER:
 	{
-		uint32_t value = 0;
+		uint32_t value = 0U;
 
 		ret = pm_clock_getdivider(pm_arg[0], &value);
 		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32);
@@ -438,7 +438,7 @@
 
 	case PM_CLOCK_GETPARENT:
 	{
-		uint32_t value = 0;
+		uint32_t value = 0U;
 
 		ret = pm_clock_getparent(pm_arg[0], &value);
 		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32U);
@@ -472,7 +472,7 @@
 
 	case PM_FPGA_READ:
 	{
-		uint32_t value = 0;
+		uint32_t value = 0U;
 
 		ret = pm_fpga_read(pm_arg[0], pm_arg[1], pm_arg[2], pm_arg[3],
 				   &value);
@@ -481,7 +481,7 @@
 
 	case PM_SECURE_AES:
 	{
-		uint32_t value = 0;
+		uint32_t value = 0U;
 
 		ret = pm_aes_engine(pm_arg[0], pm_arg[1], &value);
 		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32U);
@@ -493,7 +493,7 @@
 
 	case PM_PLL_GET_PARAMETER:
 	{
-		uint32_t value = 0;
+		uint32_t value = 0U;
 
 		ret = pm_pll_get_parameter(pm_arg[0], pm_arg[1], &value);
 		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value << 32U));
@@ -505,7 +505,7 @@
 
 	case PM_PLL_GET_MODE:
 	{
-		uint32_t mode = 0;
+		uint32_t mode = 0U;
 
 		ret = pm_pll_get_mode(pm_arg[0], &mode);
 		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)mode << 32U));
@@ -513,7 +513,7 @@
 
 	case PM_REGISTER_ACCESS:
 	{
-		uint32_t value = 0;
+		uint32_t value = 0U;
 
 		ret = pm_register_access(pm_arg[0], pm_arg[1], pm_arg[2],
 					 pm_arg[3], &value);
@@ -522,7 +522,7 @@
 
 	case PM_EFUSE_ACCESS:
 	{
-		uint32_t value = 0;
+		uint32_t value = 0U;
 
 #if defined(ZYNQMP_SECURE_EFUSES)
 		if (is_caller_non_secure(flags)) {
diff --git a/services/std_svc/rmmd/trp/trp_entry.S b/services/std_svc/rmmd/trp/trp_entry.S
index 47c1df1..3e1d8c9 100644
--- a/services/std_svc/rmmd/trp/trp_entry.S
+++ b/services/std_svc/rmmd/trp/trp_entry.S
@@ -83,7 +83,17 @@
 	mov	x3, x23
 	bl	trp_setup
 	bl	trp_main
+	b	1f
+
 warm_boot:
+	mov	x0, x20
+	mov	x1, x21
+	mov	x2, x22
+	mov	x3, x23
+	bl	trp_validate_warmboot_args
+	cbnz	x0, trp_panic /* Failed to validate warmboot args */
+
+1:
 	mov_imm	x0, RMM_BOOT_COMPLETE
 	mov	x1, xzr /* RMM_BOOT_SUCCESS */
 	smc	#0
@@ -115,7 +125,29 @@
 	 * ---------------------------------------------
 	 */
 func trp_handler
+	/*
+	 * Save Link Register and X4, as per SMCCC v1.2 its value
+	 * must be preserved unless it contains result, as specified
+	 * in the function definition.
+	 */
+	stp	x4, lr, [sp, #-16]!
+
+	/*
+	 * Zero the space for X0-X3 in trp_smc_result structure
+	 * and pass its address as the last argument.
+	 */
+	stp	xzr, xzr, [sp, #-16]!
+	stp	xzr, xzr, [sp, #-16]!
+	mov	x7, sp
+
 	bl	trp_rmi_handler
-	restore_args_call_smc
+
+	ldp	x1, x2, [sp], #16
+	ldp	x3, x4, [sp], #16
+	ldp	x5, lr, [sp], #16
+
+	ldr	x0, =RMM_RMI_REQ_COMPLETE
+	smc	#0
+
 	b	trp_handler
 endfunc trp_handler
diff --git a/services/std_svc/rmmd/trp/trp_main.c b/services/std_svc/rmmd/trp/trp_main.c
index 5a56af0..196bc11 100644
--- a/services/std_svc/rmmd/trp/trp_main.c
+++ b/services/std_svc/rmmd/trp/trp_main.c
@@ -4,7 +4,6 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-
 #include <common/debug.h>
 #include <plat/common/platform.h>
 #include <services/rmm_core_manifest.h>
@@ -31,11 +30,11 @@
 	       uint64_t x3)
 {
 	/*
-	 * Validate boot parameters.
+	 * Validate boot parameters
 	 *
-	 * According to the Boot Interface ABI v.0.1, the
-	 * parameters recived from EL3 are:
-	 * x0: CPUID (verified earlier so not used)
+	 * According to the Boot Interface ABI v.0.1,
+	 * the parameters received from EL3 are:
+	 * x0: CPUID (verified earlier, so not used)
 	 * x1: Boot Interface version
 	 * x2: PLATFORM_CORE_COUNT
 	 * x3: Pointer to the shared memory area.
@@ -66,6 +65,24 @@
 	trp_early_platform_setup((rmm_manifest_t *)trp_shared_region_start);
 }
 
+int trp_validate_warmboot_args(uint64_t x0, uint64_t x1,
+			       uint64_t x2, uint64_t x3)
+{
+	/*
+	 * Validate boot parameters for warm boot
+	 *
+	 * According to the Boot Interface ABI v.0.1, the parameters
+	 * received from EL3 during warm boot are:
+	 *
+	 * x0: CPUID (verified earlier so not used here)
+	 * [x1:x3]: RES0
+	 */
+
+	(void)x0;
+
+	return ((x1 | x2 | x3) == 0UL) ? 0 : E_RMM_BOOT_UNKNOWN;
+}
+
 /* Main function for TRP */
 void trp_main(void)
 {
@@ -73,14 +90,14 @@
 	NOTICE("TRP: %s\n", build_message);
 	NOTICE("TRP: Supported RMM-EL3 Interface ABI: v.%u.%u\n",
 		TRP_RMM_EL3_ABI_VERS_MAJOR, TRP_RMM_EL3_ABI_VERS_MINOR);
-	NOTICE("TRP: Boot Manifest Version : v.%u.%u\n",
+	NOTICE("TRP: Boot Manifest Version: v.%u.%u\n",
 		RMMD_GET_MANIFEST_VERSION_MAJOR(trp_boot_manifest_version),
 		RMMD_GET_MANIFEST_VERSION_MINOR(trp_boot_manifest_version));
-	INFO("TRP: Memory base : 0x%lx\n", (unsigned long)RMM_BASE);
-	INFO("TRP: Base address for the shared region : 0x%lx\n",
+	INFO("TRP: Memory base: 0x%lx\n", (unsigned long)RMM_BASE);
+	INFO("TRP: Shared region base address: 0x%lx\n",
 			(unsigned long)trp_shared_region_start);
-	INFO("TRP: Total size : 0x%lx bytes\n", (unsigned long)(RMM_END
-								- RMM_BASE));
+	INFO("TRP: Total size: 0x%lx bytes\n",
+			(unsigned long)(RMM_END - RMM_BASE));
 	INFO("TRP: RMM-EL3 Interface ABI reported by EL3: v.%u.%u\n",
 		TRP_RMM_EL3_VERSION_GET_MAJOR(trp_boot_abi_version),
 		TRP_RMM_EL3_VERSION_GET_MINOR(trp_boot_abi_version));
@@ -89,62 +106,73 @@
 /*******************************************************************************
  * Returning RMI version back to Normal World
  ******************************************************************************/
-static trp_args_t *trp_ret_rmi_version(void)
+static void trp_ret_rmi_version(struct trp_smc_result *smc_ret)
 {
 	VERBOSE("RMM version is %u.%u\n", RMI_ABI_VERSION_MAJOR,
 					  RMI_ABI_VERSION_MINOR);
-	return set_smc_args(RMM_RMI_REQ_COMPLETE, RMI_ABI_VERSION,
-			    0, 0, 0, 0, 0, 0);
+	smc_ret->x[0] = RMI_ABI_VERSION;
 }
 
 /*******************************************************************************
  * Transitioning granule of NON-SECURE type to REALM type
  ******************************************************************************/
-static trp_args_t *trp_asc_mark_realm(unsigned long long x1)
+static void trp_asc_mark_realm(unsigned long long x1,
+				struct trp_smc_result *smc_ret)
 {
-	unsigned long long ret;
-
 	VERBOSE("Delegating granule 0x%llx\n", x1);
-	ret = trp_smc(set_smc_args(RMM_GTSI_DELEGATE, x1, 0, 0, 0, 0, 0, 0));
+	smc_ret->x[0] = trp_smc(set_smc_args(RMM_GTSI_DELEGATE, x1,
+						0UL, 0UL, 0UL, 0UL, 0UL, 0UL));
 
-	if (ret != 0ULL) {
+	if (smc_ret->x[0] != 0ULL) {
 		ERROR("Granule transition from NON-SECURE type to REALM type "
-			"failed 0x%llx\n", ret);
+			"failed 0x%llx\n", smc_ret->x[0]);
 	}
-	return set_smc_args(RMM_RMI_REQ_COMPLETE, ret, 0, 0, 0, 0, 0, 0);
 }
 
 /*******************************************************************************
  * Transitioning granule of REALM type to NON-SECURE type
  ******************************************************************************/
-static trp_args_t *trp_asc_mark_nonsecure(unsigned long long x1)
+static void trp_asc_mark_nonsecure(unsigned long long x1,
+				   struct trp_smc_result *smc_ret)
 {
-	unsigned long long ret;
-
 	VERBOSE("Undelegating granule 0x%llx\n", x1);
-	ret = trp_smc(set_smc_args(RMM_GTSI_UNDELEGATE, x1, 0, 0, 0, 0, 0, 0));
+	smc_ret->x[0] = trp_smc(set_smc_args(RMM_GTSI_UNDELEGATE, x1,
+						0UL, 0UL, 0UL, 0UL, 0UL, 0UL));
 
-	if (ret != 0ULL) {
+	if (smc_ret->x[0] != 0ULL) {
 		ERROR("Granule transition from REALM type to NON-SECURE type "
-			"failed 0x%llx\n", ret);
+			"failed 0x%llx\n", smc_ret->x[0]);
 	}
-	return set_smc_args(RMM_RMI_REQ_COMPLETE, ret, 0, 0, 0, 0, 0, 0);
 }
 
 /*******************************************************************************
  * Main RMI SMC handler function
  ******************************************************************************/
-trp_args_t *trp_rmi_handler(unsigned long fid, unsigned long long x1)
+void trp_rmi_handler(unsigned long fid,
+		     unsigned long long x1, unsigned long long x2,
+		     unsigned long long x3, unsigned long long x4,
+		     unsigned long long x5, unsigned long long x6,
+		     struct trp_smc_result *smc_ret)
 {
+	/* Not used in the current implementation */
+	(void)x2;
+	(void)x3;
+	(void)x4;
+	(void)x5;
+	(void)x6;
+
 	switch (fid) {
 	case RMI_RMM_REQ_VERSION:
-		return trp_ret_rmi_version();
+		trp_ret_rmi_version(smc_ret);
+		break;
 	case RMI_RMM_GRANULE_DELEGATE:
-		return trp_asc_mark_realm(x1);
+		trp_asc_mark_realm(x1, smc_ret);
+		break;
 	case RMI_RMM_GRANULE_UNDELEGATE:
-		return trp_asc_mark_nonsecure(x1);
+		trp_asc_mark_nonsecure(x1, smc_ret);
+		break;
 	default:
-		ERROR("Invalid SMC code to %s, FID %lu\n", __func__, fid);
+		ERROR("Invalid SMC code to %s, FID %lx\n", __func__, fid);
+		smc_ret->x[0] = SMC_UNK;
 	}
-	return set_smc_args(SMC_UNK, 0, 0, 0, 0, 0, 0, 0);
 }
diff --git a/services/std_svc/rmmd/trp/trp_private.h b/services/std_svc/rmmd/trp/trp_private.h
index 945ae1c..d8c6960 100644
--- a/services/std_svc/rmmd/trp/trp_private.h
+++ b/services/std_svc/rmmd/trp/trp_private.h
@@ -53,5 +53,9 @@
 	       uint64_t x2,
 	       uint64_t x3);
 
+/* Validate arguments for warm boot only */
+int trp_validate_warmboot_args(uint64_t x0, uint64_t x1,
+			       uint64_t x2, uint64_t x3);
+
 #endif /* __ASSEMBLER__ */
 #endif /* TRP_PRIVATE_H */
diff --git a/services/std_svc/spm/el3_spmc/spmc.h b/services/std_svc/spm/el3_spmc/spmc.h
index 5233650..61afee3 100644
--- a/services/std_svc/spm/el3_spmc/spmc.h
+++ b/services/std_svc/spm/el3_spmc/spmc.h
@@ -228,6 +228,12 @@
 	uint32_t uuid[4];
 };
 
+/* FF-A Partition Info Get related macros. */
+#define FFA_PARTITION_INFO_GET_PROPERTIES_V1_0_MASK	U(0x7)
+#define FFA_PARTITION_INFO_GET_EXEC_STATE_SHIFT 	U(8)
+#define FFA_PARTITION_INFO_GET_AARCH32_STATE 		U(0)
+#define FFA_PARTITION_INFO_GET_AARCH64_STATE 		U(1)
+
 /* Reference to power management hooks */
 extern const spd_pm_ops_t spmc_pm;
 
diff --git a/services/std_svc/spm/el3_spmc/spmc.mk b/services/std_svc/spm/el3_spmc/spmc.mk
index c674e71..6442af0 100644
--- a/services/std_svc/spm/el3_spmc/spmc.mk
+++ b/services/std_svc/spm/el3_spmc/spmc.mk
@@ -20,7 +20,9 @@
                     ${PLAT}_el3_spmc_logical_sp.c)
 
 
+ifneq ($(wildcard $(SPMC_LP_SOURCES)),)
 SPMC_SOURCES += $(SPMC_LP_SOURCES)
+endif
 
 # Let the top-level Makefile know that we intend to include a BL32 image
 NEED_BL32		:=	yes
diff --git a/services/std_svc/spm/el3_spmc/spmc_main.c b/services/std_svc/spm/el3_spmc/spmc_main.c
index 9b8621a..08e7218 100644
--- a/services/std_svc/spm/el3_spmc/spmc_main.c
+++ b/services/std_svc/spm/el3_spmc/spmc_main.c
@@ -746,6 +746,27 @@
 }
 
 /*
+ * Helper function to populate the properties field of a Partition Info Get
+ * descriptor.
+ */
+static uint32_t
+partition_info_get_populate_properties(uint32_t sp_properties,
+				       enum sp_execution_state sp_ec_state)
+{
+	uint32_t properties = sp_properties;
+	uint32_t ec_state;
+
+	/* Determine the execution state of the SP. */
+	ec_state = sp_ec_state == SP_STATE_AARCH64 ?
+		   FFA_PARTITION_INFO_GET_AARCH64_STATE :
+		   FFA_PARTITION_INFO_GET_AARCH32_STATE;
+
+	properties |= ec_state << FFA_PARTITION_INFO_GET_EXEC_STATE_SHIFT;
+
+	return properties;
+}
+
+/*
  * Collate the partition information in a v1.1 partition information
  * descriptor format, this will be converter later if required.
  */
@@ -771,7 +792,12 @@
 			desc = &partitions[*partition_count];
 			desc->ep_id = el3_lp_descs[index].sp_id;
 			desc->execution_ctx_count = PLATFORM_CORE_COUNT;
-			desc->properties = el3_lp_descs[index].properties;
+			/* LSPs must be AArch64. */
+			desc->properties =
+				partition_info_get_populate_properties(
+					el3_lp_descs[index].properties,
+					SP_STATE_AARCH64);
+
 			if (null_uuid) {
 				copy_uuid(desc->uuid, el3_lp_descs[index].uuid);
 			}
@@ -794,7 +820,11 @@
 			 * S-EL1 SPs.
 			 */
 			desc->execution_ctx_count = PLATFORM_CORE_COUNT;
-			desc->properties = sp_desc[index].properties;
+			desc->properties =
+				partition_info_get_populate_properties(
+					sp_desc[index].properties,
+					sp_desc[index].execution_state);
+
 			if (null_uuid) {
 				copy_uuid(desc->uuid, sp_desc[index].uuid);
 			}
@@ -835,7 +865,7 @@
 
 /*
  * If the caller of the PARTITION_INFO_GET ABI was a v1.0 caller, populate
- * the coresponding descriptor format from the v1.1 descriptor array.
+ * the corresponding descriptor format from the v1.1 descriptor array.
  */
 static uint64_t partition_info_populate_v1_0(struct ffa_partition_info_v1_1
 					     *partitions,
@@ -860,8 +890,10 @@
 		v1_0_partitions[index].ep_id = partitions[index].ep_id;
 		v1_0_partitions[index].execution_ctx_count =
 			partitions[index].execution_ctx_count;
+		/* Only report v1.0 properties. */
 		v1_0_partitions[index].properties =
-			partitions[index].properties;
+			(partitions[index].properties &
+			FFA_PARTITION_INFO_GET_PROPERTIES_V1_0_MASK);
 	}
 	return 0;
 }
diff --git a/services/std_svc/spm/el3_spmc/spmc_shared_mem.c b/services/std_svc/spm/el3_spmc/spmc_shared_mem.c
index 89d7b31..c039350 100644
--- a/services/std_svc/spm/el3_spmc/spmc_shared_mem.c
+++ b/services/std_svc/spm/el3_spmc/spmc_shared_mem.c
@@ -274,13 +274,15 @@
  * spmc_shmem_obj_validate_id - Validate a partition ID is participating in
  *				a given memory transaction.
  * @sp_id:      Partition ID to validate.
- * @desc:       Descriptor of the memory transaction.
- *
+ * @obj:        The shared memory object containing the descriptor
+ *              of the memory transaction.
  * Return: true if ID is valid, else false.
  */
-bool spmc_shmem_obj_validate_id(const struct ffa_mtd *desc, uint16_t sp_id)
+bool spmc_shmem_obj_validate_id(struct spmc_shmem_obj *obj, uint16_t sp_id)
 {
 	bool found = false;
+	struct ffa_mtd *desc = &obj->desc;
+	size_t desc_size = obj->desc_size;
 
 	/* Validate the partition is a valid participant. */
 	for (unsigned int i = 0U; i < desc->emad_count; i++) {
@@ -290,6 +292,15 @@
 		emad = spmc_shmem_obj_get_emad(desc, i,
 					       MAKE_FFA_VERSION(1, 1),
 					       &emad_size);
+		/*
+		 * Validate the calculated emad address resides within the
+		 * descriptor.
+		 */
+		if ((emad == NULL) || (uintptr_t) emad >=
+		    (uintptr_t)((uint8_t *) desc + desc_size)) {
+			VERBOSE("Invalid emad.\n");
+			break;
+		}
 		if (sp_id == emad->mapd.endpoint_id) {
 			found = true;
 			break;
@@ -385,7 +396,8 @@
 	      emad_array[0].comp_mrd_offset);
 
 	/* Check the calculated address is within the memory descriptor. */
-	if ((uintptr_t) mrd >= (uintptr_t)((uint8_t *) orig + desc_size)) {
+	if (((uintptr_t) mrd + sizeof(struct ffa_comp_mrd)) >
+	    (uintptr_t)((uint8_t *) orig + desc_size)) {
 		return 0;
 	}
 	size += mrd->address_range_count * sizeof(struct ffa_cons_mrd);
@@ -424,7 +436,8 @@
 	      emad_array[0].comp_mrd_offset);
 
 	/* Check the calculated address is within the memory descriptor. */
-	if ((uintptr_t) mrd >= (uintptr_t)((uint8_t *) orig + desc_size)) {
+	if (((uintptr_t) mrd + sizeof(struct ffa_comp_mrd)) >
+	    (uintptr_t)((uint8_t *) orig + desc_size)) {
 		return 0;
 	}
 	size += mrd->address_range_count * sizeof(struct ffa_cons_mrd);
@@ -475,6 +488,12 @@
 
 	/* Copy across the emad structs. */
 	for (unsigned int i = 0U; i < out->emad_count; i++) {
+		/* Bound check for emad array. */
+		if (((uint8_t *)emad_array_in + sizeof(struct ffa_emad_v1_0)) >
+		    ((uint8_t *) mtd_orig + orig->desc_size)) {
+			VERBOSE("%s: Invalid mtd structure.\n", __func__);
+			return false;
+		}
 		memcpy(&emad_array_out[i], &emad_array_in[i],
 		       sizeof(struct ffa_emad_v1_0));
 	}
@@ -542,6 +561,7 @@
 	size_t mrd_out_offset;
 	size_t emad_out_array_size;
 	size_t mrd_size = 0;
+	size_t orig_desc_size = orig->desc_size;
 
 	/* Populate the v1.0 descriptor format from the v1.1 struct. */
 	out->sender_id = mtd_orig->sender_id;
@@ -559,6 +579,12 @@
 	/* Copy across the emad structs. */
 	emad_in = emad_array_in;
 	for (unsigned int i = 0U; i < out->emad_count; i++) {
+		/* Bound check for emad array. */
+		if (((uint8_t *)emad_in + sizeof(struct ffa_emad_v1_0)) >
+				((uint8_t *) mtd_orig + orig_desc_size)) {
+			VERBOSE("%s: Invalid mtd structure.\n", __func__);
+			return false;
+		}
 		memcpy(&emad_array_out[i], emad_in,
 		       sizeof(struct ffa_emad_v1_0));
 
@@ -1442,7 +1468,7 @@
 	}
 
 	/* Validate the caller is a valid participant. */
-	if (!spmc_shmem_obj_validate_id(&obj->desc, sp_ctx->sp_id)) {
+	if (!spmc_shmem_obj_validate_id(obj, sp_ctx->sp_id)) {
 		WARN("%s: Invalid endpoint ID (0x%x).\n",
 			__func__, sp_ctx->sp_id);
 		ret = FFA_ERROR_INVALID_PARAMETER;
@@ -1761,7 +1787,7 @@
 	}
 
 	/* Validate the caller is a valid participant. */
-	if (!spmc_shmem_obj_validate_id(&obj->desc, sp_ctx->sp_id)) {
+	if (!spmc_shmem_obj_validate_id(obj, sp_ctx->sp_id)) {
 		WARN("%s: Invalid endpoint ID (0x%x).\n",
 			__func__, req->endpoint_array[0]);
 		ret = FFA_ERROR_INVALID_PARAMETER;
diff --git a/services/std_svc/trng/trng_entropy_pool.c b/services/std_svc/trng/trng_entropy_pool.c
index 30105b3..dd08c5e 100644
--- a/services/std_svc/trng/trng_entropy_pool.c
+++ b/services/std_svc/trng/trng_entropy_pool.c
@@ -66,7 +66,7 @@
 bool trng_pack_entropy(uint32_t nbits, uint64_t *out)
 {
 	bool ret = true;
-
+	uint32_t bits_to_discard = nbits;
 	spin_lock(&trng_pool_lock);
 
 	if (!trng_fill_entropy(nbits)) {
@@ -111,9 +111,66 @@
 		 *                   5 4 3 2 1 0 7 6
 		 *                  [e,e,e,e,e,e,e,e]
 		 */
-		out[word_i] = 0;
 		out[word_i] |= entropy[ENTROPY_WORD_INDEX(word_i)] >> rshift;
 
+		/**
+		 * Discarding the used/packed entropy bits from the respective
+		 * words, (word_i) and (word_i+1) as applicable.
+		 * In each iteration of the loop, we pack 64bits of entropy to
+		 * the output buffer. The bits are picked linearly starting from
+		 * 1st word (entropy[0]) till 4th word (entropy[3]) and then
+		 * rolls back (entropy[0]). Discarding of bits is managed
+		 * similarly.
+		 *
+		 * The following diagram illustrates the logic:
+		 *
+		 *          |---------entropy pool----------|
+		 * C var    |--(word_i + 1)-|----word_i-----|
+		 * bit idx  |7 6 5 4 3 2 1 0|7 6 5 4 3 2 1 0|
+		 *          [e,e,e,e,e,e,e,e|e,e,0,0,0,0,0,0]
+		 *          |   [e,e,e,e,e,e,e,e]           |
+		 *          |   |--out[word_i]--|           |
+		 *    lshift|---|               |--rshift---|
+		 *          |e,e|0,0,0,0,0,0,0,0|0,0,0,0,0,0|
+		 *              |<==   ||    ==>|
+		 *               bits_to_discard (from these bytes)
+		 *
+		 * variable(bits_to_discard): Tracks the amount of bits to be
+		 * discarded and is updated accordingly in each iteration.
+		 *
+		 * It monitors these packed bits from respective word_i and
+		 * word_i+1 and overwrites them with zeros accordingly.
+		 * It discards linearly from the lowest index and moves upwards
+		 * until bits_to_discard variable becomes zero.
+		 *
+		 * In the above diagram,for example, we pack 2bytes(7th and 6th
+		 * from word_i) and 6bytes(0th till 5th from word_i+1), combine
+		 * and pack them as 64bit to output buffer out[i].
+		 * Depending on the number of bits requested, we discard the
+		 * bits from these packed bytes by overwriting them with zeros.
+		 */
+
+		/*
+		 * If the bits to be discarded is lesser than the amount of bits
+		 * copied to the output buffer from word_i, we discard that much
+		 * amount of bits only.
+		 */
+		if (bits_to_discard < (BITS_PER_WORD - rshift)) {
+			entropy[ENTROPY_WORD_INDEX(word_i)] &=
+			(~0ULL << ((bits_to_discard+rshift) % BITS_PER_WORD));
+			bits_to_discard = 0;
+		} else {
+		/*
+		 * If the bits to be discarded is more than the amount of valid
+		 * upper bits from word_i, which has been copied to the output
+		 * buffer, we just set the entire word_i to 0, as the lower bits
+		 * will be already zeros from previous operations, and the
+		 * bits_to_discard is updated precisely.
+		 */
+			entropy[ENTROPY_WORD_INDEX(word_i)] = 0;
+			bits_to_discard -= (BITS_PER_WORD - rshift);
+		}
+
 		/*
 		 * Note that a shift of 64 bits is treated as a shift of 0 bits.
 		 * When the shift amount is the same as the BITS_PER_WORD, we
@@ -123,6 +180,35 @@
 		if (lshift != BITS_PER_WORD) {
 			out[word_i] |= entropy[ENTROPY_WORD_INDEX(word_i + 1)]
 				<< lshift;
+			/**
+			 * Discarding the remaining packed bits from upperword
+			 * (word[i+1]) which was copied to output buffer by
+			 * overwriting with zeros.
+			 *
+			 * If the remaining bits to be discarded is lesser than
+			 * the amount of bits from [word_i+1], which has been
+			 * copied to the output buffer, we overwrite that much
+			 * amount of bits only.
+			 */
+			if (bits_to_discard < (BITS_PER_WORD - lshift)) {
+				entropy[ENTROPY_WORD_INDEX(word_i+1)]  &=
+				(~0ULL << ((bits_to_discard) % BITS_PER_WORD));
+				bits_to_discard = 0;
+			} else {
+			/*
+			 * If bits to discard is more than the bits from word_i+1
+			 * which got packed into the output, then we discard all
+			 * those copied bits.
+			 *
+			 * Note: we cannot set the entire word_i+1 to 0, as
+			 * there are still some unused valid entropy bits at the
+			 * upper end for future use.
+			 */
+				entropy[ENTROPY_WORD_INDEX(word_i+1)]  &=
+				(~0ULL << ((BITS_PER_WORD - lshift) % BITS_PER_WORD));
+				bits_to_discard -= (BITS_PER_WORD - lshift);
+		}
+
 		}
 	}
 	const uint64_t mask = ~0ULL >> (BITS_PER_WORD - (nbits % BITS_PER_WORD));
diff --git a/tools/nxp/create_pbl/create_pbl.c b/tools/nxp/create_pbl/create_pbl.c
index 9457a00..792747f 100644
--- a/tools/nxp/create_pbl/create_pbl.c
+++ b/tools/nxp/create_pbl/create_pbl.c
@@ -823,7 +823,9 @@
 		}
 	}
 
-	if ((args & MAND_ARG_MASK) != MAND_ARG_MASK) {
+	if ((args & MAND_ARG_MASK) != MAND_ARG_MASK
+			|| pblimg.rcw_nm == NULL
+			|| pblimg.imagefile == NULL) {
 		print_usage();
 	}
 
diff --git a/tools/sptool/sp_mk_generator.py b/tools/sptool/sp_mk_generator.py
index f3af584..4067331 100644
--- a/tools/sptool/sp_mk_generator.py
+++ b/tools/sptool/sp_mk_generator.py
@@ -132,20 +132,21 @@
     sp_pkg = get_sp_pkg(sp, args)
     sp_dtb_name = os.path.basename(get_file_from_layout(sp_layout[sp]["pm"]))[:-1] + "b"
     sp_dtb = os.path.join(args["out_dir"], f"fdts/{sp_dtb_name}")
+    sp_img = get_sp_img_full_path(sp_layout[sp], args)
 
     # Do not generate rule if already there.
     if is_line_in_sp_gen(f'{sp_pkg}:', args):
         return args
     write_to_sp_mk_gen(f"SP_PKGS += {sp_pkg}\n", args)
 
-    sptool_args = f" -i {get_sp_img_full_path(sp_layout[sp], args)}:{sp_dtb}"
+    sptool_args = f" -i {sp_img}:{sp_dtb}"
     pm_offset = get_pm_offset(sp_layout[sp])
     sptool_args += f" --pm-offset {pm_offset}" if pm_offset is not None else ""
     image_offset = get_image_offset(sp_layout[sp])
     sptool_args += f" --img-offset {image_offset}" if image_offset is not None else ""
     sptool_args += f" -o {sp_pkg}"
     sppkg_rule = f'''
-{sp_pkg}: {sp_dtb}
+{sp_pkg}: {sp_dtb} {sp_img}
 \t$(Q)echo Generating {sp_pkg}
 \t$(Q)$(PYTHON) $(SPTOOL) {sptool_args}
 '''