Merge changes from topic "qemu_sel2" into integration

* changes:
  docs(build): describes the SPMC_OPTEE build option
  feat(qemu): support el3 spmc
  feat(el3-spmc): make platform logical partition optional
  feat(qemu): support s-el2 spmc
  feat(qemu): update abi between spmd and spmc
  fix(sptool): add dependency to SP image
diff --git a/Makefile b/Makefile
index 1ddb7b8..c4350dc 100644
--- a/Makefile
+++ b/Makefile
@@ -526,9 +526,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
@@ -574,6 +572,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/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/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 55e265c..c07a0aa 100644
--- a/docs/design/cpu-specific-build-macros.rst
+++ b/docs/design/cpu-specific-build-macros.rst
@@ -518,6 +518,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
@@ -595,12 +599,20 @@
    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
   Cortex-X3 CPU. This needs to be enabled only for revisions r0p0 and r1p0
   of the CPU, it is fixed in r1p1.
 
+- ``ERRATA_X3_2615812``: This applies errata 2615812 workaround to Cortex-X3
+  CPU. This needs to be enabled only for revisions r0p0, r1p0 and r1p1 of the
+  CPU, it is still open.
+
 For Cortex-A510, the following errata build flags are defined :
 
 -  ``ERRATA_A510_1922240``: This applies errata 1922240 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 953b4e4..e54ff41 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.
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/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/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/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/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/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/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/lib/cpus/aarch64/cortex_x3.h b/include/lib/cpus/aarch64/cortex_x3.h
index 076a87b..ceafe66 100644
--- a/include/lib/cpus/aarch64/cortex_x3.h
+++ b/include/lib/cpus/aarch64/cortex_x3.h
@@ -10,7 +10,7 @@
 #define CORTEX_X3_MIDR				U(0x410FD4E0)
 
 /* Cortex-X3 loop count for CVE-2022-23960 mitigation */
-#define CORTEX_X3_BHB_LOOP_COUNT			U(132)
+#define CORTEX_X3_BHB_LOOP_COUNT		U(132)
 
 /*******************************************************************************
  * CPU Extended Control register specific definitions
@@ -20,8 +20,10 @@
 /*******************************************************************************
  * CPU Power Control register specific definitions
  ******************************************************************************/
-#define CORTEX_X3_CPUPWRCTLR_EL1			S3_0_C15_C2_7
-#define CORTEX_X3_CPUPWRCTLR_EL1_CORE_PWRDN_BIT	U(1)
+#define CORTEX_X3_CPUPWRCTLR_EL1				S3_0_C15_C2_7
+#define CORTEX_X3_CPUPWRCTLR_EL1_CORE_PWRDN_BIT			U(1)
+#define CORTEX_X3_CPUPWRCTLR_EL1_WFI_RET_CTRL_BITS_SHIFT	U(4)
+#define CORTEX_X3_CPUPWRCTLR_EL1_WFE_RET_CTRL_BITS_SHIFT	U(7)
 
 /*******************************************************************************
  * CPU Auxiliary Control register 2 specific definitions.
diff --git a/include/lib/psa/measured_boot.h b/include/lib/psa/measured_boot.h
index bdb79d5..231da2c 100644
--- a/include/lib/psa/measured_boot.h
+++ b/include/lib/psa/measured_boot.h
@@ -34,14 +34,14 @@
  *
  * index			Slot number in which measurement is to be stored
  * signer_id			Pointer to signer_id buffer.
- * signer_id_size		Size of the signer_id buffer in bytes.
+ * signer_id_size		Size of the signer_id in bytes.
  * version			Pointer to version buffer.
- * version_size			Size of the version buffer in bytes.
+ * version_size			Size of the version string in bytes (with \0).
  * measurement_algo		Algorithm identifier used for measurement.
  * sw_type			Pointer to sw_type buffer.
- * sw_type_size			Size of the sw_type buffer in bytes.
+ * sw_type_size			Size of the sw_type string in bytes (with \0).
  * measurement_value		Pointer to measurement_value buffer.
- * measurement_value_size	Size of the measurement_value buffer in bytes.
+ * measurement_value_size	Size of the measurement_value in bytes.
  * lock_measurement		Boolean flag requesting whether the measurement
  *				is to be locked.
  *
@@ -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/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_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/cortex_x3.S b/lib/cpus/aarch64/cortex_x3.S
index bf1b6ec..f104b48 100644
--- a/lib/cpus/aarch64/cortex_x3.S
+++ b/lib/cpus/aarch64/cortex_x3.S
@@ -59,6 +59,7 @@
 endfunc check_errata_cve_2022_23960
 
 func cortex_x3_reset_func
+	mov	x19, x30
 	/* Disable speculative loads */
 	msr	SSBS, xzr
 
@@ -71,8 +72,14 @@
 	msr	vbar_el3, x0
 #endif /* IMAGE_BL31 && WORKAROUND_CVE_2022_23960 */
 
+	bl	cpu_get_rev_var
+
+#if ERRATA_X3_2615812
+	bl	errata_cortex_x3_2615812_wa
+#endif /* ERRATA_X3_2615812 */
+
 	isb
-	ret
+	ret	x19
 endfunc cortex_x3_reset_func
 
 /* ----------------------------------------------------------------------
@@ -103,6 +110,35 @@
 	b	cpu_rev_var_ls
 endfunc check_errata_2313909
 
+/* ----------------------------------------------------------------------
+ * Errata Workaround for Cortex-X3 Erratum 2615812 on power-on.
+ * This applies to revision r0p0, r1p0, r1p1 of Cortex-X3. Open.
+ * Inputs:
+ * x0: variant[4:7] and revision[0:3] of current cpu.
+ * Shall clobber: x0-x1, x17
+ * ----------------------------------------------------------------------
+ */
+func errata_cortex_x3_2615812_wa
+	/* Check revision. */
+	mov	x17, x30
+	bl	check_errata_2615812
+	cbz	x0, 1f
+
+	/* Disable retention control for WFI and WFE. */
+	mrs	x0, CORTEX_X3_CPUPWRCTLR_EL1
+	bfi	x0, xzr, #CORTEX_X3_CPUPWRCTLR_EL1_WFI_RET_CTRL_BITS_SHIFT, #3
+	bfi	x0, xzr, #CORTEX_X3_CPUPWRCTLR_EL1_WFE_RET_CTRL_BITS_SHIFT, #3
+	msr	CORTEX_X3_CPUPWRCTLR_EL1, x0
+1:
+	ret	x17
+endfunc errata_cortex_x3_2615812_wa
+
+func check_errata_2615812
+	/* Applies to r1p1 and below. */
+	mov	x1, #0x11
+	b	cpu_rev_var_ls
+endfunc check_errata_2615812
+
 #if REPORT_ERRATA
 	/*
 	 * Errata printing function for Cortex-X3. Must follow AAPCS.
@@ -118,6 +154,7 @@
 	 * checking functions of each errata.
 	 */
 	report_errata ERRATA_X3_2313909, cortex_x3, 2313909
+	report_errata ERRATA_X3_2615812, cortex_x3, 2615812
 	report_errata WORKAROUND_CVE_2022_23960, cortex_x3, cve_2022_23960
 
 	ldp	x8, x30, [sp], #16
diff --git a/lib/cpus/cpu-ops.mk b/lib/cpus/cpu-ops.mk
index f19c16e..527a82f 100644
--- a/lib/cpus/cpu-ops.mk
+++ b/lib/cpus/cpu-ops.mk
@@ -572,6 +572,11 @@
 # 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 2067956 workaround during reset. This erratum applies
 # to revision r0p0 of the Neoverse N2 cpu and is still open.
 ERRATA_N2_2067956	?=0
@@ -655,10 +660,19 @@
 # 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
 
+# Flag to apply erratum 2615812 workaround on powerdown. This erratum applies
+# to revisions r0p0, r1p0, r1p1 of the Cortex-X3 cpu, it is still open.
+ERRATA_X3_2615812	?=0
+
 # Flag to apply erratum 1922240 workaround during reset. This erratum applies
 # to revision r0p0 of the Cortex-A510 cpu and is fixed in r0p1.
 ERRATA_A510_1922240	?=0
@@ -1204,6 +1218,10 @@
 $(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_2067956 flag
 $(eval $(call assert_boolean,ERRATA_N2_2067956))
 $(eval $(call add_define,ERRATA_N2_2067956))
@@ -1284,10 +1302,18 @@
 $(eval $(call assert_boolean,ERRATA_X2_2371105))
 $(eval $(call add_define,ERRATA_X2_2371105))
 
+# Process ERRATA_X2_2768515 flag
+$(eval $(call assert_boolean,ERRATA_X2_2768515))
+$(eval $(call add_define,ERRATA_X2_2768515))
+
 # Process ERRATA_X3_2313909 flag
 $(eval $(call assert_boolean,ERRATA_X3_2313909))
 $(eval $(call add_define,ERRATA_X3_2313909))
 
+# Process ERRATA_X3_2615812 flag
+$(eval $(call assert_boolean,ERRATA_X3_2615812))
+$(eval $(call add_define,ERRATA_X3_2615812))
+
 # Process ERRATA_A510_1922240 flag
 $(eval $(call assert_boolean,ERRATA_A510_1922240))
 $(eval $(call add_define,ERRATA_A510_1922240))
diff --git a/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index 6b88a90..b126b9c 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S
@@ -1064,16 +1064,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 +1093,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 +1106,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/psa/measured_boot.c b/lib/psa/measured_boot.c
index 6e9ff78..10c43f1 100644
--- a/lib/psa/measured_boot.c
+++ b/lib/psa/measured_boot.c
@@ -80,22 +80,24 @@
 		.lock_measurement = lock_measurement,
 		.measurement_algo = measurement_algo,
 		.sw_type = {0},
-		.sw_type_size = sw_type_size,
+		/* Removing \0 */
+		.sw_type_size = (sw_type_size > 0) ? (sw_type_size - 1) : 0,
 	};
 
 	psa_invec in_vec[] = {
 		{.base = &extend_iov,
 			.len = sizeof(struct measured_boot_extend_iovec_t)},
 		{.base = signer_id, .len = signer_id_size},
-		{.base = version, .len = version_size},
+		{.base = version,
+			.len = (version_size > 0) ? (version_size - 1) : 0},
 		{.base = measurement_value, .len = measurement_value_size}
 	};
 
 	if (sw_type != NULL) {
-		if (sw_type_size > SW_TYPE_MAX_SIZE) {
+		if (extend_iov.sw_type_size > SW_TYPE_MAX_SIZE) {
 			return PSA_ERROR_INVALID_ARGUMENT;
 		}
-		memcpy(extend_iov.sw_type, sw_type, sw_type_size);
+		memcpy(extend_iov.sw_type, sw_type, extend_iov.sw_type_size);
 	}
 
 	log_measurement(index, signer_id, signer_id_size,
@@ -109,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
@@ -131,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..1f59a4f 100644
--- a/plat/arm/board/common/board_common.mk
+++ b/plat/arm/board/common/board_common.mk
@@ -20,6 +20,7 @@
 	CRYPTO_ALG=rsa
 	ARM_ROTPK_LOCATION_ID = ARM_ROTPK_DEVEL_RSA_ID
 	ARM_ROTPK_HASH = plat/arm/board/common/rotpk/arm_rotpk_rsa_sha256.bin
+	ARM_ROTPK_S = plat/arm/board/common/rotpk/arm_dev_rotpk.S
 $(eval $(call add_define_val,ARM_ROTPK_HASH,'"$(ARM_ROTPK_HASH)"'))
 $(BUILD_PLAT)/bl2/arm_dev_rotpk.o : $(ARM_ROTPK_HASH)
 $(warning Development keys support for FVP is deprecated. Use `regs` \
@@ -28,10 +29,17 @@
 	CRYPTO_ALG=ec
 	ARM_ROTPK_LOCATION_ID = ARM_ROTPK_DEVEL_ECDSA_ID
 	ARM_ROTPK_HASH = plat/arm/board/common/rotpk/arm_rotpk_ecdsa_sha256.bin
+	ARM_ROTPK_S = plat/arm/board/common/rotpk/arm_dev_rotpk.S
 $(eval $(call add_define_val,ARM_ROTPK_HASH,'"$(ARM_ROTPK_HASH)"'))
 $(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 +75,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/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/tc_bl31_setup.c b/plat/arm/board/tc/tc_bl31_setup.c
index 0523ef8..2cdf045 100644
--- a/plat/arm/board/tc/tc_bl31_setup.c
+++ b/plat/arm/board/tc/tc_bl31_setup.c
@@ -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];
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/agilex/include/agilex_pinmux.h b/plat/intel/soc/agilex/include/agilex_pinmux.h
index fe01062..0701208 100644
--- a/plat/intel/soc/agilex/include/agilex_pinmux.h
+++ b/plat/intel/soc/agilex/include/agilex_pinmux.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,10 +7,25 @@
 #ifndef AGX_PINMUX_H
 #define AGX_PINMUX_H
 
-#define AGX_PINMUX_PIN0SEL		0xffd13000
-#define AGX_PINMUX_IO0CTRL		0xffd13130
-#define AGX_PINMUX_PINMUX_EMAC0_USEFPGA	0xffd13300
-#define AGX_PINMUX_IO0_DELAY		0xffd13400
+#define AGX_PINMUX_BASE					0xffd13000
+#define AGX_PINMUX_PIN0SEL				(AGX_PINMUX_BASE + 0x000)
+#define AGX_PINMUX_IO0CTRL				(AGX_PINMUX_BASE + 0x130)
+#define AGX_PINMUX_EMAC0_USEFPGA			(AGX_PINMUX_BASE + 0x300)
+#define AGX_PINMUX_EMAC1_USEFPGA			(AGX_PINMUX_BASE + 0x304)
+#define AGX_PINMUX_EMAC2_USEFPGA			(AGX_PINMUX_BASE + 0x308)
+#define AGX_PINMUX_NAND_USEFPGA				(AGX_PINMUX_BASE + 0x320)
+#define AGX_PINMUX_SPIM0_USEFPGA			(AGX_PINMUX_BASE + 0x328)
+#define AGX_PINMUX_SPIM1_USEFPGA			(AGX_PINMUX_BASE + 0x32c)
+#define AGX_PINMUX_SDMMC_USEFPGA			(AGX_PINMUX_BASE + 0x354)
+#define AGX_PINMUX_IO0_DELAY				(AGX_PINMUX_BASE + 0x400)
+
+#define AGX_PINMUX_NAND_USEFPGA_VAL			BIT(4)
+#define AGX_PINMUX_SDMMC_USEFPGA_VAL			BIT(8)
+#define AGX_PINMUX_SPIM0_USEFPGA_VAL			BIT(16)
+#define AGX_PINMUX_SPIM1_USEFPGA_VAL			BIT(24)
+#define AGX_PINMUX_EMAC0_USEFPGA_VAL			BIT(0)
+#define AGX_PINMUX_EMAC1_USEFPGA_VAL			BIT(8)
+#define AGX_PINMUX_EMAC2_USEFPGA_VAL			BIT(16)
 
 #include "socfpga_handoff.h"
 
diff --git a/plat/intel/soc/agilex/soc/agilex_pinmux.c b/plat/intel/soc/agilex/soc/agilex_pinmux.c
index 0b908cf..96e1ade 100644
--- a/plat/intel/soc/agilex/soc/agilex_pinmux.c
+++ b/plat/intel/soc/agilex/soc/agilex_pinmux.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -188,7 +188,27 @@
 
 void config_fpgaintf_mod(void)
 {
-	mmio_write_32(SOCFPGA_SYSMGR(FPGAINTF_EN_2), 1<<8);
+	uint32_t val;
+
+	val = 0;
+	if (mmio_read_32(AGX_PINMUX_NAND_USEFPGA) & 1)
+		val |= AGX_PINMUX_NAND_USEFPGA_VAL;
+	if (mmio_read_32(AGX_PINMUX_SDMMC_USEFPGA) & 1)
+		val |= AGX_PINMUX_SDMMC_USEFPGA_VAL;
+	if (mmio_read_32(AGX_PINMUX_SPIM0_USEFPGA) & 1)
+		val |= AGX_PINMUX_SPIM0_USEFPGA_VAL;
+	if (mmio_read_32(AGX_PINMUX_SPIM1_USEFPGA) & 1)
+		val |= AGX_PINMUX_SPIM1_USEFPGA_VAL;
+	mmio_write_32(SOCFPGA_SYSMGR(FPGAINTF_EN_2), val);
+
+	val = 0;
+	if (mmio_read_32(AGX_PINMUX_EMAC0_USEFPGA) & 1)
+		val |= AGX_PINMUX_EMAC0_USEFPGA_VAL;
+	if (mmio_read_32(AGX_PINMUX_EMAC1_USEFPGA) & 1)
+		val |= AGX_PINMUX_EMAC1_USEFPGA_VAL;
+	if (mmio_read_32(AGX_PINMUX_EMAC2_USEFPGA) & 1)
+		val |= AGX_PINMUX_EMAC2_USEFPGA_VAL;
+	mmio_write_32(SOCFPGA_SYSMGR(FPGAINTF_EN_3), val);
 }
 
 
@@ -208,8 +228,8 @@
 			hoff_ptr->pinmux_io_array[i+1]);
 	}
 
-	for (i = 0; i < 42; i += 2) {
-		mmio_write_32(AGX_PINMUX_PINMUX_EMAC0_USEFPGA +
+	for (i = 0; i < 40; i += 2) {
+		mmio_write_32(AGX_PINMUX_EMAC0_USEFPGA +
 			hoff_ptr->pinmux_fpga_array[i],
 			hoff_ptr->pinmux_fpga_array[i+1]);
 	}
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..3ab6f52 100644
--- a/plat/intel/soc/common/include/socfpga_mailbox.h
+++ b/plat/intel/soc/common/include/socfpga_mailbox.h
@@ -139,6 +139,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..f18c3e4 100644
--- a/plat/intel/soc/common/sip/socfpga_sip_fcs.c
+++ b/plat/intel/soc/common/sip/socfpga_sip_fcs.c
@@ -946,6 +946,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 +1169,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 +1567,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 +1781,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 +2047,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_firewall.c b/plat/intel/soc/common/soc/socfpga_firewall.c
index 515784b..fc3889c 100644
--- a/plat/intel/soc/common/soc/socfpga_firewall.c
+++ b/plat/intel/soc/common/soc/socfpga_firewall.c
@@ -60,6 +60,7 @@
 	mmio_write_32(SOCFPGA_L4_PER_SCR(I2C3), DISABLE_L4_FIREWALL);
 	mmio_write_32(SOCFPGA_L4_PER_SCR(I2C4), DISABLE_L4_FIREWALL);
 
+	mmio_write_32(SOCFPGA_L4_PER_SCR(SP_TIMER0), DISABLE_L4_FIREWALL);
 	mmio_write_32(SOCFPGA_L4_PER_SCR(SP_TIMER1), DISABLE_L4_FIREWALL);
 
 	mmio_write_32(SOCFPGA_L4_PER_SCR(UART0), DISABLE_L4_FIREWALL);
diff --git a/plat/intel/soc/common/soc/socfpga_mailbox.c b/plat/intel/soc/common/soc/socfpga_mailbox.c
index 79817e6..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;
 	}
@@ -601,7 +625,7 @@
 
 	res = response[RECONFIG_STATUS_SOFTFUNC_STATUS];
 	if ((res & SOFTFUNC_STATUS_SEU_ERROR) != 0U) {
-		return MBOX_CFGSTAT_STATE_ERROR_HARDWARE;
+		ERROR("SoftFunction Status SEU ERROR\n");
 	}
 
 	if ((res & SOFTFUNC_STATUS_CONF_DONE) == 0U) {
diff --git a/plat/intel/soc/common/socfpga_sip_svc.c b/plat/intel/soc/common/socfpga_sip_svc.c
index f079349..a20e61c 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 */
@@ -469,10 +491,6 @@
 /* Intel HWMON services */
 static uint32_t intel_hwmon_readtemp(uint32_t chan, uint32_t *retval)
 {
-	if (chan > TEMP_CHANNEL_MAX) {
-		return INTEL_SIP_SMC_STATUS_ERROR;
-	}
-
 	if (mailbox_hwmon_readtemp(chan, retval) < 0) {
 		return INTEL_SIP_SMC_STATUS_ERROR;
 	}
@@ -482,10 +500,6 @@
 
 static uint32_t intel_hwmon_readvolt(uint32_t chan, uint32_t *retval)
 {
-	if (chan > VOLT_CHANNEL_MAX) {
-		return INTEL_SIP_SMC_STATUS_ERROR;
-	}
-
 	if (mailbox_hwmon_readvolt(chan, retval) < 0) {
 		return INTEL_SIP_SMC_STATUS_ERROR;
 	}
@@ -935,6 +949,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,
@@ -959,6 +989,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,
@@ -981,6 +1029,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,
@@ -1022,6 +1086,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/qemu/qemu/include/platform_def.h b/plat/qemu/qemu/include/platform_def.h
index 18c5746..803f8e2 100644
--- a/plat/qemu/qemu/include/platform_def.h
+++ b/plat/qemu/qemu/include/platform_def.h
@@ -142,7 +142,7 @@
  * Put BL2 just below BL3-1. BL2_BASE is calculated using the current BL2 debug
  * size plus a little space for growth.
  */
-#define BL2_BASE			(BL31_BASE - 0x25000)
+#define BL2_BASE			(BL31_BASE - 0x35000)
 #define BL2_LIMIT			BL31_BASE
 
 /*
diff --git a/plat/qemu/qemu/platform.mk b/plat/qemu/qemu/platform.mk
index cd8c06f..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}
 
@@ -212,6 +211,12 @@
 				${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/common/plat_spmd_manifest.c	\
 				common/uuid.c				\
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/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..acfe701 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>
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/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/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..30b0176 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,19 @@
 	ipi_local_id = x1 & UNSIGNED32_MASK;
 	ipi_remote_id = x2 & UNSIGNED32_MASK;
 
-	if (SMC_ENTITY(smc_fid) >= SMC_ENTITY_TRUSTED_APP)
+	if ((GET_SMC_OEN(smc_fid) >= OEN_TAP_START) &&
+	    (GET_SMC_OEN(smc_fid) <= OEN_TOS_END)) {
 		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/versal/pm_service/pm_client.c b/plat/xilinx/versal/pm_service/pm_client.c
index ce5e533..54f4eb2 100644
--- a/plat/xilinx/versal/pm_service/pm_client.c
+++ b/plat/xilinx/versal/pm_service/pm_client.c
@@ -120,11 +120,9 @@
 {
 	uint32_t reg_num;
 	uint32_t device_id;
-	uint8_t pm_wakeup_nodes_set[XPM_NODEIDX_DEV_MAX];
+	uint8_t pm_wakeup_nodes_set[XPM_NODEIDX_DEV_MAX] = { 0U };
 	uintptr_t isenabler1 = PLAT_VERSAL_GICD_BASE + GICD_ISENABLER + 4;
 
-	zeromem(&pm_wakeup_nodes_set, (u_register_t)sizeof(pm_wakeup_nodes_set));
-
 	for (reg_num = 0U; reg_num < NUM_GICD_ISENABLER; reg_num++) {
 		uint32_t base_irq = reg_num << ISENABLER_SHIFT;
 		uint32_t reg = mmio_read_32(isenabler1 + (reg_num << 2));
diff --git a/plat/xilinx/versal/pm_service/pm_svc_main.c b/plat/xilinx/versal/pm_service/pm_svc_main.c
index 9eb426a..c90f9e1 100644
--- a/plat/xilinx/versal/pm_service/pm_svc_main.c
+++ b/plat/xilinx/versal/pm_service/pm_svc_main.c
@@ -160,7 +160,7 @@
 
 	case PM_IOCTL:
 	{
-		uint32_t value;
+		uint32_t value = 0U;
 
 		ret = pm_api_ioctl(pm_arg[0], pm_arg[1], pm_arg[2],
 				   pm_arg[3], pm_arg[4],
diff --git a/plat/xilinx/zynqmp/pm_service/pm_svc_main.c b/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
index 03fa316..4adbef3 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
@@ -207,10 +207,16 @@
  */
 int32_t pm_setup(void)
 {
+	enum pm_ret_status err;
 
 	pm_ipi_init(primary_proc);
 
-	pm_get_api_version(&pm_ctx.api_version);
+	err = pm_get_api_version(&pm_ctx.api_version);
+	if (err != PM_RET_SUCCESS) {
+		ERROR("BL31: Failed to read Platform Management API version. "
+		      "Return: %d\n", err);
+		return -EINVAL;
+	}
 	if (pm_ctx.api_version < PM_VERSION) {
 		ERROR("BL31: Platform Management API version error. Expected: "
 		      "v%d.%d - Found: v%d.%d\n", PM_VERSION_MAJOR,
@@ -350,7 +356,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);
@@ -368,7 +374,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);
@@ -395,7 +401,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);
@@ -407,7 +413,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);
@@ -436,7 +442,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);
@@ -470,7 +476,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);
@@ -479,7 +485,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);
@@ -491,7 +497,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));
@@ -503,7 +509,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));
@@ -511,7 +517,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);
@@ -520,7 +526,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_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;