Merge "feat(libfdt): add function to set MAC addresses" into integration

commit: 0d90f22ecf5e4f5160a1d30b74949a3a5e41d625 [log] [tgz]
author: Manish Pandey <manish.pandey2@arm.com> Fri Jul 08 13:29:58 2022 +0200
committer: TrustedFirmware Code Review <review@review.trustedfirmware.org> Fri Jul 08 13:29:58 2022 +0200
tree: 282d1065e27e468447add85b84258541e90df4cb
parent: 8a55bc600f31dcea3600d3d88a784cc02ec267b3 [diff]
parent: 3e8c7ec6bb320053a0b7abde981119649ac316c2 [diff]
diff --git a/.commitlintrc.js b/.commitlintrc.js
index ed971a3..cfafbed 100644
--- a/.commitlintrc.js
+++ b/.commitlintrc.js

@@ -68,7 +68,6 @@
         "type-enum": [2, "always", types], /* Error */
 
         "scope-case": [2, "always", "lower-case"], /* Error */
-        "scope-empty": [2, "never"], /* Error */
         "scope-enum": [1, "always", scopes] /* Warning */
     },
 };

diff --git a/.gitignore b/.gitignore
index f524658..b005fab 100644
--- a/.gitignore
+++ b/.gitignore

@@ -30,8 +30,7 @@
 tools/stm32image/*.o
 tools/stm32image/stm32image
 tools/stm32image/stm32image.exe
-tools/sptool/sptool
-tools/sptool/sptool.exe
+tools/sptool/__pycache__/
 
 # GNU GLOBAL files
 GPATH

diff --git a/Makefile b/Makefile
index 3a8a522..ee5e2e7 100644
--- a/Makefile
+++ b/Makefile

@@ -8,7 +8,7 @@
 # Trusted Firmware Version
 #
 VERSION_MAJOR			:= 2
-VERSION_MINOR			:= 6
+VERSION_MINOR			:= 7
 
 # Default goal is build all images
 .DEFAULT_GOAL			:= all
@@ -135,6 +135,10 @@
 ifneq (${ENABLE_PIE},0)
         $(error ENABLE_RME does not support PIE)
 endif
+# RME doesn't support BRBE
+ifneq (${ENABLE_BRBE_FOR_NS},0)
+        $(error ENABLE_RME does not support BRBE.)
+endif
 # RME requires AARCH64
 ifneq (${ARCH},aarch64)
         $(error ENABLE_RME requires AArch64)
@@ -263,24 +267,24 @@
 # Determine if FEAT_SB is supported
 ENABLE_FEAT_SB		=	$(if $(findstring sb,${arch-features}),1,0)
 
-ifneq ($(findstring armclang,$(notdir $(CC))),)
-TF_CFLAGS_aarch32	=	-target arm-arm-none-eabi $(march32-directive)
-TF_CFLAGS_aarch64	=	-target aarch64-arm-none-eabi $(march64-directive)
-LD			=	$(LINKER)
-AS			=	$(CC) -c -x assembler-with-cpp $(TF_CFLAGS_$(ARCH))
-CPP			=	$(CC) -E $(TF_CFLAGS_$(ARCH))
-PP			=	$(CC) -E $(TF_CFLAGS_$(ARCH))
-else ifneq ($(findstring clang,$(notdir $(CC))),)
-CLANG_CCDIR		=	$(if $(filter-out ./,$(dir $(CC))),$(dir $(CC)),)
-TF_CFLAGS_aarch32	=	$(target32-directive) $(march32-directive)
-TF_CFLAGS_aarch64	=	-target aarch64-elf $(march64-directive)
-LD			=	$(CLANG_CCDIR)ld.lld
-ifeq (, $(shell which $(LD)))
-$(error "No $(LD) in PATH, make sure it is installed or set LD to a different linker")
-endif
-AS			=	$(CC) -c -x assembler-with-cpp $(TF_CFLAGS_$(ARCH))
-CPP			=	$(CC) -E
-PP			=	$(CC) -E
+ifneq ($(findstring clang,$(notdir $(CC))),)
+	ifneq ($(findstring armclang,$(notdir $(CC))),)
+		TF_CFLAGS_aarch32	:=	-target arm-arm-none-eabi $(march32-directive)
+		TF_CFLAGS_aarch64	:=	-target aarch64-arm-none-eabi $(march64-directive)
+		LD			:=	$(LINKER)
+	else
+		TF_CFLAGS_aarch32	:=	$(target32-directive) $(march32-directive)
+		TF_CFLAGS_aarch64	:=	-target aarch64-elf $(march64-directive)
+		LD			:=	$(shell $(CC) --print-prog-name ld.lld)
+
+		AR			:=	$(shell $(CC) --print-prog-name llvm-ar)
+		OD			:=	$(shell $(CC) --print-prog-name llvm-objdump)
+		OC			:=	$(shell $(CC) --print-prog-name llvm-objcopy)
+	endif
+
+	CPP		:=	$(CC) -E $(TF_CFLAGS_$(ARCH))
+	PP		:=	$(CC) -E $(TF_CFLAGS_$(ARCH))
+	AS		:=	$(CC) -c -x assembler-with-cpp $(TF_CFLAGS_$(ARCH))
 else ifneq ($(findstring gcc,$(notdir $(CC))),)
 TF_CFLAGS_aarch32	=	$(march32-directive)
 TF_CFLAGS_aarch64	=	$(march64-directive)
@@ -303,13 +307,8 @@
 $(eval $(call add_define,DEBUG))
 ifneq (${DEBUG}, 0)
         BUILD_TYPE	:=	debug
-        TF_CFLAGS	+= 	-g
-
-        ifneq ($(findstring clang,$(notdir $(CC))),)
-             ASFLAGS		+= 	-g
-        else
-             ASFLAGS		+= 	-g -Wa,--gdwarf-2
-        endif
+        TF_CFLAGS	+=	-g -gdwarf-4
+        ASFLAGS		+=	-g -Wa,-gdwarf-4
 
         # Use LOG_LEVEL_INFO by default for debug builds
         LOG_LEVEL	:=	40
@@ -777,8 +776,10 @@
     endif
 endif
 
-# SME/SVE only supported on AArch64
+# Ensure that no Aarch64-only features are enabled in Aarch32 build
 ifeq (${ARCH},aarch32)
+
+    # SME/SVE only supported on AArch64
     ifeq (${ENABLE_SME_FOR_NS},1)
         $(error "ENABLE_SME_FOR_NS cannot be used with ARCH=aarch32")
     endif
@@ -786,6 +787,12 @@
         # Warning instead of error due to CI dependency on this
         $(error "ENABLE_SVE_FOR_NS cannot be used with ARCH=aarch32")
     endif
+
+    # BRBE is not supported in Aarch32
+    ifeq (${ENABLE_BRBE_FOR_NS},1)
+        $(error "ENABLE_BRBE_FOR_NS cannot be used with ARCH=aarch32")
+    endif
+
 endif
 
 # Ensure ENABLE_RME is not used with SME
@@ -821,6 +828,10 @@
     endif
 endif
 
+ifeq ($(DRTM_SUPPORT),1)
+    $(info DRTM_SUPPORT is an experimental feature)
+endif
+
 ################################################################################
 # Process platform overrideable behaviour
 ################################################################################
@@ -918,7 +929,7 @@
 
 # Variables for use with sptool
 SPTOOLPATH		?=	tools/sptool
-SPTOOL			?=	${SPTOOLPATH}/sptool${BIN_EXT}
+SPTOOL			?=	${SPTOOLPATH}/sptool.py
 SP_MK_GEN		?=	${SPTOOLPATH}/sp_mk_generator.py
 
 # Variables for use with ROMLIB
@@ -996,12 +1007,15 @@
         HW_ASSISTED_COHERENCY \
         INVERTED_MEMMAP \
         MEASURED_BOOT \
+        DRTM_SUPPORT \
         NS_TIMER_SWITCH \
         OVERRIDE_LIBC \
         PL011_GENERIC_UART \
+        PLAT_RSS_NOT_SUPPORTED \
         PROGRAMMABLE_RESET_ADDRESS \
         PSCI_EXTENDED_STATE_ID \
         RESET_TO_BL31 \
+        RESET_TO_BL31_WITH_PARAMS \
         SAVE_KEYS \
         SEPARATE_CODE_AND_RODATA \
         SEPARATE_BL2_NOLOAD_REGION \
@@ -1031,7 +1045,6 @@
         COT_DESC_IN_DTB \
         USE_SP804_TIMER \
         PSA_FWU_SUPPORT \
-        ENABLE_TRBE_FOR_NS \
         ENABLE_SYS_REG_TRACE_FOR_NS \
         ENABLE_MPMM \
         ENABLE_MPMM_FCONF \
@@ -1047,6 +1060,8 @@
         CTX_INCLUDE_PAUTH_REGS \
         CTX_INCLUDE_MTE_REGS \
         CTX_INCLUDE_NEVE_REGS \
+        ENABLE_BRBE_FOR_NS \
+        ENABLE_TRBE_FOR_NS \
         ENABLE_BTI \
         ENABLE_PAUTH \
         ENABLE_FEAT_AMUv1 \
@@ -1068,6 +1083,8 @@
         NR_OF_FW_BANKS \
         NR_OF_IMAGES_IN_FW_BANK \
         RAS_EXTENSION \
+        TWED_DELAY \
+        ENABLE_FEAT_TWED \
 )))
 
 ifdef KEY_SIZE
@@ -1127,13 +1144,16 @@
         HW_ASSISTED_COHERENCY \
         LOG_LEVEL \
         MEASURED_BOOT \
+        DRTM_SUPPORT \
         NS_TIMER_SWITCH \
         PL011_GENERIC_UART \
         PLAT_${PLAT} \
+        PLAT_RSS_NOT_SUPPORTED \
         PROGRAMMABLE_RESET_ADDRESS \
         PSCI_EXTENDED_STATE_ID \
         RAS_EXTENSION \
         RESET_TO_BL31 \
+        RESET_TO_BL31_WITH_PARAMS \
         SEPARATE_CODE_AND_RODATA \
         SEPARATE_BL2_NOLOAD_REGION \
         SEPARATE_NOBITS_REGION \
@@ -1168,6 +1188,7 @@
         NR_OF_FW_BANKS \
         NR_OF_IMAGES_IN_FW_BANK \
         PSA_FWU_SUPPORT \
+        ENABLE_BRBE_FOR_NS \
         ENABLE_TRBE_FOR_NS \
         ENABLE_SYS_REG_TRACE_FOR_NS \
         ENABLE_TRF_FOR_NS \
@@ -1184,6 +1205,8 @@
         ENABLE_FEAT_CSV2_2 \
         ENABLE_FEAT_PAN \
         FEATURE_DETECTION \
+        TWED_DELAY \
+        ENABLE_FEAT_TWED \
 )))
 
 ifeq (${SANITIZE_UB},trap)
@@ -1330,8 +1353,7 @@
 ifeq (${NEED_SP_PKG},yes)
 $(BUILD_PLAT)/sp_gen.mk: ${SP_MK_GEN} ${SP_LAYOUT_FILE} | ${BUILD_PLAT}
 	${Q}${PYTHON} "$<" "$@" $(filter-out $<,$^) $(BUILD_PLAT) ${COT}
-sp: $(SPTOOL) $(DTBS) $(BUILD_PLAT)/sp_gen.mk
-	${Q}$(SPTOOL) $(SPTOOL_ARGS)
+sp: $(DTBS) $(BUILD_PLAT)/sp_gen.mk $(SP_PKGS)
 	@${ECHO_BLANK_LINE}
 	@echo "Built SP Images successfully"
 	@${ECHO_BLANK_LINE}
@@ -1371,7 +1393,6 @@
 # to pass the gnumake flags to nmake.
 	${Q}set MAKEFLAGS= && ${MSVC_NMAKE} /nologo /f ${FIPTOOLPATH}/Makefile.msvc FIPTOOLPATH=$(subst /,\,$(FIPTOOLPATH)) FIPTOOL=$(subst /,\,$(FIPTOOL)) realclean
 endif
-	${Q}${MAKE} --no-print-directory -C ${SPTOOLPATH} clean
 	${Q}${MAKE} PLAT=${PLAT} --no-print-directory -C ${CRTTOOLPATH} realclean
 	${Q}${MAKE} PLAT=${PLAT} --no-print-directory -C ${ENCTOOLPATH} realclean
 	${Q}${MAKE} --no-print-directory -C ${ROMLIBPATH} clean
@@ -1465,10 +1486,6 @@
 	${Q}set MAKEFLAGS= && ${MSVC_NMAKE} /nologo /f ${FIPTOOLPATH}/Makefile.msvc FIPTOOLPATH=$(subst /,\,$(FIPTOOLPATH)) FIPTOOL=$(subst /,\,$(FIPTOOL))
 endif
 
-sptool: ${SPTOOL}
-${SPTOOL}: FORCE
-	${Q}${MAKE} CPPFLAGS="-DVERSION='\"${VERSION_STRING}\"'" SPTOOL=${SPTOOL} --no-print-directory -C ${SPTOOLPATH}
-
 romlib.bin: libraries FORCE
 	${Q}${MAKE} PLAT_DIR=${PLAT_DIR} BUILD_PLAT=${BUILD_PLAT} ENABLE_BTI=${ENABLE_BTI} ARM_ARCH_MINOR=${ARM_ARCH_MINOR} INCLUDES='${INCLUDES}' DEFINES='${DEFINES}' --no-print-directory -C ${ROMLIBPATH} all
 

diff --git a/bl1/aarch32/bl1_exceptions.S b/bl1/aarch32/bl1_exceptions.S
index 493d2ca..4a6815f 100644
--- a/bl1/aarch32/bl1_exceptions.S
+++ b/bl1/aarch32/bl1_exceptions.S

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -118,6 +118,14 @@
 	mov	r0, #DISABLE_DCACHE
 	bl	enable_mmu_svc_mon
 
+	/*
+	 * Invalidate `smc_ctx_t` in data cache to prevent dirty data being
+	 * used.
+	 */
+	mov	r0, r6
+	mov	r1, #SMC_CTX_SIZE
+	bl	inv_dcache_range
+
 	/* Enable the data cache. */
 	ldcopr	r9, SCTLR
 	orr	r9, r9, #SCTLR_C_BIT

diff --git a/bl2/bl2_el3.ld.S b/bl2/bl2_el3.ld.S
index 6aa7afd..c95706c 100644
--- a/bl2/bl2_el3.ld.S
+++ b/bl2/bl2_el3.ld.S

@@ -17,12 +17,12 @@
     RAM (rwx): ORIGIN = BL2_RW_BASE, LENGTH = BL2_RW_LIMIT - BL2_RW_BASE
 #else
     RAM (rwx): ORIGIN = BL2_BASE, LENGTH = BL2_LIMIT - BL2_BASE
+#endif
 #if SEPARATE_BL2_NOLOAD_REGION
     RAM_NOLOAD (rw!a): ORIGIN = BL2_NOLOAD_START, LENGTH = BL2_NOLOAD_LIMIT - BL2_NOLOAD_START
 #else
 #define RAM_NOLOAD RAM
 #endif
-#endif
 }
 
 #if !BL2_IN_XIP_MEM

diff --git a/bl31/aarch64/bl31_entrypoint.S b/bl31/aarch64/bl31_entrypoint.S
index ed05864..b0c46dc 100644
--- a/bl31/aarch64/bl31_entrypoint.S
+++ b/bl31/aarch64/bl31_entrypoint.S

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -67,6 +67,7 @@
 		_exception_vectors=runtime_exceptions		\
 		_pie_fixup_size=BL31_LIMIT - BL31_BASE
 
+#if !RESET_TO_BL31_WITH_PARAMS
 	/* ---------------------------------------------------------------------
 	 * For RESET_TO_BL31 systems, BL31 is the first bootloader to run so
 	 * there's no argument to relay from a previous bootloader. Zero the
@@ -77,6 +78,7 @@
 	mov	x21, 0
 	mov	x22, 0
 	mov	x23, 0
+#endif /* RESET_TO_BL31_WITH_PARAMS */
 #endif /* RESET_TO_BL31 */
 
 	/* --------------------------------------------------------------------

diff --git a/bl31/bl31.mk b/bl31/bl31.mk
index 214cf2f..3964469 100644
--- a/bl31/bl31.mk
+++ b/bl31/bl31.mk

@@ -115,6 +115,10 @@
 BL31_SOURCES		+=	lib/extensions/trbe/trbe.c
 endif
 
+ifeq (${ENABLE_BRBE_FOR_NS},1)
+BL31_SOURCES		+=	lib/extensions/brbe/brbe.c
+endif
+
 ifeq (${ENABLE_SYS_REG_TRACE_FOR_NS},1)
 BL31_SOURCES		+=      lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
 endif

diff --git a/bl32/sp_min/sp_min.mk b/bl32/sp_min/sp_min.mk
index 590b032..ab1287d 100644
--- a/bl32/sp_min/sp_min.mk
+++ b/bl32/sp_min/sp_min.mk

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2016-2022, Arm Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -35,6 +35,10 @@
 ifeq (${WORKAROUND_CVE_2017_5715},1)
 BL32_SOURCES		+=	bl32/sp_min/wa_cve_2017_5715_bpiall.S	\
 				bl32/sp_min/wa_cve_2017_5715_icache_inv.S
+else
+ifeq (${WORKAROUND_CVE_2022_23960},1)
+BL32_SOURCES		+=	bl32/sp_min/wa_cve_2017_5715_icache_inv.S
+endif
 endif
 
 ifeq (${TRNG_SUPPORT},1)

diff --git a/changelog.yaml b/changelog.yaml
index add81ef..01e91b4 100644
--- a/changelog.yaml
+++ b/changelog.yaml

@@ -89,6 +89,9 @@
       - title: Activity Monitors Extension (FEAT_AMU)
         scope: amu
 
+      - title: Confidential Compute Architecture (CCA)
+        scope: cca
+
       - title: Support for the `HCRX_EL2` register (FEAT_HCX)
         scope: hcx
 
@@ -116,6 +119,12 @@
       - title: Statistical profiling Extension (FEAT_SPE)
         scope: spe
 
+      - title: Branch Record Buffer Extension (FEAT_BRBE)
+        scope: brbe
+
+      - title: Extended Cache Index (FEAT_CCIDX)
+        scope: ccidx
+
   - title: Platforms
 
     subsections:
@@ -158,6 +167,9 @@
           - title: Morello
             scope: morello
 
+          - title: N1SDP
+            scope: n1sdp
+
           - title: RD
             scope: rd
 
@@ -185,6 +197,12 @@
                 deprecated:
                   - plat/tc0
 
+          - title: Corstone-1000
+            scope: corstone-1000
+
+      - title: Broadcom
+        scope: brcm
+
       - title: Intel
         scope: intel
 
@@ -246,6 +264,12 @@
               - plat/mediatek/mt8195
               - plat/mdeiatek/mt8195
 
+          - title: MT8186
+            scope: mt8186
+
+            deprecated:
+              - plat/mediatek/mt8186
+
       - title: NVIDIA
         scope: nvidia
 
@@ -260,6 +284,9 @@
               - title: Tegra 132
                 scope: tegra132
 
+              - title: Tegra 194
+                scope: tegra194
+
       - title: NXP
         scope: nxp
 
@@ -284,6 +311,12 @@
                   - plat/imx/imx8m
 
                 subsections:
+                  - title: i.MX 8M Nano
+                    scope: imx8mn
+
+                    deprecated:
+                      - plat/imx/imx8m/imx8mn
+
                   - title: i.MX 8M Mini
                     scope: imx8mm
 
@@ -296,6 +329,12 @@
                     deprecated:
                       - plat/imx/imx8m/imx8mp
 
+                  - title: i.MX 8Q
+                    scope: imx8mq
+
+                    deprecated:
+                      - plat/imx/imx8m/imx8mq
+
           - title: Layerscape
             scope: layerscape
 
@@ -316,6 +355,19 @@
                     deprecated:
                       - plat/nxp/ls1028ardb
 
+              - title: LS1043A
+                scope: ls1043a
+
+                deprecated:
+                  - plat/nxp/ls1043a
+
+                subsections:
+                  - title: LS1043ARDB
+                    scope: ls1043ardb
+
+                    deprecated:
+                      - plat/nxp/ls1043ardb
+
               - title: LX2
                 scope: lx2
 
@@ -436,12 +488,19 @@
           - plat/st
 
         subsections:
-          - title: ST32MP1
+          - title: STM32MP1
             scope: stm32mp1
 
             deprecated:
               - plat/st/stm32mp1
 
+            subsections:
+              - title: STM32MP13
+                scope: stm32mp13
+
+              - title: STM32MP15
+                scope: stm32mp15
+
       - title: Texas Instruments
         scope: ti
 
@@ -484,6 +543,9 @@
       - title: BL2
         scope: bl2
 
+      - title: BL31
+        scope: bl31
+
   - title: Services
     scope: services
 
@@ -497,16 +559,25 @@
       - title: RME
         scope: rme
 
+        subsections:
+          - title: TRP
+            scope: trp
+
+          - title: RMMD
+            scope: rmmd
+
       - title: SPM
         scope: spm
 
-        deprecated:
-          - spmc
-          - spmd
-          - SPMD
+        subsections:
+          - title: EL3 SPMC
+            scope: el3-spmc
+
+          - title: SPMD
+            scope: spmd
 
-      - title: SPM MM
-        scope: spm-mm
+          - title: SPM MM
+            scope: spm-mm
 
   - title: Libraries
 
@@ -525,6 +596,10 @@
         deprecated:
           - el3_runtime
 
+        subsections:
+          - title: Context Management
+            scope: cm
+
       - title: FCONF
         scope: fconf
 
@@ -552,6 +627,18 @@
       - title: Translation Tables
         scope: xlat
 
+      - title: C Standard Library
+        scope: libc
+
+      - title: Locks
+        scope: locks
+
+      - title: PSA
+        scope: psa
+
+      - title: Context Management
+        scope: context mgmt
+
   - title: Drivers
 
     subsections:
@@ -565,6 +652,9 @@
           - title: CryptoCell-713
             scope: cc-713
 
+      - title: Generic Clock
+        scope: clk
+
       - title: FWU
         scope: fwu
 
@@ -611,8 +701,8 @@
                 deprecated:
                   - spi_nand
 
-      - title: Partition
-        scope: partition
+      - title: GUID Partition Tables Support
+        scope: guid-partition
 
       - title: SCMI
         scope: scmi
@@ -645,6 +735,21 @@
                   - title: GIC-600AE
                     scope: gic600ae
 
+          - title: SMMU
+            scope: smmu
+
+          - title: MHU
+            scope: mhu
+
+            deprecated:
+              - drivers/arm/mhu
+
+          - title: RSS
+            scope: rss
+
+            deprecated:
+              - drivers/arm/rss
+
           - title: TZC
             scope: tzc
 
@@ -655,6 +760,12 @@
                 deprecated:
                   - drivers/tzc400
 
+              - title: TZC-380
+                scope: tzc380
+
+                deprecated:
+                  - drivers/tzc380
+
       - title: Marvell
         scope: marvell-drivers
 
@@ -771,6 +882,18 @@
           - title: GIC
             scope: nxp-gic
 
+          - title: CSU
+            scope: nxp-csu
+
+          - title: IFC NAND
+            scope: nxp-ifc-nand
+
+          - title: IFC NOR
+            scope: nxp-ifc-nor
+
+          - title: TZC-380
+            scope: nxp-tzc380
+
       - title: Renesas
         scope: renesas-drivers
 
@@ -816,9 +939,6 @@
                   - io-stm32image
                   - io_stm32image
 
-              - title: fiptool
-                scope: fiptool
-
           - title: I2C
             scope: st-i2c
 
@@ -984,6 +1104,9 @@
       - title: Prerequisites
         scope: prerequisites
 
+      - title: Threat Model
+        scope: threat-model
+
   - title: Build System
     scope: build
 
@@ -1007,6 +1130,15 @@
       - title: NXP Tools
         scope: nxp-tools
 
+      - title: Firmware Image Package Tool
+        scope: fiptool
+
+      - title: Secure Partition Tool
+        scope: sptool
+
+      - title: Certificate Creation Tool
+        scope: cert-create
+
   - title: Dependencies
     scope: deps
 

diff --git a/common/fdt_fixup.c b/common/fdt_fixup.c
index 0ab50d8..1bad74f 100644
--- a/common/fdt_fixup.c
+++ b/common/fdt_fixup.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -394,6 +394,110 @@
 	return offs;
 }
 
+/*******************************************************************************
+ * fdt_add_cpu_idle_states() - add PSCI CPU idle states to cpu nodes in the DT
+ * @dtb:	pointer to the device tree blob in memory
+ * @states:	array of idle state descriptions, ending with empty element
+ *
+ * Add information about CPU idle states to the devicetree. This function
+ * assumes that CPU idle states are not already present in the devicetree, and
+ * that all CPU states are equally applicable to all CPUs.
+ *
+ * See arm/idle-states.yaml and arm/psci.yaml in the (Linux kernel) DT binding
+ * documentation for more details.
+ *
+ * Return: 0 on success, a negative error value otherwise.
+ ******************************************************************************/
+int fdt_add_cpu_idle_states(void *dtb, const struct psci_cpu_idle_state *state)
+{
+	int cpu_node, cpus_node, idle_states_node, ret;
+	uint32_t count, phandle;
+
+	ret = fdt_find_max_phandle(dtb, &phandle);
+	phandle++;
+	if (ret < 0) {
+		return ret;
+	}
+
+	cpus_node = fdt_path_offset(dtb, "/cpus");
+	if (cpus_node < 0) {
+		return cpus_node;
+	}
+
+	/* Create the idle-states node and its child nodes. */
+	idle_states_node = fdt_add_subnode(dtb, cpus_node, "idle-states");
+	if (idle_states_node < 0) {
+		return idle_states_node;
+	}
+
+	ret = fdt_setprop_string(dtb, idle_states_node, "entry-method", "psci");
+	if (ret < 0) {
+		return ret;
+	}
+
+	for (count = 0U; state->name != NULL; count++, phandle++, state++) {
+		int idle_state_node;
+
+		idle_state_node = fdt_add_subnode(dtb, idle_states_node,
+						  state->name);
+		if (idle_state_node < 0) {
+			return idle_state_node;
+		}
+
+		fdt_setprop_string(dtb, idle_state_node, "compatible",
+				   "arm,idle-state");
+		fdt_setprop_u32(dtb, idle_state_node, "arm,psci-suspend-param",
+				state->power_state);
+		if (state->local_timer_stop) {
+			fdt_setprop_empty(dtb, idle_state_node,
+					  "local-timer-stop");
+		}
+		fdt_setprop_u32(dtb, idle_state_node, "entry-latency-us",
+				state->entry_latency_us);
+		fdt_setprop_u32(dtb, idle_state_node, "exit-latency-us",
+				state->exit_latency_us);
+		fdt_setprop_u32(dtb, idle_state_node, "min-residency-us",
+				state->min_residency_us);
+		if (state->wakeup_latency_us) {
+			fdt_setprop_u32(dtb, idle_state_node,
+					"wakeup-latency-us",
+					state->wakeup_latency_us);
+		}
+		fdt_setprop_u32(dtb, idle_state_node, "phandle", phandle);
+	}
+
+	if (count == 0U) {
+		return 0;
+	}
+
+	/* Link each cpu node to the idle state nodes. */
+	fdt_for_each_subnode(cpu_node, dtb, cpus_node) {
+		const char *device_type;
+		fdt32_t *value;
+
+		/* Only process child nodes with device_type = "cpu". */
+		device_type = fdt_getprop(dtb, cpu_node, "device_type", NULL);
+		if (device_type == NULL || strcmp(device_type, "cpu") != 0) {
+			continue;
+		}
+
+		/* Allocate space for the list of phandles. */
+		ret = fdt_setprop_placeholder(dtb, cpu_node, "cpu-idle-states",
+					      count * sizeof(phandle),
+					      (void **)&value);
+		if (ret < 0) {
+			return ret;
+		}
+
+		/* Fill in the phandles of the idle state nodes. */
+		for (uint32_t i = 0U; i < count; ++i) {
+			value[i] = cpu_to_fdt32(phandle - count + i);
+		}
+	}
+
+	return 0;
+}
+
 /**
  * fdt_adjust_gic_redist() - Adjust GICv3 redistributor size
  * @dtb: Pointer to the DT blob in memory

diff --git a/common/fdt_wrappers.c b/common/fdt_wrappers.c
index 2a9673f..1b065b1 100644
--- a/common/fdt_wrappers.c
+++ b/common/fdt_wrappers.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -618,3 +618,24 @@
 
 	return ret;
 }
+
+/*
+ * Find a given node in device tree. If not present, add it.
+ * Returns offset of node found/added on success, and < 0 on error.
+ */
+int fdtw_find_or_add_subnode(void *fdt, int parentoffset, const char *name)
+{
+	int offset;
+
+	offset = fdt_subnode_offset(fdt, parentoffset, name);
+
+	if (offset == -FDT_ERR_NOTFOUND) {
+		offset = fdt_add_subnode(fdt, parentoffset, name);
+	}
+
+	if (offset < 0) {
+		ERROR("%s: %s: %s\n", __func__, name, fdt_strerror(offset));
+	}
+
+	return offset;
+}

diff --git a/common/feat_detect.c b/common/feat_detect.c
index ef09b86..be3e20e 100644
--- a/common/feat_detect.c
+++ b/common/feat_detect.c

@@ -203,6 +203,16 @@
 #endif
 }
 
+/***********************************************************
+ * Feature : FEAT_TWED (Delayed Trapping of WFE Instruction)
+ **********************************************************/
+static void read_feat_twed(void)
+{
+#if (ENABLE_FEAT_TWED == FEAT_STATE_1)
+	feat_detect_panic(is_armv8_6_twed_present(), "TWED");
+#endif
+}
+
 /******************************************************************
  * Feature : FEAT_HCX (Extended Hypervisor Configuration Register)
  *****************************************************************/
@@ -224,6 +234,26 @@
 #endif
 }
 
+/******************************************************
+ * Feature : FEAT_BRBE (Branch Record Buffer Extension)
+ *****************************************************/
+static void read_feat_brbe(void)
+{
+#if (ENABLE_BRBE_FOR_NS == FEAT_STATE_1)
+	feat_detect_panic(is_feat_brbe_present(), "BRBE");
+#endif
+}
+
+/******************************************************
+ * Feature : FEAT_TRBE (Trace Buffer Extension)
+ *****************************************************/
+static void read_feat_trbe(void)
+{
+#if (ENABLE_TRBE_FOR_NS == FEAT_STATE_1)
+	feat_detect_panic(is_feat_trbe_present(), "TRBE");
+#endif
+}
+
 /***********************************************************************************
  * TF-A supports many Arm architectural features starting from arch version
  * (8.0 till 8.7+). These features are mostly enabled through build flags. This
@@ -279,10 +309,15 @@
 	read_feat_amuv1p1();
 	read_feat_fgt();
 	read_feat_ecv();
+	read_feat_twed();
 
 	/* v8.7 features */
 	read_feat_hcx();
 
+	/* v9.0 features */
+	read_feat_brbe();
+	read_feat_trbe();
+
 	/* v9.2 features */
 	read_feat_rme();
 }

diff --git a/common/uuid.c b/common/uuid.c
index ac6db50..3e47eb4 100644
--- a/common/uuid.c
+++ b/common/uuid.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -132,3 +132,27 @@
 	return 0;
 }
 
+/*
+ * Helper function to check if 2 UUIDs match.
+ */
+bool uuid_match(uint32_t *uuid1, uint32_t *uuid2)
+{
+	return !memcmp(uuid1, uuid2, sizeof(uint32_t) * 4);
+}
+
+/*
+ * Helper function to copy from one UUID struct to another.
+ */
+void copy_uuid(uint32_t *to_uuid, uint32_t *from_uuid)
+{
+	to_uuid[0] = from_uuid[0];
+	to_uuid[1] = from_uuid[1];
+	to_uuid[2] = from_uuid[2];
+	to_uuid[3] = from_uuid[3];
+}
+
+bool is_null_uuid(uint32_t *uuid)
+{
+	return (uuid[0] == 0 && uuid[1] == 0 &&
+		uuid[2] == 0 && uuid[3] == 0);
+}

diff --git a/docs/about/maintainers.rst b/docs/about/maintainers.rst
index c6c3e22..d16b5dd 100644
--- a/docs/about/maintainers.rst
+++ b/docs/about/maintainers.rst

@@ -48,6 +48,8 @@
 :|G|: `madhukar-Arm`_
 :|M|: Raghu Krishnamurthy <raghu.ncstate@icloud.com>
 :|G|: `raghuncstate`_
+:|M|: Manish Badarkhe <manish.badarkhe@arm.com>
+:|G|: `ManishVB-Arm`_
 
 
 .. _code owners:
@@ -75,8 +77,6 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Mark Dykes <mark.dykes@arm.com>
 :|G|: `mardyk01`_
-:|M|: John Powell <john.powell@arm.com>
-:|G|: `john-powell-arm`_
 :|F|: services/std_svc/sdei/
 
 Trusted Boot
@@ -89,8 +89,14 @@
 :|G|: `ManishVB-Arm`_
 :|F|: drivers/auth/
 
-Secure Partition Manager (SPM)
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Secure Partition Manager Core (EL3 FF-A SPMC)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+:|M|: Marc Bonnici <marc.bonnici@arm.com>
+:|G|: `marcbonnici`_
+:|F|: services/std_svc/spm/el3_spmc/\*
+
+Secure Partition Manager Dispatcher (SPMD)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Olivier Deprez <olivier.deprez@arm.com>
 :|G|: `odeprez`_
 :|M|: Manish Pandey <manish.pandey2@arm.com>
@@ -99,26 +105,30 @@
 :|G|: `max-shvetsov`_
 :|M|: Joao Alves <Joao.Alves@arm.com>
 :|G|: `J-Alves`_
-:|F|: services/std_svc/spm\*
+:|F|: services/std_svc/spmd/\*
 
 Exception Handling Framework (EHF)
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Manish Badarkhe <manish.badarkhe@arm.com>
 :|G|: `ManishVB-Arm`_
-:|M|: John Powell <john.powell@arm.com>
-:|G|: `john-powell-arm`_
 :|F|: bl31/ehf.c
 
+Realm Management Monitor Dispatcher (RMMD)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+:|M|: Javier Almansa Sobrino <javier.almansasobrino@arm.com>
+:|G|: `javieralso-arm`_
+:|F|: services/std_svc/rmmd/\*
+:|F|: include/services/rmmd_svc.h
+:|F|: include/services/rmm_core_manifest.h
+
 Realm Management Extension (RME)
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Bipin Ravi <bipin.ravi@arm.com>
 :|G|: `bipinravi-arm`_
 :|M|: Mark Dykes <mark.dykes@arm.com>
 :|G|: `mardyk01`_
-:|M|: John Powell <john.powell@arm.com>
-:|G|: `john-powell-arm`_
-:|M|: Zelalem Aweke <Zelalem.Aweke@arm.com>
-:|G|: `zelalem-aweke`_
+:|M|: Javier Almansa Sobrino <javier.almansasobrino@arm.com>
+:|G|: `javieralso-arm`_
 
 Drivers, Libraries and Framework Code
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
@@ -193,16 +203,12 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Joao Alves <Joao.Alves@arm.com>
 :|G|: `J-Alves`_
-:|M|: Jimmy Brisson <Jimmy.Brisson@arm.com>
-:|G|: `theotherjimmy`_
 :|F|: lib/pmf/
 
 Arm CPU libraries
 ^^^^^^^^^^^^^^^^^
 :|M|: Lauren Wehrmeister <Lauren.Wehrmeister@arm.com>
 :|G|: `laurenw-arm`_
-:|M|: John Powell <john.powell@arm.com>
-:|G|: `john-powell-arm`_
 :|F|: lib/cpus/
 
 Reliability Availability Serviceabilty (RAS) framework
@@ -225,8 +231,6 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Zelalem Aweke <Zelalem.Aweke@arm.com>
 :|G|: `zelalem-aweke`_
-:|M|: Jimmy Brisson <Jimmy.Brisson@arm.com>
-:|G|: `theotherjimmy`_
 :|F|: lib/extensions/mpam/
 
 Pointer Authentication (PAuth) and Branch Target Identification (BTI) extensions
@@ -241,22 +245,12 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Zelalem Aweke <Zelalem.Aweke@arm.com>
 :|G|: `zelalem-aweke`_
-:|M|: Jimmy Brisson <Jimmy.Brisson@arm.com>
-:|G|: `theotherjimmy`_
 :|F|: lib/extensions/spe/
 
-Scalable Vector Extension (SVE)
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-:|M|: Jimmy Brisson <Jimmy.Brisson@arm.com>
-:|G|: `theotherjimmy`_
-:|F|: lib/extensions/sve/
-
 Standard C library
 ^^^^^^^^^^^^^^^^^^
 :|M|: Alexei Fedorov <Alexei.Fedorov@arm.com>
 :|G|: `AlexeiFedorov`_
-:|M|: John Powell <john.powell@arm.com>
-:|G|: `john-powell-arm`_
 :|F|: lib/libc/
 
 Library At ROM (ROMlib)
@@ -293,6 +287,20 @@
 :|G|: `odeprez`_
 :|F|: drivers/arm/gic/
 
+Message Handling Unit (MHU) driver
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+:|M|: David Vincze <david.vincze@arm.com>
+:|G|: `davidvincze`_
+:|F|: include/drivers/arm/mhu.h
+:|F|: drivers/arm/mhu
+
+Runtime Security Subsystem (RSS) comms driver
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+:|M|: David Vincze <david.vincze@arm.com>
+:|G|: `davidvincze`_
+:|F|: include/drivers/arm/rss_comms.h
+:|F|: drivers/arm/rss
+
 Libfdt wrappers
 ^^^^^^^^^^^^^^^
 :|M|: Madhukar Pappireddy <Madhukar.Pappireddy@arm.com>
@@ -315,9 +323,28 @@
 :|G|: `AlexeiFedorov`_
 :|M|: Javier Almansa Sobrino <Javier.AlmansaSobrino@arm.com>
 :|G|: `javieralso-arm`_
+:|M|: Sandrine Bailleux <sandrine.bailleux@arm.com>
+:|G|: `sandrine-bailleux-arm`_
 :|F|: drivers/measured_boot
 :|F|: include/drivers/measured_boot
-:|F|: plat/arm/board/fvp/fvp_measured_boot.c
+:|F|: docs/components/measured_boot
+:|F|: plat/arm/board/fvp/fvp\*_measured_boot.c
+
+PSA Firmware Update
+^^^^^^^^^^^^^^^^^^^
+:|M|: Manish Badarkhe <manish.badarkhe@arm.com>
+:|G|: `ManishVB-Arm`_
+:|M|: Sandrine Bailleux <sandrine.bailleux@arm.com>
+:|G|: `sandrine-bailleux-arm`_
+:|F|: drivers/fwu
+:|F|: include/drivers/fwu
+
+Platform Security Architecture (PSA) APIs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+:|M|: Sandrine Bailleux <sandrine.bailleux@arm.com>
+:|G|: `sandrine-bailleux-arm`_
+:|F|: include/lib/psa
+:|F|: lib/psa
 
 System Control and Management Interface (SCMI) Server
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -339,8 +366,6 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Mark Dykes <mark.dykes@arm.com>
 :|G|: `mardyk01`_
-:|M|: John Powell <john.powell@arm.com>
-:|G|: `john-powell-arm`_
 :|F|: lib/gpt_rme
 :|F|: include/lib/gpt_rme
 
@@ -720,16 +745,26 @@
 :|F|: docs/components/spd/optee-dispatcher.rst
 :|F|: services/spd/opteed/
 
-TLK/Trusty secure payloads
+TLK
 ^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Varun Wadekar <vwadekar@nvidia.com>
 :|G|: `vwadekar`_
 :|F|: docs/components/spd/tlk-dispatcher.rst
-:|F|: docs/components/spd/trusty-dispatcher.rst
 :|F|: include/bl32/payloads/tlk.h
 :|F|: services/spd/tlkd/
+
+Trusty secure payloads
+^^^^^^^^^^^^^^^^^^^^^^^^^^
+:|M|: Arve Hjønnevåg <arve@android.com>
+:|G|: `arve-android`_
+:|M|: Marco Nelissen <marcone@google.com>
+:|G|: `marcone`_
+:|M|: Varun Wadekar <vwadekar@nvidia.com>
+:|G|: `vwadekar`_
+:|F|: docs/components/spd/trusty-dispatcher.rst
 :|F|: services/spd/trusty/
 
+
 Test Secure Payload (TSP)
 ^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Manish Badarkhe <manish.badarkhe@arm.com>
@@ -762,6 +797,8 @@
 ^^^^^^
 :|M|: Manish Pandey <manish.pandey2@arm.com>
 :|G|: `manish-pandey-arm`_
+:|M|: Joao Alves <Joao.Alves@arm.com>
+:|G|: `J-Alves`_
 :|F|: tools/sptool/
 
 Build system
@@ -799,6 +836,7 @@
 .. _b49020: https://github.com/b49020
 .. _carlocaione: https://github.com/carlocaione
 .. _danh-arm: https://github.com/danh-arm
+.. _davidvincze: https://github.com/davidvincze
 .. _etienne-lms: https://github.com/etienne-lms
 .. _glneo: https://github.com/glneo
 .. _grandpaul: https://github.com/grandpaul
@@ -846,10 +884,8 @@
 .. _javieralso-arm: https://github.com/javieralso-arm
 .. _laurenw-arm: https://github.com/laurenw-arm
 .. _zelalem-aweke: https://github.com/zelalem-aweke
-.. _theotherjimmy: https://github.com/theotherjimmy
 .. _J-Alves: https://github.com/J-Alves
 .. _madhukar-Arm: https://github.com/madhukar-Arm
-.. _john-powell-arm: https://github.com/john-powell-arm
 .. _raghuncstate: https://github.com/raghuncstate
 .. _CJKay: https://github.com/cjkay
 .. _nmenon: https://github.com/nmenon
@@ -862,5 +898,8 @@
 .. _uarif1: https://github.com/uarif1
 .. _pangupta: https://github.com/pangupta
 .. _JiafeiPan: https://github.com/JiafeiPan
+.. _arve-android: https://github.com/arve-android
+.. _marcone: https://github.com/marcone
+.. _marcbonnici: https://github.com/marcbonnici
 
 .. _Project Maintenance Process: https://developer.trustedfirmware.org/w/collaboration/project-maintenance-process/

diff --git a/docs/about/release-information.rst b/docs/about/release-information.rst
index b3553ae..e9eaa80 100644
--- a/docs/about/release-information.rst
+++ b/docs/about/release-information.rst

@@ -48,7 +48,9 @@
 +-----------------+---------------------------+------------------------------+
 | v2.6            | 4th week of Nov '21       | 2nd week of Nov '21          |
 +-----------------+---------------------------+------------------------------+
-| v2.7            | 2nd week of May '22       | 4th week of Apr '22          |
+| v2.7            | 5th week of May '22       | 3rd week of May '22          |
++-----------------+---------------------------+------------------------------+
+| v2.8            | 5th week of Nov '22       | 3rd week of Nov '22          |
 +-----------------+---------------------------+------------------------------+
 
 Removal of Deprecated Interfaces

diff --git a/docs/change-log.md b/docs/change-log.md
index ab50968..8a555ec 100644
--- a/docs/change-log.md
+++ b/docs/change-log.md

@@ -3,6 +3,1196 @@
 This document contains a summary of the new features, changes, fixes and known
 issues in each release of Trusted Firmware-A.
 
+## [2.7.0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/refs/tags/v2.6..refs/tags/v2.7.0) (2022-05-20)
+
+### New Features
+
+- **Architecture**
+
+  - **Statistical profiling Extension (FEAT_SPE)**
+
+    - add support for FEAT_SPEv1p2 ([f20eb89](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f20eb893a072bb9b404eedb886e8c65fe76ffb45))
+
+  - **Branch Record Buffer Extension (FEAT_BRBE)**
+
+    - add BRBE support for NS world ([744ad97](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/744ad97445ce7aa65adaef376d0b5bafc12a90d3))
+
+  - **Extended Cache Index (FEAT_CCIDX)**
+
+    - update the do_dcsw_op function to support FEAT_CCIDX ([d0ec1cc](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d0ec1cc437c59e64ecba44710dbce82a04ff892d))
+
+- **Platforms**
+
+  - add SZ_* macros ([1af59c4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1af59c457010e6e3e6536752736eb02115bca543))
+
+  - **Allwinner**
+
+    - add SMCCC SOCID support ([436cd75](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/436cd754f2b0f9c0ce3094961bd1e179eeff2fc1))
+    - allow to skip PMIC regulator setup ([67412e4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/67412e4d7ae3defaac78ef5e351c63e06cfd907a))
+    - apx803: add aldo1 regulator ([a29f6e7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a29f6e76cbf76d509c00f84f068b59864d210dfd))
+    - choose PSCI states to avoid translation ([159c36f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/159c36fd2fc5afbe979e5028b9e845ed4b7a40f1))
+    - provide CPU idle states to the rich OS ([e2b1877](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e2b18771fc2a0528dda18dbdaac08dd8530df25a))
+    - simplify CPU_SUSPEND power state encoding ([52466ec](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/52466ec38ef312da62ad062720a03a183329f831))
+
+  - **Arm**
+
+    - **FVP**
+
+      - measure critical data ([cf21064](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/cf21064ec8a1889f64de48e30e38285227d27745))
+      - update HW_CONFIG DT loading mechanism ([39f0b86](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/39f0b86a76534d0b7c71dd0c8b34f1a74480386b))
+      - enable RSS backend based measured boot ([c44e50b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c44e50b72567205650c6455f3a258f36af0c84dd))
+
+    - **Morello**
+
+      - add changes to enable TBBR boot ([4af5397](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/4af53977533bee7b5763d3efad1448545c2ebef7))
+      - add DTS for Morello SoC platform ([572c8ce](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/572c8ce255397f7cff9640676e510817a8e4c6a3))
+      - add support for nt_fw_config ([6ad6465](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6ad6465e5ce452688cac079f16d26f64e9f4ce3c))
+      - add TARGET_PLATFORM flag ([8840711](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/8840711f33131969ec6b62ca3da079cf0573ac8b))
+      - configure DMC-Bing mode ([9b8c431](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9b8c431e2b2d656da7f8c4158e3d32e104446fec))
+      - expose scmi protocols in fdts ([87639aa](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/87639aab0b6a30d4f49d069c0ea06900b11072a6))
+      - split platform_info sds struct ([4a7a9da](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/4a7a9dafbc953089957a0cc1a7183731a5b003e1))
+      - zero out the DDR memory space ([2d39b39](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2d39b39704c1e4f2a189543ac4ff05ae58e5f5c8))
+
+    - **N1SDP**
+
+      - add support for nt_fw_config ([cf85030](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/cf85030efe73439e06295f8185b0a6bebf7b5eae))
+      - enable trusted board boot on n1sdp ([fe2b37f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/fe2b37f6858168a56c3d393bc72f560468d02165))
+
+    - **RD**
+
+      - **RD-N2**
+
+        - add board support for rdn2cfg2 variant ([efeb438](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/efeb43808d2e3ed23e1d51d5e86460db92971e96))
+        - add support for rdedmunds variant ([ef515f0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ef515f0d3466a8beded4fd662718abbd97391b13))
+
+    - **SGI**
+
+      - add page table translation entry for secure uart ([33d10ac](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/33d10ac8bf134519f303fd7ce5fb5d583be2f515))
+      - deviate from arm css common uart related definitions ([f2cccca](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f2ccccaa81ec14a80fedb48c37226e5d852ada7a))
+      - enable fpregs context save and restore ([18fa43f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/18fa43f753b79cfc3cc5426a3ef50b04efbf6206))
+      - route TF-A logs via secure uart ([987e2b7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/987e2b7c20eb4ab4215ff5289b715300f5cec054))
+
+    - **TC**
+
+      - add reserved memory region for Gralloc ([ad60a42](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ad60a42cd79713984065dca8540c091c49755f32))
+      - enable CI-700 PMU for profiling ([fbfc598](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/fbfc59840f9cd0ea53921c7f6fb9f4850a3b42ee))
+      - enable GPU ([82117bb](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/82117bb48180175c25936b0ff9e33563e25e18f4))
+      - enable SMMU for DPU ([4a6ebee](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/4a6ebeeca37ece34a58982c8b6ebdc8cfd70814b))
+      - enable tracing ([59da207](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/59da207e2f2f028c9051c89bc5a05e95d996c18c))
+
+    - **Corstone-1000**
+
+      - identify bank to load fip ([cf89fd5](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/cf89fd57ed3286d7842eef41cd72a3977eb6d317))
+      - implement platform specific psci reset ([a599c80](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a599c80d063975cbeedbc86cfb619fca8545c487))
+      - made changes to accommodate 3MB for optee ([854d1c1](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/854d1c103a9b73bbde7ef1b89b06b29e3cc053bb))
+
+  - **Intel**
+
+    - add macro to switch between different UART PORT ([447e699](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/447e699f70f1a1d1b85a8136b445eba689166c5d))
+    - add RSU 'Max Retry' SiP SMC services ([4c26957](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/4c26957be253a7ab3acb316f42bf3ee10c409ed2))
+    - add SiP service for DCMF status ([984e236](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/984e236e0dee46708534a23c637271a931ceb67e))
+    - add SMC for enquiring firmware version ([c34b2a7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c34b2a7a1a38dba88b6b668a81bd07c757525830))
+    - add SMC support for Get USERCODE ([93a5b97](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/93a5b97ec9e97207769db18ae34886e6b8bf2ea4))
+    - add SMC support for HWMON voltage and temp sensor ([52cf9c2](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/52cf9c2cd4882534d02e8996e4ff1143ee59290e))
+    - add SMC support for ROM Patch SHA384 mailbox ([77902fc](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/77902fca8fe7449473b09198e1fe197f7b4765d7))
+    - add SMC/PSCI services for DCMF version support ([44eb782](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/44eb782e15c9af532f2455b37bd53ca93830f6e2))
+    - add SMPLSEL and DRVSEL setup for Stratix 10 MMC ([bb0fcc7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/bb0fcc7e011ec4319a79734ba44353015860e39f))
+    - add support for F2S and S2F bridge SMC with mask to enable, disable and reset bridge ([11f4f03](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/11f4f03043ef05762f4d6337804c39dc8f9af54f))
+    - allow to access all register addresses if DEBUG=1 ([7e954df](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7e954dfc2ba83262f7596dd0f17de75163e49e5e))
+    - create source file for firewall configuration ([afa0b1a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/afa0b1a82a404c616da2da8f52cdcd587938955f))
+    - enable firewall for OCRAM in BL31 ([ae19fef](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ae19fef33707700a91b0b672aa784e084a6ca500))
+    - enable SMC SoC FPGA bridges enable/disable ([b7f3044](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b7f3044e8725d9af997999547630892cf9e2f0ad))
+    - extend attestation service to Agilex family ([581182c](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/581182c1916df03860744d8e32941c72b2cc3fda))
+    - implement timer init divider via cpu frequency. ([#1](https://review.trustedfirmware.org:29418/TF-A/trusted-firmware-a/issues/1)) ([f65bdf3](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f65bdf3a54eed8f7651761c25bf6cc7437f4474b))
+    - initial commit for attestation service ([d174083](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d17408316db10db611e23716e8a5b9b9f53ad509))
+    - single certificate feature enablement ([7facace](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7facacec6328e505b243a4974d045d45fe068afd))
+    - support AES Crypt Service ([6726390](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6726390eb02e9659cfaf2d3598be9bf12fbc5901))
+    - support crypto service key operation ([342a061](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/342a0618c7ff89327ac5b34dc0713509ffae609b))
+    - support crypto service session ([6dc00c2](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6dc00c24ab0100a2aae0f416c72470f8ed17e149))
+    - support ECDH request ([4944686](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/49446866a515c2db855d456f39df3d586b2084b7))
+    - support ECDSA Get Public Key ([d2fee94](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d2fee94afa6ba7e76508e6bead7eb2936c5eafb8))
+    - support ECDSA HASH Signing ([6925410](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/692541051b8cb0f435ae46c5d7351231ee292319))
+    - support ECDSA HASH Verification ([7e25eb8](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7e25eb87016ba8355cf0a3a5f71fb8b8785de044))
+    - support ECDSA SHA-2 Data Signature Verification ([5830506](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/583050607e43cef8b544a5700386a019e54c422f))
+    - support ECDSA SHA-2 Data Signing ([07912da](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/07912da1b7663451493fb5e40e4c33deeb18a639))
+    - support extended random number generation ([24f9dc8](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/24f9dc8a43fea350416ca9312a78ab4e786da8ad))
+    - support HMAC SHA-2 MAC verify request ([c05ea29](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c05ea2969070be90a7dbb2d0344c66d89401edf6))
+    - support session based SDOS encrypt and decrypt ([537ff05](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/537ff052579862a4865d36d06940feaa796d16da))
+    - support SHA-2 hash digest generation on a blob ([7e8249a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7e8249a2dbacfa751990c47644f0403311c6e260))
+    - support SiP SVC version ([f0c40b8](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f0c40b897f8a25bc50c53239dcf750dd395ebabf))
+    - support version 2 SiP SVC SMC function ID for mailbox commands ([c436707](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c436707bc6eed31ab61408ef40db6063d05f0912))
+    - support version 2 SiP SVC SMC function ID for non-mailbox commands ([ad47f14](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ad47f1422f3f9aa4a622e08b71fc8f5caab98a98))
+    - update to support maximum response data size ([b703fac](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b703facaaae1e3fe5afa4742b436bb07e065b5e9))
+
+  - **Marvell**
+
+    - **Armada**
+
+      - **A3K**
+
+        - add north and south bridge reset registers ([a4d35ff](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a4d35ff381c625d61bcc22f9f9a1a45d8663b19d))
+
+  - **MediaTek**
+
+    - introduce mtk makefile ([500d40d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/500d40d877617653d347fb6308144973d4297ab9))
+
+    - **MT8195**
+
+      - apply erratas of CA78 for MT8195 ([c21a736](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c21a736d6f3fa9fb0647bff404b0174ebf1acd91))
+      - add EMI MPU surppot for SCP and DSP ([690cb12](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/690cb1265ea84851bd6405a0a6a57d2f1c9f03a3))
+      - dump EMI MPU configurations ([20ef588](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/20ef588e86ad8f3cf13382c164463046db261feb))
+      - improve SPM wakeup log ([ab45305](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ab45305062f50f81e5c3f800ef4c6cef5097cb04))
+
+    - **MT8186**
+
+      - add DFD control in SiP service ([e46e9df](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e46e9df0d0e05f2aaee613fc4f697fcc8d79c0b3))
+      - add SPM suspend driver ([7ac6a76](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7ac6a76c47d429778723aa804b64c48220a10f11))
+      - add Vcore DVFS driver ([635e6b1](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/635e6b108e773daf37c00f46e6fbb1cae4e78f96))
+      - disable 26MHz clock while suspending ([9457cec](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9457cec8c02f78ba56fd9298dd795766c89281a2))
+      - initialize platform for MediaTek MT8186 ([27132f1](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/27132f13ca871dc3cf1aa6938995284cf5016e00))
+      - add power-off function for PSCI ([a68346a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a68346a772859ee6971ec14c6473d2a853e9c66f))
+      - add CPU hotplug ([1da57e5](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1da57e54b2270b3b49710afa6fd947b01d61b261))
+      - add DCM driver ([95ea87f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/95ea87ffc2445c77f070e6a2f78ffa424810faed))
+      - add EMI MPU basic driver ([1b17e34](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1b17e34c5d7740a357b2027d88aef7760b346616))
+      - add MCDI drivers ([06cb65e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/06cb65ef079941d0525dca75dd0e110e9330906d))
+      - add pinctrl support ([af5a0c4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/af5a0c40aff21c4b8771365f19dcb01d6086b30d))
+      - add pwrap and pmic driver ([5bc88ec](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5bc88ec61c75ed42b41d84817aa4d6ee68a2efc8))
+      - add reboot function for PSCI ([24dd5a7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/24dd5a7b71544c503446e58cb23c0cfd09245a3c))
+      - add RTC drivers ([6e5d76b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6e5d76bac8786120d037953f5a6fd67aaff035c1))
+      - add SiP service ([5aab27d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5aab27dc4294110a6c0b69bf5ec5343e7df883a7))
+      - add sys_cirq support ([109b91e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/109b91e38c8d4f73941c8574759560a1f1636d05))
+      - apply erratas for MT8186 ([572f8ad](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/572f8adbb062c36835fbb82944dd2ed772134bfd))
+      - initialize delay_timer ([d73e15e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d73e15e66a33398c8fc51c83f975a3f35494faf5))
+      - initialize GIC ([206f125](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/206f125cc177bc110eb87d40ffc7fa18b28c01ce))
+      - initialize systimer ([a6a0af5](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a6a0af57c3369dfc6fc2f25877d812a24e9be311))
+
+  - **NXP**
+
+    - add SoC erratum a008850 ([3d14a30](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/3d14a30b88762e901e134acc89c6ac4fa9e3f321))
+    - add ifc nor and nand as io devices ([b759727](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b759727f5936a687314168dd8912d30897a8c6be))
+    - add RCPM2 registers definition ([d374060](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d374060abe9b63296f63f1e3c811aeeddb7a093c))
+    - add CORTEX A53 helper functions ([3ccc8ac](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/3ccc8ac3e5da48819a2fc90ec48a175515de38cb))
+
+    - **i.MX**
+
+      - **i.MX 8M**
+
+        - add a simple csu driver for imx8m family ([71c40d3](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/71c40d3bb7c90a6c36d5c49d0830ca95aba65a2f))
+        - add imx csu/rdc enum type defines for imx8m ([0c6dfc4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0c6dfc47847608b6ade0c00716e93afc6725362c))
+        - enable conditional build for SDEI ([d2a339d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d2a339dfa1665edf87a30a4318af954e764c205c))
+        - enable the coram_s tz by default on imx8mn/mp ([d5ede92](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d5ede92d78c829d8a3adad0759219b79e0dc0707))
+        - enable the csu init on imx8m ([0a76495](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0a76495bc2cb0c5291027020a3cd2d3adf31c8ed))
+        - do not release JR0 to NS if HAB is using it ([77850c9](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/77850c96f23bcdc76ecb0ecd27a982c00fde5d9d))
+        - switch to xlat_tables_v2 ([4f8d5b0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/4f8d5b018efc42d1ffa76fca8efb0d16a57f5edd))
+
+        - **i.MX 8M Mini**
+
+          - enable optee fdt overlay support ([9d0eed1](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9d0eed111cb1294605b6d82291fef16a51d35e46))
+          - enable Trusty OS on imx8mm ([ff3acfe](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ff3acfe3cc1658917376152913a9d1b5b9b8de34))
+          - add support for measured boot ([cb2c4f9](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/cb2c4f93c18b948fbfde9d50ab7d30362be0e00a))
+
+        - **i.MX 8M Plus**
+
+          - add trusty for imx8mp ([8b9c21b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/8b9c21b480dd5c3265be1105a9462b3f5657a6b1))
+          - enable BL32 fdt overlay support on imx8mp ([aeff146](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/aeff14640a91f6d33bfdbc0dc7b0e920f6d14b91))
+
+        - **i.MX 8M Nano**
+
+          - enable optee fdt overlay support ([2612891](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/26128912884b26fab67bce9d87ba0e1c85a0be1e))
+          - enable Trusty OS for imx8mn ([99349c8](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/99349c8ecba910dabbaa72b9be91f3ed762036f5))
+
+        - **i.MX 8M Q**
+
+          - enable optee fdt overlay support ([023750c](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/023750c6a898e77c185839f5e56f8e23538f718a))
+          - enable trusty for imx8mq ([a18e393](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a18e393339e1d481f4fdf0d621fe4f39ce93a4fe))
+
+    - **Layerscape**
+
+      - add CHASSIS 3 support for tbbr ([9550ce9](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9550ce9ddd7729a961f51ed61ea4b2030e284dcb))
+      - add new soc errata a009660 support ([785ee93](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/785ee93cc3bd9b43d88fee5acefbd131bf6f2756))
+      - add new soc errata a010539 support ([85bd092](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/85bd0929433875e0b84fdc2046d9ec2cf0164903))
+      - add soc helper macro definition for chassis 3 ([602cf53](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/602cf53b6f507cea88f4af5c07bed9325bc7a9b8))
+      - define more chassis 3 hardware address ([0d396d6](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0d396d6455a659c4e679f02fae1f9043713474b0))
+      - print DDR errata information ([3412716](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/3412716b30260958b30d1fa2e1c6d8cce195cd7d))
+
+      - **LS1043A**
+
+        - add ls1043a soc support ([3b0de91](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/3b0de9182501fae9de372efd1faaf35a7bf74f68))
+
+        - **LS1043ARDB**
+
+          - add ls1043ardb board support ([e4bd65f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e4bd65fed8a12d06181c1343cf786ac91badb6b0)
+
+      - **LX2**
+
+        - enable DDR erratas for lx2 platforms ([cd960f5](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/cd960f5009ee062bba9c479505caee6bbe644649))
+
+      - **LS1046A**
+
+        - add new SoC platform ls1046a ([cc70859](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/cc708597fa72094c5a01df60e6538e4a7429c2a0))
+
+        - **LS1046ARDB**
+
+          - add ls1046ardb board support ([bb52f75](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/bb52f7560b62043ed08a753f399dc80e8c1582d3))
+
+        - **LS1046AFRWY**
+
+          - add ls1046afrwy board support ([b51dc56](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b51dc56ab9ea79e4709f0d0ce965525d0d3da918))
+
+        - **LS1046AQDS**
+
+          - add board ls1046aqds support ([16662dc](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/16662dc40dd2578d3000528ece090ed39ed18b9c))
+
+      - **LS1088A**
+
+        - add new SoC platform ls1088a ([9df5ba0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9df5ba05b4fe4cd44157363a897b73553ba6e2f1))
+
+        - **LS1088ARDB**
+
+          - add ls1088ardb board support ([2771dd0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2771dd0293b6cda6811e8bed95f2354a3ee0124e))
+
+        - **LS1088AQDS**
+
+          - add ls1088aqds board support ([0b0e676](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0b0e67669814139c6818e61e03d0d0e3314fdc99))
+
+  - **QEMU**
+
+    - add SPMD support with SPMC at S-EL1 ([f58237c](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f58237ccd9fd2350730d60ab7de59b5c376bfb35))
+    - add support for measured boot ([5e69026](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5e690269d579d9461be3c5f5e3f59d4c666863a0))
+
+  - **QTI**
+
+    - **MSM8916**
+
+      - allow booting secondary CPU cores ([a758c0b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a758c0b65c6730fb07846899d6436ba257484d34))
+      - initial platform port ([dddba19](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/dddba19a6a3cb7a1039beaffc3169c4eb3291afd))
+      - setup hardware for non-secure world ([af64473](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/af6447315c8534331513ca6b6556af661e0ba88b))
+
+  - **Renesas**
+
+    - **R-Car**
+
+      - **R-Car 3**
+
+        - modify sequence for update value for WUPMSKCA57/53 ([d9912cf](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d9912cf3d1022fc6d38a6059290040985de56e63))
+        - modify type for Internal function argument ([ffb725b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ffb725be98ffd010c851629a6da75bf57f770c7f))
+        - update IPL and Secure Monitor Rev.3.0.3 ([14d9727](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/14d9727e334300b3f5f57e76a9f6e21431e6c6b5))
+
+  - **ST**
+
+    - add a function to configure console ([53612f7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/53612f72938f37244a5f10ae7c57abe7358c221f))
+    - add STM32CubeProgrammer support on UART ([fb3e798](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/fb3e7985c9b657c535c02b722ecc413f643e671e))
+    - add STM32MP_UART_PROGRAMMER target ([9083fa1](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9083fa11ead67272b94329e8f84257de6658620d))
+    - add early console in BL2 ([c768b2b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c768b2b22f4fb16cf8be8b4815a1984b29918c20))
+    - disable authentication based on part_number ([49abdfd](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/49abdfd8cececb91a4bc7e7b29a30c09dce461c7))
+    - get pin_count from the gpio-ranges property ([d0f2cf3](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d0f2cf3b148df75d5cbbd42dfa18012043e5d1f4))
+    - map 2MB for ROM code ([1697ad8](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1697ad8cc81307972d31cec3b27d58f589eeeb3f))
+    - protect UART during platform init ([acf28c2](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/acf28c267b3679a0770b2010f2ec3fb3c2d19975))
+    - update stm32image tool for header v2 ([2d8886a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2d8886aceed613b9be25f20900914cacc8bb0fb9))
+    - update the security based on new compatible ([812daf9](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/812daf916c9c977a4f6d7d745d22b90c8492fc71))
+    - use newly introduced clock framework ([33667d2](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/33667d299bd5398ca549f542345e0f321b483d17))
+
+    - **ST32MP1**
+
+      - adaptations for STM32MP13 image header ([a530874](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a5308745ee3ab3b77ca942052e60968bcc01340d))
+      - add "Boot mode" management for STM32MP13 ([296ac80](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/296ac8012b77ea84079b38cc60ee786a5f91857f))
+      - add a second fixed regulator ([225ce48](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/225ce4822ccf2e7c7c1fca6cf3918d4399158613))
+      - add GUID values for updatable images ([8d6b476](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/8d6b4764f3e54431c3d01342d39d1efa70c3dbf9))
+      - add GUID's for identifying firmware images to be booted ([41bd8b9](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/41bd8b9e2ad3b755505684601f07d4f7f8ec04c4))
+      - add helper to enable high speed mode in low voltage ([dea02f4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/dea02f4eaed855c2f05d8a1d7eefca313e98e5b4))
+      - add logic to pass the boot index to the Update Agent ([ba02add](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ba02add9ea8fb9a8b0a533c1065a77c7dda4f2a6))
+      - add logic to select the images to be booted ([8dd7553](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/8dd755314fdfa077465bd6cd5e248be392d90378))
+      - add NVMEM layout compatibility definition ([dfbdbd0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/dfbdbd0625990267c6742268118ea748e77c6123))
+      - add part numbers for STM32MP13 ([30eea11](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/30eea116cdd66b3fa1e1208e185eb7285a83d898))
+      - add regulator framework compilation ([bba9fde](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/bba9fdee589fb9a7aca5963f53b7ce67c30520b3))
+      - add sdmmc compatible in platform define ([3331d36](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/3331d3637c295993a78f22afe7463cf1c334d329))
+      - add sign-compare warning ([c10f3a4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c10f3a4559ebf7a654a9719fec619e81e6ee1d69))
+      - add stm32_get_boot_interface function ([a6bfa75](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a6bfa75cf25241a486ab371ae105ea7ebf2d34d8))
+      - add support for building the FWU feature ([ad216c1](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ad216c106682f1d2565b2a08e11a601b418dc8a4))
+      - add support for reading the metadata partition ([0ca180f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0ca180f6416160a523ff442f1ad0b768a9a3a948))
+      - add timeout in IO compensation ([de02e9b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/de02e9b0ec29548b8ce5ef6ee9adcd9c5edb0518))
+      - allow configuration of DDR AXI ports number ([88f4fb8](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/88f4fb8fa759b1761954067346ee674b454bdfde))
+      - call pmic_voltages_init() in platform init ([ffd1b88](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ffd1b889225a8aec124df9e330f41dc638fd7180))
+      - chip rev. Z is 0x1001 on STM32MP13 ([ef0b8a6](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ef0b8a6c1b1a0eab3626041f3168f82bdb410836))
+      - enable BL2_IN_XIP_MEM to remove relocation sections ([d958d10](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d958d10eb360024e15f3c921dc3863a0cee98830))
+      - enable format-signedness warning ([cff26c1](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/cff26c19169dd94857e8180cc46b7aa4ccac574a))
+      - get CPU info from SYSCFG on STM32MP13 ([6512c3a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6512c3a62a4a7baaf32597284b242bc7172b7e26))
+      - introduce new flag for STM32MP13 ([bdec516](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/bdec516ee862bfadc25a4d0c02a3b8d859c1fa25))
+      - manage HSLV on STM32MP13 ([fca10a8](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/fca10a8f1b47231ef92634a0adf1a26cbfc97c2a))
+      - manage monotonic counter ([f5a3688](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f5a3688b8608df0f269a0b6df18632ebb9e26a01))
+      - new way to access platform OTP ([ae3ce8b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ae3ce8b28eac73e9a41fdb28424d9f0f4b5f200e))
+      - preserve the PLL4 settings for USB boot ([bf1af15](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/bf1af154db2c89028a8a551c18885add35d38966))
+      - register fixed regulator ([967a8e6](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/967a8e63c33822680e3a4631430dcd9a4a64becd))
+      - remove unsupported features on STM32MP13 ([111a384](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/111a384c90afc629e644e7a8284abbd4311cc6b3))
+      - retry 3 times FWU trial boot ([f87de90](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f87de907c87e5b2091592c131c4d3d2f737bef01))
+      - select platform compilation either by flag or DT ([99a5d8d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/99a5d8d01d38474b056766651bd746a4fe93ab20))
+      - skip TOS_FW_CONFIG if not in FIP ([b706608](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b7066086424c2f6fd04880db852306d6f487962e))
+      - stm32mp_is_single_core() for STM32MP13 ([7b48a9f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7b48a9f3286b8f174acf8821fec48fd2e4771514))
+      - update BACKUP_BOOT_MODE for STM32MP13 ([4b031ab](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/4b031ab4c50d0b9f7127daa7f4eec634f39de970))
+      - update boot API for header v2.0 ([5f52eb1](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5f52eb15970e57d2777d114948fc1110e3dd3f6c))
+      - update CFG0 OTP for STM32MP13 ([1c37d0c](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1c37d0c1d378769249c797de5b13d73cf6f17a53))
+      - update console management for SP_min ([aafff04](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/aafff0435448c8409935132be41758e0031f0822))
+      - update IO compensation on STM32MP13 ([8e07ab5](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/8e07ab5f705b213af28831f7c3e9878154e07df0))
+      - update IP addresses for STM32MP13 ([52ac998](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/52ac9983d67522b6b821391941c8b0d01fd68941))
+      - update memory mapping for STM32MP13 ([48ede66](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/48ede6615168118c674288f2e4f8ee1b11d2fa02))
+      - updates for STM32MP13 device tree compilation ([d38eaf9](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d38eaf99d327bc1400f51c87b6d8a2f92cd828c6))
+      - usb descriptor update for STM32MP13 ([d59b9d5](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d59b9d53b9cfb2443575c62c6716eb5508374a7b))
+      - use clk_enable/disable functions ([c7a66e7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c7a66e720ae1a1a5ef98eaf9ff327cd352549010))
+      - use only one filter for TZC400 on STM32MP13 ([b7d0058](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b7d0058a3a9153a3863cf76a6763ea751b3ab48d))
+      - warn when debug enabled on secure chip ([ac4b8b0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ac4b8b06eb23134d2a9002834541d33f8d43661b))
+
+  - **Texas Instruments**
+
+    - add enter sleep method ([cf5868b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/cf5868b8cd7239dee69bdf6ba3ab87bd06bf15f5))
+    - add gic save and restore calls ([b40a467](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b40a467783e5911f97d6e92ebdeb34ca2f005552))
+    - add PSCI handlers for system suspend ([2393c27](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2393c27680a1ec636e413051e87e986df5a866fe))
+    - allow build config of low power mode support ([a9f46fa](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a9f46fad82b807a9f0a967245e3ac10ee8dd0ef1))
+    - increase SEC_SRAM_SIZE to 128k ([38164e6](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/38164e64bd853a8329475e9168c5fcb94ecc528b))
+
+  - **Xilinx**
+
+    - **Versal**
+
+      - add SPP/EMU platform support for versal ([be73459](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/be73459a945d8fa781fcc864943ccd0a8d92421c))
+      - add common interfaces to handle EEMI commands ([1397967](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1397967490c9f0ebff0d20a566260d1475fe065e))
+      - add SMCCC call TF_A_PM_REGISTER_SGI ([fcf6f46](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/fcf6f469318d693a024d42ae2d0f4afb26c1e85d))
+      - add support to reset SGI ([bf70449](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/bf70449ba2d1ffd20b01741c491dc0f565009b3d))
+      - add UART1 as console ([2c79149](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2c791499c26b40c31ce7f68c3bf0dca777fc62de))
+      - enhance PM_IOCTL EEMI API to support additional arg ([d34a5db](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d34a5db8a76abdfc8fa68f43b24b971699086a06))
+      - get version for ATF related EEMI APIs ([da6e654](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/da6e654bc8b03ee784d0e96a71c4e591e63930f2))
+      - remove the time stamp configuration ([18e2a79](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/18e2a79f8a5eaa72a2a7e641c2481beb9f827dce))
+
+    - **ZynqMP**
+
+      - disable the -mbranch-protection flag ([67abd47](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/67abd4762bd563be94e734bb0fe4087e88d5d446))
+      - fix section `coherent_ram' will not fit in region `RAM' ([9b4ed0a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9b4ed0af02a8ff1fd9a81af5213fde16d3eb8d92))
+      - add feature check support ([223a628](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/223a6284b8a0a3ead884a7f0cf333a464d32e319))
+      - add support to get info of xilfpga ([cc077c2](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/cc077c22273075db328bd30fa12c28abf9eef052))
+      - add uart1 as console ([ea66e4a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ea66e4af0baf5d5b905e72f824a672f16a6e0f98))
+      - increase the max xlat tables when debug build is enabled ([4c4b961](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/4c4b9615b1d9512a4a89aa08e722547cc491a07b))
+      - pass ioctl calls to firmware ([76ff8c4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/76ff8c459e9e6d105e614d68648bd6680806f93e))
+      - pm_api_clock_get_num_clocks cleanup ([e682d38](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e682d38b56854e1586b25d929dbc83543b4c66e4))
+
+- **Bootloader Images**
+
+  - add XLAT tables symbols in linker script ([bb5b942](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/bb5b942e6f133198daedcca0b74ec598af260a54))
+
+  - **BL2**
+
+    - add support to separate no-loadable sections ([96a8ed1](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/96a8ed14b74cca33a8caf567d0f0a2d3b2483a3b))
+
+  - **BL31**
+
+    - aarch64: RESET_TO_BL31_WITH_PARAMS ([25844ff](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/25844ff728e4a0e5430ba2032457aba7b780a701))
+
+- **Services**
+
+  - **RME**
+
+    - add dummy platform token to RMMD ([0f9159b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0f9159b7ebb7e784a8ed998869ff21095fa105b1))
+    - add dummy realm attestation key to RMMD ([a043510](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a0435105f229a65c7861b5997793f905cf90b823))
+
+  - **SPM**
+
+    - update ff-a boot protocol documentation ([573ac37](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/573ac37373d3e8b2c31b3aaeed759e4656e060ec))
+
+    - **EL3 SPMC**
+
+      - allow BL32 specific defines to be used by SPMC_AT_EL3 ([2d65ea1](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2d65ea1930d4ce26cc176a8c60e9401d0b4f862a))
+      - add plat hook for memory transactions ([a8be4cd](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a8be4cd057bce5f0b4ac6af396c0c870474d1ef4))
+      - add EL3 SPMC #defines ([44639ab](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/44639ab73e43e0b79da834dff8c85266d68e5066))
+      - introduce accessor function to obtain datastore ([6a0788b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6a0788bc0e704283e52c80990aa2bb6e047a0cc2))
+      - add FF-A secure partition manager core ([5096aeb](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5096aeb2ba646548a7a6ab59e975b996e6c9026a))
+      - add FFA_FEATURES handler ([55a2963](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/55a296387b9720855df429a08c886f47a4a45057))
+      - add FFA_PARTITION_INFO_GET handler ([f74e277](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f74e27723bb54ad1318fa462fbcff70af555b2e6))
+      - add FFA_RUN handler ([aad20c8](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/aad20c85cb6f4bc91318d3c6488cf72a20fdbe96))
+      - add FFA_RX_RELEASE handler ([f0c25a0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f0c25a082fc8b891d4d21518028118561caa4735))
+      - add function to determine the return path from the SPMC ([20fae0a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/20fae0a7ce7fd407cd3efb7745017ee6ab605159))
+      - add helper function to obtain endpoint mailbox ([f16b6ee](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f16b6ee3deac93706efe465f399c9542e12d5eeb))
+      - add helper function to obtain hyp structure ([a7c0050](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a7c00505f85684326a223535a319c170d14826f6))
+      - add helper to obtain a partitions FF-A version ([c2b1434](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c2b1434369292081f907c548e496f59e197eb2f1))
+      - add partition mailbox structs ([e1df600](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e1df6008d9b4a00da25ec08fbdcbd3a5967fdb54))
+      - add support for direct req/resp ([9741327](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9741327df577c3f43db42b26bda607429e62af0b))
+      - add support for FF-A power mgmt. messages in the EL3 SPMC ([59bd2ad](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/59bd2ad83c13ed3c84bb9b841032c95927358890))
+      - add support for FFA_MSG_WAIT ([c4db76f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c4db76f066f236fe490ebc7a50833a04e08f5151))
+      - add support for FFA_SPM_ID_GET ([46872e0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/46872e01f5efb555fef8367595b59e5d2f75cec0))
+      - add support for forwarding a secure interrupt to the SP ([729d779](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/729d7793f830781ff8ed44d144c3346c6e4251a3))
+      - add support for handling FFA_ERROR ABI ([d663fe7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d663fe7a3002ff028c190eb732278b878e78b7b7))
+      - add support for v1.1 FF-A boot protocol ([2e21921](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2e21921502b1317031cf2a2f69c5d47ac88a505d))
+      - add support for v1.1 FF-A memory data structures ([7e804f9](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7e804f9695c48681c91e9e6fc6175eb6997df867))
+      - enable building of the SPMC at EL3 ([1d63ae4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1d63ae4d0d8374a732113565be90d58861506e39))
+      - enable checking of execution ctx count ([5b0219d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5b0219ddd5da42413f4c2be9302224b5b71295ff))
+      - enable handling FF-A RX/TX Mapping ABIs ([1a75224](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1a752245ecae6487844c57667e24b704e6df8079))
+      - enable handling FFA_VERSION ABI ([0c7707f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0c7707fdf21fc2a8658f5a4bdfd2f8883d02ada5))
+      - enable handling of the NS bit ([0560b53](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0560b53e71ab6daefa8e75665a718605478746a4))
+      - enable parsing of messaging methods from manifest ([3de378f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/3de378ff8c9430c964cbe9b0c58fa5afc4d237ce))
+      - enable parsing of UUID from SP Manifest ([857f579](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/857f5790da3770a9ca52416274eec4e545c9be53))
+      - enable the SPMC to pass the linear core ID in a register ([f014300](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f0143004e548582760aacd6f15f5499b18081a69))
+      - prevent read only xlat tables with the EL3 SPMC ([70d986d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/70d986ddbbf56a20c7550c079dd4dc9462332594))
+      - support FFA_ID_GET ABI ([d5fe923](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d5fe92350cb018ae7083ed26a6a16508ccd82a86))
+      - allow forwarding of FFA_FRAG_RX/TX calls ([642db98](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/642db9840712044b9c496e04a7acd60580e54117))
+      - enable handling of FF-A SMCs with the SPMC at EL3 ([bb01a67](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/bb01a67306f47271adde051e541c760028c1a0f1))
+      - update SPMC init flow to use EL3 implementation ([6da7607](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6da76075bf4b953d621aa15c379e62a5f785de3f))
+      - add logical partition framework ([7affa25](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7affa25cad400101c016082be2d102be0f4fce80))
+      - add FF-A memory management code ([e0b1a6d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e0b1a6d59e57c0dbe87f5b8f8166f1123664f058))
+      - prevent duplicated sharing of memory regions ([fef85e1](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/fef85e1e53fcf44e8d9ed50c89d8a764bf1b7738))
+      - support multiple endpoints in memory transactions ([f0244e5](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f0244e5dd1b8cbab75ef00c1b9b56eed5b3cad4b))
+
+    - **SPMD**
+
+      - forward FFA_VERSION from SPMD to SPMC ([9944f55](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9944f55761c4d5cc1feefaf5e33bf7fb83d8f5f3))
+      - enable SPMD to forward FFA_VERSION to EL3 SPMC ([9576fa9](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9576fa93a2effc23a533b80dce41d7104a8d200b))
+      - add FFA_MSG_SEND2 forwarding in SPMD ([c2eba07](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c2eba07c47f8d831629104eeffcec11ed7d3b0a5))
+      - add FFA_RX_ACQUIRE forwarding in SPMD ([d555233](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d555233fe5a04dfd99fd6ac30bacc5284285c131))
+
+    - **SPM MM**
+
+      - add support to save and restore fp regs ([15dd6f1](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/15dd6f19da8ee4b20ba525e0a742d0df9e46e071))
+
+- **Libraries**
+
+  - **CPU Support**
+
+    - add library support for Poseidon CPU ([1471475](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1471475516cbf1b4a411d5ef853bd92d0edd542e))
+    - add support for Cortex-X1 ([6e8eca7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6e8eca78e5db966e10e2fa2737e9be4d5af51fa9))
+    - add L1PCTL macro definiton for CPUACTLR_EL1 ([8bbb1d8](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/8bbb1d80a58dbdf96fcabbdebbfbd21d2d5344a4))
+
+  - **EL3 Runtime**
+
+    - add arch-features detection mechanism ([6a0da73](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6a0da73647546aea1d10b4b2347bac9d532bcb43))
+    - replace ARM_ARCH_AT_LEAST macro with FEAT flags ([0ce220a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0ce220afb24f0511332b251952019d7011ccc282))
+
+  - **FCONF**
+
+    - add a helper to get image index ([9e3f409](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9e3f409398af447b1d03001dd981007a9bb1617e))
+    - add NS load address in configuration DTB nodes ([ed4bf52](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ed4bf52c33b6860d58a2ffc946bd293ec76bbdaa))
+
+  - **Standard C Library**
+
+    - add support for length specifiers ([701e94b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/701e94b08f382691b0deabd4df882abd87e17ab5))
+
+  - **PSA**
+
+    - add initial attestation API ([0848565](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/084856513d6730a50a3d65ac9c3bdae465117c40))
+    - add measured boot API ([758c647](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/758c64715b691be92de623f81032494e38a43cc8))
+    - mock PSA APIs ([0ce2072](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0ce2072d9b9f419bb19595454395a33a5857ca2f))
+
+- **Drivers**
+
+  - **Generic Clock**
+
+    - add a minimal clock framework ([847c6bc](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/847c6bc8e6d55b1c0f31a52407aa61515cd6c612))
+
+  - **FWU**
+
+    - add a function to pass metadata structure to platforms ([9adce87](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9adce87efc8acc947b8b49d700c9773a7f071e02))
+    - add basic definitions for GUID handling ([19d63df](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/19d63df1af72b312109b827cca793625ba6fcd16))
+    - add platform hook for getting the boot index ([40c175e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/40c175e75bc442674a5dc793c601b09681158ab9))
+    - pass a const metadata structure to platform routines ([6aaf257](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6aaf257de4a4070ebc233f35a09bce4c39ea038c))
+    - simplify the assert to check for fwu init ([40b085b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/40b085bddf60cf8c533b379ccb41e6668c5080dd))
+
+  - **Measured Boot**
+
+    - add RSS backend ([0442ebd](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0442ebd2e9bcf5fa4344d8fa8ef4b69a3b249e33))
+
+  - **GUID Partition Tables Support**
+
+    - add a function to identify a partition by GUID ([3cb1065](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/3cb1065581f6d9a8507af8dbca3779d139aa0ca7))
+    - cleanup partition and gpt headers ([2029f93](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2029f930097b0c3b1b1faa660032d16ed01a5c86))
+    - copy the partition GUID into the partition structure ([7585ec4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7585ec4d36ebb7e286cfec959b2de084eded8201))
+    - make provision to store partition GUID value ([938e8a5](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/938e8a500a25a949cfd25f0cb79f6c1359c9b40c))
+    - verify crc while loading gpt header ([a283d19](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a283d19f82ddb635d9d9fa061e7fd956167ebe60))
+
+  - **Arm**
+
+    - **GIC**
+
+      - allow overriding GICD_PIDR2_GICV2 address ([a7521bd](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a7521bd5d887bfd69d99a55a81416e38ba9ebc97))
+
+      - **GIC-600AE**
+
+        - disable SMID for unavailable blocks ([3f0094c](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/3f0094c15d433cd3de413a4633a4ac2b8e1d1f2e))
+        - enable all GICD, PPI, ITS SMs ([6a1c17c](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6a1c17c770139c00395783e7568220d61264c247))
+        - introduce support for RAS error handling ([308dce4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/308dce40679f63db504cd3d746a0c37a2a05f473))
+
+    - **SMMU**
+
+      - add SMMU abort transaction function ([6c5c532](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6c5c5320511ab8202fb9eccce9e66b4e4e0d9a33))
+      - configure SMMU Root interface ([52a314a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/52a314af254966a604e192fcc3326737354f217a))
+
+    - **MHU**
+
+      - add MHU driver ([af26d7d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/af26d7d6f01068809f17cc2d49a9b3d573c640a9))
+
+    - **RSS**
+
+      - add RSS communication driver ([ce0c40e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ce0c40edc93aa403cdd2eb6c630ad23e28b01c3e))
+
+    - **TZC**
+
+      - **TZC-380**
+
+        - add sub-region register definition ([fdafe2b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/fdafe2b5ead66a1b5175db77bcc7cedafa14a059))
+
+  - **Marvell**
+
+    - **Armada**
+
+      - **A3K**
+
+        - **A3720**
+
+          - preserve x1/x2 regs in console_a3700_core_init() ([7c85a75](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7c85a7572960efbaabe20c9db037bcec66be3e98))
+
+  - **MediaTek**
+
+    - **APU**
+
+      - add mt8195 APU clock and pll SiP call ([296b590](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/296b590206aa6db51e5c82b1a97a4f9707b49c4d))
+      - add mt8195 APU iommap regions ([339e492](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/339e4924a7a3fd11bc176e0bf3e01d76133d364c))
+      - add mt8195 APU mcu boot and stop SiP call ([88906b4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/88906b443734399be5c07a5bd690b63d3d82cefa))
+
+  - **NXP**
+
+    - **DCFG**
+
+      - add Chassis 3 support ([df02aee](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/df02aeeec640d2358301e903d9c8c473d455be9e))
+      - add gic address align register definition ([3a8c9d7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/3a8c9d78d4c65544d789bd64bd005ac10b5b352d))
+      - add some macro definition ([1b29fe5](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1b29fe534b8732193850fced2da1dc449450bd3b))
+
+    - **NXP Crypto**
+
+      - add chassis 3 support ([d60364d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d60364d48e31b33b57049d848b7462eb0e0de612))
+
+    - **DDR**
+
+      - add rawcard 1F support ([f2de48c](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f2de48cb143c20ccd7a9c141df3d34cae74049de))
+      - add workaround for errata A050958 ([291adf5](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/291adf521a54a365e54964bff4dae53d51c65936))
+
+    - **GIC**
+
+      - add some macros definition for gicv3 ([9755fd2](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9755fd2ec2894323136715848910b13053cfe0ce))
+
+    - **CSU**
+
+      - add bypass bit mask definition ([ec5fc50](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ec5fc501f15922967bf5d8260072ba1f9aec9640))
+
+    - **IFC NAND**
+
+      - add IFC NAND flash driver ([28279cf](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/28279cf2c141caf5e4e7156f874cde6f5a0d271b))
+
+    - **IFC NOR**
+
+      - add IFC nor flash driver ([e2fdc77](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e2fdc77ba4eee91f0d1490e34f0fff552fc55dc9))
+
+    - **TZC-380**
+
+      - add tzc380 platform driver support ([de9e57f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/de9e57ff1f3769e770eac44b94127eb7239a63f2))
+
+  - **ST**
+
+    - introduce fixed regulator driver ([5d6a264](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5d6a2646f7759a5a2b3daed0d8aef4588c552ba4))
+
+    - **Clock**
+
+      - add clock driver for STM32MP13 ([9be88e7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9be88e75c198b08c508d8e470964720a781294b3))
+      - assign clocks to the correct BL ([7418cf3](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7418cf397916c97cb4ecf159b1f497a84299b695))
+      - check HSE configuration in serial boot ([31e9750](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/31e9750bc17bd472d4f2a3db297461efc301be51))
+      - define secure and non-secure gate clocks ([aaa09b7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/aaa09b713c6f539fb5b2ee7e2dfd75f2d46875f5))
+      - do not refcount on non-secure clocks in bl32 ([3d69149](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/3d69149a7e9e9a899d57f48bee26f98614f88935))
+      - manage disabled oscillator ([bcccdac](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/bcccdacc7e7b7b985df942b3fae26cb9038a2574))
+
+    - **DDR**
+
+      - add read valid training support ([5def13e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5def13eb01ebac5656031bdc388a215d012fdaf8))
+
+    - **GPIO**
+
+      - allow to set a gpio in output mode ([53584e1](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/53584e1d5b2b843ea3bb9e01e3f01ea7c364ee6a))
+      - do not apply secure config in BL2 ([fc0aa10](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/fc0aa10a2cd3cab887a8baa602891d1f45db2537))
+      - add a function to reset a pin ([737ad29](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/737ad29bf992a7a79d538d1e0b47c7f38d9a4b9d))
+
+    - **SDMMC2**
+
+      - allow compatible to be defined in platform code ([6481a8f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6481a8f1e045ac80f0325b8bfe7089ba23deaf7b))
+      - manage cards power cycle ([258bef9](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/258bef913aa76ead1b10c257d1695d9c0ef1c79d))
+
+    - **ST PMIC**
+
+      - add pmic_voltages_init() function ([5278ec3](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5278ec3faf2010fd6aea1d8cd4294dd229c5c21d))
+      - register the PMIC to regulator framework ([85fb175](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/85fb175b5ef854bc4607db98a4cfb5f35d822cee))
+
+    - **STPMIC1**
+
+      - add new services ([ea552bf](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ea552bf5a57b573a6b09e396e3466b3c4af727f0))
+      - add USB OTG regulators ([13fbfe0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/13fbfe046e71393961d2c70a4f748a15f9c15f77))
+
+    - **Regulator**
+
+      - add support for regulator-always-on ([9b4ca70](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9b4ca70d97d9a2556752b511ff9fe52012faff02))
+      - add a regulator framework ([d5b4a2c](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d5b4a2c4e7fd0bcb9f08584b242e69a2e591fb71))
+
+    - **UART**
+
+      - manage oversampling by 8 ([1f60d1b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1f60d1bd33d434b0c82a74e276699ee5a2f63833))
+      - add uart driver for STM32MP1 ([165ad55](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/165ad5561ef598ea6261ba082610eeff3f208df7))
+
+- **Miscellaneous**
+
+  - **Debug**
+
+    - update print_memory_map.py ([d16bfe0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d16bfe0feffe6a20399fb91d86fd8f7282b941dd))
+
+  - **DT Bindings**
+
+    - add bindings for STM32MP13 ([1b8898e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1b8898eb32c3872a34fc59f4216736f23af0c6ea))
+    - add TZC400 bindings for STM32MP13 ([24d3da7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/24d3da76d221390bb47d501c2ed77a1a7d2b42e7))
+
+  - **FDT Wrappers**
+
+    - add function to find or add a sudnode ([dea8ee0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/dea8ee0d3f13f8d1638745b76e86bd7617bf92e7))
+
+  - **FDTs**
+
+    - add the ability to supply idle state information ([2b2b565](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2b2b565717cc0299e75e8806004d1a3548e9fbf7))
+
+    - **STM32MP1**
+
+      - add DDR support for STM32MP13 ([e6fddbc](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e6fddbc995947d4e5a5dc6607c76cd46fdd840e2))
+      - add DT files for STM32MP13 ([3b99ab6](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/3b99ab6e370a01caec14bc5422a86001eaf291b8))
+      - add nvmem_layout node and OTP definitions ([ff8767c](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ff8767cbfc2bb851a2f6cc32fbe3693ddbfb7d12))
+      - add st-io_policies node for STM32MP13 ([2bea351](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2bea35122d102492f18c427535ce6c9b7016e356))
+      - add support for STM32MP13 DK board ([2b7f7b7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2b7f7b751f4b0f7a8a0f4a35407af22cc269e529))
+      - update NVMEM nodes ([375b79b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/375b79bb4a773fe6a5dd971272c72bf12155050e))
+
+- **Documentation**
+
+  - context management refactor proposal ([3274226](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/327422633bef112a10579d4daeca0f596cd02911))
+
+  - **Threat Model**
+
+    - Threat Model for TF-A v8-R64 Support ([dc66922](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/dc669220d5666c2c808bc11ba81c86a9b071271a))
+
+- **Tools**
+
+  - **Secure Partition Tool**
+
+    - add python SpSetupActions framework ([b1e6a41](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b1e6a41572240839e62099aa00298174b18c696a))
+    - delete c version of the sptool ([f4ec476](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f4ec47613fef8db8037195147dc2ac6fb6f154ff))
+    - python version of the sptool ([2e82874](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2e82874cc9b7922e000dd4d7718e3153e347b1d7)
+    - use python version of sptool ([822c727](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/822c72791f791d26e233df0c15a655c3dbd8b117))
+
+### Resolved Issues
+
+- **Architecture**
+
+  - **Activity Monitors Extension (FEAT_AMU)**
+
+    - add default value for ENABLE_FEAT_FGT and ENABLE_FEAT_ECV flags ([820371b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/820371b13028a6f620a62cf73a951883d051666b))
+    - fault handling on EL2 context switch ([f74cb0b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f74cb0be8ac80eb3072555cb04eb09375d4cb31f))
+    - limit virtual offset register access to NS world ([a4c3945](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a4c394561af31ae0417ed9ff3b3152adb7cd5355))
+
+  - **Scalable Vector Extension (FEAT_SVE)**
+
+    - disable ENABLE_SVE_FOR_NS for AARCH32 ([24ab2c0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/24ab2c0af74be174acf755a36b3ebba867184e60))
+
+- **Platforms**
+
+  - **Allwinner**
+
+    - improve DTB patching error handling ([79808f1](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/79808f10c32d441572666551b1545846079af15b))
+
+  - **Arm**
+
+    - fix fvp and juno build with USE_ROMLIB option ([861250c](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/861250c3b26d64f859f5f37686e453d5074fa976))
+    - increase ARM_BL_REGIONS count ([dcb1959](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/dcb1959161935aa58d2bb852f3cef0b96458a4e1))
+    - remove reclamation of functions starting with "init" ([6c87abd](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6c87abdda400354ebf4f5351086c32a4620475c9))
+    - use PLAT instead of TARGET_PLATFORM ([c5f3de8](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c5f3de8dabc9b955b6051a6c6116d40b10a84f5d))
+    - fix SP count limit without dual root CoT ([9ce15fe](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9ce15fe8910580efca46b9f102e117402ce769db))
+
+    - **FVP**
+
+      - FCONF Trace Not Shown ([0c55c10](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0c55c10305df6217fd978d58ce203dbad3edd4d5))
+      - disable reclaiming init code by default ([fdb9166](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/fdb9166b9494402eb2da7e0b004c121b322725e0))
+      - extend memory map to include all DRAM memory regions ([e803542](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e80354212f591c8813dec27353e8241e03155b4c))
+      - fix NULL pointer dereference issue ([a42b426](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a42b426b8548e3304e995f1a49d2470d71072949))
+      - op-tee sp manifest doesn't map gicd ([69cde5c](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/69cde5cd9563f0c665862f1e405ae8e8d2818c6e))
+
+    - **Morello**
+
+      - change the AP runtime UART address ([07302a2](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/07302a23ec1af856b3d4de0439161a8c23414f84))
+      - fix SoC reference clock frequency ([e8b7a80](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e8b7a80436c2bc81c61fc4703d6580f2fe9226a9))
+      - include errata workaround for 1868343 ([f94c84b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f94c84baa2a2bad75397b0ec6a0922fe8a475847))
+
+    - **SGI**
+
+      - disable SVE for NS to support SPM_MM builds ([78d7e81](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/78d7e819798ace643b6e22025dc76aedb199bbd5))
+
+    - **TC**
+
+      - remove the bootargs node ([68fe3ce](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/68fe3cec25bc9ea4e1bafdb1d9f5315e245d650b))
+
+    - **Corstone-1000**
+
+      - change base address of FIP in the flash ([1559450](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1559450132c5e712f4d6896e53e4f1cb521fa465))
+
+  - **Broadcom**
+
+    - allow build to specify mbedTLS absolute path ([903d574](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/903d5742953d9d4b224e71d8b1e62635e83f44a9))
+    - fix the build failure with mbedTLS config ([95b5c01](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/95b5c0126b802b894ea0177d973978e06b6a254d))
+
+  - **Intel**
+
+    - add flash dcache after return response for INTEL_SIP_SMC_MBOX_SEND_CMD ([ac097fd](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ac097fdf07ad63b567ca751dc518f8445a0baef6))
+    - allow non-secure access to FPGA Crypto Services (FCS) ([4837a64](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/4837a640934630f8034ceec1bb84cc40673d8a6b))
+    - always set doorbell to SDM after sending command ([e93551b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e93551bb3bd8ac43779fa70c7363ee2568da45ca))
+    - assert if bl_mem_params is NULL pointer ([35fe7f4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/35fe7f400a7f1d65ff2fee5531d20f6c2f3e6f39))
+    - bit-wise configuration flag handling ([276a436](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/276a43663e8e315fa1bf0aa4824051d88705858b))
+    - change SMC return arguments for INTEL_SIP_SMC_MBOX_SEND_CMD ([108514f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/108514ff7160a86efb791449a4635ffe0f9fdf2c))
+    - configuration status based on start request ([e40910e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e40910e2dc3fa59bcce83ec1cf9a33b3e85012c4))
+    - define macros to handle buffer entries ([7db1895](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7db1895f0be2f8c6710bf51d8441d5e53e3ef0fe))
+    - enable HPS QSPI access by default ([000267b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/000267be22d3c0077c0fd0a8377ceeed5aada4c3))
+    - extend SDM command to return the SDM firmware version ([c026dfe](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c026dfe38cfae379457a6ef53130bd5ebc9d7808))
+    - extending to support large file size for AES encryption and decryption ([dcb144f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/dcb144f1fbcef73ddcc448d5ed6134aa279069b6))
+    - extending to support large file size for SHA-2 ECDSA data signing and signature verifying ([1d97dd7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1d97dd74cd128edd7ad45b725603444333c7b262))
+    - extending to support large file size for SHA2/HMAC get digest and verifying ([70a7e6a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/70a7e6af958f3541476a8de6baac8e376fcc67f9))
+    - fix bit masking issue in intel_secure_reg_update ([c9c0709](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c9c070994caedf123212aad23b6942122c5dd793))
+    - fix configuration status based on start request ([673afd6](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/673afd6f8e7266900b00a7cbeb275fe1a3d69cce))
+    - fix ddr address range checker ([12d71ac](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/12d71ac6627bb6822a0314e737794a8503df79dd))
+    - fix ECC Double Bit Error handling ([c703d75](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c703d752cce4fd101599378e72db66ccf53644fa))
+    - fix fpga config write return mechanism ([ef51b09](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ef51b097bfa906bf1cee8ee641a1b7bcc8c5f3c0))
+    - flush dcache before sending certificate to mailbox ([49d44ec](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/49d44ec5f357b1bcf8eae9e91fbd72aef09e00dd))
+    - get config status OK status ([07915a4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/07915a4fd5848fbac69dcbf28f00353eed10a942))
+    - introduce a generic response error code ([651841f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/651841f20110ce6fac650e3ac47b0a9cce18e6f3))
+    - make FPGA memory configurations platform specific ([f571183](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f571183b066b1a91b7fb178c3aad9d6360d1918c))
+    - modify how configuration type is handled ([ec4f28e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ec4f28ecec8887a685d6119c096ad346da1ea53e))
+    - null pointer handling for resp_len ([a250c04](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a250c04b0cc807f626df92a7091ff13b3a3aa9ed))
+    - refactor NOC header ([bc1a573](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/bc1a573d5519f121cb872fce1d88fe2e0db07b2c))
+    - reject non 4-byte align request size for FPGA Crypto Service (FCS) ([52ed157](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/52ed157fd66812debb13a792c21f763de01aef70))
+    - remove redundant NOC header declarations ([58690cd](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/58690cd629b4ccdefe5313f805219598074a3501))
+    - remove unused printout ([0d19eda](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0d19eda0dd2ffae27d0551b1f0a06a2b8f96c853))
+    - update certificate mask for FPGA Attestation ([fe5637f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/fe5637f27aebfdab42915c2ced2c34d8685ee2bb))
+    - update encryption and decryption command logic ([02d3ef3](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/02d3ef333d4a0a07a3e40defb12a8cde3a7cba03))
+    - use macro as return value ([e0fc2d1](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e0fc2d1907b1c8a062c44a435be77a12ffeed84b))
+
+  - **Marvell**
+
+    - **Armada**
+
+      - **A3K**
+
+        - change fatal error to warning when CM3 reset is not implemented ([30cdbe7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/30cdbe7043832f7bd96b40294ac062a8fc9c540f))
+        - fix comment about BootROM address range ([5a60efa](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5a60efa12a57cde98240f861e45609cb9b94d58d))
+
+  - **Mediatek**
+
+    - **MT8186**
+
+      - remove unused files in drivers/mcdi ([bc714ba](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/bc714bafe7ae8ca29075ba9bf3985c0e15ae0f64))
+      - extend MMU region size ([0fe7ae9](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0fe7ae9c64aa6f6d5b06a80de9c88081057d5dbe))
+
+  - **NVIDIA**
+
+    - **Tegra**
+
+      - **Tegra 194**
+
+        - remove incorrect erxctlr assert ([e272c61](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e272c61ce8185deb397dcf168ec72bdaa5926a33))
+
+  - **NXP**
+
+    - fix total dram size checking ([0259a3e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0259a3e8282ed17c1d378a27f820f44b3bebab07))
+    - increase soc name maximum length ([3ccd7e4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/3ccd7e45a2c3ff9fa7794f0284c9d0298e7cb982))
+
+    - **i.MX**
+
+      - **i.MX 8M**
+
+        - check the validation of domain id ([eb7fb93](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/eb7fb938c3ce34ccfb143ae8ba695df899098436))
+
+        - **i.MX 8M Plus**
+
+          - change the BL31 physical load address ([32d5042](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/32d5042204e8b41caa4c0c1ed5b48bad9f1cb1b5))
+
+    - **Layerscape**
+
+      - fix build issue of mmap_add_ddr_region_dynamically ([e2818d0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e2818d0afc20a60d302f85f4c915e4ae4cc3cb9c))
+      - fix coverity issue ([5161cfd](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5161cfde9bfaa3a715d160fcd4870f276adad332))
+      - update WA for Errata A-050426 ([72feaad](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/72feaad980cdc472868bc95914202bf57ed51b2d))
+
+      - **LX2**
+
+        - drop erratum A-009810 ([e36b0e4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e36b0e4910aea56f90a6ab9b8cf3dc4008220031))
+
+  - **Renesas**
+
+    - **R-Car**
+
+      - **R-Car 3**
+
+        - change stack size of BL31 ([d544dfc](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d544dfcc4959d203b06dbfb85fb0ad895178b379))
+        - fix SYSTEM_OFF processing for R-Car D3 ([1b49ba0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1b49ba0fde5eb9e47fe50152c192579101feb718))
+        - fix to bit operation for WUPMSKCA57/53 ([82bb6c2](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/82bb6c2e88314a5b3f2326c95095c3b20a389947))
+
+  - **Socionext**
+
+    - **Synquacer**
+
+      - initialise CNTFRQ in Non Secure CNTBaseN ([4d4911d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/4d4911d77d4d59c7dd18d7fc3724ddb1fa3582b7))
+
+  - **ST**
+
+    - add missing header include ([b1391b2](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b1391b294ca7803f46bc47048b4a02a15dda9a16))
+    - don't try to read boot partition on SD cards ([9492b39](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9492b391a35c66e1e7630e95347259191b28314d))
+    - fix NULL pointer dereference issues ([2deff90](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2deff904a953c6a87331ab6830ab80e3889d9e23))
+    - manage UART clock and reset only in BL2 ([9e52d45](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9e52d45fdf619561e0a7a833b77aaacc947a4dfd))
+    - remove extra chars from dtc version ([03d2077](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/03d20776efc20a04a5191a4f39965079a4d60b3c))
+
+    - **ST32MP1**
+
+      - add missing debug.h ([356ed96](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/356ed961184847dcd088cfcda44b71eeb0ef2377))
+      - correct dtc version check ([429f10e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/429f10e3367748abd33b4f6f9ee362c0ba74dd95))
+      - correct include order ([ff7675e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ff7675ebf94999618dbde14bb59741cefb2b2edd))
+      - correct types in messages ([43bbdca](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/43bbdca04f5a20bb4e648e18fc63061b6a6e4ecf))
+      - deconfigure UART RX pins ([d7176f0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d7176f0319cd399aae9a906e5d78e67b32e183f5))
+      - do not reopen debug features ([21cfa45](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/21cfa4531a76a7c3cad00e874400b97e2f68723c))
+      - fix enum prints ([ceab2fc](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ceab2fc3442dbda1c4beaff3c4fe708a04c02303))
+      - include assert.h to fix build failure ([570c71b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/570c71b20a195ade510f5d584c69325d2634c50b))
+      - remove interrupt_provider warning for dtc ([ca88c76](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ca88c761d34854ed3e0b16b9c5f39b0790d320ab))
+      - restrict DEVICE2 mapping in BL2 ([db3e0ec](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/db3e0ece7157181a3529d14172368003eb63dc30))
+      - rework switch/case for MISRA ([f7130e8](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f7130e81cf9c3682232bb9319b1798184b44920f))
+      - set reset pulse duration to 31ms ([9a73a56](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9a73a56c353d32742e03b828647562bdbe2ddbb2))
+
+  - **Xilinx**
+
+    - fix coding style violations ([bb1768c](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/bb1768c67ea06ac466e2cdc7e5338c3d23dac79d))
+    - fix mismatching function prototype ([81333ea](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/81333eac716b25a9fd112cc4f5990e069f3bdb40))
+
+    - **Versal**
+
+      - resolve misra R10.1 in pm services ([775bf1b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/775bf1bbd32c2df47f4ff597eb8a452d2983e590))
+      - resolve misra R10.3 ([b2bb3ef](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b2bb3efb8f590f31b1205c51d56be1dd6f473fbb))
+      - resolve misra R10.3 in pm services ([5d1c211](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5d1c211e225d40d2926bf34483c90f907a6c5dc3))
+      - resolve misra R10.6 ([93d4625](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/93d462562727f4f428e6f975a972226dafbfd305))
+      - resolve misra R10.6 in pm services ([fa98d7f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/fa98d7f2f8752e37f740b43f533547288552a393))
+      - resolve misra R14.4 ([a62c40d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a62c40d42703d5f60a8d80938d2cff721ee131bd))
+      - resolve misra R15.6 ([b9fa2d9](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b9fa2d9fc154feffe78e677ace54b0e34f011439))
+      - resolve misra R15.6 in pm services ([4156719](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/4156719550ceddf5b1b4a47464fb32f7506e0dca))
+      - resolve misra R15.7 ([bc2637e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/bc2637e3799dbc9642447ddb719e0262347b1309))
+      - resolve misra R16.3 in pm services ([27ae531](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/27ae5310883b0db7d4e2dd4fbc1fd58e675f75b5))
+      - resolve misra R17.7 ([526a1fd](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/526a1fd1472874561988777f8ecd8b87734a0671))
+      - resolve misra R20.7 in pm services ([5dada62](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5dada6227b949ef702bfab7986bc083689afdaf7))
+      - resolve misra R7.2 ([0623dce](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0623dcea0f6e7a5c9d65413445df8a96a2b40d42))
+      - fix coverity scan warnings ([0b15187](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0b15187225a9134e3acbc7693646b21d43617b3b))
+      - fix the incorrect log message ([ea04b3f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ea04b3fe183b6661f656b4cc38cb93a73d9bc202))
+
+    - **ZynqMP**
+
+      - define and enable ARM_XLAT_TABLES_LIB_V1 ([c884c9a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c884c9a55b167383ff3d96d2d0a30ac6842bcc86))
+      - query node status to power up APU ([b35b556](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b35b556718b60b78cb5d96b0c137e2fe82eb0086))
+      - resolve misra 7.2 warnings ([5bcbd2d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5bcbd2de127292f3ad076217e08468388c6844b0))
+      - resolve misra 8.3 warnings ([944e7ea](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/944e7ea94f2594e2b128c671cf7415265302596b))
+      - resolve misra R10.3 ([2b57da6](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2b57da6c91ebe14588e63e5a24f31ef32711eca2))
+      - resolve misra R14.4 warnings ([dd1fe71](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/dd1fe7178b578916b1e133b7c65c183e1f994371))
+      - resolve misra R15.6 warnings ([eb0d2b1](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/eb0d2b17722c01a22bf3ec1123f7bed2bf891b09))
+      - resolve misra R15.7 warnings ([16de22d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/16de22d037644359ef2a04058134f9c326b36633))
+      - resolve misra R16.3 warnings ([e7e5d30](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e7e5d30308ccfb931f7b6d0afa6c5c23971e95c0))
+      - resolve misra R8.4 warnings ([610eeac](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/610eeac89438d603435bde694eb4ddab07f46e45))
+      - update the log message to verbose ([1277af9](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1277af9bacca36b46d7aa341187bb3abef84332f))
+      - use common interface for eemi apis ([a469c1e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a469c1e1f4c1cd69f98ce45d6e0709de091b8cb3))
+
+- **Bootloader Images**
+
+  - **BL1**
+
+    - invalidate SP in data cache during secure SMC ([f1cbbd6](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f1cbbd6332bb85672dc72cbcc4ac7023323c6936))
+
+  - **BL2**
+
+    - correct messages with image_id ([e4c77db](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e4c77db9c80d87009611a3079454877e6ce45a04))
+    - define RAM_NOLOAD for XIP ([cc562e7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/cc562e74101d800b0b0ee3422fb7f4f8321ae2b7))
+
+- **Services**
+
+  - **RME**
+
+    - enable/disable SVE/FPU for Realms ([a4cc85c](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a4cc85c129d031d9c887cf59b1baeaef18a43010))
+    - align RMI and GTSI FIDs with SMCCC ([b9fd2d3](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b9fd2d3ce3d4e543a2e04dc237cd4e7ff7765c7a))
+    - preserve x4-x7 as per SMCCCv1.1 ([1157830](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/11578303fd04a8da36fddb5e6de44f026bf4d24c))
+
+    - **TRP**
+
+      - Distinguish between cold and warm boot ([00e8113](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/00e8113145aa12d89db72068bdd3157f08575d14))
+
+  - **SPM**
+
+    - **EL3 SPMC**
+
+      - fix incorrect FF-A version usage ([25eb2d4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/25eb2d41a6d2ede1e945bbc67ae3f740b92a40bb))
+      - fix FF-A memory transaction validation ([3954bc3](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/3954bc3c03439dbdc7029cf2418c79a037918ce4))
+
+- **Libraries**
+
+  - **CPU Support**
+
+    - workaround for  Cortex-A710 2282622 ([ef934cd](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ef934cd17c30dcc39cd9022a1c4e9523ec8ba617))
+    - workaround for  Cortex-A710 erratum 2267065 ([cfe1a8f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/cfe1a8f7123f0dc8376b2075cc6e8e32b13739b2))
+    - workaround for Cortex A78 AE erratum 2376748 ([92e8708](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/92e870843e9bd654fd1041d66f284c19ca9c0d4f))
+    - workaround for Cortex A78 AE erratum 2395408 ([3f4d81d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/3f4d81dfd26649fbcbbbe993a9f0236f5bb07c8a))
+    - workaround for Cortex X2 erratum 2002765 ([34ee76d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/34ee76dbdfeee85f123cb903ea95dbee5e9a44a5))
+    - workaround for Cortex X2 erratum 2058056 ([e16045d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e16045de50e8b430e6601ba0e1e47097d8310f3d))
+    - workaround for Cortex X2 erratum 2083908 ([1db6cd6](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1db6cd60279e2d082876692a65cf9c532f506a69))
+    - workaround for Cortex-A510 erratum 1922240 ([8343563](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/83435637bfafbf1ce642a5fabb52e8d7b2819e36))
+    - workaround for Cortex-A510 erratum 2041909 ([e72bbe4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e72bbe47ba7f2a0087654fd99ae24b5b7b444943))
+    - workaround for Cortex-A510 erratum 2042739 ([d48088a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d48088acbe400133037ae74acf1b722b059119bb))
+    - workaround for Cortex-A510 erratum 2172148 ([c0959d2](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c0959d2c460cbf7c14e7ba2a57d69ecddae80fd8))
+    - workaround for Cortex-A510 erratum 2218950 ([cc79018](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/cc79018b71e45acb524fc5d429d394497ad53646))
+    - workaround for Cortex-A510 erratum 2250311 ([7f304b0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7f304b02a802b7293d7a8b4f4030c5ff00158404))
+    - workaround for Cortex-A510 erratum 2288014 ([d5e2512](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d5e2512c6b86409686f5d1282922ebdf72459fc2))
+    - workaround for Cortex-A710 erratum 2008768 ([af220eb](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/af220ebbe467aa580e6b9ba554676f78ffec930f))
+    - workaround for Cortex-A710 erratum 2136059 ([8a855bd](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/8a855bd24329e081cf13a257c7d2dc3ab4e5dcca))
+    - workaround for Cortex-A78 erratum 2376745 ([5d796b3](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5d796b3a25150faff68013880f5a9350cbc53889))
+    - workaround for Cortex-A78 erratum 2395406 ([3b577ed](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/3b577ed53d104cfb324390b7519da5e7744d1001))
+    - workaround for Cortex-X2 errata 2017096 ([e7ca443](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e7ca4433fa591233e7e2912b689ab56e531f9775))
+    - workaround for Cortex-X2 errata 2081180 ([c060b53](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c060b5337a43cd42f55b99d83096bb44b51b5335))
+    - workaround for Cortex-X2 erratum 2147715 ([63446c2](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/63446c27d11453faacfddecffa44d3880615d412))
+    - workaround for Cortex-X2 erratum 2216384 ([4dff759](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/4dff7594f94f1e788aef709cc5b3d079693b6242))
+    - workaround for DSU-110 erratum 2313941 ([7e3273e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7e3273e8e4dca44e7cb88a827b94e662fa8f83e9))
+    - workaround for Rainier erratum 1868343 ([a72144f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a72144fb7a30c2782a583a3b0064e741d1fe2c9f))
+    - workarounds for cortex-x1 errata ([7b76c20](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7b76c20d8eb4271b381371ce0d510fbe6ad825bf))
+    - use CPU_NO_EXTRA3_FUNC for all variants ([b2ed998](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b2ed99894d326993961680fb8e786c267a712400))
+
+  - **EL3 Runtime**
+
+    - set unset pstate bits to default ([7d33ffe](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7d33ffe4c116506ed63e820d5b6edad81680cd11))
+
+    - **Context Management**
+
+      - add barrier before el3 ns exit ([0482503](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/04825031b2384a08504821f39e98e23bb6f93f11))
+      - remove registers accessible only from secure state from EL2 context ([7f41bcc](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7f41bcc76d8857b4678c90796ebd85794ff3ee5f))
+      - refactor the cm_setup_context function ([2bbad1d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2bbad1d126248435e26f9d0d9f5920d8806148d7))
+      - remove initialization of EL2 registers when EL2 is used ([fd5da7a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/fd5da7a84731e9687f56c263ff3aa8ebed75075a))
+      - add cm_prepare_el3_exit_ns function ([8b95e84](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/8b95e8487006ff77a7d84fba5bd20ba7e68d8330))
+      - refactor initialization of EL1 context registers ([b515f54](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b515f5414b00a8b7ca9b21363886ea976bd19914))
+
+  - **FCONF**
+
+    - correct image_id type in messages ([cec2fb2](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/cec2fb2b1a8359bf1f349a5b8c8a91a1845f4ca1))
+
+  - **PSCI**
+
+    - correct parent_node type in messages ([b9338ee](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b9338eee7fbcac7f4b55f27b064572e847810422))
+
+  - **GPT**
+
+    - rework delegating/undelegating sequence ([6a00e9b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6a00e9b0c8c37fc446f83ef63e95a75353e31e8b))
+
+  - **Translation Tables**
+
+    - fix bug on VERBOSE trace ([956d76f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/956d76f69d0c96829784c5a6d16aa79e4e0ecab1))
+
+  - **Standard C Library**
+
+    - correct some messages ([a211fde](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a211fde940d4dbd8e95e4f352af2a066a4f89f30))
+    - fix snprintf corner cases ([c1f5a09](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c1f5a0925ddf84981d9e176d146bfddb48eb45d1))
+    - limit snprintf radix value ([b30dd40](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b30dd4030dcef950eac05393013ee019c3cb3205))
+    - snprintf: include stdint.h ([410c925](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/410c925ab31693dc74d654ff9167c8eed3ec5a62))
+
+  - **Locks**
+
+    - add __unused for clang ([5a030ce](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5a030ce4aed271344087bca723903e10fef59ac9))
+
+- **Drivers**
+
+  - **FWU**
+
+    - rename is_fwu_initialized ([aae7c96](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/aae7c96de63914c954f0fc64cd795844832483fc))
+
+  - **I/O**
+
+    - **MTD**
+
+      - correct types in messages ([6e86b46](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6e86b462490429fee6db877338a649b0e199b0ec))
+
+  - **Measured Boot**
+
+    - add RMM entry to event_log_metadata ([f4e3e1e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f4e3e1e85f64d8930e89c1396bc9785512f656bd))
+
+  - **MTD**
+
+    - correct types in messages ([6e86b46](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6e86b462490429fee6db877338a649b0e199b0ec))
+
+  - **SCMI**
+
+    - add missing \n in ERROR message ([0dc9f52](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0dc9f52a2a9f0b9686c65dd60c84e0bcca552144))
+    - make msg_header variable volatile ([99477f0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/99477f051ef857a1e0600cb98858fc74c007e1ff))
+    - use same type for message_id ([2355ebf](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2355ebff6f6312086868f44b8ad7f821f6385208))
+
+  - **UFS**
+
+    - delete call to inv_dcache_range for utrd ([c5ee858](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c5ee8588bf9a36075723e5aacceefa93fd2de8c9))
+    - disables controller if enabled ([b3f03b2](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b3f03b20135fc5fcd5e6ec7e5ca49f1e59b5602e))
+    - don't zero out buf before ufs read ([2ef6b8d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2ef6b8d378e7f7c1b1eb7abe176989c3f996f2dc))
+    - don't zero out the write buffer ([cd3ea90](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/cd3ea90b200534b8c9d81619731c9ce198478a3c))
+    - fix cache maintenance issues ([38a5ecb](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/38a5ecb756e217a80ed951747797ab150449ee9b))
+    - move nutrs assignment to ufs_init ([0956319](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0956319b580726029ddc4e00cde6c5a348b99052))
+    - read and write attribute based on spec ([a475518](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a475518337e15935469543b1cce353e5b337ef52))
+
+  - **Arm**
+
+    - **GIC**
+
+      - **GICv3**
+
+        - fix iroute value wrong issue ([65bc2d2](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/65bc2d224b836c230888796c4eda455997dccd8b))
+
+    - **TZC**
+
+      - **TZC-400**
+
+        - correct message with filter ([bdc88d2](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/bdc88d2154448957f452cb472ff95ccec5808ca1))
+
+  - **Marvell**
+
+    - **COMPHY**
+
+      - change reg_set() / reg_set16() to update semantics ([95c26d6](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/95c26d6489bd8b2fc8b8e14bc2da5d2918055acc))
+
+      - **Armada 3700**
+
+        - drop MODE_REFDIV constant ([9fdecc7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9fdecc72f0fce17ca2cd8e4c3b26c01262166d10))
+        - fix comment about COMPHY status register ([4bcfd8c](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/4bcfd8c02e3e3aa27b55dedeed11fb16bac991a9))
+        - fix comments about selector register values ([71183ef](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/71183ef6654c2a485458307a84ce7c473524689a))
+        - fix Generation Setting registers names ([e5a2aac](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e5a2aac5bbc6dedb20edcc8e7850be2813cb668b))
+        - fix PIN_PU_IVREF register name ([c9f138e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c9f138ebfef90d5b7b5651f06efd81bcbc55366b))
+        - fix reference clock selection value names ([6ba97f8](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/6ba97f83dbb314b076588b97415a4078924e1903))
+        - fix SerDes frequency register value name ([bdcf44f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/bdcf44f1af496e06b693b781fe16bbc2a05fa365))
+        - use reg_set() according to update semantics ([4d01bfe](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/4d01bfe66522b13f0d9042206e986551c94fc01e))
+
+    - **Armada**
+
+      - **A3K**
+
+        - **A3720**
+
+          - configure UART after TX FIFO reset ([15546db](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/15546dbf40e5ea81a982a1e6d1e5ba729b06ae51))
+          - do external reset during initialization ([0ee80f3](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0ee80f35a28d651d243a6d56678800f9697d14c0))
+
+  - **NXP**
+
+    - ddr: corrects mapping of HNFs nodes ([e3a2349](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e3a234971abb2402cbf376eca6fcb657a7709fae))
+
+    - **QSPI**
+
+      - fix include path for QSPI driver ([ae95b17](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ae95b1782b7a3ab9bbe46ae9ab31f48fb6ebe137))
+
+    - **NXP Crypto**
+
+      - refine code to avoid hang issue for some of toolchain ([fa7fdfa](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/fa7fdfabf07d91439b0869ffd8e805f0166294bf))
+
+    - **DDR**
+
+      - fix coverity issue ([f713e59](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/f713e5954e0906443cd20ae97e229ddbb9ab7005))
+
+  - **ST**
+
+    - **Clock**
+
+      - check _clk_stm32_get_parent return ([b8eab51](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b8eab512bf9d253f96b0333ee0f1bffa1afc3170))
+      - correct stm32_clk_parse_fdt_by_name ([7417cda](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/7417cda6aeaf6abf48dfbe22dc965b626f61c613))
+      - correct types in error messages ([44fb470](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/44fb470b7f298645ac31ada4491553824d77d934))
+      - initialize pllcfg table ([175758b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/175758b2777eb6df3c4aefd79448e97e76a15272))
+      - print enums as unsigned ([9fa9a0c](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9fa9a0c55cc830e609415d2cedd2d34fcbec1008))
+
+    - **DDR**
+
+      - add missing debug.h ([15ca2c5](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/15ca2c5e14abe415e70d08fb595973dd3e3b0af9))
+      - correct DDR warnings ([a078134](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a078134e2305ca5695731bc275a5ca892cc38880))
+
+     - **FMC**
+
+      - fix type in message ([afcdc9d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/afcdc9d8d71e2b60071d3d34704f0e598e67a514))
+
+    - **SDMMC2**
+
+      - check regulator enable/disable return ([d50e7a7](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/d50e7a71cb5f8ecfbe2eb69c163d532bab82cbf0))
+      - correct cmd_idx type in messages ([bc1c98a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/bc1c98a8c79b6f72395123ea8ed857a488746d4b))
+
+    - **ST PMIC**
+
+      - add static const to pmic_ops ([57e6018](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/57e6018305a97f4e3627d16d8b1886419f274b4a))
+      - correct verbose message ([47065ff](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/47065ffe44c701b231322ec7160c8624d50a9deb))
+
+    - **SPI**
+
+      - always check SR_TCF flags in stm32_qspi_wait_cmd() ([55de583](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/55de58323e458b38b455439a8846cb663deb5508))
+      - remove SR_BUSY bit check before sending command ([5993b91](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5993b9157fd049d06194083032771ffcf73da086))
+
+    - **UART**
+
+      - correctly fill BRR register ([af7775a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/af7775ab535138ff49643f749110dca143d4122c))
+
+  - **USB**
+
+    - correct type in message ([bd9cd63](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/bd9cd63ba096cb16161efa4df40f957421660df1))
+
+- **Miscellaneous**
+
+  - **AArch64**
+
+    - fix encodings for MPAMVPM* registers ([e926558](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e92655849d0a9e5893eb2d7e5f42cf8b931d4db6))
+
+  - **FDTs**
+
+    - **STM32MP1**
+
+      - correct memory mapping for STM32MP13 ([99605fb](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/99605fb1166794db1dedf1b7280cb184945c229c))
+      - remove mmc1 alias if not needed ([a0e9724](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a0e972438b99012da422411c8e504a19bdad44a2))
+
+  - **PIE**
+
+    - align fixup_gdt_reloc() for aarch64 ([5ecde2a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5ecde2a271ac0f3762c16f5a277a70e55e172f0b))
+    - do not skip __RW_END__ address during relocation ([4f1a658](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/4f1a658f899a169e702b1c7146b59f7c04b0338b))
+
+  - **Security**
+
+    - apply SMCCC_ARCH_WORKAROUND_3 to A73/A75/A72/A57 ([9b2510b](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/9b2510b69de26cc7f571731b415f6dec82669b6c))
+    - loop workaround for CVE-2022-23960 for Cortex-A76 ([a10a5cb](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a10a5cb609045de216c01111ec3fcf09a092da0b))
+    - report CVE 2022 23960 missing for aarch32 A57 and A72 ([2e5d7a4](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/2e5d7a4b6b26d9d8b6c8e580c33d877e591b1fb3))
+    - update Cortex-A15 CPU lib files for CVE-2022-23960 ([187a617](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/187a61761ef5d59bed0c94cca725bd6f116f64d0))
+    - workaround for CVE-2022-23960 ([c2a1521](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/c2a15217c3053117f4d39233002cb1830fa96670))
+    - workaround for CVE-2022-23960 ([1fe4a9d](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/1fe4a9d181ead0dcb2bc494e90552d3e7f0aaf4c))
+    - workaround for CVE-2022-23960 for A76AE, A78AE, A78C ([5f802c8](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/5f802c8832f3c5824ca6de17593205ebbf8bf585))
+    - workaround for CVE-2022-23960 for Cortex-A57, Cortex-A72 ([be9121f](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/be9121fd311ff48c94f3d90fe7efcf84586119e4))
+    - workaround for CVE-2022-23960 for Cortex-X1 ([e81e999](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e81e999b9da33ab5d2d3e5185b1ad7c46046329c))
+
+- **Tools**
+
+  - **NXP Tools**
+
+    - fix create_pbl print log ([31af441](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/31af441a0445d4a5e88ddcc371c51b3701c25839))
+    - fix tool location path for byte_swape ([a89412a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/a89412a649020367a3ed0f87658ee131cd3dcd18))
+
+  - **Firmware Image Package Tool**
+
+    - avoid packing the zero size images in the FIP ([ab556c9](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ab556c9c646f1b5f1b500449a5813a4eecdc0302))
+    - respect OPENSSL_DIR ([0a956f8](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/0a956f81805b46b1530f30dd79d16950dc491a7b)
+
+  - **Secure Partition Tool**
+
+    - add leading zeroes in UUID conversion ([b06344a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/b06344a3f2c5a0fede3646627f37d1fce3d3d585))
+    - update Optee FF-A manifest ([ca0fdbd](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/ca0fdbd8e0d625ece0f87ca16eacabf13db70921))
+
+  - **Certificate Creation Tool**
+
+    - let distclean Makefile target remove the cert_create tool ([e15591a](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/e15591aaf47ab45941f0d7a03abf3e4a830ac1d9))
+
+- **Dependencies**
+
+  - **commitlint**
+
+    - change scope-case to lower-case ([804e52e](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/804e52e9a770de72913f27b5bc9e7dd965e114c5))
+
 ## [2.6.0](https://review.trustedfirmware.org/plugins/gitiles/TF-A/trusted-firmware-a/+/refs/tags/v2.5..refs/tags/v2.6) (2021-11-22)
 
 ### ⚠ BREAKING CHANGES
@@ -4738,7 +5928,7 @@
 
 ______________________________________________________________________
 
-*Copyright (c) 2013-2020, Arm Limited and Contributors. All rights reserved.*
+*Copyright (c) 2013-2022, Arm Limited and Contributors. All rights reserved.*
 
 [mbed tls releases]: https://tls.mbed.org/tech-updates/releases
 [pr#1002]: https://github.com/ARM-software/arm-trusted-firmware/pull/1002#issuecomment-312650193

diff --git a/docs/components/fconf/fconf_properties.rst b/docs/components/fconf/fconf_properties.rst
index 5c28a7a..20cc758 100644
--- a/docs/components/fconf/fconf_properties.rst
+++ b/docs/components/fconf/fconf_properties.rst

@@ -30,3 +30,10 @@
     - value type: <u32>
     - Image ID of the configuration.
 
+- ns-load-address [optional]
+    - value type: <u64>
+    - Physical loading base address of the configuration in the non-secure
+      memory.
+      Only needed by those configuration files which require being loaded
+      in secure memory (at load-address) as well as in non-secure memory
+      e.g. HW_CONFIG

diff --git a/docs/components/ffa-manifest-binding.rst b/docs/components/ffa-manifest-binding.rst
index df2985c..6d2f905 100644
--- a/docs/components/ffa-manifest-binding.rst
+++ b/docs/components/ffa-manifest-binding.rst

@@ -4,11 +4,8 @@
 This document defines the nodes and properties used to define a partition,
 according to the FF-A specification.
 
-Version 1.0
------------
-
 Partition Properties
-^^^^^^^^^^^^^^^^^^^^
+--------------------
 
 - compatible [mandatory]
    - value type: <string>
@@ -137,20 +134,30 @@
 
 - gp-register-num
    - value type: <u32>
-   - Presence of this field indicates that the partition expects the
-     ffa_init_info structure to be passed in via the specified general purpose
-     register.
-     The field specifies the general purpose register number but not its width.
+   - The field specifies the general purpose register number but not its width.
      The width is derived from the partition's execution state, as specified in
      the partition properties. For example, if the number value is 1 then the
      general-purpose register used will be x1 in AArch64 state and w1 in AArch32
      state.
+     Presence of this field indicates that the partition expects the address of
+     the FF-A boot information blob to be passed in the specified general purpose
+     register.
 
 - stream-endpoint-ids
    - value type: <prop-encoded-array>
    - List of <u32> tuples, identifying the IDs this partition is acting as
      proxy for.
 
+- power-management-messages
+   - value type: <u32>
+   - Specifies which power management messages a partition subscribes to.
+     A set bit means the partition should be informed of the power event, clear
+     bit - should not be informed of event:
+
+      - Bit[0]: CPU_OFF
+      - Bit[1]: CPU_SUSPEND
+      - Bit[2]: CPU_SUSPEND_RESUME
+
 Memory Regions
 --------------
 
@@ -174,13 +181,14 @@
       - 0x1: Read
       - 0x2: Write
       - 0x4: Execute
+      - 0x8: Security state
 
 - base-address
    - value type: <u64>
    - Base address of the region. The address must be aligned to the translation
      granule size.
      The address given may be a Physical Address (PA), Virtual Address (VA), or
-     Intermediate Physical Address (IPA). Refer to the FFA specification for
+     Intermediate Physical Address (IPA). Refer to the FF-A specification for
      more information on the restrictions around the address type.
      If the base address is omitted then the partition manager must map a memory
      region of the specified size into the partition's translation regime and
@@ -198,14 +206,10 @@
    - value type: <string>
    - Name of the device region e.g. for debugging purposes.
 
-- reg [mandatory]
-   - value type: <prop-encoded-array>
-   - A (address, num-pages) pair describing the device, where:
-
-      - address: The physical base address <u64> value of the device MMIO
-        region.
-      - num-pages: The <u32> number of pages of the region. The total size of
-        the region is this value multiplied by the translation granule size.
+- pages-count [mandatory]
+   - value type: <u32>
+   - Count of pages of memory region as a multiple of the translation granule
+     size
 
 - attributes [mandatory]
    - value type: <u32>
@@ -214,6 +218,15 @@
      - 0x1: Read
      - 0x2: Write
      - 0x4: Execute
+     - 0x8: Security state
+
+- base-address [mandatory]
+   - value type: <u64>
+   - Base address of the region. The address must be aligned to the translation
+     granule size.
+     The address given may be a Physical Address (PA), Virtual Address (VA), or
+     Intermediate Physical Address (IPA). Refer to the FF-A specification for
+     more information on the restrictions around the address type.
 
 - smmu-id
    - value type: <u32>
@@ -233,14 +246,32 @@
    - A list of (id, attributes) pair describing the device interrupts, where:
 
       - id: The <u32> interrupt IDs.
-      - attributes: A <u32> value,
-        containing the attributes for each interrupt ID:
+      - attributes: A <u32> value, containing attributes for each interrupt ID:
+
+        +----------------------+----------+
+        |Field                 | Bit(s)   |
+        +----------------------+----------+
+        | Priority	       | 7:0      |
+        +----------------------+----------+
+        | Security state       | 8        |
+        +----------------------+----------+
+        | Config(Edge/Level)   | 9        |
+        +----------------------+----------+
+        | Type(SPI/PPI/SGI)    | 11:10    |
+        +----------------------+----------+
 
-         - Interrupt type: SPI, PPI, SGI
-         - Interrupt configuration: Edge triggered, Level triggered
-         - Interrupt security state: Secure, Non-secure
-         - Interrupt priority value
-         - Target execution context/vCPU for each SPI
+        Security state:
+          - Secure:       1
+          - Non-secure:   0
+
+        Configuration:
+          - Edge triggered:       0
+          - Level triggered:      1
+
+        Type:
+          - SPI:  0b10
+          - PPI:  0b01
+          - SGI:  0b00
 
 - exclusive-access
    - value type: <empty>
@@ -249,4 +280,4 @@
 
 --------------
 
-*Copyright (c) 2019-2021, Arm Limited and Contributors. All rights reserved.*
+*Copyright (c) 2019-2022, Arm Limited and Contributors. All rights reserved.*

diff --git a/docs/components/index.rst b/docs/components/index.rst
index 95fe42c..192773f 100644
--- a/docs/components/index.rst
+++ b/docs/components/index.rst

@@ -21,8 +21,8 @@
    sdei
    secure-partition-manager
    secure-partition-manager-mm
-   ffa-manifest-binding
    xlat-tables-lib-v2-design
    cot-binding
    realm-management-extension
+   rmm-el3-comms-spec
    granule-protection-tables-design

diff --git a/docs/components/realm-management-extension.rst b/docs/components/realm-management-extension.rst
index 5fa5140..ea921fc 100644
--- a/docs/components/realm-management-extension.rst
+++ b/docs/components/realm-management-extension.rst

@@ -73,6 +73,14 @@
 world. It initializes the RMM and handles Realm Management Interface (RMI)
 SMC calls from Non-secure and Realm worlds.
 
+There is a contract between RMM and RMMD that defines the arguments that the
+former needs to take in order to initialize and also the possible return values.
+This contract is defined in the RMM Boot Interface, which can be found at
+:ref:`rmm_el3_boot_interface`.
+
+There is also a specification of the runtime services provided by TF-A
+to RMM. This can be found at :ref:`runtime_services_and_interface`.
+
 Test Realm Payload (TRP)
 *************************
 TRP is a small test payload that runs at R-EL2 and implements a subset of

diff --git a/docs/components/rmm-el3-comms-spec.rst b/docs/components/rmm-el3-comms-spec.rst
new file mode 100644
index 0000000..9140d54
--- /dev/null
+++ b/docs/components/rmm-el3-comms-spec.rst

@@ -0,0 +1,413 @@
+RMM-EL3 Communication interface
+*******************************
+
+This document defines the communication interface between RMM and EL3.
+There are two parts in this interface: the boot interface and the runtime
+interface.
+
+The Boot Interface defines the ABI between EL3 and RMM when the CPU enters
+R-EL2 for the first time after boot. The cold boot interface defines the ABI
+for the cold boot path and the warm boot interface defines the same for the
+warm path.
+
+The RMM-EL3 runtime interface defines the ABI for EL3 services which can be
+invoked by RMM as well as the register save-restore convention when handling an
+SMC call from NS.
+
+The below sections discuss these interfaces more in detail.
+
+.. _rmm_el3_ifc_versioning:
+
+RMM-EL3 Interface versioning
+____________________________
+
+The RMM Boot and Runtime Interface uses a version number to check
+compatibility with the register arguments passed as part of Boot Interface and
+RMM-EL3 runtime interface.
+
+The Boot Manifest, discussed later in section :ref:`rmm_el3_boot_manifest`,
+uses a separate version number but with the same scheme.
+
+The version number is a 32-bit type with the following fields:
+
+.. csv-table::
+   :header: "Bits", "Value"
+
+   [0:15],``VERSION_MINOR``
+   [16:30],``VERSION_MAJOR``
+   [31],RES0
+
+The version numbers are sequentially increased and the rules for updating them
+are explained below:
+
+  - ``VERSION_MAJOR``: This value is increased when changes break
+    compatibility with previous versions. If the changes
+    on the ABI are compatible with the previous one, ``VERSION_MAJOR``
+    remains unchanged.
+
+  - ``VERSION_MINOR``: This value is increased on any change that is backwards
+    compatible with the previous version. When ``VERSION_MAJOR`` is increased,
+    ``VERSION_MINOR`` must be set to 0.
+
+  - ``RES0``: Bit 31 of the version number is reserved 0 as to maintain
+    consistency with the versioning schemes used in other parts of RMM.
+
+This document specifies the 0.1 version of Boot Interface ABI and RMM-EL3
+services specification and the 0.1 version of the Boot Manifest.
+
+.. _rmm_el3_boot_interface:
+
+RMM Boot Interface
+__________________
+
+This section deals with the Boot Interface part of the specification.
+
+One of the goals of the Boot Interface is to allow EL3 firmware to pass
+down into RMM certain platform specific information dynamically. This allows
+RMM to be less platform dependent and be more generic across platform
+variations. It also allows RMM to be decoupled from the other boot loader
+images in the boot sequence and remain agnostic of any particular format used
+for configuration files.
+
+The Boot Interface ABI defines a set of register conventions and
+also a memory based manifest file to pass information from EL3 to RMM. The
+boot manifest and the associated platform data in it can be dynamically created
+by EL3 and there is no restriction on how the data can be obtained (e.g by DTB,
+hoblist or other).
+
+The register convention and the manifest are versioned separately to manage
+future enhancements and compatibility.
+
+RMM completes the boot by issuing the ``RMM_BOOT_COMPLETE`` SMC (0xC40001CF)
+back to EL3. After the RMM has finished the boot process, it can only be
+entered from EL3 as part of RMI handling.
+
+If RMM returns an error during boot (in any CPU), then RMM must not be entered
+from any CPU.
+
+.. _rmm_cold_boot_interface:
+
+Cold Boot Interface
+~~~~~~~~~~~~~~~~~~~
+
+During cold boot RMM expects the following register values:
+
+.. csv-table::
+   :header: "Register", "Value"
+   :widths: 1, 5
+
+   x0,Linear index of this PE. This index starts from 0 and must be less than the maximum number of CPUs to be supported at runtime (see x2).
+   x1,Version for this Boot Interface as defined in :ref:`rmm_el3_ifc_versioning`.
+   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
+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
+used to populate the Boot Manifest. Since the Boot Manifest can be accessed by
+RMM prior to enabling its MMU, EL3 must ensure that proper cache maintenance
+operations are performed after the Boot Manifest is populated.
+
+EL3 should also ensure that this shared buffer is always available for use by RMM
+during the lifetime of the system and that it can be used for runtime
+communication between RMM and EL3. For example, when RMM invokes attestation
+service commands in EL3, this buffer can be used to exchange data between RMM
+and EL3. It is also allowed for RMM to invoke runtime services provided by EL3
+utilizing this buffer during the boot phase, prior to return back to EL3 via
+RMM_BOOT_COMPLETE SMC.
+
+RMM should map this memory page into its Stage 1 page-tables using Normal
+memory attributes.
+
+During runtime, it is the RMM which initiates any communication with EL3. If that
+communication requires the use of the shared area, it is expected that RMM needs
+to do the necessary concurrency protection to prevent the use of the same buffer
+by other PEs.
+
+The following sequence diagram shows how a generic EL3 Firmware would boot RMM.
+
+.. image:: ../resources/diagrams/rmm_cold_boot_generic.png
+
+Warm Boot Interface
+~~~~~~~~~~~~~~~~~~~
+
+At warm boot, RMM is already initialized and only some per-CPU initialization
+is still pending. The only argument that is required by RMM at this stage is
+the CPU Id, which will be passed through register x0 whilst x1 to x3 are RES0.
+This is summarized in the following table:
+
+.. csv-table::
+   :header: "Register", "Value"
+   :widths: 1, 5
+
+   x0,Linear index of this PE. This index starts from 0 and must be less than the maximum number of CPUs to be supported at runtime (see x2).
+   x1 - x3,RES0
+
+Boot error handling and return values
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+After boot up and initialization, RMM returns control back to EL3 through a
+``RMM_BOOT_COMPLETE`` SMC call. The only argument of this SMC call will
+be returned in x1 and it will encode a signed integer with the error reason
+as per the following table:
+
+.. csv-table::
+   :header: "Error code", "Description", "ID"
+   :widths: 2 4 1
+
+   ``E_RMM_BOOT_SUCCESS``,Boot successful,0
+   ``E_RMM_BOOT_ERR_UNKNOWN``,Unknown error,-1
+   ``E_RMM_BOOT_VERSION_NOT_VALID``,Boot Interface version reported by EL3 is not supported by RMM,-2
+   ``E_RMM_BOOT_CPUS_OUT_OF_RAGE``,Number of CPUs reported by EL3 larger than maximum supported by RMM,-3
+   ``E_RMM_BOOT_CPU_ID_OUT_OF_RAGE``,Current CPU Id is higher or equal than the number of CPUs supported by RMM,-4
+   ``E_RMM_BOOT_INVALID_SHARED_BUFFER``,Invalid pointer to shared memory area,-5
+   ``E_RMM_BOOT_MANIFEST_VERSION_NOT_SUPPORTED``,Version reported by the boot manifest not supported by RMM,-6
+   ``E_RMM_BOOT_MANIFEST_DATA_ERROR``,Error parsing core boot manifest,-7
+
+For any error detected in RMM during cold or warm boot, RMM will return back to
+EL3 using ``RMM_BOOT_COMPLETE`` SMC with an appropriate error code. It is
+expected that EL3 will take necessary action to disable Realm world for further
+entry from NS Host on receiving an error. This will be done across all the PEs
+in the system so as to present a symmetric view to the NS Host. Any further
+warm boot by any PE should not enter RMM using the warm boot interface.
+
+.. _rmm_el3_boot_manifest:
+
+Boot Manifest
+~~~~~~~~~~~~~
+
+During cold boot, EL3 Firmware passes a memory boot manifest to RMM containing
+platform information.
+
+This boot manifest is versioned independently of the boot interface, to help
+evolve the boot manifest independent of the rest of Boot Manifest.
+The current version for the boot manifest is ``v0.1`` and the rules explained
+in :ref:`rmm_el3_ifc_versioning` apply on this version as well.
+
+The boot manifest is divided into two different components:
+
+   - Core Manifest: This is the generic parameters passed to RMM by EL3 common to all platforms.
+   - Platform data: This is defined by the platform owner and contains information specific to that platform.
+
+For the current version of the manifest, the core manifest contains a pointer
+to the platform data. EL3 must ensure that the whole boot manifest,
+including the platform data, if available, fits inside the RMM EL3 shared
+buffer.
+
+For the type specification of the RMM Boot Manifest v0.1, refer to
+:ref:`rmm_el3_manifest_struct`
+
+.. _runtime_services_and_interface:
+
+RMMM-EL3 Runtime Interface
+__________________________
+
+This section defines the RMM-EL3 runtime interface which specifies the ABI for
+EL3 services expected by RMM at runtime as well as the register save and
+restore convention between EL3 and RMM as part of RMI call handling. It is
+important to note that RMM is allowed to invoke EL3-RMM runtime interface
+services during the boot phase as well. The EL3 runtime service handling must
+not result in a world switch to another world unless specified. Both the RMM
+and EL3 are allowed to make suitable optimizations based on this assumption.
+
+If the interface requires the use of memory, then the memory references should
+be within the shared buffer communicated as part of the boot interface. See
+:ref:`rmm_cold_boot_interface` for properties of this shared buffer which both
+EL3 and RMM must adhere to.
+
+RMM-EL3 runtime service return codes
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The return codes from EL3 to RMM is a 32 bit signed integer which encapsulates
+error condition as described in the following table:
+
+.. csv-table::
+   :header: "Error code", "Description", "ID"
+   :widths: 2 4 1
+
+   ``E_RMM_OK``,No errors detected,0
+   ``E_RMM_UNK``,Unknown/Generic error,-1
+   ``E_RMM_BAD_ADDR``,The value of an address used as argument was invalid,-2
+   ``E_RMM_BAD_PAS``,Incorrect PAS,-3
+   ``E_RMM_NOMEM``,Not enough memory to perform an operation,-4
+   ``E_RMM_INVAL``,The value of an argument was invalid,-5
+
+If multiple failure conditions are detected in an RMM to EL3 command, then EL3
+is allowed to return an error code corresponding to any of the failure
+conditions.
+
+RMM-EL3 runtime services
+~~~~~~~~~~~~~~~~~~~~~~~~
+
+The following table summarizes the RMM runtime services that need to be
+implemented by EL3 Firmware.
+
+.. csv-table::
+   :header: "FID", "Command"
+   :widths: 2 5
+
+   0xC40001B2,``RMM_ATTEST_GET_REALM_KEY``
+   0xC40001B3,``RMM_ATTEST_GET_PLAT_TOKEN``
+
+RMM_ATTEST_GET_REALM_KEY command
+================================
+
+Retrieve the Realm Attestation Token Signing key from EL3.
+
+FID
+---
+
+``0xC40001B2``
+
+Input values
+------------
+
+.. csv-table::
+   :header: "Name", "Register", "Field", "Type", "Description"
+   :widths: 1 1 1 1 5
+
+   fid,x0,[63:0],UInt64,Command FID
+   buf_pa,x1,[63:0],Address,PA where the Realm Attestation Key must be stored by EL3. The PA must belong to the shared buffer
+   buf_size,x2,[63:0],Size,Size in bytes of the Realm Attestation Key buffer. ``bufPa + bufSize`` must lie within the shared buffer
+   ecc_curve,x3,[63:0],Enum,Type of the elliptic curve to which the requested attestation key belongs to. See :ref:`ecc_curves`
+
+Output values
+-------------
+
+.. csv-table::
+   :header: "Name", "Register", "Field", "Type", "Description"
+   :widths: 1 1 1 1 5
+
+   Result,x0,[63:0],Error Code,Command return status
+   keySize,x1,[63:0],Size,Size of the Realm Attestation Key
+
+Failure conditions
+------------------
+
+The table below shows all the possible error codes returned in ``Result`` upon
+a failure. The errors are ordered by condition check.
+
+.. csv-table::
+   :header: "ID", "Condition"
+   :widths: 1 5
+
+   ``E_RMM_BAD_ADDR``,``PA`` is outside the shared buffer
+   ``E_RMM_INVAL``,``PA + BSize`` is outside the shared buffer
+   ``E_RMM_INVAL``,``Curve`` is not one of the listed in :ref:`ecc_curves`
+   ``E_RMM_UNK``,An unknown error occurred whilst processing the command
+   ``E_RMM_OK``,No errors detected
+
+.. _ecc_curves:
+
+Supported ECC Curves
+--------------------
+
+.. csv-table::
+   :header: "ID", "Curve"
+   :widths: 1 5
+
+   0,ECC SECP384R1
+
+RMM_ATTEST_GET_PLAT_TOKEN command
+=================================
+
+Retrieve the Platform Token from EL3.
+
+FID
+---
+
+``0xC40001B3``
+
+Input values
+------------
+
+.. csv-table::
+   :header: "Name", "Register", "Field", "Type", "Description"
+   :widths: 1 1 1 1 5
+
+   fid,x0,[63:0],UInt64,Command FID
+   buf_pa,x1,[63:0],Address,PA of the platform attestation token. The challenge object is passed in this buffer. The PA must belong to the shared buffer
+   buf_size,x2,[63:0],Size,Size in bytes of the platform attestation token buffer. ``bufPa + bufSize`` must lie within the shared buffer
+   c_size,x3,[63:0],Size,Size in bytes of the challenge object. It corresponds to the size of one of the defined SHA algorithms
+
+Output values
+-------------
+
+.. csv-table::
+   :header: "Name", "Register", "Field", "Type", "Description"
+   :widths: 1 1 1 1 5
+
+   Result,x0,[63:0],Error Code,Command return status
+   tokenSize,x1,[63:0],Size,Size of the platform token
+
+Failure conditions
+------------------
+
+The table below shows all the possible error codes returned in ``Result`` upon
+a failure. The errors are ordered by condition check.
+
+.. csv-table::
+   :header: "ID", "Condition"
+   :widths: 1 5
+
+   ``E_RMM_BAD_ADDR``,``PA`` is outside the shared buffer
+   ``E_RMM_INVAL``,``PA + BSize`` is outside the shared buffer
+   ``E_RMM_INVAL``,``CSize`` does not represent the size of a supported SHA algorithm
+   ``E_RMM_UNK``,An unknown error occurred whilst processing the command
+   ``E_RMM_OK``,No errors detected
+
+RMM-EL3 world switch register save restore convention
+_____________________________________________________
+
+As part of NS world switch, EL3 is expected to maintain a register context
+specific to each world and will save and restore the registers
+appropriately. This section captures the contract between EL3 and RMM on the
+register set to be saved and restored.
+
+EL3 must maintain a separate register context for the following:
+
+   #. General purpose registers (x0-x30) and ``sp_el0``, ``sp_el2`` stack pointers
+   #. EL2 system register context for all enabled features by EL3. These include system registers with the ``_EL2`` prefix. The EL2 physical and virtual timer registers must not be included in this.
+
+It is the responsibility of EL3 that the above registers will not be leaked to
+the NS Host and to maintain the confidentiality of the Realm World.
+
+EL3 will not save some registers as mentioned in the below list. It is the
+responsibility of RMM to ensure that these are appropriately saved if the
+Realm World makes use of them:
+
+   #. FP/SIMD registers
+   #. SVE registers
+   #. SME registers
+   #. EL1/0 registers
+
+SMCCC v1.3 allows NS world to specify whether SVE context is in use. In this
+case, RMM could choose to not save the incoming SVE context but must ensure
+to clear SVE registers if they have been used in Realm World. The same applies
+to SME registers.
+
+Types
+_____
+
+.. _rmm_el3_manifest_struct:
+
+RMM-EL3 Boot Manifest Version
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The RMM-EL3 Boot Manifest structure contains platform boot information passed
+from EL3 to RMM. The width of the Boot Manifest is 128 bits
+
+.. image:: ../resources/diagrams/rmm_el3_manifest_struct.png
+
+The members of the RMM-EL3 Boot Manifest structure are shown in the following
+table:
+
+.. csv-table::
+   :header: "Name", "Range", "Type", Description
+   :widths: 2 1 1 4
+
+   ``Version Minor``,15:0,uint16_t,Version Minor part of the Boot Manifest Version.
+   ``Version Major``,30:16,uint16_t,Version Major part of the Boot Manifest Version.
+   ``RES0``,31,bit,Reserved. Set to 0.
+   ``Platform Data``,127:64,Address,Pointer to the Platform Data section of the Boot Manifest.

diff --git a/docs/components/secure-partition-manager.rst b/docs/components/secure-partition-manager.rst
index 2eaae75..18d870b 100644
--- a/docs/components/secure-partition-manager.rst
+++ b/docs/components/secure-partition-manager.rst

@@ -3,6 +3,9 @@
 
 .. contents::
 
+.. toctree::
+  ffa-manifest-binding
+
 Acronyms
 ========
 
@@ -23,6 +26,8 @@
 +--------+--------------------------------------+
 | IPA    | Intermediate Physical Address        |
 +--------+--------------------------------------+
+| JOP    | Jump-Oriented Programming            |
++--------+--------------------------------------+
 | NWd    | Normal World                         |
 +--------+--------------------------------------+
 | ODM    | Original Design Manufacturer         |
@@ -37,6 +42,8 @@
 +--------+--------------------------------------+
 | PVM    | Primary VM                           |
 +--------+--------------------------------------+
+| ROP    | Return-Oriented Programming          |
++--------+--------------------------------------+
 | SMMU   | System Memory Management Unit        |
 +--------+--------------------------------------+
 | SP     | Secure Partition                     |
@@ -63,24 +70,25 @@
 Foreword
 ========
 
-Two implementations of a Secure Partition Manager co-exist in the TF-A codebase:
+Three implementations of a Secure Partition Manager co-exist in the TF-A
+codebase:
 
-- SPM based on the FF-A specification `[1]`_.
-- SPM based on the MM interface to communicate with an S-EL0 partition `[2]`_.
+#. S-EL2 SPMC based on the FF-A specification `[1]`_, enabling virtualization in
+   the secure world, managing multiple S-EL1 or S-EL0 partitions.
+#. EL3 SPMC based on the FF-A specification, managing a single S-EL1 partition
+   without virtualization in the secure world.
+#. EL3 SPM based on the MM specification, legacy implementation managing a
+   single S-EL0 partition `[2]`_.
 
-Both implementations differ in their architectures and only one can be selected
-at build time.
+These implementations differ in their respective SW architecture and only one
+can be selected at build time. This document:
 
-This document:
-
-- describes the FF-A implementation where the Secure Partition Manager
-  resides at EL3 and S-EL2 (or EL3 and S-EL1).
+- describes the implementation from bullet 1. when the SPMC resides at S-EL2.
 - is not an architecture specification and it might provide assumptions
   on sections mandated as implementation-defined in the specification.
-- covers the implications to TF-A used as a bootloader, and Hafnium
-  used as a reference code base for an S-EL2 secure firmware on
-  platforms implementing the FEAT_SEL2 (formerly Armv8.4 Secure EL2)
-  architecture extension.
+- covers the implications to TF-A used as a bootloader, and Hafnium used as a
+  reference code base for an S-EL2/SPMC secure firmware on platforms
+  implementing the FEAT_SEL2 architecture extension.
 
 Terminology
 -----------
@@ -98,20 +106,23 @@
 Support for legacy platforms
 ----------------------------
 
-In the implementation, the SPM is split into SPMD and SPMC components.
-The SPMD is located at EL3 and mainly relays FF-A messages from
-NWd (Hypervisor or OS kernel) to SPMC located either at S-EL1 or S-EL2.
+The SPM is split into a dispatcher and a core component (respectively SPMD and
+SPMC) residing at different exception levels. To permit the FF-A specification
+adoption and a smooth migration, the SPMD supports an SPMC residing either at
+S-EL1 or S-EL2:
 
-Hence TF-A supports both cases where the SPMC is located either at:
+- The SPMD is located at EL3 and mainly relays the FF-A protocol from NWd
+  (Hypervisor or OS kernel) to the SPMC.
+- The same SPMD component is used for both S-EL1 and S-EL2 SPMC configurations.
+- The SPMC exception level is a build time choice.
 
-- S-EL1 supporting platforms not implementing the FEAT_SEL2 architecture
+TF-A supports both cases:
+
+- S-EL1 SPMC for platforms not supporting the FEAT_SEL2 architecture
   extension. The SPMD relays the FF-A protocol from EL3 to S-EL1.
-- or S-EL2 supporting platforms implementing the FEAT_SEL2 architecture
+- S-EL2 SPMC for platforms implementing the FEAT_SEL2 architecture
   extension. The SPMD relays the FF-A protocol from EL3 to S-EL2.
 
-The same TF-A SPMD component is used to support both configurations.
-The SPMC exception level is a build time choice.
-
 Sample reference stack
 ======================
 
@@ -137,7 +148,7 @@
   SPD=spmd is chosen.
 - **SPMC_AT_EL3**: this option adjusts the SPMC exception level to being
   at EL3.
-- If neither **SPMD_SPM_AT_SEL2** or **SPMC_AT_EL3** are enabled the SPMC
+- 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.
@@ -148,7 +159,7 @@
   providing paths to SP binary images and manifests in DTS format
   (see `Describing secure partitions`_). It
   is required when ``SPMD_SPM_AT_SEL2`` is enabled hence when multiple
-  secure partitions are to be loaded on behalf of the SPMC.
+  secure partitions are to be loaded by BL2 on behalf of the SPMC.
 
 +---------------+----------------------+------------------+-------------+
 |               | CTX_INCLUDE_EL2_REGS | SPMD_SPM_AT_SEL2 | SPMC_AT_EL3 |
@@ -168,9 +179,8 @@
 
 - Only Arm's FVP platform is supported to use with the TF-A reference software
   stack.
-- The reference software stack uses FEAT_PAuth (formerly Armv8.3-PAuth) and
-  FEAT_BTI (formerly Armv8.5-BTI) architecture extensions by default at EL3
-  and S-EL2.
+- 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.
@@ -178,10 +188,10 @@
   the Hafnium binary path (built for the secure world) or the path to a TEE
   binary implementing FF-A interfaces.
 - BL33 option can specify the TFTF binary or a normal world loader
-  such as U-Boot or the UEFI framework.
+  such as U-Boot or the UEFI framework payload.
 
-Sample TF-A build command line when SPMC is located at S-EL1
-(e.g. when the FEAT_EL2 architecture extension is not implemented):
+Sample TF-A build command line when the SPMC is located at S-EL1
+(e.g. when the FEAT_SEL2 architecture extension is not implemented):
 
 .. code:: shell
 
@@ -194,9 +204,8 @@
     PLAT=fvp \
     all fip
 
-Sample TF-A build command line for a FEAT_SEL2 enabled system where the SPMC is
-located at S-EL2:
-
+Sample TF-A build command line when FEAT_SEL2 architecture extension is
+implemented and the SPMC is located at S-EL2:
 .. code:: shell
 
     make \
@@ -207,13 +216,14 @@
     ARM_ARCH_MINOR=5 \
     BRANCH_PROTECTION=1 \
     CTX_INCLUDE_PAUTH_REGS=1 \
+    CTX_INCLUDE_MTE_REGS=1 \
     BL32=<path-to-hafnium-binary> \
     BL33=<path-to-bl33-binary> \
     SP_LAYOUT_FILE=sp_layout.json \
     all fip
 
-Same as above with enabling secure boot in addition:
-
+Sample TF-A build command line when FEAT_SEL2 architecture extension is
+implemented, the SPMC is located at S-EL2, and enabling secure boot:
 .. code:: shell
 
     make \
@@ -224,6 +234,7 @@
     ARM_ARCH_MINOR=5 \
     BRANCH_PROTECTION=1 \
     CTX_INCLUDE_PAUTH_REGS=1 \
+    CTX_INCLUDE_MTE_REGS=1 \
     BL32=<path-to-hafnium-binary> \
     BL33=<path-to-bl33-binary> \
     SP_LAYOUT_FILE=sp_layout.json \
@@ -235,7 +246,7 @@
     GENERATE_COT=1 \
     all fip
 
-Sample TF-A build command line when SPMC is located at EL3:
+Sample TF-A build command line when the SPMC is located at EL3:
 
 .. code:: shell
 
@@ -270,29 +281,33 @@
 | - cluster0.has_branch_target_exception=1          | Implements FEAT_BTI.               |
 | - cluster1.has_branch_target_exception=1          |                                    |
 +---------------------------------------------------+------------------------------------+
-| - cluster0.restriction_on_speculative_execution=2 | Required by the EL2 context        |
-| - cluster1.restriction_on_speculative_execution=2 | save/restore routine.              |
+| - cluster0.has_pointer_authentication=2           | Implements FEAT_PAuth              |
+| - cluster1.has_pointer_authentication=2           |                                    |
++---------------------------------------------------+------------------------------------+
+| - cluster0.memory_tagging_support_level=2         | Implements FEAT_MTE2               |
+| - cluster1.memory_tagging_support_level=2         |                                    |
+| - bp.dram_metadata.is_enabled=1                   |                                    |
 +---------------------------------------------------+------------------------------------+
 
 Sample FVP command line invocation:
 
 .. code:: shell
 
-    <path-to-fvp-model>/FVP_Base_RevC-2xAEMv8A -C pctl.startup=0.0.0.0
+    <path-to-fvp-model>/FVP_Base_RevC-2xAEMvA -C pctl.startup=0.0.0.0 \
     -C cluster0.NUM_CORES=4 -C cluster1.NUM_CORES=4 -C bp.secure_memory=1 \
     -C bp.secureflashloader.fname=trusted-firmware-a/build/fvp/debug/bl1.bin \
     -C bp.flashloader0.fname=trusted-firmware-a/build/fvp/debug/fip.bin \
     -C bp.pl011_uart0.out_file=fvp-uart0.log -C bp.pl011_uart1.out_file=fvp-uart1.log \
     -C bp.pl011_uart2.out_file=fvp-uart2.log \
-    -C cluster0.has_arm_v8-5=1 -C cluster1.has_arm_v8-5=1 -C pci.pci_smmuv3.mmu.SMMU_AIDR=2 \
-    -C pci.pci_smmuv3.mmu.SMMU_IDR0=0x0046123B -C pci.pci_smmuv3.mmu.SMMU_IDR1=0x00600002 \
-    -C pci.pci_smmuv3.mmu.SMMU_IDR3=0x1714 -C pci.pci_smmuv3.mmu.SMMU_IDR5=0xFFFF0472 \
-    -C pci.pci_smmuv3.mmu.SMMU_S_IDR1=0xA0000002 -C pci.pci_smmuv3.mmu.SMMU_S_IDR2=0 \
-    -C pci.pci_smmuv3.mmu.SMMU_S_IDR3=0 \
-    -C cluster0.has_branch_target_exception=1 \
-    -C cluster1.has_branch_target_exception=1 \
-    -C cluster0.restriction_on_speculative_execution=2 \
-    -C cluster1.restriction_on_speculative_execution=2
+    -C cluster0.has_arm_v8-5=1 -C cluster1.has_arm_v8-5=1 \
+    -C cluster0.has_pointer_authentication=2 -C cluster1.has_pointer_authentication=2 \
+    -C cluster0.has_branch_target_exception=1 -C cluster1.has_branch_target_exception=1 \
+    -C cluster0.memory_tagging_support_level=2 -C cluster1.memory_tagging_support_level=2 \
+    -C bp.dram_metadata.is_enabled=1 \
+    -C pci.pci_smmuv3.mmu.SMMU_AIDR=2 -C pci.pci_smmuv3.mmu.SMMU_IDR0=0x0046123B \
+    -C pci.pci_smmuv3.mmu.SMMU_IDR1=0x00600002 -C pci.pci_smmuv3.mmu.SMMU_IDR3=0x1714 \
+    -C pci.pci_smmuv3.mmu.SMMU_IDR5=0xFFFF0472 -C pci.pci_smmuv3.mmu.SMMU_S_IDR1=0xA0000002 \
+    -C pci.pci_smmuv3.mmu.SMMU_S_IDR2=0 -C pci.pci_smmuv3.mmu.SMMU_S_IDR3=0
 
 Boot process
 ============
@@ -358,6 +373,15 @@
 A json-formatted description file is passed to the build flow specifying paths
 to the SP binary image and associated DTS partition manifest file. The latter
 is processed by the dtc compiler to generate a DTB fed into the SP package.
+Optionally, the partition's json description can contain offsets for both
+the image and partition manifest within the SP package. Both offsets need to be
+4KB aligned, because it is the translation granule supported by Hafnium SPMC.
+These fields can be leveraged to support SPs with S1 translation granules that
+differ from 4KB, and to configure the regions allocated within the SP package,
+as well as to comply with the requirements for the implementation of the boot
+information protocol (see `Passing boot data to the SP`_ for more details). In
+case the offsets are absent in their json node, they default to 0x1000 and
+0x4000 for the manifest offset and image offset respectively.
 This file also specifies the SP owner (as an optional field) identifying the
 signing domain in case of dual root CoT.
 The SP owner can either be the silicon or the platform provider. The
@@ -381,7 +405,19 @@
             "image": "tee2.bin",
             "pm": "tee2.dts",
             "owner": "Plat"
-        }
+        },
+
+        "tee3" : {
+            "image": {
+                "file": "tee3.bin",
+                "offset":"0x2000"
+             },
+            "pm": {
+                "file": "tee3.dts",
+                "offset":"0x6000"
+             },
+            "owner": "Plat"
+        },
     }
 
 SPMC manifest
@@ -403,7 +439,7 @@
     attribute {
         spmc_id = <0x8000>;
         maj_ver = <0x1>;
-        min_ver = <0x0>;
+        min_ver = <0x1>;
         exec_state = <0x0>;
         load_address = <0x0 0x6000000>;
         entrypoint = <0x0 0x6000000>;
@@ -422,13 +458,13 @@
   SPMD (currently matches ``BL32_BASE``) to enter the SPMC.
 
 Other nodes in the manifest are consumed by Hafnium in the secure world.
-A sample can be found at [7]:
+A sample can be found at `[7]`_:
 
 - The *hypervisor* node describes SPs. *is_ffa_partition* boolean attribute
   indicates a FF-A compliant SP. The *load_address* field specifies the load
-  address at which TF-A loaded the SP package.
+  address at which BL2 loaded the SP package.
 - *cpus* node provide the platform topology and allows MPIDR to VMPIDR mapping.
-  Note the primary core is declared first, then secondary core are declared
+  Note the primary core is declared first, then secondary cores are declared
   in reverse order.
 - The *memory* node provides platform information on the ranges of memory
   available to the SPMC.
@@ -460,7 +496,7 @@
 
 Note this boot flow is an implementation sample on Arm's FVP platform.
 Platforms not using TF-A's *Firmware CONFiguration* framework would adjust to a
-different implementation.
+different boot flow. The flow restricts to a maximum of 8 secure partitions.
 
 Secure boot
 ~~~~~~~~~~~
@@ -475,6 +511,8 @@
 - SPMC (BL32) and SPMC manifest are signed by the SiP using the S-ROTPK.
 - BL33 may be signed by the OEM using NS-ROTPK.
 - An SP may be signed either by SiP (using S-ROTPK) or by OEM (using NS-ROTPK).
+- A maximum of 4 partitions can be signed with the S-ROTPK key and 4 partitions
+  signed with the NS-ROTPK key.
 
 Also refer to `Describing secure partitions`_ and `TF-A build options`_ sections.
 
@@ -491,20 +529,23 @@
 the secure world. Such portions are isolated in architecture specific files
 and/or enclosed by a ``SECURE_WORLD`` macro.
 
-Secure partitions CPU scheduling
-~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+Secure partitions scheduling
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
-The FF-A v1.0 specification `[1]`_ provides two ways to relinquinsh CPU time to
+The FF-A specification `[1]`_ provides two ways to relinquinsh CPU time to
 secure partitions. For this a VM (Hypervisor or OS kernel), or SP invokes one of:
 
 - the FFA_MSG_SEND_DIRECT_REQ interface.
 - the FFA_RUN interface.
 
+Additionally a secure interrupt can pre-empt the normal world execution and give
+CPU cycles by transitioning to EL3 and S-EL2.
+
 Platform topology
 ~~~~~~~~~~~~~~~~~
 
 The *execution-ctx-count* SP manifest field can take the value of one or the
-total number of PEs. The FF-A v1.0 specification `[1]`_  recommends the
+total number of PEs. The FF-A specification `[1]`_  recommends the
 following SP types:
 
 - Pinned MP SPs: an execution context matches a physical PE. MP SPs must
@@ -544,13 +585,46 @@
 Passing boot data to the SP
 ---------------------------
 
+In `[1]`_ , the section  "Boot information protocol" defines a method for passing
+data to the SPs at boot time. It specifies the format for the boot information
+descriptor and boot information header structures, which describe the data to be
+exchanged between SPMC and SP.
+The specification also defines the types of data that can be passed.
+The aggregate of both the boot info structures and the data itself is designated
+the boot information blob, and is passed to a Partition as a contiguous memory
+region.
+
+Currently, the SPM implementation supports the FDT type which is used to pass the
+partition's DTB manifest.
+
-In `[1]`_ , the "Protocol for passing data" section defines a method for passing
-boot data to SPs (not currently implemented).
+The region for the boot information blob is allocated through the SP package.
 
-Provided that the whole secure partition package image (see
-`Secure Partition packages`_) is mapped to the SP secure EL1&0 Stage-2
-translation regime, an SP can access its own manifest DTB blob and extract its
-partition manifest properties.
+.. image:: ../resources/diagrams/partition-package.png
+
+To adjust the space allocated for the boot information blob, the json description
+of the SP (see section `Describing secure partitions`_) shall be updated to contain
+the manifest offset. If no offset is provided the manifest offset defaults to 0x1000,
+which is the page size in the Hafnium SPMC.
+
+The configuration of the boot protocol is done in the SPs manifest. As defined by
+the specification, the manifest field 'gp-register-num' configures the GP register
+which shall be used to pass the address to the partitions boot information blob when
+booting the partition.
+In addition, the Hafnium SPMC implementation requires the boot information arguments
+to be listed in a designated DT node:
+
+.. code:: shell
+
+  boot-info {
+      compatible = "arm,ffa-manifest-boot-info";
+      ffa_manifest;
+  };
+
+The whole secure partition package image (see `Secure Partition packages`_) is
+mapped to the SP secure EL1&0 Stage-2 translation regime. As such, the SP can
+retrieve the address for the boot information blob in the designated GP register,
+process the boot information header and descriptors, access its own manifest
+DTB blob and extract its partition manifest properties.
 
 SP Boot order
 -------------
@@ -657,28 +731,29 @@
 receiver.
 
 There are two types of notifications supported:
+
 - Global, which are targeted to a FF-A endpoint and can be handled within any of
-its execution contexts, as determined by the scheduler of the system.
+  its execution contexts, as determined by the scheduler of the system.
 - Per-vCPU, which are targeted to a FF-A endpoint and to be handled within a
-a specific execution context, as determined by the sender.
+  a specific execution context, as determined by the sender.
 
 The type of a notification is set when invoking FFA_NOTIFICATION_BIND to give
 permissions to the sender.
 
 Notification signaling resorts to two interrupts:
-- Schedule Receiver Interrupt: Non-secure physical interrupt to be handled by
-the FF-A 'transport' driver within the receiver scheduler. At initialization
-the SPMC (as suggested by the spec) configures a secure SGI, as non-secure, and
-triggers it when there are pending notifications, and the respective receivers
-need CPU cycles to handle them.
-- Notifications Pending Interrupt: Virtual Interrupt to be handled by the
-receiver of the notification. Set when there are pending notifications. For
-per-vCPU the NPI is pended at the handling of FFA_NOTIFICATION_SET interface.
 
-The notifications receipt support is enabled in the partition FF-A manifest.
+- Schedule Receiver Interrupt: non-secure physical interrupt to be handled by
+  the FF-A driver within the receiver scheduler. At initialization the SPMC
+  donates a SGI ID chosen from the secure SGI IDs range and configures it as
+  non-secure. The SPMC triggers this SGI on the currently running core when
+  there are pending notifications, and the respective receivers need CPU cycles
+  to handle them.
+- Notifications Pending Interrupt: virtual interrupt to be handled by the
+  receiver of the notification. Set when there are pending notifications for the
+  given secure partition. The NPI is pended when the NWd relinquishes CPU cycles
+  to an SP.
 
-The subsequent section provides more details about the each one of the
-FF-A interfaces for notifications support.
+The notifications receipt support is enabled in the partition FF-A manifest.
 
 Mandatory interfaces
 --------------------
@@ -701,9 +776,12 @@
 -  ``FFA_MEM_RETRIEVE_REQ``
 -  ``FFA_MEM_RETRIEVE_RESP``
 -  ``FFA_MEM_RELINQUISH``
+-  ``FFA_MEM_FRAG_RX``
+-  ``FFA_MEM_FRAG_TX``
 -  ``FFA_MEM_RECLAIM``
+-  ``FFA_RUN``
 
-As part of the support of FF-A v1.1, the following interfaces were added:
+As part of the FF-A v1.1 support, the following interfaces were added:
 
  - ``FFA_NOTIFICATION_BITMAP_CREATE``
  - ``FFA_NOTIFICATION_BITMAP_DESTROY``
@@ -714,6 +792,8 @@
  - ``FFA_NOTIFICATION_INFO_GET``
  - ``FFA_SPM_ID_GET``
  - ``FFA_SECONDARY_EP_REGISTER``
+ - ``FFA_MEM_PERM_GET``
+ - ``FFA_MEM_PERM_SET``
 
 FFA_VERSION
 ~~~~~~~~~~~
@@ -841,24 +921,21 @@
 FFA_NOTIFICATION_SET/FFA_NOTIFICATION_GET
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
-If the notifications set are per-vCPU, the NPI interrupt is set as pending
-for a given receiver partition.
+FFA_NOTIFICATION_GET retrieves all pending global notifications and
+per-vCPU notifications targeted to the current vCPU.
 
-The FFA_NOTIFICATION_GET will retrieve all pending global notifications and all
-pending per-vCPU notifications targeted to the current vCPU.
-
-Hafnium keeps the global counting of the pending notifications, which is
-incremented and decremented at the handling of FFA_NOTIFICATION_SET and
-FFA_NOTIFICATION_GET, respectively. If the counter reaches zero, prior to SPMC
-triggering the SRI, it won't be triggered.
+Hafnium maintains a global count of pending notifications which gets incremented
+and decremented when handling FFA_NOTIFICATION_SET and FFA_NOTIFICATION_GET
+respectively. A delayed SRI is triggered if the counter is non-zero when the
+SPMC returns to normal world.
 
 FFA_NOTIFICATION_INFO_GET
 ~~~~~~~~~~~~~~~~~~~~~~~~~
 
-Hafnium keeps the global counting of pending notifications whose info has been
-retrieved by this interface. The counting is incremented and decremented at the
-handling of FFA_NOTIFICATION_INFO_GET and FFA_NOTIFICATION_GET, respectively.
-It also tracks the notifications whose info has been retrieved individually,
+Hafnium maintains a global count of pending notifications whose information
+has been retrieved by this interface. The count is incremented and decremented
+when handling FFA_NOTIFICATION_INFO_GET and FFA_NOTIFICATION_GET respectively.
+It also tracks notifications whose information has been retrieved individually,
 such that it avoids duplicating returned information for subsequent calls to
 FFA_NOTIFICATION_INFO_GET. For each notification, this state information is
 reset when receiver called FFA_NOTIFICATION_GET to retrieve them.
@@ -866,18 +943,18 @@
 FFA_SPM_ID_GET
 ~~~~~~~~~~~~~~
 
-Returns the FF-A ID allocated to the SPM component (which includes SPMC + SPMD).
-At initialization, the SPMC queries the SPMD for the SPM ID, using this
-same interface, and saves it.
+Returns the FF-A ID allocated to an SPM component which can be one of SPMD
+or SPMC.
 
-The call emitted at NS and secure physical FF-A instances returns the SPM ID
-specified in the SPMC manifest.
+At initialization, the SPMC queries the SPMD for the SPMC ID, using the
+FFA_ID_GET interface, and records it. The SPMC can also query the SPMD ID using
+the FFA_SPM_ID_GET interface at the secure physical FF-A instance.
 
-Secure partitions call this interface at the virtual instance, to which the SPMC
-shall return the priorly retrieved SPM ID.
+Secure partitions call this interface at the virtual FF-A instance, to which
+the SPMC returns the priorly retrieved SPMC ID.
 
-The Hypervisor or OS kernel can issue an FFA_SPM_ID_GET call handled by the
-SPMD, which returns the SPM ID.
+The Hypervisor or OS kernel can issue the FFA_SPM_ID_GET call handled by the
+SPMD, which returns the SPMC ID.
 
 FFA_SECONDARY_EP_REGISTER
 ~~~~~~~~~~~~~~~~~~~~~~~~~
@@ -885,7 +962,7 @@
 When the SPMC boots, all secure partitions are initialized on their primary
 Execution Context.
 
-The interface FFA_SECONDARY_EP_REGISTER is to be used by a secure partitions
+The FFA_SECONDARY_EP_REGISTER interface is to be used by a secure partition
 from its first execution context, to provide the entry point address for
 secondary execution contexts.
 
@@ -902,26 +979,35 @@
 - SPMC to SPMD direct request/response uses SMC conduit.
 - SPMD to SPMC direct request/response uses ERET conduit.
 
+This is used in particular to convey power management messages.
+
 PE MMU configuration
 --------------------
 
-With secure virtualization enabled, two IPA spaces are output from the secure
-EL1&0 Stage-1 translation (secure and non-secure). The EL1&0 Stage-2 translation
-hardware is fed by:
+With secure virtualization enabled (``HCR_EL2.VM = 1``) and for S-EL1
+partitions, two IPA spaces (secure and non-secure) are output from the
+secure EL1&0 Stage-1 translation.
+The EL1&0 Stage-2 translation hardware is fed by:
 
-- A single secure IPA space when the SP EL1&0 Stage-1 MMU is disabled.
-- Two IPA spaces (secure and non-secure) when the SP EL1&0 Stage-1 MMU is
-  enabled.
+- A secure IPA when the SP EL1&0 Stage-1 MMU is disabled.
+- One of secure or non-secure IPA when the secure EL1&0 Stage-1 MMU is enabled.
 
 ``VTCR_EL2`` and ``VSTCR_EL2`` provide configuration bits for controlling the
-NS/S IPA translations.
-``VSTCR_EL2.SW`` = 0, ``VSTCR_EL2.SA`` = 0,``VTCR_EL2.NSW`` = 0, ``VTCR_EL2.NSA`` = 1:
+NS/S IPA translations. The following controls are set up:
+``VSTCR_EL2.SW = 0`` , ``VSTCR_EL2.SA = 0``, ``VTCR_EL2.NSW = 0``,
+``VTCR_EL2.NSA = 1``:
 
 - Stage-2 translations for the NS IPA space access the NS PA space.
 - Stage-2 translation table walks for the NS IPA space are to the secure PA space.
 
+Secure and non-secure IPA regions (rooted to by ``VTTBR_EL2`` and ``VSTTBR_EL2``)
+use the same set of Stage-2 page tables within a SP.
+
+The ``VTCR_EL2/VSTCR_EL2/VTTBR_EL2/VSTTBR_EL2`` virtual address space
+configuration is made part of a vCPU context.
+
-Secure and non-secure IPA regions use the same set of Stage-2 page tables within
-a SP.
+For S-EL0 partitions with VHE enabled, a single secure EL2&0 Stage-1 translation
+regime is used for both Hafnium and the partition.
 
 Interrupt management
 --------------------
@@ -1118,16 +1204,46 @@
   (svc_off) hooks are registered.
 - The behavior for the cpu on event is described in `Secondary cores boot-up`_.
   The SPMC is entered through its secondary physical core entry point.
-- The cpu off event occurs when the NWd calls PSCI_CPU_OFF. The method by which
-  the PM event is conveyed to the SPMC is implementation-defined in context of
-  FF-A v1.0 (`SPMC-SPMD direct requests/responses`_). It consists in a SPMD-to-SPMC
-  direct request/response conveying the PM event details and SPMC response.
+- The cpu off event occurs when the NWd calls PSCI_CPU_OFF. The PM event is
+  signaled to the SPMC through a power management framework message.
+  It consists in a SPMD-to-SPMC direct request/response (`SPMC-SPMD direct
+  requests/responses`_) conveying the event details and SPMC response.
   The SPMD performs a synchronous entry into the SPMC. The SPMC is entered and
   updates its internal state to reflect the physical core is being turned off.
   In the current implementation no SP is resumed as a consequence. This behavior
   ensures a minimal support for CPU hotplug e.g. when initiated by the NWd linux
   userspace.
 
+Arm architecture extensions for security hardening
+==================================================
+
+Hafnium supports the following architecture extensions for security hardening:
+
+- Pointer authentication (FEAT_PAuth): the extension permits detection of forged
+  pointers used by ROP type of attacks through the signing of the pointer
+  value. Hafnium is built with the compiler branch protection option to permit
+  generation of a pointer authentication code for return addresses (pointer
+  authentication for instructions). The APIA key is used while Hafnium runs.
+  A random key is generated at boot time and restored upon entry into Hafnium
+  at run-time. APIA and other keys (APIB, APDA, APDB, APGA) are saved/restored
+  in vCPU contexts permitting to enable pointer authentication in VMs/SPs.
+- Branch Target Identification (FEAT_BTI): the extension permits detection of
+  unexpected indirect branches used by JOP type of attacks. Hafnium is built
+  with the compiler branch protection option, inserting land pads at function
+  prologues that are reached by indirect branch instructions (BR/BLR).
+  Hafnium code pages are marked as guarded in the EL2 Stage-1 MMU descriptors
+  such that an indirect branch must always target a landpad. A fault is
+  triggered otherwise. VMs/SPs can (independently) mark their code pages as
+  guarded in the EL1&0 Stage-1 translation regime.
+- Memory Tagging Extension (FEAT_MTE): the option permits detection of out of
+  bound memory array accesses or re-use of an already freed memory region.
+  Hafnium enables the compiler option permitting to leverage MTE stack tagging
+  applied to core stacks. Core stacks are marked as normal tagged memory in the
+  EL2 Stage-1 translation regime. A synchronous data abort is generated upon tag
+  check failure on load/stores. A random seed is generated at boot time and
+  restored upon entry into Hafnium. MTE system registers are saved/restored in
+  vCPU contexts permitting MTE usage from VMs/SPs.
+
 SMMUv3 support in Hafnium
 =========================
 
@@ -1237,7 +1353,7 @@
 -  No support for independent peripheral devices.
 
 S-EL0 Partition support
-=========================
+=======================
 The SPMC (Hafnium) has limited capability to run S-EL0 FF-A partitions using
 FEAT_VHE (mandatory with ARMv8.1 in non-secure state, and in secure world
 with ARMv8.4 and FEAT_SEL2).

diff --git a/docs/design/cpu-specific-build-macros.rst b/docs/design/cpu-specific-build-macros.rst
index 3029458..60313d5 100644
--- a/docs/design/cpu-specific-build-macros.rst
+++ b/docs/design/cpu-specific-build-macros.rst

@@ -270,6 +270,9 @@
 -  ``ERRATA_A77_1791578``: This applies errata 1791578 workaround to Cortex-A77
    CPU. This needs to be enabled for r0p0, r1p0, and r1p1, it is still open.
 
+-  ``ERRATA_A77_2356587``: This applies errata 2356587 workaround to Cortex-A77
+   CPU. This needs to be enabled for r0p0, r1p0, and r1p1, it is still open.
+
 For Cortex-A78, the following errata build flags are defined :
 
 -  ``ERRATA_A78_1688305``: This applies errata 1688305 workaround to Cortex-A78
@@ -296,6 +299,14 @@
    CPU. This needs to be enabled for revisions r1p0, r1p1, and r1p2. The issue
    is present in r0p0 but there is no workaround. It is still open.
 
+-  ``ERRATA_A78_2376745``: This applies errata 2376745 workaround to Cortex-A78
+   CPU. This needs to be enabled for revisions r0p0, r1p0, r1p1, and r1p2, and
+   it is still open.
+
+-  ``ERRATA_A78_2395406``: This applies errata 2395406 workaround to Cortex-A78
+   CPU. This needs to be enabled for revisions r0p0, r1p0, r1p1, and r1p2, and
+   it is still open.
+
 For Cortex-A78 AE, the following errata build flags are defined :
 
 - ``ERRATA_A78_AE_1941500`` : This applies errata 1941500 workaround to
@@ -314,6 +325,17 @@
   Cortex-A78 AE CPU. This needs to be enabled for revisions r0p0 and r0p1. This
   erratum is still open.
 
+For Cortex-X1 CPU, the following errata build flags are defined:
+
+- ``ERRATA_X1_1821534`` : This applies errata 1821534 workaround to Cortex-X1
+   CPU. This needs to be enabled only for revision <= r1p0 of the CPU.
+
+- ``ERRATA_X1_1688305`` : This applies errata 1688305 workaround to Cortex-X1
+   CPU. This needs to be enabled only for revision <= r1p0 of the CPU.
+
+- ``ERRATA_X1_1827429`` : This applies errata 1827429 workaround to Cortex-X1
+   CPU. This needs to be enabled only for revision <= r1p0 of the CPU.
+
 For Neoverse N1, the following errata build flags are defined :
 
 -  ``ERRATA_N1_1073348``: This applies errata 1073348 workaround to Neoverse-N1
@@ -395,6 +417,13 @@
    issue is present in r0p0 as well but there is no workaround for that
    revision.  It is still open.
 
+-  ``ERRATA_V1_2294912``: This applies errata 2294912 workaround to Neoverse-V1
+   CPU. This needs to be enabled for revisions r0p0, r1p0, and r1p1 of the CPU.
+
+-  ``ERRATA_V1_2372203``: This applies errata 2372203 workaround to Neoverse-V1
+   CPU. This needs to be enabled for revisions r0p0, r1p0 and r1p1 of the CPU.
+   It is still open.
+
 For Cortex-A710, the following errata build flags are defined :
 
 -  ``ERRATA_A710_1987031``: This applies errata 1987031 workaround to
@@ -433,6 +462,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_2008768``: This applies errata 2008768 workaround to
+   Cortex-A710 CPU. This needs to be enabled for revisions r0p0, r1p0 and r2p0
+   of the CPU and is fixed in r2p1.
+
 For Neoverse N2, the following errata build flags are defined :
 
 -  ``ERRATA_N2_2002655``: This applies errata 2002655 workaround to Neoverse-N2
@@ -553,6 +586,12 @@
    r2p0 it is fixed). However, please note that this workaround results in
    increased DSU power consumption on idle.
 
+-  ``ERRATA_DSU_2313941``: This applies errata 2313941 workaround for the
+   affected DSU configurations. This errata applies for those DSUs with
+   revisions r0p0, r1p0, r2p0, r2p1, r3p0, r3p1 and is still open. However,
+   please note that this workaround results in increased DSU power consumption
+   on idle.
+
 CPU Specific optimizations
 --------------------------
 
@@ -595,9 +634,17 @@
    This is used to control how the LL_CACHE* PMU events count.
    Default value is 0 (Disabled).
 
+GIC Errata Workarounds
+----------------------
+-  ``GIC600_ERRATA_WA_2384374``: This flag applies part 2 of errata 2384374
+   workaround for the affected GIC600 and GIC600-AE implementations. It applies
+   to implementations of GIC600 and GIC600-AE with revisions less than or equal
+   to r1p6 and r0p2 respectively. If the platform sets GICV3_SUPPORT_GIC600,
+   then this flag is enabled; otherwise, it is 0 (Disabled).
+
 --------------
 
-*Copyright (c) 2014-2021, Arm Limited and Contributors. All rights reserved.*
+*Copyright (c) 2014-2022, Arm Limited and Contributors. All rights reserved.*
 
 .. _CVE-2017-5715: http://cve.mitre.org/cgi-bin/cvename.cgi?name=CVE-2017-5715
 .. _CVE-2018-3639: http://cve.mitre.org/cgi-bin/cvename.cgi?name=CVE-2018-3639

diff --git a/docs/design/firmware-design.rst b/docs/design/firmware-design.rst
index 0831dc0..71fdfcb 100644
--- a/docs/design/firmware-design.rst
+++ b/docs/design/firmware-design.rst

@@ -131,6 +131,9 @@
    -  For other BL3x images, if the firmware configuration file is loaded by
       BL2, then its address is passed in ``arg0`` and if HW_CONFIG is loaded
       then its address is passed in ``arg1``.
+   -  In case of the Arm FVP platform, FW_CONFIG address passed in ``arg1`` to
+      BL31/SP_MIN, and the SOC_FW_CONFIG and HW_CONFIG details are retrieved
+      from FW_CONFIG device tree.
 
 BL1
 ~~~
@@ -1757,12 +1760,20 @@
                    DRAM
     0xffffffff +----------+
                :          :
-               |----------|
+    0x82100000 |----------|
                |HW_CONFIG |
-    0x83000000 |----------|  (non-secure)
+    0x82000000 |----------|  (non-secure)
                |          |
     0x80000000 +----------+
 
+               Trusted DRAM
+    0x08000000 +----------+
+               |HW_CONFIG |
+    0x07f00000 |----------|
+               :          :
+               |          |
+    0x06000000 +----------+
+
                Trusted SRAM
     0x04040000 +----------+  loaded by BL2  +----------------+
                | BL1 (rw) |  <<<<<<<<<<<<<  |                |
@@ -1790,15 +1801,18 @@
                      DRAM
     0xffffffff +--------------+
                :              :
-               |--------------|
+    0x82100000 |--------------|
                |  HW_CONFIG   |
-    0x83000000 |--------------|  (non-secure)
+    0x82000000 |--------------|  (non-secure)
                |              |
     0x80000000 +--------------+
 
-                Trusted DRAM
+                 Trusted DRAM
     0x08000000 +--------------+
-               |     BL32     |
+               |  HW_CONFIG   |
+    0x07f00000 |--------------|
+               :              :
+               |    BL32      |
     0x06000000 +--------------+
 
                  Trusted SRAM
@@ -1829,12 +1843,20 @@
                |  BL32    |  (secure)
     0xff000000 +----------+
                |          |
-               |----------|
+    0x82100000 |----------|
                |HW_CONFIG |
-    0x83000000 |----------|  (non-secure)
+    0x82000000 |----------|  (non-secure)
                |          |
     0x80000000 +----------+
 
+               Trusted DRAM
+    0x08000000 +----------+
+               |HW_CONFIG |
+    0x7f000000 |----------|
+               :          :
+               |          |
+    0x06000000 +----------+
+
                Trusted SRAM
     0x04040000 +----------+  loaded by BL2  +----------------+
                | BL1 (rw) |  <<<<<<<<<<<<<  |                |
@@ -2729,7 +2751,7 @@
 
 --------------
 
-*Copyright (c) 2013-2021, Arm Limited and Contributors. All rights reserved.*
+*Copyright (c) 2013-2022, Arm Limited and Contributors. All rights reserved.*
 
 .. _Power State Coordination Interface PDD: http://infocenter.arm.com/help/topic/com.arm.doc.den0022d/Power_State_Coordination_Interface_PDD_v1_1_DEN0022D.pdf
 .. _SMCCC: https://developer.arm.com/docs/den0028/latest

diff --git a/docs/design/reset-design.rst b/docs/design/reset-design.rst
index 7b10c95..666ee4f 100644
--- a/docs/design/reset-design.rst
+++ b/docs/design/reset-design.rst

@@ -141,19 +141,26 @@
 Platform initialization
 ~~~~~~~~~~~~~~~~~~~~~~~
 
-In this configuration, when the CPU resets to BL31 there are no parameters that
-can be passed in registers by previous boot stages. Instead, the platform code
-in BL31 needs to know, or be able to determine, the location of the BL32 (if
-required) and BL33 images and provide this information in response to the
+In this configuration, when the CPU resets to BL31 there should be no parameters
+that can be passed in registers by previous boot stages. Instead, the platform
+code in BL31 needs to know, or be able to determine, the location of the BL32
+(if required) and BL33 images and provide this information in response to the
 ``bl31_plat_get_next_image_ep_info()`` function.
 
+.. note::
+   Some platforms that configure ``RESET_TO_BL31`` might still be able to
+   receive parameters in registers depending on their actual boot sequence. On
+   those occasions, and in addition to ``RESET_TO_BL31``, these platforms should
+   set ``RESET_TO_BL31_WITH_PARAMS`` to avoid the input registers from being
+   zeroed before entering BL31.
+
 Additionally, platform software is responsible for carrying out any security
 initialisation, for example programming a TrustZone address space controller.
 This might be done by the Trusted Boot Firmware or by platform code in BL31.
 
 --------------
 
-*Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.*
+*Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.*
 
 .. |Default reset code flow| image:: ../resources/diagrams/default_reset_code.png
 .. |Reset code flow with programmable reset address| image:: ../resources/diagrams/reset_code_no_boot_type_check.png

diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index d30e22f..26d5458 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst

@@ -327,6 +327,14 @@
    This flag can take values 0 to 2, to align with the ``FEATURE_DETECTION``
    mechanism. Default is ``0``.
 
+-  ``ENABLE_FEAT_TWED``: Numeric value to enable the ``FEAT_TWED`` (Delayed
+   trapping of WFE Instruction) extension. ``FEAT_TWED`` is a optional feature
+   available on Arm v8.6. This flag can take values 0 to 2, to align with the
+   ``FEATURE_DETECTION`` mechanism. Default is ``0``.
+
+    When ``ENABLE_FEAT_TWED`` is set to ``1``, WFE instruction trapping gets
+    delayed by the amount of value in ``TWED_DELAY``.
+
 -  ``ENABLE_FEAT_VHE``: Numeric value to enable the ``FEAT_VHE`` (Virtualization
    Host Extensions) extension. It allows access to CONTEXTIDR_EL2 register
    during EL2 context save/restore operations.``FEAT_VHE`` is a mandatory
@@ -641,6 +649,15 @@
 
    This option defaults to 0.
 
+-  ``DRTM_SUPPORT``: Boolean flag to enable support for Dynamic Root of Trust
+   for Measurement (DRTM). This feature has trust dependency on BL31 for taking
+   the measurements and recording them as per `PSA DRTM specification`_. For
+   platforms which use BL2 to load/authenticate BL31 ``TRUSTED_BOARD_BOOT`` can
+   be used and for the platforms which use ``RESET_TO_BL31`` platform owners
+   should have mechanism to authenticate BL31.
+
+   This option defaults to 0.
+
 -  ``NON_TRUSTED_WORLD_KEY``: This option is used when ``GENERATE_COT=1``. It
    specifies the file that contains the Non-Trusted World private key in PEM
    format. If ``SAVE_KEYS=1``, this file name will be used to save the key.
@@ -709,6 +726,11 @@
    entrypoint) or 1 (CPU reset to BL31 entrypoint).
    The default value is 0.
 
+-  ``RESET_TO_BL31_WITH_PARAMS``: If ``RESET_TO_BL31`` has been enabled, setting
+   this additional option guarantees that the input registers are not cleared
+   therefore allowing parameters to be passed to the BL31 entrypoint.
+   The default value is 0.
+
 -  ``RESET_TO_SP_MIN``: SP_MIN is the minimal AArch32 Secure Payload provided
    in TF-A. This flag configures SP_MIN entrypoint as the CPU reset vector
    instead of the BL1 entrypoint. It can take the value 0 (CPU reset to BL1
@@ -845,6 +867,12 @@
       When ``EL3_EXCEPTION_HANDLING`` is ``1``, ``TSP_NS_INTR_ASYNC_PREEMPT``
       must also be set to ``1``.
 
+-  ``TWED_DELAY``: Numeric value to be set in order to delay the trapping of
+   WFE instruction. ``ENABLE_FEAT_TWED`` build option must be enabled to set
+   this delay. It can take values in the range (0-15). Default value is ``0``
+   and based on this value, 2^(TWED_DELAY + 8) cycles will be delayed.
+   Platforms need to explicitly update this value based on their requirements.
+
 -  ``USE_ARM_LINK``: This flag determines whether to enable support for ARM
    linker. When the ``LINKER`` build variable points to the armlink linker,
    this flag is enabled automatically. To enable support for armlink, platforms
@@ -954,11 +982,18 @@
   functions that wait for an arbitrary time length (udelay and mdelay). The
   default value is 0.
 
-- ``ENABLE_TRBE_FOR_NS``: This flag is used to enable access of trace buffer
+- ``ENABLE_BRBE_FOR_NS``: Numeric value to enable access to the branch record
+  buffer registers from NS ELs when FEAT_BRBE is implemented. BRBE is an
+  optional architectural feature for AArch64. This flag can take the values
+  0 to 2, to align with the ``FEATURE_DETECTION`` mechanism. The default is 0
+  and it is automatically disabled when the target architecture is AArch32.
+
+- ``ENABLE_TRBE_FOR_NS``: Numeric value to enable access of trace buffer
   control registers from NS ELs, NS-EL2 or NS-EL1(when NS-EL2 is implemented
   but unused) when FEAT_TRBE is implemented. TRBE is an optional architectural
-  feature for AArch64. The default is 0 and it is automatically disabled when
-  the target architecture is AArch32.
+  feature for AArch64. This flag can take the values  0 to 2, to align with the
+  ``FEATURE_DETECTION`` mechanism. The default is 0 and it is automatically
+  disabled when the target architecture is AArch32.
 
 - ``ENABLE_SYS_REG_TRACE_FOR_NS``: Boolean option to enable trace system
   registers access from NS ELs, NS-EL2 or NS-EL1 (when NS-EL2 is implemented
@@ -970,6 +1005,11 @@
   if FEAT_TRF is implemented. This flag can take the values 0 to 2, to align
   with the ``FEATURE_DETECTION`` mechanism. This flag is disabled by default.
 
+- ``PLAT_RSS_NOT_SUPPORTED``: Boolean option to enable the usage of the PSA
+  APIs on platforms that doesn't support RSS (providing Arm CCA HES
+  functionalities). When enabled (``1``), a mocked version of the APIs are used.
+  The default value is 0.
+
 GICv3 driver options
 --------------------
 
@@ -1013,11 +1053,11 @@
 
     make PLAT=<platform> DEBUG=1 V=1 all
 
-AArch64 GCC uses DWARF version 4 debugging symbols by default. Some tools (for
-example DS-5) might not support this and may need an older version of DWARF
-symbols to be emitted by GCC. This can be achieved by using the
-``-gdwarf-<version>`` flag, with the version being set to 2 or 3. Setting the
-version to 2 is recommended for DS-5 versions older than 5.16.
+AArch64 GCC 11 uses DWARF version 5 debugging symbols by default. Some tools
+(for example Arm-DS) might not support this and may need an older version of
+DWARF symbols to be emitted by GCC. This can be achieved by using the
+``-gdwarf-<version>`` flag, with the version being set to 2, 3, 4 or 5. Setting
+the version to 4 is recommended for Arm-DS.
 
 When debugging logic problems it might also be useful to disable all compiler
 optimizations by using ``-O0``.
@@ -1042,7 +1082,7 @@
 post-BL2 phase of TF-A. This can be done by rebuilding BL1 with the
 ``SPIN_ON_BL1_EXIT=1`` build flag. Refer to the :ref:`build_options_common`
 section. In this case, the developer may take control of the target using a
-debugger when indicated by the console output. When using DS-5, the following
+debugger when indicated by the console output. When using Arm-DS, the following
 commands can be used:
 
 ::
@@ -1087,3 +1127,4 @@
 
 .. _DEN0115: https://developer.arm.com/docs/den0115/latest
 .. _PSA FW update specification: https://developer.arm.com/documentation/den0118/a/
+.. _PSA DRTM specification: https://developer.arm.com/documentation/den0113/a

diff --git a/docs/getting_started/initial-build.rst b/docs/getting_started/initial-build.rst
index d4a8f01..62f1941 100644
--- a/docs/getting_started/initial-build.rst
+++ b/docs/getting_started/initial-build.rst

@@ -18,16 +18,12 @@
 
    It is possible to build TF-A using Clang or Arm Compiler 6. To do so
    ``CC`` needs to point to the clang or armclang binary, which will
-   also select the clang or armclang assembler. Be aware that for Arm Compiler,
-   the GNU linker is used by default. However for Clang LLVM linker (LLD)
-   is used by default. In case of being needed the linker can be overridden
-   using the ``LD`` variable. LLVM linker (LLD) version 9 is
-   known to work with TF-A.
-
-   In both cases ``CROSS_COMPILE`` should be set as described above.
-
-   Arm Compiler 6 will be selected when the base name of the path assigned
-   to ``CC`` matches the string 'armclang'.
+   also select the clang or armclang assembler. Arm Compiler 6 will be selected
+   when the base name of the path assigned to ``CC`` matches the string
+   'armclang'. GNU binutils are required since the TF-A build system doesn't
+   currently support Arm Scatter files. Meaning the GNU linker is used by
+   default for Arm Compiler 6. Because of this dependency, ``CROSS_COMPILE``
+   should be set as described above.
 
    For AArch64 using Arm Compiler 6:
 
@@ -36,6 +32,11 @@
        export CROSS_COMPILE=<path-to-aarch64-gcc>/bin/aarch64-none-elf-
        make CC=<path-to-armclang>/bin/armclang PLAT=<platform> all
 
+   On the other hand, Clang uses LLVM linker (LLD) and other LLVM binutils by
+   default instead of GNU utilities (LLVM linker (LLD) 14.0.0 is known to
+   work with TF-A). ``CROSS_COMPILE`` need not be set for Clang. Please note,
+   that the default linker may be manually overridden using the ``LD`` variable.
+
    Clang will be selected when the base name of the path assigned to ``CC``
    contains the string 'clang'. This is to allow both clang and clang-X.Y
    to work.
@@ -44,7 +45,6 @@
 
    .. code:: shell
 
-       export CROSS_COMPILE=<path-to-aarch64-gcc>/bin/aarch64-none-elf-
        make CC=<path-to-clang>/bin/clang PLAT=<platform> all
 
 -  Change to the root directory of the TF-A source tree and build.
@@ -115,4 +115,4 @@
 
 --------------
 
-*Copyright (c) 2020, Arm Limited. All rights reserved.*
+*Copyright (c) 2020-2022, Arm Limited. All rights reserved.*

diff --git a/docs/getting_started/porting-guide.rst b/docs/getting_started/porting-guide.rst
index 3d3b2e3..d49ddeb 100644
--- a/docs/getting_started/porting-guide.rst
+++ b/docs/getting_started/porting-guide.rst

@@ -89,6 +89,8 @@
 The following variables, functions and constants must be defined by the platform
 for the firmware to work correctly.
 
+.. _platform_def_mandatory:
+
 File : platform_def.h [mandatory]
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
@@ -2017,7 +2019,7 @@
 (that was copied during ``bl31_early_platform_setup()``) if the image exists. It
 should return NULL otherwise.
 
-Function : plat_get_cca_attest_token() [mandatory when ENABLE_RME == 1]
+Function : plat_rmmd_get_cca_attest_token() [mandatory when ENABLE_RME == 1]
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
 ::
@@ -2043,8 +2045,8 @@
 
 The function returns 0 on success, -EINVAL on failure.
 
-Function : plat_get_cca_realm_attest_key() [mandatory when ENABLE_RME == 1]
-~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+Function : plat_rmmd_get_cca_realm_attest_key() [mandatory when ENABLE_RME == 1]
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
 ::
 
@@ -2069,6 +2071,31 @@
 
 The function returns 0 on success, -EINVAL on failure.
 
+Function : plat_rmmd_get_el3_rmm_shared_mem() [when ENABLE_RME == 1]
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+::
+
+   Argument : uintptr_t *
+   Return   : size_t
+
+This function returns the size of the shared area between EL3 and RMM (or 0 on
+failure). A pointer to the shared area (or a NULL pointer on failure) is stored
+in the pointer passed as argument.
+
+Function : plat_rmmd_load_manifest() [when ENABLE_RME == 1]
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+::
+
+    Arguments : rmm_manifest_t *manifest
+    Return    : int
+
+When ENABLE_RME is enabled, this function populates a boot manifest for the
+RMM image and stores it in the area specified by manifest.
+
+When ENABLE_RME is disabled, this function is not used.
+
 Function : bl31_plat_enable_mmu [optional]
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
@@ -2118,21 +2145,6 @@
 of the system counter, which is retrieved from the first entry in the frequency
 modes table.
 
-Function : plat_arm_set_twedel_scr_el3() [optional]
-~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-
-::
-
-    Argument : void
-    Return   : uint32_t
-
-This function is used in v8.6+ systems to set the WFE trap delay value in
-SCR_EL3. If this function returns TWED_DISABLED or is left unimplemented, this
-feature is not enabled.  The only hook provided is to set the TWED fields in
-SCR_EL3, there are similar fields in HCR_EL2, SCTLR_EL2, and SCTLR_EL1 to adjust
-the WFE trap delays in lower ELs and these fields should be set by the
-appropriate EL2 or EL1 code depending on the platform configuration.
-
 #define : PLAT_PERCPU_BAKERY_LOCK_SIZE [optional]
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 

diff --git a/docs/getting_started/prerequisites.rst b/docs/getting_started/prerequisites.rst
index a9024e2..0b8a71c 100644
--- a/docs/getting_started/prerequisites.rst
+++ b/docs/getting_started/prerequisites.rst

@@ -7,7 +7,6 @@
 It may possible to build |TF-A| with combinations of software packages that are
 different from those listed below, however only the software described in this
 document can be officially supported.
-
 Build Host
 ----------
 
@@ -26,9 +25,9 @@
 |TF-A| can be built with any of the following *cross-compiler* toolchains that
 target the Armv7-A or Armv8-A architectures:
 
-- GCC >= 10.3-2021.07 (from the `Arm Developer website`_)
-- Clang >= 4.0
-- Arm Compiler >= 6.0
+- GCC >= 11.2-2022.02 (from the `Arm Developer website`_)
+- Clang >= 14.0.0
+- Arm Compiler >= 6.18
 
 In addition, a native compiler is required to build the supporting tools.
 
@@ -54,7 +53,7 @@
 The following libraries must be available to build one or more components or
 supporting tools:
 
-- OpenSSL >= 1.0.1
+- OpenSSL >= 3.0
 
    Required to build the cert_create tool.
 
@@ -71,7 +70,7 @@
    source files (``.dts`` files). DTC is available for Linux through the package
    repositories of most distributions.
 
-- Arm `Development Studio 5 (DS-5)`_
+- Arm `Development Studio (Arm-DS)`_
 
    The standard software package used for debugging software on Arm development
    platforms and |FVP| models.
@@ -160,11 +159,11 @@
 
 --------------
 
-*Copyright (c) 2021, Arm Limited. All rights reserved.*
+*Copyright (c) 2021-2022, Arm Limited. All rights reserved.*
 
 .. _Arm Developer website: https://developer.arm.com/tools-and-software/open-source-software/developer-tools/gnu-toolchain/downloads
 .. _Gerrit Code Review: https://www.gerritcodereview.com/
 .. _Linaro Release Notes: https://community.arm.com/dev-platforms/w/docs/226/old-release-notes
 .. _Linaro instructions: https://community.arm.com/dev-platforms/w/docs/304/arm-reference-platforms-deliverables
-.. _Development Studio 5 (DS-5): https://developer.arm.com/products/software-development-tools/ds-5-development-studio
+.. _Development Studio (Arm-DS): https://developer.arm.com/Tools%20and%20Software/Arm%20Development%20Studio
 .. _Linaro Release 20.01: http://releases.linaro.org/members/arm/platforms/20.01

diff --git a/docs/plat/arm/fvp/index.rst b/docs/plat/arm/fvp/index.rst
index 2aaf195..3d10e45 100644
--- a/docs/plat/arm/fvp/index.rst
+++ b/docs/plat/arm/fvp/index.rst

@@ -12,7 +12,7 @@
 (64-bit host machine only).
 
 .. note::
-   The FVP models used are Version 11.16 Build 16, unless otherwise stated.
+   The FVP models used are Version 11.17 Build 21, unless otherwise stated.
 
 -  ``Foundation_Platform``
 -  ``FVP_Base_AEMv8A-AEMv8A-AEMv8A-AEMv8A-CCN502``
@@ -48,12 +48,12 @@
 -  ``FVP_Base_Neoverse-N2x4`` (Version 11.12 build 38)
 -  ``FVP_Base_Neoverse-V1x4``
 -  ``FVP_Base_RevC-2xAEMvA``  (For certain configurations also uses 0.0/6557)
--  ``FVP_CSS_SGI-575``        (Version 11.15/26)
--  ``FVP_Morello``            (Version 0.11/19)
--  ``FVP_RD_E1_edge``         (Version 11.15/26)
--  ``FVP_RD_N1_edge_dual``    (Version 11.15/26)
--  ``FVP_RD_N1_edge``         (Version 11.15/26)
--  ``FVP_RD_V1``              (Version 11.15/26)
+-  ``FVP_CSS_SGI-575``        (Version 11.17/33)
+-  ``FVP_Morello``            (Version 0.11/33)
+-  ``FVP_RD_E1_edge``         (Version 11.17/33)
+-  ``FVP_RD_N1_edge_dual``    (Version 11.17/33)
+-  ``FVP_RD_N1_edge``         (Version 11.17/33)
+-  ``FVP_RD_V1``              (Version 11.17/33)
 -  ``FVP_TC0``
 -  ``FVP_TC1``
 
@@ -392,7 +392,8 @@
 -  BL1 is loaded at the start of the Trusted ROM.
 -  The Firmware Image Package is loaded at the start of NOR FLASH0.
 -  The firmware loads the FDT packaged in FIP to the DRAM. The FDT load address
-   is specified via the ``hw_config_addr`` property in `TB_FW_CONFIG for FVP`_.
+   is specified via the ``load-address`` property in the ``hw-config`` node of
+   `FW_CONFIG for FVP`_.
 -  The default use-case for the Foundation FVP is to use the ``--gicv3`` option
    and enable the GICv3 device in the model. Note that without this option,
    the Foundation FVP defaults to legacy (Versatile Express) memory map which
@@ -643,9 +644,9 @@
 
 --------------
 
-*Copyright (c) 2019-2021, Arm Limited. All rights reserved.*
+*Copyright (c) 2019-2022, Arm Limited. All rights reserved.*
 
-.. _TB_FW_CONFIG for FVP: https://git.trustedfirmware.org/TF-A/trusted-firmware-a.git/tree/plat/arm/board/fvp/fdts/fvp_tb_fw_config.dts
+.. _FW_CONFIG for FVP: https://git.trustedfirmware.org/TF-A/trusted-firmware-a.git/tree/plat/arm/board/fvp/fdts/fvp_fw_config.dts
 .. _Arm's website: `FVP models`_
 .. _FVP models: https://developer.arm.com/products/system-design/fixed-virtual-platforms
 .. _Linaro Release 20.01: http://releases.linaro.org/members/arm/platforms/20.01

diff --git a/docs/plat/xilinx-versal.rst b/docs/plat/xilinx-versal.rst
index d65b048..09a6ee2 100644
--- a/docs/plat/xilinx-versal.rst
+++ b/docs/plat/xilinx-versal.rst

@@ -43,6 +43,8 @@
 
 *   `VERSAL_PLATFORM`: Select the platform. Options:
     -   `versal_virt`	: Versal Virtual platform
+    -   `spp_itr6`	: SPP ITR6
+    -   `emu_itr6`	: EMU ITR6
 
 # PLM->TF-A Parameter Passing
 ------------------------------

diff --git a/docs/plat/xilinx-zynqmp.rst b/docs/plat/xilinx-zynqmp.rst
index 79c2535..af1cb22 100644
--- a/docs/plat/xilinx-zynqmp.rst
+++ b/docs/plat/xilinx-zynqmp.rst

@@ -14,13 +14,13 @@
 
 .. code:: bash
 
-    make CROSS_COMPILE=aarch64-none-elf- PLAT=zynqmp bl31
+    make CROSS_COMPILE=aarch64-none-elf- PLAT=zynqmp RESET_TO_BL31=1 bl31
 
 To build bl32 TSP you have to rebuild bl31 too:
 
 .. code:: bash
 
-    make CROSS_COMPILE=aarch64-none-elf- PLAT=zynqmp SPD=tspd bl31 bl32
+    make CROSS_COMPILE=aarch64-none-elf- PLAT=zynqmp SPD=tspd RESET_TO_BL31=1 bl31 bl32
 
 To build TF-A for JTAG DCC console:
 

diff --git a/docs/resources/diagrams/Makefile b/docs/resources/diagrams/Makefile
index 7f583b5..4e65569 100644
--- a/docs/resources/diagrams/Makefile
+++ b/docs/resources/diagrams/Makefile

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -61,7 +61,19 @@
 xlat_align_layers					= "bg,translations"
 xlat_align_opts						=
 
-all:$(RESET_PNGS) $(INT_PNGS) $(XLAT_PNG)
+RMM_DIA					= rmm_cold_boot_generic.dia
+RMM_PNG					= rmm_cold_boot_generic.png
+
+rmm_cold_boot_generic_layers		= "background"
+rmm_cold_boot_generic_opts		=
+
+RMM_EL3_MANIFEST_DIA			= rmm_el3_manifest_struct.dia
+RMM_EL3_MANIFEST_PNG			= rmm_el3_manifest_struct.png
+
+rmm_el3_manifest_struct_layers		= "Background"
+rmm_el3_manifest_struct_opts		=
+
+all:$(RESET_PNGS) $(INT_PNGS) $(XLAT_PNG) $(RMM_PNG) $(RMM_EL3_MANIFEST_PNG)
 
 $(RESET_PNGS):$(RESET_DIA)
 	$(call generate_image,$($(patsubst %.png,%_layers,$@)),$@,png,$($(patsubst %.png,%_opts,$@)),$<)
@@ -72,3 +84,9 @@
 $(XLAT_PNG):$(XLAT_DIA)
 	$(call generate_image,$($(patsubst %.png,%_layers,$@)),$(patsubst %.png,%.svg,$@),svg,$($(patsubst %.png,%_opts,$@)),$<)
 	inkscape -z $(patsubst %.png,%.svg,$@) -e $@ -d 45
+
+$(RMM_PNG):$(RMM_DIA)
+	$(call generate_image,$($(patsubst %.png,%_layers,$@)),$@,png,$($(patsubst %.png,%_opts,$@)),$<)
+
+$(RMM_EL3_MANIFEST_PNG):$(RMM_EL3_MANIFEST_DIA)
+	$(call generate_image,$($(patsubst %.png,%_layers,$@)),$@,png,$($(patsubst %.png,%_opts,$@)),$<)

diff --git a/docs/resources/diagrams/partition-package.png b/docs/resources/diagrams/partition-package.png
new file mode 100644
index 0000000..3367422
--- /dev/null
+++ b/docs/resources/diagrams/partition-package.png
Binary files differ

diff --git a/docs/resources/diagrams/rmm_cold_boot_generic.dia b/docs/resources/diagrams/rmm_cold_boot_generic.dia
new file mode 100644
index 0000000..739a1df
--- /dev/null
+++ b/docs/resources/diagrams/rmm_cold_boot_generic.dia
Binary files differ

diff --git a/docs/resources/diagrams/rmm_cold_boot_generic.png b/docs/resources/diagrams/rmm_cold_boot_generic.png
new file mode 100644
index 0000000..df4c1ba
--- /dev/null
+++ b/docs/resources/diagrams/rmm_cold_boot_generic.png
Binary files differ

diff --git a/docs/resources/diagrams/rmm_el3_manifest_struct.dia b/docs/resources/diagrams/rmm_el3_manifest_struct.dia
new file mode 100644
index 0000000..7b7a9c2
--- /dev/null
+++ b/docs/resources/diagrams/rmm_el3_manifest_struct.dia
Binary files differ

diff --git a/docs/resources/diagrams/rmm_el3_manifest_struct.png b/docs/resources/diagrams/rmm_el3_manifest_struct.png
new file mode 100644
index 0000000..8b5776c
--- /dev/null
+++ b/docs/resources/diagrams/rmm_el3_manifest_struct.png
Binary files differ

diff --git a/docs/security_advisories/security-advisory-tfv-9.rst b/docs/security_advisories/security-advisory-tfv-9.rst
index 74b85dc..a7b5984 100644
--- a/docs/security_advisories/security-advisory-tfv-9.rst
+++ b/docs/security_advisories/security-advisory-tfv-9.rst

@@ -57,20 +57,38 @@
 +----------------------+
 | Cortex-A76           |
 +----------------------+
+| Cortex-A76AE         |
++----------------------+
 | Cortex-A77           |
 +----------------------+
 | Cortex-A78           |
 +----------------------+
+| Cortex-A78AE         |
++----------------------+
+| Cortex-A78C          |
++----------------------+
+| Cortex-X1            |
++----------------------+
 | Cortex-X2            |
 +----------------------+
 | Cortex-A710          |
 +----------------------+
+| Cortex-Makalu        |
++----------------------+
+| Cortex-Makalu-ELP    |
++----------------------+
+| Cortex-Hunter        |
++----------------------+
 | Neoverse-N1          |
 +----------------------+
 | Neoverse-N2          |
 +----------------------+
 | Neoverse-V1          |
 +----------------------+
+| Neoverse-Demeter     |
++----------------------+
+| Neoverse-Poseidon    |
++----------------------+
 
 For all other cores impacted by Spectre-BHB, some of which that do not implement
 FEAT_CSV2 and some that do e.g. Cortex-A73, the recommended mitigation is to
@@ -90,7 +108,7 @@
 implementation also enables the normal world to discover the presence of this
 firmware service. This patch also implements ``SMCCC_ARCH_WORKAROUND_3`` for
 Cortex-A57, Coxtex-A72, Cortex-A73 and Cortex-A75 using the existing workaround.
-for CVE-2017-5715.
+for CVE-2017-5715. Cortex-A15 patch extends Spectre V2 mitigation to Spectre-BHB.
 
 The above workaround is enabled by default (on vulnerable CPUs only). Platforms
 can choose to disable them at compile time if they do not require them.

diff --git a/docs/threat_model/threat_model.rst b/docs/threat_model/threat_model.rst
index 072babc..38e5c87 100644
--- a/docs/threat_model/threat_model.rst
+++ b/docs/threat_model/threat_model.rst

@@ -1,9 +1,10 @@
 Generic Threat Model
 ********************
 
-************************
+************
 Introduction
-************************
+************
+
 This document provides a generic threat model for TF-A firmware.
 
 .. note::
@@ -11,9 +12,10 @@
  This threat model doesn't consider Root and Realm worlds introduced by
  :ref:`Realm Management Extension (RME)`.
 
-************************
+********************
 Target of Evaluation
-************************
+********************
+
 In this threat model, the target of evaluation is the Trusted
 Firmware for A-class Processors (TF-A). This includes the boot ROM (BL1),
 the trusted boot firmware (BL2) and the runtime EL3 firmware (BL31) as
@@ -34,8 +36,15 @@
 - There is no Secure-EL2. We don't consider threats that may come with
   Secure-EL2 software.
 
+- Measured boot is disabled. We do not consider the threats nor the mitigations
+  that may come with it.
+
+- No experimental features are enabled. We do not consider threats that may come
+  from them.
+
 Data Flow Diagram
-======================
+=================
+
 Figure 1 shows a high-level data flow diagram for TF-A. The diagram
 shows a model of the different components of a TF-A-based system and
 their interactions with TF-A. A description of each diagram element
@@ -51,26 +60,26 @@
   +-----------------+--------------------------------------------------------+
   | Diagram Element | Description                                            |
   +=================+========================================================+
-  |       ``DF1``   | | At boot time, images are loaded from non-volatile    |
+  |       DF1       | | At boot time, images are loaded from non-volatile    |
   |                 |   memory and verified by TF-A boot firmware. These     |
   |                 |   images include TF-A BL2 and BL31 images, as well as  |
   |                 |   other secure and non-secure images.                  |
   +-----------------+--------------------------------------------------------+
-  |       ``DF2``   | | TF-A log system framework outputs debug messages     |
+  |       DF2       | | TF-A log system framework outputs debug messages     |
   |                 |   over a UART interface.                               |
   +-----------------+--------------------------------------------------------+
-  |       ``DF3``   | | Debug and trace IP on a platform can allow access    |
+  |       DF3       | | Debug and trace IP on a platform can allow access    |
   |                 |   to registers and memory of TF-A.                     |
   +-----------------+--------------------------------------------------------+
-  |       ``DF4``   | | Secure world software (e.g. trusted OS) interact     |
+  |       DF4       | | Secure world software (e.g. trusted OS) interact     |
   |                 |   with TF-A through SMC call interface and/or shared   |
   |                 |   memory.                                              |
   +-----------------+--------------------------------------------------------+
-  |       ``DF5``   | | Non-secure world software (e.g. rich OS) interact    |
+  |       DF5       | | Non-secure world software (e.g. rich OS) interact    |
   |                 |   with TF-A through SMC call interface and/or shared   |
   |                 |   memory.                                              |
   +-----------------+--------------------------------------------------------+
-  |       ``DF6``   | | This path represents the interaction between TF-A and|
+  |       DF6       | | This path represents the interaction between TF-A and|
   |                 |   various hardware IPs such as TrustZone controller    |
   |                 |   and GIC. At boot time TF-A configures/initializes the|
   |                 |   IPs and interacts with them at runtime through       |
@@ -78,9 +87,10 @@
   +-----------------+--------------------------------------------------------+
 
 
-*********************
+***************
 Threat Analysis
-*********************
+***************
+
 In this section we identify and provide assessment of potential threats to TF-A
 firmware. The threats are identified for each diagram element on the
 data flow diagram above.
@@ -91,7 +101,8 @@
 potential mitigations.
 
 Assets
-==================
+======
+
 We have identified the following assets for TF-A:
 
 .. table:: Table 2: TF-A Assets
@@ -99,21 +110,22 @@
   +--------------------+---------------------------------------------------+
   | Asset              | Description                                       |
   +====================+===================================================+
-  | ``Sensitive Data`` | | These include sensitive data that an attacker   |
+  | Sensitive Data     | | These include sensitive data that an attacker   |
   |                    |   must not be able to tamper with (e.g. the Root  |
   |                    |   of Trust Public Key) or see (e.g. secure logs,  |
   |                    |   debugging information such as crash reports).   |
   +--------------------+---------------------------------------------------+
-  | ``Code Execution`` | | This represents the requirement that the        |
+  | Code Execution     | | This represents the requirement that the        |
   |                    |   platform should run only TF-A code approved by  |
   |                    |   the platform provider.                          |
   +--------------------+---------------------------------------------------+
-  | ``Availability``   | | This represents the requirement that TF-A       |
+  | Availability       | | This represents the requirement that TF-A       |
   |                    |   services should always be available for use.    |
   +--------------------+---------------------------------------------------+
 
 Threat Agents
-=====================
+=============
+
 To understand the attack surface, it is important to identify potential
 attackers, i.e. attack entry points. The following threat agents are
 in scope of this threat model.
@@ -123,16 +135,16 @@
   +-------------------+-------------------------------------------------------+
   | Threat Agent      | Description                                           |
   +===================+=======================================================+
-  |   ``NSCode``      | | Malicious or faulty code running in the Non-secure  |
+  |   NSCode          | | Malicious or faulty code running in the Non-secure  |
   |                   |   world, including NS-EL0 NS-EL1 and NS-EL2 levels    |
   +-------------------+-------------------------------------------------------+
-  |   ``SecCode``     | | Malicious or faulty code running in the secure      |
+  |   SecCode         | | Malicious or faulty code running in the secure      |
   |                   |   world, including S-EL0 and S-EL1 levels             |
   +-------------------+-------------------------------------------------------+
-  |   ``AppDebug``    | | Physical attacker using  debug signals to access    |
+  |   AppDebug        | | Physical attacker using  debug signals to access    |
   |                   |   TF-A resources                                      |
   +-------------------+-------------------------------------------------------+
-  | ``PhysicalAccess``| | Physical attacker having access to external device  |
+  |  PhysicalAccess   | | Physical attacker having access to external device  |
   |                   |   communication bus and to external flash             |
   |                   |   communication bus using common hardware             |
   +-------------------+-------------------------------------------------------+
@@ -145,7 +157,8 @@
   considered out-of-scope.
 
 Threat Types
-========================
+============
+
 In this threat model we categorize threats using the `STRIDE threat
 analysis technique`_. In this technique a threat is categorized as one
 or more of these types: ``Spoofing``, ``Tampering``, ``Repudiation``,
@@ -153,7 +166,8 @@
 ``Elevation of privilege``.
 
 Threat Risk Ratings
-========================
+===================
+
 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
@@ -165,7 +179,7 @@
   +-----------------------+-------------------------+---------------------------+
   | **Rating (Score)**    | **Impact**              | **Likelihood**            |
   +=======================+=========================+===========================+
-  | ``Critical (5)``      | | Extreme impact to     | | Threat is almost        |
+  | Critical (5)          | | Extreme impact to     | | Threat is almost        |
   |                       |   entire organization   |   certain to be exploited.|
   |                       |   if exploited.         |                           |
   |                       |                         | | Knowledge of the threat |
@@ -173,17 +187,17 @@
   |                       |                         |   are in the public       |
   |                       |                         |   domain.                 |
   +-----------------------+-------------------------+---------------------------+
-  | ``High (4)``          | | Major impact to entire| | Threat is relatively    |
+  | High (4)              | | Major impact to entire| | Threat is relatively    |
   |                       |   organization or single|   easy to detect and      |
   |                       |   line of business if   |   exploit by an attacker  |
   |                       |   exploited             |   with little skill.      |
   +-----------------------+-------------------------+---------------------------+
-  | ``Medium (3)``        | | Noticeable impact to  | | A knowledgeable insider |
+  | Medium (3)            | | Noticeable impact to  | | A knowledgeable insider |
   |                       |   line of business if   |   or expert attacker could|
   |                       |   exploited.            |   exploit the threat      |
   |                       |                         |   without much difficulty.|
   +-----------------------+-------------------------+---------------------------+
-  | ``Low (2)``           | | Minor damage if       | | Exploiting the threat   |
+  | Low (2)               | | Minor damage if       | | Exploiting the threat   |
   |                       |   exploited or could    |   would require           |
   |                       |   be used in conjunction|   considerable expertise  |
   |                       |   with other            |   and resources           |
@@ -191,7 +205,7 @@
   |                       |   perform a more serious|                           |
   |                       |   attack                |                           |
   +-----------------------+-------------------------+---------------------------+
-  | ``Informational (1)`` | | Poor programming      | | Threat is not likely    |
+  | Informational (1)     | | Poor programming      | | Threat is not likely    |
   |                       |   practice or poor      |   to be exploited on its  |
   |                       |   design decision that  |   own, but may be used to |
   |                       |   may not represent an  |   gain information for    |
@@ -235,14 +249,27 @@
 ``Internet of Things(IoT)``, ``Mobile`` and ``Server``.
 
 Threat Assessment
-============================
+=================
+
 The following threats were identified by applying STRIDE analysis on
 each diagram element of the data flow diagram.
 
+For each threat, we strive to indicate whether the mitigations are currently
+implemented or not. However, the answer to this question is not always straight
+forward. Some mitigations are partially implemented in the generic code but also
+rely on the platform code to implement some bits of it. This threat model aims
+to be platform-independent and it is important to keep in mind that such threats
+only get mitigated if the platform code properly fulfills its responsibilities.
+
+Also, some mitigations require enabling specific features, which must be
+explicitly turned on via a build flag.
+
+These are highlighted in the ``Mitigations implemented?`` box.
+
 +------------------------+----------------------------------------------------+
 | ID                     | 01                                                 |
 +========================+====================================================+
-| ``Threat``             | | **An attacker can mangle firmware images to      |
+| Threat                 | | **An attacker can mangle firmware images to      |
 |                        |   execute arbitrary code**                         |
 |                        |                                                    |
 |                        | | Some TF-A images are loaded from external        |
@@ -252,79 +279,89 @@
 |                        |   updating mechanism to modify the non-volatile    |
 |                        |   images to execute arbitrary code.                |
 +------------------------+----------------------------------------------------+
-| ``Diagram Elements``   | DF1, DF4, DF5                                      |
+| Diagram Elements       | DF1, DF4, DF5                                      |
 +------------------------+----------------------------------------------------+
-| ``Affected TF-A        | BL2, BL31                                          |
-| Components``           |                                                    |
+| Affected TF-A          | BL2, BL31                                          |
+| Components             |                                                    |
 +------------------------+----------------------------------------------------+
-| ``Assets``             | Code Execution                                     |
+| Assets                 | Code Execution                                     |
 +------------------------+----------------------------------------------------+
-| ``Threat Agent``       | PhysicalAccess, NSCode, SecCode                    |
+| Threat Agent           | PhysicalAccess, NSCode, SecCode                    |
 +------------------------+----------------------------------------------------+
-| ``Threat Type``        | Tampering, Elevation of Privilege                  |
+| Threat Type            | Tampering, Elevation of Privilege                  |
 +------------------------+------------------+-----------------+---------------+
-| ``Application``        | ``Server``       | ``IoT``         | ``Mobile``    |
+| Application            | Server           | IoT             | Mobile        |
 +------------------------+------------------+-----------------+---------------+
-| ``Impact``             | Critical (5)     | Critical (5)    | Critical (5)  |
+| Impact                 | Critical (5)     | Critical (5)    | Critical (5)  |
 +------------------------+------------------+-----------------+---------------+
-| ``Likelihood``         | Critical (5)     | Critical (5)    | Critical (5)  |
+| Likelihood             | Critical (5)     | Critical (5)    | Critical (5)  |
 +------------------------+------------------+-----------------+---------------+
-| ``Total Risk Rating``  | Critical (25)    | Critical (25)   | Critical (25) |
+| Total Risk Rating      | Critical (25)    | Critical (25)   | Critical (25) |
 +------------------------+------------------+-----------------+---------------+
-| ``Mitigations``        | | TF-A implements the `Trusted Board Boot (TBB)`_  |
+| Mitigations            | | 1) Implement the `Trusted Board Boot (TBB)`_     |
 |                        |   feature which prevents malicious firmware from   |
 |                        |   running on the platform by authenticating all    |
-|                        |   firmware images. In addition to this, the TF-A   |
-|                        |   boot firmware performs extra checks on           |
-|                        |   unauthenticated data, such as FIP metadata, prior|
-|                        |   to use.                                          |
+|                        |   firmware images.                                 |
+|                        |                                                    |
+|                        | | 2) Perform extra checks on unauthenticated data, |
+|                        |   such as FIP metadata, prior to use.              |
++------------------------+----------------------------------------------------+
+| Mitigations            | | 1) Yes, provided that the ``TRUSTED_BOARD_BOOT`` |
+| implemented?           |   build option is set to 1.                        |
+|                        |                                                    |
+|                        | | 2) Yes.                                          |
 +------------------------+----------------------------------------------------+
 
 +------------------------+----------------------------------------------------+
 | ID                     | 02                                                 |
 +========================+====================================================+
-| ``Threat``             | | **An attacker may attempt to boot outdated,      |
+| Threat                 | | **An attacker may attempt to boot outdated,      |
 |                        |   potentially vulnerable firmware image**          |
 |                        |                                                    |
 |                        | | When updating firmware, an attacker may attempt  |
 |                        |   to rollback to an older version that has unfixed |
 |                        |   vulnerabilities.                                 |
 +------------------------+----------------------------------------------------+
-| ``Diagram Elements``   | DF1, DF4, DF5                                      |
+| Diagram Elements       | DF1, DF4, DF5                                      |
 +------------------------+----------------------------------------------------+
-| ``Affected TF-A        | BL2, BL31                                          |
-| Components``           |                                                    |
+| Affected TF-A          | BL2, BL31                                          |
+| Components             |                                                    |
 +------------------------+----------------------------------------------------+
-| ``Assets``             | Code Execution                                     |
+| Assets                 | Code Execution                                     |
 +------------------------+----------------------------------------------------+
-| ``Threat Agent``       | PhysicalAccess, NSCode, SecCode                    |
+| Threat Agent           | PhysicalAccess, NSCode, SecCode                    |
 +------------------------+----------------------------------------------------+
-| ``Threat Type``        | Tampering                                          |
+| Threat Type            | Tampering                                          |
 +------------------------+------------------+-----------------+---------------+
-| ``Application``        | ``Server``       | ``IoT``         | ``Mobile``    |
+| Application            | Server           | IoT             | Mobile        |
 +------------------------+------------------+-----------------+---------------+
-| ``Impact``             | Critical (5)     | Critical (5)    | Critical (5)  |
+| Impact                 | Critical (5)     | Critical (5)    | Critical (5)  |
 +------------------------+------------------+-----------------+---------------+
-| ``Likelihood``         | Critical (5)     | Critical (5)    | Critical (5)  |
+| Likelihood             | Critical (5)     | Critical (5)    | Critical (5)  |
 +------------------------+------------------+-----------------+---------------+
-| ``Total Risk Rating``  | Critical (25)    | Critical (25)   | Critical (25) |
+| Total Risk Rating      | Critical (25)    | Critical (25)   | Critical (25) |
 +------------------------+------------------+-----------------+---------------+
-| ``Mitigations``        | | TF-A supports anti-rollback protection using     |
-|                        |   non-volatile counters (NV counters) as required  |
-|                        |   by `TBBR-Client specification`_. After a firmware|
-|                        |   image is validated, the image revision number    |
-|                        |   taken from a certificate extension field is      |
-|                        |   compared with the corresponding NV counter stored|
-|                        |   in hardware to make sure the new counter value is|
-|                        |   larger or equal to the current counter value.    |
-|                        |   Platforms must implement this protection using   |
-|                        |   platform specific hardware NV counters.          |
+| Mitigations            | Implement anti-rollback protection using           |
+|                        | non-volatile counters (NV counters) as required    |
+|                        | by `TBBR-Client specification`_.                   |
++------------------------+----------------------------------------------------+
+| Mitigations            | | Yes / Platform specific.                         |
+| implemented?           |                                                    |
+|                        | | After a firmware image is validated, the image   |
+|                        |   revision number taken from a certificate         |
+|                        |   extension field is compared with the             |
+|                        |   corresponding NV counter stored in hardware to   |
+|                        |   make sure the new counter value is larger than   |
+|                        |   the current counter value.                       |
+|                        |                                                    |
+|                        | | **Platforms must implement this protection using |
+|                        |   platform specific hardware NV counters.**        |
 +------------------------+----------------------------------------------------+
 
 +------------------------+-------------------------------------------------------+
 | ID                     | 03                                                    |
 +========================+=======================================================+
-| ``Threat``             | |  **An attacker can use Time-of-Check-Time-of-Use    |
+| Threat                 | | **An attacker can use Time-of-Check-Time-of-Use     |
 |                        |   (TOCTOU) attack to bypass image authentication      |
 |                        |   during the boot process**                           |
 |                        |                                                       |
@@ -336,33 +373,39 @@
 |                        |   after the integrity and authentication check has    |
 |                        |   been performed.                                     |
 +------------------------+-------------------------------------------------------+
-| ``Diagram Elements``   | DF1                                                   |
+| Diagram Elements       | DF1                                                   |
 +------------------------+-------------------------------------------------------+
-| ``Affected TF-A        | BL1, BL2                                              |
-| Components``           |                                                       |
+| Affected TF-A          | BL1, BL2                                              |
+| Components             |                                                       |
 +------------------------+-------------------------------------------------------+
-| ``Assets``             | Code Execution, Sensitive Data                        |
+| Assets                 | Code Execution, Sensitive Data                        |
 +------------------------+-------------------------------------------------------+
-| ``Threat Agent``       | PhysicalAccess                                        |
+| Threat Agent           | PhysicalAccess                                        |
 +------------------------+-------------------------------------------------------+
-| ``Threat Type``        | Elevation of Privilege                                |
+| Threat Type            | Elevation of Privilege                                |
 +------------------------+---------------------+-----------------+---------------+
-| ``Application``        | ``Server``          | ``IoT``         | ``Mobile``    |
+| Application            | Server              | IoT             | Mobile        |
 +------------------------+---------------------+-----------------+---------------+
-| ``Impact``             | N/A                 | Critical (5)    | Critical (5)  |
+| Impact                 | N/A                 | Critical (5)    | Critical (5)  |
 +------------------------+---------------------+-----------------+---------------+
-| ``Likelihood``         | N/A                 | Medium (3)      | Medium (3)    |
+| Likelihood             | N/A                 | Medium (3)      | Medium (3)    |
 +------------------------+---------------------+-----------------+---------------+
-| ``Total Risk Rating``  | N/A                 | High (15)       | High (15)     |
+| Total Risk Rating      | N/A                 | High (15)       | High (15)     |
 +------------------------+---------------------+-----------------+---------------+
-| ``Mitigations``        | | TF-A boot firmware copies image to on-chip          |
-|                        |   memory before authenticating an image.              |
+| Mitigations            | Copy image to on-chip memory before authenticating    |
+|                        | it.                                                   |
++------------------------+-------------------------------------------------------+
+| Mitigations            | | Platform specific.                                  |
+| implemented?           |                                                       |
+|                        | | The list of images to load and their location is    |
+|                        |   platform specific. Platforms are responsible for    |
+|                        |   arranging images to be loaded in on-chip memory.    |
 +------------------------+-------------------------------------------------------+
 
 +------------------------+-------------------------------------------------------+
 | ID                     | 04                                                    |
 +========================+=======================================================+
-| ``Threat``             | | **An attacker with physical access can execute      |
+| Threat                 | | **An attacker with physical access can execute      |
 |                        |   arbitrary image by bypassing the signature          |
 |                        |   verification stage using glitching techniques**     |
 |                        |                                                       |
@@ -381,31 +424,38 @@
 |                        |   points where the image is validated against the     |
 |                        |   signature.                                          |
 +------------------------+-------------------------------------------------------+
-| ``Diagram Elements``   | DF1                                                   |
+| Diagram Elements       | DF1                                                   |
 +------------------------+-------------------------------------------------------+
-| ``Affected TF-A        | BL1, BL2                                              |
-| Components``           |                                                       |
+| Affected TF-A          | BL1, BL2                                              |
+| Components             |                                                       |
 +------------------------+-------------------------------------------------------+
-| ``Assets``             | Code Execution                                        |
+| Assets                 | Code Execution                                        |
 +------------------------+-------------------------------------------------------+
-| ``Threat Agent``       | PhysicalAccess                                        |
+| Threat Agent           | PhysicalAccess                                        |
 +------------------------+-------------------------------------------------------+
-| ``Threat Type``        | Tampering, Elevation of Privilege                     |
+| Threat Type            | Tampering, Elevation of Privilege                     |
 +------------------------+---------------------+-----------------+---------------+
-| ``Application``        | ``Server``          | ``IoT``         | ``Mobile``    |
+| Application            | Server              | IoT             | Mobile        |
 +------------------------+---------------------+-----------------+---------------+
-| ``Impact``             | N/A                 | Critical (5)    | Critical (5)  |
+| Impact                 | N/A                 | Critical (5)    | Critical (5)  |
 +------------------------+---------------------+-----------------+---------------+
-| ``Likelihood``         | N/A                 | Medium (3)      | Medium (3)    |
+| Likelihood             | N/A                 | Medium (3)      | Medium (3)    |
 +------------------------+---------------------+-----------------+---------------+
-| ``Total Risk Rating``  | N/A                 | High (15)       | High (15)     |
+| Total Risk Rating      | N/A                 | High (15)       | High (15)     |
 +------------------------+---------------------+-----------------+---------------+
-| ``Mitigations``        | | The most effective mitigation is adding glitching   |
+| Mitigations            | Mechanisms to detect clock glitch and power           |
+|                        | variations.                                           |
++------------------------+-------------------------------------------------------+
+| Mitigations            | | No.                                                 |
+| implemented?           |                                                       |
+|                        | | The most effective mitigation is adding glitching   |
 |                        |   detection and mitigation circuit at the hardware    |
-|                        |   level. However, software techniques,                |
-|                        |   such as adding redundant checks when performing     |
-|                        |   conditional branches that are security sensitive,   |
-|                        |   can be used to harden TF-A against such attacks.    |
+|                        |   level.                                              |
+|                        |                                                       |
+|                        | | However, software techniques, such as adding        |
+|                        |   redundant checks when performing conditional        |
+|                        |   branches that are security sensitive, can be used   |
+|                        |   to harden TF-A against such attacks.                |
 |                        |   **At the moment TF-A doesn't implement such         |
 |                        |   mitigations.**                                      |
 +------------------------+-------------------------------------------------------+
@@ -413,49 +463,76 @@
 +------------------------+---------------------------------------------------+
 | ID                     | 05                                                |
 +========================+===================================================+
-| ``Threat``             | | **Information leak via UART logs such as        |
-|                        |   crashes**                                       |
+| Threat                 | | **Information leak via UART logs**              |
 |                        |                                                   |
 |                        | | During the development stages of software it is |
-|                        |   common to include crash reports with detailed   |
-|                        |   information of the CPU state including current  |
-|                        |   values of the registers, privilege level and    |
-|                        |   stack dumps. This information is useful when    |
-|                        |   debugging problems before releasing the         |
-|                        |   production version, but it could be used by an  |
-|                        |   attacker to develop a working exploit if left   |
-|                        |   in the production version.                      |
+|                        |   common to print all sorts of information on the |
+|                        |   console, including sensitive or confidential    |
+|                        |   information such as crash reports with detailed |
+|                        |   information of the CPU state, current registers |
+|                        |   values, privilege level or stack dumps.         |
+|                        |                                                   |
+|                        | | This information is useful when debugging       |
+|                        |   problems before releasing the production        |
+|                        |   version but it could be used by an attacker     |
+|                        |   to develop a working exploit if left enabled in |
+|                        |   the production version.                         |
+|                        |                                                   |
+|                        | | This happens when directly logging sensitive    |
+|                        |   information and more subtly when logging        |
+|                        |   side-channel information that can be used by an |
+|                        |   attacker to learn about sensitive information.  |
 +------------------------+---------------------------------------------------+
-| ``Diagram Elements``   | DF2                                               |
+| Diagram Elements       | DF2                                               |
 +------------------------+---------------------------------------------------+
-| ``Affected TF-A        | BL1, BL2, BL31                                    |
-| Components``           |                                                   |
+| Affected TF-A          | BL1, BL2, BL31                                    |
+| Components             |                                                   |
 +------------------------+---------------------------------------------------+
-| ``Assets``             | Sensitive Data                                    |
+| Assets                 | Sensitive Data                                    |
 +------------------------+---------------------------------------------------+
-| ``Threat Agent``       | AppDebug                                          |
+| Threat Agent           | AppDebug                                          |
 +------------------------+---------------------------------------------------+
-| ``Threat Type``        | Information Disclosure                            |
+| Threat Type            | Information Disclosure                            |
 +------------------------+------------------+----------------+---------------+
-| ``Application``        | ``Server``       | ``IoT``        | ``Mobile``    |
+| Application            | Server           | IoT            | Mobile        |
 +------------------------+------------------+----------------+---------------+
-| ``Impact``             | N/A              | Low (2)        | Low (2)       |
+| Impact                 | N/A              | Low (2)        | Low (2)       |
 +------------------------+------------------+----------------+---------------+
-| ``Likelihood``         | N/A              | High (4)       | High (4)      |
+| Likelihood             | N/A              | High (4)       | High (4)      |
 +------------------------+------------------+----------------+---------------+
-| ``Total Risk Rating``  | N/A              | Medium (8)     | Medium (8)    |
+| Total Risk Rating      | N/A              | Medium (8)     | Medium (8)    |
 +------------------------+------------------+----------------+---------------+
-| ``Mitigations``        | | In TF-A, crash reporting is only enabled for    |
-|                        |   debug builds by default. Alternatively, the log |
-|                        |   level can be tuned at build time (from verbose  |
-|                        |   to no output at all), independently of the      |
-|                        |   build type.                                     |
+| Mitigations            | | Remove sensitive information logging in         |
+|                        |   production releases.                            |
+|                        |                                                   |
+|                        | | Do not conditionally log information depending  |
+|                        |   on potentially sensitive data.                  |
+|                        |                                                   |
+|                        | | Do not log high precision timing information.   |
++------------------------+---------------------------------------------------+
+| Mitigations            | | Yes / Platform Specific.                        |
+| implemented?           |   Requires the right build options to be used.    |
+|                        |                                                   |
+|                        | | Crash reporting is only enabled for debug       |
+|                        |   builds by default, see ``CRASH_REPORTING``      |
+|                        |   build option.                                   |
+|                        |                                                   |
+|                        | | The log level can be tuned at build time, from  |
+|                        |   very verbose to no output at all. See           |
+|                        |   ``LOG_LEVEL`` build option. By default, release |
+|                        |   builds are a lot less verbose than debug ones   |
+|                        |   but still produce some output.                  |
+|                        |                                                   |
+|                        | | Messages produced by the platform code should   |
+|                        |   use the appropriate level of verbosity so as    |
+|                        |   not to leak sensitive information in production |
+|                        |   builds.                                         |
 +------------------------+---------------------------------------------------+
 
 +------------------------+----------------------------------------------------+
 | ID                     | 06                                                 |
 +========================+====================================================+
-| ``Threat``             | | **An attacker can read sensitive data and        |
+| Threat                 | | **An attacker can read sensitive data and        |
 |                        |   execute arbitrary code through the external      |
 |                        |   debug and trace interface**                      |
 |                        |                                                    |
@@ -468,37 +545,40 @@
 |                        |   attacker to read sensitive data and execute      |
 |                        |   arbitrary code.                                  |
 +------------------------+----------------------------------------------------+
-| ``Diagram Elements``   | DF3                                                |
+| Diagram Elements       | DF3                                                |
 +------------------------+----------------------------------------------------+
-| ``Affected TF-A        | BL1, BL2, BL31                                     |
-| Components``           |                                                    |
+| Affected TF-A          | BL1, BL2, BL31                                     |
+| Components             |                                                    |
 +------------------------+----------------------------------------------------+
-| ``Assets``             | Code Execution, Sensitive Data                     |
+| Assets                 | Code Execution, Sensitive Data                     |
 +------------------------+----------------------------------------------------+
-| ``Threat Agent``       | AppDebug                                           |
+| Threat Agent           | AppDebug                                           |
 +------------------------+----------------------------------------------------+
-| ``Threat Type``        | Tampering, Information Disclosure,                 |
+| Threat Type            | Tampering, Information Disclosure,                 |
 |                        | Elevation of privilege                             |
 +------------------------+------------------+---------------+-----------------+
-| ``Application``        | ``Server``       | ``IoT``       | ``Mobile``      |
+| Application            | Server           | IoT           | Mobile          |
 +------------------------+------------------+---------------+-----------------+
-| ``Impact``             | N/A              | High (4)      | High (4)        |
+| Impact                 | N/A              | High (4)      | High (4)        |
 +------------------------+------------------+---------------+-----------------+
-| ``Likelihood``         | N/A              | Critical (5)  | Critical (5)    |
+| Likelihood             | N/A              | Critical (5)  | Critical (5)    |
 +------------------------+------------------+---------------+-----------------+
-| ``Total Risk Rating``  | N/A              | Critical (20) | Critical (20)   |
+| Total Risk Rating      | N/A              | Critical (20) | Critical (20)   |
 +------------------------+------------------+---------------+-----------------+
-| ``Mitigations``        | | Configuration of debug and trace capabilities is |
-|                        |   platform specific. Therefore, platforms must     |
-|                        |   disable the debug and trace capability for       |
-|                        |   production releases or enable proper debug       |
-|                        |   authentication as recommended by [`DEN0034`_].   |
+| Mitigations            | Disable the debug and trace capability for         |
+|                        | production releases or enable proper debug         |
+|                        | authentication as recommended by [`DEN0034`_].     |
++------------------------+----------------------------------------------------+
+| Mitigations            | | Platform specific.                               |
+| implemented?           |                                                    |
+|                        | | Configuration of debug and trace capabilities is |
+|                        |   entirely platform specific.                      |
 +------------------------+----------------------------------------------------+
 
 +------------------------+------------------------------------------------------+
 | ID                     | 07                                                   |
 +========================+======================================================+
-| ``Threat``             | | **An attacker can perform a denial-of-service      |
+| Threat                 | | **An attacker can perform a denial-of-service      |
 |                        |   attack by using a broken SMC call that causes the  |
 |                        |   system to reboot or enter into unknown state.**    |
 |                        |                                                      |
@@ -508,48 +588,48 @@
 |                        |   by calling unimplemented SMC call or by passing    |
 |                        |   invalid arguments.                                 |
 +------------------------+------------------------------------------------------+
-| ``Diagram Elements``   | DF4, DF5                                             |
+| Diagram Elements       | DF4, DF5                                             |
 +------------------------+------------------------------------------------------+
-| ``Affected TF-A        | BL31                                                 |
-| Components``           |                                                      |
+| Affected TF-A          | BL31                                                 |
+| Components             |                                                      |
 +------------------------+------------------------------------------------------+
-| ``Assets``             | Availability                                         |
+| Assets                 | Availability                                         |
 +------------------------+------------------------------------------------------+
-| ``Threat Agent``       | NSCode, SecCode                                      |
+| Threat Agent           | NSCode, SecCode                                      |
 +------------------------+------------------------------------------------------+
-| ``Threat Type``        | Denial of Service                                    |
+| Threat Type            | Denial of Service                                    |
 +------------------------+-------------------+----------------+-----------------+
-| ``Application``        | ``Server``        | ``IoT``        | ``Mobile``      |
+| Application            | Server            | IoT            | Mobile          |
 +------------------------+-------------------+----------------+-----------------+
-| ``Impact``             | Medium (3)        | Medium (3)     | Medium (3)      |
+| Impact                 | Medium (3)        | Medium (3)     | Medium (3)      |
 +------------------------+-------------------+----------------+-----------------+
-| ``Likelihood``         | High (4)          | High (4)       | High (4)        |
+| Likelihood             | High (4)          | High (4)       | High (4)        |
 +------------------------+-------------------+----------------+-----------------+
-| ``Total Risk Rating``  | High (12)         | High (12)      | High (12)       |
+| Total Risk Rating      | High (12)         | High (12)      | High (12)       |
 +------------------------+-------------------+----------------+-----------------+
-| ``Mitigations``        | | The generic TF-A code validates SMC function ids   |
-|                        |   and arguments before using them.                   |
-|                        |   Platforms that implement SiP services must also    |
+| Mitigations            | Validate SMC function ids and arguments before using |
+|                        | them.                                                |
++------------------------+------------------------------------------------------+
+| Mitigations            | | Yes / Platform specific.                           |
+| implemented?           |                                                      |
+|                        | | For standard services, all input is validated.     |
+|                        |                                                      |
+|                        | | Platforms that implement SiP services must also    |
 |                        |   validate SMC call arguments.                       |
 +------------------------+------------------------------------------------------+
 
 +------------------------+------------------------------------------------------+
 | ID                     | 08                                                   |
 +========================+======================================================+
-| ``Threat``             | | **Memory corruption due to memory overflows and    |
+| Threat                 | | **Memory corruption due to memory overflows and    |
 |                        |   lack of boundary checking when accessing resources |
 |                        |   could allow an attacker to execute arbitrary code, |
 |                        |   modify some state variable to change the normal    |
 |                        |   flow of the program, or leak sensitive             |
 |                        |   information**                                      |
 |                        |                                                      |
-|                        | | Like in other software, the Trusted Firmware has   |
-|                        |   multiple points where memory corruption security   |
-|                        |   errors can arise. Memory corruption is a dangerous |
-|                        |   security issue since it could allow an attacker    |
-|                        |   to execute arbitrary code, modify some state       |
-|                        |   variable to change the normal flow of the program, |
-|                        |   or leak sensitive information.                     |
+|                        | | Like in other software, TF-A has multiple points   |
+|                        |   where memory corruption security errors can arise. |
 |                        |                                                      |
 |                        | | Some of the errors include integer overflow,       |
 |                        |   buffer overflow, incorrect array boundary checks,  |
@@ -558,37 +638,32 @@
 |                        |   validations might also result in these kinds of    |
 |                        |   errors in release builds.                          |
 +------------------------+------------------------------------------------------+
-| ``Diagram Elements``   | DF4, DF5                                             |
+| Diagram Elements       | DF4, DF5                                             |
 +------------------------+------------------------------------------------------+
-| ``Affected TF-A        | BL1, BL2, BL31                                       |
-| Components``           |                                                      |
+| Affected TF-A          | BL1, BL2, BL31                                       |
+| Components             |                                                      |
 +------------------------+------------------------------------------------------+
-| ``Assets``             | Code Execution, Sensitive Data                       |
+| Assets                 | Code Execution, Sensitive Data                       |
 +------------------------+------------------------------------------------------+
-| ``Threat Agent``       | NSCode, SecCode                                      |
+| Threat Agent           | NSCode, SecCode                                      |
 +------------------------+------------------------------------------------------+
-| ``Threat Type``        | Tampering, Information Disclosure,                   |
+| Threat Type            | Tampering, Information Disclosure,                   |
 |                        | Elevation of Privilege                               |
 +------------------------+-------------------+-----------------+----------------+
-| ``Application``        | ``Server``        | ``IoT``         | ``Mobile``     |
+| Application            | Server            | IoT             | Mobile         |
 +------------------------+-------------------+-----------------+----------------+
-| ``Impact``             | Critical (5)      | Critical (5)    | Critical (5)   |
+| Impact                 | Critical (5)      | Critical (5)    | Critical (5)   |
 +------------------------+-------------------+-----------------+----------------+
-| ``Likelihood``         | Medium (3         | Medium (3)      | Medium (3)     |
+| Likelihood             | Medium (3         | Medium (3)      | Medium (3)     |
 +------------------------+-------------------+-----------------+----------------+
-| ``Total Risk Rating``  | High (15)         | High (15)       | High (15)      |
+| Total Risk Rating      | High (15)         | High (15)       | High (15)      |
 +------------------------+-------------------+-----------------+----------------+
-| ``Mitigations``        | | TF-A uses a combination of manual code reviews and |
-|                        |   automated program analysis and testing to detect   |
-|                        |   and fix memory corruption bugs. All TF-A code      |
-|                        |   including platform code go through manual code     |
-|                        |   reviews. Additionally, static code analysis is     |
-|                        |   performed using Coverity Scan on all TF-A code.    |
-|                        |   The code is also tested  with                      |
-|                        |   `Trusted Firmware-A Tests`_ on Juno and FVP        |
-|                        |   platforms.                                         |
+| Mitigations            | | 1) Use proper input validation.                    |
 |                        |                                                      |
-|                        | | Data received from normal world, such as addresses |
+|                        | | 2) Code reviews, testing.                          |
++------------------------+------------------------------------------------------+
+| Mitigations            | | 1) Yes.                                            |
+| implemented?           |   Data received from normal world, such as addresses |
 |                        |   and sizes identifying memory regions, are          |
 |                        |   sanitized before being used. These security checks |
 |                        |   make sure that the normal world software does not  |
@@ -602,48 +677,62 @@
 |                        |   option to use *asserts* in release builds, however |
 |                        |   we recommend using proper runtime checks instead   |
 |                        |   of relying on asserts in release builds.           |
+|                        |                                                      |
+|                        | | 2) Yes.                                            |
+|                        |   TF-A uses a combination of manual code reviews     |
+|                        |   and automated program analysis and testing to      |
+|                        |   detect and fix memory corruption bugs. All TF-A    |
+|                        |   code including platform code go through manual     |
+|                        |   code reviews. Additionally, static code analysis   |
+|                        |   is performed using Coverity Scan on all TF-A code. |
+|                        |   The code is also tested  with                      |
+|                        |   `Trusted Firmware-A Tests`_ on Juno and FVP        |
+|                        |   platforms.                                         |
 +------------------------+------------------------------------------------------+
 
 +------------------------+------------------------------------------------------+
 | ID                     | 09                                                   |
 +========================+======================================================+
-| ``Threat``             | | **Improperly handled SMC calls can leak register   |
+| Threat                 | | **Improperly handled SMC calls can leak register   |
 |                        |   contents**                                         |
 |                        |                                                      |
-|                        | | When switching between secure and non-secure       |
-|                        |   states, register contents of Secure world or       |
-|                        |   register contents of other normal world clients    |
-|                        |   can be leaked.                                     |
+|                        | | When switching between worlds, TF-A register state |
+|                        |   can leak to software in different security         |
+|                        |   contexts.                                          |
 +------------------------+------------------------------------------------------+
-| ``Diagram Elements``   | DF5                                                  |
+| Diagram Elements       | DF4, DF5                                             |
 +------------------------+------------------------------------------------------+
-| ``Affected TF-A        | BL31                                                 |
-| Components``           |                                                      |
+| Affected TF-A          | BL31                                                 |
+| Components             |                                                      |
 +------------------------+------------------------------------------------------+
-| ``Assets``             | Sensitive Data                                       |
+| Assets                 | Sensitive Data                                       |
 +------------------------+------------------------------------------------------+
-| ``Threat Agent``       | NSCode                                               |
+| Threat Agent           | NSCode, SecCode                                      |
 +------------------------+------------------------------------------------------+
-| ``Threat Type``        | Information Disclosure                               |
+| Threat Type            | Information Disclosure                               |
 +------------------------+-------------------+----------------+-----------------+
-| ``Application``        | ``Server``        | ``IoT``        | ``Mobile``      |
+| Application            | Server            | IoT            | Mobile          |
 +------------------------+-------------------+----------------+-----------------+
-| ``Impact``             | Medium (3)        | Medium (3)     | Medium (3)      |
+| Impact                 | Medium (3)        | Medium (3)     | Medium (3)      |
 +------------------------+-------------------+----------------+-----------------+
-| ``Likelihood``         | High (4)          | High (4)       | High (4)        |
+| Likelihood             | High (4)          | High (4)       | High (4)        |
 +------------------------+-------------------+----------------+-----------------+
-| ``Total Risk Rating``  | High (12)         | High (12)      | High (12)       |
+| Total Risk Rating      | High (12)         | High (12)      | High (12)       |
 +------------------------+-------------------+----------------+-----------------+
-| ``Mitigations``        | | TF-A saves and restores registers                  |
-|                        |   by default when switching contexts. Build options  |
-|                        |   are also provided to save/restore additional       |
-|                        |   registers such as floating-point registers.        |
+| Mitigations            | Save and restore registers when switching contexts.  |
++------------------------+------------------------------------------------------+
+| Mitigations            | | Yes.                                               |
+| implemented?           |                                                      |
+|                        | | This is the default behaviour in TF-A.             |
+|                        |   Build options are also provided to save/restore    |
+|                        |   additional registers such as floating-point        |
+|                        |   registers. These should be enabled if required.    |
 +------------------------+------------------------------------------------------+
 
 +------------------------+-----------------------------------------------------+
 | ID                     | 10                                                  |
 +========================+=====================================================+
-| ``Threat``             | | **SMC calls can leak sensitive information from   |
+| Threat                 | | **SMC calls can leak sensitive information from   |
 |                        |   TF-A memory via microarchitectural side channels**|
 |                        |                                                     |
 |                        | | Microarchitectural side-channel attacks such as   |
@@ -652,36 +741,42 @@
 |                        |   use this kind of attack to leak sensitive         |
 |                        |   data from TF-A memory.                            |
 +------------------------+-----------------------------------------------------+
-| ``Diagram Elements``   | DF4, DF5                                            |
+| Diagram Elements       | DF4, DF5                                            |
 +------------------------+-----------------------------------------------------+
-| ``Affected TF-A        | BL31                                                |
-| Components``           |                                                     |
+| Affected TF-A          | BL31                                                |
+| Components             |                                                     |
 +------------------------+-----------------------------------------------------+
-| ``Assets``             | Sensitive Data                                      |
+| Assets                 | Sensitive Data                                      |
 +------------------------+-----------------------------------------------------+
-| ``Threat Agent``       | SecCode, NSCode                                     |
+| Threat Agent           | SecCode, NSCode                                     |
 +------------------------+-----------------------------------------------------+
-| ``Threat Type``        | Information Disclosure                              |
+| Threat Type            | Information Disclosure                              |
 +------------------------+-------------------+----------------+----------------+
-| ``Application``        | ``Server``        | ``IoT``        | ``Mobile``     |
+| Application            | Server            | IoT            | Mobile         |
 +------------------------+-------------------+----------------+----------------+
-| ``Impact``             | Medium (3)        | Medium (3)     | Medium (3)     |
+| Impact                 | Medium (3)        | Medium (3)     | Medium (3)     |
 +------------------------+-------------------+----------------+----------------+
-| ``Likelihood``         | Medium (3)        | Medium (3)     | Medium (3)     |
+| Likelihood             | Medium (3)        | Medium (3)     | Medium (3)     |
 +------------------------+-------------------+----------------+----------------+
-| ``Total Risk Rating``  | Medium (9)        | Medium (9)     | Medium (9)     |
+| Total Risk Rating      | Medium (9)        | Medium (9)     | Medium (9)     |
 +------------------------+-------------------+----------------+----------------+
-| ``Mitigations``        | | TF-A implements software mitigations for Spectre  |
+| Mitigations            | Enable appropriate side-channel protections.        |
++------------------------+-----------------------------------------------------+
+| Mitigations            | | Yes / Platform specific.                          |
+| implemented?           |                                                     |
+|                        | | TF-A implements software mitigations for Spectre  |
 |                        |   type attacks as recommended by `Cache Speculation |
-|                        |   Side-channels`_ for the generic code. SiPs should |
-|                        |   implement similar mitigations for code that is    |
-|                        |   deemed to be vulnerable to such attacks.          |
+|                        |   Side-channels`_ for the generic code.             |
+|                        |                                                     |
+|                        | | SiPs should implement similar mitigations for     |
+|                        |   code that is deemed to be vulnerable to such      |
+|                        |   attacks.                                          |
 +------------------------+-----------------------------------------------------+
 
 +------------------------+----------------------------------------------------+
 | ID                     | 11                                                 |
 +========================+====================================================+
-| ``Threat``             | | **Misconfiguration of the Memory Management Unit |
+| Threat                 | | **Misconfiguration of the Memory Management Unit |
 |                        |   (MMU) may allow a normal world software to       |
 |                        |   access sensitive data or execute arbitrary       |
 |                        |   code**                                           |
@@ -692,44 +787,50 @@
 |                        |   execute code if the proper security mechanisms   |
 |                        |   are not in place.                                |
 +------------------------+----------------------------------------------------+
-| ``Diagram Elements``   | DF5, DF6                                           |
+| Diagram Elements       | DF5, DF6                                           |
 +------------------------+----------------------------------------------------+
-| ``Affected TF-A        | BL1, BL2, BL31                                     |
-| Components``           |                                                    |
+| Affected TF-A          | BL1, BL2, BL31                                     |
+| Components             |                                                    |
 +------------------------+----------------------------------------------------+
-| ``Assets``             | Sensitive Data, Code execution                     |
+| Assets                 | Sensitive Data, Code execution                     |
 +------------------------+----------------------------------------------------+
-| ``Threat Agent``       | NSCode                                             |
+| Threat Agent           | NSCode                                             |
 +------------------------+----------------------------------------------------+
-| ``Threat Type``        | Information Disclosure, Elevation of Privilege     |
+| Threat Type            | Information Disclosure, Elevation of Privilege     |
 +------------------------+-----------------+-----------------+----------------+
-| ``Application``        | ``Server``      | ``IoT``         | ``Mobile``     |
+| Application            | Server          | IoT             | Mobile         |
 +------------------------+-----------------+-----------------+----------------+
-| ``Impact``             | Critical (5)    | Critical (5)    | Critical (5)   |
+| Impact                 | Critical (5)    | Critical (5)    | Critical (5)   |
 +------------------------+-----------------+-----------------+----------------+
-| ``Likelihood``         | High (4)        | High (4)        | High (4)       |
+| Likelihood             | High (4)        | High (4)        | High (4)       |
 +------------------------+-----------------+-----------------+----------------+
-| ``Total Risk Rating``  | Critical (20)   | Critical (20)   | Critical (20)  |
+| Total Risk Rating      | Critical (20)   | Critical (20)   | Critical (20)  |
 +------------------------+-----------------+-----------------+----------------+
-| ``Mitigations``        | | In TF-A, configuration of the MMU is done        |
-|                        |   through a translation tables library. The        |
-|                        |   library provides APIs to define memory regions   |
-|                        |   and assign attributes including memory types and |
-|                        |   access permissions. Memory configurations are    |
-|                        |   platform specific, therefore platforms need make |
-|                        |   sure the correct attributes are assigned to      |
-|                        |   memory regions. When assigning access            |
-|                        |   permissions, principle of least privilege ought  |
-|                        |   to be enforced, i.e. we should not grant more    |
-|                        |   privileges than strictly needed, e.g. code       |
-|                        |   should be read-only executable, RO data should   |
-|                        |   be read-only XN, and so on.                      |
+| Mitigations            | When configuring access permissions, the           |
+|                        | principle of least privilege ought to be           |
+|                        | enforced. This means we should not grant more      |
+|                        | privileges than strictly needed, e.g. code         |
+|                        | should be read-only executable, read-only data     |
+|                        | should be read-only execute-never, and so on.      |
++------------------------+----------------------------------------------------+
+| Mitigations            | | Platform specific.                               |
+| implemented?           |                                                    |
+|                        | | MMU configuration is platform specific,          |
+|                        |   therefore platforms need to make sure that the   |
+|                        |   correct attributes are assigned to memory        |
+|                        |   regions.                                         |
+|                        |                                                    |
+|                        | | TF-A provides a library which abstracts the      |
+|                        |   low-level details of MMU configuration. It       |
+|                        |   provides well-defined and tested APIs.           |
+|                        |   Platforms are encouraged to use it to limit the  |
+|                        |   risk of misconfiguration.                        |
 +------------------------+----------------------------------------------------+
 
 +------------------------+-----------------------------------------------------+
 | ID                     | 12                                                  |
 +========================+=====================================================+
-| ``Threat``             | | **Incorrect configuration of Performance Monitor  |
+| Threat                 | | **Incorrect configuration of Performance Monitor  |
 |                        |   Unit (PMU) counters can allow an attacker to      |
 |                        |   mount side-channel attacks using information      |
 |                        |   exposed by the counters**                         |
@@ -738,43 +839,50 @@
 |                        |   to count events at any exception level and in     |
 |                        |   both Secure and Non-secure states. This allows    |
 |                        |   a Non-secure software (or a lower-level Secure    |
-|                        |   software) to potentially  carry out               |
+|                        |   software) to potentially carry out                |
 |                        |   side-channel timing attacks against TF-A.         |
 +------------------------+-----------------------------------------------------+
-| ``Diagram Elements``   | DF5, DF6                                            |
+| Diagram Elements       | DF5, DF6                                            |
 +------------------------+-----------------------------------------------------+
-| ``Affected TF-A        | BL31                                                |
-| Components``           |                                                     |
+| Affected TF-A          | BL31                                                |
+| Components             |                                                     |
 +------------------------+-----------------------------------------------------+
-| ``Assets``             | Sensitive Data                                      |
+| Assets                 | Sensitive Data                                      |
 +------------------------+-----------------------------------------------------+
-| ``Threat Agent``       | NSCode                                              |
+| Threat Agent           | NSCode                                              |
 +------------------------+-----------------------------------------------------+
-| ``Threat Type``        | Information Disclosure                              |
+| Threat Type            | Information Disclosure                              |
 +------------------------+-------------------+----------------+----------------+
-| ``Impact``             | Medium (3)        | Medium (3)     | Medium (3)     |
+| Impact                 | Medium (3)        | Medium (3)     | Medium (3)     |
 +------------------------+-------------------+----------------+----------------+
-| ``Likelihood``         | Low (2)           | Low (2)        | Low (2)        |
+| Likelihood             | Low (2)           | Low (2)        | Low (2)        |
 +------------------------+-------------------+----------------+----------------+
-| ``Total Risk Rating``  | Medium (6)        | Medium (6)     | Medium (6)     |
+| Total Risk Rating      | Medium (6)        | Medium (6)     | Medium (6)     |
 +------------------------+-------------------+----------------+----------------+
-| ``Mitigations``        | | TF-A follows mitigation strategies as described   |
-|                        |   in `Secure Development Guidelines`_. General      |
-|                        |   events and cycle counting in the Secure world is  |
-|                        |   prohibited by default when applicable. However,   |
-|                        |   on some implementations (e.g. PMUv3) Secure world |
-|                        |   event counting depends on external debug interface|
-|                        |   signals, i.e. Secure world event counting is      |
-|                        |   enabled if external debug is enabled.             |
-|                        |   Configuration of debug signals is platform        |
+| Mitigations            | Follow mitigation strategies as described in        |
+|                        | `Secure Development Guidelines`_.                   |
++------------------------+-----------------------------------------------------+
+| Mitigations            | | Yes / platform specific.                          |
+| implemented?           |                                                     |
+|                        | | General events and cycle counting in the Secure   |
+|                        |   world is prohibited by default when applicable.   |
+|                        |                                                     |
+|                        | | However, on some implementations (e.g. PMUv3)     |
+|                        |   Secure world event counting depends on external   |
+|                        |   debug interface signals, i.e. Secure world event  |
+|                        |   counting is enabled if external debug is enabled. |
+|                        |                                                     |
+|                        | | Configuration of debug signals is platform        |
 |                        |   specific, therefore platforms need to make sure   |
 |                        |   that external debug is disabled in production or  |
-|                        |   proper debug authentication is in place.          |
+|                        |   proper debug authentication is in place. This     |
+|                        |   should be the case if threat #06 is properly      |
+|                        |   mitigated.                                        |
 +------------------------+-----------------------------------------------------+
 
 --------------
 
-*Copyright (c) 2021, Arm Limited. All rights reserved.*
+*Copyright (c) 2021-2022, Arm Limited. All rights reserved.*
 
 
 .. _STRIDE threat analysis technique: https://docs.microsoft.com/en-us/azure/security/develop/threat-modeling-tool-threats#stride-model

diff --git a/drivers/arm/gic/v3/gic-x00.c b/drivers/arm/gic/v3/gic-x00.c
index aaef485..75eb69a 100644
--- a/drivers/arm/gic/v3/gic-x00.c
+++ b/drivers/arm/gic/v3/gic-x00.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, Arm Limited and Contributors. All rights reserved.
  * Copyright (c) 2020, NVIDIA Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
@@ -42,6 +42,8 @@
 #define PWRR_ON				(0U << PWRR_RDPD_SHIFT)
 #define PWRR_OFF			(1U << PWRR_RDPD_SHIFT)
 
+static bool gic600_errata_wa_2384374 __unused;
+
 #if GICV3_SUPPORT_GIC600
 
 /* GIC-600/700 specific accessor functions */
@@ -170,3 +172,60 @@
 	}
 #endif
 }
+
+#if GIC600_ERRATA_WA_2384374
+/*******************************************************************************
+ * Apply part 2 of workaround for errata-2384374 as per SDEN:
+ * https://developer.arm.com/documentation/sden892601/latest/
+ ******************************************************************************/
+void gicv3_apply_errata_wa_2384374(uintptr_t gicr_base)
+{
+	if (gic600_errata_wa_2384374) {
+		uint32_t gicr_ctlr_val = gicr_read_ctlr(gicr_base);
+
+		gicr_write_ctlr(gicr_base, gicr_ctlr_val |
+				(GICR_CTLR_DPG0_BIT | GICR_CTLR_DPG1NS_BIT |
+				GICR_CTLR_DPG1S_BIT));
+		gicr_write_ctlr(gicr_base, gicr_ctlr_val &
+				~(GICR_CTLR_DPG0_BIT | GICR_CTLR_DPG1NS_BIT |
+				  GICR_CTLR_DPG1S_BIT));
+	}
+}
+#endif /* GIC600_ERRATA_WA_2384374 */
+
+void gicv3_check_erratas_applies(uintptr_t gicd_base)
+{
+	unsigned int gic_prod_id;
+	uint8_t gic_rev;
+
+	assert(gicd_base != 0UL);
+
+	gicv3_get_component_prodid_rev(gicd_base, &gic_prod_id, &gic_rev);
+
+	/*
+	 * This workaround applicable only to GIC600 and GIC600AE products with
+	 * revision less than r1p6 and r0p2 respectively.
+	 * As per GIC600/GIC600AE specification -
+	 * r1p6 = 0x17 => GICD_IIDR[19:12]
+	 * r0p2 = 0x04 => GICD_IIDR[19:12]
+	 */
+	if ((gic_prod_id == GIC_PRODUCT_ID_GIC600) ||
+		    (gic_prod_id == GIC_PRODUCT_ID_GIC600AE)) {
+		if (((gic_prod_id == GIC_PRODUCT_ID_GIC600) &&
+		     (gic_rev <= GIC_REV(GIC_VARIANT_R1, GIC_REV_P6))) ||
+		     ((gic_prod_id == GIC_PRODUCT_ID_GIC600AE) &&
+		     (gic_rev <= GIC_REV(GIC_VARIANT_R0, GIC_REV_P2)))) {
+#if GIC600_ERRATA_WA_2384374
+			gic600_errata_wa_2384374 = true;
+			VERBOSE("%s applies\n",
+				"GIC600/GIC600AE errata workaround 2384374");
+#else
+			WARN("%s missing\n",
+			     "GIC600/GIC600AE errata workaround 2384374");
+#endif /* GIC600_ERRATA_WA_2384374 */
+		} else {
+			VERBOSE("%s not applies\n",
+				"GIC600/GIC600AE errata workaround 2384374");
+		}
+	}
+}

diff --git a/drivers/arm/gic/v3/gicv3.mk b/drivers/arm/gic/v3/gicv3.mk
index d7e3536..1d20ff3 100644
--- a/drivers/arm/gic/v3/gicv3.mk
+++ b/drivers/arm/gic/v3/gicv3.mk

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2013-2020, Arm Limited and Contributors. All rights reserved.
+# Copyright (c) 2013-2022, Arm Limited and Contributors. All rights reserved.
 # Copyright (c) 2021, NVIDIA Corporation. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
@@ -12,6 +12,7 @@
 GICV3_OVERRIDE_DISTIF_PWR_OPS	?=	0
 GIC_ENABLE_V4_EXTN		?=	0
 GIC_EXT_INTID			?=	0
+GIC600_ERRATA_WA_2384374	?=	${GICV3_SUPPORT_GIC600}
 
 GICV3_SOURCES	+=	drivers/arm/gic/v3/gicv3_main.c		\
 			drivers/arm/gic/v3/gicv3_helpers.c	\
@@ -47,3 +48,7 @@
 # Set support for extended PPI and SPI range
 $(eval $(call assert_boolean,GIC_EXT_INTID))
 $(eval $(call add_define,GIC_EXT_INTID))
+
+# Set errata workaround for GIC600/GIC600AE
+$(eval $(call assert_boolean,GIC600_ERRATA_WA_2384374))
+$(eval $(call add_define,GIC600_ERRATA_WA_2384374))

diff --git a/drivers/arm/gic/v3/gicv3_helpers.c b/drivers/arm/gic/v3/gicv3_helpers.c
index 753d995..f3852d2 100644
--- a/drivers/arm/gic/v3/gicv3_helpers.c
+++ b/drivers/arm/gic/v3/gicv3_helpers.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -408,3 +408,34 @@
 
 	return part_id;
 }
+
+/*******************************************************************************
+ * Helper function to return product ID and revision of GIC
+ * @gicd_base:   base address of the GIC distributor
+ * @gic_prod_id: retrieved product id of GIC
+ * @gic_rev:     retrieved revision of GIC
+ ******************************************************************************/
+void gicv3_get_component_prodid_rev(const uintptr_t gicd_base,
+				    unsigned int *gic_prod_id,
+				    uint8_t *gic_rev)
+{
+	unsigned int gicd_iidr;
+	uint8_t gic_variant;
+
+	gicd_iidr = gicd_read_iidr(gicd_base);
+	*gic_prod_id = gicd_iidr >> IIDR_PRODUCT_ID_SHIFT;
+	*gic_prod_id &= IIDR_PRODUCT_ID_MASK;
+
+	gic_variant = gicd_iidr >> IIDR_VARIANT_SHIFT;
+	gic_variant &= IIDR_VARIANT_MASK;
+
+	*gic_rev = gicd_iidr >> IIDR_REV_SHIFT;
+	*gic_rev &= IIDR_REV_MASK;
+
+	/*
+	 * pack gic variant and gic_rev in 1 byte
+	 * gic_rev = gic_variant[7:4] and gic_rev[0:3]
+	 */
+	*gic_rev = *gic_rev | gic_variant << 0x4;
+
+}

diff --git a/drivers/arm/gic/v3/gicv3_main.c b/drivers/arm/gic/v3/gicv3_main.c
index 53a8fae..8ead43b 100644
--- a/drivers/arm/gic/v3/gicv3_main.c
+++ b/drivers/arm/gic/v3/gicv3_main.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -169,6 +169,8 @@
 	flush_dcache_range((uintptr_t)gicv3_driver_data,
 		sizeof(*gicv3_driver_data));
 #endif
+	gicv3_check_erratas_applies(plat_driver_data->gicd_base);
+
 	INFO("GICv%u with%s legacy support detected.\n", gic_version,
 				(gicv2_compat == 0U) ? "" : "out");
 	INFO("ARM GICv%u driver initialized in EL3\n", gic_version);
@@ -362,9 +364,17 @@
 	/* Add DSB to ensure visibility of System register writes */
 	dsb();
 
-	/* Mark the connected core as asleep */
 	gicr_base = gicv3_driver_data->rdistif_base_addrs[proc_num];
-	assert(gicr_base != 0U);
+	assert(gicr_base != 0UL);
+
+	/*
+	 * dsb() already issued previously after clearing the CPU group
+	 * enabled, apply below workaround to toggle the "DPG*"
+	 * bits of GICR_CTLR register for unblocking event.
+	 */
+	gicv3_apply_errata_wa_2384374(gicr_base);
+
+	/* Mark the connected core as asleep */
 	gicv3_rdistif_mark_core_asleep(gicr_base);
 }
 

diff --git a/drivers/arm/mhu/mhu_v2_x.c b/drivers/arm/mhu/mhu_v2_x.c
new file mode 100644
index 0000000..3103b92
--- /dev/null
+++ b/drivers/arm/mhu/mhu_v2_x.c

@@ -0,0 +1,379 @@
+/*
+ * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "mhu_v2_x.h"
+
+#define MHU_V2_X_MAX_CHANNELS		124
+#define MHU_V2_1_MAX_CHCOMB_INT		4
+#define ENABLE				0x1
+#define DISABLE				0x0
+#define CLEAR_INTR			0x1
+#define CH_PER_CH_COMB			0x20
+#define SEND_FRAME(p_mhu)		((struct mhu_v2_x_send_frame_t *)p_mhu)
+#define RECV_FRAME(p_mhu)		((struct mhu_v2_x_recv_frame_t *)p_mhu)
+
+#define MHU_MAJOR_REV_V2		0x1u
+#define MHU_MINOR_REV_2_0		0x0u
+#define MHU_MINOR_REV_2_1		0x1u
+
+struct mhu_v2_x_send_ch_window_t {
+	/* Offset: 0x00 (R/ ) Channel Status */
+	volatile uint32_t ch_st;
+	/* Offset: 0x04 (R/ ) Reserved */
+	volatile uint32_t reserved_0;
+	/* Offset: 0x08 (R/ ) Reserved */
+	volatile uint32_t reserved_1;
+	/* Offset: 0x0C ( /W) Channel Set */
+	volatile uint32_t ch_set;
+	/* Offset: 0x10 (R/ ) Channel Interrupt Status (Reserved in 2.0) */
+	volatile uint32_t ch_int_st;
+	/* Offset: 0x14 ( /W) Channel Interrupt Clear  (Reserved in 2.0) */
+	volatile uint32_t ch_int_clr;
+	/* Offset: 0x18 (R/W) Channel Interrupt Enable (Reserved in 2.0) */
+	volatile uint32_t ch_int_en;
+	/* Offset: 0x1C (R/ ) Reserved */
+	volatile uint32_t reserved_2;
+};
+
+struct mhu_v2_x_send_frame_t {
+	/* Offset: 0x000 ( / ) Sender Channel Window 0 -123 */
+	struct mhu_v2_x_send_ch_window_t send_ch_window[MHU_V2_X_MAX_CHANNELS];
+	/* Offset: 0xF80 (R/ ) Message Handling Unit Configuration */
+	volatile uint32_t mhu_cfg;
+	/* Offset: 0xF84 (R/W) Response Configuration */
+	volatile uint32_t resp_cfg;
+	/* Offset: 0xF88 (R/W) Access Request */
+	volatile uint32_t access_request;
+	/* Offset: 0xF8C (R/ ) Access Ready */
+	volatile uint32_t access_ready;
+	/* Offset: 0xF90 (R/ ) Interrupt Status */
+	volatile uint32_t int_st;
+	/* Offset: 0xF94 ( /W) Interrupt Clear */
+	volatile uint32_t int_clr;
+	/* Offset: 0xF98 (R/W) Interrupt Enable */
+	volatile uint32_t int_en;
+	/* Offset: 0xF9C (R/ ) Reserved */
+	volatile uint32_t reserved_0;
+	/* Offset: 0xFA0 (R/W) Channel Combined IRQ Stat (Reserved in 2.0) */
+	volatile uint32_t ch_comb_int_st[MHU_V2_1_MAX_CHCOMB_INT];
+	/* Offset: 0xFC4 (R/ ) Reserved */
+	volatile uint32_t reserved_1[6];
+	/* Offset: 0xFC8 (R/ ) Implementer Identification Register */
+	volatile uint32_t iidr;
+	/* Offset: 0xFCC (R/ ) Architecture Identification Register */
+	volatile uint32_t aidr;
+	/* Offset: 0xFD0 (R/ )  */
+	volatile uint32_t pid_1[4];
+	/* Offset: 0xFE0 (R/ )  */
+	volatile uint32_t pid_0[4];
+	/* Offset: 0xFF0 (R/ )  */
+	volatile uint32_t cid[4];
+};
+
+struct mhu_v2_x_rec_ch_window_t {
+	/* Offset: 0x00 (R/ ) Channel Status */
+	volatile uint32_t ch_st;
+	/* Offset: 0x04 (R/ ) Channel Status Masked */
+	volatile uint32_t ch_st_msk;
+	/* Offset: 0x08 ( /W) Channel Clear */
+	volatile uint32_t ch_clr;
+	/* Offset: 0x0C (R/ ) Reserved */
+	volatile uint32_t reserved_0;
+	/* Offset: 0x10 (R/ ) Channel Mask Status */
+	volatile uint32_t ch_msk_st;
+	/* Offset: 0x14 ( /W) Channel Mask Set */
+	volatile uint32_t ch_msk_set;
+	/* Offset: 0x18 ( /W) Channel Mask Clear */
+	volatile uint32_t ch_msk_clr;
+	/* Offset: 0x1C (R/ ) Reserved */
+	volatile uint32_t reserved_1;
+};
+
+struct mhu_v2_x_recv_frame_t {
+	/* Offset: 0x000 ( / ) Receiver Channel Window 0 -123 */
+	struct mhu_v2_x_rec_ch_window_t rec_ch_window[MHU_V2_X_MAX_CHANNELS];
+	/* Offset: 0xF80 (R/ ) Message Handling Unit Configuration */
+	volatile uint32_t mhu_cfg;
+	/* Offset: 0xF84 (R/ ) Reserved */
+	volatile uint32_t reserved_0[3];
+	/* Offset: 0xF90 (R/ ) Interrupt Status (Reserved in 2.0) */
+	volatile uint32_t int_st;
+	/* Offset: 0xF94 (R/ ) Interrupt Clear  (Reserved in 2.0) */
+	volatile uint32_t int_clr;
+	/* Offset: 0xF98 (R/W) Interrupt Enable (Reserved in 2.0) */
+	volatile uint32_t int_en;
+	/* Offset: 0xF9C (R/ ) Reserved  */
+	volatile uint32_t reserved_1;
+	/* Offset: 0xFA0 (R/ ) Channel Combined IRQ Stat (Reserved in 2.0) */
+	volatile uint32_t ch_comb_int_st[MHU_V2_1_MAX_CHCOMB_INT];
+	/* Offset: 0xFB0 (R/ ) Reserved */
+	volatile uint32_t reserved_2[6];
+	/* Offset: 0xFC8 (R/ ) Implementer Identification Register */
+	volatile uint32_t iidr;
+	/* Offset: 0xFCC (R/ ) Architecture Identification Register */
+	volatile uint32_t aidr;
+	/* Offset: 0xFD0 (R/ )  */
+	volatile uint32_t pid_1[4];
+	/* Offset: 0xFE0 (R/ )  */
+	volatile uint32_t pid_0[4];
+	/* Offset: 0xFF0 (R/ )  */
+	volatile uint32_t cid[4];
+};
+
+union mhu_v2_x_frame {
+	struct mhu_v2_x_send_frame_t send_frame;
+	struct mhu_v2_x_recv_frame_t recv_frame;
+};
+
+enum mhu_v2_x_error_t mhu_v2_x_driver_init(struct mhu_v2_x_dev_t *dev,
+	 enum mhu_v2_x_supported_revisions rev)
+{
+	uint32_t AIDR = 0;
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (dev->is_initialized) {
+		return MHU_V_2_X_ERR_ALREADY_INIT;
+	}
+
+	if (rev == MHU_REV_READ_FROM_HW) {
+		/* Read revision from HW */
+		if (dev->frame == MHU_V2_X_RECEIVER_FRAME) {
+			AIDR = p_mhu->recv_frame.aidr;
+		} else {
+			AIDR = p_mhu->send_frame.aidr;
+		}
+
+		/* Get bits 7:4 to read major revision */
+		if (((AIDR >> 4) & 0b1111) != MHU_MAJOR_REV_V2) {
+			/* Unsupported MHU version */
+			return MHU_V_2_X_ERR_UNSUPPORTED_VERSION;
+		} /* No need to save major version, driver only supports MHUv2 */
+
+		/* Get bits 3:0 to read minor revision */
+		dev->subversion = AIDR & 0b1111;
+
+		if (dev->subversion != MHU_MINOR_REV_2_0 &&
+			dev->subversion != MHU_MINOR_REV_2_1) {
+			/* Unsupported subversion */
+			return MHU_V_2_X_ERR_UNSUPPORTED_VERSION;
+		}
+	} else {
+		/* Revisions were provided by caller */
+		if (rev == MHU_REV_2_0) {
+			dev->subversion = MHU_MINOR_REV_2_0;
+		} else if (rev == MHU_REV_2_1) {
+			dev->subversion = MHU_MINOR_REV_2_1;
+		} else {
+			/* Unsupported subversion */
+			return MHU_V_2_X_ERR_UNSUPPORTED_VERSION;
+		} /* No need to save major version, driver only supports MHUv2 */
+	}
+
+	dev->is_initialized = true;
+
+	return MHU_V_2_X_ERR_NONE;
+}
+
+uint32_t mhu_v2_x_get_num_channel_implemented(const struct mhu_v2_x_dev_t *dev)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame == MHU_V2_X_SENDER_FRAME) {
+		return (SEND_FRAME(p_mhu))->mhu_cfg;
+	} else {
+		assert(dev->frame == MHU_V2_X_RECEIVER_FRAME);
+		return (RECV_FRAME(p_mhu))->mhu_cfg;
+	}
+}
+
+enum mhu_v2_x_error_t mhu_v2_x_channel_send(const struct mhu_v2_x_dev_t *dev,
+	 uint32_t channel, uint32_t val)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame == MHU_V2_X_SENDER_FRAME) {
+		(SEND_FRAME(p_mhu))->send_ch_window[channel].ch_set = val;
+		return MHU_V_2_X_ERR_NONE;
+	} else {
+		return MHU_V_2_X_ERR_INVALID_ARG;
+	}
+}
+
+enum mhu_v2_x_error_t mhu_v2_x_channel_poll(const struct mhu_v2_x_dev_t *dev,
+	 uint32_t channel, uint32_t *value)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame == MHU_V2_X_SENDER_FRAME) {
+		*value = (SEND_FRAME(p_mhu))->send_ch_window[channel].ch_st;
+		return MHU_V_2_X_ERR_NONE;
+	} else {
+		return MHU_V_2_X_ERR_INVALID_ARG;
+	}
+}
+
+enum mhu_v2_x_error_t mhu_v2_x_channel_clear(const struct mhu_v2_x_dev_t *dev,
+	 uint32_t channel)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame == MHU_V2_X_RECEIVER_FRAME) {
+		(RECV_FRAME(p_mhu))->rec_ch_window[channel].ch_clr = UINT32_MAX;
+		return MHU_V_2_X_ERR_NONE;
+	} else {
+		return MHU_V_2_X_ERR_INVALID_ARG;
+	}
+}
+
+enum mhu_v2_x_error_t mhu_v2_x_channel_receive(
+	 const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t *value)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame == MHU_V2_X_RECEIVER_FRAME) {
+		*value = (RECV_FRAME(p_mhu))->rec_ch_window[channel].ch_st;
+		return MHU_V_2_X_ERR_NONE;
+	} else {
+		return MHU_V_2_X_ERR_INVALID_ARG;
+	}
+}
+
+enum mhu_v2_x_error_t mhu_v2_x_channel_mask_set(
+	 const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t mask)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame == MHU_V2_X_RECEIVER_FRAME) {
+		(RECV_FRAME(p_mhu))->rec_ch_window[channel].ch_msk_set = mask;
+		return MHU_V_2_X_ERR_NONE;
+	} else {
+		return MHU_V_2_X_ERR_INVALID_ARG;
+	}
+}
+
+enum mhu_v2_x_error_t mhu_v2_x_channel_mask_clear(
+	 const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t mask)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame == MHU_V2_X_RECEIVER_FRAME) {
+		(RECV_FRAME(p_mhu))->rec_ch_window[channel].ch_msk_clr = mask;
+		return MHU_V_2_X_ERR_NONE;
+	} else {
+		return MHU_V_2_X_ERR_INVALID_ARG;
+	}
+}
+enum mhu_v2_x_error_t mhu_v2_x_initiate_transfer(
+	 const struct mhu_v2_x_dev_t *dev)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame != MHU_V2_X_SENDER_FRAME) {
+		return MHU_V_2_X_ERR_INVALID_ARG;
+	}
+
+	(SEND_FRAME(p_mhu))->access_request = ENABLE;
+
+	while (!((SEND_FRAME(p_mhu))->access_ready)) {
+		/* Wait in a loop for access ready signal to be high */
+		;
+	}
+
+	return MHU_V_2_X_ERR_NONE;
+}
+
+enum mhu_v2_x_error_t mhu_v2_x_close_transfer(const struct mhu_v2_x_dev_t *dev)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame != MHU_V2_X_SENDER_FRAME) {
+		return MHU_V_2_X_ERR_INVALID_ARG;
+	}
+
+	(SEND_FRAME(p_mhu))->access_request = DISABLE;
+
+	return MHU_V_2_X_ERR_NONE;
+}

diff --git a/drivers/arm/mhu/mhu_v2_x.h b/drivers/arm/mhu/mhu_v2_x.h
new file mode 100644
index 0000000..10247d2
--- /dev/null
+++ b/drivers/arm/mhu/mhu_v2_x.h

@@ -0,0 +1,210 @@
+/*
+ * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MHU_V2_X_H
+#define MHU_V2_X_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define MHU_2_X_INTR_NR2R_OFF		(0x0u)
+#define MHU_2_X_INTR_R2NR_OFF		(0x1u)
+#define MHU_2_1_INTR_CHCOMB_OFF		(0x2u)
+
+#define MHU_2_X_INTR_NR2R_MASK		(0x1u << MHU_2_X_INTR_NR2R_OFF)
+#define MHU_2_X_INTR_R2NR_MASK		(0x1u << MHU_2_X_INTR_R2NR_OFF)
+#define MHU_2_1_INTR_CHCOMB_MASK	(0x1u << MHU_2_1_INTR_CHCOMB_OFF)
+
+enum mhu_v2_x_frame_t {
+	MHU_V2_X_SENDER_FRAME   = 0x0u,
+	MHU_V2_X_RECEIVER_FRAME = 0x1u,
+};
+
+enum mhu_v2_x_supported_revisions {
+	MHU_REV_READ_FROM_HW = 0,
+	MHU_REV_2_0,
+	MHU_REV_2_1,
+};
+
+struct mhu_v2_x_dev_t {
+	uintptr_t base;
+	enum mhu_v2_x_frame_t frame;
+	uint32_t subversion;	/*!< Hardware subversion: v2.X */
+	bool is_initialized;	/*!< Indicates if the MHU driver
+				 *   is initialized and enabled
+				 */
+};
+
+/**
+ * MHU v2 error enumeration types.
+ */
+enum mhu_v2_x_error_t {
+	MHU_V_2_X_ERR_NONE			=  0,
+	MHU_V_2_X_ERR_NOT_INIT			= -1,
+	MHU_V_2_X_ERR_ALREADY_INIT		= -2,
+	MHU_V_2_X_ERR_UNSUPPORTED_VERSION	= -3,
+	MHU_V_2_X_ERR_INVALID_ARG		= -4,
+	MHU_V_2_X_ERR_GENERAL			= -5
+};
+
+/**
+ * Initializes the driver.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ * rev		MHU revision (if can't be identified from HW).
+ *
+ * Reads the MHU hardware version.
+ *
+ * Returns mhu_v2_x_error_t error code.
+ *
+ * MHU revision only has to be specified when versions can't be read
+ * from HW (ARCH_MAJOR_REV reg reads as 0x0).
+ *
+ * This function doesn't check if dev is NULL.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_driver_init(struct mhu_v2_x_dev_t *dev,
+	enum mhu_v2_x_supported_revisions rev);
+
+/**
+ * Returns the number of channels implemented.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ *
+ * This function doesn't check if dev is NULL.
+ */
+uint32_t mhu_v2_x_get_num_channel_implemented(
+		const struct mhu_v2_x_dev_t *dev);
+
+/**
+ * Sends the value over a channel.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ * channel	Channel to send the value over.
+ * val		Value to send.
+ *
+ * Sends the value over a channel.
+ *
+ * Returns mhu_v2_x_error_t error code.
+ *
+ * This function doesn't check if dev is NULL.
+ * This function doesn't check if channel is implemented.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_channel_send(const struct mhu_v2_x_dev_t *dev,
+	uint32_t channel, uint32_t val);
+
+/**
+ * Polls sender channel status.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ * channel	Channel to poll the status of.
+ * value	Pointer to variable that will store the value.
+ *
+ * Polls sender channel status.
+ *
+ * Returns mhu_v2_x_error_t error code.
+ *
+ * This function doesn't check if dev is NULL.
+ * This function doesn't check if channel is implemented.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_channel_poll(const struct mhu_v2_x_dev_t *dev,
+	uint32_t channel, uint32_t *value);
+
+/**
+ * Clears the channel after the value is send over it.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ * channel	Channel to clear.
+ *
+ * Clears the channel after the value is send over it.
+ *
+ * Returns mhu_v2_x_error_t error code..
+ *
+ * This function doesn't check if dev is NULL.
+ * This function doesn't check if channel is implemented.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_channel_clear(const struct mhu_v2_x_dev_t *dev,
+	uint32_t channel);
+
+/**
+ * Receives the value over a channel.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ * channel	Channel to receive the value from.
+ * value	Pointer to variable that will store the value.
+ *
+ * Receives the value over a channel.
+ *
+ * Returns mhu_v2_x_error_t error code.
+ *
+ * This function doesn't check if dev is NULL.
+ * This function doesn't check if channel is implemented.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_channel_receive(
+	const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t *value);
+
+/**
+ * Sets bits in the Channel Mask.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ * channel	Which channel's mask to set.
+ * mask		Mask to be set over a receiver frame.
+ *
+ * Sets bits in the Channel Mask.
+ *
+ * Returns mhu_v2_x_error_t error code..
+ *
+ * This function doesn't check if dev is NULL.
+ *  This function doesn't check if channel is implemented.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_channel_mask_set(
+	const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t mask);
+
+/**
+ * Clears bits in the Channel Mask.
+ *
+ * dev	MHU device struct mhu_v2_x_dev_t.
+ * channel	Which channel's mask to clear.
+ * mask	Mask to be clear over a receiver frame.
+ *
+ * Clears bits in the Channel Mask.
+ *
+ * Returns mhu_v2_x_error_t error code.
+ *
+ * This function doesn't check if dev is NULL.
+ *  This function doesn't check if channel is implemented.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_channel_mask_clear(
+	const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t mask);
+
+/**
+ * Initiates a MHU transfer with the handshake signals.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ *
+ * Initiates a MHU transfer with the handshake signals in a blocking mode.
+ *
+ * Returns mhu_v2_x_error_t error code.
+ *
+ * This function doesn't check if dev is NULL.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_initiate_transfer(
+	const struct mhu_v2_x_dev_t *dev);
+
+/**
+ * Closes a MHU transfer with the handshake signals.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ *
+ * Closes a MHU transfer with the handshake signals in a blocking mode.
+ *
+ * Returns mhu_v2_x_error_t error code.
+ *
+ * This function doesn't check if dev is NULL.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_close_transfer(
+	const struct mhu_v2_x_dev_t *dev);
+
+#endif /* MHU_V2_X_H */

diff --git a/drivers/arm/mhu/mhu_wrapper_v2_x.c b/drivers/arm/mhu/mhu_wrapper_v2_x.c
new file mode 100644
index 0000000..d8b7cfd
--- /dev/null
+++ b/drivers/arm/mhu/mhu_wrapper_v2_x.c

@@ -0,0 +1,302 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <drivers/arm/mhu.h>
+
+#include "mhu_v2_x.h"
+
+#define MHU_NOTIFY_VALUE	(1234u)
+
+/*
+ * MHU devices for host:
+ * HSE: Host to Secure Enclave (sender device)
+ * SEH: Secure Enclave to Host (receiver device)
+ */
+struct mhu_v2_x_dev_t MHU1_HSE_DEV = {0, MHU_V2_X_SENDER_FRAME};
+struct mhu_v2_x_dev_t MHU1_SEH_DEV = {0, MHU_V2_X_RECEIVER_FRAME};
+
+static enum mhu_error_t error_mapping_to_mhu_error_t(enum mhu_v2_x_error_t err)
+{
+	switch (err) {
+	case MHU_V_2_X_ERR_NONE:
+		return MHU_ERR_NONE;
+	case MHU_V_2_X_ERR_NOT_INIT:
+		return MHU_ERR_NOT_INIT;
+	case MHU_V_2_X_ERR_ALREADY_INIT:
+		return MHU_ERR_ALREADY_INIT;
+	case MHU_V_2_X_ERR_UNSUPPORTED_VERSION:
+		return MHU_ERR_UNSUPPORTED_VERSION;
+	case MHU_V_2_X_ERR_INVALID_ARG:
+		return MHU_ERR_INVALID_ARG;
+	case MHU_V_2_X_ERR_GENERAL:
+		return MHU_ERR_GENERAL;
+	default:
+		return MHU_ERR_GENERAL;
+	}
+}
+
+static enum mhu_v2_x_error_t signal_and_wait_for_clear(void)
+{
+	enum mhu_v2_x_error_t err;
+	struct mhu_v2_x_dev_t *dev = &MHU1_HSE_DEV;
+	uint32_t val = MHU_NOTIFY_VALUE;
+	/* Using the last channel for notifications */
+	uint32_t channel_notify = mhu_v2_x_get_num_channel_implemented(dev) - 1;
+
+	err = mhu_v2_x_channel_send(dev, channel_notify, val);
+	if (err != MHU_V_2_X_ERR_NONE) {
+		return err;
+	}
+
+	do {
+		err = mhu_v2_x_channel_poll(dev, channel_notify, &val);
+		if (err != MHU_V_2_X_ERR_NONE) {
+			break;
+		}
+	} while (val != 0);
+
+	return err;
+}
+
+static enum mhu_v2_x_error_t wait_for_signal(void)
+{
+	enum mhu_v2_x_error_t err;
+	struct mhu_v2_x_dev_t *dev = &MHU1_SEH_DEV;
+	uint32_t val = 0;
+	/* Using the last channel for notifications */
+	uint32_t channel_notify = mhu_v2_x_get_num_channel_implemented(dev) - 1;
+
+	do {
+		err = mhu_v2_x_channel_receive(dev, channel_notify, &val);
+		if (err != MHU_V_2_X_ERR_NONE) {
+			break;
+		}
+	} while (val != MHU_NOTIFY_VALUE);
+
+	return err;
+}
+
+static enum mhu_v2_x_error_t clear_and_wait_for_next_signal(void)
+{
+	enum mhu_v2_x_error_t err;
+	struct mhu_v2_x_dev_t *dev = &MHU1_SEH_DEV;
+	uint32_t num_channels = mhu_v2_x_get_num_channel_implemented(dev);
+	uint32_t i;
+
+	/* Clear all channels */
+	for (i = 0; i < num_channels; ++i) {
+		err = mhu_v2_x_channel_clear(dev, i);
+		if (err != MHU_V_2_X_ERR_NONE) {
+			return err;
+		}
+	}
+
+	return wait_for_signal();
+}
+
+enum mhu_error_t mhu_init_sender(uintptr_t mhu_sender_base)
+{
+	enum mhu_v2_x_error_t err;
+
+	assert(mhu_sender_base != (uintptr_t)NULL);
+
+	MHU1_HSE_DEV.base = mhu_sender_base;
+
+	err = mhu_v2_x_driver_init(&MHU1_HSE_DEV, MHU_REV_READ_FROM_HW);
+	return error_mapping_to_mhu_error_t(err);
+}
+
+enum mhu_error_t mhu_init_receiver(uintptr_t mhu_receiver_base)
+{
+	enum mhu_v2_x_error_t err;
+	uint32_t num_channels, i;
+
+	assert(mhu_receiver_base != (uintptr_t)NULL);
+
+	MHU1_SEH_DEV.base = mhu_receiver_base;
+
+	err = mhu_v2_x_driver_init(&MHU1_SEH_DEV, MHU_REV_READ_FROM_HW);
+	if (err != MHU_V_2_X_ERR_NONE) {
+		return error_mapping_to_mhu_error_t(err);
+	}
+
+	num_channels = mhu_v2_x_get_num_channel_implemented(&MHU1_SEH_DEV);
+
+	/* Mask all channels except the notifying channel */
+	for (i = 0; i < (num_channels - 1); ++i) {
+		err = mhu_v2_x_channel_mask_set(&MHU1_SEH_DEV, i, UINT32_MAX);
+		if (err != MHU_V_2_X_ERR_NONE) {
+			return error_mapping_to_mhu_error_t(err);
+		}
+	}
+
+	/* The last channel is used for notifications */
+	err = mhu_v2_x_channel_mask_clear(
+		&MHU1_SEH_DEV, (num_channels - 1), UINT32_MAX);
+	return error_mapping_to_mhu_error_t(err);
+}
+
+/*
+ * Public function. See mhu.h
+ *
+ * The basic steps of transferring a message:
+ * 1.	Initiate MHU transfer.
+ * 2.	Send over the size of the payload on Channel 1. It is the very first
+ *	4 Bytes of the transfer. Continue with Channel 2.
+ * 3.	Send over the payload, writing the channels one after the other
+ *	(4 Bytes each). The last available channel is reserved for controlling
+ *	the transfer.
+ *	When the last channel is reached or no more data is left, STOP.
+ * 4.	Notify the receiver using the last channel and wait for acknowledge.
+ *	If there is still data to transfer, jump to step 3. Otherwise, proceed.
+ * 5.	Close MHU transfer.
+ *
+ */
+enum mhu_error_t mhu_send_data(const uint8_t *send_buffer, size_t size)
+{
+	enum mhu_v2_x_error_t err;
+	struct mhu_v2_x_dev_t *dev = &MHU1_HSE_DEV;
+	uint32_t num_channels = mhu_v2_x_get_num_channel_implemented(dev);
+	uint32_t chan = 0;
+	uint32_t i;
+	uint32_t *p;
+
+	/* For simplicity, require the send_buffer to be 4-byte aligned */
+	if ((uintptr_t)send_buffer & 0x3U) {
+		return MHU_ERR_INVALID_ARG;
+	}
+
+	err = mhu_v2_x_initiate_transfer(dev);
+	if (err != MHU_V_2_X_ERR_NONE) {
+		return error_mapping_to_mhu_error_t(err);
+	}
+
+	/* First send over the size of the actual message */
+	err = mhu_v2_x_channel_send(dev, chan, (uint32_t)size);
+	if (err != MHU_V_2_X_ERR_NONE) {
+		return error_mapping_to_mhu_error_t(err);
+	}
+	chan++;
+
+	p = (uint32_t *)send_buffer;
+	for (i = 0; i < size; i += 4) {
+		err = mhu_v2_x_channel_send(dev, chan, *p++);
+		if (err != MHU_V_2_X_ERR_NONE) {
+			return error_mapping_to_mhu_error_t(err);
+		}
+		if (++chan == (num_channels - 1)) {
+			err = signal_and_wait_for_clear();
+			if (err != MHU_V_2_X_ERR_NONE) {
+				return error_mapping_to_mhu_error_t(err);
+			}
+			chan = 0;
+		}
+	}
+
+	/* Signal the end of transfer.
+	 *   It's not required to send a signal when the message was
+	 *   perfectly-aligned (num_channels - 1 channels were used in the last
+	 *   round) preventing it from signaling twice at the end of transfer.
+	 */
+	if (chan != 0) {
+		err = signal_and_wait_for_clear();
+		if (err != MHU_V_2_X_ERR_NONE) {
+			return error_mapping_to_mhu_error_t(err);
+		}
+	}
+
+	err = mhu_v2_x_close_transfer(dev);
+	return error_mapping_to_mhu_error_t(err);
+}
+
+/*
+ * Public function. See mhu.h
+ *
+ * The basic steps of receiving a message:
+ * 1.	Read the size of the payload from Channel 1. It is the very first
+ *	4 Bytes of the transfer. Continue with Channel 2.
+ * 2.	Receive the payload, read the channels one after the other
+ *	(4 Bytes each). The last available channel is reserved for controlling
+ *	the transfer.
+ *	When the last channel is reached clear all the channels
+ *	(also sending an acknowledge on the last channel).
+ * 3.	If there is still data to receive wait for a notification on the last
+ *	channel and jump to step 2 as soon as it arrived. Otherwise, proceed.
+ * 4.	End of transfer.
+ *
+ */
+enum mhu_error_t mhu_receive_data(uint8_t *receive_buffer, size_t *size)
+{
+	enum mhu_v2_x_error_t err;
+	struct mhu_v2_x_dev_t *dev = &MHU1_SEH_DEV;
+	uint32_t num_channels = mhu_v2_x_get_num_channel_implemented(dev);
+	uint32_t chan = 0;
+	uint32_t message_len;
+	uint32_t i;
+	uint32_t *p;
+
+	/* For simplicity, require:
+	 * - the receive_buffer to be 4-byte aligned,
+	 * - the buffer size to be a multiple of 4.
+	 */
+	if (((uintptr_t)receive_buffer & 0x3U) || (*size & 0x3U)) {
+		return MHU_ERR_INVALID_ARG;
+	}
+
+	/* Busy wait for incoming reply */
+	err = wait_for_signal();
+	if (err != MHU_V_2_X_ERR_NONE) {
+		return error_mapping_to_mhu_error_t(err);
+	}
+
+	/* The first word is the length of the actual message */
+	err = mhu_v2_x_channel_receive(dev, chan, &message_len);
+	if (err != MHU_V_2_X_ERR_NONE) {
+		return error_mapping_to_mhu_error_t(err);
+	}
+	chan++;
+
+	if (message_len > *size) {
+		/* Message buffer too small */
+		*size = message_len;
+		return MHU_ERR_BUFFER_TOO_SMALL;
+	}
+
+	p = (uint32_t *)receive_buffer;
+	for (i = 0; i < message_len; i += 4) {
+		err = mhu_v2_x_channel_receive(dev, chan, p++);
+		if (err != MHU_V_2_X_ERR_NONE) {
+			return error_mapping_to_mhu_error_t(err);
+		}
+
+		/* Only wait for next transfer if there is still missing data */
+		if (++chan == (num_channels - 1) && (message_len - i) > 4) {
+			/* Busy wait for next transfer */
+			err = clear_and_wait_for_next_signal();
+			if (err != MHU_V_2_X_ERR_NONE) {
+				return error_mapping_to_mhu_error_t(err);
+			}
+			chan = 0;
+		}
+	}
+
+	/* Clear all channels */
+	for (i = 0; i < num_channels; ++i) {
+		err = mhu_v2_x_channel_clear(dev, i);
+		if (err != MHU_V_2_X_ERR_NONE) {
+			return error_mapping_to_mhu_error_t(err);
+		}
+	}
+
+	*size = message_len;
+
+	return MHU_ERR_NONE;
+}

diff --git a/drivers/arm/rss/rss_comms.c b/drivers/arm/rss/rss_comms.c
new file mode 100644
index 0000000..28a4925
--- /dev/null
+++ b/drivers/arm/rss/rss_comms.c

@@ -0,0 +1,225 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/arm/mhu.h>
+#include <drivers/arm/rss_comms.h>
+#include <initial_attestation.h>
+#include <psa/client.h>
+
+#include <platform_def.h>
+
+#define TYPE_OFFSET	U(16)
+#define TYPE_MASK	(0xFFFFUL << TYPE_OFFSET)
+#define IN_LEN_OFFSET	U(8)
+#define IN_LEN_MASK	(0xFFUL << IN_LEN_OFFSET)
+#define OUT_LEN_OFFSET	U(0)
+#define OUT_LEN_MASK	(0xFFUL << OUT_LEN_OFFSET)
+
+#define PARAM_PACK(type, in_len, out_len)			  \
+	(((((uint32_t)type) << TYPE_OFFSET) & TYPE_MASK)	| \
+	 ((((uint32_t)in_len) << IN_LEN_OFFSET) & IN_LEN_MASK)	| \
+	 ((((uint32_t)out_len) << OUT_LEN_OFFSET) & OUT_LEN_MASK))
+
+#define PARAM_UNPACK_IN_LEN(ctrl_param) \
+	((size_t)(((ctrl_param) & IN_LEN_MASK) >> IN_LEN_OFFSET))
+
+/* Message types */
+struct __packed packed_psa_call_t {
+	uint8_t protocol_ver;
+	uint8_t seq_num;
+	uint16_t client_id;
+	psa_handle_t handle;
+	uint32_t ctrl_param; /* type, in_len, out_len */
+	uint16_t io_size[4];
+};
+
+struct __packed packed_psa_reply_t {
+	uint8_t protocol_ver;
+	uint8_t seq_num;
+	uint16_t client_id;
+	int32_t return_val;
+	uint16_t out_size[4];
+};
+
+/*
+ * In the current implementation the RoT Service request that requires the
+ * biggest message buffer is the RSS_ATTEST_GET_TOKEN. The maximum required
+ * buffer size is calculated based on the platform-specific needs of
+ * this request.
+ */
+#define MAX_REQUEST_PAYLOAD_SIZE	(PSA_INITIAL_ATTEST_CHALLENGE_SIZE_64 \
+					 + PLAT_ATTEST_TOKEN_MAX_SIZE)
+
+/* Buffer to store the messages to be sent/received. */
+static uint8_t message_buf[MAX_REQUEST_PAYLOAD_SIZE] __aligned(4);
+
+static int32_t pack_params(const psa_invec *invecs,
+			   size_t in_len,
+			   uint8_t *buf,
+			   size_t *buf_len)
+{
+	uint32_t i;
+	size_t payload_size = 0U;
+
+	for (i = 0U; i < in_len; ++i) {
+		if (invecs[i].len > *buf_len - payload_size) {
+			return -1;
+		}
+		memcpy(buf + payload_size, invecs[i].base, invecs[i].len);
+		payload_size += invecs[i].len;
+	}
+
+	*buf_len = payload_size;
+	return 0;
+}
+
+static int serialise_message(const struct packed_psa_call_t *msg,
+			     const psa_invec *invecs,
+			     uint8_t *payload_buf,
+			     size_t *payload_len)
+{
+	size_t message_len = 0U;
+	size_t len;
+
+	/* Copy the message header into the payload buffer. */
+	len = sizeof(*msg);
+	if (len > *payload_len) {
+		ERROR("[RSS-COMMS] Message buffer too small.\n");
+		return -1;
+	}
+	memcpy(payload_buf, (const void *)msg, len);
+	message_len += len;
+
+	/* The input data will follow the message header in the payload buffer. */
+	len = *payload_len - message_len;
+	if (pack_params(invecs, PARAM_UNPACK_IN_LEN(msg->ctrl_param),
+			payload_buf + message_len, &len) != 0) {
+		ERROR("[RSS-COMMS] Message buffer too small.\n");
+		return -1;
+	}
+	message_len += len;
+
+	*payload_len = message_len;
+	return 0;
+}
+
+static void unpack_params(const uint8_t *buf,
+			  psa_outvec *outvecs,
+			  size_t out_len)
+{
+	size_t i;
+
+	for (i = 0U; i < out_len; ++i) {
+		memcpy(outvecs[i].base, buf, outvecs[i].len);
+		buf += outvecs[i].len;
+	}
+}
+
+static void deserialise_reply(struct packed_psa_reply_t *reply,
+			      psa_outvec *outvecs,
+			      size_t outlen,
+			      const uint8_t *message,
+			      size_t message_len)
+{
+	uint32_t i;
+
+	memcpy(reply, message, sizeof(*reply));
+
+	/* Outvecs */
+	for (i = 0U; i < outlen; ++i) {
+		outvecs[i].len = reply->out_size[i];
+	}
+
+	unpack_params(message + sizeof(*reply), outvecs, outlen);
+}
+
+psa_status_t psa_call(psa_handle_t handle, int32_t type,
+		      const psa_invec *in_vec, size_t in_len,
+		      psa_outvec *out_vec, size_t out_len)
+{
+	enum mhu_error_t err;
+	static uint32_t seq_num = 1U;
+	struct packed_psa_call_t msg = {
+		.protocol_ver = 0U,
+		.seq_num = seq_num,
+		/* No need to distinguish callers (currently concurrent calls are not supported). */
+		.client_id = 1U,
+		.handle = handle,
+		.ctrl_param = PARAM_PACK(type, in_len, out_len),
+	};
+
+	struct packed_psa_reply_t reply = {0};
+	size_t message_size;
+	uint32_t i;
+
+	/* Fill msg iovec lengths */
+	for (i = 0U; i < in_len; ++i) {
+		msg.io_size[i] = in_vec[i].len;
+	}
+	for (i = 0U; i < out_len; ++i) {
+		msg.io_size[in_len + i] = out_vec[i].len;
+	}
+
+	message_size = sizeof(message_buf);
+	if (serialise_message(&msg, in_vec, message_buf, &message_size)) {
+		/* Local buffer is probably too small. */
+		return PSA_ERROR_INSUFFICIENT_MEMORY;
+	}
+
+	err = mhu_send_data(message_buf, message_size);
+	if (err != MHU_ERR_NONE) {
+		return PSA_ERROR_COMMUNICATION_FAILURE;
+	}
+
+	message_size = sizeof(message_buf);
+#if DEBUG
+	/*
+	 * Poisoning the message buffer (with a known pattern).
+	 * Helps in detecting hypothetical RSS communication bugs.
+	 */
+	memset(message_buf, 0xA5, message_size);
+#endif
+	err = mhu_receive_data(message_buf, &message_size);
+	if (err != MHU_ERR_NONE) {
+		return PSA_ERROR_COMMUNICATION_FAILURE;
+	}
+
+	deserialise_reply(&reply, out_vec, out_len, message_buf, message_size);
+
+	seq_num++;
+
+	VERBOSE("[RSS-COMMS] Received reply\n");
+	VERBOSE("protocol_ver=%d\n", reply.protocol_ver);
+	VERBOSE("seq_num=%d\n", reply.seq_num);
+	VERBOSE("client_id=%d\n", reply.client_id);
+	VERBOSE("return_val=%d\n", reply.return_val);
+	VERBOSE("out_size[0]=%d\n", reply.out_size[0]);
+
+	return reply.return_val;
+}
+
+int rss_comms_init(uintptr_t mhu_sender_base, uintptr_t mhu_receiver_base)
+{
+	enum mhu_error_t err;
+
+	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;
+	}
+
+	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;
+	}
+
+	return 0;
+}

diff --git a/drivers/arm/smmu/smmu_v3.c b/drivers/arm/smmu/smmu_v3.c
index a082a81..6c6f978 100644
--- a/drivers/arm/smmu/smmu_v3.c
+++ b/drivers/arm/smmu/smmu_v3.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,11 +9,12 @@
 #include <drivers/arm/smmu_v3.h>
 #include <drivers/delay_timer.h>
 #include <lib/mmio.h>
+#include <arch_features.h>
 
 /* SMMU poll number of retries */
 #define SMMU_POLL_TIMEOUT_US	U(1000)
 
-static int __init smmuv3_poll(uintptr_t smmu_reg, uint32_t mask,
+static int smmuv3_poll(uintptr_t smmu_reg, uint32_t mask,
 				uint32_t value)
 {
 	uint32_t reg_val;
@@ -79,14 +80,74 @@
 	if (smmuv3_security_init(smmu_base) != 0)
 		return -1;
 
-	/* Check if the SMMU supports secure state */
-	if ((mmio_read_32(smmu_base + SMMU_S_IDR1) &
-				SMMU_S_IDR1_SECURE_IMPL) == 0U)
-		return 0;
+#if ENABLE_RME
+
+	if (get_armv9_2_feat_rme_support() != 0U) {
+		if ((mmio_read_32(smmu_base + SMMU_ROOT_IDR0) &
+				  SMMU_ROOT_IDR0_ROOT_IMPL) == 0U) {
+			WARN("Skip SMMU GPC configuration.\n");
+		} else {
+			uint64_t gpccr_el3 = read_gpccr_el3();
+			uint64_t gptbr_el3 = read_gptbr_el3();
+
+			/* SMMU_ROOT_GPT_BASE_CFG[16] is RES0. */
+			gpccr_el3 &= ~(1UL << 16);
+
+			/*
+			 * TODO: SMMU_ROOT_GPT_BASE_CFG is 64b in the spec,
+			 * but SMMU model only accepts 32b access.
+			 */
+			mmio_write_32(smmu_base + SMMU_ROOT_GPT_BASE_CFG,
+				      gpccr_el3);
+
+			/*
+			 * pa_gpt_table_base[51:12] maps to GPTBR_EL3[39:0]
+			 * whereas it maps to SMMU_ROOT_GPT_BASE[51:12]
+			 * hence needs a 12 bit left shit.
+			 */
+			mmio_write_64(smmu_base + SMMU_ROOT_GPT_BASE,
+				      gptbr_el3 << 12);
+
+			/*
+			 * ACCESSEN=1: SMMU- and client-originated accesses are
+			 *             not terminated by this mechanism.
+			 * GPCEN=1: All clients and SMMU-originated accesses,
+			 *          except GPT-walks, are subject to GPC.
+			 */
+			mmio_setbits_32(smmu_base + SMMU_ROOT_CR0,
+					SMMU_ROOT_CR0_GPCEN |
+					SMMU_ROOT_CR0_ACCESSEN);
+
+			/* Poll for ACCESSEN and GPCEN ack bits. */
+			if (smmuv3_poll(smmu_base + SMMU_ROOT_CR0ACK,
+					SMMU_ROOT_CR0_GPCEN |
+					SMMU_ROOT_CR0_ACCESSEN,
+					SMMU_ROOT_CR0_GPCEN |
+					SMMU_ROOT_CR0_ACCESSEN) != 0) {
+				WARN("Failed enabling SMMU GPC.\n");
+
+				/*
+				 * Do not return in error, but fall back to
+				 * invalidating all entries through the secure
+				 * register file.
+				 */
+			}
+		}
+	}
+
+#endif /* ENABLE_RME */
+
 	/*
 	 * Initiate invalidation of secure caches and TLBs if the SMMU
 	 * supports secure state. If not, it's implementation defined
 	 * as to how SMMU_S_INIT register is accessed.
+	 * Arm SMMU Arch RME supplement, section 3.4: all SMMU registers
+	 * specified to be accessible only in secure physical address space are
+	 * additionally accessible in root physical address space in an SMMU
+	 * with RME.
+	 * Section 3.3: as GPT information is permitted to be cached in a TLB,
+	 * the SMMU_S_INIT.INV_ALL mechanism also invalidates GPT information
+	 * cached in TLBs.
 	 */
 	mmio_write_32(smmu_base + SMMU_S_INIT, SMMU_S_INIT_INV_ALL);
 
@@ -94,3 +155,28 @@
 	return smmuv3_poll(smmu_base + SMMU_S_INIT,
 				SMMU_S_INIT_INV_ALL, 0U);
 }
+
+int smmuv3_ns_set_abort_all(uintptr_t smmu_base)
+{
+	/* Attribute update has completed when SMMU_GBPA.Update bit is 0 */
+	if (smmuv3_poll(smmu_base + SMMU_GBPA, SMMU_GBPA_UPDATE, 0U) != 0U) {
+		return -1;
+	}
+
+	/*
+	 * Set GBPA's ABORT bit. Other GBPA fields are presumably ignored then,
+	 * so simply preserve their value.
+	 */
+	mmio_setbits_32(smmu_base + SMMU_GBPA, SMMU_GBPA_UPDATE | SMMU_GBPA_ABORT);
+	if (smmuv3_poll(smmu_base + SMMU_GBPA, SMMU_GBPA_UPDATE, 0U) != 0U) {
+		return -1;
+	}
+
+	/* Disable the SMMU to engage the GBPA fields previously configured. */
+	mmio_clrbits_32(smmu_base + SMMU_CR0, SMMU_CR0_SMMUEN);
+	if (smmuv3_poll(smmu_base + SMMU_CR0ACK, SMMU_CR0_SMMUEN, 0U) != 0U) {
+		return -1;
+	}
+
+	return 0;
+}

diff --git a/drivers/auth/cca/cot.c b/drivers/auth/cca/cot.c
new file mode 100644
index 0000000..d3f3087
--- /dev/null
+++ b/drivers/auth/cca/cot.c

@@ -0,0 +1,675 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stddef.h>
+
+#include <drivers/auth/auth_mod.h>
+#include MBEDTLS_CONFIG_FILE
+#include <tools_share/cca_oid.h>
+
+#include <platform_def.h>
+
+/*
+ * Allocate static buffers to store the authentication parameters extracted from
+ * the certificates.
+ */
+static unsigned char fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char tb_fw_hash_buf[HASH_DER_LEN];
+static unsigned char tb_fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char hw_config_hash_buf[HASH_DER_LEN];
+static unsigned char soc_fw_hash_buf[HASH_DER_LEN];
+static unsigned char soc_fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char rmm_hash_buf[HASH_DER_LEN];
+
+#ifdef IMAGE_BL2
+static unsigned char nt_world_bl_hash_buf[HASH_DER_LEN];
+static unsigned char tos_fw_hash_buf[HASH_DER_LEN];
+static unsigned char tos_fw_config_hash_buf[HASH_DER_LEN];
+static unsigned char nt_fw_config_hash_buf[HASH_DER_LEN];
+#if defined(SPD_spmd)
+static unsigned char sp_pkg_hash_buf[MAX_SP_IDS][HASH_DER_LEN];
+#endif /* SPD_spmd */
+
+static unsigned char core_swd_pk_buf[PK_DER_LEN];
+static unsigned char plat_pk_buf[PK_DER_LEN];
+#endif /* IMAGE_BL2 */
+
+/*
+ * Parameter type descriptors.
+ */
+static auth_param_type_desc_t trusted_nv_ctr = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_NV_CTR, TRUSTED_FW_NVCOUNTER_OID);
+static auth_param_type_desc_t subject_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, 0);
+static auth_param_type_desc_t sig = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_SIG, 0);
+static auth_param_type_desc_t sig_alg = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_SIG_ALG, 0);
+static auth_param_type_desc_t raw_data = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_RAW_DATA, 0);
+
+static auth_param_type_desc_t tb_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_BOOT_FW_HASH_OID);
+static auth_param_type_desc_t tb_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_BOOT_FW_CONFIG_HASH_OID);
+static auth_param_type_desc_t hw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, HW_CONFIG_HASH_OID);
+static auth_param_type_desc_t fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, FW_CONFIG_HASH_OID);
+static auth_param_type_desc_t soc_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SOC_AP_FW_HASH_OID);
+static auth_param_type_desc_t soc_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SOC_FW_CONFIG_HASH_OID);
+static auth_param_type_desc_t rmm_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, RMM_HASH_OID);
+
+#ifdef IMAGE_BL2
+static auth_param_type_desc_t non_trusted_nv_ctr = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_NV_CTR, NON_TRUSTED_FW_NVCOUNTER_OID);
+
+static auth_param_type_desc_t prot_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, PROT_PK_OID);
+static auth_param_type_desc_t swd_rot_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, SWD_ROT_PK_OID);
+static auth_param_type_desc_t core_swd_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, CORE_SWD_PK_OID);
+static auth_param_type_desc_t plat_pk = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_PUB_KEY, PLAT_PK_OID);
+
+static auth_param_type_desc_t tos_fw_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_OS_FW_HASH_OID);
+static auth_param_type_desc_t tos_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, TRUSTED_OS_FW_CONFIG_HASH_OID);
+static auth_param_type_desc_t nt_world_bl_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, NON_TRUSTED_WORLD_BOOTLOADER_HASH_OID);
+static auth_param_type_desc_t nt_fw_config_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, NON_TRUSTED_FW_CONFIG_HASH_OID);
+#if defined(SPD_spmd)
+static auth_param_type_desc_t sp_pkg1_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG1_HASH_OID);
+static auth_param_type_desc_t sp_pkg2_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG2_HASH_OID);
+static auth_param_type_desc_t sp_pkg3_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG3_HASH_OID);
+static auth_param_type_desc_t sp_pkg4_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG4_HASH_OID);
+static auth_param_type_desc_t sp_pkg5_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG5_HASH_OID);
+static auth_param_type_desc_t sp_pkg6_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG6_HASH_OID);
+static auth_param_type_desc_t sp_pkg7_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG7_HASH_OID);
+static auth_param_type_desc_t sp_pkg8_hash = AUTH_PARAM_TYPE_DESC(
+		AUTH_PARAM_HASH, SP_PKG8_HASH_OID);
+#endif /* SPD_spmd */
+#endif /* IMAGE_BL2 */
+
+/* CCA Content Certificate */
+static const auth_img_desc_t cca_content_cert = {
+	.img_id = CCA_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = NULL,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &subject_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &tb_fw_hash,
+			.data = {
+				.ptr = (void *)tb_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &tb_fw_config_hash,
+			.data = {
+				.ptr = (void *)tb_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &fw_config_hash,
+			.data = {
+				.ptr = (void *)fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[3] = {
+			.type_desc = &hw_config_hash,
+			.data = {
+				.ptr = (void *)hw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[4] = {
+			.type_desc = &soc_fw_hash,
+			.data = {
+				.ptr = (void *)soc_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[5] = {
+			.type_desc = &soc_fw_config_hash,
+			.data = {
+				.ptr = (void *)soc_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[6] = {
+			.type_desc = &rmm_hash,
+			.data = {
+				.ptr = (void *)rmm_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+#ifdef IMAGE_BL1
+static const auth_img_desc_t bl2_image = {
+	.img_id = BL2_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &cca_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tb_fw_hash
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t tb_fw_config = {
+	.img_id = TB_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &cca_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tb_fw_config_hash
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t fw_config = {
+	.img_id = FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &cca_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &fw_config_hash
+			}
+		}
+	}
+};
+#endif /* IMAGE_BL1 */
+
+#ifdef IMAGE_BL2
+/* HW Config */
+static const auth_img_desc_t hw_config = {
+	.img_id = HW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &cca_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &hw_config_hash
+			}
+		}
+	}
+};
+
+/* BL31 */
+static const auth_img_desc_t bl31_image = {
+	.img_id = BL31_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &cca_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &soc_fw_hash
+			}
+		}
+	}
+};
+
+/* BL31 Config */
+static const auth_img_desc_t soc_fw_config = {
+	.img_id = SOC_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &cca_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &soc_fw_config_hash
+			}
+		}
+	}
+};
+
+/* RMM */
+static const auth_img_desc_t rmm_image = {
+	.img_id = RMM_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &cca_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &rmm_hash
+			}
+		}
+	}
+};
+
+/* Core SWD Key Certificate */
+static const auth_img_desc_t core_swd_key_cert = {
+	.img_id = CORE_SWD_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = NULL, /* SWD ROOT CERT */
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &swd_rot_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &core_swd_pk,
+			.data = {
+				.ptr = (void *)core_swd_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+
+/* SPMC Content Certificate */
+static const auth_img_desc_t trusted_os_fw_content_cert = {
+	.img_id = TRUSTED_OS_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &core_swd_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &core_swd_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &tos_fw_hash,
+			.data = {
+				.ptr = (void *)tos_fw_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &tos_fw_config_hash,
+			.data = {
+				.ptr = (void *)tos_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+/* SPMC */
+static const auth_img_desc_t bl32_image = {
+	.img_id = BL32_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_os_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tos_fw_hash
+			}
+		}
+	}
+};
+
+/* SPM Config */
+static const auth_img_desc_t tos_fw_config = {
+	.img_id = TOS_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &trusted_os_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &tos_fw_config_hash
+			}
+		}
+	}
+};
+
+/* Platform Key Certificate */
+static const auth_img_desc_t plat_key_cert = {
+	.img_id = PLAT_KEY_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = NULL, /* PLATFORM ROOT CERT */
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &prot_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &non_trusted_nv_ctr,
+				.plat_nv_ctr = &non_trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &plat_pk,
+			.data = {
+				.ptr = (void *)plat_pk_buf,
+				.len = (unsigned int)PK_DER_LEN
+			}
+		}
+	}
+};
+
+/* Non-Trusted Firmware */
+static const auth_img_desc_t non_trusted_fw_content_cert = {
+	.img_id = NON_TRUSTED_FW_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &plat_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &plat_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &non_trusted_nv_ctr,
+				.plat_nv_ctr = &non_trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &nt_world_bl_hash,
+			.data = {
+				.ptr = (void *)nt_world_bl_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &nt_fw_config_hash,
+			.data = {
+				.ptr = (void *)nt_fw_config_hash_buf,
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+static const auth_img_desc_t bl33_image = {
+	.img_id = BL33_IMAGE_ID,
+	.img_type = IMG_RAW,
+	.parent = &non_trusted_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &nt_world_bl_hash
+			}
+		}
+	}
+};
+
+/* NT FW Config */
+static const auth_img_desc_t nt_fw_config = {
+	.img_id = NT_FW_CONFIG_ID,
+	.img_type = IMG_RAW,
+	.parent = &non_trusted_fw_content_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_HASH,
+			.param.hash = {
+				.data = &raw_data,
+				.hash = &nt_fw_config_hash
+			}
+		}
+	}
+};
+
+/*
+ * Secure Partitions
+ */
+#if defined(SPD_spmd)
+static const auth_img_desc_t sip_sp_content_cert = {
+	.img_id = SIP_SP_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &core_swd_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &core_swd_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &trusted_nv_ctr,
+				.plat_nv_ctr = &trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &sp_pkg1_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[0],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &sp_pkg2_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[1],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &sp_pkg3_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[2],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[3] = {
+			.type_desc = &sp_pkg4_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[3],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+DEFINE_SIP_SP_PKG(1);
+DEFINE_SIP_SP_PKG(2);
+DEFINE_SIP_SP_PKG(3);
+DEFINE_SIP_SP_PKG(4);
+
+static const auth_img_desc_t plat_sp_content_cert = {
+	.img_id = PLAT_SP_CONTENT_CERT_ID,
+	.img_type = IMG_CERT,
+	.parent = &plat_key_cert,
+	.img_auth_methods = (const auth_method_desc_t[AUTH_METHOD_NUM]) {
+		[0] = {
+			.type = AUTH_METHOD_SIG,
+			.param.sig = {
+				.pk = &plat_pk,
+				.sig = &sig,
+				.alg = &sig_alg,
+				.data = &raw_data
+			}
+		},
+		[1] = {
+			.type = AUTH_METHOD_NV_CTR,
+			.param.nv_ctr = {
+				.cert_nv_ctr = &non_trusted_nv_ctr,
+				.plat_nv_ctr = &non_trusted_nv_ctr
+			}
+		}
+	},
+	.authenticated_data = (const auth_param_desc_t[COT_MAX_VERIFIED_PARAMS]) {
+		[0] = {
+			.type_desc = &sp_pkg5_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[4],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[1] = {
+			.type_desc = &sp_pkg6_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[5],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[2] = {
+			.type_desc = &sp_pkg7_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[6],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		},
+		[3] = {
+			.type_desc = &sp_pkg8_hash,
+			.data = {
+				.ptr = (void *)sp_pkg_hash_buf[7],
+				.len = (unsigned int)HASH_DER_LEN
+			}
+		}
+	}
+};
+
+DEFINE_PLAT_SP_PKG(5);
+DEFINE_PLAT_SP_PKG(6);
+DEFINE_PLAT_SP_PKG(7);
+DEFINE_PLAT_SP_PKG(8);
+#endif /* SPD_spmd */
+#endif /* IMAGE_BL2 */
+/*
+ * Chain of trust definition
+ */
+#ifdef IMAGE_BL1
+static const auth_img_desc_t * const cot_desc[] = {
+	[CCA_CONTENT_CERT_ID]			=	&cca_content_cert,
+	[BL2_IMAGE_ID]				=	&bl2_image,
+	[TB_FW_CONFIG_ID]			=	&tb_fw_config,
+	[FW_CONFIG_ID]				=	&fw_config,
+};
+#else /* IMAGE_BL2 */
+static const auth_img_desc_t * const cot_desc[] = {
+	[CCA_CONTENT_CERT_ID]			=	&cca_content_cert,
+	[HW_CONFIG_ID]				=	&hw_config,
+	[BL31_IMAGE_ID]				=	&bl31_image,
+	[SOC_FW_CONFIG_ID]			=	&soc_fw_config,
+	[RMM_IMAGE_ID]				=	&rmm_image,
+	[CORE_SWD_KEY_CERT_ID]			=	&core_swd_key_cert,
+	[TRUSTED_OS_FW_CONTENT_CERT_ID]		=	&trusted_os_fw_content_cert,
+	[BL32_IMAGE_ID]				=	&bl32_image,
+	[TOS_FW_CONFIG_ID]			=	&tos_fw_config,
+	[PLAT_KEY_CERT_ID]			=	&plat_key_cert,
+	[NON_TRUSTED_FW_CONTENT_CERT_ID]	=	&non_trusted_fw_content_cert,
+	[BL33_IMAGE_ID]				=	&bl33_image,
+	[NT_FW_CONFIG_ID]			=	&nt_fw_config,
+#if defined(SPD_spmd)
+	[SIP_SP_CONTENT_CERT_ID]		=	&sip_sp_content_cert,
+	[PLAT_SP_CONTENT_CERT_ID]		=	&plat_sp_content_cert,
+	[SP_PKG1_ID]				=	&sp_pkg1,
+	[SP_PKG2_ID]				=	&sp_pkg2,
+	[SP_PKG3_ID]				=	&sp_pkg3,
+	[SP_PKG4_ID]				=	&sp_pkg4,
+	[SP_PKG5_ID]				=	&sp_pkg5,
+	[SP_PKG6_ID]				=	&sp_pkg6,
+	[SP_PKG7_ID]				=	&sp_pkg7,
+	[SP_PKG8_ID]				=       &sp_pkg8,
+#endif
+};
+#endif /* IMAGE_BL1 */
+
+/* Register the CoT in the authentication module */
+REGISTER_COT(cot_desc);

diff --git a/drivers/auth/mbedtls/mbedtls_common.mk b/drivers/auth/mbedtls/mbedtls_common.mk
index 3eb4161..16ce65f 100644
--- a/drivers/auth/mbedtls/mbedtls_common.mk
+++ b/drivers/auth/mbedtls/mbedtls_common.mk

@@ -97,18 +97,6 @@
     TF_MBEDTLS_USE_AES_GCM	:=	0
 endif
 
-ifeq ($(MEASURED_BOOT),1)
-    ifeq (${TPM_HASH_ALG}, sha256)
-        TF_MBEDTLS_TPM_HASH_ALG_ID	:=	TF_MBEDTLS_SHA256
-    else ifeq (${TPM_HASH_ALG}, sha384)
-        TF_MBEDTLS_TPM_HASH_ALG_ID	:=	TF_MBEDTLS_SHA384
-    else ifeq (${TPM_HASH_ALG}, sha512)
-        TF_MBEDTLS_TPM_HASH_ALG_ID	:=	TF_MBEDTLS_SHA512
-    else
-        $(error "TPM_HASH_ALG not defined.")
-    endif
-endif
-
 # Needs to be set to drive mbed TLS configuration correctly
 $(eval $(call add_defines,\
     $(sort \
@@ -118,10 +106,6 @@
         TF_MBEDTLS_USE_AES_GCM \
 )))
 
-ifeq ($(MEASURED_BOOT),1)
-  $(eval $(call add_define,TF_MBEDTLS_TPM_HASH_ALG_ID))
-endif
-
 $(eval $(call MAKE_LIB,mbedtls))
 
 endif

diff --git a/drivers/measured_boot/event_log/event_log.c b/drivers/measured_boot/event_log/event_log.c
index 792f235..abe469b 100644
--- a/drivers/measured_boot/event_log/event_log.c
+++ b/drivers/measured_boot/event_log/event_log.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -180,6 +180,8 @@
 
 	/* event_log_init() must have been called prior to this. */
 	assert(log_ptr != NULL);
+	assert(((uintptr_t)log_ptr + ID_EVENT_SIZE + LOC_EVENT_SIZE) <
+		log_end);
 
 	/*
 	 * Add Specification ID Event first
@@ -219,7 +221,7 @@
 	((tpmt_ha *)ptr)->algorithm_id = TPM_ALG_ID;
 
 	/* TCG_PCR_EVENT2.Digests[].Digest[] */
-	(void)memset(&((tpmt_ha *)ptr)->digest, 0, TPM_ALG_ID);
+	(void)memset(&((tpmt_ha *)ptr)->digest, 0, TCG_DIGEST_SIZE);
 	ptr = (uint8_t *)((uintptr_t)ptr +
 			offsetof(tpmt_ha, digest) + TCG_DIGEST_SIZE);
 

diff --git a/drivers/measured_boot/event_log/event_log.mk b/drivers/measured_boot/event_log/event_log.mk
index 1ff4aa8..5ea4c55 100644
--- a/drivers/measured_boot/event_log/event_log.mk
+++ b/drivers/measured_boot/event_log/event_log.mk

@@ -7,20 +7,25 @@
 # Default log level to dump the event log (LOG_LEVEL_INFO)
 EVENT_LOG_LEVEL         ?= 40
 
-# TPM hash algorithm.
+# Measured Boot hash algorithm.
 # SHA-256 (or stronger) is required for all devices that are TPM 2.0 compliant.
-TPM_HASH_ALG			:=	sha256
+ifdef TPM_HASH_ALG
+    $(warning "TPM_HASH_ALG is deprecated. Please use MBOOT_EL_HASH_ALG instead.")
+    MBOOT_EL_HASH_ALG		:=	${TPM_HASH_ALG}
+else
+    MBOOT_EL_HASH_ALG		:=	sha256
+endif
 
-ifeq (${TPM_HASH_ALG}, sha512)
+ifeq (${MBOOT_EL_HASH_ALG}, sha512)
     TPM_ALG_ID			:=	TPM_ALG_SHA512
     TCG_DIGEST_SIZE		:=	64U
-else ifeq (${TPM_HASH_ALG}, sha384)
+else ifeq (${MBOOT_EL_HASH_ALG}, sha384)
     TPM_ALG_ID			:=	TPM_ALG_SHA384
     TCG_DIGEST_SIZE		:=	48U
 else
     TPM_ALG_ID			:=	TPM_ALG_SHA256
     TCG_DIGEST_SIZE		:=	32U
-endif #TPM_HASH_ALG
+endif #MBOOT_EL_HASH_ALG
 
 # Set definitions for Measured Boot driver.
 $(eval $(call add_defines,\

diff --git a/drivers/measured_boot/rss/rss_measured_boot.c b/drivers/measured_boot/rss/rss_measured_boot.c
new file mode 100644
index 0000000..fe2baf0
--- /dev/null
+++ b/drivers/measured_boot/rss/rss_measured_boot.c

@@ -0,0 +1,125 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <assert.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <drivers/auth/crypto_mod.h>
+#include <drivers/measured_boot/rss/rss_measured_boot.h>
+#include <lib/psa/measured_boot.h>
+#include <psa/crypto_types.h>
+#include <psa/crypto_values.h>
+#include <psa/error.h>
+
+#define MBOOT_ALG_SHA512 0
+#define MBOOT_ALG_SHA384 1
+#define MBOOT_ALG_SHA256 2
+
+#if MBOOT_ALG_ID == MBOOT_ALG_SHA512
+#define	CRYPTO_MD_ID		CRYPTO_MD_SHA512
+#define PSA_CRYPTO_MD_ID	PSA_ALG_SHA_512
+#elif MBOOT_ALG_ID == MBOOT_ALG_SHA384
+#define	CRYPTO_MD_ID		CRYPTO_MD_SHA384
+#define PSA_CRYPTO_MD_ID	PSA_ALG_SHA_384
+#elif MBOOT_ALG_ID == MBOOT_ALG_SHA256
+#define	CRYPTO_MD_ID		CRYPTO_MD_SHA256
+#define PSA_CRYPTO_MD_ID	PSA_ALG_SHA_256
+#else
+#  error Invalid Measured Boot algorithm.
+#endif /* MBOOT_ALG_ID */
+
+/* Pointer to struct rss_mboot_metadata */
+static struct rss_mboot_metadata *plat_metadata_ptr;
+
+/* Functions' declarations */
+void rss_measured_boot_init(void)
+{
+	/* At this point it is expected that communication channel over MHU
+	 * is already initialised by platform init.
+	 */
+
+	/* Get pointer to platform's struct rss_mboot_metadata structure */
+	plat_metadata_ptr = plat_rss_mboot_get_metadata();
+	assert(plat_metadata_ptr != NULL);
+}
+
+int rss_mboot_measure_and_record(uintptr_t data_base, uint32_t data_size,
+				 uint32_t data_id)
+{
+	unsigned char hash_data[CRYPTO_MD_MAX_SIZE];
+	int rc;
+	psa_status_t ret;
+	const struct rss_mboot_metadata *metadata_ptr = plat_metadata_ptr;
+
+	/* Get the metadata associated with this image. */
+	while ((metadata_ptr->id != RSS_MBOOT_INVALID_ID) &&
+		(metadata_ptr->id != data_id)) {
+		metadata_ptr++;
+	}
+
+	/* If image is not present in metadata array then skip */
+	if (metadata_ptr->id == RSS_MBOOT_INVALID_ID) {
+		return 0;
+	}
+
+	/* Calculate hash */
+	rc = crypto_mod_calc_hash(CRYPTO_MD_ID,
+				  (void *)data_base, data_size, hash_data);
+	if (rc != 0) {
+		return rc;
+	}
+
+	ret = rss_measured_boot_extend_measurement(
+						metadata_ptr->slot,
+						metadata_ptr->signer_id,
+						metadata_ptr->signer_id_size,
+						metadata_ptr->version,
+						metadata_ptr->version_size,
+						PSA_CRYPTO_MD_ID,
+						metadata_ptr->sw_type,
+						metadata_ptr->sw_type_size,
+						hash_data,
+						MBOOT_DIGEST_SIZE,
+						metadata_ptr->lock_measurement);
+	if (ret != PSA_SUCCESS) {
+		return ret;
+	}
+
+	return 0;
+}
+
+int rss_mboot_set_signer_id(unsigned int img_id,
+			    const void *pk_ptr,
+			    size_t pk_len)
+{
+	unsigned char hash_data[CRYPTO_MD_MAX_SIZE];
+	struct rss_mboot_metadata *metadata_ptr = plat_metadata_ptr;
+	int rc;
+
+	/* Get the metadata associated with this image. */
+	while ((metadata_ptr->id != RSS_MBOOT_INVALID_ID) &&
+		(metadata_ptr->id != img_id)) {
+		metadata_ptr++;
+	}
+
+	/* If image is not present in metadata array then skip */
+	if (metadata_ptr->id == RSS_MBOOT_INVALID_ID) {
+		return 0;
+	}
+
+	/* Calculate public key hash */
+	rc = crypto_mod_calc_hash(CRYPTO_MD_ID, (void *)pk_ptr,
+				  pk_len, hash_data);
+	if (rc != 0) {
+		return rc;
+	}
+
+	/* Update metadata struct with the received signer_id */
+	(void)memcpy(metadata_ptr->signer_id, hash_data, MBOOT_DIGEST_SIZE);
+	metadata_ptr->signer_id_size = MBOOT_DIGEST_SIZE;
+
+	return 0;
+}

diff --git a/drivers/measured_boot/rss/rss_measured_boot.mk b/drivers/measured_boot/rss/rss_measured_boot.mk
new file mode 100644
index 0000000..18ee836
--- /dev/null
+++ b/drivers/measured_boot/rss/rss_measured_boot.mk

@@ -0,0 +1,32 @@
+#
+# Copyright (c) 2022, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# Hash algorithm for measured boot
+# SHA-256 (or stronger) is required.
+MBOOT_RSS_HASH_ALG		:=	sha256
+
+ifeq (${MBOOT_RSS_HASH_ALG}, sha512)
+    MBOOT_ALG_ID		:=	MBOOT_ALG_SHA512
+    MBOOT_DIGEST_SIZE		:=	64U
+else ifeq (${MBOOT_RSS_HASH_ALG}, sha384)
+    MBOOT_ALG_ID		:=	MBOOT_ALG_SHA384
+    MBOOT_DIGEST_SIZE		:=	48U
+else
+    MBOOT_ALG_ID		:=	MBOOT_ALG_SHA256
+    MBOOT_DIGEST_SIZE		:=	32U
+endif #MBOOT_RSS_HASH_ALG
+
+# Set definitions for Measured Boot driver.
+$(eval $(call add_defines,\
+    $(sort \
+        MBOOT_ALG_ID \
+        MBOOT_DIGEST_SIZE \
+        MBOOT_RSS_BACKEND \
+)))
+
+MEASURED_BOOT_SRC_DIR	:= drivers/measured_boot/rss/
+
+MEASURED_BOOT_SOURCES	+= ${MEASURED_BOOT_SRC_DIR}rss_measured_boot.c

diff --git a/drivers/mmc/mmc.c b/drivers/mmc/mmc.c
index c327e71..116afda 100644
--- a/drivers/mmc/mmc.c
+++ b/drivers/mmc/mmc.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -694,52 +694,6 @@
 	return size;
 }
 
-static inline void mmc_rpmb_enable(void)
-{
-	mmc_set_ext_csd(CMD_EXTCSD_PARTITION_CONFIG,
-			PART_CFG_BOOT_PARTITION1_ENABLE |
-			PART_CFG_BOOT_PARTITION1_ACCESS);
-}
-
-static inline void mmc_rpmb_disable(void)
-{
-	mmc_set_ext_csd(CMD_EXTCSD_PARTITION_CONFIG,
-			PART_CFG_BOOT_PARTITION1_ENABLE);
-}
-
-size_t mmc_rpmb_read_blocks(int lba, uintptr_t buf, size_t size)
-{
-	size_t size_read;
-
-	mmc_rpmb_enable();
-	size_read = mmc_read_blocks(lba, buf, size);
-	mmc_rpmb_disable();
-
-	return size_read;
-}
-
-size_t mmc_rpmb_write_blocks(int lba, const uintptr_t buf, size_t size)
-{
-	size_t size_written;
-
-	mmc_rpmb_enable();
-	size_written = mmc_write_blocks(lba, buf, size);
-	mmc_rpmb_disable();
-
-	return size_written;
-}
-
-size_t mmc_rpmb_erase_blocks(int lba, size_t size)
-{
-	size_t size_erased;
-
-	mmc_rpmb_enable();
-	size_erased = mmc_erase_blocks(lba, size);
-	mmc_rpmb_disable();
-
-	return size_erased;
-}
-
 static int mmc_part_switch(unsigned int part_type)
 {
 	uint8_t part_config = mmc_ext_csd[CMD_EXTCSD_PARTITION_CONFIG];
@@ -755,29 +709,51 @@
 	return PART_CFG_CURRENT_BOOT_PARTITION(mmc_ext_csd[CMD_EXTCSD_PARTITION_CONFIG]);
 }
 
-size_t mmc_boot_part_read_blocks(int lba, uintptr_t buf, size_t size)
+int mmc_part_switch_current_boot(void)
 {
-	size_t size_read;
-	int ret;
 	unsigned char current_boot_part = mmc_current_boot_part();
+	int ret;
 
 	if (current_boot_part != 1U &&
 	    current_boot_part != 2U) {
 		ERROR("Got unexpected value for active boot partition, %u\n", current_boot_part);
-		return 0;
+		return -EIO;
 	}
 
 	ret = mmc_part_switch(current_boot_part);
 	if (ret < 0) {
 		ERROR("Failed to switch to boot partition, %d\n", ret);
+	}
+
+	return ret;
+}
+
+int mmc_part_switch_user(void)
+{
+	int ret;
+
+	ret = mmc_part_switch(PART_CFG_BOOT_PARTITION_NO_ACCESS);
+	if (ret < 0) {
+		ERROR("Failed to switch to user partition, %d\n", ret);
+	}
+
+	return ret;
+}
+
+size_t mmc_boot_part_read_blocks(int lba, uintptr_t buf, size_t size)
+{
+	size_t size_read;
+	int ret;
+
+	ret = mmc_part_switch_current_boot();
+	if (ret < 0) {
 		return 0;
 	}
 
 	size_read = mmc_read_blocks(lba, buf, size);
 
-	ret = mmc_part_switch(0);
+	ret = mmc_part_switch_user();
 	if (ret < 0) {
-		ERROR("Failed to switch back to user partition, %d\n", ret);
 		return 0;
 	}
 

diff --git a/drivers/nxp/ddr/phy-gen2/phy.c b/drivers/nxp/ddr/phy-gen2/phy.c
index c8245a8..9e52145 100644
--- a/drivers/nxp/ddr/phy-gen2/phy.c
+++ b/drivers/nxp/ddr/phy-gen2/phy.c

@@ -2216,14 +2216,6 @@
 
 	size = PHY_GEN2_MAX_IMAGE_SIZE;
 	image_buf = (uintptr_t)phy_gen2_fw_img_buf;
-	ret = mmap_add_dynamic_region(phy_gen2_fw_img_buf,
-			phy_gen2_fw_img_buf,
-			PHY_GEN2_MAX_IMAGE_SIZE,
-			MT_MEMORY | MT_RW | MT_SECURE);
-	if (ret != 0) {
-		ERROR("Failed to add dynamic memory region.\n");
-		return ret;
-	}
 	ret = img_loadr(imem_id, &image_buf, &size);
 	if (ret != 0) {
 		ERROR("Failed to load %d firmware.\n", imem_id);
@@ -2592,6 +2584,15 @@
 		}
 	} else {
 #endif
+		/* Mapping IMG buffer firstly */
+		ret = mmap_add_dynamic_region(priv->phy_gen2_fw_img_buf,
+			priv->phy_gen2_fw_img_buf,
+			PHY_GEN2_MAX_IMAGE_SIZE,
+			MT_MEMORY | MT_RW | MT_SECURE);
+		if (ret != 0) {
+			ERROR("Failed to add dynamic memory region.\n");
+			return ret;
+		}
 
 		debug("Load 1D firmware\n");
 		ret = load_fw(priv->phy, &input, 0, &msg_1d,

diff --git a/drivers/partition/partition.c b/drivers/partition/partition.c
index 7706f88..c84816f 100644
--- a/drivers/partition/partition.c
+++ b/drivers/partition/partition.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,6 +10,7 @@
 #include <string.h>
 
 #include <common/debug.h>
+#include <common/tf_crc32.h>
 #include <drivers/io/io_storage.h>
 #include <drivers/partition/efi.h>
 #include <drivers/partition/partition.h>
@@ -76,7 +77,7 @@
 }
 
 /*
- * Load GPT header and check the GPT signature.
+ * Load GPT header and check the GPT signature and header CRC.
  * If partition numbers could be found, check & update it.
  */
 static int load_gpt_header(uintptr_t image_handle)
@@ -84,6 +85,7 @@
 	gpt_header_t header;
 	size_t bytes_read;
 	int result;
+	uint32_t header_crc, calc_crc;
 
 	result = io_seek(image_handle, IO_SEEK_SET, GPT_HEADER_OFFSET);
 	if (result != 0) {
@@ -99,6 +101,23 @@
 		return -EINVAL;
 	}
 
+	/*
+	 * UEFI Spec 2.8 March 2019 Page 119: HeaderCRC32 value is
+	 * computed by setting this field to 0, and computing the
+	 * 32-bit CRC for HeaderSize bytes.
+	 */
+	header_crc = header.header_crc;
+	header.header_crc = 0U;
+
+	calc_crc = tf_crc32(0U, (uint8_t *)&header, DEFAULT_GPT_HEADER_SIZE);
+	if (header_crc != calc_crc) {
+		ERROR("Invalid GPT Header CRC: Expected 0x%x but got 0x%x.\n",
+		      header_crc, calc_crc);
+		return -EINVAL;
+	}
+
+	header.header_crc = header_crc;
+
 	/* partition numbers can't exceed PLAT_PARTITION_MAX_ENTRIES */
 	list.entry_count = header.list_num;
 	if (list.entry_count > PLAT_PARTITION_MAX_ENTRIES) {

diff --git a/drivers/scmi-msg/base.c b/drivers/scmi-msg/base.c
index 2d72034..2db4d7e 100644
--- a/drivers/scmi-msg/base.c
+++ b/drivers/scmi-msg/base.c

@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: BSD-3-Clause
 /*
  * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
- * Copyright (c) 2019-2020, Linaro Limited
+ * Copyright (c) 2019-2022, Linaro Limited
  */
 #include <assert.h>
 #include <string.h>
@@ -131,15 +131,12 @@
 	return count;
 }
 
-#define MAX_PROTOCOL_IN_LIST		8U
-
 static void discover_list_protocols(struct scmi_msg *msg)
 {
 	const struct scmi_base_discover_list_protocols_a2p *a2p = NULL;
 	struct scmi_base_discover_list_protocols_p2a p2a = {
 		.status = SCMI_SUCCESS,
 	};
-	uint8_t outargs[sizeof(p2a) + MAX_PROTOCOL_IN_LIST] = { 0U };
 	const uint8_t *list = NULL;
 	unsigned int count = 0U;
 
@@ -148,24 +145,22 @@
 		return;
 	}
 
-	assert(msg->out_size > sizeof(outargs));
-
 	a2p = (void *)msg->in;
 
 	list = plat_scmi_protocol_list(msg->agent_id);
 	count = count_protocols_in_list(list);
+
 	if (count > a2p->skip) {
-		count = MIN(count - a2p->skip, MAX_PROTOCOL_IN_LIST);
+		count = MIN(count - a2p->skip, msg->out_size - sizeof(p2a));
 	} else {
 		count = 0U;
 	}
 
 	p2a.num_protocols = count;
 
-	memcpy(outargs, &p2a, sizeof(p2a));
-	memcpy(outargs + sizeof(p2a), list + a2p->skip, count);
-
-	scmi_write_response(msg, outargs, sizeof(outargs));
+	memcpy(msg->out, &p2a, sizeof(p2a));
+	memcpy(msg->out + sizeof(p2a), list + a2p->skip, count);
+	msg->out_size_out = sizeof(p2a) + round_up(count, sizeof(uint32_t));
 }
 
 static const scmi_msg_handler_t scmi_base_handler_table[] = {

diff --git a/drivers/st/clk/clk-stm32-core.c b/drivers/st/clk/clk-stm32-core.c
index 355c9da..8584a52 100644
--- a/drivers/st/clk/clk-stm32-core.c
+++ b/drivers/st/clk/clk-stm32-core.c

@@ -143,7 +143,7 @@
 {
 	struct clk_oscillator_data *osc_data = clk_oscillator_get_data(priv, id);
 
-	return _clk_stm32_gate_wait_ready(priv, osc_data->gate_id, ready_on);
+	return _clk_stm32_gate_wait_ready(priv, osc_data->gate_rdy_id, ready_on);
 }
 
 int clk_oscillator_wait_ready_on(struct stm32_clk_priv *priv, int id)
@@ -327,6 +327,9 @@
 	}
 
 	old_parent = _clk_stm32_get_parent(priv, clk);
+	if (old_parent < 0) {
+		return old_parent;
+	}
 	if (old_parent == clkp) {
 		return 0;
 	}
@@ -415,7 +418,7 @@
 		sel = clk_mux_get_parent(priv, mux_id);
 	}
 
-	if (sel < parent->num_parents) {
+	if ((sel >= 0) && (sel < parent->num_parents)) {
 		return parent->id_parents[sel];
 	}
 
@@ -488,6 +491,9 @@
 	}
 
 	parent = _clk_stm32_get_parent(priv, id);
+	if (parent < 0) {
+		return 0UL;
+	}
 
 	if (clk->ops->recalc_rate != NULL) {
 		unsigned long prate = 0UL;
@@ -517,6 +523,10 @@
 {
 	int parent_id = _clk_stm32_get_parent(priv, id);
 
+	if (parent_id < 0) {
+		return 0UL;
+	}
+
 	return _clk_stm32_get_rate(priv, parent_id);
 }
 
@@ -552,6 +562,9 @@
 
 	if (priv->gate_refcounts[id] == 0U) {
 		parent = _clk_stm32_get_parent(priv, id);
+		if (parent < 0) {
+			return parent;
+		}
 		if (parent != CLK_IS_ROOT) {
 			ret = _clk_stm32_enable_core(priv, parent);
 			if (ret) {
@@ -616,7 +629,7 @@
 	clk_stm32_disable_call_ops(priv, id);
 
 	parent = _clk_stm32_get_parent(priv, id);
-	if (parent != CLK_IS_ROOT) {
+	if ((parent >= 0) && (parent != CLK_IS_ROOT)) {
 		_clk_stm32_disable_core(priv, parent);
 	}
 }
@@ -825,8 +838,9 @@
 		}
 	}
 
-	if ((mmio_read_32(address) & mask_rdy) != mask_test)
+	if ((mmio_read_32(address) & mask_rdy) != mask_test) {
 		return -ETIMEDOUT;
+	}
 
 	return 0;
 }
@@ -1060,12 +1074,15 @@
 	uint32_t i;
 
 	cell = fdt_getprop(fdt, node, name, &len);
-	if (cell != NULL) {
-		for (i = 0; i < ((uint32_t)len / sizeof(uint32_t)); i++) {
-			uint32_t val = fdt32_to_cpu(cell[i]);
+	if (cell == NULL) {
+		*nb = 0U;
+		return 0;
+	}
 
-			tab[i] = val;
-		}
+	for (i = 0; i < ((uint32_t)len / sizeof(uint32_t)); i++) {
+		uint32_t val = fdt32_to_cpu(cell[i]);
+
+		tab[i] = val;
 	}
 
 	*nb = (uint32_t)len / sizeof(uint32_t);

diff --git a/drivers/st/spi/stm32_qspi.c b/drivers/st/spi/stm32_qspi.c
index d3c26d9..73aa9ac 100644
--- a/drivers/st/spi/stm32_qspi.c
+++ b/drivers/st/spi/stm32_qspi.c

@@ -1,13 +1,10 @@
 /*
- * Copyright (c) 2019-2021, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2019-2022, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
  */
 
 #include <inttypes.h>
-#include <libfdt.h>
-
-#include <platform_def.h>
 
 #include <common/debug.h>
 #include <common/fdt_wrappers.h>
@@ -19,6 +16,9 @@
 #include <drivers/st/stm32mp_reset.h>
 #include <lib/mmio.h>
 #include <lib/utils_def.h>
+#include <libfdt.h>
+
+#include <platform_def.h>
 
 /* Timeout for device interface reset */
 #define TIMEOUT_US_1_MS			1000U
@@ -139,10 +139,6 @@
 	int ret = 0;
 	uint64_t timeout;
 
-	if (op->data.nbytes == 0U) {
-		return stm32_qspi_wait_for_not_busy();
-	}
-
 	timeout = timeout_init_us(QSPI_CMD_TIMEOUT_US);
 	while ((mmio_read_32(qspi_base() + QSPI_SR) & QSPI_SR_TCF) == 0U) {
 		if (timeout_elapsed(timeout)) {
@@ -163,6 +159,10 @@
 	/* Clear flags */
 	mmio_write_32(qspi_base() + QSPI_FCR, QSPI_FCR_CTCF | QSPI_FCR_CTEF);
 
+	if (ret == 0) {
+		ret = stm32_qspi_wait_for_not_busy();
+	}
+
 	return ret;
 }
 
@@ -251,11 +251,6 @@
 		op->dummy.buswidth, op->data.buswidth,
 		op->addr.val, op->data.nbytes);
 
-	ret = stm32_qspi_wait_for_not_busy();
-	if (ret != 0) {
-		return ret;
-	}
-
 	addr_max = op->addr.val + op->data.nbytes + 1U;
 
 	if ((op->data.dir == SPI_MEM_DATA_IN) && (op->data.nbytes != 0U)) {

diff --git a/drivers/ufs/ufs.c b/drivers/ufs/ufs.c
index 15d80ae..7db6c0b 100644
--- a/drivers/ufs/ufs.c
+++ b/drivers/ufs/ufs.c

@@ -146,11 +146,43 @@
 	return 0;
 }
 
+static int ufshc_hce_disable(uintptr_t base)
+{
+	unsigned int data;
+	int timeout;
+
+	/* Disable Host Controller */
+	mmio_write_32(base + HCE, HCE_DISABLE);
+	timeout = HCE_DISABLE_TIMEOUT_US;
+	do {
+		data = mmio_read_32(base + HCE);
+		if ((data & HCE_ENABLE) == HCE_DISABLE) {
+			break;
+		}
+		udelay(1);
+	} while (--timeout > 0);
+
+	if (timeout <= 0) {
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+
 static int ufshc_reset(uintptr_t base)
 {
 	unsigned int data;
 	int retries, result;
 
+	/* disable controller if enabled */
+	if (mmio_read_32(base + HCE) & HCE_ENABLE) {
+		result = ufshc_hce_disable(base);
+		if (result != 0) {
+			return -EIO;
+		}
+	}
+
 	for (retries = 0; retries < HCE_ENABLE_OUTER_RETRIES; ++retries) {
 		result = ufshc_hce_enable(base);
 		if (result == 0) {
@@ -408,7 +440,7 @@
 		break;
 	case QUERY_WRITE_ATTR:
 		query_upiu->query_func = QUERY_FUNC_STD_WRITE;
-		memcpy((void *)&query_upiu->ts.attr.value, (void *)buf, length);
+		query_upiu->ts.attr.value = htobe32(*((uint32_t *)buf));
 		break;
 	default:
 		assert(0);
@@ -594,12 +626,14 @@
 	case QUERY_READ_FLAG:
 		*(uint32_t *)buf = (uint32_t)resp->ts.flag.value;
 		break;
-	case QUERY_READ_ATTR:
 	case QUERY_READ_DESC:
 		memcpy((void *)buf,
 		       (void *)(utrd.resp_upiu + sizeof(query_resp_upiu_t)),
 		       size);
 		break;
+	case QUERY_READ_ATTR:
+		*(uint32_t *)buf = htobe32(resp->ts.attr.value);
+		break;
 	default:
 		/* Do nothing in default case */
 		break;
@@ -733,16 +767,41 @@
 	return size - resp->res_trans_cnt;
 }
 
+static int ufs_set_fdevice_init(void)
+{
+	unsigned int result;
+	int timeout;
+
+	ufs_set_flag(FLAG_DEVICE_INIT);
+
+	timeout = FDEVICEINIT_TIMEOUT_MS;
+	do {
+		result = ufs_read_flag(FLAG_DEVICE_INIT);
+		if (!result) {
+			break;
+		}
+		mdelay(5);
+		timeout -= 5;
+	} while (timeout > 0);
+
+	if (result != 0U) {
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
 static void ufs_enum(void)
 {
 	unsigned int blk_num, blk_size;
-	int i;
+	int i, result;
 
 	ufs_verify_init();
 	ufs_verify_ready();
 
-	ufs_set_flag(FLAG_DEVICE_INIT);
-	mdelay(200);
+	result = ufs_set_fdevice_init();
+	assert(result == 0);
+
 	/* dump available LUNs */
 	for (i = 0; i < UFS_MAX_LUNS; i++) {
 		ufs_read_capacity(i, &blk_num, &blk_size);
@@ -751,6 +810,8 @@
 			     i, blk_num, blk_size);
 		}
 	}
+
+	(void)result;
 }
 
 static void ufs_get_device_info(struct ufs_dev_desc *card_data)

diff --git a/fdts/stm32mp13-bl2.dtsi b/fdts/stm32mp13-bl2.dtsi
index 41d6e2e..00bf1b5 100644
--- a/fdts/stm32mp13-bl2.dtsi
+++ b/fdts/stm32mp13-bl2.dtsi

@@ -101,7 +101,7 @@
 		/delete-node/ tamp@5c00a000;
 		/delete-node/ stgen@5c008000;
 
-		pin-controller@50002000 {
+		pinctrl@50002000 {
 #if !STM32MP_EMMC && !STM32MP_SDMMC
 			/delete-node/ sdmmc1-b4-0;
 			/delete-node/ sdmmc2-b4-0;

diff --git a/fdts/stm32mp13-fw-config.dtsi b/fdts/stm32mp13-fw-config.dtsi
index dc8ca1b..28f7086 100644
--- a/fdts/stm32mp13-fw-config.dtsi
+++ b/fdts/stm32mp13-fw-config.dtsi

@@ -13,11 +13,9 @@
 #endif
 
 #define DDR_NS_BASE	STM32MP_DDR_BASE
-#define DDR_SEC_SIZE	0x01e00000
+#define DDR_SEC_SIZE	0x02000000
 #define DDR_SEC_BASE	(STM32MP_DDR_BASE + (DDR_SIZE - DDR_SEC_SIZE))
-#define DDR_SHARE_SIZE	0x00200000
-#define DDR_SHARE_BASE	(DDR_SEC_BASE - DDR_SHARE_SIZE)
-#define DDR_NS_SIZE	(DDR_SHARE_BASE - DDR_NS_BASE)
+#define DDR_NS_SIZE	(DDR_SEC_BASE - DDR_NS_BASE)
 
 /dts-v1/;
 
@@ -48,8 +46,6 @@
 		compatible = "st,mem-firewall";
 		memory-ranges = <
 			DDR_NS_BASE DDR_NS_SIZE TZC_REGION_S_NONE TZC_REGION_NSEC_ALL_ACCESS_RDWR
-			DDR_SHARE_BASE DDR_SHARE_SIZE TZC_REGION_S_NONE
-			TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_A7_ID)
 			DDR_SEC_BASE DDR_SEC_SIZE TZC_REGION_S_RDWR 0>;
 	};
 };

diff --git a/fdts/stm32mp131.dtsi b/fdts/stm32mp131.dtsi
index adf7a91..decd812 100644
--- a/fdts/stm32mp131.dtsi
+++ b/fdts/stm32mp131.dtsi

@@ -480,7 +480,7 @@
 		 * Break node order to solve dependency probe issue between
 		 * pinctrl and exti.
 		 */
-		pinctrl: pin-controller@50002000 {
+		pinctrl: pinctrl@50002000 {
 			#address-cells = <1>;
 			#size-cells = <1>;
 			compatible = "st,stm32mp135-pinctrl";

diff --git a/fdts/stm32mp15-bl2.dtsi b/fdts/stm32mp15-bl2.dtsi
index d00e35b..501b092 100644
--- a/fdts/stm32mp15-bl2.dtsi
+++ b/fdts/stm32mp15-bl2.dtsi

@@ -46,7 +46,7 @@
 		/delete-node/ i2c@5c009000;
 		/delete-node/ tamp@5c00a000;
 
-		pin-controller@50002000 {
+		pinctrl@50002000 {
 #if !STM32MP_RAW_NAND
 			/delete-node/ fmc-0;
 #endif

diff --git a/fdts/stm32mp15-bl32.dtsi b/fdts/stm32mp15-bl32.dtsi
index ca4bb3e..31b24f6 100644
--- a/fdts/stm32mp15-bl32.dtsi
+++ b/fdts/stm32mp15-bl32.dtsi

@@ -27,7 +27,7 @@
 		/delete-node/ stgen@5c008000;
 		/delete-node/ i2c@5c009000;
 
-		pin-controller@50002000 {
+		pinctrl@50002000 {
 			/delete-node/ fmc-0;
 			/delete-node/ qspi-clk-0;
 			/delete-node/ qspi-bk1-0;

diff --git a/fdts/stm32mp15-fw-config.dtsi b/fdts/stm32mp15-fw-config.dtsi
index 8aece28..d583672 100644
--- a/fdts/stm32mp15-fw-config.dtsi
+++ b/fdts/stm32mp15-fw-config.dtsi

@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
 /*
- * Copyright (c) 2021, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2021-2022, STMicroelectronics - All Rights Reserved
  */
 
 #include <common/tbbr/tbbr_img_def.h>
@@ -14,7 +14,7 @@
 
 #define DDR_NS_BASE	STM32MP_DDR_BASE
 #ifdef AARCH32_SP_OPTEE
-/* OP-TEE reserved shared memory: located at DDR top */
+/* OP-TEE reserved shared memory: located at DDR top or null size */
 #define DDR_SHARE_SIZE	STM32MP_DDR_SHMEM_SIZE
 #define DDR_SHARE_BASE	(STM32MP_DDR_BASE + (DDR_SIZE - DDR_SHARE_SIZE))
 /* OP-TEE secure memory: located right below OP-TEE reserved shared memory */
@@ -70,8 +70,11 @@
 		memory-ranges = <
 			DDR_NS_BASE DDR_NS_SIZE TZC_REGION_S_NONE TZC_REGION_NSEC_ALL_ACCESS_RDWR
 			DDR_SEC_BASE DDR_SEC_SIZE TZC_REGION_S_RDWR 0
+#if STM32MP15_OPTEE_RSV_SHM
 			DDR_SHARE_BASE DDR_SHARE_SIZE TZC_REGION_S_NONE
-			TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_A7_ID)>;
+			TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_A7_ID)
+#endif
+			>;
 #else
 		memory-ranges = <
 			DDR_NS_BASE DDR_NS_SIZE TZC_REGION_S_NONE TZC_REGION_NSEC_ALL_ACCESS_RDWR>;

diff --git a/fdts/stm32mp151.dtsi b/fdts/stm32mp151.dtsi
index 63cc917..20071fe 100644
--- a/fdts/stm32mp151.dtsi
+++ b/fdts/stm32mp151.dtsi

@@ -532,7 +532,7 @@
 		 * Break node order to solve dependency probe issue between
 		 * pinctrl and exti.
 		 */
-		pinctrl: pin-controller@50002000 {
+		pinctrl: pinctrl@50002000 {
 			#address-cells = <1>;
 			#size-cells = <1>;
 			compatible = "st,stm32mp157-pinctrl";
@@ -663,7 +663,7 @@
 			};
 		};
 
-		pinctrl_z: pin-controller-z@54004000 {
+		pinctrl_z: pinctrl@54004000 {
 			#address-cells = <1>;
 			#size-cells = <1>;
 			compatible = "st,stm32mp157-z-pinctrl";

diff --git a/fdts/stm32mp157a-avenger96.dts b/fdts/stm32mp157a-avenger96.dts
index b967736..6ae97c7 100644
--- a/fdts/stm32mp157a-avenger96.dts
+++ b/fdts/stm32mp157a-avenger96.dts

@@ -115,10 +115,9 @@
 
 			vtt_ddr: ldo3 {
 				regulator-name = "vtt_ddr";
-				regulator-min-microvolt = <500000>;
-				regulator-max-microvolt = <750000>;
 				regulator-always-on;
 				regulator-over-current-protection;
+				st,regulator-sink-source;
 			};
 
 			vdd_usb: ldo4 {
@@ -143,7 +142,6 @@
 			vref_ddr: vref_ddr {
 				regulator-name = "vref_ddr";
 				regulator-always-on;
-				regulator-over-current-protection;
 			};
 
 			bst_out: boost {

diff --git a/fdts/stm32mp157c-odyssey-som.dtsi b/fdts/stm32mp157c-odyssey-som.dtsi
index 6bed339..c4e1398 100644
--- a/fdts/stm32mp157c-odyssey-som.dtsi
+++ b/fdts/stm32mp157c-odyssey-som.dtsi

@@ -140,10 +140,9 @@
 
 			vtt_ddr: ldo3 {
 				regulator-name = "vtt_ddr";
-				regulator-min-microvolt = <500000>;
-				regulator-max-microvolt = <750000>;
 				regulator-always-on;
 				regulator-over-current-protection;
+				st,regulator-sink-source;
 			};
 
 			vdd_usb: ldo4 {
@@ -170,7 +169,6 @@
 			vref_ddr: vref_ddr {
 				regulator-name = "vref_ddr";
 				regulator-always-on;
-				regulator-over-current-protection;
 			};
 
 			bst_out: boost {

diff --git a/fdts/stm32mp15xx-osd32.dtsi b/fdts/stm32mp15xx-osd32.dtsi
index 76a2561..ca67235 100644
--- a/fdts/stm32mp15xx-osd32.dtsi
+++ b/fdts/stm32mp15xx-osd32.dtsi

@@ -81,10 +81,9 @@
 
 			vtt_ddr: ldo3 {
 				regulator-name = "vtt_ddr";
-				regulator-min-microvolt = <500000>;
-				regulator-max-microvolt = <750000>;
 				regulator-always-on;
 				regulator-over-current-protection;
+				st,regulator-sink-source;
 			};
 
 			vdd_usb: ldo4 {
@@ -110,7 +109,6 @@
 			vref_ddr: vref_ddr {
 				regulator-name = "vref_ddr";
 				regulator-always-on;
-				regulator-over-current-protection;
 			};
 
 			bst_out: boost {

diff --git a/fdts/tc.dts b/fdts/tc.dts
index 7c0e842..2099229 100644
--- a/fdts/tc.dts
+++ b/fdts/tc.dts

@@ -261,6 +261,12 @@
 		arm,mhuv2-protocols = <0 1>;
 	};
 
+	cmn-pmu {
+		compatible = "arm,ci-700";
+		reg = <0x0 0x50000000 0x0 0x10000000>;
+		interrupts = <0x0 460 0x4>;
+	};
+
 	scmi {
 		compatible = "arm,scmi";
 		mbox-names = "tx", "rx";

diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index b4608ae..e55d33f 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h

@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2013-2022, Arm Limited and Contributors. All rights reserved.
- * Copyright (c) 2020, NVIDIA Corporation. All rights reserved.
+ * Copyright (c) 2020-2022, NVIDIA Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -108,14 +108,14 @@
 #define HFGWTR_EL2		S3_4_C1_C1_5
 #define ICH_HCR_EL2		S3_4_C12_C11_0
 #define ICH_VMCR_EL2		S3_4_C12_C11_7
-#define MPAMVPM0_EL2		S3_4_C10_C5_0
-#define MPAMVPM1_EL2		S3_4_C10_C5_1
-#define MPAMVPM2_EL2		S3_4_C10_C5_2
-#define MPAMVPM3_EL2		S3_4_C10_C5_3
-#define MPAMVPM4_EL2		S3_4_C10_C5_4
-#define MPAMVPM5_EL2		S3_4_C10_C5_5
-#define MPAMVPM6_EL2		S3_4_C10_C5_6
-#define MPAMVPM7_EL2		S3_4_C10_C5_7
+#define MPAMVPM0_EL2		S3_4_C10_C6_0
+#define MPAMVPM1_EL2		S3_4_C10_C6_1
+#define MPAMVPM2_EL2		S3_4_C10_C6_2
+#define MPAMVPM3_EL2		S3_4_C10_C6_3
+#define MPAMVPM4_EL2		S3_4_C10_C6_4
+#define MPAMVPM5_EL2		S3_4_C10_C6_5
+#define MPAMVPM6_EL2		S3_4_C10_C6_6
+#define MPAMVPM7_EL2		S3_4_C10_C6_7
 #define MPAMVPMV_EL2		S3_4_C10_C4_1
 #define TRFCR_EL2		S3_4_C1_C2_1
 #define PMSCR_EL2		S3_4_C9_C9_0
@@ -234,6 +234,11 @@
 #define ID_AA64DFR0_MTPMU_MASK		ULL(0xf)
 #define ID_AA64DFR0_MTPMU_SUPPORTED	ULL(1)
 
+/* ID_AA64DFR0_EL1.BRBE definitions */
+#define ID_AA64DFR0_BRBE_SHIFT		U(52)
+#define ID_AA64DFR0_BRBE_MASK		ULL(0xf)
+#define ID_AA64DFR0_BRBE_SUPPORTED	ULL(1)
+
 /* ID_AA64ISAR0_EL1 definitions */
 #define ID_AA64ISAR0_RNDR_SHIFT	U(60)
 #define ID_AA64ISAR0_RNDR_MASK	ULL(0xf)
@@ -483,7 +488,8 @@
 #define SCR_HXEn_BIT		(UL(1) << 38)
 #define SCR_ENTP2_SHIFT		U(41)
 #define SCR_ENTP2_BIT		(UL(1) << SCR_ENTP2_SHIFT)
-#define SCR_AMVOFFEN_BIT	(UL(1) << 35)
+#define SCR_AMVOFFEN_SHIFT	U(35)
+#define SCR_AMVOFFEN_BIT	(UL(1) << SCR_AMVOFFEN_SHIFT)
 #define SCR_TWEDEn_BIT		(UL(1) << 29)
 #define SCR_ECVEN_BIT		(UL(1) << 28)
 #define SCR_FGTEN_BIT		(UL(1) << 27)
@@ -512,6 +518,8 @@
 #define MDCR_EnPMSN_BIT		(ULL(1) << 36)
 #define MDCR_MPMX_BIT		(ULL(1) << 35)
 #define MDCR_MCCD_BIT		(ULL(1) << 34)
+#define MDCR_SBRBE_SHIFT	U(32)
+#define MDCR_SBRBE_MASK		ULL(0x3)
 #define MDCR_NSTB(x)		((x) << 24)
 #define MDCR_NSTB_EL1		ULL(0x3)
 #define MDCR_NSTBE		(ULL(1) << 26)
@@ -1215,7 +1223,8 @@
 #define ERXMISC0_EL1		S3_0_C5_C5_0
 #define ERXMISC1_EL1		S3_0_C5_C5_1
 
-#define ERXCTLR_ED_BIT		(U(1) << 0)
+#define ERXCTLR_ED_SHIFT	U(0)
+#define ERXCTLR_ED_BIT		(U(1) << ERXCTLR_ED_SHIFT)
 #define ERXCTLR_UE_BIT		(U(1) << 4)
 
 #define ERXPFGCTL_UC_BIT	(U(1) << 1)

diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index 29710e7..79a61b5 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h

@@ -224,4 +224,23 @@
 		ID_AA64MMFR2_EL1_NV_MASK));
 }
 
+/*******************************************************************************
+ * Function to identify the presence of FEAT_BRBE (Branch Record Buffer
+ * Extension)
+ ******************************************************************************/
+static inline bool is_feat_brbe_present(void)
+{
+	return (((read_id_aa64dfr0_el1() >> ID_AA64DFR0_BRBE_SHIFT) &
+		ID_AA64DFR0_BRBE_MASK) == ID_AA64DFR0_BRBE_SUPPORTED);
+}
+
+/*******************************************************************************
+ * Function to identify the presence of FEAT_TRBE (Trace Buffer Extension)
+ ******************************************************************************/
+static inline bool is_feat_trbe_present(void)
+{
+	return (((read_id_aa64dfr0_el1() >> ID_AA64DFR0_TRACEBUFFER_SHIFT) &
+		ID_AA64DFR0_TRACEBUFFER_MASK) == ID_AA64DFR0_TRACEBUFFER_SUPPORTED);
+}
+
 #endif /* ARCH_FEATURES_H */

diff --git a/include/common/bl_common.h b/include/common/bl_common.h
index 8cb4990..3a06cfb 100644
--- a/include/common/bl_common.h
+++ b/include/common/bl_common.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -81,6 +81,10 @@
 #define __RODATA_END__			Load$$__RODATA_EPILOGUE__$$Base
 #define __RT_SVC_DESCS_START__		Load$$__RT_SVC_DESCS__$$Base
 #define __RT_SVC_DESCS_END__		Load$$__RT_SVC_DESCS__$$Limit
+#if SPMC_AT_EL3
+#define __EL3_LP_DESCS_START__		Load$$__EL3_LP_DESCS__$$Base
+#define __EL3_LP_DESCS_END__		Load$$__EL3_LP_DESCS__$$Limit
+#endif
 #define __RW_START__			Load$$LR$$LR_RW_DATA$$Base
 #define __RW_END__			Load$$LR$$LR_END$$Base
 #define __SPM_SHIM_EXCEPTIONS_START__	Load$$__SPM_SHIM_EXCEPTIONS__$$Base

diff --git a/include/common/bl_common.ld.h b/include/common/bl_common.ld.h
index 9888a3c..080e331 100644
--- a/include/common/bl_common.ld.h
+++ b/include/common/bl_common.ld.h

@@ -39,6 +39,16 @@
 	KEEP(*(rt_svc_descs))				\
 	__RT_SVC_DESCS_END__ = .;
 
+#if SPMC_AT_EL3
+#define EL3_LP_DESCS					\
+	. = ALIGN(STRUCT_ALIGN);			\
+	__EL3_LP_DESCS_START__ = .;			\
+	KEEP(*(el3_lp_descs))				\
+	__EL3_LP_DESCS_END__ = .;
+#else
+#define EL3_LP_DESCS
+#endif
+
 #define PMF_SVC_DESCS					\
 	. = ALIGN(STRUCT_ALIGN);			\
 	__PMF_SVC_DESCS_START__ = .;			\
@@ -89,7 +99,8 @@
 	PARSER_LIB_DESCS				\
 	CPU_OPS						\
 	GOT						\
-	BASE_XLAT_TABLE_RO
+	BASE_XLAT_TABLE_RO				\
+	EL3_LP_DESCS
 
 /*
  * .data must be placed at a lower address than the stacks if the stack

diff --git a/include/common/fdt_fixup.h b/include/common/fdt_fixup.h
index 0399ff5..9531bdb 100644
--- a/include/common/fdt_fixup.h
+++ b/include/common/fdt_fixup.h

@@ -7,14 +7,29 @@
 #ifndef FDT_FIXUP_H
 #define FDT_FIXUP_H
 
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
 #define INVALID_BASE_ADDR	((uintptr_t)~0UL)
 
+struct psci_cpu_idle_state {
+	const char *name;
+	uint32_t power_state;
+	bool local_timer_stop;
+	uint32_t entry_latency_us;
+	uint32_t exit_latency_us;
+	uint32_t min_residency_us;
+	uint32_t wakeup_latency_us;
+};
+
 int dt_add_psci_node(void *fdt);
 int dt_add_psci_cpu_enable_methods(void *fdt);
 int fdt_add_reserved_memory(void *dtb, const char *node_name,
 			    uintptr_t base, size_t size);
 int fdt_add_cpus_node(void *dtb, unsigned int afflv0,
 		      unsigned int afflv1, unsigned int afflv2);
+int fdt_add_cpu_idle_states(void *dtb, const struct psci_cpu_idle_state *state);
 int fdt_adjust_gic_redist(void *dtb, unsigned int nr_cores, uintptr_t gicr_base,
 			  unsigned int gicr_frame_size);
 int fdt_set_mac_address(void *dtb, unsigned int ethernet_idx,

diff --git a/include/common/fdt_wrappers.h b/include/common/fdt_wrappers.h
index 9c7180c..2929fc2 100644
--- a/include/common/fdt_wrappers.h
+++ b/include/common/fdt_wrappers.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -44,6 +44,8 @@
 int fdtw_for_each_cpu(const void *fdt,
 		      int (*callback)(const void *dtb, int node, uintptr_t mpidr));
 
+int fdtw_find_or_add_subnode(void *fdt, int parentoffset, const char *name);
+
 static inline uint32_t fdt_blob_size(const void *dtb)
 {
 	const uint32_t *dtb_header = dtb;

diff --git a/include/common/tbbr/cot_def.h b/include/common/tbbr/cot_def.h
index 800ad07..e0c2212 100644
--- a/include/common/tbbr/cot_def.h
+++ b/include/common/tbbr/cot_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,6 +14,8 @@
 /* TBBR CoT definitions */
 #if defined(SPD_spmd)
 #define COT_MAX_VERIFIED_PARAMS		8
+#elif defined(ARM_COT_cca)
+#define COT_MAX_VERIFIED_PARAMS		8
 #else
 #define COT_MAX_VERIFIED_PARAMS		4
 #endif

diff --git a/include/common/uuid.h b/include/common/uuid.h
index 5651d0d..c8dd681 100644
--- a/include/common/uuid.h
+++ b/include/common/uuid.h

@@ -1,15 +1,18 @@
 /*
- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#ifndef UUID_H
-#define UUID_H
+#ifndef UUID_COMMON_H
+#define UUID_COMMON_H
 
 #define UUID_BYTES_LENGTH	16
 #define UUID_STRING_LENGTH	36
 
 int read_uuid(uint8_t *dest, char *uuid);
+bool uuid_match(uint32_t *uuid1, uint32_t *uuid2);
+void copy_uuid(uint32_t *to_uuid, uint32_t *from_uuid);
+bool is_null_uuid(uint32_t *uuid);
 
-#endif /* UUID_H */
+#endif /* UUID_COMMON_H */

diff --git a/include/drivers/arm/gicv3.h b/include/drivers/arm/gicv3.h
index 5efefb6..8371dd5 100644
--- a/include/drivers/arm/gicv3.h
+++ b/include/drivers/arm/gicv3.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -76,6 +76,8 @@
 
 #endif	/* GIC_EXT_INTID */
 
+#define GIC_REV(r, p)           ((r << 4) | p)
+
 /*******************************************************************************
  * GICv3 and 3.1 specific Distributor interface register offsets and constants
  ******************************************************************************/
@@ -192,6 +194,15 @@
 #define GICR_CTLR_UWP_SHIFT	31
 #define GICR_CTLR_UWP_MASK	U(0x1)
 #define GICR_CTLR_UWP_BIT	BIT_32(GICR_CTLR_UWP_SHIFT)
+#define GICR_CTLR_DPG1S_SHIFT	26
+#define GICR_CTLR_DPG1S_MASK	U(0x1)
+#define GICR_CTLR_DPG1S_BIT	BIT_32(GICR_CTLR_DPG1S_SHIFT)
+#define GICR_CTLR_DPG1NS_SHIFT	25
+#define GICR_CTLR_DPG1NS_MASK	U(0x1)
+#define GICR_CTLR_DPG1NS_BIT	BIT_32(GICR_CTLR_DPG1NS_SHIFT)
+#define GICR_CTLR_DPG0_SHIFT	24
+#define GICR_CTLR_DPG0_MASK	U(0x1)
+#define GICR_CTLR_DPG0_BIT	BIT_32(GICR_CTLR_DPG0_SHIFT)
 #define GICR_CTLR_RWP_SHIFT	3
 #define GICR_CTLR_RWP_MASK	U(0x1)
 #define GICR_CTLR_RWP_BIT	BIT_32(GICR_CTLR_RWP_SHIFT)
@@ -224,12 +235,40 @@
 #define TYPER_PPI_NUM_MASK	U(0x1f)
 
 /* GICR_IIDR bit definitions */
-#define IIDR_PRODUCT_ID_MASK	U(0xff000000)
-#define IIDR_VARIANT_MASK	U(0x000f0000)
-#define IIDR_REVISION_MASK	U(0x0000f000)
-#define IIDR_IMPLEMENTER_MASK	U(0x00000fff)
-#define IIDR_MODEL_MASK		(IIDR_PRODUCT_ID_MASK | \
-				 IIDR_IMPLEMENTER_MASK)
+#define IIDR_PRODUCT_ID_MASK	U(0xff)
+#define IIDR_VARIANT_MASK	U(0xf)
+#define IIDR_REV_MASK		U(0xf)
+#define IIDR_IMPLEMENTER_MASK	U(0xfff)
+#define IIDR_PRODUCT_ID_SHIFT	24
+#define IIDR_VARIANT_SHIFT	16
+#define IIDR_REV_SHIFT		12
+#define IIDR_IMPLEMENTER_SHIFT	0
+#define IIDR_PRODUCT_ID_BIT	BIT_32(IIDR_PRODUCT_ID_SHIFT)
+#define IIDR_VARIANT_BIT	BIT_32(IIDR_VARIANT_SHIFT)
+#define IIDR_REV_BIT		BIT_32(IIDR_REVISION_SHIFT)
+#define IIDR_IMPLEMENTER_BIT	BIT_32(IIDR_IMPLEMENTER_SHIFT)
+
+#define IIDR_MODEL_MASK		(IIDR_PRODUCT_ID_MASK << IIDR_PRODUCT_ID_SHIFT | \
+				 IIDR_IMPLEMENTER_MASK << IIDR_IMPLEMENTER_SHIFT)
+
+#define GIC_PRODUCT_ID_GIC600	U(0x2)
+#define GIC_PRODUCT_ID_GIC600AE	U(0x3)
+#define GIC_PRODUCT_ID_GIC700	U(0x4)
+
+/*
+ * Note that below revisions and variants definations are as per GIC600/GIC600AE
+ * specification.
+ */
+#define GIC_REV_P0		U(0x1)
+#define GIC_REV_P1		U(0x3)
+#define GIC_REV_P2		U(0x4)
+#define GIC_REV_P3		U(0x5)
+#define GIC_REV_P4		U(0x6)
+#define GIC_REV_P6		U(0x7)
+
+#define GIC_VARIANT_R0		U(0x0)
+#define GIC_VARIANT_R1		U(0x1)
+#define GIC_VARIANT_R2		U(0x2)
 
 /*******************************************************************************
  * GICv3 and 3.1 CPU interface registers & constants
@@ -543,5 +582,17 @@
 void gicv3_clear_interrupt_pending(unsigned int id, unsigned int proc_num);
 unsigned int gicv3_set_pmr(unsigned int mask);
 
+void gicv3_get_component_prodid_rev(const uintptr_t gicd_base,
+				    unsigned int *gic_prod_id,
+				    uint8_t *gic_rev);
+void gicv3_check_erratas_applies(const uintptr_t gicd_base);
+#if GIC600_ERRATA_WA_2384374
+void gicv3_apply_errata_wa_2384374(const uintptr_t gicr_base);
+#else
+static inline void gicv3_apply_errata_wa_2384374(const uintptr_t gicr_base)
+{
+}
+#endif /* GIC600_ERRATA_WA_2384374 */
+
 #endif /* __ASSEMBLER__ */
 #endif /* GICV3_H */

diff --git a/include/drivers/arm/mhu.h b/include/drivers/arm/mhu.h
new file mode 100644
index 0000000..7745bd9
--- /dev/null
+++ b/include/drivers/arm/mhu.h

@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MHU_H
+#define MHU_H
+
+#include <stddef.h>
+#include <stdint.h>
+
+/**
+ * Generic MHU error enumeration types.
+ */
+enum mhu_error_t {
+	MHU_ERR_NONE			=  0,
+	MHU_ERR_NOT_INIT		= -1,
+	MHU_ERR_ALREADY_INIT		= -2,
+	MHU_ERR_UNSUPPORTED_VERSION	= -3,
+	MHU_ERR_UNSUPPORTED		= -4,
+	MHU_ERR_INVALID_ARG		= -5,
+	MHU_ERR_BUFFER_TOO_SMALL	= -6,
+	MHU_ERR_GENERAL			= -7,
+};
+
+/**
+ * Initializes sender MHU.
+ *
+ * mhu_sender_base	Base address of sender MHU.
+ *
+ * Returns mhu_error_t error code.
+ *
+ * This function must be called before mhu_send_data().
+ */
+enum mhu_error_t mhu_init_sender(uintptr_t mhu_sender_base);
+
+
+/**
+ * Initializes receiver MHU.
+ *
+ * mhu_receiver_base	Base address of receiver MHU.
+ *
+ * Returns mhu_error_t error code.
+ *
+ * This function must be called before mhu_receive_data().
+ */
+enum mhu_error_t mhu_init_receiver(uintptr_t mhu_receiver_base);
+
+/**
+ * Sends data over MHU.
+ *
+ * send_buffer		Pointer to buffer containing the data to be transmitted.
+ * size			Size of the data to be transmitted in bytes.
+ *
+ * Returns mhu_error_t error code.
+ *
+ * The send_buffer must be 4-byte aligned and its length must be at least
+ * (4 - (size % 4)) bytes bigger than the data size to prevent buffer
+ * over-reading.
+ */
+enum mhu_error_t mhu_send_data(const uint8_t *send_buffer, size_t size);
+
+/**
+ * Receives data from MHU.
+ *
+ * receive_buffer	Pointer the buffer where to store the received data.
+ * size			As input the size of the receive_buffer, as output the
+ *			number of bytes received. As a limitation,
+ *			the size of the buffer must be a multiple of 4.
+ *
+ * Returns mhu_error_t error code.
+ *
+ * The receive_buffer must be 4-byte aligned and its length must be a
+ * multiple of 4.
+ */
+enum mhu_error_t mhu_receive_data(uint8_t *receive_buffer, size_t *size);
+
+#endif /* MHU_H */

diff --git a/include/drivers/arm/rss_comms.h b/include/drivers/arm/rss_comms.h
new file mode 100644
index 0000000..b96c79f
--- /dev/null
+++ b/include/drivers/arm/rss_comms.h

@@ -0,0 +1,15 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef RSS_COMMS_H
+#define RSS_COMMS_H
+
+#include <stdint.h>
+
+int rss_comms_init(uintptr_t mhu_sender_base, uintptr_t mhu_receiver_base);
+
+#endif /* RSS_COMMS_H */

diff --git a/include/drivers/arm/smmu_v3.h b/include/drivers/arm/smmu_v3.h
index a820a44..37da56f 100644
--- a/include/drivers/arm/smmu_v3.h
+++ b/include/drivers/arm/smmu_v3.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,13 +9,39 @@
 
 #include <stdint.h>
 #include <lib/utils_def.h>
+#include <platform_def.h>
 
 /* SMMUv3 register offsets from device base */
+#define SMMU_CR0	U(0x0020)
+#define SMMU_CR0ACK	U(0x0024)
 #define SMMU_GBPA	U(0x0044)
 #define SMMU_S_IDR1	U(0x8004)
 #define SMMU_S_INIT	U(0x803c)
 #define SMMU_S_GBPA	U(0x8044)
 
+/*
+ * TODO: SMMU_ROOT_PAGE_OFFSET is platform specific.
+ * Currently defined as a command line model parameter.
+ */
+#if ENABLE_RME
+
+#define SMMU_ROOT_PAGE_OFFSET	(PLAT_ARM_SMMUV3_ROOT_REG_OFFSET)
+#define SMMU_ROOT_IDR0		U(SMMU_ROOT_PAGE_OFFSET + 0x0000)
+#define SMMU_ROOT_IIDR		U(SMMU_ROOT_PAGE_OFFSET + 0x0008)
+#define SMMU_ROOT_CR0		U(SMMU_ROOT_PAGE_OFFSET + 0x0020)
+#define SMMU_ROOT_CR0ACK	U(SMMU_ROOT_PAGE_OFFSET + 0x0024)
+#define SMMU_ROOT_GPT_BASE	U(SMMU_ROOT_PAGE_OFFSET + 0x0028)
+#define SMMU_ROOT_GPT_BASE_CFG	U(SMMU_ROOT_PAGE_OFFSET + 0x0030)
+#define SMMU_ROOT_GPF_FAR	U(SMMU_ROOT_PAGE_OFFSET + 0x0038)
+#define SMMU_ROOT_GPT_CFG_FAR	U(SMMU_ROOT_PAGE_OFFSET + 0x0040)
+#define SMMU_ROOT_TLBI		U(SMMU_ROOT_PAGE_OFFSET + 0x0050)
+#define SMMU_ROOT_TLBI_CTRL	U(SMMU_ROOT_PAGE_OFFSET + 0x0058)
+
+#endif /* ENABLE_RME */
+
+/* SMMU_CR0 and SMMU_CR0ACK register fields */
+#define SMMU_CR0_SMMUEN			(1UL << 0)
+
 /* SMMU_GBPA register fields */
 #define SMMU_GBPA_UPDATE		(1UL << 31)
 #define SMMU_GBPA_ABORT			(1UL << 20)
@@ -30,7 +56,16 @@
 #define SMMU_S_GBPA_UPDATE		(1UL << 31)
 #define SMMU_S_GBPA_ABORT		(1UL << 20)
 
+/* SMMU_ROOT_IDR0 register fields */
+#define SMMU_ROOT_IDR0_ROOT_IMPL	(1UL << 0)
+
+/* SMMU_ROOT_CR0 register fields */
+#define SMMU_ROOT_CR0_GPCEN		(1UL << 1)
+#define SMMU_ROOT_CR0_ACCESSEN		(1UL << 0)
+
 int smmuv3_init(uintptr_t smmu_base);
 int smmuv3_security_init(uintptr_t smmu_base);
 
+int smmuv3_ns_set_abort_all(uintptr_t smmu_base);
+
 #endif /* SMMU_V3_H */

diff --git a/include/drivers/auth/mbedtls/mbedtls_config.h b/include/drivers/auth/mbedtls/mbedtls_config.h
index 8ad6d7a..01e261a 100644
--- a/include/drivers/auth/mbedtls/mbedtls_config.h
+++ b/include/drivers/auth/mbedtls/mbedtls_config.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2015-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -80,8 +80,7 @@
 #define	MBEDTLS_SHA512_C
 #else
    /* TBB uses SHA-256, what about measured boot? */
-#if defined(TF_MBEDTLS_TPM_HASH_ALG_ID) && \
-	(TF_MBEDTLS_TPM_HASH_ALG_ID != TF_MBEDTLS_SHA256)
+#if defined(TF_MBEDTLS_MBOOT_USE_SHA512)
 #define MBEDTLS_SHA512_C
 #endif
 #endif
@@ -141,4 +140,13 @@
 #endif
 #endif
 
+/*
+ * Warn if errors from certain functions are ignored.
+ *
+ * The warnings are always enabled (where supported) for critical functions
+ * where ignoring the return value is almost always a bug. This macro extends
+ * the warnings to more functions.
+ */
+#define MBEDTLS_CHECK_RETURN_WARNING
+
 #endif /* MBEDTLS_CONFIG_H */

diff --git a/include/drivers/measured_boot/event_log/event_log.h b/include/drivers/measured_boot/event_log/event_log.h
index 0a19f8a..f4c4fb8 100644
--- a/include/drivers/measured_boot/event_log/event_log.h
+++ b/include/drivers/measured_boot/event_log/event_log.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -78,6 +78,14 @@
 #define EVLOG_TB_FW_CONFIG_STRING	"TB_FW_CONFIG"
 #define	EVLOG_TOS_FW_CONFIG_STRING	"TOS_FW_CONFIG"
 #define EVLOG_RMM_STRING 		"RMM"
+#define EVLOG_SP1_STRING		"SP1"
+#define EVLOG_SP2_STRING		"SP2"
+#define EVLOG_SP3_STRING		"SP3"
+#define EVLOG_SP4_STRING		"SP4"
+#define EVLOG_SP5_STRING		"SP5"
+#define EVLOG_SP6_STRING		"SP6"
+#define EVLOG_SP7_STRING		"SP7"
+#define EVLOG_SP8_STRING		"SP8"
 
 typedef struct {
 	unsigned int id;

diff --git a/include/drivers/measured_boot/rss/rss_measured_boot.h b/include/drivers/measured_boot/rss/rss_measured_boot.h
new file mode 100644
index 0000000..fe88576
--- /dev/null
+++ b/include/drivers/measured_boot/rss/rss_measured_boot.h

@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef RSS_MEASURED_BOOT_H
+#define RSS_MEASURED_BOOT_H
+
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <measured_boot.h>
+
+#define RSS_MBOOT_INVALID_ID	UINT32_MAX
+
+/*
+ * Each boot measurement has some metadata (i.e. a string) that identifies
+ * what was measured and how. The sw_type field of the rss_mboot_metadata
+ * structure represents the role of the software component that was measured.
+ * The below macros define strings suitable for the sw_type.
+ * The key thing is to choose meaningful strings so that when the attestation
+ * token is verified, then the different components can be identified.
+ */
+#define RSS_MBOOT_BL2_STRING		"BL_2"
+#define RSS_MBOOT_BL31_STRING		"SECURE_RT_EL3"
+#define RSS_MBOOT_HW_CONFIG_STRING	"HW_CONFIG"
+#define RSS_MBOOT_FW_CONFIG_STRING	"FW_CONFIG"
+#define RSS_MBOOT_TB_FW_CONFIG_STRING	"TB_FW_CONFIG"
+#define RSS_MBOOT_SOC_FW_CONFIG_STRING	"SOC_FW_CONFIG"
+#define RSS_MBOOT_RMM_STRING		"RMM"
+
+
+struct rss_mboot_metadata {
+	unsigned int id;
+	uint8_t slot;
+	uint8_t signer_id[SIGNER_ID_MAX_SIZE];
+	size_t  signer_id_size;
+	uint8_t version[VERSION_MAX_SIZE];
+	size_t  version_size;
+	uint8_t sw_type[SW_TYPE_MAX_SIZE];
+	size_t  sw_type_size;
+	bool    lock_measurement;
+};
+
+/* Functions' declarations */
+void rss_measured_boot_init(void);
+struct rss_mboot_metadata *plat_rss_mboot_get_metadata(void);
+int rss_mboot_measure_and_record(uintptr_t data_base, uint32_t data_size,
+				 uint32_t data_id);
+
+/* TODO: These metadata are currently not available during TF-A boot */
+int rss_mboot_set_signer_id(unsigned int img_id, const void *pk_ptr, size_t pk_len);
+
+#endif /* RSS_MEASURED_BOOT_H */

diff --git a/include/drivers/mmc.h b/include/drivers/mmc.h
index 834a80f..c154ea5 100644
--- a/include/drivers/mmc.h
+++ b/include/drivers/mmc.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -66,6 +66,7 @@
 #define EXT_CSD_PART_CONFIG_ACC_MASK	GENMASK(2, 0)
 #define PART_CFG_BOOT_PARTITION1_ENABLE	(U(1) << 3)
 #define PART_CFG_BOOT_PARTITION1_ACCESS (U(1) << 0)
+#define PART_CFG_BOOT_PARTITION_NO_ACCESS	U(0)
 #define PART_CFG_BOOT_PART_EN_MASK		GENMASK(5, 3)
 #define PART_CFG_BOOT_PART_EN_SHIFT		3
 #define PART_CFG_CURRENT_BOOT_PARTITION(x)	(((x) & PART_CFG_BOOT_PART_EN_MASK) >> \
@@ -233,9 +234,8 @@
 size_t mmc_read_blocks(int lba, uintptr_t buf, size_t size);
 size_t mmc_write_blocks(int lba, const uintptr_t buf, size_t size);
 size_t mmc_erase_blocks(int lba, size_t size);
-size_t mmc_rpmb_read_blocks(int lba, uintptr_t buf, size_t size);
-size_t mmc_rpmb_write_blocks(int lba, const uintptr_t buf, size_t size);
-size_t mmc_rpmb_erase_blocks(int lba, size_t size);
+int mmc_part_switch_current_boot(void);
+int mmc_part_switch_user(void);
 size_t mmc_boot_part_read_blocks(int lba, uintptr_t buf, size_t size);
 int mmc_init(const struct mmc_ops *ops_ptr, unsigned int clk,
 	     unsigned int width, unsigned int flags,

diff --git a/include/drivers/nxp/gic/gicv3/plat_gic.h b/include/drivers/nxp/gic/gicv3/plat_gic.h
index 0c0d0fc..794b06b 100644
--- a/include/drivers/nxp/gic/gicv3/plat_gic.h
+++ b/include/drivers/nxp/gic/gicv3/plat_gic.h

@@ -61,7 +61,6 @@
 
 #define GICR_ICENABLER0_SGI15   0x00008000
 #define GICR_CTLR_RWP           0x8
-#define GICR_CTLR_DPG0_MASK     0x2000000
 #define GICR_IGROUPR0_SGI15     0x00008000
 #define GICR_IGRPMODR0_SGI15    0x00008000
 #define GICR_ISENABLER0_SGI15   0x00008000

diff --git a/include/drivers/partition/partition.h b/include/drivers/partition/partition.h
index b292ec7..11e5acf 100644
--- a/include/drivers/partition/partition.h
+++ b/include/drivers/partition/partition.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -29,6 +29,8 @@
 
 #define LEGACY_PARTITION_BLOCK_SIZE	512
 
+#define DEFAULT_GPT_HEADER_SIZE 	92
+
 typedef struct partition_entry {
 	uint64_t		start;
 	uint64_t		length;

diff --git a/include/drivers/ufs.h b/include/drivers/ufs.h
index c074e85..4a5e464 100644
--- a/include/drivers/ufs.h
+++ b/include/drivers/ufs.h

@@ -69,6 +69,7 @@
 /* Host Controller Enable */
 #define HCE				0x34
 #define HCE_ENABLE			1
+#define HCE_DISABLE			0
 
 /* Host UIC Error Code PHY Adapter Layer */
 #define UECPA				0x38
@@ -264,6 +265,9 @@
 #define HCE_ENABLE_OUTER_RETRIES	3
 #define HCE_ENABLE_INNER_RETRIES	50
 #define HCE_ENABLE_TIMEOUT_US		100
+#define HCE_DISABLE_TIMEOUT_US		1000
+
+#define FDEVICEINIT_TIMEOUT_MS	        1500
 
 /**
  * ufs_dev_desc - ufs device details from the device descriptor

diff --git a/include/export/common/tbbr/tbbr_img_def_exp.h b/include/export/common/tbbr/tbbr_img_def_exp.h
index 98544c0..98a0099 100644
--- a/include/export/common/tbbr/tbbr_img_def_exp.h
+++ b/include/export/common/tbbr/tbbr_img_def_exp.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -104,7 +104,16 @@
 /* Realm Monitor Manager (RMM) */
 #define RMM_IMAGE_ID			U(34)
 
+/* CCA Content Certificate ID */
+#define CCA_CONTENT_CERT_ID		U(35)
+
+/* Core SWD Key Certificate ID */
+#define CORE_SWD_KEY_CERT_ID		U(36)
+
+/* Platform Key Certificate ID */
+#define PLAT_KEY_CERT_ID		U(37)
+
 /* Max Images */
-#define MAX_IMAGE_IDS			U(35)
+#define MAX_IMAGE_IDS			U(38)
 
 #endif /* ARM_TRUSTED_FIRMWARE_EXPORT_COMMON_TBBR_TBBR_IMG_DEF_EXP_H */

diff --git a/include/lib/cpus/aarch64/cortex_a77.h b/include/lib/cpus/aarch64/cortex_a77.h
index 4a87168..63f155f 100644
--- a/include/lib/cpus/aarch64/cortex_a77.h
+++ b/include/lib/cpus/aarch64/cortex_a77.h

@@ -32,6 +32,7 @@
  ******************************************************************************/
 #define CORTEX_A77_ACTLR2_EL1				S3_0_C15_C1_1
 #define CORTEX_A77_ACTLR2_EL1_BIT_2			(ULL(1) << 2)
+#define CORTEX_A77_ACTLR2_EL1_BIT_0			ULL(1)
 
 #define CORTEX_A77_CPUPSELR_EL3				S3_6_C15_C8_0
 #define CORTEX_A77_CPUPCR_EL3				S3_6_C15_C8_1

diff --git a/include/lib/cpus/aarch64/cortex_hunter.h b/include/lib/cpus/aarch64/cortex_hunter.h
index 8b59fd9..24bd217 100644
--- a/include/lib/cpus/aarch64/cortex_hunter.h
+++ b/include/lib/cpus/aarch64/cortex_hunter.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,9 @@
 
 #define CORTEX_HUNTER_MIDR					U(0x410FD810)
 
+/* Cortex Hunter loop count for CVE-2022-23960 mitigation */
+#define CORTEX_HUNTER_BHB_LOOP_COUNT				U(132)
+
 /*******************************************************************************
  * CPU Extended Control register specific definitions
  ******************************************************************************/

diff --git a/include/lib/cpus/aarch64/cortex_makalu.h b/include/lib/cpus/aarch64/cortex_makalu.h
index 4e0dc86..ee59657 100644
--- a/include/lib/cpus/aarch64/cortex_makalu.h
+++ b/include/lib/cpus/aarch64/cortex_makalu.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,9 @@
 
 #define CORTEX_MAKALU_MIDR					U(0x410FD4D0)
 
+/* Cortex Makalu loop count for CVE-2022-23960 mitigation */
+#define CORTEX_MAKALU_BHB_LOOP_COUNT				U(38)
+
 /*******************************************************************************
  * CPU Extended Control register specific definitions
  ******************************************************************************/

diff --git a/include/lib/cpus/aarch64/cortex_makalu_elp_arm.h b/include/lib/cpus/aarch64/cortex_makalu_elp_arm.h
index a0d788e..9ed5ee3 100644
--- a/include/lib/cpus/aarch64/cortex_makalu_elp_arm.h
+++ b/include/lib/cpus/aarch64/cortex_makalu_elp_arm.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,9 @@
 
 #define CORTEX_MAKALU_ELP_ARM_MIDR				U(0x410FD4E0)
 
+/* Cortex Makalu ELP loop count for CVE-2022-23960 mitigation */
+#define CORTEX_MAKALU_ELP_ARM_BHB_LOOP_COUNT			U(132)
+
 /*******************************************************************************
  * CPU Extended Control register specific definitions
  ******************************************************************************/

diff --git a/include/lib/cpus/aarch64/cortex_x1.h b/include/lib/cpus/aarch64/cortex_x1.h
new file mode 100644
index 0000000..e3661a8
--- /dev/null
+++ b/include/lib/cpus/aarch64/cortex_x1.h

@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2022, Google LLC. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CORTEX_X1_H
+#define CORTEX_X1_H
+
+/* Cortex-X1 MIDR for r1p0 */
+#define CORTEX_X1_MIDR			U(0x411fd440)
+
+/* Cortex-X1 loop count for CVE-2022-23960 mitigation */
+#define CORTEX_X1_BHB_LOOP_COUNT	U(32)
+
+/*******************************************************************************
+ * CPU Extended Control register specific definitions.
+ ******************************************************************************/
+#define CORTEX_X1_CPUECTLR_EL1		S3_0_C15_C1_4
+
+/*******************************************************************************
+ * CPU Auxiliary Control register specific definitions.
+ ******************************************************************************/
+#define CORTEX_X1_ACTLR2_EL1		S3_0_C15_C1_1
+
+/*******************************************************************************
+ * CPU Power Control register specific definitions
+ ******************************************************************************/
+#define CORTEX_X1_CPUPWRCTLR_EL1	S3_0_C15_C2_7
+#define CORTEX_X1_CORE_PWRDN_EN_MASK	U(0x1)
+
+#endif /* CORTEX_X1_H */

diff --git a/include/lib/cpus/aarch64/dsu_def.h b/include/lib/cpus/aarch64/dsu_def.h
index 0969acf..577de61 100644
--- a/include/lib/cpus/aarch64/dsu_def.h
+++ b/include/lib/cpus/aarch64/dsu_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -32,6 +32,7 @@
 #define CLUSTERACTLR_EL1	S3_0_C15_C3_3
 
 #define CLUSTERACTLR_EL1_DISABLE_CLOCK_GATING	(ULL(1) << 15)
+#define CLUSTERACTLR_EL1_DISABLE_SCLK_GATING	(ULL(3) << 15)
 
 /********************************************************************
  * Masks applied for DSU errata workarounds

diff --git a/include/lib/cpus/aarch64/neoverse_demeter.h b/include/lib/cpus/aarch64/neoverse_demeter.h
index 230ed66..f1afae7 100644
--- a/include/lib/cpus/aarch64/neoverse_demeter.h
+++ b/include/lib/cpus/aarch64/neoverse_demeter.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,9 @@
 
 #define NEOVERSE_DEMETER_MIDR				U(0x410FD4F0)
 
+/* Neoverse Demeter loop count for CVE-2022-23960 mitigation */
+#define NEOVERSE_DEMETER_BHB_LOOP_COUNT			U(132)
+
 /*******************************************************************************
  * CPU Extended Control register specific definitions
  ******************************************************************************/

diff --git a/include/lib/cpus/aarch64/neoverse_poseidon.h b/include/lib/cpus/aarch64/neoverse_poseidon.h
index 0a8b1d1..798ecd1 100644
--- a/include/lib/cpus/aarch64/neoverse_poseidon.h
+++ b/include/lib/cpus/aarch64/neoverse_poseidon.h

@@ -10,6 +10,9 @@
 
 #define NEOVERSE_POSEIDON_MIDR                      		U(0x410FD830)
 
+/* Neoverse Poseidon loop count for CVE-2022-23960 mitigation */
+#define NEOVERSE_POSEIDON_BHB_LOOP_COUNT			U(132)
+
 /*******************************************************************************
  * CPU Extended Control register specific definitions.
  ******************************************************************************/

diff --git a/include/lib/cpus/aarch64/neoverse_v1.h b/include/lib/cpus/aarch64/neoverse_v1.h
index a904c04..181be1d 100644
--- a/include/lib/cpus/aarch64/neoverse_v1.h
+++ b/include/lib/cpus/aarch64/neoverse_v1.h

@@ -32,7 +32,9 @@
  * CPU Auxiliary Control register specific definitions.
  ******************************************************************************/
 #define NEOVERSE_V1_ACTLR2_EL1					S3_0_C15_C1_1
+#define NEOVERSE_V1_ACTLR2_EL1_BIT_0				ULL(1)
 #define NEOVERSE_V1_ACTLR2_EL1_BIT_2				(ULL(1) << 2)
 #define NEOVERSE_V1_ACTLR2_EL1_BIT_28				(ULL(1) << 28)
+#define NEOVERSE_V1_ACTLR2_EL1_BIT_40				(ULL(1) << 40)
 
 #endif /* NEOVERSE_V1_H */

diff --git a/include/lib/el3_runtime/aarch64/context.h b/include/lib/el3_runtime/aarch64/context.h
index 3a09383..6c13166 100644
--- a/include/lib/el3_runtime/aarch64/context.h
+++ b/include/lib/el3_runtime/aarch64/context.h

@@ -509,9 +509,53 @@
 void el1_sysregs_context_restore(el1_sysregs_t *regs);
 
 #if CTX_INCLUDE_EL2_REGS
-void el2_sysregs_context_save(el2_sysregs_t *regs);
-void el2_sysregs_context_restore(el2_sysregs_t *regs);
-#endif
+void el2_sysregs_context_save_common(el2_sysregs_t *regs);
+void el2_sysregs_context_restore_common(el2_sysregs_t *regs);
+#if ENABLE_SPE_FOR_LOWER_ELS
+void el2_sysregs_context_save_spe(el2_sysregs_t *regs);
+void el2_sysregs_context_restore_spe(el2_sysregs_t *regs);
+#endif /* ENABLE_SPE_FOR_LOWER_ELS */
+#if CTX_INCLUDE_MTE_REGS
+void el2_sysregs_context_save_mte(el2_sysregs_t *regs);
+void el2_sysregs_context_restore_mte(el2_sysregs_t *regs);
+#endif /* CTX_INCLUDE_MTE_REGS */
+#if ENABLE_MPAM_FOR_LOWER_ELS
+void el2_sysregs_context_save_mpam(el2_sysregs_t *regs);
+void el2_sysregs_context_restore_mpam(el2_sysregs_t *regs);
+#endif /* ENABLE_MPAM_FOR_LOWER_ELS */
+#if ENABLE_FEAT_FGT
+void el2_sysregs_context_save_fgt(el2_sysregs_t *regs);
+void el2_sysregs_context_restore_fgt(el2_sysregs_t *regs);
+#endif /* ENABLE_FEAT_FGT */
+#if ENABLE_FEAT_ECV
+void el2_sysregs_context_save_ecv(el2_sysregs_t *regs);
+void el2_sysregs_context_restore_ecv(el2_sysregs_t *regs);
+#endif /* ENABLE_FEAT_ECV */
+#if ENABLE_FEAT_VHE
+void el2_sysregs_context_save_vhe(el2_sysregs_t *regs);
+void el2_sysregs_context_restore_vhe(el2_sysregs_t *regs);
+#endif /* ENABLE_FEAT_VHE */
+#if RAS_EXTENSION
+void el2_sysregs_context_save_ras(el2_sysregs_t *regs);
+void el2_sysregs_context_restore_ras(el2_sysregs_t *regs);
+#endif /* RAS_EXTENSION */
+#if CTX_INCLUDE_NEVE_REGS
+void el2_sysregs_context_save_nv2(el2_sysregs_t *regs);
+void el2_sysregs_context_restore_nv2(el2_sysregs_t *regs);
+#endif /* CTX_INCLUDE_NEVE_REGS */
+#if ENABLE_TRF_FOR_NS
+void el2_sysregs_context_save_trf(el2_sysregs_t *regs);
+void el2_sysregs_context_restore_trf(el2_sysregs_t *regs);
+#endif /* ENABLE_TRF_FOR_NS */
+#if ENABLE_FEAT_CSV2_2
+void el2_sysregs_context_save_csv2(el2_sysregs_t *regs);
+void el2_sysregs_context_restore_csv2(el2_sysregs_t *regs);
+#endif /* ENABLE_FEAT_CSV2_2 */
+#if ENABLE_FEAT_HCX
+void el2_sysregs_context_save_hcx(el2_sysregs_t *regs);
+void el2_sysregs_context_restore_hcx(el2_sysregs_t *regs);
+#endif /* ENABLE_FEAT_HCX */
+#endif /* CTX_INCLUDE_EL2_REGS */
 
 #if CTX_INCLUDE_FPREGS
 void fpregs_context_save(fp_regs_t *regs);

diff --git a/include/lib/extensions/brbe.h b/include/lib/extensions/brbe.h
new file mode 100644
index 0000000..aac1ace
--- /dev/null
+++ b/include/lib/extensions/brbe.h

@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef BRBE_H
+#define BRBE_H
+
+void brbe_enable(void);
+
+#endif /* BRBE_H */

diff --git a/include/lib/extensions/twed.h b/include/lib/extensions/twed.h
deleted file mode 100644
index eac4aa3..0000000
--- a/include/lib/extensions/twed.h
+++ /dev/null

@@ -1,16 +0,0 @@
-/*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef TWED_H
-#define TWED_H
-
-#include <stdint.h>
-
-#define TWED_DISABLED U(0xFFFFFFFF)
-
-uint32_t plat_arm_set_twedel_scr_el3(void);
-
-#endif /* TWEDE_H */

diff --git a/include/lib/fconf/fconf_dyn_cfg_getter.h b/include/lib/fconf/fconf_dyn_cfg_getter.h
index ff51c6c..43f298e 100644
--- a/include/lib/fconf/fconf_dyn_cfg_getter.h
+++ b/include/lib/fconf/fconf_dyn_cfg_getter.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2019-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,6 +18,13 @@
 	uintptr_t config_addr;
 	uint32_t config_max_size;
 	unsigned int config_id;
+	/*
+	 * Load address in non-secure memory. Only needed by those
+	 * configuration files which require being loaded in secure
+	 * memory (at config_addr) as well as in non-secure memory
+	 * - e.g. HW_CONFIG
+	 */
+	uintptr_t ns_config_addr;
 };
 
 unsigned int dyn_cfg_dtb_info_get_index(unsigned int config_id);
@@ -25,7 +32,8 @@
 int fconf_populate_dtb_registry(uintptr_t config);
 
 /* Set config information in global DTB array */
-void set_config_info(uintptr_t config_addr, uint32_t config_max_size,
-			unsigned int config_id);
+void set_config_info(uintptr_t config_addr, uintptr_t ns_config_addr,
+		     uint32_t config_max_size,
+		     unsigned int config_id);
 
 #endif /* FCONF_DYN_CFG_GETTER_H */

diff --git a/include/lib/psa/initial_attestation.h b/include/lib/psa/initial_attestation.h
new file mode 100644
index 0000000..93169f0
--- /dev/null
+++ b/include/lib/psa/initial_attestation.h

@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2018-2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef PSA_INITIAL_ATTESTATION_H
+#define PSA_INITIAL_ATTESTATION_H
+
+#include <limits.h>
+#include <stddef.h>
+#include <stdint.h>
+
+#include "psa/error.h"
+
+/*
+ * Initial attestation API version is: 1.0.0
+ */
+#define PSA_INITIAL_ATTEST_API_VERSION_MAJOR	(1)
+#define PSA_INITIAL_ATTEST_API_VERSION_MINOR	(0)
+
+/* The allowed size of input challenge in bytes. */
+#define PSA_INITIAL_ATTEST_CHALLENGE_SIZE_32	32U
+#define PSA_INITIAL_ATTEST_CHALLENGE_SIZE_48	48U
+#define PSA_INITIAL_ATTEST_CHALLENGE_SIZE_64	64U
+
+/* Initial Attestation message types that distinguish Attest services. */
+#define RSS_ATTEST_GET_TOKEN		1001U
+#define RSS_ATTEST_GET_TOKEN_SIZE	1002U
+#define RSS_ATTEST_GET_DELEGATED_KEY	1003U
+
+/**
+ * Get the platform attestation token.
+ *
+ * auth_challenge	Pointer to buffer where challenge input is stored. This
+ *			must be the hash of the public part of the delegated
+ *			attestation key.
+ * challenge_size	Size of challenge object in bytes.
+ * token_buf		Pointer to the buffer where attestation token will be
+ *			stored.
+ * token_buf_size	Size of allocated buffer for token, in bytes.
+ * token_size		Size of the token that has been returned, in bytes.
+ *
+ * Returns error code as specified in psa_status_t.
+ */
+psa_status_t
+psa_initial_attest_get_token(const uint8_t *auth_challenge,
+			     size_t         challenge_size,
+			     uint8_t       *token_buf,
+			     size_t         token_buf_size,
+			     size_t        *token_size);
+
+#endif /* PSA_INITIAL_ATTESTATION_H */

diff --git a/include/lib/psa/measured_boot.h b/include/lib/psa/measured_boot.h
new file mode 100644
index 0000000..bdb79d5
--- /dev/null
+++ b/include/lib/psa/measured_boot.h

@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef PSA_MEASURED_BOOT_H
+#define PSA_MEASURED_BOOT_H
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+#include "psa/error.h"
+
+/* Minimum measurement value size that can be requested to store */
+#define MEASUREMENT_VALUE_MIN_SIZE	32U
+/* Maximum measurement value size that can be requested to store */
+#define MEASUREMENT_VALUE_MAX_SIZE	64U
+/* Minimum signer id size that can be requested to store */
+#define SIGNER_ID_MIN_SIZE		MEASUREMENT_VALUE_MIN_SIZE
+/* Maximum signer id size that can be requested to store */
+#define SIGNER_ID_MAX_SIZE		MEASUREMENT_VALUE_MAX_SIZE
+/* The theoretical maximum image version is: "255.255.65535\0" */
+#define VERSION_MAX_SIZE		14U
+/* Example sw_type: "BL_2, BL_33, etc." */
+#define SW_TYPE_MAX_SIZE		20U
+#define NUM_OF_MEASUREMENT_SLOTS	32U
+
+
+/**
+ * Extends and stores a measurement to the requested slot.
+ *
+ * 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.
+ * version			Pointer to version buffer.
+ * version_size			Size of the version buffer in bytes.
+ * 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.
+ * measurement_value		Pointer to measurement_value buffer.
+ * measurement_value_size	Size of the measurement_value buffer in bytes.
+ * lock_measurement		Boolean flag requesting whether the measurement
+ *				is to be locked.
+ *
+ * PSA_SUCCESS:
+ *	- Success.
+ * PSA_ERROR_INVALID_ARGUMENT:
+ *	- The size of any argument is invalid OR
+ *	- Input Measurement value is NULL OR
+ *	- Input Signer ID is NULL OR
+ *	- Requested slot index is invalid.
+ * PSA_ERROR_BAD_STATE:
+ *	- Request to lock, when slot is already locked.
+ * PSA_ERROR_NOT_PERMITTED:
+ *	- When the requested slot is not accessible to the caller.
+ */
+
+/* Not a standard PSA API, just an extension therefore use the 'rss_' prefix
+ * rather than the usual 'psa_'.
+ */
+psa_status_t
+rss_measured_boot_extend_measurement(uint8_t index,
+				     const uint8_t *signer_id,
+				     size_t signer_id_size,
+				     const uint8_t *version,
+				     size_t version_size,
+				     uint32_t measurement_algo,
+				     const uint8_t *sw_type,
+				     size_t sw_type_size,
+				     const uint8_t *measurement_value,
+				     size_t measurement_value_size,
+				     bool lock_measurement);
+
+#endif /* PSA_MEASURED_BOOT_H */

diff --git a/include/lib/psa/psa/client.h b/include/lib/psa/psa/client.h
new file mode 100644
index 0000000..56fe028
--- /dev/null
+++ b/include/lib/psa/psa/client.h

@@ -0,0 +1,102 @@
+
+/*
+ * Copyright (c) 2018-2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef PSA_CLIENT_H
+#define PSA_CLIENT_H
+
+#include <stddef.h>
+#include <stdint.h>
+
+#include <psa/error.h>
+
+#ifndef IOVEC_LEN
+#define IOVEC_LEN(arr) ((uint32_t)(sizeof(arr)/sizeof(arr[0])))
+#endif
+/*********************** PSA Client Macros and Types *************************/
+/**
+ * The version of the PSA Framework API that is being used to build the calling
+ * firmware. Only part of features of FF-M v1.1 have been implemented. FF-M v1.1
+ * is compatible with v1.0.
+ */
+#define PSA_FRAMEWORK_VERSION	(0x0101u)
+/**
+ * Return value from psa_version() if the requested RoT Service is not present
+ * in the system.
+ */
+#define PSA_VERSION_NONE	(0u)
+/**
+ * The zero-value null handle can be assigned to variables used in clients and
+ * RoT Services, indicating that there is no current connection or message.
+ */
+#define PSA_NULL_HANDLE		((psa_handle_t)0)
+/**
+ * Tests whether a handle value returned by psa_connect() is valid.
+ */
+#define PSA_HANDLE_IS_VALID(handle)	((psa_handle_t)(handle) > 0)
+/**
+ * Converts the handle value returned from a failed call psa_connect() into
+ * an error code.
+ */
+#define PSA_HANDLE_TO_ERROR(handle)	((psa_status_t)(handle))
+/**
+ * Maximum number of input and output vectors for a request to psa_call().
+ */
+#define PSA_MAX_IOVEC		(4u)
+/**
+ * An IPC message type that indicates a generic client request.
+ */
+#define PSA_IPC_CALL		(0)
+typedef int32_t psa_handle_t;
+/**
+ * A read-only input memory region provided to an RoT Service.
+ */
+typedef struct psa_invec {
+	const void *base;	/*!< the start address of the memory buffer */
+	size_t len;		/*!< the size in bytes                      */
+} psa_invec;
+/**
+ * A writable output memory region provided to an RoT Service.
+ */
+typedef struct psa_outvec {
+	void *base;		/*!< the start address of the memory buffer */
+	size_t len;		/*!< the size in bytes                      */
+} psa_outvec;
+
+/**
+ * Call an RoT Service on an established connection.
+ *
+ * handle	A handle to an established connection.
+ * type		The request type. Must be zero(PSA_IPC_CALL) or positive.
+ * in_vec	Array of input psa_invec structures.
+ * in_len	Number of input psa_invec structures.
+ * out_vec	Array of output psa_outvec structures.
+ * out_len	Number of output psa_outvec structures.
+ *
+ * Return value >=0	RoT Service-specific status value.
+ * Return value <0	RoT Service-specific error code.
+ *
+ * PSA_ERROR_PROGRAMMER_ERROR:
+ *	- The connection has been terminated by the RoT Service.
+ *
+ * The call is a PROGRAMMER ERROR if one or more of the following are true:
+ *	- An invalid handle was passed.
+ *	- The connection is already handling a request.
+ *	- type < 0.
+ *	- An invalid memory reference was provided.
+ *	- in_len + out_len > PSA_MAX_IOVEC.
+ *	- The message is unrecognized by the RoT.
+ *	- Service or incorrectly formatted.
+ */
+psa_status_t psa_call(psa_handle_t handle,
+		      int32_t type,
+		      const psa_invec *in_vec,
+		      size_t in_len,
+		      psa_outvec *out_vec,
+		      size_t out_len);
+
+#endif /* PSA_CLIENT_H */

diff --git a/include/lib/psa/psa/error.h b/include/lib/psa/psa/error.h
new file mode 100644
index 0000000..8a6eb7b
--- /dev/null
+++ b/include/lib/psa/psa/error.h

@@ -0,0 +1,42 @@
+
+/*
+ * Copyright (c) 2019-2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef PSA_ERROR_H
+#define PSA_ERROR_H
+
+#include <stdint.h>
+
+typedef int32_t psa_status_t;
+
+#define PSA_SUCCESS                     ((psa_status_t)0)
+#define PSA_SUCCESS_REBOOT              ((psa_status_t)1)
+#define PSA_SUCCESS_RESTART             ((psa_status_t)2)
+#define PSA_ERROR_PROGRAMMER_ERROR      ((psa_status_t)-129)
+#define PSA_ERROR_CONNECTION_REFUSED    ((psa_status_t)-130)
+#define PSA_ERROR_CONNECTION_BUSY       ((psa_status_t)-131)
+#define PSA_ERROR_GENERIC_ERROR         ((psa_status_t)-132)
+#define PSA_ERROR_NOT_PERMITTED         ((psa_status_t)-133)
+#define PSA_ERROR_NOT_SUPPORTED         ((psa_status_t)-134)
+#define PSA_ERROR_INVALID_ARGUMENT      ((psa_status_t)-135)
+#define PSA_ERROR_INVALID_HANDLE        ((psa_status_t)-136)
+#define PSA_ERROR_BAD_STATE             ((psa_status_t)-137)
+#define PSA_ERROR_BUFFER_TOO_SMALL      ((psa_status_t)-138)
+#define PSA_ERROR_ALREADY_EXISTS        ((psa_status_t)-139)
+#define PSA_ERROR_DOES_NOT_EXIST        ((psa_status_t)-140)
+#define PSA_ERROR_INSUFFICIENT_MEMORY   ((psa_status_t)-141)
+#define PSA_ERROR_INSUFFICIENT_STORAGE  ((psa_status_t)-142)
+#define PSA_ERROR_INSUFFICIENT_DATA     ((psa_status_t)-143)
+#define PSA_ERROR_SERVICE_FAILURE       ((psa_status_t)-144)
+#define PSA_ERROR_COMMUNICATION_FAILURE ((psa_status_t)-145)
+#define PSA_ERROR_STORAGE_FAILURE       ((psa_status_t)-146)
+#define PSA_ERROR_HARDWARE_FAILURE      ((psa_status_t)-147)
+#define PSA_ERROR_INVALID_SIGNATURE     ((psa_status_t)-149)
+#define PSA_ERROR_DEPENDENCY_NEEDED     ((psa_status_t)-156)
+#define PSA_ERROR_CURRENTLY_INSTALLING  ((psa_status_t)-157)
+
+#endif /* PSA_ERROR_H */

diff --git a/include/lib/psa/psa_manifest/sid.h b/include/lib/psa/psa_manifest/sid.h
new file mode 100644
index 0000000..947e58f
--- /dev/null
+++ b/include/lib/psa/psa_manifest/sid.h

@@ -0,0 +1,21 @@
+/*
+ * Copyright (c) 2019-2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef PSA_MANIFEST_SID_H
+#define PSA_MANIFEST_SID_H
+
+/******** PSA_SP_INITIAL_ATTESTATION ********/
+#define RSS_ATTESTATION_SERVICE_SID			(0x00000020U)
+#define RSS_ATTESTATION_SERVICE_VERSION			(1U)
+#define RSS_ATTESTATION_SERVICE_HANDLE			(0x40000103U)
+
+/******** PSA_SP_MEASURED_BOOT ********/
+#define RSS_MEASURED_BOOT_SID				(0x000000E0U)
+#define RSS_MEASURED_BOOT_VERSION			(1U)
+#define RSS_MEASURED_BOOT_HANDLE			(0x40000104U)
+
+#endif /* PSA_MANIFEST_SID_H */

diff --git a/include/lib/utils_def.h b/include/lib/utils_def.h
index 7a7012d..198b890 100644
--- a/include/lib/utils_def.h
+++ b/include/lib/utils_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
  * Copyright (c) 2020, NVIDIA Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
@@ -104,6 +104,13 @@
 #define round_down(value, boundary)		\
 	((value) & ~round_boundary(value, boundary))
 
+/**
+ * Helper macro to ensure a value lies on a given boundary.
+ */
+#define is_aligned(value, boundary)			\
+	(round_up((uintptr_t) value, boundary) ==	\
+	 round_down((uintptr_t) value, boundary))
+
 /*
  * Evaluates to 1 if (ptr + inc) overflows, 0 otherwise.
  * Both arguments must be unsigned pointer values (i.e. uintptr_t).

diff --git a/include/plat/arm/common/arm_def.h b/include/plat/arm/common/arm_def.h
index 2af8c11..ab0e4ff 100644
--- a/include/plat/arm/common/arm_def.h
+++ b/include/plat/arm/common/arm_def.h

@@ -79,6 +79,7 @@
  *   - SCP TZC DRAM: If present, DRAM reserved for SCP use
  *   - L1 GPT DRAM: Reserved for L1 GPT if RME is enabled
  *   - REALM DRAM: Reserved for Realm world if RME is enabled
+ *   - TF-A <-> RMM SHARED: Area shared for communication between TF-A and RMM
  *   - AP TZC DRAM: The remaining TZC secured DRAM reserved for AP use
  *
  *              RME enabled(64MB)                RME not enabled(16MB)
@@ -87,11 +88,16 @@
  *              |  AP TZC (~28MB)  |             |  AP TZC (~14MB) |
  *              --------------------             -------------------
  *              |                  |             |                 |
- *              |  REALM (32MB)    |             |  EL3 TZC (2MB)  |
- *              --------------------             -------------------
- *              |                  |             |                 |
- *              |  EL3 TZC (3MB)   |             |    SCP TZC      |
- *              --------------------  0xFFFF_FFFF-------------------
+ *              |   REALM (RMM)    |             |  EL3 TZC (2MB)  |
+ *              |   (32MB - 4KB)   |             -------------------
+ *              --------------------             |                 |
+ *              |                  |             |    SCP TZC      |
+ *              |   TF-A <-> RMM   |  0xFFFF_FFFF-------------------
+ *              |   SHARED (4KB)   |
+ *              --------------------
+ *              |                  |
+ *              |  EL3 TZC (3MB)   |
+ *              --------------------
  *              | L1 GPT + SCP TZC |
  *              |       (~1MB)     |
  *  0xFFFF_FFFF --------------------
@@ -106,12 +112,17 @@
  */
 #define ARM_EL3_TZC_DRAM1_SIZE		UL(0x00300000) /* 3MB */
 #define ARM_L1_GPT_SIZE			UL(0x00100000) /* 1MB */
-#define ARM_REALM_SIZE			UL(0x02000000) /* 32MB */
+
+/* 32MB - ARM_EL3_RMM_SHARED_SIZE */
+#define ARM_REALM_SIZE			(UL(0x02000000) -		\
+						ARM_EL3_RMM_SHARED_SIZE)
+#define ARM_EL3_RMM_SHARED_SIZE		(PAGE_SIZE)    /* 4KB */
 #else
 #define ARM_TZC_DRAM1_SIZE		UL(0x01000000) /* 16MB */
 #define ARM_EL3_TZC_DRAM1_SIZE		UL(0x00200000) /* 2MB */
 #define ARM_L1_GPT_SIZE			UL(0)
 #define ARM_REALM_SIZE			UL(0)
+#define ARM_EL3_RMM_SHARED_SIZE		UL(0)
 #endif /* ENABLE_RME */
 
 #define ARM_SCP_TZC_DRAM1_BASE		(ARM_DRAM1_BASE +		\
@@ -128,13 +139,20 @@
 #define ARM_L1_GPT_END			(ARM_L1_GPT_ADDR_BASE +		\
 					ARM_L1_GPT_SIZE - 1U)
 
-#define ARM_REALM_BASE			(ARM_DRAM1_BASE +		\
-					ARM_DRAM1_SIZE -		\
-					(ARM_SCP_TZC_DRAM1_SIZE +	\
-					ARM_EL3_TZC_DRAM1_SIZE +	\
-					ARM_REALM_SIZE +		\
-					ARM_L1_GPT_SIZE))
+#define ARM_REALM_BASE			(ARM_EL3_RMM_SHARED_BASE -	\
+					 ARM_REALM_SIZE)
+
 #define ARM_REALM_END                   (ARM_REALM_BASE + ARM_REALM_SIZE - 1U)
+
+#define ARM_EL3_RMM_SHARED_BASE		(ARM_DRAM1_BASE +		\
+					 ARM_DRAM1_SIZE -		\
+					(ARM_SCP_TZC_DRAM1_SIZE +	\
+					ARM_L1_GPT_SIZE +		\
+					ARM_EL3_RMM_SHARED_SIZE +	\
+					ARM_EL3_TZC_DRAM1_SIZE))
+
+#define ARM_EL3_RMM_SHARED_END		(ARM_EL3_RMM_SHARED_BASE +	\
+					 ARM_EL3_RMM_SHARED_SIZE - 1U)
 #endif /* ENABLE_RME */
 
 #define ARM_EL3_TZC_DRAM1_BASE		(ARM_SCP_TZC_DRAM1_BASE -	\
@@ -148,6 +166,7 @@
 #define ARM_AP_TZC_DRAM1_SIZE		(ARM_TZC_DRAM1_SIZE -		\
 					(ARM_SCP_TZC_DRAM1_SIZE +	\
 					ARM_EL3_TZC_DRAM1_SIZE +	\
+					ARM_EL3_RMM_SHARED_SIZE +	\
 					ARM_REALM_SIZE +		\
 					ARM_L1_GPT_SIZE))
 #define ARM_AP_TZC_DRAM1_END		(ARM_AP_TZC_DRAM1_BASE +	\
@@ -197,6 +216,7 @@
 #define ARM_NS_DRAM1_BASE		ARM_DRAM1_BASE
 #define ARM_NS_DRAM1_SIZE		(ARM_DRAM1_SIZE -		\
 					 ARM_TZC_DRAM1_SIZE)
+
 #define ARM_NS_DRAM1_END		(ARM_NS_DRAM1_BASE +		\
 					 ARM_NS_DRAM1_SIZE - 1U)
 #ifdef PLAT_ARM_DRAM1_BASE
@@ -284,17 +304,20 @@
 					ARM_EL3_TZC_DRAM1_SIZE,		\
 					MT_MEMORY | MT_RW | EL3_PAS)
 
-#if defined(SPD_spmd)
 #define ARM_MAP_TRUSTED_DRAM	MAP_REGION_FLAT(			\
 					PLAT_ARM_TRUSTED_DRAM_BASE,	\
 					PLAT_ARM_TRUSTED_DRAM_SIZE,	\
 					MT_MEMORY | MT_RW | MT_SECURE)
-#endif
 
 #if ENABLE_RME
+/*
+ * We add the EL3_RMM_SHARED size to RMM mapping to map the region as a block.
+ * Else we end up requiring more pagetables in BL2 for ROMLIB build.
+ */
 #define ARM_MAP_RMM_DRAM	MAP_REGION_FLAT(			\
 					PLAT_ARM_RMM_BASE,		\
-					PLAT_ARM_RMM_SIZE,		\
+					(PLAT_ARM_RMM_SIZE + 		\
+					ARM_EL3_RMM_SHARED_SIZE),	\
 					MT_MEMORY | MT_RW | MT_REALM)
 
 
@@ -303,6 +326,12 @@
 					ARM_L1_GPT_SIZE,		\
 					MT_MEMORY | MT_RW | EL3_PAS)
 
+#define ARM_MAP_EL3_RMM_SHARED_MEM					\
+				MAP_REGION_FLAT(			\
+					ARM_EL3_RMM_SHARED_BASE,	\
+					ARM_EL3_RMM_SHARED_SIZE,	\
+					MT_MEMORY | MT_RW | MT_REALM)
+
 #endif /* ENABLE_RME */
 
 /*
@@ -522,9 +551,20 @@
  * BL2 specific defines.
  ******************************************************************************/
 #if BL2_AT_EL3
+#if ENABLE_PIE
+/*
+ * As the BL31 image size appears to be increased when built with the ENABLE_PIE
+ * option, set BL2 base address to have enough space for BL31 in Trusted SRAM.
+ */
+#define BL2_BASE			(ARM_TRUSTED_SRAM_BASE + \
+					(PLAT_ARM_TRUSTED_SRAM_SIZE >> 1) + \
+					0x3000)
+#else
 /* Put BL2 towards the middle of the Trusted SRAM */
 #define BL2_BASE			(ARM_TRUSTED_SRAM_BASE + \
-						(PLAT_ARM_TRUSTED_SRAM_SIZE >> 1) + 0x2000)
+					(PLAT_ARM_TRUSTED_SRAM_SIZE >> 1) + \
+					0x2000)
+#endif /* ENABLE_PIE */
 #define BL2_LIMIT			(ARM_BL_RAM_BASE + ARM_BL_RAM_SIZE)
 
 #else
@@ -586,6 +626,8 @@
 #if ENABLE_RME
 #define RMM_BASE			(ARM_REALM_BASE)
 #define RMM_LIMIT			(RMM_BASE + ARM_REALM_SIZE)
+#define RMM_SHARED_BASE			(ARM_EL3_RMM_SHARED_BASE)
+#define RMM_SHARED_SIZE			(ARM_EL3_RMM_SHARED_SIZE)
 #endif
 
 #if !defined(__aarch64__) || JUNO_AARCH32_EL3_RUNTIME
@@ -620,7 +662,7 @@
  * Trusted DRAM (if available) or the DRAM region secured by the TrustZone
  * controller.
  */
-# if SPM_MM
+# if SPM_MM || SPMC_AT_EL3
 #  define TSP_SEC_MEM_BASE		(ARM_AP_TZC_DRAM1_BASE + ULL(0x200000))
 #  define TSP_SEC_MEM_SIZE		(ARM_AP_TZC_DRAM1_SIZE - ULL(0x200000))
 #  define BL32_BASE			(ARM_AP_TZC_DRAM1_BASE + ULL(0x200000))
@@ -666,12 +708,13 @@
 
 /*
  * BL32 is mandatory in AArch32. In AArch64, undefine BL32_BASE if there is no
- * SPD and no SPM-MM, as they are the only ones that can be used as BL32.
+ * SPD and no SPM-MM and no SPMC-AT-EL3, as they are the only ones that can be
+ * used as BL32.
  */
 #if defined(__aarch64__) && !JUNO_AARCH32_EL3_RUNTIME
-# if defined(SPD_none) && !SPM_MM
+# if defined(SPD_none) && !SPM_MM && !SPMC_AT_EL3
 #  undef BL32_BASE
-# endif /* defined(SPD_none) && !SPM_MM */
+# endif /* defined(SPD_none) && !SPM_MM || !SPMC_AT_EL3 */
 #endif /* defined(__aarch64__) && !JUNO_AARCH32_EL3_RUNTIME */
 
 /*******************************************************************************

diff --git a/include/plat/arm/common/arm_pas_def.h b/include/plat/arm/common/arm_pas_def.h
index 4fee41b..c199302 100644
--- a/include/plat/arm/common/arm_pas_def.h
+++ b/include/plat/arm/common/arm_pas_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -61,6 +61,10 @@
 #define ARM_PAS_2_BASE			(ARM_PAS_1_BASE + ARM_PAS_1_SIZE)
 #define ARM_PAS_2_SIZE			(ARM_NS_DRAM1_SIZE)
 
+/* Shared area between EL3 and RMM */
+#define ARM_PAS_SHARED_BASE		(ARM_EL3_RMM_SHARED_BASE)
+#define ARM_PAS_SHARED_SIZE		(ARM_EL3_RMM_SHARED_SIZE)
+
 /* Secure TZC region */
 #define ARM_PAS_3_BASE			(ARM_AP_TZC_DRAM1_BASE)
 #define ARM_PAS_3_SIZE			(ARM_AP_TZC_DRAM1_SIZE)
@@ -76,8 +80,13 @@
 							       ARM_PAS_3_SIZE, \
 							       GPT_GPI_SECURE)
 
+/*
+ * REALM and Shared area share the same PAS, so consider them a single
+ * PAS region to configure in GPT.
+ */
 #define ARM_PAS_REALM			GPT_MAP_REGION_GRANULE(ARM_REALM_BASE, \
-							       ARM_REALM_SIZE, \
+							       (ARM_PAS_SHARED_SIZE + \
+								ARM_REALM_SIZE), \
 							       GPT_GPI_REALM)
 
 #define ARM_PAS_EL3_DRAM		GPT_MAP_REGION_GRANULE(ARM_EL3_TZC_DRAM1_BASE, \

diff --git a/include/plat/arm/common/arm_reclaim_init.ld.S b/include/plat/arm/common/arm_reclaim_init.ld.S
index 717f65e..788e9ff 100644
--- a/include/plat/arm/common/arm_reclaim_init.ld.S
+++ b/include/plat/arm/common/arm_reclaim_init.ld.S

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -12,7 +12,7 @@
             . = . + PLATFORM_STACK_SIZE;
             . = ALIGN(PAGE_SIZE);
             __INIT_CODE_START__ = .;
-	    *(*text.init*);
+	    *(*text.init.*);
             __INIT_CODE_END__ = .;
             INIT_CODE_END_ALIGNED = ALIGN(PAGE_SIZE);
         } >RAM

diff --git a/include/plat/arm/common/plat_arm.h b/include/plat/arm/common/plat_arm.h
index 9618700..d060332 100644
--- a/include/plat/arm/common/plat_arm.h
+++ b/include/plat/arm/common/plat_arm.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -57,7 +57,8 @@
 	{ARM_EL3_TZC_DRAM1_BASE, ARM_L1_GPT_END, TZC_REGION_S_RDWR, 0},	    \
 	{ARM_NS_DRAM1_BASE, ARM_NS_DRAM1_END, ARM_TZC_NS_DRAM_S_ACCESS,	    \
 		PLAT_ARM_TZC_NS_DEV_ACCESS},				    \
-	{ARM_REALM_BASE, ARM_REALM_END, ARM_TZC_NS_DRAM_S_ACCESS,	    \
+	/* Realm and Shared area share the same PAS */		    \
+	{ARM_REALM_BASE, ARM_EL3_RMM_SHARED_END, ARM_TZC_NS_DRAM_S_ACCESS,  \
 		PLAT_ARM_TZC_NS_DEV_ACCESS},				    \
 	{ARM_DRAM2_BASE, ARM_DRAM2_END, ARM_TZC_NS_DRAM_S_ACCESS,	    \
 		PLAT_ARM_TZC_NS_DEV_ACCESS}

diff --git a/include/plat/common/platform.h b/include/plat/common/platform.h
index 7664509..184606a 100644
--- a/include/plat/common/platform.h
+++ b/include/plat/common/platform.h

@@ -13,6 +13,9 @@
 #if defined(SPD_spmd)
  #include <services/spm_core_manifest.h>
 #endif
+#if ENABLE_RME
+#include <services/rmm_core_manifest.h>
+#endif
 #if TRNG_SUPPORT
 #include "plat_trng.h"
 #endif
@@ -305,10 +308,14 @@
 /*******************************************************************************
  * Mandatory BL31 functions when ENABLE_RME=1
  ******************************************************************************/
-int plat_get_cca_attest_token(uintptr_t buf, size_t *len,
-			       uintptr_t hash, size_t hash_size);
-int plat_get_cca_realm_attest_key(uintptr_t buf, size_t *len,
-				   unsigned int type);
+#if ENABLE_RME
+int plat_rmmd_get_cca_attest_token(uintptr_t buf, size_t *len,
+				   uintptr_t hash, size_t hash_size);
+int plat_rmmd_get_cca_realm_attest_key(uintptr_t buf, size_t *len,
+				       unsigned int type);
+size_t plat_rmmd_get_el3_rmm_shared_mem(uintptr_t *shared);
+int plat_rmmd_load_manifest(rmm_manifest_t *manifest);
+#endif
 
 /*******************************************************************************
  * Optional BL31 functions (may be overridden)
@@ -347,6 +354,10 @@
 int plat_spm_core_manifest_load(spmc_manifest_attribute_t *manifest,
 				const void *pm_addr);
 #endif
+#if defined(SPMC_AT_EL3)
+int plat_spmc_shmem_datastore_get(uint8_t **datastore, size_t *size);
+#endif
+
 /*******************************************************************************
  * Mandatory BL image load functions(may be overridden).
  ******************************************************************************/

diff --git a/include/services/el3_spmc_ffa_memory.h b/include/services/el3_spmc_ffa_memory.h
new file mode 100644
index 0000000..2037eca
--- /dev/null
+++ b/include/services/el3_spmc_ffa_memory.h

@@ -0,0 +1,258 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef EL3_SPMC_FFA_MEM_H
+#define EL3_SPMC_FFA_MEM_H
+
+#include <assert.h>
+
+/*
+ * Subset of Arm Firmware Framework for Armv8-A
+ * (https://developer.arm.com/docs/den0077/a) needed for shared memory.
+ */
+
+/**
+ * typedef ffa_endpoint_id16_t - Endpoint ID
+ *
+ * Current implementation only supports VM IDs. FF-A spec also support stream
+ * endpoint ids.
+ */
+typedef uint16_t ffa_endpoint_id16_t;
+
+/**
+ * struct ffa_cons_mrd - Constituent memory region descriptor
+ * @address:
+ *         Start address of contiguous memory region. Must be 4K page aligned.
+ * @page_count:
+ *         Number of 4K pages in region.
+ * @reserved_12_15:
+ *         Reserve bytes 12-15 to pad struct size to 16 bytes.
+ */
+struct ffa_cons_mrd {
+	uint64_t address;
+	uint32_t page_count;
+	uint32_t reserved_12_15;
+};
+CASSERT(sizeof(struct ffa_cons_mrd) == 16, assert_ffa_cons_mrd_size_mismatch);
+
+/**
+ * struct ffa_comp_mrd - Composite memory region descriptor
+ * @total_page_count:
+ *         Number of 4k pages in memory region. Must match sum of
+ *         @address_range_array[].page_count.
+ * @address_range_count:
+ *         Number of entries in @address_range_array.
+ * @reserved_8_15:
+ *         Reserve bytes 8-15 to pad struct size to 16 byte alignment and
+ *         make @address_range_array 16 byte aligned.
+ * @address_range_array:
+ *         Array of &struct ffa_cons_mrd entries.
+ */
+struct ffa_comp_mrd {
+	uint32_t total_page_count;
+	uint32_t address_range_count;
+	uint64_t reserved_8_15;
+	struct ffa_cons_mrd address_range_array[];
+};
+CASSERT(sizeof(struct ffa_comp_mrd) == 16, assert_ffa_comp_mrd_size_mismatch);
+
+/**
+ * typedef ffa_mem_attr8_t - Memory region attributes v1.0.
+ * typedef ffa_mem_attr16_t - Memory region attributes v1.1.
+ *
+ * * @FFA_MEM_ATTR_NS_BIT:
+ *     Memory security state.
+ * * @FFA_MEM_ATTR_DEVICE_NGNRNE:
+ *     Device-nGnRnE.
+ * * @FFA_MEM_ATTR_DEVICE_NGNRE:
+ *     Device-nGnRE.
+ * * @FFA_MEM_ATTR_DEVICE_NGRE:
+ *     Device-nGRE.
+ * * @FFA_MEM_ATTR_DEVICE_GRE:
+ *     Device-GRE.
+ * * @FFA_MEM_ATTR_NORMAL_MEMORY_UNCACHED
+ *     Normal memory. Non-cacheable.
+ * * @FFA_MEM_ATTR_NORMAL_MEMORY_CACHED_WB
+ *     Normal memory. Write-back cached.
+ * * @FFA_MEM_ATTR_NON_SHAREABLE
+ *     Non-shareable. Combine with FFA_MEM_ATTR_NORMAL_MEMORY_*.
+ * * @FFA_MEM_ATTR_OUTER_SHAREABLE
+ *     Outer Shareable. Combine with FFA_MEM_ATTR_NORMAL_MEMORY_*.
+ * * @FFA_MEM_ATTR_INNER_SHAREABLE
+ *     Inner Shareable. Combine with FFA_MEM_ATTR_NORMAL_MEMORY_*.
+ */
+typedef uint8_t ffa_mem_attr8_t;
+typedef uint16_t ffa_mem_attr16_t;
+#define FFA_MEM_ATTR_NS_BIT			(0x1U << 6)
+#define FFA_MEM_ATTR_DEVICE_NGNRNE		((1U << 4) | (0x0U << 2))
+#define FFA_MEM_ATTR_DEVICE_NGNRE		((1U << 4) | (0x1U << 2))
+#define FFA_MEM_ATTR_DEVICE_NGRE		((1U << 4) | (0x2U << 2))
+#define FFA_MEM_ATTR_DEVICE_GRE			((1U << 4) | (0x3U << 2))
+#define FFA_MEM_ATTR_NORMAL_MEMORY_UNCACHED	((2U << 4) | (0x1U << 2))
+#define FFA_MEM_ATTR_NORMAL_MEMORY_CACHED_WB	((2U << 4) | (0x3U << 2))
+#define FFA_MEM_ATTR_NON_SHAREABLE		(0x0U << 0)
+#define FFA_MEM_ATTR_OUTER_SHAREABLE		(0x2U << 0)
+#define FFA_MEM_ATTR_INNER_SHAREABLE		(0x3U << 0)
+
+/**
+ * typedef ffa_mem_perm8_t - Memory access permissions
+ *
+ * * @FFA_MEM_ATTR_RO
+ *     Request or specify read-only mapping.
+ * * @FFA_MEM_ATTR_RW
+ *     Request or allow read-write mapping.
+ * * @FFA_MEM_PERM_NX
+ *     Deny executable mapping.
+ * * @FFA_MEM_PERM_X
+ *     Request executable mapping.
+ */
+typedef uint8_t ffa_mem_perm8_t;
+#define FFA_MEM_PERM_RO		(1U << 0)
+#define FFA_MEM_PERM_RW		(1U << 1)
+#define FFA_MEM_PERM_NX		(1U << 2)
+#define FFA_MEM_PERM_X		(1U << 3)
+
+/**
+ * typedef ffa_mem_flag8_t - Endpoint memory flags
+ *
+ * * @FFA_MEM_FLAG_NON_RETRIEVAL_BORROWER
+ *     Non-retrieval Borrower. Memory region must not be or was not retrieved on
+ *     behalf of this endpoint.
+ */
+typedef uint8_t ffa_mem_flag8_t;
+#define FFA_MEM_FLAG_NON_RETRIEVAL_BORROWER	(1U << 0)
+
+/**
+ * typedef ffa_mtd_flag32_t - Memory transaction descriptor flags
+ *
+ * * @FFA_MTD_FLAG_ZERO_MEMORY
+ *     Zero memory after unmapping from sender (must be 0 for share).
+ * * @FFA_MTD_FLAG_TIME_SLICING
+ *     Not supported by this implementation.
+ * * @FFA_MTD_FLAG_ZERO_MEMORY_AFTER_RELINQUISH
+ *     Zero memory after unmapping from borrowers (must be 0 for share).
+ * * @FFA_MTD_FLAG_TYPE_MASK
+ *     Bit-mask to extract memory management transaction type from flags.
+ * * @FFA_MTD_FLAG_TYPE_SHARE_MEMORY
+ *     Share memory transaction flag.
+ *     Used by @SMC_FC_FFA_MEM_RETRIEVE_RESP to indicate that memory came from
+ *     @SMC_FC_FFA_MEM_SHARE and by @SMC_FC_FFA_MEM_RETRIEVE_REQ to specify that
+ *     it must have.
+ * * @FFA_MTD_FLAG_ADDRESS_RANGE_ALIGNMENT_HINT_MASK
+ *     Not supported by this implementation.
+ */
+typedef uint32_t ffa_mtd_flag32_t;
+#define FFA_MTD_FLAG_ZERO_MEMORY			(1U << 0)
+#define FFA_MTD_FLAG_TIME_SLICING			(1U << 1)
+#define FFA_MTD_FLAG_ZERO_MEMORY_AFTER_RELINQUISH	(1U << 2)
+#define FFA_MTD_FLAG_TYPE_MASK				(3U << 3)
+#define FFA_MTD_FLAG_TYPE_SHARE_MEMORY			(1U << 3)
+#define FFA_MTD_FLAG_TYPE_LEND_MEMORY			(1U << 4)
+#define FFA_MTD_FLAG_ADDRESS_RANGE_ALIGNMENT_HINT_MASK	(0x1FU << 5)
+
+/**
+ * struct ffa_mapd - Memory access permissions descriptor
+ * @endpoint_id:
+ *         Endpoint id that @memory_access_permissions and @flags apply to.
+ *         (&typedef ffa_endpoint_id16_t).
+ * @memory_access_permissions:
+ *         FFA_MEM_PERM_* values or'ed together (&typedef ffa_mem_perm8_t).
+ * @flags:
+ *         FFA_MEM_FLAG_* values or'ed together (&typedef ffa_mem_flag8_t).
+ */
+struct ffa_mapd {
+	ffa_endpoint_id16_t endpoint_id;
+	ffa_mem_perm8_t memory_access_permissions;
+	ffa_mem_flag8_t flags;
+};
+CASSERT(sizeof(struct ffa_mapd) == 4, assert_ffa_mapd_size_mismatch);
+
+/**
+ * struct ffa_emad_v1_0 - Endpoint memory access descriptor.
+ * @mapd:  &struct ffa_mapd.
+ * @comp_mrd_offset:
+ *         Offset of &struct ffa_comp_mrd from start of &struct ffa_mtd_v1_0.
+ * @reserved_8_15:
+ *         Reserved bytes 8-15. Must be 0.
+ */
+struct ffa_emad_v1_0 {
+	struct ffa_mapd mapd;
+	uint32_t comp_mrd_offset;
+	uint64_t reserved_8_15;
+};
+CASSERT(sizeof(struct ffa_emad_v1_0) == 16, assert_ffa_emad_v1_0_size_mismatch);
+
+/**
+ * struct ffa_mtd_v1_0 - Memory transaction descriptor.
+ * @sender_id:
+ *         Sender endpoint id.
+ * @memory_region_attributes:
+ *         FFA_MEM_ATTR_* values or'ed together (&typedef ffa_mem_attr8_t).
+ * @reserved_3:
+ *         Reserved bytes 3. Must be 0.
+ * @flags:
+ *         FFA_MTD_FLAG_* values or'ed together (&typedef ffa_mtd_flag32_t).
+ * @handle:
+ *         Id of shared memory object. Must be 0 for MEM_SHARE or MEM_LEND.
+ * @tag:   Client allocated tag. Must match original value.
+ * @reserved_24_27:
+ *         Reserved bytes 24-27. Must be 0.
+ * @emad_count:
+ *         Number of entries in @emad.
+ * @emad:
+ *         Endpoint memory access descriptor array (see @struct ffa_emad_v1_0).
+ */
+struct ffa_mtd_v1_0 {
+	ffa_endpoint_id16_t sender_id;
+	ffa_mem_attr8_t memory_region_attributes;
+	uint8_t reserved_3;
+	ffa_mtd_flag32_t flags;
+	uint64_t handle;
+	uint64_t tag;
+	uint32_t reserved_24_27;
+	uint32_t emad_count;
+	struct ffa_emad_v1_0 emad[];
+};
+CASSERT(sizeof(struct ffa_mtd_v1_0) == 32, assert_ffa_mtd_size_v1_0_mismatch);
+
+/**
+ * struct ffa_mtd - Memory transaction descriptor for FF-A v1.1.
+ * @sender_id:
+ *         Sender endpoint id.
+ * @memory_region_attributes:
+ *         FFA_MEM_ATTR_* values or'ed together (&typedef ffa_mem_attr16_t).
+ * @flags:
+ *         FFA_MTD_FLAG_* values or'ed together (&typedef ffa_mtd_flag32_t).
+ * @handle:
+ *         Id of shared memory object. Must be 0 for MEM_SHARE or MEM_LEND.
+ * @tag:   Client allocated tag. Must match original value.
+ * @emad_size:
+ *         Size of the emad descriptor.
+ * @emad_count:
+ *         Number of entries in the emad array.
+ * @emad_offset:
+ *         Offset from the beginning of the descriptor to the location of the
+ *         memory access descriptor array (see @struct ffa_emad_v1_0).
+ * @reserved_36_39:
+ *         Reserved bytes 36-39. Must be 0.
+ * @reserved_40_47:
+ *         Reserved bytes 44-47. Must be 0.
+ */
+struct ffa_mtd {
+	ffa_endpoint_id16_t sender_id;
+	ffa_mem_attr16_t memory_region_attributes;
+	ffa_mtd_flag32_t flags;
+	uint64_t handle;
+	uint64_t tag;
+	uint32_t emad_size;
+	uint32_t emad_count;
+	uint32_t emad_offset;
+	uint32_t reserved_36_39;
+	uint64_t reserved_40_47;
+};
+CASSERT(sizeof(struct ffa_mtd) == 48, assert_ffa_mtd_size_mismatch);
+
+#endif /* EL3_SPMC_FFA_MEM_H */

diff --git a/include/services/el3_spmc_logical_sp.h b/include/services/el3_spmc_logical_sp.h
new file mode 100644
index 0000000..7ec9958
--- /dev/null
+++ b/include/services/el3_spmc_logical_sp.h

@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef EL3_SP_H
+#define EL3_SP_H
+
+#include <common/bl_common.h>
+#include <lib/cassert.h>
+
+/*******************************************************************************
+ * Structure definition, typedefs & constants for the Logical SPs.
+ ******************************************************************************/
+
+typedef uint64_t (*direct_msg_handler)(uint32_t smc_fid, bool secure_origin,
+				       uint64_t x1, uint64_t x2, uint64_t x3,
+				       uint64_t x4, void *cookie, void *handle,
+				       uint64_t flags);
+
+/* Prototype for logical partition initializing function. */
+typedef int32_t (*ffa_partition_init_t)(void);
+
+/* Logical Partition Descriptor. */
+struct el3_lp_desc {
+	ffa_partition_init_t init;
+	uint16_t sp_id;
+	uint32_t properties;
+	uint32_t uuid[4];  /* Little Endian. */
+	direct_msg_handler direct_req;
+	const char *debug_name;
+};
+
+/* Convenience macro to declare a logical partition descriptor. */
+#define DECLARE_LOGICAL_PARTITION(_name, _init, _sp_id, _uuid, _properties, \
+				  _direct_req)				    \
+	static const struct el3_lp_desc __partition_desc_ ## _name	    \
+		__section("el3_lp_descs") __used = {			    \
+			.debug_name = #_name,				    \
+			.init = (_init),				    \
+			.sp_id = (_sp_id),				    \
+			.uuid = _uuid,					    \
+			.properties = (_properties),			    \
+			.direct_req = (_direct_req),			    \
+		}
+
+
+/*******************************************************************************
+ * Function & variable prototypes.
+ ******************************************************************************/
+int el3_sp_desc_validate(void);
+uintptr_t handle_el3_sp(uint32_t smc_fid, void *cookie, void *handle,
+						unsigned int flags);
+IMPORT_SYM(uintptr_t, __EL3_LP_DESCS_START__,	EL3_LP_DESCS_START);
+IMPORT_SYM(uintptr_t, __EL3_LP_DESCS_END__,	EL3_LP_DESCS_END);
+
+#define EL3_LP_DESCS_COUNT ((EL3_LP_DESCS_END - EL3_LP_DESCS_START) \
+			  / sizeof(struct el3_lp_desc))
+
+#endif /* EL3_SP_H */

diff --git a/include/services/ffa_svc.h b/include/services/ffa_svc.h
index d3fb012..da016fd 100644
--- a/include/services/ffa_svc.h
+++ b/include/services/ffa_svc.h

@@ -38,6 +38,7 @@
 #define FFA_VERSION_MINOR_SHIFT		0
 #define FFA_VERSION_MINOR_MASK		U(0xFFFF)
 #define FFA_VERSION_BIT31_MASK 		U(0x1u << 31)
+#define FFA_VERSION_MASK		U(0xFFFFFFFF)
 
 
 #define MAKE_FFA_VERSION(major, minor) 	\
@@ -55,6 +56,19 @@
 	(((blk) & FFA_MSG_SEND_ATTRS_BLK_MASK) \
 	<< FFA_MSG_SEND_ATTRS_BLK_SHIFT)
 
+/* Defines for FF-A framework messages exchanged using direct messages. */
+#define FFA_FWK_MSG_BIT		BIT(31)
+#define FFA_FWK_MSG_MASK	0xFF
+#define FFA_FWK_MSG_PSCI	U(0x0)
+
+/* Defines for FF-A power management messages framework messages. */
+#define FFA_PM_MSG_WB_REQ	U(0x1) /* Warm boot request. */
+#define FFA_PM_MSG_PM_RESP	U(0x2) /* Response to PSCI or warmboot req. */
+
+/* FF-A warm boot types. */
+#define FFA_WB_TYPE_S2RAM	0x0
+#define FFA_WB_TYPE_NOTS2RAM	0x1
+
 /* Get FFA fastcall std FID from function number */
 #define FFA_FID(smc_cc, func_num)			\
 		((SMC_TYPE_FAST << FUNCID_TYPE_SHIFT) |	\
@@ -87,6 +101,8 @@
 #define FFA_FNUM_MEM_RETRIEVE_RESP		U(0x75)
 #define FFA_FNUM_MEM_RELINQUISH			U(0x76)
 #define FFA_FNUM_MEM_RECLAIM			U(0x77)
+#define FFA_FNUM_MEM_FRAG_RX			U(0x7A)
+#define FFA_FNUM_MEM_FRAG_TX			U(0x7B)
 #define FFA_FNUM_NORMAL_WORLD_RESUME		U(0x7C)
 
 /* FF-A v1.1 */
@@ -142,6 +158,8 @@
 #define FFA_NOTIFICATION_GET 	FFA_FID(SMC_32, FFA_FNUM_NOTIFICATION_GET)
 #define FFA_NOTIFICATION_INFO_GET \
 	FFA_FID(SMC_32, FFA_FNUM_NOTIFICATION_INFO_GET)
+#define FFA_MEM_FRAG_RX	FFA_FID(SMC_32, FFA_FNUM_MEM_FRAG_RX)
+#define FFA_MEM_FRAG_TX	FFA_FID(SMC_32, FFA_FNUM_MEM_FRAG_TX)
 #define FFA_SPM_ID_GET		FFA_FID(SMC_32, FFA_FNUM_SPM_ID_GET)
 #define FFA_NORMAL_WORLD_RESUME	FFA_FID(SMC_32, FFA_FNUM_NORMAL_WORLD_RESUME)
 
@@ -164,6 +182,13 @@
 	FFA_FID(SMC_64, FFA_FNUM_NOTIFICATION_INFO_GET)
 
 /*
+ * FF-A partition properties values.
+ */
+#define FFA_PARTITION_DIRECT_REQ_RECV	U(1 << 0)
+#define FFA_PARTITION_DIRECT_REQ_SEND	U(1 << 1)
+#define FFA_PARTITION_INDIRECT_MSG	U(1 << 2)
+
+/*
  * Reserve a special value for traffic targeted to the Hypervisor or SPM.
  */
 #define FFA_TARGET_INFO_MBZ		U(0x0)
@@ -188,6 +213,11 @@
 #define SPMC_SECURE_ID_SHIFT			U(15)
 
 /*
+ * Partition Count Flag in FFA_PARTITION_INFO_GET.
+ */
+#define FFA_PARTITION_INFO_GET_COUNT_FLAG_MASK U(1 << 0)
+
+/*
  * Mask for source and destination endpoint id in
  * a direct message request/response.
  */
@@ -241,4 +271,71 @@
 	return !ffa_is_secure_world_id(id);
 }
 
+
+/******************************************************************************
+ * Boot information protocol as per the FF-A v1.1 spec.
+ *****************************************************************************/
+#define FFA_INIT_DESC_SIGNATURE			0x00000FFA
+
+/* Boot information type. */
+#define FFA_BOOT_INFO_TYPE_STD			U(0x0)
+#define FFA_BOOT_INFO_TYPE_IMPL			U(0x1)
+
+#define FFA_BOOT_INFO_TYPE_MASK			U(0x1)
+#define FFA_BOOT_INFO_TYPE_SHIFT		U(0x7)
+#define FFA_BOOT_INFO_TYPE(type)		\
+	(((type) & FFA_BOOT_INFO_TYPE_MASK)	\
+	<< FFA_BOOT_INFO_TYPE_SHIFT)
+
+/* Boot information identifier. */
+#define FFA_BOOT_INFO_TYPE_ID_FDT		U(0x0)
+#define FFA_BOOT_INFO_TYPE_ID_HOB		U(0x1)
+
+#define FFA_BOOT_INFO_TYPE_ID_MASK		U(0x3F)
+#define FFA_BOOT_INFO_TYPE_ID_SHIFT		U(0x0)
+#define FFA_BOOT_INFO_TYPE_ID(type)		\
+	(((type) & FFA_BOOT_INFO_TYPE_ID_MASK)	\
+	<< FFA_BOOT_INFO_TYPE_ID_SHIFT)
+
+/* Format of Flags Name field. */
+#define FFA_BOOT_INFO_FLAG_NAME_STRING		U(0x0)
+#define FFA_BOOT_INFO_FLAG_NAME_UUID		U(0x1)
+
+#define FFA_BOOT_INFO_FLAG_NAME_MASK		U(0x3)
+#define FFA_BOOT_INFO_FLAG_NAME_SHIFT		U(0x0)
+#define FFA_BOOT_INFO_FLAG_NAME(type)		\
+	(((type) & FFA_BOOT_INFO_FLAG_NAME_MASK)\
+	<< FFA_BOOT_INFO_FLAG_NAME_SHIFT)
+
+/* Format of Flags Contents field. */
+#define FFA_BOOT_INFO_FLAG_CONTENT_ADR		U(0x0)
+#define FFA_BOOT_INFO_FLAG_CONTENT_VAL		U(0x1)
+
+#define FFA_BOOT_INFO_FLAG_CONTENT_MASK		U(0x1)
+#define FFA_BOOT_INFO_FLAG_CONTENT_SHIFT	U(0x2)
+#define FFA_BOOT_INFO_FLAG_CONTENT(content)		\
+	(((content) & FFA_BOOT_INFO_FLAG_CONTENT_MASK)	\
+	<< FFA_BOOT_INFO_FLAG_CONTENT_SHIFT)
+
+/* Boot information descriptor. */
+struct ffa_boot_info_desc {
+	uint8_t name[16];
+	uint8_t type;
+	uint8_t reserved;
+	uint16_t flags;
+	uint32_t size_boot_info;
+	uint64_t content;
+};
+
+/* Boot information header. */
+struct ffa_boot_info_header {
+	uint32_t signature; /* 0xFFA */
+	uint32_t version;
+	uint32_t size_boot_info_blob;
+	uint32_t size_boot_info_desc;
+	uint32_t count_boot_info_desc;
+	uint32_t offset_boot_info_desc;
+	uint64_t reserved;
+};
+
 #endif /* FFA_SVC_H */

diff --git a/include/services/rmm_core_manifest.h b/include/services/rmm_core_manifest.h
new file mode 100644
index 0000000..2f25858
--- /dev/null
+++ b/include/services/rmm_core_manifest.h

@@ -0,0 +1,49 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef RMM_CORE_MANIFEST_H
+#define RMM_CORE_MANIFEST_H
+
+#include <assert.h>
+#include <stddef.h>
+#include <stdint.h>
+
+#include <lib/cassert.h>
+
+#define RMMD_MANIFEST_VERSION_MAJOR		U(0)
+#define RMMD_MANIFEST_VERSION_MINOR		U(1)
+
+/*
+ * Manifest version encoding:
+ *	- Bit[31] RES0
+ *	- Bits [30:16] Major version
+ *	- Bits [15:0] Minor version
+ */
+#define _RMMD_MANIFEST_VERSION(_major, _minor)				\
+	((((_major) & 0x7FFF) << 16) | ((_minor) & 0xFFFF))
+
+#define RMMD_MANIFEST_VERSION _RMMD_MANIFEST_VERSION(			\
+				RMMD_MANIFEST_VERSION_MAJOR,		\
+				RMMD_MANIFEST_VERSION_MINOR)
+
+#define RMMD_GET_MANIFEST_VERSION_MAJOR(_version)			\
+	((_version >> 16) & 0x7FFF)
+
+#define RMMD_GET_MANIFEST_VERSION_MINOR(_version)			\
+	(_version & 0xFFFF)
+
+/* Boot manifest core structure as per v0.1 */
+typedef struct rmm_manifest {
+	uint32_t version;	/* Manifest version */
+	uintptr_t plat_data;	/* Manifest platform data */
+} rmm_manifest_t;
+
+CASSERT(offsetof(rmm_manifest_t, version) == 0,
+				rmm_manifest_t_version_unaligned);
+CASSERT(offsetof(rmm_manifest_t, plat_data) == 8,
+				rmm_manifest_t_plat_data_unaligned);
+
+#endif /* RMM_CORE_MANIFEST_H */

diff --git a/include/services/rmmd_svc.h b/include/services/rmmd_svc.h
index 2fbdddd..6adc5f3 100644
--- a/include/services/rmmd_svc.h
+++ b/include/services/rmmd_svc.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,17 +10,18 @@
 #include <lib/smccc.h>
 #include <lib/utils_def.h>
 
-/* Construct RMM fastcall std FID from function number */
-#define RMM_FID(smc_cc, func_num)			\
-	((SMC_TYPE_FAST << FUNCID_TYPE_SHIFT)	|	\
-	((smc_cc) << FUNCID_CC_SHIFT)		|	\
-	(OEN_STD_START << FUNCID_OEN_SHIFT)	|	\
-	((func_num) << FUNCID_NUM_SHIFT))
-
-/* The macros below are used to identify RMI calls from the SMC function ID */
+/* STD calls FNUM Min/Max ranges */
 #define RMI_FNUM_MIN_VALUE	U(0x150)
 #define RMI_FNUM_MAX_VALUE	U(0x18F)
 
+/* Construct RMI fastcall std FID from offset */
+#define SMC64_RMI_FID(_offset)					  \
+	((SMC_TYPE_FAST << FUNCID_TYPE_SHIFT)			| \
+	 (SMC_64 << FUNCID_CC_SHIFT)				| \
+	 (OEN_STD_START << FUNCID_OEN_SHIFT)			| \
+	 (((RMI_FNUM_MIN_VALUE + (_offset)) & FUNCID_NUM_MASK)	  \
+	  << FUNCID_NUM_SHIFT))
+
 #define is_rmi_fid(fid) __extension__ ({		\
 	__typeof__(fid) _fid = (fid);			\
 	((GET_SMC_NUM(_fid) >= RMI_FNUM_MIN_VALUE) &&	\
@@ -31,15 +32,25 @@
 	 ((_fid & 0x00FE0000) == 0U)); })
 
 /*
- * RMI_FNUM_REQ_COMPLETE is the only function in the RMI rnage that originates
+ * RMI_FNUM_REQ_COMPLETE is the only function in the RMI range that originates
  * from the Realm world and is handled by the RMMD. The RMI functions are
  * always invoked by the Normal world, forwarded by RMMD and handled by the
- * RMM
+ * RMM.
  */
-#define RMI_FNUM_REQ_COMPLETE		U(0x18F)
-#define RMMD_RMI_REQ_COMPLETE		RMM_FID(SMC_64, RMI_FNUM_REQ_COMPLETE)
+					/* 0x18F */
+#define RMMD_RMI_REQ_COMPLETE		SMC64_RMI_FID(U(0x3F))
 
-/* The SMC in the range 0x8400 0190 - 0x8400 01AF are reserved for RSIs.*/
+/* RMM_BOOT_COMPLETE arg0 error codes */
+#define E_RMM_BOOT_SUCCESS				(0)
+#define E_RMM_BOOT_UNKNOWN				(-1)
+#define E_RMM_BOOT_VERSION_MISMATCH			(-2)
+#define E_RMM_BOOT_CPUS_OUT_OF_RANGE			(-3)
+#define E_RMM_BOOT_CPU_ID_OUT_OF_RANGE			(-4)
+#define E_RMM_BOOT_INVALID_SHARED_BUFFER		(-5)
+#define E_RMM_BOOT_MANIFEST_VERSION_NOT_SUPPORTED	(-6)
+#define E_RMM_BOOT_MANIFEST_DATA_ERROR			(-7)
+
+/* The SMC in the range 0x8400 0191 - 0x8400 01AF are reserved for RSIs.*/
 
 /*
  * EL3 - RMM SMCs used for requesting RMMD services. These SMCs originate in Realm
@@ -50,6 +61,14 @@
 #define RMMD_EL3_FNUM_MIN_VALUE		U(0x1B0)
 #define RMMD_EL3_FNUM_MAX_VALUE		U(0x1CF)
 
+/* Construct RMM_EL3 fastcall std FID from offset */
+#define SMC64_RMMD_EL3_FID(_offset)					  \
+	((SMC_TYPE_FAST << FUNCID_TYPE_SHIFT)				| \
+	 (SMC_64 << FUNCID_CC_SHIFT)					| \
+	 (OEN_STD_START << FUNCID_OEN_SHIFT)				| \
+	 (((RMMD_EL3_FNUM_MIN_VALUE + (_offset)) & FUNCID_NUM_MASK)	  \
+	  << FUNCID_NUM_SHIFT))
+
 /* The macros below are used to identify GTSI calls from the SMC function ID */
 #define is_rmmd_el3_fid(fid) __extension__ ({		\
 	__typeof__(fid) _fid = (fid);			\
@@ -60,37 +79,17 @@
 	(GET_SMC_OEN(_fid) == OEN_STD_START)        &&	\
 	((_fid & 0x00FE0000) == 0U)); })
 
-/* RMMD Service Function NUmbers */
-#define GTSI_DELEGATE			U(0x1B0)
-#define GTSI_UNDELEGATE			U(0x1B1)
-#define ATTEST_GET_REALM_KEY		U(0x1B2)
-#define ATTEST_GET_PLAT_TOKEN		U(0x1B3)
-
-#define RMMD_GTSI_DELEGATE		RMM_FID(SMC_64, GTSI_DELEGATE)
-#define RMMD_GTSI_UNDELEGATE		RMM_FID(SMC_64, GTSI_UNDELEGATE)
+					/* 0x1B0 - 0x1B1 */
+#define RMMD_GTSI_DELEGATE		SMC64_RMMD_EL3_FID(U(0))
+#define RMMD_GTSI_UNDELEGATE		SMC64_RMMD_EL3_FID(U(1))
 
 /* Return error codes from RMM-EL3 SMCs */
-#define RMMD_OK				0
-#define RMMD_ERR_BAD_ADDR		-2
-#define RMMD_ERR_BAD_PAS		-3
-#define RMMD_ERR_NOMEM			-4
-#define RMMD_ERR_INVAL			-5
-#define RMMD_ERR_UNK			-6
-
-/*
- * Retrieve Platform token from EL3.
- * The arguments to this SMC are :
- *    arg0 - Function ID.
- *    arg1 - Platform attestation token buffer Physical address. (The challenge
- *           object is passed in this buffer.)
- *    arg2 - Platform attestation token buffer size (in bytes).
- *    arg3 - Challenge object size (in bytes). It has be one of the defined SHA hash
- *           sizes.
- * The return arguments are :
- *    ret0 - Status / error.
- *    ret1 - Size of the platform token if successful.
- */
-#define RMMD_ATTEST_GET_PLAT_TOKEN	RMM_FID(SMC_64, ATTEST_GET_PLAT_TOKEN)
+#define E_RMM_OK			 0
+#define E_RMM_UNK			-1
+#define E_RMM_BAD_ADDR			-2
+#define E_RMM_BAD_PAS			-3
+#define E_RMM_NOMEM			-4
+#define E_RMM_INVAL			-5
 
 /* Acceptable SHA sizes for Challenge object */
 #define SHA256_DIGEST_SIZE	32U
@@ -110,11 +109,58 @@
  *    ret0 - Status / error.
  *    ret1 - Size of the realm attestation key if successful.
  */
+					/* 0x1B2 */
+#define RMMD_ATTEST_GET_REALM_KEY	SMC64_RMMD_EL3_FID(U(2))
+
+/*
+ * Retrieve Platform token from EL3.
+ * The arguments to this SMC are :
+ *    arg0 - Function ID.
+ *    arg1 - Platform attestation token buffer Physical address. (The challenge
+ *           object is passed in this buffer.)
+ *    arg2 - Platform attestation token buffer size (in bytes).
+ *    arg3 - Challenge object size (in bytes). It has to be one of the defined
+ *           SHA hash sizes.
+ * The return arguments are :
+ *    ret0 - Status / error.
+ *    ret1 - Size of the platform token if successful.
+ */
-#define RMMD_ATTEST_GET_REALM_KEY	RMM_FID(SMC_64, ATTEST_GET_REALM_KEY)
+					/* 0x1B3 */
+#define RMMD_ATTEST_GET_PLAT_TOKEN	SMC64_RMMD_EL3_FID(U(3))
 
 /* ECC Curve types for attest key generation */
 #define ATTEST_KEY_CURVE_ECC_SECP384R1		0
 
+/*
+ * RMM_BOOT_COMPLETE originates on RMM when the boot finishes (either cold
+ * or warm boot). This is handled by the RMM-EL3 interface SMC handler.
+ *
+ * RMM_BOOT_COMPLETE FID is located at the end of the available range.
+ */
+					 /* 0x1CF */
+#define RMM_BOOT_COMPLETE		SMC64_RMMD_EL3_FID(U(0x1F))
+
+/*
+ * The major version number of the RMM Boot Interface implementation.
+ * Increase this whenever the semantics of the boot arguments change making it
+ * backwards incompatible.
+ */
+#define RMM_EL3_IFC_VERSION_MAJOR	(U(0))
+
+/*
+ * The minor version number of the RMM Boot Interface implementation.
+ * Increase this when a bug is fixed, or a feature is added without
+ * breaking compatibility.
+ */
+#define RMM_EL3_IFC_VERSION_MINOR	(U(1))
+
+#define RMM_EL3_INTERFACE_VERSION				\
+	(((RMM_EL3_IFC_VERSION_MAJOR << 16) & 0x7FFFF) |	\
+		RMM_EL3_IFC_VERSION_MINOR)
+
+#define RMM_EL3_IFC_VERSION_GET_MAJOR(_version) (((_version) >> 16) \
+								& 0x7FFF)
+#define RMM_EL3_IFC_VERSION_GET_MAJOR_MINOR(_version) ((_version) & 0xFFFF)
 
 #ifndef __ASSEMBLER__
 #include <stdint.h>

diff --git a/include/services/trp/platform_trp.h b/include/services/trp/platform_trp.h
index b34da85..1c963c8 100644
--- a/include/services/trp/platform_trp.h
+++ b/include/services/trp/platform_trp.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,9 +7,11 @@
 #ifndef PLATFORM_TRP_H
 #define PLATFORM_TRP_H
 
+#include <services/rmm_core_manifest.h>
+
 /*******************************************************************************
  * Mandatory TRP functions (only if platform contains a TRP)
  ******************************************************************************/
-void trp_early_platform_setup(void);
+void trp_early_platform_setup(rmm_manifest_t *manifest);
 
 #endif /* PLATFORM_TRP_H */

diff --git a/include/services/trp/trp_helpers.h b/include/services/trp/trp_helpers.h
new file mode 100644
index 0000000..8e786e2
--- /dev/null
+++ b/include/services/trp/trp_helpers.h

@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef TRP_HELPERS_H
+#define TRP_HELPERS_H
+
+/* Definitions to help the assembler access the SMC/ERET args structure */
+#define TRP_ARGS_SIZE		TRP_ARGS_END
+#define TRP_ARG0		0x0
+#define TRP_ARG1		0x8
+#define TRP_ARG2		0x10
+#define TRP_ARG3		0x18
+#define TRP_ARG4		0x20
+#define TRP_ARG5		0x28
+#define TRP_ARG6		0x30
+#define TRP_ARG7		0x38
+#define TRP_ARGS_END		0x40
+
+#ifndef __ASSEMBLER__
+
+#include <platform_def.h>
+
+/* Data structure to hold SMC arguments */
+typedef struct trp_args {
+	uint64_t regs[TRP_ARGS_END >> 3];
+} __aligned(CACHE_WRITEBACK_GRANULE) trp_args_t;
+
+trp_args_t *set_smc_args(uint64_t arg0,
+			 uint64_t arg1,
+			 uint64_t arg2,
+			 uint64_t arg3,
+			 uint64_t arg4,
+			 uint64_t arg5,
+			 uint64_t arg6,
+			 uint64_t arg7);
+
+__dead2 void trp_boot_abort(uint64_t err);
+
+#endif /* __ASSEMBLER __ */
+#endif /* TRP_HELPERS_H */

diff --git a/include/tools_share/cca_oid.h b/include/tools_share/cca_oid.h
new file mode 100644
index 0000000..e586b8c
--- /dev/null
+++ b/include/tools_share/cca_oid.h

@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CCA_OID_H
+#define CCA_OID_H
+
+/* Reuse the Object IDs defined by TBBR for certificate extensions. */
+#include "tbbr_oid.h"
+
+/*
+ * Assign arbitrary Object ID values that do not conflict with any of the
+ * TBBR reserved OIDs.
+ */
+/* Platform root-of-trust public key */
+#define PROT_PK_OID				"1.3.6.1.4.1.4128.2100.1102"
+/* Secure World root-of-trust public key */
+#define SWD_ROT_PK_OID				"1.3.6.1.4.1.4128.2100.1103"
+/* Core Secure World public key */
+#define CORE_SWD_PK_OID				"1.3.6.1.4.1.4128.2100.1104"
+/* Platform public key */
+#define PLAT_PK_OID				"1.3.6.1.4.1.4128.2100.1105"
+/* Realm Monitor Manager (RMM) Hash */
+#define RMM_HASH_OID				"1.3.6.1.4.1.4128.2100.1106"
+
+#endif /* CCA_OID_H */

diff --git a/include/tools_share/firmware_image_package.h b/include/tools_share/firmware_image_package.h
index bd5b14b..b73eec7 100644
--- a/include/tools_share/firmware_image_package.h
+++ b/include/tools_share/firmware_image_package.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -24,6 +24,12 @@
 	{{0x4f, 0x51, 0x1d, 0x11}, {0x2b, 0xe5}, {0x4e, 0x49}, 0xb4, 0xc5, {0x83, 0xc2, 0xf7, 0x15, 0x84, 0x0a} }
 #define UUID_TRUSTED_FWU_CERT \
 	{{0x71, 0x40, 0x8a, 0xb2}, {0x18, 0xd6}, {0x87, 0x4c}, 0x8b, 0x2e, {0xc6, 0xdc, 0xcd, 0x50, 0xf0, 0x96} }
+#define UUID_CCA_CONTENT_CERT \
+	{{0x36,  0xd8, 0x3d, 0x85}, {0x76, 0x1d}, {0x4d, 0xaf}, 0x96, 0xf1, {0xcd, 0x99, 0xd6, 0x56, 0x9b, 0x00} }
+#define UUID_CORE_SWD_KEY_CERT \
+	{{0x52,  0x22, 0x2d, 0x31}, {0x82, 0x0f}, {0x49, 0x4d}, 0x8b, 0xbc, {0xea, 0x68, 0x25, 0xd3, 0xc3, 0x5a} }
+#define UUID_PLAT_KEY_CERT \
+	{{0xd4,  0x3c, 0xd9, 0x02}, {0x5b, 0x9f}, {0x41, 0x2e}, 0x8a, 0xc6, {0x92, 0xb6, 0xd1, 0x8b, 0xe6, 0x0d} }
 #define UUID_TRUSTED_BOOT_FIRMWARE_BL2 \
 	{{0x5f, 0xf9, 0xec, 0x0b}, {0x4d, 0x22}, {0x3e, 0x4d}, 0xa5, 0x44, {0xc3, 0x9d, 0x81, 0xc7, 0x3f, 0x0a} }
 #define UUID_SCP_FIRMWARE_SCP_BL2 \

diff --git a/include/tools_share/sptool.h b/include/tools_share/sptool.h
deleted file mode 100644
index 53668e0..0000000
--- a/include/tools_share/sptool.h
+++ /dev/null

@@ -1,25 +0,0 @@
-/*
- * Copyright (c) 2018-2020, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef SPTOOL_H
-#define SPTOOL_H
-
-#include <stdint.h>
-
-/* 4 Byte magic name "SPKG" */
-#define SECURE_PARTITION_MAGIC		0x474B5053
-
-/* Header for a secure partition package. */
-struct sp_pkg_header {
-	uint32_t magic;
-	uint32_t version;
-	uint32_t pm_offset;
-	uint32_t pm_size;
-	uint32_t img_offset;
-	uint32_t img_size;
-};
-
-#endif /* SPTOOL_H */

diff --git a/lib/cpus/aarch32/cortex_a15.S b/lib/cpus/aarch32/cortex_a15.S
index ab136ad..1143e9b 100644
--- a/lib/cpus/aarch32/cortex_a15.S
+++ b/lib/cpus/aarch32/cortex_a15.S

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -100,6 +100,15 @@
 	bx	lr
 endfunc check_errata_cve_2017_5715
 
+func check_errata_cve_2022_23960
+#if WORKAROUND_CVE_2022_23960
+	mov	r0, #ERRATA_APPLIES
+#else
+	mov	r0, #ERRATA_MISSING
+#endif
+	bx	lr
+endfunc check_errata_cve_2022_23960
+
 #if REPORT_ERRATA
 /*
  * Errata printing function for Cortex A15. Must follow AAPCS.
@@ -117,6 +126,7 @@
 	report_errata ERRATA_A15_816470, cortex_a15, 816470
 	report_errata ERRATA_A15_827671, cortex_a15, 827671
 	report_errata WORKAROUND_CVE_2017_5715, cortex_a15, cve_2017_5715
+	report_errata WORKAROUND_CVE_2022_23960, cortex_a15, cve_2022_23960
 
 	pop	{r12, lr}
 	bx	lr
@@ -131,11 +141,11 @@
 	bl	errata_a15_827671_wa
 #endif
 
-#if IMAGE_BL32 && WORKAROUND_CVE_2017_5715
+#if IMAGE_BL32 && (WORKAROUND_CVE_2017_5715 || WORKAROUND_CVE_2022_23960)
 	ldcopr	r0, ACTLR
 	orr	r0, #CORTEX_A15_ACTLR_INV_BTB_BIT
 	stcopr	r0, ACTLR
-	ldr	r0, =workaround_icache_inv_runtime_exceptions
+	ldr	r0, =wa_cve_2017_5715_icache_inv_vbar
 	stcopr	r0, VBAR
 	stcopr	r0, MVBAR
 	/* isb will be applied in the course of the reset func */

diff --git a/lib/cpus/aarch32/cortex_a57.S b/lib/cpus/aarch32/cortex_a57.S
index 2e97abb..18ee1f9 100644
--- a/lib/cpus/aarch32/cortex_a57.S
+++ b/lib/cpus/aarch32/cortex_a57.S

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -396,6 +396,11 @@
 	bx	lr
 endfunc check_errata_cve_2018_3639
 
+func check_errata_cve_2022_23960
+	mov	r0, #ERRATA_MISSING
+	bx	lr
+endfunc check_errata_cve_2022_23960
+
 	/* -------------------------------------------------
 	 * The CPU Ops reset function for Cortex-A57.
 	 * Shall clobber: r0-r6
@@ -600,6 +605,7 @@
 	report_errata ERRATA_A57_859972, cortex_a57, 859972
 	report_errata WORKAROUND_CVE_2017_5715, cortex_a57, cve_2017_5715
 	report_errata WORKAROUND_CVE_2018_3639, cortex_a57, cve_2018_3639
+	report_errata WORKAROUND_CVE_2022_23960, cortex_a57, cve_2022_23960
 
 	pop	{r12, lr}
 	bx	lr

diff --git a/lib/cpus/aarch32/cortex_a72.S b/lib/cpus/aarch32/cortex_a72.S
index ff2b0e6..03914b2 100644
--- a/lib/cpus/aarch32/cortex_a72.S
+++ b/lib/cpus/aarch32/cortex_a72.S

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -101,6 +101,11 @@
 	bx	lr
 endfunc check_errata_cve_2018_3639
 
+func check_errata_cve_2022_23960
+	mov	r0, #ERRATA_MISSING
+	bx	lr
+endfunc check_errata_cve_2022_23960
+
 	/* -------------------------------------------------
 	 * The CPU Ops reset function for Cortex-A72.
 	 * -------------------------------------------------
@@ -260,6 +265,7 @@
 	report_errata ERRATA_A72_859971, cortex_a72, 859971
 	report_errata WORKAROUND_CVE_2017_5715, cortex_a72, cve_2017_5715
 	report_errata WORKAROUND_CVE_2018_3639, cortex_a72, cve_2018_3639
+	report_errata WORKAROUND_CVE_2022_23960, cortex_a72, cve_2022_23960
 
 	pop	{r12, lr}
 	bx	lr

diff --git a/lib/cpus/aarch64/cortex_a510.S b/lib/cpus/aarch64/cortex_a510.S
index 34e1082..f444077 100644
--- a/lib/cpus/aarch64/cortex_a510.S
+++ b/lib/cpus/aarch64/cortex_a510.S

@@ -301,6 +301,7 @@
 	report_errata ERRATA_A510_2250311, cortex_a510, 2250311
 	report_errata ERRATA_A510_2218950, cortex_a510, 2218950
 	report_errata ERRATA_A510_2172148, cortex_a510, 2172148
+	report_errata ERRATA_DSU_2313941, cortex_a510, dsu_2313941
 
 	ldp	x8, x30, [sp], #16
 	ret
@@ -312,12 +313,15 @@
 
 	/* Disable speculative loads */
 	msr	SSBS, xzr
-	isb
 
 	/* Get the CPU revision and stash it in x18. */
 	bl	cpu_get_rev_var
 	mov	x18, x0
 
+#if ERRATA_DSU_2313941
+	bl	errata_dsu_2313941_wa
+#endif
+
 #if ERRATA_A510_1922240
 	mov	x0, x18
 	bl	errata_cortex_a510_1922240_wa
@@ -353,6 +357,7 @@
 	bl	errata_cortex_a510_2172148_wa
 #endif
 
+	isb
 	ret	x19
 endfunc cortex_a510_reset_func
 

diff --git a/lib/cpus/aarch64/cortex_a710.S b/lib/cpus/aarch64/cortex_a710.S
index aea62ae..5d8e9a6 100644
--- a/lib/cpus/aarch64/cortex_a710.S
+++ b/lib/cpus/aarch64/cortex_a710.S

@@ -310,6 +310,49 @@
 	b       cpu_rev_var_ls
 endfunc check_errata_2282622
 
+/* ---------------------------------------------------------------
+ * Errata Workaround for Cortex-A710 Erratum 2008768.
+ * This applies to revision r0p0, r1p0 and r2p0.
+ * It is fixed in r2p1.
+ * Inputs:
+ * x0: variant[4:7] and revision[0:3] of current cpu.
+ * Shall clobber: x0, x1, x2, x17
+ * ---------------------------------------------------------------
+ */
+func errata_a710_2008768_wa
+	mov     x17, x30
+	bl      check_errata_2008768
+	cbz     x0, 1f
+
+	/* Stash ERRSELR_EL1 in x2 */
+	mrs	x2, ERRSELR_EL1
+
+	/* Select error record 0 and clear ED bit */
+	msr	ERRSELR_EL1, xzr
+	mrs	x1, ERXCTLR_EL1
+	bfi	x1, xzr, #ERXCTLR_ED_SHIFT, #1
+	msr	ERXCTLR_EL1, x1
+
+	/* Select error record 1 and clear ED bit */
+	mov	x0, #1
+	msr	ERRSELR_EL1, x0
+	mrs	x1, ERXCTLR_EL1
+	bfi	x1, xzr, #ERXCTLR_ED_SHIFT, #1
+	msr	ERXCTLR_EL1, x1
+
+	/* Restore ERRSELR_EL1 from x2 */
+	msr	ERRSELR_EL1, x2
+
+1:
+	ret     x17
+endfunc errata_a710_2008768_wa
+
+func check_errata_2008768
+	/* Applies to r0p0, r1p0 and r2p0 */
+	mov     x1, #0x20
+	b       cpu_rev_var_ls
+endfunc check_errata_2008768
+
 func check_errata_cve_2022_23960
 #if WORKAROUND_CVE_2022_23960
 	mov	x0, #ERRATA_APPLIES
@@ -324,6 +367,14 @@
 	 * ----------------------------------------------------
 	 */
 func cortex_a710_core_pwr_dwn
+
+#if ERRATA_A710_2008768
+	mov	x4, x30
+	bl	cpu_get_rev_var
+	bl	errata_a710_2008768_wa
+	mov	x30, x4
+#endif
+
 	/* ---------------------------------------------------
 	 * Enable CPU power down bit in power control register
 	 * ---------------------------------------------------
@@ -358,7 +409,9 @@
 	report_errata ERRATA_A710_2267065, cortex_a710, 2267065
 	report_errata ERRATA_A710_2136059, cortex_a710, 2136059
 	report_errata ERRATA_A710_2282622, cortex_a710, 2282622
+	report_errata ERRATA_A710_2008768, cortex_a710, 2008768
 	report_errata WORKAROUND_CVE_2022_23960, cortex_a710, cve_2022_23960
+	report_errata ERRATA_DSU_2313941, cortex_a710, dsu_2313941
 
 	ldp	x8, x30, [sp], #16
 	ret
@@ -374,6 +427,10 @@
 	bl	cpu_get_rev_var
 	mov	x18, x0
 
+#if ERRATA_DSU_2313941
+	bl	errata_dsu_2313941_wa
+#endif
+
 #if ERRATA_A710_1987031
 	mov	x0, x18
 	bl	errata_a710_1987031_wa

diff --git a/lib/cpus/aarch64/cortex_a77.S b/lib/cpus/aarch64/cortex_a77.S
index e7365e2..aa66e94 100644
--- a/lib/cpus/aarch64/cortex_a77.S
+++ b/lib/cpus/aarch64/cortex_a77.S

@@ -199,6 +199,34 @@
 	b	cpu_rev_var_ls
 endfunc check_errata_1791578
 
+	/* --------------------------------------------------
+	 * Errata Workaround for Cortex A77 Errata #2356587.
+	 * This applies to revisions r0p0, r1p0, and r1p1 and is still open.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0-x17
+	 * --------------------------------------------------
+	 */
+func errata_a77_2356587_wa
+	/* Check workaround compatibility. */
+	mov	x17, x30
+	bl	check_errata_2356587
+	cbz	x0, 1f
+
+	/* Set bit 0 in ACTLR2_EL1 */
+	mrs	x1, CORTEX_A77_ACTLR2_EL1
+	orr	x1, x1, #CORTEX_A77_ACTLR2_EL1_BIT_0
+	msr	CORTEX_A77_ACTLR2_EL1, x1
+	isb
+1:
+	ret	x17
+endfunc errata_a77_2356587_wa
+
+func check_errata_2356587
+	/* Applies to r0p0, r1p0, and r1p1 right now */
+	mov	x1, #0x11
+	b	cpu_rev_var_ls
+endfunc check_errata_2356587
+
 func check_errata_cve_2022_23960
 #if WORKAROUND_CVE_2022_23960
 	mov	x0, #ERRATA_APPLIES
@@ -238,6 +266,11 @@
 	bl	errata_a77_1791578_wa
 #endif
 
+#if ERRATA_A77_2356587
+	mov	x0, x18
+	bl	errata_a77_2356587_wa
+#endif
+
 #if IMAGE_BL31 && WORKAROUND_CVE_2022_23960
 	/*
 	 * The Cortex-A77 generic vectors are overridden to apply errata
@@ -285,6 +318,7 @@
 	report_errata ERRATA_A77_1925769, cortex_a77, 1925769
 	report_errata ERRATA_A77_1946167, cortex_a77, 1946167
 	report_errata ERRATA_A77_1791578, cortex_a77, 1791578
+	report_errata ERRATA_A77_2356587, cortex_a77, 2356587
 	report_errata WORKAROUND_CVE_2022_23960, cortex_a77, cve_2022_23960
 
 	ldp	x8, x30, [sp], #16

diff --git a/lib/cpus/aarch64/cortex_a78.S b/lib/cpus/aarch64/cortex_a78.S
index 1a6f848..be94e91 100644
--- a/lib/cpus/aarch64/cortex_a78.S
+++ b/lib/cpus/aarch64/cortex_a78.S

@@ -267,6 +267,62 @@
 	b	cpu_rev_var_range
 endfunc check_errata_2242635
 
+/* --------------------------------------------------
+ * Errata Workaround for Cortex A78 Errata 2376745.
+ * This applies to revisions r0p0, r1p0, r1p1, and r1p2.
+ * It is still open.
+ * x0: variant[4:7] and revision[0:3] of current cpu.
+ * Shall clobber: x0-x1, x17
+ * --------------------------------------------------
+ */
+func errata_a78_2376745_wa
+	/* Check revision. */
+	mov	x17, x30
+	bl	check_errata_2376745
+	cbz	x0, 1f
+
+	/* Apply the workaround. */
+	mrs	x1, CORTEX_A78_ACTLR2_EL1
+	orr	x1, x1, #BIT(0)
+	msr	CORTEX_A78_ACTLR2_EL1, x1
+1:
+	ret	x17
+endfunc errata_a78_2376745_wa
+
+func check_errata_2376745
+	/* Applies to r0p0, r0p1, r1p1, and r1p2 */
+	mov	x1, #CPU_REV(1, 2)
+	b	cpu_rev_var_ls
+endfunc check_errata_2376745
+
+/* --------------------------------------------------
+ * Errata Workaround for Cortex A78 Errata 2395406.
+ * This applies to revisions r0p0, r1p0, r1p1, and r1p2.
+ * It is still open.
+ * x0: variant[4:7] and revision[0:3] of current cpu.
+ * Shall clobber: x0-x1, x17
+ * --------------------------------------------------
+ */
+func errata_a78_2395406_wa
+	/* Check revision. */
+	mov	x17, x30
+	bl	check_errata_2395406
+	cbz	x0, 1f
+
+	/* Apply the workaround. */
+	mrs	x1, CORTEX_A78_ACTLR2_EL1
+	orr	x1, x1, #BIT(40)
+	msr	CORTEX_A78_ACTLR2_EL1, x1
+1:
+	ret	x17
+endfunc errata_a78_2395406_wa
+
+func check_errata_2395406
+	/* Applies to r0p0, r0p1, r1p1, and r1p2 */
+	mov	x1, #CPU_REV(1, 2)
+	b	cpu_rev_var_ls
+endfunc check_errata_2395406
+
 func check_errata_cve_2022_23960
 #if WORKAROUND_CVE_2022_23960
 	mov	x0, #ERRATA_APPLIES
@@ -320,6 +376,16 @@
 	bl	errata_a78_2242635_wa
 #endif
 
+#if ERRATA_A78_2376745
+	mov	x0, x18
+	bl	errata_a78_2376745_wa
+#endif
+
+#if ERRATA_A78_2395406
+	mov	x0, x18
+	bl	errata_a78_2395406_wa
+#endif
+
 #if ENABLE_AMU
 	/* Make sure accesses from EL0/EL1 and EL2 are not trapped to EL3 */
 	mrs	x0, actlr_el3
@@ -390,6 +456,8 @@
 	report_errata ERRATA_A78_1952683, cortex_a78, 1952683
 	report_errata ERRATA_A78_2132060, cortex_a78, 2132060
 	report_errata ERRATA_A78_2242635, cortex_a78, 2242635
+	report_errata ERRATA_A78_2376745, cortex_a78, 2376745
+	report_errata ERRATA_A78_2395406, cortex_a78, 2395406
 	report_errata WORKAROUND_CVE_2022_23960, cortex_a78, cve_2022_23960
 
 	ldp	x8, x30, [sp], #16

diff --git a/lib/cpus/aarch64/cortex_hunter.S b/lib/cpus/aarch64/cortex_hunter.S
index 2ab4296..973637e 100644
--- a/lib/cpus/aarch64/cortex_hunter.S
+++ b/lib/cpus/aarch64/cortex_hunter.S

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,6 +10,7 @@
 #include <cortex_hunter.h>
 #include <cpu_macros.S>
 #include <plat_macros.S>
+#include "wa_cve_2022_23960_bhb_vector.S"
 
 /* Hardware handled coherency */
 #if HW_ASSISTED_COHERENCY == 0
@@ -21,9 +22,32 @@
 #error "Cortex Hunter supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
 #endif
 
+#if WORKAROUND_CVE_2022_23960
+        wa_cve_2022_23960_bhb_vector_table CORTEX_HUNTER_BHB_LOOP_COUNT, cortex_hunter
+#endif /* WORKAROUND_CVE_2022_23960 */
+
+func check_errata_cve_2022_23960
+#if WORKAROUND_CVE_2022_23960
+	mov	x0, #ERRATA_APPLIES
+#else
+	mov	x0, #ERRATA_MISSING
+#endif
+	ret
+endfunc check_errata_cve_2022_23960
+
 func cortex_hunter_reset_func
 	/* Disable speculative loads */
 	msr	SSBS, xzr
+
+#if IMAGE_BL31 && WORKAROUND_CVE_2022_23960
+	/*
+	 * The Cortex Hunter generic vectors are overridden to apply errata
+	 * mitigation on exception entry from lower ELs.
+	 */
+	adr	x0, wa_cve_vbar_cortex_hunter
+	msr	vbar_el3, x0
+#endif /* IMAGE_BL31 && WORKAROUND_CVE_2022_23960 */
+
 	isb
 	ret
 endfunc cortex_hunter_reset_func
@@ -49,6 +73,18 @@
  * Errata printing function for Cortex Hunter. Must follow AAPCS.
  */
 func cortex_hunter_errata_report
+	stp	x8, x30, [sp, #-16]!
+
+	bl	cpu_get_rev_var
+	mov	x8, x0
+
+	/*
+	 * Report all errata. The revision-variant information is passed to
+	 * checking functions of each errata.
+	 */
+	report_errata WORKAROUND_CVE_2022_23960, cortex_hunter, cve_2022_23960
+
+	ldp	x8, x30, [sp], #16
 	ret
 endfunc cortex_hunter_errata_report
 #endif

diff --git a/lib/cpus/aarch64/cortex_makalu.S b/lib/cpus/aarch64/cortex_makalu.S
index 98c7d6d..7603210 100644
--- a/lib/cpus/aarch64/cortex_makalu.S
+++ b/lib/cpus/aarch64/cortex_makalu.S

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,6 +10,7 @@
 #include <cortex_makalu.h>
 #include <cpu_macros.S>
 #include <plat_macros.S>
+#include "wa_cve_2022_23960_bhb_vector.S"
 
 /* Hardware handled coherency */
 #if HW_ASSISTED_COHERENCY == 0
@@ -21,9 +22,32 @@
 #error "Cortex Makalu supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
 #endif
 
+#if WORKAROUND_CVE_2022_23960
+	wa_cve_2022_23960_bhb_vector_table CORTEX_MAKALU_BHB_LOOP_COUNT, cortex_makalu
+#endif /* WORKAROUND_CVE_2022_23960 */
+
+func check_errata_cve_2022_23960
+#if WORKAROUND_CVE_2022_23960
+	mov     x0, #ERRATA_APPLIES
+#else
+	mov     x0, #ERRATA_MISSING
+#endif
+	ret
+endfunc check_errata_cve_2022_23960
+
 func cortex_makalu_reset_func
 	/* Disable speculative loads */
 	msr	SSBS, xzr
+
+#if IMAGE_BL31 && WORKAROUND_CVE_2022_23960
+	/*
+	 * The Cortex Makalu generic vectors are overridden to apply errata
+	 * mitigation on exception entry from lower ELs.
+	 */
+        adr	x0, wa_cve_vbar_cortex_makalu
+        msr	vbar_el3, x0
+#endif /* IMAGE_BL31 && WORKAROUND_CVE_2022_23960 */
+
 	isb
 	ret
 endfunc cortex_makalu_reset_func
@@ -49,6 +73,18 @@
  * Errata printing function for Cortex Makalu. Must follow AAPCS.
  */
 func cortex_makalu_errata_report
+	stp	x8, x30, [sp, #-16]!
+
+	bl	cpu_get_rev_var
+	mov	x8, x0
+
+	/*
+	 * Report all errata. The revision-variant information is passed to
+	 * checking functions of each errata.
+	 */
+	report_errata WORKAROUND_CVE_2022_23960, cortex_makalu, cve_2022_23960
+
+	ldp     x8, x30, [sp], #16
 	ret
 endfunc cortex_makalu_errata_report
 #endif

diff --git a/lib/cpus/aarch64/cortex_makalu_elp_arm.S b/lib/cpus/aarch64/cortex_makalu_elp_arm.S
index fbbf205..f4d2df0 100644
--- a/lib/cpus/aarch64/cortex_makalu_elp_arm.S
+++ b/lib/cpus/aarch64/cortex_makalu_elp_arm.S

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,6 +10,7 @@
 #include <cortex_makalu_elp_arm.h>
 #include <cpu_macros.S>
 #include <plat_macros.S>
+#include "wa_cve_2022_23960_bhb_vector.S"
 
 /* Hardware handled coherency */
 #if HW_ASSISTED_COHERENCY == 0
@@ -21,6 +22,10 @@
 #error "Cortex Makalu ELP supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
 #endif
 
+#if WORKAROUND_CVE_2022_23960
+	wa_cve_2022_23960_bhb_vector_table CORTEX_MAKALU_ELP_ARM_BHB_LOOP_COUNT, cortex_makalu_elp_arm
+#endif /* WORKAROUND_CVE_2022_23960 */
+
 	/* ----------------------------------------------------
 	 * HW will do the cache maintenance while powering down
 	 * ----------------------------------------------------
@@ -37,22 +42,53 @@
 	ret
 endfunc cortex_makalu_elp_arm_core_pwr_dwn
 
-#if REPORT_ERRATA
-/*
- * Errata printing function for Cortex Makalu ELP. Must follow AAPCS.
- */
-func cortex_makalu_elp_arm_errata_report
-	ret
-endfunc cortex_makalu_elp_arm_errata_report
+func check_errata_cve_2022_23960
+#if WORKAROUND_CVE_2022_23960
+	mov	x0, #ERRATA_APPLIES
+#else
+	mov	x0, #ERRATA_MISSING
 #endif
+	ret
+endfunc check_errata_cve_2022_23960
 
 func cortex_makalu_elp_arm_reset_func
 	/* Disable speculative loads */
 	msr	SSBS, xzr
+
+#if IMAGE_BL31 && WORKAROUND_CVE_2022_23960
+	/*
+	 * The Cortex Makalu ELP generic vectors are overridden to apply
+	 * errata mitigation on exception entry from lower ELs.
+         */
+	adr	x0, wa_cve_vbar_cortex_makalu_elp_arm
+	msr	vbar_el3, x0
+#endif /* IMAGE_BL31 && WORKAROUND_CVE_2022_23960 */
+
 	isb
 	ret
 endfunc cortex_makalu_elp_arm_reset_func
 
+#if REPORT_ERRATA
+/*
+ * Errata printing function for Cortex Makalu ELP. Must follow AAPCS.
+ */
+func cortex_makalu_elp_arm_errata_report
+	stp	x8, x30, [sp, #-16]!
+
+	bl	cpu_get_rev_var
+	mov	x8, x0
+
+	/*
+	 * Report all errata. The revision-variant information is passed to
+	 * checking functions of each errata.
+	 */
+	report_errata WORKAROUND_CVE_2022_23960, cortex_makalu_elp_arm, cve_2022_23960
+
+	ldp	x8, x30, [sp], #16
+	ret
+endfunc cortex_makalu_elp_arm_errata_report
+#endif
+
 	/* ---------------------------------------------
 	 * This function provides Cortex Makalu ELP-
 	 * specific register information for crash

diff --git a/lib/cpus/aarch64/cortex_x1.S b/lib/cpus/aarch64/cortex_x1.S
new file mode 100644
index 0000000..9a7f666
--- /dev/null
+++ b/lib/cpus/aarch64/cortex_x1.S

@@ -0,0 +1,217 @@
+/*
+ * Copyright (c) 2022, Google LLC. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+#include <cortex_x1.h>
+#include <cpu_macros.S>
+#include "wa_cve_2022_23960_bhb_vector.S"
+
+/* Hardware handled coherency */
+#if HW_ASSISTED_COHERENCY == 0
+#error "Cortex-X1 must be compiled with HW_ASSISTED_COHERENCY enabled"
+#endif
+
+/* 64-bit only core */
+#if CTX_INCLUDE_AARCH32_REGS == 1
+#error "Cortex-X1 supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
+#endif
+
+#if WORKAROUND_CVE_2022_23960
+	wa_cve_2022_23960_bhb_vector_table CORTEX_X1_BHB_LOOP_COUNT, cortex_x1
+#endif /* WORKAROUND_CVE_2022_23960 */
+
+/* --------------------------------------------------
+ * Errata Workaround for X1 Erratum 1821534.
+ * This applies to revision r0p0 and r1p0 of X1.
+ * Inputs:
+ * x0: variant[4:7] and revision[0:3] of current cpu.
+ * Shall clobber: x0-x17
+ * --------------------------------------------------
+ */
+func errata_x1_1821534_wa
+	/* Compare x0 against revision r1p0 */
+	mov	x17, x30
+	bl	check_errata_1821534
+	cbz	x0, 1f
+	mrs	x1, CORTEX_X1_ACTLR2_EL1
+	orr	x1, x1, BIT(2)
+	msr	CORTEX_X1_ACTLR2_EL1, x1
+	isb
+1:
+	ret	x17
+endfunc errata_x1_1821534_wa
+
+func check_errata_1821534
+	/* Applies to r0p0 and r1p0 */
+	mov	x1, #0x10
+	b	cpu_rev_var_ls
+endfunc check_errata_1821534
+
+/* --------------------------------------------------
+ * Errata Workaround for X1 Erratum 1688305.
+ * This applies to revision r0p0 and r1p0 of X1.
+ * Inputs:
+ * x0: variant[4:7] and revision[0:3] of current cpu.
+ * Shall clobber: x0-x17
+ * --------------------------------------------------
+ */
+func errata_x1_1688305_wa
+	/* Compare x0 against revision r1p0 */
+	mov	x17, x30
+	bl	check_errata_1688305
+	cbz	x0, 1f
+	mrs	x0, CORTEX_X1_ACTLR2_EL1
+	orr	x0, x0, BIT(1)
+	msr	CORTEX_X1_ACTLR2_EL1, x0
+	isb
+
+1:
+	ret	x17
+endfunc errata_x1_1688305_wa
+
+func check_errata_1688305
+	/* Applies to r0p0 and r1p0 */
+	mov	x1, #0x10
+	b	cpu_rev_var_ls
+endfunc check_errata_1688305
+
+/* --------------------------------------------------
+ * Errata Workaround for X1 Erratum 1827429.
+ * This applies to revision r0p0 and r1p0 of X1.
+ * Inputs:
+ * x0: variant[4:7] and revision[0:3] of current cpu.
+ * Shall clobber: x0-x17
+ * --------------------------------------------------
+ */
+func errata_x1_1827429_wa
+	/* Compare x0 against revision r1p0 */
+	mov	x17, x30
+	bl	check_errata_1827429
+	cbz	x0, 1f
+	mrs	x0, CORTEX_X1_CPUECTLR_EL1
+	orr	x0, x0, BIT(53)
+	msr	CORTEX_X1_CPUECTLR_EL1, x0
+	isb
+
+1:
+	ret	x17
+endfunc errata_x1_1827429_wa
+
+func check_errata_1827429
+	/* Applies to r0p0 and r1p0 */
+	mov	x1, #0x10
+	b	cpu_rev_var_ls
+endfunc check_errata_1827429
+
+func check_errata_cve_2022_23960
+#if WORKAROUND_CVE_2022_23960
+	mov	x0, #ERRATA_APPLIES
+#else
+	mov	x0, #ERRATA_MISSING
+#endif
+	ret
+endfunc check_errata_cve_2022_23960
+
+	/* -------------------------------------------------
+	 * The CPU Ops reset function for Cortex-X1.
+	 * Shall clobber: x0-x19
+	 * -------------------------------------------------
+	 */
+func cortex_x1_reset_func
+	mov	x19, x30
+	bl	cpu_get_rev_var
+	mov	x18, x0
+
+#if ERRATA_X1_1821534
+	mov	x0, x18
+	bl	errata_x1_1821534_wa
+#endif
+
+#if ERRATA_X1_1688305
+	mov	x0, x18
+	bl	errata_x1_1688305_wa
+#endif
+
+#if ERRATA_X1_1827429
+	mov	x0, x18
+	bl	errata_x1_1827429_wa
+#endif
+
+#if IMAGE_BL31 && WORKAROUND_CVE_2022_23960
+	/*
+	 * The Cortex-X1 generic vectors are overridden to apply errata
+	 * mitigation on exception entry from lower ELs.
+	 */
+	adr	x0, wa_cve_vbar_cortex_x1
+	msr	vbar_el3, x0
+#endif /* IMAGE_BL31 && WORKAROUND_CVE_2022_23960 */
+
+	isb
+	ret	x19
+endfunc cortex_x1_reset_func
+
+	/* ---------------------------------------------
+	 * HW will do the cache maintenance while powering down
+	 * ---------------------------------------------
+	 */
+func cortex_x1_core_pwr_dwn
+	/* ---------------------------------------------
+	 * Enable CPU power down bit in power control register
+	 * ---------------------------------------------
+	 */
+	mrs	x0, CORTEX_X1_CPUPWRCTLR_EL1
+	orr	x0, x0, #CORTEX_X1_CORE_PWRDN_EN_MASK
+	msr	CORTEX_X1_CPUPWRCTLR_EL1, x0
+	isb
+	ret
+endfunc cortex_x1_core_pwr_dwn
+
+#if REPORT_ERRATA
+/*
+ * Errata printing function for Cortex X1. Must follow AAPCS.
+ */
+func cortex_x1_errata_report
+	stp	x8, x30, [sp, #-16]!
+
+	bl	cpu_get_rev_var
+	mov	x8, x0
+
+	/*
+	 * Report all errata. The revision-variant information is passed to
+	 * checking functions of each errata.
+	 */
+	report_errata ERRATA_X1_1821534, cortex_x1, 1821534
+	report_errata ERRATA_X1_1688305, cortex_x1, 1688305
+	report_errata ERRATA_X1_1827429, cortex_x1, 1827429
+	report_errata WORKAROUND_CVE_2022_23960, cortex_x1, cve_2022_23960
+
+	ldp	x8, x30, [sp], #16
+	ret
+endfunc cortex_x1_errata_report
+#endif
+
+       /* ---------------------------------------------
+	* This function provides Cortex X1 specific
+	* register information for crash reporting.
+	* It needs to return with x6 pointing to
+	* a list of register names in ascii and
+	* x8 - x15 having values of registers to be
+	* reported.
+	* ---------------------------------------------
+	*/
+.section .rodata.cortex_x1_regs, "aS"
+cortex_x1_regs:  /* The ascii list of register names to be reported */
+	.asciz	"cpuectlr_el1", ""
+
+func cortex_x1_cpu_reg_dump
+	adr	x6, cortex_x1_regs
+	mrs	x8, CORTEX_X1_CPUECTLR_EL1
+	ret
+endfunc cortex_x1_cpu_reg_dump
+
+declare_cpu_ops cortex_x1, CORTEX_X1_MIDR, \
+	cortex_x1_reset_func, \
+	cortex_x1_core_pwr_dwn

diff --git a/lib/cpus/aarch64/cortex_x2.S b/lib/cpus/aarch64/cortex_x2.S
index 90a906b..3e0810b 100644
--- a/lib/cpus/aarch64/cortex_x2.S
+++ b/lib/cpus/aarch64/cortex_x2.S

@@ -305,6 +305,7 @@
 	report_errata ERRATA_X2_2147715, cortex_x2, 2147715
 	report_errata ERRATA_X2_2216384, cortex_x2, 2216384
 	report_errata WORKAROUND_CVE_2022_23960, cortex_x2, cve_2022_23960
+	report_errata ERRATA_DSU_2313941, cortex_x2, dsu_2313941
 
 	ldp	x8, x30, [sp], #16
 	ret
@@ -316,12 +317,15 @@
 
 	/* Disable speculative loads */
 	msr	SSBS, xzr
-	isb
 
 	/* Get the CPU revision and stash it in x18. */
 	bl	cpu_get_rev_var
 	mov	x18, x0
 
+#if ERRATA_DSU_2313941
+	bl	errata_dsu_2313941_wa
+#endif
+
 #if ERRATA_X2_2002765
 	mov	x0, x18
 	bl	errata_cortex_x2_2002765_wa
@@ -367,7 +371,7 @@
 #endif /* IMAGE_BL31 && WORKAROUND_CVE_2022_23960 */
 
 	isb
-	ret x19
+	ret	x19
 endfunc cortex_x2_reset_func
 
 	/* ---------------------------------------------

diff --git a/lib/cpus/aarch64/denver.S b/lib/cpus/aarch64/denver.S
index 224ee26..3c54a6f 100644
--- a/lib/cpus/aarch64/denver.S
+++ b/lib/cpus/aarch64/denver.S

@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2015-2018, ARM Limited and Contributors. All rights reserved.
- * Copyright (c) 2020, NVIDIA Corporation. All rights reserved.
+ * Copyright (c) 2020-2022, NVIDIA Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -369,6 +369,7 @@
 		denver_reset_func, \
 		check_errata_cve_2017_5715, \
 		CPU_NO_EXTRA2_FUNC, \
+		CPU_NO_EXTRA3_FUNC, \
 		denver_core_pwr_dwn, \
 		denver_cluster_pwr_dwn
 .endm

diff --git a/lib/cpus/aarch64/dsu_helpers.S b/lib/cpus/aarch64/dsu_helpers.S
index da052d5..419b6ea 100644
--- a/lib/cpus/aarch64/dsu_helpers.S
+++ b/lib/cpus/aarch64/dsu_helpers.S

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -139,3 +139,57 @@
 1:
 	ret	x17
 endfunc errata_dsu_936184_wa
+
+	/* -----------------------------------------------------------------------
+	 * DSU erratum 2313941 check function
+	 * Checks the DSU variant, revision and configuration to determine if
+	 * the erratum applies. Erratum applies on all configurations of the
+	 * DSU and if revision-variant is r0p0, r1p0, r2p0, r2p1, r3p0, r3p1.
+	 *
+	 * The erratum is still open.
+	 *
+	 * This function is called from both assembly and C environment. So it
+	 * follows AAPCS.
+	 *
+	 * Clobbers: x0-x3
+	 * -----------------------------------------------------------------------
+	 */
+	.globl	check_errata_dsu_2313941
+	.globl	errata_dsu_2313941_wa
+
+func check_errata_dsu_2313941
+	mov	x2, #ERRATA_APPLIES
+	mov	x3, #ERRATA_NOT_APPLIES
+
+	/* Check if DSU version is less than or equal to r3p1 */
+	mrs	x1, CLUSTERIDR_EL1
+
+	/* DSU variant and revision bitfields in CLUSTERIDR are adjacent */
+	ubfx	x0, x1, #CLUSTERIDR_REV_SHIFT,\
+			#(CLUSTERIDR_REV_BITS + CLUSTERIDR_VAR_BITS)
+	mov	x1, #(0x31 << CLUSTERIDR_REV_SHIFT)
+	cmp	x0, x1
+	csel	x0, x2, x3, LS
+	ret
+endfunc check_errata_dsu_2313941
+
+	/* --------------------------------------------------
+	 * Errata Workaround for DSU erratum #2313941.
+	 *
+	 * Can clobber only: x0-x17
+	 * --------------------------------------------------
+	 */
+func errata_dsu_2313941_wa
+	mov	x17, x30
+	bl	check_errata_dsu_2313941
+	cbz	x0, 1f
+
+	/* If erratum applies, disable high-level clock gating */
+	mrs	x0, CLUSTERACTLR_EL1
+	orr	x0, x0, #CLUSTERACTLR_EL1_DISABLE_SCLK_GATING
+	msr	CLUSTERACTLR_EL1, x0
+	isb
+1:
+	ret	x17
+endfunc errata_dsu_2313941_wa
+

diff --git a/lib/cpus/aarch64/neoverse_demeter.S b/lib/cpus/aarch64/neoverse_demeter.S
index f43c18b..41cb4ee 100644
--- a/lib/cpus/aarch64/neoverse_demeter.S
+++ b/lib/cpus/aarch64/neoverse_demeter.S

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,6 +10,7 @@
 #include <neoverse_demeter.h>
 #include <cpu_macros.S>
 #include <plat_macros.S>
+#include "wa_cve_2022_23960_bhb_vector.S"
 
 /* Hardware handled coherency */
 #if HW_ASSISTED_COHERENCY == 0
@@ -21,6 +22,10 @@
 #error "Neoverse Demeter supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
 #endif
 
+#if WORKAROUND_CVE_2022_23960
+	wa_cve_2022_23960_bhb_vector_table NEOVERSE_DEMETER_BHB_LOOP_COUNT, neoverse_demeter
+#endif /* WORKAROUND_CVE_2022_23960 */
+
 	/* ----------------------------------------------------
 	 * HW will do the cache maintenance while powering down
 	 * ----------------------------------------------------
@@ -37,22 +42,52 @@
 	ret
 endfunc neoverse_demeter_core_pwr_dwn
 
-#if REPORT_ERRATA
-/*
- * Errata printing function for Neoverse Demeter. Must follow AAPCS.
- */
-func neoverse_demeter_errata_report
-	ret
-endfunc neoverse_demeter_errata_report
+func check_errata_cve_2022_23960
+#if WORKAROUND_CVE_2022_23960
+	mov	x0, #ERRATA_APPLIES
+#else
+	mov	x0, #ERRATA_MISSING
 #endif
+	ret
+endfunc check_errata_cve_2022_23960
 
 func neoverse_demeter_reset_func
 	/* Disable speculative loads */
 	msr	SSBS, xzr
+
+#if IMAGE_BL31 && WORKAROUND_CVE_2022_23960
+	/*
+	 * The Neoverse Demeter vectors are overridden to apply
+	 * errata mitigation on exception entry from lower ELs.
+	 */
+	adr	x0, wa_cve_vbar_neoverse_demeter
+	msr	vbar_el3, x0
+#endif /* IMAGE_BL31 && WORKAROUND_CVE_2022_23960 */
 	isb
 	ret
 endfunc neoverse_demeter_reset_func
 
+#if REPORT_ERRATA
+/*
+ * Errata printing function for Neoverse Demeter. Must follow AAPCS.
+ */
+func neoverse_demeter_errata_report
+	stp	x8, x30, [sp, #-16]!
+
+	bl	cpu_get_rev_var
+	mov	x8, x0
+
+	/*
+	 * Report all errata. The revision-variant information is passed to
+	 * checking functions of each errata.
+	 */
+	report_errata WORKAROUND_CVE_2022_23960, neoverse_demeter, cve_2022_23960
+
+	ldp	x8, x30, [sp], #16
+	ret
+endfunc neoverse_demeter_errata_report
+#endif
+
 	/* ---------------------------------------------
 	 * This function provides Neoverse Demeter-
 	 * specific register information for crash

diff --git a/lib/cpus/aarch64/neoverse_n2.S b/lib/cpus/aarch64/neoverse_n2.S
index b93f2a6..5b796dc 100644
--- a/lib/cpus/aarch64/neoverse_n2.S
+++ b/lib/cpus/aarch64/neoverse_n2.S

@@ -367,6 +367,10 @@
 	orr	x0, x0, #NEOVERSE_N2_CPUACTLR2_EL1_BIT_2
 	msr	NEOVERSE_N2_CPUACTLR2_EL1, x0
 
+#if ERRATA_DSU_2313941
+	bl	errata_dsu_2313941_wa
+#endif
+
 #if ERRATA_N2_2067956
 	mov	x0, x18
 	bl	errata_n2_2067956_wa
@@ -493,6 +497,7 @@
 	report_errata ERRATA_N2_2242400, neoverse_n2, 2242400
 	report_errata ERRATA_N2_2280757, neoverse_n2, 2280757
 	report_errata WORKAROUND_CVE_2022_23960, neoverse_n2, cve_2022_23960
+	report_errata ERRATA_DSU_2313941, neoverse_n2, dsu_2313941
 
 	ldp	x8, x30, [sp], #16
 	ret

diff --git a/lib/cpus/aarch64/neoverse_poseidon.S b/lib/cpus/aarch64/neoverse_poseidon.S
index 43a93aa..030293d 100644
--- a/lib/cpus/aarch64/neoverse_poseidon.S
+++ b/lib/cpus/aarch64/neoverse_poseidon.S

@@ -10,6 +10,7 @@
 #include <neoverse_poseidon.h>
 #include <cpu_macros.S>
 #include <plat_macros.S>
+#include "wa_cve_2022_23960_bhb_vector.S"
 
 /* Hardware handled coherency */
 #if HW_ASSISTED_COHERENCY == 0
@@ -21,6 +22,10 @@
 #error "Neoverse Poseidon supports only AArch64. Compile with CTX_INCLUDE_AARCH32_REGS=0"
 #endif
 
+#if WORKAROUND_CVE_2022_23960
+	wa_cve_2022_23960_bhb_vector_table NEOVERSE_POSEIDON_BHB_LOOP_COUNT, neoverse_poseidon
+#endif /* WORKAROUND_CVE_2022_23960 */
+
 	/* ---------------------------------------------
 	 * HW will do the cache maintenance while powering down
 	 * ---------------------------------------------
@@ -37,22 +42,53 @@
 	ret
 endfunc neoverse_poseidon_core_pwr_dwn
 
-#if REPORT_ERRATA
-	/*
-	 * Errata printing function for Neoverse Poseidon. Must follow AAPCS.
-	 */
-func neoverse_poseidon_errata_report
-	ret
-endfunc neoverse_poseidon_errata_report
+func check_errata_cve_2022_23960
+#if WORKAROUND_CVE_2022_23960
+	mov	x0, #ERRATA_APPLIES
+#else
+	mov	x0, #ERRATA_MISSING
 #endif
+	ret
+endfunc check_errata_cve_2022_23960
 
 func neoverse_poseidon_reset_func
 	/* Disable speculative loads */
 	msr	SSBS, xzr
+
+#if IMAGE_BL31 && WORKAROUND_CVE_2022_23960
+	/*
+	 * The Neoverse Poseidon generic vectors are overridden to apply
+	 * errata mitigation on exception entry from lower ELs.
+	 */
+	adr	x0, wa_cve_vbar_neoverse_poseidon
+	msr	vbar_el3, x0
+#endif /* IMAGE_BL31 && WORKAROUND_CVE_2022_23960 */
+
 	isb
 	ret
 endfunc neoverse_poseidon_reset_func
 
+#if REPORT_ERRATA
+	/*
+	 * Errata printing function for Neoverse Poseidon. Must follow AAPCS.
+	 */
+func neoverse_poseidon_errata_report
+	stp	x8, x30, [sp, #-16]!
+
+	bl	cpu_get_rev_var
+	mov	x8, x0
+
+	/*
+	 * Report all errata. The revision-variant information is passed to
+	 * checking functions of each errata.
+	 */
+	report_errata WORKAROUND_CVE_2022_23960, neoverse_poseidon, cve_2022_23960
+
+	ldp	x8, x30, [sp], #16
+	ret
+endfunc neoverse_poseidon_errata_report
+#endif
+
 	/* ---------------------------------------------
 	 * This function provides Neoverse-Poseidon specific
 	 * register information for crash reporting.

diff --git a/lib/cpus/aarch64/neoverse_v1.S b/lib/cpus/aarch64/neoverse_v1.S
index 6adb3a8..378cb92 100644
--- a/lib/cpus/aarch64/neoverse_v1.S
+++ b/lib/cpus/aarch64/neoverse_v1.S

@@ -330,6 +330,62 @@
 	b	cpu_rev_var_range
 endfunc check_errata_2216392
 
+	/* -----------------------------------------------------------------
+	 * Errata Workaround for Neoverse V1 Errata #2294912.
+	 * This applies to revisions r0p0, r1p0, and r1p1 and is still open.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0-x17
+	 * -----------------------------------------------------------------
+	 */
+func errata_neoverse_v1_2294912_wa
+	/* Check workaround compatibility. */
+	mov	x17, x30
+	bl	check_errata_2294912
+	cbz	x0, 1f
+
+	/* Set bit 0 in ACTLR2_EL1 */
+	mrs     x1, NEOVERSE_V1_ACTLR2_EL1
+	orr	x1, x1, #NEOVERSE_V1_ACTLR2_EL1_BIT_0
+	msr     NEOVERSE_V1_ACTLR2_EL1, x1
+	isb
+1:
+	ret	x17
+endfunc errata_neoverse_v1_2294912_wa
+
+func check_errata_2294912
+	/* Applies to r0p0, r1p0, and r1p1 right now */
+	mov	x1, #0x11
+	b	cpu_rev_var_ls
+endfunc check_errata_2294912
+
+	/* ---------------------------------------------------
+	 * Errata Workaround for Neoverse V1 Errata #2372203.
+	 * This applies to revisions <= r1p1 and is still open.
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0-x17
+	 * ----------------------------------------------------
+	 */
+func errata_neoverse_v1_2372203_wa
+	/* Check workaround compatibility. */
+	mov	x17, x30
+	bl	check_errata_2372203
+	cbz	x0, 1f
+
+	/* Set bit 40 in ACTLR2_EL1 */
+	mrs	x1, NEOVERSE_V1_ACTLR2_EL1
+	orr	x1, x1, #NEOVERSE_V1_ACTLR2_EL1_BIT_40
+	msr	NEOVERSE_V1_ACTLR2_EL1, x1
+	isb
+1:
+	ret	x17
+endfunc errata_neoverse_v1_2372203_wa
+
+func check_errata_2372203
+	/* Applies to <= r1p1. */
+	mov	x1, #0x11
+	b	cpu_rev_var_ls
+endfunc check_errata_2372203
+
 func check_errata_cve_2022_23960
 #if WORKAROUND_CVE_2022_23960
 	mov	x0, #ERRATA_APPLIES
@@ -378,6 +434,8 @@
 	report_errata ERRATA_V1_2139242, neoverse_v1, 2139242
 	report_errata ERRATA_V1_2108267, neoverse_v1, 2108267
 	report_errata ERRATA_V1_2216392, neoverse_v1, 2216392
+	report_errata ERRATA_V1_2294912, neoverse_v1, 2294912
+	report_errata ERRATA_V1_2372203, neoverse_v1, 2372203
 	report_errata WORKAROUND_CVE_2022_23960, neoverse_v1, cve_2022_23960
 
 	ldp	x8, x30, [sp], #16
@@ -437,6 +495,16 @@
 	bl	errata_neoverse_v1_2216392_wa
 #endif
 
+#if ERRATA_V1_2294912
+	mov	x0, x18
+	bl	errata_neoverse_v1_2294912_wa
+#endif
+
+#if ERRATA_V1_2372203
+	mov	x0, x18
+	bl	errata_neoverse_v1_2372203_wa
+#endif
+
 #if IMAGE_BL31 && WORKAROUND_CVE_2022_23960
 	/*
 	 * The Neoverse-V1 generic vectors are overridden to apply errata

diff --git a/lib/cpus/cpu-ops.mk b/lib/cpus/cpu-ops.mk
index 462ca9d..6d49dab 100644
--- a/lib/cpus/cpu-ops.mk
+++ b/lib/cpus/cpu-ops.mk

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2014-2022, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2014-2022, Arm Limited and Contributors. All rights reserved.
 # Copyright (c) 2020-2022, NVIDIA Corporation. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
@@ -303,6 +303,10 @@
 # to revisions r0p0, r1p0, and r1p1, it is still open.
 ERRATA_A77_1791578	?=0
 
+# Flag to apply erratum 2356587 workaround during reset. This erratum applies
+# to revisions r0p0, r1p0, and r1p1, it is still open.
+ERRATA_A77_2356587	?=0
+
 # Flag to apply erratum 1688305 workaround during reset. This erratum applies
 # to revisions r0p0 - r1p0 of the A78 cpu.
 ERRATA_A78_1688305	?=0
@@ -333,6 +337,14 @@
 # present in r0p0 as well but there is no workaround for that revision.
 ERRATA_A78_2242635	?=0
 
+# Flag to apply erratum 2376745 workaround during reset. This erratum applies
+# to revisions r0p0, r1p0, r1p1, and r1p2 of the A78 cpu. It is still open.
+ERRATA_A78_2376745	?=0
+
+# Flag to apply erratum 2395406 workaround during reset. This erratum applies
+# to revisions r0p0, r1p0, r1p1, and r1p2 of the A78 cpu. It is still open.
+ERRATA_A78_2395406	?=0
+
 # Flag to apply erratum 1941500 workaround during reset. This erratum applies
 # to revisions r0p0 and r0p1 of the A78 AE cpu. It is still open.
 ERRATA_A78_AE_1941500	?=0
@@ -349,6 +361,18 @@
 # to revisions r0p0 and r0p1 of the A78 AE cpu. It is still open.
 ERRATA_A78_AE_2395408	?=0
 
+# Flag to apply erratum 1821534 workaround during reset. This erratum applies
+# to revisions r0p0 - r1p0 of the X1 cpu and fixed in r1p1.
+ERRATA_X1_1821534	?=0
+
+# Flag to apply erratum 1688305 workaround during reset. This erratum applies
+# to revisions r0p0 - r1p0 of the X1 cpu and fixed in r1p1.
+ERRATA_X1_1688305	?=0
+
+# Flag to apply erratum 1827429 workaround during reset. This erratum applies
+# to revisions r0p0 - r1p0 of the X1 cpu and fixed in r1p1.
+ERRATA_X1_1827429	?=0
+
 # Flag to apply T32 CLREX workaround during reset. This erratum applies
 # only to r0p0 and r1p0 of the Neoverse N1 cpu.
 ERRATA_N1_1043202	?=0
@@ -448,6 +472,14 @@
 # issue exists in r0p0 as well but there is no workaround for that revision.
 ERRATA_V1_2216392	?=0
 
+# Flag to apply erratum 2294912 workaround during reset. This erratum applies
+# to revisions r0p0, r1p0, and r1p1 of the Neoverse V1 cpu and is still open.
+ERRATA_V1_2294912	?=0
+
+# Flag to apply erratum 2372203 workaround during reset. This erratum applies
+# to revisions r0p0, r1p0 and r1p1 of the Neoverse V1 cpu and is still open.
+ERRATA_V1_2372203	?=0
+
 # Flag to apply erratum 1987031 workaround during reset. This erratum applies
 # to revisions r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is still open.
 ERRATA_A710_1987031	?=0
@@ -484,6 +516,10 @@
 # to revision r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is fixed in r2p1.
 ERRATA_A710_2282622	?=0
 
+# Flag to apply erratum 2008768 workaround during reset. This erratum applies
+# to revision r0p0, r1p0 and r2p0 of the Cortex-A710 cpu and is fixed in r2p1.
+ERRATA_A710_2008768	?=0
+
 # 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
@@ -590,6 +626,11 @@
 # higher DSU power consumption on idle.
 ERRATA_DSU_936184	?=0
 
+# Flag to apply DSU erratum 2313941. This erratum applies to DSUs revisions
+# r0p0, r1p0, r2p0, r2p1, r3p0, r3p1 and is still open. Applying the workaround
+# results in higher DSU power consumption on idle.
+ERRATA_DSU_2313941	?=0
+
 # Process ERRATA_A9_794073 flag
 $(eval $(call assert_boolean,ERRATA_A9_794073))
 $(eval $(call add_define,ERRATA_A9_794073))
@@ -814,6 +855,10 @@
 $(eval $(call assert_boolean,ERRATA_A77_1791578))
 $(eval $(call add_define,ERRATA_A77_1791578))
 
+# Process ERRATA_A77_2356587 flag
+$(eval $(call assert_boolean,ERRATA_A77_2356587))
+$(eval $(call add_define,ERRATA_A77_2356587))
+
 # Process ERRATA_A78_1688305 flag
 $(eval $(call assert_boolean,ERRATA_A78_1688305))
 $(eval $(call add_define,ERRATA_A78_1688305))
@@ -842,6 +887,14 @@
 $(eval $(call assert_boolean,ERRATA_A78_2242635))
 $(eval $(call add_define,ERRATA_A78_2242635))
 
+# Process ERRATA_A78_2376745 flag
+$(eval $(call assert_boolean,ERRATA_A78_2376745))
+$(eval $(call add_define,ERRATA_A78_2376745))
+
+# Process ERRATA_A78_2395406 flag
+$(eval $(call assert_boolean,ERRATA_A78_2395406))
+$(eval $(call add_define,ERRATA_A78_2395406))
+
 # Process ERRATA_A78_AE_1941500 flag
 $(eval $(call assert_boolean,ERRATA_A78_AE_1941500))
 $(eval $(call add_define,ERRATA_A78_AE_1941500))
@@ -858,6 +911,18 @@
 $(eval $(call assert_boolean,ERRATA_A78_AE_2395408))
 $(eval $(call add_define,ERRATA_A78_AE_2395408))
 
+# Process ERRATA_X1_1821534 flag
+$(eval $(call assert_boolean,ERRATA_X1_1821534))
+$(eval $(call add_define,ERRATA_X1_1821534))
+
+# Process ERRATA_X1_1688305 flag
+$(eval $(call assert_boolean,ERRATA_X1_1688305))
+$(eval $(call add_define,ERRATA_X1_1688305))
+
+# Process ERRATA_X1_1827429 flag
+$(eval $(call assert_boolean,ERRATA_X1_1827429))
+$(eval $(call add_define,ERRATA_X1_1827429))
+
 # Process ERRATA_N1_1043202 flag
 $(eval $(call assert_boolean,ERRATA_N1_1043202))
 $(eval $(call add_define,ERRATA_N1_1043202))
@@ -954,6 +1019,14 @@
 $(eval $(call assert_boolean,ERRATA_V1_2216392))
 $(eval $(call add_define,ERRATA_V1_2216392))
 
+# Process ERRATA_V1_2294912 flag
+$(eval $(call assert_boolean,ERRATA_V1_2294912))
+$(eval $(call add_define,ERRATA_V1_2294912))
+
+# Process ERRATA_V1_2372203 flag
+$(eval $(call assert_boolean,ERRATA_V1_2372203))
+$(eval $(call add_define,ERRATA_V1_2372203))
+
 # Process ERRATA_A710_1987031 flag
 $(eval $(call assert_boolean,ERRATA_A710_1987031))
 $(eval $(call add_define,ERRATA_A710_1987031))
@@ -990,6 +1063,10 @@
 $(eval $(call assert_boolean,ERRATA_A710_2282622))
 $(eval $(call add_define,ERRATA_A710_2282622))
 
+# Process ERRATA_A710_2008768 flag
+$(eval $(call assert_boolean,ERRATA_A710_2008768))
+$(eval $(call add_define,ERRATA_A710_2008768))
+
 # Process ERRATA_N2_2067956 flag
 $(eval $(call assert_boolean,ERRATA_N2_2067956))
 $(eval $(call add_define,ERRATA_N2_2067956))
@@ -1090,6 +1167,10 @@
 $(eval $(call assert_boolean,ERRATA_DSU_936184))
 $(eval $(call add_define,ERRATA_DSU_936184))
 
+# Process ERRATA_DSU_2313941 flag
+$(eval $(call assert_boolean,ERRATA_DSU_2313941))
+$(eval $(call add_define,ERRATA_DSU_2313941))
+
 # Errata build flags
 ifneq (${ERRATA_A53_843419},0)
 TF_LDFLAGS_aarch64	+= --fix-cortex-a53-843419

diff --git a/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index 69acc2f..acfef80 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S

@@ -11,8 +11,52 @@
 #include <el3_common_macros.S>
 
 #if CTX_INCLUDE_EL2_REGS
-	.global	el2_sysregs_context_save
-	.global	el2_sysregs_context_restore
+	.global	el2_sysregs_context_save_common
+	.global	el2_sysregs_context_restore_common
+#if ENABLE_SPE_FOR_LOWER_ELS
+	.global	el2_sysregs_context_save_spe
+	.global	el2_sysregs_context_restore_spe
+#endif /* ENABLE_SPE_FOR_LOWER_ELS */
+#if CTX_INCLUDE_MTE_REGS
+	.global	el2_sysregs_context_save_mte
+	.global	el2_sysregs_context_restore_mte
+#endif /* CTX_INCLUDE_MTE_REGS */
+#if ENABLE_MPAM_FOR_LOWER_ELS
+	.global	el2_sysregs_context_save_mpam
+	.global	el2_sysregs_context_restore_mpam
+#endif /* ENABLE_MPAM_FOR_LOWER_ELS */
+#if ENABLE_FEAT_FGT
+	.global	el2_sysregs_context_save_fgt
+	.global	el2_sysregs_context_restore_fgt
+#endif /* ENABLE_FEAT_FGT */
+#if ENABLE_FEAT_ECV
+	.global	el2_sysregs_context_save_ecv
+	.global	el2_sysregs_context_restore_ecv
+#endif /* ENABLE_FEAT_ECV */
+#if ENABLE_FEAT_VHE
+	.global	el2_sysregs_context_save_vhe
+	.global	el2_sysregs_context_restore_vhe
+#endif /* ENABLE_FEAT_VHE */
+#if RAS_EXTENSION
+	.global	el2_sysregs_context_save_ras
+	.global	el2_sysregs_context_restore_ras
+#endif /* RAS_EXTENSION */
+#if CTX_INCLUDE_NEVE_REGS
+	.global	el2_sysregs_context_save_nv2
+	.global	el2_sysregs_context_restore_nv2
+#endif /* CTX_INCLUDE_NEVE_REGS */
+#if ENABLE_TRF_FOR_NS
+	.global	el2_sysregs_context_save_trf
+	.global	el2_sysregs_context_restore_trf
+#endif /* ENABLE_TRF_FOR_NS */
+#if ENABLE_FEAT_CSV2_2
+	.global	el2_sysregs_context_save_csv2
+	.global	el2_sysregs_context_restore_csv2
+#endif /* ENABLE_FEAT_CSV2_2 */
+#if ENABLE_FEAT_HCX
+	.global	el2_sysregs_context_save_hcx
+	.global	el2_sysregs_context_restore_hcx
+#endif /* ENABLE_FEAT_HCX */
 #endif /* CTX_INCLUDE_EL2_REGS */
 
 	.global	el1_sysregs_context_save
@@ -29,11 +73,16 @@
 #if CTX_INCLUDE_EL2_REGS
 
 /* -----------------------------------------------------
- * The following function strictly follows the AArch64
+ * The following functions strictly follow the AArch64
  * PCS to use x9-x16 (temporary caller-saved registers)
- * to save EL2 system register context. It assumes that
- * 'x0' is pointing to a 'el2_sys_regs' structure where
- * the register context will be saved.
+ * to save/restore EL2 system register context.
+ * el2_sysregs_context_save/restore_common functions
+ * save and restore registers that are common to all
+ * configurations. The rest of the functions save and
+ * restore EL2 system registers that are present when a
+ * particular feature is enabled. All functions assume
+ * that 'x0' is pointing to a 'el2_sys_regs' structure
+ * where the register context will be saved/restored.
  *
  * The following registers are not added.
  * AMEVCNTVOFF0<n>_EL2
@@ -43,7 +92,7 @@
  * ICH_LR<n>_EL2
  * -----------------------------------------------------
  */
-func el2_sysregs_context_save
+func el2_sysregs_context_save_common
 	mrs	x9, actlr_el2
 	mrs	x10, afsr0_el2
 	stp	x9, x10, [x0, #CTX_ACTLR_EL2]
@@ -88,11 +137,6 @@
 	mrs	x12, mdcr_el2
 	stp	x11, x12, [x0, #CTX_MAIR_EL2]
 
-#if ENABLE_SPE_FOR_LOWER_ELS
-	mrs	x13, PMSCR_EL2
-	str	x13, [x0, #CTX_PMSCR_EL2]
-#endif /* ENABLE_SPE_FOR_LOWER_ELS */
-
 	mrs	x14, sctlr_el2
 	str	x14, [x0, #CTX_SCTLR_EL2]
 
@@ -115,128 +159,10 @@
 	mrs	x15, vtcr_el2
 	mrs	x16, vttbr_el2
 	stp	x15, x16, [x0, #CTX_VTCR_EL2]
-
-#if CTX_INCLUDE_MTE_REGS
-	mrs	x9, TFSR_EL2
-	str	x9, [x0, #CTX_TFSR_EL2]
-#endif /* CTX_INCLUDE_MTE_REGS */
-
-#if ENABLE_MPAM_FOR_LOWER_ELS
-	mrs	x10, MPAM2_EL2
-	str	x10, [x0, #CTX_MPAM2_EL2]
-
-	mrs	x11, MPAMHCR_EL2
-	mrs	x12, MPAMVPM0_EL2
-	stp	x11, x12, [x0, #CTX_MPAMHCR_EL2]
-
-	mrs	x13, MPAMVPM1_EL2
-	mrs	x14, MPAMVPM2_EL2
-	stp	x13, x14, [x0, #CTX_MPAMVPM1_EL2]
-
-	mrs	x15, MPAMVPM3_EL2
-	mrs	x16, MPAMVPM4_EL2
-	stp	x15, x16, [x0, #CTX_MPAMVPM3_EL2]
-
-	mrs	x9, MPAMVPM5_EL2
-	mrs	x10, MPAMVPM6_EL2
-	stp	x9, x10, [x0, #CTX_MPAMVPM5_EL2]
-
-	mrs	x11, MPAMVPM7_EL2
-	mrs	x12, MPAMVPMV_EL2
-	stp	x11, x12, [x0, #CTX_MPAMVPM7_EL2]
-#endif /* ENABLE_MPAM_FOR_LOWER_ELS */
-
-#if ENABLE_FEAT_FGT
-	mrs	x13, HDFGRTR_EL2
-#if ENABLE_FEAT_AMUv1
-   	mrs	x14, HAFGRTR_EL2
-   	stp	x13, x14, [x0, #CTX_HDFGRTR_EL2]
-#else
-   	str	x13, [x0, #CTX_HDFGRTR_EL2]
-#endif /* ENABLE_FEAT_AMUv1 */
-	mrs	x15, HDFGWTR_EL2
-	mrs	x16, HFGITR_EL2
-	stp	x15, x16, [x0, #CTX_HDFGWTR_EL2]
-
-	mrs	x9, HFGRTR_EL2
-	mrs	x10, HFGWTR_EL2
-	stp	x9, x10, [x0, #CTX_HFGRTR_EL2]
-#endif /* ENABLE_FEAT_FGT */
-
-#if ENABLE_FEAT_ECV
-	mrs	x11, CNTPOFF_EL2
-	str	x11, [x0, #CTX_CNTPOFF_EL2]
-#endif /* ENABLE_FEAT_ECV */
-
-#if ENABLE_FEAT_VHE
-	/*
-	 * CONTEXTIDR_EL2 register is saved only when FEAT_VHE or
-	 * FEAT_Debugv8p2 (currently not in TF-A) is supported.
-	 */
-	mrs	x9, contextidr_el2
-	mrs	x10, ttbr1_el2
-	stp	x9, x10, [x0, #CTX_CONTEXTIDR_EL2]
-#endif /* ENABLE_FEAT_VHE */
-
-#if RAS_EXTENSION
-	/*
-	 * VDISR_EL2 and VSESR_EL2 registers are saved only when
-	 * FEAT_RAS is supported.
-	 */
-	mrs	x11, vdisr_el2
-	mrs	x12, vsesr_el2
-	stp	x11, x12, [x0, #CTX_VDISR_EL2]
-#endif /* RAS_EXTENSION */
-
-#if CTX_INCLUDE_NEVE_REGS
-	/*
-	 * VNCR_EL2 register is saved only when FEAT_NV2 is supported.
-	 */
-	mrs	x16, vncr_el2
-	str	x16, [x0, #CTX_VNCR_EL2]
-#endif /* CTX_INCLUDE_NEVE_REGS */
-
-#if ENABLE_TRF_FOR_NS
-	/*
-	 * TRFCR_EL2 register is saved only when FEAT_TRF is supported.
-	 */
-	mrs	x12, TRFCR_EL2
-	str	x12, [x0, #CTX_TRFCR_EL2]
-#endif /* ENABLE_TRF_FOR_NS */
-
-#if ENABLE_FEAT_CSV2_2
-	/*
-	 * SCXTNUM_EL2 register is saved only when FEAT_CSV2_2 is supported.
-	 */
-	mrs	x13, scxtnum_el2
-	str	x13, [x0, #CTX_SCXTNUM_EL2]
-#endif /* ENABLE_FEAT_CSV2_2 */
-
-#if ENABLE_FEAT_HCX
-	mrs	x14, hcrx_el2
-	str	x14, [x0, #CTX_HCRX_EL2]
-#endif /* ENABLE_FEAT_HCX */
-
 	ret
-endfunc el2_sysregs_context_save
+endfunc el2_sysregs_context_save_common
 
-
-/* -----------------------------------------------------
- * The following function strictly follows the AArch64
- * PCS to use x9-x16 (temporary caller-saved registers)
- * to restore EL2 system register context.  It assumes
- * that 'x0' is pointing to a 'el2_sys_regs' structure
- * from where the register context will be restored
-
- * The following registers are not restored
- * AMEVCNTVOFF0<n>_EL2
- * AMEVCNTVOFF1<n>_EL2
- * ICH_AP0R<n>_EL2
- * ICH_AP1R<n>_EL2
- * ICH_LR<n>_EL2
- * -----------------------------------------------------
- */
-func el2_sysregs_context_restore
+func el2_sysregs_context_restore_common
 	ldp	x9, x10, [x0, #CTX_ACTLR_EL2]
 	msr	actlr_el2, x9
 	msr	afsr0_el2, x10
@@ -281,11 +207,6 @@
 	msr	mair_el2, x11
 	msr	mdcr_el2, x12
 
-#if ENABLE_SPE_FOR_LOWER_ELS
-	ldr	x13, [x0, #CTX_PMSCR_EL2]
-	msr	PMSCR_EL2, x13
-#endif /* ENABLE_SPE_FOR_LOWER_ELS */
-
 	ldr	x14, [x0, #CTX_SCTLR_EL2]
 	msr	sctlr_el2, x14
 
@@ -308,13 +229,65 @@
 	ldp	x15, x16, [x0, #CTX_VTCR_EL2]
 	msr	vtcr_el2, x15
 	msr	vttbr_el2, x16
+	ret
+endfunc el2_sysregs_context_restore_common
+
+#if ENABLE_SPE_FOR_LOWER_ELS
+func el2_sysregs_context_save_spe
+	mrs	x13, PMSCR_EL2
+	str	x13, [x0, #CTX_PMSCR_EL2]
+	ret
+endfunc el2_sysregs_context_save_spe
+
+func el2_sysregs_context_restore_spe
+	ldr	x13, [x0, #CTX_PMSCR_EL2]
+	msr	PMSCR_EL2, x13
+	ret
+endfunc el2_sysregs_context_restore_spe
+#endif /* ENABLE_SPE_FOR_LOWER_ELS */
 
 #if CTX_INCLUDE_MTE_REGS
+func el2_sysregs_context_save_mte
+	mrs	x9, TFSR_EL2
+	str	x9, [x0, #CTX_TFSR_EL2]
+	ret
+endfunc el2_sysregs_context_save_mte
+
+func el2_sysregs_context_restore_mte
 	ldr	x9, [x0, #CTX_TFSR_EL2]
 	msr	TFSR_EL2, x9
+	ret
+endfunc el2_sysregs_context_restore_mte
 #endif /* CTX_INCLUDE_MTE_REGS */
 
 #if ENABLE_MPAM_FOR_LOWER_ELS
+func el2_sysregs_context_save_mpam
+	mrs	x10, MPAM2_EL2
+	str	x10, [x0, #CTX_MPAM2_EL2]
+
+	mrs	x11, MPAMHCR_EL2
+	mrs	x12, MPAMVPM0_EL2
+	stp	x11, x12, [x0, #CTX_MPAMHCR_EL2]
+
+	mrs	x13, MPAMVPM1_EL2
+	mrs	x14, MPAMVPM2_EL2
+	stp	x13, x14, [x0, #CTX_MPAMVPM1_EL2]
+
+	mrs	x15, MPAMVPM3_EL2
+	mrs	x16, MPAMVPM4_EL2
+	stp	x15, x16, [x0, #CTX_MPAMVPM3_EL2]
+
+	mrs	x9, MPAMVPM5_EL2
+	mrs	x10, MPAMVPM6_EL2
+	stp	x9, x10, [x0, #CTX_MPAMVPM5_EL2]
+
+	mrs	x11, MPAMVPM7_EL2
+	mrs	x12, MPAMVPMV_EL2
+	stp	x11, x12, [x0, #CTX_MPAMVPM7_EL2]
+	ret
+endfunc func el2_sysregs_context_save_mpam
+
+func el2_sysregs_context_restore_mpam
 	ldr	x10, [x0, #CTX_MPAM2_EL2]
 	msr	MPAM2_EL2, x10
 
@@ -337,10 +310,31 @@
 	ldp	x11, x12, [x0, #CTX_MPAMVPM7_EL2]
 	msr	MPAMVPM7_EL2, x11
 	msr	MPAMVPMV_EL2, x12
+	ret
+endfunc el2_sysregs_context_restore_mpam
 #endif /* ENABLE_MPAM_FOR_LOWER_ELS */
 
 #if ENABLE_FEAT_FGT
+func el2_sysregs_context_save_fgt
+	mrs	x13, HDFGRTR_EL2
 #if ENABLE_FEAT_AMUv1
+	mrs	x14, HAFGRTR_EL2
+	stp	x13, x14, [x0, #CTX_HDFGRTR_EL2]
+#else
+	str	x13, [x0, #CTX_HDFGRTR_EL2]
+#endif /* ENABLE_FEAT_AMUv1 */
+	mrs	x15, HDFGWTR_EL2
+	mrs	x16, HFGITR_EL2
+	stp	x15, x16, [x0, #CTX_HDFGWTR_EL2]
+
+	mrs	x9, HFGRTR_EL2
+	mrs	x10, HFGWTR_EL2
+	stp	x9, x10, [x0, #CTX_HFGRTR_EL2]
+	ret
+endfunc el2_sysregs_context_save_fgt
+
+func el2_sysregs_context_restore_fgt
+	#if ENABLE_FEAT_AMUv1
 	ldp	x13, x14, [x0, #CTX_HDFGRTR_EL2]
 	msr	HAFGRTR_EL2, x14
 #else
@@ -355,14 +349,37 @@
 	ldp	x9, x10, [x0, #CTX_HFGRTR_EL2]
 	msr	HFGRTR_EL2, x9
 	msr	HFGWTR_EL2, x10
+	ret
+endfunc el2_sysregs_context_restore_fgt
 #endif /* ENABLE_FEAT_FGT */
 
 #if ENABLE_FEAT_ECV
+func el2_sysregs_context_save_ecv
+	mrs	x11, CNTPOFF_EL2
+	str	x11, [x0, #CTX_CNTPOFF_EL2]
+	ret
+endfunc el2_sysregs_context_save_ecv
+
+func el2_sysregs_context_restore_ecv
 	ldr	x11, [x0, #CTX_CNTPOFF_EL2]
 	msr	CNTPOFF_EL2, x11
+	ret
+endfunc el2_sysregs_context_restore_ecv
 #endif /* ENABLE_FEAT_ECV */
 
 #if ENABLE_FEAT_VHE
+func el2_sysregs_context_save_vhe
+	/*
+	 * CONTEXTIDR_EL2 register is saved only when FEAT_VHE or
+	 * FEAT_Debugv8p2 (currently not in TF-A) is supported.
+	 */
+	mrs	x9, contextidr_el2
+	mrs	x10, ttbr1_el2
+	stp	x9, x10, [x0, #CTX_CONTEXTIDR_EL2]
+	ret
+endfunc el2_sysregs_context_save_vhe
+
+func el2_sysregs_context_restore_vhe
 	/*
 	 * CONTEXTIDR_EL2 register is restored only when FEAT_VHE or
 	 * FEAT_Debugv8p2 (currently not in TF-A) is supported.
@@ -370,9 +387,23 @@
 	ldp	x9, x10, [x0, #CTX_CONTEXTIDR_EL2]
 	msr	contextidr_el2, x9
 	msr	ttbr1_el2, x10
+	ret
+endfunc el2_sysregs_context_restore_vhe
 #endif /* ENABLE_FEAT_VHE */
 
 #if RAS_EXTENSION
+func el2_sysregs_context_save_ras
+	/*
+	 * VDISR_EL2 and VSESR_EL2 registers are saved only when
+	 * FEAT_RAS is supported.
+	 */
+	mrs	x11, vdisr_el2
+	mrs	x12, vsesr_el2
+	stp	x11, x12, [x0, #CTX_VDISR_EL2]
+	ret
+endfunc el2_sysregs_context_save_ras
+
+func el2_sysregs_context_restore_ras
 	/*
 	 * VDISR_EL2 and VSESR_EL2 registers are restored only when FEAT_RAS
 	 * is supported.
@@ -380,40 +411,83 @@
 	ldp	x11, x12, [x0, #CTX_VDISR_EL2]
 	msr	vdisr_el2, x11
 	msr	vsesr_el2, x12
+	ret
+endfunc el2_sysregs_context_restore_ras
 #endif /* RAS_EXTENSION */
 
 #if CTX_INCLUDE_NEVE_REGS
+func el2_sysregs_context_save_nv2
+	/*
+	 * VNCR_EL2 register is saved only when FEAT_NV2 is supported.
+	 */
+	mrs	x16, vncr_el2
+	str	x16, [x0, #CTX_VNCR_EL2]
+	ret
+endfunc el2_sysregs_context_save_nv2
+
+func el2_sysregs_context_restore_nv2
 	/*
 	 * VNCR_EL2 register is restored only when FEAT_NV2 is supported.
 	 */
 	ldr	x16, [x0, #CTX_VNCR_EL2]
 	msr	vncr_el2, x16
+	ret
+endfunc el2_sysregs_context_restore_nv2
 #endif /* CTX_INCLUDE_NEVE_REGS */
 
 #if ENABLE_TRF_FOR_NS
+func el2_sysregs_context_save_trf
+	/*
+	 * TRFCR_EL2 register is saved only when FEAT_TRF is supported.
+	 */
+	mrs	x12, TRFCR_EL2
+	str	x12, [x0, #CTX_TRFCR_EL2]
+	ret
+endfunc el2_sysregs_context_save_trf
+
+func el2_sysregs_context_restore_trf
 	/*
 	 * TRFCR_EL2 register is restored only when FEAT_TRF is supported.
 	 */
 	ldr	x12, [x0, #CTX_TRFCR_EL2]
 	msr	TRFCR_EL2, x12
+	ret
+endfunc el2_sysregs_context_restore_trf
 #endif /* ENABLE_TRF_FOR_NS */
 
 #if ENABLE_FEAT_CSV2_2
+func el2_sysregs_context_save_csv2
+	/*
+	 * SCXTNUM_EL2 register is saved only when FEAT_CSV2_2 is supported.
+	 */
+	mrs	x13, scxtnum_el2
+	str	x13, [x0, #CTX_SCXTNUM_EL2]
+	ret
+endfunc el2_sysregs_context_save_csv2
+
+func el2_sysregs_context_restore_csv2
 	/*
 	 * SCXTNUM_EL2 register is restored only when FEAT_CSV2_2 is supported.
 	 */
 	ldr	x13, [x0, #CTX_SCXTNUM_EL2]
 	msr	scxtnum_el2, x13
+	ret
+endfunc el2_sysregs_context_restore_csv2
 #endif /* ENABLE_FEAT_CSV2_2 */
 
 #if ENABLE_FEAT_HCX
+func el2_sysregs_context_save_hcx
+	mrs	x14, hcrx_el2
+	str	x14, [x0, #CTX_HCRX_EL2]
+	ret
+endfunc el2_sysregs_context_save_hcx
+
+func el2_sysregs_context_restore_hcx
 	ldr	x14, [x0, #CTX_HCRX_EL2]
 	msr	hcrx_el2, x14
-#endif /* ENABLE_FEAT_HCX */
-
 	ret
-endfunc el2_sysregs_context_restore
-
+endfunc el2_sysregs_context_restore_hcx
+#endif /* ENABLE_FEAT_HCX */
 #endif /* CTX_INCLUDE_EL2_REGS */
 
 /* ------------------------------------------------------------------

diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 459ca2c..da610d0 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -20,6 +20,7 @@
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/el3_runtime/pubsub_events.h>
 #include <lib/extensions/amu.h>
+#include <lib/extensions/brbe.h>
 #include <lib/extensions/mpam.h>
 #include <lib/extensions/sme.h>
 #include <lib/extensions/spe.h>
@@ -27,11 +28,72 @@
 #include <lib/extensions/sys_reg_trace.h>
 #include <lib/extensions/trbe.h>
 #include <lib/extensions/trf.h>
-#include <lib/extensions/twed.h>
 #include <lib/utils.h>
 
+#if ENABLE_FEAT_TWED
+/* Make sure delay value fits within the range(0-15) */
+CASSERT(((TWED_DELAY & ~SCR_TWEDEL_MASK) == 0U), assert_twed_delay_value_check);
+#endif /* ENABLE_FEAT_TWED */
+
 static void manage_extensions_secure(cpu_context_t *ctx);
 
+static void setup_el1_context(cpu_context_t *ctx, const struct entry_point_info *ep)
+{
+	u_register_t sctlr_elx, actlr_elx;
+
+	/*
+	 * Initialise SCTLR_EL1 to the reset value corresponding to the target
+	 * execution state setting all fields rather than relying on the hw.
+	 * Some fields have architecturally UNKNOWN reset values and these are
+	 * set to zero.
+	 *
+	 * SCTLR.EE: Endianness is taken from the entrypoint attributes.
+	 *
+	 * SCTLR.M, SCTLR.C and SCTLR.I: These fields must be zero (as
+	 * required by PSCI specification)
+	 */
+	sctlr_elx = (EP_GET_EE(ep->h.attr) != 0U) ? SCTLR_EE_BIT : 0UL;
+	if (GET_RW(ep->spsr) == MODE_RW_64) {
+		sctlr_elx |= SCTLR_EL1_RES1;
+	} else {
+		/*
+		 * If the target execution state is AArch32 then the following
+		 * fields need to be set.
+		 *
+		 * SCTRL_EL1.nTWE: Set to one so that EL0 execution of WFE
+		 *  instructions are not trapped to EL1.
+		 *
+		 * SCTLR_EL1.nTWI: Set to one so that EL0 execution of WFI
+		 *  instructions are not trapped to EL1.
+		 *
+		 * SCTLR_EL1.CP15BEN: Set to one to enable EL0 execution of the
+		 *  CP15DMB, CP15DSB, and CP15ISB instructions.
+		 */
+		sctlr_elx |= SCTLR_AARCH32_EL1_RES1 | SCTLR_CP15BEN_BIT
+					| SCTLR_NTWI_BIT | SCTLR_NTWE_BIT;
+	}
+
+#if ERRATA_A75_764081
+	/*
+	 * If workaround of errata 764081 for Cortex-A75 is used then set
+	 * SCTLR_EL1.IESB to enable Implicit Error Synchronization Barrier.
+	 */
+	sctlr_elx |= SCTLR_IESB_BIT;
+#endif
+	/* Store the initialised SCTLR_EL1 value in the cpu_context */
+	write_ctx_reg(get_el1_sysregs_ctx(ctx), CTX_SCTLR_EL1, sctlr_elx);
+
+	/*
+	 * Base the context ACTLR_EL1 on the current value, as it is
+	 * implementation defined. The context restore process will write
+	 * the value from the context to the actual register and can cause
+	 * problems for processor cores that don't expect certain bits to
+	 * be zero.
+	 */
+	actlr_elx = read_actlr_el1();
+	write_ctx_reg((get_el1_sysregs_ctx(ctx)), (CTX_ACTLR_EL1), (actlr_elx));
+}
+
 /******************************************************************************
  * This function performs initializations that are specific to SECURE state
  * and updates the cpu context specified by 'ctx'.
@@ -81,6 +143,14 @@
 
 	write_ctx_reg(state, CTX_SCR_EL3, scr_el3);
 
+	/*
+	 * Initialize EL1 context registers unless SPMC is running
+	 * at S-EL2.
+	 */
+#if !SPMD_SPM_AT_SEL2
+	setup_el1_context(ctx, ep);
+#endif
+
 	manage_extensions_secure(ctx);
 }
 
@@ -143,6 +213,9 @@
 #endif
 	write_ctx_reg(state, CTX_SCR_EL3, scr_el3);
 
+	/* Initialize EL1 context registers */
+	setup_el1_context(ctx, ep);
+
 	/* Initialize EL2 context registers */
 #if CTX_INCLUDE_EL2_REGS
 
@@ -182,7 +255,6 @@
 	u_register_t scr_el3;
 	el3_state_t *state;
 	gp_regs_t *gp_regs;
-	u_register_t sctlr_elx, actlr_elx;
 
 	/* Clear any residual register values from the context */
 	zeromem(ctx, sizeof(*ctx));
@@ -210,8 +282,10 @@
 
 	/*
 	 * SCR_EL3.ST: Traps Secure EL1 accesses to the Counter-timer Physical
-	 *  Secure timer registers to EL3, from AArch64 state only, if specified
-	 *  by the entrypoint attributes.
+	 * Secure timer registers to EL3, from AArch64 state only, if specified
+	 * by the entrypoint attributes. If SEL2 is present and enabled, the ST
+	 * bit always behaves as 1 (i.e. secure physical timer register access
+	 * is not trapped)
 	 */
 	if (EP_GET_ST(ep->h.attr) != 0U) {
 		scr_el3 |= SCR_ST_BIT;
@@ -279,90 +353,16 @@
 		}
 	}
 
-	/*
-	 * FEAT_AMUv1p1 virtual offset registers are only accessible from EL3
-	 * and EL2, when clear, this bit traps accesses from EL2 so we set it
-	 * to 1 when EL2 is present.
-	 */
-	if (is_armv8_6_feat_amuv1p1_present() &&
-		(el_implemented(2) != EL_IMPL_NONE)) {
-		scr_el3 |= SCR_AMVOFFEN_BIT;
-	}
-
-	/*
-	 * Initialise SCTLR_EL1 to the reset value corresponding to the target
-	 * execution state setting all fields rather than relying of the hw.
-	 * Some fields have architecturally UNKNOWN reset values and these are
-	 * set to zero.
-	 *
-	 * SCTLR.EE: Endianness is taken from the entrypoint attributes.
-	 *
-	 * SCTLR.M, SCTLR.C and SCTLR.I: These fields must be zero (as
-	 *  required by PSCI specification)
-	 */
-	sctlr_elx = (EP_GET_EE(ep->h.attr) != 0U) ? SCTLR_EE_BIT : 0U;
-	if (GET_RW(ep->spsr) == MODE_RW_64) {
-		sctlr_elx |= SCTLR_EL1_RES1;
-	} else {
-		/*
-		 * If the target execution state is AArch32 then the following
-		 * fields need to be set.
-		 *
-		 * SCTRL_EL1.nTWE: Set to one so that EL0 execution of WFE
-		 *  instructions are not trapped to EL1.
-		 *
-		 * SCTLR_EL1.nTWI: Set to one so that EL0 execution of WFI
-		 *  instructions are not trapped to EL1.
-		 *
-		 * SCTLR_EL1.CP15BEN: Set to one to enable EL0 execution of the
-		 *  CP15DMB, CP15DSB, and CP15ISB instructions.
-		 */
-		sctlr_elx |= SCTLR_AARCH32_EL1_RES1 | SCTLR_CP15BEN_BIT
-					| SCTLR_NTWI_BIT | SCTLR_NTWE_BIT;
-	}
-
-#if ERRATA_A75_764081
-	/*
-	 * If workaround of errata 764081 for Cortex-A75 is used then set
-	 * SCTLR_EL1.IESB to enable Implicit Error Synchronization Barrier.
-	 */
-	sctlr_elx |= SCTLR_IESB_BIT;
-#endif
-
+#if ENABLE_FEAT_TWED
 	/* Enable WFE trap delay in SCR_EL3 if supported and configured */
-	if (is_armv8_6_twed_present()) {
-		uint32_t delay = plat_arm_set_twedel_scr_el3();
-
-		if (delay != TWED_DISABLED) {
-			/* Make sure delay value fits */
-			assert((delay & ~SCR_TWEDEL_MASK) == 0U);
-
-			/* Set delay in SCR_EL3 */
-			scr_el3 &= ~(SCR_TWEDEL_MASK << SCR_TWEDEL_SHIFT);
-			scr_el3 |= ((delay & SCR_TWEDEL_MASK)
-					<< SCR_TWEDEL_SHIFT);
-
-			/* Enable WFE delay */
-			scr_el3 |= SCR_TWEDEn_BIT;
-		}
-	}
-
-	/*
-	 * Store the initialised SCTLR_EL1 value in the cpu_context - SCTLR_EL2
-	 * and other EL2 registers are set up by cm_prepare_el3_exit() as they
-	 * are not part of the stored cpu_context.
-	 */
-	write_ctx_reg(get_el1_sysregs_ctx(ctx), CTX_SCTLR_EL1, sctlr_elx);
+	/* Set delay in SCR_EL3 */
+	scr_el3 &= ~(SCR_TWEDEL_MASK << SCR_TWEDEL_SHIFT);
+	scr_el3 |= ((TWED_DELAY & SCR_TWEDEL_MASK)
+			<< SCR_TWEDEL_SHIFT);
 
-	/*
-	 * Base the context ACTLR_EL1 on the current value, as it is
-	 * implementation defined. The context restore process will write
-	 * the value from the context to the actual register and can cause
-	 * problems for processor cores that don't expect certain bits to
-	 * be zero.
-	 */
-	actlr_elx = read_actlr_el1();
-	write_ctx_reg((get_el1_sysregs_ctx(ctx)), (CTX_ACTLR_EL1), (actlr_elx));
+	/* Enable WFE delay */
+	scr_el3 |= SCR_TWEDEn_BIT;
+#endif /* ENABLE_FEAT_TWED */
 
 	/*
 	 * Populate EL3 state so that we've the right context
@@ -473,6 +473,10 @@
 	trbe_enable();
 #endif /* ENABLE_TRBE_FOR_NS */
 
+#if ENABLE_BRBE_FOR_NS
+	brbe_enable();
+#endif /* ENABLE_BRBE_FOR_NS */
+
 #if ENABLE_SYS_REG_TRACE_FOR_NS
 	sys_reg_trace_enable(ctx);
 #endif /* ENABLE_SYS_REG_TRACE_FOR_NS */
@@ -786,11 +790,47 @@
 	if ((security_state != SECURE) ||
 	    ((security_state == SECURE) && ((scr_el3 & SCR_EEL2_BIT) != 0U))) {
 		cpu_context_t *ctx;
+		el2_sysregs_t *el2_sysregs_ctx;
 
 		ctx = cm_get_context(security_state);
 		assert(ctx != NULL);
 
-		el2_sysregs_context_save(get_el2_sysregs_ctx(ctx));
+		el2_sysregs_ctx = get_el2_sysregs_ctx(ctx);
+
+		el2_sysregs_context_save_common(el2_sysregs_ctx);
+#if ENABLE_SPE_FOR_LOWER_ELS
+		el2_sysregs_context_save_spe(el2_sysregs_ctx);
+#endif
+#if CTX_INCLUDE_MTE_REGS
+		el2_sysregs_context_save_mte(el2_sysregs_ctx);
+#endif
+#if ENABLE_MPAM_FOR_LOWER_ELS
+		el2_sysregs_context_save_mpam(el2_sysregs_ctx);
+#endif
+#if ENABLE_FEAT_FGT
+		el2_sysregs_context_save_fgt(el2_sysregs_ctx);
+#endif
+#if ENABLE_FEAT_ECV
+		el2_sysregs_context_save_ecv(el2_sysregs_ctx);
+#endif
+#if ENABLE_FEAT_VHE
+		el2_sysregs_context_save_vhe(el2_sysregs_ctx);
+#endif
+#if RAS_EXTENSION
+		el2_sysregs_context_save_ras(el2_sysregs_ctx);
+#endif
+#if CTX_INCLUDE_NEVE_REGS
+		el2_sysregs_context_save_nv2(el2_sysregs_ctx);
+#endif
+#if ENABLE_TRF_FOR_NS
+		el2_sysregs_context_save_trf(el2_sysregs_ctx);
+#endif
+#if ENABLE_FEAT_CSV2_2
+		el2_sysregs_context_save_csv2(el2_sysregs_ctx);
+#endif
+#if ENABLE_FEAT_HCX
+		el2_sysregs_context_save_hcx(el2_sysregs_ctx);
+#endif
 	}
 }
 
@@ -808,11 +848,47 @@
 	if ((security_state != SECURE) ||
 	    ((security_state == SECURE) && ((scr_el3 & SCR_EEL2_BIT) != 0U))) {
 		cpu_context_t *ctx;
+		el2_sysregs_t *el2_sysregs_ctx;
 
 		ctx = cm_get_context(security_state);
 		assert(ctx != NULL);
 
-		el2_sysregs_context_restore(get_el2_sysregs_ctx(ctx));
+		el2_sysregs_ctx = get_el2_sysregs_ctx(ctx);
+
+		el2_sysregs_context_restore_common(el2_sysregs_ctx);
+#if ENABLE_SPE_FOR_LOWER_ELS
+		el2_sysregs_context_restore_spe(el2_sysregs_ctx);
+#endif
+#if CTX_INCLUDE_MTE_REGS
+		el2_sysregs_context_restore_mte(el2_sysregs_ctx);
+#endif
+#if ENABLE_MPAM_FOR_LOWER_ELS
+		el2_sysregs_context_restore_mpam(el2_sysregs_ctx);
+#endif
+#if ENABLE_FEAT_FGT
+		el2_sysregs_context_restore_fgt(el2_sysregs_ctx);
+#endif
+#if ENABLE_FEAT_ECV
+		el2_sysregs_context_restore_ecv(el2_sysregs_ctx);
+#endif
+#if ENABLE_FEAT_VHE
+		el2_sysregs_context_restore_vhe(el2_sysregs_ctx);
+#endif
+#if RAS_EXTENSION
+		el2_sysregs_context_restore_ras(el2_sysregs_ctx);
+#endif
+#if CTX_INCLUDE_NEVE_REGS
+		el2_sysregs_context_restore_nv2(el2_sysregs_ctx);
+#endif
+#if ENABLE_TRF_FOR_NS
+		el2_sysregs_context_restore_trf(el2_sysregs_ctx);
+#endif
+#if ENABLE_FEAT_CSV2_2
+		el2_sysregs_context_restore_csv2(el2_sysregs_ctx);
+#endif
+#if ENABLE_FEAT_HCX
+		el2_sysregs_context_restore_hcx(el2_sysregs_ctx);
+#endif
 	}
 }
 #endif /* CTX_INCLUDE_EL2_REGS */
@@ -829,6 +905,14 @@
 	cpu_context_t *ctx = cm_get_context(NON_SECURE);
 	assert(ctx != NULL);
 
+	/* Assert that EL2 is used. */
+#if ENABLE_ASSERTIONS
+	el3_state_t *state = get_el3state_ctx(ctx);
+	u_register_t scr_el3 = read_ctx_reg(state, CTX_SCR_EL3);
+#endif
+	assert(((scr_el3 & SCR_HCE_BIT) != 0UL) &&
+			(el_implemented(2U) != EL_IMPL_NONE));
+
 	/*
 	 * Currently some extensions are configured using
 	 * direct register updates. Therefore, do this here
@@ -842,6 +926,12 @@
 	 */
 	write_scr_el3(read_scr_el3() | SCR_NS_BIT);
 
+	/*
+	 * Ensure the NS bit change is committed before the EL2/EL1
+	 * state restoration.
+	 */
+	isb();
+
 	/* Restore EL2 and EL1 sysreg contexts */
 	cm_el2_sysregs_context_restore(NON_SECURE);
 	cm_el1_sysregs_context_restore(NON_SECURE);

diff --git a/lib/extensions/amu/aarch64/amu.c b/lib/extensions/amu/aarch64/amu.c
index d329c3d..72566fd 100644
--- a/lib/extensions/amu/aarch64/amu.c
+++ b/lib/extensions/amu/aarch64/amu.c

@@ -75,7 +75,7 @@
 		((value << CPTR_EL2_TAM_SHIFT) & CPTR_EL2_TAM_BIT));
 }
 
-static inline __unused void write_cptr_el3_tam(cpu_context_t *ctx, uint64_t tam)
+static inline __unused void ctx_write_cptr_el3_tam(cpu_context_t *ctx, uint64_t tam)
 {
 	uint64_t value = read_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3);
 
@@ -85,6 +85,16 @@
 	write_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3, value);
 }
 
+static inline __unused void ctx_write_scr_el3_amvoffen(cpu_context_t *ctx, uint64_t amvoffen)
+{
+	uint64_t value = read_ctx_reg(get_el3state_ctx(ctx), CTX_SCR_EL3);
+
+	value &= ~SCR_AMVOFFEN_BIT;
+	value |= (amvoffen << SCR_AMVOFFEN_SHIFT) & SCR_AMVOFFEN_BIT;
+
+	write_ctx_reg(get_el3state_ctx(ctx), CTX_SCR_EL3, value);
+}
+
 static inline __unused void write_hcr_el2_amvoffen(uint64_t value)
 {
 	write_hcr_el2((read_hcr_el2() & ~HCR_AMVOFFEN_BIT) |
@@ -226,7 +236,7 @@
 	 * in 'ctx'. Set CPTR_EL3.TAM to zero so that any accesses to
 	 * the Activity Monitor registers do not trap to EL3.
 	 */
-	write_cptr_el3_tam(ctx, 0U);
+	ctx_write_cptr_el3_tam(ctx, 0U);
 
 	/*
 	 * Retrieve the number of architected counters. All of these counters
@@ -285,6 +295,13 @@
 			 * used.
 			 */
 			write_hcr_el2_amvoffen(0U);
+		} else {
+			/*
+			 * Virtual offset registers are only accessible from EL3
+			 * and EL2, when clear, this bit traps accesses from EL2
+			 * so we set it to 1 when EL2 is present.
+			 */
+			ctx_write_scr_el3_amvoffen(ctx, 1U);
 		}
 
 #if AMU_RESTRICT_COUNTERS

diff --git a/lib/extensions/brbe/brbe.c b/lib/extensions/brbe/brbe.c
new file mode 100644
index 0000000..1982619
--- /dev/null
+++ b/lib/extensions/brbe/brbe.c

@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_features.h>
+#include <arch_helpers.h>
+
+void brbe_enable(void)
+{
+	uint64_t val;
+
+	if (is_feat_brbe_present()) {
+		/*
+		 * MDCR_EL3.SBRBE = 0b01
+		 *
+		 * Allows BRBE usage in non-secure world and prohibited in
+		 * secure world.
+		 */
+		val = read_mdcr_el3();
+		val &= ~(MDCR_SBRBE_MASK << MDCR_SBRBE_SHIFT);
+		val |= (0x1UL << MDCR_SBRBE_SHIFT);
+		write_mdcr_el3(val);
+	}
+}

diff --git a/lib/extensions/sme/sme.c b/lib/extensions/sme/sme.c
index 1c2b984..958b623 100644
--- a/lib/extensions/sme/sme.c
+++ b/lib/extensions/sme/sme.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -37,6 +37,8 @@
 
 	/* Make sure SME is implemented in hardware before continuing. */
 	if (!feat_sme_supported()) {
+		/* Perhaps the hardware supports SVE only */
+		sve_enable(context);
 		return;
 	}
 
@@ -83,6 +85,8 @@
 
 	/* Make sure SME is implemented in hardware before continuing. */
 	if (!feat_sme_supported()) {
+		/* Perhaps the hardware supports SVE only */
+		sve_disable(context);
 		return;
 	}
 

diff --git a/lib/extensions/trbe/trbe.c b/lib/extensions/trbe/trbe.c
index 9f754d5..b346387 100644
--- a/lib/extensions/trbe/trbe.c
+++ b/lib/extensions/trbe/trbe.c

@@ -1,10 +1,11 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #include <arch.h>
+#include <arch_features.h>
 #include <arch_helpers.h>
 #include <lib/el3_runtime/pubsub.h>
 #include <lib/extensions/trbe.h>
@@ -18,20 +19,11 @@
 	__asm__ volatile("hint #18");
 }
 
-static bool trbe_supported(void)
-{
-	uint64_t features;
-
-	features = read_id_aa64dfr0_el1() >> ID_AA64DFR0_TRACEBUFFER_SHIFT;
-	return ((features & ID_AA64DFR0_TRACEBUFFER_MASK) ==
-		ID_AA64DFR0_TRACEBUFFER_SUPPORTED);
-}
-
 void trbe_enable(void)
 {
 	uint64_t val;
 
-	if (trbe_supported()) {
+	if (is_feat_trbe_present()) {
 		/*
 		 * MDCR_EL3.NSTB = 0b11
 		 * Allow access of trace buffer control registers from NS-EL1
@@ -46,7 +38,7 @@
 
 static void *trbe_drain_trace_buffers_hook(const void *arg __unused)
 {
-	if (trbe_supported()) {
+	if (is_feat_trbe_present()) {
 		/*
 		 * Before switching from normal world to secure world
 		 * the trace buffers need to be drained out to memory. This is

diff --git a/lib/fconf/fconf_dyn_cfg_getter.c b/lib/fconf/fconf_dyn_cfg_getter.c
index 34623fb..3038c09 100644
--- a/lib/fconf/fconf_dyn_cfg_getter.c
+++ b/lib/fconf/fconf_dyn_cfg_getter.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2019-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -29,13 +29,15 @@
  * This function is used to alloc memory for config information from
  * global pool and set the configuration information.
  */
-void set_config_info(uintptr_t config_addr, uint32_t config_max_size,
-			unsigned int config_id)
+void set_config_info(uintptr_t config_addr, uintptr_t ns_config_addr,
+		     uint32_t config_max_size,
+		     unsigned int config_id)
 {
 	struct dyn_cfg_dtb_info_t *dtb_info;
 
 	dtb_info = pool_alloc(&dtb_info_pool);
 	dtb_info->config_addr = config_addr;
+	dtb_info->ns_config_addr = ns_config_addr;
 	dtb_info->config_max_size = config_max_size;
 	dtb_info->config_id = config_id;
 }
@@ -88,7 +90,7 @@
 	 */
 	if (dtb_infos[0].config_id == 0U) {
 		uint32_t config_max_size = fdt_totalsize(dtb);
-		set_config_info(config, config_max_size, FW_CONFIG_ID);
+		set_config_info(config, ~0UL, config_max_size, FW_CONFIG_ID);
 	}
 
 	/* Find the node offset point to "fconf,dyn_cfg-dtb_registry" compatible property */
@@ -102,6 +104,7 @@
 	fdt_for_each_subnode(child, dtb, node) {
 		uint32_t config_max_size, config_id;
 		uintptr_t config_addr;
+		uintptr_t ns_config_addr = ~0UL;
 		uint64_t val64;
 
 		/* Read configuration dtb information */
@@ -129,7 +132,14 @@
 		VERBOSE("\tmax-size = 0x%x\n", config_max_size);
 		VERBOSE("\tconfig-id = %u\n", config_id);
 
-		set_config_info(config_addr, config_max_size, config_id);
+		rc = fdt_read_uint64(dtb, child, "ns-load-address", &val64);
+		if (rc == 0) {
+			ns_config_addr = (uintptr_t)val64;
+			VERBOSE("\tns-load-address = %lx\n", ns_config_addr);
+		}
+
+		set_config_info(config_addr, ns_config_addr, config_max_size,
+				config_id);
 	}
 
 	if ((child < 0) && (child != -FDT_ERR_NOTFOUND)) {

diff --git a/lib/libc/snprintf.c b/lib/libc/snprintf.c
index 675d243..12f51c0 100644
--- a/lib/libc/snprintf.c
+++ b/lib/libc/snprintf.c

@@ -11,6 +11,16 @@
 #include <common/debug.h>
 #include <plat/common/platform.h>
 
+#define get_num_va_args(_args, _lcount)				\
+	(((_lcount) > 1)  ? va_arg(_args, long long int) :	\
+	(((_lcount) == 1) ? va_arg(_args, long int) :		\
+			    va_arg(_args, int)))
+
+#define get_unum_va_args(_args, _lcount)				\
+	(((_lcount) > 1)  ? va_arg(_args, unsigned long long int) :	\
+	(((_lcount) == 1) ? va_arg(_args, unsigned long int) :		\
+			    va_arg(_args, unsigned int)))
+
 #define CHECK_AND_PUT_CHAR(buf, size, chars_printed, ch)	\
 	do {						\
 		if ((chars_printed) < (size)) {		\
@@ -80,6 +90,11 @@
  * %u - unsigned decimal format
  * %p - pointer format
  *
+ * The following length specifiers are supported by this print
+ * %l - long int
+ * %ll - long long int
+ * %z - size_t sized integer formats
+ *
  * The following padding specifiers are supported by this print
  * %0NN - Left-pad the number with 0s (NN is a decimal number)
  * %NN - Left-pad the number or string with spaces (NN is a decimal number)
@@ -101,6 +116,7 @@
 	bool left;
 	bool capitalise;
 	size_t chars_printed = 0U;
+	unsigned int l_count;
 
 	if (n == 0U) {
 		/* There isn't space for anything. */
@@ -118,6 +134,7 @@
 		padc ='\0';
 		padn = 0;
 		capitalise = false;
+		l_count = 0;
 
 		if (*fmt == '%') {
 			fmt++;
@@ -152,7 +169,7 @@
 
 			case 'i':
 			case 'd':
-				num = va_arg(args, int);
+				num = get_num_va_args(args, l_count);
 
 				if (num < 0) {
 					CHECK_AND_PUT_CHAR(s, n, chars_printed,
@@ -170,10 +187,18 @@
 				string_print(&s, n, &chars_printed, str);
 				break;
 			case 'u':
-				unum = va_arg(args, unsigned int);
+				unum = get_unum_va_args(args, l_count);
 				unsigned_num_print(&s, n, &chars_printed,
 						   unum, 10, padc, padn, false);
 				break;
+			case 'z':
+				l_count = 1;
+				fmt++;
+				goto loop;
+			case 'l':
+				l_count++;
+				fmt++;
+				goto loop;
 			case 'p':
 				unum = (uintptr_t)va_arg(args, void *);
 				if (unum > 0U) {
@@ -186,7 +211,7 @@
 			case 'X':
 				capitalise = true;
 			case 'x':
-				unum = va_arg(args, unsigned int);
+				unum = get_unum_va_args(args, l_count);
 				unsigned_num_print(&s, n, &chars_printed,
 						   unum, 16, padc, padn,
 						   capitalise);

diff --git a/lib/optee/optee_utils.c b/lib/optee/optee_utils.c
index 72979cd..d30248f 100644
--- a/lib/optee/optee_utils.c
+++ b/lib/optee/optee_utils.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -192,8 +192,17 @@
 				&header->optee_image_list[num]);
 		} else if (header->optee_image_list[num].image_id ==
 				OPTEE_PAGED_IMAGE_ID) {
-			ret = parse_optee_image(paged_image_info,
-				&header->optee_image_list[num]);
+			if (paged_image_info == NULL) {
+				if (header->optee_image_list[num].size != 0U) {
+					ERROR("Paged image is not supported\n");
+					return -1;
+				}
+
+				continue;
+			} else {
+				ret = parse_optee_image(paged_image_info,
+							&header->optee_image_list[num]);
+			}
 		} else {
 			ERROR("Parse optee image failed.\n");
 			return -1;
@@ -215,8 +224,10 @@
 	 * header image arguments so that can be read by the
 	 * BL32 SPD.
 	 */
-	header_ep->args.arg1 = paged_image_info->image_base;
-	header_ep->args.arg2 = paged_image_info->image_size;
+	if (paged_image_info != NULL) {
+		header_ep->args.arg1 = paged_image_info->image_base;
+		header_ep->args.arg2 = paged_image_info->image_size;
+	}
 
 	/* Set OPTEE runtime arch - aarch32/aarch64 */
 	if (header->arch == 0) {

diff --git a/lib/psa/initial_attestation.c b/lib/psa/initial_attestation.c
new file mode 100644
index 0000000..44498a8
--- /dev/null
+++ b/lib/psa/initial_attestation.c

@@ -0,0 +1,163 @@
+/*
+ * Copyright (c) 2018-2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <initial_attestation.h>
+#include <psa/client.h>
+#include <psa_manifest/sid.h>
+
+#if !PLAT_RSS_NOT_SUPPORTED
+psa_status_t
+psa_initial_attest_get_token(const uint8_t *auth_challenge,
+			     size_t         challenge_size,
+			     uint8_t       *token_buf,
+			     size_t         token_buf_size,
+			     size_t        *token_size)
+{
+	psa_status_t status;
+	psa_invec in_vec[] = {
+		{auth_challenge, challenge_size}
+	};
+	psa_outvec out_vec[] = {
+		{token_buf, token_buf_size},
+	};
+
+	status = psa_call(RSS_ATTESTATION_SERVICE_HANDLE, RSS_ATTEST_GET_TOKEN,
+			  in_vec, IOVEC_LEN(in_vec),
+			  out_vec, IOVEC_LEN(out_vec));
+
+	if (status == PSA_SUCCESS) {
+		*token_size = out_vec[0].len;
+	}
+
+	return status;
+}
+
+#else /* !PLAT_RSS_NOT_SUPPORTED */
+
+#include <string.h>
+
+static const uint8_t platform_token[] = {
+	0xD2, 0x84, 0x43, 0xA1, 0x01, 0x26, 0xA0, 0x59,
+	0x02, 0xBE, 0xAA, 0x3A, 0x00, 0x01, 0x24, 0xFF,
+	0x58, 0x20, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB,
+	0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB,
+	0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB,
+	0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB,
+	0xAB, 0xAB, 0x3A, 0x00, 0x01, 0x24, 0xFB, 0x58,
+	0x20, 0xA0, 0xA1, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6,
+	0xA7, 0xA8, 0xA9, 0xAA, 0xAB, 0xAC, 0xAD, 0xAE,
+	0xAF, 0xB0, 0xB1, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6,
+	0xB7, 0xB8, 0xB9, 0xBA, 0xBB, 0xBC, 0xBD, 0xBE,
+	0xBF, 0x3A, 0x00, 0x01, 0x25, 0x00, 0x58, 0x21,
+	0x01, 0xFA, 0x58, 0x75, 0x5F, 0x65, 0x86, 0x27,
+	0xCE, 0x54, 0x60, 0xF2, 0x9B, 0x75, 0x29, 0x67,
+	0x13, 0x24, 0x8C, 0xAE, 0x7A, 0xD9, 0xE2, 0x98,
+	0x4B, 0x90, 0x28, 0x0E, 0xFC, 0xBC, 0xB5, 0x02,
+	0x48, 0x3A, 0x00, 0x01, 0x24, 0xFA, 0x58, 0x20,
+	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA,
+	0xBB, 0xBB, 0xBB, 0xBB, 0xBB, 0xBB, 0xBB, 0xBB,
+	0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC,
+	0xDD, 0xDD, 0xDD, 0xDD, 0xDD, 0xDD, 0xDD, 0xDD,
+	0x3A, 0x00, 0x01, 0x24, 0xF8, 0x20, 0x3A, 0x00,
+	0x01, 0x24, 0xF9, 0x00, 0x3A, 0x00, 0x01, 0x24,
+	0xFD, 0x85, 0xA5, 0x05, 0x58, 0x20, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x60,
+	0x01, 0x65, 0x42, 0x4C, 0x31, 0x5F, 0x32, 0x06,
+	0x66, 0x53, 0x48, 0x41, 0x32, 0x35, 0x36, 0x02,
+	0x58, 0x20, 0xF8, 0xB7, 0xCE, 0xAD, 0x9B, 0xE4,
+	0x5A, 0x8F, 0x5C, 0x52, 0x6F, 0x0C, 0x05, 0x25,
+	0x8F, 0xF3, 0xE9, 0x81, 0xDC, 0xBC, 0xF2, 0x05,
+	0x7F, 0x33, 0xF6, 0xBB, 0xDC, 0xD9, 0x4D, 0xA2,
+	0x34, 0x3A, 0xA5, 0x05, 0x58, 0x20, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x67,
+	0x31, 0x2E, 0x37, 0x2E, 0x32, 0x2B, 0x30, 0x01,
+	0x63, 0x42, 0x4C, 0x32, 0x06, 0x66, 0x53, 0x48,
+	0x41, 0x32, 0x35, 0x36, 0x02, 0x58, 0x20, 0x3A,
+	0xE5, 0x9E, 0x40, 0xA9, 0x6B, 0xD5, 0x29, 0x1C,
+	0xAB, 0x7A, 0x5F, 0xBD, 0x1F, 0x9A, 0xA6, 0x52,
+	0xFB, 0x77, 0x7D, 0xA3, 0xEC, 0x9C, 0x29, 0xBC,
+	0xE6, 0x5B, 0x3B, 0x43, 0xFC, 0x9D, 0x26, 0xA5,
+	0x05, 0x58, 0x20, 0xBF, 0xE6, 0xD8, 0x6F, 0x88,
+	0x26, 0xF4, 0xFF, 0x97, 0xFB, 0x96, 0xC4, 0xE6,
+	0xFB, 0xC4, 0x99, 0x3E, 0x46, 0x19, 0xFC, 0x56,
+	0x5D, 0xA2, 0x6A, 0xDF, 0x34, 0xC3, 0x29, 0x48,
+	0x9A, 0xDC, 0x38, 0x04, 0x67, 0x31, 0x2E, 0x35,
+	0x2E, 0x30, 0x2B, 0x30, 0x01, 0x64, 0x52, 0x54,
+	0x5F, 0x30, 0x06, 0x66, 0x53, 0x48, 0x41, 0x32,
+	0x35, 0x36, 0x02, 0x58, 0x20, 0x47, 0x94, 0x9D,
+	0x27, 0x33, 0x82, 0x45, 0x1A, 0xDD, 0x25, 0xF4,
+	0x9A, 0x89, 0x6F, 0x5F, 0xD9, 0xB0, 0xE8, 0x14,
+	0xD3, 0xA4, 0x9B, 0x53, 0xB0, 0x44, 0x0B, 0xCF,
+	0x32, 0x1A, 0xC4, 0xD2, 0x65, 0xA5, 0x05, 0x58,
+	0x20, 0xB3, 0x60, 0xCA, 0xF5, 0xC9, 0x8C, 0x6B,
+	0x94, 0x2A, 0x48, 0x82, 0xFA, 0x9D, 0x48, 0x23,
+	0xEF, 0xB1, 0x66, 0xA9, 0xEF, 0x6A, 0x6E, 0x4A,
+	0xA3, 0x7C, 0x19, 0x19, 0xED, 0x1F, 0xCC, 0xC0,
+	0x49, 0x04, 0x67, 0x30, 0x2E, 0x30, 0x2E, 0x37,
+	0x2B, 0x30, 0x01, 0x64, 0x52, 0x54, 0x5F, 0x31,
+	0x06, 0x66, 0x53, 0x48, 0x41, 0x32, 0x35, 0x36,
+	0x02, 0x58, 0x20, 0xCD, 0x38, 0xBE, 0xC8, 0xB7,
+	0xC0, 0x9E, 0xD5, 0x24, 0x30, 0xFE, 0xC8, 0xD0,
+	0x19, 0x12, 0x56, 0xB2, 0x7A, 0xA5, 0x53, 0x6F,
+	0xBC, 0x7D, 0x09, 0xCA, 0x11, 0xDD, 0x90, 0xD7,
+	0xD6, 0x70, 0xFD, 0xA5, 0x05, 0x58, 0x20, 0xAA,
+	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA,
+	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA,
+	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA,
+	0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0x04,
+	0x60, 0x01, 0x60, 0x06, 0x66, 0x53, 0x48, 0x41,
+	0x32, 0x35, 0x36, 0x02, 0x58, 0x20, 0x28, 0x3D,
+	0x0C, 0x25, 0x22, 0x0C, 0x87, 0x46, 0xA0, 0x58,
+	0x64, 0x6C, 0x0B, 0x14, 0x37, 0x39, 0x40, 0x9D,
+	0x2D, 0x11, 0xD1, 0xCC, 0x54, 0x51, 0xB4, 0x29,
+	0x22, 0xCD, 0x70, 0x92, 0x71, 0xC3, 0x3A, 0x00,
+	0x01, 0x25, 0x01, 0x77, 0x77, 0x77, 0x77, 0x2E,
+	0x74, 0x72, 0x75, 0x73, 0x74, 0x65, 0x64, 0x66,
+	0x69, 0x72, 0x6D, 0x77, 0x61, 0x72, 0x65, 0x2E,
+	0x6F, 0x72, 0x67, 0x3A, 0x00, 0x01, 0x24, 0xF7,
+	0x71, 0x50, 0x53, 0x41, 0x5F, 0x49, 0x4F, 0x54,
+	0x5F, 0x50, 0x52, 0x4F, 0x46, 0x49, 0x4C, 0x45,
+	0x5F, 0x31, 0x3A, 0x00, 0x01, 0x24, 0xFC, 0x70,
+	0x30, 0x36, 0x30, 0x34, 0x35, 0x36, 0x35, 0x32,
+	0x37, 0x32, 0x38, 0x32, 0x39, 0x31, 0x30, 0x30,
+	0x58, 0x40, 0x1E, 0x0D, 0x2B, 0xD8, 0x7A, 0xC9,
+	0x2D, 0xCB, 0x73, 0xD1, 0x42, 0x2F, 0xBF, 0xDA,
+	0x24, 0x71, 0xE2, 0xAF, 0xEA, 0x48, 0x60, 0x17,
+	0x23, 0x75, 0x64, 0xAC, 0xCC, 0x23, 0xA2, 0x67,
+	0xC4, 0xE7, 0x8F, 0x1C, 0x7C, 0x68, 0x49, 0x42,
+	0x4D, 0xDA, 0xC6, 0xD6, 0x21, 0x1C, 0xAA, 0x00,
+	0xDA, 0x1E, 0x68, 0x56, 0xA3, 0x48, 0xEE, 0xA7,
+	0x92, 0xA9, 0x09, 0x83, 0x42, 0x04, 0x06, 0x9E,
+	0x62, 0xBB
+};
+
+psa_status_t
+psa_initial_attest_get_token(const uint8_t *auth_challenge,
+			     size_t         challenge_size,
+			     uint8_t       *token_buf,
+			     size_t         token_buf_size,
+			     size_t        *token_size)
+{
+	(void)auth_challenge;
+	(void)challenge_size;
+
+	if (token_buf_size < sizeof(platform_token)) {
+		return PSA_ERROR_BUFFER_TOO_SMALL;
+	}
+
+	(void)memcpy(token_buf, platform_token, sizeof(platform_token));
+	*token_size = sizeof(platform_token);
+
+	return PSA_SUCCESS;
+}
+#endif /* !PLAT_RSS_NOT_SUPPORTED */

diff --git a/lib/psa/measured_boot.c b/lib/psa/measured_boot.c
new file mode 100644
index 0000000..90e4ef3
--- /dev/null
+++ b/lib/psa/measured_boot.c

@@ -0,0 +1,131 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <string.h>
+
+#include <common/debug.h>
+#include <measured_boot.h>
+#include <psa/client.h>
+#include <psa_manifest/sid.h>
+
+#include "measured_boot_private.h"
+
+static void print_byte_array(const uint8_t *array __unused, size_t len __unused)
+{
+#if LOG_LEVEL >= LOG_LEVEL_INFO
+	size_t i;
+
+	if (array == NULL || len == 0U) {
+		(void)printf("\n");
+	} else {
+		for (i = 0U; i < len; ++i) {
+			(void)printf(" %02x", array[i]);
+			if ((i & U(0xF)) == U(0xF)) {
+				(void)printf("\n");
+				if (i < (len - 1U)) {
+					INFO("\t\t:");
+				}
+			}
+		}
+	}
+#endif
+}
+
+static void log_measurement(uint8_t index,
+			    const uint8_t *signer_id,
+			    size_t signer_id_size,
+			    const uint8_t *version,     /* string */
+			    uint32_t measurement_algo,
+			    const uint8_t *sw_type,     /* string */
+			    const uint8_t *measurement_value,
+			    size_t measurement_value_size,
+			    bool lock_measurement)
+{
+	INFO("Measured boot extend measurement:\n");
+	INFO(" - slot        : %u\n", index);
+	INFO(" - signer_id   :");
+	print_byte_array(signer_id, signer_id_size);
+	INFO(" - version     : %s\n", version);
+	INFO(" - algorithm   : %x\n", measurement_algo);
+	INFO(" - sw_type     : %s\n", sw_type);
+	INFO(" - measurement :");
+	print_byte_array(measurement_value, measurement_value_size);
+	INFO(" - locking     : %s\n", lock_measurement ? "true" : "false");
+}
+
+#if !PLAT_RSS_NOT_SUPPORTED
+psa_status_t
+rss_measured_boot_extend_measurement(uint8_t index,
+				     const uint8_t *signer_id,
+				     size_t signer_id_size,
+				     const uint8_t *version,
+				     size_t version_size,
+				     uint32_t measurement_algo,
+				     const uint8_t *sw_type,
+				     size_t sw_type_size,
+				     const uint8_t *measurement_value,
+				     size_t measurement_value_size,
+				     bool lock_measurement)
+{
+	struct measured_boot_extend_iovec_t extend_iov = {
+		.index = index,
+		.lock_measurement = lock_measurement,
+		.measurement_algo = measurement_algo,
+		.sw_type = {0},
+		.sw_type_size = sw_type_size,
+	};
+
+	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 = measurement_value, .len = measurement_value_size}
+	};
+
+	uint32_t sw_type_size_limited;
+
+	if (sw_type != NULL) {
+		sw_type_size_limited = (sw_type_size < SW_TYPE_MAX_SIZE) ?
+					sw_type_size : SW_TYPE_MAX_SIZE;
+		memcpy(extend_iov.sw_type, sw_type, sw_type_size_limited);
+	}
+
+	log_measurement(index, signer_id, signer_id_size,
+			version, measurement_algo, sw_type,
+			measurement_value, measurement_value_size,
+			lock_measurement);
+
+	return psa_call(RSS_MEASURED_BOOT_HANDLE,
+			RSS_MEASURED_BOOT_EXTEND,
+			in_vec, IOVEC_LEN(in_vec),
+			NULL, 0);
+}
+
+#else /* !PLAT_RSS_NOT_SUPPORTED */
+
+psa_status_t
+rss_measured_boot_extend_measurement(uint8_t index,
+				     const uint8_t *signer_id,
+				     size_t signer_id_size,
+				     const uint8_t *version,
+				     size_t version_size,
+				     uint32_t measurement_algo,
+				     const uint8_t *sw_type,
+				     size_t sw_type_size,
+				     const uint8_t *measurement_value,
+				     size_t measurement_value_size,
+				     bool lock_measurement)
+{
+	log_measurement(index, signer_id, signer_id_size,
+			version, measurement_algo, sw_type,
+			measurement_value, measurement_value_size,
+			lock_measurement);
+
+	return PSA_SUCCESS;
+}
+#endif /* !PLAT_RSS_NOT_SUPPORTED */

diff --git a/lib/psa/measured_boot_private.h b/lib/psa/measured_boot_private.h
new file mode 100644
index 0000000..649c3f6
--- /dev/null
+++ b/lib/psa/measured_boot_private.h

@@ -0,0 +1,24 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef PSA_MEASURED_BOOT_PRIVATE_H
+#define PSA_MEASURED_BOOT_PRIVATE_H
+
+#include <stdint.h>
+
+/* Measured boot message types that distinguish its services */
+#define RSS_MEASURED_BOOT_EXTEND	1002U
+
+struct measured_boot_extend_iovec_t {
+	uint8_t  index;
+	uint8_t  lock_measurement;
+	uint32_t measurement_algo;
+	uint8_t  sw_type[SW_TYPE_MAX_SIZE];
+	uint8_t  sw_type_size;
+};
+
+#endif /* PSA_MEASURED_BOOT_PRIVATE_H */

diff --git a/make_helpers/build_macros.mk b/make_helpers/build_macros.mk
index 12aaee6..a58caf5 100644
--- a/make_helpers/build_macros.mk
+++ b/make_helpers/build_macros.mk

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -489,6 +489,10 @@
 $(ELF): romlib.bin
 endif
 
+# MODULE_OBJS can be assigned by vendors with different compiled
+# object file path, and prebuilt object file path.
+$(eval OBJS += $(MODULE_OBJS))
+
 $(ELF): $(OBJS) $(LINKERFILE) | $(1)_dirs libraries $(BL_LIBS)
 	$$(ECHO) "  LD      $$@"
 ifdef MAKE_BUILD_STRINGS
@@ -507,7 +511,7 @@
 		$(BUILD_DIR)/build_message.o $(OBJS)
 else ifneq ($(findstring gcc,$(notdir $(LD))),)
 	$$(Q)$$(LD) -o $$@ $$(TF_LDFLAGS) $$(LDFLAGS) -Wl,-Map=$(MAPFILE) \
-		-Wl,-T$(LINKERFILE) $(BUILD_DIR)/build_message.o \
+		-Wl,-dT $(LINKERFILE) $(EXTRA_LINKERFILE) $(BUILD_DIR)/build_message.o \
 		$(OBJS) $(LDPATHS) $(LIBWRAPPER) $(LDLIBS) $(BL_LIBS)
 else
 	$$(Q)$$(LD) -o $$@ $$(TF_LDFLAGS) $$(LDFLAGS) $(BL_LDFLAGS) -Map=$(MAPFILE) \

diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index 7b66569..fab6bf6 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk

@@ -169,6 +169,9 @@
 # Flag to enable Virtualization Host Extensions
 ENABLE_FEAT_VHE 		:= 0
 
+# Flag to enable delayed trapping of WFE instruction (FEAT_TWED)
+ENABLE_FEAT_TWED		:= 0
+
 # By default BL31 encryption disabled
 ENCRYPT_BL31			:= 0
 
@@ -254,6 +257,9 @@
 # By default, BL1 acts as the reset handler, not BL31
 RESET_TO_BL31			:= 0
 
+# By default, clear the input registers when RESET_TO_BL31 is enabled
+RESET_TO_BL31_WITH_PARAMS	:= 0
+
 # For Chain of Trust
 SAVE_KEYS			:= 0
 
@@ -434,6 +440,11 @@
 	override ENABLE_TRBE_FOR_NS	:= 0
 endif
 
+# By default, disable access to branch record buffer control registers from NS
+# lower ELs i.e. NS-EL2, or NS-EL1 if NS-EL2 implemented but unused
+# if FEAT_BRBE is implemented.
+ENABLE_BRBE_FOR_NS		:= 0
+
 # By default, disable access of trace system registers from NS lower
 # ELs  i.e. NS-EL2, or NS-EL1 if NS-EL2 implemented but unused if
 # system register trace is implemented.
@@ -443,3 +454,15 @@
 # lower ELs, i.e. NS-EL2, or NS-EL1 if NS-EL2 implemented but unused
 # if FEAT_TRF is implemented.
 ENABLE_TRF_FOR_NS		:= 0
+
+# In v8.6+ platforms with delayed trapping of WFE being supported
+# via FEAT_TWED, this flag takes the delay value to be set in the
+# SCR_EL3.TWEDEL(4bit) field, when FEAT_TWED is implemented.
+# By default it takes 0, and need to be updated by the platforms.
+TWED_DELAY			:= 0
+
+# By default, disable the mocking of RSS provided services
+PLAT_RSS_NOT_SUPPORTED		:= 0
+
+# Dynamic Root of Trust for Measurement support
+DRTM_SUPPORT			:= 0

diff --git a/make_helpers/tbbr/tbbr_tools.mk b/make_helpers/tbbr/tbbr_tools.mk
index 0a280b4..5ef2d85 100644
--- a/make_helpers/tbbr/tbbr_tools.mk
+++ b/make_helpers/tbbr/tbbr_tools.mk

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -25,6 +25,9 @@
 #   KEY_SIZE
 #   ROT_KEY
 #   PROT_KEY
+#   PLAT_KEY
+#   SWD_ROT_KEY
+#   CORE_SWD_KEY
 #   TRUSTED_WORLD_KEY
 #   NON_TRUSTED_WORLD_KEY
 #   SCP_BL2_KEY
@@ -46,10 +49,18 @@
 $(eval $(call CERT_ADD_CMD_OPT,${NTFW_NVCTR_VAL},--ntfw-nvctr))
 
 # Add Trusted Key certificate to the fiptool and cert_create command line options
+ifneq (${COT},cca)
 $(eval $(call TOOL_ADD_PAYLOAD,${TRUSTED_KEY_CERT},--trusted-key-cert))
+else
+$(eval $(call TOOL_ADD_PAYLOAD,${BUILD_PLAT}/cca.crt,--cca-cert))
+$(eval $(call TOOL_ADD_PAYLOAD,${BUILD_PLAT}/core-swd.crt,--core-swd-cert))
+$(eval $(call TOOL_ADD_PAYLOAD,${BUILD_PLAT}/plat-key.crt,--plat-key-cert))
+endif
 
 # Add fwu certificate to the fiptool and cert_create command line options
+ifneq (${COT},cca)
 $(eval $(call TOOL_ADD_PAYLOAD,${FWU_CERT},--fwu-cert,,FWU_))
+endif
 
 # Add the keys to the cert_create command line options (private keys are NOT
 # packed in the FIP). Developers can use their own keys by specifying the proper
@@ -63,6 +74,9 @@
 $(if ${ROT_KEY},$(eval $(call CERT_ADD_CMD_OPT,${ROT_KEY},--rot-key)))
 $(if ${ROT_KEY},$(eval $(call CERT_ADD_CMD_OPT,${ROT_KEY},--rot-key,FWU_)))
 $(if ${PROT_KEY},$(eval $(call CERT_ADD_CMD_OPT,${PROT_KEY},--prot-key)))
+$(if ${PLAT_KEY},$(eval $(call CERT_ADD_CMD_OPT,${PLAT_KEY},--plat-key)))
+$(if ${SWD_ROT_KEY},$(eval $(call CERT_ADD_CMD_OPT,${SWD_ROT_KEY},--swd-rot-key)))
+$(if ${CORE_SWD_KEY},$(eval $(call CERT_ADD_CMD_OPT,${CORE_SWD_KEY},--core-swd-key)))
 $(if ${TRUSTED_WORLD_KEY},$(eval $(call CERT_ADD_CMD_OPT,${TRUSTED_WORLD_KEY},--trusted-world-key)))
 $(if ${NON_TRUSTED_WORLD_KEY},$(eval $(call CERT_ADD_CMD_OPT,${NON_TRUSTED_WORLD_KEY},--non-trusted-world-key)))
 
@@ -70,25 +84,31 @@
 # Add the BL2 CoT (image cert)
 ifeq (${NEED_BL2},yes)
 ifeq (${BL2_AT_EL3}, 0)
+ifneq (${COT},cca)
 $(eval $(call TOOL_ADD_PAYLOAD,${BUILD_PLAT}/tb_fw.crt,--tb-fw-cert))
 endif
 endif
+endif
 
 # Add the SCP_BL2 CoT (key cert + img cert)
 ifneq (${SCP_BL2},)
+ifneq (${COT},cca)
     $(if ${SCP_BL2_KEY},$(eval $(call CERT_ADD_CMD_OPT,${SCP_BL2_KEY},--scp-fw-key)))
     $(eval $(call TOOL_ADD_PAYLOAD,${BUILD_PLAT}/scp_fw_content.crt,--scp-fw-cert))
     $(eval $(call TOOL_ADD_PAYLOAD,${BUILD_PLAT}/scp_fw_key.crt,--scp-fw-key-cert))
 endif
+endif
 
 ifeq (${ARCH},aarch64)
 ifeq (${NEED_BL31},yes)
 # Add the BL31 CoT (key cert + img cert)
 $(if ${BL31_KEY},$(eval $(call CERT_ADD_CMD_OPT,${BL31_KEY},--soc-fw-key)))
+ifneq (${COT},cca)
 $(eval $(call TOOL_ADD_PAYLOAD,${BUILD_PLAT}/soc_fw_content.crt,--soc-fw-cert))
 $(eval $(call TOOL_ADD_PAYLOAD,${BUILD_PLAT}/soc_fw_key.crt,--soc-fw-key-cert))
 endif
 endif
+endif
 
 # Add the BL32 CoT (key cert + img cert)
 ifeq (${NEED_BL32},yes)
@@ -102,7 +122,9 @@
     $(if ${BL33_KEY},$(eval $(call CERT_ADD_CMD_OPT,${BL33_KEY},--nt-fw-key)))
     $(eval $(call TOOL_ADD_PAYLOAD,${BUILD_PLAT}/nt_fw_content.crt,--nt-fw-cert))
 ifneq (${COT},dualroot)
-    $(eval $(call TOOL_ADD_PAYLOAD,${BUILD_PLAT}/nt_fw_key.crt,--nt-fw-key-cert))
+    ifneq (${COT},cca)
+        $(eval $(call TOOL_ADD_PAYLOAD,${BUILD_PLAT}/nt_fw_key.crt,--nt-fw-key-cert))
+    endif
 endif
 endif
 
@@ -112,4 +134,7 @@
 ifeq (${COT},dualroot)
     $(eval $(call TOOL_ADD_PAYLOAD,${BUILD_PLAT}/plat_sp_content.crt,--plat-sp-cert))
 endif
+ifeq (${COT},cca)
+    $(eval $(call TOOL_ADD_PAYLOAD,${BUILD_PLAT}/plat_sp_content.crt,--plat-sp-cert))
+endif
 endif

diff --git a/package-lock.json b/package-lock.json
index 469c5f5..34e7dbd 100644
--- a/package-lock.json
+++ b/package-lock.json

@@ -1,12 +1,12 @@
 {
   "name": "trusted-firmware-a",
-  "version": "2.6.0",
+  "version": "2.7.0",
   "lockfileVersion": 2,
   "requires": true,
   "packages": {
     "": {
       "name": "trusted-firmware-a",
-      "version": "2.6.0",
+      "version": "2.7.0",
       "hasInstallScript": true,
       "license": "BSD-3-Clause",
       "devDependencies": {
@@ -843,9 +843,9 @@
       }
     },
     "node_modules/commitizen/node_modules/ansi-regex": {
-      "version": "4.1.0",
-      "resolved": "https://registry.npmjs.org/ansi-regex/-/ansi-regex-4.1.0.tgz",
-      "integrity": "sha512-1apePfXM1UOSqw0o9IiFAovVz9M5S1Dg+4TrDwfMewQ6p/rmMueb7tWZjQ1rx4Loy1ArBggoqGpfqqdI4rondg==",
+      "version": "4.1.1",
+      "resolved": "https://registry.npmjs.org/ansi-regex/-/ansi-regex-4.1.1.tgz",
+      "integrity": "sha512-ILlv4k/3f6vfQ4OoP2AGvirOktlQ98ZEL1k9FaQjxa3L1abBgbuTDAdPOpvbGncC0BTVQrl+OM8xZGK6tWXt7g==",
       "dev": true,
       "engines": {
         "node": ">=6"
@@ -1073,9 +1073,9 @@
       }
     },
     "node_modules/commitizen/node_modules/string-width/node_modules/ansi-regex": {
-      "version": "3.0.0",
-      "resolved": "https://registry.npmjs.org/ansi-regex/-/ansi-regex-3.0.0.tgz",
-      "integrity": "sha1-7QMXwyIGT3lGbAKWa922Bas32Zg=",
+      "version": "3.0.1",
+      "resolved": "https://registry.npmjs.org/ansi-regex/-/ansi-regex-3.0.1.tgz",
+      "integrity": "sha512-+O9Jct8wf++lXxxFc4hc8LsjaSq0HFzzL7cVsw8pRDIPdjKD2mT4ytDZlLuSBZ4cLKZFXIrMGO7DbQCtMJJMKw==",
       "dev": true,
       "engines": {
         "node": ">=4"
@@ -4792,9 +4792,9 @@
           "dev": true
         },
         "ansi-regex": {
-          "version": "4.1.0",
-          "resolved": "https://registry.npmjs.org/ansi-regex/-/ansi-regex-4.1.0.tgz",
-          "integrity": "sha512-1apePfXM1UOSqw0o9IiFAovVz9M5S1Dg+4TrDwfMewQ6p/rmMueb7tWZjQ1rx4Loy1ArBggoqGpfqqdI4rondg==",
+          "version": "4.1.1",
+          "resolved": "https://registry.npmjs.org/ansi-regex/-/ansi-regex-4.1.1.tgz",
+          "integrity": "sha512-ILlv4k/3f6vfQ4OoP2AGvirOktlQ98ZEL1k9FaQjxa3L1abBgbuTDAdPOpvbGncC0BTVQrl+OM8xZGK6tWXt7g==",
           "dev": true
         },
         "ansi-styles": {
@@ -4975,9 +4975,9 @@
           },
           "dependencies": {
             "ansi-regex": {
-              "version": "3.0.0",
-              "resolved": "https://registry.npmjs.org/ansi-regex/-/ansi-regex-3.0.0.tgz",
-              "integrity": "sha1-7QMXwyIGT3lGbAKWa922Bas32Zg=",
+              "version": "3.0.1",
+              "resolved": "https://registry.npmjs.org/ansi-regex/-/ansi-regex-3.0.1.tgz",
+              "integrity": "sha512-+O9Jct8wf++lXxxFc4hc8LsjaSq0HFzzL7cVsw8pRDIPdjKD2mT4ytDZlLuSBZ4cLKZFXIrMGO7DbQCtMJJMKw==",
               "dev": true
             },
             "strip-ansi": {

diff --git a/package.json b/package.json
index e5cd924..0284e6f 100644
--- a/package.json
+++ b/package.json

@@ -1,6 +1,6 @@
 {
   "name": "trusted-firmware-a",
-  "version": "2.6.0",
+  "version": "2.7.0",
   "license": "BSD-3-Clause",
   "private": true,
   "scripts": {

diff --git a/plat/allwinner/common/allwinner-common.mk b/plat/allwinner/common/allwinner-common.mk
index cf559fb..61c1dbe 100644
--- a/plat/allwinner/common/allwinner-common.mk
+++ b/plat/allwinner/common/allwinner-common.mk

@@ -27,6 +27,7 @@
 				plat/common/plat_gicv2.c		\
 				plat/common/plat_psci_common.c		\
 				${AW_PLAT}/common/sunxi_bl31_setup.c	\
+				${AW_PLAT}/${PLAT}/sunxi_idle_states.c	\
 				${AW_PLAT}/common/sunxi_pm.c		\
 				${AW_PLAT}/${PLAT}/sunxi_power.c	\
 				${AW_PLAT}/common/sunxi_security.c	\

diff --git a/plat/allwinner/common/include/platform_def.h b/plat/allwinner/common/include/platform_def.h
index 49951e0..c9d075a 100644
--- a/plat/allwinner/common/include/platform_def.h
+++ b/plat/allwinner/common/include/platform_def.h

@@ -57,9 +57,10 @@
 #define PLAT_CSS_SCP_COM_SHARED_MEM_BASE \
 	(SUNXI_SRAM_A2_BASE + SUNXI_SRAM_A2_SIZE - 0x200)
 
-#define PLAT_MAX_PWR_LVL_STATES		U(2)
+/* These states are used directly for SCPI communication. */
+#define PLAT_MAX_PWR_LVL_STATES		U(3)
 #define PLAT_MAX_RET_STATE		U(1)
-#define PLAT_MAX_OFF_STATE		U(2)
+#define PLAT_MAX_OFF_STATE		U(3)
 
 #define PLAT_MAX_PWR_LVL		U(2)
 #define PLAT_NUM_PWR_DOMAINS		(U(1) + \

diff --git a/plat/allwinner/common/include/sunxi_def.h b/plat/allwinner/common/include/sunxi_def.h
index ec50887..c17ef95 100644
--- a/plat/allwinner/common/include/sunxi_def.h
+++ b/plat/allwinner/common/include/sunxi_def.h

@@ -20,4 +20,7 @@
 #define SUNXI_SOC_H616			0x1823
 #define SUNXI_SOC_R329			0x1851
 
+#define JEDEC_ALLWINNER_BKID		9U
+#define JEDEC_ALLWINNER_MFID		0x9eU
+
 #endif /* SUNXI_DEF_H */

diff --git a/plat/allwinner/common/include/sunxi_private.h b/plat/allwinner/common/include/sunxi_private.h
index efea736..6a38657 100644
--- a/plat/allwinner/common/include/sunxi_private.h
+++ b/plat/allwinner/common/include/sunxi_private.h

@@ -7,8 +7,12 @@
 #ifndef SUNXI_PRIVATE_H
 #define SUNXI_PRIVATE_H
 
+#include <common/fdt_fixup.h>
+
 #include <lib/psci/psci.h>
 
+extern const struct psci_cpu_idle_state sunxi_idle_states[];
+
 void sunxi_configure_mmu_el3(int flags);
 
 void sunxi_cpu_on(u_register_t mpidr);
@@ -24,8 +28,13 @@
 }
 #endif
 #if SUNXI_PSCI_USE_SCPI
+bool sunxi_psci_is_scpi(void);
 int sunxi_set_scpi_psci_ops(const plat_psci_ops_t **psci_ops);
 #else
+static inline bool sunxi_psci_is_scpi(void)
+{
+	return false;
+}
 static inline int sunxi_set_scpi_psci_ops(const plat_psci_ops_t **psci_ops)
 {
 	return -1;

diff --git a/plat/allwinner/common/sunxi_bl31_setup.c b/plat/allwinner/common/sunxi_bl31_setup.c
index ed3dbd4..a32124a 100644
--- a/plat/allwinner/common/sunxi_bl31_setup.c
+++ b/plat/allwinner/common/sunxi_bl31_setup.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -32,6 +32,8 @@
 
 static console_t console;
 
+static void *fdt;
+
 static const gicv2_driver_data_t sunxi_gic_data = {
 	.gicd_base = SUNXI_GICD_BASE,
 	.gicc_base = SUNXI_GICC_BASE,
@@ -49,7 +51,7 @@
  * entry point to find the load address, which should be followed by the
  * size. Adding those together gives us the address of the DTB.
  */
-static void *sunxi_find_dtb(void)
+static void sunxi_find_dtb(void)
 {
 	uint64_t *u_boot_base;
 	int i;
@@ -57,7 +59,7 @@
 	u_boot_base = (void *)SUNXI_BL33_VIRT_BASE;
 
 	for (i = 0; i < 2048 / sizeof(uint64_t); i++) {
-		uint32_t *dtb_base;
+		void *dtb_base;
 
 		if (u_boot_base[i] != PRELOADED_BL33_BASE)
 			continue;
@@ -67,15 +69,13 @@
 			continue;
 
 		/* end of the image: base address + size */
-		dtb_base = (void *)((char *)u_boot_base + u_boot_base[i + 1]);
-
-		if (fdt_check_header(dtb_base) != 0)
-			continue;
+		dtb_base = (char *)u_boot_base + u_boot_base[i + 1];
 
-		return dtb_base;
+		if (fdt_check_header(dtb_base) == 0) {
+			fdt = dtb_base;
+			return;
+		}
 	}
-
-	return NULL;
 }
 
 void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
@@ -113,7 +113,6 @@
 {
 	const char *soc_name;
 	uint16_t soc_id = sunxi_read_soc_id();
-	void *fdt;
 
 	switch (soc_id) {
 	case SUNXI_SOC_A64:
@@ -139,7 +138,7 @@
 
 	generic_delay_timer_init();
 
-	fdt = sunxi_find_dtb();
+	sunxi_find_dtb();
 	if (fdt) {
 		const char *model;
 		int length;
@@ -180,10 +179,15 @@
 
 	sunxi_pmic_setup(soc_id, fdt);
 
+	INFO("BL31: Platform setup done\n");
+}
+
+void bl31_plat_runtime_setup(void)
+{
 	/* Change the DTB if the configuration requires so. */
 	sunxi_prepare_dtb(fdt);
 
-	INFO("BL31: Platform setup done\n");
+	console_switch_state(CONSOLE_FLAG_RUNTIME);
 }
 
 entry_point_info_t *bl31_plat_get_next_image_ep_info(uint32_t type)

diff --git a/plat/allwinner/common/sunxi_common.c b/plat/allwinner/common/sunxi_common.c
index 82410b1..092659c 100644
--- a/plat/allwinner/common/sunxi_common.c
+++ b/plat/allwinner/common/sunxi_common.c

@@ -8,7 +8,9 @@
 
 #include <common/debug.h>
 #include <lib/mmio.h>
+#include <lib/smccc.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
+#include <services/arm_arch_svc.h>
 
 #include <sunxi_def.h>
 #include <sunxi_mmap.h>
@@ -157,3 +159,29 @@
 
 	return 0;
 }
+
+int32_t plat_is_smccc_feature_available(u_register_t fid)
+{
+	switch (fid) {
+	case SMCCC_ARCH_SOC_ID:
+		return SMC_ARCH_CALL_SUCCESS;
+	default:
+		return SMC_ARCH_CALL_NOT_SUPPORTED;
+	}
+}
+
+int32_t plat_get_soc_version(void)
+{
+	int32_t ret;
+
+	ret = SOC_ID_SET_JEP_106(JEDEC_ALLWINNER_BKID, JEDEC_ALLWINNER_MFID);
+
+	return ret | (sunxi_read_soc_id() & SOC_ID_IMPL_DEF_MASK);
+}
+
+int32_t plat_get_soc_revision(void)
+{
+	uint32_t reg = mmio_read_32(SRAM_VER_REG);
+
+	return reg & GENMASK_32(7, 0);
+}

diff --git a/plat/allwinner/common/sunxi_pm.c b/plat/allwinner/common/sunxi_pm.c
index eb1b7e7..3772b4a 100644
--- a/plat/allwinner/common/sunxi_pm.c
+++ b/plat/allwinner/common/sunxi_pm.c

@@ -9,12 +9,22 @@
 #include <platform_def.h>
 
 #include <common/debug.h>
+#include <common/fdt_fixup.h>
 #include <lib/mmio.h>
 #include <lib/psci/psci.h>
 
 #include <sunxi_cpucfg.h>
 #include <sunxi_private.h>
 
+static bool psci_is_scpi;
+
+#if SUNXI_PSCI_USE_SCPI
+bool sunxi_psci_is_scpi(void)
+{
+	return psci_is_scpi;
+}
+#endif
+
 int sunxi_validate_ns_entrypoint(uintptr_t ns_entrypoint)
 {
 	/* The non-secure entry point must be in DRAM */
@@ -40,6 +50,7 @@
 
 	if (sunxi_set_scpi_psci_ops(psci_ops) == 0) {
 		INFO("PSCI: Suspend is available via SCPI\n");
+		psci_is_scpi = true;
 	} else {
 		INFO("PSCI: Suspend is unavailable\n");
 		sunxi_set_native_psci_ops(psci_ops);

diff --git a/plat/allwinner/common/sunxi_prepare_dtb.c b/plat/allwinner/common/sunxi_prepare_dtb.c
index 93428ac..66af35a 100644
--- a/plat/allwinner/common/sunxi_prepare_dtb.c
+++ b/plat/allwinner/common/sunxi_prepare_dtb.c

@@ -31,16 +31,22 @@
 	if (fdt_add_reserved_memory(fdt, "tf-a@40000000", BL31_BASE,
 				    BL31_LIMIT - BL31_BASE)) {
 		WARN("Failed to add reserved memory nodes to DT.\n");
-		return;
 	}
 #endif
 
+	if (sunxi_psci_is_scpi()) {
+		ret = fdt_add_cpu_idle_states(fdt, sunxi_idle_states);
+		if (ret < 0) {
+			WARN("Failed to add idle states to DT: %d\n", ret);
+		}
+	}
+
 	ret = fdt_pack(fdt);
 	if (ret < 0) {
 		ERROR("Failed to pack devicetree at %p: error %d\n",
 		      fdt, ret);
-	} else {
-		clean_dcache_range((uintptr_t)fdt, fdt_blob_size(fdt));
-		INFO("Changed devicetree.\n");
 	}
+
+	clean_dcache_range((uintptr_t)fdt, fdt_blob_size(fdt));
+	INFO("Changed devicetree.\n");
 }

diff --git a/plat/allwinner/common/sunxi_scpi_pm.c b/plat/allwinner/common/sunxi_scpi_pm.c
index eb37daa..41dc563 100644
--- a/plat/allwinner/common/sunxi_scpi_pm.c
+++ b/plat/allwinner/common/sunxi_scpi_pm.c

@@ -33,6 +33,9 @@
  */
 #define SCP_FIRMWARE_MAGIC		0xb4400012
 
+#define PLAT_LOCAL_PSTATE_WIDTH		U(4)
+#define PLAT_LOCAL_PSTATE_MASK		((U(1) << PLAT_LOCAL_PSTATE_WIDTH) - 1)
+
 #define CPU_PWR_LVL			MPIDR_AFFLVL0
 #define CLUSTER_PWR_LVL			MPIDR_AFFLVL1
 #define SYSTEM_PWR_LVL			MPIDR_AFFLVL2
@@ -44,17 +47,6 @@
 #define SYSTEM_PWR_STATE(state) \
 	((state)->pwr_domain_state[SYSTEM_PWR_LVL])
 
-static inline scpi_power_state_t scpi_map_state(plat_local_state_t psci_state)
-{
-	if (is_local_state_run(psci_state)) {
-		return scpi_power_on;
-	}
-	if (is_local_state_retn(psci_state)) {
-		return scpi_power_retention;
-	}
-	return scpi_power_off;
-}
-
 static void sunxi_cpu_standby(plat_local_state_t cpu_state)
 {
 	u_register_t scr = read_scr_el3();
@@ -87,9 +79,9 @@
 	}
 
 	scpi_set_css_power_state(read_mpidr(),
-				 scpi_map_state(cpu_pwr_state),
-				 scpi_map_state(cluster_pwr_state),
-				 scpi_map_state(system_pwr_state));
+				 cpu_pwr_state,
+				 cluster_pwr_state,
+				 system_pwr_state);
 }
 
 static void sunxi_pwr_domain_on_finish(const psci_power_state_t *target_state)
@@ -137,7 +129,9 @@
 				      psci_power_state_t *req_state)
 {
 	unsigned int power_level = psci_get_pstate_pwrlvl(power_state);
+	unsigned int state_id = psci_get_pstate_id(power_state);
 	unsigned int type = psci_get_pstate_type(power_state);
+	unsigned int i;
 
 	assert(req_state != NULL);
 
@@ -146,28 +140,19 @@
 	}
 
 	if (type == PSTATE_TYPE_STANDBY) {
-		/* Only one retention power state is supported. */
-		if (psci_get_pstate_id(power_state) > 0) {
-			return PSCI_E_INVALID_PARAMS;
-		}
-		/* The SoC cannot be suspended without losing state */
-		if (power_level == SYSTEM_PWR_LVL) {
-			return PSCI_E_INVALID_PARAMS;
-		}
-		for (unsigned int i = 0; i <= power_level; ++i) {
-			req_state->pwr_domain_state[i] = PLAT_MAX_RET_STATE;
-		}
-	} else {
-		/* Only one off power state is supported. */
-		if (psci_get_pstate_id(power_state) > 0) {
-			return PSCI_E_INVALID_PARAMS;
-		}
-		for (unsigned int i = 0; i <= power_level; ++i) {
-			req_state->pwr_domain_state[i] = PLAT_MAX_OFF_STATE;
-		}
+		return PSCI_E_INVALID_PARAMS;
 	}
+
+	/* Pass through the requested PSCI state as-is. */
+	for (i = 0; i <= power_level; ++i) {
+		unsigned int local_pstate = state_id & PLAT_LOCAL_PSTATE_MASK;
+
+		req_state->pwr_domain_state[i] = local_pstate;
+		state_id >>= PLAT_LOCAL_PSTATE_WIDTH;
+	}
+
 	/* Higher power domain levels should all remain running */
-	for (unsigned int i = power_level + 1; i <= PLAT_MAX_PWR_LVL; ++i) {
+	for (; i <= PLAT_MAX_PWR_LVL; ++i) {
 		req_state->pwr_domain_state[i] = PSCI_LOCAL_STATE_RUN;
 	}
 

diff --git a/plat/allwinner/sun50i_a64/sunxi_idle_states.c b/plat/allwinner/sun50i_a64/sunxi_idle_states.c
new file mode 100644
index 0000000..2918bb7
--- /dev/null
+++ b/plat/allwinner/sun50i_a64/sunxi_idle_states.c

@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <sunxi_private.h>
+
+const struct psci_cpu_idle_state sunxi_idle_states[] = {
+	{
+		.name			= "cpu-sleep",
+		.power_state		= 0x00010003,
+		.local_timer_stop	= true,
+		.entry_latency_us	= 800,
+		.exit_latency_us	= 1500,
+		.min_residency_us	= 25000
+	},
+	{
+		.name			= "cluster-sleep",
+		.power_state		= 0x01010013,
+		.local_timer_stop	= true,
+		.entry_latency_us	= 850,
+		.exit_latency_us	= 1500,
+		.min_residency_us	= 50000
+	},
+	{}
+};

diff --git a/plat/allwinner/sun50i_h6/sunxi_idle_states.c b/plat/allwinner/sun50i_h6/sunxi_idle_states.c
new file mode 100644
index 0000000..4339bcd
--- /dev/null
+++ b/plat/allwinner/sun50i_h6/sunxi_idle_states.c

@@ -0,0 +1,11 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <sunxi_private.h>
+
+const struct psci_cpu_idle_state sunxi_idle_states[] = {
+	{}
+};

diff --git a/plat/allwinner/sun50i_h616/sunxi_idle_states.c b/plat/allwinner/sun50i_h616/sunxi_idle_states.c
new file mode 100644
index 0000000..4339bcd
--- /dev/null
+++ b/plat/allwinner/sun50i_h616/sunxi_idle_states.c

@@ -0,0 +1,11 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <sunxi_private.h>
+
+const struct psci_cpu_idle_state sunxi_idle_states[] = {
+	{}
+};

diff --git a/plat/allwinner/sun50i_r329/sunxi_idle_states.c b/plat/allwinner/sun50i_r329/sunxi_idle_states.c
new file mode 100644
index 0000000..4339bcd
--- /dev/null
+++ b/plat/allwinner/sun50i_r329/sunxi_idle_states.c

@@ -0,0 +1,11 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <sunxi_private.h>
+
+const struct psci_cpu_idle_state sunxi_idle_states[] = {
+	{}
+};

diff --git a/plat/arm/board/common/board_arm_trusted_boot.c b/plat/arm/board/common/board_arm_trusted_boot.c
index 66cc3e9..714c444 100644
--- a/plat/arm/board/common/board_arm_trusted_boot.c
+++ b/plat/arm/board/common/board_arm_trusted_boot.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -13,18 +13,20 @@
 #include <drivers/delay_timer.h>
 #include <lib/cassert.h>
 #include <lib/fconf/fconf.h>
-#include <plat/arm/common/plat_arm.h>
-#include <plat/arm/common/fconf_nv_cntr_getter.h>
 #include <plat/common/common_def.h>
 #include <plat/common/platform.h>
-#include <platform_def.h>
-
-#if defined(ARM_COT_tbbr)
-#include <tools_share/tbbr_oid.h>
+#if defined(ARM_COT_cca)
+#include <tools_share/cca_oid.h>
 #elif defined(ARM_COT_dualroot)
 #include <tools_share/dualroot_oid.h>
+#elif defined(ARM_COT_tbbr)
+#include <tools_share/tbbr_oid.h>
 #endif
 
+#include <plat/arm/common/fconf_nv_cntr_getter.h>
+#include <plat/arm/common/plat_arm.h>
+#include <platform_def.h>
+
 #if !ARM_CRYPTOCELL_INTEG
 #if !ARM_ROTPK_LOCATION_ID
   #error "ARM_ROTPK_LOCATION_ID not defined"
@@ -181,6 +183,40 @@
 		return 1;
 	}
 }
+
+#elif defined(ARM_COT_cca)
+
+int arm_get_rotpk_info(void *cookie, void **key_ptr, unsigned int *key_len,
+		       unsigned int *flags)
+{
+	/*
+	 * Return the right root of trust key hash based on the cookie value:
+	 *  - NULL means the primary ROTPK.
+	 *  - Otherwise, interpret cookie as the OID of the certificate
+	 *    extension containing the key.
+	 */
+	if (cookie == NULL) {
+		return get_rotpk_info(key_ptr, key_len, flags);
+	} else if (strcmp(cookie, PROT_PK_OID) == 0) {
+		extern unsigned char arm_protpk_hash[];
+		extern unsigned char arm_protpk_hash_end[];
+		*key_ptr = arm_protpk_hash;
+		*key_len = arm_protpk_hash_end - arm_protpk_hash;
+		*flags = ROTPK_IS_HASH;
+		return 0;
+	} else if (strcmp(cookie, SWD_ROT_PK_OID) == 0) {
+		extern unsigned char arm_swd_rotpk_hash[];
+		extern unsigned char arm_swd_rotpk_hash_end[];
+		*key_ptr = arm_swd_rotpk_hash;
+		*key_len = arm_swd_rotpk_hash_end - arm_swd_rotpk_hash;
+		*flags = ROTPK_IS_HASH;
+		return 0;
+	} else {
+		/* Invalid key ID. */
+		return 1;
+	}
+}
+
 #endif
 
 /*

diff --git a/plat/arm/board/common/board_common.mk b/plat/arm/board/common/board_common.mk
index 5cdf1bf..b11811c 100644
--- a/plat/arm/board/common/board_common.mk
+++ b/plat/arm/board/common/board_common.mk

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -38,6 +38,10 @@
 
 $(eval $(call add_define,ARM_ROTPK_LOCATION_ID))
 
+ifeq (${ENABLE_RME}, 1)
+COT	:=	cca
+endif
+
 # Force generation of the new hash if ROT_KEY is specified
 ifdef ROT_KEY
 	HASH_PREREQUISITES = $(ROT_KEY) FORCE
@@ -85,7 +89,32 @@
 BL2_SOURCES		+=	plat/arm/board/common/protpk/arm_dev_protpk.S
 
 $(BUILD_PLAT)/bl1/arm_dev_protpk.o: $(ARM_PROTPK_HASH)
+$(BUILD_PLAT)/bl2/arm_dev_protpk.o: $(ARM_PROTPK_HASH)
+endif
+
+ifeq (${COT},cca)
+# Platform and Secure World Root of Trust key files.
+ARM_PROT_KEY		:=	plat/arm/board/common/protpk/arm_protprivk_rsa.pem
+ARM_PROTPK_HASH		:=	plat/arm/board/common/protpk/arm_protpk_rsa_sha256.bin
+ARM_SWD_ROT_KEY		:=	plat/arm/board/common/swd_rotpk/arm_swd_rotprivk_rsa.pem
+ARM_SWD_ROTPK_HASH	:=	plat/arm/board/common/swd_rotpk/arm_swd_rotpk_rsa_sha256.bin
+
+# Provide the private keys to cert_create tool. It needs them to sign the images.
+PROT_KEY		:=	${ARM_PROT_KEY}
+SWD_ROT_KEY		:=	${ARM_SWD_ROT_KEY}
+
+$(eval $(call add_define_val,ARM_PROTPK_HASH,'"$(ARM_PROTPK_HASH)"'))
+$(eval $(call add_define_val,ARM_SWD_ROTPK_HASH,'"$(ARM_SWD_ROTPK_HASH)"'))
+
+BL1_SOURCES		+=	plat/arm/board/common/protpk/arm_dev_protpk.S \
+				plat/arm/board/common/swd_rotpk/arm_dev_swd_rotpk.S
+BL2_SOURCES		+=	plat/arm/board/common/protpk/arm_dev_protpk.S \
+				plat/arm/board/common/swd_rotpk/arm_dev_swd_rotpk.S
+
+$(BUILD_PLAT)/bl1/arm_dev_protpk.o: $(ARM_PROTPK_HASH)
+$(BUILD_PLAT)/bl1/arm_dev_swd_rotpk.o: $(ARM_SWD_ROTPK_HASH)
 $(BUILD_PLAT)/bl2/arm_dev_protpk.o: $(ARM_PROTPK_HASH)
+$(BUILD_PLAT)/bl2/arm_dev_swd_rotpk.o: $(ARM_SWD_ROTPK_HASH)
 endif
 
 endif

diff --git a/plat/arm/board/common/swd_rotpk/README b/plat/arm/board/common/swd_rotpk/README
new file mode 100644
index 0000000..b628a5f
--- /dev/null
+++ b/plat/arm/board/common/swd_rotpk/README

@@ -0,0 +1,14 @@
+This directory contains some development keys to be used as the secure world
+root-of-trust key used in the CCA chain of trust.
+
+* swd_rotprivk_rsa.pem is a 2K RSA private key in PEM format. It has been
+  generated using the openssl command line tool:
+
+  openssl genrsa 2048 > arm_swd_rotprivk_rsa.pem
+
+* swd_rotpk_rsa_sha256.bin is the SHA-256 hash of the DER-encoded public key
+  associated with the above private key. It has been generated using the openssl
+  command line tool:
+
+  openssl rsa -in arm_swd_rotprivk_rsa.pem -pubout -outform DER | \
+    openssl dgst -sha256 -binary > arm_swd_rotpk_rsa_sha256.bin

diff --git a/plat/arm/board/common/swd_rotpk/arm_dev_swd_rotpk.S b/plat/arm/board/common/swd_rotpk/arm_dev_swd_rotpk.S
new file mode 100644
index 0000000..ae4f9d2
--- /dev/null
+++ b/plat/arm/board/common/swd_rotpk/arm_dev_swd_rotpk.S

@@ -0,0 +1,18 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+	.global arm_swd_rotpk_hash
+	.global arm_swd_rotpk_hash_end
+
+	.section .rodata.arm_swd_rotpk_hash, "a"
+
+arm_swd_rotpk_hash:
+	/* DER header. */
+	.byte 0x30, 0x31, 0x30, 0x0D, 0x06, 0x09, 0x60, 0x86, 0x48
+	.byte 0x01, 0x65, 0x03, 0x04, 0x02, 0x01, 0x05, 0x00, 0x04, 0x20
+	/* Key hash. */
+	.incbin ARM_SWD_ROTPK_HASH
+arm_swd_rotpk_hash_end:

diff --git a/plat/arm/board/common/swd_rotpk/arm_swd_rotpk_rsa_sha256.bin b/plat/arm/board/common/swd_rotpk/arm_swd_rotpk_rsa_sha256.bin
new file mode 100644
index 0000000..b2f3e60
--- /dev/null
+++ b/plat/arm/board/common/swd_rotpk/arm_swd_rotpk_rsa_sha256.bin

@@ -0,0 +1 @@
+0¾âÃæÈË(ì¨0wIÓÕéã¡gk
\ No newline at end of file

diff --git a/plat/arm/board/common/swd_rotpk/arm_swd_rotprivk_rsa.pem b/plat/arm/board/common/swd_rotpk/arm_swd_rotprivk_rsa.pem
new file mode 100644
index 0000000..0de655d
--- /dev/null
+++ b/plat/arm/board/common/swd_rotpk/arm_swd_rotprivk_rsa.pem

@@ -0,0 +1,27 @@
+-----BEGIN RSA PRIVATE KEY-----
+MIIEpQIBAAKCAQEA8igTd5QdZd181kz9vINr7Au34Rr/pQ1jpesfLlc1ZXCNAI9y
+/rhQlpw00y8rwOfgZsf18gPwGWWGhDJMsXI7OPem7BEUr8xKumuJuCiOdJh1STcR
+/JoFvz8wJPyycj/DOERRGsz+RvFBs6cLjSZHNQdzKDW+DE5vVJpmNWBVkoK7MCRD
+Wh/PMZVSoq9PeJOzayYcsipKvifT1+Wo9y2MG5zTDxi28rLr/FBm0CpTepBcRe8L
+pmgS7XJKhCQYxdDSzxi/0t/qXAwWuME4jv2HbNxsUZjahiBYpA0BafXanSuxVHly
+qpD0BmKAu7PpgKrEnUcPuHpZ2W+a05lNk6zjewIDAQABAoIBAG3twYCcTYgrtvs1
+8k38vyZl33CiKAGOhXkRtpL75fKJ2IizljmKBJOKj/R6ynsFCHrANadLIFj3HMyw
+ZN59A+OFkVJDIsf3jsj3/ooKZzkI6N120YSBizBZiAqSaJOy3HWTldn7y0b7SJ88
+quLFyLeLDTzowMCnbqTSfqmmdNJQAn+Q+7RX5sZGyBQUF2pRAA67cOYzc3a5MZ5E
+zBOs2u8VboC3ulEq876XWQbcXpRh/ap3eplQ1kAdyy64IPp2WbxqyXW0IQAQqaqh
+6oj19ME6mVD5wtELcYscJCDb7pA6WJtPp6nz/og2ifCJE/75T5RJ6fc6eBFMcofQ
+STIClGECgYEA/ZC0GX1HTKEKK3c1TiS3Zy0DS5ZoN5KFK7Sp1ZAjPE63iAr1a3z9
+Kepb+L8TBSw50tVD74MF5ChEid/ghF5BrVC3/YJkiiNpM1F51SMLGFeiDPRzJcx5
+KJkSflX7Q36BAXqj85Yz5AjgTPKcBqQRVZ6pNZN1HY99MloMg22WPRECgYEA9HtU
+FXmnTplXaNnihdq+vL5Z4/KDM+1f1E95y1PB8vkLI+o1szVNFP+BYz+w42lKtHW+
+c+z40AhFBGZQ0QCx83NOyObCReFjEbP8Nz71BsHe6GyMk9tSPIpzu9XB49Rs+9EO
+DAvFM5y2j5bH+lXE0pSyS3oBf51L9ZCPhp/vB8sCgYEAydwB1Gzsbu+hFfs/v2bx
+brzh67HgY6VMSP/5WF/3/RG5gB8hQ6HsNQsyjrMmZC7SFarb+3e2H+2CqrREm3wi
+EuS4pKPCgEoyfL03HVtZgNZ61o9gf83pAk3h8Bto/VFfSBsnHEsOIlKCph9Z4NuK
+RTwa/uDWEmNhyszvO03pldECgYEA2zB7GWnhc1mNgabfLY0JtuSeaPzzXqnyYcID
+eyUT3QglUcTY8lvWSP4ufdILgEfVP2fVIdAS30iawDAPQuLxqEf4Gayx/r7s+GE6
+vjlGqxFEDXPMsX9QApFK49voop/AOiCbDHe9DOHy11ei4TDmbrn8BClVkJlxEa/S
+ziszvfMCgYEA2V0zXziooI0toaOJEWZlAYhEONS5SG2z28HMLNgbdMcueGNhseaR
+NBGgPcu3EQhbL/hD0tBs09u6gjy1WD1i0HYnm1K1YQ1flzfbjUa3BqZETMbNhugd
+CM9yv0GEL/udZyOmO401aYl+QGXZX/WwlLQOe7WqQXOXJvW73oSqy7M=
+-----END RSA PRIVATE KEY-----

diff --git a/plat/arm/board/fvp/fdts/fvp_fw_config.dts b/plat/arm/board/fvp/fdts/fvp_fw_config.dts
index c26b519..577ac74 100644
--- a/plat/arm/board/fvp/fdts/fvp_fw_config.dts
+++ b/plat/arm/board/fvp/fdts/fvp_fw_config.dts

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, ARM Limited. All rights reserved.
+ * Copyright (c) 2019-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,9 +19,10 @@
 		};
 
 		hw-config {
-			load-address = <0x0 0x82000000>;
-			max-size = <0x01000000>;
+			load-address = <0x0 0x07f00000>;
+			max-size = <0x00100000>;
 			id = <HW_CONFIG_ID>;
+			ns-load-address = <0x0 0x82000000>;
 		};
 
 		/*

diff --git a/plat/arm/board/fvp/fdts/fvp_spmc_manifest.dts b/plat/arm/board/fvp/fdts/fvp_spmc_manifest.dts
index 21a6073..4543671 100644
--- a/plat/arm/board/fvp/fdts/fvp_spmc_manifest.dts
+++ b/plat/arm/board/fvp/fdts/fvp_spmc_manifest.dts

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -82,4 +82,8 @@
 		device_type = "memory";
 		reg = <0x0 0x6000000 0x2000000>; /* Trusted DRAM */
 	};
+
+#if MEASURED_BOOT
+#include "event_log.dtsi"
+#endif
 };

diff --git a/plat/arm/board/fvp/fdts/fvp_tb_fw_config.dts b/plat/arm/board/fvp/fdts/fvp_tb_fw_config.dts
index cf4ef2d..6fd334d 100644
--- a/plat/arm/board/fvp/fdts/fvp_tb_fw_config.dts
+++ b/plat/arm/board/fvp/fdts/fvp_tb_fw_config.dts

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2021, ARM Limited. All rights reserved.
+ * Copyright (c) 2020-2022, ARM Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -47,6 +47,9 @@
 			soc_fw_cfg_uuid = "9979814b-0376-fb46-8c8e-8d267f7859e0";
 			tos_fw_cfg_uuid = "26257c1a-dbc6-7f47-8d96-c4c4b0248021";
 			nt_fw_cfg_uuid = "28da9815-93e8-7e44-ac66-1aaf801550f9";
+			cca_cert_uuid = "36d83d85-761d-4daf-96f1-cd99d6569b00";
+			core_swd_cert_uuid = "52222d31-820f-494d-8bbc-ea6825d3c35a";
+			plat_cert_uuid = "d43cd902-5b9f-412e-8ac6-92b6d18be60d";
 			t_key_cert_uuid = "827ee890-f860-e411-a1b4-777a21b4f94c";
 			scp_fw_key_uuid = "024221a1-f860-e411-8d9b-f33c0e15a014";
 			soc_fw_key_uuid = "8ab8becc-f960-e411-9ad0-eb4822d8dcf8";

diff --git a/plat/arm/board/fvp/fdts/optee_sp_manifest.dts b/plat/arm/board/fvp/fdts/optee_sp_manifest.dts
index b803340..27f4724 100644
--- a/plat/arm/board/fvp/fdts/optee_sp_manifest.dts
+++ b/plat/arm/board/fvp/fdts/optee_sp_manifest.dts

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
@@ -22,7 +22,7 @@
 	exception-level = <2>; /* S-EL1 */
 	execution-state = <0>; /* AARCH64 */
 	load-address = <0x6280000>;
-	entrypoint-offset = <0x1000>;
+	entrypoint-offset = <0x4000>;
 	xlat-granule = <0>; /* 4KiB */
 	boot-order = <0>;
 	messaging-method = <0x3>; /* Direct request/response supported. */

diff --git a/plat/arm/board/fvp/fvp_bl1_measured_boot.c b/plat/arm/board/fvp/fvp_bl1_measured_boot.c
index 5468555..76cd918 100644
--- a/plat/arm/board/fvp/fvp_bl1_measured_boot.c
+++ b/plat/arm/board/fvp/fvp_bl1_measured_boot.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,6 +7,7 @@
 #include <stdint.h>
 
 #include <drivers/measured_boot/event_log/event_log.h>
+#include <drivers/measured_boot/rss/rss_measured_boot.h>
 #include <plat/arm/common/plat_arm.h>
 
 /* Event Log data */
@@ -21,10 +22,39 @@
 	{ EVLOG_INVALID_ID, NULL, (unsigned int)(-1) }	/* Terminator */
 };
 
+/* FVP table with platform specific image IDs and metadata. Intentionally not a
+ * const struct, some members might set by bootloaders during trusted boot.
+ */
+struct rss_mboot_metadata fvp_rss_mboot_metadata[] = {
+	{
+		.id = FW_CONFIG_ID,
+		.slot = U(6),
+		.signer_id_size = SIGNER_ID_MIN_SIZE,
+		.sw_type = RSS_MBOOT_FW_CONFIG_STRING,
+		.lock_measurement = true },
+	{
+		.id = TB_FW_CONFIG_ID,
+		.slot = U(7),
+		.signer_id_size = SIGNER_ID_MIN_SIZE,
+		.sw_type = RSS_MBOOT_TB_FW_CONFIG_STRING,
+		.lock_measurement = true },
+	{
+		.id = BL2_IMAGE_ID,
+		.slot = U(8),
+		.signer_id_size = SIGNER_ID_MIN_SIZE,
+		.sw_type = RSS_MBOOT_BL2_STRING,
+		.lock_measurement = true },
+
+	{
+		.id = RSS_MBOOT_INVALID_ID }
+};
+
 void bl1_plat_mboot_init(void)
 {
 	event_log_init(event_log, event_log + sizeof(event_log));
 	event_log_write_header();
+
+	rss_measured_boot_init();
 }
 
 void bl1_plat_mboot_finish(void)

diff --git a/plat/arm/board/fvp/fvp_bl2_measured_boot.c b/plat/arm/board/fvp/fvp_bl2_measured_boot.c
index 1f38278..e938e24 100644
--- a/plat/arm/board/fvp/fvp_bl2_measured_boot.c
+++ b/plat/arm/board/fvp/fvp_bl2_measured_boot.c

@@ -6,7 +6,9 @@
 
 #include <stdint.h>
 
+#include <common/tbbr/tbbr_img_def.h>
 #include <drivers/measured_boot/event_log/event_log.h>
+#include <drivers/measured_boot/rss/rss_measured_boot.h>
 #include <tools_share/tbbr_oid.h>
 #include <fvp_critical_data.h>
 
@@ -30,11 +32,54 @@
 	{ TOS_FW_CONFIG_ID, EVLOG_TOS_FW_CONFIG_STRING, PCR_0 },
 	{ RMM_IMAGE_ID, EVLOG_RMM_STRING, PCR_0},
 
+#if defined(SPD_spmd)
+	{ SP_PKG1_ID, EVLOG_SP1_STRING, PCR_0 },
+	{ SP_PKG2_ID, EVLOG_SP2_STRING, PCR_0 },
+	{ SP_PKG3_ID, EVLOG_SP3_STRING, PCR_0 },
+	{ SP_PKG4_ID, EVLOG_SP4_STRING, PCR_0 },
+	{ SP_PKG5_ID, EVLOG_SP5_STRING, PCR_0 },
+	{ SP_PKG6_ID, EVLOG_SP6_STRING, PCR_0 },
+	{ SP_PKG7_ID, EVLOG_SP7_STRING, PCR_0 },
+	{ SP_PKG8_ID, EVLOG_SP8_STRING, PCR_0 },
+#endif
+
 	{ CRITICAL_DATA_ID, EVLOG_CRITICAL_DATA_STRING, PCR_1 },
 
 	{ EVLOG_INVALID_ID, NULL, (unsigned int)(-1) }	/* Terminator */
 };
 
+/* FVP table with platform specific image IDs and metadata. Intentionally not a
+ * const struct, some members might set by bootloaders during trusted boot.
+ */
+struct rss_mboot_metadata fvp_rss_mboot_metadata[] = {
+	{
+		.id = BL31_IMAGE_ID,
+		.slot = U(9),
+		.signer_id_size = SIGNER_ID_MIN_SIZE,
+		.sw_type = RSS_MBOOT_BL31_STRING,
+		.lock_measurement = true },
+	{
+		.id = HW_CONFIG_ID,
+		.slot = U(10),
+		.signer_id_size = SIGNER_ID_MIN_SIZE,
+		.sw_type = RSS_MBOOT_HW_CONFIG_STRING,
+		.lock_measurement = true },
+	{
+		.id = SOC_FW_CONFIG_ID,
+		.slot = U(11),
+		.signer_id_size = SIGNER_ID_MIN_SIZE,
+		.sw_type = RSS_MBOOT_SOC_FW_CONFIG_STRING,
+		.lock_measurement = true },
+	{
+		.id = RMM_IMAGE_ID,
+		.slot = U(12),
+		.signer_id_size = SIGNER_ID_MIN_SIZE,
+		.sw_type = RSS_MBOOT_RMM_STRING,
+		.lock_measurement = true },
+	{
+		.id = RSS_MBOOT_INVALID_ID }
+};
+
 void bl2_plat_mboot_init(void)
 {
 	uint8_t *event_log_start;
@@ -64,6 +109,8 @@
 				       PLAT_ARM_EVENT_LOG_MAX_SIZE);
 
 	event_log_init((uint8_t *)event_log_start, event_log_finish);
+
+	rss_measured_boot_init();
 }
 
 int plat_mboot_measure_critical_data(unsigned int critical_data_id,

diff --git a/plat/arm/board/fvp/fvp_bl2_setup.c b/plat/arm/board/fvp/fvp_bl2_setup.c
index 5a17a0d..74e5d72 100644
--- a/plat/arm/board/fvp/fvp_bl2_setup.c
+++ b/plat/arm/board/fvp/fvp_bl2_setup.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2021, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -40,17 +40,23 @@
 struct bl_params *plat_get_next_bl_params(void)
 {
 	struct bl_params *arm_bl_params;
+	const struct dyn_cfg_dtb_info_t *hw_config_info __unused;
+	bl_mem_params_node_t *param_node __unused;
 
 	arm_bl_params = arm_get_next_bl_params();
 
-#if __aarch64__ && !BL2_AT_EL3
+#if !BL2_AT_EL3 && !EL3_PAYLOAD_BASE
 	const struct dyn_cfg_dtb_info_t *fw_config_info;
-	bl_mem_params_node_t *param_node;
-	uintptr_t fw_config_base = 0U;
+	uintptr_t fw_config_base = 0UL;
 	entry_point_info_t *ep_info;
 
+#if __aarch64__
 	/* Get BL31 image node */
 	param_node = get_bl_mem_params_node(BL31_IMAGE_ID);
+#else /* aarch32 */
+	/* Get SP_MIN image node */
+	param_node = get_bl_mem_params_node(BL32_IMAGE_ID);
+#endif /* __aarch64__ */
 	assert(param_node != NULL);
 
 	/* get fw_config load address */
@@ -58,15 +64,42 @@
 	assert(fw_config_info != NULL);
 
 	fw_config_base = fw_config_info->config_addr;
-	assert(fw_config_base != 0U);
+	assert(fw_config_base != 0UL);
 
 	/*
-	 * Get the entry point info of BL31 image and override
+	 * Get the entry point info of next executable image and override
 	 * arg1 of entry point info with fw_config base address
 	 */
 	ep_info = &param_node->ep_info;
 	ep_info->args.arg1 = (uint32_t)fw_config_base;
-#endif /* __aarch64__ && !BL2_AT_EL3 */
+
+	/* grab NS HW config address */
+	hw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, HW_CONFIG_ID);
+	assert(hw_config_info != NULL);
+
+	/* To retrieve actual size of the HW_CONFIG */
+	param_node = get_bl_mem_params_node(HW_CONFIG_ID);
+	assert(param_node != NULL);
+
+	/* Copy HW config from Secure address to NS address */
+	memcpy((void *)hw_config_info->ns_config_addr,
+	       (void *)hw_config_info->config_addr,
+	       (size_t)param_node->image_info.image_size);
+
+	/*
+	 * Ensure HW-config device tree committed to memory, as there is
+	 * a possibility to use HW-config without cache and MMU enabled
+	 * at BL33
+	 */
+	flush_dcache_range(hw_config_info->ns_config_addr,
+			   param_node->image_info.image_size);
+
+	param_node = get_bl_mem_params_node(BL33_IMAGE_ID);
+	assert(param_node != NULL);
+
+	/* Update BL33's ep info with NS HW config address  */
+	param_node->ep_info.args.arg1 = hw_config_info->ns_config_addr;
+#endif /* !BL2_AT_EL3 && !EL3_PAYLOAD_BASE */
 
 	return arm_bl_params;
 }

diff --git a/plat/arm/board/fvp/fvp_bl31_setup.c b/plat/arm/board/fvp/fvp_bl31_setup.c
index a94a4f4..dd90965 100644
--- a/plat/arm/board/fvp/fvp_bl31_setup.c
+++ b/plat/arm/board/fvp/fvp_bl31_setup.c

@@ -17,6 +17,8 @@
 
 #include "fvp_private.h"
 
+static const struct dyn_cfg_dtb_info_t *hw_config_info __unused;
+
 void __init bl31_early_platform_setup2(u_register_t arg0,
 		u_register_t arg1, u_register_t arg2, u_register_t arg3)
 {
@@ -34,6 +36,17 @@
 	if (soc_fw_config_info != NULL) {
 		arg1 = soc_fw_config_info->config_addr;
 	}
+
+	/*
+	 * arg2 is currently holding the 'secure' address of HW_CONFIG.
+	 * But arm_bl31_early_platform_setup() below expects the 'non-secure'
+	 * address of HW_CONFIG (which it will pass to BL33).
+	 * This why we need to override arg2 here.
+	 */
+	hw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, HW_CONFIG_ID);
+	assert(hw_config_info != NULL);
+	assert(hw_config_info->ns_config_addr != 0UL);
+	arg2 = hw_config_info->ns_config_addr;
 #endif /* !RESET_TO_BL31 && !BL2_AT_EL3 */
 
 	arm_bl31_early_platform_setup((void *)arg0, arg1, arg2, (void *)arg3);
@@ -66,22 +79,57 @@
 
 void __init bl31_plat_arch_setup(void)
 {
+	int rc __unused;
+	uintptr_t hw_config_base_align __unused;
+	size_t mapped_size_align __unused;
+
 	arm_bl31_plat_arch_setup();
 
 	/*
 	 * For RESET_TO_BL31 systems, BL31 is the first bootloader to run.
 	 * So there is no BL2 to load the HW_CONFIG dtb into memory before
-	 * control is passed to BL31.
+	 * control is passed to BL31. The code below relies on dynamic mapping
+	 * capability, which is not supported by xlat tables lib V1.
+	 * TODO: remove the ARM_XLAT_TABLES_LIB_V1 check when its support
+	 * gets deprecated.
 	 */
-#if !RESET_TO_BL31 && !BL2_AT_EL3
-	/* HW_CONFIG was also loaded by BL2 */
-	const struct dyn_cfg_dtb_info_t *hw_config_info;
-
-	hw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, HW_CONFIG_ID);
+#if !RESET_TO_BL31 && !BL2_AT_EL3 && !ARM_XLAT_TABLES_LIB_V1
 	assert(hw_config_info != NULL);
+	assert(hw_config_info->config_addr != 0UL);
 
+	/* Page aligned address and size if necessary */
+	hw_config_base_align = page_align(hw_config_info->config_addr, DOWN);
+	mapped_size_align = page_align(hw_config_info->config_max_size, UP);
+
+	if ((hw_config_info->config_addr != hw_config_base_align) &&
+	    (hw_config_info->config_max_size == mapped_size_align)) {
+		mapped_size_align += PAGE_SIZE;
+	}
+
+	/*
+	 * map dynamically HW config region with its aligned base address and
+	 * size
+	 */
+	rc = mmap_add_dynamic_region((unsigned long long)hw_config_base_align,
+				     hw_config_base_align,
+				     mapped_size_align,
+				     MT_RO_DATA);
+	if (rc != 0) {
+		ERROR("Error while mapping HW_CONFIG device tree (%d).\n", rc);
+		panic();
+	}
+
+	/* Populate HW_CONFIG device tree with the mapped address */
 	fconf_populate("HW_CONFIG", hw_config_info->config_addr);
-#endif
+
+	/* unmap the HW_CONFIG memory region */
+	rc = mmap_remove_dynamic_region(hw_config_base_align, mapped_size_align);
+	if (rc != 0) {
+		ERROR("Error while unmapping HW_CONFIG device tree (%d).\n",
+		      rc);
+		panic();
+	}
+#endif /* !RESET_TO_BL31 && !BL2_AT_EL3 && !ARM_XLAT_TABLES_LIB_V1 */
 }
 
 unsigned int plat_get_syscnt_freq2(void)

diff --git a/plat/arm/board/fvp/fvp_common.c b/plat/arm/board/fvp/fvp_common.c
index d8d19de..f8463f1 100644
--- a/plat/arm/board/fvp/fvp_common.c
+++ b/plat/arm/board/fvp/fvp_common.c

@@ -17,6 +17,9 @@
 #include <lib/xlat_tables/xlat_tables_compat.h>
 #include <platform_def.h>
 #include <services/arm_arch_svc.h>
+#if ENABLE_RME
+#include <services/rmm_core_manifest.h>
+#endif
 #if SPM_MM
 #include <services/spm_mm_partition.h>
 #endif
@@ -104,9 +107,10 @@
 #ifdef __aarch64__
 	ARM_MAP_DRAM2,
 #endif
-#if defined(SPD_spmd)
+	/*
+	 * Required to load HW_CONFIG, SPMC and SPs to trusted DRAM.
+	 */
 	ARM_MAP_TRUSTED_DRAM,
-#endif
 #if ENABLE_RME
 	ARM_MAP_RMM_DRAM,
 	ARM_MAP_GPT_L1_DRAM,
@@ -126,7 +130,7 @@
 	 */
 	ARM_MAP_BL1_RW,
 #endif /* CRYPTO_SUPPORT && !BL2_AT_EL3 */
-#if SPM_MM
+#if SPM_MM || SPMC_AT_EL3
 	ARM_SP_IMAGE_MMAP,
 #endif
 #if ARM_BL31_IN_DRAM
@@ -166,10 +170,9 @@
 #if SPM_MM
 	ARM_SPM_BUF_EL3_MMAP,
 #endif
-	/* Required by fconf APIs to read HW_CONFIG dtb loaded into DRAM */
-	ARM_DTB_DRAM_NS,
 #if ENABLE_RME
 	ARM_MAP_GPT_L1_DRAM,
+	ARM_MAP_EL3_RMM_SHARED_MEM,
 #endif
 	{0}
 };
@@ -197,8 +200,6 @@
 	V2M_MAP_IOFPGA,
 	MAP_DEVICE0,
 	MAP_DEVICE1,
-	/* Required by fconf APIs to read HW_CONFIG dtb loaded into DRAM */
-	ARM_DTB_DRAM_NS,
 	{0}
 };
 #endif
@@ -515,3 +516,29 @@
 	return (int32_t)(((sys_id >> V2M_SYS_ID_REV_SHIFT) &
 			  V2M_SYS_ID_REV_MASK) & SOC_ID_REV_MASK);
 }
+
+#if ENABLE_RME
+/*
+ * Get a pointer to the RMM-EL3 Shared buffer and return it
+ * through the pointer passed as parameter.
+ *
+ * This function returns the size of the shared buffer.
+ */
+size_t plat_rmmd_get_el3_rmm_shared_mem(uintptr_t *shared)
+{
+	*shared = (uintptr_t)RMM_SHARED_BASE;
+
+	return (size_t)RMM_SHARED_SIZE;
+}
+
+int plat_rmmd_load_manifest(rmm_manifest_t *manifest)
+{
+	assert(manifest != NULL);
+
+	manifest->version = RMMD_MANIFEST_VERSION;
+	manifest->plat_data = (uintptr_t)NULL;
+
+	return 0;
+}
+
+#endif

diff --git a/plat/arm/board/fvp/fvp_common_measured_boot.c b/plat/arm/board/fvp/fvp_common_measured_boot.c
index 6a403d9..93aa055 100644
--- a/plat/arm/board/fvp/fvp_common_measured_boot.c
+++ b/plat/arm/board/fvp/fvp_common_measured_boot.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,27 +9,47 @@
 
 #include <common/desc_image_load.h>
 #include <drivers/measured_boot/event_log/event_log.h>
+#include <drivers/measured_boot/rss/rss_measured_boot.h>
 #include <plat/arm/common/plat_arm.h>
 #include <plat/common/platform.h>
 
 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;
+}
+
 int plat_mboot_measure_image(unsigned int image_id, image_info_t *image_data)
 {
+	int err;
+	int rc = 0;
+
 	/* 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);
+	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;
+		      "Failed to ", "record in event log", image_id, err);
+		rc = err;
 	}
 
-	return 0;
+	/* Calculate image hash and record data in RSS */
+	err = rss_mboot_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 in RSS", image_id, err);
+		rc = (rc == 0) ? err : -1;
+	}
+
+	return rc;
 }

diff --git a/plat/arm/board/fvp/fvp_el3_spmc.c b/plat/arm/board/fvp/fvp_el3_spmc.c
new file mode 100644
index 0000000..2b347ed
--- /dev/null
+++ b/plat/arm/board/fvp/fvp_el3_spmc.c

@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <services/el3_spmc_ffa_memory.h>
+
+#include <platform_def.h>
+
+/*
+ * On the FVP platform when using the EL3 SPMC implementation allocate the
+ * datastore for tracking shared memory descriptors in the TZC DRAM section
+ * to ensure sufficient storage can be allocated.
+ * Provide an implementation of the accessor method to allow the datastore
+ * details to be retrieved by the SPMC.
+ * The SPMC will take care of initializing the memory region.
+ */
+
+#define PLAT_SPMC_SHMEM_DATASTORE_SIZE 512 * 1024
+
+__section("arm_el3_tzc_dram") static uint8_t
+plat_spmc_shmem_datastore[PLAT_SPMC_SHMEM_DATASTORE_SIZE];
+
+int plat_spmc_shmem_datastore_get(uint8_t **datastore, size_t *size)
+{
+	*datastore = plat_spmc_shmem_datastore;
+	*size = PLAT_SPMC_SHMEM_DATASTORE_SIZE;
+	return 0;
+}
+
+/*
+ * Add dummy implementations of memory management related platform hooks.
+ * These can be used to implement platform specific functionality to support
+ * a memory sharing/lending operation.
+ *
+ * Note: The hooks must be located as part of the initial share request and
+ * final reclaim to prevent order dependencies with operations that may take
+ * place in the normal world without visibility of the SPMC.
+ */
+int plat_spmc_shmem_begin(struct ffa_mtd *desc)
+{
+	return 0;
+}
+int plat_spmc_shmem_reclaim(struct ffa_mtd *desc)
+{
+	return 0;
+}

diff --git a/plat/arm/board/fvp/fvp_el3_spmc_logical_sp.c b/plat/arm/board/fvp/fvp_el3_spmc_logical_sp.c
new file mode 100644
index 0000000..b9e4f86
--- /dev/null
+++ b/plat/arm/board/fvp/fvp_el3_spmc_logical_sp.c

@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <services/el3_spmc_logical_sp.h>
+#include <services/ffa_svc.h>
+#include <smccc_helpers.h>
+
+#define LP_PARTITION_ID 0xC001
+#define LP_UUID {0x47a3bf57, 0xe98e43ad, 0xb7db524f, 0x1588f4e3}
+
+/* Our Logical SP currently only supports receipt of direct messaging. */
+#define PARTITION_PROPERTIES FFA_PARTITION_DIRECT_REQ_RECV
+
+static int32_t sp_init(void)
+{
+	INFO("LSP: Init function called.\n");
+	return 0;
+}
+
+static uint64_t handle_ffa_direct_request(uint32_t smc_fid,  bool secure_origin,
+					  uint64_t x1, uint64_t x2, uint64_t x3,
+					  uint64_t x4, void *cookie,
+					  void *handle, uint64_t flags)
+{
+	uint64_t ret;
+
+	/* Determine if we have a 64 or 32 direct request. */
+	if (smc_fid == FFA_MSG_SEND_DIRECT_REQ_SMC32) {
+		ret = FFA_MSG_SEND_DIRECT_RESP_SMC32;
+	} else if (smc_fid == FFA_MSG_SEND_DIRECT_REQ_SMC64) {
+		ret = FFA_MSG_SEND_DIRECT_RESP_SMC64;
+	} else {
+		panic(); /* Unknown SMC. */
+	}
+	/*
+	 * Handle the incoming request. For testing purposes we echo the
+	 * incoming message.
+	 */
+	INFO("Logical Partition: Received Direct Request from %s world!\n",
+	     secure_origin ? "Secure" : "Normal");
+
+	/*
+	 * Logical SP's must always send a direct response so we can populate
+	 * our response directly.
+	 */
+	SMC_RET8(handle, ret, 0, 0, x4, 0, 0, 0, 0);
+}
+
+/* Register logical partition  */
+DECLARE_LOGICAL_PARTITION(
+	my_logical_partition,
+	sp_init,			/* Init Function */
+	LP_PARTITION_ID,		/* FF-A Partition ID */
+	LP_UUID,			/* UUID */
+	PARTITION_PROPERTIES,		/* Partition Properties. */
+	handle_ffa_direct_request	/* Callback for direct requests. */
+);

diff --git a/plat/arm/board/fvp/fvp_plat_attest_token.c b/plat/arm/board/fvp/fvp_plat_attest_token.c
index 5463f33..1b0854b 100644
--- a/plat/arm/board/fvp/fvp_plat_attest_token.c
+++ b/plat/arm/board/fvp/fvp_plat_attest_token.c

@@ -300,8 +300,8 @@
 	0xB0, 0x3A
 };
 
-int plat_get_cca_attest_token(uintptr_t buf, size_t *len,
-			       uintptr_t hash, size_t hash_size)
+int plat_rmmd_get_cca_attest_token(uintptr_t buf, size_t *len,
+				   uintptr_t hash, size_t hash_size)
 {
 	(void)hash;
 	(void)hash_size;

diff --git a/plat/arm/board/fvp/fvp_realm_attest_key.c b/plat/arm/board/fvp/fvp_realm_attest_key.c
index b32f557..1af1f0d 100644
--- a/plat/arm/board/fvp/fvp_realm_attest_key.c
+++ b/plat/arm/board/fvp/fvp_realm_attest_key.c

@@ -19,7 +19,8 @@
 	0xEB, 0x1A, 0x41, 0x85, 0xBD, 0x11, 0x7F, 0x68
 };
 
-int plat_get_cca_realm_attest_key(uintptr_t buf, size_t *len, unsigned int type)
+int plat_rmmd_get_cca_realm_attest_key(uintptr_t buf, size_t *len,
+				       unsigned int type)
 {
 	assert(type == ATTEST_KEY_CURVE_ECC_SECP384R1);
 

diff --git a/plat/arm/board/fvp/include/platform_def.h b/plat/arm/board/fvp/include/platform_def.h
index 5e5ddce..d0f8aa9 100644
--- a/plat/arm/board/fvp/include/platform_def.h
+++ b/plat/arm/board/fvp/include/platform_def.h

@@ -90,6 +90,31 @@
 					FVP_DTB_DRAM_MAP_START,		\
 					FVP_DTB_DRAM_MAP_SIZE,		\
 					MT_MEMORY | MT_RO | MT_NS)
+
+#if SPMC_AT_EL3
+/*
+ * Number of Secure Partitions supported.
+ * SPMC at EL3, uses this count to configure the maximum number of supported
+ * secure partitions.
+ */
+#define SECURE_PARTITION_COUNT		1
+
+/*
+ * Number of Normal World Partitions supported.
+ * SPMC at EL3, uses this count to configure the maximum number of supported
+ * NWd partitions.
+ */
+#define NS_PARTITION_COUNT		1
+
+/*
+ * Number of Logical Partitions supported.
+ * SPMC at EL3, uses this count to configure the maximum number of supported
+ * logical partitions.
+ */
+#define MAX_EL3_LP_DESCS_COUNT		1
+
+#endif /* SPMC_AT_EL3 */
+
 /*
  * Load address of BL33 for this platform port
  */
@@ -102,32 +127,36 @@
 #if defined(IMAGE_BL31)
 # if SPM_MM
 #  define PLAT_ARM_MMAP_ENTRIES		10
-#  if ENABLE_RME
-#   define MAX_XLAT_TABLES		11
-#  else
-#   define MAX_XLAT_TABLES		9
-# endif
+#  define MAX_XLAT_TABLES		9
 #  define PLAT_SP_IMAGE_MMAP_REGIONS	30
 #  define PLAT_SP_IMAGE_MAX_XLAT_TABLES	10
+# elif SPMC_AT_EL3
+#  define PLAT_ARM_MMAP_ENTRIES		13
+#  define MAX_XLAT_TABLES		11
 # else
 #  define PLAT_ARM_MMAP_ENTRIES		9
 #  if USE_DEBUGFS
 #   if ENABLE_RME
-#    define MAX_XLAT_TABLES		10
+#    define MAX_XLAT_TABLES		9
 #   else
 #    define MAX_XLAT_TABLES		8
 #   endif
 #  else
 #   if ENABLE_RME
-#    define MAX_XLAT_TABLES		9
+#    define MAX_XLAT_TABLES		8
 #   else
 #    define MAX_XLAT_TABLES		7
 #   endif
 #  endif
 # endif
 #elif defined(IMAGE_BL32)
-# define PLAT_ARM_MMAP_ENTRIES		9
-# define MAX_XLAT_TABLES		6
+# if SPMC_AT_EL3
+#  define PLAT_ARM_MMAP_ENTRIES		270
+#  define MAX_XLAT_TABLES		10
+# else
+#  define PLAT_ARM_MMAP_ENTRIES		9
+#  define MAX_XLAT_TABLES		6
+# endif
 #elif !USE_ROMLIB
 # define PLAT_ARM_MMAP_ENTRIES		11
 # define MAX_XLAT_TABLES		5
@@ -164,6 +193,9 @@
 # define PLAT_ARM_MAX_BL2_SIZE	(UL(0x1E000) - FVP_BL2_ROMLIB_OPTIMIZATION)
 #elif CRYPTO_SUPPORT
 # define PLAT_ARM_MAX_BL2_SIZE	(UL(0x1D000) - FVP_BL2_ROMLIB_OPTIMIZATION)
+#elif ARM_BL31_IN_DRAM
+/* When ARM_BL31_IN_DRAM is set, BL2 can use almost all of Trusted SRAM. */
+# define PLAT_ARM_MAX_BL2_SIZE	(UL(0x1F000) - FVP_BL2_ROMLIB_OPTIMIZATION)
 #else
 # define PLAT_ARM_MAX_BL2_SIZE	(UL(0x13000) - FVP_BL2_ROMLIB_OPTIMIZATION)
 #endif
@@ -261,6 +293,7 @@
 #define PLAT_ARM_TRP_UART_CLK_IN_HZ	V2M_IOFPGA_UART3_CLK_IN_HZ
 
 #define PLAT_FVP_SMMUV3_BASE		UL(0x2b400000)
+#define PLAT_ARM_SMMUV3_ROOT_REG_OFFSET UL(0x20000)
 
 /* CCI related constants */
 #define PLAT_FVP_CCI400_BASE		UL(0x2c090000)

diff --git a/plat/arm/board/fvp/jmptbl.i b/plat/arm/board/fvp/jmptbl.i
index b72bdab..85e6e3a 100644
--- a/plat/arm/board/fvp/jmptbl.i
+++ b/plat/arm/board/fvp/jmptbl.i

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -35,6 +35,8 @@
 fdt     fdt_get_name
 fdt     fdt_get_alias
 fdt     fdt_node_offset_by_phandle
+fdt     fdt_subnode_offset
+fdt     fdt_add_subnode
 mbedtls mbedtls_asn1_get_alg
 mbedtls mbedtls_asn1_get_alg_null
 mbedtls mbedtls_asn1_get_bitstring_null

diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index c9f5551..a7e27e8 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk

@@ -333,23 +333,14 @@
 endif
 
 # Enable the dynamic translation tables library.
-ifeq (${ARCH},aarch32)
-    ifeq (${RESET_TO_SP_MIN},1)
+ifeq ($(filter 1,${BL2_AT_EL3} ${ARM_XLAT_TABLES_LIB_V1}),)
+    ifeq (${ARCH},aarch32)
         BL32_CPPFLAGS	+=	-DPLAT_XLAT_TABLES_DYNAMIC
-    endif
-else # AArch64
-    ifeq (${RESET_TO_BL31},1)
-        BL31_CPPFLAGS	+=	-DPLAT_XLAT_TABLES_DYNAMIC
-    endif
-    ifeq (${SPD},trusty)
+    else # AArch64
         BL31_CPPFLAGS	+=	-DPLAT_XLAT_TABLES_DYNAMIC
     endif
 endif
 
-ifeq (${ENABLE_RME},1)
-    BL31_CPPFLAGS	+=	-DPLAT_XLAT_TABLES_DYNAMIC
-endif
-
 ifeq (${ALLOW_RO_XLAT_TABLES}, 1)
     ifeq (${ARCH},aarch32)
         BL32_CPPFLAGS	+=	-DPLAT_RO_XLAT_TABLES
@@ -372,14 +363,40 @@
     override BL1_SOURCES =
 endif
 
+# Include Measured Boot makefile before any Crypto library makefile.
+# Crypto library makefile may need default definitions of Measured Boot build
+# flags present in Measured Boot makefile.
+ifeq (${MEASURED_BOOT},1)
+    RSS_MEASURED_BOOT_MK := drivers/measured_boot/rss/rss_measured_boot.mk
+    $(info Including ${RSS_MEASURED_BOOT_MK})
+    include ${RSS_MEASURED_BOOT_MK}
+
+    ifneq (${MBOOT_RSS_HASH_ALG}, sha256)
+        $(eval $(call add_define,TF_MBEDTLS_MBOOT_USE_SHA512))
+    endif
+
+    BL1_SOURCES		+=	${MEASURED_BOOT_SOURCES}
+    BL2_SOURCES		+=	${MEASURED_BOOT_SOURCES}
+endif
+
 include plat/arm/board/common/board_common.mk
 include plat/arm/common/arm_common.mk
 
 ifeq (${MEASURED_BOOT},1)
 BL1_SOURCES		+=	plat/arm/board/fvp/fvp_common_measured_boot.c	\
-				plat/arm/board/fvp/fvp_bl1_measured_boot.c
+				plat/arm/board/fvp/fvp_bl1_measured_boot.c	\
+				lib/psa/measured_boot.c
+
 BL2_SOURCES		+=	plat/arm/board/fvp/fvp_common_measured_boot.c	\
-				plat/arm/board/fvp/fvp_bl2_measured_boot.c
+				plat/arm/board/fvp/fvp_bl2_measured_boot.c	\
+				lib/psa/measured_boot.c
+
+PLAT_INCLUDES		+=	-Iinclude/lib/psa
+
+# RSS is not supported on FVP right now. Thus, we use the mocked version
+# of PSA Measured Boot APIs. They return with success and hard-coded data.
+PLAT_RSS_NOT_SUPPORTED	:= 1
+
 endif
 
 ifeq (${TRUSTED_BOARD_BOOT}, 1)
@@ -394,8 +411,21 @@
 # enable trace buffer control registers access to NS by default
 ENABLE_TRBE_FOR_NS		:= 1
 
+# enable branch record buffer control registers access in NS by default
+# only enable for aarch64
+# do not enable when ENABLE_RME=1
+ifeq (${ARCH}, aarch64)
+ifeq (${ENABLE_RME},0)
+	ENABLE_BRBE_FOR_NS		:= 1
+endif
+endif
+
 # enable trace system registers access to NS by default
 ENABLE_SYS_REG_TRACE_FOR_NS	:= 1
 
 # enable trace filter control registers access to NS by default
 ENABLE_TRF_FOR_NS		:= 1
+
+ifeq (${SPMC_AT_EL3}, 1)
+PLAT_BL_COMMON_SOURCES	+=	plat/arm/board/fvp/fvp_el3_spmc.c
+endif

diff --git a/plat/arm/board/fvp/sp_min/fvp_sp_min_setup.c b/plat/arm/board/fvp/sp_min/fvp_sp_min_setup.c
index 763b42a..9ab36a6 100644
--- a/plat/arm/board/fvp/sp_min/fvp_sp_min_setup.c
+++ b/plat/arm/board/fvp/sp_min/fvp_sp_min_setup.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,15 +9,31 @@
 #include <bl32/sp_min/platform_sp_min.h>
 #include <common/debug.h>
 #include <lib/fconf/fconf.h>
+#include <lib/fconf/fconf_dyn_cfg_getter.h>
 #include <plat/arm/common/plat_arm.h>
 
 #include "../fvp_private.h"
 
-uintptr_t hw_config_dtb;
-
 void plat_arm_sp_min_early_platform_setup(u_register_t arg0, u_register_t arg1,
 			u_register_t arg2, u_register_t arg3)
 {
+	const struct dyn_cfg_dtb_info_t *tos_fw_config_info __unused;
+
+	/* Initialize the console to provide early debug support */
+	arm_console_boot_init();
+
+#if !RESET_TO_SP_MIN && !BL2_AT_EL3
+
+	INFO("SP_MIN FCONF: FW_CONFIG address = %lx\n", (uintptr_t)arg1);
+	/* Fill the properties struct with the info from the config dtb */
+	fconf_populate("FW_CONFIG", arg1);
+
+	tos_fw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, TOS_FW_CONFIG_ID);
+	if (tos_fw_config_info != NULL) {
+		arg1 = tos_fw_config_info->config_addr;
+	}
+#endif /* !RESET_TO_SP_MIN && !BL2_AT_EL3 */
+
 	arm_sp_min_early_platform_setup((void *)arg0, arg1, arg2, (void *)arg3);
 
 	/* Initialize the platform config for future decision making */
@@ -37,12 +53,15 @@
 	 * FVP PSCI code will enable coherency for other clusters.
 	 */
 	fvp_interconnect_enable();
-
-	hw_config_dtb = arg2;
 }
 
 void sp_min_plat_arch_setup(void)
 {
+	int rc __unused;
+	const struct dyn_cfg_dtb_info_t *hw_config_info __unused;
+	uintptr_t hw_config_base_align __unused;
+	size_t mapped_size_align __unused;
+
 	arm_sp_min_plat_arch_setup();
 
 	/*
@@ -50,11 +69,53 @@
 	 * to run. So there is no BL2 to load the HW_CONFIG dtb into memory
 	 * before control is passed to SP_MIN.
 	 * Also, BL2 skips loading HW_CONFIG dtb for BL2_AT_EL3 builds.
+	 * The code below relies on dynamic mapping capability, which is not
+	 * supported by xlat tables lib V1.
+	 * TODO: remove the ARM_XLAT_TABLES_LIB_V1 check when its support
+	 * gets deprecated.
 	 */
-#if !RESET_TO_SP_MIN && !BL2_AT_EL3
-	assert(hw_config_dtb != 0U);
+#if !RESET_TO_SP_MIN && !BL2_AT_EL3 && !ARM_XLAT_TABLES_LIB_V1
+	hw_config_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, HW_CONFIG_ID);
+	assert(hw_config_info != NULL);
+	assert(hw_config_info->config_addr != 0UL);
+
+	INFO("SP_MIN FCONF: HW_CONFIG address = %p\n",
+	     (void *)hw_config_info->config_addr);
+
+	/*
+	 * Preferrably we expect this address and size are page aligned,
+	 * but if they are not then align it.
+	 */
+	hw_config_base_align = page_align(hw_config_info->config_addr, DOWN);
+	mapped_size_align = page_align(hw_config_info->config_max_size, UP);
+
+	if ((hw_config_info->config_addr != hw_config_base_align) &&
+	    (hw_config_info->config_max_size == mapped_size_align)) {
+		mapped_size_align += PAGE_SIZE;
+	}
+
+	/*
+	 * map dynamically HW config region with its aligned base address and
+	 * size
+	 */
+	rc = mmap_add_dynamic_region((unsigned long long)hw_config_base_align,
+				     hw_config_base_align,
+				     mapped_size_align,
+				     MT_RO_DATA);
+	if (rc != 0) {
+		ERROR("Error while mapping HW_CONFIG device tree (%d).\n", rc);
+		panic();
+	}
+
+	/* Populate HW_CONFIG device tree with the mapped address */
+	fconf_populate("HW_CONFIG", hw_config_info->config_addr);
 
-	INFO("SP_MIN FCONF: HW_CONFIG address = %p\n", (void *)hw_config_dtb);
-	fconf_populate("HW_CONFIG", hw_config_dtb);
-#endif
+	/* unmap the HW_CONFIG memory region */
+	rc = mmap_remove_dynamic_region(hw_config_base_align, mapped_size_align);
+	if (rc != 0) {
+		ERROR("Error while unmapping HW_CONFIG device tree (%d).\n",
+		      rc);
+		panic();
+	}
+#endif /* !RESET_TO_SP_MIN && !BL2_AT_EL3 && !ARM_XLAT_TABLES_LIB_V1 */
 }

diff --git a/plat/arm/board/fvp/sp_min/sp_min-fvp.mk b/plat/arm/board/fvp/sp_min/sp_min-fvp.mk
index 0d8cca5..183d802 100644
--- a/plat/arm/board/fvp/sp_min/sp_min-fvp.mk
+++ b/plat/arm/board/fvp/sp_min/sp_min-fvp.mk

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2016-2022, Arm Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -25,7 +25,8 @@
 # Added separately from the above list for better readability
 ifeq ($(filter 1,${BL2_AT_EL3} ${RESET_TO_SP_MIN}),)
 BL32_SOURCES		+=	lib/fconf/fconf.c				\
-				plat/arm/board/fvp/fconf/fconf_hw_config_getter.c
+				lib/fconf/fconf_dyn_cfg_getter.c		\
+				plat/arm/board/fvp/fconf/fconf_hw_config_getter.c \
 
 BL32_SOURCES		+=	${FDT_WRAPPERS_SOURCES}
 

diff --git a/plat/arm/board/fvp_r/fvp_r_bl1_setup.c b/plat/arm/board/fvp_r/fvp_r_bl1_setup.c
index 68872c1..1ac0a9c 100644
--- a/plat/arm/board/fvp_r/fvp_r_bl1_setup.c
+++ b/plat/arm/board/fvp_r/fvp_r_bl1_setup.c

@@ -154,7 +154,8 @@
 
 	/* Set global DTB info for fixed fw_config information */
 	fw_config_max_size = ARM_FW_CONFIG_LIMIT - ARM_FW_CONFIG_BASE;
-	set_config_info(ARM_FW_CONFIG_BASE, fw_config_max_size, FW_CONFIG_ID);
+	set_config_info(ARM_FW_CONFIG_BASE, ~0UL, fw_config_max_size,
+			FW_CONFIG_ID);
 
 	assert(bl1_plat_get_image_desc(BL33_IMAGE_ID) != NULL);
 

diff --git a/plat/arm/board/juno/jmptbl.i b/plat/arm/board/juno/jmptbl.i
index 393a648..8932aa0 100644
--- a/plat/arm/board/juno/jmptbl.i
+++ b/plat/arm/board/juno/jmptbl.i

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -34,6 +34,8 @@
 fdt     fdt_get_name
 fdt     fdt_get_alias
 fdt     fdt_node_offset_by_phandle
+fdt     fdt_subnode_offset
+fdt     fdt_add_subnode
 mbedtls mbedtls_asn1_get_alg
 mbedtls mbedtls_asn1_get_alg_null
 mbedtls mbedtls_asn1_get_bitstring_null

diff --git a/plat/arm/board/morello/morello_bl2_setup.c b/plat/arm/board/morello/morello_bl2_setup.c
index 0d4b6d0..da1f7ae 100644
--- a/plat/arm/board/morello/morello_bl2_setup.c
+++ b/plat/arm/board/morello/morello_bl2_setup.c

@@ -1,27 +1,226 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #include <common/debug.h>
+#include <drivers/arm/css/sds.h>
+#include <lib/mmio.h>
 #include <lib/utils.h>
 #include <plat/arm/common/plat_arm.h>
 
-void bl2_platform_setup(void)
-{
+#include "morello_def.h"
+#include <platform_def.h>
+
+#ifdef TARGET_PLATFORM_FVP
+/*
+ * Platform information structure stored in SDS.
+ * This structure holds information about platform's DDR
+ * size
+ *	- Local DDR size in bytes, DDR memory in main board
+ */
+struct morello_plat_info {
+	uint64_t local_ddr_size;
+} __packed;
+#else
+/*
+ * Platform information structure stored in SDS.
+ * This structure holds information about platform's DDR
+ * size which is an information about multichip setup
+ *	- Local DDR size in bytes, DDR memory in main board
+ *	- Remote DDR size in bytes, DDR memory in remote board
+ *	- remote_chip_count
+ *	- multichip mode
+ *	- scc configuration
+ */
+struct morello_plat_info {
+	uint64_t local_ddr_size;
+	uint64_t remote_ddr_size;
+	uint8_t remote_chip_count;
+	bool multichip_mode;
+	uint32_t scc_config;
+} __packed;
+#endif
+
+/* Compile time assertion to ensure the size of structure is 18 bytes */
+CASSERT(sizeof(struct morello_plat_info) == MORELLO_SDS_PLATFORM_INFO_SIZE,
+		assert_invalid_plat_info_size);
+
 #ifdef TARGET_PLATFORM_SOC
-	/*
-	 * Morello platform supports RDIMMs with ECC capability. To use the ECC
-	 * capability, the entire DDR memory space has to be zeroed out before
-	 * enabling the ECC bits in DMC-Bing.
-	 * Zeroing DDR memory range 0x80000000 - 0xFFFFFFFF during BL2 stage,
-	 * as BL33 binary cannot be copied to DDR memory before enabling ECC.
-	 * Rest of the DDR memory space is zeroed out during BL31 stage.
-	 */
+/*
+ * Morello platform supports RDIMMs with ECC capability. To use the ECC
+ * capability, the entire DDR memory space has to be zeroed out before
+ * enabling the ECC bits in DMC-Bing. Zeroing out several gigabytes of
+ * memory from SCP is quite time consuming so the following function
+ * is added to zero out the DDR memory from application processor which is
+ * much faster compared to SCP.
+ */
+
+static void dmc_ecc_setup(struct morello_plat_info *plat_info)
+{
+	uint64_t dram2_size;
+	uint32_t val;
+	uint64_t tag_mem_base;
+	uint64_t usable_mem_size;
+
+	INFO("Total DIMM size: %uGB\n",
+			(uint32_t)(plat_info->local_ddr_size / 0x40000000));
+
+	assert(plat_info->local_ddr_size > ARM_DRAM1_SIZE);
+	dram2_size = plat_info->local_ddr_size - ARM_DRAM1_SIZE;
+
 	INFO("Zeroing DDR memory range 0x80000000 - 0xFFFFFFFF\n");
 	zero_normalmem((void *)ARM_DRAM1_BASE, ARM_DRAM1_SIZE);
 	flush_dcache_range(ARM_DRAM1_BASE, ARM_DRAM1_SIZE);
+
+	INFO("Zeroing DDR memory range 0x%llx - 0x%llx\n",
+		ARM_DRAM2_BASE, ARM_DRAM2_BASE + dram2_size);
+	zero_normalmem((void *)ARM_DRAM2_BASE, dram2_size);
+	flush_dcache_range(ARM_DRAM2_BASE, dram2_size);
+
+	/* Clear previous ECC errors while zeroing out the memory */
+	val = mmio_read_32(MORELLO_DMC0_ERR2STATUS_REG);
+	mmio_write_32(MORELLO_DMC0_ERR2STATUS_REG, val);
+
+	val = mmio_read_32(MORELLO_DMC1_ERR2STATUS_REG);
+	mmio_write_32(MORELLO_DMC1_ERR2STATUS_REG, val);
+
+	/* Set DMCs to CONFIG state before writing ERR0CTLR0 register */
+	mmio_write_32(MORELLO_DMC0_MEMC_CMD_REG, MORELLO_DMC_MEMC_CMD_CONFIG);
+	mmio_write_32(MORELLO_DMC1_MEMC_CMD_REG, MORELLO_DMC_MEMC_CMD_CONFIG);
+
+	while ((mmio_read_32(MORELLO_DMC0_MEMC_STATUS_REG) &
+			MORELLO_DMC_MEMC_STATUS_MASK) !=
+			MORELLO_DMC_MEMC_CMD_CONFIG) {
+		continue;
+	}
+
+	while ((mmio_read_32(MORELLO_DMC1_MEMC_STATUS_REG) &
+			MORELLO_DMC_MEMC_STATUS_MASK) !=
+			MORELLO_DMC_MEMC_CMD_CONFIG) {
+		continue;
+	}
+
+	/* Configure Bing client/server mode based on SCC configuration */
+	if (plat_info->scc_config & MORELLO_SCC_CLIENT_MODE_MASK) {
+		INFO("Configuring DMC Bing in client mode\n");
+		usable_mem_size = plat_info->local_ddr_size -
+			(plat_info->local_ddr_size / 128ULL);
+
+		/* Linear DDR address */
+		tag_mem_base = usable_mem_size;
+		tag_mem_base = tag_mem_base / 4;
+
+		/* Reverse translation */
+		if (tag_mem_base < ARM_DRAM1_BASE) {
+			tag_mem_base += ARM_DRAM1_BASE;
+		} else {
+			tag_mem_base = tag_mem_base - ARM_DRAM1_BASE +
+				ARM_DRAM2_BASE;
+		}
+
+		mmio_write_32(MORELLO_DMC0_CAP_CTRL_REG, 0x1);
+		mmio_write_32(MORELLO_DMC1_CAP_CTRL_REG, 0x1);
+		mmio_write_32(MORELLO_DMC0_TAG_CACHE_CFG, 0x1);
+		mmio_write_32(MORELLO_DMC1_TAG_CACHE_CFG, 0x1);
+
+		if (plat_info->scc_config & MORELLO_SCC_C1_TAG_CACHE_EN_MASK) {
+			mmio_setbits_32(MORELLO_DMC0_TAG_CACHE_CFG, 0x2);
+			mmio_setbits_32(MORELLO_DMC1_TAG_CACHE_CFG, 0x2);
+			INFO("C1 Tag Cache Enabled\n");
+		}
+
+		if (plat_info->scc_config & MORELLO_SCC_C2_TAG_CACHE_EN_MASK) {
+			mmio_setbits_32(MORELLO_DMC0_TAG_CACHE_CFG, 0x4);
+			mmio_setbits_32(MORELLO_DMC1_TAG_CACHE_CFG, 0x4);
+			INFO("C2 Tag Cache Enabled\n");
+		}
+
+		mmio_write_32(MORELLO_DMC0_MEM_ADDR_CTL,
+				(uint32_t)tag_mem_base);
+		mmio_write_32(MORELLO_DMC1_MEM_ADDR_CTL,
+				(uint32_t)tag_mem_base);
+		mmio_write_32(MORELLO_DMC0_MEM_ADDR_CTL2,
+				(uint32_t)(tag_mem_base >> 32));
+		mmio_write_32(MORELLO_DMC1_MEM_ADDR_CTL2,
+				(uint32_t)(tag_mem_base >> 32));
+
+		mmio_setbits_32(MORELLO_DMC0_MEM_ACCESS_CTL,
+				MORELLO_DMC_MEM_ACCESS_DIS);
+		mmio_setbits_32(MORELLO_DMC1_MEM_ACCESS_CTL,
+				MORELLO_DMC_MEM_ACCESS_DIS);
+
+		INFO("Tag base set to 0x%lx\n", tag_mem_base);
+		plat_info->local_ddr_size = usable_mem_size;
+	} else {
+		INFO("Configuring DMC Bing in server mode\n");
+		mmio_write_32(MORELLO_DMC0_CAP_CTRL_REG, 0x0);
+		mmio_write_32(MORELLO_DMC1_CAP_CTRL_REG, 0x0);
+	}
+
+	INFO("Enabling ECC on DMCs\n");
+	/* Enable ECC in DMCs */
+	mmio_setbits_32(MORELLO_DMC0_ERR0CTLR0_REG,
+		MORELLO_DMC_ERR0CTLR0_ECC_EN);
+	mmio_setbits_32(MORELLO_DMC1_ERR0CTLR0_REG,
+		MORELLO_DMC_ERR0CTLR0_ECC_EN);
+
+	/* Set DMCs to READY state */
+	mmio_write_32(MORELLO_DMC0_MEMC_CMD_REG, MORELLO_DMC_MEMC_CMD_READY);
+	mmio_write_32(MORELLO_DMC1_MEMC_CMD_REG, MORELLO_DMC_MEMC_CMD_READY);
+
+	while ((mmio_read_32(MORELLO_DMC0_MEMC_STATUS_REG) &
+			MORELLO_DMC_MEMC_STATUS_MASK) !=
+			MORELLO_DMC_MEMC_CMD_READY) {
+		continue;
+	}
+
+	while ((mmio_read_32(MORELLO_DMC1_MEMC_STATUS_REG) &
+			MORELLO_DMC_MEMC_STATUS_MASK) !=
+			MORELLO_DMC_MEMC_CMD_READY) {
+		continue;
+	}
+}
+#endif
+
+void bl2_platform_setup(void)
+{
+	int ret;
+	struct morello_plat_info plat_info;
+
+	ret = sds_init();
+	if (ret != SDS_OK) {
+		ERROR("SDS initialization failed. ret:%d\n", ret);
+		panic();
+	}
+
+	ret = sds_struct_read(MORELLO_SDS_PLATFORM_INFO_STRUCT_ID,
+				MORELLO_SDS_PLATFORM_INFO_OFFSET,
+				&plat_info,
+				MORELLO_SDS_PLATFORM_INFO_SIZE,
+				SDS_ACCESS_MODE_NON_CACHED);
+	if (ret != SDS_OK) {
+		ERROR("Error getting platform info from SDS. ret:%d\n", ret);
+		panic();
+	}
+
+	/* Validate plat_info SDS */
+#ifdef TARGET_PLATFORM_FVP
+	if (plat_info.local_ddr_size == 0U) {
+#else
+	if ((plat_info.local_ddr_size == 0U)
+		|| (plat_info.local_ddr_size > MORELLO_MAX_DDR_CAPACITY)
+		|| (plat_info.remote_ddr_size > MORELLO_MAX_DDR_CAPACITY)
+		|| (plat_info.remote_chip_count > MORELLO_MAX_REMOTE_CHIP_COUNT)
+		) {
+#endif
+		ERROR("platform info SDS is corrupted\n");
+		panic();
+	}
+
+#ifdef TARGET_PLATFORM_SOC
+	dmc_ecc_setup(&plat_info);
 #endif
 	arm_bl2_platform_setup();
 }

diff --git a/plat/arm/board/morello/morello_bl31_setup.c b/plat/arm/board/morello/morello_bl31_setup.c
index e418518..a044212 100644
--- a/plat/arm/board/morello/morello_bl31_setup.c
+++ b/plat/arm/board/morello/morello_bl31_setup.c

@@ -1,54 +1,16 @@
 /*
- * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <common/debug.h>
 #include <drivers/arm/css/css_mhu_doorbell.h>
 #include <drivers/arm/css/scmi.h>
-#include <drivers/arm/css/sds.h>
-#include <lib/cassert.h>
-#include <lib/utils.h>
 #include <plat/arm/common/plat_arm.h>
 
 #include "morello_def.h"
 #include <platform_def.h>
 
-#ifdef TARGET_PLATFORM_FVP
-/*
- * Platform information structure stored in SDS.
- * This structure holds information about platform's DDR
- * size
- *	- Local DDR size in bytes, DDR memory in main board
- */
-struct morello_plat_info {
-	uint64_t local_ddr_size;
-} __packed;
-#else
-/*
- * Platform information structure stored in SDS.
- * This structure holds information about platform's DDR
- * size which is an information about multichip setup
- *	- Local DDR size in bytes, DDR memory in main board
- *	- Remote DDR size in bytes, DDR memory in remote board
- *	- remote_chip_count
- *	- multichip mode
- *	- scc configuration
- */
-struct morello_plat_info {
-	uint64_t local_ddr_size;
-	uint64_t remote_ddr_size;
-	uint8_t remote_chip_count;
-	bool multichip_mode;
-	uint32_t scc_config;
-} __packed;
-#endif
-
-/* Compile time assertion to ensure the size of structure is 18 bytes */
-CASSERT(sizeof(struct morello_plat_info) == MORELLO_SDS_PLATFORM_INFO_SIZE,
-		assert_invalid_plat_info_size);
-
 static scmi_channel_plat_info_t morello_scmi_plat_info = {
 	.scmi_mbx_mem = MORELLO_SCMI_PAYLOAD_BASE,
 	.db_reg_addr = PLAT_CSS_MHU_BASE + CSS_SCMI_MHU_DB_REG_OFF,
@@ -67,177 +29,7 @@
 	return css_scmi_override_pm_ops(ops);
 }
 
-#ifdef TARGET_PLATFORM_SOC
-/*
- * Morello platform supports RDIMMs with ECC capability. To use the ECC
- * capability, the entire DDR memory space has to be zeroed out before
- * enabling the ECC bits in DMC-Bing. Zeroing out several gigabytes of
- * memory from SCP is quite time consuming so the following function
- * is added to zero out the DDR memory from application processor which is
- * much faster compared to SCP.
- */
-
-static void dmc_ecc_setup(struct morello_plat_info *plat_info)
-{
-	uint64_t dram2_size;
-	uint32_t val;
-	uint64_t tag_mem_base;
-	uint64_t usable_mem_size;
-
-	INFO("Total DIMM size: %uGB\n",
-			(uint32_t)(plat_info->local_ddr_size / 0x40000000));
-
-	assert(plat_info->local_ddr_size > ARM_DRAM1_SIZE);
-	dram2_size = plat_info->local_ddr_size - ARM_DRAM1_SIZE;
-
-	INFO("Zeroing DDR memory range 0x%llx - 0x%llx\n",
-		ARM_DRAM2_BASE, ARM_DRAM2_BASE + dram2_size);
-	zero_normalmem((void *)ARM_DRAM2_BASE, dram2_size);
-	flush_dcache_range(ARM_DRAM2_BASE, dram2_size);
-
-	/* Clear previous ECC errors while zeroing out the memory */
-	val = mmio_read_32(MORELLO_DMC0_ERR2STATUS_REG);
-	mmio_write_32(MORELLO_DMC0_ERR2STATUS_REG, val);
-
-	val = mmio_read_32(MORELLO_DMC1_ERR2STATUS_REG);
-	mmio_write_32(MORELLO_DMC1_ERR2STATUS_REG, val);
-
-	/* Set DMCs to CONFIG state before writing ERR0CTLR0 register */
-	mmio_write_32(MORELLO_DMC0_MEMC_CMD_REG, MORELLO_DMC_MEMC_CMD_CONFIG);
-	mmio_write_32(MORELLO_DMC1_MEMC_CMD_REG, MORELLO_DMC_MEMC_CMD_CONFIG);
-
-	while ((mmio_read_32(MORELLO_DMC0_MEMC_STATUS_REG) &
-			MORELLO_DMC_MEMC_STATUS_MASK) !=
-			MORELLO_DMC_MEMC_CMD_CONFIG) {
-		continue;
-	}
-
-	while ((mmio_read_32(MORELLO_DMC1_MEMC_STATUS_REG) &
-			MORELLO_DMC_MEMC_STATUS_MASK) !=
-			MORELLO_DMC_MEMC_CMD_CONFIG) {
-		continue;
-	}
-
-	/* Configure Bing client/server mode based on SCC configuration */
-	if (plat_info->scc_config & MORELLO_SCC_CLIENT_MODE_MASK) {
-		INFO("Configuring DMC Bing in client mode\n");
-		usable_mem_size = plat_info->local_ddr_size -
-			(plat_info->local_ddr_size / 128ULL);
-
-		/* Linear DDR address */
-		tag_mem_base = usable_mem_size;
-		tag_mem_base = tag_mem_base / 4;
-
-		/* Reverse translation */
-		if (tag_mem_base < ARM_DRAM1_BASE) {
-			tag_mem_base += ARM_DRAM1_BASE;
-		} else {
-			tag_mem_base = tag_mem_base - ARM_DRAM1_BASE +
-				ARM_DRAM2_BASE;
-		}
-
-		mmio_write_32(MORELLO_DMC0_CAP_CTRL_REG, 0x1);
-		mmio_write_32(MORELLO_DMC1_CAP_CTRL_REG, 0x1);
-		mmio_write_32(MORELLO_DMC0_TAG_CACHE_CFG, 0x1);
-		mmio_write_32(MORELLO_DMC1_TAG_CACHE_CFG, 0x1);
-
-		if (plat_info->scc_config & MORELLO_SCC_C1_TAG_CACHE_EN_MASK) {
-			mmio_setbits_32(MORELLO_DMC0_TAG_CACHE_CFG, 0x2);
-			mmio_setbits_32(MORELLO_DMC1_TAG_CACHE_CFG, 0x2);
-			INFO("C1 Tag Cache Enabled\n");
-		}
-
-		if (plat_info->scc_config & MORELLO_SCC_C2_TAG_CACHE_EN_MASK) {
-			mmio_setbits_32(MORELLO_DMC0_TAG_CACHE_CFG, 0x4);
-			mmio_setbits_32(MORELLO_DMC1_TAG_CACHE_CFG, 0x4);
-			INFO("C2 Tag Cache Enabled\n");
-		}
-
-		mmio_write_32(MORELLO_DMC0_MEM_ADDR_CTL,
-				(uint32_t)tag_mem_base);
-		mmio_write_32(MORELLO_DMC1_MEM_ADDR_CTL,
-				(uint32_t)tag_mem_base);
-		mmio_write_32(MORELLO_DMC0_MEM_ADDR_CTL2,
-				(uint32_t)(tag_mem_base >> 32));
-		mmio_write_32(MORELLO_DMC1_MEM_ADDR_CTL2,
-				(uint32_t)(tag_mem_base >> 32));
-
-		mmio_setbits_32(MORELLO_DMC0_MEM_ACCESS_CTL,
-				MORELLO_DMC_MEM_ACCESS_DIS);
-		mmio_setbits_32(MORELLO_DMC1_MEM_ACCESS_CTL,
-				MORELLO_DMC_MEM_ACCESS_DIS);
-
-		INFO("Tag base set to 0x%lx\n", tag_mem_base);
-		plat_info->local_ddr_size = usable_mem_size;
-	} else {
-		INFO("Configuring DMC Bing in server mode\n");
-		mmio_write_32(MORELLO_DMC0_CAP_CTRL_REG, 0x0);
-		mmio_write_32(MORELLO_DMC1_CAP_CTRL_REG, 0x0);
-	}
-
-	INFO("Enabling ECC on DMCs\n");
-	/* Enable ECC in DMCs */
-	mmio_setbits_32(MORELLO_DMC0_ERR0CTLR0_REG,
-		MORELLO_DMC_ERR0CTLR0_ECC_EN);
-	mmio_setbits_32(MORELLO_DMC1_ERR0CTLR0_REG,
-		MORELLO_DMC_ERR0CTLR0_ECC_EN);
-
-	/* Set DMCs to READY state */
-	mmio_write_32(MORELLO_DMC0_MEMC_CMD_REG, MORELLO_DMC_MEMC_CMD_READY);
-	mmio_write_32(MORELLO_DMC1_MEMC_CMD_REG, MORELLO_DMC_MEMC_CMD_READY);
-
-	while ((mmio_read_32(MORELLO_DMC0_MEMC_STATUS_REG) &
-			MORELLO_DMC_MEMC_STATUS_MASK) !=
-			MORELLO_DMC_MEMC_CMD_READY) {
-		continue;
-	}
-
-	while ((mmio_read_32(MORELLO_DMC1_MEMC_STATUS_REG) &
-			MORELLO_DMC_MEMC_STATUS_MASK) !=
-			MORELLO_DMC_MEMC_CMD_READY) {
-		continue;
-	}
-}
-#endif
-
 void bl31_platform_setup(void)
 {
-	int ret;
-	struct morello_plat_info plat_info;
-
-	ret = sds_init();
-	if (ret != SDS_OK) {
-		ERROR("SDS initialization failed. ret:%d\n", ret);
-		panic();
-	}
-
-	ret = sds_struct_read(MORELLO_SDS_PLATFORM_INFO_STRUCT_ID,
-				MORELLO_SDS_PLATFORM_INFO_OFFSET,
-				&plat_info,
-				MORELLO_SDS_PLATFORM_INFO_SIZE,
-				SDS_ACCESS_MODE_NON_CACHED);
-	if (ret != SDS_OK) {
-		ERROR("Error getting platform info from SDS. ret:%d\n", ret);
-		panic();
-	}
-
-	/* Validate plat_info SDS */
-#ifdef TARGET_PLATFORM_FVP
-	if (plat_info.local_ddr_size == 0U) {
-#else
-	if ((plat_info.local_ddr_size == 0U)
-		|| (plat_info.local_ddr_size > MORELLO_MAX_DDR_CAPACITY)
-		|| (plat_info.remote_ddr_size > MORELLO_MAX_DDR_CAPACITY)
-		|| (plat_info.remote_chip_count > MORELLO_MAX_REMOTE_CHIP_COUNT)
-		) {
-#endif
-		ERROR("platform info SDS is corrupted\n");
-		panic();
-	}
-
 	arm_bl31_platform_setup();
-
-#ifdef TARGET_PLATFORM_SOC
-	dmc_ecc_setup(&plat_info);
-#endif
 }

diff --git a/plat/arm/board/morello/morello_plat.c b/plat/arm/board/morello/morello_plat.c
index 42e5171..1da0ff9 100644
--- a/plat/arm/board/morello/morello_plat.c
+++ b/plat/arm/board/morello/morello_plat.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -15,7 +15,7 @@
  * Table of regions to map using the MMU.
  * Replace or extend the below regions as required
  */
-#if IMAGE_BL1 || IMAGE_BL31
+#if IMAGE_BL1
 const mmap_region_t plat_arm_mmap[] = {
 	ARM_MAP_SHARED_RAM,
 	MORELLO_MAP_DEVICE,
@@ -25,12 +25,23 @@
 	{0}
 };
 #endif
+
+#if IMAGE_BL31
+const mmap_region_t plat_arm_mmap[] = {
+	ARM_MAP_SHARED_RAM,
+	MORELLO_MAP_DEVICE,
+	MORELLO_MAP_NS_SRAM,
+	{0}
+};
+#endif
+
 #if IMAGE_BL2
 const mmap_region_t plat_arm_mmap[] = {
 	ARM_MAP_SHARED_RAM,
 	MORELLO_MAP_DEVICE,
 	MORELLO_MAP_NS_SRAM,
 	ARM_MAP_DRAM1,
+	ARM_MAP_DRAM2,
 #if TRUSTED_BOARD_BOOT && !BL2_AT_EL3
 	ARM_MAP_BL1_RW,
 #endif

diff --git a/plat/arm/board/morello/platform.mk b/plat/arm/board/morello/platform.mk
index 86047e3..156b7ea 100644
--- a/plat/arm/board/morello/platform.mk
+++ b/plat/arm/board/morello/platform.mk

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2020-2021, Arm Limited. All rights reserved.
+# Copyright (c) 2020-2022, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -83,6 +83,8 @@
 
 override ARM_PLAT_MT			:=	1
 
+override ARM_BL31_IN_DRAM		:=	1
+
 # Errata workarounds:
 ERRATA_N1_1868343			:=	1
 

diff --git a/plat/arm/board/sgm775/fdts/sgm775_fw_config.dts b/plat/arm/board/n1sdp/fdts/n1sdp_fw_config.dts
similarity index 62%
rename from plat/arm/board/sgm775/fdts/sgm775_fw_config.dts
rename to plat/arm/board/n1sdp/fdts/n1sdp_fw_config.dts
index 5d478e9..f61e30b 100644
--- a/plat/arm/board/sgm775/fdts/sgm775_fw_config.dts
+++ b/plat/arm/board/n1sdp/fdts/n1sdp_fw_config.dts

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,15 +7,19 @@
 #include <common/tbbr/tbbr_img_def.h>
 
 /dts-v1/;
-
 / {
 	dtb-registry {
 		compatible = "fconf,dyn_cfg-dtb_registry";
-
 		tb_fw-config {
 			load-address = <0x0 0x4001300>;
 			max-size = <0x200>;
 			id = <TB_FW_CONFIG_ID>;
 		};
+
+		nt_fw-config {
+			load-address = <0x0 0xFEF00000>;
+			max-size = <0x0100000>;
+			id = <NT_FW_CONFIG_ID>;
+		};
 	};
 };

diff --git a/plat/arm/board/n1sdp/fdts/n1sdp_nt_fw_config.dts b/plat/arm/board/n1sdp/fdts/n1sdp_nt_fw_config.dts
new file mode 100644
index 0000000..da5e04d
--- /dev/null
+++ b/plat/arm/board/n1sdp/fdts/n1sdp_nt_fw_config.dts

@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/dts-v1/;
+/ {
+	/* compatible string */
+	compatible = "arm,n1sdp";
+
+	/*
+	 * Place holder for platform-info node with default values.
+	 * The values will be set to the correct values during
+	 * the BL2 stage of boot.
+	 */
+	platform-info {
+		multichip-mode = <0x0>;
+		secondary-chip-count = <0x0>;
+		local-ddr-size = <0x0>;
+		remote-ddr-size = <0x0>;
+	};
+};
\ No newline at end of file

diff --git a/plat/arm/board/sgm775/fdts/sgm775_tb_fw_config.dts b/plat/arm/board/n1sdp/fdts/n1sdp_tb_fw_config.dts
similarity index 89%
rename from plat/arm/board/sgm775/fdts/sgm775_tb_fw_config.dts
rename to plat/arm/board/n1sdp/fdts/n1sdp_tb_fw_config.dts
index 49eda27..e5ffba3 100644
--- a/plat/arm/board/sgm775/fdts/sgm775_tb_fw_config.dts
+++ b/plat/arm/board/n1sdp/fdts/n1sdp_tb_fw_config.dts

@@ -1,11 +1,10 @@
 /*
- * Copyright (c) 2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 /dts-v1/;
-
 / {
 	tb_fw-config {
 		compatible = "arm,tb_fw";

diff --git a/plat/arm/board/n1sdp/include/platform_def.h b/plat/arm/board/n1sdp/include/platform_def.h
index cc07852..c9b81ba 100644
--- a/plat/arm/board/n1sdp/include/platform_def.h
+++ b/plat/arm/board/n1sdp/include/platform_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -27,6 +27,27 @@
 #define PLAT_ARM_DRAM2_BASE			ULL(0x8080000000)
 #define PLAT_ARM_DRAM2_SIZE			ULL(0xF80000000)
 
+#define MAX_IO_DEVICES			U(3)
+#define MAX_IO_HANDLES			U(4)
+
+#define PLAT_ARM_FLASH_IMAGE_BASE			0x18200000
+#define PLAT_ARM_FLASH_IMAGE_MAX_SIZE			0x00800000
+
+#define PLAT_ARM_NVM_BASE			0x18200000
+#define PLAT_ARM_NVM_SIZE			0x00800000
+
+#if defined NS_BL1U_BASE
+# undef NS_BL1U_BASE
+# define NS_BL1U_BASE			(PLAT_ARM_NVM_BASE + UL(0x00800000))
+#endif
+
+/* Non-volatile counters */
+#define SOC_TRUSTED_NVCTR_BASE		0x7fe70000
+#define TFW_NVCTR_BASE			(SOC_TRUSTED_NVCTR_BASE)
+#define TFW_NVCTR_SIZE			U(4)
+#define NTFW_CTR_BASE			(SOC_TRUSTED_NVCTR_BASE + 0x0004)
+#define NTFW_CTR_SIZE			U(4)
+
 /* N1SDP remote chip at 4 TB offset */
 #define PLAT_ARM_REMOTE_CHIP_OFFSET		(ULL(1) << 42)
 
@@ -59,8 +80,42 @@
 #define PLAT_CSS_SCP_COM_SHARED_MEM_BASE	0x45400000
 #endif
 
+/*
+ * Trusted SRAM in N1SDP is 512 KB but only the bottom 384 KB
+ * is used for trusted board boot flow. The top 128 KB is used
+ * to load AP-BL1 image.
+ */
+#define PLAT_ARM_TRUSTED_SRAM_SIZE                      0x00060000      /* 384 KB */
+
-#define PLAT_ARM_TRUSTED_SRAM_SIZE		0x00080000	/* 512 KB */
-#define PLAT_ARM_MAX_BL31_SIZE			0X20000
+/*
+ * PLAT_ARM_MAX_BL1_RW_SIZE is calculated using the current BL1 RW debug size
+ * plus a little space for growth.
+ */
+#define PLAT_ARM_MAX_BL1_RW_SIZE	0xE000
+
+/*
+ * PLAT_ARM_MAX_ROMLIB_RW_SIZE is define to use a full page
+ */
+
+#if USE_ROMLIB
+# define PLAT_ARM_MAX_ROMLIB_RW_SIZE	0x1000
+# define PLAT_ARM_MAX_ROMLIB_RO_SIZE	0xe000
+#else
+# define PLAT_ARM_MAX_ROMLIB_RW_SIZE	U(0)
+# define PLAT_ARM_MAX_ROMLIB_RO_SIZE	U(0)
+#endif
+
+/*
+ * PLAT_ARM_MAX_BL2_SIZE is calculated using the current BL2 debug size plus a
+ * little space for growth.
+ */
+#if TRUSTED_BOARD_BOOT
+# define PLAT_ARM_MAX_BL2_SIZE		0x20000
+#else
+# define PLAT_ARM_MAX_BL2_SIZE		0x14000
+#endif
+
+#define PLAT_ARM_MAX_BL31_SIZE		UL(0x3B000)
 
 /*******************************************************************************
  * N1SDP topology related constants
@@ -83,10 +138,48 @@
  * PLAT_ARM_MMAP_ENTRIES depends on the number of entries in the
  * plat_arm_mmap array defined for each BL stage.
  */
-#define PLAT_ARM_MMAP_ENTRIES			9
-#define MAX_XLAT_TABLES				10
+
+#ifdef IMAGE_BL1
+# define PLAT_ARM_MMAP_ENTRIES		U(6)
+# define MAX_XLAT_TABLES		U(5)
+#endif
 
-#define PLATFORM_STACK_SIZE			0x400
+#ifdef IMAGE_BL2
+#  define PLAT_ARM_MMAP_ENTRIES		U(11)
+#  define MAX_XLAT_TABLES		U(10)
+#endif
+
+#ifdef IMAGE_BL31
+#  define PLAT_ARM_MMAP_ENTRIES		U(12)
+#  define MAX_XLAT_TABLES		U(12)
+#endif
+
+/*
+ * Size of cacheable stacks
+ */
+#if defined(IMAGE_BL1)
+# if TRUSTED_BOARD_BOOT
+#  define PLATFORM_STACK_SIZE	0x1000
+# else
+#  define PLATFORM_STACK_SIZE	0x440
+# endif
+#elif defined(IMAGE_BL2)
+# if TRUSTED_BOARD_BOOT
+#  define PLATFORM_STACK_SIZE	0x1000
+# else
+#  define PLATFORM_STACK_SIZE	0x400
+# endif
+#elif defined(IMAGE_BL2U)
+# define PLATFORM_STACK_SIZE	0x400
+#elif defined(IMAGE_BL31)
+# if SPM_MM
+#  define PLATFORM_STACK_SIZE	0x500
+# else
+#  define PLATFORM_STACK_SIZE	0x400
+# endif
+#elif defined(IMAGE_BL32)
+# define PLATFORM_STACK_SIZE	0x440
+#endif
 
 #define PLAT_ARM_NSTIMER_FRAME_ID		0
 #define PLAT_CSS_MHU_BASE			0x45000000
@@ -106,6 +199,10 @@
 						PLAT_ARM_REMOTE_CHIP_OFFSET
 #define N1SDP_REMOTE_DEVICE_SIZE		N1SDP_DEVICE_SIZE
 
+/* Real base is 0x0. Changed to load BL1 at this address */
+# define PLAT_ARM_TRUSTED_ROM_BASE	0x04060000
+# define PLAT_ARM_TRUSTED_ROM_SIZE	0x00020000	/* 128KB */
+
 #define N1SDP_MAP_DEVICE		MAP_REGION_FLAT(	\
 					N1SDP_DEVICE_BASE,	\
 					N1SDP_DEVICE_SIZE,	\

diff --git a/plat/arm/board/n1sdp/n1sdp_bl1_setup.c b/plat/arm/board/n1sdp/n1sdp_bl1_setup.c
new file mode 100644
index 0000000..ed93222
--- /dev/null
+++ b/plat/arm/board/n1sdp/n1sdp_bl1_setup.c

@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <plat/arm/common/plat_arm.h>
+
+/*******************************************************************************
+ * Perform any BL1 specific platform actions.
+ ******************************************************************************/
+
+void soc_css_init_nic400(void)
+{
+}
+
+void soc_css_init_pcie(void)
+{
+}

diff --git a/plat/arm/board/n1sdp/n1sdp_bl2_setup.c b/plat/arm/board/n1sdp/n1sdp_bl2_setup.c
new file mode 100644
index 0000000..5f8af9f
--- /dev/null
+++ b/plat/arm/board/n1sdp/n1sdp_bl2_setup.c

@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <drivers/arm/css/sds.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+
+#include "n1sdp_def.h"
+#include <plat/arm/common/plat_arm.h>
+
+struct n1sdp_plat_info {
+	bool multichip_mode;
+	uint8_t secondary_count;
+	uint8_t local_ddr_size;
+	uint8_t remote_ddr_size;
+} __packed;
+
+/*
+ * N1SDP platform supports RDIMMs with ECC capability. To use the ECC
+ * capability, the entire DDR memory space has to be zeroed out before
+ * enabling the ECC bits in DMC620. Zeroing out several gigabytes of
+ * memory from SCP is quite time consuming so the following function
+ * is added to zero out the DDR memory from application processor which is
+ * much faster compared to SCP.
+ */
+
+void dmc_ecc_setup(uint8_t ddr_size_gb)
+{
+	uint64_t dram2_size;
+
+	dram2_size = (ddr_size_gb * 1024UL * 1024UL * 1024UL) -
+			ARM_DRAM1_SIZE;
+
+	INFO("Zeroing DDR memories\n");
+	zero_normalmem((void *)ARM_DRAM1_BASE, ARM_DRAM1_SIZE);
+	flush_dcache_range(ARM_DRAM1_BASE, ARM_DRAM1_SIZE);
+	zero_normalmem((void *)ARM_DRAM2_BASE, dram2_size);
+	flush_dcache_range(ARM_DRAM2_BASE, dram2_size);
+
+	INFO("Enabling ECC on DMCs\n");
+	/* Set DMCs to CONFIG state before writing ERR0CTLR0 register */
+	mmio_write_32(N1SDP_DMC0_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_CONFIG);
+	mmio_write_32(N1SDP_DMC1_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_CONFIG);
+
+	/* Enable ECC in DMCs */
+	mmio_setbits_32(N1SDP_DMC0_ERR0CTLR0_REG, N1SDP_DMC_ERR0CTLR0_ECC_EN);
+	mmio_setbits_32(N1SDP_DMC1_ERR0CTLR0_REG, N1SDP_DMC_ERR0CTLR0_ECC_EN);
+
+	/* Set DMCs to READY state */
+	mmio_write_32(N1SDP_DMC0_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_READY);
+	mmio_write_32(N1SDP_DMC1_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_READY);
+}
+
+void bl2_platform_setup(void)
+{
+	int ret;
+	struct n1sdp_plat_info plat_info;
+
+	ret = sds_init();
+	if (ret != SDS_OK) {
+		ERROR("SDS initialization failed\n");
+		panic();
+	}
+
+	ret = sds_struct_read(N1SDP_SDS_PLATFORM_INFO_STRUCT_ID,
+				N1SDP_SDS_PLATFORM_INFO_OFFSET,
+				&plat_info,
+				N1SDP_SDS_PLATFORM_INFO_SIZE,
+				SDS_ACCESS_MODE_NON_CACHED);
+	if (ret != SDS_OK) {
+		ERROR("Error getting platform info from SDS\n");
+		panic();
+	}
+	/* Validate plat_info SDS */
+	if ((plat_info.local_ddr_size == 0)
+		|| (plat_info.local_ddr_size > N1SDP_MAX_DDR_CAPACITY_GB)
+		|| (plat_info.remote_ddr_size > N1SDP_MAX_DDR_CAPACITY_GB)
+		|| (plat_info.secondary_count > N1SDP_MAX_SECONDARY_COUNT)) {
+		ERROR("platform info SDS is corrupted\n");
+		panic();
+	}
+
+	dmc_ecc_setup(plat_info.local_ddr_size);
+	arm_bl2_platform_setup();
+}

diff --git a/plat/arm/board/n1sdp/n1sdp_bl31_setup.c b/plat/arm/board/n1sdp/n1sdp_bl31_setup.c
index d7003e9..5e897fe 100644
--- a/plat/arm/board/n1sdp/n1sdp_bl31_setup.c
+++ b/plat/arm/board/n1sdp/n1sdp_bl31_setup.c

@@ -1,11 +1,9 @@
 /*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <platform_def.h>
-
 #include <common/debug.h>
 #include <drivers/arm/css/css_mhu_doorbell.h>
 #include <drivers/arm/css/scmi.h>
@@ -16,6 +14,7 @@
 #include <plat/arm/common/plat_arm.h>
 
 #include "n1sdp_def.h"
+#include <platform_def.h>
 
 /*
  * Platform information structure stored in SDS.
@@ -24,28 +23,17 @@
  * enabling the ECC capability as well as information
  * about multichip setup
  * 	- multichip mode
- * 	- slave_count
+ * 	- secondary_count
  * 	- Local DDR size in GB, DDR memory in master board
- * 	- Remote DDR size in GB, DDR memory in slave board
+ * 	- Remote DDR size in GB, DDR memory in secondary board
  */
 struct n1sdp_plat_info {
 	bool multichip_mode;
-	uint8_t slave_count;
+	uint8_t secondary_count;
 	uint8_t local_ddr_size;
 	uint8_t remote_ddr_size;
 } __packed;
 
-/*
- * BL33 image information structure stored in SDS.
- * This structure holds the source & destination addresses and
- * the size of the BL33 image which will be loaded by BL31.
- */
-struct n1sdp_bl33_info {
-	uint32_t bl33_src_addr;
-	uint32_t bl33_dst_addr;
-	uint32_t bl33_size;
-};
-
 static scmi_channel_plat_info_t n1sdp_scmi_plat_info = {
 	.scmi_mbx_mem = N1SDP_SCMI_PAYLOAD_BASE,
 	.db_reg_addr = PLAT_CSS_MHU_BASE + CSS_SCMI_MHU_DB_REG_OFF,
@@ -90,38 +78,10 @@
  * enabling the ECC bits in DMC620. Zeroing out several gigabytes of
  * memory from SCP is quite time consuming so the following function
  * is added to zero out the DDR memory from application processor which is
- * much faster compared to SCP. BL33 binary cannot be copied to DDR memory
- * before enabling ECC so copy_bl33 function is added to copy BL33 binary
- * from IOFPGA-DDR3 memory to main DDR4 memory.
+ * much faster compared to SCP. Local DDR memory is zeroed out during BL2
+ * stage. If remote chip is connected, it's DDR memory is zeroed out here.
  */
 
-void dmc_ecc_setup(uint8_t ddr_size_gb)
-{
-	uint64_t dram2_size;
-
-	dram2_size = (ddr_size_gb * 1024UL * 1024UL * 1024UL) -
-			ARM_DRAM1_SIZE;
-
-	INFO("Zeroing DDR memories\n");
-	zero_normalmem((void *)ARM_DRAM1_BASE, ARM_DRAM1_SIZE);
-	flush_dcache_range(ARM_DRAM1_BASE, ARM_DRAM1_SIZE);
-	zero_normalmem((void *)ARM_DRAM2_BASE, dram2_size);
-	flush_dcache_range(ARM_DRAM2_BASE, dram2_size);
-
-	INFO("Enabling ECC on DMCs\n");
-	/* Set DMCs to CONFIG state before writing ERR0CTLR0 register */
-	mmio_write_32(N1SDP_DMC0_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_CONFIG);
-	mmio_write_32(N1SDP_DMC1_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_CONFIG);
-
-	/* Enable ECC in DMCs */
-	mmio_setbits_32(N1SDP_DMC0_ERR0CTLR0_REG, N1SDP_DMC_ERR0CTLR0_ECC_EN);
-	mmio_setbits_32(N1SDP_DMC1_ERR0CTLR0_REG, N1SDP_DMC_ERR0CTLR0_ECC_EN);
-
-	/* Set DMCs to READY state */
-	mmio_write_32(N1SDP_DMC0_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_READY);
-	mmio_write_32(N1SDP_DMC1_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_READY);
-}
-
 void remote_dmc_ecc_setup(uint8_t remote_ddr_size)
 {
 	uint64_t remote_dram2_size;
@@ -154,22 +114,6 @@
 	mmio_write_32(N1SDP_REMOTE_DMC1_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_READY);
 }
 
-void copy_bl33(uint32_t src, uint32_t dst, uint32_t size)
-{
-	uint32_t i;
-
-	INFO("Copying BL33 to DDR memory\n");
-	for (i = 0; i < size; i = i + 8)
-		mmio_write_64((dst + i), mmio_read_64(src + i));
-
-	for (i = 0; i < size; i = i + 8) {
-		if (mmio_read_64(src + i) != mmio_read_64(dst + i)) {
-			ERROR("Copy failed!\n");
-			panic();
-		}
-	}
-}
-
 void n1sdp_bl31_multichip_setup(void)
 {
 	plat_arm_override_gicr_frames(n1sdp_multichip_gicr_frames);
@@ -180,7 +124,6 @@
 {
 	int ret;
 	struct n1sdp_plat_info plat_info;
-	struct n1sdp_bl33_info bl33_info;
 
 	ret = sds_init();
 	if (ret != SDS_OK) {
@@ -201,41 +144,18 @@
 	if ((plat_info.local_ddr_size == 0)
 		|| (plat_info.local_ddr_size > N1SDP_MAX_DDR_CAPACITY_GB)
 		|| (plat_info.remote_ddr_size > N1SDP_MAX_DDR_CAPACITY_GB)
-		|| (plat_info.slave_count > N1SDP_MAX_SLAVE_COUNT)) {
+		|| (plat_info.secondary_count > N1SDP_MAX_SECONDARY_COUNT)) {
 		ERROR("platform info SDS is corrupted\n");
 		panic();
 	}
 
 	if (plat_info.multichip_mode) {
-		n1sdp_multichip_data.chip_count = plat_info.slave_count + 1;
+		n1sdp_multichip_data.chip_count = plat_info.secondary_count + 1;
 		n1sdp_bl31_multichip_setup();
 	}
 	arm_bl31_platform_setup();
 
-	dmc_ecc_setup(plat_info.local_ddr_size);
-
 	/* Check if remote memory is present */
 	if ((plat_info.multichip_mode) && (plat_info.remote_ddr_size != 0))
 		remote_dmc_ecc_setup(plat_info.remote_ddr_size);
-
-	ret = sds_struct_read(N1SDP_SDS_BL33_INFO_STRUCT_ID,
-				N1SDP_SDS_BL33_INFO_OFFSET,
-				&bl33_info,
-				N1SDP_SDS_BL33_INFO_SIZE,
-				SDS_ACCESS_MODE_NON_CACHED);
-	if (ret != SDS_OK) {
-		ERROR("Error getting BL33 info from SDS\n");
-		panic();
-	}
-	copy_bl33(bl33_info.bl33_src_addr,
-			bl33_info.bl33_dst_addr,
-			bl33_info.bl33_size);
-	/*
-	 * Pass platform information to BL33. This method is followed as
-	 * currently there is no BL1/BL2 involved in boot flow of N1SDP.
-	 * When TBBR is implemented for N1SDP, this method should be removed
-	 * and platform information should be passed to BL33 using NT_FW_CONFIG
-	 * passing mechanism.
-	 */
-	mmio_write_32(N1SDP_PLATFORM_INFO_BASE, *(uint32_t *)&plat_info);
 }

diff --git a/plat/arm/board/n1sdp/n1sdp_def.h b/plat/arm/board/n1sdp/n1sdp_def.h
index 30e29a7..ffa6a03 100644
--- a/plat/arm/board/n1sdp/n1sdp_def.h
+++ b/plat/arm/board/n1sdp/n1sdp_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -20,12 +20,7 @@
 #define N1SDP_SDS_PLATFORM_INFO_OFFSET		0
 #define N1SDP_SDS_PLATFORM_INFO_SIZE		4
 #define N1SDP_MAX_DDR_CAPACITY_GB		64
-#define N1SDP_MAX_SLAVE_COUNT			16
-
-/* SDS BL33 image information defines */
-#define N1SDP_SDS_BL33_INFO_STRUCT_ID		9
-#define N1SDP_SDS_BL33_INFO_OFFSET		0
-#define N1SDP_SDS_BL33_INFO_SIZE		12
+#define N1SDP_MAX_SECONDARY_COUNT		16
 
 /* DMC memory command registers */
 #define N1SDP_DMC0_MEMC_CMD_REG			0x4E000008
@@ -54,7 +49,4 @@
 /* DMC ECC enable bit in ERR0CTLR0 register */
 #define N1SDP_DMC_ERR0CTLR0_ECC_EN		0x1
 
-/* Base address of non-secure SRAM where Platform information will be filled */
-#define N1SDP_PLATFORM_INFO_BASE		0x06008000
-
 #endif /* N1SDP_DEF_H */

diff --git a/plat/arm/board/sgm775/sgm775_err.c b/plat/arm/board/n1sdp/n1sdp_err.c
similarity index 66%
rename from plat/arm/board/sgm775/sgm775_err.c
rename to plat/arm/board/n1sdp/n1sdp_err.c
index dc114f0..629e76a 100644
--- a/plat/arm/board/sgm775/sgm775_err.c
+++ b/plat/arm/board/n1sdp/n1sdp_err.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,7 +7,7 @@
 #include <plat/arm/common/plat_arm.h>
 
 /*
- * sgm775 error handler
+ * n1sdp error handler
  */
 void __dead2 plat_arm_error_handler(int err)
 {

diff --git a/plat/arm/board/n1sdp/n1sdp_image_load.c b/plat/arm/board/n1sdp/n1sdp_image_load.c
new file mode 100644
index 0000000..6c3528c
--- /dev/null
+++ b/plat/arm/board/n1sdp/n1sdp_image_load.c

@@ -0,0 +1,143 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <common/desc_image_load.h>
+#include <drivers/arm/css/sds.h>
+#include <libfdt.h>
+#include <plat/common/platform.h>
+
+#include "n1sdp_def.h"
+#include <plat/arm/common/plat_arm.h>
+
+/*
+ * Platform information structure stored in SDS.
+ * This structure holds information about platform's DDR
+ * size which will be used to zero out the memory before
+ * enabling the ECC capability as well as information
+ * about multichip setup
+ * 	- multichip mode
+ * 	- secondary_count
+ * 	- Local DDR size in GB, DDR memory in master board
+ * 	- Remote DDR size in GB, DDR memory in secondary board
+ */
+struct n1sdp_plat_info {
+	bool multichip_mode;
+	uint8_t secondary_count;
+	uint8_t local_ddr_size;
+	uint8_t remote_ddr_size;
+} __packed;
+
+/*******************************************************************************
+ * This function inserts Platform information via device tree nodes as,
+ *	platform-info {
+ *		multichip-mode = <0x0>;
+ *		secondary-chip-count = <0x0>;
+ *		local-ddr-size = <0x0>;
+ *		remote-ddr-size = <0x0>;
+ *	};
+ ******************************************************************************/
+static int plat_n1sdp_append_config_node(struct n1sdp_plat_info *plat_info)
+{
+	bl_mem_params_node_t *mem_params;
+	void *fdt;
+	int nodeoffset, err;
+
+	mem_params = get_bl_mem_params_node(NT_FW_CONFIG_ID);
+	if (mem_params == NULL) {
+		ERROR("NT_FW CONFIG base address is NULL\n");
+		return -1;
+	}
+
+	fdt = (void *)(mem_params->image_info.image_base);
+
+	/* Check the validity of the fdt */
+	if (fdt_check_header(fdt) != 0) {
+		ERROR("Invalid NT_FW_CONFIG DTB passed\n");
+		return -1;
+	}
+
+	nodeoffset = fdt_subnode_offset(fdt, 0, "platform-info");
+	if (nodeoffset < 0) {
+		ERROR("NT_FW_CONFIG: Failed to get platform-info node offset\n");
+		return -1;
+	}
+
+	err = fdt_setprop_u32(fdt, nodeoffset, "multichip-mode",
+			plat_info->multichip_mode);
+	if (err < 0) {
+		ERROR("NT_FW_CONFIG: Failed to set multichip-mode\n");
+		return -1;
+	}
+
+	err = fdt_setprop_u32(fdt, nodeoffset, "secondary-chip-count",
+			plat_info->secondary_count);
+	if (err < 0) {
+		ERROR("NT_FW_CONFIG: Failed to set secondary-chip-count\n");
+		return -1;
+	}
+
+	err = fdt_setprop_u32(fdt, nodeoffset, "local-ddr-size",
+			plat_info->local_ddr_size);
+	if (err < 0) {
+		ERROR("NT_FW_CONFIG: Failed to set local-ddr-size\n");
+		return -1;
+	}
+
+	err = fdt_setprop_u32(fdt, nodeoffset, "remote-ddr-size",
+			plat_info->remote_ddr_size);
+	if (err < 0) {
+		ERROR("NT_FW_CONFIG: Failed to set remote-ddr-size\n");
+		return -1;
+	}
+
+	flush_dcache_range((uintptr_t)fdt, mem_params->image_info.image_size);
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function returns the list of executable images.
+ ******************************************************************************/
+bl_params_t *plat_get_next_bl_params(void)
+{
+	int ret;
+	struct n1sdp_plat_info plat_info;
+
+	ret = sds_init();
+	if (ret != SDS_OK) {
+		ERROR("SDS initialization failed. ret:%d\n", ret);
+		panic();
+	}
+
+	ret = sds_struct_read(N1SDP_SDS_PLATFORM_INFO_STRUCT_ID,
+				N1SDP_SDS_PLATFORM_INFO_OFFSET,
+				&plat_info,
+				N1SDP_SDS_PLATFORM_INFO_SIZE,
+				SDS_ACCESS_MODE_NON_CACHED);
+	if (ret != SDS_OK) {
+		ERROR("Error getting platform info from SDS. ret:%d\n", ret);
+		panic();
+	}
+
+	/* Validate plat_info SDS */
+	if ((plat_info.local_ddr_size == 0U)
+		|| (plat_info.local_ddr_size > N1SDP_MAX_DDR_CAPACITY_GB)
+		|| (plat_info.remote_ddr_size > N1SDP_MAX_DDR_CAPACITY_GB)
+		|| (plat_info.secondary_count > N1SDP_MAX_SECONDARY_COUNT)
+		){
+		ERROR("platform info SDS is corrupted\n");
+		panic();
+	}
+
+	ret = plat_n1sdp_append_config_node(&plat_info);
+	if (ret != 0) {
+		panic();
+	}
+
+	return arm_get_next_bl_params();
+}

diff --git a/plat/arm/board/n1sdp/n1sdp_plat.c b/plat/arm/board/n1sdp/n1sdp_plat.c
index 951a562..502268c 100644
--- a/plat/arm/board/n1sdp/n1sdp_plat.c
+++ b/plat/arm/board/n1sdp/n1sdp_plat.c

@@ -1,16 +1,13 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <platform_def.h>
+#include <assert.h>
 
-#include <common/bl_common.h>
-#include <common/debug.h>
-#include <plat/arm/common/plat_arm.h>
-#include <plat/common/platform.h>
 #include <drivers/arm/sbsa.h>
+#include <plat/arm/common/plat_arm.h>
 
 #include "n1sdp_def.h"
 
@@ -19,17 +16,51 @@
  * Replace or extend the below regions as required
  */
 
+#if IMAGE_BL1
+const mmap_region_t plat_arm_mmap[] = {
+	ARM_MAP_SHARED_RAM,
+	N1SDP_MAP_DEVICE,
+	N1SDP_MAP_NS_SRAM,
+	ARM_MAP_DRAM1,
+	{0}
+};
+#endif
+
+#if IMAGE_BL2
 const mmap_region_t plat_arm_mmap[] = {
 	ARM_MAP_SHARED_RAM,
 	N1SDP_MAP_DEVICE,
 	N1SDP_MAP_NS_SRAM,
 	ARM_MAP_DRAM1,
 	ARM_MAP_DRAM2,
+#if TRUSTED_BOARD_BOOT && !BL2_AT_EL3
+	ARM_MAP_BL1_RW,
+#endif
+	{0}
+};
+#endif
+
+#if IMAGE_BL31
+const mmap_region_t plat_arm_mmap[] = {
+	ARM_MAP_SHARED_RAM,
+	N1SDP_MAP_DEVICE,
+	N1SDP_MAP_NS_SRAM,
 	N1SDP_MAP_REMOTE_DEVICE,
 	N1SDP_MAP_REMOTE_DRAM1,
 	N1SDP_MAP_REMOTE_DRAM2,
 	{0}
 };
+#endif
+
+#if TRUSTED_BOARD_BOOT
+int plat_get_mbedtls_heap(void **heap_addr, size_t *heap_size)
+{
+	assert(heap_addr != NULL);
+	assert(heap_size != NULL);
+
+	return arm_get_mbedtls_heap(heap_addr, heap_size);
+}
+#endif
 
 void plat_arm_secure_wdt_start(void)
 {

diff --git a/plat/arm/board/n1sdp/n1sdp_trusted_boot.c b/plat/arm/board/n1sdp/n1sdp_trusted_boot.c
new file mode 100644
index 0000000..c7dc47f
--- /dev/null
+++ b/plat/arm/board/n1sdp/n1sdp_trusted_boot.c

@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <plat/arm/common/plat_arm.h>
+
+/*
+ * Return the non-volatile counter value stored in the platform. The cookie
+ * will contain the OID of the counter in the certificate.
+ *
+ * Return: 0 = success, Otherwise = error
+ */
+int plat_get_nv_ctr(void *cookie, unsigned int *nv_ctr)
+{
+	*nv_ctr = N1SDP_FW_NVCTR_VAL;
+	return 0;
+}
+
+/*
+ * Store a new non-volatile counter value. By default on ARM development
+ * platforms, the non-volatile counters are RO and cannot be modified. We expect
+ * the values in the certificates to always match the RO values so that this
+ * function is never called.
+ *
+ * Return: 0 = success, Otherwise = error
+ */
+int plat_set_nv_ctr(void *cookie, unsigned int nv_ctr)
+{
+	return 1;
+}
+
+/*
+ * Return the ROTPK hash in the following ASN.1 structure in DER format:
+ *
+ * AlgorithmIdentifier  ::=  SEQUENCE  {
+ *     algorithm         OBJECT IDENTIFIER,
+ *     parameters        ANY DEFINED BY algorithm OPTIONAL
+ * }
+ *
+ * DigestInfo ::= SEQUENCE {
+ *     digestAlgorithm   AlgorithmIdentifier,
+ *     digest            OCTET STRING
+ * }
+ */
+int plat_get_rotpk_info(void *cookie, void **key_ptr, unsigned int *key_len,
+			 unsigned int *flags)
+{
+	return arm_get_rotpk_info(cookie, key_ptr, key_len, flags);
+}
+

diff --git a/plat/arm/board/n1sdp/platform.mk b/plat/arm/board/n1sdp/platform.mk
index f20397a..740fb29 100644
--- a/plat/arm/board/n1sdp/platform.mk
+++ b/plat/arm/board/n1sdp/platform.mk

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2018-2022, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -28,30 +28,59 @@
 PLAT_BL_COMMON_SOURCES	:=	${N1SDP_BASE}/n1sdp_plat.c	        \
 				${N1SDP_BASE}/aarch64/n1sdp_helper.S
 
-BL1_SOURCES		+=	drivers/arm/sbsa/sbsa.c
+BL1_SOURCES		:=	${N1SDP_CPU_SOURCES}                \
+				${INTERCONNECT_SOURCES}             \
+				${N1SDP_BASE}/n1sdp_err.c           \
+				${N1SDP_BASE}/n1sdp_trusted_boot.c  \
+				${N1SDP_BASE}/n1sdp_bl1_setup.c     \
+				drivers/arm/sbsa/sbsa.c
+
+BL2_SOURCES		:=	${N1SDP_BASE}/n1sdp_security.c      \
+				${N1SDP_BASE}/n1sdp_err.c           \
+				${N1SDP_BASE}/n1sdp_trusted_boot.c  \
+				lib/utils/mem_region.c              \
+				${N1SDP_BASE}/n1sdp_bl2_setup.c     \
+				${N1SDP_BASE}/n1sdp_image_load.c     \
+				drivers/arm/css/sds/sds.c
 
 BL31_SOURCES		:=	${N1SDP_CPU_SOURCES}			\
 				${INTERCONNECT_SOURCES}			\
 				${N1SDP_GIC_SOURCES}			\
-				${N1SDP_BASE}/n1sdp_bl31_setup.c	        \
+				${N1SDP_BASE}/n1sdp_bl31_setup.c	\
 				${N1SDP_BASE}/n1sdp_topology.c	        \
 				${N1SDP_BASE}/n1sdp_security.c		\
 				drivers/arm/css/sds/sds.c
 
 FDT_SOURCES		+=	fdts/${PLAT}-single-chip.dts	\
-				fdts/${PLAT}-multi-chip.dts
+				fdts/${PLAT}-multi-chip.dts	\
+				${N1SDP_BASE}/fdts/n1sdp_fw_config.dts	\
+				${N1SDP_BASE}/fdts/n1sdp_tb_fw_config.dts	\
+				${N1SDP_BASE}/fdts/n1sdp_nt_fw_config.dts
+
+FW_CONFIG		:=	${BUILD_PLAT}/fdts/n1sdp_fw_config.dtb
+TB_FW_CONFIG		:=	${BUILD_PLAT}/fdts/n1sdp_tb_fw_config.dtb
+NT_FW_CONFIG		:=	${BUILD_PLAT}/fdts/n1sdp_nt_fw_config.dtb
+
+# Add the FW_CONFIG to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${FW_CONFIG},--fw-config,${FW_CONFIG}))
+# Add the TB_FW_CONFIG to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${TB_FW_CONFIG},--tb-fw-config,${TB_FW_CONFIG}))
+# Add the NT_FW_CONFIG to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${NT_FW_CONFIG},--nt-fw-config,${NT_FW_CONFIG}))
+
+# Setting to 0 as no NVCTR in N1SDP
+N1SDP_FW_NVCTR_VAL	:=	0
+TFW_NVCTR_VAL		:=	${N1SDP_FW_NVCTR_VAL}
+NTFW_NVCTR_VAL		:=	${N1SDP_FW_NVCTR_VAL}
+
+# Add N1SDP_FW_NVCTR_VAL
+$(eval $(call add_define,N1SDP_FW_NVCTR_VAL))
 
 # TF-A not required to load the SCP Images
 override CSS_LOAD_SCP_IMAGES	  	:=	0
 
-# BL1/BL2 Image not a part of the capsule Image for n1sdp
-override NEED_BL1		  	:=	no
-override NEED_BL2		  	:=	no
 override NEED_BL2U		  	:=	no
 
-#TFA for n1sdp starts from BL31
-override RESET_TO_BL31            	:=	1
-
 # 32 bit mode not supported
 override CTX_INCLUDE_AARCH32_REGS 	:=	0
 
@@ -73,4 +102,3 @@
 include plat/arm/common/arm_common.mk
 include plat/arm/css/common/css_common.mk
 include plat/arm/board/common/board_common.mk
-

diff --git a/plat/arm/board/rde1edge/include/platform_def.h b/plat/arm/board/rde1edge/include/platform_def.h
index a9b30a4..69bfd7b 100644
--- a/plat/arm/board/rde1edge/include/platform_def.h
+++ b/plat/arm/board/rde1edge/include/platform_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2018-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -26,12 +26,15 @@
 
 #define CSS_SYSTEM_PWR_DMN_LVL		ARM_PWR_LVL3
 
+/* Maximum number of address bits used per chip */
+#define CSS_SGI_ADDR_BITS_PER_CHIP	U(36)
+
 /*
  * Physical and virtual address space limits for MMU in AARCH64 & AARCH32 modes
  */
 #ifdef __aarch64__
-#define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 36)
-#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 36)
+#define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << CSS_SGI_ADDR_BITS_PER_CHIP)
+#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << CSS_SGI_ADDR_BITS_PER_CHIP)
 #else
 #define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 32)
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 32)

diff --git a/plat/arm/board/rdn1edge/include/platform_def.h b/plat/arm/board/rdn1edge/include/platform_def.h
index a61b0d5..de01902 100644
--- a/plat/arm/board/rdn1edge/include/platform_def.h
+++ b/plat/arm/board/rdn1edge/include/platform_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -30,12 +30,17 @@
 /* Virtual address used by dynamic mem_protect for chunk_base */
 #define PLAT_ARM_MEM_PROTEC_VA_FRAME	UL(0xc0000000)
 
+/* Maximum number of address bits used per chip */
+#define CSS_SGI_ADDR_BITS_PER_CHIP	U(42)
+
 /*
  * Physical and virtual address space limits for MMU in AARCH64 & AARCH32 modes
  */
 #ifdef __aarch64__
-#define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 43)
-#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 43)
+#define PLAT_PHY_ADDR_SPACE_SIZE	CSS_SGI_REMOTE_CHIP_MEM_OFFSET( \
+						CSS_SGI_CHIP_COUNT)
+#define PLAT_VIRT_ADDR_SPACE_SIZE	CSS_SGI_REMOTE_CHIP_MEM_OFFSET( \
+						CSS_SGI_CHIP_COUNT)
 #else
 #define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 32)
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 32)

diff --git a/plat/arm/board/rdn2/fdts/rdn2_nt_fw_config.dts b/plat/arm/board/rdn2/fdts/rdn2_nt_fw_config.dts
index bbc36fc..dd70141 100644
--- a/plat/arm/board/rdn2/fdts/rdn2_nt_fw_config.dts
+++ b/plat/arm/board/rdn2/fdts/rdn2_nt_fw_config.dts

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020 - 2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,5 +18,26 @@
 		platform-id = <0x0>;
 		config-id = <0x0>;
 		multi-chip-mode = <0x0>;
+		/*
+		 * First cell pair: Count of isolated CPUs in the list.
+		 * Rest of the cells: MPID list of the isolated CPUs.
+		 */
+		isolated-cpu-list = <0x0 0x0
+				     0x0 0x0
+				     0x0 0x0
+				     0x0 0x0
+				     0x0 0x0
+				     0x0 0x0
+				     0x0 0x0
+				     0x0 0x0
+				     0x0 0x0
+				     0x0 0x0
+				     0x0 0x0
+				     0x0 0x0
+				     0x0 0x0
+				     0x0 0x0
+				     0x0 0x0
+				     0x0 0x0
+				     0x0 0x0>;
 	};
 };

diff --git a/plat/arm/board/rdn2/include/platform_def.h b/plat/arm/board/rdn2/include/platform_def.h
index e4015f7..464a157 100644
--- a/plat/arm/board/rdn2/include/platform_def.h
+++ b/plat/arm/board/rdn2/include/platform_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -69,15 +69,16 @@
  */
 #ifdef __aarch64__
 #if (CSS_SGI_PLATFORM_VARIANT == 2)
+#define CSS_SGI_ADDR_BITS_PER_CHIP	U(46)	/* 64TB */
+#else
+#define CSS_SGI_ADDR_BITS_PER_CHIP	U(42)	/* 4TB */
+#endif
+
 #define PLAT_PHY_ADDR_SPACE_SIZE	CSS_SGI_REMOTE_CHIP_MEM_OFFSET( \
 						CSS_SGI_CHIP_COUNT)
 #define PLAT_VIRT_ADDR_SPACE_SIZE	CSS_SGI_REMOTE_CHIP_MEM_OFFSET( \
 						CSS_SGI_CHIP_COUNT)
 #else
-#define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 42)
-#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 42)
-#endif
-#else
 #define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 32)
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 32)
 #endif

diff --git a/plat/arm/board/rdv1/include/platform_def.h b/plat/arm/board/rdv1/include/platform_def.h
index 5b98b4e..620fa3e 100644
--- a/plat/arm/board/rdv1/include/platform_def.h
+++ b/plat/arm/board/rdv1/include/platform_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -46,12 +46,15 @@
 		(TZC_REGION_ACCESS_RDWR(TZC_NSAID_CLCD))   | \
 		(TZC_REGION_ACCESS_RDWR(TZC_NSAID_VIRTIO))
 
+/* Maximum number of address bits used per chip */
+#define CSS_SGI_ADDR_BITS_PER_CHIP	U(42)
+
 /*
  * Physical and virtual address space limits for MMU in AARCH64 & AARCH32 modes
  */
 #ifdef __aarch64__
-#define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 42)
-#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 42)
+#define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << CSS_SGI_ADDR_BITS_PER_CHIP)
+#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << CSS_SGI_ADDR_BITS_PER_CHIP)
 #else
 #define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 32)
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 32)

diff --git a/plat/arm/board/rdv1mc/include/platform_def.h b/plat/arm/board/rdv1mc/include/platform_def.h
index 12ce806..3670904 100644
--- a/plat/arm/board/rdv1mc/include/platform_def.h
+++ b/plat/arm/board/rdv1mc/include/platform_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -46,6 +46,9 @@
 /* Virtual address used by dynamic mem_protect for chunk_base */
 #define PLAT_ARM_MEM_PROTEC_VA_FRAME	UL(0xC0000000)
 
+/* Remote chip address offset (4TB per chip) */
+#define CSS_SGI_ADDR_BITS_PER_CHIP	U(42)
+
 /* Physical and virtual address space limits for MMU in AARCH64 mode */
 #define PLAT_PHY_ADDR_SPACE_SIZE	CSS_SGI_REMOTE_CHIP_MEM_OFFSET( \
 						CSS_SGI_CHIP_COUNT)

diff --git a/plat/arm/board/sgi575/include/platform_def.h b/plat/arm/board/sgi575/include/platform_def.h
index 72d5f7c..82a38c5 100644
--- a/plat/arm/board/sgi575/include/platform_def.h
+++ b/plat/arm/board/sgi575/include/platform_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -27,12 +27,15 @@
 
 #define PLAT_MAX_PWR_LVL		ARM_PWR_LVL1
 
+/* Maximum number of address bits used per chip */
+#define CSS_SGI_ADDR_BITS_PER_CHIP	U(36)
+
 /*
  * Physical and virtual address space limits for MMU in AARCH64 & AARCH32 modes
  */
 #ifdef __aarch64__
-#define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 36)
-#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 36)
+#define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << CSS_SGI_ADDR_BITS_PER_CHIP)
+#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << CSS_SGI_ADDR_BITS_PER_CHIP)
 #else
 #define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 32)
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 32)

diff --git a/plat/arm/board/sgm775/include/platform_def.h b/plat/arm/board/sgm775/include/platform_def.h
deleted file mode 100644
index e83cd57..0000000
--- a/plat/arm/board/sgm775/include/platform_def.h
+++ /dev/null

@@ -1,29 +0,0 @@
-/*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef PLATFORM_DEF_H
-#define PLATFORM_DEF_H
-
-#include <sgm_base_platform_def.h>
-
-#define PLAT_MAX_CPUS_PER_CLUSTER	U(8)
-#define PLAT_MAX_PE_PER_CPU		U(1)
-
-#define PLAT_ARM_DRAM2_BASE		ULL(0x880000000)
-#define PLAT_ARM_DRAM2_SIZE		ULL(0x180000000)
-
-/*
- * Physical and virtual address space limits for MMU in AARCH64 & AARCH32 modes
- */
-#ifdef __aarch64__
-#define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 36)
-#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 36)
-#else
-#define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 32)
-#define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 32)
-#endif
-
-#endif /* PLATFORM_DEF_H */

diff --git a/plat/arm/board/sgm775/platform.mk b/plat/arm/board/sgm775/platform.mk
deleted file mode 100644
index f8df1a7..0000000
--- a/plat/arm/board/sgm775/platform.mk
+++ /dev/null

@@ -1,39 +0,0 @@
-#
-# Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
-#
-# SPDX-License-Identifier: BSD-3-Clause
-#
-
-$(warning Platform ${PLAT} is deprecated. Some of the features might not work as expected)
-
-include plat/arm/css/sgm/sgm-common.mk
-
-SGM775_BASE= plat/arm/board/sgm775
-
-# Add the FDT_SOURCES and options for Dynamic Config
-FDT_SOURCES		+=      ${SGM775_BASE}/fdts/${PLAT}_fw_config.dts	\
-				${SGM775_BASE}/fdts/${PLAT}_tb_fw_config.dts
-FW_CONFIG		:=      ${BUILD_PLAT}/fdts/${PLAT}_fw_config.dtb
-TB_FW_CONFIG		:=      ${BUILD_PLAT}/fdts/${PLAT}_tb_fw_config.dtb
-
-# Add the FW_CONFIG to FIP and specify the same to certtool
-$(eval $(call TOOL_ADD_PAYLOAD,${FW_CONFIG},--fw-config,${FW_CONFIG}))
-# Add the TB_FW_CONFIG to FIP and specify the same to certtool
-$(eval $(call TOOL_ADD_PAYLOAD,${TB_FW_CONFIG},--tb-fw-config,${TB_FW_CONFIG}))
-
-PLAT_INCLUDES +=-I${SGM775_BASE}/include/
-
-BL1_SOURCES		+=	${SGM775_BASE}/sgm775_err.c
-
-BL2_SOURCES		+=	lib/utils/mem_region.c                  \
-				${SGM775_BASE}/sgm775_err.c		\
-				plat/arm/common/arm_nor_psci_mem_protect.c
-
-BL31_SOURCES		+=	drivers/cfi/v2m/v2m_flash.c		\
-				lib/utils/mem_region.c			\
-				plat/arm/common/arm_nor_psci_mem_protect.c
-
-ifeq (${TRUSTED_BOARD_BOOT}, 1)
-BL1_SOURCES		+=	${SGM775_BASE}/sgm775_trusted_boot.c
-BL2_SOURCES		+=	${SGM775_BASE}/sgm775_trusted_boot.c
-endif

diff --git a/plat/arm/board/sgm775/sgm775_trusted_boot.c b/plat/arm/board/sgm775/sgm775_trusted_boot.c
deleted file mode 100644
index 4592b8f..0000000
--- a/plat/arm/board/sgm775/sgm775_trusted_boot.c
+++ /dev/null

@@ -1,26 +0,0 @@
-/*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <plat/arm/common/plat_arm.h>
-
-/*
- * Return the ROTPK hash in the following ASN.1 structure in DER format:
- *
- * AlgorithmIdentifier  ::=  SEQUENCE  {
- *     algorithm         OBJECT IDENTIFIER,
- *     parameters        ANY DEFINED BY algorithm OPTIONAL
- * }
- *
- * DigestInfo ::= SEQUENCE {
- *     digestAlgorithm   AlgorithmIdentifier,
- *     digest            OCTET STRING
- * }
- */
-int plat_get_rotpk_info(void *cookie, void **key_ptr, unsigned int *key_len,
-			unsigned int *flags)
-{
-	return arm_get_rotpk_info(cookie, key_ptr, key_len, flags);
-}

diff --git a/plat/arm/board/sgm775/tsp/tsp-sgm775.mk b/plat/arm/board/sgm775/tsp/tsp-sgm775.mk
deleted file mode 100644
index 129b586..0000000
--- a/plat/arm/board/sgm775/tsp/tsp-sgm775.mk
+++ /dev/null

@@ -1,7 +0,0 @@
-#
-# Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
-#
-# SPDX-License-Identifier: BSD-3-Clause
-#
-
-include plat/arm/css/sgm/tsp/tsp-sgm.mk
\ No newline at end of file

diff --git a/plat/arm/common/arm_bl1_setup.c b/plat/arm/common/arm_bl1_setup.c
index 73338cb..7a9e04d 100644
--- a/plat/arm/common/arm_bl1_setup.c
+++ b/plat/arm/common/arm_bl1_setup.c

@@ -166,7 +166,7 @@
 
 	/* Set global DTB info for fixed fw_config information */
 	fw_config_max_size = ARM_FW_CONFIG_LIMIT - ARM_FW_CONFIG_BASE;
-	set_config_info(ARM_FW_CONFIG_BASE, fw_config_max_size, FW_CONFIG_ID);
+	set_config_info(ARM_FW_CONFIG_BASE, ~0UL, fw_config_max_size, FW_CONFIG_ID);
 
 	/* Fill the device tree information struct with the info from the config dtb */
 	err = fconf_load_config(FW_CONFIG_ID);

diff --git a/plat/arm/common/arm_bl31_setup.c b/plat/arm/common/arm_bl31_setup.c
index a6f7df5..cf403b1 100644
--- a/plat/arm/common/arm_bl31_setup.c
+++ b/plat/arm/common/arm_bl31_setup.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -164,6 +164,14 @@
 	bl33_image_ep_info.spsr = arm_get_spsr_for_bl33_entry();
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
 
+#if ENABLE_RME
+	/*
+	 * Populate entry point information for RMM.
+	 * Only PC needs to be set as other fields are determined by RMMD.
+	 */
+	rmm_image_ep_info.pc = RMM_BASE;
+#endif /* ENABLE_RME */
+
 #else /* RESET_TO_BL31 */
 
 	/*

diff --git a/plat/arm/common/arm_common.mk b/plat/arm/common/arm_common.mk
index 6d7aa2d..bd59ec0 100644
--- a/plat/arm/common/arm_common.mk
+++ b/plat/arm/common/arm_common.mk

@@ -73,6 +73,14 @@
 $(eval $(call assert_boolean,ARM_BL31_IN_DRAM))
 $(eval $(call add_define,ARM_BL31_IN_DRAM))
 
+# As per CCA security model, all root firmware must execute from on-chip secure
+# memory. This means we must not run BL31 from TZC-protected DRAM.
+ifeq (${ARM_BL31_IN_DRAM},1)
+  ifeq (${ENABLE_RME},1)
+    $(error "BL31 must not run from DRAM on RME-systems. Please set ARM_BL31_IN_DRAM to 0")
+  endif
+endif
+
 # Process ARM_PLAT_MT flag
 ARM_PLAT_MT			:=	0
 $(eval $(call assert_boolean,ARM_PLAT_MT))
@@ -373,6 +381,8 @@
         endif
     else ifeq (${COT},dualroot)
         AUTH_SOURCES	+=	drivers/auth/dualroot/cot.c
+    else ifeq (${COT},cca)
+        AUTH_SOURCES	+=	drivers/auth/cca/cot.c
     else
         $(error Unknown chain of trust ${COT})
     endif
@@ -401,6 +411,10 @@
     $(info Including ${MEASURED_BOOT_MK})
     include ${MEASURED_BOOT_MK}
 
+    ifneq (${MBOOT_EL_HASH_ALG}, sha256)
+        $(eval $(call add_define,TF_MBEDTLS_MBOOT_USE_SHA512))
+    endif
+
     BL1_SOURCES		+= 	${EVENT_LOG_SOURCES}
     BL2_SOURCES		+= 	${EVENT_LOG_SOURCES}
 endif

diff --git a/plat/arm/common/arm_dyn_cfg.c b/plat/arm/common/arm_dyn_cfg.c
index 83e3f9a..a62693c 100644
--- a/plat/arm/common/arm_dyn_cfg.c
+++ b/plat/arm/common/arm_dyn_cfg.c

@@ -131,6 +131,7 @@
 	bl_mem_params_node_t *cfg_mem_params = NULL;
 	uintptr_t image_base;
 	uint32_t image_size;
+	unsigned int error_config_id = MAX_IMAGE_IDS;
 	const unsigned int config_ids[] = {
 			HW_CONFIG_ID,
 			SOC_FW_CONFIG_ID,
@@ -142,17 +143,17 @@
 
 	/* Iterate through all the fw config IDs */
 	for (i = 0; i < ARRAY_SIZE(config_ids); i++) {
-		/* Get the config load address and size from TB_FW_CONFIG */
+		/* Get the config load address and size */
 		cfg_mem_params = get_bl_mem_params_node(config_ids[i]);
 		if (cfg_mem_params == NULL) {
-			VERBOSE("%sHW_CONFIG in bl_mem_params_node\n",
-				"Couldn't find ");
+			VERBOSE("%sconfig_id = %d in bl_mem_params_node\n",
+				"Couldn't find ", config_ids[i]);
 			continue;
 		}
 
 		dtb_info = FCONF_GET_PROPERTY(dyn_cfg, dtb, config_ids[i]);
 		if (dtb_info == NULL) {
-			VERBOSE("%sconfig_id %d load info in TB_FW_CONFIG\n",
+			VERBOSE("%sconfig_id %d load info in FW_CONFIG\n",
 				"Couldn't find ", config_ids[i]);
 			continue;
 		}
@@ -168,17 +169,32 @@
 		if (config_ids[i] != HW_CONFIG_ID) {
 
 			if (check_uptr_overflow(image_base, image_size)) {
+				VERBOSE("%s=%d as its %s is overflowing uptr\n",
+					"skip loading of firmware config",
+					config_ids[i],
+					"load-address");
+				error_config_id = config_ids[i];
 				continue;
 			}
 #ifdef	BL31_BASE
 			/* Ensure the configs don't overlap with BL31 */
 			if ((image_base >= BL31_BASE) &&
 			    (image_base <= BL31_LIMIT)) {
+				VERBOSE("%s=%d as its %s is overlapping BL31\n",
+					"skip loading of firmware config",
+					config_ids[i],
+					"load-address");
+				error_config_id = config_ids[i];
 				continue;
 			}
 #endif
 			/* Ensure the configs are loaded in a valid address */
 			if (image_base < ARM_BL_RAM_BASE) {
+				VERBOSE("%s=%d as its %s is invalid\n",
+					"skip loading of firmware config",
+					config_ids[i],
+					"load-address");
+				error_config_id = config_ids[i];
 				continue;
 			}
 #ifdef BL32_BASE
@@ -188,6 +204,11 @@
 			 */
 			if ((image_base >= BL32_BASE) &&
 			    (image_base <= BL32_LIMIT)) {
+				VERBOSE("%s=%d as its %s is overlapping BL32\n",
+					"skip loading of firmware config",
+					config_ids[i],
+					"load-address");
+				error_config_id = config_ids[i];
 				continue;
 			}
 #endif
@@ -202,4 +223,9 @@
 		 */
 		cfg_mem_params->image_info.h.attr &= ~IMAGE_ATTRIB_SKIP_LOADING;
 	}
+
+	if (error_config_id != MAX_IMAGE_IDS) {
+		ERROR("Invalid config file %u\n", error_config_id);
+		panic();
+	}
 }

diff --git a/plat/arm/common/fconf/arm_fconf_io.c b/plat/arm/common/fconf/arm_fconf_io.c
index aea2f38..6c32331 100644
--- a/plat/arm/common/fconf/arm_fconf_io.c
+++ b/plat/arm/common/fconf/arm_fconf_io.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, ARM Limited. All rights reserved.
+ * Copyright (c) 2019-2022, ARM Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -72,6 +72,9 @@
 #if TRUSTED_BOARD_BOOT
 	[TRUSTED_BOOT_FW_CERT_ID] = {UUID_TRUSTED_BOOT_FW_CERT},
 #if !ARM_IO_IN_DTB
+	[CCA_CONTENT_CERT_ID] = {UUID_CCA_CONTENT_CERT},
+	[CORE_SWD_KEY_CERT_ID] = {UUID_CORE_SWD_KEY_CERT},
+	[PLAT_KEY_CERT_ID] = {UUID_PLAT_KEY_CERT},
 	[TRUSTED_KEY_CERT_ID] = {UUID_TRUSTED_KEY_CERT},
 	[SCP_FW_KEY_CERT_ID] = {UUID_SCP_FW_KEY_CERT},
 	[SOC_FW_KEY_CERT_ID] = {UUID_SOC_FW_KEY_CERT},
@@ -196,6 +199,21 @@
 		open_fip
 	},
 #if !ARM_IO_IN_DTB
+	[CCA_CONTENT_CERT_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&arm_uuid_spec[CCA_CONTENT_CERT_ID],
+		open_fip
+	},
+	[CORE_SWD_KEY_CERT_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&arm_uuid_spec[CORE_SWD_KEY_CERT_ID],
+		open_fip
+	},
+	[PLAT_KEY_CERT_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&arm_uuid_spec[PLAT_KEY_CERT_ID],
+		open_fip
+	},
 	[TRUSTED_KEY_CERT_ID] = {
 		&fip_dev_handle,
 		(uintptr_t)&arm_uuid_spec[TRUSTED_KEY_CERT_ID],
@@ -260,7 +278,7 @@
 #ifdef IMAGE_BL2
 
 #if TRUSTED_BOARD_BOOT
-#define FCONF_ARM_IO_UUID_NUMBER	U(21)
+#define FCONF_ARM_IO_UUID_NUMBER	U(24)
 #else
 #define FCONF_ARM_IO_UUID_NUMBER	U(10)
 #endif
@@ -286,6 +304,9 @@
 	{TOS_FW_CONFIG_ID, "tos_fw_cfg_uuid"},
 	{NT_FW_CONFIG_ID, "nt_fw_cfg_uuid"},
 #if TRUSTED_BOARD_BOOT
+	{CCA_CONTENT_CERT_ID, "cca_cert_uuid"},
+	{CORE_SWD_KEY_CERT_ID, "core_swd_cert_uuid"},
+	{PLAT_KEY_CERT_ID, "plat_cert_uuid"},
 	{TRUSTED_KEY_CERT_ID, "t_key_cert_uuid"},
 	{SCP_FW_KEY_CERT_ID, "scp_fw_key_uuid"},
 	{SOC_FW_KEY_CERT_ID, "soc_fw_key_uuid"},

diff --git a/plat/arm/common/trp/arm_trp.mk b/plat/arm/common/trp/arm_trp.mk
index 997111f..204c14a 100644
--- a/plat/arm/common/trp/arm_trp.mk
+++ b/plat/arm/common/trp/arm_trp.mk

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+# Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -8,3 +8,5 @@
 RMM_SOURCES		+=	plat/arm/common/trp/arm_trp_setup.c	\
 				plat/arm/common/arm_topology.c		\
 				plat/common/aarch64/platform_mp_stack.S
+
+INCLUDES		+=	-Iinclude/services/trp

diff --git a/plat/arm/common/trp/arm_trp_setup.c b/plat/arm/common/trp/arm_trp_setup.c
index 8e48293..59b4c06 100644
--- a/plat/arm/common/trp/arm_trp_setup.c
+++ b/plat/arm/common/trp/arm_trp_setup.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -8,33 +8,65 @@
 #include <common/debug.h>
 #include <drivers/arm/pl011.h>
 #include <drivers/console.h>
+#include <services/rmm_core_manifest.h>
+#include <services/rmmd_svc.h>
+#include <services/trp/platform_trp.h>
+#include <trp_helpers.h>
+
 #include <plat/arm/common/plat_arm.h>
 #include <platform_def.h>
 
 /*******************************************************************************
+ * Received from boot manifest and populated here
+ ******************************************************************************/
+extern uint32_t trp_boot_manifest_version;
+
+/*******************************************************************************
  * Initialize the UART
  ******************************************************************************/
 static console_t arm_trp_runtime_console;
 
+static int arm_trp_process_manifest(rmm_manifest_t *manifest)
+{
+	/* Verify the Boot Manifest Version. Only the Major is considered */
+	if (RMMD_MANIFEST_VERSION_MAJOR !=
+		RMMD_GET_MANIFEST_VERSION_MAJOR(manifest->version)) {
+		return E_RMM_BOOT_MANIFEST_VERSION_NOT_SUPPORTED;
+	}
+
-void arm_trp_early_platform_setup(void)
+	trp_boot_manifest_version = manifest->version;
+	flush_dcache_range((uintptr_t)manifest, sizeof(rmm_manifest_t));
+
+	return 0;
+}
+
+void arm_trp_early_platform_setup(rmm_manifest_t *manifest)
 {
+	int rc;
+
+	rc = arm_trp_process_manifest(manifest);
+	if (rc != 0) {
+		trp_boot_abort(rc);
+	}
+
 	/*
 	 * Initialize a different console than already in use to display
 	 * messages from trp
 	 */
-	int rc = console_pl011_register(PLAT_ARM_TRP_UART_BASE,
-					PLAT_ARM_TRP_UART_CLK_IN_HZ,
-					ARM_CONSOLE_BAUDRATE,
-					&arm_trp_runtime_console);
+	rc = console_pl011_register(PLAT_ARM_TRP_UART_BASE,
+				    PLAT_ARM_TRP_UART_CLK_IN_HZ,
+				    ARM_CONSOLE_BAUDRATE,
+				    &arm_trp_runtime_console);
 	if (rc == 0) {
 		panic();
 	}
 
 	console_set_scope(&arm_trp_runtime_console,
 			  CONSOLE_FLAG_BOOT | CONSOLE_FLAG_RUNTIME);
+
 }
 
-void trp_early_platform_setup(void)
+void trp_early_platform_setup(rmm_manifest_t *manifest)
 {
-	arm_trp_early_platform_setup();
+	arm_trp_early_platform_setup(manifest);
 }

diff --git a/plat/arm/css/sgi/include/sgi_base_platform_def.h b/plat/arm/css/sgi/include/sgi_base_platform_def.h
index c9c8c04..22870c4 100644
--- a/plat/arm/css/sgi/include/sgi_base_platform_def.h
+++ b/plat/arm/css/sgi/include/sgi_base_platform_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2021, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -21,8 +21,9 @@
 
 #define PLAT_ARM_TRUSTED_SRAM_SIZE	0x00040000	/* 256 KB */
 
-/* Remote chip address offset (4TB per chip) */
-#define CSS_SGI_REMOTE_CHIP_MEM_OFFSET(n)	((ULL(1) << 42) * (n))
+/* Remote chip address offset */
+#define CSS_SGI_REMOTE_CHIP_MEM_OFFSET(n)	\
+		((ULL(1) << CSS_SGI_ADDR_BITS_PER_CHIP) * (n))
 
 /*
  * PLAT_ARM_MMAP_ENTRIES depends on the number of entries in the
@@ -35,8 +36,8 @@
 # if SPM_MM
 #  define PLAT_ARM_MMAP_ENTRIES		(9  + ((CSS_SGI_CHIP_COUNT - 1) * 3))
 #  define MAX_XLAT_TABLES		(7  + ((CSS_SGI_CHIP_COUNT - 1) * 3))
-#  define PLAT_SP_IMAGE_MMAP_REGIONS	9
-#  define PLAT_SP_IMAGE_MAX_XLAT_TABLES	11
+#  define PLAT_SP_IMAGE_MMAP_REGIONS	10
+#  define PLAT_SP_IMAGE_MAX_XLAT_TABLES	12
 # else
 #  define PLAT_ARM_MMAP_ENTRIES		(5 + ((CSS_SGI_CHIP_COUNT - 1) * 3))
 #  define MAX_XLAT_TABLES		(6 + ((CSS_SGI_CHIP_COUNT - 1) * 3))
@@ -67,7 +68,7 @@
  * PLAT_ARM_MAX_BL1_RW_SIZE is calculated using the current BL1 RW debug size
  * plus a little space for growth.
  */
-#define PLAT_ARM_MAX_BL1_RW_SIZE	0xC000
+#define PLAT_ARM_MAX_BL1_RW_SIZE	(64 * 1024)	/* 64 KB */
 
 /*
  * PLAT_ARM_MAX_ROMLIB_RW_SIZE is define to use a full page
@@ -101,7 +102,7 @@
  * calculated using the current BL31 PROGBITS debug size plus the sizes of
  * BL2 and BL1-RW
  */
-#define PLAT_ARM_MAX_BL31_SIZE		0x3B000
+#define PLAT_ARM_MAX_BL31_SIZE		0x48000
 
 /*
  * Size of cacheable stacks
@@ -130,6 +131,21 @@
 # define PLATFORM_STACK_SIZE 0x440
 #endif
 
+/* PL011 UART related constants */
+#define SOC_CSS_SEC_UART_BASE			UL(0x2A410000)
+#define SOC_CSS_NSEC_UART_BASE			UL(0x2A400000)
+#define SOC_CSS_UART_SIZE			UL(0x10000)
+#define SOC_CSS_UART_CLK_IN_HZ			UL(7372800)
+
+/* UART related constants */
+#define PLAT_ARM_BOOT_UART_BASE			SOC_CSS_SEC_UART_BASE
+#define PLAT_ARM_BOOT_UART_CLK_IN_HZ		SOC_CSS_UART_CLK_IN_HZ
+
+#define PLAT_ARM_RUN_UART_BASE			SOC_CSS_SEC_UART_BASE
+#define PLAT_ARM_RUN_UART_CLK_IN_HZ		SOC_CSS_UART_CLK_IN_HZ
+
+#define PLAT_ARM_CRASH_UART_BASE		SOC_CSS_SEC_UART_BASE
+#define PLAT_ARM_CRASH_UART_CLK_IN_HZ		SOC_CSS_UART_CLK_IN_HZ
 
 #define PLAT_ARM_NSTIMER_FRAME_ID	0
 
@@ -258,4 +274,21 @@
 		CSS_SGI_REMOTE_CHIP_MEM_OFFSET(n) + ARM_DRAM2_END,	\
 		ARM_TZC_NS_DRAM_S_ACCESS, PLAT_ARM_TZC_NS_DEV_ACCESS}
 
+#if SPM_MM
+
+/*
+ * Stand-alone MM logs would be routed via secure UART. Define page table
+ * entry for secure UART which would be common to all platforms.
+ */
+#define SOC_PLATFORM_SECURE_UART	MAP_REGION_FLAT(		\
+					SOC_CSS_SEC_UART_BASE,		\
+					SOC_CSS_UART_SIZE,		\
+					MT_DEVICE | MT_RW | 		\
+					MT_SECURE | MT_USER)
+
+#endif
+
+/* SDS ID for unusable CPU MPID list structure */
+#define SDS_ISOLATED_CPU_LIST_ID		U(128)
+
 #endif /* SGI_BASE_PLATFORM_DEF_H */

diff --git a/plat/arm/css/sgi/include/sgi_soc_css_def.h b/plat/arm/css/sgi/include/sgi_soc_css_def.h
new file mode 100644
index 0000000..f78b45a
--- /dev/null
+++ b/plat/arm/css/sgi/include/sgi_soc_css_def.h

@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SGI_SOC_CSS_DEF_H
+#define SGI_SOC_CSS_DEF_H
+
+#include <lib/utils_def.h>
+#include <plat/arm/board/common/v2m_def.h>
+#include <plat/arm/soc/common/soc_css_def.h>
+#include <plat/common/common_def.h>
+
+/*
+ * Definitions common to all ARM CSSv1-based development platforms
+ */
+
+/* Platform ID address */
+#define BOARD_CSS_PLAT_ID_REG_ADDR		UL(0x7ffe00e0)
+
+/* Platform ID related accessors */
+#define BOARD_CSS_PLAT_ID_REG_ID_MASK		0x0f
+#define BOARD_CSS_PLAT_ID_REG_ID_SHIFT		0x0
+#define BOARD_CSS_PLAT_TYPE_EMULATOR		0x02
+
+#ifndef __ASSEMBLER__
+
+#include <lib/mmio.h>
+
+#define BOARD_CSS_GET_PLAT_TYPE(addr)					\
+	((mmio_read_32(addr) & BOARD_CSS_PLAT_ID_REG_ID_MASK)		\
+	>> BOARD_CSS_PLAT_ID_REG_ID_SHIFT)
+
+#endif /* __ASSEMBLER__ */
+
+#define MAX_IO_DEVICES			3
+#define MAX_IO_HANDLES			4
+
+/* Reserve the last block of flash for PSCI MEM PROTECT flag */
+#define PLAT_ARM_FLASH_IMAGE_BASE	V2M_FLASH0_BASE
+#define PLAT_ARM_FLASH_IMAGE_MAX_SIZE	(V2M_FLASH0_SIZE - V2M_FLASH_BLOCK_SIZE)
+
+#define PLAT_ARM_NVM_BASE		V2M_FLASH0_BASE
+#define PLAT_ARM_NVM_SIZE		(V2M_FLASH0_SIZE - V2M_FLASH_BLOCK_SIZE)
+
+#endif /* SGI_SOC_CSS_DEF_H */

diff --git a/plat/arm/css/sgi/include/sgi_soc_css_def_v2.h b/plat/arm/css/sgi/include/sgi_soc_css_def_v2.h
index 639b687..acf31eb 100644
--- a/plat/arm/css/sgi/include/sgi_soc_css_def_v2.h
+++ b/plat/arm/css/sgi/include/sgi_soc_css_def_v2.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -24,17 +24,10 @@
 
 #define SOC_CSS_PCIE_CONTROL_BASE	UL(0x0ef20000)
 
-/* PL011 UART related constants */
-#define SOC_CSS_UART1_BASE		UL(0x0ef80000)
-#define SOC_CSS_UART0_BASE		UL(0x0ef70000)
-
 /* Memory controller */
 #define SOC_MEMCNTRL_BASE		UL(0x10000000)
 #define SOC_MEMCNTRL_SIZE		UL(0x10000000)
 
-#define SOC_CSS_UART0_CLK_IN_HZ		UL(7372800)
-#define SOC_CSS_UART1_CLK_IN_HZ		UL(7372800)
-
 /* SoC NIC-400 Global Programmers View (GPV) */
 #define SOC_CSS_NIC400_BASE		UL(0x0ED00000)
 
@@ -206,17 +199,4 @@
 #define PLAT_ARM_NVM_BASE		V2M_FLASH0_BASE
 #define PLAT_ARM_NVM_SIZE		(V2M_FLASH0_SIZE - V2M_FLASH_BLOCK_SIZE)
 
-/* UART related constants */
-#define PLAT_ARM_BOOT_UART_BASE			SOC_CSS_UART0_BASE
-#define PLAT_ARM_BOOT_UART_CLK_IN_HZ		SOC_CSS_UART0_CLK_IN_HZ
-
-#define PLAT_ARM_RUN_UART_BASE			SOC_CSS_UART1_BASE
-#define PLAT_ARM_RUN_UART_CLK_IN_HZ		SOC_CSS_UART1_CLK_IN_HZ
-
-#define PLAT_ARM_SP_MIN_RUN_UART_BASE		SOC_CSS_UART1_BASE
-#define PLAT_ARM_SP_MIN_RUN_UART_CLK_IN_HZ	SOC_CSS_UART1_CLK_IN_HZ
-
-#define PLAT_ARM_CRASH_UART_BASE		PLAT_ARM_RUN_UART_BASE
-#define PLAT_ARM_CRASH_UART_CLK_IN_HZ		PLAT_ARM_RUN_UART_CLK_IN_HZ
-
 #endif /* SGI_SOC_CSS_DEF_V2_H */

diff --git a/plat/arm/css/sgi/include/sgi_soc_platform_def.h b/plat/arm/css/sgi/include/sgi_soc_platform_def.h
index 405d62f..3b8d9c6 100644
--- a/plat/arm/css/sgi/include/sgi_soc_platform_def.h
+++ b/plat/arm/css/sgi/include/sgi_soc_platform_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,10 +7,10 @@
 #ifndef SGI_SOC_PLATFORM_DEF_H
 #define SGI_SOC_PLATFORM_DEF_H
 
-#include <sgi_base_platform_def.h>
-#include <plat/arm/board/common/board_css_def.h>
 #include <plat/arm/board/common/v2m_def.h>
 #include <plat/arm/soc/common/soc_css_def.h>
+#include <sgi_base_platform_def.h>
+#include <sgi_soc_css_def.h>
 
 /* Map the System registers to access from S-EL0 */
 #define CSS_SYSTEMREG_DEVICE_BASE	(0x1C010000)

diff --git a/plat/arm/css/sgi/sgi-common.mk b/plat/arm/css/sgi/sgi-common.mk
index f56fe35..4af579e 100644
--- a/plat/arm/css/sgi/sgi-common.mk
+++ b/plat/arm/css/sgi/sgi-common.mk

@@ -23,6 +23,8 @@
 # Do not enable SVE
 ENABLE_SVE_FOR_NS		:=	0
 
+CTX_INCLUDE_FPREGS		:=	1
+
 INTERCONNECT_SOURCES	:=	${CSS_ENT_BASE}/sgi_interconnect.c
 
 PLAT_INCLUDES		+=	-I${CSS_ENT_BASE}/include
@@ -42,7 +44,8 @@
 BL1_SOURCES		+=	${INTERCONNECT_SOURCES}			\
 				drivers/arm/sbsa/sbsa.c
 
-BL2_SOURCES		+=	${CSS_ENT_BASE}/sgi_image_load.c
+BL2_SOURCES		+=	${CSS_ENT_BASE}/sgi_image_load.c	\
+				drivers/arm/css/sds/sds.c
 
 BL31_SOURCES		+=	${INTERCONNECT_SOURCES}			\
 				${ENT_GIC_SOURCES}			\

diff --git a/plat/arm/css/sgi/sgi_image_load.c b/plat/arm/css/sgi/sgi_image_load.c
index 09f3b72..ac4bfd2 100644
--- a/plat/arm/css/sgi/sgi_image_load.c
+++ b/plat/arm/css/sgi/sgi_image_load.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,16 +9,68 @@
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <common/desc_image_load.h>
+#include <drivers/arm/css/sds.h>
 #include <plat/arm/common/plat_arm.h>
 #include <plat/common/platform.h>
 
+#include <platform_def.h>
+#include <sgi_base_platform_def.h>
 #include <sgi_variant.h>
 
+/*
+ * Information about the isolated CPUs obtained from SDS.
+ */
+struct isolated_cpu_mpid_list {
+	uint64_t num_entries; /* Number of entries in the list */
+	uint64_t mpid_list[PLATFORM_CORE_COUNT]; /* List of isolated CPU MPIDs */
+};
+
+/* Function to read isolated CPU MPID list from SDS. */
+void plat_arm_sgi_get_isolated_cpu_list(struct isolated_cpu_mpid_list *list)
+{
+	int ret;
+
+	ret = sds_init();
+	if (ret != SDS_OK) {
+		ERROR("SDS initialization failed, error: %d\n", ret);
+		panic();
+	}
+
+	ret = sds_struct_read(SDS_ISOLATED_CPU_LIST_ID, 0, &list->num_entries,
+			sizeof(list->num_entries), SDS_ACCESS_MODE_CACHED);
+	if (ret != SDS_OK) {
+		INFO("SDS CPU num elements read failed, error: %d\n", ret);
+		list->num_entries = 0;
+		return;
+	}
+
+	if (list->num_entries > PLATFORM_CORE_COUNT) {
+		ERROR("Isolated CPU list count %ld greater than max"
+		      " number supported %d\n",
+		      list->num_entries, PLATFORM_CORE_COUNT);
+		panic();
+	} else if (list->num_entries == 0) {
+		INFO("SDS isolated CPU list is empty\n");
+		return;
+	}
+
+	ret = sds_struct_read(SDS_ISOLATED_CPU_LIST_ID,
+			sizeof(list->num_entries),
+			&list->mpid_list,
+			sizeof(list->mpid_list[0]) * list->num_entries,
+			SDS_ACCESS_MODE_CACHED);
+	if (ret != SDS_OK) {
+		ERROR("SDS CPU list read failed. error: %d\n", ret);
+		panic();
+	}
+}
+
 /*******************************************************************************
  * This function inserts Platform information via device tree nodes as,
  * system-id {
  *    platform-id = <0>;
  *    config-id = <0>;
+ *    isolated-cpu-list = <0>
  * }
  ******************************************************************************/
 static int plat_sgi_append_config_node(void)
@@ -27,6 +79,7 @@
 	void *fdt;
 	int nodeoffset, err;
 	unsigned int platid = 0, platcfg = 0;
+	struct isolated_cpu_mpid_list cpu_mpid_list = {0};
 
 	mem_params = get_bl_mem_params_node(NT_FW_CONFIG_ID);
 	if (mem_params == NULL) {
@@ -69,6 +122,18 @@
 		return -1;
 	}
 
+	plat_arm_sgi_get_isolated_cpu_list(&cpu_mpid_list);
+	if (cpu_mpid_list.num_entries > 0) {
+		err = fdt_setprop(fdt, nodeoffset, "isolated-cpu-list",
+				&cpu_mpid_list,
+				(sizeof(cpu_mpid_list.num_entries) *
+				(cpu_mpid_list.num_entries + 1)));
+		if (err < 0) {
+			ERROR("Failed to set isolated-cpu-list, error: %d\n",
+			      err);
+		}
+	}
+
 	flush_dcache_range((uintptr_t)fdt, mem_params->image_info.image_size);
 
 	return 0;

diff --git a/plat/arm/css/sgi/sgi_plat.c b/plat/arm/css/sgi/sgi_plat.c
index 20c52e9..a0199c3 100644
--- a/plat/arm/css/sgi/sgi_plat.c
+++ b/plat/arm/css/sgi/sgi_plat.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2021, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -89,6 +89,7 @@
 const mmap_region_t plat_arm_secure_partition_mmap[] = {
 	PLAT_ARM_SECURE_MAP_SYSTEMREG,
 	PLAT_ARM_SECURE_MAP_NOR2,
+	SOC_PLATFORM_SECURE_UART,
 	PLAT_ARM_SECURE_MAP_DEVICE,
 	ARM_SP_IMAGE_MMAP,
 	ARM_SP_IMAGE_NS_BUF_MMAP,

diff --git a/plat/arm/css/sgi/sgi_plat_v2.c b/plat/arm/css/sgi/sgi_plat_v2.c
index 1a2a966..cef5345 100644
--- a/plat/arm/css/sgi/sgi_plat_v2.c
+++ b/plat/arm/css/sgi/sgi_plat_v2.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -83,6 +83,7 @@
 const mmap_region_t plat_arm_secure_partition_mmap[] = {
 	PLAT_ARM_SECURE_MAP_SYSTEMREG,
 	PLAT_ARM_SECURE_MAP_NOR2,
+	SOC_PLATFORM_SECURE_UART,
 	SOC_PLATFORM_PERIPH_MAP_DEVICE_USER,
 	ARM_SP_IMAGE_MMAP,
 	ARM_SP_IMAGE_NS_BUF_MMAP,

diff --git a/plat/arm/css/sgm/aarch64/css_sgm_helpers.S b/plat/arm/css/sgm/aarch64/css_sgm_helpers.S
deleted file mode 100644
index 32ca1bb..0000000
--- a/plat/arm/css/sgm/aarch64/css_sgm_helpers.S
+++ /dev/null

@@ -1,82 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <arch.h>
-#include <asm_macros.S>
-#include <platform_def.h>
-#include <cortex_a75.h>
-#include <cortex_a55.h>
-#include <cpu_macros.S>
-
-	.globl	plat_arm_calc_core_pos
-	.globl	plat_reset_handler
-
-	/* ---------------------------------------------------------------------
-	 * unsigned int plat_arm_calc_core_pos(u_register_t mpidr)
-	 *
-	 * Function to calculate the core position on FVP.
-	 *
-	 * (ClusterId * MAX_CPUS_PER_CLUSTER * MAX_PE_PER_CPU) +
-	 * (CPUId * MAX_PE_PER_CPU) +
-	 * ThreadId
-	 *
-	 * which can be simplified as:
-	 *
-	 * ((ClusterId * MAX_CPUS_PER_CLUSTER + CPUId) * MAX_PE_PER_CPU)
-	 * + ThreadId
-	 * ---------------------------------------------------------------------
-	 */
-func plat_arm_calc_core_pos
-	/*
-	 * Check for MT bit in MPIDR. If not set, shift MPIDR to left to make it
-	 * look as if in a multi-threaded implementation.
-	 */
-	tst	x0, #MPIDR_MT_MASK
-	lsr	x3, x0, #MPIDR_AFFINITY_BITS
-	csel	x3, x3, x0, eq
-
-	/* Extract individual affinity fields from MPIDR */
-	ubfx	x0, x3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
-	ubfx	x1, x3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
-	ubfx	x2, x3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
-
-	/* Compute linear position */
-	mov	x4, #PLAT_MAX_CPUS_PER_CLUSTER
-	madd	x1, x2, x4, x1
-	mov	x5, #PLAT_MAX_PE_PER_CPU
-	madd	x0, x1, x5, x0
-	ret
-endfunc plat_arm_calc_core_pos
-
-	/* -----------------------------------------------------
-	 * void plat_reset_handler(void);
-	 *
-	 * Determine the CPU MIDR and disable power down bit for
-	 * that CPU.
-	 * -----------------------------------------------------
-	 */
-func plat_reset_handler
-	jump_if_cpu_midr CORTEX_A75_MIDR, A75
-	jump_if_cpu_midr CORTEX_A55_MIDR, A55
-	ret
-
-	/* -----------------------------------------------------
-	 * Disable CPU power down bit in power control register
-	 * -----------------------------------------------------
-	 */
-A75:
-	mrs	x0, CORTEX_A75_CPUPWRCTLR_EL1
-	bic	x0, x0, #CORTEX_A75_CORE_PWRDN_EN_MASK
-	msr	CORTEX_A75_CPUPWRCTLR_EL1, x0
-	isb
-	ret
-A55:
-	mrs	x0, CORTEX_A55_CPUPWRCTLR_EL1
-	bic	x0, x0, #CORTEX_A55_CORE_PWRDN_EN_MASK
-	msr	CORTEX_A55_CPUPWRCTLR_EL1, x0
-	isb
-	ret
-endfunc plat_reset_handler

diff --git a/plat/arm/css/sgm/fdts/sgm_tb_fw_config.dts b/plat/arm/css/sgm/fdts/sgm_tb_fw_config.dts
deleted file mode 100644
index 54b2423..0000000
--- a/plat/arm/css/sgm/fdts/sgm_tb_fw_config.dts
+++ /dev/null

@@ -1,27 +0,0 @@
-/*
- * Copyright (c) 2020, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-/dts-v1/;
-
-/ {
-	/* Platform Config */
-	tb_fw-config {
-		compatible = "arm,tb_fw";
-		hw_config_addr = <0x0 0x83000000>;
-		hw_config_max_size = <0x01000000>;
-		/*
-		 * The following two entries are placeholders for Mbed TLS
-		 * heap information. The default values don't matter since
-		 * they will be overwritten by BL1.
-		 * In case of having shared Mbed TLS heap between BL1 and BL2,
-		 * BL1 will populate these two properties with the respective
-		 * info about the shared heap. This info will be available for
-		 * BL2 in order to locate and re-use the heap.
-		 */
-		mbedtls_heap_addr = <0x0 0x0>;
-		mbedtls_heap_size = <0x0>;
-	};
-};

diff --git a/plat/arm/css/sgm/include/plat_macros.S b/plat/arm/css/sgm/include/plat_macros.S
deleted file mode 100644
index 715ded2..0000000
--- a/plat/arm/css/sgm/include/plat_macros.S
+++ /dev/null

@@ -1,24 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-#ifndef PLAT_MACROS_S
-#define PLAT_MACROS_S
-
-#include <cci_macros.S>
-#include <css_macros.S>
-
-/* ---------------------------------------------
- * The below required platform porting macro
- * prints out relevant platform registers
- * whenever an unhandled exception is taken in
- * BL31.
- * ---------------------------------------------
- */
-.macro plat_crash_print_regs
-css_print_gic_regs
-print_cci_regs
-.endm
-
-#endif /* PLAT_MACROS_S */

diff --git a/plat/arm/css/sgm/include/sgm_base_platform_def.h b/plat/arm/css/sgm/include/sgm_base_platform_def.h
deleted file mode 100644
index 2d8e677..0000000
--- a/plat/arm/css/sgm/include/sgm_base_platform_def.h
+++ /dev/null

@@ -1,242 +0,0 @@
-/*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef SGM_BASE_PLATFORM_DEF_H
-#define SGM_BASE_PLATFORM_DEF_H
-
-#include <drivers/arm/tzc400.h>
-#include <drivers/arm/tzc_common.h>
-#include <plat/arm/board/common/board_css_def.h>
-#include <plat/arm/board/common/v2m_def.h>
-#include <plat/arm/common/arm_def.h>
-#include <plat/arm/css/common/css_def.h>
-#include <plat/arm/soc/common/soc_css_def.h>
-#include <plat/common/common_def.h>
-
-/* CPU topology */
-#define PLAT_ARM_CLUSTER_COUNT		U(1)
-#define PLAT_ARM_CLUSTER_CORE_COUNT	U(8)
-#define PLATFORM_CORE_COUNT		PLAT_ARM_CLUSTER_CORE_COUNT
-
-#define PLAT_MAX_PWR_LVL		ARM_PWR_LVL2
-#define PLAT_NUM_PWR_DOMAINS		(ARM_SYSTEM_COUNT + \
-					PLAT_ARM_CLUSTER_COUNT + \
-					PLATFORM_CORE_COUNT)
-
-/*
- * Define a list of Group 1 Secure and Group 0 interrupts as per GICv3
- * terminology. On a GICv2 system or mode, the lists will be merged and treated
- * as Group 0 interrupts.
- */
-#define PLAT_ARM_G1S_IRQ_PROPS(grp) \
-			CSS_G1S_IRQ_PROPS(grp), \
-			ARM_G1S_IRQ_PROPS(grp)
-
-#define PLAT_ARM_G0_IRQ_PROPS(grp)	ARM_G0_IRQ_PROPS(grp)
-
-/* GIC related constants */
-#define PLAT_ARM_GICD_BASE		0x30000000
-#define PLAT_ARM_GICR_BASE		0x300C0000
-#define PLAT_ARM_GICC_BASE		0x2c000000
-
-#define CSS_GIC_SIZE			0x00200000
-
-#define CSS_MAP_GIC_DEVICE		MAP_REGION_FLAT(	\
-					PLAT_ARM_GICD_BASE,	\
-					CSS_GIC_SIZE,		\
-					MT_DEVICE | MT_RW | MT_SECURE)
-
-/* Platform ID address */
-#define SSC_VERSION                     (SSC_REG_BASE + SSC_VERSION_OFFSET)
-#ifndef __ASSEMBLER__
-/* SSC_VERSION related accessors */
-/* Returns the part number of the platform */
-#define GET_PLAT_PART_NUM                                       \
-		GET_SSC_VERSION_PART_NUM(mmio_read_32(SSC_VERSION))
-/* Returns the configuration number of the platform */
-#define GET_PLAT_CONFIG_NUM                                     \
-		GET_SSC_VERSION_CONFIG(mmio_read_32(SSC_VERSION))
-#endif /* __ASSEMBLER__ */
-
-
-/*************************************************************************
- * Definitions common to all SGM CSS based platforms
- *************************************************************************/
-
-/* TZC-400 related constants */
-#define PLAT_ARM_TZC_BASE		0x2a500000
-#define TZC_NSAID_ALL_AP		0  /* Note: Same as default NSAID!! */
-#define TZC_NSAID_HDLCD0		2
-#define TZC_NSAID_HDLCD1		3
-#define TZC_NSAID_GPU			9
-#define TZC_NSAID_VIDEO			10
-#define TZC_NSAID_DISP0			11
-#define TZC_NSAID_DISP1			12
-
-
-/*************************************************************************
- * Required platform porting definitions common to all SGM CSS based
- * platforms
- *************************************************************************/
-
-#define PLAT_ARM_TRUSTED_SRAM_SIZE	0x00040000	/* 256 KB */
-
-/* MHU related constants */
-#define PLAT_CSS_MHU_BASE		0x2b1f0000
-
-#define PLAT_ARM_TRUSTED_ROM_BASE	0x00000000
-#define PLAT_ARM_TRUSTED_ROM_SIZE	0x00080000
-
-#define PLAT_ARM_CCI_BASE		0x2a000000
-
-/* Cluster to CCI slave mapping */
-#define PLAT_ARM_CCI_CLUSTER0_SL_IFACE_IX	6
-#define PLAT_ARM_CCI_CLUSTER1_SL_IFACE_IX	5
-
-/* System timer related constants */
-#define PLAT_ARM_NSTIMER_FRAME_ID	0
-
-/* TZC related constants */
-#define PLAT_ARM_TZC_NS_DEV_ACCESS	(				\
-		TZC_REGION_ACCESS_RDWR(TZC_NSAID_ALL_AP)	|	\
-		TZC_REGION_ACCESS_RDWR(TZC_NSAID_HDLCD0)	|	\
-		TZC_REGION_ACCESS_RDWR(TZC_NSAID_HDLCD1)	|	\
-		TZC_REGION_ACCESS_RDWR(TZC_NSAID_GPU)	|	\
-		TZC_REGION_ACCESS_RDWR(TZC_NSAID_VIDEO)	|	\
-		TZC_REGION_ACCESS_RDWR(TZC_NSAID_DISP0)	|	\
-		TZC_REGION_ACCESS_RDWR(TZC_NSAID_DISP1))
-
-/* Display Processor register definitions to setup the NSAIDs */
-#define MALI_DP_BASE           0x2cc00000
-#define DP_NPROT_NSAID_OFFSET     0x1000c
-#define W_NPROT_NSAID_SHIFT            24
-#define LS_NPORT_NSAID_SHIFT           12
-
-/*
- * Base address of the first memory region used for communication between AP
- * and SCP. Used by the BootOverMHU and SCPI protocols.
- */
-#if !CSS_USE_SCMI_SDS_DRIVER
-/*
- * Note that this is located at the same address as SCP_BOOT_CFG_ADDR, which
- * means the SCP/AP configuration data gets overwritten when the AP initiates
- * communication with the SCP. The configuration data is expected to be a
- * 32-bit word on all CSS platforms. Part of this configuration is
- * which CPU is the primary, according to the shift and mask definitions below.
- */
-#define PLAT_CSS_SCP_COM_SHARED_MEM_BASE	(ARM_TRUSTED_SRAM_BASE + 0x80)
-#define PLAT_CSS_PRIMARY_CPU_SHIFT		8
-#define PLAT_CSS_PRIMARY_CPU_BIT_WIDTH		4
-#endif
-
-/*
- * SCP_BL2 uses up whatever remaining space is available as it is loaded before
- * anything else in this memory region and is handed over to the SCP before
- * BL31 is loaded over the top.
- */
-#define PLAT_CSS_MAX_SCP_BL2_SIZE \
-	((SCP_BL2_LIMIT - ARM_FW_CONFIG_LIMIT) & ~PAGE_SIZE_MASK)
-
-#define PLAT_CSS_MAX_SCP_BL2U_SIZE	PLAT_CSS_MAX_SCP_BL2_SIZE
-
-/*
- * Most platform porting definitions provided by included headers
- */
-
-/*
- * PLAT_ARM_MMAP_ENTRIES depends on the number of entries in the
- * plat_arm_mmap array defined for each BL stage.
- */
-#if defined(IMAGE_BL31)
-# define PLAT_ARM_MMAP_ENTRIES		8
-# define MAX_XLAT_TABLES		5
-#elif defined(IMAGE_BL32)
-# define PLAT_ARM_MMAP_ENTRIES		8
-# define MAX_XLAT_TABLES		5
-#elif !USE_ROMLIB
-# define PLAT_ARM_MMAP_ENTRIES		11
-# define MAX_XLAT_TABLES		5
-#else
-# define PLAT_ARM_MMAP_ENTRIES		12
-# define MAX_XLAT_TABLES		6
-#endif
-
-/*
- * PLAT_ARM_MAX_BL1_RW_SIZE is calculated using the current BL1 RW debug size
- * plus a little space for growth.
- */
-#define PLAT_ARM_MAX_BL1_RW_SIZE	0xB000
-
-/*
- * PLAT_ARM_MAX_ROMLIB_RW_SIZE is define to use a full page
- */
-
-#if USE_ROMLIB
-#define PLAT_ARM_MAX_ROMLIB_RW_SIZE	0x1000
-#define PLAT_ARM_MAX_ROMLIB_RO_SIZE	0xe000
-#else
-#define PLAT_ARM_MAX_ROMLIB_RW_SIZE	0
-#define PLAT_ARM_MAX_ROMLIB_RO_SIZE	0
-#endif
-
-/*
- * PLAT_ARM_MAX_BL2_SIZE is calculated using the current BL2 debug size plus a
- * little space for growth.
- */
-#if TRUSTED_BOARD_BOOT
-# define PLAT_ARM_MAX_BL2_SIZE		0x1D000
-#else
-# define PLAT_ARM_MAX_BL2_SIZE		0x12000
-#endif
-
-/*
- * Since BL31 NOBITS overlays BL2 and BL1-RW, PLAT_ARM_MAX_BL31_SIZE is
- * calculated using the current BL31 PROGBITS debug size plus the sizes of
- * BL2 and BL1-RW
- */
-#define PLAT_ARM_MAX_BL31_SIZE		0x3B000
-
-/*
- * Size of cacheable stacks
- */
-#if defined(IMAGE_BL1)
-# if TRUSTED_BOARD_BOOT
-#  define PLATFORM_STACK_SIZE 0x1000
-# else
-#  define PLATFORM_STACK_SIZE 0x440
-# endif
-#elif defined(IMAGE_BL2)
-# if TRUSTED_BOARD_BOOT
-#  define PLATFORM_STACK_SIZE 0x1000
-# else
-#  define PLATFORM_STACK_SIZE 0x400
-# endif
-#elif defined(IMAGE_BL2U)
-# define PLATFORM_STACK_SIZE 0x400
-#elif defined(IMAGE_BL31)
-# define PLATFORM_STACK_SIZE 0x400
-#elif defined(IMAGE_BL32)
-# define PLATFORM_STACK_SIZE 0x440
-#endif
-
-/*******************************************************************************
- * Memprotect definitions
- ******************************************************************************/
-/* PSCI memory protect definitions:
- * This variable is stored in a non-secure flash because some ARM reference
- * platforms do not have secure NVRAM. Real systems that provided MEM_PROTECT
- * support must use a secure NVRAM to store the PSCI MEM_PROTECT definitions.
- */
-#define PLAT_ARM_MEM_PROT_ADDR		(V2M_FLASH0_BASE + \
-					 V2M_FLASH0_SIZE - V2M_FLASH_BLOCK_SIZE)
-
-/* System power domain level */
-#define CSS_SYSTEM_PWR_DMN_LVL		ARM_PWR_LVL2
-
-/* Number of SCMI channels on the platform */
-#define PLAT_ARM_SCMI_CHANNEL_COUNT	U(1)
-
-#endif /* SGM_BASE_PLATFORM_DEF_H */

diff --git a/plat/arm/css/sgm/include/sgm_plat_config.h b/plat/arm/css/sgm/include/sgm_plat_config.h
deleted file mode 100644
index 29b98d4..0000000
--- a/plat/arm/css/sgm/include/sgm_plat_config.h
+++ /dev/null

@@ -1,43 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef SGM_PLAT_CONFIG_H
-#define SGM_PLAT_CONFIG_H
-
-#include <drivers/arm/ccn.h>
-#include <drivers/arm/gicv3.h>
-
-/* The type of interconnect */
-typedef enum {
-	ARM_CCI = 0,
-	ARM_CCN,
-	ARM_CMN
-} css_inteconn_type_t;
-
-typedef ccn_desc_t inteconn_desc_t;
-
-/* Interconnect configurations */
-typedef struct css_inteconn_config {
-	css_inteconn_type_t ip_type;
-	const inteconn_desc_t *plat_inteconn_desc;
-} css_inteconn_config_t;
-
-/* Topology configurations */
-typedef struct css_topology {
-	const unsigned char *power_tree;
-	unsigned int plat_cluster_core_count;
-} css_topology_t;
-
-typedef struct css_plat_config {
-	const gicv3_driver_data_t *gic_data;
-	const css_inteconn_config_t *inteconn;
-	const css_topology_t *topology;
-} css_plat_config_t;
-
-void plat_config_init(void);
-css_plat_config_t *get_plat_config(void);
-
-#endif /* SGM_PLAT_CONFIG_H */

diff --git a/plat/arm/css/sgm/include/sgm_variant.h b/plat/arm/css/sgm/include/sgm_variant.h
deleted file mode 100644
index 859ddb5..0000000
--- a/plat/arm/css/sgm/include/sgm_variant.h
+++ /dev/null

@@ -1,17 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef SGM_VARIANT_H
-#define SGM_VARIANT_H
-
-/* SSC_VERSION values for sgm */
-#define SGM775_SSC_VER_PART_NUM		0x0790
-
-/* DMC configuration for sgm */
-#define SGM_DMC_SIZE			0x40000
-#define SGM775_DMC_COUNT		4
-
-#endif /* SGM_VARIANT_H */

diff --git a/plat/arm/css/sgm/sgm-common.mk b/plat/arm/css/sgm/sgm-common.mk
deleted file mode 100644
index 5b954f8..0000000
--- a/plat/arm/css/sgm/sgm-common.mk
+++ /dev/null

@@ -1,76 +0,0 @@
-#
-# Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
-#
-# SPDX-License-Identifier: BSD-3-Clause
-#
-
-CSS_USE_SCMI_SDS_DRIVER	:=	1
-
-CSS_SGM_BASE		:=	plat/arm/css/sgm
-
-PLAT_INCLUDES		:=	-I${CSS_SGM_BASE}/include
-
-PLAT_BL_COMMON_SOURCES	:=	${CSS_SGM_BASE}/sgm_mmap_config.c	\
-				${CSS_SGM_BASE}/aarch64/css_sgm_helpers.S
-
-SECURITY_SOURCES	:=	drivers/arm/tzc/tzc_dmc500.c		\
-				plat/arm/common/arm_tzc_dmc500.c	\
-				${CSS_SGM_BASE}/sgm_security.c
-
-SGM_CPU_SOURCES		:=	lib/cpus/aarch64/cortex_a55.S		\
-				lib/cpus/aarch64/cortex_a75.S
-
-INTERCONNECT_SOURCES	:=	${CSS_SGM_BASE}/sgm_interconnect.c
-
-# GIC-600 configuration
-GICV3_SUPPORT_GIC600	:=	1
-
-# Include GICv3 driver files
-include drivers/arm/gic/v3/gicv3.mk
-
-SGM_GIC_SOURCES		:=	${GICV3_SOURCES}			\
-				plat/common/plat_gicv3.c		\
-				plat/arm/common/arm_gicv3.c
-
-BL1_SOURCES		+=	$(SGM_CPU_SOURCES)			\
-				${INTERCONNECT_SOURCES}			\
-				${CSS_SGM_BASE}/sgm_bl1_setup.c		\
-				${CSS_SGM_BASE}/sgm_plat_config.c	\
-				drivers/arm/sp805/sp805.c
-
-BL2_SOURCES		+=	${SECURITY_SOURCES}			\
-				${CSS_SGM_BASE}/sgm_plat_config.c
-
-BL2U_SOURCES		+=	${SECURITY_SOURCES}
-
-BL31_SOURCES		+=	$(SGM_CPU_SOURCES)			\
-				${INTERCONNECT_SOURCES}			\
-				${SECURITY_SOURCES}			\
-				${SGM_GIC_SOURCES}			\
-				${CSS_SGM_BASE}/sgm_topology.c		\
-				${CSS_SGM_BASE}/sgm_bl31_setup.c	\
-				${CSS_SGM_BASE}/sgm_plat_config.c
-
-ifneq (${RESET_TO_BL31},0)
-  $(error "Using BL31 as the reset vector is not supported on ${PLAT} platform. \
-  Please set RESET_TO_BL31 to 0.")
-endif
-
-# sgm uses CCI-500 as Cache Coherent Interconnect
-ARM_CCI_PRODUCT_ID	:=	500
-
-# System coherency is managed in hardware
-HW_ASSISTED_COHERENCY	:=	1
-
-# When building for systems with hardware-assisted coherency, there's no need to
-# use USE_COHERENT_MEM. Require that USE_COHERENT_MEM must be set to 0 too.
-USE_COHERENT_MEM	:=	0
-
-override ARM_PLAT_MT	:=	1
-
-$(eval $(call add_define,SGM_PLAT))
-
-include plat/arm/common/arm_common.mk
-include plat/arm/board/common/board_common.mk
-include plat/arm/css/common/css_common.mk
-include plat/arm/soc/common/soc_css.mk

diff --git a/plat/arm/css/sgm/sgm_bl1_setup.c b/plat/arm/css/sgm/sgm_bl1_setup.c
deleted file mode 100644
index 5fd9655..0000000
--- a/plat/arm/css/sgm/sgm_bl1_setup.c
+++ /dev/null

@@ -1,45 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <common/bl_common.h>
-#include <common/debug.h>
-#include <plat/arm/common/plat_arm.h>
-#include <plat/arm/soc/common/soc_css.h>
-#include <plat/arm/common/arm_def.h>
-#include <drivers/arm/sp805.h>
-#include <sgm_plat_config.h>
-
-void bl1_early_platform_setup(void)
-{
-
-	/* Initialize the console before anything else */
-	arm_bl1_early_platform_setup();
-
-	/* Initialize the platform configuration structure */
-	plat_config_init();
-
-#if !HW_ASSISTED_COHERENCY
-	/*
-	 * Initialize Interconnect for this cluster during cold boot.
-	 * No need for locks as no other CPU is active.
-	 */
-	plat_arm_interconnect_init();
-	/*
-	 * Enable Interconnect coherency for the primary CPU's cluster.
-	 */
-	plat_arm_interconnect_enter_coherency();
-#endif
-}
-
-void plat_arm_secure_wdt_start(void)
-{
-	sp805_start(ARM_SP805_TWDG_BASE, ARM_TWDG_LOAD_VAL);
-}
-
-void plat_arm_secure_wdt_stop(void)
-{
-	sp805_stop(ARM_SP805_TWDG_BASE);
-}

diff --git a/plat/arm/css/sgm/sgm_bl31_setup.c b/plat/arm/css/sgm/sgm_bl31_setup.c
deleted file mode 100644
index 907e9fd..0000000
--- a/plat/arm/css/sgm/sgm_bl31_setup.c
+++ /dev/null

@@ -1,55 +0,0 @@
-/*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <common/bl_common.h>
-#include <common/debug.h>
-#include <drivers/arm/css/css_mhu_doorbell.h>
-#include <drivers/arm/css/scmi.h>
-#include <plat/arm/common/plat_arm.h>
-
-#include <sgm_plat_config.h>
-
-static scmi_channel_plat_info_t sgm775_scmi_plat_info = {
-		.scmi_mbx_mem = CSS_SCMI_PAYLOAD_BASE,
-		.db_reg_addr = PLAT_CSS_MHU_BASE + CSS_SCMI_MHU_DB_REG_OFF,
-		.db_preserve_mask = 0xfffffffe,
-		.db_modify_mask = 0x1,
-		.ring_doorbell = &mhu_ring_doorbell,
-};
-
-scmi_channel_plat_info_t *plat_css_get_scmi_info(int channel_id)
-{
-	return &sgm775_scmi_plat_info;
-}
-
-void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
-				u_register_t arg2, u_register_t arg3)
-{
-	uint32_t plat_version;
-	bl_params_node_t *bl_params;
-
-	bl_params = ((bl_params_t *)arg0)->head;
-
-	/* Initialize the platform configuration structure */
-	plat_config_init();
-
-	while (bl_params) {
-		if (bl_params->image_id == BL33_IMAGE_ID) {
-			plat_version = mmio_read_32(SSC_VERSION);
-			bl_params->ep_info->args.arg2 = plat_version;
-			break;
-		}
-
-		bl_params = bl_params->next_params_info;
-	}
-
-	arm_bl31_early_platform_setup((void *)arg0, arg1, arg2, (void *)arg3);
-}
-
-const plat_psci_ops_t *plat_arm_psci_override_pm_ops(plat_psci_ops_t *ops)
-{
-	return css_scmi_override_pm_ops(ops);
-}

diff --git a/plat/arm/css/sgm/sgm_interconnect.c b/plat/arm/css/sgm/sgm_interconnect.c
deleted file mode 100644
index 5b45341..0000000
--- a/plat/arm/css/sgm/sgm_interconnect.c
+++ /dev/null

@@ -1,34 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <cdefs.h>
-
-/*
- * As the SGM platform supports FCM (with automatic interconnect
- * enter/exit), we should not do anything in these interface functions.
- * They are used to override the weak functions in cci drivers.
- */
-
-/******************************************************************************
- * Helper function to initialize ARM interconnect driver.
- *****************************************************************************/
-void __init plat_arm_interconnect_init(void)
-{
-}
-
-/******************************************************************************
- * Helper function to place current master into coherency
- *****************************************************************************/
-void plat_arm_interconnect_enter_coherency(void)
-{
-}
-
-/******************************************************************************
- * Helper function to remove current master from coherency
- *****************************************************************************/
-void plat_arm_interconnect_exit_coherency(void)
-{
-}

diff --git a/plat/arm/css/sgm/sgm_mmap_config.c b/plat/arm/css/sgm/sgm_mmap_config.c
deleted file mode 100644
index e5b4b03..0000000
--- a/plat/arm/css/sgm/sgm_mmap_config.c
+++ /dev/null

@@ -1,89 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <platform_def.h>
-
-#include <common/bl_common.h>
-#include <common/debug.h>
-#include <plat/arm/common/plat_arm.h>
-
-#include <sgm_variant.h>
-
-/*
- * Table of regions for different BL stages to map using the MMU.
- * This doesn't include Trusted RAM as the 'mem_layout' argument passed to
- * arm_configure_mmu_elx() will give the available subset of that.
- */
-#if IMAGE_BL1
-const mmap_region_t plat_arm_mmap[] = {
-	ARM_MAP_SHARED_RAM,
-	V2M_MAP_FLASH0_RO,
-	V2M_MAP_IOFPGA,
-	CSS_MAP_DEVICE,
-	CSS_MAP_GIC_DEVICE,
-	SOC_CSS_MAP_DEVICE,
-#if TRUSTED_BOARD_BOOT
-	ARM_MAP_NS_DRAM1,
-#endif
-	{0}
-};
-#endif
-#if IMAGE_BL2
-const mmap_region_t plat_arm_mmap[] = {
-	ARM_MAP_SHARED_RAM,
-	V2M_MAP_FLASH0_RO,
-	V2M_MAP_IOFPGA,
-	CSS_MAP_DEVICE,
-	CSS_MAP_GIC_DEVICE,
-	SOC_CSS_MAP_DEVICE,
-	ARM_MAP_NS_DRAM1,
-#ifdef SPD_tspd
-	ARM_MAP_TSP_SEC_MEM,
-#endif
-#ifdef SPD_opteed
-	ARM_OPTEE_PAGEABLE_LOAD_MEM,
-#endif
-#if TRUSTED_BOARD_BOOT && !BL2_AT_EL3
-	ARM_MAP_BL1_RW,
-#endif
-	{0}
-};
-#endif
-#if IMAGE_BL2U
-const mmap_region_t plat_arm_mmap[] = {
-	ARM_MAP_SHARED_RAM,
-	CSS_MAP_DEVICE,
-	CSS_MAP_GIC_DEVICE,
-	SOC_CSS_MAP_DEVICE,
-	{0}
-};
-#endif
-#if IMAGE_BL31
-const mmap_region_t plat_arm_mmap[] = {
-	ARM_MAP_SHARED_RAM,
-	V2M_MAP_IOFPGA,
-	CSS_MAP_DEVICE,
-	CSS_MAP_GIC_DEVICE,
-	SOC_CSS_MAP_DEVICE,
-	{0}
-};
-#endif
-#if IMAGE_BL32
-const mmap_region_t plat_arm_mmap[] = {
-	V2M_MAP_IOFPGA,
-	CSS_MAP_DEVICE,
-	CSS_MAP_GIC_DEVICE,
-	SOC_CSS_MAP_DEVICE,
-	{0}
-};
-#endif
-
-ARM_CASSERT_MMAP
-
-const mmap_region_t *plat_arm_get_mmap(void)
-{
-	return plat_arm_mmap;
-}

diff --git a/plat/arm/css/sgm/sgm_plat_config.c b/plat/arm/css/sgm/sgm_plat_config.c
deleted file mode 100644
index eed3631..0000000
--- a/plat/arm/css/sgm/sgm_plat_config.c
+++ /dev/null

@@ -1,80 +0,0 @@
-/*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <assert.h>
-#include <string.h>
-
-#include <platform_def.h>
-
-#include <common/debug.h>
-#include <plat/common/platform.h>
-#include <plat/arm/common/plat_arm.h>
-
-#include <sgm_plat_config.h>
-#include <sgm_variant.h>
-
-static css_plat_config_t *css_plat_info;
-
-/* Interconnect */
-const css_inteconn_config_t sgm_inteconn = {
-	.ip_type = ARM_CCI,
-	.plat_inteconn_desc = NULL
-};
-
-/* Special definition for SGM775 */
-/* Topology configuration for SGM775 */
-const unsigned char sgm775_power_domain_tree_desc[] = {
-	/* No of root nodes */
-	ARM_SYSTEM_COUNT,
-	/* No of children for the root node */
-	PLAT_ARM_CLUSTER_COUNT,
-	/* No of children for the first cluster node */
-	PLAT_ARM_CLUSTER_CORE_COUNT,
-};
-
-const css_topology_t sgm775_topology = {
-	.power_tree = sgm775_power_domain_tree_desc,
-	.plat_cluster_core_count = PLAT_ARM_CLUSTER_CORE_COUNT
-};
-
-/* Configuration structure for SGM775 */
-css_plat_config_t sgm775_config = {
-	.inteconn = &sgm_inteconn,
-	.topology = &sgm775_topology
-};
-
-/*******************************************************************************
- * This function initializes the platform structure.
- ******************************************************************************/
-void plat_config_init(void)
-{
-	/* Get the platform configurations */
-	switch (GET_PLAT_PART_NUM) {
-	case SGM775_SSC_VER_PART_NUM:
-		css_plat_info = &sgm775_config;
-
-		break;
-	default:
-		ERROR("Not a valid sgm variant!\n");
-		panic();
-	}
-}
-
-/*******************************************************************************
- * This function returns the platform structure pointer.
- ******************************************************************************/
-css_plat_config_t *get_plat_config(void)
-{
-	assert(css_plat_info != NULL);
-	return css_plat_info;
-}
-
-#if TRUSTED_BOARD_BOOT
-int plat_get_mbedtls_heap(void **heap_addr, size_t *heap_size)
-{
-	return get_mbedtls_heap_helper(heap_addr, heap_size);
-}
-#endif

diff --git a/plat/arm/css/sgm/sgm_security.c b/plat/arm/css/sgm/sgm_security.c
deleted file mode 100644
index 21d5306..0000000
--- a/plat/arm/css/sgm/sgm_security.c
+++ /dev/null

@@ -1,65 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <common/debug.h>
-#include <drivers/arm/tzc_dmc500.h>
-#include <plat/arm/common/plat_arm.h>
-#include <plat/arm/soc/common/soc_css.h>
-
-#include <sgm_variant.h>
-
-/* Is populated with the DMC-500 controllers base addresses */
-static tzc_dmc500_driver_data_t plat_driver_data;
-
-void plat_sgm_dp_security_setup(void)
-{
-	unsigned int nprot_nsaid;
-
-	/*
-	 * At reset the Mali display processors start with NSAIDs set to zero
-	 * so the firmware must set them up to the expected values for ARM sgm
-	 * platforms.
-	 */
-
-	nprot_nsaid = mmio_read_32(MALI_DP_BASE + DP_NPROT_NSAID_OFFSET);
-	nprot_nsaid &= ~((0xF << W_NPROT_NSAID_SHIFT) |
-			(0xF << LS_NPORT_NSAID_SHIFT));
-	nprot_nsaid |= ((TZC_NSAID_DISP1 << W_NPROT_NSAID_SHIFT) |
-			(TZC_NSAID_DISP0 << LS_NPORT_NSAID_SHIFT));
-	mmio_write_32(MALI_DP_BASE + DP_NPROT_NSAID_OFFSET, nprot_nsaid);
-}
-
-void plat_arm_security_setup(void)
-{
-	unsigned int i;
-	unsigned int part_num = GET_PLAT_PART_NUM;
-
-	INFO("part_num: 0x%x\n", part_num);
-
-	/*
-	 * Initialise plat_driver_data with platform specific DMC_BASE
-	 * addresses
-	 */
-	switch (part_num) {
-	case SGM775_SSC_VER_PART_NUM:
-		for (i = 0; i < SGM775_DMC_COUNT; i++)
-			plat_driver_data.dmc_base[i] = PLAT_ARM_TZC_BASE
-					+ SGM_DMC_SIZE * i;
-		plat_driver_data.dmc_count = SGM775_DMC_COUNT;
-		break;
-	default:
-		/* Unexpected platform */
-		ERROR("Unexpected platform\n");
-		panic();
-	}
-	/* Initialize the TrustZone Controller in DMC-500 */
-	arm_tzc_dmc500_setup(&plat_driver_data, NULL);
-
-	/* Do DP NSAID setup */
-	plat_sgm_dp_security_setup();
-	/* Do ARM CSS SoC security setup */
-	soc_css_security_setup();
-}

diff --git a/plat/arm/css/sgm/sgm_topology.c b/plat/arm/css/sgm/sgm_topology.c
deleted file mode 100644
index 2d9552d..0000000
--- a/plat/arm/css/sgm/sgm_topology.c
+++ /dev/null

@@ -1,33 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <plat/arm/common/plat_arm.h>
-
-#include <sgm_plat_config.h>
-
-/*******************************************************************************
- * This function returns the topology tree information.
- ******************************************************************************/
-const unsigned char *plat_get_power_domain_tree_desc(void)
-{
-	return get_plat_config()->topology->power_tree;
-}
-
-/*******************************************************************************
- * This function returns the core count within the cluster corresponding to
- * `mpidr`.
- ******************************************************************************/
-unsigned int plat_arm_get_cluster_core_count(u_register_t mpidr)
-{
-	return get_plat_config()->topology->plat_cluster_core_count;
-}
-
-/*
- * The array mapping platform core position (implemented by plat_my_core_pos())
- * to the SCMI power domain ID implemented by SCP.
- */
-const uint32_t plat_css_core_pos_to_scmi_dmn_id_map[PLATFORM_CORE_COUNT] = {
-			0, 1, 2, 3, 4, 5, 6, 7 };

diff --git a/plat/arm/css/sgm/tsp/sgm_tsp_setup.c b/plat/arm/css/sgm/tsp/sgm_tsp_setup.c
deleted file mode 100644
index 5f40c4c..0000000
--- a/plat/arm/css/sgm/tsp/sgm_tsp_setup.c
+++ /dev/null

@@ -1,17 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <plat/arm/common/plat_arm.h>
-
-#include <sgm_plat_config.h>
-
-void tsp_early_platform_setup(void)
-{
-	/* Initialize the platform configuration structure */
-	plat_config_init();
-
-	arm_tsp_early_platform_setup();
-}

diff --git a/plat/arm/css/sgm/tsp/tsp-sgm.mk b/plat/arm/css/sgm/tsp/tsp-sgm.mk
deleted file mode 100644
index de5e5b7..0000000
--- a/plat/arm/css/sgm/tsp/tsp-sgm.mk
+++ /dev/null

@@ -1,11 +0,0 @@
-#
-# Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
-#
-# SPDX-License-Identifier: BSD-3-Clause
-#
-
-BL32_SOURCES		+= 	${SGM_GIC_SOURCES}			\
-				${CSS_SGM_BASE}/sgm_plat_config.c	\
-				plat/arm/css/sgm/tsp/sgm_tsp_setup.c
-
-include plat/arm/common/tsp/arm_tsp.mk

diff --git a/plat/common/aarch64/plat_common.c b/plat/common/aarch64/plat_common.c
index 38a5786..e807660 100644
--- a/plat/common/aarch64/plat_common.c
+++ b/plat/common/aarch64/plat_common.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -13,7 +13,6 @@
 #if RAS_EXTENSION
 #include <lib/extensions/ras.h>
 #endif
-#include <lib/extensions/twed.h>
 #include <lib/xlat_tables/xlat_mmu_helpers.h>
 #include <plat/common/platform.h>
 
@@ -23,7 +22,6 @@
  * platforms but may also be overridden by a platform if required.
  */
 #pragma weak bl31_plat_runtime_setup
-#pragma weak plat_arm_set_twedel_scr_el3
 
 #if SDEI_SUPPORT
 #pragma weak plat_sdei_handle_masked_trigger
@@ -105,16 +103,3 @@
 #endif
 	panic();
 }
-
-/*******************************************************************************
- * In v8.6+ platforms with delayed trapping of WFE this hook sets the delay. It
- * is a weak function definition so can be overridden depending on the
- * requirements of a platform.  The only hook provided is for the TWED fields
- * in SCR_EL3, the TWED fields in HCR_EL2, SCTLR_EL2, and SCTLR_EL1 should be
- * configured as needed in lower exception levels.
- ******************************************************************************/
-
-uint32_t plat_arm_set_twedel_scr_el3(void)
-{
-	return TWED_DISABLED;
-}

diff --git a/plat/hisilicon/hikey/platform.mk b/plat/hisilicon/hikey/platform.mk
index 18197cf..3e1771c 100644
--- a/plat/hisilicon/hikey/platform.mk
+++ b/plat/hisilicon/hikey/platform.mk

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -95,6 +95,10 @@
 BL2_SOURCES		+=	lib/optee/optee_utils.c
 endif
 
+include lib/zlib/zlib.mk
+PLAT_INCLUDES		+=	-Ilib/zlib
+BL2_SOURCES		+=	$(ZLIB_SOURCES)
+
 HIKEY_GIC_SOURCES	:=	drivers/arm/gic/common/gic_common.c	\
 				drivers/arm/gic/v2/gicv2_main.c		\
 				drivers/arm/gic/v2/gicv2_helpers.c	\

diff --git a/plat/hisilicon/hikey960/platform.mk b/plat/hisilicon/hikey960/platform.mk
index fc2c209..608fe09 100644
--- a/plat/hisilicon/hikey960/platform.mk
+++ b/plat/hisilicon/hikey960/platform.mk

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -94,6 +94,10 @@
 BL2_SOURCES		+=	lib/optee/optee_utils.c
 endif
 
+include lib/zlib/zlib.mk
+PLAT_INCLUDES		+=	-Ilib/zlib
+BL2_SOURCES		+=	$(ZLIB_SOURCES)
+
 BL31_SOURCES		+=	drivers/arm/cci/cci.c			\
 				drivers/arm/pl061/pl061_gpio.c		\
 				drivers/gpio/gpio.c			\

diff --git a/plat/imx/imx8m/imx8m_caam.c b/plat/imx/imx8m/imx8m_caam.c
index 478005e..644572c 100644
--- a/plat/imx/imx8m/imx8m_caam.c
+++ b/plat/imx/imx8m/imx8m_caam.c

@@ -1,13 +1,16 @@
 /*
- * Copyright (c) 2019, NXP. All rights reserved.
+ * Copyright (c) 2019-2022 NXP. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <common/debug.h>
 #include <lib/mmio.h>
 
 #include <imx8m_caam.h>
 
+#define HAB_JR0_DID	U(0x8011)
+
 void imx8m_caam_init(void)
 {
 	uint32_t sm_cmd;
@@ -20,7 +23,12 @@
 	mmio_write_32(SM_CMD, sm_cmd);
 
 	/* config CAAM JRaMID set MID to Cortex A */
-	mmio_write_32(CAAM_JR0MID, CAAM_NS_MID);
+	if (mmio_read_32(CAAM_JR0MID) == HAB_JR0_DID) {
+		NOTICE("Do not release JR0 to NS as it can be used by HAB");
+	} else {
+		mmio_write_32(CAAM_JR0MID, CAAM_NS_MID);
+	}
+
 	mmio_write_32(CAAM_JR1MID, CAAM_NS_MID);
 	mmio_write_32(CAAM_JR2MID, CAAM_NS_MID);
 

diff --git a/plat/imx/imx8m/imx8m_csu.c b/plat/imx/imx8m/imx8m_csu.c
new file mode 100644
index 0000000..2b3a7d9
--- /dev/null
+++ b/plat/imx/imx8m/imx8m_csu.c

@@ -0,0 +1,56 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/mmio.h>
+
+#include <imx8m_csu.h>
+
+void imx_csu_init(const struct imx_csu_cfg *csu_cfg)
+{
+	const struct imx_csu_cfg *csu = csu_cfg;
+	uint32_t val;
+
+	while (csu->type != CSU_INVALID) {
+		switch (csu->type) {
+		case CSU_CSL:
+			val = mmio_read_32(CSLx_REG(csu->idx));
+			if (val & CSLx_LOCK(csu->idx)) {
+				break;
+			}
+			mmio_clrsetbits_32(CSLx_REG(csu->idx), CSLx_CFG(0xff, csu->idx),
+				CSLx_CFG(csu->csl_level | (csu->lock << 8), csu->idx));
+			break;
+		case CSU_HP:
+			val = mmio_read_32(CSU_HP_REG(csu->idx));
+			if (val & CSU_HP_LOCK(csu->idx)) {
+				break;
+			}
+			mmio_clrsetbits_32(CSU_HP_REG(csu->idx), CSU_HP_CFG(0x1, csu->idx),
+				CSU_HP_CFG(csu->hp | (csu->lock << 0x1), csu->idx));
+			break;
+		case CSU_SA:
+			val = mmio_read_32(CSU_SA_REG(csu->idx));
+			if (val & CSU_SA_LOCK(csu->idx)) {
+				break;
+			}
+			mmio_clrsetbits_32(CSU_SA_REG(csu->idx), CSU_SA_CFG(0x1, csu->idx),
+				CSU_SA_CFG(csu->sa | (csu->lock << 0x1), csu->idx));
+			break;
+		case CSU_HPCONTROL:
+			val = mmio_read_32(CSU_HPCONTROL_REG(csu->idx));
+			if (val & CSU_HPCONTROL_LOCK(csu->idx)) {
+				break;
+			}
+			mmio_clrsetbits_32(CSU_HPCONTROL_REG(csu->idx), CSU_HPCONTROL_CFG(0x1, csu->idx),
+				CSU_HPCONTROL_CFG(csu->hpctrl | (csu->lock << 0x1), csu->idx));
+			break;
+		default:
+			break;
+		}
+
+		csu++;
+	}
+}

diff --git a/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c b/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c
index 40110d7..debede1 100644
--- a/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2022 ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,7 +18,7 @@
 #include <drivers/generic_delay_timer.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/mmio.h>
-#include <lib/xlat_tables/xlat_tables.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
 #include <gpc.h>
@@ -26,8 +26,11 @@
 #include <imx_uart.h>
 #include <imx_rdc.h>
 #include <imx8m_caam.h>
+#include <imx8m_csu.h>
 #include <plat_imx8.h>
 
+#define TRUSTY_PARAMS_LEN_BYTES      (4096*2)
+
 static const mmap_region_t imx_mmap[] = {
 	MAP_REGION_FLAT(IMX_GIC_BASE, IMX_GIC_SIZE, MT_DEVICE | MT_RW),
 	MAP_REGION_FLAT(IMX_AIPS_BASE, IMX_AIPS_SIZE, MT_DEVICE | MT_RW), /* AIPS map */
@@ -44,9 +47,11 @@
 
 static const struct imx_rdc_cfg rdc[] = {
 	/* Master domain assignment */
-	RDC_MDAn(0x1, DID1),
+	RDC_MDAn(RDC_MDA_M4, DID1),
 
 	/* peripherals domain permission */
+	RDC_PDAPn(RDC_PDAP_UART4, D1R | D1W),
+	RDC_PDAPn(RDC_PDAP_UART2, D0R | D0W),
 
 	/* memory region */
 
@@ -54,6 +59,20 @@
 	{0},
 };
 
+static const struct imx_csu_cfg csu_cfg[] = {
+	/* peripherals csl setting */
+	CSU_CSLx(0x1, CSU_SEC_LEVEL_0, UNLOCKED),
+
+	/* master HP0~1 */
+
+	/* SA setting */
+
+	/* HP control setting */
+
+	/* Sentinel */
+	{0}
+};
+
 static entry_point_info_t bl32_image_ep_info;
 static entry_point_info_t bl33_image_ep_info;
 
@@ -109,6 +128,8 @@
 
 	imx_rdc_init(rdc);
 
+	imx_csu_init(csu_cfg);
+
 	imx8m_caam_init();
 
 	console_imx_uart_register(IMX_BOOT_UART_BASE, IMX_BOOT_UART_CLK_IN_HZ,
@@ -124,7 +145,7 @@
 	bl33_image_ep_info.spsr = get_spsr_for_bl33_entry();
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
 
-#ifdef SPD_opteed
+#if defined(SPD_opteed) || defined(SPD_trusty)
 	/* Populate entry point information for BL32 */
 	SET_PARAM_HEAD(&bl32_image_ep_info, PARAM_EP, VERSION_1, 0);
 	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
@@ -134,6 +155,16 @@
 	/* Pass TEE base and size to bl33 */
 	bl33_image_ep_info.args.arg1 = BL32_BASE;
 	bl33_image_ep_info.args.arg2 = BL32_SIZE;
+
+#ifdef SPD_trusty
+	bl32_image_ep_info.args.arg0 = BL32_SIZE;
+	bl32_image_ep_info.args.arg1 = BL32_BASE;
+#else
+	/* Make sure memory is clean */
+	mmio_write_32(BL32_FDT_OVERLAY_ADDR, 0);
+	bl33_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+	bl32_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+#endif
 #endif
 
 	bl31_tzc380_setup();
@@ -150,6 +181,9 @@
 		(BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE),
 		MT_DEVICE | MT_RW | MT_SECURE);
 #endif
+	/* Map TEE memory */
+	mmap_add_region(BL32_BASE, BL32_BASE, BL32_SIZE, MT_MEMORY | MT_RW);
+
 	mmap_add(imx_mmap);
 
 	init_xlat_tables();
@@ -184,3 +218,12 @@
 {
 	return COUNTER_FREQUENCY;
 }
+
+#ifdef SPD_trusty
+void plat_trusty_set_boot_args(aapcs64_params_t *args)
+{
+	args->arg0 = BL32_SIZE;
+	args->arg1 = BL32_BASE;
+	args->arg2 = TRUSTY_PARAMS_LEN_BYTES;
+}
+#endif

diff --git a/plat/imx/imx8m/imx8mm/include/imx_sec_def.h b/plat/imx/imx8m/imx8mm/include/imx_sec_def.h
new file mode 100644
index 0000000..6215983
--- /dev/null
+++ b/plat/imx/imx8m/imx8mm/include/imx_sec_def.h

@@ -0,0 +1,216 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_SEC_DEF_H
+#define IMX_SEC_DEF_H
+
+/* RDC MDA index */
+enum rdc_mda_idx {
+	RDC_MDA_A53 = 0,
+	RDC_MDA_M4 = 1,
+	RDC_MDA_PCIE_CTRL1 = 2,
+	RDC_MDA_SDMA3p = 3,
+	RDC_MDA_VPU_Decoders = 4,
+	RDC_MDA_LCDIF = 5,
+	RDC_MDA_CSI1 = 6,
+	RDC_MDA_SDMA3b = 7,
+	RDC_MDA_Coresight = 8,
+	RDC_MDA_DAP = 9,
+	RDC_MDA_CAAM = 10,
+	RDC_MDA_SDMA1p = 11,
+	RDC_MDA_SDMA1b = 12,
+	RDC_MDA_APBHDMA = 13,
+	RDC_MDA_NAND = 14,
+	RDC_MDA_uSDHC1 = 15,
+	RDC_MDA_uSDHC2 = 16,
+	RDC_MDA_uSDHC3 = 17,
+	RDC_MDA_GPU = 18,
+	RDC_MDA_USB1 = 19,
+	RDC_MDA_USB2 = 20,
+	RDC_MDA_TESTPORT = 21,
+	RDC_MDA_ENET1_TX = 22,
+	RDC_MDA_ENET1_RX = 23,
+	RDC_MDA_SDMA2p = 24,
+	RDC_MDA_SDMA2b = 24,
+	RDC_MDA_SDMA2_to_SPBA2 = 24,
+	RDC_MDA_SDMA3_to_SPBA2 = 25,
+	RDC_MDA_SDMA1_to_SPBA1 = 26,
+};
+
+/* RDC Peripherals index */
+enum rdc_pdap_idx {
+	RDC_PDAP_GPIO2 = 1,
+	RDC_PDAP_GPIO3 = 2,
+	RDC_PDAP_GPIO4 = 3,
+	RDC_PDAP_GPIO5 = 4,
+	RDC_PDAP_ANA_TSENSOR = 6,
+	RDC_PDAP_ANA_OSC = 7,
+	RDC_PDAP_WDOG1 = 8,
+	RDC_PDAP_WDOG2 = 9,
+	RDC_PDAP_WDOG3 = 10,
+	RDC_PDAP_SDMA3 = 11,
+	RDC_PDAP_SDMA2 = 12,
+	RDC_PDAP_GPT1 = 13,
+	RDC_PDAP_GPT2 = 14,
+	RDC_PDAP_GPT3 = 15,
+	RDC_PDAP_ROMCP = 17,
+	RDC_PDAP_IOMUXC = 19,
+	RDC_PDAP_IOMUXC_GPR = 20,
+	RDC_PDAP_OCOTP_CTRL = 21,
+	RDC_PDAP_ANA_PLL = 22,
+	RDC_PDAP_SNVS_HP = 23,
+	RDC_PDAP_CCM = 24,
+	RDC_PDAP_SRC = 25,
+	RDC_PDAP_GPC = 26,
+	RDC_PDAP_SEMAPHORE1 = 27,
+	RDC_PDAP_SEMAPHORE2 = 28,
+	RDC_PDAP_RDC = 29,
+	RDC_PDAP_CSU = 30,
+	RDC_PDAP_LCDIF = 32,
+	RDC_PDAP_MIPI_DSI = 33,
+	RDC_PDAP_CSI = 34,
+	RDC_PDAP_MIPI_CSI = 35,
+	RDC_PDAP_USB1 = 36,
+	RDC_PDAP_PWM1 = 38,
+	RDC_PDAP_PWM2 = 39,
+	RDC_PDAP_PWM3 = 40,
+	RDC_PDAP_PWM4 = 41,
+	RDC_PDAP_System_Counter_RD = 42,
+	RDC_PDAP_System_Counter_CMP = 43,
+	RDC_PDAP_System_Counter_CTRL = 44,
+	RDC_PDAP_GPT6 = 46,
+	RDC_PDAP_GPT5 = 47,
+	RDC_PDAP_GPT4 = 48,
+	RDC_PDAP_TZASC = 56,
+	RDC_PDAP_USB2 = 59,
+	RDC_PDAP_PERFMON1 = 60,
+	RDC_PDAP_PERFMON2 = 61,
+	RDC_PDAP_PLATFORM_CTRL = 62,
+	RDC_PDAP_QoSC = 63,
+	RDC_PDAP_I2C1 = 66,
+	RDC_PDAP_I2C2 = 67,
+	RDC_PDAP_I2C3 = 68,
+	RDC_PDAP_I2C4 = 69,
+	RDC_PDAP_UART4 = 70,
+	RDC_PDAP_MU_A = 74,
+	RDC_PDAP_MU_B = 75,
+	RDC_PDAP_SEMAPHORE_HS = 76,
+	RDC_PDAP_SAI1 = 78,
+	RDC_PDAP_SAI2 = 79,
+	RDC_PDAP_SAI3 = 80,
+	RDC_PDAP_SAI5 = 82,
+	RDC_PDAP_SAI6 = 83,
+	RDC_PDAP_uSDHC1 = 84,
+	RDC_PDAP_uSDHC2 = 85,
+	RDC_PDAP_uSDHC3 = 86,
+	RDC_PDAP_PCIE_PHY1 = 88,
+	RDC_PDAP_SPBA2 = 90,
+	RDC_PDAP_QSPI = 91,
+	RDC_PDAP_SDMA1 = 93,
+	RDC_PDAP_ENET1 = 94,
+	RDC_PDAP_SPDIF1 = 97,
+	RDC_PDAP_eCSPI1 = 98,
+	RDC_PDAP_eCSPI2 = 99,
+	RDC_PDAP_eCSPI3 = 100,
+	RDC_PDAP_MICFIL = 101,
+	RDC_PDAP_UART1 = 102,
+	RDC_PDAP_UART3 = 104,
+	RDC_PDAP_UART2 = 105,
+	RDC_PDAP_SPDIF2 = 106,
+	RDC_PDAP_SPBA1 = 111,
+	RDC_PDAP_CAAM = 114,
+};
+
+enum csu_csl_idx {
+	CSU_CSL_GPIO1 = 0,
+	CSU_CSL_GPIO2 = 1,
+	CSU_CSL_GPIO3 = 2,
+	CSU_CSL_GPIO4 = 3,
+	CSU_CSL_GPIO5 = 4,
+	CSU_CSL_ANA_TSENSOR = 6,
+	CSU_CSL_ANA_OSC = 7,
+	CSU_CSL_WDOG1 = 8,
+	CSU_CSL_WDOG2 = 9,
+	CSU_CSL_WDOG3 = 10,
+	CSU_CSL_SDMA2 = 12,
+	CSU_CSL_GPT1 = 13,
+	CSU_CSL_GPT2 = 14,
+	CSU_CSL_GPT3 = 15,
+	CSU_CSL_ROMCP = 17,
+	CSU_CSL_LCDIF = 18,
+	CSU_CSL_IOMUXC = 19,
+	CSU_CSL_IOMUXC_GPR = 20,
+	CSU_CSL_OCOTP_CTRL = 21,
+	CSU_CSL_ANA_PLL = 22,
+	CSU_CSL_SNVS_HP = 23,
+	CSU_CSL_CCM = 24,
+	CSU_CSL_SRC = 25,
+	CSU_CSL_GPC = 26,
+	CSU_CSL_SEMAPHORE1 = 27,
+	CSU_CSL_SEMAPHORE2 = 28,
+	CSU_CSL_RDC = 29,
+	CSU_CSL_CSU = 30,
+	CSU_CSL_DC_MST0 = 32,
+	CSU_CSL_DC_MST1 = 33,
+	CSU_CSL_DC_MST2 = 34,
+	CSU_CSL_DC_MST3 = 35,
+	CSU_CSL_PWM1 = 38,
+	CSU_CSL_PWM2 = 39,
+	CSU_CSL_PWM3 = 40,
+	CSU_CSL_PWM4 = 41,
+	CSU_CSL_System_Counter_RD = 42,
+	CSU_CSL_System_Counter_CMP = 43,
+	CSU_CSL_System_Counter_CTRL = 44,
+	CSU_CSL_GPT6 = 46,
+	CSU_CSL_GPT5 = 47,
+	CSU_CSL_GPT4 = 48,
+	CSU_CSL_TZASC = 56,
+	CSU_CSL_MTR = 59,
+	CSU_CSL_PERFMON1 = 60,
+	CSU_CSL_PERFMON2 = 61,
+	CSU_CSL_PLATFORM_CTRL = 62,
+	CSU_CSL_QoSC = 63,
+	CSU_CSL_MIPI_PHY = 64,
+	CSU_CSL_MIPI_DSI = 65,
+	CSU_CSL_I2C1 = 66,
+	CSU_CSL_I2C2 = 67,
+	CSU_CSL_I2C3 = 68,
+	CSU_CSL_I2C4 = 69,
+	CSU_CSL_UART4 = 70,
+	CSU_CSL_MIPI_CSI1 = 71,
+	CSU_CSL_MIPI_CSI_PHY1 = 72,
+	CSU_CSL_CSI1 = 73,
+	CSU_CSL_MU_A = 74,
+	CSU_CSL_MU_B = 75,
+	CSU_CSL_SEMAPHORE_HS = 76,
+	CSU_CSL_SAI1 = 78,
+	CSU_CSL_SAI6 = 80,
+	CSU_CSL_SAI5 = 81,
+	CSU_CSL_SAI4 = 82,
+	CSU_CSL_uSDHC1 = 84,
+	CSU_CSL_uSDHC2 = 85,
+	CSU_CSL_MIPI_CSI2 = 86,
+	CSU_CSL_MIPI_CSI_PHY2 = 87,
+	CSU_CSL_CSI2 = 88,
+	CSU_CSL_SPBA2 = 90,
+	CSU_CSL_QSPI = 91,
+	CSU_CSL_SDMA1 = 93,
+	CSU_CSL_ENET1 = 94,
+	CSU_CSL_SPDIF1 = 97,
+	CSU_CSL_eCSPI1 = 98,
+	CSU_CSL_eCSPI2 = 99,
+	CSU_CSL_eCSPI3 = 100,
+	CSU_CSL_UART1 = 102,
+	CSU_CSL_UART3 = 104,
+	CSU_CSL_UART2 = 105,
+	CSU_CSL_SPDIF2 = 106,
+	CSU_CSL_SAI2 = 107,
+	CSU_CSL_SAI3 = 108,
+	CSU_CSL_SPBA1 = 111,
+	CSU_CSL_CAAM = 114,
+};
+
+#endif /* IMX_SEC_DEF_H */

diff --git a/plat/imx/imx8m/imx8mm/include/platform_def.h b/plat/imx/imx8m/imx8mm/include/platform_def.h
index 300ef9e..ed693b9 100644
--- a/plat/imx/imx8m/imx8mm/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mm/include/platform_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -59,6 +59,8 @@
 #define PLAT_NS_IMAGE_OFFSET		U(0x40200000)
 #define PLAT_NS_IMAGE_SIZE		U(0x00200000)
 
+#define BL32_FDT_OVERLAY_ADDR		(PLAT_NS_IMAGE_OFFSET + 0x3000000)
+
 /* GICv3 base address */
 #define PLAT_GICD_BASE			U(0x38800000)
 #define PLAT_GICR_BASE			U(0x38880000)

diff --git a/plat/imx/imx8m/imx8mm/platform.mk b/plat/imx/imx8m/imx8mm/platform.mk
index cd8de89..60fa325 100644
--- a/plat/imx/imx8m/imx8mm/platform.mk
+++ b/plat/imx/imx8m/imx8mm/platform.mk

@@ -1,8 +1,11 @@
 #
-# Copyright (c) 2019-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2019-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
+#
+# Translation tables library
+include lib/xlat_tables_v2/xlat_tables.mk
 
 PLAT_INCLUDES		:=	-Iplat/imx/common/include		\
 				-Iplat/imx/imx8m/include		\
@@ -25,6 +28,7 @@
 				plat/imx/imx8m/gpc_common.c			\
 				plat/imx/imx8m/imx_aipstz.c			\
 				plat/imx/imx8m/imx_rdc.c			\
+				plat/imx/imx8m/imx8m_csu.c			\
 				plat/imx/imx8m/imx8m_caam.c			\
 				plat/imx/imx8m/imx8m_psci_common.c		\
 				plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c	\
@@ -34,14 +38,11 @@
 				plat/imx/common/imx_sip_handler.c		\
 				plat/imx/common/imx_sip_svc.c			\
 				plat/imx/common/imx_uart_console.S		\
-				plat/imx/common/imx_ehf.c                       \
-				plat/imx/common/imx_sdei.c                      \
-				lib/xlat_tables/aarch64/xlat_tables.c		\
-				lib/xlat_tables/xlat_tables_common.c		\
 				lib/cpus/aarch64/cortex_a53.S			\
 				drivers/arm/tzc/tzc380.c			\
 				drivers/delay_timer/delay_timer.c		\
 				drivers/delay_timer/generic_delay_timer.c	\
+				${XLAT_TABLES_LIB_SRCS}				\
 				${IMX_GIC_SOURCES}
 
 ifeq (${NEED_BL2},yes)
@@ -150,16 +151,26 @@
 IMX_BOOT_UART_BASE	?=	0x30890000
 $(eval $(call add_define,IMX_BOOT_UART_BASE))
 
-EL3_EXCEPTION_HANDLING := 1
-SDEI_SUPPORT := 1
+EL3_EXCEPTION_HANDLING := $(SDEI_SUPPORT)
+ifeq (${SDEI_SUPPORT}, 1)
+BL31_SOURCES 		+= 	plat/imx/common/imx_ehf.c	\
+				plat/imx/common/imx_sdei.c
+endif
 
 ifeq (${MEASURED_BOOT},1)
     MEASURED_BOOT_MK := drivers/measured_boot/event_log/event_log.mk
     $(info Including ${MEASURED_BOOT_MK})
     include ${MEASURED_BOOT_MK}
 
+ifneq (${MBOOT_EL_HASH_ALG}, sha256)
+    $(eval $(call add_define,TF_MBEDTLS_MBOOT_USE_SHA512))
+endif
+
 BL2_SOURCES		+=	plat/imx/imx8m/imx8m_measured_boot.c	\
 				plat/imx/imx8m/imx8m_dyn_cfg_helpers.c	\
 				${EVENT_LOG_SOURCES}
+endif
 
+ifeq (${SPD},trusty)
+	BL31_CFLAGS    +=      -DPLAT_XLAT_TABLES_DYNAMIC=1
 endif

diff --git a/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c b/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c
index d4705ee..8147792 100644
--- a/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c

@@ -1,5 +1,5 @@
 /*
- * Copyright 2019-2020 NXP
+ * Copyright 2019-2022 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -24,9 +24,12 @@
 #include <imx_uart.h>
 #include <imx_rdc.h>
 #include <imx8m_caam.h>
+#include <imx8m_csu.h>
 #include <platform_def.h>
 #include <plat_imx8.h>
 
+#define TRUSTY_PARAMS_LEN_BYTES      (4096*2)
+
 static const mmap_region_t imx_mmap[] = {
 	GIC_MAP, AIPS_MAP, OCRAM_S_MAP, DDRC_MAP, {0},
 };
@@ -41,9 +44,11 @@
 
 static const struct imx_rdc_cfg rdc[] = {
 	/* Master domain assignment */
-	RDC_MDAn(0x1, DID1),
+	RDC_MDAn(RDC_MDA_M7, DID1),
 
 	/* peripherals domain permission */
+	RDC_PDAPn(RDC_PDAP_UART4, D1R | D1W),
+	RDC_PDAPn(RDC_PDAP_UART2, D0R | D0W),
 
 	/* memory region */
 	RDC_MEM_REGIONn(16, 0x0, 0x0, 0xff),
@@ -54,6 +59,22 @@
 	{0},
 };
 
+static const struct imx_csu_cfg csu_cfg[] = {
+	/* peripherals csl setting */
+	CSU_CSLx(CSU_CSL_OCRAM, CSU_SEC_LEVEL_2, UNLOCKED),
+	CSU_CSLx(CSU_CSL_OCRAM_S, CSU_SEC_LEVEL_2, UNLOCKED),
+
+	/* master HP0~1 */
+
+	/* SA setting */
+
+	/* HP control setting */
+
+	/* Sentinel */
+	{0}
+};
+
+
 static entry_point_info_t bl32_image_ep_info;
 static entry_point_info_t bl33_image_ep_info;
 
@@ -98,6 +119,7 @@
 		u_register_t arg2, u_register_t arg3)
 {
 	static console_t console;
+	unsigned int val;
 	int i;
 
 	/* Enable CSU NS access permission */
@@ -109,6 +131,13 @@
 
 	imx_rdc_init(rdc);
 
+	imx_csu_init(csu_cfg);
+
+	/* config the ocram memory range for secure access */
+	mmio_write_32(IMX_IOMUX_GPR_BASE + 0x2c, 0x4c1);
+	val = mmio_read_32(IMX_IOMUX_GPR_BASE + 0x2c);
+	mmio_write_32(IMX_IOMUX_GPR_BASE + 0x2c, val | 0x3DFF0000);
+
 	imx8m_caam_init();
 
 	console_imx_uart_register(IMX_BOOT_UART_BASE, IMX_BOOT_UART_CLK_IN_HZ,
@@ -124,7 +153,7 @@
 	bl33_image_ep_info.spsr = get_spsr_for_bl33_entry();
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
 
-#ifdef SPD_opteed
+#if defined(SPD_opteed) || defined(SPD_trusty)
 	/* Populate entry point information for BL32 */
 	SET_PARAM_HEAD(&bl32_image_ep_info, PARAM_EP, VERSION_1, 0);
 	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
@@ -134,6 +163,16 @@
 	/* Pass TEE base and size to bl33 */
 	bl33_image_ep_info.args.arg1 = BL32_BASE;
 	bl33_image_ep_info.args.arg2 = BL32_SIZE;
+
+#ifdef SPD_trusty
+	bl32_image_ep_info.args.arg0 = BL32_SIZE;
+	bl32_image_ep_info.args.arg1 = BL32_BASE;
+#else
+	/* Make sure memory is clean */
+	mmio_write_32(BL32_FDT_OVERLAY_ADDR, 0);
+	bl33_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+	bl32_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+#endif
 #endif
 
 	bl31_tzc380_setup();
@@ -150,6 +189,10 @@
 		(BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE),
 		MT_DEVICE | MT_RW | MT_SECURE);
 #endif
+
+	/* Map TEE memory */
+	mmap_add_region(BL32_BASE, BL32_BASE, BL32_SIZE, MT_MEMORY | MT_RW);
+
 	mmap_add(imx_mmap);
 
 	init_xlat_tables();
@@ -184,3 +227,12 @@
 {
 	return COUNTER_FREQUENCY;
 }
+
+#ifdef SPD_trusty
+void plat_trusty_set_boot_args(aapcs64_params_t *args)
+{
+	args->arg0 = BL32_SIZE;
+	args->arg1 = BL32_BASE;
+	args->arg2 = TRUSTY_PARAMS_LEN_BYTES;
+}
+#endif

diff --git a/plat/imx/imx8m/imx8mn/include/imx_sec_def.h b/plat/imx/imx8m/imx8mn/include/imx_sec_def.h
new file mode 100644
index 0000000..0ef14a9
--- /dev/null
+++ b/plat/imx/imx8m/imx8mn/include/imx_sec_def.h

@@ -0,0 +1,210 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_SEC_DEF_H
+#define IMX_SEC_DEF_H
+
+/* RDC MDA index */
+enum rdc_mda_idx {
+	RDC_MDA_A53 = 0,
+	RDC_MDA_M7 = 1,
+	RDC_MDA_SDMA3p = 3,
+	RDC_MDA_LCDIF = 5,
+	RDC_MDA_ISI = 6,
+	RDC_MDA_SDMA3b = 7,
+	RDC_MDA_Coresight = 8,
+	RDC_MDA_DAP = 9,
+	RDC_MDA_CAAM = 10,
+	RDC_MDA_SDMA1p = 11,
+	RDC_MDA_SDMA1b = 12,
+	RDC_MDA_APBHDMA = 13,
+	RDC_MDA_RAWNAND = 14,
+	RDC_MDA_uSDHC1 = 15,
+	RDC_MDA_uSDHC2 = 16,
+	RDC_MDA_uSDHC3 = 17,
+	RDC_MDA_GPU = 18,
+	RDC_MDA_USB1 = 19,
+	RDC_MDA_TESTPORT = 21,
+	RDC_MDA_ENET1_TX = 22,
+	RDC_MDA_ENET1_RX = 23,
+	RDC_MDA_SDMA2 = 24,
+};
+
+/* RDC Peripherals index */
+enum rdc_pdap_idx {
+	RDC_PDAP_GPIO1 = 0,
+	RDC_PDAP_GPIO2 = 1,
+	RDC_PDAP_GPIO3 = 2,
+	RDC_PDAP_GPIO4 = 3,
+	RDC_PDAP_GPIO5 = 4,
+	RDC_PDAP_ANA_TSENSOR = 6,
+	RDC_PDAP_ANA_OSC = 7,
+	RDC_PDAP_WDOG1 = 8,
+	RDC_PDAP_WDOG2 = 9,
+	RDC_PDAP_WDOG3 = 10,
+	RDC_PDAP_SDMA3 = 11,
+	RDC_PDAP_SDMA2 = 12,
+	RDC_PDAP_GPT1 = 13,
+	RDC_PDAP_GPT2 = 14,
+	RDC_PDAP_GPT3 = 15,
+	RDC_PDAP_ROMCP = 17,
+	RDC_PDAP_IOMUXC = 19,
+	RDC_PDAP_IOMUXC_GPR = 20,
+	RDC_PDAP_OCOTP_CTRL = 21,
+	RDC_PDAP_ANA_PLL = 22,
+	RDC_PDAP_SNVS_HP = 23,
+	RDC_PDAP_CCM = 24,
+	RDC_PDAP_SRC = 25,
+	RDC_PDAP_GPC = 26,
+	RDC_PDAP_SEMAPHORE1 = 27,
+	RDC_PDAP_SEMAPHORE2 = 28,
+	RDC_PDAP_RDC = 29,
+	RDC_PDAP_CSU = 30,
+	RDC_PDAP_LCDIF = 32,
+	RDC_PDAP_MIPI_DSI = 33,
+	RDC_PDAP_ISI = 34,
+	RDC_PDAP_MIPI_CSI = 35,
+	RDC_PDAP_USB1 = 36,
+	RDC_PDAP_PWM1 = 38,
+	RDC_PDAP_PWM2 = 39,
+	RDC_PDAP_PWM3 = 40,
+	RDC_PDAP_PWM4 = 41,
+	RDC_PDAP_System_Counter_RD = 42,
+	RDC_PDAP_System_Counter_CMP = 43,
+	RDC_PDAP_System_Counter_CTRL = 44,
+	RDC_PDAP_GPT6 = 46,
+	RDC_PDAP_GPT5 = 47,
+	RDC_PDAP_GPT4 = 48,
+	RDC_PDAP_TZASC = 56,
+	RDC_PDAP_PERFMON1 = 60,
+	RDC_PDAP_PERFMON2 = 61,
+	RDC_PDAP_PLATFORM_CTRL = 62,
+	RDC_PDAP_QoSC = 63,
+	RDC_PDAP_I2C1 = 66,
+	RDC_PDAP_I2C2 = 67,
+	RDC_PDAP_I2C3 = 68,
+	RDC_PDAP_I2C4 = 69,
+	RDC_PDAP_UART4 = 70,
+	RDC_PDAP_MU_A = 74,
+	RDC_PDAP_MU_B = 75,
+	RDC_PDAP_SEMAPHORE_HS = 76,
+	RDC_PDAP_SAI2 = 79,
+	RDC_PDAP_SAI3 = 80,
+	RDC_PDAP_SAI5 = 82,
+	RDC_PDAP_SAI6 = 83,
+	RDC_PDAP_uSDHC1 = 84,
+	RDC_PDAP_uSDHC2 = 85,
+	RDC_PDAP_uSDHC3 = 86,
+	RDC_PDAP_SAI7 = 87,
+	RDC_PDAP_SPBA2 = 90,
+	RDC_PDAP_QSPI = 91,
+	RDC_PDAP_SDMA1 = 93,
+	RDC_PDAP_ENET1 = 94,
+	RDC_PDAP_SPDIF1 = 97,
+	RDC_PDAP_eCSPI1 = 98,
+	RDC_PDAP_eCSPI2 = 99,
+	RDC_PDAP_eCSPI3 = 100,
+	RDC_PDAP_MICFIL = 101,
+	RDC_PDAP_UART1 = 102,
+	RDC_PDAP_UART3 = 104,
+	RDC_PDAP_UART2 = 105,
+	RDC_PDAP_ASRC = 107,
+	RDC_PDAP_SPBA1 = 111,
+	RDC_PDAP_CAAM = 114,
+};
+
+enum csu_csl_idx {
+	CSU_CSL_GPIO1 = 0,
+	CSU_CSL_GPIO2 = 1,
+	CSU_CSL_GPIO3 = 2,
+	CSU_CSL_GPIO4 = 3,
+	CSU_CSL_GPIO5 = 4,
+	CSU_CSL_ANA_TSENSOR = 6,
+	CSU_CSL_ANA_OSC = 7,
+	CSU_CSL_WDOG1 = 8,
+	CSU_CSL_WDOG2 = 9,
+	CSU_CSL_WDOG3 = 10,
+	CSU_CSL_SDMA2 = 12,
+	CSU_CSL_GPT1 = 13,
+	CSU_CSL_GPT2 = 14,
+	CSU_CSL_GPT3 = 15,
+	CSU_CSL_ROMCP = 17,
+	CSU_CSL_LCDIF = 18,
+	CSU_CSL_IOMUXC = 19,
+	CSU_CSL_IOMUXC_GPR = 20,
+	CSU_CSL_OCOTP_CTRL = 21,
+	CSU_CSL_ANA_PLL = 22,
+	CSU_CSL_SNVS_HP = 23,
+	CSU_CSL_CCM = 24,
+	CSU_CSL_SRC = 25,
+	CSU_CSL_GPC = 26,
+	CSU_CSL_SEMAPHORE1 = 27,
+	CSU_CSL_SEMAPHORE2 = 28,
+	CSU_CSL_RDC = 29,
+	CSU_CSL_CSU = 30,
+	CSU_CSL_DC_MST0 = 32,
+	CSU_CSL_DC_MST1 = 33,
+	CSU_CSL_DC_MST2 = 34,
+	CSU_CSL_DC_MST3 = 35,
+	CSU_CSL_PWM1 = 38,
+	CSU_CSL_PWM2 = 39,
+	CSU_CSL_PWM3 = 40,
+	CSU_CSL_PWM4 = 41,
+	CSU_CSL_System_Counter_RD = 42,
+	CSU_CSL_System_Counter_CMP = 43,
+	CSU_CSL_System_Counter_CTRL = 44,
+	CSU_CSL_GPT6 = 46,
+	CSU_CSL_GPT5 = 47,
+	CSU_CSL_GPT4 = 48,
+	CSU_CSL_TZASC = 56,
+	CSU_CSL_MTR = 59,
+	CSU_CSL_PERFMON1 = 60,
+	CSU_CSL_PERFMON2 = 61,
+	CSU_CSL_PLATFORM_CTRL = 62,
+	CSU_CSL_QoSC = 63,
+	CSU_CSL_MIPI_PHY = 64,
+	CSU_CSL_MIPI_DSI = 65,
+	CSU_CSL_I2C1 = 66,
+	CSU_CSL_I2C2 = 67,
+	CSU_CSL_I2C3 = 68,
+	CSU_CSL_I2C4 = 69,
+	CSU_CSL_UART4 = 70,
+	CSU_CSL_MIPI_CSI1 = 71,
+	CSU_CSL_MIPI_CSI_PHY1 = 72,
+	CSU_CSL_CSI1 = 73,
+	CSU_CSL_MU_A = 74,
+	CSU_CSL_MU_B = 75,
+	CSU_CSL_SEMAPHORE_HS = 76,
+	CSU_CSL_SAI1 = 78,
+	CSU_CSL_SAI6 = 80,
+	CSU_CSL_SAI5 = 81,
+	CSU_CSL_SAI4 = 82,
+	CSU_CSL_uSDHC1 = 84,
+	CSU_CSL_uSDHC2 = 85,
+	CSU_CSL_MIPI_CSI2 = 86,
+	CSU_CSL_MIPI_CSI_PHY2 = 87,
+	CSU_CSL_CSI2 = 88,
+	CSU_CSL_SPBA2 = 90,
+	CSU_CSL_QSPI = 91,
+	CSU_CSL_SDMA1 = 93,
+	CSU_CSL_ENET1 = 94,
+	CSU_CSL_SPDIF1 = 97,
+	CSU_CSL_eCSPI1 = 98,
+	CSU_CSL_eCSPI2 = 99,
+	CSU_CSL_eCSPI3 = 100,
+	CSU_CSL_UART1 = 102,
+	CSU_CSL_UART3 = 104,
+	CSU_CSL_UART2 = 105,
+	CSU_CSL_SPDIF2 = 106,
+	CSU_CSL_SAI2 = 107,
+	CSU_CSL_SAI3 = 108,
+	CSU_CSL_SPBA1 = 111,
+	CSU_CSL_CAAM = 114,
+	CSU_CSL_OCRAM = 118,
+	CSU_CSL_OCRAM_S = 119,
+};
+
+#endif /* IMX_SEC_DEF_H */

diff --git a/plat/imx/imx8m/imx8mn/include/platform_def.h b/plat/imx/imx8m/imx8mn/include/platform_def.h
index 9c46d8d..8d39ea6 100644
--- a/plat/imx/imx8m/imx8mn/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mn/include/platform_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright 2020 NXP
+ * Copyright 2020-2022 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -45,6 +45,8 @@
 /* non-secure uboot base */
 #define PLAT_NS_IMAGE_OFFSET		U(0x40200000)
 
+#define BL32_FDT_OVERLAY_ADDR		(PLAT_NS_IMAGE_OFFSET + 0x3000000)
+
 /* GICv3 base address */
 #define PLAT_GICD_BASE			U(0x38800000)
 #define PLAT_GICR_BASE			U(0x38880000)

diff --git a/plat/imx/imx8m/imx8mn/platform.mk b/plat/imx/imx8m/imx8mn/platform.mk
index 2087089..54be41b 100644
--- a/plat/imx/imx8m/imx8mn/platform.mk
+++ b/plat/imx/imx8m/imx8mn/platform.mk

@@ -1,5 +1,5 @@
 #
-# Copyright 2019-2020 NXP
+# Copyright 2019-2022 NXP
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -23,6 +23,7 @@
 				plat/imx/imx8m/imx_aipstz.c			\
 				plat/imx/imx8m/imx_rdc.c			\
 				plat/imx/imx8m/imx8m_caam.c			\
+				plat/imx/imx8m/imx8m_csu.c			\
 				plat/imx/imx8m/imx8m_psci_common.c		\
 				plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c	\
 				plat/imx/imx8m/imx8mn/imx8mn_psci.c		\
@@ -31,8 +32,6 @@
 				plat/imx/common/imx_sip_handler.c		\
 				plat/imx/common/imx_sip_svc.c			\
 				plat/imx/common/imx_uart_console.S		\
-				plat/imx/common/imx_ehf.c                       \
-				plat/imx/common/imx_sdei.c                      \
 				lib/cpus/aarch64/cortex_a53.S			\
 				drivers/arm/tzc/tzc380.c			\
 				drivers/delay_timer/delay_timer.c		\
@@ -57,5 +56,12 @@
 IMX_BOOT_UART_BASE	?=	0x30890000
 $(eval $(call add_define,IMX_BOOT_UART_BASE))
 
-EL3_EXCEPTION_HANDLING := 1
-SDEI_SUPPORT := 1
+EL3_EXCEPTION_HANDLING := $(SDEI_SUPPORT)
+ifeq (${SDEI_SUPPORT}, 1)
+BL31_SOURCES 		+= 	plat/imx/common/imx_ehf.c	\
+				plat/imx/common/imx_sdei.c
+endif
+
+ifeq (${SPD},trusty)
+	BL31_CFLAGS    +=      -DPLAT_XLAT_TABLES_DYNAMIC=1
+endif

diff --git a/plat/imx/imx8m/imx8mp/gpc.c b/plat/imx/imx8m/imx8mp/gpc.c
index d660e3d..3d68b94 100644
--- a/plat/imx/imx8m/imx8mp/gpc.c
+++ b/plat/imx/imx8m/imx8mp/gpc.c

@@ -1,5 +1,5 @@
 /*
- * Copyright 2019-2020 NXP
+ * Copyright 2019-2022 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -69,10 +69,11 @@
 	HDMIMIX,
 	HDMI_PHY,
 	DDRMIX,
+	MAX_DOMAINS,
 };
 
 /* PU domain, add some hole to minimize the uboot change */
-static struct imx_pwr_domain pu_domains[20] = {
+static struct imx_pwr_domain pu_domains[MAX_DOMAINS] = {
 	[MIPI_PHY1] = IMX_PD_DOMAIN(MIPI_PHY1, false),
 	[PCIE_PHY] = IMX_PD_DOMAIN(PCIE_PHY, false),
 	[USB1_PHY] = IMX_PD_DOMAIN(USB1_PHY, true),
@@ -174,6 +175,11 @@
 	struct imx_pwr_domain *pwr_domain = &pu_domains[domain_id];
 	unsigned int i;
 
+	/* validate the domain id */
+	if (domain_id >= MAX_DOMAINS) {
+		return;
+	}
+
 	if (domain_id == HSIOMIX) {
 		for (i = 0; i < ARRAY_SIZE(hsiomix_clk); i++) {
 			hsiomix_clk[i].val = mmio_read_32(IMX_CCM_BASE + hsiomix_clk[i].offset);

diff --git a/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c b/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c
index 22fbd5e..57e5c51 100644
--- a/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c

@@ -1,5 +1,5 @@
 /*
- * Copyright 2020 NXP
+ * Copyright 2020-2022 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -24,9 +24,12 @@
 #include <imx_uart.h>
 #include <imx_rdc.h>
 #include <imx8m_caam.h>
+#include <imx8m_csu.h>
 #include <platform_def.h>
 #include <plat_imx8.h>
 
+#define TRUSTY_PARAMS_LEN_BYTES      (4096*2)
+
 static const mmap_region_t imx_mmap[] = {
 	GIC_MAP, AIPS_MAP, OCRAM_S_MAP, DDRC_MAP,
 	NOC_MAP, {0},
@@ -42,9 +45,10 @@
 
 static const struct imx_rdc_cfg rdc[] = {
 	/* Master domain assignment */
-	RDC_MDAn(0x1, DID1),
+	RDC_MDAn(RDC_MDA_M7, DID1),
 
 	/* peripherals domain permission */
+	RDC_PDAPn(RDC_PDAP_UART2, D0R | D0W),
 
 	/* memory region */
 
@@ -52,6 +56,21 @@
 	{0},
 };
 
+static const struct imx_csu_cfg csu_cfg[] = {
+	/* peripherals csl setting */
+	CSU_CSLx(CSU_CSL_OCRAM, CSU_SEC_LEVEL_2, UNLOCKED),
+	CSU_CSLx(CSU_CSL_OCRAM_S, CSU_SEC_LEVEL_2, UNLOCKED),
+
+	/* master HP0~1 */
+
+	/* SA setting */
+
+	/* HP control setting */
+
+	/* Sentinel */
+	{0}
+};
+
 static entry_point_info_t bl32_image_ep_info;
 static entry_point_info_t bl33_image_ep_info;
 
@@ -96,6 +115,7 @@
 		u_register_t arg2, u_register_t arg3)
 {
 	static console_t console;
+	unsigned int val;
 	unsigned int i;
 
 	/* Enable CSU NS access permission */
@@ -107,6 +127,13 @@
 
 	imx_rdc_init(rdc);
 
+	imx_csu_init(csu_cfg);
+
+	/* config the ocram memory range for secure access */
+	mmio_write_32(IMX_IOMUX_GPR_BASE + 0x2c, 0x4E1);
+	val = mmio_read_32(IMX_IOMUX_GPR_BASE + 0x2c);
+	mmio_write_32(IMX_IOMUX_GPR_BASE + 0x2c, val | 0x3DFF0000);
+
 	imx8m_caam_init();
 
 	console_imx_uart_register(IMX_BOOT_UART_BASE, IMX_BOOT_UART_CLK_IN_HZ,
@@ -122,7 +149,7 @@
 	bl33_image_ep_info.spsr = get_spsr_for_bl33_entry();
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
 
-#ifdef SPD_opteed
+#if defined(SPD_opteed) || defined(SPD_trusty)
 	/* Populate entry point information for BL32 */
 	SET_PARAM_HEAD(&bl32_image_ep_info, PARAM_EP, VERSION_1, 0);
 	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
@@ -132,6 +159,16 @@
 	/* Pass TEE base and size to bl33 */
 	bl33_image_ep_info.args.arg1 = BL32_BASE;
 	bl33_image_ep_info.args.arg2 = BL32_SIZE;
+
+#ifdef SPD_trusty
+	bl32_image_ep_info.args.arg0 = BL32_SIZE;
+	bl32_image_ep_info.args.arg1 = BL32_BASE;
+#else
+	/* Make sure memory is clean */
+	mmio_write_32(BL32_FDT_OVERLAY_ADDR, 0);
+	bl33_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+	bl32_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+#endif
 #endif
 
 	bl31_tzc380_setup();
@@ -148,6 +185,10 @@
 		(BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE),
 		MT_DEVICE | MT_RW | MT_SECURE);
 #endif
+
+	/* Map TEE memory */
+	mmap_add_region(BL32_BASE, BL32_BASE, BL32_SIZE, MT_MEMORY | MT_RW);
+
 	mmap_add(imx_mmap);
 
 	init_xlat_tables();
@@ -185,3 +226,12 @@
 {
 	return COUNTER_FREQUENCY;
 }
+
+#ifdef SPD_trusty
+void plat_trusty_set_boot_args(aapcs64_params_t *args)
+{
+	args->arg0 = BL32_SIZE;
+	args->arg1 = BL32_BASE;
+	args->arg2 = TRUSTY_PARAMS_LEN_BYTES;
+}
+#endif

diff --git a/plat/imx/imx8m/imx8mp/include/imx_sec_def.h b/plat/imx/imx8m/imx8mp/include/imx_sec_def.h
new file mode 100644
index 0000000..ba248b5
--- /dev/null
+++ b/plat/imx/imx8m/imx8mp/include/imx_sec_def.h

@@ -0,0 +1,274 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_SEC_DEF_H
+#define IMX_SEC_DEF_H
+
+/* RDC MDA index */
+enum rdc_mda_idx {
+	RDC_MDA_A53 = 0,
+	RDC_MDA_M7 = 1,
+	RDC_MDA_PCIE_CTRL1 = 2,
+	RDC_MDA_SDMA3p = 3,
+	RDC_MDA_SDMA3b = 4,
+	RDC_MDA_LCDIF = 5,
+	RDC_MDA_ISI = 6,
+	RDC_MDA_NPU = 7,
+	RDC_MDA_Coresight = 8,
+	RDC_MDA_DAP = 9,
+	RDC_MDA_CAAM = 10,
+	RDC_MDA_SDMA1p = 11,
+	RDC_MDA_SDMA1b = 12,
+	RDC_MDA_APBHDMA = 13,
+	RDC_MDA_RAWNAND = 14,
+	RDC_MDA_uSDHC1 = 15,
+	RDC_MDA_uSDHC2 = 16,
+	RDC_MDA_uSDHC3 = 17,
+	RDC_MDA_AUDIO_PROCESSOR = 18,
+	RDC_MDA_USB1 = 19,
+	RDC_MDA_USB2 = 20,
+	RDC_MDA_TESTPORT = 21,
+	RDC_MDA_ENET1_TX = 22,
+	RDC_MDA_ENET1_RX = 23,
+	RDC_MDA_SDMA2 = 24,
+	RDC_MDA_SDMA3_to_SPBA2 = 25,
+	RDC_MDA_SDMA1_to_SPBA1 = 26,
+	RDC_MDA_LCDIF2 = 27,
+	RDC_MDA_HDMI_TX = 28,
+	RDC_MDA_ENET2 = 29,
+	RDC_MDA_GPU3D = 30,
+	RDC_MDA_GPU2D = 31,
+	RDC_MDA_VPU_G1 = 32,
+	RDC_MDA_VPU_G2 = 33,
+	RDC_MDA_VPU_VC8000E = 34,
+	RDC_MDA_AUDIO_EDMA = 35,
+	RDC_MDA_ISP1 = 36,
+	RDC_MDA_ISP2 = 37,
+	RDC_MDA_DEWARP = 38,
+	RDC_MDA_GIC500 = 39,
+};
+
+/* RDC Peripherals index */
+enum rdc_pdap_idx {
+	RDC_PDAP_GPIO1 = 0,
+	RDC_PDAP_GPIO2 = 1,
+	RDC_PDAP_GPIO3 = 2,
+	RDC_PDAP_GPIO4 = 3,
+	RDC_PDAP_GPIO5 = 4,
+	RDC_PDAP_MU_2_A = 5,
+	RDC_PDAP_ANA_TSENSOR = 6,
+	RDC_PDAP_ANA_OSC = 7,
+	RDC_PDAP_WDOG1 = 8,
+	RDC_PDAP_WDOG2 = 9,
+	RDC_PDAP_WDOG3 = 10,
+	RDC_PDAP_GPT1 = 13,
+	RDC_PDAP_GPT2 = 14,
+	RDC_PDAP_GPT3 = 15,
+	RDC_PDAP_MU_2_B = 16,
+	RDC_PDAP_ROMCP = 17,
+	RDC_PDAP_MU_3_A = 18,
+	RDC_PDAP_IOMUXC = 19,
+	RDC_PDAP_IOMUXC_GPR = 20,
+	RDC_PDAP_OCOTP_CTRL = 21,
+	RDC_PDAP_ANA_PLL = 22,
+	RDC_PDAP_SNVS_HP = 23,
+	RDC_PDAP_CCM = 24,
+	RDC_PDAP_SRC = 25,
+	RDC_PDAP_GPC = 26,
+	RDC_PDAP_SEMAPHORE1 = 27,
+	RDC_PDAP_SEMAPHORE2 = 28,
+	RDC_PDAP_RDC = 29,
+	RDC_PDAP_CSU = 30,
+	RDC_PDAP_MU_3_B = 31,
+	RDC_PDAP_ISI = 32,
+	RDC_PDAP_ISP0 = 33,
+	RDC_PDAP_ISP1 = 34,
+	RDC_PDAP_IPS_Dewarp = 35,
+	RDC_PDAP_MIPI_CSI0 = 36,
+	RDC_PDAP_HSIOMIX_BLK_CTL = 37,
+	RDC_PDAP_PWM1 = 38,
+	RDC_PDAP_PWM2 = 39,
+	RDC_PDAP_PWM3 = 40,
+	RDC_PDAP_PWM4 = 41,
+	RDC_PDAP_System_Counter_RD = 42,
+	RDC_PDAP_System_Counter_CMP = 43,
+	RDC_PDAP_System_Counter_CTRL = 44,
+	RDC_PDAP_I2C5 = 45,
+	RDC_PDAP_GPT6 = 46,
+	RDC_PDAP_GPT5 = 47,
+	RDC_PDAP_GPT4 = 48,
+	RDC_PDAP_MIPI_CSI1 = 49,
+	RDC_PDAP_MIPI_DSI0 = 50,
+	RDC_PDAP_MEDIAMIX_BLK_CTL = 51,
+	RDC_PDAP_LCDIF1 = 52,
+	RDC_PDAP_eDMA_Management_Page = 53,
+	RDC_PDAP_eDMA_Channels_15_0 = 54,
+	RDC_PDAP_eDMA_Channels_31_16 = 55,
+	RDC_PDAP_TZASC = 56,
+	RDC_PDAP_I2C6 = 57,
+	RDC_PDAP_CAAM = 58,
+	RDC_PDAP_LCDIF2 = 59,
+	RDC_PDAP_PERFMON1 = 60,
+	RDC_PDAP_PERFMON2 = 61,
+	RDC_PDAP_NOC_BLK_CTL = 62,
+	RDC_PDAP_QoSC = 63,
+	RDC_PDAP_LVDS0 = 64,
+	RDC_PDAP_LVDS1 = 65,
+	RDC_PDAP_I2C1 = 66,
+	RDC_PDAP_I2C2 = 67,
+	RDC_PDAP_I2C3 = 68,
+	RDC_PDAP_I2C4 = 69,
+	RDC_PDAP_UART4 = 70,
+	RDC_PDAP_HDMI_TX = 71,
+	RDC_PDAP_IRQ_STEER_Audio_Processor = 72,
+	RDC_PDAP_SDMA2 = 73,
+	RDC_PDAP_MU_1_A = 74,
+	RDC_PDAP_MU_1_B = 75,
+	RDC_PDAP_SEMAPHORE_HS = 76,
+	RDC_PDAP_SAI1 = 78,
+	RDC_PDAP_SAI2 = 79,
+	RDC_PDAP_SAI3 = 80,
+	RDC_PDAP_CAN_FD1 = 81,
+	RDC_PDAP_SAI5 = 82,
+	RDC_PDAP_SAI6 = 83,
+	RDC_PDAP_uSDHC1 = 84,
+	RDC_PDAP_uSDHC2 = 85,
+	RDC_PDAP_uSDHC3 = 86,
+	RDC_PDAP_PCIE_PHY1 = 87,
+	RDC_PDAP_HDMI_TX_AUDLNK_MSTR = 88,
+	RDC_PDAP_CAN_FD2 = 89,
+	RDC_PDAP_SPBA2 = 90,
+	RDC_PDAP_QSPI = 91,
+	RDC_PDAP_AUDIO_BLK_CTRL = 92,
+	RDC_PDAP_SDMA1 = 93,
+	RDC_PDAP_ENET1 = 94,
+	RDC_PDAP_ENET2_TSN = 95,
+	RDC_PDAP_ASRC = 97,
+	RDC_PDAP_eCSPI1 = 98,
+	RDC_PDAP_eCSPI2 = 99,
+	RDC_PDAP_eCSPI3 = 100,
+	RDC_PDAP_SAI7 = 101,
+	RDC_PDAP_UART1 = 102,
+	RDC_PDAP_UART3 = 104,
+	RDC_PDAP_UART2 = 105,
+	RDC_PDAP_PDM_MICFIL = 106,
+	RDC_PDAP_AUDIO_XCVR_RX_eARC = 107,
+	RDC_PDAP_SDMA3 = 109,
+	RDC_PDAP_SPBA1 = 111,
+};
+
+enum csu_csl_idx {
+	CSU_CSL_GPIO1 = 0,
+	CSU_CSL_GPIO2 = 1,
+	CSU_CSL_GPIO3 = 2,
+	CSU_CSL_GPIO4 = 3,
+	CSU_CSL_GPIO5 = 4,
+	CSU_CSL_MU_2_A = 5,
+	CSU_CSL_ANA_TSENSOR = 6,
+	CSU_CSL_ANA_OSC = 7,
+	CSU_CSL_WDOG1 = 8,
+	CSU_CSL_WDOG2 = 9,
+	CSU_CSL_WDOG3 = 10,
+	CSU_CSL_GPT1 = 13,
+	CSU_CSL_GPT2 = 14,
+	CSU_CSL_GPT3 = 15,
+	CSU_CSL_MU_2_B = 16,
+	CSU_CSL_ROMCP = 17,
+	CSU_CSL_MU_3_A = 18,
+	CSU_CSL_IOMUXC = 19,
+	CSU_CSL_IOMUXC_GPR = 20,
+	CSU_CSL_OCOTP_CTRL = 21,
+	CSU_CSL_ANA_PLL = 22,
+	CSU_CSL_SNVS_HP = 23,
+	CSU_CSL_CCM = 24,
+	CSU_CSL_SRC = 25,
+	CSU_CSL_GPC = 26,
+	CSU_CSL_SEMAPHORE1 = 27,
+	CSU_CSL_SEMAPHORE2 = 28,
+	CSU_CSL_RDC = 29,
+	CSU_CSL_CSU = 30,
+	CSU_CSL_MU_3_B = 31,
+	CSU_CSL_ISI = 32,
+	CSU_CSL_ISP0 = 33,
+	CSU_CSL_ISP1 = 34,
+	CSU_CSL_IPS_Dewarp = 35,
+	CSU_CSL_MIPI_CSI0 = 36,
+	CSU_CSL_HSIOMIX_BLK_CTL	= 37,
+	CSU_CSL_PWM1 = 38,
+	CSU_CSL_PWM2 = 39,
+	CSU_CSL_PWM3 = 40,
+	CSU_CSL_PWM4 = 41,
+	CSU_CSL_System_Counter_RD = 42,
+	CSU_CSL_System_Counter_CMP = 43,
+	CSU_CSL_System_Counter_CTRL = 44,
+	CSU_CSL_I2C5 = 45,
+	CSU_CSL_GPT6 = 46,
+	CSU_CSL_GPT5 = 47,
+	CSU_CSL_GPT4 = 48,
+	CSU_CSL_MIPI_CSI1 = 49,
+	CSU_CSL_MIPI_DSI0 = 50,
+	CSU_CSL_MEDIAMIX_BLK_CTL = 51,
+	CSU_CSL_LCDIF1 = 52,
+	CSU_CSL_eDMA_Management_Page = 53,
+	CSU_CSL_eDMA_Channels_15_0 = 54,
+	CSU_CSL_eDMA_Channels_31_16 = 55,
+	CSU_CSL_TZASC = 56,
+	CSU_CSL_I2C6 = 57,
+	CSU_CSL_CAAM = 58,
+	CSU_CSL_LCDIF2 = 59,
+	CSU_CSL_PERFMON1 = 60,
+	CSU_CSL_PERFMON2 = 61,
+	CSU_CSL_NOC_BLK_CTL = 62,
+	CSU_CSL_QoSC = 63,
+	CSU_CSL_LVDS0 = 64,
+	CSU_CSL_LVDS1 = 65,
+	CSU_CSL_I2C1 = 66,
+	CSU_CSL_I2C2 = 67,
+	CSU_CSL_I2C3 = 68,
+	CSU_CSL_I2C4 = 69,
+	CSU_CSL_UART4 = 70,
+	CSU_CSL_HDMI_TX = 71,
+	CSU_CSL_IRQ_STEER_Audio_Processor = 72,
+	CSU_CSL_SDMA2 = 73,
+	CSU_CSL_MU_1_A = 74,
+	CSU_CSL_MU_1_B = 75,
+	CSU_CSL_SEMAPHORE_HS = 76,
+	CSU_CSL_SAI1 = 78,
+	CSU_CSL_SAI2 = 79,
+	CSU_CSL_SAI3 = 80,
+	CSU_CSL_CAN_FD1 = 81,
+	CSU_CSL_SAI5 = 82,
+	CSU_CSL_SAI6 = 83,
+	CSU_CSL_uSDHC1 = 84,
+	CSU_CSL_uSDHC2 = 85,
+	CSU_CSL_uSDHC3 = 86,
+	CSU_CSL_PCIE_PHY1 = 87,
+	CSU_CSL_HDMI_TX_AUDLNK_MSTR = 88,
+	CSU_CSL_CAN_FD2 = 89,
+	CSU_CSL_SPBA2 = 90,
+	CSU_CSL_QSPI = 91,
+	CSU_CSL_AUDIO_BLK_CTRL = 92,
+	CSU_CSL_SDMA1 = 93,
+	CSU_CSL_ENET1 = 94,
+	CSU_CSL_ENET2_TSN = 95,
+	CSU_CSL_ASRC = 97,
+	CSU_CSL_eCSPI1 = 98,
+	CSU_CSL_eCSPI2 = 99,
+	CSU_CSL_eCSPI3 = 100,
+	CSU_CSL_SAI7 = 101,
+	CSU_CSL_UART1 = 102,
+	CSU_CSL_UART3 = 104,
+	CSU_CSL_UART2 = 105,
+	CSU_CSL_PDM_MICFIL = 106,
+	CSU_CSL_AUDIO_XCVR_RX_eARC = 107,
+	CSU_CSL_SDMA3 = 109,
+	CSU_CSL_SPBA1 = 111,
+	CSU_CSL_OCRAM_A = 113,
+	CSU_CSL_OCRAM = 118,
+	CSU_CSL_OCRAM_S = 119,
+};
+
+#endif /* IMX_SEC_DEF_H */

diff --git a/plat/imx/imx8m/imx8mp/include/platform_def.h b/plat/imx/imx8m/imx8mp/include/platform_def.h
index 486c1ee..8807f5d 100644
--- a/plat/imx/imx8m/imx8mp/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mp/include/platform_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright 2020 NXP
+ * Copyright 2020-2022 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -62,6 +62,8 @@
 #define PLAT_NS_IMAGE_OFFSET		U(0x40200000)
 #define PLAT_NS_IMAGE_SIZE		U(0x00200000)
 
+#define BL32_FDT_OVERLAY_ADDR		(PLAT_NS_IMAGE_OFFSET + 0x3000000)
+
 /* GICv3 base address */
 #define PLAT_GICD_BASE			U(0x38800000)
 #define PLAT_GICR_BASE			U(0x38880000)

diff --git a/plat/imx/imx8m/imx8mp/platform.mk b/plat/imx/imx8m/imx8mp/platform.mk
index 823b5d6..73fbd87 100644
--- a/plat/imx/imx8m/imx8mp/platform.mk
+++ b/plat/imx/imx8m/imx8mp/platform.mk

@@ -1,5 +1,5 @@
 #
-# Copyright 2019-2020 NXP
+# Copyright 2019-2022 NXP
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -25,13 +25,12 @@
 				plat/imx/imx8m/imx_aipstz.c			\
 				plat/imx/imx8m/imx_rdc.c			\
 				plat/imx/imx8m/imx8m_caam.c			\
+				plat/imx/imx8m/imx8m_csu.c			\
 				plat/imx/imx8m/imx8m_psci_common.c		\
 				plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c	\
 				plat/imx/imx8m/imx8mp/imx8mp_psci.c		\
 				plat/imx/imx8m/imx8mp/gpc.c			\
 				plat/imx/common/imx8_topology.c			\
-				plat/imx/common/imx_ehf.c                       \
-				plat/imx/common/imx_sdei.c                      \
 				plat/imx/common/imx_sip_handler.c		\
 				plat/imx/common/imx_sip_svc.c			\
 				plat/imx/common/imx_uart_console.S		\
@@ -149,5 +148,12 @@
 IMX_BOOT_UART_BASE	?=	0x30890000
 $(eval $(call add_define,IMX_BOOT_UART_BASE))
 
-EL3_EXCEPTION_HANDLING := 1
-SDEI_SUPPORT := 1
+EL3_EXCEPTION_HANDLING := $(SDEI_SUPPORT)
+ifeq (${SDEI_SUPPORT}, 1)
+BL31_SOURCES 		+= 	plat/imx/common/imx_ehf.c	\
+				plat/imx/common/imx_sdei.c
+endif
+
+ifeq (${SPD},trusty)
+	BL31_CFLAGS    +=      -DPLAT_XLAT_TABLES_DYNAMIC=1
+endif

diff --git a/plat/imx/imx8m/imx8mq/gpc.c b/plat/imx/imx8m/imx8mq/gpc.c
index 367c941..fa83324 100644
--- a/plat/imx/imx8m/imx8mq/gpc.c
+++ b/plat/imx/imx8m/imx8mq/gpc.c

@@ -9,6 +9,7 @@
 #include <stdbool.h>
 
 #include <common/debug.h>
+#include <drivers/delay_timer.h>
 #include <lib/mmio.h>
 #include <lib/psci/psci.h>
 #include <platform_def.h>
@@ -176,6 +177,13 @@
 	mmio_clrbits_32(IMX_SRC_BASE + SRC_OTG1PHY_SCR, 0x1);
 	mmio_clrbits_32(IMX_SRC_BASE + SRC_OTG2PHY_SCR, 0x1);
 
-	/* enable all the power domain by default */
-	mmio_write_32(IMX_GPC_BASE + PU_PGC_UP_TRG, 0x3fcf);
+	/*
+	 * for USB OTG, the limitation are:
+	 * 1. before system clock config, the IPG clock run at 12.5MHz, delay time
+	 *    should be longer than 82us.
+	 * 2. after system clock config, ipg clock run at 66.5MHz, delay time
+	 *    be longer that 15.3 us.
+	 *    Add 100us to make sure the USB OTG SRC is clear safely.
+	 */
+	udelay(100);
 }

diff --git a/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c b/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c
index 05b5970..e998a16 100644
--- a/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,7 +18,7 @@
 #include <drivers/generic_delay_timer.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/mmio.h>
-#include <lib/xlat_tables/xlat_tables.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
 #include <gpc.h>
@@ -27,6 +27,8 @@
 #include <imx8m_caam.h>
 #include <plat_imx8.h>
 
+#define TRUSTY_PARAMS_LEN_BYTES      (4096*2)
+
 static const mmap_region_t imx_mmap[] = {
 	MAP_REGION_FLAT(GPV_BASE, GPV_SIZE, MT_DEVICE | MT_RW), /* GPV map */
 	MAP_REGION_FLAT(IMX_ROM_BASE, IMX_ROM_SIZE, MT_MEMORY | MT_RO), /* ROM map */
@@ -146,7 +148,7 @@
 	bl33_image_ep_info.spsr = get_spsr_for_bl33_entry();
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
 
-#ifdef SPD_opteed
+#if defined(SPD_opteed) || defined(SPD_trusty)
 	/* Populate entry point information for BL32 */
 	SET_PARAM_HEAD(&bl32_image_ep_info, PARAM_EP, VERSION_1, 0);
 	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
@@ -156,6 +158,16 @@
 	/* Pass TEE base and size to bl33 */
 	bl33_image_ep_info.args.arg1 = BL32_BASE;
 	bl33_image_ep_info.args.arg2 = BL32_SIZE;
+
+#ifdef SPD_trusty
+	bl32_image_ep_info.args.arg0 = BL32_SIZE;
+	bl32_image_ep_info.args.arg1 = BL32_BASE;
+#else
+	/* Make sure memory is clean */
+	mmio_write_32(BL32_FDT_OVERLAY_ADDR, 0);
+	bl33_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+	bl32_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+#endif
 #endif
 
 	bl31_tz380_setup();
@@ -168,6 +180,9 @@
 	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);
 
 #if USE_COHERENT_MEM
@@ -215,3 +230,12 @@
 {
 	return;
 }
+
+#ifdef SPD_trusty
+void plat_trusty_set_boot_args(aapcs64_params_t *args)
+{
+	args->arg0 = BL32_SIZE;
+	args->arg1 = BL32_BASE;
+	args->arg2 = TRUSTY_PARAMS_LEN_BYTES;
+}
+#endif

diff --git a/plat/imx/imx8m/imx8mq/include/imx_sec_def.h b/plat/imx/imx8m/imx8mq/include/imx_sec_def.h
new file mode 100644
index 0000000..0f77141
--- /dev/null
+++ b/plat/imx/imx8m/imx8mq/include/imx_sec_def.h

@@ -0,0 +1,249 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_SEC_DEF_H
+#define IMX_SEC_DEF_H
+
+/* RDC MDA index */
+enum rdc_mda_idx {
+	RDC_MDA_A53 = 0,
+	RDC_MDA_M4 = 1,
+	RDC_MDA_PCIE_CTRL1 = 2,
+	RDC_MDA_PCIE_CTRL2 = 3,
+	RDC_MDA_VPU_DEC = 4,
+	RDC_MDA_LCDIF = 5,
+	RDC_MDA_CSI1 = 6,
+	RDC_MDA_CSI2 = 7,
+	RDC_MDA_Coresight = 8,
+	RDC_MDA_DAP = 9,
+	RDC_MDA_CAAM = 10,
+	RDC_MDA_SDMAp = 11,
+	RDC_MDA_SDMAb = 12,
+	RDC_MDA_APBHDMA = 13,
+	RDC_MDA_RAWNAND = 14,
+	RDC_MDA_uSDHC1 = 15,
+	RDC_MDA_uSDHC2 = 16,
+	RDC_MDA_DCSS = 17,
+	RDC_MDA_GPU = 18,
+	RDC_MDA_USB1 = 19,
+	RDC_MDA_USB2 = 20,
+	RDC_MDA_TESTPORT = 21,
+	RDC_MDA_ENET1_TX = 22,
+	RDC_MDA_ENET1_RX = 23,
+	RDC_MDA_SDMA2 = 24,
+	RDC_MDA_SDMA1 = 26,
+};
+
+/* RDC Peripherals index */
+enum rdc_pdap_idx {
+	RDC_PDAP_GPIO1 = 0,
+	RDC_PDAP_GPIO2 = 1,
+	RDC_PDAP_GPIO3 = 2,
+	RDC_PDAP_GPIO4 = 3,
+	RDC_PDAP_GPIO5 = 4,
+	RDC_PDAP_ANA_TSENSOR = 6,
+	RDC_PDAP_ANA_OSC = 7,
+	RDC_PDAP_WDOG1 = 8,
+	RDC_PDAP_WDOG2 = 9,
+	RDC_PDAP_WDOG3 = 10,
+	RDC_PDAP_SDMA2 = 12,
+	RDC_PDAP_GPT1 = 13,
+	RDC_PDAP_GPT2 = 14,
+	RDC_PDAP_GPT3 = 15,
+	RDC_PDAP_ROMCP = 17,
+	RDC_PDAP_LCDIF = 18,
+	RDC_PDAP_IOMUXC = 19,
+	RDC_PDAP_IOMUXC_GPR = 20,
+	RDC_PDAP_OCOTP_CTRL = 21,
+	RDC_PDAP_ANATOP_PLL = 22,
+	RDC_PDAP_SNVS_HP = 23,
+	RDC_PDAP_CCM = 24,
+	RDC_PDAP_SRC = 25,
+	RDC_PDAP_GPC = 26,
+	RDC_PDAP_SEMAPHORE1 = 27,
+	RDC_PDAP_SEMAPHORE2 = 28,
+	RDC_PDAP_RDC = 29,
+	RDC_PDAP_CSU = 30,
+	RDC_PDAP_MST0 = 32,
+	RDC_PDAP_MST1 = 33,
+	RDC_PDAP_MST2 = 34,
+	RDC_PDAP_MST3 = 35,
+	RDC_PDAP_HDMI_SEC = 36,
+	RDC_PDAP_PWM1 = 38,
+	RDC_PDAP_PWM2 = 39,
+	RDC_PDAP_PWM3 = 40,
+	RDC_PDAP_PWM4 = 41,
+	RDC_PDAP_SysCounter_RD = 42,
+	RDC_PDAP_SysCounter_CMP = 43,
+	RDC_PDAP_SysCounter_CTRL = 44,
+	RDC_PDAP_HDMI_CTRL = 45,
+	RDC_PDAP_GPT6 = 46,
+	RDC_PDAP_GPT5 = 47,
+	RDC_PDAP_GPT4 = 48,
+	RDC_PDAP_TZASC = 56,
+	RDC_PDAP_MTR = 59,
+	RDC_PDAP_PERFMON1 = 60,
+	RDC_PDAP_PERFMON2 = 61,
+	RDC_PDAP_PLATFORM_CTRL = 62,
+	RDC_PDAP_QoSC = 63,
+	RDC_PDAP_MIPI_PHY = 64,
+	RDC_PDAP_MIPI_DSI = 65,
+	RDC_PDAP_I2C1 = 66,
+	RDC_PDAP_I2C2 = 67,
+	RDC_PDAP_I2C3 = 68,
+	RDC_PDAP_I2C4 = 69,
+	RDC_PDAP_UART4 = 70,
+	RDC_PDAP_MIPI_CSI1 = 71,
+	RDC_PDAP_MIPI_CSI_PHY1 = 72,
+	RDC_PDAP_CSI1 = 73,
+	RDC_PDAP_MU_A = 74,
+	RDC_PDAP_MU_B = 75,
+	RDC_PDAP_SEMAPHORE_HS = 76,
+	RDC_PDAP_SAI1 = 78,
+	RDC_PDAP_SAI6 = 80,
+	RDC_PDAP_SAI5 = 81,
+	RDC_PDAP_SAI4 = 82,
+	RDC_PDAP_USDHC1 = 84,
+	RDC_PDAP_USDHC2 = 85,
+	RDC_PDAP_MIPI_CSI2 = 86,
+	RDC_PDAP_MIPI_CSI_PHY2 = 87,
+	RDC_PDAP_CSI2 = 88,
+	RDC_PDAP_QSPI = 91,
+	RDC_PDAP_SDMA1 = 93,
+	RDC_PDAP_ENET1 = 94,
+	RDC_PDAP_SPDIF1 = 97,
+	RDC_PDAP_ECSPI1 = 98,
+	RDC_PDAP_ECSPI2 = 99,
+	RDC_PDAP_ECSPI3 = 100,
+	RDC_PDAP_UART1 = 102,
+	RDC_PDAP_UART3 = 104,
+	RDC_PDAP_UART2 = 105,
+	RDC_PDAP_SPDIF2 = 106,
+	RDC_PDAP_SAI2 = 107,
+	RDC_PDAP_SAI3 = 108,
+	RDC_PDAP_SPBA1 = 111,
+	RDC_PDAP_CAAM = 114,
+	RDC_PDAP_DDRC_SEC = 115,
+	RDC_PDAP_GIC_EXSC = 116,
+	RDC_PDAP_USB_EXSC = 117,
+	RDC_PDAP_OCRAM_TZ = 118,
+	RDC_PDAP_OCRAM_S_TZ = 119,
+	RDC_PDAP_VPU_SEC = 120,
+	RDC_PDAP_DAP_EXSC = 121,
+	RDC_PDAP_ROMCP_SEC = 122,
+	RDC_PDAP_APBHDMA_SEC = 123,
+	RDC_PDAP_M4_SEC = 124,
+	RDC_PDAP_QSPI_SEC = 125,
+	RDC_PDAP_GPU_EXSC = 126,
+	RDC_PDAP_PCIE = 127,
+};
+
+enum csu_csl_idx {
+	CSU_CSL_GPIO1 = 0,
+	CSU_CSL_GPIO2 = 1,
+	CSU_CSL_GPIO3 = 2,
+	CSU_CSL_GPIO4 = 3,
+	CSU_CSL_GPIO5 = 4,
+	CSU_CSL_ANA_TSENSOR = 6,
+	CSU_CSL_ANA_OSC = 7,
+	CSU_CSL_WDOG1 = 8,
+	CSU_CSL_WDOG2 = 9,
+	CSU_CSL_WDOG3 = 10,
+	CSU_CSL_SDMA2 = 12,
+	CSU_CSL_GPT1 = 13,
+	CSU_CSL_GPT2 = 14,
+	CSU_CSL_GPT3 = 15,
+	CSU_CSL_ROMCP = 17,
+	CSU_CSL_LCDIF = 18,
+	CSU_CSL_IOMUXC = 19,
+	CSU_CSL_IOMUXC_GPR = 20,
+	CSU_CSL_OCOTP_CTRL = 21,
+	CSU_CSL_ANATOP_PLL = 22,
+	CSU_CSL_SNVS_HP = 23,
+	CSU_CSL_CCM = 24,
+	CSU_CSL_SRC = 25,
+	CSU_CSL_GPC = 26,
+	CSU_CSL_SEMAPHORE1 = 27,
+	CSU_CSL_SEMAPHORE2 = 28,
+	CSU_CSL_RDC = 29,
+	CSU_CSL_CSU = 30,
+	CSU_CSL_MST0 = 32,
+	CSU_CSL_MST1 = 33,
+	CSU_CSL_MST2 = 34,
+	CSU_CSL_MST3 = 35,
+	CSU_CSL_HDMI_SEC = 36,
+	CSU_CSL_PWM1 = 38,
+	CSU_CSL_PWM2 = 39,
+	CSU_CSL_PWM3 = 40,
+	CSU_CSL_PWM4 = 41,
+	CSU_CSL_SysCounter_RD = 42,
+	CSU_CSL_SysCounter_CMP = 43,
+	CSU_CSL_SysCounter_CTRL = 44,
+	CSU_CSL_HDMI_CTRL = 45,
+	CSU_CSL_GPT6 = 46,
+	CSU_CSL_GPT5 = 47,
+	CSU_CSL_GPT4 = 48,
+	CSU_CSL_TZASC = 56,
+	CSU_CSL_MTR = 59,
+	CSU_CSL_PERFMON1 = 60,
+	CSU_CSL_PERFMON2 = 61,
+	CSU_CSL_PLATFORM_CTRL = 62,
+	CSU_CSL_QoSC = 63,
+	CSU_CSL_MIPI_PHY = 64,
+	CSU_CSL_MIPI_DSI = 65,
+	CSU_CSL_I2C1 = 66,
+	CSU_CSL_I2C2 = 67,
+	CSU_CSL_I2C3 = 68,
+	CSU_CSL_I2C4 = 69,
+	CSU_CSL_UART4 = 70,
+	CSU_CSL_MIPI_CSI1 = 71,
+	CSU_CSL_MIPI_CSI_PHY1 = 72,
+	CSU_CSL_CSI1 = 73,
+	CSU_CSL_MU_A = 74,
+	CSU_CSL_MU_B = 75,
+	CSU_CSL_SEMAPHORE_HS = 76,
+	CSU_CSL_SAI1 = 78,
+	CSU_CSL_SAI6 = 80,
+	CSU_CSL_SAI5 = 81,
+	CSU_CSL_SAI4 = 82,
+	CSU_CSL_USDHC1 = 84,
+	CSU_CSL_USDHC2 = 85,
+	CSU_CSL_MIPI_CSI2 = 86,
+	CSU_CSL_MIPI_CSI_PHY2 = 87,
+	CSU_CSL_CSI2 = 88,
+	CSU_CSL_QSPI = 91,
+	CSU_CSL_SDMA1 = 93,
+	CSU_CSL_ENET1 = 94,
+	CSU_CSL_SPDIF1 = 97,
+	CSU_CSL_ECSPI1 = 98,
+	CSU_CSL_ECSPI2 = 99,
+	CSU_CSL_ECSPI3 = 100,
+	CSU_CSL_UART1 = 102,
+	CSU_CSL_UART3 = 104,
+	CSU_CSL_UART2 = 105,
+	CSU_CSL_SPDIF2 = 106,
+	CSU_CSL_SAI2 = 107,
+	CSU_CSL_SAI3 = 108,
+	CSU_CSL_SPBA1 = 111,
+	CSU_CSL_MOD_EN3 = 112,
+	CSU_CSL_MOD_EN0 = 113,
+	CSU_CSL_CAAM = 114,
+	CSU_CSL_DDRC_SEC = 115,
+	CSU_CSL_GIC_EXSC = 116,
+	CSU_CSL_USB_EXSC = 117,
+	CSU_CSL_OCRAM_TZ = 118,
+	CSU_CSL_OCRAM_S_TZ = 119,
+	CSU_CSL_VPU_SEC = 120,
+	CSU_CSL_DAP_EXSC = 121,
+	CSU_CSL_ROMCP_SEC = 122,
+	CSU_CSL_APBHDMA_SEC = 123,
+	CSU_CSL_M4_SEC = 124,
+	CSU_CSL_QSPI_SEC = 125,
+	CSU_CSL_GPU_EXSC = 126,
+	CSU_CSL_PCIE = 127,
+};
+
+#endif /* IMX_SEC_DEF_H */

diff --git a/plat/imx/imx8m/imx8mq/include/platform_def.h b/plat/imx/imx8m/imx8mq/include/platform_def.h
index 6d6a865..1dd22d9 100644
--- a/plat/imx/imx8m/imx8mq/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mq/include/platform_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -35,6 +35,7 @@
 
 /* non-secure uboot base */
 #define PLAT_NS_IMAGE_OFFSET		U(0x40200000)
+#define BL32_FDT_OVERLAY_ADDR		(PLAT_NS_IMAGE_OFFSET + 0x3000000)
 
 /* GICv3 base address */
 #define PLAT_GICD_BASE			U(0x38800000)
@@ -43,8 +44,13 @@
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(1ull << 32)
 #define PLAT_PHY_ADDR_SPACE_SIZE	(1ull << 32)
 
+#ifdef SPD_trusty
+#define MAX_XLAT_TABLES			5
+#define MAX_MMAP_REGIONS		15
+#else
 #define MAX_XLAT_TABLES			4
 #define MAX_MMAP_REGIONS		14
+#endif
 
 #define HAB_RVT_BASE			U(0x00000880) /* HAB_RVT for i.MX8MQ */
 
@@ -120,7 +126,7 @@
 #define OCRAM_S_SIZE			U(0x8000)
 #define OCRAM_S_LIMIT			(OCRAM_S_BASE + OCRAM_S_SIZE)
 
-#define COUNTER_FREQUENCY		8000000 /* 8MHz */
+#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 5461010..7b6df92 100644
--- a/plat/imx/imx8m/imx8mq/platform.mk
+++ b/plat/imx/imx8m/imx8mq/platform.mk

@@ -1,9 +1,12 @@
 #
-# Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+# Translation tables library
+include lib/xlat_tables_v2/xlat_tables.mk
+
 PLAT_INCLUDES		:=	-Iplat/imx/common/include		\
 				-Iplat/imx/imx8m/include		\
 				-Iplat/imx/imx8m/imx8mq/include
@@ -28,12 +31,11 @@
 				plat/imx/common/imx_sip_handler.c		\
 				plat/imx/common/imx_sip_svc.c			\
 				plat/imx/common/imx_uart_console.S		\
-				lib/xlat_tables/aarch64/xlat_tables.c		\
-				lib/xlat_tables/xlat_tables_common.c		\
 				lib/cpus/aarch64/cortex_a53.S			\
 				drivers/arm/tzc/tzc380.c			\
 				drivers/delay_timer/delay_timer.c		\
 				drivers/delay_timer/generic_delay_timer.c	\
+				${XLAT_TABLES_LIB_SRCS}				\
 				${IMX_GIC_SOURCES}
 
 USE_COHERENT_MEM	:=	1
@@ -49,3 +51,7 @@
 
 BL32_SIZE		?=	0x2000000
 $(eval $(call add_define,BL32_SIZE))
+
+ifeq (${SPD},trusty)
+	BL31_CFLAGS    +=      -DPLAT_XLAT_TABLES_DYNAMIC=1
+endif

diff --git a/plat/imx/imx8m/include/imx8m_csu.h b/plat/imx/imx8m/include/imx8m_csu.h
new file mode 100644
index 0000000..dc634ed
--- /dev/null
+++ b/plat/imx/imx8m/include/imx8m_csu.h

@@ -0,0 +1,74 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_CSU_H
+#define IMX_CSU_H
+
+#include <lib/utils_def.h>
+
+#include <platform_def.h>
+
+#define CSU_SEC_LEVEL_0		0xff
+#define CSU_SEC_LEVEL_1		0xbb
+#define CSU_SEC_LEVEL_2		0x3f
+#define CSU_SEC_LEVEL_3		0x3b
+#define CSU_SEC_LEVEL_4		0x33
+#define CSU_SEC_LEVEL_5		0x22
+#define CSU_SEC_LEVEL_6		0x03
+#define CSU_SEC_LEVEL_7		0x0
+
+#define LOCKED			0x1
+#define UNLOCKED		0x0
+
+#define CSLx_REG(x)		(IMX_CSU_BASE + ((x) / 2) * 4)
+#define CSLx_LOCK(x)		((0x1 << (((x) % 2) * 16 + 8)))
+#define CSLx_CFG(x, n)		((x) << (((n) % 2) * 16))
+
+#define CSU_HP_REG(x)		(IMX_CSU_BASE + ((x) / 16) * 4 + 0x200)
+#define CSU_HP_LOCK(x)		((0x1 << (((x) % 16) * 2 + 1)))
+#define CSU_HP_CFG(x, n)	((x) << (((n) % 16) * 2))
+
+#define CSU_SA_REG(x)		(IMX_CSU_BASE + 0x218)
+#define CSU_SA_LOCK(x)		((0x1 << (((x) % 16) * 2 + 1)))
+#define CSU_SA_CFG(x, n)	((x) << (((n) % 16) * 2))
+
+#define CSU_HPCONTROL_REG(x)		(IMX_CSU_BASE + (((x) / 16) * 4) + 0x358)
+#define CSU_HPCONTROL_LOCK(x)		((0x1 << (((x) % 16) * 2 + 1)))
+#define CSU_HPCONTROL_CFG(x, n)		((x) << (((n) % 16) * 2))
+
+enum csu_cfg_type {
+	CSU_INVALID,
+	CSU_CSL,
+	CSU_HP,
+	CSU_SA,
+	CSU_HPCONTROL,
+};
+
+struct imx_csu_cfg {
+	enum csu_cfg_type type;
+	uint16_t idx;
+	uint16_t lock : 1;
+	uint16_t csl_level : 8;
+	uint16_t hp : 1;
+	uint16_t sa : 1;
+	uint16_t hpctrl : 1;
+};
+
+#define CSU_CSLx(i, level, lk)	\
+	{CSU_CSL, .idx = (i), .csl_level = (level), .lock = (lk),}
+
+#define CSU_HPx(i, val, lk)	\
+	{CSU_HP, .idx = (i), .hp = (val), .lock = (lk), }
+
+#define CSU_SA(i, val, lk)	\
+	{CSU_SA, .idx = (i), .sa = (val), .lock = (lk), }
+
+#define CSU_HPCTRL(i, val, lk)	\
+	{CSU_HPCONTROL, .idx = (i), .hpctrl = (val), .lock = (lk), }
+
+void imx_csu_init(const struct imx_csu_cfg *csu_cfg);
+
+#endif /* IMX_CSU_H */

diff --git a/plat/imx/imx8m/include/imx_rdc.h b/plat/imx/imx8m/include/imx_rdc.h
index e25b0e6..a6e10a7 100644
--- a/plat/imx/imx8m/include/imx_rdc.h
+++ b/plat/imx/imx8m/include/imx_rdc.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, NXP. All rights reserved.
+ * Copyright (c) 2019-2022 NXP. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,7 @@
 
 #include <lib/utils_def.h>
 
+#include <imx_sec_def.h>
 #include <platform_def.h>
 
 #define MDAn(x)		(IMX_RDC_BASE + 0x200 + (x) * 4)

diff --git a/plat/imx/imx8qm/imx8qm_bl31_setup.c b/plat/imx/imx8qm/imx8qm_bl31_setup.c
index d9c9110..68eb534 100644
--- a/plat/imx/imx8qm/imx8qm_bl31_setup.c
+++ b/plat/imx/imx8qm/imx8qm_bl31_setup.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,7 +19,7 @@
 #include <drivers/console.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/mmio.h>
-#include <lib/xlat_tables/xlat_tables.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
 #include <imx8qm_pads.h>

diff --git a/plat/imx/imx8qm/platform.mk b/plat/imx/imx8qm/platform.mk
index f35fa00..c57edbe 100644
--- a/plat/imx/imx8qm/platform.mk
+++ b/plat/imx/imx8qm/platform.mk

@@ -1,9 +1,12 @@
 #
-# Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+# Translation tables library
+include lib/xlat_tables_v2/xlat_tables.mk
+
 PLAT_INCLUDES		:=	-Iplat/imx/imx8qm/include		\
 				-Iplat/imx/common/include		\
 
@@ -23,11 +26,10 @@
 				plat/imx/common/imx8_psci.c		\
 				plat/imx/common/imx_sip_svc.c		\
 				plat/imx/common/imx_sip_handler.c	\
-				lib/xlat_tables/aarch64/xlat_tables.c		\
-				lib/xlat_tables/xlat_tables_common.c		\
 				lib/cpus/aarch64/cortex_a53.S			\
 				lib/cpus/aarch64/cortex_a72.S			\
 				drivers/arm/cci/cci.c				\
+				${XLAT_TABLES_LIB_SRCS}				\
 				${IMX_GIC_SOURCES}				\
 
 include plat/imx/common/sci/sci_api.mk

diff --git a/plat/imx/imx8qx/imx8qx_bl31_setup.c b/plat/imx/imx8qx/imx8qx_bl31_setup.c
index 3739cd6..1da8d29 100644
--- a/plat/imx/imx8qx/imx8qx_bl31_setup.c
+++ b/plat/imx/imx8qx/imx8qx_bl31_setup.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,7 +19,7 @@
 #include <drivers/console.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/mmio.h>
-#include <lib/xlat_tables/xlat_tables.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
 #include <imx8qx_pads.h>

diff --git a/plat/imx/imx8qx/platform.mk b/plat/imx/imx8qx/platform.mk
index b25be07..85b5f3d 100644
--- a/plat/imx/imx8qx/platform.mk
+++ b/plat/imx/imx8qx/platform.mk

@@ -1,9 +1,12 @@
 #
-# Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+# Translation tables library
+include lib/xlat_tables_v2/xlat_tables.mk
+
 PLAT_INCLUDES		:=	-Iplat/imx/imx8qx/include		\
 				-Iplat/imx/common/include		\
 
@@ -23,9 +26,8 @@
 				plat/imx/common/imx_sip_svc.c		\
 				plat/imx/common/imx_sip_handler.c	\
 				plat/common/plat_psci_common.c		\
-				lib/xlat_tables/xlat_tables_common.c	\
-				lib/xlat_tables/aarch64/xlat_tables.c	\
 				lib/cpus/aarch64/cortex_a35.S		\
+				${XLAT_TABLES_LIB_SRCS}			\
 				${IMX_GIC_SOURCES}			\
 
 include plat/imx/common/sci/sci_api.mk

diff --git a/plat/intel/soc/agilex/bl2_plat_setup.c b/plat/intel/soc/agilex/bl2_plat_setup.c
index 03adcf3..211a7b7 100644
--- a/plat/intel/soc/agilex/bl2_plat_setup.c
+++ b/plat/intel/soc/agilex/bl2_plat_setup.c

@@ -1,6 +1,6 @@
 /*
- * Copyright (c) 2019-2021, ARM Limited and Contributors. All rights reserved.
- * Copyright (c) 2019-2021, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -23,6 +23,7 @@
 #include "ccu/ncore_ccu.h"
 #include "qspi/cadence_qspi.h"
 #include "socfpga_emac.h"
+#include "socfpga_f2sdram_manager.h"
 #include "socfpga_handoff.h"
 #include "socfpga_mailbox.h"
 #include "socfpga_private.h"
@@ -81,8 +82,10 @@
 	mailbox_init();
 	agx_mmc_init();
 
-	if (!intel_mailbox_is_fpga_not_ready())
-		socfpga_bridges_enable();
+	if (!intel_mailbox_is_fpga_not_ready()) {
+		socfpga_bridges_enable(SOC2FPGA_MASK | LWHPS2FPGA_MASK |
+					FPGA2SOC_MASK);
+	}
 }
 
 

diff --git a/plat/intel/soc/agilex/bl31_plat_setup.c b/plat/intel/soc/agilex/bl31_plat_setup.c
index 66d6b8f..b1b9514 100644
--- a/plat/intel/soc/agilex/bl31_plat_setup.c
+++ b/plat/intel/soc/agilex/bl31_plat_setup.c

@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2019-2020, ARM Limited and Contributors. All rights reserved.
- * Copyright (c) 2019-2020, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,6 +14,7 @@
 #include <lib/mmio.h>
 #include <lib/xlat_tables/xlat_tables.h>
 
+#include "ccu/ncore_ccu.h"
 #include "socfpga_mailbox.h"
 #include "socfpga_private.h"
 
@@ -114,6 +115,8 @@
 		(uint64_t)plat_secondary_cpus_bl31_entry);
 
 	mailbox_hps_stage_notify(HPS_EXECUTION_STATE_SSBL);
+
+	ncore_enable_ocram_firewall();
 }
 
 const mmap_region_t plat_agilex_mmap[] = {

diff --git a/plat/intel/soc/agilex/include/agilex_clock_manager.h b/plat/intel/soc/agilex/include/agilex_clock_manager.h
index 20667f0..f39d475 100644
--- a/plat/intel/soc/agilex/include/agilex_clock_manager.h
+++ b/plat/intel/soc/agilex/include/agilex_clock_manager.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */

diff --git a/plat/intel/soc/agilex/include/agilex_noc.h b/plat/intel/soc/agilex/include/agilex_noc.h
deleted file mode 100644
index 22db3e2..0000000
--- a/plat/intel/soc/agilex/include/agilex_noc.h
+++ /dev/null

@@ -1,69 +0,0 @@
-/*
- * Copyright (c) 2019, Intel Corporation. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef AGX_NOC_H
-#define AGX_NOC_H
-
-
-#define AXI_AP					(1<<0)
-#define FPGA2SOC				(1<<16)
-#define MPU					(1<<24)
-#define AGX_NOC_PER_SCR_NAND			0xffd21000
-#define AGX_NOC_PER_SCR_NAND_DATA		0xffd21004
-#define AGX_NOC_PER_SCR_USB0			0xffd2100c
-#define AGX_NOC_PER_SCR_USB1			0xffd21010
-#define AGX_NOC_PER_SCR_SPI_M0			0xffd2101c
-#define AGX_NOC_PER_SCR_SPI_M1			0xffd21020
-#define AGX_NOC_PER_SCR_SPI_S0			0xffd21024
-#define AGX_NOC_PER_SCR_SPI_S1			0xffd21028
-#define AGX_NOC_PER_SCR_EMAC0			0xffd2102c
-#define AGX_NOC_PER_SCR_EMAC1			0xffd21030
-#define AGX_NOC_PER_SCR_EMAC2			0xffd21034
-#define AGX_NOC_PER_SCR_SDMMC			0xffd21040
-#define AGX_NOC_PER_SCR_GPIO0			0xffd21044
-#define AGX_NOC_PER_SCR_GPIO1			0xffd21048
-#define AGX_NOC_PER_SCR_I2C0			0xffd21050
-#define AGX_NOC_PER_SCR_I2C1			0xffd21058
-#define AGX_NOC_PER_SCR_I2C2			0xffd2105c
-#define AGX_NOC_PER_SCR_I2C3			0xffd21060
-#define AGX_NOC_PER_SCR_SP_TIMER0		0xffd21064
-#define AGX_NOC_PER_SCR_SP_TIMER1		0xffd21068
-#define AGX_NOC_PER_SCR_UART0			0xffd2106c
-#define AGX_NOC_PER_SCR_UART1			0xffd21070
-
-
-#define AGX_NOC_SYS_SCR_DMA_ECC			0xffd21108
-#define AGX_NOC_SYS_SCR_EMAC0RX_ECC		0xffd2110c
-#define AGX_NOC_SYS_SCR_EMAC0TX_ECC		0xffd21110
-#define AGX_NOC_SYS_SCR_EMAC1RX_ECC		0xffd21114
-#define AGX_NOC_SYS_SCR_EMAC1TX_ECC		0xffd21118
-#define AGX_NOC_SYS_SCR_EMAC2RX_ECC		0xffd2111c
-#define AGX_NOC_SYS_SCR_EMAC2TX_ECC		0xffd21120
-#define AGX_NOC_SYS_SCR_NAND_ECC		0xffd2112c
-#define AGX_NOC_SYS_SCR_NAND_READ_ECC		0xffd21130
-#define AGX_NOC_SYS_SCR_NAND_WRITE_ECC		0xffd21134
-#define AGX_NOC_SYS_SCR_OCRAM_ECC		0xffd21138
-#define AGX_NOC_SYS_SCR_SDMMC_ECC		0xffd21140
-#define AGX_NOC_SYS_SCR_USB0_ECC		0xffd21144
-#define AGX_NOC_SYS_SCR_USB1_ECC		0xffd21148
-#define AGX_NOC_SYS_SCR_CLK_MGR			0xffd2114c
-#define AGX_NOC_SYS_SCR_IO_MGR			0xffd21154
-#define AGX_NOC_SYS_SCR_RST_MGR			0xffd21158
-#define AGX_NOC_SYS_SCR_SYS_MGR			0xffd2115c
-#define AGX_NOC_SYS_SCR_OSC0_TIMER		0xffd21160
-#define AGX_NOC_SYS_SCR_OSC1_TIMER		0xffd21164
-#define AGX_NOC_SYS_SCR_WATCHDOG0		0xffd21168
-#define AGX_NOC_SYS_SCR_WATCHDOG1		0xffd2116c
-#define AGX_NOC_SYS_SCR_WATCHDOG2		0xffd21170
-#define AGX_NOC_SYS_SCR_WATCHDOG3		0xffd21174
-#define AGX_NOC_SYS_SCR_DAP			0xffd21178
-#define AGX_NOC_SYS_SCR_L4_NOC_PROBES		0xffd21190
-#define AGX_NOC_SYS_SCR_L4_NOC_QOS		0xffd21194
-
-#define AGX_CCU_NOC_BRIDGE_CPU0_RAM		0xf7004688
-#define AGX_CCU_NOC_BRIDGE_IOM_RAM		0xf7004688
-
-#endif

diff --git a/plat/intel/soc/agilex/include/socfpga_plat_def.h b/plat/intel/soc/agilex/include/socfpga_plat_def.h
index 9c87e45..b216ab1 100644
--- a/plat/intel/soc/agilex/include/socfpga_plat_def.h
+++ b/plat/intel/soc/agilex/include/socfpga_plat_def.h

@@ -19,6 +19,9 @@
 #define INTEL_SIP_SMC_FPGA_CONFIG_SIZE		0x2000000
 
 /* Register Mapping */
+#define SOCFPGA_CCU_NOC_REG_BASE		0xf7000000
+#define SOCFPGA_F2SDRAMMGR_REG_BASE		U(0xf8024000)
+
 #define SOCFPGA_MMC_REG_BASE			0xff808000
 
 #define SOCFPGA_RSTMGR_REG_BASE			0xffd11000
@@ -29,5 +32,9 @@
 #define SOCFPGA_SOC2FPGA_SCR_REG_BASE           0xffd21200
 #define SOCFPGA_LWSOC2FPGA_SCR_REG_BASE         0xffd21300
 
-#endif /* PLAT_SOCFPGA_DEF_H */
+/* Platform specific system counter */
+#define PLAT_SYS_COUNTER_FREQ_IN_MHZ	get_cpu_clk()
 
+uint32_t get_cpu_clk(void);
+
+#endif /* PLAT_SOCFPGA_DEF_H */

diff --git a/plat/intel/soc/agilex/platform.mk b/plat/intel/soc/agilex/platform.mk
index 10a3eec..ccb4e07 100644
--- a/plat/intel/soc/agilex/platform.mk
+++ b/plat/intel/soc/agilex/platform.mk

@@ -26,6 +26,7 @@
 			lib/xlat_tables/xlat_tables_common.c 		\
 			plat/intel/soc/common/aarch64/platform_common.c \
 			plat/intel/soc/common/aarch64/plat_helpers.S	\
+			plat/intel/soc/common/drivers/ccu/ncore_ccu.c	\
 			plat/intel/soc/common/socfpga_delay_timer.c
 
 BL2_SOURCES     +=	\
@@ -44,17 +45,20 @@
 		plat/intel/soc/agilex/soc/agilex_memory_controller.c	\
 		plat/intel/soc/agilex/soc/agilex_mmc.c			\
 		plat/intel/soc/agilex/soc/agilex_pinmux.c		\
-                plat/intel/soc/common/bl2_plat_mem_params_desc.c	\
+		plat/intel/soc/common/bl2_plat_mem_params_desc.c	\
 		plat/intel/soc/common/socfpga_image_load.c		\
 		plat/intel/soc/common/socfpga_storage.c			\
 		plat/intel/soc/common/soc/socfpga_emac.c		\
+		plat/intel/soc/common/soc/socfpga_firewall.c	\
 		plat/intel/soc/common/soc/socfpga_handoff.c		\
 		plat/intel/soc/common/soc/socfpga_mailbox.c		\
 		plat/intel/soc/common/soc/socfpga_reset_manager.c	\
-		plat/intel/soc/common/soc/socfpga_system_manager.c	\
 		plat/intel/soc/common/drivers/qspi/cadence_qspi.c	\
-		plat/intel/soc/common/drivers/wdt/watchdog.c		\
-		plat/intel/soc/common/drivers/ccu/ncore_ccu.c
+		plat/intel/soc/common/drivers/wdt/watchdog.c
+
+include lib/zlib/zlib.mk
+PLAT_INCLUDES	+=	-Ilib/zlib
+BL2_SOURCES	+=	$(ZLIB_SOURCES)
 
 BL31_SOURCES	+=	\
 		drivers/arm/cci/cci.c					\
@@ -62,8 +66,10 @@
 		lib/cpus/aarch64/cortex_a53.S				\
 		plat/common/plat_psci_common.c				\
 		plat/intel/soc/agilex/bl31_plat_setup.c 		\
+		plat/intel/soc/agilex/soc/agilex_clock_manager.c	\
 		plat/intel/soc/common/socfpga_psci.c			\
 		plat/intel/soc/common/socfpga_sip_svc.c			\
+		plat/intel/soc/common/socfpga_sip_svc_v2.c		\
 		plat/intel/soc/common/socfpga_topology.c		\
 		plat/intel/soc/common/sip/socfpga_sip_ecc.c		\
 		plat/intel/soc/common/sip/socfpga_sip_fcs.c		\

diff --git a/plat/intel/soc/agilex/soc/agilex_clock_manager.c b/plat/intel/soc/agilex/soc/agilex_clock_manager.c
index 4efd713..76b9937 100644
--- a/plat/intel/soc/agilex/soc/agilex_clock_manager.c
+++ b/plat/intel/soc/agilex/soc/agilex_clock_manager.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
  */
@@ -387,3 +387,13 @@
 
 	return mmc_clk;
 }
+
+/* Get cpu freq clock */
+uint32_t get_cpu_clk(void)
+{
+	uint32_t cpu_clk;
+
+	cpu_clk = get_l3_clk()/PLAT_SYS_COUNTER_CONVERT_TO_MHZ;
+
+	return cpu_clk;
+}

diff --git a/plat/intel/soc/common/drivers/ccu/ncore_ccu.c b/plat/intel/soc/common/drivers/ccu/ncore_ccu.c
index d4716cf..d9a238e 100644
--- a/plat/intel/soc/common/drivers/ccu/ncore_ccu.c
+++ b/plat/intel/soc/common/drivers/ccu/ncore_ccu.c

@@ -118,6 +118,7 @@
 	mmio_setbits_32(COH_CPU0_BYPASS_REG(NCORE_FW_OCRAM_BLK_CGF4),
 			OCRAM_PRIVILEGED_MASK | OCRAM_SECURE_MASK);
 }
+
 uint32_t init_ncore_ccu(void)
 {
 	uint32_t status;

diff --git a/plat/intel/soc/common/include/platform_def.h b/plat/intel/soc/common/include/platform_def.h
index d37904b..a31adf7 100644
--- a/plat/intel/soc/common/include/platform_def.h
+++ b/plat/intel/soc/common/include/platform_def.h

@@ -192,7 +192,7 @@
  * System counter frequency related constants
  ******************************************************************************/
 #define PLAT_SYS_COUNTER_FREQ_IN_TICKS	(400000000)
-#define PLAT_SYS_COUNTER_FREQ_IN_MHZ	(400)
+#define PLAT_SYS_COUNTER_CONVERT_TO_MHZ	(1000000)
 
 #define PLAT_INTEL_SOCFPGA_GICD_BASE	PLAT_GICD_BASE
 #define PLAT_INTEL_SOCFPGA_GICC_BASE	PLAT_GICC_BASE

diff --git a/plat/intel/soc/common/include/socfpga_f2sdram_manager.h b/plat/intel/soc/common/include/socfpga_f2sdram_manager.h
new file mode 100644
index 0000000..82bb6cb
--- /dev/null
+++ b/plat/intel/soc/common/include/socfpga_f2sdram_manager.h

@@ -0,0 +1,39 @@
+/*
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SOCFPGA_F2SDRAMMANAGER_H
+#define SOCFPGA_F2SDRAMMANAGER_H
+
+#include "socfpga_plat_def.h"
+
+/* FPGA2SDRAM Register Map */
+#define SOCFPGA_F2SDRAMMGR_SIDEBANDMGR_FLAGINSTATUS0	0x14
+#define SOCFPGA_F2SDRAMMGR_SIDEBANDMGR_FLAGOUTCLR0	0x54
+#define SOCFPGA_F2SDRAMMGR_SIDEBANDMGR_FLAGOUTSET0	0x50
+
+#define FLAGOUTSETCLR_F2SDRAM0_ENABLE		(BIT(1))
+#define FLAGOUTSETCLR_F2SDRAM1_ENABLE		(BIT(4))
+#define FLAGOUTSETCLR_F2SDRAM2_ENABLE		(BIT(7))
+
+#define FLAGOUTSETCLR_F2SDRAM0_IDLEREQ		(BIT(0))
+#define FLAGOUTSETCLR_F2SDRAM1_IDLEREQ		(BIT(3))
+#define FLAGOUTSETCLR_F2SDRAM2_IDLEREQ		(BIT(6))
+#define FLAGINTSTATUS_F2SDRAM0_IDLEACK		(BIT(1))
+#define FLAGINTSTATUS_F2SDRAM1_IDLEACK		(BIT(5))
+#define FLAGINTSTATUS_F2SDRAM2_IDLEACK		(BIT(9))
+#define FLAGOUTSETCLR_F2SDRAM0_FORCE_DRAIN	(BIT(2))
+#define FLAGOUTSETCLR_F2SDRAM1_FORCE_DRAIN	(BIT(5))
+#define FLAGOUTSETCLR_F2SDRAM2_FORCE_DRAIN	(BIT(8))
+
+#define FLAGINTSTATUS_F2SOC_RESPEMPTY		(BIT(3))
+#define FLAGINTSTATUS_F2SDRAM0_RESPEMPTY	(BIT(3))
+#define FLAGINTSTATUS_F2SDRAM1_RESPEMPTY	(BIT(7))
+#define FLAGINTSTATUS_F2SDRAM2_RESPEMPTY	(BIT(11))
+
+#define SOCFPGA_F2SDRAMMGR(_reg)	(SOCFPGA_F2SDRAMMGR_REG_BASE \
+						+ (SOCFPGA_F2SDRAMMGR_##_reg))
+
+#endif /* SOCFPGA_F2SDRAMMGR_H */

diff --git a/plat/intel/soc/common/include/socfpga_fcs.h b/plat/intel/soc/common/include/socfpga_fcs.h
index d3b7141..893551d 100644
--- a/plat/intel/soc/common/include/socfpga_fcs.h
+++ b/plat/intel/soc/common/include/socfpga_fcs.h

@@ -9,38 +9,300 @@
 
 /* FCS Definitions */
 
-#define FCS_RANDOM_WORD_SIZE		8U
-#define FCS_PROV_DATA_WORD_SIZE		44U
-#define FCS_SHA384_WORD_SIZE		12U
+#define FCS_RANDOM_WORD_SIZE					8U
+#define FCS_PROV_DATA_WORD_SIZE					44U
+#define FCS_SHA384_WORD_SIZE					12U
 
-#define FCS_RANDOM_BYTE_SIZE		(FCS_RANDOM_WORD_SIZE * 4U)
-#define FCS_PROV_DATA_BYTE_SIZE		(FCS_PROV_DATA_WORD_SIZE * 4U)
-#define FCS_SHA384_BYTE_SIZE		(FCS_SHA384_WORD_SIZE * 4U)
+#define FCS_RANDOM_BYTE_SIZE					(FCS_RANDOM_WORD_SIZE * 4U)
+#define FCS_RANDOM_EXT_MAX_WORD_SIZE				1020U
+#define FCS_PROV_DATA_BYTE_SIZE					(FCS_PROV_DATA_WORD_SIZE * 4U)
+#define FCS_SHA384_BYTE_SIZE					(FCS_SHA384_WORD_SIZE * 4U)
 
-#define FCS_CRYPTION_DATA_0		0x10100
+#define FCS_RANDOM_EXT_OFFSET					3
 
+#define FCS_MODE_DECRYPT					0x0
+#define FCS_MODE_ENCRYPT					0x1
+#define FCS_ENCRYPTION_DATA_0					0x10100
+#define FCS_DECRYPTION_DATA_0					0x10102
+#define FCS_OWNER_ID_OFFSET					0xC
+#define FCS_CRYPTION_CRYPTO_HEADER				0x07000000
+#define FCS_CRYPTION_RESP_WORD_SIZE				4U
+#define FCS_CRYPTION_RESP_SIZE_OFFSET				3U
+
+#define PSGSIGMA_TEARDOWN_MAGIC					0xB852E2A4
+#define	PSGSIGMA_SESSION_ID_ONE					0x1
+#define PSGSIGMA_UNKNOWN_SESSION				0xFFFFFFFF
+
+#define	RESERVED_AS_ZERO					0x0
+/* FCS Single cert */
+
+#define FCS_BIG_CNTR_SEL					0x1
+
+#define FCS_SVN_CNTR_0_SEL					0x2
+#define FCS_SVN_CNTR_1_SEL					0x3
+#define FCS_SVN_CNTR_2_SEL					0x4
+#define FCS_SVN_CNTR_3_SEL					0x5
+
+#define FCS_BIG_CNTR_VAL_MAX					495U
+#define FCS_SVN_CNTR_VAL_MAX					64U
+
+/* FCS Attestation Cert Request Parameter */
+
+#define FCS_ATTEST_FIRMWARE_CERT				0x01
+#define FCS_ATTEST_DEV_ID_SELF_SIGN_CERT			0x02
+#define FCS_ATTEST_DEV_ID_ENROLL_CERT				0x04
+#define FCS_ATTEST_ENROLL_SELF_SIGN_CERT			0x08
+#define FCS_ATTEST_ALIAS_CERT					0x10
+#define FCS_ATTEST_CERT_MAX_REQ_PARAM				0xFF
+
+/* FCS Crypto Service */
+
+#define FCS_CS_KEY_OBJ_MAX_WORD_SIZE				88U
+#define FCS_CS_KEY_INFO_MAX_WORD_SIZE				36U
+#define FCS_CS_KEY_RESP_STATUS_MASK				0xFF
+#define FCS_CS_KEY_RESP_STATUS_OFFSET				16U
+
+#define FCS_CS_FIELD_SIZE_MASK					0xFFFF
+#define FCS_CS_FIELD_FLAG_OFFSET				24
+#define FCS_CS_FIELD_FLAG_INIT					BIT(0)
+#define FCS_CS_FIELD_FLAG_UPDATE				BIT(1)
+#define FCS_CS_FIELD_FLAG_FINALIZE				BIT(2)
+
+#define FCS_AES_MAX_DATA_SIZE					0x10000000	/* 256 MB */
+#define FCS_AES_MIN_DATA_SIZE					0x20		/* 32 Byte */
+#define FCS_AES_CMD_MAX_WORD_SIZE				15U
+
+#define FCS_GET_DIGEST_CMD_MAX_WORD_SIZE			7U
+#define FCS_GET_DIGEST_RESP_MAX_WORD_SIZE			19U
+#define FCS_MAC_VERIFY_CMD_MAX_WORD_SIZE			23U
+#define FCS_MAC_VERIFY_RESP_MAX_WORD_SIZE			4U
+#define FCS_SHA_HMAC_CRYPTO_PARAM_SIZE_OFFSET			8U
+
+#define FCS_ECDSA_GET_PUBKEY_MAX_WORD_SIZE			5U
+#define FCS_ECDSA_SHA2_DATA_SIGN_CMD_MAX_WORD_SIZE		7U
+#define FCS_ECDSA_SHA2_DATA_SIG_VERIFY_CMD_MAX_WORD_SIZE	43U
+#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
 /* FCS Payload Structure */
+typedef struct fcs_rng_payload_t {
+	uint32_t session_id;
+	uint32_t context_id;
+	uint32_t crypto_header;
+	uint32_t size;
+} fcs_rng_payload;
+
+typedef struct fcs_encrypt_payload_t {
+	uint32_t first_word;
+	uint32_t src_addr;
+	uint32_t src_size;
+	uint32_t dst_addr;
+	uint32_t dst_size;
+} fcs_encrypt_payload;
 
-typedef struct fcs_crypt_payload_t {
+typedef struct fcs_decrypt_payload_t {
 	uint32_t first_word;
+	uint32_t owner_id[2];
+	uint32_t src_addr;
+	uint32_t src_size;
+	uint32_t dst_addr;
+	uint32_t dst_size;
+} fcs_decrypt_payload;
+
+typedef struct fcs_encrypt_ext_payload_t {
+	uint32_t session_id;
+	uint32_t context_id;
+	uint32_t crypto_header;
+	uint32_t src_addr;
+	uint32_t src_size;
+	uint32_t dst_addr;
+	uint32_t dst_size;
+} fcs_encrypt_ext_payload;
+
+typedef struct fcs_decrypt_ext_payload_t {
+	uint32_t session_id;
+	uint32_t context_id;
+	uint32_t crypto_header;
+	uint32_t owner_id[2];
 	uint32_t src_addr;
 	uint32_t src_size;
 	uint32_t dst_addr;
 	uint32_t dst_size;
-} fcs_crypt_payload;
+} fcs_decrypt_ext_payload;
+
+typedef struct psgsigma_teardown_msg_t {
+	uint32_t reserved_word;
+	uint32_t magic_word;
+	uint32_t session_id;
+} psgsigma_teardown_msg;
+
+typedef struct fcs_cntr_set_preauth_payload_t {
+	uint32_t first_word;
+	uint32_t counter_value;
+} fcs_cntr_set_preauth_payload;
+
+typedef struct fcs_cs_key_payload_t {
+	uint32_t session_id;
+	uint32_t reserved0;
+	uint32_t reserved1;
+	uint32_t key_id;
+} fcs_cs_key_payload;
+
+typedef struct fcs_crypto_service_data_t {
+	uint32_t session_id;
+	uint32_t context_id;
+	uint32_t key_id;
+	uint32_t crypto_param_size;
+	uint64_t crypto_param;
+	uint8_t is_updated;
+} fcs_crypto_service_data;
+
+typedef struct fcs_crypto_service_aes_data_t {
+	uint32_t session_id;
+	uint32_t context_id;
+	uint32_t param_size;
+	uint32_t key_id;
+	uint32_t crypto_param[7];
+	uint8_t is_updated;
+} fcs_crypto_service_aes_data;
 
 /* Functions Definitions */
 
 uint32_t intel_fcs_random_number_gen(uint64_t addr, uint64_t *ret_size,
 				uint32_t *mbox_error);
+int intel_fcs_random_number_gen_ext(uint32_t session_id, uint32_t context_id,
+				uint32_t size, uint32_t *send_id);
 uint32_t intel_fcs_send_cert(uint64_t addr, uint64_t size,
 				uint32_t *send_id);
 uint32_t intel_fcs_get_provision_data(uint32_t *send_id);
-uint32_t intel_fcs_cryption(uint32_t mode, uint32_t src_addr,
-			uint32_t src_size, uint32_t dst_addr,
-			uint32_t dst_size, uint32_t *send_id);
+uint32_t intel_fcs_cntr_set_preauth(uint8_t counter_type,
+				int32_t counter_value,
+				uint32_t test_bit,
+				uint32_t *mbox_error);
+uint32_t intel_fcs_encryption(uint32_t src_addr, uint32_t src_size,
+				uint32_t dst_addr, uint32_t dst_size,
+				uint32_t *send_id);
+
+uint32_t intel_fcs_decryption(uint32_t src_addr, uint32_t src_size,
+				uint32_t dst_addr, uint32_t dst_size,
+				uint32_t *send_id);
 
+int intel_fcs_encryption_ext(uint32_t session_id, uint32_t context_id,
+				uint32_t src_addr, uint32_t src_size,
+				uint32_t dst_addr, uint32_t *dst_size,
+				uint32_t *mbox_error);
+int intel_fcs_decryption_ext(uint32_t sesion_id, uint32_t context_id,
+				uint32_t src_addr, uint32_t src_size,
+				uint32_t dst_addr, uint32_t *dst_size,
+				uint32_t *mbox_error);
+
+int intel_fcs_sigma_teardown(uint32_t session_id, uint32_t *mbox_error);
+int intel_fcs_chip_id(uint32_t *id_low, uint32_t *id_high, uint32_t *mbox_error);
+int intel_fcs_attestation_subkey(uint64_t src_addr, uint32_t src_size,
+				uint64_t dst_addr, uint32_t *dst_size,
+				uint32_t *mbox_error);
+int intel_fcs_get_measurement(uint64_t src_addr, uint32_t src_size,
+				uint64_t dst_addr, uint32_t *dst_size,
+				uint32_t *mbox_error);
 uint32_t intel_fcs_get_rom_patch_sha384(uint64_t addr, uint64_t *ret_size,
 				uint32_t *mbox_error);
 
+int intel_fcs_create_cert_on_reload(uint32_t cert_request,
+				uint32_t *mbox_error);
+int intel_fcs_get_attestation_cert(uint32_t cert_request, uint64_t dst_addr,
+				uint32_t *dst_size, uint32_t *mbox_error);
+
+int intel_fcs_open_crypto_service_session(uint32_t *session_id,
+				uint32_t *mbox_error);
+int intel_fcs_close_crypto_service_session(uint32_t session_id,
+				uint32_t *mbox_error);
+
+int intel_fcs_import_crypto_service_key(uint64_t src_addr, uint32_t src_size,
+				uint32_t *mbox_error);
+int intel_fcs_export_crypto_service_key(uint32_t session_id, uint32_t key_id,
+				uint64_t dst_addr, uint32_t *dst_size,
+				uint32_t *mbox_error);
+int intel_fcs_remove_crypto_service_key(uint32_t session_id, uint32_t key_id,
+				uint32_t *mbox_error);
+int intel_fcs_get_crypto_service_key_info(uint32_t session_id, uint32_t key_id,
+				uint64_t dst_addr, uint32_t *dst_size,
+				uint32_t *mbox_error);
+
+int intel_fcs_get_digest_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);
+int intel_fcs_get_digest_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);
+
+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);
+int intel_fcs_mac_verify_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);
+
+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);
+int intel_fcs_ecdsa_hash_sign_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 *mbox_error);
+
+int intel_fcs_ecdsa_hash_sig_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);
+int intel_fcs_ecdsa_hash_sig_verify_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 *mbox_error);
+
+int intel_fcs_ecdsa_sha2_data_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);
+int intel_fcs_ecdsa_sha2_data_sign_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);
+
+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,
+				uint32_t *mbox_error);
+int intel_fcs_ecdsa_sha2_data_sig_verify_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);
+
+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);
+int intel_fcs_ecdsa_get_pubkey_finalize(uint32_t session_id, uint32_t context_id,
+				uint64_t dst_addr, uint32_t *dst_size,
+				uint32_t *mbox_error);
+
+int intel_fcs_ecdh_request_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);
+int intel_fcs_ecdh_request_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 *mbox_error);
+
+int intel_fcs_aes_crypt_init(uint32_t session_id, uint32_t context_id,
+				uint32_t key_id, uint64_t param_addr,
+				uint32_t param_size, uint32_t *mbox_error);
+int intel_fcs_aes_crypt_update_finalize(uint32_t session_id,
+				uint32_t context_id, uint64_t src_addr,
+				uint32_t src_size, uint64_t dst_addr,
+				uint32_t dst_size, uint8_t is_finalised,
+				uint32_t *send_id);
+
 #endif /* SOCFPGA_FCS_H */

diff --git a/plat/intel/soc/common/include/socfpga_mailbox.h b/plat/intel/soc/common/include/socfpga_mailbox.h
index a6a3565..1f4b2a4 100644
--- a/plat/intel/soc/common/include/socfpga_mailbox.h
+++ b/plat/intel/soc/common/include/socfpga_mailbox.h

@@ -10,89 +10,124 @@
 #include <lib/utils_def.h>
 
 
-#define MBOX_OFFSET			0xffa30000
+#define MBOX_OFFSET					0xffa30000
 
-#define MBOX_ATF_CLIENT_ID		0x1U
-#define MBOX_MAX_JOB_ID			0xFU
-#define MBOX_MAX_IND_JOB_ID		(MBOX_MAX_JOB_ID - 1U)
-#define MBOX_JOB_ID			MBOX_MAX_JOB_ID
-
+#define MBOX_ATF_CLIENT_ID				0x1U
+#define MBOX_MAX_JOB_ID					0xFU
+#define MBOX_MAX_IND_JOB_ID				(MBOX_MAX_JOB_ID - 1U)
+#define MBOX_JOB_ID					MBOX_MAX_JOB_ID
+#define MBOX_TEST_BIT					BIT(31)
 
 /* Mailbox Shared Memory Register Map */
-#define MBOX_CIN			0x00
-#define MBOX_ROUT			0x04
-#define MBOX_URG			0x08
-#define MBOX_INT			0x0C
-#define MBOX_COUT			0x20
-#define MBOX_RIN			0x24
-#define MBOX_STATUS			0x2C
-#define MBOX_CMD_BUFFER			0x40
-#define MBOX_RESP_BUFFER		0xC0
+#define MBOX_CIN					0x00
+#define MBOX_ROUT					0x04
+#define MBOX_URG					0x08
+#define MBOX_INT					0x0C
+#define MBOX_COUT					0x20
+#define MBOX_RIN					0x24
+#define MBOX_STATUS					0x2C
+#define MBOX_CMD_BUFFER					0x40
+#define MBOX_RESP_BUFFER				0xC0
 
 /* Mailbox SDM doorbell */
-#define MBOX_DOORBELL_TO_SDM		0x400
-#define MBOX_DOORBELL_FROM_SDM		0x480
+#define MBOX_DOORBELL_TO_SDM				0x400
+#define MBOX_DOORBELL_FROM_SDM				0x480
 
 
 /* Mailbox commands */
 
-#define MBOX_CMD_NOOP			0x00
-#define MBOX_CMD_SYNC			0x01
-#define MBOX_CMD_RESTART		0x02
-#define MBOX_CMD_CANCEL			0x03
-#define MBOX_CMD_VAB_SRC_CERT		0x0B
-#define MBOX_CMD_GET_IDCODE		0x10
-#define MBOX_CMD_REBOOT_HPS		0x47
+#define MBOX_CMD_NOOP					0x00
+#define MBOX_CMD_SYNC					0x01
+#define MBOX_CMD_RESTART				0x02
+#define MBOX_CMD_CANCEL					0x03
+#define MBOX_CMD_VAB_SRC_CERT				0x0B
+#define MBOX_CMD_GET_IDCODE				0x10
+#define MBOX_CMD_GET_USERCODE				0x13
+#define MBOX_CMD_GET_CHIPID				0x12
+#define MBOX_CMD_REBOOT_HPS				0x47
 
 /* Reconfiguration Commands */
-#define MBOX_CONFIG_STATUS		0x04
-#define MBOX_RECONFIG			0x06
-#define MBOX_RECONFIG_DATA		0x08
-#define MBOX_RECONFIG_STATUS		0x09
+#define MBOX_CONFIG_STATUS				0x04
+#define MBOX_RECONFIG					0x06
+#define MBOX_RECONFIG_DATA				0x08
+#define MBOX_RECONFIG_STATUS				0x09
+
+/* HWMON Commands */
+#define MBOX_HWMON_READVOLT				0x18
+#define MBOX_HWMON_READTEMP				0x19
+
 
 /* QSPI Commands */
-#define MBOX_CMD_QSPI_OPEN		0x32
-#define MBOX_CMD_QSPI_CLOSE		0x33
-#define MBOX_CMD_QSPI_SET_CS		0x34
-#define MBOX_CMD_QSPI_DIRECT		0x3B
+#define MBOX_CMD_QSPI_OPEN				0x32
+#define MBOX_CMD_QSPI_CLOSE				0x33
+#define MBOX_CMD_QSPI_SET_CS				0x34
+#define MBOX_CMD_QSPI_DIRECT				0x3B
 
 /* RSU Commands */
-#define MBOX_GET_SUBPARTITION_TABLE	0x5A
-#define MBOX_RSU_STATUS			0x5B
-#define MBOX_RSU_UPDATE			0x5C
-#define MBOX_HPS_STAGE_NOTIFY		0x5D
+#define MBOX_GET_SUBPARTITION_TABLE			0x5A
+#define MBOX_RSU_STATUS					0x5B
+#define MBOX_RSU_UPDATE					0x5C
+#define MBOX_HPS_STAGE_NOTIFY				0x5D
 
 /* FCS Command */
-#define MBOX_FCS_GET_PROVISION			0x7B
-#define MBOX_FCS_ENCRYPT_REQ			0x7E
-#define MBOX_FCS_DECRYPT_REQ			0x7F
-#define MBOX_FCS_RANDOM_GEN			0x80
+#define MBOX_FCS_GET_PROVISION				0x7B
+#define MBOX_FCS_CNTR_SET_PREAUTH			0x7C
+#define MBOX_FCS_ENCRYPT_REQ				0x7E
+#define MBOX_FCS_DECRYPT_REQ				0x7F
+#define MBOX_FCS_RANDOM_GEN				0x80
+#define MBOX_FCS_AES_CRYPT_REQ				0x81
+#define MBOX_FCS_GET_DIGEST_REQ				0x82
+#define MBOX_FCS_MAC_VERIFY_REQ				0x83
+#define MBOX_FCS_ECDSA_HASH_SIGN_REQ			0x84
+#define MBOX_FCS_ECDSA_SHA2_DATA_SIGN_REQ		0x85
+#define MBOX_FCS_ECDSA_HASH_SIG_VERIFY			0x86
+#define MBOX_FCS_ECDSA_SHA2_DATA_SIGN_VERIFY		0x87
+#define MBOX_FCS_ECDSA_GET_PUBKEY			0x88
+#define MBOX_FCS_ECDH_REQUEST				0x89
+#define MBOX_FCS_OPEN_CS_SESSION			0xA0
+#define MBOX_FCS_CLOSE_CS_SESSION			0xA1
+#define MBOX_FCS_IMPORT_CS_KEY				0xA5
+#define MBOX_FCS_EXPORT_CS_KEY				0xA6
+#define MBOX_FCS_REMOVE_CS_KEY				0xA7
+#define MBOX_FCS_GET_CS_KEY_INFO			0xA8
+
+/* PSG SIGMA Commands */
+#define MBOX_PSG_SIGMA_TEARDOWN				0xD5
+
+/* Attestation Commands */
+#define MBOX_CREATE_CERT_ON_RELOAD			0x180
+#define MBOX_GET_ATTESTATION_CERT			0x181
+#define MBOX_ATTESTATION_SUBKEY				0x182
+#define MBOX_GET_MEASUREMENT				0x183
+
 /* Miscellaneous commands */
 #define MBOX_GET_ROM_PATCH_SHA384	0x1B0
 
 /* Mailbox Definitions */
 
-#define CMD_DIRECT			0
-#define CMD_INDIRECT			1
-#define CMD_CASUAL			0
-#define CMD_URGENT			1
+#define CMD_DIRECT					0
+#define CMD_INDIRECT					1
+#define CMD_CASUAL					0
+#define CMD_URGENT					1
 
-#define MBOX_WORD_BYTE			4U
-#define MBOX_RESP_BUFFER_SIZE		16
-#define MBOX_CMD_BUFFER_SIZE		32
+#define MBOX_WORD_BYTE					4U
+#define MBOX_RESP_BUFFER_SIZE				16
+#define MBOX_CMD_BUFFER_SIZE				32
+#define MBOX_INC_HEADER_MAX_WORD_SIZE			1024U
 
 /* Execution states for HPS_STAGE_NOTIFY */
-#define HPS_EXECUTION_STATE_FSBL	0
-#define HPS_EXECUTION_STATE_SSBL	1
-#define HPS_EXECUTION_STATE_OS		2
+#define HPS_EXECUTION_STATE_FSBL			0
+#define HPS_EXECUTION_STATE_SSBL			1
+#define HPS_EXECUTION_STATE_OS				2
 
 /* Status Response */
-#define MBOX_RET_OK			0
-#define MBOX_RET_ERROR			-1
-#define MBOX_NO_RESPONSE		-2
-#define MBOX_WRONG_ID			-3
-#define MBOX_BUFFER_FULL		-4
-#define MBOX_TIMEOUT			-2047
+#define MBOX_RET_OK					0
+#define MBOX_RET_ERROR					-1
+#define MBOX_NO_RESPONSE				-2
+#define MBOX_WRONG_ID					-3
+#define MBOX_BUFFER_FULL				-4
+#define MBOX_BUSY					-5
+#define MBOX_TIMEOUT					-2047
 
 /* Reconfig Status Response */
 #define RECONFIG_STATUS_STATE				0
@@ -117,34 +152,55 @@
 
 /* Mailbox Macros */
 
-#define MBOX_ENTRY_TO_ADDR(_buf, ptr)	(MBOX_OFFSET + (MBOX_##_buf##_BUFFER) \
-						+ MBOX_WORD_BYTE * (ptr))
+#define MBOX_ENTRY_TO_ADDR(_buf, ptr)			(MBOX_OFFSET + (MBOX_##_buf##_BUFFER) \
+								+ MBOX_WORD_BYTE * (ptr))
 
 /* Mailbox interrupt flags and masks */
-#define MBOX_INT_FLAG_COE		0x1
-#define MBOX_INT_FLAG_RIE		0x2
-#define MBOX_INT_FLAG_UAE		0x100
-#define MBOX_COE_BIT(INTERRUPT)		((INTERRUPT) & 0x3)
-#define MBOX_UAE_BIT(INTERRUPT)		(((INTERRUPT) & (1<<8)))
+#define MBOX_INT_FLAG_COE				0x1
+#define MBOX_INT_FLAG_RIE				0x2
+#define MBOX_INT_FLAG_UAE				0x100
+#define MBOX_COE_BIT(INTERRUPT)				((INTERRUPT) & 0x3)
+#define MBOX_UAE_BIT(INTERRUPT)				(((INTERRUPT) & (1<<8)))
 
 /* Mailbox response and status */
-#define MBOX_RESP_ERR(BUFFER)		((BUFFER) & 0x00000fff)
-#define MBOX_RESP_LEN(BUFFER)		(((BUFFER) & 0x007ff000) >> 12)
-#define MBOX_RESP_CLIENT_ID(BUFFER)	(((BUFFER) & 0xf0000000) >> 28)
-#define MBOX_RESP_JOB_ID(BUFFER)	(((BUFFER) & 0x0f000000) >> 24)
-#define MBOX_STATUS_UA_MASK		(1<<8)
+#define MBOX_RESP_ERR(BUFFER)				((BUFFER) & 0x000007ff)
+#define MBOX_RESP_LEN(BUFFER)				(((BUFFER) & 0x007ff000) >> 12)
+#define MBOX_RESP_CLIENT_ID(BUFFER)			(((BUFFER) & 0xf0000000) >> 28)
+#define MBOX_RESP_JOB_ID(BUFFER)			(((BUFFER) & 0x0f000000) >> 24)
+#define MBOX_STATUS_UA_MASK				(1<<8)
 
 /* Mailbox command and response */
-#define MBOX_CLIENT_ID_CMD(CLIENT_ID)	((CLIENT_ID) << 28)
-#define MBOX_JOB_ID_CMD(JOB_ID)		(JOB_ID<<24)
-#define MBOX_CMD_LEN_CMD(CMD_LEN)	((CMD_LEN) << 12)
-#define MBOX_INDIRECT(val)		((val) << 11)
-#define MBOX_CMD_MASK(header)		((header) & 0x7ff)
+#define MBOX_CLIENT_ID_CMD(CLIENT_ID)			((CLIENT_ID) << 28)
+#define MBOX_JOB_ID_CMD(JOB_ID)				(JOB_ID<<24)
+#define MBOX_CMD_LEN_CMD(CMD_LEN)			((CMD_LEN) << 12)
+#define MBOX_INDIRECT(val)				((val) << 11)
+#define MBOX_CMD_MASK(header)				((header) & 0x7ff)
+
+/* Mailbox payload */
+#define MBOX_DATA_MAX_LEN				0x3ff
+#define MBOX_PAYLOAD_FLAG_BUSY				BIT(0)
 
 /* RSU Macros */
-#define RSU_VERSION_ACMF		BIT(8)
-#define RSU_VERSION_ACMF_MASK		0xff00
+#define RSU_VERSION_ACMF				BIT(8)
+#define RSU_VERSION_ACMF_MASK				0xff00
+
+/* Config Status Macros */
+#define CONFIG_STATUS_WORD_SIZE		16U
+#define CONFIG_STATUS_FW_VER_OFFSET	1
+#define CONFIG_STATUS_FW_VER_MASK	0x00FFFFFF
 
+/* Data structure */
+
+typedef struct mailbox_payload {
+	uint32_t header;
+	uint32_t data[MBOX_DATA_MAX_LEN];
+} mailbox_payload_t;
+
+typedef struct mailbox_container {
+	uint32_t flag;
+	uint32_t index;
+	mailbox_payload_t *payload;
+} mailbox_container_t;
 
 /* Mailbox Function Definitions */
 
@@ -158,8 +214,13 @@
 			unsigned int *resp_len);
 int mailbox_send_cmd_async(uint32_t *job_id, uint32_t cmd, uint32_t *args,
 			unsigned int len, unsigned int indirect);
+int mailbox_send_cmd_async_ext(uint32_t header_cmd, uint32_t *args,
+			unsigned int len);
 int mailbox_read_response(uint32_t *job_id, uint32_t *response,
 			unsigned int *resp_len);
+int mailbox_read_response_async(uint32_t *job_id, uint32_t *header,
+			uint32_t *response, unsigned int *resp_len,
+			uint8_t ignore_client_id);
 int iterate_resp(uint32_t mbox_resp_len, uint32_t *resp_buf,
 			unsigned int *resp_len);
 
@@ -173,5 +234,7 @@
 int mailbox_rsu_status(uint32_t *resp_buf, uint32_t resp_buf_len);
 int mailbox_rsu_update(uint32_t *flash_offset);
 int mailbox_hps_stage_notify(uint32_t execution_stage);
+int mailbox_hwmon_readtemp(uint32_t chan, uint32_t *resp_buf);
+int mailbox_hwmon_readvolt(uint32_t chan, uint32_t *resp_buf);
 
 #endif /* SOCFPGA_MBOX_H */

diff --git a/plat/intel/soc/common/include/socfpga_noc.h b/plat/intel/soc/common/include/socfpga_noc.h
new file mode 100644
index 0000000..e3c0f73
--- /dev/null
+++ b/plat/intel/soc/common/include/socfpga_noc.h

@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2020-2022, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SOCFPGA_NOC_H
+#define SOCFPGA_NOC_H
+
+/* Macros */
+#define SCR_AXI_AP_MASK					BIT(24)
+#define SCR_FPGA2SOC_MASK				BIT(16)
+#define SCR_MPU_MASK					BIT(0)
+#define DISABLE_L4_FIREWALL		(SCR_AXI_AP_MASK | SCR_FPGA2SOC_MASK \
+						| SCR_MPU_MASK)
+#define DISABLE_BRIDGE_FIREWALL				0x0ffe0101
+
+#define SOCFPGA_CCU_NOC(_ctrl, _dev)	(SOCFPGA_CCU_NOC_REG_BASE \
+					+ (SOCFPGA_CCU_NOC_##_ctrl##_##_dev))
+
+#define SOCFPGA_L4_PER_SCR(_reg)	(SOCFPGA_L4_PER_SCR_REG_BASE \
+					+ (SOCFPGA_NOC_FW_L4_PER_SCR_##_reg))
+
+#define SOCFPGA_L4_SYS_SCR(_reg)	(SOCFPGA_L4_SYS_SCR_REG_BASE \
+					+ (SOCFPGA_NOC_FW_L4_SYS_SCR_##_reg))
+
+/* L3 Interconnect Register Map */
+#define SOCFPGA_NOC_FW_L4_PER_SCR_NAND_REGISTER			0x0000
+#define SOCFPGA_NOC_FW_L4_PER_SCR_NAND_DATA			0x0004
+#define SOCFPGA_NOC_FW_L4_PER_SCR_USB0_REGISTER			0x000c
+#define SOCFPGA_NOC_FW_L4_PER_SCR_USB1_REGISTER			0x0010
+#define SOCFPGA_NOC_FW_L4_PER_SCR_SPI_MASTER0			0x001c
+#define SOCFPGA_NOC_FW_L4_PER_SCR_SPI_MASTER1			0x0020
+#define SOCFPGA_NOC_FW_L4_PER_SCR_SPI_SLAVE0			0x0024
+#define SOCFPGA_NOC_FW_L4_PER_SCR_SPI_SLAVE1			0x0028
+#define SOCFPGA_NOC_FW_L4_PER_SCR_EMAC0				0x002c
+#define SOCFPGA_NOC_FW_L4_PER_SCR_EMAC1				0x0030
+#define SOCFPGA_NOC_FW_L4_PER_SCR_EMAC2				0x0034
+#define SOCFPGA_NOC_FW_L4_PER_SCR_SDMMC				0x0040
+#define SOCFPGA_NOC_FW_L4_PER_SCR_GPIO0				0x0044
+#define SOCFPGA_NOC_FW_L4_PER_SCR_GPIO1				0x0048
+#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C0				0x0050
+#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C1				0x0054
+#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C2				0x0058
+#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C3				0x005c
+#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C4				0x0060
+#define SOCFPGA_NOC_FW_L4_PER_SCR_SP_TIMER0			0x0064
+#define SOCFPGA_NOC_FW_L4_PER_SCR_SP_TIMER1			0x0068
+#define SOCFPGA_NOC_FW_L4_PER_SCR_UART0				0x006c
+#define SOCFPGA_NOC_FW_L4_PER_SCR_UART1				0x0070
+
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_DMA_ECC			0x0008
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC0RX_ECC			0x000c
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC0TX_ECC			0x0010
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC1RX_ECC			0x0014
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC1TX_ECC			0x0018
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC2RX_ECC			0x001c
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC2TX_ECC			0x0020
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_NAND_ECC			0x002c
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_NAND_READ_ECC			0x0030
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_NAND_WRITE_ECC		0x0034
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_OCRAM_ECC			0x0038
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_SDMMC_ECC			0x0040
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_USB0_ECC			0x0044
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_USB1_ECC			0x0048
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_CLK_MGR			0x004c
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_IO_MGR			0x0054
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_RST_MGR			0x0058
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_SYS_MGR			0x005c
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_OSC0_TIMER			0x0060
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_OSC1_TIMER			0x0064
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_WATCHDOG0			0x0068
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_WATCHDOG1			0x006c
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_WATCHDOG2			0x0070
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_WATCHDOG3			0x0074
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_DAP				0x0078
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_L4_NOC_PROBES			0x0090
+#define SOCFPGA_NOC_FW_L4_SYS_SCR_L4_NOC_QOS			0x0094
+
+/* CCU NOC Register Map */
+
+#define SOCFPGA_CCU_NOC_CPU0_RAM0				0x04688
+#define SOCFPGA_CCU_NOC_IOM_RAM0				0x18628
+
+#define SOCFPGA_CCU_NOC_ADMASK_P_MASK				BIT(0)
+#define SOCFPGA_CCU_NOC_ADMASK_NS_MASK				BIT(1)
+
+/* Function Definitions */
+
+void enable_ns_peripheral_access(void);
+void enable_ns_bridge_access(void);
+void enable_ns_ocram_access(void);
+void enable_ocram_firewall(void);
+
+#endif

diff --git a/plat/intel/soc/common/include/socfpga_private.h b/plat/intel/soc/common/include/socfpga_private.h
index ca38f62..9d389e3 100644
--- a/plat/intel/soc/common/include/socfpga_private.h
+++ b/plat/intel/soc/common/include/socfpga_private.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
  */
@@ -55,6 +55,8 @@
 
 void socfpga_gic_driver_init(void);
 
+void socfpga_delay_timer_init_args(void);
+
 uint32_t socfpga_get_spsr_for_bl32_entry(void);
 
 uint32_t socfpga_get_spsr_for_bl33_entry(void);

diff --git a/plat/intel/soc/common/include/socfpga_reset_manager.h b/plat/intel/soc/common/include/socfpga_reset_manager.h
index a976df7..cce16ab 100644
--- a/plat/intel/soc/common/include/socfpga_reset_manager.h
+++ b/plat/intel/soc/common/include/socfpga_reset_manager.h

@@ -9,11 +9,22 @@
 
 #include "socfpga_plat_def.h"
 
+#define SOCFPGA_BRIDGE_ENABLE			BIT(0)
+#define SOCFPGA_BRIDGE_HAS_MASK			BIT(1)
+
+#define SOC2FPGA_MASK				(1<<0)
+#define LWHPS2FPGA_MASK				(1<<1)
+#define FPGA2SOC_MASK				(1<<2)
+#define F2SDRAM0_MASK				(1<<3)
+#define F2SDRAM1_MASK				(1<<4)
+#define F2SDRAM2_MASK				(1<<5)
 
 /* Register Mapping */
 
 #define SOCFPGA_RSTMGR_STAT			0x000
 #define SOCFPGA_RSTMGR_HDSKEN			0x010
+#define SOCFPGA_RSTMGR_HDSKREQ			0x014
+#define SOCFPGA_RSTMGR_HDSKACK			0x018
 #define SOCFPGA_RSTMGR_MPUMODRST		0x020
 #define SOCFPGA_RSTMGR_PER0MODRST		0x024
 #define SOCFPGA_RSTMGR_PER1MODRST		0x028
@@ -78,14 +89,20 @@
 #define RSTMGR_HDSKEN_DEBUG_L3NOC		0x00020000
 #define RSTMGR_HDSKEN_SDRSELFREFEN		0x00000001
 
+#define RSTMGR_HDSKEQ_FPGAHSREQ			0x4
+
 #define RSTMGR_BRGMODRST_SOC2FPGA		0x1
 #define RSTMGR_BRGMODRST_LWHPS2FPGA		0x2
 #define RSTMGR_BRGMODRST_FPGA2SOC		0x4
+#define RSTMGR_BRGMODRST_F2SSDRAM0		0x8
 #define RSTMGR_BRGMODRST_F2SSDRAM1		0x10
 #define RSTMGR_BRGMODRST_F2SSDRAM2		0x20
 #define RSTMGR_BRGMODRST_MPFE			0x40
 #define RSTMGR_BRGMODRST_DDRSCH			0x40
 
+#define RSTMGR_HDSKREQ_FPGAHSREQ		(BIT(2))
+#define RSTMGR_HDSKACK_FPGAHSACK_MASK		(BIT(2))
+
 /* Definitions */
 
 #define RSTMGR_L2_MODRST			0x0100
@@ -94,7 +111,7 @@
 /* Macros */
 
 #define SOCFPGA_RSTMGR(_reg)		(SOCFPGA_RSTMGR_REG_BASE \
-						+ (SOCFPGA_RSTMGR_##_reg))
+					+ (SOCFPGA_RSTMGR_##_reg))
 #define RSTMGR_FIELD(_reg, _field)	(RSTMGR_##_reg##MODRST_##_field)
 
 /* Function Declarations */
@@ -102,7 +119,7 @@
 void deassert_peripheral_reset(void);
 void config_hps_hs_before_warm_reset(void);
 
-int socfpga_bridges_enable(void);
-int socfpga_bridges_disable(void);
+int socfpga_bridges_enable(uint32_t mask);
+int socfpga_bridges_disable(uint32_t mask);
 
 #endif /* SOCFPGA_RESETMANAGER_H */

diff --git a/plat/intel/soc/common/include/socfpga_sip_svc.h b/plat/intel/soc/common/include/socfpga_sip_svc.h
index 4d31c77..0803eb5 100644
--- a/plat/intel/soc/common/include/socfpga_sip_svc.h
+++ b/plat/intel/soc/common/include/socfpga_sip_svc.h

@@ -9,23 +9,43 @@
 
 
 /* SiP status response */
-#define INTEL_SIP_SMC_STATUS_OK				0
-#define INTEL_SIP_SMC_STATUS_BUSY			0x1
-#define INTEL_SIP_SMC_STATUS_REJECTED			0x2
-#define INTEL_SIP_SMC_STATUS_ERROR			0x4
-#define INTEL_SIP_SMC_RSU_ERROR				0x7
+#define INTEL_SIP_SMC_STATUS_OK					0
+#define INTEL_SIP_SMC_STATUS_BUSY				0x1
+#define INTEL_SIP_SMC_STATUS_REJECTED				0x2
+#define INTEL_SIP_SMC_STATUS_NO_RESPONSE			0x3
+#define INTEL_SIP_SMC_STATUS_ERROR				0x4
+#define INTEL_SIP_SMC_RSU_ERROR					0x7
 
 /* SiP mailbox error code */
-#define GENERIC_RESPONSE_ERROR				0x3FF
+#define GENERIC_RESPONSE_ERROR					0x3FF
 
-/* SMC SiP service function identifier */
+/* SiP V2 command code range */
+#define INTEL_SIP_SMC_CMD_MASK					0xFFFF
+#define INTEL_SIP_SMC_CMD_V2_RANGE_BEGIN			0x400
+#define INTEL_SIP_SMC_CMD_V2_RANGE_END				0x4FF
+
+/* SiP V2 protocol header */
+#define INTEL_SIP_SMC_HEADER_JOB_ID_MASK			0xF
+#define INTEL_SIP_SMC_HEADER_JOB_ID_OFFSET			0U
+#define INTEL_SIP_SMC_HEADER_CID_MASK				0xF
+#define INTEL_SIP_SMC_HEADER_CID_OFFSET				4U
+#define INTEL_SIP_SMC_HEADER_VERSION_MASK			0xF
+#define INTEL_SIP_SMC_HEADER_VERSION_OFFSET			60U
+
+/* SMC SiP service function identifier for version 1 */
 
 /* FPGA Reconfig */
-#define INTEL_SIP_SMC_FPGA_CONFIG_START			0xC2000001
-#define INTEL_SIP_SMC_FPGA_CONFIG_WRITE			0x42000002
-#define INTEL_SIP_SMC_FPGA_CONFIG_COMPLETED_WRITE	0xC2000003
-#define INTEL_SIP_SMC_FPGA_CONFIG_ISDONE		0xC2000004
-#define INTEL_SIP_SMC_FPGA_CONFIG_GET_MEM		0xC2000005
+#define INTEL_SIP_SMC_FPGA_CONFIG_START				0xC2000001
+#define INTEL_SIP_SMC_FPGA_CONFIG_WRITE				0x42000002
+#define INTEL_SIP_SMC_FPGA_CONFIG_COMPLETED_WRITE		0xC2000003
+#define INTEL_SIP_SMC_FPGA_CONFIG_ISDONE			0xC2000004
+#define INTEL_SIP_SMC_FPGA_CONFIG_GET_MEM			0xC2000005
+
+/* FPGA Bitstream Flag */
+#define FLAG_PARTIAL_CONFIG					BIT(0)
+#define FLAG_AUTHENTICATION					BIT(1)
+#define CONFIG_TEST_FLAG(_flag, _type)				(((flag) & FLAG_##_type) \
+								== FLAG_##_type)
 
 /* Secure Register Access */
 #define INTEL_SIP_SMC_REG_READ				0xC2000007
@@ -33,40 +53,121 @@
 #define INTEL_SIP_SMC_REG_UPDATE			0xC2000009
 
 /* Remote System Update */
-#define INTEL_SIP_SMC_RSU_STATUS			0xC200000B
-#define INTEL_SIP_SMC_RSU_UPDATE			0xC200000C
-#define INTEL_SIP_SMC_RSU_NOTIFY			0xC200000E
-#define INTEL_SIP_SMC_RSU_RETRY_COUNTER			0xC200000F
-#define INTEL_SIP_SMC_RSU_DCMF_VERSION			0xC2000010
-#define INTEL_SIP_SMC_RSU_COPY_DCMF_VERSION		0xC2000011
+#define INTEL_SIP_SMC_RSU_STATUS				0xC200000B
+#define INTEL_SIP_SMC_RSU_UPDATE				0xC200000C
+#define INTEL_SIP_SMC_RSU_NOTIFY				0xC200000E
+#define INTEL_SIP_SMC_RSU_RETRY_COUNTER				0xC200000F
+#define INTEL_SIP_SMC_RSU_DCMF_VERSION				0xC2000010
+#define INTEL_SIP_SMC_RSU_COPY_DCMF_VERSION			0xC2000011
+#define INTEL_SIP_SMC_RSU_MAX_RETRY				0xC2000012
+#define INTEL_SIP_SMC_RSU_COPY_MAX_RETRY			0xC2000013
+#define INTEL_SIP_SMC_RSU_DCMF_STATUS				0xC2000014
+#define INTEL_SIP_SMC_RSU_COPY_DCMF_STATUS			0xC2000015
 
+/* Hardware monitor */
+#define INTEL_SIP_SMC_HWMON_READTEMP				0xC2000020
+#define INTEL_SIP_SMC_HWMON_READVOLT				0xC2000021
+#define TEMP_CHANNEL_MAX					(1 << 15)
+#define VOLT_CHANNEL_MAX					(1 << 15)
 
 /* ECC */
-#define INTEL_SIP_SMC_ECC_DBE				0xC200000D
+#define INTEL_SIP_SMC_ECC_DBE					0xC200000D
 
 /* Generic Command */
-#define INTEL_SIP_SMC_GET_ROM_PATCH_SHA384		0xC2000040
+#define INTEL_SIP_SMC_SERVICE_COMPLETED				0xC200001E
+#define INTEL_SIP_SMC_FIRMWARE_VERSION				0xC200001F
+#define INTEL_SIP_SMC_HPS_SET_BRIDGES				0xC2000032
+#define INTEL_SIP_SMC_GET_ROM_PATCH_SHA384			0xC2000040
 
-/* Send Mailbox Command */
-#define INTEL_SIP_SMC_MBOX_SEND_CMD			0xC200001E
+#define SERVICE_COMPLETED_MODE_ASYNC				0x00004F4E
 
+/* Mailbox Command */
+#define INTEL_SIP_SMC_MBOX_SEND_CMD				0xC200003C
+#define INTEL_SIP_SMC_GET_USERCODE				0xC200003D
 
-/* SiP Definitions */
+/* 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_SHA_MODE_MASK				0xF
+#define INTEL_SIP_SMC_FCS_DIGEST_SIZE_MASK			0xF
+#define INTEL_SIP_SMC_FCS_DIGEST_SIZE_OFFSET			4U
+#define INTEL_SIP_SMC_FCS_ECC_ALGO_MASK				0xF
 
 /* ECC DBE */
-#define WARM_RESET_WFI_FLAG				BIT(31)
-#define SYSMGR_ECC_DBE_COLD_RST_MASK		(SYSMGR_ECC_OCRAM_MASK |\
-							SYSMGR_ECC_DDR0_MASK |\
-							SYSMGR_ECC_DDR1_MASK)
+#define WARM_RESET_WFI_FLAG					BIT(31)
+#define SYSMGR_ECC_DBE_COLD_RST_MASK				(SYSMGR_ECC_OCRAM_MASK |\
+								SYSMGR_ECC_DDR0_MASK |\
+								SYSMGR_ECC_DDR1_MASK)
+
+/* Non-mailbox SMC Call */
+#define INTEL_SIP_SMC_SVC_VERSION				0xC2000200
+
+/**
+ * SMC SiP service function identifier for version 2
+ * Command code from 0x400 ~ 0x4FF
+ */
+
+/* V2: Non-mailbox function identifier */
+#define INTEL_SIP_SMC_V2_GET_SVC_VERSION			0xC2000400
+#define INTEL_SIP_SMC_V2_REG_READ				0xC2000401
+#define INTEL_SIP_SMC_V2_REG_WRITE				0xC2000402
+#define INTEL_SIP_SMC_V2_REG_UPDATE				0xC2000403
+#define INTEL_SIP_SMC_V2_HPS_SET_BRIDGES			0xC2000404
+
+/* V2: Mailbox function identifier */
+#define INTEL_SIP_SMC_V2_MAILBOX_SEND_COMMAND			0xC2000420
+#define INTEL_SIP_SMC_V2_MAILBOX_POLL_RESPONSE			0xC2000421
 
 /* SMC function IDs for SiP Service queries */
-#define SIP_SVC_CALL_COUNT	0x8200ff00
-#define SIP_SVC_UID		0x8200ff01
-#define SIP_SVC_VERSION		0x8200ff03
+#define SIP_SVC_CALL_COUNT					0x8200ff00
+#define SIP_SVC_UID						0x8200ff01
+#define SIP_SVC_VERSION						0x8200ff03
 
 /* SiP Service Calls version numbers */
-#define SIP_SVC_VERSION_MAJOR	0
-#define SIP_SVC_VERSION_MINOR	1
+#define SIP_SVC_VERSION_MAJOR					1
+#define SIP_SVC_VERSION_MINOR					0
 
 
 /* Structure Definitions */
@@ -79,12 +180,38 @@
 	int block_number;
 };
 
-/* Function Definitions */
+typedef enum {
+	NO_REQUEST = 0,
+	RECONFIGURATION,
+	BITSTREAM_AUTH
+} config_type;
 
+/* Function Definitions */
+bool is_size_4_bytes_aligned(uint32_t size);
 bool is_address_in_ddr_range(uint64_t addr, uint64_t size);
 
 /* ECC DBE */
 bool cold_reset_for_ecc_dbe(void);
 uint32_t intel_ecc_dbe_notification(uint64_t dbe_value);
 
+/* Secure register access */
+uint32_t intel_secure_reg_read(uint64_t reg_addr, uint32_t *retval);
+uint32_t intel_secure_reg_write(uint64_t reg_addr, uint32_t val,
+				uint32_t *retval);
+uint32_t intel_secure_reg_update(uint64_t reg_addr, uint32_t mask,
+				 uint32_t val, uint32_t *retval);
+
+/* Miscellaneous HPS services */
+uint32_t intel_hps_set_bridges(uint64_t enable, uint64_t mask);
+
+/* SiP Service handler for version 2 */
+uintptr_t sip_smc_handler_v2(uint32_t smc_fid,
+			 u_register_t x1,
+			 u_register_t x2,
+			 u_register_t x3,
+			 u_register_t x4,
+			 void *cookie,
+			 void *handle,
+			 u_register_t flags);
+
 #endif /* SOCFPGA_SIP_SVC_H */

diff --git a/plat/intel/soc/common/include/socfpga_system_manager.h b/plat/intel/soc/common/include/socfpga_system_manager.h
index 2b13f1f..7f67313 100644
--- a/plat/intel/soc/common/include/socfpga_system_manager.h
+++ b/plat/intel/soc/common/include/socfpga_system_manager.h

@@ -38,17 +38,10 @@
 #define SYSMGR_SDMMC_DRVSEL(x)			(((x) & 0x7) << 0)
 #define SYSMGR_SDMMC_SMPLSEL(x)			(((x) & 0x7) << 4)
 
-#define IDLE_DATA_LWSOC2FPGA				BIT(0)
-#define IDLE_DATA_SOC2FPGA				BIT(4)
+#define IDLE_DATA_LWSOC2FPGA				BIT(4)
+#define IDLE_DATA_SOC2FPGA				BIT(0)
 #define IDLE_DATA_MASK		(IDLE_DATA_LWSOC2FPGA | IDLE_DATA_SOC2FPGA)
 
-#define SCR_AXI_AP_MASK					BIT(24)
-#define SCR_FPGA2SOC_MASK				BIT(16)
-#define SCR_MPU_MASK					BIT(0)
-#define DISABLE_L4_FIREWALL	(SCR_AXI_AP_MASK | SCR_FPGA2SOC_MASK \
-					| SCR_MPU_MASK)
-#define DISABLE_BRIDGE_FIREWALL				0x0ffe0101
-
 #define SYSMGR_ECC_OCRAM_MASK				BIT(1)
 #define SYSMGR_ECC_DDR0_MASK				BIT(16)
 #define SYSMGR_ECC_DDR1_MASK				BIT(17)
@@ -58,69 +51,4 @@
 #define SOCFPGA_SYSMGR(_reg)		(SOCFPGA_SYSMGR_REG_BASE \
 						+ (SOCFPGA_SYSMGR_##_reg))
 
-#define SOCFPGA_L4_PER_SCR(_reg)	(SOCFPGA_L4_PER_SCR_REG_BASE \
-					+ (SOCFPGA_NOC_FW_L4_PER_SCR_##_reg))
-
-#define SOCFPGA_L4_SYS_SCR(_reg)	(SOCFPGA_L4_SYS_SCR_REG_BASE \
-					+ (SOCFPGA_NOC_FW_L4_SYS_SCR_##_reg))
-
-/* L3 Interconnect Register Map */
-#define SOCFPGA_NOC_FW_L4_PER_SCR_NAND_REGISTER			0x0000
-#define SOCFPGA_NOC_FW_L4_PER_SCR_NAND_DATA			0x0004
-#define SOCFPGA_NOC_FW_L4_PER_SCR_USB0_REGISTER			0x000c
-#define SOCFPGA_NOC_FW_L4_PER_SCR_USB1_REGISTER			0x0010
-#define SOCFPGA_NOC_FW_L4_PER_SCR_SPI_MASTER0			0x001c
-#define SOCFPGA_NOC_FW_L4_PER_SCR_SPI_MASTER1			0x0020
-#define SOCFPGA_NOC_FW_L4_PER_SCR_SPI_SLAVE0			0x0024
-#define SOCFPGA_NOC_FW_L4_PER_SCR_SPI_SLAVE1			0x0028
-#define SOCFPGA_NOC_FW_L4_PER_SCR_EMAC0				0x002c
-#define SOCFPGA_NOC_FW_L4_PER_SCR_EMAC1				0x0030
-#define SOCFPGA_NOC_FW_L4_PER_SCR_EMAC2				0x0034
-#define SOCFPGA_NOC_FW_L4_PER_SCR_SDMMC				0x0040
-#define SOCFPGA_NOC_FW_L4_PER_SCR_GPIO0				0x0044
-#define SOCFPGA_NOC_FW_L4_PER_SCR_GPIO1				0x0048
-#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C0				0x0050
-#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C1				0x0054
-#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C2				0x0058
-#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C3				0x005c
-#define SOCFPGA_NOC_FW_L4_PER_SCR_I2C4				0x0060
-#define SOCFPGA_NOC_FW_L4_PER_SCR_SP_TIMER0			0x0064
-#define SOCFPGA_NOC_FW_L4_PER_SCR_SP_TIMER1			0x0068
-#define SOCFPGA_NOC_FW_L4_PER_SCR_UART0				0x006c
-#define SOCFPGA_NOC_FW_L4_PER_SCR_UART1				0x0070
-
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_DMA_ECC			0x0008
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC0RX_ECC			0x000c
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC0TX_ECC			0x0010
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC1RX_ECC			0x0014
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC1TX_ECC			0x0018
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC2RX_ECC			0x001c
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_EMAC2TX_ECC			0x0020
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_NAND_ECC			0x002c
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_NAND_READ_ECC			0x0030
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_NAND_WRITE_ECC		0x0034
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_OCRAM_ECC			0x0038
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_SDMMC_ECC			0x0040
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_USB0_ECC			0x0044
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_USB1_ECC			0x0048
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_CLK_MGR			0x004c
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_IO_MGR			0x0054
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_RST_MGR			0x0058
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_SYS_MGR			0x005c
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_OSC0_TIMER			0x0060
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_OSC1_TIMER			0x0064
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_WATCHDOG0			0x0068
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_WATCHDOG1			0x006c
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_WATCHDOG2			0x0070
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_WATCHDOG3			0x0074
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_DAP				0x0078
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_L4_NOC_PROBES			0x0090
-#define SOCFPGA_NOC_FW_L4_SYS_SCR_L4_NOC_QOS			0x0094
-
-#define SOCFPGA_CCU_NOC_CPU0_RAMSPACE0_0			0xf7004688
-#define SOCFPGA_CCU_NOC_IOM_RAMSPACE0_0				0xf7018628
-
-void enable_ns_peripheral_access(void);
-void enable_ns_bridge_access(void);
-
 #endif /* SOCFPGA_SYSTEMMANAGER_H */

diff --git a/plat/intel/soc/common/sip/socfpga_sip_fcs.c b/plat/intel/soc/common/sip/socfpga_sip_fcs.c
index 3a7d693..eacc4dd 100644
--- a/plat/intel/soc/common/sip/socfpga_sip_fcs.c
+++ b/plat/intel/soc/common/sip/socfpga_sip_fcs.c

@@ -11,6 +11,73 @@
 #include "socfpga_mailbox.h"
 #include "socfpga_sip_svc.h"
 
+/* FCS static variables */
+static fcs_crypto_service_aes_data fcs_aes_init_payload;
+static fcs_crypto_service_data fcs_sha_get_digest_param;
+static fcs_crypto_service_data fcs_sha_mac_verify_param;
+static fcs_crypto_service_data fcs_ecdsa_hash_sign_param;
+static fcs_crypto_service_data fcs_ecdsa_hash_sig_verify_param;
+static fcs_crypto_service_data fcs_sha2_data_sign_param;
+static fcs_crypto_service_data fcs_sha2_data_sig_verify_param;
+static fcs_crypto_service_data fcs_ecdsa_get_pubkey_param;
+static fcs_crypto_service_data fcs_ecdh_request_param;
+
+bool is_size_4_bytes_aligned(uint32_t size)
+{
+	if ((size % MBOX_WORD_BYTE) != 0U) {
+		return false;
+	} else {
+		return true;
+	}
+}
+
+static bool is_8_bytes_aligned(uint32_t data)
+{
+	if ((data % (MBOX_WORD_BYTE * 2U)) != 0U) {
+		return false;
+	} else {
+		return true;
+	}
+}
+
+static bool is_32_bytes_aligned(uint32_t data)
+{
+	if ((data % (8U * MBOX_WORD_BYTE)) != 0U) {
+		return false;
+	} else {
+		return true;
+	}
+}
+
+static int intel_fcs_crypto_service_init(uint32_t session_id,
+			uint32_t context_id, uint32_t key_id,
+			uint32_t param_size, uint64_t param_data,
+			fcs_crypto_service_data *data_addr,
+			uint32_t *mbox_error)
+{
+	if (mbox_error == NULL) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (param_size != 4) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	memset(data_addr, 0, sizeof(fcs_crypto_service_data));
+
+	data_addr->session_id = session_id;
+	data_addr->context_id = context_id;
+	data_addr->key_id = key_id;
+	data_addr->crypto_param_size = param_size;
+	data_addr->crypto_param = param_data;
+
+	data_addr->is_updated = 0;
+
+	*mbox_error = 0;
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
 uint32_t intel_fcs_random_number_gen(uint64_t addr, uint64_t *ret_size,
 					uint32_t *mbox_error)
 {
@@ -48,6 +115,45 @@
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
+int intel_fcs_random_number_gen_ext(uint32_t session_id, uint32_t context_id,
+				uint32_t size, uint32_t *send_id)
+{
+	int status;
+	uint32_t payload_size;
+	uint32_t crypto_header;
+
+	if (size > (FCS_RANDOM_EXT_MAX_WORD_SIZE *
+		MBOX_WORD_BYTE) || size == 0U) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_size_4_bytes_aligned(size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	crypto_header = (FCS_CS_FIELD_FLAG_INIT | FCS_CS_FIELD_FLAG_FINALIZE) <<
+			FCS_CS_FIELD_FLAG_OFFSET;
+
+	fcs_rng_payload payload = {
+		session_id,
+		context_id,
+		crypto_header,
+		size
+	};
+
+	payload_size = sizeof(payload) / MBOX_WORD_BYTE;
+
+	status = mailbox_send_cmd_async(send_id, MBOX_FCS_RANDOM_GEN,
+					(uint32_t *) &payload, payload_size,
+					CMD_INDIRECT);
+
+	if (status < 0) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
 uint32_t intel_fcs_send_cert(uint64_t addr, uint64_t size,
 					uint32_t *send_id)
 {
@@ -57,10 +163,16 @@
 		return INTEL_SIP_SMC_STATUS_REJECTED;
 	}
 
+	if (!is_size_4_bytes_aligned(size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
 	status = mailbox_send_cmd_async(send_id, MBOX_CMD_VAB_SRC_CERT,
 				(uint32_t *)addr, size / MBOX_WORD_BYTE,
 				CMD_DIRECT);
 
+	flush_dcache_range(addr, size);
+
 	if (status < 0) {
 		return INTEL_SIP_SMC_STATUS_ERROR;
 	}
@@ -76,46 +188,346 @@
 				NULL, 0U, CMD_DIRECT);
 
 	if (status < 0) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+uint32_t intel_fcs_cntr_set_preauth(uint8_t counter_type, int32_t counter_value,
+					uint32_t test_bit, uint32_t *mbox_error)
+{
+	int status;
+	uint32_t first_word;
+	uint32_t payload_size;
+
+	if ((test_bit != MBOX_TEST_BIT) &&
+		(test_bit != 0)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if ((counter_type < FCS_BIG_CNTR_SEL) ||
+		(counter_type > FCS_SVN_CNTR_3_SEL)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if ((counter_type == FCS_BIG_CNTR_SEL) &&
+		(counter_value > FCS_BIG_CNTR_VAL_MAX)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if ((counter_type >= FCS_SVN_CNTR_0_SEL) &&
+		(counter_type <= FCS_SVN_CNTR_3_SEL) &&
+		(counter_value > FCS_SVN_CNTR_VAL_MAX)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	first_word = test_bit | counter_type;
+	fcs_cntr_set_preauth_payload payload = {
+		first_word,
+		counter_value
+	};
+
+	payload_size = sizeof(payload) / MBOX_WORD_BYTE;
+	status =  mailbox_send_cmd(MBOX_JOB_ID, MBOX_FCS_CNTR_SET_PREAUTH,
+				  (uint32_t *) &payload, payload_size,
+				  CMD_CASUAL, NULL, NULL);
+
+	if (status < 0) {
+		*mbox_error = -status;
 		return INTEL_SIP_SMC_STATUS_ERROR;
 	}
 
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
-uint32_t intel_fcs_cryption(uint32_t mode, uint32_t src_addr,
-		uint32_t src_size, uint32_t dst_addr,
-		uint32_t dst_size, uint32_t *send_id)
+uint32_t intel_fcs_encryption(uint32_t src_addr, uint32_t src_size,
+		uint32_t dst_addr, uint32_t dst_size, uint32_t *send_id)
 {
 	int status;
-	uint32_t cmd;
+	uint32_t load_size;
+
+	fcs_encrypt_payload payload = {
+		FCS_ENCRYPTION_DATA_0,
+		src_addr,
+		src_size,
+		dst_addr,
+		dst_size };
+	load_size = sizeof(payload) / MBOX_WORD_BYTE;
 
 	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;
 	}
 
+	if (!is_size_4_bytes_aligned(src_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	status = mailbox_send_cmd_async(send_id, MBOX_FCS_ENCRYPT_REQ,
+				(uint32_t *) &payload, load_size,
+				CMD_INDIRECT);
+	inv_dcache_range(dst_addr, dst_size);
+
+	if (status < 0) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+uint32_t intel_fcs_decryption(uint32_t src_addr, uint32_t src_size,
+		uint32_t dst_addr, uint32_t dst_size, uint32_t *send_id)
+{
+	int status;
+	uint32_t load_size;
+	uintptr_t id_offset;
+
-	fcs_crypt_payload payload = {
-		FCS_CRYPTION_DATA_0,
+	id_offset = src_addr + FCS_OWNER_ID_OFFSET;
+	fcs_decrypt_payload payload = {
+		FCS_DECRYPTION_DATA_0,
+		{mmio_read_32(id_offset),
+		mmio_read_32(id_offset + MBOX_WORD_BYTE)},
 		src_addr,
 		src_size,
 		dst_addr,
 		dst_size };
+	load_size = sizeof(payload) / MBOX_WORD_BYTE;
 
-	if (mode != 0U) {
-		cmd = MBOX_FCS_ENCRYPT_REQ;
-	} else {
-		cmd = MBOX_FCS_DECRYPT_REQ;
+	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;
 	}
 
-	status = mailbox_send_cmd_async(send_id, cmd, (uint32_t *) &payload,
-				sizeof(fcs_crypt_payload) / MBOX_WORD_BYTE,
+	if (!is_size_4_bytes_aligned(src_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	status = mailbox_send_cmd_async(send_id, MBOX_FCS_DECRYPT_REQ,
+				(uint32_t *) &payload, load_size,
 				CMD_INDIRECT);
 	inv_dcache_range(dst_addr, dst_size);
 
 	if (status < 0) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+int intel_fcs_encryption_ext(uint32_t session_id, uint32_t context_id,
+		uint32_t src_addr, uint32_t src_size,
+		uint32_t dst_addr, uint32_t *dst_size, uint32_t *mbox_error)
+{
+	int status;
+	uint32_t payload_size;
+	uint32_t resp_len = FCS_CRYPTION_RESP_WORD_SIZE;
+	uint32_t resp_data[FCS_CRYPTION_RESP_WORD_SIZE] = {0U};
+
+	if ((dst_size == NULL) || (mbox_error == NULL)) {
+		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;
+	}
+
+	if (!is_size_4_bytes_aligned(src_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	fcs_encrypt_ext_payload payload = {
+		session_id,
+		context_id,
+		FCS_CRYPTION_CRYPTO_HEADER,
+		src_addr,
+		src_size,
+		dst_addr,
+		*dst_size
+	};
+
+	payload_size = sizeof(payload) / MBOX_WORD_BYTE;
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_FCS_ENCRYPT_REQ,
+				(uint32_t *) &payload, payload_size,
+				CMD_CASUAL, resp_data, &resp_len);
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	if (resp_len != FCS_CRYPTION_RESP_WORD_SIZE) {
+		*mbox_error = MBOX_RET_ERROR;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	*dst_size = resp_data[FCS_CRYPTION_RESP_SIZE_OFFSET];
+	inv_dcache_range(dst_addr, *dst_size);
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+int intel_fcs_decryption_ext(uint32_t session_id, uint32_t context_id,
+		uint32_t src_addr, uint32_t src_size,
+		uint32_t dst_addr, uint32_t *dst_size, uint32_t *mbox_error)
+{
+	int status;
+	uintptr_t id_offset;
+	uint32_t payload_size;
+	uint32_t resp_len = FCS_CRYPTION_RESP_WORD_SIZE;
+	uint32_t resp_data[FCS_CRYPTION_RESP_WORD_SIZE] = {0U};
+
+	if ((dst_size == NULL) || (mbox_error == NULL)) {
+		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;
+	}
+
+	if (!is_size_4_bytes_aligned(src_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	id_offset = src_addr + FCS_OWNER_ID_OFFSET;
+	fcs_decrypt_ext_payload payload = {
+		session_id,
+		context_id,
+		FCS_CRYPTION_CRYPTO_HEADER,
+		{mmio_read_32(id_offset),
+		mmio_read_32(id_offset + MBOX_WORD_BYTE)},
+		src_addr,
+		src_size,
+		dst_addr,
+		*dst_size
+	};
+
+	payload_size = sizeof(payload) / MBOX_WORD_BYTE;
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_FCS_DECRYPT_REQ,
+				(uint32_t *) &payload, payload_size,
+				CMD_CASUAL, resp_data, &resp_len);
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	if (resp_len != FCS_CRYPTION_RESP_WORD_SIZE) {
+		*mbox_error = MBOX_RET_ERROR;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	*dst_size = resp_data[FCS_CRYPTION_RESP_SIZE_OFFSET];
+	inv_dcache_range(dst_addr, *dst_size);
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+int intel_fcs_sigma_teardown(uint32_t session_id, uint32_t *mbox_error)
+{
+	int status;
+
+	if ((session_id != PSGSIGMA_SESSION_ID_ONE) &&
+		(session_id != PSGSIGMA_UNKNOWN_SESSION)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	psgsigma_teardown_msg message = {
+		RESERVED_AS_ZERO,
+		PSGSIGMA_TEARDOWN_MAGIC,
+		session_id
+	};
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_PSG_SIGMA_TEARDOWN,
+			(uint32_t *) &message, sizeof(message) / MBOX_WORD_BYTE,
+			CMD_CASUAL, NULL, NULL);
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+int intel_fcs_chip_id(uint32_t *id_low, uint32_t *id_high, uint32_t *mbox_error)
+{
+	int status;
+	uint32_t load_size;
+	uint32_t chip_id[2];
+
+	load_size = sizeof(chip_id) / MBOX_WORD_BYTE;
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_GET_CHIPID, NULL,
+			0U, CMD_CASUAL, (uint32_t *) chip_id, &load_size);
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	*id_low = chip_id[0];
+	*id_high = chip_id[1];
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+int intel_fcs_attestation_subkey(uint64_t src_addr, uint32_t src_size,
+		uint64_t dst_addr, uint32_t *dst_size, uint32_t *mbox_error)
+{
+	int status;
+	uint32_t send_size = src_size / MBOX_WORD_BYTE;
+	uint32_t ret_size = *dst_size / MBOX_WORD_BYTE;
+
+
+	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;
+	}
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_ATTESTATION_SUBKEY,
+			(uint32_t *) src_addr, send_size, CMD_CASUAL,
+			(uint32_t *) dst_addr, &ret_size);
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	*dst_size = ret_size * MBOX_WORD_BYTE;
+	flush_dcache_range(dst_addr, *dst_size);
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+int intel_fcs_get_measurement(uint64_t src_addr, uint32_t src_size,
+		uint64_t dst_addr, uint32_t *dst_size, uint32_t *mbox_error)
+{
+	int status;
+	uint32_t send_size = src_size / MBOX_WORD_BYTE;
+	uint32_t ret_size = *dst_size / MBOX_WORD_BYTE;
+
+	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;
 	}
 
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_GET_MEASUREMENT,
+			(uint32_t *) src_addr, send_size, CMD_CASUAL,
+			(uint32_t *) dst_addr, &ret_size);
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	*dst_size = ret_size * MBOX_WORD_BYTE;
+	flush_dcache_range(dst_addr, *dst_size);
+
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
@@ -148,3 +560,1180 @@
 
 	return INTEL_SIP_SMC_STATUS_OK;
 }
+
+int intel_fcs_get_attestation_cert(uint32_t cert_request, uint64_t dst_addr,
+			uint32_t *dst_size, uint32_t *mbox_error)
+{
+	int status;
+	uint32_t ret_size = *dst_size / MBOX_WORD_BYTE;
+
+	if (mbox_error == NULL) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (cert_request < FCS_ATTEST_FIRMWARE_CERT ||
+		cert_request > FCS_ATTEST_CERT_MAX_REQ_PARAM) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_address_in_ddr_range(dst_addr, *dst_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_GET_ATTESTATION_CERT,
+			(uint32_t *) &cert_request, 1U, CMD_CASUAL,
+			(uint32_t *) dst_addr, &ret_size);
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	*dst_size = ret_size * MBOX_WORD_BYTE;
+	flush_dcache_range(dst_addr, *dst_size);
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+int intel_fcs_create_cert_on_reload(uint32_t cert_request,
+			uint32_t *mbox_error)
+{
+	int status;
+
+	if (mbox_error == NULL) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (cert_request < FCS_ATTEST_FIRMWARE_CERT ||
+		cert_request > FCS_ATTEST_CERT_MAX_REQ_PARAM) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_CREATE_CERT_ON_RELOAD,
+			(uint32_t *) &cert_request, 1U, CMD_CASUAL,
+			NULL, NULL);
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+int intel_fcs_open_crypto_service_session(uint32_t *session_id,
+			uint32_t *mbox_error)
+{
+	int status;
+	uint32_t resp_len = 1U;
+
+	if ((session_id == NULL) || (mbox_error == NULL)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_FCS_OPEN_CS_SESSION,
+			NULL, 0U, CMD_CASUAL, session_id, &resp_len);
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+int intel_fcs_close_crypto_service_session(uint32_t session_id,
+			uint32_t *mbox_error)
+{
+	int status;
+
+	if (mbox_error == NULL) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_FCS_CLOSE_CS_SESSION,
+			&session_id, 1U, CMD_CASUAL, NULL, NULL);
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+int intel_fcs_import_crypto_service_key(uint64_t src_addr, uint32_t src_size,
+		uint32_t *send_id)
+{
+	int status;
+
+	if (src_size > (FCS_CS_KEY_OBJ_MAX_WORD_SIZE *
+		MBOX_WORD_BYTE)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_address_in_ddr_range(src_addr, src_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	status = mailbox_send_cmd_async(send_id, MBOX_FCS_IMPORT_CS_KEY,
+				(uint32_t *)src_addr, src_size / MBOX_WORD_BYTE,
+				CMD_INDIRECT);
+
+	if (status < 0) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+int intel_fcs_export_crypto_service_key(uint32_t session_id, uint32_t key_id,
+		uint64_t dst_addr, uint32_t *dst_size,
+		uint32_t *mbox_error)
+{
+	int status;
+	uint32_t i;
+	uint32_t payload_size;
+	uint32_t resp_len = FCS_CS_KEY_OBJ_MAX_WORD_SIZE;
+	uint32_t resp_data[FCS_CS_KEY_OBJ_MAX_WORD_SIZE] = {0U};
+	uint32_t op_status = 0U;
+
+	if ((dst_size == NULL) || (mbox_error == NULL)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_address_in_ddr_range(dst_addr, *dst_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	fcs_cs_key_payload payload = {
+		session_id,
+		RESERVED_AS_ZERO,
+		RESERVED_AS_ZERO,
+		key_id
+	};
+
+	payload_size = sizeof(payload) / MBOX_WORD_BYTE;
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_FCS_EXPORT_CS_KEY,
+			(uint32_t *) &payload, payload_size,
+			CMD_CASUAL, resp_data, &resp_len);
+
+	if (resp_len > 0) {
+		op_status = resp_data[0] & FCS_CS_KEY_RESP_STATUS_MASK;
+	}
+
+	if (status < 0) {
+		*mbox_error = (-status) | (op_status << FCS_CS_KEY_RESP_STATUS_OFFSET);
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	if (resp_len > 1) {
+
+		/* Export key object is start at second response data */
+		*dst_size = (resp_len - 1) * MBOX_WORD_BYTE;
+
+		for (i = 1U; i < resp_len; i++) {
+			mmio_write_32(dst_addr, resp_data[i]);
+			dst_addr += MBOX_WORD_BYTE;
+		}
+
+		flush_dcache_range(dst_addr - *dst_size, *dst_size);
+
+	} else {
+
+		/* Unexpected response, missing key object in response */
+		*mbox_error = MBOX_RET_ERROR;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+int intel_fcs_remove_crypto_service_key(uint32_t session_id, uint32_t key_id,
+		uint32_t *mbox_error)
+{
+	int status;
+	uint32_t payload_size;
+	uint32_t resp_len = 1U;
+	uint32_t resp_data = 0U;
+	uint32_t op_status = 0U;
+
+	if (mbox_error == NULL) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	fcs_cs_key_payload payload = {
+		session_id,
+		RESERVED_AS_ZERO,
+		RESERVED_AS_ZERO,
+		key_id
+	};
+
+	payload_size = sizeof(payload) / MBOX_WORD_BYTE;
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_FCS_REMOVE_CS_KEY,
+			(uint32_t *) &payload, payload_size,
+			CMD_CASUAL, &resp_data, &resp_len);
+
+	if (resp_len > 0) {
+		op_status = resp_data & FCS_CS_KEY_RESP_STATUS_MASK;
+	}
+
+	if (status < 0) {
+		*mbox_error = (-status) | (op_status << FCS_CS_KEY_RESP_STATUS_OFFSET);
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+int intel_fcs_get_crypto_service_key_info(uint32_t session_id, uint32_t key_id,
+		uint64_t dst_addr, uint32_t *dst_size,
+		uint32_t *mbox_error)
+{
+	int status;
+	uint32_t payload_size;
+	uint32_t resp_len = FCS_CS_KEY_INFO_MAX_WORD_SIZE;
+	uint32_t op_status = 0U;
+
+	if ((dst_size == NULL) || (mbox_error == NULL)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_address_in_ddr_range(dst_addr, *dst_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	fcs_cs_key_payload payload = {
+		session_id,
+		RESERVED_AS_ZERO,
+		RESERVED_AS_ZERO,
+		key_id
+	};
+
+	payload_size = sizeof(payload) / MBOX_WORD_BYTE;
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_FCS_GET_CS_KEY_INFO,
+				(uint32_t *) &payload, payload_size,
+				CMD_CASUAL, (uint32_t *) dst_addr, &resp_len);
+
+	if (resp_len > 0) {
+		op_status = mmio_read_32(dst_addr) &
+			FCS_CS_KEY_RESP_STATUS_MASK;
+	}
+
+	if (status < 0) {
+		*mbox_error = (-status) | (op_status << FCS_CS_KEY_RESP_STATUS_OFFSET);
+		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_get_digest_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)
+{
+	return intel_fcs_crypto_service_init(session_id, context_id,
+				key_id, param_size, param_data,
+				(void *) &fcs_sha_get_digest_param,
+				mbox_error);
+}
+
+int intel_fcs_get_digest_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)
+{
+	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};
+
+	if (dst_size == NULL || mbox_error == NULL) {
+		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;
+	}
+
+	/* Source data must be 8 bytes aligned */
+	if (!is_8_bytes_aligned(src_size)) {
+		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(MBOX_JOB_ID, MBOX_FCS_GET_DIGEST_REQ,
+				payload, i, CMD_CASUAL,
+				(uint32_t *) dst_addr, &resp_len);
+
+	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)
+{
+	return intel_fcs_crypto_service_init(session_id, context_id,
+				key_id, param_size, param_data,
+				(void *) &fcs_sha_mac_verify_param,
+				mbox_error);
+}
+
+int intel_fcs_mac_verify_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)
+{
+	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;
+
+	if (dst_size == NULL || mbox_error == NULL) {
+		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 (data_size >= src_size) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_size_4_bytes_aligned(src_size) ||
+		!is_8_bytes_aligned(data_size)) {
+		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 */
+		mac_offset = src_addr + data_size;
+		memcpy((uint8_t *) &payload[i], (uint8_t *) mac_offset,
+		src_size - data_size);
+
+		i += (src_size - data_size) / MBOX_WORD_BYTE;
+	}
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_FCS_MAC_VERIFY_REQ,
+				payload, i, CMD_CASUAL,
+				(uint32_t *) dst_addr, &resp_len);
+
+	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)
+{
+	return intel_fcs_crypto_service_init(session_id, context_id,
+				key_id, param_size, param_data,
+				(void *) &fcs_ecdsa_hash_sign_param,
+				mbox_error);
+}
+
+int intel_fcs_ecdsa_hash_sign_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 *mbox_error)
+{
+	int status;
+	uint32_t i;
+	uint32_t payload[FCS_ECDSA_HASH_SIGN_CMD_MAX_WORD_SIZE] = {0U};
+	uint32_t resp_len;
+	uintptr_t hash_data_addr;
+
+	if ((dst_size == NULL) || (mbox_error == NULL)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (fcs_ecdsa_hash_sign_param.session_id != session_id ||
+		fcs_ecdsa_hash_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 command payload */
+	/* Crypto header */
+	i = 0;
+	payload[i] = fcs_ecdsa_hash_sign_param.session_id;
+	i++;
+	payload[i] = fcs_ecdsa_hash_sign_param.context_id;
+
+	i++;
+	payload[i] = fcs_ecdsa_hash_sign_param.crypto_param_size
+			& FCS_CS_FIELD_SIZE_MASK;
+	payload[i] |= (FCS_CS_FIELD_FLAG_INIT | FCS_CS_FIELD_FLAG_UPDATE
+			| FCS_CS_FIELD_FLAG_FINALIZE)
+			<< FCS_CS_FIELD_FLAG_OFFSET;
+	i++;
+	payload[i] = fcs_ecdsa_hash_sign_param.key_id;
+
+	/* Crypto parameters */
+	i++;
+	payload[i] = fcs_ecdsa_hash_sign_param.crypto_param
+			& INTEL_SIP_SMC_FCS_ECC_ALGO_MASK;
+
+	/* Hash Data */
+	i++;
+	hash_data_addr = src_addr;
+	memcpy((uint8_t *) &payload[i], (uint8_t *) hash_data_addr,
+			src_size);
+
+	i += src_size / MBOX_WORD_BYTE;
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_FCS_ECDSA_HASH_SIGN_REQ,
+			payload, i, CMD_CASUAL, (uint32_t *) dst_addr,
+			&resp_len);
+
+	memset((void *) &fcs_ecdsa_hash_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_hash_sig_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)
+{
+	return intel_fcs_crypto_service_init(session_id, context_id,
+				key_id, param_size, param_data,
+				(void *) &fcs_ecdsa_hash_sig_verify_param,
+				mbox_error);
+}
+
+int intel_fcs_ecdsa_hash_sig_verify_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 *mbox_error)
+{
+	int status;
+	uint32_t i = 0;
+	uint32_t payload[FCS_ECDSA_HASH_SIG_VERIFY_CMD_MAX_WORD_SIZE] = {0U};
+	uint32_t resp_len;
+	uintptr_t hash_sig_pubkey_addr;
+
+	if ((dst_size == NULL) || (mbox_error == NULL)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (fcs_ecdsa_hash_sig_verify_param.session_id != session_id ||
+	fcs_ecdsa_hash_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 command payload */
+	/* Crypto header */
+	i = 0;
+	payload[i] = fcs_ecdsa_hash_sig_verify_param.session_id;
+
+	i++;
+	payload[i] = fcs_ecdsa_hash_sig_verify_param.context_id;
+
+	i++;
+	payload[i] = fcs_ecdsa_hash_sig_verify_param.crypto_param_size
+			& FCS_CS_FIELD_SIZE_MASK;
+	payload[i] |= (FCS_CS_FIELD_FLAG_INIT | FCS_CS_FIELD_FLAG_UPDATE
+			| FCS_CS_FIELD_FLAG_FINALIZE)
+			<< FCS_CS_FIELD_FLAG_OFFSET;
+
+	i++;
+	payload[i] = fcs_ecdsa_hash_sig_verify_param.key_id;
+
+	/* Crypto parameters */
+	i++;
+	payload[i] = fcs_ecdsa_hash_sig_verify_param.crypto_param
+			& INTEL_SIP_SMC_FCS_ECC_ALGO_MASK;
+
+	/* Hash Data Word, Signature Data Word and Public Key Data word */
+	i++;
+	hash_sig_pubkey_addr = src_addr;
+	memcpy((uint8_t *) &payload[i],
+			(uint8_t *) hash_sig_pubkey_addr, src_size);
+
+	i += (src_size / MBOX_WORD_BYTE);
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_FCS_ECDSA_HASH_SIG_VERIFY,
+			payload, i, CMD_CASUAL, (uint32_t *) dst_addr,
+			&resp_len);
+
+	memset((void *)&fcs_ecdsa_hash_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_sha2_data_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)
+{
+	return intel_fcs_crypto_service_init(session_id, context_id,
+				key_id, param_size, param_data,
+				(void *) &fcs_sha2_data_sign_param,
+				mbox_error);
+}
+
+int intel_fcs_ecdsa_sha2_data_sign_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)
+{
+	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;
+
+	if ((dst_size == NULL) || (mbox_error == NULL)) {
+		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;
+	}
+
+	/* Source data must be 8 bytes aligned */
+	if (!is_8_bytes_aligned(src_size)) {
+		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(MBOX_JOB_ID,
+			MBOX_FCS_ECDSA_SHA2_DATA_SIGN_REQ, payload,
+			i, CMD_CASUAL, (uint32_t *) dst_addr,
+			&resp_len);
+
+	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,
+				uint32_t *mbox_error)
+{
+	return intel_fcs_crypto_service_init(session_id, context_id,
+				key_id, param_size, param_data,
+				(void *) &fcs_sha2_data_sig_verify_param,
+				mbox_error);
+}
+
+int intel_fcs_ecdsa_sha2_data_sig_verify_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)
+{
+	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;
+
+	if ((dst_size == NULL) || (mbox_error == NULL)) {
+		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_size_4_bytes_aligned(src_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_8_bytes_aligned(data_size) ||
+		!is_8_bytes_aligned(src_addr)) {
+		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) {
+		/* Signature + Public Key Data */
+		sig_pubkey_offset = src_addr + data_size;
+		memcpy((uint8_t *) &payload[i], (uint8_t *) sig_pubkey_offset,
+			src_size - data_size);
+
+		i += (src_size - data_size) / MBOX_WORD_BYTE;
+	}
+
+	status = mailbox_send_cmd(MBOX_JOB_ID,
+			MBOX_FCS_ECDSA_SHA2_DATA_SIGN_VERIFY, payload, i,
+			CMD_CASUAL, (uint32_t *) dst_addr, &resp_len);
+
+	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)
+{
+	return intel_fcs_crypto_service_init(session_id, context_id,
+				key_id, param_size, param_data,
+				(void *) &fcs_ecdsa_get_pubkey_param,
+				mbox_error);
+}
+
+int intel_fcs_ecdsa_get_pubkey_finalize(uint32_t session_id, uint32_t context_id,
+				uint64_t dst_addr, uint32_t *dst_size,
+				uint32_t *mbox_error)
+{
+	int status;
+	int i;
+	uint32_t crypto_header;
+	uint32_t ret_size;
+	uint32_t payload[FCS_ECDSA_GET_PUBKEY_MAX_WORD_SIZE] = {0U};
+
+	if ((dst_size == NULL) || (mbox_error == NULL)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (fcs_ecdsa_get_pubkey_param.session_id != session_id ||
+		fcs_ecdsa_get_pubkey_param.context_id != context_id) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	ret_size = *dst_size / MBOX_WORD_BYTE;
+
+	crypto_header = ((FCS_CS_FIELD_FLAG_INIT |
+			FCS_CS_FIELD_FLAG_UPDATE |
+			FCS_CS_FIELD_FLAG_FINALIZE) <<
+			FCS_CS_FIELD_FLAG_OFFSET) |
+			fcs_ecdsa_get_pubkey_param.crypto_param_size;
+	i = 0;
+	/* Prepare command payload */
+	payload[i] = session_id;
+	i++;
+	payload[i] = context_id;
+	i++;
+	payload[i] = crypto_header;
+	i++;
+	payload[i] = fcs_ecdsa_get_pubkey_param.key_id;
+	i++;
+	payload[i] = (uint32_t) fcs_ecdsa_get_pubkey_param.crypto_param &
+			INTEL_SIP_SMC_FCS_ECC_ALGO_MASK;
+	i++;
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_FCS_ECDSA_GET_PUBKEY,
+			payload, i, CMD_CASUAL,
+			(uint32_t *) dst_addr, &ret_size);
+
+	memset((void *) &fcs_ecdsa_get_pubkey_param, 0,
+		sizeof(fcs_crypto_service_data));
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	*dst_size = ret_size * MBOX_WORD_BYTE;
+	flush_dcache_range(dst_addr, *dst_size);
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+int intel_fcs_ecdh_request_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)
+{
+	return intel_fcs_crypto_service_init(session_id, context_id,
+				key_id, param_size, param_data,
+				(void *) &fcs_ecdh_request_param,
+				mbox_error);
+}
+
+int intel_fcs_ecdh_request_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 *mbox_error)
+{
+	int status;
+	uint32_t i;
+	uint32_t payload[FCS_ECDH_REQUEST_CMD_MAX_WORD_SIZE] = {0U};
+	uint32_t resp_len;
+	uintptr_t pubkey;
+
+	if ((dst_size == NULL) || (mbox_error == NULL)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (fcs_ecdh_request_param.session_id != session_id ||
+		fcs_ecdh_request_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 command payload */
+	i = 0;
+	/* Crypto header */
+	payload[i] = fcs_ecdh_request_param.session_id;
+	i++;
+	payload[i] = fcs_ecdh_request_param.context_id;
+	i++;
+	payload[i] = fcs_ecdh_request_param.crypto_param_size
+			& FCS_CS_FIELD_SIZE_MASK;
+	payload[i] |= (FCS_CS_FIELD_FLAG_INIT | FCS_CS_FIELD_FLAG_UPDATE
+			| FCS_CS_FIELD_FLAG_FINALIZE)
+			<< FCS_CS_FIELD_FLAG_OFFSET;
+	i++;
+	payload[i] = fcs_ecdh_request_param.key_id;
+	i++;
+	/* Crypto parameters */
+	payload[i] = fcs_ecdh_request_param.crypto_param
+			& INTEL_SIP_SMC_FCS_ECC_ALGO_MASK;
+	i++;
+	/* Public key data */
+	pubkey = src_addr;
+	memcpy((uint8_t *) &payload[i], (uint8_t *) pubkey, src_size);
+	i += src_size / MBOX_WORD_BYTE;
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_FCS_ECDH_REQUEST,
+			payload, i, CMD_CASUAL, (uint32_t *) dst_addr,
+			&resp_len);
+
+	memset((void *)&fcs_ecdh_request_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_aes_crypt_init(uint32_t session_id, uint32_t context_id,
+				uint32_t key_id, uint64_t param_addr,
+				uint32_t param_size, uint32_t *mbox_error)
+{
+	if (mbox_error == NULL) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	memset((void *)&fcs_aes_init_payload, 0U, sizeof(fcs_aes_init_payload));
+
+	fcs_aes_init_payload.session_id = session_id;
+	fcs_aes_init_payload.context_id = context_id;
+	fcs_aes_init_payload.param_size = param_size;
+	fcs_aes_init_payload.key_id	= key_id;
+
+	memcpy((uint8_t *) fcs_aes_init_payload.crypto_param,
+		(uint8_t *) param_addr, param_size);
+
+	fcs_aes_init_payload.is_updated = 0;
+
+	*mbox_error = 0;
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+int intel_fcs_aes_crypt_update_finalize(uint32_t session_id,
+				uint32_t context_id, uint64_t src_addr,
+				uint32_t src_size, uint64_t dst_addr,
+				uint32_t dst_size, uint8_t is_finalised,
+				uint32_t *send_id)
+{
+	int status;
+	int i;
+	uint32_t flag;
+	uint32_t crypto_header;
+	uint32_t fcs_aes_crypt_payload[FCS_AES_CMD_MAX_WORD_SIZE];
+
+	if (fcs_aes_init_payload.session_id != session_id ||
+		fcs_aes_init_payload.context_id != context_id) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if ((!is_8_bytes_aligned(src_addr)) ||
+		(!is_32_bytes_aligned(src_size)) ||
+		(!is_address_in_ddr_range(src_addr, src_size))) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if ((!is_8_bytes_aligned(dst_addr)) ||
+		(!is_32_bytes_aligned(dst_size))) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if ((dst_size > FCS_AES_MAX_DATA_SIZE ||
+		dst_size < FCS_AES_MIN_DATA_SIZE) ||
+		(src_size > FCS_AES_MAX_DATA_SIZE ||
+		src_size < FCS_AES_MIN_DATA_SIZE)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	/* Prepare crypto header*/
+	flag = 0;
+	if (fcs_aes_init_payload.is_updated) {
+		fcs_aes_init_payload.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_aes_init_payload.is_updated = 1;
+	}
+	crypto_header = (flag << FCS_CS_FIELD_FLAG_OFFSET) |
+			fcs_aes_init_payload.param_size;
+
+	i = 0U;
+	fcs_aes_crypt_payload[i] = session_id;
+	i++;
+	fcs_aes_crypt_payload[i] = context_id;
+	i++;
+	fcs_aes_crypt_payload[i] = crypto_header;
+	i++;
+
+	if ((crypto_header >> FCS_CS_FIELD_FLAG_OFFSET) &
+		FCS_CS_FIELD_FLAG_INIT) {
+		fcs_aes_crypt_payload[i] = fcs_aes_init_payload.key_id;
+		i++;
+
+		memcpy((uint8_t *) &fcs_aes_crypt_payload[i],
+			(uint8_t *) fcs_aes_init_payload.crypto_param,
+			fcs_aes_init_payload.param_size);
+
+		i += fcs_aes_init_payload.param_size / MBOX_WORD_BYTE;
+	}
+
+	fcs_aes_crypt_payload[i] = (uint32_t) src_addr;
+	i++;
+	fcs_aes_crypt_payload[i] = src_size;
+	i++;
+	fcs_aes_crypt_payload[i] = (uint32_t) dst_addr;
+	i++;
+	fcs_aes_crypt_payload[i] = dst_size;
+	i++;
+
+	status = mailbox_send_cmd_async(send_id, MBOX_FCS_AES_CRYPT_REQ,
+					fcs_aes_crypt_payload, i,
+					CMD_INDIRECT);
+
+	if (is_finalised != 0U) {
+		memset((void *)&fcs_aes_init_payload, 0,
+			sizeof(fcs_aes_init_payload));
+	}
+
+	if (status < 0U) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}

diff --git a/plat/intel/soc/common/soc/socfpga_system_manager.c b/plat/intel/soc/common/soc/socfpga_firewall.c
similarity index 84%
rename from plat/intel/soc/common/soc/socfpga_system_manager.c
rename to plat/intel/soc/common/soc/socfpga_firewall.c
index a64053c..515784b 100644
--- a/plat/intel/soc/common/soc/socfpga_system_manager.c
+++ b/plat/intel/soc/common/soc/socfpga_firewall.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,6 +7,8 @@
 #include <lib/mmio.h>
 #include <lib/utils_def.h>
 
+#include "socfpga_noc.h"
+#include "socfpga_plat_def.h"
 #include "socfpga_system_manager.h"
 
 void enable_nonsecure_access(void)
@@ -92,12 +94,18 @@
 	mmio_write_32(SOCFPGA_L4_SYS_SCR(L4_NOC_QOS), DISABLE_L4_FIREWALL);
 
 #if PLATFORM_MODEL == PLAT_SOCFPGA_STRATIX10
-	mmio_clrbits_32(SOCFPGA_CCU_NOC_CPU0_RAMSPACE0_0, 0x03);
-	mmio_clrbits_32(SOCFPGA_CCU_NOC_IOM_RAMSPACE0_0, 0x03);
-
+	enable_ns_ocram_access();
 	mmio_write_32(SOCFPGA_SYSMGR(SDMMC), SYSMGR_SDMMC_DRVSEL(3));
 #endif
 
+}
+
+void enable_ns_ocram_access(void)
+{
+	mmio_clrbits_32(SOCFPGA_CCU_NOC(CPU0, RAM0),
+		SOCFPGA_CCU_NOC_ADMASK_P_MASK | SOCFPGA_CCU_NOC_ADMASK_NS_MASK);
+	mmio_clrbits_32(SOCFPGA_CCU_NOC(IOM, RAM0),
+		SOCFPGA_CCU_NOC_ADMASK_P_MASK | SOCFPGA_CCU_NOC_ADMASK_NS_MASK);
 }
 
 void enable_ns_bridge_access(void)
@@ -105,3 +113,11 @@
 	mmio_write_32(SOCFPGA_SOC2FPGA_SCR_REG_BASE, DISABLE_BRIDGE_FIREWALL);
 	mmio_write_32(SOCFPGA_LWSOC2FPGA_SCR_REG_BASE, DISABLE_BRIDGE_FIREWALL);
 }
+
+void enable_ocram_firewall(void)
+{
+	mmio_setbits_32(SOCFPGA_CCU_NOC(CPU0, RAM0),
+		SOCFPGA_CCU_NOC_ADMASK_P_MASK | SOCFPGA_CCU_NOC_ADMASK_NS_MASK);
+	mmio_setbits_32(SOCFPGA_CCU_NOC(IOM, RAM0),
+		SOCFPGA_CCU_NOC_ADMASK_P_MASK | SOCFPGA_CCU_NOC_ADMASK_NS_MASK);
+}

diff --git a/plat/intel/soc/common/soc/socfpga_mailbox.c b/plat/intel/soc/common/soc/socfpga_mailbox.c
index be900c9..778d4af 100644
--- a/plat/intel/soc/common/soc/socfpga_mailbox.c
+++ b/plat/intel/soc/common/soc/socfpga_mailbox.c

@@ -11,6 +11,8 @@
 #include "socfpga_mailbox.h"
 #include "socfpga_sip_svc.h"
 
+static mailbox_payload_t mailbox_resp_payload;
+static mailbox_container_t mailbox_resp_ctr = {0, 0, &mailbox_resp_payload};
 
 static bool is_mailbox_cmdbuf_full(uint32_t cin)
 {
@@ -171,6 +173,95 @@
 	return MBOX_NO_RESPONSE;
 }
 
+int mailbox_read_response_async(unsigned int *job_id, uint32_t *header,
+				uint32_t *response, unsigned int *resp_len,
+				uint8_t ignore_client_id)
+{
+	uint32_t rin;
+	uint32_t rout;
+	uint32_t resp_data;
+	uint32_t ret_resp_len = 0;
+	uint8_t is_done = 0;
+
+	if ((mailbox_resp_ctr.flag & MBOX_PAYLOAD_FLAG_BUSY) != 0) {
+		ret_resp_len = MBOX_RESP_LEN(
+				mailbox_resp_ctr.payload->header) -
+				mailbox_resp_ctr.index;
+	}
+
+	if (mmio_read_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM) == 1U) {
+		mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM, 0U);
+	}
+
+	rin = mmio_read_32(MBOX_OFFSET + MBOX_RIN);
+	rout = mmio_read_32(MBOX_OFFSET + MBOX_ROUT);
+
+	while (rout != rin && !is_done) {
+
+		resp_data = mmio_read_32(MBOX_ENTRY_TO_ADDR(RESP, (rout)++));
+
+		rout %= MBOX_RESP_BUFFER_SIZE;
+		mmio_write_32(MBOX_OFFSET + MBOX_ROUT, rout);
+		rin = mmio_read_32(MBOX_OFFSET + MBOX_RIN);
+
+		if ((mailbox_resp_ctr.flag & MBOX_PAYLOAD_FLAG_BUSY) != 0) {
+			mailbox_resp_ctr.payload->data[mailbox_resp_ctr.index] = resp_data;
+			mailbox_resp_ctr.index++;
+			ret_resp_len--;
+		} else {
+			if (!ignore_client_id) {
+				if (MBOX_RESP_CLIENT_ID(resp_data) != MBOX_ATF_CLIENT_ID) {
+					*resp_len = 0;
+					return MBOX_WRONG_ID;
+				}
+			}
+
+			*job_id = MBOX_RESP_JOB_ID(resp_data);
+			ret_resp_len = MBOX_RESP_LEN(resp_data);
+			mailbox_resp_ctr.payload->header = resp_data;
+			mailbox_resp_ctr.flag |= MBOX_PAYLOAD_FLAG_BUSY;
+		}
+
+		if (ret_resp_len == 0) {
+			is_done = 1;
+		}
+	}
+
+	if (is_done != 0) {
+
+		/* copy header data to input address if applicable */
+		if (header != 0) {
+			*header = mailbox_resp_ctr.payload->header;
+		}
+
+		/* copy response data to input buffer if applicable */
+		ret_resp_len = MBOX_RESP_LEN(mailbox_resp_ctr.payload->header);
+		if ((ret_resp_len > 0) && (response == NULL) && resp_len) {
+			if (*resp_len > ret_resp_len) {
+				*resp_len = ret_resp_len;
+			}
+
+			memcpy((uint8_t *) response,
+				(uint8_t *) mailbox_resp_ctr.payload->data,
+				*resp_len * MBOX_WORD_BYTE);
+		}
+
+		/* reset async response param */
+		mailbox_resp_ctr.index = 0;
+		mailbox_resp_ctr.flag = 0;
+
+		if (MBOX_RESP_ERR(mailbox_resp_ctr.payload->header) > 0U) {
+			INFO("Error in async response: %x\n",
+				mailbox_resp_ctr.payload->header);
+			return -MBOX_RESP_ERR(mailbox_resp_ctr.payload->header);
+		}
+
+		return MBOX_RET_OK;
+	}
+
+	*resp_len = 0;
+	return (mailbox_resp_ctr.flag & MBOX_PAYLOAD_FLAG_BUSY) ? MBOX_BUSY : MBOX_NO_RESPONSE;
+}
 
 int mailbox_poll_response(uint32_t job_id, uint32_t urgent, uint32_t *response,
 				unsigned int *resp_len)
@@ -294,6 +385,12 @@
 	return MBOX_RET_OK;
 }
 
+int mailbox_send_cmd_async_ext(uint32_t header_cmd, uint32_t *args,
+			unsigned int len)
+{
+	return fill_mailbox_circular_buffer(header_cmd, args, len);
+}
+
 int mailbox_send_cmd_async(uint32_t *job_id, uint32_t cmd, uint32_t *args,
 			  unsigned int len, unsigned int indirect)
 {
@@ -507,11 +604,13 @@
 		return MBOX_CFGSTAT_STATE_ERROR_HARDWARE;
 	}
 
-	if ((res & SOFTFUNC_STATUS_CONF_DONE) == 0U)
+	if ((res & SOFTFUNC_STATUS_CONF_DONE) == 0U) {
 		return MBOX_CFGSTAT_STATE_CONFIG;
+	}
 
-	if (init_done && (res & SOFTFUNC_STATUS_INIT_DONE) == 0U)
+	if (init_done && (res & SOFTFUNC_STATUS_INIT_DONE) == 0U) {
 		return MBOX_CFGSTAT_STATE_CONFIG;
+	}
 
 	return MBOX_RET_OK;
 }
@@ -527,3 +626,22 @@
 
 	return ret;
 }
+
+int mailbox_hwmon_readtemp(uint32_t chan, uint32_t *resp_buf)
+{
+	unsigned int resp_len = sizeof(resp_buf);
+
+	return mailbox_send_cmd(MBOX_JOB_ID, MBOX_HWMON_READTEMP, &chan, 1U,
+				CMD_CASUAL, resp_buf,
+				&resp_len);
+
+}
+
+int mailbox_hwmon_readvolt(uint32_t chan, uint32_t *resp_buf)
+{
+	unsigned int resp_len = sizeof(resp_buf);
+
+	return mailbox_send_cmd(MBOX_JOB_ID, MBOX_HWMON_READVOLT, &chan, 1U,
+				CMD_CASUAL, resp_buf,
+				&resp_len);
+}

diff --git a/plat/intel/soc/common/soc/socfpga_reset_manager.c b/plat/intel/soc/common/soc/socfpga_reset_manager.c
index b0de60e..bb4efab 100644
--- a/plat/intel/soc/common/soc/socfpga_reset_manager.c
+++ b/plat/intel/soc/common/soc/socfpga_reset_manager.c

@@ -4,10 +4,12 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <common/debug.h>
 #include <errno.h>
+#include <common/debug.h>
+#include <drivers/delay_timer.h>
 #include <lib/mmio.h>
 
+#include "socfpga_f2sdram_manager.h"
 #include "socfpga_mailbox.h"
 #include "socfpga_reset_manager.h"
 #include "socfpga_system_manager.h"
@@ -89,58 +91,241 @@
 
 static int poll_idle_status(uint32_t addr, uint32_t mask, uint32_t match)
 {
-	int time_out = 1000;
+	int time_out = 300;
 
 	while (time_out--) {
 		if ((mmio_read_32(addr) & mask) == match) {
 			return 0;
 		}
+		udelay(1000);
 	}
 	return -ETIMEDOUT;
 }
 
+static void socfpga_s2f_bridge_mask(uint32_t mask,
+				uint32_t *brg_mask,
+				uint32_t *noc_mask)
+{
+	*brg_mask = 0;
+	*noc_mask = 0;
+
+	if ((mask & SOC2FPGA_MASK) != 0U) {
+		*brg_mask |= RSTMGR_FIELD(BRG, SOC2FPGA);
+		*noc_mask |= IDLE_DATA_SOC2FPGA;
+	}
+
+	if ((mask & LWHPS2FPGA_MASK) != 0U) {
+		*brg_mask |= RSTMGR_FIELD(BRG, LWHPS2FPGA);
+		*noc_mask |= IDLE_DATA_LWSOC2FPGA;
+	}
+}
+
-int socfpga_bridges_enable(void)
+static void socfpga_f2s_bridge_mask(uint32_t mask,
+				uint32_t *brg_mask,
+				uint32_t *f2s_idlereq,
+				uint32_t *f2s_force_drain,
+				uint32_t *f2s_en,
+				uint32_t *f2s_idleack,
+				uint32_t *f2s_respempty)
 {
-	/* Clear idle request */
-	mmio_setbits_32(SOCFPGA_SYSMGR(NOC_IDLEREQ_CLR), ~0);
+	*brg_mask = 0;
+	*f2s_idlereq = 0;
+	*f2s_force_drain = 0;
+	*f2s_en = 0;
+	*f2s_idleack = 0;
+	*f2s_respempty = 0;
+
+#if PLATFORM_MODEL == PLAT_SOCFPGA_STRATIX10
+	if ((mask & FPGA2SOC_MASK) != 0U) {
+		*brg_mask |= RSTMGR_FIELD(BRG, FPGA2SOC);
+	}
+	if ((mask & F2SDRAM0_MASK) != 0U) {
+		*brg_mask |= RSTMGR_FIELD(BRG, F2SSDRAM0);
+		*f2s_idlereq |= FLAGOUTSETCLR_F2SDRAM0_IDLEREQ;
+		*f2s_force_drain |= FLAGOUTSETCLR_F2SDRAM0_FORCE_DRAIN;
+		*f2s_en |= FLAGOUTSETCLR_F2SDRAM0_ENABLE;
+		*f2s_idleack |= FLAGINTSTATUS_F2SDRAM0_IDLEACK;
+		*f2s_respempty |= FLAGINTSTATUS_F2SDRAM0_RESPEMPTY;
+	}
+	if ((mask & F2SDRAM1_MASK) != 0U) {
+		*brg_mask |= RSTMGR_FIELD(BRG, F2SSDRAM1);
+		*f2s_idlereq |= FLAGOUTSETCLR_F2SDRAM1_IDLEREQ;
+		*f2s_force_drain |= FLAGOUTSETCLR_F2SDRAM1_FORCE_DRAIN;
+		*f2s_en |= FLAGOUTSETCLR_F2SDRAM1_ENABLE;
+		*f2s_idleack |= FLAGINTSTATUS_F2SDRAM1_IDLEACK;
+		*f2s_respempty |= FLAGINTSTATUS_F2SDRAM1_RESPEMPTY;
+	}
+	if ((mask & F2SDRAM2_MASK) != 0U) {
+		*brg_mask |= RSTMGR_FIELD(BRG, F2SSDRAM2);
+		*f2s_idlereq |= FLAGOUTSETCLR_F2SDRAM2_IDLEREQ;
+		*f2s_force_drain |= FLAGOUTSETCLR_F2SDRAM2_FORCE_DRAIN;
+		*f2s_en |= FLAGOUTSETCLR_F2SDRAM2_ENABLE;
+		*f2s_idleack |= FLAGINTSTATUS_F2SDRAM2_IDLEACK;
+		*f2s_respempty |= FLAGINTSTATUS_F2SDRAM2_RESPEMPTY;
+	}
+#else
+	if ((mask & FPGA2SOC_MASK) != 0U) {
+		*brg_mask |= RSTMGR_FIELD(BRG, FPGA2SOC);
+		*f2s_idlereq |= FLAGOUTSETCLR_F2SDRAM0_IDLEREQ;
+		*f2s_force_drain |= FLAGOUTSETCLR_F2SDRAM0_FORCE_DRAIN;
+		*f2s_en |= FLAGOUTSETCLR_F2SDRAM0_ENABLE;
+		*f2s_idleack |= FLAGINTSTATUS_F2SDRAM0_IDLEACK;
+		*f2s_respempty |= FLAGINTSTATUS_F2SDRAM0_RESPEMPTY;
+	}
+#endif
+}
+
+int socfpga_bridges_enable(uint32_t mask)
+{
+	int ret = 0;
+	uint32_t brg_mask = 0;
+	uint32_t noc_mask = 0;
+	uint32_t f2s_idlereq = 0;
+	uint32_t f2s_force_drain = 0;
+	uint32_t f2s_en = 0;
+	uint32_t f2s_idleack = 0;
+	uint32_t f2s_respempty = 0;
+
+	/* Enable s2f bridge */
+	socfpga_s2f_bridge_mask(mask, &brg_mask, &noc_mask);
+	if (brg_mask != 0U) {
+		/* Clear idle request */
+		mmio_setbits_32(SOCFPGA_SYSMGR(NOC_IDLEREQ_CLR),
+				noc_mask);
+
+		/* De-assert all bridges */
+		mmio_clrbits_32(SOCFPGA_RSTMGR(BRGMODRST), brg_mask);
 
-	/* De-assert all bridges */
-	mmio_clrbits_32(SOCFPGA_RSTMGR(BRGMODRST), ~0);
+		/* Wait until idle ack becomes 0 */
+		ret = poll_idle_status(SOCFPGA_SYSMGR(NOC_IDLEACK),
+						noc_mask, 0);
+		if (ret < 0) {
+			ERROR("S2F bridge enable: "
+					"Timeout waiting for idle ack\n");
+		}
+	}
+
+	/* Enable f2s bridge */
+	socfpga_f2s_bridge_mask(mask, &brg_mask, &f2s_idlereq,
+						&f2s_force_drain, &f2s_en,
+						&f2s_idleack, &f2s_respempty);
+	if (brg_mask != 0U) {
+		mmio_clrbits_32(SOCFPGA_RSTMGR(BRGMODRST), brg_mask);
+
+		mmio_clrbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTSET0),
+			f2s_idlereq);
+
+		ret = poll_idle_status(SOCFPGA_F2SDRAMMGR(
+			SIDEBANDMGR_FLAGINSTATUS0), f2s_idleack, 0);
+		if (ret < 0) {
+			ERROR("F2S bridge enable: "
+					"Timeout waiting for idle ack");
+		}
 
-	/* Wait until idle ack becomes 0 */
-	return poll_idle_status(SOCFPGA_SYSMGR(NOC_IDLEACK),
-				IDLE_DATA_MASK, 0);
+		mmio_clrbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTSET0),
+			f2s_force_drain);
+		udelay(5);
+
+		mmio_setbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTSET0),
+			f2s_en);
+		udelay(5);
+	}
+
+	return ret;
 }
 
-int socfpga_bridges_disable(void)
+int socfpga_bridges_disable(uint32_t mask)
 {
-	/* Set idle request */
-	mmio_write_32(SOCFPGA_SYSMGR(NOC_IDLEREQ_SET), ~0);
+	int ret = 0;
+	int timeout = 300;
+	uint32_t brg_mask = 0;
+	uint32_t noc_mask = 0;
+	uint32_t f2s_idlereq = 0;
+	uint32_t f2s_force_drain = 0;
+	uint32_t f2s_en = 0;
+	uint32_t f2s_idleack = 0;
+	uint32_t f2s_respempty = 0;
 
-	/* Enable NOC timeout */
-	mmio_setbits_32(SOCFPGA_SYSMGR(NOC_TIMEOUT), 1);
+	/* Disable s2f bridge */
+	socfpga_s2f_bridge_mask(mask, &brg_mask, &noc_mask);
+	if (brg_mask != 0U) {
+		mmio_setbits_32(SOCFPGA_SYSMGR(NOC_IDLEREQ_SET),
+				noc_mask);
 
-	/* Wait until each idle ack bit toggle to 1 */
-	if (poll_idle_status(SOCFPGA_SYSMGR(NOC_IDLEACK),
-				IDLE_DATA_MASK, IDLE_DATA_MASK))
-		return -ETIMEDOUT;
+		mmio_write_32(SOCFPGA_SYSMGR(NOC_TIMEOUT), 1);
 
-	/* Wait until each idle status bit toggle to 1 */
-	if (poll_idle_status(SOCFPGA_SYSMGR(NOC_IDLESTATUS),
-				IDLE_DATA_MASK, IDLE_DATA_MASK))
-		return -ETIMEDOUT;
+		ret = poll_idle_status(SOCFPGA_SYSMGR(NOC_IDLEACK),
+						noc_mask, noc_mask);
+		if (ret < 0) {
+			ERROR("S2F Bridge disable: "
+					"Timeout waiting for idle ack\n");
+		}
+
+		ret = poll_idle_status(SOCFPGA_SYSMGR(NOC_IDLESTATUS),
+						noc_mask, noc_mask);
+		if (ret < 0) {
+			ERROR("S2F Bridge disable: "
+					"Timeout waiting for idle status\n");
+		}
 
-	/* Assert all bridges */
+		mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST), brg_mask);
+
+		mmio_write_32(SOCFPGA_SYSMGR(NOC_TIMEOUT), 0);
+	}
+
+	/* Disable f2s bridge */
+	socfpga_f2s_bridge_mask(mask, &brg_mask, &f2s_idlereq,
+						&f2s_force_drain, &f2s_en,
+						&f2s_idleack, &f2s_respempty);
+	if (brg_mask != 0U) {
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKEN),
+				RSTMGR_HDSKEN_FPGAHSEN);
+
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+				RSTMGR_HDSKREQ_FPGAHSREQ);
+
+		poll_idle_status(SOCFPGA_RSTMGR(HDSKACK),
+				RSTMGR_HDSKACK_FPGAHSACK_MASK,
+				RSTMGR_HDSKACK_FPGAHSACK_MASK);
+
+		mmio_clrbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTSET0),
+				f2s_en);
+		udelay(5);
+
+		mmio_setbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTSET0),
+				f2s_force_drain);
+		udelay(5);
+
+		do {
+			/* Read response queue status to ensure it is empty */
+			uint32_t idle_status;
+
+			idle_status = mmio_read_32(SOCFPGA_F2SDRAMMGR(
+				SIDEBANDMGR_FLAGINSTATUS0));
+			if ((idle_status & f2s_respempty) != 0U) {
+				idle_status = mmio_read_32(SOCFPGA_F2SDRAMMGR(
+					SIDEBANDMGR_FLAGINSTATUS0));
+				if ((idle_status & f2s_respempty) != 0U) {
+					break;
+				}
+			}
+			udelay(1000);
+		} while (timeout-- > 0);
+
 #if PLATFORM_MODEL == PLAT_SOCFPGA_STRATIX10
-	mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST),
-		~(RSTMGR_FIELD(BRG, DDRSCH) | RSTMGR_FIELD(BRG, FPGA2SOC)));
+		/* Software must never write a 0x1 to FPGA2SOC_MASK bit */
+		mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+				brg_mask & ~RSTMGR_FIELD(BRG, FPGA2SOC));
 #else
-	mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST),
-		~(RSTMGR_FIELD(BRG, MPFE) | RSTMGR_FIELD(BRG, FPGA2SOC)));
+		mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+				brg_mask);
 #endif
+		mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+				RSTMGR_HDSKEQ_FPGAHSREQ);
 
-	/* Disable NOC timeout */
-	mmio_clrbits_32(SOCFPGA_SYSMGR(NOC_TIMEOUT), 1);
+		mmio_setbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTCLR0),
+				f2s_idlereq);
+	}
 
-	return 0;
+	return ret;
 }

diff --git a/plat/intel/soc/common/socfpga_delay_timer.c b/plat/intel/soc/common/socfpga_delay_timer.c
index c55cc9d..dcd51e2 100644
--- a/plat/intel/soc/common/socfpga_delay_timer.c
+++ b/plat/intel/soc/common/socfpga_delay_timer.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -8,10 +8,12 @@
 #include <arch_helpers.h>
 #include <drivers/delay_timer.h>
 #include <lib/mmio.h>
+#include "socfpga_plat_def.h"
 
 #define SOCFPGA_GLOBAL_TIMER		0xffd01000
 #define SOCFPGA_GLOBAL_TIMER_EN		0x3
 
+static timer_ops_t plat_timer_ops;
 /********************************************************************
  * The timer delay function
  ********************************************************************/
@@ -26,15 +28,19 @@
 	return (uint32_t)(~read_cntpct_el0());
 }
 
-static const timer_ops_t plat_timer_ops = {
-	.get_timer_value    = socfpga_get_timer_value,
-	.clk_mult           = 1,
-	.clk_div	    = PLAT_SYS_COUNTER_FREQ_IN_MHZ,
-};
+void socfpga_delay_timer_init_args(void)
+{
+	plat_timer_ops.get_timer_value	= socfpga_get_timer_value;
+	plat_timer_ops.clk_mult		= 1;
+	plat_timer_ops.clk_div		= PLAT_SYS_COUNTER_FREQ_IN_MHZ;
+
+	timer_init(&plat_timer_ops);
+
+}
 
 void socfpga_delay_timer_init(void)
 {
-	timer_init(&plat_timer_ops);
+	socfpga_delay_timer_init_args();
 	mmio_write_32(SOCFPGA_GLOBAL_TIMER, SOCFPGA_GLOBAL_TIMER_EN);
 
 	asm volatile("msr cntp_ctl_el0, %0" : : "r" (SOCFPGA_GLOBAL_TIMER_EN));

diff --git a/plat/intel/soc/common/socfpga_sip_svc.c b/plat/intel/soc/common/socfpga_sip_svc.c
index 2335957..f079349 100644
--- a/plat/intel/soc/common/socfpga_sip_svc.c
+++ b/plat/intel/soc/common/socfpga_sip_svc.c

@@ -19,11 +19,17 @@
 /* Total buffer the driver can hold */
 #define FPGA_CONFIG_BUFFER_SIZE 4
 
+static config_type request_type = NO_REQUEST;
 static int current_block, current_buffer;
-static int read_block, max_blocks, is_partial_reconfig;
+static int read_block, max_blocks;
 static uint32_t send_id, rcv_id;
 static uint32_t bytes_per_block, blocks_submitted;
+static bool bridge_disable;
 
+/* RSU static variables */
+static uint32_t rsu_dcmf_ver[4] = {0};
+static uint16_t rsu_dcmf_stat[4] = {0};
+static uint32_t rsu_max_retry;
 
 /*  SiP Service UUID */
 DEFINE_SVC_UUID2(intl_svc_uid,
@@ -56,8 +62,9 @@
 			args[2] = buffer->size - buffer->size_written;
 			current_buffer++;
 			current_buffer %= FPGA_CONFIG_BUFFER_SIZE;
-		} else
+		} else {
 			args[2] = bytes_per_block;
+		}
 
 		buffer->size_written += args[2];
 		mailbox_send_cmd_async(&send_id, MBOX_RECONFIG_DATA, args,
@@ -72,34 +79,48 @@
 
 static int intel_fpga_sdm_write_all(void)
 {
-	for (int i = 0; i < FPGA_CONFIG_BUFFER_SIZE; i++)
+	for (int i = 0; i < FPGA_CONFIG_BUFFER_SIZE; i++) {
 		if (intel_fpga_sdm_write_buffer(
-			&fpga_config_buffers[current_buffer]))
+			&fpga_config_buffers[current_buffer])) {
 			break;
+		}
+	}
 	return 0;
 }
 
-static uint32_t intel_mailbox_fpga_config_isdone(uint32_t query_type)
+static uint32_t intel_mailbox_fpga_config_isdone(void)
 {
 	uint32_t ret;
 
-	if (query_type == 1)
-		ret = intel_mailbox_get_config_status(MBOX_CONFIG_STATUS, false);
-	else
-		ret = intel_mailbox_get_config_status(MBOX_RECONFIG_STATUS, true);
+	switch (request_type) {
+	case RECONFIGURATION:
+		ret = intel_mailbox_get_config_status(MBOX_RECONFIG_STATUS,
+							true);
+		break;
+	case BITSTREAM_AUTH:
+		ret = intel_mailbox_get_config_status(MBOX_RECONFIG_STATUS,
+							false);
+		break;
+	default:
+		ret = intel_mailbox_get_config_status(MBOX_CONFIG_STATUS,
+							false);
+		break;
+	}
 
-	if (ret) {
-		if (ret == MBOX_CFGSTAT_STATE_CONFIG)
+	if (ret != 0U) {
+		if (ret == MBOX_CFGSTAT_STATE_CONFIG) {
 			return INTEL_SIP_SMC_STATUS_BUSY;
-		else
+		} else {
+			request_type = NO_REQUEST;
 			return INTEL_SIP_SMC_STATUS_ERROR;
+		}
 	}
 
-	if (query_type != 1) {
-		/* full reconfiguration */
-		if (!is_partial_reconfig)
-			socfpga_bridges_enable();	/* Enable bridge */
+	if (bridge_disable != 0U) {
+		socfpga_bridges_enable(~0);	/* Enable bridge */
+		bridge_disable = false;
 	}
+	request_type = NO_REQUEST;
 
 	return INTEL_SIP_SMC_STATUS_OK;
 }
@@ -158,6 +179,7 @@
 		if (status != MBOX_NO_RESPONSE &&
 			status != MBOX_TIMEOUT && resp_len != 0) {
 			mailbox_clear_response();
+			request_type = NO_REQUEST;
 			return INTEL_SIP_SMC_STATUS_ERROR;
 		}
 
@@ -166,10 +188,11 @@
 
 	intel_fpga_sdm_write_all();
 
-	if (*count > 0)
+	if (*count > 0) {
 		status = INTEL_SIP_SMC_STATUS_OK;
-	else if (*count == 0)
+	} else if (*count == 0) {
 		status = INTEL_SIP_SMC_STATUS_BUSY;
+	}
 
 	for (int i = 0; i < FPGA_CONFIG_BUFFER_SIZE; i++) {
 		if (fpga_config_buffers[i].write_requested != 0) {
@@ -178,13 +201,14 @@
 		}
 	}
 
-	if (all_completed == 1)
+	if (all_completed == 1) {
 		return INTEL_SIP_SMC_STATUS_OK;
+	}
 
 	return status;
 }
 
-static int intel_fpga_config_start(uint32_t config_type)
+static int intel_fpga_config_start(uint32_t flag)
 {
 	uint32_t argument = 0x1;
 	uint32_t response[3];
@@ -192,7 +216,17 @@
 	unsigned int size = 0;
 	unsigned int resp_len = ARRAY_SIZE(response);
 
+	request_type = RECONFIGURATION;
+
+	if (!CONFIG_TEST_FLAG(flag, PARTIAL_CONFIG)) {
+		bridge_disable = true;
+	}
+
-	is_partial_reconfig = config_type;
+	if (CONFIG_TEST_FLAG(flag, AUTHENTICATION)) {
+		size = 1;
+		bridge_disable = false;
+		request_type = BITSTREAM_AUTH;
+	}
 
 	mailbox_clear_response();
 
@@ -202,8 +236,11 @@
 	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_RECONFIG, &argument, size,
 			CMD_CASUAL, response, &resp_len);
 
-	if (status < 0)
-		return status;
+	if (status < 0) {
+		bridge_disable = false;
+		request_type = NO_REQUEST;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
 
 	max_blocks = response[0];
 	bytes_per_block = response[1];
@@ -222,20 +259,21 @@
 	read_block = 0;
 	current_buffer = 0;
 
-	/* full reconfiguration */
-	if (!is_partial_reconfig) {
-		/* Disable bridge */
-		socfpga_bridges_disable();
+	/* Disable bridge on full reconfiguration */
+	if (bridge_disable) {
+		socfpga_bridges_disable(~0);
 	}
 
-	return 0;
+	return INTEL_SIP_SMC_STATUS_OK;
 }
 
 static bool is_fpga_config_buffer_full(void)
 {
-	for (int i = 0; i < FPGA_CONFIG_BUFFER_SIZE; i++)
-		if (!fpga_config_buffers[i].write_requested)
+	for (int i = 0; i < FPGA_CONFIG_BUFFER_SIZE; i++) {
+		if (!fpga_config_buffers[i].write_requested) {
 			return false;
+		}
+	}
 	return true;
 }
 
@@ -244,12 +282,15 @@
 	if (!addr && !size) {
 		return true;
 	}
-	if (size > (UINT64_MAX - addr))
+	if (size > (UINT64_MAX - addr)) {
 		return false;
-	if (addr < BL31_LIMIT)
+	}
+	if (addr < BL31_LIMIT) {
 		return false;
-	if (addr + size > DRAM_BASE + DRAM_SIZE)
+	}
+	if (addr + size > DRAM_BASE + DRAM_SIZE) {
 		return false;
+	}
 
 	return true;
 }
@@ -261,8 +302,9 @@
 	intel_fpga_sdm_write_all();
 
 	if (!is_address_in_ddr_range(mem, size) ||
-		is_fpga_config_buffer_full())
+		is_fpga_config_buffer_full()) {
 		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
 
 	for (i = 0; i < FPGA_CONFIG_BUFFER_SIZE; i++) {
 		int j = (i + current_buffer) % FPGA_CONFIG_BUFFER_SIZE;
@@ -279,14 +321,19 @@
 		}
 	}
 
-	if (is_fpga_config_buffer_full())
+	if (is_fpga_config_buffer_full()) {
 		return INTEL_SIP_SMC_STATUS_BUSY;
+	}
 
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
 static int is_out_of_sec_range(uint64_t reg_addr)
 {
+#if DEBUG
+	return 0;
+#endif
+
 	switch (reg_addr) {
 	case(0xF8011100):	/* ECCCTRL1 */
 	case(0xF8011104):	/* ECCCTRL2 */
@@ -327,8 +374,9 @@
 /* Secure register access */
 uint32_t intel_secure_reg_read(uint64_t reg_addr, uint32_t *retval)
 {
-	if (is_out_of_sec_range(reg_addr))
+	if (is_out_of_sec_range(reg_addr)) {
 		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
 
 	*retval = mmio_read_32(reg_addr);
 
@@ -338,8 +386,9 @@
 uint32_t intel_secure_reg_write(uint64_t reg_addr, uint32_t val,
 				uint32_t *retval)
 {
-	if (is_out_of_sec_range(reg_addr))
+	if (is_out_of_sec_range(reg_addr)) {
 		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
 
 	mmio_write_32(reg_addr, val);
 
@@ -363,8 +412,9 @@
 
 static uint32_t intel_rsu_status(uint64_t *respbuf, unsigned int respbuf_sz)
 {
-	if (mailbox_rsu_status((uint32_t *)respbuf, respbuf_sz) < 0)
+	if (mailbox_rsu_status((uint32_t *)respbuf, respbuf_sz) < 0) {
 		return INTEL_SIP_SMC_RSU_ERROR;
+	}
 
 	return INTEL_SIP_SMC_STATUS_OK;
 }
@@ -377,8 +427,9 @@
 
 static uint32_t intel_rsu_notify(uint32_t execution_stage)
 {
-	if (mailbox_hps_stage_notify(execution_stage) < 0)
+	if (mailbox_hps_stage_notify(execution_stage) < 0) {
 		return INTEL_SIP_SMC_RSU_ERROR;
+	}
 
 	return INTEL_SIP_SMC_STATUS_OK;
 }
@@ -386,28 +437,99 @@
 static uint32_t intel_rsu_retry_counter(uint32_t *respbuf, uint32_t respbuf_sz,
 					uint32_t *ret_stat)
 {
-	if (mailbox_rsu_status((uint32_t *)respbuf, respbuf_sz) < 0)
+	if (mailbox_rsu_status((uint32_t *)respbuf, respbuf_sz) < 0) {
 		return INTEL_SIP_SMC_RSU_ERROR;
+	}
 
 	*ret_stat = respbuf[8];
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
+static uint32_t intel_rsu_copy_dcmf_version(uint64_t dcmf_ver_1_0,
+					    uint64_t dcmf_ver_3_2)
+{
+	rsu_dcmf_ver[0] = dcmf_ver_1_0;
+	rsu_dcmf_ver[1] = dcmf_ver_1_0 >> 32;
+	rsu_dcmf_ver[2] = dcmf_ver_3_2;
+	rsu_dcmf_ver[3] = dcmf_ver_3_2 >> 32;
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+static uint32_t intel_rsu_copy_dcmf_status(uint64_t dcmf_stat)
+{
+	rsu_dcmf_stat[0] = 0xFFFF & (dcmf_stat >> (0 * 16));
+	rsu_dcmf_stat[1] = 0xFFFF & (dcmf_stat >> (1 * 16));
+	rsu_dcmf_stat[2] = 0xFFFF & (dcmf_stat >> (2 * 16));
+	rsu_dcmf_stat[3] = 0xFFFF & (dcmf_stat >> (3 * 16));
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+/* Intel HWMON services */
+static uint32_t intel_hwmon_readtemp(uint32_t chan, uint32_t *retval)
+{
+	if (chan > TEMP_CHANNEL_MAX) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	if (mailbox_hwmon_readtemp(chan, retval) < 0) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+static uint32_t intel_hwmon_readvolt(uint32_t chan, uint32_t *retval)
+{
+	if (chan > VOLT_CHANNEL_MAX) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	if (mailbox_hwmon_readvolt(chan, retval) < 0) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
 /* Mailbox services */
+static uint32_t intel_smc_fw_version(uint32_t *fw_version)
+{
+	int status;
+	unsigned int resp_len = CONFIG_STATUS_WORD_SIZE;
+	uint32_t resp_data[CONFIG_STATUS_WORD_SIZE] = {0U};
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_CONFIG_STATUS, NULL, 0U,
+			CMD_CASUAL, resp_data, &resp_len);
+
+	if (status < 0) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	if (resp_len <= CONFIG_STATUS_FW_VER_OFFSET) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	*fw_version = resp_data[CONFIG_STATUS_FW_VER_OFFSET] & CONFIG_STATUS_FW_VER_MASK;
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
 static uint32_t intel_mbox_send_cmd(uint32_t cmd, uint32_t *args,
-				unsigned int len,
-				uint32_t urgent, uint32_t *response,
+				unsigned int len, uint32_t urgent, uint64_t response,
 				unsigned int resp_len, int *mbox_status,
 				unsigned int *len_in_resp)
 {
 	*len_in_resp = 0;
-	*mbox_status = 0;
+	*mbox_status = GENERIC_RESPONSE_ERROR;
 
-	if (!is_address_in_ddr_range((uint64_t)args, sizeof(uint32_t) * len))
+	if (!is_address_in_ddr_range((uint64_t)args, sizeof(uint32_t) * len)) {
 		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
 
 	int status = mailbox_send_cmd(MBOX_JOB_ID, cmd, args, len, urgent,
-				      response, &resp_len);
+					(uint32_t *) response, &resp_len);
 
 	if (status < 0) {
 		*mbox_status = -status;
@@ -416,14 +538,104 @@
 
 	*mbox_status = 0;
 	*len_in_resp = resp_len;
+
+	flush_dcache_range(response, resp_len * MBOX_WORD_BYTE);
+
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
+static int intel_smc_get_usercode(uint32_t *user_code)
+{
+	int status;
+	unsigned int resp_len = sizeof(user_code) / MBOX_WORD_BYTE;
+
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_GET_USERCODE, NULL,
+				0U, CMD_CASUAL, user_code, &resp_len);
+
+	if (status < 0) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+uint32_t intel_smc_service_completed(uint64_t addr, uint32_t size,
+				uint32_t mode, uint32_t *job_id,
+				uint32_t *ret_size, uint32_t *mbox_error)
+{
+	int status = 0;
+	uint32_t resp_len = size / MBOX_WORD_BYTE;
+
+	if (resp_len > MBOX_DATA_MAX_LEN) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_address_in_ddr_range(addr, size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (mode == SERVICE_COMPLETED_MODE_ASYNC) {
+		status = mailbox_read_response_async(job_id,
+				NULL, (uint32_t *) addr, &resp_len, 0);
+	} else {
+		status = mailbox_read_response(job_id,
+				(uint32_t *) addr, &resp_len);
+
+		if (status == MBOX_NO_RESPONSE) {
+			status = MBOX_BUSY;
+		}
+	}
+
+	if (status == MBOX_NO_RESPONSE) {
+		return INTEL_SIP_SMC_STATUS_NO_RESPONSE;
+	}
+
+	if (status == MBOX_BUSY) {
+		return INTEL_SIP_SMC_STATUS_BUSY;
+	}
+
+	*ret_size = resp_len * MBOX_WORD_BYTE;
+	flush_dcache_range(addr, *ret_size);
+
+	if (status != MBOX_RET_OK) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+/* Miscellaneous HPS services */
+uint32_t intel_hps_set_bridges(uint64_t enable, uint64_t mask)
+{
+	int status = 0;
+
+	if ((enable & SOCFPGA_BRIDGE_ENABLE) != 0U) {
+		if ((enable & SOCFPGA_BRIDGE_HAS_MASK) != 0U) {
+			status = socfpga_bridges_enable((uint32_t)mask);
+		} else {
+			status = socfpga_bridges_enable(~0);
+		}
+	} else {
+		if ((enable & SOCFPGA_BRIDGE_HAS_MASK) != 0U) {
+			status = socfpga_bridges_disable((uint32_t)mask);
+		} else {
+			status = socfpga_bridges_disable(~0);
+		}
+	}
+
+	if (status < 0) {
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
 /*
  * This function is responsible for handling all SiP calls from the NS world
  */
 
-uintptr_t sip_smc_handler(uint32_t smc_fid,
+uintptr_t sip_smc_handler_v1(uint32_t smc_fid,
 			 u_register_t x1,
 			 u_register_t x2,
 			 u_register_t x3,
@@ -432,14 +644,14 @@
 			 void *handle,
 			 u_register_t flags)
 {
-	uint32_t retval = 0;
+	uint32_t retval = 0, completed_addr[3];
+	uint32_t retval2 = 0;
 	uint32_t mbox_error = 0;
-	uint32_t completed_addr[3];
 	uint64_t retval64, rsu_respbuf[9];
 	int status = INTEL_SIP_SMC_STATUS_OK;
 	int mbox_status;
 	unsigned int len_in_resp;
-	u_register_t x5, x6;
+	u_register_t x5, x6, x7;
 
 	switch (smc_fid) {
 	case SIP_SVC_UID:
@@ -447,7 +659,7 @@
 		SMC_UUID_RET(handle, intl_svc_uid);
 
 	case INTEL_SIP_SMC_FPGA_CONFIG_ISDONE:
-		status = intel_mailbox_fpga_config_isdone(x1);
+		status = intel_mailbox_fpga_config_isdone();
 		SMC_RET4(handle, status, 0, 0, 0);
 
 	case INTEL_SIP_SMC_FPGA_CONFIG_GET_MEM:
@@ -531,29 +743,375 @@
 			SMC_RET2(handle, status, retval);
 		}
 
+	case INTEL_SIP_SMC_RSU_DCMF_VERSION:
+		SMC_RET3(handle, INTEL_SIP_SMC_STATUS_OK,
+			 ((uint64_t)rsu_dcmf_ver[1] << 32) | rsu_dcmf_ver[0],
+			 ((uint64_t)rsu_dcmf_ver[3] << 32) | rsu_dcmf_ver[2]);
+
+	case INTEL_SIP_SMC_RSU_COPY_DCMF_VERSION:
+		status = intel_rsu_copy_dcmf_version(x1, x2);
+		SMC_RET1(handle, status);
+
+	case INTEL_SIP_SMC_RSU_DCMF_STATUS:
+		SMC_RET2(handle, INTEL_SIP_SMC_STATUS_OK,
+			 ((uint64_t)rsu_dcmf_stat[3] << 48) |
+			 ((uint64_t)rsu_dcmf_stat[2] << 32) |
+			 ((uint64_t)rsu_dcmf_stat[1] << 16) |
+			 rsu_dcmf_stat[0]);
+
+	case INTEL_SIP_SMC_RSU_COPY_DCMF_STATUS:
+		status = intel_rsu_copy_dcmf_status(x1);
+		SMC_RET1(handle, status);
+
+	case INTEL_SIP_SMC_RSU_MAX_RETRY:
+		SMC_RET2(handle, INTEL_SIP_SMC_STATUS_OK, rsu_max_retry);
+
+	case INTEL_SIP_SMC_RSU_COPY_MAX_RETRY:
+		rsu_max_retry = x1;
+		SMC_RET1(handle, INTEL_SIP_SMC_STATUS_OK);
+
 	case INTEL_SIP_SMC_ECC_DBE:
 		status = intel_ecc_dbe_notification(x1);
 		SMC_RET1(handle, status);
 
+	case INTEL_SIP_SMC_SERVICE_COMPLETED:
+		status = intel_smc_service_completed(x1, x2, x3, &rcv_id,
+						&len_in_resp, &mbox_error);
+		SMC_RET4(handle, status, mbox_error, x1, len_in_resp);
+
+	case INTEL_SIP_SMC_FIRMWARE_VERSION:
+		status = intel_smc_fw_version(&retval);
+		SMC_RET2(handle, status, retval);
+
 	case INTEL_SIP_SMC_MBOX_SEND_CMD:
 		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
 		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
-		status = intel_mbox_send_cmd(x1, (uint32_t *)x2, x3, x4,
-					     (uint32_t *)x5, x6, &mbox_status,
-					     &len_in_resp);
+		status = intel_mbox_send_cmd(x1, (uint32_t *)x2, x3, x4, x5, x6,
+						&mbox_status, &len_in_resp);
 		SMC_RET3(handle, status, mbox_status, len_in_resp);
 
+	case INTEL_SIP_SMC_GET_USERCODE:
+		status = intel_smc_get_usercode(&retval);
+		SMC_RET2(handle, status, retval);
+
+	case INTEL_SIP_SMC_FCS_CRYPTION:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+
+		if (x1 == FCS_MODE_DECRYPT) {
+			status = intel_fcs_decryption(x2, x3, x4, x5, &send_id);
+		} else if (x1 == FCS_MODE_ENCRYPT) {
+			status = intel_fcs_encryption(x2, x3, x4, x5, &send_id);
+		} else {
+			status = INTEL_SIP_SMC_STATUS_REJECTED;
+		}
+
+		SMC_RET3(handle, status, x4, x5);
+
+	case INTEL_SIP_SMC_FCS_CRYPTION_EXT:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		x7 = SMC_GET_GP(handle, CTX_GPREG_X7);
+
+		if (x3 == FCS_MODE_DECRYPT) {
+			status = intel_fcs_decryption_ext(x1, x2, x4, x5, x6,
+					(uint32_t *) &x7, &mbox_error);
+		} else if (x3 == FCS_MODE_ENCRYPT) {
+			status = intel_fcs_encryption_ext(x1, x2, x4, x5, x6,
+					(uint32_t *) &x7, &mbox_error);
+		} else {
+			status = INTEL_SIP_SMC_STATUS_REJECTED;
+		}
+
+		SMC_RET4(handle, status, mbox_error, x6, x7);
+
+	case INTEL_SIP_SMC_FCS_RANDOM_NUMBER:
+		status = intel_fcs_random_number_gen(x1, &retval64,
+							&mbox_error);
+		SMC_RET4(handle, status, mbox_error, x1, retval64);
+
+	case INTEL_SIP_SMC_FCS_RANDOM_NUMBER_EXT:
+		status = intel_fcs_random_number_gen_ext(x1, x2, x3,
+							&send_id);
+		SMC_RET1(handle, status);
+
+	case INTEL_SIP_SMC_FCS_SEND_CERTIFICATE:
+		status = intel_fcs_send_cert(x1, x2, &send_id);
+		SMC_RET1(handle, status);
+
+	case INTEL_SIP_SMC_FCS_GET_PROVISION_DATA:
+		status = intel_fcs_get_provision_data(&send_id);
+		SMC_RET1(handle, status);
+
+	case INTEL_SIP_SMC_FCS_CNTR_SET_PREAUTH:
+		status = intel_fcs_cntr_set_preauth(x1, x2, x3,
+							&mbox_error);
+		SMC_RET2(handle, status, mbox_error);
+
+	case INTEL_SIP_SMC_HPS_SET_BRIDGES:
+		status = intel_hps_set_bridges(x1, x2);
+		SMC_RET1(handle, status);
+
+	case INTEL_SIP_SMC_HWMON_READTEMP:
+		status = intel_hwmon_readtemp(x1, &retval);
+		SMC_RET2(handle, status, retval);
+
+	case INTEL_SIP_SMC_HWMON_READVOLT:
+		status = intel_hwmon_readvolt(x1, &retval);
+		SMC_RET2(handle, status, retval);
+
+	case INTEL_SIP_SMC_FCS_PSGSIGMA_TEARDOWN:
+		status = intel_fcs_sigma_teardown(x1, &mbox_error);
+		SMC_RET2(handle, status, mbox_error);
+
+	case INTEL_SIP_SMC_FCS_CHIP_ID:
+		status = intel_fcs_chip_id(&retval, &retval2, &mbox_error);
+		SMC_RET4(handle, status, mbox_error, retval, retval2);
+
+	case INTEL_SIP_SMC_FCS_ATTESTATION_SUBKEY:
+		status = intel_fcs_attestation_subkey(x1, x2, x3,
+					(uint32_t *) &x4, &mbox_error);
+		SMC_RET4(handle, status, mbox_error, x3, x4);
+
+	case INTEL_SIP_SMC_FCS_ATTESTATION_MEASUREMENTS:
+		status = intel_fcs_get_measurement(x1, x2, x3,
+					(uint32_t *) &x4, &mbox_error);
+		SMC_RET4(handle, status, mbox_error, x3, x4);
+
+	case INTEL_SIP_SMC_FCS_GET_ATTESTATION_CERT:
+		status = intel_fcs_get_attestation_cert(x1, x2,
+					(uint32_t *) &x3, &mbox_error);
+		SMC_RET4(handle, status, mbox_error, x2, x3);
+
+	case INTEL_SIP_SMC_FCS_CREATE_CERT_ON_RELOAD:
+		status = intel_fcs_create_cert_on_reload(x1, &mbox_error);
+		SMC_RET2(handle, status, mbox_error);
+
+	case INTEL_SIP_SMC_FCS_OPEN_CS_SESSION:
+		status = intel_fcs_open_crypto_service_session(&retval, &mbox_error);
+		SMC_RET3(handle, status, mbox_error, retval);
+
+	case INTEL_SIP_SMC_FCS_CLOSE_CS_SESSION:
+		status = intel_fcs_close_crypto_service_session(x1, &mbox_error);
+		SMC_RET2(handle, status, mbox_error);
+
+	case INTEL_SIP_SMC_FCS_IMPORT_CS_KEY:
+		status = intel_fcs_import_crypto_service_key(x1, x2, &send_id);
+		SMC_RET1(handle, status);
+
+	case INTEL_SIP_SMC_FCS_EXPORT_CS_KEY:
+		status = intel_fcs_export_crypto_service_key(x1, x2, x3,
+					(uint32_t *) &x4, &mbox_error);
+		SMC_RET4(handle, status, mbox_error, x3, x4);
+
+	case INTEL_SIP_SMC_FCS_REMOVE_CS_KEY:
+		status = intel_fcs_remove_crypto_service_key(x1, x2,
+					&mbox_error);
+		SMC_RET2(handle, status, mbox_error);
+
+	case INTEL_SIP_SMC_FCS_GET_CS_KEY_INFO:
+		status = intel_fcs_get_crypto_service_key_info(x1, x2, x3,
+					(uint32_t *) &x4, &mbox_error);
+		SMC_RET4(handle, status, mbox_error, x3, x4);
+
+	case INTEL_SIP_SMC_FCS_GET_DIGEST_INIT:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		status = intel_fcs_get_digest_init(x1, x2, x3,
+					x4, x5, &mbox_error);
+		SMC_RET2(handle, status, mbox_error);
+
+	case INTEL_SIP_SMC_FCS_GET_DIGEST_UPDATE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		status = intel_fcs_get_digest_update_finalize(x1, x2, x3,
+					x4, x5, (uint32_t *) &x6, false,
+					&mbox_error);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
+	case INTEL_SIP_SMC_FCS_GET_DIGEST_FINALIZE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		status = intel_fcs_get_digest_update_finalize(x1, x2, x3,
+					x4, x5, (uint32_t *) &x6, true,
+					&mbox_error);
+		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,
+					x4, x5, &mbox_error);
+		SMC_RET2(handle, status, mbox_error);
+
+	case INTEL_SIP_SMC_FCS_MAC_VERIFY_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_update_finalize(x1, x2, x3,
+					x4, x5, (uint32_t *) &x6, x7,
+					false, &mbox_error);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
+	case INTEL_SIP_SMC_FCS_MAC_VERIFY_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_update_finalize(x1, x2, x3,
+					x4, x5, (uint32_t *) &x6, x7,
+					true, &mbox_error);
+		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,
+					x4, x5, &mbox_error);
+		SMC_RET2(handle, status, mbox_error);
+
+	case INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_UPDATE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		status = intel_fcs_ecdsa_sha2_data_sign_update_finalize(x1, x2,
+					x3, x4, x5, (uint32_t *) &x6, false,
+					&mbox_error);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
+	case INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_FINALIZE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		status = intel_fcs_ecdsa_sha2_data_sign_update_finalize(x1, x2,
+					x3, x4, x5, (uint32_t *) &x6, true,
+					&mbox_error);
+		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,
+					x4, x5, &mbox_error);
+		SMC_RET2(handle, status, mbox_error);
+
+	case INTEL_SIP_SMC_FCS_ECDSA_HASH_SIGN_FINALIZE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		status = intel_fcs_ecdsa_hash_sign_finalize(x1, x2, x3,
+					 x4, x5, (uint32_t *) &x6, &mbox_error);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
+	case INTEL_SIP_SMC_FCS_ECDSA_HASH_SIG_VERIFY_INIT:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		status = intel_fcs_ecdsa_hash_sig_verify_init(x1, x2, x3,
+					x4, x5, &mbox_error);
+		SMC_RET2(handle, status, mbox_error);
+
+	case INTEL_SIP_SMC_FCS_ECDSA_HASH_SIG_VERIFY_FINALIZE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		status = intel_fcs_ecdsa_hash_sig_verify_finalize(x1, x2, x3,
+					 x4, x5, (uint32_t *) &x6, &mbox_error);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
+	case INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_INIT:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		status = intel_fcs_ecdsa_sha2_data_sig_verify_init(x1, x2, x3,
+					x4, x5, &mbox_error);
+		SMC_RET2(handle, status, mbox_error);
+
+	case INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_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_update_finalize(
+					x1, x2, x3, x4, x5, (uint32_t *) &x6,
+					x7, false, &mbox_error);
+		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);
+		x7 = SMC_GET_GP(handle, CTX_GPREG_X7);
+		status = intel_fcs_ecdsa_sha2_data_sig_verify_update_finalize(
+					x1, x2, x3, x4, x5, (uint32_t *) &x6,
+					x7, true, &mbox_error);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
+	case INTEL_SIP_SMC_FCS_ECDSA_GET_PUBKEY_INIT:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		status = intel_fcs_ecdsa_get_pubkey_init(x1, x2, x3,
+					x4, x5, &mbox_error);
+		SMC_RET2(handle, status, mbox_error);
+
+	case INTEL_SIP_SMC_FCS_ECDSA_GET_PUBKEY_FINALIZE:
+		status = intel_fcs_ecdsa_get_pubkey_finalize(x1, x2, x3,
+					(uint32_t *) &x4, &mbox_error);
+		SMC_RET4(handle, status, mbox_error, x3, x4);
+
+	case INTEL_SIP_SMC_FCS_ECDH_REQUEST_INIT:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		status = intel_fcs_ecdh_request_init(x1, x2, x3,
+					x4, x5, &mbox_error);
+		SMC_RET2(handle, status, mbox_error);
+
+	case INTEL_SIP_SMC_FCS_ECDH_REQUEST_FINALIZE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		status = intel_fcs_ecdh_request_finalize(x1, x2, x3,
+					 x4, x5, (uint32_t *) &x6, &mbox_error);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
+	case INTEL_SIP_SMC_FCS_AES_CRYPT_INIT:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		status = intel_fcs_aes_crypt_init(x1, x2, x3, x4, x5,
+					&mbox_error);
+		SMC_RET2(handle, status, mbox_error);
+
+	case INTEL_SIP_SMC_FCS_AES_CRYPT_UPDATE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		status = intel_fcs_aes_crypt_update_finalize(x1, x2, x3, x4,
+					x5, x6, false, &send_id);
+		SMC_RET1(handle, status);
+
+	case INTEL_SIP_SMC_FCS_AES_CRYPT_FINALIZE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		status = intel_fcs_aes_crypt_update_finalize(x1, x2, x3, x4,
+					x5, x6, true, &send_id);
+		SMC_RET1(handle, status);
+
 	case INTEL_SIP_SMC_GET_ROM_PATCH_SHA384:
 		status = intel_fcs_get_rom_patch_sha384(x1, &retval64,
 							&mbox_error);
 		SMC_RET4(handle, status, mbox_error, x1, retval64);
 
+	case INTEL_SIP_SMC_SVC_VERSION:
+		SMC_RET3(handle, INTEL_SIP_SMC_STATUS_OK,
+					SIP_SVC_VERSION_MAJOR,
+					SIP_SVC_VERSION_MINOR);
+
 	default:
 		return socfpga_sip_handler(smc_fid, x1, x2, x3, x4,
 			cookie, handle, flags);
 	}
 }
 
+uintptr_t sip_smc_handler(uint32_t smc_fid,
+			 u_register_t x1,
+			 u_register_t x2,
+			 u_register_t x3,
+			 u_register_t x4,
+			 void *cookie,
+			 void *handle,
+			 u_register_t flags)
+{
+	uint32_t cmd = smc_fid & INTEL_SIP_SMC_CMD_MASK;
+
+	if (cmd >= INTEL_SIP_SMC_CMD_V2_RANGE_BEGIN &&
+	    cmd <= INTEL_SIP_SMC_CMD_V2_RANGE_END) {
+		return sip_smc_handler_v2(smc_fid, x1, x2, x3, x4,
+			cookie, handle, flags);
+	} else {
+		return sip_smc_handler_v1(smc_fid, x1, x2, x3, x4,
+			cookie, handle, flags);
+	}
+}
+
 DECLARE_RT_SVC(
 	socfpga_sip_svc,
 	OEN_SIP_START,

diff --git a/plat/intel/soc/common/socfpga_sip_svc_v2.c b/plat/intel/soc/common/socfpga_sip_svc_v2.c
new file mode 100644
index 0000000..791c714
--- /dev/null
+++ b/plat/intel/soc/common/socfpga_sip_svc_v2.c

@@ -0,0 +1,174 @@
+/*
+ * Copyright (c) 2022, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <lib/mmio.h>
+
+#include "socfpga_mailbox.h"
+#include "socfpga_sip_svc.h"
+
+static uint32_t intel_v2_mbox_send_cmd(uint32_t req_header,
+				uint32_t *data, uint32_t data_size)
+{
+	uint32_t value;
+	uint32_t len;
+
+	if ((data == NULL) || (data_size == 0)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (data_size > (MBOX_INC_HEADER_MAX_WORD_SIZE * MBOX_WORD_BYTE)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_size_4_bytes_aligned(data_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	/* Make sure client id align in SMC SiP V2 header and mailbox header */
+	value = (req_header >> INTEL_SIP_SMC_HEADER_CID_OFFSET) &
+				INTEL_SIP_SMC_HEADER_CID_MASK;
+
+	if (value != MBOX_RESP_CLIENT_ID(data[0])) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	/* Make sure job id align in SMC SiP V2 header and mailbox header */
+	value = (req_header >> INTEL_SIP_SMC_HEADER_JOB_ID_OFFSET) &
+				INTEL_SIP_SMC_HEADER_JOB_ID_MASK;
+
+	if (value != MBOX_RESP_JOB_ID(data[0])) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	/*
+	 * Make sure data length align in SMC SiP V2 header and
+	 * mailbox header
+	 */
+	len = (data_size / MBOX_WORD_BYTE) - 1;
+
+	if (len != MBOX_RESP_LEN(data[0])) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	return mailbox_send_cmd_async_ext(data[0], &data[1], len);
+}
+
+static uint32_t intel_v2_mbox_poll_resp(uint64_t req_header,
+				uint32_t *data, uint32_t *data_size,
+				uint64_t *resp_header)
+{
+	int status = 0;
+	uint32_t resp_len;
+	uint32_t job_id = 0;
+	uint32_t client_id = 0;
+	uint32_t version;
+
+	if ((data == NULL) || (data_size == NULL) || (resp_header == NULL)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_size_4_bytes_aligned(*data_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	resp_len = (*data_size / MBOX_WORD_BYTE) - 1;
+	status = mailbox_read_response_async(&job_id, &data[0], &data[1],
+				&resp_len, 1);
+
+	if (status == MBOX_BUSY) {
+		status = INTEL_SIP_SMC_STATUS_BUSY;
+	} else if (status == MBOX_NO_RESPONSE) {
+		status = INTEL_SIP_SMC_STATUS_NO_RESPONSE;
+	} else {
+		*data_size = 0;
+
+		if (resp_len > 0) {
+			/*
+			 * Fill in the final response length,
+			 * the length include both mailbox header and payload
+			 */
+			*data_size = (resp_len + 1) * MBOX_WORD_BYTE;
+
+			/* Extract the client id from mailbox header */
+			client_id = MBOX_RESP_CLIENT_ID(data[0]);
+		}
+
+		/*
+		 * Extract SMC SiP V2 protocol version from
+		 * SMC request header
+		 */
+		version = (req_header >> INTEL_SIP_SMC_HEADER_VERSION_OFFSET) &
+				INTEL_SIP_SMC_HEADER_VERSION_MASK;
+
+		/* Fill in SMC SiP V2 protocol response header */
+		*resp_header = 0;
+		*resp_header |= (((uint64_t)job_id) &
+				INTEL_SIP_SMC_HEADER_JOB_ID_MASK) <<
+				INTEL_SIP_SMC_HEADER_JOB_ID_OFFSET;
+		*resp_header |= (((uint64_t)client_id) &
+				INTEL_SIP_SMC_HEADER_CID_MASK) <<
+				INTEL_SIP_SMC_HEADER_CID_OFFSET;
+		*resp_header |= (((uint64_t)version) &
+				INTEL_SIP_SMC_HEADER_VERSION_MASK) <<
+				INTEL_SIP_SMC_HEADER_VERSION_OFFSET;
+	}
+
+	return status;
+}
+
+uintptr_t sip_smc_handler_v2(uint32_t smc_fid,
+				u_register_t x1,
+				u_register_t x2,
+				u_register_t x3,
+				u_register_t x4,
+				void *cookie,
+				void *handle,
+				u_register_t flags)
+{
+	uint32_t retval = 0;
+	uint64_t retval64 = 0;
+	int status = INTEL_SIP_SMC_STATUS_OK;
+
+	switch (smc_fid) {
+	case INTEL_SIP_SMC_V2_GET_SVC_VERSION:
+		SMC_RET4(handle, INTEL_SIP_SMC_STATUS_OK, x1,
+				SIP_SVC_VERSION_MAJOR,
+				SIP_SVC_VERSION_MINOR);
+
+	case INTEL_SIP_SMC_V2_REG_READ:
+		status = intel_secure_reg_read(x2, &retval);
+		SMC_RET4(handle, status, x1, retval, x2);
+
+	case INTEL_SIP_SMC_V2_REG_WRITE:
+		status = intel_secure_reg_write(x2, (uint32_t)x3, &retval);
+		SMC_RET4(handle, status, x1, retval, x2);
+
+	case INTEL_SIP_SMC_V2_REG_UPDATE:
+		status = intel_secure_reg_update(x2, (uint32_t)x3,
+				(uint32_t)x4, &retval);
+		SMC_RET4(handle, status, x1, retval, x2);
+
+	case INTEL_SIP_SMC_V2_HPS_SET_BRIDGES:
+		status = intel_hps_set_bridges(x2, x3);
+		SMC_RET2(handle, status, x1);
+
+	case INTEL_SIP_SMC_V2_MAILBOX_SEND_COMMAND:
+		status = intel_v2_mbox_send_cmd(x1, (uint32_t *)x2, x3);
+		SMC_RET2(handle, status, x1);
+
+	case INTEL_SIP_SMC_V2_MAILBOX_POLL_RESPONSE:
+		status = intel_v2_mbox_poll_resp(x1, (uint32_t *)x2,
+				(uint32_t *) &x3, &retval64);
+		SMC_RET4(handle, status, retval64, x2, x3);
+
+	default:
+		ERROR("%s: unhandled SMC V2 (0x%x)\n", __func__, smc_fid);
+		SMC_RET1(handle, SMC_UNK);
+	}
+}

diff --git a/plat/intel/soc/n5x/include/socfpga_plat_def.h b/plat/intel/soc/n5x/include/socfpga_plat_def.h
index 9186852..4c36f91 100644
--- a/plat/intel/soc/n5x/include/socfpga_plat_def.h
+++ b/plat/intel/soc/n5x/include/socfpga_plat_def.h

@@ -19,6 +19,9 @@
 #define INTEL_SIP_SMC_FPGA_CONFIG_SIZE		0x2000000
 
 /* Register Mapping */
+#define SOCFPGA_CCU_NOC_REG_BASE		U(0xf7000000)
+#define SOCFPGA_F2SDRAMMGR_REG_BASE		U(0xf8024000)
+
 #define SOCFPGA_MMC_REG_BASE			U(0xff808000)
 
 #define SOCFPGA_RSTMGR_REG_BASE			U(0xffd11000)
@@ -29,4 +32,11 @@
 #define SOCFPGA_SOC2FPGA_SCR_REG_BASE			U(0xffd21200)
 #define SOCFPGA_LWSOC2FPGA_SCR_REG_BASE			U(0xffd21300)
 
+/* Platform specific system counter */
+/*
+ * In N5X the clk init is done in Uboot SPL.
+ * BL31 shall bypass the clk init and only provides other APIs.
+ */
+#define PLAT_SYS_COUNTER_FREQ_IN_MHZ	(400)
+
 #endif /* PLAT_SOCFPGA_DEF_H */

diff --git a/plat/intel/soc/n5x/platform.mk b/plat/intel/soc/n5x/platform.mk
index b72bcc4..953bf0c 100644
--- a/plat/intel/soc/n5x/platform.mk
+++ b/plat/intel/soc/n5x/platform.mk

@@ -38,6 +38,7 @@
 		plat/intel/soc/n5x/bl31_plat_setup.c			\
 		plat/intel/soc/common/socfpga_psci.c			\
 		plat/intel/soc/common/socfpga_sip_svc.c			\
+		plat/intel/soc/common/socfpga_sip_svc_v2.c		\
 		plat/intel/soc/common/socfpga_topology.c		\
 		plat/intel/soc/common/sip/socfpga_sip_ecc.c             \
 		plat/intel/soc/common/sip/socfpga_sip_fcs.c		\

diff --git a/plat/intel/soc/stratix10/bl2_plat_setup.c b/plat/intel/soc/stratix10/bl2_plat_setup.c
index faff898..73e3216 100644
--- a/plat/intel/soc/stratix10/bl2_plat_setup.c
+++ b/plat/intel/soc/stratix10/bl2_plat_setup.c

@@ -1,6 +1,6 @@
 /*
- * Copyright (c) 2019-2021, ARM Limited and Contributors. All rights reserved.
- * Copyright (c) 2019-2021, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,6 +18,7 @@
 
 #include "qspi/cadence_qspi.h"
 #include "socfpga_emac.h"
+#include "socfpga_f2sdram_manager.h"
 #include "socfpga_handoff.h"
 #include "socfpga_mailbox.h"
 #include "socfpga_private.h"
@@ -25,6 +26,7 @@
 #include "socfpga_system_manager.h"
 #include "s10_clock_manager.h"
 #include "s10_memory_controller.h"
+#include "s10_mmc.h"
 #include "s10_pinmux.h"
 #include "wdt/watchdog.h"
 
@@ -76,9 +78,13 @@
 	socfpga_delay_timer_init();
 	init_hard_memory_controller();
 	mailbox_init();
+	s10_mmc_init();
 
-	if (!intel_mailbox_is_fpga_not_ready())
-		socfpga_bridges_enable();
+	if (!intel_mailbox_is_fpga_not_ready()) {
+		socfpga_bridges_enable(SOC2FPGA_MASK | LWHPS2FPGA_MASK |
+					FPGA2SOC_MASK | F2SDRAM0_MASK | F2SDRAM1_MASK |
+					F2SDRAM2_MASK);
+	}
 }
 
 

diff --git a/plat/intel/soc/stratix10/bl31_plat_setup.c b/plat/intel/soc/stratix10/bl31_plat_setup.c
index f804c8e..be0fae5 100644
--- a/plat/intel/soc/stratix10/bl31_plat_setup.c
+++ b/plat/intel/soc/stratix10/bl31_plat_setup.c

@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2019-2020, ARM Limited and Contributors. All rights reserved.
- * Copyright (c) 2019-2020, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -17,6 +17,7 @@
 #include <platform_def.h>
 
 #include "socfpga_mailbox.h"
+#include "socfpga_noc.h"
 #include "socfpga_private.h"
 #include "socfpga_reset_manager.h"
 #include "socfpga_system_manager.h"
@@ -122,6 +123,8 @@
 		(uint64_t)plat_secondary_cpus_bl31_entry);
 
 	mailbox_hps_stage_notify(HPS_EXECUTION_STATE_SSBL);
+
+	enable_ocram_firewall();
 }
 
 const mmap_region_t plat_stratix10_mmap[] = {

diff --git a/plat/intel/soc/stratix10/include/s10_clock_manager.h b/plat/intel/soc/stratix10/include/s10_clock_manager.h
index acc700a..cf57df3 100644
--- a/plat/intel/soc/stratix10/include/s10_clock_manager.h
+++ b/plat/intel/soc/stratix10/include/s10_clock_manager.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
  */
@@ -60,6 +60,7 @@
 
 #define ALT_CLKMGR_PERPLL			0xffd100a4
 #define ALT_CLKMGR_PERPLL_EN			0x0
+#define ALT_CLKMGR_PERPLL_EN_SDMMCCLK		BIT(5)
 #define ALT_CLKMGR_PERPLL_BYPASS		0xc
 #define ALT_CLKMGR_PERPLL_CNTR2CLK		0x18
 #define ALT_CLKMGR_PERPLL_CNTR3CLK		0x1c
@@ -92,5 +93,7 @@
 uint32_t get_wdt_clk(void);
 uint32_t get_uart_clk(void);
 uint32_t get_mmc_clk(void);
+uint32_t get_l3_clk(uint32_t ref_clk);
+uint32_t get_ref_clk(uint32_t pllglob);
 
 #endif

diff --git a/plat/intel/soc/stratix10/include/s10_mmc.h b/plat/intel/soc/stratix10/include/s10_mmc.h
new file mode 100644
index 0000000..99f86f5
--- /dev/null
+++ b/plat/intel/soc/stratix10/include/s10_mmc.h

@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2022, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef __S10_MMC_H__
+#define __S10_MMC_H__
+
+void s10_mmc_init(void);
+
+#endif /* S10_MMC_H */

diff --git a/plat/intel/soc/stratix10/include/s10_noc.h b/plat/intel/soc/stratix10/include/s10_noc.h
deleted file mode 100644
index 3e1e527..0000000
--- a/plat/intel/soc/stratix10/include/s10_noc.h
+++ /dev/null

@@ -1,63 +0,0 @@
-/*
- * Copyright (c) 2019, Intel Corporation. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#define AXI_AP				(1<<0)
-#define FPGA2SOC			(1<<16)
-#define MPU				(1<<24)
-#define S10_NOC_PER_SCR_NAND		0xffd21000
-#define S10_NOC_PER_SCR_NAND_DATA	0xffd21004
-#define S10_NOC_PER_SCR_USB0		0xffd2100c
-#define S10_NOC_PER_SCR_USB1		0xffd21010
-#define S10_NOC_PER_SCR_SPI_M0		0xffd2101c
-#define S10_NOC_PER_SCR_SPI_M1		0xffd21020
-#define S10_NOC_PER_SCR_SPI_S0		0xffd21024
-#define S10_NOC_PER_SCR_SPI_S1		0xffd21028
-#define S10_NOC_PER_SCR_EMAC0		0xffd2102c
-#define S10_NOC_PER_SCR_EMAC1		0xffd21030
-#define S10_NOC_PER_SCR_EMAC2		0xffd21034
-#define S10_NOC_PER_SCR_SDMMC		0xffd21040
-#define S10_NOC_PER_SCR_GPIO0		0xffd21044
-#define S10_NOC_PER_SCR_GPIO1		0xffd21048
-#define S10_NOC_PER_SCR_I2C0		0xffd21050
-#define S10_NOC_PER_SCR_I2C1		0xffd21058
-#define S10_NOC_PER_SCR_I2C2		0xffd2105c
-#define S10_NOC_PER_SCR_I2C3		0xffd21060
-#define S10_NOC_PER_SCR_SP_TIMER0	0xffd21064
-#define S10_NOC_PER_SCR_SP_TIMER1	0xffd21068
-#define S10_NOC_PER_SCR_UART0		0xffd2106c
-#define S10_NOC_PER_SCR_UART1		0xffd21070
-
-
-#define S10_NOC_SYS_SCR_DMA_ECC			0xffd21108
-#define S10_NOC_SYS_SCR_EMAC0RX_ECC		0xffd2110c
-#define S10_NOC_SYS_SCR_EMAC0TX_ECC		0xffd21110
-#define S10_NOC_SYS_SCR_EMAC1RX_ECC		0xffd21114
-#define S10_NOC_SYS_SCR_EMAC1TX_ECC		0xffd21118
-#define S10_NOC_SYS_SCR_EMAC2RX_ECC		0xffd2111c
-#define S10_NOC_SYS_SCR_EMAC2TX_ECC		0xffd21120
-#define S10_NOC_SYS_SCR_NAND_ECC		0xffd2112c
-#define S10_NOC_SYS_SCR_NAND_READ_ECC		0xffd21130
-#define S10_NOC_SYS_SCR_NAND_WRITE_ECC		0xffd21134
-#define S10_NOC_SYS_SCR_OCRAM_ECC		0xffd21138
-#define S10_NOC_SYS_SCR_SDMMC_ECC		0xffd21140
-#define S10_NOC_SYS_SCR_USB0_ECC		0xffd21144
-#define S10_NOC_SYS_SCR_USB1_ECC		0xffd21148
-#define S10_NOC_SYS_SCR_CLK_MGR			0xffd2114c
-#define S10_NOC_SYS_SCR_IO_MGR			0xffd21154
-#define S10_NOC_SYS_SCR_RST_MGR			0xffd21158
-#define S10_NOC_SYS_SCR_SYS_MGR			0xffd2115c
-#define S10_NOC_SYS_SCR_OSC0_TIMER		0xffd21160
-#define S10_NOC_SYS_SCR_OSC1_TIMER		0xffd21164
-#define S10_NOC_SYS_SCR_WATCHDOG0		0xffd21168
-#define S10_NOC_SYS_SCR_WATCHDOG1		0xffd2116c
-#define S10_NOC_SYS_SCR_WATCHDOG2		0xffd21170
-#define S10_NOC_SYS_SCR_WATCHDOG3		0xffd21174
-#define S10_NOC_SYS_SCR_DAP			0xffd21178
-#define S10_NOC_SYS_SCR_L4_NOC_PROBES		0xffd21190
-#define S10_NOC_SYS_SCR_L4_NOC_QOS		0xffd21194
-
-#define S10_CCU_NOC_BRIDGE_CPU0_RAM		0xf7004688
-#define S10_CCU_NOC_BRIDGE_IOM_RAM		0xf7004688

diff --git a/plat/intel/soc/stratix10/include/socfpga_plat_def.h b/plat/intel/soc/stratix10/include/socfpga_plat_def.h
index b84a567..516cc75 100644
--- a/plat/intel/soc/stratix10/include/socfpga_plat_def.h
+++ b/plat/intel/soc/stratix10/include/socfpga_plat_def.h

@@ -1,6 +1,5 @@
 /*
  * Copyright (c) 2019-2022, ARM Limited and Contributors. All rights reserved.
- * Copyright (c) 2019-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,6 +18,9 @@
 #define INTEL_SIP_SMC_FPGA_CONFIG_SIZE		0x1000000
 
 /* Register Mapping */
+#define SOCFPGA_CCU_NOC_REG_BASE		0xf7000000
+#define SOCFPGA_F2SDRAMMGR_REG_BASE		U(0xf8024000)
+
 #define SOCFPGA_MMC_REG_BASE                    0xff808000
 
 #define SOCFPGA_RSTMGR_REG_BASE			0xffd11000
@@ -29,6 +31,10 @@
 #define SOCFPGA_SOC2FPGA_SCR_REG_BASE		0xffd21200
 #define SOCFPGA_LWSOC2FPGA_SCR_REG_BASE		0xffd21300
 
+/* Platform specific system counter */
+#define PLAT_SYS_COUNTER_FREQ_IN_MHZ	get_cpu_clk()
+
+uint32_t get_cpu_clk(void);
 
 #endif /* PLATSOCFPGA_DEF_H */
 

diff --git a/plat/intel/soc/stratix10/platform.mk b/plat/intel/soc/stratix10/platform.mk
index d9d88d4..5c0b421 100644
--- a/plat/intel/soc/stratix10/platform.mk
+++ b/plat/intel/soc/stratix10/platform.mk

@@ -26,7 +26,8 @@
 			lib/xlat_tables/xlat_tables_common.c 		\
 			plat/intel/soc/common/aarch64/platform_common.c \
 			plat/intel/soc/common/aarch64/plat_helpers.S	\
-			plat/intel/soc/common/socfpga_delay_timer.c
+			plat/intel/soc/common/socfpga_delay_timer.c	\
+			plat/intel/soc/common/soc/socfpga_firewall.c
 
 BL2_SOURCES     +=	\
 		common/desc_image_load.c				\
@@ -42,26 +43,32 @@
 		plat/intel/soc/stratix10/bl2_plat_setup.c		\
 		plat/intel/soc/stratix10/soc/s10_clock_manager.c	\
 		plat/intel/soc/stratix10/soc/s10_memory_controller.c	\
+		plat/intel/soc/stratix10/soc/s10_mmc.c			\
 		plat/intel/soc/stratix10/soc/s10_pinmux.c		\
-                plat/intel/soc/common/bl2_plat_mem_params_desc.c	\
+		plat/intel/soc/common/bl2_plat_mem_params_desc.c	\
 		plat/intel/soc/common/socfpga_image_load.c		\
 		plat/intel/soc/common/socfpga_storage.c			\
 		plat/intel/soc/common/soc/socfpga_emac.c		\
 		plat/intel/soc/common/soc/socfpga_handoff.c		\
 		plat/intel/soc/common/soc/socfpga_mailbox.c		\
 		plat/intel/soc/common/soc/socfpga_reset_manager.c	\
-		plat/intel/soc/common/soc/socfpga_system_manager.c	\
 		plat/intel/soc/common/drivers/qspi/cadence_qspi.c	\
 		plat/intel/soc/common/drivers/wdt/watchdog.c
 
+include lib/zlib/zlib.mk
+PLAT_INCLUDES	+=	-Ilib/zlib
+BL2_SOURCES	+=	$(ZLIB_SOURCES)
+
 BL31_SOURCES	+=	\
 		drivers/arm/cci/cci.c					\
 		lib/cpus/aarch64/aem_generic.S				\
 		lib/cpus/aarch64/cortex_a53.S				\
 		plat/common/plat_psci_common.c				\
+		plat/intel/soc/stratix10/soc/s10_clock_manager.c	\
 		plat/intel/soc/stratix10/bl31_plat_setup.c	 	\
 		plat/intel/soc/common/socfpga_psci.c			\
 		plat/intel/soc/common/socfpga_sip_svc.c			\
+		plat/intel/soc/common/socfpga_sip_svc_v2.c		\
 		plat/intel/soc/common/socfpga_topology.c		\
 		plat/intel/soc/common/sip/socfpga_sip_ecc.c		\
 		plat/intel/soc/common/sip/socfpga_sip_fcs.c		\

diff --git a/plat/intel/soc/stratix10/soc/s10_clock_manager.c b/plat/intel/soc/stratix10/soc/s10_clock_manager.c
index 1e092de..30009f7 100644
--- a/plat/intel/soc/stratix10/soc/s10_clock_manager.c
+++ b/plat/intel/soc/stratix10/soc/s10_clock_manager.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
  */
@@ -307,3 +307,16 @@
 
 	return mmc_clk;
 }
+
+/* Get cpu freq clock */
+uint32_t get_cpu_clk(void)
+{
+	uint32_t data32, ref_clk, cpu_clk;
+
+	data32 = mmio_read_32(ALT_CLKMGR_MAINPLL + ALT_CLKMGR_MAINPLL_PLLGLOB);
+	ref_clk = get_ref_clk(data32);
+
+	cpu_clk = get_l3_clk(ref_clk)/PLAT_SYS_COUNTER_CONVERT_TO_MHZ;
+
+	return cpu_clk;
+}

diff --git a/plat/intel/soc/stratix10/soc/s10_mmc.c b/plat/intel/soc/stratix10/soc/s10_mmc.c
new file mode 100644
index 0000000..333bdd6
--- /dev/null
+++ b/plat/intel/soc/stratix10/soc/s10_mmc.c

@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2022, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <lib/mmio.h>
+
+#include "s10_clock_manager.h"
+#include "socfpga_system_manager.h"
+
+void s10_mmc_init(void)
+{
+	mmio_clrbits_32(ALT_CLKMGR_PERPLL + ALT_CLKMGR_PERPLL_EN,
+		ALT_CLKMGR_PERPLL_EN_SDMMCCLK);
+	mmio_write_32(SOCFPGA_SYSMGR(SDMMC),
+		SYSMGR_SDMMC_SMPLSEL(2) | SYSMGR_SDMMC_DRVSEL(3));
+	mmio_setbits_32(ALT_CLKMGR_PERPLL + ALT_CLKMGR_PERPLL_EN,
+		ALT_CLKMGR_PERPLL_EN_SDMMCCLK);
+}

diff --git a/plat/mediatek/build_helpers/conditional_eval_options.mk b/plat/mediatek/build_helpers/conditional_eval_options.mk
new file mode 100644
index 0000000..6bb3b4e
--- /dev/null
+++ b/plat/mediatek/build_helpers/conditional_eval_options.mk

@@ -0,0 +1,51 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# Conditional makefile variable assignment
+
+# Options depend on BUILD_TYPE variable
+ifeq ($(BUILD_TYPE),release)
+MTK_DEBUGSYS_LOCK := 1
+MTK_GET_PERM_DIS := 1
+ERRATA_KLEIN_2218950 := 0
+ERRATA_KLEIN_2184257 := 0
+ERRATA_KLEIN_BOOKER := 0
+ERRATA_MTH_BOOKER := 0
+ERRATA_MTHELP_BOOKER := 0
+CRASH_REPORTING := 1
+CONFIG_MTK_BL31_RAMDUMP := 0
+endif
+
+ifeq ($(BUILD_TYPE),debug)
+MTK_PTP3_PROC_DEBUG := 1
+MTK_SRAMRC_DEBUG := 1
+MTK_IOMMU_DEBUG := 1
+MTK_DCM_DEBUG := 1
+MTK_EMI_MPU_DEBUG := 1
+endif
+
+ifeq (${SPD},none)
+SPD_NONE:=1
+$(eval $(call add_define,SPD_NONE))
+endif
+
+# TEE OS config
+ifeq ($(SPD), tbase)
+CONFIG_TBASE := y
+endif
+
+# MICROTRUST OS config
+ifeq ($(SPD), teeid)
+CONFIG_MICROTRUST_TEEI := y
+endif
+
+ifeq (${CONFIG_ARCH_ARM_V8_2},y)
+ARCH_VERSION := armv8_2
+endif
+
+ifeq (${CONFIG_ARCH_ARM_V9},y)
+ARCH_VERSION := armv9
+endif

diff --git a/plat/mediatek/build_helpers/mtk_build_helpers.mk b/plat/mediatek/build_helpers/mtk_build_helpers.mk
new file mode 100644
index 0000000..47d96fa
--- /dev/null
+++ b/plat/mediatek/build_helpers/mtk_build_helpers.mk

@@ -0,0 +1,139 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# Get local directory path
+define GET_LOCAL_DIR
+$(patsubst %/,%,$(dir $(word $(words $(MAKEFILE_LIST)),$(MAKEFILE_LIST))))
+endef
+
+# Clear module source variable
+define CLEAR_LOCAL_SRCS
+$(eval $(1) :=)
+endef
+
+define EXPAND_SUB_MAKEFILE
+include $(S)
+endef
+
+# Expand sub rules.mk
+define INCLUDE_MAKEFILE
+$(eval MODULES_SUB_MAKEFILE := $(patsubst %,%/rules.mk,$(1)))
+$(foreach S,$(MODULES_SUB_MAKEFILE),$(eval $(EXPAND_SUB_MAKEFILE)))
+endef
+
+# Determine option variable is defined or not then define it
+define add_defined_option
+ifdef $(1)
+ifeq ($(findstring $(value $(1)), $(uppercase_table)),)
+DEFINES += -D$(1)$(if $(value $(1)),=$(value $(1)),)
+else
+ifeq ($(strip $(value $(1))),y)
+DEFINES += -D$(1)$(if $(value $(1)),=1,)
+endif
+endif
+endif
+endef
+
+define EXPAND_RULES_MAKEFILE
+LOCAL_SRCS-y :=
+MODULE :=
+SUB_RULES-y :=
+include $(S)
+endef
+
+# INCLUDE_MODULES macro expand included modules rules.mk
+# Arguments:
+#   $(1) = MODULES variables
+define INCLUDE_MODULES
+$(eval MODULES_TEMP := $(1))
+$(eval MODULES_MAKEFILE := $(patsubst %,%/rules.mk,$(MODULES_TEMP)))
+$(foreach S,$(MODULES_MAKEFILE),$(eval $(EXPAND_RULES_MAKEFILE)))
+endef
+
+# MAKE_LOCALS expand module source file variable to BL${BL}_SOURCES
+# Arguments:
+#   $(1) = source file
+#   $(2) = BL stage (1, 2, 2u, 31, 32)
+define MAKE_LOCALS
+$(eval $(call uppercase,$(2))_SOURCES += $(1))
+endef
+
+# MAKE_LINKERFILE change linker script source file name to
+# target linker script
+#   $(1) = linker script source file
+#   $(2) = BL stage
+define MAKE_LINKERFILE
+$(eval EXTRA_GENERATED_LINKER_SCRIPT += $(BUILD_PLAT)/$(2)/$(patsubst %.ld.S,%.ld,$(notdir $(1))))
+endef
+
+# MAKE_LINKERFILE_ITER call MAKE_LINKERFILE iteratively
+#   $(1) = linker script source file
+#   $(2) = BL stage
+define MAKE_LINKERFILE_ITER
+$(eval $(foreach link_src,$(1),$(call MAKE_LINKERFILE,$(link_src),$(2))))
+endef
+
+# MAKE_LD_ITER generate the linker scripts using the C preprocessor iteratively
+#   $(1) = output linker script
+#   $(2) = input template
+#   $(3) = BL stage (1, 2, 2u, 31, 32)
+define MAKE_LD_ITER
+$(eval index_list=$(shell seq $(words $(1))))
+$(eval $(foreach i, $(index_list), \
+$(call MAKE_LD,$(word $(i), $(1)), $(word $(i), $(2)),$(3))))
+endef
+
+# MAKE_MODULE reference MAKE_OBJS.
+# Create module folder under out/bl$(BL)/$(module)
+# Arguments:
+#   $(1) = module name
+#   $(2) = source file
+#   $(3) = BL stage
+define MAKE_MODULE
+        $(eval MODULE := $(strip $(1)))
+        $(eval BUILD_DIR  := ${BUILD_PLAT}/${3})
+        $(eval SOURCES    := $(2))
+        $(eval OBJS_TEMP  := $(addprefix $(BUILD_DIR)/$(MODULE)/,$(call SOURCES_TO_OBJS,$(SOURCES))))
+        $(eval MODULE_OBJS += $(OBJS_TEMP))
+        # We use sort only to get a list of unique object directory names.
+        # ordering is not relevant but sort removes duplicates.
+        $(eval TEMP_OBJ_DIRS := $(sort $(dir ${OBJS_TEMP} ${LINKERFILE})))
+        # The $(dir ) function leaves a trailing / on the directory names
+        # Rip off the / to match directory names with make rule targets.
+        $(eval OBJ_DIRS := $(patsubst %/,%,$(TEMP_OBJ_DIRS)))
+
+$(eval $(foreach objd,${OBJ_DIRS},$(call MAKE_PREREQ_DIR,${objd},${BUILD_DIR})))
+${3}_dirs: | ${OBJ_DIRS}
+
+$(eval $(call MAKE_OBJS,$(BUILD_DIR)/$(MODULE),$(SOURCES),${3}))
+
+libraries: $(OBJS_TEMP)
+endef
+
+# Include MTK configuration files
+
+# MTK makefile variables
+MTK_PLAT      := plat/mediatek
+MTK_PLAT_SOC  := ${MTK_PLAT}/${MTK_SOC}
+MTK_COMMON_CFG := $(MTK_PLAT)/common/common_config.mk
+MTK_PLAT_CFG := $(MTK_PLAT_SOC)/plat_config.mk
+MTK_PROJECT_CFG := $(MTK_PLAT)/project/$(PLAT)/project_config.mk
+MTK_OPTIONS := $(MTK_PLAT)/build_helpers/options.mk
+MTK_COND_EVAL := $(MTK_PLAT)/build_helpers/conditional_eval_options.mk
+
+# Indicate which BL should be built in command line
+ifeq (${NEED_BL31},yes)
+MTK_BL := bl31
+endif
+ifeq (${NEED_BL32},yes)
+MTK_BL := bl32
+endif
+# Include common, platform, board level config
+include $(MTK_COMMON_CFG)
+include $(MTK_PLAT_CFG)
+-include $(MTK_PROJECT_CFG)
+include $(MTK_COND_EVAL)
+include $(MTK_OPTIONS)

diff --git a/plat/mediatek/build_helpers/mtk_build_helpers_epilogue.mk b/plat/mediatek/build_helpers/mtk_build_helpers_epilogue.mk
new file mode 100644
index 0000000..22a546c
--- /dev/null
+++ b/plat/mediatek/build_helpers/mtk_build_helpers_epilogue.mk

@@ -0,0 +1,30 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# Expand include modules
+$(eval $(call INCLUDE_MODULES,$(MODULES-y)))
+
+# Make next section align to page size
+ifneq ($(MTK_EXTRA_LINKERFILE),)
+$(eval $(call MAKE_LINKERFILE_ITER,$(MTK_LINKERFILE_SOURCE),bl31))
+
+# EXTRA_GENERATED_LINKER_SCRIPT is a global variable of derived linker
+# script list(from MTK_LINKERFILE_SOURCE) after MAKE_LINKERFILE_ITER
+# function call
+EXTRA_LINKERFILE += ${EXTRA_GENERATED_LINKER_SCRIPT}
+
+# Expand derived linker script as build target
+$(eval $(call MAKE_LD_ITER, $(EXTRA_GENERATED_LINKER_SCRIPT),$(MTK_LINKERFILE_SOURCE),bl31))
+
+# mtk_align.ld MUST BE THE LAST LINKER SCRIPT!
+EXTRA_LINKERFILE += ${MTK_PLAT}/include/mtk_align.ld
+
+# bl31.ld should depend on EXTRA_LINKERFILE
+$(eval ${BUILD_PLAT}/bl31/bl31.ld: ${EXTRA_LINKERFILE})
+EXTRA_LINKERFILE := $(addprefix -T,$(EXTRA_LINKERFILE))
+else
+EXTRA_LINKERFILE :=
+endif

diff --git a/plat/mediatek/build_helpers/options.mk b/plat/mediatek/build_helpers/options.mk
new file mode 100644
index 0000000..394a605
--- /dev/null
+++ b/plat/mediatek/build_helpers/options.mk

@@ -0,0 +1,17 @@
+#
+# Copyright (c) 2022, MediaTek Inc. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# call add_defined_option to evaluate MTK defined value
+$(eval $(call add_defined_option,MTK_SIP_KERNEL_BOOT_ENABLE))
+$(eval $(call add_defined_option,PLAT_EXTRA_LD_SCRIPT))
+$(eval $(call add_defined_option,MTK_EXTRA_LINKERFILE))
+$(eval $(call add_defined_option,MTK_BL31_AS_BL2))
+$(eval $(call add_defined_option,MTK_BL33_IS_64BIT))
+$(eval $(call add_defined_option,PLAT_XLAT_TABLES_DYNAMIC))
+$(eval $(call add_defined_option,MTK_ADAPTED))
+$(eval $(call add_defined_option,MTK_SOC))
+$(eval $(call add_defined_option,UART_CLOCK))
+$(eval $(call add_defined_option,UART_BAUDRATE))

diff --git a/plat/mediatek/mt6795/aarch64/plat_helpers.S b/plat/mediatek/mt6795/aarch64/plat_helpers.S
deleted file mode 100644
index aaddb2b..0000000
--- a/plat/mediatek/mt6795/aarch64/plat_helpers.S
+++ /dev/null

@@ -1,148 +0,0 @@
-/*
- * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-#include <arch.h>
-#include <asm_macros.S>
-#include <platform_def.h>
-
-	.globl	plat_secondary_cold_boot_setup
-	.globl	plat_report_exception
-	.globl	platform_is_primary_cpu
-	.globl	plat_crash_console_init
-	.globl	plat_crash_console_putc
-	.globl	plat_crash_console_flush
-	.globl	platform_mem_init
-
-
-	.macro crash_ram_log
-	 /*
-	 * Check teearg->atf_log_buf_size.
-	 * Exit if atf_log_buf_size equals 0
-	 */
-	adr	x2, ptr_atf_crash_flag
-	ldr	x2, [x2]
-	/* exit if ptr_atf_crash_flag equals NULL */
-	cbz x2, exit_putc
-
-	/*
-	 * set atf crash magic number
-	 */
-1:
-	adr	x2, ptr_atf_crash_flag
-	ldr	x2, [x2]
-	mov_imm x1, 0xdead1abf
-	/* p_atf_log_ctrl->atf_crash_flag = 0xdead1abf */
-	str	w1, [x2]
-	/* can't use w3 return addr, w4, start of buffer addr */
-	ldr	w2, [x2]
-	cmp	w2, w1
-	b.ne	1b
-
-	/*
-	 * get cpu id
-	 */
-	mrs	x1, mpidr_el1
-	/* refer to platform_get_core_pos */
-	and	x2, x1, #MPIDR_CPU_MASK
-	and	x1, x1, #MPIDR_CLUSTER_MASK
-	/* x1 = cpu id (cpu id = aff0 + aff1*4 ) */
-	add	x1, x2, x1, LSR #6
-
-	adr	x2, ptr_atf_except_write_pos_per_cpu
-	ldr	x2, [x2]
-	/*
-	 * plus (cpu_id * 8)-->
-	 * &p_atf_log_ctrl->atf_except_write_pos_per_cpu[cpu_id]
-	 * x2 = &p_atf_log_ctrl->atf_except_write_pos_per_cpu[cpu_id];
-	 */
-	add x2, x2, x1, LSL # 3
-	/* log write */
-	/* w1 = p_atf_log_ctrl->atf_except_write_pos_per_cpu[cpu_id] */
-	ldr	x1, [x2]
-	/* *x1 = w0-->
-	 *  *(p_atf_log_ctrl->atf_except_write_pos_per_cpu[cpu_id]) = c)
-	 */
-	strb	w0, [x1]
-	/* w1++ */
-	add	x1, x1, #1
-	/* p_atf_log_ctrl->atf_except_write_pos_per_cpu[cpu_id] = w1 */
-	str	x1, [x2]
-exit_putc:
-	.endm
-
-	/* -----------------------------------------------------
-	 * void plat_secondary_cold_boot_setup (void);
-	 *
-	 * This function performs any platform specific actions
-	 * needed for a secondary cpu after a cold reset e.g
-	 * mark the cpu's presence, mechanism to place it in a
-	 * holding pen etc.
-	 * -----------------------------------------------------
-	 */
-func plat_secondary_cold_boot_setup
-	/* Do not do cold boot for secondary CPU */
-cb_panic:
-	b	cb_panic
-endfunc plat_secondary_cold_boot_setup
-
-func platform_is_primary_cpu
-	and	x0, x0, #(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK)
-	cmp	x0, #PLAT_PRIMARY_CPU
-	cset	x0, eq
-	ret
-endfunc platform_is_primary_cpu
-
-	/* ---------------------------------------------
-	 * int plat_crash_console_init(void)
-	 * Function to initialize the crash console
-	 * without a C Runtime to print crash report.
-	 * Clobber list : x0, x1, x2
-	 * ---------------------------------------------
-	 */
-func plat_crash_console_init
-	mov_imm	x0, UART0_BASE
-	mov_imm	x1, UART_CLOCK
-	mov_imm	x2, UART_BAUDRATE
-	b	console_init
-	ret
-endfunc plat_crash_console_init
-
-	/* ---------------------------------------------
-	 * int plat_crash_console_putc(void)
-	 * Function to print a character on the crash
-	 * console without a C Runtime.
-	 * Clobber list : x1, x2
-	 * ---------------------------------------------
-	 */
-func plat_crash_console_putc
-	mov_imm x1, UART0_BASE
-	b	console_core_putc
-	ret
-endfunc plat_crash_console_putc
-
-	/* ---------------------------------------------
-	 * void plat_crash_console_flush(int c)
-	 * Function to force a write of all buffered
-	 * data that hasn't been output.
-	 * Out : void.
-	 * Clobber list : x0, x1
-	 * ---------------------------------------------
-	 */
-func plat_crash_console_flush
-	mov_imm	x0, UART0_BASE
-	b	console_core_flush
-endfunc plat_crash_console_flush
-
-	/* --------------------------------------------------------
-	 * void platform_mem_init (void);
-	 *
-	 * Any memory init, relocation to be done before the
-	 * platform boots. Called very early in the boot process.
-	 * --------------------------------------------------------
-	 */
-func platform_mem_init
-	ret
-endfunc platform_mem_init
-

diff --git a/plat/mediatek/mt6795/bl31.ld.S b/plat/mediatek/mt6795/bl31.ld.S
deleted file mode 100644
index 3d881fc..0000000
--- a/plat/mediatek/mt6795/bl31.ld.S
+++ /dev/null

@@ -1,116 +0,0 @@
-/*
- * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <common/bl_common.ld.h>
-#include <lib/xlat_tables/xlat_tables_defs.h>
-
-OUTPUT_FORMAT(PLATFORM_LINKER_FORMAT)
-OUTPUT_ARCH(PLATFORM_LINKER_ARCH)
-ENTRY(bl31_entrypoint)
-
-
-MEMORY {
-	RAM (rwx): ORIGIN = BL31_BASE, LENGTH = BL31_TZRAM_SIZE
-	RAM2 (rwx): ORIGIN = TZRAM2_BASE, LENGTH = TZRAM2_SIZE
-}
-
-
-SECTIONS
-{
-    . = BL31_BASE;
-
-    ASSERT(. == ALIGN(2048),
-           "vector base is not aligned on a 2K boundary.")
-
-    __RO_START__ = .;
-    vector . : {
-        *(.vectors)
-    } >RAM
-
-    ASSERT(. == ALIGN(PAGE_SIZE),
-           "BL31_BASE address is not aligned on a page boundary.")
-
-    ro . : {
-        *bl31_entrypoint.o(.text*)
-        *(.text*)
-        *(.rodata*)
-
-	RODATA_COMMON
-
-        __RO_END_UNALIGNED__ = .;
-        /*
-         * Memory page(s) mapped to this section will be marked as read-only,
-         * executable.  No RW data from the next section must creep in.
-         * Ensure the rest of the current memory page is unused.
-         */
-        . = ALIGN(PAGE_SIZE);
-        __RO_END__ = .;
-    } >RAM
-
-    ASSERT(__CPU_OPS_END__ > __CPU_OPS_START__,
-           "cpu_ops not defined for this platform.")
-
-    /*
-     * Define a linker symbol to mark start of the RW memory area for this
-     * image.
-     */
-    __RW_START__ = . ;
-
-    DATA_SECTION >RAM
-
-#ifdef BL31_PROGBITS_LIMIT
-    ASSERT(. <= BL31_PROGBITS_LIMIT, "BL3-1 progbits has exceeded its limit.")
-#endif
-
-    STACK_SECTION >RAM
-    BSS_SECTION >RAM
-    __RW_END__ = __BSS_END__;
-
-    ASSERT(. <= BL31_LIMIT, "BL3-1 image has exceeded its limit.")
-
-    XLAT_TABLE_SECTION >RAM2
-
-#if USE_COHERENT_MEM
-    /*
-     * The base address of the coherent memory section must be page-aligned (4K)
-     * to guarantee that the coherent data are stored on their own pages and
-     * are not mixed with normal data.  This is required to set up the correct
-     * memory attributes for the coherent data page tables.
-     */
-    coherent_ram (NOLOAD) : ALIGN(PAGE_SIZE) {
-        __COHERENT_RAM_START__ = .;
-        /*
-         * Bakery locks are stored in coherent memory
-         *
-         * Each lock's data is contiguous and fully allocated by the compiler
-         */
-        *(bakery_lock)
-        *(tzfw_coherent_mem)
-        __COHERENT_RAM_END_UNALIGNED__ = .;
-        /*
-         * Memory page(s) mapped to this section will be marked
-         * as device memory.  No other unexpected data must creep in.
-         * Ensure the rest of the current memory page is unused.
-         */
-        . = ALIGN(PAGE_SIZE);
-        __COHERENT_RAM_END__ = .;
-    } >RAM2
-#endif
-
-    /*
-     * Define a linker symbol to mark end of the RW memory area for this
-     * image.
-     */
-    __BL31_END__ = .;
-
-    __BSS_SIZE__ = SIZEOF(.bss);
-#if USE_COHERENT_MEM
-    __COHERENT_RAM_UNALIGNED_SIZE__ =
-        __COHERENT_RAM_END_UNALIGNED__ - __COHERENT_RAM_START__;
-#endif
-
-    ASSERT(. <= TZRAM2_LIMIT, "TZRAM2 image has exceeded its limit.")
-}

diff --git a/plat/mediatek/mt6795/bl31_plat_setup.c b/plat/mediatek/mt6795/bl31_plat_setup.c
deleted file mode 100644
index 2051fe7..0000000
--- a/plat/mediatek/mt6795/bl31_plat_setup.c
+++ /dev/null

@@ -1,449 +0,0 @@
-/*
- * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <assert.h>
-#include <string.h>
-
-#include <arch_helpers.h>
-#include <common/bl_common.h>
-#include <common/debug.h>
-#include <drivers/arm/cci.h>
-#include <drivers/console.h>
-#include <drivers/generic_delay_timer.h>
-#include <lib/el3_runtime/context_mgmt.h>
-#include <lib/mmio.h>
-#include <lib/utils_def.h>
-#include <lib/xlat_tables/xlat_tables.h>
-#include <plat/common/common_def.h>
-#include <plat/common/platform.h>
-
-#include <mcucfg.h>
-#include <mt_cpuxgpt.h>
-#include <mtk_plat_common.h>
-#include <mtk_sip_svc.h>
-#include <plat_private.h>
-
-/*******************************************************************************
- * Declarations of linker defined symbols which will help us find the layout
- * of trusted SRAM
- ******************************************************************************/
-/*
- * The next 2 constants identify the extents of the code & RO data region.
- * These addresses are used by the MMU setup code and therefore they must be
- * page-aligned.  It is the responsibility of the linker script to ensure that
- * __RO_START__ and __RO_END__ linker symbols refer to page-aligned addresses.
- */
-IMPORT_SYM(unsigned long, __RO_START__,	BL31_RO_BASE);
-IMPORT_SYM(unsigned long, __RO_END__,	BL31_RO_LIMIT);
-
-/*
- * Placeholder variables for copying the arguments that have been passed to
- * BL3-1 from BL2.
- */
-static entry_point_info_t bl32_image_ep_info;
-static entry_point_info_t bl33_image_ep_info;
-
-static const int cci_map[] = {
-	PLAT_MT_CCI_CLUSTER0_SL_IFACE_IX,
-	PLAT_MT_CCI_CLUSTER1_SL_IFACE_IX
-};
-
-static uint32_t cci_map_length = ARRAY_SIZE(cci_map);
-
-/* Table of regions to map using the MMU.  */
-static const mmap_region_t plat_mmap[] = {
-	/* for TF text, RO, RW */
-	MAP_REGION_FLAT(MTK_DEV_RNG0_BASE, MTK_DEV_RNG0_SIZE,
-			MT_DEVICE | MT_RW | MT_SECURE),
-	MAP_REGION_FLAT(MTK_DEV_RNG1_BASE, MTK_DEV_RNG1_SIZE,
-			MT_DEVICE | MT_RW | MT_SECURE),
-	MAP_REGION_FLAT(RAM_CONSOLE_BASE & ~(PAGE_SIZE_MASK), RAM_CONSOLE_SIZE,
-						MT_DEVICE | MT_RW | MT_NS),
-	{ 0 }
-
-};
-
-/*******************************************************************************
- * Macro generating the code for the function setting up the pagetables as per
- * the platform memory map & initialize the mmu, for the given exception level
- ******************************************************************************/
-#define DEFINE_CONFIGURE_MMU_EL(_el)					\
-	void plat_configure_mmu_el ## _el(unsigned long total_base,	\
-				unsigned long total_size,	\
-				unsigned long ro_start,	\
-				unsigned long ro_limit,	\
-				unsigned long coh_start,	\
-				unsigned long coh_limit)	\
-	{								\
-		mmap_add_region(total_base, total_base,			\
-				total_size,				\
-				MT_MEMORY | MT_RW | MT_SECURE);		\
-		mmap_add_region(ro_start, ro_start,			\
-				ro_limit - ro_start,			\
-				MT_MEMORY | MT_RO | MT_SECURE);		\
-		mmap_add_region(coh_start, coh_start,			\
-				coh_limit - coh_start,			\
-				MT_DEVICE | MT_RW | MT_SECURE);		\
-		mmap_add(plat_mmap);					\
-		init_xlat_tables();					\
-									\
-		enable_mmu_el ## _el(0);				\
-	}
-
-/* Define EL3 variants of the function initialising the MMU */
-DEFINE_CONFIGURE_MMU_EL(3)
-
-unsigned int plat_get_syscnt_freq2(void)
-{
-	return SYS_COUNTER_FREQ_IN_TICKS;
-}
-
-void plat_cci_init(void)
-{
-	/* Initialize CCI driver */
-	cci_init(PLAT_MT_CCI_BASE, cci_map, cci_map_length);
-}
-
-void plat_cci_enable(void)
-{
-	/*
-	 * Enable CCI coherency for this cluster.
-	 * No need for locks as no other cpu is active at the moment.
-	 */
-	cci_enable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr()));
-}
-
-void plat_cci_disable(void)
-{
-	cci_disable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr()));
-}
-
-
-static void platform_setup_cpu(void)
-{
-	/* setup big cores */
-	mmio_write_32((uintptr_t)&mt6795_mcucfg->mp1_config_res,
-		MP1_DIS_RGU0_WAIT_PD_CPUS_L1_ACK |
-		MP1_DIS_RGU1_WAIT_PD_CPUS_L1_ACK |
-		MP1_DIS_RGU2_WAIT_PD_CPUS_L1_ACK |
-		MP1_DIS_RGU3_WAIT_PD_CPUS_L1_ACK |
-		MP1_DIS_RGU_NOCPU_WAIT_PD_CPUS_L1_ACK);
-	mmio_setbits_32((uintptr_t)&mt6795_mcucfg->mp1_miscdbg, MP1_AINACTS);
-	mmio_setbits_32((uintptr_t)&mt6795_mcucfg->mp1_clkenm_div,
-		MP1_SW_CG_GEN);
-	mmio_clrbits_32((uintptr_t)&mt6795_mcucfg->mp1_rst_ctl,
-		MP1_L2RSTDISABLE);
-
-	/* set big cores arm64 boot mode */
-	mmio_setbits_32((uintptr_t)&mt6795_mcucfg->mp1_cpucfg,
-		MP1_CPUCFG_64BIT);
-
-	/* set LITTLE cores arm64 boot mode */
-	mmio_setbits_32((uintptr_t)&mt6795_mcucfg->mp0_rv_addr[0].rv_addr_hw,
-		MP0_CPUCFG_64BIT);
-}
-
-/*******************************************************************************
- * Return a pointer to the 'entry_point_info' structure of the next image for
- * the security state specified. BL33 corresponds to the non-secure image type
- * while BL32 corresponds to the secure image type. A NULL pointer is returned
- * if the image does not exist.
- ******************************************************************************/
-entry_point_info_t *bl31_plat_get_next_image_ep_info(uint32_t type)
-{
-	entry_point_info_t *next_image_info;
-
-	next_image_info = (type == NON_SECURE) ?
-			&bl33_image_ep_info : &bl32_image_ep_info;
-
-	/* None of the images on this platform can have 0x0 as the entrypoint */
-	if (next_image_info->pc)
-		return next_image_info;
-	else
-		return NULL;
-}
-
-/*******************************************************************************
- * Perform any BL3-1 early platform setup. Here is an opportunity to copy
- * parameters passed by the calling EL (S-EL1 in BL2 & EL3 in BL1) before they
- * are lost (potentially). This needs to be done before the MMU is initialized
- * so that the memory layout can be used while creating page tables.
- * BL2 has flushed this information to memory, so we are guaranteed to pick up
- * good data.
- ******************************************************************************/
-void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
-				u_register_t arg2, u_register_t arg3)
-{
-	struct mtk_bl_param_t *pmtk_bl_param = (struct mtk_bl_param_t *)arg0;
-	struct atf_arg_t *teearg;
-	unsigned long long normal_base;
-	unsigned long long atf_base;
-
-	assert(pmtk_bl_param != NULL);
-	/*
-	 * Mediatek preloader(i.e, BL2) is in 32 bit state, high 32bits
-	 * of 64 bit GP registers are UNKNOWN if CPU warm reset from 32 bit
-	 * to 64 bit state. So we need to clear high 32bit,
-	 * which may be random value.
-	 */
-	pmtk_bl_param =
-	(struct mtk_bl_param_t *)((uint64_t)pmtk_bl_param & 0x00000000ffffffff);
-
-	teearg  = (struct atf_arg_t *)pmtk_bl_param->tee_info_addr;
-
-	console_init(teearg->atf_log_port, UART_CLOCK, UART_BAUDRATE);
-	memcpy((void *)&gteearg, (void *)teearg, sizeof(struct atf_arg_t));
-
-	normal_base = 0;
-    /* in ATF boot time, timer for cntpct_el0 is not initialized
-     * so it will not count now.
-     */
-	atf_base = read_cntpct_el0();
-	sched_clock_init(normal_base, atf_base);
-
-	VERBOSE("bl31_setup\n");
-
-	/* Populate entry point information for BL3-2 and BL3-3 */
-	SET_PARAM_HEAD(&bl32_image_ep_info,
-				PARAM_EP,
-				VERSION_1,
-				0);
-	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
-	bl32_image_ep_info.pc = BL32_BASE;
-
-	SET_PARAM_HEAD(&bl33_image_ep_info,
-				PARAM_EP,
-				VERSION_1,
-				0);
-	/*
-	 * Tell BL3-1 where the non-trusted software image
-	 * is located and the entry state information
-	 */
-	/* BL33_START_ADDRESS */
-	bl33_image_ep_info.pc = pmtk_bl_param->bl33_start_addr;
-	bl33_image_ep_info.spsr = plat_get_spsr_for_bl33_entry();
-	bl33_image_ep_info.args.arg4 =  pmtk_bl_param->bootarg_loc;
-	bl33_image_ep_info.args.arg5 =  pmtk_bl_param->bootarg_size;
-	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
-}
-/*******************************************************************************
- * Perform any BL3-1 platform setup code
- ******************************************************************************/
-
-void bl31_platform_setup(void)
-{
-	platform_setup_cpu();
-
-	generic_delay_timer_init();
-
-	plat_mt_gic_driver_init();
-	/* Initialize the gic cpu and distributor interfaces */
-	plat_mt_gic_init();
-
-	/* Topologies are best known to the platform. */
-	mt_setup_topology();
-}
-/*******************************************************************************
- * Perform the very early platform specific architectural setup here. At the
- * moment this is only intializes the mmu in a quick and dirty way.
- * Init MTK propiartary log buffer control field.
- ******************************************************************************/
-void bl31_plat_arch_setup(void)
-{
-	/* Enable non-secure access to CCI-400 registers */
-	mmio_write_32(CCI400_BASE + CCI_SEC_ACCESS_OFFSET, 0x1);
-
-	plat_cci_init();
-	plat_cci_enable();
-
-	if (gteearg.atf_log_buf_size != 0) {
-		INFO("mmap atf buffer : 0x%x, 0x%x\n\r",
-			gteearg.atf_log_buf_start,
-			gteearg.atf_log_buf_size);
-
-		mmap_add_region(
-			gteearg.atf_log_buf_start &
-			~(PAGE_SIZE_2MB_MASK),
-			gteearg.atf_log_buf_start &
-			~(PAGE_SIZE_2MB_MASK),
-			PAGE_SIZE_2MB,
-			MT_DEVICE | MT_RW | MT_NS);
-
-		INFO("mmap atf buffer (force 2MB aligned):0x%x, 0x%x\n",
-			(gteearg.atf_log_buf_start & ~(PAGE_SIZE_2MB_MASK)),
-		PAGE_SIZE_2MB);
-	}
-	/*
-	 * add TZRAM_BASE to memory map
-	 * then set RO and COHERENT to different attribute
-	 */
-	plat_configure_mmu_el3(
-		(TZRAM_BASE & ~(PAGE_SIZE_MASK)),
-		(TZRAM_SIZE & ~(PAGE_SIZE_MASK)),
-		(BL31_RO_BASE & ~(PAGE_SIZE_MASK)),
-		BL31_RO_LIMIT,
-		BL_COHERENT_RAM_BASE,
-		BL_COHERENT_RAM_END);
-	/* Initialize for ATF log buffer */
-	if (gteearg.atf_log_buf_size != 0) {
-		gteearg.atf_aee_debug_buf_size = ATF_AEE_BUFFER_SIZE;
-		gteearg.atf_aee_debug_buf_start =
-			gteearg.atf_log_buf_start +
-			gteearg.atf_log_buf_size - ATF_AEE_BUFFER_SIZE;
-		INFO("ATF log service is registered (0x%x, aee:0x%x)\n",
-			gteearg.atf_log_buf_start,
-			gteearg.atf_aee_debug_buf_start);
-	} else{
-		gteearg.atf_aee_debug_buf_size = 0;
-		gteearg.atf_aee_debug_buf_start = 0;
-	}
-
-	/* Platform code before bl31_main */
-	/* compatible to the earlier chipset */
-
-	/* Show to ATF log buffer & UART */
-	INFO("BL3-1: %s\n", version_string);
-	INFO("BL3-1: %s\n", build_message);
-
-}
-#if 0
-/* MTK Define */
-#define ACTLR_CPUECTLR_BIT    (1 << 1)
-
-void enable_ns_access_to_cpuectlr(void)
-{
-	unsigned int next_actlr;
-
-
-	/* ACTLR_EL1 do not implement CUPECTLR  */
-	next_actlr = read_actlr_el2();
-	next_actlr |= ACTLR_CPUECTLR_BIT;
-	write_actlr_el2(next_actlr);
-
-	next_actlr = read_actlr_el3();
-	next_actlr |= ACTLR_CPUECTLR_BIT;
-	write_actlr_el3(next_actlr);
-}
-#endif
-/*******************************************************************************
- * This function prepare boot argument for 64 bit kernel entry
- ******************************************************************************/
-static entry_point_info_t *bl31_plat_get_next_kernel64_ep_info(void)
-{
-	entry_point_info_t *next_image_info;
-	unsigned int mode;
-
-	mode = 0;
-
-	/* Kernel image is always non-secured */
-	next_image_info = &bl33_image_ep_info;
-
-	/* Figure out what mode we enter the non-secure world in */
-	if (el_implemented(2) != EL_IMPL_NONE) {
-		INFO("Kernel_EL2\n");
-		mode = MODE_EL2;
-	} else{
-		INFO("Kernel_EL1\n");
-		mode = MODE_EL1;
-	}
-
-	INFO("Kernel is 64Bit\n");
-	next_image_info->spsr =
-		SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
-	next_image_info->pc = get_kernel_info_pc();
-	next_image_info->args.arg0 = get_kernel_info_r0();
-	next_image_info->args.arg1 = get_kernel_info_r1();
-
-	INFO("pc=0x%lx, r0=0x%lx, r1=0x%lx\n",
-				 next_image_info->pc,
-				 next_image_info->args.arg0,
-				 next_image_info->args.arg1);
-
-
-	SET_SECURITY_STATE(next_image_info->h.attr, NON_SECURE);
-
-	/* None of the images on this platform can have 0x0 as the entrypoint */
-	if (next_image_info->pc)
-		return next_image_info;
-	else
-		return NULL;
-}
-
-/*******************************************************************************
- * This function prepare boot argument for 32 bit kernel entry
- ******************************************************************************/
-static entry_point_info_t *bl31_plat_get_next_kernel32_ep_info(void)
-{
-	entry_point_info_t *next_image_info;
-	unsigned int mode;
-
-	mode = 0;
-
-	/* Kernel image is always non-secured */
-	next_image_info = &bl33_image_ep_info;
-
-	/* Figure out what mode we enter the non-secure world in */
-	mode = MODE32_hyp;
-	/*
-	* TODO: Consider the possibility of specifying the SPSR in
-	* the FIP ToC and allowing the platform to have a say as
-	* well.
-	*/
-
-	INFO("Kernel is 32Bit\n");
-	next_image_info->spsr =
-		SPSR_MODE32(mode, SPSR_T_ARM, SPSR_E_LITTLE,
-		(DAIF_FIQ_BIT | DAIF_IRQ_BIT | DAIF_ABT_BIT));
-	next_image_info->pc = get_kernel_info_pc();
-	next_image_info->args.arg0 = get_kernel_info_r0();
-	next_image_info->args.arg1 = get_kernel_info_r1();
-	next_image_info->args.arg2 = get_kernel_info_r2();
-
-	INFO("pc=0x%lx, r0=0x%lx, r1=0x%lx, r2=0x%lx\n",
-				 next_image_info->pc,
-				 next_image_info->args.arg0,
-				 next_image_info->args.arg1,
-				 next_image_info->args.arg2);
-
-
-	SET_SECURITY_STATE(next_image_info->h.attr, NON_SECURE);
-
-	/* None of the images on this platform can have 0x0 as the entrypoint */
-	if (next_image_info->pc)
-		return next_image_info;
-	else
-		return NULL;
-}
-
-/*******************************************************************************
- * This function prepare boot argument for kernel entrypoint
- ******************************************************************************/
-void bl31_prepare_kernel_entry(uint64_t k32_64)
-{
-	entry_point_info_t *next_image_info;
-	uint32_t image_type;
-
-	/* Determine which image to execute next */
-	/* image_type = bl31_get_next_image_type(); */
-	image_type = NON_SECURE;
-
-	/* Program EL3 registers to enable entry into the next EL */
-	if (k32_64 == 0)
-		next_image_info = bl31_plat_get_next_kernel32_ep_info();
-	else
-		next_image_info = bl31_plat_get_next_kernel64_ep_info();
-
-	assert(next_image_info);
-	assert(image_type == GET_SECURITY_STATE(next_image_info->h.attr));
-
-	INFO("BL3-1: Preparing for EL3 exit to %s world, Kernel\n",
-		(image_type == SECURE) ? "secure" : "normal");
-	INFO("BL3-1: Next image address = 0x%llx\n",
-		(unsigned long long) next_image_info->pc);
-	INFO("BL3-1: Next image spsr = 0x%x\n", next_image_info->spsr);
-	cm_init_my_context(next_image_info);
-	cm_prepare_el3_exit(image_type);
-}

diff --git a/plat/mediatek/mt6795/drivers/timer/mt_cpuxgpt.c b/plat/mediatek/mt6795/drivers/timer/mt_cpuxgpt.c
deleted file mode 100644
index 3696f8e..0000000
--- a/plat/mediatek/mt6795/drivers/timer/mt_cpuxgpt.c
+++ /dev/null

@@ -1,67 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <stdint.h>
-
-#include <arch_helpers.h>
-#include <common/debug.h>
-#include <lib/mmio.h>
-#include <plat/common/platform.h>
-
-#include <mt_cpuxgpt.h>
-
-#define CPUXGPT_BASE	0x10200000
-#define INDEX_BASE		(CPUXGPT_BASE+0x0674)
-#define CTL_BASE		(CPUXGPT_BASE+0x0670)
-
-uint64_t normal_time_base;
-uint64_t atf_time_base;
-
-void sched_clock_init(uint64_t normal_base, uint64_t atf_base)
-{
-	normal_time_base = normal_base;
-	atf_time_base = atf_base;
-}
-
-uint64_t sched_clock(void)
-{
-	uint64_t cval;
-
-	cval = (((read_cntpct_el0() - atf_time_base)*1000)/
-		SYS_COUNTER_FREQ_IN_MHZ) + normal_time_base;
-	return cval;
-}
-
-/*
-  * Return: 0 - Trying to disable the CPUXGPT control bit,
-  * and not allowed to disable it.
-  * Return: 1 - reg_addr is not realted to disable the control bit.
-  */
-unsigned char check_cpuxgpt_write_permission(unsigned int reg_addr,
-	unsigned int reg_value)
-{
-	unsigned int idx;
-	unsigned int ctl_val;
-
-	if (reg_addr == CTL_BASE) {
-		idx = mmio_read_32(INDEX_BASE);
-
-		/* idx 0: CPUXGPT system control */
-		if (idx == 0) {
-			ctl_val = mmio_read_32(CTL_BASE);
-			if (ctl_val & 1) {
-				/*
-				 * if enable bit already set,
-				 * then bit 0 is not allow to set as 0
-				 */
-				if (!(reg_value & 1))
-					return 0;
-			}
-		}
-	}
-	return 1;
-}
-

diff --git a/plat/mediatek/mt6795/drivers/timer/mt_cpuxgpt.h b/plat/mediatek/mt6795/drivers/timer/mt_cpuxgpt.h
deleted file mode 100644
index 84df081..0000000
--- a/plat/mediatek/mt6795/drivers/timer/mt_cpuxgpt.h
+++ /dev/null

@@ -1,28 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef MT_CPUXGPT_H
-#define MT_CPUXGPT_H
-
-/* REG */
-#define INDEX_CTL_REG       0x000
-#define INDEX_STA_REG       0x004
-#define INDEX_CNT_L_INIT    0x008
-#define INDEX_CNT_H_INIT    0x00C
-
-/* CTL_REG SET */
-#define EN_CPUXGPT          0x01
-#define EN_AHLT_DEBUG       0x02
-#define CLK_DIV1            (0x1 << 8)
-#define CLK_DIV2            (0x2 << 8)
-#define CLK_DIV4            (0x4 << 8)
-#define CLK_DIV_MASK        (~(0x7<<8))
-
-void generic_timer_backup(void);
-void sched_clock_init(uint64_t normal_base, uint64_t atf_base);
-uint64_t sched_clock(void);
-
-#endif /* MT_CPUXGPT_H */

diff --git a/plat/mediatek/mt6795/include/mcucfg.h b/plat/mediatek/mt6795/include/mcucfg.h
deleted file mode 100644
index 21c5394..0000000
--- a/plat/mediatek/mt6795/include/mcucfg.h
+++ /dev/null

@@ -1,155 +0,0 @@
-/*
- * Copyright (c) 2016-2017, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef MCUCFG_H
-#define MCUCFG_H
-
-#include <stdint.h>
-
-#include <platform_def.h>
-
-struct mt6795_mcucfg_regs {
-	uint32_t mp0_ca7l_cache_config;
-	struct {
-		uint32_t mem_delsel0;
-		uint32_t mem_delsel1;
-	} mp0_cpu[4];
-	uint32_t mp0_cache_mem_delsel0;
-	uint32_t mp0_cache_mem_delsel1;
-	uint32_t mp0_axi_config;
-	uint32_t mp0_misc_config[2];
-	struct {
-		uint32_t rv_addr_lw;
-		uint32_t rv_addr_hw;
-	} mp0_rv_addr[4];
-	uint32_t mp0_ca7l_cfg_dis;
-	uint32_t mp0_ca7l_clken_ctrl;
-	uint32_t mp0_ca7l_rst_ctrl;
-	uint32_t mp0_ca7l_misc_config;
-	uint32_t mp0_ca7l_dbg_pwr_ctrl;
-	uint32_t mp0_rw_rsvd0;
-	uint32_t mp0_rw_rsvd1;
-	uint32_t mp0_ro_rsvd;
-	uint32_t reserved0_0[100];
-	uint32_t mp1_cpucfg;
-	uint32_t mp1_miscdbg;
-	uint32_t reserved0_1[13];
-	uint32_t mp1_rst_ctl;
-	uint32_t mp1_clkenm_div;
-	uint32_t reserved0_2[7];
-	uint32_t mp1_config_res;
-	uint32_t reserved0_3[13];
-	struct {
-		uint32_t rv_addr_lw;
-		uint32_t rv_addr_hw;
-	} mp1_rv_addr[2];
-	uint32_t reserved0_4[84];
-	uint32_t mp0_rst_status;		/* 0x400 */
-	uint32_t mp0_dbg_ctrl;
-	uint32_t mp0_dbg_flag;
-	uint32_t mp0_ca7l_ir_mon;
-	struct {
-		uint32_t pc_lw;
-		uint32_t pc_hw;
-		uint32_t fp_arch32;
-		uint32_t sp_arch32;
-		uint32_t fp_arch64_lw;
-		uint32_t fp_arch64_hw;
-		uint32_t sp_arch64_lw;
-		uint32_t sp_arch64_hw;
-	} mp0_dbg_core[4];
-	uint32_t dfd_ctrl;
-	uint32_t dfd_cnt_l;
-	uint32_t dfd_cnt_h;
-	uint32_t misccfg_mp0_rw_rsvd;
-	uint32_t misccfg_sec_vio_status0;
-	uint32_t misccfg_sec_vio_status1;
-	uint32_t reserved1[22];
-	uint32_t misccfg_rw_rsvd;		/* 0x500 */
-	uint32_t mcusys_dbg_mon_sel_a;
-	uint32_t mcusys_dbg_mon;
-	uint32_t reserved2[61];
-	uint32_t mcusys_config_a;		/* 0x600 */
-	uint32_t mcusys_config1_a;
-	uint32_t mcusys_gic_peribase_a;
-	uint32_t reserved3;
-	uint32_t sec_range0_start;		/* 0x610 */
-	uint32_t sec_range0_end;
-	uint32_t sec_range_enable;
-	uint32_t reserved4;
-	uint32_t int_pol_ctl[8];		/* 0x620 */
-	uint32_t aclken_div;			/* 0x640 */
-	uint32_t pclken_div;
-	uint32_t l2c_sram_ctrl;
-	uint32_t armpll_jit_ctrl;
-	uint32_t cci_addrmap;			/* 0x650 */
-	uint32_t cci_config;
-	uint32_t cci_periphbase;
-	uint32_t cci_nevntcntovfl;
-	uint32_t cci_clk_ctrl;			/* 0x660 */
-	uint32_t cci_acel_s1_ctrl;
-	uint32_t bus_fabric_dcm_ctrl;
-	uint32_t reserved5;
-	uint32_t xgpt_ctl;			/* 0x670 */
-	uint32_t xgpt_idx;
-	uint32_t ptpod2_ctl0;
-	uint32_t ptpod2_ctl1;
-	uint32_t mcusys_revid;
-	uint32_t mcusys_rw_rsvd0;
-	uint32_t mcusys_rw_rsvd1;
-};
-
-static struct mt6795_mcucfg_regs *const mt6795_mcucfg = (void *)MCUCFG_BASE;
-
-/* cpu boot mode */
-#define	MP0_CPUCFG_64BIT_SHIFT	12
-#define	MP1_CPUCFG_64BIT_SHIFT	28
-#define	MP0_CPUCFG_64BIT	(U(0xf) << MP0_CPUCFG_64BIT_SHIFT)
-#define	MP1_CPUCFG_64BIT	(U(0xf) << MP1_CPUCFG_64BIT_SHIFT)
-
-/* scu related */
-enum {
-	MP0_ACINACTM_SHIFT = 4,
-	MP1_ACINACTM_SHIFT = 0,
-	MP0_ACINACTM = 1 << MP0_ACINACTM_SHIFT,
-	MP1_ACINACTM = 1 << MP1_ACINACTM_SHIFT
-};
-
-enum {
-	MP1_DIS_RGU0_WAIT_PD_CPUS_L1_ACK_SHIFT = 0,
-	MP1_DIS_RGU1_WAIT_PD_CPUS_L1_ACK_SHIFT = 4,
-	MP1_DIS_RGU2_WAIT_PD_CPUS_L1_ACK_SHIFT = 8,
-	MP1_DIS_RGU3_WAIT_PD_CPUS_L1_ACK_SHIFT = 12,
-	MP1_DIS_RGU_NOCPU_WAIT_PD_CPUS_L1_ACK_SHIFT = 16,
-
-	MP1_DIS_RGU0_WAIT_PD_CPUS_L1_ACK =
-		0xf << MP1_DIS_RGU0_WAIT_PD_CPUS_L1_ACK_SHIFT,
-	MP1_DIS_RGU1_WAIT_PD_CPUS_L1_ACK =
-		0xf << MP1_DIS_RGU1_WAIT_PD_CPUS_L1_ACK_SHIFT,
-	MP1_DIS_RGU2_WAIT_PD_CPUS_L1_ACK =
-		0xf << MP1_DIS_RGU2_WAIT_PD_CPUS_L1_ACK_SHIFT,
-	MP1_DIS_RGU3_WAIT_PD_CPUS_L1_ACK =
-		0xf << MP1_DIS_RGU3_WAIT_PD_CPUS_L1_ACK_SHIFT,
-	MP1_DIS_RGU_NOCPU_WAIT_PD_CPUS_L1_ACK =
-		0xf << MP1_DIS_RGU_NOCPU_WAIT_PD_CPUS_L1_ACK_SHIFT
-};
-
-enum {
-	MP1_AINACTS_SHIFT = 4,
-	MP1_AINACTS = 1 << MP1_AINACTS_SHIFT
-};
-
-enum {
-	MP1_SW_CG_GEN_SHIFT = 12,
-	MP1_SW_CG_GEN = 1 << MP1_SW_CG_GEN_SHIFT
-};
-
-enum {
-	MP1_L2RSTDISABLE_SHIFT = 14,
-	MP1_L2RSTDISABLE = 1 << MP1_L2RSTDISABLE_SHIFT
-};
-
-#endif  /* MCUCFG_H */

diff --git a/plat/mediatek/mt6795/include/plat_macros.S b/plat/mediatek/mt6795/include/plat_macros.S
deleted file mode 100644
index d198fdc..0000000
--- a/plat/mediatek/mt6795/include/plat_macros.S
+++ /dev/null

@@ -1,85 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <drivers/arm/cci.h>
-#include <platform_def.h>
-
-.section .rodata.gic_reg_name, "aS"
-gicc_regs:
-	.asciz "gicc_hppir", "gicc_ahppir", "gicc_ctlr", ""
-gicd_pend_reg:
-	.asciz "gicd_ispendr regs (Offsets 0x200 - 0x278)\n"	\
-		" Offset:\t\t\tvalue\n"
-newline:
-	.asciz "\n"
-spacer:
-	.asciz ":\t\t0x"
-
-	/* ---------------------------------------------
-	 * The below macro prints out relevant GIC
-	 * registers whenever an unhandled exception is
-	 * taken in BL3-1.
-	 * Clobbers: x0 - x10, x16, x17, sp
-	 * ---------------------------------------------
-	 */
-	.macro plat_crash_print_regs
-	mov_imm x16, BASE_GICD_BASE
-	mov_imm x17, BASE_GICC_BASE
-	/* Load the gicc reg list to x6 */
-	adr	x6, gicc_regs
-	/* Load the gicc regs to gp regs used by str_in_crash_buf_print */
-	ldr	w8, [x17, #GICC_HPPIR]
-	ldr	w9, [x17, #GICC_AHPPIR]
-	ldr	w10, [x17, #GICC_CTLR]
-	/* Store to the crash buf and print to console */
-	bl	str_in_crash_buf_print
-
-	/* Print the GICD_ISPENDR regs */
-	add	x7, x16, #GICD_ISPENDR
-	adr	x4, gicd_pend_reg
-	bl	asm_print_str
-gicd_ispendr_loop:
-	sub	x4, x7, x16
-	cmp	x4, #0x280
-	b.eq	exit_print_gic_regs
-	bl	asm_print_hex
-
-	adr	x4, spacer
-	bl	asm_print_str
-
-	ldr	x4, [x7], #8
-	bl	asm_print_hex
-
-	adr	x4, newline
-	bl	asm_print_str
-	b	gicd_ispendr_loop
-exit_print_gic_regs:
-	.endm
-
-.section .rodata.cci_reg_name, "aS"
-cci_iface_regs:
-	.asciz "cci_snoop_ctrl_cluster0", "cci_snoop_ctrl_cluster1" , ""
-
-	/* ------------------------------------------------
-	 * The below macro prints out relevant interconnect
-	 * registers whenever an unhandled exception is
-	 * taken in BL3-1.
-	 * Clobbers: x0 - x9, sp
-	 * ------------------------------------------------
-	 */
-	.macro plat_print_interconnect_regs
-	adr	x6, cci_iface_regs
-	/* Store in x7 the base address of the first interface */
-	mov_imm	x7, (PLAT_MT_CCI_BASE + SLAVE_IFACE_OFFSET(	\
-			PLAT_MT_CCI_CLUSTER0_SL_IFACE_IX))
-	ldr	w8, [x7, #SNOOP_CTRL_REG]
-	/* Store in x7 the base address of the second interface */
-	mov_imm	x7, (PLAT_MT_CCI_BASE + SLAVE_IFACE_OFFSET(	\
-			PLAT_MT_CCI_CLUSTER1_SL_IFACE_IX))
-	ldr	w9, [x7, #SNOOP_CTRL_REG]
-	/* Store to the crash buf and print to console */
-	bl	str_in_crash_buf_print
-	.endm

diff --git a/plat/mediatek/mt6795/include/plat_private.h b/plat/mediatek/mt6795/include/plat_private.h
deleted file mode 100644
index f7450ca..0000000
--- a/plat/mediatek/mt6795/include/plat_private.h
+++ /dev/null

@@ -1,38 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef PLAT_PRIVATE_H
-#define PLAT_PRIVATE_H
-
-#include <stdint.h>
-
-#include <lib/xlat_tables/xlat_tables.h>
-
-void plat_configure_mmu_el3(unsigned long total_base,
-					unsigned long total_size,
-					unsigned long,
-					unsigned long,
-					unsigned long,
-					unsigned long);
-
-void plat_cci_init(void);
-void plat_cci_enable(void);
-void plat_cci_disable(void);
-
-/* Declarations for plat_mt_gic.c */
-void plat_mt_gic_init(void);
-
-/* Declarations for plat_topology.c */
-int mt_setup_topology(void);
-void plat_delay_timer_init(void);
-
-void plat_mt_gic_driver_init(void);
-void plat_mt_gic_init(void);
-void plat_mt_gic_cpuif_enable(void);
-void plat_mt_gic_cpuif_disable(void);
-void plat_mt_gic_pcpu_init(void);
-
-#endif /* PLAT_PRIVATE_H */

diff --git a/plat/mediatek/mt6795/include/plat_sip_calls.h b/plat/mediatek/mt6795/include/plat_sip_calls.h
deleted file mode 100644
index b9a1363..0000000
--- a/plat/mediatek/mt6795/include/plat_sip_calls.h
+++ /dev/null

@@ -1,15 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef PLAT_SIP_CALLS_H
-#define PLAT_SIP_CALLS_H
-
-/*******************************************************************************
- * Plat SiP function constants
- ******************************************************************************/
-#define MTK_PLAT_SIP_NUM_CALLS	0
-
-#endif /* PLAT_SIP_CALLS_H */

diff --git a/plat/mediatek/mt6795/include/platform_def.h b/plat/mediatek/mt6795/include/platform_def.h
deleted file mode 100644
index b353a3d..0000000
--- a/plat/mediatek/mt6795/include/platform_def.h
+++ /dev/null

@@ -1,238 +0,0 @@
-/*
- * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef PLATFORM_DEF_H
-#define PLATFORM_DEF_H
-
-#include <lib/utils_def.h>
-
-#define PLAT_PRIMARY_CPU  0x0
-
-/* Special value used to verify platform parameters from BL2 to BL3-1 */
-#define MT_BL31_PLAT_PARAM_VAL  0x0f1e2d3c4b5a6978ULL
-
-#define IO_PHYS             (0x10000000)
-#define INFRACFG_AO_BASE    (IO_PHYS + 0x1000)
-#define MCUCFG_BASE         (IO_PHYS + 0x200000)
-#define PERI_BASE           (IO_PHYS + 0x1000000)
-
-
-#define GPIO_BASE           (IO_PHYS + 0x370000)
-#define SPM_BASE            (IO_PHYS + 0x6000)
-#define RGU_BASE            (MCUCFG_BASE + 0x11000)
-#define PMIC_WRAP_BASE      (IO_PHYS + 0x10000)
-
-#define TRNG_base           (MCUCFG_BASE + 0x230000)
-#define MT_GIC_BASE         (0x10220000)
-#define MCU_SYS_SIZE        (0x700000)
-#define PLAT_MT_CCI_BASE    (IO_PHYS + 0x390000)
-
-/* Aggregate of all devices in the first GB */
-#define MTK_DEV_RNG0_BASE   IO_PHYS
-#define MTK_DEV_RNG0_SIZE   0x400000
-#define MTK_DEV_RNG1_BASE   (PERI_BASE)
-#define MTK_DEV_RNG1_SIZE   0x4000000
-
-/*******************************************************************************
- * UART related constants
- ******************************************************************************/
-#define UART0_BASE (PERI_BASE + 0x2000)
-
-#define UART_BAUDRATE   (921600)
-#define UART_CLOCK (26000000)
-
-/*******************************************************************************
- * System counter frequency related constants
- ******************************************************************************/
-#define SYS_COUNTER_FREQ_IN_TICKS	13000000
-#define SYS_COUNTER_FREQ_IN_MHZ		(SYS_COUNTER_FREQ_IN_TICKS/1000000)
-
-/*******************************************************************************
- * GIC-400 & interrupt handling related constants
- ******************************************************************************/
-
-/* Base MTK_platform compatible GIC memory map */
-#define BASE_GICD_BASE      (MT_GIC_BASE+0x1000)
-#define BASE_GICC_BASE      (MT_GIC_BASE + 0x2000)
-#define BASE_GICR_BASE      (MT_GIC_BASE + 0x200000)
-#define BASE_GICH_BASE      (MT_GIC_BASE + 0x4000)
-#define BASE_GICV_BASE      (MT_GIC_BASE + 0x6000)
-
-#define INT_POL_CTL0        0x10200620
-#define GIC_PRIVATE_SIGNALS (32)
-
-/*******************************************************************************
- * CCI-400 related constants
- ******************************************************************************/
-#define PLAT_MT_CCI_CLUSTER0_SL_IFACE_IX  4
-#define PLAT_MT_CCI_CLUSTER1_SL_IFACE_IX  3
-
-/*******************************************************************************
- * WDT Registers
- ******************************************************************************/
-#define MTK_WDT_BASE                        (RGU_BASE)
-#define MTK_WDT_SIZE                        (0x1000)
-#define MTK_WDT_MODE                        (MTK_WDT_BASE+0x0000)
-#define MTK_WDT_LENGTH                      (MTK_WDT_BASE+0x0004)
-#define MTK_WDT_RESTART                     (MTK_WDT_BASE+0x0008)
-#define MTK_WDT_STATUS                      (MTK_WDT_BASE+0x000C)
-#define MTK_WDT_INTERVAL                    (MTK_WDT_BASE+0x0010)
-#define MTK_WDT_SWRST                       (MTK_WDT_BASE+0x0014)
-#define MTK_WDT_SWSYSRST                    (MTK_WDT_BASE+0x0018)
-#define MTK_WDT_NONRST_REG                  (MTK_WDT_BASE+0x0020)
-#define MTK_WDT_NONRST_REG2                 (MTK_WDT_BASE+0x0024)
-#define MTK_WDT_REQ_MODE                    (MTK_WDT_BASE+0x0030)
-#define MTK_WDT_REQ_IRQ_EN                  (MTK_WDT_BASE+0x0034)
-#define MTK_WDT_DEBUG_CTL                   (MTK_WDT_BASE+0x0040)
-
-/*WDT_STATUS*/
-#define MTK_WDT_STATUS_HWWDT_RST            (0x80000000)
-#define MTK_WDT_STATUS_SWWDT_RST            (0x40000000)
-#define MTK_WDT_STATUS_IRQWDT_RST           (0x20000000)
-#define MTK_WDT_STATUS_DEBUGWDT_RST         (0x00080000)
-#define MTK_WDT_STATUS_SPMWDT_RST           (0x0002)
-#define MTK_WDT_STATUS_SPM_THERMAL_RST      (0x0001)
-#define MTK_WDT_STATUS_THERMAL_DIRECT_RST   (1<<18)
-#define MTK_WDT_STATUS_SECURITY_RST         (1<<28)
-
-#define MTK_WDT_MODE_DUAL_MODE              0x0040
-#define MTK_WDT_MODE_IRQ                    0x0008
-#define MTK_WDT_MODE_KEY                    0x22000000
-#define MTK_WDT_MODE_EXTEN                  0x0004
-#define MTK_WDT_SWRST_KEY                   0x1209
-#define MTK_WDT_RESTART_KEY                 (0x1971)
-
-/* FIQ platform related define */
-#define MT_IRQ_SEC_SGI_0  8
-#define MT_IRQ_SEC_SGI_1  9
-#define MT_IRQ_SEC_SGI_2  10
-#define MT_IRQ_SEC_SGI_3  11
-#define MT_IRQ_SEC_SGI_4  12
-#define MT_IRQ_SEC_SGI_5  13
-#define MT_IRQ_SEC_SGI_6  14
-#define MT_IRQ_SEC_SGI_7  15
-
-#define FIQ_SMP_CALL_SGI  MT_IRQ_SEC_SGI_5
-
-/*******************************************************************************
- * Platform binary types for linking
- ******************************************************************************/
-#define PLATFORM_LINKER_FORMAT    "elf64-littleaarch64"
-#define PLATFORM_LINKER_ARCH      aarch64
-
-/*******************************************************************************
- * Generic platform constants
- ******************************************************************************/
-
-/* Size of cacheable stacks */
-#if defined(IMAGE_BL1)
-#define PLATFORM_STACK_SIZE 0x440
-#elif defined(IMAGE_BL2)
-#define PLATFORM_STACK_SIZE 0x400
-#elif defined(IMAGE_BL31)
-#define PLATFORM_STACK_SIZE 0x800
-#elif defined(IMAGE_BL32)
-#define PLATFORM_STACK_SIZE 0x440
-#endif
-
-#define FIRMWARE_WELCOME_STR    "Booting Trusted Firmware\n"
-#define PLAT_MAX_PWR_LVL        U(2) /* MPIDR_AFFLVL2 */
-
-#define PLAT_MAX_RET_STATE	U(1)
-#define PLAT_MAX_OFF_STATE	U(2)
-
-#define PLATFORM_CACHE_LINE_SIZE      64
-#define PLATFORM_SYSTEM_COUNT         U(1)
-#define PLATFORM_CLUSTER_COUNT        U(2)
-#define PLATFORM_CLUSTER0_CORE_COUNT  U(4)
-#define PLATFORM_CLUSTER1_CORE_COUNT  U(4)
-#define PLATFORM_CORE_COUNT   (PLATFORM_CLUSTER1_CORE_COUNT + \
-					PLATFORM_CLUSTER0_CORE_COUNT)
-#define PLATFORM_MAX_CPUS_PER_CLUSTER U(4)
-#define PLATFORM_NUM_AFFS   (PLATFORM_SYSTEM_COUNT +  \
-					PLATFORM_CLUSTER_COUNT + \
-					PLATFORM_CORE_COUNT)
-
-/*******************************************************************************
- * Platform memory map related constants
- ******************************************************************************/
-/* ATF Argument */
-#define ATF_ARG_SIZE      (0x800)
-
-/* TF txet, ro, rw, internal SRAM, Size: release: 80KB, debug: 92KB */
-#define TZRAM_BASE        (0x110000)
-#if DEBUG
-#define TZRAM_SIZE        (0x1C400)
-#else
-#define TZRAM_SIZE        (0x1C400)
-#endif
-#define TZRAM2_BASE	   0x00100000
-#define TZRAM2_SIZE	   0xDC00
-#define TZRAM2_LIMIT		(TZRAM2_BASE + TZRAM2_SIZE)
-
-#define RAM_CONSOLE_BASE  0x0012D000
-#define RAM_CONSOLE_SIZE  0x00001000
-/*******************************************************************************
- * BL31 specific defines.
- ******************************************************************************/
-/*
- * Put BL3-1 at the top of the Trusted SRAM (just below the shared memory, if
- * present). BL31_BASE is calculated using the current BL3-1 debug size plus a
- * little space for growth.
- */
-#define BL31_BASE           (TZRAM_BASE + 0x1000)
-#define BL31_LIMIT          (TZRAM_BASE + TZRAM_SIZE)
-#define BSS1_STACK_LIMIT    (TZRAM_BASE + TZRAM_SIZE)
-#define BL31_TZRAM_SIZE     (TZRAM_SIZE - ATF_ARG_SIZE)
-
-/*******************************************************************************
- * Platform specific page table and MMU setup constants
- ******************************************************************************/
-#define PLAT_VIRT_ADDR_SPACE_SIZE   (1ULL << 32)
-#define PLAT_PHY_ADDR_SPACE_SIZE    (1ULL << 32)
-#define MAX_XLAT_TABLES   7
-#define MAX_MMAP_REGIONS  16
-
-
-/*******************************************************************************
- * CCI-400 related constants
- ******************************************************************************/
-#define CCI400_BASE                     0x10390000
-#define CCI400_SL_IFACE_CLUSTER0        4
-#define CCI400_SL_IFACE_CLUSTER1        3
-#define CCI400_SL_IFACE_INDEX(mpidr)  (mpidr & MPIDR_CLUSTER_MASK ? \
-					CCI400_SL_IFACE_CLUSTER1 :   \
-					CCI400_SL_IFACE_CLUSTER0)
-#define CCI_SEC_ACCESS_OFFSET           (0x8)
-
-
-/*******************************************************************************
- * Declarations and constants to access the mailboxes safely. Each mailbox is
- * aligned on the biggest cache line size in the platform. This is known only
- * to the platform as it might have a combination of integrated and external
- * caches. Such alignment ensures that two maiboxes do not sit on the same cache
- * line at any cache level. They could belong to different cpus/clusters &
- * get written while being protected by different locks causing corruption of
- * a valid mailbox address.
- ******************************************************************************/
-#define CACHE_WRITEBACK_SHIFT     6
-#define CACHE_WRITEBACK_GRANULE   (1 << CACHE_WRITEBACK_SHIFT)
-
-#define BL32_BASE                 (0x0)
-
-/*
- * Load address of BL3-3 for this platform port
- */
-#define LK_SIZE_LIMIT				(0x100000)
-#define PLAT_MTK_NS_IMAGE_OFFSET	(0x41E00000)
-/* 16KB */
-#define ATF_AEE_BUFFER_SIZE         (0x4000)
-#define PAGE_SIZE_2MB_MASK          (PAGE_SIZE_2MB - 1)
-#define IS_PAGE_2MB_ALIGNED(addr)   (((addr) & PAGE_SIZE_2MB_MASK) == 0)
-#define PAGE_SIZE_2MB               (1 << PAGE_SIZE_2MB_SHIFT)
-#define PAGE_SIZE_2MB_SHIFT         TWO_MB_SHIFT
-
-#endif /* PLATFORM_DEF_H */

diff --git a/plat/mediatek/mt6795/include/power_tracer.h b/plat/mediatek/mt6795/include/power_tracer.h
deleted file mode 100644
index 8c98dbd..0000000
--- a/plat/mediatek/mt6795/include/power_tracer.h
+++ /dev/null

@@ -1,19 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef POWER_TRACER_H
-#define POWER_TRACER_H
-
-#define CPU_UP		0
-#define CPU_DOWN	1
-#define CPU_SUSPEND	2
-#define CLUSTER_UP	3
-#define CLUSTER_DOWN	4
-#define CLUSTER_SUSPEND	5
-
-void trace_power_flow(unsigned long mpidr, unsigned char mode);
-
-#endif /* POWER_TRACER_H */

diff --git a/plat/mediatek/mt6795/include/scu.h b/plat/mediatek/mt6795/include/scu.h
deleted file mode 100644
index 625418a..0000000
--- a/plat/mediatek/mt6795/include/scu.h
+++ /dev/null

@@ -1,13 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef SCU_H
-#define SCU_H
-
-void disable_scu(unsigned long mpidr);
-void enable_scu(unsigned long mpidr);
-
-#endif /* SCU_H */

diff --git a/plat/mediatek/mt6795/include/spm.h b/plat/mediatek/mt6795/include/spm.h
deleted file mode 100644
index 5227e1d..0000000
--- a/plat/mediatek/mt6795/include/spm.h
+++ /dev/null

@@ -1,198 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef SPM_H
-#define SPM_H
-
-#define SPM_POWERON_CONFIG_SET			(SPM_BASE + 0x000)
-#define SPM_POWER_ON_VAL0			(SPM_BASE + 0x010)
-#define SPM_POWER_ON_VAL1			(SPM_BASE + 0x014)
-#define SPM_CLK_SETTLE				(SPM_BASE + 0x100)
-#define SPM_CA7_CPU1_PWR_CON			(SPM_BASE + 0x218)
-#define SPM_CA7_CPU2_PWR_CON			(SPM_BASE + 0x21c)
-#define SPM_CA7_CPU3_PWR_CON			(SPM_BASE + 0x220)
-#define SPM_CA7_CPU1_L1_PDN			(SPM_BASE + 0x264)
-#define SPM_CA7_CPU2_L1_PDN			(SPM_BASE + 0x26c)
-#define SPM_CA7_CPU3_L1_PDN			(SPM_BASE + 0x274)
-#define SPM_MD32_SRAM_CON			(SPM_BASE + 0x2c8)
-#define SPM_PCM_CON0				(SPM_BASE + 0x310)
-#define SPM_PCM_CON1				(SPM_BASE + 0x314)
-#define SPM_PCM_IM_PTR				(SPM_BASE + 0x318)
-#define SPM_PCM_IM_LEN				(SPM_BASE + 0x31c)
-#define SPM_PCM_REG_DATA_INI			(SPM_BASE + 0x320)
-#define SPM_PCM_EVENT_VECTOR0			(SPM_BASE + 0x340)
-#define SPM_PCM_EVENT_VECTOR1			(SPM_BASE + 0x344)
-#define SPM_PCM_EVENT_VECTOR2			(SPM_BASE + 0x348)
-#define SPM_PCM_EVENT_VECTOR3			(SPM_BASE + 0x34c)
-#define SPM_PCM_MAS_PAUSE_MASK			(SPM_BASE + 0x354)
-#define SPM_PCM_PWR_IO_EN			(SPM_BASE + 0x358)
-#define SPM_PCM_TIMER_VAL			(SPM_BASE + 0x35c)
-#define SPM_PCM_TIMER_OUT			(SPM_BASE + 0x360)
-#define SPM_PCM_REG0_DATA			(SPM_BASE + 0x380)
-#define SPM_PCM_REG1_DATA			(SPM_BASE + 0x384)
-#define SPM_PCM_REG2_DATA			(SPM_BASE + 0x388)
-#define SPM_PCM_REG3_DATA			(SPM_BASE + 0x38c)
-#define SPM_PCM_REG4_DATA			(SPM_BASE + 0x390)
-#define SPM_PCM_REG5_DATA			(SPM_BASE + 0x394)
-#define SPM_PCM_REG6_DATA			(SPM_BASE + 0x398)
-#define SPM_PCM_REG7_DATA			(SPM_BASE + 0x39c)
-#define SPM_PCM_REG8_DATA			(SPM_BASE + 0x3a0)
-#define SPM_PCM_REG9_DATA			(SPM_BASE + 0x3a4)
-#define SPM_PCM_REG10_DATA			(SPM_BASE + 0x3a8)
-#define SPM_PCM_REG11_DATA			(SPM_BASE + 0x3ac)
-#define SPM_PCM_REG12_DATA			(SPM_BASE + 0x3b0)
-#define SPM_PCM_REG13_DATA			(SPM_BASE + 0x3b4)
-#define SPM_PCM_REG14_DATA			(SPM_BASE + 0x3b8)
-#define SPM_PCM_REG15_DATA			(SPM_BASE + 0x3bc)
-#define SPM_PCM_EVENT_REG_STA			(SPM_BASE + 0x3c0)
-#define SPM_PCM_FSM_STA				(SPM_BASE + 0x3c4)
-#define SPM_PCM_IM_HOST_RW_PTR			(SPM_BASE + 0x3c8)
-#define SPM_PCM_IM_HOST_RW_DAT			(SPM_BASE + 0x3cc)
-#define SPM_PCM_EVENT_VECTOR4			(SPM_BASE + 0x3d0)
-#define SPM_PCM_EVENT_VECTOR5			(SPM_BASE + 0x3d4)
-#define SPM_PCM_EVENT_VECTOR6			(SPM_BASE + 0x3d8)
-#define SPM_PCM_EVENT_VECTOR7			(SPM_BASE + 0x3dc)
-#define SPM_PCM_SW_INT_SET			(SPM_BASE + 0x3e0)
-#define SPM_PCM_SW_INT_CLEAR			(SPM_BASE + 0x3e4)
-#define SPM_CLK_CON				(SPM_BASE + 0x400)
-#define SPM_SLEEP_PTPOD2_CON			(SPM_BASE + 0x408)
-#define SPM_APMCU_PWRCTL			(SPM_BASE + 0x600)
-#define SPM_AP_DVFS_CON_SET			(SPM_BASE + 0x604)
-#define SPM_AP_STANBY_CON			(SPM_BASE + 0x608)
-#define SPM_PWR_STATUS				(SPM_BASE + 0x60c)
-#define SPM_PWR_STATUS_2ND			(SPM_BASE + 0x610)
-#define SPM_AP_BSI_REQ				(SPM_BASE + 0x614)
-#define SPM_SLEEP_TIMER_STA			(SPM_BASE + 0x720)
-#define SPM_SLEEP_WAKEUP_EVENT_MASK		(SPM_BASE + 0x810)
-#define SPM_SLEEP_CPU_WAKEUP_EVENT		(SPM_BASE + 0x814)
-#define SPM_SLEEP_MD32_WAKEUP_EVENT_MASK	(SPM_BASE + 0x818)
-#define SPM_PCM_WDT_TIMER_VAL			(SPM_BASE + 0x824)
-#define SPM_PCM_WDT_TIMER_OUT			(SPM_BASE + 0x828)
-#define SPM_PCM_MD32_MAILBOX			(SPM_BASE + 0x830)
-#define SPM_PCM_MD32_IRQ			(SPM_BASE + 0x834)
-#define SPM_SLEEP_ISR_MASK			(SPM_BASE + 0x900)
-#define SPM_SLEEP_ISR_STATUS			(SPM_BASE + 0x904)
-#define SPM_SLEEP_ISR_RAW_STA			(SPM_BASE + 0x910)
-#define SPM_SLEEP_MD32_ISR_RAW_STA		(SPM_BASE + 0x914)
-#define SPM_SLEEP_WAKEUP_MISC			(SPM_BASE + 0x918)
-#define SPM_SLEEP_BUS_PROTECT_RDY		(SPM_BASE + 0x91c)
-#define SPM_SLEEP_SUBSYS_IDLE_STA		(SPM_BASE + 0x920)
-#define SPM_PCM_RESERVE				(SPM_BASE + 0xb00)
-#define SPM_PCM_RESERVE2			(SPM_BASE + 0xb04)
-#define SPM_PCM_FLAGS				(SPM_BASE + 0xb08)
-#define SPM_PCM_SRC_REQ				(SPM_BASE + 0xb0c)
-#define SPM_PCM_DEBUG_CON			(SPM_BASE + 0xb20)
-#define SPM_CA7_CPU0_IRQ_MASK			(SPM_BASE + 0xb30)
-#define SPM_CA7_CPU1_IRQ_MASK			(SPM_BASE + 0xb34)
-#define SPM_CA7_CPU2_IRQ_MASK			(SPM_BASE + 0xb38)
-#define SPM_CA7_CPU3_IRQ_MASK			(SPM_BASE + 0xb3c)
-#define SPM_CA15_CPU0_IRQ_MASK			(SPM_BASE + 0xb40)
-#define SPM_CA15_CPU1_IRQ_MASK			(SPM_BASE + 0xb44)
-#define SPM_CA15_CPU2_IRQ_MASK			(SPM_BASE + 0xb48)
-#define SPM_CA15_CPU3_IRQ_MASK			(SPM_BASE + 0xb4c)
-#define SPM_PCM_PASR_DPD_0			(SPM_BASE + 0xb60)
-#define SPM_PCM_PASR_DPD_1			(SPM_BASE + 0xb64)
-#define SPM_PCM_PASR_DPD_2			(SPM_BASE + 0xb68)
-#define SPM_PCM_PASR_DPD_3			(SPM_BASE + 0xb6c)
-#define SPM_SLEEP_CA7_WFI0_EN			(SPM_BASE + 0xf00)
-#define SPM_SLEEP_CA7_WFI1_EN			(SPM_BASE + 0xf04)
-#define SPM_SLEEP_CA7_WFI2_EN			(SPM_BASE + 0xf08)
-#define SPM_SLEEP_CA7_WFI3_EN			(SPM_BASE + 0xf0c)
-#define SPM_SLEEP_CA15_WFI0_EN			(SPM_BASE + 0xf10)
-#define SPM_SLEEP_CA15_WFI1_EN			(SPM_BASE + 0xf14)
-#define SPM_SLEEP_CA15_WFI2_EN			(SPM_BASE + 0xf18)
-#define SPM_SLEEP_CA15_WFI3_EN			(SPM_BASE + 0xf1c)
-
-#define SPM_PROJECT_CODE	0xb16
-
-#define SPM_REGWR_EN		(1U << 0)
-#define SPM_REGWR_CFG_KEY	(SPM_PROJECT_CODE << 16)
-
-#define SPM_CPU_PDN_DIS		(1U << 0)
-#define SPM_INFRA_PDN_DIS	(1U << 1)
-#define SPM_DDRPHY_PDN_DIS	(1U << 2)
-#define SPM_DUALVCORE_PDN_DIS	(1U << 3)
-#define SPM_PASR_DIS		(1U << 4)
-#define SPM_DPD_DIS		(1U << 5)
-#define SPM_SODI_DIS		(1U << 6)
-#define SPM_MEMPLL_RESET	(1U << 7)
-#define SPM_MAINPLL_PDN_DIS	(1U << 8)
-#define SPM_CPU_DVS_DIS		(1U << 9)
-#define SPM_CPU_DORMANT		(1U << 10)
-#define SPM_EXT_VSEL_GPIO103	(1U << 11)
-#define SPM_DDR_HIGH_SPEED	(1U << 12)
-#define SPM_OPT			(1U << 13)
-
-#define POWER_ON_VAL1_DEF	0x01011820
-#define PCM_FSM_STA_DEF		0x48490
-#define PCM_END_FSM_STA_DEF	0x08490
-#define PCM_END_FSM_STA_MASK	0x3fff0
-#define PCM_HANDSHAKE_SEND1	0xbeefbeef
-
-#define PCM_WDT_TIMEOUT		(30 * 32768)
-#define PCM_TIMER_MAX		(0xffffffff - PCM_WDT_TIMEOUT)
-
-#define CON0_PCM_KICK		(1U << 0)
-#define CON0_IM_KICK		(1U << 1)
-#define CON0_IM_SLEEP_DVS	(1U << 3)
-#define CON0_PCM_SW_RESET	(1U << 15)
-#define CON0_CFG_KEY		(SPM_PROJECT_CODE << 16)
-
-#define CON1_IM_SLAVE		(1U << 0)
-#define CON1_MIF_APBEN		(1U << 3)
-#define CON1_PCM_TIMER_EN	(1U << 5)
-#define CON1_IM_NONRP_EN	(1U << 6)
-#define CON1_PCM_WDT_EN		(1U << 8)
-#define CON1_PCM_WDT_WAKE_MODE	(1U << 9)
-#define CON1_SPM_SRAM_SLP_B	(1U << 10)
-#define CON1_SPM_SRAM_ISO_B	(1U << 11)
-#define CON1_EVENT_LOCK_EN	(1U << 12)
-#define CON1_CFG_KEY		(SPM_PROJECT_CODE << 16)
-
-#define PCM_PWRIO_EN_R0		(1U << 0)
-#define PCM_PWRIO_EN_R7		(1U << 7)
-#define PCM_RF_SYNC_R0		(1U << 16)
-#define PCM_RF_SYNC_R2		(1U << 18)
-#define PCM_RF_SYNC_R6		(1U << 22)
-#define PCM_RF_SYNC_R7		(1U << 23)
-
-#define CC_SYSCLK0_EN_0		(1U << 0)
-#define CC_SYSCLK0_EN_1		(1U << 1)
-#define CC_SYSCLK1_EN_0		(1U << 2)
-#define CC_SYSCLK1_EN_1		(1U << 3)
-#define CC_SYSSETTLE_SEL	(1U << 4)
-#define CC_LOCK_INFRA_DCM	(1U << 5)
-#define CC_SRCLKENA_MASK_0	(1U << 6)
-#define CC_CXO32K_RM_EN_MD1	(1U << 9)
-#define CC_CXO32K_RM_EN_MD2	(1U << 10)
-#define CC_CLKSQ1_SEL		(1U << 12)
-#define CC_DISABLE_DORM_PWR	(1U << 14)
-#define CC_MD32_DCM_EN		(1U << 18)
-
-#define WFI_OP_AND		1
-#define WFI_OP_OR		0
-
-#define WAKE_MISC_PCM_TIMER	(1U << 19)
-#define WAKE_MISC_CPU_WAKE	(1U << 20)
-
-/* define WAKE_SRC_XXX */
-#define WAKE_SRC_SPM_MERGE	(1 << 0)
-#define WAKE_SRC_KP		(1 << 2)
-#define WAKE_SRC_WDT		(1 << 3)
-#define WAKE_SRC_GPT		(1 << 4)
-#define WAKE_SRC_EINT		(1 << 6)
-#define WAKE_SRC_LOW_BAT	(1 << 9)
-#define WAKE_SRC_MD32		(1 << 10)
-#define WAKE_SRC_USB_CD		(1 << 14)
-#define WAKE_SRC_USB_PDN	(1 << 15)
-#define WAKE_SRC_AFE		(1 << 20)
-#define WAKE_SRC_THERM		(1 << 21)
-#define WAKE_SRC_SYSPWREQ	(1 << 24)
-#define WAKE_SRC_SEJ		(1 << 27)
-#define WAKE_SRC_ALL_MD32	(1 << 28)
-#define WAKE_SRC_CPU_IRQ	(1 << 29)
-
-#endif /* SPM_H */

diff --git a/plat/mediatek/mt6795/plat_delay_timer.c b/plat/mediatek/mt6795/plat_delay_timer.c
deleted file mode 100644
index 965b653..0000000
--- a/plat/mediatek/mt6795/plat_delay_timer.c
+++ /dev/null

@@ -1,31 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <platform_def.h>
-
-#include <arch_helpers.h>
-#include <drivers/delay_timer.h>
-
-static uint32_t plat_get_timer_value(void)
-{
-	/*
-	 * Generic delay timer implementation expects the timer to be a down
-	 * counter. We apply bitwise NOT operator to the tick values returned
-	 * by read_cntpct_el0() to simulate the down counter.
-	 */
-	return (uint32_t)(~read_cntpct_el0());
-}
-
-static const timer_ops_t plat_timer_ops = {
-	.get_timer_value	= plat_get_timer_value,
-	.clk_mult		= 1,
-	.clk_div		= SYS_COUNTER_FREQ_IN_MHZ,
-};
-
-void plat_delay_timer_init(void)
-{
-	timer_init(&plat_timer_ops);
-}

diff --git a/plat/mediatek/mt6795/plat_mt_gic.c b/plat/mediatek/mt6795/plat_mt_gic.c
deleted file mode 100644
index 20cb26d..0000000
--- a/plat/mediatek/mt6795/plat_mt_gic.c
+++ /dev/null

@@ -1,50 +0,0 @@
-/*
- * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <platform_def.h>
-
-#include <common/interrupt_props.h>
-#include <drivers/arm/gicv2.h>
-#include <plat/common/platform.h>
-
-static const interrupt_prop_t g0_interrupt_props[] = {
-	INTR_PROP_DESC(FIQ_SMP_CALL_SGI, GIC_HIGHEST_SEC_PRIORITY,
-		       GICV2_INTR_GROUP0, GIC_INTR_CFG_LEVEL),
-};
-
-gicv2_driver_data_t arm_gic_data = {
-	.gicd_base = BASE_GICD_BASE,
-	.gicc_base = BASE_GICC_BASE,
-	.interrupt_props = g0_interrupt_props,
-	.interrupt_props_num = ARRAY_SIZE(g0_interrupt_props),
-};
-
-void plat_mt_gic_driver_init(void)
-{
-	gicv2_driver_init(&arm_gic_data);
-}
-
-void plat_mt_gic_init(void)
-{
-	gicv2_distif_init();
-	gicv2_pcpu_distif_init();
-	gicv2_cpuif_enable();
-}
-
-void plat_mt_gic_cpuif_enable(void)
-{
-	gicv2_cpuif_enable();
-}
-
-void plat_mt_gic_cpuif_disable(void)
-{
-	gicv2_cpuif_disable();
-}
-
-void plat_mt_gic_pcpu_init(void)
-{
-	gicv2_pcpu_distif_init();
-}

diff --git a/plat/mediatek/mt6795/plat_pm.c b/plat/mediatek/mt6795/plat_pm.c
deleted file mode 100644
index 0dfbd18..0000000
--- a/plat/mediatek/mt6795/plat_pm.c
+++ /dev/null

@@ -1,473 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <assert.h>
-#include <errno.h>
-
-#include <platform_def.h>
-
-#include <arch_helpers.h>
-#include <common/debug.h>
-#include <drivers/arm/cci.h>
-#include <drivers/console.h>
-#include <lib/bakery_lock.h>
-#include <lib/mmio.h>
-#include <lib/psci/psci.h>
-
-#include <mcucfg.h>
-#include <plat_private.h>
-#include <power_tracer.h>
-#include <scu.h>
-
-struct core_context {
-	unsigned long timer_data[8];
-	unsigned int count;
-	unsigned int rst;
-	unsigned int abt;
-	unsigned int brk;
-};
-
-struct cluster_context {
-	struct core_context core[PLATFORM_MAX_CPUS_PER_CLUSTER];
-};
-
-/*
- * Top level structure to hold the complete context of a multi cluster system
- */
-struct system_context {
-	struct cluster_context cluster[PLATFORM_CLUSTER_COUNT];
-};
-
-/*
- * Top level structure which encapsulates the context of the entire system
- */
-static struct system_context dormant_data[1];
-
-static inline struct cluster_context *system_cluster(
-						struct system_context *system,
-						uint32_t clusterid)
-{
-	return &system->cluster[clusterid];
-}
-
-static inline struct core_context *cluster_core(struct cluster_context *cluster,
-						uint32_t cpuid)
-{
-	return &cluster->core[cpuid];
-}
-
-static struct cluster_context *get_cluster_data(unsigned long mpidr)
-{
-	uint32_t clusterid;
-
-	clusterid = (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS;
-
-	return system_cluster(dormant_data, clusterid);
-}
-
-static struct core_context *get_core_data(unsigned long mpidr)
-{
-	struct cluster_context *cluster;
-	uint32_t cpuid;
-
-	cluster = get_cluster_data(mpidr);
-	cpuid = mpidr & MPIDR_CPU_MASK;
-
-	return cluster_core(cluster, cpuid);
-}
-
-static void mt_save_generic_timer(unsigned long *container)
-{
-	uint64_t ctl;
-	uint64_t val;
-
-	__asm__ volatile("mrs	%x0, cntkctl_el1\n\t"
-			 "mrs	%x1, cntp_cval_el0\n\t"
-			 "stp	%x0, %x1, [%2, #0]"
-			 : "=&r" (ctl), "=&r" (val)
-			 : "r" (container)
-			 : "memory");
-
-	__asm__ volatile("mrs	%x0, cntp_tval_el0\n\t"
-			 "mrs	%x1, cntp_ctl_el0\n\t"
-			 "stp	%x0, %x1, [%2, #16]"
-			 : "=&r" (val), "=&r" (ctl)
-			 : "r" (container)
-			 : "memory");
-
-	__asm__ volatile("mrs	%x0, cntv_tval_el0\n\t"
-			 "mrs	%x1, cntv_ctl_el0\n\t"
-			 "stp	%x0, %x1, [%2, #32]"
-			 : "=&r" (val), "=&r" (ctl)
-			 : "r" (container)
-			 : "memory");
-}
-
-static void mt_restore_generic_timer(unsigned long *container)
-{
-	uint64_t ctl;
-	uint64_t val;
-
-	__asm__ volatile("ldp	%x0, %x1, [%2, #0]\n\t"
-			 "msr	cntkctl_el1, %x0\n\t"
-			 "msr	cntp_cval_el0, %x1"
-			 : "=&r" (ctl), "=&r" (val)
-			 : "r" (container)
-			 : "memory");
-
-	__asm__ volatile("ldp	%x0, %x1, [%2, #16]\n\t"
-			 "msr	cntp_tval_el0, %x0\n\t"
-			 "msr	cntp_ctl_el0, %x1"
-			 : "=&r" (val), "=&r" (ctl)
-			 : "r" (container)
-			 : "memory");
-
-	__asm__ volatile("ldp	%x0, %x1, [%2, #32]\n\t"
-			 "msr	cntv_tval_el0, %x0\n\t"
-			 "msr	cntv_ctl_el0, %x1"
-			 : "=&r" (val), "=&r" (ctl)
-			 : "r" (container)
-			 : "memory");
-}
-
-static void stop_generic_timer(void)
-{
-	/*
-	 * Disable the timer and mask the irq to prevent
-	 * suprious interrupts on this cpu interface. It
-	 * will bite us when we come back if we don't. It
-	 * will be replayed on the inbound cluster.
-	 */
-	uint64_t cntpctl = read_cntp_ctl_el0();
-
-	write_cntp_ctl_el0(clr_cntp_ctl_enable(cntpctl));
-}
-
-static void mt_cpu_save(unsigned long mpidr)
-{
-	struct core_context *core;
-
-	core = get_core_data(mpidr);
-	mt_save_generic_timer(core->timer_data);
-
-	/* disable timer irq, and upper layer should enable it again. */
-	stop_generic_timer();
-}
-
-static void mt_cpu_restore(unsigned long mpidr)
-{
-	struct core_context *core;
-
-	core = get_core_data(mpidr);
-	mt_restore_generic_timer(core->timer_data);
-}
-
-static void mt_platform_save_context(unsigned long mpidr)
-{
-	/* mcusys_save_context: */
-	mt_cpu_save(mpidr);
-}
-
-static void mt_platform_restore_context(unsigned long mpidr)
-{
-	/* mcusys_restore_context: */
-	mt_cpu_restore(mpidr);
-}
-
-/*******************************************************************************
-* Private function which is used to determine if any platform actions
-* should be performed for the specified affinity instance given its
-* state. Nothing needs to be done if the 'state' is not off or if this is not
-* the highest affinity level which will enter the 'state'.
-*******************************************************************************/
-static int32_t plat_do_plat_actions(unsigned int afflvl, unsigned int state)
-{
-	unsigned int max_phys_off_afflvl;
-
-	assert(afflvl <= MPIDR_AFFLVL2);
-
-	if (state != PSCI_STATE_OFF)
-		return -EAGAIN;
-
-	/*
-	 * Find the highest affinity level which will be suspended and postpone
-	 * all the platform specific actions until that level is hit.
-	 */
-	max_phys_off_afflvl = psci_get_max_phys_off_afflvl();
-	assert(max_phys_off_afflvl != PSCI_INVALID_DATA);
-	if (afflvl != max_phys_off_afflvl)
-		return -EAGAIN;
-
-	return 0;
-}
-
-/*******************************************************************************
- * MTK_platform handler called when an affinity instance is about to enter
- * standby.
- ******************************************************************************/
-static void plat_affinst_standby(unsigned int power_state)
-{
-	unsigned int target_afflvl;
-
-	/* Sanity check the requested state */
-	target_afflvl = psci_get_pstate_afflvl(power_state);
-
-	/*
-	 * It's possible to enter standby only on affinity level 0 i.e. a cpu
-	 * on the MTK_platform. Ignore any other affinity level.
-	 */
-	if (target_afflvl == MPIDR_AFFLVL0) {
-		/*
-		 * Enter standby state. dsb is good practice before using wfi
-		 * to enter low power states.
-		 */
-		dsb();
-		wfi();
-	}
-}
-
-/*******************************************************************************
- * MTK_platform handler called when an affinity instance is about to be turned
- * on. The level and mpidr determine the affinity instance.
- ******************************************************************************/
-static int plat_affinst_on(unsigned long mpidr,
-		    unsigned long sec_entrypoint,
-		    unsigned int afflvl,
-		    unsigned int state)
-{
-	int rc = PSCI_E_SUCCESS;
-	unsigned long cpu_id;
-	unsigned long cluster_id;
-	uintptr_t rv;
-
-	/*
-	 * It's possible to turn on only affinity level 0 i.e. a cpu
-	 * on the MTK_platform. Ignore any other affinity level.
-	 */
-	if (afflvl != MPIDR_AFFLVL0)
-		return rc;
-
-	cpu_id = mpidr & MPIDR_CPU_MASK;
-	cluster_id = mpidr & MPIDR_CLUSTER_MASK;
-
-	if (cluster_id)
-		rv = (uintptr_t)&mt6795_mcucfg->mp1_rv_addr[cpu_id].rv_addr_lw;
-	else
-		rv = (uintptr_t)&mt6795_mcucfg->mp0_rv_addr[cpu_id].rv_addr_lw;
-
-	mmio_write_32(rv, sec_entrypoint);
-	INFO("mt_on[%ld:%ld], entry %x\n",
-		cluster_id, cpu_id, mmio_read_32(rv));
-
-	return rc;
-}
-
-/*******************************************************************************
- * MTK_platform handler called when an affinity instance is about to be turned
- * off. The level and mpidr determine the affinity instance. The 'state' arg.
- * allows the platform to decide whether the cluster is being turned off and
- * take apt actions.
- *
- * CAUTION: This function is called with coherent stacks so that caches can be
- * turned off, flushed and coherency disabled. There is no guarantee that caches
- * will remain turned on across calls to this function as each affinity level is
- * dealt with. So do not write & read global variables across calls. It will be
- * wise to do flush a write to the global to prevent unpredictable results.
- ******************************************************************************/
-static void plat_affinst_off(unsigned int afflvl, unsigned int state)
-{
-	unsigned long mpidr = read_mpidr_el1();
-
-	/* Determine if any platform actions need to be executed. */
-	if (plat_do_plat_actions(afflvl, state) == -EAGAIN)
-		return;
-
-	/* Prevent interrupts from spuriously waking up this cpu */
-	plat_mt_gic_cpuif_disable();
-
-	trace_power_flow(mpidr, CPU_DOWN);
-
-	if (afflvl != MPIDR_AFFLVL0) {
-		/* Disable coherency if this cluster is to be turned off */
-		plat_cci_disable();
-
-		trace_power_flow(mpidr, CLUSTER_DOWN);
-	}
-}
-
-/*******************************************************************************
- * MTK_platform handler called when an affinity instance is about to be
- * suspended. The level and mpidr determine the affinity instance. The 'state'
- * arg. allows the platform to decide whether the cluster is being turned off
- * and take apt actions.
- *
- * CAUTION: This function is called with coherent stacks so that caches can be
- * turned off, flushed and coherency disabled. There is no guarantee that caches
- * will remain turned on across calls to this function as each affinity level is
- * dealt with. So do not write & read global variables across calls. It will be
- * wise to do flush a write to the global to prevent unpredictable results.
- ******************************************************************************/
-static void plat_affinst_suspend(unsigned long sec_entrypoint,
-			  unsigned int afflvl,
-			  unsigned int state)
-{
-	unsigned long mpidr = read_mpidr_el1();
-	unsigned long cluster_id;
-	unsigned long cpu_id;
-	uintptr_t rv;
-
-	/* Determine if any platform actions need to be executed. */
-	if (plat_do_plat_actions(afflvl, state) == -EAGAIN)
-		return;
-
-	cpu_id = mpidr & MPIDR_CPU_MASK;
-	cluster_id = mpidr & MPIDR_CLUSTER_MASK;
-
-	if (cluster_id)
-		rv = (uintptr_t)&mt6795_mcucfg->mp1_rv_addr[cpu_id].rv_addr_lw;
-	else
-		rv = (uintptr_t)&mt6795_mcucfg->mp0_rv_addr[cpu_id].rv_addr_lw;
-
-	mmio_write_32(rv, sec_entrypoint);
-
-	if (afflvl >= MPIDR_AFFLVL0)
-		mt_platform_save_context(mpidr);
-
-	/* Perform the common cluster specific operations */
-	if (afflvl >= MPIDR_AFFLVL1) {
-		/* Disable coherency if this cluster is to be turned off */
-		plat_cci_disable();
-		disable_scu(mpidr);
-
-		trace_power_flow(mpidr, CLUSTER_SUSPEND);
-	}
-
-	if (afflvl >= MPIDR_AFFLVL2) {
-		/* Prevent interrupts from spuriously waking up this cpu */
-		plat_mt_gic_cpuif_disable();
-	}
-}
-
-/*******************************************************************************
- * MTK_platform handler called when an affinity instance has just been powered
- * on after being turned off earlier. The level and mpidr determine the affinity
- * instance. The 'state' arg. allows the platform to decide whether the cluster
- * was turned off prior to wakeup and do what's necessary to setup it up
- * correctly.
- ******************************************************************************/
-static void plat_affinst_on_finish(unsigned int afflvl, unsigned int state)
-{
-	unsigned long mpidr = read_mpidr_el1();
-
-	/* Determine if any platform actions need to be executed. */
-	if (plat_do_plat_actions(afflvl, state) == -EAGAIN)
-		return;
-
-	/* Perform the common cluster specific operations */
-	if (afflvl >= MPIDR_AFFLVL1) {
-		enable_scu(mpidr);
-
-		/* Enable coherency if this cluster was off */
-		plat_cci_enable();
-		trace_power_flow(mpidr, CLUSTER_UP);
-	}
-
-	/* Enable the gic cpu interface */
-	plat_mt_gic_cpuif_enable();
-	plat_mt_gic_pcpu_init();
-	trace_power_flow(mpidr, CPU_UP);
-}
-
-/*******************************************************************************
- * MTK_platform handler called when an affinity instance has just been powered
- * on after having been suspended earlier. The level and mpidr determine the
- * affinity instance.
- ******************************************************************************/
-static void plat_affinst_suspend_finish(unsigned int afflvl, unsigned int state)
-{
-	unsigned long mpidr = read_mpidr_el1();
-
-	/* Determine if any platform actions need to be executed. */
-	if (plat_do_plat_actions(afflvl, state) == -EAGAIN)
-		return;
-
-	if (afflvl >= MPIDR_AFFLVL2) {
-		/* Enable the gic cpu interface */
-		plat_mt_gic_init();
-		plat_mt_gic_cpuif_enable();
-	}
-
-	/* Perform the common cluster specific operations */
-	if (afflvl >= MPIDR_AFFLVL1) {
-		enable_scu(mpidr);
-
-		/* Enable coherency if this cluster was off */
-		plat_cci_enable();
-		trace_power_flow(mpidr, CLUSTER_UP);
-	}
-
-	if (afflvl >= MPIDR_AFFLVL0)
-		mt_platform_restore_context(mpidr);
-
-	plat_mt_gic_pcpu_init();
-}
-
-static unsigned int plat_get_sys_suspend_power_state(void)
-{
-	/* StateID: 0, StateType: 1(power down), PowerLevel: 2(system) */
-	return psci_make_powerstate(0, 1, 2);
-}
-
-/*******************************************************************************
- * MTK handlers to shutdown/reboot the system
- ******************************************************************************/
-static void __dead2 plat_system_off(void)
-{
-	INFO("MTK System Off\n");
-	wfi();
-	ERROR("MTK System Off: operation not handled.\n");
-	panic();
-}
-
-static void __dead2 plat_system_reset(void)
-{
-	/* Write the System Configuration Control Register */
-	INFO("MTK System Reset\n");
-
-	mmio_clrbits_32(MTK_WDT_BASE,
-		(MTK_WDT_MODE_DUAL_MODE | MTK_WDT_MODE_IRQ));
-	mmio_setbits_32(MTK_WDT_BASE, (MTK_WDT_MODE_KEY | MTK_WDT_MODE_EXTEN));
-	mmio_setbits_32(MTK_WDT_SWRST, MTK_WDT_SWRST_KEY);
-
-	wfi();
-	ERROR("MTK System Reset: operation not handled.\n");
-	panic();
-}
-
-/*******************************************************************************
- * Export the platform handlers to enable psci to invoke them
- ******************************************************************************/
-static const plat_pm_ops_t plat_plat_pm_ops = {
-	.affinst_standby		= plat_affinst_standby,
-	.affinst_on			= plat_affinst_on,
-	.affinst_off			= plat_affinst_off,
-	.affinst_suspend		= plat_affinst_suspend,
-	.affinst_on_finish		= plat_affinst_on_finish,
-	.affinst_suspend_finish		= plat_affinst_suspend_finish,
-	.system_off			= plat_system_off,
-	.system_reset			= plat_system_reset,
-	.get_sys_suspend_power_state	= plat_get_sys_suspend_power_state,
-};
-
-/*******************************************************************************
- * Export the platform specific power ops & initialize the mtk_platform power
- * controller
- ******************************************************************************/
-int platform_setup_pm(const plat_pm_ops_t **plat_ops)
-{
-	*plat_ops = &plat_plat_pm_ops;
-	return 0;
-}

diff --git a/plat/mediatek/mt6795/plat_topology.c b/plat/mediatek/mt6795/plat_topology.c
deleted file mode 100644
index 7425d26..0000000
--- a/plat/mediatek/mt6795/plat_topology.c
+++ /dev/null

@@ -1,34 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <platform_def.h>
-
-#include <arch.h>
-#include <lib/psci/psci.h>
-
-unsigned int plat_get_aff_count(unsigned int aff_lvl, unsigned long mpidr)
-{
-	/* Report 1 (absent) instance at levels higher that the cluster level */
-	if (aff_lvl > MPIDR_AFFLVL1)
-		return PLATFORM_SYSTEM_COUNT;
-
-	if (aff_lvl == MPIDR_AFFLVL1)
-		return PLATFORM_CLUSTER_COUNT;
-
-	return mpidr & 0x100 ? PLATFORM_CLUSTER1_CORE_COUNT :
-			       PLATFORM_CLUSTER0_CORE_COUNT;
-}
-
-unsigned int plat_get_aff_state(unsigned int aff_lvl, unsigned long mpidr)
-{
-	return aff_lvl <= MPIDR_AFFLVL2 ? PSCI_AFF_PRESENT : PSCI_AFF_ABSENT;
-}
-
-int mt_setup_topology(void)
-{
-	/* [TODO] Make topology configurable via SCC */
-	return 0;
-}

diff --git a/plat/mediatek/mt6795/platform.mk b/plat/mediatek/mt6795/platform.mk
deleted file mode 100644
index 4ab692d..0000000
--- a/plat/mediatek/mt6795/platform.mk
+++ /dev/null

@@ -1,67 +0,0 @@
-#
-# Copyright (c) 2016-2017, ARM Limited and Contributors. All rights reserved.
-#
-# SPDX-License-Identifier: BSD-3-Clause
-#
-
-MTK_PLAT		:=	plat/mediatek
-MTK_PLAT_SOC		:=	${MTK_PLAT}/${PLAT}
-
-# Add OEM customized codes
-OEMS				:= true
-MTK_SIP_KERNEL_BOOT_ENABLE := 1
-
-
-ifneq (${OEMS},none)
-  OEMS_INCLUDES		:= -I${MTK_PLAT}/common/custom/
-  OEMS_SOURCES		:=	${MTK_PLAT}/common/custom/oem_svc.c
-endif
-
-PLAT_INCLUDES		:=	-I${MTK_PLAT}/common/				\
-				-I${MTK_PLAT}/common/drivers/uart			\
-				-I${MTK_PLAT_SOC}/				\
-				-I${MTK_PLAT_SOC}/drivers/timer/			\
-				-I${MTK_PLAT_SOC}/include/					\
-				-Iinclude/plat/arm/common/					\
-				${OEMS_INCLUDES}
-
-PLAT_BL_COMMON_SOURCES	:=	lib/xlat_tables/aarch64/xlat_tables.c			\
-				lib/xlat_tables/xlat_tables_common.c			\
-				plat/common/plat_gic.c
-
-BL31_SOURCES		+=	drivers/arm/cci/cci.c				\
-				drivers/delay_timer/generic_delay_timer.c	\
-				drivers/arm/gic/common/gic_common.c		\
-				drivers/arm/gic/v2/gicv2_main.c			\
-				drivers/arm/gic/v2/gicv2_helpers.c		\
-				plat/common/plat_gicv2.c			\
-				drivers/console/aarch64/console.S		\
-				drivers/delay_timer/delay_timer.c		\
-				lib/cpus/aarch64/cortex_a53.S			\
-				${MTK_PLAT_SOC}/bl31_plat_setup.c		\
-				${MTK_PLAT_SOC}/plat_mt_gic.c			\
-				${MTK_PLAT}/common/mtk_sip_svc.c		\
-				${MTK_PLAT}/common/mtk_plat_common.c		\
-				${MTK_PLAT}/common/drivers/uart/8250_console.S		\
-				${MTK_PLAT_SOC}/aarch64/plat_helpers.S		\
-				${MTK_PLAT_SOC}/drivers/timer/mt_cpuxgpt.c	\
-				${MTK_PLAT_SOC}/plat_delay_timer.c		\
-				${MTK_PLAT_SOC}/plat_pm.c			\
-				${MTK_PLAT_SOC}/plat_topology.c			\
-				${MTK_PLAT_SOC}/power_tracer.c			\
-				${MTK_PLAT_SOC}/scu.c		\
-				${OEMS_SOURCES}
-
-# Enable workarounds for selected Cortex-A53 erratas.
-ERRATA_A53_826319	:=	1
-ERRATA_A53_836870	:=	1
-
-WORKAROUND_CVE_2017_5715	:=	0
-
-# indicate the reset vector address can be programmed
-PROGRAMMABLE_RESET_ADDRESS	:=	1
-
-$(eval $(call add_define,MTK_SIP_KERNEL_BOOT_ENABLE))
-
-# Do not enable SVE
-ENABLE_SVE_FOR_NS	:=	0

diff --git a/plat/mediatek/mt6795/power_tracer.c b/plat/mediatek/mt6795/power_tracer.c
deleted file mode 100644
index 64d086d..0000000
--- a/plat/mediatek/mt6795/power_tracer.c
+++ /dev/null

@@ -1,48 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <arch.h>
-#include <common/debug.h>
-
-#include <power_tracer.h>
-
-#define trace_log(...)  INFO("psci: " __VA_ARGS__)
-
-void trace_power_flow(unsigned long mpidr, unsigned char mode)
-{
-	switch (mode) {
-	case CPU_UP:
-		trace_log("core %lld:%lld ON\n",
-			  (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS,
-			  (mpidr & MPIDR_CPU_MASK));
-		break;
-	case CPU_DOWN:
-		trace_log("core %lld:%lld OFF\n",
-			  (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS,
-			  (mpidr & MPIDR_CPU_MASK));
-		break;
-	case CPU_SUSPEND:
-		trace_log("core %lld:%lld SUSPEND\n",
-			  (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS,
-			  (mpidr & MPIDR_CPU_MASK));
-		break;
-	case CLUSTER_UP:
-		trace_log("cluster %lld ON\n",
-			  (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS);
-		break;
-	case CLUSTER_DOWN:
-		trace_log("cluster %lld OFF\n",
-			  (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS);
-		break;
-	case CLUSTER_SUSPEND:
-		trace_log("cluster %lld SUSPEND\n",
-			  (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS);
-		break;
-	default:
-		trace_log("unknown power mode\n");
-		break;
-	}
-}

diff --git a/plat/mediatek/mt6795/scu.c b/plat/mediatek/mt6795/scu.c
deleted file mode 100644
index 3b74527..0000000
--- a/plat/mediatek/mt6795/scu.c
+++ /dev/null

@@ -1,30 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <arch.h>
-#include <lib/mmio.h>
-
-#include <mcucfg.h>
-
-void disable_scu(unsigned long mpidr)
-{
-	if (mpidr & MPIDR_CLUSTER_MASK)
-		mmio_setbits_32((uintptr_t)&mt6795_mcucfg->mp1_miscdbg,
-			MP1_ACINACTM);
-	else
-		mmio_setbits_32((uintptr_t)&mt6795_mcucfg->mp0_axi_config,
-			MP0_ACINACTM);
-}
-
-void enable_scu(unsigned long mpidr)
-{
-	if (mpidr & MPIDR_CLUSTER_MASK)
-		mmio_clrbits_32((uintptr_t)&mt6795_mcucfg->mp1_miscdbg,
-			MP1_ACINACTM);
-	else
-		mmio_clrbits_32((uintptr_t)&mt6795_mcucfg->mp0_axi_config,
-			MP0_ACINACTM);
-}

diff --git a/plat/qemu/common/qemu_common.c b/plat/qemu/common/qemu_common.c
index 47ec791..0c184f4 100644
--- a/plat/qemu/common/qemu_common.c
+++ b/plat/qemu/common/qemu_common.c

@@ -1,6 +1,6 @@
 
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -11,6 +11,7 @@
 #include <common/bl_common.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
 
+#include <plat/common/platform.h>
 #include "qemu_private.h"
 
 #define MAP_DEVICE0	MAP_REGION_FLAT(DEVICE0_BASE,			\
@@ -160,4 +161,9 @@
 DEFINE_CONFIGURE_MMU_EL(svc_mon)
 #endif
 
-
+#if MEASURED_BOOT || TRUSTED_BOARD_BOOT
+int plat_get_mbedtls_heap(void **heap_addr, size_t *heap_size)
+{
+	return get_mbedtls_heap_helper(heap_addr, heap_size);
+}
+#endif

diff --git a/plat/qemu/common/qemu_private.h b/plat/qemu/common/qemu_private.h
index 4dc62f5..c313cb6 100644
--- a/plat/qemu/common/qemu_private.h
+++ b/plat/qemu/common/qemu_private.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -34,4 +34,18 @@
 void qemu_pwr_gic_on_finish(void);
 void qemu_pwr_gic_off(void);
 
+int qemu_set_tos_fw_info(uintptr_t config_base, uintptr_t log_addr,
+			size_t log_size);
+
+int qemu_set_nt_fw_info(
+/*
+ * Currently OP-TEE does not support reading DTBs from Secure memory
+ * and this option should be removed when feature is supported.
+ */
+#ifdef SPD_opteed
+			uintptr_t log_addr,
+#endif
+			size_t log_size,
+			uintptr_t *ns_log_addr);
+
 #endif /* QEMU_PRIVATE_H */

diff --git a/plat/qemu/common/qemu_trusted_boot.c b/plat/qemu/common/qemu_trusted_boot.c
index 1ef7e43..6a8edca 100644
--- a/plat/qemu/common/qemu_trusted_boot.c
+++ b/plat/qemu/common/qemu_trusted_boot.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -29,8 +29,3 @@
 {
 	return 1;
 }
-
-int plat_get_mbedtls_heap(void **heap_addr, size_t *heap_size)
-{
-	return get_mbedtls_heap_helper(heap_addr, heap_size);
-}

diff --git a/plat/qemu/qemu/include/platform_def.h b/plat/qemu/qemu/include/platform_def.h
index c02eff9..78467c4 100644
--- a/plat/qemu/qemu/include/platform_def.h
+++ b/plat/qemu/qemu/include/platform_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -270,4 +270,9 @@
  */
 #define SYS_COUNTER_FREQ_IN_TICKS	((1000 * 1000 * 1000) / 16)
 
+/*
+ * Maximum size of Event Log buffer used in Measured Boot Event Log driver
+ */
+#define	PLAT_EVENT_LOG_MAX_SIZE		UL(0x400)
+
 #endif /* PLATFORM_DEF_H */

diff --git a/plat/qemu/qemu/platform.mk b/plat/qemu/qemu/platform.mk
index a8f978a..8e7f7c8 100644
--- a/plat/qemu/qemu/platform.mk
+++ b/plat/qemu/qemu/platform.mk

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2013-2021, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -57,11 +57,7 @@
 
 ifneq (${TRUSTED_BOARD_BOOT},0)
 
-    include drivers/auth/mbedtls/mbedtls_crypto.mk
-    include drivers/auth/mbedtls/mbedtls_x509.mk
-
     AUTH_SOURCES	:=	drivers/auth/auth_mod.c			\
-				drivers/auth/crypto_mod.c		\
 				drivers/auth/img_parser_mod.c		\
 				drivers/auth/tbbr/tbbr_cot_common.c
 
@@ -78,6 +74,8 @@
 				$(PLAT_QEMU_COMMON_PATH)/qemu_rotpk.S	\
 				drivers/auth/tbbr/tbbr_cot_bl2.c
 
+    include drivers/auth/mbedtls/mbedtls_x509.mk
+
     ROT_KEY             = $(BUILD_PLAT)/rot_key.pem
     ROTPK_HASH          = $(BUILD_PLAT)/rotpk_sha256.bin
 
@@ -98,6 +96,38 @@
 	openssl dgst -sha256 -binary > $@ 2>/dev/null
 endif
 
+# Include Measured Boot makefile before any Crypto library makefile.
+# Crypto library makefile may need default definitions of Measured Boot build
+# flags present in Measured Boot makefile.
+ifeq (${MEASURED_BOOT},1)
+    MEASURED_BOOT_MK := drivers/measured_boot/event_log/event_log.mk
+    $(info Including ${MEASURED_BOOT_MK})
+    include ${MEASURED_BOOT_MK}
+
+    ifneq (${MBOOT_EL_HASH_ALG}, sha256)
+        $(eval $(call add_define,TF_MBEDTLS_MBOOT_USE_SHA512))
+    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}
+
+     BL1_SOURCES	+=      plat/qemu/qemu/qemu_bl1_measured_boot.c
+
+endif
+
+ifneq ($(filter 1,${MEASURED_BOOT} ${TRUSTED_BOARD_BOOT}),)
+    CRYPTO_SOURCES	:=	drivers/auth/crypto_mod.c
+
+    BL1_SOURCES		+=	${CRYPTO_SOURCES}
+    BL2_SOURCES		+=	${CRYPTO_SOURCES}
+
+    # We expect to locate the *.mk files under the directories specified below
+    #
+    include drivers/auth/mbedtls/mbedtls_crypto.mk
+endif
+
 BL1_SOURCES		+=	drivers/io/io_semihosting.c		\
 				drivers/io/io_storage.c			\
 				drivers/io/io_fip.c			\
@@ -131,6 +161,7 @@
 				${PLAT_QEMU_COMMON_PATH}/qemu_bl2_mem_params_desc.c	\
 				${PLAT_QEMU_COMMON_PATH}/qemu_image_load.c		\
 				common/fdt_fixup.c					\
+				common/fdt_wrappers.c					\
 				common/desc_image_load.c
 
 ifeq ($(add-lib-optee),yes)

diff --git a/plat/qemu/qemu/qemu_bl1_measured_boot.c b/plat/qemu/qemu/qemu_bl1_measured_boot.c
new file mode 100644
index 0000000..3d20f97
--- /dev/null
+++ b/plat/qemu/qemu/qemu_bl1_measured_boot.c

@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2022, Linaro.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include <common/desc_image_load.h>
+
+/*
+ * Add dummy functions for measured boot for BL1.
+ * In most of the SoC's, ROM/BL1 code is pre-built. So we are assumimg that
+ * it doesn't have the capability to do measurements and extend eventlog.
+ * hence these are dummy functions.
+ */
+void bl1_plat_mboot_init(void)
+{
+}
+
+void bl1_plat_mboot_finish(void)
+{
+}
+
+int plat_mboot_measure_image(unsigned int image_id, image_info_t *image_data)
+{
+	return 0;
+}

diff --git a/plat/qemu/qemu/qemu_common_measured_boot.c b/plat/qemu/qemu/qemu_common_measured_boot.c
new file mode 100644
index 0000000..41f7f87
--- /dev/null
+++ b/plat/qemu/qemu/qemu_common_measured_boot.c

@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) 2022, Linaro.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdint.h>
+
+#include <common/desc_image_load.h>
+#include <drivers/measured_boot/event_log/event_log.h>
+#include <plat/common/platform.h>
+
+extern event_log_metadata_t qemu_event_log_metadata[];
+
+const event_log_metadata_t *plat_event_log_get_metadata(void)
+{
+	return qemu_event_log_metadata;
+}
+
+int plat_mboot_measure_image(unsigned int image_id, image_info_t *image_data)
+{
+	/* Calculate image hash and record data in Event Log */
+	int err = event_log_measure_and_record(image_data->image_base,
+					       image_data->image_size,
+					       image_id);
+	if (err != 0) {
+		ERROR("%s%s image id %u (%i)\n",
+		      "Failed to ", "record", image_id, err);
+		return err;
+	}
+
+	return 0;
+}

diff --git a/plat/qemu/qemu/qemu_helpers.c b/plat/qemu/qemu/qemu_helpers.c
new file mode 100644
index 0000000..01b8249
--- /dev/null
+++ b/plat/qemu/qemu/qemu_helpers.c

@@ -0,0 +1,214 @@
+/*
+ * Copyright (c) 2022, Linaro.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#if MEASURED_BOOT
+#include <common/desc_image_load.h>
+#endif
+#include <common/fdt_wrappers.h>
+
+#include <libfdt.h>
+
+#ifdef SPD_opteed
+/*
+ * Currently OP-TEE does not support reading DTBs from Secure memory
+ * and this property should be removed when this feature is supported.
+ */
+#define DTB_PROP_HW_SM_LOG_ADDR	"tpm_event_log_sm_addr"
+#endif
+
+#define DTB_PROP_HW_LOG_ADDR	"tpm_event_log_addr"
+#define DTB_PROP_HW_LOG_SIZE    "tpm_event_log_size"
+
+#if MEASURED_BOOT
+
+#ifdef SPD_opteed
+int qemu_set_tee_fw_info(uintptr_t config_base, uintptr_t log_addr,
+			 size_t log_size)
+{
+	int offs, err = 0;
+	void *dtb = (void *)config_base;
+	const char *compatible = "arm,tpm_event_log";
+	uint64_t sec_base = cpu_to_fdt64(log_addr);
+	uint32_t sz = cpu_to_fdt32(log_size);
+
+	offs = fdtw_find_or_add_subnode(dtb, 0, "tpm-event-log");
+	if (offs < 0) {
+		ERROR("Failed to add node tpm-event-log %d\n", offs);
+		return offs;
+	}
+
+	if (fdt_appendprop(dtb, offs, "compatible", compatible,
+			   strlen(compatible) + 1) < 0) {
+		return -1;
+	}
+
+	err = fdt_setprop(dtb, offs, DTB_PROP_HW_SM_LOG_ADDR, &sec_base, 8);
+	if (err < 0) {
+		ERROR("Failed to add log addr err %d\n", err);
+		return err;
+	}
+
+	err = fdt_setprop(dtb, offs, DTB_PROP_HW_LOG_SIZE, &sz, 4);
+	if (err < 0) {
+		ERROR("Failed to add log addr err %d\n", err);
+		return err;
+	}
+
+	return err;
+}
+#endif
+
+/*
+ * Write the Event Log address and its size in the DTB.
+ *
+ * This function is supposed to be called only by BL2.
+ *
+ * Returns:
+ *	0 = success
+ *    < 0 = error
+ */
+static int qemu_set_event_log_info(uintptr_t config_base,
+#ifdef SPD_opteed
+				  uintptr_t sm_log_addr,
+#endif
+				  uintptr_t log_addr, size_t log_size)
+{
+	/* As libfdt uses void *, we can't avoid this cast */
+	void *dtb = (void *)config_base;
+	const char *compatible_tpm = "tcg,tpm-tis-mmio";
+	uint64_t base = cpu_to_fdt64(log_addr);
+	uint32_t sz = cpu_to_fdt32(log_size);
+	int err, node;
+
+	err = fdt_open_into(dtb, dtb, PLAT_QEMU_DT_MAX_SIZE);
+	if (err < 0) {
+		ERROR("Invalid Device Tree at %p: error %d\n", dtb, err);
+		return err;
+	}
+
+	/*
+	 * Verify that the DTB is valid, before attempting to write to it,
+	 * and get the DTB root node.
+	 */
+
+	/* Check if the pointer to DT is correct */
+	err = fdt_check_header(dtb);
+	if (err < 0) {
+		WARN("Invalid DTB file passed\n");
+		return err;
+	}
+
+	/*
+	 * Find the TPM node in device tree. On qemu, we assume it will
+	 * be sw-tpm.
+	 */
+	node = fdt_node_offset_by_compatible(dtb, -1, compatible_tpm);
+	if (node < 0) {
+		ERROR("The compatible property '%s' not%s", compatible_tpm,
+			" found in the config\n");
+		return node;
+	}
+
+	err = fdt_setprop(dtb, node, DTB_PROP_HW_LOG_ADDR, &base, 8);
+	if (err < 0) {
+		ERROR("Failed to add log addr err %d\n", err);
+		return err;
+	}
+
+	err = fdt_setprop(dtb, node, DTB_PROP_HW_LOG_SIZE, &sz, 4);
+	if (err < 0) {
+		ERROR("Failed to add log addr err %d\n", err);
+		return err;
+	}
+
+#ifdef SPD_opteed
+	err = qemu_set_tee_fw_info(config_base, sm_log_addr, log_size);
+	if (err < 0) {
+		ERROR("Failed to add tpm-event-node at %p: err %d\n", dtb, err);
+		return err;
+	}
+#endif
+
+	err = fdt_pack(dtb);
+	if (err < 0) {
+		ERROR("Failed to pack Device Tree at %p: err %d\n", dtb, err);
+		return err;
+	}
+
+	/*
+	 * Ensure that the info written to the DTB is visible
+	 * to other images.
+	 */
+	flush_dcache_range(config_base, fdt_totalsize(dtb));
+
+	return err;
+}
+
+/*
+ * This function writes the Event Log address and its size
+ * in the TOS_FW_CONFIG DTB.
+ *
+ * This function is supposed to be called only by BL2.
+ *
+ * Returns:
+ *	0 = success
+ *    < 0 = error
+ */
+int qemu_set_tos_fw_info(uintptr_t config_base, uintptr_t log_addr,
+			size_t log_size)
+{
+	int err = 0;
+
+	assert(config_base != 0UL);
+	assert(log_addr != 0UL);
+
+	/*
+	 * FIXME - add code to add/update Log address and it's
+	 * size in TOS FW CONFIG.
+	 * For now we don't have support for TOS FW config in OP-TEE.
+	 * So leave this function blank
+	 */
+
+	return err;
+}
+
+/*
+ * This function writes the Event Log address and its size
+ * in the QEMU DTB.
+ *
+ * This function is supposed to be called only by BL2.
+ *
+ * Returns:
+ *	0 = success
+ *    < 0 = error
+ */
+int qemu_set_nt_fw_info(
+#ifdef SPD_opteed
+			uintptr_t log_addr,
+#endif
+			size_t log_size, uintptr_t *ns_log_addr)
+{
+	uintptr_t ns_addr;
+	int err;
+
+	assert(ns_log_addr != NULL);
+
+	ns_addr = PLAT_QEMU_DT_BASE + PLAT_QEMU_DT_MAX_SIZE;
+
+	/* Write the Event Log address and its size in the DTB */
+	err = qemu_set_event_log_info(PLAT_QEMU_DT_BASE,
+#ifdef SPD_opteed
+					log_addr,
+#endif
+					ns_addr, log_size);
+
+	/* Return Event Log address in Non-secure memory */
+	*ns_log_addr = (err < 0) ? 0UL : ns_addr;
+	return err;
+}
+#endif /* MEASURED_BOOT */

diff --git a/plat/qemu/qemu/qemu_measured_boot.c b/plat/qemu/qemu/qemu_measured_boot.c
new file mode 100644
index 0000000..d9e475a
--- /dev/null
+++ b/plat/qemu/qemu/qemu_measured_boot.c

@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2022, Linaro.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <drivers/measured_boot/event_log/event_log.h>
+#include <plat/common/common_def.h>
+#include <tools_share/tbbr_oid.h>
+
+#include "../common/qemu_private.h"
+
+/* Event Log data */
+static uint8_t event_log[PLAT_EVENT_LOG_MAX_SIZE];
+static uint64_t event_log_base;
+
+/* FVP table with platform specific image IDs, names and PCRs */
+const event_log_metadata_t qemu_event_log_metadata[] = {
+	{ BL31_IMAGE_ID, EVLOG_BL31_STRING, PCR_0 },
+	{ BL32_IMAGE_ID, EVLOG_BL32_STRING, PCR_0 },
+	{ BL32_EXTRA1_IMAGE_ID, EVLOG_BL32_EXTRA1_STRING, PCR_0 },
+	{ BL32_EXTRA2_IMAGE_ID, EVLOG_BL32_EXTRA2_STRING, PCR_0 },
+	{ BL33_IMAGE_ID, EVLOG_BL33_STRING, PCR_0 },
+	{ HW_CONFIG_ID, EVLOG_HW_CONFIG_STRING, PCR_0 },
+	{ NT_FW_CONFIG_ID, EVLOG_NT_FW_CONFIG_STRING, PCR_0 },
+	{ SCP_BL2_IMAGE_ID, EVLOG_SCP_BL2_STRING, PCR_0 },
+	{ SOC_FW_CONFIG_ID, EVLOG_SOC_FW_CONFIG_STRING, PCR_0 },
+	{ TOS_FW_CONFIG_ID, EVLOG_TOS_FW_CONFIG_STRING, PCR_0 },
+
+	{ EVLOG_INVALID_ID, NULL, (unsigned int)(-1) }	/* Terminator */
+};
+
+void bl2_plat_mboot_init(void)
+{
+	/*
+	 * Here we assume that BL1/ROM code doesn't have the driver
+	 * to measure the BL2 code which is a common case for
+	 * already existing platforms
+	 */
+	event_log_init(event_log, event_log + sizeof(event_log));
+	event_log_write_header();
+
+	/*
+	 * TBD - Add code to do self measurement of BL2 code and add an
+	 * event for BL2 measurement
+	 */
+
+	event_log_base = (uintptr_t)event_log;
+}
+
+void bl2_plat_mboot_finish(void)
+{
+	int rc;
+
+	/* Event Log address in Non-Secure memory */
+	uintptr_t ns_log_addr;
+
+	/* Event Log filled size */
+	size_t event_log_cur_size;
+
+	event_log_cur_size = event_log_get_cur_size((uint8_t *)event_log_base);
+
+	rc = qemu_set_nt_fw_info(
+#ifdef SPD_opteed
+			    (uintptr_t)event_log_base,
+#endif
+			    event_log_cur_size, &ns_log_addr);
+	if (rc != 0) {
+		ERROR("%s(): Unable to update %s_FW_CONFIG\n",
+		      __func__, "NT");
+		/*
+		 * It is a fatal error because on QEMU secure world software
+		 * assumes that a valid event log exists and will use it to
+		 * record the measurements into the fTPM or sw-tpm.
+		 * Note: In QEMU platform, OP-TEE uses nt_fw_config to get the
+		 * secure Event Log buffer address.
+		 */
+		panic();
+	}
+
+	/* Copy Event Log to Non-secure memory */
+	(void)memcpy((void *)ns_log_addr, (const void *)event_log_base,
+		     event_log_cur_size);
+
+	/* Ensure that the Event Log is visible in Non-secure memory */
+	flush_dcache_range(ns_log_addr, event_log_cur_size);
+
+#if defined(SPD_tspd) || defined(SPD_spmd)
+	/* Set Event Log data in TOS_FW_CONFIG */
+	rc = qemu_set_tos_fw_info((uintptr_t)event_log_base,
+				 event_log_cur_size);
+	if (rc != 0) {
+		ERROR("%s(): Unable to update %s_FW_CONFIG\n",
+		      __func__, "TOS");
+		panic();
+	}
+#endif /* defined(SPD_tspd) || defined(SPD_spmd) */
+
+	dump_event_log((uint8_t *)event_log_base, event_log_cur_size);
+}

diff --git a/plat/socionext/synquacer/include/platform_def.h b/plat/socionext/synquacer/include/platform_def.h
index 49ffbf9..a84660b 100644
--- a/plat/socionext/synquacer/include/platform_def.h
+++ b/plat/socionext/synquacer/include/platform_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -42,16 +42,52 @@
 #define MAX_XLAT_TABLES			8
 #define MAX_MMAP_REGIONS		8
 
+#if TRUSTED_BOARD_BOOT
+#define PLATFORM_STACK_SIZE		0x1000
+#else
 #define PLATFORM_STACK_SIZE		0x400
+#endif
+
+#if !RESET_TO_BL31
+
+/* A mailbox page will be mapped from BL2 and BL31 */
+#define BL2_MAILBOX_BASE		0x0403f000
+#define BL2_MAILBOX_SIZE		0x1000
+
+#define PLAT_SQ_BOOTIDX_BASE		0x08510000
+#define PLAT_SQ_MAX_BOOT_INDEX		2
+
+#define MAX_IO_HANDLES			2
+#define MAX_IO_DEVICES			2
+#define MAX_IO_BLOCK_DEVICES	U(1)
+
+#define BL2_BASE			0x04000000
+#define BL2_SIZE			(256 * 1024)
+#define BL2_LIMIT			(BL2_BASE + BL2_SIZE)
+
+/* If BL2 is enabled, the BL31 is loaded on secure DRAM */
+#define BL31_BASE			0xfbe00000
+#define BL31_SIZE			0x00100000
+#else
 
 #define BL31_BASE			0x04000000
 #define BL31_SIZE			0x00080000
+#endif
+
 #define BL31_LIMIT			(BL31_BASE + BL31_SIZE)
 
 #define BL32_BASE			0xfc000000
 #define BL32_SIZE			0x03c00000
 #define BL32_LIMIT			(BL32_BASE + BL32_SIZE)
 
+/* Alternative BL33 */
+#define PLAT_SQ_BL33_BASE		0xe0000000
+#define PLAT_SQ_BL33_SIZE		0x00100000
+
+/* FWU FIP IO base */
+#define PLAT_SQ_FIP_IOBASE		0x08600000
+#define PLAT_SQ_FIP_MAXSIZE		0x00400000
+
 #define PLAT_SQ_CCN_BASE		0x32000000
 #define PLAT_SQ_CLUSTER_TO_CCN_ID_MAP					\
 					0,	/* Cluster 0 */		\

diff --git a/plat/socionext/synquacer/include/sq_common.h b/plat/socionext/synquacer/include/sq_common.h
index b09d22a..eef0e1f 100644
--- a/plat/socionext/synquacer/include/sq_common.h
+++ b/plat/socionext/synquacer/include/sq_common.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -39,6 +39,8 @@
 void sq_gic_cpuif_disable(void);
 void sq_gic_pcpu_init(void);
 
+int sq_io_setup(void);
+struct image_info *sq_get_image_info(unsigned int image_id);
 void sq_mmap_setup(uintptr_t total_base, size_t total_size,
 		   const struct mmap_region *mmap);
 

diff --git a/plat/socionext/synquacer/platform.mk b/plat/socionext/synquacer/platform.mk
index dcd5d31..3eab3d6 100644
--- a/plat/socionext/synquacer/platform.mk
+++ b/plat/socionext/synquacer/platform.mk

@@ -1,18 +1,26 @@
 #
-# Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
-override RESET_TO_BL31			:= 1
 override PROGRAMMABLE_RESET_ADDRESS	:= 1
 override USE_COHERENT_MEM		:= 1
 override SEPARATE_CODE_AND_RODATA	:= 1
 override ENABLE_SVE_FOR_NS		:= 0
 # Enable workarounds for selected Cortex-A53 erratas.
 ERRATA_A53_855873		:= 1
-# Enable SCMI support
-SQ_USE_SCMI_DRIVER		?= 0
+
+ifeq (${RESET_TO_BL31}, 1)
+override RESET_TO_BL31          := 1
+override TRUSTED_BOARD_BOOT     := 0
+SQ_USE_SCMI_DRIVER              ?= 0
+else
+override RESET_TO_BL31          := 0
+override BL2_AT_EL3             := 1
+SQ_USE_SCMI_DRIVER              := 1
+BL2_CPPFLAGS                    += -DPLAT_XLAT_TABLES_DYNAMIC
+endif
 
 # Libraries
 include lib/xlat_tables_v2/xlat_tables.mk
@@ -28,14 +36,55 @@
 				drivers/arm/pl011/aarch64/pl011_console.S \
 				drivers/delay_timer/delay_timer.c	\
 				drivers/delay_timer/generic_delay_timer.c \
+				lib/cpus/aarch64/cortex_a53.S		\
+				$(PLAT_PATH)/sq_xlat_setup.c	\
 				${XLAT_TABLES_LIB_SRCS}
 
 # Include GICv3 driver files
 include drivers/arm/gic/v3/gicv3.mk
 
+ifneq (${RESET_TO_BL31}, 1)
+BL2_SOURCES		+=	common/desc_image_load.c		\
+				drivers/io/io_fip.c			\
+				drivers/io/io_memmap.c			\
+				drivers/io/io_storage.c			\
+				$(PLAT_PATH)/sq_bl2_setup.c		\
+				$(PLAT_PATH)/sq_image_desc.c	\
+				$(PLAT_PATH)/sq_io_storage.c
+
+ifeq (${TRUSTED_BOARD_BOOT},1)
+include drivers/auth/mbedtls/mbedtls_crypto.mk
+include drivers/auth/mbedtls/mbedtls_x509.mk
+BL2_SOURCES		+=	drivers/auth/auth_mod.c			\
+				drivers/auth/crypto_mod.c		\
+				drivers/auth/img_parser_mod.c		\
+				drivers/auth/tbbr/tbbr_cot_common.c	\
+				drivers/auth/tbbr/tbbr_cot_bl2.c	\
+				plat/common/tbbr/plat_tbbr.c		\
+				$(PLAT_PATH)/sq_rotpk.S		\
+				$(PLAT_PATH)/sq_tbbr.c
+
+ROT_KEY			= $(BUILD_PLAT)/rot_key.pem
+ROTPK_HASH		= $(BUILD_PLAT)/rotpk_sha256.bin
+
+$(eval $(call add_define_val,ROTPK_HASH,'"$(ROTPK_HASH)"'))
+$(BUILD_PLAT)/bl2/sq_rotpk.o: $(ROTPK_HASH)
+
+certificates: $(ROT_KEY)
+$(ROT_KEY): | $(BUILD_PLAT)
+	@echo "  OPENSSL $@"
+	$(Q)openssl genrsa 2048 > $@ 2>/dev/null
+
+$(ROTPK_HASH): $(ROT_KEY)
+	@echo "  OPENSSL $@"
+	$(Q)openssl rsa -in $< -pubout -outform DER 2>/dev/null |\
+	openssl dgst -sha256 -binary > $@ 2>/dev/null
+
+endif	# TRUSTED_BOARD_BOOT
+endif
+
 BL31_SOURCES		+=	drivers/arm/ccn/ccn.c			\
 				${GICV3_SOURCES}			\
-				lib/cpus/aarch64/cortex_a53.S		\
 				plat/common/plat_gicv3.c		\
 				plat/common/plat_psci_common.c		\
 				$(PLAT_PATH)/sq_bl31_setup.c		\
@@ -43,7 +92,6 @@
 				$(PLAT_PATH)/sq_topology.c		\
 				$(PLAT_PATH)/sq_psci.c			\
 				$(PLAT_PATH)/sq_gicv3.c			\
-				$(PLAT_PATH)/sq_xlat_setup.c	\
 				$(PLAT_PATH)/drivers/scp/sq_scp.c
 
 ifeq (${SQ_USE_SCMI_DRIVER},0)

diff --git a/plat/socionext/synquacer/sq_bl2_setup.c b/plat/socionext/synquacer/sq_bl2_setup.c
new file mode 100644
index 0000000..a98d912
--- /dev/null
+++ b/plat/socionext/synquacer/sq_bl2_setup.c

@@ -0,0 +1,84 @@
+/*
+ * Copyright (c) 2022, Socionext Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <common/desc_image_load.h>
+#include <common/image_decompress.h>
+#include <drivers/arm/pl011.h>
+#include <drivers/io/io_storage.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <plat/common/platform.h>
+
+#include <platform_def.h>
+#include <sq_common.h>
+
+static console_t console;
+
+void bl2_el3_early_platform_setup(u_register_t x0, u_register_t x1,
+				  u_register_t x2, u_register_t x3)
+{
+	/* Initialize the console to provide early debug support */
+	(void)console_pl011_register(PLAT_SQ_BOOT_UART_BASE,
+			       PLAT_SQ_BOOT_UART_CLK_IN_HZ,
+			       SQ_CONSOLE_BAUDRATE, &console);
+	console_set_scope(&console, CONSOLE_FLAG_BOOT);
+}
+
+void bl2_el3_plat_arch_setup(void)
+{
+	int ret;
+
+	sq_mmap_setup(BL2_BASE, BL2_SIZE, NULL);
+
+	ret = sq_io_setup();
+	if (ret) {
+		ERROR("failed to setup io devices\n");
+		plat_error_handler(ret);
+	}
+}
+
+void bl2_platform_setup(void)
+{
+}
+
+void plat_flush_next_bl_params(void)
+{
+	flush_bl_params_desc();
+}
+
+bl_load_info_t *plat_get_bl_image_load_info(void)
+{
+	return get_bl_load_info_from_mem_params_desc();
+}
+
+bl_params_t *plat_get_next_bl_params(void)
+{
+	return get_next_bl_params_from_mem_params_desc();
+}
+
+void bl2_plat_preload_setup(void)
+{
+}
+
+int bl2_plat_handle_pre_image_load(unsigned int image_id)
+{
+	struct image_info *image_info;
+
+	image_info = sq_get_image_info(image_id);
+
+	return mmap_add_dynamic_region(image_info->image_base,
+				      image_info->image_base,
+				      image_info->image_max_size,
+				      MT_MEMORY | MT_RW | MT_NS);
+}
+
+int bl2_plat_handle_post_image_load(unsigned int image_id)
+{
+	return 0;
+}

diff --git a/plat/socionext/synquacer/sq_bl31_setup.c b/plat/socionext/synquacer/sq_bl31_setup.c
index a7a0ce0..967437b 100644
--- a/plat/socionext/synquacer/sq_bl31_setup.c
+++ b/plat/socionext/synquacer/sq_bl31_setup.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -44,6 +44,35 @@
 	return type == NON_SECURE ? &bl33_image_ep_info : &bl32_image_ep_info;
 }
 
+#if !RESET_TO_BL31
+void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
+				u_register_t arg2, u_register_t arg3)
+{
+	void *from_bl2 = (void *) arg0;
+	bl_params_node_t *bl_params = ((bl_params_t *) from_bl2)->head;
+
+	/* Initialize the console to provide early debug support */
+	(void)console_pl011_register(PLAT_SQ_BOOT_UART_BASE,
+			       PLAT_SQ_BOOT_UART_CLK_IN_HZ,
+			       SQ_CONSOLE_BAUDRATE, &console);
+
+	console_set_scope(&console, CONSOLE_FLAG_BOOT | CONSOLE_FLAG_RUNTIME);
+
+	/* Initialize power controller before setting up topology */
+	plat_sq_pwrc_setup();
+
+	while (bl_params) {
+		if (bl_params->image_id == BL32_IMAGE_ID)
+			bl32_image_ep_info = *bl_params->ep_info;
+
+		if (bl_params->image_id == BL33_IMAGE_ID)
+			bl33_image_ep_info = *bl_params->ep_info;
+
+		bl_params = bl_params->next_params_info;
+	}
+}
+
+#else
 /*******************************************************************************
  * Gets SPSR for BL32 entry
  ******************************************************************************/
@@ -129,6 +158,7 @@
 	bl33_image_ep_info.spsr = sq_get_spsr_for_bl33_entry();
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
 }
+#endif
 
 static void sq_configure_sys_timer(void)
 {
@@ -192,6 +222,11 @@
 				PLAT_SQ_SP_PRIV_SIZE,
 				MT_RW_DATA | MT_SECURE),
 #endif
+#if !RESET_TO_BL31
+		MAP_REGION_FLAT(BL2_MAILBOX_BASE,
+				BL2_MAILBOX_SIZE,
+				MT_RW | MT_SECURE),
+#endif
 		{0},
 	};
 

diff --git a/plat/socionext/synquacer/sq_helpers.S b/plat/socionext/synquacer/sq_helpers.S
index 7a2d97b..5f9eab4 100644
--- a/plat/socionext/synquacer/sq_helpers.S
+++ b/plat/socionext/synquacer/sq_helpers.S

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -46,7 +46,12 @@
  * code that secondary CPUs jump to.
  */
 func plat_secondary_cold_boot_setup
+#if !RESET_TO_BL31
+	mov_imm	x0, BL2_MAILBOX_BASE
+	ldr	x0, [x0]
+#else
 	ldr	x0, sq_sec_entrypoint
+#endif
 
 	/* Wait until the mailbox gets populated */
 poll_mailbox:

diff --git a/plat/socionext/synquacer/sq_image_desc.c b/plat/socionext/synquacer/sq_image_desc.c
new file mode 100644
index 0000000..5fe125b
--- /dev/null
+++ b/plat/socionext/synquacer/sq_image_desc.c

@@ -0,0 +1,76 @@
+/*
+ * Copyright (c) 2022, Socionext Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch.h>
+#include <common/desc_image_load.h>
+
+#include <platform_def.h>
+
+static struct bl_mem_params_node sq_image_descs[] = {
+	{
+		.image_id = BL31_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+				      VERSION_2, image_info_t, 0),
+		.image_info.image_base = BL31_BASE,
+		.image_info.image_max_size = BL31_SIZE,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+				      VERSION_2, entry_point_info_t,
+				      SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = BL31_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+					DISABLE_ALL_EXCEPTIONS),
+
+		.next_handoff_image_id = BL32_IMAGE_ID,
+	},
+	{
+		.image_id = BL32_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+				      VERSION_2, image_info_t, 0),
+		.image_info.image_base = BL32_BASE,
+		.image_info.image_max_size = BL32_SIZE,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+				      VERSION_2, entry_point_info_t,
+				      SECURE | EXECUTABLE),
+		.ep_info.pc = BL32_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+					DISABLE_ALL_EXCEPTIONS),
+
+		.next_handoff_image_id = BL33_IMAGE_ID,
+	},
+	{
+		.image_id = BL33_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+				      VERSION_2, image_info_t, 0),
+		.image_info.image_base = PLAT_SQ_BL33_BASE,
+		.image_info.image_max_size = PLAT_SQ_BL33_SIZE,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+				      VERSION_2, entry_point_info_t,
+				      NON_SECURE | EXECUTABLE),
+		.ep_info.pc = PLAT_SQ_BL33_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL2, MODE_SP_ELX,
+					DISABLE_ALL_EXCEPTIONS),
+
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+};
+REGISTER_BL_IMAGE_DESCS(sq_image_descs)
+
+struct image_info *sq_get_image_info(unsigned int image_id)
+{
+	struct bl_mem_params_node *desc;
+
+	desc = get_bl_mem_params_node(image_id);
+	assert(desc);
+	return &desc->image_info;
+}

diff --git a/plat/socionext/synquacer/sq_io_storage.c b/plat/socionext/synquacer/sq_io_storage.c
new file mode 100644
index 0000000..ea83dad
--- /dev/null
+++ b/plat/socionext/synquacer/sq_io_storage.c

@@ -0,0 +1,246 @@
+/*
+ * Copyright (c) 2022, Socionext Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdint.h>
+
+#include <drivers/io/io_block.h>
+#include <drivers/io/io_driver.h>
+#include <drivers/io/io_fip.h>
+#include <drivers/io/io_memmap.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <tools_share/firmware_image_package.h>
+
+#include <platform_def.h>
+#include <sq_common.h>
+
+static const io_dev_connector_t *sq_fip_dev_con;
+static uintptr_t sq_fip_dev_handle;
+
+static const io_dev_connector_t *sq_backend_dev_con;
+static uintptr_t sq_backend_dev_handle;
+
+static io_block_spec_t sq_fip_spec = {
+	.offset = PLAT_SQ_FIP_IOBASE,	/* FIP Image is at 5MB offset on memory-mapped NOR flash */
+	.length = PLAT_SQ_FIP_MAXSIZE,	/* Expected maximum FIP image size */
+};
+
+static const io_uuid_spec_t sq_bl2_spec = {
+	.uuid = UUID_TRUSTED_BOOT_FIRMWARE_BL2,
+};
+
+static const io_uuid_spec_t sq_bl31_spec = {
+	.uuid = UUID_EL3_RUNTIME_FIRMWARE_BL31,
+};
+
+static const io_uuid_spec_t sq_bl32_spec = {
+	.uuid = UUID_SECURE_PAYLOAD_BL32,
+};
+
+static const io_uuid_spec_t sq_bl33_spec = {
+	.uuid = UUID_NON_TRUSTED_FIRMWARE_BL33,
+};
+
+#if TRUSTED_BOARD_BOOT
+static const io_uuid_spec_t sq_tb_fw_cert_spec = {
+	.uuid = UUID_TRUSTED_BOOT_FW_CERT,
+};
+
+static const io_uuid_spec_t sq_trusted_key_cert_spec = {
+	.uuid = UUID_TRUSTED_KEY_CERT,
+};
+
+static const io_uuid_spec_t sq_soc_fw_key_cert_spec = {
+	.uuid = UUID_SOC_FW_KEY_CERT,
+};
+
+static const io_uuid_spec_t sq_tos_fw_key_cert_spec = {
+	.uuid = UUID_TRUSTED_OS_FW_KEY_CERT,
+};
+
+static const io_uuid_spec_t sq_nt_fw_key_cert_spec = {
+	.uuid = UUID_NON_TRUSTED_FW_KEY_CERT,
+};
+
+static const io_uuid_spec_t sq_soc_fw_cert_spec = {
+	.uuid = UUID_SOC_FW_CONTENT_CERT,
+};
+
+static const io_uuid_spec_t sq_tos_fw_cert_spec = {
+	.uuid = UUID_TRUSTED_OS_FW_CONTENT_CERT,
+};
+
+static const io_uuid_spec_t sq_nt_fw_cert_spec = {
+	.uuid = UUID_NON_TRUSTED_FW_CONTENT_CERT,
+};
+#endif /* TRUSTED_BOARD_BOOT */
+
+struct sq_io_policy {
+	uintptr_t *dev_handle;
+	uintptr_t image_spec;
+	uintptr_t init_params;
+};
+
+static const struct sq_io_policy sq_io_policies[] = {
+	[FIP_IMAGE_ID] = {
+		.dev_handle = &sq_backend_dev_handle,
+		.image_spec = (uintptr_t)&sq_fip_spec,
+	},
+	[BL2_IMAGE_ID] = {
+		.dev_handle = &sq_fip_dev_handle,
+		.image_spec = (uintptr_t)&sq_bl2_spec,
+		.init_params = FIP_IMAGE_ID,
+	},
+	[BL31_IMAGE_ID] = {
+		.dev_handle = &sq_fip_dev_handle,
+		.image_spec = (uintptr_t)&sq_bl31_spec,
+		.init_params = FIP_IMAGE_ID,
+	},
+	[BL32_IMAGE_ID] = {
+		.dev_handle = &sq_fip_dev_handle,
+		.image_spec = (uintptr_t)&sq_bl32_spec,
+		.init_params = FIP_IMAGE_ID,
+	},
+	[BL33_IMAGE_ID] = {
+		.dev_handle = &sq_fip_dev_handle,
+		.image_spec = (uintptr_t)&sq_bl33_spec,
+		.init_params = FIP_IMAGE_ID,
+	},
+#if TRUSTED_BOARD_BOOT
+	[TRUSTED_BOOT_FW_CERT_ID] = {
+		.dev_handle = &sq_fip_dev_handle,
+		.image_spec = (uintptr_t)&sq_tb_fw_cert_spec,
+		.init_params = FIP_IMAGE_ID,
+	},
+	[TRUSTED_KEY_CERT_ID] = {
+		.dev_handle = &sq_fip_dev_handle,
+		.image_spec = (uintptr_t)&sq_trusted_key_cert_spec,
+		.init_params = FIP_IMAGE_ID,
+	},
+	[SOC_FW_KEY_CERT_ID] = {
+		.dev_handle = &sq_fip_dev_handle,
+		.image_spec = (uintptr_t)&sq_soc_fw_key_cert_spec,
+		.init_params = FIP_IMAGE_ID,
+	},
+	[TRUSTED_OS_FW_KEY_CERT_ID] = {
+		.dev_handle = &sq_fip_dev_handle,
+		.image_spec = (uintptr_t)&sq_tos_fw_key_cert_spec,
+		.init_params = FIP_IMAGE_ID,
+	},
+	[NON_TRUSTED_FW_KEY_CERT_ID] = {
+		.dev_handle = &sq_fip_dev_handle,
+		.image_spec = (uintptr_t)&sq_nt_fw_key_cert_spec,
+		.init_params = FIP_IMAGE_ID,
+	},
+	[SOC_FW_CONTENT_CERT_ID] = {
+		.dev_handle = &sq_fip_dev_handle,
+		.image_spec = (uintptr_t)&sq_soc_fw_cert_spec,
+		.init_params = FIP_IMAGE_ID,
+	},
+	[TRUSTED_OS_FW_CONTENT_CERT_ID] = {
+		.dev_handle = &sq_fip_dev_handle,
+		.image_spec = (uintptr_t)&sq_tos_fw_cert_spec,
+		.init_params = FIP_IMAGE_ID,
+	},
+	[NON_TRUSTED_FW_CONTENT_CERT_ID] = {
+		.dev_handle = &sq_fip_dev_handle,
+		.image_spec = (uintptr_t)&sq_nt_fw_cert_spec,
+		.init_params = FIP_IMAGE_ID,
+	},
+#endif
+};
+
+static int sq_update_fip_spec(void)
+{
+	uint32_t boot_index;
+	int ret;
+
+	ret = mmap_add_dynamic_region(PLAT_SQ_BOOTIDX_BASE, PLAT_SQ_BOOTIDX_BASE,
+				      PAGE_SIZE, MT_RO_DATA | MT_SECURE);
+	if (ret) {
+		return ret;
+	}
+
+	boot_index = mmio_read_32(PLAT_SQ_BOOTIDX_BASE);
+	if (boot_index < PLAT_SQ_MAX_BOOT_INDEX) {
+		sq_fip_spec.offset += PLAT_SQ_FIP_MAXSIZE * boot_index;
+		INFO("FWU Enabled: boot_index %d\n", boot_index);
+	} else {
+		WARN("FWU Disabled: wrong boot_index value. Fallback to index 0.\n");
+	}
+
+	mmap_remove_dynamic_region(PLAT_SQ_BOOTIDX_BASE, PAGE_SIZE);
+	return 0;
+}
+
+static int sq_io_memmap_setup(void)
+{
+	int ret;
+
+	ret = sq_update_fip_spec();
+	if (ret) {
+		return ret;
+	}
+
+	ret = mmap_add_dynamic_region(sq_fip_spec.offset, sq_fip_spec.offset,
+				      sq_fip_spec.length, MT_RO_DATA | MT_SECURE);
+	if (ret) {
+		return ret;
+	}
+
+	ret = register_io_dev_memmap(&sq_backend_dev_con);
+	if (ret) {
+		return ret;
+	}
+
+	return io_dev_open(sq_backend_dev_con, 0, &sq_backend_dev_handle);
+}
+
+static int sq_io_fip_setup(void)
+{
+	int ret;
+
+	ret = register_io_dev_fip(&sq_fip_dev_con);
+	if (ret) {
+		return ret;
+	}
+
+	return io_dev_open(sq_fip_dev_con, 0, &sq_fip_dev_handle);
+}
+
+int sq_io_setup(void)
+{
+	int ret;
+
+	ret = sq_io_memmap_setup();
+	if (ret) {
+		return ret;
+	}
+
+	ret = sq_io_fip_setup();
+	if (ret) {
+		return ret;
+	}
+
+	return 0;
+}
+
+int plat_get_image_source(unsigned int image_id, uintptr_t *dev_handle,
+			  uintptr_t *image_spec)
+{
+	uintptr_t init_params;
+
+	assert(image_id < ARRAY_SIZE(sq_io_policies));
+
+	*dev_handle = *sq_io_policies[image_id].dev_handle;
+	*image_spec = sq_io_policies[image_id].image_spec;
+	init_params = sq_io_policies[image_id].init_params;
+
+	return io_dev_init(*dev_handle, init_params);
+}

diff --git a/plat/socionext/synquacer/sq_psci.c b/plat/socionext/synquacer/sq_psci.c
index 3062f63..017516f 100644
--- a/plat/socionext/synquacer/sq_psci.c
+++ b/plat/socionext/synquacer/sq_psci.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -197,9 +197,17 @@
 int plat_setup_psci_ops(uintptr_t sec_entrypoint,
 			const struct plat_psci_ops **psci_ops)
 {
+#if !RESET_TO_BL31
+	uintptr_t *sq_sec_ep = (uintptr_t *)BL2_MAILBOX_BASE;
+
+	*sq_sec_ep = sec_entrypoint;
+	flush_dcache_range((uint64_t)sq_sec_ep,
+			   sizeof(*sq_sec_ep));
+#else
 	sq_sec_entrypoint = sec_entrypoint;
 	flush_dcache_range((uint64_t)&sq_sec_entrypoint,
 			   sizeof(sq_sec_entrypoint));
+#endif
 
 	*psci_ops = &sq_psci_ops;
 

diff --git a/plat/socionext/synquacer/sq_rotpk.S b/plat/socionext/synquacer/sq_rotpk.S
new file mode 100644
index 0000000..61227ed
--- /dev/null
+++ b/plat/socionext/synquacer/sq_rotpk.S

@@ -0,0 +1,16 @@
+/*
+ * Copyright (c) 2022, Socionext Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+	.global sq_rotpk_hash
+	.global sq_rotpk_hash_end
+	.section .rodata.sq_rotpk_hash, "a"
+sq_rotpk_hash:
+	/* DER header */
+	.byte 0x30, 0x31, 0x30, 0x0D, 0x06, 0x09, 0x60, 0x86, 0x48
+	.byte 0x01, 0x65, 0x03, 0x04, 0x02, 0x01, 0x05, 0x00, 0x04, 0x20
+	/* SHA256 */
+	.incbin ROTPK_HASH
+sq_rotpk_hash_end:

diff --git a/plat/socionext/synquacer/sq_tbbr.c b/plat/socionext/synquacer/sq_tbbr.c
new file mode 100644
index 0000000..e9fa18c
--- /dev/null
+++ b/plat/socionext/synquacer/sq_tbbr.c

@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2022, Socionext Inc. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <plat/common/platform.h>
+
+extern char sq_rotpk_hash[], sq_rotpk_hash_end[];
+
+int plat_get_rotpk_info(void *cookie, void **key_ptr, unsigned int *key_len,
+			unsigned int *flags)
+{
+	*key_ptr = sq_rotpk_hash;
+	*key_len = sq_rotpk_hash_end - sq_rotpk_hash;
+	*flags = ROTPK_IS_HASH;
+
+	return 0;
+}
+
+int plat_get_nv_ctr(void *cookie, unsigned int *nv_ctr)
+{
+	/*
+	 * No support for non-volatile counter.  Update the ROT key to protect
+	 * the system against rollback.
+	 */
+	*nv_ctr = 0;
+
+	return 0;
+}
+
+int plat_set_nv_ctr(void *cookie, unsigned int nv_ctr)
+{
+	return 0;
+}
+
+int plat_get_mbedtls_heap(void **heap_addr, size_t *heap_size)
+{
+	return get_mbedtls_heap_helper(heap_addr, heap_size);
+}

diff --git a/plat/st/common/bl2_io_storage.c b/plat/st/common/bl2_io_storage.c
index 5cc3390..94c36d9 100644
--- a/plat/st/common/bl2_io_storage.c
+++ b/plat/st/common/bl2_io_storage.c

@@ -38,6 +38,7 @@
 #include <platform_def.h>
 #include <stm32cubeprogrammer.h>
 #include <stm32mp_fconf_getter.h>
+#include <stm32mp_io_storage.h>
 #include <usb_dfu.h>
 
 /* IO devices */
@@ -121,6 +122,37 @@
 	return io_dev_init(storage_dev_handle, 0);
 }
 
+#if STM32MP_EMMC_BOOT
+static uint32_t get_boot_part_fip_header(void)
+{
+	io_block_spec_t emmc_boot_fip_block_spec = {
+		.offset = STM32MP_EMMC_BOOT_FIP_OFFSET,
+		.length = MMC_BLOCK_SIZE, /* We are interested only in first 4 bytes */
+	};
+	uint32_t magic = 0U;
+	int io_result;
+	size_t bytes_read;
+	uintptr_t fip_hdr_handle;
+
+	io_result = io_open(storage_dev_handle, (uintptr_t)&emmc_boot_fip_block_spec,
+			    &fip_hdr_handle);
+	assert(io_result == 0);
+
+	io_result = io_read(fip_hdr_handle, (uintptr_t)&magic, sizeof(magic),
+			    &bytes_read);
+	if ((io_result != 0) || (bytes_read != sizeof(magic))) {
+		panic();
+	}
+
+	io_close(fip_hdr_handle);
+
+	VERBOSE("%s: eMMC boot magic at offset 256K: %08x\n",
+		__func__, magic);
+
+	return magic;
+}
+#endif
+
 static void print_boot_device(boot_api_context_t *boot_context)
 {
 	switch (boot_context->boot_interface_selected) {
@@ -194,7 +226,7 @@
 		panic();
 	}
 
-	/* Open MMC as a block device to read GPT table */
+	/* Open MMC as a block device to read FIP */
 	io_result = register_io_dev_block(&mmc_dev_con);
 	if (io_result != 0) {
 		panic();
@@ -203,6 +235,25 @@
 	io_result = io_dev_open(mmc_dev_con, (uintptr_t)&mmc_block_dev_spec,
 				&storage_dev_handle);
 	assert(io_result == 0);
+
+#if STM32MP_EMMC_BOOT
+	if (mmc_dev_type == MMC_IS_EMMC) {
+		io_result = mmc_part_switch_current_boot();
+		assert(io_result == 0);
+
+		if (get_boot_part_fip_header() != TOC_HEADER_NAME) {
+			WARN("%s: Can't find FIP header on eMMC boot partition. Trying GPT\n",
+			     __func__);
+			io_result = mmc_part_switch_user();
+			assert(io_result == 0);
+			return;
+		}
+
+		VERBOSE("%s: FIP header found on eMMC boot partition\n",
+			__func__);
+		image_block_spec.offset = STM32MP_EMMC_BOOT_FIP_OFFSET;
+	}
+#endif
 }
 #endif /* STM32MP_SDMMC || STM32MP_EMMC */
 
@@ -384,8 +435,14 @@
 
 	switch (boot_itf) {
 #if STM32MP_SDMMC || STM32MP_EMMC
-	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_SD:
 	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_EMMC:
+#if STM32MP_EMMC_BOOT
+		if (image_block_spec.offset == STM32MP_EMMC_BOOT_FIP_OFFSET) {
+			break;
+		}
+#endif
+		/* fallthrough */
+	case BOOT_API_CTX_BOOT_INTERFACE_SEL_FLASH_SD:
 		if (!gpt_init_done) {
 /*
  * With FWU Multi Bank feature enabled, the selection of
@@ -409,6 +466,7 @@
 			gpt_init_done = true;
 		} else {
 			bl_mem_params_node_t *bl_mem_params = get_bl_mem_params_node(image_id);
+			assert(bl_mem_params != NULL);
 
 			mmc_block_dev_spec.buffer.offset = bl_mem_params->image_info.image_base;
 			mmc_block_dev_spec.buffer.length = bl_mem_params->image_info.image_max_size;

diff --git a/plat/st/common/include/stm32mp_common.h b/plat/st/common/include/stm32mp_common.h
index 0010cd8..79f81db 100644
--- a/plat/st/common/include/stm32mp_common.h
+++ b/plat/st/common/include/stm32mp_common.h

@@ -127,6 +127,9 @@
 void stm32_save_boot_interface(uint32_t interface, uint32_t instance);
 void stm32_get_boot_interface(uint32_t *interface, uint32_t *instance);
 
+/* Functions to save and get boot authentication status and partition used */
+void stm32_save_boot_auth(uint32_t auth_status, uint32_t boot_partition);
+
 #if !STM32MP_USE_STM32IMAGE && PSA_FWU_SUPPORT
 void stm32mp1_fwu_set_boot_idx(void);
 uint32_t stm32_get_and_dec_fwu_trial_boot_cnt(void);

diff --git a/plat/st/stm32mp1/bl2_plat_setup.c b/plat/st/stm32mp1/bl2_plat_setup.c
index 88d0f8a..5015f7d 100644
--- a/plat/st/stm32mp1/bl2_plat_setup.c
+++ b/plat/st/stm32mp1/bl2_plat_setup.c

@@ -316,6 +316,8 @@
 
 	stm32_save_boot_interface(boot_context->boot_interface_selected,
 				  boot_context->boot_interface_instance);
+	stm32_save_boot_auth(boot_context->auth_status,
+			     boot_context->boot_partition_used_toboot);
 
 #if STM32MP_USB_PROGRAMMER && STM32MP15
 	/* Deconfigure all UART RX pins configured by ROM code */
@@ -432,7 +434,8 @@
 #if !STM32MP_USE_STM32IMAGE
 	case FW_CONFIG_ID:
 		/* Set global DTB info for fixed fw_config information */
-		set_config_info(STM32MP_FW_CONFIG_BASE, STM32MP_FW_CONFIG_MAX_SIZE, FW_CONFIG_ID);
+		set_config_info(STM32MP_FW_CONFIG_BASE, ~0UL, STM32MP_FW_CONFIG_MAX_SIZE,
+				FW_CONFIG_ID);
 		fconf_populate("FW_CONFIG", STM32MP_FW_CONFIG_BASE);
 
 		idx = dyn_cfg_dtb_info_get_index(TOS_FW_CONFIG_ID);
@@ -462,16 +465,20 @@
 
 				/* In case of OPTEE, initialize address space with tos_fw addr */
 				pager_mem_params = get_bl_mem_params_node(BL32_EXTRA1_IMAGE_ID);
+				assert(pager_mem_params != NULL);
 				pager_mem_params->image_info.image_base = config_info->config_addr;
 				pager_mem_params->image_info.image_max_size =
 					config_info->config_max_size;
 
 				/* Init base and size for pager if exist */
 				paged_mem_params = get_bl_mem_params_node(BL32_EXTRA2_IMAGE_ID);
-				paged_mem_params->image_info.image_base = STM32MP_DDR_BASE +
-					(dt_get_ddr_size() - STM32MP_DDR_S_SIZE -
-					 STM32MP_DDR_SHMEM_SIZE);
-				paged_mem_params->image_info.image_max_size = STM32MP_DDR_S_SIZE;
+				if (paged_mem_params != NULL) {
+					paged_mem_params->image_info.image_base = STM32MP_DDR_BASE +
+						(dt_get_ddr_size() - STM32MP_DDR_S_SIZE -
+						 STM32MP_DDR_SHMEM_SIZE);
+					paged_mem_params->image_info.image_max_size =
+						STM32MP_DDR_S_SIZE;
+				}
 				break;
 
 			case BL33_IMAGE_ID:
@@ -491,11 +498,17 @@
 
 	case BL32_IMAGE_ID:
 		if (optee_header_is_valid(bl_mem_params->image_info.image_base)) {
+			image_info_t *paged_image_info = NULL;
+
 			/* BL32 is OP-TEE header */
 			bl_mem_params->ep_info.pc = bl_mem_params->image_info.image_base;
 			pager_mem_params = get_bl_mem_params_node(BL32_EXTRA1_IMAGE_ID);
+			assert(pager_mem_params != NULL);
+
 			paged_mem_params = get_bl_mem_params_node(BL32_EXTRA2_IMAGE_ID);
-			assert((pager_mem_params != NULL) && (paged_mem_params != NULL));
+			if (paged_mem_params != NULL) {
+				paged_image_info = &paged_mem_params->image_info;
+			}
 
 #if STM32MP_USE_STM32IMAGE && defined(AARCH32_SP_OPTEE)
 			/* Set OP-TEE extra image load areas at run-time */
@@ -511,20 +524,27 @@
 
 			err = parse_optee_header(&bl_mem_params->ep_info,
 						 &pager_mem_params->image_info,
-						 &paged_mem_params->image_info);
-			if (err) {
+						 paged_image_info);
+			if (err != 0) {
 				ERROR("OPTEE header parse error.\n");
 				panic();
 			}
 
 			/* Set optee boot info from parsed header data */
-			bl_mem_params->ep_info.args.arg0 = paged_mem_params->image_info.image_base;
-			bl_mem_params->ep_info.args.arg1 = 0; /* Unused */
-			bl_mem_params->ep_info.args.arg2 = 0; /* No DT supported */
+			if (paged_mem_params != NULL) {
+				bl_mem_params->ep_info.args.arg0 =
+					paged_mem_params->image_info.image_base;
+			} else {
+				bl_mem_params->ep_info.args.arg0 = 0U;
+			}
+
+			bl_mem_params->ep_info.args.arg1 = 0U; /* Unused */
+			bl_mem_params->ep_info.args.arg2 = 0U; /* No DT supported */
 		} else {
 #if !STM32MP_USE_STM32IMAGE
 			bl_mem_params->ep_info.pc = bl_mem_params->image_info.image_base;
 			tos_fw_mem_params = get_bl_mem_params_node(TOS_FW_CONFIG_ID);
+			assert(tos_fw_mem_params != NULL);
 			bl_mem_params->image_info.image_max_size +=
 				tos_fw_mem_params->image_info.image_max_size;
 #endif /* !STM32MP_USE_STM32IMAGE */

diff --git a/plat/st/stm32mp1/plat_bl2_mem_params_desc.c b/plat/st/stm32mp1/plat_bl2_mem_params_desc.c
index 7963c4a..9ca0930 100644
--- a/plat/st/stm32mp1/plat_bl2_mem_params_desc.c
+++ b/plat/st/stm32mp1/plat_bl2_mem_params_desc.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -68,6 +68,7 @@
 
 		.next_handoff_image_id = INVALID_IMAGE_ID,
 	},
+#if STM32MP15
 	/* Fill BL32 external 2 image related information */
 	{
 		.image_id = BL32_EXTRA2_IMAGE_ID,
@@ -82,6 +83,7 @@
 
 		.next_handoff_image_id = INVALID_IMAGE_ID,
 	},
+#endif
 
 	/* Fill HW_CONFIG related information if it exists */
 	{

diff --git a/plat/st/stm32mp1/plat_image_load.c b/plat/st/stm32mp1/plat_image_load.c
index 36a3a1c..f68eb38 100644
--- a/plat/st/stm32mp1/plat_image_load.c
+++ b/plat/st/stm32mp1/plat_image_load.c

@@ -1,14 +1,16 @@
 /*
- * Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <platform_def.h>
+#include <assert.h>
 
 #include <common/desc_image_load.h>
 #include <plat/common/platform.h>
 
+#include <platform_def.h>
+
 /*******************************************************************************
  * This function flushes the data structures so that they are visible
  * in memory for the next BL image.
@@ -27,6 +29,8 @@
 	bl_mem_params_node_t *bl33 = get_bl_mem_params_node(BL33_IMAGE_ID);
 	uint32_t ddr_ns_size = stm32mp_get_ddr_ns_size();
 
+	assert(bl33 != NULL);
+
 	/* Max size is non-secure DDR end address minus image_base */
 	bl33->image_info.image_max_size = STM32MP_DDR_BASE + ddr_ns_size -
 					  bl33->image_info.image_base;

diff --git a/plat/st/stm32mp1/platform.mk b/plat/st/stm32mp1/platform.mk
index 9e67989..a903a16 100644
--- a/plat/st/stm32mp1/platform.mk
+++ b/plat/st/stm32mp1/platform.mk

@@ -65,6 +65,10 @@
 # STM32 image header version v1.0
 STM32_HEADER_VERSION_MAJOR:=	1
 STM32_HEADER_VERSION_MINOR:=	0
+
+# Add OP-TEE reserved shared memory area in mapping
+STM32MP15_OPTEE_RSV_SHM	:=	1
+$(eval $(call add_defines,STM32MP15_OPTEE_RSV_SHM))
 endif
 
 # STM32 image header binary type for BL2
@@ -320,12 +324,14 @@
 				plat/st/stm32mp1/stm32mp1_security.c
 endif
 
-ifeq (${PSA_FWU_SUPPORT},1)
 include lib/zlib/zlib.mk
+
+ifeq (${PSA_FWU_SUPPORT},1)
 include drivers/fwu/fwu.mk
+endif
+
 
 BL2_SOURCES		+=	$(ZLIB_SOURCES)
-endif
 
 BL2_SOURCES		+=	drivers/io/io_block.c					\
 				drivers/io/io_mtd.c					\

diff --git a/plat/st/stm32mp1/stm32mp1_def.h b/plat/st/stm32mp1/stm32mp1_def.h
index 7e0745a..8ca6123 100644
--- a/plat/st/stm32mp1/stm32mp1_def.h
+++ b/plat/st/stm32mp1/stm32mp1_def.h

@@ -182,7 +182,12 @@
  #endif
 #endif
 
+#if STM32MP13
+#define STM32MP_BL33_BASE		STM32MP_DDR_BASE
+#endif
+#if STM32MP15
 #define STM32MP_BL33_BASE		(STM32MP_DDR_BASE + U(0x100000))
+#endif
 #define STM32MP_BL33_MAX_SIZE		U(0x400000)
 
 /* Define maximum page size for NAND devices */
@@ -247,8 +252,14 @@
 /*******************************************************************************
  * STM32MP1 UART
  ******************************************************************************/
+#if STM32MP13
+#define USART1_BASE			U(0x4C000000)
+#define USART2_BASE			U(0x4C001000)
+#endif
+#if STM32MP15
 #define USART1_BASE			U(0x5C000000)
 #define USART2_BASE			U(0x4000E000)
+#endif
 #define USART3_BASE			U(0x4000F000)
 #define UART4_BASE			U(0x40010000)
 #define UART5_BASE			U(0x40011000)

diff --git a/plat/st/stm32mp1/stm32mp1_fip_def.h b/plat/st/stm32mp1/stm32mp1_fip_def.h
index 7a277fd..82e53db 100644
--- a/plat/st/stm32mp1/stm32mp1_fip_def.h
+++ b/plat/st/stm32mp1/stm32mp1_fip_def.h

@@ -7,8 +7,13 @@
 #ifndef STM32MP1_FIP_DEF_H
 #define STM32MP1_FIP_DEF_H
 
+#if STM32MP15_OPTEE_RSV_SHM
 #define STM32MP_DDR_S_SIZE		U(0x01E00000)	/* 30 MB */
 #define STM32MP_DDR_SHMEM_SIZE		U(0x00200000)	/* 2 MB */
+#else
+#define STM32MP_DDR_S_SIZE		U(0x02000000)	/* 32 MB */
+#define STM32MP_DDR_SHMEM_SIZE		U(0)		/* empty */
+#endif
 
 #if STM32MP13
 #define STM32MP_BL2_RO_SIZE		U(0x00015000)	/* 84 KB */
@@ -98,8 +103,9 @@
 #endif
 
 /*******************************************************************************
- * STM32MP1 RAW partition offset for MTD devices
+ * STM32MP1 RAW partition offset for devices without GPT
  ******************************************************************************/
+#define STM32MP_EMMC_BOOT_FIP_OFFSET	U(0x00040000)
 #define STM32MP_NOR_FIP_OFFSET		U(0x00080000)
 #define STM32MP_NAND_FIP_OFFSET		U(0x00200000)
 

diff --git a/plat/st/stm32mp1/stm32mp1_private.c b/plat/st/stm32mp1/stm32mp1_private.c
index 1617afd..86b9f23 100644
--- a/plat/st/stm32mp1/stm32mp1_private.c
+++ b/plat/st/stm32mp1/stm32mp1_private.c

@@ -43,8 +43,10 @@
 #if STM32MP15
 #define TAMP_BOOT_MODE_BACKUP_REG_ID	U(20)
 #endif
-#define TAMP_BOOT_MODE_ITF_MASK		U(0x0000FF00)
+#define TAMP_BOOT_MODE_ITF_MASK		GENMASK(15, 8)
 #define TAMP_BOOT_MODE_ITF_SHIFT	8
+#define TAMP_BOOT_MODE_AUTH_MASK	GENMASK(23, 16)
+#define TAMP_BOOT_MODE_AUTH_SHIFT	16
 
 /*
  * Backup register to store fwu update information.
@@ -52,9 +54,9 @@
  * (so it should be in Zone 2).
  */
 #define TAMP_BOOT_FWU_INFO_REG_ID	U(10)
-#define TAMP_BOOT_FWU_INFO_IDX_MSK	U(0xF)
+#define TAMP_BOOT_FWU_INFO_IDX_MSK	GENMASK(3, 0)
 #define TAMP_BOOT_FWU_INFO_IDX_OFF	U(0)
-#define TAMP_BOOT_FWU_INFO_CNT_MSK	U(0xF0)
+#define TAMP_BOOT_FWU_INFO_CNT_MSK	GENMASK(7, 4)
 #define TAMP_BOOT_FWU_INFO_CNT_OFF	U(4)
 
 #if defined(IMAGE_BL2)
@@ -199,6 +201,8 @@
 
 int stm32_get_gpio_bank_pinctrl_node(void *fdt, unsigned int bank)
 {
+	const char *node_compatible = NULL;
+
 	switch (bank) {
 	case GPIO_BANK_A:
 	case GPIO_BANK_B:
@@ -209,18 +213,24 @@
 	case GPIO_BANK_G:
 	case GPIO_BANK_H:
 	case GPIO_BANK_I:
+#if STM32MP13
+		node_compatible = "st,stm32mp135-pinctrl";
+		break;
+#endif
 #if STM32MP15
 	case GPIO_BANK_J:
 	case GPIO_BANK_K:
-#endif
-		return fdt_path_offset(fdt, "/soc/pin-controller");
-#if STM32MP15
+		node_compatible = "st,stm32mp157-pinctrl";
+		break;
 	case GPIO_BANK_Z:
-		return fdt_path_offset(fdt, "/soc/pin-controller-z");
+		node_compatible = "st,stm32mp157-z-pinctrl";
+		break;
 #endif
 	default:
 		panic();
 	}
+
+	return fdt_node_offset_by_compatible(fdt, -1, node_compatible);
 }
 
 #if STM32MP_UART_PROGRAMMER || !defined(IMAGE_BL2)
@@ -738,6 +748,20 @@
 	*instance = itf & 0xFU;
 }
 
+void stm32_save_boot_auth(uint32_t auth_status, uint32_t boot_partition)
+{
+	uint32_t boot_status = tamp_bkpr(TAMP_BOOT_MODE_BACKUP_REG_ID);
+
+	clk_enable(RTCAPB);
+
+	mmio_clrsetbits_32(boot_status,
+			   TAMP_BOOT_MODE_AUTH_MASK,
+			   ((auth_status << 4) | (boot_partition & 0xFU)) <<
+			   TAMP_BOOT_MODE_AUTH_SHIFT);
+
+	clk_disable(RTCAPB);
+}
+
 #if !STM32MP_USE_STM32IMAGE && PSA_FWU_SUPPORT
 void stm32mp1_fwu_set_boot_idx(void)
 {

diff --git a/plat/st/stm32mp1/stm32mp1_stm32image_def.h b/plat/st/stm32mp1/stm32mp1_stm32image_def.h
index 8efa342..6260cb9 100644
--- a/plat/st/stm32mp1/stm32mp1_stm32image_def.h
+++ b/plat/st/stm32mp1/stm32mp1_stm32image_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2021, STMicroelectronics - All Rights Reserved
+ * Copyright (C) 2021-2022, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -8,9 +8,14 @@
 #define STM32MP1_STM32IMAGE_DEF_H
 
 #ifdef AARCH32_SP_OPTEE
+#if STM32MP15_OPTEE_RSV_SHM
 #define STM32MP_DDR_S_SIZE		U(0x01E00000)	/* 30 MB */
 #define STM32MP_DDR_SHMEM_SIZE		U(0x00200000)	/* 2 MB */
 #else
+#define STM32MP_DDR_S_SIZE		U(0x02000000)	/* 32 MB */
+#define STM32MP_DDR_SHMEM_SIZE		U(0)		/* empty */
+#endif
+#else
 #define STM32MP_DDR_S_SIZE		U(0)
 #define STM32MP_DDR_SHMEM_SIZE		U(0)
 #endif

diff --git a/plat/ti/k3/board/lite/include/board_def.h b/plat/ti/k3/board/lite/include/board_def.h
index 18b7f42..fd4e5b1 100644
--- a/plat/ti/k3/board/lite/include/board_def.h
+++ b/plat/ti/k3/board/lite/include/board_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -33,7 +33,7 @@
  * defined as default for our platform.
  */
 #define SEC_SRAM_BASE			UL(0x00000000) /* PIE remapped on fly */
-#define SEC_SRAM_SIZE			UL(0x0001c000) /* 112k */
+#define SEC_SRAM_SIZE			UL(0x00020000) /* 128k */
 
 #define PLAT_MAX_OFF_STATE		U(2)
 #define PLAT_MAX_RET_STATE		U(1)

diff --git a/plat/ti/k3/common/drivers/ti_sci/ti_sci.c b/plat/ti/k3/common/drivers/ti_sci/ti_sci.c
index 2c3313c..2cbfa3d 100644
--- a/plat/ti/k3/common/drivers/ti_sci/ti_sci.c
+++ b/plat/ti/k3/common/drivers/ti_sci/ti_sci.c

@@ -2,7 +2,7 @@
  * Texas Instruments System Control Interface Driver
  *   Based on Linux and U-Boot implementation
  *
- * Copyright (C) 2018 Texas Instruments Incorporated - http://www.ti.com/
+ * Copyright (C) 2018-2022 Texas Instruments Incorporated - https://www.ti.com/
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -1664,6 +1664,57 @@
 }
 
 /**
+ * ti_sci_enter_sleep - Command to initiate system transition into suspend.
+ *
+ * @proc_id: Processor ID.
+ * @mode: Low power mode to enter.
+ * @core_resume_addr: Address that core should be
+ *		      resumed from after low power transition.
+ *
+ * Return: 0 if all goes well, else appropriate error message
+ */
+int ti_sci_enter_sleep(uint8_t proc_id,
+		       uint8_t mode,
+		       uint64_t core_resume_addr)
+{
+	struct ti_sci_msg_req_enter_sleep req;
+	struct ti_sci_msg_hdr *hdr;
+	struct k3_sec_proxy_msg tx_message;
+	int ret;
+
+	/* Ensure we have sane transfer size */
+	if (sizeof(req) > TI_SCI_MAX_MESSAGE_SIZE) {
+		return -ERANGE;
+	}
+
+	hdr = (struct ti_sci_msg_hdr *)&req;
+	hdr->seq = ++message_sequence;
+	hdr->type = TI_SCI_MSG_ENTER_SLEEP;
+	hdr->host = TI_SCI_HOST_ID;
+	/* Setup with NORESPONSE flag to keep response queue clean */
+	hdr->flags = TI_SCI_FLAG_REQ_GENERIC_NORESPONSE;
+
+	req.processor_id = proc_id;
+	req.mode = mode;
+	req.core_resume_lo = core_resume_addr & TISCI_ADDR_LOW_MASK;
+	req.core_resume_hi = (core_resume_addr & TISCI_ADDR_HIGH_MASK) >>
+			     TISCI_ADDR_HIGH_SHIFT;
+
+	tx_message.buf = (uint8_t *)&req;
+	tx_message.len = sizeof(req);
+
+	/* Send message */
+	ret = k3_sec_proxy_send(SP_HIGH_PRIORITY, &tx_message);
+	if (ret != 0) {
+		ERROR("Message sending failed (%d)\n", ret);
+		return ret;
+	}
+
+	/* Return without waiting for response */
+	return 0;
+}
+
+/**
  * ti_sci_init() - Basic initialization
  *
  * Return: 0 if all goes well, else appropriate error message

diff --git a/plat/ti/k3/common/drivers/ti_sci/ti_sci.h b/plat/ti/k3/common/drivers/ti_sci/ti_sci.h
index c7b09b3..06944a7 100644
--- a/plat/ti/k3/common/drivers/ti_sci/ti_sci.h
+++ b/plat/ti/k3/common/drivers/ti_sci/ti_sci.h

@@ -2,7 +2,7 @@
  * Texas Instruments System Control Interface API
  *   Based on Linux and U-Boot implementation
  *
- * Copyright (C) 2018 Texas Instruments Incorporated - http://www.ti.com/
+ * Copyright (C) 2018-2022 Texas Instruments Incorporated - https://www.ti.com/
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -207,6 +207,22 @@
 					 uint32_t status_flags_1_clr_any_wait);
 
 /**
+ * System Low Power Operations
+ *
+ * - ti_sci_enter_sleep - Command to initiate system transition into suspend.
+ *		@proc_id: Processor ID.
+ *		@mode: Low power mode to enter.
+ *		@core_resume_addr: Address that core should be resumed from
+ *				   after low power transition.
+ *
+ * NOTE: for all these functions, the following are generic in nature:
+ * Returns 0 for successful request, else returns corresponding error message.
+ */
+int ti_sci_enter_sleep(uint8_t proc_id,
+		       uint8_t mode,
+		       uint64_t core_resume_addr);
+
+/**
  * ti_sci_init() - Basic initialization
  *
  * Return: 0 if all goes good, else appropriate error message.

diff --git a/plat/ti/k3/common/drivers/ti_sci/ti_sci_protocol.h b/plat/ti/k3/common/drivers/ti_sci/ti_sci_protocol.h
index 310bf45..d220612 100644
--- a/plat/ti/k3/common/drivers/ti_sci/ti_sci_protocol.h
+++ b/plat/ti/k3/common/drivers/ti_sci/ti_sci_protocol.h

@@ -5,7 +5,7 @@
  * The system works in a message response protocol
  * See: http://processors.wiki.ti.com/index.php/TISCI for details
  *
- * Copyright (C) 2018 Texas Instruments Incorporated - http://www.ti.com/
+ * Copyright (C) 2018-2022 Texas Instruments Incorporated - https://www.ti.com/
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -28,6 +28,9 @@
 #define TI_SCI_MSG_GET_DEVICE_STATE	0x0201
 #define TI_SCI_MSG_SET_DEVICE_RESETS	0x0202
 
+/* Low Power Mode Requests */
+#define TI_SCI_MSG_ENTER_SLEEP		0x0301
+
 /* Clock requests */
 #define TI_SCI_MSG_SET_CLOCK_STATE	0x0100
 #define TI_SCI_MSG_GET_CLOCK_STATE	0x0101
@@ -706,4 +709,26 @@
 	uint32_t status_flags_1_clr_any_wait;
 } __packed;
 
+/**
+ * struct ti_sci_msg_req_enter_sleep - Request for TI_SCI_MSG_ENTER_SLEEP.
+ *
+ * @hdr		    Generic Header
+ * @mode	    Low power mode to enter.
+ * @proc_id	    Processor id to be restored.
+ * @core_resume_lo  Low 32-bits of physical pointer to address for core
+ *		    to begin execution upon resume.
+ * @core_resume_hi  High 32-bits of physical pointer to address for core
+ *		    to begin execution upon resume.
+ *
+ * This message is to be sent after TI_SCI_MSG_PREPARE_SLEEP is sent from OS
+ * and is what actually triggers entry into the specified low power mode.
+ */
+struct ti_sci_msg_req_enter_sleep {
+	struct ti_sci_msg_hdr hdr;
+	uint8_t mode;
+	uint8_t processor_id;
+	uint32_t core_resume_lo;
+	uint32_t core_resume_hi;
+} __packed;
+
 #endif /* TI_SCI_PROTOCOL_H */

diff --git a/plat/ti/k3/common/k3_gicv3.c b/plat/ti/k3/common/k3_gicv3.c
index 1932eaa..0199822 100644
--- a/plat/ti/k3/common/k3_gicv3.c
+++ b/plat/ti/k3/common/k3_gicv3.c

@@ -19,6 +19,11 @@
 /* The GICv3 driver only needs to be initialized in EL3 */
 uintptr_t rdistif_base_addrs[PLATFORM_CORE_COUNT];
 
+#if K3_PM_SYSTEM_SUSPEND
+static gicv3_redist_ctx_t rdist_ctx[PLATFORM_CORE_COUNT];
+static gicv3_dist_ctx_t dist_ctx;
+#endif
+
 static const interrupt_prop_t k3_interrupt_props[] = {
 	PLAT_ARM_G1S_IRQ_PROPS(INTR_GROUP1S),
 	PLAT_ARM_G0_IRQ_PROPS(INTR_GROUP0)
@@ -88,3 +93,21 @@
 {
 	gicv3_rdistif_init(plat_my_core_pos());
 }
+
+#if K3_PM_SYSTEM_SUSPEND
+void k3_gic_save_context(void)
+{
+	for (unsigned int i = 0U; i < PLATFORM_CORE_COUNT; i++) {
+		gicv3_rdistif_save(i, &rdist_ctx[i]);
+	}
+	gicv3_distif_save(&dist_ctx);
+}
+
+void k3_gic_restore_context(void)
+{
+	gicv3_distif_init_restore(&dist_ctx);
+	for (unsigned int i = 0U; i < PLATFORM_CORE_COUNT; i++) {
+		gicv3_rdistif_init_restore(i, &rdist_ctx[i]);
+	}
+}
+#endif

diff --git a/plat/ti/k3/common/k3_psci.c b/plat/ti/k3/common/k3_psci.c
index 0500740..6febbc6 100644
--- a/plat/ti/k3/common/k3_psci.c
+++ b/plat/ti/k3/common/k3_psci.c

@@ -234,11 +234,50 @@
 	return PSCI_E_SUCCESS;
 }
 
+#if K3_PM_SYSTEM_SUSPEND
+static void k3_pwr_domain_suspend(const psci_power_state_t *target_state)
+{
+	unsigned int core, proc_id;
+
+	core = plat_my_core_pos();
+	proc_id = PLAT_PROC_START_ID + core;
+
+	/* Prevent interrupts from spuriously waking up this cpu */
+	k3_gic_cpuif_disable();
+	k3_gic_save_context();
+
+	k3_pwr_domain_off(target_state);
+
+	ti_sci_enter_sleep(proc_id, 0, k3_sec_entrypoint);
+}
+
+static void k3_pwr_domain_suspend_finish(const psci_power_state_t *target_state)
+{
+	k3_gic_restore_context();
+	k3_gic_cpuif_enable();
+}
+
+static void k3_get_sys_suspend_power_state(psci_power_state_t *req_state)
+{
+	unsigned int i;
+
+	/* CPU & cluster off, system in retention */
+	for (i = MPIDR_AFFLVL0; i <= PLAT_MAX_PWR_LVL; i++) {
+		req_state->pwr_domain_state[i] = PLAT_MAX_OFF_STATE;
+	}
+}
+#endif
+
 static const plat_psci_ops_t k3_plat_psci_ops = {
 	.cpu_standby = k3_cpu_standby,
 	.pwr_domain_on = k3_pwr_domain_on,
 	.pwr_domain_off = k3_pwr_domain_off,
 	.pwr_domain_on_finish = k3_pwr_domain_on_finish,
+#if K3_PM_SYSTEM_SUSPEND
+	.pwr_domain_suspend = k3_pwr_domain_suspend,
+	.pwr_domain_suspend_finish = k3_pwr_domain_suspend_finish,
+	.get_sys_suspend_power_state = k3_get_sys_suspend_power_state,
+#endif
 	.system_off = k3_system_off,
 	.system_reset = k3_system_reset,
 	.validate_power_state = k3_validate_power_state,

diff --git a/plat/ti/k3/common/plat_common.mk b/plat/ti/k3/common/plat_common.mk
index ab7366b..e299c30 100644
--- a/plat/ti/k3/common/plat_common.mk
+++ b/plat/ti/k3/common/plat_common.mk

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -45,6 +45,10 @@
 K3_USART_BAUD		:=	115200
 $(eval $(call add_define,K3_USART_BAUD))
 
+# Enable system suspend modes
+K3_PM_SYSTEM_SUSPEND	:=	0
+$(eval $(call add_define,K3_PM_SYSTEM_SUSPEND))
+
 # Libraries
 include lib/xlat_tables_v2/xlat_tables.mk
 

diff --git a/plat/ti/k3/include/k3_gicv3.h b/plat/ti/k3/include/k3_gicv3.h
index 2329a16..2c68a75 100644
--- a/plat/ti/k3/include/k3_gicv3.h
+++ b/plat/ti/k3/include/k3_gicv3.h

@@ -14,5 +14,7 @@
 void k3_gic_cpuif_enable(void);
 void k3_gic_cpuif_disable(void);
 void k3_gic_pcpu_init(void);
+void k3_gic_save_context(void);
+void k3_gic_restore_context(void);
 
 #endif /* K3_GICV3_H */

diff --git a/plat/xilinx/common/include/ipi.h b/plat/xilinx/common/include/ipi.h
index 483902e..ac76bf0 100644
--- a/plat/xilinx/common/include/ipi.h
+++ b/plat/xilinx/common/include/ipi.h

@@ -47,7 +47,7 @@
  ********************************************************************/
 
 /* Initialize IPI configuration table */
-void ipi_config_table_init(const struct ipi_config *ipi_table,
+void ipi_config_table_init(const struct ipi_config *ipi_config_table,
 			   uint32_t total_ipi);
 
 /* Validate IPI mailbox access */

diff --git a/plat/xilinx/common/include/plat_startup.h b/plat/xilinx/common/include/plat_startup.h
index 66e7933..6799e21 100644
--- a/plat/xilinx/common/include/plat_startup.h
+++ b/plat/xilinx/common/include/plat_startup.h

@@ -15,8 +15,8 @@
 	FSBL_HANDOFF_TOO_MANY_PARTS
 };
 
-enum fsbl_handoff fsbl_atf_handover(entry_point_info_t *bl32_image_ep_info,
-					entry_point_info_t *bl33_image_ep_info,
+enum fsbl_handoff fsbl_atf_handover(entry_point_info_t *bl32,
+					entry_point_info_t *bl33,
 					uint64_t atf_handoff_addr);
 
 #endif /* PLAT_STARTUP_H */

diff --git a/plat/xilinx/common/include/pm_client.h b/plat/xilinx/common/include/pm_client.h
index e91bb8f..dc012b7 100644
--- a/plat/xilinx/common/include/pm_client.h
+++ b/plat/xilinx/common/include/pm_client.h

@@ -16,7 +16,7 @@
 #include "pm_defs.h"
 
 /* Functions to be implemented by each PU */
-void pm_client_suspend(const struct pm_proc *proc, unsigned int state);
+void pm_client_suspend(const struct pm_proc *proc, uint32_t state);
 void pm_client_abort_suspend(void);
 void pm_client_wakeup(const struct pm_proc *proc);
 
@@ -24,7 +24,6 @@
 extern const struct pm_proc *primary_proc;
 
 #ifndef VERSAL_PLATFORM
-enum pm_ret_status set_ocm_retention(void);
 enum pm_ret_status pm_set_suspend_mode(uint32_t mode);
 const struct pm_proc *pm_get_proc_by_node(enum pm_node_id nid);
 #endif

diff --git a/plat/xilinx/common/include/pm_common.h b/plat/xilinx/common/include/pm_common.h
index 0c24a36..06efa4b 100644
--- a/plat/xilinx/common/include/pm_common.h
+++ b/plat/xilinx/common/include/pm_common.h

@@ -48,10 +48,10 @@
  */
 struct pm_proc {
 	const uint32_t node_id;
-	const unsigned int pwrdn_mask;
+	const uint32_t pwrdn_mask;
 	const struct pm_ipi *ipi;
 };
 
-const struct pm_proc *pm_get_proc(unsigned int cpuid);
+const struct pm_proc *pm_get_proc(uint32_t cpuid);
 
 #endif /* PM_COMMON_H */

diff --git a/plat/xilinx/common/include/pm_ipi.h b/plat/xilinx/common/include/pm_ipi.h
index 8c7738d..2d20b9f 100644
--- a/plat/xilinx/common/include/pm_ipi.h
+++ b/plat/xilinx/common/include/pm_ipi.h

@@ -13,7 +13,7 @@
 #define IPI_BLOCKING		1
 #define IPI_NON_BLOCKING	0
 
-int pm_ipi_init(const struct pm_proc *proc);
+int32_t pm_ipi_init(const struct pm_proc *proc);
 
 enum pm_ret_status pm_ipi_send(const struct pm_proc *proc,
 			       uint32_t payload[PAYLOAD_ARG_CNT]);
@@ -21,8 +21,8 @@
 					    uint32_t payload[PAYLOAD_ARG_CNT]);
 enum pm_ret_status pm_ipi_send_sync(const struct pm_proc *proc,
 				    uint32_t payload[PAYLOAD_ARG_CNT],
-				    unsigned int *value, size_t count);
-void pm_ipi_buff_read_callb(unsigned int *value, size_t count);
+				    uint32_t *value, size_t count);
+void pm_ipi_buff_read_callb(uint32_t *value, size_t count);
 void pm_ipi_irq_enable(const struct pm_proc *proc);
 void pm_ipi_irq_clear(const struct pm_proc *proc);
 uint32_t pm_ipi_irq_status(const struct pm_proc *proc);

diff --git a/plat/xilinx/common/ipi.c b/plat/xilinx/common/ipi.c
index 0b8020b..2f52f38 100644
--- a/plat/xilinx/common/ipi.c
+++ b/plat/xilinx/common/ipi.c

@@ -69,8 +69,9 @@
 {
 	int ret = 1;
 
-	if (remote >= ipi_total || local >= ipi_total)
+	if (remote >= ipi_total || local >= ipi_total) {
 		ret = 0;
+	}
 
 	return ret;
 }
@@ -88,12 +89,15 @@
 {
 	int ret = 0;
 
-	if (!is_ipi_mb_within_range(local, remote))
+	if (!is_ipi_mb_within_range(local, remote)) {
 		ret = -EINVAL;
-	else if (IPI_IS_SECURE(local) && !is_secure)
+	} else if (IPI_IS_SECURE(local) && !is_secure) {
 		ret = -EPERM;
-	else if (IPI_IS_SECURE(remote) && !is_secure)
+	} else if (IPI_IS_SECURE(remote) && !is_secure) {
 		ret = -EPERM;
+	} else {
+		/* To fix the misra 15.7 warning */
+	}
 
 	return ret;
 }
@@ -141,11 +145,13 @@
 	uint32_t status;
 
 	status = mmio_read_32(IPI_REG_BASE(local) + IPI_OBR_OFFSET);
-	if (status & IPI_BIT_MASK(remote))
+	if (status & IPI_BIT_MASK(remote)) {
 		ret |= IPI_MB_STATUS_SEND_PENDING;
+	}
 	status = mmio_read_32(IPI_REG_BASE(local) + IPI_ISR_OFFSET);
-	if (status & IPI_BIT_MASK(remote))
+	if (status & IPI_BIT_MASK(remote)) {
 		ret |= IPI_MB_STATUS_RECV_PENDING;
+	}
 
 	return ret;
 }

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 f531158..cb6aaa5 100644
--- a/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c
+++ b/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.c

@@ -64,13 +64,13 @@
  * function with rt_svc_handle signature
  */
 uint64_t ipi_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2,
-			 uint64_t x3, uint64_t x4, void *cookie,
+			 uint64_t x3, uint64_t x4, const void *cookie,
 			 void *handle, uint64_t flags)
 {
-	int ret;
+	int32_t ret;
 	uint32_t ipi_local_id;
 	uint32_t ipi_remote_id;
-	unsigned int is_secure;
+	uint32_t is_secure;
 
 	ipi_local_id = x1 & UNSIGNED32_MASK;
 	ipi_remote_id = x2 & UNSIGNED32_MASK;
@@ -94,7 +94,7 @@
 		SMC_RET1(handle, 0);
 	case IPI_MAILBOX_STATUS_ENQUIRY:
 	{
-		int disable_irq;
+		int32_t disable_irq;
 
 		disable_irq = (x3 & IPI_SMC_ENQUIRY_DIRQ_MASK) ? 1 : 0;
 		ret = ipi_mb_enquire_status(ipi_local_id, ipi_remote_id);
@@ -112,7 +112,7 @@
 	}
 	case IPI_MAILBOX_ACK:
 	{
-		int enable_irq;
+		int32_t enable_irq;
 
 		enable_irq = (x3 & IPI_SMC_ACK_EIRQ_MASK) ? 1 : 0;
 		ipi_mb_ack(ipi_local_id, ipi_remote_id);

diff --git a/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.h b/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.h
index 10682d8..af13db9 100644
--- a/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.h
+++ b/plat/xilinx/common/ipi_mailbox_service/ipi_mailbox_svc.h

@@ -33,7 +33,7 @@
 
 /* IPI SMC handler */
 uint64_t ipi_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2,
-			 uint64_t x3, uint64_t x4, void *cookie, void *handle,
+			 uint64_t x3, uint64_t x4, const void *cookie, void *handle,
 			 uint64_t flags);
 
 #endif /* IPI_MAILBOX_SVC_H */

diff --git a/plat/xilinx/common/plat_startup.c b/plat/xilinx/common/plat_startup.c
index f02f41e..a0900c4 100644
--- a/plat/xilinx/common/plat_startup.c
+++ b/plat/xilinx/common/plat_startup.c

@@ -24,36 +24,36 @@
  * CPU#			5:6		00 -> A53_0, 01 -> A53_1, 10 -> A53_2, 11 -> A53_3
  */
 
-#define FSBL_FLAGS_ESTATE_SHIFT		0
-#define FSBL_FLAGS_ESTATE_MASK		(1 << FSBL_FLAGS_ESTATE_SHIFT)
-#define FSBL_FLAGS_ESTATE_A64		0
-#define FSBL_FLAGS_ESTATE_A32		1
+#define FSBL_FLAGS_ESTATE_SHIFT		0U
+#define FSBL_FLAGS_ESTATE_MASK		(1U << FSBL_FLAGS_ESTATE_SHIFT)
+#define FSBL_FLAGS_ESTATE_A64		0U
+#define FSBL_FLAGS_ESTATE_A32		1U
 
-#define FSBL_FLAGS_ENDIAN_SHIFT		1
-#define FSBL_FLAGS_ENDIAN_MASK		(1 << FSBL_FLAGS_ENDIAN_SHIFT)
-#define FSBL_FLAGS_ENDIAN_LE		0
-#define FSBL_FLAGS_ENDIAN_BE		1
+#define FSBL_FLAGS_ENDIAN_SHIFT		1U
+#define FSBL_FLAGS_ENDIAN_MASK		(1U << FSBL_FLAGS_ENDIAN_SHIFT)
+#define FSBL_FLAGS_ENDIAN_LE		0U
+#define FSBL_FLAGS_ENDIAN_BE		1U
 
-#define FSBL_FLAGS_TZ_SHIFT		2
-#define FSBL_FLAGS_TZ_MASK		(1 << FSBL_FLAGS_TZ_SHIFT)
-#define FSBL_FLAGS_NON_SECURE		0
-#define FSBL_FLAGS_SECURE		1
+#define FSBL_FLAGS_TZ_SHIFT		2U
+#define FSBL_FLAGS_TZ_MASK		(1U << FSBL_FLAGS_TZ_SHIFT)
+#define FSBL_FLAGS_NON_SECURE		0U
+#define FSBL_FLAGS_SECURE		1U
 
-#define FSBL_FLAGS_EL_SHIFT		3
-#define FSBL_FLAGS_EL_MASK		(3 << FSBL_FLAGS_EL_SHIFT)
-#define FSBL_FLAGS_EL0			0
-#define FSBL_FLAGS_EL1			1
-#define FSBL_FLAGS_EL2			2
-#define FSBL_FLAGS_EL3			3
+#define FSBL_FLAGS_EL_SHIFT		3U
+#define FSBL_FLAGS_EL_MASK		(3U << FSBL_FLAGS_EL_SHIFT)
+#define FSBL_FLAGS_EL0			0U
+#define FSBL_FLAGS_EL1			1U
+#define FSBL_FLAGS_EL2			2U
+#define FSBL_FLAGS_EL3			3U
 
-#define FSBL_FLAGS_CPU_SHIFT		5
-#define FSBL_FLAGS_CPU_MASK		(3 << FSBL_FLAGS_CPU_SHIFT)
-#define FSBL_FLAGS_A53_0		0
-#define FSBL_FLAGS_A53_1		1
-#define FSBL_FLAGS_A53_2		2
-#define FSBL_FLAGS_A53_3		3
+#define FSBL_FLAGS_CPU_SHIFT		5U
+#define FSBL_FLAGS_CPU_MASK		(3U << FSBL_FLAGS_CPU_SHIFT)
+#define FSBL_FLAGS_A53_0		0U
+#define FSBL_FLAGS_A53_1		1U
+#define FSBL_FLAGS_A53_2		2U
+#define FSBL_FLAGS_A53_3		3U
 
-#define FSBL_MAX_PARTITIONS		8
+#define FSBL_MAX_PARTITIONS		8U
 
 /* Structure corresponding to each partition entry */
 struct xfsbl_partition {
@@ -75,7 +75,7 @@
  *
  * Return: FSBL_FLAGS_A53_0, FSBL_FLAGS_A53_1, FSBL_FLAGS_A53_2 or FSBL_FLAGS_A53_3
  */
-static int get_fsbl_cpu(const struct xfsbl_partition *partition)
+static int32_t get_fsbl_cpu(const struct xfsbl_partition *partition)
 {
 	uint64_t flags = partition->flags & FSBL_FLAGS_CPU_MASK;
 
@@ -89,7 +89,7 @@
  *
  * Return: FSBL_FLAGS_EL0, FSBL_FLAGS_EL1, FSBL_FLAGS_EL2 or FSBL_FLAGS_EL3
  */
-static int get_fsbl_el(const struct xfsbl_partition *partition)
+static int32_t get_fsbl_el(const struct xfsbl_partition *partition)
 {
 	uint64_t flags = partition->flags & FSBL_FLAGS_EL_MASK;
 
@@ -103,7 +103,7 @@
  *
  * Return: FSBL_FLAGS_NON_SECURE or FSBL_FLAGS_SECURE
  */
-static int get_fsbl_ss(const struct xfsbl_partition *partition)
+static int32_t get_fsbl_ss(const struct xfsbl_partition *partition)
 {
 	uint64_t flags = partition->flags & FSBL_FLAGS_TZ_MASK;
 
@@ -117,16 +117,17 @@
  *
  * Return: SPSR_E_LITTLE or SPSR_E_BIG
  */
-static int get_fsbl_endian(const struct xfsbl_partition *partition)
+static int32_t get_fsbl_endian(const struct xfsbl_partition *partition)
 {
 	uint64_t flags = partition->flags & FSBL_FLAGS_ENDIAN_MASK;
 
 	flags >>= FSBL_FLAGS_ENDIAN_SHIFT;
 
-	if (flags == FSBL_FLAGS_ENDIAN_BE)
+	if (flags == FSBL_FLAGS_ENDIAN_BE) {
 		return SPSR_E_BIG;
-	else
+	} else {
 		return SPSR_E_LITTLE;
+	}
 }
 
 /**
@@ -136,7 +137,7 @@
  *
  * Return: FSBL_FLAGS_ESTATE_A32 or FSBL_FLAGS_ESTATE_A64
  */
-static int get_fsbl_estate(const struct xfsbl_partition *partition)
+static int32_t get_fsbl_estate(const struct xfsbl_partition *partition)
 {
 	uint64_t flags = partition->flags & FSBL_FLAGS_ESTATE_MASK;
 
@@ -192,8 +193,8 @@
 	 */
 	for (size_t i = 0; i < ATFHandoffParams->num_entries; i++) {
 		entry_point_info_t *image;
-		int target_estate, target_secure;
-		int target_cpu, target_endianness, target_el;
+		int32_t target_estate, target_secure, target_cpu;
+		uint32_t target_endianness, target_el;
 
 		VERBOSE("BL31: %zd: entry:0x%" PRIx64 ", flags:0x%" PRIx64 "\n", i,
 			ATFHandoffParams->partition[i].entry_point,
@@ -226,30 +227,33 @@
 		if (target_secure == FSBL_FLAGS_SECURE) {
 			image = bl32;
 
-			if (target_estate == FSBL_FLAGS_ESTATE_A32)
+			if (target_estate == FSBL_FLAGS_ESTATE_A32) {
 				bl32->spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM,
 							 target_endianness,
 							 DISABLE_ALL_EXCEPTIONS);
-			else
+			} else {
 				bl32->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX,
 						     DISABLE_ALL_EXCEPTIONS);
+			}
 		} else {
 			image = bl33;
 
 			if (target_estate == FSBL_FLAGS_ESTATE_A32) {
-				if (target_el == FSBL_FLAGS_EL2)
+				if (target_el == FSBL_FLAGS_EL2) {
 					target_el = MODE32_hyp;
-				else
+				} else {
 					target_el = MODE32_sys;
+				}
 
 				bl33->spsr = SPSR_MODE32(target_el, SPSR_T_ARM,
 							 target_endianness,
 							 DISABLE_ALL_EXCEPTIONS);
 			} else {
-				if (target_el == FSBL_FLAGS_EL2)
+				if (target_el == FSBL_FLAGS_EL2) {
 					target_el = MODE_EL2;
-				else
+				} else {
 					target_el = MODE_EL1;
+				}
 
 				bl33->spsr = SPSR_64(target_el, MODE_SP_ELX,
 						     DISABLE_ALL_EXCEPTIONS);
@@ -262,10 +266,11 @@
 			target_el);
 		image->pc = ATFHandoffParams->partition[i].entry_point;
 
-		if (target_endianness == SPSR_E_BIG)
+		if (target_endianness == SPSR_E_BIG) {
 			EP_SET_EE(image->h.attr, EP_EE_BIG);
-		else
+		} else {
 			EP_SET_EE(image->h.attr, EP_EE_LITTLE);
+		}
 	}
 
 	return FSBL_HANDOFF_SUCCESS;

diff --git a/plat/xilinx/common/pm_service/pm_ipi.c b/plat/xilinx/common/pm_service/pm_ipi.c
index e362347..12313f2 100644
--- a/plat/xilinx/common/pm_service/pm_ipi.c
+++ b/plat/xilinx/common/pm_service/pm_ipi.c

@@ -30,7 +30,7 @@
  *
  * Called from pm_setup initialization function
  */
-int pm_ipi_init(const struct pm_proc *proc)
+int32_t pm_ipi_init(const struct pm_proc *proc)
 {
 	bakery_lock_init(&pm_secure_lock);
 	ipi_mb_open(proc->ipi->local_ipi_id, proc->ipi->remote_ipi_id);
@@ -131,12 +131,12 @@
  * @return	Returns status, either success or error+reason
  */
 static enum pm_ret_status pm_ipi_buff_read(const struct pm_proc *proc,
-					   unsigned int *value, size_t count)
+					   uint32_t *value, size_t count)
 {
 	size_t i;
 #if IPI_CRC_CHECK
 	size_t j;
-	unsigned int response_payload[PAYLOAD_ARG_CNT];
+	uint32_t response_payload[PAYLOAD_ARG_CNT];
 #endif
 	uintptr_t buffer_base = proc->ipi->buffer_base +
 				IPI_BUFFER_TARGET_REMOTE_OFFSET +
@@ -154,14 +154,16 @@
 		value++;
 	}
 #if IPI_CRC_CHECK
-	for (j = 0; j < PAYLOAD_ARG_CNT; j++)
+	for (j = 0; j < PAYLOAD_ARG_CNT; j++) {
 		response_payload[j] = mmio_read_32(buffer_base +
 						(j * PAYLOAD_ARG_SIZE));
+	}
 
 	if (response_payload[PAYLOAD_CRC_POS] !=
-			calculate_crc(response_payload, IPI_W0_TO_W6_SIZE))
+			calculate_crc(response_payload, IPI_W0_TO_W6_SIZE)) {
 		NOTICE("ERROR in CRC response payload value:0x%x\n",
 					response_payload[PAYLOAD_CRC_POS]);
+	}
 #endif
 
 	return mmio_read_32(buffer_base);
@@ -175,7 +177,7 @@
  *
  * @return	Returns status, either success or error+reason
  */
-void pm_ipi_buff_read_callb(unsigned int *value, size_t count)
+void pm_ipi_buff_read_callb(uint32_t *value, size_t count)
 {
 	size_t i;
 #if IPI_CRC_CHECK
@@ -186,22 +188,25 @@
 				IPI_BUFFER_TARGET_LOCAL_OFFSET +
 				IPI_BUFFER_REQ_OFFSET;
 
-	if (count > IPI_BUFFER_MAX_WORDS)
+	if (count > IPI_BUFFER_MAX_WORDS) {
 		count = IPI_BUFFER_MAX_WORDS;
+	}
 
 	for (i = 0; i <= count; i++) {
 		*value = mmio_read_32(buffer_base + (i * PAYLOAD_ARG_SIZE));
 		value++;
 	}
 #if IPI_CRC_CHECK
-	for (j = 0; j < PAYLOAD_ARG_CNT; j++)
+	for (j = 0; j < PAYLOAD_ARG_CNT; j++) {
 		response_payload[j] = mmio_read_32(buffer_base +
 						(j * PAYLOAD_ARG_SIZE));
+	}
 
 	if (response_payload[PAYLOAD_CRC_POS] !=
-			calculate_crc(response_payload, IPI_W0_TO_W6_SIZE))
+			calculate_crc(response_payload, IPI_W0_TO_W6_SIZE)) {
 		NOTICE("ERROR in CRC response payload value:0x%x\n",
 					response_payload[PAYLOAD_CRC_POS]);
+	}
 #endif
 }
 
@@ -219,15 +224,16 @@
  */
 enum pm_ret_status pm_ipi_send_sync(const struct pm_proc *proc,
 				    uint32_t payload[PAYLOAD_ARG_CNT],
-				    unsigned int *value, size_t count)
+				    uint32_t *value, size_t count)
 {
 	enum pm_ret_status ret;
 
 	bakery_lock_get(&pm_secure_lock);
 
 	ret = pm_ipi_send_common(proc, payload, IPI_BLOCKING);
-	if (ret != PM_RET_SUCCESS)
+	if (ret != PM_RET_SUCCESS) {
 		goto unlock;
+	}
 
 	ret = ERROR_CODE_MASK & (pm_ipi_buff_read(proc, value, count));
 
@@ -249,14 +255,15 @@
 
 uint32_t pm_ipi_irq_status(const struct pm_proc *proc)
 {
-	int ret;
+	int32_t ret;
 
 	ret = ipi_mb_enquire_status(proc->ipi->local_ipi_id,
 				    proc->ipi->remote_ipi_id);
-	if (ret & IPI_MB_STATUS_RECV_PENDING)
+	if (ret & IPI_MB_STATUS_RECV_PENDING) {
 		return 1;
-	else
+	} else {
 		return 0;
+	}
 }
 
 #if IPI_CRC_CHECK

diff --git a/plat/xilinx/versal/aarch64/versal_common.c b/plat/xilinx/versal/aarch64/versal_common.c
index 897ed59..f55cde9 100644
--- a/plat/xilinx/versal/aarch64/versal_common.c
+++ b/plat/xilinx/versal/aarch64/versal_common.c

@@ -47,7 +47,7 @@
 	generic_delay_timer_init();
 }
 
-unsigned int plat_get_syscnt_freq2(void)
+uint32_t plat_get_syscnt_freq2(void)
 {
 	return VERSAL_CPU_CLOCK;
 }

diff --git a/plat/xilinx/versal/bl31_versal_setup.c b/plat/xilinx/versal/bl31_versal_setup.c
index 78bfc29..0d0d598 100644
--- a/plat/xilinx/versal/bl31_versal_setup.c
+++ b/plat/xilinx/versal/bl31_versal_setup.c

@@ -67,19 +67,19 @@
 	if (VERSAL_CONSOLE_IS(pl011) || (VERSAL_CONSOLE_IS(pl011_1))) {
 		static console_t versal_runtime_console;
 		/* Initialize the console to provide early debug support */
-		int rc = console_pl011_register((unsigned long)VERSAL_UART_BASE,
-						(unsigned int)VERSAL_UART_CLOCK,
-						(unsigned int)VERSAL_UART_BAUDRATE,
+		int32_t rc = console_pl011_register((unsigned long)VERSAL_UART_BASE,
+						(uint32_t)VERSAL_UART_CLOCK,
+						(uint32_t)VERSAL_UART_BAUDRATE,
 						&versal_runtime_console);
 		if (rc == 0) {
 			panic();
 		}
 
-		console_set_scope(&versal_runtime_console, (unsigned int)(CONSOLE_FLAG_BOOT |
+		console_set_scope(&versal_runtime_console, (uint32_t)(CONSOLE_FLAG_BOOT |
 				  CONSOLE_FLAG_RUNTIME));
 	} else if (VERSAL_CONSOLE_IS(dcc)) {
 		/* Initialize the dcc console for debug */
-		int rc = console_dcc_register();
+		int32_t rc = console_dcc_register();
 		if (rc == 0) {
 			panic();
 		}
@@ -126,7 +126,7 @@
 
 static interrupt_type_handler_t type_el3_interrupt_handler;
 
-int request_intr_type_el3(uint32_t id, interrupt_type_handler_t handler)
+int32_t request_intr_type_el3(uint32_t id, interrupt_type_handler_t handler)
 {
 	/* Validate 'handler'*/
 	if (handler == NULL) {

diff --git a/plat/xilinx/versal/include/plat_private.h b/plat/xilinx/versal/include/plat_private.h
index d12d13a..109c95e 100644
--- a/plat/xilinx/versal/include/plat_private.h
+++ b/plat/xilinx/versal/include/plat_private.h

@@ -22,11 +22,11 @@
 void plat_versal_gic_save(void);
 void plat_versal_gic_resume(void);
 
-unsigned int versal_calc_core_pos(u_register_t mpidr);
+uint32_t versal_calc_core_pos(u_register_t mpidr);
 /*
  * Register handler to specific GIC entrance
  * for INTR_TYPE_EL3 type of interrupt
  */
-int request_intr_type_el3(uint32_t irq, interrupt_type_handler_t fiq_handler);
+int32_t request_intr_type_el3(uint32_t irq, interrupt_type_handler_t fiq_handler);
 
 #endif /* PLAT_PRIVATE_H */

diff --git a/plat/xilinx/versal/include/versal_def.h b/plat/xilinx/versal/include/versal_def.h
index 9372954..731742d 100644
--- a/plat/xilinx/versal/include/versal_def.h
+++ b/plat/xilinx/versal/include/versal_def.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -20,6 +20,8 @@
 
 /* List all supported platforms */
 #define VERSAL_PLATFORM_ID_versal_virt	1
+#define VERSAL_PLATFORM_ID_spp_itr6	2
+#define VERSAL_PLATFORM_ID_emu_itr6	3
 #define VERSAL_PLATFORM_ID_silicon	4
 
 #define VERSAL_PLATFORM_IS(con)	(VERSAL_PLATFORM_ID_ ## con == VERSAL_PLATFORM)
@@ -92,6 +94,16 @@
 # define VERSAL_UART_CLOCK	100000000
 # define VERSAL_UART_BAUDRATE	115200
 # define VERSAL_CPU_CLOCK	100000000
+#elif VERSAL_PLATFORM_IS(spp_itr6)
+# define PLATFORM_NAME          "SPP ITR6"
+# define VERSAL_UART_CLOCK      25000000
+# define VERSAL_UART_BAUDRATE   115200
+# define VERSAL_CPU_CLOCK       2720000
+#elif VERSAL_PLATFORM_IS(emu_itr6)
+# define PLATFORM_NAME          "EMU ITR6"
+# define VERSAL_UART_CLOCK      212000
+# define VERSAL_UART_BAUDRATE   9600
+# define VERSAL_CPU_CLOCK       212000
 #endif
 
 /* Access control register defines */

diff --git a/plat/xilinx/versal/plat_psci.c b/plat/xilinx/versal/plat_psci.c
index eb05e58..acecbb1 100644
--- a/plat/xilinx/versal/plat_psci.c
+++ b/plat/xilinx/versal/plat_psci.c

@@ -19,9 +19,9 @@
 
 static uintptr_t versal_sec_entry;
 
-static int versal_pwr_domain_on(u_register_t mpidr)
+static int32_t versal_pwr_domain_on(u_register_t mpidr)
 {
-	int cpu_id = plat_core_pos_by_mpidr(mpidr);
+	int32_t cpu_id = plat_core_pos_by_mpidr(mpidr);
 	const struct pm_proc *proc;
 
 	VERBOSE("%s: mpidr: 0x%lx\n", __func__, mpidr);
@@ -30,7 +30,7 @@
 		return PSCI_E_INTERN_FAIL;
 	}
 
-	proc = pm_get_proc((unsigned int)cpu_id);
+	proc = pm_get_proc((uint32_t)cpu_id);
 
 	/* Send request to PMC to wake up selected ACPU core */
 	(void)pm_req_wakeup(proc->node_id, (versal_sec_entry & 0xFFFFFFFFU) | 0x1U,
@@ -50,8 +50,8 @@
  */
 static void versal_pwr_domain_suspend(const psci_power_state_t *target_state)
 {
-	unsigned int state;
-	unsigned int cpu_id = plat_my_core_pos();
+	uint32_t state;
+	uint32_t cpu_id = plat_my_core_pos();
 	const struct pm_proc *proc = pm_get_proc(cpu_id);
 
 	for (size_t i = 0U; i <= PLAT_MAX_PWR_LVL; i++) {
@@ -88,7 +88,7 @@
 static void versal_pwr_domain_suspend_finish(
 					const psci_power_state_t *target_state)
 {
-	unsigned int cpu_id = plat_my_core_pos();
+	uint32_t cpu_id = plat_my_core_pos();
 	const struct pm_proc *proc = pm_get_proc(cpu_id);
 
 	for (size_t i = 0U; i <= PLAT_MAX_PWR_LVL; i++) {
@@ -156,7 +156,7 @@
  */
 static void versal_pwr_domain_off(const psci_power_state_t *target_state)
 {
-	unsigned int cpu_id = plat_my_core_pos();
+	uint32_t cpu_id = plat_my_core_pos();
 	const struct pm_proc *proc = pm_get_proc(cpu_id);
 
 	for (size_t i = 0U; i <= PLAT_MAX_PWR_LVL; i++) {
@@ -188,12 +188,12 @@
  *
  * @return	Returns status, either success or reason
  */
-static int versal_validate_power_state(unsigned int power_state,
+static int32_t versal_validate_power_state(uint32_t power_state,
 				       psci_power_state_t *req_state)
 {
 	VERBOSE("%s: power_state: 0x%x\n", __func__, power_state);
 
-	unsigned int pstate = psci_get_pstate_type(power_state);
+	uint32_t pstate = psci_get_pstate_type(power_state);
 
 	assert(req_state);
 

diff --git a/plat/xilinx/versal/plat_topology.c b/plat/xilinx/versal/plat_topology.c
index 66d4fae..6a94544 100644
--- a/plat/xilinx/versal/plat_topology.c
+++ b/plat/xilinx/versal/plat_topology.c

@@ -6,9 +6,9 @@
 
 #include <platform_def.h>
 
-static const unsigned char plat_power_domain_tree_desc[] = {1, PLATFORM_CORE_COUNT};
+static const uint8_t plat_power_domain_tree_desc[] = {1, PLATFORM_CORE_COUNT};
 
-const unsigned char *plat_get_power_domain_tree_desc(void)
+const uint8_t *plat_get_power_domain_tree_desc(void)
 {
 	return plat_power_domain_tree_desc;
 }

diff --git a/plat/xilinx/versal/plat_versal.c b/plat/xilinx/versal/plat_versal.c
index 54c35b6..132c7b7 100644
--- a/plat/xilinx/versal/plat_versal.c
+++ b/plat/xilinx/versal/plat_versal.c

@@ -7,7 +7,7 @@
 #include <plat_private.h>
 #include <plat/common/platform.h>
 
-int plat_core_pos_by_mpidr(u_register_t mpidr)
+int32_t plat_core_pos_by_mpidr(u_register_t mpidr)
 {
 	if ((mpidr & MPIDR_CLUSTER_MASK) != 0U) {
 		return -1;
@@ -17,5 +17,5 @@
 		return -1;
 	}
 
-	return (int)versal_calc_core_pos(mpidr);
+	return (int32_t)versal_calc_core_pos(mpidr);
 }

diff --git a/plat/xilinx/versal/pm_service/pm_api_sys.c b/plat/xilinx/versal/pm_service/pm_api_sys.c
index 534d910..db78049 100644
--- a/plat/xilinx/versal/pm_service/pm_api_sys.c
+++ b/plat/xilinx/versal/pm_service/pm_api_sys.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -25,15 +25,17 @@
 #define LOADER_MODULE_ID	0x7U
 
 #define  MODE	0x80000000U
+#define MODULE_ID_MASK		0x0000ff00
+
 /* default shutdown/reboot scope is system(2) */
-static unsigned int pm_shutdown_scope = XPM_SHUTDOWN_SUBTYPE_RST_SYSTEM;
+static uint32_t pm_shutdown_scope = XPM_SHUTDOWN_SUBTYPE_RST_SYSTEM;
 
 /**
  * pm_get_shutdown_scope() - Get the currently set shutdown scope
  *
  * @return	Shutdown scope value
  */
-unsigned int pm_get_shutdown_scope(void)
+uint32_t pm_get_shutdown_scope(void)
 {
 	return pm_shutdown_scope;
 }
@@ -73,38 +75,30 @@
 /* PM API functions */
 
 /**
- * pm_get_api_version() - Get version number of PMC PM firmware
- * @version	Returns 32-bit version number of PMC Power Management Firmware
+ * pm_handle_eemi_call() - PM call for processor to send eemi payload
  * @flag	0 - Call from secure source
  *		1 - Call from non-secure source
+ * @x0 to x5	Arguments received per SMC64 standard
+ * @result	Payload received from firmware
  *
- * @return	Returns status, either success or error+reason
+ * @return	 PM_RET_SUCCESS on success or error code
  */
-enum pm_ret_status pm_get_api_version(unsigned int *version, uint32_t flag)
+enum pm_ret_status pm_handle_eemi_call(uint32_t flag, uint32_t x0, uint32_t x1,
+				       uint32_t x2, uint32_t x3, uint32_t x4,
+				       uint32_t x5, uint64_t *result)
 {
-	uint32_t payload[PAYLOAD_ARG_CNT];
+	uint32_t payload[PAYLOAD_ARG_CNT] = {0};
+	uint32_t module_id;
 
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD1(payload, LIBPM_MODULE_ID, flag, PM_GET_API_VERSION);
-	return pm_ipi_send_sync(primary_proc, payload, version, 1);
-}
+	module_id = (x0 & MODULE_ID_MASK) >> 8;
 
-/**
- * pm_init_finalize() - Call to notify PMC PM firmware that master has power
- *			management enabled and that it has finished its
- *			initialization
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Status returned by the PMU firmware
- */
-enum pm_ret_status pm_init_finalize(uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
+	//default module id is for LIBPM
+	if (module_id == 0) {
+		module_id = LIBPM_MODULE_ID;
+	}
 
-	/* Send request to the PMU */
-	PM_PACK_PAYLOAD1(payload, LIBPM_MODULE_ID, flag, PM_INIT_FINALIZE);
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
+	PM_PACK_PAYLOAD6(payload, module_id, flag, x0, x1, x2, x3, x4, x5);
+	return pm_ipi_send_sync(primary_proc, payload, (uint32_t *)result, PAYLOAD_ARG_CNT);
 }
 
 /**
@@ -122,12 +116,12 @@
  * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_self_suspend(uint32_t nid,
-				   unsigned int latency,
-				   unsigned int state,
+				   uint32_t latency,
+				   uint32_t state,
 				   uintptr_t address, uint32_t flag)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
-	unsigned int cpuid = plat_my_core_pos();
+	uint32_t cpuid = plat_my_core_pos();
 	const struct pm_proc *proc = pm_get_proc(cpuid);
 
 	if (proc == NULL) {
@@ -189,7 +183,7 @@
  * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_req_suspend(uint32_t target, uint8_t ack,
-				  unsigned int latency, unsigned int state,
+				  uint32_t latency, uint32_t state,
 				  uint32_t flag)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
@@ -235,134 +229,6 @@
 }
 
 /**
- * pm_request_device() - Request a device
- * @device_id		Device ID
- * @capabilities	Requested capabilities for the device
- * @qos			Required Quality of Service
- * @ack			Flag to specify whether acknowledge requested
- * @flag		0 - Call from secure source
- *			1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_request_device(uint32_t device_id, uint32_t capabilities,
-				     uint32_t qos, uint32_t ack, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD5(payload, LIBPM_MODULE_ID, flag, PM_REQUEST_DEVICE,
-			 device_id, capabilities, qos, ack);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_release_device() - Release a device
- * @device_id		Device ID
- * @flag		0 - Call from secure source
- *			1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_release_device(uint32_t device_id, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_RELEASE_DEVICE,
-			 device_id);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_set_requirement() - Set requirement for the device
- * @device_id		Device ID
- * @capabilities	Requested capabilities for the device
- * @latency		Requested maximum latency
- * @qos			Required Quality of Service
- * @flag		0 - Call from secure source
- *			1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_set_requirement(uint32_t device_id, uint32_t capabilities,
-				      uint32_t latency, uint32_t qos,
-				      uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD5(payload, LIBPM_MODULE_ID, flag, PM_SET_REQUIREMENT,
-			 device_id, capabilities, latency, qos);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_get_device_status() - Get device's status
- * @device_id		Device ID
- * @response		Buffer to store device status response
- * @flag		0 - Call from secure source
- *			1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_get_device_status(uint32_t device_id, uint32_t *response,
-					uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_GET_DEVICE_STATUS,
-			 device_id);
-
-	return pm_ipi_send_sync(primary_proc, payload, response, 3);
-}
-
-/**
- * pm_reset_assert() - Assert/De-assert reset
- * @reset	Reset ID
- * @assert	Assert (1) or de-assert (0)
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_reset_assert(uint32_t reset, bool assert, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD3(payload, LIBPM_MODULE_ID, flag, PM_RESET_ASSERT, reset,
-			 assert);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_reset_get_status() - Get current status of a reset line
- * @reset	Reset ID
- * @status	Returns current status of selected reset ID
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_reset_get_status(uint32_t reset, uint32_t *status,
-				       uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_RESET_ASSERT,
-			 reset);
-
-	return pm_ipi_send_sync(primary_proc, payload, status, 1);
-}
-
-/**
  * pm_get_callbackdata() - Read from IPI response buffer
  * @data - array of PAYLOAD_ARG_CNT elements
  * @flag - 0 - Call from secure source
@@ -382,294 +248,12 @@
 }
 
 /**
- * pm_pinctrl_request() - Request a pin
- * @pin		Pin ID
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_pinctrl_request(uint32_t pin, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_PINCTRL_REQUEST,
-			 pin);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_pinctrl_release() - Release a pin
- * @pin		Pin ID
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_pinctrl_release(uint32_t pin, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_PINCTRL_RELEASE,
-			 pin);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_pinctrl_set_function() - Set pin function
- * @pin		Pin ID
- * @function	Function ID
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_pinctrl_set_function(uint32_t pin, uint32_t function,
-					   uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD3(payload, LIBPM_MODULE_ID, flag,
-			 PM_PINCTRL_SET_FUNCTION, pin, function)
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_pinctrl_get_function() - Get function set on the pin
- * @pin		Pin ID
- * @function	Function set on the pin
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_pinctrl_get_function(uint32_t pin, uint32_t *function,
-					   uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag,
-			 PM_PINCTRL_SET_FUNCTION, pin);
-
-	return pm_ipi_send_sync(primary_proc, payload, function, 1);
-}
-
-/**
- * pm_pinctrl_set_pin_param() - Set configuration parameter for the pin
- * @pin		Pin ID
- * @param	Parameter ID
- * @value	Parameter value
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_pinctrl_set_pin_param(uint32_t pin, uint32_t param,
-					    uint32_t value, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD4(payload, LIBPM_MODULE_ID, flag,
-			 PM_PINCTRL_CONFIG_PARAM_SET, pin, param, value);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_pinctrl_get_pin_param() - Get configuration parameter value for the pin
- * @pin		Pin ID
- * @param	Parameter ID
- * @value	Buffer to store parameter value
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_pinctrl_get_pin_param(uint32_t pin, uint32_t param,
-					    uint32_t *value, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD3(payload, LIBPM_MODULE_ID, flag,
-			 PM_PINCTRL_CONFIG_PARAM_GET, pin, param);
-
-	return pm_ipi_send_sync(primary_proc, payload, value, 1);
-}
-
-/**
- * pm_clock_enable() - Enable the clock
- * @clk_id	Clock ID
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_clock_enable(uint32_t clk_id, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_CLOCK_ENABLE,
-			 clk_id);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_clock_disable() - Disable the clock
- * @clk_id	Clock ID
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_clock_disable(uint32_t clk_id, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_CLOCK_DISABLE,
-			 clk_id);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_clock_get_state() - Get clock status
- * @clk_id	Clock ID
- * @state:	Buffer to store clock status (1: Enabled, 0:Disabled)
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_clock_get_state(uint32_t clk_id, uint32_t *state,
-				      uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_CLOCK_GETSTATE,
-			 clk_id);
-
-	return pm_ipi_send_sync(primary_proc, payload, state, 1);
-}
-
-/**
- * pm_clock_set_divider() - Set divider for the clock
- * @clk_id	Clock ID
- * @divider	Divider value
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_clock_set_divider(uint32_t clk_id, uint32_t divider,
-					uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD3(payload, LIBPM_MODULE_ID, flag, PM_CLOCK_SETDIVIDER,
-			 clk_id, divider);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_clock_get_divider() - Get divider value for the clock
- * @clk_id	Clock ID
- * @divider:	Buffer to store clock divider value
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_clock_get_divider(uint32_t clk_id, uint32_t *divider,
-					uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_CLOCK_GETDIVIDER,
-			 clk_id);
-
-	return pm_ipi_send_sync(primary_proc, payload, divider, 1);
-}
-
-/**
- * pm_clock_set_parent() - Set parent for the clock
- * @clk_id	Clock ID
- * @parent	Parent ID
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
+ * pm_pll_set_param() - Set PLL parameter
  *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_clock_set_parent(uint32_t clk_id, uint32_t parent,
-				       uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD3(payload, LIBPM_MODULE_ID, flag, PM_CLOCK_SETPARENT,
-			 clk_id, parent);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
- * pm_clock_get_parent() - Get parent value for the clock
- * @clk_id	Clock ID
- * @parent:	Buffer to store clock parent value
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
+ * This API is deprecated and maintained here for backward compatibility.
+ * New use of this API should be avoided for versal platform.
+ * This API and its use cases will be removed for versal platform.
  *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_clock_get_parent(uint32_t clk_id, uint32_t *parent,
-				       uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_CLOCK_GETPARENT,
-			 clk_id);
-
-	return pm_ipi_send_sync(primary_proc, payload, parent, 1);
-}
-/**
- * pm_clock_get_rate() - Get the rate value for the clock
- * @clk_id	Clock ID
- * @rate:	Buffer to store clock rate value
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_clock_get_rate(uint32_t clk_id, uint32_t *clk_rate,
-				     uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag, PM_CLOCK_GETRATE,
-			 clk_id);
-
-	return pm_ipi_send_sync(primary_proc, payload, clk_rate, 2);
-}
-
-/**
- * pm_pll_set_param() - Set PLL parameter
  * @clk_id	PLL clock ID
  * @param	PLL parameter ID
  * @value	Value to set for PLL parameter
@@ -692,6 +276,11 @@
 
 /**
  * pm_pll_get_param() - Get PLL parameter value
+ *
+ * This API is deprecated and maintained here for backward compatibility.
+ * New use of this API should be avoided for versal platform.
+ * This API and its use cases will be removed for versal platform.
+ *
  * @clk_id	PLL clock ID
  * @param	PLL parameter ID
  * @value:	Buffer to store PLL parameter value
@@ -714,6 +303,11 @@
 
 /**
  * pm_pll_set_mode() - Set PLL mode
+ *
+ * This API is deprecated and maintained here for backward compatibility.
+ * New use of this API should be avoided for versal platform.
+ * This API and its use cases will be removed for versal platform.
+ *
  * @clk_id	PLL clock ID
  * @mode	PLL mode
  * @flag	0 - Call from secure source
@@ -735,6 +329,11 @@
 
 /**
  * pm_pll_get_mode() - Get PLL mode
+ *
+ * This API is deprecated and maintained here for backward compatibility.
+ * New use of this API should be avoided for versal platform.
+ * This API and its use cases will be removed for versal platform.
+ *
  * @clk_id	PLL clock ID
  * @mode:	Buffer to store PLL mode
  * @flag	0 - Call from secure source
@@ -808,22 +407,28 @@
 }
 
 /**
-* pm_query_data() -  PM API for querying firmware data
-* @qid	The type of data to query
-* @arg1	Argument 1 to requested query data call
-* @arg2	Argument 2 to requested query data call
-* @arg3	Argument 3 to requested query data call
-* @data	Returned output data
-* @flag 0 - Call from secure source
-*	1 - Call from non-secure source
-*
-* This function returns requested data.
-*/
+ * pm_query_data() -  PM API for querying firmware data
+ *
+ * This API is deprecated and maintained here for backward compatibility.
+ * New use of this API should be avoided for versal platform.
+ * This API and its use cases will be removed for versal platform.
+ *
+ * @qid	The type of data to query
+ * @arg1	Argument 1 to requested query data call
+ * @arg2	Argument 2 to requested query data call
+ * @arg3	Argument 3 to requested query data call
+ * @data	Returned output data
+ * @flag 0 - Call from secure source
+ *	1 - Call from non-secure source
+ *
+ * @retur - 0 if success else non-zero error code of type
+ * enum pm_ret_status
+ */
 enum pm_ret_status pm_query_data(uint32_t qid, uint32_t arg1, uint32_t arg2,
 				 uint32_t arg3, uint32_t *data, uint32_t flag)
 {
 	uint32_t ret;
-	uint32_t version;
+	uint32_t version[PAYLOAD_ARG_CNT] = {0};
 	uint32_t payload[PAYLOAD_ARG_CNT];
 	uint32_t fw_api_version;
 
@@ -831,42 +436,50 @@
 	PM_PACK_PAYLOAD5(payload, LIBPM_MODULE_ID, flag, PM_QUERY_DATA, qid,
 			 arg1, arg2, arg3);
 
-	ret = pm_feature_check(PM_QUERY_DATA, &version, flag);
-	if (PM_RET_SUCCESS == ret) {
-		fw_api_version = version & 0xFFFFU;
-		if ((2U == fw_api_version) &&
-		    ((XPM_QID_CLOCK_GET_NAME == qid) ||
-		     (XPM_QID_PINCTRL_GET_FUNCTION_NAME == qid))) {
-			ret = pm_ipi_send_sync(primary_proc, payload, data, 8);
-			ret = data[0];
-			data[0] = data[1];
-			data[1] = data[2];
-			data[2] = data[3];
+	ret = pm_feature_check(PM_QUERY_DATA, &version[0], flag);
+	if (ret == PM_RET_SUCCESS) {
+		fw_api_version = version[0] & 0xFFFF;
+		if ((fw_api_version == 2U) &&
+		    ((qid == XPM_QID_CLOCK_GET_NAME) ||
+		     (qid == XPM_QID_PINCTRL_GET_FUNCTION_NAME))) {
+			ret = pm_ipi_send_sync(primary_proc, payload, data, PAYLOAD_ARG_CNT);
+			if (ret == PM_RET_SUCCESS) {
+				ret = data[0];
+				data[0] = data[1];
+				data[1] = data[2];
+				data[2] = data[3];
+			}
 		} else {
-			ret = pm_ipi_send_sync(primary_proc, payload, data, 4);
+			ret = pm_ipi_send_sync(primary_proc, payload, data, PAYLOAD_ARG_CNT);
 		}
 	}
 	return ret;
 }
 /**
  * pm_api_ioctl() -  PM IOCTL API for device control and configs
+ *
+ * This API is deprecated and maintained here for backward compatibility.
+ * New use of this API should be avoided for versal platform.
+ * This API and its use cases will be removed for versal platform.
+ *
  * @device_id	Device ID
  * @ioctl_id	ID of the requested IOCTL
  * @arg1	Argument 1 to requested IOCTL call
  * @arg2	Argument 2 to requested IOCTL call
+ * @arg3	Argument 3 to requested IOCTL call
  * @value	Returned output value
  * @flag	0 - Call from secure source
  *		1 - Call from non-secure source
  *
  * This function calls IOCTL to firmware for device control and configuration.
  *
- * @return	Returns status, either success or error+reason
+ * @return	Returns status, either 0 on success or non-zero error code
+ * of type enum pm_ret_status
  */
 enum pm_ret_status pm_api_ioctl(uint32_t device_id, uint32_t ioctl_id,
-				uint32_t arg1, uint32_t arg2, uint32_t *value,
-				uint32_t flag)
+				uint32_t arg1, uint32_t arg2, uint32_t arg3,
+				uint32_t *value, uint32_t flag)
 {
-	uint32_t payload[PAYLOAD_ARG_CNT];
 	enum pm_ret_status ret;
 
 	switch (ioctl_id) {
@@ -884,19 +497,16 @@
 		break;
 	case IOCTL_SET_SGI:
 		/* Get the sgi number */
-		if (pm_register_sgi(arg1) != 0) {
+		ret = pm_register_sgi(arg1, arg2);
+		if (ret != 0) {
 			return PM_RET_ERROR_ARGS;
 		}
 		gicd_write_irouter(gicv3_driver_data->gicd_base,
-				  (unsigned int)PLAT_VERSAL_IPI_IRQ, MODE);
+				  (uint32_t)PLAT_VERSAL_IPI_IRQ, MODE);
 		ret =  PM_RET_SUCCESS;
 		break;
 	default:
-		/* Send request to the PMC */
-		PM_PACK_PAYLOAD5(payload, LIBPM_MODULE_ID, flag, PM_IOCTL,
-				 device_id, ioctl_id, arg1, arg2);
-		ret =  pm_ipi_send_sync(primary_proc, payload, value, 1);
-		break;
+		return PM_RET_ERROR_NOTSUPPORTED;
 	}
 
 	return ret;
@@ -923,116 +533,48 @@
 }
 
 /**
- * pm_get_chipid() - Read silicon ID registers
- * @value       Buffer for return values. Must be large enough
- *		to hold 8 bytes.
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return      Returns silicon ID registers
- */
-enum pm_ret_status pm_get_chipid(uint32_t *value, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD1(payload, LIBPM_MODULE_ID, flag, PM_GET_CHIPID);
-
-	return pm_ipi_send_sync(primary_proc, payload, value, 2);
-}
-
-/**
  * pm_feature_check() - Returns the supported API version if supported
  * @api_id	API ID to check
- * @value	Returned supported API version
  * @flag	0 - Call from secure source
  *		1 - Call from non-secure source
+ * @ret_payload pointer to array of PAYLOAD_ARG_CNT number of
+ *		words Returned supported API version and bitmasks
+ *		for IOCTL and QUERY ID
  *
  * @return	Returns status, either success or error+reason
  */
-enum pm_ret_status pm_feature_check(uint32_t api_id, unsigned int *version,
+enum pm_ret_status pm_feature_check(uint32_t api_id, uint32_t *ret_payload,
 				    uint32_t flag)
 {
-	uint32_t payload[PAYLOAD_ARG_CNT], fw_api_version;
-	enum pm_ret_status status = PM_RET_ERROR_NOFEATURE;
+	uint32_t payload[PAYLOAD_ARG_CNT];
+	uint32_t module_id;
 
+	/* Return version of API which are implemented in ATF only */
 	switch (api_id) {
 	case PM_GET_CALLBACK_DATA:
 	case PM_GET_TRUSTZONE_VERSION:
+		ret_payload[0] = PM_API_VERSION_2;
+		return PM_RET_SUCCESS;
 	case PM_LOAD_PDI:
-		*version = (PM_API_BASE_VERSION << 16);
-		status = PM_RET_SUCCESS;
-		break;
-	case PM_GET_API_VERSION:
-	case PM_GET_DEVICE_STATUS:
-	case PM_GET_OP_CHARACTERISTIC:
-	case PM_REQ_SUSPEND:
-	case PM_SELF_SUSPEND:
-	case PM_FORCE_POWERDOWN:
-	case PM_ABORT_SUSPEND:
-	case PM_REQ_WAKEUP:
-	case PM_SET_WAKEUP_SOURCE:
-	case PM_SYSTEM_SHUTDOWN:
-	case PM_REQUEST_DEVICE:
-	case PM_RELEASE_DEVICE:
-	case PM_SET_REQUIREMENT:
-	case PM_RESET_ASSERT:
-	case PM_RESET_GET_STATUS:
-	case PM_GET_CHIPID:
-	case PM_PINCTRL_REQUEST:
-	case PM_PINCTRL_RELEASE:
-	case PM_PINCTRL_GET_FUNCTION:
-	case PM_PINCTRL_SET_FUNCTION:
-	case PM_PINCTRL_CONFIG_PARAM_GET:
-	case PM_PINCTRL_CONFIG_PARAM_SET:
-	case PM_IOCTL:
-	case PM_CLOCK_ENABLE:
-	case PM_CLOCK_DISABLE:
-	case PM_CLOCK_GETSTATE:
-	case PM_CLOCK_SETDIVIDER:
-	case PM_CLOCK_GETDIVIDER:
-	case PM_CLOCK_SETPARENT:
-	case PM_CLOCK_GETPARENT:
-	case PM_CLOCK_GETRATE:
-	case PM_PLL_SET_PARAMETER:
-	case PM_PLL_GET_PARAMETER:
-	case PM_PLL_SET_MODE:
-	case PM_PLL_GET_MODE:
-	case PM_FEATURE_CHECK:
-	case PM_INIT_FINALIZE:
-	case PM_SET_MAX_LATENCY:
-	case PM_REGISTER_NOTIFIER:
-		*version = (PM_API_BASE_VERSION << 16);
-		status = PM_RET_SUCCESS;
-		break;
-	case PM_QUERY_DATA:
-		*version = (PM_API_QUERY_DATA_VERSION << 16);
-		status = PM_RET_SUCCESS;
-		break;
+		ret_payload[0] = PM_API_BASE_VERSION;
+		return PM_RET_SUCCESS;
 	default:
-		*version = 0U;
-		status = PM_RET_ERROR_NOFEATURE;
 		break;
 	}
 
-	if (status != PM_RET_SUCCESS) {
-		goto done;
+	module_id = (api_id & MODULE_ID_MASK) >> 8;
+
+	/*
+	 * feature check should be done only for LIBPM module
+	 * If module_id is 0, then we consider it LIBPM module as default id
+	 */
+	if ((module_id > 0) && (module_id != LIBPM_MODULE_ID)) {
+		return PM_RET_SUCCESS;
 	}
 
 	PM_PACK_PAYLOAD2(payload, LIBPM_MODULE_ID, flag,
 			 PM_FEATURE_CHECK, api_id);
-
-	status = pm_ipi_send_sync(primary_proc, payload, &fw_api_version, 1);
-	if (status != PM_RET_SUCCESS) {
-		goto done;
-	}
-
-	*version |= fw_api_version;
-
-	status = PM_RET_SUCCESS;
-
-done:
-	return status;
+	return pm_ipi_send_sync(primary_proc, payload, ret_payload, PAYLOAD_ARG_CNT);
 }
 
 /**
@@ -1060,54 +602,6 @@
 }
 
 /**
- * pm_get_op_characteristic() - PM call to request operating characteristics
- *                              of a device
- * @device_id   Device id
- * @type        Type of the operating characteristic
- *              (power, temperature and latency)
- * @result      Returns the operating characteristic for the requested device,
- *              specified by the type
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return      Returns status, either success or error+reason
- */
-enum pm_ret_status pm_get_op_characteristic(uint32_t device_id,
-					    enum pm_opchar_type type,
-					    uint32_t *result, uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD3(payload, LIBPM_MODULE_ID, flag,
-			 PM_GET_OP_CHARACTERISTIC, device_id, type);
-	return pm_ipi_send_sync(primary_proc, payload, result, 1);
-}
-
-/**
- * pm_set_max_latency() - PM call to change in the maximum wake-up latency
- *			  requirements for a specific device currently
- *			  used by that CPU.
- * @device_id	Device ID
- * @latency	Latency value
- * @flag	0 - Call from secure source
- *		1 - Call from non-secure source
- *
- * @return	Returns status, either success or error+reason
- */
-enum pm_ret_status pm_set_max_latency(uint32_t device_id, uint32_t latency,
-				      uint32_t flag)
-{
-	uint32_t payload[PAYLOAD_ARG_CNT];
-
-	/* Send request to the PMC */
-	PM_PACK_PAYLOAD3(payload, LIBPM_MODULE_ID, flag, PM_SET_MAX_LATENCY,
-			 device_id, latency);
-
-	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-}
-
-/**
  * pm_register_notifier() - PM call to register a subsystem to be notified
  * 			    about the device event
  * @device_id	Device ID for the Node to which the event is related

diff --git a/plat/xilinx/versal/pm_service/pm_api_sys.h b/plat/xilinx/versal/pm_service/pm_api_sys.h
index 5a92704..8343533 100644
--- a/plat/xilinx/versal/pm_service/pm_api_sys.h
+++ b/plat/xilinx/versal/pm_service/pm_api_sys.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,57 +14,23 @@
  * PM API function declarations
  **********************************************************/
 
-enum pm_ret_status pm_get_api_version(unsigned int *version, uint32_t flag);
-enum pm_ret_status pm_init_finalize(uint32_t flag);
+enum pm_ret_status pm_handle_eemi_call(uint32_t flag, uint32_t x0, uint32_t x1,
+				       uint32_t x2, uint32_t x3, uint32_t x4,
+				       uint32_t x5, uint64_t *result);
 enum pm_ret_status pm_self_suspend(uint32_t nid,
-				   unsigned int latency,
-				   unsigned int state,
+				   uint32_t latency,
+				   uint32_t state,
 				   uintptr_t address, uint32_t flag);
 enum pm_ret_status pm_abort_suspend(enum pm_abort_reason reason, uint32_t flag);
 enum pm_ret_status pm_req_suspend(uint32_t target,
 				  uint8_t ack,
-				  unsigned int latency,
-				  unsigned int state, uint32_t flag);
+				  uint32_t latency,
+				  uint32_t state, uint32_t flag);
 enum pm_ret_status pm_req_wakeup(uint32_t target, uint32_t set_address,
 				 uintptr_t address, uint8_t ack, uint32_t flag);
 enum pm_ret_status pm_set_wakeup_source(uint32_t target, uint32_t device_id,
 					uint8_t enable, uint32_t flag);
-enum pm_ret_status pm_request_device(uint32_t device_id, uint32_t capabilities,
-				     uint32_t qos, uint32_t ack, uint32_t flag);
-enum pm_ret_status pm_release_device(uint32_t device_id, uint32_t flag);
-enum pm_ret_status pm_set_requirement(uint32_t device_id, uint32_t capabilities,
-				      uint32_t latency, uint32_t qos,
-				      uint32_t flag);
-enum pm_ret_status pm_get_device_status(uint32_t device_id, uint32_t *response,
-					uint32_t flag);
-enum pm_ret_status pm_reset_assert(uint32_t reset, bool assert, uint32_t flag);
-enum pm_ret_status pm_reset_get_status(uint32_t reset, uint32_t *status,
-				       uint32_t flag);
 void pm_get_callbackdata(uint32_t *data, size_t count, uint32_t flag);
-enum pm_ret_status pm_pinctrl_request(uint32_t pin, uint32_t flag);
-enum pm_ret_status pm_pinctrl_release(uint32_t pin, uint32_t flag);
-enum pm_ret_status pm_pinctrl_set_function(uint32_t pin, uint32_t function,
-					   uint32_t flag);
-enum pm_ret_status pm_pinctrl_get_function(uint32_t pin, uint32_t *function,
-					   uint32_t flag);
-enum pm_ret_status pm_pinctrl_set_pin_param(uint32_t pin, uint32_t param,
-					    uint32_t value, uint32_t flag);
-enum pm_ret_status pm_pinctrl_get_pin_param(uint32_t pin, uint32_t param,
-					    uint32_t *value, uint32_t flag);
-enum pm_ret_status pm_clock_enable(uint32_t clk_id, uint32_t flag);
-enum pm_ret_status pm_clock_disable(uint32_t clk_id, uint32_t flag);
-enum pm_ret_status pm_clock_get_state(uint32_t clk_id, uint32_t *state,
-				      uint32_t flag);
-enum pm_ret_status pm_clock_set_divider(uint32_t clk_id, uint32_t divider,
-					uint32_t flag);
-enum pm_ret_status pm_clock_get_divider(uint32_t clk_id, uint32_t *divider,
-					uint32_t flag);
-enum pm_ret_status pm_clock_set_parent(uint32_t clk_id, uint32_t parent,
-				       uint32_t flag);
-enum pm_ret_status pm_clock_get_parent(uint32_t clk_id, uint32_t *parent,
-				       uint32_t flag);
-enum pm_ret_status pm_clock_get_rate(uint32_t clk_id, uint32_t *clk_rate,
-				     uint32_t flag);
 enum pm_ret_status pm_pll_set_param(uint32_t clk_id, uint32_t param,
 				    uint32_t value, uint32_t flag);
 enum pm_ret_status pm_pll_get_param(uint32_t clk_id, uint32_t param,
@@ -78,21 +44,15 @@
 enum pm_ret_status pm_system_shutdown(uint32_t type, uint32_t subtype,
 				      uint32_t flag);
 enum pm_ret_status pm_api_ioctl(uint32_t device_id, uint32_t ioctl_id,
-				uint32_t arg1, uint32_t arg2, uint32_t *value,
-				uint32_t flag);
+				uint32_t arg1, uint32_t arg2, uint32_t arg3,
+				uint32_t *value, uint32_t flag);
 enum pm_ret_status pm_query_data(uint32_t qid, uint32_t arg1, uint32_t arg2,
 				 uint32_t arg3, uint32_t *data, uint32_t flag);
-unsigned int pm_get_shutdown_scope(void);
-enum pm_ret_status pm_get_chipid(uint32_t *value, uint32_t flag);
-enum pm_ret_status pm_feature_check(uint32_t api_id, unsigned int *version,
+uint32_t pm_get_shutdown_scope(void);
+enum pm_ret_status pm_feature_check(uint32_t api_id, uint32_t *ret_payload,
 				    uint32_t flag);
 enum pm_ret_status pm_load_pdi(uint32_t src, uint32_t address_low,
 			       uint32_t address_high, uint32_t flag);
-enum pm_ret_status pm_get_op_characteristic(uint32_t device_id,
-					    enum pm_opchar_type type,
-					    uint32_t *result, uint32_t flag);
-enum pm_ret_status pm_set_max_latency(uint32_t device_id, uint32_t latency,
-				      uint32_t flag);
 enum pm_ret_status pm_register_notifier(uint32_t device_id, uint32_t event,
 					uint32_t wake, uint32_t enable,
 					uint32_t flag);

diff --git a/plat/xilinx/versal/pm_service/pm_client.c b/plat/xilinx/versal/pm_service/pm_client.c
index 4c1d340..ce5e533 100644
--- a/plat/xilinx/versal/pm_service/pm_client.c
+++ b/plat/xilinx/versal/pm_service/pm_client.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -21,6 +21,7 @@
 #include <plat/common/platform.h>
 #include "pm_api_sys.h"
 #include "pm_client.h"
+#include "pm_defs.h"
 
 #define UNDEFINED_CPUID		(~0)
 #define IRQ_MAX		142U
@@ -104,7 +105,7 @@
  *
  * Return:	PM node index corresponding to the specified interrupt
  */
-static enum pm_device_node_idx irq_to_pm_node_idx(unsigned int irq)
+static enum pm_device_node_idx irq_to_pm_node_idx(uint32_t irq)
 {
 	assert(irq <= IRQ_MAX);
 	return irq_node_map[irq];
@@ -147,14 +148,16 @@
 			node_idx = irq_to_pm_node_idx(irq);
 			reg &= ~lowest_set;
 
-			if ((node_idx != XPM_NODEIDX_DEV_MIN) &&
-			    (pm_wakeup_nodes_set[node_idx] == 0U)) {
-				/* Get device ID from node index */
-				device_id = PERIPH_DEVID(node_idx);
-				ret = pm_set_wakeup_source(node_id,
-							   device_id, 1,
-							   SECURE_FLAG);
-				pm_wakeup_nodes_set[node_idx] = (uint8_t)(!ret);
+			if (node_idx > XPM_NODEIDX_DEV_MIN && node_idx < XPM_NODEIDX_DEV_MAX) {
+				if (pm_wakeup_nodes_set[node_idx] == 0U) {
+					/* Get device ID from node index */
+					device_id = PERIPH_DEVID(node_idx);
+					ret = pm_set_wakeup_source(node_id,
+								   device_id, 1,
+								   SECURE_FLAG);
+					pm_wakeup_nodes_set[node_idx] = (ret == PM_RET_SUCCESS) ?
+											 1 : 0;
+				}
 			}
 		}
 	}
@@ -167,7 +170,7 @@
  * required prior to sending suspend request to PMU
  * Actions taken depend on the state system is suspending to.
  */
-void pm_client_suspend(const struct pm_proc *proc, unsigned int state)
+void pm_client_suspend(const struct pm_proc *proc, uint32_t state)
 {
 	bakery_lock_get(&pm_client_secure_lock);
 
@@ -208,7 +211,7 @@
  *
  * Return: the cpu ID (starting from 0) for the subsystem
  */
-static unsigned int pm_get_cpuid(uint32_t nid)
+static uint32_t pm_get_cpuid(uint32_t nid)
 {
 	for (size_t i = 0U; i < ARRAY_SIZE(pm_procs_all); i++) {
 		if (pm_procs_all[i].node_id == nid) {
@@ -226,7 +229,7 @@
  */
 void pm_client_wakeup(const struct pm_proc *proc)
 {
-	unsigned int cpuid = pm_get_cpuid(proc->node_id);
+	uint32_t cpuid = pm_get_cpuid(proc->node_id);
 
 	if (cpuid == UNDEFINED_CPUID) {
 		return;
@@ -248,7 +251,7 @@
  *
  * Return: pointer to a proc structure if proc is found, otherwise NULL
  */
-const struct pm_proc *pm_get_proc(unsigned int cpuid)
+const struct pm_proc *pm_get_proc(uint32_t cpuid)
 {
 	if (cpuid < ARRAY_SIZE(pm_procs_all)) {
 		return &pm_procs_all[cpuid];

diff --git a/plat/xilinx/versal/pm_service/pm_defs.h b/plat/xilinx/versal/pm_service/pm_defs.h
index 08b46e2..9206120 100644
--- a/plat/xilinx/versal/pm_service/pm_defs.h
+++ b/plat/xilinx/versal/pm_service/pm_defs.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -35,16 +35,15 @@
 
 #define PM_GET_CALLBACK_DATA		0xa01U
 #define PM_GET_TRUSTZONE_VERSION	0xa03U
+#define TF_A_PM_REGISTER_SGI		0xa04U
 
 /* PM API Versions */
 #define PM_API_BASE_VERSION		1U
+#define PM_API_VERSION_2                2U
 
 #define PM_API_QUERY_DATA_VERSION	2U
 
 /* PM API ids */
-#define PM_GET_API_VERSION		1U
-#define PM_GET_DEVICE_STATUS		3U
-#define PM_GET_OP_CHARACTERISTIC	4U
 #define PM_REGISTER_NOTIFIER		5U
 #define PM_REQ_SUSPEND			6U
 #define PM_SELF_SUSPEND			7U
@@ -53,31 +52,8 @@
 #define PM_REQ_WAKEUP			10U
 #define PM_SET_WAKEUP_SOURCE		11U
 #define PM_SYSTEM_SHUTDOWN		12U
-#define PM_REQUEST_DEVICE		13U
-#define PM_RELEASE_DEVICE		14U
-#define PM_SET_REQUIREMENT		15U
-#define PM_SET_MAX_LATENCY		16U
-#define PM_RESET_ASSERT			17U
-#define PM_RESET_GET_STATUS		18U
-#define PM_INIT_FINALIZE		21U
-#define PM_GET_CHIPID			24U
-#define	PM_PINCTRL_REQUEST		28U
-#define	PM_PINCTRL_RELEASE		29U
-#define	PM_PINCTRL_GET_FUNCTION		30U
-#define	PM_PINCTRL_SET_FUNCTION		31U
-#define	PM_PINCTRL_CONFIG_PARAM_GET	32U
-#define	PM_PINCTRL_CONFIG_PARAM_SET	33U
 #define PM_IOCTL			34U
 #define PM_QUERY_DATA			35U
-#define PM_CLOCK_ENABLE			36U
-#define PM_CLOCK_DISABLE		37U
-#define PM_CLOCK_GETSTATE		38U
-#define PM_CLOCK_SETDIVIDER		39U
-#define PM_CLOCK_GETDIVIDER		40U
-#define PM_CLOCK_SETRATE		41U
-#define PM_CLOCK_GETRATE		42U
-#define PM_CLOCK_SETPARENT		43U
-#define PM_CLOCK_GETPARENT		44U
 #define PM_PLL_SET_PARAMETER		48U
 #define PM_PLL_GET_PARAMETER		49U
 #define PM_PLL_SET_MODE			50U

diff --git a/plat/xilinx/versal/pm_service/pm_svc_main.c b/plat/xilinx/versal/pm_service/pm_svc_main.c
index b082acb..f4d04b8 100644
--- a/plat/xilinx/versal/pm_service/pm_svc_main.c
+++ b/plat/xilinx/versal/pm_service/pm_svc_main.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -25,19 +25,19 @@
 
 /* pm_up = true - UP, pm_up = false - DOWN */
 static bool pm_up;
-static unsigned int sgi = (unsigned int)INVALID_SGI;
+static uint32_t sgi = (uint32_t)INVALID_SGI;
 
 static uint64_t ipi_fiq_handler(uint32_t id, uint32_t flags, void *handle,
 				void *cookie)
 {
-	unsigned int cpu;
-	unsigned int reg;
+	uint32_t cpu;
+	uint32_t reg;
 
 	(void)plat_ic_acknowledge_interrupt();
 	cpu = plat_my_core_pos() + 1U;
 
-	if ((unsigned int)sgi != (unsigned int)INVALID_SGI) {
-		reg = (cpu | ((unsigned int)sgi << (unsigned int)XSCUGIC_SGIR_EL1_INITID_SHIFT));
+	if ((uint32_t)sgi != (uint32_t)INVALID_SGI) {
+		reg = (cpu | ((uint32_t)sgi << (uint32_t)XSCUGIC_SGIR_EL1_INITID_SHIFT));
 		write_icc_asgi1r_el1(reg);
 	}
 
@@ -51,6 +51,7 @@
  * pm_register_sgi() - PM register the IPI interrupt
  *
  * @sgi -  SGI number to be used for communication.
+ * @reset -  Reset to invalid SGI when reset=1.
  * @return	On success, the initialization function must return 0.
  *		Any other return value will cause the framework to ignore
  *		the service
@@ -58,9 +59,14 @@
  * Update the SGI number to be used.
  *
  */
-int pm_register_sgi(unsigned int sgi_num)
+int pm_register_sgi(uint32_t sgi_num, uint32_t reset)
 {
-	if ((unsigned int)sgi != (unsigned int)INVALID_SGI) {
+	if (reset == 1U) {
+		sgi = INVALID_SGI;
+		return 0;
+	}
+
+	if (sgi != INVALID_SGI) {
 		return -EBUSY;
 	}
 
@@ -68,7 +74,7 @@
 		return -EINVAL;
 	}
 
-	sgi = (unsigned int)sgi_num;
+	sgi = (uint32_t)sgi_num;
 	return 0;
 }
 
@@ -87,7 +93,7 @@
  */
 int pm_setup(void)
 {
-	int status, ret = 0;
+	int32_t status, ret = 0;
 
 	status = pm_ipi_init(primary_proc);
 
@@ -114,49 +120,84 @@
 }
 
 /**
- * pm_smc_handler() - SMC handler for PM-API calls coming from EL1/EL2.
- * @smc_fid - Function Identifier
- * @x1 - x4 - Arguments
- * @cookie  - Unused
- * @handler - Pointer to caller's context structure
- *
- * @return  - Unused
+ * eemi_for_compatibility() - EEMI calls handler for deprecated calls
  *
- * Determines that smc_fid is valid and supported PM SMC Function ID from the
- * list of pm_api_ids, otherwise completes the request with
- * the unknown SMC Function ID
+ * @return - If EEMI API found then, uintptr_t type address, else 0
  *
- * The SMC calls for PM service are forwarded from SIP Service SMC handler
- * function with rt_svc_handle signature
+ * Some EEMI API's use case needs to be changed in Linux driver, so they
+ * can take advantage of common EEMI handler in TF-A. As of now the old
+ * implementation of these APIs are required to maintain backward compatibility
+ * until their use case in linux driver changes.
  */
-uint64_t pm_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3,
-			uint64_t x4, void *cookie, void *handle, uint64_t flags)
+static uintptr_t eemi_for_compatibility(uint32_t api_id, uint32_t *pm_arg,
+					void *handle, uint32_t security_flag)
 {
 	enum pm_ret_status ret;
 
-	uint32_t pm_arg[4];
-	uint32_t security_flag = SECURE_FLAG;
+	switch (api_id) {
 
-	/* Handle case where PM wasn't initialized properly */
-	if (pm_up == false) {
-		SMC_RET1(handle, SMC_UNK);
+	case PM_IOCTL:
+	{
+		uint32_t value;
+
+		ret = pm_api_ioctl(pm_arg[0], pm_arg[1], pm_arg[2],
+				   pm_arg[3], pm_arg[4],
+				   &value, security_flag);
+		if (ret == PM_RET_ERROR_NOTSUPPORTED)
+			return (uintptr_t)0;
+
+		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32);
 	}
 
-	pm_arg[0] = (uint32_t)x1;
-	pm_arg[1] = (uint32_t)(x1 >> 32);
-	pm_arg[2] = (uint32_t)x2;
-	pm_arg[3] = (uint32_t)(x2 >> 32);
+	case PM_QUERY_DATA:
+	{
+		uint32_t data[PAYLOAD_ARG_CNT] = { 0 };
 
-	/*
-	 * Mark BIT24 payload (i.e 1st bit of pm_arg[3] ) as non-secure (1)
-	 * if smc called is non secure
-	 */
-	if (is_caller_non_secure(flags) != 0) {
-		security_flag = NON_SECURE_FLAG;
+		ret = pm_query_data(pm_arg[0], pm_arg[1], pm_arg[2],
+				      pm_arg[3], data, security_flag);
+
+		SMC_RET2(handle, (uint64_t)ret  | ((uint64_t)data[0] << 32),
+				 (uint64_t)data[1] | ((uint64_t)data[2] << 32));
 	}
 
-	switch (smc_fid & FUNCID_NUM_MASK) {
-	/* PM API Functions */
+	case PM_FEATURE_CHECK:
+	{
+		uint32_t result[PAYLOAD_ARG_CNT] = {0U};
+
+		ret = pm_feature_check(pm_arg[0], result, security_flag);
+		SMC_RET2(handle, (uint64_t)ret | ((uint64_t)result[0] << 32),
+			 (uint64_t)result[1] | ((uint64_t)result[2] << 32));
+	}
+
+	case PM_LOAD_PDI:
+	{
+		ret = pm_load_pdi(pm_arg[0], pm_arg[1], pm_arg[2],
+				  security_flag);
+		SMC_RET1(handle, (uint64_t)ret);
+	}
+
+	default:
+		return (uintptr_t)0;
+	}
+}
+
+/**
+ * eemi_psci_debugfs_handler() - EEMI API invoked from PSCI
+ *
+ * These EEMI APIs performs CPU specific power management tasks.
+ * These EEMI APIs are invoked either from PSCI or from debugfs in kernel.
+ * These calls require CPU specific processing before sending IPI request to
+ * Platform Management Controller. For example enable/disable CPU specific
+ * interrupts. This requires separate handler for these calls and may not be
+ * handled using common eemi handler
+ */
+static uintptr_t eemi_psci_debugfs_handler(uint32_t api_id, uint32_t *pm_arg,
+					   void *handle, uint32_t security_flag)
+{
+	enum pm_ret_status ret;
+
+	switch (api_id) {
+
 	case PM_SELF_SUSPEND:
 		ret = pm_self_suspend(pm_arg[0], pm_arg[1], pm_arg[2],
 				      pm_arg[3], security_flag);
@@ -179,65 +220,34 @@
 		ret = pm_system_shutdown(pm_arg[0], pm_arg[1], security_flag);
 		SMC_RET1(handle, (uint64_t)ret);
 
-	case PM_REQ_WAKEUP:
-		ret = pm_req_wakeup(pm_arg[0], pm_arg[1], pm_arg[2], pm_arg[3],
-				    security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_SET_WAKEUP_SOURCE:
-		ret = pm_set_wakeup_source(pm_arg[0], pm_arg[1], pm_arg[2],
-					   security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_REQUEST_DEVICE:
-		ret = pm_request_device(pm_arg[0], pm_arg[1], pm_arg[2],
-					pm_arg[3], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_RELEASE_DEVICE:
-		ret = pm_release_device(pm_arg[0], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_SET_REQUIREMENT:
-		ret = pm_set_requirement(pm_arg[0], pm_arg[1], pm_arg[2],
-					 pm_arg[3], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_GET_API_VERSION:
-	{
-		uint32_t api_version;
-
-		ret = pm_get_api_version(&api_version, security_flag);
-		SMC_RET1(handle, (u_register_t)PM_RET_SUCCESS |
-				 ((u_register_t)api_version << 32));
-	}
-
-	case PM_GET_DEVICE_STATUS:
-	{
-		uint32_t buff[3];
-
-		ret = pm_get_device_status(pm_arg[0], buff, security_flag);
-		SMC_RET2(handle, (u_register_t)ret | ((u_register_t)buff[0] << 32),
-			 (u_register_t)buff[1] | ((u_register_t)buff[2] << 32));
+	default:
+		return (uintptr_t)0;
 	}
+}
 
-	case PM_RESET_ASSERT:
-		ret = pm_reset_assert(pm_arg[0], pm_arg[1], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
+/**
+ * TF_A_specific_handler() - SMC handler for TF-A specific functionality
+ *
+ * These EEMI calls performs functionality that does not require
+ * IPI transaction. The handler ends in TF-A and returns requested data to
+ * kernel from TF-A
+ */
+static uintptr_t TF_A_specific_handler(uint32_t api_id, uint32_t *pm_arg,
+				       void *handle, uint32_t security_flag)
+{
+	switch (api_id) {
 
-	case PM_RESET_GET_STATUS:
+	case TF_A_PM_REGISTER_SGI:
 	{
-		uint32_t reset_status;
+		int32_t ret;
 
-		ret = pm_reset_get_status(pm_arg[0], &reset_status,
-					  security_flag);
-		SMC_RET1(handle, (u_register_t)ret |
-			 ((u_register_t)reset_status << 32));
-	}
+		ret = pm_register_sgi(pm_arg[0], pm_arg[1]);
+		if (ret != 0) {
+			SMC_RET1(handle, (uint32_t)PM_RET_ERROR_ARGS);
+		}
 
-	case PM_INIT_FINALIZE:
-		ret = pm_init_finalize(security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
+		SMC_RET1(handle, (uint32_t)PM_RET_SUCCESS);
+	}
 
 	case PM_GET_CALLBACK_DATA:
 	{
@@ -245,192 +255,119 @@
 
 		pm_get_callbackdata(result, ARRAY_SIZE(result), security_flag);
 		SMC_RET2(handle,
-			 (u_register_t)result[0] | ((u_register_t)result[1] << 32),
-			 (u_register_t)result[2] | ((u_register_t)result[3] << 32));
-	}
-
-	case PM_PINCTRL_REQUEST:
-		ret = pm_pinctrl_request(pm_arg[0], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_PINCTRL_RELEASE:
-		ret = pm_pinctrl_release(pm_arg[0], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_PINCTRL_GET_FUNCTION:
-	{
-		uint32_t value = 0;
-
-		ret = pm_pinctrl_get_function(pm_arg[0], &value, security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)value) << 32);
-	}
-
-	case PM_PINCTRL_SET_FUNCTION:
-		ret = pm_pinctrl_set_function(pm_arg[0], pm_arg[1],
-					      security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_PINCTRL_CONFIG_PARAM_GET:
-	{
-		uint32_t value;
-
-		ret = pm_pinctrl_get_pin_param(pm_arg[0], pm_arg[1], &value,
-					       security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)value) << 32);
-	}
-
-	case PM_PINCTRL_CONFIG_PARAM_SET:
-		ret = pm_pinctrl_set_pin_param(pm_arg[0], pm_arg[1], pm_arg[2],
-					       security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_IOCTL:
-	{
-		uint32_t value;
-
-		ret = pm_api_ioctl(pm_arg[0], pm_arg[1], pm_arg[2],
-				   pm_arg[3], &value, security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)value) << 32);
-	}
-
-	case PM_QUERY_DATA:
-	{
-		uint32_t data[8] = { 0 };
-
-		ret = pm_query_data(pm_arg[0], pm_arg[1], pm_arg[2],
-				      pm_arg[3], data, security_flag);
-
-		SMC_RET2(handle, (u_register_t)ret  | ((u_register_t)data[0] << 32),
-				 (u_register_t)data[1] | ((u_register_t)data[2] << 32));
-
-	}
-	case PM_CLOCK_ENABLE:
-		ret = pm_clock_enable(pm_arg[0], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_CLOCK_DISABLE:
-		ret = pm_clock_disable(pm_arg[0], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_CLOCK_GETSTATE:
-	{
-		uint32_t value;
-
-		ret = pm_clock_get_state(pm_arg[0], &value, security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)value) << 32);
-	}
-
-	case PM_CLOCK_SETDIVIDER:
-		ret = pm_clock_set_divider(pm_arg[0], pm_arg[1], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_CLOCK_GETDIVIDER:
-	{
-		uint32_t value;
-
-		ret = pm_clock_get_divider(pm_arg[0], &value, security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)value) << 32);
+			(uint64_t)result[0] | ((uint64_t)result[1] << 32),
+			(uint64_t)result[2] | ((uint64_t)result[3] << 32));
 	}
 
-	case PM_CLOCK_SETPARENT:
-		ret = pm_clock_set_parent(pm_arg[0], pm_arg[1], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
-
-	case PM_CLOCK_GETPARENT:
-	{
-		uint32_t value;
-
-		ret = pm_clock_get_parent(pm_arg[0], &value, security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)value) << 32);
-	}
-
-	case PM_CLOCK_GETRATE:
-	{
-		uint32_t rate[2] = { 0 };
+	case PM_GET_TRUSTZONE_VERSION:
+		SMC_RET1(handle, (uint64_t)PM_RET_SUCCESS |
+			 ((uint64_t)VERSAL_TZ_VERSION << 32));
 
-		ret = pm_clock_get_rate(pm_arg[0], rate, security_flag);
-		SMC_RET2(handle, (u_register_t)ret | ((u_register_t)rate[0] << 32),
-			 (u_register_t)rate[1] | ((u_register_t)0U << 32));
+	default:
+		return (uintptr_t)0;
 	}
-
-	case PM_PLL_SET_PARAMETER:
-		ret = pm_pll_set_param(pm_arg[0], pm_arg[1], pm_arg[2],
-				       security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
+}
 
-	case PM_PLL_GET_PARAMETER:
-	{
-		uint32_t value;
+/**
+ * eemi_handler() - Prepare EEMI payload and perform IPI transaction
+ *
+ * EEMI - Embedded Energy Management Interface is Xilinx proprietary protocol
+ * to allow communication between power management controller and different
+ * processing clusters.
+ *
+ * This handler prepares EEMI protocol payload received from kernel and performs
+ * IPI transaction.
+ */
+static uintptr_t eemi_handler(uint32_t api_id, uint32_t *pm_arg,
+			      void *handle, uint32_t security_flag)
+{
+	enum pm_ret_status ret;
+	uint32_t buf[PAYLOAD_ARG_CNT] = {0};
 
-		ret = pm_pll_get_param(pm_arg[0], pm_arg[1], &value,
-				       security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)value << 32));
+	ret = pm_handle_eemi_call(security_flag, api_id, pm_arg[0], pm_arg[1],
+				  pm_arg[2], pm_arg[3], pm_arg[4],
+				  (uint64_t *)buf);
+	/*
+	 * Two IOCTLs, to get clock name and pinctrl name of pm_query_data API
+	 * receives 5 words of respoonse from firmware. Currently linux driver can
+	 * receive only 4 words from TF-A. So, this needs to be handled separately
+	 * than other eemi calls.
+	 */
+	if (api_id == PM_QUERY_DATA) {
+		if ((pm_arg[0] == XPM_QID_CLOCK_GET_NAME ||
+		    pm_arg[0] == XPM_QID_PINCTRL_GET_FUNCTION_NAME) &&
+		    ret == PM_RET_SUCCESS) {
+			SMC_RET2(handle, (uint64_t)buf[0] | ((uint64_t)buf[1] << 32),
+				(uint64_t)buf[2] | ((uint64_t)buf[3] << 32));
+		}
 	}
 
-	case PM_PLL_SET_MODE:
-		ret = pm_pll_set_mode(pm_arg[0], pm_arg[1], security_flag);
-		SMC_RET1(handle, (uint64_t)ret);
+	SMC_RET2(handle, (uint64_t)ret | ((uint64_t)buf[0] << 32),
+		 (uint64_t)buf[1] | ((uint64_t)buf[2] << 32));
+}
 
-	case PM_PLL_GET_MODE:
-	{
-		uint32_t mode;
+/**
+ * pm_smc_handler() - SMC handler for PM-API calls coming from EL1/EL2.
+ * @smc_fid - Function Identifier
+ * @x1 - x4 - SMC64 Arguments from kernel
+ *	      x3 (upper 32-bits) and x4 are Unused
+ * @cookie  - Unused
+ * @handler - Pointer to caller's context structure
+ *
+ * @return  - Unused
+ *
+ * Determines that smc_fid is valid and supported PM SMC Function ID from the
+ * list of pm_api_ids, otherwise completes the request with
+ * the unknown SMC Function ID
+ *
+ * The SMC calls for PM service are forwarded from SIP Service SMC handler
+ * function with rt_svc_handle signature
+ */
+uint64_t pm_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3,
+			uint64_t x4, const void *cookie, void *handle, uint64_t flags)
+{
+	uintptr_t ret;
+	uint32_t pm_arg[PAYLOAD_ARG_CNT] = {0};
+	uint32_t security_flag = SECURE_FLAG;
+	uint32_t api_id;
 
-		ret = pm_pll_get_mode(pm_arg[0], &mode, security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)mode << 32));
+	/* Handle case where PM wasn't initialized properly */
+	if (!pm_up) {
+		SMC_RET1(handle, SMC_UNK);
 	}
 
-	case PM_GET_TRUSTZONE_VERSION:
-		SMC_RET1(handle, (uint64_t)PM_RET_SUCCESS |
-			 ((uint64_t)VERSAL_TZ_VERSION << 32));
-
-	case PM_GET_CHIPID:
-	{
-		uint32_t result[2];
-
-		ret = pm_get_chipid(result, security_flag);
-		SMC_RET2(handle, (u_register_t)ret | ((u_register_t)result[0] << 32),
-			 (u_register_t)result[1] | ((u_register_t)0U << 32));
+	/*
+	 * Mark BIT24 payload (i.e 1st bit of pm_arg[3] ) as non-secure (1)
+	 * if smc called is non secure
+	 */
+	if (is_caller_non_secure(flags)) {
+		security_flag = NON_SECURE_FLAG;
 	}
 
-	case PM_FEATURE_CHECK:
-	{
-		uint32_t version;
-
-		ret = pm_feature_check(pm_arg[0], &version, security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)version << 32));
-	}
+	pm_arg[0] = (uint32_t)x1;
+	pm_arg[1] = (uint32_t)(x1 >> 32);
+	pm_arg[2] = (uint32_t)x2;
+	pm_arg[3] = (uint32_t)(x2 >> 32);
+	pm_arg[4] = (uint32_t)x3;
+	(void)(x4);
+	api_id = smc_fid & FUNCID_NUM_MASK;
 
-	case PM_LOAD_PDI:
-	{
-		ret = pm_load_pdi(pm_arg[0], pm_arg[1], pm_arg[2],
-				  security_flag);
-		SMC_RET1(handle, (u_register_t)ret);
+	ret = eemi_for_compatibility(api_id, pm_arg, handle, security_flag);
+	if (ret != (uintptr_t)0) {
+		return ret;
 	}
 
-	case PM_GET_OP_CHARACTERISTIC:
-	{
-		uint32_t result;
-
-		ret = pm_get_op_characteristic(pm_arg[0], pm_arg[1], &result,
-					       security_flag);
-		SMC_RET1(handle, (u_register_t)ret | ((u_register_t)result << 32));
+	ret = eemi_psci_debugfs_handler(api_id, pm_arg, handle, flags);
+	if (ret !=  (uintptr_t)0) {
+		return ret;
 	}
 
-	case PM_SET_MAX_LATENCY:
-	{
-		ret = pm_set_max_latency(pm_arg[0], pm_arg[1], security_flag);
-		SMC_RET1(handle, (u_register_t)ret);
+	ret = TF_A_specific_handler(api_id, pm_arg, handle, security_flag);
+	if (ret !=  (uintptr_t)0) {
+		return ret;
 	}
 
-	case PM_REGISTER_NOTIFIER:
-	{
-		ret = pm_register_notifier(pm_arg[0], pm_arg[1], pm_arg[2],
-					   pm_arg[3], security_flag);
-		SMC_RET1(handle, (u_register_t)ret);
-	}
+	ret = eemi_handler(api_id, pm_arg, handle, security_flag);
 
-	default:
-		WARN("Unimplemented PM Service Call: 0x%x\n", smc_fid);
-		SMC_RET1(handle, SMC_UNK);
-	}
+	return ret;
 }

diff --git a/plat/xilinx/versal/pm_service/pm_svc_main.h b/plat/xilinx/versal/pm_service/pm_svc_main.h
index 4f8dc2b..b6e764f 100644
--- a/plat/xilinx/versal/pm_service/pm_svc_main.h
+++ b/plat/xilinx/versal/pm_service/pm_svc_main.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Xilinx, Inc. All rights reserved.
+ * Copyright (c) 2019-2022, Xilinx, Inc. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,10 +9,10 @@
 
 #include <pm_common.h>
 
-int pm_setup(void);
+int32_t pm_setup(void);
 uint64_t pm_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3,
-			uint64_t x4, void *cookie, void *handle,
+			uint64_t x4, const void *cookie, void *handle,
 			uint64_t flags);
 
-int pm_register_sgi(unsigned int sgi_num);
+int32_t pm_register_sgi(uint32_t sgi_num, uint32_t reset);
 #endif /* PM_SVC_MAIN_H */

diff --git a/plat/xilinx/versal/versal_gicv3.c b/plat/xilinx/versal/versal_gicv3.c
index 08e7cf9..d410906 100644
--- a/plat/xilinx/versal/versal_gicv3.c
+++ b/plat/xilinx/versal/versal_gicv3.c

@@ -53,7 +53,7 @@
  *   - All CPUs implemented in the system have MPIDR_EL1.MT bit set;
  *   - No CPUs implemented in the system use affinity level 3.
  */
-static unsigned int versal_gicv3_mpidr_hash(u_register_t mpidr)
+static uint32_t versal_gicv3_mpidr_hash(u_register_t mpidr)
 {
 	mpidr |= (read_mpidr_el1() & MPIDR_MT_MASK);
 	return versal_calc_core_pos(mpidr);

diff --git a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
index fae73cf..3946e9b 100644
--- a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
+++ b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -29,7 +29,7 @@
 	{0}
 };
 
-static unsigned int zynqmp_get_silicon_ver(void)
+static uint32_t zynqmp_get_silicon_ver(void)
 {
 	static unsigned int ver;
 
@@ -43,20 +43,21 @@
 	return ver;
 }
 
-unsigned int zynqmp_get_uart_clk(void)
+uint32_t zynqmp_get_uart_clk(void)
 {
 	unsigned int ver = zynqmp_get_silicon_ver();
 
-	if (ver == ZYNQMP_CSU_VERSION_QEMU)
+	if (ver == ZYNQMP_CSU_VERSION_QEMU) {
 		return 133000000;
-	else
+	} else {
 		return 100000000;
+	}
 }
 
 #if LOG_LEVEL >= LOG_LEVEL_NOTICE
 static const struct {
-	unsigned int id;
-	unsigned int ver;
+	uint32_t id;
+	uint32_t ver;
 	char *name;
 	bool evexists;
 } zynqmp_devices[] = {
@@ -214,7 +215,8 @@
 #define ZYNQMP_PL_STATUS_MASK	BIT(ZYNQMP_PL_STATUS_BIT)
 #define ZYNQMP_CSU_VERSION_MASK	~(ZYNQMP_PL_STATUS_MASK)
 
-#define SILICON_ID_XCK26       0x4724093
+#define SILICON_ID_XCK24	0x4714093U
+#define SILICON_ID_XCK26	0x4724093U
 
 static char *zynqmp_get_silicon_idcode_name(void)
 {
@@ -232,8 +234,9 @@
 	chipid[0] = mmio_read_32(ZYNQMP_CSU_BASEADDR + ZYNQMP_CSU_IDCODE_OFFSET);
 	chipid[1] = mmio_read_32(EFUSE_BASEADDR + EFUSE_IPDISABLE_OFFSET);
 #else
-	if (pm_get_chipid(chipid) != PM_RET_SUCCESS)
+	if (pm_get_chipid(chipid) != PM_RET_SUCCESS) {
 		return "XCZUUNKN";
+	}
 #endif
 
 	id = chipid[0] & (ZYNQMP_CSU_IDCODE_DEVICE_CODE_MASK |
@@ -243,23 +246,29 @@
 
 	for (i = 0; i < ARRAY_SIZE(zynqmp_devices); i++) {
 		if (zynqmp_devices[i].id == id &&
-		    zynqmp_devices[i].ver == (ver & ZYNQMP_CSU_VERSION_MASK))
+		    zynqmp_devices[i].ver == (ver & ZYNQMP_CSU_VERSION_MASK)) {
 			break;
+		}
 	}
 
 	if (i >= ARRAY_SIZE(zynqmp_devices)) {
-		if (chipid[0] == SILICON_ID_XCK26) {
+		switch (chipid[0]) {
+		case SILICON_ID_XCK24:
+			return "XCK24";
+		case SILICON_ID_XCK26:
 			return "XCK26";
-		} else {
+		default:
 			return "XCZUUNKN";
 		}
 	}
 
-	if (!zynqmp_devices[i].evexists)
+	if (!zynqmp_devices[i].evexists) {
 		return zynqmp_devices[i].name;
+	}
 
-	if (ver & ZYNQMP_PL_STATUS_MASK)
+	if ((ver & ZYNQMP_PL_STATUS_MASK) != 0U) {
 		return zynqmp_devices[i].name;
+	}
 
 	len = strlen(zynqmp_devices[i].name) - 2;
 	for (j = 0; j < strlen(name); j++) {
@@ -301,20 +310,20 @@
 	return zynqmp_get_silicon_idcode_name();
 }
 
-static unsigned int zynqmp_get_ps_ver(void)
+static uint32_t zynqmp_get_ps_ver(void)
 {
 	uint32_t ver = mmio_read_32(ZYNQMP_CSU_BASEADDR + ZYNQMP_CSU_VERSION_OFFSET);
 
 	ver &= ZYNQMP_PS_VER_MASK;
 	ver >>= ZYNQMP_PS_VER_SHIFT;
 
-	return ver + 1;
+	return ver + 1U;
 }
 
 static void zynqmp_print_platform_name(void)
 {
-	unsigned int ver = zynqmp_get_silicon_ver();
-	unsigned int rtl = zynqmp_get_rtl_ver();
+	uint32_t ver = zynqmp_get_silicon_ver();
+	uint32_t rtl = zynqmp_get_rtl_ver();
 	char *label = "Unknown";
 
 	switch (ver) {
@@ -329,8 +338,8 @@
 		break;
 	}
 
-	NOTICE("TF-A running on %s/%s at 0x%x\n",
-	       zynqmp_print_silicon_idcode(), label, BL31_BASE);
+	VERBOSE("TF-A running on %s/%s at 0x%x\n",
+		zynqmp_print_silicon_idcode(), label, BL31_BASE);
 	VERBOSE("TF-A running on v%d/RTL%d.%d\n",
 	       zynqmp_get_ps_ver(), (rtl & 0xf0) >> 4, rtl & 0xf);
 }
@@ -338,15 +347,16 @@
 static inline void zynqmp_print_platform_name(void) { }
 #endif
 
-unsigned int zynqmp_get_bootmode(void)
+uint32_t zynqmp_get_bootmode(void)
 {
 	uint32_t r;
 	unsigned int ret;
 
 	ret = pm_mmio_read(CRL_APB_BOOT_MODE_USER, &r);
 
-	if (ret != PM_RET_SUCCESS)
+	if (ret != PM_RET_SUCCESS) {
 		r = mmio_read_32(CRL_APB_BOOT_MODE_USER);
+	}
 
 	return r & CRL_APB_BOOT_MODE_MASK;
 }
@@ -369,12 +379,13 @@
 	generic_delay_timer_init();
 }
 
-unsigned int plat_get_syscnt_freq2(void)
+uint32_t plat_get_syscnt_freq2(void)
 {
-	unsigned int ver = zynqmp_get_silicon_ver();
+	uint32_t ver = zynqmp_get_silicon_ver();
 
-	if (ver == ZYNQMP_CSU_VERSION_QEMU)
+	if (ver == ZYNQMP_CSU_VERSION_QEMU) {
 		return 65000000;
-	else
+	} else {
 		return mmio_read_32(IOU_SCNTRS_BASEFREQ);
+	}
 }

diff --git a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
index 58eee3a..38ad32b 100644
--- a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
+++ b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c

@@ -33,15 +33,18 @@
  * while BL32 corresponds to the secure image type. A NULL pointer is returned
  * if the image does not exist.
  */
-entry_point_info_t *bl31_plat_get_next_image_ep_info(uint32_t type)
+struct entry_point_info *bl31_plat_get_next_image_ep_info(uint32_t type)
 {
-	assert(sec_state_is_valid(type));
+	entry_point_info_t *next_image_info;
 
+	assert(sec_state_is_valid(type));
 	if (type == NON_SECURE) {
-		return &bl33_image_ep_info;
+		next_image_info = &bl33_image_ep_info;
+	} else {
+		next_image_info = &bl32_image_ep_info;
 	}
 
-	return &bl32_image_ep_info;
+	return next_image_info;
 }
 
 /*
@@ -79,10 +82,12 @@
 				  CONSOLE_FLAG_RUNTIME | CONSOLE_FLAG_BOOT);
 	} else if (ZYNQMP_CONSOLE_IS(dcc)) {
 		/* Initialize the dcc console for debug */
-		int rc = console_dcc_register();
+		int32_t rc = console_dcc_register();
 		if (rc == 0) {
 			panic();
 		}
+	} else {
+		ERROR("BL31: No console device found.\n");
 	}
 	/* Initialize the platform config for future decision making */
 	zynqmp_config_setup();
@@ -119,10 +124,10 @@
 			panic();
 		}
 	}
-	if (bl32_image_ep_info.pc) {
+	if (bl32_image_ep_info.pc != 0) {
 		VERBOSE("BL31: Secure code at 0x%lx\n", bl32_image_ep_info.pc);
 	}
-	if (bl33_image_ep_info.pc) {
+	if (bl33_image_ep_info.pc != 0) {
 		VERBOSE("BL31: Non secure code at 0x%lx\n", bl33_image_ep_info.pc);
 	}
 }

diff --git a/plat/xilinx/zynqmp/include/plat_private.h b/plat/xilinx/zynqmp/include/plat_private.h
index 288cc53..534777b 100644
--- a/plat/xilinx/zynqmp/include/plat_private.h
+++ b/plat/xilinx/zynqmp/include/plat_private.h

@@ -15,11 +15,11 @@
 
 void zynqmp_config_setup(void);
 
-unsigned int zynqmp_calc_core_pos(u_register_t mpidr);
+uint32_t zynqmp_calc_core_pos(u_register_t mpidr);
 
 /* ZynqMP specific functions */
-unsigned int zynqmp_get_uart_clk(void);
-unsigned int zynqmp_get_bootmode(void);
+uint32_t zynqmp_get_uart_clk(void);
+uint32_t zynqmp_get_bootmode(void);
 
 
 #if ZYNQMP_WDT_RESTART

diff --git a/plat/xilinx/zynqmp/include/platform_def.h b/plat/xilinx/zynqmp/include/platform_def.h
index 1c4daa1..66bbf30 100644
--- a/plat/xilinx/zynqmp/include/platform_def.h
+++ b/plat/xilinx/zynqmp/include/platform_def.h

@@ -37,11 +37,11 @@
  */
 #ifndef ZYNQMP_ATF_MEM_BASE
 #if !DEBUG && defined(SPD_none) && !SDEI_SUPPORT
-# define BL31_BASE			0xfffea000
-# define BL31_LIMIT			0x100000000
+# define BL31_BASE			U(0xfffea000)
+# define BL31_LIMIT			U(0x100000000)
 #else
-# define BL31_BASE			0x1000
-# define BL31_LIMIT			0x7ffff
+# define BL31_BASE			U(0xfff5a000)
+# define BL31_LIMIT			U(0x100000000)
 #endif
 #else
 # define BL31_BASE			(ZYNQMP_ATF_MEM_BASE)
@@ -55,8 +55,8 @@
  * BL32 specific defines.
  ******************************************************************************/
 #ifndef ZYNQMP_BL32_MEM_BASE
-# define BL32_BASE			0x60000000
-# define BL32_LIMIT			0x7fffffff
+# define BL32_BASE			U(0x60000000)
+# define BL32_LIMIT			U(0x7fffffff)
 #else
 # define BL32_BASE			(ZYNQMP_BL32_MEM_BASE)
 # define BL32_LIMIT			(ZYNQMP_BL32_MEM_BASE + ZYNQMP_BL32_MEM_SIZE - 1)
@@ -66,7 +66,7 @@
  * BL33 specific defines.
  ******************************************************************************/
 #ifndef PRELOADED_BL33_BASE
-# define PLAT_ARM_NS_IMAGE_BASE	0x8000000
+# define PLAT_ARM_NS_IMAGE_BASE	U(0x8000000)
 #else
 # define PLAT_ARM_NS_IMAGE_BASE	PRELOADED_BL33_BASE
 #endif
@@ -83,9 +83,9 @@
 /*******************************************************************************
  * Platform specific page table and MMU setup constants
  ******************************************************************************/
-#define XILINX_OF_BOARD_DTB_ADDR	0x100000
-#define XILINX_OF_BOARD_DTB_MAX_SIZE	0x200000
-#define PLAT_DDR_LOWMEM_MAX		0x80000000
+#define XILINX_OF_BOARD_DTB_ADDR	U(0x100000)
+#define XILINX_OF_BOARD_DTB_MAX_SIZE	U(0x200000)
+#define PLAT_DDR_LOWMEM_MAX		U(0x80000000)
 
 #define PLAT_PHY_ADDR_SPACE_SIZE	(1ULL << 32)
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 32)

diff --git a/plat/xilinx/zynqmp/include/zynqmp_def.h b/plat/xilinx/zynqmp/include/zynqmp_def.h
index 7e58391..877127b 100644
--- a/plat/xilinx/zynqmp/include/zynqmp_def.h
+++ b/plat/xilinx/zynqmp/include/zynqmp_def.h

@@ -74,11 +74,11 @@
 #define ZYNQMP_ULPI_RESET_VAL_LOW	CRL_APB_BOOT_ENABLE_PIN_1
 
 /* system counter registers and bitfields */
-#define IOU_SCNTRS_BASE			0xFF260000
+#define IOU_SCNTRS_BASE			U(0xFF260000)
 #define IOU_SCNTRS_BASEFREQ		(IOU_SCNTRS_BASE + 0x20)
 
 /* APU registers and bitfields */
-#define APU_BASE		0xFD5C0000
+#define APU_BASE		U(0xFD5C0000)
 #define APU_CONFIG_0		(APU_BASE + 0x20)
 #define APU_RVBAR_L_0		(APU_BASE + 0x40)
 #define APU_RVBAR_H_0		(APU_BASE + 0x44)
@@ -91,7 +91,7 @@
 #define APU_3_PWRCTL_CPUPWRDWNREQ_MASK		8
 
 /* PMU registers and bitfields */
-#define PMU_GLOBAL_BASE			0xFFD80000
+#define PMU_GLOBAL_BASE			U(0xFFD80000)
 #define PMU_GLOBAL_CNTRL		(PMU_GLOBAL_BASE + 0)
 #define PMU_GLOBAL_GEN_STORAGE6		(PMU_GLOBAL_BASE + 0x48)
 #define PMU_GLOBAL_REQ_PWRUP_STATUS	(PMU_GLOBAL_BASE + 0x110)
@@ -104,22 +104,22 @@
 /*******************************************************************************
  * CCI-400 related constants
  ******************************************************************************/
-#define PLAT_ARM_CCI_BASE		0xFD6E0000
+#define PLAT_ARM_CCI_BASE		U(0xFD6E0000)
 #define PLAT_ARM_CCI_CLUSTER0_SL_IFACE_IX	3
 #define PLAT_ARM_CCI_CLUSTER1_SL_IFACE_IX	4
 
 /*******************************************************************************
  * GIC-400 & interrupt handling related constants
  ******************************************************************************/
-#define BASE_GICD_BASE		0xF9010000
-#define BASE_GICC_BASE		0xF9020000
-#define BASE_GICH_BASE		0xF9040000
-#define BASE_GICV_BASE		0xF9060000
+#define BASE_GICD_BASE		U(0xF9010000)
+#define BASE_GICC_BASE		U(0xF9020000)
+#define BASE_GICH_BASE		U(0xF9040000)
+#define BASE_GICV_BASE		U(0xF9060000)
 
 #if ZYNQMP_WDT_RESTART
 #define IRQ_SEC_IPI_APU		67
 #define IRQ_TTC3_1		77
-#define TTC3_BASE_ADDR		0xFF140000
+#define TTC3_BASE_ADDR		U(0xFF140000)
 #define TTC3_INTR_REGISTER_1	(TTC3_BASE_ADDR + 0x54)
 #define TTC3_INTR_ENABLE_1	(TTC3_BASE_ADDR + 0x60)
 #endif
@@ -140,8 +140,8 @@
 /*******************************************************************************
  * UART related constants
  ******************************************************************************/
-#define ZYNQMP_UART0_BASE		0xFF000000
-#define ZYNQMP_UART1_BASE		0xFF010000
+#define ZYNQMP_UART0_BASE		U(0xFF000000)
+#define ZYNQMP_UART1_BASE		U(0xFF010000)
 
 #if ZYNQMP_CONSOLE_IS(cadence) || ZYNQMP_CONSOLE_IS(dcc)
 # define ZYNQMP_UART_BASE	ZYNQMP_UART0_BASE
@@ -163,43 +163,43 @@
 #define ZYNQMP_CSU_VERSION_SILICON	0
 #define ZYNQMP_CSU_VERSION_QEMU		3
 
-#define ZYNQMP_RTL_VER_MASK		0xFF0
+#define ZYNQMP_RTL_VER_MASK		0xFF0U
 #define ZYNQMP_RTL_VER_SHIFT		4
 
-#define ZYNQMP_PS_VER_MASK		0xF
+#define ZYNQMP_PS_VER_MASK		0xFU
 #define ZYNQMP_PS_VER_SHIFT		0
 
-#define ZYNQMP_CSU_BASEADDR		0xFFCA0000
-#define ZYNQMP_CSU_IDCODE_OFFSET	0x40
+#define ZYNQMP_CSU_BASEADDR		U(0xFFCA0000)
+#define ZYNQMP_CSU_IDCODE_OFFSET	0x40U
 
-#define ZYNQMP_CSU_IDCODE_XILINX_ID_SHIFT	0
-#define ZYNQMP_CSU_IDCODE_XILINX_ID_MASK	(0xFFF << \
+#define ZYNQMP_CSU_IDCODE_XILINX_ID_SHIFT	0U
+#define ZYNQMP_CSU_IDCODE_XILINX_ID_MASK	(0xFFFU << \
 					ZYNQMP_CSU_IDCODE_XILINX_ID_SHIFT)
 #define ZYNQMP_CSU_IDCODE_XILINX_ID		0x093
 
-#define ZYNQMP_CSU_IDCODE_SVD_SHIFT		12
-#define ZYNQMP_CSU_IDCODE_SVD_MASK		(0x7 << \
+#define ZYNQMP_CSU_IDCODE_SVD_SHIFT		12U
+#define ZYNQMP_CSU_IDCODE_SVD_MASK		(0x7U << \
 						 ZYNQMP_CSU_IDCODE_SVD_SHIFT)
-#define ZYNQMP_CSU_IDCODE_DEVICE_CODE_SHIFT	15
-#define ZYNQMP_CSU_IDCODE_DEVICE_CODE_MASK	(0xF << \
+#define ZYNQMP_CSU_IDCODE_DEVICE_CODE_SHIFT	15U
+#define ZYNQMP_CSU_IDCODE_DEVICE_CODE_MASK	(0xFU << \
 					ZYNQMP_CSU_IDCODE_DEVICE_CODE_SHIFT)
-#define ZYNQMP_CSU_IDCODE_SUB_FAMILY_SHIFT	19
-#define ZYNQMP_CSU_IDCODE_SUB_FAMILY_MASK	(0x3 << \
+#define ZYNQMP_CSU_IDCODE_SUB_FAMILY_SHIFT	19U
+#define ZYNQMP_CSU_IDCODE_SUB_FAMILY_MASK	(0x3U << \
 					ZYNQMP_CSU_IDCODE_SUB_FAMILY_SHIFT)
-#define ZYNQMP_CSU_IDCODE_FAMILY_SHIFT		21
-#define ZYNQMP_CSU_IDCODE_FAMILY_MASK		(0x7F << \
+#define ZYNQMP_CSU_IDCODE_FAMILY_SHIFT		21U
+#define ZYNQMP_CSU_IDCODE_FAMILY_MASK		(0x7FU << \
 					ZYNQMP_CSU_IDCODE_FAMILY_SHIFT)
 #define ZYNQMP_CSU_IDCODE_FAMILY		0x23
 
-#define ZYNQMP_CSU_IDCODE_REVISION_SHIFT	28
-#define ZYNQMP_CSU_IDCODE_REVISION_MASK		(0xF << \
+#define ZYNQMP_CSU_IDCODE_REVISION_SHIFT	28U
+#define ZYNQMP_CSU_IDCODE_REVISION_MASK		(0xFU << \
 					ZYNQMP_CSU_IDCODE_REVISION_SHIFT)
-#define ZYNQMP_CSU_IDCODE_REVISION		0
+#define ZYNQMP_CSU_IDCODE_REVISION		0U
 
-#define ZYNQMP_CSU_VERSION_OFFSET	0x44
+#define ZYNQMP_CSU_VERSION_OFFSET	0x44U
 
 /* Efuse */
-#define EFUSE_BASEADDR		0xFFCC0000
+#define EFUSE_BASEADDR		U(0xFFCC0000)
 #define EFUSE_IPDISABLE_OFFSET	0x1018
 #define EFUSE_IPDISABLE_VERSION	0x1FFU
 #define ZYNQMP_EFUSE_IPDISABLE_SHIFT	20
@@ -357,9 +357,9 @@
 #define  FABRIC_WIDTH		U(3)
 
 /* CSUDMA Module Base Address*/
-#define CSUDMA_BASE		0xFFC80000
+#define CSUDMA_BASE		U(0xFFC80000)
 
 /* RSA-CORE Module Base Address*/
-#define RSA_CORE_BASE		0xFFCE0000
+#define RSA_CORE_BASE		U(0xFFCE0000)
 
 #endif /* ZYNQMP_DEF_H */

diff --git a/plat/xilinx/zynqmp/plat_psci.c b/plat/xilinx/zynqmp/plat_psci.c
index b2b473a..b7408b1 100644
--- a/plat/xilinx/zynqmp/plat_psci.c
+++ b/plat/xilinx/zynqmp/plat_psci.c

@@ -19,9 +19,9 @@
 #include "pm_api_sys.h"
 #include "pm_client.h"
 
-uintptr_t zynqmp_sec_entry;
+static uintptr_t zynqmp_sec_entry;
 
-void zynqmp_cpu_standby(plat_local_state_t cpu_state)
+static void zynqmp_cpu_standby(plat_local_state_t cpu_state)
 {
 	VERBOSE("%s: cpu_state: 0x%x\n", __func__, cpu_state);
 
@@ -29,18 +29,18 @@
 	wfi();
 }
 
-static int zynqmp_pwr_domain_on(u_register_t mpidr)
+static int32_t zynqmp_pwr_domain_on(u_register_t mpidr)
 {
-	unsigned int cpu_id = plat_core_pos_by_mpidr(mpidr);
+	uint32_t cpu_id = plat_core_pos_by_mpidr(mpidr);
 	const struct pm_proc *proc;
 	uint32_t buff[3];
 	enum pm_ret_status ret;
 
 	VERBOSE("%s: mpidr: 0x%lx\n", __func__, mpidr);
 
-	if (cpu_id == -1)
+	if (cpu_id == -1) {
 		return PSCI_E_INTERN_FAIL;
-
+	}
 	proc = pm_get_proc(cpu_id);
 
 	/* Check the APU proc status before wakeup */
@@ -60,12 +60,13 @@
 
 static void zynqmp_pwr_domain_off(const psci_power_state_t *target_state)
 {
-	unsigned int cpu_id = plat_my_core_pos();
+	uint32_t cpu_id = plat_my_core_pos();
 	const struct pm_proc *proc = pm_get_proc(cpu_id);
 
-	for (size_t i = 0; i <= PLAT_MAX_PWR_LVL; i++)
+	for (size_t i = 0; i <= PLAT_MAX_PWR_LVL; i++) {
 		VERBOSE("%s: target_state->pwr_domain_state[%lu]=%x\n",
 			__func__, i, target_state->pwr_domain_state[i]);
+	}
 
 	/* Prevent interrupts from spuriously waking up this cpu */
 	gicv2_cpuif_disable();
@@ -83,8 +84,8 @@
 
 static void zynqmp_pwr_domain_suspend(const psci_power_state_t *target_state)
 {
-	unsigned int state;
-	unsigned int cpu_id = plat_my_core_pos();
+	uint32_t state;
+	uint32_t cpu_id = plat_my_core_pos();
 	const struct pm_proc *proc = pm_get_proc(cpu_id);
 
 	for (size_t i = 0; i <= PLAT_MAX_PWR_LVL; i++)
@@ -106,21 +107,23 @@
 
 static void zynqmp_pwr_domain_on_finish(const psci_power_state_t *target_state)
 {
-	for (size_t i = 0; i <= PLAT_MAX_PWR_LVL; i++)
+	for (size_t i = 0; i <= PLAT_MAX_PWR_LVL; i++) {
 		VERBOSE("%s: target_state->pwr_domain_state[%lu]=%x\n",
 			__func__, i, target_state->pwr_domain_state[i]);
+	}
 	plat_arm_gic_pcpu_init();
 	gicv2_cpuif_enable();
 }
 
 static void zynqmp_pwr_domain_suspend_finish(const psci_power_state_t *target_state)
 {
-	unsigned int cpu_id = plat_my_core_pos();
+	uint32_t cpu_id = plat_my_core_pos();
 	const struct pm_proc *proc = pm_get_proc(cpu_id);
 
-	for (size_t i = 0; i <= PLAT_MAX_PWR_LVL; i++)
+	for (size_t i = 0; i <= PLAT_MAX_PWR_LVL; i++) {
 		VERBOSE("%s: target_state->pwr_domain_state[%lu]=%x\n",
 			__func__, i, target_state->pwr_domain_state[i]);
+	}
 
 	/* Clear the APU power control register for this cpu */
 	pm_client_wakeup(proc);
@@ -149,8 +152,9 @@
 	pm_system_shutdown(PMF_SHUTDOWN_TYPE_SHUTDOWN,
 			   pm_get_shutdown_scope());
 
-	while (1)
+	while (1) {
 		wfi();
+	}
 }
 
 static void __dead2 zynqmp_system_reset(void)
@@ -162,33 +166,35 @@
 	pm_system_shutdown(PMF_SHUTDOWN_TYPE_RESET,
 			   pm_get_shutdown_scope());
 
-	while (1)
+	while (1) {
 		wfi();
+	}
 }
 
-int zynqmp_validate_power_state(unsigned int power_state,
+static int32_t zynqmp_validate_power_state(uint32_t power_state,
 				psci_power_state_t *req_state)
 {
 	VERBOSE("%s: power_state: 0x%x\n", __func__, power_state);
 
-	int pstate = psci_get_pstate_type(power_state);
+	uint32_t pstate = psci_get_pstate_type(power_state);
 
 	assert(req_state);
 
 	/* Sanity check the requested state */
-	if (pstate == PSTATE_TYPE_STANDBY)
+	if (pstate == PSTATE_TYPE_STANDBY) {
 		req_state->pwr_domain_state[MPIDR_AFFLVL0] = PLAT_MAX_RET_STATE;
-	else
+	} else {
 		req_state->pwr_domain_state[MPIDR_AFFLVL0] = PLAT_MAX_OFF_STATE;
-
+	}
 	/* We expect the 'state id' to be zero */
-	if (psci_get_pstate_id(power_state))
+	if (psci_get_pstate_id(power_state)) {
 		return PSCI_E_INVALID_PARAMS;
+	}
 
 	return PSCI_E_SUCCESS;
 }
 
-void zynqmp_get_sys_suspend_power_state(psci_power_state_t *req_state)
+static void zynqmp_get_sys_suspend_power_state(psci_power_state_t *req_state)
 {
 	req_state->pwr_domain_state[PSCI_CPU_PWR_LVL] = PLAT_MAX_OFF_STATE;
 	req_state->pwr_domain_state[1] = PLAT_MAX_OFF_STATE;

diff --git a/plat/xilinx/zynqmp/plat_topology.c b/plat/xilinx/zynqmp/plat_topology.c
index aab24aa..41add9f 100644
--- a/plat/xilinx/zynqmp/plat_topology.c
+++ b/plat/xilinx/zynqmp/plat_topology.c

@@ -3,10 +3,11 @@
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
+#include <stdint.h>
 
-static const unsigned char plat_power_domain_tree_desc[] = {1, 4};
+static const uint8_t plat_power_domain_tree_desc[] = {1, 4};
 
-const unsigned char *plat_get_power_domain_tree_desc(void)
+const uint8_t *plat_get_power_domain_tree_desc(void)
 {
 	return plat_power_domain_tree_desc;
 }

diff --git a/plat/xilinx/zynqmp/plat_zynqmp.c b/plat/xilinx/zynqmp/plat_zynqmp.c
index 58a52a3..25ebac6 100644
--- a/plat/xilinx/zynqmp/plat_zynqmp.c
+++ b/plat/xilinx/zynqmp/plat_zynqmp.c

@@ -7,7 +7,7 @@
 #include <plat_private.h>
 #include <plat/common/platform.h>
 
-int plat_core_pos_by_mpidr(u_register_t mpidr)
+int32_t plat_core_pos_by_mpidr(u_register_t mpidr)
 {
 	if (mpidr & MPIDR_CLUSTER_MASK) {
 		return -1;

diff --git a/plat/xilinx/zynqmp/platform.mk b/plat/xilinx/zynqmp/platform.mk
index 620bf6c..ea8a5d1 100644
--- a/plat/xilinx/zynqmp/platform.mk
+++ b/plat/xilinx/zynqmp/platform.mk

@@ -21,6 +21,10 @@
 
 WORKAROUND_CVE_2017_5715	:=	0
 
+ARM_XLAT_TABLES_LIB_V1         :=      1
+$(eval $(call assert_boolean,ARM_XLAT_TABLES_LIB_V1))
+$(eval $(call add_define,ARM_XLAT_TABLES_LIB_V1))
+
 ifdef ZYNQMP_ATF_MEM_BASE
     $(eval $(call add_define,ZYNQMP_ATF_MEM_BASE))
 

diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_clock.c b/plat/xilinx/zynqmp/pm_service/pm_api_clock.c
index 4109830..c6bed7d 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_clock.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_clock.c

@@ -228,8 +228,8 @@
 struct pm_clock {
 	char name[CLK_NAME_LEN];
 	uint8_t num_nodes;
-	unsigned int control_reg;
-	unsigned int status_reg;
+	uint32_t control_reg;
+	uint32_t status_reg;
 	int32_t (*parents)[];
 	struct pm_clock_node(*nodes)[];
 };
@@ -2396,7 +2396,7 @@
  *
  * Return: Returns 1 if clock is valid else 0.
  */
-static bool pm_clock_valid(unsigned int clock_id)
+static bool pm_clock_valid(uint32_t clock_id)
 {
 	unsigned int i;
 
@@ -2415,7 +2415,7 @@
  *
  * Return: Returns type of clock (OUTPUT/EXTERNAL).
  */
-static unsigned int pm_clock_type(unsigned int clock_id)
+static uint32_t pm_clock_type(uint32_t clock_id)
 {
 	return (clock_id < CLK_MAX_OUTPUT_CLK) ?
 		CLK_TYPE_OUTPUT : CLK_TYPE_EXTERNAL;
@@ -2429,7 +2429,7 @@
  *
  * @return	Returns success.
  */
-enum pm_ret_status pm_api_clock_get_num_clocks(unsigned int *nclocks)
+enum pm_ret_status pm_api_clock_get_num_clocks(uint32_t *nclocks)
 {
 	*nclocks = CLK_MAX;
 
@@ -2444,18 +2444,19 @@
  * This function is used by master to get nmae of clock specified
  * by given clock ID.
  */
-void pm_api_clock_get_name(unsigned int clock_id, char *name)
+void pm_api_clock_get_name(uint32_t clock_id, char *name)
 {
-	if (clock_id == CLK_MAX)
+	if (clock_id == CLK_MAX) {
 		memcpy(name, END_OF_CLK, sizeof(END_OF_CLK) > CLK_NAME_LEN ?
 					 CLK_NAME_LEN : sizeof(END_OF_CLK));
-	else if (!pm_clock_valid(clock_id))
+	} else if (!pm_clock_valid(clock_id)) {
 		memset(name, 0, CLK_NAME_LEN);
-	else if (clock_id < CLK_MAX_OUTPUT_CLK)
+	} else if (clock_id < CLK_MAX_OUTPUT_CLK) {
 		memcpy(name, clocks[clock_id].name, CLK_NAME_LEN);
-	else
+	} else {
 		memcpy(name, ext_clocks[clock_id - CLK_MAX_OUTPUT_CLK].name,
 		       CLK_NAME_LEN);
+	}
 }
 
 /**
@@ -2471,33 +2472,37 @@
  *
  * @return	Returns status, either success or error+reason
  */
-enum pm_ret_status pm_api_clock_get_topology(unsigned int clock_id,
-					     unsigned int index,
+enum pm_ret_status pm_api_clock_get_topology(uint32_t clock_id,
+					     uint32_t index,
 					     uint32_t *topology)
 {
 	struct pm_clock_node *clock_nodes;
 	uint8_t num_nodes;
-	unsigned int i;
+	uint32_t i;
 	uint16_t typeflags;
 
-	if (!pm_clock_valid(clock_id))
+	if (!pm_clock_valid(clock_id)) {
 		return PM_RET_ERROR_ARGS;
+	}
 
-	if (pm_clock_type(clock_id) != CLK_TYPE_OUTPUT)
+	if (pm_clock_type(clock_id) != CLK_TYPE_OUTPUT) {
 		return PM_RET_ERROR_NOTSUPPORTED;
-
+	}
 
 	memset(topology, 0, CLK_TOPOLOGY_PAYLOAD_LEN);
 	clock_nodes = *clocks[clock_id].nodes;
 	num_nodes = clocks[clock_id].num_nodes;
 
 	/* Skip parent till index */
-	if (index >= num_nodes)
+	if (index >= num_nodes) {
 		return PM_RET_SUCCESS;
+	}
 
 	for (i = 0; i < 3U; i++) {
-		if ((index + i) == num_nodes)
+		if ((index + i) == num_nodes) {
 			break;
+		}
+
 		topology[i] = clock_nodes[index + i].type;
 		topology[i] |= clock_nodes[index + i].clkflags <<
 					CLK_CLKFLAGS_SHIFT;
@@ -2523,19 +2528,21 @@
  *
  * @return	Returns status, either success or error+reason
  */
-enum pm_ret_status pm_api_clock_get_fixedfactor_params(unsigned int clock_id,
+enum pm_ret_status pm_api_clock_get_fixedfactor_params(uint32_t clock_id,
 						       uint32_t *mul,
 						       uint32_t *div)
 {
 	struct pm_clock_node *clock_nodes;
 	uint8_t num_nodes;
-	unsigned int type, i;
+	uint32_t type, i;
 
-	if (!pm_clock_valid(clock_id))
+	if (!pm_clock_valid(clock_id)) {
 		return PM_RET_ERROR_ARGS;
+	}
 
-	if (pm_clock_type(clock_id) != CLK_TYPE_OUTPUT)
+	if (pm_clock_type(clock_id) != CLK_TYPE_OUTPUT) {
 		return PM_RET_ERROR_NOTSUPPORTED;
+	}
 
 	clock_nodes = *clocks[clock_id].nodes;
 	num_nodes = clocks[clock_id].num_nodes;
@@ -2550,8 +2557,9 @@
 	}
 
 	/* Clock is not fixed clock */
-	if (i == num_nodes)
+	if (i == num_nodes) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	return PM_RET_SUCCESS;
 }
@@ -2573,34 +2581,40 @@
  *
  * @return	Returns status, either success or error+reason
  */
-enum pm_ret_status pm_api_clock_get_parents(unsigned int clock_id,
-					    unsigned int index,
+enum pm_ret_status pm_api_clock_get_parents(uint32_t clock_id,
+					    uint32_t index,
 					    uint32_t *parents)
 {
-	unsigned int i;
+	uint32_t i;
 	int32_t *clk_parents;
 
-	if (!pm_clock_valid(clock_id))
+	if (!pm_clock_valid(clock_id)) {
 		return PM_RET_ERROR_ARGS;
+	}
 
-	if (pm_clock_type(clock_id) != CLK_TYPE_OUTPUT)
+	if (pm_clock_type(clock_id) != CLK_TYPE_OUTPUT) {
 		return PM_RET_ERROR_NOTSUPPORTED;
+	}
 
 	clk_parents = *clocks[clock_id].parents;
-	if (clk_parents == NULL)
+	if (clk_parents == NULL) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	memset(parents, 0, CLK_PARENTS_PAYLOAD_LEN);
 
 	/* Skip parent till index */
-	for (i = 0; i < index; i++)
-		if (clk_parents[i] == CLK_NA_PARENT)
+	for (i = 0; i < index; i++) {
+		if (clk_parents[i] == CLK_NA_PARENT) {
 			return PM_RET_SUCCESS;
+		}
+	}
 
-	for (i = 0; i < 3; i++) {
+	for (i = 0; i < 3U; i++) {
 		parents[i] = clk_parents[index + i];
-		if (clk_parents[index + i] == CLK_NA_PARENT)
+		if (clk_parents[index + i] == CLK_NA_PARENT) {
 			break;
+		}
 	}
 
 	return PM_RET_SUCCESS;
@@ -2616,11 +2630,12 @@
  *
  * @return	Returns status, either success or error+reason
  */
-enum pm_ret_status pm_api_clock_get_attributes(unsigned int clock_id,
+enum pm_ret_status pm_api_clock_get_attributes(uint32_t clock_id,
 					       uint32_t *attr)
 {
-	if (clock_id >= CLK_MAX)
+	if (clock_id >= CLK_MAX) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Clock valid bit */
 	*attr = pm_clock_valid(clock_id);
@@ -2648,8 +2663,9 @@
 	uint32_t i;
 	struct pm_clock_node *nodes;
 
-	if (clock_id >= CLK_MAX_OUTPUT_CLK)
+	if (clock_id >= CLK_MAX_OUTPUT_CLK) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	nodes = *clocks[clock_id].nodes;
 	for (i = 0; i < clocks[clock_id].num_nodes; i++) {
@@ -2738,8 +2754,9 @@
 	uint32_t i;
 
 	for (i = 0; i < ARRAY_SIZE(pm_plls); i++) {
-		if (pm_plls[i].cid == clock_id)
+		if (pm_plls[i].cid == clock_id) {
 			return &pm_plls[i];
+		}
 	}
 
 	return NULL;
@@ -2798,12 +2815,14 @@
  */
 enum pm_ret_status pm_clock_pll_enable(struct pm_pll *pll)
 {
-	if (!pll)
+	if (!pll) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Set the PLL mode according to the buffered mode value */
-	if (pll->mode == PLL_FRAC_MODE)
+	if (pll->mode == PLL_FRAC_MODE) {
 		return pm_pll_set_mode(pll->nid, PM_PLL_MODE_FRACTIONAL);
+	}
 
 	return pm_pll_set_mode(pll->nid, PM_PLL_MODE_INTEGER);
 }
@@ -2819,8 +2838,9 @@
  */
 enum pm_ret_status pm_clock_pll_disable(struct pm_pll *pll)
 {
-	if (!pll)
+	if (!pll) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	return pm_pll_set_mode(pll->nid, PM_PLL_MODE_RESET);
 }
@@ -2837,22 +2857,25 @@
  * returned state value is valid or an error if returned by PMU
  */
 enum pm_ret_status pm_clock_pll_get_state(struct pm_pll *pll,
-					  unsigned int *state)
+					  uint32_t *state)
 {
 	enum pm_ret_status status;
 	enum pm_pll_mode mode;
 
-	if (!pll || !state)
+	if (!pll || !state) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	status = pm_pll_get_mode(pll->nid, &mode);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
-	if (mode == PM_PLL_MODE_RESET)
+	if (mode == PM_PLL_MODE_RESET) {
 		*state = 0;
-	else
+	} else {
 		*state = 1;
+	}
 
 	return PM_RET_SUCCESS;
 }
@@ -2871,19 +2894,23 @@
  */
 enum pm_ret_status pm_clock_pll_set_parent(struct pm_pll *pll,
 					   enum clock_id clock_id,
-					   unsigned int parent_index)
+					   uint32_t parent_index)
 {
-	if (!pll)
+	if (!pll) {
 		return PM_RET_ERROR_ARGS;
-	if (pll->pre_src == clock_id)
+	}
+	if (pll->pre_src == clock_id) {
 		return pm_pll_set_parameter(pll->nid, PM_PLL_PARAM_PRE_SRC,
 					    parent_index);
-	if (pll->post_src == clock_id)
+	}
+	if (pll->post_src == clock_id) {
 		return pm_pll_set_parameter(pll->nid, PM_PLL_PARAM_POST_SRC,
 					    parent_index);
-	if (pll->div2 == clock_id)
+	}
+	if (pll->div2 == clock_id) {
 		return pm_pll_set_parameter(pll->nid, PM_PLL_PARAM_DIV2,
 					    parent_index);
+	}
 
 	return PM_RET_ERROR_ARGS;
 }
@@ -2900,19 +2927,23 @@
  */
 enum pm_ret_status pm_clock_pll_get_parent(struct pm_pll *pll,
 					   enum clock_id clock_id,
-					   unsigned int *parent_index)
+					   uint32_t *parent_index)
 {
-	if (!pll)
+	if (!pll) {
 		return PM_RET_ERROR_ARGS;
-	if (pll->pre_src == clock_id)
+	}
+	if (pll->pre_src == clock_id) {
 		return pm_pll_get_parameter(pll->nid, PM_PLL_PARAM_PRE_SRC,
 					    parent_index);
-	if (pll->post_src == clock_id)
+	}
+	if (pll->post_src == clock_id) {
 		return pm_pll_get_parameter(pll->nid, PM_PLL_PARAM_POST_SRC,
 					    parent_index);
-	if (pll->div2 == clock_id)
+	}
+	if (pll->div2 == clock_id) {
 		return pm_pll_get_parameter(pll->nid, PM_PLL_PARAM_DIV2,
 					    parent_index);
+	}
 	if (pll->bypass == clock_id) {
 		*parent_index = 0;
 		return PM_RET_SUCCESS;
@@ -2931,12 +2962,13 @@
  * @return      Success if mode is buffered or error if an argument is invalid
  */
 enum pm_ret_status pm_clock_set_pll_mode(enum clock_id clock_id,
-					 unsigned int mode)
+					 uint32_t mode)
 {
 	struct pm_pll *pll = pm_clock_get_pll(clock_id);
 
-	if (!pll || (mode != PLL_FRAC_MODE && mode != PLL_INT_MODE))
+	if (!pll || (mode != PLL_FRAC_MODE && mode != PLL_INT_MODE)) {
 		return PM_RET_ERROR_ARGS;
+	}
 	pll->mode = mode;
 
 	return PM_RET_SUCCESS;
@@ -2952,12 +2984,13 @@
  * @return      Success if mode is stored or error if an argument is invalid
  */
 enum pm_ret_status pm_clock_get_pll_mode(enum clock_id clock_id,
-					 unsigned int *mode)
+					 uint32_t *mode)
 {
 	struct pm_pll *pll = pm_clock_get_pll(clock_id);
 
-	if (!pll || !mode)
+	if (!pll || !mode) {
 		return PM_RET_ERROR_ARGS;
+	}
 	*mode = pll->mode;
 
 	return PM_RET_SUCCESS;
@@ -2969,13 +3002,15 @@
  *
  * @return     Returns success if clock_id is valid, otherwise an error
  */
-enum pm_ret_status pm_clock_id_is_valid(unsigned int clock_id)
+enum pm_ret_status pm_clock_id_is_valid(uint32_t clock_id)
 {
-	if (!pm_clock_valid(clock_id))
+	if (!pm_clock_valid(clock_id)) {
 		return PM_RET_ERROR_ARGS;
+	}
 
-	if (pm_clock_type(clock_id) != CLK_TYPE_OUTPUT)
+	if (pm_clock_type(clock_id) != CLK_TYPE_OUTPUT) {
 		return PM_RET_ERROR_NOTSUPPORTED;
+	}
 
 	return PM_RET_SUCCESS;
 }
@@ -2987,13 +3022,14 @@
  *
  * @return	True(1)=clock has the divider, false(0)=otherwise
  */
-uint8_t pm_clock_has_div(unsigned int clock_id, enum pm_clock_div_id div_id)
+uint8_t pm_clock_has_div(uint32_t clock_id, enum pm_clock_div_id div_id)
 {
 	uint32_t i;
 	struct pm_clock_node *nodes;
 
-	if (clock_id >= CLK_MAX_OUTPUT_CLK)
+	if (clock_id >= CLK_MAX_OUTPUT_CLK) {
 		return 0;
+	}
 
 	nodes = *clocks[clock_id].nodes;
 	for (i = 0; i < clocks[clock_id].num_nodes; i++) {
@@ -3003,6 +3039,8 @@
 		} else if (nodes[i].type == TYPE_DIV2) {
 			if (div_id == PM_CLOCK_DIV1_ID)
 				return 1;
+		} else {
+			/* To fix the misra 15.7 warning */
 		}
 	}
 

diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_clock.h b/plat/xilinx/zynqmp/pm_service/pm_api_clock.h
index 5efd63f..bc15592 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_clock.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_clock.h

@@ -292,20 +292,20 @@
 struct pm_pll;
 struct pm_pll *pm_clock_get_pll(enum clock_id clock_id);
 struct pm_pll *pm_clock_get_pll_by_related_clk(enum clock_id clock_id);
-uint8_t pm_clock_has_div(unsigned int clock_id, enum pm_clock_div_id div_id);
+uint8_t pm_clock_has_div(uint32_t clock_id, enum pm_clock_div_id div_id);
 
-void pm_api_clock_get_name(unsigned int clock_id, char *name);
-enum pm_ret_status pm_api_clock_get_num_clocks(unsigned int *nclocks);
-enum pm_ret_status pm_api_clock_get_topology(unsigned int clock_id,
-					     unsigned int index,
+void pm_api_clock_get_name(uint32_t clock_id, char *name);
+enum pm_ret_status pm_api_clock_get_num_clocks(uint32_t *nclocks);
+enum pm_ret_status pm_api_clock_get_topology(uint32_t clock_id,
+					     uint32_t index,
 					     uint32_t *topology);
-enum pm_ret_status pm_api_clock_get_fixedfactor_params(unsigned int clock_id,
+enum pm_ret_status pm_api_clock_get_fixedfactor_params(uint32_t clock_id,
 						       uint32_t *mul,
 						       uint32_t *div);
-enum pm_ret_status pm_api_clock_get_parents(unsigned int clock_id,
-					    unsigned int index,
+enum pm_ret_status pm_api_clock_get_parents(uint32_t clock_id,
+					    uint32_t index,
 					    uint32_t *parents);
-enum pm_ret_status pm_api_clock_get_attributes(unsigned int clock_id,
+enum pm_ret_status pm_api_clock_get_attributes(uint32_t clock_id,
 					       uint32_t *attr);
 enum pm_ret_status pm_api_clock_get_max_divisor(enum clock_id clock_id,
 						uint8_t div_type,
@@ -313,21 +313,21 @@
 
 enum pm_ret_status pm_clock_get_pll_node_id(enum clock_id clock_id,
 					    enum pm_node_id *node_id);
-enum pm_ret_status pm_clock_id_is_valid(unsigned int clock_id);
+enum pm_ret_status pm_clock_id_is_valid(uint32_t clock_id);
 
 enum pm_ret_status pm_clock_pll_enable(struct pm_pll *pll);
 enum pm_ret_status pm_clock_pll_disable(struct pm_pll *pll);
 enum pm_ret_status pm_clock_pll_get_state(struct pm_pll *pll,
-					  unsigned int *state);
+					  uint32_t *state);
 enum pm_ret_status pm_clock_pll_set_parent(struct pm_pll *pll,
 					   enum clock_id clock_id,
-					   unsigned int parent_index);
+					   uint32_t parent_index);
 enum pm_ret_status pm_clock_pll_get_parent(struct pm_pll *pll,
 					   enum clock_id clock_id,
-					   unsigned int *parent_index);
+					   uint32_t *parent_index);
 enum pm_ret_status pm_clock_set_pll_mode(enum clock_id clock_id,
-					 unsigned int mode);
+					 uint32_t mode);
 enum pm_ret_status pm_clock_get_pll_mode(enum clock_id clock_id,
-					 unsigned int *mode);
+					 uint32_t *mode);
 
 #endif /* PM_API_CLOCK_H */

diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c b/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c
index a87681b..8a5a25a 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.c

@@ -29,16 +29,17 @@
  *
  * @return	Returns status, either success or error+reason
  */
-static enum pm_ret_status pm_ioctl_get_rpu_oper_mode(unsigned int *mode)
+static enum pm_ret_status pm_ioctl_get_rpu_oper_mode(uint32_t *mode)
 {
-	unsigned int val;
+	uint32_t val;
 
 	val = mmio_read_32(ZYNQMP_RPU_GLBL_CNTL);
 	val &= ZYNQMP_SLSPLIT_MASK;
-	if (val == 0)
+	if (val == 0U) {
 		*mode = PM_RPU_MODE_LOCKSTEP;
-	else
+	} else {
 		*mode = PM_RPU_MODE_SPLIT;
+	}
 
 	return PM_RET_SUCCESS;
 }
@@ -54,12 +55,13 @@
  *
  * @return	Returns status, either success or error+reason
  */
-static enum pm_ret_status pm_ioctl_set_rpu_oper_mode(unsigned int mode)
+static enum pm_ret_status pm_ioctl_set_rpu_oper_mode(uint32_t mode)
 {
-	unsigned int val;
+	uint32_t val;
 
-	if (mmio_read_32(CRL_APB_RST_LPD_TOP) & CRL_APB_RPU_AMBA_RESET)
+	if (mmio_read_32(CRL_APB_RST_LPD_TOP) & CRL_APB_RPU_AMBA_RESET) {
 		return PM_RET_ERROR_ACCESS;
+	}
 
 	val = mmio_read_32(ZYNQMP_RPU_GLBL_CNTL);
 
@@ -90,25 +92,27 @@
  * @return	Returns status, either success or error+reason
  */
 static enum pm_ret_status pm_ioctl_config_boot_addr(enum pm_node_id nid,
-						    unsigned int value)
+						    uint32_t value)
 {
-	unsigned int rpu_cfg_addr, val;
+	uint32_t rpu_cfg_addr, val;
 
-	if (nid == NODE_RPU_0)
+	if (nid == NODE_RPU_0) {
 		rpu_cfg_addr = ZYNQMP_RPU0_CFG;
-	else if (nid == NODE_RPU_1)
+	} else if (nid == NODE_RPU_1) {
 		rpu_cfg_addr = ZYNQMP_RPU1_CFG;
-	else
+	} else {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	val = mmio_read_32(rpu_cfg_addr);
 
-	if (value == PM_RPU_BOOTMEM_LOVEC)
+	if (value == PM_RPU_BOOTMEM_LOVEC) {
 		val &= ~ZYNQMP_VINITHI_MASK;
-	else if (value == PM_RPU_BOOTMEM_HIVEC)
+	} else if (value == PM_RPU_BOOTMEM_HIVEC) {
 		val |= ZYNQMP_VINITHI_MASK;
-	else
+	} else {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	mmio_write_32(rpu_cfg_addr, val);
 
@@ -124,18 +128,19 @@
  *
  * @return	Returns status, either success or error+reason
  */
-static enum pm_ret_status pm_ioctl_config_tcm_comb(unsigned int value)
+static enum pm_ret_status pm_ioctl_config_tcm_comb(uint32_t value)
 {
-	unsigned int val;
+	uint32_t val;
 
 	val = mmio_read_32(ZYNQMP_RPU_GLBL_CNTL);
 
-	if (value == PM_RPU_TCM_SPLIT)
+	if (value == PM_RPU_TCM_SPLIT) {
 		val &= ~ZYNQMP_TCM_COMB_MASK;
-	else if (value == PM_RPU_TCM_COMB)
+	} else if (value == PM_RPU_TCM_COMB) {
 		val |= ZYNQMP_TCM_COMB_MASK;
-	else
+	} else {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	mmio_write_32(ZYNQMP_RPU_GLBL_CNTL, val);
 
@@ -151,12 +156,13 @@
  *
  * @return	Returns status, either success or error+reason
  */
-static enum pm_ret_status pm_ioctl_set_tapdelay_bypass(unsigned int type,
-						       unsigned int value)
+static enum pm_ret_status pm_ioctl_set_tapdelay_bypass(uint32_t type,
+						       uint32_t value)
 {
 	if ((value != PM_TAPDELAY_BYPASS_ENABLE &&
-	     value != PM_TAPDELAY_BYPASS_DISABLE) || type >= PM_TAPDELAY_MAX)
+	     value != PM_TAPDELAY_BYPASS_DISABLE) || type >= PM_TAPDELAY_MAX) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	return pm_mmio_write(IOU_TAPDLY_BYPASS, TAP_DELAY_MASK, value << type);
 }
@@ -173,13 +179,14 @@
  * @return	Returns status, either success or error+reason
  */
 static enum pm_ret_status pm_ioctl_set_sgmii_mode(enum pm_node_id nid,
-						  unsigned int value)
+						  uint32_t value)
 {
-	unsigned int val, mask, shift;
+	uint32_t val, mask, shift;
 	enum pm_ret_status ret;
 
-	if (value != PM_SGMII_DISABLE && value != PM_SGMII_ENABLE)
+	if (value != PM_SGMII_DISABLE && value != PM_SGMII_ENABLE) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	switch (nid) {
 	case NODE_ETH_0:
@@ -206,8 +213,9 @@
 		mask = SGMII_SD_MASK << SGMII_SD_OFFSET * shift;
 		val = SGMII_PCS_SD_1 << SGMII_SD_OFFSET * shift;
 		ret = pm_mmio_write(IOU_GEM_CTRL, mask, val);
-		if (ret != PM_RET_SUCCESS)
+		if (ret != PM_RET_SUCCESS) {
 			return ret;
+		}
 
 		/* Set the GEM to SGMII mode */
 		mask = GEM_CLK_CTRL_MASK << GEM_CLK_CTRL_OFFSET * shift;
@@ -229,9 +237,9 @@
  * @return	Returns status, either success or error+reason
  */
 static enum pm_ret_status pm_ioctl_sd_dll_reset(enum pm_node_id nid,
-						unsigned int type)
+						uint32_t type)
 {
-	unsigned int mask, val;
+	uint32_t mask, val;
 	enum pm_ret_status ret;
 
 	if (nid == NODE_SD_0) {
@@ -248,11 +256,13 @@
 	case PM_DLL_RESET_ASSERT:
 	case PM_DLL_RESET_PULSE:
 		ret = pm_mmio_write(ZYNQMP_SD_DLL_CTRL, mask, val);
-		if (ret != PM_RET_SUCCESS)
+		if (ret != PM_RET_SUCCESS) {
 			return ret;
+		}
 
-		if (type == PM_DLL_RESET_ASSERT)
+		if (type == PM_DLL_RESET_ASSERT) {
 			break;
+		}
 		mdelay(1);
 		/* Fallthrough */
 	case PM_DLL_RESET_RELEASE:
@@ -278,11 +288,11 @@
  */
 static enum pm_ret_status pm_ioctl_sd_set_tapdelay(enum pm_node_id nid,
 						   enum tap_delay_type type,
-						   unsigned int value)
+						   uint32_t value)
 {
-	unsigned int shift;
+	uint32_t shift;
 	enum pm_ret_status ret;
-	unsigned int val, mask;
+	uint32_t val, mask;
 
 	if (nid == NODE_SD_0) {
 		shift = 0;
@@ -299,7 +309,7 @@
 		return ret;
 	}
 
-	if ((val & mask) == 0) {
+	if ((val & mask) == 0U) {
 		ret = pm_ioctl_sd_dll_reset(nid, PM_DLL_RESET_ASSERT);
 		if (ret != PM_RET_SUCCESS) {
 			return ret;
@@ -310,31 +320,44 @@
 		ret = pm_mmio_write(ZYNQMP_SD_ITAP_DLY,
 				    (ZYNQMP_SD_ITAPCHGWIN_MASK << shift),
 				    (ZYNQMP_SD_ITAPCHGWIN << shift));
-		if (ret != PM_RET_SUCCESS)
+
+		if (ret != PM_RET_SUCCESS) {
 			goto reset_release;
-		if (value == 0)
+		}
+
+		if (value == 0U) {
 			ret = pm_mmio_write(ZYNQMP_SD_ITAP_DLY,
 					    (ZYNQMP_SD_ITAPDLYENA_MASK <<
 					     shift), 0);
-		else
+		} else {
 			ret = pm_mmio_write(ZYNQMP_SD_ITAP_DLY,
 					    (ZYNQMP_SD_ITAPDLYENA_MASK <<
 					    shift), (ZYNQMP_SD_ITAPDLYENA <<
 					    shift));
-		if (ret != PM_RET_SUCCESS)
+		}
+
+		if (ret != PM_RET_SUCCESS) {
 			goto reset_release;
+		}
+
 		ret = pm_mmio_write(ZYNQMP_SD_ITAP_DLY,
 				    (ZYNQMP_SD_ITAPDLYSEL_MASK << shift),
 				    (value << shift));
-		if (ret != PM_RET_SUCCESS)
+
+		if (ret != PM_RET_SUCCESS) {
 			goto reset_release;
+		}
+
 		ret = pm_mmio_write(ZYNQMP_SD_ITAP_DLY,
 				    (ZYNQMP_SD_ITAPCHGWIN_MASK << shift), 0);
 	} else if (type == PM_TAPDELAY_OUTPUT) {
 		ret = pm_mmio_write(ZYNQMP_SD_OTAP_DLY,
 				    (ZYNQMP_SD_OTAPDLYENA_MASK << shift), 0);
-		if (ret != PM_RET_SUCCESS)
+
+		if (ret != PM_RET_SUCCESS) {
 			goto reset_release;
+		}
+
 		ret = pm_mmio_write(ZYNQMP_SD_OTAP_DLY,
 				    (ZYNQMP_SD_OTAPDLYSEL_MASK << shift),
 				    (value << shift));
@@ -361,7 +384,7 @@
  * @return      Returns status, either success or error+reason
  */
 static enum pm_ret_status pm_ioctl_set_pll_frac_mode
-			(unsigned int pll, unsigned int mode)
+			(uint32_t pll, uint32_t mode)
 {
 	return pm_clock_set_pll_mode(pll, mode);
 }
@@ -377,7 +400,7 @@
  * @return      Returns status, either success or error+reason
  */
 static enum pm_ret_status pm_ioctl_get_pll_frac_mode
-			(unsigned int pll, unsigned int *mode)
+			(uint32_t pll, uint32_t *mode)
 {
 	return pm_clock_get_pll_mode(pll, mode);
 }
@@ -394,15 +417,16 @@
  * @return      Returns status, either success or error+reason
  */
 static enum pm_ret_status pm_ioctl_set_pll_frac_data
-			(unsigned int pll, unsigned int data)
+			(uint32_t pll, uint32_t data)
 {
 	enum pm_node_id pll_nid;
 	enum pm_ret_status status;
 
 	/* Get PLL node ID using PLL clock ID */
 	status = pm_clock_get_pll_node_id(pll, &pll_nid);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
 	return pm_pll_set_parameter(pll_nid, PM_PLL_PARAM_DATA, data);
 }
@@ -418,15 +442,16 @@
  * @return      Returns status, either success or error+reason
  */
 static enum pm_ret_status pm_ioctl_get_pll_frac_data
-			(unsigned int pll, unsigned int *data)
+			(uint32_t pll, uint32_t *data)
 {
 	enum pm_node_id pll_nid;
 	enum pm_ret_status status;
 
 	/* Get PLL node ID using PLL clock ID */
 	status = pm_clock_get_pll_node_id(pll, &pll_nid);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
 	return pm_pll_get_parameter(pll_nid, PM_PLL_PARAM_DATA, data);
 }
@@ -441,11 +466,12 @@
  *
  * @return      Returns status, either success or error+reason
  */
-static enum pm_ret_status pm_ioctl_write_ggs(unsigned int index,
-					     unsigned int value)
+static enum pm_ret_status pm_ioctl_write_ggs(uint32_t index,
+					     uint32_t value)
 {
-	if (index >= GGS_NUM_REGS)
+	if (index >= GGS_NUM_REGS) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	return pm_mmio_write(GGS_BASEADDR + (index << 2),
 			     0xFFFFFFFFU, value);
@@ -461,11 +487,12 @@
  *
  * @return      Returns status, either success or error+reason
  */
-static enum pm_ret_status pm_ioctl_read_ggs(unsigned int index,
-					    unsigned int *value)
+static enum pm_ret_status pm_ioctl_read_ggs(uint32_t index,
+					    uint32_t *value)
 {
-	if (index >= GGS_NUM_REGS)
+	if (index >= GGS_NUM_REGS) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	return pm_mmio_read(GGS_BASEADDR + (index << 2), value);
 }
@@ -480,11 +507,12 @@
  *
  * @return      Returns status, either success or error+reason
  */
-static enum pm_ret_status pm_ioctl_write_pggs(unsigned int index,
-					      unsigned int value)
+static enum pm_ret_status pm_ioctl_write_pggs(uint32_t index,
+					      uint32_t value)
 {
-	if (index >= PGGS_NUM_REGS)
+	if (index >= PGGS_NUM_REGS) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	return pm_mmio_write(PGGS_BASEADDR + (index << 2),
 			     0xFFFFFFFFU, value);
@@ -499,35 +527,37 @@
  *
  * @return      Returns status, either success or error+reason
  */
-static enum pm_ret_status pm_ioctl_afi(unsigned int index,
-					      unsigned int value)
+static enum pm_ret_status pm_ioctl_afi(uint32_t index,
+					      uint32_t value)
 {
-	unsigned int mask;
-	unsigned int regarr[] = {0xFD360000,
-				0xFD360014,
-				0xFD370000,
-				0xFD370014,
-				0xFD380000,
-				0xFD380014,
-				0xFD390000,
-				0xFD390014,
-				0xFD3a0000,
-				0xFD3a0014,
-				0xFD3b0000,
-				0xFD3b0014,
-				0xFF9b0000,
-				0xFF9b0014,
-				0xFD615000,
-				0xFF419000,
+	uint32_t mask;
+	uint32_t regarr[] = {0xFD360000U,
+				0xFD360014U,
+				0xFD370000U,
+				0xFD370014U,
+				0xFD380000U,
+				0xFD380014U,
+				0xFD390000U,
+				0xFD390014U,
+				0xFD3a0000U,
+				0xFD3a0014U,
+				0xFD3b0000U,
+				0xFD3b0014U,
+				0xFF9b0000U,
+				0xFF9b0014U,
+				0xFD615000U,
+				0xFF419000U,
 				};
 
-	if (index >= ARRAY_SIZE(regarr))
+	if (index >= ARRAY_SIZE(regarr)) {
 		return PM_RET_ERROR_ARGS;
+	}
 
-	if (index < AFIFM6_WRCTRL)
+	if (index < AFIFM6_WRCTRL) {
 		mask = FABRIC_WIDTH;
-	else
+	} else {
 		mask = 0xf00;
+	}
 
 	return pm_mmio_write(regarr[index], mask, value);
 }
@@ -542,11 +572,12 @@
  *
  * @return      Returns status, either success or error+reason
  */
-static enum pm_ret_status pm_ioctl_read_pggs(unsigned int index,
-					     unsigned int *value)
+static enum pm_ret_status pm_ioctl_read_pggs(uint32_t index,
+					     uint32_t *value)
 {
-	if (index >= PGGS_NUM_REGS)
+	if (index >= PGGS_NUM_REGS) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	return pm_mmio_read(PGGS_BASEADDR + (index << 2), value);
 }
@@ -565,16 +596,18 @@
 
 	ret = pm_mmio_write(CRL_APB_BOOT_PIN_CTRL, CRL_APB_BOOT_PIN_MASK,
 			    ZYNQMP_ULPI_RESET_VAL_HIGH);
-	if (ret != PM_RET_SUCCESS)
+	if (ret != PM_RET_SUCCESS) {
 		return ret;
+	}
 
 	/* Drive ULPI assert for atleast 1ms */
 	mdelay(1);
 
 	ret = pm_mmio_write(CRL_APB_BOOT_PIN_CTRL, CRL_APB_BOOT_PIN_MASK,
 			    ZYNQMP_ULPI_RESET_VAL_LOW);
-	if (ret != PM_RET_SUCCESS)
+	if (ret != PM_RET_SUCCESS) {
 		return ret;
+	}
 
 	/* Drive ULPI de-assert for atleast 1ms */
 	mdelay(1);
@@ -593,7 +626,7 @@
  *
  * @return      Returns status, either success or error+reason
  */
-static enum pm_ret_status pm_ioctl_set_boot_health_status(unsigned int value)
+static enum pm_ret_status pm_ioctl_set_boot_health_status(uint32_t value)
 {
 	return pm_mmio_write(PMU_GLOBAL_GEN_STORAGE4,
 			     PM_BOOT_HEALTH_STATUS_MASK, value);
@@ -612,10 +645,10 @@
  * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_api_ioctl(enum pm_node_id nid,
-				unsigned int ioctl_id,
-				unsigned int arg1,
-				unsigned int arg2,
-				unsigned int *value)
+				uint32_t ioctl_id,
+				uint32_t arg1,
+				uint32_t arg2,
+				uint32_t *value)
 {
 	enum pm_ret_status ret;
 	uint32_t payload[PAYLOAD_ARG_CNT];
@@ -717,7 +750,7 @@
 		IOCTL_AFI,
 	};
 	uint8_t i, ioctl_id;
-	int ret;
+	int32_t ret;
 
 	for (i = 0U; i < ARRAY_SIZE(supported_ids); i++) {
 		ioctl_id = supported_ids[i];

diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.h b/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.h
index 0c5f33f..3b0d6ee 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_ioctl.h

@@ -88,9 +88,9 @@
 #define	PM_DLL_RESET_PULSE 2U
 
 enum pm_ret_status pm_api_ioctl(enum pm_node_id nid,
-				unsigned int ioctl_id,
-				unsigned int arg1,
-				unsigned int arg2,
-				unsigned int *value);
+				uint32_t ioctl_id,
+				uint32_t arg1,
+				uint32_t arg2,
+				uint32_t *value);
 enum pm_ret_status atf_ioctl_bitmask(uint32_t *bit_mask);
 #endif /* PM_API_IOCTL_H */

diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c b/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c
index 9a6b497..0192f81 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.c

@@ -21,7 +21,8 @@
 
 struct pinctrl_function {
 	char name[FUNCTION_NAME_LEN];
-	uint16_t (*groups)[];
+	uint16_t group_base;
+	uint8_t group_size;
 	uint8_t regval;
 };
 
@@ -36,904 +37,344 @@
 	[PINCTRL_FUNC_CAN0] = {
 		.name = "can0",
 		.regval = 0x20,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_CAN0_0,
-			PINCTRL_GRP_CAN0_1,
-			PINCTRL_GRP_CAN0_2,
-			PINCTRL_GRP_CAN0_3,
-			PINCTRL_GRP_CAN0_4,
-			PINCTRL_GRP_CAN0_5,
-			PINCTRL_GRP_CAN0_6,
-			PINCTRL_GRP_CAN0_7,
-			PINCTRL_GRP_CAN0_8,
-			PINCTRL_GRP_CAN0_9,
-			PINCTRL_GRP_CAN0_10,
-			PINCTRL_GRP_CAN0_11,
-			PINCTRL_GRP_CAN0_12,
-			PINCTRL_GRP_CAN0_13,
-			PINCTRL_GRP_CAN0_14,
-			PINCTRL_GRP_CAN0_15,
-			PINCTRL_GRP_CAN0_16,
-			PINCTRL_GRP_CAN0_17,
-			PINCTRL_GRP_CAN0_18,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_CAN0_0,
+		.group_size = PINCTRL_GRP_CAN0_18 - PINCTRL_GRP_CAN0_0 + 1U,
 	},
 	[PINCTRL_FUNC_CAN1] = {
 		.name = "can1",
 		.regval = 0x20,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_CAN1_0,
-			PINCTRL_GRP_CAN1_1,
-			PINCTRL_GRP_CAN1_2,
-			PINCTRL_GRP_CAN1_3,
-			PINCTRL_GRP_CAN1_4,
-			PINCTRL_GRP_CAN1_5,
-			PINCTRL_GRP_CAN1_6,
-			PINCTRL_GRP_CAN1_7,
-			PINCTRL_GRP_CAN1_8,
-			PINCTRL_GRP_CAN1_9,
-			PINCTRL_GRP_CAN1_10,
-			PINCTRL_GRP_CAN1_11,
-			PINCTRL_GRP_CAN1_12,
-			PINCTRL_GRP_CAN1_13,
-			PINCTRL_GRP_CAN1_14,
-			PINCTRL_GRP_CAN1_15,
-			PINCTRL_GRP_CAN1_16,
-			PINCTRL_GRP_CAN1_17,
-			PINCTRL_GRP_CAN1_18,
-			PINCTRL_GRP_CAN1_19,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_CAN1_0,
+		.group_size = PINCTRL_GRP_CAN1_19 - PINCTRL_GRP_CAN1_0 + 1U,
 	},
 	[PINCTRL_FUNC_ETHERNET0] = {
 		.name = "ethernet0",
 		.regval = 0x02,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_ETHERNET0_0,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_ETHERNET0_0,
+		.group_size = PINCTRL_GRP_ETHERNET0_0 - PINCTRL_GRP_ETHERNET0_0 + 1U,
 	},
 	[PINCTRL_FUNC_ETHERNET1] = {
 		.name = "ethernet1",
 		.regval = 0x02,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_ETHERNET1_0,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_ETHERNET1_0,
+		.group_size = PINCTRL_GRP_ETHERNET1_0 - PINCTRL_GRP_ETHERNET1_0 + 1U,
 	},
 	[PINCTRL_FUNC_ETHERNET2] = {
 		.name = "ethernet2",
 		.regval = 0x02,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_ETHERNET2_0,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_ETHERNET2_0,
+		.group_size = PINCTRL_GRP_ETHERNET2_0 - PINCTRL_GRP_ETHERNET2_0 + 1U,
 	},
 	[PINCTRL_FUNC_ETHERNET3] = {
 		.name = "ethernet3",
 		.regval = 0x02,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_ETHERNET3_0,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_ETHERNET3_0,
+		.group_size = PINCTRL_GRP_ETHERNET3_0 - PINCTRL_GRP_ETHERNET3_0 + 1U,
 	},
 	[PINCTRL_FUNC_GEMTSU0] = {
 		.name = "gemtsu0",
 		.regval = 0x02,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_GEMTSU0_0,
-			PINCTRL_GRP_GEMTSU0_1,
-			PINCTRL_GRP_GEMTSU0_2,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_GEMTSU0_0,
+		.group_size = PINCTRL_GRP_GEMTSU0_2 - PINCTRL_GRP_GEMTSU0_0 + 1U,
 	},
 	[PINCTRL_FUNC_GPIO0] = {
 		.name = "gpio0",
 		.regval = 0x00,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_GPIO0_0,
-			PINCTRL_GRP_GPIO0_1,
-			PINCTRL_GRP_GPIO0_2,
-			PINCTRL_GRP_GPIO0_3,
-			PINCTRL_GRP_GPIO0_4,
-			PINCTRL_GRP_GPIO0_5,
-			PINCTRL_GRP_GPIO0_6,
-			PINCTRL_GRP_GPIO0_7,
-			PINCTRL_GRP_GPIO0_8,
-			PINCTRL_GRP_GPIO0_9,
-			PINCTRL_GRP_GPIO0_10,
-			PINCTRL_GRP_GPIO0_11,
-			PINCTRL_GRP_GPIO0_12,
-			PINCTRL_GRP_GPIO0_13,
-			PINCTRL_GRP_GPIO0_14,
-			PINCTRL_GRP_GPIO0_15,
-			PINCTRL_GRP_GPIO0_16,
-			PINCTRL_GRP_GPIO0_17,
-			PINCTRL_GRP_GPIO0_18,
-			PINCTRL_GRP_GPIO0_19,
-			PINCTRL_GRP_GPIO0_20,
-			PINCTRL_GRP_GPIO0_21,
-			PINCTRL_GRP_GPIO0_22,
-			PINCTRL_GRP_GPIO0_23,
-			PINCTRL_GRP_GPIO0_24,
-			PINCTRL_GRP_GPIO0_25,
-			PINCTRL_GRP_GPIO0_26,
-			PINCTRL_GRP_GPIO0_27,
-			PINCTRL_GRP_GPIO0_28,
-			PINCTRL_GRP_GPIO0_29,
-			PINCTRL_GRP_GPIO0_30,
-			PINCTRL_GRP_GPIO0_31,
-			PINCTRL_GRP_GPIO0_32,
-			PINCTRL_GRP_GPIO0_33,
-			PINCTRL_GRP_GPIO0_34,
-			PINCTRL_GRP_GPIO0_35,
-			PINCTRL_GRP_GPIO0_36,
-			PINCTRL_GRP_GPIO0_37,
-			PINCTRL_GRP_GPIO0_38,
-			PINCTRL_GRP_GPIO0_39,
-			PINCTRL_GRP_GPIO0_40,
-			PINCTRL_GRP_GPIO0_41,
-			PINCTRL_GRP_GPIO0_42,
-			PINCTRL_GRP_GPIO0_43,
-			PINCTRL_GRP_GPIO0_44,
-			PINCTRL_GRP_GPIO0_45,
-			PINCTRL_GRP_GPIO0_46,
-			PINCTRL_GRP_GPIO0_47,
-			PINCTRL_GRP_GPIO0_48,
-			PINCTRL_GRP_GPIO0_49,
-			PINCTRL_GRP_GPIO0_50,
-			PINCTRL_GRP_GPIO0_51,
-			PINCTRL_GRP_GPIO0_52,
-			PINCTRL_GRP_GPIO0_53,
-			PINCTRL_GRP_GPIO0_54,
-			PINCTRL_GRP_GPIO0_55,
-			PINCTRL_GRP_GPIO0_56,
-			PINCTRL_GRP_GPIO0_57,
-			PINCTRL_GRP_GPIO0_58,
-			PINCTRL_GRP_GPIO0_59,
-			PINCTRL_GRP_GPIO0_60,
-			PINCTRL_GRP_GPIO0_61,
-			PINCTRL_GRP_GPIO0_62,
-			PINCTRL_GRP_GPIO0_63,
-			PINCTRL_GRP_GPIO0_64,
-			PINCTRL_GRP_GPIO0_65,
-			PINCTRL_GRP_GPIO0_66,
-			PINCTRL_GRP_GPIO0_67,
-			PINCTRL_GRP_GPIO0_68,
-			PINCTRL_GRP_GPIO0_69,
-			PINCTRL_GRP_GPIO0_70,
-			PINCTRL_GRP_GPIO0_71,
-			PINCTRL_GRP_GPIO0_72,
-			PINCTRL_GRP_GPIO0_73,
-			PINCTRL_GRP_GPIO0_74,
-			PINCTRL_GRP_GPIO0_75,
-			PINCTRL_GRP_GPIO0_76,
-			PINCTRL_GRP_GPIO0_77,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_GPIO0_0,
+		.group_size = PINCTRL_GRP_GPIO0_77 - PINCTRL_GRP_GPIO0_0 + 1U,
 	},
 	[PINCTRL_FUNC_I2C0] = {
 		.name = "i2c0",
 		.regval = 0x40,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_I2C0_0,
-			PINCTRL_GRP_I2C0_1,
-			PINCTRL_GRP_I2C0_2,
-			PINCTRL_GRP_I2C0_3,
-			PINCTRL_GRP_I2C0_4,
-			PINCTRL_GRP_I2C0_5,
-			PINCTRL_GRP_I2C0_6,
-			PINCTRL_GRP_I2C0_7,
-			PINCTRL_GRP_I2C0_8,
-			PINCTRL_GRP_I2C0_9,
-			PINCTRL_GRP_I2C0_10,
-			PINCTRL_GRP_I2C0_11,
-			PINCTRL_GRP_I2C0_12,
-			PINCTRL_GRP_I2C0_13,
-			PINCTRL_GRP_I2C0_14,
-			PINCTRL_GRP_I2C0_15,
-			PINCTRL_GRP_I2C0_16,
-			PINCTRL_GRP_I2C0_17,
-			PINCTRL_GRP_I2C0_18,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_I2C0_0,
+		.group_size = PINCTRL_GRP_I2C0_18 - PINCTRL_GRP_I2C0_0 + 1U,
 	},
 	[PINCTRL_FUNC_I2C1] = {
 		.name = "i2c1",
 		.regval = 0x40,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_I2C1_0,
-			PINCTRL_GRP_I2C1_1,
-			PINCTRL_GRP_I2C1_2,
-			PINCTRL_GRP_I2C1_3,
-			PINCTRL_GRP_I2C1_4,
-			PINCTRL_GRP_I2C1_5,
-			PINCTRL_GRP_I2C1_6,
-			PINCTRL_GRP_I2C1_7,
-			PINCTRL_GRP_I2C1_8,
-			PINCTRL_GRP_I2C1_9,
-			PINCTRL_GRP_I2C1_10,
-			PINCTRL_GRP_I2C1_11,
-			PINCTRL_GRP_I2C1_12,
-			PINCTRL_GRP_I2C1_13,
-			PINCTRL_GRP_I2C1_14,
-			PINCTRL_GRP_I2C1_15,
-			PINCTRL_GRP_I2C1_16,
-			PINCTRL_GRP_I2C1_17,
-			PINCTRL_GRP_I2C1_18,
-			PINCTRL_GRP_I2C1_19,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_I2C1_0,
+		.group_size = PINCTRL_GRP_I2C1_19 - PINCTRL_GRP_I2C1_0 + 1U,
 	},
 	[PINCTRL_FUNC_MDIO0] = {
 		.name = "mdio0",
 		.regval = 0x60,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_MDIO0_0,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_MDIO0_0,
+		.group_size = PINCTRL_GRP_MDIO0_0 - PINCTRL_GRP_MDIO0_0 + 1U,
 	},
 	[PINCTRL_FUNC_MDIO1] = {
 		.name = "mdio1",
 		.regval = 0x80,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_MDIO1_0,
-			PINCTRL_GRP_MDIO1_1,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_MDIO1_0,
+		.group_size = PINCTRL_GRP_MDIO1_1 - PINCTRL_GRP_MDIO1_0 + 1U,
 	},
 	[PINCTRL_FUNC_MDIO2] = {
 		.name = "mdio2",
 		.regval = 0xa0,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_MDIO2_0,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_MDIO2_0,
+		.group_size = PINCTRL_GRP_MDIO2_0 - PINCTRL_GRP_MDIO2_0 + 1U,
 	},
 	[PINCTRL_FUNC_MDIO3] = {
 		.name = "mdio3",
 		.regval = 0xc0,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_MDIO3_0,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_MDIO3_0,
+		.group_size = PINCTRL_GRP_MDIO3_0 - PINCTRL_GRP_MDIO3_0 + 1U,
 	},
 	[PINCTRL_FUNC_QSPI0] = {
 		.name = "qspi0",
 		.regval = 0x02,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_QSPI0_0,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_QSPI0_0,
+		.group_size = PINCTRL_GRP_QSPI0_0 - PINCTRL_GRP_QSPI0_0 + 1U,
 	},
 	[PINCTRL_FUNC_QSPI_FBCLK] = {
 		.name = "qspi_fbclk",
 		.regval = 0x02,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_QSPI_FBCLK,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_QSPI_FBCLK,
+		.group_size = PINCTRL_GRP_QSPI_FBCLK - PINCTRL_GRP_QSPI_FBCLK + 1U,
 	},
 	[PINCTRL_FUNC_QSPI_SS] = {
 		.name = "qspi_ss",
 		.regval = 0x02,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_QSPI_SS,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_QSPI_SS,
+		.group_size = PINCTRL_GRP_QSPI_SS - PINCTRL_GRP_QSPI_SS + 1U,
 	},
 	[PINCTRL_FUNC_SPI0] = {
 		.name = "spi0",
 		.regval = 0x80,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_SPI0_0,
-			PINCTRL_GRP_SPI0_1,
-			PINCTRL_GRP_SPI0_2,
-			PINCTRL_GRP_SPI0_3,
-			PINCTRL_GRP_SPI0_4,
-			PINCTRL_GRP_SPI0_5,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_SPI0_0,
+		.group_size = PINCTRL_GRP_SPI0_5 - PINCTRL_GRP_SPI0_0 + 1U,
 	},
 	[PINCTRL_FUNC_SPI1] = {
 		.name = "spi1",
 		.regval = 0x80,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_SPI1_0,
-			PINCTRL_GRP_SPI1_1,
-			PINCTRL_GRP_SPI1_2,
-			PINCTRL_GRP_SPI1_3,
-			PINCTRL_GRP_SPI1_4,
-			PINCTRL_GRP_SPI1_5,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_SPI1_0,
+		.group_size = PINCTRL_GRP_SPI1_5 - PINCTRL_GRP_SPI1_0 + 1U,
 	},
 	[PINCTRL_FUNC_SPI0_SS] = {
 		.name = "spi0_ss",
 		.regval = 0x80,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_SPI0_0_SS0,
-			PINCTRL_GRP_SPI0_0_SS1,
-			PINCTRL_GRP_SPI0_0_SS2,
-			PINCTRL_GRP_SPI0_1_SS0,
-			PINCTRL_GRP_SPI0_1_SS1,
-			PINCTRL_GRP_SPI0_1_SS2,
-			PINCTRL_GRP_SPI0_2_SS0,
-			PINCTRL_GRP_SPI0_2_SS1,
-			PINCTRL_GRP_SPI0_2_SS2,
-			PINCTRL_GRP_SPI0_3_SS0,
-			PINCTRL_GRP_SPI0_3_SS1,
-			PINCTRL_GRP_SPI0_3_SS2,
-			PINCTRL_GRP_SPI0_4_SS0,
-			PINCTRL_GRP_SPI0_4_SS1,
-			PINCTRL_GRP_SPI0_4_SS2,
-			PINCTRL_GRP_SPI0_5_SS0,
-			PINCTRL_GRP_SPI0_5_SS1,
-			PINCTRL_GRP_SPI0_5_SS2,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_SPI0_0_SS0,
+		.group_size = PINCTRL_GRP_SPI0_5_SS2 - PINCTRL_GRP_SPI0_0_SS0 + 1U,
 	},
 	[PINCTRL_FUNC_SPI1_SS] = {
 		.name = "spi1_ss",
 		.regval = 0x80,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_SPI1_0_SS0,
-			PINCTRL_GRP_SPI1_0_SS1,
-			PINCTRL_GRP_SPI1_0_SS2,
-			PINCTRL_GRP_SPI1_1_SS0,
-			PINCTRL_GRP_SPI1_1_SS1,
-			PINCTRL_GRP_SPI1_1_SS2,
-			PINCTRL_GRP_SPI1_2_SS0,
-			PINCTRL_GRP_SPI1_2_SS1,
-			PINCTRL_GRP_SPI1_2_SS2,
-			PINCTRL_GRP_SPI1_3_SS0,
-			PINCTRL_GRP_SPI1_3_SS1,
-			PINCTRL_GRP_SPI1_3_SS2,
-			PINCTRL_GRP_SPI1_4_SS0,
-			PINCTRL_GRP_SPI1_4_SS1,
-			PINCTRL_GRP_SPI1_4_SS2,
-			PINCTRL_GRP_SPI1_5_SS0,
-			PINCTRL_GRP_SPI1_5_SS1,
-			PINCTRL_GRP_SPI1_5_SS2,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_SPI1_0_SS0,
+		.group_size = PINCTRL_GRP_SPI1_5_SS2 - PINCTRL_GRP_SPI1_0_SS0 + 1U,
 	},
 	[PINCTRL_FUNC_SDIO0] = {
 		.name = "sdio0",
 		.regval = 0x08,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_SDIO0_0,
-			PINCTRL_GRP_SDIO0_1,
-			PINCTRL_GRP_SDIO0_2,
-			PINCTRL_GRP_SDIO0_4BIT_0_0,
-			PINCTRL_GRP_SDIO0_4BIT_0_1,
-			PINCTRL_GRP_SDIO0_4BIT_1_0,
-			PINCTRL_GRP_SDIO0_4BIT_1_1,
-			PINCTRL_GRP_SDIO0_4BIT_2_0,
-			PINCTRL_GRP_SDIO0_4BIT_2_1,
-			PINCTRL_GRP_SDIO0_1BIT_0_0,
-			PINCTRL_GRP_SDIO0_1BIT_0_1,
-			PINCTRL_GRP_SDIO0_1BIT_0_2,
-			PINCTRL_GRP_SDIO0_1BIT_0_3,
-			PINCTRL_GRP_SDIO0_1BIT_0_4,
-			PINCTRL_GRP_SDIO0_1BIT_0_5,
-			PINCTRL_GRP_SDIO0_1BIT_0_6,
-			PINCTRL_GRP_SDIO0_1BIT_0_7,
-			PINCTRL_GRP_SDIO0_1BIT_1_0,
-			PINCTRL_GRP_SDIO0_1BIT_1_1,
-			PINCTRL_GRP_SDIO0_1BIT_1_2,
-			PINCTRL_GRP_SDIO0_1BIT_1_3,
-			PINCTRL_GRP_SDIO0_1BIT_1_4,
-			PINCTRL_GRP_SDIO0_1BIT_1_5,
-			PINCTRL_GRP_SDIO0_1BIT_1_6,
-			PINCTRL_GRP_SDIO0_1BIT_1_7,
-			PINCTRL_GRP_SDIO0_1BIT_2_0,
-			PINCTRL_GRP_SDIO0_1BIT_2_1,
-			PINCTRL_GRP_SDIO0_1BIT_2_2,
-			PINCTRL_GRP_SDIO0_1BIT_2_3,
-			PINCTRL_GRP_SDIO0_1BIT_2_4,
-			PINCTRL_GRP_SDIO0_1BIT_2_5,
-			PINCTRL_GRP_SDIO0_1BIT_2_6,
-			PINCTRL_GRP_SDIO0_1BIT_2_7,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_SDIO0_0,
+		.group_size = PINCTRL_GRP_SDIO0_1BIT_2_7 - PINCTRL_GRP_SDIO0_0 + 1U,
 	},
 	[PINCTRL_FUNC_SDIO0_PC] = {
 		.name = "sdio0_pc",
 		.regval = 0x08,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_SDIO0_0_PC,
-			PINCTRL_GRP_SDIO0_1_PC,
-			PINCTRL_GRP_SDIO0_2_PC,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_SDIO0_0_PC,
+		.group_size = PINCTRL_GRP_SDIO0_2_PC - PINCTRL_GRP_SDIO0_0_PC + 1U,
 	},
 	[PINCTRL_FUNC_SDIO0_CD] = {
 		.name = "sdio0_cd",
 		.regval = 0x08,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_SDIO0_0_CD,
-			PINCTRL_GRP_SDIO0_1_CD,
-			PINCTRL_GRP_SDIO0_2_CD,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_SDIO0_0_CD,
+		.group_size = PINCTRL_GRP_SDIO0_2_CD - PINCTRL_GRP_SDIO0_0_CD + 1U,
 	},
 	[PINCTRL_FUNC_SDIO0_WP] = {
 		.name = "sdio0_wp",
 		.regval = 0x08,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_SDIO0_0_WP,
-			PINCTRL_GRP_SDIO0_1_WP,
-			PINCTRL_GRP_SDIO0_2_WP,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_SDIO0_0_WP,
+		.group_size = PINCTRL_GRP_SDIO0_2_WP - PINCTRL_GRP_SDIO0_0_WP + 1U,
 	},
 	[PINCTRL_FUNC_SDIO1] = {
 		.name = "sdio1",
 		.regval = 0x10,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_SDIO1_0,
-			PINCTRL_GRP_SDIO1_4BIT_0_0,
-			PINCTRL_GRP_SDIO1_4BIT_0_1,
-			PINCTRL_GRP_SDIO1_4BIT_1_0,
-			PINCTRL_GRP_SDIO1_1BIT_0_0,
-			PINCTRL_GRP_SDIO1_1BIT_0_1,
-			PINCTRL_GRP_SDIO1_1BIT_0_2,
-			PINCTRL_GRP_SDIO1_1BIT_0_3,
-			PINCTRL_GRP_SDIO1_1BIT_0_4,
-			PINCTRL_GRP_SDIO1_1BIT_0_5,
-			PINCTRL_GRP_SDIO1_1BIT_0_6,
-			PINCTRL_GRP_SDIO1_1BIT_0_7,
-			PINCTRL_GRP_SDIO1_1BIT_1_0,
-			PINCTRL_GRP_SDIO1_1BIT_1_1,
-			PINCTRL_GRP_SDIO1_1BIT_1_2,
-			PINCTRL_GRP_SDIO1_1BIT_1_3,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_SDIO1_0,
+		.group_size = PINCTRL_GRP_SDIO1_1BIT_1_3 - PINCTRL_GRP_SDIO1_0 + 1U,
 	},
 	[PINCTRL_FUNC_SDIO1_PC] = {
 		.name = "sdio1_pc",
 		.regval = 0x10,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_SDIO1_0_PC,
-			PINCTRL_GRP_SDIO1_1_PC,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_SDIO1_0_PC,
+		.group_size = PINCTRL_GRP_SDIO1_1_PC - PINCTRL_GRP_SDIO1_0_PC + 1U,
 	},
 	[PINCTRL_FUNC_SDIO1_CD] = {
 		.name = "sdio1_cd",
 		.regval = 0x10,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_SDIO1_0_CD,
-			PINCTRL_GRP_SDIO1_1_CD,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_SDIO1_0_CD,
+		.group_size = PINCTRL_GRP_SDIO1_1_CD - PINCTRL_GRP_SDIO1_0_CD + 1U,
 	},
 	[PINCTRL_FUNC_SDIO1_WP] = {
 		.name = "sdio1_wp",
 		.regval = 0x10,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_SDIO1_0_WP,
-			PINCTRL_GRP_SDIO1_1_WP,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_SDIO1_0_WP,
+		.group_size = PINCTRL_GRP_SDIO1_1_WP - PINCTRL_GRP_SDIO1_0_WP + 1U,
 	},
 	[PINCTRL_FUNC_NAND0] = {
 		.name = "nand0",
 		.regval = 0x04,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_NAND0_0,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_NAND0_0,
+		.group_size = PINCTRL_GRP_NAND0_0 - PINCTRL_GRP_NAND0_0 + 1U,
 	},
 	[PINCTRL_FUNC_NAND0_CE] = {
 		.name = "nand0_ce",
 		.regval = 0x04,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_NAND0_0_CE,
-			PINCTRL_GRP_NAND0_1_CE,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_NAND0_0_CE,
+		.group_size = PINCTRL_GRP_NAND0_1_CE - PINCTRL_GRP_NAND0_0_CE + 1U,
 	},
 	[PINCTRL_FUNC_NAND0_RB] = {
 		.name = "nand0_rb",
 		.regval = 0x04,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_NAND0_0_RB,
-			PINCTRL_GRP_NAND0_1_RB,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_NAND0_0_RB,
+		.group_size = PINCTRL_GRP_NAND0_1_RB - PINCTRL_GRP_NAND0_0_RB + 1U,
 	},
 	[PINCTRL_FUNC_NAND0_DQS] = {
 		.name = "nand0_dqs",
 		.regval = 0x04,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_NAND0_0_DQS,
-			PINCTRL_GRP_NAND0_1_DQS,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_NAND0_0_DQS,
+		.group_size = PINCTRL_GRP_NAND0_1_DQS - PINCTRL_GRP_NAND0_0_DQS + 1U,
 	},
 	[PINCTRL_FUNC_TTC0_CLK] = {
 		.name = "ttc0_clk",
 		.regval = 0xa0,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_TTC0_0_CLK,
-			PINCTRL_GRP_TTC0_1_CLK,
-			PINCTRL_GRP_TTC0_2_CLK,
-			PINCTRL_GRP_TTC0_3_CLK,
-			PINCTRL_GRP_TTC0_4_CLK,
-			PINCTRL_GRP_TTC0_5_CLK,
-			PINCTRL_GRP_TTC0_6_CLK,
-			PINCTRL_GRP_TTC0_7_CLK,
-			PINCTRL_GRP_TTC0_8_CLK,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_TTC0_0_CLK,
+		.group_size = PINCTRL_GRP_TTC0_8_CLK - PINCTRL_GRP_TTC0_0_CLK + 1U,
 	},
 	[PINCTRL_FUNC_TTC0_WAV] = {
 		.name = "ttc0_wav",
 		.regval = 0xa0,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_TTC0_0_WAV,
-			PINCTRL_GRP_TTC0_1_WAV,
-			PINCTRL_GRP_TTC0_2_WAV,
-			PINCTRL_GRP_TTC0_3_WAV,
-			PINCTRL_GRP_TTC0_4_WAV,
-			PINCTRL_GRP_TTC0_5_WAV,
-			PINCTRL_GRP_TTC0_6_WAV,
-			PINCTRL_GRP_TTC0_7_WAV,
-			PINCTRL_GRP_TTC0_8_WAV,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_TTC0_0_WAV,
+		.group_size = PINCTRL_GRP_TTC0_8_WAV - PINCTRL_GRP_TTC0_0_WAV + 1U,
 	},
 	[PINCTRL_FUNC_TTC1_CLK] = {
 		.name = "ttc1_clk",
 		.regval = 0xa0,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_TTC1_0_CLK,
-			PINCTRL_GRP_TTC1_1_CLK,
-			PINCTRL_GRP_TTC1_2_CLK,
-			PINCTRL_GRP_TTC1_3_CLK,
-			PINCTRL_GRP_TTC1_4_CLK,
-			PINCTRL_GRP_TTC1_5_CLK,
-			PINCTRL_GRP_TTC1_6_CLK,
-			PINCTRL_GRP_TTC1_7_CLK,
-			PINCTRL_GRP_TTC1_8_CLK,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_TTC1_0_CLK,
+		.group_size = PINCTRL_GRP_TTC1_8_CLK - PINCTRL_GRP_TTC1_0_CLK + 1U,
 	},
 	[PINCTRL_FUNC_TTC1_WAV] = {
 		.name = "ttc1_wav",
-		.regval = 0xa0,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_TTC1_0_WAV,
-			PINCTRL_GRP_TTC1_1_WAV,
-			PINCTRL_GRP_TTC1_2_WAV,
-			PINCTRL_GRP_TTC1_3_WAV,
-			PINCTRL_GRP_TTC1_4_WAV,
-			PINCTRL_GRP_TTC1_5_WAV,
-			PINCTRL_GRP_TTC1_6_WAV,
-			PINCTRL_GRP_TTC1_7_WAV,
-			PINCTRL_GRP_TTC1_8_WAV,
-			END_OF_GROUPS,
-		}),
+		.regval = 0xa0,
+		.group_base = PINCTRL_GRP_TTC1_0_WAV,
+		.group_size = PINCTRL_GRP_TTC1_8_WAV - PINCTRL_GRP_TTC1_0_WAV + 1U,
 	},
 	[PINCTRL_FUNC_TTC2_CLK] = {
 		.name = "ttc2_clk",
 		.regval = 0xa0,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_TTC2_0_CLK,
-			PINCTRL_GRP_TTC2_1_CLK,
-			PINCTRL_GRP_TTC2_2_CLK,
-			PINCTRL_GRP_TTC2_3_CLK,
-			PINCTRL_GRP_TTC2_4_CLK,
-			PINCTRL_GRP_TTC2_5_CLK,
-			PINCTRL_GRP_TTC2_6_CLK,
-			PINCTRL_GRP_TTC2_7_CLK,
-			PINCTRL_GRP_TTC2_8_CLK,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_TTC2_0_CLK,
+		.group_size = PINCTRL_GRP_TTC2_8_CLK - PINCTRL_GRP_TTC2_0_CLK + 1U,
 	},
 	[PINCTRL_FUNC_TTC2_WAV] = {
 		.name = "ttc2_wav",
 		.regval = 0xa0,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_TTC2_0_WAV,
-			PINCTRL_GRP_TTC2_1_WAV,
-			PINCTRL_GRP_TTC2_2_WAV,
-			PINCTRL_GRP_TTC2_3_WAV,
-			PINCTRL_GRP_TTC2_4_WAV,
-			PINCTRL_GRP_TTC2_5_WAV,
-			PINCTRL_GRP_TTC2_6_WAV,
-			PINCTRL_GRP_TTC2_7_WAV,
-			PINCTRL_GRP_TTC2_8_WAV,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_TTC2_0_WAV,
+		.group_size = PINCTRL_GRP_TTC2_8_WAV - PINCTRL_GRP_TTC2_0_WAV + 1U,
 	},
 	[PINCTRL_FUNC_TTC3_CLK] = {
 		.name = "ttc3_clk",
 		.regval = 0xa0,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_TTC3_0_CLK,
-			PINCTRL_GRP_TTC3_1_CLK,
-			PINCTRL_GRP_TTC3_2_CLK,
-			PINCTRL_GRP_TTC3_3_CLK,
-			PINCTRL_GRP_TTC3_4_CLK,
-			PINCTRL_GRP_TTC3_5_CLK,
-			PINCTRL_GRP_TTC3_6_CLK,
-			PINCTRL_GRP_TTC3_7_CLK,
-			PINCTRL_GRP_TTC3_8_CLK,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_TTC3_0_CLK,
+		.group_size = PINCTRL_GRP_TTC3_8_CLK - PINCTRL_GRP_TTC3_0_CLK + 1U,
 	},
 	[PINCTRL_FUNC_TTC3_WAV] = {
 		.name = "ttc3_wav",
 		.regval = 0xa0,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_TTC3_0_WAV,
-			PINCTRL_GRP_TTC3_1_WAV,
-			PINCTRL_GRP_TTC3_2_WAV,
-			PINCTRL_GRP_TTC3_3_WAV,
-			PINCTRL_GRP_TTC3_4_WAV,
-			PINCTRL_GRP_TTC3_5_WAV,
-			PINCTRL_GRP_TTC3_6_WAV,
-			PINCTRL_GRP_TTC3_7_WAV,
-			PINCTRL_GRP_TTC3_8_WAV,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_TTC3_0_WAV,
+		.group_size = PINCTRL_GRP_TTC3_8_WAV - PINCTRL_GRP_TTC3_0_WAV + 1U,
 	},
 	[PINCTRL_FUNC_UART0] = {
 		.name = "uart0",
 		.regval = 0xc0,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_UART0_0,
-			PINCTRL_GRP_UART0_1,
-			PINCTRL_GRP_UART0_2,
-			PINCTRL_GRP_UART0_3,
-			PINCTRL_GRP_UART0_4,
-			PINCTRL_GRP_UART0_5,
-			PINCTRL_GRP_UART0_6,
-			PINCTRL_GRP_UART0_7,
-			PINCTRL_GRP_UART0_8,
-			PINCTRL_GRP_UART0_9,
-			PINCTRL_GRP_UART0_10,
-			PINCTRL_GRP_UART0_11,
-			PINCTRL_GRP_UART0_12,
-			PINCTRL_GRP_UART0_13,
-			PINCTRL_GRP_UART0_14,
-			PINCTRL_GRP_UART0_15,
-			PINCTRL_GRP_UART0_16,
-			PINCTRL_GRP_UART0_17,
-			PINCTRL_GRP_UART0_18,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_UART0_0,
+		.group_size = PINCTRL_GRP_UART0_18 - PINCTRL_GRP_UART0_0 + 1U,
 	},
 	[PINCTRL_FUNC_UART1] = {
 		.name = "uart1",
 		.regval = 0xc0,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_UART1_0,
-			PINCTRL_GRP_UART1_1,
-			PINCTRL_GRP_UART1_2,
-			PINCTRL_GRP_UART1_3,
-			PINCTRL_GRP_UART1_4,
-			PINCTRL_GRP_UART1_5,
-			PINCTRL_GRP_UART1_6,
-			PINCTRL_GRP_UART1_7,
-			PINCTRL_GRP_UART1_8,
-			PINCTRL_GRP_UART1_9,
-			PINCTRL_GRP_UART1_10,
-			PINCTRL_GRP_UART1_11,
-			PINCTRL_GRP_UART1_12,
-			PINCTRL_GRP_UART1_13,
-			PINCTRL_GRP_UART1_14,
-			PINCTRL_GRP_UART1_15,
-			PINCTRL_GRP_UART1_16,
-			PINCTRL_GRP_UART1_17,
-			PINCTRL_GRP_UART1_18,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_UART1_0,
+		.group_size = PINCTRL_GRP_UART1_18 - PINCTRL_GRP_UART1_0 + 1U,
 	},
 	[PINCTRL_FUNC_USB0] = {
 		.name = "usb0",
 		.regval = 0x04,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_USB0_0,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_USB0_0,
+		.group_size = PINCTRL_GRP_USB0_0 - PINCTRL_GRP_USB0_0 + 1U,
 	},
 	[PINCTRL_FUNC_USB1] = {
 		.name = "usb1",
 		.regval = 0x04,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_USB1_0,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_USB1_0,
+		.group_size = PINCTRL_GRP_USB1_0 - PINCTRL_GRP_USB1_0 + 1U,
 	},
 	[PINCTRL_FUNC_SWDT0_CLK] = {
 		.name = "swdt0_clk",
 		.regval = 0x60,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_SWDT0_0_CLK,
-			PINCTRL_GRP_SWDT0_1_CLK,
-			PINCTRL_GRP_SWDT0_2_CLK,
-			PINCTRL_GRP_SWDT0_3_CLK,
-			PINCTRL_GRP_SWDT0_4_CLK,
-			PINCTRL_GRP_SWDT0_5_CLK,
-			PINCTRL_GRP_SWDT0_6_CLK,
-			PINCTRL_GRP_SWDT0_7_CLK,
-			PINCTRL_GRP_SWDT0_8_CLK,
-			PINCTRL_GRP_SWDT0_9_CLK,
-			PINCTRL_GRP_SWDT0_10_CLK,
-			PINCTRL_GRP_SWDT0_11_CLK,
-			PINCTRL_GRP_SWDT0_12_CLK,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_SWDT0_0_CLK,
+		.group_size = PINCTRL_GRP_SWDT0_12_CLK - PINCTRL_GRP_SWDT0_0_CLK + 1U,
 	},
 	[PINCTRL_FUNC_SWDT0_RST] = {
 		.name = "swdt0_rst",
 		.regval = 0x60,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_SWDT0_0_RST,
-			PINCTRL_GRP_SWDT0_1_RST,
-			PINCTRL_GRP_SWDT0_2_RST,
-			PINCTRL_GRP_SWDT0_3_RST,
-			PINCTRL_GRP_SWDT0_4_RST,
-			PINCTRL_GRP_SWDT0_5_RST,
-			PINCTRL_GRP_SWDT0_6_RST,
-			PINCTRL_GRP_SWDT0_7_RST,
-			PINCTRL_GRP_SWDT0_8_RST,
-			PINCTRL_GRP_SWDT0_9_RST,
-			PINCTRL_GRP_SWDT0_10_RST,
-			PINCTRL_GRP_SWDT0_11_RST,
-			PINCTRL_GRP_SWDT0_12_RST,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_SWDT0_0_RST,
+		.group_size = PINCTRL_GRP_SWDT0_12_RST - PINCTRL_GRP_SWDT0_0_RST + 1U,
 	},
 	[PINCTRL_FUNC_SWDT1_CLK] = {
 		.name = "swdt1_clk",
 		.regval = 0x60,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_SWDT1_0_CLK,
-			PINCTRL_GRP_SWDT1_1_CLK,
-			PINCTRL_GRP_SWDT1_2_CLK,
-			PINCTRL_GRP_SWDT1_3_CLK,
-			PINCTRL_GRP_SWDT1_4_CLK,
-			PINCTRL_GRP_SWDT1_5_CLK,
-			PINCTRL_GRP_SWDT1_6_CLK,
-			PINCTRL_GRP_SWDT1_7_CLK,
-			PINCTRL_GRP_SWDT1_8_CLK,
-			PINCTRL_GRP_SWDT1_9_CLK,
-			PINCTRL_GRP_SWDT1_10_CLK,
-			PINCTRL_GRP_SWDT1_11_CLK,
-			PINCTRL_GRP_SWDT1_12_CLK,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_SWDT1_0_CLK,
+		.group_size = PINCTRL_GRP_SWDT1_12_CLK - PINCTRL_GRP_SWDT1_0_CLK + 1U,
 	},
 	[PINCTRL_FUNC_SWDT1_RST] = {
 		.name = "swdt1_rst",
 		.regval = 0x60,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_SWDT1_0_RST,
-			PINCTRL_GRP_SWDT1_1_RST,
-			PINCTRL_GRP_SWDT1_2_RST,
-			PINCTRL_GRP_SWDT1_3_RST,
-			PINCTRL_GRP_SWDT1_4_RST,
-			PINCTRL_GRP_SWDT1_5_RST,
-			PINCTRL_GRP_SWDT1_6_RST,
-			PINCTRL_GRP_SWDT1_7_RST,
-			PINCTRL_GRP_SWDT1_8_RST,
-			PINCTRL_GRP_SWDT1_9_RST,
-			PINCTRL_GRP_SWDT1_10_RST,
-			PINCTRL_GRP_SWDT1_11_RST,
-			PINCTRL_GRP_SWDT1_12_RST,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_SWDT1_0_RST,
+		.group_size = PINCTRL_GRP_SWDT1_12_RST - PINCTRL_GRP_SWDT1_0_RST + 1U,
 	},
 	[PINCTRL_FUNC_PMU0] = {
 		.name = "pmu0",
 		.regval = 0x08,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_PMU0_0,
-			PINCTRL_GRP_PMU0_1,
-			PINCTRL_GRP_PMU0_2,
-			PINCTRL_GRP_PMU0_3,
-			PINCTRL_GRP_PMU0_4,
-			PINCTRL_GRP_PMU0_5,
-			PINCTRL_GRP_PMU0_6,
-			PINCTRL_GRP_PMU0_7,
-			PINCTRL_GRP_PMU0_8,
-			PINCTRL_GRP_PMU0_9,
-			PINCTRL_GRP_PMU0_10,
-			PINCTRL_GRP_PMU0_11,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_PMU0_0,
+		.group_size = PINCTRL_GRP_PMU0_11 - PINCTRL_GRP_PMU0_0 + 1U,
 	},
 	[PINCTRL_FUNC_PCIE0] = {
 		.name = "pcie0",
 		.regval = 0x04,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_PCIE0_0,
-			PINCTRL_GRP_PCIE0_1,
-			PINCTRL_GRP_PCIE0_2,
-			PINCTRL_GRP_PCIE0_3,
-			PINCTRL_GRP_PCIE0_4,
-			PINCTRL_GRP_PCIE0_5,
-			PINCTRL_GRP_PCIE0_6,
-			PINCTRL_GRP_PCIE0_7,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_PCIE0_0,
+		.group_size = PINCTRL_GRP_PCIE0_7 - PINCTRL_GRP_PCIE0_0 + 1U,
 	},
 	[PINCTRL_FUNC_CSU0] = {
 		.name = "csu0",
 		.regval = 0x18,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_CSU0_0,
-			PINCTRL_GRP_CSU0_1,
-			PINCTRL_GRP_CSU0_2,
-			PINCTRL_GRP_CSU0_3,
-			PINCTRL_GRP_CSU0_4,
-			PINCTRL_GRP_CSU0_5,
-			PINCTRL_GRP_CSU0_6,
-			PINCTRL_GRP_CSU0_7,
-			PINCTRL_GRP_CSU0_8,
-			PINCTRL_GRP_CSU0_9,
-			PINCTRL_GRP_CSU0_10,
-			PINCTRL_GRP_CSU0_11,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_CSU0_0,
+		.group_size = PINCTRL_GRP_CSU0_11 - PINCTRL_GRP_CSU0_0 + 1U,
 	},
 	[PINCTRL_FUNC_DPAUX0] = {
 		.name = "dpaux0",
 		.regval = 0x18,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_DPAUX0_0,
-			PINCTRL_GRP_DPAUX0_1,
-			PINCTRL_GRP_DPAUX0_2,
-			PINCTRL_GRP_DPAUX0_3,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_DPAUX0_0,
+		.group_size = PINCTRL_GRP_DPAUX0_3 - PINCTRL_GRP_DPAUX0_0 + 1U,
 	},
 	[PINCTRL_FUNC_PJTAG0] = {
 		.name = "pjtag0",
 		.regval = 0x60,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_PJTAG0_0,
-			PINCTRL_GRP_PJTAG0_1,
-			PINCTRL_GRP_PJTAG0_2,
-			PINCTRL_GRP_PJTAG0_3,
-			PINCTRL_GRP_PJTAG0_4,
-			PINCTRL_GRP_PJTAG0_5,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_PJTAG0_0,
+		.group_size = PINCTRL_GRP_PJTAG0_5 - PINCTRL_GRP_PJTAG0_0 + 1U,
 	},
 	[PINCTRL_FUNC_TRACE0] = {
 		.name = "trace0",
 		.regval = 0xe0,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_TRACE0_0,
-			PINCTRL_GRP_TRACE0_1,
-			PINCTRL_GRP_TRACE0_2,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_TRACE0_0,
+		.group_size = PINCTRL_GRP_TRACE0_2 - PINCTRL_GRP_TRACE0_0 + 1U,
 	},
 	[PINCTRL_FUNC_TRACE0_CLK] = {
 		.name = "trace0_clk",
 		.regval = 0xe0,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_TRACE0_0_CLK,
-			PINCTRL_GRP_TRACE0_1_CLK,
-			PINCTRL_GRP_TRACE0_2_CLK,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_TRACE0_0_CLK,
+		.group_size = PINCTRL_GRP_TRACE0_2_CLK - PINCTRL_GRP_TRACE0_0_CLK + 1U,
 	},
 	[PINCTRL_FUNC_TESTSCAN0] = {
 		.name = "testscan0",
 		.regval = 0x10,
-		.groups = &((uint16_t []) {
-			PINCTRL_GRP_TESTSCAN0_0,
-			END_OF_GROUPS,
-		}),
+		.group_base = PINCTRL_GRP_TESTSCAN0_0,
+		.group_size = PINCTRL_GRP_TESTSCAN0_0 - PINCTRL_GRP_TESTSCAN0_0 + 1U,
 	},
 };
 
@@ -2511,7 +1952,7 @@
  *
  * @return	Returns success.
  */
-enum pm_ret_status pm_api_pinctrl_get_num_pins(unsigned int *npins)
+enum pm_ret_status pm_api_pinctrl_get_num_pins(uint32_t *npins)
 {
 	*npins = MAX_PIN;
 
@@ -2526,7 +1967,7 @@
  *
  * @return	Returns success.
  */
-enum pm_ret_status pm_api_pinctrl_get_num_functions(unsigned int *nfuncs)
+enum pm_ret_status pm_api_pinctrl_get_num_functions(uint32_t *nfuncs)
 {
 	*nfuncs = MAX_FUNCTION;
 
@@ -2543,23 +1984,14 @@
  *
  * @return	Returns success.
  */
-enum pm_ret_status pm_api_pinctrl_get_num_func_groups(unsigned int fid,
-						      unsigned int *ngroups)
+enum pm_ret_status pm_api_pinctrl_get_num_func_groups(uint32_t fid,
+						      uint32_t *ngroups)
 {
-	int i = 0;
-	uint16_t *grps;
-
-	if (fid >= MAX_FUNCTION)
+	if (fid >= MAX_FUNCTION) {
 		return PM_RET_ERROR_ARGS;
-
-	*ngroups = 0;
-
-	grps = *pinctrl_functions[fid].groups;
-	if (grps == NULL)
-		return PM_RET_SUCCESS;
+	}
 
-	while (grps[i++] != (uint16_t)END_OF_GROUPS)
-		(*ngroups)++;
+	*ngroups = pinctrl_functions[fid].group_size;
 
 	return PM_RET_SUCCESS;
 }
@@ -2572,12 +2004,13 @@
  * This function is used by master to get name of function specified
  * by given function ID.
  */
-void pm_api_pinctrl_get_function_name(unsigned int fid, char *name)
+void pm_api_pinctrl_get_function_name(uint32_t fid, char *name)
 {
-	if (fid >= MAX_FUNCTION)
+	if (fid >= MAX_FUNCTION) {
 		memcpy(name, END_OF_FUNCTION, FUNCTION_NAME_LEN);
-	else
+	} else {
 		memcpy(name, pinctrl_functions[fid].name, FUNCTION_NAME_LEN);
+	}
 }
 
 /**
@@ -2598,31 +2031,28 @@
  *
  * Return: Returns status, either success or error+reason.
  */
-enum pm_ret_status pm_api_pinctrl_get_function_groups(unsigned int fid,
-						      unsigned int index,
+enum pm_ret_status pm_api_pinctrl_get_function_groups(uint32_t fid,
+						      uint32_t index,
 						      uint16_t *groups)
 {
+	uint16_t grps;
+	uint16_t end_of_grp_offset;
 	unsigned int i;
-	uint16_t *grps;
 
-	if (fid >= MAX_FUNCTION)
+	if (fid >= MAX_FUNCTION) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	memset(groups, END_OF_GROUPS, GROUPS_PAYLOAD_LEN);
 
-	grps = *pinctrl_functions[fid].groups;
-	if (grps == NULL)
-		return PM_RET_SUCCESS;
-
-	/* Skip groups till index */
-	for (i = 0; i < index; i++)
-		if (grps[i] == (uint16_t)END_OF_GROUPS)
-			return PM_RET_SUCCESS;
+	grps = pinctrl_functions[fid].group_base;
+	end_of_grp_offset = grps + pinctrl_functions[fid].group_size;
 
 	for (i = 0; i < NUM_GROUPS_PER_RESP; i++) {
-		groups[i] = grps[index + i];
-		if (groups[i] == (uint16_t)END_OF_GROUPS)
+		if ((grps + index + i) >= end_of_grp_offset) {
 			break;
+		}
+		groups[i] = (grps + index + i);
 	}
 
 	return PM_RET_SUCCESS;
@@ -2646,31 +2076,36 @@
  *
  * Return: Returns status, either success or error+reason.
  */
-enum pm_ret_status pm_api_pinctrl_get_pin_groups(unsigned int pin,
-						 unsigned int index,
+enum pm_ret_status pm_api_pinctrl_get_pin_groups(uint32_t pin,
+						 uint32_t index,
 						 uint16_t *groups)
 {
-	unsigned int i;
+	uint32_t i;
 	uint16_t *grps;
 
-	if (pin >= MAX_PIN)
+	if (pin >= MAX_PIN) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	memset(groups, END_OF_GROUPS, GROUPS_PAYLOAD_LEN);
 
 	grps = *zynqmp_pin_groups[pin].groups;
-	if (!grps)
+	if (grps == NULL) {
 		return PM_RET_SUCCESS;
+	}
 
 	/* Skip groups till index */
-	for (i = 0; i < index; i++)
-		if (grps[i] == (uint16_t)END_OF_GROUPS)
+	for (i = 0; i < index; i++) {
+		if (grps[i] == (uint16_t)END_OF_GROUPS) {
 			return PM_RET_SUCCESS;
+		}
+	}
 
 	for (i = 0; i < NUM_GROUPS_PER_RESP; i++) {
 		groups[i] = grps[index + i];
-		if (groups[i] == (uint16_t)END_OF_GROUPS)
+		if (groups[i] == (uint16_t)END_OF_GROUPS) {
 			break;
+		}
 	}
 
 	return PM_RET_SUCCESS;

diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.h b/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.h
index 2b8fca3..b3159c2 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_pinctrl.h

@@ -187,50 +187,50 @@
 	PINCTRL_GRP_QSPI_SS,
 	PINCTRL_GRP_QSPI_FBCLK,
 	PINCTRL_GRP_SPI0_0,
+	PINCTRL_GRP_SPI0_1,
+	PINCTRL_GRP_SPI0_2,
+	PINCTRL_GRP_SPI0_3,
+	PINCTRL_GRP_SPI0_4,
+	PINCTRL_GRP_SPI0_5,
 	PINCTRL_GRP_SPI0_0_SS0,
 	PINCTRL_GRP_SPI0_0_SS1,
 	PINCTRL_GRP_SPI0_0_SS2,
-	PINCTRL_GRP_SPI0_1,
 	PINCTRL_GRP_SPI0_1_SS0,
 	PINCTRL_GRP_SPI0_1_SS1,
 	PINCTRL_GRP_SPI0_1_SS2,
-	PINCTRL_GRP_SPI0_2,
 	PINCTRL_GRP_SPI0_2_SS0,
 	PINCTRL_GRP_SPI0_2_SS1,
 	PINCTRL_GRP_SPI0_2_SS2,
-	PINCTRL_GRP_SPI0_3,
 	PINCTRL_GRP_SPI0_3_SS0,
 	PINCTRL_GRP_SPI0_3_SS1,
 	PINCTRL_GRP_SPI0_3_SS2,
-	PINCTRL_GRP_SPI0_4,
 	PINCTRL_GRP_SPI0_4_SS0,
 	PINCTRL_GRP_SPI0_4_SS1,
 	PINCTRL_GRP_SPI0_4_SS2,
-	PINCTRL_GRP_SPI0_5,
 	PINCTRL_GRP_SPI0_5_SS0,
 	PINCTRL_GRP_SPI0_5_SS1,
 	PINCTRL_GRP_SPI0_5_SS2,
 	PINCTRL_GRP_SPI1_0,
+	PINCTRL_GRP_SPI1_1,
+	PINCTRL_GRP_SPI1_2,
+	PINCTRL_GRP_SPI1_3,
+	PINCTRL_GRP_SPI1_4,
+	PINCTRL_GRP_SPI1_5,
 	PINCTRL_GRP_SPI1_0_SS0,
 	PINCTRL_GRP_SPI1_0_SS1,
 	PINCTRL_GRP_SPI1_0_SS2,
-	PINCTRL_GRP_SPI1_1,
 	PINCTRL_GRP_SPI1_1_SS0,
 	PINCTRL_GRP_SPI1_1_SS1,
 	PINCTRL_GRP_SPI1_1_SS2,
-	PINCTRL_GRP_SPI1_2,
 	PINCTRL_GRP_SPI1_2_SS0,
 	PINCTRL_GRP_SPI1_2_SS1,
 	PINCTRL_GRP_SPI1_2_SS2,
-	PINCTRL_GRP_SPI1_3,
 	PINCTRL_GRP_SPI1_3_SS0,
 	PINCTRL_GRP_SPI1_3_SS1,
 	PINCTRL_GRP_SPI1_3_SS2,
-	PINCTRL_GRP_SPI1_4,
 	PINCTRL_GRP_SPI1_4_SS0,
 	PINCTRL_GRP_SPI1_4_SS1,
 	PINCTRL_GRP_SPI1_4_SS2,
-	PINCTRL_GRP_SPI1_5,
 	PINCTRL_GRP_SPI1_5_SS0,
 	PINCTRL_GRP_SPI1_5_SS1,
 	PINCTRL_GRP_SPI1_5_SS2,
@@ -268,13 +268,13 @@
 	PINCTRL_GRP_SDIO0_1BIT_2_6,
 	PINCTRL_GRP_SDIO0_1BIT_2_7,
 	PINCTRL_GRP_SDIO0_0_PC,
-	PINCTRL_GRP_SDIO0_0_CD,
-	PINCTRL_GRP_SDIO0_0_WP,
 	PINCTRL_GRP_SDIO0_1_PC,
-	PINCTRL_GRP_SDIO0_1_CD,
-	PINCTRL_GRP_SDIO0_1_WP,
 	PINCTRL_GRP_SDIO0_2_PC,
+	PINCTRL_GRP_SDIO0_0_CD,
+	PINCTRL_GRP_SDIO0_1_CD,
 	PINCTRL_GRP_SDIO0_2_CD,
+	PINCTRL_GRP_SDIO0_0_WP,
+	PINCTRL_GRP_SDIO0_1_WP,
 	PINCTRL_GRP_SDIO0_2_WP,
 	PINCTRL_GRP_SDIO1_0,
 	PINCTRL_GRP_SDIO1_4BIT_0_0,
@@ -293,17 +293,17 @@
 	PINCTRL_GRP_SDIO1_1BIT_1_2,
 	PINCTRL_GRP_SDIO1_1BIT_1_3,
 	PINCTRL_GRP_SDIO1_0_PC,
-	PINCTRL_GRP_SDIO1_0_CD,
-	PINCTRL_GRP_SDIO1_0_WP,
 	PINCTRL_GRP_SDIO1_1_PC,
+	PINCTRL_GRP_SDIO1_0_CD,
 	PINCTRL_GRP_SDIO1_1_CD,
+	PINCTRL_GRP_SDIO1_0_WP,
 	PINCTRL_GRP_SDIO1_1_WP,
 	PINCTRL_GRP_NAND0_0,
 	PINCTRL_GRP_NAND0_0_CE,
-	PINCTRL_GRP_NAND0_0_RB,
-	PINCTRL_GRP_NAND0_0_DQS,
 	PINCTRL_GRP_NAND0_1_CE,
+	PINCTRL_GRP_NAND0_0_RB,
 	PINCTRL_GRP_NAND0_1_RB,
+	PINCTRL_GRP_NAND0_0_DQS,
 	PINCTRL_GRP_NAND0_1_DQS,
 	PINCTRL_GRP_CAN0_0,
 	PINCTRL_GRP_CAN0_1,
@@ -422,128 +422,128 @@
 	PINCTRL_GRP_I2C1_18,
 	PINCTRL_GRP_I2C1_19,
 	PINCTRL_GRP_TTC0_0_CLK,
-	PINCTRL_GRP_TTC0_0_WAV,
 	PINCTRL_GRP_TTC0_1_CLK,
-	PINCTRL_GRP_TTC0_1_WAV,
 	PINCTRL_GRP_TTC0_2_CLK,
-	PINCTRL_GRP_TTC0_2_WAV,
 	PINCTRL_GRP_TTC0_3_CLK,
-	PINCTRL_GRP_TTC0_3_WAV,
 	PINCTRL_GRP_TTC0_4_CLK,
-	PINCTRL_GRP_TTC0_4_WAV,
 	PINCTRL_GRP_TTC0_5_CLK,
-	PINCTRL_GRP_TTC0_5_WAV,
 	PINCTRL_GRP_TTC0_6_CLK,
-	PINCTRL_GRP_TTC0_6_WAV,
 	PINCTRL_GRP_TTC0_7_CLK,
-	PINCTRL_GRP_TTC0_7_WAV,
 	PINCTRL_GRP_TTC0_8_CLK,
+	PINCTRL_GRP_TTC0_0_WAV,
+	PINCTRL_GRP_TTC0_1_WAV,
+	PINCTRL_GRP_TTC0_2_WAV,
+	PINCTRL_GRP_TTC0_3_WAV,
+	PINCTRL_GRP_TTC0_4_WAV,
+	PINCTRL_GRP_TTC0_5_WAV,
+	PINCTRL_GRP_TTC0_6_WAV,
+	PINCTRL_GRP_TTC0_7_WAV,
 	PINCTRL_GRP_TTC0_8_WAV,
 	PINCTRL_GRP_TTC1_0_CLK,
-	PINCTRL_GRP_TTC1_0_WAV,
 	PINCTRL_GRP_TTC1_1_CLK,
-	PINCTRL_GRP_TTC1_1_WAV,
 	PINCTRL_GRP_TTC1_2_CLK,
-	PINCTRL_GRP_TTC1_2_WAV,
 	PINCTRL_GRP_TTC1_3_CLK,
-	PINCTRL_GRP_TTC1_3_WAV,
 	PINCTRL_GRP_TTC1_4_CLK,
-	PINCTRL_GRP_TTC1_4_WAV,
 	PINCTRL_GRP_TTC1_5_CLK,
-	PINCTRL_GRP_TTC1_5_WAV,
 	PINCTRL_GRP_TTC1_6_CLK,
-	PINCTRL_GRP_TTC1_6_WAV,
 	PINCTRL_GRP_TTC1_7_CLK,
-	PINCTRL_GRP_TTC1_7_WAV,
 	PINCTRL_GRP_TTC1_8_CLK,
+	PINCTRL_GRP_TTC1_0_WAV,
+	PINCTRL_GRP_TTC1_1_WAV,
+	PINCTRL_GRP_TTC1_2_WAV,
+	PINCTRL_GRP_TTC1_3_WAV,
+	PINCTRL_GRP_TTC1_4_WAV,
+	PINCTRL_GRP_TTC1_5_WAV,
+	PINCTRL_GRP_TTC1_6_WAV,
+	PINCTRL_GRP_TTC1_7_WAV,
 	PINCTRL_GRP_TTC1_8_WAV,
 	PINCTRL_GRP_TTC2_0_CLK,
-	PINCTRL_GRP_TTC2_0_WAV,
 	PINCTRL_GRP_TTC2_1_CLK,
-	PINCTRL_GRP_TTC2_1_WAV,
 	PINCTRL_GRP_TTC2_2_CLK,
-	PINCTRL_GRP_TTC2_2_WAV,
 	PINCTRL_GRP_TTC2_3_CLK,
-	PINCTRL_GRP_TTC2_3_WAV,
 	PINCTRL_GRP_TTC2_4_CLK,
-	PINCTRL_GRP_TTC2_4_WAV,
 	PINCTRL_GRP_TTC2_5_CLK,
-	PINCTRL_GRP_TTC2_5_WAV,
 	PINCTRL_GRP_TTC2_6_CLK,
-	PINCTRL_GRP_TTC2_6_WAV,
 	PINCTRL_GRP_TTC2_7_CLK,
-	PINCTRL_GRP_TTC2_7_WAV,
 	PINCTRL_GRP_TTC2_8_CLK,
+	PINCTRL_GRP_TTC2_0_WAV,
+	PINCTRL_GRP_TTC2_1_WAV,
+	PINCTRL_GRP_TTC2_2_WAV,
+	PINCTRL_GRP_TTC2_3_WAV,
+	PINCTRL_GRP_TTC2_4_WAV,
+	PINCTRL_GRP_TTC2_5_WAV,
+	PINCTRL_GRP_TTC2_6_WAV,
+	PINCTRL_GRP_TTC2_7_WAV,
 	PINCTRL_GRP_TTC2_8_WAV,
 	PINCTRL_GRP_TTC3_0_CLK,
-	PINCTRL_GRP_TTC3_0_WAV,
 	PINCTRL_GRP_TTC3_1_CLK,
-	PINCTRL_GRP_TTC3_1_WAV,
 	PINCTRL_GRP_TTC3_2_CLK,
-	PINCTRL_GRP_TTC3_2_WAV,
 	PINCTRL_GRP_TTC3_3_CLK,
-	PINCTRL_GRP_TTC3_3_WAV,
 	PINCTRL_GRP_TTC3_4_CLK,
-	PINCTRL_GRP_TTC3_4_WAV,
 	PINCTRL_GRP_TTC3_5_CLK,
-	PINCTRL_GRP_TTC3_5_WAV,
 	PINCTRL_GRP_TTC3_6_CLK,
-	PINCTRL_GRP_TTC3_6_WAV,
 	PINCTRL_GRP_TTC3_7_CLK,
-	PINCTRL_GRP_TTC3_7_WAV,
 	PINCTRL_GRP_TTC3_8_CLK,
+	PINCTRL_GRP_TTC3_0_WAV,
+	PINCTRL_GRP_TTC3_1_WAV,
+	PINCTRL_GRP_TTC3_2_WAV,
+	PINCTRL_GRP_TTC3_3_WAV,
+	PINCTRL_GRP_TTC3_4_WAV,
+	PINCTRL_GRP_TTC3_5_WAV,
+	PINCTRL_GRP_TTC3_6_WAV,
+	PINCTRL_GRP_TTC3_7_WAV,
 	PINCTRL_GRP_TTC3_8_WAV,
 	PINCTRL_GRP_SWDT0_0_CLK,
-	PINCTRL_GRP_SWDT0_0_RST,
 	PINCTRL_GRP_SWDT0_1_CLK,
-	PINCTRL_GRP_SWDT0_1_RST,
 	PINCTRL_GRP_SWDT0_2_CLK,
-	PINCTRL_GRP_SWDT0_2_RST,
 	PINCTRL_GRP_SWDT0_3_CLK,
-	PINCTRL_GRP_SWDT0_3_RST,
 	PINCTRL_GRP_SWDT0_4_CLK,
-	PINCTRL_GRP_SWDT0_4_RST,
 	PINCTRL_GRP_SWDT0_5_CLK,
-	PINCTRL_GRP_SWDT0_5_RST,
 	PINCTRL_GRP_SWDT0_6_CLK,
-	PINCTRL_GRP_SWDT0_6_RST,
 	PINCTRL_GRP_SWDT0_7_CLK,
-	PINCTRL_GRP_SWDT0_7_RST,
 	PINCTRL_GRP_SWDT0_8_CLK,
-	PINCTRL_GRP_SWDT0_8_RST,
 	PINCTRL_GRP_SWDT0_9_CLK,
-	PINCTRL_GRP_SWDT0_9_RST,
 	PINCTRL_GRP_SWDT0_10_CLK,
-	PINCTRL_GRP_SWDT0_10_RST,
 	PINCTRL_GRP_SWDT0_11_CLK,
-	PINCTRL_GRP_SWDT0_11_RST,
 	PINCTRL_GRP_SWDT0_12_CLK,
+	PINCTRL_GRP_SWDT0_0_RST,
+	PINCTRL_GRP_SWDT0_1_RST,
+	PINCTRL_GRP_SWDT0_2_RST,
+	PINCTRL_GRP_SWDT0_3_RST,
+	PINCTRL_GRP_SWDT0_4_RST,
+	PINCTRL_GRP_SWDT0_5_RST,
+	PINCTRL_GRP_SWDT0_6_RST,
+	PINCTRL_GRP_SWDT0_7_RST,
+	PINCTRL_GRP_SWDT0_8_RST,
+	PINCTRL_GRP_SWDT0_9_RST,
+	PINCTRL_GRP_SWDT0_10_RST,
+	PINCTRL_GRP_SWDT0_11_RST,
 	PINCTRL_GRP_SWDT0_12_RST,
 	PINCTRL_GRP_SWDT1_0_CLK,
-	PINCTRL_GRP_SWDT1_0_RST,
 	PINCTRL_GRP_SWDT1_1_CLK,
-	PINCTRL_GRP_SWDT1_1_RST,
 	PINCTRL_GRP_SWDT1_2_CLK,
-	PINCTRL_GRP_SWDT1_2_RST,
 	PINCTRL_GRP_SWDT1_3_CLK,
-	PINCTRL_GRP_SWDT1_3_RST,
 	PINCTRL_GRP_SWDT1_4_CLK,
-	PINCTRL_GRP_SWDT1_4_RST,
 	PINCTRL_GRP_SWDT1_5_CLK,
-	PINCTRL_GRP_SWDT1_5_RST,
 	PINCTRL_GRP_SWDT1_6_CLK,
-	PINCTRL_GRP_SWDT1_6_RST,
 	PINCTRL_GRP_SWDT1_7_CLK,
-	PINCTRL_GRP_SWDT1_7_RST,
 	PINCTRL_GRP_SWDT1_8_CLK,
-	PINCTRL_GRP_SWDT1_8_RST,
 	PINCTRL_GRP_SWDT1_9_CLK,
-	PINCTRL_GRP_SWDT1_9_RST,
 	PINCTRL_GRP_SWDT1_10_CLK,
-	PINCTRL_GRP_SWDT1_10_RST,
 	PINCTRL_GRP_SWDT1_11_CLK,
-	PINCTRL_GRP_SWDT1_11_RST,
 	PINCTRL_GRP_SWDT1_12_CLK,
+	PINCTRL_GRP_SWDT1_0_RST,
+	PINCTRL_GRP_SWDT1_1_RST,
+	PINCTRL_GRP_SWDT1_2_RST,
+	PINCTRL_GRP_SWDT1_3_RST,
+	PINCTRL_GRP_SWDT1_4_RST,
+	PINCTRL_GRP_SWDT1_5_RST,
+	PINCTRL_GRP_SWDT1_6_RST,
+	PINCTRL_GRP_SWDT1_7_RST,
+	PINCTRL_GRP_SWDT1_8_RST,
+	PINCTRL_GRP_SWDT1_9_RST,
+	PINCTRL_GRP_SWDT1_10_RST,
+	PINCTRL_GRP_SWDT1_11_RST,
 	PINCTRL_GRP_SWDT1_12_RST,
 	PINCTRL_GRP_GPIO0_0,
 	PINCTRL_GRP_GPIO0_1,
@@ -668,10 +668,10 @@
 	PINCTRL_GRP_PJTAG0_4,
 	PINCTRL_GRP_PJTAG0_5,
 	PINCTRL_GRP_TRACE0_0,
-	PINCTRL_GRP_TRACE0_0_CLK,
 	PINCTRL_GRP_TRACE0_1,
-	PINCTRL_GRP_TRACE0_1_CLK,
 	PINCTRL_GRP_TRACE0_2,
+	PINCTRL_GRP_TRACE0_0_CLK,
+	PINCTRL_GRP_TRACE0_1_CLK,
 	PINCTRL_GRP_TRACE0_2_CLK,
 	PINCTRL_GRP_TESTSCAN0_0,
 };
@@ -709,15 +709,15 @@
 #define	PINCTRL_DRIVE_STRENGTH_8MA 2U
 #define	PINCTRL_DRIVE_STRENGTH_12MA 3U
 
-void pm_api_pinctrl_get_function_name(unsigned int fid, char *name);
-enum pm_ret_status pm_api_pinctrl_get_function_groups(unsigned int fid,
-						      unsigned int index,
+void pm_api_pinctrl_get_function_name(uint32_t fid, char *name);
+enum pm_ret_status pm_api_pinctrl_get_function_groups(uint32_t fid,
+						      uint32_t index,
 						      uint16_t *groups);
-enum pm_ret_status pm_api_pinctrl_get_pin_groups(unsigned int pin,
-						 unsigned int index,
+enum pm_ret_status pm_api_pinctrl_get_pin_groups(uint32_t pin,
+						 uint32_t index,
 						 uint16_t *groups);
-enum pm_ret_status pm_api_pinctrl_get_num_pins(unsigned int *npins);
-enum pm_ret_status pm_api_pinctrl_get_num_functions(unsigned int *nfuncs);
-enum pm_ret_status pm_api_pinctrl_get_num_func_groups(unsigned int fid,
-						      unsigned int *ngroups);
+enum pm_ret_status pm_api_pinctrl_get_num_pins(uint32_t *npins);
+enum pm_ret_status pm_api_pinctrl_get_num_functions(uint32_t *nfuncs);
+enum pm_ret_status pm_api_pinctrl_get_num_func_groups(uint32_t fid,
+						      uint32_t *ngroups);
 #endif /* PM_API_PINCTRL_H */

diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_sys.c b/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
index f9af451..a17b6c5 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_sys.c

@@ -240,14 +240,14 @@
 };
 
 /* default shutdown/reboot scope is system(2) */
-static unsigned int pm_shutdown_scope = PMF_SHUTDOWN_SUBTYPE_SYSTEM;
+static uint32_t pm_shutdown_scope = PMF_SHUTDOWN_SUBTYPE_SYSTEM;
 
 /**
  * pm_get_shutdown_scope() - Get the currently set shutdown scope
  *
  * @return	Shutdown scope value
  */
-unsigned int pm_get_shutdown_scope(void)
+uint32_t pm_get_shutdown_scope(void)
 {
 	return pm_shutdown_scope;
 }
@@ -269,12 +269,12 @@
  * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_self_suspend(enum pm_node_id nid,
-				   unsigned int latency,
-				   unsigned int state,
+				   uint32_t latency,
+				   uint32_t state,
 				   uintptr_t address)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
-	unsigned int cpuid = plat_my_core_pos();
+	uint32_t cpuid = plat_my_core_pos();
 	const struct pm_proc *proc = pm_get_proc(cpuid);
 
 	/*
@@ -300,16 +300,17 @@
  */
 enum pm_ret_status pm_req_suspend(enum pm_node_id target,
 				  enum pm_request_ack ack,
-				  unsigned int latency, unsigned int state)
+				  uint32_t latency, uint32_t state)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD5(payload, PM_REQ_SUSPEND, target, ack, latency, state);
-	if (ack == REQ_ACK_BLOCKING)
+	if (ack == REQ_ACK_BLOCKING) {
 		return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-	else
+	} else {
 		return pm_ipi_send(primary_proc, payload);
+	}
 }
 
 /**
@@ -329,7 +330,7 @@
  * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_req_wakeup(enum pm_node_id target,
-				 unsigned int set_address,
+				 uint32_t set_address,
 				 uintptr_t address,
 				 enum pm_request_ack ack)
 {
@@ -345,10 +346,11 @@
 	PM_PACK_PAYLOAD5(payload, PM_REQ_WAKEUP, target, encoded_address,
 			 encoded_address >> 32, ack);
 
-	if (ack == REQ_ACK_BLOCKING)
+	if (ack == REQ_ACK_BLOCKING) {
 		return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-	else
+	} else {
 		return pm_ipi_send(primary_proc, payload);
+	}
 }
 
 /**
@@ -367,10 +369,11 @@
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD3(payload, PM_FORCE_POWERDOWN, target, ack);
 
-	if (ack == REQ_ACK_BLOCKING)
+	if (ack == REQ_ACK_BLOCKING) {
 		return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-	else
+	} else {
 		return pm_ipi_send(primary_proc, payload);
+	}
 }
 
 /**
@@ -409,7 +412,7 @@
  */
 enum pm_ret_status pm_set_wakeup_source(enum pm_node_id target,
 					enum pm_node_id wkup_node,
-					unsigned int enable)
+					uint32_t enable)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
@@ -425,7 +428,7 @@
  *
  * @return	Returns status, either success or error+reason
  */
-enum pm_ret_status pm_system_shutdown(unsigned int type, unsigned int subtype)
+enum pm_ret_status pm_system_shutdown(uint32_t type, uint32_t subtype)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
@@ -451,18 +454,19 @@
  * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_req_node(enum pm_node_id nid,
-			       unsigned int capabilities,
-			       unsigned int qos,
+			       uint32_t capabilities,
+			       uint32_t qos,
 			       enum pm_request_ack ack)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
 	PM_PACK_PAYLOAD5(payload, PM_REQ_NODE, nid, capabilities, qos, ack);
 
-	if (ack == REQ_ACK_BLOCKING)
+	if (ack == REQ_ACK_BLOCKING) {
 		return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-	else
+	} else {
 		return pm_ipi_send(primary_proc, payload);
+	}
 }
 
 /**
@@ -477,8 +481,8 @@
  * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_set_requirement(enum pm_node_id nid,
-				      unsigned int capabilities,
-				      unsigned int qos,
+				      uint32_t capabilities,
+				      uint32_t qos,
 				      enum pm_request_ack ack)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
@@ -486,10 +490,11 @@
 	PM_PACK_PAYLOAD5(payload, PM_SET_REQUIREMENT, nid, capabilities, qos,
 			 ack);
 
-	if (ack == REQ_ACK_BLOCKING)
+	if (ack == REQ_ACK_BLOCKING) {
 		return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
-	else
+	} else {
 		return pm_ipi_send(primary_proc, payload);
+	}
 }
 
 /* Miscellaneous API functions */
@@ -500,7 +505,7 @@
  *
  * @return	Returns status, either success or error+reason
  */
-enum pm_ret_status pm_get_api_version(unsigned int *version)
+enum pm_ret_status pm_get_api_version(uint32_t *version)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
@@ -540,8 +545,8 @@
  * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_mmio_write(uintptr_t address,
-				 unsigned int mask,
-				 unsigned int value)
+				 uint32_t mask,
+				 uint32_t value)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
@@ -560,7 +565,7 @@
  *
  * @return	Returns status, either success or error+reason
  */
-enum pm_ret_status pm_mmio_read(uintptr_t address, unsigned int *value)
+enum pm_ret_status pm_mmio_read(uintptr_t address, uint32_t *value)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
@@ -604,7 +609,7 @@
  * the fpga status
  * @return      Returns status, either success or error+reason
  */
-enum pm_ret_status pm_fpga_get_status(unsigned int *value)
+enum pm_ret_status pm_fpga_get_status(uint32_t *value)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
@@ -689,8 +694,9 @@
 void pm_get_callbackdata(uint32_t *data, size_t count)
 {
 	/* Return if interrupt is not from PMU */
-	if (!pm_ipi_irq_status(primary_proc))
+	if (!pm_ipi_irq_status(primary_proc)) {
 		return;
+	}
 
 	pm_ipi_buff_read_callb(data, count);
 	pm_ipi_irq_clear(primary_proc);
@@ -709,10 +715,10 @@
  * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_ioctl(enum pm_node_id nid,
-			    unsigned int ioctl_id,
-			    unsigned int arg1,
-			    unsigned int arg2,
-			    unsigned int *value)
+			    uint32_t ioctl_id,
+			    uint32_t arg1,
+			    uint32_t arg2,
+			    uint32_t *value)
 {
 	return pm_api_ioctl(nid, ioctl_id, arg1, arg2, value);
 }
@@ -941,7 +947,7 @@
  *
  * Return: Returns status, either success or error+reason.
  */
-static enum pm_ret_status pm_clock_get_max_divisor(unsigned int clock_id,
+static enum pm_ret_status pm_clock_get_max_divisor(uint32_t clock_id,
 						   uint8_t div_type,
 						   uint32_t *max_div)
 {
@@ -969,7 +975,7 @@
  * This function is used by master to get nmae of clock specified
  * by given clock ID.
  */
-static void pm_clock_get_name(unsigned int clock_id, char *name)
+static void pm_clock_get_name(uint32_t clock_id, char *name)
 {
 	pm_api_clock_get_name(clock_id, name);
 }
@@ -987,8 +993,8 @@
  *
  * @return	Returns status, either success or error+reason
  */
-static enum pm_ret_status pm_clock_get_topology(unsigned int clock_id,
-						unsigned int index,
+static enum pm_ret_status pm_clock_get_topology(uint32_t clock_id,
+						uint32_t index,
 						uint32_t *topology)
 {
 	return pm_api_clock_get_topology(clock_id, index, topology);
@@ -1006,7 +1012,7 @@
  *
  * @return	Returns status, either success or error+reason
  */
-static enum pm_ret_status pm_clock_get_fixedfactor_params(unsigned int clock_id,
+static enum pm_ret_status pm_clock_get_fixedfactor_params(uint32_t clock_id,
 							  uint32_t *mul,
 							  uint32_t *div)
 {
@@ -1030,8 +1036,8 @@
  *
  * @return	Returns status, either success or error+reason
  */
-static enum pm_ret_status pm_clock_get_parents(unsigned int clock_id,
-					       unsigned int index,
+static enum pm_ret_status pm_clock_get_parents(uint32_t clock_id,
+					       uint32_t index,
 					       uint32_t *parents)
 {
 	return pm_api_clock_get_parents(clock_id, index, parents);
@@ -1047,7 +1053,7 @@
  *
  * @return	Returns status, either success or error+reason
  */
-static enum pm_ret_status pm_clock_get_attributes(unsigned int clock_id,
+static enum pm_ret_status pm_clock_get_attributes(uint32_t clock_id,
 						  uint32_t *attr)
 {
 	return pm_api_clock_get_attributes(clock_id, attr);
@@ -1061,8 +1067,8 @@
  * @return	Error if an argument is not valid or status as returned by the
  *		PM controller (PMU)
  */
-static enum pm_ret_status pm_clock_gate(unsigned int clock_id,
-					unsigned char enable)
+static enum pm_ret_status pm_clock_gate(uint32_t clock_id,
+					uint8_t enable)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
 	enum pm_ret_status status;
@@ -1070,21 +1076,24 @@
 
 	/* Check if clock ID is valid and return an error if it is not */
 	status = pm_clock_id_is_valid(clock_id);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
-	if (enable)
+	if (enable) {
 		api_id = PM_CLOCK_ENABLE;
-	else
+	} else {
 		api_id = PM_CLOCK_DISABLE;
+	}
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD2(payload, api_id, clock_id);
 	status = pm_ipi_send_sync(primary_proc, payload, NULL, 0);
 
 	/* If action fails due to the lack of permissions filter the error */
-	if (status == PM_RET_ERROR_ACCESS)
+	if (status == PM_RET_ERROR_ACCESS) {
 		status = PM_RET_SUCCESS;
+	}
 
 	return status;
 }
@@ -1099,14 +1108,15 @@
  * @return:	Error if an argument is not valid or status as returned by the
  *		pm_clock_gate
  */
-enum pm_ret_status pm_clock_enable(unsigned int clock_id)
+enum pm_ret_status pm_clock_enable(uint32_t clock_id)
 {
 	struct pm_pll *pll;
 
 	/* First try to handle it as a PLL */
 	pll = pm_clock_get_pll(clock_id);
-	if (pll)
+	if (pll) {
 		return pm_clock_pll_enable(pll);
+	}
 
 	/* It's an on-chip clock, PMU should configure clock's gate */
 	return pm_clock_gate(clock_id, 1);
@@ -1122,14 +1132,15 @@
  * @return:	Error if an argument is not valid or status as returned by the
  *		pm_clock_gate
  */
-enum pm_ret_status pm_clock_disable(unsigned int clock_id)
+enum pm_ret_status pm_clock_disable(uint32_t clock_id)
 {
 	struct pm_pll *pll;
 
 	/* First try to handle it as a PLL */
 	pll = pm_clock_get_pll(clock_id);
-	if (pll)
+	if (pll) {
 		return pm_clock_pll_disable(pll);
+	}
 
 	/* It's an on-chip clock, PMU should configure clock's gate */
 	return pm_clock_gate(clock_id, 0);
@@ -1145,8 +1156,8 @@
  *
  * Return: Returns status, either success or error+reason.
  */
-enum pm_ret_status pm_clock_getstate(unsigned int clock_id,
-				     unsigned int *state)
+enum pm_ret_status pm_clock_getstate(uint32_t clock_id,
+				     uint32_t *state)
 {
 	struct pm_pll *pll;
 	uint32_t payload[PAYLOAD_ARG_CNT];
@@ -1159,8 +1170,9 @@
 
 	/* Check if clock ID is a valid on-chip clock */
 	status = pm_clock_id_is_valid(clock_id);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD2(payload, PM_CLOCK_GETSTATE, clock_id);
@@ -1177,8 +1189,8 @@
  *
  * Return: Returns status, either success or error+reason.
  */
-enum pm_ret_status pm_clock_setdivider(unsigned int clock_id,
-				       unsigned int divider)
+enum pm_ret_status pm_clock_setdivider(uint32_t clock_id,
+				       uint32_t divider)
 {
 	enum pm_ret_status status;
 	enum pm_node_id nid;
@@ -1190,13 +1202,15 @@
 
 	/* Get PLL node ID using PLL clock ID */
 	status = pm_clock_get_pll_node_id(clock_id, &nid);
-	if (status == PM_RET_SUCCESS)
+	if (status == PM_RET_SUCCESS) {
 		return pm_pll_set_parameter(nid, PM_PLL_PARAM_FBDIV, divider);
+	}
 
 	/* Check if clock ID is a valid on-chip clock */
 	status = pm_clock_id_is_valid(clock_id);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
 	if (div0 == (divider & div0)) {
 		div_id = PM_CLOCK_DIV0_ID;
@@ -1223,8 +1237,8 @@
  *
  * Return: Returns status, either success or error+reason.
  */
-enum pm_ret_status pm_clock_getdivider(unsigned int clock_id,
-				       unsigned int *divider)
+enum pm_ret_status pm_clock_getdivider(uint32_t clock_id,
+				       uint32_t *divider)
 {
 	enum pm_ret_status status;
 	enum pm_node_id nid;
@@ -1233,21 +1247,24 @@
 
 	/* Get PLL node ID using PLL clock ID */
 	status = pm_clock_get_pll_node_id(clock_id, &nid);
-	if (status == PM_RET_SUCCESS)
+	if (status == PM_RET_SUCCESS) {
 		return pm_pll_get_parameter(nid, PM_PLL_PARAM_FBDIV, divider);
+	}
 
 	/* Check if clock ID is a valid on-chip clock */
 	status = pm_clock_id_is_valid(clock_id);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
 	if (pm_clock_has_div(clock_id, PM_CLOCK_DIV0_ID)) {
 		/* Send request to the PMU to get div0 */
 		PM_PACK_PAYLOAD3(payload, PM_CLOCK_GETDIVIDER, clock_id,
 				 PM_CLOCK_DIV0_ID);
 		status = pm_ipi_send_sync(primary_proc, payload, &val, 1);
-		if (status != PM_RET_SUCCESS)
+		if (status != PM_RET_SUCCESS) {
 			return status;
+		}
 		*divider = val;
 	}
 
@@ -1256,8 +1273,9 @@
 		PM_PACK_PAYLOAD3(payload, PM_CLOCK_GETDIVIDER, clock_id,
 				 PM_CLOCK_DIV1_ID);
 		status = pm_ipi_send_sync(primary_proc, payload, &val, 1);
-		if (status != PM_RET_SUCCESS)
+		if (status != PM_RET_SUCCESS) {
 			return status;
+		}
 		*divider |= val << 16;
 	}
 
@@ -1273,7 +1291,7 @@
  *
  * Return: Returns status, either success or error+reason.
  */
-enum pm_ret_status pm_clock_setrate(unsigned int clock_id,
+enum pm_ret_status pm_clock_setrate(uint32_t clock_id,
 				    uint64_t rate)
 {
 	return PM_RET_ERROR_NOTSUPPORTED;
@@ -1289,7 +1307,7 @@
  *
  * Return: Returns status, either success or error+reason.
  */
-enum pm_ret_status pm_clock_getrate(unsigned int clock_id,
+enum pm_ret_status pm_clock_getrate(uint32_t clock_id,
 				    uint64_t *rate)
 {
 	return PM_RET_ERROR_NOTSUPPORTED;
@@ -1304,8 +1322,8 @@
  *
  * Return: Returns status, either success or error+reason.
  */
-enum pm_ret_status pm_clock_setparent(unsigned int clock_id,
-				      unsigned int parent_index)
+enum pm_ret_status pm_clock_setparent(uint32_t clock_id,
+				      uint32_t parent_index)
 {
 	struct pm_pll *pll;
 	uint32_t payload[PAYLOAD_ARG_CNT];
@@ -1313,13 +1331,15 @@
 
 	/* First try to handle it as a PLL */
 	pll = pm_clock_get_pll_by_related_clk(clock_id);
-	if (pll)
+	if (pll) {
 		return pm_clock_pll_set_parent(pll, clock_id, parent_index);
+	}
 
 	/* Check if clock ID is a valid on-chip clock */
 	status = pm_clock_id_is_valid(clock_id);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD3(payload, PM_CLOCK_SETPARENT, clock_id, parent_index);
@@ -1336,8 +1356,8 @@
  *
  * Return: Returns status, either success or error+reason.
  */
-enum pm_ret_status pm_clock_getparent(unsigned int clock_id,
-				      unsigned int *parent_index)
+enum pm_ret_status pm_clock_getparent(uint32_t clock_id,
+				      uint32_t *parent_index)
 {
 	struct pm_pll *pll;
 	uint32_t payload[PAYLOAD_ARG_CNT];
@@ -1345,13 +1365,15 @@
 
 	/* First try to handle it as a PLL */
 	pll = pm_clock_get_pll_by_related_clk(clock_id);
-	if (pll)
+	if (pll) {
 		return pm_clock_pll_get_parent(pll, clock_id, parent_index);
+	}
 
 	/* Check if clock ID is a valid on-chip clock */
 	status = pm_clock_id_is_valid(clock_id);
-	if (status != PM_RET_SUCCESS)
+	if (status != PM_RET_SUCCESS) {
 		return status;
+	}
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD2(payload, PM_CLOCK_GETPARENT, clock_id);
@@ -1395,7 +1417,7 @@
  *
  * Return: Returns status, either success or error+reason.
  */
-static enum pm_ret_status pm_pinctrl_get_num_function_groups(unsigned int fid,
+static enum pm_ret_status pm_pinctrl_get_num_function_groups(uint32_t fid,
 							     uint32_t *ngroups)
 {
 	return pm_api_pinctrl_get_num_func_groups(fid, ngroups);
@@ -1409,7 +1431,7 @@
  * This function is used by master to get name of function specified
  * by given function Id
  */
-static void pm_pinctrl_get_function_name(unsigned int fid, char *name)
+static void pm_pinctrl_get_function_name(uint32_t fid, char *name)
 {
 	pm_api_pinctrl_get_function_name(fid, name);
 }
@@ -1431,8 +1453,8 @@
  *
  * Return: Returns status, either success or error+reason.
  */
-static enum pm_ret_status pm_pinctrl_get_function_groups(unsigned int fid,
-							 unsigned int index,
+static enum pm_ret_status pm_pinctrl_get_function_groups(uint32_t fid,
+							 uint32_t index,
 							 uint16_t *groups)
 {
 	return pm_api_pinctrl_get_function_groups(fid, index, groups);
@@ -1455,8 +1477,8 @@
  *
  * Return: Returns status, either success or error+reason.
  */
-static enum pm_ret_status pm_pinctrl_get_pin_groups(unsigned int pin_id,
-						    unsigned int index,
+static enum pm_ret_status pm_pinctrl_get_pin_groups(uint32_t pin_id,
+						    uint32_t index,
 						    uint16_t *groups)
 {
 	return pm_api_pinctrl_get_pin_groups(pin_id, index, groups);
@@ -1472,8 +1494,8 @@
  *
  * This function returns requested data.
  */
-void pm_query_data(enum pm_query_id qid, unsigned int arg1, unsigned int arg2,
-		   unsigned int arg3, unsigned int *data)
+void pm_query_data(enum pm_query_id qid, uint32_t arg1, uint32_t arg2,
+		   uint32_t arg3, uint32_t *data)
 {
 	switch (qid) {
 	case PM_QID_CLOCK_GET_NAME:
@@ -1522,6 +1544,7 @@
 	default:
 		data[0] = PM_RET_ERROR_ARGS;
 		WARN("Unimplemented query service call: 0x%x\n", qid);
+		break;
 	}
 }
 
@@ -1609,17 +1632,19 @@
  */
 enum pm_ret_status pm_pll_set_parameter(enum pm_node_id nid,
 					enum pm_pll_param param_id,
-					unsigned int value)
+					uint32_t value)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
 	/* Check if given node ID is a PLL node */
-	if (nid < NODE_APLL || nid > NODE_IOPLL)
+	if (nid < NODE_APLL || nid > NODE_IOPLL) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Check if parameter ID is valid and return an error if it's not */
-	if (param_id >= PM_PLL_PARAM_MAX)
+	if (param_id >= PM_PLL_PARAM_MAX) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD4(payload, PM_PLL_SET_PARAMETER, nid, param_id, value);
@@ -1637,17 +1662,19 @@
  */
 enum pm_ret_status pm_pll_get_parameter(enum pm_node_id nid,
 					enum pm_pll_param param_id,
-					unsigned int *value)
+					uint32_t *value)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
 	/* Check if given node ID is a PLL node */
-	if (nid < NODE_APLL || nid > NODE_IOPLL)
+	if (nid < NODE_APLL || nid > NODE_IOPLL) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Check if parameter ID is valid and return an error if it's not */
-	if (param_id >= PM_PLL_PARAM_MAX)
+	if (param_id >= PM_PLL_PARAM_MAX) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD3(payload, PM_PLL_GET_PARAMETER, nid, param_id);
@@ -1672,12 +1699,14 @@
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
 	/* Check if given node ID is a PLL node */
-	if (nid < NODE_APLL || nid > NODE_IOPLL)
+	if (nid < NODE_APLL || nid > NODE_IOPLL) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Check if PLL mode is valid */
-	if (mode >= PM_PLL_MODE_MAX)
+	if (mode >= PM_PLL_MODE_MAX) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD3(payload, PM_PLL_SET_MODE, nid, mode);
@@ -1697,8 +1726,9 @@
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
 	/* Check if given node ID is a PLL node */
-	if (nid < NODE_APLL || nid > NODE_IOPLL)
+	if (nid < NODE_APLL || nid > NODE_IOPLL) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD2(payload, PM_PLL_GET_MODE, nid);
@@ -1722,19 +1752,20 @@
  *
  * @return	Returns status, either success or error+reason
  */
-enum pm_ret_status pm_register_access(unsigned int register_access_id,
-				      unsigned int address,
-				      unsigned int mask,
-				      unsigned int value,
-				      unsigned int *out)
+enum pm_ret_status pm_register_access(uint32_t register_access_id,
+				      uint32_t address,
+				      uint32_t mask,
+				      uint32_t value,
+				      uint32_t *out)
 {
 	enum pm_ret_status ret;
 
 	if (((ZYNQMP_CSU_BASEADDR & address) != ZYNQMP_CSU_BASEADDR) &&
 			((CSUDMA_BASE & address) != CSUDMA_BASE) &&
 			((RSA_CORE_BASE & address) != RSA_CORE_BASE) &&
-			((PMU_GLOBAL_BASE & address) != PMU_GLOBAL_BASE))
+			((PMU_GLOBAL_BASE & address) != PMU_GLOBAL_BASE)) {
 		return PM_RET_ERROR_ACCESS;
+	}
 
 	switch (register_access_id) {
 	case CONFIG_REG_WRITE:
@@ -1746,6 +1777,7 @@
 	default:
 		ret = PM_RET_ERROR_ARGS;
 		WARN("Unimplemented register_access call\n\r");
+		break;
 	}
 	return ret;
 }
@@ -1776,7 +1808,7 @@
 	return pm_ipi_send_sync(primary_proc, payload, value, 1);
 }
 
-enum pm_ret_status em_set_action(unsigned int *value)
+enum pm_ret_status em_set_action(uint32_t *value)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
@@ -1785,7 +1817,7 @@
 	return pm_ipi_send_sync(primary_proc, payload, value, 1);
 }
 
-enum pm_ret_status em_remove_action(unsigned int *value)
+enum pm_ret_status em_remove_action(uint32_t *value)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
@@ -1794,7 +1826,7 @@
 	return pm_ipi_send_sync(primary_proc, payload, value, 1);
 }
 
-enum pm_ret_status em_send_errors(unsigned int *value)
+enum pm_ret_status em_send_errors(uint32_t *value)
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
 

diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_sys.h b/plat/xilinx/zynqmp/pm_service/pm_api_sys.h
index 48b3877..d3e9a34 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_sys.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_sys.h

@@ -68,100 +68,89 @@
 /**********************************************************
  * System-level API function declarations
  **********************************************************/
-enum pm_ret_status pm_req_suspend(enum pm_node_id nid,
+enum pm_ret_status pm_req_suspend(enum pm_node_id target,
 				  enum pm_request_ack ack,
-				  unsigned int latency,
-				  unsigned int state);
+				  uint32_t latency,
+				  uint32_t state);
 
 enum pm_ret_status pm_self_suspend(enum pm_node_id nid,
-				   unsigned int latency,
-				   unsigned int state,
+				   uint32_t latency,
+				   uint32_t state,
 				   uintptr_t address);
 
-enum pm_ret_status pm_force_powerdown(enum pm_node_id nid,
+enum pm_ret_status pm_force_powerdown(enum pm_node_id target,
 				      enum pm_request_ack ack);
 
 enum pm_ret_status pm_abort_suspend(enum pm_abort_reason reason);
 
-enum pm_ret_status pm_req_wakeup(enum pm_node_id nid,
-				 unsigned int set_address,
+enum pm_ret_status pm_req_wakeup(enum pm_node_id target,
+				 uint32_t set_address,
 				 uintptr_t address,
 				 enum pm_request_ack ack);
 
 enum pm_ret_status pm_set_wakeup_source(enum pm_node_id target,
 					enum pm_node_id wkup_node,
-					unsigned int enable);
+					uint32_t enable);
 
-enum pm_ret_status pm_system_shutdown(unsigned int type, unsigned int subtype);
-
-enum pm_ret_status pm_init_suspend_cb(enum pm_suspend_reason reason,
-				      unsigned int latency,
-				      unsigned int state,
-				      unsigned int timeout);
+enum pm_ret_status pm_system_shutdown(uint32_t type, uint32_t subtype);
 
 /* API functions for managing PM Slaves */
 enum pm_ret_status pm_req_node(enum pm_node_id nid,
-			       unsigned int capabilities,
-			       unsigned int qos,
+			       uint32_t capabilities,
+			       uint32_t qos,
 			       enum pm_request_ack ack);
 
 enum pm_ret_status pm_set_requirement(enum pm_node_id nid,
-				      unsigned int capabilities,
-				      unsigned int qos,
+				      uint32_t capabilities,
+				      uint32_t qos,
 				      enum pm_request_ack ack);
 
 /* Miscellaneous API functions */
-enum pm_ret_status pm_get_api_version(unsigned int *version);
-enum pm_ret_status pm_get_node_status(enum pm_node_id node,
+enum pm_ret_status pm_get_api_version(uint32_t *version);
+enum pm_ret_status pm_get_node_status(enum pm_node_id nid,
 				      uint32_t *ret_buff);
-enum pm_ret_status pm_acknowledge_cb(enum pm_node_id nid,
-				     enum pm_ret_status status,
-				     unsigned int oppoint);
-enum pm_ret_status pm_notify_cb(enum pm_node_id nid,
-				unsigned int event,
-				unsigned int oppoint);
 
 /* Direct-Control API functions */
 enum pm_ret_status pm_mmio_write(uintptr_t address,
-				 unsigned int mask,
-				 unsigned int value);
-enum pm_ret_status pm_mmio_read(uintptr_t address, unsigned int *value);
+				 uint32_t mask,
+				 uint32_t value);
+enum pm_ret_status pm_mmio_read(uintptr_t address, uint32_t *value);
 enum pm_ret_status pm_fpga_load(uint32_t address_low,
 				uint32_t address_high,
 				uint32_t size,
 				uint32_t flags);
-enum pm_ret_status pm_fpga_get_status(unsigned int *value);
+enum pm_ret_status pm_fpga_get_status(uint32_t *value);
 
 enum pm_ret_status pm_get_chipid(uint32_t *value);
-enum pm_ret_status pm_secure_rsaaes(uint32_t address_high,
-				    uint32_t address_low,
+enum pm_ret_status pm_secure_rsaaes(uint32_t address_low,
+				    uint32_t address_high,
 				    uint32_t size,
 				    uint32_t flags);
 unsigned int pm_get_shutdown_scope(void);
 void pm_get_callbackdata(uint32_t *data, size_t count);
 enum pm_ret_status pm_ioctl(enum pm_node_id nid,
-			    unsigned int ioctl_id,
-			    unsigned int arg1,
-			    unsigned int arg2,
-			    unsigned int *value);
-enum pm_ret_status pm_clock_enable(unsigned int clock_id);
-enum pm_ret_status pm_clock_disable(unsigned int clock_id);
-enum pm_ret_status pm_clock_getstate(unsigned int clock_id,
-				     unsigned int *state);
-enum pm_ret_status pm_clock_setdivider(unsigned int clock_id,
-				       unsigned int divider);
-enum pm_ret_status pm_clock_getdivider(unsigned int clock_id,
-				       unsigned int *divider);
-enum pm_ret_status pm_clock_setrate(unsigned int clock_id,
+			    uint32_t ioctl_id,
+			    uint32_t arg1,
+			    uint32_t arg2,
+			    uint32_t *value);
+enum pm_ret_status pm_clock_enable(uint32_t clock_id);
+enum pm_ret_status pm_clock_disable(uint32_t clock_id);
+enum pm_ret_status pm_clock_getstate(uint32_t clock_id,
+				     uint32_t *state);
+enum pm_ret_status pm_clock_setdivider(uint32_t clock_id,
+				       uint32_t divider);
+enum pm_ret_status pm_clock_getdivider(uint32_t clock_id,
+				       uint32_t *divider);
+enum pm_ret_status pm_clock_setrate(uint32_t clock_id,
 				    uint64_t rate);
-enum pm_ret_status pm_clock_getrate(unsigned int clock_id,
+enum pm_ret_status pm_clock_getrate(uint32_t clock_id,
 				    uint64_t *rate);
-enum pm_ret_status pm_clock_setparent(unsigned int clock_id,
-				      unsigned int parent_id);
-enum pm_ret_status pm_clock_getparent(unsigned int clock_id,
-				      unsigned int *parent_id);
-void pm_query_data(enum pm_query_id qid, unsigned int arg1, unsigned int arg2,
-		   unsigned int arg3, unsigned int *data);
+enum pm_ret_status pm_clock_setparent(uint32_t clock_id,
+				      uint32_t parent_index);
+enum pm_ret_status pm_clock_getparent(uint32_t clock_id,
+				      uint32_t *parent_index);
+void pm_query_data(enum pm_query_id qid, uint32_t arg1, uint32_t arg2,
+		   uint32_t arg3, uint32_t *data);
 enum pm_ret_status pm_sha_hash(uint32_t address_high,
 				    uint32_t address_low,
 				    uint32_t size,
@@ -183,28 +172,24 @@
 enum pm_ret_status pm_aes_engine(uint32_t address_high,
 				 uint32_t address_low,
 				 uint32_t  *value);
-enum pm_ret_status pm_register_access(unsigned int register_access_id,
-				      unsigned int address,
-				      unsigned int mask,
-				      unsigned int value,
-				      unsigned int *out);
+enum pm_ret_status pm_register_access(uint32_t register_access_id,
+				      uint32_t address,
+				      uint32_t mask,
+				      uint32_t value,
+				      uint32_t *out);
 enum pm_ret_status pm_pll_set_parameter(enum pm_node_id nid,
 					enum pm_pll_param param_id,
-					unsigned int value);
+					uint32_t value);
 enum pm_ret_status pm_pll_get_parameter(enum pm_node_id nid,
 					enum pm_pll_param param_id,
-					unsigned int *value);
+					uint32_t *value);
 enum pm_ret_status pm_pll_set_mode(enum pm_node_id nid, enum pm_pll_mode mode);
 enum pm_ret_status pm_pll_get_mode(enum pm_node_id nid, enum pm_pll_mode *mode);
 enum pm_ret_status pm_efuse_access(uint32_t address_high,
 				   uint32_t address_low, uint32_t *value);
-enum pm_ret_status em_set_action(unsigned int *value);
-enum pm_ret_status em_remove_action(unsigned int *value);
-enum pm_ret_status em_send_errors(unsigned int *value);
-enum pm_ret_status pm_feature_config(unsigned int ioctl_id,
-				     unsigned int config_id,
-				     unsigned int value,
-				     unsigned int *response);
+enum pm_ret_status em_set_action(uint32_t *value);
+enum pm_ret_status em_remove_action(uint32_t *value);
+enum pm_ret_status em_send_errors(uint32_t *value);
 enum pm_ret_status pm_feature_check(uint32_t api_id, uint32_t *version,
 				    uint32_t *bit_mask, uint8_t len);
 enum pm_ret_status check_api_dependency(uint8_t id);

diff --git a/plat/xilinx/zynqmp/pm_service/pm_client.c b/plat/xilinx/zynqmp/pm_service/pm_client.c
index 163e891..34b931e 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_client.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_client.c

@@ -163,7 +163,7 @@
  *
  * Return:	PM node ID corresponding to the specified interrupt
  */
-static enum pm_node_id irq_to_pm_node(unsigned int irq)
+static enum pm_node_id irq_to_pm_node(uint32_t irq)
 {
 	assert(irq <= IRQ_MAX);
 	return irq_node_map[irq];
@@ -188,8 +188,9 @@
 		 * If NODE_EXTERN could not be set as wake source, proceed with
 		 * standard suspend (no one will wake the system otherwise)
 		 */
-		if (ret == PM_RET_SUCCESS)
+		if (ret == PM_RET_SUCCESS) {
 			return;
+		}
 	}
 
 	zeromem(&pm_wakeup_nodes_set, sizeof(pm_wakeup_nodes_set));
@@ -198,8 +199,9 @@
 		uint32_t base_irq = reg_num << ISENABLER_SHIFT;
 		uint32_t reg = mmio_read_32(isenabler1 + (reg_num << 2));
 
-		if (!reg)
+		if (!reg) {
 			continue;
+		}
 
 		while (reg) {
 			enum pm_node_id node;
@@ -208,16 +210,18 @@
 			idx = __builtin_ctz(lowest_set);
 			irq = base_irq + idx;
 
-			if (irq > IRQ_MAX)
+			if (irq > IRQ_MAX) {
 				break;
+			}
 
 			node = irq_to_pm_node(irq);
 			reg &= ~lowest_set;
 
-			if ((node != NODE_UNKNOWN) &&
-			    (!pm_wakeup_nodes_set[node])) {
-				ret = pm_set_wakeup_source(NODE_APU, node, 1);
-				pm_wakeup_nodes_set[node] = !ret;
+			if (node > NODE_UNKNOWN && node < NODE_MAX) {
+				if (pm_wakeup_nodes_set[node] == 0U) {
+					ret = pm_set_wakeup_source(NODE_APU, node, 1U);
+					pm_wakeup_nodes_set[node] = (ret == PM_RET_SUCCESS) ? 1U : 0U;
+				}
 			}
 		}
 	}
@@ -229,10 +233,11 @@
  *
  * Return: pointer to a proc structure if proc is found, otherwise NULL
  */
-const struct pm_proc *pm_get_proc(unsigned int cpuid)
+const struct pm_proc *pm_get_proc(uint32_t cpuid)
 {
-	if (cpuid < ARRAY_SIZE(pm_procs_all))
+	if (cpuid < ARRAY_SIZE(pm_procs_all)) {
 		return &pm_procs_all[cpuid];
+	}
 
 	return NULL;
 }
@@ -246,8 +251,9 @@
 const struct pm_proc *pm_get_proc_by_node(enum pm_node_id nid)
 {
 	for (size_t i = 0; i < ARRAY_SIZE(pm_procs_all); i++) {
-		if (nid == pm_procs_all[i].node_id)
+		if (nid == pm_procs_all[i].node_id) {
 			return &pm_procs_all[i];
+		}
 	}
 	return NULL;
 }
@@ -258,11 +264,12 @@
  *
  * Return: the cpu ID (starting from 0) for the subsystem
  */
-static unsigned int pm_get_cpuid(enum pm_node_id nid)
+static uint32_t pm_get_cpuid(enum pm_node_id nid)
 {
 	for (size_t i = 0; i < ARRAY_SIZE(pm_procs_all); i++) {
-		if (pm_procs_all[i].node_id == nid)
+		if (pm_procs_all[i].node_id == nid) {
 			return i;
+		}
 	}
 	return UNDEFINED_CPUID;
 }
@@ -276,12 +283,13 @@
  * required prior to sending suspend request to PMU
  * Actions taken depend on the state system is suspending to.
  */
-void pm_client_suspend(const struct pm_proc *proc, unsigned int state)
+void pm_client_suspend(const struct pm_proc *proc, uint32_t state)
 {
 	bakery_lock_get(&pm_client_secure_lock);
 
-	if (state == PM_STATE_SUSPEND_TO_RAM)
+	if (state == PM_STATE_SUSPEND_TO_RAM) {
 		pm_client_set_wakeup_sources();
+	}
 
 	/* Set powerdown request */
 	mmio_write_32(APU_PWRCTL, mmio_read_32(APU_PWRCTL) | proc->pwrdn_mask);
@@ -318,10 +326,11 @@
  */
 void pm_client_wakeup(const struct pm_proc *proc)
 {
-	unsigned int cpuid = pm_get_cpuid(proc->node_id);
+	uint32_t cpuid = pm_get_cpuid(proc->node_id);
 
-	if (cpuid == UNDEFINED_CPUID)
+	if (cpuid == UNDEFINED_CPUID) {
 		return;
+	}
 
 	bakery_lock_get(&pm_client_secure_lock);
 
@@ -336,8 +345,9 @@
 enum pm_ret_status pm_set_suspend_mode(uint32_t mode)
 {
 	if ((mode != PM_SUSPEND_MODE_STD) &&
-	    (mode != PM_SUSPEND_MODE_POWER_OFF))
+	    (mode != PM_SUSPEND_MODE_POWER_OFF)) {
 		return PM_RET_ERROR_ARGS;
+	}
 
 	suspend_mode = mode;
 	return PM_RET_SUCCESS;

diff --git a/plat/xilinx/zynqmp/pm_service/pm_defs.h b/plat/xilinx/zynqmp/pm_service/pm_defs.h
index 8eb197a..d48df55 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_defs.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_defs.h

@@ -17,10 +17,10 @@
  * Version number is a 32bit value, like:
  * (PM_VERSION_MAJOR << 16) | PM_VERSION_MINOR
  */
-#define PM_VERSION_MAJOR	1
-#define PM_VERSION_MINOR	1
+#define PM_VERSION_MAJOR	1U
+#define PM_VERSION_MINOR	1U
 
-#define PM_VERSION	((PM_VERSION_MAJOR << 16) | PM_VERSION_MINOR)
+#define PM_VERSION	((PM_VERSION_MAJOR << 16U) | PM_VERSION_MINOR)
 
 /**
  * PM API versions

diff --git a/plat/xilinx/zynqmp/pm_service/pm_svc_main.c b/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
index d88e5fa..a136ebc 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_svc_main.c

@@ -203,9 +203,9 @@
  * Called from sip_svc_setup initialization function with the
  * rt_svc_init signature.
  */
-int pm_setup(void)
+int32_t pm_setup(void)
 {
-	int status, ret;
+	int32_t status, ret;
 
 	status = pm_ipi_init(primary_proc);
 
@@ -214,7 +214,7 @@
 		ERROR("BL31: Platform Management API version error. Expected: "
 		      "v%d.%d - Found: v%d.%d\n", PM_VERSION_MAJOR,
 		      PM_VERSION_MINOR, pm_ctx.api_version >> 16,
-		      pm_ctx.api_version & 0xFFFF);
+		      pm_ctx.api_version & 0xFFFFU);
 		return -EINVAL;
 	}
 
@@ -255,12 +255,12 @@
  * function with rt_svc_handle signature
  */
 uint64_t pm_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3,
-			uint64_t x4, void *cookie, void *handle, uint64_t flags)
+			uint64_t x4, const void *cookie, void *handle, uint64_t flags)
 {
 	enum pm_ret_status ret;
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
-	uint32_t pm_arg[4];
+	uint32_t pm_arg[5];
 	uint32_t result[PAYLOAD_ARG_CNT];
 	uint32_t api_id;
 
@@ -291,7 +291,7 @@
 	case PM_REQ_WAKEUP:
 	{
 		/* Use address flag is encoded in the 1st bit of the low-word */
-		unsigned int set_addr = pm_arg[1] & 0x1;
+		uint32_t set_addr = pm_arg[1] & 0x1;
 		uint64_t address = (uint64_t)pm_arg[2] << 32;
 
 		address |= pm_arg[1] & (~0x1);
@@ -419,7 +419,7 @@
 
 	case PM_CLOCK_GETRATE:
 	{
-		uint64_t value;
+		uint64_t value = 0;
 
 		ret = pm_clock_getrate(pm_arg[0], &value);
 		SMC_RET2(handle, (uint64_t)ret |
@@ -538,7 +538,7 @@
 
 	case PM_FEATURE_CHECK:
 	{
-		uint32_t version;
+		uint32_t version = 0;
 		uint32_t bit_mask[2] = {0};
 
 		ret = pm_feature_check(pm_arg[0], &version, bit_mask,
@@ -575,7 +575,7 @@
  * function with rt_svc_handle signature
  */
 uint64_t em_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3,
-			uint64_t x4, void *cookie, void *handle, uint64_t flags)
+			uint64_t x4, const void *cookie, void *handle, uint64_t flags)
 {
 	enum pm_ret_status ret;
 

diff --git a/plat/xilinx/zynqmp/pm_service/pm_svc_main.h b/plat/xilinx/zynqmp/pm_service/pm_svc_main.h
index abadd40..c1781f3 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_svc_main.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_svc_main.h

@@ -9,12 +9,12 @@
 
 #include "pm_common.h"
 
-int pm_setup(void);
+int32_t pm_setup(void);
 uint64_t pm_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3,
-			uint64_t x4, void *cookie, void *handle,
+			uint64_t x4, const void *cookie, void *handle,
 			uint64_t flags);
 
 uint64_t em_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3,
-			uint64_t x4, void *cookie, void *handle,
+			uint64_t x4, const void *cookie, void *handle,
 			uint64_t flags);
 #endif /* PM_SVC_MAIN_H */

diff --git a/plat/xilinx/zynqmp/sip_svc_setup.c b/plat/xilinx/zynqmp/sip_svc_setup.c
index 114da33..4ce9b8a 100644
--- a/plat/xilinx/zynqmp/sip_svc_setup.c
+++ b/plat/xilinx/zynqmp/sip_svc_setup.c

@@ -13,9 +13,9 @@
 #include "pm_svc_main.h"
 
 /* SMC function IDs for SiP Service queries */
-#define ZYNQMP_SIP_SVC_CALL_COUNT	0x8200ff00
-#define ZYNQMP_SIP_SVC_UID		0x8200ff01
-#define ZYNQMP_SIP_SVC_VERSION		0x8200ff03
+#define ZYNQMP_SIP_SVC_CALL_COUNT	U(0x8200ff00)
+#define ZYNQMP_SIP_SVC_UID		U(0x8200ff01)
+#define ZYNQMP_SIP_SVC_VERSION		U(0x8200ff03)
 
 /* SiP Service Calls version numbers */
 #define SIP_SVC_VERSION_MAJOR	0
@@ -53,7 +53,7 @@
  * Handler for all SiP SMC calls. Handles standard SIP requests
  * and calls PM SMC handler if the call is for a PM-API function.
  */
-uintptr_t sip_svc_smc_handler(uint32_t smc_fid,
+static uintptr_t sip_svc_smc_handler(uint32_t smc_fid,
 			      u_register_t x1,
 			      u_register_t x2,
 			      u_register_t x3,
@@ -100,6 +100,6 @@
 		sip_svc,
 		OEN_SIP_START,
 		OEN_SIP_END,
-		SMC_TYPE_FAST,
+		(uint8_t)SMC_TYPE_FAST,
 		sip_svc_setup,
 		sip_svc_smc_handler);

diff --git a/plat/xilinx/zynqmp/zynqmp_ipi.c b/plat/xilinx/zynqmp/zynqmp_ipi.c
index f57369f..4ea3c6a 100644
--- a/plat/xilinx/zynqmp/zynqmp_ipi.c
+++ b/plat/xilinx/zynqmp/zynqmp_ipi.c

@@ -25,67 +25,67 @@
 	/* APU IPI */
 	{
 		.ipi_bit_mask = 0x1,
-		.ipi_reg_base = 0xFF300000,
+		.ipi_reg_base = 0xFF300000U,
 		.secure_only = 0,
 	},
 	/* RPU0 IPI */
 	{
 		.ipi_bit_mask = 0x100,
-		.ipi_reg_base = 0xFF310000,
+		.ipi_reg_base = 0xFF310000U,
 		.secure_only = 0,
 	},
 	/* RPU1 IPI */
 	{
 		.ipi_bit_mask = 0x200,
-		.ipi_reg_base = 0xFF320000,
+		.ipi_reg_base = 0xFF320000U,
 		.secure_only = 0,
 	},
 	/* PMU0 IPI */
 	{
 		.ipi_bit_mask = 0x10000,
-		.ipi_reg_base = 0xFF330000,
+		.ipi_reg_base = 0xFF330000U,
 		.secure_only = IPI_SECURE_MASK,
 	},
 	/* PMU1 IPI */
 	{
 		.ipi_bit_mask = 0x20000,
-		.ipi_reg_base = 0xFF331000,
+		.ipi_reg_base = 0xFF331000U,
 		.secure_only = 0,
 	},
 	/* PMU2 IPI */
 	{
 		.ipi_bit_mask = 0x40000,
-		.ipi_reg_base = 0xFF332000,
+		.ipi_reg_base = 0xFF332000U,
 		.secure_only = IPI_SECURE_MASK,
 	},
 	/* PMU3 IPI */
 	{
 		.ipi_bit_mask = 0x80000,
-		.ipi_reg_base = 0xFF333000,
+		.ipi_reg_base = 0xFF333000U,
 		.secure_only = IPI_SECURE_MASK,
 	},
 	/* PL0 IPI */
 	{
 		.ipi_bit_mask = 0x1000000,
-		.ipi_reg_base = 0xFF340000,
+		.ipi_reg_base = 0xFF340000U,
 		.secure_only = 0,
 	},
 	/* PL1 IPI */
 	{
 		.ipi_bit_mask = 0x2000000,
-		.ipi_reg_base = 0xFF350000,
+		.ipi_reg_base = 0xFF350000U,
 		.secure_only = 0,
 	},
 	/* PL2 IPI */
 	{
 		.ipi_bit_mask = 0x4000000,
-		.ipi_reg_base = 0xFF360000,
+		.ipi_reg_base = 0xFF360000U,
 		.secure_only = 0,
 	},
 	/* PL3 IPI */
 	{
 		.ipi_bit_mask = 0x8000000,
-		.ipi_reg_base = 0xFF370000,
+		.ipi_reg_base = 0xFF370000U,
 		.secure_only = 0,
 	},
 };

diff --git a/services/std_svc/rmmd/rmmd_attest.c b/services/std_svc/rmmd/rmmd_attest.c
index 0432ec3..25adf50 100644
--- a/services/std_svc/rmmd/rmmd_attest.c
+++ b/services/std_svc/rmmd/rmmd_attest.c

@@ -5,6 +5,7 @@
  */
 #include <stdint.h>
 #include <string.h>
+
 #include <common/debug.h>
 #include <lib/spinlock.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
@@ -56,110 +57,96 @@
 }
 
 /*
- * TODO: Have different error codes for different errors so that the caller can
- * differentiate various error cases.
+ * Helper function to validate that the buffer base and length are
+ * within range.
  */
-int rmmd_attest_get_platform_token(uint64_t buf_pa, uint64_t *buf_len, uint64_t challenge_hash_len)
+static int validate_buffer_params(uint64_t buf_pa, uint64_t buf_len)
 {
-	int err;
-	uintptr_t va;
-	uint8_t temp_buf[SHA512_DIGEST_SIZE];
+	unsigned long shared_buf_page;
+	uintptr_t shared_buf_base;
 
-	/*
-	 * TODO: Currently we don't validate incoming buf_pa. This is a
-	 * prototype and we will need to allocate static buffer for EL3-RMM
-	 * communication.
-	 */
+	(void)plat_rmmd_get_el3_rmm_shared_mem(&shared_buf_base);
 
-	/* We need a page of buffer to pass data */
-	if (*buf_len != PAGE_SIZE) {
-		ERROR("Invalid buffer length\n");
-		return RMMD_ERR_INVAL;
+	shared_buf_page = shared_buf_base & ~PAGE_SIZE_MASK;
+
+	/* Validate the buffer pointer */
+	if ((buf_pa & ~PAGE_SIZE_MASK) != shared_buf_page) {
+		ERROR("Buffer PA out of range\n");
+		return E_RMM_BAD_ADDR;
 	}
 
-	if ((challenge_hash_len != SHA256_DIGEST_SIZE) &&
-	    (challenge_hash_len != SHA384_DIGEST_SIZE) &&
-	    (challenge_hash_len != SHA512_DIGEST_SIZE)) {
-		ERROR("Invalid hash size: %lu\n", challenge_hash_len);
-		return RMMD_ERR_INVAL;
+	/* Validate the size of the shared area */
+	if (((buf_pa + buf_len - 1UL) & ~PAGE_SIZE_MASK) != shared_buf_page) {
+		ERROR("Invalid buffer length\n");
+		return E_RMM_INVAL;
 	}
 
-	spin_lock(&lock);
+	return 0; /* No error */
+}
+
+int rmmd_attest_get_platform_token(uint64_t buf_pa, uint64_t *buf_size,
+				   uint64_t c_size)
+{
+	int err;
+	uint8_t temp_buf[SHA512_DIGEST_SIZE];
 
-	/* Map the buffer that was provided by the RMM. */
-	err = mmap_add_dynamic_region_alloc_va(buf_pa, &va, PAGE_SIZE,
-					       MT_RW_DATA | MT_REALM);
+	err = validate_buffer_params(buf_pa, *buf_size);
 	if (err != 0) {
-		ERROR("mmap_add_dynamic_region_alloc_va failed: %d (%p).\n"
-		      , err, (void *)buf_pa);
-		spin_unlock(&lock);
-		return RMMD_ERR_NOMEM;
+		return err;
+	}
+
+	if ((c_size != SHA256_DIGEST_SIZE) &&
+	    (c_size != SHA384_DIGEST_SIZE) &&
+	    (c_size != SHA512_DIGEST_SIZE)) {
+		ERROR("Invalid hash size: %lu\n", c_size);
+		return E_RMM_INVAL;
 	}
 
-	(void)memcpy(temp_buf, (void *)va, challenge_hash_len);
+	spin_lock(&lock);
 
-	print_challenge((uint8_t *)temp_buf, challenge_hash_len);
+	(void)memcpy(temp_buf, (void *)buf_pa, c_size);
+
+	print_challenge((uint8_t *)temp_buf, c_size);
 
 	/* Get the platform token. */
-	err = plat_get_cca_attest_token(va,
-		buf_len, (uintptr_t)temp_buf, challenge_hash_len);
+	err = plat_rmmd_get_cca_attest_token((uintptr_t)buf_pa,
+		buf_size, (uintptr_t)temp_buf, c_size);
 
 	if (err != 0) {
 		ERROR("Failed to get platform token: %d.\n", err);
-		err = RMMD_ERR_UNK;
+		err = E_RMM_UNK;
 	}
 
-	/* Unmap RMM memory. */
-	(void)mmap_remove_dynamic_region(va, PAGE_SIZE);
 	spin_unlock(&lock);
 
 	return err;
 }
 
-int rmmd_attest_get_signing_key(uint64_t buf_pa, uint64_t *buf_len,
+int rmmd_attest_get_signing_key(uint64_t buf_pa, uint64_t *buf_size,
 				uint64_t ecc_curve)
 {
 	int err;
-	uintptr_t va;
 
-	/*
-	 * TODO: Currently we don't validate incoming buf_pa. This is a
-	 * prototype and we will need to allocate static buffer for EL3-RMM
-	 * communication.
-	 */
-
-	/* We need a page of buffer to pass data */
-	if (*buf_len != PAGE_SIZE) {
-		ERROR("Invalid buffer length\n");
-		return RMMD_ERR_INVAL;
+	err = validate_buffer_params(buf_pa, *buf_size);
+	if (err != 0) {
+		return err;
 	}
 
 	if (ecc_curve != ATTEST_KEY_CURVE_ECC_SECP384R1) {
 		ERROR("Invalid ECC curve specified\n");
-		return RMMD_ERR_INVAL;
+		return E_RMM_INVAL;
 	}
 
 	spin_lock(&lock);
 
-	/* Map the buffer that was provided by the RMM. */
-	err = mmap_add_dynamic_region_alloc_va(buf_pa, &va, PAGE_SIZE,
-					       MT_RW_DATA | MT_REALM);
-	if (err != 0) {
-		ERROR("mmap_add_dynamic_region_alloc_va failed: %d (%p).\n"
-		      , err, (void *)buf_pa);
-		spin_unlock(&lock);
-		return RMMD_ERR_NOMEM;
-	}
-
 	/* Get the Realm attestation key. */
-	err = plat_get_cca_realm_attest_key(va, buf_len, (unsigned int)ecc_curve);
+	err = plat_rmmd_get_cca_realm_attest_key((uintptr_t)buf_pa, buf_size,
+						 (unsigned int)ecc_curve);
 	if (err != 0) {
 		ERROR("Failed to get attestation key: %d.\n", err);
-		err =  RMMD_ERR_UNK;
+		err =  E_RMM_UNK;
 	}
 
-	/* Unmap RMM memory. */
-	(void)mmap_remove_dynamic_region(va, PAGE_SIZE);
 	spin_unlock(&lock);
 
 	return err;

diff --git a/services/std_svc/rmmd/rmmd_main.c b/services/std_svc/rmmd/rmmd_main.c
index 746419e..322d9f2 100644
--- a/services/std_svc/rmmd/rmmd_main.c
+++ b/services/std_svc/rmmd/rmmd_main.c

@@ -33,6 +33,11 @@
 #include "rmmd_private.h"
 
 /*******************************************************************************
+ * RMM boot failure flag
+ ******************************************************************************/
+static bool rmm_boot_failed;
+
+/*******************************************************************************
  * RMM context information.
  ******************************************************************************/
 rmmd_rmm_context_t rmm_context[PLATFORM_CORE_COUNT];
@@ -132,13 +137,10 @@
  ******************************************************************************/
 static int32_t rmm_init(void)
 {
-
-	uint64_t rc;
-
+	long rc;
 	rmmd_rmm_context_t *ctx = &rmm_context[plat_my_core_pos()];
 
 	INFO("RMM init start.\n");
-	ctx->state = RMM_STATE_RESET;
 
 	/* Enable architecture extensions */
 	manage_extensions_realm(&ctx->cpu_ctx);
@@ -147,12 +149,13 @@
 	rmm_el2_context_init(&ctx->cpu_ctx.el2_sysregs_ctx);
 
 	rc = rmmd_rmm_sync_entry(ctx);
-	if (rc != 0ULL) {
-		ERROR("RMM initialisation failed 0x%" PRIx64 "\n", rc);
-		panic();
+	if (rc != E_RMM_BOOT_SUCCESS) {
+		ERROR("RMM init failed: %ld\n", rc);
+		/* Mark the boot as failed for all the CPUs */
+		rmm_boot_failed = true;
+		return 0;
 	}
 
-	ctx->state = RMM_STATE_IDLE;
 	INFO("RMM init end.\n");
 
 	return 1;
@@ -163,9 +166,13 @@
  ******************************************************************************/
 int rmmd_setup(void)
 {
+	size_t shared_buf_size __unused;
+	uintptr_t shared_buf_base;
 	uint32_t ep_attr;
 	unsigned int linear_id = plat_my_core_pos();
 	rmmd_rmm_context_t *rmm_ctx = &rmm_context[linear_id];
+	rmm_manifest_t *manifest;
+	int rc;
 
 	/* Make sure RME is supported. */
 	assert(get_armv9_2_feat_rme_support() != 0U);
@@ -192,6 +199,34 @@
 					MODE_SP_ELX,
 					DISABLE_ALL_EXCEPTIONS);
 
+	shared_buf_size =
+			plat_rmmd_get_el3_rmm_shared_mem(&shared_buf_base);
+
+	assert((shared_buf_size == SZ_4K) &&
+					((void *)shared_buf_base != NULL));
+
+	/* Load the boot manifest at the beginning of the shared area */
+	manifest = (rmm_manifest_t *)shared_buf_base;
+	rc = plat_rmmd_load_manifest(manifest);
+	if (rc != 0) {
+		ERROR("Error loading RMM Boot Manifest (%i)\n", rc);
+		return rc;
+	}
+	flush_dcache_range((uintptr_t)shared_buf_base, shared_buf_size);
+
+	/*
+	 * Prepare coldboot arguments for RMM:
+	 * arg0: This CPUID (primary processor).
+	 * arg1: Version for this Boot Interface.
+	 * arg2: PLATFORM_CORE_COUNT.
+	 * arg3: Base address for the EL3 <-> RMM shared area. The boot
+	 *       manifest will be stored at the beginning of this area.
+	 */
+	rmm_ep_info->args.arg0 = linear_id;
+	rmm_ep_info->args.arg1 = RMM_EL3_INTERFACE_VERSION;
+	rmm_ep_info->args.arg2 = PLATFORM_CORE_COUNT;
+	rmm_ep_info->args.arg3 = shared_buf_base;
+
 	/* Initialise RMM context with this entry point information */
 	cm_setup_context(&rmm_ctx->cpu_ctx, rmm_ep_info);
 
@@ -244,9 +279,14 @@
 				uint64_t x3, uint64_t x4, void *cookie,
 				void *handle, uint64_t flags)
 {
-	rmmd_rmm_context_t *ctx = &rmm_context[plat_my_core_pos()];
 	uint32_t src_sec_state;
 
+	/* If RMM failed to boot, treat any RMI SMC as unknown */
+	if (rmm_boot_failed) {
+		WARN("RMMD: Failed to boot up RMM. Ignoring RMI call\n");
+		SMC_RET1(handle, SMC_UNK);
+	}
+
 	/* Determine which security state this SMC originated from */
 	src_sec_state = caller_sec_state(flags);
 
@@ -272,11 +312,6 @@
 
 	switch (smc_fid) {
 	case RMMD_RMI_REQ_COMPLETE:
-		if (ctx->state == RMM_STATE_RESET) {
-			VERBOSE("RMMD: running rmmd_rmm_sync_exit\n");
-			rmmd_rmm_sync_exit(x1);
-		}
-
 		return rmmd_smc_forward(REALM, NON_SECURE, x1,
 					x2, x3, x4, 0, handle);
 
@@ -293,11 +328,26 @@
  ******************************************************************************/
 static void *rmmd_cpu_on_finish_handler(const void *arg)
 {
-	int32_t rc;
+	long rc;
 	uint32_t linear_id = plat_my_core_pos();
 	rmmd_rmm_context_t *ctx = &rmm_context[linear_id];
 
-	ctx->state = RMM_STATE_RESET;
+	if (rmm_boot_failed) {
+		/* RMM Boot failed on a previous CPU. Abort. */
+		ERROR("RMM Failed to initialize. Ignoring for CPU%d\n",
+								linear_id);
+		return NULL;
+	}
+
+	/*
+	 * Prepare warmboot arguments for RMM:
+	 * arg0: This CPUID.
+	 * arg1 to arg3: Not used.
+	 */
+	rmm_ep_info->args.arg0 = linear_id;
+	rmm_ep_info->args.arg1 = 0ULL;
+	rmm_ep_info->args.arg2 = 0ULL;
+	rmm_ep_info->args.arg3 = 0ULL;
 
 	/* Initialise RMM context with this entry point information */
 	cm_setup_context(&ctx->cpu_ctx, rmm_ep_info);
@@ -309,13 +359,13 @@
 	rmm_el2_context_init(&ctx->cpu_ctx.el2_sysregs_ctx);
 
 	rc = rmmd_rmm_sync_entry(ctx);
-	if (rc != 0) {
-		ERROR("RMM initialisation failed (%d) on CPU%d\n", rc,
-		      linear_id);
-		panic();
+
+	if (rc != E_RMM_BOOT_SUCCESS) {
+		ERROR("RMM init failed on CPU%d: %ld\n", linear_id, rc);
+		/* Mark the boot as failed for any other booting CPU */
+		rmm_boot_failed = true;
 	}
 
-	ctx->state = RMM_STATE_IDLE;
 	return NULL;
 }
 
@@ -328,15 +378,15 @@
 	int ret;
 
 	if (error == 0) {
-		return RMMD_OK;
+		return E_RMM_OK;
 	}
 
 	if (error == -EINVAL) {
-		ret = RMMD_ERR_BAD_ADDR;
+		ret = E_RMM_BAD_ADDR;
 	} else {
 		/* This is the only other error code we expect */
 		assert(error == -EPERM);
-		ret = RMMD_ERR_BAD_PAS;
+		ret = E_RMM_BAD_PAS;
 	}
 
 	ERROR("RMMD: PAS Transition failed. GPT ret = %d, PA: 0x%"PRIx64 ", FID = 0x%x\n",
@@ -354,6 +404,12 @@
 	uint32_t src_sec_state;
 	int ret;
 
+	/* If RMM failed to boot, treat any RMM-EL3 interface SMC as unknown */
+	if (rmm_boot_failed) {
+		WARN("RMMD: Failed to boot up RMM. Ignoring RMM-EL3 call\n");
+		SMC_RET1(handle, SMC_UNK);
+	}
+
 	/* Determine which security state this SMC originated from */
 	src_sec_state = caller_sec_state(flags);
 
@@ -375,6 +431,11 @@
 	case RMMD_ATTEST_GET_REALM_KEY:
 		ret = rmmd_attest_get_signing_key(x1, &x2, x3);
 		SMC_RET2(handle, ret, x2);
+
+	case RMM_BOOT_COMPLETE:
+		VERBOSE("RMMD: running rmmd_rmm_sync_exit\n");
+		rmmd_rmm_sync_exit(x1);
+
 	default:
 		WARN("RMMD: Unsupported RMM-EL3 call 0x%08x\n", smc_fid);
 		SMC_RET1(handle, SMC_UNK);

diff --git a/services/std_svc/rmmd/rmmd_private.h b/services/std_svc/rmmd/rmmd_private.h
index 73df2b8..4954a43 100644
--- a/services/std_svc/rmmd/rmmd_private.h
+++ b/services/std_svc/rmmd/rmmd_private.h

@@ -32,11 +32,6 @@
 #ifndef __ASSEMBLER__
 #include <stdint.h>
 
-typedef enum rmm_state {
-	RMM_STATE_RESET = 0,
-	RMM_STATE_IDLE
-} rmm_state_t;
-
 /*
  * Data structure used by the RMM dispatcher (RMMD) in EL3 to track context of
  * the RMM at R-EL2.
@@ -44,7 +39,6 @@
 typedef struct rmmd_rmm_context {
 	uint64_t c_rt_ctx;
 	cpu_context_t cpu_ctx;
-	rmm_state_t state;
 } rmmd_rmm_context_t;
 
 /* Functions used to enter/exit the RMM synchronously */
@@ -52,10 +46,10 @@
 __dead2 void rmmd_rmm_sync_exit(uint64_t rc);
 
 /* Functions implementing attestation utilities for RMM */
-int rmmd_attest_get_platform_token(uint64_t buf_pa, uint64_t *buf_len,
-				   uint64_t challenge_hash_len);
-int rmmd_attest_get_signing_key(uint64_t buf_pa, uint64_t *buf_len,
-				   uint64_t ecc_curve);
+int rmmd_attest_get_platform_token(uint64_t buf_pa, uint64_t *buf_size,
+				   uint64_t c_size);
+int rmmd_attest_get_signing_key(uint64_t buf_pa, uint64_t *buf_size,
+				uint64_t ecc_curve);
 
 /* Assembly helpers */
 uint64_t rmmd_rmm_enter(uint64_t *c_rt_ctx);

diff --git a/services/std_svc/rmmd/trp/trp.mk b/services/std_svc/rmmd/trp/trp.mk
index a4f6e03..44bbf22 100644
--- a/services/std_svc/rmmd/trp/trp.mk
+++ b/services/std_svc/rmmd/trp/trp.mk

@@ -1,11 +1,12 @@
 #
-# Copyright (c) 2021 Arm Limited and Contributors. All rights reserved.
+# Copyright (c) 2021-2022 Arm Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
-RMM_SOURCES		+=	services/std_svc/rmmd/trp/trp_entry.S	\
-				services/std_svc/rmmd/trp/trp_main.c
+RMM_SOURCES		+=	services/std_svc/rmmd/trp/trp_entry.S \
+				services/std_svc/rmmd/trp/trp_main.c  \
+				services/std_svc/rmmd/trp/trp_helpers.c
 
 RMM_LINKERFILE		:=	services/std_svc/rmmd/trp/linker.lds
 

diff --git a/services/std_svc/rmmd/trp/trp_entry.S b/services/std_svc/rmmd/trp/trp_entry.S
index 1b03c9f..47c1df1 100644
--- a/services/std_svc/rmmd/trp/trp_entry.S
+++ b/services/std_svc/rmmd/trp/trp_entry.S

@@ -6,6 +6,8 @@
 
 #include <asm_macros.S>
 #include <services/rmmd_svc.h>
+
+#include <platform_def.h>
 #include "trp_private.h"
 
 .global trp_head
@@ -31,6 +33,28 @@
 	 * ---------------------------------------------
 	 */
 trp_head:
+	/*
+	 * Stash arguments from previous boot stage
+	 */
+	mov	x20, x0
+	mov	x21, x1
+	mov	x22, x2
+	mov	x23, x3
+
+	/*
+	 * Validate CPUId before allocating a stack.
+	 */
+	cmp	x20, #PLATFORM_CORE_COUNT
+	b.lo	1f
+
+	mov_imm	x0, RMM_BOOT_COMPLETE
+	mov_imm	x1, E_RMM_BOOT_CPU_ID_OUT_OF_RANGE
+	smc	#0
+
+	/* EL3 should never return back here, so panic if it does */
+	b	trp_panic
+
+1:
 	bl	plat_set_my_stack
 
 	/*
@@ -45,7 +69,6 @@
 	adr	x2, cold_boot_flag
 	str	xzr, [x2]
 
-
 	/* ---------------------------------------------
 	 * Zero out BSS section
 	 * ---------------------------------------------
@@ -54,15 +77,21 @@
 	ldr	x1, =__BSS_SIZE__
 	bl	zeromem
 
+	mov	x0, x20
+	mov	x1, x21
+	mov	x2, x22
+	mov	x3, x23
 	bl	trp_setup
-
 	bl	trp_main
 warm_boot:
-	mov_imm	x0, RMMD_RMI_REQ_COMPLETE
-	mov	x1, xzr
+	mov_imm	x0, RMM_BOOT_COMPLETE
+	mov	x1, xzr /* RMM_BOOT_SUCCESS */
 	smc	#0
 	b	trp_handler
 
+trp_panic:
+	no_ret plat_panic_handler
+
 	/*
 	 * Flag to mark if it is a cold boot.
 	 * 1: cold boot, 0: warmboot.

diff --git a/services/std_svc/rmmd/trp/trp_helpers.c b/services/std_svc/rmmd/trp/trp_helpers.c
new file mode 100644
index 0000000..159f3a5
--- /dev/null
+++ b/services/std_svc/rmmd/trp/trp_helpers.c

@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+
+#include <plat/common/platform.h>
+#include <services/rmmd_svc.h>
+#include "trp_private.h"
+
+/*
+ * Per cpu data structure to populate parameters for an SMC in C code and use
+ * a pointer to this structure in assembler code to populate x0-x7
+ */
+static trp_args_t trp_smc_args[PLATFORM_CORE_COUNT];
+
+/*
+ * Set the arguments for SMC call
+ */
+trp_args_t *set_smc_args(uint64_t arg0,
+			uint64_t arg1,
+			uint64_t arg2,
+			uint64_t arg3,
+			uint64_t arg4,
+			uint64_t arg5,
+			uint64_t arg6,
+			uint64_t arg7)
+{
+	uint32_t linear_id;
+	trp_args_t *pcpu_smc_args;
+
+	/*
+	 * Return to Secure Monitor by raising an SMC. The results of the
+	 * service are passed as an arguments to the SMC
+	 */
+	linear_id = plat_my_core_pos();
+	pcpu_smc_args = &trp_smc_args[linear_id];
+	write_trp_arg(pcpu_smc_args, TRP_ARG0, arg0);
+	write_trp_arg(pcpu_smc_args, TRP_ARG1, arg1);
+	write_trp_arg(pcpu_smc_args, TRP_ARG2, arg2);
+	write_trp_arg(pcpu_smc_args, TRP_ARG3, arg3);
+	write_trp_arg(pcpu_smc_args, TRP_ARG4, arg4);
+	write_trp_arg(pcpu_smc_args, TRP_ARG5, arg5);
+	write_trp_arg(pcpu_smc_args, TRP_ARG6, arg6);
+	write_trp_arg(pcpu_smc_args, TRP_ARG7, arg7);
+
+	return pcpu_smc_args;
+}
+
+/*
+ * Abort the boot process with the reason given in err.
+ */
+__dead2 void trp_boot_abort(uint64_t err)
+{
+	(void)trp_smc(set_smc_args(RMM_BOOT_COMPLETE, err, 0, 0, 0, 0, 0, 0));
+	panic();
+}

diff --git a/services/std_svc/rmmd/trp/trp_main.c b/services/std_svc/rmmd/trp/trp_main.c
index 2e3f076..cf6ec7b 100644
--- a/services/std_svc/rmmd/trp/trp_main.c
+++ b/services/std_svc/rmmd/trp/trp_main.c

@@ -7,58 +7,63 @@
 
 #include <common/debug.h>
 #include <plat/common/platform.h>
+#include <services/rmm_core_manifest.h>
 #include <services/rmmd_svc.h>
 #include <services/trp/platform_trp.h>
+#include <trp_helpers.h>
+#include "trp_private.h"
 
 #include <platform_def.h>
-#include "trp_private.h"
 
-/*******************************************************************************
- * Per cpu data structure to populate parameters for an SMC in C code and use
- * a pointer to this structure in assembler code to populate x0-x7
- ******************************************************************************/
-static trp_args_t trp_smc_args[PLATFORM_CORE_COUNT];
+/* Parameters received from the previous image */
+static unsigned int trp_boot_abi_version;
+static uintptr_t trp_shared_region_start;
+
+/* Parameters received from boot manifest */
+uint32_t trp_boot_manifest_version;
 
 /*******************************************************************************
- * Set the arguments for SMC call
+ * Setup function for TRP.
  ******************************************************************************/
-static trp_args_t *set_smc_args(uint64_t arg0,
-				uint64_t arg1,
-				uint64_t arg2,
-				uint64_t arg3,
-				uint64_t arg4,
-				uint64_t arg5,
-				uint64_t arg6,
-				uint64_t arg7)
+void trp_setup(uint64_t x0,
+	       uint64_t x1,
+	       uint64_t x2,
+	       uint64_t x3)
 {
-	uint32_t linear_id;
-	trp_args_t *pcpu_smc_args;
-
 	/*
-	 * Return to Secure Monitor by raising an SMC. The results of the
-	 * service are passed as an arguments to the SMC
+	 * 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)
+	 * x1: Boot Interface version
+	 * x2: PLATFORM_CORE_COUNT
+	 * x3: Pointer to the shared memory area.
 	 */
-	linear_id = plat_my_core_pos();
-	pcpu_smc_args = &trp_smc_args[linear_id];
-	write_trp_arg(pcpu_smc_args, TRP_ARG0, arg0);
-	write_trp_arg(pcpu_smc_args, TRP_ARG1, arg1);
-	write_trp_arg(pcpu_smc_args, TRP_ARG2, arg2);
-	write_trp_arg(pcpu_smc_args, TRP_ARG3, arg3);
-	write_trp_arg(pcpu_smc_args, TRP_ARG4, arg4);
-	write_trp_arg(pcpu_smc_args, TRP_ARG5, arg5);
-	write_trp_arg(pcpu_smc_args, TRP_ARG6, arg6);
-	write_trp_arg(pcpu_smc_args, TRP_ARG7, arg7);
 
-	return pcpu_smc_args;
-}
+	(void)x0;
 
-/*******************************************************************************
- * Setup function for TRP.
- ******************************************************************************/
-void trp_setup(void)
-{
+	if (TRP_RMM_EL3_VERSION_GET_MAJOR(x1) != TRP_RMM_EL3_ABI_VERS_MAJOR) {
+		trp_boot_abort(E_RMM_BOOT_VERSION_MISMATCH);
+	}
+
+	if ((void *)x3 == NULL) {
+		trp_boot_abort(E_RMM_BOOT_INVALID_SHARED_BUFFER);
+	}
+
+	if (x2 > TRP_PLATFORM_CORE_COUNT) {
+		trp_boot_abort(E_RMM_BOOT_CPUS_OUT_OF_RANGE);
+	}
+
+	trp_boot_abi_version = x1;
+	trp_shared_region_start = x3;
+	flush_dcache_range((uintptr_t)&trp_boot_abi_version,
+			   sizeof(trp_boot_abi_version));
+	flush_dcache_range((uintptr_t)&trp_shared_region_start,
+			   sizeof(trp_shared_region_start));
+
 	/* Perform early platform-specific setup */
-	trp_early_platform_setup();
+	trp_early_platform_setup((rmm_manifest_t *)trp_shared_region_start);
 }
 
 /* Main function for TRP */
@@ -66,9 +71,19 @@
 {
 	NOTICE("TRP: %s\n", version_string);
 	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",
+		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",
+			(unsigned long)trp_shared_region_start);
 	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));
 }
 
 /*******************************************************************************

diff --git a/services/std_svc/rmmd/trp/trp_private.h b/services/std_svc/rmmd/trp/trp_private.h
index 43a4a4b..945ae1c 100644
--- a/services/std_svc/rmmd/trp/trp_private.h
+++ b/services/std_svc/rmmd/trp/trp_private.h

@@ -7,48 +7,39 @@
 #ifndef TRP_PRIVATE_H
 #define TRP_PRIVATE_H
 
-/* Definitions to help the assembler access the SMC/ERET args structure */
-#define TRP_ARGS_SIZE		TRP_ARGS_END
-#define TRP_ARG0		0x0
-#define TRP_ARG1		0x8
-#define TRP_ARG2		0x10
-#define TRP_ARG3		0x18
-#define TRP_ARG4		0x20
-#define TRP_ARG5		0x28
-#define TRP_ARG6		0x30
-#define TRP_ARG7		0x38
-#define TRP_ARGS_END		0x40
+#include <services/rmmd_svc.h>
+#include <trp_helpers.h>
+
+/* Definitions for RMM-EL3 Interface ABI VERSION */
+#define TRP_RMM_EL3_ABI_VERS_MAJOR	RMM_EL3_IFC_VERSION_MAJOR
+#define TRP_RMM_EL3_ABI_VERS_MINOR	RMM_EL3_IFC_VERSION_MINOR
+#define TRP_RMM_EL3_ABI_VERS	(((TRP_RMM_EL3_ABI_VERS_MAJOR & 0x7FFF) << 16) | \
+				 (TRP_RMM_EL3_ABI_VERS_MINOR & 0xFFFF))
+
+#define TRP_PLATFORM_CORE_COUNT		PLATFORM_CORE_COUNT
 
 #ifndef __ASSEMBLER__
 
 #include <stdint.h>
 
-/* Data structure to hold SMC arguments */
-typedef struct trp_args {
-	uint64_t regs[TRP_ARGS_END >> 3];
-} __aligned(CACHE_WRITEBACK_GRANULE) trp_args_t;
-
 #define write_trp_arg(args, offset, val) (((args)->regs[offset >> 3])	\
 					 = val)
-
-/* RMI handled by TRP */
-#define RMI_FNUM_VERSION_REQ		U(0x150)
-
-#define RMI_FNUM_GRANULE_DELEGATE	U(0x151)
-#define RMI_FNUM_GRANULE_UNDELEGATE	U(0x152)
-
-#define RMI_RMM_REQ_VERSION		RMM_FID(SMC_64, RMI_FNUM_VERSION_REQ)
-
-#define RMI_RMM_GRANULE_DELEGATE	RMM_FID(SMC_64, \
-						RMI_FNUM_GRANULE_DELEGATE)
-#define RMI_RMM_GRANULE_UNDELEGATE	RMM_FID(SMC_64, \
-						RMI_FNUM_GRANULE_UNDELEGATE)
+/* RMI SMC64 FIDs handled by the TRP */
+#define RMI_RMM_REQ_VERSION		SMC64_RMI_FID(U(0))
+#define RMI_RMM_GRANULE_DELEGATE	SMC64_RMI_FID(U(1))
+#define RMI_RMM_GRANULE_UNDELEGATE	SMC64_RMI_FID(U(2))
 
 /* Definitions for RMI VERSION */
 #define RMI_ABI_VERSION_MAJOR		U(0x0)
 #define RMI_ABI_VERSION_MINOR		U(0x0)
-#define RMI_ABI_VERSION			((RMI_ABI_VERSION_MAJOR << 16) | \
-					RMI_ABI_VERSION_MINOR)
+#define RMI_ABI_VERSION			(((RMI_ABI_VERSION_MAJOR & 0x7FFF) \
+								  << 16) | \
+					 (RMI_ABI_VERSION_MINOR & 0xFFFF))
+
+#define TRP_RMM_EL3_VERSION_GET_MAJOR(x)		\
+				RMM_EL3_IFC_VERSION_GET_MAJOR((x))
+#define TRP_RMM_EL3_VERSION_GET_MINOR(x)		\
+				RMM_EL3_IFC_VERSION_GET_MAJOR_MINOR((x))
 
 /* Helper to issue SMC calls to BL31 */
 uint64_t trp_smc(trp_args_t *);
@@ -57,7 +48,10 @@
 void trp_main(void);
 
 /* Setup TRP. Executed only by Primary CPU */
-void trp_setup(void);
+void trp_setup(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/logical_sp.c b/services/std_svc/spm/el3_spmc/logical_sp.c
new file mode 100644
index 0000000..e080832
--- /dev/null
+++ b/services/std_svc/spm/el3_spmc/logical_sp.c

@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <services/el3_spmc_logical_sp.h>
+#include <services/ffa_svc.h>
+#include "spmc.h"
+
+/*******************************************************************************
+ * Validate any logical partition descriptors before we initialise.
+ * Initialization of said partitions will be taken care of during SPMC boot.
+ ******************************************************************************/
+int el3_sp_desc_validate(void)
+{
+	struct el3_lp_desc *lp_array;
+
+	/*
+	 * Assert the number of descriptors is less than maximum allowed.
+	 * This constant should be define on a per platform basis.
+	 */
+	assert(EL3_LP_DESCS_COUNT <= MAX_EL3_LP_DESCS_COUNT);
+
+	/* Check the array bounds are valid. */
+	assert(EL3_LP_DESCS_END >= EL3_LP_DESCS_START);
+
+	/* If no logical partitions are implemented then simply bail out. */
+	if (EL3_LP_DESCS_COUNT == 0U) {
+		return 0;
+	}
+
+	lp_array = get_el3_lp_array();
+
+	for (unsigned int index = 0; index < EL3_LP_DESCS_COUNT; index++) {
+		struct el3_lp_desc *lp_desc = &lp_array[index];
+
+		/* Validate our logical partition descriptors. */
+		if (lp_desc == NULL) {
+			ERROR("Invalid Logical SP Descriptor\n");
+			return -EINVAL;
+		}
+
+		/*
+		 * Ensure the ID follows the convention to indidate it resides
+		 * in the secure world.
+		 */
+		if (!ffa_is_secure_world_id(lp_desc->sp_id)) {
+			ERROR("Invalid Logical SP ID (0x%x)\n",
+			      lp_desc->sp_id);
+			return -EINVAL;
+		}
+
+		/* Ensure we don't conflict with the SPMC partition ID. */
+		if (lp_desc->sp_id == FFA_SPMC_ID) {
+			ERROR("Logical SP ID clashes with SPMC ID(0x%x)\n",
+			      lp_desc->sp_id);
+			return -EINVAL;
+		}
+
+		/* Ensure the UUID is not the NULL UUID. */
+		if (lp_desc->uuid[0] == 0 && lp_desc->uuid[1] == 0 &&
+		    lp_desc->uuid[2] == 0 && lp_desc->uuid[3] == 0) {
+			ERROR("Invalid UUID for Logical SP (0x%x)\n",
+			      lp_desc->sp_id);
+			return -EINVAL;
+		}
+
+		/* Ensure init function callback is registered. */
+		if (lp_desc->init == NULL) {
+			ERROR("Missing init function for Logical SP(0x%x)\n",
+			      lp_desc->sp_id);
+			return -EINVAL;
+		}
+
+		/* Ensure that LP only supports receiving direct requests. */
+		if (lp_desc->properties &
+		    ~(FFA_PARTITION_DIRECT_REQ_RECV)) {
+			ERROR("Invalid partition properties (0x%x)\n",
+			      lp_desc->properties);
+			return -EINVAL;
+		}
+
+		/* Ensure direct request function callback is registered. */
+		if (lp_desc->direct_req == NULL) {
+			ERROR("No Direct Req handler for Logical SP (0x%x)\n",
+			      lp_desc->sp_id);
+			return -EINVAL;
+		}
+
+		/* Ensure that all partition IDs are unique. */
+		for (unsigned int inner_idx = index + 1;
+		     inner_idx < EL3_LP_DESCS_COUNT; inner_idx++) {
+			if (lp_desc->sp_id == lp_array[inner_idx].sp_id) {
+				ERROR("Duplicate SP ID Detected (0x%x)\n",
+				      lp_desc->sp_id);
+				return -EINVAL;
+			}
+		}
+	}
+	return 0;
+}

diff --git a/services/std_svc/spm/el3_spmc/spmc.h b/services/std_svc/spm/el3_spmc/spmc.h
index df0aa61..d62be91 100644
--- a/services/std_svc/spm/el3_spmc/spmc.h
+++ b/services/std_svc/spm/el3_spmc/spmc.h

@@ -11,6 +11,7 @@
 
 #include <lib/psci/psci.h>
 #include <lib/spinlock.h>
+#include <services/el3_spmc_logical_sp.h>
 #include "spm_common.h"
 
 /*
@@ -32,9 +33,29 @@
 /* Align with Hafnium implementation */
 #define INV_SP_ID		0x7FFF
 
-/* FF-A warm boot types. */
-#define FFA_WB_TYPE_S2RAM	0
-#define FFA_WB_TYPE_NOTS2RAM	1
+/* FF-A Related helper macros. */
+#define FFA_ID_MASK			U(0xFFFF)
+#define FFA_PARTITION_ID_SHIFT		U(16)
+#define FFA_FEATURES_BIT31_MASK		U(0x1u << 31)
+#define FFA_FEATURES_RET_REQ_NS_BIT	U(0x1 << 1)
+
+#define FFA_RUN_EP_ID(ep_vcpu_ids) \
+		((ep_vcpu_ids >> FFA_PARTITION_ID_SHIFT) & FFA_ID_MASK)
+#define FFA_RUN_VCPU_ID(ep_vcpu_ids) \
+		(ep_vcpu_ids & FFA_ID_MASK)
+
+#define FFA_PAGE_SIZE (4096)
+#define FFA_RXTX_PAGE_COUNT_MASK 0x1F
+
+/* Ensure that the page size used by TF-A is 4k aligned. */
+CASSERT((PAGE_SIZE % FFA_PAGE_SIZE) == 0, assert_aligned_page_size);
+
+/*
+ * Defines to allow an SP to subscribe for power management messages
+ */
+#define FFA_PM_MSG_SUB_CPU_OFF			U(1 << 0)
+#define FFA_PM_MSG_SUB_CPU_SUSPEND		U(1 << 1)
+#define FFA_PM_MSG_SUB_CPU_SUSPEND_RESUME	U(1 << 2)
 
 /*
  * Runtime states of an execution context as per the FF-A v1.1 specification.
@@ -68,6 +89,28 @@
 	SP_STATE_AARCH32
 };
 
+enum mailbox_state {
+	/* There is no message in the mailbox. */
+	MAILBOX_STATE_EMPTY,
+
+	/* There is a message that has been populated in the mailbox. */
+	MAILBOX_STATE_FULL,
+};
+
+struct mailbox {
+	enum mailbox_state state;
+
+	/* RX/TX Buffers. */
+	void *rx_buffer;
+	const void *tx_buffer;
+
+	/* Size of RX/TX Buffer. */
+	uint32_t rxtx_page_count;
+
+	/* Lock access to mailbox. */
+	spinlock_t lock;
+};
+
 /*
  * Execution context members for an SP. This is a bit like struct
  * vcpu in a hypervisor.
@@ -118,8 +161,22 @@
 	/* Execution State. */
 	enum sp_execution_state execution_state;
 
+	/* Mailbox tracking. */
+	struct mailbox mailbox;
+
 	/* Secondary entrypoint. Only valid for a S-EL1 SP. */
 	uintptr_t secondary_ep;
+
+	/*
+	 * Store whether the SP has subscribed to any power management messages.
+	 */
+	uint16_t pwr_mgmt_msgs;
+
+	/*
+	 * Store whether the SP has requested the use of the NS bit for memory
+	 * management transactions if it is using FF-A v1.0.
+	 */
+	bool ns_bit_requested;
 };
 
 /*
@@ -142,14 +199,41 @@
 	uint16_t ns_ep_id;
 
 	/*
+	 * Mailbox tracking.
+	 */
+	struct mailbox mailbox;
+
+	/*
-	 * Supported FF-A Version.
+	 * Supported FF-A Version
 	 */
 	uint32_t ffa_version;
 };
 
+/**
+ * Holds information returned for each partition by the FFA_PARTITION_INFO_GET
+ * interface.
+ */
+struct ffa_partition_info_v1_0 {
+	uint16_t ep_id;
+	uint16_t execution_ctx_count;
+	uint32_t properties;
+};
+
+/* Extended structure for v1.1. */
+struct ffa_partition_info_v1_1 {
+	uint16_t ep_id;
+	uint16_t execution_ctx_count;
+	uint32_t properties;
+	uint32_t uuid[4];
+};
+
+/* Reference to power management hooks */
+extern const spd_pm_ops_t spmc_pm;
+
 /* Setup Function for different SP types. */
 void spmc_sp_common_setup(struct secure_partition_desc *sp,
-			  entry_point_info_t *ep_info);
+			  entry_point_info_t *ep_info,
+			  int32_t boot_info_reg);
 void spmc_el1_sp_setup(struct secure_partition_desc *sp,
 		       entry_point_info_t *ep_info);
 void spmc_sp_common_ep_commit(struct secure_partition_desc *sp,
@@ -184,4 +268,28 @@
  */
 bool is_ffa_secure_id_valid(uint16_t partition_id);
 
+/*
+ * Helper function to obtain the array storing the EL3
+ * Logical Partition descriptors.
+ */
+struct el3_lp_desc *get_el3_lp_array(void);
+
+/*
+ * Helper function to obtain the RX/TX buffer pair descriptor of the Hypervisor
+ * or OS kernel in the normal world or the last SP that was run.
+ */
+struct mailbox *spmc_get_mbox_desc(bool secure_origin);
+
+/*
+ * Helper function to obtain the context of an SP with a given partition ID.
+ */
+struct secure_partition_desc *spmc_get_sp_ctx(uint16_t id);
+
+/*
+ * Add helper function to obtain the FF-A version of the calling
+ * partition.
+ */
+uint32_t get_partition_ffa_version(bool secure_origin);
+
+
 #endif /* SPMC_H */

diff --git a/services/std_svc/spm/el3_spmc/spmc.mk b/services/std_svc/spm/el3_spmc/spmc.mk
index 2b154dd..aa591d9 100644
--- a/services/std_svc/spm/el3_spmc/spmc.mk
+++ b/services/std_svc/spm/el3_spmc/spmc.mk

@@ -10,8 +10,17 @@
 
 SPMC_SOURCES	:=	$(addprefix services/std_svc/spm/el3_spmc/,	\
 			spmc_main.c				\
-			spmc_setup.c)
+			spmc_setup.c				\
+			logical_sp.c				\
+			spmc_pm.c				\
+			spmc_shared_mem.c)
+
+# Specify platform specific logical partition implementation.
+SPMC_LP_SOURCES  := $(addprefix ${PLAT_DIR}/, \
+                    ${PLAT}_el3_spmc_logical_sp.c)
+
 
+SPMC_SOURCES += $(SPMC_LP_SOURCES)
 
 # Let the top-level Makefile know that we intend to include a BL32 image
 NEED_BL32		:=	yes

diff --git a/services/std_svc/spm/el3_spmc/spmc_main.c b/services/std_svc/spm/el3_spmc/spmc_main.c
index 3fd8c78..9b8621a 100644
--- a/services/std_svc/spm/el3_spmc/spmc_main.c
+++ b/services/std_svc/spm/el3_spmc/spmc_main.c

@@ -10,22 +10,29 @@
 #include <arch_helpers.h>
 #include <bl31/bl31.h>
 #include <bl31/ehf.h>
+#include <bl31/interrupt_mgmt.h>
 #include <common/debug.h>
 #include <common/fdt_wrappers.h>
 #include <common/runtime_svc.h>
+#include <common/uuid.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/smccc.h>
 #include <lib/utils.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
 #include <libfdt.h>
 #include <plat/common/platform.h>
+#include <services/el3_spmc_logical_sp.h>
 #include <services/ffa_svc.h>
 #include <services/spmc_svc.h>
 #include <services/spmd_svc.h>
 #include "spmc.h"
+#include "spmc_shared_mem.h"
 
 #include <platform_def.h>
 
+/* Declare the maximum number of SPs and El3 LPs. */
+#define MAX_SP_LP_PARTITIONS SECURE_PARTITION_COUNT + MAX_EL3_LP_DESCS_COUNT
+
 /*
  * Allocate a secure partition descriptor to describe each SP in the system that
  * does not reside at EL3.
@@ -40,6 +47,20 @@
  */
 static struct ns_endpoint_desc ns_ep_desc[NS_PARTITION_COUNT];
 
+static uint64_t spmc_sp_interrupt_handler(uint32_t id,
+					  uint32_t flags,
+					  void *handle,
+					  void *cookie);
+
+/*
+ * Helper function to obtain the array storing the EL3
+ * Logical Partition descriptors.
+ */
+struct el3_lp_desc *get_el3_lp_array(void)
+{
+	return (struct el3_lp_desc *) EL3_LP_DESCS_START;
+}
+
 /*
  * Helper function to obtain the descriptor of the last SP to whom control was
  * handed to on this physical cpu. Currently, we assume there is only one SP.
@@ -62,7 +83,7 @@
 /* Helper function to get pointer to SP context from its ID. */
 struct secure_partition_desc *spmc_get_sp_ctx(uint16_t id)
 {
-	/* Check for SWd Partitions. */
+	/* Check for Secure World Partitions. */
 	for (unsigned int i = 0U; i < SECURE_PARTITION_COUNT; i++) {
 		if (sp_desc[i].sp_id == id) {
 			return &(sp_desc[i]);
@@ -71,6 +92,29 @@
 	return NULL;
 }
 
+/*
+ * Helper function to obtain the descriptor of the Hypervisor or OS kernel.
+ * We assume that the first descriptor is reserved for this entity.
+ */
+struct ns_endpoint_desc *spmc_get_hyp_ctx(void)
+{
+	return &(ns_ep_desc[0]);
+}
+
+/*
+ * Helper function to obtain the RX/TX buffer pair descriptor of the Hypervisor
+ * or OS kernel in the normal world or the last SP that was run.
+ */
+struct mailbox *spmc_get_mbox_desc(bool secure_origin)
+{
+	/* Obtain the RX/TX buffer pair descriptor. */
+	if (secure_origin) {
+		return &(spmc_get_current_sp_ctx()->mailbox);
+	} else {
+		return &(spmc_get_hyp_ctx()->mailbox);
+	}
+}
+
 /******************************************************************************
  * This function returns to the place where spmc_sp_synchronous_entry() was
  * called originally.
@@ -105,6 +149,8 @@
  ******************************************************************************/
 bool is_ffa_secure_id_valid(uint16_t partition_id)
 {
+	struct el3_lp_desc *el3_lp_descs = get_el3_lp_array();
+
 	/* Ensure the ID is not the invalid partition ID. */
 	if (partition_id == INV_SP_ID) {
 		return false;
@@ -133,12 +179,22 @@
 		return false;
 	}
 
+	/* Ensure we don't clash with any Logical SP's. */
+	for (unsigned int i = 0U; i < EL3_LP_DESCS_COUNT; i++) {
+		if (el3_lp_descs[i].sp_id == partition_id) {
+			return false;
+		}
+	}
+
 	return true;
 }
 
 /*******************************************************************************
  * This function either forwards the request to the other world or returns
  * with an ERET depending on the source of the call.
+ * We can assume that the destination is for an entity at a lower exception
+ * level as any messages destined for a logical SP resident in EL3 will have
+ * already been taken care of by the SPMC before entering this function.
  ******************************************************************************/
 static uint64_t spmc_smc_return(uint32_t smc_fid,
 				bool secure_origin,
@@ -185,13 +241,20 @@
  ******************************************************************************/
 static inline bool direct_msg_validate_arg2(uint64_t x2)
 {
-	/*
-	 * We currently only support partition messages, therefore ensure x2 is
-	 * not set.
-	 */
-	if (x2 != (uint64_t) 0) {
-		VERBOSE("Arg2 MBZ for partition messages (0x%lx).\n", x2);
-		return false;
+	/* Check message type. */
+	if (x2 & FFA_FWK_MSG_BIT) {
+		/* We have a framework message, ensure it is a known message. */
+		if (x2 & ~(FFA_FWK_MSG_MASK | FFA_FWK_MSG_BIT)) {
+			VERBOSE("Invalid message format 0x%lx.\n", x2);
+			return false;
+		}
+	} else {
+		/* We have a partition messages, ensure x2 is not set. */
+		if (x2 != (uint64_t) 0) {
+			VERBOSE("Arg2 MBZ for partition messages. (0x%lx).\n",
+				x2);
+			return false;
+		}
 	}
 	return true;
 }
@@ -210,6 +273,7 @@
 				       uint64_t flags)
 {
 	uint16_t dst_id = ffa_endpoint_destination(x1);
+	struct el3_lp_desc *el3_lp_descs;
 	struct secure_partition_desc *sp;
 	unsigned int idx;
 
@@ -219,11 +283,22 @@
 					     FFA_ERROR_INVALID_PARAMETER);
 	}
 
+	el3_lp_descs = get_el3_lp_array();
+
+	/* Check if the request is destined for a Logical Partition. */
+	for (unsigned int i = 0U; i < MAX_EL3_LP_DESCS_COUNT; i++) {
+		if (el3_lp_descs[i].sp_id == dst_id) {
+			return el3_lp_descs[i].direct_req(
+					smc_fid, secure_origin, x1, x2, x3, x4,
+					cookie, handle, flags);
+		}
+	}
+
 	/*
-	 * If called by the secure world it is an invalid call since a
-	 * SP cannot call into the Normal world and there is no other SP to call
-	 * into. If there are other SPs in future then the partition runtime
-	 * model would need to be validated as well.
+	 * If the request was not targeted to a LSP and from the secure world
+	 * then it is invalid since a SP cannot call into the Normal world and
+	 * there is no other SP to call into. If there are other SPs in future
+	 * then the partition runtime model would need to be validated as well.
 	 */
 	if (secure_origin) {
 		VERBOSE("Direct request not supported to the Normal World.\n");
@@ -457,14 +532,870 @@
 	return spmc_ffa_error_return(handle, FFA_ERROR_NOT_SUPPORTED);
 }
 
+static uint64_t ffa_version_handler(uint32_t smc_fid,
+				    bool secure_origin,
+				    uint64_t x1,
+				    uint64_t x2,
+				    uint64_t x3,
+				    uint64_t x4,
+				    void *cookie,
+				    void *handle,
+				    uint64_t flags)
+{
+	uint32_t requested_version = x1 & FFA_VERSION_MASK;
+
+	if (requested_version & FFA_VERSION_BIT31_MASK) {
+		/* Invalid encoding, return an error. */
+		SMC_RET1(handle, FFA_ERROR_NOT_SUPPORTED);
+		/* Execution stops here. */
+	}
+
+	/* Determine the caller to store the requested version. */
+	if (secure_origin) {
+		/*
+		 * Ensure that the SP is reporting the same version as
+		 * specified in its manifest. If these do not match there is
+		 * something wrong with the SP.
+		 * TODO: Should we abort the SP? For now assert this is not
+		 *       case.
+		 */
+		assert(requested_version ==
+		       spmc_get_current_sp_ctx()->ffa_version);
+	} else {
+		/*
+		 * If this is called by the normal world, record this
+		 * information in its descriptor.
+		 */
+		spmc_get_hyp_ctx()->ffa_version = requested_version;
+	}
+
+	SMC_RET1(handle, MAKE_FFA_VERSION(FFA_VERSION_MAJOR,
+					  FFA_VERSION_MINOR));
+}
+
+/*******************************************************************************
+ * Helper function to obtain the FF-A version of the calling partition.
+ ******************************************************************************/
+uint32_t get_partition_ffa_version(bool secure_origin)
+{
+	if (secure_origin) {
+		return spmc_get_current_sp_ctx()->ffa_version;
+	} else {
+		return spmc_get_hyp_ctx()->ffa_version;
+	}
+}
+
+static uint64_t rxtx_map_handler(uint32_t smc_fid,
+				 bool secure_origin,
+				 uint64_t x1,
+				 uint64_t x2,
+				 uint64_t x3,
+				 uint64_t x4,
+				 void *cookie,
+				 void *handle,
+				 uint64_t flags)
+{
+	int ret;
+	uint32_t error_code;
+	uint32_t mem_atts = secure_origin ? MT_SECURE : MT_NS;
+	struct mailbox *mbox;
+	uintptr_t tx_address = x1;
+	uintptr_t rx_address = x2;
+	uint32_t page_count = x3 & FFA_RXTX_PAGE_COUNT_MASK; /* Bits [5:0] */
+	uint32_t buf_size = page_count * FFA_PAGE_SIZE;
+
+	/*
+	 * The SPMC does not support mapping of VM RX/TX pairs to facilitate
+	 * indirect messaging with SPs. Check if the Hypervisor has invoked this
+	 * ABI on behalf of a VM and reject it if this is the case.
+	 */
+	if (tx_address == 0 || rx_address == 0) {
+		WARN("Mapping RX/TX Buffers on behalf of VM not supported.\n");
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Ensure the specified buffers are not the same. */
+	if (tx_address == rx_address) {
+		WARN("TX Buffer must not be the same as RX Buffer.\n");
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Ensure the buffer size is not 0. */
+	if (buf_size == 0U) {
+		WARN("Buffer size must not be 0\n");
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/*
+	 * Ensure the buffer size is a multiple of the translation granule size
+	 * in TF-A.
+	 */
+	if (buf_size % PAGE_SIZE != 0U) {
+		WARN("Buffer size must be aligned to translation granule.\n");
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Obtain the RX/TX buffer pair descriptor. */
+	mbox = spmc_get_mbox_desc(secure_origin);
+
+	spin_lock(&mbox->lock);
+
+	/* Check if buffers have already been mapped. */
+	if (mbox->rx_buffer != 0 || mbox->tx_buffer != 0) {
+		WARN("RX/TX Buffers already mapped (%p/%p)\n",
+		     (void *) mbox->rx_buffer, (void *)mbox->tx_buffer);
+		error_code = FFA_ERROR_DENIED;
+		goto err;
+	}
+
+	/* memmap the TX buffer as read only. */
+	ret = mmap_add_dynamic_region(tx_address, /* PA */
+			tx_address, /* VA */
+			buf_size, /* size */
+			mem_atts | MT_RO_DATA); /* attrs */
+	if (ret != 0) {
+		/* Return the correct error code. */
+		error_code = (ret == -ENOMEM) ? FFA_ERROR_NO_MEMORY :
+						FFA_ERROR_INVALID_PARAMETER;
+		WARN("Unable to map TX buffer: %d\n", error_code);
+		goto err;
+	}
+
+	/* memmap the RX buffer as read write. */
+	ret = mmap_add_dynamic_region(rx_address, /* PA */
+			rx_address, /* VA */
+			buf_size, /* size */
+			mem_atts | MT_RW_DATA); /* attrs */
+
+	if (ret != 0) {
+		error_code = (ret == -ENOMEM) ? FFA_ERROR_NO_MEMORY :
+						FFA_ERROR_INVALID_PARAMETER;
+		WARN("Unable to map RX buffer: %d\n", error_code);
+		/* Unmap the TX buffer again. */
+		mmap_remove_dynamic_region(tx_address, buf_size);
+		goto err;
+	}
+
+	mbox->tx_buffer = (void *) tx_address;
+	mbox->rx_buffer = (void *) rx_address;
+	mbox->rxtx_page_count = page_count;
+	spin_unlock(&mbox->lock);
+
+	SMC_RET1(handle, FFA_SUCCESS_SMC32);
+	/* Execution stops here. */
+err:
+	spin_unlock(&mbox->lock);
+	return spmc_ffa_error_return(handle, error_code);
+}
+
+static uint64_t rxtx_unmap_handler(uint32_t smc_fid,
+				   bool secure_origin,
+				   uint64_t x1,
+				   uint64_t x2,
+				   uint64_t x3,
+				   uint64_t x4,
+				   void *cookie,
+				   void *handle,
+				   uint64_t flags)
+{
+	struct mailbox *mbox = spmc_get_mbox_desc(secure_origin);
+	uint32_t buf_size = mbox->rxtx_page_count * FFA_PAGE_SIZE;
+
+	/*
+	 * The SPMC does not support mapping of VM RX/TX pairs to facilitate
+	 * indirect messaging with SPs. Check if the Hypervisor has invoked this
+	 * ABI on behalf of a VM and reject it if this is the case.
+	 */
+	if (x1 != 0UL) {
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	spin_lock(&mbox->lock);
+
+	/* Check if buffers are currently mapped. */
+	if (mbox->rx_buffer == 0 || mbox->tx_buffer == 0) {
+		spin_unlock(&mbox->lock);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Unmap RX Buffer */
+	if (mmap_remove_dynamic_region((uintptr_t) mbox->rx_buffer,
+				       buf_size) != 0) {
+		WARN("Unable to unmap RX buffer!\n");
+	}
+
+	mbox->rx_buffer = 0;
+
+	/* Unmap TX Buffer */
+	if (mmap_remove_dynamic_region((uintptr_t) mbox->tx_buffer,
+				       buf_size) != 0) {
+		WARN("Unable to unmap TX buffer!\n");
+	}
+
+	mbox->tx_buffer = 0;
+	mbox->rxtx_page_count = 0;
+
+	spin_unlock(&mbox->lock);
+	SMC_RET1(handle, FFA_SUCCESS_SMC32);
+}
+
+/*
+ * Collate the partition information in a v1.1 partition information
+ * descriptor format, this will be converter later if required.
+ */
+static int partition_info_get_handler_v1_1(uint32_t *uuid,
+					   struct ffa_partition_info_v1_1
+						  *partitions,
+					   uint32_t max_partitions,
+					   uint32_t *partition_count)
+{
+	uint32_t index;
+	struct ffa_partition_info_v1_1 *desc;
+	bool null_uuid = is_null_uuid(uuid);
+	struct el3_lp_desc *el3_lp_descs = get_el3_lp_array();
+
+	/* Deal with Logical Partitions. */
+	for (index = 0U; index < EL3_LP_DESCS_COUNT; index++) {
+		if (null_uuid || uuid_match(uuid, el3_lp_descs[index].uuid)) {
+			/* Found a matching UUID, populate appropriately. */
+			if (*partition_count >= max_partitions) {
+				return FFA_ERROR_NO_MEMORY;
+			}
+
+			desc = &partitions[*partition_count];
+			desc->ep_id = el3_lp_descs[index].sp_id;
+			desc->execution_ctx_count = PLATFORM_CORE_COUNT;
+			desc->properties = el3_lp_descs[index].properties;
+			if (null_uuid) {
+				copy_uuid(desc->uuid, el3_lp_descs[index].uuid);
+			}
+			(*partition_count)++;
+		}
+	}
+
+	/* Deal with physical SP's. */
+	for (index = 0U; index < SECURE_PARTITION_COUNT; index++) {
+		if (null_uuid || uuid_match(uuid, sp_desc[index].uuid)) {
+			/* Found a matching UUID, populate appropriately. */
+			if (*partition_count >= max_partitions) {
+				return FFA_ERROR_NO_MEMORY;
+			}
+
+			desc = &partitions[*partition_count];
+			desc->ep_id = sp_desc[index].sp_id;
+			/*
+			 * Execution context count must match No. cores for
+			 * S-EL1 SPs.
+			 */
+			desc->execution_ctx_count = PLATFORM_CORE_COUNT;
+			desc->properties = sp_desc[index].properties;
+			if (null_uuid) {
+				copy_uuid(desc->uuid, sp_desc[index].uuid);
+			}
+			(*partition_count)++;
+		}
+	}
+	return 0;
+}
+
+/*
+ * Handle the case where that caller only wants the count of partitions
+ * matching a given UUID and does not want the corresponding descriptors
+ * populated.
+ */
+static uint32_t partition_info_get_handler_count_only(uint32_t *uuid)
+{
+	uint32_t index = 0;
+	uint32_t partition_count = 0;
+	bool null_uuid = is_null_uuid(uuid);
+	struct el3_lp_desc *el3_lp_descs = get_el3_lp_array();
+
+	/* Deal with Logical Partitions. */
+	for (index = 0U; index < EL3_LP_DESCS_COUNT; index++) {
+		if (null_uuid ||
+		    uuid_match(uuid, el3_lp_descs[index].uuid)) {
+			(partition_count)++;
+		}
+	}
+
+	/* Deal with physical SP's. */
+	for (index = 0U; index < SECURE_PARTITION_COUNT; index++) {
+		if (null_uuid || uuid_match(uuid, sp_desc[index].uuid)) {
+			(partition_count)++;
+		}
+	}
+	return partition_count;
+}
+
+/*
+ * If the caller of the PARTITION_INFO_GET ABI was a v1.0 caller, populate
+ * the coresponding descriptor format from the v1.1 descriptor array.
+ */
+static uint64_t partition_info_populate_v1_0(struct ffa_partition_info_v1_1
+					     *partitions,
+					     struct mailbox *mbox,
+					     int partition_count)
+{
+	uint32_t index;
+	uint32_t buf_size;
+	uint32_t descriptor_size;
+	struct ffa_partition_info_v1_0 *v1_0_partitions =
+		(struct ffa_partition_info_v1_0 *) mbox->rx_buffer;
+
+	buf_size = mbox->rxtx_page_count * FFA_PAGE_SIZE;
+	descriptor_size = partition_count *
+			  sizeof(struct ffa_partition_info_v1_0);
+
+	if (descriptor_size > buf_size) {
+		return FFA_ERROR_NO_MEMORY;
+	}
+
+	for (index = 0U; index < partition_count; index++) {
+		v1_0_partitions[index].ep_id = partitions[index].ep_id;
+		v1_0_partitions[index].execution_ctx_count =
+			partitions[index].execution_ctx_count;
+		v1_0_partitions[index].properties =
+			partitions[index].properties;
+	}
+	return 0;
+}
+
+/*
+ * Main handler for FFA_PARTITION_INFO_GET which supports both FF-A v1.1 and
+ * v1.0 implementations.
+ */
+static uint64_t partition_info_get_handler(uint32_t smc_fid,
+					   bool secure_origin,
+					   uint64_t x1,
+					   uint64_t x2,
+					   uint64_t x3,
+					   uint64_t x4,
+					   void *cookie,
+					   void *handle,
+					   uint64_t flags)
+{
+	int ret;
+	uint32_t partition_count = 0;
+	uint32_t size = 0;
+	uint32_t ffa_version = get_partition_ffa_version(secure_origin);
+	struct mailbox *mbox;
+	uint64_t info_get_flags;
+	bool count_only;
+	uint32_t uuid[4];
+
+	uuid[0] = x1;
+	uuid[1] = x2;
+	uuid[2] = x3;
+	uuid[3] = x4;
+
+	/* Determine if the Partition descriptors should be populated. */
+	info_get_flags = SMC_GET_GP(handle, CTX_GPREG_X5);
+	count_only = (info_get_flags & FFA_PARTITION_INFO_GET_COUNT_FLAG_MASK);
+
+	/* Handle the case where we don't need to populate the descriptors. */
+	if (count_only) {
+		partition_count = partition_info_get_handler_count_only(uuid);
+		if (partition_count == 0) {
+			return spmc_ffa_error_return(handle,
+						FFA_ERROR_INVALID_PARAMETER);
+		}
+	} else {
+		struct ffa_partition_info_v1_1 partitions[MAX_SP_LP_PARTITIONS];
+
+		/*
+		 * Handle the case where the partition descriptors are required,
+		 * check we have the buffers available and populate the
+		 * appropriate structure version.
+		 */
+
+		/* Obtain the v1.1 format of the descriptors. */
+		ret = partition_info_get_handler_v1_1(uuid, partitions,
+						      MAX_SP_LP_PARTITIONS,
+						      &partition_count);
+
+		/* Check if an error occurred during discovery. */
+		if (ret != 0) {
+			goto err;
+		}
+
+		/* If we didn't find any matches the UUID is unknown. */
+		if (partition_count == 0) {
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err;
+		}
+
+		/* Obtain the partition mailbox RX/TX buffer pair descriptor. */
+		mbox = spmc_get_mbox_desc(secure_origin);
+
+		/*
+		 * If the caller has not bothered registering its RX/TX pair
+		 * then return an error code.
+		 */
+		spin_lock(&mbox->lock);
+		if (mbox->rx_buffer == NULL) {
+			ret = FFA_ERROR_BUSY;
+			goto err_unlock;
+		}
+
+		/* Ensure the RX buffer is currently free. */
+		if (mbox->state != MAILBOX_STATE_EMPTY) {
+			ret = FFA_ERROR_BUSY;
+			goto err_unlock;
+		}
+
+		/* Zero the RX buffer before populating. */
+		(void)memset(mbox->rx_buffer, 0,
+			     mbox->rxtx_page_count * FFA_PAGE_SIZE);
+
+		/*
+		 * Depending on the FF-A version of the requesting partition
+		 * we may need to convert to a v1.0 format otherwise we can copy
+		 * directly.
+		 */
+		if (ffa_version == MAKE_FFA_VERSION(U(1), U(0))) {
+			ret = partition_info_populate_v1_0(partitions,
+							   mbox,
+							   partition_count);
+			if (ret != 0) {
+				goto err_unlock;
+			}
+		} else {
+			uint32_t buf_size = mbox->rxtx_page_count *
+					    FFA_PAGE_SIZE;
+
+			/* Ensure the descriptor will fit in the buffer. */
+			size = sizeof(struct ffa_partition_info_v1_1);
+			if (partition_count * size  > buf_size) {
+				ret = FFA_ERROR_NO_MEMORY;
+				goto err_unlock;
+			}
+			memcpy(mbox->rx_buffer, partitions,
+			       partition_count * size);
+		}
+
+		mbox->state = MAILBOX_STATE_FULL;
+		spin_unlock(&mbox->lock);
+	}
+	SMC_RET4(handle, FFA_SUCCESS_SMC32, 0, partition_count, size);
+
+err_unlock:
+	spin_unlock(&mbox->lock);
+err:
+	return spmc_ffa_error_return(handle, ret);
+}
+
+static uint64_t ffa_feature_success(void *handle, uint32_t arg2)
+{
+	SMC_RET3(handle, FFA_SUCCESS_SMC32, 0, arg2);
+}
+
+static uint64_t ffa_features_retrieve_request(bool secure_origin,
+					      uint32_t input_properties,
+					      void *handle)
+{
+	/*
+	 * If we're called by the normal world we don't support any
+	 * additional features.
+	 */
+	if (!secure_origin) {
+		if ((input_properties & FFA_FEATURES_RET_REQ_NS_BIT) != 0U) {
+			return spmc_ffa_error_return(handle,
+						     FFA_ERROR_NOT_SUPPORTED);
+		}
+
+	} else {
+		struct secure_partition_desc *sp = spmc_get_current_sp_ctx();
+		/*
+		 * If v1.1 the NS bit must be set otherwise it is an invalid
+		 * call. If v1.0 check and store whether the SP has requested
+		 * the use of the NS bit.
+		 */
+		if (sp->ffa_version == MAKE_FFA_VERSION(1, 1)) {
+			if ((input_properties &
+			     FFA_FEATURES_RET_REQ_NS_BIT) == 0U) {
+				return spmc_ffa_error_return(handle,
+						       FFA_ERROR_NOT_SUPPORTED);
+			}
+			return ffa_feature_success(handle,
+						   FFA_FEATURES_RET_REQ_NS_BIT);
+		} else {
+			sp->ns_bit_requested = (input_properties &
+					       FFA_FEATURES_RET_REQ_NS_BIT) !=
+					       0U;
+		}
+		if (sp->ns_bit_requested) {
+			return ffa_feature_success(handle,
+						   FFA_FEATURES_RET_REQ_NS_BIT);
+		}
+	}
+	SMC_RET1(handle, FFA_SUCCESS_SMC32);
+}
+
+static uint64_t ffa_features_handler(uint32_t smc_fid,
+				     bool secure_origin,
+				     uint64_t x1,
+				     uint64_t x2,
+				     uint64_t x3,
+				     uint64_t x4,
+				     void *cookie,
+				     void *handle,
+				     uint64_t flags)
+{
+	uint32_t function_id = (uint32_t) x1;
+	uint32_t input_properties = (uint32_t) x2;
+
+	/* Check if a Feature ID was requested. */
+	if ((function_id & FFA_FEATURES_BIT31_MASK) == 0U) {
+		/* We currently don't support any additional features. */
+		return spmc_ffa_error_return(handle, FFA_ERROR_NOT_SUPPORTED);
+	}
+
+	/*
+	 * Handle the cases where we have separate handlers due to additional
+	 * properties.
+	 */
+	switch (function_id) {
+	case FFA_MEM_RETRIEVE_REQ_SMC32:
+	case FFA_MEM_RETRIEVE_REQ_SMC64:
+		return ffa_features_retrieve_request(secure_origin,
+						     input_properties,
+						     handle);
+	}
+
+	/*
+	 * We don't currently support additional input properties for these
+	 * other ABIs therefore ensure this value is set to 0.
+	 */
+	if (input_properties != 0U) {
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_NOT_SUPPORTED);
+	}
+
+	/* Report if any other FF-A ABI is supported. */
+	switch (function_id) {
+	/* Supported features from both worlds. */
+	case FFA_ERROR:
+	case FFA_SUCCESS_SMC32:
+	case FFA_INTERRUPT:
+	case FFA_SPM_ID_GET:
+	case FFA_ID_GET:
+	case FFA_FEATURES:
+	case FFA_VERSION:
+	case FFA_RX_RELEASE:
+	case FFA_MSG_SEND_DIRECT_REQ_SMC32:
+	case FFA_MSG_SEND_DIRECT_REQ_SMC64:
+	case FFA_PARTITION_INFO_GET:
+	case FFA_RXTX_MAP_SMC32:
+	case FFA_RXTX_MAP_SMC64:
+	case FFA_RXTX_UNMAP:
+	case FFA_MEM_FRAG_TX:
+	case FFA_MSG_RUN:
+
+		/*
+		 * We are relying on the fact that the other registers
+		 * will be set to 0 as these values align with the
+		 * currently implemented features of the SPMC. If this
+		 * changes this function must be extended to handle
+		 * reporting the additional functionality.
+		 */
+
+		SMC_RET1(handle, FFA_SUCCESS_SMC32);
+		/* Execution stops here. */
+
+	/* Supported ABIs only from the secure world. */
+	case FFA_SECONDARY_EP_REGISTER_SMC64:
+	case FFA_MSG_SEND_DIRECT_RESP_SMC32:
+	case FFA_MSG_SEND_DIRECT_RESP_SMC64:
+	case FFA_MEM_RELINQUISH:
+	case FFA_MSG_WAIT:
+
+		if (!secure_origin) {
+			return spmc_ffa_error_return(handle,
+				FFA_ERROR_NOT_SUPPORTED);
+		}
+		SMC_RET1(handle, FFA_SUCCESS_SMC32);
+		/* Execution stops here. */
+
+	/* Supported features only from the normal world. */
+	case FFA_MEM_SHARE_SMC32:
+	case FFA_MEM_SHARE_SMC64:
+	case FFA_MEM_LEND_SMC32:
+	case FFA_MEM_LEND_SMC64:
+	case FFA_MEM_RECLAIM:
+	case FFA_MEM_FRAG_RX:
+
+		if (secure_origin) {
+			return spmc_ffa_error_return(handle,
+					FFA_ERROR_NOT_SUPPORTED);
+		}
+		SMC_RET1(handle, FFA_SUCCESS_SMC32);
+		/* Execution stops here. */
+
+	default:
+		return spmc_ffa_error_return(handle,
+					FFA_ERROR_NOT_SUPPORTED);
+	}
+}
+
+static uint64_t ffa_id_get_handler(uint32_t smc_fid,
+				   bool secure_origin,
+				   uint64_t x1,
+				   uint64_t x2,
+				   uint64_t x3,
+				   uint64_t x4,
+				   void *cookie,
+				   void *handle,
+				   uint64_t flags)
+{
+	if (secure_origin) {
+		SMC_RET3(handle, FFA_SUCCESS_SMC32, 0x0,
+			 spmc_get_current_sp_ctx()->sp_id);
+	} else {
+		SMC_RET3(handle, FFA_SUCCESS_SMC32, 0x0,
+			 spmc_get_hyp_ctx()->ns_ep_id);
+	}
+}
+
+/*
+ * Enable an SP to query the ID assigned to the SPMC.
+ */
+static uint64_t ffa_spm_id_get_handler(uint32_t smc_fid,
+				       bool secure_origin,
+				       uint64_t x1,
+				       uint64_t x2,
+				       uint64_t x3,
+				       uint64_t x4,
+				       void *cookie,
+				       void *handle,
+				       uint64_t flags)
+{
+	assert(x1 == 0UL);
+	assert(x2 == 0UL);
+	assert(x3 == 0UL);
+	assert(x4 == 0UL);
+	assert(SMC_GET_GP(handle, CTX_GPREG_X5) == 0UL);
+	assert(SMC_GET_GP(handle, CTX_GPREG_X6) == 0UL);
+	assert(SMC_GET_GP(handle, CTX_GPREG_X7) == 0UL);
+
+	SMC_RET3(handle, FFA_SUCCESS_SMC32, 0x0, FFA_SPMC_ID);
+}
+
+static uint64_t ffa_run_handler(uint32_t smc_fid,
+				bool secure_origin,
+				uint64_t x1,
+				uint64_t x2,
+				uint64_t x3,
+				uint64_t x4,
+				void *cookie,
+				void *handle,
+				uint64_t flags)
+{
+	struct secure_partition_desc *sp;
+	uint16_t target_id = FFA_RUN_EP_ID(x1);
+	uint16_t vcpu_id = FFA_RUN_VCPU_ID(x1);
+	unsigned int idx;
+	unsigned int *rt_state;
+	unsigned int *rt_model;
+
+	/* Can only be called from the normal world. */
+	if (secure_origin) {
+		ERROR("FFA_RUN can only be called from NWd.\n");
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Cannot run a Normal world partition. */
+	if (ffa_is_normal_world_id(target_id)) {
+		ERROR("Cannot run a NWd partition (0x%x).\n", target_id);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Check that the target SP exists. */
+	sp = spmc_get_sp_ctx(target_id);
+		ERROR("Unknown partition ID (0x%x).\n", target_id);
+	if (sp == NULL) {
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	idx = get_ec_index(sp);
+	if (idx != vcpu_id) {
+		ERROR("Cannot run vcpu %d != %d.\n", idx, vcpu_id);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+	rt_state = &((sp->ec[idx]).rt_state);
+	rt_model = &((sp->ec[idx]).rt_model);
+	if (*rt_state == RT_STATE_RUNNING) {
+		ERROR("Partition (0x%x) is already running.\n", target_id);
+		return spmc_ffa_error_return(handle, FFA_ERROR_BUSY);
+	}
+
+	/*
+	 * Sanity check that if the execution context was not waiting then it
+	 * was either in the direct request or the run partition runtime model.
+	 */
+	if (*rt_state == RT_STATE_PREEMPTED || *rt_state == RT_STATE_BLOCKED) {
+		assert(*rt_model == RT_MODEL_RUN ||
+		       *rt_model == RT_MODEL_DIR_REQ);
+	}
+
+	/*
+	 * If the context was waiting then update the partition runtime model.
+	 */
+	if (*rt_state == RT_STATE_WAITING) {
+		*rt_model = RT_MODEL_RUN;
+	}
+
+	/*
+	 * Forward the request to the correct SP vCPU after updating
+	 * its state.
+	 */
+	*rt_state = RT_STATE_RUNNING;
+
+	return spmc_smc_return(smc_fid, secure_origin, x1, 0, 0, 0,
+			       handle, cookie, flags, target_id);
+}
+
+static uint64_t rx_release_handler(uint32_t smc_fid,
+				   bool secure_origin,
+				   uint64_t x1,
+				   uint64_t x2,
+				   uint64_t x3,
+				   uint64_t x4,
+				   void *cookie,
+				   void *handle,
+				   uint64_t flags)
+{
+	struct mailbox *mbox = spmc_get_mbox_desc(secure_origin);
+
+	spin_lock(&mbox->lock);
+
+	if (mbox->state != MAILBOX_STATE_FULL) {
+		spin_unlock(&mbox->lock);
+		return spmc_ffa_error_return(handle, FFA_ERROR_DENIED);
+	}
+
+	mbox->state = MAILBOX_STATE_EMPTY;
+	spin_unlock(&mbox->lock);
+
+	SMC_RET1(handle, FFA_SUCCESS_SMC32);
+}
+
+/*
+ * Perform initial validation on the provided secondary entry point.
+ * For now ensure it does not lie within the BL31 Image or the SP's
+ * RX/TX buffers as these are mapped within EL3.
+ * TODO: perform validation for additional invalid memory regions.
+ */
+static int validate_secondary_ep(uintptr_t ep, struct secure_partition_desc *sp)
+{
+	struct mailbox *mb;
+	uintptr_t buffer_size;
+	uintptr_t sp_rx_buffer;
+	uintptr_t sp_tx_buffer;
+	uintptr_t sp_rx_buffer_limit;
+	uintptr_t sp_tx_buffer_limit;
+
+	mb = &sp->mailbox;
+	buffer_size = (uintptr_t) (mb->rxtx_page_count * FFA_PAGE_SIZE);
+	sp_rx_buffer = (uintptr_t) mb->rx_buffer;
+	sp_tx_buffer = (uintptr_t) mb->tx_buffer;
+	sp_rx_buffer_limit = sp_rx_buffer + buffer_size;
+	sp_tx_buffer_limit = sp_tx_buffer + buffer_size;
+
+	/*
+	 * Check if the entry point lies within BL31, or the
+	 * SP's RX or TX buffer.
+	 */
+	if ((ep >= BL31_BASE && ep < BL31_LIMIT) ||
+	    (ep >= sp_rx_buffer && ep < sp_rx_buffer_limit) ||
+	    (ep >= sp_tx_buffer && ep < sp_tx_buffer_limit)) {
+		return -EINVAL;
+	}
+	return 0;
+}
+
 /*******************************************************************************
+ * This function handles the FFA_SECONDARY_EP_REGISTER SMC to allow an SP to
+ *  register an entry point for initialization during a secondary cold boot.
+ ******************************************************************************/
+static uint64_t ffa_sec_ep_register_handler(uint32_t smc_fid,
+					    bool secure_origin,
+					    uint64_t x1,
+					    uint64_t x2,
+					    uint64_t x3,
+					    uint64_t x4,
+					    void *cookie,
+					    void *handle,
+					    uint64_t flags)
+{
+	struct secure_partition_desc *sp;
+	struct sp_exec_ctx *sp_ctx;
+
+	/* This request cannot originate from the Normal world. */
+	if (!secure_origin) {
+		WARN("%s: Can only be called from SWd.\n", __func__);
+		return spmc_ffa_error_return(handle, FFA_ERROR_NOT_SUPPORTED);
+	}
+
+	/* Get the context of the current SP. */
+	sp = spmc_get_current_sp_ctx();
+	if (sp == NULL) {
+		WARN("%s: Cannot find SP context.\n", __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Only an S-EL1 SP should be invoking this ABI. */
+	if (sp->runtime_el != S_EL1) {
+		WARN("%s: Can only be called for a S-EL1 SP.\n", __func__);
+		return spmc_ffa_error_return(handle, FFA_ERROR_DENIED);
+	}
+
+	/* Ensure the SP is in its initialization state. */
+	sp_ctx = spmc_get_sp_ec(sp);
+	if (sp_ctx->rt_model != RT_MODEL_INIT) {
+		WARN("%s: Can only be called during SP initialization.\n",
+		     __func__);
+		return spmc_ffa_error_return(handle, FFA_ERROR_DENIED);
+	}
+
+	/* Perform initial validation of the secondary entry point. */
+	if (validate_secondary_ep(x1, sp)) {
+		WARN("%s: Invalid entry point provided (0x%lx).\n",
+		     __func__, x1);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/*
+	 * Update the secondary entrypoint in SP context.
+	 * We don't need a lock here as during partition initialization there
+	 * will only be a single core online.
+	 */
+	sp->secondary_ep = x1;
+	VERBOSE("%s: 0x%lx\n", __func__, sp->secondary_ep);
+
+	SMC_RET1(handle, FFA_SUCCESS_SMC32);
+}
+
+/*******************************************************************************
  * This function will parse the Secure Partition Manifest. From manifest, it
  * will fetch details for preparing Secure partition image context and secure
  * partition image boot arguments if any.
  ******************************************************************************/
 static int sp_manifest_parse(void *sp_manifest, int offset,
 			     struct secure_partition_desc *sp,
-			     entry_point_info_t *ep_info)
+			     entry_point_info_t *ep_info,
+			     int32_t *boot_info_reg)
 {
 	int32_t ret, node;
 	uint32_t config_32;
@@ -479,6 +1410,13 @@
 		return node;
 	}
 
+	ret = fdt_read_uint32_array(sp_manifest, node, "uuid",
+				    ARRAY_SIZE(sp->uuid), sp->uuid);
+	if (ret != 0) {
+		ERROR("Missing Secure Partition UUID.\n");
+		return ret;
+	}
+
 	ret = fdt_read_uint32(sp_manifest, node, "exception-level", &config_32);
 	if (ret != 0) {
 		ERROR("Missing SP Exception Level information.\n");
@@ -503,7 +1441,43 @@
 
 	sp->execution_state = config_32;
 
+	ret = fdt_read_uint32(sp_manifest, node,
+			      "messaging-method", &config_32);
+	if (ret != 0) {
+		ERROR("Missing Secure Partition messaging method.\n");
+		return ret;
+	}
+
+	/* Validate this entry, we currently only support direct messaging. */
+	if ((config_32 & ~(FFA_PARTITION_DIRECT_REQ_RECV |
+			  FFA_PARTITION_DIRECT_REQ_SEND)) != 0U) {
+		WARN("Invalid Secure Partition messaging method (0x%x)\n",
+		     config_32);
+		return -EINVAL;
+	}
+
+	sp->properties = config_32;
+
+	ret = fdt_read_uint32(sp_manifest, node,
+			      "execution-ctx-count", &config_32);
+
+	if (ret != 0) {
+		ERROR("Missing SP Execution Context Count.\n");
+		return ret;
+	}
+
 	/*
+	 * Ensure this field is set correctly in the manifest however
+	 * since this is currently a hardcoded value for S-EL1 partitions
+	 * we don't need to save it here, just validate.
+	 */
+	if (config_32 != PLATFORM_CORE_COUNT) {
+		ERROR("SP Execution Context Count (%u) must be %u.\n",
+			config_32, PLATFORM_CORE_COUNT);
+		return -EINVAL;
+	}
+
+	/*
 	 * Look for the optional fields that are expected to be present in
 	 * an SP manifest.
 	 */
@@ -519,6 +1493,39 @@
 		sp->sp_id = config_32;
 	}
 
+	ret = fdt_read_uint32(sp_manifest, node,
+			      "power-management-messages", &config_32);
+	if (ret != 0) {
+		WARN("Missing Power Management Messages entry.\n");
+	} else {
+		/*
+		 * Ensure only the currently supported power messages have
+		 * been requested.
+		 */
+		if (config_32 & ~(FFA_PM_MSG_SUB_CPU_OFF |
+				  FFA_PM_MSG_SUB_CPU_SUSPEND |
+				  FFA_PM_MSG_SUB_CPU_SUSPEND_RESUME)) {
+			ERROR("Requested unsupported PM messages (%x)\n",
+			      config_32);
+			return -EINVAL;
+		}
+		sp->pwr_mgmt_msgs = config_32;
+	}
+
+	ret = fdt_read_uint32(sp_manifest, node,
+			      "gp-register-num", &config_32);
+	if (ret != 0) {
+		WARN("Missing boot information register.\n");
+	} else {
+		/* Check if a register number between 0-3 is specified. */
+		if (config_32 < 4) {
+			*boot_info_reg = config_32;
+		} else {
+			WARN("Incorrect boot information register (%u).\n",
+			     config_32);
+		}
+	}
+
 	return 0;
 }
 
@@ -534,7 +1541,7 @@
 	uintptr_t manifest_base;
 	uintptr_t manifest_base_align;
 	entry_point_info_t *next_image_ep_info;
-	int32_t ret;
+	int32_t ret, boot_info_reg = -1;
 	struct secure_partition_desc *sp;
 
 	next_image_ep_info = bl31_plat_get_next_image_ep_info(SECURE);
@@ -593,7 +1600,8 @@
 		       SECURE | EP_ST_ENABLE);
 
 	/* Parse the SP manifest. */
-	ret = sp_manifest_parse(sp_manifest, ret, sp, next_image_ep_info);
+	ret = sp_manifest_parse(sp_manifest, ret, sp, next_image_ep_info,
+				&boot_info_reg);
 	if (ret != 0) {
 		ERROR("Error in Secure Partition manifest parsing.\n");
 		return ret;
@@ -606,7 +1614,7 @@
 	}
 
 	/* Perform any common initialisation. */
-	spmc_sp_common_setup(sp, next_image_ep_info);
+	spmc_sp_common_setup(sp, next_image_ep_info, boot_info_reg);
 
 	/* Perform any initialisation specific to S-EL1 SPs. */
 	spmc_el1_sp_setup(sp, next_image_ep_info);
@@ -621,6 +1629,37 @@
  * This function takes an SP context pointer and performs a synchronous entry
  * into it.
  ******************************************************************************/
+static int32_t logical_sp_init(void)
+{
+	int32_t rc = 0;
+	struct el3_lp_desc *el3_lp_descs;
+
+	/* Perform initial validation of the Logical Partitions. */
+	rc = el3_sp_desc_validate();
+	if (rc != 0) {
+		ERROR("Logical Partition validation failed!\n");
+		return rc;
+	}
+
+	el3_lp_descs = get_el3_lp_array();
+
+	INFO("Logical Secure Partition init start.\n");
+	for (unsigned int i = 0U; i < EL3_LP_DESCS_COUNT; i++) {
+		rc = el3_lp_descs[i].init();
+		if (rc != 0) {
+			ERROR("Logical SP (0x%x) Failed to Initialize\n",
+			      el3_lp_descs[i].sp_id);
+			return rc;
+		}
+		VERBOSE("Logical SP (0x%x) Initialized\n",
+			      el3_lp_descs[i].sp_id);
+	}
+
+	INFO("Logical Secure Partition init completed.\n");
+
+	return rc;
+}
+
 uint64_t spmc_sp_synchronous_entry(struct sp_exec_ctx *ec)
 {
 	uint64_t rc;
@@ -684,6 +1723,9 @@
 	for (unsigned int i = 0U; i < SECURE_PARTITION_COUNT; i++) {
 		sp = &sp_desc[i];
 		sp->sp_id = INV_SP_ID;
+		sp->mailbox.rx_buffer = NULL;
+		sp->mailbox.tx_buffer = NULL;
+		sp->mailbox.state = MAILBOX_STATE_EMPTY;
 		sp->secondary_ep = 0;
 	}
 }
@@ -700,6 +1742,9 @@
 		 */
 		ns_ep->ns_ep_id = 0;
 		ns_ep->ffa_version = 0;
+		ns_ep->mailbox.rx_buffer = NULL;
+		ns_ep->mailbox.tx_buffer = NULL;
+		ns_ep->mailbox.state = MAILBOX_STATE_EMPTY;
 	}
 }
 
@@ -720,11 +1765,31 @@
 int32_t spmc_setup(void)
 {
 	int32_t ret;
+	uint32_t flags;
 
 	/* Initialize endpoint descriptors */
 	initalize_sp_descs();
 	initalize_ns_ep_descs();
 
+	/*
+	 * Retrieve the information of the datastore for tracking shared memory
+	 * requests allocated by platform code and zero the region if available.
+	 */
+	ret = plat_spmc_shmem_datastore_get(&spmc_shmem_obj_state.data,
+					    &spmc_shmem_obj_state.data_size);
+	if (ret != 0) {
+		ERROR("Failed to obtain memory descriptor backing store!\n");
+		return ret;
+	}
+	memset(spmc_shmem_obj_state.data, 0, spmc_shmem_obj_state.data_size);
+
+	/* Setup logical SPs. */
+	ret = logical_sp_init();
+	if (ret != 0) {
+		ERROR("Failed to initialize Logical Partitions.\n");
+		return ret;
+	}
+
 	/* Perform physical SP setup. */
 
 	/* Disable MMU at EL1 (initialized by BL2) */
@@ -739,6 +1804,24 @@
 		return ret;
 	}
 
+	/* Register power management hooks with PSCI */
+	psci_register_spd_pm_hook(&spmc_pm);
+
+	/*
+	 * Register an interrupt handler for S-EL1 interrupts
+	 * when generated during code executing in the
+	 * non-secure state.
+	 */
+	flags = 0;
+	set_interrupt_rm_flag(flags, NON_SECURE);
+	ret = register_interrupt_type_handler(INTR_TYPE_S_EL1,
+					      spmc_sp_interrupt_handler,
+					      flags);
+	if (ret != 0) {
+		ERROR("Failed to register interrupt handler! (%d)\n", ret);
+		panic();
+	}
+
 	/* Register init function for deferred init.  */
 	bl31_register_bl32_init(&sp_init);
 
@@ -762,6 +1845,27 @@
 {
 	switch (smc_fid) {
 
+	case FFA_VERSION:
+		return ffa_version_handler(smc_fid, secure_origin, x1, x2, x3,
+					   x4, cookie, handle, flags);
+
+	case FFA_SPM_ID_GET:
+		return ffa_spm_id_get_handler(smc_fid, secure_origin, x1, x2,
+					     x3, x4, cookie, handle, flags);
+
+	case FFA_ID_GET:
+		return ffa_id_get_handler(smc_fid, secure_origin, x1, x2, x3,
+					  x4, cookie, handle, flags);
+
+	case FFA_FEATURES:
+		return ffa_features_handler(smc_fid, secure_origin, x1, x2, x3,
+					    x4, cookie, handle, flags);
+
+	case FFA_SECONDARY_EP_REGISTER_SMC64:
+		return ffa_sec_ep_register_handler(smc_fid, secure_origin, x1,
+						   x2, x3, x4, cookie, handle,
+						   flags);
+
 	case FFA_MSG_SEND_DIRECT_REQ_SMC32:
 	case FFA_MSG_SEND_DIRECT_REQ_SMC64:
 		return direct_req_smc_handler(smc_fid, secure_origin, x1, x2,
@@ -772,6 +1876,24 @@
 		return direct_resp_smc_handler(smc_fid, secure_origin, x1, x2,
 					       x3, x4, cookie, handle, flags);
 
+	case FFA_RXTX_MAP_SMC32:
+	case FFA_RXTX_MAP_SMC64:
+		return rxtx_map_handler(smc_fid, secure_origin, x1, x2, x3, x4,
+					cookie, handle, flags);
+
+	case FFA_RXTX_UNMAP:
+		return rxtx_unmap_handler(smc_fid, secure_origin, x1, x2, x3,
+					  x4, cookie, handle, flags);
+
+	case FFA_PARTITION_INFO_GET:
+		return partition_info_get_handler(smc_fid, secure_origin, x1,
+						  x2, x3, x4, cookie, handle,
+						  flags);
+
+	case FFA_RX_RELEASE:
+		return rx_release_handler(smc_fid, secure_origin, x1, x2, x3,
+					  x4, cookie, handle, flags);
+
 	case FFA_MSG_WAIT:
 		return msg_wait_handler(smc_fid, secure_origin, x1, x2, x3, x4,
 					cookie, handle, flags);
@@ -780,9 +1902,94 @@
 		return ffa_error_handler(smc_fid, secure_origin, x1, x2, x3, x4,
 					cookie, handle, flags);
 
+	case FFA_MSG_RUN:
+		return ffa_run_handler(smc_fid, secure_origin, x1, x2, x3, x4,
+				       cookie, handle, flags);
+
+	case FFA_MEM_SHARE_SMC32:
+	case FFA_MEM_SHARE_SMC64:
+	case FFA_MEM_LEND_SMC32:
+	case FFA_MEM_LEND_SMC64:
+		return spmc_ffa_mem_send(smc_fid, secure_origin, x1, x2, x3, x4,
+					 cookie, handle, flags);
+
+	case FFA_MEM_FRAG_TX:
+		return spmc_ffa_mem_frag_tx(smc_fid, secure_origin, x1, x2, x3,
+					    x4, cookie, handle, flags);
+
+	case FFA_MEM_FRAG_RX:
+		return spmc_ffa_mem_frag_rx(smc_fid, secure_origin, x1, x2, x3,
+					    x4, cookie, handle, flags);
+
+	case FFA_MEM_RETRIEVE_REQ_SMC32:
+	case FFA_MEM_RETRIEVE_REQ_SMC64:
+		return spmc_ffa_mem_retrieve_req(smc_fid, secure_origin, x1, x2,
+						 x3, x4, cookie, handle, flags);
+
+	case FFA_MEM_RELINQUISH:
+		return spmc_ffa_mem_relinquish(smc_fid, secure_origin, x1, x2,
+					       x3, x4, cookie, handle, flags);
+
+	case FFA_MEM_RECLAIM:
+		return spmc_ffa_mem_reclaim(smc_fid, secure_origin, x1, x2, x3,
+					    x4, cookie, handle, flags);
+
 	default:
 		WARN("Unsupported FF-A call 0x%08x.\n", smc_fid);
 		break;
 	}
 	return spmc_ffa_error_return(handle, FFA_ERROR_NOT_SUPPORTED);
 }
+
+/*******************************************************************************
+ * This function is the handler registered for S-EL1 interrupts by the SPMC. It
+ * validates the interrupt and upon success arranges entry into the SP for
+ * handling the interrupt.
+ ******************************************************************************/
+static uint64_t spmc_sp_interrupt_handler(uint32_t id,
+					  uint32_t flags,
+					  void *handle,
+					  void *cookie)
+{
+	struct secure_partition_desc *sp = spmc_get_current_sp_ctx();
+	struct sp_exec_ctx *ec;
+	uint32_t linear_id = plat_my_core_pos();
+
+	/* Sanity check for a NULL pointer dereference. */
+	assert(sp != NULL);
+
+	/* Check the security state when the exception was generated. */
+	assert(get_interrupt_src_ss(flags) == NON_SECURE);
+
+	/* Panic if not an S-EL1 Partition. */
+	if (sp->runtime_el != S_EL1) {
+		ERROR("Interrupt received for a non S-EL1 SP on core%u.\n",
+		      linear_id);
+		panic();
+	}
+
+	/* Obtain a reference to the SP execution context. */
+	ec = spmc_get_sp_ec(sp);
+
+	/* Ensure that the execution context is in waiting state else panic. */
+	if (ec->rt_state != RT_STATE_WAITING) {
+		ERROR("SP EC on core%u is not waiting (%u), it is (%u).\n",
+		      linear_id, RT_STATE_WAITING, ec->rt_state);
+		panic();
+	}
+
+	/* Update the runtime model and state of the partition. */
+	ec->rt_model = RT_MODEL_INTR;
+	ec->rt_state = RT_STATE_RUNNING;
+
+	VERBOSE("SP (0x%x) interrupt start on core%u.\n", sp->sp_id, linear_id);
+
+	/*
+	 * Forward the interrupt to the S-EL1 SP. The interrupt ID is not
+	 * populated as the SP can determine this by itself.
+	 */
+	return spmd_smc_switch_state(FFA_INTERRUPT, false,
+				     FFA_PARAM_MBZ, FFA_PARAM_MBZ,
+				     FFA_PARAM_MBZ, FFA_PARAM_MBZ,
+				     handle);
+}

diff --git a/services/std_svc/spm/el3_spmc/spmc_pm.c b/services/std_svc/spm/el3_spmc/spmc_pm.c
new file mode 100644
index 0000000..d25344c
--- /dev/null
+++ b/services/std_svc/spm/el3_spmc/spmc_pm.c

@@ -0,0 +1,283 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/spinlock.h>
+#include <plat/common/common_def.h>
+#include <plat/common/platform.h>
+#include <services/ffa_svc.h>
+#include "spmc.h"
+
+#include <platform_def.h>
+
+/*******************************************************************************
+ * spmc_build_pm_message
+ *
+ * Builds an SPMC to SP direct message request.
+ ******************************************************************************/
+static void spmc_build_pm_message(gp_regs_t *gpregs,
+				  unsigned long long message,
+				  uint8_t pm_msg_type,
+				  uint16_t sp_id)
+{
+	write_ctx_reg(gpregs, CTX_GPREG_X0, FFA_MSG_SEND_DIRECT_REQ_SMC32);
+	write_ctx_reg(gpregs, CTX_GPREG_X1,
+		      (FFA_SPMC_ID << FFA_DIRECT_MSG_SOURCE_SHIFT) |
+		      sp_id);
+	write_ctx_reg(gpregs, CTX_GPREG_X2, FFA_FWK_MSG_BIT |
+		      (pm_msg_type & FFA_FWK_MSG_MASK));
+	write_ctx_reg(gpregs, CTX_GPREG_X3, message);
+}
+
+/*******************************************************************************
+ * This CPU has been turned on. Enter the SP to initialise S-EL1.
+ ******************************************************************************/
+static void spmc_cpu_on_finish_handler(u_register_t unused)
+{
+	struct secure_partition_desc *sp = spmc_get_current_sp_ctx();
+	struct sp_exec_ctx *ec;
+	unsigned int linear_id = plat_my_core_pos();
+	entry_point_info_t sec_ec_ep_info = {0};
+	uint64_t rc;
+
+	/* Sanity check for a NULL pointer dereference. */
+	assert(sp != NULL);
+
+	/* Initialize entry point information for the SP. */
+	SET_PARAM_HEAD(&sec_ec_ep_info, PARAM_EP, VERSION_1,
+		       SECURE | EP_ST_ENABLE);
+
+	/*
+	 * Check if the primary execution context registered an entry point else
+	 * bail out early.
+	 * TODO: Add support for boot reason in manifest to allow jumping to
+	 * entrypoint into the primary execution context.
+	 */
+	if (sp->secondary_ep == 0) {
+		WARN("%s: No secondary ep on core%u\n", __func__, linear_id);
+		return;
+	}
+
+	sec_ec_ep_info.pc = sp->secondary_ep;
+
+	/*
+	 * Setup and initialise the SP execution context on this physical cpu.
+	 */
+	spmc_el1_sp_setup(sp, &sec_ec_ep_info);
+	spmc_sp_common_ep_commit(sp, &sec_ec_ep_info);
+
+	/* Obtain a reference to the SP execution context. */
+	ec = spmc_get_sp_ec(sp);
+
+	/*
+	 * TODO: Should we do some PM related state tracking of the SP execution
+	 * context here?
+	 */
+
+	/* Update the runtime model and state of the partition. */
+	ec->rt_model = RT_MODEL_INIT;
+	ec->rt_state = RT_STATE_RUNNING;
+
+	INFO("SP (0x%x) init start on core%u.\n", sp->sp_id, linear_id);
+
+	rc = spmc_sp_synchronous_entry(ec);
+	if (rc != 0ULL) {
+		ERROR("%s failed (%lu) on CPU%u\n", __func__, rc, linear_id);
+	}
+
+	/* Update the runtime state of the partition. */
+	ec->rt_state = RT_STATE_WAITING;
+
+	VERBOSE("CPU %u on!\n", linear_id);
+}
+/*******************************************************************************
+ * Helper function to send a FF-A power management message to an SP.
+ ******************************************************************************/
+static int32_t spmc_send_pm_msg(uint8_t pm_msg_type,
+				unsigned long long psci_event)
+{
+	struct secure_partition_desc *sp = spmc_get_current_sp_ctx();
+	struct sp_exec_ctx *ec;
+	gp_regs_t *gpregs_ctx;
+	unsigned int linear_id = plat_my_core_pos();
+	u_register_t resp;
+	uint64_t rc;
+
+	/* Obtain a reference to the SP execution context. */
+	ec = spmc_get_sp_ec(sp);
+
+	/*
+	 * TODO: Should we do some PM related state tracking of the SP execution
+	 * context here?
+	 */
+
+	/*
+	 * Build an SPMC to SP direct message request.
+	 * Note that x4-x6 should be populated with the original PSCI arguments.
+	 */
+	spmc_build_pm_message(get_gpregs_ctx(&ec->cpu_ctx),
+			      psci_event,
+			      pm_msg_type,
+			      sp->sp_id);
+
+	/* Sanity check partition state. */
+	assert(ec->rt_state == RT_STATE_WAITING);
+
+	/* Update the runtime model and state of the partition. */
+	ec->rt_model = RT_MODEL_DIR_REQ;
+	ec->rt_state = RT_STATE_RUNNING;
+
+	rc = spmc_sp_synchronous_entry(ec);
+	if (rc != 0ULL) {
+		ERROR("%s failed (%lu) on CPU%u.\n", __func__, rc, linear_id);
+		assert(false);
+		return -EINVAL;
+	}
+
+	/*
+	 * Validate we receive an expected response from the SP.
+	 * TODO: We don't currently support aborting an SP in the scenario
+	 * where it is misbehaving so assert these conditions are not
+	 * met for now.
+	 */
+	gpregs_ctx = get_gpregs_ctx(&ec->cpu_ctx);
+
+	/* Expect a direct message response from the SP. */
+	resp = read_ctx_reg(gpregs_ctx, CTX_GPREG_X0);
+	if (resp != FFA_MSG_SEND_DIRECT_RESP_SMC32) {
+		ERROR("%s invalid SP response (%lx).\n", __func__, resp);
+		assert(false);
+		return -EINVAL;
+	}
+
+	/* Ensure the sender and receiver are populated correctly. */
+	resp = read_ctx_reg(gpregs_ctx, CTX_GPREG_X1);
+	if (!(ffa_endpoint_source(resp) == sp->sp_id &&
+	      ffa_endpoint_destination(resp) == FFA_SPMC_ID)) {
+		ERROR("%s invalid src/dst response (%lx).\n", __func__, resp);
+		assert(false);
+		return -EINVAL;
+	}
+
+	/* Expect a PM message response from the SP. */
+	resp = read_ctx_reg(gpregs_ctx, CTX_GPREG_X2);
+	if ((resp & FFA_FWK_MSG_BIT) == 0U ||
+	    ((resp & FFA_FWK_MSG_MASK) != FFA_PM_MSG_PM_RESP)) {
+		ERROR("%s invalid PM response (%lx).\n", __func__, resp);
+		assert(false);
+		return -EINVAL;
+	}
+
+	/* Update the runtime state of the partition. */
+	ec->rt_state = RT_STATE_WAITING;
+
+	/* Return the status code returned by the SP */
+	return read_ctx_reg(gpregs_ctx, CTX_GPREG_X3);
+}
+
+/*******************************************************************************
+ * spmc_cpu_suspend_finish_handler
+ ******************************************************************************/
+static void spmc_cpu_suspend_finish_handler(u_register_t unused)
+{
+	struct secure_partition_desc *sp = spmc_get_current_sp_ctx();
+	unsigned int linear_id = plat_my_core_pos();
+	int32_t rc;
+
+	/* Sanity check for a NULL pointer dereference. */
+	assert(sp != NULL);
+
+	/*
+	 * Check if the SP has subscribed for this power management message.
+	 * If not then we don't have anything else to do here.
+	 */
+	if ((sp->pwr_mgmt_msgs & FFA_PM_MSG_SUB_CPU_SUSPEND_RESUME) == 0U) {
+		goto exit;
+	}
+
+	rc = spmc_send_pm_msg(FFA_PM_MSG_WB_REQ, FFA_WB_TYPE_NOTS2RAM);
+	if (rc < 0) {
+		ERROR("%s failed (%d) on CPU%u\n", __func__, rc, linear_id);
+		return;
+	}
+
+exit:
+	VERBOSE("CPU %u resumed!\n", linear_id);
+}
+
+/*******************************************************************************
+ * spmc_cpu_suspend_handler
+ ******************************************************************************/
+static void spmc_cpu_suspend_handler(u_register_t unused)
+{
+	struct secure_partition_desc *sp = spmc_get_current_sp_ctx();
+	unsigned int linear_id = plat_my_core_pos();
+	int32_t rc;
+
+	/* Sanity check for a NULL pointer dereference. */
+	assert(sp != NULL);
+
+	/*
+	 * Check if the SP has subscribed for this power management message.
+	 * If not then we don't have anything else to do here.
+	 */
+	if ((sp->pwr_mgmt_msgs & FFA_PM_MSG_SUB_CPU_SUSPEND) == 0U) {
+		goto exit;
+	}
+
+	rc = spmc_send_pm_msg(FFA_FWK_MSG_PSCI, PSCI_CPU_SUSPEND_AARCH64);
+	if (rc < 0) {
+		ERROR("%s failed (%d) on CPU%u\n", __func__, rc, linear_id);
+		return;
+	}
+exit:
+	VERBOSE("CPU %u suspend!\n", linear_id);
+}
+
+/*******************************************************************************
+ * spmc_cpu_off_handler
+ ******************************************************************************/
+static int32_t spmc_cpu_off_handler(u_register_t unused)
+{
+	struct secure_partition_desc *sp = spmc_get_current_sp_ctx();
+	unsigned int linear_id = plat_my_core_pos();
+	int32_t ret = 0;
+
+	/* Sanity check for a NULL pointer dereference. */
+	assert(sp != NULL);
+
+	/*
+	 * Check if the SP has subscribed for this power management message.
+	 * If not then we don't have anything else to do here.
+	 */
+	if ((sp->pwr_mgmt_msgs & FFA_PM_MSG_SUB_CPU_OFF) == 0U) {
+		goto exit;
+	}
+
+	ret = spmc_send_pm_msg(FFA_FWK_MSG_PSCI, PSCI_CPU_OFF);
+	if (ret < 0) {
+		ERROR("%s failed (%d) on CPU%u\n", __func__, ret, linear_id);
+		return ret;
+	}
+
+exit:
+	VERBOSE("CPU %u off!\n", linear_id);
+	return ret;
+}
+
+/*******************************************************************************
+ * Structure populated by the SPM Core to perform any bookkeeping before
+ * PSCI executes a power mgmt. operation.
+ ******************************************************************************/
+const spd_pm_ops_t spmc_pm = {
+	.svc_on_finish = spmc_cpu_on_finish_handler,
+	.svc_off = spmc_cpu_off_handler,
+	.svc_suspend = spmc_cpu_suspend_handler,
+	.svc_suspend_finish = spmc_cpu_suspend_finish_handler
+};

diff --git a/services/std_svc/spm/el3_spmc/spmc_setup.c b/services/std_svc/spm/el3_spmc/spmc_setup.c
index 7b23c9e..8ebae28 100644
--- a/services/std_svc/spm/el3_spmc/spmc_setup.c
+++ b/services/std_svc/spm/el3_spmc/spmc_setup.c

@@ -10,19 +10,139 @@
 #include <arch.h>
 #include <arch_helpers.h>
 #include <common/debug.h>
+#include <common/fdt_wrappers.h>
 #include <context.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/utils.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
+#include <libfdt.h>
 #include <plat/common/common_def.h>
 #include <plat/common/platform.h>
 #include <services/ffa_svc.h>
 #include "spm_common.h"
 #include "spmc.h"
+#include <tools_share/firmware_image_package.h>
 
 #include <platform_def.h>
 
 /*
+ * Statically allocate a page of memory for passing boot information to an SP.
+ */
+static uint8_t ffa_boot_info_mem[PAGE_SIZE] __aligned(PAGE_SIZE);
+
+/*
+ * This function creates a initialization descriptor in the memory reserved
+ * for passing boot information to an SP. It then copies the partition manifest
+ * into this region and ensures that its reference in the initialization
+ * descriptor is updated.
+ */
+static void spmc_create_boot_info(entry_point_info_t *ep_info,
+				  struct secure_partition_desc *sp)
+{
+	struct ffa_boot_info_header *boot_header;
+	struct ffa_boot_info_desc *boot_descriptor;
+	uintptr_t manifest_addr;
+
+	/*
+	 * Calculate the maximum size of the manifest that can be accommodated
+	 * in the boot information memory region.
+	 */
+	const unsigned int
+	max_manifest_sz = sizeof(ffa_boot_info_mem) -
+			  (sizeof(struct ffa_boot_info_header) +
+			   sizeof(struct ffa_boot_info_desc));
+
+	/*
+	 * The current implementation only supports the FF-A v1.1
+	 * implementation of the boot protocol, therefore check
+	 * that a v1.0 SP has not requested use of the protocol.
+	 */
+	if (sp->ffa_version == MAKE_FFA_VERSION(1, 0)) {
+		ERROR("FF-A boot protocol not supported for v1.0 clients\n");
+		return;
+	}
+
+	/*
+	 * Check if the manifest will fit into the boot info memory region else
+	 * bail.
+	 */
+	if (ep_info->args.arg1 > max_manifest_sz) {
+		WARN("Unable to copy manifest into boot information. ");
+		WARN("Max sz = %u bytes. Manifest sz = %lu bytes\n",
+		     max_manifest_sz, ep_info->args.arg1);
+		return;
+	}
+
+	/* Zero the memory region before populating. */
+	memset(ffa_boot_info_mem, 0, PAGE_SIZE);
+
+	/*
+	 * Populate the ffa_boot_info_header at the start of the boot info
+	 * region.
+	 */
+	boot_header = (struct ffa_boot_info_header *) ffa_boot_info_mem;
+
+	/* Position the ffa_boot_info_desc after the ffa_boot_info_header. */
+	boot_header->offset_boot_info_desc =
+					sizeof(struct ffa_boot_info_header);
+	boot_descriptor = (struct ffa_boot_info_desc *)
+			  (ffa_boot_info_mem +
+			   boot_header->offset_boot_info_desc);
+
+	/*
+	 * We must use the FF-A version coresponding to the version implemented
+	 * by the SP. Currently this can only be v1.1.
+	 */
+	boot_header->version = sp->ffa_version;
+
+	/* Populate the boot information header. */
+	boot_header->size_boot_info_desc = sizeof(struct ffa_boot_info_desc);
+
+	/* Set the signature "0xFFA". */
+	boot_header->signature = FFA_INIT_DESC_SIGNATURE;
+
+	/* Set the count. Currently 1 since only the manifest is specified. */
+	boot_header->count_boot_info_desc = 1;
+
+	/* Populate the boot information descriptor for the manifest. */
+	boot_descriptor->type =
+		FFA_BOOT_INFO_TYPE(FFA_BOOT_INFO_TYPE_STD) |
+		FFA_BOOT_INFO_TYPE_ID(FFA_BOOT_INFO_TYPE_ID_FDT);
+
+	boot_descriptor->flags =
+		FFA_BOOT_INFO_FLAG_NAME(FFA_BOOT_INFO_FLAG_NAME_UUID) |
+		FFA_BOOT_INFO_FLAG_CONTENT(FFA_BOOT_INFO_FLAG_CONTENT_ADR);
+
+	/*
+	 * Copy the manifest into boot info region after the boot information
+	 * descriptor.
+	 */
+	boot_descriptor->size_boot_info = (uint32_t) ep_info->args.arg1;
+
+	manifest_addr = (uintptr_t) (ffa_boot_info_mem +
+				     boot_header->offset_boot_info_desc +
+				     boot_header->size_boot_info_desc);
+
+	memcpy((void *) manifest_addr, (void *) ep_info->args.arg0,
+	       boot_descriptor->size_boot_info);
+
+	boot_descriptor->content = manifest_addr;
+
+	/* Calculate the size of the total boot info blob. */
+	boot_header->size_boot_info_blob = boot_header->offset_boot_info_desc +
+					   boot_descriptor->size_boot_info +
+					   (boot_header->count_boot_info_desc *
+					    boot_header->size_boot_info_desc);
+
+	INFO("SP boot info @ 0x%lx, size: %u bytes.\n",
+	     (uintptr_t) ffa_boot_info_mem,
+	     boot_header->size_boot_info_blob);
+	INFO("SP manifest @ 0x%lx, size: %u bytes.\n",
+	     boot_descriptor->content,
+	     boot_descriptor->size_boot_info);
+}
+
+/*
  * We are assuming that the index of the execution
  * context used is the linear index of the current physical cpu.
  */
@@ -44,6 +164,12 @@
 				DISABLE_ALL_EXCEPTIONS);
 
 	/*
+	 * TF-A Implementation defined behaviour to provide the linear
+	 * core ID in the x4 register.
+	 */
+	ep_info->args.arg4 = (uintptr_t) plat_my_core_pos();
+
+	/*
 	 * Check whether setup is being performed for the primary or a secondary
 	 * execution context. In the latter case, indicate to the SP that this
 	 * is a warm boot.
@@ -62,7 +188,8 @@
 
 /* Common initialisation for all SPs. */
 void spmc_sp_common_setup(struct secure_partition_desc *sp,
-			  entry_point_info_t *ep_info)
+			  entry_point_info_t *ep_info,
+			  int32_t boot_info_reg)
 {
 	uint16_t sp_id;
 
@@ -90,11 +217,50 @@
 	 */
 	assert(sp->runtime_el == S_EL1);
 
-	/*
-	 * Clear the general purpose registers. These should be populated as
-	 * required.
-	 */
-	zeromem(&ep_info->args, sizeof(ep_info->args));
+	/* Check if the SP wants to use the FF-A boot protocol. */
+	if (boot_info_reg >= 0) {
+		/*
+		 * Create a boot information descriptor and copy the partition
+		 * manifest into the reserved memory region for consumption by
+		 * the SP.
+		 */
+		spmc_create_boot_info(ep_info, sp);
+
+		/*
+		 * We have consumed what we need from ep args so we can now
+		 * zero them before we start populating with new information
+		 * specifically for the SP.
+		 */
+		zeromem(&ep_info->args, sizeof(ep_info->args));
+
+		/*
+		 * Pass the address of the boot information in the
+		 * boot_info_reg.
+		 */
+		switch (boot_info_reg) {
+		case 0:
+			ep_info->args.arg0 = (uintptr_t) ffa_boot_info_mem;
+			break;
+		case 1:
+			ep_info->args.arg1 = (uintptr_t) ffa_boot_info_mem;
+			break;
+		case 2:
+			ep_info->args.arg2 = (uintptr_t) ffa_boot_info_mem;
+			break;
+		case 3:
+			ep_info->args.arg3 = (uintptr_t) ffa_boot_info_mem;
+			break;
+		default:
+			ERROR("Invalid value for \"gp-register-num\" %d.\n",
+			      boot_info_reg);
+		}
+	} else {
+		/*
+		 * We don't need any of the information that was populated
+		 * in ep_args so we can clear them.
+		 */
+		zeromem(&ep_info->args, sizeof(ep_info->args));
+	}
 }
 
 /*

diff --git a/services/std_svc/spm/el3_spmc/spmc_shared_mem.c b/services/std_svc/spm/el3_spmc/spmc_shared_mem.c
new file mode 100644
index 0000000..4a24108
--- /dev/null
+++ b/services/std_svc/spm/el3_spmc/spmc_shared_mem.c

@@ -0,0 +1,1812 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <assert.h>
+#include <errno.h>
+
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <lib/object_pool.h>
+#include <lib/spinlock.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <services/ffa_svc.h>
+#include "spmc.h"
+#include "spmc_shared_mem.h"
+
+#include <platform_def.h>
+
+/**
+ * struct spmc_shmem_obj - Shared memory object.
+ * @desc_size:      Size of @desc.
+ * @desc_filled:    Size of @desc already received.
+ * @in_use:         Number of clients that have called ffa_mem_retrieve_req
+ *                  without a matching ffa_mem_relinquish call.
+ * @desc:           FF-A memory region descriptor passed in ffa_mem_share.
+ */
+struct spmc_shmem_obj {
+	size_t desc_size;
+	size_t desc_filled;
+	size_t in_use;
+	struct ffa_mtd desc;
+};
+
+/*
+ * Declare our data structure to store the metadata of memory share requests.
+ * The main datastore is allocated on a per platform basis to ensure enough
+ * storage can be made available.
+ * The address of the data store will be populated by the SPMC during its
+ * initialization.
+ */
+
+struct spmc_shmem_obj_state spmc_shmem_obj_state = {
+	/* Set start value for handle so top 32 bits are needed quickly. */
+	.next_handle = 0xffffffc0U,
+};
+
+/**
+ * spmc_shmem_obj_size - Convert from descriptor size to object size.
+ * @desc_size:  Size of struct ffa_memory_region_descriptor object.
+ *
+ * Return: Size of struct spmc_shmem_obj object.
+ */
+static size_t spmc_shmem_obj_size(size_t desc_size)
+{
+	return desc_size + offsetof(struct spmc_shmem_obj, desc);
+}
+
+/**
+ * spmc_shmem_obj_alloc - Allocate struct spmc_shmem_obj.
+ * @state:      Global state.
+ * @desc_size:  Size of struct ffa_memory_region_descriptor object that
+ *              allocated object will hold.
+ *
+ * Return: Pointer to newly allocated object, or %NULL if there not enough space
+ *         left. The returned pointer is only valid while @state is locked, to
+ *         used it again after unlocking @state, spmc_shmem_obj_lookup must be
+ *         called.
+ */
+static struct spmc_shmem_obj *
+spmc_shmem_obj_alloc(struct spmc_shmem_obj_state *state, size_t desc_size)
+{
+	struct spmc_shmem_obj *obj;
+	size_t free = state->data_size - state->allocated;
+
+	if (state->data == NULL) {
+		ERROR("Missing shmem datastore!\n");
+		return NULL;
+	}
+
+	if (spmc_shmem_obj_size(desc_size) > free) {
+		WARN("%s(0x%zx) failed, free 0x%zx\n",
+		     __func__, desc_size, free);
+		return NULL;
+	}
+	obj = (struct spmc_shmem_obj *)(state->data + state->allocated);
+	obj->desc = (struct ffa_mtd) {0};
+	obj->desc_size = desc_size;
+	obj->desc_filled = 0;
+	obj->in_use = 0;
+	state->allocated += spmc_shmem_obj_size(desc_size);
+	return obj;
+}
+
+/**
+ * spmc_shmem_obj_free - Free struct spmc_shmem_obj.
+ * @state:      Global state.
+ * @obj:        Object to free.
+ *
+ * Release memory used by @obj. Other objects may move, so on return all
+ * pointers to struct spmc_shmem_obj object should be considered invalid, not
+ * just @obj.
+ *
+ * The current implementation always compacts the remaining objects to simplify
+ * the allocator and to avoid fragmentation.
+ */
+
+static void spmc_shmem_obj_free(struct spmc_shmem_obj_state *state,
+				  struct spmc_shmem_obj *obj)
+{
+	size_t free_size = spmc_shmem_obj_size(obj->desc_size);
+	uint8_t *shift_dest = (uint8_t *)obj;
+	uint8_t *shift_src = shift_dest + free_size;
+	size_t shift_size = state->allocated - (shift_src - state->data);
+
+	if (shift_size != 0U) {
+		memmove(shift_dest, shift_src, shift_size);
+	}
+	state->allocated -= free_size;
+}
+
+/**
+ * spmc_shmem_obj_lookup - Lookup struct spmc_shmem_obj by handle.
+ * @state:      Global state.
+ * @handle:     Unique handle of object to return.
+ *
+ * Return: struct spmc_shmem_obj_state object with handle matching @handle.
+ *         %NULL, if not object in @state->data has a matching handle.
+ */
+static struct spmc_shmem_obj *
+spmc_shmem_obj_lookup(struct spmc_shmem_obj_state *state, uint64_t handle)
+{
+	uint8_t *curr = state->data;
+
+	while (curr - state->data < state->allocated) {
+		struct spmc_shmem_obj *obj = (struct spmc_shmem_obj *)curr;
+
+		if (obj->desc.handle == handle) {
+			return obj;
+		}
+		curr += spmc_shmem_obj_size(obj->desc_size);
+	}
+	return NULL;
+}
+
+/**
+ * spmc_shmem_obj_get_next - Get the next memory object from an offset.
+ * @offset:     Offset used to track which objects have previously been
+ *              returned.
+ *
+ * Return: the next struct spmc_shmem_obj_state object from the provided
+ *	   offset.
+ *	   %NULL, if there are no more objects.
+ */
+static struct spmc_shmem_obj *
+spmc_shmem_obj_get_next(struct spmc_shmem_obj_state *state, size_t *offset)
+{
+	uint8_t *curr = state->data + *offset;
+
+	if (curr - state->data < state->allocated) {
+		struct spmc_shmem_obj *obj = (struct spmc_shmem_obj *)curr;
+
+		*offset += spmc_shmem_obj_size(obj->desc_size);
+
+		return obj;
+	}
+	return NULL;
+}
+
+/*******************************************************************************
+ * FF-A memory descriptor helper functions.
+ ******************************************************************************/
+/**
+ * spmc_shmem_obj_get_emad - Get the emad from a given index depending on the
+ *                           clients FF-A version.
+ * @desc:         The memory transaction descriptor.
+ * @index:        The index of the emad element to be accessed.
+ * @ffa_version:  FF-A version of the provided structure.
+ * @emad_size:    Will be populated with the size of the returned emad
+ *                descriptor.
+ * Return: A pointer to the requested emad structure.
+ */
+static void *
+spmc_shmem_obj_get_emad(const struct ffa_mtd *desc, uint32_t index,
+			uint32_t ffa_version, size_t *emad_size)
+{
+	uint8_t *emad;
+	/*
+	 * If the caller is using FF-A v1.0 interpret the descriptor as a v1.0
+	 * format, otherwise assume it is a v1.1 format.
+	 */
+	if (ffa_version == MAKE_FFA_VERSION(1, 0)) {
+		/* Cast our descriptor to the v1.0 format. */
+		struct ffa_mtd_v1_0 *mtd_v1_0 =
+					(struct ffa_mtd_v1_0 *) desc;
+		emad = (uint8_t *)  &(mtd_v1_0->emad);
+		*emad_size = sizeof(struct ffa_emad_v1_0);
+	} else {
+		if (!is_aligned(desc->emad_offset, 16)) {
+			WARN("Emad offset is not aligned.\n");
+			return NULL;
+		}
+		emad = ((uint8_t *) desc + desc->emad_offset);
+		*emad_size = desc->emad_size;
+	}
+	return (emad + (*emad_size * index));
+}
+
+/**
+ * spmc_shmem_obj_get_comp_mrd - Get comp_mrd from a mtd struct based on the
+ *				 FF-A version of the descriptor.
+ * @obj:    Object containing ffa_memory_region_descriptor.
+ *
+ * Return: struct ffa_comp_mrd object corresponding to the composite memory
+ *	   region descriptor.
+ */
+static struct ffa_comp_mrd *
+spmc_shmem_obj_get_comp_mrd(struct spmc_shmem_obj *obj, uint32_t ffa_version)
+{
+	size_t emad_size;
+	/*
+	 * The comp_mrd_offset field of the emad descriptor remains consistent
+	 * between FF-A versions therefore we can use the v1.0 descriptor here
+	 * in all cases.
+	 */
+	struct ffa_emad_v1_0 *emad = spmc_shmem_obj_get_emad(&obj->desc, 0,
+							     ffa_version,
+							     &emad_size);
+	/* Ensure the emad array was found. */
+	if (emad == NULL) {
+		return NULL;
+	}
+
+	/* Ensure the composite descriptor offset is aligned. */
+	if (!is_aligned(emad->comp_mrd_offset, 8)) {
+		WARN("Unaligned composite memory region descriptor offset.\n");
+		return NULL;
+	}
+
+	return (struct ffa_comp_mrd *)
+	       ((uint8_t *)(&obj->desc) + emad->comp_mrd_offset);
+}
+
+/**
+ * spmc_shmem_obj_ffa_constituent_size - Calculate variable size part of obj.
+ * @obj:    Object containing ffa_memory_region_descriptor.
+ *
+ * Return: Size of ffa_constituent_memory_region_descriptors in @obj.
+ */
+static size_t
+spmc_shmem_obj_ffa_constituent_size(struct spmc_shmem_obj *obj,
+				    uint32_t ffa_version)
+{
+	struct ffa_comp_mrd *comp_mrd;
+
+	comp_mrd = spmc_shmem_obj_get_comp_mrd(obj, ffa_version);
+	if (comp_mrd == NULL) {
+		return 0;
+	}
+	return comp_mrd->address_range_count * sizeof(struct ffa_cons_mrd);
+}
+
+/*
+ * Compare two memory regions to determine if any range overlaps with another
+ * ongoing memory transaction.
+ */
+static bool
+overlapping_memory_regions(struct ffa_comp_mrd *region1,
+			   struct ffa_comp_mrd *region2)
+{
+	uint64_t region1_start;
+	uint64_t region1_size;
+	uint64_t region1_end;
+	uint64_t region2_start;
+	uint64_t region2_size;
+	uint64_t region2_end;
+
+	assert(region1 != NULL);
+	assert(region2 != NULL);
+
+	if (region1 == region2) {
+		return true;
+	}
+
+	/*
+	 * Check each memory region in the request against existing
+	 * transactions.
+	 */
+	for (size_t i = 0; i < region1->address_range_count; i++) {
+
+		region1_start = region1->address_range_array[i].address;
+		region1_size =
+			region1->address_range_array[i].page_count *
+			PAGE_SIZE_4KB;
+		region1_end = region1_start + region1_size;
+
+		for (size_t j = 0; j < region2->address_range_count; j++) {
+
+			region2_start = region2->address_range_array[j].address;
+			region2_size =
+				region2->address_range_array[j].page_count *
+				PAGE_SIZE_4KB;
+			region2_end = region2_start + region2_size;
+
+			if ((region1_start >= region2_start &&
+			     region1_start < region2_end) ||
+			    (region1_end > region2_start
+			     && region1_end < region2_end)) {
+				WARN("Overlapping mem regions 0x%lx-0x%lx & 0x%lx-0x%lx\n",
+				     region1_start, region1_end,
+				     region2_start, region2_end);
+				return true;
+			}
+		}
+	}
+	return false;
+}
+
+/*******************************************************************************
+ * FF-A v1.0 Memory Descriptor Conversion Helpers.
+ ******************************************************************************/
+/**
+ * spmc_shm_get_v1_1_descriptor_size - Calculate the required size for a v1.1
+ *                                     converted descriptor.
+ * @orig:       The original v1.0 memory transaction descriptor.
+ * @desc_size:  The size of the original v1.0 memory transaction descriptor.
+ *
+ * Return: the size required to store the descriptor store in the v1.1 format.
+ */
+static size_t
+spmc_shm_get_v1_1_descriptor_size(struct ffa_mtd_v1_0 *orig, size_t desc_size)
+{
+	size_t size = 0;
+	struct ffa_comp_mrd *mrd;
+	struct ffa_emad_v1_0 *emad_array = orig->emad;
+
+	/* Get the size of the v1.1 descriptor. */
+	size += sizeof(struct ffa_mtd);
+
+	/* Add the size of the emad descriptors. */
+	size += orig->emad_count * sizeof(struct ffa_emad_v1_0);
+
+	/* Add the size of the composite mrds. */
+	size += sizeof(struct ffa_comp_mrd);
+
+	/* Add the size of the constituent mrds. */
+	mrd = (struct ffa_comp_mrd *) ((uint8_t *) orig +
+	      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)) {
+		return 0;
+	}
+	size += mrd->address_range_count * sizeof(struct ffa_cons_mrd);
+
+	return size;
+}
+
+/**
+ * spmc_shm_get_v1_0_descriptor_size - Calculate the required size for a v1.0
+ *                                     converted descriptor.
+ * @orig:       The original v1.1 memory transaction descriptor.
+ * @desc_size:  The size of the original v1.1 memory transaction descriptor.
+ *
+ * Return: the size required to store the descriptor store in the v1.0 format.
+ */
+static size_t
+spmc_shm_get_v1_0_descriptor_size(struct ffa_mtd *orig, size_t desc_size)
+{
+	size_t size = 0;
+	struct ffa_comp_mrd *mrd;
+	struct ffa_emad_v1_0 *emad_array = (struct ffa_emad_v1_0 *)
+					   ((uint8_t *) orig +
+					    orig->emad_offset);
+
+	/* Get the size of the v1.0 descriptor. */
+	size += sizeof(struct ffa_mtd_v1_0);
+
+	/* Add the size of the v1.0 emad descriptors. */
+	size += orig->emad_count * sizeof(struct ffa_emad_v1_0);
+
+	/* Add the size of the composite mrds. */
+	size += sizeof(struct ffa_comp_mrd);
+
+	/* Add the size of the constituent mrds. */
+	mrd = (struct ffa_comp_mrd *) ((uint8_t *) orig +
+	      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)) {
+		return 0;
+	}
+	size += mrd->address_range_count * sizeof(struct ffa_cons_mrd);
+
+	return size;
+}
+
+/**
+ * spmc_shm_convert_shmem_obj_from_v1_0 - Converts a given v1.0 memory object.
+ * @out_obj:	The shared memory object to populate the converted descriptor.
+ * @orig:	The shared memory object containing the v1.0 descriptor.
+ *
+ * Return: true if the conversion is successful else false.
+ */
+static bool
+spmc_shm_convert_shmem_obj_from_v1_0(struct spmc_shmem_obj *out_obj,
+				     struct spmc_shmem_obj *orig)
+{
+	struct ffa_mtd_v1_0 *mtd_orig = (struct ffa_mtd_v1_0 *) &orig->desc;
+	struct ffa_mtd *out = &out_obj->desc;
+	struct ffa_emad_v1_0 *emad_array_in;
+	struct ffa_emad_v1_0 *emad_array_out;
+	struct ffa_comp_mrd *mrd_in;
+	struct ffa_comp_mrd *mrd_out;
+
+	size_t mrd_in_offset;
+	size_t mrd_out_offset;
+	size_t mrd_size = 0;
+
+	/* Populate the new descriptor format from the v1.0 struct. */
+	out->sender_id = mtd_orig->sender_id;
+	out->memory_region_attributes = mtd_orig->memory_region_attributes;
+	out->flags = mtd_orig->flags;
+	out->handle = mtd_orig->handle;
+	out->tag = mtd_orig->tag;
+	out->emad_count = mtd_orig->emad_count;
+	out->emad_size = sizeof(struct ffa_emad_v1_0);
+
+	/*
+	 * We will locate the emad descriptors directly after the ffa_mtd
+	 * struct. This will be 8-byte aligned.
+	 */
+	out->emad_offset = sizeof(struct ffa_mtd);
+
+	emad_array_in = mtd_orig->emad;
+	emad_array_out = (struct ffa_emad_v1_0 *)
+			 ((uint8_t *) out + out->emad_offset);
+
+	/* Copy across the emad structs. */
+	for (unsigned int i = 0U; i < out->emad_count; i++) {
+		memcpy(&emad_array_out[i], &emad_array_in[i],
+		       sizeof(struct ffa_emad_v1_0));
+	}
+
+	/* Place the mrd descriptors after the end of the emad descriptors.*/
+	mrd_in_offset = emad_array_in->comp_mrd_offset;
+	mrd_out_offset = out->emad_offset + (out->emad_size * out->emad_count);
+	mrd_out = (struct ffa_comp_mrd *) ((uint8_t *) out + mrd_out_offset);
+
+	/* Add the size of the composite memory region descriptor. */
+	mrd_size += sizeof(struct ffa_comp_mrd);
+
+	/* Find the mrd descriptor. */
+	mrd_in = (struct ffa_comp_mrd *) ((uint8_t *) mtd_orig + mrd_in_offset);
+
+	/* Add the size of the constituent memory region descriptors. */
+	mrd_size += mrd_in->address_range_count * sizeof(struct ffa_cons_mrd);
+
+	/*
+	 * Update the offset in the emads by the delta between the input and
+	 * output addresses.
+	 */
+	for (unsigned int i = 0U; i < out->emad_count; i++) {
+		emad_array_out[i].comp_mrd_offset =
+			emad_array_in[i].comp_mrd_offset +
+			(mrd_out_offset - mrd_in_offset);
+	}
+
+	/* Verify that we stay within bound of the memory descriptors. */
+	if ((uintptr_t)((uint8_t *) mrd_in + mrd_size) >
+	     (uintptr_t)((uint8_t *) mtd_orig + orig->desc_size) ||
+	    ((uintptr_t)((uint8_t *) mrd_out + mrd_size) >
+	     (uintptr_t)((uint8_t *) out + out_obj->desc_size))) {
+		ERROR("%s: Invalid mrd structure.\n", __func__);
+		return false;
+	}
+
+	/* Copy the mrd descriptors directly. */
+	memcpy(mrd_out, mrd_in, mrd_size);
+
+	return true;
+}
+
+/**
+ * spmc_shm_convert_mtd_to_v1_0 - Converts a given v1.1 memory object to
+ *                                v1.0 memory object.
+ * @out_obj:    The shared memory object to populate the v1.0 descriptor.
+ * @orig:       The shared memory object containing the v1.1 descriptor.
+ *
+ * Return: true if the conversion is successful else false.
+ */
+static bool
+spmc_shm_convert_mtd_to_v1_0(struct spmc_shmem_obj *out_obj,
+			     struct spmc_shmem_obj *orig)
+{
+	struct ffa_mtd *mtd_orig = &orig->desc;
+	struct ffa_mtd_v1_0 *out = (struct ffa_mtd_v1_0 *) &out_obj->desc;
+	struct ffa_emad_v1_0 *emad_in;
+	struct ffa_emad_v1_0 *emad_array_in;
+	struct ffa_emad_v1_0 *emad_array_out;
+	struct ffa_comp_mrd *mrd_in;
+	struct ffa_comp_mrd *mrd_out;
+
+	size_t mrd_in_offset;
+	size_t mrd_out_offset;
+	size_t emad_out_array_size;
+	size_t mrd_size = 0;
+
+	/* Populate the v1.0 descriptor format from the v1.1 struct. */
+	out->sender_id = mtd_orig->sender_id;
+	out->memory_region_attributes = mtd_orig->memory_region_attributes;
+	out->flags = mtd_orig->flags;
+	out->handle = mtd_orig->handle;
+	out->tag = mtd_orig->tag;
+	out->emad_count = mtd_orig->emad_count;
+
+	/* Determine the location of the emad array in both descriptors. */
+	emad_array_in = (struct ffa_emad_v1_0 *)
+			((uint8_t *) mtd_orig + mtd_orig->emad_offset);
+	emad_array_out = out->emad;
+
+	/* Copy across the emad structs. */
+	emad_in = emad_array_in;
+	for (unsigned int i = 0U; i < out->emad_count; i++) {
+		memcpy(&emad_array_out[i], emad_in,
+		       sizeof(struct ffa_emad_v1_0));
+
+		emad_in +=  mtd_orig->emad_size;
+	}
+
+	/* Place the mrd descriptors after the end of the emad descriptors. */
+	emad_out_array_size = sizeof(struct ffa_emad_v1_0) * out->emad_count;
+
+	mrd_out_offset =  (uint8_t *) out->emad - (uint8_t *) out +
+			  emad_out_array_size;
+
+	mrd_out = (struct ffa_comp_mrd *) ((uint8_t *) out + mrd_out_offset);
+
+	mrd_in_offset = mtd_orig->emad_offset +
+			(mtd_orig->emad_size * mtd_orig->emad_count);
+
+	/* Add the size of the composite memory region descriptor. */
+	mrd_size += sizeof(struct ffa_comp_mrd);
+
+	/* Find the mrd descriptor. */
+	mrd_in = (struct ffa_comp_mrd *) ((uint8_t *) mtd_orig + mrd_in_offset);
+
+	/* Add the size of the constituent memory region descriptors. */
+	mrd_size += mrd_in->address_range_count * sizeof(struct ffa_cons_mrd);
+
+	/*
+	 * Update the offset in the emads by the delta between the input and
+	 * output addresses.
+	 */
+	emad_in = emad_array_in;
+
+	for (unsigned int i = 0U; i < out->emad_count; i++) {
+		emad_array_out[i].comp_mrd_offset = emad_in->comp_mrd_offset +
+						    (mrd_out_offset -
+						     mrd_in_offset);
+		emad_in +=  mtd_orig->emad_size;
+	}
+
+	/* Verify that we stay within bound of the memory descriptors. */
+	if ((uintptr_t)((uint8_t *) mrd_in + mrd_size) >
+	     (uintptr_t)((uint8_t *) mtd_orig + orig->desc_size) ||
+	    ((uintptr_t)((uint8_t *) mrd_out + mrd_size) >
+	     (uintptr_t)((uint8_t *) out + out_obj->desc_size))) {
+		ERROR("%s: Invalid mrd structure.\n", __func__);
+		return false;
+	}
+
+	/* Copy the mrd descriptors directly. */
+	memcpy(mrd_out, mrd_in, mrd_size);
+
+	return true;
+}
+
+/**
+ * spmc_populate_ffa_v1_0_descriptor - Converts a given v1.1 memory object to
+ *                                     the v1.0 format and populates the
+ *                                     provided buffer.
+ * @dst:	    Buffer to populate v1.0 ffa_memory_region_descriptor.
+ * @orig_obj:	    Object containing v1.1 ffa_memory_region_descriptor.
+ * @buf_size:	    Size of the buffer to populate.
+ * @offset:	    The offset of the converted descriptor to copy.
+ * @copy_size:	    Will be populated with the number of bytes copied.
+ * @out_desc_size:  Will be populated with the total size of the v1.0
+ *                  descriptor.
+ *
+ * Return: 0 if conversion and population succeeded.
+ * Note: This function invalidates the reference to @orig therefore
+ * `spmc_shmem_obj_lookup` must be called if further usage is required.
+ */
+static uint32_t
+spmc_populate_ffa_v1_0_descriptor(void *dst, struct spmc_shmem_obj *orig_obj,
+				 size_t buf_size, size_t offset,
+				 size_t *copy_size, size_t *v1_0_desc_size)
+{
+		struct spmc_shmem_obj *v1_0_obj;
+
+		/* Calculate the size that the v1.0 descriptor will require. */
+		*v1_0_desc_size = spmc_shm_get_v1_0_descriptor_size(
+					&orig_obj->desc, orig_obj->desc_size);
+
+		if (*v1_0_desc_size == 0) {
+			ERROR("%s: cannot determine size of descriptor.\n",
+			      __func__);
+			return FFA_ERROR_INVALID_PARAMETER;
+		}
+
+		/* Get a new obj to store the v1.0 descriptor. */
+		v1_0_obj = spmc_shmem_obj_alloc(&spmc_shmem_obj_state,
+						*v1_0_desc_size);
+
+		if (!v1_0_obj) {
+			return FFA_ERROR_NO_MEMORY;
+		}
+
+		/* Perform the conversion from v1.1 to v1.0. */
+		if (!spmc_shm_convert_mtd_to_v1_0(v1_0_obj, orig_obj)) {
+			spmc_shmem_obj_free(&spmc_shmem_obj_state, v1_0_obj);
+			return FFA_ERROR_INVALID_PARAMETER;
+		}
+
+		*copy_size = MIN(v1_0_obj->desc_size - offset, buf_size);
+		memcpy(dst, (uint8_t *) &v1_0_obj->desc + offset, *copy_size);
+
+		/*
+		 * We're finished with the v1.0 descriptor for now so free it.
+		 * Note that this will invalidate any references to the v1.1
+		 * descriptor.
+		 */
+		spmc_shmem_obj_free(&spmc_shmem_obj_state, v1_0_obj);
+
+		return 0;
+}
+
+/**
+ * spmc_shmem_check_obj - Check that counts in descriptor match overall size.
+ * @obj:	  Object containing ffa_memory_region_descriptor.
+ * @ffa_version:  FF-A version of the provided descriptor.
+ *
+ * Return: 0 if object is valid, -EINVAL if constituent_memory_region_descriptor
+ * offset or count is invalid.
+ */
+static int spmc_shmem_check_obj(struct spmc_shmem_obj *obj,
+				uint32_t ffa_version)
+{
+	uint32_t comp_mrd_offset = 0;
+
+	if (obj->desc.emad_count == 0U) {
+		WARN("%s: unsupported attribute desc count %u.\n",
+		     __func__, obj->desc.emad_count);
+		return -EINVAL;
+	}
+
+	for (size_t emad_num = 0; emad_num < obj->desc.emad_count; emad_num++) {
+		size_t size;
+		size_t count;
+		size_t expected_size;
+		size_t total_page_count;
+		size_t emad_size;
+		size_t desc_size;
+		size_t header_emad_size;
+		uint32_t offset;
+		struct ffa_comp_mrd *comp;
+		struct ffa_emad_v1_0 *emad;
+
+		emad = spmc_shmem_obj_get_emad(&obj->desc, emad_num,
+					       ffa_version, &emad_size);
+		if (emad == NULL) {
+			WARN("%s: invalid emad structure.\n", __func__);
+			return -EINVAL;
+		}
+
+		/*
+		 * Validate the calculated emad address resides within the
+		 * descriptor.
+		 */
+		if ((uintptr_t) emad >=
+		    (uintptr_t)((uint8_t *) &obj->desc + obj->desc_size)) {
+			WARN("Invalid emad access.\n");
+			return -EINVAL;
+		}
+
+		offset = emad->comp_mrd_offset;
+
+		if (ffa_version == MAKE_FFA_VERSION(1, 0)) {
+			desc_size =  sizeof(struct ffa_mtd_v1_0);
+		} else {
+			desc_size =  sizeof(struct ffa_mtd);
+		}
+
+		header_emad_size = desc_size +
+			(obj->desc.emad_count * emad_size);
+
+		if (offset < header_emad_size) {
+			WARN("%s: invalid object, offset %u < header + emad %zu\n",
+			     __func__, offset, header_emad_size);
+			return -EINVAL;
+		}
+
+		size = obj->desc_size;
+
+		if (offset > size) {
+			WARN("%s: invalid object, offset %u > total size %zu\n",
+			     __func__, offset, obj->desc_size);
+			return -EINVAL;
+		}
+		size -= offset;
+
+		if (size < sizeof(struct ffa_comp_mrd)) {
+			WARN("%s: invalid object, offset %u, total size %zu, no header space.\n",
+			     __func__, offset, obj->desc_size);
+			return -EINVAL;
+		}
+		size -= sizeof(struct ffa_comp_mrd);
+
+		count = size / sizeof(struct ffa_cons_mrd);
+
+		comp = spmc_shmem_obj_get_comp_mrd(obj, ffa_version);
+
+		if (comp == NULL) {
+			WARN("%s: invalid comp_mrd offset\n", __func__);
+			return -EINVAL;
+		}
+
+		if (comp->address_range_count != count) {
+			WARN("%s: invalid object, desc count %u != %zu\n",
+			     __func__, comp->address_range_count, count);
+			return -EINVAL;
+		}
+
+		expected_size = offset + sizeof(*comp) +
+				spmc_shmem_obj_ffa_constituent_size(obj,
+								    ffa_version);
+
+		if (expected_size != obj->desc_size) {
+			WARN("%s: invalid object, computed size %zu != size %zu\n",
+			       __func__, expected_size, obj->desc_size);
+			return -EINVAL;
+		}
+
+		if (obj->desc_filled < obj->desc_size) {
+			/*
+			 * The whole descriptor has not yet been received.
+			 * Skip final checks.
+			 */
+			return 0;
+		}
+
+		/*
+		 * The offset provided to the composite memory region descriptor
+		 * should be consistent across endpoint descriptors. Store the
+		 * first entry and compare against subsequent entries.
+		 */
+		if (comp_mrd_offset == 0) {
+			comp_mrd_offset = offset;
+		} else {
+			if (comp_mrd_offset != offset) {
+				ERROR("%s: mismatching offsets provided, %u != %u\n",
+				       __func__, offset, comp_mrd_offset);
+				return -EINVAL;
+			}
+		}
+
+		total_page_count = 0;
+
+		for (size_t i = 0; i < count; i++) {
+			total_page_count +=
+				comp->address_range_array[i].page_count;
+		}
+		if (comp->total_page_count != total_page_count) {
+			WARN("%s: invalid object, desc total_page_count %u != %zu\n",
+			     __func__, comp->total_page_count,
+			total_page_count);
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+/**
+ * spmc_shmem_check_state_obj - Check if the descriptor describes memory
+ *				regions that are currently involved with an
+ *				existing memory transactions. This implies that
+ *				the memory is not in a valid state for lending.
+ * @obj:    Object containing ffa_memory_region_descriptor.
+ *
+ * Return: 0 if object is valid, -EINVAL if invalid memory state.
+ */
+static int spmc_shmem_check_state_obj(struct spmc_shmem_obj *obj,
+				      uint32_t ffa_version)
+{
+	size_t obj_offset = 0;
+	struct spmc_shmem_obj *inflight_obj;
+
+	struct ffa_comp_mrd *other_mrd;
+	struct ffa_comp_mrd *requested_mrd = spmc_shmem_obj_get_comp_mrd(obj,
+								  ffa_version);
+
+	if (requested_mrd == NULL) {
+		return -EINVAL;
+	}
+
+	inflight_obj = spmc_shmem_obj_get_next(&spmc_shmem_obj_state,
+					       &obj_offset);
+
+	while (inflight_obj != NULL) {
+		/*
+		 * Don't compare the transaction to itself or to partially
+		 * transmitted descriptors.
+		 */
+		if ((obj->desc.handle != inflight_obj->desc.handle) &&
+		    (obj->desc_size == obj->desc_filled)) {
+			other_mrd = spmc_shmem_obj_get_comp_mrd(inflight_obj,
+							  FFA_VERSION_COMPILED);
+			if (other_mrd == NULL) {
+				return -EINVAL;
+			}
+			if (overlapping_memory_regions(requested_mrd,
+						       other_mrd)) {
+				return -EINVAL;
+			}
+		}
+
+		inflight_obj = spmc_shmem_obj_get_next(&spmc_shmem_obj_state,
+						       &obj_offset);
+	}
+	return 0;
+}
+
+static long spmc_ffa_fill_desc(struct mailbox *mbox,
+			       struct spmc_shmem_obj *obj,
+			       uint32_t fragment_length,
+			       ffa_mtd_flag32_t mtd_flag,
+			       uint32_t ffa_version,
+			       void *smc_handle)
+{
+	int ret;
+	size_t emad_size;
+	uint32_t handle_low;
+	uint32_t handle_high;
+	struct ffa_emad_v1_0 *emad;
+	struct ffa_emad_v1_0 *other_emad;
+
+	if (mbox->rxtx_page_count == 0U) {
+		WARN("%s: buffer pair not registered.\n", __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_arg;
+	}
+
+	if (fragment_length > mbox->rxtx_page_count * PAGE_SIZE_4KB) {
+		WARN("%s: bad fragment size %u > %u buffer size\n", __func__,
+		     fragment_length, mbox->rxtx_page_count * PAGE_SIZE_4KB);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_arg;
+	}
+
+	memcpy((uint8_t *)&obj->desc + obj->desc_filled,
+	       (uint8_t *) mbox->tx_buffer, fragment_length);
+
+	if (fragment_length > obj->desc_size - obj->desc_filled) {
+		WARN("%s: bad fragment size %u > %zu remaining\n", __func__,
+		     fragment_length, obj->desc_size - obj->desc_filled);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_arg;
+	}
+
+	/* Ensure that the sender ID resides in the normal world. */
+	if (ffa_is_secure_world_id(obj->desc.sender_id)) {
+		WARN("%s: Invalid sender ID 0x%x.\n",
+		     __func__, obj->desc.sender_id);
+		ret = FFA_ERROR_DENIED;
+		goto err_arg;
+	}
+
+	/* Ensure the NS bit is set to 0. */
+	if ((obj->desc.memory_region_attributes & FFA_MEM_ATTR_NS_BIT) != 0U) {
+		WARN("%s: NS mem attributes flags MBZ.\n", __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_arg;
+	}
+
+	/*
+	 * We don't currently support any optional flags so ensure none are
+	 * requested.
+	 */
+	if (obj->desc.flags != 0U && mtd_flag != 0U &&
+	    (obj->desc.flags != mtd_flag)) {
+		WARN("%s: invalid memory transaction flags %u != %u\n",
+		     __func__, obj->desc.flags, mtd_flag);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_arg;
+	}
+
+	if (obj->desc_filled == 0U) {
+		/* First fragment, descriptor header has been copied */
+		obj->desc.handle = spmc_shmem_obj_state.next_handle++;
+		obj->desc.flags |= mtd_flag;
+	}
+
+	obj->desc_filled += fragment_length;
+	ret = spmc_shmem_check_obj(obj, ffa_version);
+	if (ret != 0) {
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_bad_desc;
+	}
+
+	handle_low = (uint32_t)obj->desc.handle;
+	handle_high = obj->desc.handle >> 32;
+
+	if (obj->desc_filled != obj->desc_size) {
+		SMC_RET8(smc_handle, FFA_MEM_FRAG_RX, handle_low,
+			 handle_high, obj->desc_filled,
+			 (uint32_t)obj->desc.sender_id << 16, 0, 0, 0);
+	}
+
+	/* The full descriptor has been received, perform any final checks. */
+
+	/*
+	 * If a partition ID resides in the secure world validate that the
+	 * partition ID is for a known partition. Ignore any partition ID
+	 * belonging to the normal world as it is assumed the Hypervisor will
+	 * have validated these.
+	 */
+	for (size_t i = 0; i < obj->desc.emad_count; i++) {
+		emad = spmc_shmem_obj_get_emad(&obj->desc, i, ffa_version,
+					       &emad_size);
+		if (emad == NULL) {
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_bad_desc;
+		}
+
+		ffa_endpoint_id16_t ep_id = emad->mapd.endpoint_id;
+
+		if (ffa_is_secure_world_id(ep_id)) {
+			if (spmc_get_sp_ctx(ep_id) == NULL) {
+				WARN("%s: Invalid receiver id 0x%x\n",
+				     __func__, ep_id);
+				ret = FFA_ERROR_INVALID_PARAMETER;
+				goto err_bad_desc;
+			}
+		}
+	}
+
+	/* Ensure partition IDs are not duplicated. */
+	for (size_t i = 0; i < obj->desc.emad_count; i++) {
+		emad = spmc_shmem_obj_get_emad(&obj->desc, i, ffa_version,
+					       &emad_size);
+		if (emad == NULL) {
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_bad_desc;
+		}
+		for (size_t j = i + 1; j < obj->desc.emad_count; j++) {
+			other_emad = spmc_shmem_obj_get_emad(&obj->desc, j,
+							     ffa_version,
+							     &emad_size);
+			if (other_emad == NULL) {
+				ret = FFA_ERROR_INVALID_PARAMETER;
+				goto err_bad_desc;
+			}
+
+			if (emad->mapd.endpoint_id ==
+				other_emad->mapd.endpoint_id) {
+				WARN("%s: Duplicated endpoint id 0x%x\n",
+				     __func__, emad->mapd.endpoint_id);
+				ret = FFA_ERROR_INVALID_PARAMETER;
+				goto err_bad_desc;
+			}
+		}
+	}
+
+	ret = spmc_shmem_check_state_obj(obj, ffa_version);
+	if (ret) {
+		ERROR("%s: invalid memory region descriptor.\n", __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_bad_desc;
+	}
+
+	/*
+	 * Everything checks out, if the sender was using FF-A v1.0, convert
+	 * the descriptor format to use the v1.1 structures.
+	 */
+	if (ffa_version == MAKE_FFA_VERSION(1, 0)) {
+		struct spmc_shmem_obj *v1_1_obj;
+		uint64_t mem_handle;
+
+		/* Calculate the size that the v1.1 descriptor will required. */
+		size_t v1_1_desc_size =
+		    spmc_shm_get_v1_1_descriptor_size((void *) &obj->desc,
+						      fragment_length);
+
+		if (v1_1_desc_size == 0U) {
+			ERROR("%s: cannot determine size of descriptor.\n",
+			      __func__);
+			goto err_arg;
+		}
+
+		/* Get a new obj to store the v1.1 descriptor. */
+		v1_1_obj =
+		    spmc_shmem_obj_alloc(&spmc_shmem_obj_state, v1_1_desc_size);
+
+		if (!obj) {
+			ret = FFA_ERROR_NO_MEMORY;
+			goto err_arg;
+		}
+
+		/* Perform the conversion from v1.0 to v1.1. */
+		v1_1_obj->desc_size = v1_1_desc_size;
+		v1_1_obj->desc_filled = v1_1_desc_size;
+		if (!spmc_shm_convert_shmem_obj_from_v1_0(v1_1_obj, obj)) {
+			ERROR("%s: Could not convert mtd!\n", __func__);
+			spmc_shmem_obj_free(&spmc_shmem_obj_state, v1_1_obj);
+			goto err_arg;
+		}
+
+		/*
+		 * We're finished with the v1.0 descriptor so free it
+		 * and continue our checks with the new v1.1 descriptor.
+		 */
+		mem_handle = obj->desc.handle;
+		spmc_shmem_obj_free(&spmc_shmem_obj_state, obj);
+		obj = spmc_shmem_obj_lookup(&spmc_shmem_obj_state, mem_handle);
+		if (obj == NULL) {
+			ERROR("%s: Failed to find converted descriptor.\n",
+			     __func__);
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			return spmc_ffa_error_return(smc_handle, ret);
+		}
+	}
+
+	/* Allow for platform specific operations to be performed. */
+	ret = plat_spmc_shmem_begin(&obj->desc);
+	if (ret != 0) {
+		goto err_arg;
+	}
+
+	SMC_RET8(smc_handle, FFA_SUCCESS_SMC32, 0, handle_low, handle_high, 0,
+		 0, 0, 0);
+
+err_bad_desc:
+err_arg:
+	spmc_shmem_obj_free(&spmc_shmem_obj_state, obj);
+	return spmc_ffa_error_return(smc_handle, ret);
+}
+
+/**
+ * spmc_ffa_mem_send - FFA_MEM_SHARE/LEND implementation.
+ * @client:             Client state.
+ * @total_length:       Total length of shared memory descriptor.
+ * @fragment_length:    Length of fragment of shared memory descriptor passed in
+ *                      this call.
+ * @address:            Not supported, must be 0.
+ * @page_count:         Not supported, must be 0.
+ * @smc_handle:         Handle passed to smc call. Used to return
+ *                      FFA_MEM_FRAG_RX or SMC_FC_FFA_SUCCESS.
+ *
+ * Implements a subset of the FF-A FFA_MEM_SHARE and FFA_MEM_LEND calls needed
+ * to share or lend memory from non-secure os to secure os (with no stream
+ * endpoints).
+ *
+ * Return: 0 on success, error code on failure.
+ */
+long spmc_ffa_mem_send(uint32_t smc_fid,
+			bool secure_origin,
+			uint64_t total_length,
+			uint32_t fragment_length,
+			uint64_t address,
+			uint32_t page_count,
+			void *cookie,
+			void *handle,
+			uint64_t flags)
+
+{
+	long ret;
+	struct spmc_shmem_obj *obj;
+	struct mailbox *mbox = spmc_get_mbox_desc(secure_origin);
+	ffa_mtd_flag32_t mtd_flag;
+	uint32_t ffa_version = get_partition_ffa_version(secure_origin);
+
+	if (address != 0U || page_count != 0U) {
+		WARN("%s: custom memory region for message not supported.\n",
+		     __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	if (secure_origin) {
+		WARN("%s: unsupported share direction.\n", __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/*
+	 * Check if the descriptor is smaller than the v1.0 descriptor. The
+	 * descriptor cannot be smaller than this structure.
+	 */
+	if (fragment_length < sizeof(struct ffa_mtd_v1_0)) {
+		WARN("%s: bad first fragment size %u < %zu\n",
+		     __func__, fragment_length, sizeof(struct ffa_mtd_v1_0));
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	if ((smc_fid & FUNCID_NUM_MASK) == FFA_FNUM_MEM_SHARE) {
+		mtd_flag = FFA_MTD_FLAG_TYPE_SHARE_MEMORY;
+	} else if ((smc_fid & FUNCID_NUM_MASK) == FFA_FNUM_MEM_LEND) {
+		mtd_flag = FFA_MTD_FLAG_TYPE_LEND_MEMORY;
+	} else {
+		WARN("%s: invalid memory management operation.\n", __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	spin_lock(&spmc_shmem_obj_state.lock);
+	obj = spmc_shmem_obj_alloc(&spmc_shmem_obj_state, total_length);
+	if (obj == NULL) {
+		ret = FFA_ERROR_NO_MEMORY;
+		goto err_unlock;
+	}
+
+	spin_lock(&mbox->lock);
+	ret = spmc_ffa_fill_desc(mbox, obj, fragment_length, mtd_flag,
+				 ffa_version, handle);
+	spin_unlock(&mbox->lock);
+
+	spin_unlock(&spmc_shmem_obj_state.lock);
+	return ret;
+
+err_unlock:
+	spin_unlock(&spmc_shmem_obj_state.lock);
+	return spmc_ffa_error_return(handle, ret);
+}
+
+/**
+ * spmc_ffa_mem_frag_tx - FFA_MEM_FRAG_TX implementation.
+ * @client:             Client state.
+ * @handle_low:         Handle_low value returned from FFA_MEM_FRAG_RX.
+ * @handle_high:        Handle_high value returned from FFA_MEM_FRAG_RX.
+ * @fragment_length:    Length of fragments transmitted.
+ * @sender_id:          Vmid of sender in bits [31:16]
+ * @smc_handle:         Handle passed to smc call. Used to return
+ *                      FFA_MEM_FRAG_RX or SMC_FC_FFA_SUCCESS.
+ *
+ * Return: @smc_handle on success, error code on failure.
+ */
+long spmc_ffa_mem_frag_tx(uint32_t smc_fid,
+			  bool secure_origin,
+			  uint64_t handle_low,
+			  uint64_t handle_high,
+			  uint32_t fragment_length,
+			  uint32_t sender_id,
+			  void *cookie,
+			  void *handle,
+			  uint64_t flags)
+{
+	long ret;
+	uint32_t desc_sender_id;
+	uint32_t ffa_version = get_partition_ffa_version(secure_origin);
+	struct mailbox *mbox = spmc_get_mbox_desc(secure_origin);
+
+	struct spmc_shmem_obj *obj;
+	uint64_t mem_handle = handle_low | (((uint64_t)handle_high) << 32);
+
+	spin_lock(&spmc_shmem_obj_state.lock);
+
+	obj = spmc_shmem_obj_lookup(&spmc_shmem_obj_state, mem_handle);
+	if (obj == NULL) {
+		WARN("%s: invalid handle, 0x%lx, not a valid handle.\n",
+		     __func__, mem_handle);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock;
+	}
+
+	desc_sender_id = (uint32_t)obj->desc.sender_id << 16;
+	if (sender_id != desc_sender_id) {
+		WARN("%s: invalid sender_id 0x%x != 0x%x\n", __func__,
+		     sender_id, desc_sender_id);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock;
+	}
+
+	if (obj->desc_filled == obj->desc_size) {
+		WARN("%s: object desc already filled, %zu\n", __func__,
+		     obj->desc_filled);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock;
+	}
+
+	spin_lock(&mbox->lock);
+	ret = spmc_ffa_fill_desc(mbox, obj, fragment_length, 0, ffa_version,
+				 handle);
+	spin_unlock(&mbox->lock);
+
+	spin_unlock(&spmc_shmem_obj_state.lock);
+	return ret;
+
+err_unlock:
+	spin_unlock(&spmc_shmem_obj_state.lock);
+	return spmc_ffa_error_return(handle, ret);
+}
+
+/**
+ * spmc_ffa_mem_retrieve_set_ns_bit - Set the NS bit in the response descriptor
+ *				      if the caller implements a version greater
+ *				      than FF-A 1.0 or if they have requested
+ *				      the functionality.
+ *				      TODO: We are assuming that the caller is
+ *				      an SP. To support retrieval from the
+ *				      normal world this function will need to be
+ *				      expanded accordingly.
+ * @resp:       Descriptor populated in callers RX buffer.
+ * @sp_ctx:     Context of the calling SP.
+ */
+void spmc_ffa_mem_retrieve_set_ns_bit(struct ffa_mtd *resp,
+			 struct secure_partition_desc *sp_ctx)
+{
+	if (sp_ctx->ffa_version > MAKE_FFA_VERSION(1, 0) ||
+	    sp_ctx->ns_bit_requested) {
+		/*
+		 * Currently memory senders must reside in the normal
+		 * world, and we do not have the functionlaity to change
+		 * the state of memory dynamically. Therefore we can always set
+		 * the NS bit to 1.
+		 */
+		resp->memory_region_attributes |= FFA_MEM_ATTR_NS_BIT;
+	}
+}
+
+/**
+ * spmc_ffa_mem_retrieve_req - FFA_MEM_RETRIEVE_REQ implementation.
+ * @smc_fid:            FID of SMC
+ * @total_length:       Total length of retrieve request descriptor if this is
+ *                      the first call. Otherwise (unsupported) must be 0.
+ * @fragment_length:    Length of fragment of retrieve request descriptor passed
+ *                      in this call. Only @fragment_length == @length is
+ *                      supported by this implementation.
+ * @address:            Not supported, must be 0.
+ * @page_count:         Not supported, must be 0.
+ * @smc_handle:         Handle passed to smc call. Used to return
+ *                      FFA_MEM_RETRIEVE_RESP.
+ *
+ * Implements a subset of the FF-A FFA_MEM_RETRIEVE_REQ call.
+ * Used by secure os to retrieve memory already shared by non-secure os.
+ * If the data does not fit in a single FFA_MEM_RETRIEVE_RESP message,
+ * the client must call FFA_MEM_FRAG_RX until the full response has been
+ * received.
+ *
+ * Return: @handle on success, error code on failure.
+ */
+long
+spmc_ffa_mem_retrieve_req(uint32_t smc_fid,
+			  bool secure_origin,
+			  uint32_t total_length,
+			  uint32_t fragment_length,
+			  uint64_t address,
+			  uint32_t page_count,
+			  void *cookie,
+			  void *handle,
+			  uint64_t flags)
+{
+	int ret;
+	size_t buf_size;
+	size_t copy_size = 0;
+	size_t min_desc_size;
+	size_t out_desc_size = 0;
+
+	/*
+	 * Currently we are only accessing fields that are the same in both the
+	 * v1.0 and v1.1 mtd struct therefore we can use a v1.1 struct directly
+	 * here. We only need validate against the appropriate struct size.
+	 */
+	struct ffa_mtd *resp;
+	const struct ffa_mtd *req;
+	struct spmc_shmem_obj *obj = NULL;
+	struct mailbox *mbox = spmc_get_mbox_desc(secure_origin);
+	uint32_t ffa_version = get_partition_ffa_version(secure_origin);
+	struct secure_partition_desc *sp_ctx = spmc_get_current_sp_ctx();
+
+	if (!secure_origin) {
+		WARN("%s: unsupported retrieve req direction.\n", __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	if (address != 0U || page_count != 0U) {
+		WARN("%s: custom memory region not supported.\n", __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	spin_lock(&mbox->lock);
+
+	req = mbox->tx_buffer;
+	resp = mbox->rx_buffer;
+	buf_size = mbox->rxtx_page_count * FFA_PAGE_SIZE;
+
+	if (mbox->rxtx_page_count == 0U) {
+		WARN("%s: buffer pair not registered.\n", __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_mailbox;
+	}
+
+	if (mbox->state != MAILBOX_STATE_EMPTY) {
+		WARN("%s: RX Buffer is full! %d\n", __func__, mbox->state);
+		ret = FFA_ERROR_DENIED;
+		goto err_unlock_mailbox;
+	}
+
+	if (fragment_length != total_length) {
+		WARN("%s: fragmented retrieve request not supported.\n",
+		     __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_mailbox;
+	}
+
+	if (req->emad_count == 0U) {
+		WARN("%s: unsupported attribute desc count %u.\n",
+		     __func__, obj->desc.emad_count);
+		return -EINVAL;
+	}
+
+	/* Determine the appropriate minimum descriptor size. */
+	if (ffa_version == MAKE_FFA_VERSION(1, 0)) {
+		min_desc_size = sizeof(struct ffa_mtd_v1_0);
+	} else {
+		min_desc_size = sizeof(struct ffa_mtd);
+	}
+	if (total_length < min_desc_size) {
+		WARN("%s: invalid length %u < %zu\n", __func__, total_length,
+		     min_desc_size);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_mailbox;
+	}
+
+	spin_lock(&spmc_shmem_obj_state.lock);
+
+	obj = spmc_shmem_obj_lookup(&spmc_shmem_obj_state, req->handle);
+	if (obj == NULL) {
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	if (obj->desc_filled != obj->desc_size) {
+		WARN("%s: incomplete object desc filled %zu < size %zu\n",
+		     __func__, obj->desc_filled, obj->desc_size);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	if (req->emad_count != 0U && req->sender_id != obj->desc.sender_id) {
+		WARN("%s: wrong sender id 0x%x != 0x%x\n",
+		     __func__, req->sender_id, obj->desc.sender_id);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	if (req->emad_count != 0U && req->tag != obj->desc.tag) {
+		WARN("%s: wrong tag 0x%lx != 0x%lx\n",
+		     __func__, req->tag, obj->desc.tag);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	if (req->emad_count != 0U && req->emad_count != obj->desc.emad_count) {
+		WARN("%s: mistmatch of endpoint counts %u != %u\n",
+		     __func__, req->emad_count, obj->desc.emad_count);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	/* Ensure the NS bit is set to 0 in the request. */
+	if ((req->memory_region_attributes & FFA_MEM_ATTR_NS_BIT) != 0U) {
+		WARN("%s: NS mem attributes flags MBZ.\n", __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	if (req->flags != 0U) {
+		if ((req->flags & FFA_MTD_FLAG_TYPE_MASK) !=
+		    (obj->desc.flags & FFA_MTD_FLAG_TYPE_MASK)) {
+			/*
+			 * If the retrieve request specifies the memory
+			 * transaction ensure it matches what we expect.
+			 */
+			WARN("%s: wrong mem transaction flags %x != %x\n",
+			__func__, req->flags, obj->desc.flags);
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_unlock_all;
+		}
+
+		if (req->flags != FFA_MTD_FLAG_TYPE_SHARE_MEMORY &&
+		    req->flags != FFA_MTD_FLAG_TYPE_LEND_MEMORY) {
+			/*
+			 * Current implementation does not support donate and
+			 * it supports no other flags.
+			 */
+			WARN("%s: invalid flags 0x%x\n", __func__, req->flags);
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_unlock_all;
+		}
+	}
+
+	/* Validate that the provided emad offset and structure is valid.*/
+	for (size_t i = 0; i < req->emad_count; i++) {
+		size_t emad_size;
+		struct ffa_emad_v1_0 *emad;
+
+		emad = spmc_shmem_obj_get_emad(req, i, ffa_version,
+					       &emad_size);
+		if (emad == NULL) {
+			WARN("%s: invalid emad structure.\n", __func__);
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_unlock_all;
+		}
+
+		if ((uintptr_t) emad >= (uintptr_t)
+					((uint8_t *) req + total_length)) {
+			WARN("Invalid emad access.\n");
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_unlock_all;
+		}
+	}
+
+	/*
+	 * Validate all the endpoints match in the case of multiple
+	 * borrowers. We don't mandate that the order of the borrowers
+	 * must match in the descriptors therefore check to see if the
+	 * endpoints match in any order.
+	 */
+	for (size_t i = 0; i < req->emad_count; i++) {
+		bool found = false;
+		size_t emad_size;
+		struct ffa_emad_v1_0 *emad;
+		struct ffa_emad_v1_0 *other_emad;
+
+		emad = spmc_shmem_obj_get_emad(req, i, ffa_version,
+					       &emad_size);
+		if (emad == NULL) {
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_unlock_all;
+		}
+
+		for (size_t j = 0; j < obj->desc.emad_count; j++) {
+			other_emad = spmc_shmem_obj_get_emad(
+					&obj->desc, j, MAKE_FFA_VERSION(1, 1),
+					&emad_size);
+
+			if (other_emad == NULL) {
+				ret = FFA_ERROR_INVALID_PARAMETER;
+				goto err_unlock_all;
+			}
+
+			if (req->emad_count &&
+			    emad->mapd.endpoint_id ==
+			    other_emad->mapd.endpoint_id) {
+				found = true;
+				break;
+			}
+		}
+
+		if (!found) {
+			WARN("%s: invalid receiver id (0x%x).\n",
+			     __func__, emad->mapd.endpoint_id);
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_unlock_all;
+		}
+	}
+
+	mbox->state = MAILBOX_STATE_FULL;
+
+	if (req->emad_count != 0U) {
+		obj->in_use++;
+	}
+
+	/*
+	 * If the caller is v1.0 convert the descriptor, otherwise copy
+	 * directly.
+	 */
+	if (ffa_version == MAKE_FFA_VERSION(1, 0)) {
+		ret = spmc_populate_ffa_v1_0_descriptor(resp, obj, buf_size, 0,
+							&copy_size,
+							&out_desc_size);
+		if (ret != 0U) {
+			ERROR("%s: Failed to process descriptor.\n", __func__);
+			goto err_unlock_all;
+		}
+	} else {
+		copy_size = MIN(obj->desc_size, buf_size);
+		out_desc_size = obj->desc_size;
+
+		memcpy(resp, &obj->desc, copy_size);
+	}
+
+	/* Set the NS bit in the response if applicable. */
+	spmc_ffa_mem_retrieve_set_ns_bit(resp, sp_ctx);
+
+	spin_unlock(&spmc_shmem_obj_state.lock);
+	spin_unlock(&mbox->lock);
+
+	SMC_RET8(handle, FFA_MEM_RETRIEVE_RESP, out_desc_size,
+		 copy_size, 0, 0, 0, 0, 0);
+
+err_unlock_all:
+	spin_unlock(&spmc_shmem_obj_state.lock);
+err_unlock_mailbox:
+	spin_unlock(&mbox->lock);
+	return spmc_ffa_error_return(handle, ret);
+}
+
+/**
+ * spmc_ffa_mem_frag_rx - FFA_MEM_FRAG_RX implementation.
+ * @client:             Client state.
+ * @handle_low:         Handle passed to &FFA_MEM_RETRIEVE_REQ. Bit[31:0].
+ * @handle_high:        Handle passed to &FFA_MEM_RETRIEVE_REQ. Bit[63:32].
+ * @fragment_offset:    Byte offset in descriptor to resume at.
+ * @sender_id:          Bit[31:16]: Endpoint id of sender if client is a
+ *                      hypervisor. 0 otherwise.
+ * @smc_handle:         Handle passed to smc call. Used to return
+ *                      FFA_MEM_FRAG_TX.
+ *
+ * Return: @smc_handle on success, error code on failure.
+ */
+long spmc_ffa_mem_frag_rx(uint32_t smc_fid,
+			  bool secure_origin,
+			  uint32_t handle_low,
+			  uint32_t handle_high,
+			  uint32_t fragment_offset,
+			  uint32_t sender_id,
+			  void *cookie,
+			  void *handle,
+			  uint64_t flags)
+{
+	int ret;
+	void *src;
+	size_t buf_size;
+	size_t copy_size;
+	size_t full_copy_size;
+	uint32_t desc_sender_id;
+	struct mailbox *mbox = spmc_get_mbox_desc(secure_origin);
+	uint64_t mem_handle = handle_low | (((uint64_t)handle_high) << 32);
+	struct spmc_shmem_obj *obj;
+	uint32_t ffa_version = get_partition_ffa_version(secure_origin);
+
+	if (!secure_origin) {
+		WARN("%s: can only be called from swld.\n",
+		     __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	spin_lock(&spmc_shmem_obj_state.lock);
+
+	obj = spmc_shmem_obj_lookup(&spmc_shmem_obj_state, mem_handle);
+	if (obj == NULL) {
+		WARN("%s: invalid handle, 0x%lx, not a valid handle.\n",
+		     __func__, mem_handle);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_shmem;
+	}
+
+	desc_sender_id = (uint32_t)obj->desc.sender_id << 16;
+	if (sender_id != 0U && sender_id != desc_sender_id) {
+		WARN("%s: invalid sender_id 0x%x != 0x%x\n", __func__,
+		     sender_id, desc_sender_id);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_shmem;
+	}
+
+	if (fragment_offset >= obj->desc_size) {
+		WARN("%s: invalid fragment_offset 0x%x >= 0x%zx\n",
+		     __func__, fragment_offset, obj->desc_size);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_shmem;
+	}
+
+	spin_lock(&mbox->lock);
+
+	if (mbox->rxtx_page_count == 0U) {
+		WARN("%s: buffer pair not registered.\n", __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	if (mbox->state != MAILBOX_STATE_EMPTY) {
+		WARN("%s: RX Buffer is full!\n", __func__);
+		ret = FFA_ERROR_DENIED;
+		goto err_unlock_all;
+	}
+
+	buf_size = mbox->rxtx_page_count * FFA_PAGE_SIZE;
+
+	mbox->state = MAILBOX_STATE_FULL;
+
+	/*
+	 * If the caller is v1.0 convert the descriptor, otherwise copy
+	 * directly.
+	 */
+	if (ffa_version == MAKE_FFA_VERSION(1, 0)) {
+		size_t out_desc_size;
+
+		ret = spmc_populate_ffa_v1_0_descriptor(mbox->rx_buffer, obj,
+							buf_size,
+							fragment_offset,
+							&copy_size,
+							&out_desc_size);
+		if (ret != 0U) {
+			ERROR("%s: Failed to process descriptor.\n", __func__);
+			goto err_unlock_all;
+		}
+	} else {
+		full_copy_size = obj->desc_size - fragment_offset;
+		copy_size = MIN(full_copy_size, buf_size);
+
+		src = &obj->desc;
+
+		memcpy(mbox->rx_buffer, src + fragment_offset, copy_size);
+	}
+
+	spin_unlock(&mbox->lock);
+	spin_unlock(&spmc_shmem_obj_state.lock);
+
+	SMC_RET8(handle, FFA_MEM_FRAG_TX, handle_low, handle_high,
+		 copy_size, sender_id, 0, 0, 0);
+
+err_unlock_all:
+	spin_unlock(&mbox->lock);
+err_unlock_shmem:
+	spin_unlock(&spmc_shmem_obj_state.lock);
+	return spmc_ffa_error_return(handle, ret);
+}
+
+/**
+ * spmc_ffa_mem_relinquish - FFA_MEM_RELINQUISH implementation.
+ * @client:             Client state.
+ *
+ * Implements a subset of the FF-A FFA_MEM_RELINQUISH call.
+ * Used by secure os release previously shared memory to non-secure os.
+ *
+ * The handle to release must be in the client's (secure os's) transmit buffer.
+ *
+ * Return: 0 on success, error code on failure.
+ */
+int spmc_ffa_mem_relinquish(uint32_t smc_fid,
+			    bool secure_origin,
+			    uint32_t handle_low,
+			    uint32_t handle_high,
+			    uint32_t fragment_offset,
+			    uint32_t sender_id,
+			    void *cookie,
+			    void *handle,
+			    uint64_t flags)
+{
+	int ret;
+	struct mailbox *mbox = spmc_get_mbox_desc(secure_origin);
+	struct spmc_shmem_obj *obj;
+	const struct ffa_mem_relinquish_descriptor *req;
+
+	if (!secure_origin) {
+		WARN("%s: unsupported relinquish direction.\n", __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	spin_lock(&mbox->lock);
+
+	if (mbox->rxtx_page_count == 0U) {
+		WARN("%s: buffer pair not registered.\n", __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_mailbox;
+	}
+
+	req = mbox->tx_buffer;
+
+	if (req->flags != 0U) {
+		WARN("%s: unsupported flags 0x%x\n", __func__, req->flags);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_mailbox;
+	}
+
+	if (req->endpoint_count == 0) {
+		WARN("%s: endpoint count cannot be 0.\n", __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_mailbox;
+	}
+
+	spin_lock(&spmc_shmem_obj_state.lock);
+
+	obj = spmc_shmem_obj_lookup(&spmc_shmem_obj_state, req->handle);
+	if (obj == NULL) {
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	if (obj->desc.emad_count != req->endpoint_count) {
+		WARN("%s: mismatch of endpoint count %u != %u\n", __func__,
+		     obj->desc.emad_count, req->endpoint_count);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	/* Validate requested endpoint IDs match descriptor. */
+	for (size_t i = 0; i < req->endpoint_count; i++) {
+		bool found = false;
+		size_t emad_size;
+		struct ffa_emad_v1_0 *emad;
+
+		for (unsigned int j = 0; j < obj->desc.emad_count; j++) {
+			emad = spmc_shmem_obj_get_emad(&obj->desc, j,
+							MAKE_FFA_VERSION(1, 1),
+							&emad_size);
+			if (req->endpoint_array[i] ==
+			    emad->mapd.endpoint_id) {
+				found = true;
+				break;
+			}
+		}
+
+		if (!found) {
+			WARN("%s: Invalid endpoint ID (0x%x).\n",
+			     __func__, req->endpoint_array[i]);
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_unlock_all;
+		}
+	}
+
+	if (obj->in_use == 0U) {
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+	obj->in_use--;
+
+	spin_unlock(&spmc_shmem_obj_state.lock);
+	spin_unlock(&mbox->lock);
+
+	SMC_RET1(handle, FFA_SUCCESS_SMC32);
+
+err_unlock_all:
+	spin_unlock(&spmc_shmem_obj_state.lock);
+err_unlock_mailbox:
+	spin_unlock(&mbox->lock);
+	return spmc_ffa_error_return(handle, ret);
+}
+
+/**
+ * spmc_ffa_mem_reclaim - FFA_MEM_RECLAIM implementation.
+ * @client:         Client state.
+ * @handle_low:     Unique handle of shared memory object to reclaim. Bit[31:0].
+ * @handle_high:    Unique handle of shared memory object to reclaim.
+ *                  Bit[63:32].
+ * @flags:          Unsupported, ignored.
+ *
+ * Implements a subset of the FF-A FFA_MEM_RECLAIM call.
+ * Used by non-secure os reclaim memory previously shared with secure os.
+ *
+ * Return: 0 on success, error code on failure.
+ */
+int spmc_ffa_mem_reclaim(uint32_t smc_fid,
+			 bool secure_origin,
+			 uint32_t handle_low,
+			 uint32_t handle_high,
+			 uint32_t mem_flags,
+			 uint64_t x4,
+			 void *cookie,
+			 void *handle,
+			 uint64_t flags)
+{
+	int ret;
+	struct spmc_shmem_obj *obj;
+	uint64_t mem_handle = handle_low | (((uint64_t)handle_high) << 32);
+
+	if (secure_origin) {
+		WARN("%s: unsupported reclaim direction.\n", __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	if (mem_flags != 0U) {
+		WARN("%s: unsupported flags 0x%x\n", __func__, mem_flags);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	spin_lock(&spmc_shmem_obj_state.lock);
+
+	obj = spmc_shmem_obj_lookup(&spmc_shmem_obj_state, mem_handle);
+	if (obj == NULL) {
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock;
+	}
+	if (obj->in_use != 0U) {
+		ret = FFA_ERROR_DENIED;
+		goto err_unlock;
+	}
+
+	/* Allow for platform specific operations to be performed. */
+	ret = plat_spmc_shmem_reclaim(&obj->desc);
+	if (ret != 0) {
+		goto err_unlock;
+	}
+
+	spmc_shmem_obj_free(&spmc_shmem_obj_state, obj);
+	spin_unlock(&spmc_shmem_obj_state.lock);
+
+	SMC_RET1(handle, FFA_SUCCESS_SMC32);
+
+err_unlock:
+	spin_unlock(&spmc_shmem_obj_state.lock);
+	return spmc_ffa_error_return(handle, ret);
+}

diff --git a/services/std_svc/spm/el3_spmc/spmc_shared_mem.h b/services/std_svc/spm/el3_spmc/spmc_shared_mem.h
new file mode 100644
index 0000000..839f7a1
--- /dev/null
+++ b/services/std_svc/spm/el3_spmc/spmc_shared_mem.h

@@ -0,0 +1,115 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SPMC_SHARED_MEM_H
+#define SPMC_SHARED_MEM_H
+
+#include <services/el3_spmc_ffa_memory.h>
+
+/**
+ * struct ffa_mem_relinquish_descriptor - Relinquish request descriptor.
+ * @handle:
+ *         Id of shared memory object to relinquish.
+ * @flags:
+ *         If bit 0 is set clear memory after unmapping from borrower. Must be 0
+ *         for share. Bit[1]: Time slicing. Not supported, must be 0. All other
+ *         bits are reserved 0.
+ * @endpoint_count:
+ *         Number of entries in @endpoint_array.
+ * @endpoint_array:
+ *         Array of endpoint ids.
+ */
+struct ffa_mem_relinquish_descriptor {
+	uint64_t handle;
+	uint32_t flags;
+	uint32_t endpoint_count;
+	ffa_endpoint_id16_t endpoint_array[];
+};
+CASSERT(sizeof(struct ffa_mem_relinquish_descriptor) == 16,
+	assert_ffa_mem_relinquish_descriptor_size_mismatch);
+
+/**
+ * struct spmc_shmem_obj_state - Global state.
+ * @data:           Backing store for spmc_shmem_obj objects.
+ * @data_size:      The size allocated for the backing store.
+ * @allocated:      Number of bytes allocated in @data.
+ * @next_handle:    Handle used for next allocated object.
+ * @lock:           Lock protecting all state in this file.
+ */
+struct spmc_shmem_obj_state {
+	uint8_t *data;
+	size_t data_size;
+	size_t allocated;
+	uint64_t next_handle;
+	spinlock_t lock;
+};
+
+extern struct spmc_shmem_obj_state spmc_shmem_obj_state;
+extern int plat_spmc_shmem_begin(struct ffa_mtd *desc);
+extern int plat_spmc_shmem_reclaim(struct ffa_mtd *desc);
+
+long spmc_ffa_mem_send(uint32_t smc_fid,
+		       bool secure_origin,
+		       uint64_t total_length,
+		       uint32_t fragment_length,
+		       uint64_t address,
+		       uint32_t page_count,
+		       void *cookie,
+		       void *handle,
+		       uint64_t flags);
+
+long spmc_ffa_mem_frag_tx(uint32_t smc_fid,
+			  bool secure_origin,
+			  uint64_t handle_low,
+			  uint64_t handle_high,
+			  uint32_t fragment_length,
+			  uint32_t sender_id,
+			  void *cookie,
+			  void *handle,
+			  uint64_t flags);
+
+long spmc_ffa_mem_retrieve_req(uint32_t smc_fid,
+			       bool secure_origin,
+			       uint32_t total_length,
+			       uint32_t fragment_length,
+			       uint64_t address,
+			       uint32_t page_count,
+			       void *cookie,
+			       void *handle,
+			       uint64_t flags);
+
+long spmc_ffa_mem_frag_rx(uint32_t smc_fid,
+			  bool secure_origin,
+			  uint32_t handle_low,
+			  uint32_t handle_high,
+			  uint32_t fragment_offset,
+			  uint32_t sender_id,
+			  void *cookie,
+			  void *handle,
+			  uint64_t flags);
+
+
+int spmc_ffa_mem_relinquish(uint32_t smc_fid,
+			    bool secure_origin,
+			    uint32_t handle_low,
+			    uint32_t handle_high,
+			    uint32_t fragment_offset,
+			    uint32_t sender_id,
+			    void *cookie,
+			    void *handle,
+			    uint64_t flags);
+
+int spmc_ffa_mem_reclaim(uint32_t smc_fid,
+			 bool secure_origin,
+			 uint32_t handle_low,
+			 uint32_t handle_high,
+			 uint32_t mem_flags,
+			 uint64_t x4,
+			 void *cookie,
+			 void *handle,
+			 uint64_t flags);
+
+#endif /* SPMC_SHARED_MEM_H */

diff --git a/services/std_svc/spm/spm_mm/spm_mm.mk b/services/std_svc/spm/spm_mm/spm_mm.mk
index 78ef0c9..f6691c3 100644
--- a/services/std_svc/spm/spm_mm/spm_mm.mk
+++ b/services/std_svc/spm/spm_mm/spm_mm.mk

@@ -16,6 +16,9 @@
 ifeq (${ENABLE_SME_FOR_NS},1)
         $(error "Error: SPM_MM is not compatible with ENABLE_SME_FOR_NS")
 endif
+ifeq (${CTX_INCLUDE_FPREGS},0)
+        $(warning "Warning: SPM_MM: CTX_INCLUDE_FPREGS is set to 0")
+endif
 
 SPM_MM_SOURCES	:=	$(addprefix services/std_svc/spm/spm_mm/,	\
 			${ARCH}/spm_mm_shim_exceptions.S		\

diff --git a/services/std_svc/spm/spm_mm/spm_mm_main.c b/services/std_svc/spm/spm_mm/spm_mm_main.c
index e71e65b..8525cd2 100644
--- a/services/std_svc/spm/spm_mm/spm_mm_main.c
+++ b/services/std_svc/spm/spm_mm/spm_mm_main.c

@@ -190,6 +190,14 @@
 	uint64_t rc;
 	sp_context_t *sp_ptr = &sp_ctx;
 
+#if CTX_INCLUDE_FPREGS
+	/*
+	 * SP runs to completion, no need to restore FP registers of secure context.
+	 * Save FP registers only for non secure context.
+	 */
+	fpregs_context_save(get_fpregs_ctx(cm_get_context(NON_SECURE)));
+#endif
+
 	/* Wait until the Secure Partition is idle and set it to busy. */
 	sp_state_wait_switch(sp_ptr, SP_STATE_IDLE, SP_STATE_BUSY);
 
@@ -208,6 +216,14 @@
 	assert(sp_ptr->state == SP_STATE_BUSY);
 	sp_state_set(sp_ptr, SP_STATE_IDLE);
 
+#if CTX_INCLUDE_FPREGS
+	/*
+	 * SP runs to completion, no need to save FP registers of secure context.
+	 * Restore only non secure world FP registers.
+	 */
+	fpregs_context_restore(get_fpregs_ctx(cm_get_context(NON_SECURE)));
+#endif
+
 	return rc;
 }
 

diff --git a/services/std_svc/spmd/spmd_main.c b/services/std_svc/spmd/spmd_main.c
index 5b131cd..7e6c89d 100644
--- a/services/std_svc/spmd/spmd_main.c
+++ b/services/std_svc/spmd/spmd_main.c

@@ -626,7 +626,8 @@
 		 * If caller is secure and SPMC was initialized,
 		 * return FFA_VERSION of SPMD.
 		 * If caller is non secure and SPMC was initialized,
-		 * return SPMC's version.
+		 * forward to the EL3 SPMC if enabled, otherwise return
+		 * the SPMC version if implemented at a lower EL.
 		 * Sanity check to "input_version".
 		 * If the EL3 SPMC is enabled, ignore the SPMC state as
 		 * this is not used.
@@ -635,6 +636,17 @@
 		    (!is_spmc_at_el3() && (ctx->state == SPMC_STATE_RESET))) {
 			ret = FFA_ERROR_NOT_SUPPORTED;
 		} else if (!secure_origin) {
+			if (is_spmc_at_el3()) {
+				/*
+				 * Forward the call directly to the EL3 SPMC, if
+				 * enabled, as we don't need to wrap the call in
+				 * a direct request.
+				 */
+				return spmd_smc_forward(smc_fid, secure_origin,
+							x1, x2, x3, x4, cookie,
+							handle, flags);
+			}
+
 			gp_regs_t *gpregs = get_gpregs_ctx(&ctx->cpu_ctx);
 			uint64_t rc;
 
@@ -672,7 +684,7 @@
 			    (SMC_GET_GP(gpregs, CTX_GPREG_X0) !=
 				FFA_MSG_SEND_DIRECT_RESP_SMC32) ||
 			    (SMC_GET_GP(gpregs, CTX_GPREG_X2) !=
-				(SPMD_FWK_MSG_BIT |
+				(FFA_FWK_MSG_BIT |
 				 SPMD_FWK_MSG_FFA_VERSION_RESP))) {
 				ERROR("Failed to forward FFA_VERSION\n");
 				ret = FFA_ERROR_NOT_SUPPORTED;
@@ -791,6 +803,14 @@
 		break; /* not reached */
 
 	case FFA_MSG_SEND_DIRECT_REQ_SMC32:
+	case FFA_MSG_SEND_DIRECT_REQ_SMC64:
+		if (!secure_origin) {
+			/* Validate source endpoint is non-secure for non-secure caller. */
+			if (ffa_is_secure_world_id(ffa_endpoint_source(x1))) {
+				return spmd_ffa_error_return(handle,
+						FFA_ERROR_INVALID_PARAMETER);
+			}
+		}
 		if (secure_origin && spmd_is_spmc_message(x1)) {
 			ret = spmd_handle_spmc_message(x3, x4,
 				SMC_GET_GP(handle, CTX_GPREG_X5),
@@ -850,7 +870,6 @@
 
 		/* Fall through to forward the call to the other world */
 	case FFA_MSG_SEND:
-	case FFA_MSG_SEND_DIRECT_REQ_SMC64:
 	case FFA_MSG_SEND_DIRECT_RESP_SMC64:
 	case FFA_MEM_DONATE_SMC32:
 	case FFA_MEM_DONATE_SMC64:
@@ -863,6 +882,8 @@
 	case FFA_MEM_RETRIEVE_RESP:
 	case FFA_MEM_RELINQUISH:
 	case FFA_MEM_RECLAIM:
+	case FFA_MEM_FRAG_TX:
+	case FFA_MEM_FRAG_RX:
 	case FFA_SUCCESS_SMC32:
 	case FFA_SUCCESS_SMC64:
 		/*

diff --git a/services/std_svc/spmd/spmd_pm.c b/services/std_svc/spmd/spmd_pm.c
index b719161..a2704dd 100644
--- a/services/std_svc/spmd/spmd_pm.c
+++ b/services/std_svc/spmd/spmd_pm.c

@@ -123,7 +123,7 @@
 
 	/* Build an SPMD to SPMC direct message request. */
 	spmd_build_spmc_message(get_gpregs_ctx(&ctx->cpu_ctx),
-				SPMD_FWK_MSG_PSCI, PSCI_CPU_OFF);
+				FFA_FWK_MSG_PSCI, PSCI_CPU_OFF);
 
 	rc = spmd_spm_core_sync_entry(ctx);
 	if (rc != 0ULL) {

diff --git a/services/std_svc/spmd/spmd_private.h b/services/std_svc/spmd/spmd_private.h
index 4c298c9..07fecb6 100644
--- a/services/std_svc/spmd/spmd_private.h
+++ b/services/std_svc/spmd/spmd_private.h

@@ -59,8 +59,6 @@
 #define FFA_NS_ENDPOINT_ID			U(0)
 
 /* Define SPMD target function IDs for framework messages to the SPMC */
-#define SPMD_FWK_MSG_BIT			BIT(31)
-#define SPMD_FWK_MSG_PSCI			U(0)
 #define SPMD_FWK_MSG_FFA_VERSION_REQ		U(0x8)
 #define SPMD_FWK_MSG_FFA_VERSION_RESP		U(0x9)
 

diff --git a/tools/cert_create/Makefile b/tools/cert_create/Makefile
index 77d2007..d951286 100644
--- a/tools/cert_create/Makefile
+++ b/tools/cert_create/Makefile

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -35,6 +35,8 @@
   include src/tbbr/tbbr.mk
 else ifeq (${COT},dualroot)
   include src/dualroot/cot.mk
+else ifeq (${COT},cca)
+  include src/cca/cot.mk
 else
   $(error Unknown chain of trust ${COT})
 endif
@@ -62,7 +64,14 @@
 # Make soft links and include from local directory otherwise wrong headers
 # could get pulled in from firmware tree.
 INC_DIR += -I ./include -I ${PLAT_INCLUDE} -I ${OPENSSL_DIR}/include
-LIB_DIR := -L ${OPENSSL_DIR}/lib
+
+# Include library directories where OpenSSL library files are located.
+# For a normal installation (i.e.: when ${OPENSSL_DIR} = /usr or
+# /usr/local), binaries are located under the ${OPENSSL_DIR}/lib/
+# directory. However, for a local build of OpenSSL, the built binaries are
+# located under the main project directory (i.e.: ${OPENSSL_DIR}, not
+# ${OPENSSL_DIR}/lib/).
+LIB_DIR := -L ${OPENSSL_DIR}/lib -L ${OPENSSL_DIR}
 LIB := -lssl -lcrypto
 
 HOSTCC ?= gcc

diff --git a/tools/cert_create/include/cca/cca_cot.h b/tools/cert_create/include/cca/cca_cot.h
new file mode 100644
index 0000000..56585fb
--- /dev/null
+++ b/tools/cert_create/include/cca/cca_cot.h

@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CCA_COT_H
+#define CCA_COT_H
+
+/* Certificates. */
+enum {
+	/* Certificates owned by the silicon provider. */
+	CCA_CONTENT_CERT,
+	CORE_SWD_KEY_CERT,
+	SPMC_CONTENT_CERT,
+	SIP_SECURE_PARTITION_CONTENT_CERT,
+
+	/* Certificates owned by the platform owner. */
+	PLAT_KEY_CERT,
+	PLAT_SECURE_PARTITION_CONTENT_CERT,
+	NON_TRUSTED_FW_CONTENT_CERT,
+};
+
+/* Certificate extensions. */
+enum {
+	/* Extensions used in certificates owned by the silicon provider. */
+	TRUSTED_FW_NVCOUNTER_EXT,
+	TRUSTED_BOOT_FW_HASH_EXT,
+	TRUSTED_BOOT_FW_CONFIG_HASH_EXT,
+	HW_CONFIG_HASH_EXT,
+	FW_CONFIG_HASH_EXT,
+	SWD_ROT_PK_EXT,
+	CORE_SWD_PK_EXT,
+	SOC_AP_FW_HASH_EXT,
+	SOC_FW_CONFIG_HASH_EXT,
+	RMM_HASH_EXT,
+	TRUSTED_OS_FW_HASH_EXT,
+	TRUSTED_OS_FW_CONFIG_HASH_EXT,
+	SP_PKG1_HASH_EXT,
+	SP_PKG2_HASH_EXT,
+	SP_PKG3_HASH_EXT,
+	SP_PKG4_HASH_EXT,
+
+	/* Extensions used in certificates owned by the platform owner. */
+	PROT_PK_EXT,
+	PLAT_PK_EXT,
+	SP_PKG5_HASH_EXT,
+	SP_PKG6_HASH_EXT,
+	SP_PKG7_HASH_EXT,
+	SP_PKG8_HASH_EXT,
+	NON_TRUSTED_FW_NVCOUNTER_EXT,
+	NON_TRUSTED_WORLD_BOOTLOADER_HASH_EXT,
+	NON_TRUSTED_FW_CONFIG_HASH_EXT,
+};
+
+/* Keys. */
+enum {
+	/* Keys owned by the silicon provider. */
+	ROT_KEY,
+	SWD_ROT_KEY,
+	CORE_SWD_KEY,
+
+	/* Keys owned by the platform owner. */
+	PROT_KEY,
+	PLAT_KEY,
+};
+
+#endif /* CCA_COT_H */

diff --git a/tools/cert_create/src/cca/cot.c b/tools/cert_create/src/cca/cot.c
new file mode 100644
index 0000000..5a35ff6
--- /dev/null
+++ b/tools/cert_create/src/cca/cot.c

@@ -0,0 +1,439 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "cca/cca_cot.h"
+
+#include <cca_oid.h>
+
+#include "cert.h"
+#include "ext.h"
+#include "key.h"
+
+/*
+ * Certificates used in the chain of trust.
+ *
+ * All certificates are self-signed so the issuer certificate field points to
+ * itself.
+ */
+static cert_t cot_certs[] = {
+	[CCA_CONTENT_CERT] = {
+		.id = CCA_CONTENT_CERT,
+		.opt = "cca-cert",
+		.help_msg = "CCA Content Certificate (output file)",
+		.cn = "CCA Content Certificate",
+		.key = ROT_KEY,
+		.issuer = CCA_CONTENT_CERT,
+		.ext = {
+			TRUSTED_FW_NVCOUNTER_EXT,
+			SOC_AP_FW_HASH_EXT,
+			SOC_FW_CONFIG_HASH_EXT,
+			RMM_HASH_EXT,
+			TRUSTED_BOOT_FW_HASH_EXT,
+			TRUSTED_BOOT_FW_CONFIG_HASH_EXT,
+			HW_CONFIG_HASH_EXT,
+			FW_CONFIG_HASH_EXT,
+		},
+		.num_ext = 8
+	},
+
+	[CORE_SWD_KEY_CERT] = {
+		.id = CORE_SWD_KEY_CERT,
+		.opt = "core-swd-cert",
+		.help_msg = "Core Secure World Key Certificate (output file)",
+		.cn = "Core Secure World Key Certificate",
+		.key = SWD_ROT_KEY,
+		.issuer = CORE_SWD_KEY_CERT,
+		.ext = {
+			TRUSTED_FW_NVCOUNTER_EXT,
+			SWD_ROT_PK_EXT,
+			CORE_SWD_PK_EXT,
+		},
+		.num_ext = 3
+	},
+
+	[SPMC_CONTENT_CERT] = {
+		.id = SPMC_CONTENT_CERT,
+		.opt = "tos-fw-cert",
+		.help_msg = "SPMC Content Certificate (output file)",
+		.cn = "SPMC Content Certificate",
+		.key = CORE_SWD_KEY,
+		.issuer = SPMC_CONTENT_CERT,
+		.ext = {
+			TRUSTED_FW_NVCOUNTER_EXT,
+			TRUSTED_OS_FW_HASH_EXT,
+			TRUSTED_OS_FW_CONFIG_HASH_EXT,
+		},
+		.num_ext = 3
+	},
+
+	[SIP_SECURE_PARTITION_CONTENT_CERT] = {
+		.id = SIP_SECURE_PARTITION_CONTENT_CERT,
+		.opt = "sip-sp-cert",
+		.help_msg = "SiP owned Secure Partition Content Certificate (output file)",
+		.cn = "SiP owned Secure Partition Content Certificate",
+		.key = CORE_SWD_KEY,
+		.issuer = SIP_SECURE_PARTITION_CONTENT_CERT,
+		.ext = {
+			TRUSTED_FW_NVCOUNTER_EXT,
+			SP_PKG1_HASH_EXT,
+			SP_PKG2_HASH_EXT,
+			SP_PKG3_HASH_EXT,
+			SP_PKG4_HASH_EXT,
+		},
+		.num_ext = 5
+	},
+
+	[PLAT_KEY_CERT] = {
+		.id = PLAT_KEY_CERT,
+		.opt = "plat-key-cert",
+		.help_msg = "Platform Key Certificate (output file)",
+		.cn = "Platform Key Certificate",
+		.key = PROT_KEY,
+		.issuer = PLAT_KEY_CERT,
+		.ext = {
+			NON_TRUSTED_FW_NVCOUNTER_EXT,
+			PROT_PK_EXT,
+			PLAT_PK_EXT,
+		},
+		.num_ext = 3
+	},
+
+	[PLAT_SECURE_PARTITION_CONTENT_CERT] = {
+		.id = PLAT_SECURE_PARTITION_CONTENT_CERT,
+		.opt = "plat-sp-cert",
+		.help_msg = "Platform owned Secure Partition Content Certificate (output file)",
+		.cn = "Platform owned Secure Partition Content Certificate",
+		.key = PLAT_KEY,
+		.issuer = PLAT_SECURE_PARTITION_CONTENT_CERT,
+		.ext = {
+			NON_TRUSTED_FW_NVCOUNTER_EXT,
+			SP_PKG5_HASH_EXT,
+			SP_PKG6_HASH_EXT,
+			SP_PKG7_HASH_EXT,
+			SP_PKG8_HASH_EXT,
+		},
+		.num_ext = 5
+	},
+
+	[NON_TRUSTED_FW_CONTENT_CERT] = {
+		.id = NON_TRUSTED_FW_CONTENT_CERT,
+		.opt = "nt-fw-cert",
+		.help_msg = "Non-Trusted Firmware Content Certificate (output file)",
+		.cn = "Non-Trusted Firmware Content Certificate",
+		.key = PLAT_KEY,
+		.issuer = NON_TRUSTED_FW_CONTENT_CERT,
+		.ext = {
+			NON_TRUSTED_FW_NVCOUNTER_EXT,
+			NON_TRUSTED_WORLD_BOOTLOADER_HASH_EXT,
+			NON_TRUSTED_FW_CONFIG_HASH_EXT,
+		},
+		.num_ext = 3
+	},
+};
+
+REGISTER_COT(cot_certs);
+
+
+/* Certificate extensions. */
+static ext_t cot_ext[] = {
+	[TRUSTED_FW_NVCOUNTER_EXT] = {
+		.oid = TRUSTED_FW_NVCOUNTER_OID,
+		.opt = "tfw-nvctr",
+		.help_msg = "Trusted Firmware Non-Volatile counter value",
+		.sn = "TrustedWorldNVCounter",
+		.ln = "Trusted World Non-Volatile counter",
+		.asn1_type = V_ASN1_INTEGER,
+		.type = EXT_TYPE_NVCOUNTER,
+		.attr.nvctr_type = NVCTR_TYPE_TFW
+	},
+
+	[TRUSTED_BOOT_FW_HASH_EXT] = {
+		.oid = TRUSTED_BOOT_FW_HASH_OID,
+		.opt = "tb-fw",
+		.help_msg = "Trusted Boot Firmware image file",
+		.sn = "TrustedBootFirmwareHash",
+		.ln = "Trusted Boot Firmware hash (SHA256)",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH
+	},
+
+	[TRUSTED_BOOT_FW_CONFIG_HASH_EXT] = {
+		.oid = TRUSTED_BOOT_FW_CONFIG_HASH_OID,
+		.opt = "tb-fw-config",
+		.help_msg = "Trusted Boot Firmware Config file",
+		.sn = "TrustedBootFirmwareConfigHash",
+		.ln = "Trusted Boot Firmware Config hash",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH,
+		.optional = 1
+	},
+
+	[HW_CONFIG_HASH_EXT] = {
+		.oid = HW_CONFIG_HASH_OID,
+		.opt = "hw-config",
+		.help_msg = "HW Config file",
+		.sn = "HWConfigHash",
+		.ln = "HW Config hash",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH,
+		.optional = 1
+	},
+
+	[FW_CONFIG_HASH_EXT] = {
+		.oid = FW_CONFIG_HASH_OID,
+		.opt = "fw-config",
+		.help_msg = "Firmware Config file",
+		.sn = "FirmwareConfigHash",
+		.ln = "Firmware Config hash",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH,
+		.optional = 1
+	},
+
+	[SWD_ROT_PK_EXT] = {
+		.oid = SWD_ROT_PK_OID,
+		.sn = "SWDRoTKey",
+		.ln = "Secure World Root of Trust Public Key",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_PKEY,
+		.attr.key = SWD_ROT_KEY
+	},
+
+	[CORE_SWD_PK_EXT] = {
+		.oid = CORE_SWD_PK_OID,
+		.sn = "CORESWDKey",
+		.ln = "Core Secure World Public Key",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_PKEY,
+		.attr.key = CORE_SWD_KEY
+	},
+
+	[SOC_AP_FW_HASH_EXT] = {
+		.oid = SOC_AP_FW_HASH_OID,
+		.opt = "soc-fw",
+		.help_msg = "SoC AP Firmware image file",
+		.sn = "SoCAPFirmwareHash",
+		.ln = "SoC AP Firmware hash (SHA256)",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH
+	},
+
+	[SOC_FW_CONFIG_HASH_EXT] = {
+		.oid = SOC_FW_CONFIG_HASH_OID,
+		.opt = "soc-fw-config",
+		.help_msg = "SoC Firmware Config file",
+		.sn = "SocFirmwareConfigHash",
+		.ln = "SoC Firmware Config hash",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH,
+		.optional = 1
+	},
+
+	[RMM_HASH_EXT] = {
+		.oid = RMM_HASH_OID,
+		.opt = "rmm-fw",
+		.help_msg = "RMM Firmware image file",
+		.sn = "RMMFirmwareHash",
+		.ln = "RMM Firmware hash (SHA256)",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH
+	},
+
+	[TRUSTED_OS_FW_HASH_EXT] = {
+		.oid = TRUSTED_OS_FW_HASH_OID,
+		.opt = "tos-fw",
+		.help_msg = "Trusted OS image file",
+		.sn = "TrustedOSHash",
+		.ln = "Trusted OS hash (SHA256)",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH
+	},
+
+	[TRUSTED_OS_FW_CONFIG_HASH_EXT] = {
+		.oid = TRUSTED_OS_FW_CONFIG_HASH_OID,
+		.opt = "tos-fw-config",
+		.help_msg = "Trusted OS Firmware Config file",
+		.sn = "TrustedOSFirmwareConfigHash",
+		.ln = "Trusted OS Firmware Config hash",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH,
+		.optional = 1
+	},
+
+	[SP_PKG1_HASH_EXT] = {
+		.oid = SP_PKG1_HASH_OID,
+		.opt = "sp-pkg1",
+		.help_msg = "Secure Partition Package1 file",
+		.sn = "SPPkg1Hash",
+		.ln = "SP Pkg1 hash (SHA256)",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH,
+		.optional = 1
+	},
+	[SP_PKG2_HASH_EXT] = {
+		.oid = SP_PKG2_HASH_OID,
+		.opt = "sp-pkg2",
+		.help_msg = "Secure Partition Package2 file",
+		.sn = "SPPkg2Hash",
+		.ln = "SP Pkg2 hash (SHA256)",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH,
+		.optional = 1
+	},
+	[SP_PKG3_HASH_EXT] = {
+		.oid = SP_PKG3_HASH_OID,
+		.opt = "sp-pkg3",
+		.help_msg = "Secure Partition Package3 file",
+		.sn = "SPPkg3Hash",
+		.ln = "SP Pkg3 hash (SHA256)",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH,
+		.optional = 1
+	},
+	[SP_PKG4_HASH_EXT] = {
+		.oid = SP_PKG4_HASH_OID,
+		.opt = "sp-pkg4",
+		.help_msg = "Secure Partition Package4 file",
+		.sn = "SPPkg4Hash",
+		.ln = "SP Pkg4 hash (SHA256)",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH,
+		.optional = 1
+	},
+
+	[PROT_PK_EXT] = {
+		.oid = PROT_PK_OID,
+		.sn = "PlatformRoTKey",
+		.ln = "Platform Root of Trust Public Key",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_PKEY,
+		.attr.key = PROT_KEY
+	},
+
+	[PLAT_PK_EXT] = {
+		.oid = PLAT_PK_OID,
+		.sn = "PLATKey",
+		.ln = "Platform Public Key",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_PKEY,
+		.attr.key = PLAT_KEY
+	},
+
+	[SP_PKG5_HASH_EXT] = {
+		.oid = SP_PKG5_HASH_OID,
+		.opt = "sp-pkg5",
+		.help_msg = "Secure Partition Package5 file",
+		.sn = "SPPkg5Hash",
+		.ln = "SP Pkg5 hash (SHA256)",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH,
+		.optional = 1
+	},
+	[SP_PKG6_HASH_EXT] = {
+		.oid = SP_PKG6_HASH_OID,
+		.opt = "sp-pkg6",
+		.help_msg = "Secure Partition Package6 file",
+		.sn = "SPPkg6Hash",
+		.ln = "SP Pkg6 hash (SHA256)",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH,
+		.optional = 1
+	},
+	[SP_PKG7_HASH_EXT] = {
+		.oid = SP_PKG7_HASH_OID,
+		.opt = "sp-pkg7",
+		.help_msg = "Secure Partition Package7 file",
+		.sn = "SPPkg7Hash",
+		.ln = "SP Pkg7 hash (SHA256)",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH,
+		.optional = 1
+	},
+	[SP_PKG8_HASH_EXT] = {
+		.oid = SP_PKG8_HASH_OID,
+		.opt = "sp-pkg8",
+		.help_msg = "Secure Partition Package8 file",
+		.sn = "SPPkg8Hash",
+		.ln = "SP Pkg8 hash (SHA256)",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH,
+		.optional = 1
+	},
+
+	[NON_TRUSTED_FW_NVCOUNTER_EXT] = {
+		.oid = NON_TRUSTED_FW_NVCOUNTER_OID,
+		.opt = "ntfw-nvctr",
+		.help_msg = "Non-Trusted Firmware Non-Volatile counter value",
+		.sn = "NormalWorldNVCounter",
+		.ln = "Non-Trusted Firmware Non-Volatile counter",
+		.asn1_type = V_ASN1_INTEGER,
+		.type = EXT_TYPE_NVCOUNTER,
+		.attr.nvctr_type = NVCTR_TYPE_NTFW
+	},
+
+	[NON_TRUSTED_WORLD_BOOTLOADER_HASH_EXT] = {
+		.oid = NON_TRUSTED_WORLD_BOOTLOADER_HASH_OID,
+		.opt = "nt-fw",
+		.help_msg = "Non-Trusted World Bootloader image file",
+		.sn = "NonTrustedWorldBootloaderHash",
+		.ln = "Non-Trusted World hash (SHA256)",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH
+	},
+
+	[NON_TRUSTED_FW_CONFIG_HASH_EXT] = {
+		.oid = NON_TRUSTED_FW_CONFIG_HASH_OID,
+		.opt = "nt-fw-config",
+		.help_msg = "Non Trusted OS Firmware Config file",
+		.sn = "NonTrustedOSFirmwareConfigHash",
+		.ln = "Non-Trusted OS Firmware Config hash",
+		.asn1_type = V_ASN1_OCTET_STRING,
+		.type = EXT_TYPE_HASH,
+		.optional = 1
+	},
+};
+
+REGISTER_EXTENSIONS(cot_ext);
+
+/* Keys used to establish the chain of trust. */
+static key_t cot_keys[] = {
+	[ROT_KEY] = {
+		.id = ROT_KEY,
+		.opt = "rot-key",
+		.help_msg = "Root Of Trust key (input/output file)",
+		.desc = "Root Of Trust key"
+	},
+
+	[SWD_ROT_KEY] = {
+		.id = SWD_ROT_KEY,
+		.opt = "swd-rot-key",
+		.help_msg = "Secure World Root of Trust key",
+		.desc = "Secure World Root of Trust key"
+	},
+
+	[CORE_SWD_KEY] = {
+		.id = CORE_SWD_KEY,
+		.opt = "core-swd-key",
+		.help_msg = "Core Secure World key",
+		.desc = "Core Secure World key"
+	},
+
+	[PROT_KEY] = {
+		.id = PROT_KEY,
+		.opt = "prot-key",
+		.help_msg = "Platform Root of Trust key",
+		.desc = "Platform Root of Trust key"
+	},
+
+	[PLAT_KEY] = {
+		.id = PLAT_KEY,
+		.opt = "plat-key",
+		.help_msg = "Platform key",
+		.desc = "Platform key"
+	},
+};
+
+REGISTER_KEYS(cot_keys);

diff --git a/tools/cert_create/src/cca/cot.mk b/tools/cert_create/src/cca/cot.mk
new file mode 100644
index 0000000..d0c80bb
--- /dev/null
+++ b/tools/cert_create/src/cca/cot.mk

@@ -0,0 +1,10 @@
+#
+# Copyright (c) 2022, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+PLAT_MSG		:=	Confidential Compute Architecture root of trust
+PLAT_INCLUDE		:=	../../include/tools_share
+
+OBJECTS += src/cca/cot.o

diff --git a/tools/cert_create/src/cert.c b/tools/cert_create/src/cert.c
index 4b35d73..67ae1d6 100644
--- a/tools/cert_create/src/cert.c
+++ b/tools/cert_create/src/cert.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -39,7 +39,7 @@
 	if (!btmp)
 		return 0;
 
-	if (!BN_pseudo_rand(btmp, SERIAL_RAND_BITS, 0, 0))
+	if (!BN_rand(btmp, SERIAL_RAND_BITS, 0, 0))
 		goto error;
 	if (ai && !BN_to_ASN1_INTEGER(btmp, ai))
 		goto error;

diff --git a/tools/cert_create/src/key.c b/tools/cert_create/src/key.c
index 6435975..2857a3b 100644
--- a/tools/cert_create/src/key.c
+++ b/tools/cert_create/src/key.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -40,69 +40,25 @@
 
 static int key_create_rsa(key_t *key, int key_bits)
 {
-	BIGNUM *e;
-	RSA *rsa = NULL;
-
-	e = BN_new();
-	if (e == NULL) {
-		printf("Cannot create RSA exponent\n");
-		goto err;
-	}
-
-	if (!BN_set_word(e, RSA_F4)) {
-		printf("Cannot assign RSA exponent\n");
-		goto err;
-	}
-
-	rsa = RSA_new();
+	EVP_PKEY *rsa = EVP_RSA_gen(key_bits);
 	if (rsa == NULL) {
-		printf("Cannot create RSA key\n");
-		goto err;
-	}
-
-	if (!RSA_generate_key_ex(rsa, key_bits, e, NULL)) {
 		printf("Cannot generate RSA key\n");
-		goto err;
-	}
-
-	if (!EVP_PKEY_assign_RSA(key->key, rsa)) {
-		printf("Cannot assign RSA key\n");
-		goto err;
+		return 0;
 	}
-
-	BN_free(e);
+	key->key = rsa;
 	return 1;
-err:
-	RSA_free(rsa);
-	BN_free(e);
-	return 0;
 }
 
 #ifndef OPENSSL_NO_EC
 static int key_create_ecdsa(key_t *key, int key_bits)
 {
-	EC_KEY *ec;
-
-	ec = EC_KEY_new_by_curve_name(NID_X9_62_prime256v1);
+	EVP_PKEY *ec = EVP_EC_gen("prime256v1");
 	if (ec == NULL) {
-		printf("Cannot create EC key\n");
-		goto err;
-	}
-	if (!EC_KEY_generate_key(ec)) {
 		printf("Cannot generate EC key\n");
-		goto err;
-	}
-	EC_KEY_set_flags(ec, EC_PKEY_NO_PARAMETERS);
-	EC_KEY_set_asn1_flag(ec, OPENSSL_EC_NAMED_CURVE);
-	if (!EVP_PKEY_assign_EC_KEY(key->key, ec)) {
-		printf("Cannot assign EC key\n");
-		goto err;
+		return 0;
 	}
-
+	key->key = ec;
 	return 1;
-err:
-	EC_KEY_free(ec);
-	return 0;
 }
 #endif /* OPENSSL_NO_EC */
 

diff --git a/tools/cert_create/src/sha.c b/tools/cert_create/src/sha.c
index 3d977fb..06ef360 100644
--- a/tools/cert_create/src/sha.c
+++ b/tools/cert_create/src/sha.c

@@ -1,26 +1,38 @@
 /*
- * Copyright (c) 2015-2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <openssl/sha.h>
 #include <stdio.h>
 #include "debug.h"
 #include "key.h"
+#include <openssl/evp.h>
+#include <openssl/obj_mac.h>
 
 #define BUFFER_SIZE	256
 
+static int get_algorithm_nid(int hash_alg)
+{
+	int nids[] = {NID_sha256, NID_sha384, NID_sha512};
+	if (hash_alg < 0 || hash_alg >= sizeof(nids) / sizeof(*nids)) {
+		return NID_undef;
+	}
+	return nids[hash_alg];
+}
+
 int sha_file(int md_alg, const char *filename, unsigned char *md)
 {
 	FILE *inFile;
-	SHA256_CTX shaContext;
-	SHA512_CTX sha512Context;
+	EVP_MD_CTX *mdctx;
+	const EVP_MD *md_type;
 	int bytes;
+	int alg_nid;
+	unsigned int total_bytes;
 	unsigned char data[BUFFER_SIZE];
 
 	if ((filename == NULL) || (md == NULL)) {
-		ERROR("%s(): NULL argument\n", __FUNCTION__);
+		ERROR("%s(): NULL argument\n", __func__);
 		return 0;
 	}
 
@@ -30,26 +42,37 @@
 		return 0;
 	}
 
+	mdctx = EVP_MD_CTX_new();
+	if (mdctx == NULL) {
+		fclose(inFile);
+		ERROR("%s(): Could not create EVP MD context\n", __func__);
+		return 0;
+	}
+
-	if (md_alg == HASH_ALG_SHA384) {
-		SHA384_Init(&sha512Context);
-		while ((bytes = fread(data, 1, BUFFER_SIZE, inFile)) != 0) {
-			SHA384_Update(&sha512Context, data, bytes);
-		}
-		SHA384_Final(md, &sha512Context);
-	} else if (md_alg == HASH_ALG_SHA512) {
-		SHA512_Init(&sha512Context);
-		while ((bytes = fread(data, 1, BUFFER_SIZE, inFile)) != 0) {
-			SHA512_Update(&sha512Context, data, bytes);
-		}
-		SHA512_Final(md, &sha512Context);
-	} else {
-		SHA256_Init(&shaContext);
-		while ((bytes = fread(data, 1, BUFFER_SIZE, inFile)) != 0) {
-			SHA256_Update(&shaContext, data, bytes);
-		}
-		SHA256_Final(md, &shaContext);
+	alg_nid = get_algorithm_nid(md_alg);
+	if (alg_nid == NID_undef) {
+		ERROR("%s(): Invalid hash algorithm\n", __func__);
+		goto err;
 	}
 
+	md_type = EVP_get_digestbynid(alg_nid);
+	if (EVP_DigestInit_ex(mdctx, md_type, NULL) == 0) {
+		ERROR("%s(): Could not initialize EVP MD digest\n", __func__);
+		goto err;
+	}
+
+	while ((bytes = fread(data, 1, BUFFER_SIZE, inFile)) != 0) {
+		EVP_DigestUpdate(mdctx, data, bytes);
+	}
+	EVP_DigestFinal_ex(mdctx, md, &total_bytes);
+
 	fclose(inFile);
+	EVP_MD_CTX_free(mdctx);
 	return 1;
+
+err:
+	fclose(inFile);
+	EVP_MD_CTX_free(mdctx);
+	return 0;
 }
+

diff --git a/tools/conventional-changelog-tf-a/package.json b/tools/conventional-changelog-tf-a/package.json
index 404ef90..0008b53 100644
--- a/tools/conventional-changelog-tf-a/package.json
+++ b/tools/conventional-changelog-tf-a/package.json

@@ -1,6 +1,6 @@
 {
   "name": "conventional-changelog-tf-a",
-  "version": "2.6.0",
+  "version": "2.7.0",
   "license": "BSD-3-Clause",
   "private": true,
   "main": "index.js",

diff --git a/tools/encrypt_fw/Makefile b/tools/encrypt_fw/Makefile
index 96dff23..60bd8ea 100644
--- a/tools/encrypt_fw/Makefile
+++ b/tools/encrypt_fw/Makefile

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2019-2020, Linaro Limited. All rights reserved.
+# Copyright (c) 2019-2022, Linaro Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -39,7 +39,14 @@
 # Make soft links and include from local directory otherwise wrong headers
 # could get pulled in from firmware tree.
 INC_DIR := -I ./include -I ../../include/tools_share -I ${OPENSSL_DIR}/include
-LIB_DIR := -L ${OPENSSL_DIR}/lib
+
+# Include library directories where OpenSSL library files are located.
+# For a normal installation (i.e.: when ${OPENSSL_DIR} = /usr or
+# /usr/local), binaries are located under the ${OPENSSL_DIR}/lib/
+# directory. However, for a local build of OpenSSL, the built binaries are
+# located under the main project directory (i.e.: ${OPENSSL_DIR}, not
+# ${OPENSSL_DIR}/lib/).
+LIB_DIR := -L ${OPENSSL_DIR}/lib -L ${OPENSSL_DIR}
 LIB := -lssl -lcrypto
 
 HOSTCC ?= gcc

diff --git a/tools/fiptool/Makefile b/tools/fiptool/Makefile
index 7c2a083..e6aeba9 100644
--- a/tools/fiptool/Makefile
+++ b/tools/fiptool/Makefile

@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2014-2021, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2014-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -22,7 +22,14 @@
 else
   HOSTCCFLAGS += -O2
 endif
-LDLIBS := -L${OPENSSL_DIR}/lib -lcrypto
+
+# Include library directories where OpenSSL library files are located.
+# For a normal installation (i.e.: when ${OPENSSL_DIR} = /usr or
+# /usr/local), binaries are located under the ${OPENSSL_DIR}/lib/
+# directory. However, for a local build of OpenSSL, the built binaries are
+# located under the main project directory (i.e.: ${OPENSSL_DIR}, not
+# ${OPENSSL_DIR}/lib/).
+LDLIBS := -L${OPENSSL_DIR}/lib -L${OPENSSL_DIR} -lcrypto
 
 ifeq (${V},0)
   Q := @

diff --git a/tools/fiptool/tbbr_config.c b/tools/fiptool/tbbr_config.c
index 4998bb2..cdbf389 100644
--- a/tools/fiptool/tbbr_config.c
+++ b/tools/fiptool/tbbr_config.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -172,6 +172,21 @@
 		.cmdline_name = "plat-sp-cert"
 	},
 	{
+		.name = "CCA Content Certificate",
+		.uuid = UUID_CCA_CONTENT_CERT,
+		.cmdline_name = "cca-cert"
+	},
+	{
+		.name = "Core Secure World Key Certificate",
+		.uuid = UUID_CORE_SWD_KEY_CERT,
+		.cmdline_name = "core-swd-cert"
+	},
+	{
+		.name = "Platform Key Certificate",
+		.uuid = UUID_PLAT_KEY_CERT,
+		.cmdline_name = "plat-key-cert"
+	},
+	{
 		.name = NULL,
 		.uuid = { {0} },
 		.cmdline_name = NULL,

diff --git a/tools/sptool/sp_mk_generator.py b/tools/sptool/sp_mk_generator.py
old mode 100755
new mode 100644
index 82d5c1b..814d051
--- a/tools/sptool/sp_mk_generator.py
+++ b/tools/sptool/sp_mk_generator.py

@@ -46,112 +46,187 @@
 }
 
 """
-
-import getopt
 import json
 import os
 import re
 import sys
 import uuid
+from spactions import SpSetupActions
 
-with open(sys.argv[2],'r') as in_file:
-    data = json.load(in_file)
-json_file = os.path.abspath(sys.argv[2])
-json_dir = os.path.dirname(json_file)
-gen_file = os.path.abspath(sys.argv[1])
-out_dir = os.path.abspath(sys.argv[3])
-dtb_dir = out_dir + "/fdts/"
 MAX_SP = 8
-dualroot = sys.argv[4].lower() == "dualroot"
-split = int(MAX_SP / 2)
-print(dtb_dir)
-platform_count = 1
-sip_count = 1
+UUID_LEN = 4
 
-with open(gen_file, 'w') as out_file:
-    for idx, key in enumerate(data.keys()):
+# Some helper functions to access args propagated to the action functions in
+# SpSetupActions framework.
+def check_sp_mk_gen(args :dict):
+    if "sp_gen_mk" not in args.keys():
+        raise Exception(f"Path to file sp_gen.mk needs to be in 'args'.")
 
-        pkg_num = idx + 1
+def check_out_dir(args :dict):
+    if "out_dir" not in args.keys() or not os.path.isdir(args["out_dir"]):
+        raise Exception("Define output folder with \'out_dir\' key.")
 
-        if (pkg_num > MAX_SP):
-            print("WARNING: Too many secure partitions\n")
-            exit(-1)
+def check_sp_layout_dir(args :dict):
+    if "sp_layout_dir" not in args.keys() or not os.path.isdir(args["sp_layout_dir"]):
+        raise Exception("Define output folder with \'sp_layout_dir\' key.")
 
-        if dualroot:
-            owner = data[key].get('owner')
-            if owner == "Plat":
-                if (platform_count > split):
-                    print("WARNING: Maximum Secure partitions by Plat " +
-                    "have been exceeded (" + str(split) + ")\n")
-                    exit(-1)
-                pkg_num = split + platform_count
-                platform_count += 1
-            elif (sip_count > split):
-                print("WARNING: Maximum Secure partitions by SiP " +
-                "have been exceeded (" + str(split) + ")\n")
-                exit(-1)
-            else:
-                pkg_num = sip_count
-                sip_count += 1
+def write_to_sp_mk_gen(content, args :dict):
+    check_sp_mk_gen(args)
+    with open(args["sp_gen_mk"], "a") as f:
+        f.write(f"{content}\n")
 
-        """
-        Append FDT_SOURCES
-        """
-        dts = os.path.join(json_dir, data[key]['pm'])
-        dtb = dtb_dir + os.path.basename(data[key]['pm'][:-1] + "b")
-        out_file.write("FDT_SOURCES += " + dts + "\n")
+def get_sp_manifest_full_path(sp_node, args :dict):
+    check_sp_layout_dir(args)
+    return os.path.join(args["sp_layout_dir"], get_file_from_layout(sp_node["pm"]))
 
-        """
-        Update SPTOOL_ARGS
-        """
-        dst = out_dir + "/" + key + ".pkg"
-        src = [ json_dir + "/" + data[key]['image'] , dtb  ]
-        out_file.write("SPTOOL_ARGS += -i " + ":".join(src) + " -o " + dst + "\n")
+def get_sp_img_full_path(sp_node, args :dict):
+    check_sp_layout_dir(args)
+    return os.path.join(args["sp_layout_dir"], get_file_from_layout(sp_node["image"]))
 
-        if "uuid" in data[key]:
-            """
-            Extract the UUID from the JSON file if the SP entry has a 'uuid' field
-            """
-            uuid_std = uuid.UUID(data[key]['uuid'])
-        else:
-            """
-            Extract uuid from partition manifest
-            """
-            pm_file = open(dts)
-            for line in pm_file:
-                if "uuid" in line:
-                    # re.findall returns a list of string tuples.
-                    # uuid_hex is the first item in this list representing the four
-                    # uuid hex integers from the manifest uuid field. The heading
-                    # '0x' of the hexadecimal representation is stripped out.
-                    # e.g. uuid = <0x1e67b5b4 0xe14f904a 0x13fb1fb8 0xcbdae1da>;
-                    # uuid_hex = ('1e67b5b4', 'e14f904a', '13fb1fb8', 'cbdae1da')
-                    uuid_hex = re.findall(r'0x([0-9a-f]+) 0x([0-9a-f]+) 0x([0-9a-f]+) 0x([0-9a-f]+)', line)[0];
+def get_sp_pkg(sp, args :dict):
+    check_out_dir(args)
+    return os.path.join(args["out_dir"], f"{sp}.pkg")
+
+def is_line_in_sp_gen(line, args :dict):
+    with open(args["sp_gen_mk"], "r") as f:
+        sppkg_rule = [l for l in f if line in l]
+    return len(sppkg_rule) is not 0
+
+def get_file_from_layout(node):
+    ''' Helper to fetch a file path from sp_layout.json. '''
+    if type(node) is dict and "file" in node.keys():
+        return node["file"]
+    return node
 
-            # uuid_hex is a list of four hex string values
-            if len(uuid_hex) != 4:
-                print("ERROR: malformed UUID")
-                exit(-1)
+def get_offset_from_layout(node):
+    ''' Helper to fetch an offset from sp_layout.json. '''
+    if type(node) is dict and "offset" in node.keys():
+        return int(node["offset"], 0)
+    return None
 
-            # The uuid field in SP manifest is the little endian representation
-            # mapped to arguments as described in SMCCC section 5.3.
-            # Convert each unsigned integer value to a big endian representation
-            # required by fiptool.
-            y=list(map(bytearray.fromhex, uuid_hex))
-            z=(int.from_bytes(y[0], byteorder='little', signed=False),
-            int.from_bytes(y[1], byteorder='little', signed=False),
-            int.from_bytes(y[2], byteorder='little', signed=False),
-            int.from_bytes(y[3], byteorder='little', signed=False))
-            uuid_std = uuid.UUID(f'{z[0]:08x}{z[1]:08x}{z[2]:08x}{z[3]:08x}')
+def get_image_offset(node):
+    ''' Helper to fetch image offset from sp_layout.json '''
+    return get_offset_from_layout(node["image"])
+
+def get_pm_offset(node):
+    ''' Helper to fetch pm offset from sp_layout.json '''
+    return get_offset_from_layout(node["pm"])
+
+@SpSetupActions.sp_action(global_action=True)
+def check_max_sps(sp_layout, _, args :dict):
+    ''' Check validate the maximum number of SPs is respected. '''
+    if len(sp_layout.keys()) > MAX_SP:
+        raise Exception(f"Too many SPs in SP layout file. Max: {MAX_SP}")
+    return args
+
+@SpSetupActions.sp_action
+def gen_fdt_sources(sp_layout, sp, args :dict):
+    ''' Generate FDT_SOURCES values for a given SP. '''
+    manifest_path = get_sp_manifest_full_path(sp_layout[sp], args)
+    write_to_sp_mk_gen(f"FDT_SOURCES += {manifest_path}", args)
+    return args
 
-        """
-        Append FIP_ARGS
-        """
-        out_file.write("FIP_ARGS += --blob uuid=" + str(uuid_std) + ",file=" + dst + "\n")
+@SpSetupActions.sp_action
+def gen_sptool_args(sp_layout, sp, args :dict):
+    ''' Generate Sp Pkgs rules. '''
+    sp_pkg = get_sp_pkg(sp, args)
+    sp_dtb_name = os.path.basename(get_file_from_layout(sp_layout[sp]["pm"]))[:-1] + "b"
+    sp_dtb = os.path.join(args["out_dir"], f"fdts/{sp_dtb_name}")
 
-        """
-        Append CRT_ARGS
-        """
+    # Do not generate rule if already there.
+    if is_line_in_sp_gen(f'{sp_pkg}:', args):
+        return args
+    write_to_sp_mk_gen(f"SP_PKGS += {sp_pkg}\n", args)
+
+    sptool_args = f" -i {get_sp_img_full_path(sp_layout[sp], args)}:{sp_dtb}"
+    pm_offset = get_pm_offset(sp_layout[sp])
+    sptool_args += f" --pm-offset {pm_offset}" if pm_offset is not None else ""
+    image_offset = get_image_offset(sp_layout[sp])
+    sptool_args += f" --img-offset {image_offset}" if image_offset is not None else ""
+    sptool_args += f" -o {sp_pkg}"
+    sppkg_rule = f'''
+{sp_pkg}: {sp_dtb}
+\t$(Q)echo Generating {sp_pkg}
+\t$(Q)$(PYTHON) $(SPTOOL) {sptool_args}
+'''
+    write_to_sp_mk_gen(sppkg_rule, args)
+    return args
+
+@SpSetupActions.sp_action(global_action=True, exec_order=1)
+def check_dualroot(sp_layout, _, args :dict):
+    ''' Validate the amount of SPs from SiP and Platform owners. '''
+    if not args.get("dualroot"):
+        return args
+    args["split"] =  int(MAX_SP / 2)
+    owners = [sp_layout[sp].get("owner") for sp in sp_layout]
+    args["plat_max_count"] = owners.count("Plat")
+    # If it is owned by the platform owner, it is assigned to the SiP.
+    args["sip_max_count"] = len(sp_layout.keys()) - args["plat_max_count"]
+    if  args["sip_max_count"] > args["split"] or args["sip_max_count"] > args["split"]:
+        print(f"WARN: SiP Secure Partitions should not be more than {args['split']}")
+    # Counters for gen_crt_args.
+    args["sip_count"] = 1
+    args["plat_count"] = 1
+    return args
+
+@SpSetupActions.sp_action
+def gen_crt_args(sp_layout, sp, args :dict):
+    ''' Append CRT_ARGS. '''
+    # If "dualroot" is configured, 'sp_pkg_idx' depends on whether the SP is owned
+    # by the "SiP" or the "Plat".
+    if args.get("dualroot"):
+        # If the owner is not specified as "Plat", default to "SiP".
+        if sp_layout[sp].get("owner") == "Plat":
+            if args["plat_count"] > args["plat_max_count"]:
+                raise ValueError("plat_count can't surpass plat_max_count in args.")
+            sp_pkg_idx = args["plat_count"] + args["split"]
+            args["plat_count"] += 1
+        else:
+            if args["sip_count"] > args["sip_max_count"]:
+                raise ValueError("sip_count can't surpass sip_max_count in args.")
+            sp_pkg_idx = args["sip_count"]
+            args["sip_count"] += 1
+    else:
+        sp_pkg_idx = [k for k in sp_layout.keys()].index(sp) + 1
+    write_to_sp_mk_gen(f"CRT_ARGS += --sp-pkg{sp_pkg_idx} {get_sp_pkg(sp, args)}\n", args)
+    return args
+
+@SpSetupActions.sp_action
+def gen_fiptool_args(sp_layout, sp, args :dict):
+    ''' Generate arguments for the FIP Tool. '''
+    if "uuid" in sp_layout[sp]:
+        # Extract the UUID from the JSON file if the SP entry has a 'uuid' field
+        uuid_std = uuid.UUID(data[key]['uuid'])
+    else:
+        with open(get_sp_manifest_full_path(sp_layout[sp], args), "r") as pm_f:
+            uuid_lines = [l for l in pm_f if 'uuid' in l]
+        assert(len(uuid_lines) is 1)
+        # The uuid field in SP manifest is the little endian representation
+        # mapped to arguments as described in SMCCC section 5.3.
+        # Convert each unsigned integer value to a big endian representation
+        # required by fiptool.
+        uuid_parsed = re.findall("0x([0-9a-f]+)", uuid_lines[0])
+        y = list(map(bytearray.fromhex, uuid_parsed))
+        z = [int.from_bytes(i, byteorder='little', signed=False) for i in y]
+        uuid_std = uuid.UUID(f'{z[0]:08x}{z[1]:08x}{z[2]:08x}{z[3]:08x}')
+    write_to_sp_mk_gen(f"FIP_ARGS += --blob uuid={str(uuid_std)},file={get_sp_pkg(sp, args)}\n", args)
+    return args
+
+def init_sp_actions(sys):
+    sp_layout_file = os.path.abspath(sys.argv[2])
+    with open(sp_layout_file) as json_file:
+        sp_layout = json.load(json_file)
+    # Initialize arguments for the SP actions framework
+    args = {}
+    args["sp_gen_mk"] = os.path.abspath(sys.argv[1])
+    args["sp_layout_dir"] = os.path.dirname(sp_layout_file)
+    args["out_dir"] = os.path.abspath(sys.argv[3])
+    args["dualroot"] = sys.argv[4] == "dualroot"
+    #Clear content of file "sp_gen.mk".
+    with open(args["sp_gen_mk"], "w"):
+        None
+    return args, sp_layout
 
-        out_file.write("CRT_ARGS += --sp-pkg" + str(pkg_num) + " " + dst + "\n")
-        out_file.write("\n")
+if __name__ == "__main__":
+    args, sp_layout = init_sp_actions(sys)
+    SpSetupActions.run_actions(sp_layout, args)

diff --git a/tools/sptool/spactions.py b/tools/sptool/spactions.py
new file mode 100644
index 0000000..ff28ebb
--- /dev/null
+++ b/tools/sptool/spactions.py

@@ -0,0 +1,155 @@
+#!/usr/bin/python3
+# Copyright (c) 2022, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+'''
+This is a python module for defining and executing SP setup actions, targeting
+a system deploying an SPM implementation.
+Each action consists of a function, that processes the SP layout json file and
+other provided arguments.
+At the core of this is the SpSetupActions which provides a means to register
+the functions into a table of actions, and execute them all when invoking
+SpSetupActions.run_actions.
+Registering the function is done by using the decorator '@SpSetupActions.sp_action'
+at function definition.
+
+Functions can be called:
+- once only, or per SP defined in the SP layout file;
+- following an order, from lowest to highest of their execution order.
+More information in the doc comments below.
+'''
+import bisect
+
+DEFAULT_ACTION_ORDER = 100
+
+class _ConfiguredAction:
+    """
+    Wraps action function with its configuration.
+    """
+    def __init__(self, action, exec_order=DEFAULT_ACTION_ORDER, global_action=True, log_calls = False):
+        self.exec_order = exec_order
+        self.__name__ = action.__name__
+        def logged_action(action):
+            def inner_logged_action(sp_layout, sp, args :dict):
+                print(f"Calling {action.__name__} -> {sp}")
+                return action(sp_layout, sp, args)
+            return inner_logged_action
+        self.action = logged_action(action) if log_calls is True else action
+        self.global_action = global_action
+
+    def __lt__(self, other):
+        """
+        To allow for ordered inserts in a list of actions.
+        """
+        return self.exec_order < other.exec_order
+
+    def __call__(self, sp_layout, sp, args :dict):
+        """
+        Calls action function.
+        """
+        return self.action(sp_layout, sp, args)
+
+    def __repr__(self) -> str:
+        """
+        Pretty format to show debug information about the action.
+        """
+        return f"func: {self.__name__}; global:{self.global_action}; exec_order: {self.exec_order}"
+
+class SpSetupActions:
+    actions = []
+
+    def sp_action(in_action = None, global_action = False, log_calls=False, exec_order=DEFAULT_ACTION_ORDER):
+        """
+        Function decorator that registers and configures action.
+
+        :param in_action - function to register
+        :param global_action - make the function global, i.e. make it be
+        only called once.
+        :param log_calls - at every call to action, a useful log will be printed.
+        :param exec_order - action's calling order.
+        """
+        def append_action(action):
+            action = _ConfiguredAction(action, exec_order, global_action, log_calls)
+            bisect.insort(SpSetupActions.actions, action)
+            return action
+        if in_action is not None:
+            return append_action(in_action)
+        return append_action
+
+    def run_actions(sp_layout: dict, args: dict, verbose=False):
+        """
+        Executes all actions in accordance to their registering configuration:
+        - If set as "global" it will be called once.
+        - Actions are called respecting the order established by their "exec_order" field.
+
+        :param sp_layout - dictionary containing the SP layout information.
+        :param args - arguments to be propagated through the call of actions.
+        :param verbose - prints actions information in order of execution.
+        """
+        args["called"] = [] # for debug purposes
+        def append_called(action, sp, args :dict):
+            args["called"].append(f"{action.__name__} -> {sp}")
+            return args
+
+        for action in SpSetupActions.actions:
+            if verbose:
+                print(f"Calling {action}")
+            if action.global_action:
+                scope = "global"
+                args = action(sp_layout, scope, args)
+                args = append_called(action, scope, args)
+            else:
+                # Functions that are not global called for each SP defined in
+                # the SP layout.
+                for sp in sp_layout.keys():
+                    args = action(sp_layout, sp, args)
+                    args = append_called(action, sp, args)
+
+if __name__ == "__main__":
+    # Executing this module will have the following test code/playground executed
+    sp_layout = {
+        "partition1" : {
+            "boot-info": True,
+            "image": {
+                "file": "partition.bin",
+                "offset":"0x2000"
+            },
+            "pm": {
+                "file": "cactus.dts",
+                "offset":"0x1000"
+            },
+            "owner": "SiP"
+        },
+        "partition2" : {
+            "image": "partition.bin",
+            "pm": "cactus-secondary.dts",
+            "owner": "Plat"
+        },
+        "partition3" : {
+            "image": "partition.bin",
+            "pm": "cactus-tertiary.dts",
+            "owner": "Plat"
+        },
+        "partition4" : {
+            "image": "ivy.bin",
+            "pm": "ivy.dts",
+            "owner": "Plat"
+        }
+    }
+
+    #Example of how to use this module
+    @SpSetupActions.sp_action(global_action=True)
+    def my_action1(sp_layout, _, args :dict):
+        print(f"inside function my_action1{sp_layout}\n\n args:{args})")
+        return args # Always return args in action function.
+    @SpSetupActions.sp_action(exec_order=1)
+    def my_action2(sp_layout, sp_name, args :dict):
+        print(f"inside function my_action2; SP: {sp_name} {sp_layout} args:{args}")
+        return args
+
+    # Example arguments to be propagated through the functions.
+    # 'args' can be extended in the action functions.
+    args = dict()
+    args["arg1"] = 0xEEE
+    args["arg2"] = 0xFF
+    SpSetupActions.run_actions(sp_layout, args)

diff --git a/tools/sptool/sptool.c b/tools/sptool/sptool.c
deleted file mode 100644
index 38baa2c..0000000
--- a/tools/sptool/sptool.c
+++ /dev/null

@@ -1,360 +0,0 @@
-/*
- * Copyright (c) 2018-2020, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <stdarg.h>
-#include <stdbool.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-
-#include "sptool.h"
-
-#define PAGE_SIZE		4096
-
-/*
- * Entry describing Secure Partition package.
- */
-struct sp_pkg_info {
-	/* Location of the files in the host's RAM. */
-	void *img_data, *pm_data;
-
-	/* Size of the files. */
-	uint32_t img_size, pm_size;
-
-	/* Location of the binary files inside the package output file */
-	uint32_t img_offset, pm_offset;
-};
-
-/*
- * List of input provided by user
- */
-struct arg_list {
-	char *usr_input;
-	struct arg_list *next;
-};
-
-/* Align an address to a power-of-two boundary. */
-static unsigned int align_to(unsigned int address, unsigned int boundary)
-{
-	unsigned int mask = boundary - 1U;
-
-	if ((address & mask) != 0U)
-		return (address + boundary) & ~mask;
-	else
-		return address;
-}
-
-/* Allocate a memory area of 'size' bytes and zero it. */
-static void *xzalloc(size_t size, const char *msg)
-{
-	void *d;
-
-	d = malloc(size);
-	if (d == NULL) {
-		fprintf(stderr, "error: malloc: %s\n", msg);
-		exit(1);
-	}
-
-	memset(d, 0, size);
-
-	return d;
-}
-
-/*
- * Write 'size' bytes from 'buf' into the specified file stream.
- * Exit the program on error.
- */
-static void xfwrite(void *buf, size_t size, FILE *fp)
-{
-	if (fwrite(buf, 1, size, fp) != size) {
-		fprintf(stderr, "error: Failed to write to output file.\n");
-		exit(1);
-	}
-}
-
-/*
- * Set the file position indicator for the specified file stream.
- * Exit the program on error.
- */
-static void xfseek(FILE *fp, long offset, int whence)
-{
-	if (fseek(fp, offset, whence) != 0) {
-		fprintf(stderr, "error: Failed to set file to offset 0x%lx (%d).\n",
-		       offset, whence);
-		perror(NULL);
-		exit(1);
-	}
-}
-
-/*
- * Free SP package structure
- */
-static void cleanup(struct sp_pkg_info *sp)
-{
-
-	if (sp != NULL) {
-		if (sp->img_data != NULL) {
-			free(sp->img_data);
-		}
-
-		if (sp->pm_data != NULL) {
-			free(sp->pm_data);
-		}
-
-		free(sp);
-
-	}
-}
-
-/*
- * Free argument list structure
- */
-static void freelist(struct arg_list *head)
-{
-	struct arg_list *tmp;
-
-	while (head != NULL) {
-		tmp = head;
-		head = head->next;
-		free(tmp);
-	}
-}
-
-/*
- * Append user inputs in argument list structure
- */
-static void append_user_input(struct arg_list **head, char *args)
-{
-	struct arg_list *tmp = *head;
-
-	if (tmp == NULL) {
-		tmp = xzalloc(sizeof(struct arg_list),
-				"Failed to allocate arg_list struct");
-		tmp->usr_input = args;
-		*head = tmp;
-	} else {
-		while (tmp->next != NULL) {
-			tmp = tmp->next;
-		}
-		tmp->next = xzalloc(sizeof(struct arg_list),
-				"Failed to allocate arg_list struct");
-		tmp = tmp->next;
-		tmp->usr_input = args;
-	}
-}
-
-/*
- * Allocate a buffer big enough to store the content of the specified file and
- * load the file into it. Fill 'size' with the file size. Exit the program on
- * error.
- */
-static void load_file(const char *path, void **ptr, uint32_t *size)
-{
-	FILE *f = fopen(path, "rb");
-	if (f == NULL) {
-		fprintf(stderr, "error: %s couldn't be opened.\n", path);
-		exit(1);
-	}
-
-	xfseek(f, 0, SEEK_END);
-	*size = ftell(f);
-	if (*size == 0) {
-		fprintf(stderr, "error: Size of %s is 0\n", path);
-		exit(1);
-	}
-
-	rewind(f);
-
-	*ptr = malloc(*size);
-	if (*ptr == NULL) {
-		fprintf(stderr, "error: Not enough memory to load %s\n", path);
-		exit(1);
-	}
-
-	if (fread(*ptr, *size, 1, f) != 1) {
-		fprintf(stderr, "error: Couldn't read %s\n", path);
-		exit(1);
-	}
-
-	fclose(f);
-}
-
-/*
- * Parse the string containing input payloads and fill in the
- * SP Package data structure.
- */
-static void load_sp_pm(char *path, struct sp_pkg_info **sp_out)
-{
-	struct sp_pkg_info *sp_pkg;
-
-	char *split_mark = strstr(path, ":");
-
-	*split_mark = '\0';
-
-	char *sp_path = path;
-	char *pm_path = split_mark + 1;
-
-	sp_pkg = xzalloc(sizeof(struct sp_pkg_info),
-		"Failed to allocate sp_pkg_info struct");
-
-	load_file(pm_path, &sp_pkg->pm_data, &sp_pkg->pm_size);
-	printf("\nLoaded SP Manifest file %s (%u bytes)\n", pm_path, sp_pkg->pm_size);
-
-	load_file(sp_path, &sp_pkg->img_data, &sp_pkg->img_size);
-	printf("Loaded SP Image file %s (%u bytes)\n", sp_path, sp_pkg->img_size);
-
-	*sp_out = sp_pkg;
-}
-
-/*
- * Write SP package data structure into output file.
- */
-static void output_write(const char *path, struct sp_pkg_info *sp, bool header)
-{
-	struct sp_pkg_header sp_header_info;
-	unsigned int file_ptr = 0;
-
-	FILE *f = fopen(path, "wb");
-	if (f == NULL) {
-		fprintf(stderr, "error: Failed to open %s\n", path);
-		exit(1);
-	}
-
-	/* Reserve Header size */
-	if (header) {
-		file_ptr = sizeof(struct sp_pkg_header);
-	}
-
-	/* Save partition manifest */
-	xfseek(f, file_ptr, SEEK_SET);
-	printf("Writing SP Manifest at offset 0x%x (%u bytes)\n",
-	       file_ptr, sp->pm_size);
-
-	sp->pm_offset = file_ptr;
-	xfwrite(sp->pm_data, sp->pm_size, f);
-
-	/* Save partition image aligned to Page size */
-	file_ptr = align_to((sp->pm_offset + sp->pm_size), PAGE_SIZE);
-	xfseek(f, file_ptr, SEEK_SET);
-	printf("Writing SP Image at offset 0x%x (%u bytes)\n",
-	       file_ptr, sp->img_size);
-
-	sp->img_offset = file_ptr;
-	xfwrite(sp->img_data, sp->img_size, f);
-
-	/* Finally, write header, if needed */
-	if (header) {
-		sp_header_info.magic = SECURE_PARTITION_MAGIC;
-		sp_header_info.version = 0x1;
-		sp_header_info.img_offset = sp->img_offset;
-		sp_header_info.img_size = sp->img_size;
-		sp_header_info.pm_offset = sp->pm_offset;
-		sp_header_info.pm_size = sp->pm_size;
-
-		xfseek(f, 0, SEEK_SET);
-
-		printf("Writing package header\n");
-
-		xfwrite(&sp_header_info, sizeof(struct sp_pkg_header), f);
-	}
-
-	/* All information has been written now */
-	printf("\nsptool: Built Secure Partition blob %s\n", path);
-
-	fclose(f);
-}
-
-static void usage(void)
-{
-	printf("usage: sptool ");
-#ifdef VERSION
-	printf(VERSION);
-#else
-	/* If built from sptool directory, VERSION is not set. */
-	printf("version unknown");
-#endif
-	printf(" [<args>]\n\n");
-
-	printf("This tool takes as input set of image binary files and the\n"
-	       "partition manifest blobs as input and generates set of\n"
-	       "output package files\n"
-	       "Usage example: sptool -i sp1.bin:sp1.dtb -o sp1.pkg\n"
-	       "                      -i sp2.bin:sp2.dtb -o sp2.pkg ...\n\n");
-	printf("Commands supported:\n");
-	printf("  -o <path>            Set output file path.\n");
-	printf("  -i <sp_path:pm_path> Add Secure Partition image and\n"
-	       "                       Manifest blob (specified in two paths\n"
-	       "                       separated by a colon).\n");
-	printf("  -n                   Generate package without header\n");
-	printf("  -h                   Show this message.\n");
-	exit(1);
-}
-
-int main(int argc, char *argv[])
-{
-	struct sp_pkg_info *sp_pkg = NULL;
-	struct arg_list *in_head = NULL;
-	struct arg_list *out_head = NULL;
-	struct arg_list *in_list = NULL;
-	struct arg_list *out_list = NULL;
-	unsigned int match_counter = 0;
-	bool need_header = true;
-
-	int ch;
-
-	if (argc <= 1) {
-		fprintf(stderr, "error: File paths must be provided.\n\n");
-		usage();
-		return 1;
-	}
-
-	while ((ch = getopt(argc, argv, "hni:o:")) != -1) {
-		switch (ch) {
-		case 'i':
-			append_user_input(&in_head, optarg);
-			match_counter++;
-			break;
-		case 'o':
-			append_user_input(&out_head, optarg);
-			match_counter--;
-			break;
-		case 'n':
-			need_header = false;
-			break;
-		case 'h':
-		default:
-			usage();
-		}
-	}
-
-	if (match_counter) {
-		fprintf(stderr, "error: Input/Output count mismatch.\n\n");
-		freelist(in_head);
-		freelist(out_head);
-		usage();
-		return 1;
-	}
-
-	in_list = in_head;
-	out_list = out_head;
-	while (in_list != NULL) {
-		load_sp_pm(in_list->usr_input, &sp_pkg);
-		output_write(out_list->usr_input, sp_pkg, need_header);
-		in_list = in_list->next;
-		out_list = out_list->next;
-	}
-
-	argc -= optind;
-	argv += optind;
-
-	cleanup(sp_pkg);
-	freelist(in_head);
-	freelist(out_head);
-
-	return 0;
-}

diff --git a/tools/sptool/sptool.py b/tools/sptool/sptool.py
new file mode 100755
index 0000000..ae7df92
--- /dev/null
+++ b/tools/sptool/sptool.py

@@ -0,0 +1,145 @@
+#!/usr/bin/python3
+# Copyright (c) 2022, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+#
+# Copyright 2022 The Hafnium Authors.
+#
+# Use of this source code is governed by a BSD-style
+# license that can be found in the LICENSE file or at
+# https://opensource.org/licenses/BSD-3-Clause.
+
+"""
+Script which generates a Secure Partition package.
+https://trustedfirmware-a.readthedocs.io/en/latest/components/secure-partition-manager.html#secure-partition-packages
+"""
+
+import argparse
+from collections import namedtuple
+import sys
+from shutil import copyfileobj
+import os
+
+HF_PAGE_SIZE = 0x1000 # bytes
+HEADER_ELEMENT_BYTES = 4 # bytes
+MANIFEST_IMAGE_SPLITTER=':'
+PM_OFFSET_DEFAULT = "0x1000"
+IMG_OFFSET_DEFAULT = "0x4000"
+
+def split_dtb_bin(i : str):
+    return i.split(MANIFEST_IMAGE_SPLITTER)
+
+def align_to_page(n):
+    return HF_PAGE_SIZE * \
+          (round(n / HF_PAGE_SIZE) + \
+           (1 if n % HF_PAGE_SIZE else 0))
+
+def to_bytes(value):
+    return int(value).to_bytes(HEADER_ELEMENT_BYTES, 'little')
+
+class SpPkg:
+    def __init__(self, pm_path : str, img_path : str, pm_offset: int,
+                 img_offset: int):
+        if not os.path.isfile(pm_path) or not os.path.isfile(img_path):
+            raise Exception(f"Parameters should be path.  \
+                              manifest: {pm_path}; img: {img_path}")
+        self.pm_path = pm_path
+        self.img_path = img_path
+        self._SpPkgHeader = namedtuple("SpPkgHeader",
+                             ("magic", "version",
+                              "pm_offset", "pm_size",
+                              "img_offset", "img_size"))
+
+        if pm_offset >= img_offset:
+            raise ValueError("pm_offset must be smaller than img_offset")
+
+        is_hfpage_aligned = lambda val : val % HF_PAGE_SIZE == 0
+        if not is_hfpage_aligned(pm_offset) or not is_hfpage_aligned(img_offset):
+           raise ValueError(f"Offsets provided need to be page aligned: pm-{pm_offset}, img-{img_offset}")
+
+        if img_offset - pm_offset < self.pm_size:
+            raise ValueError(f"pm_offset and img_offset do not fit the specified file:{pm_path})")
+
+        self.pm_offset = pm_offset
+        self.img_offset = img_offset
+
+    def __str__(self):
+        return \
+        f'''--SP package Info--
+        header:{self.header}
+        pm: {self.pm_path}
+        img: {self.img_path}
+        '''
+
+    @property
+    def magic(self):
+        return "SPKG".encode()
+
+    @property
+    def version(self):
+        return 0x2
+
+    @property
+    def pm_size(self):
+        return os.path.getsize(self.pm_path)
+
+    @property
+    def img_size(self):
+        return os.path.getsize(self.img_path)
+
+    @property
+    def header(self):
+        return self._SpPkgHeader(
+                self.magic,
+                self.version,
+                self.pm_offset,
+                self.pm_size,
+                self.img_offset,
+                self.img_size)
+
+    @property
+    def header_size(self):
+        return len(self._SpPkgHeader._fields)
+
+    def generate(self, f_out : str):
+        with open(f_out, "wb+") as output:
+            for h in self.header:
+                to_write = h if type(h) is bytes else to_bytes(h)
+                output.write(to_write)
+            output.seek(self.pm_offset)
+            with open(self.pm_path, "rb") as pm:
+                copyfileobj(pm, output)
+            output.seek(self.img_offset)
+            with open(self.img_path, "rb") as img:
+                copyfileobj(img, output)
+
+def Main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("-i", required=True,
+                        help="path to partition's image and manifest separated by a colon.")
+    parser.add_argument("--pm-offset", required=False, default=PM_OFFSET_DEFAULT,
+                        help="set partitition manifest offset.")
+    parser.add_argument("--img-offset", required=False, default=IMG_OFFSET_DEFAULT,
+                        help="set partition image offset.")
+    parser.add_argument("-o", required=True, help="set output file path.")
+    parser.add_argument("-v", required=False, action="store_true",
+                        help="print package information.")
+    args = parser.parse_args()
+
+    if not os.path.exists(os.path.dirname(args.o)):
+        raise Exception("Provide a valid output file path!\n")
+
+    image_path, manifest_path = split_dtb_bin(args.i)
+    pm_offset = int(args.pm_offset, 0)
+    img_offset = int(args.img_offset, 0)
+    pkg = SpPkg(manifest_path, image_path, pm_offset, img_offset)
+    pkg.generate(args.o)
+
+    if args.v is True:
+        print(pkg)
+
+    return 0
+
+if __name__ == "__main__":
+    sys.exit(Main())
commit	0d90f22ecf5e4f5160a1d30b74949a3a5e41d625	[log] [tgz]
author	Manish Pandey <manish.pandey2@arm.com>	Fri Jul 08 13:29:58 2022 +0200
committer	TrustedFirmware Code Review <review@review.trustedfirmware.org>	Fri Jul 08 13:29:58 2022 +0200
tree	282d1065e27e468447add85b84258541e90df4cb
parent	8a55bc600f31dcea3600d3d88a784cc02ec267b3 [diff]
parent	3e8c7ec6bb320053a0b7abde981119649ac316c2 [diff]